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 36 #include "sensors.h" 37 #include "MPLSensor.h" 38 39 /* 40 * Vendor-defined Accel Load Calibration File Method 41 * @param[out] Accel bias, length 3. In HW units scaled by 2^16 in body frame 42 * @return '0' for a successful load, '1' otherwise 43 * example: int AccelLoadConfig(long* offset); 44 * End of Vendor-defined Accel Load Cal Method 45 */ 46 47 /*****************************************************************************/ 48 /* The SENSORS Module */ 49 50 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION 51 #define LOCAL_SENSORS (NumSensors + 1) 52 #else 53 #define LOCAL_SENSORS (NumSensors) 54 #endif 55 56 struct handle_entry { 57 SIMPLEQ_ENTRY(handle_entry) entries; 58 int handle; 59 }; 60 61 static SIMPLEQ_HEAD(simplehead, handle_entry) pending_flush_items_head; 62 struct simplehead *headp; 63 static pthread_mutex_t flush_handles_mutex = PTHREAD_MUTEX_INITIALIZER; 64 65 static const char *smdWakelockStr = "significant motion"; 66 67 static struct sensor_t sSensorList[LOCAL_SENSORS]; 68 static int sensors = (sizeof(sSensorList) / sizeof(sensor_t)); 69 70 static int open_sensors(const struct hw_module_t* module, const char* id, 71 struct hw_device_t** device); 72 73 static int sensors__get_sensors_list(struct sensors_module_t* module, 74 struct sensor_t const** list) 75 { 76 *list = sSensorList; 77 return sensors; 78 } 79 80 static struct hw_module_methods_t sensors_module_methods = { 81 open: open_sensors 82 }; 83 84 struct sensors_module_t HAL_MODULE_INFO_SYM = { 85 common: { 86 tag: HARDWARE_MODULE_TAG, 87 version_major: 1, 88 version_minor: 0, 89 id: SENSORS_HARDWARE_MODULE_ID, 90 name: "Invensense module", 91 author: "Invensense Inc.", 92 methods: &sensors_module_methods, 93 dso: NULL, 94 reserved: {0} 95 }, 96 get_sensors_list: sensors__get_sensors_list, 97 }; 98 99 struct sensors_poll_context_t { 100 sensors_poll_device_1_t device; // must be first 101 102 sensors_poll_context_t(); 103 ~sensors_poll_context_t(); 104 int activate(int handle, int enabled); 105 int setDelay(int handle, int64_t ns); 106 int pollEvents(sensors_event_t* data, int count); 107 int query(int what, int *value); 108 int batch(int handle, int flags, int64_t period_ns, int64_t timeout); 109 #if defined ANDROID_KITKAT || defined ANDROID_LOLLIPOP 110 int flush(int handle); 111 #endif 112 113 private: 114 enum { 115 mpl = 0, 116 compass, 117 dmpOrient, 118 dmpSign, 119 dmpPed, 120 numSensorDrivers, 121 numFds, 122 }; 123 124 struct pollfd mPollFds[numFds]; 125 SensorBase *mSensor; 126 CompassSensor *mCompassSensor; 127 128 /* Significant Motion wakelock support */ 129 bool mSMDWakelockHeld; 130 131 }; 132 133 /******************************************************************************/ 134 135 sensors_poll_context_t::sensors_poll_context_t() { 136 VFUNC_LOG; 137 138 /* TODO: Handle external pressure sensor */ 139 mCompassSensor = new CompassSensor(); 140 MPLSensor *mplSensor = new MPLSensor(mCompassSensor); 141 142 /* No significant motion events pending yet */ 143 mSMDWakelockHeld = false; 144 145 /* For Vendor-defined Accel Calibration File Load 146 * Use the Following Constructor and Pass Your Load Cal File Function 147 * 148 * MPLSensor *mplSensor = new MPLSensor(mCompassSensor, AccelLoadConfig); 149 */ 150 151 // Initialize pending flush queue 152 SIMPLEQ_INIT(&pending_flush_items_head); 153 154 // populate the sensor list 155 sensors = 156 mplSensor->populateSensorList(sSensorList, sizeof(sSensorList)); 157 158 mSensor = mplSensor; 159 mPollFds[mpl].fd = mSensor->getFd(); 160 mPollFds[mpl].events = POLLIN; 161 mPollFds[mpl].revents = 0; 162 163 mPollFds[compass].fd = mCompassSensor->getFd(); 164 mPollFds[compass].events = POLLIN; 165 mPollFds[compass].revents = 0; 166 167 mPollFds[dmpOrient].fd = ((MPLSensor*) mSensor)->getDmpOrientFd(); 168 mPollFds[dmpOrient].events = POLLPRI; 169 mPollFds[dmpOrient].revents = 0; 170 171 mPollFds[dmpSign].fd = ((MPLSensor*) mSensor)->getDmpSignificantMotionFd(); 172 mPollFds[dmpSign].events = POLLPRI; 173 mPollFds[dmpSign].revents = 0; 174 175 mPollFds[dmpPed].fd = ((MPLSensor*) mSensor)->getDmpPedometerFd(); 176 mPollFds[dmpPed].events = POLLPRI; 177 mPollFds[dmpPed].revents = 0; 178 } 179 180 sensors_poll_context_t::~sensors_poll_context_t() { 181 FUNC_LOG; 182 delete mSensor; 183 delete mCompassSensor; 184 for (int i = 0; i < numSensorDrivers; i++) { 185 close(mPollFds[i].fd); 186 } 187 } 188 189 int sensors_poll_context_t::activate(int handle, int enabled) { 190 FUNC_LOG; 191 192 int err; 193 err = mSensor->enable(handle, enabled); 194 return err; 195 } 196 197 int sensors_poll_context_t::setDelay(int handle, int64_t ns) 198 { 199 FUNC_LOG; 200 return mSensor->setDelay(handle, ns); 201 } 202 203 int 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 if (mSMDWakelockHeld) { 211 mSMDWakelockHeld = false; 212 release_wake_lock(smdWakelockStr); 213 } 214 215 struct handle_entry *handle_element; 216 pthread_mutex_lock(&flush_handles_mutex); 217 if (!SIMPLEQ_EMPTY(&pending_flush_items_head)) { 218 sensors_event_t flushCompleteEvent; 219 flushCompleteEvent.type = SENSOR_TYPE_META_DATA; 220 flushCompleteEvent.sensor = 0; 221 handle_element = SIMPLEQ_FIRST(&pending_flush_items_head); 222 flushCompleteEvent.meta_data.sensor = handle_element->handle; 223 SIMPLEQ_REMOVE_HEAD(&pending_flush_items_head, entries); 224 free(handle_element); 225 memcpy(data, (void *) &flushCompleteEvent, sizeof(flushCompleteEvent)); 226 LOGI_IF(1, "pollEvents() Returning fake flush event completion for handle %d", 227 flushCompleteEvent.meta_data.sensor); 228 pthread_mutex_unlock(&flush_handles_mutex); 229 return 1; 230 } 231 pthread_mutex_unlock(&flush_handles_mutex); 232 233 polltime = ((MPLSensor*) mSensor)->getStepCountPollTime(); 234 235 // look for new events 236 nb = poll(mPollFds, numSensorDrivers, polltime); 237 LOGI_IF(0, "poll nb=%d, count=%d, pt=%d", nb, count, polltime); 238 if (nb > 0) { 239 for (int i = 0; count && i < numSensorDrivers; i++) { 240 if (mPollFds[i].revents & (POLLIN | POLLPRI)) { 241 nb = 0; 242 if (i == mpl) { 243 ((MPLSensor*) mSensor)->buildMpuEvent(); 244 mPollFds[i].revents = 0; 245 } else if (i == compass) { 246 ((MPLSensor*) mSensor)->buildCompassEvent(); 247 mPollFds[i].revents = 0; 248 } else if (i == dmpOrient) { 249 nb = ((MPLSensor*)mSensor)-> 250 readDmpOrientEvents(data, count); 251 mPollFds[dmpOrient].revents= 0; 252 if (isDmpScreenAutoRotationEnabled() && nb > 0) { 253 count -= nb; 254 nbEvents += nb; 255 data += nb; 256 } 257 } else if (i == dmpSign) { 258 nb = ((MPLSensor*) mSensor)-> 259 readDmpSignificantMotionEvents(data, count); 260 mPollFds[i].revents = 0; 261 if (nb) { 262 if (!mSMDWakelockHeld) { 263 /* Hold wakelock until Sensor Services reads event */ 264 acquire_wake_lock(PARTIAL_WAKE_LOCK, smdWakelockStr); 265 LOGI_IF(1, "HAL: grabbed %s wakelock", smdWakelockStr); 266 mSMDWakelockHeld = true; 267 } 268 269 count -= nb; 270 nbEvents += nb; 271 data += nb; 272 } 273 } else if (i == dmpPed) { 274 nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents( 275 data, count, ID_P, 0); 276 mPollFds[i].revents = 0; 277 count -= nb; 278 nbEvents += nb; 279 data += nb; 280 } 281 if(nb == 0) { 282 nb = ((MPLSensor*) mSensor)->readEvents(data, count); 283 LOGI_IF(0, "sensors_mpl:readEvents() - " 284 "i=%d, nb=%d, count=%d, nbEvents=%d, " 285 "data->timestamp=%lld, data->data[0]=%f,", 286 i, nb, count, nbEvents, data->timestamp, 287 data->data[0]); 288 if (nb > 0) { 289 count -= nb; 290 nbEvents += nb; 291 data += nb; 292 } 293 } 294 } 295 } 296 297 /* to see if any step counter events */ 298 if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) { 299 nb = 0; 300 nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents( 301 data, count, ID_SC, 0); 302 LOGI_IF(SensorBase::HANDLER_DATA, "sensors_mpl:readStepCount() - " 303 "nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, ", 304 nb, count, nbEvents, data->timestamp); 305 if (nb > 0) { 306 count -= nb; 307 nbEvents += nb; 308 data += nb; 309 } 310 } 311 } else if(nb == 0) { 312 /* to see if any step counter events */ 313 if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) { 314 nb = 0; 315 nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents( 316 data, count, ID_SC, 0); 317 LOGI_IF(SensorBase::HANDLER_DATA, "sensors_mpl:readStepCount() - " 318 "nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, ", 319 nb, count, nbEvents, data->timestamp); 320 if (nb > 0) { 321 count -= nb; 322 nbEvents += nb; 323 data += nb; 324 } 325 } 326 } 327 return nbEvents; 328 } 329 330 int sensors_poll_context_t::query(int what, int* value) 331 { 332 FUNC_LOG; 333 return mSensor->query(what, value); 334 } 335 336 int sensors_poll_context_t::batch(int handle, int flags, int64_t period_ns, 337 int64_t timeout) 338 { 339 FUNC_LOG; 340 return mSensor->batch(handle, flags, period_ns, timeout); 341 } 342 343 #if defined ANDROID_KITKAT || defined ANDROID_LOLLIPOP 344 345 void inv_pending_flush(int handle) { 346 struct handle_entry *the_entry; 347 pthread_mutex_lock(&flush_handles_mutex); 348 the_entry = (struct handle_entry*) malloc(sizeof(struct handle_entry)); 349 if (the_entry != NULL) { 350 LOGI_IF(0, "Inserting %d into pending list", handle); 351 the_entry->handle = handle; 352 SIMPLEQ_INSERT_TAIL(&pending_flush_items_head, the_entry, entries); 353 } else { 354 LOGE("ERROR malloc'ing space for pending handler flush entry"); 355 } 356 pthread_mutex_unlock(&flush_handles_mutex); 357 } 358 359 int sensors_poll_context_t::flush(int handle) 360 { 361 FUNC_LOG; 362 return mSensor->flush(handle); 363 } 364 #endif 365 366 /******************************************************************************/ 367 368 static int poll__close(struct hw_device_t *dev) 369 { 370 FUNC_LOG; 371 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 372 if (ctx) { 373 delete ctx; 374 } 375 return 0; 376 } 377 378 static int poll__activate(struct sensors_poll_device_t *dev, 379 int handle, int enabled) 380 { 381 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 382 return ctx->activate(handle, enabled); 383 } 384 385 static int poll__setDelay(struct sensors_poll_device_t *dev, 386 int handle, int64_t ns) 387 { 388 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 389 int s= ctx->setDelay(handle, ns); 390 return s; 391 } 392 393 static int poll__poll(struct sensors_poll_device_t *dev, 394 sensors_event_t* data, int count) 395 { 396 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 397 return ctx->pollEvents(data, count); 398 } 399 400 static int poll__query(struct sensors_poll_device_1 *dev, 401 int what, int *value) 402 { 403 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 404 return ctx->query(what, value); 405 } 406 407 static int poll__batch(struct sensors_poll_device_1 *dev, 408 int handle, int flags, int64_t period_ns, int64_t timeout) 409 { 410 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 411 return ctx->batch(handle, flags, period_ns, timeout); 412 } 413 414 #if defined ANDROID_KITKAT || defined ANDROID_LOLLIPOP 415 static int poll__flush(struct sensors_poll_device_1 *dev, 416 int handle) 417 { 418 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 419 int status = ctx->flush(handle); 420 if (handle == SENSORS_STEP_COUNTER_HANDLE) { 421 LOGI_IF(0, "creating flush completion event for handle %d", handle); 422 inv_pending_flush(handle); 423 return 0; 424 } 425 return status; 426 } 427 #endif 428 /******************************************************************************/ 429 430 /** Open a new instance of a sensor device using name */ 431 static int open_sensors(const struct hw_module_t* module, const char* id, 432 struct hw_device_t** device) 433 { 434 FUNC_LOG; 435 int status = -EINVAL; 436 sensors_poll_context_t *dev = new sensors_poll_context_t(); 437 438 memset(&dev->device, 0, sizeof(sensors_poll_device_1)); 439 440 dev->device.common.tag = HARDWARE_DEVICE_TAG; 441 dev->device.common.version = SENSORS_DEVICE_API_VERSION_1_3; 442 dev->device.flush = poll__flush; 443 dev->device.common.module = const_cast<hw_module_t*>(module); 444 dev->device.common.close = poll__close; 445 dev->device.activate = poll__activate; 446 dev->device.setDelay = poll__setDelay; 447 dev->device.poll = poll__poll; 448 dev->device.batch = poll__batch; 449 450 *device = &dev->device.common; 451 status = 0; 452 453 return status; 454 } 455
