map/das-dn/third_party/AdsLib/Standalone/AdsLib.cpp

// SPDX-License-Identifier: MIT
/**
   Copyright (c) 2015 - 2022 Beckhoff Automation GmbH & Co. KG
 */

#include "AdsLib.h"
#include "AmsRouter.h"
#include <iostream>

namespace Beckhoff
{
namespace Ads
{
static AmsRouter& GetLocalAmsRouter()
{
    static AmsRouter router;
    return router;
}

#define ASSERT_PORT(port) do { \
        if ((port) <= 0 || (port) > UINT16_MAX) { \
            return ADSERR_CLIENT_PORTNOTOPEN; \
        } \
} while (false)

#define ASSERT_PORT_AND_AMSADDR(port, pAddr) do { \
        ASSERT_PORT(port); \
        if (!(pAddr)) { \
            return ADSERR_CLIENT_NOAMSADDR; \
        } \
} while (false)

long ConnectTarget(const AmsNetId ams, const char* ip)
{
    try {
        return GetLocalAmsRouter().ConnectTarget(ams, ip);
    } catch (const std::bad_alloc&) {
        return GLOBALERR_NO_MEMORY;
    } catch (const std::runtime_error&) {
        return GLOBALERR_TARGET_PORT;
    }
}

void DisconnectTarget(const AmsNetId ams)
{
    GetLocalAmsRouter().DisconnectTarget(ams);
}

void SetLocalAmsNetId(const AmsNetId ams)
{
    GetLocalAmsRouter().SetAmsNetId(ams);
}

long CloseLocalPort(long localPort)
{
    ASSERT_PORT(localPort);
    return GetLocalAmsRouter().ClosePort((uint16_t)localPort);
}

long OpenLocalPort()
{
    return GetLocalAmsRouter().OpenPort();
}

long GetLocalAmsAddr(long localPort, AmsAddr* pAddr)
{
    ASSERT_PORT_AND_AMSADDR(localPort, pAddr);
    return GetLocalAmsRouter().GetAmsAddr((uint16_t)localPort, pAddr);
}

long AdsSyncReadReqEx2(long           localPort,
                       const AmsAddr* pAddr,
                       uint32_t       indexGroup,
                       uint32_t       indexOffset,
                       uint32_t       bufferLength,
                       void*          buffer,
                       uint32_t*      bytesRead)
{
    ASSERT_PORT_AND_AMSADDR(localPort, pAddr);
    if (!buffer) {
        return ADSERR_CLIENT_INVALIDPARM;
    }

    try {
        AmsRequest request {
            *pAddr,
            (uint16_t)localPort,
            AoEHeader::READ,
            bufferLength,
            buffer,
            bytesRead,
            sizeof(AoERequestHeader)
        };
        request.frame.prepend(AoERequestHeader {
            indexGroup,
            indexOffset,
            bufferLength
        });
        return GetLocalAmsRouter().SendAdsRequest(request);
    } catch (const std::bad_alloc&) {
        return GLOBALERR_NO_MEMORY;
    }
}

long AdsSyncReadDeviceInfoReqEx(long localPort, const AmsAddr* pAddr, char* devName, AdsVersion* version)
{
    ASSERT_PORT_AND_AMSADDR(localPort, pAddr);
    if (!devName || !version) {
        return ADSERR_CLIENT_INVALIDPARM;
    }

    try {
        static const size_t NAME_LENGTH = 16;
        uint8_t buffer[sizeof(*version) + NAME_LENGTH];
        AmsRequest request {
            *pAddr,
            (uint16_t)localPort,
            AoEHeader::READ_DEVICE_INFO,
            sizeof(buffer),
            buffer
        };
        const auto status = GetLocalAmsRouter().SendAdsRequest(request);
        if (!status) {
            version->version = buffer[0];
            version->revision = buffer[1];
            version->build = Beckhoff::letoh<uint16_t>(buffer + offsetof(AdsVersion, build));
            memcpy(devName, buffer + sizeof(*version), NAME_LENGTH);
        }
        return status;
    } catch (const std::bad_alloc&) {
        return GLOBALERR_NO_MEMORY;
    }
}

long AdsSyncReadStateReqEx(long localPort, const AmsAddr* pAddr, uint16_t* adsState, uint16_t* devState)
{
    ASSERT_PORT_AND_AMSADDR(localPort, pAddr);
    if (!adsState || !devState) {
        return ADSERR_CLIENT_INVALIDPARM;
    }

    try {
        uint16_t buffer[2];
        AmsRequest request {
            *pAddr,
            (uint16_t)localPort,
            AoEHeader::READ_STATE,
            sizeof(buffer),
            buffer
        };
        const auto status = GetLocalAmsRouter().SendAdsRequest(request);
        if (!status) {
            *adsState = Beckhoff::letoh(buffer[0]);
            *devState = Beckhoff::letoh(buffer[1]);
        }
        return status;
    } catch (const std::bad_alloc&) {
        return GLOBALERR_NO_MEMORY;
    }
}

