av7110_ipack.c

  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
  7void 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
 22int 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
 35void av7110_ipack_free(struct ipack *p)
 36{
 37	vfree(p->buf);
 38}
 39
 40static 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
106void 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
116static 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
139int 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}