canfestival: comparison src/lss.c

equal deleted inserted replaced

-:fe47568b8281
+:854c43cdc24a
 /* 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);
+UNS8 sendMasterLSSMessage(CO_Data* d, UNS8 command,void *dat1,void *dat2);
+void LssAlarmMSG(CO_Data* d, UNS32 id);
-#define StopLSS_TIMER(id){\
+void LssAlarmSDELAY(CO_Data* d, UNS32 id);
-MSG_WAR(0x3D01, "StopLSS_TIMER for timer : ", id);\
-d->lss_transfer.timers[id] = DelAlarm(d->lss_transfer.timers[id]);}
+#define StopLSS_MSG_TIMER(){\
+MSG_WAR(0x3D01, "StopLSS_MSG_TIMER", 0);\
+d->lss_transfer.timerMSG = DelAlarm(d->lss_transfer.timerMSG);}
 #define StartLSS_MSG_TIMER(){\
-MSG_WAR(0x3D02, "StartLSS_TIMER for MSG_TIMER",0);\
+MSG_WAR(0x3D02, "StartLSS_MSG_TIMER",0);\
-d->lss_transfer.timers[LSS_MSG_TIMER] = SetAlarm(d,LSS_MSG_TIMER,&LssAlarm,MS_TO_TIMEVAL(LSS_TIMEOUT_MS),0);}
+d->lss_transfer.timerMSG = SetAlarm(d,0,&LssAlarmMSG,MS_TO_TIMEVAL(LSS_TIMEOUT_MS),0);}
+#define StopLSS_SDELAY_TIMER(){\
+MSG_WAR(0x3D03, "StopLSS_SDELAY_TIMER", 0);\
+d->lss_transfer.timerSDELAY = DelAlarm(d->lss_transfer.timerSDELAY);}
 #define StartLSS_SDELAY_TIMER(){\
-MSG_WAR(0x3D03, "StartLSS_TIMER for SDELAY_TIMER",0);\
+MSG_WAR(0x3D04, "StartLSS_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));}
+d->lss_transfer.timerSDELAY= SetAlarm(d,0,&LssAlarmSDELAY,MS_TO_TIMEVAL(d->lss_transfer.switchDelay),MS_TO_TIMEVAL(d->lss_transfer.switchDelay));}
+#ifdef CO_ENABLE_LSS_FS
+/* Prototypes for internals functions */
+void LssAlarmFS(CO_Data* d, UNS32 id);
+#define StopLSS_FS_TIMER(){\
+MSG_WAR(0x3D05, "StopLSS_FS_TIMER", id);\
+d->lss_transfer.timerFS = DelAlarm(d->lss_transfer.timerFS);}
 #define StartLSS_FS_TIMER(){\
-MSG_WAR(0x3D04, "StartLSS_TIMER for FS_TIMER",0);\
+MSG_WAR(0x3D06, "StartLSS_FS_TIMER",0);\
-d->lss_transfer.timers[LSS_FS_TIMER] = SetAlarm(d,LSS_FS_TIMER,&LssAlarm,MS_TO_TIMEVAL(LSS_FS_TIMEOUT_MS),0);}
+d->lss_transfer.timerFS = SetAlarm(d,0,&LssAlarmFS,MS_TO_TIMEVAL(LSS_FS_TIMEOUT_MS),0);}
+#endif
-UNS8 sendMasterLSSMessage(CO_Data* d, UNS8 command,void *dat1,void *dat2);
+void LssAlarmMSG(CO_Data* d, UNS32 id)
+{
+	StopLSS_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(0x1D07, "LSS FastScan timeout. FastScan_SM inconsisten state.", d->lss_transfer.FastScan_SM);
+		}
+	}
+	else
+#endif
+	if(d->lss_transfer.command==LSS_IDENT_REMOTE_NON_CONF){
+			MSG_WAR(0x2D08, "LSS timeout. There are not no-configured slaves in the net", 0);
+			d->lss_transfer.state = LSS_FINISHED;
+			d->lss_transfer.dat1=1;
+	}
+	else{
+		MSG_ERR(0x1D09, "LSS timeout. LSS response not received.", 0);
+	MSG_WAR(0x2D0A, "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*/
+	(*d->lss_transfer.Callback)(d,d->lss_transfer.command);
+}
+}
 /*!
 **
 **
 ** @param d
 ** @param id
 **/
