emcy.c

Go to the documentation of this file.

#include "co.h"

#ifndef LELY_NO_CO_EMCY

#include <lely/can/buf.h>
#include <lely/co/dev.h>
#include <lely/co/emcy.h>
#include <lely/co/obj.h>
#include <lely/co/sdo.h>
#include <lely/co/val.h>
#include <lely/util/diag.h>
#include <lely/util/endian.h>
#include <lely/util/time.h>

#include <assert.h>
#include <stdlib.h>

struct co_emcy_msg {
        co_unsigned16_t eec;
        co_unsigned8_t er;
};

struct co_emcy_node {
        co_emcy_t *emcy;
        co_unsigned8_t id;
        can_recv_t *recv;
};

static struct co_emcy_node *co_emcy_node_create(
                co_emcy_t *emcy, co_unsigned8_t id);

static void co_emcy_node_destroy(struct co_emcy_node *node);

static int co_emcy_node_recv(const struct can_msg *msg, void *data);

struct __co_emcy {
        can_net_t *net;
        co_dev_t *dev;
        co_sub_t *sub_1001_00;
        co_obj_t *obj_1003;
        size_t nmsg;
        struct co_emcy_msg *msgs;
        struct can_buf buf;
        can_timer_t *timer;
        struct timespec inhibit;
        struct co_emcy_node *nodes[CO_NUM_NODES];
        co_emcy_ind_t *ind;
        void *data;
};

struct co_1003 {
        co_unsigned8_t n;
        co_unsigned32_t ef[0xfe];
};

static int co_emcy_set_1003(co_emcy_t *emcy);

static co_unsigned32_t co_1003_dn_ind(
                co_sub_t *sub, struct co_sdo_req *req, void *data);

static co_unsigned32_t co_1014_dn_ind(
                co_sub_t *sub, struct co_sdo_req *req, void *data);

static int co_emcy_set_1028(
                co_emcy_t *emcy, co_unsigned8_t id, co_unsigned32_t cobid);

static co_unsigned32_t co_1028_dn_ind(
                co_sub_t *sub, struct co_sdo_req *req, void *data);

static int co_emcy_timer(const struct timespec *tp, void *data);

static int co_emcy_send(co_emcy_t *emcy, co_unsigned16_t eec, co_unsigned8_t er,
                const uint8_t msef[5]);

static void co_emcy_flush(co_emcy_t *emcy);

void *
__co_emcy_alloc(void)
{
        void *ptr = malloc(sizeof(struct __co_emcy));
        if (__unlikely(!ptr))
                set_errc(errno2c(errno));
        return ptr;
}

void
__co_emcy_free(void *ptr)
{
        free(ptr);
}

struct __co_emcy *
__co_emcy_init(struct __co_emcy *emcy, can_net_t *net, co_dev_t *dev)
{
        assert(emcy);
        assert(net);
        assert(dev);

        int errc = 0;

        emcy->net = net;
        emcy->dev = dev;

        emcy->sub_1001_00 = co_dev_find_sub(emcy->dev, 0x1001, 0x00);
        if (__unlikely(!emcy->sub_1001_00)) {
                errc = errnum2c(ERRNUM_NOSYS);
                goto error_sub_1001_00;
        }
        emcy->obj_1003 = co_dev_find_obj(emcy->dev, 0x1003);

        emcy->nmsg = 0;
        emcy->msgs = NULL;

        if (__unlikely(can_buf_init(&emcy->buf, 0) == -1)) {
                errc = get_errc();
                goto error_init_buf;
        }

        emcy->timer = can_timer_create();
        if (__unlikely(!emcy->timer)) {
                errc = get_errc();
                goto error_create_timer;
        }
        can_timer_set_func(emcy->timer, &co_emcy_timer, emcy);

        can_net_get_time(emcy->net, &emcy->inhibit);

        memset(emcy->nodes, 0, CO_NUM_NODES * sizeof(struct co_emcy_node *));

        emcy->ind = NULL;
        emcy->data = NULL;

        // Set the download indication function for the pre-defined error field.
        if (emcy->obj_1003)
                co_obj_set_dn_ind(emcy->obj_1003, &co_1003_dn_ind, emcy);

        // Set the download indication function for the EMCY COB-ID object.
        co_obj_t *obj_1014 = co_dev_find_obj(emcy->dev, 0x1014);
        if (obj_1014)
                co_obj_set_dn_ind(obj_1014, &co_1014_dn_ind, emcy);

        co_obj_t *obj_1028 = co_dev_find_obj(emcy->dev, 0x1028);
        if (obj_1028) {
                // Set the download indication function for the emergency
                // consumer object.
                co_obj_set_dn_ind(obj_1028, &co_1028_dn_ind, emcy);
                // Initialize the nodes.
                co_unsigned8_t maxid = MIN(co_obj_get_val_u8(obj_1028, 0x00),
                                CO_NUM_NODES);
                for (co_unsigned8_t id = 1; id <= maxid; id++) {
                        co_unsigned32_t cobid =
                                        co_obj_get_val_u32(obj_1028, id);
                        // clang-format off
                        if (__unlikely(co_emcy_set_1028(emcy, id, cobid)
                                        == -1)) {
                                // clang-format on
                                errc = get_errc();
                                goto error_set_1028;
                        }
                }
        }

        return emcy;

error_set_1028:
        if (obj_1028)
                co_obj_set_dn_ind(obj_1028, NULL, NULL);
        if (obj_1014)
                co_obj_set_dn_ind(obj_1014, NULL, NULL);
        if (emcy->obj_1003)
                co_obj_set_dn_ind(emcy->obj_1003, NULL, NULL);
        can_timer_destroy(emcy->timer);
error_create_timer:
        can_buf_fini(&emcy->buf);
error_init_buf:
error_sub_1001_00:
        set_errc(errc);
        return NULL;
}

