Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found
Select Git revision

Target

Select target project
  • anders_blomdell/extctrl2014
  • tommyo/extctrl2014
2 results
Select Git revision
Show changes
Commits on Source (9)
......@@ -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) {
......
......@@ -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;
}
......
......@@ -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')
......
......@@ -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:
......