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 55 static int min(int a, int b) { 56 return (a < b) ? a : b; 57 } 58 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 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 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 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 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 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 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 548 CwMcuSensor::~CwMcuSensor() { 549 if (!mEnabled.isEmpty()) { 550 setEnable(0, 0); 551 } 552 } 553 554 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 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 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 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 756 int CwMcuSensor::getEnable(int32_t handle) { 757 ALOGV("CwMcuSensor::getEnable: handle = %d\n", handle); 758 return 0; 759 } 760 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 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 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 992 bool CwMcuSensor::hasPendingEvents() const { 993 return !mPendingMask.isEmpty(); 994 } 995 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 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 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 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 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 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
