Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
authorMongo
Thu, 13 Oct 2011 17:51:27 +0200
changeset 669 50da44ebaf00
parent 668 ba53613e94e4
child 670 e37511baf56b
Configuration manager with DCF in object 0x1F22 rewritten almost from scratch.
1. The boot-up message from a device starts a verification of entries
2. If all entries matches the node is started
3. If an entry differs the whole dcf is written and a save is done
4. A reset is send to the node
5. If several boot-up are received at the same time they will be managed one
by one thus only one free sdo client is needed for the whole process.
include/data.h
include/dcf.h
src/dcf.c
src/lifegrd.c
src/sdo.c
--- a/include/data.h	Mon Sep 12 10:52:02 2011 +0200
+++ b/include/data.h	Thu Oct 13 17:51:27 2011 +0200
@@ -107,7 +107,9 @@
     const indextable* dcf_odentry;
 	UNS8* dcf_cursor;
 	UNS32 dcf_entries_count;
-	UNS8 dcf_request;
+	UNS8 dcf_status;
+    UNS32 dcf_size;
+    UNS8* dcf_data;
 	
 	/* EMCY */
 	e_errorState error_state;
@@ -297,7 +299,9 @@
     NULL,       /*dcf_odentry*/\
 	NULL,		/*dcf_cursor*/\
 	1,		/*dcf_entries_count*/\
-	0,		/* dcf_request*/\
+	0,		/* dcf_status*/\
+    0,      /* dcf_size */\
+    NULL,   /* dcf_data */\
 	\
 	/* EMCY */\
 	Error_free,                      /* error_state */\
--- a/include/dcf.h	Mon Sep 12 10:52:02 2011 +0200
+++ b/include/dcf.h	Thu Oct 13 17:51:27 2011 +0200
@@ -19,11 +19,33 @@
 License along with this library; if not, write to the Free Software
 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */
+#include "data.h"
+
+#define DCF_STATUS_INIT         0
+#define DCF_STATUS_READ_CHECK   1
+#define DCF_STATUS_WRITE        2
+#define DCF_STATUS_SAVED        3
+#define DCF_STATUS_VERIF_OK     4
 
 /** 
- * @brief Send the consise dcf to node corresping to nodeId
+ * @brief Init the consise dcf in CO_Data for nodeId
+ *
  * @param *d Pointer on a CAN object data structure
  * @param nodeId Id of the slave node
- * @return
+ * @return 1: dcf check started
+ *         0: nothing to do   
  */
-UNS8 send_consise_dcf(CO_Data* d, UNS8 nodeId);
+UNS8 init_consise_dcf(CO_Data* d, UNS8 nodeId);
+
+/**
+ * @brief Function to be called from post_SlaveBootup 
+ * for starting the configuration manager
+ *
+ * @param *d Pointer on a CAN object data structure
+ * @param nodeId Id of the slave node
+ * @return 0: configuration manager busy
+ *         1: nothing to check, node started
+ *         2: dcf check started
+*/
+UNS8 check_and_start_node(CO_Data* d, UNS8 nodeId);
+
--- a/src/dcf.c	Mon Sep 12 10:52:02 2011 +0200
+++ b/src/dcf.c	Thu Oct 13 17:51:27 2011 +0200
@@ -36,167 +36,233 @@
 
 #include "data.h"
 #include "sysdep.h"
-
-extern UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
-                               UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize);
-
-
-static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId);
-
-/* Seek to next NodeID's DCF */
-#define SEEK_NEXT_DCF() \
-   	nodeId=(nodeId+1) % d->dcf_odentry->bSubCount; \
-   	if(nodeId==0) nodeId=1; \
-   	d->dcf_cursor = NULL;
-
-/**
-**
+#include "dcf.h"
+
+typedef struct {
+    UNS16 Index;
+    UNS8 Subindex;
+    UNS32 Size;
+    UNS8 *Data;
+} dcf_entry_t;
+
+void SaveNode(CO_Data* d, UNS8 nodeId);
+static UNS8 read_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId);
+static UNS8 write_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId);
+UNS8 init_consise_dcf(CO_Data* d,UNS8 nodeId);
+
+/**
+** @brief Function to be called from post_SlaveBootup
 **
 ** @param d
 ** @param nodeId
 */
