Merge pull request #80 from lrsdnlssn/master

SOES extensions: Add Download SDO Segment

soes/esc.c

@@ -395,6 +395,9 @@ void ESC_stopmbx (void)
    ESCvar.frags = 0;
    ESCvar.fragsleft = 0;
    ESCvar.txcue = 0;
+   ESCvar.index = 0;
+   ESCvar.subindex = 0;
+   ESCvar.flags = 0;
 }
 
 /** Read Receive mailbox and store data in local ESCvar.MBX variable.

soes/esc.h

@@ -161,17 +161,41 @@
 #define MBXERR_INVALIDSIZE              0x0008
 
 #define ABORT_NOTOGGLE                  0x05030000
+#define ABORT_TRANSFER_TIMEOUT          0x05040000
 #define ABORT_UNKNOWN                   0x05040001
+#define ABORT_INVALID_BLOCK_SIZE        0x05040002
+#define ABORT_INVALID_SEQUENCE_NUMBER   0x05040003
+#define ABORT_BLOCK_CRC_ERROR           0x05040004
+#define ABORT_OUT_OF_MEMORY             0x05040005
 #define ABORT_UNSUPPORTED               0x06010000
 #define ABORT_WRITEONLY                 0x06010001
 #define ABORT_READONLY                  0x06010002
 #define ABORT_SUBINDEX0_NOT_ZERO        0x06010003
+#define ABORT_CA_NOT_SUPPORTED          0x06010004
 #define ABORT_EXCEEDS_MBOX_SIZE         0x06010005
+#define ABORT_SDO_DOWNLOAD_BLOCKED      0x06010006
 #define ABORT_NOOBJECT                  0x06020000
+#define ABORT_MAPPING_OBJECT_ERROR      0x06040041
+#define ABORT_MAPPING_LENGTH_ERROR      0x06040042
+#define ABORT_GENERAL_PARAMETER_ERROR   0x06040043
+#define ABORT_GENERAL_DEVICE_ERROR      0x06040047
+#define ABORT_HARDWARE_ERROR            0x06060000
 #define ABORT_TYPEMISMATCH              0x06070010
+#define ABORT_DATATYPE_TOO_HIGH         0x06070012
+#define ABORT_DATATYPE_TOO_LOW          0x06070013
 #define ABORT_NOSUBINDEX                0x06090011
+#define ABORT_VALUE_EXCEEDED            0x06090030
+#define ABORT_VALUE_TOO_HIGH            0x06090031
+#define ABORT_VALUE_TOO_LOW             0x06090032
+#define ABORT_MODULE_LIST_MISMATCH      0x06090033
+#define ABORT_MAX_VAL_LESS_THAN_MIN_VAL 0x06090036
+#define ABORT_RESOURCE_NOT_AVAILABLE    0x060A0023
 #define ABORT_GENERALERROR              0x08000000
+#define ABORT_DATA_STORE_ERROR          0x08000020
+#define ABORT_DATA_STORE_LOCAL_ERROR    0x08000021
 #define ABORT_NOTINTHISSTATE            0x08000022
+#define ABORT_OBJECT_DICTIONARY_ERROR   0x08000023
+#define ABORT_NO_DATA_AVAILABLE         0x08000024
 
 #define MBXstate_idle                   0x00
 #define MBXstate_inclaim                0x01
@@ -194,6 +218,8 @@
 #define COE_COMMAND_UPLOADSEGREQ        0x60
 #define COE_COMMAND_DOWNLOADREQUEST     0x20
 #define COE_COMMAND_DOWNLOADRESPONSE    0x60
+#define COE_COMMAND_DOWNLOADSEGREQ      0x00
+#define COE_COMMAND_DOWNLOADSEGRESP     0x20
 #define COE_COMMAND_LASTSEGMENTBIT      0x01
 #define COE_SIZE_INDICATOR              0x01
 #define COE_EXPEDITED_INDICATOR         0x02
@@ -456,6 +482,8 @@ typedef struct
    uint16_t entries;
    uint16_t frags;
    uint16_t fragsleft;
+   uint16_t index;
+   uint8_t subindex;
    uint16_t flags;
 
    uint8_t toggle;

soes/esc_coe.c

@@ -296,26 +296,28 @@ void SDO_upload (void)
                /* expedited response i.e. length<=4 bytes */
                coeres->command = COE_COMMAND_UPLOADRESPONSE +
                   COE_SIZE_INDICATOR + COE_EXPEDITED_INDICATOR + dss;
