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