src/lss.c
author lbessard
Fri, 04 Jan 2008 17:50:06 +0100
changeset 348 c50feb2bb590
parent 343 118c1cabd0b0
child 361 2220813498f2
permissions -rw-r--r--
Bug when close objdictedit fixed
/*
This file is part of CanFestival, a library implementing CanOpen Stack. 


Copyright (C): Jorge Berzosa


See COPYING file for copyrights details.


This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.


This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
Lesser General Public License for more details.


You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/


/*!
** @file   lss.c
** @author Jorge Berzosa
** @date   Mon Oct  22 05:44:32 2007
**
** @brief
**
**
*/

#ifdef CO_ENABLE_LSS

#include "data.h"
#include "lss.h"
#include "canfestival.h"

#define LSS_TIMEOUT_MS	(TIMEVAL)1000  /* ms */

/* Returns the LSS ident field from a Message struct */
#define getLSSIdent(msg) ((msg->data[4] << 24) | (msg->data[3] << 16) | (msg->data[2] << 8) | (msg->data[1]))

/* Returns the LSS switch delay field from a Message struct */
#define getLSSDelay(msg) ((msg->data[2] << 8) | (msg->data[1]))

/* Returns the LSS FastScan BitCheck field from a Message struct */
#define getLSSBitCheck(msg) msg->data[5]

/* Returns the LSS FastScan LSSSub field from a Message struct */
#define getLSSSub(msg) msg->data[6]

/* Returns the LSS FastScan LSSNext field from a Message struct */
#define getLSSNext(msg) msg->data[7]

/* Prototypes for internals functions */
void LssAlarm(CO_Data* d, UNS32 id);

#define StopLSS_TIMER(id){\
 MSG_WAR(0x3D01, "StopLSS_TIMER for timer : ", id);\
 d->lss_transfer.timers[id] = DelAlarm(d->lss_transfer.timers[id]);}

#define StartLSS_MSG_TIMER(){\
 MSG_WAR(0x3D02, "StartLSS_TIMER for MSG_TIMER",0);\
 d->lss_transfer.timers[LSS_MSG_TIMER] = SetAlarm(d,LSS_MSG_TIMER,&LssAlarm,MS_TO_TIMEVAL(LSS_TIMEOUT_MS),0);}

#define StartLSS_SDELAY_TIMER(){\
 MSG_WAR(0x3D03, "StartLSS_TIMER for SDELAY_TIMER",0);\
 d->lss_transfer.timers[LSS_SWITCH_DELAY_TIMER] = SetAlarm(d,LSS_SWITCH_DELAY_TIMER,&LssAlarm,MS_TO_TIMEVAL(d->lss_transfer.switchDelay),MS_TO_TIMEVAL(d->lss_transfer.switchDelay));}


/*!                                                                                                
**                                                                                                 
**                                                                                                 
** @param d                                                                                        
** @param id                                                                                       
**/   
//struct timeval current_time,init_time;
void LssAlarm(CO_Data* d, UNS32 id)
{	
	/*unsigned long time_period;
	
	gettimeofday(&current_time,NULL);
 	time_period=(current_time.tv_sec - init_time.tv_sec)* 1000000 + current_time.tv_usec - init_time.tv_usec;
 	printf("%3ld.%3ld.%3ld --",time_period/1000000,(time_period%1000000)/1000,time_period%1000);*/
 
	switch(id){
	case LSS_MSG_TIMER:
	 	MSG_ERR(0x1D04, "LSS timeout. LSS response not received.", 0);
    	MSG_WAR(0x2D05, "LSS timeout command specifier : ", d->lss_transfer.command);
    
		StopLSS_TIMER(LSS_MSG_TIMER);
    	/* Set aborted state */
    	d->lss_transfer.state = LSS_ABORTED_INTERNAL;
    	/* Call the user function to inform of the problem.*/
    	if(d->lss_transfer.Callback){
    		/*If there is a callback, it is responsible of the error*/
    		(*d->lss_transfer.Callback)(d,d->lss_transfer.command);
    	}
    break;
    case LSS_SWITCH_DELAY_TIMER:
  		/* The first switch_delay period has expired. Store the node state, change it 
 		 * so no CAN messages will be sent or received, call the ChangeBaudRate function*/
   		if(d->lss_transfer.switchDelayState==SDELAY_FIRST){
   			MSG_WAR(0x3D06, "LSS switch delay first period expired",0);
    		d->lss_transfer.switchDelayState=SDELAY_SECOND;
    		//d->lss_transfer.currentState=getState(d);
    		//setState(d, LssTimingDelay);
    		(*d->lss_ChangeBaudRate)(d->lss_transfer.baudRate);
    	}
    	else{ /* d->lss_transfer.switchDelayState==SDELAY_SECOND */
    		MSG_WAR(0x3D07, "LSS switch delay second period expired",0);
    		d->lss_transfer.switchDelayState=SDELAY_OFF;
    		StopLSS_TIMER(LSS_SWITCH_DELAY_TIMER);
    		
    		setState(d, d->lss_transfer.currentState);
    	}
     break;
	}
}