-//struct timeval current_time,init_time;
+void LssAlarmSDELAY(CO_Data* d, UNS32 id)
-void LssAlarm(CO_Data* d, UNS32 id)
 {
-	/*unsigned long time_period;
+	/* The first switch_delay period expired. Store the node state, change it
-	gettimeofday(&current_time,NULL);
+	 * so no CAN messages will be sent or received, call the ChangeBaudRate function*/
-	time_period=(current_time.tv_sec - init_time.tv_sec)* 1000000 + current_time.tv_usec - init_time.tv_usec;
+	if(d->lss_transfer.switchDelayState==SDELAY_FIRST){
-	printf("%3ld.%3ld.%3ld --",time_period/1000000,(time_period%1000000)/1000,time_period%1000);*/
+		MSG_WAR(0x3D0B, "LSS switch delay first period expired",0);
+	d->lss_transfer.switchDelayState=SDELAY_SECOND;
-	switch(id){
+	(*d->lss_ChangeBaudRate)(d,d->lss_transfer.baudRate);
-	case LSS_MSG_TIMER:
+}
-	 	StopLSS_TIMER(LSS_MSG_TIMER);
+else{ /* d->lss_transfer.switchDelayState==SDELAY_SECOND */
+	MSG_WAR(0x3D0C, "LSS switch delay second period expired",0);
+	d->lss_transfer.switchDelayState=SDELAY_OFF;
+	StopLSS_SDELAY_TIMER();
+	if (*(d->iam_a_slave))
+		d->canHandle=d->lss_transfer.canHandle_t;
+	else{
+		d->lss_transfer.dat1=0;
+		d->lss_transfer.state=LSS_FINISHED;
+		/* Call the user function */
+		if(d->lss_transfer.Callback){
+	    		(*d->lss_transfer.Callback)(d,d->lss_transfer.command);
+		}
+	}
+}
+}
 #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){
+** @param d
-					d->lss_transfer.LSSanswer=0;
+** @param id
-					d->lss_transfer.BitChecked=31;
+**/
-					d->lss_transfer.IDNumber=0;
+void LssAlarmFS(CO_Data* d, UNS32 id)
-					d->lss_transfer.FastScan_SM=LSS_FS_PROCESSING;
+{
-					StartLSS_FS_TIMER();
+	StopLSS_FS_TIMER();
-					sendMasterLSSMessage(d,LSS_IDENT_FASTSCAN,0,0);
-					return;
+	switch(d->lss_transfer.FastScan_SM){
-				}
+	case LSS_FS_RESET:
-				else{
+	{
+	   	/* This should not happen, an error ocurred*/
-				d->lss_transfer.state = LSS_FINISHED;
+		MSG_ERR(0x1D0D, "LSS FastScan timeout. FastScan_SM inconsisten state.", d->lss_transfer.FastScan_SM);
-				/* Inform the application that there aren't not configured nodes in the net  */
+	}
-				d->lss_transfer.dat1=1;
+	break;
-				}
+	case LSS_FS_PROCESSING:
-			}
+	{
-			else{
+		/* If there isn't any answer, set the bit */
-				/* This should not happen, an error ocurred*/
+		if(d->lss_transfer.LSSanswer==0){
-				MSG_ERR(0x1D05, "LSS FastScan timeout. FastScan_SM inconsisten state.", d->lss_transfer.FastScan_SM);
+			UNS32 Mask=0x1;
-			}
+			Mask<<=d->lss_transfer.BitChecked;
-		}
+			d->lss_transfer.IDNumber|=Mask;
-		else
+		}
+		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(0x1D0E, "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);
+	}
+}
 #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*/
