From d5d69b9745f01ed3a22fa173ecfd0309b55b27c6 Mon Sep 17 00:00:00 2001
From: hadaq <hadaq>
Date: Tue, 17 Jul 2012 18:41:01 +0000
Subject: [PATCH] enabled register_modify function for TRB3, should only be
 used via trbnetd

---
 libtrbnet/trbnet.c | 159 +++++++++++++++++++++++----------------------
 1 file changed, 83 insertions(+), 76 deletions(-)

diff --git a/libtrbnet/trbnet.c b/libtrbnet/trbnet.c
index d6a8520..4a419fa 100644
--- a/libtrbnet/trbnet.c
+++ b/libtrbnet/trbnet.c
@@ -1,4 +1,10 @@
-const char trbnet_version[] = "$Revision: 4.26 $  Local";
+#ifdef PEXOR
+const char trbnet_version[] = "$Revision: 4.27 $  Local Pexor";
+#elif defined TRB3
+const char trbnet_version[] = "$Revision: 4.27 $  Local TRB3";
+#else
+const char trbnet_version[] = "$Revision: 4.27 $  Local Etrax";
+#endif
 
 #include <stdlib.h>
 #include <signal.h>
@@ -2815,6 +2821,82 @@ 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,
@@ -2909,81 +2991,6 @@ 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) == -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;
-}
 #else /* NOT TRB3 */
 
 int trb_ipu_data_read(uint8_t type,
-- 
2.43.0