summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Makefile2
-rw-r--r--vw-bap-rx.c171
-rw-r--r--vw-bap.c165
3 files changed, 172 insertions, 166 deletions
diff --git a/Makefile b/Makefile
index 6a0fe71..2ae5603 100644
--- a/Makefile
+++ b/Makefile
@@ -1,6 +1,6 @@
CFLAGS += -Wall -g
-BAPLIB = vw-bap.c vw-bap-frame.c
+BAPLIB = vw-bap.c vw-bap-frame.c vw-bap-rx.c
all: vw-bap-dump vw-bap-sniffer
diff --git a/vw-bap-rx.c b/vw-bap-rx.c
new file mode 100644
index 0000000..229a55d
--- /dev/null
+++ b/vw-bap-rx.c
@@ -0,0 +1,171 @@
+#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->function = (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->function = (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);
+}
diff --git a/vw-bap.c b/vw-bap.c
index 1864cea..0779855 100644
--- a/vw-bap.c
+++ b/vw-bap.c
@@ -6,168 +6,3 @@
#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->function = (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->function = (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);
-}
generated by cgit v1.2.3 (git 2.39.1) at 2024-10-16 13:31:09 +0000