-		(*d->lss_transfer.Callback)(d,d->lss_transfer.command);
-	}
-break;
-case LSS_SWITCH_DELAY_TIMER:
-		/* 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(0x3D08, "LSS switch delay first period expired",0);
-		d->lss_transfer.switchDelayState=SDELAY_SECOND;
-		(*d->lss_ChangeBaudRate)(d->lss_transfer.baudRate);
-	}
-	else{ /* d->lss_transfer.switchDelayState==SDELAY_SECOND */
-		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
-	}
-}
 /*!
 **
 **
 ** @param d
 **/
 {
 Message m;
 UNS8 i;
 if (!d->CurrentCommunicationState.csLSS){
-	MSG_WAR(0x2D0C, "unable to send the LSS message, not in the proper state =>", d->nodeState);
+	MSG_WAR(0x2D17, "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;
 m.len = 8;
 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(0x1D0D, "send Slave LSS command not implemented", command);
+	MSG_ERR(0x1D18, "send Slave LSS command not implemented", command);
 	return 0xFF;
 }
 return canSend(d->canHandle,&m);
 }
 **/
 UNS8 sendMasterLSSMessage(CO_Data* d, UNS8 command,void *dat1,void *dat2)
 {
 Message m;
 UNS8 i;
+UNS8 res;
+UNS8 hasResponse=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=UNS16_LE(MLSS_ADRESS);
 /* Tha data sent with the msg depends on the command */
 switch(command){
+case LSS_CONF_NODE_ID: /* Configure Node-ID */
+	hasResponse=1;
 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;
+	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(0x1D19, "Master-> Baud rate not supported",0);
+				d->lss_transfer.dat1=0xFF;
+				goto ErrorBitRateMaster;
+				break;
+			}
+		hasResponse=1;
+		break;
+	}
+	else{
+		MSG_ERR(0x1D1A, "Master-> Bit timing not supported",0);
+		d->lss_transfer.dat1=0x01;
+	}
+ErrorBitRateMaster:
+	d->lss_transfer.dat2=0;
+	/* If there is a callback, it is responsible of the error */
+	if(d->lss_transfer.Callback)
+	(*d->lss_transfer.Callback)(d,d->lss_transfer.command);
+	return 0xFF;
 	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);
+	if(d->lss_transfer.baudRate!="none"){
+		d->lss_transfer.switchDelay=(UNS16)(*(UNS32*)dat1 & 0xFFFF);
+		d->lss_transfer.switchDelayState=SDELAY_FIRST;
+		res=canSend(d->canHandle,&m);
+		if(res==0){
+			StartLSS_SDELAY_TIMER();
+			d->lss_transfer.state=LSS_TRANS_IN_PROGRESS;
+		}
+		return res;
+	}
+	else{
+		MSG_ERR(0x1D1B, "Master-> Baud rate not specified",0);
+		d->lss_transfer.dat1=1;
+	/* If there is a callback, it is responsible of the error */
+	if(d->lss_transfer.Callback){
+	    	(*d->lss_transfer.Callback)(d,d->lss_transfer.command);
+	}
+		return 0xFF;
+	}
 	break;
+case LSS_SM_SELECTIVE_SERIAL:
+case LSS_IDENT_REMOTE_SERIAL_HIGH:
+	hasResponse=1;
 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 */
+	 hasResponse=1;
 	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);
 		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;
+		/* it will generate a response only if it is the start of the FastScan protocol*/
+		if(d->lss_transfer.FastScan_SM==LSS_FS_RESET)hasResponse=1;
 	break;
 #endif
 default:
-	MSG_ERR(0x1D0E, "send Master LSS command not implemented", command);
+	MSG_ERR(0x1D1C, "send Master LSS command not implemented", command);
 	return 0xFF;
 }
-return canSend(d->canHandle,&m);
+res=canSend(d->canHandle,&m);
+if(res==0 && hasResponse==1){
+	StartLSS_MSG_TIMER();
+	d->lss_transfer.state=LSS_TRANS_IN_PROGRESS;
+}
+return res;
 }
 /*!
 **
 **
 		//MSG_WAR(0x3D0D, "MasterLSS proceedLSS; unexpected message arrived;command ", m->data[0]);
 		//return 0;
 		goto ErrorProcessMaster;
 	}
-	//#ifdef CO_ENABLE_LSS_FS
+	MSG_WAR(0x3D1E, "MasterLSS proceedLSS; command ", m->data[0]);
-	/* 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_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(0x1D10, "Master LSS command not implemented", msg_cs);
+			MSG_ERR(0x1D1F, "Master LSS command not implemented", msg_cs);
 			return 0xFF;
 	}
-	StopLSS_TIMER(LSS_MSG_TIMER);
+	StopLSS_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 */
 	(*d->lss_transfer.Callback)(d,d->lss_transfer.command);
 return 0;
 ErrorProcessMaster:
