diff options
| -rw-r--r-- | vw-bap-frame.c | 20 | ||||
| -rw-r--r-- | vw-bap-send.c | 24 | ||||
| -rw-r--r-- | vw-bap-tx.c | 106 | ||||
| -rw-r--r-- | vw-bap.h | 7 |
4 files changed, 126 insertions, 31 deletions
diff --git a/vw-bap-frame.c b/vw-bap-frame.c index 4fa2ec2..41d9e90 100644 --- a/vw-bap-frame.c +++ b/vw-bap-frame.c @@ -39,6 +39,26 @@ int vw_bap_frame_is_valid(struct BAP_Frame *bap_frame) } +struct BAP_Frame* vw_bap_frame_clone(struct BAP_Frame *bap_frame) +{ + struct BAP_Frame *new_frame; + + if (!vw_bap_frame_is_valid(bap_frame)) { + return NULL; + } + + new_frame = vw_bap_frame_alloc(); + if (!new_frame) { + return NULL; + } + + memcpy(new_frame, bap_frame, sizeof(*new_frame)); + + return new_frame; +} + + + void vw_bap_frame_dump(struct BAP_Frame *bap_frame) { unsigned i; diff --git a/vw-bap-send.c b/vw-bap-send.c index 1bb1373..04f9118 100644 --- a/vw-bap-send.c +++ b/vw-bap-send.c @@ -74,12 +74,25 @@ static int net_init(char *ifname) +static void can_tx(int socket, struct can_frame *frame) +{ + ssize_t ret; + + ret = write(socket, frame, sizeof(*frame)); + if (ret != sizeof(*frame)) { + perror("write to CAN socket"); + exit(1); + } +} + + int main(int argc, char **argv) { fd_set wfds; - int s; + int s, ret; + struct can_frame frame; struct BAP_TXer *bap; struct BAP_Frame *bap_frame; unsigned can_id; @@ -155,6 +168,13 @@ int main(int argc, char **argv) printf("Will send frame:\n"); vw_bap_frame_dump(bap_frame); + ret = vw_bap_txer_queue(bap, bap_frame, &frame); + frame.can_id = can_id; + can_tx(s, &frame); + + /* HACK since we don't have continuation frames yet */ + return 0; + while (1) { int retval; @@ -171,8 +191,6 @@ int main(int argc, char **argv) } else if (!retval) { /* Timeout, we NEED to check this first */ } else if (FD_ISSET(s, &wfds)) { - struct can_frame frame; - ssize_t ret; continue; } 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; + } } @@ -58,7 +58,11 @@ struct BAP_RXer { struct BAP_TXer { + /* Temporary storage for frames not yet fully sent */ + struct BAP_Frame *slot[4]; + /* How many bytes have we already sent on each channel? */ + BAP_FrameLen slot_done[4]; }; @@ -69,6 +73,8 @@ struct BAP_Frame* vw_bap_frame_alloc(void); int vw_bap_frame_is_valid(struct BAP_Frame *bap_frame); +struct BAP_Frame* vw_bap_frame_clone(struct BAP_Frame *bap_frame); + void vw_bap_frame_dump(struct BAP_Frame *bap_frame); @@ -79,6 +85,7 @@ void vw_bap_rxer_free(struct BAP_RXer *bap); /* BAP transmission */ +int vw_bap_txer_queue(struct BAP_TXer* bap, struct BAP_Frame *bap_frame, struct can_frame *frame); struct BAP_TXer* vw_bap_txer_alloc(); void vw_bap_txer_free(struct BAP_TXer *bap); |