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         if (p->buf)
  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                                 /* fall through */
 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
 263                 case AUDIO_STREAM_S ... AUDIO_STREAM_E:
 264                 case VIDEO_STREAM_S ... VIDEO_STREAM_E:
 265                 case PRIVATE_STREAM1:
 266
 267                         if (p->mpeg == 2 && p->found == 9) {
 268                                 write_ipack(p, &p->flag1, 1);
 269                                 write_ipack(p, &p->flag2, 1);
 270                                 write_ipack(p, &p->hlength, 1);
 271                         }
 272
 273                         if (p->mpeg == 1 && p->found == 7)
 274                                 write_ipack(p, &p->flag1, 1);
 275
 276                         if (p->mpeg == 2 && (p->flag2 & PTS_ONLY) &&
 277                             p->found < 14) {
 278                                 while (c < count && p->found < 14) {
 279                                         p->pts[p->found-9] = buf[c];
 280                                         write_ipack(p, buf+c, 1);
 281                                         c++;
 282                                         p->found++;
 283                                 }
 284                                 if (c == count)
 285                                         return count;
 286                         }
 287
 288                         if (p->mpeg == 1 && p->which < 2000) {
 289
 290                                 if (p->found == 7) {
 291                                         p->check = p->flag1;
 292                                         p->hlength = 1;
 293                                 }
 294
 295                                 while (!p->which && c < count &&
 296                                        p->check == 0xff){
 297                                         p->check = buf[c];
 298                                         write_ipack(p, buf+c, 1);
 299                                         c++;
 300                                         p->found++;
 301                                         p->hlength++;
 302                                 }
 303
 304                                 if (c == count)
 305                                         return count;
 306
 307                                 if ((p->check & 0xc0) == 0x40 && !p->which) {
 308                                         p->check = buf[c];
 309                                         write_ipack(p, buf+c, 1);
 310                                         c++;
 311                                         p->found++;
 312                                         p->hlength++;
 313
 314                                         p->which = 1;
 315                                         if (c == count)
 316                                                 return count;
 317                                         p->check = buf[c];
 318                                         write_ipack(p, buf+c, 1);
 319                                         c++;
 320                                         p->found++;
 321                                         p->hlength++;
 322                                         p->which = 2;
 323                                         if (c == count)
 324                                                 return count;
 325                                 }
 326
 327                                 if (p->which == 1){
 328                                         p->check = buf[c];
 329                                         write_ipack(p, buf+c, 1);
 330                                         c++;
 331                                         p->found++;
 332                                         p->hlength++;
 333                                         p->which = 2;
 334                                         if (c == count)
 335                                                 return count;
 336                                 }
 337
 338                                 if ((p->check & 0x30) && p->check != 0xff) {
 339                                         p->flag2 = (p->check & 0xf0) << 2;
 340                                         p->pts[0] = p->check;
 341                                         p->which = 3;
 342                                 }
 343
 344                                 if (c == count)
 345                                         return count;
 346                                 if (p->which > 2){
 347                                         if ((p->flag2 & PTS_DTS_FLAGS) == PTS_ONLY) {
 348                                                 while (c < count && p->which < 7) {
 349                                                         p->pts[p->which - 2] = buf[c];
 350                                                         write_ipack(p,buf+c,1);
 351                                                         c++;
 352                                                         p->found++;
 353                                                         p->which++;
 354                                                         p->hlength++;
 355                                                 }
 356                                                 if (c == count)
 357                                                         return count;
 358                                         } else if ((p->flag2 & PTS_DTS_FLAGS) == PTS_DTS) {
 359                                                 while (c < count && p->which < 12) {
 360                                                         if (p->which< 7)
 361                                                                 p->pts[p->which - 2] = buf[c];
 362                                                         write_ipack(p,buf+c,1);
 363                                                         c++;
 364                                                         p->found++;
 365                                                         p->which++;
 366                                                         p->hlength++;
 367                                                 }
 368                                                 if (c == count)
 369                                                         return count;
 370                                         }
 371                                         p->which = 2000;
 372                                 }
 373
 374                         }
 375
 376                         while (c < count && p->found < p->plength+6){
 377                                 l = count -c;
 378                                 if (l+p->found > p->plength+6)
 379                                         l = p->plength+6-p->found;
 380                                 write_ipack(p, buf+c, l);
 381                                 p->found += l;
 382                                 c += l;
 383                         }
 384
 385                         break;
 386                 }
 387
 388
 389                 if ( p->done ){
 390                         if( p->found + count - c < p->plength+6){
 391                                 p->found += count-c;
 392                                 c = count;
 393                         } else {
 394                                 c += p->plength+6 - p->found;
 395                                 p->found = p->plength+6;
 396                         }
 397                 }
 398
 399                 if (p->plength && p->found == p->plength+6) {
 400                         send_ipack(p);
 401                         av7110_ipack_reset(p);
 402                         if (c < count)
 403                                 av7110_ipack_instant_repack(buf+c, count-c, p);
 404                 }
 405         }
 406         return count;
 407 }
 408