/*
* Busy wait spinning until we reach (or slightly pass) the desired time.
* Optionally return the current time as retrieved on the last time check
* to the caller. Optionally also increment a counter provided by the
* caller each time we loop.
*/
static int
wait_time(struct timespec ts, struct timespec *wakeup_ts, long long *waited)
{
struct timespec curtime;
curtime.tv_sec = 0;
curtime.tv_nsec = 0;
if (clock_gettime(CLOCK_REALTIME, &curtime) == -1) {
perror("clock_gettime");
return (-1);
}
#if 0
if (timespec_ge(&curtime, &ts))
printf("warning: wait_time missed deadline without spinning\n");
#endif
while (timespec_ge(&ts, &curtime)) {
if (waited != NULL)
(*waited)++;
if (clock_gettime(CLOCK_REALTIME, &curtime) == -1) {
perror("clock_gettime");
return (-1);
}
}
if (wakeup_ts != NULL)
*wakeup_ts = curtime;
return (0);
}
void keyretrigger(usbdevice* kb){
// Repeat the key as many times as needed to catch up
struct timespec time;
clock_gettime(CLOCK_MONOTONIC, &time);
while(!(kb->lastkeypress & (SCAN_SILENT | SCAN_MOUSE)) && timespec_ge(time, kb->keyrepeat))
_keyretrigger(kb);
}
static const void *inject_drive_command(const void *buf)
{
const int RESET_TIMES = 10;
//static struct timespec latest_tag_time;
//static uint64_t latest_tag;
static int reset_counter;
struct timespec now;
//uint64_t tag;
if (!shm)
return buf;
if (!watchdog_active())
return buf;
clock_gettime(CLOCK_MONOTONIC, &now);
//tag = ((const struct drive_command *)buf)->tag;
//if (latest_tag < tag) {
// latest_tag = tag;
// latest_tag_time = now;
// return buf;
//}
static struct drive_command cmd;
static struct timespec last_deadline;
struct timespec deadline;
pthread_mutex_lock(&shm->cmd_lock);
deadline = shm->cmd_time;
if (!timespec_eq(&deadline, &last_deadline)) {
last_deadline = deadline;
cmd = shm->cmd;
reset_counter = RESET_TIMES;
}
pthread_mutex_unlock(&shm->cmd_lock);
if (timespec_ge(&now, &deadline)) {
if (reset_counter <= 0)
return buf;
cmd.lin_vel = 0;
cmd.ang_vel = 0;
cmd.lin_acc = 0;
cmd.ang_acc = 0;
cmd.lin_lim = 0;
cmd.ang_lim = 0;
reset_counter--;
}
//uint64_t msecs = timespec_diff_msec(&latest_tag_time, &now);
//cmd.tag = ((latest_tag >> 33) + msecs) << 33;
cmd.tag = limiter_tag;
cmd.crc = _docrc(&cmd, sizeof(cmd) - sizeof(cmd.crc));
return &cmd;
}
// Key repeat thread
void* krthread(void* context){
while(1){
// Re-scan every 1ms
usleep(1000);
struct timespec time;
clock_gettime(CLOCK_MONOTONIC, &time);
for(int i = 1; i < DEV_MAX; i++){
if(IS_CONNECTED(keyboard + i)){
// Scan all connected devices
pthread_mutex_lock(&keyboard[i].keymutex);
// Repeat the key as many times as needed to catch up
while(keyboard[i].lastkeypress != KEY_NONE && timespec_ge(time, keyboard[i].keyrepeat))
keyretrigger(keyboard + i);
pthread_mutex_unlock(&keyboard[i].keymutex);
}
}
}
return 0;
}
static void
main_thread(struct glob_arg *g)
{
int i;
uint64_t prev = 0;
uint64_t count = 0;
double delta_t;
struct timeval tic, toc;
gettimeofday(&toc, NULL);
for (;;) {
struct timeval now, delta;
uint64_t pps, usec, my_count, npkts;
int done = 0;
delta.tv_sec = g->report_interval/1000;
delta.tv_usec = (g->report_interval%1000)*1000;
select(0, NULL, NULL, NULL, &delta);
gettimeofday(&now, NULL);
timersub(&now, &toc, &toc);
my_count = 0;
for (i = 0; i < g->nthreads; i++) {
my_count += targs[i].count;
if (targs[i].used == 0)
done++;
}
usec = toc.tv_sec* 1000000 + toc.tv_usec;
if (usec < 10000)
continue;
npkts = my_count - prev;
pps = (npkts*1000000 + usec/2) / usec;
D("%llu pps (%llu pkts in %llu usec)",
(unsigned long long)pps,
(unsigned long long)npkts,
(unsigned long long)usec);
prev = my_count;
toc = now;
if (done == g->nthreads)
break;
}
timerclear(&tic);
timerclear(&toc);
for (i = 0; i < g->nthreads; i++) {
struct timespec t_tic, t_toc;
/*
* Join active threads, unregister interfaces and close
* file descriptors.
*/
if (targs[i].used)
pthread_join(targs[i].thread, NULL);
close(targs[i].fd);
if (targs[i].completed == 0)
D("ouch, thread %d exited with error", i);
/*
* Collect threads output and extract information about
* how long it took to send all the packets.
*/
count += targs[i].count;
t_tic = timeval2spec(&tic);
t_toc = timeval2spec(&toc);
if (!timerisset(&tic) || timespec_ge(&targs[i].tic, &t_tic))
tic = timespec2val(&targs[i].tic);
if (!timerisset(&toc) || timespec_ge(&targs[i].toc, &t_toc))
toc = timespec2val(&targs[i].toc);
}
/* print output. */
timersub(&toc, &tic, &toc);
delta_t = toc.tv_sec + 1e-6* toc.tv_usec;
if (g->td_body == sender_body)
tx_output(count, g->pkt_size, delta_t);
else
rx_output(count, delta_t);
if (g->dev_type == DEV_NETMAP) {
munmap(g->nmd->mem, g->nmd->req.nr_memsize);
close(g->main_fd);
}
}
/*
* Calculate a second-aligned starting time for the packet stream. Busy
* wait between our calculated interval and dropping the provided packet
* into the socket. If we hit our duration limit, bail.
* We sweep the ports from a->port to a->port_max included.
* If the two ports are the same we connect() the socket upfront, which
* almost halves the cost of the sendto() call.
*/
static int
timing_loop(struct _a *a)
{
struct timespec nexttime, starttime, tmptime;
long long waited;
u_int32_t counter;
long finishtime;
long send_errors, send_calls;
/* do not call gettimeofday more than every 20us */
long minres_ns = 200000;
int ic, gettimeofday_cycles;
int cur_port;
uint64_t n, ns;
if (clock_getres(CLOCK_REALTIME, &tmptime) == -1) {
perror("clock_getres");
return (-1);
}
ns = a->interval.tv_nsec;
if (timespec_ge(&tmptime, &a->interval))
fprintf(stderr,
"warning: interval (%jd.%09ld) less than resolution (%jd.%09ld)\n",
(intmax_t)a->interval.tv_sec, a->interval.tv_nsec,
(intmax_t)tmptime.tv_sec, tmptime.tv_nsec);
/* interval too short, limit the number of gettimeofday()
* calls, but also make sure there is at least one every
* some 100 packets.
*/
if ((long)ns < minres_ns/100)
gettimeofday_cycles = 100;
else
gettimeofday_cycles = minres_ns/ns;
fprintf(stderr,
"calling time every %d cycles\n", gettimeofday_cycles);
if (clock_gettime(CLOCK_REALTIME, &starttime) == -1) {
perror("clock_gettime");
return (-1);
}
tmptime.tv_sec = 2;
tmptime.tv_nsec = 0;
timespec_add(&starttime, &tmptime);
starttime.tv_nsec = 0;
if (wait_time(starttime, NULL, NULL) == -1)
return (-1);
nexttime = starttime;
finishtime = starttime.tv_sec + a->duration;
send_errors = send_calls = 0;
counter = 0;
waited = 0;
ic = gettimeofday_cycles;
cur_port = a->port;
if (a->port == a->port_max) {
if (a->ipv6) {
if (connect(a->s, (struct sockaddr *)&a->sin6, sizeof(a->sin6))) {
perror("connect (ipv6)");
return (-1);
}
} else {
if (connect(a->s, (struct sockaddr *)&a->sin, sizeof(a->sin))) {
perror("connect (ipv4)");
return (-1);
}
}
}
while (1) {
int ret;
timespec_add(&nexttime, &a->interval);
if (--ic <= 0) {
ic = gettimeofday_cycles;
if (wait_time(nexttime, &tmptime, &waited) == -1)
return (-1);
}
/*
* We maintain and, if there's room, send a counter. Note
* that even if the error is purely local, we still increment
* the counter, so missing sequence numbers on the receive
* side should not be assumed to be packets lost in transit.
* For example, if the UDP socket gets back an ICMP from a
* previous send, the error will turn up the current send
* operation, causing the current sequence number also to be
* skipped.
* The counter is incremented only on the initial port number,
* so all destinations will see the same set of packets.
*/
if (cur_port == a->port && a->packet_len >= 4) {
be32enc(a->packet, counter);
counter++;
}
if (a->port == a->port_max) { /* socket already bound */
ret = send(a->s, a->packet, a->packet_len, 0);
} else {
a->sin.sin_port = htons(cur_port++);
if (cur_port > a->port_max)
cur_port = a->port;
if (a->ipv6) {
ret = sendto(a->s, a->packet, a->packet_len, 0,
(struct sockaddr *)&a->sin6, sizeof(a->sin6));
} else {
ret = sendto(a->s, a->packet, a->packet_len, 0,
(struct sockaddr *)&a->sin, sizeof(a->sin));
}
}
if (ret < 0)
send_errors++;
send_calls++;
if (a->duration != 0 && tmptime.tv_sec >= finishtime)
goto done;
}
done:
if (clock_gettime(CLOCK_REALTIME, &tmptime) == -1) {
perror("clock_gettime");
return (-1);
}
printf("\n");
printf("start: %jd.%09ld\n", (intmax_t)starttime.tv_sec,
starttime.tv_nsec);
printf("finish: %jd.%09ld\n", (intmax_t)tmptime.tv_sec,
tmptime.tv_nsec);
printf("send calls: %ld\n", send_calls);
printf("send errors: %ld\n", send_errors);
printf("approx send rate: %ld pps\n", (send_calls - send_errors) /
a->duration);
n = send_calls - send_errors;
if (n > 0) {
ns = (tmptime.tv_sec - starttime.tv_sec) * 1000000000UL +
(tmptime.tv_nsec - starttime.tv_nsec);
n = ns / n;
}
printf("time/packet: %u ns\n", (u_int)n);
printf("approx error rate: %ld\n", (send_errors / send_calls));
printf("waited: %lld\n", waited);
printf("approx waits/sec: %lld\n", (long long)(waited / a->duration));
printf("approx wait rate: %lld\n", (long long)(waited / send_calls));
return (0);
}
