Sketch BAP encapsulation (first/single frame)

diff --git a/vw-bap-tx.c b/vw-bap-tx.c
index 2f66e0557e9620f599d230997ffb4199e48f364b..f94d24a7c1af4d990c7a38c1917ecca6fa6675ce 100644 (file)
--- a/vw-bap-tx.c
+++ b/vw-bap-tx.c
@@ -9,6 +9,45 @@
 
 
 
+int vw_bap_txer_build_first_can_frame(struct BAP_Frame *bap_frame, struct can_frame *frame)
+{
+       if (!vw_bap_frame_is_valid(bap_frame)) {
+               return -1;
+       }
+
+       /* 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);
+       } 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);
+               }
+       }
+
+       return 0;
+}
+
+
 
 struct BAP_TXer* vw_bap_txer_alloc()
 {