# HG changeset patch
# User fbeaulier
# Date 1314629995 -7200
# Node ID a03f0aa7d219fda483ea369ad570950e0c9c8d18
# Parent  70fc3603e36f03f8101dd6253d791cff370f24b1
CHANGE: SDO block mode added, without CRC support
WARNING : API change in client functions

diff -r 70fc3603e36f -r a03f0aa7d219 configure
--- a/configure	Tue Aug 16 14:15:52 2011 +0200
+++ b/configure	Mon Aug 29 16:59:55 2011 +0200
@@ -32,6 +32,10 @@
 #For a normal transfert, (usually for a string), put the maximum string size to transfer.
 SDO_MAX_LENGTH_TRANSFERT=32
 
+# For block transfert, number of segments transmitted at once.
+# SDO_BLOCK_SIZE CAN frames must fit into the CAN Tx buffer
+SDO_BLOCK_SIZE=16
+
 # Number of SDO from differents nodes that the node can manage concurrently.
 #for a slave node, usually put 1.
 SDO_MAX_SIMULTANEOUS_TRANSFERTS=4
@@ -122,6 +126,7 @@
 	--debug=*)	DEBUG=$optarg;;
 	--MAX_CAN_BUS_ID=*)	MAX_CAN_BUS_ID=$optarg;;
 	--SDO_MAX_LENGTH_TRANSFERT=*)	SDO_MAX_LENGTH_TRANSFERT=$optarg;;
+	--SDO_BLOCK_SIZE=*)	SDO_BLOCK_SIZE=$optarg;;
 	--SDO_MAX_SIMULTANEOUS_TRANSFERTS=*)	SDO_MAX_SIMULTANEOUS_TRANSFERTS=$optarg;;
 	--NMT_MAX_NODE_ID=*)	NMT_MAX_NODE_ID=$optarg;;
 	--SDO_TIMEOUT_MS=*)	SDO_TIMEOUT_MS=$optarg;;
@@ -179,6 +184,7 @@
 		echo	"Stack compilation constants"
 		echo	" --MAX_CAN_BUS_ID [=1] Number of can bus to use"
 		echo	" --SDO_MAX_LENGTH_TRANSFERT [=32] max bytes to transmit by SDO"
+		echo	" --SDO_BLOCK_SIZE [=16] max CAN frames transmitted at once for block transfert"
 		echo	" --SDO_MAX_SIMULTANEOUS_TRANSFERTS [=4] Number of SDO that the node can manage concurrently"
 		echo	" --NMT_MAX_NODE_ID [=128] can be reduced to gain memory on small network"
 		echo	" --SDO_TIMEOUT_MS [=3000] Timeout in milliseconds for SDO (None to disable the feature)"
@@ -657,6 +663,7 @@
 for i in \
  MAX_CAN_BUS_ID\
  SDO_MAX_LENGTH_TRANSFERT\
+ SDO_BLOCK_SIZE\
  SDO_MAX_SIMULTANEOUS_TRANSFERTS\
  NMT_MAX_NODE_ID\
  SDO_TIMEOUT_MS\
diff -r 70fc3603e36f -r a03f0aa7d219 examples/CANOpenShell/CANOpenShell.c
--- a/examples/CANOpenShell/CANOpenShell.c	Tue Aug 16 14:15:52 2011 +0200
+++ b/examples/CANOpenShell/CANOpenShell.c	Mon Aug 29 16:59:55 2011 +0200
@@ -136,19 +136,19 @@
 				printf("##################################\n");
 				printf("#### Informations for node %x ####\n", nodeid);
 				printf("##################################\n");
-				readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1000, 0x00, 0, CheckReadInfoSDO);
+				readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1000, 0x00, 0, CheckReadInfoSDO, 0);
 				break;
 
 			case 2: /* Get Vendor ID */
-				readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1018, 0x01, 0, CheckReadInfoSDO);
+				readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1018, 0x01, 0, CheckReadInfoSDO, 0);
 				break;
 
 			case 3: /* Get Product Code */
-				readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1018, 0x02, 0, CheckReadInfoSDO);
+				readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1018, 0x02, 0, CheckReadInfoSDO, 0);
 				break;
 
 			case 4: /* Get Revision Number */
-				readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1018, 0x03, 0, CheckReadInfoSDO);
+				readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1018, 0x03, 0, CheckReadInfoSDO, 0);
 				break;
 
 			case 5: /* Print node info */
@@ -192,7 +192,7 @@
 		printf("Index    : %4.4x\n", index);
 		printf("SubIndex : %2.2x\n", subindex);
 
-		readNetworkDictCallback(CANOpenShellOD_Data, (UNS8)nodeid, (UNS16)index, (UNS8)subindex, (UNS8)datatype, CheckReadSDO);
+		readNetworkDictCallback(CANOpenShellOD_Data, (UNS8)nodeid, (UNS16)index, (UNS8)subindex, (UNS8)datatype, CheckReadSDO, 0);
 	}
 	else
 		printf("Wrong command  : %s\n", sdo);
@@ -234,7 +234,7 @@
 		printf("Size     : %2.2x\n", size);
 		printf("Data     : %x\n", data);
 
-		writeNetworkDictCallBack(CANOpenShellOD_Data, nodeid, index, subindex, size, 0, &data, CheckWriteSDO);
+		writeNetworkDictCallBack(CANOpenShellOD_Data, nodeid, index, subindex, size, 0, &data, CheckWriteSDO, 0);
 	}
 	else
 		printf("Wrong command  : %s\n", sdo);
diff -r 70fc3603e36f -r a03f0aa7d219 examples/TestMasterMicroMod/TestMasterMicroMod.c
--- a/examples/TestMasterMicroMod/TestMasterMicroMod.c	Tue Aug 16 14:15:52 2011 +0200
+++ b/examples/TestMasterMicroMod/TestMasterMicroMod.c	Mon Aug 29 16:59:55 2011 +0200
@@ -135,7 +135,8 @@
 					4, /*UNS8 count*/
 					0, /*UNS8 dataType*/
 					&TPDO_COBId,/*void *data*/
-					CheckSDOAndContinue); /*SDOCallback_t Callback*/
+					CheckSDOAndContinue, /*SDOCallback_t Callback*/
+                    0); /* use block mode */
 		}			
 		break;
 
@@ -152,7 +153,8 @@
 					1, /*UNS8 count*/
 					0, /*UNS8 dataType*/
 					&Transmission_Type,/*void *data*/
-					CheckSDOAndContinue); /*SDOCallback_t Callback*/
+					CheckSDOAndContinue, /*SDOCallback_t Callback*/
+                    0); /* use block mode */
 		}			
 		break;
 
@@ -169,7 +171,8 @@
 					4, /*UNS8 count*/
 					0, /*UNS8 dataType*/
 					&TPDO_COBId,/*void *data*/
-					CheckSDOAndContinue); /*SDOCallback_t Callback*/
+					CheckSDOAndContinue, /*SDOCallback_t Callback*/
+                    0); /* use block mode */
 		}			
 		break;
 					
@@ -186,7 +189,8 @@
 					4, /*UNS8 count*/
 					0, /*UNS8 dataType*/
 					&TPDO_COBId,/*void *data*/
-					CheckSDOAndContinue); /*SDOCallback_t Callback*/
+					CheckSDOAndContinue, /*SDOCallback_t Callback*/
+                    0); /* use block mode */
 		}			
 		break;
 
@@ -204,7 +208,8 @@
 					1, /*UNS8 count*/
 					0, /*UNS8 dataType*/
 					&Transmission_Type,/*void *data*/
-					CheckSDOAndContinue); /*SDOCallback_t Callback*/
+					CheckSDOAndContinue, /*SDOCallback_t Callback*/
+                    0); /* use block mode */
 		}	
 		break;
 
@@ -221,7 +226,8 @@
 					4, /*UNS8 count*/
 					0, /*UNS8 dataType*/
 					&TPDO_COBId,/*void *data*/
-					CheckSDOAndContinue); /*SDOCallback_t Callback*/
+					CheckSDOAndContinue, /*SDOCallback_t Callback*/
+                    0); /* use block mode */
 		}			
 		break;
 		
@@ -238,7 +244,8 @@
 					2, /*UNS8 count*/
 					0, /*UNS8 dataType*/
 					&Heartbeat_Producer_Time,/*void *data*/
-					CheckSDOAndContinue); /*SDOCallback_t Callback*/
+					CheckSDOAndContinue, /*SDOCallback_t Callback*/
+                    0); /* use block mode */
 		}			
 		break;
 
@@ -255,7 +262,8 @@
 					4, /*UNS8 count*/
 					0, /*UNS8 dataType*/
 					&TPDO_COBId,/*void *data*/
