drivers/media/dvb/ttpci/av7110_ipack.c

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