Added FastScan support to the LSS services.
authorgroke6
Thu, 17 Jan 2008 16:22:21 +0100
changeset 361 2220813498f2
parent 360 09af3515dd42
child 362 1134fc19aa74
Added FastScan support to the LSS services.
include/data.h
include/lss.h
src/dcf.c
src/lss.c
--- a/include/data.h	Thu Jan 17 16:21:07 2008 +0100
+++ b/include/data.h	Thu Jan 17 16:22:21 2008 +0100
@@ -159,8 +159,9 @@
 		"none",         		/* BaudRate */\
 		0,          			/* SwitchDelay */\
 		SDELAY_OFF,   			/* SwitchDelayState */\
-		{-1,-1},          		/* Timers[2] */\
+		{-1,-1,-1},          		/* Timers[3] */\
 		NULL,        			/* Callback */\
+		0,						/* LSSanswer */\
 		0,						/* IDNumber */\
   		128, 					/* BitChecked */\
   		0,						/* LSSSub */\
--- a/include/lss.h	Thu Jan 17 16:21:07 2008 +0100
+++ b/include/lss.h	Thu Jan 17 16:22:21 2008 +0100
@@ -28,6 +28,7 @@
 
 #define LSS_MSG_TIMER 0
 #define LSS_SWITCH_DELAY_TIMER 1
+#define LSS_FS_TIMER 2
 
 #define SDELAY_OFF		0
 #define SDELAY_FIRST 	1
@@ -68,6 +69,8 @@
 
 /*FastScan State Machine*/
 #define LSS_FS_RESET	0
+#define LSS_FS_PROCESSING 1
+#define LSS_FS_CONFIRMATION 2
 
 
 typedef void (*LSSCallback_t)(CO_Data* d, UNS8 command); 
@@ -105,7 +108,7 @@
   UNS16 switchDelay;		/* the period of the two delay */
   UNS8  switchDelayState;   /* the state machine for the switchDelay */
 
-  TIMER_HANDLE timers[2];      /* Time counters to implement a timeout in milliseconds.
+  TIMER_HANDLE timers[3];      /* Time counters to implement a timeout in milliseconds.
                               * LSS_MSG_TIMER (index 0) is automatically incremented whenever 
                               * the lss state is in LSS_TRANS_IN_PROGRESS, and reseted to 0 
                               * when the response LSS have been received.
@@ -115,12 +118,15 @@
                               */
   LSSCallback_t Callback;   /* The user callback func to be called at LSS transaction end */
   
-  UNS32 IDNumber;
-  UNS8 BitChecked;
-  UNS8 LSSSub;
-  UNS8 LSSNext;
-  UNS8 LSSPos;
-  UNS8 FastScan_SM;
+  UNS8 LSSanswer;			/* stores if a message has been received during a timer period */
+  
+  UNS32 IDNumber;			/* in the master, the LSS address parameter which it currently tries to identify.
+  							 * in the slave, the LSS address parameter which is being checked (LSS-ID[sub]). */
+  UNS8 BitChecked;			/* bits of the current IDNumber that are currently checked */
+  UNS8 LSSSub;				/* which part of the LSS-ID is currently checked in IDNumber */
+  UNS8 LSSNext;				/* which LSSSub value will be used in the next request */
+  UNS8 LSSPos;				/* in the slave, which part of the LSS-ID is currently processed*/
+  UNS8 FastScan_SM;			/* the state machine for the FastScan protocol */
 };
 
 #ifdef CO_ENABLE_LSS
--- a/src/dcf.c	Thu Jan 17 16:21:07 2008 +0100
+++ b/src/dcf.c	Thu Jan 17 16:22:21 2008 +0100
@@ -76,6 +76,7 @@
 */
 UNS8 send_consise_dcf(CO_Data* d,UNS8 nodeId)
 {
+  UNS8 szData;
   /* Fetch DCF OD entry, if not already done */
   if(!d->dcf_odentry)
   {
@@ -86,7 +87,8 @@
     if (errorCode != OD_SUCCESSFUL) goto DCF_finish;
   }
 
-  UNS8 szData = d->dcf_odentry->pSubindex[nodeId].size;
+  szData = d->dcf_odentry->pSubindex[nodeId].size;
+  
   /* if the entry for the nodeId is not empty. */
   if(szData!=0){
   	/* if the entry for the nodeId is already been processing, quit.*/
--- a/src/lss.c	Thu Jan 17 16:21:07 2008 +0100
+++ b/src/lss.c	Thu Jan 17 16:22:21 2008 +0100
@@ -42,7 +42,8 @@
 #include "lss.h"
 #include "canfestival.h"
 
-#define LSS_TIMEOUT_MS	(TIMEVAL)1000  /* ms */
+//#define LSS_TIMEOUT_MS	(TIMEVAL)1000  /* ms */
+//#define LSS_FS_TIMEOUT_MS	(TIMEVAL)100  /* 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]))
@@ -74,6 +75,11 @@
  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));}
 
