vw-bap-tx.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 int vw_bap_txer_build_first_can_frame(struct BAP_Frame *bap_frame, struct can_frame *frame)
  13 {
  14         if (!vw_bap_frame_is_valid(bap_frame)) {
  15                 return -1;
  16         }
  17
  18         /* Build header */
  19         if (!bap_frame->is_multiframe) {
  20                 frame->data[0]  = (bap_frame->opcode & 0x7) << 4;
  21                 frame->data[0] |= (bap_frame->node & 0x3f) >> 2;
  22                 frame->data[1]  = (bap_frame->node & 0x3) << 6;
  23                 frame->data[1] |=  bap_frame->function & 0x3f;
  24
  25                 frame->can_dlc = 2 + bap_frame->len;
  26                 memcpy(&frame->data[2], bap_frame->data, bap_frame->len);
  27         } else { /* bap->frame->is_multiframe */
  28                 /* TODO, maybe: Support simultaneous streams */
  29                 frame->data[0]  = 0x80;
  30                 frame->data[0] |= (bap_frame->len & 0xfff) >> 8;
  31                 frame->data[1]  =  bap_frame->len & 0xff;
  32
  33                 frame->data[2]  = (bap_frame->opcode & 0x7) << 4;
  34                 frame->data[2] |= (bap_frame->node & 0x3f) >> 2;
  35                 frame->data[3]  = (bap_frame->node & 0x3) << 6;
  36                 frame->data[3] |=  bap_frame->function & 0x3f;
  37
  38                 if (bap_frame->len > 4) {
  39                         return -2;
  40                         /* TODO: Support continuation frames */
  41                 } else {
  42                         frame->can_dlc = 4 + bap_frame->len;
  43                         memcpy(&frame->data[4], bap_frame->data, bap_frame->len);
  44                 }
  45         }
  46
  47         return 0;
  48 }
  49
  50
  51
  52 struct BAP_TXer* vw_bap_txer_alloc()
  53 {
  54         struct BAP_TXer *bap;
  55
  56         bap = calloc(1, sizeof(*bap));
  57         if (!bap) {
  58                 return NULL;
  59         }
  60
  61         return bap;
  62 }
  63
  64
  65
  66
  67 void vw_bap_txer_free(struct BAP_TXer *bap)
  68 {
  69         free(bap);
  70 }