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

Last change on this file since 3639 was 3639, checked in by cmv, 16 years ago

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