nmt_hb.c

Go to the documentation of this file.

#include "nmt_hb.h"
#include "co.h"
#include <lely/co/dev.h>
#include <lely/util/diag.h>

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

struct __co_nmt_hb {
        can_net_t *net;
        co_nmt_t *nmt;
        can_recv_t *recv;
        can_timer_t *timer;
        co_unsigned8_t id;
        co_unsigned8_t st;
        co_unsigned16_t ms;
        int state;
};

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

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

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

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

struct __co_nmt_hb *
__co_nmt_hb_init(struct __co_nmt_hb *hb, can_net_t *net, co_nmt_t *nmt)
{
        assert(hb);
        assert(net);
        assert(nmt);

        int errc = 0;

        hb->net = net;
        hb->nmt = nmt;

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

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

        hb->id = 0;
        hb->st = 0;
        hb->ms = 0;
        hb->state = CO_NMT_EC_RESOLVED;

        return hb;

        can_timer_destroy(hb->timer);
error_create_timer:
        can_recv_destroy(hb->recv);
error_create_recv:
        set_errc(errc);
        return NULL;
}

void
__co_nmt_hb_fini(struct __co_nmt_hb *hb)
{
        assert(hb);

        can_timer_destroy(hb->timer);
        can_recv_destroy(hb->recv);
}

co_nmt_hb_t *
co_nmt_hb_create(can_net_t *net, co_nmt_t *nmt)
{
        int errc = 0;

        co_nmt_hb_t *hb = __co_nmt_hb_alloc();
        if (__unlikely(!hb)) {
                errc = get_errc();
                goto error_alloc_hb;
        }

        if (__unlikely(!__co_nmt_hb_init(hb, net, nmt))) {
                errc = get_errc();
                goto error_init_hb;
        }

        return hb;

error_init_hb:
        __co_nmt_hb_free(hb);
error_alloc_hb:
        set_errc(errc);
        return NULL;
}

void
co_nmt_hb_destroy(co_nmt_hb_t *hb)
{
        if (hb) {
                __co_nmt_hb_fini(hb);
                __co_nmt_hb_free(hb);
        }
}

void
co_nmt_hb_set_1016(co_nmt_hb_t *hb, co_unsigned8_t id, co_unsigned16_t ms)
{
        assert(hb);

        can_recv_stop(hb->recv);
        can_timer_stop(hb->timer);

        hb->id = id;
        hb->st = 0;
        hb->ms = ms;
        hb->state = CO_NMT_EC_RESOLVED;

        if (hb->id && hb->id <= CO_NUM_NODES && hb->ms)
                can_recv_start(hb->recv, hb->net, CO_NMT_EC_CANID(hb->id), 0);
}

void
co_nmt_hb_set_st(co_nmt_hb_t *hb, co_unsigned8_t st)
{
        assert(hb);

        if (hb->id && hb->id <= CO_NUM_NODES && hb->ms) {
                hb->st = st;
                hb->state = CO_NMT_EC_RESOLVED;
                // Reset the CAN timer for the heartbeat consumer.
                can_timer_timeout(hb->timer, hb->net, hb->ms);
        }
}

static int
co_nmt_hb_recv(const struct can_msg *msg, void *data)
{
        assert(msg);
        co_nmt_hb_t *hb = data;
        assert(hb);
        assert(hb->id && hb->id <= CO_NUM_NODES);
        assert(msg->id == (uint32_t)CO_NMT_EC_CANID(hb->id));
        assert(hb->ms);

        // Obtain the node status from the CAN frame. Ignore if the toggle bit
        // is set, since then it is not a heartbeat message.
        if (__unlikely(msg->len < 1))
                return 0;
        co_unsigned8_t st = msg->data[0];
        if (__unlikely(st & CO_NMT_ST_TOGGLE))
                return 0;

        // Update the state.
        co_unsigned8_t old_st = hb->st;
        co_nmt_hb_set_st(hb, st);

        if (hb->state == CO_NMT_EC_OCCURRED) {
                diag(DIAG_INFO, 0,
                                "NMT: heartbeat time out resolved for node %d",
                                hb->id);
                // If a heartbeat timeout event occurred, notify the user that
                // it has been resolved.
                hb->state = CO_NMT_EC_RESOLVED;
                co_nmt_hb_ind(hb->nmt, hb->id, hb->state, CO_NMT_EC_TIMEOUT, 0);
        }

        // Notify the application of the occurrence of a state change.
        if (st != old_st) {
                diag(DIAG_INFO, 0,
                                "NMT: heartbeat state change occurred for node %d",
                                hb->id);
                co_nmt_hb_ind(hb->nmt, hb->id, CO_NMT_EC_OCCURRED,
                                CO_NMT_EC_STATE, st);
        }

        return 0;
}

static int
co_nmt_hb_timer(const struct timespec *tp, void *data)
{
        (void)tp;
        co_nmt_hb_t *hb = data;
        assert(hb);

        // Notify the application of the occurrence of a heartbeat timeout
        // event.
        diag(DIAG_INFO, 0, "NMT: heartbeat time out occurred for node %d",
                        hb->id);
        hb->state = CO_NMT_EC_OCCURRED;
        co_nmt_hb_ind(hb->nmt, hb->id, hb->state, CO_NMT_EC_TIMEOUT, 0);

        return 0;
}