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