long AdsSyncReadWriteReqEx2(long           localPort,
                            const AmsAddr* pAddr,
                            uint32_t       indexGroup,
                            uint32_t       indexOffset,
                            uint32_t       readLength,
                            void*          readData,
                            uint32_t       writeLength,
                            const void*    writeData,
                            uint32_t*      bytesRead)
{
    ASSERT_PORT_AND_AMSADDR(localPort, pAddr);
    if ((readLength && !readData) || (writeLength && !writeData)) {
        return ADSERR_CLIENT_INVALIDPARM;
    }

    try {
        AmsRequest request {
            *pAddr,
            (uint16_t)localPort,
            AoEHeader::READ_WRITE,
            readLength,
            readData,
            bytesRead,
            sizeof(AoEReadWriteReqHeader) + writeLength
        };
        request.frame.prepend(writeData, writeLength);
        request.frame.prepend(AoEReadWriteReqHeader {
            indexGroup,
            indexOffset,
            readLength,
            writeLength
        });
        return GetLocalAmsRouter().SendAdsRequest(request);
    } catch (const std::bad_alloc&) {
        return GLOBALERR_NO_MEMORY;
    }
}

long AdsSyncWriteReqEx(long           localPort,
                       const AmsAddr* pAddr,
                       uint32_t       indexGroup,
                       uint32_t       indexOffset,
                       uint32_t       bufferLength,
                       const void*    buffer)
{
    ASSERT_PORT_AND_AMSADDR(localPort, pAddr);
    if (!buffer) {
        return ADSERR_CLIENT_INVALIDPARM;
    }

    try {
        AmsRequest request {
            *pAddr,
            (uint16_t)localPort,
            AoEHeader::WRITE,
            0, nullptr, nullptr,
            sizeof(AoERequestHeader) + bufferLength,
        };
        request.frame.prepend(buffer, bufferLength);
        request.frame.prepend<AoERequestHeader>({
            indexGroup,
            indexOffset,
            bufferLength
        });
        return GetLocalAmsRouter().SendAdsRequest(request);
    } catch (const std::bad_alloc&) {
        return GLOBALERR_NO_MEMORY;
    }
}

long AdsSyncWriteControlReqEx(long           localPort,
                              const AmsAddr* pAddr,
                              uint16_t       adsState,
                              uint16_t       devState,
                              uint32_t       bufferLength,
                              const void*    buffer)
{
    ASSERT_PORT_AND_AMSADDR(localPort, pAddr);
    try {
        AmsRequest request {
            *pAddr,
            (uint16_t)localPort,
            AoEHeader::WRITE_CONTROL,
            0, nullptr, nullptr,
            sizeof(AdsWriteCtrlRequest) + bufferLength
        };
        request.frame.prepend(buffer, bufferLength);
        request.frame.prepend<AdsWriteCtrlRequest>({
            adsState,
            devState,
            bufferLength
        });
        return GetLocalAmsRouter().SendAdsRequest(request);
    } catch (const std::bad_alloc&) {
        return GLOBALERR_NO_MEMORY;
    }
}

long AdsSyncAddDeviceNotificationReqEx(long                         localPort,
                                       const AmsAddr*               pAddr,
                                       uint32_t                     indexGroup,
                                       uint32_t                     indexOffset,
                                       const AdsNotificationAttrib* pAttrib,
                                       PAdsNotificationFuncEx       pFunc,
                                       uint32_t                     hUser,
                                       uint32_t*                    pNotification)
{
    ASSERT_PORT_AND_AMSADDR(localPort, pAddr);
    if (!pAttrib || !pFunc || !pNotification) {
        return ADSERR_CLIENT_INVALIDPARM;
    }

    try {
        uint8_t buffer[sizeof(*pNotification)];
        AmsRequest request {
            *pAddr,
            (uint16_t)localPort,
            AoEHeader::ADD_DEVICE_NOTIFICATION,
            sizeof(buffer),
            buffer,
            nullptr,
            sizeof(AdsAddDeviceNotificationRequest)
        };
        request.frame.prepend(AdsAddDeviceNotificationRequest {
            indexGroup,
            indexOffset,
            pAttrib->cbLength,
            pAttrib->nTransMode,
            pAttrib->nMaxDelay,
            pAttrib->nCycleTime
        });
        auto notify = std::make_shared<Notification>(pFunc, hUser, pAttrib->cbLength, *pAddr, (uint16_t)localPort);
        return GetLocalAmsRouter().AddNotification(
            request,
            pNotification,
            notify);
    } catch (const std::bad_alloc&) {
        return GLOBALERR_NO_MEMORY;
    }
}

long AdsSyncDelDeviceNotificationReqEx(long localPort, const AmsAddr* pAddr, uint32_t hNotification)
{
    ASSERT_PORT_AND_AMSADDR(localPort, pAddr);
    return GetLocalAmsRouter().DelNotification((uint16_t)localPort, pAddr, hNotification);
}

long AdsSyncGetTimeoutEx(long localPort, uint32_t* timeout)
{
    ASSERT_PORT(localPort);
    if (!timeout) {
        return ADSERR_CLIENT_INVALIDPARM;
    }
    return GetLocalAmsRouter().GetTimeout((uint16_t)localPort, *timeout);
}

long AdsSyncSetTimeoutEx(long localPort, uint32_t timeout)
{
    ASSERT_PORT(localPort);
    return GetLocalAmsRouter().SetTimeout((uint16_t)localPort, timeout);
}
}
}