void scheduler::add_task(task* tsk, task_queue* q)
{
auto ts = task_ext::get_inited(tsk);
ts->queue = q;
auto delay = (uint64_t)tsk->delay_milliseconds() * 1000000;
tsk->set_delay(0);
_wheel.add_event(now_ns() + delay, tsk);
}
void sim_timer_init(uint8_t scale) {
time_scale = scale;
then = begin = now_ns();
if (scale==0)
sim_info("timer_init: warp-speed");
else if (scale==1)
sim_info("timer_init: real-time");
else
sim_info("timer_init: 1/%u time", scale);
timer_initialised = true;
}
static void track_clk100ns( const usb_pkt_rx *rx )
{
/* track clk100ns */
if (!start_clk100ns) {
last_clk100ns = start_clk100ns = rx->clk100ns;
abs_start_ns = now_ns( );
}
/* detect clk100ns roll-over */
if (rx->clk100ns < last_clk100ns) {
clk100ns_upper += 1;
}
last_clk100ns = rx->clk100ns;
}
static void track_clk100ns( ubertooth_t* ut, const usb_pkt_rx* rx )
{
/* track clk100ns */
if (!ut->start_clk100ns) {
ut->last_clk100ns = ut->start_clk100ns = rx->clk100ns;
ut->abs_start_ns = now_ns( );
}
/* detect clk100ns roll-over */
if (rx->clk100ns < ut->last_clk100ns) {
ut->clk100ns_upper += 1;
}
ut->last_clk100ns = rx->clk100ns;
}
int
main(int argc, char* argv[])
{
char buf[MSGBUFSIZE];
struct sockaddr_in addr, cli_addr;
int sockfd, listen_port, optval;
unsigned long long sequence, timestamp;
if (argc != 2) {
fprintf(stderr, "usage: %s LISTEN_PORT\n", argv[0]);
exit(1);
}
listen_port = atoi(argv[1]);
sockfd = socket(AF_INET, SOCK_DGRAM, 0);
if (sockfd < 0) {
perror("socket");
exit(1);
}
optval = 1;
setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, (const void *)&optval , sizeof(optval));
bzero((char *) &addr, sizeof(addr));
addr.sin_family = AF_INET;
addr.sin_addr.s_addr = htonl(INADDR_ANY);
addr.sin_port = htons((unsigned short)listen_port);
if (bind(sockfd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
perror("bind");
exit(1);
}
while (1) {
socklen_t addrlen = sizeof(cli_addr);
int recv_bytes = recvfrom(sockfd, buf, MSGBUFSIZE, 0, (struct sockaddr*)&cli_addr, &addrlen);
if (recv_bytes < 0) {
perror("recvfrom");
exit(1);
}
timestamp = now_ns();
if (recv_bytes >= sizeof(sequence)) {
bcopy(buf, (char *)&sequence, sizeof(sequence));
printf("%llu %llu\n", timestamp, sequence);
fflush(stdout);
}
}
return 0;
}
static void timer1_isr(void) {
const uint8_t tr = timer_reason;
if ( ! sim_interrupts) {
// Interrupts disabled. Schedule another callback in 10us.
schedule_timer(10);
return;
}
timer_reason = 0;
cli();
#ifdef SIM_DEBUG
uint64_t now = now_ns();
static unsigned int cc_1s = 0, prev_1s = 0;
if ( ! clock_counter_1s && prev_1s) ++cc_1s;
prev_1s = clock_counter_1s;
//uint16_t now = sim_tick_counter();
uint64_t real = (now-begin) / 1000;
uint64_t avr = cc_1s * 4 + clock_counter_1s;
avr *= 250;
avr += clock_counter_250ms * 10;
avr += clock_counter_10ms;
avr *= 1000 ;
printf("test: Real: %us %u.%03ums AVR: %us %u.%03ums Real/AVR: %u\n",
real / 1000000 , (real % 1000000)/1000 , real % 1000 ,
avr / 1000000 , (avr % 1000000)/1000 , avr % 1000 ,
real / (avr?avr:1) );
printf("test: 10ms=%u 250ms=%u 1s=%u total=%luns actual=%luns\n",
clock_counter_10ms, clock_counter_250ms, clock_counter_1s,
now - begin, now - then, sim_runtime_ns());
//printf(" timer1_isr tick_time=%04X now=%04X delta=%u total=%u\n",
// TICK_TIME , now, now_us() - then, (now_us() - begin)/1000000 ) ;
then = now;
#endif
if (tr & TIMER_OCR1A) TIMER1_COMPA_vect();
if (tr & TIMER_OCR1B) TIMER1_COMPB_vect();
sei();
// Setup next timer
sim_timer_set();
}
map_info_t* acquire_my_map_info_list() {
pthread_mutex_lock(&g_my_map_info_list_mutex);
int64_t time = now_ns();
if (g_my_map_info_list != NULL) {
my_map_info_data_t* data = (my_map_info_data_t*)g_my_map_info_list->data;
int64_t age = time - data->timestamp;
if (age >= MAX_CACHE_AGE) {
ALOGV("Invalidated my_map_info_list %p, age=%lld.", g_my_map_info_list, age);
dec_ref(g_my_map_info_list, data);
g_my_map_info_list = NULL;
} else {
ALOGV("Reusing my_map_info_list %p, age=%lld.", g_my_map_info_list, age);
}
}
if (g_my_map_info_list == NULL) {
my_map_info_data_t* data = (my_map_info_data_t*)malloc(sizeof(my_map_info_data_t));
g_my_map_info_list = load_map_info_list(getpid());
if (g_my_map_info_list != NULL) {
ALOGV("Loaded my_map_info_list %p.", g_my_map_info_list);
g_my_map_info_list->data = data;
data->refs = 1;
data->timestamp = time;
} else {
free(data);
}
}
map_info_t* milist = g_my_map_info_list;
if (milist) {
my_map_info_data_t* data = (my_map_info_data_t*)g_my_map_info_list->data;
data->refs += 1;
}
pthread_mutex_unlock(&g_my_map_info_list_mutex);
return milist;
}
/* Block until new sensor events are reported by the emulator, or if a
* 'wake' command is received through the service. On succes, return 0
* and updates the |pendingEvents| and |sensors| fields of |dev|.
* On failure, return -errno.
*
* Note: The device lock must be acquired when calling this function, and
* will still be held on return. However, the function releases the
* lock temporarily during the blocking wait.
*/
static int sensor_device_poll_event_locked(SensorDevice* dev)
{
D("%s: dev=%p", __FUNCTION__, dev);
int fd = sensor_device_get_fd_locked(dev);
if (fd < 0) {
E("%s: Could not get pipe channel: %s", __FUNCTION__, strerror(-fd));
return fd;
}
// Accumulate pending events into |events| and |new_sensors| mask
// until a 'sync' or 'wake' command is received. This also simplifies the
// code a bit.
uint32_t new_sensors = 0U;
sensors_event_t* events = dev->sensors;
int64_t event_time = -1;
int ret = 0;
for (;;) {
/* Release the lock since we're going to block on recv() */
pthread_mutex_unlock(&dev->lock);
/* read the next event */
char buff[256];
int len = qemud_channel_recv(fd, buff, sizeof(buff) - 1U);
/* re-acquire the lock to modify the device state. */
pthread_mutex_lock(&dev->lock);
if (len < 0) {
ret = -errno;
E("%s(fd=%d): Could not receive event data len=%d, errno=%d: %s",
__FUNCTION__, fd, len, errno, strerror(errno));
break;
}
buff[len] = 0;
D("%s(fd=%d): received [%s]", __FUNCTION__, fd, buff);
/* "wake" is sent from the emulator to exit this loop. */
/* TODO(digit): Is it still needed? */
if (!strcmp((const char*)buff, "wake")) {
ret = 0x7FFFFFFF;
break;
}
float params[3];
/* "acceleration:<x>:<y>:<z>" corresponds to an acceleration event */
if (sscanf(buff, "acceleration:%g:%g:%g", params+0, params+1, params+2)
== 3) {
new_sensors |= SENSORS_ACCELERATION;
events[ID_ACCELERATION].acceleration.x = params[0];
events[ID_ACCELERATION].acceleration.y = params[1];
events[ID_ACCELERATION].acceleration.z = params[2];
events[ID_ACCELERATION].type = SENSOR_TYPE_ACCELEROMETER;
continue;
}
/* "orientation:<azimuth>:<pitch>:<roll>" is sent when orientation
* changes */
if (sscanf(buff, "orientation:%g:%g:%g", params+0, params+1, params+2)
== 3) {
new_sensors |= SENSORS_ORIENTATION;
events[ID_ORIENTATION].orientation.azimuth = params[0];
events[ID_ORIENTATION].orientation.pitch = params[1];
events[ID_ORIENTATION].orientation.roll = params[2];
events[ID_ORIENTATION].orientation.status =
SENSOR_STATUS_ACCURACY_HIGH;
events[ID_ACCELERATION].type = SENSOR_TYPE_ORIENTATION;
continue;
}
/* "magnetic:<x>:<y>:<z>" is sent for the params of the magnetic
* field */
if (sscanf(buff, "magnetic:%g:%g:%g", params+0, params+1, params+2)
== 3) {
new_sensors |= SENSORS_MAGNETIC_FIELD;
events[ID_MAGNETIC_FIELD].magnetic.x = params[0];
events[ID_MAGNETIC_FIELD].magnetic.y = params[1];
events[ID_MAGNETIC_FIELD].magnetic.z = params[2];
events[ID_MAGNETIC_FIELD].magnetic.status =
SENSOR_STATUS_ACCURACY_HIGH;
events[ID_ACCELERATION].type = SENSOR_TYPE_MAGNETIC_FIELD;
continue;
}
/* "temperature:<celsius>" */
if (sscanf(buff, "temperature:%g", params+0) == 1) {
new_sensors |= SENSORS_TEMPERATURE;
events[ID_TEMPERATURE].temperature = params[0];
events[ID_ACCELERATION].type = SENSOR_TYPE_TEMPERATURE;
continue;
}
/* "proximity:<value>" */
if (sscanf(buff, "proximity:%g", params+0) == 1) {
new_sensors |= SENSORS_PROXIMITY;
events[ID_PROXIMITY].distance = params[0];
events[ID_ACCELERATION].type = SENSOR_TYPE_PROXIMITY;
continue;
}
/* "sync:<time>" is sent after a series of sensor events.
* where 'time' is expressed in micro-seconds and corresponds
* to the VM time when the real poll occured.
*/
if (sscanf(buff, "sync:%lld", &event_time) == 1) {
if (new_sensors) {
goto out;
}
D("huh ? sync without any sensor data ?");
continue;
}
D("huh ? unsupported command");
}
out:
if (new_sensors) {
/* update the time of each new sensor event. */
dev->pendingSensors |= new_sensors;
int64_t t = (event_time < 0) ? 0 : event_time * 1000LL;
/* use the time at the first sync: as the base for later
* time values */
if (dev->timeStart == 0) {
dev->timeStart = now_ns();
dev->timeOffset = dev->timeStart - t;
}
t += dev->timeOffset;
while (new_sensors) {
uint32_t i = 31 - __builtin_clz(new_sensors);
new_sensors &= ~(1U << i);
dev->sensors[i].timestamp = t;
}
}
return ret;
}
static uint64_t now_us(void) {
// nanoseconds to microseconds
return now_ns() / 1000;
}
uint64_t sim_runtime_ns(void) {
if (time_scale)
return (now_ns() - begin) / time_scale;
return TICKS_TO_US(ticks) * 1000 ;
}
