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