1

/*

     2

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

     3

     4

  Copyright (C): Jaroslav Fojtik

     5

*/

     6

     7

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

     8

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

     9

#else

    10

#include <stdio.h>

    11

#include <string.h>

    12

#include <errno.h>

    13

#include <fcntl.h>

    14

#endif

    15

    16

    17

#include "cancfg.h"

    18

#include "can_driver.h"

    19

#include "def.h"

    20

    21

UNS8 LIBAPI canSend_driver(CAN_HANDLE fd0, Message const *m);

    22

    25

    26

#define VERSION_2

    27

    28

/* dummy implementation for older version. */

    29

#ifndef VERSION_2

    30

void CAN_SetRcvEvent(HANDLE hEventx)

    31

{

    32

  SetEvent(hEventx);

    33

}

    34

#endif

    35

    36

    37

#define SLAVE_COUNT 10

    38

#define QueueSize 100

    39

    40

    41

#ifndef extra_PCAN_init_params

    42

    #define extra_PCAN_init_params  /**/

    43

#else

    44

    long int print_getenv(const char* pcanparam)

    45

    {

    46

        char* param=NULL;

    47

        long int res=0;

    48

    49

        param = getenv(pcanparam);

    50

        if(param != NULL){

    51

            res = strtol(param,NULL,0);

    52

        }

    53

        else

    54

            printf("Environment variable %s not defined !\n", pcanparam);

    55

        printf("Found environment variable %s : %ld\n", pcanparam ,res);

    56

        return res;

    57

    }

    58

    #define extra_PCAN_init_params\

    59

        ,print_getenv("PCANHwType")\

    60

        ,print_getenv("PCANIO_Port")\

    61

        ,print_getenv("PCANInterupt")

    62

#endif

    63

    64

    65

    66

typedef struct 

    67

{

    68

  s_BOARD *board;

    69

  Message MQueue[QueueSize];

    70

  unsigned QStart, QEnd;

    71

  HANDLE hEventx;

    72

} QueueRecord;

    73

    74

int initialisedQ = 0;

    75

QueueRecord Q_DATA[10];

    76

    77

    78

/** Store message into a queue. */

    79

static void PushMsgToQueue(QueueRecord *QR, Message *m)

    80

{

    81

  if(QR==NULL || m==NULL) return;

    82

  if(QR->board==NULL) return;							// No Board assigned yet

    83

  memcpy(&QR->MQueue[QR->QStart], m, sizeof(Message));

    84

  QR->QStart = (QR->QStart + 1) % QueueSize;

    85

  if(QR->QEnd==QR->QStart) QR->QEnd = (QR->QEnd+1) % QueueSize;

    86

  if(QR->hEventx) SetEvent(QR->hEventx);					// Signalise internal flag

    87

}

    88

    89

    90

/** Get message from a queue. */

    91

static int PopMsgFromQueue(QueueRecord *QR, Message *m)

    92

{

    93

  if(QR==NULL || m==NULL) return 0;

    94

  if(QR->QEnd == QR->QStart) return 0;

    95

    96

  memcpy(m, &QR->MQueue[QR->QEnd], sizeof(Message));

    97

  QR->QEnd = (QR->QEnd+1) % QueueSize;

    98

  return 1;

    99

}

   100

   101

   102

/** Create the Event for the first board */

   103

HANDLE hEvent1 = NULL;

   104

CRITICAL_SECTION InitLock;

   105

   106

   107

// Define for rtr CAN message

   108

#define CAN_INIT_TYPE_ST_RTR MSGTYPE_STANDARD | MSGTYPE_RTR

   109

   110

   111

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

   112

static int TranslateBaudeRate(char* optarg)

   113

{

   114

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

   115

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

   116

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

   117

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

   118

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

   119

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

   120

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

   121

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

   122

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

   123

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

   124

  return 0x0000;

   125

}

   126

   127

   128

