# HG changeset patch
# User Christian Taedcke
# Date 1284040566 -7200
# Node ID 033fe6f1ec3c6360e6596230936920d23b473bcb
# Parent  30a0e926bb2eb0d00e70a0feba90b3a849de9fd8
ADDED: - dynamic memory allocation for sdo transfer using malloc and free
CHANGED: - enabled lss, enabled dynamic buffer allocation and set max sim sdo transfers to 32

diff -r 30a0e926bb2e -r 033fe6f1ec3c include/data.h
--- a/include/data.h	Thu Sep 09 15:38:20 2010 +0200
+++ b/include/data.h	Thu Sep 09 15:56:06 2010 +0200
@@ -128,6 +128,25 @@
 
 #define NMTable_Initializer Unknown_state,
 
+#ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
+#define s_transfer_Initializer {\
+		0,          /* nodeId */\
+		0,          /* wohami */\
+		SDO_RESET,  /* state */\
+		0,          /* toggle */\
+		0,          /* abortCode */\
+		0,          /* index */\
+		0,          /* subIndex */\
+		0,          /* count */\
+		0,          /* offset */\
+		{0},        /* data (static use, so that all the table is initialize at 0)*/\
+    NULL,       /* dynamicData */ \
+    0,          /* dynamicDataSize */ \
+		0,          /* dataType */\
+		-1,         /* timer */\
+		NULL        /* Callback */\
+	  },
+#else
 #define s_transfer_Initializer {\
 		0,          /* nodeId */\
 		0,          /* wohami */\
@@ -143,6 +162,7 @@
 		-1,         /* timer */\
 		NULL        /* Callback */\
 	  },
+#endif //SDO_DYNAMIC_BUFFER_ALLOCATION
 
 #define ERROR_DATA_INITIALIZER \
 	{\
diff -r 30a0e926bb2e -r 033fe6f1ec3c include/sdo.h
--- a/include/sdo.h	Thu Sep 09 15:38:20 2010 +0200
+++ b/include/sdo.h	Thu Sep 09 15:56:06 2010 +0200
@@ -65,6 +65,10 @@
                               * (with respect to CANOPEN_BIG_ENDIAN)
                               */
   UNS8           data [SDO_MAX_LENGTH_TRANSFERT];
+#ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
+  UNS8           *dynamicData;
+  UNS32          dynamicDataSize;
+#endif //SDO_DYNAMIC_BUFFER_ALLOCATION
   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.
diff -r 30a0e926bb2e -r 033fe6f1ec3c include/win32/config.h
--- a/include/win32/config.h	Thu Sep 09 15:38:20 2010 +0200
+++ b/include/win32/config.h	Thu Sep 09 15:56:06 2010 +0200
@@ -22,9 +22,13 @@
 #ifndef _CONFIG_H_
 #define _CONFIG_H_
 
+#define CO_ENABLE_LSS
+
 #define MAX_CAN_BUS_ID 1
+#define SDO_DYNAMIC_BUFFER_ALLOCATION //New define, if SDO_MAX_LENGTH_TRANSFERT is exceeded allocate data buffer dynamically
+#define SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE (1024 * 128)
 #define SDO_MAX_LENGTH_TRANSFERT 32
-#define SDO_MAX_SIMULTANEOUS_TRANSFERTS 4
+#define SDO_MAX_SIMULTANEOUS_TRANSFERTS 32
 #define NMT_MAX_NODE_ID 128
 #define SDO_TIMEOUT_MS 3000
 #define MAX_NB_TIMER 32
@@ -38,7 +42,7 @@
 #define LSS_FS_TIMEOUT_MS 100
 
 #define REPEAT_SDO_MAX_SIMULTANEOUS_TRANSFERTS_TIMES(repeat)\
-repeat repeat repeat repeat
+repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat
 #define REPEAT_NMT_MAX_NODE_ID_TIMES(repeat)\
 repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat repeat
 #define REPEAT_EMCY_MAX_ERRORS_TIMES(repeat)\
diff -r 30a0e926bb2e -r 033fe6f1ec3c src/sdo.c
--- a/src/sdo.c	Thu Sep 09 15:38:20 2010 +0200
+++ b/src/sdo.c	Thu Sep 09 15:56:06 2010 +0200
@@ -191,8 +191,23 @@
   if( d->transfers[line].count == 0)
   	d->transfers[line].count = d->transfers[line].offset;
   size = d->transfers[line].count;
+
+#ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
+  if (size > SDO_MAX_LENGTH_TRANSFERT)
+  {
+    errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,
+			 (void *) d->transfers[line].dynamicData, &size, 1);
+  }
+  else
+  {
+   errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,
+			 (void *) d->transfers[line].data, &size, 1);
+  }
+#else //SDO_DYNAMIC_BUFFER_ALLOCATION
   errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,
 			 (void *) d->transfers[line].data, &size, 1);
