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