-					CheckSDOAndContinue); /*SDOCallback_t Callback*/
+					CheckSDOAndContinue, /*SDOCallback_t Callback*/
+                    0); /* use block mode */
 		}			
 		break;
 
@@ -272,7 +280,8 @@
 					4, /*UNS8 count*/
 					0, /*UNS8 dataType*/
 					&TPDO_COBId,/*void *data*/
-					CheckSDOAndContinue); /*SDOCallback_t Callback*/
+					CheckSDOAndContinue, /*SDOCallback_t Callback*/
+                    0); /* use block mode */
 		}
 		break;			
 
@@ -289,7 +298,8 @@
 					4, /*UNS8 count*/
 					0, /*UNS8 dataType*/
 					&TPDO_COBId,/*void *data*/
-					CheckSDOAndContinue); /*SDOCallback_t Callback*/
+					CheckSDOAndContinue, /*SDOCallback_t Callback*/
+                    0); /* use block mode */
 		}			
 		break;			
 		
diff -r 70fc3603e36f -r a03f0aa7d219 examples/TestMasterSlave/Master.c
--- a/examples/TestMasterSlave/Master.c	Tue Aug 16 14:15:52 2011 +0200
+++ b/examples/TestMasterSlave/Master.c	Mon Aug 29 16:59:55 2011 +0200
@@ -119,7 +119,8 @@
 					1, /*UNS8 count*/
 					0, /*UNS8 dataType*/
 					&Transmission_Type,/*void *data*/
-					CheckSDOAndContinue); /*SDOCallback_t Callback*/
+					CheckSDOAndContinue, /*SDOCallback_t Callback*/
+                    0); /* use block mode */
 					break;
 		
 		case 2:	/*Second step*/
@@ -131,7 +132,8 @@
 					1, /*UNS8 count*/
 					0, /*UNS8 dataType*/
 					&Transmission_Type,/*void *data*/
-					CheckSDOAndContinue); /*SDOCallback_t Callback*/
+					CheckSDOAndContinue, /*SDOCallback_t Callback*/
+                    0); /* use block mode */
 					break;
 		case 3: 
 		
@@ -220,8 +222,9 @@
 					1, /*UNS8 count*/
 					0, /*UNS8 dataType*/
 					&transmitiontype,/*void *data*/
-					CheckSDO); /*SDOCallback_t Callback*/
-	}
+					CheckSDO, /*SDOCallback_t Callback*/
+                    0); /* use block mode */
+	}   
 	if(MasterSyncCount % 50 == 25){
 		UNS8 transmitiontype = 0x00;
 		eprintf("Master : Change slave's transmit type to 0x00\n");
@@ -232,7 +235,8 @@
 					1, /*UNS8 count*/
 					0, /*UNS8 dataType*/
 					&transmitiontype,/*void *data*/
-					CheckSDO); /*SDOCallback_t Callback*/
+					CheckSDO, /*SDOCallback_t Callback*/
+                    0); /* use block mode */
 	}
 	MasterSyncCount++;
 }
diff -r 70fc3603e36f -r a03f0aa7d219 include/data.h
--- a/include/data.h	Tue Aug 16 14:15:52 2011 +0200
+++ b/include/data.h	Mon Aug 29 16:59:55 2011 +0200
@@ -95,7 +95,7 @@
 	post_sync_t post_sync;
 	post_TPDO_t post_TPDO;
 	post_SlaveBootup_t post_SlaveBootup;
-  post_SlaveStateChange_t post_SlaveStateChange;
+    post_SlaveStateChange_t post_SlaveStateChange;
 	
 	/* General */
 	UNS8 toggle;
@@ -130,7 +130,7 @@
 
 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
 #define s_transfer_Initializer {\
-		0,          /* nodeId */\
+		0,          /* CliServNbr */\
 		0,          /* wohami */\
 		SDO_RESET,  /* state */\
 		0,          /* toggle */\
@@ -142,6 +142,15 @@
 		{0},        /* data (static use, so that all the table is initialize at 0)*/\
     NULL,       /* dynamicData */ \
     0,          /* dynamicDataSize */ \
+		0,          /* peerCRCsupport */\
+		0,          /* blksize */\
+		0,          /* ackseq */\
+		0,          /* objsize */\
+		0,          /* lastblockoffset */\
+		0,          /* seqno */\
+		0,          /* endfield */\
+		RXSTEP_INIT,/* rxstep */\
+		{0},        /* tmpData */\
 		0,          /* dataType */\
 		-1,         /* timer */\
 		NULL        /* Callback */\
@@ -158,9 +167,18 @@
 		0,          /* count */\
 		0,          /* offset */\
 		{0},        /* data (static use, so that all the table is initialize at 0)*/\
-		0,          /* dataType */\
-		-1,         /* timer */\
-		NULL        /* Callback */\
+		0,          /* peerCRCsupport */\
+		0,          /* blksize */\
+		0,          /* ackseq */\
+		0,          /* objsize */\
+		0,          /* lastblockoffset */\
+		0,          /* seqno */\
+		0,          /* endfield */\
+		RXSTEP_INIT,/* rxstep */\
+		{0},        /* tmpData */\
+		0,          /*  */\
+		-1,         /*  */\
+		NULL        /*  */\
 	  },
 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION
 
diff -r 70fc3603e36f -r a03f0aa7d219 include/def.h
--- a/include/def.h	Tue Aug 16 14:15:52 2011 +0200
+++ b/include/def.h	Mon Aug 29 16:59:55 2011 +0200
@@ -75,6 +75,12 @@
 #define	SDO_ABORTED_INTERNAL     0x85     /* Aborted but not because of an abort message. */
 #define	SDO_DOWNLOAD_IN_PROGRESS 0x2 
 #define	SDO_UPLOAD_IN_PROGRESS   0x3   
+#define	SDO_BLOCK_DOWNLOAD_IN_PROGRESS 0x4 
+#define	SDO_BLOCK_UPLOAD_IN_PROGRESS   0x5
+
+/** getReadResultNetworkDict may return any of above status value or this one
+ */
+#define SDO_PROVIDED_BUFFER_TOO_SMALL   0x8A
 
 /* Status of the node during the SDO transfert : */
 #define SDO_SERVER  0x1
@@ -87,6 +93,8 @@
 #define INITIATE_UPLOAD_REQUEST      2
 #define UPLOAD_SEGMENT_REQUEST       3
 #define ABORT_TRANSFER_REQUEST       4
+#define BLOCK_UPLOAD_REQUEST         5
+#define BLOCK_DOWNLOAD_REQUEST       6
 
 /* SDOtx scs: server command specifier */
 #define UPLOAD_SEGMENT_RESPONSE      0
@@ -94,7 +102,27 @@
 #define INITIATE_DOWNLOAD_RESPONSE   3
 #define INITIATE_UPLOAD_RESPONSE     2
 #define ABORT_TRANSFER_REQUEST       4
+#define BLOCK_DOWNLOAD_RESPONSE    	 5
+#define BLOCK_UPLOAD_RESPONSE        6
 
