nmt.c

Go to the documentation of this file.

 
#include "co.h"
#include <lely/util/diag.h>
#ifndef LELY_NO_CO_MASTER
#include <lely/can/buf.h>
#include <lely/co/csdo.h>
#include <lely/util/time.h>
#endif
#include <lely/co/dev.h>
#ifndef LELY_NO_CO_EMCY
#include <lely/co/emcy.h>
#endif
#include <lely/co/nmt.h>
#include <lely/co/obj.h>
#ifndef LELY_NO_CO_RPDO
#include <lely/co/rpdo.h>
#endif
#include <lely/co/sdo.h>
#ifndef LELY_NO_CO_TPDO
#include <lely/co/tpdo.h>
#endif
#include <lely/co/val.h>
#ifndef LELY_NO_CO_MASTER
#include "nmt_boot.h"
#include "nmt_cfg.h"
#endif
#include "nmt_hb.h"
#include "nmt_srv.h"
 
#include <assert.h>
#include <stdlib.h>
 
struct __co_nmt_state;
typedef const struct __co_nmt_state co_nmt_state_t;
 
#ifndef LELY_NO_CO_MASTER
struct co_nmt_slave {
        co_nmt_t *nmt;
        can_recv_t *recv;
        can_timer_t *timer;
        co_unsigned32_t assignment;
        co_unsigned8_t est;
        co_unsigned8_t rst;
        char es;
        int booted;
        co_nmt_boot_t *boot;
        co_nmt_cfg_t *cfg;
        co_nmt_cfg_con_t *cfg_con;
        void *cfg_data;
        co_unsigned16_t gt;
        co_unsigned8_t ltf;
        co_unsigned8_t rtr;
        int ng_state;
};
#endif
 
struct __co_nmt {
        can_net_t *net;
        co_dev_t *dev;
        co_unsigned8_t id;
        void *dcf_node;
        void *dcf_comm;
        co_nmt_state_t *state;
        struct co_nmt_srv srv;
        co_unsigned32_t startup;
#ifndef LELY_NO_CO_MASTER
        int master;
#endif
        can_recv_t *recv_000;
        co_nmt_cs_ind_t *cs_ind;
        void *cs_data;
        can_recv_t *recv_700;
#ifndef LELY_NO_CO_MASTER
        co_nmt_ng_ind_t *ng_ind;
        void *ng_data;
#endif
        can_timer_t *ec_timer;
        co_unsigned8_t st;
        co_unsigned16_t gt;
        co_unsigned8_t ltf;
        int lg_state;
        co_nmt_lg_ind_t *lg_ind;
        void *lg_data;
        co_unsigned16_t ms;
        co_nmt_hb_t **hbs;
        co_unsigned8_t nhb;
        co_nmt_hb_ind_t *hb_ind;
        void *hb_data;
        co_nmt_st_ind_t *st_ind;
        void *st_data;
#ifndef LELY_NO_CO_MASTER
        struct can_buf buf;
        struct timespec inhibit;
        can_timer_t *cs_timer;
#ifndef LELY_NO_CO_LSS
        co_nmt_lss_req_t *lss_req;
        void *lss_data;
#endif
        int halt;
        struct co_nmt_slave slaves[CO_NUM_NODES];
        int timeout;
        co_nmt_boot_ind_t *boot_ind;
        void *boot_data;
        co_nmt_cfg_ind_t *cfg_ind;
        void *cfg_data;
        co_nmt_sdo_ind_t *dn_ind;
        void *dn_data;
        co_nmt_sdo_ind_t *up_ind;
        void *up_data;
#endif
        co_nmt_sync_ind_t *sync_ind;
        void *sync_data;
};
 
static co_unsigned32_t co_100c_dn_ind(
                co_sub_t *sub, struct co_sdo_req *req, void *data);
 
static co_unsigned32_t co_100d_dn_ind(
                co_sub_t *sub, struct co_sdo_req *req, void *data);
 
static co_unsigned32_t co_1016_dn_ind(
                co_sub_t *sub, struct co_sdo_req *req, void *data);
 
static co_unsigned32_t co_1017_dn_ind(
                co_sub_t *sub, struct co_sdo_req *req, void *data);
 
#ifndef LELY_NO_CO_MASTER
static co_unsigned32_t co_1f25_dn_ind(
                co_sub_t *sub, struct co_sdo_req *req, void *data);
#endif
 
static co_unsigned32_t co_1f80_dn_ind(
                co_sub_t *sub, struct co_sdo_req *req, void *data);
 
#ifndef LELY_NO_CO_MASTER
static co_unsigned32_t co_1f82_dn_ind(
                co_sub_t *sub, struct co_sdo_req *req, void *data);
#endif
 
static int co_nmt_recv_000(const struct can_msg *msg, void *data);
 
static int co_nmt_recv_700(const struct can_msg *msg, void *data);
 
#ifndef LELY_NO_CO_MASTER
static int co_nmt_ng_timer(const struct timespec *tp, void *data);
#endif
 
static int co_nmt_ec_timer(const struct timespec *tp, void *data);
 
#ifndef LELY_NO_CO_MASTER
static int co_nmt_cs_timer(const struct timespec *tp, void *data);
#endif
 
static void co_nmt_st_ind(co_nmt_t *nmt, co_unsigned8_t id, co_unsigned8_t st);
 
#ifndef LELY_NO_CO_MASTER
static void default_ng_ind(co_nmt_t *nmt, co_unsigned8_t id, int state,
                int reason, void *data);
#endif
 
static void default_lg_ind(co_nmt_t *nmt, int state, void *data);
 
static void default_hb_ind(co_nmt_t *nmt, co_unsigned8_t id, int state,
                int reason, void *data);
 
static void default_st_ind(co_nmt_t *nmt, co_unsigned8_t id, co_unsigned8_t st,
                void *data);
 
#ifndef LELY_NO_CO_MASTER
 
static void co_nmt_dn_ind(const co_csdo_t *sdo, co_unsigned16_t idx,
                co_unsigned8_t subidx, size_t size, size_t nbyte, void *data);
 
static void co_nmt_up_ind(const co_csdo_t *sdo, co_unsigned16_t idx,
                co_unsigned8_t subidx, size_t size, size_t nbyte, void *data);
 
#endif
 
static void co_nmt_enter(co_nmt_t *nmt, co_nmt_state_t *next);
 
static inline void co_nmt_emit_cs(co_nmt_t *nmt, co_unsigned8_t cs);
 
#ifndef LELY_NO_CO_MASTER
static inline void co_nmt_emit_boot(
                co_nmt_t *nmt, co_unsigned8_t id, co_unsigned8_t st, char es);
#endif
 
struct __co_nmt_state {
        co_nmt_state_t *(*on_enter)(co_nmt_t *nmt);
        co_nmt_state_t *(*on_cs)(co_nmt_t *nmt, co_unsigned8_t cs);
#ifndef LELY_NO_CO_MASTER
        co_nmt_state_t *(*on_boot)(co_nmt_t *nmt, co_unsigned8_t id,
                        co_unsigned8_t st, char es);
#endif
        void (*on_leave)(co_nmt_t *nmt);
};
 
#define LELY_CO_DEFINE_STATE(name, ...) \
        static co_nmt_state_t *const name = &(co_nmt_state_t){ __VA_ARGS__ };
 
#ifndef LELY_NO_CO_MASTER
static co_nmt_state_t *co_nmt_default_on_boot(
                co_nmt_t *nmt, co_unsigned8_t id, co_unsigned8_t st, char es);
#endif
 
static co_nmt_state_t *co_nmt_init_on_cs(co_nmt_t *nmt, co_unsigned8_t cs);
 
// clang-format off
LELY_CO_DEFINE_STATE(co_nmt_init_state,
        .on_cs = &co_nmt_init_on_cs
)
// clang-format on
 
 
static co_nmt_state_t *co_nmt_reset_node_on_enter(co_nmt_t *nmt);
 
// clang-format off
LELY_CO_DEFINE_STATE(co_nmt_reset_node_state,
        .on_enter = &co_nmt_reset_node_on_enter
)
// clang-format on
 
static co_nmt_state_t *co_nmt_reset_comm_on_enter(co_nmt_t *nmt);
 
static co_nmt_state_t *co_nmt_reset_comm_on_cs(
                co_nmt_t *nmt, co_unsigned8_t cs);
 
// clang-format off
LELY_CO_DEFINE_STATE(co_nmt_reset_comm_state,
        .on_enter = &co_nmt_reset_comm_on_enter,
        .on_cs = &co_nmt_reset_comm_on_cs
)
// clang-format on
 
static co_nmt_state_t *co_nmt_bootup_on_enter(co_nmt_t *nmt);
 
static co_nmt_state_t *co_nmt_bootup_on_cs(co_nmt_t *nmt, co_unsigned8_t cs);
 
// clang-format off
LELY_CO_DEFINE_STATE(co_nmt_bootup_state,
        .on_enter = &co_nmt_bootup_on_enter,
        .on_cs = &co_nmt_bootup_on_cs
)
// clang-format on
 
static co_nmt_state_t *co_nmt_preop_on_enter(co_nmt_t *nmt);
 
static co_nmt_state_t *co_nmt_preop_on_cs(co_nmt_t *nmt, co_unsigned8_t cs);
 
#ifndef LELY_NO_CO_MASTER
static co_nmt_state_t *co_nmt_preop_on_boot(
                co_nmt_t *nmt, co_unsigned8_t id, co_unsigned8_t st, char es);
#endif
 
// clang-format off
#ifdef LELY_NO_CO_MASTER
LELY_CO_DEFINE_STATE(co_nmt_preop_state,
        .on_enter = &co_nmt_preop_on_enter,
        .on_cs = &co_nmt_preop_on_cs
)
#else
LELY_CO_DEFINE_STATE(co_nmt_preop_state,
        .on_enter = &co_nmt_preop_on_enter,
        .on_cs = &co_nmt_preop_on_cs,
        .on_boot = &co_nmt_preop_on_boot
)
#endif
// clang-format on
 
static co_nmt_state_t *co_nmt_start_on_enter(co_nmt_t *nmt);
 
static co_nmt_state_t *co_nmt_start_on_cs(co_nmt_t *nmt, co_unsigned8_t cs);
 
// clang-format off
#ifdef LELY_NO_CO_MASTER
LELY_CO_DEFINE_STATE(co_nmt_start_state,
        .on_enter = &co_nmt_start_on_enter,
        .on_cs = &co_nmt_start_on_cs
)
#else
LELY_CO_DEFINE_STATE(co_nmt_start_state,
        .on_enter = &co_nmt_start_on_enter,
        .on_cs = &co_nmt_start_on_cs,
        .on_boot = &co_nmt_default_on_boot
)
#endif
// clang-format on
 
static co_nmt_state_t *co_nmt_stop_on_enter(co_nmt_t *nmt);
 
static co_nmt_state_t *co_nmt_stop_on_cs(co_nmt_t *nmt, co_unsigned8_t cs);
 
// clang-format off
#ifdef LELY_NO_CO_MASTER
LELY_CO_DEFINE_STATE(co_nmt_stop_state,
        .on_enter = &co_nmt_stop_on_enter,
        .on_cs = &co_nmt_stop_on_cs
)
#else
LELY_CO_DEFINE_STATE(co_nmt_stop_state,
        .on_enter = &co_nmt_stop_on_enter,
        .on_cs = &co_nmt_stop_on_cs,
        .on_boot = &co_nmt_default_on_boot
)
#endif
// clang-format on
 
#undef LELY_CO_DEFINE_STATE
 
static co_nmt_state_t *co_nmt_startup(co_nmt_t *nmt);
 
#ifndef LELY_NO_CO_MASTER
static co_nmt_state_t *co_nmt_startup_master(co_nmt_t *nmt);
#endif
 
static co_nmt_state_t *co_nmt_startup_slave(co_nmt_t *nmt);
 
static void co_nmt_ec_init(co_nmt_t *nmt);
 
static void co_nmt_ec_fini(co_nmt_t *nmt);
 
static int co_nmt_ec_update(co_nmt_t *nmt);
 
static int co_nmt_ec_send_res(co_nmt_t *nmt, co_unsigned8_t st);
 
static void co_nmt_hb_init(co_nmt_t *nmt);
 
static void co_nmt_hb_fini(co_nmt_t *nmt);
 
#ifndef LELY_NO_CO_MASTER
 
static void co_nmt_slaves_init(co_nmt_t *nmt);
 
static void co_nmt_slaves_fini(co_nmt_t *nmt);
 
static int co_nmt_slaves_boot(co_nmt_t *nmt);
 
#endif
 
#define CO_NMT_PREOP_SRV \
        (CO_NMT_STOP_SRV | CO_NMT_SRV_SDO | CO_NMT_SRV_SYNC | CO_NMT_SRV_TIME \
                        | CO_NMT_SRV_EMCY)
 
#define CO_NMT_START_SRV (CO_NMT_PREOP_SRV | CO_NMT_SRV_PDO)
 
#define CO_NMT_STOP_SRV CO_NMT_SRV_LSS
 
