mc/hijacknet_extctrl2014.c

@@ -3,6 +3,7 @@
 #include <sysLib.h>
 #include <string.h>
 #include <taskLib.h>
+#include <fppLib.h>
 #include <stdlib.h>
 #include <extctrl_pub.irc5_main_110310.h>
 #include "extctrl2014_lc.h"
@@ -43,7 +44,8 @@ acN.value[0] acN.value[1] acN.value[2] acN.value[3] idN
 #define MAX_JOINT 128
 #define MAX_ROBOT 4
 #define MAX_FORCE_SENSORS 1
-#define BUFFER_SIZE 1500
+#define WRITE_BUFFER_SIZE 1500
+#define READ_BUFFER_SIZE 8192
 
 extern unsigned long int cpuFrequency;
 
@@ -84,19 +86,31 @@ static struct state {
   unsigned char dst[6];
   int channel;
   int cookie;
-  struct encoder_writer {
+  int received;
+  struct {
     struct labcomm2014_writer writer;
     struct labcomm2014_writer_action_context action_context;
-    int need_ack;
+    struct labcomm2014_encoder *encoder;
+    SEM_ID sem;
+    unsigned int sequence;
+    int unadjusted;
+    int awaited_ack;
     int frag_start;
-    unsigned char buffer[BUFFER_SIZE];
+    unsigned char buffer[WRITE_BUFFER_SIZE];
   } writer;
   struct {
-    struct labcomm2014_encoder *encoder;
-    int sequence;
-  } encoder;
-  struct {
+    struct labcomm2014_reader reader;
+    struct labcomm2014_reader_action_context action_context;
     struct labcomm2014_decoder *decoder;
+    SEM_ID sem;
+    unsigned int sequence;
+    int lpconsume;
+    unsigned char buffer[READ_BUFFER_SIZE];
+    unsigned int read_pos;
+    unsigned int write_pos;
+    unsigned int available;
+  } reader;
+  struct {
     int sequence;
     int fragment;
     int pos;
@@ -136,12 +150,6 @@ static struct state {
   extctrl2014_force_torque_net force_net;
 } state;
 
-#if 0
-
-#define PROTOCOL_ERROR					    \
-  printf("PROTOCOL ERROR %s:%d\n", __FUNCTION__, __LINE__); \
-  state->state = s_error;
-
 static inline unsigned int rdtsc()
 {
   // Since we have at least one system with broken rdtsc (high and low word
@@ -152,6 +160,20 @@ static inline unsigned int rdtsc()
   return result;
 }
 
+static inline unsigned int next_sequence(unsigned int sequence)
+{
+  do {
+    sequence++;
+  } while (sequence == 0);
+  return sequence;
+}
+
+#if 0
+
+#define PROTOCOL_ERROR					    \
+  printf("PROTOCOL ERROR %s:%d\n", __FUNCTION__, __LINE__); \
+  state->state = s_error;
+
 static int get_robot_index(axis_configuration_t ac)
 {
   int i;
@@ -953,8 +975,8 @@ static int writer_alloc(
 {
   printf("%s %s %s\n", __FUNCTION__, __DATE__, __TIME__);
   w->data = state.writer.buffer;
-  w->data_size = BUFFER_SIZE;
-  w->count = BUFFER_SIZE;
+  w->data_size = WRITE_BUFFER_SIZE;
+  w->count = WRITE_BUFFER_SIZE;
   w->pos = 0;
 
   return w->error;
@@ -985,12 +1007,14 @@ static int writer_start(
   
   printf("%s %p %p\n", __FUNCTION__, signature, value);
   w->pos = 0;
+  state.writer.unadjusted = 1;
+  state.writer.sequence = next_sequence(state.writer.sequence);
   if (signature != NULL && value != NULL) {
     /* DATA packet */
-    state.writer.need_ack = 0;
+    state.writer.awaited_ack = 0;
     flags = kind_DATA | flag_LAST_FRAG;
   } else {
-    state.writer.need_ack = 1;
+    state.writer.awaited_ack = state.writer.sequence;
     if (signature == NULL && value == NULL) {
       /* INIT packet, labcomm not involved */
       flags = kind_INIT | flag_LAST_FRAG | flag_NEED_ACK;
@@ -1012,7 +1036,7 @@ static int writer_start(
   labcomm2014_write_byte(w, flags);
   labcomm2014_write_packed32(w, state.channel);
   labcomm2014_write_int(w, cookie);
-  labcomm2014_write_int(w, index);
+  labcomm2014_write_int(w, state.writer.sequence);
   /* reserve maximum space for frag_num and frag_length */
   state.writer.frag_start = w->pos;
   w->pos += 10;
@@ -1026,6 +1050,8 @@ static int writer_end(
 {
   int first, last, length, offset;
 
+  if (state.writer.unadjusted) {
+    state.writer.unadjusted = 0;
     first = state.writer.frag_start + 10;
     last = w->pos;
     length = last - first;
@@ -1033,14 +1059,26 @@ static int writer_end(
     labcomm2014_write_packed32(w, 0); /* frag_num */
     labcomm2014_write_packed32(w, length); /* frag_length */
     offset = first - w->pos;
-  w->data[last] = 'X';
     memmove(&w->data[w->pos], &w->data[first], length);
     last = last - offset;
-  w->data[last] = 'Y';
     printf("%s %d %d %d %d\n", __FUNCTION__, first, last, length, offset);
-  hijacknet_send(state.driver, w->data, last + offset);
-  w->pos = 0;
-   
+    while (last < 60) {
+      /* Null padding at end */
+      w->data[last] = 0;
+      last++;
+    }
+    w->pos = last;
+  }
+  printf("%s %d\n", __FUNCTION__, w->pos);
+  while (1) {
+    printf("!%x\n", state.writer.awaited_ack);
+    hijacknet_send(state.driver, w->data, w->pos);
+    if (state.writer.awaited_ack == 0) {
+      break;
+    } else {
+      semTake(state.writer.sem, 1000);
+    }
+  }
   return w->error;
 }
 
@@ -1071,11 +1109,188 @@ static void init_writer()
   state.writer.writer.action_context = &state.writer.action_context;
   state.writer.writer.memory = NULL;
   state.writer.writer.data = state.writer.buffer;
-  state.writer.writer.data_size = BUFFER_SIZE;
-  state.writer.writer.count = BUFFER_SIZE;
+  state.writer.writer.data_size = WRITE_BUFFER_SIZE;
+  state.writer.writer.count = WRITE_BUFFER_SIZE;
   state.writer.writer.pos = 0;
 }
 
+static int reader_fill(
+  struct labcomm2014_reader *r, 
+  struct labcomm2014_reader_action_context *action_context)
+{
+  int result = 0;
+
+  if (state.reader.available) {
+    unsigned int consume;
+
+    consume = state.reader.available;
+    if (r->pos == READ_BUFFER_SIZE) {
+      /* Handle wrap around */
+      r->pos = 0;
+    } else if (r->pos + consume > READ_BUFFER_SIZE) {
+      /* Packet wraps around, consume first part */
+      consume = READ_BUFFER_SIZE - r->pos;
+    }
+    r->count = r->pos + consume;
+    state.reader.available -= consume;
+    result = consume;
+    printf("Fill %d %d %d\n", consume, state.reader.available, r->pos);
+  } else {
+    r->count = 0;
+    r->error = -ENODATA;
+    printf("Fill error\n");
+  }
+  return result;
+}
+
+static const struct labcomm2014_reader_action reader_action = {
+  .alloc = NULL,
+  .free = NULL,
+  .start = NULL,
+  .fill = reader_fill,
+  .end = NULL,
+  .ioctl = NULL
+};
+
+static void init_reader()
+{
+  state.reader.read_pos = 0;
+  state.reader.write_pos = 0;
+  state.reader.available = 0;
+  state.reader.action_context.next = NULL;
+  state.reader.action_context.action = &reader_action;
+  state.reader.action_context.context = &state.reader;
+  state.reader.reader.action_context = &state.reader.action_context;
+  state.reader.reader.memory = NULL;
+  state.reader.reader.data = state.reader.buffer;
+  state.reader.reader.data_size = READ_BUFFER_SIZE;
+  state.reader.reader.count = 0;
+  state.reader.reader.pos = 0;
+}
+
+static int receive_hook(HIJACKNET_DRIVER *driver,
+                        void *data, int length,
+                        void *context)
+{
+  FP_CONTEXT  fp_context;
+  unsigned int free, avail;
+  unsigned char *src = data;
+  
+  fppSave(&fp_context);
+  avail = (state.reader.write_pos - state.reader.read_pos) % READ_BUFFER_SIZE;
+  free = READ_BUFFER_SIZE - avail;
+  if (free >= length + 2) {
+    unsigned int i, pos;
+
+    pos = state.reader.write_pos;
+    state.reader.buffer[pos] = (length & 0xff00) >> 8;
+    pos = (pos + 1) % READ_BUFFER_SIZE;
+    state.reader.buffer[pos] = (length & 0xff);
+    pos = (pos + 1) % READ_BUFFER_SIZE;
+    for (i = 0 ; i < length ; i++) {
+      state.reader.buffer[pos] = src[i];
+      pos = (pos + 1) % READ_BUFFER_SIZE;
+    }
+    state.reader.write_pos = pos;
+  }
+
+  fppRestore(&fp_context);
+  return length;
+}
+
+static int lp_consume()
+{
+  unsigned int avail;
+
+  while (1) {
+    avail = (state.reader.write_pos - state.reader.read_pos) % READ_BUFFER_SIZE;
+    if (avail <= 2) {
+      semTake(state.reader.sem, 1000);
+    } else {
+      unsigned int length, pos, sane, i, channel, cookie, index;
+      unsigned int frag_num, frag_length;
+      unsigned short id;
+      unsigned char flags;
+      
+      pos = state.reader.read_pos;
+      printf("read_pos=%d avail=%d\n", pos, avail);
+      
+      /* Length of raw packet */
+      length = state.reader.buffer[pos] << 8;
+      pos = (pos + 1) % READ_BUFFER_SIZE;
+      printf("length=%d pos=%d\n", length, pos);
+      length |= state.reader.buffer[pos];
+      pos = (pos + 1) % READ_BUFFER_SIZE;
+      printf("length=%d pos=%d\n", length, pos);
+
+      /* Setup reader */
+      state.reader.available = length;
+      state.reader.reader.pos = pos;
+      labcomm2014_reader_fill(&state.reader.reader,
+                              state.reader.reader.action_context);
+
+
+      /* Sanity check package */
+      sane = 1;
+      for (i = 0 ; i < 6 ; i++) {
+        unsigned char b;
+
+        b = labcomm2014_read_byte(&state.reader.reader);
+        sane &= (b == state.src[i]);
+      }
+      printf("sane=%d\n", sane);
+      for (i = 0 ; i < 6 ; i++) {
+        unsigned char b;
+
+        b = labcomm2014_read_byte(&state.reader.reader);
+        sane &= (b == state.dst[i]);
+      }
+      id = labcomm2014_read_short(&state.reader.reader);
+      sane &= (id == EXTCTRL2015_ETH_ID);
+      flags = labcomm2014_read_byte(&state.reader.reader);
+      channel = labcomm2014_read_packed32(&state.reader.reader);
+      sane &= (channel == state.channel);
+      cookie = labcomm2014_read_int(&state.reader.reader);
+      index = labcomm2014_read_int(&state.reader.reader);
+      frag_num = labcomm2014_read_packed32(&state.reader.reader);
+      frag_length = labcomm2014_read_packed32(&state.reader.reader);
+
+      printf("sane=%d flags=%02x channel=%d cookie=%x index=%x\n",
+             sane, flags, channel, cookie, index);
+
+      if (!sane) { goto out; }
+      if ((flags & flag_LAST_FRAG) == 0) {
+        printf("Implement defragmentation\n");
+        goto out;
+      }
+      if ((flags & flag_IS_ACK) != 0) {
+        if ((flags & kind_MASK) == kind_INIT && state.reader.sequence == 0) {
+          unsigned int remote_index;
+          remote_index = labcomm2014_read_int(&state.reader.reader);
+          state.reader.sequence = remote_index;
+          state.cookie = state.reader.sequence ^ state.writer.sequence;
+          printf("remote_index=%x cookie=%x\n", remote_index, state.cookie);
+        }
+        if (index == state.writer.awaited_ack) {
+          state.writer.awaited_ack = 0;
+          semGive(state.writer.sem);
+        }
+      }
+      /* Decode one package */
+      
+
+    out:
+      /* Update read position */
+      state.reader.read_pos =
+        (state.reader.read_pos + 2 + length) % READ_BUFFER_SIZE;
+      
+
+    }
+    printf("Available: %d\n", avail);
+  }
+  return 0;
+}
+
 static int read_robot_data(char *path)
 {
   int result = 0;
@@ -1249,7 +1464,22 @@ int hijacknet_extctrl2014_connect(char *interface,
     return -9;
   }
 
+  state.cpu_MHz = cpuFrequency / 1000000;
+  state.mutex = semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE);
+  state.sem = semBCreate(0, 0);
+  state.sem_communicate = semBCreate(0, 0);
+  state.writer.sem = semBCreate(0, 0);
+  state.writer.sequence = rdtsc();
+  state.reader.sem = semBCreate(0, 0);
+
+  /* Enumerate signatures, since no proper code init in VxWorks */
   init_extctrl2014__signatures();
+  init_writer();
+  init_reader();
+
+  state.reader.lpconsume = taskSpawn("HNlpconsume", 254, 0, 20000, lp_consume,
+                                     0,0,0,0,0,0,0,0,0,0);
+
   state.driver = hijacknet_open(interface);
   hijacknet_get_mac_address(state.driver, state.src);
   
@@ -1257,7 +1487,9 @@ int hijacknet_extctrl2014_connect(char *interface,
   printf("%s", etoa(state.src));
 
   hijacknet_extctrl2014_status();
-  init_writer();
+  hijacknet_add_receive_hook(state.driver, EXTCTRL2015_ETH_ID,
+                             receive_hook, NULL);
+  
   /* Send INIT */
   labcomm2014_writer_start(&state.writer.writer,
                            state.writer.writer.action_context,
@@ -1266,26 +1498,27 @@ int hijacknet_extctrl2014_connect(char *interface,
                          state.writer.writer.action_context);
 
   /* register data types */
-  state.encoder.encoder = labcomm2014_encoder_new(
+  state.writer.encoder = labcomm2014_encoder_new(
     &state.writer.writer,
     labcomm2014_default_error_handler,
     labcomm2014_default_memory,
     labcomm2014_default_scheduler);
   labcomm2014_encoder_register_extctrl2014_irb2ext_net(
-    state.encoder.encoder);
+    state.writer.encoder);
   labcomm2014_encoder_register_extctrl2014_force_torque_net(
-    state.encoder.encoder);    
-  labcomm2014_encoder_free(state.encoder.encoder);
+    state.writer.encoder);    
+  taskDelay(30000);
+  labcomm2014_encoder_free(state.writer.encoder);
+  hijacknet_del_receive_hook(state.driver, EXTCTRL2015_ETH_ID,
+                             receive_hook, NULL);
   hijacknet_close(state.driver);
+  printf("%d %d\n", state.reader.write_pos, state.reader.read_pos);
+  taskDelete(state.reader.lpconsume);
 #if 0
 #endif
 //  state.decoder.decoder = labcomm2014_decoder_new(reader, &state);
 
 //  state.unit = unit;
-  state.cpu_MHz = cpuFrequency / 1000000;
-  state.mutex = semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE);
-  state.sem = semBCreate(0, 0);
-  state.sem_communicate = semBCreate(0, 0);
 
 #if 0
   if (lth_net_add_hook(state.unit, hook, &state) != 0) {

mc/hijacknet_fei.mc.c

@@ -392,7 +392,6 @@ static void hijackedInt(DRV_CTRL *pDrvCtrl)
 	   tag = &pDrvCtrl->rbdTags[hijack[unit].irqRbdIndex]) {
       unsigned char *data;
       int length, newlength, ether_type;
-//      struct hijacknet_hook *hook;
 
       data = (unsigned char*)tag->pMblk->mBlkHdr.mData;
       length = status & 0x3fff;
@@ -577,54 +576,67 @@ static int fei_hijacknet_send(HIJACKNET_DRIVER *driver,
 }
 
 static int fei_hijacknet_close(HIJACKNET_DRIVER *driver)
-{
-  printf("IMPLEMENT %s\n", __FUNCTION__);
-#if 0
-int hijacknet_uninstall(int unit)
 {
   int result = 0;
-  END_OBJ *interface;
+  int old_level;
+  struct hijack *hijack;
   DRV_CTRL *drv;
   VOIDFUNCPTR *vector;
-  int old_level;
 
-  if (unit < 0 || unit >= NUM_INTERFACES) {
-    printf("Index out of bounds %d\n", unit);
+  printf("NOT USING new %s (driver still stuck in memory)\n", __FUNCTION__);
+  goto out;
+  
+  hijack = driver_to_hijack(driver);
+  if (hijack == NULL) {
     result = 1;
     goto out;
   }
-  interface = endFindByName("fei", unit);
-  if (interface == NULL) {
-    printf("Failed to find fei%d\n", unit);
+  drv = hijack->drv;  
+  vector = INUM_TO_IVEC (drv->board.vector);
+
+  old_level  = intLock();
+  hijack->active--;
+  if (hijack->active > 0) {
+    /* Somebody else has driver open */
     result = 2;
-    goto out;
+    goto out_unlock;
   }
-  drv = (DRV_CTRL*)interface;
-  vector = INUM_TO_IVEC (drv->board.vector);
-  if (pciIntDisconnect2(vector, hijack_irq_handler, (int)drv) != 0) {
-    printf("Failed to disconnect hijack_irq_handler from fei%d\n", unit);
+  if (pciIntDisconnect2(vector, hijackedInt, (int)drv) != 0) {
     result = 3;
-    goto out;
+    goto out_unlock;
   }
-  old_level  = intLock();
-  hijack[unit].active = 0;
-  hijack[unit].receive_hook = NULL;
-  hijack[unit].send_hook = NULL;
-  intUnlock(old_level);
+  drv->endObj.pFuncTable = hijack->origNetFuncs;
   pciIntConnect(vector, fei82557Int, (int)drv);
-  printf("fei%d connected to fei82557Int\n", unit);
+  result = 4;
   
+out_unlock:
+  intUnlock(old_level);
 out:
-  return result;
+  switch (result) {
+    case 1: {
+      printf("No corresponding hijack for driver %p\n", driver);
+    } break;
+    case 2: {
+      printf("More processes using hijack driver\n");
+    } break;
+    case 3: {
+      printf("Failed to disconnect hijack_irq_handler from fei\n");
+    } break;
+    case 4: {
+      printf("fei reconnected to fei82557Int\n");
+    } break;
+    default: {
+      printf("Unknown result %d\n", result);
+    } break;
   }
-#endif
-  return 0;
+
+  return result;
 }
 
 HIJACKNET_DRIVER *fei_hijacknet_open(int unit)
 {
   HIJACKNET_DRIVER *result = NULL;
-  int error = 0;
+  int error = 0, info = 0;
   END_OBJ *interface;
   DRV_CTRL *drv;
   VOIDFUNCPTR *vector;
@@ -639,6 +651,11 @@ HIJACKNET_DRIVER *fei_hijacknet_open(int unit)
     goto out;
   }
   result = &hijack[unit].driver;
+  if (hijack[unit].active >= 1) {
+    snprintf(msg, sizeof(msg), "Locking hijack driver fei%d in memory\n", unit);
+    info = 1;
+    goto out;
+  }
   hijack[unit].active++;
   if (hijack[unit].active > 1) {
     /* device has already been successfully opened */
@@ -691,7 +708,9 @@ error_out:
   hijack[unit].active--;
 out:
   intUnlock(old_level);
-  if (error) {
+  if (info) {
+    printf("%s", msg);
+  } else if (error) {
     printf("%s", msg);
     result = NULL;
   }

simulator/ethernet.py

@@ -31,6 +31,8 @@ class ETH(object):
         u = random.uniform(0, 1.0)
         if u >= self.send_loss:
             data = data[0:6] + self.macaddr + data[12:]
+            if len(data) < 60:
+                data = data.ljust(60, '\0')
             self.socket.send(data)
         else:
             logger.log('Dropped sent packet')

simulator/extctrl2014.py

@@ -216,7 +216,7 @@ class ExtCtrl(object):
         self.max_length = max_length
         self.read_data = list()
         self.cond = threading.Condition()
-        self.local_index = random.randint(0,2**32-1)
+        self.local_index = random.randint(1,2**32-1)
         self.remote_index = None
         self.cookie = None
         self.ack = None
@@ -228,6 +228,8 @@ class ExtCtrl(object):
         elif self.robot:
             self.await_INIT()
 
+    def next_index(self):
+        self.local_index = max(1, (self.local_index + 1) & 0xffffffff)
 
     def recv_loop(self):
         ack = None
@@ -330,7 +332,7 @@ class ExtCtrl(object):
         for i in self.retries(self.local_index):
             for f in fragmenter.fragments(max_length=self.max_length):
                 self.ethernet.send(f.getvalue())
-        self.local_index += 1
+        self.next_index()
 
     def send_INIT_ACK(self):
         flags = kind_INIT | flag_IS_ACK
@@ -364,7 +366,7 @@ class ExtCtrl(object):
         for i in self.retries(self.local_index):
             for f in fragmenter.fragments(max_length=self.max_length):
                 self.ethernet.send(f.getvalue())
-        self.local_index += 1
+        self.next_index()
 
     def send_DATA(self, data):
         flags = kind_DATA
@@ -376,7 +378,7 @@ class ExtCtrl(object):
         fragmenter.write(data)
         for f in fragmenter.fragments(max_length=self.max_length):
             self.ethernet.send(f.getvalue())
-        self.local_index += 1
+        self.next_index()
 
     def writer(self):
         class Writer: