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

    23

    24

#define VERSION_2

    25

    26

/* dummy implementation for older version. */

    27

#ifndef VERSION_2

    28

void CAN_SetRcvEvent(HANDLE hEventx)

    29

{

    30

  SetEvent(hEventx);

    31

}

    32

#endif

    33

    34

    35

#define SLAVE_COUNT 10

    36

#define QueueSize 100

    37

    38

    39

#ifndef extra_PCAN_init_params

    40

    #define extra_PCAN_init_params  /**/

    41

#else

    42

    long int print_getenv(const char* pcanparam)

    43

    {

    44

        char* param=NULL;

    45

        long int res=0;

    46

    47

        param = getenv(pcanparam);

    48

        if(param != NULL){

    49

            res = strtol(param,NULL,0);

    50

        }

    51

        else

    52

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

    53

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

    54

        return res;

    55

    }

    56

    #define extra_PCAN_init_params\

    57

        ,print_getenv("PCANHwType")\

    58

        ,print_getenv("PCANIO_Port")\

    59

        ,print_getenv("PCANInterupt")

    60

#endif

    61

    62

    63

    64

typedef struct 

    65

{

    66

  s_BOARD *board;

    67

  Message MQueue[QueueSize];

    68

  unsigned QStart, QEnd;

    69

  HANDLE hEventx;

    70

} QueueRecord;

    71

    72

int initialisedQ = 0;

    73

QueueRecord Q_DATA[10];

    74

    75

    76

/** Store message into a queue. */

    77

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

    78

{

    79

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

    80

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

    81

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

    82

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

    83

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

    84

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

    85

}

    86

    87

    88

/** Get message from a queue. */

    89

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

    90

{

    91

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

    92

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

    93

    94

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

    95

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

    96

  return 1;

    97

}

    98

    99

   100

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

   101

HANDLE hEvent1 = NULL;

   102

CRITICAL_SECTION InitLock;

   103

   104

   105

// Define for rtr CAN message

   106

#define CAN_INIT_TYPE_ST_RTR MSGTYPE_STANDARD | MSGTYPE_RTR

   107

   108

   109

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

   110

static int TranslateBaudeRate(char* optarg)

   111

{

   112

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

   113

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

   114

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

   115

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

   116

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

   117

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

   118

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

   119

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

   120

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

   121

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

   122

  return 0x0000;

   123

}

   124

   125

   126

static UNS8 canInit(s_BOARD *board)

   127

{

   128

int baudrate;

   129

int ret = 0;

   130

   131

  if(hEvent1==NULL)

   132

  {	//Create the Event for the first board

   133

    hEvent1 = CreateEvent(NULL, // lpEventAttributes

   134

                        FALSE,  // bManualReset

   135

                        FALSE,  // bInitialState

   136

                        "");    // lpName

   137

    InitializeCriticalSection(&InitLock);

   138

  }

   139

   140

  EnterCriticalSection(&InitLock);  

   141

   142

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

   143

  {

   144

    ret = CAN_Init(baudrate, CAN_INIT_TYPE_ST extra_PCAN_init_params);

   145

    if(ret != CAN_ERR_OK) 

   146

    {

   147

      LeaveCriticalSection(&InitLock);      

   148

      return 0;

   149

    }

   150

  }

   151

   152

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

   153

  LeaveCriticalSection(&InitLock);    

   154

  return 1;

   155

}

   156

   157

   158

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

   159

UNS8 LIBAPI canReceive_driver(CAN_HANDLE fd0, Message *m)

   160

