X-Git-Url: http://rtime.felk.cvut.cz/gitweb/can-benchmark.git/blobdiff_plain/a3b8b7b78302d32d42d600ee7342552f29a84f06..dc7f5a3374014b178414d75c09b9484403f186cc:/latester/latester.c diff --git a/latester/latester.c b/latester/latester.c index ff627dc..b396fd2 100644 --- a/latester/latester.c +++ b/latester/latester.c @@ -34,6 +34,7 @@ #include "canframelen.h" #include #include +#include #include "histogram.h" @@ -93,6 +94,7 @@ int num_interfaces = 0; int count = 0; /* Number of sent messages */ unsigned msg_in_progress = 0; int completion_pipe[2]; +struct timespec can_tsdiff = {0, 0}; /* can_rx2_ts - can_rx1_ts */ struct msg_info { canid_t id; @@ -101,7 +103,6 @@ struct msg_info { struct timespec ts_rx_onwire, ts_rx_onwire_kern; struct timespec ts_rx_final, ts_rx_final_kern; struct can_frame sent, received; - unsigned lat_measured_us, tx_time_us; }; #define MAX_INFOS 10000 @@ -162,18 +163,27 @@ static inline char *tstamp_str(const void *ctx, struct timespec *tstamp) tstamp->tv_sec, tstamp->tv_nsec/1000); } -int timespec_subtract (struct timespec *result, struct timespec *x, struct timespec *yy); +int timespec_subtract (struct timespec *result, const struct timespec *x, const struct timespec *yy); -void msg_info_print(FILE *f, struct msg_info *mi) +static inline unsigned get_timestamp_diff_us(struct msg_info *mi); + +void msg_info_process(FILE *f, struct msg_info *mi) { - struct timespec diff, routing_delay, tx_time; + struct timespec diff; void *local = talloc_new (NULL); static long num = 0; char sent[64], received[64]; + int diff_us = get_timestamp_diff_us(mi); + int tx_time_us = calc_frame_length(&mi->received); + + /* Update histograms */ + histogram_add(&histogram, diff_us); + histogram_add(&histogram_gw, diff_us - tx_time_us); if (!f) return; + /* Print information to a file */ sprint_canframe(sent, &mi->sent, true); sprint_canframe(received, &mi->received, true); @@ -186,15 +196,10 @@ void msg_info_print(FILE *f, struct msg_info *mi) num, S(mi->ts_sent), sent, S(mi->ts_rx_final_kern), S(mi->ts_rx_final), received, DIFF(mi->ts_sent, mi->ts_rx_final_kern), DIFF(mi->ts_sent, mi->ts_rx_final), - mi->tx_time_us); + tx_time_us); break; case 3: - tx_time.tv_sec = 0; - tx_time.tv_nsec = mi->tx_time_us*1000; - timespec_subtract(&routing_delay, &mi->ts_rx_final_kern, &mi->ts_rx_onwire_kern); - timespec_subtract(&routing_delay, &routing_delay, &tx_time); - - fprintf(f, "%ld: %s %s -> %s (%s) -> %s (%s) %s = %s (%s), %s (%s) %d %s\n", + fprintf(f, "%ld: %s %s -> %s (%s) -> %s (%s) %s = %s (%s), %s (%s) %d %.6f\n", num, S(mi->ts_sent), sent, S(mi->ts_rx_onwire_kern), S(mi->ts_rx_onwire), S(mi->ts_rx_final_kern), S(mi->ts_rx_final), received, @@ -202,8 +207,8 @@ void msg_info_print(FILE *f, struct msg_info *mi) DIFF(mi->ts_sent, mi->ts_rx_onwire), DIFF(mi->ts_rx_onwire_kern, mi->ts_rx_final_kern), DIFF(mi->ts_rx_onwire, mi->ts_rx_final), - mi->tx_time_us, - S(routing_delay)); + tx_time_us, + 1e-6*(diff_us - tx_time_us)); break; } #undef S @@ -215,7 +220,7 @@ void msg_info_print(FILE *f, 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) +int timespec_subtract (struct timespec *result, const struct timespec *x, const struct timespec *yy) { struct timespec ylocal = *yy, *y = &ylocal; /* Perform the carry for the later subtraction by updating Y. */ @@ -245,7 +250,7 @@ void dbg_print_timespec(char *msg, struct timespec *tv) printf("%s sec=%ld nsec=%ld\n", msg, tv->tv_sec, tv->tv_nsec); } -static inline void calc_msg_latencies(struct msg_info *mi) +static inline unsigned get_timestamp_diff_us(struct msg_info *mi) { struct timespec diff; switch (num_interfaces) { @@ -262,10 +267,10 @@ static inline void calc_msg_latencies(struct msg_info *mi) timespec_subtract(&diff, &mi->ts_rx_final_kern, &mi->ts_sent); break; default: - return; + return 0; } - mi->lat_measured_us = diff.tv_sec * 1000000 + diff.tv_nsec/1000; - mi->tx_time_us = calc_frame_length(&mi->received); + unsigned us = diff.tv_sec * 1000000 + diff.tv_nsec/1000; + return us; } void set_sched_policy_and_prio(int policy, int rtprio) @@ -302,7 +307,7 @@ static inline int sock_get_if_index(int s, const char *if_name) static inline void get_tstamp(struct timespec *ts) { - clock_gettime(CLOCK_REALTIME, ts); + clock_gettime(CLOCK_MONOTONIC/*REALTIME*/, ts); } @@ -471,6 +476,7 @@ void receive(int s, struct can_frame *frame, struct timespec *ts_kern, struct ti struct sockaddr_can addr; int nbytes; static uint64_t dropcnt = 0; + bool have_hwtstamp = 0; iov.iov_base = frame; msg.msg_name = &addr; @@ -496,8 +502,15 @@ void receive(int s, struct can_frame *frame, struct timespec *ts_kern, struct ti for (cmsg = CMSG_FIRSTHDR(&msg); cmsg && (cmsg->cmsg_level == SOL_SOCKET); cmsg = CMSG_NXTHDR(&msg,cmsg)) { - if (cmsg->cmsg_type == SO_TIMESTAMPNS) - memcpy(ts_kern, CMSG_DATA(cmsg), sizeof(struct timespec)); + if (cmsg->cmsg_type == SO_TIMESTAMPNS) { + if(!have_hwtstamp) + memcpy(ts_kern, CMSG_DATA(cmsg), sizeof(struct timespec)); + } + else if (cmsg->cmsg_type == SO_TIMESTAMPING) { + struct timespec *ts = (struct timespec *) CMSG_DATA(cmsg); + memcpy(ts_kern, &ts[2], sizeof(struct timespec)); + have_hwtstamp = 1; + } else if (cmsg->cmsg_type == SO_RXQ_OVFL) { uint32_t ovfl; memcpy(&ovfl, CMSG_DATA(cmsg), sizeof(ovfl)); @@ -540,16 +553,67 @@ void process_final_rx(int s) mi->ts_rx_final = ts_user; mi->received = frame; - calc_msg_latencies(mi); - - histogram_add(&histogram, mi->lat_measured_us); - histogram_add(&histogram_gw, mi->lat_measured_us - mi->tx_time_us); - ret = write(completion_pipe[1], &mi, sizeof(mi)); if (ret == -1) error(1, errno, "completion_pipe write"); } +void determine_can_timing_offset(struct pollfd *pfd, int nifc) +{ + int uiofd; + volatile uint32_t *can_crossbar; + uint32_t oldreg; + struct can_frame frame; + struct timespec ts_rx1, ts_rx2, ts_dummy; + int res; + + if(nifc < 2) + return; + + uiofd = open("/dev/uio0", O_RDWR); + if(uiofd < 0) + { + perror("determine_can_timing_offset: /dev/uio0"); + return; + } + + can_crossbar = (volatile uint32_t *) mmap(NULL, 4096, PROT_READ | PROT_WRITE, MAP_SHARED, uiofd, 0); + if(can_crossbar == MAP_FAILED) + { + perror("mmap"); + goto cleanup1; + } + + // Bit: 20 19 18 17 16 14 13 12 11 10 9 8 7 6 5 4 3 2 1 0 + // STBY E3 E2 E1 E0 I3 I2 I1 I0 C3 C2 C1 C0 + // C*: can* (outer can physical interface) connection line assignment (0-3) + // I*: ifc* (inner can network interface) connection line assignment (0-3) + // E*: can* output enable: 1 = connect to physical interface, 0 = disconnect from physical interface, conenect RX and TX together + // STBY: CAN Transceivers standby mode + oldreg = can_crossbar[0]; + can_crossbar[0] = 0x00000000; // connect all interfaces together, disable output, disable standby + + frame.can_id = 0x00; + frame.can_dlc = 4; + memcpy(frame.data, "SYNC", 4); + res = write(pfd[0].fd, &frame, sizeof(frame)); + if(res != sizeof(frame)) + { + perror("write"); + goto cleanup2; + } + + receive(pfd[1].fd, &frame, &ts_rx1, &ts_dummy); + receive(pfd[2].fd, &frame, &ts_rx2, &ts_dummy); + timespec_subtract(&can_tsdiff, &ts_rx2, &ts_rx1); + +cleanup2: + can_crossbar[0] = oldreg; + munmap((void*)can_crossbar, 4096); +cleanup1: + close(uiofd); +} + void *measure_thread(void *arg) { int s, i, ret; @@ -579,10 +643,20 @@ void *measure_thread(void *arg) if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) error(1, errno, "bind"); - const int timestamp_on = 1; - if (setsockopt(s, SOL_SOCKET, SO_TIMESTAMPNS, - ×tamp_on, sizeof(timestamp_on)) < 0) - error(1, errno, "setsockopt SO_TIMESTAMP"); + // TODO: set HW TSTAMP flags? + const int so_timestamping_flags = SOF_TIMESTAMPING_RAW_HARDWARE; + if (setsockopt(s, SOL_SOCKET, SO_TIMESTAMPING, + &so_timestamping_flags, + sizeof(so_timestamping_flags)) < 0) + { + perror("setsockopt SO_TIMESTAMPING"); + fprintf(stderr, "Falling back to SW timestamps.\n"); + + const int timestamp_on = 1; + if (setsockopt(s, SOL_SOCKET, SO_TIMESTAMPNS, + ×tamp_on, sizeof(timestamp_on)) < 0) + error(1, errno, "setsockopt SO_TIMESTAMPNS"); + } const int dropmonitor_on = 1; if (setsockopt(s, SOL_SOCKET, SO_RXQ_OVFL, @@ -596,6 +670,9 @@ void *measure_thread(void *arg) pfd[i].events = POLLIN; } + determine_can_timing_offset(pfd, num_interfaces); + + set_sched_policy_and_prio(SCHED_FIFO, 40); #define SEND() send_and_check(pfd[0].fd) @@ -814,7 +891,8 @@ int main(int argc, const char *argv[]) ret = read(completion_pipe[0], &mi, sizeof(mi)); if (ret < sizeof(mi)) error(1, errno, "read completion returned %d", ret); - msg_info_print(opt.f_msgs, mi); + + msg_info_process(opt.f_msgs, mi); msg_info_free(mi); completed++; } @@ -894,6 +972,8 @@ int main(int argc, const char *argv[]) histogram_stats(&histogram_gw, &hs); double avg = hs.count ? (double)hs.sum/hs.count : 0; fprintf(opt.f_stat, "avg=%g\n", avg); + fprintf(opt.f_stat, "out_of_range_below=%d\n", hs.below); + fprintf(opt.f_stat, "out_of_range_above=%d\n", hs.above); for (i = 0; i <= 20; i++) fprintf(opt.f_stat, "percentile%d=%d\n", i*5, hs.percentile[i*5]);