drivers/can_serial/can_serial_hub.c
changeset 448 732c33c2d8a7
child 451 236c2e901e2e
equal deleted inserted replaced
447:c9d01296d6d9 448:732c33c2d8a7
       
     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 1 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 (rv <= 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 (FD_ISSET(hub_ports[i].fd, &rfds_copy)) {
       
   432 
       
   433 				rv = read_write(i, hub_ports, can_h, &rfds, max_fd);
       
   434 
       
   435 				if (rv < 0 && i < MAX_HUB_PORTS) {
       
   436 
       
   437 					FD_CLR(hub_ports[i].fd, &rfds);
       
   438 
       
   439 					hub_close(&hub_ports[i]);
       
   440 
       
   441 					rv = hub_open(&hub_ports[i]);
       
   442 
       
   443 					if (rv < 0) {
       
   444 						ret = 1;
       
   445 						break;
       
   446 					}
       
   447 
       
   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