13 struct BAP_Frame* vw_bap_handle_can_frame(struct BAP_RXer *bap, struct can_frame *frame)
15 struct BAP_Frame *bap_frame = NULL;
16 unsigned short header;
19 //printf("Received BAP frame from CAN ID %03x\n", frame->can_id);
21 if (frame->can_dlc < 2) {
22 printf("Error: Frame too short\n");
25 if (frame->data[0] & 0x80) {
26 /* This is a multi-frame BAP message */
27 int mfchannel = (frame->data[0] >> 4) & 0x3;
29 if (!(frame->data[0] & 0x40)) {
32 unsigned short header;
35 if (frame->can_dlc < 4) {
36 printf("Error: Frame too short\n");
39 if (bap->mfchannel[mfchannel]) {
40 printf("bap_handle_can_frame: new start frame for open channel\n");
42 bap->mfchannel[mfchannel] = NULL;
44 bap_frame = vw_bap_frame_alloc();
46 printf("bap_handle_can_frame: Failed to allocate new frame\n");
50 bap_frame->is_multiframe = 1;
52 header = (frame->data[2] << 8) | frame->data[3];
53 bap_frame->opcode = (header >> 12) & 0x7;
54 bap_frame->node = (header >> 6) & 0x3F;
55 bap_frame->function = (header >> 0) & 0x3F;
57 bap_frame->len = ((frame->data[0] & 0xF) << 8) | frame->data[1];
59 this_len = frame->can_dlc - 4;
61 if (this_len > bap_frame->len) {
62 printf("bap_handle_can_frame: this_len > len\n");
65 bap->mfchannel[mfchannel] = NULL;
70 memcpy(&bap_frame->data[0], &frame->data[frame->can_dlc - this_len], this_len);
71 bap->len_done[mfchannel] = this_len;
73 if (bap->len_done[mfchannel] == bap_frame->len) {
74 /* Frame is complete, remove from buffer */
75 bap->mfchannel[mfchannel] = NULL;
78 /* Frame is incomplete, don't emit it yet */
79 bap->mfchannel[mfchannel] = bap_frame;
83 /* Continuation frame */
85 bap_frame = bap->mfchannel[mfchannel];
88 printf("bap_handle_can_frame: continuation frame for unknown mf channel %d\n",
92 this_len = frame->can_dlc - 1;
94 if ((bap->len_done[mfchannel] + this_len) > bap_frame->len) {
95 printf("bap_handle_can_frame: len_done + this_len > len\n");
98 bap->mfchannel[mfchannel] = NULL;
103 memcpy(&bap_frame->data[bap->len_done[mfchannel]],
104 &frame->data[frame->can_dlc - this_len],
106 bap->len_done[mfchannel] += this_len;
108 if (bap->len_done[mfchannel] == bap_frame->len) {
109 /* Frame is complete, remove from buffer */
110 bap->mfchannel[mfchannel] = NULL;
113 /* Frame is incomplete, don't emit it yet */
118 /* Single-frame BAP message */
120 bap_frame = calloc(1, sizeof(struct BAP_Frame));
122 printf("bap_handle_can_frame: Failed to allocate new frame\n");
126 bap_frame->is_multiframe = 0;
128 header = (frame->data[0] << 8) | frame->data[1];
129 bap_frame->opcode = (header >> 12) & 0x7;
130 bap_frame->node = (header >> 6) & 0x3F;
131 bap_frame->function = (header >> 0) & 0x3F;
133 this_len = frame->can_dlc - 2;
135 bap_frame->len = this_len;
137 memcpy(&bap_frame->data[0], &frame->data[frame->can_dlc - this_len], this_len);
147 struct BAP_RXer* vw_bap_rxer_alloc()
149 struct BAP_RXer *bap;
151 bap = calloc(1, sizeof(*bap));
162 void vw_bap_rxer_free(struct BAP_RXer *bap)
166 for (i = 0; i < 8; i++) {
167 if (bap->mfchannel[i]) {
168 vw_bap_frame_free(bap->mfchannel[i]);