diff --git a/lib/nodes/can-test/Makefile b/lib/nodes/can-test/Makefile deleted file mode 100644 index b31ede6f4..000000000 --- a/lib/nodes/can-test/Makefile +++ /dev/null @@ -1,40 +0,0 @@ -CC_PATH=/opt/Xilinx/SDK/2019.1/gnu/aarch64/lin/aarch64-linux/bin -CC_PREFIX=aarch64-linux-gnu- -CPATH=$(CC_PATH)/include -LD_LIBRARY_PATH=$(CC_PATH)/lib64:$(CC_PATH)/lib - -CC=$(CC_PATH)/$(CC_PREFIX)gcc -LD=$(CC) - -CC_FLAGS= -LD_FLAGS= - -OBJ_FILES=main.o -SRC_FILES=main.c -MISC_FILES=create_vcan.sh -FPGA_IP=137.226.133.231 - -BINARY_SEND=can_send -BINARY_RECEIVE=can_receive - -.PHONY: all clean deploy - -all : $(BINARY_SEND) $(BINARY_RECEIVE) - -$(BINARY_SEND) : $(SRC_FILES) - $(LD) $(CC_FLAGS) -DSEND -o $@ $^ $(LD_FLAGS) - -$(BINARY_RECEIVE) : $(SRC_FILES) - $(LD) $(CC_FLAGS) -o $@ $^ $(LD_FLAGS) - -#%.o : %.c -# $(CC) $(CC_FLAGS) -c -o $@ $< - -clean: - rm -f $(BINARY) $(OBJ_FILES) - -deploy: $(BINARY) - scp $(BINARY) root@$(FPGA_IP):~ - scp $(MISC_FILES) root@$(FPGA_IP):~ - ssh root@$(FPGA_IP) /home/root/create_vcan.sh - ssh root@$(FPGA_IP) /home/root/$(BINARY) diff --git a/lib/nodes/can-test/create_vcan.sh b/lib/nodes/can-test/create_vcan.sh deleted file mode 100755 index b464d1aa7..000000000 --- a/lib/nodes/can-test/create_vcan.sh +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/bash - -# source: https://en.wikipedia.org/wiki/SocketCAN - -ip link add dev vcan0 type vcan -ip link set up vcan0 -ip link show vcan0 diff --git a/lib/nodes/can-test/main.c b/lib/nodes/can-test/main.c deleted file mode 100644 index 40f4a520c..000000000 --- a/lib/nodes/can-test/main.c +++ /dev/null @@ -1,189 +0,0 @@ -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - - -/** An accurate timestamp can be obtained with an ioctl(2) call after reading - * a message from the socket: - * - * struct timeval tv; - * ioctl(s, SIOCGSTAMP, &tv); - * - * The timestamp has a resolution of one microsecond and is set automatically - * at the reception of a CAN frame. - */ -void can_receive_timestamp(int socket) -{ - struct timeval tv; - ioctl(socket, SIOCGSTAMP, &tv); - printf("%0d:%03d:%03d s:ms:us\n", tv.tv_sec, tv.tv_usec/1000, tv.tv_usec%1000); -} - -int can_receive(char *ifn) -{ - int s; - int nbytes; - struct sockaddr_can addr; - struct can_frame frame; - struct ifreq ifr; - - const char *ifname = ifn; - struct timeval start, end; - - if((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { - perror("Error while opening socket"); - return -1; - } - - strcpy(ifr.ifr_name, ifname); - ioctl(s, SIOCGIFINDEX, &ifr); - - addr.can_family = AF_CAN; - addr.can_ifindex = ifr.ifr_ifindex; - - printf("%s at index %d\n", ifname, ifr.ifr_ifindex); - - if(bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) { - perror("Error in socket bind"); - return -2; - } - int i,j; - printf("warmup...\n"); - for (j=0; j < 10; ++j) { - for (i=0; i < 1000; ++i) { - nbytes = read(s, &frame, sizeof(struct can_frame)); - if (nbytes != sizeof(struct can_frame)) { - printf("ERROR: can read() error. read() returned %d\n", nbytes); - } - //can_receive_timestamp(s); - //printf("id:%d, len:%d, data: 0x%x:0x%x\n", frame.can_id, - // frame.can_dlc, - // ((uint32_t*)&frame.data)[0], - // ((uint32_t*)&frame.data)[1]); - } - } - - printf("measure...\n"); - int sec=0, usec=0; - for (j=0; j < 10; ++j) { - gettimeofday(&start, NULL); - for (i=0; i < 1000; ++i) { - nbytes = read(s, &frame, sizeof(struct can_frame)); - } - gettimeofday(&end, NULL); - sec += end.tv_sec-start.tv_sec; - usec += end.tv_usec-start.tv_usec; - printf("%d frames in %0d:%03d:%03d s:ms:us\n", i, end.tv_sec-start.tv_sec, - (end.tv_usec-start.tv_usec)/1000, - (end.tv_usec-start.tv_usec)%1000); - } - usec=((usec + sec*1000*1000)/j) % (1000*1000); - sec /= j; - - printf("average: %0d:%03d:%03d s:ms:us\n", sec, usec/1000, usec%1000); - usec=((usec + sec*1000*1000)/i) % (1000*1000); - sec /= i; - printf("average per read: %0d:%03d:%03d s:ms:us\n", sec, usec/1000, usec%1000); - return 0; -} - - - -int can_send(char *ifn) -{ - int s; - int nbytes; - struct sockaddr_can addr; - struct can_frame frame; - struct ifreq ifr; - - const char *ifname = ifn; - uint32_t high = 0, low = 0; - struct timeval start, end; - - if((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { - perror("Error while opening socket"); - return -1; - } - - printf("%d,%d\n", a.tv_sec, a.tv_usec); - - strcpy(ifr.ifr_name, ifname); - ioctl(s, SIOCGIFINDEX, &ifr); - - addr.can_family = AF_CAN; - addr.can_ifindex = ifr.ifr_ifindex; - - printf("%s at index %d\n", ifname, ifr.ifr_ifindex); - - if(bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) { - perror("Error in socket bind"); - return -2; - } - - frame.can_id = 0x1; - frame.can_dlc = 8; - - int i,j; - printf("warmup...\n"); - for (j=0; j < 10; ++j) { - for (i=0; i < 1000; ++i) { - ((uint32_t*)frame.data)[0] = high++; - ((uint32_t*)frame.data)[1] = low++; - - nbytes = write(s, &frame, sizeof(struct can_frame)); - usleep(250); - } - } - - printf("measure...\n"); - int sec=0, usec=0; - for (j=0; j < 10; ++j) { - gettimeofday(&start, NULL); - for (i=0; i < 1000; ++i) { - ((uint32_t*)frame.data)[0] = high++; - ((uint32_t*)frame.data)[1] = low++; - - nbytes = write(s, &frame, sizeof(struct can_frame)); - usleep(250); - } - gettimeofday(&end, NULL); - sec += end.tv_sec-start.tv_sec; - usec += end.tv_usec-start.tv_usec; - printf("%d frames in %0d:%03d:%03d s:ms:us\n", i, end.tv_sec-start.tv_sec, - (end.tv_usec-start.tv_usec)/1000, - (end.tv_usec-start.tv_usec)%1000); - } - usec=((usec + sec*1000*1000)/j) % (1000*1000); - sec /= j; - - printf("average: %0d:%03d:%03d s:ms:us\n", sec, usec/1000, usec%1000); - usec=((usec + sec*1000*1000)/i) % (1000*1000); - sec /= i; - printf("average per write: %0d:%03d:%03d s:ms:us\n", sec, usec/1000, usec%1000); - return 0; -} - -int main() -{ -#ifdef SEND - printf("can_send\n"); - return can_send("vcan0"); -#else - printf("can_receive\n"); - return can_receive("vcan0"); -#endif - -} - diff --git a/lib/nodes/can.cpp b/lib/nodes/can.cpp index c891ba3cc..a350cc155 100644 --- a/lib/nodes/can.cpp +++ b/lib/nodes/can.cpp @@ -203,6 +203,8 @@ int can_parse(struct node *n, json_t *cfg) if (ret != 0) { free(c->in); free(c->out); + c->in = nullptr; + c->out = nullptr; } return ret; } @@ -332,7 +334,7 @@ int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rele warning("Could not get timestamp from CAN driver on interface \"%s\".", c->interface_name); smps[nread]->ts.received = time_now(); } else { - TIMEVAL_TO_TIMESPEC(&tv, &smps[nread]->ts.received); + TIMEVAL_TO_TIMESPEC(&tv, &smps[nread]->ts.received); } for (size_t i=0; i < vlist_length(&(n->in.signals)); i++) { @@ -344,31 +346,71 @@ int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rele ((uint8_t*)&frame.data) + c->in[i].offset, c->in[i].size); } - } /* Set signals, because other VILLASnode parts expect us to */ smps[nread]->signals = &n->in.signals; } - - - return 1; + return nread; } int can_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release) { int nbytes; - struct can_frame frame = {0}; - struct can *c = (struct can *) n->_vd; + unsigned nwrite; + struct can_frame *frame; + size_t fsize = 0; /* number of frames in use */ + + struct can *c = (struct can *) n->_vd; + + assert(cnt >= 1 && smps[0]->capacity >= 1); + + frame = (struct can_frame*) calloc(sizeof(struct can_frame), + vlist_length(&(n->out.signals))); + + for (nwrite=0; nwrite < cnt; nwrite++) { + for (size_t i=0; i < vlist_length(&(n->out.signals)); i++) { + if (c->out[i].offset != 0) { /* frame is shared */ + continue; + } + frame[fsize].can_dlc = c->out[i].size; + frame[fsize].can_id = c->out[i].id; + memcpy(((uint8_t*)&frame[fsize].data) + c->out[i].offset, + &smps[nwrite]->data[i], + c->in[i].size); + fsize++; + } + for (size_t i=0; i < vlist_length(&(n->out.signals)); i++) { + if (c->out[i].offset == 0) { /* frame already stored */ + continue; + } + for (size_t j=0; j < fsize; j++) { + if (c->out[i].id == frame[j].can_id) { + frame[j].can_dlc += c->out[i].size; + memcpy(((uint8_t*)&frame[j].data) + c->out[i].offset, + &smps[nwrite]->data[i], + c->in[i].size); + } + } + } + for (size_t j=0; j < fsize; j++) { + printf("id:%d, len:%u, data: 0x%x:0x%x\n", frame[j].can_id, + frame[j].can_dlc, + ((uint32_t*)&frame[j].data)[0], + ((uint32_t*)&frame[j].data)[1]); + + if ((nbytes = write(c->socket, &frame, sizeof(struct can_frame))) == -1) { + error("CAN write() returned -1. Is the CAN interface up?"); + return nwrite; + } + if ((unsigned)nbytes != sizeof(struct can_frame)) { + error("CAN write() error. write() returned %d bytes but expected %zu", + nbytes, sizeof(struct can_frame)); + return nwrite; + } + } - nbytes = write(c->socket, &frame, sizeof(struct can_frame)); - if (nbytes != sizeof(struct can_frame)) { - error("CAN write() error. write() returned %u bytes but expected %zu", - nbytes, sizeof(struct can_frame)); - return 0; } - - - return 1; + return nwrite; } int can_reverse(struct node *n)