co_unsigned32_t
co_dev_cfg_hb(co_dev_t *dev, co_unsigned8_t id, co_unsigned16_t ms)
{
        assert(dev);
 
        co_obj_t *obj_1016 = co_dev_find_obj(dev, 0x1016);
        if (__unlikely(!obj_1016))
                return CO_SDO_AC_NO_OBJ;
 
        co_unsigned8_t n = co_obj_get_val_u8(obj_1016, 0x00);
        co_unsigned8_t i = 0;
        // If the node-ID is valid, find an existing heartbeat consumer with the
        // same ID.
        if (id && id <= CO_NUM_NODES) {
                for (i = 1; i <= n; i++) {
                        co_unsigned32_t val_i = co_obj_get_val_u32(obj_1016, i);
                        co_unsigned8_t id_i = (val_i >> 16) & 0xff;
                        if (id_i == id)
                                break;
                }
        }
        // If the node-ID is invalid or no heartbeat consumer exists, find an
        // unused consumer.
        if (!i || i > n) {
                for (i = 1; i <= n; i++) {
                        co_unsigned32_t val_i = co_obj_get_val_u32(obj_1016, i);
                        co_unsigned8_t id_i = (val_i >> 16) & 0xff;
                        if (!id_i || id_i > CO_NUM_NODES)
                                break;
                }
        }
 
        if (__unlikely(!i || i > n))
                return CO_SDO_AC_NO_SUB;
        co_sub_t *sub = co_obj_find_sub(obj_1016, i);
        if (__unlikely(!sub))
                return CO_SDO_AC_NO_SUB;
 
        co_unsigned32_t val = ((co_unsigned32_t)id << 16) | ms;
        return co_sub_dn_ind_val(sub, CO_DEFTYPE_UNSIGNED32, &val);
}
 
const char *
co_nmt_es2str(char es)
{
        switch (es) {
        case 'A': return "The CANopen device is not listed in object 1F81.";
        case 'B':
                return "No response received for upload request of object 1000.";
        case 'C':
                return "Value of object 1000 from CANopen device is different to value in object 1F84 (Device type).";
        case 'D':
                return "Value of object 1018 sub-index 01 from CANopen device is different to value in object 1F85 (Vendor-ID).";
        case 'E':
                return "Heartbeat event. No heartbeat message received from CANopen device.";
        case 'F':
                return "Node guarding event. No confirmation for guarding request received from CANopen device.";
        case 'G':
                return "Objects for program download are not configured or inconsistent.";
        case 'H':
                return "Software update is required, but not allowed because of configuration or current status.";
        case 'I':
                return "Software update is required, but program download failed.";
        case 'J': return "Configuration download failed.";
        case 'K':
                return "Heartbeat event during start error control service. No heartbeat message received from CANopen device during start error control service.";
        case 'L': return "NMT slave was initially operational.";
        case 'M':
                return "Value of object 1018 sub-index 02 from CANopen device is different to value in object 1F86 (Product code).";
        case 'N':
                return "Value of object 1018 sub-index 03 from CANopen device is different to value in object 1F87 (Revision number).";
        case 'O':
                return "Value of object 1018 sub-index 04 from CANopen device is different to value in object 1F88 (Serial number).";
        default: return "Unknown error status";
        }
}
 
void *
__co_nmt_alloc(void)
{
        void *ptr = malloc(sizeof(struct __co_nmt));
        if (__unlikely(!ptr))
                set_errc(errno2c(errno));
        return ptr;
}
 
void
__co_nmt_free(void *ptr)
{
        free(ptr);
}
 
struct __co_nmt *
__co_nmt_init(struct __co_nmt *nmt, can_net_t *net, co_dev_t *dev)
{
        assert(nmt);
        assert(net);
        assert(dev);
 
        int errc = 0;
 
        nmt->net = net;
        nmt->dev = dev;
 
        nmt->id = co_dev_get_id(nmt->dev);
 
        // Store a concise DCF containing the application parameters.
        // clang-format off
        if (__unlikely(co_dev_write_dcf(nmt->dev, 0x2000, 0x9fff,
                        &nmt->dcf_node) == -1)) {
                // clang-format on
                errc = get_errc();
                goto error_write_dcf_node;
        }
 
        // Store a concise DCF containing the communication parameters.
        // clang-format off
        if (__unlikely(co_dev_write_dcf(nmt->dev, 0x1000, 0x1fff,
                        &nmt->dcf_comm) == -1)) {
                // clang-format on
                errc = get_errc();
                goto error_write_dcf_comm;
        }
 
        nmt->state = NULL;
 
        co_nmt_srv_init(&nmt->srv, nmt);
 
        nmt->startup = 0;
#ifndef LELY_NO_CO_MASTER
        nmt->master = 0;
#endif
 
        // Create the CAN frame receiver for NMT messages.
        nmt->recv_000 = can_recv_create();
        if (__unlikely(!nmt->recv_000)) {
                errc = get_errc();
                goto error_create_recv_000;
        }
        can_recv_set_func(nmt->recv_000, &co_nmt_recv_000, nmt);
 
        nmt->cs_ind = NULL;
        nmt->cs_data = NULL;
 
        nmt->recv_700 = NULL;
 
#ifndef LELY_NO_CO_MASTER
        nmt->ng_ind = &default_ng_ind;
        nmt->ng_data = NULL;
#endif
 
        nmt->ec_timer = NULL;
 
        nmt->st = CO_NMT_ST_BOOTUP;
        nmt->gt = 0;
        nmt->ltf = 0;
 
        nmt->lg_state = CO_NMT_EC_RESOLVED;
        nmt->lg_ind = &default_lg_ind;
        nmt->lg_data = NULL;
 
        nmt->ms = 0;
 
        nmt->hbs = NULL;
        nmt->nhb = 0;
        nmt->hb_ind = &default_hb_ind;
        nmt->hb_data = NULL;
 
        nmt->st_ind = &default_st_ind;
        nmt->st_data = NULL;
 
#ifndef LELY_NO_CO_MASTER
        nmt->buf = (struct can_buf)CAN_BUF_INIT;
        can_net_get_time(nmt->net, &nmt->inhibit);
        nmt->cs_timer = NULL;
 
#ifndef LELY_NO_CO_LSS
        nmt->lss_req = NULL;
        nmt->lss_data = NULL;
#endif
 
        nmt->halt = 0;
 
        for (co_unsigned8_t id = 1; id <= CO_NUM_NODES; id++) {
                struct co_nmt_slave *slave = &nmt->slaves[id - 1];
                slave->nmt = nmt;
 
                slave->recv = NULL;
                slave->timer = NULL;
 
                slave->assignment = 0;
                slave->est = 0;
                slave->rst = 0;
                slave->es = 0;
 
                slave->booted = 0;
                slave->boot = NULL;
 
                slave->cfg = NULL;
                slave->cfg_con = NULL;
                slave->cfg_data = NULL;
 
                slave->gt = 0;
                slave->ltf = 0;
                slave->rtr = 0;
                slave->ng_state = CO_NMT_EC_RESOLVED;
        }
 
        nmt->timeout = LELY_CO_NMT_TIMEOUT;
 
        nmt->boot_ind = NULL;
        nmt->boot_data = NULL;
        nmt->cfg_ind = NULL;
        nmt->cfg_data = NULL;
        nmt->dn_ind = NULL;
        nmt->dn_data = NULL;
        nmt->up_ind = NULL;
        nmt->up_data = NULL;
#endif
        nmt->sync_ind = NULL;
        nmt->sync_data = NULL;
 
        // Set the download indication function for the guard time.
        co_obj_t *obj_100c = co_dev_find_obj(nmt->dev, 0x100c);
        if (obj_100c)
                co_obj_set_dn_ind(obj_100c, &co_100c_dn_ind, nmt);
 
        // Set the download indication function for the life time factor.
        co_obj_t *obj_100d = co_dev_find_obj(nmt->dev, 0x100d);
        if (obj_100d)
                co_obj_set_dn_ind(obj_100d, &co_100d_dn_ind, nmt);
 
        // Set the download indication function for the consumer heartbeat time.
        co_obj_t *obj_1016 = co_dev_find_obj(nmt->dev, 0x1016);
        if (obj_1016)
                co_obj_set_dn_ind(obj_1016, &co_1016_dn_ind, nmt);
 
        // Set the download indication function for the producer heartbeat time.
        co_obj_t *obj_1017 = co_dev_find_obj(nmt->dev, 0x1017);
        if (obj_1017)
                co_obj_set_dn_ind(obj_1017, &co_1017_dn_ind, nmt);
 
#ifndef LELY_NO_CO_MASTER
        // Set the download indication function for the configuration request
        // value.
        co_obj_t *obj_1f25 = co_dev_find_obj(nmt->dev, 0x1f25);
        if (obj_1f25)
                co_obj_set_dn_ind(obj_1f25, &co_1f25_dn_ind, nmt);
#endif
 
        // Set the download indication function for the NMT startup value.
        co_obj_t *obj_1f80 = co_dev_find_obj(nmt->dev, 0x1f80);
        if (obj_1f80)
                co_obj_set_dn_ind(obj_1f80, &co_1f80_dn_ind, nmt);
 
#ifndef LELY_NO_CO_MASTER
        // Set the download indication function for the request NMT value.
        co_obj_t *obj_1f82 = co_dev_find_obj(nmt->dev, 0x1f82);
        if (obj_1f82)
                co_obj_set_dn_ind(obj_1f82, &co_1f82_dn_ind, nmt);
#endif
 
        co_nmt_enter(nmt, co_nmt_init_state);
        return nmt;
 
#ifndef LELY_NO_CO_MASTER
        if (obj_1f82)
                co_obj_set_dn_ind(obj_1f82, NULL, NULL);
#endif
        if (obj_1f80)
                co_obj_set_dn_ind(obj_1f80, NULL, NULL);
#ifndef LELY_NO_CO_MASTER
        if (obj_1f25)
                co_obj_set_dn_ind(obj_1f25, NULL, NULL);
#endif
        if (obj_1017)
                co_obj_set_dn_ind(obj_1017, NULL, NULL);
        if (obj_1016)
                co_obj_set_dn_ind(obj_1016, NULL, NULL);
        if (obj_100d)
                co_obj_set_dn_ind(obj_100d, NULL, NULL);
        if (obj_100c)
                co_obj_set_dn_ind(obj_100c, NULL, NULL);
        can_recv_destroy(nmt->recv_000);
error_create_recv_000:
        co_nmt_srv_fini(&nmt->srv);
        co_val_fini(CO_DEFTYPE_DOMAIN, &nmt->dcf_comm);
error_write_dcf_comm:
        co_val_fini(CO_DEFTYPE_DOMAIN, &nmt->dcf_node);
error_write_dcf_node:
        set_errc(errc);
        return NULL;
}
 
void
__co_nmt_fini(struct __co_nmt *nmt)
{
        assert(nmt);
 
#ifndef LELY_NO_CO_MASTER
        // Remove the download indication function for the request NMT value.
        co_obj_t *obj_1f82 = co_dev_find_obj(nmt->dev, 0x1f82);
        if (obj_1f82)
                co_obj_set_dn_ind(obj_1f82, NULL, NULL);
#endif
 
        // Remove the download indication function for the NMT startup value.
        co_obj_t *obj_1f80 = co_dev_find_obj(nmt->dev, 0x1f80);
        if (obj_1f80)
                co_obj_set_dn_ind(obj_1f80, NULL, NULL);
 
#ifndef LELY_NO_CO_MASTER
        // Remove the download indication function for the configuration request
        // value.
        co_obj_t *obj_1f25 = co_dev_find_obj(nmt->dev, 0x1f25);
        if (obj_1f25)
                co_obj_set_dn_ind(obj_1f25, NULL, NULL);
#endif
 
        // Remove the download indication function for the producer heartbeat
        // time.
        co_obj_t *obj_1017 = co_dev_find_obj(nmt->dev, 0x1017);
        if (obj_1017)
                co_obj_set_dn_ind(obj_1017, NULL, NULL);
 
        // Remove the download indication function for the consumer heartbeat
        // time.
        co_obj_t *obj_1016 = co_dev_find_obj(nmt->dev, 0x1016);
        if (obj_1016)
                co_obj_set_dn_ind(obj_1016, NULL, NULL);
 
        // Remove the download indication function for the life time factor.
        co_obj_t *obj_100d = co_dev_find_obj(nmt->dev, 0x100d);
        if (obj_100d)
                co_obj_set_dn_ind(obj_100d, NULL, NULL);
 
        // Remove the download indication function for the guard time.
        co_obj_t *obj_100c = co_dev_find_obj(nmt->dev, 0x100c);
        if (obj_100c)
                co_obj_set_dn_ind(obj_100c, NULL, NULL);
 
#ifndef LELY_NO_CO_MASTER
        co_nmt_slaves_fini(nmt);
#endif
 
        co_nmt_hb_fini(nmt);
 
        co_nmt_ec_fini(nmt);
 
        can_timer_destroy(nmt->ec_timer);
        can_recv_destroy(nmt->recv_700);
 
#ifndef LELY_NO_CO_MASTER
        can_timer_destroy(nmt->cs_timer);
        can_buf_fini(&nmt->buf);
#endif
 
        can_recv_destroy(nmt->recv_000);
 
        co_nmt_srv_fini(&nmt->srv);
 
        co_val_fini(CO_DEFTYPE_DOMAIN, &nmt->dcf_comm);
        co_val_fini(CO_DEFTYPE_DOMAIN, &nmt->dcf_node);
}
 
