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

#define MyCase(fc) case fc: printf(#fc);break;

    70

void print_message(Message *m)

    71

{

    72

	int i;

    73

	switch(m->cob_id.w >> 7)

    74

	{

    75

		MyCase(SYNC)

    76

		MyCase(TIME_STAMP)

    77

		MyCase(PDO1tx)

    78

		MyCase(PDO1rx)

    79

		MyCase(PDO2tx)

    80

		MyCase(PDO2rx)

    81

		MyCase(PDO3tx)

    82

		MyCase(PDO3rx)

    83

		MyCase(PDO4tx)

    84

		MyCase(PDO4rx)

    85

		MyCase(SDOtx)

    86

		MyCase(SDOrx)

    87

		MyCase(NODE_GUARD)

    88

		MyCase(NMT)

    89

	}

    90

	printf(" rtr:%d", m->rtr);

    91

	printf(" len:%d", m->len);

    92

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

    93

		printf(" %02x", m->data[i]);

    94

	printf("\n");

    95

}

    96

    97

void

    98

canInit (s_BOARD *board)

    99

{

   100

	int baudrate;

   101

   102

//#ifdef PCAN2_HEADER_

   103

	// if not the first handler

   104

	if(second_board == (s_BOARD *)board)

   105

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

   106

			CAN2_Init (baudrate,

   107

			  CAN_INIT_TYPE_ST extra_PCAN_init_params);

   108

//#endif

   109

	if(first_board == (s_BOARD *)board)

   110

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

   111

			CAN_Init (baudrate,

   112

			  CAN_INIT_TYPE_ST extra_PCAN_init_params);

   113

}

   114

   115

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

   116

UNS8

   117

canReceive_driver (CAN_HANDLE fd0, Message * m)

   118

{

   119

	UNS8 data;

   120

	TPCANMsg peakMsg;

   121

	DWORD Res;

   122

	do{

   123

		// We read the queue looking for messages.

   124

		// 

   125

		//pthread_mutex_lock (&PeakCan_mutex);

   126

#ifdef PCAN2_HEADER_

   127

		// if not the first handler

   128

		if(second_board == (s_BOARD *)fd0)

   129

			Res = CAN2_Read (&peakMsg);

   130

		else

   131

#endif

   132

		if(first_board == (s_BOARD *)fd0)

   133

			Res = CAN_Read (&peakMsg);

   134

		else

   135

			Res = CAN_ERR_BUSOFF;

   136

		// A message was received

   137

		// We process the message(s)

   138

		// 

   139

		if (Res == CAN_ERR_OK)

   140

		{

   141

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

   142

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

   143

			{

   144

				if (peakMsg.MSGTYPE == CAN_ERR_BUSOFF)

   145

				{

   146

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

   147

					canInit((s_BOARD*) fd0);

   148

					usleep (10000);

   149

				}

   150

   151

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

   152

				//pthread_mutex_unlock (&PeakCan_mutex);

   153

				return peakMsg.MSGTYPE ==

   154

					MSGTYPE_STATUS ? peakMsg.DATA[2] : CAN_ERR_OVERRUN;

   155

			}

   156

			m->cob_id.w = peakMsg.ID;

   157

   158

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

   159

				m->rtr = 0;

   160

			else

   161

				m->rtr = 1;

   162

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

   163

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

   164

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

   165

   166

		}else{

   167

		//pthread_mutex_unlock (&PeakCan_mutex);

   168

		//if (Res != CAN_ERR_OK)

   169

		//{

   170

			if (!

   171

				(Res & CAN_ERR_QRCVEMPTY || Res & CAN_ERR_BUSLIGHT

   172

				 || Res & CAN_ERR_BUSHEAVY))

   173

			{

   174

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

   175

				return 1;

   176

			}

   177

			usleep (1000);		

   178

		}

   179

	}while(Res != CAN_ERR_OK);

   180

	return 0;

   181

}

   182

   183

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

   184

UNS8

   185

canSend_driver (CAN_HANDLE fd0, Message * m)

   186

{

   187

	UNS8 data;

   188

	TPCANMsg peakMsg;

   189

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

   190

	if (m->rtr == 0)

   191

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

   192

	else

   193

	{

   194

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

   195

	}

   196

	peakMsg.LEN = m->len;

   197

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

   198

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

   199

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

   200

   201

	do

   202

	{

   203

#ifdef PCAN2_HEADER_

   204

		// if not the first handler

   205

		if(second_board == (s_BOARD *)fd0)

   206

		{

   207

			errno = CAN2_Write (&peakMsg);

   208

		}

   209

		else 

   210

#endif

   211

		if(first_board == (s_BOARD *)fd0)

   212

			{

   213

				errno = CAN_Write (&peakMsg);

   214

			}

   215

		else 

   216

			goto fail;

   217

		if (errno)

   218

		{

   219

			if (errno == CAN_ERR_BUSOFF)

   220

			{

   221

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

   222

				canInit((s_BOARD*)fd0);

   223

				usleep (10000);

   224

			}

   225

			usleep (1000);

   226

		}

   227

	}

   228

	while (errno != CAN_ERR_OK);

   229

	return 0;

   230

fail:

   231

	return 1;

   232

}

   233

   234

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

   235

CAN_HANDLE

   236

canOpen_driver (s_BOARD * board)

   237

{

   238

  char busname[64];

   239

  char* pEnd;

   240

   241

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

   242

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

   243

  {

   244

      first_board = board;

   245

      printf ("First Board selected\n");

   246

  }

   247

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

   248

  {

   249

     second_board = board;

   250

     printf ("Second Board selected\n");

   251

  }

   252

	canInit(board);

   253

   254

	return (CAN_HANDLE)board;

   255

}

   256

   257

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

   258

int

   259

canClose_driver (CAN_HANDLE fd0)

   260

{

   261

#ifdef PCAN2_HEADER_

   262

	// if not the first handler

   263

	if(second_board == (s_BOARD *)fd0)

   264

	{

   265

		CAN2_Close ();

   266

		second_board = (s_BOARD *)NULL;

   267

	}else	

   268

#endif

   269

	if(first_board == (s_BOARD *)fd0)

   270

	{

   271

		CAN_Close ();

   272

		first_board = (s_BOARD *)NULL;

   273

	}

   274

	return 0;

   275

}