summaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/bap-frame.c92
-rw-r--r--src/bap-rx.c171
-rw-r--r--src/bap-tx.c120
-rw-r--r--src/bap.c8
4 files changed, 391 insertions, 0 deletions
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 <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);
+}
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 <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);
+}
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 <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);
+}
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 <ctype.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <linux/can.h>
+
+#include "vag-bap.h"
generated by cgit v1.2.3 (git 2.39.1) at 2024-10-16 12:17:37 +0000