diff --git a/mc/hijacknet_extctrl2014.c b/mc/hijacknet_extctrl2014.c
index ba77d4e855ae5f84092851379a6f9c784efe67b7..5fd573c74f1e532f591b7c96ccb060fa47c34bea 100644
--- a/mc/hijacknet_extctrl2014.c
+++ b/mc/hijacknet_extctrl2014.c
@@ -91,16 +91,19 @@ static struct state {
struct labcomm2014_writer writer;
struct labcomm2014_writer_action_context action_context;
struct labcomm2014_encoder *encoder;
- int sequence;
+ SEM_ID sem;
+ unsigned int sequence;
int unadjusted;
- int need_ack;
+ int awaited_ack;
int frag_start;
unsigned char buffer[WRITE_BUFFER_SIZE];
} writer;
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;
@@ -108,7 +111,6 @@ static struct state {
unsigned int available;
} reader;
struct {
- struct labcomm2014_decoder *decoder;
int sequence;
int fragment;
int pos;
@@ -148,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
@@ -164,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;
@@ -998,12 +1008,13 @@ 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;
@@ -1048,17 +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);
+ while (last < 60) {
+ /* Null padding at end */
+ w->data[last] = 0;
+ last++;
+ }
w->pos = last;
}
printf("%s %d\n", __FUNCTION__, w->pos);
- hijacknet_send(state.driver, w->data, w->pos);
-
- taskDelay(1000);
+ 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;
}
@@ -1183,10 +1203,12 @@ 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) {
+ 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;
@@ -1230,8 +1252,10 @@ static int lp_consume()
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=%u\n",
+ printf("sane=%d flags=%02x channel=%d cookie=%x index=%x\n",
sane, flags, channel, cookie, index);
if (!sane) { goto out; }
@@ -1240,11 +1264,18 @@ static int lp_consume()
goto out;
}
if ((flags & flag_IS_ACK) != 0) {
- if (index == state.ack) {
- self.ack = 0;
- NOTIFY;
+ 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);
}
- if (index ==
+ }
/* Decode one package */
@@ -1437,6 +1468,8 @@ int hijacknet_extctrl2014_connect(char *interface,
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 */
@@ -1463,10 +1496,6 @@ int hijacknet_extctrl2014_connect(char *interface,
0, NULL, NULL);
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.writer.encoder = labcomm2014_encoder_new(
@@ -1478,7 +1507,7 @@ int hijacknet_extctrl2014_connect(char *interface,
state.writer.encoder);
labcomm2014_encoder_register_extctrl2014_force_torque_net(
state.writer.encoder);
- taskDelay(10000);
+ taskDelay(30000);
labcomm2014_encoder_free(state.writer.encoder);
hijacknet_del_receive_hook(state.driver, EXTCTRL2015_ETH_ID,
receive_hook, NULL);