From d9f29e5082bf844731ba88f95ab1ed9eb45d4434 Mon Sep 17 00:00:00 2001 From: norly Date: Sat, 2 Mar 2019 13:19:27 +0100 Subject: Clean up directory structure and Makefile --- Makefile | 57 +++++++++++--- include/vag-bap.h | 93 ++++++++++++++++++++++ src/bap-frame.c | 92 +++++++++++++++++++++ src/bap-rx.c | 171 ++++++++++++++++++++++++++++++++++++++++ src/bap-tx.c | 120 ++++++++++++++++++++++++++++ src/bap.c | 8 ++ tools/vag-bap-dump.c | 153 +++++++++++++++++++++++++++++++++++ tools/vag-bap-send.c | 206 ++++++++++++++++++++++++++++++++++++++++++++++++ tools/vag-bap-sniffer.c | 184 ++++++++++++++++++++++++++++++++++++++++++ vag-bap-dump.c | 153 ----------------------------------- vag-bap-frame.c | 92 --------------------- vag-bap-rx.c | 171 ---------------------------------------- vag-bap-send.c | 206 ------------------------------------------------ vag-bap-sniffer.c | 184 ------------------------------------------ vag-bap-tx.c | 120 ---------------------------- vag-bap.c | 8 -- vag-bap.h | 93 ---------------------- 17 files changed, 1075 insertions(+), 1036 deletions(-) create mode 100644 include/vag-bap.h create mode 100644 src/bap-frame.c create mode 100644 src/bap-rx.c create mode 100644 src/bap-tx.c create mode 100644 src/bap.c create mode 100644 tools/vag-bap-dump.c create mode 100644 tools/vag-bap-send.c create mode 100644 tools/vag-bap-sniffer.c delete mode 100644 vag-bap-dump.c delete mode 100644 vag-bap-frame.c delete mode 100644 vag-bap-rx.c delete mode 100644 vag-bap-send.c delete mode 100644 vag-bap-sniffer.c delete mode 100644 vag-bap-tx.c delete mode 100644 vag-bap.c delete mode 100644 vag-bap.h diff --git a/Makefile b/Makefile index 6ce1ee0..b50a6cc 100644 --- a/Makefile +++ b/Makefile @@ -1,21 +1,60 @@ -CFLAGS += -Wall -g +LIBNAME := vag-bap -BAPLIB = vag-bap.c vag-bap-frame.c vag-bap-rx.c vag-bap-tx.c +BUILDDIR := build +STATICLIB := $(BUILDDIR)/lib$(LIBNAME).a -EXES = vag-bap-dump vag-bap-sniffer vag-bap-send +INCLUDEDIR := include +HEADERS := $(shell find $(INCLUDEDIR)/) +SRCDIR := src +SOURCES := $(shell find $(SRCDIR)/ -iname "*.c") +OBJS := $(patsubst %.c, $(BUILDDIR)/%.o, $(SOURCES)) -all: $(EXES) +INCLUDES := $(patsubst %, -I%, $(INCLUDEDIR)) +CFLAGS := -Wall -g +TOOLDIR := tools +TOOLSOURCES := $(shell find $(TOOLDIR)/ -iname "*.c") +TOOLEXES := $(patsubst %.c, $(BUILDDIR)/%, $(TOOLSOURCES)) +TOOLLIBS := -lncurses -vag-bap-dump: vag-bap-dump.c $(BAPLIB) -vag-bap-sniffer: vag-bap-sniffer.c $(BAPLIB) - gcc -o $@ $^ -lncurses -vag-bap-send: vag-bap-send.c $(BAPLIB) + + +.PHONY: tools +tools: $(TOOLEXES) + + +$(BUILDDIR)/$(TOOLDIR)/%: $(TOOLDIR)/%.c $(HEADERS) $(STATICLIB) + @if [ ! -d $(dir $@) ] ; then mkdir -p $(dir $@) ; fi + $(CC) $(INCLUDES) $(CFLAGS) -o $@ $< $(STATICLIB) $(TOOLLIBS) + + + +.PHONY: lib +lib: $(STATICLIB) + +$(STATICLIB): $(OBJS) + @if [ ! -d $(BUILDDIR) ] ; then echo "Error: Build dir '$(BUILDDIR)' does not exist." ; false ; fi + ar rcs $@ $^ + + +$(BUILDDIR)/$(SRCDIR)/%.o: $(SRCDIR)/%.c $(HEADERS) + @if [ ! -d $(dir $@) ] ; then mkdir -p $(dir $@) ; fi + $(CC) $(INCLUDES) $(CFLAGS) -c -o $@ $< + .PHONY: clean clean: - rm -f $(EXES) + rm -f $(STATICLIB) + rm -f $(OBJS) + rm -f $(TOOLEXES) + rm -rf $(BUILDDIR)/ + + +.PHONY: distclean +distclean: clean + find . -xdev -name "*~" -exec rm {} \; + find . -xdev -name "core" -exec rm {} \; diff --git a/include/vag-bap.h b/include/vag-bap.h new file mode 100644 index 0000000..cf24fd7 --- /dev/null +++ b/include/vag-bap.h @@ -0,0 +1,93 @@ +#ifndef __VAG_BAP_H__ +#define __VAG_BAP_H__ + +#include + + +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 diff --git a/src/bap-frame.c b/src/bap-frame.c new file mode 100644 index 0000000..4117d84 --- /dev/null +++ b/src/bap-frame.c @@ -0,0 +1,92 @@ +#include +#include +#include +#include + +#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); +} diff --git a/src/bap-rx.c b/src/bap-rx.c new file mode 100644 index 0000000..2e6c0cd --- /dev/null +++ b/src/bap-rx.c @@ -0,0 +1,171 @@ +#include +#include +#include +#include + +#include + +#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); +} diff --git a/src/bap-tx.c b/src/bap-tx.c new file mode 100644 index 0000000..5beee30 --- /dev/null +++ b/src/bap-tx.c @@ -0,0 +1,120 @@ +#include +#include +#include +#include +#include +#include + +#include + +#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); +} diff --git a/src/bap.c b/src/bap.c new file mode 100644 index 0000000..3603e5a --- /dev/null +++ b/src/bap.c @@ -0,0 +1,8 @@ +#include +#include +#include +#include + +#include + +#include "vag-bap.h" diff --git a/tools/vag-bap-dump.c b/tools/vag-bap-dump.c new file mode 100644 index 0000000..e9fb9bc --- /dev/null +++ b/tools/vag-bap-dump.c @@ -0,0 +1,153 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/tools/vag-bap-send.c b/tools/vag-bap-send.c new file mode 100644 index 0000000..3a0f2d1 --- /dev/null +++ b/tools/vag-bap-send.c @@ -0,0 +1,206 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/tools/vag-bap-sniffer.c b/tools/vag-bap-sniffer.c new file mode 100644 index 0000000..1c17a25 --- /dev/null +++ b/tools/vag-bap-sniffer.c @@ -0,0 +1,184 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#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; +} diff --git a/vag-bap-dump.c b/vag-bap-dump.c deleted file mode 100644 index e9fb9bc..0000000 --- a/vag-bap-dump.c +++ /dev/null @@ -1,153 +0,0 @@ -/* - * 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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; -} diff --git a/vag-bap-frame.c b/vag-bap-frame.c deleted file mode 100644 index 4117d84..0000000 --- a/vag-bap-frame.c +++ /dev/null @@ -1,92 +0,0 @@ -#include -#include -#include -#include - -#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); -} diff --git a/vag-bap-rx.c b/vag-bap-rx.c deleted file mode 100644 index 2e6c0cd..0000000 --- a/vag-bap-rx.c +++ /dev/null @@ -1,171 +0,0 @@ -#include -#include -#include -#include - -#include - -#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); -} diff --git a/vag-bap-send.c b/vag-bap-send.c deleted file mode 100644 index 3a0f2d1..0000000 --- a/vag-bap-send.c +++ /dev/null @@ -1,206 +0,0 @@ -/* - * 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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; -} diff --git a/vag-bap-sniffer.c b/vag-bap-sniffer.c deleted file mode 100644 index 1c17a25..0000000 --- a/vag-bap-sniffer.c +++ /dev/null @@ -1,184 +0,0 @@ -/* - * 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#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; -} diff --git a/vag-bap-tx.c b/vag-bap-tx.c deleted file mode 100644 index 5beee30..0000000 --- a/vag-bap-tx.c +++ /dev/null @@ -1,120 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include - -#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); -} diff --git a/vag-bap.c b/vag-bap.c deleted file mode 100644 index 3603e5a..0000000 --- a/vag-bap.c +++ /dev/null @@ -1,8 +0,0 @@ -#include -#include -#include -#include - -#include - -#include "vag-bap.h" diff --git a/vag-bap.h b/vag-bap.h deleted file mode 100644 index cf24fd7..0000000 --- a/vag-bap.h +++ /dev/null @@ -1,93 +0,0 @@ -#ifndef __VAG_BAP_H__ -#define __VAG_BAP_H__ - -#include - - -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 -- cgit v1.2.3