rmc_proto_test_sub.c

// Copyright (C) 2018, Jaguar Land Rover
// This program is licensed under the terms and conditions of the
// Mozilla Public License, version 2.0.  The full text of the
// Mozilla Public License is at https://www.mozilla.org/MPL/2.0/
//
// Author: Magnus Feuer (mfeuer1@jaguarlandrover.com)

#include "rmc_proto_test_common.h"
#include "rmc_internal.h"
#include "rmc_log.h"

// Maximum number of publishers an rmc_sub_context_t can have.
#define RMC_MAX_CONNECTIONS 16


// Indexed by publisher node_id, as received in the
// payload
typedef struct {
    enum {
        // We will not process traffic for this node_id.
        // Any traffic received will trigger error.
        RMC_TEST_SUB_INACTIVE = 0,

        // We expect traffic on this ctx-id (as provided by -e <ctx-id>,
        // But haven't seen any yet.
        RMC_TEST_SUB_NOT_STARTED = 1,

        // We are in the process of receiving traffic
        RMC_TEST_SUB_IN_PROGRESS = 2,

        // We have received all expected traffic for the given ctx-id.
        RMC_TEST_SUB_COMPLETED = 3
    } status;

    uint64_t max_expected;
    uint64_t max_received;
    uint64_t expect_sum;
    uint64_t calc_sum;
    usec_timestamp_t start_ts; // First packet received
    usec_timestamp_t stop_ts; // Last packet received
} sub_expect_t;


__attribute__ ((unused))
static uint8_t _test_print_pending(sub_packet_node_t* node, void* dt)
{

    sub_packet_t* pack = (sub_packet_t*) node->data;
    int indent = *((int*) dt);

    printf("%*cPacket          %p\n", indent*2, ' ', pack);
    printf("%*c  PID             %llu\n", indent*2, ' ', (long long unsigned) pack->pid);
    printf("%*c  Payload Length  %d\n", indent*2, ' ', pack->payload_len);
    putchar('\n');
    return 1;
}


static int _descriptor(rmc_sub_context_t* ctx,
                       rmc_index_t index)
{
    switch(index) {
    case RMC_MULTICAST_INDEX:
        return ctx->mcast_recv_descriptor;

    case RMC_NIL_INDEX:
        return -1;


    default:
        return ctx->conn_vec.connections[index].descriptor;

    }
}

static int check_exit_condition( sub_expect_t* expect, int expect_sz)
{
    int ind = expect_sz;

    while(ind--) {
        if (expect[ind].status == RMC_TEST_SUB_NOT_STARTED ||
            expect[ind].status == RMC_TEST_SUB_IN_PROGRESS)
            return 0;
    }
    return 1;
}


static uint8_t announce_cb(struct rmc_sub_context* ctx,
                           uint32_t listen_ip,
                           in_port_t listen_port,
                           rmc_node_id_t node_id,
                           void* payload,
                           payload_len_t payload_len)
{

    if (!rmc_log_get_start_time()) {
        RMC_LOG_INFO("Announce detected. Starting clock");
        rmc_log_set_start_time();
    } else
        RMC_LOG_INFO("Announce detected. Clock already running.");

    return 1;
}




static int process_incoming_signal(rmc_sub_context_t* ctx,
                                   rmc_index_t index,
                                   char* data,
                                   sub_expect_t* expect,
                                   int expect_sz)


