Usable version of latester
authorMichal Sojka <sojkam1@fel.cvut.cz>
Mon, 29 Nov 2010 22:03:58 +0000 (23:03 +0100)
committerMichal Sojka <sojkam1@fel.cvut.cz>
Mon, 29 Nov 2010 22:03:58 +0000 (23:03 +0100)
latester/latester.c

index 8d38540..4bf186e 100644 (file)
@@ -58,14 +58,17 @@ struct options {
        unsigned oneattime;
        char *file;
        char *histogram_fn;
-} opt = {
+};
+
+struct options opt = {
        .id = 10,
        .period_us = 0,
        .timeout_ms = 1000,
-       
+
 };
 
 int num_interfaces = 0;
+int count = 0;                 /* Number of sent messages */
 
 struct msg_info {
        canid_t id;
@@ -77,7 +80,6 @@ struct msg_info {
 
 #define MAX_INFOS 10000
 struct msg_info msg_infos[MAX_INFOS];
-uint16_t curr_msg = 0;
 
 struct histogram histogram;
 
@@ -103,15 +105,15 @@ void print_msg_info(struct msg_info *mi)
 {
        struct timespec diff;
        void *local = talloc_new (NULL);
-       
+
 #define S(ts) tstamp_str(local, &ts)
 #define DIFF(a, b) (timespec_subtract(&diff, &b, &a), S(diff))
-       
-       if (num_interfaces == 2) 
+
+       if (num_interfaces == 2)
                printf("%s -> %s (%s) = %s (%s)\n",
                       S(mi->ts_sent), S(mi->ts_rx_final_kern), S(mi->ts_rx_final),
-                      DIFF(mi->ts_sent, mi->ts_rx_onwire_kern),
-                      DIFF(mi->ts_sent, mi->ts_rx_onwire));
+                      DIFF(mi->ts_sent, mi->ts_rx_final_kern),
+                      DIFF(mi->ts_sent, mi->ts_rx_final));
        else
                printf("%s -> %s (%s) -> %s (%s) = %s (%s), %s (%s)\n",
                       S(mi->ts_sent),
@@ -129,7 +131,7 @@ void print_msg_info(struct msg_info *mi)
 
 /* Subtract the `struct timespec' values X and Y, storing the result in
    RESULT.  Return 1 if the difference is negative, otherwise 0.  */
-     
+
 int timespec_subtract (struct timespec *result, struct timespec *x, struct timespec *yy)
 {
        struct timespec ylocal = *yy, *y = &ylocal;
@@ -144,12 +146,12 @@ int timespec_subtract (struct timespec *result, struct timespec *x, struct times
                y->tv_nsec += 1000000000 * nsec;
                y->tv_sec -= nsec;
        }
-     
+
        /* Compute the time remaining to wait.
           `tv_nsec' is certainly positive. */
        result->tv_sec = x->tv_sec - y->tv_sec;
        result->tv_nsec = x->tv_nsec - y->tv_nsec;
-     
+
        /* Return 1 if result is negative. */
        return x->tv_sec < y->tv_sec;
 }
@@ -163,7 +165,10 @@ void dbg_print_timespec(char *msg, struct timespec *tv)
 static inline unsigned get_msg_latency_us(struct msg_info *mi)
 {
        struct timespec diff;
-       timespec_subtract(&diff, &mi->ts_rx_final_kern, &mi->ts_rx_onwire_kern);
+       if (num_interfaces == 3)
+               timespec_subtract(&diff, &mi->ts_rx_final_kern, &mi->ts_rx_onwire_kern);
+       else
+               timespec_subtract(&diff, &mi->ts_rx_final_kern, &mi->ts_sent);
        return diff.tv_sec * 1000000 + diff.tv_nsec/1000;
 }
 
@@ -192,7 +197,7 @@ static inline int sock_get_if_index(int s, const char *if_name)
 {
        struct ifreq ifr;
        MEMSET_ZERO(ifr);
-       
+
        strcpy(ifr.ifr_name, if_name);
        if (ioctl(s, SIOCGIFINDEX, &ifr) < 0)
                error(1, errno, "SIOCGIFINDEX '%s'", if_name);
@@ -209,24 +214,47 @@ int send_frame(int socket)
        struct can_frame frame;
        struct msg_info *mi;
        int ret;
+       static int curr_msg = -1;
+       int i;
+       uint16_t idx;
+       
+       i = curr_msg+1;
+       while (msg_infos[i].id != 0 && i != curr_msg) {
+               i++;
+               if (i >= MAX_INFOS)
+                       i = 0;
+       }
+       if (i == curr_msg)
+               error(1, 0, "Msg info table is full! Probably, many packets were lost.");
+       else
+               curr_msg = i;
+
 
        frame.can_id = opt.id;
        frame.can_dlc = 2;
-       memcpy(frame.data, &curr_msg, sizeof(curr_msg));
+       idx = curr_msg;
+       memcpy(frame.data, &idx, sizeof(idx));
        mi = frame2info(&frame);
 
+       mi->id = frame.can_id;
+       mi->length = frame.can_dlc;
        get_tstamp(&mi->ts_sent);
        ret = write(socket, &frame, sizeof(frame));
        return ret;
 }
 
+void msg_info_free(struct msg_info *mi)
+{
+       mi->id = 0;
+}
+
 static inline void get_next_timeout(struct timespec *timeout)
 {
        struct timespec now;
        static struct timespec last = {-1, 0 };
 
        clock_gettime(CLOCK_MONOTONIC, &now);
-       
+
        if (last.tv_sec == -1)
                last = now;
        if (opt.period_us != 0) {
@@ -235,7 +263,7 @@ static inline void get_next_timeout(struct timespec *timeout)
                while (last.tv_nsec >= 1000000000) {
                        last.tv_nsec -= 1000000000;
                        last.tv_sec++;
-               }               
+               }
                timespec_subtract(timeout, &last, &now);
        } else if (opt.timeout_ms != 0) {
                timeout->tv_sec = opt.timeout_ms/1000;
@@ -263,7 +291,7 @@ void receive(int s, struct can_frame *frame, struct timespec *ts_kern, struct ti
        /* these settings may be modified by recvmsg() */
        iov.iov_len = sizeof(*frame);
        msg.msg_namelen = sizeof(addr);
-       msg.msg_controllen = sizeof(ctrlmsg);  
+       msg.msg_controllen = sizeof(ctrlmsg);
        msg.msg_flags = 0;
 
        nbytes = recvmsg(s, &msg, 0);
@@ -316,8 +344,9 @@ void process_final_rx(int s)
        if (opt.histogram_fn)
                histogram_add(&histogram, get_msg_latency_us(mi));
 
-       if (!opt.file)
+       if (0 && !opt.file)
                print_msg_info(mi);
+       msg_info_free(mi);
 }
 
 void *measure_thread(void *arg)
@@ -327,11 +356,10 @@ void *measure_thread(void *arg)
        struct timespec timeout;
        struct sockaddr_can addr;
        sigset_t set;
-       int count = 0;
        unsigned msg_in_progress = 0;
 
        MEMSET_ZERO(pfd);
-       
+
        for (i=0; i<num_interfaces; i++) {
                if ((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0)
                        error(1, errno, "socket");
@@ -356,10 +384,10 @@ void *measure_thread(void *arg)
                               &timestamp_on, sizeof(timestamp_on)) < 0)
                        error(1, errno, "setsockopt SO_TIMESTAMP");
 
-//             const int dropmonitor_on = 1;
-//             if (setsockopt(s, SOL_SOCKET, SO_RXQ_OVFL,
-//                            &dropmonitor_on, sizeof(dropmonitor_on)) < 0)
-//                     error(1, errno, "setsockopt SO_RXQ_OVFL not supported by your Linux Kernel");
+               const int dropmonitor_on = 1;
+               if (setsockopt(s, SOL_SOCKET, SO_RXQ_OVFL,
+                              &dropmonitor_on, sizeof(dropmonitor_on)) < 0)
+                       error(1, errno, "setsockopt SO_RXQ_OVFL not supported by your Linux Kernel");
 
                pfd[i].fd = s;
                if (i == 0)
@@ -371,10 +399,10 @@ void *measure_thread(void *arg)
        set_sched_policy_and_prio(SCHED_FIFO, 99);
 
 #define SEND()                                         \
-       do {    printf("send\n");                       \
+       do {                                            \
                ret = send_frame(pfd[0].fd);            \
                if (ret != sizeof(struct can_frame))    \
-                       error(1, errno, "send_frame %d", __LINE__);     \
+                       error(1, errno, "send_frame (line %d)", __LINE__); \
                count++;                                \
                msg_in_progress++;                      \
        } while (0)
@@ -388,10 +416,9 @@ void *measure_thread(void *arg)
               (opt.count == 0 || count < opt.count || msg_in_progress != 0)) {
 
                get_next_timeout(&timeout);
-               printf("ppoll");
-               fflush(stdout);
+               //printf("ppoll"); fflush(stdout);
                ret = ppoll(pfd, num_interfaces, &timeout, NULL);
-               printf("=%d\n", ret);
+               //printf("=%d\n", ret);
                switch (ret) {
                case -1: // Error
                        if (!INTERRUPTED_SYSCALL(errno))
@@ -401,9 +428,9 @@ void *measure_thread(void *arg)
                        if (opt.period_us) {
                                if (opt.count == 0 || count < opt.count) {
                                        SEND();
-                               } else {
-                                       error(1, 0, "poll timeout");
                                }
+                       } else {
+                               error(1, 0, "poll timeout");
                        }
                        break;
                default: // Event
@@ -448,7 +475,7 @@ struct poptOption optionsTable[] = {
        { "timeout",'t', POPT_ARG_INT|POPT_ARGFLAG_SHOW_DEFAULT,  &opt.timeout_ms,0, "Timeout when period is zero", "ms"},
        { "oneattime",'o', POPT_ARG_NONE,                         &opt.oneattime,0,  "Send the next message only when the previous was finally received"},
        { "verbose",'v', POPT_ARG_NONE,                           NULL, 'v',         "Send the next message only when the previous was finally received"},
-       { "file",   'f', POPT_ARG_STRING,                         &opt.file,    0,   "File where to store results", "filename"},
+//     { "file",   'f', POPT_ARG_STRING,                         &opt.file,    0,   "File where to store results", "filename"},
        { "histogram", 'h', POPT_ARG_STRING,                      &opt.histogram_fn, 0, "Store histogram in file", "filename"},
        POPT_AUTOHELP
        { NULL, 0, 0, NULL, 0 }
@@ -458,7 +485,7 @@ int parse_options(int argc, const char *argv[])
 {
        int c;
        poptContext optCon;   /* context for parsing command-line options */
-       
+
        optCon = poptGetContext(NULL, argc, argv, optionsTable, 0);
        //poptSetOtherOptionHelp(optCon, "[OPTIONS]* <port>");
 
@@ -474,7 +501,7 @@ int parse_options(int argc, const char *argv[])
                error(1, 0, "%s: %s\n",
                      poptBadOption(optCon, POPT_BADOPTION_NOALIAS),
                      poptStrerror(c));
-       
+
        if (num_interfaces < 2 || num_interfaces > 3)
                error(1, 0, "-d option must be given exactly 2 or 3 times");
 
@@ -493,16 +520,16 @@ int main(int argc, const char *argv[])
        sigset_t set;
        int ret;
        FILE *fhist;
-       
+
        parse_options(argc, argv);
-                          
+
        mlockall(MCL_CURRENT | MCL_FUTURE);
 
        signal(SIGINT, term_handler);
        signal(SIGTERM, term_handler);
 
        if (opt.histogram_fn) {
-               histogram_init(&histogram, 1000000, 5);
+               histogram_init(&histogram, 1000000, 1);
                fhist = fopen(opt.histogram_fn, "w");
                if (fhist == NULL)
                        error(1, errno, "open %s", opt.histogram_fn);
@@ -511,9 +538,15 @@ int main(int argc, const char *argv[])
 
        pthread_create(&thread, 0, measure_thread, NULL);
 
-/*     while (!finish_flag) { */
-/*             sleep(1); */
-/*     } */
+       while (!finish_flag && (opt.count == 0 || count < opt.count)) {
+               if (1 || opt.file) {
+                       printf("\rMessage %d", count);
+                       fflush(stdout);
+               }
+               usleep(100000);
+       }
+       if (1 || opt.file)
+               printf("\rMessage %d\n", count);
 
        pthread_join(thread, NULL);
 
@@ -521,6 +554,6 @@ int main(int argc, const char *argv[])
                histogram_fprint(&histogram, fhist);
                fclose(fhist);
        }
-       
+
        return 0;
 }