+/* SDO block upload client subcommand */
+#define SDO_BCS_INITIATE_UPLOAD_REQUEST 0
+#define SDO_BCS_END_UPLOAD_REQUEST      1
+#define SDO_BCS_UPLOAD_RESPONSE         2
+#define SDO_BCS_START_UPLOAD            3
+
+/* SDO block upload server subcommand */
+#define SDO_BSS_INITIATE_UPLOAD_RESPONSE 0
+#define SDO_BSS_END_UPLOAD_RESPONSE      1
+
+/* SDO block download client subcommand */
+#define SDO_BCS_INITIATE_DOWNLOAD_REQUEST 0
+#define SDO_BCS_END_DOWNLOAD_REQUEST      1
+
+/* SDO block download server subcommand */
+#define SDO_BSS_INITIATE_DOWNLOAD_RESPONSE 0
+#define SDO_BSS_END_DOWNLOAD_RESPONSE      1
+#define SDO_BSS_DOWNLOAD_RESPONSE          2
 
 /*  Function Codes 
    ---------------
diff -r 70fc3603e36f -r a03f0aa7d219 include/sdo.h
--- a/include/sdo.h	Tue Aug 16 14:15:52 2011 +0200
+++ b/include/sdo.h	Mon Aug 29 16:59:55 2011 +0200
@@ -38,12 +38,19 @@
 
 #include "timer.h"
 
+/* Block mode : Data consumer receive step 
+ * - set to RXSTEP_STARTED when client receive initiate upload response 
+ * - set to RXSTEP_END when last segment of a block received 
+ */
+typedef enum {RXSTEP_INIT, RXSTEP_STARTED, RXSTEP_END } rxStep_t;
+
 typedef void (*SDOCallback_t)(CO_Data* d, UNS8 nodeId);
 
 /* The Transfer structure
 Used to store the different segments of
  - a SDO received before writing in the dictionary
  - the reading of the dictionary to put on a SDO to transmit
+WARNING : after a change in this structure check the macro s_transfer_Initializer in data.h
 */
 
 struct struct_s_transfer {
@@ -69,6 +76,17 @@
   UNS8           *dynamicData;
   UNS32          dynamicDataSize;
 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION
+                                    
+  UNS8           peerCRCsupport;    /**< True if peer supports CRC */
+  UNS8           blksize;           /**< Number of segments per block with 0 < blksize < 128 */
+  UNS8           ackseq;            /**< sequence number of last segment that was received successfully */
+  UNS32          objsize;           /**< Size in bytes of the object provided by data producer */
+  UNS8           lastblockoffset;   /**< Value of offset before last block */
+  UNS8           seqno;             /**< Last sequence number received OK or transmitted */   
+  UNS8           endfield;          /**< nbr of bytes in last segment of last block that do not contain data */
+  rxStep_t       rxstep;            /**< data consumer receive step - set to true when last segment of a block received */
+  UNS8           tmpData[8];        /**< temporary segment storage */
+
   UNS8           dataType;   /**< Defined in objdictdef.h Value is visible_string
                               * if it is a string, any other value if it is not a string,
                               * like 0. In fact, it is used only if client.
@@ -86,22 +104,6 @@
 
 #include "data.h"
 
-#if 0
-struct BODY{
-    UNS8 data[8]; /**< The 8 bytes data of the SDO */
-};
-
-/* The SDO structure ...*/
-struct struct_s_SDO {
-  UNS8 nodeId;		/**< In any case, Node ID of the server (case sender or receiver).*/
-  struct BODY body;
-};
-
-
-typedef struct struct_s_SDO s_SDO;
-
-#endif
-
 /** 
  * @brief Reset of a SDO exchange on timeout.
  * Send a SDO abort.
@@ -307,7 +309,7 @@
  * - 0xFF is returned when error occurs.
  */
 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
-		       UNS8 subIndex, UNS32 count, UNS8 dataType, void *data);
+		       UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, UNS8 useBlockMode);
 
 /** 
  * @ingroup sdo
@@ -328,7 +330,7 @@
  * - 0xFF is returned when error occurs.
  */
 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index,
-		       UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback);
+		       UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 useBlockMode);
 
 /**
  * @ingroup sdo 
@@ -352,7 +354,7 @@
  * - 0xFF is returned when error occurs.
  */
 UNS8 writeNetworkDictCallBackAI (CO_Data* d, UNS8 nodeId, UNS16 index,
-		       UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize);
+		       UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize, UNS8 useBlockMode);
 
 /**
  * @ingroup sdo 
@@ -367,7 +369,7 @@
  * - 0xFE is returned when no sdo client to communicate with node.
  * - 0xFF is returned when error occurs.
  */
-UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType);
+UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, UNS8 useBlockMode);
 
 /** 
  * @ingroup sdo
@@ -385,7 +387,7 @@
  * - 0xFE is returned when no sdo client to communicate with node.
  * - 0xFF is returned when error occurs.
  */
-UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback);
+UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode);
 
 /** 
  * @ingroup sdo
@@ -403,7 +405,7 @@
  * - 0 is returned upon success.
  * - 0xFF is returned when error occurs.
  */
-UNS8 readNetworkDictCallbackAI (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback);
+UNS8 readNetworkDictCallbackAI (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode);
 
 /** 
  * @ingroup sdo
@@ -411,8 +413,9 @@
  * 
  * @param *d Pointer to a CAN object data structure
  * @param nodeId Node Id of the slave
- * @param *data Pointer to the datas
- * @param *size Pointer to the size
+ * @param *data Pointer to the buffer to get the data
+ * @param *size Pointer to the size : MUST contain the size of the buffer before calling
+ *                                    The function set it to the actual number of written bytes
  * @param *abortCode Pointer to the abortcode. (0 = not available. Else : SDO abort code. (received if return SDO_ABORTED_RCV)
  * 
  * 
@@ -422,6 +425,7 @@
  *           - SDO_ABORTED_INTERNAL     // Transfert failed (internal abort)
  *           - SDO_UPLOAD_IN_PROGRESS   // Datas are not yet available
  *           - SDO_DOWNLOAD_IN_PROGRESS // Download is in progress
+ *           - SDO_PROVIDED_BUFFER_TOO_SMALL //The value *size is not enough to store the received data
  * \n\n
  * example :
  * @code
diff -r 70fc3603e36f -r a03f0aa7d219 src/sdo.c
--- a/src/sdo.c	Tue Aug 16 14:15:52 2011 +0200
+++ b/src/sdo.c	Mon Aug 29 16:59:55 2011 +0200
@@ -62,7 +62,7 @@
  ** @return
  **/
 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
-		UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize);
+		UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize, UNS8 useBlockMode);
 
 /*!
  ** Called by readNetworkDict
@@ -77,7 +77,7 @@
  ** @return
  **/
 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex,
-		UNS8 dataType, SDOCallback_t Callback);
+		UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode);
 
 
 /***************************************************************************/
@@ -119,6 +119,11 @@
 */
 #define getSDOsubIndex(byte3) (byte3)
 
+/** Returns the subcommand in SDO block transfert
+*/
+#define getSDOblockSC(byte) (byte & 3)
+
+
 /*!
  **
  **
@@ -136,9 +141,9 @@
 	}
 	nodeId = *((UNS32*) d->objdict[offset+d->transfers[id].CliServNbr].pSubindex[3].pObject);
 	MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0);
-	MSG_WAR(0x2A02, "server node : ", NodeId);
-	MSG_WAR(0x2A02, "      index : ", d->transfers[id].index);
-	MSG_WAR(0x2A02, "   subIndex : ", d->transfers[id].subIndex);
+	MSG_WAR(0x2A02, "server node id : ", nodeId);
+	MSG_WAR(0x2A02, "         index : ", d->transfers[id].index);
+	MSG_WAR(0x2A02, "      subIndex : ", d->transfers[id].subIndex);
 	/* Reset timer handler */
 	d->transfers[id].timer = TIMER_NONE;
 	/*Set aborted state*/
@@ -402,6 +407,7 @@
 	if ((! err) && (whoami == SDO_CLIENT)) {
 		StopSDO_TIMER(line);
 		d->transfers[line].state = SDO_ABORTED_INTERNAL;
+		d->transfers[line].abortCode = abortCode;
 	}
 	MSG_WAR(0x3A22, "Sending SDO abort ", 0);
 	err = sendSDOabort(d, whoami, CliServNbr, index, subIndex, abortCode);
@@ -427,11 +433,6 @@
 		d->transfers[line].data[i] = 0;
 	d->transfers[line].whoami = 0;
 	d->transfers[line].abortCode = 0;
-#ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
-	free(d->transfers[line].dynamicData);
-	d->transfers[line].dynamicData = 0;
-	d->transfers[line].dynamicDataSize = 0;
-#endif //SDO_DYNAMIC_BUFFER_ALLOCATION
 }
 
 /*!
@@ -449,7 +450,8 @@
 UNS8 initSDOline (CO_Data* d, UNS8 line, UNS8 CliServNbr, UNS16 index, UNS8 subIndex, UNS8 state)
 {
 	MSG_WAR(0x3A25, "init SDO line nb : ", line);
-	if (state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS){
+	if (state == SDO_DOWNLOAD_IN_PROGRESS       || state == SDO_UPLOAD_IN_PROGRESS ||
+        state == SDO_BLOCK_DOWNLOAD_IN_PROGRESS || state == SDO_BLOCK_UPLOAD_IN_PROGRESS){
 		StartSDO_TIMER(line)
 	}else{
 		StopSDO_TIMER(line)
@@ -461,6 +463,14 @@
 	d->transfers[line].toggle = 0;
 	d->transfers[line].count = 0;
 	d->transfers[line].offset = 0;
+    d->transfers[line].peerCRCsupport = 0;
+    d->transfers[line].blksize = 0;
+    d->transfers[line].ackseq = 0;
+    d->transfers[line].objsize = 0;
+    d->transfers[line].lastblockoffset = 0;
+    d->transfers[line].seqno = 0;
+    d->transfers[line].endfield = 0;
+    d->transfers[line].rxstep = RXSTEP_INIT;
 	d->transfers[line].dataType = 0;
 	d->transfers[line].Callback = NULL;
 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
@@ -643,7 +653,7 @@
 			return 0xFF;
 		}
 		m.cob_id = *((UNS32*) d->objdict[offset+CliServNbr].pSubindex[2].pObject);
-		MSG_WAR(0x3A41, "I am server cobId : ", m.cob_id);
+		MSG_WAR(0x3A41, "I am server Tx cobId : ", m.cob_id);
 	}
 	else {			/*case client*/
 		/* Get the client->server cobid.*/
