First working non-multiframe sending

diff --git a/vw-bap-frame.c b/vw-bap-frame.c
index 4fa2ec2507162709ad289230ed7907bae3ce9ea4..41d9e909d3d182d4a1518de30c57d1c343c2a775 100644 (file)
--- 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 1bb13731cb9e3d86992eac42835754ed75628903..04f91186eee0354c01f4bea6566a09d72895aa1a 100644 (file)
--- 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 f94d24a7c1af4d990c7a38c1917ecca6fa6675ce..f8a88b77a4aebb94e8a8588e8d45b428f195f68f 100644 (file)
--- 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;
+       }
 }
 
 

diff --git a/vw-bap.h b/vw-bap.h
index 30f59c693ce8f5bc1c8b08f81045d5848f056f2f..87a2fc38a7a57c1086cc293aa7dd5fb0b12fa34e 100644 (file)
--- a/vw-bap.h
+++ b/vw-bap.h
@@ -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);