static UNS8 canInit(s_BOARD *board)

   129

{

   130

int baudrate;

   131

int ret = 0;

   132

   133

  if(hEvent1==NULL)

   134

  {	//Create the Event for the first board

   135

    hEvent1 = CreateEvent(NULL, // lpEventAttributes

   136

                        FALSE,  // bManualReset

   137

                        FALSE,  // bInitialState

   138

                        "");    // lpName

   139

    InitializeCriticalSection(&InitLock);

   140

  }

   141

   142

  EnterCriticalSection(&InitLock);  

   143

   144

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

   145

  {

   146

    ret = CAN_Init(baudrate, CAN_INIT_TYPE_ST extra_PCAN_init_params);

   147

    if(ret != CAN_ERR_OK) 

   148

    {

   149

      LeaveCriticalSection(&InitLock);      

   150

      return 0;

   151

    }

   152

  }

   153

   154

  CAN_SetRcvEvent(hEvent1);  //Set Event Handler for CANReadExt

   155

  LeaveCriticalSection(&InitLock);    

   156

  return 1;

   157

}

   158

   159

   160

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

   161

UNS8 LIBAPI canReceive_driver(CAN_HANDLE fd0, Message *m)

   162

{

   163

static int HeavyCounter = 0;

   164

int ret=0;

   165

UNS8 data;

   166

TPCANMsg peakMsg;

   167

DWORD Res;

   168

DWORD result;

   169

HANDLE hh[2];

   170

int i;

   171

   172

#ifdef CAN_READ_EX

   173

    TPCANTimestamp peakRcvTime;

   174

#endif      

   175

   176

  i = strtol(((s_BOARD *)fd0)->busname,NULL,0);

   177

  if(i>=SLAVE_COUNT || i<0) return 1;           // error

   178

  if(Q_DATA[i].board!=(s_BOARD *)fd0) return 1;      // error

   179

   180

  hh[0]=hEvent1; hh[1]=Q_DATA[i].hEventx;

   181

   182

	// loop until valid message or fatal error

   183

  do

   184

  {

   185

CONTINUE:

   186

     if(PopMsgFromQueue(&Q_DATA[i],m)) return 0;	//message is waiting in the internal queue

   187

   188

        // We read the queue looking for messages.

   189

#ifdef VERSION_2

   190

     result = WaitForMultipleObjects(2,hh,FALSE,15);  

   191

     if(Q_DATA[i].board==NULL) return 1;		//exit hook, exit immediatelly when device is closed

   192

   193

     if(result == WAIT_OBJECT_0+1)

   194

	 goto CONTINUE;	//look to a PopMsgFromQueue() (continue will check while(), goto skips it)

   195

   196

     if(result==WAIT_OBJECT_0 || result==WAIT_TIMEOUT)

   197

     {

   198

#endif     

   199

#ifdef CAN_READ_EX

   200

       Res = CAN_ReadEx(&peakMsg, &peakRcvTime);

   201

#else

   202

       Res = CAN_Read(&peakMsg);

   203

#endif

   204

                // Exit receive thread when handle is no more valid

   205

#ifdef CAN_ERRMASK_ILLHANDLE

   206

       if(Res & CAN_ERRMASK_ILLHANDLE) return 1;

   207

#else

   208

       if(Res & CAN_ERR_ILLHANDLE) return 1;

   209

#endif

   210

   211

#ifndef VERSION_2

   212

       if(Res != CAN_ERR_OK) 

   213

             result = WaitForSingleObject(hEvent1, 1);   //pooling for pcan release<2

   214

#endif

   215

	if(Res==CAN_ERR_QRCVEMPTY) goto CONTINUE;

   216

#ifdef VERSION_2       

   217

     }

   218

     else

   219

     {

   220

       //if(result==WAIT_TIMEOUT || result==(WAIT_OBJECT_0+1)) 

   221

       //     Res = CAN_ERR_BUSLIGHT;

   222

       //else 

   223

              Res = CAN_ERR_UNKNOWN;

   224

     }

   225

#endif

   226

   227

    if(Res==CAN_ERR_BUSHEAVY)

   228

    {

   229

      if(HeavyCounter++>10) 

   230

      {

   231

	HeavyCounter = 0;

   232

	Res=CAN_ERR_BUSOFF;

   233

      }

   234

    }

   235

   236

    if(Res & CAN_ERR_BUSOFF)

   237

    {      

   238

      peakMsg.MSGTYPE = MSGTYPE_STATUS;

   239

      peakMsg.DATA[3] = CAN_ERR_BUSOFF;

   240

      Res = CAN_ERR_OK;

   241

    }   

   242

   243

        // A message was received : we process the message(s)

   244

    if(Res == CAN_ERR_OK)

   245

    {

   246

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

   247

	switch(peakMsg.MSGTYPE)

   248

	{

   249

	  case MSGTYPE_STATUS:

   250

		  switch(peakMsg.DATA[3])

   251

	          {

   252

		    case CAN_ERR_BUSHEAVY: 

   253

				      break;

   254

	            case CAN_ERR_BUSOFF: 

   255

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

   256

				      canInit((s_BOARD*)fd0);

   257

				      usleep(2000);

   258

				      break;

   259

		  }

   260

		  return peakMsg.DATA[3];	/* if something different that 11bit or rtr... problem */

   261

   262

	  case MSGTYPE_STANDARD:		/* bits of MSGTYPE_ */

   263

	  case MSGTYPE_EXTENDED:

   264

			  m->rtr = 0;

   265

			  break;

   266

   267

	  case MSGTYPE_RTR:			/* bits of MSGTYPE_ */

   268

			  m->rtr = 1;

   269

			  break;

   270

   271

	  default: return CAN_ERR_OVERRUN;	/* If status, return status if 29bit, return overrun. */

   272

   273

	}

   274

   275

      m->cob_id = peakMsg.ID;

   276

   277

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

   278

                m->rtr = 0;

   279

      else

   280

                m->rtr = 1;

   281

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

   282

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

   283

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

   284

#if defined DEBUG_MSG_CONSOLE_ON

   285

      MSG("in : ");

   286

      print_message(m);

   287

#endif

   288

    }

   289

    else

   290

    {       // not benign error => fatal error

   291

      if(!(Res & CAN_ERR_QRCVEMPTY

   292

                    || Res & CAN_ERR_BUSLIGHT

   293

                    || Res & CAN_ERR_BUSHEAVY))

   294

      {

   295

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

   296

        return 1;

   297

      }

   298

    }

   299

  } while(Res != CAN_ERR_OK);

   300

   301

   302

     // populate message received to other drivers

   303

  for(i=0; i<SLAVE_COUNT; i++)

   304

  {

   305

    if(Q_DATA[i].board != (s_BOARD *)fd0)	// do not populate to own queue

   306

    {

   307

      PushMsgToQueue(&Q_DATA[i],m);

   308

    }   

   309

  }

   310

   311

  return 0;

   312

}

   313

   314

   315

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

   316

   317