{

   161

static int HeavyCounter = 0;

   162

int ret=0;

   163

UNS8 data;

   164

TPCANMsg peakMsg;

   165

DWORD Res;

   166

DWORD result;

   167

HANDLE hh[2];

   168

int i;

   169

   170

#ifdef CAN_READ_EX

   171

    TPCANTimestamp peakRcvTime;

   172

#endif      

   173

   174

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

   175

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

   176

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

   177

   178

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

   179

   180

	// loop until valid message or fatal error

   181

  do

   182

  {

   183

CONTINUE:

   184

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

   185

   186

        // We read the queue looking for messages.

   187

#ifdef VERSION_2

   188

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

   189

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

   190

   191

     if(result == WAIT_OBJECT_0+1)

   192

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

   193

   194

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

   195

     {

   196

#endif     

   197

#ifdef CAN_READ_EX

   198

       Res = CAN_ReadEx(&peakMsg, &peakRcvTime);

   199

#else

   200

       Res = CAN_Read(&peakMsg);

   201

#endif

   202

                // Exit receive thread when handle is no more valid

   203

#ifdef CAN_ERRMASK_ILLHANDLE

   204

       if(Res & CAN_ERRMASK_ILLHANDLE) return 1;

   205

#else

   206

       if(Res & CAN_ERR_ILLHANDLE) return 1;

   207

#endif

   208

   209

#ifndef VERSION_2

   210

       if(Res != CAN_ERR_OK) 

   211

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

   212

#endif

   213

	if(Res==CAN_ERR_QRCVEMPTY) goto CONTINUE;

   214

#ifdef VERSION_2       

   215

     }

   216

     else

   217

     {

   218

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

   219

       //     Res = CAN_ERR_BUSLIGHT;

   220

       //else 

   221

              Res = CAN_ERR_UNKNOWN;

   222

     }

   223

#endif

   224

   225

    if(Res==CAN_ERR_BUSHEAVY)

   226

    {

   227

      if(HeavyCounter++>10) 

   228

      {

   229

	HeavyCounter = 0;

   230

	Res=CAN_ERR_BUSOFF;

   231

      }

   232

    }

   233

   234

    if(Res & CAN_ERR_BUSOFF)

   235

    {      

   236

      peakMsg.MSGTYPE = MSGTYPE_STATUS;

   237

      peakMsg.DATA[3] = CAN_ERR_BUSOFF;

   238

      Res = CAN_ERR_OK;

   239

    }   

   240

   241

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

   242

    if(Res == CAN_ERR_OK)

   243

    {

   244

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

   245

	switch(peakMsg.MSGTYPE)

   246

	{

   247

	  case MSGTYPE_STATUS:

   248

		  switch(peakMsg.DATA[3])

   249

	          {

   250

		    case CAN_ERR_BUSHEAVY: 

   251

				      break;

   252

	            case CAN_ERR_BUSOFF: 

   253

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

   254

				      canInit((s_BOARD*)fd0);

   255

				      usleep(2000);

   256

				      break;

   257

		  }

   258

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

   259

   260

	  case MSGTYPE_STANDARD:		/* bits of MSGTYPE_ */

   261

	  case MSGTYPE_EXTENDED:

   262

			  m->rtr = 0;

   263

			  break;

   264

   265

	  case MSGTYPE_RTR:			/* bits of MSGTYPE_ */

   266

			  m->rtr = 1;

   267

			  break;

   268

   269

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

   270

   271

	}

   272

   273

      m->cob_id = peakMsg.ID;

   274

   275

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

   276

                m->rtr = 0;

   277

      else

   278

                m->rtr = 1;

   279

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

   280

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

   281

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

   282

#if defined DEBUG_MSG_CONSOLE_ON

   283

      MSG("in : ");

   284

      print_message(m);

   285

#endif

   286

    }

   287

    else

   288

    {       // not benign error => fatal error

   289

      if(!(Res & CAN_ERR_QRCVEMPTY

   290

                    || Res & CAN_ERR_BUSLIGHT

   291

                    || Res & CAN_ERR_BUSHEAVY))

   292

      {

   293

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

   294

        return 1;

   295

      }

   296

    }

   297

  } while(Res != CAN_ERR_OK);

   298

   299

   300

     // populate message received to other drivers

   301

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

   302

  {

   303

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

   304

    {

   305

      PushMsgToQueue(&Q_DATA[i],m);

   306

    }   

   307

  }

   308

   309

  return 0;

   310

}

   311

   312

   313

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

   314

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

   315

{

   316

UNS8 data;

   317

TPCANMsg peakMsg;

   318

int i, j;

   319

int loc_errno;

   320

int MaxLoops = 100;

   321

   322

  i = -1;

   323

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

   324

  {

   325

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

   326

    {

   327

      PushMsgToQueue(&Q_DATA[j],m);

   328

      i = j;

   329

    }

   330

  }

   331

   332

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

   333

   334

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

   335

  if(m->rtr == 0)

   336

  {

   337

    if(peakMsg.ID > 0x7FF)

   338

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

   339

    else

   340

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

   341

  }

   342

  else	

   343

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

   344

   345

  peakMsg.LEN = m->len;

   346

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

   347

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

   348

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

   349

   350

  do 

   351

  {

   352

    errno = loc_errno = CAN_Write(&peakMsg);

   353

   354

    if(loc_errno)

   355

    {

   356

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

   357

      {

   358

#if defined DEBUG_MSG_CONSOLE_ON

   359

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

   360

#endif

   361

        canInit((s_BOARD*)fd0);

   362

        usleep(1000);

   363

      }

   364

      usleep(80);

   365

    }

   366

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

   367

  } while(loc_errno != CAN_ERR_OK);

   368

   369

#if defined DEBUG_MSG_CONSOLE_ON

   370

    MSG("out : ");

   371

    print_message(m);

   372

#endif

   373

    return 0;

   374

}

   375

   376

   377

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

   378

UNS8 LIBAPI canChangeBaudRate_driver(CAN_HANDLE fd, char* baud)

   379

{

   380

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

   381

    return 0;

   382

}

   383

   384

   385

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

   386

LIBPUBLIC CAN_HANDLE LIBAPI canOpen_driver(s_BOARD * board)

   387

{

   388

int ret;

   389

int i;

   390

   391

  if(!initialisedQ)

   392

  {

   393

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

   394

    initialisedQ = 1;

   395

  }

   396

   397

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

   398

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

   399

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

   400

  {

   401

    Q_DATA[i].board = board;    

   402

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

   403

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

   404

    {

   405

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

   406

    }

   407

   408

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

   409

   410

    ret = canInit(board);

   411

    if(ret)

   412

      return (CAN_HANDLE)board;

   413

  }

   414

   415

  return NULL;

   416

}

   417

   418

   419

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

   420

int LIBAPI canClose_driver(CAN_HANDLE fd0)

   421

{

   422

s_BOARD *x_board = NULL;

   423

int ActiveBoards = 0;

   424

int i;

   425

   426

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

   427

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

   428

  {

   429

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

   430

    {

   431

      x_board = Q_DATA[i].board;

   432

      Q_DATA[i].board = NULL;

   433

      CloseHandle(Q_DATA[i].hEventx);

   434

      Q_DATA[i].hEventx = NULL;

   435

    }

   436

    else

   437

      ActiveBoards++;

   438

  }

   439

   440

  if(ActiveBoards<=0)

   441

  {				// No can device is used.

   442

    CAN_SetRcvEvent(NULL);

   443

    CAN_Close();    

   444

    if(hEvent1) 

   445

    {

   446

      SetEvent(hEvent1);

   447

      CloseHandle(hEvent1);

   448

      hEvent1 = NULL;

   449

    }

   450

  }

   451

  else    

   452

    SetEvent(hEvent1);

   453

   454

  return 0;

   455

}

   456

   457

   458

   459

   460

   461