tools/vag-bap-send.c

   1 /*
   2  * Copyright 2017 Max Staudt
   3  *
   4  * This program is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License 2 as published
   6  * by the Free Software Foundation.
   7  */
   8
   9 #include <stdio.h>
  10 #include <stdint.h>
  11 #include <unistd.h>
  12 #include <stdlib.h>
  13 #include <string.h>
  14 #include <sys/types.h>
  15 #include <sys/socket.h>
  16 #include <linux/can.h>
  17 #include <linux/can/raw.h>
  18 #include <net/if.h>
  19 #include <sys/ioctl.h>
  20
  21 #include "vag-bap.h"
  22
  23
  24 static unsigned hex2bin(char c) {
  25         if (c >= '0' && c <= '9') {
  26                 return c - '0';
  27         } else if (c >= 'A' && c <= 'F') {
  28                 return c + 10 - 'A';
  29         } else if (c >= 'a' && c <= 'f') {
  30                 return c + 10 - 'a';
  31         } else {
  32                 return 127;
  33         }
  34 }
  35
  36
  37
  38 static int net_init(char *ifname)
  39 {
  40         int s;
  41         int recv_own_msgs;
  42         struct sockaddr_can addr;
  43         struct ifreq ifr;
  44
  45         s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
  46         if (s < 0) {
  47                 perror("socket");
  48                 exit(1);
  49         }
  50
  51         /* Convert interface name to index */
  52         memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
  53         strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
  54         if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
  55                 perror("SIOCGIFINDEX");
  56                 exit(1);
  57         }
  58
  59         /* Open the CAN interface */
  60         memset(&addr, 0, sizeof(addr));
  61         addr.can_family = AF_CAN;
  62         addr.can_ifindex = ifr.ifr_ifindex;
  63         if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
  64                 perror("bind");
  65                 return 0;
  66         }
  67
  68         recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
  69         setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
  70                         &recv_own_msgs, sizeof(recv_own_msgs));
  71
  72         return s;
  73 }
  74
  75
  76
  77 static void can_tx(int socket, struct can_frame *frame)
  78 {
  79         ssize_t ret;
  80
  81         ret = write(socket, frame, sizeof(*frame));
  82         if (ret != sizeof(*frame)) {
  83                 perror("write to CAN socket");
  84                 exit(1);
  85         }
  86 }
  87
  88
  89
  90
  91 int main(int argc, char **argv)
  92 {
  93         fd_set wfds;
  94         int s;
  95         struct can_frame frame;
  96         struct BAP_TXer *bap;
  97         struct BAP_Frame *bap_frame;
  98         unsigned can_id;
  99         unsigned bap_multiframe;
 100         unsigned bap_opcode;
 101         unsigned bap_node;
 102         unsigned bap_port;
 103
 104         if (argc < 8) {
 105                 printf("syntax: %s IFNAME CAN_ID multiframe bap_opcode bap_node bap_port bap_data\n", argv[0]);
 106                 return 1;
 107         }
 108
 109         /* Parse arguments */
 110         can_id = strtoul(argv[2], NULL, 0);
 111         bap_multiframe = strtoul(argv[3], NULL, 0);
 112         bap_opcode = strtoul(argv[4], NULL, 0);
 113         bap_node = strtoul(argv[5], NULL, 0);
 114         bap_port = strtoul(argv[6], NULL, 0);
 115         /* bap_data */
 116
 117
 118         bap = vag_bap_txer_alloc();
 119         if (!bap) {
 120                 printf("Out of memory allocating BAP TXer struct.\n");
 121                 return 1;
 122         }
 123
 124         bap_frame = vag_bap_frame_alloc();
 125         if (!bap_frame) {
 126                 printf("Out of memory allocating BAP frame.\n");
 127                 return 1;
 128         }
 129
 130
 131
 132         /* Fill frame */
 133         bap_frame->is_multiframe = !(!bap_multiframe);
 134         bap_frame->opcode = bap_opcode & 0x7;
 135         bap_frame->node = bap_node & 0x3f;
 136         bap_frame->port = bap_port & 0x3f;
 137         bap_frame->len = 0;
 138
 139         /* Fill payload */
 140         while(bap_frame->len < 4095) {
 141                 unsigned high, low;
 142                 unsigned i = bap_frame->len;
 143
 144                 if (!argv[7][2*i] || !argv[7][2*i + 1]) {
 145                         break;
 146                 }
 147
 148                 high = hex2bin(argv[7][2*i]);
 149                 if (high > 15) {
 150                         break;
 151                 }
 152                 low = hex2bin(argv[7][2*i + 1]);
 153                 if (low > 15) {
 154                         break;
 155                 }
 156
 157                 bap_frame->data[i] = (high << 4) | low;
 158                 bap_frame->len++;
 159         }
 160
 161
 162
 163         s = net_init(argv[1]);
 164
 165
 166         /* Fill TXer */
 167
 168         printf("Will send frame:\n");
 169         vag_bap_frame_dump(bap_frame);
 170
 171         /* ret = */ vag_bap_txer_queue(bap, bap_frame, &frame);
 172         frame.can_id = can_id;
 173         can_tx(s, &frame);
 174
 175         /* HACK since we don't have continuation frames yet */
 176         return 0;
 177
 178
 179         while (1) {
 180                 int retval;
 181
 182                 FD_ZERO(&wfds);
 183                 FD_SET(s, &wfds);
 184
 185                 retval = select(s+1, NULL, &wfds, NULL, NULL);
 186                 /* We currently rely on Linux timeout behavior here,
 187                  * i.e. the timeout now reflects the remaining time */
 188                 if (retval < 0) {
 189                         perror("select");
 190                         return 1;
 191                 } else if (!retval) {
 192                         /* Timeout, we NEED to check this first */
 193                 } else if (FD_ISSET(s, &wfds)) {
 194
 195                         continue;
 196                 }
 197         }
 198
 199
 200
 201         vag_bap_txer_free(bap);
 202
 203         close(s);
 204
 205         return 0;
 206 }