• Home
  • History
  • Annotate
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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