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