can-rtt.c

/*
 * Round-Trip-Time Test - sends and receives messages and measures the
 *                        time in between.
 *
 * Copyright (C) 2006 Wolfgang Grandegger <wg@grandegger.com>
 *
 * Based on RTnet's examples/xenomai/posix/rtt-sender.c.
 *
 * Copyright (C) 2002 Ulrich Marx <marx@kammer.uni-hannover.de>
 *               2002 Marc Kleine-Budde <kleine-budde@gmx.de>
 *               2006 Jan Kiszka <jan.kiszka@web.de>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 *
 * The program sends out CAN messages periodically and copies the current
 * time-stamp to the payload. At reception, that time-stamp is compared
 * with the current time to determine the round-trip time. The jitter
 * values are printer out regularly. Concurrent tests can be carried out
 * by starting the program with different message identifiers. It is also
 * possible to use this program on a remote system as simple repeater to
 * loopback messages.
 */

#include <errno.h>
#include <mqueue.h>
#include <signal.h>
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <limits.h>
#include <getopt.h>
#include <memory.h>
#include <netinet/in.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <rtdm/can.h>
#include <xenomai/init.h>

#define NSEC_PER_SEC 1000000000

static unsigned int cycle = 10000; /* 10 ms */
static canid_t can_id = 0x1;

static pthread_t txthread, rxthread;
static int txsock, rxsock;
static mqd_t mq;
static int txcount, rxcount;
static int overruns;
static int repeater;

struct rtt_stat {
    long long rtt;
    long long rtt_min;
    long long rtt_max;
    long long rtt_sum;
    long long rtt_sum_last;
    int counts_per_sec;
};

void application_usage(void)
{
    fprintf(stderr, "usage: %s [options] <tx-can-interface> <rx-can-interface>:\n",
            get_program_name());
    fprintf(stderr,
            " -r, --repeater                    Repeater, send back received messages\n"
            " -i, --id=ID                       CAN Identifier (default = 0x1)\n"
            " -c, --cycle                       Cycle time in us (default = 10000us)\n");
}

static void *transmitter(void *arg)
{
    struct sched_param  param = { .sched_priority = 80 };
    struct timespec next_period;
    struct timespec time;
    struct can_frame frame;
    long long *rtt_time = (long long *)&frame.data, t;

    /* Pre-fill CAN frame */
    frame.can_id = can_id;
    frame.can_dlc = sizeof(*rtt_time);

    pthread_setname_np(pthread_self(), "rtcan_rtt_transmitter");
    pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);

    clock_gettime(CLOCK_MONOTONIC, &next_period);

    while(1) {
        next_period.tv_nsec += cycle * 1000;
        while (next_period.tv_nsec >= NSEC_PER_SEC) {
                next_period.tv_nsec -= NSEC_PER_SEC;
                next_period.tv_sec++;
        }

        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_period, NULL);

        if (rxcount != txcount) {
            overruns++;
            continue;
        }

        clock_gettime(CLOCK_MONOTONIC, &time);
        t = (long long)time.tv_sec * NSEC_PER_SEC + time.tv_nsec;
        memcpy(rtt_time, &t, sizeof(t));

        /* Transmit the message containing the local time */
        if (send(txsock, (void *)&frame, sizeof(struct can_frame), 0) < 0) {
            if (errno == EBADF)
                printf("terminating transmitter thread\n");
            else
                perror("send failed");
            return NULL;
        }
        txcount++;
    }
}