@@ -653,7 +663,7 @@
 			return 0xFF;
 		}
 		m.cob_id = *((UNS32*) d->objdict[offset+CliServNbr].pSubindex[1].pObject);
-		MSG_WAR(0x3A41, "I am client cobId : ", m.cob_id);
+		MSG_WAR(0x3A41, "I am client Tx cobId : ", m.cob_id);
 	}
 	/* message copy for sending */
 	m.rtr = NOT_A_REQUEST;
@@ -709,6 +719,7 @@
 UNS8 proceedSDO (CO_Data* d, Message *m)
 {
 	UNS8 err;
+	UNS8 cs;
 	UNS8 line;
 	UNS32 nbBytes; 		/* received or to be transmited. */
 	UNS8 nodeId = 0;  	/* The node Id of the server if client otherwise unused */
@@ -724,6 +735,10 @@
 	UNS32 *pCobId = NULL;
 	UNS16 offset;
 	UNS16 lastIndex;
+	UNS8 SubCommand;	/* Block transfert only */
+    UNS8 SeqNo;         /* Sequence number in block transfert */
+    UNS8 AckSeq;        /* Sequence number of last segment that was received successfully */
+	UNS8 NbBytesNoData; /* Number of bytes that do not contain data in last segment of block transfert */ 
 
 	MSG_WAR(0x3A60, "proceedSDO ", 0);
 	whoami = SDO_UNKNOWN;
@@ -763,7 +778,7 @@
 			pCobId = (UNS32*) d->objdict[offset].pSubindex[2].pObject;
 			if (*pCobId == UNS16_LE(m->cob_id) ) {
 				whoami = SDO_CLIENT;
-				MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j);
+				MSG_WAR(0x3A64, "proceedSDO. I am client index : ", 0x1280 + j);
 				/* Defining Client number = index minus 0x1280 where the cobid received is defined. */
 				CliServNbr = j;
 				/* Reading the server node ID, if client it is mandatory in the OD */
@@ -792,17 +807,34 @@
 		MSG_WAR(0x3A69, "I am SERVER number ", CliServNbr);
 	}
 
+	/* Look for an SDO transfert already initiated. */
+	err = getSDOlineOnUse( d, CliServNbr, whoami, &line );
+
+	/* Let's find cs value, first it is set as "not valid" */
+	cs = 0xFF; 
+	/* Special cases for block transfert : in frames with segment data cs is not spécified */
+   	if (!err) {
+		if ((whoami == SDO_SERVER) && (d->transfers[line].state == SDO_BLOCK_DOWNLOAD_IN_PROGRESS) ||
+			(whoami == SDO_CLIENT) && (d->transfers[line].state == SDO_BLOCK_UPLOAD_IN_PROGRESS)) {		
+			if(m->data[0] == 0x80)	/* If first byte is 0x80 it is an abort frame (seqno = 0 not allowed) */
+				cs = 4;
+			else
+				cs = 6;
+		}
+	}
+	/* Other cases : cs is specified */
+	if (cs == 0xFF)
+		cs = getSDOcs(m->data[0]);
+
 	/* Testing the command specifier */
-	/* Allowed : cs = 0, 1, 2, 3, 4. (=  all except those for block tranfert). */
+	/* Allowed : cs = 0, 1, 2, 3, 4, 5, 6 */
 	/* cs = other : Not allowed -> abort. */