-MSG_WAR(0x3D11, "MasterLSS proceedLSS; unexpected message arrived;command ", m->data[0]);
+MSG_WAR(0x3D20, "MasterLSS proceedLSS; unexpected message arrived;command ", m->data[0]);
 	return 0xFF;
 }
 /*!
 **/
 UNS8 proceedLSS_Slave(CO_Data* d, Message* m )
 {
 	UNS8 msg_cs;
-	MSG_WAR(0x3D12, "SlaveLSS proceedLSS; command ", m->data[0]);
+	MSG_WAR(0x3D21, "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(0x3D13, "SlaveLSS already in the mode ", m->data[1]);
+			MSG_WAR(0x3D22, "SlaveLSS already in the mode ", m->data[1]);
 			break;
 		}
 		if(m->data[1]==LSS_CONFIGURATION_MODE)	{
-			MSG_WAR(0x3D14, "SlaveLSS switching to configuration mode ", 0);
+			MSG_WAR(0x3D23, "SlaveLSS switching to configuration mode ", 0);
 			/* Store the NodeId in case it will be changed */
-			d->lss_transfer.nodeID=getNodeId(d);
+			//d->lss_transfer.nodeID=getNodeId(d);
 			d->lss_transfer.mode=LSS_CONFIGURATION_MODE;
 		}
 		else if(m->data[1]==LSS_WAITING_MODE){
-			MSG_WAR(0x3D15, "SlaveLSS switching to operational mode ", 0);
+			MSG_WAR(0x3D24, "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 the nodeID has changed update it and put the node state to Initialisation. */
+			if(d->lss_transfer.nodeID!=getNodeId(d)){
-				if(d->lss_transfer.nodeID!=getNodeId(d)){
+				if(getNodeId(d)==0xFF){/* The nodeID was 0xFF; initialize the application*/
-					MSG_WAR(0x3D16, "The node Id has changed. Reseting to Initialisation state",0);
+					MSG_WAR(0x3D25, "The node Id has changed. Reseting to Initialisation state",0);
 					setNodeId(d, d->lss_transfer.nodeID);
 					setState(d, Initialisation);
+				}
+				else{/* The nodeID will be changed on NMT_Reset Request*/
+					setNodeId(d, d->lss_transfer.nodeID);
 				}
 			}
 			d->lss_transfer.mode=LSS_WAITING_MODE;
 		}
 	break;
 		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(0x1D17, "NodeID out of range",0);
+				MSG_ERR(0x1D26, "NodeID out of range",0);
 				error_code=1; /* NodeID out of range */
 			}
 			else{
 				d->lss_transfer.nodeID=m->data[1];
 			}
 		}
 		else{
-			MSG_WAR(0x3D18, "SlaveLSS not in configuration mode",0);
+			MSG_WAR(0x3D27, "SlaveLSS not in configuration mode",0);
 			//error_code=0xFF;
 			break;
 		}
 		sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error);
 	}
 				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(0x1D19, "Baud rate not supported",0);
+					MSG_ERR(0x1D28, "Baud rate not supported",0);
 					error_code=0xFF; /* Baud rate not supported*/
 					break;
 				}
 			}
 			else
 			{
-				MSG_ERR(0x1D1A, "Bit timing not supported",0);
+				MSG_ERR(0x1D29, "Bit timing not supported",0);
 				error_code=0x01; /* bit timing not supported */
 			}
 		}
 		else{
-			MSG_WAR(0x3D1B, "SlaveLSS not in configuration mode",0);
+			MSG_WAR(0x3D2A, "SlaveLSS not in configuration mode",0);
 			//error_code=0xFF;
 			break;
 		}
 		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(0x3D1C, "SlaveLSS not in configuration mode",0);
+			MSG_ERR(0x3D2B, "SlaveLSS not in configuration mode",0);
 			break;
 		}
 		if(d->lss_transfer.baudRate!="none"){
 			d->lss_transfer.switchDelay=getLSSDelay(m);
-			MSG_WAR(0x3D1D, "Slave Switch Delay set to: ",d->lss_transfer.switchDelay);
+			MSG_WAR(0x3D2C, "Slave Switch Delay set to: ",d->lss_transfer.switchDelay);
 			d->lss_transfer.switchDelayState=SDELAY_FIRST;
-			d->lss_transfer.currentState=getState(d);
+			//d->lss_transfer.currentState=getState(d);
-			setState(d, LssTimingDelay);
+			//setState(d, LssTimingDelay);
+			d->lss_transfer.canHandle_t=d->canHandle;
+			d->canHandle=NULL;
 			StartLSS_SDELAY_TIMER();
 		}
 	break;
 	case LSS_CONF_STORE: /* Store Configured Parameters */
 	{
 		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);