co_nmt_t *
co_nmt_create(can_net_t *net, co_dev_t *dev)
{
        int errc = 0;
 
        co_nmt_t *nmt = __co_nmt_alloc();
        if (__unlikely(!nmt)) {
                errc = get_errc();
                goto error_alloc_nmt;
        }
 
        if (__unlikely(!__co_nmt_init(nmt, net, dev))) {
                errc = get_errc();
                goto error_init_nmt;
        }
 
        return nmt;
 
error_init_nmt:
        __co_nmt_free(nmt);
error_alloc_nmt:
        set_errc(errc);
        return NULL;
}
 
void
co_nmt_destroy(co_nmt_t *nmt)
{
        if (nmt) {
                __co_nmt_fini(nmt);
                __co_nmt_free(nmt);
        }
}
 
can_net_t *
co_nmt_get_net(const co_nmt_t *nmt)
{
        assert(nmt);
 
        return nmt->net;
}
 
co_dev_t *
co_nmt_get_dev(const co_nmt_t *nmt)
{
        assert(nmt);
 
        return nmt->dev;
}
 
void
co_nmt_get_cs_ind(const co_nmt_t *nmt, co_nmt_cs_ind_t **pind, void **pdata)
{
        assert(nmt);
 
        if (pind)
                *pind = nmt->cs_ind;
        if (pdata)
                *pdata = nmt->cs_data;
}
 
void
co_nmt_set_cs_ind(co_nmt_t *nmt, co_nmt_cs_ind_t *ind, void *data)
{
        assert(nmt);
 
        nmt->cs_ind = ind;
        nmt->cs_data = data;
}
 
#ifndef LELY_NO_CO_MASTER
 
void
co_nmt_get_ng_ind(const co_nmt_t *nmt, co_nmt_ng_ind_t **pind, void **pdata)
{
        assert(nmt);
 
        if (pind)
                *pind = nmt->ng_ind;
        if (pdata)
                *pdata = nmt->ng_data;
}
 
void
co_nmt_set_ng_ind(co_nmt_t *nmt, co_nmt_ng_ind_t *ind, void *data)
{
        assert(nmt);
 
        nmt->ng_ind = ind ? ind : &default_ng_ind;
        nmt->ng_data = ind ? data : NULL;
}
 
void
co_nmt_on_ng(co_nmt_t *nmt, co_unsigned8_t id, int state, int reason)
{
        assert(nmt);
        (void)reason;
 
        if (__unlikely(!id || id > CO_NUM_NODES))
                return;
 
        if (co_nmt_is_master(nmt) && state == CO_NMT_EC_OCCURRED)
                co_nmt_node_err_ind(nmt, id);
}
 
#endif // !LELY_NO_CO_MASTER
 
void
co_nmt_get_lg_ind(const co_nmt_t *nmt, co_nmt_lg_ind_t **pind, void **pdata)
{
        assert(nmt);
 
        if (pind)
                *pind = nmt->lg_ind;
        if (pdata)
                *pdata = nmt->lg_data;
}
 
void
co_nmt_set_lg_ind(co_nmt_t *nmt, co_nmt_lg_ind_t *ind, void *data)
{
        assert(nmt);
 
        nmt->lg_ind = ind ? ind : &default_lg_ind;
        nmt->lg_data = ind ? data : NULL;
}
 
void
co_nmt_on_lg(co_nmt_t *nmt, int state)
{
        assert(nmt);
 
        if (state == CO_NMT_EC_OCCURRED)
                co_nmt_on_err(nmt, 0x8130, 0x10, NULL);
}
 
void
co_nmt_get_hb_ind(const co_nmt_t *nmt, co_nmt_hb_ind_t **pind, void **pdata)
{
        assert(nmt);
 
        if (pind)
                *pind = nmt->hb_ind;
        if (pdata)
                *pdata = nmt->hb_data;
}
 
void
co_nmt_set_hb_ind(co_nmt_t *nmt, co_nmt_hb_ind_t *ind, void *data)
{
        assert(nmt);
 
        nmt->hb_ind = ind ? ind : &default_hb_ind;
        nmt->hb_data = ind ? data : NULL;
}
 
void
co_nmt_on_hb(co_nmt_t *nmt, co_unsigned8_t id, int state, int reason)
{
        assert(nmt);
 
        if (__unlikely(!id || id > CO_NUM_NODES))
                return;
 
        if (state == CO_NMT_EC_OCCURRED && reason == CO_NMT_EC_TIMEOUT) {
#ifndef LELY_NO_CO_MASTER
                if (co_nmt_is_master(nmt)) {
                        co_nmt_node_err_ind(nmt, id);
                        return;
                }
#endif
                co_nmt_on_err(nmt, 0x8130, 0x10, NULL);
        }
}
 
void
co_nmt_get_st_ind(const co_nmt_t *nmt, co_nmt_st_ind_t **pind, void **pdata)
{
        assert(nmt);
 
        if (pind)
                *pind = nmt->st_ind;
        if (pdata)
                *pdata = nmt->st_data;
}
 
void
co_nmt_set_st_ind(co_nmt_t *nmt, co_nmt_st_ind_t *ind, void *data)
{
        assert(nmt);
 
        nmt->st_ind = ind ? ind : &default_st_ind;
        nmt->st_data = ind ? data : NULL;
}
 
void
co_nmt_on_st(co_nmt_t *nmt, co_unsigned8_t id, co_unsigned8_t st)
{
        assert(nmt);
 
        if (__unlikely(!id || id > CO_NUM_NODES))
                return;
 
#ifdef LELY_NO_CO_MASTER
        (void)nmt;
        (void)st;
#else
        if (co_nmt_is_master(nmt) && st == CO_NMT_ST_BOOTUP) {
                int errc = get_errc();
                co_nmt_boot_req(nmt, id, nmt->timeout);
                set_errc(errc);
        }
#endif
}
 
#ifndef LELY_NO_CO_MASTER
 
#ifndef LELY_NO_CO_LSS
 
void
co_nmt_get_lss_req(const co_nmt_t *nmt, co_nmt_lss_req_t **pind, void **pdata)
{
        assert(nmt);
 
        if (pind)
                *pind = nmt->lss_req;
        if (pdata)
                *pdata = nmt->lss_data;
}
 
void
co_nmt_set_lss_req(co_nmt_t *nmt, co_nmt_lss_req_t *ind, void *data)
{
        assert(nmt);
 
        nmt->lss_req = ind;
        nmt->lss_data = data;
}
 
#endif
 
void
co_nmt_get_boot_ind(const co_nmt_t *nmt, co_nmt_boot_ind_t **pind, void **pdata)
{
        assert(nmt);
 
        if (pind)
                *pind = nmt->boot_ind;
        if (pdata)
                *pdata = nmt->boot_data;
}
 
void
co_nmt_set_boot_ind(co_nmt_t *nmt, co_nmt_boot_ind_t *ind, void *data)
{
        assert(nmt);
 
        nmt->boot_ind = ind;
        nmt->boot_data = data;
}
 
void
co_nmt_get_cfg_ind(const co_nmt_t *nmt, co_nmt_cfg_ind_t **pind, void **pdata)
{
        assert(nmt);
 
        if (pind)
                *pind = nmt->cfg_ind;
        if (pdata)
                *pdata = nmt->cfg_data;
}
 
void
co_nmt_set_cfg_ind(co_nmt_t *nmt, co_nmt_cfg_ind_t *ind, void *data)
{
        assert(nmt);
 
        nmt->cfg_ind = ind;
        nmt->cfg_data = data;
}
 
void
co_nmt_get_dn_ind(const co_nmt_t *nmt, co_nmt_sdo_ind_t **pind, void **pdata)
{
        assert(nmt);
 
        if (pind)
                *pind = nmt->dn_ind;
        if (pdata)
                *pdata = nmt->dn_data;
}
 
void
co_nmt_set_dn_ind(co_nmt_t *nmt, co_nmt_sdo_ind_t *ind, void *data)
{
        assert(nmt);
 
        nmt->dn_ind = ind;
        nmt->dn_data = data;
}
 
void
co_nmt_get_up_ind(const co_nmt_t *nmt, co_nmt_sdo_ind_t **pind, void **pdata)
{
        assert(nmt);
 
        if (pind)
                *pind = nmt->up_ind;
        if (pdata)
                *pdata = nmt->up_data;
}
 
void
co_nmt_set_up_ind(co_nmt_t *nmt, co_nmt_sdo_ind_t *ind, void *data)
{
        assert(nmt);
 
        nmt->up_ind = ind;
        nmt->up_data = data;
}
 
#endif // !LELY_NO_CO_MASTER
 
void
co_nmt_get_sync_ind(const co_nmt_t *nmt, co_nmt_sync_ind_t **pind, void **pdata)
{
        assert(nmt);
 
        if (pind)
                *pind = nmt->sync_ind;
        if (pdata)
                *pdata = nmt->sync_data;
}
 
void
co_nmt_set_sync_ind(co_nmt_t *nmt, co_nmt_sync_ind_t *ind, void *data)
{
        assert(nmt);
 
        nmt->sync_ind = ind;
        nmt->sync_data = data;
}
 
void
co_nmt_on_sync(co_nmt_t *nmt, co_unsigned8_t cnt)
{
        assert(nmt);
 
        // Handle TPDOs before RPDOs. This prevents a possible race condition if
        // the same object is mapped to both an RPDO and a TPDO. In accordance
        // with CiA 301 v4.2.0 we transmit the value from the previous
        // synchronous window before updating it with a received PDO.
#ifndef LELY_NO_CO_TPDO
        for (co_unsigned16_t i = 0; i < nmt->srv.ntpdo; i++) {
                if (nmt->srv.tpdos[i])
                        co_tpdo_sync(nmt->srv.tpdos[i], cnt);
        }
#endif
#ifndef LELY_NO_CO_RPDO
        for (co_unsigned16_t i = 0; i < nmt->srv.nrpdo; i++) {
                if (nmt->srv.rpdos[i])
                        co_rpdo_sync(nmt->srv.rpdos[i], cnt);
        }
#endif
 
        if (nmt->sync_ind)
                nmt->sync_ind(nmt, cnt, nmt->sync_data);
}
 
void
co_nmt_on_err(co_nmt_t *nmt, co_unsigned16_t eec, co_unsigned8_t er,
                const uint8_t msef[5])
{
        assert(nmt);
 
        if (eec) {
#ifdef LELY_NO_CO_EMCY
                (void)er;
                (void)msef;
#else
                if (nmt->srv.emcy)
                        co_emcy_push(nmt->srv.emcy, eec, er, msef);
#endif
                // In case of a communication error (0x81xx), invoke the
                // behavior specified by 1029:01.
                if ((eec & 0xff00) == 0x8100)
                        co_nmt_comm_err_ind(nmt);
        }
}
 
co_unsigned8_t
co_nmt_get_id(const co_nmt_t *nmt)
{
        assert(nmt);
 
        return nmt->id;
}
 
int
co_nmt_set_id(co_nmt_t *nmt, co_unsigned8_t id)
{
        assert(nmt);
 
        if (__unlikely(!id || (id > CO_NUM_NODES && id != 0xff))) {
                set_errnum(ERRNUM_INVAL);
                return -1;
        }
 
        nmt->id = id;
 
        return 0;
}
 
co_unsigned8_t
co_nmt_get_st(const co_nmt_t *nmt)
{
        assert(nmt);
 
        return nmt->st & ~CO_NMT_ST_TOGGLE;
}
 
int
co_nmt_is_master(const co_nmt_t *nmt)
{
#ifdef LELY_NO_CO_MASTER
        (void)nmt;
 
        return 0;
#else
        assert(nmt);
 
        return nmt->master;
#endif
}
 
#ifndef LELY_NO_CO_MASTER
 
int
co_nmt_get_timeout(const co_nmt_t *nmt)
{
        assert(nmt);
 
        return nmt->timeout;
}
 
void
co_nmt_set_timeout(co_nmt_t *nmt, int timeout)
{
        assert(nmt);
 
        nmt->timeout = timeout;
}
 
