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