MARTe-Integrated-Components/Source/Components/Interfaces/TCPStreamService/TCPSServiceStream.cpp

/**
 * @file TCPSServiceStream.cpp
 * @brief Implementation of TCPSServiceStream.
 */

#define DLL_API

#include "TCPSServiceStream.h"

#include "AdvancedErrorManagement.h"
#include "ObjectRegistryDatabase.h"
#include "TCPSConn.h"
#include "TCPSWorker.h"

#include <string.h>

namespace MARTe {

const uint32 TCPSServiceStream::TCPSS_DEFAULT_MAX_PAYLOAD;

static const uint32 TCPSS_MAX_FRAME_PAYLOAD = 1048576u;

TCPSServiceStream::TCPSServiceStream(const char8 *serviceObjectName, const char8 *name)
    : serviceName(serviceObjectName), streamName(name), registered(false),
      maxPayloadSize(TCPSS_DEFAULT_MAX_PAYLOAD), subs(NULL_PTR(TCPSConn **)), subIdx(NULL_PTR(uint16 *)),
      capSubs(0u), numSubs(0u), cachedConfigFrame(NULL_PTR(uint8 *)),
      cachedConfigBytes(0u), cachedConfigCap(0u), configCounter(0u), drops(0u),
      numTaps(0u), dataFrame(NULL_PTR(uint8 *)), dataFrameCap(0u) {
    for (uint32 i = 0u; i < TCPSS_MAX_TAPS; i++) {
        taps[i] = NULL_PTR(TCPSStreamTapI *);
    }
}

TCPSServiceStream::~TCPSServiceStream() {
    (void)Stop();
    if (cachedConfigFrame != NULL_PTR(uint8 *)) {
        delete[] cachedConfigFrame;
    }
    if (dataFrame != NULL_PTR(uint8 *)) {
        delete[] dataFrame;
    }
    if (subs != NULL_PTR(TCPSConn **)) {
        delete[] subs;
    }
    if (subIdx != NULL_PTR(uint16 *)) {
        delete[] subIdx;
    }
}

bool TCPSServiceStream::Initialise(StructuredDataI &data) {
    bool ok = true;
    maxPayloadSize = TCPSS_DEFAULT_MAX_PAYLOAD;
    (void)data.Read("MaxPayloadSize", maxPayloadSize);
    if ((maxPayloadSize == 0u) || (maxPayloadSize > TCPSS_MAX_FRAME_PAYLOAD)) {
        REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
                            "TCPSServiceStream: MaxPayloadSize %u must be in 1..%u.",
                            maxPayloadSize, TCPSS_MAX_FRAME_PAYLOAD);
        ok = false;
    }
    if ((streamName.Size() > 0u) && (strchr(streamName.Buffer(), ',') != NULL_PTR(char8 *))) {
        REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
                            "TCPSServiceStream: StreamName '%s' may not contain a comma.",
                            streamName.Buffer());
        ok = false;
    }
    if ((streamName.Size() == 0u) || (streamName.Size() > TCPS_MAX_STREAM_NAME)) {
        REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
                            "TCPSServiceStream: StreamName must be 1..%u characters.",
                            TCPS_MAX_STREAM_NAME);
        ok = false;
    }
    return ok;
}

bool TCPSServiceStream::Start() {
    if (registered) {
        return true;
    }
    if (!service.IsValid()) {
        ReferenceT<TCPStreamService> found =
            ObjectRegistryDatabase::Instance()->Find(serviceName.Buffer());
        if (!found.IsValid()) {
            REPORT_ERROR_STATIC(ErrorManagement::InitialisationError,
                                "TCPSServiceStream: no TCPStreamService named '%s'.",
                                serviceName.Buffer());
            return false;
        }
        service = found;
    }
    mutex.FastLock();
    if (subs == NULL_PTR(TCPSConn **)) {
        capSubs = service->MaxClients();
        subs = new TCPSConn *[capSubs];
        subIdx = new uint16[capSubs];
    }
    numSubs = 0u;
    mutex.FastUnLock();
    registered = service->RegisterStream(this);
    return registered;
}