/* return 0 - OK; 1 - failure */

   318

UNS8 LIBAPI canSend_driver(CAN_HANDLE fd0, Message const *m)

   319

{

   320

UNS8 data;

   321

TPCANMsg peakMsg;

   322

int i, j;

   323

int loc_errno;

   324

int MaxLoops = 100;

   325

   326

  i = -1;

   327

  for(j=0; j<SLAVE_COUNT; j++)

   328

  {

   329

    if(Q_DATA[j].board != (s_BOARD *)fd0)   // store this message forr all other drivers

   330

    {

   331

      PushMsgToQueue(&Q_DATA[j],m);

   332

      i = j;

   333

    }

   334

  }

   335

   336

  if(i<0) return 1;                         // no board found

   337

   338

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

   339

  if(m->rtr == 0)

   340

  {

   341

    if(peakMsg.ID > 0x7FF)

   342

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

   343

    else

   344

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

   345

  }

   346

  else	

   347

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

   348

   349

  peakMsg.LEN = m->len;

   350

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

   351

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

   352

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

   353

   354

  do 

   355

  {

   356

    errno = loc_errno = CAN_Write(&peakMsg);

   357

   358

    if(loc_errno)

   359

    {

   360

      if(loc_errno==CAN_ERR_BUSOFF && (MaxLoops%20)==1)

   361

      {

   362

#if defined DEBUG_MSG_CONSOLE_ON

   363

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

   364

#endif

   365

        canInit((s_BOARD*)fd0);

   366

        usleep(1000);

   367

      }

   368

      usleep(80);

   369

    }

   370

    if(MaxLoops-- == 0) break;			// limit max looping

   371

  } while(loc_errno != CAN_ERR_OK);

   372

   373

#if defined DEBUG_MSG_CONSOLE_ON

   374

    MSG("out : ");

   375

    print_message(m);

   376

#endif

   377

    return 0;					// OK

   378

}

   379

   380

   381

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

   382

