1 /* 2 * Copyright (C) 2009 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 /* this implements a sensors hardware library for the Android emulator. 18 * the following code should be built as a shared library that will be 19 * placed into /system/lib/hw/sensors.goldfish.so 20 * 21 * it will be loaded by the code in hardware/libhardware/hardware.c 22 * which is itself called from com_android_server_SensorService.cpp 23 */ 24 25 26 /* we connect with the emulator through the "sensors" qemud service 27 */ 28 #define SENSORS_SERVICE_NAME "sensors" 29 30 #define LOG_TAG "QemuSensors" 31 32 #include <unistd.h> 33 #include <fcntl.h> 34 #include <errno.h> 35 #include <string.h> 36 #include <cutils/log.h> 37 #include <cutils/native_handle.h> 38 #include <cutils/sockets.h> 39 #include <hardware/sensors.h> 40 41 #if 0 42 #define D(...) LOGD(__VA_ARGS__) 43 #else 44 #define D(...) ((void)0) 45 #endif 46 47 #define E(...) LOGE(__VA_ARGS__) 48 49 #include <hardware/qemud.h> 50 51 /** SENSOR IDS AND NAMES 52 **/ 53 54 #define MAX_NUM_SENSORS 4 55 56 #define SUPPORTED_SENSORS ((1<<MAX_NUM_SENSORS)-1) 57 58 #define ID_BASE SENSORS_HANDLE_BASE 59 #define ID_ACCELERATION (ID_BASE+0) 60 #define ID_MAGNETIC_FIELD (ID_BASE+1) 61 #define ID_ORIENTATION (ID_BASE+2) 62 #define ID_TEMPERATURE (ID_BASE+3) 63 64 #define SENSORS_ACCELERATION (1 << ID_ACCELERATION) 65 #define SENSORS_MAGNETIC_FIELD (1 << ID_MAGNETIC_FIELD) 66 #define SENSORS_ORIENTATION (1 << ID_ORIENTATION) 67 #define SENSORS_TEMPERATURE (1 << ID_TEMPERATURE) 68 69 #define ID_CHECK(x) ((unsigned)((x)-ID_BASE) < 4) 70 71 #define SENSORS_LIST \ 72 SENSOR_(ACCELERATION,"acceleration") \ 73 SENSOR_(MAGNETIC_FIELD,"magnetic-field") \ 74 SENSOR_(ORIENTATION,"orientation") \ 75 SENSOR_(TEMPERATURE,"temperature") \ 76 77 static const struct { 78 const char* name; 79 int id; } _sensorIds[MAX_NUM_SENSORS] = 80 { 81 #define SENSOR_(x,y) { y, ID_##x }, 82 SENSORS_LIST 83 #undef SENSOR_ 84 }; 85 86 static const char* 87 _sensorIdToName( int id ) 88 { 89 int nn; 90 for (nn = 0; nn < MAX_NUM_SENSORS; nn++) 91 if (id == _sensorIds[nn].id) 92 return _sensorIds[nn].name; 93 return "<UNKNOWN>"; 94 } 95 96 static int 97 _sensorIdFromName( const char* name ) 98 { 99 int nn; 100 101 if (name == NULL) 102 return -1; 103 104 for (nn = 0; nn < MAX_NUM_SENSORS; nn++) 105 if (!strcmp(name, _sensorIds[nn].name)) 106 return _sensorIds[nn].id; 107 108 return -1; 109 } 110 111 /** SENSORS CONTROL DEVICE 112 ** 113 ** This one is used to send commands to the sensors drivers. 114 ** We implement this by sending directly commands to the emulator 115 ** through the QEMUD channel. 116 **/ 117 118 typedef struct SensorControl { 119 struct sensors_control_device_t device; 120 int fd; 121 uint32_t active_sensors; 122 } SensorControl; 123 124 /* this must return a file descriptor that will be used to read 125 * the sensors data (it is passed to data__data_open() below 126 */ 127 static native_handle_t* 128 control__open_data_source(struct sensors_control_device_t *dev) 129 { 130 SensorControl* ctl = (void*)dev; 131 native_handle_t* handle; 132 133 if (ctl->fd < 0) { 134 ctl->fd = qemud_channel_open(SENSORS_SERVICE_NAME); 135 } 136 D("%s: fd=%d", __FUNCTION__, ctl->fd); 137 handle = native_handle_create(1, 0); 138 handle->data[0] = dup(ctl->fd); 139 return handle; 140 } 141 142 static int 143 control__activate(struct sensors_control_device_t *dev, 144 int handle, 145 int enabled) 146 { 147 SensorControl* ctl = (void*)dev; 148 uint32_t mask, sensors, active, new_sensors, changed; 149 char command[128]; 150 int ret; 151 152 D("%s: handle=%s (%d) fd=%d enabled=%d", __FUNCTION__, 153 _sensorIdToName(handle), handle, ctl->fd, enabled); 154 155 if (!ID_CHECK(handle)) { 156 E("%s: bad handle ID", __FUNCTION__); 157 return -1; 158 } 159 160 mask = (1<<handle); 161 sensors = enabled ? mask : 0; 162 163 active = ctl->active_sensors; 164 new_sensors = (active & ~mask) | (sensors & mask); 165 changed = active ^ new_sensors; 166 167 if (!changed) 168 return 0; 169 170 snprintf(command, sizeof command, "set:%s:%d", 171 _sensorIdToName(handle), enabled != 0); 172 173 if (ctl->fd < 0) { 174 ctl->fd = qemud_channel_open(SENSORS_SERVICE_NAME); 175 } 176 177 ret = qemud_channel_send(ctl->fd, command, -1); 178 if (ret < 0) { 179 E("%s: when sending command errno=%d: %s", __FUNCTION__, errno, strerror(errno)); 180 return -1; 181 } 182 ctl->active_sensors = new_sensors; 183 184 return 0; 185 } 186 187 static int 188 control__set_delay(struct sensors_control_device_t *dev, int32_t ms) 189 { 190 SensorControl* ctl = (void*)dev; 191 char command[128]; 192 193 D("%s: dev=%p delay-ms=%d", __FUNCTION__, dev, ms); 194 195 snprintf(command, sizeof command, "set-delay:%d", ms); 196 197 return qemud_channel_send(ctl->fd, command, -1); 198 } 199 200 /* this function is used to force-stop the blocking read() in 201 * data__poll. In order to keep the implementation as simple 202 * as possible here, we send a command to the emulator which 203 * shall send back an appropriate data block to the system. 204 */ 205 static int 206 control__wake(struct sensors_control_device_t *dev) 207 { 208 SensorControl* ctl = (void*)dev; 209 D("%s: dev=%p", __FUNCTION__, dev); 210 return qemud_channel_send(ctl->fd, "wake", -1); 211 } 212 213 214 static int 215 control__close(struct hw_device_t *dev) 216 { 217 SensorControl* ctl = (void*)dev; 218 close(ctl->fd); 219 free(ctl); 220 return 0; 221 } 222 223 /** SENSORS DATA DEVICE 224 ** 225 ** This one is used to read sensor data from the hardware. 226 ** We implement this by simply reading the data from the 227 ** emulator through the QEMUD channel. 228 **/ 229 230 231 typedef struct SensorData { 232 struct sensors_data_device_t device; 233 sensors_data_t sensors[MAX_NUM_SENSORS]; 234 int events_fd; 235 uint32_t pendingSensors; 236 int64_t timeStart; 237 int64_t timeOffset; 238 } SensorData; 239 240 /* return the current time in nanoseconds */ 241 static int64_t 242 data__now_ns(void) 243 { 244 struct timespec ts; 245 246 clock_gettime(CLOCK_MONOTONIC, &ts); 247 248 return (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec; 249 } 250 251 static int 252 data__data_open(struct sensors_data_device_t *dev, native_handle_t* handle) 253 { 254 SensorData* data = (void*)dev; 255 int i; 256 D("%s: dev=%p fd=%d", __FUNCTION__, dev, handle->data[0]); 257 memset(&data->sensors, 0, sizeof(data->sensors)); 258 259 for (i=0 ; i<MAX_NUM_SENSORS ; i++) { 260 data->sensors[i].vector.status = SENSOR_STATUS_ACCURACY_HIGH; 261 } 262 data->pendingSensors = 0; 263 data->timeStart = 0; 264 data->timeOffset = 0; 265 266 data->events_fd = dup(handle->data[0]); 267 D("%s: dev=%p fd=%d (was %d)", __FUNCTION__, dev, data->events_fd, handle->data[0]); 268 native_handle_close(handle); 269 native_handle_delete(handle); 270 return 0; 271 } 272 273 static int 274 data__data_close(struct sensors_data_device_t *dev) 275 { 276 SensorData* data = (void*)dev; 277 D("%s: dev=%p", __FUNCTION__, dev); 278 if (data->events_fd > 0) { 279 close(data->events_fd); 280 data->events_fd = -1; 281 } 282 return 0; 283 } 284 285 static int 286 pick_sensor(SensorData* data, 287 sensors_data_t* values) 288 { 289 uint32_t mask = SUPPORTED_SENSORS; 290 while (mask) { 291 uint32_t i = 31 - __builtin_clz(mask); 292 mask &= ~(1<<i); 293 if (data->pendingSensors & (1<<i)) { 294 data->pendingSensors &= ~(1<<i); 295 *values = data->sensors[i]; 296 values->sensor = (1<<i); 297 D("%s: %d [%f, %f, %f]", __FUNCTION__, 298 (1<<i), 299 values->vector.x, 300 values->vector.y, 301 values->vector.z); 302 return i; 303 } 304 } 305 LOGE("No sensor to return!!! pendingSensors=%08x", data->pendingSensors); 306 // we may end-up in a busy loop, slow things down, just in case. 307 usleep(100000); 308 return -EINVAL; 309 } 310 311 static int 312 data__poll(struct sensors_data_device_t *dev, sensors_data_t* values) 313 { 314 SensorData* data = (void*)dev; 315 int fd = data->events_fd; 316 317 D("%s: data=%p", __FUNCTION__, dev); 318 319 // there are pending sensors, returns them now... 320 if (data->pendingSensors) { 321 return pick_sensor(data, values); 322 } 323 324 // wait until we get a complete event for an enabled sensor 325 uint32_t new_sensors = 0; 326 327 while (1) { 328 /* read the next event */ 329 char buff[256]; 330 int len = qemud_channel_recv(data->events_fd, buff, sizeof buff-1); 331 float params[3]; 332 int64_t event_time; 333 334 if (len < 0) { 335 E("%s: len=%d, errno=%d: %s", __FUNCTION__, len, errno, strerror(errno)); 336 return -errno; 337 } 338 339 buff[len] = 0; 340 341 /* "wake" is sent from the emulator to exit this loop. This shall 342 * really be because another thread called "control__wake" in this 343 * process. 344 */ 345 if (!strcmp((const char*)data, "wake")) { 346 return 0x7FFFFFFF; 347 } 348 349 /* "acceleration:<x>:<y>:<z>" corresponds to an acceleration event */ 350 if (sscanf(buff, "acceleration:%g:%g:%g", params+0, params+1, params+2) == 3) { 351 new_sensors |= SENSORS_ACCELERATION; 352 data->sensors[ID_ACCELERATION].acceleration.x = params[0]; 353 data->sensors[ID_ACCELERATION].acceleration.y = params[1]; 354 data->sensors[ID_ACCELERATION].acceleration.z = params[2]; 355 continue; 356 } 357 358 /* "orientation:<azimuth>:<pitch>:<roll>" is sent when orientation changes */ 359 if (sscanf(buff, "orientation:%g:%g:%g", params+0, params+1, params+2) == 3) { 360 new_sensors |= SENSORS_ORIENTATION; 361 data->sensors[ID_ORIENTATION].orientation.azimuth = params[0]; 362 data->sensors[ID_ORIENTATION].orientation.pitch = params[1]; 363 data->sensors[ID_ORIENTATION].orientation.roll = params[2]; 364 continue; 365 } 366 367 /* "magnetic:<x>:<y>:<z>" is sent for the params of the magnetic field */ 368 if (sscanf(buff, "magnetic:%g:%g:%g", params+0, params+1, params+2) == 3) { 369 new_sensors |= SENSORS_MAGNETIC_FIELD; 370 data->sensors[ID_MAGNETIC_FIELD].magnetic.x = params[0]; 371 data->sensors[ID_MAGNETIC_FIELD].magnetic.y = params[1]; 372 data->sensors[ID_MAGNETIC_FIELD].magnetic.z = params[2]; 373 continue; 374 } 375 376 /* "temperature:<celsius>" */ 377 if (sscanf(buff, "temperature:%g", params+0) == 2) { 378 new_sensors |= SENSORS_TEMPERATURE; 379 data->sensors[ID_TEMPERATURE].temperature = params[0]; 380 continue; 381 } 382 383 /* "sync:<time>" is sent after a series of sensor events. 384 * where 'time' is expressed in micro-seconds and corresponds 385 * to the VM time when the real poll occured. 386 */ 387 if (sscanf(buff, "sync:%lld", &event_time) == 1) { 388 if (new_sensors) { 389 data->pendingSensors = new_sensors; 390 int64_t t = event_time * 1000LL; /* convert to nano-seconds */ 391 392 /* use the time at the first sync: as the base for later 393 * time values */ 394 if (data->timeStart == 0) { 395 data->timeStart = data__now_ns(); 396 data->timeOffset = data->timeStart - t; 397 } 398 t += data->timeOffset; 399 400 while (new_sensors) { 401 uint32_t i = 31 - __builtin_clz(new_sensors); 402 new_sensors &= ~(1<<i); 403 data->sensors[i].time = t; 404 } 405 return pick_sensor(data, values); 406 } else { 407 D("huh ? sync without any sensor data ?"); 408 } 409 continue; 410 } 411 D("huh ? unsupported command"); 412 } 413 } 414 415 static int 416 data__close(struct hw_device_t *dev) 417 { 418 SensorData* data = (SensorData*)dev; 419 if (data) { 420 if (data->events_fd > 0) { 421 //LOGD("(device close) about to close fd=%d", data->events_fd); 422 close(data->events_fd); 423 } 424 free(data); 425 } 426 return 0; 427 } 428 429 430 /** MODULE REGISTRATION SUPPORT 431 ** 432 ** This is required so that hardware/libhardware/hardware.c 433 ** will dlopen() this library appropriately. 434 **/ 435 436 /* 437 * the following is the list of all supported sensors. 438 * this table is used to build sSensorList declared below 439 * according to which hardware sensors are reported as 440 * available from the emulator (see get_sensors_list below) 441 * 442 * note: numerical values for maxRange/resolution/power were 443 * taken from the reference AK8976A implementation 444 */ 445 static const struct sensor_t sSensorListInit[] = { 446 { .name = "Goldfish 3-axis Accelerometer", 447 .vendor = "The Android Open Source Project", 448 .version = 1, 449 .handle = ID_ACCELERATION, 450 .type = SENSOR_TYPE_ACCELEROMETER, 451 .maxRange = 2.8f, 452 .resolution = 1.0f/4032.0f, 453 .power = 3.0f, 454 .reserved = {} 455 }, 456 457 { .name = "Goldfish 3-axis Magnetic field sensor", 458 .vendor = "The Android Open Source Project", 459 .version = 1, 460 .handle = ID_MAGNETIC_FIELD, 461 .type = SENSOR_TYPE_MAGNETIC_FIELD, 462 .maxRange = 2000.0f, 463 .resolution = 1.0f, 464 .power = 6.7f, 465 .reserved = {} 466 }, 467 468 { .name = "Goldfish Orientation sensor", 469 .vendor = "The Android Open Source Project", 470 .version = 1, 471 .handle = ID_ORIENTATION, 472 .type = SENSOR_TYPE_ORIENTATION, 473 .maxRange = 360.0f, 474 .resolution = 1.0f, 475 .power = 9.7f, 476 .reserved = {} 477 }, 478 479 { .name = "Goldfish Temperature sensor", 480 .vendor = "The Android Open Source Project", 481 .version = 1, 482 .handle = ID_TEMPERATURE, 483 .type = SENSOR_TYPE_TEMPERATURE, 484 .maxRange = 80.0f, 485 .resolution = 1.0f, 486 .power = 0.0f, 487 .reserved = {} 488 }, 489 }; 490 491 static struct sensor_t sSensorList[MAX_NUM_SENSORS]; 492 493 static uint32_t sensors__get_sensors_list(struct sensors_module_t* module, 494 struct sensor_t const** list) 495 { 496 int fd = qemud_channel_open(SENSORS_SERVICE_NAME); 497 char buffer[12]; 498 int mask, nn, count; 499 500 int ret; 501 if (fd < 0) { 502 E("%s: no qemud connection", __FUNCTION__); 503 return 0; 504 } 505 ret = qemud_channel_send(fd, "list-sensors", -1); 506 if (ret < 0) { 507 E("%s: could not query sensor list: %s", __FUNCTION__, 508 strerror(errno)); 509 close(fd); 510 return 0; 511 } 512 ret = qemud_channel_recv(fd, buffer, sizeof buffer-1); 513 if (ret < 0) { 514 E("%s: could not receive sensor list: %s", __FUNCTION__, 515 strerror(errno)); 516 close(fd); 517 return 0; 518 } 519 buffer[ret] = 0; 520 close(fd); 521 522 /* the result is a integer used as a mask for available sensors */ 523 mask = atoi(buffer); 524 count = 0; 525 for (nn = 0; nn < MAX_NUM_SENSORS; nn++) { 526 if (((1 << nn) & mask) == 0) 527 continue; 528 529 sSensorList[count++] = sSensorListInit[nn]; 530 } 531 D("%s: returned %d sensors (mask=%d)", __FUNCTION__, count, mask); 532 *list = sSensorList; 533 return count; 534 } 535 536 537 static int 538 open_sensors(const struct hw_module_t* module, 539 const char* name, 540 struct hw_device_t* *device) 541 { 542 int status = -EINVAL; 543 544 D("%s: name=%s", __FUNCTION__, name); 545 546 if (!strcmp(name, SENSORS_HARDWARE_CONTROL)) 547 { 548 SensorControl *dev = malloc(sizeof(*dev)); 549 550 memset(dev, 0, sizeof(*dev)); 551 552 dev->device.common.tag = HARDWARE_DEVICE_TAG; 553 dev->device.common.version = 0; 554 dev->device.common.module = (struct hw_module_t*) module; 555 dev->device.common.close = control__close; 556 dev->device.open_data_source = control__open_data_source; 557 dev->device.activate = control__activate; 558 dev->device.set_delay = control__set_delay; 559 dev->device.wake = control__wake; 560 dev->fd = -1; 561 562 *device = &dev->device.common; 563 status = 0; 564 } 565 else if (!strcmp(name, SENSORS_HARDWARE_DATA)) { 566 SensorData *dev = malloc(sizeof(*dev)); 567 568 memset(dev, 0, sizeof(*dev)); 569 570 dev->device.common.tag = HARDWARE_DEVICE_TAG; 571 dev->device.common.version = 0; 572 dev->device.common.module = (struct hw_module_t*) module; 573 dev->device.common.close = data__close; 574 dev->device.data_open = data__data_open; 575 dev->device.data_close = data__data_close; 576 dev->device.poll = data__poll; 577 dev->events_fd = -1; 578 579 *device = &dev->device.common; 580 status = 0; 581 } 582 return status; 583 } 584 585 586 static struct hw_module_methods_t sensors_module_methods = { 587 .open = open_sensors 588 }; 589 590 const struct sensors_module_t HAL_MODULE_INFO_SYM = { 591 .common = { 592 .tag = HARDWARE_MODULE_TAG, 593 .version_major = 1, 594 .version_minor = 0, 595 .id = SENSORS_HARDWARE_MODULE_ID, 596 .name = "Goldfish SENSORS Module", 597 .author = "The Android Open Source Project", 598 .methods = &sensors_module_methods, 599 }, 600 .get_sensors_list = sensors__get_sensors_list 601 }; 602