bool TCPSServiceStream::Stop() {
    if (registered) {
        service->UnregisterStream(this); // drops every subscriber
        registered = false;
    }
    mutex.FastLock();
    if (cachedConfigFrame != NULL_PTR(uint8 *)) {
        cachedConfigBytes = 0u;
    }
    mutex.FastUnLock();
    return true;
}

void TCPSServiceStream::ServiceClients() {
}

bool TCPSServiceStream::BuildFrame(uint8 *&buf, uint32 &cap, uint8 type, uint32 counter,
                                   const uint8 *payload, uint32 payloadSize,
                                   uint32 &frameBytes) {
    bool ok = (payloadSize <= TCPSS_MAX_FRAME_PAYLOAD);
    if (ok) {
        const uint32 need = UDPS_HEADER_SIZE + payloadSize;
        if (cap < need) {
            if (buf != NULL_PTR(uint8 *)) {
                delete[] buf;
            }
            buf = new uint8[need];
            cap = need;
        }
        UDPSBuildHeader(buf, type, counter, 0u, 1u, payloadSize);
        if (payloadSize > 0u) {
            memcpy(&buf[UDPS_HEADER_SIZE], payload, payloadSize);
        }
        frameBytes = need;
    }
    return ok;
}

/** Caller holds `mutex`. */
void TCPSServiceStream::Distribute(bool isConfig, const uint8 *frame, uint32 frameBytes) {
    for (uint32 i = 0u; i < numSubs; i++) {
        TCPSConn *c = subs[i];
        uint32 dropped = 0u;
        c->qMutex.FastLock();
        const bool wasEmpty = c->q.Empty();
        (void)c->q.Push(subIdx[i], isConfig, frame, frameBytes, dropped);
        c->qMutex.FastUnLock();
        drops += dropped;
        if (wasEmpty) {
            c->worker->Wake();
        }
    }
}

bool TCPSServiceStream::SendConfig(const uint8 *payload, uint32 payloadSize) {
    if (!registered) {
        return false;
    }
    if (payloadSize > TCPSS_MAX_FRAME_PAYLOAD) {
        REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
                            "TCPSServiceStream: CONFIG of %u bytes exceeds the frame limit.",
                            payloadSize);
        return false;
    }
    mutex.FastLock();
    configCounter++;
    bool ok = BuildFrame(cachedConfigFrame, cachedConfigCap, UDPS_TYPE_CONFIG, configCounter,
                         payload, payloadSize, cachedConfigBytes);
    if (ok) {
        Distribute(true, cachedConfigFrame, cachedConfigBytes);
    }
    mutex.FastUnLock();
    if (ok && (numTaps > 0u)) {
        tapMutex.FastLock();
        for (uint32 i = 0u; i < numTaps; i++) {
            taps[i]->OnTapConfig(this, payload, payloadSize);
        }
        tapMutex.FastUnLock();
    }
    return ok;
}

bool TCPSServiceStream::SendData(uint32 counter, const uint8 *payload, uint32 payloadSize) {
    if (!registered) {
        return false;
    }
    if (numTaps > 0u) {
        tapMutex.FastLock();
        for (uint32 i = 0u; i < numTaps; i++) {
            taps[i]->OnTapData(this, counter, payload, payloadSize);
        }
        tapMutex.FastUnLock();
    }
    if (numSubs == 0u) {
        return true;
    }
    uint32 frameBytes = 0u;
    if (!BuildFrame(dataFrame, dataFrameCap, UDPS_TYPE_DATA, counter, payload, payloadSize,
                    frameBytes)) {
        REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
                            "TCPSServiceStream: DATA of %u bytes exceeds the frame limit.",
                            payloadSize);
        return false;
    }
    mutex.FastLock();
    Distribute(false, dataFrame, frameBytes);
    mutex.FastUnLock();
    return true;
}

