ESI.cpp

Go to the documentation of this file.

#include "ESI.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include "../ErrorReporting.h"
 
extern "C"
{
#include <ethercat.h>
}
 
namespace armarx::control::ethercat
{
 
    std::optional<std::vector<std::byte>>
    ESIHandler::readESIBinaryBlob(std::uint16_t slaveIndex,
                                  std::uint16_t startAddress,
                                  std::uint16_t endAddress) const
    {
        bool pdiHadEEPROMControl = static_cast<bool>(ec_slave[slaveIndex].eep_pdi);
        if (ec_eeprom2master(slaveIndex) == 0)
        {
            GENERAL_ERROR(
                "Failed to read ESI of slave at index %u - could not get control of the EEPROM.",
                slaveIndex);
            return std::nullopt;
        }
 
        std::vector<std::byte> result = readFromEEPROM(
            slaveIndex, startAddress, endAddress, ec_slave[slaveIndex].eep_8byte > 0); // NOLINT
 
        if (pdiHadEEPROMControl)
        {
            ec_eeprom2pdi(slaveIndex);
        }
 
        return std::move(result);
    }
 
    std::vector<std::byte>
    ESIHandler::readFromEEPROM(std::uint16_t slaveIndex,
                               std::uint16_t startAddress,
                               std::uint16_t endAddress,
                               bool has64BitPackets) const
    {
        ARMARX_CHECK_GREATER_EQUAL(endAddress, startAddress);
 
        std::vector<std::byte> result;
 
        // Some slaves return data from the EEPROM in 4 byte (2 word) units,
        // others in 8 byte (4 word) units.
        std::uint16_t addressIncr = 2;
        if (has64BitPackets)
        {
            addressIncr = 4;
        }
 
        result.reserve((endAddress - startAddress) * addressIncr * 2);
 
        std::uint16_t slaveConfiguredAddress = ec_slave[slaveIndex].configadr;
 
        for (std::uint16_t currentAddress = startAddress; currentAddress < endAddress;
             currentAddress += addressIncr)
        {
            std::uint64_t eepromData =
                ec_readeepromFP(slaveConfiguredAddress, currentAddress, EC_TIMEOUTEEP);
 
            // Repack the result into some std::bytes
            for (int i = 0; i < addressIncr * 2; ++i)
            {
                static const int byteLength = 8;
                static const int byteMask = 0xFF;
                result.push_back(std::byte((eepromData >> (i * byteLength)) & byteMask));
            }
        }
 
        return result;
    }
 
 
    std::uint8_t
    getU8(const std::vector<std::byte>& esiBinary, std::uint16_t address)
    {
        if (esiBinary.size() < static_cast<std::size_t>(address + 1))
            ARMARX_FATAL << "The ESI binary ended unexpectedly!";
        return static_cast<std::uint8_t>(esiBinary.at(address));
    }
 
    std::uint16_t
    getU16(const std::vector<std::byte>& esiBinary, std::uint16_t address)
    {
        if (esiBinary.size() < static_cast<std::size_t>(address + 2))
            ARMARX_FATAL << "The ESI binary ended unexpectedly!";
        const std::uint16_t byteSize = 8;
        return static_cast<std::uint16_t>(esiBinary.at(address)) &
               (static_cast<std::uint16_t>(esiBinary.at(address + 1)) << byteSize);
    }
 
    std::int16_t
    get16(const std::vector<std::byte>& esiBinary, std::uint16_t address)
    {
        auto unsignedValue = getU16(esiBinary, address);
        return *reinterpret_cast<std::int16_t*>(&unsignedValue);
    }
 
    std::uint16_t
    getU16W(const std::vector<std::byte>& esiBinary, std::uint16_t wordAddress)
    {
        return getU16(esiBinary, wordAddress * 2);
    }
 
    std::uint32_t
    getU32(const std::vector<std::byte>& esiBinary, std::uint16_t address)
    {
        if (esiBinary.size() < static_cast<std::size_t>(address + 4))
            ARMARX_FATAL << "The ESI binary ended unexpectedly!";
        const auto byteSize = 8;
        return static_cast<std::uint32_t>(esiBinary.at(address)) +
               (static_cast<std::uint32_t>(esiBinary.at(address + 1)) << byteSize) +
               (static_cast<std::uint32_t>(esiBinary.at(address + 2)) << 2 * byteSize) +
               (static_cast<std::uint32_t>(esiBinary.at(address + 3)) << 3 * byteSize);
    }
 
    std::uint32_t
    getU32W(const std::vector<std::byte>& esiBinary, std::uint16_t wordAddress)
    {
        return getU32(esiBinary, wordAddress * 2);
    }
 