/*!                                                                                                
**                                                                                                 
**                                                                                                 
** @param d                                                                                        
**/ 
void startLSS(CO_Data* d)
{
	/*MSG_WAR(0x3D09, "LSS services started",0);*/
}

/*!                                                                                                
**                                                                                                 
**                                                                                                 
** @param d                                                                                        
**/   
void stopLSS(CO_Data* d)
{
	/*MSG_WAR(0x3D09, "LSS services stopped",0);*/
}

/*!                                                                                                
**                                                                                                 
**                                                                                                 
** @param d                                                                                        
** @param cob_id                                                                                   
**                                                                                                 
** @return                                                                                         
**/  
UNS8 sendSlaveLSSMessage(CO_Data* d, UNS8 command,void *dat1,void *dat2)
{
  Message m;
  UNS8 i;
  
  if (!d->CurrentCommunicationState.csLSS){
  	MSG_WAR(0x2D0A, "unable to send the LSS message (not in stop or pre-op mode", d->nodeState);
  	return 0;
  }
   
  for(i=1;i<8;i++)m.data[i]=0;
  m.len = 8;
  m.rtr = NOT_A_REQUEST;
  m.data[0]=command;
  m.cob_id.w=SLSS_ADRESS;
  
  /* Tha data sent with the msg depends on the command */
  switch(command){
  case LSS_INQ_NODE_ID: /* Inquire Node-ID */
  	m.data[1]=*(UNS8 *)dat1;
  	break;
  case LSS_CONF_NODE_ID: /* Configure Node-ID */
  case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */
  case LSS_CONF_STORE: /* Store Configured Parameters */
  	m.data[1]=*(UNS8 *)dat1;
  	m.data[2]=*(UNS8 *)dat2;
  	break; 
  case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */
  case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */
  case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */
  case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */
	m.data[1]=(UNS8)(*(UNS32*)dat1 & 0xFF);
	m.data[2]=(UNS8)(*(UNS32*)dat1>>8 & 0xFF);
	m.data[3]=(UNS8)(*(UNS32*)dat1>>16 & 0xFF);
	m.data[4]=(UNS8)(*(UNS32*)dat1>>24 & 0xFF);
	break;
  case LSS_SM_SELECTIVE_RESP: /* Switch Mode Selective response*/
  case LSS_IDENT_SLAVE: /* LSS Identify Slave */
  case LSS_IDENT_NON_CONF_SLAVE: /* LSS identify non-configured remote slave */
  	break;
  default:
  	MSG_ERR(0x1D0B, "send Slave LSS command not implemented", command);
  	return 0xFF;
  break;
  }
  
  return canSend(d->canHandle,&m);
}

