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 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 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 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 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 196 int sensors_poll_context_t::setDelay(int handle, int64_t ns) 197 { 198 FUNC_LOG; 199 return mSensor->setDelay(handle, ns); 200 } 201 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 304 int sensors_poll_context_t::query(int what, int* value) 305 { 306 FUNC_LOG; 307 return mSensor->query(what, value); 308 } 309 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 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 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 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 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 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 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 */ 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