-	switch (getSDOcs(m->data[0])) {
+	switch (cs) {
 
 		case 0:
 			/* I am SERVER */
 			if (whoami == SDO_SERVER) {
-				/* Receiving a download segment data. */
-				/* A SDO transfert should have been yet initiated. */
-				err = getSDOlineOnUse( d, CliServNbr, whoami, &line );
+				/* Receiving a download segment data : an SDO transfert should have been yet initiated. */
 				if (!err)
 					err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
 				if (err) {
@@ -856,7 +888,6 @@
 			else { /* if CLIENT */
 				/* I am CLIENT */
 				/* It is a request for a previous upload segment. We should find a line opened for this.*/
-				err = getSDOlineOnUse( d, CliServNbr, whoami, &line);
 				if (!err)
 					err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
 				if (err) {
@@ -917,7 +948,6 @@
 				MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex);
 
 				/* Search if a SDO transfert have been yet initiated */
-				err = getSDOlineOnUse( d, CliServNbr, whoami, &line );
 				if (! err) {
 					MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0);
 					failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
@@ -980,7 +1010,6 @@
 			else {
 				/* I am CLIENT */
 				/* It is a response for a previous download segment. We should find a line opened for this. */
-				err = getSDOlineOnUse( d, CliServNbr, whoami, &line);
 				if (!err)
 					err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
 				if (err) {
@@ -1049,7 +1078,6 @@
 				MSG_WAR(0x3A90, "Reading at index : ", index);
 				MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex);
 				/* Search if a SDO transfert have been yet initiated*/
-				err = getSDOlineOnUse( d, CliServNbr, whoami, &line );
 				if (! err) {
 					MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line);
 					MSG_WAR(0x3A93, "Server Nbr = ", CliServNbr);
@@ -1083,12 +1111,11 @@
 					data[1] = index & 0xFF;        /* LSB */
 					data[2] = (index >> 8) & 0xFF; /* MSB */
 					data[3] = subIndex;
-					data[4] = (UNS8)nbBytes; /* Limitation of canfestival2 : Max tranfert is 256 bytes.*/
-					/* It takes too much memory to upgrate to 2^32 because the size of data is also coded */
-					/* in the object dictionary, at every index and subindex. */
-					for (i = 5 ; i < 8 ; i++)
-						data[i] = 0;
-					MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId);
+					data[4] = nbBytes; 
+					data[5] = nbBytes >> 8; 
+					data[6] = nbBytes >> 16; 
+					data[7] = nbBytes >> 24; 
+ 					MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId);
 					sendSDO(d, whoami, CliServNbr, data);
 				}
 				else {
@@ -1115,7 +1142,6 @@
 				/* I am CLIENT */
 				/* It is the response for the previous initiate upload request.*/
 				/* We should find a line opened for this. */
-				err = getSDOlineOnUse( d, CliServNbr, whoami, &line);
 				if (!err)
 					err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
 				if (err) {
@@ -1170,7 +1196,6 @@
 			if (whoami == SDO_SERVER) {
 				/* Receiving a upload segment. */
 				/* A SDO transfert should have been yet initiated. */
-				err = getSDOlineOnUse( d, CliServNbr, whoami, &line );
 				if (!err)
 					err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
 				if (err) {
@@ -1227,7 +1252,6 @@
 				/* I am CLIENT */
 				/* It is the response for the previous initiate download request. */
 				/* We should find a line opened for this. */
-				err = getSDOlineOnUse( d, CliServNbr, whoami, &line);
 				if (!err)
 					err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
 				if (err) {
@@ -1283,9 +1307,7 @@
 				((UNS32)m->data[6] << 16) |
 				((UNS32)m->data[7] << 24);
 			/* Received SDO abort. */
-			/* Looking for the line concerned. */
 			if (whoami == SDO_SERVER) {
-				err = getSDOlineOnUse( d, CliServNbr, whoami, &line );
 				if (!err) {
 					resetSDOline( d, line );
 					MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode);
@@ -1296,7 +1318,6 @@
 				/* Its is ok, I think.*/
 			}
 			else { /* If I am CLIENT */
-				err = getSDOlineOnUse( d, CliServNbr, whoami, &line );
 				if (!err) {
 					/* The line *must* be released by the core program. */
 					StopSDO_TIMER(line)
@@ -1309,9 +1330,440 @@
 					MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode);
 			}
 			break;
+		case 5: /* Command specifier for data transmission - the client or server is the data producer */
+			SubCommand = getSDOblockSC(m->data[0]);
+			if (whoami == SDO_SERVER) { /* Server block upload */
+				if (SubCommand == SDO_BCS_INITIATE_UPLOAD_REQUEST) {
+				    index = getSDOindex(m->data[1],m->data[2]);
+				    subIndex = getSDOsubIndex(m->data[3]);
+				    MSG_WAR(0x3AB2, "Received SDO Initiate block upload defined at index 0x1200 + ",
+						CliServNbr);
+				    MSG_WAR(0x3AB3, "Reading at index : ", index);
+				    MSG_WAR(0x3AB4, "Reading at subIndex : ", subIndex);
+				    /* Search if a SDO transfert have been yet initiated */
+				    if (! err) {
+					    MSG_ERR(0x1A93, "SDO error : Transmission yet started at line : ", line);
+					    MSG_WAR(0x3AB5, "Server Nbr = ", CliServNbr);
+					    failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
+					    return 0xFF;
+				    }
+				    /* No line on use. Great !*/
+				    /* Try to open a new line.*/
+				    err = getSDOfreeLine( d, whoami, &line );
+				    if (err) {
+					    MSG_ERR(0x1A73, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
+					    failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
+					    return 0xFF;
+				    }
+				    initSDOline(d, line, CliServNbr, index, subIndex, SDO_BLOCK_UPLOAD_IN_PROGRESS);
+                    d->transfers[line].peerCRCsupport = ((m->data[0])>>2) & 1;
+                    d->transfers[line].blksize = m->data[4];
+				    /* Transfer data from dictionary to the line structure. */
+				    errorCode = objdictToSDOline(d, line);
+				    if (errorCode) {
+					    MSG_ERR(0x1A95, "SDO error : Unable to copy the data from object dictionary. Err code : ",
+							errorCode);
+					    failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode);
+					    return 0xFF;
+				    }
+ 				    /* Preparing the response.*/
+				    getSDOlineRestBytes(d, line, &nbBytes);	/* get Nb bytes to transfer */
+                    d->transfers[line].objsize = nbBytes;
+                    data[0] = (6 << 5) | (1 << 1) | SDO_BSS_INITIATE_UPLOAD_RESPONSE;
+					data[1] = index & 0xFF;        /* LSB */
+					data[2] = (index >> 8) & 0xFF; /* MSB */
+					data[3] = subIndex;
+					data[4] = nbBytes;
+					data[5] = nbBytes >> 8;
+					data[6] = nbBytes >> 16;
+					data[7] = nbBytes >> 24;
+					MSG_WAR(0x3A9A, "SDO. Sending normal block upload initiate response defined at index 0x1200 + ", nodeId);
+					sendSDO(d, whoami, CliServNbr, data);
+                }
+				else if (SubCommand == SDO_BCS_END_UPLOAD_REQUEST) {
+				    MSG_WAR(0x3AA2, "Received SDO block END upload request defined at index 0x1200 + ", CliServNbr);
+ 				    /* A SDO transfert should have been yet initiated. */
+				    if (!err)
+					    err = d->transfers[line].state != SDO_BLOCK_UPLOAD_IN_PROGRESS;
+				    if (err) {
+					    MSG_ERR(0x1AA1, "SDO error : Received block upload request for unstarted trans. index 0x1200 + ",
+							    CliServNbr);
+					    failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
+					    return 0xFF;
+				    }
+                    /* Release the line */
+					resetSDOline(d, line);
+                }
+				else if ((SubCommand == SDO_BCS_UPLOAD_RESPONSE) || (SubCommand == SDO_BCS_START_UPLOAD)) {
+ 				    /* A SDO transfert should have been yet initiated. */
+				    if (!err)
+					    err = d->transfers[line].state != SDO_BLOCK_UPLOAD_IN_PROGRESS;
+				    if (err) {
+					    MSG_ERR(0x1AA1, "SDO error : Received block upload response for unstarted trans. index 0x1200 + ",
+							    CliServNbr);
+					    failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
+					    return 0xFF;
+				    }
+				    /* Reset the wathdog */
+				    RestartSDO_TIMER(line);
+				    /* Uploading first or next block */
+				    index = d->transfers[line].index;
+				    subIndex = d->transfers[line].subIndex;
+                    if (SubCommand == SDO_BCS_UPLOAD_RESPONSE) {
+					    MSG_WAR(0x3AA2, "Received SDO block upload response defined at index 0x1200 + ", CliServNbr);
+                        d->transfers[line].blksize = m->data[2];
+                        AckSeq = (m->data[1]) & 0x7f;
+                        getSDOlineRestBytes(d, line, &nbBytes);
+                        if((nbBytes == 0) && (AckSeq == d->transfers[line].seqno)){ /* Si tout est envoyé et confirmé reçu on envoi un block end upload response */
+                            data[0] = (6 << 5) | ((d->transfers[line].endfield) << 2) | SDO_BSS_END_UPLOAD_RESPONSE;
+                            for (i = 1 ; i < 8 ; i++)
+						        data[i] = 0;
+					        MSG_WAR(0x3AA5, "SDO. Sending block END upload response defined at index 0x1200 + ", CliServNbr);
+					        sendSDO(d, whoami, CliServNbr, data);
+                            break;
+                        }
+                        else
+                            d->transfers[line].offset = d->transfers[line].lastblockoffset + 7 * AckSeq;
+                        if(d->transfers[line].offset > d->transfers[line].count) { /* Bad AckSeq reveived (too high) */
+					        MSG_ERR(0x1AA1, "SDO error : Received upload response with bad ackseq index 0x1200 + ",
+							    CliServNbr);
+					        failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
+					        return 0xFF;
+                        }
+           			}
+                    else
+					    MSG_WAR(0x3AA2, "Received SDO block START upload defined at index 0x1200 + ", CliServNbr);
+                    d->transfers[line].lastblockoffset = d->transfers[line].offset;
+                    for(SeqNo = 1 ; SeqNo <= d->transfers[line].blksize ; SeqNo++) {
+                        d->transfers[line].seqno = SeqNo;
+				        getSDOlineRestBytes(d, line, &nbBytes);
+                        if (nbBytes > 7) {
+					        /* The segment to transfer is not the last one.*/
+ 					        data[0] = SeqNo;
+					        err = lineToSDO(d, line, 7, data + 1);
+					        if (err) {
+						        failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
+						        return 0xFF;
+					        }
+					        MSG_WAR(0x3AA5, "SDO. Sending upload segment defined at index 0x1200 + ", CliServNbr);
+					        sendSDO(d, whoami, CliServNbr, data);
+				        }
+				        else {
+					        /* Last segment is in this block */
+					        data[0] = 0x80 | SeqNo;
+					        err = lineToSDO(d, line, nbBytes, data + 1);
+					        if (err) {
+						        failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
+						        return 0xFF;
+					        }
+					        for (i = nbBytes + 1 ; i < 8 ; i++)
+						        data[i] = 0;
+					        MSG_WAR(0x3AA5, "SDO. Sending last upload segment defined at index 0x1200 + ", CliServNbr);
+					        sendSDO(d, whoami, CliServNbr, data);
+                            d->transfers[line].endfield = 7 - nbBytes;
+                            break;
+				        }
+                    }
+                }
+			}      /* end if SERVER */
+			else { /* if CLIENT (block download) */
+                if ((SubCommand == SDO_BSS_INITIATE_DOWNLOAD_RESPONSE) || (SubCommand == SDO_BSS_DOWNLOAD_RESPONSE)) {
+                    /* We should find a line opened for this. */
+                    if (!err)
+                        err = d->transfers[line].state != SDO_BLOCK_DOWNLOAD_IN_PROGRESS;
+                    if (err) {
+                        MSG_ERR(0x1AAA, "SDO error : Received response for unknown block download request from node id", nodeId);
+                        failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
+                        return 0xFF;
+                    }
+                    /* Reset the watchdog */
+                    RestartSDO_TIMER(line)
+                    if (SubCommand == SDO_BSS_INITIATE_DOWNLOAD_RESPONSE) {
+                           index = d->transfers[line].index;
+                        subIndex = d->transfers[line].subIndex;
+                        d->transfers[line].peerCRCsupport = ((m->data[0])>>2) & 1;
+                        d->transfers[line].blksize = m->data[4];
+                    }
+                    else {
+                    	d->transfers[line].blksize = m->data[2];
+                        AckSeq = (m->data[1]) & 0x7f;
+                        getSDOlineRestBytes(d, line, &nbBytes);
+                        if((nbBytes == 0) && (AckSeq == d->transfers[line].seqno)){ /* Si tout est envoyé et confirmé reçu on envoi un block end download request */
+                            data[0] = (6 << 5) | ((d->transfers[line].endfield) << 2) | SDO_BCS_END_DOWNLOAD_REQUEST;
+                            for (i = 1 ; i < 8 ; i++)
+						        data[i] = 0;
+					        MSG_WAR(0x3AA5, "SDO. Sending block END download request defined at index 0x1200 + ", CliServNbr);
+					        sendSDO(d, whoami, CliServNbr, data);
+                            break;
+                        }
+                        else
+                            d->transfers[line].offset = d->transfers[line].lastblockoffset + 7 * AckSeq;
+                        if(d->transfers[line].offset > d->transfers[line].count) { /* Bad AckSeq reveived (too high) */
+					        MSG_ERR(0x1AA1, "SDO error : Received upload segment with bad ackseq index 0x1200 + ",
+							    CliServNbr);
+					        failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
+					        return 0xFF;
+                        }
+					}
+                 	d->transfers[line].lastblockoffset = d->transfers[line].offset;
+                	for(SeqNo = 1 ; SeqNo <= d->transfers[line].blksize ; SeqNo++) {
+                        d->transfers[line].seqno = SeqNo;
+				        getSDOlineRestBytes(d, line, &nbBytes);
+                        if (nbBytes > 7) {
+					        /* The segment to transfer is not the last one.*/
+ 					        data[0] = SeqNo;
+					        err = lineToSDO(d, line, 7, data + 1);
+					        if (err) {
+						        failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
+						        return 0xFF;
+					        }
+					        MSG_WAR(0x3AAB, "SDO. Sending download segment to node id ", nodeId);
+					        sendSDO(d, whoami, CliServNbr, data);
+				        }
+				        else {
+					        /* Last segment is in this block */
+					        data[0] = 0x80 | SeqNo;
+					        err = lineToSDO(d, line, nbBytes, data + 1);
+					        if (err) {
+						        failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
+						        return 0xFF;
+					        }
+					        for (i = nbBytes + 1 ; i < 8 ; i++)
+						        data[i] = 0;
+					        MSG_WAR(0x3AAB, "SDO. Sending last download segment to node id ", nodeId);
+					        sendSDO(d, whoami, CliServNbr, data);
+                            d->transfers[line].endfield = 7 - nbBytes;
+                            break;
+				        }
+                    }
+				}
+				else if (SubCommand == SDO_BSS_END_DOWNLOAD_RESPONSE) {
+					MSG_WAR(0x3AAC, "SDO End block download response from nodeId", nodeId);
+					StopSDO_TIMER(line)
+					d->transfers[line].state = SDO_FINISHED;
+					if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
+					return 0x00;
+				}
+				else {
+			    	MSG_ERR(0x1AAB, "SDO error block download : Received wrong subcommand from nodeId", nodeId);
+    				failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
+	    			return 0xFF;
+				}
+			}      /* end if CLIENT */
+			break;
+		case 6: /* Command specifier for data reception - the client or server is the data consumer */
+			if (whoami == SDO_SERVER) { /* Server block download */
+				if (err) {
+					/* Nothing already started */
+					SubCommand = (m->data[0]) & 1;
+					if (SubCommand != SDO_BCS_INITIATE_DOWNLOAD_REQUEST) {
+			    	    MSG_ERR(0x1AAC, "SDO error block download : Received wrong subcommand from node id", nodeId);
+    				    failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
+	    			    return 0xFF;
+				    }
+					index = getSDOindex(m->data[1],m->data[2]);
+					subIndex = getSDOsubIndex(m->data[3]);
+					MSG_WAR(0x3A9B, "Received SDO block download initiate defined at index 0x1200 + ",
+						CliServNbr);
+					MSG_WAR(0x3A9B, "Writing at index : ", index);
+					MSG_WAR(0x3A9B, "Writing at subIndex : ", subIndex);
+					/* Try to open a new line. */
+					err = getSDOfreeLine( d, whoami, &line );
+					if (err) {
+						MSG_ERR(0x1A89, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
+						failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
+						return 0xFF;
+					}
+					initSDOline(d, line, CliServNbr, index, subIndex, SDO_BLOCK_DOWNLOAD_IN_PROGRESS);
+                    d->transfers[line].rxstep = RXSTEP_STARTED;
+                    d->transfers[line].peerCRCsupport = ((m->data[0])>>2) & 1;
+					if ((m->data[0]) & 2)	/* if data set size is indicated */
+                    	d->transfers[line].objsize = (UNS32)m->data[4] + (UNS32)m->data[5]*256 + (UNS32)m->data[6]*256*256 + (UNS32)m->data[7]*256*256*256;
+                    data[0] = (5 << 5) | SDO_BSS_INITIATE_DOWNLOAD_RESPONSE;
+					data[1] = index;        /* LSB */
+					data[2] = index >> 8;   /* MSB */
+					data[3] = subIndex;
+					data[4] = SDO_BLOCK_SIZE;
+					data[5] = data[6] = data[7] = 0;
+					MSG_WAR(0x3AAD, "SDO. Sending block download initiate response - index 0x1200 + ", CliServNbr);
+					sendSDO(d, whoami, CliServNbr, data);
+				}
+				else if (d->transfers[line].rxstep == RXSTEP_STARTED) {
+					MSG_WAR(0x3A9B, "Received SDO block download data segment - index 0x1200 + ", CliServNbr);
+    		    	RestartSDO_TIMER(line)
+					SeqNo = m->data[0] & 0x7F;
+					if (m->data[0] & 0x80) {	/* Last segment ? */
+					    if(SeqNo == (d->transfers[line].seqno + 1)) {
+							d->transfers[line].rxstep = RXSTEP_END;
+							d->transfers[line].seqno = SeqNo;
+							/* Store the data temporary because we don't know yet how many bytes do not contain data */
+							memcpy(d->transfers[line].tmpData, m->data, 8);
+						}
+						data[0] = (5 << 5) | SDO_BSS_DOWNLOAD_RESPONSE;
+						data[1] = d->transfers[line].seqno;
+						data[2] = SDO_BLOCK_SIZE;
+						data[3] = data[4] = data[5] = data[6] = data[7] = 0;
+						MSG_WAR(0x3AAE, "SDO. Sending block download response - index 0x1200 + ", CliServNbr);
+						sendSDO(d, whoami, CliServNbr, data);
+                        d->transfers[line].seqno = 0;
+					}
+					else {
+					   	if (SeqNo == (d->transfers[line].seqno + 1)) {	
+							d->transfers[line].seqno = SeqNo;
+							/* Store the data in the transfert structure. */
+							err = SDOtoLine(d, line, 7, (*m).data + 1);
+							if (err) {
+								failedSDO(d, CliServNbr, whoami, d->transfers[line].index,  d->transfers[line].subIndex, SDOABT_GENERAL_ERROR);
+								return 0xFF;
+							}
+						}
+						if (SeqNo == SDO_BLOCK_SIZE) {
+							data[0] = (5 << 5) | SDO_BSS_DOWNLOAD_RESPONSE;
+							data[1] = d->transfers[line].seqno;
+							data[2] = SDO_BLOCK_SIZE;
+							data[3] = data[4] = data[5] = data[6] = data[7] = 0;
+							MSG_WAR(0x3AAE, "SDO. Sending block download response - index 0x1200 + ", CliServNbr);
+							sendSDO(d, whoami, CliServNbr, data);
+                            d->transfers[line].seqno = 0;
+						}
+					}
+				}
+				else if (d->transfers[line].rxstep == RXSTEP_END) { /* endphase */
+					MSG_WAR(0x3A9B, "Received SDO block download end request - index 0x1200 + ", CliServNbr);
+					/* here store remaining bytes in tmpData to line, check size and confirm or abort */
+					if ((m->data[0] & 1) != SDO_BCS_END_DOWNLOAD_REQUEST) {
+		    			MSG_ERR(0x1AAD, "SDO error block download : Received wrong subcommand - index 0x1200 + ", CliServNbr);
+    					failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
+	    				return 0xFF;
+					}
+    		    	RestartSDO_TIMER(line)
+					NbBytesNoData = (m->data[0]>>2) & 0x07;
+					/* Store the data in the transfert structure. */
+					err = SDOtoLine(d, line, 7-NbBytesNoData, d->transfers[line].tmpData + 1);
+					if (err) {
+						failedSDO(d, CliServNbr, whoami, d->transfers[line].index,  d->transfers[line].subIndex, SDOABT_GENERAL_ERROR);
+						return 0xFF;
+					}
+					if(d->transfers[line].objsize){ /* If size was indicated in the initiate request */
+						if (d->transfers[line].objsize != d->transfers[line].offset){
+		    					MSG_ERR(0x1AAE, "SDO error block download : sizes do not match - index 0x1200 + ", CliServNbr);
+    							failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_LOCAL_CTRL_ERROR);
+	    						return 0xFF;
+						}
+					}
+					data[0] = (5 << 5) | SDO_BSS_END_DOWNLOAD_RESPONSE;
+					for (i = 1 ; i < 8 ; i++)
+						data[i] = 0;
+					MSG_WAR(0x3AAF, "SDO. Sending block download end response - index 0x1200 + ", CliServNbr);
+					sendSDO(d, whoami, CliServNbr, data);
+					/* Transfering line data to object dictionary. */
+					errorCode = SDOlineToObjdict(d, line);
+					if (errorCode) {
+						MSG_ERR(0x1AAF, "SDO error : Unable to copy the data in the object dictionary", 0);
+						failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, errorCode);
+						return 0xFF;
+					}
+					/* Release of the line */
+					resetSDOline(d, line);
+					MSG_WAR(0x3AAF, "SDO. End of block download defined at index 0x1200 + ", CliServNbr);
+				}
+		    }      /* end if SERVER */
+		    else { /* if CLIENT (block upload) */
+				if (err) {
+       				/* Nothing already started */
+			    	MSG_ERR(0x1AAD, "SDO error block upload : no transmission started", nodeId);
+    				failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
+	    			return 0xFF;
+				}
+    			RestartSDO_TIMER(line)
+				if (d->transfers[line].rxstep == RXSTEP_INIT) {
+				    if ((m->data[0] & 1) == SDO_BSS_INITIATE_UPLOAD_RESPONSE) {
+					    MSG_WAR(0x3A9C, "Received SDO block upload response from node id ", nodeId);
+						d->transfers[line].rxstep = RXSTEP_STARTED;
+                        d->transfers[line].peerCRCsupport = ((m->data[0])>>2) & 1;
+					    if ((m->data[0]) & 2)	/* if data set size is indicated */
+                    	    d->transfers[line].objsize = (UNS32)m->data[4] + (UNS32)m->data[5]*256 + (UNS32)m->data[6]*256*256 + (UNS32)m->data[7]*256*256*256;
+                        data[0] = (5 << 5) | SDO_BCS_START_UPLOAD;
+					    for (i = 1 ; i < 8 ; i++)
+						    data[i] = 0;
+                        MSG_WAR(0x3AB6, "SDO. Sending block upload start to node id ", nodeId);
+					    sendSDO(d, whoami, CliServNbr, data);
+                    }
+                }
+				else if (d->transfers[line].rxstep == RXSTEP_STARTED) {
+					SeqNo = m->data[0] & 0x7F;
+					if (m->data[0] & 0x80) {	/* Last segment ? */
+					    if(SeqNo == (d->transfers[line].seqno + 1)) {
+							d->transfers[line].rxstep = RXSTEP_END;
+							d->transfers[line].seqno = SeqNo;
+							/* Store the data temporary because we don't know yet how many bytes do not contain data */
+							memcpy(d->transfers[line].tmpData, m->data, 8);
+						}
+						data[0] = (5 << 5) | SDO_BCS_UPLOAD_RESPONSE;
+						data[1] = d->transfers[line].seqno;
+						data[2] = SDO_BLOCK_SIZE;
+						data[3] = data[4] = data[5] = data[6] = data[7] = 0;
+						MSG_WAR(0x3AB7, "SDO. Sending block upload response to node id ", nodeId);
+						sendSDO(d, whoami, CliServNbr, data);
+                        d->transfers[line].seqno = 0;
+					}
+					else {
+					   	if (SeqNo == (d->transfers[line].seqno + 1)) {	
+							d->transfers[line].seqno = SeqNo;
+							/* Store the data in the transfert structure. */
+							err = SDOtoLine(d, line, 7, (*m).data + 1);
+							if (err) {
+								failedSDO(d, CliServNbr, whoami, d->transfers[line].index,  d->transfers[line].subIndex, SDOABT_GENERAL_ERROR);
+								return 0xFF;
+							}
+						}
+						if (SeqNo == SDO_BLOCK_SIZE) {
+							data[0] = (5 << 5) | SDO_BCS_UPLOAD_RESPONSE;
+							data[1] = d->transfers[line].seqno;
+							data[2] = SDO_BLOCK_SIZE;
+							data[3] = data[4] = data[5] = data[6] = data[7] = 0;
+							MSG_WAR(0x3AAE, "SDO. Sending block upload response to node id ", nodeId);
+							sendSDO(d, whoami, CliServNbr, data);
+                            d->transfers[line].seqno = 0;
+						}
+					}
+				}
+				else if (d->transfers[line].rxstep == RXSTEP_END) { /* endphase */
+					/* here store remaining bytes in tmpData to line, check size and confirm or abort */
+					if ((m->data[0] & 1) != SDO_BSS_END_UPLOAD_RESPONSE) {
+			    		MSG_ERR(0x1AAD, "SDO error block upload : Received wrong subcommand from node id ", nodeId);
+    					failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
+	    				return 0xFF;
+					}
+					NbBytesNoData = (m->data[0]>>2) & 0x07;
+					/* Store the data in the transfert structure. */
+					err = SDOtoLine(d, line, 7-NbBytesNoData, d->transfers[line].tmpData + 1);
+					if (err) {
+						failedSDO(d, CliServNbr, whoami, d->transfers[line].index,  d->transfers[line].subIndex, SDOABT_GENERAL_ERROR);
+						return 0xFF;
+					}
+					if(d->transfers[line].objsize){ /* If size was indicated in the initiate request */
+						if (d->transfers[line].objsize != d->transfers[line].offset){
+			    				MSG_ERR(0x1AAE, "SDO error block download : sizes do not match - from node id ", nodeId);
+    							failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_LOCAL_CTRL_ERROR);
+	    						return 0xFF;
+						}
+					}
+					data[0] = (5 << 5) | SDO_BCS_END_UPLOAD_REQUEST;
+					for (i = 1 ; i < 8 ; i++)
+						data[i] = 0;
+					MSG_WAR(0x3AAF, "SDO. Sending block upload end request to node id ", nodeId);
+					sendSDO(d, whoami, CliServNbr, data);
+					MSG_WAR(0x3AAF, "SDO. End of block upload request", 0);
+                    StopSDO_TIMER(line)
+					d->transfers[line].state = SDO_FINISHED;
+				    if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);
+				}
+			}      /* end if CLIENT */
+			break;
 		default:
 			/* Error : Unknown cs */
-			MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", getSDOcs(m->data[0]));
+			MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", cs);
 			return 0xFF;
 
 	} /* End switch */
@@ -1388,7 +1840,7 @@
  ** @return
  **/
 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
-		UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize)
+		UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize, UNS8 useBlockMode)
 {
 	UNS8 err;
 	UNS8 line;
@@ -1418,10 +1870,14 @@
 		MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId);
 		return (0xFF);
 	}
-	initSDOline(d, line, CliNbr, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
+    if(useBlockMode) {
+	    initSDOline(d, line, CliNbr, index, subIndex, SDO_BLOCK_DOWNLOAD_IN_PROGRESS);
+	    d->transfers[line].objsize = count;
+    }
+    else 
+	    initSDOline(d, line, CliNbr, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
 	d->transfers[line].count = count;
 	d->transfers[line].dataType = dataType;
-
 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
 	{
 		UNS8* lineData = d->transfers[line].data;
@@ -1461,18 +1917,25 @@
 #  endif
 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION
 	}
-	/* Send the SDO to the server. Initiate download, cs=1. */
-	if (count <= 4) { /* Expedited transfert */
-		buf[0] = (UNS8)((1 << 5) | ((4 - count) << 2) | 3);
-		for (i = 4 ; i < 8 ; i++)
-			buf[i] = d->transfers[line].data[i - 4];
-		d->transfers[line].offset = count;
-	}
-	else { /** Normal transfert */
-		buf[0] = (1 << 5) | 1;
-		for (i = 0 ; i < 4 ; i++)
-			buf[i+4] = (UNS8)((count >> (i<<3))); /* i*8 */
-	}
+    if(useBlockMode) {
+	    buf[0] = (6 << 5) | (1 << 1 );   /* CCS = 6 , CC = 0 , S = 1 , CS = 0 */
+ 	    for (i = 0 ; i < 4 ; i++)
+		    buf[i+4] = (UNS8)((count >> (i<<3))); /* i*8 */
+    }
+    else {
+	    /* Send the SDO to the server. Initiate download, cs=1. */
+	    if (count <= 4) { /* Expedited transfert */
+		    buf[0] = (UNS8)((1 << 5) | ((4 - count) << 2) | 3);
+		    for (i = 4 ; i < 8 ; i++)
+			    buf[i] = d->transfers[line].data[i - 4];
+		    d->transfers[line].offset = count;
+	    }
+	    else { /** Normal transfert */
+		    buf[0] = (1 << 5) | 1;
+		    for (i = 0 ; i < 4 ; i++)
+			    buf[i+4] = (UNS8)((count >> (i<<3))); /* i*8 */
+	    }
+    }
 	buf[1] = index & 0xFF;        /* LSB */
 	buf[2] = (index >> 8) & 0xFF; /* MSB */
 	buf[3] = subIndex;
@@ -1505,9 +1968,9 @@
  ** @return
  **/
 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
-		UNS8 subIndex, UNS32 count, UNS8 dataType, void *data)
-{
-	return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL, 1);
+		UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, UNS8 useBlockMode)
+{
+	return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL, 1, useBlockMode);
 }
 
 /*!
@@ -1525,13 +1988,13 @@
  ** @return
  **/
 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index,
