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

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

#include "TCPSFrameQueue.h"

#include "HighResolutionTimer.h"

#include <errno.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/uio.h>
#include <sys/types.h>

namespace MARTe {

struct TCPSFrameQueue::Slot {
    Slot() : buf(NULL_PTR(uint8 *)), cap(0u), len(0u), isConfig(false), stream(0u) {}
    ~Slot() {
        if (buf != NULL_PTR(uint8 *)) {
            delete[] buf;
        }
    }
    uint8 *buf;
    uint32 cap;
    uint32 len;
    bool   isConfig;
    uint16 stream;
};

/** magic(4) + type(1) + counter(4): offset of the header's fragmentIdx field. */
static const uint32 TCPS_FRAGIDX_OFFSET = 9u;

TCPSFrameQueue::TCPSFrameQueue()
    : slots(NULL_PTR(Slot **)), cap(0u), depth(0u), count(0u), sent(0u), lastProgress(0u) {
}

TCPSFrameQueue::~TCPSFrameQueue() {
    if (slots != NULL_PTR(Slot **)) {
        for (uint32 i = 0u; i < cap; i++) {
            delete slots[i];
        }
        delete[] slots;
    }
}

void TCPSFrameQueue::Init(uint32 d) {
    depth = d;
    cap = d + 2u;
    slots = new Slot *[cap];
    for (uint32 i = 0u; i < cap; i++) {
        slots[i] = new Slot();
    }
    count = 0u;
    sent = 0u;
    lastProgress = HighResolutionTimer::Counter();
}

void TCPSFrameQueue::Reserve(uint32 numStreams) {
    const uint32 want = numStreams * (depth + 2u);
    if (want > cap) {
        Slot **grown = new Slot *[want];
        for (uint32 i = 0u; i < cap; i++) {
            grown[i] = slots[i];
        }
        for (uint32 i = cap; i < want; i++) {
            grown[i] = new Slot();
        }
        delete[] slots;
        slots = grown;
        cap = want;
    }
}

/** Remove slot @p i, keeping order; the slot object is recycled at the end. */
void TCPSFrameQueue::RemoveAt(uint32 i) {
    Slot *v = slots[i];
    for (uint32 j = i + 1u; j < count; j++) {
        slots[j - 1u] = slots[j];
    }
    slots[count - 1u] = v;
    count--;
}

bool TCPSFrameQueue::Push(uint16 streamIdx, bool isConfig, const uint8 *frame,
                          uint32 frameBytes, uint32 &dropped) {
    const uint32 firstFree = (sent > 0u) ? 1u : 0u; // slots below this are on the wire

    if (isConfig) {
        uint32 i = firstFree;
        while (i < count) {
            if (slots[i]->stream == streamIdx) {
                if (!slots[i]->isConfig) {
                    dropped++;
                }
                RemoveAt(i);
            } else {
                i++;
            }
        }
    } else {
        uint32 dataCount = 0u;
        for (uint32 i = 0u; i < count; i++) {
            if ((slots[i]->stream == streamIdx) && !slots[i]->isConfig) {
                dataCount++;
            }
        }
        if (dataCount >= depth) {
            uint32 victim = count;
            for (uint32 i = firstFree; (i < count) && (victim == count); i++) {
                if ((slots[i]->stream == streamIdx) && !slots[i]->isConfig) {
                    victim = i;
                }
            }
            dropped++;
            if (victim == count) {
                return false; // only the frame on the wire is queued
            }
            RemoveAt(victim);
        }
    }

    if (count >= cap) {
        dropped++;
        return false;
    }

    Slot &s = *slots[count];
    if (s.cap < frameBytes) {
        if (s.buf != NULL_PTR(uint8 *)) {
            delete[] s.buf;
        }
        s.buf = new uint8[frameBytes];
        s.cap = frameBytes;
    }
    memcpy(s.buf, frame, frameBytes);
    if (frameBytes >= (TCPS_FRAGIDX_OFFSET + 2u)) {
        memcpy(&s.buf[TCPS_FRAGIDX_OFFSET], &streamIdx, 2u);
    }
    s.len = frameBytes;
    s.isConfig = isConfig;
    s.stream = streamIdx;
    if (count == 0u) {
        sent = 0u;
        lastProgress = HighResolutionTimer::Counter();
    }
    count++;
    return true;
}

/** Account for @p bytes written from the head of the queue. */
void TCPSFrameQueue::Consume(uint32 bytes) {
    while ((bytes > 0u) && (count > 0u)) {
        const uint32 left = slots[0]->len - sent;
        if (bytes >= left) {
            bytes -= left;
            RemoveAt(0u);
            sent = 0u;
        } else {
            sent += bytes;
            bytes = 0u;
        }
    }
}

bool TCPSFrameQueue::Flush(int fd) {
    bool alive = true;
    while (alive && (count > 0u)) {
        struct iovec iov[TCPS_MAX_BATCH];
        uint32 n = (count < TCPS_MAX_BATCH) ? count : TCPS_MAX_BATCH;
        for (uint32 i = 0u; i < n; i++) {
            const uint32 off = (i == 0u) ? sent : 0u;
            iov[i].iov_base = &slots[i]->buf[off];
            iov[i].iov_len = slots[i]->len - off;
        }
        struct msghdr msg;
        memset(&msg, 0, sizeof(msg));
        msg.msg_iov = iov;
        msg.msg_iovlen = n;
        ssize_t w = sendmsg(fd, &msg, MSG_NOSIGNAL | MSG_DONTWAIT);
        if (w > 0) {
            lastProgress = HighResolutionTimer::Counter();
            Consume(static_cast<uint32>(w));
        } else if ((w < 0) && ((errno == EAGAIN) || (errno == EWOULDBLOCK))) {
            break;
        } else if ((w < 0) && (errno == EINTR)) {
            continue;
        } else {
            alive = false;
        }
    }
    return alive;
}

bool TCPSFrameQueue::Empty() const {
    return count == 0u;
}

uint64 TCPSFrameQueue::LastProgress() const {
    return lastProgress;
}

} // namespace MARTe