src/bap-tx.c

   1 #include <assert.h>
   2 #include <ctype.h>
   3 #include <errno.h>
   4 #include <stdio.h>
   5 #include <stdlib.h>
   6 #include <string.h>
   7
   8 #include <linux/can.h>
   9
  10 #include "vag-bap.h"
  11
  12
  13
  14 static void vag_bap_txer_build_only_can_frame(struct BAP_Frame *bap_frame, struct can_frame *frame)
  15 {
  16         assert(!bap_frame->is_multiframe);
  17
  18         frame->data[0]  = (bap_frame->opcode & 0x7) << 4;
  19         frame->data[0] |= (bap_frame->node & 0x3f) >> 2;
  20         frame->data[1]  = (bap_frame->node & 0x3) << 6;
  21         frame->data[1] |=  bap_frame->port & 0x3f;
  22
  23         frame->can_dlc = 2 + bap_frame->len;
  24         memcpy(&frame->data[2], bap_frame->data, bap_frame->len);
  25 }
  26
  27
  28 static void vag_bap_txer_build_first_can_frame(struct BAP_TXer* bap, unsigned slotnum, struct can_frame *frame)
  29 {
  30         struct BAP_Frame *bap_frame = bap->slot[slotnum];
  31
  32         assert(slotnum < 4);
  33
  34         /* Build header */
  35         /* TODO, maybe: Support simultaneous streams */
  36         frame->data[0]  = 0x80 + slotnum;
  37         frame->data[0] |= (bap_frame->len & 0xfff) >> 8;
  38         frame->data[1]  =  bap_frame->len & 0xff;
  39
  40         frame->data[2]  = (bap_frame->opcode & 0x7) << 4;
  41         frame->data[2] |= (bap_frame->node & 0x3f) >> 2;
  42         frame->data[3]  = (bap_frame->node & 0x3) << 6;
  43         frame->data[3] |=  bap_frame->port & 0x3f;
  44
  45         if (bap_frame->len <= 4) {
  46                 frame->can_dlc = 4 + bap_frame->len;
  47                 memcpy(&frame->data[4], bap_frame->data, bap_frame->len);
  48
  49                 bap->slot[slotnum] = NULL;
  50         } else {
  51                 /* Send first 4 data bytes */
  52                 frame->can_dlc = 4 + 4;
  53                 memcpy(&frame->data[4], bap_frame->data, 4);
  54
  55                 /* Note bytes done */
  56                 bap->slot_done[slotnum] = 4;
  57         }
  58 }
  59
  60
  61
  62 int vag_bap_txer_queue(struct BAP_TXer* bap, struct BAP_Frame *bap_frame, struct can_frame *frame)
  63 {
  64         if (!vag_bap_frame_is_valid(bap_frame)) {
  65                 return -EINVAL;
  66         }
  67
  68         if (!bap_frame->is_multiframe) {
  69                 vag_bap_txer_build_only_can_frame(bap_frame, frame);
  70                 return 0;
  71         } else { /* bap->frame->is_multiframe */
  72                 unsigned i;
  73
  74                 /* Find available slot */
  75                 for (i = 0; i < 4; i++) {
  76                         if (!bap->slot[i]) {
  77                                 break;
  78                         }
  79                 }
  80                 if (i > 3) {
  81                         return -EBUSY;
  82                 }
  83
  84                 /* Found empty slot */
  85                 bap->slot[i] = vag_bap_frame_clone(bap_frame);
  86                 if (!bap->slot[i]) {
  87                         return -ENOMEM;
  88                 }
  89
  90                 vag_bap_txer_build_first_can_frame(bap, i, frame);
  91
  92                 if (bap->slot[i]) {
  93                         return 1;
  94                 }
  95
  96                 return 0;
  97         }
  98 }
  99
 100
 101
 102 struct BAP_TXer* vag_bap_txer_alloc()
 103 {
 104         struct BAP_TXer *bap;
 105
 106         bap = calloc(1, sizeof(*bap));
 107         if (!bap) {
 108                 return NULL;
 109         }
 110
 111         return bap;
 112 }
 113
 114
 115
 116
 117 void vag_bap_txer_free(struct BAP_TXer *bap)
 118 {
 119         free(bap);
 120 }