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 #include "hubconnection.h"
18 #include "eventnums.h"
19 #include "sensType.h"
20
21 #define LOG_TAG "nanohub"
22 #include <utils/Log.h>
23 #include <utils/SystemClock.h>
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
33 #include <cutils/properties.h>
34 #include <linux/input.h>
35 #include <linux/uinput.h>
36 #include <media/stagefright/foundation/ADebug.h>
37 #include <sched.h>
38 #include <sys/inotify.h>
39
40 #define APP_ID_GET_VENDOR(appid) ((appid) >> 24)
41 #define APP_ID_MAKE(vendor, app) ((((uint64_t)(vendor)) << 24) | ((app) & 0x00FFFFFF))
42 #define APP_ID_VENDOR_GOOGLE 0x476f6f676cULL // "Googl"
43 #define APP_ID_APP_BMI160 2
44
45 #define SENS_TYPE_TO_EVENT(_sensorType) (EVT_NO_FIRST_SENSOR_EVENT + (_sensorType))
46
47 #define NANOHUB_FILE_PATH "/dev/nanohub"
48 #define NANOHUB_LOCK_DIR "/data/system/nanohub_lock"
49 #define NANOHUB_LOCK_FILE NANOHUB_LOCK_DIR "/lock"
50 #define MAG_BIAS_FILE_PATH "/sys/class/power_supply/battery/compass_compensation"
51 #define DOUBLE_TOUCH_FILE_PATH "/sys/android_touch/synaptics_rmi4_dsx/wake_event"
52
53 #define NANOHUB_LOCK_DIR_PERMS (S_IRUSR | S_IWUSR | S_IXUSR)
54
55 #define SENSOR_RATE_ONCHANGE 0xFFFFFF01UL
56 #define SENSOR_RATE_ONESHOT 0xFFFFFF02UL
57
58 #define MIN_MAG_SQ (10.0f * 10.0f)
59 #define MAX_MAG_SQ (80.0f * 80.0f)
60
61 #define ACCEL_RAW_KSCALE (8.0f * 9.81f / 32768.0f)
62
63 #define OS_LOG_EVENT 0x474F4C41 // ascii: ALOG
64
65 #define HUBCONNECTION_SCHED_FIFO_PRIORITY 10
66
67 #ifdef LID_STATE_REPORTING_ENABLED
68 const char LID_STATE_PROPERTY[] = "sensors.contexthub.lid_state";
69 const char LID_STATE_UNKNOWN[] = "unknown";
70 const char LID_STATE_OPEN[] = "open";
71 const char LID_STATE_CLOSED[] = "closed";
72 #endif // LID_STATE_REPORTING_ENABLED
73
74 static const uint32_t delta_time_encoded = 1;
75 static const uint32_t delta_time_shift_table[2] = {9, 0};
76
77 namespace android {
78
79 // static
80 Mutex HubConnection::sInstanceLock;
81
82 // static
83 HubConnection *HubConnection::sInstance = NULL;
84
getInstance()85 HubConnection *HubConnection::getInstance()
86 {
87 Mutex::Autolock autoLock(sInstanceLock);
88 if (sInstance == NULL) {
89 sInstance = new HubConnection;
90 }
91 return sInstance;
92 }
93
isActivitySensor(int sensorIndex)94 static bool isActivitySensor(int sensorIndex) {
95 return sensorIndex >= COMMS_SENSOR_ACTIVITY_FIRST
96 && sensorIndex <= COMMS_SENSOR_ACTIVITY_LAST;
97 }
98
HubConnection()99 HubConnection::HubConnection()
100 : Thread(false /* canCallJava */),
101 mRing(10 *1024),
102 mActivityEventHandler(NULL),
103 mStepCounterOffset(0ull),
104 mLastStepCount(0ull)
105 {
106 mMagBias[0] = mMagBias[1] = mMagBias[2] = 0.0f;
107 mMagAccuracy = SENSOR_STATUS_UNRELIABLE;
108 mMagAccuracyRestore = SENSOR_STATUS_UNRELIABLE;
109 mGyroBias[0] = mGyroBias[1] = mGyroBias[2] = 0.0f;
110 mAccelBias[0] = mAccelBias[1] = mAccelBias[2] = 0.0f;
111
112 memset(&mSensorState, 0x00, sizeof(mSensorState));
113 mFd = open(NANOHUB_FILE_PATH, O_RDWR);
114 mPollFds[0].fd = mFd;
115 mPollFds[0].events = POLLIN;
116 mPollFds[0].revents = 0;
117 mNumPollFds = 1;
118
119 initNanohubLock();
120
121 #ifdef USB_MAG_BIAS_REPORTING_ENABLED
122 mUsbMagBias = 0;
123 mMagBiasPollIndex = -1;
124 int magBiasFd = open(MAG_BIAS_FILE_PATH, O_RDONLY);
125 if (magBiasFd < 0) {
126 ALOGW("Mag bias file open failed: %s", strerror(errno));
127 } else {
128 mPollFds[mNumPollFds].fd = magBiasFd;
129 mPollFds[mNumPollFds].events = 0;
130 mPollFds[mNumPollFds].revents = 0;
131 mMagBiasPollIndex = mNumPollFds;
132 mNumPollFds++;
133 }
134 #endif // USB_MAG_BIAS_REPORTING_ENABLED
135
136 #ifdef DOUBLE_TOUCH_ENABLED
137 mDoubleTouchPollIndex = -1;
138 int doubleTouchFd = open(DOUBLE_TOUCH_FILE_PATH, O_RDONLY);
139 if (doubleTouchFd < 0) {
140 ALOGW("Double touch file open failed: %s", strerror(errno));
141 } else {
142 mPollFds[mNumPollFds].fd = doubleTouchFd;
143 mPollFds[mNumPollFds].events = 0;
144 mPollFds[mNumPollFds].revents = 0;
145 mDoubleTouchPollIndex = mNumPollFds;
146 mNumPollFds++;
147 }
148 #endif // DOUBLE_TOUCH_ENABLED
149
150 mSensorState[COMMS_SENSOR_ACCEL].sensorType = SENS_TYPE_ACCEL;
151 mSensorState[COMMS_SENSOR_GYRO].sensorType = SENS_TYPE_GYRO;
152 mSensorState[COMMS_SENSOR_GYRO].alt = COMMS_SENSOR_GYRO_UNCALIBRATED;
153 mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].sensorType = SENS_TYPE_GYRO;
154 mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].alt = COMMS_SENSOR_GYRO;
155 mSensorState[COMMS_SENSOR_MAG].sensorType = SENS_TYPE_MAG;
156 mSensorState[COMMS_SENSOR_MAG].alt = COMMS_SENSOR_MAG_UNCALIBRATED;
157 mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].sensorType = SENS_TYPE_MAG;
158 mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].alt = COMMS_SENSOR_MAG;
159 mSensorState[COMMS_SENSOR_LIGHT].sensorType = SENS_TYPE_ALS;
160 mSensorState[COMMS_SENSOR_PROXIMITY].sensorType = SENS_TYPE_PROX;
161 mSensorState[COMMS_SENSOR_PRESSURE].sensorType = SENS_TYPE_BARO;
162 mSensorState[COMMS_SENSOR_TEMPERATURE].sensorType = SENS_TYPE_TEMP;
163 mSensorState[COMMS_SENSOR_ORIENTATION].sensorType = SENS_TYPE_ORIENTATION;
164 mSensorState[COMMS_SENSOR_WINDOW_ORIENTATION].sensorType = SENS_TYPE_WIN_ORIENTATION;
165 mSensorState[COMMS_SENSOR_WINDOW_ORIENTATION].rate = SENSOR_RATE_ONCHANGE;
166 mSensorState[COMMS_SENSOR_STEP_DETECTOR].sensorType = SENS_TYPE_STEP_DETECT;
167 mSensorState[COMMS_SENSOR_STEP_DETECTOR].rate = SENSOR_RATE_ONCHANGE;
168 mSensorState[COMMS_SENSOR_STEP_COUNTER].sensorType = SENS_TYPE_STEP_COUNT;
169 mSensorState[COMMS_SENSOR_SIGNIFICANT_MOTION].sensorType = SENS_TYPE_SIG_MOTION;
170 mSensorState[COMMS_SENSOR_SIGNIFICANT_MOTION].rate = SENSOR_RATE_ONESHOT;
171 mSensorState[COMMS_SENSOR_GRAVITY].sensorType = SENS_TYPE_GRAVITY;
172 mSensorState[COMMS_SENSOR_LINEAR_ACCEL].sensorType = SENS_TYPE_LINEAR_ACCEL;
173 mSensorState[COMMS_SENSOR_ROTATION_VECTOR].sensorType = SENS_TYPE_ROTATION_VECTOR;
174 mSensorState[COMMS_SENSOR_GEO_MAG].sensorType = SENS_TYPE_GEO_MAG_ROT_VEC;
175 mSensorState[COMMS_SENSOR_GAME_ROTATION_VECTOR].sensorType = SENS_TYPE_GAME_ROT_VECTOR;
176 mSensorState[COMMS_SENSOR_HALL].sensorType = SENS_TYPE_HALL;
177 mSensorState[COMMS_SENSOR_HALL].rate = SENSOR_RATE_ONCHANGE;
178 mSensorState[COMMS_SENSOR_SYNC].sensorType = SENS_TYPE_VSYNC;
179 mSensorState[COMMS_SENSOR_SYNC].rate = SENSOR_RATE_ONCHANGE;
180 mSensorState[COMMS_SENSOR_TILT].sensorType = SENS_TYPE_TILT;
181 mSensorState[COMMS_SENSOR_TILT].rate = SENSOR_RATE_ONCHANGE;
182 mSensorState[COMMS_SENSOR_GESTURE].sensorType = SENS_TYPE_GESTURE;
183 mSensorState[COMMS_SENSOR_GESTURE].rate = SENSOR_RATE_ONESHOT;
184 mSensorState[COMMS_SENSOR_DOUBLE_TWIST].sensorType = SENS_TYPE_DOUBLE_TWIST;
185 mSensorState[COMMS_SENSOR_DOUBLE_TWIST].rate = SENSOR_RATE_ONCHANGE;
186 mSensorState[COMMS_SENSOR_DOUBLE_TAP].sensorType = SENS_TYPE_DOUBLE_TAP;
187 mSensorState[COMMS_SENSOR_DOUBLE_TAP].rate = SENSOR_RATE_ONCHANGE;
188 mSensorState[COMMS_SENSOR_WRIST_TILT].sensorType = SENS_TYPE_WRIST_TILT;
189 mSensorState[COMMS_SENSOR_WRIST_TILT].rate = SENSOR_RATE_ONCHANGE;
190 mSensorState[COMMS_SENSOR_DOUBLE_TOUCH].sensorType = SENS_TYPE_DOUBLE_TOUCH;
191 mSensorState[COMMS_SENSOR_DOUBLE_TOUCH].rate = SENSOR_RATE_ONESHOT;
192 mSensorState[COMMS_SENSOR_ACTIVITY_IN_VEHICLE_START].sensorType = SENS_TYPE_ACTIVITY_IN_VEHICLE_START;
193 mSensorState[COMMS_SENSOR_ACTIVITY_IN_VEHICLE_START].rate = SENSOR_RATE_ONCHANGE;
194 mSensorState[COMMS_SENSOR_ACTIVITY_IN_VEHICLE_STOP].sensorType = SENS_TYPE_ACTIVITY_IN_VEHICLE_STOP;
195 mSensorState[COMMS_SENSOR_ACTIVITY_IN_VEHICLE_STOP].rate = SENSOR_RATE_ONCHANGE;
196 mSensorState[COMMS_SENSOR_ACTIVITY_ON_BICYCLE_START].sensorType = SENS_TYPE_ACTIVITY_ON_BICYCLE_START;
197 mSensorState[COMMS_SENSOR_ACTIVITY_ON_BICYCLE_START].rate = SENSOR_RATE_ONCHANGE;
198 mSensorState[COMMS_SENSOR_ACTIVITY_ON_BICYCLE_STOP].sensorType = SENS_TYPE_ACTIVITY_ON_BICYCLE_STOP;
199 mSensorState[COMMS_SENSOR_ACTIVITY_ON_BICYCLE_STOP].rate = SENSOR_RATE_ONCHANGE;
200 mSensorState[COMMS_SENSOR_ACTIVITY_WALKING_START].sensorType = SENS_TYPE_ACTIVITY_WALKING_START;
201 mSensorState[COMMS_SENSOR_ACTIVITY_WALKING_START].rate = SENSOR_RATE_ONCHANGE;
202 mSensorState[COMMS_SENSOR_ACTIVITY_WALKING_STOP].sensorType = SENS_TYPE_ACTIVITY_WALKING_STOP;
203 mSensorState[COMMS_SENSOR_ACTIVITY_WALKING_STOP].rate = SENSOR_RATE_ONCHANGE;
204 mSensorState[COMMS_SENSOR_ACTIVITY_RUNNING_START].sensorType = SENS_TYPE_ACTIVITY_RUNNING_START;
205 mSensorState[COMMS_SENSOR_ACTIVITY_RUNNING_START].rate = SENSOR_RATE_ONCHANGE;
206 mSensorState[COMMS_SENSOR_ACTIVITY_RUNNING_STOP].sensorType = SENS_TYPE_ACTIVITY_RUNNING_STOP;
207 mSensorState[COMMS_SENSOR_ACTIVITY_RUNNING_STOP].rate = SENSOR_RATE_ONCHANGE;
208 mSensorState[COMMS_SENSOR_ACTIVITY_STILL_START].sensorType = SENS_TYPE_ACTIVITY_STILL_START;
209 mSensorState[COMMS_SENSOR_ACTIVITY_STILL_START].rate = SENSOR_RATE_ONCHANGE;
210 mSensorState[COMMS_SENSOR_ACTIVITY_STILL_STOP].sensorType = SENS_TYPE_ACTIVITY_STILL_STOP;
211 mSensorState[COMMS_SENSOR_ACTIVITY_STILL_STOP].rate = SENSOR_RATE_ONCHANGE;
212 mSensorState[COMMS_SENSOR_ACTIVITY_TILTING].sensorType = SENS_TYPE_ACTIVITY_TILTING;
213 mSensorState[COMMS_SENSOR_ACTIVITY_TILTING].rate = SENSOR_RATE_ONCHANGE;
214 mSensorState[COMMS_SENSOR_GAZE].sensorType = SENS_TYPE_GAZE;
215 mSensorState[COMMS_SENSOR_GAZE].rate = SENSOR_RATE_ONESHOT;
216 mSensorState[COMMS_SENSOR_UNGAZE].sensorType = SENS_TYPE_UNGAZE;
217 mSensorState[COMMS_SENSOR_UNGAZE].rate = SENSOR_RATE_ONESHOT;
218
219 #ifdef LID_STATE_REPORTING_ENABLED
220 initializeUinputNode();
221
222 // set initial lid state
223 if (property_set(LID_STATE_PROPERTY, LID_STATE_UNKNOWN) < 0) {
224 ALOGE("could not set lid_state property");
225 }
226
227 // enable hall sensor for folio
228 if (mFd >= 0) {
229 queueActivate(COMMS_SENSOR_HALL, true /* enable */);
230 }
231 #endif // LID_STATE_REPORTING_ENABLED
232 }
233
~HubConnection()234 HubConnection::~HubConnection()
235 {
236 close(mFd);
237 }
238
onFirstRef()239 void HubConnection::onFirstRef()
240 {
241 run("HubConnection", PRIORITY_URGENT_DISPLAY);
242 enableSchedFifoMode();
243 }
244
245 // Set main thread to SCHED_FIFO to lower sensor event latency when system is under load
enableSchedFifoMode()246 void HubConnection::enableSchedFifoMode() {
247 struct sched_param param = {0};
248 param.sched_priority = HUBCONNECTION_SCHED_FIFO_PRIORITY;
249 if (sched_setscheduler(getTid(), SCHED_FIFO | SCHED_RESET_ON_FORK, ¶m) != 0) {
250 ALOGE("Couldn't set SCHED_FIFO for HubConnection thread");
251 }
252 }
253
initCheck() const254 status_t HubConnection::initCheck() const
255 {
256 return mFd < 0 ? UNKNOWN_ERROR : OK;
257 }
258
getAliveCheck()259 status_t HubConnection::getAliveCheck()
260 {
261 return OK;
262 }
263
readSettings(File * file)264 static sp<JSONObject> readSettings(File *file) {
265 off64_t size = file->seekTo(0, SEEK_END);
266 file->seekTo(0, SEEK_SET);
267
268 sp<JSONObject> root;
269
270 if (size > 0) {
271 char *buf = (char *)malloc(size);
272 CHECK_EQ(file->read(buf, size), (ssize_t)size);
273 file->seekTo(0, SEEK_SET);
274
275 sp<JSONCompound> in = JSONCompound::Parse(buf, size);
276 free(buf);
277 buf = NULL;
278
279 if (in != NULL && in->isObject()) {
280 root = (JSONObject *)in.get();
281 }
282 }
283
284 if (root == NULL) {
285 root = new JSONObject;
286 }
287
288 return root;
289 }
290
getCalibrationInt32(const sp<JSONObject> & settings,const char * key,int32_t * out,size_t numArgs)291 static bool getCalibrationInt32(
292 const sp<JSONObject> &settings, const char *key, int32_t *out,
293 size_t numArgs) {
294 sp<JSONArray> array;
295 for (size_t i = 0; i < numArgs; i++) {
296 out[i] = 0;
297 }
298 if (!settings->getArray(key, &array)) {
299 return false;
300 } else {
301 for (size_t i = 0; i < numArgs; i++) {
302 if (!array->getInt32(i, &out[i])) {
303 return false;
304 }
305 }
306 }
307 return true;
308 }
309
getCalibrationFloat(const sp<JSONObject> & settings,const char * key,float out[3])310 static bool getCalibrationFloat(
311 const sp<JSONObject> &settings, const char *key, float out[3]) {
312 sp<JSONArray> array;
313 for (size_t i = 0; i < 3; i++) {
314 out[i] = 0.0f;
315 }
316 if (!settings->getArray(key, &array)) {
317 return false;
318 } else {
319 for (size_t i = 0; i < 3; i++) {
320 if (!array->getFloat(i, &out[i])) {
321 return false;
322 }
323 }
324 }
325 return true;
326 }
327
loadSensorSettings(sp<JSONObject> * settings,sp<JSONObject> * saved_settings)328 static void loadSensorSettings(sp<JSONObject>* settings,
329 sp<JSONObject>* saved_settings) {
330 File settings_file(CONTEXTHUB_SETTINGS_PATH, "r");
331 File saved_settings_file(CONTEXTHUB_SAVED_SETTINGS_PATH, "r");
332
333 status_t err;
334 if ((err = settings_file.initCheck()) != OK) {
335 ALOGE("settings file open failed: %d (%s)",
336 err,
337 strerror(-err));
338
339 *settings = new JSONObject;
340 } else {
341 *settings = readSettings(&settings_file);
342 }
343
344 if ((err = saved_settings_file.initCheck()) != OK) {
345 ALOGE("saved settings file open failed: %d (%s)",
346 err,
347 strerror(-err));
348 *saved_settings = new JSONObject;
349 } else {
350 *saved_settings = readSettings(&saved_settings_file);
351 }
352 }
353
saveSensorSettings() const354 void HubConnection::saveSensorSettings() const {
355 File saved_settings_file(CONTEXTHUB_SAVED_SETTINGS_PATH, "w");
356 sp<JSONObject> settingsObject = new JSONObject;
357
358 status_t err;
359 if ((err = saved_settings_file.initCheck()) != OK) {
360 ALOGE("saved settings file open failed %d (%s)",
361 err,
362 strerror(-err));
363 return;
364 }
365
366 // Build a settings object.
367 sp<JSONArray> magArray = new JSONArray;
368 #ifdef USB_MAG_BIAS_REPORTING_ENABLED
369 magArray->addFloat(mMagBias[0] + mUsbMagBias);
370 #else
371 magArray->addFloat(mMagBias[0]);
372 #endif // USB_MAG_BIAS_REPORTING_ENABLED
373 magArray->addFloat(mMagBias[1]);
374 magArray->addFloat(mMagBias[2]);
375 settingsObject->setArray("mag", magArray);
376
377 // Add gyro settings
378 sp<JSONArray> gyroArray = new JSONArray;
379 gyroArray->addFloat(mGyroBias[0]);
380 gyroArray->addFloat(mGyroBias[1]);
381 gyroArray->addFloat(mGyroBias[2]);
382 settingsObject->setArray("gyro_sw", gyroArray);
383
384 // Add accel settings
385 sp<JSONArray> accelArray = new JSONArray;
386 accelArray->addFloat(mAccelBias[0]);
387 accelArray->addFloat(mAccelBias[1]);
388 accelArray->addFloat(mAccelBias[2]);
389 settingsObject->setArray("accel_sw", accelArray);
390
391 // Write the JSON string to disk.
392 AString serializedSettings = settingsObject->toString();
393 size_t size = serializedSettings.size();
394 if ((err = saved_settings_file.write(serializedSettings.c_str(), size)) != (ssize_t)size) {
395 ALOGE("saved settings file write failed %d (%s)",
396 err,
397 strerror(-err));
398 }
399 }
400
initEv(sensors_event_t * ev,uint64_t timestamp,uint32_t type,uint32_t sensor)401 sensors_event_t *HubConnection::initEv(sensors_event_t *ev, uint64_t timestamp, uint32_t type, uint32_t sensor)
402 {
403 memset(ev, 0x00, sizeof(sensors_event_t));
404 ev->version = sizeof(sensors_event_t);
405 ev->timestamp = timestamp;
406 ev->type = type;
407 ev->sensor = sensor;
408
409 return ev;
410 }
411
processSample(uint64_t timestamp,uint32_t type,uint32_t sensor,struct OneAxisSample * sample,bool highAccuracy)412 void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct OneAxisSample *sample, __attribute__((unused)) bool highAccuracy)
413 {
414 sensors_event_t nev[1];
415 int cnt = 0;
416
417 switch (sensor) {
418 case COMMS_SENSOR_ACTIVITY_IN_VEHICLE_START:
419 case COMMS_SENSOR_ACTIVITY_IN_VEHICLE_STOP:
420 case COMMS_SENSOR_ACTIVITY_ON_BICYCLE_START:
421 case COMMS_SENSOR_ACTIVITY_ON_BICYCLE_STOP:
422 case COMMS_SENSOR_ACTIVITY_WALKING_START:
423 case COMMS_SENSOR_ACTIVITY_WALKING_STOP:
424 case COMMS_SENSOR_ACTIVITY_RUNNING_START:
425 case COMMS_SENSOR_ACTIVITY_RUNNING_STOP:
426 case COMMS_SENSOR_ACTIVITY_STILL_START:
427 case COMMS_SENSOR_ACTIVITY_STILL_STOP:
428 case COMMS_SENSOR_ACTIVITY_TILTING:
429 if (mActivityEventHandler != NULL) {
430 mActivityEventHandler->OnActivityEvent(sensor, sample->idata & 0xff,
431 timestamp);
432 }
433 break;
434 case COMMS_SENSOR_PRESSURE:
435 initEv(&nev[cnt++], timestamp, type, sensor)->pressure = sample->fdata;
436 break;
437 case COMMS_SENSOR_TEMPERATURE:
438 initEv(&nev[cnt++], timestamp, type, sensor)->temperature = sample->fdata;
439 break;
440 case COMMS_SENSOR_PROXIMITY:
441 initEv(&nev[cnt++], timestamp, type, sensor)->distance = sample->fdata;
442 break;
443 case COMMS_SENSOR_LIGHT:
444 initEv(&nev[cnt++], timestamp, type, sensor)->light = sample->fdata;
445 break;
446 case COMMS_SENSOR_STEP_COUNTER:
447 // We'll stash away the last step count in case we need to reset
448 // the hub. This last step count would then become the new offset.
449 mLastStepCount = mStepCounterOffset + sample->idata;
450 initEv(&nev[cnt++], timestamp, type, sensor)->u64.step_counter = mLastStepCount;
451 break;
452 case COMMS_SENSOR_STEP_DETECTOR:
453 case COMMS_SENSOR_SIGNIFICANT_MOTION:
454 case COMMS_SENSOR_TILT:
455 case COMMS_SENSOR_DOUBLE_TWIST:
456 case COMMS_SENSOR_WRIST_TILT:
457 initEv(&nev[cnt++], timestamp, type, sensor)->data[0] = 1.0f;
458 break;
459 case COMMS_SENSOR_GAZE:
460 case COMMS_SENSOR_UNGAZE:
461 case COMMS_SENSOR_GESTURE:
462 case COMMS_SENSOR_SYNC:
463 case COMMS_SENSOR_DOUBLE_TOUCH:
464 initEv(&nev[cnt++], timestamp, type, sensor)->data[0] = sample->idata;
465 break;
466 case COMMS_SENSOR_HALL:
467 #ifdef LID_STATE_REPORTING_ENABLED
468 sendFolioEvent(sample->idata);
469 #endif // LID_STATE_REPORTING_ENABLED
470 break;
471 case COMMS_SENSOR_WINDOW_ORIENTATION:
472 initEv(&nev[cnt++], timestamp, type, sensor)->data[0] = sample->idata;
473 break;
474 default:
475 break;
476 }
477
478 if (cnt > 0)
479 mRing.write(nev, cnt);
480 }
481
magAccuracyUpdate(float x,float y,float z)482 void HubConnection::magAccuracyUpdate(float x, float y, float z)
483 {
484 float magSq = x * x + y * y + z * z;
485
486 if (magSq < MIN_MAG_SQ || magSq > MAX_MAG_SQ) {
487 // save last good accuracy (either MEDIUM or HIGH)
488 if (mMagAccuracy != SENSOR_STATUS_UNRELIABLE)
489 mMagAccuracyRestore = mMagAccuracy;
490 mMagAccuracy = SENSOR_STATUS_UNRELIABLE;
491 } else if (mMagAccuracy == SENSOR_STATUS_UNRELIABLE) {
492 // restore
493 mMagAccuracy = mMagAccuracyRestore;
494 }
495 }
496
processSample(uint64_t timestamp,uint32_t type,uint32_t sensor,struct RawThreeAxisSample * sample,bool highAccuracy)497 void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct RawThreeAxisSample *sample, __attribute__((unused)) bool highAccuracy)
498 {
499 sensors_vec_t *sv;
500 sensors_event_t nev[2];
501 int cnt = 0;
502
503 switch (sensor) {
504 case COMMS_SENSOR_ACCEL:
505 sv = &initEv(&nev[cnt++], timestamp, type, sensor)->acceleration;
506 sv->x = sample->ix * ACCEL_RAW_KSCALE;
507 sv->y = sample->iy * ACCEL_RAW_KSCALE;
508 sv->z = sample->iz * ACCEL_RAW_KSCALE;
509 sv->status = SENSOR_STATUS_ACCURACY_HIGH;
510 break;
511 default:
512 break;
513 }
514
515 if (cnt > 0)
516 mRing.write(nev, cnt);
517 }
518
processSample(uint64_t timestamp,uint32_t type,uint32_t sensor,struct ThreeAxisSample * sample,bool highAccuracy)519 void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct ThreeAxisSample *sample, bool highAccuracy)
520 {
521 sensors_vec_t *sv;
522 uncalibrated_event_t *ue;
523 sensors_event_t *ev;
524 sensors_event_t nev[2];
525 static const float heading_accuracy = M_PI / 6.0f;
526 float w;
527 int cnt = 0;
528
529 switch (sensor) {
530 case COMMS_SENSOR_ACCEL:
531 sv = &initEv(&nev[cnt++], timestamp, type, sensor)->acceleration;
532 sv->x = sample->x;
533 sv->y = sample->y;
534 sv->z = sample->z;
535 sv->status = SENSOR_STATUS_ACCURACY_HIGH;
536 break;
537 case COMMS_SENSOR_GYRO:
538 if (mSensorState[sensor].enable) {
539 sv = &initEv(&nev[cnt++], timestamp, type, sensor)->gyro;
540 sv->x = sample->x;
541 sv->y = sample->y;
542 sv->z = sample->z;
543 sv->status = SENSOR_STATUS_ACCURACY_HIGH;
544 }
545
546 if (mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].enable) {
547 ue = &initEv(&nev[cnt++], timestamp,
548 SENSOR_TYPE_GYROSCOPE_UNCALIBRATED,
549 COMMS_SENSOR_GYRO_UNCALIBRATED)->uncalibrated_gyro;
550 ue->x_uncalib = sample->x + mGyroBias[0];
551 ue->y_uncalib = sample->y + mGyroBias[1];
552 ue->z_uncalib = sample->z + mGyroBias[2];
553 ue->x_bias = mGyroBias[0];
554 ue->y_bias = mGyroBias[1];
555 ue->z_bias = mGyroBias[2];
556 }
557 break;
558 case COMMS_SENSOR_ACCEL_BIAS:
559 mAccelBias[0] = sample->x;
560 mAccelBias[1] = sample->y;
561 mAccelBias[2] = sample->z;
562 saveSensorSettings();
563 break;
564 case COMMS_SENSOR_GYRO_BIAS:
565 mGyroBias[0] = sample->x;
566 mGyroBias[1] = sample->y;
567 mGyroBias[2] = sample->z;
568 saveSensorSettings();
569 break;
570 case COMMS_SENSOR_MAG:
571 magAccuracyUpdate(sample->x, sample->y, sample->z);
572
573 if (mSensorState[sensor].enable) {
574 sv = &initEv(&nev[cnt++], timestamp, type, sensor)->magnetic;
575 sv->x = sample->x;
576 sv->y = sample->y;
577 sv->z = sample->z;
578 sv->status = mMagAccuracy;
579 }
580
581 if (mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].enable) {
582 ue = &initEv(&nev[cnt++], timestamp,
583 SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED,
584 COMMS_SENSOR_MAG_UNCALIBRATED)->uncalibrated_magnetic;
585 ue->x_uncalib = sample->x + mMagBias[0];
586 ue->y_uncalib = sample->y + mMagBias[1];
587 ue->z_uncalib = sample->z + mMagBias[2];
588 ue->x_bias = mMagBias[0];
589 ue->y_bias = mMagBias[1];
590 ue->z_bias = mMagBias[2];
591 }
592 break;
593 case COMMS_SENSOR_MAG_BIAS:
594 mMagAccuracy = highAccuracy ? SENSOR_STATUS_ACCURACY_HIGH : SENSOR_STATUS_ACCURACY_MEDIUM;
595 mMagBias[0] = sample->x;
596 mMagBias[1] = sample->y;
597 mMagBias[2] = sample->z;
598
599 saveSensorSettings();
600 break;
601 case COMMS_SENSOR_ORIENTATION:
602 case COMMS_SENSOR_LINEAR_ACCEL:
603 case COMMS_SENSOR_GRAVITY:
604 sv = &initEv(&nev[cnt++], timestamp, type, sensor)->orientation;
605 sv->x = sample->x;
606 sv->y = sample->y;
607 sv->z = sample->z;
608 sv->status = mMagAccuracy;
609 break;
610 case COMMS_SENSOR_DOUBLE_TAP:
611 ev = initEv(&nev[cnt++], timestamp, type, sensor);
612 ev->data[0] = sample->x;
613 ev->data[1] = sample->y;
614 ev->data[2] = sample->z;
615 break;
616 case COMMS_SENSOR_ROTATION_VECTOR:
617 ev = initEv(&nev[cnt++], timestamp, type, sensor);
618 w = sample->x * sample->x + sample->y * sample->y + sample->z * sample->z;
619 if (w < 1.0f)
620 w = sqrt(1.0f - w);
621 else
622 w = 0.0f;
623 ev->data[0] = sample->x;
624 ev->data[1] = sample->y;
625 ev->data[2] = sample->z;
626 ev->data[3] = w;
627 ev->data[4] = (4 - mMagAccuracy) * heading_accuracy;
628 break;
629 case COMMS_SENSOR_GEO_MAG:
630 case COMMS_SENSOR_GAME_ROTATION_VECTOR:
631 ev = initEv(&nev[cnt++], timestamp, type, sensor);
632 w = sample->x * sample->x + sample->y * sample->y + sample->z * sample->z;
633 if (w < 1.0f)
634 w = sqrt(1.0f - w);
635 else
636 w = 0.0f;
637 ev->data[0] = sample->x;
638 ev->data[1] = sample->y;
639 ev->data[2] = sample->z;
640 ev->data[3] = w;
641 break;
642 default:
643 break;
644 }
645
646 if (cnt > 0)
647 mRing.write(nev, cnt);
648 }
649
discardInotifyEvent()650 void HubConnection::discardInotifyEvent() {
651 // Read & discard an inotify event. We only use the presence of an event as
652 // a trigger to perform the file existence check (for simplicity)
653 if (mInotifyPollIndex >= 0) {
654 char buf[sizeof(struct inotify_event) + NAME_MAX + 1];
655 int ret = ::read(mPollFds[mInotifyPollIndex].fd, buf, sizeof(buf));
656 ALOGD("Discarded %d bytes of inotify data", ret);
657 }
658 }
659
waitOnNanohubLock()660 void HubConnection::waitOnNanohubLock() {
661 if (mInotifyPollIndex < 0) {
662 return;
663 }
664 struct pollfd *pfd = &mPollFds[mInotifyPollIndex];
665
666 // While the lock file exists, poll on the inotify fd (with timeout)
667 while (access(NANOHUB_LOCK_FILE, F_OK) == 0) {
668 ALOGW("Nanohub is locked; blocking read thread");
669 int ret = poll(pfd, 1, 5000);
670 if ((ret > 0) && (pfd->revents & POLLIN)) {
671 discardInotifyEvent();
672 }
673 }
674 }
675
restoreSensorState()676 void HubConnection::restoreSensorState()
677 {
678 Mutex::Autolock autoLock(mLock);
679
680 sendCalibrationOffsets();
681
682 for (int i = 0; i < NUM_COMMS_SENSORS_PLUS_1; i++) {
683 if (mSensorState[i].sensorType && mSensorState[i].enable) {
684 struct ConfigCmd cmd;
685
686 initConfigCmd(&cmd, i);
687
688 ALOGI("restoring: sensor=%d, handle=%d, enable=%d, period=%" PRId64 ", latency=%" PRId64,
689 cmd.sensorType, i, mSensorState[i].enable, frequency_q10_to_period_ns(mSensorState[i].rate),
690 mSensorState[i].latency);
691
692 int ret = TEMP_FAILURE_RETRY(write(mFd, &cmd, sizeof(cmd)));
693 if (ret != sizeof(cmd)) {
694 ALOGE("failed to send config command to restore sensor %d\n", cmd.sensorType);
695 }
696
697 cmd.cmd = CONFIG_CMD_FLUSH;
698
699 for (int j = 0; j < mSensorState[i].flushCnt; j++) {
700 int ret = TEMP_FAILURE_RETRY(write(mFd, &cmd, sizeof(cmd)));
701 if (ret != sizeof(cmd)) {
702 ALOGE("failed to send flush command to sensor %d\n", cmd.sensorType);
703 }
704 }
705 }
706 }
707
708 mStepCounterOffset = mLastStepCount;
709
710 if (mActivityEventHandler != NULL) {
711 mActivityEventHandler->OnSensorHubReset();
712 }
713 }
714
postOsLog(uint8_t * buf,ssize_t len)715 void HubConnection::postOsLog(uint8_t *buf, ssize_t len)
716 {
717 // if len is less than 6, it's either an invalid or an empty log message.
718 if (len < 6)
719 return;
720
721 buf[len] = 0x00;
722 switch (buf[4]) {
723 case 'E':
724 ALOGE("osLog: %s", &buf[5]);
725 break;
726 case 'W':
727 ALOGW("osLog: %s", &buf[5]);
728 break;
729 case 'I':
730 ALOGI("osLog: %s", &buf[5]);
731 break;
732 case 'D':
733 ALOGD("osLog: %s", &buf[5]);
734 break;
735 default:
736 break;
737 }
738 }
739
processBuf(uint8_t * buf,size_t len)740 ssize_t HubConnection::processBuf(uint8_t *buf, size_t len)
741 {
742 struct nAxisEvent *data = (struct nAxisEvent *)buf;
743 uint32_t type, sensor, bias, currSensor;
744 int i, numSamples;
745 bool one, rawThree, three;
746 sensors_event_t ev;
747 uint64_t timestamp;
748 ssize_t ret = 0;
749
750 if (len >= sizeof(data->evtType)) {
751 ret = sizeof(data->evtType);
752 one = three = rawThree = false;
753 bias = 0;
754 switch (data->evtType) {
755 case OS_LOG_EVENT:
756 postOsLog(buf, len);
757 return 0;
758 case SENS_TYPE_TO_EVENT(SENS_TYPE_ACCEL):
759 type = SENSOR_TYPE_ACCELEROMETER;
760 sensor = COMMS_SENSOR_ACCEL;
761 bias = COMMS_SENSOR_ACCEL_BIAS;
762 three = true;
763 break;
764 case SENS_TYPE_TO_EVENT(SENS_TYPE_ACCEL_RAW):
765 type = SENSOR_TYPE_ACCELEROMETER;
766 sensor = COMMS_SENSOR_ACCEL;
767 rawThree = true;
768 break;
769 case SENS_TYPE_TO_EVENT(SENS_TYPE_GYRO):
770 type = SENSOR_TYPE_GYROSCOPE;
771 sensor = COMMS_SENSOR_GYRO;
772 bias = COMMS_SENSOR_GYRO_BIAS;
773 three = true;
774 break;
775 case SENS_TYPE_TO_EVENT(SENS_TYPE_MAG):
776 type = SENSOR_TYPE_MAGNETIC_FIELD;
777 sensor = COMMS_SENSOR_MAG;
778 bias = COMMS_SENSOR_MAG_BIAS;
779 three = true;
780 break;
781 case SENS_TYPE_TO_EVENT(SENS_TYPE_ALS):
782 type = SENSOR_TYPE_LIGHT;
783 sensor = COMMS_SENSOR_LIGHT;
784 one = true;
785 break;
786 case SENS_TYPE_TO_EVENT(SENS_TYPE_PROX):
787 type = SENSOR_TYPE_PROXIMITY;
788 sensor = COMMS_SENSOR_PROXIMITY;
789 one = true;
790 break;
791 case SENS_TYPE_TO_EVENT(SENS_TYPE_BARO):
792 type = SENSOR_TYPE_PRESSURE;
793 sensor = COMMS_SENSOR_PRESSURE;
794 one = true;
795 break;
796 case SENS_TYPE_TO_EVENT(SENS_TYPE_TEMP):
797 // nanohub only has one temperature sensor type, which is mapped to
798 // internal temp because we currently don't have ambient temp
799 type = SENSOR_TYPE_INTERNAL_TEMPERATURE;
800 sensor = COMMS_SENSOR_TEMPERATURE;
801 one = true;
802 break;
803 case SENS_TYPE_TO_EVENT(SENS_TYPE_ORIENTATION):
804 type = SENSOR_TYPE_ORIENTATION;
805 sensor = COMMS_SENSOR_ORIENTATION;
806 three = true;
807 break;
808 case SENS_TYPE_TO_EVENT(SENS_TYPE_WIN_ORIENTATION):
809 type = SENSOR_TYPE_DEVICE_ORIENTATION;
810 sensor = COMMS_SENSOR_WINDOW_ORIENTATION;
811 one = true;
812 break;
813 case SENS_TYPE_TO_EVENT(SENS_TYPE_STEP_DETECT):
814 type = SENSOR_TYPE_STEP_DETECTOR;
815 sensor = COMMS_SENSOR_STEP_DETECTOR;
816 one = true;
817 break;
818 case SENS_TYPE_TO_EVENT(SENS_TYPE_STEP_COUNT):
819 type = SENSOR_TYPE_STEP_COUNTER;
820 sensor = COMMS_SENSOR_STEP_COUNTER;
821 one = true;
822 break;
823 case SENS_TYPE_TO_EVENT(SENS_TYPE_SIG_MOTION):
824 type = SENSOR_TYPE_SIGNIFICANT_MOTION;
825 sensor = COMMS_SENSOR_SIGNIFICANT_MOTION;
826 one = true;
827 break;
828 case SENS_TYPE_TO_EVENT(SENS_TYPE_GRAVITY):
829 type = SENSOR_TYPE_GRAVITY;
830 sensor = COMMS_SENSOR_GRAVITY;
831 three = true;
832 break;
833 case SENS_TYPE_TO_EVENT(SENS_TYPE_LINEAR_ACCEL):
834 type = SENSOR_TYPE_LINEAR_ACCELERATION;
835 sensor = COMMS_SENSOR_LINEAR_ACCEL;
836 three = true;
837 break;
838 case SENS_TYPE_TO_EVENT(SENS_TYPE_ROTATION_VECTOR):
839 type = SENSOR_TYPE_ROTATION_VECTOR;
840 sensor = COMMS_SENSOR_ROTATION_VECTOR;
841 three = true;
842 break;
843 case SENS_TYPE_TO_EVENT(SENS_TYPE_GEO_MAG_ROT_VEC):
844 type = SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR;
845 sensor = COMMS_SENSOR_GEO_MAG;
846 three = true;
847 break;
848 case SENS_TYPE_TO_EVENT(SENS_TYPE_GAME_ROT_VECTOR):
849 type = SENSOR_TYPE_GAME_ROTATION_VECTOR;
850 sensor = COMMS_SENSOR_GAME_ROTATION_VECTOR;
851 three = true;
852 break;
853 case SENS_TYPE_TO_EVENT(SENS_TYPE_HALL):
854 type = 0;
855 sensor = COMMS_SENSOR_HALL;
856 one = true;
857 break;
858 case SENS_TYPE_TO_EVENT(SENS_TYPE_VSYNC):
859 type = SENSOR_TYPE_SYNC;
860 sensor = COMMS_SENSOR_SYNC;
861 one = true;
862 break;
863 case SENS_TYPE_TO_EVENT(SENS_TYPE_TILT):
864 type = SENSOR_TYPE_TILT_DETECTOR;
865 sensor = COMMS_SENSOR_TILT;
866 one = true;
867 break;
868 case SENS_TYPE_TO_EVENT(SENS_TYPE_GESTURE):
869 type = SENSOR_TYPE_PICK_UP_GESTURE;
870 sensor = COMMS_SENSOR_GESTURE;
871 one = true;
872 break;
873 case SENS_TYPE_TO_EVENT(SENS_TYPE_DOUBLE_TWIST):
874 type = SENSOR_TYPE_DOUBLE_TWIST;
875 sensor = COMMS_SENSOR_DOUBLE_TWIST;
876 one = true;
877 break;
878 case SENS_TYPE_TO_EVENT(SENS_TYPE_DOUBLE_TAP):
879 type = SENSOR_TYPE_DOUBLE_TAP;
880 sensor = COMMS_SENSOR_DOUBLE_TAP;
881 three = true;
882 break;
883 case SENS_TYPE_TO_EVENT(SENS_TYPE_WRIST_TILT):
884 type = SENSOR_TYPE_WRIST_TILT_GESTURE;
885 sensor = COMMS_SENSOR_WRIST_TILT;
886 one = true;
887 break;
888 case SENS_TYPE_TO_EVENT(SENS_TYPE_DOUBLE_TOUCH):
889 type = SENSOR_TYPE_DOUBLE_TOUCH;
890 sensor = COMMS_SENSOR_DOUBLE_TOUCH;
891 one = true;
892 break;
893 case SENS_TYPE_TO_EVENT(SENS_TYPE_ACTIVITY_IN_VEHICLE_START):
894 type = 0;
895 sensor = COMMS_SENSOR_ACTIVITY_IN_VEHICLE_START;
896 one = true;
897 break;
898 case SENS_TYPE_TO_EVENT(SENS_TYPE_ACTIVITY_IN_VEHICLE_STOP):
899 type = 0;
900 sensor = COMMS_SENSOR_ACTIVITY_IN_VEHICLE_STOP;
901 one = true;
902 break;
903 case SENS_TYPE_TO_EVENT(SENS_TYPE_ACTIVITY_ON_BICYCLE_START):
904 type = 0;
905 sensor = COMMS_SENSOR_ACTIVITY_ON_BICYCLE_START;
906 one = true;
907 break;
908 case SENS_TYPE_TO_EVENT(SENS_TYPE_ACTIVITY_ON_BICYCLE_STOP):
909 type = 0;
910 sensor = COMMS_SENSOR_ACTIVITY_ON_BICYCLE_STOP;
911 one = true;
912 break;
913 case SENS_TYPE_TO_EVENT(SENS_TYPE_ACTIVITY_WALKING_START):
914 type = 0;
915 sensor = COMMS_SENSOR_ACTIVITY_WALKING_START;
916 one = true;
917 break;
918 case SENS_TYPE_TO_EVENT(SENS_TYPE_ACTIVITY_WALKING_STOP):
919 type = 0;
920 sensor = COMMS_SENSOR_ACTIVITY_WALKING_STOP;
921 one = true;
922 break;
923 case SENS_TYPE_TO_EVENT(SENS_TYPE_ACTIVITY_RUNNING_START):
924 type = 0;
925 sensor = COMMS_SENSOR_ACTIVITY_RUNNING_START;
926 one = true;
927 break;
928 case SENS_TYPE_TO_EVENT(SENS_TYPE_ACTIVITY_RUNNING_STOP):
929 type = 0;
930 sensor = COMMS_SENSOR_ACTIVITY_RUNNING_STOP;
931 one = true;
932 break;
933 case SENS_TYPE_TO_EVENT(SENS_TYPE_ACTIVITY_STILL_START):
934 type = 0;
935 sensor = COMMS_SENSOR_ACTIVITY_STILL_START;
936 one = true;
937 break;
938 case SENS_TYPE_TO_EVENT(SENS_TYPE_ACTIVITY_STILL_STOP):
939 type = 0;
940 sensor = COMMS_SENSOR_ACTIVITY_STILL_STOP;
941 one = true;
942 break;
943 case SENS_TYPE_TO_EVENT(SENS_TYPE_ACTIVITY_TILTING):
944 type = 0;
945 sensor = COMMS_SENSOR_ACTIVITY_TILTING;
946 one = true;
947 break;
948 case SENS_TYPE_TO_EVENT(SENS_TYPE_GAZE):
949 type = SENSOR_TYPE_GAZE;
950 sensor = COMMS_SENSOR_GAZE;
951 one = true;
952 break;
953 case SENS_TYPE_TO_EVENT(SENS_TYPE_UNGAZE):
954 type = SENSOR_TYPE_UNGAZE;
955 sensor = COMMS_SENSOR_UNGAZE;
956 one = true;
957 break;
958 case EVT_RESET_REASON:
959 uint32_t resetReason;
960 memcpy(&resetReason, data->buffer, sizeof(resetReason));
961 ALOGI("Observed hub reset: 0x%08" PRIx32, resetReason);
962 restoreSensorState();
963 return 0;
964 default:
965 ALOGE("unknown evtType: 0x%08x\n", data->evtType);
966 return -1;
967 }
968 } else {
969 ALOGE("too little data: len=%zu\n", len);
970 return -1;
971 }
972
973 if (len >= sizeof(data->evtType) + sizeof(data->referenceTime) + sizeof(data->firstSample)) {
974 ret += sizeof(data->referenceTime);
975 timestamp = data->referenceTime;
976 numSamples = data->firstSample.numSamples;
977 for (i=0; i<numSamples; i++) {
978 if (data->firstSample.biasPresent && data->firstSample.biasSample == i)
979 currSensor = bias;
980 else
981 currSensor = sensor;
982
983 if (one) {
984 if (ret + sizeof(data->oneSamples[i]) > len) {
985 ALOGE("sensor %d (one): ret=%zd, numSamples=%d, i=%d\n", currSensor, ret, numSamples, i);
986 return -1;
987 }
988 if (i > 0)
989 timestamp += ((uint64_t)data->oneSamples[i].deltaTime) << delta_time_shift_table[data->oneSamples[i].deltaTime & delta_time_encoded];
990 processSample(timestamp, type, currSensor, &data->oneSamples[i], data->firstSample.highAccuracy);
991 ret += sizeof(data->oneSamples[i]);
992 } else if (rawThree) {
993 if (ret + sizeof(data->rawThreeSamples[i]) > len) {
994 ALOGE("sensor %d (rawThree): ret=%zd, numSamples=%d, i=%d\n", currSensor, ret, numSamples, i);
995 return -1;
996 }
997 if (i > 0)
998 timestamp += ((uint64_t)data->rawThreeSamples[i].deltaTime) << delta_time_shift_table[data->rawThreeSamples[i].deltaTime & delta_time_encoded];
999 processSample(timestamp, type, currSensor, &data->rawThreeSamples[i], data->firstSample.highAccuracy);
1000 ret += sizeof(data->rawThreeSamples[i]);
1001 } else if (three) {
1002 if (ret + sizeof(data->threeSamples[i]) > len) {
1003 ALOGE("sensor %d (three): ret=%zd, numSamples=%d, i=%d\n", currSensor, ret, numSamples, i);
1004 return -1;
1005 }
1006 if (i > 0)
1007 timestamp += ((uint64_t)data->threeSamples[i].deltaTime) << delta_time_shift_table[data->threeSamples[i].deltaTime & delta_time_encoded];
1008 processSample(timestamp, type, currSensor, &data->threeSamples[i], data->firstSample.highAccuracy);
1009 ret += sizeof(data->threeSamples[i]);
1010 } else {
1011 ALOGE("sensor %d (unknown): cannot processSample\n", currSensor);
1012 return -1;
1013 }
1014 }
1015
1016 if (!numSamples)
1017 ret += sizeof(data->firstSample);
1018
1019 for (i=0; i<data->firstSample.numFlushes; i++) {
1020 if (isActivitySensor(sensor) && mActivityEventHandler != NULL) {
1021 mActivityEventHandler->OnFlush();
1022 } else {
1023 memset(&ev, 0x00, sizeof(sensors_event_t));
1024 ev.version = META_DATA_VERSION;
1025 ev.timestamp = 0;
1026 ev.type = SENSOR_TYPE_META_DATA;
1027 ev.sensor = 0;
1028 ev.meta_data.what = META_DATA_FLUSH_COMPLETE;
1029 if (mSensorState[sensor].alt && mSensorState[mSensorState[sensor].alt].flushCnt > 0) {
1030 mSensorState[mSensorState[sensor].alt].flushCnt --;
1031 ev.meta_data.sensor = mSensorState[sensor].alt;
1032 } else {
1033 mSensorState[sensor].flushCnt --;
1034 ev.meta_data.sensor = sensor;
1035 }
1036
1037 mRing.write(&ev, 1);
1038 ALOGI("flushing %d", ev.meta_data.sensor);
1039 }
1040 }
1041 } else {
1042 ALOGE("too little data for sensor %d: len=%zu\n", sensor, len);
1043 return -1;
1044 }
1045
1046 return ret;
1047 }
1048
sendCalibrationOffsets()1049 void HubConnection::sendCalibrationOffsets()
1050 {
1051 sp<JSONObject> settings;
1052 sp<JSONObject> saved_settings;
1053 struct {
1054 int32_t hw[3];
1055 float sw[3];
1056 } gyro, accel;
1057 int32_t proximity, proximity_array[4];
1058 float barometer, mag[3], light;
1059 bool gyro_hw_cal_exists, gyro_sw_cal_exists;
1060 bool accel_hw_cal_exists, accel_sw_cal_exists;
1061
1062 loadSensorSettings(&settings, &saved_settings);
1063
1064 accel_hw_cal_exists = getCalibrationInt32(settings, "accel", accel.hw, 3);
1065 accel_sw_cal_exists = getCalibrationFloat(saved_settings, "accel_sw", accel.sw);
1066 if (accel_hw_cal_exists || accel_sw_cal_exists) {
1067 // Store SW bias so we can remove bias for uncal data
1068 mAccelBias[0] = accel.sw[0];
1069 mAccelBias[1] = accel.sw[1];
1070 mAccelBias[2] = accel.sw[2];
1071
1072 queueDataInternal(COMMS_SENSOR_ACCEL, &accel, sizeof(accel));
1073 }
1074
1075 gyro_hw_cal_exists = getCalibrationInt32(settings, "gyro", gyro.hw, 3);
1076 gyro_sw_cal_exists = getCalibrationFloat(saved_settings, "gyro_sw", gyro.sw);
1077 if (gyro_hw_cal_exists || gyro_sw_cal_exists) {
1078 // Store SW bias so we can remove bias for uncal data
1079 mGyroBias[0] = gyro.sw[0];
1080 mGyroBias[1] = gyro.sw[1];
1081 mGyroBias[2] = gyro.sw[2];
1082
1083 queueDataInternal(COMMS_SENSOR_GYRO, &gyro, sizeof(gyro));
1084 }
1085
1086 if (settings->getFloat("barometer", &barometer))
1087 queueDataInternal(COMMS_SENSOR_PRESSURE, &barometer, sizeof(barometer));
1088
1089 if (settings->getInt32("proximity", &proximity))
1090 queueDataInternal(COMMS_SENSOR_PROXIMITY, &proximity, sizeof(proximity));
1091
1092 if (getCalibrationInt32(settings, "proximity", proximity_array, 4))
1093 queueDataInternal(COMMS_SENSOR_PROXIMITY, proximity_array, sizeof(proximity_array));
1094
1095 if (settings->getFloat("light", &light))
1096 queueDataInternal(COMMS_SENSOR_LIGHT, &light, sizeof(light));
1097
1098 if (getCalibrationFloat(saved_settings, "mag", mag)) {
1099 // Store SW bias so we can remove bias for uncal data
1100 mMagBias[0] = mag[0];
1101 mMagBias[1] = mag[1];
1102 mMagBias[2] = mag[2];
1103
1104 queueDataInternal(COMMS_SENSOR_MAG, mag, sizeof(mag));
1105 }
1106 }
1107
threadLoop()1108 bool HubConnection::threadLoop() {
1109 ALOGI("threadLoop: starting");
1110
1111 if (mFd < 0) {
1112 ALOGE("threadLoop: exiting prematurely: nanohub is unavailable");
1113 return false;
1114 }
1115 waitOnNanohubLock();
1116
1117 sendCalibrationOffsets();
1118
1119 while (!Thread::exitPending()) {
1120 ssize_t ret;
1121
1122 do {
1123 ret = poll(mPollFds, mNumPollFds, -1);
1124 } while (ret < 0 && errno == EINTR);
1125
1126 if (mInotifyPollIndex >= 0 && mPollFds[mInotifyPollIndex].revents & POLLIN) {
1127 discardInotifyEvent();
1128 waitOnNanohubLock();
1129 }
1130
1131 #ifdef USB_MAG_BIAS_REPORTING_ENABLED
1132 if (mMagBiasPollIndex >= 0 && mPollFds[mMagBiasPollIndex].revents & POLLERR) {
1133 // Read from mag bias file
1134 char buf[16];
1135 lseek(mPollFds[mMagBiasPollIndex].fd, 0, SEEK_SET);
1136 ::read(mPollFds[mMagBiasPollIndex].fd, buf, 16);
1137 float bias = atof(buf);
1138 mUsbMagBias = bias;
1139 queueUsbMagBias();
1140 }
1141 #endif // USB_MAG_BIAS_REPORTING_ENABLED
1142
1143 #ifdef DOUBLE_TOUCH_ENABLED
1144 if (mDoubleTouchPollIndex >= 0 && mPollFds[mDoubleTouchPollIndex].revents & POLLERR) {
1145 // Read from double touch file
1146 char buf[16];
1147 lseek(mPollFds[mDoubleTouchPollIndex].fd, 0, SEEK_SET);
1148 ::read(mPollFds[mDoubleTouchPollIndex].fd, buf, 16);
1149 sensors_event_t gestureEvent;
1150 initEv(&gestureEvent, elapsedRealtimeNano(), SENSOR_TYPE_PICK_UP_GESTURE, COMMS_SENSOR_GESTURE)->data[0] = 8;
1151 mRing.write(&gestureEvent, 1);
1152 }
1153 #endif // DOUBLE_TOUCH_ENABLED
1154
1155 if (mPollFds[0].revents & POLLIN) {
1156 uint8_t recv[256];
1157 ssize_t len = ::read(mFd, recv, sizeof(recv));
1158
1159 if (len >= 0) {
1160 for (ssize_t offset = 0; offset < len;) {
1161 ret = processBuf(recv + offset, len - offset);
1162
1163 if (ret > 0)
1164 offset += ret;
1165 else
1166 break;
1167 }
1168 } else {
1169 ALOGE("read -1: errno=%d\n", errno);
1170 }
1171 }
1172 }
1173
1174 return false;
1175 }
1176
read(sensors_event_t * ev,size_t size)1177 ssize_t HubConnection::read(sensors_event_t *ev, size_t size) {
1178 return mRing.read(ev, size);
1179 }
1180
setActivityCallback(ActivityEventHandler * eventHandler)1181 void HubConnection::setActivityCallback(ActivityEventHandler *eventHandler)
1182 {
1183 Mutex::Autolock autoLock(mLock);
1184 mActivityEventHandler = eventHandler;
1185 }
1186
initConfigCmd(struct ConfigCmd * cmd,int handle)1187 void HubConnection::initConfigCmd(struct ConfigCmd *cmd, int handle)
1188 {
1189 uint8_t alt = mSensorState[handle].alt;
1190
1191 memset(cmd, 0x00, sizeof(*cmd));
1192
1193 cmd->evtType = EVT_NO_SENSOR_CONFIG_EVENT;
1194 cmd->sensorType = mSensorState[handle].sensorType;
1195
1196 if (alt && mSensorState[alt].enable && mSensorState[handle].enable) {
1197 cmd->cmd = CONFIG_CMD_ENABLE;
1198 if (mSensorState[alt].rate > mSensorState[handle].rate)
1199 cmd->rate = mSensorState[alt].rate;
1200 else
1201 cmd->rate = mSensorState[handle].rate;
1202 if (mSensorState[alt].latency < mSensorState[handle].latency)
1203 cmd->latency = mSensorState[alt].latency;
1204 else
1205 cmd->latency = mSensorState[handle].latency;
1206 } else if (alt && mSensorState[alt].enable) {
1207 cmd->cmd = mSensorState[alt].enable ? CONFIG_CMD_ENABLE : CONFIG_CMD_DISABLE;
1208 cmd->rate = mSensorState[alt].rate;
1209 cmd->latency = mSensorState[alt].latency;
1210 } else { /* !alt || !mSensorState[alt].enable */
1211 cmd->cmd = mSensorState[handle].enable ? CONFIG_CMD_ENABLE : CONFIG_CMD_DISABLE;
1212 cmd->rate = mSensorState[handle].rate;
1213 cmd->latency = mSensorState[handle].latency;
1214 }
1215 }
1216
queueActivate(int handle,bool enable)1217 void HubConnection::queueActivate(int handle, bool enable)
1218 {
1219 struct ConfigCmd cmd;
1220 int ret;
1221
1222 Mutex::Autolock autoLock(mLock);
1223
1224 if (mSensorState[handle].sensorType) {
1225 mSensorState[handle].enable = enable;
1226
1227 initConfigCmd(&cmd, handle);
1228
1229 ret = TEMP_FAILURE_RETRY(write(mFd, &cmd, sizeof(cmd)));
1230 if (ret == sizeof(cmd))
1231 ALOGI("queueActivate: sensor=%d, handle=%d, enable=%d",
1232 cmd.sensorType, handle, enable);
1233 else
1234 ALOGE("queueActivate: failed to send command: sensor=%d, handle=%d, enable=%d",
1235 cmd.sensorType, handle, enable);
1236 } else {
1237 ALOGI("queueActivate: unhandled handle=%d, enable=%d", handle, enable);
1238 }
1239 }
1240
queueSetDelay(int handle,nsecs_t sampling_period_ns)1241 void HubConnection::queueSetDelay(int handle, nsecs_t sampling_period_ns)
1242 {
1243 struct ConfigCmd cmd;
1244 int ret;
1245
1246 Mutex::Autolock autoLock(mLock);
1247
1248 if (mSensorState[handle].sensorType) {
1249 if (sampling_period_ns > 0 &&
1250 mSensorState[handle].rate != SENSOR_RATE_ONCHANGE &&
1251 mSensorState[handle].rate != SENSOR_RATE_ONESHOT) {
1252 mSensorState[handle].rate = period_ns_to_frequency_q10(sampling_period_ns);
1253 }
1254
1255 initConfigCmd(&cmd, handle);
1256
1257 ret = TEMP_FAILURE_RETRY(write(mFd, &cmd, sizeof(cmd)));
1258 if (ret == sizeof(cmd))
1259 ALOGI("queueSetDelay: sensor=%d, handle=%d, period=%" PRId64,
1260 cmd.sensorType, handle, sampling_period_ns);
1261 else
1262 ALOGE("queueSetDelay: failed to send command: sensor=%d, handle=%d, period=%" PRId64,
1263 cmd.sensorType, handle, sampling_period_ns);
1264 } else {
1265 ALOGI("queueSetDelay: unhandled handle=%d, period=%" PRId64, handle, sampling_period_ns);
1266 }
1267 }
1268
queueBatch(int handle,nsecs_t sampling_period_ns,nsecs_t max_report_latency_ns)1269 void HubConnection::queueBatch(
1270 int handle,
1271 nsecs_t sampling_period_ns,
1272 nsecs_t max_report_latency_ns)
1273 {
1274 struct ConfigCmd cmd;
1275 int ret;
1276
1277 Mutex::Autolock autoLock(mLock);
1278
1279 if (mSensorState[handle].sensorType) {
1280 if (sampling_period_ns > 0 &&
1281 mSensorState[handle].rate != SENSOR_RATE_ONCHANGE &&
1282 mSensorState[handle].rate != SENSOR_RATE_ONESHOT) {
1283 mSensorState[handle].rate = period_ns_to_frequency_q10(sampling_period_ns);
1284 }
1285 mSensorState[handle].latency = max_report_latency_ns;
1286
1287 initConfigCmd(&cmd, handle);
1288
1289 ret = TEMP_FAILURE_RETRY(write(mFd, &cmd, sizeof(cmd)));
1290 if (ret == sizeof(cmd))
1291 ALOGI("queueBatch: sensor=%d, handle=%d, period=%" PRId64 ", latency=%" PRId64,
1292 cmd.sensorType, handle, sampling_period_ns, max_report_latency_ns);
1293 else
1294 ALOGE("queueBatch: failed to send command: sensor=%d, handle=%d, period=%" PRId64 ", latency=%" PRId64,
1295 cmd.sensorType, handle, sampling_period_ns, max_report_latency_ns);
1296 } else {
1297 ALOGI("queueBatch: unhandled handle=%d, period=%" PRId64 ", latency=%" PRId64,
1298 handle, sampling_period_ns, max_report_latency_ns);
1299 }
1300 }
1301
queueFlush(int handle)1302 void HubConnection::queueFlush(int handle)
1303 {
1304 struct ConfigCmd cmd;
1305 int ret;
1306
1307 Mutex::Autolock autoLock(mLock);
1308
1309 if (mSensorState[handle].sensorType) {
1310 mSensorState[handle].flushCnt++;
1311
1312 initConfigCmd(&cmd, handle);
1313 cmd.cmd = CONFIG_CMD_FLUSH;
1314
1315 ret = TEMP_FAILURE_RETRY(write(mFd, &cmd, sizeof(cmd)));
1316 if (ret == sizeof(cmd)) {
1317 ALOGI("queueFlush: sensor=%d, handle=%d",
1318 cmd.sensorType, handle);
1319 } else {
1320 ALOGE("queueFlush: failed to send command: sensor=%d, handle=%d"
1321 " with error %s", cmd.sensorType, handle, strerror(errno));
1322 }
1323 } else {
1324 ALOGI("queueFlush: unhandled handle=%d", handle);
1325 }
1326 }
1327
queueDataInternal(int handle,void * data,size_t length)1328 void HubConnection::queueDataInternal(int handle, void *data, size_t length)
1329 {
1330 struct ConfigCmd *cmd = (struct ConfigCmd *)malloc(sizeof(struct ConfigCmd) + length);
1331 size_t ret;
1332
1333 if (cmd && mSensorState[handle].sensorType) {
1334 initConfigCmd(cmd, handle);
1335 memcpy(cmd->data, data, length);
1336 cmd->cmd = CONFIG_CMD_CFG_DATA;
1337
1338 ret = TEMP_FAILURE_RETRY(write(mFd, cmd, sizeof(*cmd) + length));
1339 if (ret == sizeof(*cmd) + length)
1340 ALOGI("queueData: sensor=%d, length=%zu",
1341 cmd->sensorType, length);
1342 else
1343 ALOGE("queueData: failed to send command: sensor=%d, length=%zu",
1344 cmd->sensorType, length);
1345 free(cmd);
1346 } else {
1347 ALOGI("queueData: unhandled handle=%d", handle);
1348 }
1349 }
1350
queueData(int handle,void * data,size_t length)1351 void HubConnection::queueData(int handle, void *data, size_t length)
1352 {
1353 Mutex::Autolock autoLock(mLock);
1354 queueDataInternal(handle, data, length);
1355 }
1356
initNanohubLock()1357 void HubConnection::initNanohubLock() {
1358 // Create the lock directory (if it doesn't already exist)
1359 if (mkdir(NANOHUB_LOCK_DIR, NANOHUB_LOCK_DIR_PERMS) < 0 && errno != EEXIST) {
1360 ALOGE("Couldn't create Nanohub lock directory: %s", strerror(errno));
1361 return;
1362 }
1363
1364 mInotifyPollIndex = -1;
1365 int inotifyFd = inotify_init1(IN_NONBLOCK);
1366 if (inotifyFd < 0) {
1367 ALOGE("Couldn't initialize inotify: %s", strerror(errno));
1368 } else if (inotify_add_watch(inotifyFd, NANOHUB_LOCK_DIR, IN_CREATE | IN_DELETE) < 0) {
1369 ALOGE("Couldn't add inotify watch: %s", strerror(errno));
1370 close(inotifyFd);
1371 } else {
1372 mPollFds[mNumPollFds].fd = inotifyFd;
1373 mPollFds[mNumPollFds].events = POLLIN;
1374 mPollFds[mNumPollFds].revents = 0;
1375 mInotifyPollIndex = mNumPollFds;
1376 mNumPollFds++;
1377 }
1378 }
1379
1380 #ifdef USB_MAG_BIAS_REPORTING_ENABLED
queueUsbMagBias()1381 void HubConnection::queueUsbMagBias()
1382 {
1383 struct MsgCmd *cmd = (struct MsgCmd *)malloc(sizeof(struct MsgCmd) + sizeof(float));
1384 size_t ret;
1385
1386 if (cmd) {
1387 cmd->evtType = EVT_APP_FROM_HOST;
1388 cmd->msg.appId = APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, APP_ID_APP_BMI160);
1389 cmd->msg.dataLen = sizeof(float);
1390 memcpy((float *)(cmd+1), &mUsbMagBias, sizeof(float));
1391
1392 ret = TEMP_FAILURE_RETRY(write(mFd, cmd, sizeof(*cmd) + sizeof(float)));
1393 if (ret == sizeof(*cmd) + sizeof(float))
1394 ALOGI("queueUsbMagBias: bias=%f\n", mUsbMagBias);
1395 else
1396 ALOGE("queueUsbMagBias: failed to send command: bias=%f\n", mUsbMagBias);
1397 free(cmd);
1398 }
1399 }
1400 #endif // USB_MAG_BIAS_REPORTING_ENABLED
1401
1402 #ifdef LID_STATE_REPORTING_ENABLED
initializeUinputNode()1403 status_t HubConnection::initializeUinputNode()
1404 {
1405 int ret = 0;
1406
1407 // Open uinput dev node
1408 mUinputFd = TEMP_FAILURE_RETRY(open("/dev/uinput", O_WRONLY | O_NONBLOCK));
1409 if (mUinputFd < 0) {
1410 ALOGE("could not open uinput node: %s", strerror(errno));
1411 return UNKNOWN_ERROR;
1412 }
1413
1414 // Enable SW_LID events
1415 ret = TEMP_FAILURE_RETRY(ioctl(mUinputFd, UI_SET_EVBIT, EV_SW));
1416 ret |= TEMP_FAILURE_RETRY(ioctl(mUinputFd, UI_SET_EVBIT, EV_SYN));
1417 ret |= TEMP_FAILURE_RETRY(ioctl(mUinputFd, UI_SET_SWBIT, SW_LID));
1418 if (ret < 0) {
1419 ALOGE("could not send ioctl to uinput node: %s", strerror(errno));
1420 return UNKNOWN_ERROR;
1421 }
1422
1423 // Create uinput node for SW_LID
1424 struct uinput_user_dev uidev;
1425 memset(&uidev, 0, sizeof(uidev));
1426 snprintf(uidev.name, UINPUT_MAX_NAME_SIZE, "uinput-folio");
1427 uidev.id.bustype = BUS_SPI;
1428 uidev.id.vendor = 0;
1429 uidev.id.product = 0;
1430 uidev.id.version = 0;
1431
1432 ret = TEMP_FAILURE_RETRY(write(mUinputFd, &uidev, sizeof(uidev)));
1433 if (ret < 0) {
1434 ALOGE("write to uinput node failed: %s", strerror(errno));
1435 return UNKNOWN_ERROR;
1436 }
1437
1438 ret = TEMP_FAILURE_RETRY(ioctl(mUinputFd, UI_DEV_CREATE));
1439 if (ret < 0) {
1440 ALOGE("could not send ioctl to uinput node: %s", strerror(errno));
1441 return UNKNOWN_ERROR;
1442 }
1443
1444 return OK;
1445 }
1446
sendFolioEvent(int32_t data)1447 void HubConnection::sendFolioEvent(int32_t data) {
1448 ssize_t ret = 0;
1449 struct input_event ev;
1450
1451 memset(&ev, 0, sizeof(ev));
1452
1453 ev.type = EV_SW;
1454 ev.code = SW_LID;
1455 ev.value = data;
1456 ret = TEMP_FAILURE_RETRY(write(mUinputFd, &ev, sizeof(ev)));
1457 if (ret < 0) {
1458 ALOGE("write to uinput node failed: %s", strerror(errno));
1459 return;
1460 }
1461
1462 // Force flush with EV_SYN event
1463 ev.type = EV_SYN;
1464 ev.code = SYN_REPORT;
1465 ev.value = 0;
1466 ret = TEMP_FAILURE_RETRY(write(mUinputFd, &ev, sizeof(ev)));
1467 if (ret < 0) {
1468 ALOGE("write to uinput node failed: %s", strerror(errno));
1469 return;
1470 }
1471
1472 // Set lid state property
1473 if (property_set(LID_STATE_PROPERTY,
1474 (data ? LID_STATE_CLOSED : LID_STATE_OPEN)) < 0) {
1475 ALOGE("could not set lid_state property");
1476 }
1477 }
1478 #endif // LID_STATE_REPORTING_ENABLED
1479
1480 } // namespace android
1481