source: Sophya/trunk/AddOn/TAcq/brpaqu.cc@ 3630

Last change on this file since 3630 was 3630, checked in by ansari, 16 years ago

Petite correction ds comptage paquets perdus, Reza 21/05/2009
File size: 15.1 KB
Line 
1#include "brpaqu.h"
2
3
4static inline void bswap4(void* p)
5{
6 UInt32 tmp = *(UInt32*)p;
7 *(UInt32*)p = ((tmp >> 24) & 0x000000FF) |
8 ((tmp >> 8) & 0x0000FF00) |
9 ((tmp & 0x0000FF00) << 8) |
10 ((tmp & 0x000000FF) << 24);
11}
12
13/* --Methode__ */
14BRPaquet::BRPaquet(Byte* src, Byte* dst, int paqsz, BRDataFmtConv fgswap)
15 // swapall = true -> on swap tout le paquet, sinon swap entete seulement
16{
17 dst_ = dst;
18 sz_ = paqsz;
19 if ((src == NULL) || (dst == NULL)) return;
20 // Il faut mettre une protection (throw) si dst==NULL ou sz==0
21
22 UInt32* src32 = (UInt32*)src;
23 UInt32* dst32 = (UInt32*)dst;
24
25 switch ( fgswap ) {
26 case BR_DoNothing : // rien a faire
27 break;
28 case BR_Copy : // copie directe
29 memcpy(dst_, src, sz_);
30 break;
31 case BR_Swap32 : // On swappe toutes les donnees du paquet
32 // les bytes sont dans l'ordre par paquet de 4 octets (Int32) , les deux Int32 de
33 // On copie la zone donnees en faisant un byte-swap correspondant a 8 octets (4->8 Reza/firmware SGDMA)
34 for(int ka=0; ka<sz_/4; ka+=2) {
35 dst32[ka] = src32[ka+1];
36 dst32[ka+1] = src32[ka];
37 }
38
39 break;
40 case BR_SwapAll:
41 for(int ka=0; ka<sz_; ka+=8) {
42 for(int kb=0; kb<4; kb++) {
43 dst_[ka+kb] = src[ka+3-kb+4];
44 dst_[ka+kb+4] = src[ka+3-kb];
45 }
46 }
47 for(int ka=HeaderSize()+DataSize(); ka<sz_; ka+=8) {
48 for(int kb=0; kb<4; kb++) {
49 dst_[ka+kb] = src[ka+3-kb+4];
50 dst_[ka+kb+4] = src[ka+3-kb];
51 }
52 }
53 break;
54 case BR_SwapHDR :
55 case BR_FFTOneChan :
56 case BR_FFTTwoChan :
57 // Byte swap (sur 8 octets) de l'entete
58 // ByteSwap 8 (4->8 Reza/firmware SGDMA) de l'enete
59 for(int ka=0; ka<BRHDRSIZE; ka+=8) {
60 for(int kb=0; kb<4; kb++) {
61 dst_[ka+kb] = src[ka+3-kb+4];
62 dst_[ka+kb+4] = src[ka+3-kb];
63 }
64 }
65
66 // on reoordonne les coeff FFT et on swappe en mem temps (Byte-swap sur 8 octets)
67 if (fgswap == BR_FFTOneChan) ReorderFFTData(src+HeaderSize(), dst_+HeaderSize(), DataSize());
68 else if (fgswap == BR_FFTTwoChan) {
69 ReorderFFTData(src+HeaderSize(), dst_+HeaderSize(), DataSize()/2);
70 ReorderFFTData(src+HeaderSize()+DataSize()/2, dst_+HeaderSize()+DataSize()/2, DataSize()/2);
71 }
72 // Byte swap (sur 8 octets) du trailer
73 for(int ka=HeaderSize()+DataSize(); ka<sz_; ka+=8) {
74 for(int kb=0; kb<4; kb++) {
75 dst_[ka+kb] = src[ka+3-kb+4];
76 dst_[ka+kb+4] = src[ka+3-kb];
77 }
78 }
79 break;
80
81 case BR_FFTOneChan32 :
82 case BR_FFTTwoChan32 :
83 // swap du header uniquement : Echange de deux mots de 4 octets
84 for(int ka=0; ka<BRHDRSIZE/4; ka+=2) {
85 dst32[ka] = src32[ka+1];
86 dst32[ka+1] = src32[ka];
87 }
88
89 // on reoordonne et on swappe en mem temps (Echange de deux mots de 4 octets)
90 if (fgswap == BR_FFTOneChan32) ReorderFFTData32(src+HeaderSize(), dst_+HeaderSize(), DataSize());
91 else if (fgswap == BR_FFTTwoChan32) {
92 ReorderFFTData32(src+HeaderSize(), dst_+HeaderSize(), DataSize()/2);
93 ReorderFFTData32(src+HeaderSize()+DataSize()/2, dst_+HeaderSize()+DataSize()/2, DataSize()/2);
94 }
95 // swap du trailler uniquement : Echange de deux mots de 4 octets
96 for(int ka=(HeaderSize()+DataSize())/4;ka < sz_/4; ka+=2) {
97 dst32[ka] = src32[ka+1];
98 dst32[ka+1] = src32[ka];
99 }
100
101 case BR_FFTOneChanNoSwap :
102 case BR_FFTTwoChanNoSwap :
103 // copie du header uniquement
104 for(int ka=0; ka<BRHDRSIZE/4; ka++) dst32[ka] = src32[ka];
105
106 //DEL on a plus de swapdonc il faut copier dans dst
107 //DEL memcpy(dst_, src, sz_);
108
109 // on reoordonne les coeff FFT (pas de swap)
110 if (fgswap == BR_FFTOneChanNoSwap) ReorderFFTDataNoSwap(src+HeaderSize(), dst_+HeaderSize(), DataSize());
111 else if (fgswap == BR_FFTTwoChanNoSwap) {
112 ReorderFFTDataNoSwap(src+HeaderSize(), dst_+HeaderSize(), DataSize()/2);
113 ReorderFFTDataNoSwap(src+HeaderSize()+DataSize()/2, dst_+HeaderSize()+DataSize()/2, DataSize()/2);
114 }
115 // copie du trailler uniquement :
116 for(int ka=(HeaderSize()+DataSize())/4;ka < sz_/4; ka++) dst32[ka] = src32[ka];
117
118 break;
119 } // Fin switch
120
121}
122
123/* --Methode__ */
124BRPaquet::BRPaquet(Byte* srcdst, int paqsz)
125{
126 dst_ = srcdst;
127 sz_ = paqsz;
128 // Il faut mettre une protection (throw) si srcdst==NULL ou sz==0
129}
130
131
132/* --Methode__ */
133UInt16 BRPaquet::ChannelID()
134{
135
136 UInt16 ChnId=ChanId();
137 UInt16 ChpId=ChipId();
138
139
140 if (ChpId == 2)
141 {
142 if (ChnId == 1) ChnId = Ch3;
143 if (ChnId == 2) ChnId = Ch4;
144 if (ChnId == 3) ChnId = Ch3_4;
145 }
146 return(ChnId);
147}
148
149/* --Methode__ */
150UInt16 BRPaquet::ModeAcquisition()
151{
152 UInt16 ModAq;
153//DEL printf("Mod Acq %x \n",ModeAcq());
154 ModAq = ((ModeAcq() & 0x30)>> 4);
155 return(ModAq);
156}
157
158/* --Methode__ */
159void BRPaquet::SetHDRMarker64(UInt64 htag)
160{
161 *((UInt64*)(dst_+OFFSET)) = htag;
162 return;
163}
164
165/* --Methode__ */
166void BRPaquet::SetTRLMarker64(UInt64 ttag)
167{
168 *((UInt64*)(dst_+(sz_-BRTRLSIZE+OFFSET+1))) = 0;
169 *((UInt64*)(dst_+(sz_-BRTRLSIZE+OFFSET))) = ttag;
170 return;
171}
172
173/* --Methode__ */
174void BRPaquet::SetFrameCounter(UInt32 fc)
175{
176 UInt32* wp = (UInt32*)(dst_+(BRFRCPTOFF+OFFSET));
177 *wp = (*wp & 0x0000FFFF) | ((fc<<16) & 0xFFFF0000);
178 return;
179}
180
181/* --Methode__ */
182void BRPaquet::SetTimeTag(UInt64 timtag)
183{
184 UInt32* wp = (UInt32*)(dst_+(BRFRCPTOFF+OFFSET));
185 UInt32 fc = *wp;
186 *((UInt64*)(dst_+(BRTMTAGOFF+OFFSET))) = timtag;
187 *wp = (*wp & 0x0000FFFF) | (fc & 0xFFFF0000);
188 return;
189}
190
191/* --Methode__ */
192ostream& BRPaquet::Print(ostream & os, int nelt, bool prht)
193{
194 os << endl << "BRPaquet::Print() PaqSz=" << PaquetSize() << " DataSize=" << DataSize()
195 << " dst_pointer=(hex)" << hex << (unsigned long)dst_ << dec << endl;
196 if (dst_ == NULL) {
197 os << " ...NULL paquet " << endl;
198 return os;
199 }
200 os << " BR AcqMode: " << ModeAcquisition() << " Channel: " << ChannelID()
201 << " FrameCounter=" << FrameCounter() << " FrameDataLen=" << PaqLen() << endl;
202 if (TrailerSize() > 0)
203 os << " ...HDRMarker(hex)=" << hex << HDRMarker() << " TRLMarker=" << TRLMarker() << dec << endl;
204 else
205 os << " ...HDRMarker(hex)=" << hex << HDRMarker() << " NO TRLMarker=" << dec << endl;
206 UInt32 tt1, tt2;
207 tt2 = TimeTag1();
208 tt1 = TimeTag2();
209 os << " ...TimeTag (hex)=" << hex << " TT1= " << tt1 << " TT2=" << tt2
210 << dec << " ->TimeTag()=" << TimeTag() << endl;
211 // os << " ...Position Chariot (hex)= " << hex << PositionChariot() << endl;
212 if (nelt > DataSize()/2) nelt = DataSize()/2;
213 os << " ...Data[1.." << nelt << "]= ";
214
215 for(int k=0; k<nelt; k++) os << (int)(*(Data1()+k)) << " , ";
216 os << endl;
217 os << " ...Data[" << DataSize()-nelt << ".." << DataSize()-1 << "]= ";
218 for(int k=DataSize()-nelt; k<DataSize(); k++) os << (int)(*(Data1()+k)) << " , ";
219 os << endl;
220 if (prht) { // Impression header / trailer
221 UInt32* hdr = (UInt32*)Header();
222 os << " ...Header (hex):" << hex ;
223 for(int k=0; k<HeaderSize()/sizeof(UInt32); k++)
224 os << hdr[k] << " , " ;
225 os << dec << endl;
226 if (TrailerSize() > 0) {
227 UInt32* trl = (UInt32*)Trailer();
228 os << " ...Trailer (hex):" << hex ;
229 for(int k=0; k<TrailerSize()/sizeof(UInt32); k++)
230 os << trl[k] << " , " ;
231 os << dec << endl;
232 }
233 }
234 return os;
235}
236
237
238// ---------------------------------------------------------
239// **** REMARQUE N/2+1 complexes -> N/2 complexes *****
240// N = Nb d'echantillon en temps -> N/2 paires (real, imag)
241// Il y a le continu, et N/2 frequences ---> N/2+1 nombres complexes,
242// mais avec la contrainte Z(0).imag = 0 Z(N/2).imag = 0
243// f(i) i=0...N-1 ===> Z(k) ( Z complexe , k=0...N/2 )
244// mais avec la contrainte Z(0).imag = 0 Z(N/2).imag = 0
245// On peut donc tout mettre ds N/2 complexes en choisissant
246// de mettre ds Z(0).imag Z(N/2).real
247// ----------------------------------------------------------
248
249// Fonction magique qui donne le pointeur permettant de tenir compte du byte-swp sur 8 octets
250static inline int IndexByteSwap8(int idx)
251{
252 return ( (idx-(idx%8))+(7-idx%8) ) ;
253}
254
255/* --Methode__ */
256void BRPaquet::ReorderFFTData(Byte* src, Byte* dst, int N)
257{
258 // Code recopie depuis /Dev/DisplayData/HistoWindow.cc
259 // fonction TraceWind::DisplayBaoDatasFFT() et adapte aux structures BRPaquet et Cie
260 // Modif par rapport au code de Bruno : N/2 elements complexes au lieu de N/2+1 - Remarque ci-dessus
261
262 int nCoef = N / 2; // to change
263 int debutIndex = N / 4 + 1;
264 int fifoSize = N / 4 - 1;
265 int i;
266
267 TwoByteComplex* dstcmplx = (TwoByteComplex*)dst;
268
269 // cout << " Display BAO Datas FFT (" << N << ")" << " : from 0 to "<< nCoef << endl;
270 // cout << " Variables : debutIndex, fifoSize " << debutIndex << ", " << fifoSize << endl;
271
272
273 // Sortie 1
274 for (i = 0; i < fifoSize ; i++)
275 {
276 dstcmplx[debutIndex + i].realB() = src[IndexByteSwap8(2*i)];
277 dstcmplx[debutIndex + i].imagB() = src[IndexByteSwap8(2*i + 1)];
278 }
279
280 // element au milieu
281 dstcmplx[N / 4].realB() = src[IndexByteSwap8(2*fifoSize)];
282 dstcmplx[N / 4].imagB() = src[IndexByteSwap8(2*fifoSize + 1)];
283
284 // Sortie 2
285 for (i = 0; i < fifoSize ; i++)
286 {
287 dstcmplx[fifoSize - i].realB() = src[IndexByteSwap8(nCoef + 2*i)];
288 dstcmplx[fifoSize - i].imagB() = src[IndexByteSwap8(nCoef + 2*i + 1)];
289 }
290
291 // k = 0 et k = N/2
292 dstcmplx[0].realB() = src[IndexByteSwap8(N - 2)];
293 // Voir Remarque ci-dessus Z(N/2).real -> Z(0).image
294 dstcmplx[0].imagB() = src[IndexByteSwap8(N - 1)]; // Attention, on met ici la real(fmax)
295
296 return ;
297}
298
299static inline int IndexByteSwap8_32(int idx)
300{
301 return ( (idx-(idx%8))+((4+idx%8)%8) ) ;
302}
303
304void BRPaquet::ReorderFFTData32(Byte* src, Byte* dst, int N)
305{
306 // Code recopie depuis /Dev/DisplayData/HistoWindow.cc
307 // fonction TraceWind::DisplayBaoDatasFFT() et adapte aux structures BRPaquet et Cie
308 // Modif par rapport au code de Bruno : N/2 elements complexes au lieu de N/2+1 - Remarque ci-dessus
309
310 int nCoef = N / 2; // to change
311 int debutIndex = N / 4 + 1;
312 int fifoSize = N / 4 - 1;
313 int i;
314
315 TwoByteComplex* dstcmplx = (TwoByteComplex*)dst;
316
317 // cout << " Display BAO Datas FFT (" << N << ")" << " : from 0 to "<< nCoef << endl;
318 // cout << " Variables : debutIndex, fifoSize " << debutIndex << ", " << fifoSize << endl;
319
320
321 // Sortie 1
322 for (i = 0; i < fifoSize ; i++)
323 {
324 dstcmplx[debutIndex + i].realB() = src[IndexByteSwap8_32(2*i)];
325 dstcmplx[debutIndex + i].imagB() = src[IndexByteSwap8_32(2*i + 1)];
326 }
327
328 // element au milieu
329 dstcmplx[N / 4].realB() = src[IndexByteSwap8_32(2*fifoSize)];
330 dstcmplx[N / 4].imagB() = src[IndexByteSwap8_32(2*fifoSize + 1)];
331
332 // Sortie 2
333 for (i = 0; i < fifoSize ; i++)
334 {
335 dstcmplx[fifoSize - i].realB() = src[IndexByteSwap8_32(nCoef + 2*i)];
336 dstcmplx[fifoSize - i].imagB() = src[IndexByteSwap8_32(nCoef + 2*i + 1)];
337 }
338
339 // k = 0 et k = N/2
340 dstcmplx[0].realB() = src[IndexByteSwap8_32(N - 2)];
341 // Voir Remarque ci-dessus Z(N/2).real -> Z(0).image
342 dstcmplx[0].imagB() = src[IndexByteSwap8_32(N - 1)]; // Attention, on met ici la real(fmax)
343
344 return ;
345}
346void BRPaquet::ReorderFFTDataNoSwap(Byte* src, Byte* dst, int N)
347{
348 // Code recopie depuis /Dev/DisplayData/HistoWindow.cc
349 // fonction TraceWind::DisplayBaoDatasFFT() et adapte aux structures BRPaquet et Cie
350 // Modif par rapport au code de Bruno : N/2 elements complexes au lieu de N/2+1 - Remarque ci-dessus
351
352 int nCoef = N / 2; // to change
353 int debutIndex = N / 4 + 1;
354 int fifoSize = N / 4 - 1;
355 int i;
356
357 TwoByteComplex* dstcmplx = (TwoByteComplex*)dst;
358
359 // cout << " Display BAO Datas FFT (" << N << ")" << " : from 0 to "<< nCoef << endl;
360 // cout << " Variables : debutIndex, fifoSize " << debutIndex << ", " << fifoSize << endl;
361
362
363 // Sortie 1
364 for (i = 0; i < fifoSize ; i++)
365 {
366 dstcmplx[debutIndex + i].realB() = src[(2*i)];
367 dstcmplx[debutIndex + i].imagB() = src[(2*i + 1)];
368 }
369
370 // element au milieu
371 dstcmplx[N / 4].realB() = src[(2*fifoSize)];
372 dstcmplx[N / 4].imagB() = src[(2*fifoSize + 1)];
373
374 // Sortie 2
375 for (i = 0; i < fifoSize ; i++)
376 {
377 dstcmplx[fifoSize - i].realB() = src[(nCoef + 2*i)];
378 dstcmplx[fifoSize - i].imagB() = src[(nCoef + 2*i + 1)];
379 }
380
381 // k = 0 et k = N/2
382 dstcmplx[0].realB() = src[(N - 2)];
383 // Voir Remarque ci-dessus Z(N/2).real -> Z(0).image
384 dstcmplx[0].imagB() = src[(N - 1)]; // Attention, on met ici la real(fmax)
385
386 return ;
387}
388
389/* --Methode__ */
390const char* BRPaquet::FmtConvToString(BRDataFmtConv fgswap)
391{
392 const char * rs="";
393 switch ( fgswap ) {
394 case BR_DoNothing :
395 rs = "BR_DoNothing";
396 break;
397 case BR_Copy :
398 rs = "BR_Copy";
399 break;
400 case BR_SwapAll :
401 rs = "BR_SwapAll";
402 break;
403 case BR_SwapHDR :
404 rs = "BR_SwapHDR";
405 break;
406 case BR_FFTOneChan :
407 rs = "BR_FFTOneChan";
408 break;
409 case BR_FFTTwoChan :
410 rs = "BR_FFTTwoChan";
411 break;
412 case BR_Swap32 :
413 rs = "BR_Swap32";
414 break;
415 case BR_FFTOneChan32 :
416 rs = "BR_FFTOneChan32";
417 break;
418 case BR_FFTTwoChan32 :
419 rs = "BR_FFTTwoChan32";
420 break;
421 case BR_FFTOneChanNoSwap :
422 rs = "BR_FFTOneChanNoSwap";
423 break;
424 case BR_FFTTwoChanNoSwap :
425 rs = "BR_FFTTwoChanNoSwap";
426 break;
427 default:
428 rs = "?????";
429 break;
430 } // Fin switch
431 return rs;
432}
433
434// --------------------------------------------------------------------------
435// Classe pour effectuer des verifications d'integrite sur les paquets/frames
436// --------------------------------------------------------------------------
437
438BRPaqChecker::BRPaqChecker()
439{
440 totnframes = 0;
441 nframeok = 0;
442 lostframes = 0;
443 frclst = 0;
444 DefineHDRTag();
445 DefineTRLTag();
446}
447
448BRPaqChecker::~BRPaqChecker()
449{
450}
451
452UInt64 BRPaqChecker::DefineHDRTag(UInt32 hdr1, UInt32 hdr2)
453{
454 hdrtag_ = (UInt64)hdr1 + ((UInt64)hdr2 << 32);
455 return hdrtag_;
456}
457
458UInt64 BRPaqChecker::DefineTRLTag(UInt32 trl1, UInt32 trl2)
459{
460 trltag_ = (UInt64)trl1 + ((UInt64)trl2 << 32);
461 return trltag_;
462}
463
464
465static int cnt_pb = 0;
466bool BRPaqChecker::Check(BRPaquet& paq)
467{
468 totnframes++;
469 if (paq.HDRMarker64() != HDRTag()) return false;
470 if (paq.TRLMarker64() != TRLTag()) return false;
471 /* DBG
472 if (paq.TRLMarker64() != TRLTag()) {
473 cnt_pb++;
474 if (cnt_pb < 5) paq.Print();
475 return false;
476 }
477 */
478 unsigned int curfc = paq.FrameCounter();
479 unsigned int delfc = 0;
480 if (nframeok > 0) {
481 if (curfc>frclst) delfc = (curfc-frclst);
482 else delfc = (65536-frclst+curfc);
483 lostframes += (unsigned long long)delfc - 1;
484 if (delfc != 1) {
485 cnt_pb++;
486 if (cnt_pb < 5) {
487 cout << "BRPaqChecker::Check([NumFrameOK=" << nframeok
488 << ")/Debug FrameCounter Cur=" << curfc
489 << " Last=" << frclst << " -> delta=" << delfc << endl;
490 paq.Print();
491 }
492 }
493 }
494 nframeok++; frclst = curfc;
495//DBG if (cnt_pb<5) { cnt_pb++; paq.Print(); }
496 return true;
497}
498
499ostream& BRPaqChecker::Print(ostream& os)
500{
501 // os << "BRPaqChecker: HDRTag=" << hex << HDRTag() << " TRLTag=" << TRLTag() << dec << "\n"
502 // << " ... Tot.Nb.Frames.Proc=" << totnframes << " NbFrame HDR/TRL OK=" << nframeok
503 os << "BRPaqChecker: Tot.Nb.Frames.Proc=" << totnframes << " NbFrame HDR/TRL OK=" << nframeok
504 << " LostFrames=" << lostframes
505 << " Loss=" << (double)lostframes*100./(double)totnframes << " %" << endl;
506 return os;
507}
Note: See TracBrowser for help on using the repository browser.