static void *receiver(void *arg)
{
    struct sched_param param = { .sched_priority = 82 };
    struct timespec time;
    struct can_frame frame;
    long long *rtt_time = (long long *)frame.data, t;
    struct rtt_stat rtt_stat = {0, 1000000000000000000LL, -1000000000000000000LL,
                                0, 0, 0};

    pthread_setname_np(pthread_self(), "rtcan_rtt_receiver");
    pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);

    rtt_stat.counts_per_sec = 1000000 / cycle;

    while (1) {
        if (recv(rxsock, (void *)&frame, sizeof(struct can_frame), 0) < 0) {
            if (errno == EBADF)
                printf("terminating receiver thread\n");
            else
                perror("recv failed");
            return NULL;
        }
        if (repeater) {
            /* Transmit the message back as is */
            if (send(txsock, (void *)&frame, sizeof(struct can_frame), 0) < 0) {
                if (errno == EBADF)
                    printf("terminating transmitter thread\n");
                else
                    perror("send failed");
                return NULL;
            }
            txcount++;
        } else {
            memcpy(&t, rtt_time, sizeof(t));
            clock_gettime(CLOCK_MONOTONIC, &time);
            if (rxcount > 0) {
                rtt_stat.rtt = ((long long)time.tv_sec * 1000000000LL +
                                time.tv_nsec - t);
                rtt_stat.rtt_sum += rtt_stat.rtt;
                if (rtt_stat.rtt <  rtt_stat.rtt_min)
                    rtt_stat.rtt_min = rtt_stat.rtt;
                if (rtt_stat.rtt > rtt_stat.rtt_max)
                    rtt_stat.rtt_max = rtt_stat.rtt;
            }
        }
        rxcount++;

        if ((rxcount % rtt_stat.counts_per_sec) == 0) {
            mq_send(mq, (char *)&rtt_stat, sizeof(rtt_stat), 0);
            rtt_stat.rtt_sum_last = rtt_stat.rtt_sum;
        }
    }
}

static void catch_signal(int sig)
{
    mq_close(mq);
    close(rxsock);
    close(txsock);
}


