summaryrefslogtreecommitdiff
path: root/vw-bap-tx.c
diff options
context:
space:
mode:
Diffstat (limited to 'vw-bap-tx.c')
-rw-r--r--vw-bap-tx.c106
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;
+ }
}