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