+#endif //SDO_DYNAMIC_BUFFER_ALLOCATION
+
   if (errorCode != OD_SUCCESSFUL)
     return errorCode;
   MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line);
@@ -217,10 +232,18 @@
   MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index);
   MSG_WAR(0x3A06, "  subIndex : ", d->transfers[line].subIndex);
 
+#ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
+  //TODO: Read the size of the object. Depending o it put data into data or dynamicData
   errorCode = getODentry(d, 	d->transfers[line].index,
   				d->transfers[line].subIndex,
   				(void *)d->transfers[line].data,
   				&size, &dataType, 1);
+#else //SDO_DYNAMIC_BUFFER_ALLOCATION
+  errorCode = getODentry(d, 	d->transfers[line].index,
+  				d->transfers[line].subIndex,
+  				(void *)d->transfers[line].data,
+  				&size, &dataType, 1);
+#endif //SDO_DYNAMIC_BUFFER_ALLOCATION
 
   if (errorCode != OD_SUCCESSFUL)
     return errorCode;
@@ -245,17 +268,38 @@
   UNS8 i;
   UNS32 offset;
 
+#ifndef SDO_DYNAMIC_BUFFER_ALLOCATION
   if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
     MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
     return 0xFF;
   }
+#endif //SDO_DYNAMIC_BUFFER_ALLOCATION
+
     if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) {
     MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes);
     return 0xFF;
   }
   offset = d->transfers[line].offset;
-  for (i = 0 ; i < nbBytes ; i++)
+#ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
+  if (d->transfers[line].count <= SDO_MAX_LENGTH_TRANSFERT)
+  {
+    for (i = 0 ; i < nbBytes ; i++)
+      * (data + i) = d->transfers[line].data[offset + i];
+  }
+  else
+  {
+    if (d->transfers[line].dynamicData == NULL)
+    {
+      MSG_ERR(0x1A11,"SDO's dynamic buffer not allocated. Line", line);
+      return 0xFF;
+    }
+    for (i = 0 ; i < nbBytes ; i++)
+      * (data + i) = d->transfers[line].dynamicData[offset + i];
+  }
+#else //SDO_DYNAMIC_BUFFER_ALLOCATION
+    for (i = 0 ; i < nbBytes ; i++)
     * (data + i) = d->transfers[line].data[offset + i];
+#endif //SDO_DYNAMIC_BUFFER_ALLOCATION
   d->transfers[line].offset = d->transfers[line].offset + nbBytes;
   return 0;
 }
@@ -274,14 +318,50 @@
 {
   UNS8 i;
   UNS32 offset;
-
+#ifndef SDO_DYNAMIC_BUFFER_ALLOCATION
   if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
     MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
     return 0xFF;
   }
+#endif //SDO_DYNAMIC_BUFFER_ALLOCATION
+
   offset = d->transfers[line].offset;
+#ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
+  {
+    UNS8* lineData = d->transfers[line].data;
+    if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
+      if (d->transfers[line].dynamicData == NULL) {
+        d->transfers[line].dynamicData = (UNS8*) malloc(SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);
+        d->transfers[line].dynamicDataSize = SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE;
+
+        if (d->transfers[line].dynamicData == NULL) {
+          MSG_ERR(0x1A15,"SDO allocating dynamic buffer failed, size", SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);
+          return 0xFF;
+        }
+        //Copy present data
+        memcpy(d->transfers[line].dynamicData, d->transfers[line].data, offset);
+      }
+      else if ((d->transfers[line].offset + nbBytes) > d->transfers[line].dynamicDataSize)
+      {
+        UNS8* newDynamicBuffer = (UNS8*) realloc(d->transfers[line].dynamicData, d->transfers[line].dynamicDataSize + SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);
+        if (newDynamicBuffer == NULL) {
+          MSG_ERR(0x1A15,"SDO reallocating dynamic buffer failed, size", d->transfers[line].dynamicDataSize + SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);
+          return 0xFF;
+        }
+        d->transfers[line].dynamicData = newDynamicBuffer;
+        d->transfers[line].dynamicDataSize += SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE;
+      }
+      lineData = d->transfers[line].dynamicData;
+    }
+    
+    for (i = 0 ; i < nbBytes ; i++)
+      lineData[offset + i] = * (data + i);
+  }
+#else //SDO_DYNAMIC_BUFFER_ALLOCATION
   for (i = 0 ; i < nbBytes ; i++)
     d->transfers[line].data[offset + i] = * (data + i);
