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