added setbit/clearbit/loadbit

diff --git a/libtrbnet/trbcmd.c b/libtrbnet/trbcmd.c
index a367a3594b15ea0ee6a59bd85120fc7009b36684..965b56fc4a05f0c65357e29d24df1fc0bddd8a98 100644 (file)
--- a/libtrbnet/trbcmd.c
+++ b/libtrbnet/trbcmd.c
@@ -35,7 +35,7 @@
 
 static int hexMode = HEXMODE;
 
-static const char trbcmd_version[] = "$Revision: 2.50 $";
+static const char trbcmd_version[] = "$Revision: 2.51 $";
 
 #define BACKLOG 10
 static uint16_t tcp_port = 55555;
@@ -43,7 +43,7 @@ static int tcp_debug = 0;        /* turn to 0 to suppress TCP/IP output */
 
 /* ---- User Buffer Size ----------------------------------------------- */
 
-static size_t NUM_ENDPOINTS = 1024;     /* Maximum of 16KByte */
+static const size_t NUM_ENDPOINTS = 1024;     /* Maximum of 16KByte */
 static size_t USER_BUFFER_SIZE = 0;
 
 /* ------ MAIN ---------------------------------------------------------- */
@@ -90,6 +90,14 @@ void usage(const char *progName)
           "send trigger to RICH only\n", '%');
   fprintf(stdout, "   I <type> <random> <info> <number|%cctr>           -> "
           "read IPU data\n", '%');
+  
+  fprintf(stdout, "   setbit <trbaddress> <register> <bitmask>         -> "
+          "set bits of a register\n");
+  fprintf(stdout, "   clearbit <trbaddress> <register> <bitmask>       -> "
+          "clear bits of a register\n");
+  fprintf(stdout, "   loadbit <trbaddress> <register> <bitmask> <val>  -> "
+          "load bits of a register\n");
+  
   fprintf(stdout, "   reload <trbaddress>                              -> "
           "reload FPGA\n");
   fprintf(stdout, "   reset                                            -> "
@@ -108,7 +116,7 @@ void usage(const char *progName)
           "disconnect from server\n"
           "                                                       "
           "(tcp-server mode only)\n\n");
-
+  
   fprintf(stdout, "Start as TCP/IP-Server:\n");
   fprintf(stdout, "Usage: %s [-h] [-d] [-p portnumber] [-b] [-V] tcp\n",
           progName);
@@ -1068,7 +1076,138 @@ int start(int argc, char **argv)
           trb_error("fpga_register_write failed");
           return -1;
         }
