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*
_sensorIdToName(int id)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
_sensorIdFromName(const char * name)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*
control__open_data_source(struct sensors_control_device_t * dev)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
control__activate(struct sensors_control_device_t * dev,int handle,int enabled)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
control__set_delay(struct sensors_control_device_t * dev,int32_t ms)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
control__wake(struct sensors_control_device_t * dev)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
control__close(struct hw_device_t * dev)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
data__now_ns(void)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
data__data_open(struct sensors_data_device_t * dev,native_handle_t * handle)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
data__data_close(struct sensors_data_device_t * dev)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
pick_sensor(SensorData * data,sensors_data_t * values)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
data__poll(struct sensors_data_device_t * dev,sensors_data_t * values)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
data__close(struct hw_device_t * dev)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
sensors__get_sensors_list(struct sensors_module_t * module,struct sensor_t const ** list)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
open_sensors(const struct hw_module_t * module,const char * name,struct hw_device_t ** device)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