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 <utils/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 };
84
85 struct sensors_poll_context_t {
86 sensors_poll_device_1_t device; // must be first
87
88 sensors_poll_context_t();
89 ~sensors_poll_context_t();
90 int activate(int handle, int enabled);
91 int setDelay(int handle, int64_t ns);
92 int pollEvents(sensors_event_t* data, int count);
93 int query(int what, int *value);
94 int batch(int handle, int flags, int64_t period_ns, int64_t timeout);
95
96 private:
97 enum {
98 mpl = 0,
99 compass,
100 dmpOrient,
101 dmpSign,
102 dmpPed,
103 numSensorDrivers, // wake pipe goes here
104 numFds,
105 };
106
107 struct pollfd mPollFds[numFds];
108 SensorBase *mSensor;
109 CompassSensor *mCompassSensor;
110
111 static const size_t wake = numSensorDrivers;
112 static const char WAKE_MESSAGE = 'W';
113 int mWritePipeFd;
114 };
115
116 /******************************************************************************/
117
sensors_poll_context_t()118 sensors_poll_context_t::sensors_poll_context_t() {
119 VFUNC_LOG;
120
121 /* TODO: Handle external pressure sensor */
122 mCompassSensor = new CompassSensor();
123 MPLSensor *mplSensor = new MPLSensor(mCompassSensor);
124
125 /* For Vendor-defined Accel Calibration File Load
126 * Use the Following Constructor and Pass Your Load Cal File Function
127 *
128 * MPLSensor *mplSensor = new MPLSensor(mCompassSensor, AccelLoadConfig);
129 */
130
131 // setup the callback object for handing mpl callbacks
132 setCallbackObject(mplSensor);
133
134 // populate the sensor list
135 sensors =
136 mplSensor->populateSensorList(sSensorList, sizeof(sSensorList));
137
138 mSensor = mplSensor;
139 mPollFds[mpl].fd = mSensor->getFd();
140 mPollFds[mpl].events = POLLIN;
141 mPollFds[mpl].revents = 0;
142
143 mPollFds[compass].fd = mCompassSensor->getFd();
144 mPollFds[compass].events = POLLIN;
145 mPollFds[compass].revents = 0;
146
147 mPollFds[dmpOrient].fd = ((MPLSensor*) mSensor)->getDmpOrientFd();
148 mPollFds[dmpOrient].events = POLLPRI;
149 mPollFds[dmpOrient].revents = 0;
150
151 mPollFds[dmpSign].fd = ((MPLSensor*) mSensor)->getDmpSignificantMotionFd();
152 mPollFds[dmpSign].events = POLLPRI;
153 mPollFds[dmpSign].revents = 0;
154
155 mPollFds[dmpPed].fd = ((MPLSensor*) mSensor)->getDmpPedometerFd();
156 mPollFds[dmpPed].events = POLLPRI;
157 mPollFds[dmpPed].revents = 0;
158
159 /* Timer based sensor initialization */
160 int wakeFds[2];
161 int result = pipe(wakeFds);
162 LOGE_IF(result<0, "error creating wake pipe (%s)", strerror(errno));
163 fcntl(wakeFds[0], F_SETFL, O_NONBLOCK);
164 fcntl(wakeFds[1], F_SETFL, O_NONBLOCK);
165 mWritePipeFd = wakeFds[1];
166
167 mPollFds[numSensorDrivers].fd = wakeFds[0];
168 mPollFds[numSensorDrivers].events = POLLIN;
169 mPollFds[numSensorDrivers].revents = 0;
170 }
171
~sensors_poll_context_t()172 sensors_poll_context_t::~sensors_poll_context_t() {
173 FUNC_LOG;
174 delete mSensor;
175 delete mCompassSensor;
176 for (int i = 0; i < numSensorDrivers; i++) {
177 close(mPollFds[i].fd);
178 }
179 close(mWritePipeFd);
180 }
181
activate(int handle,int enabled)182 int sensors_poll_context_t::activate(int handle, int enabled) {
183 FUNC_LOG;
184
185 int err;
186 err = mSensor->enable(handle, enabled);
187 if (!err) {
188 const char wakeMessage(WAKE_MESSAGE);
189 int result = write(mWritePipeFd, &wakeMessage, 1);
190 LOGE_IF(result < 0,
191 "error sending wake message (%s)", strerror(errno));
192 }
193 return err;
194 }
195
setDelay(int handle,int64_t ns)196 int sensors_poll_context_t::setDelay(int handle, int64_t ns)
197 {
198 FUNC_LOG;
199 return mSensor->setDelay(handle, ns);
200 }
201
pollEvents(sensors_event_t * data,int count)202 int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count)
203 {
204 VHANDLER_LOG;
205
206 int nbEvents = 0;
207 int nb, polltime = -1;
208
209 polltime = ((MPLSensor*) mSensor)->getStepCountPollTime();
210
211 // look for new events
212 nb = poll(mPollFds, numFds, polltime);
213 LOGI_IF(0, "poll nb=%d, count=%d, pt=%d", nb, count, polltime);
214 if (nb > 0) {
215 for (int i = 0; count && i < numSensorDrivers; i++) {
216 if (mPollFds[i].revents & (POLLIN | POLLPRI)) {
217 LOGI_IF(0, "poll found=%d", i);
218 nb = 0;
219 if (i == mpl) {
220 ((MPLSensor*) mSensor)->buildMpuEvent();
221 mPollFds[i].revents = 0;
222 } else if (i == compass) {
223 ((MPLSensor*) mSensor)->buildCompassEvent();
224 mPollFds[i].revents = 0;
225 } else if (i == dmpOrient) {
226 nb = ((MPLSensor*) mSensor)->readDmpOrientEvents(data, count);
227 mPollFds[dmpOrient].revents= 0;
228 if (isDmpScreenAutoRotationEnabled() && nb > 0) {
229 count -= nb;
230 nbEvents += nb;
231 data += nb;
232 }
233 } else if (i == dmpSign) {
234 LOGI("HAL: dmpSign interrupt");
235 nb = ((MPLSensor*) mSensor)->readDmpSignificantMotionEvents(data, count);
236 mPollFds[i].revents = 0;
237 count -= nb;
238 nbEvents += nb;
239 data += nb;
240 } else if (i == dmpPed) {
241 LOGI("HAL: dmpPed interrupt");
242 nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents(data, count, ID_P, SENSOR_TYPE_STEP_DETECTOR, 0);
243 mPollFds[i].revents = 0;
244 count -= nb;
245 nbEvents += nb;
246 data += nb;
247 }
248 nb = ((MPLSensor*) mSensor)->readEvents(data, count);
249 LOGI_IF(0, "sensors_mpl:readEvents() - i=%d, nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, data->data[0]=%f,",
250 i, nb, count, nbEvents, data->timestamp, data->data[0]);
251 if (nb > 0) {
252 count -= nb;
253 nbEvents += nb;
254 data += nb;
255 }
256 }
257 }
258 // nb = ((MPLSensor*) mSensor)->readEvents(data, count);
259 // LOGI_IF(0, "sensors_mpl:readEvents() - nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, data->data[0]=%f,",
260 // nb, count, nbEvents, data->timestamp, data->data[0]);
261 // if (nb > 0) {
262 // count -= nb;
263 // nbEvents += nb;
264 // data += nb;
265 // }
266
267 /* to see if any step counter events */
268 if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) {
269 nb = 0;
270 nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents(data, count, ID_SC, SENSOR_TYPE_STEP_COUNTER, 0);
271 LOGI_IF(HANDLER_DATA, "sensors_mpl:readStepCount() - nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, data->data[0]=%f,",
272 nb, count, nbEvents, data->timestamp, data->data[0]);
273 if (nb > 0) {
274 count -= nb;
275 nbEvents += nb;
276 data += nb;
277 }
278 }
279 } else if(nb == 0){
280
281 /* to see if any step counter events */
282 if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) {
283 nb = 0;
284 nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents(data, count, ID_SC, SENSOR_TYPE_STEP_COUNTER, 0);
285 LOGI_IF(HANDLER_DATA, "sensors_mpl:readStepCount() - nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, data->data[0]=%f,",
286 nb, count, nbEvents, data->timestamp, data->data[0]);
287 if (nb > 0) {
288 count -= nb;
289 nbEvents += nb;
290 data += nb;
291 }
292 }
293
294 if (mPollFds[numSensorDrivers].revents & POLLIN) {
295 char msg;
296 int result = read(mPollFds[numSensorDrivers].fd, &msg, 1);
297 LOGE_IF(result < 0, "error reading from wake pipe (%s)", strerror(errno));
298 mPollFds[numSensorDrivers].revents = 0;
299 }
300 }
301 return nbEvents;
302 }
303
query(int what,int * value)304 int sensors_poll_context_t::query(int what, int* value)
305 {
306 FUNC_LOG;
307 return mSensor->query(what, value);
308 }
309
batch(int handle,int flags,int64_t period_ns,int64_t timeout)310 int sensors_poll_context_t::batch(int handle, int flags, int64_t period_ns, int64_t timeout)
311 {
312 FUNC_LOG;
313 return mSensor->batch(handle, flags, period_ns, timeout);
314 }
315
316 /******************************************************************************/
317
poll__close(struct hw_device_t * dev)318 static int poll__close(struct hw_device_t *dev)
319 {
320 FUNC_LOG;
321 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
322 if (ctx) {
323 delete ctx;
324 }
325 return 0;
326 }
327
poll__activate(struct sensors_poll_device_t * dev,int handle,int enabled)328 static int poll__activate(struct sensors_poll_device_t *dev,
329 int handle, int enabled)
330 {
331 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
332 return ctx->activate(handle, enabled);
333 }
334
poll__setDelay(struct sensors_poll_device_t * dev,int handle,int64_t ns)335 static int poll__setDelay(struct sensors_poll_device_t *dev,
336 int handle, int64_t ns)
337 {
338 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
339 int s= ctx->setDelay(handle, ns);
340 return s;
341 }
342
poll__poll(struct sensors_poll_device_t * dev,sensors_event_t * data,int count)343 static int poll__poll(struct sensors_poll_device_t *dev,
344 sensors_event_t* data, int count)
345 {
346 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
347 return ctx->pollEvents(data, count);
348 }
349
poll__query(struct sensors_poll_device_1 * dev,int what,int * value)350 static int poll__query(struct sensors_poll_device_1 *dev,
351 int what, int *value)
352 {
353 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
354 return ctx->query(what, value);
355 }
356
poll__batch(struct sensors_poll_device_1 * dev,int handle,int flags,int64_t period_ns,int64_t timeout)357 static int poll__batch(struct sensors_poll_device_1 *dev,
358 int handle, int flags, int64_t period_ns, int64_t timeout)
359 {
360 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
361 return ctx->batch(handle, flags, period_ns, timeout);
362 }
363
364 /******************************************************************************/
365
366 /** 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)367 static int open_sensors(const struct hw_module_t* module, const char* id,
368 struct hw_device_t** device)
369 {
370 FUNC_LOG;
371 int status = -EINVAL;
372 sensors_poll_context_t *dev = new sensors_poll_context_t();
373
374 memset(&dev->device, 0, sizeof(sensors_poll_device_1));
375
376 dev->device.common.tag = HARDWARE_DEVICE_TAG;
377 dev->device.common.version = SENSORS_DEVICE_API_VERSION_1_0;
378 dev->device.common.module = const_cast<hw_module_t*>(module);
379 dev->device.common.close = poll__close;
380 dev->device.activate = poll__activate;
381 dev->device.setDelay = poll__setDelay;
382 dev->device.poll = poll__poll;
383
384 /* Batch processing */
385 dev->device.batch = poll__batch;
386
387 *device = &dev->device.common;
388 status = 0;
389
390 return status;
391 }
392