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