-
+        
+      } else if (strncmp(cmd[0], "setbit", CMD_SIZE) == 0) {
+        
+        /*******************************************/
+        /* Register Set Bits                       */
+        /*******************************************/
+        
+        int status = 0;
+        
+        uint32_t bitMask;
+                
+        if (cmdLen != 4) {
+          if (scriptFile != NULL) {
+            fprintf(stderr, "Line #%d: Invalid command\n", lineCtr);
+          } else {
+            usage(basename(argv[0]));
+          }
+          return -1;
+        }
+        
+        trb_address = strtoul(cmd[1], NULL, hexMode == 1 ? 16 : 0);
+        reg_address = strtoul(cmd[2], NULL, hexMode == 1 ? 16 : 0);
+        bitMask = strtoul(cmd[3], NULL, hexMode == 1 ? 16 : 0);
+        
+        /* DEBUG Info */
+        if (trb_debug > 0) {
+          fprintf(stderr,
+                  "Command: SETBIT: trb_address: 0x%04x, "
+                  "reg_address: 0x%04x, bitMask: 0x%04x\n",
+                  trb_address, reg_address, bitMask);
+        }
+        
+        status = trb_register_modify(trb_address, reg_address,
+                                     1, bitMask, 0);
+        if (status == -1) {
+          if (scriptFile != NULL) {
+            fprintf(stderr, "Line #%d: ", lineCtr);
+          }
+          trb_error("setbit of register failed");
+          if (trb_errno != TRB_ENDPOINT_NOT_REACHED) {
+            return -1;
+          }
+        }
+        
+      } else if (strncmp(cmd[0], "clearbit", CMD_SIZE) == 0) {
+        
+        /*******************************************/
+        /* Register Clear Bits                       */
+        /*******************************************/
+        
+        int status = 0;
+        
+        uint32_t bitMask;
+        
+        if (cmdLen != 4) {
+          if (scriptFile != NULL) {
+            fprintf(stderr, "Line #%d: Invalid command\n", lineCtr);
+          } else {
+            usage(basename(argv[0]));
+          }
+          return -1;
+        }
+        
+        trb_address = strtoul(cmd[1], NULL, hexMode == 1 ? 16 : 0);
+        reg_address = strtoul(cmd[2], NULL, hexMode == 1 ? 16 : 0);
+        bitMask = strtoul(cmd[3], NULL, hexMode == 1 ? 16 : 0);
+        
+        /* DEBUG Info */
+        if (trb_debug > 0) {
+          fprintf(stderr,
+                  "Command: CLEARBIT: trb_address: 0x%04x, "
+                  "reg_address: 0x%04x, bitMask: 0x%04x\n",
+                  trb_address, reg_address, bitMask);
+        }
+        
+        status = trb_register_modify(trb_address, reg_address,
+                                     2, bitMask, 0);
+        if (status == -1) {
+          if (scriptFile != NULL) {
+            fprintf(stderr, "Line #%d: ", lineCtr);
+          }
+          trb_error("clearbit of register failed");
+          if (trb_errno != TRB_ENDPOINT_NOT_REACHED) {
+            return -1;
+          }
+        }
+        
+      } else if (strncmp(cmd[0], "loadbit", CMD_SIZE) == 0) {
+        
+        /*******************************************/
+        /* Register Load Bits                       */
+        /*******************************************/
+        
+        int status = 0;
+        
+        uint32_t bitMask;
+        uint32_t bitValue;
+        
+        if (cmdLen != 5) {
+          if (scriptFile != NULL) {
+            fprintf(stderr, "Line #%d: Invalid command\n", lineCtr);
+          } else {
+            usage(basename(argv[0]));
+          }
+          return -1;
+        }
+        
+        trb_address = strtoul(cmd[1], NULL, hexMode == 1 ? 16 : 0);
+        reg_address = strtoul(cmd[2], NULL, hexMode == 1 ? 16 : 0);
+        bitMask = strtoul(cmd[3], NULL, hexMode == 1 ? 16 : 0);
+        bitValue = strtoul(cmd[4], NULL, hexMode == 1 ? 16 : 0);
+        
+        /* DEBUG Info */
+        if (trb_debug > 0) {
+          fprintf(stderr,
+                  "Command: LOADBIT: trb_address: 0x%04x, "
+                  "reg_address: 0x%04x, bitMask: 0x%04x bitValue: 0x%04x\n",
+                  trb_address, reg_address, bitMask, bitValue);
+        }
+        
+        status = trb_register_modify(trb_address, reg_address,
+                                     3, bitMask, bitValue);
+        if (status == -1) {
+          if (scriptFile != NULL) {
+            fprintf(stderr, "Line #%d: ", lineCtr);
+          }
+          trb_error("loadbit of register failed");
+          if (trb_errno != TRB_ENDPOINT_NOT_REACHED) {
+            return -1;
+          }
+        }
+        
       } else {
 
         /*******************************************/

diff --git a/libtrbnet/trbnet.c b/libtrbnet/trbnet.c
index 2c50612fd67f68e0342bd28187d5f10b751e62e3..5bd3ee2cf593d7795c4de42f5f70634b0dc9b79e 100644 (file)
--- a/libtrbnet/trbnet.c
+++ b/libtrbnet/trbnet.c
@@ -1,4 +1,4 @@
-const char trbnet_version[] = "$Revision: 2.64 $";
+const char trbnet_version[] = "$Revision: 2.65 $";
 
 #include <stdlib.h>
 #include <signal.h>
@@ -930,6 +930,9 @@ static int trb_fifo_read(uint8_t channel,
   return dataCtr;
 }
 
+
+static int ports_locked = 0;
+
 static int lockPorts()
 {
   struct sembuf sops = {
@@ -937,7 +940,9 @@ static int lockPorts()
     -1,          /* sem_op: decrement semaphore by 1, i.e. lock it        */
     SEM_UNDO     /* sem_flg: remove lock if process gets killed           */
   };
-
+  
+  if (ports_locked == 1) return 0;
+  
   /* Wait for semaphore and lock it */
   if (semop(semid, &sops, 1) == -1) {
     trb_errno = TRB_SEMAPHORE;
@@ -950,6 +955,7 @@ static int lockPorts()
   /* Get FifoToggleBit-Status, needed by read32_from_FPGA ... */
   fifoToggleBit = readPC() & FIFO_TOGGLE_BIT;
 
+  ports_locked = 1;
   return 0;
 }
 
@@ -961,6 +967,8 @@ static int unlockPorts()
     SEM_UNDO               /*  */
   };
 
+  if (ports_locked == 0) return 0;
+
   /* Release semaphore */
   if (semop(semid, &sops, 1) == -1) {
     trb_errno = TRB_SEMAPHORE;
@@ -969,7 +977,8 @@ static int unlockPorts()
 
   /* Unblock Signals */
   sigprocmask(SIG_SETMASK, &blockSetOld, NULL);
-
+  
+  ports_locked = 0;
   return 0;
 }
 
@@ -1692,3 +1701,80 @@ int com_reset()
 
   return 0;
 }
