src/bap-rx.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 "vag-bap.h"
   9
  10
  11 struct BAP_Frame* vag_bap_handle_can_frame(struct BAP_RXer *bap, struct can_frame *frame)
  12 {
  13         struct BAP_Frame *bap_frame = NULL;
  14         unsigned short header;
  15         unsigned this_len;
  16
  17         //printf("Received BAP frame from CAN ID %03x\n", frame->can_id);
  18
  19         if (frame->can_dlc < 2) {
  20                 VAG_DEBUG("Error: Frame too short\n");
  21         }
  22
  23         if (frame->data[0] & 0x80) {
  24                 /* This is a multi-frame BAP message */
  25                 int mfchannel = (frame->data[0] >> 4) & 0x3;
  26
  27                 if (!(frame->data[0] & 0x40)) {
  28                         /* Start frame */
  29
  30                         unsigned short header;
  31                         unsigned this_len;
  32
  33                         /* Sanity checks */
  34                         if (frame->can_dlc < 4) {
  35                                 VAG_DEBUG("bap_handle_can_frame: Frame too short\n");
  36                         }
  37
  38                         if (bap->mfchannel[mfchannel]) {
  39                                 VAG_DEBUG("bap_handle_can_frame: new start frame for open channel\n");
  40                         }
  41                         bap->mfchannel[mfchannel] = NULL;
  42
  43                         /* Frame looks okay, start parsing */
  44                         bap_frame = vag_bap_frame_alloc();
  45                         if (!bap_frame) {
  46                                 VAG_DEBUG("bap_handle_can_frame: Failed to allocate new frame\n");
  47                                 return NULL;
  48                         }
  49
  50                         bap_frame->is_multiframe = 1;
  51
  52                         header = (frame->data[2] << 8) | frame->data[3];
  53                         bap_frame->opcode = (header >> 12) & 0x7;
  54                         bap_frame->node = (header >> 6) & 0x3F;
  55                         bap_frame->port = (header >> 0) & 0x3F;
  56
  57                         bap_frame->len = ((frame->data[0] & 0xF) << 8) | frame->data[1];
  58
  59                         this_len = frame->can_dlc - 4;
  60
  61                         if (this_len > bap_frame->len) {
  62                                 VAG_DEBUG("bap_handle_can_frame: this_len > len\n");
  63
  64                                 free(bap_frame);
  65                                 bap->mfchannel[mfchannel] = NULL;
  66
  67                                 return NULL;
  68                         }
  69
  70                         memcpy(&bap_frame->data[0], &frame->data[frame->can_dlc - this_len], this_len);
  71                         bap->len_done[mfchannel] = this_len;
  72
  73                         if (bap->len_done[mfchannel] == bap_frame->len) {
  74                                 /* Frame is complete, remove from buffer */
  75                                 bap->mfchannel[mfchannel] = NULL;
  76                                 return bap_frame;
  77                         } else {
  78                                 /* Frame is incomplete, don't emit it yet */
  79                                 bap->mfchannel[mfchannel] = bap_frame;
  80                                 return NULL;
  81                         }
  82                 } else {
  83                         /* Continuation frame */
  84
  85                         bap_frame = bap->mfchannel[mfchannel];
  86
  87                         if (!bap_frame) {
  88                                 VAG_DEBUG("bap_handle_can_frame: continuation frame for unknown mf channel %d\n",
  89                                         mfchannel);
  90                         }
  91
  92                         this_len = frame->can_dlc - 1;
  93
  94                         if ((bap->len_done[mfchannel] + this_len) > bap_frame->len) {
  95                                 VAG_DEBUG("bap_handle_can_frame: len_done + this_len > len\n");
  96
  97                                 free(bap_frame);
  98                                 bap->mfchannel[mfchannel] = NULL;
  99
 100                                 return NULL;
 101                         }
 102
 103                         memcpy(&bap_frame->data[bap->len_done[mfchannel]],
 104                                 &frame->data[frame->can_dlc - this_len],
 105                                 this_len);
 106                         bap->len_done[mfchannel] += this_len;
 107
 108                         if (bap->len_done[mfchannel] == bap_frame->len) {
 109                                 /* Frame is complete, remove from buffer */
 110                                 bap->mfchannel[mfchannel] = NULL;
 111                                 return bap_frame;
 112                         } else {
 113                                 /* Frame is incomplete, don't emit it yet */
 114                                 return NULL;
 115                         }
 116                 }
 117         } else {
 118                 /* Single-frame BAP message */
 119
 120                 bap_frame = calloc(1, sizeof(struct BAP_Frame));
 121                 if (!bap_frame) {
 122                         VAG_DEBUG("bap_handle_can_frame: Failed to allocate new frame\n");
 123                         return NULL;
 124                 }
 125
 126                 bap_frame->is_multiframe = 0;
 127
 128                 header = (frame->data[0] << 8) | frame->data[1];
 129                 bap_frame->opcode = (header >> 12) & 0x7;
 130                 bap_frame->node = (header >> 6) & 0x3F;
 131                 bap_frame->port = (header >> 0) & 0x3F;
 132
 133                 this_len = frame->can_dlc - 2;
 134
 135                 bap_frame->len = this_len;
 136
 137                 memcpy(&bap_frame->data[0], &frame->data[frame->can_dlc - this_len], this_len);
 138
 139                 return bap_frame;
 140         }
 141 }
 142
 143
 144
 145
 146
 147 struct BAP_RXer* vag_bap_rxer_alloc()
 148 {
 149         struct BAP_RXer *bap;
 150
 151         bap = calloc(1, sizeof(*bap));
 152         if (!bap) {
 153                 return NULL;
 154         }
 155
 156         return bap;
 157 }
 158
 159
 160
 161
 162 void vag_bap_rxer_free(struct BAP_RXer *bap)
 163 {
 164         int i;
 165
 166         for (i = 0; i < 8; i++) {
 167                 if (bap->mfchannel[i]) {
 168                         vag_bap_frame_free(bap->mfchannel[i]);
 169                 }
 170         }
 171
 172         free(bap);
 173 }