vw-bap.c

   1 #include <ctype.h>
   2 #include <stdio.h>
   3 #include <stdlib.h>
   4 #include <string.h>
   5
   6 #include <linux/can.h>
   7
   8 #include "vw-bap.h"
   9
  10
  11 void vw_bap_frame_dump(struct BAP_Frame *bap_frame)
  12 {
  13         unsigned i;
  14
  15         printf("%u. %2i/%-2i .%02i --",
  16                 bap_frame->opcode,
  17                 bap_frame->subnode,
  18                 bap_frame->function,
  19                 bap_frame->len);
  20
  21         for (i = 0; i < bap_frame->len; i++) {
  22                 if (!(i % 4)) {
  23                         printf(" ");
  24                 }
  25                 printf("%02x", (unsigned char)(bap_frame->data[i]));
  26         }
  27
  28         printf("\n        '");
  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') {
  32                         c = '.';
  33                 }
  34                 printf("%c", c);
  35         }
  36         printf("'");
  37
  38         printf("\n");
  39
  40         fflush(stdout);
  41 }
  42
  43
  44
  45
  46
  47 struct BAP_Frame* vw_bap_handle_can_frame(struct BAP_RXer *bap, struct can_frame *frame)
  48 {
  49         struct BAP_Frame *bap_frame = NULL;
  50         unsigned short header;
  51         unsigned this_len;
  52
  53         //printf("Received BAP frame from CAN ID %03x\n", frame->can_id);
  54
  55         if (frame->can_dlc < 2) {
  56                 printf("Error: Frame too short\n");
  57         }
  58
  59         if (frame->data[0] & 0x80) {
  60                 /* This is a multi-frame BAP message */
  61                 int mfchannel = (frame->data[0] >> 4) & 0x3;
  62
  63                 if (!(frame->data[0] & 0x40)) {
  64                         /* Start frame */
  65
  66                         unsigned short header;
  67                         unsigned this_len;
  68
  69                         if (frame->can_dlc < 4) {
  70                                 printf("Error: Frame too short\n");
  71                         }
  72
  73                         if (bap->mfchannel[mfchannel]) {
  74                                 printf("bap_handle_can_frame: new start frame for open channel\n");
  75                         }
  76                         bap->mfchannel[mfchannel] = NULL;
  77
  78                         bap_frame = calloc(1, sizeof(struct BAP_Frame));
  79                         if (!bap_frame) {
  80                                 printf("bap_handle_can_frame: Failed to allocate new frame\n");
  81                                 return NULL;
  82                         }
  83
  84                         bap_frame->is_multiframe = 1;
  85
  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;
  90
  91                         bap_frame->len = ((frame->data[0] & 0xF) << 8) | frame->data[1];
  92
  93                         this_len = frame->can_dlc - 4;
  94
  95                         if (this_len > bap_frame->len) {
  96                                 printf("bap_handle_can_frame: this_len > len\n");
  97
  98                                 free(bap_frame);
  99                                 bap->mfchannel[mfchannel] = NULL;
 100
 101                                 return NULL;
 102                         }
 103
 104                         memcpy(&bap_frame->data[0], &frame->data[frame->can_dlc - this_len], this_len);
 105                         bap->len_done[mfchannel] = this_len;
 106
 107                         if (bap->len_done[mfchannel] == bap_frame->len) {
 108                                 /* Frame is complete, remove from buffer */
 109                                 bap->mfchannel[mfchannel] = NULL;
 110                                 return bap_frame;
 111                         } else {
 112                                 /* Frame is incomplete, don't emit it yet */
 113                                 bap->mfchannel[mfchannel] = bap_frame;
 114                                 return NULL;
 115                         }
 116                 } else {
 117                         /* Continuation frame */
 118
 119                         bap_frame = bap->mfchannel[mfchannel];
 120
 121                         if (!bap_frame) {
 122                                 printf("bap_handle_can_frame: continuation frame for unknown mf channel %d\n",
 123                                         mfchannel);
 124                         }
 125
 126                         this_len = frame->can_dlc - 1;
 127
 128                         if ((bap->len_done[mfchannel] + this_len) > bap_frame->len) {
 129                                 printf("bap_handle_can_frame: len_done + this_len > len\n");
 130
 131                                 free(bap_frame);
 132                                 bap->mfchannel[mfchannel] = NULL;
 133
 134                                 return NULL;
 135                         }
 136
 137                         memcpy(&bap_frame->data[bap->len_done[mfchannel]],
 138                                 &frame->data[frame->can_dlc - this_len],
 139                                 this_len);
 140                         bap->len_done[mfchannel] += this_len;
 141
 142                         if (bap->len_done[mfchannel] == bap_frame->len) {
 143                                 /* Frame is complete, remove from buffer */
 144                                 bap->mfchannel[mfchannel] = NULL;
 145                                 return bap_frame;
 146                         } else {
 147                                 /* Frame is incomplete, don't emit it yet */
 148                                 return NULL;
 149                         }
 150                 }
 151         } else {
 152                 /* Single-frame BAP message */
 153
 154                 bap_frame = calloc(1, sizeof(struct BAP_Frame));
 155                 if (!bap_frame) {
 156                         printf("bap_handle_can_frame: Failed to allocate new frame\n");
 157                         return NULL;
 158                 }
 159
 160                 bap_frame->is_multiframe = 0;
 161
 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;
 166
 167                 this_len = frame->can_dlc - 2;
 168
 169                 bap_frame->len = this_len;
 170
 171                 memcpy(&bap_frame->data[0], &frame->data[frame->can_dlc - this_len], this_len);
 172
 173                 return bap_frame;
 174         }
 175 }
 176
 177
 178
 179
 180
 181 void vw_bap_frame_free(struct BAP_Frame *bap_frame)
 182 {
 183         free(bap_frame);
 184 }
 185
 186
 187
 188 struct BAP_RXer* vw_bap_alloc()
 189 {
 190         struct BAP_RXer *bap;
 191
 192         bap = calloc(1, sizeof(*bap));
 193         if (!bap) {
 194                 return NULL;
 195         }
 196
 197         return bap;
 198 }
 199
 200
 201
 202
 203 void vw_bap_free(struct BAP_RXer *bap)
 204 {
 205         int i;
 206
 207         for (i = 0; i < 8; i++) {
 208                 if (bap->mfchannel[i]) {
 209                         vw_bap_frame_free(bap->mfchannel[i]);
 210                 }
 211         }
 212
 213         free(bap);
 214 }