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