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