modify enabled

diff --git a/libtrbnet/trbnet.c b/libtrbnet/trbnet.c
index 5dcdeeb81125134475b14627f7a075265f62ccb0..c3e1555119813d78ec869f4672033b01549c1855 100644 (file)
--- a/libtrbnet/trbnet.c
+++ b/libtrbnet/trbnet.c
@@ -1,11 +1,11 @@
 #ifdef ETRAX
-const char trbnet_version[] = "$Revision: 4.36 $  Local Etrax";
+const char trbnet_version[] = "$Revision: 4.37 $  Local Etrax";
 #elif defined PEXOR
-const char trbnet_version[] = "$Revision: 4.36 $  Local Pexor";
+const char trbnet_version[] = "$Revision: 4.37 $  Local Pexor";
 #elif defined TRB3
-const char trbnet_version[] = "$Revision: 4.36 $  Local TRB3";
+const char trbnet_version[] = "$Revision: 4.37 $  Local TRB3";
 #else
-const char trbnet_version[] = "$Revision: 4.36 $  UNKNOWN, i.e. ERROR";
+const char trbnet_version[] = "$Revision: 4.37 $  UNKNOWN, i.e. ERROR";
 #endif
 
 #include <stdlib.h>
@@ -2835,82 +2835,6 @@ int fpga_register_write(uint32_t reg_address, uint32_t value)
   return status;
 }
 
-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) == -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(1);
-    return -1;
-  }
-
-  /* Now set bits on all endpoints             */
-  /* check, whether all registers are the same */
-  singleWrite = 1;
-  value = data[1];
-  for (i = 2; (i + 1) < status; i += 2) {
-    if (data[i + 1] != value) {
-      singleWrite = 0;
-      break;
-    }
-  }
-
-  /* Write modified register value(s) */
-  for (i = 0; (i + 1) < (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(1);
-      return -1;
-    }
-
-    if (trb_register_write(trb_address, reg_address, value) == -1) {
-      free(data);
-      unlockPorts(1);
-      return -1;
-    }
-  }
-
-  free(data);
-  if (unlockPorts(1) == -1) return -1;
-
-  return 0;
-}
-
 #ifdef PEXOR
 int fpga_register_read_mem(uint32_t reg_address,
                            uint32_t* data,
@@ -3092,18 +3016,6 @@ int com_reset()
   return -1;
 }
 
-int trb_register_modify(uint16_t trb_address,
-                        uint16_t reg_address,
-                        int mode,
-                        uint32_t bitMask,
-                        uint32_t bitValue)
-{
-  trb_errno = TRB_TRB3_CMD_NOT_SUPPORTED;
-  return -1;
-}
-
-
-
 #endif
 
 int trb_nettrace(uint16_t trb_address,
@@ -3158,3 +3070,79 @@ int trb_nettrace(uint16_t trb_address,
   free(buffer);
   return ctr;
 }
+
+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) == -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(1);
+    return -1;
+  }
+
+  /* Now set bits on all endpoints             */
+  /* check, whether all registers are the same */
+  singleWrite = 1;
+  value = data[1];
+  for (i = 2; (i + 1) < status; i += 2) {
+    if (data[i + 1] != value) {
+      singleWrite = 0;
+      break;
+    }
+  }
+
+  /* Write modified register value(s) */
+  for (i = 0; (i + 1) < (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(1);
+      return -1;
+    }
+
+    if (trb_register_write(trb_address, reg_address, value) == -1) {
+      free(data);
+      unlockPorts(1);
+      return -1;
+    }
+  }
+
+  free(data);
+  if (unlockPorts(1) == -1) return -1;
+
+  return 0;
+}