1

/*

     2

This file is part of CanFestival, a library implementing CanOpen Stack. 

     3

     4

Copyright (C): Edouard TISSERANT and Francis DUPIN

     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

#if defined(WIN32) && !defined(__CYGWIN__)

    24

#define usleep(micro) Sleep(micro%1000 ? (micro/1000) + 1 : (micro/1000))

    25

#else

    26

#include <stdio.h>

    27

#include <string.h>

    28

#include <errno.h>

    29

#include <fcntl.h>

    30

#endif

    31

    32

#include "cancfg.h"

    33

#include "can_driver.h"

    34

#include "def.h"

    35

#ifndef extra_PCAN_init_params

    36

	#define extra_PCAN_init_params /**/

    37

#else

    38

	#define extra_PCAN_init_params\

    39

		,getenv("PCANHwType") ? strtol(getenv("PCANHwType"),NULL,0):0\

    40

		,getenv("PCANIO_Port") ? strtol(getenv("PCANIO_Port"),NULL,0):0\

    41

		,getenv("PCANInterupt") ? strtol(getenv("PCANInterupt"),NULL,0):0

    42

#endif

    43

    44

static s_BOARD *first_board = NULL;

    45

#ifdef PCAN2_HEADER_

    46

static s_BOARD *second_board = NULL;

    47

#endif

    48

    49

//pthread_mutex_t PeakCan_mutex = PTHREAD_MUTEX_INITIALIZER;

    50

    51

// Define for rtr CAN message

    52

#define CAN_INIT_TYPE_ST_RTR MSGTYPE_STANDARD | MSGTYPE_RTR

    53

    54

/***************************************************************************/

    55

int TranslateBaudeRate(char* optarg){

    56

	if(!strcmp( optarg, "1M")) return CAN_BAUD_1M;

    57

	if(!strcmp( optarg, "500K")) return CAN_BAUD_500K;

    58

	if(!strcmp( optarg, "250K")) return CAN_BAUD_250K;

    59

	if(!strcmp( optarg, "125K")) return CAN_BAUD_125K;

    60

	if(!strcmp( optarg, "100K")) return CAN_BAUD_100K;

    61

	if(!strcmp( optarg, "50K")) return CAN_BAUD_50K;

    62

	if(!strcmp( optarg, "20K")) return CAN_BAUD_20K;

    63

	if(!strcmp( optarg, "10K")) return CAN_BAUD_10K;

    64

	if(!strcmp( optarg, "5K")) return CAN_BAUD_5K;

    65

	if(!strcmp( optarg, "none")) return 0;

    66

	return 0x0000;

    67

}

    68

    69

void

    70

canInit (s_BOARD *board)

    71

{

    72

	int baudrate;

    73

    74

#ifdef PCAN2_HEADER_

    75

	// if not the first handler

    76

	if(second_board == (s_BOARD *)board)

    77

		if(baudrate = TranslateBaudeRate(board->baudrate))

    78

			CAN2_Init (baudrate,

    79

			  CAN_INIT_TYPE_ST extra_PCAN_init_params);

    80

#endif

    81

	if(first_board == (s_BOARD *)board)

    82

		if(baudrate = TranslateBaudeRate(board->baudrate))

    83

			CAN_Init (baudrate,

    84

			  CAN_INIT_TYPE_ST extra_PCAN_init_params);

    85

}

    86

    87

/*********functions which permit to communicate with the board****************/

    88

UNS8

    89

canReceive_driver (CAN_HANDLE fd0, Message * m)

    90

{

    91

	UNS8 data;

    92

	TPCANMsg peakMsg;

    93

	DWORD Res;

    94

	do{

    95

		// We read the queue looking for messages.

    96

		// 

    97

		//pthread_mutex_lock (&PeakCan_mutex);

    98

#ifdef PCAN2_HEADER_

    99

		// if not the first handler

   100

		if(second_board == (s_BOARD *)fd0)

   101

			Res = CAN2_Read (&peakMsg);

   102

		else

   103

#endif

   104

		if(first_board == (s_BOARD *)fd0)

   105

			Res = CAN_Read (&peakMsg);

   106

		else

   107

			Res = CAN_ERR_BUSOFF;

   108

		// A message was received

   109

		// We process the message(s)

   110

		// 

   111

		if (Res == CAN_ERR_OK)

   112

		{

   113

			// if something different that 11bit or rtr... problem

   114

			if (peakMsg.MSGTYPE & ~(MSGTYPE_STANDARD | MSGTYPE_RTR))

   115

			{

   116

				if (peakMsg.MSGTYPE == CAN_ERR_BUSOFF)

   117

				{

   118

					printf ("!!! Peak board read : re-init\n");

   119

					canInit((s_BOARD*) fd0);

   120

					usleep (10000);

   121

				}

   122

   123

				// If status, return status if 29bit, return overrun

   124

				//pthread_mutex_unlock (&PeakCan_mutex);

   125

				return peakMsg.MSGTYPE ==

   126

					MSGTYPE_STATUS ? peakMsg.DATA[2] : CAN_ERR_OVERRUN;

   127

			}

   128

			m->cob_id.w = peakMsg.ID;

   129

   130

			if (peakMsg.MSGTYPE == CAN_INIT_TYPE_ST)	/* bits of MSGTYPE_ */

   131

				m->rtr = 0;

   132

			else

   133

				m->rtr = 1;

   134

			m->len = peakMsg.LEN;	/* count of data bytes (0..8) */

   135

			for (data = 0; data < peakMsg.LEN; data++)

   136

				m->data[data] = peakMsg.DATA[data];	/* data bytes, up to 8 */

   137

#if defined DEBUG_MSG_CONSOLE_ON

   138

			printf("in : ");

   139

			print_message(m);

   140

#endif

   141

		}else{

   142

		//pthread_mutex_unlock (&PeakCan_mutex);

   143

		//if (Res != CAN_ERR_OK)

   144

		//{

   145

			if (!

   146

				(Res & CAN_ERR_QRCVEMPTY || Res & CAN_ERR_BUSLIGHT

   147

				 || Res & CAN_ERR_BUSHEAVY))

   148

			{

   149

				printf ("canReceive returned error (%d)\n", Res);

   150

				return 1;

   151

			}

   152

			usleep (1000);		

   153

		}

   154

	}while(Res != CAN_ERR_OK);

   155

	return 0;

   156

}

   157

   158

