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

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

#define DLL_API

#include "TCPSWorker.h"

#include "AdvancedErrorManagement.h"
#include "Atomic.h"
#include "HighResolutionTimer.h"
#include "TCPSConn.h"
#include "TCPStreamService.h"
#include "UDPSProtocol.h"

#include <errno.h>
#include <fcntl.h>
#include <string.h>
#include <sys/socket.h>
#include <unistd.h>

namespace MARTe {

/** Longest wait in poll(): bounds Stop() latency and the stall / handshake checks. */
static const int TCPS_WORKER_SLICE_MS = 50;

TCPSWorker::TCPSWorker(TCPStreamService &owner, uint32 maxConnections)
    : service(owner), maxConns(maxConnections), thread(*this),
      incoming(new TCPSConn *[maxConnections]), numIncoming(0u),
      conns(new TCPSConn *[maxConnections]), numConns(0u),
      pfds(new struct pollfd[maxConnections + 1u]), load(0) {
    wakeFd[0] = -1;
    wakeFd[1] = -1;
    if (pipe(wakeFd) == 0) {
        for (int i = 0; i < 2; i++) {
            int flags = fcntl(wakeFd[i], F_GETFL, 0);
            if (flags >= 0) {
                (void)fcntl(wakeFd[i], F_SETFL, flags | O_NONBLOCK);
            }
        }
    }
}

TCPSWorker::~TCPSWorker() {
    Stop();
    CloseAll();
    for (uint32 i = 0u; i < numIncoming; i++) {
        delete incoming[i];
        service.ConnClosed();
    }
    delete[] incoming;
    delete[] conns;
    delete[] pfds;
    for (int i = 0; i < 2; i++) {
        if (wakeFd[i] >= 0) {
            (void)close(wakeFd[i]);
        }
    }
}

bool TCPSWorker::Start(uint32 cpuMask, uint32 stackSize) {
    if (wakeFd[0] < 0) {
        return false;
    }
    thread.SetCPUMask(cpuMask);
    thread.SetStackSize(stackSize);
    return thread.Start() == ErrorManagement::NoError;
}

void TCPSWorker::Stop() {
    (void)thread.Stop();
}

bool TCPSWorker::AddIncoming(TCPSConn *c) {
    bool ok = false;
    incomingMutex.FastLock();
    if (numIncoming < maxConns) {
        incoming[numIncoming] = c;
        numIncoming++;
        ok = true;
    }
    incomingMutex.FastUnLock();
    if (ok) {
        Atomic::Increment(&load);
        Wake();
    }
    return ok;
}

void TCPSWorker::Wake() {
    if (wakeFd[1] >= 0) {
        const uint8 b = 1u;
        // EAGAIN means the pipe is already full of wake-ups: nothing to add.
        ssize_t r = write(wakeFd[1], &b, 1u);
        (void)r;
    }
}

int32 TCPSWorker::Load() const {
    return load;
}

void TCPSWorker::AdoptIncoming() {
    incomingMutex.FastLock();
    for (uint32 i = 0u; i < numIncoming; i++) {
        conns[numConns] = incoming[i];
        numConns++;
    }
    numIncoming = 0u;
    incomingMutex.FastUnLock();
}

void TCPSWorker::CloseConn(TCPSConn *c) {
    service.Detach(c);
    delete c; // closes the socket
    service.ConnClosed();
    Atomic::Decrement(&load);
}

void TCPSWorker::CloseAll() {
    for (uint32 i = 0u; i < numConns; i++) {
        CloseConn(conns[i]);
    }
    numConns = 0u;
}

bool TCPSWorker::Drain(TCPSConn &c) {
    uint8 scratch[64];
    bool alive = true;
    for (;;) {
        ssize_t n = recv(c.fd, scratch, sizeof(scratch), MSG_DONTWAIT);
        if (n > 0) {
            continue; // clients have nothing more to say after SUBSCRIBE
        }
        if (n == 0) {
            alive = false;
        } else if ((errno == EAGAIN) || (errno == EWOULDBLOCK)) {
            alive = true;
        } else if (errno == EINTR) {
            continue;
        } else {
            alive = false;
        }
        break;
    }
    return alive;
}

bool TCPSWorker::HandshakeStep(TCPSConn &c) {
    ssize_t n = recv(c.fd, &c.hs[c.hsHave], c.hsWant - c.hsHave, MSG_DONTWAIT);
    if (n == 0) {
        return false;
    }
    if (n < 0) {
        return (errno == EAGAIN) || (errno == EWOULDBLOCK) || (errno == EINTR);
    }
    c.hsHave += static_cast<uint32>(n);

    if (!c.hsHeaderOk && (c.hsHave >= UDPS_HEADER_SIZE)) {
        UDPSPacketHeader h;
        memcpy(&h, c.hs, UDPS_HEADER_SIZE);
        if ((h.magic != UDPS_MAGIC) || (h.type != UDPS_TYPE_SUBSCRIBE) ||
            (h.payloadBytes == 0u) || (h.payloadBytes > TCPS_MAX_SUBSCRIBE_PAYLOAD)) {
            REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                "TCPStreamService: connection did not open with a valid SUBSCRIBE; closing.");
            service.NoteRejected();
            return false;
        }
        c.hsHeaderOk = true;
        c.hsWant = UDPS_HEADER_SIZE + h.payloadBytes;
    }
    if (c.hsHeaderOk && (c.hsHave == c.hsWant)) {
        c.hs[c.hsWant] = 0u;
        char8 *names = reinterpret_cast<char8 *>(&c.hs[UDPS_HEADER_SIZE]);
        if (!service.Attach(&c, names)) {
            REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                "TCPStreamService: cannot attach to '%s'; closing.", names);
            service.NoteRejected();
            return false;
        }
        c.subscribed = true;
    }
    return true;
}