-static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
-{
-  UNS32 abortCode = 0;
-
-  if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
-    {
-      MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode);
-      MSG_WAR(0x2A02, "server node : ", nodeId);
-    }
-
-  closeSDOtransfer(d, nodeId, SDO_CLIENT);
-  /* Timedout ? */
-  if(abortCode == SDOABT_TIMED_OUT){
-    /* Node may not be ready, try another one */
-    /* Warning, this might leed to endless attempts */
-    /* if node does never answer */
-	SEEK_NEXT_DCF()
-  }
-  send_consise_dcf_loop(d,nodeId);
-}
-
+UNS8 check_and_start_node(CO_Data* d, UNS8 nodeId)
+{   
+    if(d->dcf_status != DCF_STATUS_INIT)
+        return 0;
+    if((init_consise_dcf(d, nodeId) == 0) || (read_consise_dcf_next_entry(d, nodeId) == 0)){
+	    /* Ask slave node to go in operational mode */
+        masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
+        d->NMTable[nodeId] = Operational;
+        return 1;
+    }
+    d->dcf_status = DCF_STATUS_READ_CHECK;
+    return 2;
+}
 
 /**
 **
 **
 ** @param d
 ** @param nodeId
+*/
+static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
+{
+    UNS32 abortCode = 0;
+    UNS8 buf[4], match = 0, node;
+    UNS32 size=4;
+    if(d->dcf_status == DCF_STATUS_READ_CHECK){
+        // printf("DCF_STATUS_READ_CHECK \n");
+        if(getReadResultNetworkDict (d, nodeId, buf, &size, &abortCode) != SDO_FINISHED)
+            goto dcferror;
+        /* Check if data received match the DCF */
+        if(size == d->dcf_size){
+            match = 1;
+            while(--size)
+                if(buf[size] != d->dcf_data[size])
+                    match = 0;
+        }
+        if(match) {
+            if(read_consise_dcf_next_entry(d, nodeId) == 0){
+                masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
+                d->NMTable[nodeId] = Operational;
+                d->dcf_status = DCF_STATUS_INIT;
+                /* Look for other nodes waiting to be started */
+                for(node = 0 ; node<NMT_MAX_NODE_ID ; node++){
+                    if(d->NMTable[node] != Initialisation)
+                        continue;
+                    if(check_and_start_node(d, node) == 2)
+                        break;
+                }
+            }
+        }
+        else { /* Data received does not match : start rewriting all */
+            if((init_consise_dcf(d, nodeId) == 0) || (write_consise_dcf_next_entry(d, nodeId) == 0))
+                goto dcferror;                
+            d->dcf_status = DCF_STATUS_WRITE;
+        }
+    }
+    else if(d->dcf_status == DCF_STATUS_WRITE){
+        // printf("DCF_STATUS_WRITE \n");
+        if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
+            goto dcferror;
+        if(write_consise_dcf_next_entry(d, nodeId) == 0){
+            SaveNode(d, nodeId);
+            d->dcf_status = DCF_STATUS_SAVED;
+        }
+    }
+    else if(d->dcf_status == DCF_STATUS_SAVED){
+        // printf("DCF_STATUS_SAVED \n");
+        if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
+            goto dcferror;
+        masterSendNMTstateChange (d, nodeId, NMT_Reset_Node);
+        d->dcf_status = DCF_STATUS_INIT;
+        d->NMTable[nodeId] = Unknown_state;
+    }
+    return;
+dcferror:
+    MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode);
+    MSG_WAR(0x2A02, "server node : ", nodeId);
+    d->NMTable[nodeId] = Unknown_state;
+}
+
+/**
+**
+**
+** @param d
+** @param nodeId
 **
 ** @return
 */