+#define StartLSS_FS_TIMER(){\
+ MSG_WAR(0x3D04, "StartLSS_TIMER for FS_TIMER",0);\
+ d->lss_transfer.timers[LSS_FS_TIMER] = SetAlarm(d,LSS_FS_TIMER,&LssAlarm,MS_TO_TIMEVAL(LSS_FS_TIMEOUT_MS),0);}
+
+UNS8 sendMasterLSSMessage(CO_Data* d, UNS8 command,void *dat1,void *dat2);
 
 /*!                                                                                                
 **                                                                                                 
@@ -92,36 +98,143 @@
  
 	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;
+	 	StopLSS_TIMER(LSS_MSG_TIMER);
+#ifdef CO_ENABLE_LSS_FS
+		if(d->lss_transfer.command==LSS_IDENT_FASTSCAN){
+			if(d->lss_transfer.FastScan_SM==LSS_FS_RESET){
+   				/* if at least one node had answered before the timer expired, start the FastScan protocol*/
+   				if(d->lss_transfer.LSSanswer!=0){
+   					d->lss_transfer.LSSanswer=0;
+   					d->lss_transfer.BitChecked=31;
+   					d->lss_transfer.IDNumber=0;
+   					d->lss_transfer.FastScan_SM=LSS_FS_PROCESSING;
+   					StartLSS_FS_TIMER();
+   					sendMasterLSSMessage(d,LSS_IDENT_FASTSCAN,0,0);
+   					return;
+   				}
+   				else{ 
+   				
+    				d->lss_transfer.state = LSS_FINISHED;
+    				/* Inform the application that there aren't not configured nodes in the net  */
+    				d->lss_transfer.dat1=1;
+   				}
+   			}
+   			else{
+				/* This should not happen, an error ocurred*/
+				MSG_ERR(0x1D05, "LSS FastScan timeout. FastScan_SM inconsisten state.", d->lss_transfer.FastScan_SM);
+   			}
+		}
+		else
+#endif
+		{
+			MSG_ERR(0x1D06, "LSS timeout. LSS response not received.", 0);
+    		MSG_WAR(0x2D07, "LSS timeout command specifier : ", d->lss_transfer.command);
+    		/* 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*/
+	    	/*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 
+  		/* The first switch_delay period 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);
+   			MSG_WAR(0x3D08, "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);
+    		MSG_WAR(0x3D09, "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;
+#ifdef CO_ENABLE_LSS_FS
+     case LSS_FS_TIMER:
+	 	StopLSS_TIMER(LSS_FS_TIMER);
+		
+		switch(d->lss_transfer.FastScan_SM){
+   		case LSS_FS_RESET:
+   		{
+   		   	/* This should not happen, an error ocurred*/
+			MSG_ERR(0x1D0A, "LSS FastScan timeout. FastScan_SM inconsisten state.", d->lss_transfer.FastScan_SM);
+   		}
+   		break;
+		case LSS_FS_PROCESSING:
+		{
+			/* If there isn't any answer, set the bit */
+			if(d->lss_transfer.LSSanswer==0){
+				UNS32 Mask=0x1;
+				Mask<<=d->lss_transfer.BitChecked;
+				d->lss_transfer.IDNumber|=Mask;
+			}
+			
+			if(d->lss_transfer.BitChecked==0){
+				/* We finished with the current LSS-ID[sub], confirm it */
+				d->lss_transfer.FastScan_SM=LSS_FS_CONFIRMATION;
+				if(d->lss_transfer.LSSNext<3)d->lss_transfer.LSSNext++;
+			}
+			else{
+				d->lss_transfer.BitChecked--;
+			}
+   			
+   			d->lss_transfer.LSSanswer=0;
+  			StartLSS_FS_TIMER();
+   			sendMasterLSSMessage(d,LSS_IDENT_FASTSCAN,0,0);
+   			return;
+   		}
+		break;
+		case LSS_FS_CONFIRMATION:
+		{
+			if(d->lss_transfer.LSSanswer!=0){
+				d->lss_transfer.LSSanswer=0;
+				
+				if(d->lss_transfer.LSSSub==3){
+					/* The LSS FastScan protocol finished correctly. Restore the parameters */
+					d->lss_transfer.BitChecked=128;
+					d->lss_transfer.FastScan_SM=LSS_FS_RESET;
+					d->lss_transfer.LSSSub=0;
+					d->lss_transfer.LSSNext=0;
+   					d->lss_transfer.IDNumber=0;
+					
+					/* Inform the application that the FastScan finished correctly */
+					d->lss_transfer.state = LSS_FINISHED;
+					d->lss_transfer.dat1=0;
+				}
+				else{
+					/* Start with the next LSS-ID[sub] */
+					d->lss_transfer.LSSSub++;
+					d->lss_transfer.BitChecked=31;
+   					d->lss_transfer.IDNumber=0;
+   					d->lss_transfer.FastScan_SM=LSS_FS_PROCESSING;
+   					StartLSS_FS_TIMER();
+   					sendMasterLSSMessage(d,LSS_IDENT_FASTSCAN,0,0);
+   					return;
+				}
+			}
+			else{
+				/* This should not happen, an error ocurred*/
+				MSG_ERR(0x1D0B, "LSS FastScan timeout. FastScan response not received.", 0);
+				/* Set aborted state */
+    			d->lss_transfer.state = LSS_ABORTED_INTERNAL;
+			}
+		}
+		break;
+   		}
+
+    	/* 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;
+#endif
 	}
 }
 
@@ -159,8 +272,8 @@
   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;
+  	MSG_WAR(0x2D0C, "unable to send the LSS message, not in the proper state =>", d->nodeState);
+  	return 0xFF;
   }
    
   for(i=1;i<8;i++)m.data[i]=0;
@@ -194,9 +307,8 @@
   case LSS_IDENT_NON_CONF_SLAVE: /* LSS identify non-configured remote slave */
   	break;
   default:
-  	MSG_ERR(0x1D0B, "send Slave LSS command not implemented", command);
+  	MSG_ERR(0x1D0D, "send Slave LSS command not implemented", command);
   	return 0xFF;
-  break;
   }
   
   return canSend(d->canHandle,&m);
@@ -259,6 +371,7 @@
   case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */
   case LSS_INQ_NODE_ID: /* Inquire Node-ID */
   	break;
+#ifdef CO_ENABLE_LSS_FS
   case LSS_IDENT_FASTSCAN:
 		m.data[1]=(UNS8)(d->lss_transfer.IDNumber & 0xFF);
 		m.data[2]=(UNS8)(d->lss_transfer.IDNumber>>8 & 0xFF);
@@ -268,10 +381,10 @@
 		m.data[6]=d->lss_transfer.LSSSub;
 		m.data[7]=d->lss_transfer.LSSNext;
 	break;
+#endif
   default:
-   	MSG_ERR(0x1D0C, "send Master LSS command not implemented", command);
+   	MSG_ERR(0x1D0E, "send Master LSS command not implemented", command);
   	return 0xFF;
-  break;
   }
 	
   return canSend(d->canHandle,&m);
@@ -314,18 +427,33 @@
 	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;
+	if(d->lss_transfer.state!=LSS_TRANS_IN_PROGRESS)
+	{
+		//MSG_WAR(0x3D0D, "MasterLSS proceedLSS; unexpected message arrived;command ", m->data[0]);
+		//return 0;
+		goto ErrorProcessMaster;
+	}
+	
+	//#ifdef CO_ENABLE_LSS_FS
+	/* The FastScan protocol doesn't stops the timers when a message has been received */
+	/*if(d->lss_transfer.command!=LSS_IDENT_FASTSCAN)
+#endif
+	{
+		StopLSS_TIMER(LSS_MSG_TIMER);
+    	d->lss_transfer.state = LSS_FINISHED;
+	}*/
+	
+ 	MSG_WAR(0x3D0F, "MasterLSS proceedLSS; command ", m->data[0]);
 	
    	switch(msg_cs=m->data[0]){
    		case LSS_INQ_NODE_ID: /* Inquire Node-ID */
+   			if(d->lss_transfer.command!=LSS_INQ_NODE_ID)goto ErrorProcessMaster;
    			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 */
+   			if(d->lss_transfer.command!=msg_cs)goto ErrorProcessMaster;
    			Dat1=m->data[1];
    			Dat2=m->data[2];
    			break;
@@ -333,29 +461,56 @@
    		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.command!=msg_cs)goto ErrorProcessMaster;
    			Dat1=getLSSIdent(m);
  			break;
  		case LSS_IDENT_SLAVE: /* LSS Identify Slave */
