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