+
+int trb_register_modify(uint16_t trb_address,
+                        uint16_t reg_address,
+                        int mode,
+                        uint32_t bitMask,
+                        uint32_t bitValue)
+{
+  static const size_t NUM_ENDPOINTS = 1024;
+  int status = 0;
+
+  uint32_t value;
+  int singleWrite = 0;
+  uint32_t *data = NULL;
+  int i;
+  
+  if (lockPorts() == -1) return -1;
+  
+  data = (uint32_t *) malloc(sizeof(uint32_t) * NUM_ENDPOINTS * 2);
+  if (data == NULL) abort();
+  
+  status = trb_register_read(trb_address, reg_address,
+                             data, NUM_ENDPOINTS * 2);
+  if (status == -1) {
+    free(data);
+    unlockPorts();
+    return -1; 
+  }
+  
+  /* Now set bits on all endpoints */
+  /* check, whether all registers are the same */ 
+  singleWrite = 1;
+  value = data[1];
+  for (i = 2; i < status; i += 2) {
+    if (data[i + 1] != value) {
+      singleWrite = 0;
+      break;
+    }
+  }
+        
+  /* Write modified register value(s) */
+  for (i = 0; i < (singleWrite == 0 ? status : 2); i += 2) {
+    if (singleWrite == 0) {
+      trb_address = data[i];
+      value = data[i + 1];
+    }
+    switch (mode) {
+    case 1:
+      value |= bitMask;
+      break;
+    
+    case 2:
+      value &= ~bitMask;
+      break;
+    
+    case 3:
+      value = (value & ~bitMask) | (bitValue & bitMask);
+      break;
+    
+    default:
+      free(data);
+      unlockPorts();
+      return -1;
+    }
+    
+    if ((trb_register_write(trb_address, reg_address, value) == -1) &&
+        (trb_errno != TRB_ENDPOINT_NOT_REACHED)) {
+      free(data);
+      unlockPorts();
+      return -1;
+    } 
+  }         
+  
+  free(data);
+  if (unlockPorts() == -1) return -1;
+  
+  return 0;
+}

diff --git a/libtrbnet/trbnet.h b/libtrbnet/trbnet.h
index 271c788f65aca6c421723ef40f9cc8e9c5ca5f84..f6cc54c38a8122063fc05b2ee65be475cdbb07ce 100644 (file)
--- a/libtrbnet/trbnet.h
+++ b/libtrbnet/trbnet.h
@@ -78,6 +78,12 @@ int trb_fifo_flush(uint8_t channel);
 int network_reset();
 
 int com_reset();
+  
+int trb_register_modify(uint16_t trb_address,
+                        uint16_t reg_address,
+                        int mode,
+                        uint32_t bitMask,
+                        uint32_t bitValue);
 
 /* ---------------------------------------------------------------------- */