-   			/*if(d->lss_transfer.command==LSS_IDENT_FASTSCAN){
-   			 * ************ TODO *******************
-  			}*/
+#ifdef CO_ENABLE_LSS_FS
+   			if(d->lss_transfer.command==LSS_IDENT_FASTSCAN){
+   				/* A message arrived during the timer period */
+   				d->lss_transfer.LSSanswer=1;
+   				return 0;
+  			}
+  			else
+#endif
+			if(d->lss_transfer.command!=LSS_IDENT_REMOTE_VENDOR && \
+ 				d->lss_transfer.command!=LSS_IDENT_REMOTE_PRODUCT && \
+ 				d->lss_transfer.command!=LSS_IDENT_REMOTE_REV_LOW && \
+ 				d->lss_transfer.command!=LSS_IDENT_REMOTE_REV_HIGH && \
+ 				d->lss_transfer.command!=LSS_IDENT_REMOTE_SERIAL_LOW && \
+ 				d->lss_transfer.command!=LSS_IDENT_REMOTE_SERIAL_HIGH )
+ 					goto ErrorProcessMaster;
   		break;
  		case LSS_SM_SELECTIVE_RESP: /* Switch Mode Selective response */
+ 			if(d->lss_transfer.command!=LSS_SM_SELECTIVE_VENDOR && \
+ 				d->lss_transfer.command!=LSS_SM_SELECTIVE_PRODUCT && \
+ 				d->lss_transfer.command!=LSS_SM_SELECTIVE_REVISION && \
+ 				d->lss_transfer.command!=LSS_SM_SELECTIVE_SERIAL )
+ 					goto ErrorProcessMaster;
+ 			break;
    		case LSS_IDENT_NON_CONF_SLAVE: /* LSS identify non-configured remote slave */
+   			if(d->lss_transfer.command!=LSS_IDENT_REMOTE_NON_CONF)goto ErrorProcessMaster;
    			break;
    		default:
-   			MSG_ERR(0x1D0E, "Master LSS command not implemented", msg_cs);
+   			MSG_ERR(0x1D10, "Master LSS command not implemented", msg_cs);
   			return 0xFF;
-   		break;
    	}
-
+	
+	StopLSS_TIMER(LSS_MSG_TIMER);
+    d->lss_transfer.state = LSS_FINISHED;
+    	
 	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);
+    	(*d->lss_transfer.Callback)(d,d->lss_transfer.command);
     			
    return 0;
+   
+ErrorProcessMaster:
+    MSG_WAR(0x3D11, "MasterLSS proceedLSS; unexpected message arrived;command ", m->data[0]);
+	return 0xFF;
+		
 }
 
 /*!                                                                                                
@@ -370,29 +525,29 @@
 {  
 	UNS8 msg_cs;
 	
-  	MSG_WAR(0x3D0F, "SlaveLSS proceedLSS; command ", m->data[0]);
+  	MSG_WAR(0x3D12, "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]);
+   			MSG_WAR(0x3D13, "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);
+			MSG_WAR(0x3D14, "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);
+			MSG_WAR(0x3D15, "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);
+					MSG_WAR(0x3D16, "The node Id has changed. Reseting to Initialisation state",0);
 					setNodeId(d, d->lss_transfer.nodeID);
 					setState(d, Initialisation);
 				}
@@ -407,7 +562,7 @@
 			
 		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);
+				MSG_ERR(0x1D17, "NodeID out of range",0);
 				error_code=1; /* NodeID out of range */
 			}
 			else{
@@ -415,8 +570,9 @@
 			}
 		}
 		else{
-			MSG_ERR(0x1D15, "SlaveLSS not in configuration mode",0);
-			error_code=0xFF;
+			MSG_WAR(0x3D18, "SlaveLSS not in configuration mode",0);
+			//error_code=0xFF;
+			break;
 		}
 		sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error);
 	}	