int
co_nmt_cs_req(co_nmt_t *nmt, co_unsigned8_t cs, co_unsigned8_t id)
{
        assert(nmt);
 
        if (__unlikely(!nmt->master)) {
                set_errnum(ERRNUM_PERM);
                return -1;
        }
 
        switch (cs) {
        case CO_NMT_CS_START:
        case CO_NMT_CS_STOP:
        case CO_NMT_CS_ENTER_PREOP:
        case CO_NMT_CS_RESET_NODE:
        case CO_NMT_CS_RESET_COMM: break;
        default: set_errnum(ERRNUM_INVAL); return -1;
        }
 
        if (__unlikely(id > CO_NUM_NODES)) {
                set_errnum(ERRNUM_INVAL);
                return -1;
        }
 
        if (id == co_dev_get_id(nmt->dev))
                return co_nmt_cs_ind(nmt, cs);
 
        trace("NMT: sending command specifier %d to node %d", cs, id);
 
        struct can_msg msg = CAN_MSG_INIT;
        msg.id = CO_NMT_CS_CANID;
        msg.len = 2;
        msg.data[0] = cs;
        msg.data[1] = id;
 
        // Add the frame to the buffer.
        if (__unlikely(!can_buf_write(&nmt->buf, &msg, 1))) {
                if (__unlikely(!can_buf_reserve(&nmt->buf, 1)))
                        return -1;
                can_buf_write(&nmt->buf, &msg, 1);
        }
 
        // Send the frame by triggering the inhibit timer.
        return co_nmt_cs_timer(NULL, nmt);
}
 
#ifndef LELY_NO_CO_LSS
int
co_nmt_lss_con(co_nmt_t *nmt)
{
        assert(nmt);
 
        if (__unlikely(!nmt->master || nmt->state != co_nmt_reset_comm_state)) {
                set_errnum(ERRNUM_PERM);
                return -1;
        }
 
        co_nmt_enter(nmt, co_nmt_bootup_state);
 
        return 0;
}
#endif
 
int
co_nmt_boot_req(co_nmt_t *nmt, co_unsigned8_t id, int timeout)
{
        assert(nmt);
 
        int errc = 0;
 
        if (__unlikely(!nmt->master)) {
                errc = errnum2c(ERRNUM_PERM);
                goto error_param;
        }
 
        // clang-format off
        if (__unlikely(!id || id > CO_NUM_NODES
                        || id == co_dev_get_id(nmt->dev))) {
                // clang-format on
                errc = errnum2c(ERRNUM_INVAL);
                goto error_param;
        }
        struct co_nmt_slave *slave = &nmt->slaves[id - 1];
 
        if (__unlikely(slave->boot)) {
                errc = errnum2c(ERRNUM_INPROGRESS);
                goto error_param;
        }
 
        trace("NMT: booting slave %d", id);
 
        slave->boot = co_nmt_boot_create(nmt->net, nmt->dev, nmt);
        if (__unlikely(!slave->boot)) {
                errc = get_errc();
                goto error_create_boot;
        }
 
        // clang-format off
        if (__unlikely(co_nmt_boot_boot_req(slave->boot, id, timeout,
                        &co_nmt_dn_ind, &co_nmt_up_ind, nmt) == -1)) {
                // clang-format on
                errc = get_errc();
                goto error_boot_req;
        }
 
        return 0;
 
error_boot_req:
        co_nmt_boot_destroy(slave->boot);
        slave->boot = NULL;
error_create_boot:
error_param:
        set_errc(errc);
        return -1;
}
 
int
co_nmt_is_booting(const co_nmt_t *nmt, co_unsigned8_t id)
{
        assert(nmt);
 
        if (__unlikely(!nmt->master))
                return 0;
 
        // clang-format off
        if (__unlikely(!id || id > CO_NUM_NODES
                        || id == co_dev_get_id(nmt->dev)))
                // clang-format on
                return 0;
 
        return !!nmt->slaves[id - 1].boot;
}
 
int
co_nmt_cfg_req(co_nmt_t *nmt, co_unsigned8_t id, int timeout,
                co_nmt_cfg_con_t *con, void *data)
{
        assert(nmt);
 
        int errc = 0;
 
        if (__unlikely(!nmt->master)) {
                errc = errnum2c(ERRNUM_PERM);
                goto error_param;
        }
 
        // clang-format off
        if (__unlikely(!id || id > CO_NUM_NODES
                        || id == co_dev_get_id(nmt->dev))) {
                // clang-format on
                errc = errnum2c(ERRNUM_INVAL);
                goto error_param;
        }
        struct co_nmt_slave *slave = &nmt->slaves[id - 1];
 
        if (__unlikely(slave->cfg)) {
                errc = errnum2c(ERRNUM_INPROGRESS);
                goto error_param;
        }
 
        trace("NMT: starting update configuration process for node %d", id);
 
        slave->cfg = co_nmt_cfg_create(nmt->net, nmt->dev, nmt);
        if (__unlikely(!slave->cfg)) {
                errc = get_errc();
                goto error_create_cfg;
        }
        slave->cfg_con = con;
        slave->cfg_data = data;
 
        // clang-format off
        if (__unlikely(co_nmt_cfg_cfg_req(slave->cfg, id, timeout,
                        &co_nmt_dn_ind, &co_nmt_up_ind, nmt) == -1)) {
                // clang-format on
                errc = get_errc();
                goto error_cfg_req;
        }
 
        return 0;
 
error_cfg_req:
        co_nmt_cfg_destroy(slave->cfg);
        slave->cfg = NULL;
error_create_cfg:
error_param:
        set_errc(errc);
        return -1;
}
 
int
co_nmt_cfg_res(co_nmt_t *nmt, co_unsigned8_t id, co_unsigned32_t ac)
{
        assert(nmt);
 
        if (__unlikely(!nmt->master)) {
                set_errnum(ERRNUM_PERM);
                return -1;
        }
 
        if (__unlikely(!id || id > CO_NUM_NODES || !nmt->slaves[id - 1].cfg)) {
                set_errnum(ERRNUM_INVAL);
                return -1;
        }
 
        return co_nmt_cfg_cfg_res(nmt->slaves[id - 1].cfg, ac);
}
 
int
co_nmt_ng_req(co_nmt_t *nmt, co_unsigned8_t id, co_unsigned16_t gt,
                co_unsigned8_t ltf)
{
        assert(nmt);
 
        if (__unlikely(!nmt->master)) {
                set_errnum(ERRNUM_PERM);
                return -1;
        }
 
        // clang-format off
        if (__unlikely(!id || id > CO_NUM_NODES
                        || id == co_dev_get_id(nmt->dev))) {
                // clang-format on
                set_errnum(ERRNUM_INVAL);
                return -1;
        }
        struct co_nmt_slave *slave = &nmt->slaves[id - 1];
 
        if (!gt || !ltf) {
                can_timer_destroy(slave->timer);
                slave->timer = 0;
 
                slave->gt = 0;
                slave->ltf = 0;
                slave->rtr = 0;
        } else {
                if (!slave->timer) {
                        slave->timer = can_timer_create();
                        if (__unlikely(!slave->timer))
                                return -1;
                        can_timer_set_func(
                                        slave->timer, &co_nmt_ng_timer, slave);
                }
 
                slave->gt = gt;
                slave->ltf = ltf;
                slave->rtr = 0;
 
                can_timer_timeout(slave->timer, nmt->net, slave->gt);
        }
 
        return 0;
}
 
#endif // !LELY_NO_CO_MASTER
 
int
co_nmt_cs_ind(co_nmt_t *nmt, co_unsigned8_t cs)
{
        assert(nmt);
 
        switch (cs) {
        case CO_NMT_CS_START:
        case CO_NMT_CS_STOP:
        case CO_NMT_CS_ENTER_PREOP:
        case CO_NMT_CS_RESET_NODE:
        case CO_NMT_CS_RESET_COMM:
                trace("NMT: received command specifier %d", cs);
                co_nmt_emit_cs(nmt, cs);
                return 0;
        default: set_errnum(ERRNUM_INVAL); return -1;
        }
}
 
void
co_nmt_comm_err_ind(co_nmt_t *nmt)
{
        assert(nmt);
 
        diag(DIAG_INFO, 0, "NMT: communication error indicated");
        switch (co_dev_get_val_u8(nmt->dev, 0x1029, 0x01)) {
        case 0:
                if (co_nmt_get_st(nmt) == CO_NMT_ST_START)
                        co_nmt_cs_ind(nmt, CO_NMT_CS_ENTER_PREOP);
                break;
        case 2: co_nmt_cs_ind(nmt, CO_NMT_CS_STOP); break;
        }
}
 
#ifndef LELY_NO_CO_MASTER
int
co_nmt_node_err_ind(co_nmt_t *nmt, co_unsigned8_t id)
{
        assert(nmt);
 
        if (!nmt->master) {
                set_errnum(ERRNUM_PERM);
                return -1;
        }
 
        if (__unlikely(!id || id > CO_NUM_NODES)) {
                set_errnum(ERRNUM_INVAL);
                return -1;
        }
 
        co_unsigned32_t assignment = co_dev_get_val_u32(nmt->dev, 0x1f81, id);
        // Ignore the error event if the slave is no longer in the network list.
        if (!(assignment & 0x01))
                return 0;
        int mandatory = !!(assignment & 0x08);
 
        diag(DIAG_INFO, 0, "NMT: error indicated for %s slave %d",
                        mandatory ? "mandatory" : "optional", id);
 
        if (mandatory && (nmt->startup & 0x40)) {
                // If the slave is mandatory and bit 6 of the NMT startup value
                // is set, stop all nodes, including the master.
                co_nmt_cs_req(nmt, CO_NMT_CS_STOP, 0);
                return co_nmt_cs_ind(nmt, CO_NMT_CS_STOP);
        } else if (mandatory && (nmt->startup & 0x10)) {
                // If the slave is mandatory and bit 4 of the NMT startup value
                // is set, reset all nodes, including the master.
                co_nmt_cs_req(nmt, CO_NMT_CS_RESET_NODE, 0);
                return co_nmt_cs_ind(nmt, CO_NMT_CS_RESET_NODE);
        } else {
                // If the slave is not mandatory, or bits 4 and 6 of the NMT
                // startup value are zero, reset the node individually.
                co_nmt_cs_req(nmt, CO_NMT_CS_RESET_NODE, id);
                return 0;
        }
}
#endif
 
co_rpdo_t *
co_nmt_get_rpdo(const co_nmt_t *nmt, co_unsigned16_t n)
{
        assert(nmt);
 
        if (__unlikely(!n || n > nmt->srv.nrpdo))
                return NULL;
 
        return nmt->srv.rpdos[n - 1];
}
 
co_tpdo_t *
co_nmt_get_tpdo(const co_nmt_t *nmt, co_unsigned16_t n)
{
        assert(nmt);
 
        if (__unlikely(!n || n > nmt->srv.ntpdo))
                return NULL;
 
        return nmt->srv.tpdos[n - 1];
}
 
co_ssdo_t *
co_nmt_get_ssdo(const co_nmt_t *nmt, co_unsigned8_t n)
{
        assert(nmt);
 
        if (__unlikely(!n || n > nmt->srv.nssdo))
                return NULL;
 
        return nmt->srv.ssdos[n - 1];
}
 
co_csdo_t *
co_nmt_get_csdo(const co_nmt_t *nmt, co_unsigned8_t n)
{
        assert(nmt);
 
        if (__unlikely(!n || n > nmt->srv.ncsdo))
                return NULL;
 
        return nmt->srv.csdos[n - 1];
}
 
co_sync_t *
co_nmt_get_sync(const co_nmt_t *nmt)
{
        assert(nmt);
 
        return nmt->srv.sync;
}
 
co_time_t *
co_nmt_get_time(const co_nmt_t *nmt)
{
        assert(nmt);
 
        return nmt->srv.time;
}
 
co_emcy_t *
co_nmt_get_emcy(const co_nmt_t *nmt)
{
        assert(nmt);
 
        return nmt->srv.emcy;
}
 
co_lss_t *
co_nmt_get_lss(const co_nmt_t *nmt)
{
        assert(nmt);
 
        return nmt->srv.lss;
}
 
#ifndef LELY_NO_CO_MASTER
 
void
co_nmt_boot_con(co_nmt_t *nmt, co_unsigned8_t id, co_unsigned8_t st, char es)
{
        assert(nmt);
        assert(nmt->master);
        assert(id && id <= CO_NUM_NODES);
 
        // Update the NMT slave state, including the assignment, in case it
        // changed during the 'boot slave' procedure.
        struct co_nmt_slave *slave = &nmt->slaves[id - 1];
        slave->assignment = co_dev_get_val_u32(nmt->dev, 0x1f81, id);
        slave->est = st & ~CO_NMT_ST_TOGGLE;
        // If we did not (yet) receive a state but the error control service was
        // successfully started, assume the node is pre-operational.
        if (!slave->est && (!es || es == 'L'))
                slave->est = CO_NMT_ST_PREOP;
        slave->rst = st;
        slave->es = es;
        slave->booted = 1;
        co_nmt_boot_destroy(slave->boot);
        slave->boot = NULL;
 
        // Update object 1F82 (Request NMT) with the NMT state.
        co_sub_t *sub = co_dev_find_sub(nmt->dev, 0x1f82, id);
        if (sub)
                co_sub_set_val_u8(sub, st & ~CO_NMT_ST_TOGGLE);
 
        // If the slave booted successfully and can be started by the NMT
        // service, and if the master is allowed to start the nodes (bit 3 of
        // the NMT startup value) and has to start the slaves individually (bit
        // 1) or is in the operational state, send the NMT 'start' command to
        // the slave.
        // clang-format off
        if (!es && (slave->assignment & 0x05) == 0x05 && !(nmt->startup & 0x08)
                        && (!(nmt->startup & 0x02)
                        || co_nmt_get_st(nmt) == CO_NMT_ST_START))
                // clang-format on
                co_nmt_cs_req(nmt, CO_NMT_CS_START, id);
 
        // If the error control service was successfully started, enable
        // heartbeat consumption or node guarding.
        if (!es || es == 'L') {
                co_obj_t *obj_1016 = co_dev_find_obj(nmt->dev, 0x1016);
                for (co_unsigned8_t i = 0; i < nmt->nhb; i++) {
                        co_unsigned32_t val =
                                        co_obj_get_val_u32(obj_1016, i + 1);
                        if (id != ((val >> 16) & 0xff))
                                continue;
                        co_nmt_hb_set_st(nmt->hbs[i], st);
                        // Disable node guarding.
                        slave->assignment &= 0xff;
                }
                // Enable node guarding if the guard time and lifetime factor
                // are non-zero.
                co_unsigned16_t gt = (slave->assignment >> 16) & 0xffff;
                co_unsigned8_t ltf = (slave->assignment >> 8) & 0xff;
                if (__unlikely(co_nmt_ng_req(nmt, id, gt, ltf) == -1))
                        diag(DIAG_ERROR, get_errc(),
                                        "unable to guard node %02X", id);
        }
 
        trace("NMT: slave %d finished booting with error status %c", id,
                        es ? es : '0');
        if (nmt->boot_ind)
                nmt->boot_ind(nmt, id, st, es, nmt->boot_data);
 
        co_nmt_emit_boot(nmt, id, st, es);
}
 
