Add check before resetting the FIFO for SLC

Pcie40Libraries/pcie40_b2slc.cpp

@@ -14,6 +14,17 @@ extern "C" unsigned ecs_read(int dev, int bar, unsigned add);
 ///////////////////////////////////////////////////////////////////////////
 // Internal functions for slow control,, do not use outside of this library
 ///////////////////////////////////////////////////////////////////////////
+int pcie40_writeFifoFillLevel( int dev , int ch ) {
+  unsigned ret = 0 ;
+  ret = ecs_read( dev , SLC_BAR , SLC_RFIFO_STATUS + ch * 0x20 ) ;
+  return ( ret & 0x1FF ) ; 
+}
+
+int pcie40_readFifoFillLevel( int dev , int ch ) {
+  unsigned ret = ecs_read( dev , SLC_BAR , SLC_RFIFO_STATUS + ch * 0x20 ) ;
+  ret = ( ret & 0xFF800 ) >> 11 ;
+  return ret ;
+}
 
 int pcie40_resetWriteFifo( int dev , int ch ) { 
   unsigned ret = 0 ;
@@ -27,6 +38,21 @@ int pcie40_resetWriteFifo( int dev , int ch ) {
 
 int pcie40_resetReadFifo( int dev , int ch ) { 
   unsigned ret = 0 ;
+  unsigned rle = 0 ; 
+  unsigned wle = 0 ; 
+  // wait that the FIFO are empty
+  int i ;
+  for ( i=0 ; i<10 ; i++ ) {
+    usleep( 10 ) ; //10 ms
+    rle = pcie40_readFifoFillLevel( dev , ch ) ; 
+    wle = pcie40_writeFifoFillLevel( dev , ch ) ; 
+    if ( ( rle == 0 ) && ( wle == 0 ) ) break;
+  }
+
+  if (i == 10) {
+    printf( "Timeout\n" ) ;
+  }
+  
   ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_RESET_ADD , 
        1 << SLC_WFIFO_RESET_BIT ) ;
   if ( ret != 0 ) return -1 ;
@@ -63,8 +89,7 @@ int pcie40_waitRead( int dev , int ch , int length ) {
   int i ;
   for ( i=0 ; i<10 ; i++ ) {
     usleep( 10 ) ; //10 ms
-    ret = ecs_read( dev , SLC_BAR , SLC_RFIFO_STATUS + ch * 0x20 ) ;
-    ret = ( ret & 0xFF00 ) >> 8 ;
+    ret = pcie40_readFifoFillLevel( dev , ch ) ; 
     if ( ret == length ) break;
   }
 
@@ -112,7 +137,7 @@ int pcie40_readfee8( int dev , int ch , int adr) {
   if ( ret != 0 ) return -1 ;
 
   // Wait for the result to come back (40 words in the fifo)
-  ret = pcie40_waitRead( dev , ch , 40 ) ;
+  ret = pcie40_waitRead( dev , ch , 5 ) ;
   if ( ret != 0 ) return -1 ;
 
   // Read the value 
@@ -181,12 +206,12 @@ unsigned long pcie40_readfee32( int dev , int ch , int adr ) {
   if ( ret != 0 ) return -1 ;
 
   // Wait for the result to come back
-  ret = pcie40_waitRead( dev , ch , 56 ) ;
+  ret = pcie40_waitRead( dev , ch , 7 ) ;
   if ( ret != 0 ) return -1 ;
 
   // Read the value 
   std::vector< int > result ;
-  ret = pcie40_readData( dev , ch , result , 6 ) ;
+  ret = pcie40_readData( dev , ch , result , 7 ) ;
 
   int t_ret1 = result.at(0);
   int t_ret2 = result.at(1);