-		UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback)
-{
-	return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1);
+		UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 useBlockMode)
+{
+	return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1, useBlockMode);
 }
 
 UNS8 writeNetworkDictCallBackAI (CO_Data* d, UNS8 nodeId, UNS16 index,
-		UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize)
+		UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize, UNS8 useBlockMode)
 {
 	UNS8 ret;
 	UNS16 lastIndex;
@@ -1539,7 +2002,7 @@
 	UNS8 nodeIdServer;
 	UNS8 i;
 
-	ret = _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, endianize);
+	ret = _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, endianize, useBlockMode);
 	if(ret == 0xFE)
 	{
 		offset = d->firstIndex->SDO_CLT;
@@ -1563,7 +2026,7 @@
 				*(UNS32*)d->objdict[offset].pSubindex[1].pObject = (UNS32)(0x600 + nodeId);
 				*(UNS32*)d->objdict[offset].pSubindex[2].pObject = (UNS32)(0x580 + nodeId);
 				*(UNS8*) d->objdict[offset].pSubindex[3].pObject = nodeId;
-				return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1);
+				return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1, useBlockMode);
 			}
 			offset++;
 		}
@@ -1591,7 +2054,7 @@
  **
  ** @return
  **/
-INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
+INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode)
 {
 	UNS8 err;
 	UNS8 i;
@@ -1623,16 +2086,29 @@
 	else
 		MSG_WAR(0x3AE0, "Transmission on line : ", line);
 
-	initSDOline(d, line, CliNbr, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
-	getSDOlineOnUse(d, CliNbr, SDO_CLIENT, &line);
-	/* Send the SDO to the server. Initiate upload, cs=2. */
-	d->transfers[line].dataType = dataType;
-	data[0] = (2 << 5);
-	data[1] = index & 0xFF;        /* LSB */
-	data[2] = (index >> 8) & 0xFF; /* MSB */
-	data[3] = subIndex;
-	for (i = 4 ; i < 8 ; i++)
-		data[i] = 0;
+    if(useBlockMode) {
+	    initSDOline(d, line, CliNbr, index, subIndex, SDO_BLOCK_UPLOAD_IN_PROGRESS);
+	    /* Send the SDO to the server. Initiate block upload, cs=0. */
+	    d->transfers[line].dataType = dataType;
+	    data[0] = (5 << 5) | SDO_BCS_INITIATE_UPLOAD_REQUEST;
+	    data[1] = index & 0xFF;        /* LSB */
+	    data[2] = (index >> 8) & 0xFF; /* MSB */
+	    data[3] = subIndex;
+	    data[4] = SDO_BLOCK_SIZE;
+	    for (i = 5 ; i < 8 ; i++)
+		    data[i] = 0;
+    }
+    else {
+	    initSDOline(d, line, CliNbr, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
+	    /* Send the SDO to the server. Initiate upload, cs=2. */
+	    d->transfers[line].dataType = dataType;
+	    data[0] = (2 << 5);
+	    data[1] = index & 0xFF;        /* LSB */
+	    data[2] = (index >> 8) & 0xFF; /* MSB */
+	    data[3] = subIndex;
+	    for (i = 4 ; i < 8 ; i++)
+		    data[i] = 0;
+    }
 	d->transfers[line].Callback = Callback;
 	err = sendSDO(d, SDO_CLIENT, CliNbr, data);
 	if (err) {
@@ -1655,9 +2131,9 @@
  **
  ** @return
  **/
-UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType)
-{
-	return _readNetworkDict (d, nodeId, index, subIndex, dataType, NULL);
+UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, UNS8 useBlockMode)
+{
+	return _readNetworkDict (d, nodeId, index, subIndex, dataType, NULL, useBlockMode);
 }
 
 /*!
@@ -1672,12 +2148,12 @@
  **
  ** @return
  **/
-UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
-{
-	return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback);
-}
-
-UNS8 readNetworkDictCallbackAI (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback)
+UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode)
+{
+	return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback, useBlockMode);
+}
+
+UNS8 readNetworkDictCallbackAI (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode)
 {
 	UNS8 ret;
 	UNS16 lastIndex;
@@ -1685,7 +2161,7 @@
 	UNS8 nodeIdServer;
 	UNS8 i;
 
-	ret = _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback);
+	ret = _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback, useBlockMode);
 	if(ret == 0xFE)
 	{
 		offset = d->firstIndex->SDO_CLT;
@@ -1709,7 +2185,7 @@
 				*(UNS32*)d->objdict[offset].pSubindex[1].pObject = (UNS32)(0x600 + nodeId);
 				*(UNS32*)d->objdict[offset].pSubindex[2].pObject = (UNS32)(0x580 + nodeId);
 				*(UNS8*) d->objdict[offset].pSubindex[3].pObject = nodeId;
-				return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback);
+				return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback, useBlockMode);
 			}
 			offset++;
 		}