void
co_nmt_cfg_ind(co_nmt_t *nmt, co_unsigned8_t id, co_csdo_t *sdo)
{
        assert(nmt);
        assert(nmt->master);
        assert(id && id <= CO_NUM_NODES);
 
        if (nmt->cfg_ind) {
                nmt->cfg_ind(nmt, id, sdo, nmt->cfg_data);
        } else {
                diag(DIAG_WARNING, errnum2c(ERRNUM_NOSYS),
                                "skipping NMT update configuration process");
                co_nmt_cfg_res(nmt, id, 0);
        }
}
 
void
co_nmt_cfg_con(co_nmt_t *nmt, co_unsigned8_t id, co_unsigned32_t ac)
{
        assert(nmt);
        assert(nmt->master);
        assert(id && id <= CO_NUM_NODES);
 
        struct co_nmt_slave *slave = &nmt->slaves[id - 1];
        co_nmt_cfg_destroy(slave->cfg);
        slave->cfg = NULL;
 
        trace("NMT: update configuration process completed for slave %d", id);
        if (slave->cfg_con)
                slave->cfg_con(nmt, id, ac, slave->cfg_data);
}
 
#endif // !LELY_NO_CO_MASTER
 
void
co_nmt_hb_ind(co_nmt_t *nmt, co_unsigned8_t id, int state, int reason,
                co_unsigned8_t st)
{
        assert(nmt);
        assert(nmt->hb_ind);
 
        if (__unlikely(!id || id > CO_NUM_NODES))
                return;
 
        nmt->hb_ind(nmt, id, state, reason, nmt->hb_data);
 
        if (reason == CO_NMT_EC_STATE)
                co_nmt_st_ind(nmt, id, st);
}
 
static co_unsigned32_t
co_100c_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
{
        assert(sub);
        assert(co_obj_get_idx(co_sub_get_obj(sub)) == 0x100c);
        assert(req);
        co_nmt_t *nmt = data;
        assert(nmt);
 
        co_unsigned32_t ac = 0;
 
        co_unsigned16_t type = co_sub_get_type(sub);
        union co_val val;
        if (__unlikely(co_sdo_req_dn_val(req, type, &val, &ac) == -1))
                return ac;
 
        if (__unlikely(co_sub_get_subidx(sub))) {
                ac = CO_SDO_AC_NO_SUB;
                goto error;
        }
 
        assert(type == CO_DEFTYPE_UNSIGNED16);
        co_unsigned16_t gt = val.u16;
        co_unsigned16_t gt_old = co_sub_get_val_u16(sub);
        if (gt == gt_old)
                goto error;
 
        nmt->gt = gt;
 
        co_sub_dn(sub, &val);
        co_val_fini(type, &val);
 
        co_nmt_ec_update(nmt);
        return 0;
 
error:
        co_val_fini(type, &val);
        return ac;
}
 
static co_unsigned32_t
co_100d_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
{
        assert(sub);
        assert(co_obj_get_idx(co_sub_get_obj(sub)) == 0x100d);
        assert(req);
        co_nmt_t *nmt = data;
        assert(nmt);
 
        co_unsigned32_t ac = 0;
 
        co_unsigned16_t type = co_sub_get_type(sub);
        union co_val val;
        if (__unlikely(co_sdo_req_dn_val(req, type, &val, &ac) == -1))
                return ac;
 
        if (__unlikely(co_sub_get_subidx(sub))) {
                ac = CO_SDO_AC_NO_SUB;
                goto error;
        }
 
        assert(type == CO_DEFTYPE_UNSIGNED8);
        co_unsigned8_t ltf = val.u8;
        co_unsigned8_t ltf_old = co_sub_get_val_u8(sub);
        if (ltf == ltf_old)
                return 0;
 
        nmt->ltf = ltf;
 
        co_sub_dn(sub, &val);
        co_val_fini(type, &val);
 
        co_nmt_ec_update(nmt);
        return 0;
 
error:
        co_val_fini(type, &val);
        return ac;
}
 
static co_unsigned32_t
co_1016_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
{
        assert(sub);
        assert(co_obj_get_idx(co_sub_get_obj(sub)) == 0x1016);
        assert(req);
        co_nmt_t *nmt = data;
        assert(nmt);
 
        co_unsigned32_t ac = 0;
 
        co_unsigned16_t type = co_sub_get_type(sub);
        union co_val val;
        if (__unlikely(co_sdo_req_dn_val(req, type, &val, &ac) == -1))
                return ac;
 
        co_unsigned8_t subidx = co_sub_get_subidx(sub);
        if (__unlikely(!subidx)) {
                ac = CO_SDO_AC_NO_WRITE;
                goto error;
        }
        if (__unlikely(subidx > nmt->nhb)) {
                ac = CO_SDO_AC_NO_SUB;
                goto error;
        }
 
        assert(type == CO_DEFTYPE_UNSIGNED32);
        if (val.u32 == co_sub_get_val_u32(sub))
                goto error;
 
        co_unsigned8_t id = (val.u32 >> 16) & 0xff;
        co_unsigned16_t ms = val.u32 & 0xffff;
 
        // If the heartbeat consumer is active (valid node-ID and non-zero
        // heartbeat time), check the other entries for duplicate node-IDs.
        co_obj_t *obj_1016 = co_dev_find_obj(nmt->dev, 0x1016);
        if (id && id <= CO_NUM_NODES && ms) {
                for (co_unsigned8_t i = 1; i <= CO_NUM_NODES; i++) {
                        // Skip the current entry.
                        if (i == subidx)
                                continue;
                        co_unsigned32_t val_i = co_obj_get_val_u32(obj_1016, i);
                        co_unsigned8_t id_i = (val_i >> 16) & 0xff;
                        co_unsigned16_t ms_i = val_i & 0xffff;
                        // It's not allowed to have two active heartbeat
                        // consumers with the same node-ID.
                        if (__unlikely(id_i == id && ms_i)) {
                                ac = CO_SDO_AC_PARAM;
                                goto error;
                        }
                }
        }
 
        co_sub_dn(sub, &val);
        co_val_fini(type, &val);
 
        co_nmt_hb_set_1016(nmt->hbs[subidx - 1], id, ms);
        return 0;
 
error:
        co_val_fini(type, &val);
        return ac;
}
 
static co_unsigned32_t
co_1017_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
{
        assert(sub);
        assert(co_obj_get_idx(co_sub_get_obj(sub)) == 0x1017);
        assert(req);
        co_nmt_t *nmt = data;
        assert(nmt);
 
        co_unsigned32_t ac = 0;
 
        co_unsigned16_t type = co_sub_get_type(sub);
        union co_val val;
        if (__unlikely(co_sdo_req_dn_val(req, type, &val, &ac) == -1))
                return ac;
 
        if (__unlikely(co_sub_get_subidx(sub))) {
                ac = CO_SDO_AC_NO_SUB;
                goto error;
        }
 
        assert(type == CO_DEFTYPE_UNSIGNED16);
        co_unsigned16_t ms = val.u16;
        co_unsigned16_t ms_old = co_sub_get_val_u16(sub);
        if (ms == ms_old)
                goto error;
 
        nmt->ms = ms;
 
        co_sub_dn(sub, &val);
        co_val_fini(type, &val);
 
        co_nmt_ec_update(nmt);
        return 0;
 
error:
        co_val_fini(type, &val);
        return ac;
}
 
#ifndef LELY_NO_CO_MASTER
static co_unsigned32_t
co_1f25_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
{
        assert(sub);
        assert(co_obj_get_idx(co_sub_get_obj(sub)) == 0x1f25);
        assert(req);
        co_nmt_t *nmt = data;
        assert(nmt);
 
        co_unsigned32_t ac = 0;
 
        co_unsigned16_t type = co_sub_get_type(sub);
        union co_val val;
        if (__unlikely(co_sdo_req_dn_val(req, type, &val, &ac) == -1))
                return ac;
 
        co_unsigned8_t subidx = co_sub_get_subidx(sub);
        if (__unlikely(!subidx)) {
                ac = CO_SDO_AC_NO_WRITE;
                goto error;
        }
 
        // Sub-index 80 indicates all nodes.
        co_unsigned8_t id = subidx == 0x80 ? 0 : subidx;
        // Abort with an error if the node-ID is unknown.
        // clang-format off
        if (__unlikely(id > CO_NUM_NODES
                        || (id && !(nmt->slaves[id - 1].assignment & 0x01)))) {
                // clang-format on
                ac = CO_SDO_AC_PARAM_VAL;
                goto error;
        }
 
        // Check if the value 'conf' was downloaded.
        if (__unlikely(!nmt->master || val.u32 != UINT32_C(0x666e6f63))) {
                ac = CO_SDO_AC_DATA_CTL;
                goto error;
        }
 
        if (id) {
                // Check if the entry for this node is present in object 1F20
                // (Store DCF) or 1F22 (Concise DCF).
                // clang-format off
                if (__unlikely(!co_dev_get_val(nmt->dev, 0x1f20, id)
                                && !co_dev_get_val(nmt->dev, 0x1f22, id))) {
                        // clang-format on
                        ac = CO_SDO_AC_NO_DATA;
                        goto error;
                }
                // Abort if the slave is already being configured.
                if (__unlikely(nmt->slaves[id - 1].cfg)) {
                        ac = CO_SDO_AC_DATA_DEV;
                        goto error;
                }
                co_nmt_cfg_req(nmt, id, nmt->timeout, NULL, NULL);
        } else {
                // Check if object 1F20 (Store DCF) or 1F22 (Concise DCF)
                // exists.
                // clang-format off
                if (__unlikely(!co_dev_find_obj(nmt->dev, 0x1f20)
                                && !co_dev_find_obj(nmt->dev, 0x1f22))) {
                        // clang-format on
                        ac = CO_SDO_AC_NO_DATA;
                        goto error;
                }
                for (id = 1; id <= CO_NUM_NODES; id++) {
                        struct co_nmt_slave *slave = &nmt->slaves[id - 1];
                        // Skip slaves that are not in the network list or are
                        // already being configured.
                        if (!(slave->assignment & 0x01) || slave->cfg)
                                continue;
                        co_nmt_cfg_req(nmt, id, nmt->timeout, NULL, NULL);
                }
        }
 
error:
        co_val_fini(type, &val);
        return ac;
}
#endif // !LELY_NO_CO_MASTER
 
static co_unsigned32_t
co_1f80_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
{
        assert(sub);
        assert(co_obj_get_idx(co_sub_get_obj(sub)) == 0x1f80);
        assert(req);
        (void)data;
 
        co_unsigned32_t ac = 0;
 
        co_unsigned16_t type = co_sub_get_type(sub);
        union co_val val;
        if (__unlikely(co_sdo_req_dn_val(req, type, &val, &ac) == -1))
                return ac;
 
        if (__unlikely(co_sub_get_subidx(sub))) {
                ac = CO_SDO_AC_NO_SUB;
                goto error;
        }
 
        assert(type == CO_DEFTYPE_UNSIGNED32);
        co_unsigned32_t startup = val.u32;
        co_unsigned32_t startup_old = co_sub_get_val_u32(sub);
        if (startup == startup_old)
                goto error;
 
        // Only bits 0..4 and 6 are supported.
        if (__unlikely((startup ^ startup_old) != 0x5f)) {
                ac = CO_SDO_AC_PARAM_VAL;
                goto error;
        }
 
        co_sub_dn(sub, &val);
error:
        co_val_fini(type, &val);
        return ac;
}
 
