dataTransmitter.c

#include <stdio.h>
#include <netdb.h>
#include <string.h>
#include "../../defines.h"
#include "../block.h"
#include "../receiver.h"
#include "../socket.h"
#include "../../util/dbg.h"
#include "../../util/common.h"
#include "dataTransmitter.h"

bool send_to_socket(PrrtSocket* sock_ptr, PrrtReceiver *recv, uint8_t* buf, prrtPacketLength_t length,  struct timespec *packet_stamp_ptr) {
    if(sock_ptr->isHardwareTimestamping) {
        struct msghdr msg;
        struct iovec iov;
        char control[1024];
        ssize_t got;
        int check = 0;
        const int check_max = 9999;

        iov.iov_base = buf;
        iov.iov_len = length;
        msg.msg_iov = &iov;
        msg.msg_iovlen = 1;
        msg.msg_name = recv->ai->ai_addr;
        msg.msg_namelen = recv->ai->ai_addrlen;
        msg.msg_control = control;
        msg.msg_controllen = 0;

        sendmsg(sock_ptr->dataSocketFd, &msg, 0);

        msg.msg_control = control;
        iov.iov_len = MAX_PAYLOAD_LENGTH;
        do {
            msg.msg_controllen = 1024;
            got = recvmsg(sock_ptr->dataSocketFd, &msg, MSG_ERRQUEUE);
        } while(got < 0  && errno == EAGAIN && check++ < check_max);
        check(!(got < 0 && errno == EAGAIN), "Failed to get stamp. Gave up.");

        struct cmsghdr* cmsg;
        for(cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)) {
            if (cmsg->cmsg_level != SOL_SOCKET) {
                continue;
            }
            switch(cmsg->cmsg_type) {
                case SO_TIMESTAMPING: {
                    // Note: The raw stamp [2] is used, because the others are 0.
                    struct timespec *hardstamp = &(((struct timespec*) CMSG_DATA(cmsg))[2]); //TODO: This is ugly.
                    memcpy(packet_stamp_ptr, hardstamp, sizeof(struct timespec));
                    debug(DEBUG_HARDSTAMPING, "Hardware TS:\t%ld.%09ld", (long) packet_stamp_ptr->tv_sec, (long) packet_stamp_ptr->tv_nsec);
                    break;
                }
                default:
                    debug(DEBUG_DATARECEIVER, "Msgtype: %d", cmsg->cmsg_type);
                    break;
            }
        }
    } else {
        // TODO: [LATENCY] By knowing the time encoding etc. that happens upfront, one could make an adjustment here.
        check(sendto(sock_ptr->dataSocketFd, buf, length, 0, recv->ai->ai_addr, recv->ai->ai_addrlen) ==
              length, "Sendto failed.");
        clock_gettime(CLOCK_REALTIME, packet_stamp_ptr);
        debug(DEBUG_HARDSTAMPING, "Software TS:\t%ld.%09ld", (long) packet_stamp_ptr->tv_sec, (long) packet_stamp_ptr->tv_nsec);
    }
    return true;

    error:
    PERROR("Sending packet failed.%s", "")
    return false;

}

