int
fd_epoll_flush_task(fd_epoll_t *fe, struct timeval *tm)
{
int i;
int fd;
fd_item_t *fi;
int *tasks;
int ntask;
int ret = 1;
if (unlikely(!fe || !tm)) {
_FE_ERR("invalid argument\n");
return -1;
}
if (fe->ntask < 1)
return 0;
tasks = fe->tasks;
ntask = fe->ntask;
fe->tasks = fe->tasks1;
fe->tasks1 = tasks;
fe->ntask = 0;
for (i = 0; i < ntask; i++) {
fd = tasks[i];
if (unlikely(fd <= 0)) continue;
//fi = &fe->maps[fd];
fi = fd_epoll_map(fe, fd);
if (unlikely(!fi)) continue;
if (unlikely(!fi->arg1)) continue;
if (unlikely(!fi->arg2)) continue;
if (unlikely(!fi->is_tasked)) continue;
if (unlikely(!fi->iocb)) continue;
/* no time out, process task. */
if (timeval_sub(tm, &fi->timestamp) < 100) {
//printf("====%p fd %d call task\n", fi, fd);
ret = fi->iocb(fd, 0, fi->arg1, fi->arg2);
//printf("====%p fd %d call task finished\n", fi, fd);
}
/* task run success or timeout, add it to epoll event to handle later data */
if (ret > 0) {
fi->is_tasked = 0;
fd_epoll_alloc_update(fe, fd, fi->events, fi->iocb);
continue;
}
/* not recv any data, add to task */
else {
fe->ntask++;
fe->tasks[fe->ntask - 1] = fd;
fi->is_tasked = 1;
}
}
return 0;
}
void BroadcastReceiver::operator() (timeval *pTimeout) throw (FatalError)
{
// only one thread allowed per instance
if(0 == std::atomic_fetch_add(&mNumInstances, 1))
try {
size_t numRemotes = mIpTable.size();
timeval endTime, now;
gettimeofday(&endTime, NULL);
timeval_add(&endTime, pTimeout);
do
{
const Endpoint remote = GetNextRemote(pTimeout);
if(remote.IsValid()) {
numRemotes++;
}
gettimeofday(&now, NULL);
}
while(mIsRunning && timeval_sub(&endTime, &now, pTimeout));
} catch(FatalError& e) {
std::atomic_fetch_sub(&mNumInstances, 1);
throw(e);
}
std::atomic_fetch_sub(&mNumInstances, 1);
}
// Does all the operations to properly update the train
//void tracker_updating_train(train_t* train, char sector, telemetry_t* tel) {
void tracker_updating_train(train_t* train, char sector, telemetry_t* tel) {
struct timeval diff, now, last;
float speed;
static char estimation1_str[20];
static char estimation2_str[20];
struct train_data_t* t;
rt_printf(" sector %d \n", sector);
for (t = tracker_trains; t->train; ++t) {
if (train_get_ID(train) == t->IRsimbolicId) {
t->storedDirection = train_get_direction(train);
//rt_printf(" updated train %d direction \n" , train_get_ID(train));
}
}
char prevsector =train_get_sector(train);
train_set_current_sector(train, sector);
//last = train_get_timestamp(train);
train_get_timestamp(train, &last);
gettimeofday(&now, NULL);
train_set_timestamp(train, &now);
timeval_sub(&diff, &now, &last);
// Prevents negative sectors
speed = (float) sector_lengths[prevsector]
/ ((float) diff.tv_sec + ((float) diff.tv_usec / 1.0E6));
train_set_current_speed(train, speed);
train_set_time_estimation(train,
(float) sector_lengths[sector] / train_get_speed(train));
//rt_printf(" updated train %d, speed %d sector %c \n",
// train_get_ID(train), train_get_speed(train) , train_get_sector(train));
}
/* There were num_tasks running between prev_tasks_time and now. */
static void update_tasks_sum(struct lbalance *lb,
const struct timeval *now)
{
lb->tasks_sum += timeval_sub(*now, lb->prev_tasks_time)
* lb->num_tasks;
lb->prev_tasks_time = *now;
}
void lbalance_task_free(struct lbalance_task *task,
const struct rusage *usage)
{
float work_done, duration;
unsigned int num_tasks;
struct timeval now;
struct rusage ru;
gettimeofday(&now, NULL);
duration = timeval_sub(now, task->start);
getrusage(RUSAGE_CHILDREN, &ru);
if (usage) {
work_done = usage->ru_utime.tv_usec + usage->ru_stime.tv_usec
+ (usage->ru_utime.tv_sec + usage->ru_stime.tv_sec)
* 1000000;
} else {
/* Take difference in rusage as rusage of that task. */
work_done = timeval_sub(ru.ru_utime,
task->lb->prev_usage.ru_utime)
+ timeval_sub(ru.ru_stime,
task->lb->prev_usage.ru_utime);
}
/* Update previous usage. */
task->lb->prev_usage = ru;
/* Record that we ran num_tasks up until now. */
update_tasks_sum(task->lb, &now);
/* So, on average, how many tasks were running during this time? */
num_tasks = (task->lb->tasks_sum - task->tasks_sum_start)
/ duration + 0.5;
/* Record the work rate for that many tasks. */
add_to_stats(task->lb, num_tasks, work_done / duration);
/* We throw away old stats. */
degrade_stats(task->lb);
/* We need to recalculate the target. */
task->lb->target_uptodate = false;
/* Remove this task. */
tlist_del_from(&task->lb->tasks, task, list);
task->lb->num_tasks--;
free(task);
}
static void try_display(struct timeval const *now)
{
if (timeval_is_set(&last_display) && timeval_sub(now, &last_display) < refresh_rate) return;
last_display = *now;
do_display(now);
dup_reset();
}
/* Function that checks if sleep time is over and assigne new sleep time. */
static int check_time(systime_t *sleep, const uint32_t freq) {
timeval_t now = get_uptime();
timeval_t diff = timeval_sub(&now, &start);
if (tv2st(diff) < (*sleep))
return 1;
(*sleep) += (rand() & freq);
return 0;
}
/**
* Return the number of milliseconds the historic time value was before now.
*/
uint32_t timeval_durationBeforeNow(struct timeval *a) {
struct timeval b;
gettimeofday(&b, NULL);
struct timeval delta = timeval_sub(&b, a);
if (delta.tv_sec < 0) {
return 0;
}
return timeval_toMsecs(&delta);
} // timeval_durationBeforeNow
static void
_ipmidetectd_loop (void)
{
struct pollfd *pfds = NULL;
unsigned int i;
_ipmidetectd_setup ();
assert (nodes_count);
/* +1 fd for the server fd */
if (!(pfds = (struct pollfd *)malloc ((fds_count + 1)*sizeof (struct pollfd))))
err_exit ("malloc: %s", strerror (errno));
while (exit_flag)
{
struct timeval now, timeout;
unsigned int timeout_ms;
int num;
if (gettimeofday (&now, NULL) < 0)
err_exit ("gettimeofday: %s", strerror (errno));
if (timeval_gt (&now, &ipmidetectd_next_send))
{
_ipmidetectd_send_pings ();
if (gettimeofday (&now, NULL) < 0)
err_exit ("gettimeofday: %s", strerror (errno));
timeval_add_ms (&now, conf.ipmiping_period, &ipmidetectd_next_send);
}
_setup_pfds (pfds);
timeval_sub (&ipmidetectd_next_send, &now, &timeout);
timeval_millisecond_calc (&timeout, &timeout_ms);
if ((num = poll (pfds, fds_count + 1, timeout_ms)) < 0)
err_exit ("poll: %s", strerror (errno));
if (num)
{
for (i = 0; i < fds_count; i++)
{
if (pfds[i].revents & POLLERR)
_receive_ping (fds[i]);
else if (pfds[i].revents & POLLIN)
_receive_ping (fds[i]);
}
if (pfds[fds_count].revents & POLLIN)
_send_ping_data ();
}
}
}
/**
* Return the number of milliseconds until future time value.
*/
uint32_t timeval_durationFromNow(struct timeval *a) {
struct timeval b;
gettimeofday(&b, NULL);
struct timeval delta = timeval_sub(a, &b);
if (delta.tv_sec < 0) {
return 0;
}
return timeval_toMsecs(&delta);
// assuming that a is later than b, then the result is a-b
} // timeval_durationFromNow
static ssize_t aps_irq_perf_show(struct device *dev,
struct device_attribute *attr, char *buf)
{
struct aps_ts_info *info = dev_get_drvdata(dev);
struct aps_irq_perf *d = info->perf_irq_data;
int offset = 0;
int i;
int prev_j;
if (!d)
return scnprintf(buf, PAGE_SIZE,
"No performance info available.\n");
offset += scnprintf(buf + offset, PAGE_SIZE - offset,
"Prev Delta Start End Fingers id x y maj press\n");
for (i = 0; i < info->perf_irq_count && (PAGE_SIZE - offset) > 0; ++i) {
struct timespec delta_prev = {0};
struct timespec delta_irq;
int j = (i + info->perf_irq_first) & PERF_IRQ_MAX_BITS;
if (i != 0)
timeval_sub(&delta_prev, d[j].start, d[prev_j].start);
prev_j = j;
timeval_sub(&delta_irq, d[j].end, d[j].start);
offset += scnprintf(buf + offset, PAGE_SIZE - offset,
"%4ld.%03ld %03ld %4ld.%03ld %4ld.%03ld %d %s\n",
delta_prev.tv_sec%1000,
delta_prev.tv_nsec/1000000,
delta_irq.tv_nsec/1000000,
d[j].start.tv_sec%1000,
d[j].start.tv_nsec/1000000,
d[j].end.tv_sec%1000,
d[j].end.tv_nsec/1000000,
d[j].finger_count,
d[j].debug);
}
return offset;
}
static void callids_2_sdps_timeout(struct timeval const *now)
{
PTHREAD_ASSERT_LOCK(&callids_2_sdps_mutex.mutex);
struct callid_2_sdp *c2s;
while (NULL != (c2s = TAILQ_FIRST(&callids_2_sdps_used))) {
if (likely_(timeval_sub(now, &c2s->last_used) <= CALLID_TIMEOUT * 1000000LL)) break;
SLOG(LOG_DEBUG, "Timeouting callid_2_sdp@%p for callid '%s'", c2s, c2s->call_id);
callid_2_sdp_del(c2s);
}
}
/**
* @brief
* Checks whether value 1 comes after value2
* If yes returns 1 else returns 0
* @param value1
* Time value1
* @param value2
* Time value 2
* @return
* 1 - If value1 comes after value2
* 0 - If value2 comes after value1
*/
int timeval_after(const timeval_t *value1, const timeval_t *value2)
{
timeval_t diff;
diff.tval64 = timeval_sub(value1, value2);
if(diff.tval.sec > 0 ||
((diff.tval.sec == 0) && (diff.tval.nsec > 1000)))
return 1;
else
return 0;
}
static void capfile_may_rotate(struct capfile *capfile, struct timeval const *now)
{
mutex_lock(&capfile->lock);
if (
(capfile->max_pkts && capfile->nb_pkts >= capfile->max_pkts) ||
(capfile->max_size && capfile->file_size >= capfile->max_size) ||
(capfile->max_secs && timeval_sub(now, &capfile->start) > 1000000LL * capfile->max_secs)
) {
capfile->ops->close(capfile);
if (capfile->rotation) { // reopens the capfile
SLOG(LOG_DEBUG, "Rotating capfile of %u packets", capfile->nb_pkts);
capfile->ops->open(capfile, capfile_path(capfile), now);
}
}
mutex_unlock(&capfile->lock);
}
int sound_stop()
{
int ret = 0;
struct generic_receiver *receiver = NULL;
struct generic_receiver *receiver_next = NULL;
if (!sound.device) {
return -1;
}
if (sound.ev) {
event_free(sound.ev);
}
receiver = sound.receiver;
while(receiver != NULL) {
receiver_next = receiver->next;
sound_remove_receiver(receiver);
receiver = receiver_next;
}
ret = snd_pcm_oss_device_untrigger(sound.device);
if (ret) {
return -1;
}
#ifdef ENABLE_TIMING
timing_helper_free(sound_read);
timing_helper_free(sound_send);
#endif
struct timeval end;
float totaltime;
float fps;
gettimeofday(&end, NULL);
timeval_sub(&end, &(sound.begin), &end);
totaltime = timeval_to_float(&end);
fps = ((float)sound.cbcount) / totaltime;
printf("Current sound connections %d\n", sound.connection_now);
printf("Device running time: %.06f seconds\n", totaltime);
printf("Statistis: %d frame, avg %2.2f fps\n", sound.cbcount, fps);
return 0;
}
/* Usually called from the timer callback of @progress->timer. */
void
update_progress(struct progress *progress, off_t loaded, off_t size, off_t pos)
{
off_t bytes_delta;
timeval_T now, elapsed, dis_b_max, dis_b_interval;
timeval_now(&now);
timeval_sub(&elapsed, &progress->last_time, &now);
timeval_copy(&progress->last_time, &now);
progress->loaded = loaded;
bytes_delta = progress->loaded - progress->last_loaded;
progress->last_loaded = progress->loaded;
timeval_add_interval(&progress->elapsed, &elapsed);
timeval_add_interval(&progress->dis_b, &elapsed);
timeval_from_milliseconds(&dis_b_max, mult_ms(SPD_DISP_TIME, CURRENT_SPD_SEC));
timeval_from_milliseconds(&dis_b_interval, SPD_DISP_TIME);
while (timeval_cmp(&progress->dis_b, &dis_b_max) >= 0) {
progress->cur_loaded -= progress->data_in_secs[0];
memmove(progress->data_in_secs, progress->data_in_secs + 1,
sizeof(*progress->data_in_secs) * (CURRENT_SPD_SEC - 1));
progress->data_in_secs[CURRENT_SPD_SEC - 1] = 0;
timeval_sub_interval(&progress->dis_b, &dis_b_interval);
}
progress->data_in_secs[CURRENT_SPD_SEC - 1] += bytes_delta;
progress->cur_loaded += bytes_delta;
progress->current_speed = progress->cur_loaded / (CURRENT_SPD_SEC * ((long) SPD_DISP_TIME) / 1000);
progress->pos = pos;
progress->size = size;
if (progress->size != -1 && progress->size < progress->pos)
progress->size = progress->pos;
progress->average_speed = timeval_div_off_t(progress->loaded, &progress->elapsed);
if (progress->average_speed) /* Division by zero risk */
timeval_from_seconds(&progress->estimated_time,
(progress->size - progress->pos) / progress->average_speed);
install_timer(&progress->timer, SPD_DISP_TIME,
progress_timeout, progress);
}
static
void*
snake (void* arg)
{
static int x = 3;
static int y = 3;
struct timeval next_activation;
struct timeval now, timeout;
struct timeval period = { 0, 500000 };
gettimeofday (&next_activation, NULL);
while (1) {
gettimeofday (&now, NULL);
timeval_sub (&timeout, &next_activation, &now);
select (0, NULL, NULL, NULL, &timeout) ;
timeval_add (&next_activation, &next_activation, &period);
x += deltax;
y += deltay;
screen_printxy (x, y, "*");
}
}
int measure_1ms(void)
{
struct timeval tv_start, tv_end, tv_interval;
/* the number of loop that consume 1ms. */
int loop_1ms = 1000; /* just a random initial value. */
retry:
gettimeofday(&tv_start, NULL);
LOOP(loop_1ms);
gettimeofday(&tv_end, NULL);
timeval_sub(&tv_end, &tv_start, &tv_interval);
/* hope it will not deverge... */
if (tv_interval.tv_usec != USEC_1MS) {
/* P-control. */
loop_1ms += Kp * (USEC_1MS - tv_interval.tv_usec);
goto retry;
}
printf("%d times loop consumes %d usec\n",
loop_1ms, tv_interval.tv_usec);
return loop_1ms;
}
void
ipmipower_ping_process_pings (int *timeout)
{
int i, send_pings_flag = 0;
struct timeval cur_time, result;
unsigned int ms_time;
assert (timeout);
if (!cmd_args.common_args.hostname)
return;
if (!cmd_args.ping_interval)
return;
if (gettimeofday (&cur_time, NULL) < 0)
{
IPMIPOWER_ERROR (("gettimeofday: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (timeval_gt (&cur_time, &next_ping_sends_time) || force_discovery_sweep)
{
force_discovery_sweep = 0;
timeval_add_ms (&cur_time, cmd_args.ping_interval, &next_ping_sends_time);
send_pings_flag++;
}
for (i = 0; i < ics_len; i++)
{
uint8_t buf[IPMIPOWER_PACKET_BUFLEN];
int ret, len;
if (send_pings_flag)
{
int dropped = 0;
memset (buf, '\0', IPMIPOWER_PACKET_BUFLEN);
/* deal with packet heuristics */
if (cmd_args.ping_packet_count && cmd_args.ping_percent)
{
if (ics[i].ping_packet_count_send == cmd_args.ping_packet_count)
{
if ((((double)(ics[i].ping_packet_count_send - ics[i].ping_packet_count_recv))/ics[i].ping_packet_count_send) > ((double)cmd_args.ping_percent/100))
ics[i].link_state = IPMIPOWER_LINK_STATE_BAD;
else
ics[i].link_state = IPMIPOWER_LINK_STATE_GOOD;
ics[i].ping_packet_count_send = 0;
ics[i].ping_packet_count_recv = 0;
}
}
if (cmd_args.ping_consec_count)
{
if (!ics[i].ping_last_packet_recv_flag)
ics[i].ping_consec_count = 0;
ics[i].ping_last_packet_recv_flag = 0;
}
/* must increment count before setting message tag, so we
* can check sequence number correctly later on
*/
ics[i].ping_sequence_number_counter++;
/* Workaround
*
* Some motherboards don't support RMCP ping/pong :-(
*
* Discovered on Intel Windmill, Quanta Winterfell, and Wiwynn Windmill
*/
if (cmd_args.common_args.section_specific_workaround_flags & IPMI_PARSE_SECTION_SPECIFIC_WORKAROUND_FLAGS_IPMIPING)
{
fiid_obj_t obj_rmcp_hdr = NULL;
fiid_obj_t obj_lan_session_hdr = NULL;
fiid_obj_t obj_lan_msg_hdr = NULL;
fiid_obj_t obj_cmd = NULL;
if (!(obj_rmcp_hdr = fiid_obj_create (tmpl_rmcp_hdr)))
{
IPMIPOWER_ERROR (("fiid_obj_create: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (!(obj_lan_session_hdr = fiid_obj_create (tmpl_lan_session_hdr)))
{
IPMIPOWER_ERROR (("fiid_obj_create: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (!(obj_lan_msg_hdr = fiid_obj_create (tmpl_lan_msg_hdr_rq)))
{
IPMIPOWER_ERROR (("fiid_obj_create: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (!(obj_cmd = fiid_obj_create (tmpl_cmd_get_channel_authentication_capabilities_rq)))
{
IPMIPOWER_ERROR (("fiid_obj_create: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (fill_rmcp_hdr_ipmi (obj_rmcp_hdr) < 0)
{
IPMIPOWER_ERROR (("fill_rmcp_hdr_ipmi: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (fill_lan_session_hdr (IPMI_AUTHENTICATION_TYPE_NONE,
0,
0,
obj_lan_session_hdr) < 0)
{
IPMIPOWER_ERROR (("fill_lan_session_hdr: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (fill_lan_msg_hdr (IPMI_SLAVE_ADDRESS_BMC,
IPMI_NET_FN_APP_RQ,
IPMI_BMC_IPMB_LUN_BMC,
(ics[i].ping_sequence_number_counter % (IPMI_RQ_SEQ_MAX + 1)),
obj_lan_msg_hdr) < 0)
{
IPMIPOWER_ERROR (("fill_lan_msg_hdr: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (fill_cmd_get_channel_authentication_capabilities (IPMI_CHANNEL_NUMBER_CURRENT_CHANNEL,
IPMI_PRIVILEGE_LEVEL_USER,
IPMI_GET_IPMI_V15_DATA,
obj_cmd) < 0)
{
IPMIPOWER_ERROR (("fill_cmd_get_channel_authentication_capabilities: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if ((len = assemble_ipmi_lan_pkt (obj_rmcp_hdr,
obj_lan_session_hdr,
obj_lan_msg_hdr,
obj_cmd,
NULL,
0,
buf,
IPMIPOWER_PACKET_BUFLEN,
IPMI_INTERFACE_FLAGS_DEFAULT)) < 0)
{
IPMIPOWER_ERROR (("assemble_ipmi_lan_pkt: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
#ifndef NDEBUG
if (cmd_args.rmcpdump)
{
char hdrbuf[DEBUG_UTIL_HDR_BUFLEN];
const char *str_cmd = NULL;
str_cmd = ipmi_cmd_str (IPMI_NET_FN_APP_RQ, IPMI_CMD_GET_CHANNEL_AUTHENTICATION_CAPABILITIES);
debug_hdr_str (DEBUG_UTIL_TYPE_IPMI_1_5,
DEBUG_UTIL_DIRECTION_REQUEST,
DEBUG_UTIL_FLAGS_DEFAULT,
str_cmd,
hdrbuf,
DEBUG_UTIL_HDR_BUFLEN);
if (ipmi_dump_lan_packet (STDERR_FILENO,
ics[i].hostname,
hdrbuf,
NULL,
buf,
len,
tmpl_lan_msg_hdr_rq,
tmpl_cmd_get_channel_authentication_capabilities_rq) < 0)
IPMIPOWER_DEBUG (("ipmi_dump_lan_packet: %s", strerror (errno)));
}
#endif /* NDEBUG */
fiid_obj_destroy (obj_rmcp_hdr);
fiid_obj_destroy (obj_lan_session_hdr);
fiid_obj_destroy (obj_lan_msg_hdr);
fiid_obj_destroy (obj_cmd);
}
else /* !IPMI_PARSE_SECTION_SPECIFIC_WORKAROUND_FLAGS_IPMIPING */
{
fiid_obj_t rmcp_hdr = NULL;
fiid_obj_t rmcp_ping = NULL;
if (!(rmcp_hdr = fiid_obj_create (tmpl_rmcp_hdr)))
{
IPMIPOWER_ERROR (("fiid_obj_create: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (!(rmcp_ping = fiid_obj_create (tmpl_cmd_asf_presence_ping)))
{
IPMIPOWER_ERROR (("fiid_obj_create: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (fill_rmcp_hdr_asf (rmcp_hdr) < 0)
{
IPMIPOWER_ERROR (("fill_rmcp_hdr_asf: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (fill_cmd_asf_presence_ping ((ics[i].ping_sequence_number_counter %
(RMCP_ASF_MESSAGE_TAG_MAX + 1)),
rmcp_ping) < 0)
{
IPMIPOWER_ERROR (("fill_cmd_asf_presence_ping: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if ((len = assemble_rmcp_pkt (rmcp_hdr,
rmcp_ping,
buf,
IPMIPOWER_PACKET_BUFLEN,
IPMI_INTERFACE_FLAGS_DEFAULT)) < 0)
{
IPMIPOWER_ERROR (("assemble_rmcp_pkt: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
#ifndef NDEBUG
if (cmd_args.rmcpdump)
{
char hdrbuf[DEBUG_UTIL_HDR_BUFLEN];
debug_hdr_str (DEBUG_UTIL_TYPE_NONE,
DEBUG_UTIL_DIRECTION_NONE,
DEBUG_UTIL_FLAGS_DEFAULT,
DEBUG_UTIL_RMCPPING_STR,
hdrbuf,
DEBUG_UTIL_HDR_BUFLEN);
if (ipmi_dump_rmcp_packet (STDERR_FILENO,
ics[i].hostname,
hdrbuf,
NULL,
buf,
len,
tmpl_cmd_asf_presence_ping) < 0)
IPMIPOWER_DEBUG (("ipmi_dump_rmcp_packet: %s", strerror (errno)));
}
#endif /* NDEBUG */
fiid_obj_destroy (rmcp_hdr);
fiid_obj_destroy (rmcp_ping);
} /* !IPMI_PARSE_SECTION_SPECIFIC_WORKAROUND_FLAGS_IPMIPING */
if ((ret = cbuf_write (ics[i].ping_out, buf, len, &dropped)) < 0)
{
IPMIPOWER_ERROR (("cbuf_write: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (ret != len)
{
IPMIPOWER_ERROR (("cbuf_write: incorrect bytes written %d", ret));
exit (EXIT_FAILURE);
}
if (dropped)
IPMIPOWER_DEBUG (("cbuf_write: dropped %d bytes", dropped));
ics[i].last_ping_send.tv_sec = cur_time.tv_sec;
ics[i].last_ping_send.tv_usec = cur_time.tv_usec;
if (cmd_args.ping_packet_count && cmd_args.ping_percent)
ics[i].ping_packet_count_send++;
}
/* Did we receive something? */
memset (buf, '\0', IPMIPOWER_PACKET_BUFLEN);
len = ipmipower_cbuf_peek_and_drop (ics[i].ping_in, buf, IPMIPOWER_PACKET_BUFLEN);
if (len > 0)
{
uint8_t message_type = 0, ipmi_supported = 0;
uint64_t val;
/* Workaround
*
* Some motherboards don't support RMCP ping/pong :-(
*
* Discovered on Intel Windmill, Quanta Winterfell, and Wiwynn Windmill
*/
if (cmd_args.common_args.section_specific_workaround_flags & IPMI_PARSE_SECTION_SPECIFIC_WORKAROUND_FLAGS_IPMIPING)
{
fiid_obj_t obj_rmcp_hdr = NULL;
fiid_obj_t obj_lan_session_hdr = NULL;
fiid_obj_t obj_lan_msg_hdr = NULL;
fiid_obj_t obj_cmd = NULL;
fiid_obj_t obj_lan_msg_trlr = NULL;
int checksum_ret = 0;
int unassemble_ret = 0;
int cmd_ret = 0;
if (!(obj_rmcp_hdr = fiid_obj_create (tmpl_rmcp_hdr)))
{
IPMIPOWER_ERROR (("fiid_obj_create: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (!(obj_lan_session_hdr = fiid_obj_create (tmpl_lan_session_hdr)))
{
IPMIPOWER_ERROR (("fiid_obj_create: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (!(obj_lan_msg_hdr = fiid_obj_create (tmpl_lan_msg_hdr_rs)))
{
IPMIPOWER_ERROR (("fiid_obj_create: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (!(obj_cmd = fiid_obj_create (tmpl_cmd_get_channel_authentication_capabilities_rs)))
{
IPMIPOWER_ERROR (("fiid_obj_create: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (!(obj_lan_msg_trlr = fiid_obj_create (tmpl_lan_msg_trlr)))
{
IPMIPOWER_ERROR (("fiid_obj_create: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
#ifndef NDEBUG
if (cmd_args.rmcpdump)
{
char hdrbuf[DEBUG_UTIL_HDR_BUFLEN];
const char *str_cmd = NULL;
str_cmd = ipmi_cmd_str (IPMI_NET_FN_APP_RQ, IPMI_CMD_GET_CHANNEL_AUTHENTICATION_CAPABILITIES);
debug_hdr_str (DEBUG_UTIL_TYPE_IPMI_1_5,
DEBUG_UTIL_DIRECTION_RESPONSE,
DEBUG_UTIL_FLAGS_DEFAULT,
str_cmd,
hdrbuf,
DEBUG_UTIL_HDR_BUFLEN);
if (ipmi_dump_lan_packet (STDERR_FILENO,
ics[i].hostname,
hdrbuf,
NULL,
buf,
len,
tmpl_lan_msg_hdr_rs,
tmpl_cmd_get_channel_authentication_capabilities_rs) < 0)
IPMIPOWER_DEBUG (("ipmi_dump_lan_packet: %s", strerror (errno)));
}
#endif /* NDEBUG */
if ((checksum_ret = ipmi_lan_check_packet_checksum (buf, len)) < 0)
{
IPMIPOWER_ERROR (("ipmi_lan_check_packet_checksum: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (checksum_ret
&& ((unassemble_ret = unassemble_ipmi_lan_pkt (buf,
len,
obj_rmcp_hdr,
obj_lan_session_hdr,
obj_lan_msg_hdr,
obj_cmd,
obj_lan_msg_trlr,
IPMI_INTERFACE_FLAGS_DEFAULT)) < 0))
{
IPMIPOWER_ERROR (("unassemble_ipmi_lan_pkt: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
/* achu: check for cmd type, but don't bother checking
* sequence numbers or completion code. The fact it
* returns is sufficient. We just need to make sure we
* get something back from the BMC to ensure the machine
* is still there.
*/
if (checksum_ret
&& unassemble_ret
&& ((cmd_ret = ipmi_check_cmd (obj_cmd, IPMI_CMD_GET_CHANNEL_AUTHENTICATION_CAPABILITIES)) < 0))
{
IPMIPOWER_ERROR (("ipmi_check_cmd: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (checksum_ret && unassemble_ret && cmd_ret)
{
/* We'll say this is equivalent to what pong response from RMCP */
message_type = RMCP_ASF_MESSAGE_TYPE_PRESENCE_PONG;
ipmi_supported = 1;
}
fiid_obj_destroy (obj_rmcp_hdr);
fiid_obj_destroy (obj_lan_session_hdr);
fiid_obj_destroy (obj_lan_msg_hdr);
fiid_obj_destroy (obj_cmd);
fiid_obj_destroy (obj_lan_msg_trlr);
}
else /* !IPMI_PARSE_SECTION_SPECIFIC_WORKAROUND_FLAGS_IPMIPING */
{
fiid_obj_t rmcp_hdr = NULL;
fiid_obj_t rmcp_pong = NULL;
if (!(rmcp_hdr = fiid_obj_create (tmpl_rmcp_hdr)))
{
IPMIPOWER_ERROR (("fiid_obj_create: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (!(rmcp_pong = fiid_obj_create (tmpl_cmd_asf_presence_pong)))
{
IPMIPOWER_ERROR (("fiid_obj_create: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
#ifndef NDEBUG
if (cmd_args.rmcpdump)
{
char hdrbuf[DEBUG_UTIL_HDR_BUFLEN];
debug_hdr_str (DEBUG_UTIL_TYPE_NONE,
DEBUG_UTIL_DIRECTION_NONE,
DEBUG_UTIL_FLAGS_DEFAULT,
DEBUG_UTIL_RMCPPING_STR,
hdrbuf,
DEBUG_UTIL_HDR_BUFLEN);
if (ipmi_dump_rmcp_packet (STDERR_FILENO,
ics[i].hostname,
hdrbuf,
NULL,
buf,
len,
tmpl_cmd_asf_presence_pong) < 0)
IPMIPOWER_DEBUG (("ipmi_dump_rmcp_packet: %s", strerror (errno)));
}
#endif /* NDEBUG */
if ((ret = unassemble_rmcp_pkt (buf,
len,
rmcp_hdr,
rmcp_pong,
IPMI_INTERFACE_FLAGS_DEFAULT)) < 0)
{
IPMIPOWER_ERROR (("unassemble_rmcp_pkt: %s", strerror (errno)));
exit (EXIT_FAILURE);
}
if (ret)
{
/* achu: check for ipmi_support and pong type, but don't
* check for message tag. On occassion, I have witnessed
* BMCs send message tags "out of sync". For example, you
* send 8, BMC returns 7. You send 9, BMC returns 8. We
* really don't care if the BMC is out of sync. We just
* need to make sure we get something back from the BMC to
* ensure the machine is still there.
*/
if (FIID_OBJ_GET (rmcp_pong,
"message_type",
&val) < 0)
{
IPMIPOWER_ERROR (("FIID_OBJ_GET: 'message_type': %s",
fiid_obj_errormsg (rmcp_pong)));
exit (EXIT_FAILURE);
}
message_type = val;
if (FIID_OBJ_GET (rmcp_pong,
"supported_entities.ipmi_supported",
&val) < 0)
{
IPMIPOWER_ERROR (("FIID_OBJ_GET: 'supported_entities.ipmi_supported': %s",
fiid_obj_errormsg (rmcp_pong)));
exit (EXIT_FAILURE);
}
ipmi_supported = val;
}
fiid_obj_destroy (rmcp_hdr);
fiid_obj_destroy (rmcp_pong);
}
if (message_type == RMCP_ASF_MESSAGE_TYPE_PRESENCE_PONG && ipmi_supported)
{
if (cmd_args.ping_packet_count && cmd_args.ping_percent)
ics[i].ping_packet_count_recv++;
if (cmd_args.ping_consec_count)
{
/* Don't increment twice, its possible a previous pong
* response was late, and we quickly receive two
* pong responses
*/
if (!ics[i].ping_last_packet_recv_flag)
ics[i].ping_consec_count++;
ics[i].ping_last_packet_recv_flag++;
}
if (cmd_args.ping_packet_count && cmd_args.ping_percent)
{
if (ics[i].link_state == IPMIPOWER_LINK_STATE_GOOD)
ics[i].discover_state = IPMIPOWER_DISCOVER_STATE_DISCOVERED;
else
{
if (cmd_args.ping_consec_count
&& ics[i].ping_consec_count >= cmd_args.ping_consec_count)
ics[i].discover_state = IPMIPOWER_DISCOVER_STATE_DISCOVERED;
else
ics[i].discover_state = IPMIPOWER_DISCOVER_STATE_BADCONNECTION;
}
}
else
{
ics[i].discover_state = IPMIPOWER_DISCOVER_STATE_DISCOVERED;
}
ics[i].last_ping_recv.tv_sec = cur_time.tv_sec;
ics[i].last_ping_recv.tv_usec = cur_time.tv_usec;
}
} /* !IPMI_PARSE_SECTION_SPECIFIC_WORKAROUND_FLAGS_IPMIPING */
/* Is the node gone?? */
timeval_sub (&cur_time, &ics[i].last_ping_recv, &result);
timeval_millisecond_calc (&result, &ms_time);
if (ms_time >= cmd_args.ping_timeout)
ics[i].discover_state = IPMIPOWER_DISCOVER_STATE_UNDISCOVERED;
}
timeval_sub (&next_ping_sends_time, &cur_time, &result);
timeval_millisecond_calc (&result, &ms_time);
*timeout = ms_time;
}
int
main (int argc, char **argv)
{
int fd;
int ret, ch, i;
int keyspec, keyx, keyy;
unsigned long long offset, length;
unsigned long level, boffset, nblock;
unsigned long alevel, aboffset, anblock;
int branch;
char *filename = NULL;
char *endptr;
unsigned long leaf_level = HORUS_DEFAULT_LEAF_LEVEL;
struct horus_file_config c;
int dumb_ass_mode = 0;
struct horus_stats stats;
struct timeval start, end, res;
double time;
u_int16_t port;
char *portspec = NULL;
char *server[HORUS_MAX_SERVER_NUM];
struct sockaddr_in serv_addr[HORUS_MAX_SERVER_NUM];
int nservers = 0;
memset (&serv_addr, 0, sizeof (serv_addr));
alevel = anblock = aboffset = 0;
int nthread = 1;
keyspec = 0;
keyx = keyy = 0;
offset = length = 0;
level = boffset = nblock = 0;
progname = (1 ? "kds_client" : argv[0]);
while ((ch = getopt_long (argc, argv, optstring, longopts, NULL)) != -1)
{
switch (ch)
{
case 'v':
horus_verbose++;
break;
case 'd':
horus_debug++;
break;
case 'b':
benchmark++;
break;
case 'n':
nthread = (int) strtol (optarg, &endptr, 0);
if (*endptr != '\0')
{
fprintf (stderr, "invalid \'%c\' in %s\n", *endptr, optarg);
return -1;
}
break;
case 's':
if (nservers >= HORUS_MAX_SERVER_NUM)
break;
server[nservers] = optarg;
portspec = index (optarg, ':');
if (portspec)
*portspec++ = '\0';
serv_addr[nservers].sin_family = AF_INET;
ret = inet_pton (AF_INET, server[nservers],
&serv_addr[nservers].sin_addr);
if (ret != 1)
{
fprintf (stderr, "invalid server address: %s\n", optarg);
return -1;
}
if (portspec)
{
port = (u_int16_t) strtol (portspec, &endptr, 0);
if (*endptr != '\0')
{
fprintf (stderr, "invalid port number \'%c\' in %s\n",
*endptr, portspec);
return -1;
}
serv_addr[nservers].sin_port = htons (port);
}
else
serv_addr[nservers].sin_port = htons (HORUS_KDS_SERVER_PORT);
nservers++;
break;
case 'x':
keyx = strtol (optarg, &endptr, 0);
if (*endptr != '\0')
{
fprintf (stderr, "invalid \'%c\' in %s\n", *endptr, optarg);
return -1;
}
keyspec++;
break;
case 'y':
keyy = strtol (optarg, &endptr, 0);
if (*endptr != '\0')
{
fprintf (stderr, "invalid \'%c\' in %s\n", *endptr, optarg);
return -1;
}
keyspec++;
break;
case 'o':
offset = canonical_byte_size (optarg, &endptr);
if (*endptr != '\0')
{
fprintf (stderr, "invalid \'%c\' in %s\n", *endptr, optarg);
return -1;
}
break;
case 'l':
length = canonical_byte_size (optarg, &endptr);
if (*endptr != '\0')
{
fprintf (stderr, "invalid \'%c\' in %s\n", *endptr, optarg);
return -1;
}
break;
case 'a':
aggregate++;
alevel = strtol (optarg, &endptr, 0);
if (*endptr != '\0')
{
fprintf (stderr, "invalid \'%c\' in %s\n", *endptr, optarg);
return -1;
}
break;
case 'e':
simulate++;
break;
case 'w':
spinwait++;
break;
case 'u':
useconds = (useconds_t) strtol (optarg, &endptr, 0);
if (*endptr != '\0')
{
fprintf (stderr, "invalid \'%c\' in %s\n", *endptr, optarg);
return -1;
}
break;
case 't':
nanoseconds = (long) strtol (optarg, &endptr, 0);
if (*endptr != '\0')
{
fprintf (stderr, "invalid \'%c\' in %s\n", *endptr, optarg);
return -1;
}
break;
case 'r':
nread = (int) strtol (optarg, &endptr, 0);
if (*endptr != '\0')
{
fprintf (stderr, "invalid \'%c\' in %s\n", *endptr, optarg);
return -1;
}
break;
default:
usage ();
break;
}
}
argc -= optind;
argv += optind;
if (argc > 0)
{
filename = argv[0];
argc--;
argv++;
}
if (! filename)
usage ();
if (nthread < 1 || HORUS_THREAD_MAX < nthread)
{
printf ("#threads must be: 1 - %d\n", HORUS_THREAD_MAX);
exit (1);
}
if (nservers == 0)
{
/* default setting */
server[nservers] = HORUS_KDS_SERVER_ADDR;
serv_addr[nservers].sin_family = AF_INET;
ret = inet_pton (AF_INET, server[nservers],
&serv_addr[nservers].sin_addr);
assert (ret == 1);
port = HORUS_KDS_SERVER_PORT;
serv_addr[nservers].sin_port = htons (port);
nservers++;
}
memset (&c, 0, sizeof (c));
fd = open (filename, O_RDONLY);
if (fd < 0)
{
printf ("cannot open file: %s\n", filename);
exit (1);
}
ret = horus_get_file_config (fd, &c);
if (ret < 0)
{
printf ("cannot read Horus config: %s\n", filename);
exit (1);
}
close (fd);
if (! horus_is_valid_config (&c))
{
printf ("invalid Horus config: %s\n", filename);
exit (1);
}
/* calculate leaf level */
for (i = 0; i < HORUS_MAX_KHT_DEPTH; i++)
if (c.kht_block_size[i])
leaf_level = i;
if (horus_verbose)
printf ("leaf level: %lu\n", leaf_level);
thread = (struct thread_info *)
malloc (sizeof (struct thread_info) * nthread);
assert (thread);
memset (thread, 0, sizeof (struct thread_info) * nthread);
/* start point */
if (offset)
{
level = leaf_level;
boffset = (offset ? offset / HORUS_BLOCK_SIZE : 0);
}
/* when key is specified (override the start point by offset) */
if (keyspec)
{
level = keyx;
boffset = keyy;
}
/* length */
nblock = 1;
if (length)
{
level = leaf_level;
nblock = length / HORUS_BLOCK_SIZE;
}
/* when key is specified (override the length) */
if (keyspec && simulate)
{
nblock = 1;
for (level = keyx; level < leaf_level; level++)
{
branch = c.kht_block_size[level] / c.kht_block_size[level + 1];
boffset *= branch;
nblock *= branch;
}
if (horus_verbose)
printf ("range by key spec: K_%d,%d leaf: K_%lu,%lu nblock: %lu\n",
keyx, keyy, level, boffset, nblock);
}
/* aggregate */
if (aggregate)
{
/* calculate range as the aggregate level */
aboffset = boffset;
anblock = nblock;
for (i = level; alevel < i; i--)
{
branch = c.kht_block_size[i - 1] / c.kht_block_size[i];
if (anblock / branch == 0)
break;
anblock = (anblock / branch) + (anblock % branch ? 1 : 0);
aboffset /= branch;
}
alevel = i;
}
if (horus_verbose || benchmark)
{
printf ("nthread: %d\n", nthread);
printf ("filename: %s\n", filename);
if (benchmark)
printf ("benchmark: ");
if (dumb_ass_mode)
printf ("dumb-ass: ");
if (keyspec)
printf ("keyx: %d keyy: %d\n", keyx, keyy);
printf ("level: %lu boffset: %lu nblock: %lu\n",
level, boffset, nblock);
if (aggregate)
printf ("alevel: %lu aboffset: %lu anblock: %lu\n",
alevel, aboffset, anblock);
}
if (benchmark)
gettimeofday (&start, NULL);
for (i = 0; i < nthread; i++)
{
int unit;
thread[i].id = i;
thread[i].fd = fd;
thread[i].filename = filename;
thread[i].server = server[i % nservers];
thread[i].serv_addr = &serv_addr[i % nservers];
thread[i].leaf_level = leaf_level;
//thread[i].kht_block_size = &c.kht_block_size[0];
thread[i].kht_block_size = c.kht_block_size;
if (aggregate)
{
thread[i].level = alevel;
unit = anblock / nthread;
if (i < anblock % nthread)
{
thread[i].boffset = aboffset + unit * i + i;
thread[i].nblock = unit + 1;
}
else
{
thread[i].boffset = aboffset + unit * i + anblock % nthread;
thread[i].nblock = unit;
}
}
else
{
thread[i].level = level;
unit = nblock / nthread;
if (i < nblock % nthread)
{
thread[i].boffset = boffset + unit * i + i;
thread[i].nblock = unit + 1;
}
else
{
thread[i].boffset = boffset + unit * i + nblock % nthread;
thread[i].nblock = unit;
}
}
pthread_create (&thread[i].pthread, NULL,
thread_read_write, (void *) &thread[i]);
}
for (i = 0; i < nthread; i++)
pthread_join (thread[i].pthread, NULL);
if (benchmark)
gettimeofday (&end, NULL);
memset (&stats, 0, sizeof (stats));
for (i = 0; i < nthread; i++)
horus_stats_merge (&stats, &thread[i].stats);
horus_stats_print (&stats);
if (benchmark)
{
timeval_sub (&end, &start, &res);
time = res.tv_sec + res.tv_usec * 0.000001;
printf ("benchmark: overall-requested: %llu keys in %f secs by %f q/s\n",
stats.success, time, stats.success/time);
}
if (benchmark)
{
double qps = 0.0;
unsigned long long total = 0;
for (i = 0; i < nthread; i++)
{
if (thread[i].stats.success)
{
time = thread[i].timeval.tv_sec +
thread[i].timeval.tv_usec * 0.000001;
total += thread[i].stats.success;
qps += thread[i].stats.success / time;
}
}
printf ("benchmark: per-thread-requested: %llu keys by %f q/s\n",
total, qps);
}
if (benchmark && simulate)
{
double qps = 0.0;
unsigned long long total = 0;
for (i = 0; i < nthread; i++)
{
if (thread[i].stats.keycalculated)
{
time = thread[i].timeval.tv_sec +
thread[i].timeval.tv_usec * 0.000001;
total += thread[i].stats.keycalculated;
qps += thread[i].stats.keycalculated / time;
}
}
printf ("benchmark: per-thread-simulated: %llu leaf keys by %f q/s\n",
total, qps);
}
free (thread);
return 0;
}
static void timeval_sub_check_(struct timeval const *a, struct timeval const *b, int64_t expected)
{
int64_t s = timeval_sub(a, b);
assert(s == expected);
}
void boot_slave( void ) {
int sv[2];
int i;
int maxfds;
char *s;
#ifdef HAVE_GETDTABLESIZE
maxfds = getdtablesize();
#else
maxfds = sysconf( _SC_OPEN_MAX );
#endif
if( slave_socket != -1 ) {
close( slave_socket );
slave_socket = -1;
}
if( socketpair( AF_UNIX, SOCK_DGRAM, 0, sv ) < 0 ) {
return;
}
/*
* set to nonblocking
*/
#ifdef FNDELAY
if( fcntl( sv[0], F_SETFL, FNDELAY ) == -1 ) {
#else
if( fcntl( sv[0], F_SETFL, O_NDELAY ) == -1 ) {
#endif
close( sv[0] );
close( sv[1] );
return;
}
slave_pid = vfork();
switch( slave_pid ) {
case -1:
close( sv[0] );
close( sv[1] );
return;
case 0: /* child */
close( sv[0] );
if( dup2( sv[1], 0 ) == -1 ) {
_exit( 1 );
}
if( dup2( sv[1], 1 ) == -1 ) {
_exit( 1 );
}
for( i = 3; i < maxfds; ++i ) {
close( i );
}
s = ( char * ) XMALLOC( MBUF_SIZE, "boot_slave" );
sprintf( s, "%s/slave", mudconf.binhome );
execlp( s, "slave", NULL );
XFREE( s, "boot_slave" );
_exit( 1 );
}
close( sv[1] );
#ifdef FNDELAY
if( fcntl( sv[0], F_SETFL, FNDELAY ) == -1 ) {
#else
if( fcntl( sv[0], F_SETFL, O_NDELAY ) == -1 ) {
#endif
close( sv[0] );
return;
}
slave_socket = sv[0];
log_write( LOG_ALWAYS, "NET", "SLAVE", "DNS lookup slave started on fd %d", slave_socket );
}
int make_socket( int port ) {
int s, opt;
struct sockaddr_in server;
s = socket( AF_INET, SOCK_STREAM, 0 );
if( s < 0 ) {
log_perror( "NET", "FAIL", NULL, "creating master socket" );
exit( 3 );
}
opt = 1;
if( setsockopt( s, SOL_SOCKET, SO_REUSEADDR,
( char * ) &opt, sizeof( opt ) ) < 0 ) {
log_perror( "NET", "FAIL", NULL, "setsockopt" );
}
server.sin_family = AF_INET;
server.sin_addr.s_addr = INADDR_ANY;
server.sin_port = ( unsigned short ) htons( ( unsigned short ) port );
if( !mudstate.restarting )
if( bind( s, ( struct sockaddr * ) &server, sizeof( server ) ) ) {
log_perror( "NET", "FAIL", NULL, "bind" );
close( s );
exit( 4 );
}
listen( s, 5 );
return s;
}
void shovechars( int port ) {
fd_set input_set, output_set;
struct timeval last_slice, current_time, next_slice, timeout,
slice_timeout;
int found, check;
DESC *d, *dnext, *newd;
int avail_descriptors, maxfds;
struct stat fstatbuf;
#define CheckInput(x) FD_ISSET(x, &input_set)
#define CheckOutput(x) FD_ISSET(x, &output_set)
mudstate.debug_cmd = ( char * ) "< shovechars >";
if( !mudstate.restarting ) {
sock = make_socket( port );
}
if( !mudstate.restarting ) {
maxd = sock + 1;
}
get_tod( &last_slice );
#ifdef HAVE_GETDTABLESIZE
maxfds = getdtablesize();
#else
maxfds = sysconf( _SC_OPEN_MAX );
#endif
avail_descriptors = maxfds - 7;
while( mudstate.shutdown_flag == 0 ) {
get_tod( ¤t_time );
last_slice = update_quotas( last_slice, current_time );
process_commands();
if( mudstate.shutdown_flag ) {
break;
}
/*
* We've gotten a signal to dump flatfiles
*/
if( mudstate.flatfile_flag && !mudstate.dumping ) {
if( *mudconf.dump_msg ) {
raw_broadcast( 0, "%s", mudconf.dump_msg );
}
mudstate.dumping = 1;
log_write( LOG_DBSAVES, "DMP", "CHKPT", "Flatfiling: %s.#%d#", mudconf.db_file, mudstate.epoch );
dump_database_internal( DUMP_DB_FLATFILE );
mudstate.dumping = 0;
if( *mudconf.postdump_msg ) {
raw_broadcast( 0, "%s", mudconf.postdump_msg );
}
mudstate.flatfile_flag = 0;
}
/*
* test for events
*/
dispatch();
/*
* any queued robot commands waiting?
*/
timeout.tv_sec = que_next();
timeout.tv_usec = 0;
next_slice = msec_add( last_slice, mudconf.timeslice );
slice_timeout = timeval_sub( next_slice, current_time );
FD_ZERO( &input_set );
FD_ZERO( &output_set );
/*
* Listen for new connections if there are free descriptors
*/
if( ndescriptors < avail_descriptors ) {
FD_SET( sock, &input_set );
}
/*
* Listen for replies from the slave socket
*/
if( slave_socket != -1 ) {
FD_SET( slave_socket, &input_set );
}
/*
* Mark sockets that we want to test for change in status
*/
DESC_ITER_ALL( d ) {
if( !d->input_head ) {
FD_SET( d->descriptor, &input_set );
}
if( d->output_head ) {
FD_SET( d->descriptor, &output_set );
}
}
/*
* Wait for something to happen
*/
found = select( maxd, &input_set, &output_set, ( fd_set * ) NULL,
&timeout );
if( found < 0 ) {
if( errno == EBADF ) {
/*
* This one is bad, as it results in a spiral
* of doom, unless we can figure out what the
* bad file descriptor is and get rid of it.
*/
log_perror( "NET", "FAIL", "checking for activity", "select" );
DESC_ITER_ALL( d ) {
if( fstat( d->descriptor,
&fstatbuf ) < 0 ) {
/*
* It's a player. Just toss
* the connection.
*/
log_write( LOG_PROBLEMS, "ERR", "EBADF", "Bad descriptor %d", d->descriptor );
shutdownsock( d,R_SOCKDIED );
}
}
if( ( slave_socket == -1 ) ||
( fstat( slave_socket, &fstatbuf ) < 0 ) ) {
/*
* Try to restart the slave, since it
* presumably died.
*/
log_write( LOG_PROBLEMS, "ERR", "EBADF", "Bad slave descriptor %d", slave_socket );
boot_slave();
}
if( ( sock != -1 ) &&
( fstat( sock, &fstatbuf ) < 0 ) ) {
/*
* That's it, game over.
*/
log_write( LOG_PROBLEMS, "ERR", "EBADF", "Bad game port descriptor %d", sock );
break;
}
} else if( errno != EINTR ) {
log_perror( "NET", "FAIL", "checking for activity", "select" );
}
continue;
}
static unsigned char sReadByte(Serialport * serialport, const unsigned int msTimeout){
pthread_mutex_lock(serialport->mutex);
//
// Get the current time. Throw an exception if we are unable
// to read the current time.
//
struct timeval entry_time ;
if ( gettimeofday( &entry_time, NULL ) < 0 ){
if(serial_log) printf("\nERROR: %s.", (char*)strerror( errno ));
pthread_mutex_unlock(serialport->mutex);
return -1;
}
//
// Wait for data to be available.
//
const int MICROSECONDS_PER_MS = 1000 ;
const int MILLISECONDS_PER_SEC = 1000 ;
//
struct timeval curr_time, elapsed_time ;
unsigned int elapsed_ms;
while( serialport->mInputBuffer_get-serialport->mInputBuffer_put == 0 ){
//
// Read the current time.
//
if ( gettimeofday( &curr_time,NULL ) < 0 ){
if(serial_log) printf("\nERROR: %s.",(char*)strerror( errno ));
pthread_mutex_unlock(serialport->mutex);
return -1;
}
//
// Obtain the elapsed time.
//
elapsed_time = timeval_sub(curr_time,entry_time);
//
// Increase the elapsed number of milliseconds.
//
elapsed_ms = ( elapsed_time.tv_sec * MILLISECONDS_PER_SEC +
elapsed_time.tv_usec / MICROSECONDS_PER_MS ) ;
//
// If more than msTimeout milliseconds have elapsed while
// waiting for data, then we throw a ReadTimeout exception.
//
if ( ( msTimeout > 0 ) && ( elapsed_ms > msTimeout ) ){
//if(serial_log) printf("\nTimeout."); //todo print bom para debug
pthread_mutex_unlock(serialport->mutex);
return -1;
}
//
// Wait for 1ms (1000us) for data to arrive.
//
pthread_mutex_unlock(serialport->mutex);
usleep( MICROSECONDS_PER_MS ) ;
pthread_mutex_lock(serialport->mutex);
}
//
// Return the first byte and remove it from the queue.
//
unsigned char next_char = *(serialport->mInputBuffer_get++);
if(serialport->mInputBuffer_get==serialport->mInputBuffer+INPUTBUFFER_SIZE)
serialport->mInputBuffer_get = serialport->mInputBuffer;
if(next_char>32 && next_char<126){
if(serial_log) printf("R: %x (%d) (%c)\r\n",next_char,next_char,next_char);
}else
if(serial_log) printf("R: %x (%d)\r\n",next_char,next_char);
//if(serial_log) printf("%c",next_char);
pthread_mutex_unlock(serialport->mutex);
return next_char ;
}
void *
thread_read_write (void *arg)
{
struct thread_info *info = (struct thread_info *) arg;
int i, ret;
int id, fd;
/* for request */
u_int32_t reqx, reqy;
unsigned long boffset, nblock;
struct sockaddr_in *serv_addr;
struct key_request_packet kreq;
/* for read */
u_int32_t resx, resy;
struct sockaddr_in addr;
socklen_t addrlen;
struct key_response_packet kresp;
int kresperr, krespsuberr, krespkeylen;
struct timeval start, end, res;
double time;
int success;
unsigned long send_count, read_count;
id = info->id;
memset (&info->stats, 0, sizeof (struct horus_stats));
send_count = read_count = 0;
fd = socket (PF_INET, SOCK_DGRAM, 0);
if (fd < 0)
{
printf ("thread[%d]: Unable to open socket!: %s\n",
id, strerror (errno));
return NULL;
}
serv_addr = info->serv_addr;
boffset = info->boffset;
nblock = info->nblock;
memset (&kreq, 0, sizeof (kreq));
reqx = info->level;
kreq.x = htonl (reqx);
strncpy (kreq.filename, info->filename, sizeof (kreq.filename));
if (benchmark || horus_verbose)
printf ("thread[%d]: server %s:%d bsize %d boffset %lu nblock %lu\n",
id, info->server, ntohs (serv_addr->sin_port),
HORUS_BLOCK_SIZE, boffset, nblock);
if (spinwait)
{
fcntl (fd, F_SETFL, O_NONBLOCK);
printf ("thread[%d]: spinwait: usleep %d nanosleep %ld "
"nsend %d nread %d\n",
id, useconds, nanoseconds, nsend, nread);
}
if (benchmark)
gettimeofday (&start, NULL);
for (i = 0; i < nblock; i++)
{
reqy = boffset + i;
kreq.y = htonl (reqy);
success = 0;
send_count = nsend;
do {
ret = sendto (fd, &kreq, sizeof (key_request_packet), 0,
(struct sockaddr *) serv_addr,
sizeof (struct sockaddr_in));
send_count--;
if (ret != sizeof (key_request_packet))
{
if (horus_debug)
printf ("thread[%d]: sendto(): failed: %d "
"send_count: %ld\n", id, ret, send_count);
info->stats.sendfail++;
continue;
}
else
{
if (horus_debug)
printf ("thread[%d]: request %d,%d send_count: %ld\n",
id, reqx, reqy, send_count);
}
read_count = nread;
do {
if (spinwait)
{
if (useconds)
usleep (useconds);
if (nanoseconds)
{
struct timespec nanospec;
nanospec.tv_sec = 0;
nanospec.tv_nsec = nanoseconds;
nanosleep (&nanospec, NULL);
}
}
addrlen = sizeof (struct sockaddr_in);
ret = recvfrom (fd, &kresp, sizeof (key_response_packet), 0,
(struct sockaddr *) &addr, &addrlen);
read_count--;
if (ret != sizeof (key_response_packet))
{
if (horus_debug)
printf ("thread[%d]: recvfrom(): failed: %d "
"read_count: %ld\n", id, ret, read_count);
info->stats.recvfail++;
continue;
}
else
{
if (horus_debug)
printf ("thread[%d]: recvfrom(): received %d\n", id, ret);
resx = ntohl (kresp.x);
resy = ntohl (kresp.y);
if (resx == reqx && resy == reqy)
success++;
else
{
if (horus_debug)
printf ("thread[%d]: mismatch: "
"req: %d,%d: resp: %d,%d\n",
id, reqx, reqy, resx, resy);
info->stats.resmismatch++;
}
}
} while (! success && read_count > 0);
} while (! success && send_count > 0);
info->stats.sendretry += nsend - send_count - 1;
info->stats.recvretry += nread - read_count - 1;
if (! success)
{
if (horus_verbose)
printf ("thread[%d]: give up K_%d,%d: resend: %lu reread: %lu\n",
id, reqx, reqy, send_count, read_count);
info->stats.giveup++;
continue;
}
info->stats.success++;
kresperr = (int) ntohs (kresp.err);
krespsuberr = (int) ntohs (kresp.suberr);
krespkeylen = (int) ntohl (kresp.key_len);
horus_stats_record (&info->stats, kresperr, krespsuberr);
if (horus_verbose && ! benchmark)
{
if (kresperr)
printf ("thread[%d]: err = %d : %s\n", id,
kresperr, horus_strerror (kresperr));
if (krespsuberr)
printf ("thread[%d]: suberr = %d : %s\n", id,
krespsuberr, strerror (krespsuberr));
if (! kresperr)
printf ("thread[%d]: key_%d,%d: key_%d,%d/%d = %s\n", id,
reqx, reqy, resx, resy, krespkeylen,
print_key (kresp.key, krespkeylen));
}
if (simulate && ! kresperr)
{
char key[HORUS_MAX_KEY_LEN];
size_t key_len;
int simx, simy;
unsigned long sboffset, snblock;
u_int32_t *kht_block_size;
int j;
assert (reqx == resx && reqy == resy);
simx = info->leaf_level;
kht_block_size = info->kht_block_size;
sboffset = resy * (kht_block_size[resx] / kht_block_size[simx]);
snblock = kht_block_size[resx];
if (resx == info->leaf_level)
{
info->stats.keycalculated = info->stats.success;
}
else
{
for (j = 0; j < snblock; j++)
{
simy = sboffset + j;
key_len = sizeof (key);
horus_block_key (key, &key_len, simx, simy,
kresp.key, krespkeylen, resx, resy,
kht_block_size);
info->stats.keycalculated++;
if (horus_verbose && ! benchmark)
printf ("thread[%d]: simulated: K_%d,%d = %s\n", id,
simx, simy, print_key (key, key_len));
}
}
}
}
if (benchmark)
gettimeofday (&end, NULL);
close (fd);
if (benchmark)
{
timeval_sub (&end, &start, &res);
time = res.tv_sec + res.tv_usec * 0.000001;
info->timeval = res;
printf ("thread[%d]: %llu/%lu keys in %f secs ( %f q/s\n",
id, info->stats.success, nblock, time, info->stats.success/time);
if (simulate)
printf ("thread[%d]: %llu keys calculated in %f secs ( %f q/s\n",
id, info->stats.keycalculated, time,
info->stats.keycalculated/time);
}
else if (horus_verbose)
{
printf ("thread[%d]: %llu/%lu keys processed.\n",
id, info->stats.success, nblock);
if (simulate)
printf ("thread[%d]: %llu keys calculated\n",
id, info->stats.keycalculated);
}
return NULL;
}
static int recv_file(int sock, const char *filepath,
unsigned char digest[SHA_DIGEST_LENGTH])
{
char recvbuf[RECVBUF_SIZE];
ssize_t n;
size_t total_bytes = 0, total_bytes_session = 0;
SHA_CTX ctx;
int ret = EXIT_FAILURE;
FILE *f;
struct timeval start_time, end_time, diff_time = { 0, 0 };
if (!filepath)
filepath = "/tmp/data";
f = fopen(filepath, "w");
if (!f) {
fprintf(stderr, "Could not open file %s for writing\n",
filepath);
return EXIT_FAILURE;
}
SHA1_Init(&ctx);
gettimeofday(&start_time, NULL);
printf("Writing data to %s\n", filepath);
while (!should_exit) {
n = recv_sv(sock, recvbuf, RECVBUF_SIZE, 0);
/* printf("received %zd bytes\n", n); */
if (n < 0) {
fprintf(stderr, "\rerror receiving data: %s\n",
strerror(errno));
ret = EXIT_FAILURE;
break;
}
if (n == 0) {
fprintf(stdout, "\rconnection closed\n");
ret = EXIT_SUCCESS;
should_exit = 1;
SHA1_Final(digest, &ctx);
gettimeofday(&end_time, NULL);
break;
}
total_bytes += n;
total_bytes_session += n;
print_tick();
//printf("Received %zd bytes data, total=%zu\n", n, total_bytes);
//long pos = ftell(f);
size_t nmem = fwrite(recvbuf, n, 1, f);
SHA1_Update(&ctx, recvbuf, n);
if (nmem != 1) {
fprintf(stderr, "\rError writing to file\n");
break;
}
//printf("Wrote %ld bytes data to %s\n", ftell(f) - pos, filepath);
}
if (ret == EXIT_SUCCESS) {
timeval_sub(&diff_time, &end_time, &start_time);
printf("Finished successfully in %ld.%06ld seconds\n",
diff_time.tv_sec, (long)diff_time.tv_usec);
}
fprintf(stdout, "Read %zu bytes total\n", total_bytes);
fprintf(stdout, "Wrote to file %s\n", filepath);
fprintf(stdout, "Closing sockets...\n");
return ret;
}
void updateState(const fdm_packet* pkt) {
static double last_timestamp = 0; // in seconds
static uint64_t last_realtime = 0; // in uS
static struct timespec last_ts; // last packet
struct timespec now_ts;
clock_gettime(CLOCK_MONOTONIC, &now_ts);
const uint64_t realtime_now = micros64_real();
if (realtime_now > last_realtime + 500*1e3) { // 500ms timeout
last_timestamp = pkt->timestamp;
last_realtime = realtime_now;
sendMotorUpdate();
return;
}
const double deltaSim = pkt->timestamp - last_timestamp; // in seconds
if (deltaSim < 0) { // don't use old packet
return;
}
int16_t x,y,z;
x = constrain(-pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE, -32767, 32767);
y = constrain(-pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE, -32767, 32767);
z = constrain(-pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE, -32767, 32767);
fakeAccSet(fakeAccDev, x, y, z);
// printf("[acc]%lf,%lf,%lf\n", pkt->imu_linear_acceleration_xyz[0], pkt->imu_linear_acceleration_xyz[1], pkt->imu_linear_acceleration_xyz[2]);
x = constrain(pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG, -32767, 32767);
y = constrain(-pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG, -32767, 32767);
z = constrain(-pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG, -32767, 32767);
fakeGyroSet(fakeGyroDev, x, y, z);
// printf("[gyr]%lf,%lf,%lf\n", pkt->imu_angular_velocity_rpy[0], pkt->imu_angular_velocity_rpy[1], pkt->imu_angular_velocity_rpy[2]);
#if defined(SKIP_IMU_CALC)
#if defined(SET_IMU_FROM_EULER)
// set from Euler
double qw = pkt->imu_orientation_quat[0];
double qx = pkt->imu_orientation_quat[1];
double qy = pkt->imu_orientation_quat[2];
double qz = pkt->imu_orientation_quat[3];
double ysqr = qy * qy;
double xf, yf, zf;
// roll (x-axis rotation)
double t0 = +2.0 * (qw * qx + qy * qz);
double t1 = +1.0 - 2.0 * (qx * qx + ysqr);
xf = atan2(t0, t1) * RAD2DEG;
// pitch (y-axis rotation)
double t2 = +2.0 * (qw * qy - qz * qx);
t2 = t2 > 1.0 ? 1.0 : t2;
t2 = t2 < -1.0 ? -1.0 : t2;
yf = asin(t2) * RAD2DEG; // from wiki
// yaw (z-axis rotation)
double t3 = +2.0 * (qw * qz + qx * qy);
double t4 = +1.0 - 2.0 * (ysqr + qz * qz);
zf = atan2(t3, t4) * RAD2DEG;
imuSetAttitudeRPY(xf, -yf, zf); // yes! pitch was inverted!!
#else
imuSetAttitudeQuat(pkt->imu_orientation_quat[0], pkt->imu_orientation_quat[1], pkt->imu_orientation_quat[2], pkt->imu_orientation_quat[3]);
#endif
#endif
#if defined(SIMULATOR_IMU_SYNC)
imuSetHasNewData(deltaSim*1e6);
imuUpdateAttitude(micros());
#endif
if (deltaSim < 0.02 && deltaSim > 0) { // simulator should run faster than 50Hz
// simRate = simRate * 0.5 + (1e6 * deltaSim / (realtime_now - last_realtime)) * 0.5;
struct timespec out_ts;
timeval_sub(&out_ts, &now_ts, &last_ts);
simRate = deltaSim / (out_ts.tv_sec + 1e-9*out_ts.tv_nsec);
}
// printf("simRate = %lf, millis64 = %lu, millis64_real = %lu, deltaSim = %lf\n", simRate, millis64(), millis64_real(), deltaSim*1e6);
last_timestamp = pkt->timestamp;
last_realtime = micros64_real();
last_ts.tv_sec = now_ts.tv_sec;
last_ts.tv_nsec = now_ts.tv_nsec;
pthread_mutex_unlock(&updateLock); // can send PWM output now
#if defined(SIMULATOR_GYROPID_SYNC)
pthread_mutex_unlock(&mainLoopLock); // can run main loop
#endif
}