    ESIData
    ESIParser::parseESI(const std::vector<std::byte>& esiBinary)
    {
        ESIData esiData{};
        esiData.header = parseHeader(esiBinary);
        std::uint16_t categoryOffset = 0x0040;
        auto categoryType = getU16W(esiBinary, categoryOffset);
        // Attention - the MSB is undefined! -> setting to zero.
        categoryType &= (1 << 15) - 1;
        auto categorySize = getU16W(esiBinary, categoryOffset + 1);
        while (categoryType != 0x7fff)
        {
            switch (categoryType)
            {
                case 0:
                    // NOP
                    break;
                // 1 - 9 are device specific categories -> ignored
                case 10:
                    // STRINGS
                    esiData.strings = parseStrings(esiBinary, categoryOffset + 2);
                    break;
                case 20:
                    // DataTypes
                    // "for future use" -> ignored
                    break;
                case 30:
                    // General
                    esiData.general = parseGeneral(esiBinary, categoryOffset + 2);
                    break;
                case 40:
                    // FMMU
                    esiData.fmmu = parseFMMU(esiBinary, categoryOffset + 2, categorySize);
                    break;
                case 41:
                    // SyncM
                    esiData.syncM = parseSyncM(esiBinary, categoryOffset + 2, categorySize);
                    break;
                case 50:
                    // TxPDO
                    esiData.txPDO = parsePDOs(esiBinary, categoryOffset + 2, categorySize);
                    break;
                case 51:
                    // RxPDO
                    esiData.rxPDO = parsePDOs(esiBinary, categoryOffset + 2, categorySize);
                    break;
                    // 60+ are reserved or vendor specific -> ignored
            }
            categoryOffset += 2 + categorySize;
            categoryType = getU16W(esiBinary, categoryOffset);
            // Attention - the MSB is undefined! -> setting to zero.
            categoryType &= (1 << 15) - 1;
            categorySize = getU16W(esiBinary, categoryOffset + 1);
        }
        return esiData;
    }
 
    ESIHeader
    ESIParser::parseHeader(const std::vector<std::byte>& esiBinary)
    {
        ESIHeader esiHeader;
        esiHeader.pdiControl = getU16W(esiBinary, 0x0000);
        esiHeader.pdiConfiguration = getU16W(esiBinary, 0x0001);
        esiHeader.syncImpulseLen = getU16W(esiBinary, 0x0002);
        esiHeader.pdiConfiguration2 = getU16W(esiBinary, 0x0003);
        esiHeader.stationAlias = getU16W(esiBinary, 0x0004);
        esiHeader.checkSum = getU16W(esiBinary, 0x0007);
        esiHeader.vendorID = getU32W(esiBinary, 0x0008);
        esiHeader.productCode = getU32W(esiBinary, 0x000A);
        esiHeader.revisionNumber = getU32W(esiBinary, 0x000C);
        esiHeader.serialNumber = getU32W(esiBinary, 0x000E);
        esiHeader.bootstrapReceiveMailboxOffset = getU16W(esiBinary, 0x0014);
        esiHeader.bootstrapReceiveMailboxSize = getU16W(esiBinary, 0x0015);
        esiHeader.bootstrapSendMailboxOffset = getU16W(esiBinary, 0x0016);
        esiHeader.bootstrapSendMailboxSize = getU16W(esiBinary, 0x0017);
        esiHeader.standardReceiveMailboxOffset = getU16W(esiBinary, 0x0018);
        esiHeader.standardReceiveMailboxSize = getU16W(esiBinary, 0x0019);
        esiHeader.standardSendMailboxOffset = getU16W(esiBinary, 0x001A);
        esiHeader.standardSendMailboxSize = getU16W(esiBinary, 0x001B);
        esiHeader.mailboxProtocol = getU16W(esiBinary, 0x001C);
        esiHeader.eepromSize = getU16W(esiBinary, 0x003E);
        esiHeader.version = getU16W(esiBinary, 0x003F);
        return esiHeader;
    }
 
    std::vector<std::string>
    ESIParser::parseStrings(const std::vector<std::byte>& esiBinary, std::uint16_t wordOffset)
    {
        std::vector<std::string> strings;
        auto nStrings = getU8(esiBinary, 2 * wordOffset);
        strings.reserve(nStrings);
        std::uint16_t currentOffset = 2 * wordOffset + 1;
        for (auto i = 0; i < nStrings; ++i)
        {
            std::uint8_t len;
            std::string tmp;
            len = static_cast<std::uint8_t>(esiBinary.at(currentOffset));
            ++currentOffset;
            if (currentOffset + len > esiBinary.size())
                ARMARX_FATAL << "The ESI binary ended unexpectedly while reading a string!";
            tmp.assign(reinterpret_cast<const char*>(&esiBinary[currentOffset]), len);
            strings.emplace_back(tmp);
            currentOffset += len;
        }
        return strings;
    }
 
