diff options
author | norly <ny-git@enpas.org> | 2017-04-28 23:13:38 +0200 |
---|---|---|
committer | norly <ny-git@enpas.org> | 2017-04-28 23:13:38 +0200 |
commit | 04795b52688dde6f4ad0284a633f5ffb4aec80e0 (patch) | |
tree | 0c17e0aae07bc20312d14de79db2afd838e9c749 /vw-bap-tx.c | |
parent | c93c33404af608d755ef05754dca3575aad79f56 (diff) |
First working non-multiframe sending
Diffstat (limited to 'vw-bap-tx.c')
-rw-r--r-- | vw-bap-tx.c | 106 |
1 files changed, 78 insertions, 28 deletions
diff --git a/vw-bap-tx.c b/vw-bap-tx.c index f94d24a..f8a88b7 100644 --- a/vw-bap-tx.c +++ b/vw-bap-tx.c @@ -1,4 +1,6 @@ +#include <assert.h> #include <ctype.h> +#include <errno.h> #include <stdio.h> #include <stdlib.h> #include <string.h> @@ -9,42 +11,90 @@ -int vw_bap_txer_build_first_can_frame(struct BAP_Frame *bap_frame, struct can_frame *frame) +static void vw_bap_txer_build_only_can_frame(struct BAP_Frame *bap_frame, struct can_frame *frame) +{ + assert(!bap_frame->is_multiframe); + + frame->data[0] = (bap_frame->opcode & 0x7) << 4; + frame->data[0] |= (bap_frame->node & 0x3f) >> 2; + frame->data[1] = (bap_frame->node & 0x3) << 6; + frame->data[1] |= bap_frame->function & 0x3f; + + frame->can_dlc = 2 + bap_frame->len; + memcpy(&frame->data[2], bap_frame->data, bap_frame->len); +} + + +static void vw_bap_txer_build_first_can_frame(struct BAP_TXer* bap, unsigned slotnum, struct can_frame *frame) +{ + struct BAP_Frame *bap_frame = bap->slot[slotnum]; + + assert(slotnum < 4); + + /* Build header */ + /* TODO, maybe: Support simultaneous streams */ + frame->data[0] = 0x80 + slotnum; + frame->data[0] |= (bap_frame->len & 0xfff) >> 8; + frame->data[1] = bap_frame->len & 0xff; + + frame->data[2] = (bap_frame->opcode & 0x7) << 4; + frame->data[2] |= (bap_frame->node & 0x3f) >> 2; + frame->data[3] = (bap_frame->node & 0x3) << 6; + frame->data[3] |= bap_frame->function & 0x3f; + + if (bap_frame->len <= 4) { + frame->can_dlc = 4 + bap_frame->len; + memcpy(&frame->data[4], bap_frame->data, bap_frame->len); + + bap->slot[slotnum] = NULL; + } else { + /* Send first 4 data bytes */ + frame->can_dlc = 4 + 4; + memcpy(&frame->data[4], bap_frame->data, 4); + + /* Note bytes done */ + bap->slot_done[slotnum] = 4; + } +} + + + +int vw_bap_txer_queue(struct BAP_TXer* bap, struct BAP_Frame *bap_frame, struct can_frame *frame) { if (!vw_bap_frame_is_valid(bap_frame)) { - return -1; + return -EINVAL; } - /* Build header */ if (!bap_frame->is_multiframe) { - frame->data[0] = (bap_frame->opcode & 0x7) << 4; - frame->data[0] |= (bap_frame->node & 0x3f) >> 2; - frame->data[1] = (bap_frame->node & 0x3) << 6; - frame->data[1] |= bap_frame->function & 0x3f; - - frame->can_dlc = 2 + bap_frame->len; - memcpy(&frame->data[2], bap_frame->data, bap_frame->len); + vw_bap_txer_build_only_can_frame(bap_frame, frame); + return 0; } else { /* bap->frame->is_multiframe */ - /* TODO, maybe: Support simultaneous streams */ - frame->data[0] = 0x80; - frame->data[0] |= (bap_frame->len & 0xfff) >> 8; - frame->data[1] = bap_frame->len & 0xff; - - frame->data[2] = (bap_frame->opcode & 0x7) << 4; - frame->data[2] |= (bap_frame->node & 0x3f) >> 2; - frame->data[3] = (bap_frame->node & 0x3) << 6; - frame->data[3] |= bap_frame->function & 0x3f; - - if (bap_frame->len > 4) { - return -2; - /* TODO: Support continuation frames */ - } else { - frame->can_dlc = 4 + bap_frame->len; - memcpy(&frame->data[4], bap_frame->data, bap_frame->len); + unsigned i; + + /* Find available slot */ + for (i = 0; i < 4; i++) { + if (!bap->slot[i]) { + break; + } + } + if (i > 3) { + return -EBUSY; } - } - return 0; + /* Found empty slot */ + bap->slot[i] = vw_bap_frame_clone(bap_frame); + if (!bap->slot[i]) { + return -ENOMEM; + } + + vw_bap_txer_build_first_can_frame(bap, i, frame); + + if (bap->slot[i]) { + return 1; + } + + return 0; + } } |