Add type Integer48

Let's add integer48 type by using uint64_t as base data type.

Signed-off-by: Giulio Benetti <[email protected]>

src/core/co_core.h

@@ -34,6 +34,7 @@ extern "C" {
 #include "co_integer8.h"
 #include "co_integer16.h"
 #include "co_integer32.h"
+#include "co_integer48.h"
 
 /* cia301 types */
 #include "co_emcy_hist.h"

src/object/basic/co_integer48.c

@@ -0,0 +1,121 @@
+/******************************************************************************
+   Copyright 2020 Embedded Office GmbH & Co. KG
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
+******************************************************************************/
+
+/******************************************************************************
+* INCLUDES
+******************************************************************************/
+
+#include "co_core.h"
+
+/******************************************************************************
+* PRIVATE DEFINES
+******************************************************************************/
+
+#define COT_ENTRY_SIZE    (uint32_t)6
+
+/******************************************************************************
+* PRIVATE FUNCTIONS
+******************************************************************************/
+
+static uint32_t COTInt48Size (struct CO_OBJ_T *obj, struct CO_NODE_T *node, uint32_t width);
+static CO_ERR   COTInt48Read (struct CO_OBJ_T *obj, struct CO_NODE_T *node, void *buffer, uint32_t size);
+static CO_ERR   COTInt48Write(struct CO_OBJ_T *obj, struct CO_NODE_T *node, void *buffer, uint32_t size);
+
+/******************************************************************************
+* PUBLIC GLOBALS
+******************************************************************************/
+
+const CO_OBJ_TYPE COTInt48 = { COTInt48Size, 0, COTInt48Read, COTInt48Write, 0 };
+
+/******************************************************************************
+* FUNCTIONS
+******************************************************************************/
+
+static uint32_t COTInt48Size(struct CO_OBJ_T *obj, struct CO_NODE_T *node, uint32_t width)
+{
+    uint32_t result = (uint32_t)0;
+
+    CO_UNUSED(node);
+    CO_UNUSED(width);
+
+    if (CO_IS_DIRECT(obj->Key) != 0) {
+        result = (uint32_t)COT_ENTRY_SIZE;
+    } else {
+        /* check for valid reference */
+        if ((obj->Data) != (CO_DATA)0) {
+            result = (uint32_t)COT_ENTRY_SIZE;
+        }
+    }
+    return (result);
+}
+
+static CO_ERR COTInt48Read(struct CO_OBJ_T *obj, struct CO_NODE_T *node, void *buffer, uint32_t size)
+{
+    CO_ERR   result = CO_ERR_NONE;
+    uint64_t value;
+
+    CO_UNUSED(node);
+    ASSERT_PTR_ERR(obj, CO_ERR_BAD_ARG);
+    ASSERT_PTR_ERR(buffer, CO_ERR_BAD_ARG);
+
+    if (CO_IS_DIRECT(obj->Key) != 0) {
+        value = (uint64_t)(obj->Data);
+    } else {
+        value = *((uint64_t *)(obj->Data));
+    }
+    if (CO_IS_NODEID(obj->Key) != 0) {
+        value += node->NodeId;
+    }
+    if (size == COT_ENTRY_SIZE) {
+        *((uint64_t *)buffer) = value;
+    } else {
+        result = CO_ERR_BAD_ARG;
+    }
+    return (result);
+}
+
+static CO_ERR COTInt48Write(struct CO_OBJ_T *obj, struct CO_NODE_T *node, void *buffer, uint32_t size)
+{
+    CO_ERR   result = CO_ERR_NONE;
+    uint64_t value;
+    uint64_t oldValue;
+
+    CO_UNUSED(node);
+    ASSERT_PTR_ERR(obj, CO_ERR_BAD_ARG);
+    ASSERT_PTR_ERR(buffer, CO_ERR_BAD_ARG);
+
+    value = *((uint64_t *)buffer);
+    if (size == COT_ENTRY_SIZE) {
+        if (CO_IS_NODEID(obj->Key) != 0) {
+            value -= node->NodeId;
+        }
+        if (CO_IS_DIRECT(obj->Key) != 0) {
+            oldValue = (uint64_t)obj->Data;
+            obj->Data = (CO_DATA)(value);
+        } else {
+            oldValue = *((uint64_t *)(obj->Data));
+            *((uint64_t *)(obj->Data)) = value;
+        }
+        if ((CO_IS_ASYNC(obj->Key)  != 0    ) &&
+            (CO_IS_PDOMAP(obj->Key) != 0    ) &&
+            (oldValue               != value)) {
+            COTPdoTrigObj(node->TPdo, obj);
+        }
+    } else {
+        result = CO_ERR_BAD_ARG;
+    }
+    return (result);
+}

src/object/basic/co_integer48.h

@@ -0,0 +1,54 @@
+/******************************************************************************
+   Copyright 2020 Embedded Office GmbH & Co. KG
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
+******************************************************************************/
+
+#ifndef CO_INTEGER48_H_
+#define CO_INTEGER48_H_
+
+#ifdef __cplusplus               /* for compatibility with C++ environments  */
+extern "C" {
+#endif
+
+/******************************************************************************
+* INCLUDES
+******************************************************************************/
+
+#include "co_types.h"
+#include "co_err.h"
+#include "co_obj.h"
+
+/******************************************************************************
+* DEFINES
+******************************************************************************/
+
+#define CO_TUNSIGNED48  ((const CO_OBJ_TYPE *)&COTInt48)
+#define CO_TSIGNED48    ((const CO_OBJ_TYPE *)&COTInt48)
+
+/******************************************************************************
+* PUBLIC CONSTANTS
+******************************************************************************/
+
+/*! \brief OBJECT TYPE SIGNED48 / UNSIGNED48
+*
+*    This type is a basic type for 48bit values, regardless of signed or
+*    unsigned usage.
+*/
+extern const CO_OBJ_TYPE COTInt48;
+
+#ifdef __cplusplus               /* for compatibility with C++ environments  */
+}
+#endif
+
+#endif  /* #ifndef CO_INTEGER48_H_ */

src/service/cia301/co_pdo.c

@@ -269,7 +269,7 @@ void COTPdoTx(CO_TPDO *pdo)
     CO_IF_FRM  frm;
     uint32_t   sz;
     uint8_t    pdosz;
-    uint32_t   data;
+    uint64_t   data;
     uint8_t    num;
 
     if ((pdo->Node->Nmt.Allowed & CO_PDO_ALLOWED) == 0) {
@@ -310,8 +310,8 @@ void COTPdoTx(CO_TPDO *pdo)
     frm.DLC        = 0;
     for (num = 0; num < pdo->ObjNum; num++) {
         pdosz = pdo->Size[num];
-        if (pdosz <= 4) {
-            /* supported mapping: 1 to 4 bytes */
+        if (pdosz <= 6) {
+            /* supported mapping: 1 to 6 bytes */
             sz = COObjGetSize(pdo->Map[num], pdo->Node, 0L);
             if (sz <= (uint32_t)(8 - frm.DLC)) {
                 if (pdosz == 3) {
@@ -331,6 +331,9 @@ void COTPdoTx(CO_TPDO *pdo)
                     } else {
                         CO_SET_LONG(&frm, data, frm.DLC);
                     }
+                } else if (sz == 6u) {
+                    CO_SET_WORD(&frm, data, frm.DLC);
+                    CO_SET_LONG(&frm, (data >> 16), (frm.DLC + 2));
                 }
                 frm.DLC += pdosz;
             }
@@ -572,6 +575,7 @@ CO_RPDO *CORPdoCheck(CO_RPDO *pdo, CO_IF_FRM *frm)
 void CORPdoWrite(CO_RPDO *pdo, CO_IF_FRM *frm)
 {
     CO_OBJ  *obj;
+    uint64_t val64;
     uint32_t val32;
     uint16_t val16;
     uint8_t  val08;
@@ -584,8 +588,8 @@ void CORPdoWrite(CO_RPDO *pdo, CO_IF_FRM *frm)
         obj   = pdo->Map[on];
         pdosz = pdo->Size[on];
         if (obj != 0) {
-            if (pdosz <= 4) {
-                /* supported mapping: 1 to 4 bytes */
+            if (pdosz <= 6) {
+                /* supported mapping: 1 to 6 bytes */
                 sz = (uint8_t)COObjGetSize(obj, pdo->Node, 0L);
                 if (sz == 1u) {
                     val08 = CO_GET_BYTE(frm, dlc);
@@ -602,6 +606,12 @@ void CORPdoWrite(CO_RPDO *pdo, CO_IF_FRM *frm)
                     }
                     dlc += pdosz;
                     COObjWrValue(obj, pdo->Node, (void *)&val32, sz);
+                } else if (sz == 6u) {
+                    val64 = CO_GET_LONG(frm, dlc);
+                    val64 |= (uint64_t)(CO_GET_WORD(frm, dlc + 4)) << 32;
+                    val64 &= 0x0000FFFFFFFFFFFF;
+                    dlc += pdosz;
+                    COObjWrValue(obj, pdo->Node, (void *)&val64, sz);
                 }
             } else {
                 CORpdoWriteData(frm, dlc, pdosz, obj);