int main(int argc, char *argv[])
{
    struct sched_param param = { .sched_priority = 1 };
    pthread_attr_t thattr;
    struct mq_attr mqattr;
    struct sockaddr_can rxaddr, txaddr;
    struct can_filter rxfilter[1];
    struct rtt_stat rtt_stat;
    char mqname[32];
    char *txdev, *rxdev;
    struct can_ifreq ifr;
    int ret, opt;

    struct option long_options[] = {
        { "id", required_argument, 0, 'i'},
        { "cycle", required_argument, 0, 'c'},
        { "repeater", no_argument, 0, 'r'},
        { 0, 0, 0, 0},
    };

    while ((opt = getopt_long(argc, argv, "ri:c:",
                              long_options, NULL)) != -1) {
        switch (opt) {
        case 'c':
            cycle = atoi(optarg);
            break;

        case 'i':
            can_id = strtoul(optarg, NULL, 0);
            break;

        case 'r':
            repeater = 1;
            break;

        default:
            fprintf(stderr, "Unknown option %c\n", opt);
            exit(-1);
        }
    }

    printf("%d %d\n", optind, argc);
    if (optind + 2 != argc) {
        xenomai_usage();
        exit(0);
    }

    txdev = argv[optind];
    rxdev = argv[optind + 1];

    /* Create and configure RX socket */
    if ((rxsock = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
        perror("RX socket failed");
        return -1;
    }

    strncpy(ifr.ifr_name, rxdev, IFNAMSIZ);
    printf("RX rxsock=%d, ifr_name=%s\n", rxsock, ifr.ifr_name);

    if (ioctl(rxsock, SIOCGIFINDEX, &ifr) < 0) {
        perror("RX ioctl SIOCGIFINDEX failed");
        goto failure1;
    }

    /* We only want to receive our own messages */
    rxfilter[0].can_id = can_id;
    rxfilter[0].can_mask = 0x3ff;
    if (setsockopt(rxsock, SOL_CAN_RAW, CAN_RAW_FILTER,
                   &rxfilter, sizeof(struct can_filter)) < 0) {
        perror("RX setsockopt CAN_RAW_FILTER failed");
        goto failure1;
    }
    memset(&rxaddr, 0, sizeof(rxaddr));
    rxaddr.can_ifindex = ifr.ifr_ifindex;
    rxaddr.can_family = AF_CAN;
    if (bind(rxsock, (struct sockaddr *)&rxaddr, sizeof(rxaddr)) < 0) {
        perror("RX bind failed\n");
        goto failure1;
    }

    /* Create and configure TX socket */

    if (strcmp(rxdev, txdev) == 0) {
        txsock = rxsock;
    } else {
        if ((txsock = socket(PF_CAN, SOCK_RAW, 0)) < 0) {
            perror("TX socket failed");
            goto failure1;
        }

        strncpy(ifr.ifr_name, txdev, IFNAMSIZ);
        printf("TX txsock=%d, ifr_name=%s\n", txsock, ifr.ifr_name);

        if (ioctl(txsock, SIOCGIFINDEX, &ifr) < 0) {
            perror("TX ioctl SIOCGIFINDEX failed");
            goto failure2;
        }

        /* Suppress definiton of a default receive filter list */
        if (setsockopt(txsock, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0) < 0) {
            perror("TX setsockopt CAN_RAW_FILTER failed");
            goto failure2;
        }

        memset(&txaddr, 0, sizeof(txaddr));
        txaddr.can_ifindex = ifr.ifr_ifindex;
        txaddr.can_family = AF_CAN;

        if (bind(txsock, (struct sockaddr *)&txaddr, sizeof(txaddr)) < 0) {
                perror("TX bind failed\n");
                goto failure2;
        }
    }

    signal(SIGTERM, catch_signal);
    signal(SIGINT, catch_signal);
    signal(SIGHUP, catch_signal);

    printf("Round-Trip-Time test %s -> %s with CAN ID 0x%x\n",
           argv[optind], argv[optind + 1], can_id);
    printf("Cycle time: %d us\n", cycle);
    printf("All RTT timing figures are in us.\n");

    /* Create statistics message queue */
    snprintf(mqname, sizeof(mqname), "/rtcan_rtt-%d", getpid());
    mqattr.mq_flags   = 0;
    mqattr.mq_maxmsg  = 100;
    mqattr.mq_msgsize = sizeof(struct rtt_stat);
    mq = mq_open(mqname, O_RDWR | O_CREAT | O_EXCL, 0600, &mqattr);
    if (mq == (mqd_t)-1) {
        perror("opening mqueue failed");
        goto failure2;
    }

    /* Create receiver RT-thread */
    pthread_attr_init(&thattr);
    pthread_attr_setdetachstate(&thattr, PTHREAD_CREATE_JOINABLE);
    ret = pthread_create(&rxthread, &thattr, &receiver, NULL);
    if (ret) {
        fprintf(stderr, "%s: pthread_create(receiver) failed\n",
                strerror(-ret));
        goto failure3;
    }

    if (!repeater) {
        /* Create transitter RT-thread */
        ret = pthread_create(&txthread, &thattr, &transmitter, NULL);
        if (ret) {
            fprintf(stderr, "%s: pthread_create(transmitter) failed\n",
                    strerror(-ret));
            goto failure4;
        }
    }

    pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);

    if (repeater)
        printf("Messages\n");
    else
        printf("Messages RTTlast RTT_avg RTT_min RTT_max Overruns\n");

    while (1) {
        long long rtt_avg;

        ret = mq_receive(mq, (char *)&rtt_stat, sizeof(rtt_stat), NULL);
        if (ret != sizeof(rtt_stat)) {
            if (ret < 0) {
                if (errno == EBADF)
                    printf("terminating mq_receive\n");
                else
                    perror("mq_receive failed");
            } else
                fprintf(stderr,
                        "mq_receive returned invalid length %d\n", ret);
            break;
        }

        if (repeater) {
            printf("%8d\n", rxcount);
        } else {
            rtt_avg = ((rtt_stat.rtt_sum - rtt_stat.rtt_sum_last) /
                       rtt_stat.counts_per_sec);
            printf("%8d %7ld %7ld %7ld %7ld %8d\n", rxcount,
                   (long)(rtt_stat.rtt / 1000), (long)(rtt_avg / 1000),
                   (long)(rtt_stat.rtt_min / 1000),
                   (long)(rtt_stat.rtt_max / 1000),
                   overruns);
        }
    }

    /* This call also leaves primary mode, required for socket cleanup. */
    printf("shutting down\n");

    /* Important: First close the sockets! */
    close(rxsock);
    close(txsock);
    pthread_join(txthread, NULL);
    pthread_cancel(rxthread);
    pthread_join(rxthread, NULL);

    return 0;

 failure4:
    pthread_cancel(rxthread);
    pthread_join(rxthread, NULL);
 failure3:
    mq_close(mq);
 failure2:
    close(txsock);
 failure1:
    close(rxsock);

    return 1;
}