static bool send_packet(PrrtSocket *sock_ptr, PrrtPacket *packet) {
    uint8_t buf[MAX_PAYLOAD_LENGTH];
    memset(buf, 0, sizeof(buf));
    prrtPacketLength_t length = PrrtPacket_size(packet);

    check(PrrtPacket_encode(buf, MAX_PAYLOAD_LENGTH, packet), "Buffer too small.");

    switch (PrrtPacket_type(packet)) {
        case PACKET_TYPE_DATA:
            XlapTimeStampCycle(sock_ptr, ts_data_packet, packet->sequenceNumber, LinkTransmitStart);
            break;
        case PACKET_TYPE_REDUNDANCY:
            XlapTimeStampCycle(sock_ptr, ts_redundancy_packet, packet->sequenceNumber, LinkTransmitStart);
            break;
        case PACKET_TYPE_REPETITION:
        case PACKET_TYPE_FEEDBACK:
        case PACKET_TYPE_PRESENT_REDUNDANCY:
        case PACKET_TYPE_CHANNEL_FEEDBACK:
        default:;
    }

    struct timespec timestamp;
    send_to_socket(sock_ptr, sock_ptr->receiver, buf, length, &timestamp);
    XlapTimeStampValue(sock_ptr, ts_data_packet, packet->sequenceNumber, ChannelTransmit, timestamp);

    PrrtReceiver_add_outstanding_packet_state(sock_ptr->receiver, packet, PrrtClock_TimespecToPrrtTimestamp(timestamp));

    switch (PrrtPacket_type(packet)) {
        case PACKET_TYPE_DATA:
            XlapTimeStampClock(sock_ptr, ts_data_packet, packet->sequenceNumber, LinkTransmitEnd);
            XlapTimeStampCycle(sock_ptr, ts_data_packet, packet->sequenceNumber, LinkTransmitEnd);
            break;
        case PACKET_TYPE_REDUNDANCY:
            XlapTimeStampClock(sock_ptr, ts_redundancy_packet, packet->sequenceNumber, LinkTransmitEnd);
            XlapTimeStampCycle(sock_ptr, ts_redundancy_packet, packet->sequenceNumber, LinkTransmitEnd);
            break;
        case PACKET_TYPE_REPETITION:
        case PACKET_TYPE_FEEDBACK:
        case PACKET_TYPE_PRESENT_REDUNDANCY:
        case PACKET_TYPE_CHANNEL_FEEDBACK:
        default:;
    }

    return true;

    error:
    PERROR("Sending packet failed.%s", "")
    return false;
}

void *send_data_loop(void *ptr) {
    PrrtSocket *sock_ptr = ptr;
    PrrtBlock *block = NULL;

    while (1) {
        ListNode *job;
        do {
            job = MPSCQueue_pop(sock_ptr->sendDataQueue);
            if (PrrtSocket_closing(sock_ptr)) {
                if (block != NULL) {
                    PrrtBlock_destroy(block);
                }
                return NULL;
            }
        } while (!job);
        PrrtPacket *packet = PrrtPacket_byListNode(job);
        XlapTimeStampClock(sock_ptr, ts_data_packet, packet->sequenceNumber, PrrtTransmitStart);
        XlapTimeStampCycle(sock_ptr, ts_data_packet, packet->sequenceNumber, PrrtTransmitStart);
        if (block == NULL) {
            block = PrrtBlock_create(sock_ptr->codingParameters, packet->sequenceNumber);
        }

        packet->index = (uint8_t) (packet->sequenceNumber - block->baseSequenceNumber);

        PrrtPacketDataPayload *payload = packet->payload;
        payload->groupRTprop_us = PrrtSocket_get_rtprop(sock_ptr);

        PrrtPacket *packetToSend = PrrtPacket_copy(packet);
        send_packet(sock_ptr, packetToSend);
        XlapTimeStampClock(sock_ptr, ts_data_packet, packet->sequenceNumber, PrrtTransmitEnd);
        XlapTimeStampCycle(sock_ptr, ts_data_packet, packet->sequenceNumber, PrrtTransmitEnd);

        PrrtBlock_insert_data_packet(block, packet);

        // TODO: redundancy should only be sent when necessary
        if (PrrtBlock_encode_ready(block)) {
            uint32_t j = 0;
            unsigned int redundancy_seqno = sock_ptr->sequenceNumberRedundancy;
            XlapTimeStampCycle(sock_ptr, ts_redundancy_packet, redundancy_seqno, PrrtEncodeStart);
            PrrtBlock_encode(block, &sock_ptr->sequenceNumberRedundancy);
            XlapTimeStampCycle(sock_ptr, ts_redundancy_packet, redundancy_seqno, PrrtEncodeEnd);

            uint32_t redundancyPackets = List_count(block->redundancyPackets);
            for (j = 0; j < redundancyPackets; j++) {
                PrrtPacket *red_pkt = PrrtBlock_get_first_red_data(block);
                send_packet(sock_ptr, red_pkt);
            }

            PrrtBlock_destroy(block);
            block = NULL;
        }
        PrrtSocket_cleanup(sock_ptr);
    }
}