Add handling of write-only objects

Change-Id: I9bfdb11c5d1d83cb2c23115e204ffe6875b4457b

soes/esc_coe.c

@@ -29,6 +29,16 @@
 #define SDO_COMPLETE_ACCESS(byte) \
    ((byte & COE_COMPLETEACCESS) == COE_COMPLETEACCESS)
 
+#define READ_ACCESS(access, state) \
+      (((access & ATYPE_Rpre) && (state == ESCpreop))   || \
+       ((access & ATYPE_Rsafe) && (state == ESCsafeop)) || \
+       ((access & ATYPE_Rop) && (state == ESCop)))
+
+#define WRITE_ACCESS(access, state) \
+      (((access & ATYPE_Wpre) && (state == ESCpreop))   || \
+       ((access & ATYPE_Wsafe) && (state == ESCsafeop)) || \
+       ((access & ATYPE_Wop) && (state == ESCop)))
+
 typedef enum { UPLOAD, DOWNLOAD } load_t;
 
 /** Search for an object sub-index.
@@ -241,6 +251,19 @@ void SDO_abort (uint16_t index, uint8_t subindex, uint32_t abortcode)
    }
 }
 
+static void set_state_idle(uint16_t index,
+                           uint8_t subindex,
+                           uint32_t abortcode)
+{
+   if (abortcode != 0)
+   {
+      SDO_abort (index, subindex, abortcode);
+   }
+
+   MBXcontrol[0].state = MBXstate_idle;
+   ESCvar.xoe = 0;
+}
+
 /** Function for responding on requested SDO Upload, sending the content
  *  requested in a free Mailbox buffer. Depending of size of data expedited,
  *  normal or segmented transfer is used. On error an SDO Abort will be sent.
@@ -266,6 +289,13 @@ void SDO_upload (void)
       if (nsub >= 0)
       {
          objd = SDOobjects[nidx].objdesc;
+         uint8_t access = (objd + nsub)->flags & 0x3f;
+         uint8_t state = ESCvar.ALstatus & 0x0f;
+         if (!READ_ACCESS(access, state))
+         {
+            set_state_idle (index, subindex, ABORT_WRITEONLY);
+            return;
+         }
          MBXout = ESC_claimbuffer ();
          if (MBXout)
          {
@@ -384,19 +414,6 @@ void SDO_upload (void)
    ESCvar.xoe = 0;
 }
 
-static void set_state_idle(uint16_t index,
-                           uint8_t subindex,
-                           uint32_t abortcode)
-{
-   if (abortcode != 0)
-   {
-      SDO_abort (index, subindex, abortcode);
-   }
-
-   MBXcontrol[0].state = MBXstate_idle;
-   ESCvar.xoe = 0;
-}
-
 static uint32_t complete_access_get_variables(_COEsdo *coesdo, uint16_t *index,
                                               uint8_t *subindex, int16_t *nidx,
                                               int16_t *nsub)
@@ -443,23 +460,12 @@ static uint32_t complete_access_subindex_loop(const _objd *objd,
 
    while (nsub <= SDOobjects[nidx].maxsub)
    {
-      if (load_type == DOWNLOAD)
-      {
-         uint8_t access = (objd + nsub)->flags & 0x3f;
-         uint8_t state = ESCvar.ALstatus & 0x0f;
-         if ((access != ATYPE_RW) &&
-             ((access != ATYPE_RWpre) ||
-              ((state != ESCpreop) && (state != ESCsafeop) && (state != ESCop))))
-         {
-            return (access == ATYPE_RWpre) ?
-                  ABORT_NOTINTHISSTATE : ABORT_READONLY;
-         }
-      }
-
       uint8_t bitlen = (objd + nsub)->bitlength;
       void *ul_source = ((objd + nsub)->data != NULL) ?
             (objd + nsub)->data : (void *)&((objd + nsub)->value);
       uint8_t bitoffset = size % 8;
+      uint8_t access = (objd + nsub)->flags & 0x3f;
+      uint8_t state = ESCvar.ALstatus & 0x0f;
 
       if ((bitlen % 8) == 0)
       {
@@ -473,10 +479,19 @@ static uint32_t complete_access_subindex_loop(const _objd *objd,
             /* copy a non-bit data type to a byte boundary */
             if (load_type == UPLOAD)
             {
-               memcpy(&mbxdata[BITS2BYTES(size)], ul_source,
+               if (READ_ACCESS(access, state))
-                     BITS2BYTES(bitlen));
+               {
+                  memcpy(&mbxdata[BITS2BYTES(size)], ul_source,
+                        BITS2BYTES(bitlen));
+               }
+               else
+               {
+                  /* return zeroes for upload of WO objects */
+                  memset(&mbxdata[BITS2BYTES(size)], 0, BITS2BYTES(bitlen));
+               }
             }
-            else
+            /* download of RO objects shall be ignored */
+            else if (WRITE_ACCESS(access, state))
             {
                memcpy((objd + nsub)->data, &mbxdata[BITS2BYTES(size)],
                      BITS2BYTES(bitlen));
@@ -487,8 +502,15 @@ static uint32_t complete_access_subindex_loop(const _objd *objd,
       {
          /* copy a bit data type into correct position */
          uint8_t bitmask = (1 << bitlen) - 1;
-         mbxdata[BITS2BYTES(size)] |=
+         if (READ_ACCESS(access, state))
-               (*(uint8_t *)ul_source & bitmask) << bitoffset;
+         {
+            mbxdata[BITS2BYTES(size)] |=
+                  (*(uint8_t *)ul_source & bitmask) << bitoffset;
+         }
+         else
+         {
+            mbxdata[BITS2BYTES(size)] &= ~(bitmask << bitoffset);
+         }
       }
 
       /* Subindex 0 is padded to 16 bit */
@@ -721,8 +743,7 @@ void SDO_download (void)
          objd = SDOobjects[nidx].objdesc;
          uint8_t access = (objd + nsub)->flags & 0x3f;
          uint8_t state = ESCvar.ALstatus & 0x0f;
-         if (access == ATYPE_RW ||
+         if (WRITE_ACCESS(access, state))
-             (access == ATYPE_RWpre && state == ESCpreop))
          {
             /* expedited? */
             if (coesdo->command & COE_EXPEDITED_INDICATOR)

soes/esc_coe.h

@@ -94,7 +94,8 @@ typedef struct
 #define ATYPE_TXPDO             0x80
 
 #define ATYPE_RO                (ATYPE_Rpre | ATYPE_Rsafe | ATYPE_Rop)
-#define ATYPE_RW                (ATYPE_Wpre | ATYPE_Wsafe | ATYPE_Wop | ATYPE_RO)
+#define ATYPE_WO                (ATYPE_Wpre | ATYPE_Wsafe | ATYPE_Wop)
+#define ATYPE_RW                (ATYPE_RO | ATYPE_WO)
 #define ATYPE_RWpre             (ATYPE_Wpre | ATYPE_RO)
 
 #define TX_PDO_OBJIDX           0x1c13