/*!                                                                                                
**                                                                                                 
**                                                                                                 
** @param d                                                                                        
** @param cob_id                                                                                   
**                                                                                                 
** @return                                                                                         
**/  
UNS8 sendMasterLSSMessage(CO_Data* d, UNS8 command,void *dat1,void *dat2)
{
  Message m;
  UNS8 i;
  
  for(i=1;i<8;i++)m.data[i]=0;
  m.len = 8;
  m.rtr = NOT_A_REQUEST;
  m.data[0]=command;
  m.cob_id.w=MLSS_ADRESS;
  
  /* Tha data sent with the msg depends on the command */	
  switch(command){
  case LSS_SM_GLOBAL: /* Switch Mode Global */
  	d->lss_transfer.state=LSS_FINISHED;
  case LSS_CONF_NODE_ID: /* Configure Node-ID */
  	m.data[1]=*(UNS8 *)dat1;
  	break;
  case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */
  	m.data[1]=*(UNS8 *)dat1;
  	m.data[2]=*(UNS8 *)dat2;
  	break;
  case LSS_CONF_ACT_BIT_TIMING: /* Activate Bit Timing Parameters */
	m.data[1]=(UNS8)(*(UNS32*)dat1 & 0xFF);
	m.data[2]=(UNS8)(*(UNS32*)dat1>>8 & 0xFF);
  	break;
  case LSS_SM_SELECTIVE_VENDOR: /* Switch Mode Selective */
  case LSS_SM_SELECTIVE_PRODUCT:
  case LSS_SM_SELECTIVE_REVISION:
  case LSS_SM_SELECTIVE_SERIAL:
  case LSS_IDENT_REMOTE_VENDOR: /* LSS Identify Remote Slaves */
  case LSS_IDENT_REMOTE_PRODUCT:
  case LSS_IDENT_REMOTE_REV_LOW:
  case LSS_IDENT_REMOTE_REV_HIGH:
  case LSS_IDENT_REMOTE_SERIAL_LOW:
  case LSS_IDENT_REMOTE_SERIAL_HIGH:
	m.data[1]=(UNS8)(*(UNS32*)dat1 & 0xFF);
	m.data[2]=(UNS8)(*(UNS32*)dat1>>8 & 0xFF);
	m.data[3]=(UNS8)(*(UNS32*)dat1>>16 & 0xFF);
	m.data[4]=(UNS8)(*(UNS32*)dat1>>24 & 0xFF);
	break;
  case LSS_CONF_STORE: /* Store Configured Parameters */
  case LSS_IDENT_REMOTE_NON_CONF: /* LSS identify non-configured remote slave */
  case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */
  case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */
  case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */
  case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */
  case LSS_INQ_NODE_ID: /* Inquire Node-ID */
  	break;
  case LSS_IDENT_FASTSCAN:
		m.data[1]=(UNS8)(d->lss_transfer.IDNumber & 0xFF);
		m.data[2]=(UNS8)(d->lss_transfer.IDNumber>>8 & 0xFF);
		m.data[3]=(UNS8)(d->lss_transfer.IDNumber>>16 & 0xFF);
		m.data[4]=(UNS8)(d->lss_transfer.IDNumber>>24 & 0xFF);
		m.data[5]=d->lss_transfer.BitChecked;
		m.data[6]=d->lss_transfer.LSSSub;
		m.data[7]=d->lss_transfer.LSSNext;
	break;
  default:
   	MSG_ERR(0x1D0C, "send Master LSS command not implemented", command);
  	return 0xFF;
  break;
  }
	
  return canSend(d->canHandle,&m);
}

/*!                                                                                                
**                                                                                                 
**                                                                                                 
** @param d                                                                                        
** @param cob_id                                                                                   
**                                                                                                 
** @return                                                                                         
**/  
UNS8 sendLSS(CO_Data* d, UNS8 command,void *dat1,void *dat2)
{
  UNS8 res=1;
  
  /* Tha data sent with the msg depends on the command and if the sender is a master or a slave */
  if (*(d->iam_a_slave)){ 
  	res = sendSlaveLSSMessage(d, command,dat1,dat2);
  }
  else {/* It is a Master */
  	res = sendMasterLSSMessage(d, command,dat1,dat2);
  }
  return res ;
}


/*!                                                                                                
**                                                                                                 
**                                                                                                 
** @param d                                                                                        
** @param m                                                                                        
**                                                                                                 
** @return                                                                                         
**/ 
UNS8 proceedLSS_Master(CO_Data* d, Message* m )
{ 
	UNS8 msg_cs;
	UNS32 Dat1=0;
	UNS8 Dat2=0;
	
 	MSG_WAR(0x3D0D, "MasterLSS proceedLSS; command ", m->data[0]);
	StopLSS_TIMER(LSS_MSG_TIMER);
    /* Set state */
    d->lss_transfer.state = LSS_FINISHED;
	
   	switch(msg_cs=m->data[0]){
   		case LSS_INQ_NODE_ID: /* Inquire Node-ID */
   			Dat1=m->data[1];
   			break;
   		case LSS_CONF_NODE_ID: /* Configure Node-ID */
   		case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */
   		case LSS_CONF_STORE: /* Store Configured Parameters */
   			Dat1=m->data[1];
   			Dat2=m->data[2];
   			break;
   		case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */
   		case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */
   		case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */
   		case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */
   			Dat1=getLSSIdent(m);
 			break;
 		case LSS_IDENT_SLAVE: /* LSS Identify Slave */
   			/*if(d->lss_transfer.command==LSS_IDENT_FASTSCAN){
   			 * ************ TODO *******************
  			}*/
  		break;
 		case LSS_SM_SELECTIVE_RESP: /* Switch Mode Selective response */
   		case LSS_IDENT_NON_CONF_SLAVE: /* LSS identify non-configured remote slave */
   			break;
   		default:
   			MSG_ERR(0x1D0E, "Master LSS command not implemented", msg_cs);
  			return 0xFF;
   		break;
   	}

	d->lss_transfer.dat1=Dat1;
	d->lss_transfer.dat2=Dat2;
 	/* If there is a callback, it is responsible of the received response */
	if(d->lss_transfer.Callback)
    	(*d->lss_transfer.Callback)(d,msg_cs);
    			
   return 0;
}

