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#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
57struct handle_entry {
58    SIMPLEQ_ENTRY(handle_entry) entries;
59    int handle;
60};
61
62static SIMPLEQ_HEAD(simplehead, handle_entry) pending_flush_items_head;
63struct simplehead *headp;
64static pthread_mutex_t flush_handles_mutex = PTHREAD_MUTEX_INITIALIZER;
65
66static const char *smdWakelockStr = "significant motion";
67
68static struct sensor_t sSensorList[LOCAL_SENSORS];
69static int sensors = (sizeof(sSensorList) / sizeof(sensor_t));
70
71static int open_sensors(const struct hw_module_t* module, const char* id,
72                        struct hw_device_t** device);
73
74static 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
81static struct hw_module_methods_t sensors_module_methods = {
82        open: open_sensors
83};
84
85static 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
93struct 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
109struct 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
124private:
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
145sensors_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
190sensors_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
199int 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
207int sensors_poll_context_t::setDelay(int handle, int64_t ns)
208{
209    FUNC_LOG;
210    return mSensor->setDelay(handle, ns);
211}
212
213int64_t sensors_poll_context_t::getTimestamp()
214{
215    return android::elapsedRealtimeNano();
216}
217
218int 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
351int sensors_poll_context_t::query(int what, int* value)
352{
353    FUNC_LOG;
354    return mSensor->query(what, value);
355}
356
357int 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
366void 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
380int sensors_poll_context_t::flush(int handle)
381{
382    FUNC_LOG;
383    return mSensor->flush(handle);
384}
385#endif
386
387/******************************************************************************/
388
389static 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
399static 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
406static 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
414static 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
421static 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
428static 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
436static 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 */
452static 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