void
__co_emcy_fini(struct __co_emcy *emcy)
{
        assert(emcy);

        // Remove the download indication function for the emergency consumer
        // object.
        co_obj_t *obj_1028 = co_dev_find_obj(emcy->dev, 0x1028);
        if (obj_1028)
                co_obj_set_dn_ind(obj_1028, NULL, NULL);

        // Remove the download indication function for the EMCY COB-ID object.
        co_obj_t *obj_1014 = co_dev_find_obj(emcy->dev, 0x1014);
        if (obj_1014)
                co_obj_set_dn_ind(obj_1014, NULL, NULL);

        // Remove the download indication function for the pre-defined error
        // field.
        if (emcy->obj_1003)
                co_obj_set_dn_ind(emcy->obj_1003, NULL, NULL);

        for (co_unsigned8_t id = 1; id <= CO_NUM_NODES; id++)
                co_emcy_node_destroy(emcy->nodes[id - 1]);

        can_timer_destroy(emcy->timer);

        can_buf_fini(&emcy->buf);

        free(emcy->msgs);
}

co_emcy_t *
co_emcy_create(can_net_t *net, co_dev_t *dev)
{
        trace("creating EMCY producer service");

        int errc = 0;

        co_emcy_t *emcy = __co_emcy_alloc();
        if (__unlikely(!emcy)) {
                errc = get_errc();
                goto error_alloc_emcy;
        }

        if (__unlikely(!__co_emcy_init(emcy, net, dev))) {
                errc = get_errc();
                goto error_init_emcy;
        }

        return emcy;

error_init_emcy:
        __co_emcy_free(emcy);
error_alloc_emcy:
        set_errc(errc);
        return NULL;
}

void
co_emcy_destroy(co_emcy_t *emcy)
{
        if (emcy) {
                trace("destroying EMCY producer service");
                __co_emcy_fini(emcy);
                __co_emcy_free(emcy);
        }
}

can_net_t *
co_emcy_get_net(const co_emcy_t *emcy)
{
        assert(emcy);

        return emcy->net;
}

co_dev_t *
co_emcy_get_dev(const co_emcy_t *emcy)
{
        assert(emcy);

        return emcy->dev;
}

int
co_emcy_push(co_emcy_t *emcy, co_unsigned16_t eec, co_unsigned8_t er,
                const uint8_t msef[5])
{
        assert(emcy);

        if (__unlikely(!eec)) {
                set_errnum(ERRNUM_INVAL);
                return -1;
        }
        // Bit 0 (generic error) shall be signaled at any error situation.
        er |= 0x01;

        if (msef)
                diag(DIAG_INFO, 0, "EMCY: %04X %02X %u %u %u %u %u", eec, er,
                                msef[0], msef[1], msef[2], msef[3], msef[4]);
        else
                diag(DIAG_INFO, 0, "EMCY: %04X %02X", eec, er);

        // Make room on the stack.
        struct co_emcy_msg *msgs = realloc(emcy->msgs,
                        (emcy->nmsg + 1) * sizeof(struct co_emcy_msg));
        if (__unlikely(!msgs)) {
                set_errc(errno2c(errno));
                return -1;
        }
        emcy->msgs = msgs;
        if (emcy->nmsg) {
                // Copy the current error register.
                er |= emcy->msgs[0].er;
                // Move the older messages.
                memmove(emcy->msgs + 1, emcy->msgs,
                                emcy->nmsg * sizeof(struct co_emcy_msg));
        }
        emcy->nmsg++;
        // Push the error to the stack.
        emcy->msgs[0].eec = eec;
        emcy->msgs[0].er = er;

        // Update the pre-defined error field.
        if (emcy->obj_1003)
                co_emcy_set_1003(emcy);

        return co_emcy_send(emcy, eec, er, msef);
}

int
co_emcy_pop(co_emcy_t *emcy, co_unsigned16_t *peec, co_unsigned8_t *per)
{
        assert(emcy);

        co_emcy_peek(emcy, peec, per);

        if (!emcy->nmsg)
                return 0;

        // Move the older messages.
        if (--emcy->nmsg)
                memmove(emcy->msgs, emcy->msgs + 1,
                                emcy->nmsg * sizeof(struct co_emcy_msg));

        // Update the pre-defined error field.
        if (emcy->obj_1003)
                co_emcy_set_1003(emcy);

        if (emcy->nmsg) {
                // Store the next error in the error register and the
                // manufacturer-specific field.
                uint8_t msef[5] = { 0 };
                stle_u16(msef, emcy->msgs[0].eec);
                return co_emcy_send(emcy, 0, emcy->msgs[0].er, msef);
        } else {
                return co_emcy_send(emcy, 0, 0, NULL);
        }
}

void
co_emcy_peek(const co_emcy_t *emcy, co_unsigned16_t *peec, co_unsigned8_t *per)
{
        assert(emcy);

        if (peec)
                *peec = emcy->nmsg ? emcy->msgs[0].eec : 0;
        if (per)
                *per = emcy->nmsg ? emcy->msgs[0].er : 0;
}

int
co_emcy_clear(co_emcy_t *emcy)
{
        assert(emcy);

        if (!emcy->nmsg)
                return 0;

        // Clear the stack.
        emcy->nmsg = 0;

        // Clear the pre-defined error field.
        if (emcy->obj_1003)
                co_emcy_set_1003(emcy);

        // Send the 'error reset/no error' message.
        return co_emcy_send(emcy, 0, 0, NULL);
}

void
co_emcy_get_ind(const co_emcy_t *emcy, co_emcy_ind_t **pind, void **pdata)
{
        assert(emcy);

        if (pind)
                *pind = emcy->ind;
        if (pdata)
                *pdata = emcy->data;
}

void
co_emcy_set_ind(co_emcy_t *emcy, co_emcy_ind_t *ind, void *data)
{
        assert(emcy);

        emcy->ind = ind;
        emcy->data = data;
}

struct co_emcy_node *
co_emcy_node_create(co_emcy_t *emcy, co_unsigned8_t id)
{
        assert(emcy);
        assert(id && id <= CO_NUM_NODES);

        trace("creating EMCY consumer service for node %d", id);

        int errc = 0;

        struct co_emcy_node *node = malloc(sizeof(*node));
        if (__unlikely(!node)) {
                errc = errno2c(errno);
                goto error_alloc_node;
        }

        node->emcy = emcy;
        node->id = id;

        node->recv = can_recv_create();
        if (__unlikely(!node->recv)) {
                errc = get_errc();
                goto error_create_recv;
        }
        can_recv_set_func(node->recv, &co_emcy_node_recv, node);

        return node;

error_create_recv:
        free(node);
error_alloc_node:
        set_errc(errc);
        return NULL;
}

void
co_emcy_node_destroy(struct co_emcy_node *node)
{
        if (node) {
                trace("destroying EMCY consumer service for node %d", node->id);
                can_recv_destroy(node->recv);
                free(node);
        }
}

static int
co_emcy_node_recv(const struct can_msg *msg, void *data)
{
        assert(msg);
        struct co_emcy_node *node = data;
        assert(node);
        co_emcy_t *emcy = node->emcy;
        assert(emcy);

        // Ignore remote frames.
        if (__unlikely(msg->flags & CAN_FLAG_RTR))
                return 0;

#ifndef LELY_NO_CANFD
        // Ignore CAN FD format frames.
        if (__unlikely(msg->flags & CAN_FLAG_EDL))
                return 0;
#endif

        // Extract the parameters from the frame.
        co_unsigned16_t eec = 0;
        if (msg->len >= 2)
                eec = ldle_u16(msg->data);
        co_unsigned8_t er = msg->len >= 3 ? msg->data[2] : 0;
        co_unsigned8_t msef[5] = { 0 };
        if (msg->len >= 4)
                memcpy(msef, msg->data + 3, MAX((uint8_t)(msg->len - 3), 5));

        // Notify the user.
        trace("EMCY: received %04X %02X", eec, er);
        if (emcy->ind)
                emcy->ind(emcy, node->id, eec, er, msef, emcy->data);

        return 0;
}

static int
co_emcy_set_1003(co_emcy_t *emcy)
{
        assert(emcy);
        assert(emcy->obj_1003);

        struct co_1003 *val_1003 = co_obj_addressof_val(emcy->obj_1003);
        co_unsigned8_t nsubidx = co_obj_get_subidx(emcy->obj_1003, 0, NULL);
        if (__unlikely(!nsubidx))
                return 0;

        // Copy the emergency error codes.
        val_1003->n = MIN((co_unsigned8_t)emcy->nmsg, nsubidx - 1);
        for (int i = 0; i < val_1003->n; i++)
                val_1003->ef[i] = emcy->msgs[i].eec;
        for (int i = val_1003->n; i < nsubidx - 1; i++)
                val_1003->ef[i] = 0;

        return 0;
}

static co_unsigned32_t
co_1003_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)) == 0x1003);
        assert(req);
        co_emcy_t *emcy = data;
        assert(emcy);

        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_WRITE;
                goto error;
        }

        // Only the value 0 is allowed.
        assert(type == CO_DEFTYPE_UNSIGNED8);
        if (__unlikely(val.u8)) {
                ac = CO_SDO_AC_PARAM_VAL;
                goto error;
        }

        emcy->nmsg = 0;

        co_sub_dn(sub, &val);
        co_val_fini(type, &val);

        co_emcy_set_1003(emcy);
        return 0;

error:
        co_val_fini(type, &val);
        return ac;
}

static co_unsigned32_t
co_1014_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)) == 0x1014);
        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 cobid = val.u32;
        co_unsigned32_t cobid_old = co_sub_get_val_u32(sub);
        if (cobid == cobid_old)
                goto error;

        // The CAN-ID cannot be changed when the EMCY is and remains valid.
        int valid = !(cobid & CO_EMCY_COBID_VALID);
        int valid_old = !(cobid_old & CO_EMCY_COBID_VALID);
        uint32_t canid = cobid & CAN_MASK_EID;
        uint32_t canid_old = cobid_old & CAN_MASK_EID;
        if (__unlikely(valid && valid_old && canid != canid_old)) {
                ac = CO_SDO_AC_PARAM_VAL;
                goto error;
        }

        // A 29-bit CAN-ID is only valid if the frame bit is set.
        // clang-format off
        if (__unlikely(!(cobid & CO_EMCY_COBID_FRAME)
                        && (cobid & (CAN_MASK_EID ^ CAN_MASK_BID)))) {
                // clang-format on
                ac = CO_SDO_AC_PARAM_VAL;
                goto error;
        }

        co_sub_dn(sub, &val);
error:
        co_val_fini(type, &val);
        return ac;
}

static int
co_emcy_set_1028(co_emcy_t *emcy, co_unsigned8_t id, co_unsigned32_t cobid)
{
        assert(emcy);
        assert(id && id <= CO_NUM_NODES);
        struct co_emcy_node *node = emcy->nodes[id - 1];

        if (!(cobid & CO_EMCY_COBID_VALID)) {
                if (!node) {
                        node = co_emcy_node_create(emcy, id);
                        if (__unlikely(!node))
                                return -1;
                        emcy->nodes[id - 1] = node;
                }
                // Register the receiver under the specified CAN-ID.
                uint32_t id = cobid;
                uint8_t flags = 0;
                if (id & CO_EMCY_COBID_FRAME) {
                        id &= CAN_MASK_EID;
                        flags |= CAN_FLAG_IDE;
                } else {
                        id &= CAN_MASK_BID;
                }
                can_recv_start(node->recv, emcy->net, id, flags);
        } else if (node) {
                co_emcy_node_destroy(node);
                emcy->nodes[id - 1] = NULL;
        }

        return 0;
}

static co_unsigned32_t
co_1028_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)) == 0x1028);
        assert(req);
        co_emcy_t *emcy = data;
        assert(emcy);

        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 id = co_sub_get_subidx(sub);
        if (__unlikely(!id)) {
                ac = CO_SDO_AC_NO_WRITE;
                goto error;
        }
        co_unsigned8_t maxid = MIN(co_obj_get_val_u8(co_sub_get_obj(sub), 0),
                        CO_NUM_NODES);
        if (__unlikely(id > maxid)) {
                ac = CO_SDO_AC_NO_SUB;
                goto error;
        }

        assert(type == CO_DEFTYPE_UNSIGNED32);
        co_unsigned32_t cobid = val.u32;
        co_unsigned32_t cobid_old = co_sub_get_val_u32(sub);
        if (cobid == cobid_old)
                goto error;

        // The CAN-ID cannot be changed when the EMCY is and remains valid.
        int valid = !(cobid & CO_EMCY_COBID_VALID);
        int valid_old = !(cobid_old & CO_EMCY_COBID_VALID);
        uint32_t canid = cobid & CAN_MASK_EID;
        uint32_t canid_old = cobid_old & CAN_MASK_EID;
        if (__unlikely(valid && valid_old && canid != canid_old)) {
                ac = CO_SDO_AC_PARAM_VAL;
                goto error;
        }

        // A 29-bit CAN-ID is only valid if the frame bit is set.
        // clang-format off
        if (__unlikely(!(cobid & CO_EMCY_COBID_FRAME)
                        && (cobid & (CAN_MASK_EID ^ CAN_MASK_BID)))) {
                // clang-format on
                ac = CO_SDO_AC_PARAM_VAL;
                goto error;
        }

        if (__unlikely(co_emcy_set_1028(emcy, id, cobid) == -1)) {
                ac = CO_SDO_AC_ERROR;
                goto error;
        }

        co_sub_dn(sub, &val);
error:
        co_val_fini(type, &val);
        return ac;
}

static int
co_emcy_timer(const struct timespec *tp, void *data)
{
        (void)tp;
        co_emcy_t *emcy = data;
        assert(emcy);

        co_emcy_flush(emcy);

        return 0;
}

static int
co_emcy_send(co_emcy_t *emcy, co_unsigned16_t eec, co_unsigned8_t er,
                const uint8_t msef[5])
{
        assert(emcy);

        // Update the error register in the object dictionary.
        co_sub_set_val_u8(emcy->sub_1001_00, er);

        // Check whether the EMCY COB-ID exists and is valid.
        co_obj_t *obj_1014 = co_dev_find_obj(emcy->dev, 0x1014);
        if (!obj_1014)
                return 0;
        co_unsigned32_t cobid = co_obj_get_val_u32(obj_1014, 0x00);
        if (cobid & CO_EMCY_COBID_VALID)
                return 0;

        // Create the frame.
        struct can_msg msg = CAN_MSG_INIT;
        msg.id = cobid;
        if (cobid & CO_EMCY_COBID_FRAME) {
                msg.id &= CAN_MASK_EID;
                msg.flags |= CAN_FLAG_IDE;
        } else {
                msg.id &= CAN_MASK_BID;
        }
        msg.len = CAN_MAX_LEN;
        stle_u32(msg.data, eec);
        msg.data[2] = er;
        if (msef)
                memcpy(msg.data + 3, msef, 5);

        // Add the frame to the buffer.
        if (__unlikely(!can_buf_write(&emcy->buf, &msg, 1))) {
                if (__unlikely(!can_buf_reserve(&emcy->buf, 1)))
                        return -1;
                can_buf_write(&emcy->buf, &msg, 1);
        }

        co_emcy_flush(emcy);
        return 0;
}

static void
co_emcy_flush(co_emcy_t *emcy)
{
        assert(emcy);

        co_unsigned16_t inhibit = co_dev_get_val_u16(emcy->dev, 0x1015, 0x00);

        struct timespec now = { 0, 0 };
        can_net_get_time(emcy->net, &now);

        while (can_buf_size(&emcy->buf)) {
                if (timespec_cmp(&now, &emcy->inhibit) < 0) {
                        can_timer_start(emcy->timer, emcy->net, &emcy->inhibit,
                                        NULL);
                        return;
                }
                // Update the inhibit time.
                emcy->inhibit = now;
                timespec_add_usec(&emcy->inhibit, inhibit * 100);
                // Send the frame.
                struct can_msg msg;
                if (__likely(can_buf_read(&emcy->buf, &msg, 1)))
                        can_net_send(emcy->net, &msg);
        }
}

#endif // !LELY_NO_CO_EMCY