-               if ((objd + nsub)->data == NULL)
+               /* convert bits to bytes */
+               size = BITS2BYTES(size);
+               void *dataptr = ((objd + nsub)->data) ?
+                     (objd + nsub)->data : (void *)&((objd + nsub)->value);
+               abort = ESC_upload_pre_objecthandler (index, subindex,
+                     dataptr, size, (objd + nsub)->flags);
+               if (abort == 0)
                {
-                  /* use constant value */
+                  if ((objd + nsub)->data == NULL)
-                  coeres->size = htoel ((objd + nsub)->value);
+                  {
-               }
+                     /* use constant value */
-               else
+                     coeres->size = htoel ((objd + nsub)->value);
-               {
+                  }
-                  /* convert bits to bytes */
+                  else
-                  size = BITS2BYTES(size);
-                  abort = ESC_upload_pre_objecthandler (index, subindex,
-                        (objd + nsub)->data, size, (objd + nsub)->flags);
-                  if (abort == 0)
                   {
                      /* use dynamic data */
                      copy2mbx ((objd + nsub)->data, &(coeres->size), size);
                   }
-                  else
+               }
-                  {
+               else
-                     SDO_abort (index, subindex, abort);
+               {
-                  }
+                  SDO_abort (index, subindex, abort);
                }
             }
             else
@@ -325,12 +327,12 @@ void SDO_upload (void)
                   COE_SIZE_INDICATOR;
                /* convert bits to bytes */
                size = BITS2BYTES(size);
+               /* set total size in bytes */
+               ESCvar.frags = size;
                coeres->size = htoel (size);
                if ((size + COE_HEADERSIZE) > ESC_MBXDSIZE)
                {
                   /* segmented transfer needed */
-                  /* set total size in bytes */
-                  ESCvar.frags = size;
                   /* limit to mailbox size */
                   size = ESC_MBXDSIZE - COE_HEADERSIZE;
                   /* number of bytes done */
@@ -346,7 +348,7 @@ void SDO_upload (void)
                }
                coeres->mbxheader.length = htoes (COE_HEADERSIZE + size);
                abort = ESC_upload_pre_objecthandler (index, subindex,
-                     (objd + nsub)->data, size, (objd + nsub)->flags);
+                     (objd + nsub)->data, ESCvar.frags, (objd + nsub)->flags);
                if (abort == 0)
                {
                   /* use dynamic data */
@@ -434,7 +436,7 @@ static uint32_t complete_access_subindex_loop(const _objd *objd,
        (objd->datatype == DTYPE_OCTET_STRING)   ||
        (objd->datatype == DTYPE_UNICODE_STRING))
    {
-      return ABORT_UNSUPPORTED;
+      return ABORT_CA_NOT_SUPPORTED;
    }
 
    uint32_t size = 0;
@@ -604,6 +606,7 @@ static void SDO_upload_complete_access (void)
          /* signal segmented transfer */
          ESCvar.segmented = MBXSEU;
          ESCvar.data = ESCvar.mbxdata;
+         ESCvar.flags = COMPLETE_ACCESS_FLAG;
       }
 
       coeres->mbxheader.length = htoes (COE_HEADERSIZE + size);
@@ -629,7 +632,7 @@ static void SDO_upload_complete_access (void)
 /** Function for handling the following SDO Upload if previous SDOUpload
  * response was flagged it needed to be segmented.
  */