@@ -1731,10 +2207,13 @@
  ** @param d
  ** @param nodeId
  ** @param data
- ** @param size pointer to expected size, changed into returned size. Expected size will be truncated to transfered data size
+ ** @param size : *size MUST contain the size of *data buffer before calling
+ **                     The function set it to the actual number of written bytes
  ** @param abortCode
  **
  ** @return
+ **    SDO_PROVIDED_BUFFER_TOO_SMALL if *data is not big enough
+ **    or any transmission status value.
  **/
 UNS8 getReadResultNetworkDict (CO_Data* d, UNS8 nodeId, void* data, UNS32 *size,
 		UNS32 * abortCode)
@@ -1747,25 +2226,40 @@
 
 	/* First let's find the corresponding SDO client in our OD  */
 	CliNbr = GetSDOClientFromNodeId(d, nodeId);
-	if(CliNbr >= 0xFE)
+	if(CliNbr >= 0xFE) {
+        *size = 0;
 		return SDO_ABORTED_INTERNAL;
+    }
 
 	/* Looking for the line tranfert. */
 	err = getSDOlineOnUse(d, CliNbr, SDO_CLIENT, &line);
 	if (err) {
 		MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId);
-		return SDO_ABORTED_INTERNAL;
-	}
-	* abortCode = d->transfers[line].abortCode;
-	if (d->transfers[line].state != SDO_FINISHED)
+        *size = 0;
+        return SDO_ABORTED_INTERNAL;
+	}
+
+    /* If transfert not finished just return, but if aborted set abort code and size to 0 */
+    if (d->transfers[line].state != SDO_FINISHED) {
+	    if((d->transfers[line].state == SDO_ABORTED_RCV) || (d->transfers[line].state == SDO_ABORTED_INTERNAL)) {
+            *abortCode = d->transfers[line].abortCode;
+            *size = 0;
+        }
 		return d->transfers[line].state;
+    }
 
 	/* if SDO initiated with e=0 and s=0 count is null, offset carry effective size*/
 	if( d->transfers[line].count == 0)
 		d->transfers[line].count = d->transfers[line].offset;
-	/* use transfers[line].count as max size */
-	if( d->transfers[line].count < *size )
-		*size = d->transfers[line].count;
+
+    /* Check if the provided buffer is big enough */
+    if(*size < d->transfers[line].count) {
+		*size = 0;
+		return SDO_PROVIDED_BUFFER_TOO_SMALL;
+    }
+	
+    /* Give back actual size */
+    *size = d->transfers[line].count;
 
 	/* Copy payload to data pointer */
 #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
@@ -1799,6 +2293,7 @@
 # endif
 	}
 #endif //SDO_DYNAMIC_BUFFER_ALLOCATION
+    resetSDOline(d, line);
 	return SDO_FINISHED;
 }
 
@@ -1830,5 +2325,8 @@
 		return SDO_ABORTED_INTERNAL;
 	}
 	* abortCode = d->transfers[line].abortCode;
-	return d->transfers[line].state;
-}
+    if (d->transfers[line].state != SDO_FINISHED)
+	    return d->transfers[line].state;
+    resetSDOline(d, line);
+	return SDO_FINISHED;
+}