Commit 722a5f16 authored by Anders Blomdell's avatar Anders Blomdell
Browse files

Intermidate version, some acking works.

parent 2306e057
......@@ -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);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment