git.gag.com Git - debian/gnuradio/blob - gnuradio-core/src/lib/general/gr_align_on_samplenumbers_ss.cc

   1 /* -*- c++ -*- */
   2 /*
   3  * Copyright 2005 Free Software Foundation, Inc.
   4  *
   5  * This file is part of GNU Radio
   6  *
   7  * GNU Radio is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License as published by
   9  * the Free Software Foundation; either version 2, or (at your option)
  10  * any later version.
  11  *
  12  * GNU Radio is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License
  18  * along with GNU Radio; see the file COPYING.  If not, write to
  19  * the Free Software Foundation, Inc., 51 Franklin Street,
  20  * Boston, MA 02110-1301, USA.
  21  */
  22
  23 #ifdef HAVE_CONFIG_H
  24 #include "config.h"
  25 #endif
  26
  27 #include <gr_align_on_samplenumbers_ss.h>
  28 #include <gr_io_signature.h>
  29 #include <assert.h>
  30 #include <stdexcept>
  31
  32 //define ALIGN_ADVANCED_IMPLEMENTATION to have an alternative implementation of the align algoritm which exactly follows the align_interval spec.
  33 //It is more resource intensive, less tested and probably not needed
  34 //define ALIGN_ADVANCED_IMPLEMENTATION
  35
  36 //define DEBUG_TOCONSUME to see debug messages about the synchronisation part of this block
  37 //define DEBUG_TOCONSUME
  38 #ifdef DEBUG_TOCONSUME
  39 #define tcPrintf if(dprint) printf
  40 #else
  41 #define tcPrintf //printf
  42 #endif
  43
  44 #define ePrintf printf
  45
  46 gr_align_on_samplenumbers_ss_sptr
  47 gr_make_align_on_samplenumbers_ss (int nchan, int align_interval)
  48 {
  49   return gr_align_on_samplenumbers_ss_sptr (new gr_align_on_samplenumbers_ss (nchan,align_interval));
  50 }
  51
  52 gr_align_on_samplenumbers_ss::gr_align_on_samplenumbers_ss (int nchan,int align_interval)
  53   : gr_block ("align_on_samplenumbers_ss",
  54         gr_make_io_signature (2, -1, sizeof (short)), //2, -1
  55         gr_make_io_signature (2, -1, sizeof (short))), //2,-1
  56   d_align_interval (align_interval),
  57   d_nchan(nchan),
  58   d_ninputs(0)
  59 {
  60   if (d_align_interval<0)
  61     set_output_multiple (d_nchan*2);
  62   else
  63   {
  64     set_output_multiple (d_align_interval*d_nchan*2);
  65   }
  66
  67 }
  68
  69 gr_align_on_samplenumbers_ss::~gr_align_on_samplenumbers_ss()
  70 {
  71
  72 }
  73 void
  74 gr_align_on_samplenumbers_ss::forecast (int noutput_items, gr_vector_int &ninput_items_required)
  75 {
  76   //assert (0 == noutput_items % d_align_interval);
  77   unsigned ninputs = ninput_items_required.size();
  78   for (unsigned int i = 0; i < ninputs; i++)
  79     ninput_items_required[i] = std::max(noutput_items*d_nchan*2+ history() - 1,1024*d_nchan*2+ history() - 1);//TODO include the diffs found in determine input_items_required
  80 }
  81
  82 bool
  83 gr_align_on_samplenumbers_ss::check_topology (int ninputs, int noutputs)
  84 {
  85   bool result=true;
  86   if(noutputs!=ninputs)
  87   {
  88     result=false;
  89     ePrintf("gr_align_on_samplenumbers_ss: ERROR noutputs %i != ninputs %i\n",noutputs,ninputs);
  90   }
  91   if(d_nchan<2)
  92   {
  93     result=false;
  94     ePrintf("gr_align_on_samplenumbers_ss: ERROR nchan %i<2 \n",d_nchan);
  95   }
  96   if((int)d_ninputs!=ninputs)
  97   {
  98     //Only resize and reset the status if there really changed something
  99     //Don't reset the status if the user just called stop() and start(), although maybe we should.
 100     d_state.resize(ninputs);
 101     d_ninputs=ninputs;
 102     for(unsigned int i=0;i<d_ninputs;i++)
 103     {
 104       d_state[i].sync_found=false;
 105       d_state[i].sync_end_found=false;
 106     }
 107     d_in_presync=false;
 108   }
 109   return result;
 110 }
 111
 112 #ifdef ALIGN_ADVANCED_IMPLEMENTATION
 113 int
 114 gr_align_on_samplenumbers_ss::general_work (int noutput_items,
 115         gr_vector_int &ninput_items,
 116         gr_vector_const_void_star &input_items,
 117         gr_vector_void_star &output_items)
 118 {
 119 #ifdef DEBUG_TOCONSUME
 120   static int dcount=0;
 121   bool dprint=false;
 122   dcount++;
 123   if(dcount>200)
 124   {
 125     dcount=0;
 126     dprint=true;
 127   }
 128 #endif
 129   const size_t item_size = output_signature()->sizeof_stream_item (0);
 130   const unsigned ninputs = input_items.size();
 131   const unsigned noutputs = output_items.size();
 132
 133   int align_interval=d_align_interval*2*d_nchan;
 134   if(d_align_interval<0)
 135   {
 136     //align once per noutput_items
 137     align_interval=noutput_items;
 138     align_interval=align_interval/(2*d_nchan);
 139     align_interval=align_interval*(2*d_nchan);
 140   }
 141
 142   int min_ninput_items=noutput_items;//numeric_limits<int>::max();
 143   int noutput_items_produced=0;
 144   for(unsigned int i=0;i<ninputs;i++)
 145   {
 146     d_state[i].ninput_items=ninput_items[i];
 147     d_state[i].ninput_items_used=0;
 148     min_ninput_items=std::min(ninput_items[i],min_ninput_items);
 149   }
 150   for(int j=0;j<noutput_items-align_interval+1;j+=align_interval)
 151   {
 152     int common_end;
 153     if(min_ninput_items>=align_interval)
 154       common_end=align_interval;
 155     else
 156     {
 157       common_end=min_ninput_items/(d_nchan*2);
 158       common_end=common_end*(d_nchan*2);
 159     }
 160     if (common_end<=0) break;
 161
 162     bool all_diffs_zero=true;
 163     //bool sync_found=false;
 164     int diff_comp_end_max=0;
 165     for(unsigned int i=0;i<ninputs;i++)
 166     {
 167       unsigned short * uin=&(((unsigned short*)input_items[i])[d_state[i].ninput_items_used]);
 168       unsigned int  x_high16bits = uin[0];
 169       unsigned int  x_low16bits = uin[1];
 170       d_state[i].ucounter_begin = x_high16bits<<16 | x_low16bits;
 171       d_state[i].diff=d_state[0].ucounter_begin-d_state[i].ucounter_begin;//Result is a signed value,Will wrap around on 32 bit boundary
 172       int common_last=std::max(common_end-d_nchan*2,0);
 173       x_high16bits = uin[d_nchan*2];
 174       x_low16bits = uin[d_nchan*2+1];
 175       unsigned int ucounter_begin2 = x_high16bits<<16 | x_low16bits;
 176 #ifdef DEBUG_TOCONSUME
 177       if((d_state[i].ucounter_begin+1)!=(ucounter_begin2))
 178         if(ucounter_begin2==0)
 179           ePrintf("SYNC counters are 0\n");
 180         else
 181            ePrintf("Error: counter not continuous.\n ucounter_begin[%i]=%i +1 !=  ucounter_begin2=%i\n",i,d_state[i].ucounter_begin,ucounter_begin2);
 182 #endif
 183       x_high16bits = uin[common_last];
 184       x_low16bits = uin[common_last+1];
 185       d_state[i].ucounter_end = x_high16bits<<16 | x_low16bits;
 186       d_state[i].diff_end=d_state[0].ucounter_end-d_state[i].ucounter_end;//Result is a signed value,Will wrap around on 32 bit boundary
 187       d_state[i].diff_comp_end=d_state[i].ucounter_end-d_state[0].ucounter_end;
 188       diff_comp_end_max=std::max(d_state[i].diff_comp_end,diff_comp_end_max);
 189 #ifdef DEBUG_TOCONSUME
 190       if(d_state[i].diff>256000000 || d_state[i].diff_end>256000000 || d_state[i].diff_comp_end>256000000)
 191       {
 192         tcPrintf("diff[%i]=%i diff_end=%i diff_comp_end=%i\n",i,d_state[i].diff,d_state[i].diff_end,d_state[i].diff_comp_end);
 193       }
 194 #endif
 195       all_diffs_zero=all_diffs_zero && (0==d_state[i].diff_end);
 196       if(d_state[i].ucounter_end<d_state[i].ucounter_begin+(unsigned)(common_last/(d_nchan*2))) //(unsigned)(common_last/(d_nchan*2)))
 197       {
 198         //printf("sync 1 ");// found ucounter_end[%i]=%i ucounter_begin[%i]=%i \n",i,d_state[i].ucounter_end,i,d_state[i].ucounter_begin);
 199         //sync_found=true;//sync_found or 32 bit counter  wraparound (0xffffffff -> 0x00000000)
 200         if(!d_in_presync)
 201         {
 202 #ifdef DEBUG_TOCONSUME
 203           printf("presync START with %i\n",i);
 204 #endif
 205          for(unsigned int k=0;k<ninputs;k++)
 206          {
 207           d_state[k].sync_found=false;
 208           d_state[i].sync_end_found=false;
 209          }
 210          d_in_presync=true;
 211          d_state[i].sync_found=true;
 212         } else
 213         {
 214           //d_in_presync=true;
 215 #ifdef DEBUG_TOCONSUME
 216           if(d_state[i].sync_found)
 217             printf("presync CONTINUE with %i\n",i);
 218           else
 219             printf("presync NEXT with %i\n",i);
 220 #endif
 221           d_state[i].sync_found=true;
 222           d_state[i].sync_end_found=false;
 223         }
 224       } else
 225       {
 226         if(d_in_presync && d_state[i].sync_found)
 227         {
 228           d_state[i].sync_end_found=true;
 229           bool all_syncs_found=true;
 230           for(unsigned int k=0;k<ninputs;k++)
 231             all_syncs_found=all_syncs_found && d_state[k].sync_end_found;
 232           d_in_presync=!all_syncs_found;
 233           if(!d_in_presync)
 234           {
 235             for(unsigned int k=0;k<ninputs;k++)
 236             {
 237               d_state[k].sync_found=false;
 238               d_state[i].sync_end_found=false;
 239             }
 240 #ifdef DEBUG_TOCONSUME
 241             printf("presync END\n");
 242 #endif
 243           }
 244         }
 245       }
 246     }
 247     if(d_in_presync || all_diffs_zero)
 248     {
 249       for(unsigned int i=0;i<ninputs;i++)
 250       {
 251          memcpy(&(((unsigned short*)output_items[i])[j]),&(((const unsigned short*)input_items[i])[d_state[i].ninput_items_used]),common_end*item_size);
 252          //consume(i,common_end);
 253          d_state[i].ninput_items-=common_end;
 254          d_state[i].ninput_items_used+=common_end;
 255          min_ninput_items=std::min(d_state[i].ninput_items,min_ninput_items);
 256 #ifdef DEBUG_TOCONSUME
 257          if(common_end<256)
 258            tcPrintf("common_end %i\n",common_end);
 259 #endif
 260       }
 261       //min_ninput_items-=common_end;
 262       noutput_items_produced+=common_end;
 263       //return common_end;
 264     } else
 265     {
 266       //printf("sync 2");
 267       for(unsigned int i=0;i<ninputs;i++)
 268       {
 269         int toconsume=std::min((d_state[i].diff_end+diff_comp_end_max)*d_nchan*2,d_state[i].ninput_items);
 270         toconsume=toconsume/(d_nchan*2);
 271         toconsume=toconsume*(d_nchan*2);
 272         d_state[i].ninput_items-=toconsume;
 273         d_state[i].ninput_items_used+=toconsume;
 274         min_ninput_items=std::min(d_state[i].ninput_items,min_ninput_items);
 275 #ifdef DEBUG_TOCONSUME
 276       static int toconsume_counter=0;
 277       toconsume_counter++;
 278       //if(toconsume_counter>10)
 279       {
 280         tcPrintf("toconsume=%i diff_end[%i]*d_nchan*2=%i diff_comp_end_max*d_nchan*2=%i ninput_items[%i]=%i\n",toconsume,i,d_state[i].diff_end*d_nchan*2,diff_comp_end_max*d_nchan*2,i,ninput_items[i]);
 281         toconsume_counter=0;
 282       }
 283 #endif
 284         //printf("toconsume[%i]=%i\n",i,toconsume);
 285         //consume(i,toconsume);//skip the difference in samplenumber items
 286       }
 287       //return 0;
 288     }
 289   }
 290   for(unsigned int i=0;i<ninputs;i++)
 291     consume(i,d_state[i].ninput_items_used);
 292 #ifdef DEBUG_TOCONSUME
 293   if(noutput_items_produced<256)
 294     tcPrintf("noutput_items_produced %i\n",noutput_items_produced);
 295 #endif
 296   return noutput_items_produced;
 297 }
 298
 299
 300 #else /*ALIGN_ADVANCED_IMPLEMENTATION*/
 301 int
 302 gr_align_on_samplenumbers_ss::general_work (int noutput_items,
 303         gr_vector_int &ninput_items,
 304         gr_vector_const_void_star &input_items,
 305         gr_vector_void_star &output_items)
 306 {
 307 #ifdef DEBUG_TOCONSUME
 308   static int dcount=0;
 309   bool dprint=false;
 310   dcount++;
 311   if(dcount>2000)
 312   {
 313     dcount=0;
 314     dprint=true;
 315   }
 316 #endif
 317   const size_t item_size = output_signature()->sizeof_stream_item (0);
 318   const unsigned ninputs = input_items.size();
 319
 320   int common_end=noutput_items;
 321   //int diff_min=INT_MAX;
 322   //int diff_max=INT_MIN;
 323   for(unsigned int i=0;i<ninputs;i++)
 324   {
 325     unsigned short * uin=(unsigned short*)input_items[i];
 326     unsigned int  x_high16bits = uin[0];
 327     unsigned int  x_low16bits = uin[1];
 328     d_state[i].ucounter_begin = x_high16bits<<16 | x_low16bits;
 329     d_state[i].diff=d_state[0].ucounter_end-d_state[i].ucounter_end;//Result is a signed value,Will wrap around on 32 bit boundary
 330     x_high16bits = uin[d_nchan*2];
 331     x_low16bits = uin[d_nchan*2+1];
 332     unsigned int ucounter_begin2 = x_high16bits<<16 | x_low16bits;
 333     if((d_state[i].ucounter_begin+1)!=(ucounter_begin2))
 334       if(ucounter_begin2==0)
 335       {
 336 #ifdef DEBUG_TOCONSUME
 337         ePrintf("SYNC counters are 0\n");
 338 #endif
 339       }
 340       else
 341       {
 342         ePrintf("Error: counter not continuous.\n ucounter_begin[%i]=%i +1 !=  ucounter_begin2=%i\n",i,d_state[i].ucounter_begin,ucounter_begin2);
 343       }
 344
 345     //diff_comp[i]=ucounter[i]-ucounter[0];
 346     //diff_min=std::min(diff[i],diff_min);
 347     //diff_max=std::max(diff[i],diff_max);
 348     common_end=std::max(std::min(ninput_items[i],common_end),0);
 349   }
 350   common_end=common_end/(d_nchan*2);
 351   common_end=common_end*(d_nchan*2);
 352 #ifdef DEBUG_TOCONSUME
 353   if(common_end<d_nchan*2)
 354   {
 355     printf(" common_end %i\n",common_end);
 356     for(int j=0;j<ninputs;j++)
 357       printf("ninput_items[%i]=%i\n",j,ninput_items[j]);
 358   }
 359 #endif
 360   bool all_diffs_zero=true;
 361   bool sync_found=false;
 362   int diff_comp_end_max=0;
 363   for(unsigned int i=0;i<ninputs;i++)
 364   {
 365     unsigned short * uin=(unsigned short*)input_items[i];
 366     int common_last=common_end-(d_nchan*2);
 367     unsigned int  x_high16bits = uin[common_last];
 368     unsigned int  x_low16bits = uin[common_last+1];
 369     d_state[i].ucounter_end = x_high16bits<<16 | x_low16bits;
 370     d_state[i].diff_end=d_state[0].ucounter_end-d_state[i].ucounter_end;//Result is a signed value,Will wrap around on 32 bit boundary
 371     d_state[i].diff_comp_end=d_state[i].ucounter_end-d_state[0].ucounter_end;
 372     //diff_end_min=std::min(diff_end[i],diff_end_min);
 373     //diff_end_max=std::max(diff_end[i],diff_end_max);
 374     diff_comp_end_max=std::max(d_state[i].diff_comp_end,diff_comp_end_max);
 375 #ifdef DEBUG_TOCONSUME
 376     if(d_state[i].diff_end!=d_state[i].diff)
 377     {
 378       //samples_lost_or_syncstart=true;
 379       printf("Us%i %i %i ",i,d_state[i].diff_end,d_state[i].diff);
 380     }
 381 #endif
 382     all_diffs_zero=all_diffs_zero && (0==d_state[i].diff_end);
 383     if((d_state[i].ucounter_end<d_state[i].ucounter_begin+(unsigned)(common_last/(d_nchan*2))) || (0==d_state[i].ucounter_end) || (0==d_state[i].ucounter_begin)) //(unsigned)(common_last/(d_nchan*2)))
 384     {
 385       sync_found=true;//sync_found or 32 bit counter  wraparound (0xffffffff -> 0x00000000)
 386 #ifdef DEBUG_TOCONSUME
 387       tcPrintf("SYNC diff_end[%i]=%i ucounter_end[%i]=%i ucounter_begin[%i]=%i \n",i,d_state[i].diff_end,i,d_state[i].ucounter_end,i,d_state[i].ucounter_begin);
 388       tcPrintf("ucounter_end=%i < %i = ucounter_begin+(unsigned)(common_last/(d_nchan*2) \n",d_state[i].ucounter_end,d_state[i].ucounter_begin+(unsigned)(common_last/(d_nchan*2)));
 389
 390       printf("ucounter_end[%i]=%i ucounter_begin[%i]=%i\n",i,d_state[i].ucounter_end,i,d_state[i].ucounter_begin);
 391       int expected_sync_position=common_last - d_state[i].ucounter_end*(d_nchan*2);
 392       if(0==uin[expected_sync_position] && 0==uin[expected_sync_position+1])
 393       {
 394         printf("sync found on input %i at position %i \n",i,expected_sync_position);
 395         //sync_start[i]=expected_sync_position;
 396       } else
 397       {
 398         printf("sync found on input %i position unclear, expected at %i value there %i\n",i,expected_sync_position,uin[expected_sync_position]<<16 | uin[expected_sync_position+1]);
 399         //sync_start[i]=-1;
 400       }
 401     } else
 402     {
 403       tcPrintf("NOsync diff_end[%i]=%i ucounter_end[%i]=%i ucounter_begin[%i]=%i \n",i,d_state[i].diff_end,i,d_state[i].ucounter_end,i,d_state[i].ucounter_begin);
 404 #endif
 405     }
 406   }
 407   bool problem=false;
 408   for(unsigned int i=0;i<ninputs;i++)
 409     if((d_state[i].diff_end+diff_comp_end_max) >0x4000000)
 410       {
 411         problem=true;
 412         ePrintf("Warning: counter diff greater as 64 Million\n");
 413         ePrintf("         You might want to swap master and slave.\n");
 414         ePrintf("          i=%i,d_state[i].diff_end+diff_comp_end_max=%i,d_state[i].diff_end=%i,diff_comp_end_max=%i,ucounter[i]=%i,ucounter[0]=%i\n",
 415                           i,d_state[i].diff_end+diff_comp_end_max,d_state[i].diff_end,diff_comp_end_max,d_state[i].ucounter_end,d_state[0].ucounter_end);
 416         //ePrintf("        toconsume=%i\n",toconsume);
 417       }
 418   if(sync_found || all_diffs_zero || problem)
 419   {
 420 #ifdef DEBUG_TOCONSUME
 421     if(all_diffs_zero) tcPrintf("ZERO\n");
 422     if(sync_found) tcPrintf("SYNC\n");
 423 #endif
 424     for(unsigned int i=0;i<ninputs;i++)
 425     {
 426        memcpy(output_items[i],input_items[i],common_end*item_size);
 427        consume(i,common_end);
 428 #ifdef DEBUG_TOCONSUME
 429       if(common_end<256)
 430         tcPrintf("common_end %i\n",common_end);
 431 #endif
 432     }
 433     return common_end;
 434   } else
 435   {
 436     //int minconsume=0;//common_end/(2*d_nchan*2);
 437     //min_consume=min_consume*d_nchan*2;
 438     for(unsigned int i=0;i<ninputs;i++)
 439     {
 440       int toconsume=std::min((d_state[i].diff_end+diff_comp_end_max)*d_nchan*2,ninput_items[i]);
 441       toconsume=toconsume/(d_nchan*2);
 442       toconsume=toconsume*(d_nchan*2);
 443 #ifdef DEBUG_TOCONSUME
 444       //printf("dcount %i\n",dcount);
 445       static int toconsume_counter=0;
 446       toconsume_counter++;
 447       //if(toconsume_counter>10)
 448       {
 449         tcPrintf("toconsume=%i diff_end[[%i]*d_nchan*2=%i diff_comp_end_max)*d_nchan*2=%i ninput_items[%i]=%i\n",
 450                   toconsume,i,d_state[i].diff_end*d_nchan*2,diff_comp_end_max*d_nchan*2,i,ninput_items[i]);
 451         toconsume_counter=0;
 452       }
 453 #endif
 454       consume(i,toconsume);//skip the difference in samplenumber items
 455       //printf("toconsume%i %i diff_comp_end_max %i diff_end[[%i] %i\n",i,toconsume,diff_comp_end_max,i,d_state[i].diff_end);
 456     }
 457     return 0;
 458   }
 459   return -1;//Should never come here
 460 }
 461 #endif /*ALIGN_ADVANCED_IMPLEMENTATION*/