11 void vw_bap_frame_dump(struct BAP_Frame *bap_frame)
15 printf("%u. %2i/%-2i .%02i --",
21 for (i = 0; i < bap_frame->len; i++) {
25 printf("%02x", (unsigned char)(bap_frame->data[i]));
29 for (i = 0; i < bap_frame->len; i++) {
30 unsigned char c = bap_frame->data[i];
31 if (!isprint(c) || c == '\n' || c == '\r') {
47 struct BAP_Frame* vw_bap_handle_can_frame(struct BAP_RXer *bap, struct can_frame *frame)
49 struct BAP_Frame *bap_frame = NULL;
50 unsigned short header;
53 //printf("Received BAP frame from CAN ID %03x\n", frame->can_id);
55 if (frame->can_dlc < 2) {
56 printf("Error: Frame too short\n");
59 if (frame->data[0] & 0x80) {
60 /* This is a multi-frame BAP message */
61 int mfchannel = (frame->data[0] >> 4) & 0x3;
63 if (!(frame->data[0] & 0x40)) {
66 unsigned short header;
69 if (frame->can_dlc < 4) {
70 printf("Error: Frame too short\n");
73 if (bap->mfchannel[mfchannel]) {
74 printf("bap_handle_can_frame: new start frame for open channel\n");
76 bap->mfchannel[mfchannel] = NULL;
78 bap_frame = calloc(1, sizeof(struct BAP_Frame));
80 printf("bap_handle_can_frame: Failed to allocate new frame\n");
84 bap_frame->is_multiframe = 1;
86 header = (frame->data[2] << 8) | frame->data[3];
87 bap_frame->opcode = (header >> 12) & 0x7;
88 bap_frame->subnode = (header >> 6) & 0x3F;
89 bap_frame->function = (header >> 0) & 0x3F;
91 bap_frame->len = ((frame->data[0] & 0xF) << 8) | frame->data[1];
93 this_len = frame->can_dlc - 4;
95 if (this_len > bap_frame->len) {
96 printf("bap_handle_can_frame: this_len > len\n");
99 bap->mfchannel[mfchannel] = NULL;
104 memcpy(&bap_frame->data[0], &frame->data[frame->can_dlc - this_len], this_len);
105 bap->len_done[mfchannel] = this_len;
107 if (bap->len_done[mfchannel] == bap_frame->len) {
108 /* Frame is complete, remove from buffer */
109 bap->mfchannel[mfchannel] = NULL;
112 /* Frame is incomplete, don't emit it yet */
113 bap->mfchannel[mfchannel] = bap_frame;
117 /* Continuation frame */
119 bap_frame = bap->mfchannel[mfchannel];
122 printf("bap_handle_can_frame: continuation frame for unknown mf channel %d\n",
126 this_len = frame->can_dlc - 1;
128 if ((bap->len_done[mfchannel] + this_len) > bap_frame->len) {
129 printf("bap_handle_can_frame: len_done + this_len > len\n");
132 bap->mfchannel[mfchannel] = NULL;
137 memcpy(&bap_frame->data[bap->len_done[mfchannel]],
138 &frame->data[frame->can_dlc - this_len],
140 bap->len_done[mfchannel] += this_len;
142 if (bap->len_done[mfchannel] == bap_frame->len) {
143 /* Frame is complete, remove from buffer */
144 bap->mfchannel[mfchannel] = NULL;
147 /* Frame is incomplete, don't emit it yet */
152 /* Single-frame BAP message */
154 bap_frame = calloc(1, sizeof(struct BAP_Frame));
156 printf("bap_handle_can_frame: Failed to allocate new frame\n");
160 bap_frame->is_multiframe = 0;
162 header = (frame->data[0] << 8) | frame->data[1];
163 bap_frame->opcode = (header >> 12) & 0x7;
164 bap_frame->subnode = (header >> 6) & 0x3F;
165 bap_frame->function = (header >> 0) & 0x3F;
167 this_len = frame->can_dlc - 2;
169 bap_frame->len = this_len;
171 memcpy(&bap_frame->data[0], &frame->data[frame->can_dlc - this_len], this_len);
181 void vw_bap_frame_free(struct BAP_Frame *bap_frame)
188 struct BAP_RXer* vw_bap_alloc()
190 struct BAP_RXer *bap;
192 bap = calloc(1, sizeof(*bap));
203 void vw_bap_free(struct BAP_RXer *bap)
207 for (i = 0; i < 8; i++) {
208 if (bap->mfchannel[i]) {
209 vw_bap_frame_free(bap->mfchannel[i]);