#ifndef LELY_NO_CO_MASTER
static co_unsigned32_t
co_1f82_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
{
        assert(sub);
        assert(co_obj_get_idx(co_sub_get_obj(sub)) == 0x1f82);
        assert(req);
        co_nmt_t *nmt = data;
        assert(nmt);
 
        co_unsigned32_t ac = 0;
 
        co_unsigned16_t type = co_sub_get_type(sub);
        union co_val val;
        if (__unlikely(co_sdo_req_dn_val(req, type, &val, &ac) == -1))
                return ac;
 
        co_unsigned8_t subidx = co_sub_get_subidx(sub);
        if (__unlikely(!subidx)) {
                ac = CO_SDO_AC_NO_WRITE;
                goto error;
        }
 
        // Sub-index 80 indicates all nodes.
        co_unsigned8_t id = subidx == 0x80 ? 0 : subidx;
        // Abort with an error if the node-ID is unknown.
        // clang-format off
        if (__unlikely(id > CO_NUM_NODES
                        || (id && !(nmt->slaves[id - 1].assignment & 0x01)))) {
                // clang-format on
                ac = CO_SDO_AC_PARAM_VAL;
                goto error;
        }
 
        if (__unlikely(!nmt->master)) {
                ac = CO_SDO_AC_DATA_CTL;
                goto error;
        }
 
        assert(type == CO_DEFTYPE_UNSIGNED8);
        switch (val.u8) {
        case CO_NMT_ST_STOP: co_nmt_cs_req(nmt, CO_NMT_CS_STOP, id); break;
        case CO_NMT_ST_START: co_nmt_cs_req(nmt, CO_NMT_CS_START, id); break;
        case CO_NMT_ST_RESET_NODE:
                co_nmt_cs_req(nmt, CO_NMT_CS_RESET_NODE, id);
                break;
        case CO_NMT_ST_RESET_COMM:
                co_nmt_cs_req(nmt, CO_NMT_CS_RESET_COMM, id);
                break;
        case CO_NMT_ST_PREOP:
                co_nmt_cs_req(nmt, CO_NMT_CS_ENTER_PREOP, id);
                break;
        default: ac = CO_SDO_AC_PARAM_VAL; break;
        }
 
error:
        co_val_fini(type, &val);
        return ac;
}
#endif // !LELY_NO_CO_MASTER
 
static int
co_nmt_recv_000(const struct can_msg *msg, void *data)
{
        assert(msg);
        co_nmt_t *nmt = data;
        assert(nmt);
 
#ifndef LELY_NO_CO_MASTER
        // Ignore NMT commands if we're the master.
        if (nmt->master)
                return 0;
#endif
 
        if (__unlikely(msg->len < 2))
                return 0;
        co_unsigned8_t cs = msg->data[0];
        co_unsigned8_t id = msg->data[1];
 
        // Ignore NMT commands to other nodes.
        if (id && id != co_dev_get_id(nmt->dev))
                return 0;
 
        co_nmt_emit_cs(nmt, cs);
 
        return 0;
}
 
static int
co_nmt_recv_700(const struct can_msg *msg, void *data)
{
        assert(msg);
        assert(msg->id > 0x700 && msg->id <= 0x77f);
        co_nmt_t *nmt = data;
        assert(nmt);
 
        if (msg->flags & CAN_FLAG_RTR) {
                assert(nmt->gt && nmt->ltf);
                assert(nmt->lg_ind);
 
                // Respond with the state and flip the toggle bit.
                co_nmt_ec_send_res(nmt, nmt->st);
                nmt->st ^= CO_NMT_ST_TOGGLE;
 
                // Reset the life guarding timer.
                can_timer_timeout(nmt->ec_timer, nmt->net, nmt->gt * nmt->ltf);
 
                if (nmt->lg_state == CO_NMT_EC_OCCURRED) {
                        diag(DIAG_INFO, 0, "NMT: life guarding event resolved");
                        // Notify the user of the resolution of a life guarding
                        // error.
                        nmt->lg_state = CO_NMT_EC_RESOLVED;
                        nmt->lg_ind(nmt, nmt->lg_state, nmt->lg_data);
                }
#ifndef LELY_NO_CO_MASTER
        } else {
                assert(nmt->master);
                assert(nmt->ng_ind);
 
                co_unsigned8_t id = (msg->id - 0x700) & 0x7f;
                if (__unlikely(!id))
                        return 0;
                struct co_nmt_slave *slave = &nmt->slaves[id - 1];
 
                // Ignore messages from booting slaves.
                if (slave->boot)
                        return 0;
 
                if (__unlikely(msg->len < 1))
                        return 0;
                co_unsigned8_t st = msg->data[0];
 
                if (st == CO_NMT_ST_BOOTUP) {
                        // The expected state after a boot-up event is
                        // pre-operational.
                        slave->est = CO_NMT_ST_PREOP;
                        // Inform the application of the boot-up event.
                        co_nmt_st_ind(nmt, id, st);
                        return 0;
                }
 
                // Ignore messages if node guarding is disabled.
                if (!slave->gt || !slave->ltf)
                        return 0;
 
                // Check the toggle bit and ignore the message if it does not
                // match.
                if (__unlikely(!((st ^ slave->rst) & CO_NMT_ST_TOGGLE)))
                        return 0;
                slave->rst ^= CO_NMT_ST_TOGGLE;
 
                // Notify the application of the resolution of a node guarding
                // timeout.
                if (slave->rtr >= slave->ltf) {
                        diag(DIAG_INFO, 0,
                                        "NMT: node guarding time out resolved for node %d",
                                        id);
                        nmt->ng_ind(nmt, id, CO_NMT_EC_RESOLVED,
                                        CO_NMT_EC_TIMEOUT, nmt->ng_data);
                }
                slave->rtr = 0;
 
                // Notify the application of the occurrence or resolution of an
                // unexpected state change.
                if (slave->est != (st & ~CO_NMT_ST_TOGGLE)
                                && slave->ng_state == CO_NMT_EC_RESOLVED) {
                        diag(DIAG_INFO, 0,
                                        "NMT: node guarding state change occurred for node %d",
                                        id);
                        slave->ng_state = CO_NMT_EC_OCCURRED;
                        nmt->ng_ind(nmt, id, slave->ng_state, CO_NMT_EC_STATE,
                                        nmt->ng_data);
                } else if (slave->est == (st & ~CO_NMT_ST_TOGGLE)
                                && slave->ng_state == CO_NMT_EC_OCCURRED) {
                        diag(DIAG_INFO, 0,
                                        "NMT: node guarding state change resolved for node %d",
                                        id);
                        slave->ng_state = CO_NMT_EC_RESOLVED;
                        nmt->ng_ind(nmt, id, slave->ng_state, CO_NMT_EC_STATE,
                                        nmt->ng_data);
                }
 
                // Notify the application of the occurrence of a state change.
                if (st != slave->rst)
                        co_nmt_st_ind(nmt, id, st);
#endif
        }
 
        return 0;
}
 
#ifndef LELY_NO_CO_MASTER
static int
co_nmt_ng_timer(const struct timespec *tp, void *data)
{
        (void)tp;
        struct co_nmt_slave *slave = data;
        assert(slave);
        assert(slave->gt && slave->ltf);
        co_nmt_t *nmt = slave->nmt;
        assert(nmt);
        assert(nmt->master);
        assert(nmt->ng_ind);
        co_unsigned8_t id = slave - nmt->slaves + 1;
        assert(id && id <= CO_NUM_NODES);
 
        // Reset the timer for the next RTR.
        can_timer_timeout(slave->timer, nmt->net, slave->gt);
 
        // Do not send node guarding RTRs to slaves that have not finished
        // booting.
        if (!slave->booted)
                return 0;
 
        // Notify the application once of the occurrence of a node guarding
        // timeout.
        // clang-format off
        if (__unlikely(slave->rtr <= slave->ltf
                        && ++slave->rtr == slave->ltf)) {
                // clang-format on
                diag(DIAG_INFO, 0,
                                "NMT: node guarding time out occurred for node %d",
                                id);
                nmt->ng_ind(nmt, id, CO_NMT_EC_OCCURRED, CO_NMT_EC_TIMEOUT,
                                nmt->ng_data);
                return 0;
        }
 
        struct can_msg msg = CAN_MSG_INIT;
        msg.id = CO_NMT_EC_CANID(id);
        msg.flags |= CAN_FLAG_RTR;
 
        return can_net_send(nmt->net, &msg);
}
#endif
 
static int
co_nmt_ec_timer(const struct timespec *tp, void *data)
{
        (void)tp;
        co_nmt_t *nmt = data;
        assert(nmt);
        assert(nmt->lg_ind);
 
        if (nmt->ms) {
                // Send the state of the NMT service (excluding the toggle bit).
                co_nmt_ec_send_res(nmt, nmt->st & ~CO_NMT_ST_TOGGLE);
        } else if (nmt->gt && nmt->ltf) {
                // Notify the user of the occurrence of a life guarding error.
                diag(DIAG_INFO, 0, "NMT: life guarding event occurred");
                nmt->lg_state = CO_NMT_EC_OCCURRED;
                nmt->lg_ind(nmt, nmt->lg_state, nmt->lg_data);
        }
 
        return 0;
}
 
#ifndef LELY_NO_CO_MASTER
int
co_nmt_cs_timer(const struct timespec *tp, void *data)
{
        (void)tp;
        co_nmt_t *nmt = data;
        assert(nmt);
        assert(nmt->master);
 
        co_unsigned16_t inhibit = co_dev_get_val_u16(nmt->dev, 0x102a, 0x00);
 
        if (inhibit) {
                if (nmt->cs_timer) {
                        can_timer_stop(nmt->cs_timer);
                } else {
                        nmt->cs_timer = can_timer_create();
                        if (__unlikely(!nmt->cs_timer))
                                return -1;
                        can_timer_set_func(
                                        nmt->cs_timer, &co_nmt_cs_timer, nmt);
                }
        } else if (nmt->cs_timer) {
                can_timer_destroy(nmt->cs_timer);
                nmt->cs_timer = NULL;
        }
 
        struct timespec now = { 0, 0 };
        can_net_get_time(nmt->net, &now);
 
        struct can_msg msg;
        while (can_buf_peek(&nmt->buf, &msg, 1)) {
                assert(msg.id == CO_NMT_CS_CANID);
                assert(msg.len == 2);
                // Wait until the inhibit time has elapsed.
                if (inhibit && timespec_cmp(&now, &nmt->inhibit) < 0) {
                        can_timer_start(nmt->cs_timer, nmt->net, &nmt->inhibit,
                                        NULL);
                        return 0;
                }
                // Try to send the frame.
                if (__unlikely(can_net_send(nmt->net, &msg) == -1))
                        return -1;
                can_buf_read(&nmt->buf, NULL, 1);
                // Update the expected state of the node(s).
                co_unsigned8_t st = 0;
                switch (msg.data[0]) {
                case CO_NMT_CS_START: st = CO_NMT_ST_START; break;
                case CO_NMT_CS_STOP: st = CO_NMT_ST_STOP; break;
                case CO_NMT_CS_ENTER_PREOP: st = CO_NMT_ST_PREOP; break;
                }
                co_unsigned8_t id = msg.data[1];
                assert(id <= CO_NUM_NODES);
                if (id) {
                        if (nmt->slaves[id - 1].est)
                                nmt->slaves[id - 1].est = st;
                } else {
                        for (id = 1; id <= CO_NUM_NODES; id++) {
                                if (nmt->slaves[id - 1].est)
                                        nmt->slaves[id - 1].est = st;
                        }
                }
                // Update the inhibit time.
                can_net_get_time(nmt->net, &now);
                nmt->inhibit = now;
                timespec_add_usec(&nmt->inhibit, inhibit * 100);
        }
 
        return 0;
}
#endif // !LELY_NO_CO_MASTER
 
static void
co_nmt_st_ind(co_nmt_t *nmt, co_unsigned8_t id, co_unsigned8_t st)
{
        assert(nmt);
        assert(nmt->st_ind);
 
        if (__unlikely(!id || id > CO_NUM_NODES))
                return;
 
#ifndef LELY_NO_CO_MASTER
        if (nmt->master) {
                nmt->slaves[id - 1].rst = st;
 
                // Update object 1F82 (Request NMT) with the NMT state.
                co_sub_t *sub = co_dev_find_sub(nmt->dev, 0x1f82, id);
                if (sub)
                        co_sub_set_val_u8(sub, st & ~CO_NMT_ST_TOGGLE);
        }
#endif
 
        nmt->st_ind(nmt, id, st & ~CO_NMT_ST_TOGGLE, nmt->st_data);
}
 
#ifndef LELY_NO_CO_MASTER
static void
default_ng_ind(co_nmt_t *nmt, co_unsigned8_t id, int state, int reason,
                void *data)
{
        (void)data;
 
        co_nmt_on_ng(nmt, id, state, reason);
}
#endif
 
static void
default_lg_ind(co_nmt_t *nmt, int state, void *data)
{
        (void)data;
 
        co_nmt_on_lg(nmt, state);
}
 
static void
default_hb_ind(co_nmt_t *nmt, co_unsigned8_t id, int state, int reason,
                void *data)
{
        (void)data;
 
        co_nmt_on_hb(nmt, id, state, reason);
}
 