ErrorManagement::ErrorType TCPSWorker::Execute(ExecutionInfo &info) {
    if (info.GetStage() == ExecutionInfo::TerminationStage) {
        return ErrorManagement::NoError;
    }
    if (info.GetStage() != ExecutionInfo::MainStage) {
        return ErrorManagement::NoError;
    }

    AdoptIncoming();

    pfds[0].fd = wakeFd[0];
    pfds[0].events = POLLIN;
    pfds[0].revents = 0;
    for (uint32 i = 0u; i < numConns; i++) {
        TCPSConn *c = conns[i];
        short ev = POLLIN;
        if (c->subscribed) {
            c->qMutex.FastLock();
            if (!c->q.Empty()) {
                ev = static_cast<short>(ev | POLLOUT);
            }
            c->qMutex.FastUnLock();
        }
        pfds[i + 1u].fd = c->fd;
        pfds[i + 1u].events = ev;
        pfds[i + 1u].revents = 0;
    }

    int pr = poll(pfds, static_cast<nfds_t>(numConns + 1u), TCPS_WORKER_SLICE_MS);
    if (pr < 0) {
        return ErrorManagement::NoError; // EINTR
    }
    if ((pfds[0].revents & POLLIN) != 0) {
        uint8 sink[64];
        while (read(wakeFd[0], sink, sizeof(sink)) > 0) {
        }
    }

    const uint64 now = HighResolutionTimer::Counter();
    const uint64 stallTicks = service.StallTicks();
    uint32 kept = 0u;
    for (uint32 i = 0u; i < numConns; i++) {
        TCPSConn *c = conns[i];
        const short re = pfds[i + 1u].revents;
        bool keep = !c->closeRequested && ((re & (POLLERR | POLLNVAL)) == 0);

        if (keep && ((re & (POLLIN | POLLHUP)) != 0)) {
            keep = c->subscribed ? Drain(*c) : HandshakeStep(*c);
        }
        if (keep && c->subscribed) {
            c->qMutex.FastLock();
            keep = c->q.Flush(c->fd);
            if (keep && !c->q.Empty() && (stallTicks > 0u) &&
                (now > c->q.LastProgress()) && ((now - c->q.LastProgress()) > stallTicks)) {
                REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                    "TCPStreamService: client stalled; disconnecting.");
                service.NoteEviction();
                keep = false;
            }
            c->qMutex.FastUnLock();
        }
        if (keep && !c->subscribed && (now > c->hsDeadline)) {
            REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                "TCPStreamService: SUBSCRIBE timeout; closing.");
            service.NoteRejected();
            keep = false;
        }

        if (keep) {
            conns[kept] = c;
            kept++;
        } else {
            CloseConn(c);
        }
    }
    numConns = kept;
    return ErrorManagement::NoError;
}

} // namespace MARTe