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 enum {
  36         CEGW_ERR_UNKNOWN,
  37         CEGW_ERR_COLON,
  38         CEGW_ERR_GETADDRI,
  39         CEGW_ERR_FLTALCK,
  40         CEGW_ERR_FLTPARSE,
  41         CEGW_ERR_LAST
  42 };
  43
  44 char *cegw_errlist[] = {
  45         [CEGW_ERR_COLON   ] = "expected ':' (<hostname>:<port>)",
  46         [CEGW_ERR_GETADDRI] = "getaddrinfo failed",
  47         [CEGW_ERR_FLTALCK ] = "filter alloc failed",
  48         [CEGW_ERR_FLTPARSE] = "filter parsing failed"
  49 };
  50
  51 static const char help_msg[] = "usage:\n"
  52                                "        %s <can_if>[,filter]* <udp_listen_addr>:<port> <udp_dest_addr>:<port>\n"
  53                                "                [list of additional udp recipients <addr>:<port>]\n"
  54                                "example:\n"
  55                                "        %s can0 192.168.0.1:10501 192.168.0.4:980 192.168.0.7:1160\n\n"
  56                                "        Executing this command will set the gateway so that it will\n"
  57                                "        listen for udp messages on 192.168.0.1:10501 and send them\n"
  58                                "        to can0. Simultaneously, it will send all messages from can0\n"
  59                                "        to 192.168.0.4:980 and 192.168.0.7:1160 via udp. The message is\n"
  60                                "        therefore cloned. Notice that there can be more udp recipients.\n"
  61                                "        The can filter is specified in the same way as in candump utility.\n";
  62
  63 const struct addrinfo hints = {
  64         .ai_family = AF_INET,
  65         .ai_socktype = SOCK_DGRAM
  66 };
  67
  68 static void perr(char *s)
  69 {
  70         if (s) {
  71                 if (cegw_errno == 0 || cegw_errno >= CEGW_ERR_LAST) {
  72                         fprintf(stderr, "error: %s\n", s);
  73
  74                 } else {
  75                         fprintf(stderr, "error: %s, %s\n", s, cegw_errlist[cegw_errno]);
  76                 }
  77                 return;
  78         }
  79
  80         fprintf(stderr, "error: %s\n", cegw_errlist[cegw_errno]);
  81 }
  82
  83 /**
  84  * readsockaddr - parses @in for eth address.
  85  * Valid input is e.g. 127.0.0.1:10502. If parsing fails
  86  * the cause is stored in cegw_errno. Please note that
  87  * the function modifies content of arg.
  88  *
  89  * @param[in]  arg   hostname:port string
  90  * @param[out] addr  filled sockaddr_in structure
  91  * @return 0 on success, -1 otherwise
  92  */
  93 int readsockaddr(char *arg, struct sockaddr_in *addr)
  94 {
  95         int ret;
  96         char *delim;
  97         struct addrinfo *res;
  98
  99         delim = strchr(arg, ':');
 100
 101         if (delim == NULL) {
 102                 cegw_errno = CEGW_ERR_COLON;
 103                 return -1;
 104         }
 105
 106         *delim = '\0';
 107         delim++;
 108
 109         ret = getaddrinfo(arg, delim, &hints, &res);
 110         if (ret != 0) {
 111                 fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(ret));
 112                 cegw_errno = CEGW_ERR_GETADDRI;
 113                 return -1;
 114         }
 115
 116         memcpy(addr, res->ai_addr, sizeof(*addr));
 117
 118         freeaddrinfo(res);
 119         return 0;
 120 }
 121
 122 /**
 123  * readfilter - reads can filter definition from nptr
 124  */
 125 int readfilter(char *nptr, struct can_filter **filter, int *out_numfilter, can_err_mask_t *err_mask)
 126 {
 127         char *ptr;
 128         int numfilter;
 129         struct can_filter *rfilter;
 130
 131         numfilter = 0;
 132         ptr = nptr;
 133         while (ptr) {
 134                 numfilter++;
 135                 ptr++; /* hop behind the ',' */
 136                 ptr = strchr(ptr, ','); /* exit condition */
 137         }
 138
 139         rfilter = malloc(numfilter * sizeof(*rfilter));
 140         if (!rfilter) {
 141                 cegw_errno = CEGW_ERR_FLTALCK;
 142                 return -1;
 143         }
 144
 145         numfilter = 0;
 146         *err_mask = 0;
 147
 148         while (nptr) {
 149
 150                 ptr = nptr+1; /* hop behind the ',' */
 151                 nptr = strchr(ptr, ','); /* update exit condition */
 152
 153                 if (sscanf(ptr, "%x:%x",
 154                                         &rfilter[numfilter].can_id,
 155                                         &rfilter[numfilter].can_mask) == 2) {
 156                         rfilter[numfilter].can_mask &= ~CAN_ERR_FLAG;
 157                         numfilter++;
 158                 } else if (sscanf(ptr, "%x~%x",
 159                                         &rfilter[numfilter].can_id,
 160                                         &rfilter[numfilter].can_mask) == 2) {
 161                         rfilter[numfilter].can_id |= CAN_INV_FILTER;
 162                         rfilter[numfilter].can_mask &= ~CAN_ERR_FLAG;
 163                         numfilter++;
 164                 } else if (sscanf(ptr, "#%x", err_mask) != 1) {
 165                         cegw_errno = CEGW_ERR_FLTPARSE;
 166                         free(rfilter);
 167                         return -1;
 168                 }
 169         }
 170
 171         *filter = rfilter;
 172         *out_numfilter = numfilter;
 173         return 0;
 174 }
 175
 176 int main(int argc, char *argv[])
 177 {
 178         int i;
 179         int fd;
 180         int tmpi;
 181         int dstcnt;
 182         char *nptr;
 183         int numfilter = 0;
 184         int udp_sock, can_sock;
 185         can_err_mask_t err_mask = 0;
 186         struct sockaddr_in udp_addr;
 187         struct sockaddr_can can_addr;
 188         struct sockaddr_in *dst = NULL;
 189         struct cegw_ioctl *gwctl = NULL;
 190         struct can_filter *filter = NULL;
 191
 192         if (argc == 1 || (argc == 2 && strcmp(argv[1], "-h") == 0)) {
 193                 printf(help_msg, argv[0], argv[0]);
 194                 return 0;
 195         }
 196
 197         if (argc < 4) {
 198                 perr("not enough arguments");
 199                 printf(help_msg, argv[0], argv[0]);
 200                 return 1;
 201         }
 202
 203         dstcnt = argc-3;
 204         gwctl = (struct cegw_ioctl*)malloc(sizeof(*gwctl) + (dstcnt)*sizeof(struct sockaddr_in));
 205
 206         for (i=1; i<argc; i++) {
 207                 switch (i) {
 208                         case 1: /* can ifindex */
 209                                 nptr = strchr(argv[i], ',');
 210                                 if (nptr) {
 211                                         *nptr = '\0';
 212                                 }
 213
 214                                 can_addr.can_family = AF_CAN;
 215                                 tmpi = if_nametoindex(argv[i]);
 216                                 if (tmpi == 0) {
 217                                         perr("given can interface not found");
 218                                         free(gwctl);
 219                                         return 1;
 220                                 }
 221                                 can_addr.can_ifindex = tmpi;
 222                                 break;
 223                         case 2: /* listen addr */
 224                                 if (readsockaddr(argv[i], &udp_addr) != 0) {
 225                                         perr("reading listening address failed");
 226                                         free(gwctl);
 227                                         return 1;
 228                                 }
 229                                 break;
 230                         default: /* udp destination */
 231                                 dst = &gwctl->udp_dst[i-3];
 232                                 if (readsockaddr(argv[i], dst) != 0) {
 233                                         perr("reading udp destination failed");
 234                                         free(gwctl);
 235                                         return 1;
 236                                 }
 237                                 break;
 238                 }
 239         }
 240
 241         /* prepare udp socket */
 242         udp_sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
 243         if (udp_sock == -1) {
 244                 perror("udp socket()");
 245                 free(gwctl);
 246                 return 1;
 247         }
 248
 249         if (bind(udp_sock, (struct sockaddr *)&udp_addr, sizeof(struct sockaddr_in)) != 0) {
 250                 perror("bind(udp)");
 251                 free(gwctl);
 252                 return 1;
 253         }
 254
 255         /* prepare can socket */
 256         can_sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
 257         if (can_sock == -1) {
 258                 perror("can socket()");
 259                 free(gwctl);
 260                 return 1;
 261         }
 262
 263         if (bind(can_sock, (struct sockaddr *)&can_addr, sizeof(struct sockaddr_can)) != 0) {
 264                 perror("bind(can)");
 265                 free(gwctl);
 266                 return 1;
 267         }
 268
 269         /* can filter */
 270         if (nptr && (readfilter(nptr, &filter, &numfilter, &err_mask) != 0)) {
 271                 perr("can filter");
 272                 free(gwctl);
 273                 return 1;
 274         }
 275
 276         if (err_mask)
 277                 setsockopt(can_sock, SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
 278                                 &err_mask, sizeof(err_mask));
 279
 280         if (numfilter)
 281                 setsockopt(can_sock, SOL_CAN_RAW, CAN_RAW_FILTER,
 282                                 filter, numfilter * sizeof(struct can_filter));
 283         free(filter);
 284
 285         /* send it to kernel gateway */
 286         fd = open("/dev/canethgw", O_RDONLY);
 287         if (fd == -1) {
 288                 perror("/dev/canethgw");
 289                 free(gwctl);
 290                 return 1;
 291         }
 292
 293         gwctl->can_sock = can_sock;
 294         gwctl->udp_sock = udp_sock;
 295         gwctl->udp_dstcnt = dstcnt;
 296         gwctl->udp_addrlen = sizeof(struct sockaddr_in);
 297
 298         if (ioctl(fd, CEGW_IOCTL_START, gwctl) != 0) {
 299                 perror("ioctl");
 300                 free(gwctl);
 301                 return 1;
 302         }
 303         printf("gateway successfully set and running\n");
 304         free(gwctl);
 305
 306         /* sleep until someone kills me */
 307         pause();
 308
 309         return 0;
 310 }