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 2, 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>
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
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
41 #define tcPrintf //printf
44 #define ePrintf printf
46 gr_align_on_samplenumbers_ss_sptr
47 gr_make_align_on_samplenumbers_ss (int nchan, int align_interval)
49 return gr_align_on_samplenumbers_ss_sptr (new gr_align_on_samplenumbers_ss (nchan,align_interval));
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),
60 if (d_align_interval<0)
61 set_output_multiple (d_nchan*2);
64 set_output_multiple (d_align_interval*d_nchan*2);
69 gr_align_on_samplenumbers_ss::~gr_align_on_samplenumbers_ss()
74 gr_align_on_samplenumbers_ss::forecast (int noutput_items, gr_vector_int &ninput_items_required)
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
83 gr_align_on_samplenumbers_ss::check_topology (int ninputs, int noutputs)
89 ePrintf("gr_align_on_samplenumbers_ss: ERROR noutputs %i != ninputs %i\n",noutputs,ninputs);
94 ePrintf("gr_align_on_samplenumbers_ss: ERROR nchan %i<2 \n",d_nchan);
96 if((int)d_ninputs!=ninputs)
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);
102 for(unsigned int i=0;i<d_ninputs;i++)
104 d_state[i].sync_found=false;
105 d_state[i].sync_end_found=false;
112 #ifdef ALIGN_ADVANCED_IMPLEMENTATION
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)
119 #ifdef DEBUG_TOCONSUME
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();
133 int align_interval=d_align_interval*2*d_nchan;
134 if(d_align_interval<0)
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);
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++)
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);
150 for(int j=0;j<noutput_items-align_interval+1;j+=align_interval)
153 if(min_ninput_items>=align_interval)
154 common_end=align_interval;
157 common_end=min_ninput_items/(d_nchan*2);
158 common_end=common_end*(d_nchan*2);
160 if (common_end<=0) break;
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++)
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");
181 ePrintf("Error: counter not continuous.\n ucounter_begin[%i]=%i +1 != ucounter_begin2=%i\n",i,d_state[i].ucounter_begin,ucounter_begin2);
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)
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);
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)))
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)
202 #ifdef DEBUG_TOCONSUME
203 printf("presync START with %i\n",i);
205 for(unsigned int k=0;k<ninputs;k++)
207 d_state[k].sync_found=false;
208 d_state[i].sync_end_found=false;
211 d_state[i].sync_found=true;
215 #ifdef DEBUG_TOCONSUME
216 if(d_state[i].sync_found)
217 printf("presync CONTINUE with %i\n",i);
219 printf("presync NEXT with %i\n",i);
221 d_state[i].sync_found=true;
222 d_state[i].sync_end_found=false;
226 if(d_in_presync && d_state[i].sync_found)
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;
235 for(unsigned int k=0;k<ninputs;k++)
237 d_state[k].sync_found=false;
238 d_state[i].sync_end_found=false;
240 #ifdef DEBUG_TOCONSUME
241 printf("presync END\n");
247 if(d_in_presync || all_diffs_zero)
249 for(unsigned int i=0;i<ninputs;i++)
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
258 tcPrintf("common_end %i\n",common_end);
261 //min_ninput_items-=common_end;
262 noutput_items_produced+=common_end;
267 for(unsigned int i=0;i<ninputs;i++)
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;
278 //if(toconsume_counter>10)
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]);
284 //printf("toconsume[%i]=%i\n",i,toconsume);
285 //consume(i,toconsume);//skip the difference in samplenumber items
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);
296 return noutput_items_produced;
300 #else /*ALIGN_ADVANCED_IMPLEMENTATION*/
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)
307 #ifdef DEBUG_TOCONSUME
317 const size_t item_size = output_signature()->sizeof_stream_item (0);
318 const unsigned ninputs = input_items.size();
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++)
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)
336 #ifdef DEBUG_TOCONSUME
337 ePrintf("SYNC counters are 0\n");
342 ePrintf("Error: counter not continuous.\n ucounter_begin[%i]=%i +1 != ucounter_begin2=%i\n",i,d_state[i].ucounter_begin,ucounter_begin2);
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);
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)
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]);
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++)
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)
378 //samples_lost_or_syncstart=true;
379 printf("Us%i %i %i ",i,d_state[i].diff_end,d_state[i].diff);
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)))
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)));
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])
394 printf("sync found on input %i at position %i \n",i,expected_sync_position);
395 //sync_start[i]=expected_sync_position;
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]);
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);
408 for(unsigned int i=0;i<ninputs;i++)
409 if((d_state[i].diff_end+diff_comp_end_max) >0x4000000)
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);
418 if(sync_found || all_diffs_zero || problem)
420 #ifdef DEBUG_TOCONSUME
421 if(all_diffs_zero) tcPrintf("ZERO\n");
422 if(sync_found) tcPrintf("SYNC\n");
424 for(unsigned int i=0;i<ninputs;i++)
426 memcpy(output_items[i],input_items[i],common_end*item_size);
427 consume(i,common_end);
428 #ifdef DEBUG_TOCONSUME
430 tcPrintf("common_end %i\n",common_end);
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++)
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;
447 //if(toconsume_counter>10)
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]);
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);
459 return -1;//Should never come here
461 #endif /*ALIGN_ADVANCED_IMPLEMENTATION*/