From cb9228fd27de022cd4a39c08e46335931b93a319 Mon Sep 17 00:00:00 2001 From: Giulio Benetti Date: Wed, 21 Sep 2022 00:59:30 +0200 Subject: [PATCH] Add type Integer48 Let's add integer48 type by using uint64_t as base data type. Note that Integer48 type can't use Direct Storage. Signed-off-by: Giulio Benetti --- src/core/co_core.h | 1 + src/object/basic/co_integer48.c | 120 ++++++++++++++++++++++++++++++++ src/object/basic/co_integer48.h | 54 ++++++++++++++ src/service/cia301/co_pdo.c | 20 ++++-- 4 files changed, 190 insertions(+), 5 deletions(-) create mode 100644 src/object/basic/co_integer48.c create mode 100644 src/object/basic/co_integer48.h diff --git a/src/core/co_core.h b/src/core/co_core.h index 812fa23f..092585a6 100644 --- a/src/core/co_core.h +++ b/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" diff --git a/src/object/basic/co_integer48.c b/src/object/basic/co_integer48.c new file mode 100644 index 00000000..b10ef88c --- /dev/null +++ b/src/object/basic/co_integer48.c @@ -0,0 +1,120 @@ +/****************************************************************************** + 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) { + CONodeFatalError(); + } 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) { + return CO_ERR_OBJ_READ; + } 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) { + return CO_ERR_OBJ_WRITE; + } 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); +} diff --git a/src/object/basic/co_integer48.h b/src/object/basic/co_integer48.h new file mode 100644 index 00000000..81df3362 --- /dev/null +++ b/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_ */ diff --git a/src/service/cia301/co_pdo.c b/src/service/cia301/co_pdo.c index 3794dfb3..78f2ff3f 100644 --- a/src/service/cia301/co_pdo.c +++ b/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; } @@ -559,6 +562,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; @@ -571,8 +575,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); @@ -589,6 +593,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);