CFLAGS += -Wall -g
-BAPLIB = vw-bap.c vw-bap-frame.c vw-bap-rx.c vw-bap-tx.c
+BAPLIB = vag-bap.c vag-bap-frame.c vag-bap-rx.c vag-bap-tx.c
-all: vw-bap-dump vw-bap-sniffer vw-bap-send
+EXES = vag-bap-dump vag-bap-sniffer vag-bap-send
-vw-bap-dump: vw-bap-dump.c $(BAPLIB)
+all: $(EXES)
-vw-bap-sniffer: vw-bap-sniffer.c $(BAPLIB)
+
+vag-bap-dump: vag-bap-dump.c $(BAPLIB)
+
+vag-bap-sniffer: vag-bap-sniffer.c $(BAPLIB)
gcc -o $@ $^ -lncurses
-vw-bap-send: vw-bap-send.c $(BAPLIB)
+vag-bap-send: vag-bap-send.c $(BAPLIB)
+
+
+.PHONY: clean
+clean:
+ rm -f $(EXES)
--- /dev/null
+/*
+ * Copyright 2017 Max Staudt
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License 2 as published
+ * by the Free Software Foundation.
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
+
+#include "vag-bap.h"
+
+
+
+
+static int net_init(char *ifname)
+{
+ int s;
+ int recv_own_msgs;
+ struct sockaddr_can addr;
+ struct ifreq ifr;
+
+ s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+ if (s < 0) {
+ perror("socket");
+ exit(1);
+ }
+
+ /* Convert interface name to index */
+ memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
+ strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
+ if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
+ perror("SIOCGIFINDEX");
+ exit(1);
+ }
+
+ /* Open the CAN interface */
+ memset(&addr, 0, sizeof(addr));
+ addr.can_family = AF_CAN;
+ addr.can_ifindex = ifr.ifr_ifindex;
+ if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+ perror("bind");
+ return 0;
+ }
+
+ recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
+ setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
+ &recv_own_msgs, sizeof(recv_own_msgs));
+
+ return s;
+}
+
+
+
+
+
+int main(int argc, char **argv)
+{
+ fd_set rdfs;
+ int s;
+ int i;
+ int can_id_count;
+ unsigned *can_ids;
+ struct BAP_RXer **baps;
+
+ if (argc < 3) {
+ printf("syntax: %s IFNAME CAN_ID [CAN_ID CAN_ID ...]\n", argv[0]);
+ return 1;
+ }
+
+ can_id_count = argc - 2;
+
+ can_ids = calloc(can_id_count, sizeof(*can_ids));
+ baps = calloc(can_id_count, sizeof(*baps));
+ if (!can_ids || !baps) {
+ printf("Out of memory allocating meta structs.\n");
+ return 1;
+ }
+
+ for (i = 0; i < can_id_count; i++) {
+ can_ids[i] = strtoul(argv[2 + i], NULL, 0);
+ baps[i] = vag_bap_rxer_alloc();
+ if (!baps[i]) {
+ printf("Out of memory allocating BAP struct.\n");
+ return 1;
+ }
+
+ printf("Listening for CAN ID: %x\n", can_ids[i]);
+ }
+
+
+ s = net_init(argv[1]);
+
+ while (1) {
+ int retval;
+
+ FD_ZERO(&rdfs);
+ FD_SET(s, &rdfs);
+
+ retval = select(s+1, &rdfs, NULL, NULL, NULL);
+ /* We currently rely on Linux timeout behavior here,
+ * i.e. the timeout now reflects the remaining time */
+ if (retval < 0) {
+ perror("select");
+ return 1;
+ } else if (!retval) {
+ /* Timeout, we NEED to check this first */
+ } else if (FD_ISSET(s, &rdfs)) {
+ struct can_frame frame;
+ ssize_t ret;
+
+ ret = read(s, &frame, sizeof(frame));
+ if (ret < 0) {
+ perror("read from CAN socket");
+ return 1;
+ }
+
+ for (i = 0; i < can_id_count; i++) {
+ if (can_ids[i] == frame.can_id) {
+ struct BAP_Frame *bap_frame;
+ struct BAP_RXer *bap = baps[i];
+
+ bap_frame = vag_bap_handle_can_frame(bap, &frame);
+ if (bap_frame) {
+ printf("%03x: ", frame.can_id);
+ vag_bap_frame_dump(bap_frame);
+ vag_bap_frame_free(bap_frame);
+ }
+
+ break;
+ }
+ }
+
+ continue;
+ }
+ }
+
+ /* TODO: Free */
+
+ close(s);
+
+ return 0;
+}
--- /dev/null
+#include <ctype.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "vag-bap.h"
+
+
+
+struct BAP_Frame* vag_bap_frame_alloc(void)
+{
+ struct BAP_Frame* bap_frame;
+
+ bap_frame = calloc(1, sizeof(struct BAP_Frame));
+
+ return bap_frame;
+}
+
+
+void vag_bap_frame_free(struct BAP_Frame *bap_frame)
+{
+ free(bap_frame);
+}
+
+
+int vag_bap_frame_is_valid(struct BAP_Frame *bap_frame)
+{
+ if ( 0
+ || (bap_frame->opcode > 7)
+ || (bap_frame->node > 63)
+ || (bap_frame->port > 63)
+ || (bap_frame->len > 4095)
+ || (!bap_frame->is_multiframe && bap_frame->len > 6)
+ ) {
+ return 0;
+ }
+
+ return 1;
+}
+
+
+struct BAP_Frame* vag_bap_frame_clone(struct BAP_Frame *bap_frame)
+{
+ struct BAP_Frame *new_frame;
+
+ if (!vag_bap_frame_is_valid(bap_frame)) {
+ return NULL;
+ }
+
+ new_frame = vag_bap_frame_alloc();
+ if (!new_frame) {
+ return NULL;
+ }
+
+ memcpy(new_frame, bap_frame, sizeof(*new_frame));
+
+ return new_frame;
+}
+
+
+
+void vag_bap_frame_dump(struct BAP_Frame *bap_frame)
+{
+ unsigned i;
+
+ printf("%u. %2i/%-2i .%02i --",
+ bap_frame->opcode,
+ bap_frame->node,
+ bap_frame->port,
+ bap_frame->len);
+
+ for (i = 0; i < bap_frame->len; i++) {
+ if (!(i % 4)) {
+ printf(" ");
+ }
+ printf("%02x", (unsigned char)(bap_frame->data[i]));
+ }
+
+ printf("\n '");
+ for (i = 0; i < bap_frame->len; i++) {
+ unsigned char c = bap_frame->data[i];
+ if (!isprint(c) || c == '\n' || c == '\r') {
+ c = '.';
+ }
+ printf("%c", c);
+ }
+ printf("'");
+
+ printf("\n");
+
+ fflush(stdout);
+}
--- /dev/null
+#include <ctype.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <linux/can.h>
+
+#include "vag-bap.h"
+
+
+struct BAP_Frame* vag_bap_handle_can_frame(struct BAP_RXer *bap, struct can_frame *frame)
+{
+ struct BAP_Frame *bap_frame = NULL;
+ unsigned short header;
+ unsigned this_len;
+
+ //printf("Received BAP frame from CAN ID %03x\n", frame->can_id);
+
+ if (frame->can_dlc < 2) {
+ printf("Error: Frame too short\n");
+ }
+
+ if (frame->data[0] & 0x80) {
+ /* This is a multi-frame BAP message */
+ int mfchannel = (frame->data[0] >> 4) & 0x3;
+
+ if (!(frame->data[0] & 0x40)) {
+ /* Start frame */
+
+ unsigned short header;
+ unsigned this_len;
+
+ if (frame->can_dlc < 4) {
+ printf("Error: Frame too short\n");
+ }
+
+ if (bap->mfchannel[mfchannel]) {
+ printf("bap_handle_can_frame: new start frame for open channel\n");
+ }
+ bap->mfchannel[mfchannel] = NULL;
+
+ bap_frame = vag_bap_frame_alloc();
+ if (!bap_frame) {
+ printf("bap_handle_can_frame: Failed to allocate new frame\n");
+ return NULL;
+ }
+
+ bap_frame->is_multiframe = 1;
+
+ header = (frame->data[2] << 8) | frame->data[3];
+ bap_frame->opcode = (header >> 12) & 0x7;
+ bap_frame->node = (header >> 6) & 0x3F;
+ bap_frame->port = (header >> 0) & 0x3F;
+
+ bap_frame->len = ((frame->data[0] & 0xF) << 8) | frame->data[1];
+
+ this_len = frame->can_dlc - 4;
+
+ if (this_len > bap_frame->len) {
+ printf("bap_handle_can_frame: this_len > len\n");
+
+ free(bap_frame);
+ bap->mfchannel[mfchannel] = NULL;
+
+ return NULL;
+ }
+
+ memcpy(&bap_frame->data[0], &frame->data[frame->can_dlc - this_len], this_len);
+ bap->len_done[mfchannel] = this_len;
+
+ if (bap->len_done[mfchannel] == bap_frame->len) {
+ /* Frame is complete, remove from buffer */
+ bap->mfchannel[mfchannel] = NULL;
+ return bap_frame;
+ } else {
+ /* Frame is incomplete, don't emit it yet */
+ bap->mfchannel[mfchannel] = bap_frame;
+ return NULL;
+ }
+ } else {
+ /* Continuation frame */
+
+ bap_frame = bap->mfchannel[mfchannel];
+
+ if (!bap_frame) {
+ printf("bap_handle_can_frame: continuation frame for unknown mf channel %d\n",
+ mfchannel);
+ }
+
+ this_len = frame->can_dlc - 1;
+
+ if ((bap->len_done[mfchannel] + this_len) > bap_frame->len) {
+ printf("bap_handle_can_frame: len_done + this_len > len\n");
+
+ free(bap_frame);
+ bap->mfchannel[mfchannel] = NULL;
+
+ return NULL;
+ }
+
+ memcpy(&bap_frame->data[bap->len_done[mfchannel]],
+ &frame->data[frame->can_dlc - this_len],
+ this_len);
+ bap->len_done[mfchannel] += this_len;
+
+ if (bap->len_done[mfchannel] == bap_frame->len) {
+ /* Frame is complete, remove from buffer */
+ bap->mfchannel[mfchannel] = NULL;
+ return bap_frame;
+ } else {
+ /* Frame is incomplete, don't emit it yet */
+ return NULL;
+ }
+ }
+ } else {
+ /* Single-frame BAP message */
+
+ bap_frame = calloc(1, sizeof(struct BAP_Frame));
+ if (!bap_frame) {
+ printf("bap_handle_can_frame: Failed to allocate new frame\n");
+ return NULL;
+ }
+
+ bap_frame->is_multiframe = 0;
+
+ header = (frame->data[0] << 8) | frame->data[1];
+ bap_frame->opcode = (header >> 12) & 0x7;
+ bap_frame->node = (header >> 6) & 0x3F;
+ bap_frame->port = (header >> 0) & 0x3F;
+
+ this_len = frame->can_dlc - 2;
+
+ bap_frame->len = this_len;
+
+ memcpy(&bap_frame->data[0], &frame->data[frame->can_dlc - this_len], this_len);
+
+ return bap_frame;
+ }
+}
+
+
+
+
+
+struct BAP_RXer* vag_bap_rxer_alloc()
+{
+ struct BAP_RXer *bap;
+
+ bap = calloc(1, sizeof(*bap));
+ if (!bap) {
+ return NULL;
+ }
+
+ return bap;
+}
+
+
+
+
+void vag_bap_rxer_free(struct BAP_RXer *bap)
+{
+ int i;
+
+ for (i = 0; i < 8; i++) {
+ if (bap->mfchannel[i]) {
+ vag_bap_frame_free(bap->mfchannel[i]);
+ }
+ }
+
+ free(bap);
+}
--- /dev/null
+/*
+ * Copyright 2017 Max Staudt
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License 2 as published
+ * by the Free Software Foundation.
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
+
+#include "vag-bap.h"
+
+
+static unsigned hex2bin(char c) {
+ if (c >= '0' && c <= '9') {
+ return c - '0';
+ } else if (c >= 'A' && c <= 'F') {
+ return c + 10 - 'A';
+ } else if (c >= 'a' && c <= 'f') {
+ return c + 10 - 'a';
+ } else {
+ return 127;
+ }
+}
+
+
+
+static int net_init(char *ifname)
+{
+ int s;
+ int recv_own_msgs;
+ struct sockaddr_can addr;
+ struct ifreq ifr;
+
+ s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+ if (s < 0) {
+ perror("socket");
+ exit(1);
+ }
+
+ /* Convert interface name to index */
+ memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
+ strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
+ if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
+ perror("SIOCGIFINDEX");
+ exit(1);
+ }
+
+ /* Open the CAN interface */
+ memset(&addr, 0, sizeof(addr));
+ addr.can_family = AF_CAN;
+ addr.can_ifindex = ifr.ifr_ifindex;
+ if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+ perror("bind");
+ return 0;
+ }
+
+ recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
+ setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
+ &recv_own_msgs, sizeof(recv_own_msgs));
+
+ return s;
+}
+
+
+
+static void can_tx(int socket, struct can_frame *frame)
+{
+ ssize_t ret;
+
+ ret = write(socket, frame, sizeof(*frame));
+ if (ret != sizeof(*frame)) {
+ perror("write to CAN socket");
+ exit(1);
+ }
+}
+
+
+
+
+int main(int argc, char **argv)
+{
+ fd_set wfds;
+ int s, ret;
+ struct can_frame frame;
+ struct BAP_TXer *bap;
+ struct BAP_Frame *bap_frame;
+ unsigned can_id;
+ unsigned bap_multiframe;
+ unsigned bap_opcode;
+ unsigned bap_node;
+ unsigned bap_port;
+
+ if (argc < 8) {
+ printf("syntax: %s IFNAME CAN_ID multiframe bap_opcode bap_node bap_port bap_data\n", argv[0]);
+ return 1;
+ }
+
+ /* Parse arguments */
+ can_id = strtoul(argv[2], NULL, 0);
+ bap_multiframe = strtoul(argv[3], NULL, 0);
+ bap_opcode = strtoul(argv[4], NULL, 0);
+ bap_node = strtoul(argv[5], NULL, 0);
+ bap_port = strtoul(argv[6], NULL, 0);
+ /* bap_data */
+
+
+ bap = vag_bap_txer_alloc();
+ if (!bap) {
+ printf("Out of memory allocating BAP TXer struct.\n");
+ return 1;
+ }
+
+ bap_frame = vag_bap_frame_alloc();
+ if (!bap_frame) {
+ printf("Out of memory allocating BAP frame.\n");
+ return 1;
+ }
+
+
+
+ /* Fill frame */
+ bap_frame->is_multiframe = !(!bap_multiframe);
+ bap_frame->opcode = bap_opcode & 0x7;
+ bap_frame->node = bap_node & 0x3f;
+ bap_frame->port = bap_port & 0x3f;
+ bap_frame->len = 0;
+
+ /* Fill payload */
+ while(bap_frame->len < 4095) {
+ unsigned high, low;
+ unsigned i = bap_frame->len;
+
+ if (!argv[7][2*i] || !argv[7][2*i + 1]) {
+ break;
+ }
+
+ high = hex2bin(argv[7][2*i]);
+ if (high > 15) {
+ break;
+ }
+ low = hex2bin(argv[7][2*i + 1]);
+ if (low > 15) {
+ break;
+ }
+
+ bap_frame->data[i] = (high << 4) | low;
+ bap_frame->len++;
+ }
+
+
+
+ s = net_init(argv[1]);
+
+
+ /* Fill TXer */
+
+ printf("Will send frame:\n");
+ vag_bap_frame_dump(bap_frame);
+
+ ret = vag_bap_txer_queue(bap, bap_frame, &frame);
+ frame.can_id = can_id;
+ can_tx(s, &frame);
+
+ /* HACK since we don't have continuation frames yet */
+ return 0;
+
+
+ while (1) {
+ int retval;
+
+ FD_ZERO(&wfds);
+ FD_SET(s, &wfds);
+
+ retval = select(s+1, NULL, &wfds, NULL, NULL);
+ /* We currently rely on Linux timeout behavior here,
+ * i.e. the timeout now reflects the remaining time */
+ if (retval < 0) {
+ perror("select");
+ return 1;
+ } else if (!retval) {
+ /* Timeout, we NEED to check this first */
+ } else if (FD_ISSET(s, &wfds)) {
+
+ continue;
+ }
+ }
+
+
+
+ vag_bap_txer_free(bap);
+
+ close(s);
+
+ return 0;
+}
--- /dev/null
+/*
+ * Copyright 2017 Max Staudt
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License 2 as published
+ * by the Free Software Foundation.
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
+
+#include <ncurses.h>
+
+#include "vag-bap.h"
+
+
+
+
+static int net_init(char *ifname)
+{
+ int s;
+ int recv_own_msgs;
+ struct sockaddr_can addr;
+ struct ifreq ifr;
+
+ s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+ if (s < 0) {
+ perror("socket");
+ exit(1);
+ }
+
+ /* Convert interface name to index */
+ memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
+ strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
+ if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
+ perror("SIOCGIFINDEX");
+ exit(1);
+ }
+
+ /* Open the CAN interface */
+ memset(&addr, 0, sizeof(addr));
+ addr.can_family = AF_CAN;
+ addr.can_ifindex = ifr.ifr_ifindex;
+ if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+ perror("bind");
+ return 0;
+ }
+
+ recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
+ setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
+ &recv_own_msgs, sizeof(recv_own_msgs));
+
+ return s;
+}
+
+
+
+
+
+int main(int argc, char **argv)
+{
+ fd_set rdfs;
+ int s;
+ unsigned can_id;
+ struct BAP_RXer *bap;
+ unsigned node_id;
+
+ if (argc < 4) {
+ printf("syntax: %s IFNAME CAN_ID bap_node \n", argv[0]);
+ return 1;
+ }
+
+ can_id = strtoul(argv[2], NULL, 0);
+ node_id = strtoul(argv[3], NULL, 0);
+
+ bap = vag_bap_rxer_alloc();
+ if (!bap) {
+ printf("Out of memory allocating BAP struct.\n");
+ return 1;
+ }
+
+ printf("Listening for CAN ID: %x and Node %d\n", can_id, node_id);
+
+
+
+ s = net_init(argv[1]);
+
+
+ initscr();
+
+
+ while (1) {
+ int retval;
+
+ FD_ZERO(&rdfs);
+ FD_SET(s, &rdfs);
+
+ retval = select(s+1, &rdfs, NULL, NULL, NULL);
+ /* We currently rely on Linux timeout behavior here,
+ * i.e. the timeout now reflects the remaining time */
+ if (retval < 0) {
+ perror("select");
+ return 1;
+ } else if (!retval) {
+ /* Timeout, we NEED to check this first */
+ } else if (FD_ISSET(s, &rdfs)) {
+ struct can_frame frame;
+ ssize_t ret;
+
+ ret = read(s, &frame, sizeof(frame));
+ if (ret < 0) {
+ perror("read from CAN socket");
+ return 1;
+ }
+
+ if (can_id == frame.can_id) {
+ struct BAP_Frame *bap_frame;
+ unsigned i;
+
+ bap_frame = vag_bap_handle_can_frame(bap, &frame);
+ if (bap_frame && bap_frame->node == node_id) {
+ mvprintw(bap_frame->port, 0, "");
+
+ printw("%u. %2i/%-2i .%c%02i --",
+ bap_frame->opcode,
+ bap_frame->node,
+ bap_frame->port,
+ bap_frame->is_multiframe ? 'm' : 's',
+ bap_frame->len);
+
+ /* Limit huge packets */
+ if (bap_frame->len > 20) {
+ bap_frame->len = 20;
+ }
+
+ /* Hex dump */
+ for (i = 0; i < bap_frame->len; i++) {
+ if (!(i % 4)) {
+ printw(" ");
+ }
+ printw("%02x", (unsigned char)(bap_frame->data[i]));
+ }
+
+ printw("\n");
+
+ /* ASCII dump */
+ mvprintw(bap_frame->port, 64, "");
+ for (i = 0; i < bap_frame->len; i++) {
+ unsigned char c = bap_frame->data[i];
+ if (!isprint(c) || c == '\n' || c == '\r') {
+ c = '.';
+ }
+ printw("%c", c);
+ }
+ printw("'");
+ printw("\r");
+
+ refresh();
+
+ vag_bap_frame_free(bap_frame);
+ }
+ }
+
+ continue;
+ }
+ }
+
+ endwin();
+
+ vag_bap_rxer_free(bap);
+
+ close(s);
+
+ return 0;
+}
--- /dev/null
+#include <assert.h>
+#include <ctype.h>
+#include <errno.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <linux/can.h>
+
+#include "vag-bap.h"
+
+
+
+static void vag_bap_txer_build_only_can_frame(struct BAP_Frame *bap_frame, struct can_frame *frame)
+{
+ assert(!bap_frame->is_multiframe);
+
+ frame->data[0] = (bap_frame->opcode & 0x7) << 4;
+ frame->data[0] |= (bap_frame->node & 0x3f) >> 2;
+ frame->data[1] = (bap_frame->node & 0x3) << 6;
+ frame->data[1] |= bap_frame->port & 0x3f;
+
+ frame->can_dlc = 2 + bap_frame->len;
+ memcpy(&frame->data[2], bap_frame->data, bap_frame->len);
+}
+
+
+static void vag_bap_txer_build_first_can_frame(struct BAP_TXer* bap, unsigned slotnum, struct can_frame *frame)
+{
+ struct BAP_Frame *bap_frame = bap->slot[slotnum];
+
+ assert(slotnum < 4);
+
+ /* Build header */
+ /* TODO, maybe: Support simultaneous streams */
+ frame->data[0] = 0x80 + slotnum;
+ frame->data[0] |= (bap_frame->len & 0xfff) >> 8;
+ frame->data[1] = bap_frame->len & 0xff;
+
+ frame->data[2] = (bap_frame->opcode & 0x7) << 4;
+ frame->data[2] |= (bap_frame->node & 0x3f) >> 2;
+ frame->data[3] = (bap_frame->node & 0x3) << 6;
+ frame->data[3] |= bap_frame->port & 0x3f;
+
+ if (bap_frame->len <= 4) {
+ frame->can_dlc = 4 + bap_frame->len;
+ memcpy(&frame->data[4], bap_frame->data, bap_frame->len);
+
+ bap->slot[slotnum] = NULL;
+ } else {
+ /* Send first 4 data bytes */
+ frame->can_dlc = 4 + 4;
+ memcpy(&frame->data[4], bap_frame->data, 4);
+
+ /* Note bytes done */
+ bap->slot_done[slotnum] = 4;
+ }
+}
+
+
+
+int vag_bap_txer_queue(struct BAP_TXer* bap, struct BAP_Frame *bap_frame, struct can_frame *frame)
+{
+ if (!vag_bap_frame_is_valid(bap_frame)) {
+ return -EINVAL;
+ }
+
+ if (!bap_frame->is_multiframe) {
+ vag_bap_txer_build_only_can_frame(bap_frame, frame);
+ return 0;
+ } else { /* bap->frame->is_multiframe */
+ unsigned i;
+
+ /* Find available slot */
+ for (i = 0; i < 4; i++) {
+ if (!bap->slot[i]) {
+ break;
+ }
+ }
+ if (i > 3) {
+ return -EBUSY;
+ }
+
+ /* Found empty slot */
+ bap->slot[i] = vag_bap_frame_clone(bap_frame);
+ if (!bap->slot[i]) {
+ return -ENOMEM;
+ }
+
+ vag_bap_txer_build_first_can_frame(bap, i, frame);
+
+ if (bap->slot[i]) {
+ return 1;
+ }
+
+ return 0;
+ }
+}
+
+
+
+struct BAP_TXer* vag_bap_txer_alloc()
+{
+ struct BAP_TXer *bap;
+
+ bap = calloc(1, sizeof(*bap));
+ if (!bap) {
+ return NULL;
+ }
+
+ return bap;
+}
+
+
+
+
+void vag_bap_txer_free(struct BAP_TXer *bap)
+{
+ free(bap);
+}
--- /dev/null
+#include <ctype.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <linux/can.h>
+
+#include "vag-bap.h"
--- /dev/null
+#ifndef __VAG_BAP_H__
+#define __VAG_BAP_H__
+
+#include <linux/can.h>
+
+
+typedef unsigned char BAP_OpCode;
+typedef unsigned char BAP_Node;
+typedef unsigned char BAP_Port;
+typedef unsigned short BAP_FrameLen;
+
+
+/* A BAP frame at the BCL (BAP communication layer) as defined in
+ * http://www.itwissen.info/BCL-BAP-communication-layer.html
+ *
+ * This is basically BAP's equivalent of IP and TCP.
+ */
+struct BAP_Frame {
+ /* True if frame was/will be transmitted in multi-frame syntax */
+ int is_multiframe;
+
+
+ /* Request/reply, kind of */
+ BAP_OpCode opcode;
+
+ /* LSG = Logisches Steuergeraet
+ * https://www.springerprofessional.de/technische-informatik/eingebettete-systeme/neues-protokoll-vereinfacht-kommunikation-von-steuergeraeten/6592480
+ *
+ * BAP's equivalent of an IP address (to be confirmed).
+ *
+ * Always the same per CAN ID in the (simple) devices I looked at.
+ */
+ BAP_Node node;
+
+ /* The "RPC" port, or "status register" ID.
+ *
+ * BAP's equivalent of a "UDP port".
+ */
+ BAP_Port port;
+
+ /* Payload length, up to 2^12-1 = 4095 bytes. */
+ BAP_FrameLen len;
+
+
+ /* Payload */
+ char data[4096];
+};
+
+
+
+struct BAP_RXer {
+ /* Temporary storage for incomplete frames */
+ struct BAP_Frame *mfchannel[8];
+
+ /* How many bytes have we already received on each channel? */
+ BAP_FrameLen len_done[8];
+};
+
+
+struct BAP_TXer {
+ /* Temporary storage for frames not yet fully sent */
+ struct BAP_Frame *slot[4];
+
+ /* How many bytes have we already sent on each channel? */
+ BAP_FrameLen slot_done[4];
+};
+
+
+
+/* BAP frame struct handling */
+struct BAP_Frame* vag_bap_frame_alloc(void);
+ void vag_bap_frame_free(struct BAP_Frame *bap_frame);
+
+int vag_bap_frame_is_valid(struct BAP_Frame *bap_frame);
+
+struct BAP_Frame* vag_bap_frame_clone(struct BAP_Frame *bap_frame);
+
+void vag_bap_frame_dump(struct BAP_Frame *bap_frame);
+
+
+/* BAP reception */
+struct BAP_Frame* vag_bap_handle_can_frame(struct BAP_RXer *bap, struct can_frame *frame);
+struct BAP_RXer* vag_bap_rxer_alloc();
+void vag_bap_rxer_free(struct BAP_RXer *bap);
+
+
+/* BAP transmission */
+int vag_bap_txer_queue(struct BAP_TXer* bap, struct BAP_Frame *bap_frame, struct can_frame *frame);
+struct BAP_TXer* vag_bap_txer_alloc();
+void vag_bap_txer_free(struct BAP_TXer *bap);
+
+
+#endif
+++ /dev/null
-/*
- * Copyright 2017 Max Staudt
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License 2 as published
- * by the Free Software Foundation.
- */
-
-#include <stdio.h>
-#include <stdint.h>
-#include <unistd.h>
-#include <stdlib.h>
-#include <string.h>
-#include <sys/types.h>
-#include <sys/socket.h>
-#include <linux/can.h>
-#include <linux/can/raw.h>
-#include <net/if.h>
-#include <sys/ioctl.h>
-
-#include "vw-bap.h"
-
-
-
-
-static int net_init(char *ifname)
-{
- int s;
- int recv_own_msgs;
- struct sockaddr_can addr;
- struct ifreq ifr;
-
- s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
- if (s < 0) {
- perror("socket");
- exit(1);
- }
-
- /* Convert interface name to index */
- memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
- strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
- if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
- perror("SIOCGIFINDEX");
- exit(1);
- }
-
- /* Open the CAN interface */
- memset(&addr, 0, sizeof(addr));
- addr.can_family = AF_CAN;
- addr.can_ifindex = ifr.ifr_ifindex;
- if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
- perror("bind");
- return 0;
- }
-
- recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
- setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
- &recv_own_msgs, sizeof(recv_own_msgs));
-
- return s;
-}
-
-
-
-
-
-int main(int argc, char **argv)
-{
- fd_set rdfs;
- int s;
- int i;
- int can_id_count;
- unsigned *can_ids;
- struct BAP_RXer **baps;
-
- if (argc < 3) {
- printf("syntax: %s IFNAME CAN_ID [CAN_ID CAN_ID ...]\n", argv[0]);
- return 1;
- }
-
- can_id_count = argc - 2;
-
- can_ids = calloc(can_id_count, sizeof(*can_ids));
- baps = calloc(can_id_count, sizeof(*baps));
- if (!can_ids || !baps) {
- printf("Out of memory allocating meta structs.\n");
- return 1;
- }
-
- for (i = 0; i < can_id_count; i++) {
- can_ids[i] = strtoul(argv[2 + i], NULL, 0);
- baps[i] = vw_bap_rxer_alloc();
- if (!baps[i]) {
- printf("Out of memory allocating BAP struct.\n");
- return 1;
- }
-
- printf("Listening for CAN ID: %x\n", can_ids[i]);
- }
-
-
- s = net_init(argv[1]);
-
- while (1) {
- int retval;
-
- FD_ZERO(&rdfs);
- FD_SET(s, &rdfs);
-
- retval = select(s+1, &rdfs, NULL, NULL, NULL);
- /* We currently rely on Linux timeout behavior here,
- * i.e. the timeout now reflects the remaining time */
- if (retval < 0) {
- perror("select");
- return 1;
- } else if (!retval) {
- /* Timeout, we NEED to check this first */
- } else if (FD_ISSET(s, &rdfs)) {
- struct can_frame frame;
- ssize_t ret;
-
- ret = read(s, &frame, sizeof(frame));
- if (ret < 0) {
- perror("read from CAN socket");
- return 1;
- }
-
- for (i = 0; i < can_id_count; i++) {
- if (can_ids[i] == frame.can_id) {
- struct BAP_Frame *bap_frame;
- struct BAP_RXer *bap = baps[i];
-
- bap_frame = vw_bap_handle_can_frame(bap, &frame);
- if (bap_frame) {
- printf("%03x: ", frame.can_id);
- vw_bap_frame_dump(bap_frame);
- vw_bap_frame_free(bap_frame);
- }
-
- break;
- }
- }
-
- continue;
- }
- }
-
- /* TODO: Free */
-
- close(s);
-
- return 0;
-}
+++ /dev/null
-#include <ctype.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-
-#include "vw-bap.h"
-
-
-
-struct BAP_Frame* vw_bap_frame_alloc(void)
-{
- struct BAP_Frame* bap_frame;
-
- bap_frame = calloc(1, sizeof(struct BAP_Frame));
-
- return bap_frame;
-}
-
-
-void vw_bap_frame_free(struct BAP_Frame *bap_frame)
-{
- free(bap_frame);
-}
-
-
-int vw_bap_frame_is_valid(struct BAP_Frame *bap_frame)
-{
- if ( 0
- || (bap_frame->opcode > 7)
- || (bap_frame->node > 63)
- || (bap_frame->port > 63)
- || (bap_frame->len > 4095)
- || (!bap_frame->is_multiframe && bap_frame->len > 6)
- ) {
- return 0;
- }
-
- return 1;
-}
-
-
-struct BAP_Frame* vw_bap_frame_clone(struct BAP_Frame *bap_frame)
-{
- struct BAP_Frame *new_frame;
-
- if (!vw_bap_frame_is_valid(bap_frame)) {
- return NULL;
- }
-
- new_frame = vw_bap_frame_alloc();
- if (!new_frame) {
- return NULL;
- }
-
- memcpy(new_frame, bap_frame, sizeof(*new_frame));
-
- return new_frame;
-}
-
-
-
-void vw_bap_frame_dump(struct BAP_Frame *bap_frame)
-{
- unsigned i;
-
- printf("%u. %2i/%-2i .%02i --",
- bap_frame->opcode,
- bap_frame->node,
- bap_frame->port,
- bap_frame->len);
-
- for (i = 0; i < bap_frame->len; i++) {
- if (!(i % 4)) {
- printf(" ");
- }
- printf("%02x", (unsigned char)(bap_frame->data[i]));
- }
-
- printf("\n '");
- for (i = 0; i < bap_frame->len; i++) {
- unsigned char c = bap_frame->data[i];
- if (!isprint(c) || c == '\n' || c == '\r') {
- c = '.';
- }
- printf("%c", c);
- }
- printf("'");
-
- printf("\n");
-
- fflush(stdout);
-}
+++ /dev/null
-#include <ctype.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-
-#include <linux/can.h>
-
-#include "vw-bap.h"
-
-
-struct BAP_Frame* vw_bap_handle_can_frame(struct BAP_RXer *bap, struct can_frame *frame)
-{
- struct BAP_Frame *bap_frame = NULL;
- unsigned short header;
- unsigned this_len;
-
- //printf("Received BAP frame from CAN ID %03x\n", frame->can_id);
-
- if (frame->can_dlc < 2) {
- printf("Error: Frame too short\n");
- }
-
- if (frame->data[0] & 0x80) {
- /* This is a multi-frame BAP message */
- int mfchannel = (frame->data[0] >> 4) & 0x3;
-
- if (!(frame->data[0] & 0x40)) {
- /* Start frame */
-
- unsigned short header;
- unsigned this_len;
-
- if (frame->can_dlc < 4) {
- printf("Error: Frame too short\n");
- }
-
- if (bap->mfchannel[mfchannel]) {
- printf("bap_handle_can_frame: new start frame for open channel\n");
- }
- bap->mfchannel[mfchannel] = NULL;
-
- bap_frame = vw_bap_frame_alloc();
- if (!bap_frame) {
- printf("bap_handle_can_frame: Failed to allocate new frame\n");
- return NULL;
- }
-
- bap_frame->is_multiframe = 1;
-
- header = (frame->data[2] << 8) | frame->data[3];
- bap_frame->opcode = (header >> 12) & 0x7;
- bap_frame->node = (header >> 6) & 0x3F;
- bap_frame->port = (header >> 0) & 0x3F;
-
- bap_frame->len = ((frame->data[0] & 0xF) << 8) | frame->data[1];
-
- this_len = frame->can_dlc - 4;
-
- if (this_len > bap_frame->len) {
- printf("bap_handle_can_frame: this_len > len\n");
-
- free(bap_frame);
- bap->mfchannel[mfchannel] = NULL;
-
- return NULL;
- }
-
- memcpy(&bap_frame->data[0], &frame->data[frame->can_dlc - this_len], this_len);
- bap->len_done[mfchannel] = this_len;
-
- if (bap->len_done[mfchannel] == bap_frame->len) {
- /* Frame is complete, remove from buffer */
- bap->mfchannel[mfchannel] = NULL;
- return bap_frame;
- } else {
- /* Frame is incomplete, don't emit it yet */
- bap->mfchannel[mfchannel] = bap_frame;
- return NULL;
- }
- } else {
- /* Continuation frame */
-
- bap_frame = bap->mfchannel[mfchannel];
-
- if (!bap_frame) {
- printf("bap_handle_can_frame: continuation frame for unknown mf channel %d\n",
- mfchannel);
- }
-
- this_len = frame->can_dlc - 1;
-
- if ((bap->len_done[mfchannel] + this_len) > bap_frame->len) {
- printf("bap_handle_can_frame: len_done + this_len > len\n");
-
- free(bap_frame);
- bap->mfchannel[mfchannel] = NULL;
-
- return NULL;
- }
-
- memcpy(&bap_frame->data[bap->len_done[mfchannel]],
- &frame->data[frame->can_dlc - this_len],
- this_len);
- bap->len_done[mfchannel] += this_len;
-
- if (bap->len_done[mfchannel] == bap_frame->len) {
- /* Frame is complete, remove from buffer */
- bap->mfchannel[mfchannel] = NULL;
- return bap_frame;
- } else {
- /* Frame is incomplete, don't emit it yet */
- return NULL;
- }
- }
- } else {
- /* Single-frame BAP message */
-
- bap_frame = calloc(1, sizeof(struct BAP_Frame));
- if (!bap_frame) {
- printf("bap_handle_can_frame: Failed to allocate new frame\n");
- return NULL;
- }
-
- bap_frame->is_multiframe = 0;
-
- header = (frame->data[0] << 8) | frame->data[1];
- bap_frame->opcode = (header >> 12) & 0x7;
- bap_frame->node = (header >> 6) & 0x3F;
- bap_frame->port = (header >> 0) & 0x3F;
-
- this_len = frame->can_dlc - 2;
-
- bap_frame->len = this_len;
-
- memcpy(&bap_frame->data[0], &frame->data[frame->can_dlc - this_len], this_len);
-
- return bap_frame;
- }
-}
-
-
-
-
-
-struct BAP_RXer* vw_bap_rxer_alloc()
-{
- struct BAP_RXer *bap;
-
- bap = calloc(1, sizeof(*bap));
- if (!bap) {
- return NULL;
- }
-
- return bap;
-}
-
-
-
-
-void vw_bap_rxer_free(struct BAP_RXer *bap)
-{
- int i;
-
- for (i = 0; i < 8; i++) {
- if (bap->mfchannel[i]) {
- vw_bap_frame_free(bap->mfchannel[i]);
- }
- }
-
- free(bap);
-}
+++ /dev/null
-/*
- * Copyright 2017 Max Staudt
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License 2 as published
- * by the Free Software Foundation.
- */
-
-#include <stdio.h>
-#include <stdint.h>
-#include <unistd.h>
-#include <stdlib.h>
-#include <string.h>
-#include <sys/types.h>
-#include <sys/socket.h>
-#include <linux/can.h>
-#include <linux/can/raw.h>
-#include <net/if.h>
-#include <sys/ioctl.h>
-
-#include "vw-bap.h"
-
-
-static unsigned hex2bin(char c) {
- if (c >= '0' && c <= '9') {
- return c - '0';
- } else if (c >= 'A' && c <= 'F') {
- return c + 10 - 'A';
- } else if (c >= 'a' && c <= 'f') {
- return c + 10 - 'a';
- } else {
- return 127;
- }
-}
-
-
-
-static int net_init(char *ifname)
-{
- int s;
- int recv_own_msgs;
- struct sockaddr_can addr;
- struct ifreq ifr;
-
- s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
- if (s < 0) {
- perror("socket");
- exit(1);
- }
-
- /* Convert interface name to index */
- memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
- strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
- if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
- perror("SIOCGIFINDEX");
- exit(1);
- }
-
- /* Open the CAN interface */
- memset(&addr, 0, sizeof(addr));
- addr.can_family = AF_CAN;
- addr.can_ifindex = ifr.ifr_ifindex;
- if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
- perror("bind");
- return 0;
- }
-
- recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
- setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
- &recv_own_msgs, sizeof(recv_own_msgs));
-
- return s;
-}
-
-
-
-static void can_tx(int socket, struct can_frame *frame)
-{
- ssize_t ret;
-
- ret = write(socket, frame, sizeof(*frame));
- if (ret != sizeof(*frame)) {
- perror("write to CAN socket");
- exit(1);
- }
-}
-
-
-
-
-int main(int argc, char **argv)
-{
- fd_set wfds;
- int s, ret;
- struct can_frame frame;
- struct BAP_TXer *bap;
- struct BAP_Frame *bap_frame;
- unsigned can_id;
- unsigned bap_multiframe;
- unsigned bap_opcode;
- unsigned bap_node;
- unsigned bap_port;
-
- if (argc < 8) {
- printf("syntax: %s IFNAME CAN_ID multiframe bap_opcode bap_node bap_port bap_data\n", argv[0]);
- return 1;
- }
-
- /* Parse arguments */
- can_id = strtoul(argv[2], NULL, 0);
- bap_multiframe = strtoul(argv[3], NULL, 0);
- bap_opcode = strtoul(argv[4], NULL, 0);
- bap_node = strtoul(argv[5], NULL, 0);
- bap_port = strtoul(argv[6], NULL, 0);
- /* bap_data */
-
-
- bap = vw_bap_txer_alloc();
- if (!bap) {
- printf("Out of memory allocating BAP TXer struct.\n");
- return 1;
- }
-
- bap_frame = vw_bap_frame_alloc();
- if (!bap_frame) {
- printf("Out of memory allocating BAP frame.\n");
- return 1;
- }
-
-
-
- /* Fill frame */
- bap_frame->is_multiframe = !(!bap_multiframe);
- bap_frame->opcode = bap_opcode & 0x7;
- bap_frame->node = bap_node & 0x3f;
- bap_frame->port = bap_port & 0x3f;
- bap_frame->len = 0;
-
- /* Fill payload */
- while(bap_frame->len < 4095) {
- unsigned high, low;
- unsigned i = bap_frame->len;
-
- if (!argv[7][2*i] || !argv[7][2*i + 1]) {
- break;
- }
-
- high = hex2bin(argv[7][2*i]);
- if (high > 15) {
- break;
- }
- low = hex2bin(argv[7][2*i + 1]);
- if (low > 15) {
- break;
- }
-
- bap_frame->data[i] = (high << 4) | low;
- bap_frame->len++;
- }
-
-
-
- s = net_init(argv[1]);
-
-
- /* Fill TXer */
-
- printf("Will send frame:\n");
- vw_bap_frame_dump(bap_frame);
-
- ret = vw_bap_txer_queue(bap, bap_frame, &frame);
- frame.can_id = can_id;
- can_tx(s, &frame);
-
- /* HACK since we don't have continuation frames yet */
- return 0;
-
-
- while (1) {
- int retval;
-
- FD_ZERO(&wfds);
- FD_SET(s, &wfds);
-
- retval = select(s+1, NULL, &wfds, NULL, NULL);
- /* We currently rely on Linux timeout behavior here,
- * i.e. the timeout now reflects the remaining time */
- if (retval < 0) {
- perror("select");
- return 1;
- } else if (!retval) {
- /* Timeout, we NEED to check this first */
- } else if (FD_ISSET(s, &wfds)) {
-
- continue;
- }
- }
-
-
-
- vw_bap_txer_free(bap);
-
- close(s);
-
- return 0;
-}
+++ /dev/null
-/*
- * Copyright 2017 Max Staudt
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License 2 as published
- * by the Free Software Foundation.
- */
-
-#include <stdio.h>
-#include <stdint.h>
-#include <unistd.h>
-#include <stdlib.h>
-#include <string.h>
-#include <sys/types.h>
-#include <sys/socket.h>
-#include <linux/can.h>
-#include <linux/can/raw.h>
-#include <net/if.h>
-#include <sys/ioctl.h>
-
-#include <ncurses.h>
-
-#include "vw-bap.h"
-
-
-
-
-static int net_init(char *ifname)
-{
- int s;
- int recv_own_msgs;
- struct sockaddr_can addr;
- struct ifreq ifr;
-
- s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
- if (s < 0) {
- perror("socket");
- exit(1);
- }
-
- /* Convert interface name to index */
- memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
- strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
- if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
- perror("SIOCGIFINDEX");
- exit(1);
- }
-
- /* Open the CAN interface */
- memset(&addr, 0, sizeof(addr));
- addr.can_family = AF_CAN;
- addr.can_ifindex = ifr.ifr_ifindex;
- if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
- perror("bind");
- return 0;
- }
-
- recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
- setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
- &recv_own_msgs, sizeof(recv_own_msgs));
-
- return s;
-}
-
-
-
-
-
-int main(int argc, char **argv)
-{
- fd_set rdfs;
- int s;
- unsigned can_id;
- struct BAP_RXer *bap;
- unsigned node_id;
-
- if (argc < 4) {
- printf("syntax: %s IFNAME CAN_ID bap_node \n", argv[0]);
- return 1;
- }
-
- can_id = strtoul(argv[2], NULL, 0);
- node_id = strtoul(argv[3], NULL, 0);
-
- bap = vw_bap_rxer_alloc();
- if (!bap) {
- printf("Out of memory allocating BAP struct.\n");
- return 1;
- }
-
- printf("Listening for CAN ID: %x and Node %d\n", can_id, node_id);
-
-
-
- s = net_init(argv[1]);
-
-
- initscr();
-
-
- while (1) {
- int retval;
-
- FD_ZERO(&rdfs);
- FD_SET(s, &rdfs);
-
- retval = select(s+1, &rdfs, NULL, NULL, NULL);
- /* We currently rely on Linux timeout behavior here,
- * i.e. the timeout now reflects the remaining time */
- if (retval < 0) {
- perror("select");
- return 1;
- } else if (!retval) {
- /* Timeout, we NEED to check this first */
- } else if (FD_ISSET(s, &rdfs)) {
- struct can_frame frame;
- ssize_t ret;
-
- ret = read(s, &frame, sizeof(frame));
- if (ret < 0) {
- perror("read from CAN socket");
- return 1;
- }
-
- if (can_id == frame.can_id) {
- struct BAP_Frame *bap_frame;
- unsigned i;
-
- bap_frame = vw_bap_handle_can_frame(bap, &frame);
- if (bap_frame && bap_frame->node == node_id) {
- mvprintw(bap_frame->port, 0, "");
-
- printw("%u. %2i/%-2i .%c%02i --",
- bap_frame->opcode,
- bap_frame->node,
- bap_frame->port,
- bap_frame->is_multiframe ? 'm' : 's',
- bap_frame->len);
-
- /* Limit huge packets */
- if (bap_frame->len > 20) {
- bap_frame->len = 20;
- }
-
- /* Hex dump */
- for (i = 0; i < bap_frame->len; i++) {
- if (!(i % 4)) {
- printw(" ");
- }
- printw("%02x", (unsigned char)(bap_frame->data[i]));
- }
-
- printw("\n");
-
- /* ASCII dump */
- mvprintw(bap_frame->port, 64, "");
- for (i = 0; i < bap_frame->len; i++) {
- unsigned char c = bap_frame->data[i];
- if (!isprint(c) || c == '\n' || c == '\r') {
- c = '.';
- }
- printw("%c", c);
- }
- printw("'");
- printw("\r");
-
- refresh();
-
- vw_bap_frame_free(bap_frame);
- }
- }
-
- continue;
- }
- }
-
- endwin();
-
- vw_bap_rxer_free(bap);
-
- close(s);
-
- return 0;
-}
+++ /dev/null
-#include <assert.h>
-#include <ctype.h>
-#include <errno.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-
-#include <linux/can.h>
-
-#include "vw-bap.h"
-
-
-
-static void vw_bap_txer_build_only_can_frame(struct BAP_Frame *bap_frame, struct can_frame *frame)
-{
- assert(!bap_frame->is_multiframe);
-
- frame->data[0] = (bap_frame->opcode & 0x7) << 4;
- frame->data[0] |= (bap_frame->node & 0x3f) >> 2;
- frame->data[1] = (bap_frame->node & 0x3) << 6;
- frame->data[1] |= bap_frame->port & 0x3f;
-
- frame->can_dlc = 2 + bap_frame->len;
- memcpy(&frame->data[2], bap_frame->data, bap_frame->len);
-}
-
-
-static void vw_bap_txer_build_first_can_frame(struct BAP_TXer* bap, unsigned slotnum, struct can_frame *frame)
-{
- struct BAP_Frame *bap_frame = bap->slot[slotnum];
-
- assert(slotnum < 4);
-
- /* Build header */
- /* TODO, maybe: Support simultaneous streams */
- frame->data[0] = 0x80 + slotnum;
- frame->data[0] |= (bap_frame->len & 0xfff) >> 8;
- frame->data[1] = bap_frame->len & 0xff;
-
- frame->data[2] = (bap_frame->opcode & 0x7) << 4;
- frame->data[2] |= (bap_frame->node & 0x3f) >> 2;
- frame->data[3] = (bap_frame->node & 0x3) << 6;
- frame->data[3] |= bap_frame->port & 0x3f;
-
- if (bap_frame->len <= 4) {
- frame->can_dlc = 4 + bap_frame->len;
- memcpy(&frame->data[4], bap_frame->data, bap_frame->len);
-
- bap->slot[slotnum] = NULL;
- } else {
- /* Send first 4 data bytes */
- frame->can_dlc = 4 + 4;
- memcpy(&frame->data[4], bap_frame->data, 4);
-
- /* Note bytes done */
- bap->slot_done[slotnum] = 4;
- }
-}
-
-
-
-int vw_bap_txer_queue(struct BAP_TXer* bap, struct BAP_Frame *bap_frame, struct can_frame *frame)
-{
- if (!vw_bap_frame_is_valid(bap_frame)) {
- return -EINVAL;
- }
-
- if (!bap_frame->is_multiframe) {
- vw_bap_txer_build_only_can_frame(bap_frame, frame);
- return 0;
- } else { /* bap->frame->is_multiframe */
- unsigned i;
-
- /* Find available slot */
- for (i = 0; i < 4; i++) {
- if (!bap->slot[i]) {
- break;
- }
- }
- if (i > 3) {
- return -EBUSY;
- }
-
- /* Found empty slot */
- bap->slot[i] = vw_bap_frame_clone(bap_frame);
- if (!bap->slot[i]) {
- return -ENOMEM;
- }
-
- vw_bap_txer_build_first_can_frame(bap, i, frame);
-
- if (bap->slot[i]) {
- return 1;
- }
-
- return 0;
- }
-}
-
-
-
-struct BAP_TXer* vw_bap_txer_alloc()
-{
- struct BAP_TXer *bap;
-
- bap = calloc(1, sizeof(*bap));
- if (!bap) {
- return NULL;
- }
-
- return bap;
-}
-
-
-
-
-void vw_bap_txer_free(struct BAP_TXer *bap)
-{
- free(bap);
-}
+++ /dev/null
-#include <ctype.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-
-#include <linux/can.h>
-
-#include "vw-bap.h"
+++ /dev/null
-#ifndef __VW_BAP_H__
-#define __VW_BAP_H__
-
-#include <linux/can.h>
-
-
-typedef unsigned char BAP_OpCode;
-typedef unsigned char BAP_Node;
-typedef unsigned char BAP_Port;
-typedef unsigned short BAP_FrameLen;
-
-
-/* A BAP frame at the BCL (BAP communication layer) as defined in
- * http://www.itwissen.info/BCL-BAP-communication-layer.html
- *
- * This is basically BAP's equivalent of IP and TCP.
- */
-struct BAP_Frame {
- /* True if frame was/will be transmitted in multi-frame syntax */
- int is_multiframe;
-
-
- /* Request/reply, kind of */
- BAP_OpCode opcode;
-
- /* LSG = Logisches Steuergeraet
- * https://www.springerprofessional.de/technische-informatik/eingebettete-systeme/neues-protokoll-vereinfacht-kommunikation-von-steuergeraeten/6592480
- *
- * BAP's equivalent of an IP address (to be confirmed).
- *
- * Always the same per CAN ID in the (simple) devices I looked at.
- */
- BAP_Node node;
-
- /* The "RPC" port, or "status register" ID.
- *
- * BAP's equivalent of a "UDP port".
- */
- BAP_Port port;
-
- /* Payload length, up to 2^12-1 = 4095 bytes. */
- BAP_FrameLen len;
-
-
- /* Payload */
- char data[4096];
-};
-
-
-
-struct BAP_RXer {
- /* Temporary storage for incomplete frames */
- struct BAP_Frame *mfchannel[8];
-
- /* How many bytes have we already received on each channel? */
- BAP_FrameLen len_done[8];
-};
-
-
-struct BAP_TXer {
- /* Temporary storage for frames not yet fully sent */
- struct BAP_Frame *slot[4];
-
- /* How many bytes have we already sent on each channel? */
- BAP_FrameLen slot_done[4];
-};
-
-
-
-/* BAP frame struct handling */
-struct BAP_Frame* vw_bap_frame_alloc(void);
- void vw_bap_frame_free(struct BAP_Frame *bap_frame);
-
-int vw_bap_frame_is_valid(struct BAP_Frame *bap_frame);
-
-struct BAP_Frame* vw_bap_frame_clone(struct BAP_Frame *bap_frame);
-
-void vw_bap_frame_dump(struct BAP_Frame *bap_frame);
-
-
-/* BAP reception */
-struct BAP_Frame* vw_bap_handle_can_frame(struct BAP_RXer *bap, struct can_frame *frame);
-struct BAP_RXer* vw_bap_rxer_alloc();
-void vw_bap_rxer_free(struct BAP_RXer *bap);
-
-
-/* BAP transmission */
-int vw_bap_txer_queue(struct BAP_TXer* bap, struct BAP_Frame *bap_frame, struct can_frame *frame);
-struct BAP_TXer* vw_bap_txer_alloc();
-void vw_bap_txer_free(struct BAP_TXer *bap);
-
-
-#endif
projects / revag-bap.git / commitdiff