+	  			(*d->lss_StoreConfiguration)(d,&error_code,&spec_error);
 			}
 			else{
-				MSG_ERR(0x1D1E, "Store configuration not supported",0);
+				MSG_ERR(0x1D2E, "Store configuration not supported",0);
 				error_code=1; /* store configuration is not supported */
 			}
 		}
 		else{
-			MSG_WAR(0x3D1F, "SlaveLSS not in configuration mode",0);
+			MSG_WAR(0x3D2F, "SlaveLSS not in configuration mode",0);
 			//error_code=0xFF;
 			break;
 		}
 		sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error);
 	}
 		ODCallback_t *Callback;
 		UNS32 _SpecificNodeInfo;
 		if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE)
 		{
-			MSG_ERR(0x1D20, "Switch Mode Selective only supported in operational mode",0);
+			MSG_ERR(0x1D30, "Switch Mode Selective only supported in operational mode",0);
 			break;
 		}
 		_SpecificNodeInfo=getLSSIdent(m);
 			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(0x3D21, "SlaveLSS switching to configuration mode ", 0);
+				MSG_WAR(0x3D31, "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(0x3D22, "LSS identity field doesn't match ", _SpecificNodeInfo);
+			MSG_WAR(0x3D32, "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_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(0x3D23, "SlaveLSS identified ", 0);
+			MSG_WAR(0x3D33, "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(0x3D24, "LSS identify field doesn't match ", _SpecificNodeInfo);
+			MSG_WAR(0x3D34, "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(0x3D25, "SlaveLSS non-configured ", 0);
+			MSG_WAR(0x3D35, "SlaveLSS non-configured ", 0);
 			sendSlaveLSSMessage(d,LSS_IDENT_NON_CONF_SLAVE,0,0);
 		}
 		else{
-			MSG_WAR(0x3D26, "SlaveLSS already configured ", 0);
+			MSG_WAR(0x3D36, "SlaveLSS already configured ", 0);
 		}
 	}
 	break;
 	case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */
 	case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */
 		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(0x3D27, "SlaveLSS identity field inquired ", _SpecificNodeInfo);
+		MSG_WAR(0x3D37, "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(0x3D28, "SlaveLSS Node ID inquired ", NodeID);
+			MSG_WAR(0x3D38, "SlaveLSS Node ID inquired ", NodeID);
 			sendSlaveLSSMessage(d,msg_cs,&NodeID,0);
 		}
 		else{
-			MSG_WAR(0x3D29, "SlaveLSS not in configuration mode",0);
+			MSG_WAR(0x3D39, "SlaveLSS not in configuration mode",0);
 		}
 	break;
 #ifdef CO_ENABLE_LSS_FS
 	case LSS_IDENT_FASTSCAN:
 	{
 		{
 			UNS32 errorCode;
 			const indextable *ptrTable;
 			ODCallback_t *Callback;
-			MSG_WAR(0x3D2A, "SlaveLSS Reseting LSSPos", 0);
+			MSG_WAR(0x3D3A, "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;
 		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(0x3D3B, "SlaveLSS FastScan IDNumber", getLSSIdent(m));
-				MSG_WAR(0x3D2C, "SlaveLSS FastScan BitMask ", Mask);
+				MSG_WAR(0x3D3C, "SlaveLSS FastScan BitMask ", Mask);
-				MSG_WAR(0x3D2D, "SlaveLSS FastScan LSS-ID  ", d->lss_transfer.IDNumber);
+				MSG_WAR(0x3D3D, "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(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);
+					MSG_WAR(0x3D3E, "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);
+						MSG_WAR(0x3D3F, "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;
 					}
 		}
 	}
 	break;
 #endif
 	default:
-		MSG_ERR(0x1D30, "SlaveLSS command not implemented", msg_cs);
+		MSG_ERR(0x1D40, "SlaveLSS command not implemented", msg_cs);
 		return 0xFF;
 	}
 return 0;
 }
-UNS8 configNetworkNode(CO_Data* d, UNS8 command, void *dat1, void* dat2)
+/*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)
+UNS8 configNetworkNode (CO_Data* d, UNS8 command, void *dat1, void* dat2, LSSCallback_t Callback)
 {
-	d->lss_transfer.state=LSS_TRANS_IN_PROGRESS;
+	//d->lss_transfer.state=LSS_TRANS_IN_PROGRESS;
 	d->lss_transfer.Callback=Callback;
 	d->lss_transfer.command=command;
-	StopLSS_TIMER(LSS_MSG_TIMER);
+	StopLSS_MSG_TIMER();
-	StartLSS_MSG_TIMER();
+	//StartLSS_MSG_TIMER();
 	return sendMasterLSSMessage(d,command,dat1,dat2);
 }
 UNS8 getConfigResultNetworkNode (CO_Data* d, UNS8 command, UNS32* dat1, UNS8* dat2)

changeset 381	854c43cdc24a
parent 370	6fecf36df407
child 384	83793fc7ce48