11 struct BAP_Frame* vw_bap_handle_can_frame(struct BAP_RXer *bap, struct can_frame *frame)
13 struct BAP_Frame *bap_frame = NULL;
14 unsigned short header;
17 //printf("Received BAP frame from CAN ID %03x\n", frame->can_id);
19 if (frame->can_dlc < 2) {
20 printf("Error: Frame too short\n");
23 if (frame->data[0] & 0x80) {
24 /* This is a multi-frame BAP message */
25 int mfchannel = (frame->data[0] >> 4) & 0x3;
27 if (!(frame->data[0] & 0x40)) {
30 unsigned short header;
33 if (frame->can_dlc < 4) {
34 printf("Error: Frame too short\n");
37 if (bap->mfchannel[mfchannel]) {
38 printf("bap_handle_can_frame: new start frame for open channel\n");
40 bap->mfchannel[mfchannel] = NULL;
42 bap_frame = vw_bap_frame_alloc();
44 printf("bap_handle_can_frame: Failed to allocate new frame\n");
48 bap_frame->is_multiframe = 1;
50 header = (frame->data[2] << 8) | frame->data[3];
51 bap_frame->opcode = (header >> 12) & 0x7;
52 bap_frame->node = (header >> 6) & 0x3F;
53 bap_frame->port = (header >> 0) & 0x3F;
55 bap_frame->len = ((frame->data[0] & 0xF) << 8) | frame->data[1];
57 this_len = frame->can_dlc - 4;
59 if (this_len > bap_frame->len) {
60 printf("bap_handle_can_frame: this_len > len\n");
63 bap->mfchannel[mfchannel] = NULL;
68 memcpy(&bap_frame->data[0], &frame->data[frame->can_dlc - this_len], this_len);
69 bap->len_done[mfchannel] = this_len;
71 if (bap->len_done[mfchannel] == bap_frame->len) {
72 /* Frame is complete, remove from buffer */
73 bap->mfchannel[mfchannel] = NULL;
76 /* Frame is incomplete, don't emit it yet */
77 bap->mfchannel[mfchannel] = bap_frame;
81 /* Continuation frame */
83 bap_frame = bap->mfchannel[mfchannel];
86 printf("bap_handle_can_frame: continuation frame for unknown mf channel %d\n",
90 this_len = frame->can_dlc - 1;
92 if ((bap->len_done[mfchannel] + this_len) > bap_frame->len) {
93 printf("bap_handle_can_frame: len_done + this_len > len\n");
96 bap->mfchannel[mfchannel] = NULL;
101 memcpy(&bap_frame->data[bap->len_done[mfchannel]],
102 &frame->data[frame->can_dlc - this_len],
104 bap->len_done[mfchannel] += this_len;
106 if (bap->len_done[mfchannel] == bap_frame->len) {
107 /* Frame is complete, remove from buffer */
108 bap->mfchannel[mfchannel] = NULL;
111 /* Frame is incomplete, don't emit it yet */
116 /* Single-frame BAP message */
118 bap_frame = calloc(1, sizeof(struct BAP_Frame));
120 printf("bap_handle_can_frame: Failed to allocate new frame\n");
124 bap_frame->is_multiframe = 0;
126 header = (frame->data[0] << 8) | frame->data[1];
127 bap_frame->opcode = (header >> 12) & 0x7;
128 bap_frame->node = (header >> 6) & 0x3F;
129 bap_frame->port = (header >> 0) & 0x3F;
131 this_len = frame->can_dlc - 2;
133 bap_frame->len = this_len;
135 memcpy(&bap_frame->data[0], &frame->data[frame->can_dlc - this_len], this_len);
145 struct BAP_RXer* vw_bap_rxer_alloc()
147 struct BAP_RXer *bap;
149 bap = calloc(1, sizeof(*bap));
160 void vw_bap_rxer_free(struct BAP_RXer *bap)
164 for (i = 0; i < 8; i++) {
165 if (bap->mfchannel[i]) {
166 vw_bap_frame_free(bap->mfchannel[i]);