Sketch BAP sending

diff --git a/Makefile b/Makefile
index 2ae56032994c9697feceb9f49bb128092a77f599..f26afef27dd26e38af0e84b7ecfbd5a7a7a3ca58 100644 (file)
--- a/Makefile
+++ b/Makefile
@@ -1,11 +1,13 @@
 CFLAGS += -Wall -g
 
-BAPLIB = vw-bap.c vw-bap-frame.c vw-bap-rx.c
+BAPLIB = vw-bap.c vw-bap-frame.c vw-bap-rx.c vw-bap-tx.c
 
-all: vw-bap-dump vw-bap-sniffer
+all: vw-bap-dump vw-bap-sniffer vw-bap-send
 
 
 vw-bap-dump: vw-bap-dump.c $(BAPLIB)
 
 vw-bap-sniffer: vw-bap-sniffer.c $(BAPLIB)
        gcc -o $@ $^ -lncurses
+
+vw-bap-send: vw-bap-send.c $(BAPLIB)

diff --git a/vw-bap-send.c b/vw-bap-send.c
new file mode 100644 (file)
index 0000000..1bb1373
--- /dev/null
+++ b/vw-bap-send.c
@@ -0,0 +1,188 @@
+/*
+ * Copyright 2017 Max Staudt
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License 2 as published
+ * by the Free Software Foundation.
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
+
+#include "vw-bap.h"
+
+
+static unsigned hex2bin(char c) {
+       if (c >= '0' && c <= '9') {
+               return c - '0';
+       } else if (c >= 'A' && c <= 'F') {
+               return c + 10 - 'A';
+       } else if (c >= 'a' && c <= 'f') {
+               return c + 10 - 'a';
+       } else {
+               return 127;
+       }
+}
+
+
+
+static int net_init(char *ifname)
+{
+        int s;
+       int recv_own_msgs;
+       struct sockaddr_can addr;
+       struct ifreq ifr;
+
+       s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+       if (s < 0) {
+               perror("socket");
+               exit(1);
+       }
+
+       /* Convert interface name to index */
+       memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
+       strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
+       if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
+               perror("SIOCGIFINDEX");
+               exit(1);
+       }
+
+       /* Open the CAN interface */
+       memset(&addr, 0, sizeof(addr));
+       addr.can_family = AF_CAN;
+       addr.can_ifindex = ifr.ifr_ifindex;
+       if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+               perror("bind");
+               return 0;
+       }
+
+       recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
+       setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
+                       &recv_own_msgs, sizeof(recv_own_msgs));
+
+       return s;
+}
+
+
+
+
+
+int main(int argc, char **argv)
+{
+       fd_set wfds;
+       int s;
+       struct BAP_TXer *bap;
+       struct BAP_Frame *bap_frame;
+       unsigned can_id;
+       unsigned bap_multiframe;
+       unsigned bap_opcode;
+       unsigned bap_node;
+       unsigned bap_function;
+
+       if (argc < 8) {
+               printf("syntax: %s IFNAME CAN_ID multiframe bap_opcode bap_node bap_function bap_data\n", argv[0]);
+               return 1;
+       }
+
+       /* Parse arguments */
+       can_id = strtoul(argv[2], NULL, 0);
+       bap_multiframe = strtoul(argv[3], NULL, 0);
+       bap_opcode = strtoul(argv[4], NULL, 0);
+       bap_node = strtoul(argv[5], NULL, 0);
+       bap_function = strtoul(argv[6], NULL, 0);
+       /* bap_data */
+
+
+       bap = vw_bap_txer_alloc();
+       if (!bap) {
+               printf("Out of memory allocating BAP TXer struct.\n");
+               return 1;
+       }
+
+       bap_frame = vw_bap_frame_alloc();
+       if (!bap_frame) {
+               printf("Out of memory allocating BAP frame.\n");
+               return 1;
+       }
+
+
+
+       /* Fill frame */
+       bap_frame->is_multiframe = !(!bap_multiframe);
+       bap_frame->opcode = bap_opcode & 0x7;
+       bap_frame->node = bap_node & 0x3f;
+       bap_frame->function = bap_function & 0x3f;
+       bap_frame->len = 0;
+
+       /* Fill payload */
+       while(bap_frame->len < 4095) {
+               unsigned high, low;
+               unsigned i = bap_frame->len;
+
+               if (!argv[7][2*i] || !argv[7][2*i + 1]) {
+                       break;
+               }
+
+               high = hex2bin(argv[7][2*i]);
+               if (high > 15) {
+                       break;
+               }
+               low = hex2bin(argv[7][2*i + 1]);
+               if (low > 15) {
+                       break;
+               }
+
+               bap_frame->data[i] = (high << 4) | low;
+               bap_frame->len++;
+       }
+
+
+
+       s = net_init(argv[1]);
+
+
+       /* Fill TXer */
+
+       printf("Will send frame:\n");
+       vw_bap_frame_dump(bap_frame);
+
+
+       while (1) {
+               int retval;
+
+               FD_ZERO(&wfds);
+               FD_SET(s, &wfds);
+
+               retval = select(s+1, NULL, &wfds, NULL, NULL);
+               /* We currently rely on Linux timeout behavior here,
+                * i.e. the timeout now reflects the remaining time */
+               if (retval < 0) {
+                       perror("select");
+                       return 1;
+               } else if (!retval) {
+                       /* Timeout, we NEED to check this first */
+               } else if (FD_ISSET(s, &wfds)) {
+                       struct can_frame frame;
+                       ssize_t ret;
+
+                       continue;
+               }
+       }
+
+
+
+       vw_bap_txer_free(bap);
+
+       close(s);
+
+       return 0;
+}

diff --git a/vw-bap-tx.c b/vw-bap-tx.c
new file mode 100644 (file)
index 0000000..2f66e05
--- /dev/null
+++ b/vw-bap-tx.c
@@ -0,0 +1,31 @@
+#include <ctype.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <linux/can.h>
+
+#include "vw-bap.h"
+
+
+
+
+struct BAP_TXer* vw_bap_txer_alloc()
+{
+       struct BAP_TXer *bap;
+
+       bap = calloc(1, sizeof(*bap));
+       if (!bap) {
+               return NULL;
+       }
+
+       return bap;
+}
+
+
+
+
+void vw_bap_txer_free(struct BAP_TXer *bap)
+{
+       free(bap);
+}

diff --git a/vw-bap.h b/vw-bap.h
index 656ff0e8a7eb1f2a88b636b4f4bedc59f240cb3c..30f59c693ce8f5bc1c8b08f81045d5848f056f2f 100644 (file)
--- a/vw-bap.h
+++ b/vw-bap.h
@@ -57,6 +57,11 @@ struct BAP_RXer {
 };
 
 
+struct BAP_TXer {
+
+};
+
+
 
 /* BAP frame struct handling */
 struct BAP_Frame* vw_bap_frame_alloc(void);
@@ -73,4 +78,9 @@ struct BAP_RXer* vw_bap_rxer_alloc();
 void vw_bap_rxer_free(struct BAP_RXer *bap);
 
 
+/* BAP transmission */
+struct BAP_TXer* vw_bap_txer_alloc();
+void vw_bap_txer_free(struct BAP_TXer *bap);
+
+
 #endif