etisserant@38: /*
etisserant@145: This file is part of CanFestival, a library implementing CanOpen Stack. 
etisserant@145: 
etisserant@145: Copyright (C): Edouard TISSERANT and Francis DUPIN
etisserant@145: 
etisserant@145: See COPYING file for copyrights details.
etisserant@145: 
etisserant@145: This library is free software; you can redistribute it and/or
etisserant@145: modify it under the terms of the GNU Lesser General Public
etisserant@145: License as published by the Free Software Foundation; either
etisserant@145: version 2.1 of the License, or (at your option) any later version.
etisserant@145: 
etisserant@145: This library is distributed in the hope that it will be useful,
etisserant@145: but WITHOUT ANY WARRANTY; without even the implied warranty of
etisserant@145: MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
etisserant@145: Lesser General Public License for more details.
etisserant@145: 
etisserant@145: You should have received a copy of the GNU Lesser General Public
etisserant@145: License along with this library; if not, write to the Free Software
etisserant@145: Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
etisserant@38: */
etisserant@38: 
etisserant@145: #if defined(WIN32) && !defined(__CYGWIN__)
etisserant@145: #define usleep(micro) Sleep(micro%1000 ? (micro/1000) + 1 : (micro/1000))
etisserant@145: #else
etisserant@145: #include <stdio.h>
etisserant@145: #include <string.h>
etisserant@145: #include <errno.h>
etisserant@145: #include <fcntl.h>
etisserant@145: #endif
etisserant@145: 
etisserant@145: #include "cancfg.h"
etisserant@145: #include "can_driver.h"
greg@267: #include "def.h"
etisserant@41: #ifndef extra_PCAN_init_params
etisserant@145: 	#define extra_PCAN_init_params /**/
etisserant@145: #else
etisserant@145: 	#define extra_PCAN_init_params\
etisserant@145: 		,getenv("PCANHwType") ? strtol(getenv("PCANHwType"),NULL,0):0\
etisserant@145: 		,getenv("PCANIO_Port") ? strtol(getenv("PCANIO_Port"),NULL,0):0\
etisserant@145: 		,getenv("PCANInterupt") ? strtol(getenv("PCANInterupt"),NULL,0):0
etisserant@145: #endif
etisserant@145: 
etisserant@145: static s_BOARD *first_board = NULL;
etisserant@145: #ifdef PCAN2_HEADER_
etisserant@145: static s_BOARD *second_board = NULL;
etisserant@145: #endif
etisserant@145: 
etisserant@145: //pthread_mutex_t PeakCan_mutex = PTHREAD_MUTEX_INITIALIZER;
etisserant@145: 
etisserant@145: // Define for rtr CAN message
etisserant@145: #define CAN_INIT_TYPE_ST_RTR MSGTYPE_STANDARD | MSGTYPE_RTR
etisserant@145: 
etisserant@145: /***************************************************************************/
etisserant@145: int TranslateBaudeRate(char* optarg){
etisserant@145: 	if(!strcmp( optarg, "1M")) return CAN_BAUD_1M;
etisserant@145: 	if(!strcmp( optarg, "500K")) return CAN_BAUD_500K;
etisserant@145: 	if(!strcmp( optarg, "250K")) return CAN_BAUD_250K;
etisserant@145: 	if(!strcmp( optarg, "125K")) return CAN_BAUD_125K;
etisserant@145: 	if(!strcmp( optarg, "100K")) return CAN_BAUD_100K;
etisserant@145: 	if(!strcmp( optarg, "50K")) return CAN_BAUD_50K;
etisserant@145: 	if(!strcmp( optarg, "20K")) return CAN_BAUD_20K;
etisserant@145: 	if(!strcmp( optarg, "10K")) return CAN_BAUD_10K;
etisserant@145: 	if(!strcmp( optarg, "5K")) return CAN_BAUD_5K;
etisserant@145: 	if(!strcmp( optarg, "none")) return 0;
etisserant@145: 	return 0x0000;
etisserant@145: }
etisserant@145: 
greg@267: #define MyCase(fc) case fc: printf(#fc);break;
greg@267: void print_message(Message *m)
greg@267: {
greg@267: 	int i;
greg@267: 	switch(m->cob_id.w >> 7)
greg@267: 	{
greg@267: 		MyCase(SYNC)
greg@267: 		MyCase(TIME_STAMP)
greg@267: 		MyCase(PDO1tx)
greg@267: 		MyCase(PDO1rx)
greg@267: 		MyCase(PDO2tx)
greg@267: 		MyCase(PDO2rx)
greg@267: 		MyCase(PDO3tx)
greg@267: 		MyCase(PDO3rx)
greg@267: 		MyCase(PDO4tx)
greg@267: 		MyCase(PDO4rx)
greg@267: 		MyCase(SDOtx)
greg@267: 		MyCase(SDOrx)
greg@267: 		MyCase(NODE_GUARD)
greg@267: 		MyCase(NMT)
greg@267: 	}
greg@267: 	printf(" rtr:%d", m->rtr);
greg@267: 	printf(" len:%d", m->len);
greg@267: 	for (i = 0 ; i < m->len ; i++)
greg@267: 		printf(" %02x", m->data[i]);
greg@267: 	printf("\n");
greg@267: }
greg@267: 
etisserant@145: void
etisserant@145: canInit (s_BOARD *board)
etisserant@145: {
etisserant@145: 	int baudrate;
etisserant@145: 	
greg@267: //#ifdef PCAN2_HEADER_
etisserant@145: 	// if not the first handler
etisserant@145: 	if(second_board == (s_BOARD *)board)
etisserant@145: 		if(baudrate = TranslateBaudeRate(board->baudrate))
etisserant@145: 			CAN2_Init (baudrate,
etisserant@145: 			  CAN_INIT_TYPE_ST extra_PCAN_init_params);
greg@267: //#endif
etisserant@145: 	if(first_board == (s_BOARD *)board)
etisserant@145: 		if(baudrate = TranslateBaudeRate(board->baudrate))
etisserant@145: 			CAN_Init (baudrate,
etisserant@145: 			  CAN_INIT_TYPE_ST extra_PCAN_init_params);
etisserant@145: }
etisserant@145: 
etisserant@145: /*********functions which permit to communicate with the board****************/
etisserant@145: UNS8
etisserant@145: canReceive_driver (CAN_HANDLE fd0, Message * m)
etisserant@145: {
etisserant@145: 	UNS8 data;
etisserant@145: 	TPCANMsg peakMsg;
etisserant@145: 	DWORD Res;
etisserant@145: 	do{
etisserant@145: 		// We read the queue looking for messages.
etisserant@145: 		// 
etisserant@145: 		//pthread_mutex_lock (&PeakCan_mutex);
etisserant@145: #ifdef PCAN2_HEADER_
etisserant@145: 		// if not the first handler
etisserant@145: 		if(second_board == (s_BOARD *)fd0)
etisserant@145: 			Res = CAN2_Read (&peakMsg);
etisserant@145: 		else
etisserant@145: #endif
etisserant@145: 		if(first_board == (s_BOARD *)fd0)
etisserant@145: 			Res = CAN_Read (&peakMsg);
etisserant@145: 		else
etisserant@145: 			Res = CAN_ERR_BUSOFF;
etisserant@145: 		// A message was received
etisserant@145: 		// We process the message(s)
etisserant@145: 		// 
etisserant@145: 		if (Res == CAN_ERR_OK)
etisserant@145: 		{
etisserant@145: 			// if something different that 11bit or rtr... problem
etisserant@145: 			if (peakMsg.MSGTYPE & ~(MSGTYPE_STANDARD | MSGTYPE_RTR))
etisserant@145: 			{
etisserant@145: 				if (peakMsg.MSGTYPE == CAN_ERR_BUSOFF)
etisserant@145: 				{
etisserant@145: 					printf ("!!! Peak board read : re-init\n");
etisserant@145: 					canInit((s_BOARD*) fd0);
etisserant@145: 					usleep (10000);
etisserant@145: 				}
etisserant@145: 	
etisserant@145: 				// If status, return status if 29bit, return overrun
etisserant@145: 				//pthread_mutex_unlock (&PeakCan_mutex);
etisserant@145: 				return peakMsg.MSGTYPE ==
etisserant@145: 					MSGTYPE_STATUS ? peakMsg.DATA[2] : CAN_ERR_OVERRUN;
etisserant@145: 			}
etisserant@145: 			m->cob_id.w = peakMsg.ID;
greg@267: 			
etisserant@145: 			if (peakMsg.MSGTYPE == CAN_INIT_TYPE_ST)	/* bits of MSGTYPE_ */
etisserant@145: 				m->rtr = 0;
etisserant@145: 			else
etisserant@145: 				m->rtr = 1;
etisserant@145: 			m->len = peakMsg.LEN;	/* count of data bytes (0..8) */
etisserant@145: 			for (data = 0; data < peakMsg.LEN; data++)
etisserant@145: 				m->data[data] = peakMsg.DATA[data];	/* data bytes, up to 8 */
greg@267: 		
etisserant@145: 		}else{
etisserant@145: 		//pthread_mutex_unlock (&PeakCan_mutex);
etisserant@145: 		//if (Res != CAN_ERR_OK)
etisserant@145: 		//{
etisserant@145: 			if (!
etisserant@145: 				(Res & CAN_ERR_QRCVEMPTY || Res & CAN_ERR_BUSLIGHT
etisserant@145: 				 || Res & CAN_ERR_BUSHEAVY))
etisserant@145: 			{
etisserant@145: 				printf ("canReceive returned error (%d)\n", Res);
etisserant@145: 				return 1;
etisserant@145: 			}
etisserant@145: 			usleep (1000);		
etisserant@145: 		}
etisserant@145: 	}while(Res != CAN_ERR_OK);
etisserant@145: 	return 0;
etisserant@145: }
etisserant@145: 
etisserant@145: /***************************************************************************/
etisserant@145: UNS8
etisserant@145: canSend_driver (CAN_HANDLE fd0, Message * m)
etisserant@145: {
etisserant@145: 	UNS8 data;
etisserant@145: 	TPCANMsg peakMsg;
etisserant@145: 	peakMsg.ID = m->cob_id.w;	/* 11/29 bit code */
etisserant@145: 	if (m->rtr == 0)
etisserant@145: 		peakMsg.MSGTYPE = CAN_INIT_TYPE_ST;	/* bits of MSGTYPE_ */
etisserant@145: 	else
etisserant@145: 	{
etisserant@145: 		peakMsg.MSGTYPE = CAN_INIT_TYPE_ST_RTR;	/* bits of MSGTYPE_ */
etisserant@145: 	}
etisserant@145: 	peakMsg.LEN = m->len;
etisserant@145: 	/* count of data bytes (0..8) */
etisserant@145: 	for (data = 0; data < m->len; data++)
etisserant@145: 		peakMsg.DATA[data] = m->data[data];	/* data bytes, up to 8 */
greg@267: 	
etisserant@145: 	do
etisserant@145: 	{
etisserant@145: #ifdef PCAN2_HEADER_
etisserant@145: 		// if not the first handler
etisserant@145: 		if(second_board == (s_BOARD *)fd0)
greg@267: 		{
etisserant@145: 			errno = CAN2_Write (&peakMsg);
greg@267: 		}
etisserant@145: 		else 
etisserant@145: #endif
etisserant@145: 		if(first_board == (s_BOARD *)fd0)
greg@267: 			{
greg@267: 				errno = CAN_Write (&peakMsg);
greg@267: 			}
etisserant@145: 		else 
etisserant@145: 			goto fail;
etisserant@145: 		if (errno)
etisserant@145: 		{
etisserant@145: 			if (errno == CAN_ERR_BUSOFF)
etisserant@145: 			{
etisserant@145: 				printf ("!!! Peak board write : re-init\n");
etisserant@145: 				canInit((s_BOARD*)fd0);
etisserant@145: 				usleep (10000);
etisserant@145: 			}
etisserant@145: 			usleep (1000);
etisserant@145: 		}
etisserant@145: 	}
etisserant@145: 	while (errno != CAN_ERR_OK);
etisserant@145: 	return 0;
etisserant@145: fail:
etisserant@145: 	return 1;
etisserant@145: }
etisserant@145: 
etisserant@145: /***************************************************************************/
etisserant@145: CAN_HANDLE
etisserant@145: canOpen_driver (s_BOARD * board)
etisserant@145: {
greg@267:   char busname[64];
greg@267:   char* pEnd;
greg@267: 
greg@267:   //printf ("Board Busname=%d.\n",strtol(board->busname, &pEnd,0));
greg@267:   if (strtol(board->busname, &pEnd,0) == 0)
greg@267:   {
greg@267:       first_board = board;
greg@267:       printf ("First Board selected\n");
greg@267:   }
greg@267:   if (strtol(board->busname, &pEnd,0) == 1)
greg@267:   {
greg@267:      second_board = board;
greg@267:      printf ("Second Board selected\n");
greg@267:   }
etisserant@145: 	canInit(board);
etisserant@145: 	
etisserant@145: 	return (CAN_HANDLE)board;
etisserant@145: }
etisserant@145: 
etisserant@145: /***************************************************************************/
etisserant@145: int
etisserant@145: canClose_driver (CAN_HANDLE fd0)
etisserant@145: {
etisserant@145: #ifdef PCAN2_HEADER_
etisserant@145: 	// if not the first handler
etisserant@145: 	if(second_board == (s_BOARD *)fd0)
etisserant@145: 	{
etisserant@145: 		CAN2_Close ();
etisserant@145: 		second_board = (s_BOARD *)NULL;
etisserant@145: 	}else	
etisserant@145: #endif
etisserant@145: 	if(first_board == (s_BOARD *)fd0)
etisserant@145: 	{
etisserant@145: 		CAN_Close ();
etisserant@145: 		first_board = (s_BOARD *)NULL;
etisserant@145: 	}
etisserant@145: 	return 0;
etisserant@145: }