nmt_srv.c

Go to the documentation of this file.

#include "co.h"
#include <lely/util/diag.h>
#ifndef LELY_NO_CO_CSDO
#include <lely/co/csdo.h>
#endif
#include <lely/co/dev.h>
#ifndef LELY_NO_CO_EMCY
#include <lely/co/emcy.h>
#endif
#ifndef LELY_NO_CO_LSS
#include <lely/co/lss.h>
#endif
#ifndef LELY_NO_CO_RPDO
#include <lely/co/rpdo.h>
#endif
#include <lely/co/ssdo.h>
#ifndef LELY_NO_CO_SYNC
#include <lely/co/sync.h>
#endif
#ifndef LELY_NO_CO_TIME
#include <lely/co/time.h>
#endif
#ifndef LELY_NO_CO_TPDO
#include <lely/co/tpdo.h>
#endif
#include "nmt_srv.h"

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

#if !defined(LELY_NO_CO_RPDO) || !defined(LELY_NO_CO_TPDO)
static void co_nmt_srv_init_pdo(
                struct co_nmt_srv *srv, can_net_t *net, co_dev_t *dev);
static void co_nmt_srv_fini_pdo(struct co_nmt_srv *srv);
#ifndef LELY_NO_CO_RPDO
static void co_nmt_srv_rpdo_err(co_rpdo_t *pdo, co_unsigned16_t eec,
                co_unsigned8_t er, void *data);
#endif
#endif

static void co_nmt_srv_init_sdo(
                struct co_nmt_srv *srv, can_net_t *net, co_dev_t *dev);
static void co_nmt_srv_fini_sdo(struct co_nmt_srv *srv);

#ifndef LELY_NO_CO_SYNC
static void co_nmt_srv_init_sync(
                struct co_nmt_srv *srv, can_net_t *net, co_dev_t *dev);
static void co_nmt_srv_fini_sync(struct co_nmt_srv *srv);
static void co_nmt_srv_sync_ind(
                co_sync_t *sync, co_unsigned8_t cnt, void *data);
static void co_nmt_srv_sync_err(co_sync_t *sync, co_unsigned16_t eec,
                co_unsigned8_t er, void *data);
#endif

#ifndef LELY_NO_CO_TIME
static void co_nmt_srv_init_time(
                struct co_nmt_srv *srv, can_net_t *net, co_dev_t *dev);
static void co_nmt_srv_fini_time(struct co_nmt_srv *srv);
#endif

#ifndef LELY_NO_CO_EMCY
static void co_nmt_srv_init_emcy(
                struct co_nmt_srv *srv, can_net_t *net, co_dev_t *dev);
static void co_nmt_srv_fini_emcy(struct co_nmt_srv *srv);
#endif

#ifndef LELY_NO_CO_LSS
static void co_nmt_srv_init_lss(struct co_nmt_srv *srv, co_nmt_t *nmt);
static void co_nmt_srv_fini_lss(struct co_nmt_srv *srv);
#endif

#define CO_NUM_PDO 512

#define CO_NUM_SDO 128

void
co_nmt_srv_init(struct co_nmt_srv *srv, co_nmt_t *nmt)
{
        assert(srv);
        assert(nmt);

        srv->nmt = nmt;

        srv->set = 0;

        srv->rpdos = NULL;
        srv->nrpdo = 0;
        srv->tpdos = NULL;
        srv->ntpdo = 0;

        srv->ssdos = NULL;
        srv->nssdo = 0;
        srv->csdos = NULL;
        srv->ncsdo = 0;

        srv->sync = NULL;
        srv->time = NULL;
        srv->emcy = NULL;

        srv->lss = NULL;
}

void
co_nmt_srv_fini(struct co_nmt_srv *srv)
{
        assert(srv);

        co_nmt_srv_set(srv, NULL, 0);
}

void
co_nmt_srv_set(struct co_nmt_srv *srv, co_nmt_t *nmt, int set)
{
        assert(srv);

        can_net_t *net = nmt ? co_nmt_get_net(nmt) : NULL;
        co_dev_t *dev = nmt ? co_nmt_get_dev(nmt) : NULL;

#ifndef LELY_NO_CO_LSS
        if ((srv->set & ~set) & CO_NMT_SRV_LSS)
                co_nmt_srv_fini_lss(srv);
#endif
#ifndef LELY_NO_CO_EMCY
        if ((srv->set & ~set) & CO_NMT_SRV_EMCY)
                co_nmt_srv_fini_emcy(srv);
#endif
#ifndef LELY_NO_CO_TIME
        if ((srv->set & ~set) & CO_NMT_SRV_TIME)
                co_nmt_srv_fini_time(srv);
#endif
#ifndef LELY_NO_CO_SYNC
        if ((srv->set & ~set) & CO_NMT_SRV_SYNC)
                co_nmt_srv_fini_sync(srv);
#endif
        if ((srv->set & ~set) & CO_NMT_SRV_SDO)
                co_nmt_srv_fini_sdo(srv);
#if !defined(LELY_NO_CO_RPDO) || !defined(LELY_NO_CO_TPDO)
        if ((srv->set & ~set) & CO_NMT_SRV_PDO)
                co_nmt_srv_fini_pdo(srv);
#endif

#if !defined(LELY_NO_CO_RPDO) || !defined(LELY_NO_CO_TPDO)
        if ((set & ~srv->set) & CO_NMT_SRV_PDO)
                co_nmt_srv_init_pdo(srv, net, dev);
#endif
        if ((set & ~srv->set) & CO_NMT_SRV_SDO)
                co_nmt_srv_init_sdo(srv, net, dev);
#ifndef LELY_NO_CO_SYNC
        if ((set & ~srv->set) & CO_NMT_SRV_SYNC)
                co_nmt_srv_init_sync(srv, net, dev);
#endif
#ifndef LELY_NO_CO_TIME
        if ((set & ~srv->set) & CO_NMT_SRV_TIME)
                co_nmt_srv_init_time(srv, net, dev);
#endif
#ifndef LELY_NO_CO_EMCY
        if ((set & ~srv->set) & CO_NMT_SRV_EMCY)
                co_nmt_srv_init_emcy(srv, net, dev);
#endif
#ifdef LELY_NO_CO_LSS
        (void)nmt;
#else
        if ((set & ~srv->set) & CO_NMT_SRV_LSS)
                co_nmt_srv_init_lss(srv, nmt);
#endif
}

#if !defined(LELY_NO_CO_RPDO) || !defined(LELY_NO_CO_TPDO)

static void
co_nmt_srv_init_pdo(struct co_nmt_srv *srv, can_net_t *net, co_dev_t *dev)
{
        assert(srv);
        assert(!(srv->set & CO_NMT_SRV_PDO));
        assert(!srv->rpdos);
        assert(!srv->nrpdo);
        assert(!srv->tpdos);
        assert(!srv->ntpdo);

        srv->set |= CO_NMT_SRV_PDO;

#ifndef LELY_NO_CO_RPDO
        // Create the Receive-PDOs.
        for (co_unsigned16_t i = 0; i < CO_NUM_PDO; i++) {
                co_obj_t *obj_1400 = co_dev_find_obj(dev, 0x1400 + i);
                co_obj_t *obj_1600 = co_dev_find_obj(dev, 0x1600 + i);
                if (!obj_1400 || !obj_1600)
                        continue;

                co_rpdo_t **rpdos =
                                realloc(srv->rpdos, (i + 1) * sizeof(*rpdos));
                if (__unlikely(!rpdos)) {
                        set_errc(errno2c(errno));
                        goto error;
                }
                srv->rpdos = rpdos;

                for (size_t j = srv->nrpdo; j < i; j++)
                        srv->rpdos[j] = NULL;
                srv->rpdos[i] = co_rpdo_create(net, dev, i + 1);
                if (__unlikely(!srv->rpdos[i]))
                        goto error;
                co_rpdo_set_err(srv->rpdos[i], &co_nmt_srv_rpdo_err, srv->nmt);

                srv->nrpdo = i + 1;
        }
#endif

#ifndef LELY_NO_CO_TPDO
        // Create the Transmit-PDOs.
        for (co_unsigned16_t i = 0; i < CO_NUM_PDO; i++) {
                co_obj_t *obj_1800 = co_dev_find_obj(dev, 0x1800 + i);
                co_obj_t *obj_1a00 = co_dev_find_obj(dev, 0x1a00 + i);
                if (!obj_1800 || !obj_1a00)
                        continue;

                co_tpdo_t **tpdos =
                                realloc(srv->tpdos, (i + 1) * sizeof(*tpdos));
                if (__unlikely(!tpdos)) {
                        set_errc(errno2c(errno));
                        goto error;
                }
                srv->tpdos = tpdos;

                for (size_t j = srv->ntpdo; j < i; j++)
                        srv->tpdos[j] = NULL;
                srv->tpdos[i] = co_tpdo_create(net, dev, i + 1);
                if (__unlikely(!srv->tpdos[i]))
                        goto error;

                srv->ntpdo = i + 1;
        }
#endif

        return;

error:
        diag(DIAG_ERROR, get_errc(), "unable to initialize PDO services");
}

static void
co_nmt_srv_fini_pdo(struct co_nmt_srv *srv)
{
        assert(srv);
        assert(srv->set & CO_NMT_SRV_PDO);

        srv->set &= ~CO_NMT_SRV_PDO;

#ifndef LELY_NO_CO_TPDO
        // Destroy the Transmit-PDOs.
        for (size_t i = 0; i < srv->ntpdo; i++)
                co_tpdo_destroy(srv->tpdos[i]);
        free(srv->tpdos);
        srv->tpdos = NULL;
        srv->ntpdo = 0;
#endif

#ifndef LELY_NO_CO_RPDO
        // Destroy the Receive-PDOs.
        for (size_t i = 0; i < srv->nrpdo; i++)
                co_rpdo_destroy(srv->rpdos[i]);
        free(srv->rpdos);
        srv->rpdos = NULL;
        srv->nrpdo = 0;
#endif
}

#ifndef LELY_NO_CO_RPDO
static void
co_nmt_srv_rpdo_err(co_rpdo_t *pdo, co_unsigned16_t eec, co_unsigned8_t er,
                void *data)
{
        (void)pdo;
        co_nmt_t *nmt = data;
        assert(nmt);

        co_nmt_on_err(nmt, eec, er, NULL);
}
#endif

#endif // !LELY_NO_CO_RPDO || !LELY_NO_CO_TPDO

static void
co_nmt_srv_init_sdo(struct co_nmt_srv *srv, can_net_t *net, co_dev_t *dev)
{
        assert(srv);
        assert(!(srv->set & CO_NMT_SRV_SDO));
        assert(!srv->ssdos);
        assert(!srv->nssdo);
        assert(!srv->csdos);
        assert(!srv->ncsdo);

        srv->set |= CO_NMT_SRV_SDO;

        // Create the Server-SDOs.
        for (co_unsigned8_t i = 0; i < CO_NUM_SDO; i++) {
                co_obj_t *obj_1200 = co_dev_find_obj(dev, 0x1200 + i);
                // The default Server-SDO does not have to exist in the object
                // dictionary.
                if (i && !obj_1200)
                        continue;

                co_ssdo_t **ssdos =
                                realloc(srv->ssdos, (i + 1) * sizeof(*ssdos));
                if (__unlikely(!ssdos)) {
                        set_errc(errno2c(errno));
                        goto error;
                }
                srv->ssdos = ssdos;

                for (size_t j = srv->nssdo; j < i; j++)
                        srv->ssdos[j] = NULL;
                srv->ssdos[i] = co_ssdo_create(net, dev, i + 1);
                if (__unlikely(!srv->ssdos[i]))
                        goto error;

                srv->nssdo = i + 1;
        }

#ifndef LELY_NO_CO_CSDO
        // Create the Client-SDOs.
        for (co_unsigned8_t i = 0; i < CO_NUM_SDO; i++) {
                co_obj_t *obj_1280 = co_dev_find_obj(dev, 0x1280 + i);
                if (!obj_1280)
                        continue;

                co_csdo_t **csdos =
                                realloc(srv->csdos, (i + 1) * sizeof(*csdos));
                if (__unlikely(!csdos)) {
                        set_errc(errno2c(errno));
                        goto error;
                }
                srv->csdos = csdos;

                for (size_t j = srv->ncsdo; j < i; j++)
                        srv->csdos[j] = NULL;
                srv->csdos[i] = co_csdo_create(net, dev, i + 1);
                if (__unlikely(!srv->csdos[i]))
                        goto error;

                srv->ncsdo = i + 1;
        }
#endif

        return;

error:
        diag(DIAG_ERROR, get_errc(), "unable to initialize SDO services");
}

static void
co_nmt_srv_fini_sdo(struct co_nmt_srv *srv)
{
        assert(srv);
        assert(srv->set & CO_NMT_SRV_SDO);

        srv->set &= ~CO_NMT_SRV_SDO;

#ifndef LELY_NO_CO_CSDO
        // Destroy the Client-SDOs.
        for (size_t i = 0; i < srv->ncsdo; i++)
                co_csdo_destroy(srv->csdos[i]);
        free(srv->csdos);
        srv->csdos = NULL;
        srv->ncsdo = 0;
#endif

        // Destroy the Server-SDOs (skipping the default one).
        for (size_t i = 0; i < srv->nssdo; i++)
                co_ssdo_destroy(srv->ssdos[i]);
        free(srv->ssdos);
        srv->ssdos = NULL;
        srv->nssdo = 0;
}

#ifndef LELY_NO_CO_SYNC

static void
co_nmt_srv_init_sync(struct co_nmt_srv *srv, can_net_t *net, co_dev_t *dev)
{
        assert(srv);
        assert(!(srv->set & CO_NMT_SRV_SYNC));
        assert(!srv->sync);

        srv->set |= CO_NMT_SRV_SYNC;

        co_obj_t *obj_1005 = co_dev_find_obj(dev, 0x1005);
        if (!obj_1005)
                return;

        srv->sync = co_sync_create(net, dev);
        if (__unlikely(!srv->sync)) {
                diag(DIAG_ERROR, get_errc(),
                                "unable to initialize SYNC service");
                return;
        }
        co_sync_set_ind(srv->sync, &co_nmt_srv_sync_ind, srv->nmt);
        co_sync_set_err(srv->sync, &co_nmt_srv_sync_err, srv->nmt);
}

static void
co_nmt_srv_fini_sync(struct co_nmt_srv *srv)
{
        assert(srv);
        assert(srv->set & CO_NMT_SRV_SYNC);

        srv->set &= ~CO_NMT_SRV_SYNC;

        co_sync_destroy(srv->sync);
        srv->sync = NULL;
}

static void
co_nmt_srv_sync_ind(co_sync_t *sync, co_unsigned8_t cnt, void *data)
{
        (void)sync;
        co_nmt_t *nmt = data;
        assert(nmt);

        co_nmt_on_sync(nmt, cnt);
}

static void
co_nmt_srv_sync_err(co_sync_t *sync, co_unsigned16_t eec, co_unsigned8_t er,
                void *data)
{
        (void)sync;
        co_nmt_t *nmt = data;
        assert(nmt);

        co_nmt_on_err(nmt, eec, er, NULL);
}

#endif // !LELY_NO_CO_SYNC

#ifndef LELY_NO_CO_TIME

static void
co_nmt_srv_init_time(struct co_nmt_srv *srv, can_net_t *net, co_dev_t *dev)
{
        assert(srv);
        assert(!(srv->set & CO_NMT_SRV_TIME));
        assert(!srv->time);

        srv->set |= CO_NMT_SRV_TIME;

        co_obj_t *obj_1012 = co_dev_find_obj(dev, 0x1012);
        if (!obj_1012)
                return;

        srv->time = co_time_create(net, dev);
        if (__unlikely(!srv->time))
                diag(DIAG_ERROR, get_errc(),
                                "unable to initialize TIME service");
}

static void
co_nmt_srv_fini_time(struct co_nmt_srv *srv)
{
        assert(srv);
        assert(srv->set & CO_NMT_SRV_TIME);

        srv->set &= ~CO_NMT_SRV_TIME;

        co_time_destroy(srv->time);
        srv->time = NULL;
}

#endif

#ifndef LELY_NO_CO_EMCY

static void
co_nmt_srv_init_emcy(struct co_nmt_srv *srv, can_net_t *net, co_dev_t *dev)
{
        assert(srv);
        assert(!(srv->set & CO_NMT_SRV_EMCY));
        assert(!srv->emcy);

        srv->set |= CO_NMT_SRV_EMCY;

        co_obj_t *obj_1001 = co_dev_find_obj(dev, 0x1001);
        if (__unlikely(!obj_1001))
                return;

        srv->emcy = co_emcy_create(net, dev);
        if (__unlikely(!srv->emcy))
                diag(DIAG_ERROR, get_errc(),
                                "unable to initialize EMCY service");
}

static void
co_nmt_srv_fini_emcy(struct co_nmt_srv *srv)
{
        assert(srv);
        assert(srv->set & CO_NMT_SRV_EMCY);

        srv->set &= ~CO_NMT_SRV_EMCY;

        co_emcy_destroy(srv->emcy);
        srv->emcy = NULL;
}

#endif

#ifndef LELY_NO_CO_LSS

static void
co_nmt_srv_init_lss(struct co_nmt_srv *srv, co_nmt_t *nmt)
{
        assert(srv);
        assert(!(srv->set & CO_NMT_SRV_LSS));
        assert(!srv->lss);

        if (!co_dev_get_lss(co_nmt_get_dev(nmt)))
                return;

        srv->set |= CO_NMT_SRV_LSS;

        srv->lss = co_lss_create(nmt);
        if (__unlikely(!srv->lss))
                diag(DIAG_ERROR, get_errc(),
                                "unable to initialize LSS service");
}

static void
co_nmt_srv_fini_lss(struct co_nmt_srv *srv)
{
        assert(srv);
        assert(srv->set & CO_NMT_SRV_LSS);

        srv->set &= ~CO_NMT_SRV_LSS;

        co_lss_destroy(srv->lss);
        srv->lss = NULL;
}

#endif