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