static void
default_st_ind(co_nmt_t *nmt, co_unsigned8_t id, co_unsigned8_t st, void *data)
{
        (void)data;
 
        co_nmt_on_st(nmt, id, st);
}
 
#ifndef LELY_NO_CO_MASTER
 
static void
co_nmt_dn_ind(const co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx,
                size_t size, size_t nbyte, void *data)
{
        co_nmt_t *nmt = data;
        assert(nmt);
 
        if (nmt->dn_ind)
                nmt->dn_ind(nmt, co_csdo_get_num(sdo), idx, subidx, size, nbyte,
                                nmt->dn_data);
}
 
static void
co_nmt_up_ind(const co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx,
                size_t size, size_t nbyte, void *data)
{
        co_nmt_t *nmt = data;
        assert(nmt);
 
        if (nmt->up_ind)
                nmt->up_ind(nmt, co_csdo_get_num(sdo), idx, subidx, size, nbyte,
                                nmt->up_data);
}
 
#endif
 
static void
co_nmt_enter(co_nmt_t *nmt, co_nmt_state_t *next)
{
        assert(nmt);
 
        while (next) {
                co_nmt_state_t *prev = nmt->state;
                nmt->state = next;
 
                if (prev && prev->on_leave)
                        prev->on_leave(nmt);
 
                next = next->on_enter ? next->on_enter(nmt) : NULL;
        }
}
 
static inline void
co_nmt_emit_cs(co_nmt_t *nmt, co_unsigned8_t cs)
{
        assert(nmt);
        assert(nmt->state);
        assert(nmt->state->on_cs);
 
        co_nmt_enter(nmt, nmt->state->on_cs(nmt, cs));
}
 
#ifndef LELY_NO_CO_MASTER
 
static inline void
co_nmt_emit_boot(co_nmt_t *nmt, co_unsigned8_t id, co_unsigned8_t st, char es)
{
        assert(nmt);
        assert(nmt->state);
        assert(nmt->state->on_boot);
 
        co_nmt_enter(nmt, nmt->state->on_boot(nmt, id, st, es));
}
 
static co_nmt_state_t *
co_nmt_default_on_boot(
                co_nmt_t *nmt, co_unsigned8_t id, co_unsigned8_t st, char es)
{
        (void)nmt;
        (void)id;
        (void)st;
        (void)es;
 
        return NULL;
}
 
#endif // !LELY_NO_CO_MASTER
 
static co_nmt_state_t *
co_nmt_init_on_cs(co_nmt_t *nmt, co_unsigned8_t cs)
{
        (void)nmt;
 
        switch (cs) {
        case CO_NMT_CS_RESET_NODE: return co_nmt_reset_node_state;
        default: return NULL;
        }
}
 
static co_nmt_state_t *
co_nmt_reset_node_on_enter(co_nmt_t *nmt)
{
        assert(nmt);
 
        diag(DIAG_INFO, 0, "NMT: entering reset application state");
 
#ifndef LELY_NO_CO_MASTER
        // Disable NMT slave management.
        co_nmt_slaves_fini(nmt);
        nmt->halt = 0;
#endif
 
        // Disable all services.
        co_nmt_srv_set(&nmt->srv, nmt, 0);
 
        // Disable heartbeat consumption.
        co_nmt_hb_fini(nmt);
 
        // Disable error control services.
        co_nmt_ec_fini(nmt);
 
        // Stop receiving NMT commands.
        can_recv_stop(nmt->recv_000);
 
        // Reset application parameters.
        // clang-format off
        if (__unlikely(co_dev_read_dcf(nmt->dev, NULL, NULL, &nmt->dcf_node)
                        == -1))
                // clang-format on
                diag(DIAG_ERROR, get_errc(),
                                "unable to reset application parameters");
 
        nmt->st = CO_NMT_ST_RESET_NODE;
        co_nmt_st_ind(nmt, co_dev_get_id(nmt->dev), 0);
 
        if (nmt->cs_ind)
                nmt->cs_ind(nmt, CO_NMT_CS_RESET_NODE, nmt->cs_data);
 
        return co_nmt_reset_comm_state;
}
 
static co_nmt_state_t *
co_nmt_reset_comm_on_enter(co_nmt_t *nmt)
{
        assert(nmt);
 
        diag(DIAG_INFO, 0, "NMT: entering reset communication state");
 
#ifndef LELY_NO_CO_MASTER
        // Disable NMT slave management.
        co_nmt_slaves_fini(nmt);
        nmt->halt = 0;
#endif
 
        // Disable all services.
        co_nmt_srv_set(&nmt->srv, nmt, 0);
 
        // Disable heartbeat consumption.
        co_nmt_hb_fini(nmt);
 
        // Disable error control services.
        co_nmt_ec_fini(nmt);
 
        // Stop receiving NMT commands.
        can_recv_stop(nmt->recv_000);
 
        // Reset communication parameters.
        // clang-format off
        if (__unlikely(co_dev_read_dcf(nmt->dev, NULL, NULL, &nmt->dcf_comm)
                        == -1))
                // clang-format on
                diag(DIAG_ERROR, get_errc(),
                                "unable to reset communication parameters");
 
        // Update the node-ID if necessary.
        if (nmt->id != co_dev_get_id(nmt->dev)) {
                co_dev_set_id(nmt->dev, nmt->id);
                co_val_fini(CO_DEFTYPE_DOMAIN, &nmt->dcf_comm);
                // clang-format off
                if (__unlikely(co_dev_write_dcf(nmt->dev, 0x1000, 0x1fff,
                                &nmt->dcf_comm) == -1))
                        // clang-format on
                        diag(DIAG_ERROR, get_errc(),
                                        "unable to store communication parameters");
        }
 
        // Load the NMT startup value.
        nmt->startup = co_dev_get_val_u32(nmt->dev, 0x1f80, 0x00);
#ifndef LELY_NO_CO_MASTER
        // Bit 0 of the NMT startup value determines whether we are a master or
        // a slave.
        nmt->master = !!(nmt->startup & 0x01);
#endif
        diag(DIAG_INFO, 0, "NMT: running as %s",
                        co_nmt_is_master(nmt) ? "master" : "slave");
 
        nmt->st = CO_NMT_ST_RESET_COMM;
        co_nmt_st_ind(nmt, co_dev_get_id(nmt->dev), 0);
 
        // Start receiving NMT commands.
        if (!co_nmt_is_master(nmt))
                can_recv_start(nmt->recv_000, nmt->net, CO_NMT_CS_CANID, 0);
 
        // Enable LSS.
        co_nmt_srv_set(&nmt->srv, nmt, CO_NMT_SRV_LSS);
 
        if (nmt->cs_ind)
                nmt->cs_ind(nmt, CO_NMT_CS_RESET_COMM, nmt->cs_data);
 
#if !defined(LELY_NO_CO_MASTER) && !defined(LELY_NO_CO_LSS)
        // If LSS is required, invoked the user-defined callback function and
        // wait for the process to complete.
        if (nmt->master && nmt->lss_req) {
                nmt->lss_req(nmt, co_nmt_get_lss(nmt), nmt->lss_data);
                return NULL;
        }
#endif
 
        return co_nmt_bootup_state;
}
 
static co_nmt_state_t *
co_nmt_reset_comm_on_cs(co_nmt_t *nmt, co_unsigned8_t cs)
{
        (void)nmt;
 
        switch (cs) {
        case CO_NMT_CS_RESET_NODE: return co_nmt_reset_node_state;
        case CO_NMT_CS_RESET_COMM: return co_nmt_reset_comm_state;
        default: return NULL;
        }
}
 
static co_nmt_state_t *
co_nmt_bootup_on_enter(co_nmt_t *nmt)
{
        assert(nmt);
 
        // Don't enter the 'pre-operational' state if the node-ID is invalid.
        if (co_dev_get_id(nmt->dev) == 0xff) {
                diag(DIAG_INFO, 0, "NMT: unconfigured node-ID");
                return NULL;
        }
 
        // Enable error control services.
        co_nmt_ec_init(nmt);
 
        // Enable heartbeat consumption.
        co_nmt_hb_init(nmt);
 
        // Send the boot-up signal to notify the master we exist.
        co_nmt_ec_send_res(nmt, CO_NMT_ST_BOOTUP);
 
        return co_nmt_preop_state;
}
 
static co_nmt_state_t *
co_nmt_bootup_on_cs(co_nmt_t *nmt, co_unsigned8_t cs)
{
        (void)nmt;
 
        switch (cs) {
        case CO_NMT_CS_RESET_NODE: return co_nmt_reset_node_state;
        case CO_NMT_CS_RESET_COMM: return co_nmt_reset_comm_state;
        default: return NULL;
        }
}
 
static co_nmt_state_t *
co_nmt_preop_on_enter(co_nmt_t *nmt)
{
        assert(nmt);
 
        diag(DIAG_INFO, 0, "NMT: entering pre-operational state");
 
#ifndef LELY_NO_CO_MASTER
        // Disable NMT slave management.
        co_nmt_slaves_fini(nmt);
        nmt->halt = 0;
#endif
 
        // Enable all services except PDO.
        co_nmt_srv_set(&nmt->srv, nmt, CO_NMT_PREOP_SRV);
 
        nmt->st = CO_NMT_ST_PREOP | (nmt->st & CO_NMT_ST_TOGGLE);
        co_nmt_st_ind(nmt, co_dev_get_id(nmt->dev), nmt->st);
 
        if (nmt->cs_ind)
                nmt->cs_ind(nmt, CO_NMT_CS_ENTER_PREOP, nmt->cs_data);
 
        return co_nmt_startup(nmt);
}
 
static co_nmt_state_t *
co_nmt_preop_on_cs(co_nmt_t *nmt, co_unsigned8_t cs)
{
        (void)nmt;
 
        switch (cs) {
        case CO_NMT_CS_START: return co_nmt_start_state;
        case CO_NMT_CS_STOP: return co_nmt_stop_state;
        case CO_NMT_CS_RESET_NODE: return co_nmt_reset_node_state;
        case CO_NMT_CS_RESET_COMM: return co_nmt_reset_comm_state;
        default: return NULL;
        }
}
 
#ifndef LELY_NO_CO_MASTER
static co_nmt_state_t *
co_nmt_preop_on_boot(
                co_nmt_t *nmt, co_unsigned8_t id, co_unsigned8_t st, char es)
{
        assert(nmt);
        assert(nmt->master);
        assert(id && id <= CO_NUM_NODES);
        (void)st;
 
        // If the 'boot slave' process failed for a mandatory slave, halt the
        // network boot-up procedure.
        if ((nmt->slaves[id - 1].assignment & 0x09) == 0x09 && es && es != 'L')
                nmt->halt = 1;
 
        // Wait for any mandatory slaves that have not yet finished booting.
        int wait = nmt->halt;
        for (co_unsigned8_t id = 1; !wait && id <= CO_NUM_NODES; id++)
                wait = (nmt->slaves[id - 1].assignment & 0x09) == 0x09
                                && nmt->slaves[id - 1].boot;
        if (!wait) {
                trace("NMT: all mandatory slaves started successfully");
                return co_nmt_startup_slave(nmt);
        }
        return NULL;
}
#endif
 
static co_nmt_state_t *
co_nmt_start_on_enter(co_nmt_t *nmt)
{
        assert(nmt);
 
        diag(DIAG_INFO, 0, "NMT: entering operational state");
 
        // Enable all services.
        co_nmt_srv_set(&nmt->srv, nmt, CO_NMT_START_SRV);
 
        nmt->st = CO_NMT_ST_START | (nmt->st & CO_NMT_ST_TOGGLE);
        co_nmt_st_ind(nmt, co_dev_get_id(nmt->dev), nmt->st);
 
#ifndef LELY_NO_CO_MASTER
        // If we're the master and bit 3 of the NMT startup value is 0 and bit 1
        // is 1, send the NMT start remote node command to all nodes (see Fig. 2
        // in CiA 302-2 version 4.1.0).
        if (nmt->master && (nmt->startup & 0x0a) == 0x02) {
                // Check if all slaves booted successfully.
                int boot = 1;
                for (co_unsigned8_t id = 1; boot && id <= CO_NUM_NODES; id++) {
                        struct co_nmt_slave *slave = &nmt->slaves[id - 1];
                        // Skip those slaves that are not in the network list.
                        if (!(slave->assignment & 0x01))
                                continue;
                        // Check if the slave finished booting successfully and
                        // can be started by the master.
                        boot = slave->booted && (!slave->es || slave->es == 'L')
                                        && !(slave->assignment & 0x04);
                }
                if (boot) {
                        // Start all NMT slaves at once.
                        co_nmt_cs_req(nmt, CO_NMT_CS_START, 0);
                } else {
                        for (co_unsigned8_t id = 1; id <= CO_NUM_NODES; id++) {
                                struct co_nmt_slave *slave =
                                                &nmt->slaves[id - 1];
                                // Skip those slaves that are not in the network
                                // list (bit 0), or that we are not allowed to
                                // boot (bit 2).
                                if ((slave->assignment & 0x05) != 0x05)
                                        continue;
                                // Only start slaves that have finished booting
                                // successfully and are not already (expected to
                                // be) operational.
                                if (slave->booted
                                                && (!slave->es || slave->es == 'L')
                                                && slave->est != CO_NMT_ST_START)
                                        co_nmt_cs_req(nmt, CO_NMT_CS_START, id);
                        }
                }
        }
#endif
 
        if (nmt->cs_ind)
                nmt->cs_ind(nmt, CO_NMT_CS_START, nmt->cs_data);
 
        return NULL;
}
 