    ESIGeneral
    ESIParser::parseGeneral(const std::vector<std::byte>& esiBinary, std::uint16_t wordOffset)
    {
        ESIGeneral esiGeneral;
        std::uint16_t offset = 2 * wordOffset;
        esiGeneral.groupIdx = getU8(esiBinary, offset + 0x0000);
        esiGeneral.imgIdx = getU8(esiBinary, offset + 0x0001);
        esiGeneral.orderIdx = getU8(esiBinary, offset + 0x0002);
        esiGeneral.nameIdx = getU8(esiBinary, offset + 0x0003);
        esiGeneral.coEDetails = getU8(esiBinary, offset + 0x0005);
        esiGeneral.foEDetails = getU8(esiBinary, offset + 0x0006);
        esiGeneral.eoEDetails = getU8(esiBinary, offset + 0x0007);
        esiGeneral.soEChannels = getU8(esiBinary, offset + 0x0008);
        esiGeneral.dS402Channels = getU8(esiBinary, offset + 0x0009);
        esiGeneral.sysmanClass = getU8(esiBinary, offset + 0x000a);
        esiGeneral.flags = getU8(esiBinary, offset + 0x000b);
        esiGeneral.currentOnEBus = get16(esiBinary, offset + 0x000c);
        esiGeneral.physicalPort = getU16(esiBinary, offset + 0x0010);
        esiGeneral.physicalMemoryAddress = getU16(esiBinary, offset + 0x0012);
        return esiGeneral;
    }
 
    ESIFMMU
    ESIParser::parseFMMU(const std::vector<std::byte>& esiBinary,
                         std::uint16_t wordOffset,
                         std::uint16_t len)
    {
        std::uint16_t count = 2 * len;
        ESIFMMU esiFmmu;
        esiFmmu.reserve(count);
        for (std::uint16_t i = 0; i < count; ++i)
        {
            esiFmmu.emplace_back(getU8(esiBinary, 2 * wordOffset + i));
        }
        return esiFmmu;
    }
 
    ESISyncM
    ESIParser::parseSyncM(const std::vector<std::byte>& esiBinary,
                          std::uint16_t wordOffset,
                          std::uint16_t len)
    {
        const std::uint16_t syncMElementLen = 8;
        const std::uint16_t count = 2 * len / syncMElementLen;
        ESISyncM esiSyncM;
        esiSyncM.reserve(count);
        std::uint16_t currentOffset = 2 * wordOffset;
        for (size_t i = 0; i < count; ++i)
        {
            ESISyncMElement elem;
            elem.physicalStartAddress = getU16(esiBinary, currentOffset + 0x0000);
            elem.length = getU16(esiBinary, currentOffset + 0x0002);
            elem.controlRegister = getU8(esiBinary, currentOffset + 0x0004);
            elem.statusRegister = getU8(esiBinary, currentOffset + 0x0005);
            elem.enableSynchManager = getU8(esiBinary, currentOffset + 0x0006);
            elem.syncManagerType = getU8(esiBinary, currentOffset + 0x0007);
            esiSyncM.emplace_back(elem);
            currentOffset += syncMElementLen;
        }
        return esiSyncM;
    }
 
    std::vector<ESIPDOObject>
    ESIParser::parsePDOs(const std::vector<std::byte>& esiBinary,
                         std::uint16_t wordOffset,
                         std::uint16_t len)
    {
        std::vector<ESIPDOObject> esiPDOs;
        std::uint16_t currentOffset = 2 * wordOffset;
        while (currentOffset < 2 * (wordOffset + len))
        {
            ESIPDOObject pdoObject;
            pdoObject.pdoIndex = getU16(esiBinary, currentOffset + 0x0000);
            pdoObject.entryCount = getU8(esiBinary, currentOffset + 0x0002);
            pdoObject.syncManager = getU8(esiBinary, currentOffset + 0x0003);
            pdoObject.synchronization = getU8(esiBinary, currentOffset + 0x0004);
            pdoObject.nameIdx = getU8(esiBinary, currentOffset + 0x0005);
            pdoObject.flags = getU16(esiBinary, currentOffset + 0x0006);
            pdoObject.entries = std::vector<ESIPDOEntry>{pdoObject.entryCount};
            currentOffset += 8;
            for (int i = 0; i < pdoObject.entryCount; ++i)
            {
                ESIPDOEntry pdoEntry;
                pdoEntry.index = getU16(esiBinary, currentOffset + 0x0000);
                pdoEntry.subIndex = getU8(esiBinary, currentOffset + 0x0002);
                pdoEntry.nameIdx = getU8(esiBinary, currentOffset + 0x0003);
                pdoEntry.dataType = getU8(esiBinary, currentOffset + 0x0004);
                pdoEntry.bitLength = getU8(esiBinary, currentOffset + 0x0005);
                pdoEntry.flags = getU16(esiBinary, currentOffset + 0x0006);
                pdoObject.entries.emplace_back(pdoEntry);
                currentOffset += 8;
            }
            esiPDOs.emplace_back(pdoObject);
        }
        return esiPDOs;
    }
} // namespace armarx::control::ethercat