diff options
Diffstat (limited to 'vw-bap.c')
| -rw-r--r-- | vw-bap.c | 165 |
1 files changed, 0 insertions, 165 deletions
@@ -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); -} |