+#endif //SDO_DYNAMIC_BUFFER_ALLOCATION
+
   d->transfers[line].offset = d->transfers[line].offset + nbBytes;
   return 0;
 }
@@ -338,6 +418,11 @@
     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
 }
 
 /*!
@@ -369,6 +454,11 @@
   d->transfers[line].offset = 0;
   d->transfers[line].dataType = 0;
   d->transfers[line].Callback = NULL;
+#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
   return 0;
 }
 
@@ -417,7 +507,7 @@
      (d->transfers[i].state != SDO_ABORTED_INTERNAL) &&
 	 (d->transfers[i].nodeId == nodeId) &&
 	 (d->transfers[i].whoami == whoami) ) {
-      *line = i;
+      if (line) *line = i;
       return 0;
     }
   }
@@ -476,10 +566,13 @@
 **/
 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS32 nbBytes)
 {
+#ifndef SDO_DYNAMIC_BUFFER_ALLOCATION
   if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) {
     MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
     return 0xFF;
   }
+#endif //SDO_DYNAMIC_BUFFER_ALLOCATION
+
   d->transfers[line].count = nbBytes;
   return 0;
 }
@@ -1315,8 +1408,35 @@
   d->transfers[line].count = count;
   d->transfers[line].dataType = dataType;
 
+#ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
+  {
+    UNS8* lineData = d->transfers[line].data;
+    if (count > SDO_MAX_LENGTH_TRANSFERT)
+    {
+      d->transfers[line].dynamicData = (UNS8*) malloc(count);
+      d->transfers[line].dynamicDataSize = count;
+      if (d->transfers[line].dynamicData == NULL)
+      {
+        MSG_ERR(0x1AC9, "SDO. Error. Could not allocate enough bytes : ", count);
+        return 0xFE;
+      }
+      lineData = d->transfers[line].dynamicData;
+    }
+#endif //SDO_DYNAMIC_BUFFER_ALLOCATION
+
   /* Copy data to transfers structure. */
   for (j = 0 ; j < count ; j++) {
+#ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
+# ifdef CANOPEN_BIG_ENDIAN
+      if (dataType == 0 && endianize)
+        lineData[count - 1 - j] = ((char *)data)[j];
+      else /* String of bytes. */
+        lineData[j] = ((char *)data)[j];
+#  else
+      lineData[j] = ((char *)data)[j];
+#  endif
+  }
+#else //SDO_DYNAMIC_BUFFER_ALLOCATION
 # ifdef CANOPEN_BIG_ENDIAN
     if (dataType == 0 && endianize)
       d->transfers[line].data[count - 1 - j] = ((char *)data)[j];
@@ -1325,6 +1445,7 @@
 #  else
     d->transfers[line].data[j] = ((char *)data)[j];
 #  endif
+#endif //SDO_DYNAMIC_BUFFER_ALLOCATION
   }
   /* Send the SDO to the server. Initiate download, cs=1. */
   sdo.nodeId = nodeId;
@@ -1655,7 +1776,28 @@
   /* use transfers[line].count as max size */
   if( d->transfers[line].count < *size )
   	*size = d->transfers[line].count;
+
   /* Copy payload to data pointer */
+#ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
+  {
+    UNS8 *lineData = d->transfers[line].data;
+
+    if (d->transfers[line].dynamicData && d->transfers[line].dynamicDataSize)
+    {
+      lineData = d->transfers[line].dynamicData;
+    }
+    for  ( i = 0 ; i < *size ; i++) {
+# ifdef CANOPEN_BIG_ENDIAN
+      if (d->transfers[line].dataType != visible_string)
+        ( (char *) data)[*size - 1 - i] = lineData[i];
+      else /* String of bytes. */
+        ( (char *) data)[i] = lineData[i];
+# else
+      ( (char *) data)[i] = lineData[i];
+# endif
+    }
+  }
+#else //SDO_DYNAMIC_BUFFER_ALLOCATION
   for  ( i = 0 ; i < *size ; i++) {
 # ifdef CANOPEN_BIG_ENDIAN
     if (d->transfers[line].dataType != visible_string)
@@ -1666,6 +1808,7 @@
     ( (char *) data)[i] = d->transfers[line].data[i];
 # endif
   }
+#endif //SDO_DYNAMIC_BUFFER_ALLOCATION
   return SDO_FINISHED;
 }