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