-UNS8 send_consise_dcf(CO_Data* d,UNS8 nodeId)
-{
-  UNS32 szData;
-  /* Fetch DCF OD entry, if not already done */
-  if(!d->dcf_odentry)
-  {
+UNS8 init_consise_dcf(CO_Data* d,UNS8 nodeId)
+{
+    /* Fetch DCF OD entry */
     UNS32 errorCode;
     ODCallback_t *Callback;
     d->dcf_odentry = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback);
     /* If DCF entry do not exist... Nothing to do.*/
     if (errorCode != OD_SUCCESSFUL) goto DCF_finish;
+    /* Fix DCF table overflow */
+    if(nodeId > d->dcf_odentry->bSubCount) goto DCF_finish;
+    /* If DCF empty... Nothing to do */
+    if(! d->dcf_odentry->pSubindex[nodeId].size) goto DCF_finish;
+    UNS8* dcf = (UNS8*)d->dcf_odentry->pSubindex[nodeId].pObject;
+    // printf("%.2x %.2x %.2x %.2x\n",dcf[0],dcf[1],dcf[2],dcf[3]);
+    d->dcf_cursor = dcf + 4;
+    d->dcf_entries_count = 0;
+    d->dcf_status = DCF_STATUS_INIT;
+    return 1;
+    DCF_finish:
+    return 0;
+}
+
+UNS8 get_next_DCF_data(CO_Data* d, dcf_entry_t *dcf_entry, UNS8 nodeId)
+{
+  UNS8* dcfend;
+  UNS32 nb_entries;
+  UNS32 szData;
+  if(!d->dcf_odentry)
+     return 0;
+  if(nodeId > d->dcf_odentry->bSubCount)
+     return 0;
+  szData = d->dcf_odentry->pSubindex[nodeId].size;
+  UNS8* dcf = (UNS8*)d->dcf_odentry->pSubindex[nodeId].pObject;
+  nb_entries = UNS32_LE(*((UNS32*)dcf));
+  dcfend = dcf + szData;
+  if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries){
+    /* DCF data may not be 32/16b aligned, 
+    * we cannot directly dereference d->dcf_cursor 
+    * as UNS16 or UNS32 
+    * Do it byte per byte taking care on endianess*/
+#ifdef CANOPEN_BIG_ENDIAN
+    dcf_entry->Index = *(d->dcf_cursor++) << 8 | 
+     	               *(d->dcf_cursor++);
+#else
+    memcpy(&dcf_entry->Index, d->dcf_cursor,2);
+       	d->dcf_cursor+=2;
+#endif
+    dcf_entry->Subindex = *(d->dcf_cursor++);
+#ifdef CANOPEN_BIG_ENDIAN
+    dcf_entry->Size = *(d->dcf_cursor++) << 24 | 
+     	              *(d->dcf_cursor++) << 16 | 
+        	          *(d->dcf_cursor++) << 8 | 
+        	          *(d->dcf_cursor++);
+#else
+    memcpy(&dcf_entry->Size, d->dcf_cursor,4);
+    d->dcf_cursor+=4;
+#endif
+    d->dcf_data = dcf_entry->Data = d->dcf_cursor;
+    d->dcf_size = dcf_entry->Size;
+    d->dcf_cursor += dcf_entry->Size;
+    d->dcf_entries_count++;
+    return 1;
   }
-  if(d->dcf_odentry->bSubCount<=nodeId) goto DCF_finish; /* Fix DCF table overflow */
-  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.*/
-  	if(d->dcf_odentry->pSubindex[nodeId].bAccessType & DCF_TO_SEND) return 1;
-  	
-  	d->dcf_odentry->pSubindex[nodeId].bAccessType|=DCF_TO_SEND;
-  	d->dcf_request++;
-  	if(d->dcf_request==1)
-  		send_consise_dcf_loop(d,nodeId);
-  	return 1;
-  }
-  
-  DCF_finish:
   return 0;
 }
 