{
    signal_t *signal = (signal_t*) data;
    rmc_node_id_t node_id = signal->node_id;
    uint64_t max_expected = signal->max_signal_id;
    uint64_t current = signal->signal_id;


    // Is node ID within our expetcted range
    if (node_id >= expect_sz) {
        RMC_LOG_INDEX_FATAL(index,
                            "ContextID [%u] is out of range (0-%d)",
                            node_id, expect_sz);
        exit(255);
     }

    // Is this context expected?
    if (expect[node_id].status == RMC_TEST_SUB_INACTIVE) {
        RMC_LOG_INDEX_FATAL(index,
                            "ContextID [%u] not expected. Use -e %u to setup subscriber expectations.",
                            node_id, node_id);
        exit(255);
    }

    // Have we already completed all expected packages here?
    if (expect[node_id].status == RMC_TEST_SUB_COMPLETED) {
        RMC_LOG_INDEX_FATAL(index,
                            "ContextID [%u] have already processed its [%llu] packets. Got Current[%llu] Max[%llu].",
                            node_id,
                            (long long unsigned) expect[node_id].max_received,
                            (long long unsigned) current,
                            (long long unsigned) max_expected);
        exit(255);
    }

    // Check if this is the first packet from an expected source.
    // If so, set things up.
    if (expect[node_id].status == RMC_TEST_SUB_NOT_STARTED) {
        int ind = 0;
        expect[node_id].status = RMC_TEST_SUB_IN_PROGRESS;
        expect[node_id].max_expected = max_expected;
        expect[node_id].max_received = 0;
        expect[node_id].expect_sum = 0;
        expect[node_id].calc_sum = 0;
        expect[node_id].stop_ts = 0;

        // Calculate sum
        for(ind = 1; ind <= max_expected; ++ind)
            expect[node_id].expect_sum += ind;

        expect[node_id].start_ts = rmc_usec_monotonic_timestamp();

        RMC_LOG_INDEX_INFO(index,
                           "Activate: node_id[%u] current[%llu] max_expected[%llu] expected sum[%llu]",
                           node_id, current, max_expected, expect[node_id].calc_sum);

        // Fall through to the next if statement
    }

    // Check if we are in progress.
    // If so, check that packets are correctly numbrered.

    if (expect[node_id].status == RMC_TEST_SUB_IN_PROGRESS) {

        // Check that max_expected hasn't changed.
        if (max_expected != expect[node_id].max_expected) {
            RMC_LOG_INDEX_FATAL(index,
                                "ContextID [%u] max_expected changed from [%llu] to [%llu]",
                                node_id, expect[node_id].max_expected, max_expected);
            exit(255);
        }

        // Check that packet is consecutive.
        if (current != expect[node_id].max_received + 1) {
            RMC_LOG_INDEX_FATAL(index,
                                "ContextID [%u] Wanted packet[%llu] Got[%llu]",
                                node_id, expect[node_id].max_received + 1, current);
            exit(255);
        }

        expect[node_id].max_received = current;
        expect[node_id].calc_sum += current;

        // Check if we are complete
        if (current == max_expected) {
            int signal_sec = 0;
            printf("%s[%.3d]%s ContextID [%u] %s**COMPLETE*%s* at[%llu]\n",
                   rmc_index_color(index), index, rmc_log_color_none(),
                   node_id, rmc_log_color_green(), rmc_log_color_none(),
                   (long long unsigned) current);

            // Did we see data corruption?
            if (expect[node_id].expect_sum !=  expect[node_id].calc_sum) {
                RMC_LOG_INDEX_FATAL(index, "DATA CORRUPTION! Expected total sum: %llu. Got %llu\n",
                                    (long long unsigned) expect[node_id].expect_sum,
                                    (long long unsigned) expect[node_id].calc_sum);
                exit(0);
            }

            expect[node_id].status = RMC_TEST_SUB_COMPLETED;
            expect[node_id].stop_ts = rmc_usec_monotonic_timestamp();
            signal_sec = (int) expect[node_id].max_received /
                ((double) (expect[node_id].stop_ts - expect[node_id].start_ts) / 1000000.0);

            printf("%s[%.3d]%s %llu signals (%u bytes each) in %llu msec -> %s%d signals / sec%s -> %s%u kbyte / sec%s\n",
                   rmc_index_color(index), index, rmc_log_color_none(),
                   (long long unsigned) expect[node_id].max_received,
                   (unsigned int) sizeof(signal_t),
                   (long long unsigned) ((expect[node_id].stop_ts - expect[node_id].start_ts) / 1000),
                   rmc_log_color_green(),
                   signal_sec,
                   rmc_log_color_none(),
                   rmc_log_color_green(),
                   (unsigned) (signal_sec * sizeof(signal_t) / 1024),
                   rmc_log_color_none());

            // Check if this is the last one out.
            if (check_exit_condition(expect, expect_sz))
                return 0;
        }

        return 1;
    }

    RMC_LOG_INDEX_FATAL(index,
                        "Eh? expect[%u:%llu:%llu] status[%d]  data[%u:%llu:%llu]\n",
                        node_id,
                        (long long unsigned) expect[node_id].max_received,
                        (long long unsigned) expect[node_id].max_expected,
                        expect[node_id].status,
                        node_id,
                        (long long unsigned) current,
                        (long long unsigned) max_expected);

    exit(255);
}

static int process_incoming_packet(rmc_sub_context_t* ctx,
                                   sub_packet_t* pack,
                                   sub_expect_t* expect,
                                   int expect_sz)
{
    int pack_ind = 0;


    while(pack_ind < pack->payload_len) {
        if (!process_incoming_signal(ctx, sub_packet_user_data(pack).u32, pack->payload + pack_ind, expect, expect_sz))
            return 0;

        pack_ind += sizeof(signal_t);
    }

    // Will free payload
    rmc_sub_packet_dispatched(ctx, pack);
    return 1;
}