static co_nmt_state_t *
co_nmt_start_on_cs(co_nmt_t *nmt, co_unsigned8_t cs)
{
        (void)nmt;
 
        switch (cs) {
        case CO_NMT_CS_STOP: return co_nmt_stop_state;
        case CO_NMT_CS_ENTER_PREOP: return co_nmt_preop_state;
        case CO_NMT_CS_RESET_NODE: return co_nmt_reset_node_state;
        case CO_NMT_CS_RESET_COMM: return co_nmt_reset_comm_state;
        default: return NULL;
        }
}
 
static co_nmt_state_t *
co_nmt_stop_on_enter(co_nmt_t *nmt)
{
        assert(nmt);
 
        diag(DIAG_INFO, 0, "NMT: entering stopped state");
 
        // Disable all services (except LSS).
        co_nmt_srv_set(&nmt->srv, nmt, CO_NMT_STOP_SRV);
 
        nmt->st = CO_NMT_ST_STOP | (nmt->st & CO_NMT_ST_TOGGLE);
        co_nmt_st_ind(nmt, co_dev_get_id(nmt->dev), nmt->st);
 
        if (nmt->cs_ind)
                nmt->cs_ind(nmt, CO_NMT_CS_STOP, nmt->cs_data);
 
        return NULL;
}
 
static co_nmt_state_t *
co_nmt_stop_on_cs(co_nmt_t *nmt, co_unsigned8_t cs)
{
        (void)nmt;
 
        switch (cs) {
        case CO_NMT_CS_START: return co_nmt_start_state;
        case CO_NMT_CS_ENTER_PREOP: return co_nmt_preop_state;
        case CO_NMT_CS_RESET_NODE: return co_nmt_reset_node_state;
        case CO_NMT_CS_RESET_COMM: return co_nmt_reset_comm_state;
        default: return NULL;
        }
}
 
static co_nmt_state_t *
co_nmt_startup(co_nmt_t *nmt)
{
        assert(nmt);
 
#ifndef LELY_NO_CO_MASTER
        if (nmt->master)
                return co_nmt_startup_master(nmt);
#endif
        return co_nmt_startup_slave(nmt);
}
 
#ifndef LELY_NO_CO_MASTER
static co_nmt_state_t *
co_nmt_startup_master(co_nmt_t *nmt)
{
        assert(nmt);
        assert(nmt->master);
 
        // Enable NMT slave management.
        co_nmt_slaves_init(nmt);
 
        // Check if any node has the keep-alive bit set.
        int keep = 0;
        for (co_unsigned8_t id = 1; !keep && id <= CO_NUM_NODES; id++)
                keep = (nmt->slaves[id - 1].assignment & 0x11) == 0x11;
 
        // Send the NMT 'reset communication' command to all slaves with
        // the keep-alive bit _not_ set. This includes slaves which are not in
        // the network list.
        if (keep) {
                for (co_unsigned8_t id = 1; id <= CO_NUM_NODES; id++) {
                        // Do not reset the master itself.
                        if (id == co_dev_get_id(nmt->dev))
                                continue;
                        if ((nmt->slaves[id - 1].assignment & 0x11) != 0x11)
                                co_nmt_cs_req(nmt, CO_NMT_CS_RESET_COMM, id);
                }
        } else {
                co_nmt_cs_req(nmt, CO_NMT_CS_RESET_COMM, 0);
        }
 
        // Start the 'boot slave' processes.
        switch (co_nmt_slaves_boot(nmt)) {
        case -1:
                // Halt the network boot-up procedure if the 'boot slave'
                // process failed for a mandatory slave.
                nmt->halt = 1;
                return NULL;
        case 0: return co_nmt_startup_slave(nmt);
        default:
                // Wait for all mandatory slaves to finish booting.
                trace("NMT: waiting for mandatory slaves to start");
                return NULL;
        }
}
#endif
 
static co_nmt_state_t *
co_nmt_startup_slave(co_nmt_t *nmt)
{
        assert(nmt);
 
        // Enter the operational state automatically if bit 2 of the NMT startup
        // value is 0.
        return (nmt->startup & 0x04) ? NULL : co_nmt_start_state;
}
 
static void
co_nmt_ec_init(co_nmt_t *nmt)
{
        assert(nmt);
 
        // Enable life guarding or heartbeat production.
        nmt->gt = co_dev_get_val_u16(nmt->dev, 0x100c, 0x00);
        nmt->ltf = co_dev_get_val_u8(nmt->dev, 0x100d, 0x00);
        nmt->ms = co_dev_get_val_u16(nmt->dev, 0x1017, 0x00);
 
        nmt->lg_state = CO_NMT_EC_RESOLVED;
 
        if (__unlikely(co_nmt_ec_update(nmt) == -1))
                diag(DIAG_ERROR, get_errc(), "unable to start %s",
                                nmt->ms ? "heartbeat production"
                                        : "life guarding");
}
 
static void
co_nmt_ec_fini(co_nmt_t *nmt)
{
        assert(nmt);
 
        // Disable life guarding and heartbeat production.
        nmt->gt = 0;
        nmt->ltf = 0;
        nmt->ms = 0;
 
        nmt->lg_state = CO_NMT_EC_RESOLVED;
 
        co_nmt_ec_update(nmt);
}
 
static int
co_nmt_ec_update(co_nmt_t *nmt)
{
        assert(nmt);
 
        // Heartbeat production has precedence over life guarding.
        int lt = nmt->ms ? 0 : nmt->gt * nmt->ltf;
#ifndef LELY_NO_CO_MASTER
        // Disable life guarding for the master.
        if (nmt->master)
                lt = 0;
#endif
 
        if (lt) {
                if (!nmt->recv_700) {
                        nmt->recv_700 = can_recv_create();
                        if (__unlikely(!nmt->recv_700))
                                return -1;
                        can_recv_set_func(nmt->recv_700, &co_nmt_recv_700, nmt);
                }
                // Start the CAN frame receiver for node guarding RTRs.
                can_recv_start(nmt->recv_700, nmt->net,
                                CO_NMT_EC_CANID(co_dev_get_id(nmt->dev)),
                                CAN_FLAG_RTR);
        } else if (nmt->recv_700) {
                can_recv_destroy(nmt->recv_700);
                nmt->recv_700 = NULL;
        }
 
        if (nmt->ms || lt) {
                if (!nmt->ec_timer) {
                        nmt->ec_timer = can_timer_create();
                        if (__unlikely(!nmt->ec_timer))
                                return -1;
                        can_timer_set_func(
                                        nmt->ec_timer, &co_nmt_ec_timer, nmt);
                }
                // Start the CAN timer for heartbeat production or life
                // guarding.
                int ms = nmt->ms ? nmt->ms : lt;
                struct timespec interval = { ms / 1000, (ms % 1000) * 1000000 };
                can_timer_start(nmt->ec_timer, nmt->net, NULL, &interval);
        } else if (nmt->ec_timer) {
                can_timer_destroy(nmt->ec_timer);
                nmt->ec_timer = NULL;
        }
 
        return 0;
}
 
static int
co_nmt_ec_send_res(co_nmt_t *nmt, co_unsigned8_t st)
{
        assert(nmt);
 
        struct can_msg msg = CAN_MSG_INIT;
        msg.id = CO_NMT_EC_CANID(co_dev_get_id(nmt->dev));
        msg.len = 1;
        msg.data[0] = st;
 
        return can_net_send(nmt->net, &msg);
}
 
static void
co_nmt_hb_init(co_nmt_t *nmt)
{
        assert(nmt);
 
        // Create and initialize the heartbeat consumers.
        assert(!nmt->hbs);
        assert(!nmt->nhb);
        co_obj_t *obj_1016 = co_dev_find_obj(nmt->dev, 0x1016);
        if (obj_1016) {
                nmt->nhb = co_obj_get_val_u8(obj_1016, 0x00);
                nmt->hbs = calloc(nmt->nhb, sizeof(*nmt->hbs));
                if (__unlikely(!nmt->hbs && nmt->nhb)) {
                        nmt->nhb = 0;
                        set_errc(errno2c(errno));
                        diag(DIAG_ERROR, get_errc(),
                                        "unable to create heartbeat consumers");
                }
        }
 
        for (co_unsigned8_t i = 0; i < nmt->nhb; i++) {
                nmt->hbs[i] = co_nmt_hb_create(nmt->net, nmt);
                if (__unlikely(!nmt->hbs[i])) {
                        diag(DIAG_ERROR, get_errc(),
                                        "unable to create heartbeat consumer 0x%02X",
                                        (co_unsigned8_t)(i + 1));
                        continue;
                }
 
                co_unsigned32_t val = co_obj_get_val_u32(obj_1016, i + 1);
                co_unsigned8_t id = (val >> 16) & 0xff;
                co_unsigned16_t ms = val & 0xffff;
                co_nmt_hb_set_1016(nmt->hbs[i], id, ms);
        }
}
 
static void
co_nmt_hb_fini(co_nmt_t *nmt)
{
        assert(nmt);
 
        // Destroy all heartbeat consumers.
        for (size_t i = 0; i < nmt->nhb; i++)
                co_nmt_hb_destroy(nmt->hbs[i]);
        free(nmt->hbs);
        nmt->hbs = NULL;
        nmt->nhb = 0;
}
 
#ifndef LELY_NO_CO_MASTER
 
static void
co_nmt_slaves_init(co_nmt_t *nmt)
{
        assert(nmt);
        assert(nmt->master);
 
        co_nmt_slaves_fini(nmt);
 
        for (co_unsigned8_t id = 1; id <= CO_NUM_NODES; id++) {
                struct co_nmt_slave *slave = &nmt->slaves[id - 1];
                slave->recv = can_recv_create();
                if (__unlikely(!slave->recv)) {
                        diag(DIAG_ERROR, get_errc(),
                                        "unable to create CAN frame receiver");
                        continue;
                }
                can_recv_set_func(slave->recv, &co_nmt_recv_700, nmt);
                // Start listening for boot-up notifications.
                can_recv_start(slave->recv, nmt->net, CO_NMT_EC_CANID(id), 0);
        }
 
        co_obj_t *obj_1f81 = co_dev_find_obj(nmt->dev, 0x1f81);
        if (__unlikely(!obj_1f81))
                return;
 
        co_unsigned8_t n = co_obj_get_val_u8(obj_1f81, 0x00);
        for (co_unsigned8_t i = 0; i < MIN(n, CO_NUM_NODES); i++)
                nmt->slaves[i].assignment = co_obj_get_val_u32(obj_1f81, i + 1);
}
 
static void
co_nmt_slaves_fini(co_nmt_t *nmt)
{
        assert(nmt);
 
        for (co_unsigned8_t id = 1; id <= CO_NUM_NODES; id++) {
                struct co_nmt_slave *slave = &nmt->slaves[id - 1];
 
                can_recv_destroy(slave->recv);
                slave->recv = NULL;
                can_timer_destroy(slave->timer);
                slave->timer = NULL;
 
                slave->assignment = 0;
                slave->est = 0;
                slave->rst = 0;
                slave->es = 0;
 
                slave->booted = 0;
                co_nmt_boot_destroy(slave->boot);
                slave->boot = NULL;
 
                co_nmt_cfg_destroy(slave->cfg);
                slave->cfg = NULL;
                slave->cfg_con = NULL;
                slave->cfg_data = NULL;
 
                slave->gt = 0;
                slave->ltf = 0;
                slave->rtr = 0;
                slave->ng_state = CO_NMT_EC_RESOLVED;
        }
}
 
static int
co_nmt_slaves_boot(co_nmt_t *nmt)
{
        assert(nmt);
        assert(nmt->master);
 
        int res = 0;
        for (co_unsigned8_t id = 1; id <= CO_NUM_NODES; id++) {
                struct co_nmt_slave *slave = &nmt->slaves[id - 1];
                // Skip those slaves that are not in the network list (bit 0).
                if ((slave->assignment & 0x01) != 0x01)
                        continue;
                int mandatory = !!(slave->assignment & 0x08);
                // Wait for all mandatory slaves to finish booting.
                if (!res && mandatory)
                        res = 1;
                // Nodes with the keep-alive bit _not_ set are booted when we
                // receive their boot-up signal.
                if (!(slave->assignment & 0x10))
                        continue;
                // Halt the network boot-up procedure if the 'boot slave'
                // process failed for a mandatory slave with the keep-alive bit
                // set.
                // clang-format off
                if (__unlikely(co_nmt_boot_req(nmt, id, nmt->timeout) == -1
                                && mandatory))
                        // clang-format on
                        res = -1;
        }
        return res;
}
 
#endif // !LELY_NO_CO_MASTER