Merge /spare/repo/netdev-2.6 branch 'ieee80211'
[linux-2.6] / drivers / media / dvb / ttpci / av7110_ipack.c
1 #include "dvb_filter.h"
2 #include "av7110_ipack.h"
3 #include <linux/string.h>       /* for memcpy() */
4 #include <linux/vmalloc.h>
5
6
7 void av7110_ipack_reset(struct ipack *p)
8 {
9         p->found = 0;
10         p->cid = 0;
11         p->plength = 0;
12         p->flag1 = 0;
13         p->flag2 = 0;
14         p->hlength = 0;
15         p->mpeg = 0;
16         p->check = 0;
17         p->which = 0;
18         p->done = 0;
19         p->count = 0;
20 }
21
22
23 int av7110_ipack_init(struct ipack *p, int size,
24                       void (*func)(u8 *buf, int size, void *priv))
25 {
26         if (!(p->buf = vmalloc(size*sizeof(u8)))) {
27                 printk(KERN_WARNING "Couldn't allocate memory for ipack\n");
28                 return -ENOMEM;
29         }
30         p->size = size;
31         p->func = func;
32         p->repack_subids = 0;
33         av7110_ipack_reset(p);
34         return 0;
35 }
36
37
38 void av7110_ipack_free(struct ipack *p)
39 {
40         vfree(p->buf);
41 }
42
43
44 static void send_ipack(struct ipack *p)
45 {
46         int off;
47         struct dvb_audio_info ai;
48         int ac3_off = 0;
49         int streamid = 0;
50         int nframes = 0;
51         int f = 0;
52
53         switch (p->mpeg) {
54         case 2:
55                 if (p->count < 10)
56                         return;
57                 p->buf[3] = p->cid;
58                 p->buf[4] = (u8)(((p->count - 6) & 0xff00) >> 8);
59                 p->buf[5] = (u8)((p->count - 6) & 0x00ff);
60                 if (p->repack_subids && p->cid == PRIVATE_STREAM1) {
61                         off = 9 + p->buf[8];
62                         streamid = p->buf[off];
63                         if ((streamid & 0xf8) == 0x80) {
64                                 ai.off = 0;
65                                 ac3_off = ((p->buf[off + 2] << 8)|
66                                            p->buf[off + 3]);
67                                 if (ac3_off < p->count)
68                                         f = dvb_filter_get_ac3info(p->buf + off + 3 + ac3_off,
69                                                                    p->count - ac3_off, &ai, 0);
70                                 if (!f) {
71                                         nframes = (p->count - off - 3 - ac3_off) /
72                                                 ai.framesize + 1;
73                                         p->buf[off + 2] = (ac3_off >> 8) & 0xff;
74                                         p->buf[off + 3] = (ac3_off) & 0xff;
75                                         p->buf[off + 1] = nframes;
76                                         ac3_off +=  nframes * ai.framesize - p->count;
77                                 }
78                         }
79                 }
80                 p->func(p->buf, p->count, p->data);
81
82                 p->buf[6] = 0x80;
83                 p->buf[7] = 0x00;
84                 p->buf[8] = 0x00;
85                 p->count = 9;
86                 if (p->repack_subids && p->cid == PRIVATE_STREAM1
87                     && (streamid & 0xf8) == 0x80) {
88                         p->count += 4;
89                         p->buf[9] = streamid;
90                         p->buf[10] = (ac3_off >> 8) & 0xff;
91                         p->buf[11] = (ac3_off) & 0xff;
92                         p->buf[12] = 0;
93                 }
94                 break;
95
96         case 1:
97                 if (p->count < 8)
98                         return;
99                 p->buf[3] = p->cid;
100                 p->buf[4] = (u8)(((p->count - 6) & 0xff00) >> 8);
101                 p->buf[5] = (u8)((p->count - 6) & 0x00ff);
102                 p->func(p->buf, p->count, p->data);
103
104                 p->buf[6] = 0x0f;
105                 p->count = 7;
106                 break;
107         }
108 }
109
110
111 void av7110_ipack_flush(struct ipack *p)
112 {
113         if (p->plength != MMAX_PLENGTH - 6 || p->found <= 6)
114                 return;
115         p->plength = p->found - 6;
116         p->found = 0;
117         send_ipack(p);
118         av7110_ipack_reset(p);
119 }
120
121
122 static void write_ipack(struct ipack *p, const u8 *data, int count)
123 {
124         u8 headr[3] = { 0x00, 0x00, 0x01 };
125
126         if (p->count < 6) {
127                 memcpy(p->buf, headr, 3);
128                 p->count = 6;
129         }
130
131         if (p->count + count < p->size){
132                 memcpy(p->buf+p->count, data, count);
133                 p->count += count;
134         } else {
135                 int rest = p->size - p->count;
136                 memcpy(p->buf+p->count, data, rest);
137                 p->count += rest;
138                 send_ipack(p);
139                 if (count - rest > 0)
140                         write_ipack(p, data + rest, count - rest);
141         }
142 }
143
144
145 int av7110_ipack_instant_repack (const u8 *buf, int count, struct ipack *p)
146 {
147         int l;
148         int c = 0;
149
150         while (c < count && (p->mpeg == 0 ||
151                              (p->mpeg == 1 && p->found < 7) ||
152                              (p->mpeg == 2 && p->found < 9))
153                &&  (p->found < 5 || !p->done)) {
154                 switch (p->found) {
155                 case 0:
156                 case 1:
157                         if (buf[c] == 0x00)
158                                 p->found++;
159                         else
160                                 p->found = 0;
161                         c++;
162                         break;
163                 case 2:
164                         if (buf[c] == 0x01)
165                                 p->found++;
166                         else if (buf[c] == 0)
167                                 p->found = 2;
168                         else
169                                 p->found = 0;
170                         c++;
171                         break;
172                 case 3:
173                         p->cid = 0;
174                         switch (buf[c]) {
175                         case PROG_STREAM_MAP:
176                         case PRIVATE_STREAM2:
177                         case PROG_STREAM_DIR:
178                         case ECM_STREAM     :
179                         case EMM_STREAM     :
180                         case PADDING_STREAM :
181                         case DSM_CC_STREAM  :
182                         case ISO13522_STREAM:
183                                 p->done = 1;
184                                 /* fall through */
185                         case PRIVATE_STREAM1:
186                         case VIDEO_STREAM_S ... VIDEO_STREAM_E:
187                         case AUDIO_STREAM_S ... AUDIO_STREAM_E:
188                                 p->found++;
189                                 p->cid = buf[c];
190                                 c++;
191                                 break;
192                         default:
193                                 p->found = 0;
194                                 break;
195                         }
196                         break;
197
198                 case 4:
199                         if (count-c > 1) {
200                                 p->plen[0] = buf[c];
201                                 c++;
202                                 p->plen[1] = buf[c];
203                                 c++;
204                                 p->found += 2;
205                                 p->plength = (p->plen[0] << 8) | p->plen[1];
206                         } else {
207                                 p->plen[0] = buf[c];
208                                 p->found++;
209                                 return count;
210                         }
211                         break;
212                 case 5:
213                         p->plen[1] = buf[c];
214                         c++;
215                         p->found++;
216                         p->plength = (p->plen[0] << 8) | p->plen[1];
217                         break;
218                 case 6:
219                         if (!p->done) {
220                                 p->flag1 = buf[c];
221                                 c++;
222                                 p->found++;
223                                 if ((p->flag1 & 0xc0) == 0x80)
224                                         p->mpeg = 2;
225                                 else {
226                                         p->hlength = 0;
227                                         p->which = 0;
228                                         p->mpeg = 1;
229                                         p->flag2 = 0;
230                                 }
231                         }
232                         break;
233
234                 case 7:
235                         if (!p->done && p->mpeg == 2) {
236                                 p->flag2 = buf[c];
237                                 c++;
238                                 p->found++;
239                         }
240                         break;
241
242                 case 8:
243                         if (!p->done && p->mpeg == 2) {
244                                 p->hlength = buf[c];
245                                 c++;
246                                 p->found++;
247                         }
248                         break;
249                 }
250         }
251
252         if (c == count)
253                 return count;
254
255         if (!p->plength)
256                 p->plength = MMAX_PLENGTH - 6;
257
258         if (p->done || ((p->mpeg == 2 && p->found >= 9) ||
259                         (p->mpeg == 1 && p->found >= 7))) {
260                 switch (p->cid) {
261                 case AUDIO_STREAM_S ... AUDIO_STREAM_E:
262                 case VIDEO_STREAM_S ... VIDEO_STREAM_E:
263                 case PRIVATE_STREAM1:
264                         if (p->mpeg == 2 && p->found == 9) {
265                                 write_ipack(p, &p->flag1, 1);
266                                 write_ipack(p, &p->flag2, 1);
267                                 write_ipack(p, &p->hlength, 1);
268                         }
269
270                         if (p->mpeg == 1 && p->found == 7)
271                                 write_ipack(p, &p->flag1, 1);
272
273                         if (p->mpeg == 2 && (p->flag2 & PTS_ONLY) &&
274                             p->found < 14) {
275                                 while (c < count && p->found < 14) {
276                                         p->pts[p->found - 9] = buf[c];
277                                         write_ipack(p, buf + c, 1);
278                                         c++;
279                                         p->found++;
280                                 }
281                                 if (c == count)
282                                         return count;
283                         }
284
285                         if (p->mpeg == 1 && p->which < 2000) {
286
287                                 if (p->found == 7) {
288                                         p->check = p->flag1;
289                                         p->hlength = 1;
290                                 }
291
292                                 while (!p->which && c < count &&
293                                        p->check == 0xff){
294                                         p->check = buf[c];
295                                         write_ipack(p, buf + c, 1);
296                                         c++;
297                                         p->found++;
298                                         p->hlength++;
299                                 }
300
301                                 if (c == count)
302                                         return count;
303
304                                 if ((p->check & 0xc0) == 0x40 && !p->which) {
305                                         p->check = buf[c];
306                                         write_ipack(p, buf + c, 1);
307                                         c++;
308                                         p->found++;
309                                         p->hlength++;
310
311                                         p->which = 1;
312                                         if (c == count)
313                                                 return count;
314                                         p->check = buf[c];
315                                         write_ipack(p, buf + c, 1);
316                                         c++;
317                                         p->found++;
318                                         p->hlength++;
319                                         p->which = 2;
320                                         if (c == count)
321                                                 return count;
322                                 }
323
324                                 if (p->which == 1) {
325                                         p->check = buf[c];
326                                         write_ipack(p, buf + c, 1);
327                                         c++;
328                                         p->found++;
329                                         p->hlength++;
330                                         p->which = 2;
331                                         if (c == count)
332                                                 return count;
333                                 }
334
335                                 if ((p->check & 0x30) && p->check != 0xff) {
336                                         p->flag2 = (p->check & 0xf0) << 2;
337                                         p->pts[0] = p->check;
338                                         p->which = 3;
339                                 }
340
341                                 if (c == count)
342                                         return count;
343                                 if (p->which > 2){
344                                         if ((p->flag2 & PTS_DTS_FLAGS) == PTS_ONLY) {
345                                                 while (c < count && p->which < 7) {
346                                                         p->pts[p->which - 2] = buf[c];
347                                                         write_ipack(p, buf + c, 1);
348                                                         c++;
349                                                         p->found++;
350                                                         p->which++;
351                                                         p->hlength++;
352                                                 }
353                                                 if (c == count)
354                                                         return count;
355                                         } else if ((p->flag2 & PTS_DTS_FLAGS) == PTS_DTS) {
356                                                 while (c < count && p->which < 12) {
357                                                         if (p->which < 7)
358                                                                 p->pts[p->which - 2] = buf[c];
359                                                         write_ipack(p, buf + c, 1);
360                                                         c++;
361                                                         p->found++;
362                                                         p->which++;
363                                                         p->hlength++;
364                                                 }
365                                                 if (c == count)
366                                                         return count;
367                                         }
368                                         p->which = 2000;
369                                 }
370
371                         }
372
373                         while (c < count && p->found < p->plength + 6) {
374                                 l = count - c;
375                                 if (l + p->found > p->plength + 6)
376                                         l = p->plength + 6 - p->found;
377                                 write_ipack(p, buf + c, l);
378                                 p->found += l;
379                                 c += l;
380                         }
381                         break;
382                 }
383
384
385                 if (p->done) {
386                         if (p->found + count - c < p->plength + 6) {
387                                 p->found += count - c;
388                                 c = count;
389                         } else {
390                                 c += p->plength + 6 - p->found;
391                                 p->found = p->plength + 6;
392                         }
393                 }
394
395                 if (p->plength && p->found == p->plength + 6) {
396                         send_ipack(p);
397                         av7110_ipack_reset(p);
398                         if (c < count)
399                                 av7110_ipack_instant_repack(buf + c, count - c, p);
400                 }
401         }
402         return count;
403 }