1 /*
2 * Copyright (C) 2008-2014 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 <ctype.h>
18 #include <dirent.h>
19 #include <errno.h>
20 #include <fcntl.h>
21 #include <inttypes.h>
22 #include <math.h>
23 #include <poll.h>
24 #include <pthread.h>
25 #include <stdlib.h>
26 #include <sys/select.h>
27 #include <unistd.h>
28
29 #define LOG_TAG "CwMcuSensor"
30 #include <cutils/log.h>
31 #include <cutils/properties.h>
32
33 #include "CwMcuSensor.h"
34
35
36 #define REL_Significant_Motion REL_WHEEL
37 #define LIGHTSENSOR_LEVEL 10
38 #define DEBUG_DATA 0
39 #define COMPASS_CALIBRATION_DATA_SIZE 26
40 #define G_SENSOR_CALIBRATION_DATA_SIZE 3
41 #define NS_PER_MS 1000000LL
42 #define EXHAUSTED_MAGIC 0x77
43
44 /*****************************************************************************/
45 #define IIO_MAX_BUFF_SIZE 4096
46 #define IIO_MAX_DATA_SIZE 24
47 #define IIO_MAX_NAME_LENGTH 30
48 #define IIO_BUF_SIZE_RETRY 8
49 #define INT32_CHAR_LEN 12
50
51 #define INIT_TRIGGER_RETRY 5
52
53 static const char iio_dir[] = "/sys/bus/iio/devices/";
54
min(int a,int b)55 static int min(int a, int b) {
56 return (a < b) ? a : b;
57 }
58
chomp(char * buf,size_t len)59 static int chomp(char *buf, size_t len) {
60 if (buf == NULL)
61 return -1;
62
63 while (len > 0 && isspace(buf[len-1])) {
64 buf[len - 1] = '\0';
65 len--;
66 }
67
68 return 0;
69 }
70
sysfs_set_input_attr(const char * attr,char * value,size_t len)71 int CwMcuSensor::sysfs_set_input_attr(const char *attr, char *value, size_t len) {
72 char fname[PATH_MAX];
73 int fd;
74 int rc;
75
76 snprintf(fname, sizeof(fname), "%s/%s", mDevPath, attr);
77 fname[sizeof(fname) - 1] = '\0';
78
79 fd = open(fname, O_WRONLY);
80 if (fd < 0) {
81 ALOGE("%s: fname = %s, fd = %d, failed: %s\n", __func__, fname, fd, strerror(errno));
82 return -EACCES;
83 }
84
85 rc = write(fd, value, (size_t)len);
86 if (rc < 0) {
87 ALOGE("%s: write failed: fd = %d, rc = %d, strerr = %s\n", __func__, fd, rc, strerror(errno));
88 close(fd);
89 return -EIO;
90 }
91
92 close(fd);
93
94 return 0;
95 }
96
sysfs_set_input_attr_by_int(const char * attr,int value)97 int CwMcuSensor::sysfs_set_input_attr_by_int(const char *attr, int value) {
98 char buf[INT32_CHAR_LEN];
99
100 size_t n = snprintf(buf, sizeof(buf), "%d", value);
101 if (n > sizeof(buf)) {
102 return -1;
103 }
104
105 return sysfs_set_input_attr(attr, buf, n);
106 }
107
find_type_by_name(const char * name,const char * type)108 static inline int find_type_by_name(const char *name, const char *type) {
109 const struct dirent *ent;
110 int number, numstrlen;
111
112 DIR *dp;
113 char thisname[IIO_MAX_NAME_LENGTH];
114 char *filename;
115 size_t size;
116 size_t typeLen = strlen(type);
117 size_t nameLen = strlen(name);
118
119 if (nameLen >= sizeof(thisname) - 1) {
120 return -ERANGE;
121 }
122
123 dp = opendir(iio_dir);
124 if (dp == NULL) {
125 return -ENODEV;
126 }
127
128 while (ent = readdir(dp), ent != NULL) {
129 if (strcmp(ent->d_name, ".") != 0 &&
130 strcmp(ent->d_name, "..") != 0 &&
131 strlen(ent->d_name) > typeLen &&
132 strncmp(ent->d_name, type, typeLen) == 0) {
133 numstrlen = sscanf(ent->d_name + typeLen,
134 "%d", &number);
135
136 /* verify the next character is not a colon */
137 if (ent->d_name[strlen(type) + numstrlen] != ':') {
138 size = sizeof(iio_dir) - 1 + typeLen + numstrlen + 6;
139 filename = (char *)malloc(size);
140
141 if (filename == NULL)
142 return -ENOMEM;
143
144 snprintf(filename, size,
145 "%s%s%d/name",
146 iio_dir, type, number);
147
148 int fd = open(filename, O_RDONLY);
149 free(filename);
150 if (fd < 0) {
151 continue;
152 }
153 size = read(fd, thisname, sizeof(thisname) - 1);
154 close(fd);
155 if (size < nameLen) {
156 continue;
157 }
158 thisname[size] = '\0';
159 if (strncmp(name, thisname, nameLen)) {
160 continue;
161 }
162 // check for termination or whitespace
163 if (!thisname[nameLen] || isspace(thisname[nameLen])) {
164 return number;
165 }
166 }
167 }
168 }
169 return -ENODEV;
170 }
171
172 int fill_block_debug = 0;
173
174 pthread_mutex_t sys_fs_mutex = PTHREAD_MUTEX_INITIALIZER;
175 pthread_mutex_t sync_timestamp_algo_mutex = PTHREAD_MUTEX_INITIALIZER;
176 pthread_mutex_t last_timestamp_mutex = PTHREAD_MUTEX_INITIALIZER;
177
sync_time_thread_in_class(void)178 void CwMcuSensor::sync_time_thread_in_class(void) {
179 int fd;
180 char buf[24];
181 int err;
182 uint64_t mcu_current_time;
183 uint64_t cpu_current_time;
184 int open_errno;
185
186 ALOGV("sync_time_thread_in_class++:\n");
187
188 pthread_mutex_lock(&sys_fs_mutex);
189
190 strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "batch_enable");
191
192 fd = open(fixed_sysfs_path, O_RDWR);
193 open_errno = errno;
194 pthread_mutex_unlock(&sys_fs_mutex);
195 if (fd >= 0) {
196 err = read(fd, buf, sizeof(buf) - 1);
197 cpu_current_time = getTimestamp();
198 if (err < 0) {
199 ALOGE("sync_time_thread_in_class: read fail, err = %d\n", err);
200 } else {
201 buf[err] = '\0';
202 mcu_current_time = strtoull(buf, NULL, 10) * NS_PER_US;
203 if (errno == ERANGE) {
204 ALOGE("sync_time_thread_in_class: strtoll fails, strerr = %s, buf = %s\n",
205 strerror(errno), buf);
206 } else {
207 pthread_mutex_lock(&sync_timestamp_algo_mutex);
208
209 if (mcu_current_time == 0) {
210 // Do a recovery mechanism of timestamp estimation when the sensor_hub reset happened
211 ALOGE("Sync: sensor hub is on reset\n");
212 time_slope = 1;
213 memset(last_mcu_timestamp, 0, sizeof(last_mcu_timestamp));
214 memset(last_cpu_timestamp, 0, sizeof(last_cpu_timestamp));
215 } else if ((mcu_current_time <= last_mcu_sync_time) || (last_mcu_sync_time == 0)) {
216 ALOGV("Sync: time_slope was not estimated yet\n");
217 time_slope = 1;
218 time_offset = cpu_current_time - mcu_current_time;
219 } else {
220 time_slope = (float)(cpu_current_time - last_cpu_sync_time) /
221 (float)(mcu_current_time - last_mcu_sync_time);
222 time_offset = cpu_current_time - mcu_current_time;
223 }
224
225 for (int i=0; i<numSensors; i++) {
226 offset_reset[i] = true;
227 }
228
229 ALOGV("Sync: time_offset = %" PRId64 ", time_slope = %f\n", time_offset, time_slope);
230 ALOGV("Sync: mcu_current_time = %" PRId64 ", last_mcu_sync_time = %" PRId64 "\n", mcu_current_time, last_mcu_sync_time);
231 ALOGV("Sync: cpu_current_time = %" PRId64 ", last_cpu_sync_time = %" PRId64 "\n", cpu_current_time, last_cpu_sync_time);
232
233 last_mcu_sync_time = mcu_current_time;
234 last_cpu_sync_time = cpu_current_time;
235
236 pthread_mutex_unlock(&sync_timestamp_algo_mutex);
237 }
238 }
239 close(fd);
240 } else {
241 ALOGE("sync_time_thread_in_class: open failed, path = .../batch_enable, fd = %d,"
242 " strerr = %s\n", fd, strerror(open_errno));
243 }
244
245 ALOGV("sync_time_thread_in_class--:\n");
246 }
247
sync_time_thread_run(void * context)248 void *sync_time_thread_run(void *context) {
249 CwMcuSensor *myClass = (CwMcuSensor *)context;
250
251 while (1) {
252 ALOGV("sync_time_thread_run++:\n");
253 myClass->sync_time_thread_in_class();
254 sleep(PERIODIC_SYNC_TIME_SEC);
255 ALOGV("sync_time_thread_run--:\n");
256 }
257 return NULL;
258 }
259
CwMcuSensor()260 CwMcuSensor::CwMcuSensor()
261 : SensorBase(NULL, "CwMcuSensor")
262 , mEnabled(0)
263 , mInputReader(IIO_MAX_BUFF_SIZE)
264 , time_slope(1)
265 , time_offset(0)
266 , init_trigger_done(false) {
267
268 int rc;
269
270 memset(last_mcu_timestamp, 0, sizeof(last_mcu_timestamp));
271 memset(last_cpu_timestamp, 0, sizeof(last_cpu_timestamp));
272 for (int i=0; i<numSensors; i++) {
273 offset_reset[i] = true;
274 }
275
276 mPendingEvents[CW_ACCELERATION].version = sizeof(sensors_event_t);
277 mPendingEvents[CW_ACCELERATION].sensor = ID_A;
278 mPendingEvents[CW_ACCELERATION].type = SENSOR_TYPE_ACCELEROMETER;
279 mPendingEvents[CW_ACCELERATION].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
280
281 mPendingEvents[CW_MAGNETIC].version = sizeof(sensors_event_t);
282 mPendingEvents[CW_MAGNETIC].sensor = ID_M;
283 mPendingEvents[CW_MAGNETIC].type = SENSOR_TYPE_MAGNETIC_FIELD;
284
285 mPendingEvents[CW_GYRO].version = sizeof(sensors_event_t);
286 mPendingEvents[CW_GYRO].sensor = ID_GY;
287 mPendingEvents[CW_GYRO].type = SENSOR_TYPE_GYROSCOPE;
288 mPendingEvents[CW_GYRO].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
289
290 mPendingEvents[CW_LIGHT].version = sizeof(sensors_event_t);
291 mPendingEvents[CW_LIGHT].sensor = ID_L;
292 mPendingEvents[CW_LIGHT].type = SENSOR_TYPE_LIGHT;
293 memset(mPendingEvents[CW_LIGHT].data, 0, sizeof(mPendingEvents[CW_LIGHT].data));
294
295 mPendingEvents[CW_PRESSURE].version = sizeof(sensors_event_t);
296 mPendingEvents[CW_PRESSURE].sensor = ID_PS;
297 mPendingEvents[CW_PRESSURE].type = SENSOR_TYPE_PRESSURE;
298 memset(mPendingEvents[CW_PRESSURE].data, 0, sizeof(mPendingEvents[CW_PRESSURE].data));
299
300 mPendingEvents[CW_ORIENTATION].version = sizeof(sensors_event_t);
301 mPendingEvents[CW_ORIENTATION].sensor = ID_O;
302 mPendingEvents[CW_ORIENTATION].type = SENSOR_TYPE_ORIENTATION;
303 mPendingEvents[CW_ORIENTATION].orientation.status = SENSOR_STATUS_ACCURACY_HIGH;
304
305 mPendingEvents[CW_ROTATIONVECTOR].version = sizeof(sensors_event_t);
306 mPendingEvents[CW_ROTATIONVECTOR].sensor = ID_RV;
307 mPendingEvents[CW_ROTATIONVECTOR].type = SENSOR_TYPE_ROTATION_VECTOR;
308
309 mPendingEvents[CW_LINEARACCELERATION].version = sizeof(sensors_event_t);
310 mPendingEvents[CW_LINEARACCELERATION].sensor = ID_LA;
311 mPendingEvents[CW_LINEARACCELERATION].type = SENSOR_TYPE_LINEAR_ACCELERATION;
312
313 mPendingEvents[CW_GRAVITY].version = sizeof(sensors_event_t);
314 mPendingEvents[CW_GRAVITY].sensor = ID_G;
315 mPendingEvents[CW_GRAVITY].type = SENSOR_TYPE_GRAVITY;
316
317 mPendingEvents[CW_MAGNETIC_UNCALIBRATED].version = sizeof(sensors_event_t);
318 mPendingEvents[CW_MAGNETIC_UNCALIBRATED].sensor = ID_CW_MAGNETIC_UNCALIBRATED;
319 mPendingEvents[CW_MAGNETIC_UNCALIBRATED].type = SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED;
320
321 mPendingEvents[CW_GYROSCOPE_UNCALIBRATED].version = sizeof(sensors_event_t);
322 mPendingEvents[CW_GYROSCOPE_UNCALIBRATED].sensor = ID_CW_GYROSCOPE_UNCALIBRATED;
323 mPendingEvents[CW_GYROSCOPE_UNCALIBRATED].type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED;
324
325 mPendingEvents[CW_GAME_ROTATION_VECTOR].version = sizeof(sensors_event_t);
326 mPendingEvents[CW_GAME_ROTATION_VECTOR].sensor = ID_CW_GAME_ROTATION_VECTOR;
327 mPendingEvents[CW_GAME_ROTATION_VECTOR].type = SENSOR_TYPE_GAME_ROTATION_VECTOR;
328
329 mPendingEvents[CW_GEOMAGNETIC_ROTATION_VECTOR].version = sizeof(sensors_event_t);
330 mPendingEvents[CW_GEOMAGNETIC_ROTATION_VECTOR].sensor = ID_CW_GEOMAGNETIC_ROTATION_VECTOR;
331 mPendingEvents[CW_GEOMAGNETIC_ROTATION_VECTOR].type = SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR;
332
333 mPendingEvents[CW_SIGNIFICANT_MOTION].version = sizeof(sensors_event_t);
334 mPendingEvents[CW_SIGNIFICANT_MOTION].sensor = ID_CW_SIGNIFICANT_MOTION;
335 mPendingEvents[CW_SIGNIFICANT_MOTION].type = SENSOR_TYPE_SIGNIFICANT_MOTION;
336
337 mPendingEvents[CW_STEP_DETECTOR].version = sizeof(sensors_event_t);
338 mPendingEvents[CW_STEP_DETECTOR].sensor = ID_CW_STEP_DETECTOR;
339 mPendingEvents[CW_STEP_DETECTOR].type = SENSOR_TYPE_STEP_DETECTOR;
340
341 mPendingEvents[CW_STEP_COUNTER].version = sizeof(sensors_event_t);
342 mPendingEvents[CW_STEP_COUNTER].sensor = ID_CW_STEP_COUNTER;
343 mPendingEvents[CW_STEP_COUNTER].type = SENSOR_TYPE_STEP_COUNTER;
344
345
346 mPendingEvents[CW_ACCELERATION_W].version = sizeof(sensors_event_t);
347 mPendingEvents[CW_ACCELERATION_W].sensor = ID_A_W;
348 mPendingEvents[CW_ACCELERATION_W].type = SENSOR_TYPE_ACCELEROMETER;
349 mPendingEvents[CW_ACCELERATION_W].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
350
351 mPendingEvents[CW_MAGNETIC_W].version = sizeof(sensors_event_t);
352 mPendingEvents[CW_MAGNETIC_W].sensor = ID_M_W;
353 mPendingEvents[CW_MAGNETIC_W].type = SENSOR_TYPE_MAGNETIC_FIELD;
354
355 mPendingEvents[CW_GYRO_W].version = sizeof(sensors_event_t);
356 mPendingEvents[CW_GYRO_W].sensor = ID_GY_W;
357 mPendingEvents[CW_GYRO_W].type = SENSOR_TYPE_GYROSCOPE;
358 mPendingEvents[CW_GYRO_W].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
359
360 mPendingEvents[CW_PRESSURE_W].version = sizeof(sensors_event_t);
361 mPendingEvents[CW_PRESSURE_W].sensor = ID_PS_W;
362 mPendingEvents[CW_PRESSURE_W].type = SENSOR_TYPE_PRESSURE;
363 memset(mPendingEvents[CW_PRESSURE_W].data, 0, sizeof(mPendingEvents[CW_PRESSURE_W].data));
364
365 mPendingEvents[CW_ORIENTATION_W].version = sizeof(sensors_event_t);
366 mPendingEvents[CW_ORIENTATION_W].sensor = ID_O_W;
367 mPendingEvents[CW_ORIENTATION_W].type = SENSOR_TYPE_ORIENTATION;
368 mPendingEvents[CW_ORIENTATION_W].orientation.status = SENSOR_STATUS_ACCURACY_HIGH;
369
370 mPendingEvents[CW_ROTATIONVECTOR_W].version = sizeof(sensors_event_t);
371 mPendingEvents[CW_ROTATIONVECTOR_W].sensor = ID_RV_W;
372 mPendingEvents[CW_ROTATIONVECTOR_W].type = SENSOR_TYPE_ROTATION_VECTOR;
373
374 mPendingEvents[CW_LINEARACCELERATION_W].version = sizeof(sensors_event_t);
375 mPendingEvents[CW_LINEARACCELERATION_W].sensor = ID_LA_W;
376 mPendingEvents[CW_LINEARACCELERATION_W].type = SENSOR_TYPE_LINEAR_ACCELERATION;
377
378 mPendingEvents[CW_GRAVITY_W].version = sizeof(sensors_event_t);
379 mPendingEvents[CW_GRAVITY_W].sensor = ID_G_W;
380 mPendingEvents[CW_GRAVITY_W].type = SENSOR_TYPE_GRAVITY;
381
382 mPendingEvents[CW_MAGNETIC_UNCALIBRATED_W].version = sizeof(sensors_event_t);
383 mPendingEvents[CW_MAGNETIC_UNCALIBRATED_W].sensor = ID_CW_MAGNETIC_UNCALIBRATED_W;
384 mPendingEvents[CW_MAGNETIC_UNCALIBRATED_W].type = SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED;
385
386 mPendingEvents[CW_GYROSCOPE_UNCALIBRATED_W].version = sizeof(sensors_event_t);
387 mPendingEvents[CW_GYROSCOPE_UNCALIBRATED_W].sensor = ID_CW_GYROSCOPE_UNCALIBRATED_W;
388 mPendingEvents[CW_GYROSCOPE_UNCALIBRATED_W].type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED;
389
390 mPendingEvents[CW_GAME_ROTATION_VECTOR_W].version = sizeof(sensors_event_t);
391 mPendingEvents[CW_GAME_ROTATION_VECTOR_W].sensor = ID_CW_GAME_ROTATION_VECTOR_W;
392 mPendingEvents[CW_GAME_ROTATION_VECTOR_W].type = SENSOR_TYPE_GAME_ROTATION_VECTOR;
393
394 mPendingEvents[CW_GEOMAGNETIC_ROTATION_VECTOR_W].version = sizeof(sensors_event_t);
395 mPendingEvents[CW_GEOMAGNETIC_ROTATION_VECTOR_W].sensor = ID_CW_GEOMAGNETIC_ROTATION_VECTOR_W;
396 mPendingEvents[CW_GEOMAGNETIC_ROTATION_VECTOR_W].type = SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR;
397
398 mPendingEvents[CW_STEP_DETECTOR_W].version = sizeof(sensors_event_t);
399 mPendingEvents[CW_STEP_DETECTOR_W].sensor = ID_CW_STEP_DETECTOR_W;
400 mPendingEvents[CW_STEP_DETECTOR_W].type = SENSOR_TYPE_STEP_DETECTOR;
401
402 mPendingEvents[CW_STEP_COUNTER_W].version = sizeof(sensors_event_t);
403 mPendingEvents[CW_STEP_COUNTER_W].sensor = ID_CW_STEP_COUNTER_W;
404 mPendingEvents[CW_STEP_COUNTER_W].type = SENSOR_TYPE_STEP_COUNTER;
405
406
407 mPendingEventsFlush.version = META_DATA_VERSION;
408 mPendingEventsFlush.sensor = 0;
409 mPendingEventsFlush.type = SENSOR_TYPE_META_DATA;
410
411 char buffer_access[PATH_MAX];
412 const char *device_name = "CwMcuSensor";
413 int rate = 20, dev_num, enabled = 0, i;
414
415 dev_num = find_type_by_name(device_name, "iio:device");
416 if (dev_num < 0)
417 dev_num = 0;
418
419 snprintf(buffer_access, sizeof(buffer_access),
420 "/dev/iio:device%d", dev_num);
421
422 data_fd = open(buffer_access, O_RDWR);
423 if (data_fd < 0) {
424 ALOGE("CwMcuSensor::CwMcuSensor: open file '%s' failed: %s\n",
425 buffer_access, strerror(errno));
426 }
427
428 if (data_fd >= 0) {
429 int i;
430 int fd;
431 int iio_buf_size;
432
433 ALOGV("%s: 11 Before pthread_mutex_lock()\n", __func__);
434 pthread_mutex_lock(&sys_fs_mutex);
435 ALOGV("%s: 11 Acquired pthread_mutex_lock()\n", __func__);
436
437 strcpy(fixed_sysfs_path,"/sys/class/htc_sensorhub/sensor_hub/");
438 fixed_sysfs_path_len = strlen(fixed_sysfs_path);
439
440 snprintf(mDevPath, sizeof(mDevPath), "%s%s", fixed_sysfs_path, "iio");
441
442 snprintf(mTriggerName, sizeof(mTriggerName), "%s-dev%d",
443 device_name, dev_num);
444 ALOGV("CwMcuSensor::CwMcuSensor: mTriggerName = %s\n", mTriggerName);
445
446 if (sysfs_set_input_attr_by_int("buffer/enable", 0) < 0) {
447 ALOGE("CwMcuSensor::CwMcuSensor: set IIO buffer enable failed00: %s\n",
448 strerror(errno));
449 }
450
451 // This is a piece of paranoia that retry for current_trigger
452 for (i = 0; i < INIT_TRIGGER_RETRY; i++) {
453 rc = sysfs_set_input_attr("trigger/current_trigger",
454 mTriggerName, strlen(mTriggerName));
455 if (rc < 0) {
456 if (sysfs_set_input_attr_by_int("buffer/enable", 0) < 0) {
457 ALOGE("CwMcuSensor::CwMcuSensor: set IIO buffer enable failed11: %s\n",
458 strerror(errno));
459 }
460 ALOGE("CwMcuSensor::CwMcuSensor: set current trigger failed: rc = %d, strerr() = %s"
461 ", i = %d\n",
462 rc, strerror(errno), i);
463 } else {
464 init_trigger_done = true;
465 break;
466 }
467 }
468
469 iio_buf_size = IIO_MAX_BUFF_SIZE;
470 for (i = 0; i < IIO_BUF_SIZE_RETRY; i++) {
471 if (sysfs_set_input_attr_by_int("buffer/length", iio_buf_size) < 0) {
472 ALOGE("CwMcuSensor::CwMcuSensor: set IIO buffer length (%d) failed: %s\n",
473 iio_buf_size, strerror(errno));
474 } else {
475 if (sysfs_set_input_attr_by_int("buffer/enable", 1) < 0) {
476 ALOGE("CwMcuSensor::CwMcuSensor: set IIO buffer enable failed22: %s, "
477 "i = %d, iio_buf_size = %d\n", strerror(errno), i, iio_buf_size);
478 } else {
479 ALOGI("CwMcuSensor::CwMcuSensor: set IIO buffer length success: %d\n", iio_buf_size);
480 break;
481 }
482 }
483 iio_buf_size /= 2;
484 }
485
486 strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "calibrator_en");
487 fd = open(fixed_sysfs_path, O_RDWR);
488 if (fd >= 0) {
489 static const char buf[] = "12";
490
491 rc = write(fd, buf, sizeof(buf) - 1);
492 if (rc < 0) {
493 ALOGE("%s: write buf = %s, failed: %s", __func__, buf, strerror(errno));
494 }
495
496 close(fd);
497 } else {
498 ALOGE("%s open %s failed: %s", __func__, fixed_sysfs_path, strerror(errno));
499 }
500
501 pthread_mutex_unlock(&sys_fs_mutex);
502
503 ALOGV("%s: data_fd = %d", __func__, data_fd);
504 ALOGV("%s: iio_device_path = %s", __func__, buffer_access);
505 ALOGV("%s: ctrl sysfs_path = %s", __func__, fixed_sysfs_path);
506
507 setEnable(0, 1); // Inside this function call, we use sys_fs_mutex
508 }
509
510 int gs_temp_data[G_SENSOR_CALIBRATION_DATA_SIZE] = {0};
511 int compass_temp_data[COMPASS_CALIBRATION_DATA_SIZE] = {0};
512
513
514 ALOGV("%s: 22 Before pthread_mutex_lock()\n", __func__);
515 pthread_mutex_lock(&sys_fs_mutex);
516 ALOGV("%s: 22 Acquired pthread_mutex_lock()\n", __func__);
517
518 //Sensor Calibration init . Waiting for firmware ready
519 rc = cw_read_calibrator_file(CW_MAGNETIC, SAVE_PATH_MAG, compass_temp_data);
520 if (rc == 0) {
521 ALOGD("Get compass calibration data from data/misc/ x is %d ,y is %d ,z is %d\n",
522 compass_temp_data[0], compass_temp_data[1], compass_temp_data[2]);
523 strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "calibrator_data_mag");
524 cw_save_calibrator_file(CW_MAGNETIC, fixed_sysfs_path, compass_temp_data);
525 } else {
526 ALOGI("Compass calibration data does not exist\n");
527 }
528
529 rc = cw_read_calibrator_file(CW_ACCELERATION, SAVE_PATH_ACC, gs_temp_data);
530 if (rc == 0) {
531 ALOGD("Get g-sensor user calibration data from data/misc/ x is %d ,y is %d ,z is %d\n",
532 gs_temp_data[0],gs_temp_data[1],gs_temp_data[2]);
533 strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "calibrator_data_acc");
534 if(!(gs_temp_data[0] == 0 && gs_temp_data[1] == 0 && gs_temp_data[2] == 0 )) {
535 cw_save_calibrator_file(CW_ACCELERATION, fixed_sysfs_path, gs_temp_data);
536 }
537 } else {
538 ALOGI("G-Sensor user calibration data does not exist\n");
539 }
540
541 pthread_mutex_unlock(&sys_fs_mutex);
542
543 pthread_create(&sync_time_thread, (const pthread_attr_t *) NULL,
544 sync_time_thread_run, (void *)this);
545
546 }
547
~CwMcuSensor()548 CwMcuSensor::~CwMcuSensor() {
549 if (!mEnabled.isEmpty()) {
550 setEnable(0, 0);
551 }
552 }
553
indexToValue(size_t index) const554 float CwMcuSensor::indexToValue(size_t index) const {
555 static const float luxValues[LIGHTSENSOR_LEVEL] = {
556 0.0, 10.0, 40.0, 90.0, 160.0,
557 225.0, 320.0, 640.0, 1280.0,
558 2600.0
559 };
560
561 const size_t maxIndex = (LIGHTSENSOR_LEVEL - 1);
562 if (index > maxIndex) {
563 index = maxIndex;
564 }
565 return luxValues[index];
566 }
567
find_handle(int32_t sensors_id)568 int CwMcuSensor::find_handle(int32_t sensors_id) {
569 switch (sensors_id) {
570 case CW_ACCELERATION:
571 return ID_A;
572 case CW_MAGNETIC:
573 return ID_M;
574 case CW_GYRO:
575 return ID_GY;
576 case CW_PRESSURE:
577 return ID_PS;
578 case CW_ORIENTATION:
579 return ID_O;
580 case CW_ROTATIONVECTOR:
581 return ID_RV;
582 case CW_LINEARACCELERATION:
583 return ID_LA;
584 case CW_GRAVITY:
585 return ID_G;
586 case CW_MAGNETIC_UNCALIBRATED:
587 return ID_CW_MAGNETIC_UNCALIBRATED;
588 case CW_GYROSCOPE_UNCALIBRATED:
589 return ID_CW_GYROSCOPE_UNCALIBRATED;
590 case CW_GAME_ROTATION_VECTOR:
591 return ID_CW_GAME_ROTATION_VECTOR;
592 case CW_GEOMAGNETIC_ROTATION_VECTOR:
593 return ID_CW_GEOMAGNETIC_ROTATION_VECTOR;
594 case CW_LIGHT:
595 return ID_L;
596 case CW_SIGNIFICANT_MOTION:
597 return ID_CW_SIGNIFICANT_MOTION;
598 case CW_STEP_DETECTOR:
599 return ID_CW_STEP_DETECTOR;
600 case CW_STEP_COUNTER:
601 return ID_CW_STEP_COUNTER;
602 case CW_ACCELERATION_W:
603 return ID_A_W;
604 case CW_MAGNETIC_W:
605 return ID_M_W;
606 case CW_GYRO_W:
607 return ID_GY_W;
608 case CW_PRESSURE_W:
609 return ID_PS_W;
610 case CW_ORIENTATION_W:
611 return ID_O_W;
612 case CW_ROTATIONVECTOR_W:
613 return ID_RV_W;
614 case CW_LINEARACCELERATION_W:
615 return ID_LA_W;
616 case CW_GRAVITY_W:
617 return ID_G_W;
618 case CW_MAGNETIC_UNCALIBRATED_W:
619 return ID_CW_MAGNETIC_UNCALIBRATED_W;
620 case CW_GYROSCOPE_UNCALIBRATED_W:
621 return ID_CW_GYROSCOPE_UNCALIBRATED_W;
622 case CW_GAME_ROTATION_VECTOR_W:
623 return ID_CW_GAME_ROTATION_VECTOR_W;
624 case CW_GEOMAGNETIC_ROTATION_VECTOR_W:
625 return ID_CW_GEOMAGNETIC_ROTATION_VECTOR_W;
626 case CW_STEP_DETECTOR_W:
627 return ID_CW_STEP_DETECTOR_W;
628 case CW_STEP_COUNTER_W:
629 return ID_CW_STEP_COUNTER_W;
630 default:
631 return 0xFF;
632 }
633 }
634
is_batch_wake_sensor(int32_t handle)635 bool CwMcuSensor::is_batch_wake_sensor(int32_t handle) {
636 switch (handle) {
637 case ID_A_W:
638 case ID_M_W:
639 case ID_GY_W:
640 case ID_PS_W:
641 case ID_O_W:
642 case ID_RV_W:
643 case ID_LA_W:
644 case ID_G_W:
645 case ID_CW_MAGNETIC_UNCALIBRATED_W:
646 case ID_CW_GYROSCOPE_UNCALIBRATED_W:
647 case ID_CW_GAME_ROTATION_VECTOR_W:
648 case ID_CW_GEOMAGNETIC_ROTATION_VECTOR_W:
649 case ID_CW_STEP_DETECTOR_W:
650 case ID_CW_STEP_COUNTER_W:
651 return true;
652 default:
653 return false;
654 }
655 }
656
find_sensor(int32_t handle)657 int CwMcuSensor::find_sensor(int32_t handle) {
658 int what = -1;
659
660 switch (handle) {
661 case ID_A:
662 what = CW_ACCELERATION;
663 break;
664 case ID_A_W:
665 what = CW_ACCELERATION_W;
666 break;
667 case ID_M:
668 what = CW_MAGNETIC;
669 break;
670 case ID_M_W:
671 what = CW_MAGNETIC_W;
672 break;
673 case ID_GY:
674 what = CW_GYRO;
675 break;
676 case ID_GY_W:
677 what = CW_GYRO_W;
678 break;
679 case ID_PS:
680 what = CW_PRESSURE;
681 break;
682 case ID_PS_W:
683 what = CW_PRESSURE_W;
684 break;
685 case ID_O:
686 what = CW_ORIENTATION;
687 break;
688 case ID_O_W:
689 what = CW_ORIENTATION_W;
690 break;
691 case ID_RV:
692 what = CW_ROTATIONVECTOR;
693 break;
694 case ID_RV_W:
695 what = CW_ROTATIONVECTOR_W;
696 break;
697 case ID_LA:
698 what = CW_LINEARACCELERATION;
699 break;
700 case ID_LA_W:
701 what = CW_LINEARACCELERATION_W;
702 break;
703 case ID_G:
704 what = CW_GRAVITY;
705 break;
706 case ID_G_W:
707 what = CW_GRAVITY_W;
708 break;
709 case ID_CW_MAGNETIC_UNCALIBRATED:
710 what = CW_MAGNETIC_UNCALIBRATED;
711 break;
712 case ID_CW_MAGNETIC_UNCALIBRATED_W:
713 what = CW_MAGNETIC_UNCALIBRATED_W;
714 break;
715 case ID_CW_GYROSCOPE_UNCALIBRATED:
716 what = CW_GYROSCOPE_UNCALIBRATED;
717 break;
718 case ID_CW_GYROSCOPE_UNCALIBRATED_W:
719 what = CW_GYROSCOPE_UNCALIBRATED_W;
720 break;
721 case ID_CW_GAME_ROTATION_VECTOR:
722 what = CW_GAME_ROTATION_VECTOR;
723 break;
724 case ID_CW_GAME_ROTATION_VECTOR_W:
725 what = CW_GAME_ROTATION_VECTOR_W;
726 break;
727 case ID_CW_GEOMAGNETIC_ROTATION_VECTOR:
728 what = CW_GEOMAGNETIC_ROTATION_VECTOR;
729 break;
730 case ID_CW_GEOMAGNETIC_ROTATION_VECTOR_W:
731 what = CW_GEOMAGNETIC_ROTATION_VECTOR_W;
732 break;
733 case ID_CW_SIGNIFICANT_MOTION:
734 what = CW_SIGNIFICANT_MOTION;
735 break;
736 case ID_CW_STEP_DETECTOR:
737 what = CW_STEP_DETECTOR;
738 break;
739 case ID_CW_STEP_DETECTOR_W:
740 what = CW_STEP_DETECTOR_W;
741 break;
742 case ID_CW_STEP_COUNTER:
743 what = CW_STEP_COUNTER;
744 break;
745 case ID_CW_STEP_COUNTER_W:
746 what = CW_STEP_COUNTER_W;
747 break;
748 case ID_L:
749 what = CW_LIGHT;
750 break;
751 }
752
753 return what;
754 }
755
getEnable(int32_t handle)756 int CwMcuSensor::getEnable(int32_t handle) {
757 ALOGV("CwMcuSensor::getEnable: handle = %d\n", handle);
758 return 0;
759 }
760
setEnable(int32_t handle,int en)761 int CwMcuSensor::setEnable(int32_t handle, int en) {
762
763 int what;
764 int err = 0;
765 int flags = !!en;
766 int fd;
767 char buf[10];
768 int temp_data[COMPASS_CALIBRATION_DATA_SIZE];
769 char value[PROPERTY_VALUE_MAX] = {0};
770 int rc;
771
772 ALOGV("%s: Before pthread_mutex_lock()\n", __func__);
773 pthread_mutex_lock(&sys_fs_mutex);
774 ALOGV("%s: Acquired pthread_mutex_lock()\n", __func__);
775
776 property_get("debug.sensorhal.fill.block", value, "0");
777 ALOGV("CwMcuSensor::setEnable: debug.sensorhal.fill.block= %s", value);
778 fill_block_debug = atoi(value) == 1;
779
780 what = find_sensor(handle);
781
782 ALOGV("CwMcuSensor::setEnable: "
783 "[v13-Dynamic adjust the IIO buffer], handle = %d, en = %d, what = %d\n",
784 handle, en, what);
785
786 if (uint32_t(what) >= numSensors) {
787 pthread_mutex_unlock(&sys_fs_mutex);
788 return -EINVAL;
789 }
790
791 if (en) offset_reset[what] = true;
792
793 strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "enable");
794 fd = open(fixed_sysfs_path, O_RDWR);
795 if (fd >= 0) {
796 int n = snprintf(buf, sizeof(buf), "%d %d\n", what, flags);
797 err = write(fd, buf, min(n, sizeof(buf)));
798 if (err < 0) {
799 ALOGE("%s: write failed: %s", __func__, strerror(errno));
800 }
801
802 close(fd);
803
804 if (flags) {
805 mEnabled.markBit(what);
806 } else {
807 mEnabled.clearBit(what);
808 }
809
810 if (mEnabled.isEmpty()) {
811 if (sysfs_set_input_attr_by_int("buffer/enable", 0) < 0) {
812 ALOGE("CwMcuSensor::setEnable: set buffer disable failed: %s\n", strerror(errno));
813 } else {
814 ALOGV("CwMcuSensor::setEnable: set IIO buffer enable = 0\n");
815 }
816 }
817 } else {
818 ALOGE("%s open failed: %s", __func__, strerror(errno));
819 }
820
821
822 // Sensor Calibration init. Waiting for firmware ready
823 if (!flags &&
824 ((what == CW_MAGNETIC) ||
825 (what == CW_ORIENTATION) ||
826 (what == CW_ROTATIONVECTOR))) {
827 ALOGV("Save Compass calibration data");
828 strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "calibrator_data_mag");
829 rc = cw_read_calibrator_file(CW_MAGNETIC, fixed_sysfs_path, temp_data);
830 if (rc== 0) {
831 cw_save_calibrator_file(CW_MAGNETIC, SAVE_PATH_MAG, temp_data);
832 } else {
833 ALOGI("Compass calibration data from driver fails\n");
834 }
835 }
836
837 pthread_mutex_unlock(&sys_fs_mutex);
838 return 0;
839 }
840
batch(int handle,int flags,int64_t period_ns,int64_t timeout)841 int CwMcuSensor::batch(int handle, int flags, int64_t period_ns, int64_t timeout)
842 {
843 int what;
844 int fd;
845 char buf[32] = {0};
846 int err;
847 int delay_ms;
848 int timeout_ms;
849 bool dryRun = false;
850
851 ALOGV("CwMcuSensor::batch++: handle = %d, flags = %d, period_ns = %" PRId64 ", timeout = %" PRId64 "\n",
852 handle, flags, period_ns, timeout);
853
854 what = find_sensor(handle);
855 delay_ms = period_ns/NS_PER_MS; // int64_t is being dropped to an int type
856 timeout_ms = timeout/NS_PER_MS; // int64_t is being dropped to an int type
857
858 if(flags & SENSORS_BATCH_DRY_RUN) {
859 dryRun = true;
860 }
861
862 if (uint32_t(what) >= CW_SENSORS_ID_END) {
863 return -EINVAL;
864 }
865
866 if(is_batch_wake_sensor(handle)) {
867 flags |= SENSORS_BATCH_WAKE_UPON_FIFO_FULL;
868 ALOGV("CwMcuSensor::batch: SENSORS_BATCH_WAKE_UPON_FIFO_FULL~!!\n");
869 } else
870 flags &= ~SENSORS_BATCH_WAKE_UPON_FIFO_FULL;
871
872 switch (what) {
873 case CW_LIGHT:
874 case CW_SIGNIFICANT_MOTION:
875 if (timeout > 0) {
876 ALOGI("CwMcuSensor::batch: handle = %d, not support batch mode", handle);
877 return -EINVAL;
878 }
879 break;
880 default:
881 break;
882 }
883
884 if (dryRun == true) {
885 ALOGV("CwMcuSensor::batch: SENSORS_BATCH_DRY_RUN is set\n");
886 return 0;
887 }
888
889 ALOGV("%s: Before pthread_mutex_lock()\n", __func__);
890 pthread_mutex_lock(&sys_fs_mutex);
891 ALOGV("%s: Acquired pthread_mutex_lock()\n", __func__);
892
893 if (mEnabled.isEmpty()) {
894 int i;
895 int iio_buf_size;
896
897 if (!init_trigger_done) {
898 err = sysfs_set_input_attr("trigger/current_trigger",
899 mTriggerName, strlen(mTriggerName));
900 if (err < 0) {
901 ALOGE("CwMcuSensor::batch: set current trigger failed: err = %d, strerr() = %s\n",
902 err, strerror(errno));
903 } else {
904 init_trigger_done = true;
905 }
906 }
907
908 iio_buf_size = IIO_MAX_BUFF_SIZE;
909 for (i = 0; i < IIO_BUF_SIZE_RETRY; i++) {
910 if (sysfs_set_input_attr_by_int("buffer/length", iio_buf_size) < 0) {
911 ALOGE("CwMcuSensor::batch: set IIO buffer length (%d) failed: %s\n",
912 iio_buf_size, strerror(errno));
913 } else {
914 if (sysfs_set_input_attr_by_int("buffer/enable", 1) < 0) {
915 ALOGE("CwMcuSensor::batch: set IIO buffer enable failed: %s, i = %d, "
916 "iio_buf_size = %d\n", strerror(errno), i , iio_buf_size);
917 } else {
918 ALOGI("CwMcuSensor::batch: set IIO buffer length = %d, success\n", iio_buf_size);
919 break;
920 }
921 }
922 iio_buf_size /= 2;
923 }
924 }
925
926 strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "batch_enable");
927
928 fd = open(fixed_sysfs_path, O_RDWR);
929 if (fd < 0) {
930 err = -errno;
931 } else {
932 int n = snprintf(buf, sizeof(buf), "%d %d %d %d\n", what, flags, delay_ms, timeout_ms);
933 err = write(fd, buf, min(n, sizeof(buf)));
934 if (err < 0) {
935 err = -errno;
936 } else {
937 err = 0;
938 }
939 close(fd);
940 }
941 pthread_mutex_unlock(&sys_fs_mutex);
942
943 ALOGV("CwMcuSensor::batch: fd = %d, sensors_id = %d, flags = %d, delay_ms= %d,"
944 " timeout_ms = %d, path = %s, err = %d\n",
945 fd , what, flags, delay_ms, timeout_ms, fixed_sysfs_path, err);
946
947 return err;
948 }
949
950
flush(int handle)951 int CwMcuSensor::flush(int handle)
952 {
953 int what;
954 int fd;
955 char buf[10] = {0};
956 int err;
957
958 what = find_sensor(handle);
959
960 if (uint32_t(what) >= CW_SENSORS_ID_END) {
961 return -EINVAL;
962 }
963
964 ALOGV("%s: Before pthread_mutex_lock()\n", __func__);
965 pthread_mutex_lock(&sys_fs_mutex);
966 ALOGV("%s: Acquired pthread_mutex_lock()\n", __func__);
967
968 strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "flush");
969
970 fd = open(fixed_sysfs_path, O_RDWR);
971 if (fd >= 0) {
972 int n = snprintf(buf, sizeof(buf), "%d\n", what);
973 err = write(fd, buf, min(n, sizeof(buf)));
974 if (err < 0) {
975 err = -errno;
976 } else {
977 err = 0;
978 }
979 close(fd);
980 } else {
981 ALOGI("CwMcuSensor::flush: flush not supported\n");
982 err = -EINVAL;
983 }
984
985 pthread_mutex_unlock(&sys_fs_mutex);
986 ALOGI("CwMcuSensor::flush: fd = %d, sensors_id = %d, path = %s, err = %d\n",
987 fd, what, fixed_sysfs_path, err);
988 return err;
989 }
990
991
hasPendingEvents() const992 bool CwMcuSensor::hasPendingEvents() const {
993 return !mPendingMask.isEmpty();
994 }
995
setDelay(int32_t handle,int64_t delay_ns)996 int CwMcuSensor::setDelay(int32_t handle, int64_t delay_ns) {
997 char buf[80];
998 int fd;
999 int what;
1000 int rc;
1001
1002 ALOGV("%s: Before pthread_mutex_lock()\n", __func__);
1003 pthread_mutex_lock(&sys_fs_mutex);
1004 ALOGV("%s: Acquired pthread_mutex_lock()\n", __func__);
1005
1006 ALOGV("CwMcuSensor::setDelay: handle = %" PRId32 ", delay_ns = %" PRId64 "\n",
1007 handle, delay_ns);
1008
1009 what = find_sensor(handle);
1010 if (uint32_t(what) >= numSensors) {
1011 pthread_mutex_unlock(&sys_fs_mutex);
1012 return -EINVAL;
1013 }
1014 strcpy(&fixed_sysfs_path[fixed_sysfs_path_len], "delay_ms");
1015 fd = open(fixed_sysfs_path, O_RDWR);
1016 if (fd >= 0) {
1017 size_t n = snprintf(buf, sizeof(buf), "%d %lld\n", what, (delay_ns/NS_PER_MS));
1018 write(fd, buf, min(n, sizeof(buf)));
1019 close(fd);
1020 }
1021
1022 pthread_mutex_unlock(&sys_fs_mutex);
1023 return 0;
1024
1025 }
1026
calculate_rv_4th_element(int sensors_id)1027 void CwMcuSensor::calculate_rv_4th_element(int sensors_id) {
1028 switch (sensors_id) {
1029 case CW_ROTATIONVECTOR:
1030 case CW_GAME_ROTATION_VECTOR:
1031 case CW_GEOMAGNETIC_ROTATION_VECTOR:
1032 case CW_ROTATIONVECTOR_W:
1033 case CW_GAME_ROTATION_VECTOR_W:
1034 case CW_GEOMAGNETIC_ROTATION_VECTOR_W:
1035 float q0, q1, q2, q3;
1036
1037 q1 = mPendingEvents[sensors_id].data[0];
1038 q2 = mPendingEvents[sensors_id].data[1];
1039 q3 = mPendingEvents[sensors_id].data[2];
1040
1041 q0 = 1 - q1*q1 - q2*q2 - q3*q3;
1042 q0 = (q0 > 0) ? (float)sqrt(q0) : 0;
1043
1044 mPendingEvents[sensors_id].data[3] = q0;
1045 break;
1046 default:
1047 break;
1048 }
1049 }
1050
readEvents(sensors_event_t * data,int count)1051 int CwMcuSensor::readEvents(sensors_event_t* data, int count) {
1052 uint64_t mtimestamp;
1053
1054 if (count < 1) {
1055 return -EINVAL;
1056 }
1057
1058 ALOGD_IF(fill_block_debug == 1, "CwMcuSensor::readEvents: Before fill\n");
1059 ssize_t n = mInputReader.fill(data_fd);
1060 ALOGD_IF(fill_block_debug == 1, "CwMcuSensor::readEvents: After fill, n = %zd\n", n);
1061 if (n < 0) {
1062 return n;
1063 }
1064
1065 cw_event const* event;
1066 uint8_t data_temp[24];
1067 int id;
1068 int numEventReceived = 0;
1069
1070 while (count && mInputReader.readEvent(&event)) {
1071
1072 memcpy(data_temp, event->data, sizeof(data_temp));
1073
1074 id = processEvent(data_temp);
1075 if (id == CW_META_DATA) {
1076 *data++ = mPendingEventsFlush;
1077 count--;
1078 numEventReceived++;
1079 ALOGV("CwMcuSensor::readEvents: metadata = %d\n", mPendingEventsFlush.meta_data.sensor);
1080 } else if ((id == TIME_DIFF_EXHAUSTED) || (id == CW_TIME_BASE)) {
1081 ALOGV("readEvents: id = %d\n", id);
1082 } else {
1083 /*** The algorithm which parsed mcu_time into cpu_time for each event ***/
1084 uint64_t event_mcu_time = mPendingEvents[id].timestamp;
1085 uint64_t event_cpu_time;
1086
1087 if (event_mcu_time < last_mcu_timestamp[id]) {
1088 ALOGE("Do syncronization due to wrong delta mcu_timestamp\n");
1089 ALOGE("curr_ts = %" PRIu64 " ns, last_ts = %" PRIu64 " ns",
1090 event_mcu_time, last_mcu_timestamp[id]);
1091 sync_time_thread_in_class();
1092 }
1093
1094 pthread_mutex_lock(&sync_timestamp_algo_mutex);
1095
1096 if (offset_reset[id]) {
1097 ALOGV("offset changed, id = %d, offset = %" PRId64 "\n", id, time_offset);
1098 offset_reset[id] = false;
1099 event_cpu_time = event_mcu_time + time_offset;
1100 if (event_cpu_time <= last_cpu_timestamp[id]) {
1101 int64_t event_mcu_diff = (event_mcu_time - last_mcu_timestamp[id]);
1102 int64_t event_cpu_diff = event_mcu_diff * time_slope;
1103 event_cpu_time = last_cpu_timestamp[id] + event_cpu_diff;
1104 }
1105 } else {
1106 int64_t event_mcu_diff = (event_mcu_time - last_mcu_timestamp[id]);
1107 int64_t event_cpu_diff = event_mcu_diff * time_slope;
1108 event_cpu_time = last_cpu_timestamp[id] + event_cpu_diff;
1109 }
1110 pthread_mutex_unlock(&sync_timestamp_algo_mutex);
1111
1112 pthread_mutex_lock(&last_timestamp_mutex);
1113
1114 mtimestamp = getTimestamp();
1115 ALOGV("readEvents: id = %d, accuracy = %d\n"
1116 , id
1117 , mPendingEvents[id].acceleration.status);
1118 ALOGV("readEvents: id = %d,"
1119 " mcu_time = %" PRId64 " ms,"
1120 " cpu_time = %" PRId64 " ns,"
1121 " delta = %" PRId64 " us,"
1122 " HALtime = %" PRId64 " ns\n",
1123 id,
1124 event_mcu_time / NS_PER_MS,
1125 event_cpu_time,
1126 (event_cpu_time - last_cpu_timestamp[id]) / NS_PER_US,
1127 mtimestamp);
1128 event_cpu_time = (mtimestamp > event_cpu_time) ? event_cpu_time : mtimestamp;
1129 last_mcu_timestamp[id] = event_mcu_time;
1130 last_cpu_timestamp[id] = event_cpu_time;
1131 pthread_mutex_unlock(&last_timestamp_mutex);
1132 /*** The algorithm which parsed mcu_time into cpu_time for each event ***/
1133
1134 mPendingEvents[id].timestamp = event_cpu_time;
1135
1136 if (mEnabled.hasBit(id)) {
1137 if (id == CW_SIGNIFICANT_MOTION) {
1138 setEnable(ID_CW_SIGNIFICANT_MOTION, 0);
1139 }
1140 calculate_rv_4th_element(id);
1141 *data++ = mPendingEvents[id];
1142 count--;
1143 numEventReceived++;
1144 }
1145 }
1146
1147 mInputReader.next();
1148 }
1149 return numEventReceived;
1150 }
1151
1152
processEvent(uint8_t * event)1153 int CwMcuSensor::processEvent(uint8_t *event) {
1154 int sensorsid = 0;
1155 int16_t data[3];
1156 int16_t bias[3];
1157 int64_t time;
1158
1159 sensorsid = (int)event[0];
1160 memcpy(data, &event[1], 6);
1161 memcpy(bias, &event[7], 6);
1162 memcpy(&time, &event[13], 8);
1163
1164 mPendingEvents[sensorsid].timestamp = time * NS_PER_MS;
1165
1166 switch (sensorsid) {
1167 case CW_ORIENTATION:
1168 case CW_ORIENTATION_W:
1169 mPendingMask.markBit(sensorsid);
1170 if ((sensorsid == CW_ORIENTATION) || (sensorsid == CW_ORIENTATION_W)) {
1171 mPendingEvents[sensorsid].orientation.status = bias[0];
1172 }
1173 mPendingEvents[sensorsid].data[0] = (float)data[0] * CONVERT_10;
1174 mPendingEvents[sensorsid].data[1] = (float)data[1] * CONVERT_10;
1175 mPendingEvents[sensorsid].data[2] = (float)data[2] * CONVERT_10;
1176 break;
1177 case CW_ACCELERATION:
1178 case CW_MAGNETIC:
1179 case CW_GYRO:
1180 case CW_LINEARACCELERATION:
1181 case CW_GRAVITY:
1182 case CW_ACCELERATION_W:
1183 case CW_MAGNETIC_W:
1184 case CW_GYRO_W:
1185 case CW_LINEARACCELERATION_W:
1186 case CW_GRAVITY_W:
1187 mPendingMask.markBit(sensorsid);
1188 if ((sensorsid == CW_MAGNETIC) || (sensorsid == CW_MAGNETIC_W)) {
1189 mPendingEvents[sensorsid].magnetic.status = bias[0];
1190 ALOGV("CwMcuSensor::processEvent: magnetic accuracy = %d\n",
1191 mPendingEvents[sensorsid].magnetic.status);
1192 }
1193 mPendingEvents[sensorsid].data[0] = (float)data[0] * CONVERT_100;
1194 mPendingEvents[sensorsid].data[1] = (float)data[1] * CONVERT_100;
1195 mPendingEvents[sensorsid].data[2] = (float)data[2] * CONVERT_100;
1196 break;
1197 case CW_PRESSURE:
1198 case CW_PRESSURE_W:
1199 mPendingMask.markBit(sensorsid);
1200 // .pressure is data[0] and the unit is hectopascal (hPa)
1201 mPendingEvents[sensorsid].pressure = ((float)*(int32_t *)(&data[0])) * CONVERT_100;
1202 // data[1] is not used, and data[2] is the temperature
1203 mPendingEvents[sensorsid].data[2] = ((float)data[2]) * CONVERT_100;
1204 break;
1205 case CW_ROTATIONVECTOR:
1206 case CW_GAME_ROTATION_VECTOR:
1207 case CW_GEOMAGNETIC_ROTATION_VECTOR:
1208 case CW_ROTATIONVECTOR_W:
1209 case CW_GAME_ROTATION_VECTOR_W:
1210 case CW_GEOMAGNETIC_ROTATION_VECTOR_W:
1211 mPendingMask.markBit(sensorsid);
1212 mPendingEvents[sensorsid].data[0] = (float)data[0] * CONVERT_10000;
1213 mPendingEvents[sensorsid].data[1] = (float)data[1] * CONVERT_10000;
1214 mPendingEvents[sensorsid].data[2] = (float)data[2] * CONVERT_10000;
1215 break;
1216 case CW_MAGNETIC_UNCALIBRATED:
1217 case CW_GYROSCOPE_UNCALIBRATED:
1218 case CW_MAGNETIC_UNCALIBRATED_W:
1219 case CW_GYROSCOPE_UNCALIBRATED_W:
1220 mPendingMask.markBit(sensorsid);
1221 mPendingEvents[sensorsid].data[0] = (float)data[0] * CONVERT_100;
1222 mPendingEvents[sensorsid].data[1] = (float)data[1] * CONVERT_100;
1223 mPendingEvents[sensorsid].data[2] = (float)data[2] * CONVERT_100;
1224 mPendingEvents[sensorsid].data[3] = (float)bias[0] * CONVERT_100;
1225 mPendingEvents[sensorsid].data[4] = (float)bias[1] * CONVERT_100;
1226 mPendingEvents[sensorsid].data[5] = (float)bias[2] * CONVERT_100;
1227 break;
1228 case CW_SIGNIFICANT_MOTION:
1229 mPendingMask.markBit(sensorsid);
1230 mPendingEvents[sensorsid].data[0] = 1.0;
1231 ALOGV("SIGNIFICANT timestamp = %" PRIu64 "\n", mPendingEvents[sensorsid].timestamp);
1232 break;
1233 case CW_LIGHT:
1234 mPendingMask.markBit(sensorsid);
1235 mPendingEvents[sensorsid].light = indexToValue(data[0]);
1236 break;
1237 case CW_STEP_DETECTOR:
1238 case CW_STEP_DETECTOR_W:
1239 mPendingMask.markBit(sensorsid);
1240 mPendingEvents[sensorsid].data[0] = data[0];
1241 ALOGV("STEP_DETECTOR, timestamp = %" PRIu64 "\n", mPendingEvents[sensorsid].timestamp);
1242 break;
1243 case CW_STEP_COUNTER:
1244 case CW_STEP_COUNTER_W:
1245 mPendingMask.markBit(sensorsid);
1246 // We use 4 bytes in SensorHUB
1247 mPendingEvents[sensorsid].u64.step_counter = *(uint32_t *)&data[0];
1248 mPendingEvents[sensorsid].u64.step_counter += 0x100000000LL * (*(uint32_t *)&bias[0]);
1249 ALOGV("processEvent: step counter = %" PRId64 "\n",
1250 mPendingEvents[sensorsid].u64.step_counter);
1251 break;
1252 case CW_META_DATA:
1253 mPendingEventsFlush.meta_data.what = META_DATA_FLUSH_COMPLETE;
1254 mPendingEventsFlush.meta_data.sensor = find_handle(data[0]);
1255 ALOGV("CW_META_DATA: meta_data.sensor = %d, data[0] = %d\n",
1256 mPendingEventsFlush.meta_data.sensor, data[0]);
1257 break;
1258 default:
1259 ALOGW("%s: Unknown sensorsid = %d\n", __func__, sensorsid);
1260 break;
1261 }
1262
1263 return sensorsid;
1264 }
1265
1266
cw_save_calibrator_file(int type,const char * path,int * str)1267 void CwMcuSensor::cw_save_calibrator_file(int type, const char * path, int* str) {
1268 FILE *fp_file;
1269 int i;
1270 int rc;
1271
1272 ALOGV("CwMcuSensor::cw_save_calibrator_file: path = %s\n", path);
1273
1274 fp_file = fopen(path, "w+");
1275 if (!fp_file) {
1276 ALOGE("CwMcuSensor::cw_save_calibrator_file: open file '%s' failed: %s\n",
1277 path, strerror(errno));
1278 return;
1279 }
1280
1281 if ((type == CW_GYRO) || (type == CW_ACCELERATION)) {
1282 fprintf(fp_file, "%d %d %d\n", str[0], str[1], str[2]);
1283 } else if(type == CW_MAGNETIC) {
1284 for (i = 0; i < COMPASS_CALIBRATION_DATA_SIZE; i++) {
1285 ALOGV("CwMcuSensor::cw_save_calibrator_file: str[%d] = %d\n", i, str[i]);
1286 rc = fprintf(fp_file, "%d%c", str[i], (i == (COMPASS_CALIBRATION_DATA_SIZE-1)) ? '\n' : ' ');
1287 if (rc < 0) {
1288 ALOGE("CwMcuSensor::cw_save_calibrator_file: fprintf fails, rc = %d\n", rc);
1289 }
1290 }
1291 }
1292
1293 fclose(fp_file);
1294 return;
1295 }
1296
cw_read_calibrator_file(int type,const char * path,int * str)1297 int CwMcuSensor::cw_read_calibrator_file(int type, const char * path, int* str) {
1298 FILE *fp;
1299 int readBytes;
1300 int data[COMPASS_CALIBRATION_DATA_SIZE] = {0};
1301 unsigned int i;
1302 int my_errno;
1303
1304 ALOGV("CwMcuSensor::cw_read_calibrator_file: path = %s\n", path);
1305
1306 fp = fopen(path, "r");
1307 if (!fp) {
1308 ALOGE("CwMcuSensor::cw_read_calibrator_file: open file '%s' failed: %s\n",
1309 path, strerror(errno));
1310 // errno is reset to 0 before return
1311 return -1;
1312 }
1313
1314 if (type == CW_GYRO || type == CW_ACCELERATION) {
1315 readBytes = fscanf(fp, "%d %d %d\n", &str[0], &str[1], &str[2]);
1316 my_errno = errno;
1317 if (readBytes != 3) {
1318 ALOGE("CwMcuSensor::cw_read_calibrator_file: fscanf3, readBytes = %d, strerror = %s\n", readBytes, strerror(my_errno));
1319 }
1320
1321 } else if (type == CW_MAGNETIC) {
1322 ALOGV("CwMcuSensor::cw_read_calibrator_file: COMPASS_CALIBRATION_DATA_SIZE = %d\n", COMPASS_CALIBRATION_DATA_SIZE);
1323 // COMPASS_CALIBRATION_DATA_SIZE is 26
1324 for (i = 0; i < COMPASS_CALIBRATION_DATA_SIZE; i++) {
1325 readBytes = fscanf(fp, "%d ", &str[i]);
1326 my_errno = errno;
1327 ALOGV("CwMcuSensor::cw_read_calibrator_file: str[%d] = %d\n", i, str[i]);
1328 if (readBytes < 1) {
1329 ALOGE("CwMcuSensor::cw_read_calibrator_file: fscanf26, readBytes = %d, strerror = %s\n", readBytes, strerror(my_errno));
1330 fclose(fp);
1331 return readBytes;
1332 }
1333 }
1334 }
1335 fclose(fp);
1336 return 0;
1337 }
1338