-static void SDO_uploadsegment (bool complete_access)
+static void SDO_uploadsegment (void)
 {
    _COEsdo *coesdo, *coeres;
    uint8_t MBXout;
@@ -643,10 +646,6 @@ static void SDO_uploadsegment (bool complete_access)
       size = ESCvar.frags - ESCvar.fragsleft;
       uint8_t command = COE_COMMAND_UPLOADSEGMENT +
             (coesdo->command & COE_TOGGLEBIT);  /* copy toggle bit */
-      if (complete_access)
-      {
-         command |= COE_COMPLETEACCESS;
-      }
       init_coesdo(coeres, COE_SDORESPONSE, command,
             coesdo->index, coesdo->subindex);
       if ((size + COE_SEGMENTHEADERSIZE) > ESC_MBXDSIZE)
@@ -680,15 +679,8 @@ static void SDO_uploadsegment (bool complete_access)
 
       if (ESCvar.segmented == 0)
       {
-         uint16_t flags = ESCvar.flags;
-
-         if (complete_access)
-         {
-            flags |= COMPLETE_ACCESS_FLAG;
-         }
-
          abort = ESC_upload_post_objecthandler (etohs (coesdo->index),
-               coesdo->subindex, flags);
+               coesdo->subindex, ESCvar.flags);
          if (abort != 0)
          {
             SDO_abort (etohs (coesdo->index), coesdo->subindex, abort);
@@ -756,6 +748,21 @@ void SDO_download (void)
                );
                if (abort == 0)
                {
+                  if ((size > 4) &&
+                      (size > (coesdo->mbxheader.length - COE_HEADERSIZE)))
+                  {
+                     size = coesdo->mbxheader.length - COE_HEADERSIZE;
+                     /* signal segmented transfer */
+                     ESCvar.segmented = MBXSED;
+                     ESCvar.data = (objd + nsub)->data + size;
+                     ESCvar.index = index;
+                     ESCvar.subindex = subindex;
+                     ESCvar.flags = (objd + nsub)->flags;
+                  }
+                  else
+                  {
+                     ESCvar.segmented = 0;
+                  }
                   copy2mbx (mbxdata, (objd + nsub)->data, size);
                   MBXout = ESC_claimbuffer ();
                   if (MBXout)
@@ -771,11 +778,14 @@ void SDO_download (void)
                      coeres->size = htoel (0);
                      MBXcontrol[MBXout].state = MBXstate_outreq;
                   }
-                  /* external object write handler */
+                  if (ESCvar.segmented == 0)
-                  abort = ESC_download_post_objecthandler (index, subindex, (objd + nsub)->flags);
-                  if (abort != 0)
                   {
-                     SDO_abort (index, subindex, abort);
+                     /* external object write handler */
+                     abort = ESC_download_post_objecthandler (index, subindex, (objd + nsub)->flags);
+                     if (abort != 0)
+                     {
+                        SDO_abort (index, subindex, abort);
+                     }
                   }
                }
                else
@@ -906,6 +916,51 @@ static void SDO_download_complete_access (void)
    set_state_idle (index, subindex, 0);
 }
 
