3 * Copyright 2005 Free Software Foundation, Inc.
5 * This file is part of GNU Radio
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)
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.
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.
27 #include <gr_align_on_samplenumbers_ss.h>
28 #include <gr_io_signature.h>
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
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
43 #define tcPrintf //printf
46 #define ePrintf printf
48 gr_align_on_samplenumbers_ss_sptr
49 gr_make_align_on_samplenumbers_ss (int nchan, int align_interval)
51 return gr_align_on_samplenumbers_ss_sptr (new gr_align_on_samplenumbers_ss (nchan,align_interval));
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),
62 if (d_align_interval<0)
63 set_output_multiple (d_nchan*2);
66 set_output_multiple (d_align_interval*d_nchan*2);
71 gr_align_on_samplenumbers_ss::~gr_align_on_samplenumbers_ss()
76 gr_align_on_samplenumbers_ss::forecast (int noutput_items, gr_vector_int &ninput_items_required)
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
85 gr_align_on_samplenumbers_ss::check_topology (int ninputs, int noutputs)
91 ePrintf("gr_align_on_samplenumbers_ss: ERROR noutputs %i != ninputs %i\n",noutputs,ninputs);
96 ePrintf("gr_align_on_samplenumbers_ss: ERROR nchan %i<2 \n",d_nchan);
98 if((int)d_ninputs!=ninputs)
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);
104 for(unsigned int i=0;i<d_ninputs;i++)
106 d_state[i].sync_found=false;
107 d_state[i].sync_end_found=false;
114 #ifdef ALIGN_ADVANCED_IMPLEMENTATION
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)
121 #ifdef DEBUG_TOCONSUME
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();
135 int align_interval=d_align_interval*2*d_nchan;
136 if(d_align_interval<0)
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);
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++)
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);
152 for(int j=0;j<noutput_items-align_interval+1;j+=align_interval)
155 if(min_ninput_items>=align_interval)
156 common_end=align_interval;
159 common_end=min_ninput_items/(d_nchan*2);
160 common_end=common_end*(d_nchan*2);
162 if (common_end<=0) break;
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++)
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");
183 ePrintf("Error: counter not continuous.\n ucounter_begin[%i]=%i +1 != ucounter_begin2=%i\n",i,d_state[i].ucounter_begin,ucounter_begin2);
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)
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);
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)))
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)
204 #ifdef DEBUG_TOCONSUME
205 printf("presync START with %i\n",i);
207 for(unsigned int k=0;k<ninputs;k++)
209 d_state[k].sync_found=false;
210 d_state[i].sync_end_found=false;
213 d_state[i].sync_found=true;
217 #ifdef DEBUG_TOCONSUME
218 if(d_state[i].sync_found)
219 printf("presync CONTINUE with %i\n",i);
221 printf("presync NEXT with %i\n",i);
223 d_state[i].sync_found=true;
224 d_state[i].sync_end_found=false;
228 if(d_in_presync && d_state[i].sync_found)
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;
237 for(unsigned int k=0;k<ninputs;k++)
239 d_state[k].sync_found=false;
240 d_state[i].sync_end_found=false;
242 #ifdef DEBUG_TOCONSUME
243 printf("presync END\n");
249 if(d_in_presync || all_diffs_zero)
251 for(unsigned int i=0;i<ninputs;i++)
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
260 tcPrintf("common_end %i\n",common_end);
263 //min_ninput_items-=common_end;
264 noutput_items_produced+=common_end;
269 for(unsigned int i=0;i<ninputs;i++)
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;
280 //if(toconsume_counter>10)
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]);
286 //printf("toconsume[%i]=%i\n",i,toconsume);
287 //consume(i,toconsume);//skip the difference in samplenumber items
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);
298 return noutput_items_produced;
302 #else /*ALIGN_ADVANCED_IMPLEMENTATION*/
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)
309 #ifdef DEBUG_TOCONSUME
319 const size_t item_size = output_signature()->sizeof_stream_item (0);
320 const unsigned ninputs = input_items.size();
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++)
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)
338 #ifdef DEBUG_TOCONSUME
339 ePrintf("SYNC counters are 0\n");
344 ePrintf("Error: counter not continuous.\n ucounter_begin[%i]=%i +1 != ucounter_begin2=%i\n",i,d_state[i].ucounter_begin,ucounter_begin2);
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);
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)
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]);
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++)
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)
381 //samples_lost_or_syncstart=true;
382 printf("Us%i %i %i ",i,d_state[i].diff_end,d_state[i].diff);
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)))
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)));
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])
397 printf("sync found on input %i at position %i \n",i,expected_sync_position);
398 //sync_start[i]=expected_sync_position;
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]);
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);
411 for(unsigned int i=0;i<ninputs;i++)
412 if((d_state[i].diff_end+diff_comp_end_max) >0x4000000)
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);
421 if(sync_found || all_diffs_zero || problem)
423 #ifdef DEBUG_TOCONSUME
424 if(all_diffs_zero) tcPrintf("ZERO\n");
425 if(sync_found) tcPrintf("SYNC\n");
427 for(unsigned int i=0;i<ninputs;i++)
429 memcpy(output_items[i],input_items[i],common_end*item_size);
430 consume(i,common_end);
431 #ifdef DEBUG_TOCONSUME
433 tcPrintf("common_end %i\n",common_end);
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++)
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;
450 //if(toconsume_counter>10)
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]);
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);
462 return -1;//Should never come here
464 #endif /*ALIGN_ADVANCED_IMPLEMENTATION*/