-static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId)
-{
-  if(nodeId > d->dcf_odentry->bSubCount) return;
-/* Loop on all DCF subindexes, corresponding to node ID until there is no request*/
-  //while (nodeId < d->dcf_odentry->bSubCount){
-  while (d->dcf_request>0){
-  	if(d->dcf_odentry->pSubindex[nodeId].bAccessType & DCF_TO_SEND){   	 
-        UNS8* dcfend;
-  		UNS32 nb_entries;
-  		UNS32 szData = d->dcf_odentry->pSubindex[nodeId].size;
-      	 
-   		{
-	   		UNS8* dcf = *((UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject);
-   			dcfend = dcf + szData;
-	   		if (!d->dcf_cursor){
-    	  		d->dcf_cursor = (UNS8*)dcf + 4;
-       			d->dcf_entries_count = 0;
-   			}
-   			nb_entries = UNS32_LE(*((UNS32*)dcf));
-   		}
-
-    	/* condition on consise DCF string for NodeID, if big enough */
-    	if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries){
-    	
-        	UNS16 target_Index;
-        	UNS8 target_Subindex;
-        	UNS32 target_Size;
-
-			/* DCF data may not be 32/16b aligned, 
-			 * we cannot directly dereference d->dcf_cursor 
-			 * as UNS16 or UNS32 
-			 * Do it byte per byte taking care on endianess*/
-#ifdef CANOPEN_BIG_ENDIAN
-        	target_Index = *(d->dcf_cursor++) << 8 | 
-        	               *(d->dcf_cursor++);
-#else
-        	memcpy(&target_Index, d->dcf_cursor,2);
-        	d->dcf_cursor+=2;
-#endif
-
-        	target_Subindex = *(d->dcf_cursor++);
-
-#ifdef CANOPEN_BIG_ENDIAN
-        	target_Size = *(d->dcf_cursor++) << 24 | 
-        	              *(d->dcf_cursor++) << 16 | 
-        	              *(d->dcf_cursor++) << 8 | 
-        	              *(d->dcf_cursor++);
-#else
-        	memcpy(&target_Size, d->dcf_cursor,4);
-        	d->dcf_cursor+=4;
-#endif
-	
-    	    _writeNetworkDict(d, /* CO_Data* d*/
-                                nodeId, /* UNS8 nodeId*/
-                                target_Index, /* UNS16 index*/
-                                target_Subindex, /* UNS8 subindex*/
-                                (UNS8)target_Size, /* UNS8 count*/
-                                0, /* UNS8 dataType*/
-                                d->dcf_cursor,/* void *data*/
-                                CheckSDOAndContinue,/* SDOCallback_t
-                                                      Callback*/
-                                0); /* no endianize*/
-        	/* Push d->dcf_cursor to the end of data*/
-
-        	d->dcf_cursor += target_Size;
-        	d->dcf_entries_count++;
-
-        	/* send_consise_dcf_loop will be called by CheckSDOAndContinue for next DCF entry*/
-        	return;
-      	}
-      	else
-      	{
-      		/* We have finished with the dcf entry. Change the flag, decrement the request
-      		 *  and execute the bootup callback. */
-      		d->dcf_odentry->pSubindex[nodeId].bAccessType&=~DCF_TO_SEND;
-      		d->dcf_request--;
-      		(*d->post_SlaveBootup)(d, nodeId);
-      	}
- 	}
- 	
-	SEEK_NEXT_DCF()
-  }
-
-}
+static UNS8 write_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId)
+{
+    UNS8 Ret;
+    dcf_entry_t dcf_entry;
+    if(!get_next_DCF_data(d, &dcf_entry, nodeId))
+        return 0;
+    Ret = writeNetworkDictCallBackAI(d, /* CO_Data* d*/
+                    nodeId, /* UNS8 nodeId*/
+                    dcf_entry.Index, /* UNS16 index*/
+                    dcf_entry.Subindex, /* UNS8 subindex*/
+                    (UNS8)dcf_entry.Size, /* UNS8 count*/
+                    0, /* UNS8 dataType*/
+                    dcf_entry.Data,/* void *data*/
+                    CheckSDOAndContinue,/* Callback*/
+                    0,   /* no endianize*/
+                    0); /* no block mode */
+    if(Ret)
+        MSG_ERR(0x1A02,"Erreur writeNetworkDictCallBackAI",Ret);
+    return 1;
+}
+
+static UNS8 read_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId)
+{
+    UNS8 Ret;
+    dcf_entry_t dcf_entry;
+    if(!get_next_DCF_data(d, &dcf_entry, nodeId))
+        return 0;
+    Ret = readNetworkDictCallbackAI(d, /* CO_Data* d*/
+                   nodeId, /* UNS8 nodeId*/
+                   dcf_entry.Index, /* UNS16 index*/
+                   dcf_entry.Subindex, /* UNS8 subindex*/
+                   0, /* UNS8 dataType*/
+                   CheckSDOAndContinue,/* Callback*/
+                   0); /* no block mode */
+    if(Ret)
+        MSG_ERR(0x1A03,"Erreur readNetworkDictCallbackAI",Ret);
+    return 1;
+}
+
+void SaveNode(CO_Data* d, UNS8 nodeId)
+{
+    UNS8 Ret;
+    UNS32 data=0x65766173;
+    Ret = writeNetworkDictCallBackAI(d, /* CO_Data* d*/
+                    nodeId, /* UNS8 nodeId*/
+                    0x1010, /* UNS16 index*/
+                    1, /* UNS8 subindex*/
+                    4, /* UNS8 count*/
+                    0, /* UNS8 dataType*/
+                    (void *)&data,/* void *data*/
+                    CheckSDOAndContinue,/* Callback*/
+                    0,   /* no endianize*/
+                    0); /* no block mode */
+    if(Ret)
+        MSG_ERR(0x1A04,"Erreur writeNetworkDictCallBackAI",Ret);
+}
--- a/src/lifegrd.c	Mon Sep 12 10:52:02 2011 +0200
+++ b/src/lifegrd.c	Thu Oct 13 17:51:27 2011 +0200
@@ -142,23 +142,16 @@
 
       /* Boot-Up frame reception */
       if ( d->NMTable[nodeId] == Initialisation)
-        {
+      {
           /*
           ** The device send the boot-up message (Initialisation)
           ** to indicate the master that it is entered in
           ** pre_operational mode
-          ** Because the  device enter automaticaly in pre_operational
-          ** mode,
-          ** the pre_operational mode is stored
-          ** NMTable[bus_id][nodeId] = Pre_operational
           */
           MSG_WAR(0x3100, "The NMT is a bootup from node : ", nodeId);
-          
-          if(!send_consise_dcf(d,nodeId)){
-             /* call post SlaveBootup with NodeId */
-		  	(*d->post_SlaveBootup)(d, nodeId);
-          }
-        }
+          /* call post SlaveBootup with NodeId */
+		  (*d->post_SlaveBootup)(d, nodeId);
+      }
 
       if( d->NMTable[nodeId] != Unknown_state ) {
         UNS8 index, ConsummerHeartBeat_nodeId ;
--- a/src/sdo.c	Mon Sep 12 10:52:02 2011 +0200
+++ b/src/sdo.c	Thu Oct 13 17:51:27 2011 +0200
@@ -48,6 +48,8 @@
 
 /*Internals prototypes*/
 
+UNS8 GetSDOClientFromNodeId( CO_Data* d, UNS8 nodeId );
+
 /*!
  ** Called by writeNetworkDict
  **
@@ -588,11 +590,16 @@
  **
  ** @return
  **/
-UNS8 closeSDOtransfer (CO_Data* d, UNS8 CliServNbr, UNS8 whoami)
+UNS8 closeSDOtransfer (CO_Data* d, UNS8 nodeId, UNS8 whoami)
 {
 	UNS8 err;
 	UNS8 line;
-	err = getSDOlineToClose(d, CliServNbr, whoami, &line);
+    UNS8 CliNbr;
+	/* First let's find the corresponding SDO client in our OD  */
+	CliNbr = GetSDOClientFromNodeId(d, nodeId);
+	if(CliNbr >= 0xFE)
+		return SDO_ABORTED_INTERNAL;
+    err = getSDOlineToClose(d, CliNbr, whoami, &line);
 	if (err) {
 		MSG_WAR(0x2A30, "No SDO communication to close", 0);
 		return 0xFF;
@@ -1889,6 +1896,8 @@
 		MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId);
 		return (0xFF);
 	}
+	else
+		MSG_WAR(0x3AE1, "Transmission on line : ", line);
     if(useBlockMode) {
 	    initSDOline(d, line, CliNbr, index, subIndex, SDO_BLOCK_DOWNLOAD_IN_PROGRESS);
 	    d->transfers[line].objsize = count;