bool TCPSServiceStream::AddTap(TCPSStreamTapI *tap) {
    bool ok = false;
    uint8 *cfg = NULL_PTR(uint8 *);
    uint32 cfgBytes = 0u;
    mutex.FastLock();
    if (cachedConfigBytes > UDPS_HEADER_SIZE) {
        cfgBytes = cachedConfigBytes - UDPS_HEADER_SIZE;
        cfg = new uint8[cfgBytes];
        memcpy(cfg, &cachedConfigFrame[UDPS_HEADER_SIZE], cfgBytes);
    }
    mutex.FastUnLock();
    tapMutex.FastLock();
    if (numTaps < TCPSS_MAX_TAPS) {
        taps[numTaps] = tap;
        numTaps = numTaps + 1u;
        ok = true;
        if (cfg != NULL_PTR(uint8 *)) {
            tap->OnTapConfig(this, cfg, cfgBytes);
        }
    }
    tapMutex.FastUnLock();
    if (cfg != NULL_PTR(uint8 *)) {
        delete[] cfg;
    }
    return ok;
}

void TCPSServiceStream::RemoveTap(TCPSStreamTapI *tap) {
    tapMutex.FastLock();
    for (uint32 i = 0u; i < numTaps; i++) {
        if (taps[i] == tap) {
            taps[i] = taps[numTaps - 1u];
            numTaps = numTaps - 1u;
            break;
        }
    }
    tapMutex.FastUnLock();
}

bool TCPSServiceStream::HasClients() const {
    /* A tap (TCPStreamJoin) consumes frames like a subscriber: the producer must keep publishing. */
    return (numSubs > 0u) || (numTaps > 0u);
}

bool TCPSServiceStream::IsMulticast() const {
    return false;
}

uint16 TCPSServiceStream::GetPort() const {
    return service.IsValid() ? service->GetBoundPort() : static_cast<uint16>(0u);
}

const char8 *TCPSServiceStream::GetStreamName() const {
    return streamName.Buffer();
}

uint32 TCPSServiceStream::GetClientCount() const {
    return numSubs;
}

uint64 TCPSServiceStream::GetClientDrops() const {
    return drops;
}

uint32 TCPSServiceStream::GetMaxPayloadSize() const {
    return maxPayloadSize;
}

bool TCPSServiceStream::AttachConn(TCPSConn *c, uint16 streamIdx) {
    bool ok = false;
    mutex.FastLock();
    if (numSubs < capSubs) {
        // The CONFIG goes in first so the client always decodes DATA against it.
        if (cachedConfigBytes > 0u) {
            uint32 dropped = 0u;
            c->qMutex.FastLock();
            (void)c->q.Push(streamIdx, true, cachedConfigFrame, cachedConfigBytes, dropped);
            c->qMutex.FastUnLock();
        }
        subs[numSubs] = c;
        subIdx[numSubs] = streamIdx;
        numSubs = numSubs + 1u;
        ok = true;
    }
    mutex.FastUnLock();
    return ok;
}

void TCPSServiceStream::DetachConn(TCPSConn *c) {
    mutex.FastLock();
    for (uint32 i = 0u; i < numSubs; i++) {
        if (subs[i] == c) {
            subs[i] = subs[numSubs - 1u];
            subIdx[i] = subIdx[numSubs - 1u];
            numSubs = numSubs - 1u;
            break;
        }
    }
    mutex.FastUnLock();
}

void TCPSServiceStream::DropAllConns() {
    mutex.FastLock();
    for (uint32 i = 0u; i < numSubs; i++) {
        TCPSConn *c = subs[i];
        for (uint32 k = 0u; k < c->numStreams; k++) {
            if (c->streams[k] == this) {
                c->streams[k] = NULL_PTR(TCPSServiceStream *);
            }
        }
        subs[i]->closeRequested = true;
        subs[i]->worker->Wake();
    }
    numSubs = 0u;
    mutex.FastUnLock();
}

} // namespace MARTe