+static void SDO_downloadsegment (void)
+{
+   _COEsdo *coesdo = (_COEsdo *) &MBX[0];
+   uint8_t MBXout = ESC_claimbuffer ();
+   if (MBXout)
+   {
+      _COEsdo *coeres = (_COEsdo *) &MBX[MBXout * ESC_MBXSIZE];
+      uint32_t size = coesdo->mbxheader.length - 3;
+      if (size == 7)
+      {
+         size = 7 - ((coesdo->command >> 1) & 7);
+      }
+      uint8_t command = COE_COMMAND_DOWNLOADSEGRESP +
+            (coesdo->command & COE_TOGGLEBIT);  /* copy toggle bit */
+      init_coesdo(coeres, COE_SDORESPONSE, command, 0, 0);
+
+      uint32_t *mbxdata = (uint32_t *)&(coesdo->index);  /* data pointer */
+      copy2mbx (mbxdata, (uint8_t *)ESCvar.data, size);
+
+      if (coesdo->command & COE_COMMAND_LASTSEGMENTBIT)
+      {
+         /* last segment */
+         ESCvar.segmented = 0;
+
+         /* external object write handler */
+         uint32_t abort = ESC_download_post_objecthandler
+               (ESCvar.index, ESCvar.subindex, ESCvar.flags);
+         if (abort != 0)
+         {
+            set_state_idle (ESCvar.index, ESCvar.subindex, abort);
+            return;
+         }
+      }
+      else
+      {
+         /* more segmented transfer needed: increase offset */
+         ESCvar.data += size;
+      }
+
+      MBXcontrol[MBXout].state = MBXstate_outreq;
+   }
+
+   set_state_idle (0, 0, 0);
+}
+
 /** Function for sending an SDO Info Error reply.
  *
  * @param[in] abortcode  = = abort code to send in reply
@@ -1258,34 +1313,31 @@ void ESC_coeprocess (void)
       coesdo = (_COEsdo *) &MBX[0];
       coeobjdesc = (_COEobjdesc *) &MBX[0];
       service = etohs (coesdo->coeheader.numberservice) >> 12;
-      /* initiate SDO upload request */
+      if (service == COE_SDOREQUEST)
-      if ((service == COE_SDOREQUEST)
-          && (SDO_COMMAND(coesdo->command) == COE_COMMAND_UPLOADREQUEST)
-          && (etohs (coesdo->mbxheader.length) == COE_HEADERSIZE))
       {
-         if (SDO_COMPLETE_ACCESS(coesdo->command))
+         if ((SDO_COMMAND(coesdo->command) == COE_COMMAND_UPLOADREQUEST)
+               && (etohs (coesdo->mbxheader.length) == COE_HEADERSIZE))
          {
-            SDO_upload_complete_access ();
+            /* initiate SDO upload request */
+            if (SDO_COMPLETE_ACCESS(coesdo->command))
+            {
+               SDO_upload_complete_access ();
+            }
+            else
+            {
+               SDO_upload ();
+            }
          }
-         else
+         else if (((coesdo->command & 0xef) == COE_COMMAND_UPLOADSEGREQ)
+               && (etohs (coesdo->mbxheader.length) == COE_HEADERSIZE)
+               && (ESCvar.segmented == MBXSEU))
          {
-            SDO_upload ();
+            /* SDO upload segment request */
+            SDO_uploadsegment ();
          }
-      }
+         else if (SDO_COMMAND(coesdo->command) == COE_COMMAND_DOWNLOADREQUEST)
-      /* SDO upload segment request */
-      if ((service == COE_SDOREQUEST)
-          && ((coesdo->command & 0xef) == COE_COMMAND_UPLOADSEGREQ)
-          && (etohs (coesdo->mbxheader.length) == COE_HEADERSIZE)
-          && (ESCvar.segmented == MBXSEU))
-      {
-         SDO_uploadsegment (SDO_COMPLETE_ACCESS(coesdo->command));
-      }
-      /* initiate SDO download request */
-      else
-      {
-         if ((service == COE_SDOREQUEST)
-             && (SDO_COMMAND(coesdo->command) == COE_COMMAND_DOWNLOADREQUEST))
          {
+            /* initiate SDO download request */
             if (SDO_COMPLETE_ACCESS(coesdo->command))
             {
                SDO_download_complete_access ();
@@ -1295,46 +1347,51 @@ void ESC_coeprocess (void)
                SDO_download ();
             }
          }
-         /* initiate SDO get OD list */
+         else if (SDO_COMMAND(coesdo->command) == COE_COMMAND_DOWNLOADSEGREQ)
+         {
+            /* SDO download segment request */
+            SDO_downloadsegment ();
+         }
+      }
+      /* initiate SDO get OD list */
+      else
+      {
+         if ((service == COE_SDOINFORMATION)
+               && (coeobjdesc->infoheader.opcode == 0x01))
+         {
+            SDO_getodlist ();
+         }
+         /* initiate SDO get OD */
          else
          {
             if ((service == COE_SDOINFORMATION)
-                && (coeobjdesc->infoheader.opcode == 0x01))
+                  && (coeobjdesc->infoheader.opcode == 0x03))
             {
-               SDO_getodlist ();
+               SDO_getod ();
             }
-            /* initiate SDO get OD */
+            /* initiate SDO get ED */
             else
             {
                if ((service == COE_SDOINFORMATION)
-                   && (coeobjdesc->infoheader.opcode == 0x03))
+                     && (coeobjdesc->infoheader.opcode == 0x05))
                {
-                  SDO_getod ();
+                  SDO_geted ();
                }
-               /* initiate SDO get ED */
                else
                {
-                  if ((service == COE_SDOINFORMATION)
+                  /* COE not recognised above */
-                      && (coeobjdesc->infoheader.opcode == 0x05))
+                  if (ESCvar.xoe == MBXCOE)
                   {
-                     SDO_geted ();
+                     if (service == 0)
-                  }
-                  else
-                  {
-                     /* COE not recognised above */
-                     if (ESCvar.xoe == MBXCOE)
                      {
-                        if (service == 0)
+                        MBX_error (MBXERR_INVALIDHEADER);
-                        {
-                           MBX_error (MBXERR_INVALIDHEADER);
-                        }
-                        else
-                        {
-                           SDO_abort (etohs (coesdo->index), coesdo->subindex, ABORT_UNSUPPORTED);
-                        }
-                        MBXcontrol[0].state = MBXstate_idle;
-                        ESCvar.xoe = 0;
                      }
+                     else
+                     {
+                        SDO_abort (etohs (coesdo->index), coesdo->subindex, ABORT_UNSUPPORTED);
+                     }
+                     MBXcontrol[0].state = MBXstate_idle;
+                     ESCvar.xoe = 0;
                   }
                }
             }
