JaFojtik@695: /*
JaFojtik@695:   This file is part of CanFestival, a library implementing CanOpen Stack. 
JaFojtik@695: 
JaFojtik@695:   Copyright (C): Jaroslav Fojtik
JaFojtik@695: */
JaFojtik@695: 
JaFojtik@695: #if defined(WIN32) && !defined(__CYGWIN__)
JaFojtik@695: #define usleep(micro) Sleep(micro%1000 ? (micro/1000) + 1 : (micro/1000))
JaFojtik@695: #else
JaFojtik@695: #include <stdio.h>
JaFojtik@695: #include <string.h>
JaFojtik@695: #include <errno.h>
JaFojtik@695: #include <fcntl.h>
JaFojtik@695: #endif
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: #include "cancfg.h"
JaFojtik@695: #include "can_driver.h"
JaFojtik@695: #include "def.h"
JaFojtik@695: 
JaFojtik@695: UNS8 LIBAPI canSend_driver(CAN_HANDLE fd0, Message const *m);
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: #define VERSION_2
JaFojtik@695: 
JaFojtik@695: /* dummy implementation for older version. */
JaFojtik@695: #ifndef VERSION_2
JaFojtik@695: void CAN_SetRcvEvent(HANDLE hEventx)
JaFojtik@695: {
JaFojtik@695:   SetEvent(hEventx);
JaFojtik@695: }
JaFojtik@695: #endif
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: #define SLAVE_COUNT 10
JaFojtik@695: #define QueueSize 100
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: #ifndef extra_PCAN_init_params
JaFojtik@695:     #define extra_PCAN_init_params  /**/
JaFojtik@695: #else
JaFojtik@695:     long int print_getenv(const char* pcanparam)
JaFojtik@695:     {
JaFojtik@695:         char* param=NULL;
JaFojtik@695:         long int res=0;
JaFojtik@695: 
JaFojtik@695:         param = getenv(pcanparam);
JaFojtik@695:         if(param != NULL){
JaFojtik@695:             res = strtol(param,NULL,0);
JaFojtik@695:         }
JaFojtik@695:         else
JaFojtik@695:             printf("Environment variable %s not defined !\n", pcanparam);
JaFojtik@695:         printf("Found environment variable %s : %ld\n", pcanparam ,res);
JaFojtik@695:         return res;
JaFojtik@695:     }
JaFojtik@695:     #define extra_PCAN_init_params\
JaFojtik@695:         ,print_getenv("PCANHwType")\
JaFojtik@695:         ,print_getenv("PCANIO_Port")\
JaFojtik@695:         ,print_getenv("PCANInterupt")
JaFojtik@695: #endif
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: typedef struct 
JaFojtik@695: {
JaFojtik@695:   s_BOARD *board;
JaFojtik@695:   Message MQueue[QueueSize];
JaFojtik@695:   unsigned QStart, QEnd;
JaFojtik@695:   HANDLE hEventx;
JaFojtik@695: } QueueRecord;
JaFojtik@695: 
JaFojtik@695: int initialisedQ = 0;
JaFojtik@695: QueueRecord Q_DATA[10];
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: /** Store message into a queue. */
JaFojtik@695: static void PushMsgToQueue(QueueRecord *QR, Message *m)
JaFojtik@695: {
JaFojtik@695:   if(QR==NULL || m==NULL) return;
JaFojtik@695:   if(QR->board==NULL) return;							// No Board assigned yet
JaFojtik@695:   memcpy(&QR->MQueue[QR->QStart], m, sizeof(Message));
JaFojtik@695:   QR->QStart = (QR->QStart + 1) % QueueSize;
JaFojtik@695:   if(QR->QEnd==QR->QStart) QR->QEnd = (QR->QEnd+1) % QueueSize;
JaFojtik@695:   if(QR->hEventx) SetEvent(QR->hEventx);					// Signalise internal flag
JaFojtik@695: }
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: /** Get message from a queue. */
JaFojtik@695: static int PopMsgFromQueue(QueueRecord *QR, Message *m)
JaFojtik@695: {
JaFojtik@695:   if(QR==NULL || m==NULL) return 0;
JaFojtik@695:   if(QR->QEnd == QR->QStart) return 0;
JaFojtik@695: 
JaFojtik@695:   memcpy(m, &QR->MQueue[QR->QEnd], sizeof(Message));
JaFojtik@695:   QR->QEnd = (QR->QEnd+1) % QueueSize;
JaFojtik@695:   return 1;
JaFojtik@695: }
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: /** Create the Event for the first board */
JaFojtik@695: HANDLE hEvent1 = NULL;
JaFojtik@695: CRITICAL_SECTION InitLock;
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: // Define for rtr CAN message
JaFojtik@695: #define CAN_INIT_TYPE_ST_RTR MSGTYPE_STANDARD | MSGTYPE_RTR
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: /***************************************************************************/
JaFojtik@695: static int TranslateBaudeRate(char* optarg)
JaFojtik@695: {
JaFojtik@695:   if(!strcmp( optarg, "1M")) return CAN_BAUD_1M;
JaFojtik@695:   if(!strcmp( optarg, "500K")) return CAN_BAUD_500K;
JaFojtik@695:   if(!strcmp( optarg, "250K")) return CAN_BAUD_250K;
JaFojtik@695:   if(!strcmp( optarg, "125K")) return CAN_BAUD_125K;
JaFojtik@695:   if(!strcmp( optarg, "100K")) return CAN_BAUD_100K;
JaFojtik@695:   if(!strcmp( optarg, "50K")) return CAN_BAUD_50K;
JaFojtik@695:   if(!strcmp( optarg, "20K")) return CAN_BAUD_20K;
JaFojtik@695:   if(!strcmp( optarg, "10K")) return CAN_BAUD_10K;
JaFojtik@695:   if(!strcmp( optarg, "5K")) return CAN_BAUD_5K;
JaFojtik@695:   if(!strcmp( optarg, "none")) return 0;
JaFojtik@695:   return 0x0000;
JaFojtik@695: }
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: static UNS8 canInit(s_BOARD *board)
JaFojtik@695: {
JaFojtik@695: int baudrate;
JaFojtik@695: int ret = 0;
JaFojtik@695: 
JaFojtik@695:   if(hEvent1==NULL)
JaFojtik@695:   {	//Create the Event for the first board
JaFojtik@695:     hEvent1 = CreateEvent(NULL, // lpEventAttributes
JaFojtik@695:                         FALSE,  // bManualReset
JaFojtik@695:                         FALSE,  // bInitialState
JaFojtik@695:                         "");    // lpName
JaFojtik@695:     InitializeCriticalSection(&InitLock);
JaFojtik@695:   }
JaFojtik@695: 
JaFojtik@695:   EnterCriticalSection(&InitLock);  
JaFojtik@695: 
JaFojtik@695:   if(baudrate = TranslateBaudeRate(board->baudrate))
JaFojtik@695:   {
JaFojtik@695:     ret = CAN_Init(baudrate, CAN_INIT_TYPE_ST extra_PCAN_init_params);
JaFojtik@695:     if(ret != CAN_ERR_OK) 
JaFojtik@695:     {
JaFojtik@695:       LeaveCriticalSection(&InitLock);      
JaFojtik@695:       return 0;
JaFojtik@695:     }
JaFojtik@695:   }
JaFojtik@695:         
JaFojtik@695:   CAN_SetRcvEvent(hEvent1);  //Set Event Handler for CANReadExt
JaFojtik@695:   LeaveCriticalSection(&InitLock);    
JaFojtik@695:   return 1;
JaFojtik@695: }
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: /********* functions which permit to communicate with the board ****************/
JaFojtik@695: UNS8 LIBAPI canReceive_driver(CAN_HANDLE fd0, Message *m)
JaFojtik@695: {
JaFojtik@695: static int HeavyCounter = 0;
JaFojtik@695: int ret=0;
JaFojtik@695: UNS8 data;
JaFojtik@695: TPCANMsg peakMsg;
JaFojtik@695: DWORD Res;
JaFojtik@695: DWORD result;
JaFojtik@695: HANDLE hh[2];
JaFojtik@695: int i;
JaFojtik@695: 
JaFojtik@695: #ifdef CAN_READ_EX
JaFojtik@695:     TPCANTimestamp peakRcvTime;
JaFojtik@695: #endif      
JaFojtik@695: 
JaFojtik@695:   i = strtol(((s_BOARD *)fd0)->busname,NULL,0);
JaFojtik@695:   if(i>=SLAVE_COUNT || i<0) return 1;           // error
JaFojtik@695:   if(Q_DATA[i].board!=(s_BOARD *)fd0) return 1;      // error
JaFojtik@695:   
JaFojtik@695:   hh[0]=hEvent1; hh[1]=Q_DATA[i].hEventx;
JaFojtik@695:   
JaFojtik@695: 	// loop until valid message or fatal error
JaFojtik@695:   do
JaFojtik@695:   {
JaFojtik@695: CONTINUE:
JaFojtik@695:      if(PopMsgFromQueue(&Q_DATA[i],m)) return 0;	//message is waiting in the internal queue
JaFojtik@695: 
JaFojtik@695:         // We read the queue looking for messages.
JaFojtik@695: #ifdef VERSION_2
JaFojtik@695:      result = WaitForMultipleObjects(2,hh,FALSE,15);  
JaFojtik@695:      if(Q_DATA[i].board==NULL) return 1;		//exit hook, exit immediatelly when device is closed
JaFojtik@695: 
JaFojtik@695:      if(result == WAIT_OBJECT_0+1)
JaFojtik@695: 	 goto CONTINUE;	//look to a PopMsgFromQueue() (continue will check while(), goto skips it)
JaFojtik@695: 
JaFojtik@695:      if(result==WAIT_OBJECT_0 || result==WAIT_TIMEOUT)
JaFojtik@695:      {
JaFojtik@695: #endif     
JaFojtik@695: #ifdef CAN_READ_EX
JaFojtik@695:        Res = CAN_ReadEx(&peakMsg, &peakRcvTime);
JaFojtik@695: #else
JaFojtik@695:        Res = CAN_Read(&peakMsg);
JaFojtik@695: #endif
JaFojtik@695:                 // Exit receive thread when handle is no more valid
JaFojtik@695: #ifdef CAN_ERRMASK_ILLHANDLE
JaFojtik@695:        if(Res & CAN_ERRMASK_ILLHANDLE) return 1;
JaFojtik@695: #else
JaFojtik@695:        if(Res & CAN_ERR_ILLHANDLE) return 1;
JaFojtik@695: #endif
JaFojtik@695: 
JaFojtik@695: #ifndef VERSION_2
JaFojtik@695:        if(Res != CAN_ERR_OK) 
JaFojtik@695:              result = WaitForSingleObject(hEvent1, 1);   //pooling for pcan release<2
JaFojtik@695: #endif
JaFojtik@695: 	if(Res==CAN_ERR_QRCVEMPTY) goto CONTINUE;
JaFojtik@695: #ifdef VERSION_2       
JaFojtik@695:      }
JaFojtik@695:      else
JaFojtik@695:      {
JaFojtik@695:        //if(result==WAIT_TIMEOUT || result==(WAIT_OBJECT_0+1)) 
JaFojtik@695:        //     Res = CAN_ERR_BUSLIGHT;
JaFojtik@695:        //else 
JaFojtik@695:               Res = CAN_ERR_UNKNOWN;
JaFojtik@695:      }
JaFojtik@695: #endif
JaFojtik@695: 
JaFojtik@695:     if(Res==CAN_ERR_BUSHEAVY)
JaFojtik@695:     {
JaFojtik@695:       if(HeavyCounter++>10) 
JaFojtik@695:       {
JaFojtik@695: 	HeavyCounter = 0;
JaFojtik@695: 	Res=CAN_ERR_BUSOFF;
JaFojtik@695:       }
JaFojtik@695:     }
JaFojtik@695: 
JaFojtik@695:     if(Res & CAN_ERR_BUSOFF)
JaFojtik@695:     {      
JaFojtik@695:       peakMsg.MSGTYPE = MSGTYPE_STATUS;
JaFojtik@695:       peakMsg.DATA[3] = CAN_ERR_BUSOFF;
JaFojtik@695:       Res = CAN_ERR_OK;
JaFojtik@695:     }   
JaFojtik@695: 
JaFojtik@695:         // A message was received : we process the message(s)
JaFojtik@695:     if(Res == CAN_ERR_OK)
JaFojtik@695:     {
JaFojtik@695:             // if something different that 11bit or rtr... problem      
JaFojtik@695: 	switch(peakMsg.MSGTYPE)
JaFojtik@695: 	{
JaFojtik@695: 	  case MSGTYPE_STATUS:
JaFojtik@695: 		  switch(peakMsg.DATA[3])
JaFojtik@695: 	          {
JaFojtik@695: 		    case CAN_ERR_BUSHEAVY: 
JaFojtik@695: 				      break;
JaFojtik@695: 	            case CAN_ERR_BUSOFF: 
JaFojtik@695: 			              printf ("Peak board read BUSOFF: re-init!!!\n");
JaFojtik@695: 				      canInit((s_BOARD*)fd0);
JaFojtik@695: 				      usleep(2000);
JaFojtik@695: 				      break;
JaFojtik@695: 		  }
JaFojtik@695: 		  return peakMsg.DATA[3];	/* if something different that 11bit or rtr... problem */
JaFojtik@695: 
JaFojtik@695: 	  case MSGTYPE_STANDARD:		/* bits of MSGTYPE_ */
JaFojtik@695: 	  case MSGTYPE_EXTENDED:
JaFojtik@695: 			  m->rtr = 0;
JaFojtik@695: 			  break;
JaFojtik@695: 
JaFojtik@695: 	  case MSGTYPE_RTR:			/* bits of MSGTYPE_ */
JaFojtik@695: 			  m->rtr = 1;
JaFojtik@695: 			  break;
JaFojtik@695: 
JaFojtik@695: 	  default: return CAN_ERR_OVERRUN;	/* If status, return status if 29bit, return overrun. */
JaFojtik@695: 		    
JaFojtik@695: 	}
JaFojtik@695:    
JaFojtik@695:       m->cob_id = peakMsg.ID;
JaFojtik@695: 
JaFojtik@695:       if (peakMsg.MSGTYPE == CAN_INIT_TYPE_ST)  /* bits of MSGTYPE_ */
JaFojtik@695:                 m->rtr = 0;
JaFojtik@695:       else
JaFojtik@695:                 m->rtr = 1;
JaFojtik@695:       m->len = peakMsg.LEN; /* count of data bytes (0..8) */
JaFojtik@695:       for(data=0; data<peakMsg.LEN; data++)
JaFojtik@695:                 m->Data[data] = peakMsg.DATA[data]; /* data bytes, up to 8 */
JaFojtik@695: #if defined DEBUG_MSG_CONSOLE_ON
JaFojtik@695:       MSG("in : ");
JaFojtik@695:       print_message(m);
JaFojtik@695: #endif
JaFojtik@695:     }
JaFojtik@695:     else
JaFojtik@695:     {       // not benign error => fatal error
JaFojtik@695:       if(!(Res & CAN_ERR_QRCVEMPTY
JaFojtik@695:                     || Res & CAN_ERR_BUSLIGHT
JaFojtik@695:                     || Res & CAN_ERR_BUSHEAVY))
JaFojtik@695:       {
JaFojtik@695:         printf ("canReceive returned error (%d)\n", Res);
JaFojtik@695:         return 1;
JaFojtik@695:       }
JaFojtik@695:     }
JaFojtik@695:   } while(Res != CAN_ERR_OK);
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695:      // populate message received to other drivers
JaFojtik@695:   for(i=0; i<SLAVE_COUNT; i++)
JaFojtik@695:   {
JaFojtik@695:     if(Q_DATA[i].board != (s_BOARD *)fd0)	// do not populate to own queue
JaFojtik@695:     {
JaFojtik@695:       PushMsgToQueue(&Q_DATA[i],m);
JaFojtik@695:     }   
JaFojtik@695:   }
JaFojtik@695: 
JaFojtik@695:   return 0;
JaFojtik@695: }
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: /***************************************************************************/
>@758: 
>@758: /* return 0 - OK; 1 - failure */
JaFojtik@695: UNS8 LIBAPI canSend_driver(CAN_HANDLE fd0, Message const *m)
JaFojtik@695: {
JaFojtik@695: UNS8 data;
JaFojtik@695: TPCANMsg peakMsg;
JaFojtik@695: int i, j;
JaFojtik@695: int loc_errno;
JaFojtik@695: int MaxLoops = 100;
JaFojtik@695: 
JaFojtik@695:   i = -1;
JaFojtik@695:   for(j=0; j<SLAVE_COUNT; j++)
JaFojtik@695:   {
JaFojtik@695:     if(Q_DATA[j].board != (s_BOARD *)fd0)   // store this message forr all other drivers
JaFojtik@695:     {
JaFojtik@695:       PushMsgToQueue(&Q_DATA[j],m);
JaFojtik@695:       i = j;
JaFojtik@695:     }
JaFojtik@695:   }
JaFojtik@695: 
JaFojtik@695:   if(i<0) return 1;                         // no board found
JaFojtik@695: 
JaFojtik@695:   peakMsg.ID = m->cob_id;                   /* 11/29 bit code */
JaFojtik@695:   if(m->rtr == 0)
JaFojtik@695:   {
JaFojtik@695:     if(peakMsg.ID > 0x7FF)
JaFojtik@695: 	peakMsg.MSGTYPE = MSGTYPE_EXTENDED; /* bits of MSGTYPE_ */
JaFojtik@695:     else
JaFojtik@695: 	peakMsg.MSGTYPE = MSGTYPE_STANDARD; /* bits of MSGTYPE_ */
JaFojtik@695:   }
JaFojtik@695:   else	
JaFojtik@695:     peakMsg.MSGTYPE = MSGTYPE_RTR;	    /* bits of MSGTYPE_ */
JaFojtik@695:   
JaFojtik@695:   peakMsg.LEN = m->len;
JaFojtik@695:     /* count of data bytes (0..8) */
JaFojtik@695:   for(data = 0; data < m->len; data++)
JaFojtik@695:         peakMsg.DATA[data] = m->Data[data]; /* data bytes, up to 8 */
JaFojtik@695: 
JaFojtik@695:   do 
JaFojtik@695:   {
JaFojtik@695:     errno = loc_errno = CAN_Write(&peakMsg);
JaFojtik@695: 
JaFojtik@695:     if(loc_errno)
JaFojtik@695:     {
JaFojtik@695:       if(loc_errno==CAN_ERR_BUSOFF && (MaxLoops%20)==1)
JaFojtik@695:       {
JaFojtik@695: #if defined DEBUG_MSG_CONSOLE_ON
JaFojtik@695:         printf ("Peak board write: re-init!!!\n");
JaFojtik@695: #endif
JaFojtik@695:         canInit((s_BOARD*)fd0);
JaFojtik@695:         usleep(1000);
JaFojtik@695:       }
JaFojtik@695:       usleep(80);
JaFojtik@695:     }
JaFojtik@695:     if(MaxLoops-- == 0) break;			// limit max looping
JaFojtik@695:   } while(loc_errno != CAN_ERR_OK);
JaFojtik@695: 
JaFojtik@695: #if defined DEBUG_MSG_CONSOLE_ON
JaFojtik@695:     MSG("out : ");
JaFojtik@695:     print_message(m);
JaFojtik@695: #endif
>@758:     return 0;					// OK
JaFojtik@695: }
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: /***************************************************************************/
JaFojtik@695: UNS8 LIBAPI canChangeBaudRate_driver(CAN_HANDLE fd, char* baud)
JaFojtik@695: {
JaFojtik@695:     printf("canChangeBaudRate not yet supported by this driver\n");
JaFojtik@695:     return 0;
JaFojtik@695: }
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: /***************************************************************************/
>@758: CAN_HANDLE LIBAPI canOpen_driver(s_BOARD * board)
JaFojtik@695: {
JaFojtik@695: int ret;
JaFojtik@695: int i;
JaFojtik@695: 
JaFojtik@695:   if(!initialisedQ)
JaFojtik@695:   {
JaFojtik@695:     memset(Q_DATA,0,sizeof(Q_DATA));
JaFojtik@695:     initialisedQ = 1;
JaFojtik@695:   }
JaFojtik@695: 
JaFojtik@695:   i = strtol(board->busname,NULL,0);			// Get slot name
JaFojtik@695:   //printf ("Board Busname=%d.\n",strtol(board->busname, &pEnd,0));
JaFojtik@695:   if(i<SLAVE_COUNT && i>=0)
JaFojtik@695:   {
JaFojtik@695:     Q_DATA[i].board = board;    
JaFojtik@695:       //printf ("First Board selected\n");
JaFojtik@695:     if(Q_DATA[i].hEventx==NULL)				// Create local event
JaFojtik@695:     {
JaFojtik@695:       Q_DATA[i].hEventx = CreateEvent(NULL, FALSE, FALSE, "");
JaFojtik@695:     }
JaFojtik@695: 
JaFojtik@695:     if(hEvent1!=NULL) return (CAN_HANDLE)board;		// Create global event, if needed
JaFojtik@695: 
JaFojtik@695:     ret = canInit(board);
JaFojtik@695:     if(ret)
JaFojtik@695:       return (CAN_HANDLE)board;
JaFojtik@695:   }
JaFojtik@695: 
JaFojtik@695:   return NULL;
JaFojtik@695: }
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: /***************************************************************************/
JaFojtik@695: int LIBAPI canClose_driver(CAN_HANDLE fd0)
JaFojtik@695: {
JaFojtik@695: s_BOARD *x_board = NULL;
JaFojtik@695: int ActiveBoards = 0;
JaFojtik@695: int i;
JaFojtik@695: 
JaFojtik@695:   if((s_BOARD *)fd0==NULL) return 0;
JaFojtik@695:   for(i=0; i<SLAVE_COUNT; i++)
JaFojtik@695:   {
JaFojtik@695:     if(Q_DATA[i].board == (s_BOARD *)fd0)
JaFojtik@695:     {
JaFojtik@695:       x_board = Q_DATA[i].board;
JaFojtik@695:       Q_DATA[i].board = NULL;
JaFojtik@695:       CloseHandle(Q_DATA[i].hEventx);
JaFojtik@695:       Q_DATA[i].hEventx = NULL;
JaFojtik@695:     }
JaFojtik@695:     else
JaFojtik@695:       ActiveBoards++;
JaFojtik@695:   }
JaFojtik@695: 
JaFojtik@695:   if(ActiveBoards<=0)
JaFojtik@695:   {				// No can device is used.
JaFojtik@695:     CAN_SetRcvEvent(NULL);
JaFojtik@695:     CAN_Close();    
JaFojtik@695:     if(hEvent1) 
JaFojtik@695:     {
JaFojtik@695:       SetEvent(hEvent1);
JaFojtik@695:       CloseHandle(hEvent1);
JaFojtik@695:       hEvent1 = NULL;
JaFojtik@695:     }
JaFojtik@695:   }
JaFojtik@695:   else    
JaFojtik@695:     SetEvent(hEvent1);
JaFojtik@695: 
JaFojtik@695:   return 0;
JaFojtik@695: }
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: 
JaFojtik@695: