powerlink.cpp

Go to the documentation of this file.

// Copyright (C) 2026 Douglas Quigg (dstroy0) <dquigg123@gmail.com>
// SPDX-License-Identifier: AGPL-3.0-or-later
 
/**
 * @file powerlink.cpp
 * @brief Ethernet POWERLINK basic frame codec (see powerlink.h).
 */
 
#include "services/powerlink/powerlink.h"
 
#if DETWS_ENABLE_POWERLINK
 
#include <string.h>
 
size_t detws_epl_build(uint8_t msg_type, uint8_t dest, uint8_t source, const uint8_t *payload, size_t payload_len,
                       uint8_t *out, size_t cap)
{
    if (!out || (payload_len && !payload))
        return 0;
    size_t n = 3 + payload_len;
    if (n > cap)
        return 0;
    out[0] = msg_type;
    out[1] = dest;
    out[2] = source;
    if (payload_len)
        memcpy(out + 3, payload, payload_len);
    return n;
}
 
size_t detws_epl_soc(uint8_t source, uint8_t *out, size_t cap)
{
    return detws_epl_build(Epl::EPL_MSG_SOC, Epl::EPL_NODE_BROADCAST, source, nullptr, 0, out, cap);
}
 
size_t detws_epl_preq(uint8_t dest_cn, uint8_t source, const uint8_t *pdo, size_t pdo_len, uint8_t *out, size_t cap)
{
    return detws_epl_build(Epl::EPL_MSG_PREQ, dest_cn, source, pdo, pdo_len, out, cap);
}
 
size_t detws_epl_pres(uint8_t source_cn, const uint8_t *pdo, size_t pdo_len, uint8_t *out, size_t cap)
{
    return detws_epl_build(Epl::EPL_MSG_PRES, Epl::EPL_NODE_BROADCAST, source_cn, pdo, pdo_len, out, cap);
}
 
bool detws_epl_parse(const uint8_t *frame, size_t len, EplFrame *out)
{
    if (!frame || !out || len < 3)
        return false;
    uint8_t mt = frame[0];
    if (mt != Epl::EPL_MSG_SOC && mt != Epl::EPL_MSG_PREQ && mt != Epl::EPL_MSG_PRES && mt != Epl::EPL_MSG_SOA &&
        mt != Epl::EPL_MSG_ASND)
        return false;
    out->msg_type = mt;
    out->dest = frame[1];
    out->source = frame[2];
    out->payload = (len > 3) ? (frame + 3) : nullptr;
    out->payload_len = len - 3;
    return true;
}
 
#endif // DETWS_ENABLE_POWERLINK