1 /* 2 * Copyright (C) 2015 The Android Open Source Project 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 LOG_TAG "nanohub" 18 19 #include "hubconnection.h" 20 21 #include "file.h" 22 #include "JSONObject.h" 23 24 #include <errno.h> 25 #include <unistd.h> 26 #include <math.h> 27 #include <inttypes.h> 28 #include <sched.h> 29 #include <sys/inotify.h> 30 31 #include <linux/input.h> 32 #include <linux/uinput.h> 33 34 #include <cutils/ashmem.h> 35 #include <cutils/properties.h> 36 #include <hardware_legacy/power.h> 37 #include <media/stagefright/foundation/ADebug.h> 38 39 #include <algorithm> 40 #include <cmath> 41 #include <sstream> 42 #include <vector> 43 44 #define APP_ID_GET_VENDOR(appid) ((appid) >> 24) 45 #define APP_ID_MAKE(vendor, app) ((((uint64_t)(vendor)) << 24) | ((app) & 0x00FFFFFF)) 46 #define APP_ID_VENDOR_GOOGLE 0x476f6f676cULL // "Googl" 47 #define APP_ID_APP_BMI160 2 48 #define APP_ID_APP_WRIST_TILT_DETECT 0x1005 49 #define APP_ID_APP_GAZE_DETECT 0x1009 50 #define APP_ID_APP_UNGAZE_DETECT 0x100a 51 52 #define SENS_TYPE_TO_EVENT(_sensorType) (EVT_NO_FIRST_SENSOR_EVENT + (_sensorType)) 53 54 #define NANOHUB_FILE_PATH "/dev/nanohub" 55 #define NANOHUB_LOCK_DIR "/data/vendor/sensor/nanohub_lock" 56 #define NANOHUB_LOCK_FILE NANOHUB_LOCK_DIR "/lock" 57 #define MAG_BIAS_FILE_PATH "/sys/class/power_supply/battery/compass_compensation" 58 #define DOUBLE_TOUCH_FILE_PATH "/sys/android_touch/synaptics_rmi4_dsx/wake_event" 59 60 #define NANOHUB_LOCK_DIR_PERMS (S_IRUSR | S_IWUSR | S_IXUSR) 61 62 #define SENSOR_RATE_ONCHANGE 0xFFFFFF01UL 63 #define SENSOR_RATE_ONESHOT 0xFFFFFF02UL 64 65 #define MIN_MAG_SQ (10.0f * 10.0f) 66 #define MAX_MAG_SQ (80.0f * 80.0f) 67 68 #define OS_LOG_EVENT 0x474F4C41 // ascii: ALOG 69 70 #define MAX_RETRY_CNT 5 71 72 #ifdef LID_STATE_REPORTING_ENABLED 73 const char LID_STATE_PROPERTY[] = "sensors.contexthub.lid_state"; 74 const char LID_STATE_UNKNOWN[] = "unknown"; 75 const char LID_STATE_OPEN[] = "open"; 76 const char LID_STATE_CLOSED[] = "closed"; 77 #endif // LID_STATE_REPORTING_ENABLED 78 79 constexpr int HUBCONNECTION_SCHED_FIFO_PRIORITY = 3; 80 81 static const uint32_t delta_time_encoded = 1; 82 static const uint32_t delta_time_shift_table[2] = {9, 0}; 83 84 namespace android { 85 86 // static 87 Mutex HubConnection::sInstanceLock; 88 89 // static 90 HubConnection *HubConnection::sInstance = NULL; 91 92 HubConnection *HubConnection::getInstance() 93 { 94 Mutex::Autolock autoLock(sInstanceLock); 95 if (sInstance == NULL) { 96 sInstance = new HubConnection; 97 } 98 return sInstance; 99 } 100 101 static bool isWakeEvent(int32_t sensor) 102 { 103 switch (sensor) { 104 case COMMS_SENSOR_DOUBLE_TOUCH: 105 case COMMS_SENSOR_DOUBLE_TWIST: 106 case COMMS_SENSOR_GESTURE: 107 case COMMS_SENSOR_PROXIMITY: 108 case COMMS_SENSOR_SIGNIFICANT_MOTION: 109 case COMMS_SENSOR_TILT: 110 return true; 111 default: 112 return false; 113 } 114 } 115 116 HubConnection::HubConnection() 117 : Thread(false /* canCallJava */), 118 mRing(10 *1024), 119 mScaleAccel(1.0f), 120 mScaleMag(1.0f), 121 mStepCounterOffset(0ull), 122 mLastStepCount(0ull) 123 { 124 mMagBias[0] = mMagBias[1] = mMagBias[2] = 0.0f; 125 mMagAccuracy = SENSOR_STATUS_UNRELIABLE; 126 mMagAccuracyRestore = SENSOR_STATUS_UNRELIABLE; 127 mGyroBias[0] = mGyroBias[1] = mGyroBias[2] = 0.0f; 128 mAccelBias[0] = mAccelBias[1] = mAccelBias[2] = 0.0f; 129 mAccelEnabledBias[0] = mAccelEnabledBias[1] = mAccelEnabledBias[2] = 0.0f; 130 mAccelEnabledBiasStored = true; 131 memset(&mGyroOtcData, 0, sizeof(mGyroOtcData)); 132 133 mLefty.accel = false; 134 mLefty.gyro = false; 135 mLefty.hub = false; 136 137 memset(&mSensorState, 0x00, sizeof(mSensorState)); 138 mFd = open(NANOHUB_FILE_PATH, O_RDWR); 139 mPollFds[0].fd = mFd; 140 mPollFds[0].events = POLLIN; 141 mPollFds[0].revents = 0; 142 mNumPollFds = 1; 143 144 mWakelockHeld = false; 145 mWakeEventCount = 0; 146 mWriteFailures = 0; 147 148 initNanohubLock(); 149 150 #ifdef USB_MAG_BIAS_REPORTING_ENABLED 151 mUsbMagBias = 0; 152 mMagBiasPollIndex = -1; 153 int magBiasFd = open(MAG_BIAS_FILE_PATH, O_RDONLY); 154 if (magBiasFd < 0) { 155 ALOGW("Mag bias file open failed: %s", strerror(errno)); 156 } else { 157 mPollFds[mNumPollFds].fd = magBiasFd; 158 mPollFds[mNumPollFds].events = 0; 159 mPollFds[mNumPollFds].revents = 0; 160 mMagBiasPollIndex = mNumPollFds; 161 mNumPollFds++; 162 } 163 #endif // USB_MAG_BIAS_REPORTING_ENABLED 164 165 #ifdef DOUBLE_TOUCH_ENABLED 166 mDoubleTouchPollIndex = -1; 167 int doubleTouchFd = open(DOUBLE_TOUCH_FILE_PATH, O_RDONLY); 168 if (doubleTouchFd < 0) { 169 ALOGW("Double touch file open failed: %s", strerror(errno)); 170 } else { 171 mPollFds[mNumPollFds].fd = doubleTouchFd; 172 mPollFds[mNumPollFds].events = 0; 173 mPollFds[mNumPollFds].revents = 0; 174 mDoubleTouchPollIndex = mNumPollFds; 175 mNumPollFds++; 176 } 177 #endif // DOUBLE_TOUCH_ENABLED 178 179 mSensorState[COMMS_SENSOR_ACCEL].sensorType = SENS_TYPE_ACCEL; 180 mSensorState[COMMS_SENSOR_ACCEL].alt[0] = COMMS_SENSOR_ACCEL_UNCALIBRATED; 181 mSensorState[COMMS_SENSOR_ACCEL].alt[1] = COMMS_SENSOR_ACCEL_WRIST_AWARE; 182 mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].sensorType = SENS_TYPE_ACCEL; 183 mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].primary = COMMS_SENSOR_ACCEL; 184 mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].alt[0] = COMMS_SENSOR_ACCEL; 185 mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].alt[1] = COMMS_SENSOR_ACCEL_WRIST_AWARE; 186 mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].sensorType = SENS_TYPE_ACCEL; 187 mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].primary = COMMS_SENSOR_ACCEL; 188 mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].alt[0] = COMMS_SENSOR_ACCEL; 189 mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].alt[1] = COMMS_SENSOR_ACCEL_UNCALIBRATED; 190 mSensorState[COMMS_SENSOR_GYRO].sensorType = SENS_TYPE_GYRO; 191 mSensorState[COMMS_SENSOR_GYRO].alt[0] = COMMS_SENSOR_GYRO_UNCALIBRATED; 192 mSensorState[COMMS_SENSOR_GYRO].alt[1] = COMMS_SENSOR_GYRO_WRIST_AWARE; 193 mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].sensorType = SENS_TYPE_GYRO; 194 mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].primary = COMMS_SENSOR_GYRO; 195 mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].alt[0] = COMMS_SENSOR_GYRO; 196 mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].alt[1] = COMMS_SENSOR_GYRO_WRIST_AWARE; 197 mSensorState[COMMS_SENSOR_GYRO_WRIST_AWARE].sensorType = SENS_TYPE_GYRO; 198 mSensorState[COMMS_SENSOR_GYRO_WRIST_AWARE].primary = COMMS_SENSOR_GYRO; 199 mSensorState[COMMS_SENSOR_GYRO_WRIST_AWARE].alt[0] = COMMS_SENSOR_GYRO; 200 mSensorState[COMMS_SENSOR_GYRO_WRIST_AWARE].alt[1] = COMMS_SENSOR_GYRO_UNCALIBRATED; 201 mSensorState[COMMS_SENSOR_MAG].sensorType = SENS_TYPE_MAG; 202 mSensorState[COMMS_SENSOR_MAG].alt[0] = COMMS_SENSOR_MAG_UNCALIBRATED; 203 mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].sensorType = SENS_TYPE_MAG; 204 mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].primary = COMMS_SENSOR_MAG; 205 mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].alt[0] = COMMS_SENSOR_MAG; 206 mSensorState[COMMS_SENSOR_LIGHT].sensorType = SENS_TYPE_ALS; 207 mSensorState[COMMS_SENSOR_PROXIMITY].sensorType = SENS_TYPE_PROX; 208 mSensorState[COMMS_SENSOR_PRESSURE].sensorType = SENS_TYPE_BARO; 209 mSensorState[COMMS_SENSOR_TEMPERATURE].sensorType = SENS_TYPE_TEMP; 210 mSensorState[COMMS_SENSOR_AMBIENT_TEMPERATURE].sensorType = SENS_TYPE_AMBIENT_TEMP; 211 mSensorState[COMMS_SENSOR_ORIENTATION].sensorType = SENS_TYPE_ORIENTATION; 212 mSensorState[COMMS_SENSOR_WINDOW_ORIENTATION].sensorType = SENS_TYPE_WIN_ORIENTATION; 213 mSensorState[COMMS_SENSOR_WINDOW_ORIENTATION].rate = SENSOR_RATE_ONCHANGE; 214 mSensorState[COMMS_SENSOR_STEP_DETECTOR].sensorType = SENS_TYPE_STEP_DETECT; 215 mSensorState[COMMS_SENSOR_STEP_DETECTOR].rate = SENSOR_RATE_ONCHANGE; 216 mSensorState[COMMS_SENSOR_STEP_COUNTER].sensorType = SENS_TYPE_STEP_COUNT; 217 mSensorState[COMMS_SENSOR_SIGNIFICANT_MOTION].sensorType = SENS_TYPE_SIG_MOTION; 218 mSensorState[COMMS_SENSOR_SIGNIFICANT_MOTION].rate = SENSOR_RATE_ONESHOT; 219 mSensorState[COMMS_SENSOR_GRAVITY].sensorType = SENS_TYPE_GRAVITY; 220 mSensorState[COMMS_SENSOR_LINEAR_ACCEL].sensorType = SENS_TYPE_LINEAR_ACCEL; 221 mSensorState[COMMS_SENSOR_ROTATION_VECTOR].sensorType = SENS_TYPE_ROTATION_VECTOR; 222 mSensorState[COMMS_SENSOR_GEO_MAG].sensorType = SENS_TYPE_GEO_MAG_ROT_VEC; 223 mSensorState[COMMS_SENSOR_GAME_ROTATION_VECTOR].sensorType = SENS_TYPE_GAME_ROT_VECTOR; 224 mSensorState[COMMS_SENSOR_HALL].sensorType = SENS_TYPE_HALL; 225 mSensorState[COMMS_SENSOR_HALL].rate = SENSOR_RATE_ONCHANGE; 226 mSensorState[COMMS_SENSOR_SYNC].sensorType = SENS_TYPE_VSYNC; 227 mSensorState[COMMS_SENSOR_SYNC].rate = SENSOR_RATE_ONCHANGE; 228 mSensorState[COMMS_SENSOR_TILT].sensorType = SENS_TYPE_TILT; 229 mSensorState[COMMS_SENSOR_TILT].rate = SENSOR_RATE_ONCHANGE; 230 mSensorState[COMMS_SENSOR_GESTURE].sensorType = SENS_TYPE_GESTURE; 231 mSensorState[COMMS_SENSOR_GESTURE].rate = SENSOR_RATE_ONESHOT; 232 mSensorState[COMMS_SENSOR_DOUBLE_TWIST].sensorType = SENS_TYPE_DOUBLE_TWIST; 233 mSensorState[COMMS_SENSOR_DOUBLE_TWIST].rate = SENSOR_RATE_ONCHANGE; 234 mSensorState[COMMS_SENSOR_DOUBLE_TAP].sensorType = SENS_TYPE_DOUBLE_TAP; 235 mSensorState[COMMS_SENSOR_DOUBLE_TAP].rate = SENSOR_RATE_ONCHANGE; 236 mSensorState[COMMS_SENSOR_WRIST_TILT].sensorType = SENS_TYPE_WRIST_TILT; 237 mSensorState[COMMS_SENSOR_DOUBLE_TOUCH].sensorType = SENS_TYPE_DOUBLE_TOUCH; 238 mSensorState[COMMS_SENSOR_DOUBLE_TOUCH].rate = SENSOR_RATE_ONESHOT; 239 mSensorState[COMMS_SENSOR_GAZE].sensorType = SENS_TYPE_GAZE; 240 mSensorState[COMMS_SENSOR_GAZE].rate = SENSOR_RATE_ONESHOT; 241 mSensorState[COMMS_SENSOR_UNGAZE].sensorType = SENS_TYPE_UNGAZE; 242 mSensorState[COMMS_SENSOR_UNGAZE].rate = SENSOR_RATE_ONESHOT; 243 mSensorState[COMMS_SENSOR_HUMIDITY].sensorType = SENS_TYPE_HUMIDITY; 244 245 #ifdef LID_STATE_REPORTING_ENABLED 246 initializeUinputNode(); 247 248 // set initial lid state 249 if (property_set(LID_STATE_PROPERTY, LID_STATE_UNKNOWN) < 0) { 250 ALOGW("could not set lid_state property"); 251 } 252 253 // enable hall sensor for folio 254 if (mFd >= 0) { 255 queueActivate(COMMS_SENSOR_HALL, true /* enable */); 256 } 257 #endif // LID_STATE_REPORTING_ENABLED 258 259 #ifdef DIRECT_REPORT_ENABLED 260 mDirectChannelHandle = 1; 261 mSensorToChannel.emplace(COMMS_SENSOR_ACCEL, 262 std::unordered_map<int32_t, DirectChannelTimingInfo>()); 263 mSensorToChannel.emplace(COMMS_SENSOR_GYRO, 264 std::unordered_map<int32_t, DirectChannelTimingInfo>()); 265 mSensorToChannel.emplace(COMMS_SENSOR_MAG, 266 std::unordered_map<int32_t, DirectChannelTimingInfo>()); 267 mSensorToChannel.emplace(COMMS_SENSOR_ACCEL_UNCALIBRATED, 268 std::unordered_map<int32_t, DirectChannelTimingInfo>()); 269 mSensorToChannel.emplace(COMMS_SENSOR_GYRO_UNCALIBRATED, 270 std::unordered_map<int32_t, DirectChannelTimingInfo>()); 271 mSensorToChannel.emplace(COMMS_SENSOR_MAG_UNCALIBRATED, 272 std::unordered_map<int32_t, DirectChannelTimingInfo>()); 273 #endif // DIRECT_REPORT_ENABLED 274 } 275 276 HubConnection::~HubConnection() 277 { 278 close(mFd); 279 } 280 281 void HubConnection::onFirstRef() 282 { 283 run("HubConnection", PRIORITY_URGENT_DISPLAY); 284 enableSchedFifoMode(); 285 } 286 287 // Set main thread to SCHED_FIFO to lower sensor event latency when system is under load 288 void HubConnection::enableSchedFifoMode() { 289 struct sched_param param = {0}; 290 param.sched_priority = HUBCONNECTION_SCHED_FIFO_PRIORITY; 291 if (sched_setscheduler(getTid(), SCHED_FIFO | SCHED_RESET_ON_FORK, ¶m) != 0) { 292 ALOGW("Couldn't set SCHED_FIFO for HubConnection thread"); 293 } 294 } 295 296 status_t HubConnection::initCheck() const 297 { 298 return mFd < 0 ? UNKNOWN_ERROR : OK; 299 } 300 301 status_t HubConnection::getAliveCheck() 302 { 303 return OK; 304 } 305 306 static sp<JSONObject> readSettings(File *file) { 307 off64_t size = file->seekTo(0, SEEK_END); 308 file->seekTo(0, SEEK_SET); 309 310 sp<JSONObject> root; 311 312 if (size > 0) { 313 char *buf = (char *)malloc(size); 314 CHECK_EQ(file->read(buf, size), (ssize_t)size); 315 file->seekTo(0, SEEK_SET); 316 317 sp<JSONCompound> in = JSONCompound::Parse(buf, size); 318 free(buf); 319 buf = NULL; 320 321 if (in != NULL && in->isObject()) { 322 root = (JSONObject *)in.get(); 323 } 324 } 325 326 if (root == NULL) { 327 root = new JSONObject; 328 } 329 330 return root; 331 } 332 333 static bool getCalibrationInt32( 334 const sp<JSONObject> &settings, const char *key, int32_t *out, 335 size_t numArgs) { 336 sp<JSONArray> array; 337 for (size_t i = 0; i < numArgs; i++) { 338 out[i] = 0; 339 } 340 if (!settings->getArray(key, &array)) { 341 return false; 342 } else { 343 for (size_t i = 0; i < numArgs; i++) { 344 if (!array->getInt32(i, &out[i])) { 345 return false; 346 } 347 } 348 } 349 return true; 350 } 351 352 static bool getCalibrationFloat( 353 const sp<JSONObject> &settings, const char *key, float out[3]) { 354 sp<JSONArray> array; 355 for (size_t i = 0; i < 3; i++) { 356 out[i] = 0.0f; 357 } 358 if (!settings->getArray(key, &array)) { 359 return false; 360 } else { 361 for (size_t i = 0; i < 3; i++) { 362 if (!array->getFloat(i, &out[i])) { 363 return false; 364 } 365 } 366 } 367 return true; 368 } 369 370 static std::vector<int32_t> getInt32Setting(const sp<JSONObject> &settings, const char *key) { 371 std::vector<int32_t> ret; 372 373 sp<JSONArray> array; 374 if (settings->getArray(key, &array)) { 375 ret.resize(array->size()); 376 for (size_t i = 0; i < array->size(); ++i) { 377 array->getInt32(i, &ret[i]); 378 } 379 } 380 return ret; 381 } 382 383 static std::vector<float> getFloatSetting(const sp<JSONObject> &settings, const char *key) { 384 std::vector<float> ret; 385 386 sp<JSONArray> array; 387 if (settings->getArray(key, &array)) { 388 ret.resize(array->size()); 389 for (size_t i = 0; i < array->size(); ++i) { 390 array->getFloat(i, &ret[i]); 391 } 392 } 393 return ret; 394 } 395 396 static void loadSensorSettings(sp<JSONObject>* settings, 397 sp<JSONObject>* saved_settings) { 398 File settings_file(CONTEXTHUB_SETTINGS_PATH, "r"); 399 File saved_settings_file(CONTEXTHUB_SAVED_SETTINGS_PATH, "r"); 400 401 status_t err; 402 if ((err = settings_file.initCheck()) != OK) { 403 ALOGW("settings file open failed: %d (%s)", 404 err, 405 strerror(-err)); 406 407 *settings = new JSONObject; 408 } else { 409 *settings = readSettings(&settings_file); 410 } 411 412 if ((err = saved_settings_file.initCheck()) != OK) { 413 ALOGW("saved settings file open failed: %d (%s)", 414 err, 415 strerror(-err)); 416 *saved_settings = new JSONObject; 417 } else { 418 *saved_settings = readSettings(&saved_settings_file); 419 } 420 } 421 422 void HubConnection::saveSensorSettings() const { 423 File saved_settings_file(CONTEXTHUB_SAVED_SETTINGS_PATH, "w"); 424 sp<JSONObject> settingsObject = new JSONObject; 425 426 status_t err; 427 if ((err = saved_settings_file.initCheck()) != OK) { 428 ALOGW("saved settings file open failed %d (%s)", 429 err, 430 strerror(-err)); 431 return; 432 } 433 434 // Build a settings object. 435 sp<JSONArray> magArray = new JSONArray; 436 #ifdef USB_MAG_BIAS_REPORTING_ENABLED 437 magArray->addFloat(mMagBias[0] + mUsbMagBias); 438 #else 439 magArray->addFloat(mMagBias[0]); 440 #endif // USB_MAG_BIAS_REPORTING_ENABLED 441 magArray->addFloat(mMagBias[1]); 442 magArray->addFloat(mMagBias[2]); 443 settingsObject->setArray(MAG_BIAS_TAG, magArray); 444 445 // Add gyro settings 446 sp<JSONArray> gyroArray = new JSONArray; 447 gyroArray->addFloat(mGyroBias[0]); 448 gyroArray->addFloat(mGyroBias[1]); 449 gyroArray->addFloat(mGyroBias[2]); 450 settingsObject->setArray(GYRO_SW_BIAS_TAG, gyroArray); 451 452 // Add accel settings 453 sp<JSONArray> accelArray = new JSONArray; 454 accelArray->addFloat(mAccelBias[0]); 455 accelArray->addFloat(mAccelBias[1]); 456 accelArray->addFloat(mAccelBias[2]); 457 settingsObject->setArray(ACCEL_SW_BIAS_TAG, accelArray); 458 459 // Add overtemp calibration values for gyro 460 sp<JSONArray> gyroOtcDataArray = new JSONArray; 461 const float *f; 462 size_t i; 463 for (f = reinterpret_cast<const float *>(&mGyroOtcData), i = 0; 464 i < sizeof(mGyroOtcData)/sizeof(float); ++i, ++f) { 465 gyroOtcDataArray->addFloat(*f); 466 } 467 settingsObject->setArray(GYRO_OTC_DATA_TAG, gyroOtcDataArray); 468 469 // Write the JSON string to disk. 470 AString serializedSettings = settingsObject->toString(); 471 size_t size = serializedSettings.size(); 472 if ((err = saved_settings_file.write(serializedSettings.c_str(), size)) != (ssize_t)size) { 473 ALOGW("saved settings file write failed %d (%s)", 474 err, 475 strerror(-err)); 476 } 477 } 478 479 ssize_t HubConnection::sendCmd(const void *buf, size_t count) 480 { 481 ssize_t ret; 482 int retryCnt = 0; 483 484 do { 485 ret = TEMP_FAILURE_RETRY(::write(mFd, buf, count)); 486 } while (ret == 0 && retryCnt++ < MAX_RETRY_CNT); 487 488 if (retryCnt > 0) 489 ALOGW("sendCmd: retry: count=%zu, ret=%zd, retryCnt=%d", 490 count, ret, retryCnt); 491 else if (ret < 0 || static_cast<size_t>(ret) != count) 492 ALOGW("sendCmd: failed: count=%zu, ret=%zd, errno=%d", 493 count, ret, errno); 494 495 return ret; 496 } 497 498 void HubConnection::setLeftyMode(bool enable) { 499 struct MsgCmd *cmd; 500 size_t ret; 501 502 Mutex::Autolock autoLock(mLock); 503 504 if (enable == mLefty.hub) return; 505 506 cmd = (struct MsgCmd *)malloc(sizeof(struct MsgCmd) + sizeof(bool)); 507 508 if (cmd) { 509 cmd->evtType = EVT_APP_FROM_HOST; 510 cmd->msg.appId = APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, APP_ID_APP_GAZE_DETECT); 511 cmd->msg.dataLen = sizeof(bool); 512 memcpy((bool *)(cmd+1), &enable, sizeof(bool)); 513 514 ret = sendCmd(cmd, sizeof(*cmd) + sizeof(bool)); 515 if (ret == sizeof(*cmd) + sizeof(bool)) 516 ALOGV("setLeftyMode: lefty (gaze) = %s\n", 517 (enable ? "true" : "false")); 518 else 519 ALOGE("setLeftyMode: failed to send command lefty (gaze) = %s\n", 520 (enable ? "true" : "false")); 521 522 cmd->msg.appId = APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, APP_ID_APP_UNGAZE_DETECT); 523 524 ret = sendCmd(cmd, sizeof(*cmd) + sizeof(bool)); 525 if (ret == sizeof(*cmd) + sizeof(bool)) 526 ALOGV("setLeftyMode: lefty (ungaze) = %s\n", 527 (enable ? "true" : "false")); 528 else 529 ALOGE("setLeftyMode: failed to send command lefty (ungaze) = %s\n", 530 (enable ? "true" : "false")); 531 532 cmd->msg.appId = APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, APP_ID_APP_WRIST_TILT_DETECT); 533 534 ret = sendCmd(cmd, sizeof(*cmd) + sizeof(bool)); 535 if (ret == sizeof(*cmd) + sizeof(bool)) 536 ALOGV("setLeftyMode: lefty (tilt) = %s\n", 537 (enable ? "true" : "false")); 538 else 539 ALOGE("setLeftyMode: failed to send command lefty (tilt) = %s\n", 540 (enable ? "true" : "false")); 541 542 free(cmd); 543 } else { 544 ALOGE("setLeftyMode: failed to allocate command\n"); 545 return; 546 } 547 548 queueFlushInternal(COMMS_SENSOR_ACCEL_WRIST_AWARE, true); 549 queueFlushInternal(COMMS_SENSOR_GYRO_WRIST_AWARE, true); 550 551 mLefty.hub = enable; 552 } 553 554 sensors_event_t *HubConnection::initEv(sensors_event_t *ev, uint64_t timestamp, uint32_t type, uint32_t sensor) 555 { 556 memset(ev, 0x00, sizeof(sensors_event_t)); 557 ev->version = sizeof(sensors_event_t); 558 ev->timestamp = timestamp; 559 ev->type = type; 560 ev->sensor = sensor; 561 562 return ev; 563 } 564 565 ssize_t HubConnection::decrementIfWakeEventLocked(int32_t sensor) 566 { 567 if (isWakeEvent(sensor)) { 568 if (mWakeEventCount > 0) 569 mWakeEventCount--; 570 else 571 ALOGW("%s: sensor=%d, unexpected count=%d, no-op", 572 __FUNCTION__, sensor, mWakeEventCount); 573 } 574 575 return mWakeEventCount; 576 } 577 578 void HubConnection::protectIfWakeEventLocked(int32_t sensor) 579 { 580 if (isWakeEvent(sensor)) { 581 if (mWakelockHeld == false) { 582 acquire_wake_lock(PARTIAL_WAKE_LOCK, WAKELOCK_NAME); 583 mWakelockHeld = true; 584 } 585 mWakeEventCount++; 586 } 587 } 588 589 void HubConnection::releaseWakeLockIfAppropriate() 590 { 591 Mutex::Autolock autoLock(mLock); 592 593 if (mWakelockHeld && (mWakeEventCount == 0)) { 594 mWakelockHeld = false; 595 release_wake_lock(WAKELOCK_NAME); 596 } 597 } 598 599 void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct OneAxisSample *sample, __attribute__((unused)) bool highAccuracy) 600 { 601 sensors_event_t nev[1]; 602 int cnt = 0; 603 604 switch (sensor) { 605 case COMMS_SENSOR_PRESSURE: 606 initEv(&nev[cnt++], timestamp, type, sensor)->pressure = sample->fdata; 607 break; 608 case COMMS_SENSOR_HUMIDITY: 609 initEv(&nev[cnt++], timestamp, type, sensor)->relative_humidity = sample->fdata; 610 break; 611 case COMMS_SENSOR_TEMPERATURE: 612 initEv(&nev[cnt++], timestamp, type, sensor)->temperature = sample->fdata; 613 break; 614 case COMMS_SENSOR_AMBIENT_TEMPERATURE: 615 initEv(&nev[cnt++], timestamp, type, sensor)->temperature = sample->fdata; 616 break; 617 case COMMS_SENSOR_PROXIMITY: 618 initEv(&nev[cnt++], timestamp, type, sensor)->distance = sample->fdata; 619 break; 620 case COMMS_SENSOR_LIGHT: 621 initEv(&nev[cnt++], timestamp, type, sensor)->light = sample->fdata; 622 break; 623 case COMMS_SENSOR_STEP_COUNTER: 624 // We'll stash away the last step count in case we need to reset 625 // the hub. This last step count would then become the new offset. 626 mLastStepCount = mStepCounterOffset + sample->idata; 627 initEv(&nev[cnt++], timestamp, type, sensor)->u64.step_counter = mLastStepCount; 628 break; 629 case COMMS_SENSOR_STEP_DETECTOR: 630 case COMMS_SENSOR_SIGNIFICANT_MOTION: 631 case COMMS_SENSOR_TILT: 632 case COMMS_SENSOR_DOUBLE_TWIST: 633 case COMMS_SENSOR_WRIST_TILT: 634 initEv(&nev[cnt++], timestamp, type, sensor)->data[0] = 1.0f; 635 break; 636 case COMMS_SENSOR_GAZE: 637 case COMMS_SENSOR_UNGAZE: 638 case COMMS_SENSOR_GESTURE: 639 case COMMS_SENSOR_SYNC: 640 case COMMS_SENSOR_DOUBLE_TOUCH: 641 initEv(&nev[cnt++], timestamp, type, sensor)->data[0] = sample->idata; 642 break; 643 case COMMS_SENSOR_HALL: 644 #ifdef LID_STATE_REPORTING_ENABLED 645 sendFolioEvent(sample->idata); 646 #endif // LID_STATE_REPORTING_ENABLED 647 break; 648 case COMMS_SENSOR_WINDOW_ORIENTATION: 649 initEv(&nev[cnt++], timestamp, type, sensor)->data[0] = sample->idata; 650 break; 651 default: 652 break; 653 } 654 655 if (cnt > 0) 656 write(nev, cnt); 657 } 658 659 uint8_t HubConnection::magAccuracyUpdate(sensors_vec_t *sv) 660 { 661 float magSq = sv->x * sv->x + sv->y * sv->y + sv->z * sv->z; 662 663 if (magSq < MIN_MAG_SQ || magSq > MAX_MAG_SQ) { 664 // save last good accuracy (either MEDIUM or HIGH) 665 if (mMagAccuracy != SENSOR_STATUS_UNRELIABLE) 666 mMagAccuracyRestore = mMagAccuracy; 667 mMagAccuracy = SENSOR_STATUS_UNRELIABLE; 668 } else if (mMagAccuracy == SENSOR_STATUS_UNRELIABLE) { 669 // restore 670 mMagAccuracy = mMagAccuracyRestore; 671 } 672 673 return mMagAccuracy; 674 } 675 676 void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct RawThreeAxisSample *sample, __attribute__((unused)) bool highAccuracy) 677 { 678 sensors_vec_t *sv; 679 uncalibrated_event_t *ue; 680 sensors_event_t nev[3]; 681 int cnt = 0; 682 683 switch (sensor) { 684 case COMMS_SENSOR_ACCEL: 685 sv = &initEv(&nev[cnt], timestamp, type, sensor)->acceleration; 686 sv->x = sample->ix * mScaleAccel; 687 sv->y = sample->iy * mScaleAccel; 688 sv->z = sample->iz * mScaleAccel; 689 sv->status = SENSOR_STATUS_ACCURACY_HIGH; 690 691 sendDirectReportEvent(&nev[cnt], 1); 692 if (mSensorState[sensor].enable && isSampleIntervalSatisfied(sensor, timestamp)) { 693 if (!mAccelEnabledBiasStored) { 694 // accel is enabled, but no enabled bias. Store latest bias and use 695 // for accel and uncalibrated accel due to: 696 // https://source.android.com/devices/sensors/sensor-types.html 697 // "The bias and scale calibration must only be updated while the sensor is deactivated, 698 // so as to avoid causing jumps in values during streaming." 699 mAccelEnabledBiasStored = true; 700 mAccelEnabledBias[0] = mAccelBias[0]; 701 mAccelEnabledBias[1] = mAccelBias[1]; 702 mAccelEnabledBias[2] = mAccelBias[2]; 703 } 704 // samples arrive using latest bias 705 // adjust for enabled bias being different from lastest bias 706 sv->x += mAccelBias[0] - mAccelEnabledBias[0]; 707 sv->y += mAccelBias[1] - mAccelEnabledBias[1]; 708 sv->z += mAccelBias[2] - mAccelEnabledBias[2]; 709 ++cnt; 710 } 711 712 ue = &initEv(&nev[cnt], timestamp, 713 SENSOR_TYPE_ACCELEROMETER_UNCALIBRATED, 714 COMMS_SENSOR_ACCEL_UNCALIBRATED)->uncalibrated_accelerometer; 715 ue->x_uncalib = sample->ix * mScaleAccel + mAccelBias[0]; 716 ue->y_uncalib = sample->iy * mScaleAccel + mAccelBias[1]; 717 ue->z_uncalib = sample->iz * mScaleAccel + mAccelBias[2]; 718 if (!mAccelEnabledBiasStored) { 719 // No enabled bias (which means accel is disabled). Use latest bias. 720 ue->x_bias = mAccelBias[0]; 721 ue->y_bias = mAccelBias[1]; 722 ue->z_bias = mAccelBias[2]; 723 } else { 724 // enabled bias is valid, so use it 725 ue->x_bias = mAccelEnabledBias[0]; 726 ue->y_bias = mAccelEnabledBias[1]; 727 ue->z_bias = mAccelEnabledBias[2]; 728 } 729 730 sendDirectReportEvent(&nev[cnt], 1); 731 if (mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].enable 732 && isSampleIntervalSatisfied(COMMS_SENSOR_ACCEL_UNCALIBRATED, timestamp)) { 733 ++cnt; 734 } 735 736 if (mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].enable 737 && isSampleIntervalSatisfied(COMMS_SENSOR_ACCEL_WRIST_AWARE, timestamp)) { 738 sv = &initEv(&nev[cnt++], timestamp, 739 SENSOR_TYPE_ACCELEROMETER_WRIST_AWARE, 740 COMMS_SENSOR_ACCEL_WRIST_AWARE)->acceleration; 741 sv->x = sample->ix * mScaleAccel; 742 sv->y = (mLefty.accel ? -sample->iy : sample->iy) * mScaleAccel; 743 sv->z = sample->iz * mScaleAccel; 744 sv->status = SENSOR_STATUS_ACCURACY_HIGH; 745 } 746 break; 747 case COMMS_SENSOR_MAG: 748 sv = &initEv(&nev[cnt], timestamp, type, sensor)->magnetic; 749 sv->x = sample->ix * mScaleMag; 750 sv->y = sample->iy * mScaleMag; 751 sv->z = sample->iz * mScaleMag; 752 sv->status = magAccuracyUpdate(sv); 753 754 sendDirectReportEvent(&nev[cnt], 1); 755 if (mSensorState[sensor].enable && isSampleIntervalSatisfied(sensor, timestamp)) { 756 ++cnt; 757 } 758 759 ue = &initEv(&nev[cnt], timestamp, 760 SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED, 761 COMMS_SENSOR_MAG_UNCALIBRATED)->uncalibrated_magnetic; 762 ue->x_uncalib = sample->ix * mScaleMag + mMagBias[0]; 763 ue->y_uncalib = sample->iy * mScaleMag + mMagBias[1]; 764 ue->z_uncalib = sample->iz * mScaleMag + mMagBias[2]; 765 ue->x_bias = mMagBias[0]; 766 ue->y_bias = mMagBias[1]; 767 ue->z_bias = mMagBias[2]; 768 769 sendDirectReportEvent(&nev[cnt], 1); 770 if (mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].enable 771 && isSampleIntervalSatisfied(COMMS_SENSOR_MAG_UNCALIBRATED, timestamp)) { 772 ++cnt; 773 } 774 break; 775 default: 776 break; 777 } 778 779 if (cnt > 0) 780 write(nev, cnt); 781 } 782 783 void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct ThreeAxisSample *sample, bool highAccuracy) 784 { 785 sensors_vec_t *sv; 786 uncalibrated_event_t *ue; 787 sensors_event_t *ev; 788 sensors_event_t nev[3]; 789 static const float heading_accuracy = M_PI / 6.0f; 790 float w; 791 int cnt = 0; 792 793 switch (sensor) { 794 case COMMS_SENSOR_ACCEL: 795 sv = &initEv(&nev[cnt], timestamp, type, sensor)->acceleration; 796 sv->x = sample->x; 797 sv->y = sample->y; 798 sv->z = sample->z; 799 sv->status = SENSOR_STATUS_ACCURACY_HIGH; 800 801 sendDirectReportEvent(&nev[cnt], 1); 802 if (mSensorState[sensor].enable && isSampleIntervalSatisfied(sensor, timestamp)) { 803 ++cnt; 804 } 805 806 ue = &initEv(&nev[cnt], timestamp, 807 SENSOR_TYPE_ACCELEROMETER_UNCALIBRATED, 808 COMMS_SENSOR_ACCEL_UNCALIBRATED)->uncalibrated_accelerometer; 809 ue->x_uncalib = sample->x + mAccelBias[0]; 810 ue->y_uncalib = sample->y + mAccelBias[1]; 811 ue->z_uncalib = sample->z + mAccelBias[2]; 812 ue->x_bias = mAccelBias[0]; 813 ue->y_bias = mAccelBias[1]; 814 ue->z_bias = mAccelBias[2]; 815 816 sendDirectReportEvent(&nev[cnt], 1); 817 if (mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].enable 818 && isSampleIntervalSatisfied(COMMS_SENSOR_ACCEL_UNCALIBRATED, timestamp)) { 819 ++cnt; 820 } 821 822 if (mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].enable 823 && isSampleIntervalSatisfied(COMMS_SENSOR_ACCEL_WRIST_AWARE, timestamp)) { 824 sv = &initEv(&nev[cnt], timestamp, 825 SENSOR_TYPE_ACCELEROMETER_WRIST_AWARE, 826 COMMS_SENSOR_ACCEL_WRIST_AWARE)->acceleration; 827 sv->x = sample->x; 828 sv->y = (mLefty.accel ? -sample->y : sample->y); 829 sv->z = sample->z; 830 sv->status = SENSOR_STATUS_ACCURACY_HIGH; 831 ++cnt; 832 } 833 break; 834 case COMMS_SENSOR_GYRO: 835 sv = &initEv(&nev[cnt], timestamp, type, sensor)->gyro; 836 sv->x = sample->x; 837 sv->y = sample->y; 838 sv->z = sample->z; 839 sv->status = SENSOR_STATUS_ACCURACY_HIGH; 840 841 sendDirectReportEvent(&nev[cnt], 1); 842 if (mSensorState[sensor].enable && isSampleIntervalSatisfied(sensor, timestamp)) { 843 ++cnt; 844 } 845 846 ue = &initEv(&nev[cnt], timestamp, 847 SENSOR_TYPE_GYROSCOPE_UNCALIBRATED, 848 COMMS_SENSOR_GYRO_UNCALIBRATED)->uncalibrated_gyro; 849 ue->x_uncalib = sample->x + mGyroBias[0]; 850 ue->y_uncalib = sample->y + mGyroBias[1]; 851 ue->z_uncalib = sample->z + mGyroBias[2]; 852 ue->x_bias = mGyroBias[0]; 853 ue->y_bias = mGyroBias[1]; 854 ue->z_bias = mGyroBias[2]; 855 sendDirectReportEvent(&nev[cnt], 1); 856 857 if (mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].enable 858 && isSampleIntervalSatisfied(COMMS_SENSOR_GYRO_UNCALIBRATED, timestamp)) { 859 ++cnt; 860 } 861 862 if (mSensorState[COMMS_SENSOR_GYRO_WRIST_AWARE].enable 863 && isSampleIntervalSatisfied(COMMS_SENSOR_GYRO_WRIST_AWARE, timestamp)) { 864 sv = &initEv(&nev[cnt], timestamp, 865 SENSOR_TYPE_GYROSCOPE_WRIST_AWARE, 866 COMMS_SENSOR_GYRO_WRIST_AWARE)->gyro; 867 sv->x = (mLefty.gyro ? -sample->x : sample->x); 868 sv->y = sample->y; 869 sv->z = (mLefty.gyro ? -sample->z : sample->z); 870 sv->status = SENSOR_STATUS_ACCURACY_HIGH; 871 ++cnt; 872 } 873 break; 874 case COMMS_SENSOR_ACCEL_BIAS: 875 mAccelBias[0] = sample->x; 876 mAccelBias[1] = sample->y; 877 mAccelBias[2] = sample->z; 878 saveSensorSettings(); 879 break; 880 case COMMS_SENSOR_GYRO_BIAS: 881 mGyroBias[0] = sample->x; 882 mGyroBias[1] = sample->y; 883 mGyroBias[2] = sample->z; 884 saveSensorSettings(); 885 break; 886 case COMMS_SENSOR_MAG: 887 sv = &initEv(&nev[cnt], timestamp, type, sensor)->magnetic; 888 sv->x = sample->x; 889 sv->y = sample->y; 890 sv->z = sample->z; 891 sv->status = magAccuracyUpdate(sv); 892 sendDirectReportEvent(&nev[cnt], 1); 893 894 if (mSensorState[sensor].enable && isSampleIntervalSatisfied(sensor, timestamp)) { 895 ++cnt; 896 } 897 898 ue = &initEv(&nev[cnt], timestamp, 899 SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED, 900 COMMS_SENSOR_MAG_UNCALIBRATED)->uncalibrated_magnetic; 901 ue->x_uncalib = sample->x + mMagBias[0]; 902 ue->y_uncalib = sample->y + mMagBias[1]; 903 ue->z_uncalib = sample->z + mMagBias[2]; 904 ue->x_bias = mMagBias[0]; 905 ue->y_bias = mMagBias[1]; 906 ue->z_bias = mMagBias[2]; 907 sendDirectReportEvent(&nev[cnt], 1); 908 909 if (mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].enable 910 && isSampleIntervalSatisfied(COMMS_SENSOR_MAG_UNCALIBRATED, timestamp)) { 911 ++cnt; 912 } 913 break; 914 case COMMS_SENSOR_MAG_BIAS: 915 mMagAccuracy = highAccuracy ? SENSOR_STATUS_ACCURACY_HIGH : SENSOR_STATUS_ACCURACY_MEDIUM; 916 mMagBias[0] = sample->x; 917 mMagBias[1] = sample->y; 918 mMagBias[2] = sample->z; 919 920 saveSensorSettings(); 921 break; 922 case COMMS_SENSOR_ORIENTATION: 923 case COMMS_SENSOR_LINEAR_ACCEL: 924 case COMMS_SENSOR_GRAVITY: 925 sv = &initEv(&nev[cnt++], timestamp, type, sensor)->orientation; 926 sv->x = sample->x; 927 sv->y = sample->y; 928 sv->z = sample->z; 929 sv->status = mMagAccuracy; 930 break; 931 case COMMS_SENSOR_DOUBLE_TAP: 932 ev = initEv(&nev[cnt++], timestamp, type, sensor); 933 ev->data[0] = sample->x; 934 ev->data[1] = sample->y; 935 ev->data[2] = sample->z; 936 break; 937 case COMMS_SENSOR_ROTATION_VECTOR: 938 ev = initEv(&nev[cnt++], timestamp, type, sensor); 939 w = sample->x * sample->x + sample->y * sample->y + sample->z * sample->z; 940 if (w < 1.0f) 941 w = sqrt(1.0f - w); 942 else 943 w = 0.0f; 944 ev->data[0] = sample->x; 945 ev->data[1] = sample->y; 946 ev->data[2] = sample->z; 947 ev->data[3] = w; 948 ev->data[4] = (4 - mMagAccuracy) * heading_accuracy; 949 break; 950 case COMMS_SENSOR_GEO_MAG: 951 case COMMS_SENSOR_GAME_ROTATION_VECTOR: 952 ev = initEv(&nev[cnt++], timestamp, type, sensor); 953 w = sample->x * sample->x + sample->y * sample->y + sample->z * sample->z; 954 if (w < 1.0f) 955 w = sqrt(1.0f - w); 956 else 957 w = 0.0f; 958 ev->data[0] = sample->x; 959 ev->data[1] = sample->y; 960 ev->data[2] = sample->z; 961 ev->data[3] = w; 962 break; 963 default: 964 break; 965 } 966 967 if (cnt > 0) 968 write(nev, cnt); 969 } 970 971 void HubConnection::discardInotifyEvent() { 972 // Read & discard an inotify event. We only use the presence of an event as 973 // a trigger to perform the file existence check (for simplicity) 974 if (mInotifyPollIndex >= 0) { 975 char buf[sizeof(struct inotify_event) + NAME_MAX + 1]; 976 int ret = ::read(mPollFds[mInotifyPollIndex].fd, buf, sizeof(buf)); 977 ALOGV("Discarded %d bytes of inotify data", ret); 978 } 979 } 980 981 void HubConnection::waitOnNanohubLock() { 982 if (mInotifyPollIndex < 0) { 983 return; 984 } 985 struct pollfd *pfd = &mPollFds[mInotifyPollIndex]; 986 987 // While the lock file exists, poll on the inotify fd (with timeout) 988 while (access(NANOHUB_LOCK_FILE, F_OK) == 0) { 989 ALOGW("Nanohub is locked; blocking read thread"); 990 int ret = poll(pfd, 1, 5000); 991 if ((ret > 0) && (pfd->revents & POLLIN)) { 992 discardInotifyEvent(); 993 } 994 } 995 } 996 997 void HubConnection::restoreSensorState() 998 { 999 Mutex::Autolock autoLock(mLock); 1000 1001 sendCalibrationOffsets(); 1002 1003 for (int i = 0; i < NUM_COMMS_SENSORS_PLUS_1; i++) { 1004 if (mSensorState[i].sensorType && mSensorState[i].enable) { 1005 struct ConfigCmd cmd; 1006 1007 initConfigCmd(&cmd, i); 1008 1009 ALOGV("restoring: sensor=%d, handle=%d, enable=%d, period=%" PRId64 ", latency=%" PRId64, 1010 cmd.sensorType, i, mSensorState[i].enable, frequency_q10_to_period_ns(mSensorState[i].rate), 1011 mSensorState[i].latency); 1012 1013 int ret = sendCmd(&cmd, sizeof(cmd)); 1014 if (ret != sizeof(cmd)) { 1015 ALOGW("failed to send config command to restore sensor %d\n", cmd.sensorType); 1016 } 1017 1018 cmd.cmd = CONFIG_CMD_FLUSH; 1019 1020 for (auto iter = mFlushesPending[i].cbegin(); iter != mFlushesPending[i].cend(); ++iter) { 1021 for (int j = 0; j < iter->count; j++) { 1022 int ret = sendCmd(&cmd, sizeof(cmd)); 1023 if (ret != sizeof(cmd)) { 1024 ALOGW("failed to send flush command to sensor %d\n", cmd.sensorType); 1025 } 1026 } 1027 } 1028 } 1029 } 1030 1031 mStepCounterOffset = mLastStepCount; 1032 } 1033 1034 void HubConnection::postOsLog(uint8_t *buf, ssize_t len) 1035 { 1036 // if len is less than 6, it's either an invalid or an empty log message. 1037 if (len < 6) 1038 return; 1039 1040 buf[len] = 0x00; 1041 switch (buf[4]) { 1042 case 'E': 1043 ALOGE("osLog: %s", &buf[5]); 1044 break; 1045 case 'W': 1046 ALOGW("osLog: %s", &buf[5]); 1047 break; 1048 case 'I': 1049 ALOGI("osLog: %s", &buf[5]); 1050 break; 1051 case 'D': 1052 ALOGD("osLog: %s", &buf[5]); 1053 break; 1054 case 'V': 1055 ALOGV("osLog: %s", &buf[5]); 1056 break; 1057 default: 1058 break; 1059 } 1060 } 1061 1062 void HubConnection::processAppData(uint8_t *buf, ssize_t len) { 1063 if (len < static_cast<ssize_t>(sizeof(AppToSensorHalDataBuffer))) 1064 return; 1065 1066 AppToSensorHalDataPayload *data = 1067 &(reinterpret_cast<AppToSensorHalDataBuffer *>(buf)->payload); 1068 if (data->size + sizeof(AppToSensorHalDataBuffer) != len) { 1069 ALOGW("Received corrupted data update packet, len %zd, size %u", len, data->size); 1070 return; 1071 } 1072 1073 switch (data->type & APP_TO_SENSOR_HAL_TYPE_MASK) { 1074 case HALINTF_TYPE_GYRO_OTC_DATA: 1075 if (data->size != sizeof(GyroOtcData)) { 1076 ALOGW("Corrupted HALINTF_TYPE_GYRO_OTC_DATA with size %u", data->size); 1077 return; 1078 } 1079 mGyroOtcData = data->gyroOtcData[0]; 1080 saveSensorSettings(); 1081 break; 1082 default: 1083 ALOGW("Unknown app to hal data type 0x%04x", data->type); 1084 break; 1085 } 1086 } 1087 1088 ssize_t HubConnection::processBuf(uint8_t *buf, size_t len) 1089 { 1090 struct nAxisEvent *data = (struct nAxisEvent *)buf; 1091 uint32_t type, sensor, bias, currSensor; 1092 int i, numSamples; 1093 bool one, rawThree, three; 1094 sensors_event_t ev; 1095 uint64_t timestamp; 1096 ssize_t ret = 0; 1097 uint32_t primary; 1098 1099 if (len >= sizeof(data->evtType)) { 1100 ret = sizeof(data->evtType); 1101 one = three = rawThree = false; 1102 bias = 0; 1103 switch (data->evtType) { 1104 case OS_LOG_EVENT: 1105 postOsLog(buf, len); 1106 return 0; 1107 case EVT_APP_TO_SENSOR_HAL_DATA: 1108 processAppData(buf, len); 1109 return 0; 1110 case SENS_TYPE_TO_EVENT(SENS_TYPE_ACCEL): 1111 type = SENSOR_TYPE_ACCELEROMETER; 1112 sensor = COMMS_SENSOR_ACCEL; 1113 bias = COMMS_SENSOR_ACCEL_BIAS; 1114 three = true; 1115 break; 1116 case SENS_TYPE_TO_EVENT(SENS_TYPE_ACCEL_RAW): 1117 type = SENSOR_TYPE_ACCELEROMETER; 1118 sensor = COMMS_SENSOR_ACCEL; 1119 rawThree = true; 1120 break; 1121 case SENS_TYPE_TO_EVENT(SENS_TYPE_GYRO): 1122 type = SENSOR_TYPE_GYROSCOPE; 1123 sensor = COMMS_SENSOR_GYRO; 1124 bias = COMMS_SENSOR_GYRO_BIAS; 1125 three = true; 1126 break; 1127 case SENS_TYPE_TO_EVENT(SENS_TYPE_MAG): 1128 type = SENSOR_TYPE_MAGNETIC_FIELD; 1129 sensor = COMMS_SENSOR_MAG; 1130 bias = COMMS_SENSOR_MAG_BIAS; 1131 three = true; 1132 break; 1133 case SENS_TYPE_TO_EVENT(SENS_TYPE_MAG_RAW): 1134 type = SENSOR_TYPE_MAGNETIC_FIELD; 1135 sensor = COMMS_SENSOR_MAG; 1136 rawThree = true; 1137 break; 1138 case SENS_TYPE_TO_EVENT(SENS_TYPE_ALS): 1139 type = SENSOR_TYPE_LIGHT; 1140 sensor = COMMS_SENSOR_LIGHT; 1141 one = true; 1142 break; 1143 case SENS_TYPE_TO_EVENT(SENS_TYPE_PROX): 1144 type = SENSOR_TYPE_PROXIMITY; 1145 sensor = COMMS_SENSOR_PROXIMITY; 1146 one = true; 1147 break; 1148 case SENS_TYPE_TO_EVENT(SENS_TYPE_BARO): 1149 type = SENSOR_TYPE_PRESSURE; 1150 sensor = COMMS_SENSOR_PRESSURE; 1151 one = true; 1152 break; 1153 case SENS_TYPE_TO_EVENT(SENS_TYPE_HUMIDITY): 1154 type = SENSOR_TYPE_RELATIVE_HUMIDITY; 1155 sensor = COMMS_SENSOR_HUMIDITY; 1156 one = true; 1157 break; 1158 case SENS_TYPE_TO_EVENT(SENS_TYPE_TEMP): 1159 // nanohub only has one temperature sensor type, which is mapped to 1160 // internal temp because we currently don't have ambient temp 1161 type = SENSOR_TYPE_INTERNAL_TEMPERATURE; 1162 sensor = COMMS_SENSOR_TEMPERATURE; 1163 one = true; 1164 break; 1165 case SENS_TYPE_TO_EVENT(SENS_TYPE_AMBIENT_TEMP): 1166 type = SENSOR_TYPE_AMBIENT_TEMPERATURE; 1167 sensor = COMMS_SENSOR_AMBIENT_TEMPERATURE; 1168 one = true; 1169 break; 1170 case SENS_TYPE_TO_EVENT(SENS_TYPE_ORIENTATION): 1171 type = SENSOR_TYPE_ORIENTATION; 1172 sensor = COMMS_SENSOR_ORIENTATION; 1173 three = true; 1174 break; 1175 case SENS_TYPE_TO_EVENT(SENS_TYPE_WIN_ORIENTATION): 1176 type = SENSOR_TYPE_DEVICE_ORIENTATION; 1177 sensor = COMMS_SENSOR_WINDOW_ORIENTATION; 1178 one = true; 1179 break; 1180 case SENS_TYPE_TO_EVENT(SENS_TYPE_STEP_DETECT): 1181 type = SENSOR_TYPE_STEP_DETECTOR; 1182 sensor = COMMS_SENSOR_STEP_DETECTOR; 1183 one = true; 1184 break; 1185 case SENS_TYPE_TO_EVENT(SENS_TYPE_STEP_COUNT): 1186 type = SENSOR_TYPE_STEP_COUNTER; 1187 sensor = COMMS_SENSOR_STEP_COUNTER; 1188 one = true; 1189 break; 1190 case SENS_TYPE_TO_EVENT(SENS_TYPE_SIG_MOTION): 1191 type = SENSOR_TYPE_SIGNIFICANT_MOTION; 1192 sensor = COMMS_SENSOR_SIGNIFICANT_MOTION; 1193 one = true; 1194 break; 1195 case SENS_TYPE_TO_EVENT(SENS_TYPE_GRAVITY): 1196 type = SENSOR_TYPE_GRAVITY; 1197 sensor = COMMS_SENSOR_GRAVITY; 1198 three = true; 1199 break; 1200 case SENS_TYPE_TO_EVENT(SENS_TYPE_LINEAR_ACCEL): 1201 type = SENSOR_TYPE_LINEAR_ACCELERATION; 1202 sensor = COMMS_SENSOR_LINEAR_ACCEL; 1203 three = true; 1204 break; 1205 case SENS_TYPE_TO_EVENT(SENS_TYPE_ROTATION_VECTOR): 1206 type = SENSOR_TYPE_ROTATION_VECTOR; 1207 sensor = COMMS_SENSOR_ROTATION_VECTOR; 1208 three = true; 1209 break; 1210 case SENS_TYPE_TO_EVENT(SENS_TYPE_GEO_MAG_ROT_VEC): 1211 type = SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR; 1212 sensor = COMMS_SENSOR_GEO_MAG; 1213 three = true; 1214 break; 1215 case SENS_TYPE_TO_EVENT(SENS_TYPE_GAME_ROT_VECTOR): 1216 type = SENSOR_TYPE_GAME_ROTATION_VECTOR; 1217 sensor = COMMS_SENSOR_GAME_ROTATION_VECTOR; 1218 three = true; 1219 break; 1220 case SENS_TYPE_TO_EVENT(SENS_TYPE_HALL): 1221 type = 0; 1222 sensor = COMMS_SENSOR_HALL; 1223 one = true; 1224 break; 1225 case SENS_TYPE_TO_EVENT(SENS_TYPE_VSYNC): 1226 type = SENSOR_TYPE_SYNC; 1227 sensor = COMMS_SENSOR_SYNC; 1228 one = true; 1229 break; 1230 case SENS_TYPE_TO_EVENT(SENS_TYPE_TILT): 1231 type = SENSOR_TYPE_TILT_DETECTOR; 1232 sensor = COMMS_SENSOR_TILT; 1233 one = true; 1234 break; 1235 case SENS_TYPE_TO_EVENT(SENS_TYPE_GESTURE): 1236 type = SENSOR_TYPE_PICK_UP_GESTURE; 1237 sensor = COMMS_SENSOR_GESTURE; 1238 one = true; 1239 break; 1240 case SENS_TYPE_TO_EVENT(SENS_TYPE_DOUBLE_TWIST): 1241 type = SENSOR_TYPE_DOUBLE_TWIST; 1242 sensor = COMMS_SENSOR_DOUBLE_TWIST; 1243 one = true; 1244 break; 1245 case SENS_TYPE_TO_EVENT(SENS_TYPE_DOUBLE_TAP): 1246 type = SENSOR_TYPE_DOUBLE_TAP; 1247 sensor = COMMS_SENSOR_DOUBLE_TAP; 1248 three = true; 1249 break; 1250 case SENS_TYPE_TO_EVENT(SENS_TYPE_WRIST_TILT): 1251 type = SENSOR_TYPE_WRIST_TILT_GESTURE; 1252 sensor = COMMS_SENSOR_WRIST_TILT; 1253 one = true; 1254 break; 1255 case SENS_TYPE_TO_EVENT(SENS_TYPE_DOUBLE_TOUCH): 1256 type = SENSOR_TYPE_DOUBLE_TOUCH; 1257 sensor = COMMS_SENSOR_DOUBLE_TOUCH; 1258 one = true; 1259 break; 1260 case SENS_TYPE_TO_EVENT(SENS_TYPE_GAZE): 1261 type = SENSOR_TYPE_GAZE; 1262 sensor = COMMS_SENSOR_GAZE; 1263 one = true; 1264 break; 1265 case SENS_TYPE_TO_EVENT(SENS_TYPE_UNGAZE): 1266 type = SENSOR_TYPE_UNGAZE; 1267 sensor = COMMS_SENSOR_UNGAZE; 1268 one = true; 1269 break; 1270 case EVT_RESET_REASON: 1271 uint32_t resetReason; 1272 memcpy(&resetReason, data->buffer, sizeof(resetReason)); 1273 ALOGI("Observed hub reset: 0x%08" PRIx32, resetReason); 1274 restoreSensorState(); 1275 return 0; 1276 default: 1277 ALOGW("unknown evtType: 0x%08x len: %zu\n", data->evtType, len); 1278 return -1; 1279 } 1280 } else { 1281 ALOGW("too little data: len=%zu\n", len); 1282 return -1; 1283 } 1284 1285 if (len >= sizeof(data->evtType) + sizeof(data->referenceTime) + sizeof(data->firstSample)) { 1286 ret += sizeof(data->referenceTime); 1287 timestamp = data->referenceTime; 1288 numSamples = data->firstSample.numSamples; 1289 for (i=0; i<numSamples; i++) { 1290 if (data->firstSample.biasPresent && data->firstSample.biasSample == i) 1291 currSensor = bias; 1292 else 1293 currSensor = sensor; 1294 1295 if (one) { 1296 if (ret + sizeof(data->oneSamples[i]) > len) { 1297 ALOGW("sensor %d (one): ret=%zd, numSamples=%d, i=%d\n", currSensor, ret, numSamples, i); 1298 return -1; 1299 } 1300 if (i > 0) 1301 timestamp += ((uint64_t)data->oneSamples[i].deltaTime) << delta_time_shift_table[data->oneSamples[i].deltaTime & delta_time_encoded]; 1302 processSample(timestamp, type, currSensor, &data->oneSamples[i], data->firstSample.highAccuracy); 1303 ret += sizeof(data->oneSamples[i]); 1304 } else if (rawThree) { 1305 if (ret + sizeof(data->rawThreeSamples[i]) > len) { 1306 ALOGW("sensor %d (rawThree): ret=%zd, numSamples=%d, i=%d\n", currSensor, ret, numSamples, i); 1307 return -1; 1308 } 1309 if (i > 0) 1310 timestamp += ((uint64_t)data->rawThreeSamples[i].deltaTime) << delta_time_shift_table[data->rawThreeSamples[i].deltaTime & delta_time_encoded]; 1311 processSample(timestamp, type, currSensor, &data->rawThreeSamples[i], data->firstSample.highAccuracy); 1312 ret += sizeof(data->rawThreeSamples[i]); 1313 } else if (three) { 1314 if (ret + sizeof(data->threeSamples[i]) > len) { 1315 ALOGW("sensor %d (three): ret=%zd, numSamples=%d, i=%d\n", currSensor, ret, numSamples, i); 1316 return -1; 1317 } 1318 if (i > 0) 1319 timestamp += ((uint64_t)data->threeSamples[i].deltaTime) << delta_time_shift_table[data->threeSamples[i].deltaTime & delta_time_encoded]; 1320 processSample(timestamp, type, currSensor, &data->threeSamples[i], data->firstSample.highAccuracy); 1321 ret += sizeof(data->threeSamples[i]); 1322 } else { 1323 ALOGW("sensor %d (unknown): cannot processSample\n", currSensor); 1324 return -1; 1325 } 1326 } 1327 1328 if (!numSamples) 1329 ret += sizeof(data->firstSample); 1330 1331 // If no primary sensor type is specified, 1332 // then 'sensor' is the primary sensor type. 1333 primary = mSensorState[sensor].primary; 1334 primary = (primary ? primary : sensor); 1335 1336 for (i=0; i<data->firstSample.numFlushes; i++) { 1337 bool internal = false; 1338 1339 { 1340 Mutex::Autolock autoLock(mLock); 1341 struct Flush& flush = mFlushesPending[primary].front(); 1342 memset(&ev, 0x00, sizeof(sensors_event_t)); 1343 ev.version = META_DATA_VERSION; 1344 ev.timestamp = 0; 1345 ev.type = SENSOR_TYPE_META_DATA; 1346 ev.sensor = 0; 1347 ev.meta_data.what = META_DATA_FLUSH_COMPLETE; 1348 ev.meta_data.sensor = flush.handle; 1349 1350 if (flush.internal) { 1351 internal = true; 1352 if (flush.handle == COMMS_SENSOR_ACCEL_WRIST_AWARE) 1353 mLefty.accel = !mLefty.accel; 1354 else if (flush.handle == COMMS_SENSOR_GYRO_WRIST_AWARE) 1355 mLefty.gyro = !mLefty.gyro; 1356 } 1357 1358 if (--flush.count == 0) 1359 mFlushesPending[primary].pop_front(); 1360 } 1361 1362 if (!internal) 1363 write(&ev, 1); 1364 1365 ALOGV("flushing %d", ev.meta_data.sensor); 1366 } 1367 } else { 1368 ALOGW("too little data for sensor %d: len=%zu\n", sensor, len); 1369 return -1; 1370 } 1371 1372 return ret; 1373 } 1374 1375 void HubConnection::sendCalibrationOffsets() 1376 { 1377 sp<JSONObject> settings; 1378 sp<JSONObject> saved_settings; 1379 struct { 1380 int32_t hw[3]; 1381 float sw[3]; 1382 } accel; 1383 1384 int32_t proximity, proximity_array[4]; 1385 float barometer, humidity, light; 1386 bool accel_hw_cal_exists, accel_sw_cal_exists; 1387 1388 loadSensorSettings(&settings, &saved_settings); 1389 1390 accel_hw_cal_exists = getCalibrationInt32(settings, ACCEL_BIAS_TAG, accel.hw, 3); 1391 accel_sw_cal_exists = getCalibrationFloat(saved_settings, ACCEL_SW_BIAS_TAG, accel.sw); 1392 if (accel_hw_cal_exists || accel_sw_cal_exists) { 1393 // Store SW bias so we can remove bias for uncal data 1394 mAccelBias[0] = accel.sw[0]; 1395 mAccelBias[1] = accel.sw[1]; 1396 mAccelBias[2] = accel.sw[2]; 1397 1398 queueDataInternal(COMMS_SENSOR_ACCEL, &accel, sizeof(accel)); 1399 } 1400 1401 ALOGV("Use new configuration format"); 1402 std::vector<int32_t> hardwareGyroBias = getInt32Setting(settings, GYRO_BIAS_TAG); 1403 std::vector<float> softwareGyroBias = getFloatSetting(saved_settings, GYRO_SW_BIAS_TAG); 1404 if (hardwareGyroBias.size() == 3 || softwareGyroBias.size() == 3) { 1405 struct { 1406 AppToSensorHalDataPayload header; 1407 GyroCalBias data; 1408 } packet = { 1409 .header = { 1410 .size = sizeof(GyroCalBias), 1411 .type = HALINTF_TYPE_GYRO_CAL_BIAS } 1412 }; 1413 if (hardwareGyroBias.size() == 3) { 1414 std::copy(hardwareGyroBias.begin(), hardwareGyroBias.end(), 1415 packet.data.hardwareBias); 1416 } 1417 if (softwareGyroBias.size() == 3) { 1418 // Store SW bias so we can remove bias for uncal data 1419 std::copy(softwareGyroBias.begin(), softwareGyroBias.end(), 1420 mGyroBias); 1421 1422 std::copy(softwareGyroBias.begin(), softwareGyroBias.end(), 1423 packet.data.softwareBias); 1424 } 1425 // send packet to hub 1426 queueDataInternal(COMMS_SENSOR_GYRO, &packet, sizeof(packet)); 1427 } 1428 1429 // over temp cal 1430 std::vector<float> gyroOtcData = getFloatSetting(saved_settings, GYRO_OTC_DATA_TAG); 1431 if (gyroOtcData.size() == sizeof(GyroOtcData) / sizeof(float)) { 1432 std::copy(gyroOtcData.begin(), gyroOtcData.end(), 1433 reinterpret_cast<float*>(&mGyroOtcData)); 1434 struct { 1435 AppToSensorHalDataPayload header; 1436 GyroOtcData data; 1437 } packet = { 1438 .header = { 1439 .size = sizeof(GyroOtcData), 1440 .type = HALINTF_TYPE_GYRO_OTC_DATA }, 1441 .data = mGyroOtcData 1442 }; 1443 1444 // send it to hub 1445 queueDataInternal(COMMS_SENSOR_GYRO, &packet, sizeof(packet)); 1446 } else { 1447 ALOGW("Illegal otc_gyro data size = %zu", gyroOtcData.size()); 1448 } 1449 1450 std::vector<float> magBiasData = getFloatSetting(saved_settings, MAG_BIAS_TAG); 1451 if (magBiasData.size() == 3) { 1452 // Store SW bias so we can remove bias for uncal data 1453 std::copy(magBiasData.begin(), magBiasData.end(), mMagBias); 1454 1455 struct { 1456 AppToSensorHalDataPayload header; 1457 MagCalBias mag; 1458 } packet = { 1459 .header = { 1460 .size = sizeof(MagCalBias), 1461 .type = HALINTF_TYPE_MAG_CAL_BIAS } 1462 }; 1463 std::copy(magBiasData.begin(), magBiasData.end(), packet.mag.bias); 1464 queueDataInternal(COMMS_SENSOR_MAG, &packet, sizeof(packet)); 1465 } 1466 1467 if (settings->getFloat("barometer", &barometer)) 1468 queueDataInternal(COMMS_SENSOR_PRESSURE, &barometer, sizeof(barometer)); 1469 1470 if (settings->getFloat("humidity", &humidity)) 1471 queueDataInternal(COMMS_SENSOR_HUMIDITY, &humidity, sizeof(humidity)); 1472 1473 if (settings->getInt32("proximity", &proximity)) 1474 queueDataInternal(COMMS_SENSOR_PROXIMITY, &proximity, sizeof(proximity)); 1475 1476 if (getCalibrationInt32(settings, "proximity", proximity_array, 4)) 1477 queueDataInternal(COMMS_SENSOR_PROXIMITY, proximity_array, sizeof(proximity_array)); 1478 1479 if (settings->getFloat("light", &light)) 1480 queueDataInternal(COMMS_SENSOR_LIGHT, &light, sizeof(light)); 1481 } 1482 1483 bool HubConnection::threadLoop() { 1484 ALOGV("threadLoop: starting"); 1485 1486 if (mFd < 0) { 1487 ALOGW("threadLoop: exiting prematurely: nanohub is unavailable"); 1488 return false; 1489 } 1490 waitOnNanohubLock(); 1491 1492 sendCalibrationOffsets(); 1493 1494 while (!Thread::exitPending()) { 1495 ssize_t ret; 1496 1497 do { 1498 ret = poll(mPollFds, mNumPollFds, -1); 1499 } while (ret < 0 && errno == EINTR); 1500 1501 if (mInotifyPollIndex >= 0 && mPollFds[mInotifyPollIndex].revents & POLLIN) { 1502 discardInotifyEvent(); 1503 waitOnNanohubLock(); 1504 } 1505 1506 #ifdef USB_MAG_BIAS_REPORTING_ENABLED 1507 if (mMagBiasPollIndex >= 0 && mPollFds[mMagBiasPollIndex].revents & POLLERR) { 1508 // Read from mag bias file 1509 char buf[16]; 1510 lseek(mPollFds[mMagBiasPollIndex].fd, 0, SEEK_SET); 1511 ::read(mPollFds[mMagBiasPollIndex].fd, buf, 16); 1512 float bias = atof(buf); 1513 mUsbMagBias = bias; 1514 queueUsbMagBias(); 1515 } 1516 #endif // USB_MAG_BIAS_REPORTING_ENABLED 1517 1518 #ifdef DOUBLE_TOUCH_ENABLED 1519 if (mDoubleTouchPollIndex >= 0 && mPollFds[mDoubleTouchPollIndex].revents & POLLERR) { 1520 // Read from double touch file 1521 char buf[16]; 1522 lseek(mPollFds[mDoubleTouchPollIndex].fd, 0, SEEK_SET); 1523 ::read(mPollFds[mDoubleTouchPollIndex].fd, buf, 16); 1524 sensors_event_t gestureEvent; 1525 initEv(&gestureEvent, elapsedRealtimeNano(), SENSOR_TYPE_PICK_UP_GESTURE, COMMS_SENSOR_GESTURE)->data[0] = 8; 1526 write(&gestureEvent, 1); 1527 } 1528 #endif // DOUBLE_TOUCH_ENABLED 1529 1530 if (mPollFds[0].revents & POLLIN) { 1531 uint8_t recv[256]; 1532 ssize_t len = ::read(mFd, recv, sizeof(recv)); 1533 1534 if (len >= 0) { 1535 for (ssize_t offset = 0; offset < len;) { 1536 ret = processBuf(recv + offset, len - offset); 1537 1538 if (ret > 0) 1539 offset += ret; 1540 else 1541 break; 1542 } 1543 } else { 1544 ALOGW("read -1: errno=%d\n", errno); 1545 } 1546 } 1547 } 1548 1549 return false; 1550 } 1551 1552 void HubConnection::initConfigCmd(struct ConfigCmd *cmd, int handle) 1553 { 1554 memset(cmd, 0x00, sizeof(*cmd)); 1555 1556 cmd->evtType = EVT_NO_SENSOR_CONFIG_EVENT; 1557 cmd->sensorType = mSensorState[handle].sensorType; 1558 1559 if (mSensorState[handle].enable) { 1560 cmd->cmd = CONFIG_CMD_ENABLE; 1561 cmd->rate = mSensorState[handle].rate; 1562 cmd->latency = mSensorState[handle].latency; 1563 } else { 1564 cmd->cmd = CONFIG_CMD_DISABLE; 1565 // set rate and latency to values that will always be overwritten by the 1566 // first enabled alt sensor 1567 cmd->rate = UINT32_C(0); 1568 cmd->latency = UINT64_MAX; 1569 } 1570 1571 for (int i=0; i<MAX_ALTERNATES; ++i) { 1572 uint8_t alt = mSensorState[handle].alt[i]; 1573 1574 if (alt == COMMS_SENSOR_INVALID) continue; 1575 if (!mSensorState[alt].enable) continue; 1576 1577 cmd->cmd = CONFIG_CMD_ENABLE; 1578 1579 if (mSensorState[alt].rate > cmd->rate) { 1580 cmd->rate = mSensorState[alt].rate; 1581 } 1582 if (mSensorState[alt].latency < cmd->latency) { 1583 cmd->latency = mSensorState[alt].latency; 1584 } 1585 } 1586 1587 // will be a nop if direct report mode is not enabled 1588 mergeDirectReportRequest(cmd, handle); 1589 } 1590 1591 void HubConnection::queueActivate(int handle, bool enable) 1592 { 1593 struct ConfigCmd cmd; 1594 int ret; 1595 1596 Mutex::Autolock autoLock(mLock); 1597 1598 if (isValidHandle(handle)) { 1599 // disabling accel, so no longer need to use the bias from when 1600 // accel was first enabled 1601 if (handle == COMMS_SENSOR_ACCEL && !enable) 1602 mAccelEnabledBiasStored = false; 1603 1604 mSensorState[handle].enable = enable; 1605 1606 initConfigCmd(&cmd, handle); 1607 1608 ret = sendCmd(&cmd, sizeof(cmd)); 1609 if (ret == sizeof(cmd)) { 1610 updateSampleRate(handle, enable ? CONFIG_CMD_ENABLE : CONFIG_CMD_DISABLE); 1611 ALOGV("queueActivate: sensor=%d, handle=%d, enable=%d", 1612 cmd.sensorType, handle, enable); 1613 } 1614 else 1615 ALOGW("queueActivate: failed to send command: sensor=%d, handle=%d, enable=%d", 1616 cmd.sensorType, handle, enable); 1617 } else { 1618 ALOGV("queueActivate: unhandled handle=%d, enable=%d", handle, enable); 1619 } 1620 } 1621 1622 void HubConnection::queueSetDelay(int handle, nsecs_t sampling_period_ns) 1623 { 1624 struct ConfigCmd cmd; 1625 int ret; 1626 1627 Mutex::Autolock autoLock(mLock); 1628 1629 if (isValidHandle(handle)) { 1630 if (sampling_period_ns > 0 && 1631 mSensorState[handle].rate != SENSOR_RATE_ONCHANGE && 1632 mSensorState[handle].rate != SENSOR_RATE_ONESHOT) { 1633 mSensorState[handle].rate = period_ns_to_frequency_q10(sampling_period_ns); 1634 } 1635 1636 initConfigCmd(&cmd, handle); 1637 1638 ret = sendCmd(&cmd, sizeof(cmd)); 1639 if (ret == sizeof(cmd)) 1640 ALOGV("queueSetDelay: sensor=%d, handle=%d, period=%" PRId64, 1641 cmd.sensorType, handle, sampling_period_ns); 1642 else 1643 ALOGW("queueSetDelay: failed to send command: sensor=%d, handle=%d, period=%" PRId64, 1644 cmd.sensorType, handle, sampling_period_ns); 1645 } else { 1646 ALOGV("queueSetDelay: unhandled handle=%d, period=%" PRId64, handle, sampling_period_ns); 1647 } 1648 } 1649 1650 void HubConnection::queueBatch( 1651 int handle, 1652 nsecs_t sampling_period_ns, 1653 nsecs_t max_report_latency_ns) 1654 { 1655 struct ConfigCmd cmd; 1656 int ret; 1657 1658 Mutex::Autolock autoLock(mLock); 1659 1660 if (isValidHandle(handle)) { 1661 if (sampling_period_ns > 0 && 1662 mSensorState[handle].rate != SENSOR_RATE_ONCHANGE && 1663 mSensorState[handle].rate != SENSOR_RATE_ONESHOT) { 1664 mSensorState[handle].rate = period_ns_to_frequency_q10(sampling_period_ns); 1665 } 1666 mSensorState[handle].latency = max_report_latency_ns; 1667 1668 initConfigCmd(&cmd, handle); 1669 1670 ret = sendCmd(&cmd, sizeof(cmd)); 1671 if (ret == sizeof(cmd)) { 1672 updateSampleRate(handle, CONFIG_CMD_ENABLE); // batch uses CONFIG_CMD_ENABLE command 1673 ALOGV("queueBatch: sensor=%d, handle=%d, period=%" PRId64 ", latency=%" PRId64, 1674 cmd.sensorType, handle, sampling_period_ns, max_report_latency_ns); 1675 } else { 1676 ALOGW("queueBatch: failed to send command: sensor=%d, handle=%d, period=%" PRId64 ", latency=%" PRId64, 1677 cmd.sensorType, handle, sampling_period_ns, max_report_latency_ns); 1678 } 1679 } else { 1680 ALOGV("queueBatch: unhandled handle=%d, period=%" PRId64 ", latency=%" PRId64, 1681 handle, sampling_period_ns, max_report_latency_ns); 1682 } 1683 } 1684 1685 void HubConnection::queueFlush(int handle) 1686 { 1687 Mutex::Autolock autoLock(mLock); 1688 queueFlushInternal(handle, false); 1689 } 1690 1691 void HubConnection::queueFlushInternal(int handle, bool internal) 1692 { 1693 struct ConfigCmd cmd; 1694 uint32_t primary; 1695 int ret; 1696 1697 if (isValidHandle(handle)) { 1698 // If no primary sensor type is specified, 1699 // then 'handle' is the primary sensor type. 1700 primary = mSensorState[handle].primary; 1701 primary = (primary ? primary : handle); 1702 1703 std::list<Flush>& flushList = mFlushesPending[primary]; 1704 1705 if (!flushList.empty() && 1706 flushList.back().internal == internal && 1707 flushList.back().handle == handle) { 1708 ++flushList.back().count; 1709 } else { 1710 flushList.push_back((struct Flush){handle, 1, internal}); 1711 } 1712 1713 initConfigCmd(&cmd, handle); 1714 cmd.cmd = CONFIG_CMD_FLUSH; 1715 1716 ret = sendCmd(&cmd, sizeof(cmd)); 1717 if (ret == sizeof(cmd)) { 1718 ALOGV("queueFlush: sensor=%d, handle=%d", 1719 cmd.sensorType, handle); 1720 } else { 1721 ALOGW("queueFlush: failed to send command: sensor=%d, handle=%d" 1722 " with error %s", cmd.sensorType, handle, strerror(errno)); 1723 } 1724 } else { 1725 ALOGV("queueFlush: unhandled handle=%d", handle); 1726 } 1727 } 1728 1729 void HubConnection::queueDataInternal(int handle, void *data, size_t length) 1730 { 1731 struct ConfigCmd *cmd = (struct ConfigCmd *)malloc(sizeof(struct ConfigCmd) + length); 1732 size_t ret; 1733 1734 if (cmd && isValidHandle(handle)) { 1735 initConfigCmd(cmd, handle); 1736 memcpy(cmd->data, data, length); 1737 cmd->cmd = CONFIG_CMD_CFG_DATA; 1738 1739 ret = sendCmd(cmd, sizeof(*cmd) + length); 1740 if (ret == sizeof(*cmd) + length) 1741 ALOGV("queueData: sensor=%d, length=%zu", 1742 cmd->sensorType, length); 1743 else 1744 ALOGW("queueData: failed to send command: sensor=%d, length=%zu", 1745 cmd->sensorType, length); 1746 } else { 1747 ALOGV("queueData: unhandled handle=%d", handle); 1748 } 1749 free(cmd); 1750 } 1751 1752 void HubConnection::queueData(int handle, void *data, size_t length) 1753 { 1754 Mutex::Autolock autoLock(mLock); 1755 queueDataInternal(handle, data, length); 1756 } 1757 1758 void HubConnection::setOperationParameter(const additional_info_event_t &info) { 1759 switch (info.type) { 1760 case AINFO_LOCAL_GEOMAGNETIC_FIELD: { 1761 ALOGV("local geomag field update: strength %fuT, dec %fdeg, inc %fdeg", 1762 static_cast<double>(info.data_float[0]), 1763 info.data_float[1] * 180 / M_PI, 1764 info.data_float[2] * 180 / M_PI); 1765 1766 struct { 1767 AppToSensorHalDataPayload header; 1768 MagLocalField magLocalField; 1769 } packet = { 1770 .header = { 1771 .size = sizeof(MagLocalField), 1772 .type = HALINTF_TYPE_MAG_LOCAL_FIELD }, 1773 .magLocalField = { 1774 .strength = info.data_float[0], 1775 .declination = info.data_float[1], 1776 .inclination = info.data_float[2]} 1777 }; 1778 queueDataInternal(COMMS_SENSOR_MAG, &packet, sizeof(packet)); 1779 break; 1780 } 1781 default: 1782 break; 1783 } 1784 } 1785 1786 void HubConnection::initNanohubLock() { 1787 // Create the lock directory (if it doesn't already exist) 1788 if (mkdir(NANOHUB_LOCK_DIR, NANOHUB_LOCK_DIR_PERMS) < 0 && errno != EEXIST) { 1789 ALOGW("Couldn't create Nanohub lock directory: %s", strerror(errno)); 1790 return; 1791 } 1792 1793 mInotifyPollIndex = -1; 1794 int inotifyFd = inotify_init1(IN_NONBLOCK); 1795 if (inotifyFd < 0) { 1796 ALOGW("Couldn't initialize inotify: %s", strerror(errno)); 1797 } else if (inotify_add_watch(inotifyFd, NANOHUB_LOCK_DIR, IN_CREATE | IN_DELETE) < 0) { 1798 ALOGW("Couldn't add inotify watch: %s", strerror(errno)); 1799 close(inotifyFd); 1800 } else { 1801 mPollFds[mNumPollFds].fd = inotifyFd; 1802 mPollFds[mNumPollFds].events = POLLIN; 1803 mPollFds[mNumPollFds].revents = 0; 1804 mInotifyPollIndex = mNumPollFds; 1805 mNumPollFds++; 1806 } 1807 } 1808 1809 ssize_t HubConnection::read(sensors_event_t *ev, size_t size) { 1810 ssize_t n = mRing.read(ev, size); 1811 1812 Mutex::Autolock autoLock(mLock); 1813 1814 // We log the first failure in write, so only log 2+ errors 1815 if (mWriteFailures > 1) { 1816 ALOGW("%s: mRing.write failed %d times", 1817 __FUNCTION__, mWriteFailures); 1818 mWriteFailures = 0; 1819 } 1820 1821 for (ssize_t i = 0; i < n; i++) 1822 decrementIfWakeEventLocked(ev[i].sensor); 1823 1824 return n; 1825 } 1826 1827 1828 ssize_t HubConnection::write(const sensors_event_t *ev, size_t n) { 1829 ssize_t ret = 0; 1830 1831 Mutex::Autolock autoLock(mLock); 1832 1833 for (size_t i=0; i<n; i++) { 1834 if (mRing.write(&ev[i], 1) == 1) { 1835 ret++; 1836 // If event is a wake event, protect it with a wakelock 1837 protectIfWakeEventLocked(ev[i].sensor); 1838 } else { 1839 if (mWriteFailures++ == 0) 1840 ALOGW("%s: mRing.write failed @ %zu/%zu", 1841 __FUNCTION__, i, n); 1842 break; 1843 } 1844 } 1845 1846 return ret; 1847 } 1848 1849 #ifdef USB_MAG_BIAS_REPORTING_ENABLED 1850 void HubConnection::queueUsbMagBias() 1851 { 1852 struct MsgCmd *cmd = (struct MsgCmd *)malloc(sizeof(struct MsgCmd) + sizeof(float)); 1853 size_t ret; 1854 1855 if (cmd) { 1856 cmd->evtType = EVT_APP_FROM_HOST; 1857 cmd->msg.appId = APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, APP_ID_APP_BMI160); 1858 cmd->msg.dataLen = sizeof(float); 1859 memcpy((float *)(cmd+1), &mUsbMagBias, sizeof(float)); 1860 1861 ret = sendCmd(cmd, sizeof(*cmd) + sizeof(float)); 1862 if (ret == sizeof(*cmd) + sizeof(float)) 1863 ALOGV("queueUsbMagBias: bias=%f\n", mUsbMagBias); 1864 else 1865 ALOGW("queueUsbMagBias: failed to send command: bias=%f\n", mUsbMagBias); 1866 free(cmd); 1867 } 1868 } 1869 #endif // USB_MAG_BIAS_REPORTING_ENABLED 1870 1871 #ifdef LID_STATE_REPORTING_ENABLED 1872 status_t HubConnection::initializeUinputNode() 1873 { 1874 int ret = 0; 1875 1876 // Open uinput dev node 1877 mUinputFd = TEMP_FAILURE_RETRY(open("/dev/uinput", O_WRONLY | O_NONBLOCK)); 1878 if (mUinputFd < 0) { 1879 ALOGW("could not open uinput node: %s", strerror(errno)); 1880 return UNKNOWN_ERROR; 1881 } 1882 1883 // Enable SW_LID events 1884 ret = TEMP_FAILURE_RETRY(ioctl(mUinputFd, UI_SET_EVBIT, EV_SW)); 1885 ret |= TEMP_FAILURE_RETRY(ioctl(mUinputFd, UI_SET_EVBIT, EV_SYN)); 1886 ret |= TEMP_FAILURE_RETRY(ioctl(mUinputFd, UI_SET_SWBIT, SW_LID)); 1887 if (ret < 0) { 1888 ALOGW("could not send ioctl to uinput node: %s", strerror(errno)); 1889 return UNKNOWN_ERROR; 1890 } 1891 1892 // Create uinput node for SW_LID 1893 struct uinput_user_dev uidev; 1894 memset(&uidev, 0, sizeof(uidev)); 1895 snprintf(uidev.name, UINPUT_MAX_NAME_SIZE, "uinput-folio"); 1896 uidev.id.bustype = BUS_SPI; 1897 uidev.id.vendor = 0; 1898 uidev.id.product = 0; 1899 uidev.id.version = 0; 1900 1901 ret = TEMP_FAILURE_RETRY(::write(mUinputFd, &uidev, sizeof(uidev))); 1902 if (ret < 0) { 1903 ALOGW("write to uinput node failed: %s", strerror(errno)); 1904 return UNKNOWN_ERROR; 1905 } 1906 1907 ret = TEMP_FAILURE_RETRY(ioctl(mUinputFd, UI_DEV_CREATE)); 1908 if (ret < 0) { 1909 ALOGW("could not send ioctl to uinput node: %s", strerror(errno)); 1910 return UNKNOWN_ERROR; 1911 } 1912 1913 return OK; 1914 } 1915 1916 void HubConnection::sendFolioEvent(int32_t data) { 1917 ssize_t ret = 0; 1918 struct input_event ev; 1919 1920 memset(&ev, 0, sizeof(ev)); 1921 1922 ev.type = EV_SW; 1923 ev.code = SW_LID; 1924 ev.value = data; 1925 ret = TEMP_FAILURE_RETRY(::write(mUinputFd, &ev, sizeof(ev))); 1926 if (ret < 0) { 1927 ALOGW("write to uinput node failed: %s", strerror(errno)); 1928 return; 1929 } 1930 1931 // Force flush with EV_SYN event 1932 ev.type = EV_SYN; 1933 ev.code = SYN_REPORT; 1934 ev.value = 0; 1935 ret = TEMP_FAILURE_RETRY(::write(mUinputFd, &ev, sizeof(ev))); 1936 if (ret < 0) { 1937 ALOGW("write to uinput node failed: %s", strerror(errno)); 1938 return; 1939 } 1940 1941 // Set lid state property 1942 if (property_set(LID_STATE_PROPERTY, 1943 (data ? LID_STATE_CLOSED : LID_STATE_OPEN)) < 0) { 1944 ALOGW("could not set lid_state property"); 1945 } 1946 } 1947 #endif // LID_STATE_REPORTING_ENABLED 1948 1949 #ifdef DIRECT_REPORT_ENABLED 1950 void HubConnection::sendDirectReportEvent(const sensors_event_t *nev, size_t n) { 1951 // short circuit to avoid lock operation 1952 if (n == 0) { 1953 return; 1954 } 1955 1956 // no intention to block sensor delivery thread. when lock is needed ignore 1957 // the event (this only happens when the channel is reconfiured, so it's ok 1958 if (mDirectChannelLock.tryLock() == NO_ERROR) { 1959 while (n--) { 1960 auto i = mSensorToChannel.find(nev->sensor); 1961 if (i != mSensorToChannel.end()) { 1962 for (auto &j : i->second) { 1963 if ((uint64_t)nev->timestamp > j.second.lastTimestamp 1964 && intervalLargeEnough( 1965 nev->timestamp - j.second.lastTimestamp, 1966 rateLevelToDeviceSamplingPeriodNs( 1967 nev->sensor, j.second.rateLevel))) { 1968 mDirectChannel[j.first]->write(nev); 1969 j.second.lastTimestamp = nev->timestamp; 1970 } 1971 } 1972 } 1973 ++nev; 1974 } 1975 mDirectChannelLock.unlock(); 1976 } 1977 } 1978 1979 void HubConnection::mergeDirectReportRequest(struct ConfigCmd *cmd, int handle) { 1980 int maxRateLevel = SENSOR_DIRECT_RATE_STOP; 1981 1982 auto j = mSensorToChannel.find(handle); 1983 if (j != mSensorToChannel.end()) { 1984 for (auto &i : j->second) { 1985 maxRateLevel = std::max(i.second.rateLevel, maxRateLevel); 1986 } 1987 } 1988 for (auto handle : mSensorState[handle].alt) { 1989 auto j = mSensorToChannel.find(handle); 1990 if (j != mSensorToChannel.end()) { 1991 for (auto &i : j->second) { 1992 maxRateLevel = std::max(i.second.rateLevel, maxRateLevel); 1993 } 1994 } 1995 } 1996 1997 uint64_t period = rateLevelToDeviceSamplingPeriodNs(handle, maxRateLevel); 1998 if (period != INT64_MAX) { 1999 rate_q10_t rate; 2000 rate = period_ns_to_frequency_q10(period); 2001 2002 cmd->rate = (rate > cmd->rate || cmd->cmd == CONFIG_CMD_DISABLE) ? rate : cmd->rate; 2003 cmd->latency = 0; 2004 cmd->cmd = CONFIG_CMD_ENABLE; 2005 } 2006 } 2007 2008 int HubConnection::addDirectChannel(const struct sensors_direct_mem_t *mem) { 2009 std::unique_ptr<DirectChannelBase> ch; 2010 int ret = NO_MEMORY; 2011 2012 Mutex::Autolock autoLock(mDirectChannelLock); 2013 for (const auto& c : mDirectChannel) { 2014 if (c.second->memoryMatches(mem)) { 2015 // cannot reusing same memory 2016 return BAD_VALUE; 2017 } 2018 } 2019 switch(mem->type) { 2020 case SENSOR_DIRECT_MEM_TYPE_ASHMEM: 2021 ch = std::make_unique<AshmemDirectChannel>(mem); 2022 break; 2023 case SENSOR_DIRECT_MEM_TYPE_GRALLOC: 2024 ch = std::make_unique<GrallocDirectChannel>(mem); 2025 break; 2026 default: 2027 ret = INVALID_OPERATION; 2028 } 2029 2030 if (ch) { 2031 if (ch->isValid()) { 2032 ret = mDirectChannelHandle++; 2033 mDirectChannel.insert(std::make_pair(ret, std::move(ch))); 2034 } else { 2035 ret = ch->getError(); 2036 ALOGW("Direct channel object(type:%d) has error %d upon init", mem->type, ret); 2037 } 2038 } 2039 2040 return ret; 2041 } 2042 2043 int HubConnection::removeDirectChannel(int channel_handle) { 2044 // make sure no active sensor in this channel 2045 std::vector<int32_t> activeSensorList; 2046 stopAllDirectReportOnChannel(channel_handle, &activeSensorList); 2047 2048 // sensor service is responsible for stop all sensors before remove direct 2049 // channel. Thus, this is an error. 2050 if (!activeSensorList.empty()) { 2051 std::stringstream ss; 2052 std::copy(activeSensorList.begin(), activeSensorList.end(), 2053 std::ostream_iterator<int32_t>(ss, ",")); 2054 ALOGW("Removing channel %d when sensors (%s) are not stopped.", 2055 channel_handle, ss.str().c_str()); 2056 } 2057 2058 // remove the channel record 2059 Mutex::Autolock autoLock(mDirectChannelLock); 2060 mDirectChannel.erase(channel_handle); 2061 return NO_ERROR; 2062 } 2063 2064 int HubConnection::stopAllDirectReportOnChannel( 2065 int channel_handle, std::vector<int32_t> *activeSensorList) { 2066 Mutex::Autolock autoLock(mDirectChannelLock); 2067 if (mDirectChannel.find(channel_handle) == mDirectChannel.end()) { 2068 return BAD_VALUE; 2069 } 2070 2071 std::vector<int32_t> sensorToStop; 2072 for (auto &it : mSensorToChannel) { 2073 auto j = it.second.find(channel_handle); 2074 if (j != it.second.end()) { 2075 it.second.erase(j); 2076 if (it.second.empty()) { 2077 sensorToStop.push_back(it.first); 2078 } 2079 } 2080 } 2081 2082 if (activeSensorList != nullptr) { 2083 *activeSensorList = sensorToStop; 2084 } 2085 2086 // re-evaluate and send config for all sensor that need to be stopped 2087 bool ret = true; 2088 for (auto sensor_handle : sensorToStop) { 2089 Mutex::Autolock autoLock2(mLock); 2090 struct ConfigCmd cmd; 2091 initConfigCmd(&cmd, sensor_handle); 2092 2093 int result = sendCmd(&cmd, sizeof(cmd)); 2094 ret = ret && (result == sizeof(cmd)); 2095 } 2096 return ret ? NO_ERROR : BAD_VALUE; 2097 } 2098 2099 int HubConnection::configDirectReport(int sensor_handle, int channel_handle, int rate_level) { 2100 if (sensor_handle == -1 && rate_level == SENSOR_DIRECT_RATE_STOP) { 2101 return stopAllDirectReportOnChannel(channel_handle, nullptr); 2102 } 2103 2104 if (!isValidHandle(sensor_handle)) { 2105 return BAD_VALUE; 2106 } 2107 2108 // clamp to fast 2109 if (rate_level > SENSOR_DIRECT_RATE_FAST) { 2110 rate_level = SENSOR_DIRECT_RATE_FAST; 2111 } 2112 2113 // manage direct channel data structure 2114 Mutex::Autolock autoLock(mDirectChannelLock); 2115 auto i = mDirectChannel.find(channel_handle); 2116 if (i == mDirectChannel.end()) { 2117 return BAD_VALUE; 2118 } 2119 2120 auto j = mSensorToChannel.find(sensor_handle); 2121 if (j == mSensorToChannel.end()) { 2122 return BAD_VALUE; 2123 } 2124 2125 j->second.erase(channel_handle); 2126 if (rate_level != SENSOR_DIRECT_RATE_STOP) { 2127 j->second.insert(std::make_pair(channel_handle, (DirectChannelTimingInfo){0, rate_level})); 2128 } 2129 2130 Mutex::Autolock autoLock2(mLock); 2131 struct ConfigCmd cmd; 2132 initConfigCmd(&cmd, sensor_handle); 2133 2134 int ret = sendCmd(&cmd, sizeof(cmd)); 2135 2136 if (rate_level == SENSOR_DIRECT_RATE_STOP) { 2137 ret = NO_ERROR; 2138 } else { 2139 ret = (ret == sizeof(cmd)) ? sensor_handle : BAD_VALUE; 2140 } 2141 return ret; 2142 } 2143 2144 bool HubConnection::isDirectReportSupported() const { 2145 return true; 2146 } 2147 2148 void HubConnection::updateSampleRate(int handle, int reason) { 2149 bool affected = mSensorToChannel.find(handle) != mSensorToChannel.end(); 2150 for (size_t i = 0; i < MAX_ALTERNATES && !affected; ++i) { 2151 if (mSensorState[handle].alt[i] != COMMS_SENSOR_INVALID) { 2152 affected |= 2153 mSensorToChannel.find(mSensorState[handle].alt[i]) != mSensorToChannel.end(); 2154 } 2155 } 2156 if (!affected) { 2157 return; 2158 } 2159 2160 switch (reason) { 2161 case CONFIG_CMD_ENABLE: { 2162 constexpr uint64_t PERIOD_800HZ = 1250000; 2163 uint64_t period_multiplier = 2164 (frequency_q10_to_period_ns(mSensorState[handle].rate) + PERIOD_800HZ / 2) 2165 / PERIOD_800HZ; 2166 uint64_t desiredTSample = PERIOD_800HZ; 2167 while (period_multiplier /= 2) { 2168 desiredTSample *= 2; 2169 } 2170 mSensorState[handle].desiredTSample = desiredTSample; 2171 ALOGV("DesiredTSample for handle 0x%x set to %" PRIu64, handle, desiredTSample); 2172 break; 2173 } 2174 case CONFIG_CMD_DISABLE: 2175 mSensorState[handle].desiredTSample = INT64_MAX; 2176 ALOGV("DesiredTSample 0x%x set to disable", handle); 2177 break; 2178 default: 2179 ALOGW("%s: unexpected reason = %d, no-op", __FUNCTION__, reason); 2180 break; 2181 } 2182 } 2183 2184 bool HubConnection::isSampleIntervalSatisfied(int handle, uint64_t timestamp) { 2185 if (mSensorToChannel.find(handle) == mSensorToChannel.end()) { 2186 return true; 2187 } 2188 2189 if (mSensorState[handle].lastTimestamp >= timestamp 2190 || mSensorState[handle].desiredTSample == INT64_MAX) { 2191 return false; 2192 } else if (intervalLargeEnough(timestamp - mSensorState[handle].lastTimestamp, 2193 mSensorState[handle].desiredTSample)) { 2194 mSensorState[handle].lastTimestamp = timestamp; 2195 return true; 2196 } else { 2197 return false; 2198 } 2199 } 2200 2201 uint64_t HubConnection::rateLevelToDeviceSamplingPeriodNs(int handle, int rateLevel) const { 2202 if (mSensorToChannel.find(handle) == mSensorToChannel.end()) { 2203 return INT64_MAX; 2204 } 2205 2206 switch (rateLevel) { 2207 case SENSOR_DIRECT_RATE_VERY_FAST: 2208 [[fallthrough]]; // No sensor support VERY_FAST, fall through 2209 case SENSOR_DIRECT_RATE_FAST: 2210 if (handle != COMMS_SENSOR_MAG && handle != COMMS_SENSOR_MAG_UNCALIBRATED) { 2211 return 2500*1000; // 400Hz 2212 } 2213 [[fallthrough]]; 2214 case SENSOR_DIRECT_RATE_NORMAL: 2215 return 20*1000*1000; // 50 Hz 2216 default: 2217 return INT64_MAX; 2218 } 2219 } 2220 #else // DIRECT_REPORT_ENABLED 2221 // nop functions if feature is turned off 2222 int HubConnection::addDirectChannel(const struct sensors_direct_mem_t *) { 2223 return INVALID_OPERATION; 2224 } 2225 2226 int HubConnection::removeDirectChannel(int) { 2227 return INVALID_OPERATION; 2228 } 2229 2230 int HubConnection::configDirectReport(int, int, int) { 2231 return INVALID_OPERATION; 2232 } 2233 2234 void HubConnection::sendDirectReportEvent(const sensors_event_t *, size_t) { 2235 } 2236 2237 void HubConnection::mergeDirectReportRequest(struct ConfigCmd *, int) { 2238 } 2239 2240 bool HubConnection::isDirectReportSupported() const { 2241 return false; 2242 } 2243 2244 void HubConnection::updateSampleRate(int, int) { 2245 } 2246 2247 bool HubConnection::isSampleIntervalSatisfied(int, uint64_t) { 2248 return true; 2249 } 2250 #endif // DIRECT_REPORT_ENABLED 2251 2252 } // namespace android 2253