@@ -1439,17 +1496,20 @@ static uint64_t COE_getValue (const _objd * obj)
    case DTYPE_BOOLEAN:
    case DTYPE_UNSIGNED8:
    case DTYPE_INTEGER8:
+   case DTYPE_BITARR8:
       value = *(uint8_t *)obj->data;
       break;
 
    case DTYPE_UNSIGNED16:
    case DTYPE_INTEGER16:
+   case DTYPE_BITARR16:
       value = *(uint16_t *)obj->data;
       break;
 
    case DTYPE_REAL32:
    case DTYPE_UNSIGNED32:
    case DTYPE_INTEGER32:
+   case DTYPE_BITARR32:
       value = *(uint32_t *)obj->data;
       break;
 
@@ -1490,17 +1550,20 @@ static void COE_setValue (const _objd * obj, uint64_t value)
    case DTYPE_BOOLEAN:
    case DTYPE_UNSIGNED8:
    case DTYPE_INTEGER8:
+   case DTYPE_BITARR8:
       *(uint8_t *)obj->data = value & UINT8_MAX;
       break;
 
    case DTYPE_UNSIGNED16:
    case DTYPE_INTEGER16:
+   case DTYPE_BITARR16:
       *(uint16_t *)obj->data = value & UINT16_MAX;
       break;
 
    case DTYPE_REAL32:
    case DTYPE_UNSIGNED32:
    case DTYPE_INTEGER32:
+   case DTYPE_BITARR32:
       *(uint32_t *)obj->data = value & UINT32_MAX;
       break;
 

soes/esc_coe.h

@@ -72,6 +72,9 @@ typedef struct
 #define DTYPE_REAL64            0x0011
 #define DTYPE_PDO_MAPPING       0x0021
 #define DTYPE_IDENTITY          0x0023
+#define DTYPE_BITARR8           0x002D
+#define DTYPE_BITARR16          0x002E
+#define DTYPE_BITARR32          0x002F
 #define DTYPE_BIT1              0x0030
 #define DTYPE_BIT2              0x0031
 #define DTYPE_BIT3              0x0032