@@ -443,20 +599,21 @@
 				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);
+					MSG_ERR(0x1D19, "Baud rate not supported",0);
 					error_code=0xFF; /* Baud rate not supported*/
 					break; 
 				}		
 			}
 			else
 			{
-				MSG_ERR(0x1D17, "Bit timing not supported",0);
+				MSG_ERR(0x1D1A, "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;
+			MSG_WAR(0x3D1B, "SlaveLSS not in configuration mode",0);
+			//error_code=0xFF;
+			break;
 		}
 		
 		sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error);
@@ -465,13 +622,13 @@
 	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);
+			MSG_ERR(0x3D1C, "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);
+			MSG_WAR(0x3D1D, "Slave Switch Delay set to: ",d->lss_transfer.switchDelay);
 			d->lss_transfer.switchDelayState=SDELAY_FIRST;
 			d->lss_transfer.currentState=getState(d);
 			setState(d, LssTimingDelay);
@@ -489,13 +646,14 @@
 	  			(*d->lss_StoreConfiguration)(&error_code,&spec_error);
 			}
 			else{
-				MSG_ERR(0x1D1B, "Store configuration not supported",0);
+				MSG_ERR(0x1D1E, "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;
+			MSG_WAR(0x3D1F, "SlaveLSS not in configuration mode",0);
+			//error_code=0xFF;
+			break;
 		}
 		sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error);
 	}
@@ -512,7 +670,7 @@
   
   		if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE)
   		{
-  			MSG_ERR(0x1D1D, "Switch Mode Selective only supported in operational mode",0);
+  			MSG_ERR(0x1D20, "Switch Mode Selective only supported in operational mode",0);
   			break;
   		}
   			
@@ -525,7 +683,7 @@
 			/* If all the fields has been set */
 			if(d->lss_transfer.addr_sel_match==0x0F){
 				
-				MSG_WAR(0x3D1E, "SlaveLSS switching to configuration mode ", 0);
+				MSG_WAR(0x3D21, "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;
@@ -534,7 +692,7 @@
 			}
 		}	
 		else {
-			MSG_WAR(0x3D1F, "LSS identity field doesn't match ", _SpecificNodeInfo);
+			MSG_WAR(0x3D22, "LSS identity field doesn't match ", _SpecificNodeInfo);
 			d->lss_transfer.addr_sel_match=0;
 		}	
 	}	
@@ -566,23 +724,23 @@
 		}
 		/* If all the fields has been set.. */
 		if(d->lss_transfer.addr_ident_match==0x3F){
-			MSG_WAR(0x3D20, "SlaveLSS identified ", 0);
+			MSG_WAR(0x3D23, "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);
+			MSG_WAR(0x3D24, "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);
+			MSG_WAR(0x3D25, "SlaveLSS non-configured ", 0);
 			sendSlaveLSSMessage(d,LSS_IDENT_NON_CONF_SLAVE,0,0);
 		}
 		else{
-			MSG_WAR(0x3D23, "SlaveLSS already configured ", 0);
+			MSG_WAR(0x3D26, "SlaveLSS already configured ", 0);
 		}
 	}
 	break;
@@ -600,7 +758,7 @@
   
   		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);
+  		MSG_WAR(0x3D27, "SlaveLSS identity field inquired ", _SpecificNodeInfo);
 			
 		sendSlaveLSSMessage(d,msg_cs,&_SpecificNodeInfo,0);
 	}
@@ -611,22 +769,96 @@
 			UNS8 NodeID;
 	
 			NodeID=getNodeId(d);
