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 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* 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                         header = (frame->data[2] << 8) | frame->data[3];
  85                         bap_frame->opcode = (header >> 12) & 0x7;
  86                         bap_frame->subnode = (header >> 6) & 0x3F;
  87                         bap_frame->function = (header >> 0) & 0x3F;
  88
  89                         bap_frame->len = ((frame->data[0] & 0xF) << 8) | frame->data[1];
  90
  91                         this_len = frame->can_dlc - 4;
  92
  93                         if (this_len > bap_frame->len) {
  94                                 printf("bap_handle_can_frame: this_len > len\n");
  95
  96                                 free(bap_frame);
  97                                 bap->mfchannel[mfchannel] = NULL;
  98
  99                                 return NULL;
 100                         }
 101
 102                         memcpy(&bap_frame->data[0], &frame->data[frame->can_dlc - this_len], this_len);
 103                         bap_frame->len_done = this_len;
 104
 105                         if (bap_frame->len_done == bap_frame->len) {
 106                                 /* Frame is complete, remove from buffer */
 107                                 bap->mfchannel[mfchannel] = NULL;
 108                                 return bap_frame;
 109                         } else {
 110                                 /* Frame is incomplete, don't emit it yet */
 111                                 bap->mfchannel[mfchannel] = bap_frame;
 112                                 return NULL;
 113                         }
 114                 } else {
 115                         /* Continuation frame */
 116
 117                         bap_frame = bap->mfchannel[mfchannel];
 118
 119                         if (!bap_frame) {
 120                                 printf("bap_handle_can_frame: continuation frame for unknown mf channel %d\n",
 121                                         mfchannel);
 122                         }
 123
 124                         this_len = frame->can_dlc - 1;
 125
 126                         if ((bap_frame->len_done + this_len) > bap_frame->len) {
 127                                 printf("bap_handle_can_frame: len_done + this_len > len\n");
 128
 129                                 free(bap_frame);
 130                                 bap->mfchannel[mfchannel] = NULL;
 131
 132                                 return NULL;
 133                         }
 134
 135                         memcpy(&bap_frame->data[bap_frame->len_done],
 136                                 &frame->data[frame->can_dlc - this_len],
 137                                 this_len);
 138                         bap_frame->len_done += this_len;
 139
 140                         if (bap_frame->len_done == bap_frame->len) {
 141                                 /* Frame is complete, remove from buffer */
 142                                 bap->mfchannel[mfchannel] = NULL;
 143                                 return bap_frame;
 144                         } else {
 145                                 /* Frame is incomplete, don't emit it yet */
 146                                 return NULL;
 147                         }
 148                 }
 149         } else {
 150                 /* Single-frame BAP message */
 151
 152                 bap_frame = calloc(1, sizeof(struct BAP_Frame));
 153                 if (!bap_frame) {
 154                         printf("bap_handle_can_frame: Failed to allocate new frame\n");
 155                         return NULL;
 156                 }
 157
 158                 header = (frame->data[0] << 8) | frame->data[1];
 159                 bap_frame->opcode = (header >> 12) & 0x7;
 160                 bap_frame->subnode = (header >> 6) & 0x3F;
 161                 bap_frame->function = (header >> 0) & 0x3F;
 162
 163                 this_len = frame->can_dlc - 2;
 164
 165                 bap_frame->len = this_len;
 166
 167                 memcpy(&bap_frame->data[0], &frame->data[frame->can_dlc - this_len], this_len);
 168                 bap_frame->len_done = this_len;
 169
 170                 return bap_frame;
 171         }
 172 }
 173
 174
 175
 176
 177
 178 struct BAP_RXer* bap_alloc()
 179 {
 180         struct BAP_RXer *bap;
 181
 182         bap = calloc(1, sizeof(*bap));
 183         if (!bap) {
 184                 return NULL;
 185         }
 186
 187         return bap;
 188 }
 189
 190
 191
 192
 193 void bap_free(struct BAP_RXer *bap)
 194 {
 195         /* TODO */
 196         free(bap);
 197 }
 198
 199
 200
 201 void bap_frame_free(struct BAP_Frame *bap_frame)
 202 {
 203         /* TODO */
 204         free(bap_frame);
 205 }