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 30f217a9ce411ba0e45718dc04bb4a0b45b5e301..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);
 }
 
@@ -132,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*/
@@ -161,20 +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);
        }
        }
        
-       /* 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)();
+
 }