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