# root/branches/streaming-rework/src/libstreaming/cip.c

Revision 386, 6.7 kB (checked in by pieterpalmers, 15 years ago) |
---|

Line | |
---|---|

1 | /* |

2 | * libiec61883 - Linux IEEE 1394 streaming media library. |

3 | * Copyright (C) 2004 Kristian Hogsberg, Dan Dennedy, and Dan Maas. |

4 | * This file written by Kristian Hogsberg. |

5 | * |

6 | * This library is free software; you can redistribute it and/or |

7 | * modify it under the terms of the GNU Lesser General Public |

8 | * License as published by the Free Software Foundation; either |

9 | * version 2.1 of the License, or (at your option) any later version. |

10 | * |

11 | * This library is distributed in the hope that it will be useful, |

12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |

13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |

14 | * Lesser General Public License for more details. |

15 | * |

16 | * You should have received a copy of the GNU Lesser General Public |

17 | * License along with this library; if not, write to the Free Software |

18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |

19 | */ |

20 | |

21 | #include <libiec61883/iec61883.h> |

22 | #include "cip.h" |

23 | |

24 | #include <netinet/in.h> |

25 | |

26 | |

27 | /* Integer fractional math. When we transmit a 44k1Hz signal we must |

28 | * send 5 41/80 samples per isochronous cycle, as these occur 8000 |

29 | * times a second. Of course, we must send an integral number of |

30 | * samples in a packet, so we use the integer math to alternate |

31 | * between sending 5 and 6 samples per packet. |

32 | */ |

33 | |

34 | static void |

35 | fraction_init(struct iec61883_fraction *f, int numerator, int denominator) |

36 | { |

37 | f->integer = numerator / denominator; |

38 | f->numerator = numerator % denominator; |

39 | f->denominator = denominator; |

40 | } |

41 | |

42 | static __inline__ void |

43 | fraction_add(struct iec61883_fraction *dst, |

44 | struct iec61883_fraction *src1, struct iec61883_fraction *src2) |

45 | { |

46 | /* assert: src1->denominator == src2->denominator */ |

47 | |

48 | int sum, denom; |

49 | |

50 | /* We use these two local variables to allow gcc to optimize |

51 | * the division and the modulo into only one division. */ |

52 | |

53 | sum = src1->numerator + src2->numerator; |

54 | denom = src1->denominator; |

55 | dst->integer = src1->integer + src2->integer + sum / denom; |

56 | dst->numerator = sum % denom; |

57 | dst->denominator = denom; |

58 | } |

59 | |

60 | static __inline__ void |

61 | fraction_sub_int(struct iec61883_fraction *dst, struct iec61883_fraction *src, int integer) |

62 | { |

63 | dst->integer = src->integer - integer; |

64 | dst->numerator = src->numerator; |

65 | dst->denominator = src->denominator; |

66 | } |

67 | |

68 | static __inline__ int |

69 | fraction_floor(struct iec61883_fraction *frac) |

70 | { |

71 | return frac->integer; |

72 | } |

73 | |

74 | static __inline__ int |

75 | fraction_ceil(struct iec61883_fraction *frac) |

76 | { |

77 | return frac->integer + (frac->numerator > 0 ? 1 : 0); |

78 | } |

79 | |

80 | void |

81 | iec61883_cip_init(struct iec61883_cip *ptz, int format, int fdf, |

82 | int rate, int dbs, int syt_interval) |

83 | { |

84 | const int transfer_delay = CIP_TRANSFER_DELAY; |

85 | |

86 | ptz->rate = rate; |

87 | ptz->cycle_count = transfer_delay / 3072; |

88 | ptz->cycle_count2 = 0; |

89 | ptz->format = format; |

90 | ptz->fdf = fdf; |

91 | ptz->mode = IEC61883_MODE_BLOCKING_EMPTY; |

92 | ptz->dbs = dbs; |

93 | ptz->dbc = 0; |

94 | ptz->syt_interval = syt_interval; |

95 | |

96 | fraction_init(&ptz->samples_per_cycle, ptz->rate, 8000); |

97 | fraction_init(&ptz->ready_samples, 0, 8000); |

98 | |

99 | /* The ticks_per_syt_offset is initialized to the number of |

100 | * ticks between syt_interval events. The number of ticks per |

101 | * second is 24.576e6, so the number of ticks between |

102 | * syt_interval events is 24.576e6 * syt_interval / rate. |

103 | */ |

104 | fraction_init(&ptz->ticks_per_syt_offset, |

105 | 24576000 * ptz->syt_interval, ptz->rate); |

106 | fraction_init(&ptz->cycle_offset, |

107 | (transfer_delay % 3072) * ptz->rate, ptz->rate); |

108 | } |

109 | |

110 | void |

111 | iec61883_cip_set_transmission_mode(struct iec61883_cip *ptz, int mode) |

112 | { |

113 | ptz->mode = mode; |

114 | } |

115 | |

116 | int |

117 | iec61883_cip_get_max_packet_size(struct iec61883_cip *ptz) |