/*!                                                                                                
**                                                                                                 
**                                                                                                 
** @param d                                                                                        
** @param m                                                                                        
**                                                                                                 
** @return                                                                                         
**/ 
UNS8 proceedLSS_Slave(CO_Data* d, Message* m )
{  
	UNS8 msg_cs;
	
  	MSG_WAR(0x3D0F, "SlaveLSS proceedLSS; command ", m->data[0]);
  	
   	switch(msg_cs=m->data[0]){
   	case LSS_SM_GLOBAL:		/* Switch Mode Global */
   		/* if there is not a mode change break*/
   		if(m->data[1] == d->lss_transfer.mode){
   			MSG_WAR(0x3D10, "SlaveLSS already in the mode ", m->data[1]);
   			break;
   		}
   		
		if(m->data[1]==LSS_CONFIGURATION_MODE)	{
			MSG_WAR(0x3D11, "SlaveLSS switching to configuration mode ", 0);
			/* Store the NodeId in case it will be changed */
			d->lss_transfer.nodeID=getNodeId(d);
			d->lss_transfer.mode=LSS_CONFIGURATION_MODE;
		}
		else if(m->data[1]==LSS_WAITING_MODE){
			MSG_WAR(0x3D12, "SlaveLSS switching to operational mode ", 0);
			
			if(d->lss_transfer.switchDelayState==SDELAY_OFF){
				/* If the nodeID has changed update it and put the node state to Initialisation. */
				if(d->lss_transfer.nodeID!=getNodeId(d)){
					MSG_WAR(0x3D13, "The node Id has changed. Reseting to Initialisation state",0);
					setNodeId(d, d->lss_transfer.nodeID);
					setState(d, Initialisation);
				}
			}
			d->lss_transfer.mode=LSS_WAITING_MODE;
		}
	break;
	case LSS_CONF_NODE_ID: /* Configure Node-ID */
	{ 
		UNS8 error_code=0;
		UNS8 spec_error=0;
			
		if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE){
			if(m->data[1]>127 && m->data[1]!=0xFF){
				MSG_ERR(0x1D14, "NodeID out of range",0);
				error_code=1; /* NodeID out of range */
			}
			else{
				d->lss_transfer.nodeID=m->data[1];
			}
		}
		else{
			MSG_ERR(0x1D15, "SlaveLSS not in configuration mode",0);
			error_code=0xFF;
		}
		sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error);
	}	
	break;
	case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */
	{
		UNS8 error_code=0;
		UNS8 spec_error=0;
			
		if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE){
			/* Change the baud rate only if the function lss_ChangeBaudRate 
			 * has been implemented. If not send an error.
			 */
			if(d->lss_ChangeBaudRate){
				/* If a baud rate is not supported just comment the line. */
				switch(m->data[2]){
				case 0x00:d->lss_transfer.baudRate="1M";break;
				case 0x01:d->lss_transfer.baudRate="800K";break;
				case 0x02:d->lss_transfer.baudRate="500K";break;
				case 0x03:d->lss_transfer.baudRate="250K";break;
				case 0x04:d->lss_transfer.baudRate="125K";break;
				case 0x05:d->lss_transfer.baudRate="100K";break;
				case 0x06:d->lss_transfer.baudRate="50K";break;
				case 0x07:d->lss_transfer.baudRate="20K";break;
				case 0x08:d->lss_transfer.baudRate="10K";break;
				default:
					MSG_ERR(0x1D16, "Baud rate not supported",0);
					error_code=0xFF; /* Baud rate not supported*/
					break; 
				}		
			}
			else
			{
				MSG_ERR(0x1D17, "Bit timing not supported",0);
				error_code=0x01; /* bit timing not supported */
			}
		}
		else{
			MSG_ERR(0x1D18, "SlaveLSS not in configuration mode",0);
			error_code=0xFF;
		}
		
		sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error);
	}
	break;
	case LSS_CONF_ACT_BIT_TIMING: /* Activate Bit Timing Parameters */
		
		if(d->lss_transfer.mode!=LSS_CONFIGURATION_MODE){
			MSG_ERR(0x1D19, "SlaveLSS not in configuration mode",0);
			break;
		}
		
		if(d->lss_transfer.baudRate!="none"){
			d->lss_transfer.switchDelay=getLSSDelay(m);
			MSG_WAR(0x3D1A, "Slave Switch Delay set to: ",d->lss_transfer.switchDelay);
			d->lss_transfer.switchDelayState=SDELAY_FIRST;
			d->lss_transfer.currentState=getState(d);
			setState(d, LssTimingDelay);
			StartLSS_SDELAY_TIMER();
		}
	break;
	case LSS_CONF_STORE: /* Store Configured Parameters */
	{
		UNS8 error_code=0;
		UNS8 spec_error=0;
		
		if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE){ 
			if(d->lss_StoreConfiguration){
				 /* call lss_StoreConfiguration with NodeId */
	  			(*d->lss_StoreConfiguration)(&error_code,&spec_error);
			}
			else{
				MSG_ERR(0x1D1B, "Store configuration not supported",0);
				error_code=1; /* store configuration is not supported */
			}	
		}
		else{
			MSG_ERR(0x1D1C, "SlaveLSS not in configuration mode",0);
			error_code=0xFF;
		}
		sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error);
	}
	break;
	case LSS_SM_SELECTIVE_VENDOR:	/* Switch Mode Selective */
	case LSS_SM_SELECTIVE_PRODUCT:
	case LSS_SM_SELECTIVE_REVISION:
	case LSS_SM_SELECTIVE_SERIAL:
	{
		UNS32 errorCode;
  		const indextable *ptrTable;
  		ODCallback_t *Callback;
  		UNS32 _SpecificNodeInfo;
  
  		if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE)
  		{
  			MSG_ERR(0x1D1D, "Switch Mode Selective only supported in operational mode",0);
  			break;
  		}
  			
		_SpecificNodeInfo=getLSSIdent(m);
				
		ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback);
		if(_SpecificNodeInfo==*(UNS32*)ptrTable->pSubindex[msg_cs-(LSS_SM_SELECTIVE_VENDOR-1)].pObject){
			
			d->lss_transfer.addr_sel_match|=(0x01<<(msg_cs-LSS_SM_SELECTIVE_VENDOR));
			/* If all the fields has been set */
			if(d->lss_transfer.addr_sel_match==0x0F){
				
				MSG_WAR(0x3D1E, "SlaveLSS switching to configuration mode ", 0);
				d->lss_transfer.addr_sel_match=0;
				d->lss_transfer.nodeID=getNodeId(d);
				d->lss_transfer.mode=LSS_CONFIGURATION_MODE;

				sendSlaveLSSMessage(d,LSS_SM_SELECTIVE_RESP,0,0);
			}
		}	
		else {
			MSG_WAR(0x3D1F, "LSS identity field doesn't match ", _SpecificNodeInfo);
			d->lss_transfer.addr_sel_match=0;
		}	
	}	
	break;
	case LSS_IDENT_REMOTE_VENDOR: /* LSS Identify Remote Slaves */
	case LSS_IDENT_REMOTE_PRODUCT:
	case LSS_IDENT_REMOTE_REV_LOW:
	case LSS_IDENT_REMOTE_REV_HIGH:
	case LSS_IDENT_REMOTE_SERIAL_LOW:
	case LSS_IDENT_REMOTE_SERIAL_HIGH:
	{
		UNS32 errorCode;
  		const indextable *ptrTable;
  		ODCallback_t *Callback;
  		UNS32 _SpecificNodeInfo;
  		
		_SpecificNodeInfo=getLSSIdent(m);
		
		ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback);
			
		/* Check if the data match the identity object. */
		switch(msg_cs){
		case LSS_IDENT_REMOTE_VENDOR:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo == *(UNS32*)ptrTable->pSubindex[1].pObject)? d->lss_transfer.addr_ident_match|0x01:0;	break;
		case LSS_IDENT_REMOTE_PRODUCT:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo == *(UNS32*)ptrTable->pSubindex[2].pObject)? d->lss_transfer.addr_ident_match|0x02:0;	break;
		case LSS_IDENT_REMOTE_REV_LOW:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo <= *(UNS32*)ptrTable->pSubindex[3].pObject)? d->lss_transfer.addr_ident_match|0x04:0; break;
		case LSS_IDENT_REMOTE_REV_HIGH:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo >= *(UNS32*)ptrTable->pSubindex[3].pObject)? d->lss_transfer.addr_ident_match|0x08:0;	break;
		case LSS_IDENT_REMOTE_SERIAL_LOW:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo <= *(UNS32*)ptrTable->pSubindex[4].pObject)? d->lss_transfer.addr_ident_match|0x10:0;	break;
		case LSS_IDENT_REMOTE_SERIAL_HIGH:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo >= *(UNS32*)ptrTable->pSubindex[4].pObject)? d->lss_transfer.addr_ident_match|0x20:0;	break;
		}
		/* If all the fields has been set.. */
		if(d->lss_transfer.addr_ident_match==0x3F){
			MSG_WAR(0x3D20, "SlaveLSS identified ", 0);
			d->lss_transfer.addr_ident_match=0;
			sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0);
		}
		else if(d->lss_transfer.addr_ident_match==0){
			MSG_WAR(0x3D21, "LSS identify field doesn't match ", _SpecificNodeInfo);
		}
	}
	break;
	case LSS_IDENT_REMOTE_NON_CONF: /* LSS identify non-configured remote slave */
	{
		if(getNodeId(d)==0xFF){		
			MSG_WAR(0x3D22, "SlaveLSS non-configured ", 0);
			sendSlaveLSSMessage(d,LSS_IDENT_NON_CONF_SLAVE,0,0);
		}
		else{
			MSG_WAR(0x3D23, "SlaveLSS already configured ", 0);
		}
	}
	break;
	case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */
	case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */
	case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */
	case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */
	if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE)
	{
	
		UNS32 errorCode;
  		const indextable *ptrTable;
  		ODCallback_t *Callback;
  		UNS32 _SpecificNodeInfo;
  
  		ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback);
  		_SpecificNodeInfo=*(UNS32*)ptrTable->pSubindex[msg_cs-(LSS_INQ_VENDOR_ID-1)].pObject;
  		MSG_WAR(0x3D24, "SlaveLSS identity field inquired ", _SpecificNodeInfo);
			
		sendSlaveLSSMessage(d,msg_cs,&_SpecificNodeInfo,0);
	}
	break;
	case LSS_INQ_NODE_ID: /* Inquire Node-ID */
		if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE)
		{
			UNS8 NodeID;
	
			NodeID=getNodeId(d);
			MSG_WAR(0x3D25, "SlaveLSS Node ID inquired ", NodeID);
			sendSlaveLSSMessage(d,msg_cs,&NodeID,0);
		}
		else{
			MSG_ERR(0x1D26, "SlaveLSS not in configuration mode",0);
		}
	break;
	case LSS_IDENT_FASTSCAN:
	{
		/******* TODO *******/
	}	
	break;
   	default:
   		MSG_ERR(0x1D27, "SlaveLSS command not implemented", msg_cs);
  		return 0xFF;
  		break;
   	}
   
    return 0;
}

UNS8 configNetworkNode(CO_Data* d, UNS8 command, void *dat1, void* dat2)
{
	return sendMasterLSSMessage(d,command,dat1,dat2);
}

UNS8 configNetworkNodeCallBack (CO_Data* d, UNS8 command, void *dat1, void* dat2, LSSCallback_t Callback)
{
	d->lss_transfer.state=LSS_TRANS_IN_PROGRESS;
	d->lss_transfer.Callback=Callback;
	d->lss_transfer.command=command;
	
	StopLSS_TIMER(LSS_MSG_TIMER);
  	StartLSS_MSG_TIMER();
  	
	return sendMasterLSSMessage(d,command,dat1,dat2);
}

UNS8 getConfigResultNetworkNode (CO_Data* d, UNS8 command, UNS32* dat1, UNS8* dat2)
{ 
  *dat1=d->lss_transfer.dat1;
  *dat2=d->lss_transfer.dat2;
  return d->lss_transfer.state;
}

//void _lss_StoreConfiguration(UNS8 *error, UNS8 *spec_error){printf("_lss_StoreConfiguration\n");}
//void _lss_ChangeBaudRate(char *BaudRate){printf("_lss_ChangeBaudRate\n");}

#endif