FIFO_TOGGLE_BIT-BUG Workaround com_reset() fixed deadlock, as well as code housekeeping

diff --git a/libtrbnet/trbnet.c b/libtrbnet/trbnet.c
index 6f475aa898c07c58b8358b5e0dcbee0143c5123e..76dfd94fbd24757dbcc84f79b3b62cc67b5d5f54 100644 (file)
--- a/libtrbnet/trbnet.c
+++ b/libtrbnet/trbnet.c
@@ -1,4 +1,4 @@
-const char trbnet_version[] = "$Revision: 2.92 $";
+const char trbnet_version[] = "$Revision: 2.93 $";
 
 #include <stdlib.h>
 #include <signal.h>
@@ -270,13 +270,23 @@ static inline void read32_from_FPGA(uint16_t address, uint32_t* value)
 }
 
 static inline int read32_from_FPGA_dma(uint16_t fifo_address,
-                         uint32_t* values,
-                         uint32_t size)
+                                       uint32_t* values,
+                                       uint32_t size)
 {
   /* Do Not Used */
   return -1;
 }
 
+static inline void com_reset_FPGA()
+{
+  setbitsPC(0x30000);
+  clrbitsPC(0x30000);
+  usleep(100);
+  
+  // Reset FifoToggleBit
+  fifoToggleBit = 0;
+}
+
 /* ------ Internal Functions -------------------------------------------- */
 
 static void TRB_Package_dump(const TRB_Package* pkg)
@@ -432,7 +442,7 @@ static int trb_init_transfer(uint8_t channel)
   read32_from_FPGA(CHANNEL_N_SENDER_STATUS | ((channel * 2 + 1) << 4), &tmp);
   if (tmp != 0) {
     /* FIFO_TOGGLE_BIT-BUG Workaround */
-    com_reset();
+    com_reset_FPGA();
   }
   read32_from_FPGA(CHANNEL_N_SENDER_STATUS | ((channel * 2 + 1) << 4), &tmp);
   if (tmp != 0) {
@@ -445,7 +455,7 @@ static int trb_init_transfer(uint8_t channel)
                    &tmp);
   if ((tmp & MASK_FIFO_EMPTY) == 0) {
     /* FIFO_TOGGLE_BIT-BUG Workaround */
-    com_reset();
+    com_reset_FPGA();
   }
   read32_from_FPGA(CHANNEL_N_RECEIVER_FIFO_STATUS | ((channel * 2 + 1) << 4), 
                    &tmp);
