utils/cegw/cegw.c

   1 /*
   2  * Copyright: (c) 2012 Czech Technical University in Prague
   3  *
   4  * Authors:
   5  *      Radek Matějka <radek.matejka@gmail.com>
   6  *      Michal Sojka  <sojkam1@fel.cvut.cz>
   7  *
   8  * Funded by: Volkswagen Group Research
   9  *
  10  * This program is free software; you can redistribute it and/or
  11  * modify it under the terms of the GNU General Public License
  12  * as published by the Free Software Foundation; either version
  13  * 2 of the License, or (at your option) any later version.
  14  */
  15
  16
  17 #include <stdio.h>
  18 #include <stdlib.h>
  19 #include <string.h>
  20 #include <sys/socket.h>
  21 #include <netdb.h>
  22 #include <net/if.h>
  23 #include <arpa/inet.h>
  24 #include <linux/can.h>
  25 #include <limits.h>
  26 #include <unistd.h>
  27 #include <fcntl.h>
  28 #include <sys/ioctl.h>
  29 #include <sys/types.h>
  30 #include <linux/can/raw.h>
  31 #include <linux/can/canethgw.h>
  32
  33 unsigned int cegw_errno = 0;
  34
  35 static const char help_msg[] = "usage:\n"
  36                                "        %s <can_if>[,filter]* <udp_listen_addr>:<port> <udp_dest_addr>:<port>\n"
  37                                "                [list of additional udp recipients <addr>:<port>]\n"
  38                                "example:\n"
  39                                "        %s can0 192.168.0.1:10501 192.168.0.4:980 192.168.0.7:1160\n\n"
  40                                "        Executing this command will set the gateway so that it will\n"
  41                                "        listen for udp messages on 192.168.0.1:10501 and send them\n"
  42                                "        to can0. Simultaneously, it will send all messages from can0\n"
  43                                "        to 192.168.0.4:980 and 192.168.0.7:1160 via udp. The message is\n"
  44                                "        therefore cloned. Notice that there can be more udp recipients.\n"
  45                                "        The can filter is specified in the same way as in candump utility.\n";
  46
  47 struct addrinfo hints = {
  48         .ai_socktype = SOCK_DGRAM
  49 };
  50
  51 /**
  52  * readsockaddr - parses @in for eth address.
  53  * Valid input is e.g. 127.0.0.1:10502. If parsing fails
  54  * the cause is stored in cegw_errno. Please note that
  55  * the function modifies content of arg.
  56  *
  57  * @param[in]  arg   hostname:port string
  58  * @param[out] addr  filled sockaddr_in structure
  59  * @return 0 on success, -1 otherwise
  60  */
  61 int readsockaddr(char *arg, struct sockaddr *addr, int ai_family)
  62 {
  63         int ret;
  64         char *delim;
  65         struct addrinfo *res;
  66
  67         delim = strchr(arg, ':');
  68
  69         if (delim == NULL) {
  70                 fprintf(stderr, "expected ':' (<hostname>:<port>)");
  71                 exit(1);
  72         }
  73
  74         *delim = '\0';
  75         delim++;
  76
  77         hints.ai_family = ai_family;
  78
  79         ret = getaddrinfo(arg, delim, &hints, &res);
  80         if (ret != 0) {
  81                 fprintf(stderr, "getaddrinfo failed: %s\n", gai_strerror(ret));
  82                 exit(1);
  83         }
  84
  85         memcpy(addr, res->ai_addr, res->ai_addrlen);
  86
  87         freeaddrinfo(res);
  88         return 0;
  89 }
  90
  91 /**
  92  * readfilter - reads can filter definition from nptr
  93  */
  94 int readfilter(char *nptr, struct can_filter **filter, int *out_numfilter, can_err_mask_t *err_mask)
  95 {
  96         char *ptr;
  97         int numfilter;
  98         struct can_filter *rfilter;
  99
 100         numfilter = 0;
 101         ptr = nptr;
 102         while (ptr) {
 103                 numfilter++;
 104                 ptr++; /* hop behind the ',' */
 105                 ptr = strchr(ptr, ','); /* exit condition */
 106         }
 107
 108         rfilter = malloc(numfilter * sizeof(*rfilter));
 109         if (!rfilter) {
 110                 fprintf(stderr, "filter malloc failed");
 111                 exit(1);
 112         }
 113
 114         numfilter = 0;
 115         *err_mask = 0;
 116
 117         while (nptr) {
 118
 119                 ptr = nptr+1; /* hop behind the ',' */
 120                 nptr = strchr(ptr, ','); /* update exit condition */
 121
 122                 if (sscanf(ptr, "%x:%x",
 123                                         &rfilter[numfilter].can_id,
 124                                         &rfilter[numfilter].can_mask) == 2) {
 125                         rfilter[numfilter].can_mask &= ~CAN_ERR_FLAG;
 126                         numfilter++;
 127                 } else if (sscanf(ptr, "%x~%x",
 128                                         &rfilter[numfilter].can_id,
 129                                         &rfilter[numfilter].can_mask) == 2) {
 130                         rfilter[numfilter].can_id |= CAN_INV_FILTER;
 131                         rfilter[numfilter].can_mask &= ~CAN_ERR_FLAG;
 132                         numfilter++;
 133                 } else if (sscanf(ptr, "#%x", err_mask) != 1) {
 134                         fprintf(stderr, "filter parsing failed");
 135                         exit(1);
 136                 }
 137         }
 138
 139         *filter = rfilter;
 140         *out_numfilter = numfilter;
 141         return 0;
 142 }
 143
 144 int main(int argc, char *argv[])
 145 {
 146         int i;
 147         int fd;
 148         int tmpi;
 149         int dstcnt;
 150         char *nptr;
 151         int numfilter = 0;
 152         int udp_sock, can_sock;
 153         int addrlen;
 154         can_err_mask_t err_mask = 0;
 155         struct sockaddr_can can_addr;
 156         struct sockaddr *dst = NULL;
 157         struct cegw_ioctl *gwctl = NULL;
 158         struct can_filter *filter = NULL;
 159
 160         /* udp_addr can store both - in and in6 addresses */
 161         struct sockaddr_in6 udp6_addr;
 162         struct sockaddr *udp_addr = (struct sockaddr *) &udp6_addr;
 163
 164
 165         if (argc == 1) {
 166                 printf(help_msg, argv[0], argv[0]);
 167                 return 0;
 168         }
 169
 170         if (argc < 4) {
 171                 fprintf(stderr, "not enough arguments\n");
 172                 printf(help_msg, argv[0], argv[0]);
 173                 /* ToDo: print usage */
 174                 return 1;
 175         }
 176
 177         dstcnt = argc-3;
 178
 179         for (i=1; i<argc; i++) {
 180                 switch (i) {
 181                         case 1: /* can ifindex */
 182                                 nptr = strchr(argv[i], ',');
 183                                 if (nptr) {
 184                                         *nptr = '\0';
 185                                 }
 186
 187                                 can_addr.can_family = AF_CAN;
 188                                 tmpi = if_nametoindex(argv[i]);
 189                                 if (tmpi == 0) {
 190                                         fprintf(stderr, "given can interface not found\n");
 191                                         return 1;
 192                                 }
 193
 194                                 can_addr.can_ifindex = tmpi;
 195                                 break;
 196                         case 2: /* listen addr */
 197                                 readsockaddr(argv[i], udp_addr, AF_UNSPEC);
 198                                 switch (udp_addr->sa_family) {
 199                                         case AF_INET:
 200                                                 addrlen = sizeof(struct sockaddr_in);
 201                                                 break;
 202                                         case AF_INET6:
 203                                                 addrlen = sizeof(struct sockaddr_in6);
 204                                                 break;
 205                                         default:
 206                                                 fprintf(stderr, "unexpected sockaddr family");
 207                                                 break;
 208                                 }
 209                                 gwctl = (struct cegw_ioctl*)malloc(sizeof(*gwctl) + dstcnt*addrlen);
 210                                 break;
 211                         default: /* udp destination */
 212                                 dst = (struct sockaddr *)(gwctl->udp_dst + (i-3)*addrlen);
 213                                 readsockaddr(argv[i], dst, udp_addr->sa_family);
 214
 215                                 break;
 216                 }
 217         }
 218
 219         /* prepare udp socket */
 220         udp_sock = socket(udp_addr->sa_family, SOCK_DGRAM, IPPROTO_UDP);
 221         if (udp_sock == -1) {
 222                 perror("udp socket(..)");
 223                 return 1;
 224         }
 225
 226         if (bind(udp_sock, udp_addr, addrlen) != 0) {
 227                 perror("bind(udp)");
 228                 return 1;
 229         }
 230
 231         /* prepare can socket */
 232         can_sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
 233         if (can_sock == -1) {
 234                 perror("can socket(..)");
 235                 return 1;
 236         }
 237
 238         if (bind(can_sock, (struct sockaddr *)&can_addr, sizeof(struct sockaddr_can)) != 0) {
 239                 perror("bind(can)");
 240                 return 1;
 241         }
 242
 243         /* can filter */
 244         if (nptr)
 245                 readfilter(nptr, &filter, &numfilter, &err_mask);
 246
 247         if (err_mask)
 248                 setsockopt(can_sock, SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
 249                                 &err_mask, sizeof(err_mask));
 250
 251         if (numfilter)
 252                 setsockopt(can_sock, SOL_CAN_RAW, CAN_RAW_FILTER,
 253                                 filter, numfilter * sizeof(struct can_filter));
 254         free(filter);
 255
 256         /* send it to kernel gateway */
 257         fd = open("/dev/canethgw", O_RDONLY);
 258         if (fd == -1) {
 259                 perror("/dev/canethgw");
 260                 return 1;
 261         }
 262
 263         gwctl->can_sock = can_sock;
 264         gwctl->udp_sock = udp_sock;
 265         gwctl->udp_dstcnt = dstcnt;
 266         gwctl->udp_addrlen = addrlen;
 267
 268         if (ioctl(fd, CEGW_IOCTL_START, gwctl) != 0) {
 269                 perror("ioctl");
 270                 return 1;
 271         }
 272         printf("gateway successfully set and running\n");
 273         free(gwctl);
 274
 275         /* sleep until someone kills me */
 276         pause();
 277
 278         return 0;
 279 }
 280