Correct sequence wrap around for simulator, add intermediate VxWorks code.

mc/hijacknet_extctrl2014.c

@@ -44,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;
 
@@ -86,18 +87,26 @@ static struct state {
   int channel;
   int cookie;
   int received;
-  struct encoder_writer {
+  struct {
     struct labcomm2014_writer writer;
     struct labcomm2014_writer_action_context action_context;
+    struct labcomm2014_encoder *encoder;
+    int sequence;
     int unadjusted;
     int need_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 labcomm2014_reader reader;
+    struct labcomm2014_reader_action_context action_context;
+    SEM_ID sem;
+    int lpconsume;
+    unsigned char buffer[READ_BUFFER_SIZE];
+    unsigned int read_pos;
+    unsigned int write_pos;
+    unsigned int available;
+  } reader;
   struct {
     struct labcomm2014_decoder *decoder;
     int sequence;
@@ -956,8 +965,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;
@@ -1016,7 +1025,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;
@@ -1049,6 +1058,7 @@ static int writer_end(
   printf("%s %d\n", __FUNCTION__, w->pos);
   hijacknet_send(state.driver, w->data, w->pos);
 
+  taskDelay(1000);
   return w->error;
 }
 
@@ -1079,23 +1089,177 @@ 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);
-  state.received += length;
+  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) {
+    semTake(state.reader.sem, 1000);
+    avail = (state.reader.write_pos - state.reader.read_pos) % READ_BUFFER_SIZE;
+    if (avail > 2) {
+      unsigned int length, pos, sane, i, channel, cookie, index;
+      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);
+
+      printf("sane=%d flags=%02x channel=%d cookie=%x index=%u\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 (index == state.ack) {
+          self.ack = 0;
+          NOTIFY;
+        }
+        if (index ==
+      /* 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;
@@ -1269,8 +1433,20 @@ 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.reader.sem = semBCreate(0, 0);
+
+  /* Enumerate signatures, since no proper code init in VxWorks */
   init_extctrl2014__signatures();
-  state.received = 0;
+  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);
   
@@ -1278,7 +1454,6 @@ 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);
   
@@ -1289,34 +1464,32 @@ int hijacknet_extctrl2014_connect(char *interface,
   labcomm2014_writer_end(&state.writer.writer,
                          state.writer.writer.action_context);
   /* Immediate retry... */
+  taskDelay(1000);
   labcomm2014_writer_end(&state.writer.writer,
                          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);    
-  taskDelay(1000);
-  labcomm2014_encoder_free(state.encoder.encoder);
+    state.writer.encoder);    
+  taskDelay(10000);
+  labcomm2014_encoder_free(state.writer.encoder);
   hijacknet_del_receive_hook(state.driver, EXTCTRL2015_ETH_ID,
                              receive_hook, NULL);
   hijacknet_close(state.driver);
-  printf("Received: %d\n", state.received);
+  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) {

simulator/extctrl2014.py

@@ -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: