Implemented retry on SDO timeout in dcf.c.

[CanFestival-3.git] / src / dcf.c

diff --git a/src/dcf.c b/src/dcf.c
index e876b5f0bd8571e7d30b4aaaddc10e5bd0b38e7d..90a8c19f035837925066d26076a59ac50cd64fcd 100644 (file)
--- a/src/dcf.c
+++ b/src/dcf.c
@@ -34,9 +34,7 @@
 */
 
 
-#include "objacces.h"
-#include "sdo.h"
-#include "dcf.h"
+#include "data.h"
 #include "sysdep.h"
 
 extern UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
@@ -45,6 +43,12 @@ extern UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
 
 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;
+
 /**
 **
 **
@@ -53,7 +57,7 @@ static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId);
 */
 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
 {
-  UNS32 abortCode;
+  UNS32 abortCode = 0;
 
   if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
     {
@@ -62,6 +66,13 @@ static void CheckSDOAndContinue(CO_Data* d, UNS8 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);
 }
 
@@ -76,6 +87,7 @@ static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
 */
 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 +98,8 @@ UNS8 send_consise_dcf(CO_Data* d,UNS8 nodeId)
     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.*/
@@ -109,9 +122,9 @@ static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId)
   //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;
-               UNS8 szData = d->dcf_odentry->pSubindex[nodeId].size;
-               UNS8* dcfend;
+               UNS8 szData = d->dcf_odentry->pSubindex[nodeId].size;
         
                {
                        UNS8* dcf = *((UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject);
@@ -130,10 +143,29 @@ static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId)
                UNS8 target_Subindex;
                UNS32 target_Size;
 
-               /* pointer to the DCF string for NodeID */
-               target_Index = UNS16_LE(*((UNS16*)(d->dcf_cursor))); d->dcf_cursor += 2;
-               target_Subindex = *((UNS8*)((UNS8*)d->dcf_cursor++));
-               target_Size = UNS32_LE(*((UNS32*)(d->dcf_cursor))); d->dcf_cursor += 4;
+                       /* 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*/
@@ -159,21 +191,11 @@ static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId)
                 *  and execute the bootup callback. */
                d->dcf_odentry->pSubindex[nodeId].bAccessType&=~DCF_TO_SEND;
                d->dcf_request--;
-               (*d->post_SlaveBootup)(nodeId);
+               (*d->post_SlaveBootup)(d, nodeId);
        }
        }
-       else
-       {
-               /* Check the next element*/
-               //nodeId++;
-               nodeId=(nodeId+1) % d->dcf_odentry->bSubCount;
-               if(nodeId==d->dcf_odentry->bSubCount)nodeId=1;
-               d->dcf_cursor = NULL;
-       }
+       
+       SEEK_NEXT_DCF()
   }
-  
-  //DCF_finish:
-  /*  Switch Master to preOperational state */
- 
-  //(*d->preOperational)();
+
 }