snatch/snatchconvert.c

   1 /*
   2  *
   3  *  snatch.c
   4  *
   5  *      Copyright (C) 2001 Monty
   6  *
   7  *  This file is part of snatch2{wav,yuv}, for use with the MJPEG
   8  *  tool suite.
   9  *
  10  *  snatch2wav is free software; you can redistribute it and/or modify
  11  *  it under the terms of the GNU General Public License as published by
  12  *  the Free Software Foundation; either version 2, or (at your option)
  13  *  any later version.
  14  *
  15  *  snatch2wav is distributed in the hope that it will be useful,
  16  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  *  GNU General Public License for more details.
  19  *
  20  *  You should have received a copy of the GNU General Public License
  21  *  along with GNU Make; see the file COPYING.  If not, write to
  22  *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
  23  *
  24  *
  25  */
  26
  27 /* Snatch files can consist of multiple depths, rates and channels.
  28    The ideal choice is to resample the whole thing to one common
  29    format */
  30
  31 /* this isn't added into the lav tools because the audio/video sync
  32    isn't premade.  It has to be calculated from timestamps and
  33    maintain an arbitrary sized buffer. */
  34
  35 int audio_p;
  36 int video_p;
  37
  38 double begin_time=0.;
  39 double last_time=-1.;
  40 double fudge_time=0.;
  41 double end_time=1e90;
  42 double global_zerotime=0.;
  43
  44 unsigned char *buftemp;
  45 long           buftemphead;
  46 long           buftemptail;
  47 long           buftempsize;
  48
  49 long  fpsgraph[61];
  50
  51 /* audio resampling ripped from sox and simplified */
  52 /*
  53  * July 5, 1991
  54  * Copyright 1991 Lance Norskog And Sundry Contributors
  55  * This source code is freely redistributable and may be used for
  56  * any purpose.  This copyright notice must be maintained.
  57  * Lance Norskog And Sundry Contributors are not responsible for
  58  * the consequences of using this software.
  59  */
  60 /*
  61  * October 29, 1999
  62  * Various changes, bugfixes(?), increased precision, by Stan Brooks.
  63  */
  64 /*
  65  * November 13, 2001
  66  * Turned a horridly complex sixteen page module prone to roundoff creep
  67  * into a simpler, easier to use four page program with no accumulated
  68  * error.  --Monty
  69  */
  70
  71 #include <math.h>
  72 #include <unistd.h>
  73 #include <stdlib.h>
  74 #include <stdio.h>
  75 #include <string.h>
  76
  77 #define La        16
  78 #define Lc        7
  79 #define Lp       (Lc+La)
  80 #define Nc       (1<<Lc)
  81 #define Np       (1<<Lp)
  82
  83 #define Amask    ((1<<La)-1)
  84
  85 static double rolloff=0.80; /* roll-off frequency */
  86 static double beta=16;      /* passband/stopband tuning magic */
  87 static long Nmult=45;
  88
  89 typedef struct {
  90   long input_rate;
  91   long output_rate;
  92
  93   long input_head;
  94   long input_base;
  95   long output_position;
  96   long del;
  97   long m;
  98   double ratio;
  99   double factor;
 100
 101   double *filter;
 102   double *data;
 103   long buffersize;
 104   long bufferhead;
 105
 106   int nofakep;
 107 } resample_t;
 108
 109 /********************* on the fly audio resampling ******************/
 110
 111 #define IzeroEPSILON 1E-21
 112 static double Izero(double x){
 113    double sum, u, halfx, temp;
 114    long n;
 115
 116    sum = u = n = 1;
 117    halfx = x*.5;
 118    do {
 119       temp = halfx/(double)n;
 120       n += 1;
 121       temp *= temp;
 122       u *= temp;
 123       sum += u;
 124    } while (u >= IzeroEPSILON*sum);
 125    return(sum);
 126 }
 127
 128 static void LpFilter(double *filter,int n,double roll,double beta,long m){
 129   long i;
 130
 131   /* Calculate filter coeffs: */
 132   filter[0] = roll;
 133   for(i=1;i<n;i++) {
 134     double x=M_PI*(double)i/(double)(m);
 135     filter[i]=sin(x*roll)/x;
 136   }
 137
 138   if(beta>2) { /* Apply Kaiser window to filter coeffs: */
 139     double ibeta = 1.0/Izero(beta);
 140     for(i=1;i<n;i++) {
 141       double x=(double)i/(double)n;
 142       filter[i]*=Izero(beta*sqrt(1.-x*x))*ibeta;
 143     }
 144   }else
 145     exit(1);
 146 }
 147
 148 int makeFilter(double *filter,long nwing,double roll,double beta,long m){
 149   double gain=0.;
 150   long mwing, i;
 151
 152    /* it does help accuracy a bit to have the window stop at
 153     * a zero-crossing of the sinc function */
 154    mwing = floor((double)nwing/(m/roll))*(m/roll)+0.5;
 155
 156    /* Design a Nuttall or Kaiser windowed Sinc low-pass filter */
 157    LpFilter(filter, mwing, roll, beta, m);
 158
 159    /* 'correct' the DC gain of the lowpass filter */
 160    for (i=m; i<mwing; i+=m)
 161      gain+=filter[i];
 162    gain = 2*gain + filter[0];    /* DC gain of real coefficients */
 163
 164    gain = 1.0/gain;
 165    for (i=0; i<mwing; i++)
 166      filter[i]*=gain;
 167    for (;i<=nwing;i++)filter[i] = 0;
 168
 169    return(mwing);
 170 }
 171
 172
 173 static void resample_init(resample_t *r, long inputrate, long outputrate,
 174                           int nofakep){
 175   long nwing = Nc * (Nmult/2+1) + 1;
 176   memset(r,0,sizeof(*r));
 177
 178   if(nofakep){
 179     r->filter = malloc(sizeof(*r->filter)*(nwing+1));
 180     makeFilter(r->filter, nwing, rolloff, beta, Nc);
 181   }
 182
 183   r->nofakep=nofakep;
 184   r->input_rate=inputrate;
 185   r->output_rate=outputrate;
 186   r->ratio=r->input_rate/(double)r->output_rate;
 187   r->factor=r->output_rate/(double)r->input_rate;
 188
 189   r->del=Np; /* Fixed-point Filter sampling-time-increment */
 190   if(r->factor<1.0)r->del=r->factor*Np+0.5;
 191   r->m=(nwing<<La)/r->del;
 192
 193   r->buffersize=(r->m+r->input_rate)*16;
 194   if(nofakep){
 195     r->data=calloc(r->buffersize,sizeof(*r->data));
 196   }
 197
 198   r->input_head=r->m*2+1; /* this eliminates output delay at the cost of
 199                             introducing a small absolute time shift */
 200   r->input_base=r->m;
 201 }
 202
 203 static void resample_clear(resample_t *r){
 204   if(r){
 205     if(r->filter)free(r->filter);
 206     if(r->data)free(r->data);
 207   }
 208 }
 209
 210 static double linear_product(const double *filter, const double *data,
 211               long dir, double t, long del, long m){
 212   const double f = 1./(1<<La);
 213   double v=0;
 214   long h=t*del + (m-1)*del; /* so double sum starts with smallest coef's */
 215   data+=(m-1)*dir;
 216
 217   do{
 218     long hh=h>>La;
 219
 220     /* filter coef, lower La bits by linear interpolation */
 221     double coef = filter[hh] + (filter[hh+1]-filter[hh]) * (h&Amask) * f;
 222
 223     v    += coef * *data;
 224     data -= dir;
 225     h    -= del;
 226   }while(--m);
 227
 228   return v;
 229 }
 230
 231 static void resample_in(resample_t *r,double in){
 232   if(r->input_head==r->buffersize){
 233     if(r->nofakep)
 234       memmove(r->data,r->data+r->input_base-r->m,
 235               r->buffersize-r->input_base+r->m);
 236     r->input_head+=r->m-r->input_base;
 237     r->input_base=r->m;
 238   }
 239   if(r->nofakep)
 240     r->data[r->input_head]=in;
 241   r->input_head++;
 242 }
 243
 244 static int resample_out(resample_t *r,double *out){
 245   double input_integer;
 246   double input_fraction=modf(r->output_position*r->ratio,&input_integer);
 247   long offset=r->input_base+(int)input_integer;
 248
 249   if(offset+r->m+1>=r->input_head)
 250     return 0;
 251   else{
 252     double v;
 253
 254     if(r->nofakep){
 255       double *data=r->data+offset;
 256
 257       /* Past inner product: needs Np*Nmult in 31 bits */
 258       v=linear_product(r->filter,data,-1,input_fraction,r->del,r->m);
 259       /* Future inner product: prefer even total */
 260       v+=linear_product(r->filter,data+1,1,1.-input_fraction,r->del,r->m);
 261
 262       if (r->factor<1.)v*=r->factor;
 263       *out=v;
 264     }
 265     r->output_position++;
 266     if(r->output_position>=r->output_rate){
 267       r->output_position=0;
 268       r->input_base+=r->input_rate;
 269     }
 270     return(1);
 271   }
 272 }
 273
 274
 275 /****************** simple audio sample handling ****************/
 276
 277 short     *audbuf;
 278 long long  audbuf_samples;  /* singluar samples, not pairs */
 279 double     audbuf_zerotime;
 280 int        audbuf_head;  /* singluar samples, not pairs */
 281 int        audbuf_tail;  /* singluar samples, not pairs */
 282 int        audbuf_size;  /* singluar samples, not pairs */
 283 long       audbuf_rate;
 284 int        audbuf_channels;
 285
 286 long long samplesin=0;
 287 long long samplesout=0;
 288 long long samplesmissing=0;
 289 long long samplesdiscarded=0;
 290
 291 static void buffer_sample(short v,int nofakep){
 292   if(audbuf_head>=audbuf_size){
 293     if(audbuf_tail>audbuf_size*3/4){
 294       audbuf_head-=audbuf_tail;
 295       if(nofakep)
 296         memmove(audbuf,audbuf+audbuf_tail,audbuf_head*sizeof(*audbuf));
 297       audbuf_tail=0;
 298     }else{
 299       if(audbuf){
 300         audbuf_size*=2;
 301         if(nofakep)
 302           audbuf=realloc(audbuf,audbuf_size*sizeof(*audbuf));
 303       }else{
 304         if(nofakep){
 305           audbuf_size=256*1024;
 306           audbuf=malloc(audbuf_size*sizeof(*audbuf));
 307         }else{
 308           audbuf_size=1;
 309           audbuf=malloc(audbuf_size*sizeof(*audbuf));
 310         }
 311       }
 312     }
 313   }
 314   if(nofakep)
 315     audbuf[audbuf_head]=v;
 316
 317   audbuf_head++;
 318   audbuf_samples++;
 319 }
 320
 321 static void lebuffer_sample(short v,int nofakep){
 322 #ifdef __BIG_ENDIAN__
 323   buffer_sample(((v<<8)&0xff00) | ((v>>8)&0xff),nofakep);
 324 #else
 325   buffer_sample(v,nofakep);
 326 #endif
 327 }
 328
 329 static void rebuffer_sample(long pos,int nofakep){
 330   short v=0;
 331   if(nofakep)v=audbuf[audbuf_head+pos];
 332   buffer_sample(v,nofakep);
 333 }
 334
 335 static int convert_input(unsigned char *buf,int fmt,int *v){
 336   switch(fmt){
 337   case 4:
 338     *v=((int)buf[0]-128)<<8;
 339     return(1);
 340   case 5:
 341     *v=(buf[0]&0xff) | (((signed char *)buf)[1]<<8);
 342     return(2);
 343   case 6:
 344     *v=(buf[1]&0xff) | (((signed char *)buf)[0]<<8);
 345     return(2);
 346   case 7:
 347     *v=((signed char *)buf)[0]<<8;
 348     return(1);
 349   case 8:
 350     *v=(int)((buf[0]&0xff)|((buf[1]<<8)&0xff00))-32768;
 351     return(2);
 352   case 9:
 353     *v=(int)((buf[1]&0xff)|((buf[0]<<8)&0xff00))-32768;
 354     return(2);
 355   }
 356   *v=0;
 357   return(1);
 358 }
 359
 360 static void pre_buffer_audio(long samples,int nofakep){
 361   long i,c=audbuf_head-audbuf_tail;
 362
 363   /* just to expand lazily */
 364   for(i=0;i<samples;i++)
 365     buffer_sample(0,nofakep);
 366
 367   /* shift memory */
 368   if(nofakep){
 369     memmove(audbuf+audbuf_tail+samples,audbuf+audbuf_tail,c*sizeof(*audbuf));
 370     memset(audbuf+audbuf_tail,0,samples*sizeof(*audbuf));
 371   }
 372 }
 373
 374 static double snip_gap_cleanly(double now,int audio){
 375   /* fudge the clock to zip through most/all of the gap or overlap */
 376
 377   if(audio || !audio_p){
 378     if(last_time!=-1){
 379       if(last_time+2.<now){
 380         fudge_time-=now-last_time-1.;
 381       }
 382       if(last_time>now)
 383         fudge_time+=last_time-now;
 384     }
 385     last_time=now;
 386   }
 387   return(now+fudge_time);
 388 }
 389
 390 /************************ snatch parsing *********************/
 391
 392 static int read_snatch_header(FILE *f){
 393   char buffer[12];
 394   int ret=fread(buffer,1,12,f);
 395   if(ret<0){
 396     fprintf(stderr,"Error reading header: %s\n",strerror(ferror(f)));
 397     exit(1);
 398   }
 399   if(ret<6){
 400     fprintf(stderr,"EOF when header expected!\n");
 401     exit(1);
 402   }
 403   if(strncmp(buffer,"SNATCH",6)){
 404     fprintf(stderr,"Input does not begin with a Snatch header!\n");
 405     exit(1);
 406   }
 407
 408   audio_p=0;
 409   video_p=0;
 410   if(buffer[6]=='A')audio_p=1;
 411   if(buffer[7]=='V')video_p=1;
 412
 413
 414   memset(fpsgraph,0,sizeof(fpsgraph));
 415   return(1);
 416 }
 417
 418 int read_snatch_frame_helper(FILE *f,long length,int verify){
 419   long toread=length-buftemphead+buftemptail+5;
 420
 421   if(toread>0){
 422     if(toread+buftemphead>buftempsize){
 423       if(buftemp)
 424         buftemp=realloc(buftemp,(toread+buftemphead)*sizeof(*buftemp));
 425       else
 426         buftemp=malloc((toread+buftemphead)*sizeof(*buftemp));
 427       buftempsize+=toread;
 428     }
 429
 430     if((long)fread(buftemp+buftemphead,1,toread,f)!=toread)return(-1);
 431     buftemphead+=toread;
 432   }
 433
 434   if(verify){
 435     if(!strncmp(buftemp+buftemptail+length,"AUDIO",5))return(length);
 436     if(!strncmp(buftemp+buftemptail+length,"VIDEO",5))return(length);
 437     if(!strncmp(buftemp+buftemptail+length,"YUV12",5))return(length);
 438     return(0);
 439   }else{
 440     return(length);
 441   }
 442 }
 443
 444 resample_t resampler[2];
 445
 446 static int process_audio_frame(char *head,FILE *f,int track_or_process){
 447   int ret;
 448   long long nextsamplepos=audbuf_samples;
 449   double t;
 450   char *s=head+6;
 451   long a=atoi(s),b,ch,ra,fmt,length;
 452   s=strchr(s,' ');
 453   if(!s)return(0);
 454   b=atoi(s);
 455   t=a+b*.000001;
 456
 457   s=strchr(s+1,' ');
 458   if(!s)return(0);
 459   ch=atoi(s);
 460
 461   s=strchr(s+1,' ');
 462   if(!s)return(0);
 463   ra=atoi(s);
 464
 465   s=strchr(s+1,' ');
 466   if(!s)return(0);
 467   fmt=atoi(s);
 468
 469   s=strchr(s+1,' ');
 470   if(!s)return(0);
 471   length=atoi(s);
 472
 473   if((ret=read_snatch_frame_helper(f,length,1))!=length)
 474     return(ret);
 475
 476   if(global_zerotime==0){
 477     global_zerotime=t;
 478     begin_time+=t;
 479     end_time+=t;
 480   }
 481
 482   if(audbuf_rate==0)audbuf_rate=ra;
 483   if(audbuf_channels==0)audbuf_channels=ch;
 484
 485   if(t<begin_time){
 486     int bps=1;
 487     switch(fmt){
 488     case 5:case 6:case 8:case 9:
 489       bps=2;
 490       break;
 491     }
 492     samplesin+=length/(ch*bps);
 493     return(length);
 494   }
 495
 496   if(t>end_time)
 497     return(-1);
 498
 499   /* do we have a large capture gap (eg>2s)? */
 500   t=snip_gap_cleanly(t,1);
 501
 502   if(audbuf_zerotime==0){
 503     audbuf_zerotime=t;
 504     audbuf_samples=0;
 505   }else{
 506     long long actualpos=(t-audbuf_zerotime)*audbuf_rate*audbuf_channels+.5;
 507     long i;
 508
 509     /* we do not nail conversion to a realtime clock when only audio
 510        has been captured; Snatch may have faked the audio interface,
 511        which decoupled playback from any clock */
 512
 513     if(video_p){
 514
 515       //fprintf(stderr,"audio sample jitter: %ld [%ld:%ld]\n",
 516       //(long)(nextsamplepos-actualpos),(long)nextsamplepos,(long)actualpos);
 517
 518       /* hold last sample through any gap, assuming a bit of
 519          hysteresis.  That also holds us through roundoff error (the
 520          roundoff error does *not* creep frame to frame) */
 521       if(audbuf_channels>1){
 522         for(i=actualpos-nextsamplepos-12;i>0;i-=2){
 523           rebuffer_sample(-2,track_or_process);
 524           rebuffer_sample(-2,track_or_process);
 525           samplesmissing++;
 526           //fprintf(stderr,".");
 527         }
 528       }else{
 529         for(i=actualpos-nextsamplepos-12;i>0;i--){
 530           rebuffer_sample(-1,track_or_process);
 531           samplesmissing++;
 532           //fprintf(stderr,".");
 533         }
 534       }
 535
 536       /* discard samples if we're way too far ahead; only likely to
 537          happen due to a fault or misuse of splicing */
 538       if(nextsamplepos-actualpos>12){
 539         /* if we're so far ahead more than 10% of the frame must
 540            disappear, just discard, else compact things a bit by
 541            dropping samples */
 542
 543         fprintf(stderr,"audio sync got way ahead; this case not currently handled\n");
 544         exit(1);
 545
 546       }
 547     }
 548   }
 549
 550   if(audbuf_rate!=ra && resampler[0].input_rate!=ra){
 551     /* set up resampling */
 552     resample_clear(&resampler[0]);
 553     resample_clear(&resampler[1]);
 554     resample_init(&resampler[0],ra,audbuf_rate,track_or_process);
 555     resample_init(&resampler[1],ra,audbuf_rate,track_or_process);
 556   }
 557
 558   if(audbuf_rate!=ra){
 559     long n=length,i;
 560     int ileft,iright;
 561     double left,right;
 562
 563     for(i=0;i<n;){
 564       i+=convert_input(buftemp+buftemptail+i,fmt,&ileft);
 565       samplesin++;
 566       left=ileft*3.0517578e-5;
 567
 568       if(ch>1){
 569         i+=convert_input(buftemp+buftemptail+i,fmt,&iright);
 570         right=iright*3.0517578e-5;
 571       }
 572
 573       if(audbuf_channels>1){
 574         if(ch>1){
 575           resample_in(&resampler[0],left);
 576           resample_in(&resampler[1],right);
 577         }else{
 578           resample_in(&resampler[0],left);
 579         }
 580       }else{
 581         if(ch>1){
 582           left+=right;
 583           resample_in(&resampler[0],left*.5);
 584         }else{
 585           resample_in(&resampler[0],left);
 586         }
 587       }
 588
 589       /* output is always S16LE */
 590       while(1){
 591         int flag=resample_out(&resampler[0],&left);
 592         int sleft,sright;
 593         if(!flag)break;
 594
 595         sleft=left*32767.+.5,sright;
 596         if(sleft>32767)sleft=32767;
 597         if(sleft<-32768)sleft=-32768;
 598         lebuffer_sample(sleft,track_or_process);
 599
 600         if(audbuf_channels>1){
 601           if(ch>1){
 602             flag=resample_out(&resampler[1],&right);
 603             sright=right*32767.+.5;
 604             if(sright>32767)sright=32767;
 605             if(sright<-32768)sright=-32768;
 606             lebuffer_sample(sright,track_or_process);
 607           }else{
 608             lebuffer_sample(sleft,track_or_process);
 609           }
 610         }
 611       }
 612     }
 613
 614   }else{
 615     /* output is always S16LE */
 616     long n=length,i;
 617     int left,right;
 618
 619     for(i=0;i<n;){
 620       samplesin++;
 621       i+=convert_input(buftemp+buftemptail+i,fmt,&left);
 622       if(ch>1)
 623         i+=convert_input(buftemp+buftemptail+i,fmt,&right);
 624
 625       lebuffer_sample(left,track_or_process);
 626       if(audbuf_channels>1){
 627         if(ch>1){
 628           lebuffer_sample(right,track_or_process);
 629         }else{
 630           lebuffer_sample(left,track_or_process);
 631         }
 632       }
 633     }
 634   }
 635   return(length);
 636 }
 637
 638 /*********************** video manipulation ***********************/
 639
 640 /* planar YUV12 (4:2:0) */
 641 void yuvscale(unsigned char *src,int sw,int sh,
 642               unsigned char *dst,int dw,int dh,
 643               int w, int h){
 644   int x,y;
 645   int dxo=(dw-sw)/4,sxo=0;
 646   int dyo=(dh-sh)/4,syo=0;
 647
 648   /* dirt simple for now. No scaling, just centering */
 649
 650   if(dyo<0){
 651     syo= -dyo;
 652     dyo=0;
 653   }
 654   if(dxo<0){
 655     sxo= -dxo;
 656     dxo=0;
 657   }
 658
 659   for(y=0;y<dyo*2;y++){
 660     unsigned char *dptr=dst+y*dw;
 661     for(x=0;x<dw;x++)
 662       *dptr++=0;
 663   }
 664
 665   for(y=0;y<sh && y<dh;y++){
 666     unsigned char *sptr=src+(y+syo*2)*sw+sxo*2;
 667     unsigned char *dptr=dst+(y+dyo*2)*dw;
 668     for(x=0;x<dxo*2;x++)
 669       *dptr++=0;
 670     for(x=0;x<sw && x<dw;x++)
 671       *dptr++=*sptr++;
 672     for(;x<dw-dxo*2;x++)
 673       *dptr++=0;
 674   }
 675
 676   for(;y<dh-dyo*2;y++){
 677     unsigned char *dptr=dst+(y+dyo*2)*dw;
 678     for(x=0;x<dw;x++)
 679       *dptr++=0;
 680   }
 681
 682   src+=sw*sh;
 683   dst+=dw*dh;
 684   sw/=2;
 685   dw/=2;
 686   sh/=2;
 687   dh/=2;
 688
 689   for(y=0;y<dyo;y++){
 690     unsigned char *dptr=dst+y*dw;
 691     for(x=0;x<dw;x++)
 692       *dptr++=128;
 693   }
 694
 695   for(y=0;y<sh && y<dh;y++){
 696     unsigned char *sptr=src+(y+syo)*sw+sxo;
 697     unsigned char *dptr=dst+(y+dyo)*dw;
 698     for(x=0;x<dxo;x++)
 699       *dptr++=128;
 700     for(x=0;x<sw && x<dw;x++)
 701       *dptr++=*sptr++;
 702     for(;x<dw-dxo;x++)
 703       *dptr++=128;
 704   }
 705
 706   for(;y<dh-dyo;y++){
 707     unsigned char *dptr=dst+(y+dyo)*dw;
 708     for(x=0;x<dw;x++)
 709       *dptr++=128;
 710   }
 711
 712
 713   src+=sw*sh;
 714   dst+=dw*dh;
 715
 716   for(y=0;y<dyo;y++){
 717     unsigned char *dptr=dst+y*dw;
 718     for(x=0;x<dw;x++)
 719       *dptr++=128;
 720   }
 721
 722   for(y=0;y<sh && y<dh;y++){
 723     unsigned char *sptr=src+(y+syo)*sw+sxo;
 724     unsigned char *dptr=dst+(y+dyo)*dw;
 725     for(x=0;x<dxo;x++)
 726       *dptr++=128;
 727     for(x=0;x<sw && x<dw;x++)
 728       *dptr++=*sptr++;
 729     for(;x<dw-dxo;x++)
 730       *dptr++=128;
 731   }
 732
 733   for(;y<dh-dyo;y++){
 734     unsigned char *dptr=dst+(y+dyo)*dw;
 735     for(x=0;x<dw;x++)
 736       *dptr++=128;
 737   }
 738
 739 }
 740
 741 void rgbscale(unsigned char *rgb,int sw,int sh,
 742               unsigned char *dst,int dw,int dh,
 743               unsigned int w, int h){
 744   int ih=sh/2*2;
 745   int iw=sw/2*2;
 746
 747   unsigned char *y=alloca(ih*iw*3/2);
 748   unsigned char *u=y+ih*iw;
 749   unsigned char *v=u+ih*iw/4;
 750
 751   int every=0,other=sw*3,c4=0,i,j;
 752   unsigned char *ye=y,*yo=y+iw;
 753
 754   for(i=0;i<ih;i+=2){
 755     for(j=0;j<iw;j+=2){
 756       long yval,uval,vval;
 757
 758       yval =   rgb[every]*19595 + rgb[every+1]*38470 + rgb[every+2]*7471;
 759       uval = rgb[every+2]*65536 -   rgb[every]*22117 - rgb[every+1]*43419;
 760       vval =   rgb[every]*65536 - rgb[every+1]*54878 - rgb[every+2]*10658;
 761       *ye++ =yval>>16;
 762       every+=3;
 763       yval =   rgb[every]*19595 + rgb[every+1]*38470 + rgb[every+2]*7471;
 764       uval+= rgb[every+2]*65536 -   rgb[every]*22117 - rgb[every+1]*43419;
 765       vval+=   rgb[every]*65536 - rgb[every+1]*54878 - rgb[every+2]*10658;
 766       *ye++ =yval>>16;
 767       every+=3;
 768
 769       yval =   rgb[other]*19595 + rgb[other+1]*38470 + rgb[other+2]*7471;
 770       uval = rgb[other+2]*65536 -   rgb[other]*22117 - rgb[other+1]*43419;
 771       vval =   rgb[other]*65536 - rgb[other+1]*54878 - rgb[other+2]*10658;
 772       *yo++ =yval>>16;
 773       other+=3;
 774       yval =   rgb[other]*19595 + rgb[other+1]*38470 + rgb[other+2]*7471;
 775       uval+= rgb[other+2]*65536 -   rgb[other]*22117 - rgb[other+1]*43419;
 776       vval+=   rgb[other]*65536 - rgb[other+1]*54878 - rgb[other+2]*10658;
 777       *yo++ =yval>>16;
 778       other+=3;
 779
 780       u[c4]  =(uval>>19)+128;
 781       v[c4++]=(vval>>19)+128;
 782
 783     }
 784     ye+=iw;
 785     yo+=iw;
 786     every+=sw*3 + sw%2*3;
 787     other+=sw*3 + sw%2*3;
 788
 789   }
 790
 791   yuvscale(y,iw,ih,dst,dw,dh,w,h);
 792
 793 }
 794
 795 unsigned char     **vidbuf;
 796 long long *vidbuf_frameno;
 797 double     vidbuf_zerotime;
 798 long long  vidbuf_frames;
 799 int        vidbuf_head;
 800 int        vidbuf_tail;
 801 int        vidbuf_size;
 802 int        vidbuf_height;
 803 int        vidbuf_width;
 804 double     vidin_fps;
 805 double     vidout_fps;
 806
 807 int scale_width;
 808 int scale_height;
 809
 810 long long framesin=0;
 811 long long framesout=0;
 812 long long framesmissing=0;
 813 long long framesdiscarded=0;
 814
 815 double video_last_time=-1;
 816
 817 static int process_video_frame(char *buffer,FILE *f,int notfakep,int yuvp){
 818   char *s=buffer+6;
 819   long a=atoi(s),b,w,h,length;
 820   double t;
 821   int ret;
 822
 823   s=strchr(s,' ');
 824   if(!s)return(0);
 825   b=atoi(s);
 826   t=a+b*.000001;
 827
 828   s=strchr(s+1,' ');
 829   if(!s)return(0);
 830   w=atoi(s);
 831
 832   s=strchr(s+1,' ');
 833   if(!s)return(0);
 834   h=atoi(s);
 835
 836   s=strchr(s+1,' ');
 837   if(!s)return(0);
 838   length=atoi(s);
 839
 840   if((ret=read_snatch_frame_helper(f,length,1))!=length)
 841     return(ret);
 842
 843   if(global_zerotime==0){
 844     global_zerotime=t;
 845     begin_time+=t;
 846     end_time+=t;
 847   }
 848
 849   if(t<begin_time)return(length);
 850   if(t>end_time)
 851     return(-1);
 852
 853   /* do we have a large capture gap (eg>2s)? */
 854   t=snip_gap_cleanly(t,0);
 855
 856   if(video_last_time!=-1){
 857     double del_t=t-video_last_time;
 858     if(del_t>0){
 859       int val=ceil(1./(t-video_last_time));
 860       if(val>0 && val<61)
 861         fpsgraph[val]++;
 862     }
 863   }
 864   video_last_time=t;
 865
 866   /* video sync is fundamentally different from audio. We assume that
 867      frames never appear early; an frame that seems early in context
 868      of the previous frame is due backlogged framebuffer catching up
 869      and this frame is actually late, as are however many 'on time'
 870      frames behind it. */
 871   if(vidbuf_zerotime==0.){
 872     vidbuf_zerotime=t;
 873   }else{
 874     double ideal=(double)vidbuf_frames;
 875     double actual=(t-vidbuf_zerotime)*vidin_fps;
 876     double drift=actual-ideal;
 877     int i;
 878
 879     /* intentional range for hysteresis */
 880     if(drift<.5){
 881       /* 'early' frame; bump the whole train back if possible,
 882          else discard */
 883       if(vidbuf_head-vidbuf_tail <
 884          vidbuf_frameno[vidbuf_head-1]+1-
 885          vidbuf_frameno[vidbuf_tail]){
 886
 887         /* yes, there's a hole. look for it */
 888
 889         vidbuf_frameno[vidbuf_head-1]--;
 890         for(i=vidbuf_head-1;i>vidbuf_tail;i--){
 891           if(vidbuf_frameno[i]==vidbuf_frameno[i-1])
 892             vidbuf_frameno[i-1]--;
 893           else
 894             break;
 895         }
 896         vidbuf_frames=ideal-1;
 897
 898       }else{
 899
 900         /* no room to bump back.  Discard the 'early' frame
 901            in order to reclaim sync, even if destructively. */
 902         framesdiscarded++;
 903         return(length);
 904       }
 905     }
 906
 907     if(drift>1.){
 908       /* 'late' frame.  Skip the counter ahead.  Don't
 909          carry forward through the gap yet. */
 910       vidbuf_frames=rint(actual);
 911     }
 912   }
 913
 914   if(vidbuf_width==0)vidbuf_width=(w>>1)<<1;
 915   if(vidbuf_height==0)vidbuf_height=(h>>1)<<1;
 916
 917   /* get a buffer */
 918   if(vidbuf_head>=vidbuf_size){
 919     if(vidbuf_tail){
 920
 921       while(vidbuf_tail){
 922         unsigned char *temp;
 923
 924         vidbuf_head--;
 925         vidbuf_tail--;
 926         if(notfakep){
 927           temp=vidbuf[0];
 928           memmove(vidbuf,vidbuf+1,(vidbuf_size-1)*sizeof(*vidbuf));
 929           vidbuf[vidbuf_size-1]=temp;
 930         }
 931         memmove(vidbuf_frameno,vidbuf_frameno+1,
 932                 (vidbuf_size-1)*sizeof(*vidbuf_frameno));
 933       }
 934     }else{
 935       if(vidbuf){
 936         vidbuf_size++;
 937         if(notfakep)vidbuf=realloc(vidbuf,vidbuf_size*sizeof(*vidbuf));
 938         vidbuf_frameno=realloc(vidbuf_frameno,vidbuf_size*sizeof(*vidbuf_frameno));
 939       }else{
 940         vidbuf_size=1;
 941         vidbuf=malloc(vidbuf_size*sizeof(*vidbuf));
 942         vidbuf_frameno=malloc(vidbuf_size*sizeof(*vidbuf_frameno));
 943       }
 944       if(notfakep)
 945         vidbuf[vidbuf_size-1]=malloc(vidbuf_width*vidbuf_height*3/2);
 946     }
 947   }
 948   vidbuf_frameno[vidbuf_head]=vidbuf_frames;
 949   vidbuf_frames++;
 950
 951   /* scale image into buffer */
 952   if(notfakep){
 953     if(yuvp)
 954       yuvscale(buftemp+buftemptail,w,h,vidbuf[vidbuf_head],vidbuf_width,vidbuf_height,
 955                scale_width,scale_height);
 956     else
 957       rgbscale(buftemp+buftemptail,w,h,vidbuf[vidbuf_head],vidbuf_width,vidbuf_height,
 958                scale_width,scale_height);
 959   }
 960   /* finally any needed invasive blanking */
 961
 962   vidbuf_head++;
 963   return(length);
 964 }
 965
 966
 967 static char *strrstr(char *string,char *test){
 968   char *ret=NULL;
 969   char *temp;
 970   while((temp=strstr(string,test))){
 971     ret=temp;
 972     string=temp+1;
 973   }
 974   return ret;
 975 }
 976
 977 /* more complicated than it used to be; we need to check framing */
 978 static int read_snatch_frame(FILE *f,int wa,int wv){
 979   while(!feof(f)){
 980     if(buftemptail){
 981       memmove(buftemp,buftemp+buftemptail,buftemphead-buftemptail);
 982       buftemphead-=buftemptail;
 983       buftemptail=0;
 984     }
 985
 986     if(!read_snatch_frame_helper(f,2048,0))return(0);
 987
 988     {
 989       unsigned char *poss=memchr(buftemp,':',buftemphead);
 990       int pos=-1;
 991       if(poss)pos=poss-buftemp;
 992       if(poss){
 993         char *audio,*video,*yuv12;
 994         int ret=0;
 995         buftemp[pos]='\0';
 996
 997         /* search *backwards* from the colon */
 998         audio=strrstr(buftemp,"AUDIO");
 999         video=strrstr(buftemp,"VIDEO");
1000         yuv12=strrstr(buftemp,"YUV12");
1001
1002         buftemptail=pos+1;
1003
1004         if(audio || video || yuv12){
1005           if(audio)
1006             ret=process_audio_frame(audio, f, wa);
1007           else if(video){
1008             ret=process_video_frame(video, f, wv,0);
1009             framesin++;
1010           }else{
1011             ret=process_video_frame(yuv12, f, wv,1);
1012             framesin++;
1013           }
1014
1015           if(ret<0)return(0);
1016           if(ret>0){
1017             buftemptail+=ret;
1018             return(ret);
1019           }
1020         }
1021
1022       }else{
1023         buftemptail=buftemphead-130;
1024       }
1025     }
1026   }
1027   return(0);
1028 }
1029
1030 /* writes a wav header without the length set.  This is also the 32
1031    bit WAV header variety... please please please let the mjpeg tools
1032    ignore the length... */
1033 void PutNumLE(long num,FILE *f,int bytes){
1034   int i=0;
1035   while(bytes--){
1036     if(fputc((num>>(i<<3))&0xff,f)==EOF){
1037       fprintf(stderr,"Unable to write output: %s\n",strerror(ferror(f)));
1038       exit(1);
1039     }
1040     i++;
1041   }
1042 }
1043
1044 void WriteWav(FILE *f,long channels,long rate,long bits){
1045   fprintf(f,"RIFF");
1046   PutNumLE(0x7fffffffUL,f,4);
1047   fprintf(f,"WAVEfmt ");
1048   PutNumLE(16,f,4);
1049   PutNumLE(1,f,2);
1050   PutNumLE(channels,f,2);
1051   PutNumLE(rate,f,4);
1052   PutNumLE(rate*channels*((bits-1)/8+1),f,4);
1053   PutNumLE(((bits-1)/8+1)*channels,f,2);
1054   PutNumLE(bits,f,2);
1055   fprintf(f,"data");
1056   PutNumLE(0x7fffffffUL,f,4);
1057 }
1058
1059 void WriteYuv(FILE *f,int w,int h,int fpscode){
1060   fprintf(f,"YUV4MPEG %d %d %d\n",w,h,fpscode);
1061 }
1062
1063 static int frameratesn[]={
1064   0,   24000,  24,  25,  30000,  30,  50,  60000,  60 };
1065 static int frameratesd[]={
1066   0.,   1001,   1,   1,   1001,   1,   1,   1001,   1 };
1067
1068 void WriteYuv2(FILE *f,int w,int h,int fpscode){
1069   fprintf(f,"YUV4MPEG2 W%d H%d F%d:%d Ip A1:1\n",w,h,
1070           frameratesn[fpscode],frameratesd[fpscode]);
1071 }
1072
1073 /* YV12 aka 4:2:0 planar */
1074 void YUVout(unsigned char *buf,FILE *f){
1075   fprintf(f,"FRAME\n");
1076   fwrite(buf,1,vidbuf_width*vidbuf_height*3/2,f);
1077 }
1078
1079 static int begun;
1080 static int synced;
1081 static int drain;
1082 static int header;
1083 int ratecode;
1084 int video_timeahead;
1085
1086 int snatch_iterator(FILE *in,FILE *out,int process_audio,int process_video){
1087   if(!header){
1088     if(read_snatch_header(in)){
1089
1090       if(process_audio && !audio_p){
1091         fprintf(stderr,"No audio in this stream\n");
1092         exit(1);
1093       }
1094       if(process_video && !video_p){
1095         fprintf(stderr,"No video in this stream\n");
1096         exit(1);
1097       }
1098
1099       header=1;
1100       return(0);
1101     }
1102     return(1);
1103   }
1104
1105   if(!drain){
1106     int ret=read_snatch_frame(in,process_audio,process_video);
1107
1108     if(ret==0)drain=1;
1109   }
1110
1111   if(audio_p && video_p){
1112     if(!synced){
1113       /* pad beginning of stream for sync */
1114       if(vidbuf_head-vidbuf_tail && audbuf_head-audbuf_tail){
1115         double time_dif=vidbuf_zerotime-audbuf_zerotime;
1116         long frames=0;
1117         long samples=0;
1118         int i;
1119
1120         if(!begun){
1121           if(process_audio)
1122             WriteWav(out,audbuf_channels,audbuf_rate,16);
1123           if(process_video==2)
1124             WriteYuv2(out,vidbuf_width,vidbuf_height,ratecode);
1125           if(process_video==1)
1126             WriteYuv(out,vidbuf_width,vidbuf_height,ratecode);
1127           begun=1;
1128         }
1129
1130         /* we don't write frames/samples here; we queue new ones out
1131            ahead until everything's even */
1132
1133         if(time_dif>0){
1134           /* audio started first; prestretch video, then repad with
1135              audio */
1136           frames=ceil(time_dif*vidin_fps);
1137           time_dif-=(frames/vidin_fps);
1138           vidbuf_zerotime-=(frames/vidin_fps);
1139           for(i=vidbuf_tail+1;i<vidbuf_head;i++)
1140             vidbuf_frameno[i]+=frames;
1141           framesmissing+=frames;
1142         }
1143
1144         samples= -time_dif*audbuf_rate;
1145         samplesmissing+=samples;
1146
1147         pre_buffer_audio(samples*audbuf_channels,process_audio);
1148         audbuf_zerotime=vidbuf_zerotime;
1149
1150         synced=1;
1151       }else{
1152         if(drain){
1153           /* that was short; either no vid or no audio */
1154           if(vidbuf_head-vidbuf_tail==0)
1155             fprintf(stderr,"Audio/Video stream contained no video.\n");
1156           if(audbuf_head-audbuf_tail==0)
1157             fprintf(stderr,"Audio/Video stream contained no audio.\n");
1158           return 1;
1159         }
1160       }
1161       return 0;
1162     }
1163   }else{
1164     if(!begun){
1165       if(process_audio)
1166         WriteWav(out,audbuf_channels,audbuf_rate,16);
1167       if(process_video==2)
1168         WriteYuv2(out,vidbuf_width,vidbuf_height,ratecode);
1169       if(process_video==1)
1170         WriteYuv(out,vidbuf_width,vidbuf_height,ratecode);
1171       begun=1;
1172     }
1173   }
1174
1175   if(drain){
1176     int i;
1177
1178     /* write out all pending audio/video */
1179
1180     if(video_p){
1181       for(i=vidbuf_tail;i<vidbuf_head;i++){
1182         int oframes=(i+1<vidbuf_head)?
1183           vidbuf_frameno[i+1]*vidout_fps/vidin_fps-
1184           vidbuf_frameno[i]  *vidout_fps/vidin_fps:
1185           vidout_fps/vidin_fps;
1186         int iframes=(i+1<vidbuf_head)?
1187           vidbuf_frameno[i+1]-
1188           vidbuf_frameno[i]  :
1189           1;
1190
1191         framesout+=oframes;
1192         framesmissing+=iframes-1;
1193
1194         if(process_video)
1195           while(oframes--)
1196             YUVout(vidbuf[i],out);
1197       }
1198     }
1199
1200     if(audio_p){
1201       long samples=audbuf_head-audbuf_tail;
1202       samplesout+=samples/audbuf_channels;
1203       if(process_audio)
1204         fwrite(audbuf+audbuf_tail,2,samples,out);
1205     }
1206     return 1;
1207   }
1208
1209   if(video_p && process_video){
1210     while(vidbuf_head-vidbuf_tail>video_timeahead){
1211       int oframes=
1212         vidbuf_frameno[vidbuf_tail+1]*vidout_fps/vidin_fps-
1213         vidbuf_frameno[vidbuf_tail]  *vidout_fps/vidin_fps;
1214       int iframes=
1215         vidbuf_frameno[vidbuf_tail+1]-
1216         vidbuf_frameno[vidbuf_tail]  ;
1217       framesout+=oframes;
1218       framesmissing+=iframes-1;
1219
1220       if(process_video)
1221         while(oframes--)
1222           YUVout(vidbuf[vidbuf_tail],out);
1223
1224       vidbuf_tail++;
1225     }
1226   }
1227
1228   if(audio_p && process_audio){
1229     if(audbuf_head-audbuf_tail){
1230       long samples=audbuf_head-audbuf_tail;
1231       if(process_audio)
1232         fwrite(audbuf+audbuf_tail,2,samples,out);
1233       samplesout+=samples/audbuf_channels;
1234       audbuf_tail+=samples;
1235     }
1236   }
1237
1238   return(0);
1239 }
1240
1241
1242
1243