-			MSG_WAR(0x3D25, "SlaveLSS Node ID inquired ", NodeID);
+			MSG_WAR(0x3D28, "SlaveLSS Node ID inquired ", NodeID);
 			sendSlaveLSSMessage(d,msg_cs,&NodeID,0);
 		}
 		else{
-			MSG_ERR(0x1D26, "SlaveLSS not in configuration mode",0);
-		}
-	break;
+			MSG_WAR(0x3D29, "SlaveLSS not in configuration mode",0);
+		}
+	break;
+#ifdef CO_ENABLE_LSS_FS
 	case LSS_IDENT_FASTSCAN:
 	{
-		/******* TODO *******/
+		/* If the nodeID isn't 0xFF the slave shall not participate  */
+		if(getNodeId(d)!=0xFF)break;
+		if(getLSSBitCheck(m)==128)d->lss_transfer.FastScan_SM=LSS_FS_RESET;
+		
+   		switch(d->lss_transfer.FastScan_SM){
+   		case LSS_FS_RESET:
+   		{
+   			UNS32 errorCode;
+  			const indextable *ptrTable;
+  			ODCallback_t *Callback;
+  				
+			MSG_WAR(0x3D2A, "SlaveLSS Reseting LSSPos", 0);
+			d->lss_transfer.LSSPos=0;
+			d->lss_transfer.FastScan_SM=LSS_FS_PROCESSING;
+			
+  			ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback);
+  			d->lss_transfer.IDNumber=*(UNS32*)ptrTable->pSubindex[d->lss_transfer.LSSPos+1].pObject;
+			
+			sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0);
+   		}
+		break;
+		case LSS_FS_PROCESSING:/*if(getLSSBitCheck(m)<32)*/
+			if(d->lss_transfer.LSSPos==getLSSSub(m))
+			{
+				UNS32 Mask=0xFFFFFFFF<<getLSSBitCheck(m);
+				
+				MSG_WAR(0x3D2B, "SlaveLSS FastScan IDNumber", getLSSIdent(m));
+				MSG_WAR(0x3D2C, "SlaveLSS FastScan BitMask ", Mask);
+				MSG_WAR(0x3D2D, "SlaveLSS FastScan LSS-ID  ", d->lss_transfer.IDNumber);
+				
+				if((getLSSIdent(m) & Mask)==(d->lss_transfer.IDNumber & Mask))
+				{
+					sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0);
+				}
+				
+				if(getLSSBitCheck(m)==0)
+				{
+					d->lss_transfer.FastScan_SM=LSS_FS_CONFIRMATION;
+				}
+			}
+			break;
+		case LSS_FS_CONFIRMATION:
+			if(d->lss_transfer.LSSPos==getLSSSub(m))
+			{
+				if(getLSSIdent(m)==d->lss_transfer.IDNumber)
+				{
+					/* Current LSS-ID[sub] confirmed correctly */
+					MSG_WAR(0x3D2E, "SlaveLSS FastScan IDNumber and LSS-ID match=>", d->lss_transfer.IDNumber);
+					if(d->lss_transfer.LSSPos==3)
+					{
+						/* All LSS-ID[sub] identified correctly, switching to configuration mode */
+						MSG_WAR(0x3D2F, "SlaveLSS switching to configuration mode ", 0);
+			   			d->lss_transfer.nodeID=getNodeId(d);
+			   			d->lss_transfer.mode=LSS_CONFIGURATION_MODE;
+			    		d->lss_transfer.FastScan_SM=LSS_FS_RESET;
+			    		d->lss_transfer.LSSPos=0xFF;
+					}		
+					else
+					{
+						/* Switch to the next LSS-ID[sub] */
+						UNS32 errorCode;
+  						const indextable *ptrTable;
+  						ODCallback_t *Callback;
+		
+						d->lss_transfer.LSSPos=getLSSNext(m);
+						ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback);
+  						d->lss_transfer.IDNumber=*(UNS32*)ptrTable->pSubindex[d->lss_transfer.LSSPos+1].pObject;
+						d->lss_transfer.FastScan_SM=LSS_FS_PROCESSING;						
+					}
+					sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0);
+				}
+			}
+			break;
+		}
 	}	
 	break;
+#endif
    	default:
-   		MSG_ERR(0x1D27, "SlaveLSS command not implemented", msg_cs);
+   		MSG_ERR(0x1D30, "SlaveLSS command not implemented", msg_cs);
   		return 0xFF;
-  		break;
    	}
    
     return 0;