sdk/emulator/sensors/sensors_qemu.c

   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 };