UNS8 LIBAPI canChangeBaudRate_driver(CAN_HANDLE fd, char* baud)

   383

{

   384

    printf("canChangeBaudRate not yet supported by this driver\n");

   385

    return 0;

   386

}

   387

   388

   389

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

   390

CAN_HANDLE LIBAPI canOpen_driver(s_BOARD * board)

   391

{

   392

int ret;

   393

int i;

   394

   395

  if(!initialisedQ)

   396

  {

   397

    memset(Q_DATA,0,sizeof(Q_DATA));

   398

    initialisedQ = 1;

   399

  }

   400

   401

  i = strtol(board->busname,NULL,0);			// Get slot name

   402

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

   403

  if(i<SLAVE_COUNT && i>=0)

   404

  {

   405

    Q_DATA[i].board = board;    

   406

      //printf ("First Board selected\n");

   407

    if(Q_DATA[i].hEventx==NULL)				// Create local event

   408

    {

   409

      Q_DATA[i].hEventx = CreateEvent(NULL, FALSE, FALSE, "");

   410

    }

   411

   412

    if(hEvent1!=NULL) return (CAN_HANDLE)board;		// Create global event, if needed

   413

   414

    ret = canInit(board);

   415

    if(ret)

   416

      return (CAN_HANDLE)board;

   417

  }

   418

   419

  return NULL;

   420

}

   421

   422

   423

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

   424

int LIBAPI canClose_driver(CAN_HANDLE fd0)

   425

{

   426

s_BOARD *x_board = NULL;

   427

int ActiveBoards = 0;

   428

int i;

   429

   430

  if((s_BOARD *)fd0==NULL) return 0;

   431

  for(i=0; i<SLAVE_COUNT; i++)

   432

  {

   433

    if(Q_DATA[i].board == (s_BOARD *)fd0)

   434

    {

   435

      x_board = Q_DATA[i].board;

   436

      Q_DATA[i].board = NULL;

   437

      CloseHandle(Q_DATA[i].hEventx);

   438

      Q_DATA[i].hEventx = NULL;

   439

    }

   440

    else

   441

      ActiveBoards++;

   442

  }

   443

   444

  if(ActiveBoards<=0)

   445

  {				// No can device is used.

   446

    CAN_SetRcvEvent(NULL);

   447

    CAN_Close();    

   448

    if(hEvent1) 

   449

    {

   450

      SetEvent(hEvent1);

   451

      CloseHandle(hEvent1);

   452

      hEvent1 = NULL;

   453

    }

   454

  }

   455

  else    

   456

    SetEvent(hEvent1);

   457

   458

  return 0;

   459

}

   460

   461

   462

   463

   464

   465