118 | { |

119 | int max_nevents; |

120 | |

121 | if (ptz->mode == IEC61883_MODE_BLOCKING_EMPTY || ptz->mode == IEC61883_MODE_BLOCKING_NODATA) |

122 | max_nevents = ptz->syt_interval; |

123 | else |

124 | max_nevents = fraction_ceil(&ptz->samples_per_cycle); |

125 | |

126 | return max_nevents * ptz->dbs * 4 + 8; |

127 | } |

128 | |

129 | |

130 | int |

131 | iec61883_cip_fill_header(int node_id, struct iec61883_cip *ptz, |

132 | struct iec61883_packet *packet) |

133 | { |

134 | struct iec61883_fraction next; |

135 | int nevents, nevents_dbc, syt_index, syt; |

136 | |

137 | fraction_add(&next, &ptz->ready_samples, &ptz->samples_per_cycle); |

138 | if (ptz->mode == IEC61883_MODE_BLOCKING_EMPTY || |

139 | ptz->mode == IEC61883_MODE_BLOCKING_NODATA) { |

140 | if (fraction_floor(&next) >= ptz->syt_interval) |

141 | nevents = ptz->syt_interval; |

142 | else |

143 | nevents = 0; |

144 | } |

145 | else |

146 | nevents = fraction_floor(&next); |

147 | |

148 | if (ptz->mode == IEC61883_MODE_BLOCKING_NODATA) { |

149 | /* The DBC is incremented even with NO_DATA packets. */ |

150 | nevents_dbc = ptz->syt_interval; |

151 | } |

152 | else { |

153 | nevents_dbc = nevents; |

154 | } |

155 | |

156 | /* Now that we know how many events to put in the packet, update the |

157 | * fraction ready_samples. */ |

158 | fraction_sub_int(&ptz->ready_samples, &next, nevents); |

159 | |

160 | /* Calculate synchronization timestamp (syt). First we |

161 | * determine syt_index, that is, the index in the packet of |

162 | * the sample for which the timestamp is valid. */ |

163 | syt_index = (ptz->syt_interval - ptz->dbc) & (ptz->syt_interval - 1); |

164 | if (syt_index < nevents) { |

165 | syt = ((ptz->cycle_count << 12) | fraction_floor(&ptz->cycle_offset)) & 0xffff; |

166 | fraction_add(&ptz->cycle_offset, &ptz->cycle_offset, |

167 | &ptz->ticks_per_syt_offset); |

168 | |

169 | /* This next addition should be modulo 8000 (0x1f40), |

170 | * but we only use the lower 4 bits of cycle_count, so |

171 | * we don't need the modulo. */ |

172 | ptz->cycle_count += ptz->cycle_offset.integer / 3072; |

173 | ptz->cycle_offset.integer %= 3072; |

174 | } |

175 | else |

176 | syt = 0xffff; |

177 | |

178 | packet->eoh0 = 0; |

179 | |

180 | /* Our node ID can change after a bus reset, so it is best to fetch |

181 | * our node ID for each packet. */ |

182 | packet->sid = node_id & 0x3f; |

183 | |

184 | packet->dbs = ptz->dbs; |

185 | packet->fn = 0; |

186 | packet->qpc = 0; |

187 | packet->sph = 0; |

188 | packet->reserved = 0; |

189 | packet->dbc = ptz->dbc; |

190 | packet->eoh1 = 2; |

191 | packet->fmt = ptz->format; |

192 | |

193 | if ( nevents == 0 && ptz->mode == IEC61883_MODE_BLOCKING_NODATA ) { |

194 | /* FDF code for packets containing dummy data. */ |

195 | packet->fdf = IEC61883_FDF_NODATA; |

196 | } |

197 | else { |

198 | /* FDF code for non-blocking mode and for blocking mode with empty packets. */ |

199 | packet->fdf = ptz->fdf; |

200 | } |

201 | |

202 | packet->syt = htons(syt); |

203 | |

204 | ptz->dbc += nevents_dbc; |

205 | |

206 | return nevents; |

207 | } |

208 | |

209 | // note that we don't implement timestamp increase for nodata |

210 | // FIXME: check if this is standards compliant!! |

211 | int |

212 | iec61883_cip_fill_header_nodata(int node_id, struct iec61883_cip *ptz, |

213 | struct iec61883_packet *packet) |

214 | { |

215 | int nevents; |

216 | |

217 | packet->eoh0 = 0; |

218 | |

219 | /* Our node ID can change after a bus reset, so it is best to fetch |

220 | * our node ID for each packet. */ |

221 | packet->sid = node_id & 0x3f; |

222 | |

223 | packet->dbs = ptz->dbs; |

224 | packet->fn = 0; |

225 | packet->qpc = 0; |

226 | packet->sph = 0; |

227 | packet->reserved = 0; |

228 | packet->dbc = ptz->dbc; |

229 | packet->eoh1 = 2; |

230 | packet->fmt = ptz->format; |

231 | |

232 | packet->fdf = IEC61883_FDF_NODATA; |

233 | packet->syt = 0xffff; |

234 | |

235 | ptz->dbc += ptz->syt_interval; |

236 | |

237 | return nevents; |

238 | } |

**Note:**See TracBrowser for help on using the browser.