/***************************************************************************/

   159

UNS8

   160

canSend_driver (CAN_HANDLE fd0, Message * m)

   161

{

   162

	UNS8 data;

   163

	TPCANMsg peakMsg;

   164

	peakMsg.ID = m->cob_id.w;	/* 11/29 bit code */

   165

	if (m->rtr == 0)

   166

		peakMsg.MSGTYPE = CAN_INIT_TYPE_ST;	/* bits of MSGTYPE_ */

   167

	else

   168

	{

   169

		peakMsg.MSGTYPE = CAN_INIT_TYPE_ST_RTR;	/* bits of MSGTYPE_ */

   170

	}

   171

	peakMsg.LEN = m->len;

   172

	/* count of data bytes (0..8) */

   173

	for (data = 0; data < m->len; data++)

   174

		peakMsg.DATA[data] = m->data[data];	/* data bytes, up to 8 */

   175

   176

	do

   177

	{

   178

#ifdef PCAN2_HEADER_

   179

		// if not the first handler

   180

		if(second_board == (s_BOARD *)fd0)

   181

		{

   182

			errno = CAN2_Write (&peakMsg);

   183

		}

   184

		else 

   185

#endif

   186

		if(first_board == (s_BOARD *)fd0)

   187

			{

   188

				errno = CAN_Write (&peakMsg);

   189

			}

   190

		else 

   191

			goto fail;

   192

		if (errno)

   193

		{

   194

			if (errno == CAN_ERR_BUSOFF)

   195

			{

   196

				printf ("!!! Peak board write : re-init\n");

   197

				canInit((s_BOARD*)fd0);

   198

				usleep (10000);

   199

			}

   200

			usleep (1000);

   201

		}

   202

	}

   203

	while (errno != CAN_ERR_OK);

   204

#if defined DEBUG_MSG_CONSOLE_ON

   205

	printf("out : ");

   206

	print_message(m);

   207

#endif

   208

	return 0;

   209

fail:

   210

	return 1;

   211

}

   212

   213

/***************************************************************************/

   214

CAN_HANDLE

   215

canOpen_driver (s_BOARD * board)

   216

{

   217

  char busname[64];

   218

  char* pEnd;

   219

   220

  //printf ("Board Busname=%d.\n",strtol(board->busname, &pEnd,0));

   221

  if (strtol(board->busname, &pEnd,0) == 0)

   222

  {

   223

      first_board = board;

   224

      printf ("First Board selected\n");

   225

      canInit(board);	

   226

      return (CAN_HANDLE)board;

   227

  }

   228

  #ifdef PCAN2_HEADER_

   229

  if (strtol(board->busname, &pEnd,0) == 1)

   230

  {

   231

      second_board = board;

   232

      printf ("Second Board selected\n");

   233

      canInit(board);	

   234

      return (CAN_HANDLE)board;

   235

  }

   236

  #endif

   237

  return NULL;

   238

}

   239

   240

/***************************************************************************/

   241

int

   242

canClose_driver (CAN_HANDLE fd0)

   243

{

   244

#ifdef PCAN2_HEADER_

   245

	// if not the first handler

   246

	if(second_board == (s_BOARD *)fd0)

   247

	{

   248

		CAN2_Close ();

   249

		second_board = (s_BOARD *)NULL;

   250

	}else	

   251

#endif

   252

	if(first_board == (s_BOARD *)fd0)

   253

	{

   254

		CAN_Close ();

   255

		first_board = (s_BOARD *)NULL;

   256

	}

   257

	return 0;

   258

}