static int process_events(rmc_sub_context_t* ctx,
                          int epollfd,
                          usec_timestamp_t timeout_ts)
{
    struct epoll_event events[RMC_MAX_CONNECTIONS];
    int nfds = 0;

    if (timeout_ts != -1) {
        timeout_ts -= rmc_usec_monotonic_timestamp();
        if (timeout_ts < 0)
            timeout_ts = 0;
    }

    nfds = epoll_wait(epollfd, events,
                      rmc_sub_get_max_publisher_count(ctx),
                      (timeout_ts == -1)?-1:(timeout_ts / 1000) + 1);

    if (nfds == -1) {
        RMC_LOG_FATAL("epoll_wait(): %s", strerror(errno));
        exit(255);
    }

    // Timeout
    if (nfds == 0)
        return ETIME;

    // printf("poll_wait(): %d results\n", nfds);
    while(nfds--) {
        int res = 0;
        uint8_t op_res = 0;
        rmc_index_t c_ind = (rmc_index_t) events[nfds].data.u32;

        RMC_LOG_INDEX_DEBUG(c_ind, "%d - %s%s%s",
                            _descriptor(ctx, c_ind),
                            ((events[nfds].events & EPOLLIN)?" read":""),
                            ((events[nfds].events & EPOLLOUT)?" write":""),
                            ((events[nfds].events & EPOLLHUP)?" disconnect":""));


        if (events[nfds].events & EPOLLIN) {
            errno = 0;
            res = rmc_sub_read(ctx, c_ind, &op_res);
            // Did we read a loopback message we sent ourselves?
            RMC_LOG_INDEX_DEBUG(c_ind, "read result: %s - %s", _op_res_string(op_res),   strerror(res));
        }

        if (events[nfds].events & EPOLLOUT) {
            if (rmc_sub_write(ctx, c_ind, &op_res) != 0) {
                RMC_LOG_INDEX_INFO(c_ind, "write result: %s - %s", _op_res_string(op_res),   strerror(res));
                rmc_sub_close_connection(ctx, c_ind);
            }
        }
    }

    return 0;
}



void test_rmc_proto_sub(char* mcast_group_addr,
                        char* mcast_if_addr,
                        int mcast_port,
                        rmc_node_id_t node_id,
                        uint8_t* node_id_map,
                        int node_id_map_size)
{
    rmc_sub_context_t* ctx = 0;
    int epollfd = -1;
    int ind = 0;
    usec_timestamp_t timeout_ts = 0;
    uint8_t *conn_vec_mem = 0;
    int do_exit = 0;

    // Indexed by publisher node_id
    sub_expect_t expect[node_id_map_size];


    epollfd = epoll_create1(0);
    for (ind = 0; ind < node_id_map_size; ++ind) {
        expect[ind].status = RMC_TEST_SUB_INACTIVE;
        expect[ind].max_received = 0;
        expect[ind].max_expected = 0;

        // Check if we are expecting traffic on this one
        if (node_id_map[ind])
            expect[ind].status = RMC_TEST_SUB_NOT_STARTED;
    }

    if (epollfd == -1) {
        perror("epoll_create1");
        exit(255);
    }

    conn_vec_mem = malloc(sizeof(rmc_connection_t)*RMC_MAX_CONNECTIONS);
    memset(conn_vec_mem, 0, sizeof(rmc_connection_t)*RMC_MAX_CONNECTIONS);
    rmc_sub_init_context(&ctx,
                         0, // Assign random node id
                         mcast_group_addr,
                         mcast_port,
                         mcast_if_addr,
                         (user_data_t) { .i32 = epollfd },
                         poll_add, poll_modify, poll_remove,
                         RMC_MAX_CONNECTIONS,
                         0,0);

    _test("rmc_proto_test_sub[%d.%d] activate_context(): %s",
          1, 1,
          rmc_sub_activate_context(ctx));

    rmc_sub_set_announce_callback(ctx, announce_cb);

    RMC_LOG_INFO("ctx[%.9X] mcast_addr[%s] mcast_port[%d]",
                  rmc_sub_node_id(ctx), mcast_group_addr, mcast_port);

    while(1) {
        sub_packet_t* pack = 0;
        packet_id_t first_pid = 0;
        usec_timestamp_t current_ts = rmc_usec_monotonic_timestamp();

        rmc_sub_timeout_get_next(ctx, &timeout_ts);

        while(timeout_ts != -1 && timeout_ts < current_ts) {
            rmc_sub_timeout_process(ctx);
            rmc_sub_timeout_get_next(ctx, &timeout_ts);
        }

        RMC_LOG_COMMENT("timeout [%ld] msec", (timeout_ts == -1)?-1:(timeout_ts  - current_ts)/1000);
        if (process_events(ctx, epollfd, timeout_ts) == ETIME) {
            RMC_LOG_COMMENT("Got timeout");
            continue;
        }
        else
            RMC_LOG_DEBUG("No timeout");

        // Process as many packets as possible.

        while((pack = rmc_sub_get_next_dispatch_ready(ctx))) {
            if (!first_pid)
                first_pid = pack->pid;

            if (!process_incoming_packet(ctx, pack, expect, node_id_map_size)) {
                do_exit = 1;
                break;
            }
        }
        RMC_LOG_COMMENT("max_pid_ready[%llu]  max_pid_received[%llu]",
                        (long long unsigned) ctx->publishers[0].max_pid_ready,
                        (long long unsigned) ctx->publishers[0].max_pid_ready);

        if (do_exit)
            break;
    }

    // process events until we get a timeout.
    // This ensures that we flush all outbound buffers.
    while(process_events(ctx, epollfd, 100) != ETIME);

    rmc_sub_deactivate_context(ctx);

    RMC_LOG_INFO("Done.");
    exit(0);
}