etisserant@0: /*
etisserant@0: This file is part of CanFestival, a library implementing CanOpen Stack. 
etisserant@0: 
etisserant@0: Copyright (C): Edouard TISSERANT and Francis DUPIN
etisserant@0: 
etisserant@0: See COPYING file for copyrights details.
etisserant@0: 
etisserant@0: This library is free software; you can redistribute it and/or
etisserant@0: modify it under the terms of the GNU Lesser General Public
etisserant@0: License as published by the Free Software Foundation; either
etisserant@0: version 2.1 of the License, or (at your option) any later version.
etisserant@0: 
etisserant@0: This library is distributed in the hope that it will be useful,
etisserant@0: but WITHOUT ANY WARRANTY; without even the implied warranty of
etisserant@0: MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
etisserant@0: Lesser General Public License for more details.
etisserant@0: 
etisserant@0: You should have received a copy of the GNU Lesser General Public
etisserant@0: License along with this library; if not, write to the Free Software
etisserant@0: Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
etisserant@0: */
nico@208: /*!
nico@208: ** @file   states.c
nico@208: ** @author Edouard TISSERANT and Francis DUPIN
nico@208: ** @date   Tue Jun  5 09:32:32 2007
nico@208: **
nico@208: ** @brief
nico@208: **
nico@208: **
nico@208: */
etisserant@0: 
etisserant@378: #include "data.h"
etisserant@370: #include "sysdep.h"
etisserant@370: 
nico@208: /** Prototypes for internals functions */
nico@208: /*!                                                                                                
nico@208: **                                                                                                 
nico@208: **                                                                                                 
nico@208: ** @param d                                                                                        
nico@208: ** @param newCommunicationState                                                                    
nico@208: **/     
frdupin@53: void switchCommunicationState(CO_Data* d, 
frdupin@53: 	s_state_communication *newCommunicationState);
frdupin@53: 	
nico@208: /*!                                                                                                
nico@208: **                                                                                                 
nico@208: **                                                                                                 
nico@208: ** @param d                                                                                        
nico@208: **                                                                                                 
nico@208: ** @return                                                                                         
nico@208: **/    
etisserant@0: e_nodeState getState(CO_Data* d)
etisserant@0: {
etisserant@0:   return d->nodeState;
etisserant@0: }
etisserant@0: 
nico@208: /*!                                                                                                
nico@208: **                                                                                                 
nico@208: **                                                                                                 
nico@208: ** @param d                                                                                        
nico@208: ** @param m                                                                                        
nico@208: **/  
etisserant@0: void canDispatch(CO_Data* d, Message *m)
etisserant@0: {
etisserant@370: 	UNS16 cob_id = UNS16_LE(m->cob_id);
etisserant@370: 	 switch(cob_id >> 7)
etisserant@0: 	{
luis@284: 		case SYNC:		/* can be a SYNC or a EMCY message */
etisserant@370: 			if(cob_id == 0x080)	/* SYNC */
luis@284: 			{
luis@284: 				if(d->CurrentCommunicationState.csSYNC)
luis@284: 					proceedSYNC(d);
luis@284: 			} else 		/* EMCY */
luis@284: 				if(d->CurrentCommunicationState.csEmergency)
luis@284: 					proceedEMCY(d,m);
etisserant@0: 			break;
nico@215: 		/* case TIME_STAMP: */
etisserant@0: 		case PDO1tx:
etisserant@0: 		case PDO1rx:
etisserant@0: 		case PDO2tx:
etisserant@0: 		case PDO2rx:
etisserant@0: 		case PDO3tx:
etisserant@0: 		case PDO3rx:
etisserant@0: 		case PDO4tx:
etisserant@0: 		case PDO4rx:
etisserant@0: 			if (d->CurrentCommunicationState.csPDO)
etisserant@0: 				proceedPDO(d,m);
etisserant@0: 			break;
etisserant@0: 		case SDOtx:
etisserant@0: 		case SDOrx:
etisserant@0: 			if (d->CurrentCommunicationState.csSDO)
etisserant@0: 				proceedSDO(d,m);
etisserant@0: 			break;
etisserant@0: 		case NODE_GUARD:
mwildbolz@750: 			if (d->CurrentCommunicationState.csLifeGuard)
etisserant@0: 				proceedNODE_GUARD(d,m);
etisserant@0: 			break;
etisserant@0: 		case NMT:
frdupin@88: 			if (*(d->iam_a_slave))
etisserant@0: 			{
etisserant@0: 				proceedNMTstateChange(d,m);
etisserant@0: 			}
Mongo@668:             break;
etisserant@343: #ifdef CO_ENABLE_LSS
etisserant@343: 		case LSS:
etisserant@343: 			if (!d->CurrentCommunicationState.csLSS)break;
etisserant@370: 			if ((*(d->iam_a_slave)) && cob_id==MLSS_ADRESS)
etisserant@343: 			{
etisserant@343: 				proceedLSS_Slave(d,m);
etisserant@343: 			}
etisserant@370: 			else if(!(*(d->iam_a_slave)) && cob_id==SLSS_ADRESS)
etisserant@343: 			{
etisserant@343: 				proceedLSS_Master(d,m);
etisserant@343: 			}
etisserant@343: 			break;
etisserant@343: #endif
etisserant@0: 	}
etisserant@0: }
etisserant@0: 
etisserant@0: #define StartOrStop(CommType, FuncStart, FuncStop) \
etisserant@291: 	if(newCommunicationState->CommType && d->CurrentCommunicationState.CommType == 0){\
etisserant@195: 		MSG_WAR(0x9999,#FuncStart, 9999);\
etisserant@0: 		d->CurrentCommunicationState.CommType = 1;\
etisserant@0: 		FuncStart;\
etisserant@291: 	}else if(!newCommunicationState->CommType && d->CurrentCommunicationState.CommType == 1){\
etisserant@195: 		MSG_WAR(0x9999,#FuncStop, 9999);\
etisserant@0: 		d->CurrentCommunicationState.CommType = 0;\
etisserant@0: 		FuncStop;\
etisserant@0: 	}
etisserant@0: #define None
nico@208: 
nico@208: /*!                                                                                                
nico@208: **                                                                                                 
nico@208: **                                                                                                 
nico@208: ** @param d                                                                                        
nico@208: ** @param newCommunicationState                                                                    
nico@208: **/  	
etisserant@0: void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState)
etisserant@0: {
groke6@381: #ifdef CO_ENABLE_LSS
groke6@381: 	StartOrStop(csLSS,	startLSS(d),	stopLSS(d))
groke6@381: #endif
etisserant@0: 	StartOrStop(csSDO,	None,		resetSDO(d))
etisserant@0: 	StartOrStop(csSYNC,	startSYNC(d),		stopSYNC(d))
mwildbolz@750: 	StartOrStop(csLifeGuard,	lifeGuardInit(d),	lifeGuardStop(d))
luis@284: 	StartOrStop(csEmergency,	emergencyInit(d),	emergencyStop(d)) 
etisserant@235: 	StartOrStop(csPDO,	PDOInit(d),	PDOStop(d))
dejoigny@14: 	StartOrStop(csBoot_Up,	None,	slaveSendBootUp(d))
etisserant@0: }
etisserant@0: 
nico@208: /*!                                                                                                
nico@208: **                                                                                                 
nico@208: **                                                                                                 
nico@208: ** @param d                                                                                        
nico@208: ** @param newState                                                                                 
nico@208: **                                                                                                 
nico@208: ** @return                                                                                         
nico@208: **/  
etisserant@0: UNS8 setState(CO_Data* d, e_nodeState newState)
etisserant@0: {
etisserant@378: 	if(newState != d->nodeState){
etisserant@0: 		switch( newState ){
etisserant@0: 			case Initialisation:
etisserant@0: 			{
etisserant@343: 				s_state_communication newCommunicationState = {1, 0, 0, 0, 0, 0, 0};
etisserant@0: 				d->nodeState = Initialisation;
etisserant@378: 				switchCommunicationState(d, &newCommunicationState);
etisserant@378: 				/* call user app init callback now. */
etisserant@378: 				/* d->initialisation MUST NOT CALL SetState */
etisserant@378: 				(*d->initialisation)(d);				
etisserant@378: 			}
etisserant@378: 
etisserant@378: 			/* Automatic transition - No break statement ! */
etisserant@378: 			/* Transition from Initialisation to Pre_operational */
etisserant@378: 			/* is automatic as defined in DS301. */
etisserant@378: 			/* App don't have to call SetState(d, Pre_operational) */
etisserant@0: 								
etisserant@0: 			case Pre_operational:
etisserant@0: 			{
greg@178: 				
etisserant@343: 				s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 0, 1};
etisserant@0: 				d->nodeState = Pre_operational;
etisserant@0: 				switchCommunicationState(d, &newCommunicationState);
etisserant@378:                 (*d->preOperational)(d);
etisserant@0: 			}
etisserant@0: 			break;
etisserant@0: 								
etisserant@0: 			case Operational:
etisserant@0: 			if(d->nodeState == Initialisation) return 0xFF;
etisserant@0: 			{
etisserant@343: 				s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 1, 0};
etisserant@0: 				d->nodeState = Operational;
etisserant@0: 				newState = Operational;
etisserant@0: 				switchCommunicationState(d, &newCommunicationState);
etisserant@378: 				(*d->operational)(d);
etisserant@0: 			}
etisserant@0: 			break;
etisserant@0: 						
etisserant@0: 			case Stopped:
etisserant@0: 			if(d->nodeState == Initialisation) return 0xFF;
etisserant@0: 			{
etisserant@343: 				s_state_communication newCommunicationState = {0, 0, 0, 0, 1, 0, 1};
etisserant@0: 				d->nodeState = Stopped;
etisserant@0: 				newState = Stopped;
etisserant@0: 				switchCommunicationState(d, &newCommunicationState);
etisserant@378: 				(*d->stopped)(d);
etisserant@0: 			}
etisserant@0: 			break;
groke6@381: 			default:
groke6@381: 				return 0xFF;
groke6@381: 
nico@215: 		}/* end switch case */
etisserant@94: 	
etisserant@0: 	}
etisserant@378: 	/* d->nodeState contains the final state */
etisserant@378: 	/* may not be the requested state */
etisserant@378:     return d->nodeState;  
etisserant@0: }
etisserant@0: 
nico@208: /*!                                                                                                
nico@208: **                                                                                                 
nico@208: **                                                                                                 
nico@208: ** @param d                                                                                        
nico@208: **                                                                                                 
nico@208: ** @return                                                                                         
nico@208: **/ 
etisserant@0: UNS8 getNodeId(CO_Data* d)
etisserant@0: {
etisserant@0:   return *d->bDeviceNodeId;
etisserant@0: }
etisserant@0: 
nico@208: /*!                                                                                                
nico@208: **                                                                                                 
nico@208: **                                                                                                 
nico@208: ** @param d                                                                                        
nico@208: ** @param nodeId                                                                                   
nico@208: **/   
etisserant@0: void setNodeId(CO_Data* d, UNS8 nodeId)
etisserant@0: {
etisserant@0:   UNS16 offset = d->firstIndex->SDO_SVR;
etisserant@343:   
etisserant@343: #ifdef CO_ENABLE_LSS
groke6@381:   d->lss_transfer.nodeID=nodeId;
groke6@517:   if(nodeId==0xFF){
etisserant@343:   	*d->bDeviceNodeId = nodeId;
etisserant@343:   	return;
etisserant@343:   }
groke6@517:   else
etisserant@343: #endif
groke6@517:   if(!(nodeId>0 && nodeId<=127)){
groke6@517: 	  MSG_WAR(0x2D01, "Invalid NodeID",nodeId);
groke6@517: 	  return;
groke6@517:   }
etisserant@343: 
etisserant@0:   if(offset){
groke6@349:     /* Adjust COB-ID Client->Server (rx) only id already set to default value or id not valid (id==0xFF)*/
groke6@349:     if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == 0x600 + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)){
nico@215:       /* cob_id_client = 0x600 + nodeId; */
etisserant@447:       *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId;
etisserant@303:     }
groke6@349:     /* Adjust COB-ID Server -> Client (tx) only id already set to default value or id not valid (id==0xFF)*/
groke6@349:     if((*(UNS32*)d->objdict[offset].pSubindex[2].pObject == 0x580 + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)){
nico@215:       /* cob_id_server = 0x580 + nodeId; */
etisserant@447:       *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId;
etisserant@303:     }
etisserant@0:   }
etisserant@0: 
nico@215:   /* 
nico@208:    	Initialize the server(s) SDO parameters
nico@208:   	Remember that only one SDO server is allowed, defined at index 0x1200	
nico@208:  		
nico@208:   	Initialize the client(s) SDO parameters 	
nico@208:   	Nothing to initialize (no default values required by the DS 401)	
nico@208:   	Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403 
nico@208:   */
etisserant@0:   {
etisserant@0:     UNS8 i = 0;
etisserant@0:     UNS16 offset = d->firstIndex->PDO_RCV;
etisserant@0:     UNS16 lastIndex = d->lastIndex->PDO_RCV;
etisserant@0:     UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500};
etisserant@0:     if( offset ) while( (offset <= lastIndex) && (i < 4)) {
groke6@349:       if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF))
etisserant@0: 	      *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
etisserant@0:       i ++;
etisserant@0:       offset ++;
etisserant@0:     }
etisserant@0:   }
frdupin@71:   /* ** Initialize the transmit PDO communication parameters. Only for 0x1800 to 0x1803 */
etisserant@0:   {
etisserant@0:     UNS8 i = 0;
etisserant@0:     UNS16 offset = d->firstIndex->PDO_TRS;
etisserant@0:     UNS16 lastIndex = d->lastIndex->PDO_TRS;
etisserant@0:     UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480};
etisserant@0:     i = 0;
etisserant@0:     if( offset ) while ((offset <= lastIndex) && (i < 4)) {
groke6@349:       if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF))
etisserant@0: 	      *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
etisserant@0:       i ++;
etisserant@0:       offset ++;
etisserant@0:     }
etisserant@0:   }
etisserant@314: 
etisserant@314:   /* Update EMCY COB-ID if already set to default*/
groke6@349:   if((*d->error_cobid == *d->bDeviceNodeId + 0x80)||(*d->bDeviceNodeId==0xFF))
etisserant@314:     *d->error_cobid = nodeId + 0x80;
etisserant@314: 
nico@215:   /* bDeviceNodeId is defined in the object dictionary. */
etisserant@0:   *d->bDeviceNodeId = nodeId;
etisserant@0: }
etisserant@149: 
etisserant@378: void _initialisation(CO_Data* d){}
Edouard@746: void _preOperational(CO_Data* d){
Edouard@746:     if (!(*(d->iam_a_slave)))
Edouard@746:     {
Edouard@746:         masterSendNMTstateChange (d, 0, NMT_Reset_Node);
Edouard@746:     }
Edouard@746: }
etisserant@378: void _operational(CO_Data* d){}
etisserant@378: void _stopped(CO_Data* d){}