1 /*
2 * Copyright (C) 2014 Invensense, Inc.
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 #define FUNC_LOG LOGV("%s", __PRETTY_FUNCTION__)
18
19 #include <hardware_legacy/power.h>
20 #include <hardware/sensors.h>
21 #include <fcntl.h>
22 #include <errno.h>
23 #include <dirent.h>
24 #include <math.h>
25 #include <poll.h>
26 #include <pthread.h>
27 #include <stdlib.h>
28
29 #include <sys/queue.h>
30
31 #include <linux/input.h>
32
33 #include <cutils/atomic.h>
34 #include <utils/Log.h>
35 #include <utils/SystemClock.h>
36
37 #include "sensors.h"
38 #include "MPLSensor.h"
39
40 /*
41 * Vendor-defined Accel Load Calibration File Method
42 * @param[out] Accel bias, length 3. In HW units scaled by 2^16 in body frame
43 * @return '0' for a successful load, '1' otherwise
44 * example: int AccelLoadConfig(long* offset);
45 * End of Vendor-defined Accel Load Cal Method
46 */
47
48 /*****************************************************************************/
49 /* The SENSORS Module */
50
51 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
52 #define LOCAL_SENSORS (NumSensors + 1)
53 #else
54 #define LOCAL_SENSORS (NumSensors)
55 #endif
56
57 struct handle_entry {
58 SIMPLEQ_ENTRY(handle_entry) entries;
59 int handle;
60 };
61
62 static SIMPLEQ_HEAD(simplehead, handle_entry) pending_flush_items_head;
63 struct simplehead *headp;
64 static pthread_mutex_t flush_handles_mutex = PTHREAD_MUTEX_INITIALIZER;
65
66 static const char *smdWakelockStr = "significant motion";
67
68 static struct sensor_t sSensorList[LOCAL_SENSORS];
69 static int sensors = (sizeof(sSensorList) / sizeof(sensor_t));
70
71 static int open_sensors(const struct hw_module_t* module, const char* id,
72 struct hw_device_t** device);
73
sensors__get_sensors_list(struct sensors_module_t * module,struct sensor_t const ** list)74 static int sensors__get_sensors_list(struct sensors_module_t* module,
75 struct sensor_t const** list)
76 {
77 *list = sSensorList;
78 return sensors;
79 }
80
81 static struct hw_module_methods_t sensors_module_methods = {
82 open: open_sensors
83 };
84
sensors_set_operation_mode(unsigned int mode)85 static int sensors_set_operation_mode(unsigned int mode)
86 {
87 LOGI("%s", __FUNCTION__);
88 LOGI("%s: stub function: ignoring mode request (%d)", __FUNCTION__,
89 mode);
90 return 0;
91 }
92
93 struct sensors_module_t HAL_MODULE_INFO_SYM = {
94 common: {
95 tag: HARDWARE_MODULE_TAG,
96 version_major: 1,
97 version_minor: 0,
98 id: SENSORS_HARDWARE_MODULE_ID,
99 name: "Invensense module",
100 author: "Invensense Inc.",
101 methods: &sensors_module_methods,
102 dso: NULL,
103 reserved: {0}
104 },
105 get_sensors_list: sensors__get_sensors_list,
106 set_operation_mode: sensors_set_operation_mode
107 };
108
109 struct sensors_poll_context_t {
110 sensors_poll_device_1_t device; // must be first
111
112 sensors_poll_context_t();
113 ~sensors_poll_context_t();
114 int activate(int handle, int enabled);
115 int setDelay(int handle, int64_t ns);
116 int pollEvents(sensors_event_t* data, int count);
117 int query(int what, int *value);
118 int batch(int handle, int flags, int64_t period_ns, int64_t timeout);
119 #if defined ANDROID_KITKAT || defined ANDROID_LOLLIPOP
120 int flush(int handle);
121 #endif
122 int64_t getTimestamp();
123
124 private:
125 enum {
126 mpl = 0,
127 compass,
128 dmpOrient,
129 dmpSign,
130 dmpPed,
131 numSensorDrivers,
132 numFds,
133 };
134
135 struct pollfd mPollFds[numFds];
136 SensorBase *mSensor;
137 CompassSensor *mCompassSensor;
138
139 /* Significant Motion wakelock support */
140 bool mSMDWakelockHeld;
141 };
142
143 /******************************************************************************/
144
sensors_poll_context_t()145 sensors_poll_context_t::sensors_poll_context_t() {
146 VFUNC_LOG;
147
148 /* TODO: Handle external pressure sensor */
149 mCompassSensor = new CompassSensor();
150 MPLSensor *mplSensor = new MPLSensor(mCompassSensor);
151
152 /* No significant motion events pending yet */
153 mSMDWakelockHeld = false;
154
155 /* For Vendor-defined Accel Calibration File Load
156 * Use the Following Constructor and Pass Your Load Cal File Function
157 *
158 * MPLSensor *mplSensor = new MPLSensor(mCompassSensor, AccelLoadConfig);
159 */
160
161 // Initialize pending flush queue
162 SIMPLEQ_INIT(&pending_flush_items_head);
163
164 // populate the sensor list
165 sensors =
166 mplSensor->populateSensorList(sSensorList, sizeof(sSensorList));
167
168 mSensor = mplSensor;
169 mPollFds[mpl].fd = mSensor->getFd();
170 mPollFds[mpl].events = POLLIN;
171 mPollFds[mpl].revents = 0;
172
173 mPollFds[compass].fd = mCompassSensor->getFd();
174 mPollFds[compass].events = POLLIN;
175 mPollFds[compass].revents = 0;
176
177 mPollFds[dmpOrient].fd = ((MPLSensor*) mSensor)->getDmpOrientFd();
178 mPollFds[dmpOrient].events = POLLPRI;
179 mPollFds[dmpOrient].revents = 0;
180
181 mPollFds[dmpSign].fd = ((MPLSensor*) mSensor)->getDmpSignificantMotionFd();
182 mPollFds[dmpSign].events = POLLPRI;
183 mPollFds[dmpSign].revents = 0;
184
185 mPollFds[dmpPed].fd = ((MPLSensor*) mSensor)->getDmpPedometerFd();
186 mPollFds[dmpPed].events = POLLPRI;
187 mPollFds[dmpPed].revents = 0;
188 }
189
~sensors_poll_context_t()190 sensors_poll_context_t::~sensors_poll_context_t() {
191 FUNC_LOG;
192 delete mSensor;
193 delete mCompassSensor;
194 for (int i = 0; i < numSensorDrivers; i++) {
195 close(mPollFds[i].fd);
196 }
197 }
198
activate(int handle,int enabled)199 int sensors_poll_context_t::activate(int handle, int enabled) {
200 FUNC_LOG;
201
202 int err;
203 err = mSensor->enable(handle, enabled);
204 return err;
205 }
206
setDelay(int handle,int64_t ns)207 int sensors_poll_context_t::setDelay(int handle, int64_t ns)
208 {
209 FUNC_LOG;
210 return mSensor->setDelay(handle, ns);
211 }
212
getTimestamp()213 int64_t sensors_poll_context_t::getTimestamp()
214 {
215 return android::elapsedRealtimeNano();
216 }
217
pollEvents(sensors_event_t * data,int count)218 int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count)
219 {
220 VHANDLER_LOG;
221
222 int nbEvents = 0;
223 int nb, polltime = -1;
224
225 if (mSMDWakelockHeld) {
226 mSMDWakelockHeld = false;
227 release_wake_lock(smdWakelockStr);
228 }
229
230 struct handle_entry *handle_element;
231 pthread_mutex_lock(&flush_handles_mutex);
232 if (!SIMPLEQ_EMPTY(&pending_flush_items_head)) {
233 sensors_event_t flushCompleteEvent;
234 flushCompleteEvent.type = SENSOR_TYPE_META_DATA;
235 flushCompleteEvent.sensor = 0;
236 handle_element = SIMPLEQ_FIRST(&pending_flush_items_head);
237 flushCompleteEvent.meta_data.sensor = handle_element->handle;
238 SIMPLEQ_REMOVE_HEAD(&pending_flush_items_head, entries);
239 free(handle_element);
240 memcpy(data, (void *) &flushCompleteEvent, sizeof(flushCompleteEvent));
241 LOGI_IF(1, "pollEvents() Returning fake flush event completion for handle %d",
242 flushCompleteEvent.meta_data.sensor);
243 pthread_mutex_unlock(&flush_handles_mutex);
244 return 1;
245 }
246 pthread_mutex_unlock(&flush_handles_mutex);
247
248 polltime = ((MPLSensor*) mSensor)->getStepCountPollTime();
249
250 // look for new events
251 nb = poll(mPollFds, numSensorDrivers, polltime);
252 LOGI_IF(0, "poll nb=%d, count=%d, pt=%d ts=%lld", nb, count, polltime, getTimestamp());
253 if (nb == 0 && count > 0) {
254 /* to see if any step counter events */
255 if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) {
256 nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents(
257 data, count, ID_SC, 0);
258 LOGI_IF(SensorBase::HANDLER_DATA, "sensors_mpl:readStepCount() - "
259 "nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, ",
260 nb, count, nbEvents, data->timestamp);
261 if (nb > 0) {
262 count -= nb;
263 nbEvents += nb;
264 data += nb;
265 }
266 }
267 } else while (nb > 0) {
268 for (int i = 0; count && i < numSensorDrivers; i++) {
269 if (mPollFds[i].revents & (POLLIN | POLLPRI)) {
270 nb = 0;
271 if (i == mpl) {
272 ((MPLSensor*) mSensor)->buildMpuEvent();
273 mPollFds[i].revents = 0;
274 } else if (i == compass) {
275 ((MPLSensor*) mSensor)->buildCompassEvent();
276 mPollFds[i].revents = 0;
277 } else if (i == dmpOrient) {
278 nb = ((MPLSensor*)mSensor)->
279 readDmpOrientEvents(data, count);
280 mPollFds[dmpOrient].revents= 0;
281 if (isDmpScreenAutoRotationEnabled() && nb > 0) {
282 count -= nb;
283 nbEvents += nb;
284 data += nb;
285 }
286 } else if (i == dmpSign) {
287 nb = ((MPLSensor*) mSensor)->
288 readDmpSignificantMotionEvents(data, count);
289 mPollFds[i].revents = 0;
290 if (nb) {
291 if (!mSMDWakelockHeld) {
292 /* Hold wakelock until Sensor Services reads event */
293 acquire_wake_lock(PARTIAL_WAKE_LOCK, smdWakelockStr);
294 LOGI_IF(1, "HAL: grabbed %s wakelock", smdWakelockStr);
295 mSMDWakelockHeld = true;
296 }
297
298 count -= nb;
299 nbEvents += nb;
300 data += nb;
301 }
302 } else if (i == dmpPed) {
303 nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents(
304 data, count, ID_P, 0);
305 mPollFds[i].revents = 0;
306 count -= nb;
307 nbEvents += nb;
308 data += nb;
309 }
310
311 if(nb == 0) {
312 nb = ((MPLSensor*) mSensor)->readEvents(data, count);
313 LOGI_IF(0, "sensors_mpl:readEvents() - "
314 "i=%d, nb=%d, count=%d, nbEvents=%d, "
315 "data->timestamp=%lld, data->data[0]=%f,",
316 i, nb, count, nbEvents, data->timestamp,
317 data->data[0]);
318 if (nb > 0) {
319 count -= nb;
320 nbEvents += nb;
321 data += nb;
322 }
323 }
324 }
325 }
326
327 /* to see if any step counter events */
328 if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) {
329 nb = 0;
330 nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents(
331 data, count, ID_SC, 0);
332 LOGI_IF(SensorBase::HANDLER_DATA, "sensors_mpl:readStepCount() - "
333 "nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, ",
334 nb, count, nbEvents, data->timestamp);
335 if (nb > 0) {
336 count -= nb;
337 nbEvents += nb;
338 data += nb;
339 }
340 }
341 if (count > 0) {
342 // We still have room for more events, try an immediate poll for more data
343 nb = poll(mPollFds, numSensorDrivers, 0);
344 } else {
345 nb = 0;
346 }
347 }
348 return nbEvents;
349 }
350
query(int what,int * value)351 int sensors_poll_context_t::query(int what, int* value)
352 {
353 FUNC_LOG;
354 return mSensor->query(what, value);
355 }
356
batch(int handle,int flags,int64_t period_ns,int64_t timeout)357 int sensors_poll_context_t::batch(int handle, int flags, int64_t period_ns,
358 int64_t timeout)
359 {
360 FUNC_LOG;
361 return mSensor->batch(handle, flags, period_ns, timeout);
362 }
363
364 #if defined ANDROID_KITKAT || defined ANDROID_LOLLIPOP
365
inv_pending_flush(int handle)366 void inv_pending_flush(int handle) {
367 struct handle_entry *the_entry;
368 pthread_mutex_lock(&flush_handles_mutex);
369 the_entry = (struct handle_entry*) malloc(sizeof(struct handle_entry));
370 if (the_entry != NULL) {
371 LOGI_IF(0, "Inserting %d into pending list", handle);
372 the_entry->handle = handle;
373 SIMPLEQ_INSERT_TAIL(&pending_flush_items_head, the_entry, entries);
374 } else {
375 LOGE("ERROR malloc'ing space for pending handler flush entry");
376 }
377 pthread_mutex_unlock(&flush_handles_mutex);
378 }
379
flush(int handle)380 int sensors_poll_context_t::flush(int handle)
381 {
382 FUNC_LOG;
383 return mSensor->flush(handle);
384 }
385 #endif
386
387 /******************************************************************************/
388
poll__close(struct hw_device_t * dev)389 static int poll__close(struct hw_device_t *dev)
390 {
391 FUNC_LOG;
392 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
393 if (ctx) {
394 delete ctx;
395 }
396 return 0;
397 }
398
poll__activate(struct sensors_poll_device_t * dev,int handle,int enabled)399 static int poll__activate(struct sensors_poll_device_t *dev,
400 int handle, int enabled)
401 {
402 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
403 return ctx->activate(handle, enabled);
404 }
405
poll__setDelay(struct sensors_poll_device_t * dev,int handle,int64_t ns)406 static int poll__setDelay(struct sensors_poll_device_t *dev,
407 int handle, int64_t ns)
408 {
409 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
410 int s= ctx->setDelay(handle, ns);
411 return s;
412 }
413
poll__poll(struct sensors_poll_device_t * dev,sensors_event_t * data,int count)414 static int poll__poll(struct sensors_poll_device_t *dev,
415 sensors_event_t* data, int count)
416 {
417 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
418 return ctx->pollEvents(data, count);
419 }
420
poll__query(struct sensors_poll_device_1 * dev,int what,int * value)421 static int poll__query(struct sensors_poll_device_1 *dev,
422 int what, int *value)
423 {
424 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
425 return ctx->query(what, value);
426 }
427
poll__batch(struct sensors_poll_device_1 * dev,int handle,int flags,int64_t period_ns,int64_t timeout)428 static int poll__batch(struct sensors_poll_device_1 *dev,
429 int handle, int flags, int64_t period_ns, int64_t timeout)
430 {
431 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
432 return ctx->batch(handle, flags, period_ns, timeout);
433 }
434
435 #if defined ANDROID_KITKAT || defined ANDROID_LOLLIPOP
poll__flush(struct sensors_poll_device_1 * dev,int handle)436 static int poll__flush(struct sensors_poll_device_1 *dev,
437 int handle)
438 {
439 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
440 int status = ctx->flush(handle);
441 if (handle == SENSORS_STEP_COUNTER_HANDLE) {
442 LOGI_IF(0, "creating flush completion event for handle %d", handle);
443 inv_pending_flush(handle);
444 return 0;
445 }
446 return status;
447 }
448 #endif
449 /******************************************************************************/
450
451 /** Open a new instance of a sensor device using name */
open_sensors(const struct hw_module_t * module,const char * id,struct hw_device_t ** device)452 static int open_sensors(const struct hw_module_t* module, const char* id,
453 struct hw_device_t** device)
454 {
455 FUNC_LOG;
456 int status = -EINVAL;
457 sensors_poll_context_t *dev = new sensors_poll_context_t();
458
459 memset(&dev->device, 0, sizeof(sensors_poll_device_1));
460
461 dev->device.common.tag = HARDWARE_DEVICE_TAG;
462 dev->device.common.version = SENSORS_DEVICE_API_VERSION_1_3;
463 dev->device.flush = poll__flush;
464 dev->device.common.module = const_cast<hw_module_t*>(module);
465 dev->device.common.close = poll__close;
466 dev->device.activate = poll__activate;
467 dev->device.setDelay = poll__setDelay;
468 dev->device.poll = poll__poll;
469 dev->device.batch = poll__batch;
470
471 *device = &dev->device.common;
472 status = 0;
473
474 return status;
475 }
476