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