drivers/can_serial/can_serial_hub.c

   1 /*
   2 This file is part of CanFestival, a library implementing CanOpen Stack.
   3
   4 Copyright (C): James Steward
   5
   6 See COPYING file for copyrights details.
   7
   8 This library is free software; you can redistribute it and/or
   9 modify it under the terms of the GNU Lesser General Public
  10 License as published by the Free Software Foundation; either
  11 version 2.1 of the License, or (at your option) any later version.
  12
  13 This library is distributed in the hope that it will be useful,
  14 but WITHOUT ANY WARRANTY; without even the implied warranty of
  15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16 Lesser General Public License for more details.
  17
  18 You should have received a copy of the GNU Lesser General Public
  19 License along with this library; if not, write to the Free Software
  20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  21 */
  22
  23 /**
  24         Pseudo CAN hub application.
  25 */
  26
  27 #define _GNU_SOURCE  //for asprintf()
  28
  29 #include <stdio.h>
  30 #include <unistd.h>
  31 #include <fcntl.h>
  32 #include <errno.h>
  33 #include <string.h>
  34 #include <termios.h>
  35 #include <stdlib.h>
  36 #include <dlfcn.h>
  37
  38 #define NEED_PRINT_MESSAGE
  39
  40 #ifdef DLL_CALL
  41 #undef DLL_CALL
  42 #endif
  43
  44 #define DLL_CALL(funcname) (* funcname##_driver)
  45
  46 #include "canfestival.h"
  47
  48 int DLL_CALL(canfd)(CAN_HANDLE)FCT_PTR_INIT;
  49
  50 #ifdef DLL_CALL
  51 #undef DLL_CALL
  52 #endif
  53
  54 #define DLL_CALL(funcname) funcname##_driver
  55
  56 #define DLSYM(name)\
  57         *(void **) (&name##_driver) = dlsym(handle, #name"_driver");\
  58         if ((error = dlerror()) != NULL)  {\
  59                 fprintf (stderr, "%s\n", error);\
  60                 UnLoadCanDriver(handle);\
  61                 return NULL;\
  62         }
  63
  64 #define MAX_HUB_PORTS 16
  65
  66 typedef struct {
  67         int fd;
  68         struct termios old_termio, new_termio;
  69         char *name;
  70 } port;
  71
  72 port hub_ports[MAX_HUB_PORTS + 1];  //An extra for a CAN driver port
  73
  74 s_BOARD bus_if = { "/dev/ttyS0", "125K" };
  75
  76 /* Receive a CAN message from a hub (pseudo) port */
  77 int hub_receive(port *p, Message *m)
  78 {
  79         int rv, N, n = 0;
  80         fd_set rfds;
  81         struct timeval tv;
  82
  83         N = 4; //initially try to read 4 bytes, including the length byte
  84
  85 retry:
  86         rv = read(p->fd, &((char *)m)[n], N - n);
  87
  88         if (rv == -1) {
  89                 fprintf(stderr, "read: %d, %s\n", p->fd, strerror(errno));
  90                 return -1;
  91         }
  92
  93         n += rv;
  94
  95         if (n == 4) {
  96                 N = (4 + m->len);
  97
  98                 if (m->len > 8) {
  99                         fprintf(stderr, "Warning: invalid message length %d\n",
 100                                         m->len);
 101
 102                         //try to resync
 103                         return 0;
 104                 }
 105
 106         }
 107
 108         if (n < N) {
 109
 110                 FD_ZERO(&rfds);
 111                 FD_SET(p->fd, &rfds);
 112
 113                 tv.tv_sec = 0;
 114                 tv.tv_usec=100000;
 115
 116                 rv = select(p->fd + 1, &rfds, NULL, NULL, &tv);
 117                 if (rv == 0 || rv == -1) {
 118                         fprintf(stderr, "select: %s\n", strerror(errno));
 119                         return 0;
 120                 }
 121
 122                 goto retry;
 123         }
 124
 125         return 1;
 126 }
 127
 128 /* send a CAN message to one of the hub ports */
 129 UNS8 hub_send(port *p, Message *m)
 130 {
 131         int rv;
 132
 133         rv = write(p->fd, m, 4 + m->len);
 134
 135         if (rv != 4 + m->len) {
 136                 return 1;
 137         }
 138
 139         return 0;
 140 }
 141
 142 /* Open a hub port */
 143 int hub_open(port *p)
 144 {
 145         if (p->fd != -1) {
 146                 fprintf(stderr, "Warning, port %s is already open, fd %d!\n",
 147                                         p->name, p->fd);
 148         }
 149
 150         p->fd = open(p->name, O_RDWR);
 151
 152         if (p->fd < 0) {
 153                 fprintf(stderr, "open: %s, %s\n", p->name, strerror(errno));
 154                 goto exit_here;
 155         }
 156
 157         if (tcgetattr(p->fd, &p->old_termio) != 0) {
 158                 fprintf(stderr, "tcgetattr: %s, %s\n",
 159                                         p->name, strerror(errno));
 160                 close(p->fd);
 161                 p->fd = -1;
 162                 goto exit_here;
 163         }
 164
 165         memcpy(&p->new_termio, &p->old_termio, sizeof(p->old_termio));
 166         cfmakeraw(&p->new_termio);
 167         cfsetispeed(&p->new_termio, B115200);
 168         cfsetospeed(&p->new_termio, B115200);
 169         tcsetattr(p->fd, TCSANOW, &p->new_termio);
 170
 171 exit_here:
 172         return p->fd;
 173 }
 174
 175 /* Close a hub port*/
 176 int hub_close(port *p)
 177 {
 178         if (p->fd >= 0) {
 179                 tcsetattr(p->fd, TCSANOW, &p->old_termio);
 180                 close(p->fd);
 181                 p->fd = -1;
 182         }
 183
 184         return 0;
 185 }
 186
 187 /** Read from the port index rd_port, and write to all other ports. */
 188 int read_write(int rd_port, port *p, CAN_HANDLE h, fd_set *wfds, int max_fd)
 189 {
 190         Message m;
 191         int rv, i;
 192         fd_set wfds_copy;
 193         struct timeval tv = {.tv_sec = 0, .tv_usec = 0}; //wait 0 msec
 194
 195         if (rd_port == MAX_HUB_PORTS) {
 196                 rv = DLL_CALL(canReceive)(h, &m);
 197
 198                 if (rv == 1) {
 199                         return 0;
 200                 }
 201         } else {
 202                 rv = hub_receive(&p[rd_port], &m);
 203
 204                 if (rv != 1) {
 205                         return rv;
 206                 }
 207         }
 208
 209         memcpy(&wfds_copy, wfds, sizeof(fd_set));
 210
 211         rv = select(max_fd + 1, NULL, &wfds_copy, NULL, &tv);
 212
 213         if (rv <= 0) {
 214                 return 0;
 215         }
 216
 217         for (i = 0; i < MAX_HUB_PORTS + 1; i++) {
 218
 219                 if (i == rd_port) {
 220                         fprintf(stderr, "[%d] ", i);
 221                         continue;
 222                 }
 223
 224                 if (p[i].fd < 0 || !FD_ISSET(p[i].fd, &wfds_copy)) {
 225                         fprintf(stderr, "{%d} ", i);
 226                         continue;
 227                 }
 228
 229                 fprintf(stderr, "<%d> ", i);
 230
 231                 if (i == MAX_HUB_PORTS && h) {
 232                         DLL_CALL(canSend)(h, &m);
 233                 } else {
 234                         hub_send(&p[i], &m);
 235                 }
 236         }
 237
 238         print_message(&m);
 239
 240         return 0;
 241 }
 242
 243 void help(void)
 244 {
 245         printf("\n\n");
 246         printf("This is a software hub for the CANFestival library, \n");
 247         printf("based on the *nix pseudo tty.  It supports up to 16\n");
 248         printf("connections from clients, and a connection to a CANFestival\n");
 249         printf("driver, for connection to the outside world.\n");
 250         printf("\n");
 251         printf("Basic use is simply to run can_hub.  Without arguments, it\n");
 252         printf("will use /dev/ptya[0..f] ptys.  You should then run your\n");
 253         printf("linux CANFestival app using libcanfestival_can_serial.so\n");
 254         printf("with the bus name /dev/ttyaX, where X is 0..f and unused.\n");
 255         printf("\n");
 256         printf("You can alter the pty base with -p /dev/ptyx .\n");
 257         printf("\n");
 258         printf("If you want to interface with some other CAN driver, supply\n");
 259         printf("the option -l /path/to/libcanfestival_can_foo.so .\n");
 260         printf("The default bus name and baud are /dev/ttyS0 and 125k.\n");
 261         printf("These can be overridden with -b /dev/{bus name} and -s {baud}.\n");
 262 }
 263
 264 /*UnLoads the dll*/
 265 UNS8 UnLoadCanDriver(LIB_HANDLE handle)
 266 {
 267         if(handle!=NULL)
 268         {
 269                 dlclose(handle);
 270
 271                 handle=NULL;
 272                 return 0;
 273         }
 274         return -1;
 275 }
 276
 277 /*Loads the dll and get funcs ptr*/
 278 LIB_HANDLE LoadCanDriver(char* driver_name)
 279 {
 280         LIB_HANDLE handle = NULL;
 281         char *error;
 282
 283         if(handle==NULL)
 284         {
 285                 handle = dlopen(driver_name, RTLD_LAZY);
 286         }
 287
 288         if (!handle) {
 289                 fprintf (stderr, "%s\n", dlerror());
 290                 return NULL;
 291         }
 292
 293         /*Get function ptr*/
 294         DLSYM(canReceive)
 295         DLSYM(canSend)
 296         DLSYM(canOpen)
 297         DLSYM(canChangeBaudRate)
 298         DLSYM(canClose)
 299         DLSYM(canfd)
 300
 301         return handle;
 302 }
 303
 304 /**
 305 */
 306 int main(int argc, char **argv)
 307 {
 308         int i, rv, max_fd = 0, ret = 0;
 309         fd_set rfds, rfds_copy;
 310         CAN_HANDLE can_h = NULL;
 311         LIB_HANDLE lib_h = NULL;
 312
 313         int c;
 314         extern char *optarg;
 315
 316         char *can_drv = NULL;
 317         char *pty_base = "/dev/ptya";
 318
 319         while ((c = getopt(argc, argv, "-b:s:l:p:h")) != EOF) {
 320                 switch (c) {
 321                 case 'b':
 322                         if (optarg[0] == 0) {
 323                                 help();
 324                                 exit(1);
 325                         }
 326                         bus_if.busname = optarg;
 327                         break;
 328                 case 's':
 329                         if (optarg[0] == 0) {
 330                                 help();
 331                                 exit(1);
 332                         }
 333                         bus_if.baudrate = optarg;
 334                         break;
 335                 case 'l':
 336                         if (optarg[0] == 0) {
 337                                 help();
 338                                 exit(1);
 339                         }
 340                         can_drv = optarg;
 341                         break;
 342                 case 'p':
 343                         if (optarg[0] == 0) {
 344                                 help();
 345                                 exit(1);
 346                         }
 347                         pty_base = optarg;
 348                         break;
 349                 case 'h':
 350                         help();
 351                         exit(1);
 352                         break;
 353                 default:
 354                         help();
 355                         exit(1);
 356                 }
 357         }
 358
 359         FD_ZERO(&rfds);
 360
 361         hub_ports[MAX_HUB_PORTS].fd = -1;
 362
 363         if (can_drv) {
 364                 lib_h = LoadCanDriver(can_drv);
 365                 if (lib_h == NULL) {
 366                         printf("Unable to load library: %s\n", can_drv);
 367                         exit(1);
 368                 }
 369
 370                 can_h = DLL_CALL(canOpen)(&bus_if);
 371                 if(!can_h) {
 372                         fprintf(stderr,"canOpen : failed\n");
 373                         exit(1);
 374                 }
 375
 376                 hub_ports[MAX_HUB_PORTS].fd = DLL_CALL(canfd)(can_h);
 377
 378                 FD_SET(hub_ports[MAX_HUB_PORTS].fd, &rfds);
 379
 380                 if (hub_ports[MAX_HUB_PORTS].fd > max_fd) {
 381                         max_fd = hub_ports[MAX_HUB_PORTS].fd;
 382                 }
 383
 384         }
 385
 386         for (i = 0; i < MAX_HUB_PORTS; i++) {
 387
 388                 hub_ports[i].fd = -1;
 389                 hub_ports[i].name = NULL;
 390
 391                 rv = asprintf(&hub_ports[i].name, "%s%x", pty_base, i);
 392
 393                 if (rv < 0) {
 394                         fprintf(stderr, "asprintf: %s\n", strerror(errno));
 395                         ret = 1;
 396                         break;
 397                 }
 398
 399                 rv = hub_open(&hub_ports[i]);
 400
 401                 if (rv < 0) {
 402                         ret = 1;
 403                         break;
 404                 }
 405
 406                 FD_SET(rv, &rfds);
 407
 408                 if (rv > max_fd) {
 409                         max_fd = rv;
 410                 }
 411         }
 412
 413         if (ret) {
 414                 return ret;
 415         }
 416
 417         while (!ret) {
 418                 memcpy(&rfds_copy, &rfds, sizeof(rfds));
 419
 420                 rv = select(max_fd + 1, &rfds_copy, NULL, NULL, NULL);
 421
 422                 if (rv < 0) {
 423                         //select error
 424                         fprintf(stderr, "select: %s\n", strerror(errno));
 425                         ret = 1;
 426                         continue;
 427                 }
 428
 429                 //as timeout is NULL, must be a rfds set.
 430                 for (i = 0; i < MAX_HUB_PORTS + 1; i++) {
 431                         if (hub_ports[i].fd >= 0 &&
 432                                         FD_ISSET(hub_ports[i].fd, &rfds_copy)) {
 433
 434                                 rv = read_write(i, hub_ports, can_h, &rfds, max_fd);
 435
 436                                 if (rv < 0 && i < MAX_HUB_PORTS) {
 437
 438                                         FD_CLR(hub_ports[i].fd, &rfds);
 439
 440                                         hub_close(&hub_ports[i]);
 441                                 }
 442                         }
 443
 444                         if (hub_ports[i].fd < 0 && i < MAX_HUB_PORTS) {
 445                                 rv = hub_open(&hub_ports[i]);
 446
 447                                 if (rv >= 0) {
 448                                         FD_SET(rv, &rfds);
 449
 450                                         if (rv > max_fd) {
 451                                                 max_fd = rv;
 452                                         }
 453                                 }
 454                         }
 455                 }
 456         }
 457
 458         for (i = 0; i < MAX_HUB_PORTS; i++) {
 459                 hub_close(&hub_ports[i]);
 460         }
 461
 462         if (hub_ports[MAX_HUB_PORTS].fd >= 0) {
 463                 DLL_CALL(canClose)(&bus_if);
 464         }
 465
 466         return ret;
 467 }
 468