diff --git a/mc/hijacknet_extctrl2014.c b/mc/hijacknet_extctrl2014.c
index b1c0919cfc6a90e1ef020fe70c5ec5099008a472..ba77d4e855ae5f84092851379a6f9c784efe67b7 100644
--- a/mc/hijacknet_extctrl2014.c
+++ b/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;
@@ -1048,7 +1057,8 @@ 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) {
diff --git a/simulator/extctrl2014.py b/simulator/extctrl2014.py
index 28e7029a21484146b3b818a888dc4cae87539acd..4ed94e443b5b56bd5736ba65350565e1515b0c01 100644
--- a/simulator/extctrl2014.py
+++ b/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
@@ -227,7 +227,9 @@ class ExtCtrl(object):
self.send_INIT()
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: