1 /*
2 * Copyright (C) 2012 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/sensors.h>
20 #include <fcntl.h>
21 #include <errno.h>
22 #include <dirent.h>
23 #include <math.h>
24 #include <poll.h>
25 #include <pthread.h>
26 #include <stdlib.h>
27 
28 #include <linux/input.h>
29 
30 #include <cutils/atomic.h>
31 #include <utils/Log.h>
32 
33 #include "sensors.h"
34 #include "MPLSensor.h"
35 
36 /*
37  * Vendor-defined Accel Load Calibration File Method
38  * @param[out] Accel bias, length 3.  In HW units scaled by 2^16 in body frame
39  * @return '0' for a successful load, '1' otherwise
40  * example: int AccelLoadConfig(long* offset);
41  * End of Vendor-defined Accel Load Cal Method
42  */
43 
44 /*****************************************************************************/
45 /* The SENSORS Module */
46 
47 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
48 #define LOCAL_SENSORS (MPLSensor::NumSensors + 1)
49 #else
50 #define LOCAL_SENSORS MPLSensor::NumSensors
51 #endif
52 
53 static struct sensor_t sSensorList[LOCAL_SENSORS];
54 static int sensors = (sizeof(sSensorList) / sizeof(sensor_t));
55 
56 static int open_sensors(const struct hw_module_t* module, const char* id,
57                         struct hw_device_t** device);
58 
sensors__get_sensors_list(struct sensors_module_t * module,struct sensor_t const ** list)59 static int sensors__get_sensors_list(struct sensors_module_t* module,
60                                      struct sensor_t const** list)
61 {
62     *list = sSensorList;
63     return sensors;
64 }
65 
66 static struct hw_module_methods_t sensors_module_methods = {
67         open: open_sensors
68 };
69 
70 struct sensors_module_t HAL_MODULE_INFO_SYM = {
71         common: {
72                 tag: HARDWARE_MODULE_TAG,
73                 version_major: 1,
74                 version_minor: 0,
75                 id: SENSORS_HARDWARE_MODULE_ID,
76                 name: "Invensense module",
77                 author: "Invensense Inc.",
78                 methods: &sensors_module_methods,
79                 dso: NULL,
80                 reserved: {0}
81         },
82         get_sensors_list: sensors__get_sensors_list,
83         set_operation_mode: NULL
84 };
85 
86 struct sensors_poll_context_t {
87     sensors_poll_device_1_t device; // must be first
88 
89     sensors_poll_context_t();
90     ~sensors_poll_context_t();
91     int activate(int handle, int enabled);
92     int setDelay(int handle, int64_t ns);
93     int pollEvents(sensors_event_t* data, int count);
94     int query(int what, int *value);
95     int batch(int handle, int flags, int64_t period_ns, int64_t timeout);
96     int flush(int handle);
97 
98 private:
99     enum {
100         mpl = 0,
101         compass,
102         dmpOrient,
103         dmpSign,
104         dmpPed,
105         numSensorDrivers,   // wake pipe goes here
106         numFds,
107     };
108 
109     struct pollfd mPollFds[numFds];
110     SensorBase *mSensor;
111     CompassSensor *mCompassSensor;
112 
113     static const size_t wake = numSensorDrivers;
114     static const char WAKE_MESSAGE = 'W';
115     int mWritePipeFd;
116 };
117 
118 /******************************************************************************/
119 
sensors_poll_context_t()120 sensors_poll_context_t::sensors_poll_context_t() {
121     VFUNC_LOG;
122 
123     /* TODO: Handle external pressure sensor */
124     mCompassSensor = new CompassSensor();
125     MPLSensor *mplSensor = new MPLSensor(mCompassSensor);
126 
127    /* For Vendor-defined Accel Calibration File Load
128     * Use the Following Constructor and Pass Your Load Cal File Function
129     *
130     * MPLSensor *mplSensor = new MPLSensor(mCompassSensor, AccelLoadConfig);
131     */
132 
133     // setup the callback object for handing mpl callbacks
134     setCallbackObject(mplSensor);
135 
136     // populate the sensor list
137     sensors =
138             mplSensor->populateSensorList(sSensorList, sizeof(sSensorList));
139 
140     mSensor = mplSensor;
141     mPollFds[mpl].fd = mSensor->getFd();
142     mPollFds[mpl].events = POLLIN;
143     mPollFds[mpl].revents = 0;
144 
145     mPollFds[compass].fd = mCompassSensor->getFd();
146     mPollFds[compass].events = POLLIN;
147     mPollFds[compass].revents = 0;
148 
149     mPollFds[dmpOrient].fd = ((MPLSensor*) mSensor)->getDmpOrientFd();
150     mPollFds[dmpOrient].events = POLLPRI;
151     mPollFds[dmpOrient].revents = 0;
152 
153     mPollFds[dmpSign].fd = ((MPLSensor*) mSensor)->getDmpSignificantMotionFd();
154     mPollFds[dmpSign].events = POLLPRI;
155     mPollFds[dmpSign].revents = 0;
156 
157     mPollFds[dmpPed].fd = ((MPLSensor*) mSensor)->getDmpPedometerFd();
158     mPollFds[dmpPed].events = POLLPRI;
159     mPollFds[dmpPed].revents = 0;
160 
161     /* Timer based sensor initialization */
162     int wakeFds[2];
163     int result = pipe(wakeFds);
164     LOGE_IF(result<0, "error creating wake pipe (%s)", strerror(errno));
165     fcntl(wakeFds[0], F_SETFL, O_NONBLOCK);
166     fcntl(wakeFds[1], F_SETFL, O_NONBLOCK);
167     mWritePipeFd = wakeFds[1];
168 
169     mPollFds[numSensorDrivers].fd = wakeFds[0];
170     mPollFds[numSensorDrivers].events = POLLIN;
171     mPollFds[numSensorDrivers].revents = 0;
172 }
173 
~sensors_poll_context_t()174 sensors_poll_context_t::~sensors_poll_context_t() {
175     FUNC_LOG;
176     delete mSensor;
177     delete mCompassSensor;
178     for (int i = 0; i < numSensorDrivers; i++) {
179         close(mPollFds[i].fd);
180     }
181     close(mWritePipeFd);
182 }
183 
activate(int handle,int enabled)184 int sensors_poll_context_t::activate(int handle, int enabled) {
185     FUNC_LOG;
186 
187     int err;
188     err = mSensor->enable(handle, enabled);
189     if (!err) {
190         const char wakeMessage(WAKE_MESSAGE);
191         int result = write(mWritePipeFd, &wakeMessage, 1);
192         LOGE_IF(result < 0,
193                 "error sending wake message (%s)", strerror(errno));
194     }
195     return err;
196 }
197 
setDelay(int handle,int64_t ns)198 int sensors_poll_context_t::setDelay(int handle, int64_t ns)
199 {
200     FUNC_LOG;
201     return mSensor->setDelay(handle, ns);
202 }
203 
pollEvents(sensors_event_t * data,int count)204 int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count)
205 {
206     VHANDLER_LOG;
207 
208     int nbEvents = 0;
209     int nb, polltime = -1;
210 
211     polltime = ((MPLSensor*) mSensor)->getStepCountPollTime();
212 
213     // look for new events
214     nb = poll(mPollFds, numSensorDrivers, polltime);
215     LOGI_IF(0, "poll nb=%d, count=%d, pt=%d", nb, count, polltime);
216     if (nb > 0) {
217         for (int i = 0; count && i < numSensorDrivers; i++) {
218             if (mPollFds[i].revents & (POLLIN | POLLPRI)) {
219                 nb = 0;
220                 if (i == mpl) {
221                     ((MPLSensor*) mSensor)->buildMpuEvent();
222                     mPollFds[i].revents = 0;
223                 } else if (i == compass) {
224                     ((MPLSensor*) mSensor)->buildCompassEvent();
225                     mPollFds[i].revents = 0;
226                 } else if (i == dmpOrient) {
227                     nb = ((MPLSensor*)mSensor)->
228                                         readDmpOrientEvents(data, count);
229                     mPollFds[dmpOrient].revents= 0;
230                     if (isDmpScreenAutoRotationEnabled() && nb > 0) {
231                         count -= nb;
232                         nbEvents += nb;
233                         data += nb;
234                     }
235                 } else if (i == dmpSign) {
236                     LOGI("HAL: dmpSign interrupt");
237                     nb = ((MPLSensor*) mSensor)->
238                                     readDmpSignificantMotionEvents(data, count);
239                     mPollFds[i].revents = 0;
240                     count -= nb;
241                     nbEvents += nb;
242                     data += nb;
243                 } else if (i == dmpPed) {
244                     LOGI("HAL: dmpPed interrupt");
245                     nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents(
246                             data, count, ID_P, SENSOR_TYPE_STEP_DETECTOR, 0);
247                     mPollFds[i].revents = 0;
248                     count -= nb;
249                     nbEvents += nb;
250                     data += nb;
251                 }
252                 #if 1
253                 if(nb == 0) {
254                     nb = ((MPLSensor*) mSensor)->readEvents(data, count);
255                     LOGI_IF(0, "sensors_mpl:readEvents() - "
256                             "i=%d, nb=%d, count=%d, nbEvents=%d, "
257                             "data->timestamp=%lld, data->data[0]=%f,",
258                             i, nb, count, nbEvents, data->timestamp,
259                             data->data[0]);
260                     if (nb > 0) {
261                         count -= nb;
262                         nbEvents += nb;
263                         data += nb;
264                     }
265                 }
266                 #endif
267             }
268         }
269 
270         /* to see if any step counter events */
271         if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) {
272             nb = 0;
273             nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents(
274                             data, count, ID_SC, SENSOR_TYPE_STEP_COUNTER, 0);
275             LOGI_IF(SensorBase::HANDLER_DATA, "sensors_mpl:readStepCount() - "
276                     "nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, "
277                     "data->data[0]=%f,",
278                     nb, count, nbEvents, data->timestamp, data->data[0]);
279             if (nb > 0) {
280                 count -= nb;
281                 nbEvents += nb;
282                 data += nb;
283             }
284         }
285     } else if(nb == 0) {
286         /* to see if any step counter events */
287         if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) {
288             nb = 0;
289             nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents(
290                             data, count, ID_SC, SENSOR_TYPE_STEP_COUNTER, 0);
291             LOGI_IF(SensorBase::HANDLER_DATA, "sensors_mpl:readStepCount() - "
292                     "nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, "
293                     "data->data[0]=%f,",
294                     nb, count, nbEvents, data->timestamp, data->data[0]);
295             if (nb > 0) {
296                 count -= nb;
297                 nbEvents += nb;
298                 data += nb;
299             }
300         }
301 
302         if (mPollFds[numSensorDrivers].revents & POLLIN) {
303             char msg;
304             int result = read(mPollFds[numSensorDrivers].fd, &msg, 1);
305             LOGE_IF(result < 0,
306                     "error reading from wake pipe (%s)", strerror(errno));
307             mPollFds[numSensorDrivers].revents = 0;
308         }
309     }
310     return nbEvents;
311 }
312 
query(int what,int * value)313 int sensors_poll_context_t::query(int what, int* value)
314 {
315     FUNC_LOG;
316     return mSensor->query(what, value);
317 }
318 
batch(int handle,int flags,int64_t period_ns,int64_t timeout)319 int sensors_poll_context_t::batch(int handle, int flags, int64_t period_ns,
320                                   int64_t timeout)
321 {
322     FUNC_LOG;
323     return mSensor->batch(handle, flags, period_ns, timeout);
324 }
325 
flush(int handle)326 int sensors_poll_context_t::flush(int handle)
327 {
328     FUNC_LOG;
329     return mSensor->flush(handle);
330 }
331 
332 /******************************************************************************/
333 
poll__close(struct hw_device_t * dev)334 static int poll__close(struct hw_device_t *dev)
335 {
336     FUNC_LOG;
337     sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
338     if (ctx) {
339         delete ctx;
340     }
341     return 0;
342 }
343 
poll__activate(struct sensors_poll_device_t * dev,int handle,int enabled)344 static int poll__activate(struct sensors_poll_device_t *dev,
345                           int handle, int enabled)
346 {
347     sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
348     return ctx->activate(handle, enabled);
349 }
350 
poll__setDelay(struct sensors_poll_device_t * dev,int handle,int64_t ns)351 static int poll__setDelay(struct sensors_poll_device_t *dev,
352                           int handle, int64_t ns)
353 {
354     sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
355     int s= ctx->setDelay(handle, ns);
356     return s;
357 }
358 
poll__poll(struct sensors_poll_device_t * dev,sensors_event_t * data,int count)359 static int poll__poll(struct sensors_poll_device_t *dev,
360                       sensors_event_t* data, int count)
361 {
362     sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
363     return ctx->pollEvents(data, count);
364 }
365 
poll__batch(struct sensors_poll_device_1 * dev,int handle,int flags,int64_t period_ns,int64_t timeout)366 static int poll__batch(struct sensors_poll_device_1 *dev,
367                       int handle, int flags, int64_t period_ns, int64_t timeout)
368 {
369     sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
370     return ctx->batch(handle, flags, period_ns, timeout);
371 }
372 
poll__flush(struct sensors_poll_device_1 * dev,int handle)373 static int poll__flush(struct sensors_poll_device_1 *dev,
374                       int handle)
375 {
376     sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
377     return ctx->flush(handle);
378 }
379 /******************************************************************************/
380 
381 /** 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)382 static int open_sensors(const struct hw_module_t* module, const char* id,
383                         struct hw_device_t** device)
384 {
385     FUNC_LOG;
386     int status = -EINVAL;
387     sensors_poll_context_t *dev = new sensors_poll_context_t();
388 
389     memset(&dev->device, 0, sizeof(sensors_poll_device_1));
390 
391     dev->device.common.tag = HARDWARE_DEVICE_TAG;
392     dev->device.common.version  = SENSORS_DEVICE_API_VERSION_1_0;
393     dev->device.common.module   = const_cast<hw_module_t*>(module);
394     dev->device.common.close    = poll__close;
395     dev->device.activate        = poll__activate;
396     dev->device.setDelay        = poll__setDelay;
397     dev->device.poll            = poll__poll;
398 
399     /* Batch processing */
400     dev->device.batch           = poll__batch;
401     dev->device.flush           = poll__flush;
402 
403     *device = &dev->device.common;
404     status = 0;
405 
406     return status;
407 }
408