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(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(p->buf + off + 3 + ac3_off,
70 								   p->count - ac3_off, &ai, 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 + rest, 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, &p->flag1, 1);
267 				write_ipack(p, &p->flag2, 1);
268 				write_ipack(p, &p->hlength, 1);
269 			}
270 
271 			if (p->mpeg == 1 && p->found == 7)
272 				write_ipack(p, &p->flag1, 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, buf + c, 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, buf + c, 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, buf + c, 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, buf + c, 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, buf + c, 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, buf + c, 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, buf + c, 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, buf + c, 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 + c, count - c, p);
401 		}
402 	}
403 	return count;
404 }
405