@@ -465,7 +475,6 @@ enum FIFO_READ_MODE {
   FIFO_MODE_REGTIME_READ,
   FIFO_MODE_REG_READ_MEM,
   FIFO_MODE_REGTIME_READ_MEM,
-  FIFO_MODE_REG_WRITE,
   FIFO_MODE_IPU_DATA,
   FIFO_MODE_UID,
   FIFO_MODE_SET_ADDRESS
@@ -581,18 +590,23 @@ static int trb_fifo_read(uint8_t channel,
     case 0:
       package.H0 = *tmp;
       break;
+      
     case 1:
       package.F0 = *tmp;
       break;
+    
     case 2:
       package.F1 = *tmp;
       break;
+    
     case 3:
       package.F2 = *tmp;
       break;
+    
     case 4:
       package.F3 = *tmp;
       break;
+    
     default:
       abort();
     }
@@ -636,7 +650,6 @@ static int trb_fifo_read(uint8_t channel,
       /* Get Data F0 - F3 and store it in User-Data-Buffer if requested */
       if (userBufferOvf == 0) {
         switch (mode) {
-
         case FIFO_MODE_TERM_ONLY:
           if (packageCtr > 0) {
             fifo_flush(channel);
@@ -651,7 +664,6 @@ static int trb_fifo_read(uint8_t channel,
           break;
 
         case FIFO_MODE_REG_READ:
-        
           switch (headerType) {
           case HEADER_HDR:
             if ((packageCtr - endPointCtr * 2) != 0) {
@@ -699,7 +711,6 @@ static int trb_fifo_read(uint8_t channel,
           break;
           
         case FIFO_MODE_REGTIME_READ:
-          
           switch (headerType) {
           case HEADER_HDR:
             if ((packageCtr - endPointCtr * 2) != 0) {
@@ -750,51 +761,51 @@ static int trb_fifo_read(uint8_t channel,
           break;
 
         case FIFO_MODE_REG_READ_MEM:
-            switch (headerType) {
-            case HEADER_HDR:
-              if (dataCtr < dsize) {
-                if (lastHeader != NULL) {
-                  *lastHeader |= (memLen << 16);
-                }
-                memLen = 0;
-                lastHeader = &data[dataCtr];
-                data[dataCtr++] = (uint32_t)package.F0;
+          switch (headerType) {
+          case HEADER_HDR:
+            if (dataCtr < dsize) {
+              if (lastHeader != NULL) {
+                *lastHeader |= (memLen << 16);
+              }
+              memLen = 0;
+              lastHeader = &data[dataCtr];
+              data[dataCtr++] = (uint32_t)package.F0;
 #ifdef TRB_DEBUGGER
-                fprintf(stderr, "D: H: 0x%04x\n", data[dataCtr - 1]);
+              fprintf(stderr, "D: H: 0x%04x\n", data[dataCtr - 1]);
 #endif
-              } else {
-                userBufferOvf = 1;
-              }
-              break;
+            } else {
+              userBufferOvf = 1;
+            }
+            break;
 
-            case HEADER_DAT:
-              if (dataCtr < dsize) {
-                if (package.F0 == 0x0000) break; /* it a hack, ask Jan */
-                data[dataCtr++] = (((uint32_t)package.F1 << 16) |
-                                   ((uint32_t)package.F2));
+          case HEADER_DAT:
+            if (dataCtr < dsize) {
+              if (package.F0 == 0x0000) break; /* it a hack, ask Jan */
+              data[dataCtr++] = (((uint32_t)package.F1 << 16) |
+                                 ((uint32_t)package.F2));
 #ifdef TRB_DEBUGGER
-                fprintf(stderr,
-                        "D: 0x%04x  0x%08x\n", memLen, data[dataCtr - 1]); 
+              fprintf(stderr,
+                      "D: 0x%04x  0x%08x\n", memLen, data[dataCtr - 1]); 
 #endif
-                memLen++;
-              } else {
-                userBufferOvf = 1;
-              }
-              break;
-
-            case HEADER_TRM:
-              if (lastHeader != NULL) {
-                *lastHeader |= (memLen << 16);
-              }
-              break;
+              memLen++;
+            } else {
+              userBufferOvf = 1;
+            }
+            break;
 
-            default:
-              fifo_flush(channel);
-              trb_errno = TRB_FIFO_INVALID_HEADER;
-              return -1;
+          case HEADER_TRM:
+            if (lastHeader != NULL) {
+              *lastHeader |= (memLen << 16);
             }
             break;
 
+          default:
+            fifo_flush(channel);
+            trb_errno = TRB_FIFO_INVALID_HEADER;
+            return -1;
+          }
+          break;
+
 
         case FIFO_MODE_REGTIME_READ_MEM:
           switch (headerType) {
@@ -815,7 +826,7 @@ static int trb_fifo_read(uint8_t channel,
             break;
             
           case HEADER_DAT:
-            if (dataCtr + 1< dsize) {
+            if (dataCtr + 1 < dsize) {
               if (package.F0 == 0x0000) break; /* it a hack, ask Jan */
               data[dataCtr++] = (((uint32_t)package.F1 << 16) |
                                  ((uint32_t)package.F2));
@@ -843,25 +854,11 @@ static int trb_fifo_read(uint8_t channel,
           }
           break;
           
-        case FIFO_MODE_REG_WRITE:
-          if (headerType == HEADER_TRM) break;
-          break;
-          
         case FIFO_MODE_IPU_DATA:
           {
             unsigned int i;
 
             switch (headerType) {
-
-            case HEADER_TRM:
-              if ((packageCtr > 0) && (dataCtr != memLen)) {
-                /* Error invalid length */
-                fifo_flush(channel);
-                trb_errno = TRB_HDR_DLEN;
-                return -1;
-              }
-              break;
-
             case HEADER_HDR:
               if (packageCtr != 0) {
                 fifo_flush(channel);
@@ -870,7 +867,7 @@ static int trb_fifo_read(uint8_t channel,
               }
               memLen = (unsigned int)package.F2;
               break;
-
+              
             case HEADER_DAT:
               for (i = 0; (i < 2) && (dataCtr < memLen); i++) {
                 if (dataCtr < dsize) {
@@ -887,6 +884,15 @@ static int trb_fifo_read(uint8_t channel,
                 }
               }
               break;
+              
+            case HEADER_TRM:
+              if ((packageCtr > 0) && (dataCtr != memLen)) {
+                /* Error invalid length */
+                fifo_flush(channel);
+                trb_errno = TRB_HDR_DLEN;
+                return -1;
+              }
+              break;
 
             default:
               fifo_flush(channel);
@@ -911,7 +917,7 @@ static int trb_fifo_read(uint8_t channel,
               }
               sourceAddress = (uint32_t)package.F0;
               break;
-
+              
             case HEADER_DAT:
               if ((packageCtr - endPointCtr * 3) == 1) {
                 uidHigh = (((uint32_t)package.F0 <<  0) |
@@ -940,6 +946,7 @@ static int trb_fifo_read(uint8_t channel,
                 }
                 break;
               }
+              
             case HEADER_TRM:
               break;
 
@@ -952,24 +959,32 @@ static int trb_fifo_read(uint8_t channel,
           break;
 
         case FIFO_MODE_SET_ADDRESS:
-          if (headerType == HEADER_TRM) break;
-
-          if ((packageCtr == 1) && (headerType == HEADER_DAT)) {
-            if (package.F0 != NET_ACKADDRESS) {
-              fifo_flush(channel);
-              return -1;
-            }
-          }
-
-          if (packageCtr > 1) {
+          if (packageCtr > 2) {
             fifo_flush(channel);
             trb_errno = TRB_INVALID_PKG_NUMBER;
             return -1;
           }
-
-          dataCtr++;
+                  
+          switch (headerType) {
+          case HEADER_HDR:
+            break;
+            
+          case HEADER_DAT:
+            if ((packageCtr == 1) && (package.F0 == NET_ACKADDRESS)) {
+              dataCtr++;
+            }
+            break;
+            
+          case HEADER_TRM:
+            break;
+            
+          default:
+            fifo_flush(channel);
+            trb_errno = TRB_FIFO_INVALID_HEADER;
+            return -1;
+          }
           break;
-
+          
         default:
           fifo_flush(channel);
           trb_errno = TRB_FIFO_INVALID_MODE;
@@ -987,13 +1002,13 @@ static int trb_fifo_read(uint8_t channel,
     } else {
       timeout = 0;
       do {
-       read32_from_FPGA(fifoBuffer, tmp);
+        read32_from_FPGA(fifoBuffer, tmp);
       } while (((*tmp & MASK_FIFO_VALID) == 0) && (++timeout < MAX_TIMEOUT));
 
       if (timeout >= MAX_TIMEOUT) {
-       fifo_flush(channel);
-       trb_errno = TRB_FIFO_INCOMPLETE;
-       return -1;
+        fifo_flush(channel);
+        trb_errno = TRB_FIFO_INCOMPLETE;
+        return -1;
       }
     }
   }
@@ -1041,7 +1056,6 @@ static int trb_fifo_read(uint8_t channel,
   return dataCtr;
 }
 
-
 static int master_lock = 0;
 
 static int lockPorts(int masterLock)
@@ -1152,11 +1166,11 @@ int init_ports()
   }
   
   mem = (uint32_t*)mmap((void*)0,
-                   2 * 4 * 4 * 8192,
-                   PROT_READ | PROT_WRITE,
-                   MAP_SHARED,
-                   memfd,
-                   0xb0000000);
+                        2 * 4 * 4 * 8192,
+                        PROT_READ | PROT_WRITE,
+                        MAP_SHARED,
+                        memfd,
+                        0xb0000000);
   
   if ((void*)mem == MAP_FAILED) {
     trb_errno = TRB_FIFO_SHARED_MEM;
@@ -1175,7 +1189,7 @@ int init_ports()
   
   close(memfd);
 
-  if (lockPorts(1) == -1) return -1;
+  if (lockPorts(0) == -1) return -1;
 
   /* Set output enabled if not done yet */
   if ((*GPIOB_OE_OFFSET != GPIOB_OE_PINS) ||
@@ -1188,9 +1202,9 @@ int init_ports()
   }
   
   /* Reset Ports */
-  com_reset();
+  com_reset_FPGA();
   
-  if (unlockPorts(1) == -1) return -1;
+  if (unlockPorts(0) == -1) return -1;
  
   return 0;
 }
@@ -1498,7 +1512,7 @@ int trb_register_write(uint16_t trb_address,
     fprintf(stderr, "CMD_REGISTER_WRITE started.\n");
   }
 
-  status = trb_fifo_read(3, FIFO_MODE_REG_WRITE, NULL, 0);
+  status = trb_fifo_read(3, FIFO_MODE_TERM_ONLY, NULL, 0);
 
   if (unlockPorts(0) == -1) return -1;
 
@@ -1566,7 +1580,7 @@ int trb_register_write_mem(uint16_t trb_address,
       fprintf(stderr, "CMD_REGISTER_WRITE_MEM started %d.\n", len);
     }
     
-    status = trb_fifo_read(3, FIFO_MODE_REG_WRITE, NULL, 0);
+    status = trb_fifo_read(3, FIFO_MODE_TERM_ONLY, NULL, 0);
   }
   
   if (unlockPorts(0) == -1) return -1;
@@ -1670,9 +1684,9 @@ int trb_set_address(uint64_t uid,
   status = trb_fifo_read(3, FIFO_MODE_SET_ADDRESS, NULL, 0);
 
   if (unlockPorts(0) == -1) return -1;
-
+  
   if (status == -1) return -1;
-  if (status != 2) {
+  if (status != 1) {
     trb_errno = TRB_ENDPOINT_NOT_REACHED;
     return -1;
   }
@@ -1894,9 +1908,9 @@ int network_reset()
 {
   trb_errno = TRB_NONE;
   
-  if (lockPorts(1) == -1) return -1;
+  if (lockPorts(0) == -1) return -1;
 
-  com_reset();
+  com_reset_FPGA();
   
   /* DEBUG INFO */
   if (trb_debug > 0) {
@@ -1909,9 +1923,9 @@ int network_reset()
   write32_to_FPGA(0x10, 0x8000);
   sleep(8);
   
-  com_reset();
+  com_reset_FPGA();
      
-  if (unlockPorts(1) == -1) return -1;
+  if (unlockPorts(0) == -1) return -1;
   
   return 0;
 }
@@ -1919,7 +1933,7 @@ int network_reset()
 int com_reset()
 {
   trb_errno = TRB_NONE;
-
+  
   if (lockPorts(0) == -1) return -1;
 
   /* DEBUG INFO */
@@ -1927,15 +1941,10 @@ int com_reset()
     fprintf(stderr, "com_reset started.\n");
   }
   
-  setbitsPC(0x30000);
-  clrbitsPC(0x30000);
-  usleep(100);
+  com_reset_FPGA();
   
-  // Reset FifoToggleBit
-  fifoToggleBit = 0;
-
   if (unlockPorts(0) == -1) return -1;
-
+  
   return 0;
 }