1/*
2* Copyright (C) 2012 Invensense, Inc.
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#define LOG_NDEBUG 0
18
19//#define KLP 1 //Key Lime Pie Temporary Test Define
20//see also the EXTRA_VERBOSE define in the MPLSensor.h header file
21
22#include <fcntl.h>
23#include <errno.h>
24#include <math.h>
25#include <float.h>
26#include <poll.h>
27#include <unistd.h>
28#include <dirent.h>
29#include <stdlib.h>
30#include <sys/select.h>
31#include <sys/syscall.h>
32#include <dlfcn.h>
33#include <pthread.h>
34#include <cutils/log.h>
35#include <utils/KeyedVector.h>
36#include <utils/String8.h>
37#include <string.h>
38#include <linux/input.h>
39#include <utils/Atomic.h>
40
41#include "MPLSensor.h"
42#include "PressureSensor.IIO.secondary.h"
43#include "MPLSupport.h"
44#include "sensor_params.h"
45
46#include "invensense.h"
47#include "invensense_adv.h"
48#include "ml_stored_data.h"
49#include "ml_load_dmp.h"
50#include "ml_sysfs_helper.h"
51
52#define ENABLE_MULTI_RATE
53// #define TESTING
54// #define USE_LPQ_AT_FASTEST
55#define ENABLE_PRESSSURE
56
57#ifdef THIRD_PARTY_ACCEL
58#pragma message("HAL:build third party accel support")
59#define USE_THIRD_PARTY_ACCEL (1)
60#else
61#define USE_THIRD_PARTY_ACCEL (0)
62#endif
63
64#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))
65
66/******************************************************************************/
67/*  MPL interface misc.                                                       */
68/******************************************************************************/
69static int hertz_request = 200;
70
71#define DEFAULT_MPL_GYRO_RATE           (20000L)     //us
72#define DEFAULT_MPL_COMPASS_RATE        (20000L)     //us
73
74#define DEFAULT_HW_GYRO_RATE            (100)        //Hz
75#define DEFAULT_HW_ACCEL_RATE           (20)         //ms
76#define DEFAULT_HW_COMPASS_RATE         (20000000L)  //ns
77#define DEFAULT_HW_AKMD_COMPASS_RATE    (200000000L) //ns
78
79/* convert ns to hardware units */
80#define HW_GYRO_RATE_NS                 (1000000000LL / rate_request) // to Hz
81#define HW_ACCEL_RATE_NS                (rate_request / (1000000L))   // to ms
82#define HW_COMPASS_RATE_NS              (rate_request)                // to ns
83
84/* convert Hz to hardware units */
85#define HW_GYRO_RATE_HZ                 (hertz_request)
86#define HW_ACCEL_RATE_HZ                (1000 / hertz_request)
87#define HW_COMPASS_RATE_HZ              (1000000000LL / hertz_request)
88
89#define RATE_200HZ                      5000000LL
90#define RATE_15HZ                       66667000LL
91#define RATE_5HZ                        200000000LL
92
93// mask of virtual sensors that require gyro + accel + compass data
94#define VIRTUAL_SENSOR_9AXES_MASK ( \
95        (1 << Orientation)          \
96        | (1 << RotationVector)     \
97        | (1 << LinearAccel)        \
98        | (1 << Gravity)            \
99)
100// mask of virtual sensors that require gyro + accel data (but no compass data)
101#define VIRTUAL_SENSOR_GYRO_6AXES_MASK ( \
102        (1 << GameRotationVector)   \
103)
104// mask of virtual sensors that require mag + accel data (but no gyro data)
105#define VIRTUAL_SENSOR_MAG_6AXES_MASK (    \
106        (1 << GeomagneticRotationVector)   \
107)
108// mask of all virtual sensors
109#define VIRTUAL_SENSOR_ALL_MASK (       \
110        VIRTUAL_SENSOR_9AXES_MASK       \
111        | VIRTUAL_SENSOR_GYRO_6AXES_MASK     \
112        | VIRTUAL_SENSOR_MAG_6AXES_MASK \
113)
114
115static struct timespec mt_pre;
116static struct sensor_t sSensorList[] =
117{
118    {"MPL Gyroscope", "Invensense", 1,
119     SENSORS_GYROSCOPE_HANDLE,
120     SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
121    {"MPL Raw Gyroscope", "Invensense", 1,
122     SENSORS_RAW_GYROSCOPE_HANDLE,
123     SENSOR_TYPE_GYROSCOPE_UNCALIBRATED, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
124    {"MPL Accelerometer", "Invensense", 1,
125     SENSORS_ACCELERATION_HANDLE,
126     SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
127    {"MPL Magnetic Field", "Invensense", 1,
128     SENSORS_MAGNETIC_FIELD_HANDLE,
129     SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
130#ifdef ENABLE_PRESSURE
131     {"MPL Pressure", "Invensense", 1,
132     SENSORS_PRESSURE_HANDLE,
133     SENSOR_TYPE_PRESSURE, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
134#endif
135    {"MPL Raw Magnetic Field", "Invensense", 1,
136     SENSORS_RAW_MAGNETIC_FIELD_HANDLE,
137     SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
138    {"MPL Orientation", "Invensense", 1,
139     SENSORS_ORIENTATION_HANDLE,
140     SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, {}},
141    {"MPL Rotation Vector", "Invensense", 1,
142     SENSORS_ROTATION_VECTOR_HANDLE,
143     SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
144    {"MPL Game Rotation Vector", "Invensense", 1,
145     SENSORS_GAME_ROTATION_VECTOR_HANDLE,
146     SENSOR_TYPE_GAME_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
147    {"MPL Linear Acceleration", "Invensense", 1,
148     SENSORS_LINEAR_ACCEL_HANDLE,
149     SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
150    {"MPL Gravity", "Invensense", 1,
151     SENSORS_GRAVITY_HANDLE,
152     SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
153    {"MPL Significant Motion", "Invensense", 1,
154     SENSORS_SIGNIFICANT_MOTION_HANDLE,
155     SENSOR_TYPE_SIGNIFICANT_MOTION, 100.0f, 1.0f, 1.1f, 0, 0, 0, {}},
156    {"MPL Step Detector", "Invensense", 1,
157     SENSORS_PEDOMETER_HANDLE,
158     SENSOR_TYPE_STEP_DETECTOR, 100.0f, 1.0f, 1.1f, 0, 0, 0, {}},
159    {"MPL Step Counter", "Invensense", 1,
160     SENSORS_STEP_COUNTER_HANDLE,
161     SENSOR_TYPE_STEP_COUNTER, 100.0f, 1.0f, 1.1f, 0, 0, 0, {}},
162    {"MPL Geomagnetic Rotation Vector", "Invensense", 1,
163     SENSORS_GEOMAGNETIC_ROTATION_VECTOR_HANDLE,
164     SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
165#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
166    {"MPL Screen Orientation", "Invensense ", 1,
167     SENSORS_SCREEN_ORIENTATION_HANDLE,
168     SENSOR_TYPE_SCREEN_ORIENTATION, 100.0f, 1.0f, 1.1f, 0, 0, 0, {}},
169#endif
170};
171
172MPLSensor *MPLSensor::gMPLSensor = NULL;
173
174extern "C" {
175void procData_cb_wrapper()
176{
177    if(MPLSensor::gMPLSensor) {
178        MPLSensor::gMPLSensor->cbProcData();
179    }
180}
181
182void setCallbackObject(MPLSensor* gbpt)
183{
184    MPLSensor::gMPLSensor = gbpt;
185}
186
187MPLSensor* getCallbackObject() {
188    return MPLSensor::gMPLSensor;
189}
190
191} // end of extern C
192
193//#define IINV_PLAYBACK_DBG
194#ifdef INV_PLAYBACK_DBG
195static FILE *logfile = NULL;
196#endif
197
198/*******************************************************************************
199 * MPLSensor class implementation
200 ******************************************************************************/
201
202// following extended initializer list would only be available with -std=c++11
203//  or -std=gnu+11
204MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *))
205                       : SensorBase(NULL, NULL),
206                         mNewData(0),
207                         mMasterSensorMask(INV_ALL_SENSORS),
208                         mLocalSensorMask(0),
209                         mPollTime(-1),
210                         mStepCountPollTime(-1),
211                         mHaveGoodMpuCal(0),
212                         mGyroAccuracy(0),
213                         mAccelAccuracy(0),
214                         mCompassAccuracy(0),
215                         mSampleCount(0),
216                         dmp_orient_fd(-1),
217                         mDmpOrientationEnabled(0),
218                         dmp_sign_motion_fd(-1),
219                         mDmpSignificantMotionEnabled(0),
220                         dmp_pedometer_fd(-1),
221                         mDmpPedometerEnabled(0),
222                         mDmpStepCountEnabled(0),
223                         mEnabled(0),
224                         mBatchEnabled(0),
225                         mAccelInputReader(4),
226                         mGyroInputReader(32),
227                         mTempScale(0),
228                         mTempOffset(0),
229                         mTempCurrentTime(0),
230                         mAccelScale(2),
231                         mGyroScale(2000),
232                         mCompassScale(0),
233                         mFactoryGyroBiasAvailable(false),
234                         mGyroBiasAvailable(false),
235                         mGyroBiasApplied(false),
236                         mFactoryAccelBiasAvailable(false),
237                         mAccelBiasAvailable(false),
238                         mAccelBiasApplied(false),
239                         mPendingMask(0),
240                         mSensorMask(0),
241                         mMplFeatureActiveMask(0),
242                         mFeatureActiveMask(0),
243                         mDmpOn(0),
244                         mPedUpdate(0),
245                         mQuatSensorTimestamp(0),
246                         mStepSensorTimestamp(0),
247                         mLastStepCount(0),
248                         mLeftOverBufferSize(0) {
249    VFUNC_LOG;
250
251    inv_error_t rv;
252    int i, fd;
253    char *port = NULL;
254    char *ver_str;
255    unsigned long mSensorMask;
256    int res;
257    FILE *fptr;
258
259    mCompassSensor = compass;
260
261    LOGV_IF(EXTRA_VERBOSE,
262            "HAL:MPLSensor constructor : NumSensors = %d", NumSensors);
263
264    pthread_mutex_init(&mMplMutex, NULL);
265    pthread_mutex_init(&mHALMutex, NULL);
266    memset(mGyroOrientation, 0, sizeof(mGyroOrientation));
267    memset(mAccelOrientation, 0, sizeof(mAccelOrientation));
268
269    /* setup sysfs paths */
270    inv_init_sysfs_attributes();
271
272    /* get chip name */
273    if (inv_get_chip_name(chip_ID) != INV_SUCCESS) {
274        LOGE("HAL:ERR- Failed to get chip ID\n");
275    } else {
276        LOGV_IF(PROCESS_VERBOSE, "HAL:Chip ID= %s\n", chip_ID);
277    }
278
279    enable_iio_sysfs();
280
281    /* instantiate pressure sensor on secondary bus */
282    if (strcmp(mSysfsPath, "") != 0) {
283        mPressureSensor = new PressureSensor((const char*)mSysfsPath);
284    } else {
285        LOGE("HAL:ERR - Failed to instantiate pressure sensor class");
286    }
287
288    /* reset driver master enable */
289    masterEnable(0);
290
291    //Always load DMP for KLP
292    /* Load DMP image if capable, ie. MPU6xxx/9xxx */
293    loadDMP();
294
295    /* open temperature fd for temp comp */
296    LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature);
297    gyro_temperature_fd = open(mpu.temperature, O_RDONLY);
298    if (gyro_temperature_fd == -1) {
299        LOGE("HAL:could not open temperature node");
300    } else {
301        LOGV_IF(EXTRA_VERBOSE,
302                "HAL:temperature_fd opened: %s", mpu.temperature);
303    }
304
305    /* read gyro FSR to calculate accel scale later */
306    char gyroBuf[5];
307    int count = 0;
308         LOGV_IF(SYSFS_VERBOSE,
309             "HAL:sysfs:cat %s (%lld)", mpu.gyro_fsr, getTimestamp());
310
311    fd = open(mpu.gyro_fsr, O_RDONLY);
312    if(fd < 0) {
313        LOGE("HAL:Error opening gyro FSR");
314    } else {
315        memset(gyroBuf, 0, sizeof(gyroBuf));
316        count = read_attribute_sensor(fd, gyroBuf, sizeof(gyroBuf));
317        if(count < 1) {
318            LOGE("HAL:Error reading gyro FSR");
319        } else {
320            count = sscanf(gyroBuf, "%ld", &mGyroScale);
321            if(count)
322                LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro FSR used %ld", mGyroScale);
323        }
324        close(fd);
325    }
326
327    /* read gyro self test scale used to calculate factory cal bias later */
328    char gyroScale[5];
329         LOGV_IF(SYSFS_VERBOSE,
330             "HAL:sysfs:cat %s (%lld)", mpu.in_gyro_self_test_scale, getTimestamp());
331    fd = open(mpu.in_gyro_self_test_scale, O_RDONLY);
332    if(fd < 0) {
333        LOGE("HAL:Error opening gyro self test scale");
334    } else {
335        memset(gyroBuf, 0, sizeof(gyroBuf));
336        count = read_attribute_sensor(fd, gyroScale, sizeof(gyroScale));
337        if(count < 1) {
338            LOGE("HAL:Error reading gyro self test scale");
339        } else {
340            count = sscanf(gyroScale, "%ld", &mGyroSelfTestScale);
341            if(count)
342                LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro self test scale used %ld", mGyroSelfTestScale);
343        }
344        close(fd);
345    }
346
347    /* open Factory Gyro Bias fd */
348    /* mFactoryGyBias contains bias values that will be used for device offset */
349    memset(mFactoryGyroBias, 0, sizeof(mFactoryGyroBias));
350    LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro x offset path: %s", mpu.in_gyro_x_offset);
351    LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro y offset path: %s", mpu.in_gyro_y_offset);
352    LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro z offset path: %s", mpu.in_gyro_z_offset);
353    gyro_x_offset_fd = open(mpu.in_gyro_x_offset, O_RDWR);
354    gyro_y_offset_fd = open(mpu.in_gyro_y_offset, O_RDWR);
355    gyro_z_offset_fd = open(mpu.in_gyro_z_offset, O_RDWR);
356    if (gyro_x_offset_fd == -1 ||
357             gyro_y_offset_fd == -1 || gyro_z_offset_fd == -1) {
358            LOGE("HAL:could not open factory gyro calibrated bias");
359    } else {
360        LOGV_IF(EXTRA_VERBOSE,
361             "HAL:gyro_offset opened");
362    }
363
364    /* open Gyro Bias fd */
365    /* mGyroBias contains bias values that will be used for framework */
366    /* mGyroChipBias contains bias values that will be used for dmp */
367    memset(mGyroBias, 0, sizeof(mGyroBias));
368    memset(mGyroChipBias, 0, sizeof(mGyroChipBias));
369    LOGV_IF(EXTRA_VERBOSE, "HAL: gyro x dmp bias path: %s", mpu.in_gyro_x_dmp_bias);
370    LOGV_IF(EXTRA_VERBOSE, "HAL: gyro y dmp bias path: %s", mpu.in_gyro_y_dmp_bias);
371    LOGV_IF(EXTRA_VERBOSE, "HAL: gyro z dmp bias path: %s", mpu.in_gyro_z_dmp_bias);
372    gyro_x_dmp_bias_fd = open(mpu.in_gyro_x_dmp_bias, O_RDWR);
373    gyro_y_dmp_bias_fd = open(mpu.in_gyro_y_dmp_bias, O_RDWR);
374    gyro_z_dmp_bias_fd = open(mpu.in_gyro_z_dmp_bias, O_RDWR);
375    if (gyro_x_dmp_bias_fd == -1 ||
376             gyro_y_dmp_bias_fd == -1 || gyro_z_dmp_bias_fd == -1) {
377            LOGE("HAL:could not open gyro DMP calibrated bias");
378    } else {
379        LOGV_IF(EXTRA_VERBOSE,
380             "HAL:gyro_dmp_bias opened");
381    }
382
383    /* read accel FSR to calcuate accel scale later */
384    if (USE_THIRD_PARTY_ACCEL == 0) {
385        char buf[3];
386        int count = 0;
387        LOGV_IF(SYSFS_VERBOSE,
388                "HAL:sysfs:cat %s (%lld)", mpu.accel_fsr, getTimestamp());
389
390        fd = open(mpu.accel_fsr, O_RDONLY);
391        if(fd < 0) {
392            LOGE("HAL:Error opening accel FSR");
393        } else {
394           memset(buf, 0, sizeof(buf));
395           count = read_attribute_sensor(fd, buf, sizeof(buf));
396           if(count < 1) {
397               LOGE("HAL:Error reading accel FSR");
398           } else {
399               count = sscanf(buf, "%d", &mAccelScale);
400               if(count)
401                   LOGV_IF(EXTRA_VERBOSE, "HAL:Accel FSR used %d", mAccelScale);
402           }
403           close(fd);
404        }
405
406        /* open Accel Bias fd */
407        /* mAccelBias contains bias that will be used for dmp */
408        memset(mAccelBias, 0, sizeof(mAccelBias));
409        LOGV_IF(EXTRA_VERBOSE, "HAL:accel x dmp bias path: %s", mpu.in_accel_x_dmp_bias);
410        LOGV_IF(EXTRA_VERBOSE, "HAL:accel y dmp bias path: %s", mpu.in_accel_y_dmp_bias);
411        LOGV_IF(EXTRA_VERBOSE, "HAL:accel z dmp bias path: %s", mpu.in_accel_z_dmp_bias);
412        accel_x_dmp_bias_fd = open(mpu.in_accel_x_dmp_bias, O_RDWR);
413        accel_y_dmp_bias_fd = open(mpu.in_accel_y_dmp_bias, O_RDWR);
414        accel_z_dmp_bias_fd = open(mpu.in_accel_z_dmp_bias, O_RDWR);
415        if (accel_x_dmp_bias_fd == -1 ||
416                 accel_y_dmp_bias_fd == -1 || accel_z_dmp_bias_fd == -1) {
417            LOGE("HAL:could not open accel DMP calibrated bias");
418        } else {
419            LOGV_IF(EXTRA_VERBOSE,
420                 "HAL:accel_dmp_bias opened");
421        }
422    }
423
424    dmp_sign_motion_fd = open(mpu.event_smd, O_RDONLY | O_NONBLOCK);
425    if (dmp_sign_motion_fd < 0) {
426        LOGE("HAL:ERR couldn't open dmp_sign_motion node");
427    } else {
428        LOGV_IF(PROCESS_VERBOSE, "HAL:dmp_sign_motion_fd opened : %d", dmp_sign_motion_fd);
429    }
430
431    dmp_pedometer_fd = open(mpu.event_pedometer, O_RDONLY | O_NONBLOCK);
432    if (dmp_pedometer_fd < 0) {
433        LOGE("HAL:ERR couldn't open dmp_pedometer node");
434    } else {
435        LOGV_IF(PROCESS_VERBOSE, "HAL:dmp_pedometer_fd opened : %d", dmp_pedometer_fd);
436    }
437
438    initBias();
439
440    (void)inv_get_version(&ver_str);
441    LOGV_IF(PROCESS_VERBOSE, "%s\n", ver_str);
442
443    /* setup MPL */
444    inv_constructor_init();
445
446#ifdef INV_PLAYBACK_DBG
447    LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging");
448    logfile = fopen("/data/playback.bin", "w+");
449    if (logfile)
450        inv_turn_on_data_logging(logfile);
451#endif
452
453    /* setup orientation matrix and scale */
454    inv_set_device_properties();
455
456    /* initialize sensor data */
457    memset(mPendingEvents, 0, sizeof(mPendingEvents));
458
459    mPendingEvents[RotationVector].version = sizeof(sensors_event_t);
460    mPendingEvents[RotationVector].sensor = ID_RV;
461    mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR;
462    mPendingEvents[RotationVector].acceleration.status
463            = SENSOR_STATUS_ACCURACY_HIGH;
464
465    mPendingEvents[GameRotationVector].version = sizeof(sensors_event_t);
466    mPendingEvents[GameRotationVector].sensor = ID_GRV;
467    mPendingEvents[GameRotationVector].type = SENSOR_TYPE_GAME_ROTATION_VECTOR;
468    mPendingEvents[GameRotationVector].acceleration.status
469            = SENSOR_STATUS_ACCURACY_HIGH;
470
471    mPendingEvents[LinearAccel].version = sizeof(sensors_event_t);
472    mPendingEvents[LinearAccel].sensor = ID_LA;
473    mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
474    mPendingEvents[LinearAccel].acceleration.status
475            = SENSOR_STATUS_ACCURACY_HIGH;
476
477    mPendingEvents[Gravity].version = sizeof(sensors_event_t);
478    mPendingEvents[Gravity].sensor = ID_GR;
479    mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
480    mPendingEvents[Gravity].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
481
482    mPendingEvents[Gyro].version = sizeof(sensors_event_t);
483    mPendingEvents[Gyro].sensor = ID_GY;
484    mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
485    mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
486
487    mPendingEvents[RawGyro].version = sizeof(sensors_event_t);
488    mPendingEvents[RawGyro].sensor = ID_RG;
489    mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED;
490    mPendingEvents[RawGyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
491
492    mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
493    mPendingEvents[Accelerometer].sensor = ID_A;
494    mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
495    mPendingEvents[Accelerometer].acceleration.status
496            = SENSOR_STATUS_ACCURACY_HIGH;
497
498    /* Invensense compass calibration */
499    mPendingEvents[MagneticField].version = sizeof(sensors_event_t);
500    mPendingEvents[MagneticField].sensor = ID_M;
501    mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD;
502    mPendingEvents[MagneticField].magnetic.status =
503        SENSOR_STATUS_ACCURACY_HIGH;
504
505    mPendingEvents[RawMagneticField].version = sizeof(sensors_event_t);
506    mPendingEvents[RawMagneticField].sensor = ID_RM;
507    mPendingEvents[RawMagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED;
508    mPendingEvents[RawMagneticField].magnetic.status =
509        SENSOR_STATUS_ACCURACY_HIGH;
510
511    mPendingEvents[Pressure].version = sizeof(sensors_event_t);
512    mPendingEvents[Pressure].sensor = ID_PS;
513    mPendingEvents[Pressure].type = SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED;
514    mPendingEvents[Pressure].magnetic.status =
515        SENSOR_STATUS_ACCURACY_HIGH;
516
517    mPendingEvents[Orientation].version = sizeof(sensors_event_t);
518    mPendingEvents[Orientation].sensor = ID_O;
519    mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION;
520    mPendingEvents[Orientation].orientation.status
521            = SENSOR_STATUS_ACCURACY_HIGH;
522
523    mPendingEvents[GeomagneticRotationVector].version = sizeof(sensors_event_t);
524    mPendingEvents[GeomagneticRotationVector].sensor = ID_GMRV;
525    mPendingEvents[GeomagneticRotationVector].type
526            = SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR;
527    mPendingEvents[GeomagneticRotationVector].acceleration.status
528            = SENSOR_STATUS_ACCURACY_HIGH;
529
530#ifndef KLP
531    mHandlers[RotationVector] = &MPLSensor::rvHandler;
532#else
533    mHandlers[RotationVector] = &MPLSensor::grvHandler;
534#endif
535    mHandlers[GameRotationVector] = &MPLSensor::grvHandler;
536    mHandlers[LinearAccel] = &MPLSensor::laHandler;
537    mHandlers[Gravity] = &MPLSensor::gravHandler;
538#ifndef KLP
539    mHandlers[Gyro] = &MPLSensor::gyroHandler;
540#else
541    mHandlers[Gyro] = &MPLSensor::rawGyroHandler;
542#endif
543    mHandlers[RawGyro] = &MPLSensor::rawGyroHandler;
544    mHandlers[Accelerometer] = &MPLSensor::accelHandler;
545#ifndef KLP
546    mHandlers[MagneticField] = &MPLSensor::compassHandler;
547#else
548    mHandlers[MagneticField] = &MPLSensor::rawCompassHandler;
549#endif
550    mHandlers[RawMagneticField] = &MPLSensor::rawCompassHandler;
551    mHandlers[Orientation] = &MPLSensor::orienHandler;
552    mHandlers[GeomagneticRotationVector] = &MPLSensor::gmHandler;
553    mHandlers[Pressure] = &MPLSensor::psHandler;
554
555    for (int i = 0; i < NumSensors; i++) {
556        mDelays[i] = 1000000000LL;
557        mBatchDelays[i] = 1000000000LL;
558        mBatchTimeouts[i] = 30000000000LL;
559    }
560
561    /* initialize Compass Bias */
562    memset(mCompassBias, 0, sizeof(mCompassBias));
563
564    /* initialize Factory Accel Bias */
565    memset(mFactoryAccelBias, 0, sizeof(mFactoryAccelBias));
566
567    /* initialize Gyro Bias */
568    memset(mGyroBias, 0, sizeof(mGyroBias));
569    memset(mGyroChipBias, 0, sizeof(mGyroChipBias));
570
571    /* load calibration file from /data/inv_cal_data.bin */
572    rv = inv_load_calibration();
573    if(rv == INV_SUCCESS) {
574        LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded");
575        /* Get initial values */
576        getCompassBias();
577        getGyroBias();
578        getAccelBias();
579        getFactoryGyroBias();
580        if (mFactoryGyroBiasAvailable) {
581            setFactoryGyroBias();
582        }
583        /* disabled because no request for factory cal accel data yet */
584        //getFactoryAccelBias();
585    }
586    else
587        LOGE("HAL:Could not open or load MPL calibration file (%d)", rv);
588
589    /* takes external accel calibration load workflow */
590    if( m_pt2AccelCalLoadFunc != NULL) {
591        long accel_offset[3];
592        long tmp_offset[3];
593        int result = m_pt2AccelCalLoadFunc(accel_offset);
594        if(result)
595            LOGW("HAL:Vendor accelerometer calibration file load failed %d\n",
596                 result);
597        else {
598            LOGW("HAL:Vendor accelerometer calibration file successfully "
599                 "loaded");
600            inv_get_accel_bias(tmp_offset, NULL);
601            LOGV_IF(PROCESS_VERBOSE,
602                    "HAL:Original accel offset, %ld, %ld, %ld\n",
603               tmp_offset[0], tmp_offset[1], tmp_offset[2]);
604            inv_set_accel_bias(accel_offset, mAccelAccuracy);
605            inv_get_accel_bias(tmp_offset, NULL);
606            LOGV_IF(PROCESS_VERBOSE, "HAL:Set accel offset, %ld, %ld, %ld\n",
607               tmp_offset[0], tmp_offset[1], tmp_offset[2]);
608        }
609    }
610    /* end of external accel calibration load workflow */
611
612    /* disable driver master enable the first sensor goes on */
613    masterEnable(0);
614    enableGyro(0);
615    enableAccel(0);
616    enableCompass(0,0);
617    enablePressure(0);
618    enableBatch(0);
619
620    if (isLowPowerQuatEnabled()) {
621        enableLPQuaternion(0);
622    }
623
624    if (isDmpDisplayOrientationOn()) {
625        // open DMP Orient Fd
626        openDmpOrientFd();
627        enableDmpOrientation(!isDmpScreenAutoRotationEnabled());
628    }
629}
630
631void MPLSensor::enable_iio_sysfs(void)
632{
633    VFUNC_LOG;
634
635    char iio_device_node[MAX_CHIP_ID_LEN];
636    FILE *tempFp = NULL;
637
638    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)",
639            mpu.in_timestamp_en, getTimestamp());
640    // Either fopen()/open() are okay for sysfs access
641    // developers could choose what they want
642    // with fopen(), the benefit is that fprintf()/fscanf() are available
643    tempFp = fopen(mpu.in_timestamp_en, "w");
644    if (tempFp == NULL) {
645        LOGE("HAL:could not open timestamp enable");
646    } else {
647        if(fprintf(tempFp, "%d", 1) < 0 || fclose(tempFp) < 0) {
648            LOGE("HAL:could not enable timestamp");
649        }
650    }
651
652    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
653            IIO_BUFFER_LENGTH, mpu.buffer_length, getTimestamp());
654    tempFp = fopen(mpu.buffer_length, "w");
655    if (tempFp == NULL) {
656        LOGE("HAL:could not open buffer length");
657    } else {
658        if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0 || fclose(tempFp) < 0) {
659            LOGE("HAL:could not write buffer length");
660        }
661    }
662
663    inv_get_iio_device_node(iio_device_node);
664    iio_fd = open(iio_device_node, O_RDONLY);
665    if (iio_fd < 0) {
666        LOGE("HAL:could not open iio device node");
667    } else {
668        LOGV_IF(PROCESS_VERBOSE, "HAL:iio iio_fd opened : %d", iio_fd);
669    }
670}
671
672int MPLSensor::inv_constructor_init(void)
673{
674    VFUNC_LOG;
675
676    inv_error_t result = inv_init_mpl();
677    if (result) {
678        LOGE("HAL:inv_init_mpl() failed");
679        return result;
680    }
681    result = inv_constructor_default_enable();
682    result = inv_start_mpl();
683    if (result) {
684        LOGE("HAL:inv_start_mpl() failed");
685        LOG_RESULT_LOCATION(result);
686        return result;
687    }
688
689    return result;
690}
691
692int MPLSensor::inv_constructor_default_enable(void)
693{
694    VFUNC_LOG;
695
696    inv_error_t result;
697
698/*******************************************************************************
699
700********************************************************************************
701
702The InvenSense binary file (libmplmpu.so) is subject to Google's standard terms
703and conditions as accepted in the click-through agreement required to download
704this library.
705The library includes, but is not limited to the following function calls:
706inv_enable_quaternion().
707
708ANY VIOLATION OF SUCH TERMS AND CONDITIONS WILL BE STRICTLY ENFORCED.
709
710********************************************************************************
711
712*******************************************************************************/
713
714    result = inv_enable_quaternion();
715    if (result) {
716        LOGE("HAL:Cannot enable quaternion\n");
717        return result;
718    }
719
720    result = inv_enable_in_use_auto_calibration();
721    if (result) {
722        return result;
723    }
724
725    result = inv_enable_fast_nomot();
726    if (result) {
727        return result;
728    }
729
730    result = inv_enable_gyro_tc();
731    if (result) {
732        return result;
733    }
734
735    result = inv_enable_hal_outputs();
736    if (result) {
737        return result;
738    }
739
740    if (!mCompassSensor->providesCalibration()) {
741        /* Invensense compass calibration */
742        LOGV_IF(PROCESS_VERBOSE, "HAL:Invensense vector compass cal enabled");
743        result = inv_enable_vector_compass_cal();
744        if (result) {
745            LOG_RESULT_LOCATION(result);
746            return result;
747        } else {
748            mMplFeatureActiveMask |= INV_COMPASS_CAL;
749        }
750        // specify MPL's trust weight, used by compass algorithms
751        inv_vector_compass_cal_sensitivity(3);
752
753        /* disabled by default
754        result = inv_enable_compass_bias_w_gyro();
755        if (result) {
756            LOG_RESULT_LOCATION(result);
757            return result;
758        }
759        */
760
761        result = inv_enable_heading_from_gyro();
762        if (result) {
763            LOG_RESULT_LOCATION(result);
764            return result;
765        }
766
767        result = inv_enable_magnetic_disturbance();
768        if (result) {
769            LOG_RESULT_LOCATION(result);
770            return result;
771        }
772        //inv_enable_magnetic_disturbance_logging();
773    }
774
775    result = inv_enable_9x_sensor_fusion();
776    if (result) {
777        LOG_RESULT_LOCATION(result);
778        return result;
779    } else {
780        // 9x sensor fusion enables Compass fit
781        mMplFeatureActiveMask |= INV_COMPASS_FIT;
782    }
783
784    result = inv_enable_no_gyro_fusion();
785    if (result) {
786        LOG_RESULT_LOCATION(result);
787        return result;
788    }
789
790    return result;
791}
792
793/* TODO: create function pointers to calculate scale */
794void MPLSensor::inv_set_device_properties(void)
795{
796    VFUNC_LOG;
797
798    unsigned short orient;
799
800    inv_get_sensors_orientation();
801
802    inv_set_gyro_sample_rate(DEFAULT_MPL_GYRO_RATE);
803    inv_set_compass_sample_rate(DEFAULT_MPL_COMPASS_RATE);
804
805    /* gyro setup */
806    orient = inv_orientation_matrix_to_scalar(mGyroOrientation);
807    inv_set_gyro_orientation_and_scale(orient, mGyroScale << 15);
808    LOGI_IF(EXTRA_VERBOSE, "HAL: Set MPL Gyro Scale %ld", mGyroScale << 15);
809
810    /* accel setup */
811    orient = inv_orientation_matrix_to_scalar(mAccelOrientation);
812    /* use for third party accel input subsystem driver
813    inv_set_accel_orientation_and_scale(orient, 1LL << 22);
814    */
815    inv_set_accel_orientation_and_scale(orient, (long)mAccelScale << 15);
816    LOGI_IF(EXTRA_VERBOSE,
817            "HAL: Set MPL Accel Scale %ld", (long)mAccelScale << 15);
818
819    /* compass setup */
820    signed char orientMtx[9];
821    mCompassSensor->getOrientationMatrix(orientMtx);
822    orient =
823        inv_orientation_matrix_to_scalar(orientMtx);
824    long sensitivity;
825    sensitivity = mCompassSensor->getSensitivity();
826    inv_set_compass_orientation_and_scale(orient, sensitivity);
827    mCompassScale = sensitivity;
828    LOGI_IF(EXTRA_VERBOSE,
829            "HAL: Set MPL Compass Scale %ld", mCompassScale);
830}
831
832void MPLSensor::loadDMP(void)
833{
834    int res, fd;
835    FILE *fptr;
836
837    if (isMpuNonDmp()) {
838        //DMP support only for MPU6xxx/9xxx currently
839        return;
840    }
841
842    /* load DMP firmware */
843    LOGV_IF(SYSFS_VERBOSE,
844            "HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp());
845    fd = open(mpu.firmware_loaded, O_RDONLY);
846    if(fd < 0) {
847        LOGE("HAL:could not open dmp state");
848    } else {
849        if(inv_read_dmp_state(fd) == 0) {
850            LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware);
851            fptr = fopen(mpu.dmp_firmware, "w");
852            if(fptr == NULL) {
853                LOGE("HAL:could not write to dmp");
854            } else {
855                if (inv_load_dmp(fptr) < 0 || fclose(fptr) < 0) {
856                    LOGE("HAL:load DMP failed");
857                } else {
858                    fclose(fptr);
859                    LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded");
860                };
861            }
862        } else {
863            LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is already loaded");
864        }
865    }
866
867    // onDmp(1);    //Can't enable here. See note onDmp()
868}
869
870void MPLSensor::inv_get_sensors_orientation(void)
871{
872    FILE *fptr;
873
874    // get gyro orientation
875    LOGV_IF(SYSFS_VERBOSE,
876            "HAL:sysfs:cat %s (%lld)", mpu.gyro_orient, getTimestamp());
877    fptr = fopen(mpu.gyro_orient, "r");
878    if (fptr != NULL) {
879        int om[9];
880        fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
881               &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
882               &om[6], &om[7], &om[8]);
883        fclose(fptr);
884
885        LOGV_IF(EXTRA_VERBOSE,
886                "HAL:gyro mounting matrix: "
887                "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
888                om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
889
890        mGyroOrientation[0] = om[0];
891        mGyroOrientation[1] = om[1];
892        mGyroOrientation[2] = om[2];
893        mGyroOrientation[3] = om[3];
894        mGyroOrientation[4] = om[4];
895        mGyroOrientation[5] = om[5];
896        mGyroOrientation[6] = om[6];
897        mGyroOrientation[7] = om[7];
898        mGyroOrientation[8] = om[8];
899    } else {
900        LOGE("HAL:Couldn't read gyro mounting matrix");
901    }
902
903    // get accel orientation
904    LOGV_IF(SYSFS_VERBOSE,
905            "HAL:sysfs:cat %s (%lld)", mpu.accel_orient, getTimestamp());
906    fptr = fopen(mpu.accel_orient, "r");
907    if (fptr != NULL) {
908        int om[9];
909        fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
910               &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
911               &om[6], &om[7], &om[8]);
912        fclose(fptr);
913
914        LOGV_IF(EXTRA_VERBOSE,
915                "HAL:accel mounting matrix: "
916                "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
917                om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
918
919        mAccelOrientation[0] = om[0];
920        mAccelOrientation[1] = om[1];
921        mAccelOrientation[2] = om[2];
922        mAccelOrientation[3] = om[3];
923        mAccelOrientation[4] = om[4];
924        mAccelOrientation[5] = om[5];
925        mAccelOrientation[6] = om[6];
926        mAccelOrientation[7] = om[7];
927        mAccelOrientation[8] = om[8];
928    } else {
929        LOGE("HAL:Couldn't read accel mounting matrix");
930    }
931}
932
933MPLSensor::~MPLSensor()
934{
935    VFUNC_LOG;
936
937    /* Close open fds */
938    if (iio_fd > 0)
939        close(iio_fd);
940    if( accel_fd > 0 )
941        close(accel_fd );
942    if (gyro_temperature_fd > 0)
943        close(gyro_temperature_fd);
944    if (sysfs_names_ptr)
945        free(sysfs_names_ptr);
946
947    closeDmpOrientFd();
948
949    if (accel_x_dmp_bias_fd > 0) {
950        close(accel_x_dmp_bias_fd);
951    }
952    if (accel_y_dmp_bias_fd > 0) {
953        close(accel_y_dmp_bias_fd);
954    }
955    if (accel_z_dmp_bias_fd > 0) {
956        close(accel_z_dmp_bias_fd);
957    }
958
959    if (gyro_x_dmp_bias_fd > 0) {
960        close(gyro_x_dmp_bias_fd);
961    }
962    if (gyro_y_dmp_bias_fd > 0) {
963        close(gyro_y_dmp_bias_fd);
964    }
965    if (gyro_z_dmp_bias_fd > 0) {
966        close(gyro_z_dmp_bias_fd);
967    }
968
969    if (gyro_x_offset_fd > 0) {
970        close(gyro_x_dmp_bias_fd);
971    }
972    if (gyro_y_offset_fd > 0) {
973        close(gyro_y_offset_fd);
974    }
975    if (gyro_z_offset_fd > 0) {
976        close(accel_z_offset_fd);
977    }
978
979    /* Turn off Gyro master enable          */
980    /* A workaround until driver handles it */
981    /* TODO: Turn off and close all sensors */
982    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
983            0, mpu.chip_enable, getTimestamp());
984    write_sysfs_int(mpu.chip_enable, 0);
985
986#ifdef INV_PLAYBACK_DBG
987    inv_turn_off_data_logging();
988    fclose(logfile);
989#endif
990}
991
992#define GY_ENABLED  ((1 << ID_GY) & enabled_sensors)
993#define RGY_ENABLED ((1 << ID_RG) & enabled_sensors)
994#define A_ENABLED   ((1 << ID_A)  & enabled_sensors)
995#define M_ENABLED   ((1 << ID_M) & enabled_sensors)
996#define RM_ENABLED  ((1 << ID_RM) & enabled_sensors)
997#define PS_ENABLED  ((1 << ID_PS) & enabled_sensors)
998#define O_ENABLED   ((1 << ID_O)  & enabled_sensors)
999#define LA_ENABLED  ((1 << ID_LA) & enabled_sensors)
1000#define GR_ENABLED  ((1 << ID_GR) & enabled_sensors)
1001#define RV_ENABLED  ((1 << ID_RV) & enabled_sensors)
1002#define GRV_ENABLED ((1 << ID_GRV) & enabled_sensors)
1003#define GMRV_ENABLED ((1 << ID_GMRV) & enabled_sensors)
1004
1005/* TODO: this step is optional, remove?  */
1006int MPLSensor::setGyroInitialState(void)
1007{
1008    VFUNC_LOG;
1009
1010    int res = 0;
1011
1012    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1013            HW_GYRO_RATE_HZ, mpu.gyro_rate, getTimestamp());
1014    int fd = open(mpu.gyro_rate, O_RDWR);
1015    res = errno;
1016    if(fd < 0) {
1017        LOGE("HAL:open of %s failed with '%s' (%d)",
1018             mpu.gyro_rate, strerror(res), res);
1019        return res;
1020    }
1021    res = write_attribute_sensor(fd, HW_GYRO_RATE_HZ);
1022    if(res < 0) {
1023        LOGE("HAL:write_attribute_sensor : error writing %s with %d",
1024             mpu.gyro_rate, HW_GYRO_RATE_HZ);
1025        return res;
1026    }
1027
1028    // Setting LPF is deprecated
1029
1030    return 0;
1031}
1032
1033/* this applies to BMA250 Input Subsystem Driver only */
1034int MPLSensor::setAccelInitialState()
1035{
1036    VFUNC_LOG;
1037
1038    struct input_absinfo absinfo_x;
1039    struct input_absinfo absinfo_y;
1040    struct input_absinfo absinfo_z;
1041    float value;
1042    if (!ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo_x) &&
1043        !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo_y) &&
1044        !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo_z)) {
1045        value = absinfo_x.value;
1046        mPendingEvents[Accelerometer].data[0] = value * CONVERT_A_X;
1047        value = absinfo_y.value;
1048        mPendingEvents[Accelerometer].data[1] = value * CONVERT_A_Y;
1049        value = absinfo_z.value;
1050        mPendingEvents[Accelerometer].data[2] = value * CONVERT_A_Z;
1051        //mHasPendingEvent = true;
1052    }
1053    return 0;
1054}
1055
1056int MPLSensor::onDmp(int en)
1057{
1058    VFUNC_LOG;
1059
1060    int res = -1;
1061    int status;
1062    mDmpOn = en;
1063
1064    //Sequence to enable DMP
1065    //1. Load DMP image if not already loaded
1066    //2. Either Gyro or Accel must be enabled/configured before next step
1067    //3. Enable DMP
1068
1069    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
1070            mpu.firmware_loaded, getTimestamp());
1071    if(read_sysfs_int(mpu.firmware_loaded, &status) < 0){
1072        LOGE("HAL:ERR can't get firmware_loaded status");
1073    } else if (status == 1) {
1074        //Write only if curr DMP state <> request
1075        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
1076                mpu.dmp_on, getTimestamp());
1077        if (read_sysfs_int(mpu.dmp_on, &status) < 0) {
1078            LOGE("HAL:ERR can't read DMP state");
1079        } else if (status != en) {
1080            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1081                    en, mpu.dmp_on, getTimestamp());
1082            if (write_sysfs_int(mpu.dmp_on, en) < 0) {
1083                LOGE("HAL:ERR can't write dmp_on");
1084            } else {
1085                mDmpOn = en;
1086                res = 0;	//Indicate write successful
1087                if(!en) {
1088                    setAccelBias();
1089                }
1090            }
1091            //Enable DMP interrupt
1092            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1093                    en, mpu.dmp_int_on, getTimestamp());
1094            if (write_sysfs_int(mpu.dmp_int_on, en) < 0) {
1095                LOGE("HAL:ERR can't en/dis DMP interrupt");
1096            }
1097
1098            // disable DMP event interrupt if needed
1099            if (!en) {
1100                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1101                        en, mpu.dmp_event_int_on, getTimestamp());
1102                if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
1103                    res = -1;
1104                    LOGE("HAL:ERR can't enable DMP event interrupt");
1105                }
1106            }
1107        } else {
1108            mDmpOn = en;
1109            res = 0;  	//DMP already set as requested
1110            if(!en) {
1111                setAccelBias();
1112            }
1113        }
1114    } else {
1115        LOGE("HAL:ERR No DMP image");
1116    }
1117    return res;
1118}
1119
1120/* called when batch and hw sensor enabled*/
1121int MPLSensor::enablePedIndicator(int en)
1122{
1123    VFUNC_LOG;
1124
1125    int res = 0;
1126    if (en) {
1127        if (!(mFeatureActiveMask & INV_DMP_PED_QUATERNION)) {
1128            //Disable DMP Pedometer Interrupt
1129            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1130                        0, mpu.pedometer_int_on, getTimestamp());
1131            if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) {
1132               LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
1133               res = -1;   // indicate an err
1134               return res;
1135            }
1136        }
1137    }
1138
1139    LOGV_IF(PROCESS_VERBOSE, "HAL:Toggling step indicator to %d", en);
1140    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1141                en, mpu.step_indicator_on, getTimestamp());
1142    if (write_sysfs_int(mpu.step_indicator_on, en) < 0) {
1143        res = -1;
1144        LOGE("HAL:ERR can't write to DMP step_indicator_on");
1145    }
1146    return res;
1147}
1148
1149int MPLSensor::checkPedStandaloneEnabled(void)
1150{
1151    return ((mFeatureActiveMask & INV_DMP_PED_STANDALONE)? 1:0);
1152
1153}
1154
1155/* This feature is only used in batch mode */
1156/* Stand-alone Step Detector */
1157int MPLSensor::enablePedStandalone(int en)
1158{
1159    VFUNC_LOG;
1160
1161    if (!en) {
1162        enablePedStandaloneData(0);
1163        mFeatureActiveMask &= ~INV_DMP_PED_STANDALONE;
1164        if (mFeatureActiveMask == 0) {
1165            onDmp(0);
1166        } else if(mFeatureActiveMask & INV_DMP_PEDOMETER) {
1167             //Re-enable DMP Pedometer Interrupt
1168             LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1169                     1, mpu.pedometer_int_on, getTimestamp());
1170             if (write_sysfs_int(mpu.pedometer_int_on, 1) < 0) {
1171                 LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
1172                 return (-1);
1173             }
1174            //Disable data interrupt if no continuous data
1175            if (mEnabled == 0) {
1176                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1177                       1, mpu.dmp_event_int_on, getTimestamp());
1178                if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) {
1179                    LOGE("HAL:ERR can't enable DMP event interrupt");
1180                    return (-1);
1181                }
1182            }
1183        }
1184        LOGV_IF(PROCESS_VERBOSE, "HAL:Ped Standalone disabled");
1185    } else {
1186        if (enablePedStandaloneData(1) < 0 || onDmp(1) < 0) {
1187            LOGE("HAL:ERR can't enable Ped Standalone");
1188        } else {
1189            mFeatureActiveMask |= INV_DMP_PED_STANDALONE;
1190            //Disable DMP Pedometer Interrupt
1191            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1192                    0, mpu.pedometer_int_on, getTimestamp());
1193            if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) {
1194                LOGE("HAL:ERR can't disable Android Pedometer Interrupt");
1195                return (-1);
1196            }
1197            //Enable Data Interrupt
1198            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1199                       0, mpu.dmp_event_int_on, getTimestamp());
1200            if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
1201                LOGE("HAL:ERR can't enable DMP event interrupt");
1202                return (-1);
1203            }
1204            LOGV_IF(PROCESS_VERBOSE, "HAL:Ped Standalone enabled");
1205        }
1206    }
1207    return 0;
1208}
1209
1210int MPLSensor:: enablePedStandaloneData(int en)
1211{
1212    VFUNC_LOG;
1213
1214    int res = 0;
1215
1216    // Enable DMP Ped standalone
1217    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1218            en, mpu.step_detector_on, getTimestamp());
1219    if (write_sysfs_int(mpu.step_detector_on, en) < 0) {
1220        LOGE("HAL:ERR can't write DMP step_detector_on");
1221        res = -1;   //Indicate an err
1222    }
1223
1224    // Disable DMP Step indicator
1225    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1226            en, mpu.step_indicator_on, getTimestamp());
1227    if (write_sysfs_int(mpu.step_indicator_on, en) < 0) {
1228        LOGE("HAL:ERR can't write DMP step_indicator_on");
1229        res = -1;   //Indicate an err
1230    }
1231
1232    if (!en) {
1233      LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling ped standalone");
1234      //Disable Accel if no sensor needs it
1235      if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
1236                               && (!(mLocalSensorMask & mMasterSensorMask
1237                                                   & INV_THREE_AXIS_ACCEL))) {
1238          res = enableAccel(0);
1239          if (res < 0)
1240              return res;
1241      }
1242      if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
1243                               && (!(mLocalSensorMask & mMasterSensorMask
1244                                                   & INV_THREE_AXIS_GYRO))) {
1245          res = enableGyro(0);
1246          if (res < 0)
1247              return res;
1248      }
1249    } else {
1250        LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling ped standalone");
1251        // enable accel engine
1252        res = enableAccel(1);
1253        if (res < 0)
1254            return res;
1255        LOGV_IF(EXTRA_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask);
1256        // disable accel FIFO
1257        if (!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_ACCEL)) {
1258            res = turnOffAccelFifo();
1259            if (res < 0)
1260                return res;
1261        }
1262    }
1263
1264    return res;
1265}
1266
1267int MPLSensor::checkPedQuatEnabled(void)
1268{
1269    return ((mFeatureActiveMask & INV_DMP_PED_QUATERNION)? 1:0);
1270}
1271
1272/* This feature is only used in batch mode */
1273/* Step Detector && Game Rotation Vector */
1274int MPLSensor::enablePedQuaternion(int en)
1275{
1276    VFUNC_LOG;
1277
1278    if (!en) {
1279        enablePedQuaternionData(0);
1280        mFeatureActiveMask &= ~INV_DMP_PED_QUATERNION;
1281        if (mFeatureActiveMask == 0) {
1282            onDmp(0);
1283        } else if(mFeatureActiveMask & INV_DMP_PEDOMETER) {
1284             //Re-enable DMP Pedometer Interrupt
1285             LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1286                     1, mpu.pedometer_int_on, getTimestamp());
1287             if (write_sysfs_int(mpu.pedometer_int_on, 1) < 0) {
1288                 LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
1289                 return (-1);
1290             }
1291            //Disable data interrupt if no continuous data
1292            if (mEnabled == 0) {
1293                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1294                       1, mpu.dmp_event_int_on, getTimestamp());
1295                if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
1296                    LOGE("HAL:ERR can't enable DMP event interrupt");
1297                    return (-1);
1298                }
1299            }
1300        }
1301        LOGV_IF(PROCESS_VERBOSE, "HAL:Ped Quat disabled");
1302    } else {
1303        if (enablePedQuaternionData(1) < 0 || onDmp(1) < 0) {
1304            LOGE("HAL:ERR can't enable Ped Quaternion");
1305        } else {
1306            mFeatureActiveMask |= INV_DMP_PED_QUATERNION;
1307            //Disable DMP Pedometer Interrupt
1308            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1309                    0, mpu.pedometer_int_on, getTimestamp());
1310            if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) {
1311                LOGE("HAL:ERR can't disable Android Pedometer Interrupt");
1312                return (-1);
1313            }
1314            //Enable Data Interrupt
1315            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1316                       0, mpu.dmp_event_int_on, getTimestamp());
1317            if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
1318                LOGE("HAL:ERR can't enable DMP event interrupt");
1319                return (-1);
1320            }
1321            LOGV_IF(PROCESS_VERBOSE, "HAL:Ped Quat enabled");
1322        }
1323    }
1324    return 0;
1325}
1326
1327int MPLSensor::enablePedQuaternionData(int en)
1328{
1329    VFUNC_LOG;
1330
1331    int res = 0;
1332
1333    // Enable DMP quaternion
1334    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1335            en, mpu.ped_q_on, getTimestamp());
1336    if (write_sysfs_int(mpu.ped_q_on, en) < 0) {
1337        LOGE("HAL:ERR can't write DMP ped_q_on");
1338        res = -1;   //Indicate an err
1339    }
1340
1341    // toggle DMP step indicator
1342    /*LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1343            en, mpu.step_indicator_on, getTimestamp());
1344    if (write_sysfs_int(mpu.step_indicator_on, en) < 0) {
1345        LOGE("HAL:ERR can't write DMP step_indicator_on");
1346        res = -1;   //Indicate an err
1347    }*/
1348
1349    if (!en) {
1350        LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling ped quat");
1351        //Disable Accel if no sensor needs it
1352        if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
1353                               && (!(mLocalSensorMask & mMasterSensorMask
1354                                                   & INV_THREE_AXIS_ACCEL))) {
1355          res = enableAccel(0);
1356          if (res < 0)
1357              return res;
1358        }
1359        if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
1360                               && (!(mLocalSensorMask & mMasterSensorMask
1361                                                   & INV_THREE_AXIS_GYRO))) {
1362            res = enableGyro(0);
1363            if (res < 0)
1364                return res;
1365        }
1366      if (mFeatureActiveMask & INV_DMP_QUATERNION) {
1367            res = write_sysfs_int(mpu.gyro_fifo_enable, 1);
1368            res += write_sysfs_int(mpu.accel_fifo_enable, 1);
1369            if (res < 0)
1370                return res;
1371      }
1372//LOGV("before mLocalSensorMask=0x%lx", mLocalSensorMask);
1373      // reset global mask for buildMpuEvent()
1374      if (mEnabled & (1 << GameRotationVector)) {
1375          mLocalSensorMask |= INV_THREE_AXIS_GYRO;
1376          mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
1377      } else if (mEnabled & (1 << Accelerometer)) {
1378          mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
1379      } else if ((mEnabled & ( 1 << Gyro)) ||
1380          (mEnabled & (1 << RawGyro))) {
1381          mLocalSensorMask |= INV_THREE_AXIS_GYRO;
1382      }
1383//LOGV("after mLocalSensorMask=0x%lx", mLocalSensorMask);
1384    } else {
1385        LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling ped quat");
1386        // enable accel engine
1387        res = enableAccel(1);
1388        if (res < 0)
1389            return res;
1390
1391        // enable gyro engine
1392        res = enableGyro(1);
1393        if (res < 0)
1394            return res;
1395        LOGV_IF(EXTRA_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask);
1396        // disable accel FIFO
1397        if ((!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_ACCEL)) ||
1398                !(mBatchEnabled & (1 << Accelerometer))) {
1399            res = turnOffAccelFifo();
1400            if (res < 0)
1401                return res;
1402            mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
1403        }
1404
1405        // disable gyro FIFO
1406        if ((!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_GYRO)) ||
1407                !((mBatchEnabled & (1 << Gyro)) || (mBatchEnabled & (1 << RawGyro)))) {
1408            res = turnOffGyroFifo();
1409            if (res < 0)
1410                return res;
1411            mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
1412        }
1413    }
1414
1415    return res;
1416}
1417
1418int MPLSensor::check6AxisQuatEnabled(void)
1419{
1420    return ((mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION)? 1:0);
1421}
1422
1423/* This is used for batch mode only */
1424/* GRV is batched but not along with ped */
1425int MPLSensor::enable6AxisQuaternion(int en)
1426{
1427    VFUNC_LOG;
1428
1429    if (!en) {
1430        enable6AxisQuaternionData(0);
1431        mFeatureActiveMask &= ~INV_DMP_6AXIS_QUATERNION;
1432        if (mFeatureActiveMask == 0) {
1433            onDmp(0);
1434        }
1435        LOGV_IF(PROCESS_VERBOSE, "HAL:6 Axis Quat disabled");
1436    } else {
1437        if (enable6AxisQuaternionData(1) < 0 || onDmp(1) < 0) {
1438            LOGE("HAL:ERR can't enable 6 Axis Quaternion");
1439        } else {
1440            mFeatureActiveMask |= INV_DMP_6AXIS_QUATERNION;
1441            LOGV_IF(PROCESS_VERBOSE, "HAL:6 Axis Quat enabled");
1442        }
1443    }
1444    return 0;
1445}
1446
1447int MPLSensor::enable6AxisQuaternionData(int en)
1448{
1449    int res = 0;
1450    VFUNC_LOG;
1451
1452    // Enable DMP quaternion
1453    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1454            en, mpu.six_axis_q_on, getTimestamp());
1455    if (write_sysfs_int(mpu.six_axis_q_on, en) < 0) {
1456        LOGE("HAL:ERR can't write DMP six_axis_q_on");
1457        res = -1;   //Indicate an err
1458    }
1459
1460    if (!en) {
1461        LOGV_IF(EXTRA_VERBOSE, "HAL:DMP six axis quaternion data was turned off");
1462        if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
1463                                 && (!(mLocalSensorMask & mMasterSensorMask
1464                                                     & INV_THREE_AXIS_ACCEL))) {
1465            res = enableAccel(0);
1466            if (res < 0)
1467                return res;
1468        }
1469        if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
1470                                 && (!(mLocalSensorMask & mMasterSensorMask
1471                                                     & INV_THREE_AXIS_GYRO))) {
1472            res = enableGyro(0);
1473            if (res < 0)
1474                return res;
1475        }
1476        if (mFeatureActiveMask & INV_DMP_QUATERNION) {
1477            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1478                    1, mpu.gyro_fifo_enable, getTimestamp());
1479            res = write_sysfs_int(mpu.gyro_fifo_enable, 1);
1480            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1481                    1, mpu.accel_fifo_enable, getTimestamp());
1482            res += write_sysfs_int(mpu.accel_fifo_enable, 1);
1483            if (res < 0)
1484                return res;
1485        }
1486LOGV("before mLocalSensorMask=0x%lx", mLocalSensorMask);
1487        // reset global mask for buildMpuEvent()
1488        if (mEnabled & (1 << GameRotationVector)) {
1489            if (!(mFeatureActiveMask & INV_DMP_PED_QUATERNION)) {
1490                mLocalSensorMask |= INV_THREE_AXIS_GYRO;
1491                mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
1492            }
1493        } else if (mEnabled & (1 << Accelerometer)) {
1494            mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
1495        } else if ((mEnabled & ( 1 << Gyro)) ||
1496                (mEnabled & (1 << RawGyro))) {
1497            mLocalSensorMask |= INV_THREE_AXIS_GYRO;
1498        }
1499LOGV("after mLocalSensorMask=0x%lx", mLocalSensorMask);
1500    } else {
1501        LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling six axis quat");
1502        if (mEnabled & ( 1 << GameRotationVector)) {
1503            // enable accel engine
1504            res = enableAccel(1);
1505            if (res < 0)
1506                return res;
1507
1508            // enable gyro engine
1509            res = enableGyro(1);
1510            if (res < 0)
1511                return res;
1512            LOGV_IF(EXTRA_VERBOSE, "before: mLocalSensorMask=0x%lx", mLocalSensorMask);
1513            if ((!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) ||
1514                   (!(mBatchEnabled & (1 << Accelerometer)) ||
1515                       (!(mEnabled & (1 << Accelerometer))))) {
1516                res = turnOffAccelFifo();
1517                if (res < 0)
1518                    return res;
1519                mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
1520            }
1521
1522            if ((!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_GYRO)) ||
1523                    (!(mBatchEnabled & (1 << Gyro)) ||
1524                       (!(mEnabled & (1 << Gyro))))) {
1525                if (!(mBatchEnabled & (1 << RawGyro)) ||
1526                        (!(mEnabled & (1 << RawGyro)))) {
1527                    res = turnOffGyroFifo();
1528                    if (res < 0)
1529                        return res;
1530                     mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
1531                     }
1532            }
1533LOGV("after: mLocalSensorMask=0x%lx", mLocalSensorMask);
1534        }
1535    }
1536
1537    return res;
1538}
1539
1540int MPLSensor::checkLPQuaternion(void)
1541{
1542    return ((mFeatureActiveMask & INV_DMP_QUATERNION)? 1:0);
1543}
1544
1545int MPLSensor::enableLPQuaternion(int en)
1546{
1547    VFUNC_LOG;
1548
1549    if (!en) {
1550        enableQuaternionData(0);
1551        onDmp(0);
1552        mFeatureActiveMask &= ~INV_DMP_QUATERNION;
1553        LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat disabled");
1554    } else {
1555        if (enableQuaternionData(1) < 0 || onDmp(1) < 0) {
1556            LOGE("HAL:ERR can't enable LP Quaternion");
1557        } else {
1558            mFeatureActiveMask |= INV_DMP_QUATERNION;
1559            LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat enabled");
1560        }
1561    }
1562    return 0;
1563}
1564
1565int MPLSensor::enableQuaternionData(int en)
1566{
1567    int res = 0;
1568    VFUNC_LOG;
1569
1570    // Enable DMP quaternion
1571    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1572            en, mpu.three_axis_q_on, getTimestamp());
1573    if (write_sysfs_int(mpu.three_axis_q_on, en) < 0) {
1574        LOGE("HAL:ERR can't write DMP three_axis_q__on");
1575        res = -1;	//Indicate an err
1576    }
1577
1578    if (!en) {
1579        LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off");
1580        inv_quaternion_sensor_was_turned_off();
1581    } else {
1582        LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling three axis quat");
1583    }
1584
1585    return res;
1586}
1587
1588int MPLSensor::enableDmpPedometer(int en, int interruptMode)
1589{
1590    VFUNC_LOG;
1591    int res = 0;
1592    int enabled_sensors = mEnabled;
1593
1594    if (isMpuNonDmp())
1595        return res;
1596
1597    // reset master enable
1598    res = masterEnable(0);
1599    if (res < 0) {
1600        return res;
1601    }
1602
1603    if (en == 1) {
1604        //Enable DMP Pedometer Function
1605        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1606                en, mpu.pedometer_on, getTimestamp());
1607        if (write_sysfs_int(mpu.pedometer_on, en) < 0) {
1608            LOGE("HAL:ERR can't enable Android Pedometer");
1609            res = -1;   // indicate an err
1610            return res;
1611        }
1612
1613        if (interruptMode || (mFeatureActiveMask & INV_DMP_PEDOMETER)) {
1614            //Enable DMP Pedometer Interrupt
1615            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1616                    en, mpu.pedometer_int_on, getTimestamp());
1617            if (write_sysfs_int(mpu.pedometer_int_on, en) < 0) {
1618                LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
1619                res = -1;   // indicate an err
1620                return res;
1621            }
1622        }
1623        // enable DMP
1624        res = onDmp(1);
1625        if (res < 0) {
1626            return res;
1627        }
1628
1629        // set DMP rate to 200Hz
1630        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1631                200, mpu.accel_fifo_rate, getTimestamp());
1632        if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) {
1633            res = -1;
1634            LOGE("HAL:ERR can't set rate to 200Hz");
1635            return res;
1636        }
1637
1638        // enable accel engine
1639        res = enableAccel(1);
1640        if (res < 0) {
1641            return res;
1642        }
1643
1644        // disable accel FIFO
1645        if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) {
1646            res = turnOffAccelFifo();
1647            if (res < 0)
1648                return res;
1649        }
1650
1651        // disable data interrupt
1652        //if (!batchPed && enabled_sensors == 0) {
1653        if (enabled_sensors == 0) {
1654            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1655                        en, mpu.dmp_event_int_on, getTimestamp());
1656            if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
1657                res = -1;
1658                LOGE("HAL:ERR can't enable DMP event interrupt");
1659            }
1660        }
1661        if (interruptMode) {
1662            mFeatureActiveMask |= INV_DMP_PEDOMETER;
1663            /*if (batchPed) {
1664                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1665                    en, mpu.step_detector_on, getTimestamp());
1666                if (write_sysfs_int(mpu.step_detector_on, en) < 0) {
1667                    LOGE("HAL:ERR can't write DMP step_detector_on");
1668                    res = -1;   //Indicate an err
1669                }
1670                mFeatureActiveMask |= INV_DMP_PED_STANDALONE;
1671            }*/
1672        }
1673        else {
1674            mFeatureActiveMask |= INV_DMP_PEDOMETER_STEP;
1675        }
1676
1677        clock_gettime(CLOCK_MONOTONIC, &mt_pre);
1678    } else {
1679        if (interruptMode) {
1680            mFeatureActiveMask &= ~INV_DMP_PEDOMETER;
1681        }
1682        else {
1683            mFeatureActiveMask &= ~INV_DMP_PEDOMETER_STEP;
1684        }
1685
1686        /* if neither step detector or step count is on */
1687        if (!(mFeatureActiveMask & (INV_DMP_PEDOMETER | INV_DMP_PEDOMETER_STEP))) {
1688            //Disable DMP Pedometer Function
1689            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1690                    en, mpu.pedometer_on, getTimestamp());
1691            if (write_sysfs_int(mpu.pedometer_on, en) < 0) {
1692                LOGE("HAL:ERR can't enable Android Pedometer");
1693                res = -1;   // indicate an err
1694                return res;
1695            }
1696        }
1697
1698        if (mFeatureActiveMask == 0 ) {
1699            // disable DMP
1700            res = onDmp(0);
1701            if (res < 0) {
1702                return res;
1703            }
1704
1705             // disable accel engine
1706             if (!(mLocalSensorMask & mMasterSensorMask
1707                        & INV_THREE_AXIS_ACCEL)) {
1708                res = enableAccel(0);
1709                if (res < 0) {
1710                    return res;
1711                }
1712            }
1713        }
1714
1715        /* if feature is not step detector */
1716        if (!(mFeatureActiveMask & INV_DMP_PEDOMETER)) {
1717            //Disable DMP Pedometer Interrupt
1718            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1719                    en, mpu.pedometer_int_on, getTimestamp());
1720            if (write_sysfs_int(mpu.pedometer_int_on, en) < 0) {
1721                LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
1722                res = -1;   // indicate an err
1723                return res;
1724            }
1725        }
1726
1727        //enable data interrupts if applicable
1728        if (enabled_sensors) {
1729            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1730                        en, mpu.dmp_event_int_on, getTimestamp());
1731            if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
1732                res = -1;
1733                LOGE("HAL:ERR can't enable DMP event interrupt");
1734            }
1735        }
1736    }
1737
1738    if(en || enabled_sensors || mFeatureActiveMask) {
1739        res = masterEnable(1);
1740    }
1741    return res;
1742}
1743
1744int MPLSensor::masterEnable(int en)
1745{
1746    VFUNC_LOG;
1747
1748    int res = 0;
1749    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1750            en, mpu.chip_enable, getTimestamp());
1751    res = write_sysfs_int(mpu.chip_enable, en);
1752    return res;
1753}
1754
1755int MPLSensor::enableGyro(int en)
1756{
1757    VFUNC_LOG;
1758
1759    int res = 0;
1760
1761    /* need to also turn on/off the master enable */
1762    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1763            en, mpu.gyro_enable, getTimestamp());
1764    res = write_sysfs_int(mpu.gyro_enable, en);
1765    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1766            en, mpu.gyro_fifo_enable, getTimestamp());
1767    res += write_sysfs_int(mpu.gyro_fifo_enable, en);
1768
1769    if (!en) {
1770        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off");
1771        inv_gyro_was_turned_off();
1772    }
1773
1774    return res;
1775}
1776
1777int MPLSensor::enableAccel(int en)
1778{
1779    VFUNC_LOG;
1780
1781    int res;
1782
1783    /* need to also turn on/off the master enable */
1784    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1785            en, mpu.accel_enable, getTimestamp());
1786    res = write_sysfs_int(mpu.accel_enable, en);
1787    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1788            en, mpu.accel_fifo_enable, getTimestamp());
1789    res += write_sysfs_int(mpu.accel_fifo_enable, en);
1790
1791    if (!en) {
1792        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off");
1793        inv_accel_was_turned_off();
1794    }
1795
1796    return res;
1797}
1798
1799int MPLSensor::enableCompass(int en, int rawSensorRequested)
1800{
1801    VFUNC_LOG;
1802
1803    int res = 0;
1804    /* handle ID_RM if third party compass cal is used */
1805    if (rawSensorRequested && mCompassSensor->providesCalibration()) {
1806        res = mCompassSensor->enable(ID_RM, en);
1807    } else {
1808        res = mCompassSensor->enable(ID_M, en);
1809    }
1810    if (en == 0 || res != 0) {
1811        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off %d", res);
1812        inv_compass_was_turned_off();
1813    }
1814
1815    return res;
1816}
1817
1818int MPLSensor::enablePressure(int en)
1819{
1820    VFUNC_LOG;
1821
1822    int res = 0;
1823
1824    if (mPressureSensor) {
1825        res = mPressureSensor->enable(ID_PS, en);
1826    } else {
1827        LOGV_IF(ENG_VERBOSE, "HAL:PRESSURE sensor not detected");
1828    }
1829
1830    return res;
1831}
1832
1833/* use this function for initialization */
1834int MPLSensor::enableBatch(int64_t timeout)
1835{
1836    VFUNC_LOG;
1837
1838    int res = 0;
1839
1840    res = write_sysfs_int(mpu.batchmode_timeout, timeout);
1841    if (timeout == 0) {
1842        res = write_sysfs_int(mpu.six_axis_q_on, 0);
1843        res = write_sysfs_int(mpu.ped_q_on, 0);
1844        res = write_sysfs_int(mpu.step_detector_on, 0);
1845        res = write_sysfs_int(mpu.step_indicator_on, 0);
1846    }
1847
1848    if (timeout == 0) {
1849        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:batchmode timeout is zero");
1850    }
1851
1852    return res;
1853}
1854
1855void MPLSensor::computeLocalSensorMask(int enabled_sensors)
1856{
1857    VFUNC_LOG;
1858
1859    do {
1860        /* Invensense Pressure on secondary bus */
1861        if (PS_ENABLED) {
1862            LOGV_IF(ENG_VERBOSE, "PS ENABLED");
1863            mLocalSensorMask |= INV_ONE_AXIS_PRESSURE;
1864        } else {
1865            LOGV_IF(ENG_VERBOSE, "PS DISABLED");
1866            mLocalSensorMask &= ~INV_ONE_AXIS_PRESSURE;
1867        }
1868
1869        if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED
1870                       || (GRV_ENABLED && GMRV_ENABLED)) {
1871            LOGV_IF(ENG_VERBOSE, "FUSION ENABLED");
1872            mLocalSensorMask = ALL_MPL_SENSORS_NP;
1873            break;
1874        }
1875
1876        if (GRV_ENABLED) {
1877            if (!(mBatchEnabled & (1 << GameRotationVector))) {
1878                LOGV_IF(ENG_VERBOSE, "6 Axis Fusion ENABLED");
1879                mLocalSensorMask |= INV_THREE_AXIS_GYRO;
1880                mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
1881            } else {
1882                if (GY_ENABLED || RGY_ENABLED) {
1883                    LOGV_IF(ENG_VERBOSE, "G ENABLED");
1884                    mLocalSensorMask |= INV_THREE_AXIS_GYRO;
1885                } else {
1886                    LOGV_IF(ENG_VERBOSE, "G DISABLED");
1887                    mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
1888                }
1889                if (A_ENABLED) {
1890                    LOGV_IF(ENG_VERBOSE, "A ENABLED");
1891                    mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
1892                } else {
1893                    LOGV_IF(ENG_VERBOSE, "A DISABLED");
1894                    mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
1895                }
1896            }
1897            /* takes care of MAG case */
1898            if (M_ENABLED || RM_ENABLED) {
1899                LOGV_IF(1, "M ENABLED");
1900                mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
1901            } else {
1902                LOGV_IF(1, "M DISABLED");
1903                mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS;
1904            }
1905            break;
1906        }
1907
1908        if (GMRV_ENABLED) {
1909            LOGV_IF(ENG_VERBOSE, "6 Axis Geomagnetic Fusion ENABLED");
1910            mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
1911            mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
1912
1913            /* takes care of Gyro case */
1914            if (GY_ENABLED || RGY_ENABLED) {
1915                LOGV_IF(1, "G ENABLED");
1916                mLocalSensorMask |= INV_THREE_AXIS_GYRO;
1917            } else {
1918                LOGV_IF(1, "G DISABLED");
1919                mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
1920            }
1921            break;
1922        }
1923
1924        if(!A_ENABLED && !M_ENABLED && !RM_ENABLED &&
1925               !GRV_ENABLED && !GMRV_ENABLED && !GY_ENABLED && !RGY_ENABLED &&
1926               !PS_ENABLED) {
1927            /* Invensense compass cal */
1928            LOGV_IF(ENG_VERBOSE, "ALL DISABLED");
1929            mLocalSensorMask = 0;
1930            break;
1931        }
1932
1933        if (GY_ENABLED || RGY_ENABLED) {
1934            LOGV_IF(ENG_VERBOSE, "G ENABLED");
1935            mLocalSensorMask |= INV_THREE_AXIS_GYRO;
1936        } else {
1937            LOGV_IF(ENG_VERBOSE, "G DISABLED");
1938            mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
1939        }
1940
1941        if (A_ENABLED) {
1942            LOGV_IF(ENG_VERBOSE, "A ENABLED");
1943            mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
1944        } else {
1945            LOGV_IF(ENG_VERBOSE, "A DISABLED");
1946            mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
1947        }
1948
1949        /* Invensense compass calibration */
1950        if (M_ENABLED || RM_ENABLED) {
1951            LOGV_IF(ENG_VERBOSE, "M ENABLED");
1952            mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
1953        } else {
1954            LOGV_IF(ENG_VERBOSE, "M DISABLED");
1955            mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS;
1956        }
1957    } while (0);
1958}
1959
1960int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed)
1961{
1962    VFUNC_LOG;
1963
1964    inv_error_t res = -1;
1965    int on = 1;
1966    int off = 0;
1967    int cal_stored = 0;
1968
1969    // Sequence to enable or disable a sensor
1970    // 1. reset master enable (=0)
1971    // 2. enable or disable a sensor
1972    // 3. set master enable (=1)
1973
1974    if (isLowPowerQuatEnabled() ||
1975        changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
1976        (mCompassSensor->isIntegrated() << MagneticField) |
1977        (mCompassSensor->isIntegrated() << RawMagneticField) |
1978        (mPressureSensor->isIntegrated() << Pressure))) {
1979
1980        /* reset master enable */
1981        res = masterEnable(0);
1982        if(res < 0) {
1983            return res;
1984        }
1985    }
1986
1987    LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x",
1988            (unsigned int)sensors);
1989
1990    if (changed & ((1 << Gyro) | (1 << RawGyro))) {
1991        LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - gyro %s",
1992            (sensors & INV_THREE_AXIS_GYRO? "enable": "disable"));
1993        res = enableGyro(!!(sensors & INV_THREE_AXIS_GYRO));
1994        if(res < 0) {
1995            return res;
1996        }
1997
1998        if (!cal_stored && (!en && (changed & (1 << Gyro)))) {
1999            storeCalibration();
2000            cal_stored = 1;
2001        }
2002    }
2003
2004    if (changed & (1 << Accelerometer)) {
2005        LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - accel %s",
2006            (sensors & INV_THREE_AXIS_ACCEL? "enable": "disable"));
2007        res = enableAccel(!!(sensors & INV_THREE_AXIS_ACCEL));
2008        if(res < 0) {
2009            return res;
2010        }
2011
2012        if (!(sensors & INV_THREE_AXIS_ACCEL) && !cal_stored) {
2013            storeCalibration();
2014            cal_stored = 1;
2015        }
2016    }
2017
2018    if (changed & ((1 << MagneticField) | (1 << RawMagneticField))) {
2019        LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - compass %s",
2020            (sensors & INV_THREE_AXIS_COMPASS? "enable": "disable"));
2021        res = enableCompass(!!(sensors & INV_THREE_AXIS_COMPASS), changed & (1 << RawMagneticField));
2022        if(res < 0) {
2023            return res;
2024        }
2025
2026        if (!cal_stored && (!en && (changed & (1 << MagneticField)))) {
2027            storeCalibration();
2028            cal_stored = 1;
2029        }
2030    }
2031
2032     if (changed & (1 << Pressure)) {
2033        LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - pressure %s",
2034            (sensors & INV_ONE_AXIS_PRESSURE? "enable": "disable"));
2035        res = enablePressure(!!(sensors & INV_ONE_AXIS_PRESSURE));
2036        if(res < 0) {
2037            return res;
2038        }
2039    }
2040
2041    if (isLowPowerQuatEnabled()) {
2042        // Enable LP Quat
2043        if ((mEnabled & VIRTUAL_SENSOR_9AXES_MASK)
2044                      || (mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK)) {
2045  LOGI("HAL: 9 axis or game rot enabled");
2046            if (!(changed & ((1 << Gyro)
2047                           | (1 << RawGyro)
2048                           | (1 << Accelerometer)
2049                           | (mCompassSensor->isIntegrated() << MagneticField)
2050                           | (mCompassSensor->isIntegrated() << RawMagneticField)))
2051            ) {
2052                /* reset master enable */
2053                res = masterEnable(0);
2054                if(res < 0) {
2055                    return res;
2056                }
2057            }
2058            if (!checkLPQuaternion()) {
2059                enableLPQuaternion(1);
2060            } else {
2061                LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled");
2062            }
2063        } else if (checkLPQuaternion()) {
2064            enableLPQuaternion(0);
2065        }
2066    }
2067
2068    /* apply accel/gyro bias to DMP bias                        */
2069    /* precondition: masterEnable(0), mGyroBiasAvailable=true   */
2070    /* postcondition: bias is applied upon masterEnable(1)      */
2071    if(!(sensors & INV_THREE_AXIS_GYRO)) {
2072        setGyroBias();
2073    }
2074    if(!(sensors & INV_THREE_AXIS_ACCEL)) {
2075        setAccelBias();
2076    }
2077
2078    /* to batch or not to batch */
2079    int batchMode = computeBatchSensorMask(mEnabled, mBatchEnabled);
2080    setBatch(batchMode,0);
2081
2082    if (changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
2083            (mCompassSensor->isIntegrated() << MagneticField) |
2084            (mCompassSensor->isIntegrated() << RawMagneticField) |
2085            (mPressureSensor->isIntegrated() << Pressure))) {
2086        LOGV_IF(ENG_VERBOSE, "HAL DEBUG: Gyro, Accel, Compass, Pressure changes");
2087        if ((checkSmdSupport() == 1) || (checkPedometerSupport() == 1) || (sensors &
2088            (INV_THREE_AXIS_GYRO
2089                | INV_THREE_AXIS_ACCEL
2090                | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated())
2091                | (INV_ONE_AXIS_PRESSURE * mPressureSensor->isIntegrated())))) {
2092            LOGV_IF(ENG_VERBOSE, "SMD or Hardware sensors enabled");
2093            LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
2094            if (mFeatureActiveMask & DMP_FEATURE_MASK) {
2095                LOGV_IF(ENG_VERBOSE, "HAL DEBUG: LPQ, SMD, SO enabled");
2096                // disable DMP event interrupt only (w/ data interrupt)
2097                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
2098                        0, mpu.dmp_event_int_on, getTimestamp());
2099                if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
2100                    res = -1;
2101                    LOGE("HAL:ERR can't disable DMP event interrupt");
2102                    return res;
2103                }
2104            }
2105
2106            if ((mFeatureActiveMask & DMP_FEATURE_MASK) &&
2107                    !((mFeatureActiveMask & 0x240) ||
2108                        (mFeatureActiveMask & 0x220) ||
2109                        (mFeatureActiveMask & 0x280))) {
2110LOGI("mFeatureActiveMask=%lld", mFeatureActiveMask);
2111                // enable DMP
2112                onDmp(1);
2113                res = enableAccel(on);
2114                if(res < 0) {
2115                    return res;
2116                }
2117                if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
2118                    res = turnOffAccelFifo();
2119                }
2120                if(res < 0) {
2121                    return res;
2122                }
2123            }
2124            res = masterEnable(1);
2125            if(res < 0) {
2126                return res;
2127            }
2128        } else { // all sensors idle -> reduce power
2129            LOGV_IF(ENG_VERBOSE, "HAL DEBUG: not SMD or Hardware sensors");
2130            if (isDmpDisplayOrientationOn()
2131                    && (mDmpOrientationEnabled
2132                            || !isDmpScreenAutoRotationEnabled())) {
2133                enableDmpOrientation(1);
2134            }
2135
2136            if (!cal_stored) {
2137                storeCalibration();
2138                cal_stored = 1;
2139            }
2140        }
2141    } else if ((changed &
2142                    ((!mCompassSensor->isIntegrated()) << MagneticField) ||
2143                    ((!mCompassSensor->isIntegrated()) << RawMagneticField))
2144                    &&
2145              !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL
2146                | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated()))))
2147    ) {
2148        LOGV_IF(ENG_VERBOSE, "HAL DEBUG: Gyro, Accel, Compass no change");
2149        if (!cal_stored) {
2150            storeCalibration();
2151            cal_stored = 1;
2152        }
2153    } else {
2154      LOGV_IF(ENG_VERBOSE, "HAL DEBUG: mEnabled");
2155      if (sensors &
2156            (INV_THREE_AXIS_GYRO
2157                | INV_THREE_AXIS_ACCEL
2158                | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
2159            res = masterEnable(1);
2160            if(res < 0)
2161                return res;
2162        }
2163    }
2164
2165    return res;
2166}
2167
2168/* check if batch mode should be turned on or not */
2169int MPLSensor::computeBatchSensorMask(int enableSensors, int tempBatchSensor)
2170{
2171    VFUNC_LOG;
2172    int batchMode = 1;
2173
2174    LOGV("HAL:computeBatchSensorMask: enableSensors=%d tempBatchSensor=%d", enableSensors, tempBatchSensor);
2175
2176    // check for possible continuous data mode
2177    for(int i = 0; i <= Pressure; i++) {
2178        if ((enableSensors & (1 << i)) && !(tempBatchSensor & (1 << i))) {
2179            LOGV("HAL:computeBatchSensorMask: hardware sensor on continuous mode:%d", i);
2180            // if any one of the hardware sensor is in continuous data mode
2181            // turn off batch mode.
2182            return 0;
2183        }
2184        if ((enableSensors & (1 << i)) && (tempBatchSensor & (1 << i))) {
2185            LOGV("HAL:computeBatchSensorMask: hardware sensor is batch:%d", i);
2186            // if hardware sensor is batched, check if virtual sensor is batched
2187            if ((enableSensors & (1 << GameRotationVector))
2188                            && !(tempBatchSensor & (1 << GameRotationVector))) {
2189            LOGV("HAL:computeBatchSensorMask: but virtual sensor is not:%d", i);
2190                return 0;
2191            }
2192        }
2193    }
2194
2195    for(int i = Orientation; i <= GeomagneticRotationVector; i++) {
2196        if ((enableSensors & (1 << i)) && !(tempBatchSensor & (1 << i))) {
2197             LOGV("HAL:computeBatchSensorMask: composite sensor on continuous mode:%d", i);
2198            // if composite sensors are on but not batched
2199            // turn off batch mode.
2200            return 0;
2201        }
2202    }
2203
2204    LOGV_IF(EXTRA_VERBOSE, "HAL:computeBatchSensorMask: batchMode=%d, mBatchEnabled=%0x", batchMode, tempBatchSensor);
2205    return (batchMode && tempBatchSensor);
2206}
2207
2208/* This function is called by enable() */
2209int MPLSensor::setBatch(int en, int toggleEnable)
2210{
2211    VFUNC_LOG;
2212
2213    int res = 0;
2214    int64_t wanted = 1000000000LL;
2215    int64_t timeout = 0;
2216    int timeoutInMs = 0;
2217    int featureMask = computeBatchDataOutput();
2218
2219    // reset master enable
2220    res = masterEnable(0);
2221    if (res < 0) {
2222        return res;
2223    }
2224
2225    if (en) {
2226        /* take the minimum batchmode timeout */
2227        int64_t timeout = 30000000000LL;
2228        int64_t ns;
2229        for (int i = 0; i < NumSensors; i++) {
2230            LOGV_IF(0, "mFeatureActiveMask=0x%016llx, mEnabled=0x%01x, mBatchEnabled=0x%x",
2231                            mFeatureActiveMask, mEnabled, mBatchEnabled);
2232            if ((mEnabled & (1 << i)) && (mBatchEnabled & (1 << i)) ||
2233                    (((featureMask & INV_DMP_PED_STANDALONE) && (mBatchEnabled & (1 << StepDetector))))) {
2234                LOGV_IF(ENG_VERBOSE, "sensor=%d, timeout=%lld", i, mBatchTimeouts[i]);
2235                ns = mBatchTimeouts[i];
2236                timeout = (ns < timeout) ? ns : timeout;
2237            }
2238        }
2239        /* Convert ns to millisecond */
2240        timeoutInMs = timeout / 1000000;
2241    } else {
2242        timeoutInMs = 0;
2243    }
2244
2245    LOGV_IF(PROCESS_VERBOSE, "HAL: batch timeout set to %dms", timeoutInMs);
2246
2247    /* step detector is enabled and */
2248    /* batch mode is standalone */
2249    if (en && (mFeatureActiveMask & INV_DMP_PEDOMETER) &&
2250            (featureMask & INV_DMP_PED_STANDALONE)) {
2251        LOGI("ID_P only = 0x%x", mBatchEnabled);
2252        enablePedStandalone(1);
2253    } else {
2254        enablePedStandalone(0);
2255    }
2256
2257    /* step detector and GRV are enabled and */
2258    /* batch mode is ped q */
2259    if (en && (mFeatureActiveMask & INV_DMP_PEDOMETER) &&
2260            (mEnabled & (1 << GameRotationVector)) &&
2261            (featureMask & INV_DMP_PED_QUATERNION)) {
2262        LOGI("ID_P and GRV or ALL = 0x%x", mBatchEnabled);
2263        LOGI("ID_P is enabled for batching, PED quat will be automatically enabled");
2264        enableLPQuaternion(0);
2265        enablePedQuaternion(1);
2266    } else if (!(featureMask & INV_DMP_PED_STANDALONE)){
2267        enablePedQuaternion(0);
2268    }
2269
2270    /* step detector and hardware sensors enabled */
2271    if (en && (featureMask & INV_DMP_PED_INDICATOR) &&
2272            ((mEnabled) ||
2273            (mFeatureActiveMask & INV_DMP_PED_STANDALONE))) {
2274        enablePedIndicator(1);
2275    } else {
2276        enablePedIndicator(0);
2277    }
2278
2279    /* GRV is enabled and */
2280    /* batch mode is 6axis q */
2281    if (en && (mEnabled & (1 << GameRotationVector)) &&
2282            (featureMask & INV_DMP_6AXIS_QUATERNION)) {
2283        LOGI("GRV = 0x%x", mBatchEnabled);
2284        enableLPQuaternion(0);
2285        enable6AxisQuaternion(1);
2286    } else if (!(featureMask & INV_DMP_PED_QUATERNION)){
2287        LOGI("Toggle back to normal 6 axis");
2288        if (mEnabled & (1 << GameRotationVector)) {
2289            enableLPQuaternion(1);
2290        }
2291        enable6AxisQuaternion(0);
2292    }
2293
2294    /* write required timeout to sysfs */
2295    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
2296            timeoutInMs, mpu.batchmode_timeout, getTimestamp());
2297    if (write_sysfs_int(mpu.batchmode_timeout, timeoutInMs) < 0) {
2298        LOGE("HAL:ERR can't write batchmode_timeout");
2299    }
2300
2301    if (en) {
2302        // enable DMP
2303        res = onDmp(1);
2304        if (res < 0) {
2305            return res;
2306        }
2307
2308        // set batch rates
2309        if (setBatchDataRates() < 0) {
2310            LOGE("HAL:ERR can't set batch data rates");
2311        }
2312
2313        // default fifo rate to 200Hz
2314        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
2315                200, mpu.gyro_fifo_rate, getTimestamp());
2316        if (write_sysfs_int(mpu.gyro_fifo_rate, 200) < 0) {
2317            res = -1;
2318            LOGE("HAL:ERR can't set rate to 200Hz");
2319            return res;
2320        }
2321    } else {
2322        if (mFeatureActiveMask == 0) {
2323            // disable DMP
2324            res = onDmp(0);
2325            if (res < 0) {
2326                return res;
2327            }
2328        }
2329        /* reset sensor rate */
2330        /*if (resetDataRates() < 0) {
2331            LOGE("HAL:ERR can't reset output rate back to original setting");
2332        }*/
2333    }
2334    if (toggleEnable == 1) {
2335        if (mFeatureActiveMask || mEnabled)
2336            masterEnable(1);
2337    }
2338    return res;
2339}
2340
2341/* Store calibration file */
2342void MPLSensor::storeCalibration(void)
2343{
2344    if(mHaveGoodMpuCal == true
2345        || mAccelAccuracy >= 2
2346        || mCompassAccuracy >= 3) {
2347       int res = inv_store_calibration();
2348       if (res) {
2349           LOGE("HAL:Cannot store calibration on file");
2350       } else {
2351           LOGV_IF(PROCESS_VERBOSE, "HAL:Cal file updated");
2352       }
2353    }
2354}
2355
2356void MPLSensor::cbProcData(void)
2357{
2358    mNewData = 1;
2359    mSampleCount++;
2360    LOGV_IF(EXTRA_VERBOSE, "HAL:new data");
2361}
2362
2363/*  these handlers transform mpl data into one of the Android sensor types */
2364int MPLSensor::gyroHandler(sensors_event_t* s)
2365{
2366    VHANDLER_LOG;
2367    int update;
2368    update = inv_get_sensor_type_gyroscope(s->gyro.v, &s->gyro.status,
2369                                           &s->timestamp);
2370    LOGV_IF(HANDLER_DATA, "HAL:gyro data : %+f %+f %+f -- %lld - %d",
2371            s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
2372    return update;
2373}
2374
2375int MPLSensor::rawGyroHandler(sensors_event_t* s)
2376{
2377    VHANDLER_LOG;
2378    int update;
2379    update = inv_get_sensor_type_gyroscope_raw(s->uncalibrated_gyro.uncalib,
2380                                               &s->gyro.status, &s->timestamp);
2381    if(update) {
2382        memcpy(s->uncalibrated_gyro.bias, mGyroBias, sizeof(mGyroBias));
2383        LOGV_IF(HANDLER_DATA,"HAL:gyro bias data : %+f %+f %+f -- %lld - %d",
2384            s->uncalibrated_gyro.bias[0], s->uncalibrated_gyro.bias[1],
2385            s->uncalibrated_gyro.bias[2], s->timestamp, update);
2386    }
2387    s->gyro.status = SENSOR_STATUS_UNRELIABLE;
2388    LOGV_IF(HANDLER_DATA, "HAL:raw gyro data : %+f %+f %+f -- %lld - %d",
2389            s->uncalibrated_gyro.uncalib[0], s->uncalibrated_gyro.uncalib[1],
2390            s->uncalibrated_gyro.uncalib[2], s->timestamp, update);
2391    return update;
2392}
2393
2394int MPLSensor::accelHandler(sensors_event_t* s)
2395{
2396    VHANDLER_LOG;
2397    int update;
2398    update = inv_get_sensor_type_accelerometer(
2399        s->acceleration.v, &s->acceleration.status, &s->timestamp);
2400    LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d",
2401            s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2],
2402            s->timestamp, update);
2403    mAccelAccuracy = s->acceleration.status;
2404    return update;
2405}
2406
2407int MPLSensor::compassHandler(sensors_event_t* s)
2408{
2409    VHANDLER_LOG;
2410    int update;
2411    update = inv_get_sensor_type_magnetic_field(
2412        s->magnetic.v, &s->magnetic.status, &s->timestamp);
2413    LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d",
2414            s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2],
2415            s->timestamp, update);
2416    mCompassAccuracy = s->magnetic.status;
2417    return update;
2418}
2419
2420int MPLSensor::rawCompassHandler(sensors_event_t* s)
2421{
2422    VHANDLER_LOG;
2423    int update;
2424    //TODO:: need to handle uncalib data and bias for 3rd party compass
2425    if(mCompassSensor->providesCalibration()) {
2426        update = mCompassSensor->readRawSample(s->uncalibrated_magnetic.uncalib, &s->timestamp);
2427    }
2428    else {
2429        update = inv_get_sensor_type_magnetic_field_raw(s->uncalibrated_magnetic.uncalib,
2430                     &s->magnetic.status, &s->timestamp);
2431    }
2432    if(update) {
2433        memcpy(s->uncalibrated_magnetic.bias, mCompassBias, sizeof(mCompassBias));
2434        LOGV_IF(HANDLER_DATA, "HAL:compass bias data: %+f %+f %+f -- %lld - %d",
2435                s->uncalibrated_magnetic.bias[0], s->uncalibrated_magnetic.bias[1],
2436                s->uncalibrated_magnetic.bias[2], s->timestamp, update);
2437    }
2438    s->magnetic.status = SENSOR_STATUS_UNRELIABLE;
2439    LOGV_IF(HANDLER_DATA, "HAL:compass raw data: %+f %+f %+f %d -- %lld - %d",
2440        s->uncalibrated_magnetic.uncalib[0], s->uncalibrated_magnetic.uncalib[1],
2441                    s->uncalibrated_magnetic.uncalib[2], s->magnetic.status, s->timestamp, update);
2442    return update;
2443}
2444
2445/*
2446    Rotation Vector handler.
2447    NOTE: rotation vector does not have an accuracy or status
2448*/
2449int MPLSensor::rvHandler(sensors_event_t* s)
2450{
2451    VHANDLER_LOG;
2452    int8_t status;
2453    int update;
2454    update = inv_get_sensor_type_rotation_vector(s->data, &status,
2455                                                 &s->timestamp);
2456    update |= isCompassDisabled();
2457    LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f %+f- %+lld - %d",
2458            s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->timestamp,
2459            update);
2460
2461    return update;
2462}
2463
2464/*
2465    Game Rotation Vector handler.
2466    NOTE: rotation vector does not have an accuracy or status
2467*/
2468int MPLSensor::grvHandler(sensors_event_t* s)
2469{
2470    VHANDLER_LOG;
2471    int8_t status;
2472    int update;
2473    update = inv_get_sensor_type_rotation_vector_6_axis(s->data, &status,
2474                                                     &s->timestamp);
2475    /*hack*/
2476    /*s->data[0] =  mCached6AxisQuaternionData[0];
2477    s->data[1] =  mCached6AxisQuaternionData[1];
2478    s->data[2] =  mCached6AxisQuaternionData[2];
2479    update = 1;*/
2480
2481
2482    LOGV_IF(HANDLER_DATA, "HAL:grv data: %+f %+f %+f %+f %+f - %+lld - %d",
2483            s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->timestamp,
2484            update);
2485    return update;
2486}
2487
2488int MPLSensor::laHandler(sensors_event_t* s)
2489{
2490    VHANDLER_LOG;
2491    int update;
2492    update = inv_get_sensor_type_linear_acceleration(
2493            s->gyro.v, &s->gyro.status, &s->timestamp);
2494    update |= isCompassDisabled();
2495    LOGV_IF(HANDLER_DATA, "HAL:la data: %+f %+f %+f - %lld - %d",
2496            s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
2497    return update;
2498}
2499
2500int MPLSensor::gravHandler(sensors_event_t* s)
2501{
2502    VHANDLER_LOG;
2503    int update;
2504    update = inv_get_sensor_type_gravity(s->gyro.v, &s->gyro.status,
2505                                         &s->timestamp);
2506    update |= isCompassDisabled();
2507    LOGV_IF(HANDLER_DATA, "HAL:gr data: %+f %+f %+f - %lld - %d",
2508            s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
2509    return update;
2510}
2511
2512int MPLSensor::orienHandler(sensors_event_t* s)
2513{
2514    VHANDLER_LOG;
2515    int update;
2516    update = inv_get_sensor_type_orientation(
2517            s->orientation.v, &s->orientation.status, &s->timestamp);
2518    update |= isCompassDisabled();
2519    LOGV_IF(HANDLER_DATA, "HAL:or data: %f %f %f - %lld - %d",
2520            s->orientation.v[0], s->orientation.v[1], s->orientation.v[2],
2521            s->timestamp, update);
2522    return update;
2523}
2524
2525int MPLSensor::smHandler(sensors_event_t* s)
2526{
2527    VHANDLER_LOG;
2528    int update = 1;
2529
2530    /* When event is triggered, set data to 1 */
2531    s->data[0] = 1.f;
2532    s->data[1] = 0.f;
2533    s->data[2] = 0.f;
2534    s->acceleration.status
2535            = SENSOR_STATUS_UNRELIABLE;
2536
2537    /* Capture timestamp in HAL */
2538    struct timespec ts;
2539    clock_gettime(CLOCK_MONOTONIC, &ts);
2540    s->timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
2541
2542    /* Identify which sensor this event is for */
2543    s->version = sizeof(sensors_event_t);
2544    s->sensor = ID_SM;
2545    s->type = SENSOR_TYPE_SIGNIFICANT_MOTION;
2546
2547    LOGV_IF(HANDLER_DATA, "HAL:sm data: %f - %lld - %d",
2548            s->data[0], s->timestamp, update);
2549    return update;
2550}
2551
2552int MPLSensor::scHandler(sensors_event_t* s)
2553{
2554    VHANDLER_LOG;
2555    int update = 0;
2556
2557    //update = readDmpPedometerEvents(s, 1);
2558    LOGV_IF(HANDLER_DATA, "HAL:sc data: %f - %lld - %d",
2559            s->data[0], s->timestamp, update);
2560    return update < 1 ? 0 :1;
2561}
2562
2563int MPLSensor::gmHandler(sensors_event_t* s)
2564{
2565    VHANDLER_LOG;
2566    int8_t status;
2567    int update = 0;
2568    update = inv_get_sensor_type_geomagnetic_rotation_vector(s->data, &status,
2569                                                             &s->timestamp);
2570
2571    LOGV_IF(HANDLER_DATA, "HAL:gm data: %+f %+f %+f %+f %+f- %+lld - %d",
2572            s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->timestamp, update);
2573    return update < 1 ? 0 :1;
2574
2575}
2576
2577int MPLSensor::psHandler(sensors_event_t* s)
2578{
2579    VHANDLER_LOG;
2580    int8_t status;
2581    int update = 0;
2582
2583    s->data[0] = mCachedPressureData / 100; //hpa (millibar)
2584    s->data[1] = 0;
2585    s->data[2] = 0;
2586    s->timestamp = mPressureTimestamp;
2587    s->magnetic.status = 0;
2588    update = 1;
2589
2590    LOGV_IF(HANDLER_DATA, "HAL:ps data: %+f %+f %+f %+f- %+lld - %d",
2591            s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update);
2592    return update < 1 ? 0 :1;
2593
2594}
2595
2596int MPLSensor::enable(int32_t handle, int en)
2597{
2598    VFUNC_LOG;
2599
2600    android::String8 sname;
2601    int what = -1, err = 0;
2602    int batchMode = 0;
2603
2604    switch (handle) {
2605    case ID_SC:
2606         what = StepCounter;
2607         sname = "Step Counter";
2608         LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
2609                sname.string(), handle,
2610                (mDmpStepCountEnabled? "en": "dis"),
2611                (en? "en" : "dis"));
2612        enableDmpPedometer(en, 0);
2613        mDmpStepCountEnabled = !!en;
2614        return 0;
2615    case ID_P:
2616        sname = "StepDetector";
2617        LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
2618                sname.string(), handle,
2619                (mDmpPedometerEnabled? "en": "dis"),
2620                (en? "en" : "dis"));
2621        enableDmpPedometer(en, 1);
2622        mDmpPedometerEnabled = !!en;
2623        batchMode = computeBatchSensorMask(mEnabled, mBatchEnabled);
2624        setBatch(batchMode,1);
2625        return 0;
2626    case ID_SM:
2627        sname = "Significant Motion";
2628        LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
2629                sname.string(), handle,
2630                (mDmpSignificantMotionEnabled? "en": "dis"),
2631                (en? "en" : "dis"));
2632        enableDmpSignificantMotion(en);
2633        mDmpSignificantMotionEnabled = !!en;
2634        return 0;
2635    case ID_SO:
2636        sname = "Screen Orientation";
2637        LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
2638                sname.string(), handle,
2639                (mDmpOrientationEnabled? "en": "dis"),
2640                (en? "en" : "dis"));
2641        enableDmpOrientation(en && isDmpDisplayOrientationOn());
2642        mDmpOrientationEnabled = !!en;
2643        return 0;
2644    case ID_A:
2645        what = Accelerometer;
2646        sname = "Accelerometer";
2647        break;
2648    case ID_M:
2649        what = MagneticField;
2650        sname = "MagneticField";
2651        break;
2652    case ID_RM:
2653        what = RawMagneticField;
2654        sname = "MagneticField Uncalibrated";
2655        break;
2656    case ID_O:
2657        what = Orientation;
2658        sname = "Orientation";
2659        break;
2660    case ID_GY:
2661        what = Gyro;
2662        sname = "Gyro";
2663        break;
2664    case ID_RG:
2665        what = RawGyro;
2666        sname = "Gyro Uncalibrated";
2667        break;
2668    case ID_GR:
2669        what = Gravity;
2670        sname = "Gravity";
2671        break;
2672    case ID_RV:
2673        what = RotationVector;
2674        sname = "RotationVector";
2675        break;
2676    case ID_GRV:
2677        what = GameRotationVector;
2678        sname = "GameRotationVector";
2679        break;
2680    case ID_LA:
2681        what = LinearAccel;
2682        sname = "LinearAccel";
2683        break;
2684    case ID_GMRV:
2685        what = GeomagneticRotationVector;
2686        sname = "GeomagneticRotationVector";
2687        break;
2688    case ID_PS:
2689        what = Pressure;
2690        sname = "Pressure";
2691        break;
2692    default: //this takes care of all the gestures
2693        what = handle;
2694        sname = "Others";
2695        break;
2696    }
2697
2698    if (uint32_t(what) >= NumSensors)
2699        return -EINVAL;
2700
2701    int newState = en ? 1 : 0;
2702    unsigned long sen_mask;
2703
2704    LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
2705            sname.string(), handle,
2706            ((mEnabled & (1 << what)) ? "en" : "dis"),
2707            ((uint32_t(newState) << what) ? "en" : "dis"));
2708    LOGV_IF(PROCESS_VERBOSE,
2709            "HAL:%s sensor state change what=%d", sname.string(), what);
2710
2711    // pthread_mutex_lock(&mMplMutex);
2712    // pthread_mutex_lock(&mHALMutex);
2713
2714    if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) {
2715        uint32_t sensor_type;
2716        short flags = newState;
2717        uint32_t lastEnabled = mEnabled, changed = 0;
2718
2719        mEnabled &= ~(1 << what);
2720        mEnabled |= (uint32_t(flags) << what);
2721
2722        LOGV_IF(PROCESS_VERBOSE, "HAL:handle = %d", handle);
2723        LOGV_IF(PROCESS_VERBOSE, "HAL:flags = %d", flags);
2724        computeLocalSensorMask(mEnabled);
2725        LOGV_IF(PROCESS_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled);
2726        LOGV_IF(ENG_VERBOSE, "HAL:last enable : lastEnabled = %d", lastEnabled);
2727        sen_mask = mLocalSensorMask & mMasterSensorMask;
2728        mSensorMask = sen_mask;
2729        LOGV_IF(PROCESS_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask);
2730
2731        switch (what) {
2732            case Gyro:
2733            case RawGyro:
2734            case Accelerometer:
2735                if ((!(mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK) &&
2736                    !(mEnabled & VIRTUAL_SENSOR_9AXES_MASK)) &&
2737                    ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) {
2738                    changed |= (1 << what);
2739                }
2740                if (mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION) {
2741                     changed |= (1 << what);
2742                }
2743                break;
2744            case MagneticField:
2745            case RawMagneticField:
2746                if (!(mEnabled & VIRTUAL_SENSOR_9AXES_MASK) &&
2747                    ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) {
2748                    changed |= (1 << what);
2749                }
2750                break;
2751            case Pressure:
2752                if ((lastEnabled & (1 << what)) != (mEnabled & (1 << what))) {
2753                    changed |= (1 << what);
2754                }
2755                break;
2756            case GameRotationVector:
2757                if (!en)
2758                    storeCalibration();
2759                if ((en && !(lastEnabled & VIRTUAL_SENSOR_ALL_MASK))
2760                         ||
2761                    (en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK))
2762                         ||
2763                    (!en && !(mEnabled & VIRTUAL_SENSOR_ALL_MASK))
2764                         ||
2765                    (!en && (mEnabled & VIRTUAL_SENSOR_MAG_6AXES_MASK))) {
2766                    for (int i = Gyro; i <= RawMagneticField; i++) {
2767                        if (!(mEnabled & (1 << i))) {
2768                            changed |= (1 << i);
2769                        }
2770                    }
2771                }
2772                break;
2773
2774            case Orientation:
2775            case RotationVector:
2776            case LinearAccel:
2777            case Gravity:
2778                if (!en)
2779                    storeCalibration();
2780                if ((en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK))
2781                         ||
2782                    (!en && !(mEnabled & VIRTUAL_SENSOR_9AXES_MASK))) {
2783                    for (int i = Gyro; i <= RawMagneticField; i++) {
2784                        if (!(mEnabled & (1 << i))) {
2785                            changed |= (1 << i);
2786                        }
2787                    }
2788                }
2789                break;
2790            case GeomagneticRotationVector:
2791                if (!en)
2792                    storeCalibration();
2793                if ((en && !(lastEnabled & VIRTUAL_SENSOR_ALL_MASK))
2794                        ||
2795                    (en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK))
2796                         ||
2797                   (!en && !(mEnabled & VIRTUAL_SENSOR_ALL_MASK))
2798                         ||
2799                   (!en && (mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK))) {
2800                   for (int i = Accelerometer; i <= RawMagneticField; i++) {
2801                       if (!(mEnabled & (1<<i))) {
2802                          changed |= (1 << i);
2803                       }
2804                   }
2805                }
2806                break;
2807        }
2808        LOGV_IF(PROCESS_VERBOSE, "HAL:changed = %d", changed);
2809        enableSensors(sen_mask, flags, changed);
2810    }
2811
2812    // pthread_mutex_unlock(&mMplMutex);
2813    // pthread_mutex_unlock(&mHALMutex);
2814
2815#ifdef INV_PLAYBACK_DBG
2816    /* apparently the logging needs to go through this sequence
2817       to properly flush the log file */
2818    inv_turn_off_data_logging();
2819    fclose(logfile);
2820    logfile = fopen("/data/playback.bin", "ab");
2821    if (logfile)
2822        inv_turn_on_data_logging(logfile);
2823#endif
2824
2825    return err;
2826}
2827
2828void MPLSensor::getHandle(int32_t handle, int &what, android::String8 &sname)
2829{
2830   VFUNC_LOG;
2831
2832   what = -1;
2833
2834   switch (handle) {
2835   case ID_P:
2836        what = StepDetector;
2837        sname = "StepDetector";
2838        break;
2839   case ID_SC:
2840        what = StepCounter;
2841        sname = "StepCounter";
2842        break;
2843   case ID_SM:
2844        what = SignificantMotion;
2845        sname = "SignificantMotion";
2846        break;
2847   case ID_SO:
2848        what = handle;
2849        sname = "ScreenOrienation";
2850   case ID_A:
2851        what = Accelerometer;
2852        sname = "Accelerometer";
2853        break;
2854   case ID_M:
2855        what = MagneticField;
2856        sname = "MagneticField";
2857        break;
2858   case ID_RM:
2859        what = RawMagneticField;
2860        sname = "MagneticField Uncalibrated";
2861        break;
2862   case ID_O:
2863        what = Orientation;
2864        sname = "Orientation";
2865        break;
2866   case ID_GY:
2867        what = Gyro;
2868        sname = "Gyro";
2869        break;
2870   case ID_RG:
2871        what = RawGyro;
2872        sname = "Gyro Uncalibrated";
2873        break;
2874   case ID_GR:
2875        what = Gravity;
2876        sname = "Gravity";
2877        break;
2878   case ID_RV:
2879        what = RotationVector;
2880        sname = "RotationVector";
2881        break;
2882   case ID_GRV:
2883        what = GameRotationVector;
2884        sname = "GameRotationVector";
2885        break;
2886   case ID_LA:
2887        what = LinearAccel;
2888        sname = "LinearAccel";
2889        break;
2890   case ID_PS:
2891        what = Pressure;
2892        sname = "Pressure";
2893        break;
2894   default: // this takes care of all the gestures
2895        what = handle;
2896        sname = "Others";
2897        break;
2898    }
2899
2900    LOGI_IF(EXTRA_VERBOSE, "HAL:getHandle - what=%d, sname=%s", what, sname.string());
2901    return;
2902}
2903
2904int MPLSensor::setDelay(int32_t handle, int64_t ns)
2905{
2906    VFUNC_LOG;
2907
2908    android::String8 sname;
2909    int what = -1;
2910
2911#if 0
2912    // skip the 1st call for enalbing sensors called by ICS/JB sensor service
2913    static int counter_delay = 0;
2914    if (!(mEnabled & (1 << what))) {
2915        counter_delay = 0;
2916    } else {
2917        if (++counter_delay == 1) {
2918            return 0;
2919        }
2920        else {
2921            counter_delay = 0;
2922        }
2923    }
2924#endif
2925
2926    getHandle(handle, what, sname);
2927    if (uint32_t(what) >= NumSensors)
2928        return -EINVAL;
2929
2930    if (ns < 0)
2931        return -EINVAL;
2932
2933    LOGV_IF(PROCESS_VERBOSE,
2934            "setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns);
2935
2936    // limit all rates to reasonable ones */
2937    if (ns < 5000000LL) {
2938        ns = 5000000LL;
2939    }
2940
2941    /* store request rate to mDelays arrary for each sensor */
2942    mDelays[what] = ns;
2943
2944    switch (what) {
2945        case ID_SC:
2946            /* set limits of delivery rate of events */
2947            mStepCountPollTime = ns;
2948            LOGV_IF(ENG_VERBOSE, "step count rate =%lld ns", ns);
2949            break;
2950        case ID_P:
2951        case SignificantMotion:
2952        case ID_SO:
2953            update_delay();
2954            break;
2955        case Gyro:
2956        case RawGyro:
2957        case Accelerometer:
2958            for (int i = Gyro;
2959                    i <= Accelerometer + mCompassSensor->isIntegrated();
2960                    i++) {
2961                if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
2962                    LOGV_IF(PROCESS_VERBOSE,
2963                            "HAL:ignore delay set due to sensor %d", i);
2964                    return 0;
2965                }
2966            }
2967            break;
2968
2969        case MagneticField:
2970        case RawMagneticField:
2971            if (mCompassSensor->isIntegrated() &&
2972                    (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) ||
2973                    ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) ||
2974                    ((mEnabled & (1 << Accelerometer)) &&
2975                        ns > mDelays[Accelerometer]))) {
2976                 LOGV_IF(PROCESS_VERBOSE,
2977                         "HAL:ignore delay set due to gyro/accel");
2978                 return 0;
2979            }
2980            break;
2981
2982        case Orientation:
2983        case RotationVector:
2984        case GameRotationVector:
2985        case GeomagneticRotationVector:
2986        case LinearAccel:
2987        case Gravity:
2988            if (isLowPowerQuatEnabled()) {
2989                LOGV_IF(PROCESS_VERBOSE,
2990                        "HAL:need to update delay due to LPQ");
2991                break;
2992            }
2993
2994            for (int i = 0; i < NumSensors; i++) {
2995                if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
2996                    LOGV_IF(PROCESS_VERBOSE,
2997                            "HAL:ignore delay set due to sensor %d", i);
2998                    return 0;
2999                }
3000            }
3001            break;
3002    }
3003
3004    // pthread_mutex_lock(&mHALMutex);
3005    int res = update_delay();
3006    // pthread_mutex_unlock(&mHALMutex);
3007    return res;
3008}
3009
3010int MPLSensor::update_delay(void)
3011{
3012    VHANDLER_LOG;
3013
3014    int res = 0;
3015    int64_t got;
3016
3017    if (mEnabled) {
3018        int64_t wanted = 1000000000LL;
3019        int64_t wanted_3rd_party_sensor = 1000000000LL;
3020
3021        // Sequence to change sensor's FIFO rate
3022        // 1. reset master enable
3023        // 2. Update delay
3024        // 3. set master enable
3025
3026        // reset master enable
3027        masterEnable(0);
3028
3029        int64_t gyroRate;
3030        int64_t accelRate;
3031        int64_t compassRate;
3032        int64_t pressureRate;
3033
3034        int rateInus;
3035        int mplGyroRate;
3036        int mplAccelRate;
3037        int mplCompassRate;
3038        int tempRate = wanted;
3039
3040        /* search the minimum delay requested across all enabled sensors */
3041        for (int i = 0; i < NumSensors; i++) {
3042            if (mEnabled & (1 << i)) {
3043                int64_t ns = mDelays[i];
3044                wanted = wanted < ns ? wanted : ns;
3045            }
3046        }
3047
3048        if (mDmpOn) {
3049            gyroRate = mDelays[Gyro] < mDelays[RawGyro] ? mDelays[Gyro] : mDelays[RawGyro];
3050            accelRate = mDelays[Accelerometer];
3051            compassRate = mDelays[MagneticField] < mDelays[RawMagneticField] ? mDelays[MagneticField] : mDelays[RawMagneticField];
3052            pressureRate = mDelays[Pressure];
3053
3054#ifdef ENABLE_MULTI_RATE
3055            gyroRate = wanted;
3056            accelRate = wanted;
3057            compassRate = wanted;
3058            pressureRate = wanted;
3059            // same delay for 3rd party Accel or Compass
3060            wanted_3rd_party_sensor = wanted;
3061#endif
3062
3063        }
3064        else {
3065            gyroRate = wanted;
3066            accelRate = wanted;
3067            compassRate = wanted;
3068            pressureRate = wanted;
3069            // same delay for 3rd party Accel or Compass
3070            wanted_3rd_party_sensor = wanted;
3071        }
3072
3073        /* mpl rate in us in future maybe different for
3074           gyro vs compass vs accel */
3075 /*       rateInus = (int)wanted / 1000LL;
3076        mplGyroRate = (int)gyroRate / 1000LL;
3077        mplAccelRate = (int)accelRate / 1000LL;
3078        mplCompassRate = (int)compassRate / 1000LL;
3079
3080        LOGV_IF(PROCESS_VERBOSE, "HAL:wanted rate for all sensors : "
3081             "%llu ns, mpl rate: %d us, (%.2f Hz)",
3082             wanted, rateInus, 1000000000.f / wanted);
3083
3084        /* set rate in MPL */
3085        /* compass can only do 100Hz max */
3086 /*       inv_set_gyro_sample_rate(mplGyroRate);
3087        inv_set_accel_sample_rate(mplAccelRate);
3088        inv_set_compass_sample_rate(mplCompassRate);
3089
3090        LOGV_IF(PROCESS_VERBOSE,
3091                "HAL:MPL gyro sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplGyroRate, 1000000000.f / gyroRate);
3092        LOGV_IF(PROCESS_VERBOSE,
3093                "HAL:MPL accel sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplAccelRate, 1000000000.f / accelRate);
3094        LOGV_IF(PROCESS_VERBOSE,
3095                "HAL:MPL compass sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplCompassRate, 1000000000.f / compassRate);
3096*/
3097        int enabled_sensors = mEnabled;
3098        int tempFd = -1;
3099
3100        if(mFeatureActiveMask & INV_DMP_BATCH_MODE) {
3101            // set batch rates
3102            LOGV_IF(ENG_VERBOSE, "HAL: mFeatureActiveMask=%016llx", mFeatureActiveMask);
3103            LOGV("HAL: batch mode is set, set batch data rates");
3104            if (setBatchDataRates() < 0) {
3105                LOGE("HAL:ERR can't set batch data rates");
3106            }
3107        } else {
3108        if (LA_ENABLED || GR_ENABLED || RV_ENABLED
3109                       || GRV_ENABLED || O_ENABLED || GMRV_ENABLED) {
3110            LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
3111            //TODO: may be able to combine DMP_FEATURE_MASK, DMP_SENSOR_MASK in the future
3112            if(mFeatureActiveMask & DMP_FEATURE_MASK) {
3113                bool setDMPrate= 0;
3114                gyroRate = wanted;
3115                accelRate = wanted;
3116                compassRate = wanted;
3117                // same delay for 3rd party Accel or Compass
3118                wanted_3rd_party_sensor = wanted;
3119                rateInus = (int)wanted / 1000LL;
3120
3121                 /* set rate in MPL */
3122                 /* compass can only do 100Hz max */
3123                 inv_set_gyro_sample_rate(rateInus);
3124                 inv_set_accel_sample_rate(rateInus);
3125                 inv_set_compass_sample_rate(rateInus);
3126
3127                 LOGV_IF(PROCESS_VERBOSE,
3128                "HAL:MPL gyro sample rate: (mpl)=%d us (mpu)=%.2f Hz", rateInus, 1000000000.f / gyroRate);
3129                 LOGV_IF(PROCESS_VERBOSE,
3130                "HAL:MPL accel sample rate: (mpl)=%d us (mpu)=%.2f Hz", rateInus, 1000000000.f / accelRate);
3131                 LOGV_IF(PROCESS_VERBOSE,
3132                "HAL:MPL compass sample rate: (mpl)=%d us (mpu)=%.2f Hz", rateInus, 1000000000.f / compassRate);
3133
3134                // Set LP Quaternion sample rate if enabled
3135                if (checkLPQuaternion()) {
3136                    if (wanted <= RATE_200HZ) {
3137#ifndef USE_LPQ_AT_FASTEST
3138                        enableLPQuaternion(0);
3139#endif
3140                    } else {
3141                        inv_set_quat_sample_rate(rateInus);
3142                        setDMPrate= 1;
3143                    }
3144                }
3145                //if((mFeatureActiveMask & DMP_SENSOR_MASK) || setDMPrate==1) {
3146                //    getDmpRate(&wanted);
3147                //}
3148            }
3149
3150            LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion");
3151            //nsToHz
3152            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
3153                    1000000000.f / gyroRate, mpu.gyro_rate,
3154                    getTimestamp());
3155            tempFd = open(mpu.gyro_rate, O_RDWR);
3156            res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate);
3157            if(res < 0) {
3158                LOGE("HAL:GYRO update delay error");
3159            }
3160
3161            if(USE_THIRD_PARTY_ACCEL == 1) {
3162            // 3rd party accelerometer - if applicable
3163            // nsToHz (BMA250)
3164                LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)",
3165                        wanted_3rd_party_sensor / 1000000L, mpu.accel_rate,
3166                        getTimestamp());
3167                tempFd = open(mpu.accel_rate, O_RDWR);
3168                res = write_attribute_sensor(tempFd,
3169                        wanted_3rd_party_sensor / 1000000L);
3170                LOGE_IF(res < 0, "HAL:ACCEL update delay error");
3171            } else {
3172            // mpu accel
3173               LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
3174                        1000000000.f / accelRate, mpu.accel_rate,
3175                        getTimestamp());
3176                tempFd = open(mpu.accel_rate, O_RDWR);
3177                res = write_attribute_sensor(tempFd, 1000000000.f / accelRate);
3178                LOGE_IF(res < 0, "HAL:ACCEL update delay error");
3179            }
3180
3181            if (!mCompassSensor->isIntegrated()) {
3182            // stand-alone compass - if applicable
3183                LOGV_IF(PROCESS_VERBOSE,
3184                        "HAL:Ext compass delay %lld", wanted_3rd_party_sensor);
3185                LOGV_IF(PROCESS_VERBOSE, "HAL:Ext compass rate %.2f Hz",
3186                        1000000000.f / wanted_3rd_party_sensor);
3187                if (wanted_3rd_party_sensor <
3188                        mCompassSensor->getMinDelay() * 1000LL) {
3189                    wanted_3rd_party_sensor =
3190                        mCompassSensor->getMinDelay() * 1000LL;
3191                }
3192                LOGV_IF(PROCESS_VERBOSE,
3193                        "HAL:Ext compass delay %lld", wanted_3rd_party_sensor);
3194                LOGV_IF(PROCESS_VERBOSE, "HAL:Ext compass rate %.2f Hz",
3195                        1000000000.f / wanted_3rd_party_sensor);
3196                mCompassSensor->setDelay(ID_M, wanted_3rd_party_sensor);
3197                got = mCompassSensor->getDelay(ID_M);
3198                inv_set_compass_sample_rate(got / 1000);
3199            } else {
3200            // compass on secondary bus
3201                if (compassRate < mCompassSensor->getMinDelay() * 1000LL) {
3202                    compassRate = mCompassSensor->getMinDelay() * 1000LL;
3203                }
3204                mCompassSensor->setDelay(ID_M, compassRate);
3205            }
3206
3207            /*
3208            //nsTons - nothing to be done
3209            strcpy(&compass_sensor_sysfs_path[compass_sensor_sysfs_path_len],
3210                   COMPASS_SENSOR_DELAY);
3211            tempFd = open(compass_sensor_sysfs_path, O_RDWR);
3212            LOGV_IF(PROCESS_VERBOSE,
3213                    "setDelay - open path: %s", compass_sensor_sysfs_path);
3214            wanted = 20000000LLU;
3215            res = write_attribute_sensor(tempFd, wanted);
3216            if(res < 0) {
3217                LOGE("Compass update delay error");
3218            }
3219            */
3220
3221        } else {
3222
3223            if (GY_ENABLED || RGY_ENABLED) {
3224                wanted = (mDelays[Gyro] <= mDelays[RawGyro]?
3225                    (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]):
3226                    (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro]));
3227                LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
3228                if (mFeatureActiveMask & DMP_FEATURE_MASK) {
3229                    //int64_t tempWanted;
3230                    //getDmpRate(&tempWanted);
3231                }
3232
3233                inv_set_gyro_sample_rate((int)wanted/1000LL);
3234                LOGV_IF(PROCESS_VERBOSE,
3235                    "HAL:MPL gyro sample rate: (mpl)=%d us", int(wanted/1000LL));
3236                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
3237                        1000000000.f / wanted, mpu.gyro_rate, getTimestamp());
3238                tempFd = open(mpu.gyro_rate, O_RDWR);
3239                res = write_attribute_sensor(tempFd, 1000000000.f / wanted);
3240                LOGE_IF(res < 0, "HAL:GYRO update delay error");
3241            }
3242
3243            if (A_ENABLED) { /* there is only 1 fifo rate for MPUxxxx */
3244
3245#if (0)
3246                wanted = mDelays[Accelerometer];
3247#else
3248                if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) {
3249                    wanted = mDelays[Gyro];
3250                } else if (RGY_ENABLED && mDelays[RawGyro]
3251                            < mDelays[Accelerometer]) {
3252                    wanted = mDelays[RawGyro];
3253                } else {
3254                    wanted = mDelays[Accelerometer];
3255                }
3256#endif
3257                LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
3258                if (mFeatureActiveMask & DMP_FEATURE_MASK) {
3259                    //int64_t tempWanted;
3260                    //getDmpRate(&tempWanted);
3261                }
3262
3263                inv_set_accel_sample_rate((int)wanted/1000LL);
3264                LOGV_IF(PROCESS_VERBOSE,
3265                    "HAL:MPL accel sample rate: (mpl)=%d us", int(wanted/1000LL));
3266                /* TODO: use function pointers to calculate delay value specific
3267                   to vendor */
3268                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
3269                        1000000000.f / wanted, mpu.accel_rate,
3270                        getTimestamp());
3271                tempFd = open(mpu.accel_rate, O_RDWR);
3272                if(USE_THIRD_PARTY_ACCEL == 1) {
3273                    //BMA250 in ms
3274                    res = write_attribute_sensor(tempFd, wanted / 1000000L);
3275                }
3276                else {
3277                    //MPUxxxx in hz
3278                    res = write_attribute_sensor(tempFd, 1000000000.f/wanted);
3279                }
3280                LOGE_IF(res < 0, "HAL:ACCEL update delay error");
3281            }
3282
3283            /* Invensense compass calibration */
3284            if (M_ENABLED || RM_ENABLED) {
3285                int64_t compassWanted = (mDelays[MagneticField] <= mDelays[RawMagneticField]?
3286                    (mEnabled & (1 << MagneticField)? mDelays[MagneticField]: mDelays[RawMagneticField]):
3287                    (mEnabled & (1 << RawMagneticField)? mDelays[RawMagneticField]: mDelays[MagneticField]));
3288                if (!mCompassSensor->isIntegrated()) {
3289                    wanted = compassWanted;
3290                } else {
3291#if (0)
3292                    wanted = compassWanted;
3293#else
3294                    if (GY_ENABLED
3295                        && (mDelays[Gyro] < compassWanted)) {
3296                        wanted = mDelays[Gyro];
3297                    } else if (RGY_ENABLED
3298                               && mDelays[RawGyro] < compassWanted) {
3299                        wanted = mDelays[RawGyro];
3300                    } else if (A_ENABLED && mDelays[Accelerometer]
3301                                < compassWanted) {
3302                        wanted = mDelays[Accelerometer];
3303                    } else {
3304                        wanted = compassWanted;
3305                    }
3306#endif
3307                    LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
3308                    if (mFeatureActiveMask & DMP_FEATURE_MASK) {
3309                        //int64_t tempWanted;
3310                        //getDmpRate(&tempWanted);
3311                    }
3312                }
3313
3314                mCompassSensor->setDelay(ID_M, wanted);
3315                got = mCompassSensor->getDelay(ID_M);
3316                inv_set_compass_sample_rate(got / 1000);
3317                LOGV_IF(PROCESS_VERBOSE,
3318                    "HAL:MPL compass sample rate: (mpl)=%d us", int(got/1000LL));
3319            }
3320
3321            if (PS_ENABLED) {
3322#if (0)
3323                wanted = mDelays[Pressure];
3324#else
3325                wanted = mDelays[Pressure];
3326#endif
3327                LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
3328                if (mFeatureActiveMask & DMP_FEATURE_MASK) {
3329                    //int64_t tempWanted;
3330                    //getDmpRate(&tempWanted);
3331                }
3332
3333                if (mPressureSensor) {
3334                    mPressureSensor->setDelay(ID_PS, wanted);
3335                } else {
3336                    LOGV_IF(ENG_VERBOSE, "HAL:PRESSURE sensor not detected");
3337                }
3338                LOGE_IF(res < 0, "HAL:PRESSURE update delay error");
3339            }
3340        }
3341
3342        /* set master sampling frequency */
3343        /* only use for non multi rate */
3344        int64_t tempWanted = wanted;
3345        getDmpRate(&tempWanted);
3346        /* driver only looks at sampling frequency if DMP is off */
3347        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
3348                1000000000.f / tempWanted, mpu.gyro_fifo_rate, getTimestamp());
3349        tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
3350        res = write_attribute_sensor(tempFd, 1000000000.f / tempWanted);
3351            LOGE_IF(res < 0, "HAL:sampling frequency update delay error");
3352        } //end of non batch mode
3353
3354        unsigned long sensors = mLocalSensorMask & mMasterSensorMask;
3355        if (sensors &
3356            (INV_THREE_AXIS_GYRO
3357                | INV_THREE_AXIS_ACCEL
3358                | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()
3359                | (INV_ONE_AXIS_PRESSURE * mPressureSensor->isIntegrated())))) {
3360            LOGV_IF(ENG_VERBOSE, "sensors=%lu", sensors);
3361            res = masterEnable(1);
3362            if(res < 0)
3363                return res;
3364        } else { // all sensors idle -> reduce power, unless DMP is needed
3365            LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
3366            if(mFeatureActiveMask & DMP_FEATURE_MASK) {
3367                res = masterEnable(1);
3368                if(res < 0)
3369                    return res;
3370            }
3371        }
3372    }
3373
3374    return res;
3375}
3376
3377/* For Third Party Accel Input Subsystem Drivers only */
3378int MPLSensor::readAccelEvents(sensors_event_t* data, int count)
3379{
3380    VHANDLER_LOG;
3381
3382    if (count < 1)
3383        return -EINVAL;
3384
3385    ssize_t n = mAccelInputReader.fill(accel_fd);
3386    if (n < 0) {
3387        LOGE("HAL:missed accel events, exit");
3388        return n;
3389    }
3390
3391    int numEventReceived = 0;
3392    input_event const* event;
3393    int nb, done = 0;
3394
3395    while (done == 0 && count && mAccelInputReader.readEvent(&event)) {
3396        int type = event->type;
3397        if (type == EV_ABS) {
3398            if (event->code == EVENT_TYPE_ACCEL_X) {
3399                mPendingMask |= 1 << Accelerometer;
3400                mCachedAccelData[0] = event->value;
3401            } else if (event->code == EVENT_TYPE_ACCEL_Y) {
3402                mPendingMask |= 1 << Accelerometer;
3403                mCachedAccelData[1] = event->value;
3404            } else if (event->code == EVENT_TYPE_ACCEL_Z) {
3405                mPendingMask |= 1 << Accelerometer;
3406                mCachedAccelData[2] =event-> value;
3407            }
3408        } else if (type == EV_SYN) {
3409            done = 1;
3410            if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
3411                inv_build_accel(mCachedAccelData, 0, getTimestamp());
3412            }
3413        } else {
3414            LOGE("HAL:AccelSensor: unknown event (type=%d, code=%d)",
3415                    type, event->code);
3416        }
3417        mAccelInputReader.next();
3418    }
3419
3420    return numEventReceived;
3421}
3422
3423/**
3424 *  Should be called after reading at least one of gyro
3425 *  compass or accel data. (Also okay for handling all of them).
3426 *  @returns 0, if successful, error number if not.
3427 */
3428int MPLSensor::readEvents(sensors_event_t* data, int count)
3429{
3430    //VFUNC_LOG;
3431
3432    inv_execute_on_data();
3433
3434    int numEventReceived = 0;
3435
3436    long msg;
3437    msg = inv_get_message_level_0(1);
3438    if (msg) {
3439        if (msg & INV_MSG_MOTION_EVENT) {
3440            LOGV_IF(PROCESS_VERBOSE, "HAL:**** Motion ****\n");
3441        }
3442        if (msg & INV_MSG_NO_MOTION_EVENT) {
3443            LOGV_IF(PROCESS_VERBOSE, "HAL:***** No Motion *****\n");
3444            /* after the first no motion, the gyro should be
3445               calibrated well */
3446            mGyroAccuracy = SENSOR_STATUS_ACCURACY_HIGH;
3447            /* if gyros are on and we got a no motion, set a flag
3448               indicating that the cal file can be written. */
3449            mHaveGoodMpuCal = true;
3450        }
3451        if(msg & INV_MSG_NEW_AB_EVENT) {
3452            getAccelBias();
3453            mAccelAccuracy = inv_get_accel_accuracy();
3454        }
3455        if(msg & INV_MSG_NEW_GB_EVENT) {
3456            getGyroBias();
3457        }
3458        if(msg & INV_MSG_NEW_FGB_EVENT) {
3459            getFactoryGyroBias();
3460        }
3461        if(msg & INV_MSG_NEW_CB_EVENT) {
3462            getCompassBias();
3463            mCompassAccuracy = inv_get_mag_accuracy();
3464        }
3465    }
3466
3467    for (int i = 0; i < NumSensors; i++) {
3468        int update = 0;
3469        // handle step detector when ped_q is enabled
3470        if ((i == StepDetector) && (mPedUpdate == 1)) {
3471            mPedUpdate = 0;
3472            update = readDmpPedometerEvents(data, count, ID_P, SENSOR_TYPE_STEP_DETECTOR, 1);
3473            if(update == 1 && count > 0) {
3474                data->timestamp = mStepSensorTimestamp;
3475                //LOGI("sensor=%d type=%d data=%d", data->sensor, data->type, data->data[0]);
3476                count--;
3477                numEventReceived++;
3478                continue;
3479            }
3480        }
3481
3482        // load up virtual sensors
3483        if (mEnabled & (1 << i)) {
3484            update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i);
3485            mPendingMask |= (1 << i);
3486
3487            if (update && (count > 0)) {
3488                *data++ = mPendingEvents[i];
3489                count--;
3490                numEventReceived++;
3491            }
3492        }
3493    }
3494
3495    return numEventReceived;
3496}
3497
3498// collect data for MPL (but NOT sensor service currently), from driver layer
3499void MPLSensor::buildMpuEvent(void)
3500{
3501    int64_t mGyroSensorTimestamp=0, mAccelSensorTimestamp=0, latestTimestamp=0;
3502    int lp_quaternion_on = 0, sixAxis_quaternion_on = 0,
3503        ped_quaternion_on = 0, ped_standalone_on = 0;
3504    size_t nbyte;
3505    unsigned short data_format = 0;
3506    int i, nb, mask = 0,
3507        sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 : 0) +
3508            ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 : 0) +
3509            (((mLocalSensorMask & INV_THREE_AXIS_COMPASS)
3510                && mCompassSensor->isIntegrated())? 1 : 0) +
3511            ((mLocalSensorMask & INV_ONE_AXIS_PRESSURE)? 1 : 0);
3512    //LOGV("mLocalSensorMask=0x%lx", mLocalSensorMask);
3513    char *rdata = mIIOBuffer;
3514    ssize_t rsize = 0;
3515    size_t readCounter = 0;
3516    char *rdataP = NULL;
3517
3518    /* 2 Bytes header + 6 Bytes x,y,z data | 8 bytes timestamp */
3519    nbyte= (BYTES_PER_SENSOR + 8) * sensors * 1;
3520
3521    /* special case for 6 Axis or LPQ */
3522    /* 2 Bytes header + 4 Bytes x data + 2 Bytes n/a */
3523    /* 4 Bytes y data | 4 Bytes z data */
3524    /* 8 Bytes timestamp */
3525    if (isLowPowerQuatEnabled()) {
3526        lp_quaternion_on = checkLPQuaternion();
3527        if (lp_quaternion_on == 1) {
3528            nbyte += BYTES_QUAT_DATA;
3529        }
3530    }
3531
3532    if ((sixAxis_quaternion_on = check6AxisQuatEnabled())) {
3533        // sixAxis is mutually exclusive to LPQ
3534        // and it is also never enabled when continuous data is enabled
3535        // mLocalSensorMask does not need to be accounted for here
3536        // because accel/gyro are turned off anyway
3537        nbyte += BYTES_QUAT_DATA;
3538    }
3539
3540    if ((ped_quaternion_on = checkPedQuatEnabled())) {
3541        nbyte += BYTES_PER_SENSOR_PACKET;
3542    }
3543
3544    if ((ped_standalone_on = checkPedStandaloneEnabled())) {
3545        nbyte += BYTES_PER_SENSOR_PACKET;
3546    }
3547
3548    /* check previous copied buffer */
3549    /* append with just read data */
3550    if (mLeftOverBufferSize > 0) {
3551        LOGV_IF(0, "append old buffer size=%d", mLeftOverBufferSize);
3552        memcpy(rdata, mLeftOverBuffer, mLeftOverBufferSize);
3553            LOGV_IF(0,
3554            "HAL:input retrieve rdata=:%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, "
3555            "%d, %d,%d, %d, %d, %d\n",
3556            rdata[0], rdata[1], rdata[2], rdata[3],
3557            rdata[4], rdata[5], rdata[6], rdata[7],
3558            rdata[8], rdata[9], rdata[10], rdata[11],
3559            rdata[12], rdata[13], rdata[14], rdata[15]);
3560    }
3561    rdataP = rdata + mLeftOverBufferSize;
3562
3563    /* read expected  number of bytes */
3564    rsize =  read(iio_fd, rdataP, nbyte);
3565    if(rsize < 0) {
3566        /* IIO buffer might have old data.
3567           Need to flush it when enabling no sensors, to avoid infinite
3568           read loop.*/
3569        LOGE("HAL:input data file descriptor not available - (%s)",
3570             strerror(errno));
3571        if (sensors == 0) {
3572            rsize = read(iio_fd, rdata, MAX_SUSPEND_BATCH_PACKET_SIZE);
3573        }
3574        return;
3575    }
3576
3577    /* reset data and count pointer */
3578    rdataP = rdata;
3579    readCounter = rsize + mLeftOverBufferSize;
3580
3581#ifdef TESTING
3582    LOGV_IF(INPUT_DATA,
3583         "HAL:input rdataP:r=%ld, n=%d,"
3584         "%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d\n",
3585         rsize, nbyte,
3586         rdataP[0], rdataP[1], rdataP[2], rdataP[3],
3587         rdataP[4], rdataP[5], rdataP[6], rdataP[7],
3588         rdataP[8], rdataP[9], rdataP[10], rdataP[11],
3589         rdataP[12], rdataP[13], rdataP[14], rdataP[15]);
3590#endif
3591
3592    LOGV_IF(INPUT_DATA && ENG_VERBOSE,
3593            "HAL:input rdata= %d nbyte= %d rsize= %ld readCounter= %d",
3594            *((short *) rdata), nbyte, rsize, readCounter);
3595    LOGV_IF(INPUT_DATA && ENG_VERBOSE,
3596            "HAL:input sensors= %d, lp_q_on= %d, 6axis_q_on= %d, "
3597            "ped_q_on= %d, ped_standalone_on= %d",
3598            sensors, lp_quaternion_on, sixAxis_quaternion_on,
3599            ped_quaternion_on, ped_standalone_on);
3600
3601    while (readCounter > 0) {
3602        mLeftOverBufferSize = 0;
3603        mask = 0;
3604        data_format = *((short *)(rdata));
3605        LOGV_IF(INPUT_DATA && ENG_VERBOSE,
3606                "HAL:input data_format=%x", data_format);
3607
3608        if ((data_format & ~DATA_FORMAT_MASK) || (data_format == 0)) {
3609            LOGE("HAL:input invalid data_format 0x%02X", data_format);
3610            return;
3611        }
3612
3613        if (data_format & DATA_FORMAT_STEP) {
3614            LOGV_IF(ENG_VERBOSE, "STEP DETECTED:0x%x", data_format);
3615            mPedUpdate = 1;
3616            mask |= DATA_FORMAT_STEP;
3617            // cancels step bit
3618            data_format &= (~DATA_FORMAT_STEP);
3619        }
3620
3621        if (data_format == DATA_FORMAT_QUAT) {
3622            mCachedQuaternionData[0] = *((int *) (rdata + 4));
3623            mCachedQuaternionData[1] = *((int *) (rdata + 8));
3624            mCachedQuaternionData[2] = *((int *) (rdata + 12));
3625            rdata += QUAT_ONLY_LAST_PACKET_OFFSET;
3626            mQuatSensorTimestamp = *((long long*) (rdata));
3627            mask |= DATA_FORMAT_QUAT;
3628            readCounter -= BYTES_QUAT_DATA;
3629        }
3630        else if (data_format == DATA_FORMAT_6_AXIS) {
3631            mCached6AxisQuaternionData[0] = *((int *) (rdata + 4));
3632            mCached6AxisQuaternionData[1] = *((int *) (rdata + 8));
3633            mCached6AxisQuaternionData[2] = *((int *) (rdata + 12));
3634            rdata += QUAT_ONLY_LAST_PACKET_OFFSET;
3635            mQuatSensorTimestamp = *((long long*) (rdata));
3636            mask |= DATA_FORMAT_6_AXIS;
3637            readCounter -= BYTES_QUAT_DATA;
3638        }
3639        else if (data_format == DATA_FORMAT_PED_QUAT) {
3640            mCachedPedQuaternionData[0] = *((short *) (rdata + 2));
3641            mCachedPedQuaternionData[1] = *((short *) (rdata + 4));
3642            mCachedPedQuaternionData[2] = *((short *) (rdata + 6));
3643            rdata += BYTES_PER_SENSOR;
3644            mQuatSensorTimestamp = *((long long*) (rdata));
3645            mask |= DATA_FORMAT_PED_QUAT;
3646            readCounter -= BYTES_PER_SENSOR_PACKET;
3647        }
3648        else if (data_format == DATA_FORMAT_PED_STANDALONE) {
3649            LOGV_IF(ENG_VERBOSE, "STEP DETECTED:0x%x", data_format);
3650            rdata += BYTES_PER_SENSOR;
3651            mStepSensorTimestamp = *((long long*) (rdata));
3652            mask |= DATA_FORMAT_PED_STANDALONE;
3653            readCounter -= BYTES_PER_SENSOR_PACKET;
3654            mPedUpdate = 1;
3655        }
3656        else if (data_format == DATA_FORMAT_GYRO) {
3657            mCachedGyroData[0] = *((short *) (rdata + 2));
3658            mCachedGyroData[1] = *((short *) (rdata + 4));
3659            mCachedGyroData[2] = *((short *) (rdata + 6));
3660            rdata += BYTES_PER_SENSOR;
3661            mGyroSensorTimestamp = *((long long*) (rdata));
3662            mask |= DATA_FORMAT_GYRO;
3663            readCounter -= BYTES_PER_SENSOR_PACKET;
3664        }
3665        else if (data_format == DATA_FORMAT_ACCEL) {
3666            mCachedAccelData[0] = *((short *) (rdata + 2));
3667            mCachedAccelData[1] = *((short *) (rdata + 4));
3668            mCachedAccelData[2] = *((short *) (rdata + 6));
3669            rdata += BYTES_PER_SENSOR;
3670            mAccelSensorTimestamp = *((long long*) (rdata));
3671            mask |= DATA_FORMAT_ACCEL;
3672            readCounter -= BYTES_PER_SENSOR_PACKET;
3673        }
3674        else  if (data_format == DATA_FORMAT_COMPASS) {
3675            if (mCompassSensor->isIntegrated()) {
3676                mCachedCompassData[0] = *((short *) (rdata + 2));
3677                mCachedCompassData[1] = *((short *) (rdata + 4));
3678                mCachedCompassData[2] = *((short *) (rdata + 6));
3679                rdata += BYTES_PER_SENSOR;
3680                mCompassTimestamp = *((long long*) (rdata));
3681                if (mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0
3682                                               || mCachedCompassData[2] != 0) {
3683                    mask |= DATA_FORMAT_COMPASS;
3684                }
3685                readCounter -= BYTES_PER_SENSOR_PACKET;
3686            }
3687        }
3688        else if (data_format == DATA_FORMAT_PRESSURE) {
3689            if (mPressureSensor->isIntegrated()) {
3690                mCachedPressureData =
3691                    ((*((short *)(rdata + 4))) << 16) +
3692                    (*((unsigned short *) (rdata + 6)));
3693                rdata += BYTES_PER_SENSOR;
3694                mPressureTimestamp = *((long long*) (rdata));
3695                if (mCachedPressureData!= 0) {
3696                    mask |= DATA_FORMAT_PRESSURE;
3697                }
3698                readCounter -= BYTES_PER_SENSOR_PACKET;
3699            }
3700        }
3701        rdata += BYTES_PER_SENSOR;
3702
3703        /* read ahead and store left over data if any */
3704        if ((readCounter != 0) && (rsize != (ssize_t)nbyte) && (readCounter <= 24)) {
3705            int counter = readCounter;
3706            int currentBufferCounter = 0;
3707            LOGV_IF(0, "!!! not enough data readCounter=%d, expected nbyte=%d", readCounter, nbyte);
3708            memcpy(mLeftOverBuffer, rdata, readCounter);
3709            LOGV_IF(0,
3710                    "HAL:input store rdata=:%d, %d, %d, %d,%d, %d, %d, %d,%d, "
3711                    "%d, %d, %d,%d, %d, %d, %d\n",
3712                    mLeftOverBuffer[0], mLeftOverBuffer[1], mLeftOverBuffer[2], mLeftOverBuffer[3],
3713                    mLeftOverBuffer[4], mLeftOverBuffer[5], mLeftOverBuffer[6], mLeftOverBuffer[7],
3714                    mLeftOverBuffer[8], mLeftOverBuffer[9], mLeftOverBuffer[10], mLeftOverBuffer[11],
3715                    mLeftOverBuffer[12],mLeftOverBuffer[13],mLeftOverBuffer[14], mLeftOverBuffer[15]);
3716
3717            mLeftOverBufferSize = readCounter;
3718            readCounter = 0;
3719            LOGV_IF(0, "!!! stored number of bytes:%d", mLeftOverBufferSize);
3720        }
3721
3722        /* handle data read */
3723        if (mask & DATA_FORMAT_GYRO) {
3724            /* batch mode does not batch temperature */
3725            /* disable temperature read */
3726            if (!(mFeatureActiveMask & INV_DMP_BATCH_MODE)) {
3727                // send down temperature every 0.5 seconds
3728                // with timestamp measured in "driver" layer
3729                if(mGyroSensorTimestamp - mTempCurrentTime >= 500000000LL) {
3730                    mTempCurrentTime = mGyroSensorTimestamp;
3731                    long long temperature[2];
3732                    if(inv_read_temperature(temperature) == 0) {
3733                        LOGV_IF(INPUT_DATA,
3734                        "HAL:input inv_read_temperature = %lld, timestamp= %lld",
3735                        temperature[0], temperature[1]);
3736                        inv_build_temp(temperature[0], temperature[1]);
3737                     }
3738#ifdef TESTING
3739                    long bias[3], temp, temp_slope[3];
3740                    inv_get_mpl_gyro_bias(bias, &temp);
3741                    inv_get_gyro_ts(temp_slope);
3742                    LOGI("T: %.3f "
3743                     "GB: %+13f %+13f %+13f "
3744                     "TS: %+13f %+13f %+13f "
3745                     "\n",
3746                     (float)temperature[0] / 65536.f,
3747                     (float)bias[0] / 65536.f / 16.384f,
3748                     (float)bias[1] / 65536.f / 16.384f,
3749                     (float)bias[2] / 65536.f / 16.384f,
3750                     temp_slope[0] / 65536.f,
3751                     temp_slope[1] / 65536.f,
3752                     temp_slope[2] / 65536.f);
3753#endif
3754                }
3755            }
3756            mPendingMask |= 1 << Gyro;
3757            mPendingMask |= 1 << RawGyro;
3758
3759            if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
3760                inv_build_gyro(mCachedGyroData, mGyroSensorTimestamp);
3761                LOGV_IF(INPUT_DATA,
3762                        "HAL:input inv_build_gyro: %+8d %+8d %+8d - %lld",
3763                        mCachedGyroData[0], mCachedGyroData[1],
3764                        mCachedGyroData[2], mGyroSensorTimestamp);
3765           }
3766           latestTimestamp = mGyroSensorTimestamp;
3767        }
3768
3769        if (mask & DATA_FORMAT_ACCEL) {
3770            mPendingMask |= 1 << Accelerometer;
3771            if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
3772                inv_build_accel(mCachedAccelData, 0, mAccelSensorTimestamp);
3773                 LOGV_IF(INPUT_DATA,
3774                    "HAL:input inv_build_accel: %+8ld %+8ld %+8ld - %lld",
3775                    mCachedAccelData[0], mCachedAccelData[1],
3776                    mCachedAccelData[2], mAccelSensorTimestamp);
3777            }
3778            latestTimestamp = mAccelSensorTimestamp;
3779        }
3780
3781        if ((mask & DATA_FORMAT_COMPASS) && mCompassSensor->isIntegrated()) {
3782            int status = 0;
3783            if (mCompassSensor->providesCalibration()) {
3784                status = mCompassSensor->getAccuracy();
3785                status |= INV_CALIBRATED;
3786            }
3787            if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
3788            inv_build_compass(mCachedCompassData, status,
3789                              mCompassTimestamp);
3790            LOGV_IF(INPUT_DATA,
3791                    "HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld",
3792                    mCachedCompassData[0], mCachedCompassData[1],
3793                    mCachedCompassData[2], mCompassTimestamp);
3794            }
3795            latestTimestamp = mCompassTimestamp;
3796        }
3797
3798        if (isLowPowerQuatEnabled() && lp_quaternion_on == 1
3799                                && (mask & DATA_FORMAT_QUAT)) {
3800            /* if bias was applied to DMP bias,
3801               set status bits to disable gyro bias cal */
3802            int status = 0;
3803            if (mGyroBiasApplied == true) {
3804                LOGV_IF(INPUT_DATA, "HAL:input mpl bias not used");
3805                status |= INV_BIAS_APPLIED;
3806                mGyroBiasApplied = false;
3807            }
3808            status |= 32 | INV_QUAT_3AXIS; /* default 32 (16/32bits) */
3809            inv_build_quat(mCachedQuaternionData,
3810                       status,
3811                       mQuatSensorTimestamp);
3812            LOGV_IF(INPUT_DATA,
3813                    "HAL:input inv_build_quat: %+8ld %+8ld %+8ld - %lld",
3814                    mCachedQuaternionData[0], mCachedQuaternionData[1],
3815                    mCachedQuaternionData[2],
3816                    mQuatSensorTimestamp);
3817            latestTimestamp = mQuatSensorTimestamp;
3818        }
3819
3820        if ((mask & DATA_FORMAT_6_AXIS) && check6AxisQuatEnabled()
3821                                    && (sixAxis_quaternion_on == 1)) {
3822            /* if bias was applied to DMP bias,
3823               set status bits to disable gyro bias cal */
3824            int status = 0;
3825            if (mGyroBiasApplied == true) {
3826                status |= INV_QUAT_6AXIS;
3827                mGyroBiasApplied = false;
3828            }
3829            status |= 32 | INV_QUAT_3AXIS; /* default 32 (16/32bits) */
3830            inv_build_quat(mCached6AxisQuaternionData,
3831                       status,
3832                       mQuatSensorTimestamp);
3833            LOGV_IF(INPUT_DATA,
3834                "HAL:input 6 axis ped quat: %+8ld %+8ld %+8ld - %lld",
3835                mCached6AxisQuaternionData[0], mCached6AxisQuaternionData[1],
3836                mCached6AxisQuaternionData[2],
3837                mQuatSensorTimestamp);
3838            latestTimestamp = mQuatSensorTimestamp;
3839        }
3840
3841        if ((mask & DATA_FORMAT_PED_QUAT) && checkPedQuatEnabled()
3842                                          && (ped_quaternion_on == 1)) {
3843            /* if bias was applied to DMP bias,
3844               set status bits to disable gyro bias cal */
3845            int status = 0;
3846            if (mGyroBiasApplied == true) {
3847                status |= INV_QUAT_6AXIS;
3848                mGyroBiasApplied = false;
3849            }
3850            status |= 32 | INV_QUAT_3AXIS; /* default 32 (16/32bits) */
3851            inv_build_quat(mCachedPedQuaternionData,
3852                       status,
3853                       mQuatSensorTimestamp);
3854
3855            LOGV_IF(INPUT_DATA,
3856            "HAL:input ped quat: %+8ld %+8ld %+8ld - %lld",
3857            mCachedPedQuaternionData[0], mCachedPedQuaternionData[1],
3858            mCachedPedQuaternionData[2],
3859            mQuatSensorTimestamp);
3860            latestTimestamp = mQuatSensorTimestamp;
3861        }
3862
3863        if ((mask & DATA_FORMAT_PRESSURE) && mPressureSensor->isIntegrated()) {
3864            int status = 0;
3865            if (mLocalSensorMask & INV_ONE_AXIS_PRESSURE) {
3866
3867            LOGV_IF(INPUT_DATA,
3868                    "HAL:input inv_build_pressure: %+8ld - %lld",
3869                    mCachedPressureData, mPressureTimestamp);
3870            }
3871            latestTimestamp = mPressureTimestamp;
3872        }
3873
3874        /* take the latest timestamp */
3875        if (mask & DATA_FORMAT_STEP) {
3876            mStepSensorTimestamp = latestTimestamp;
3877        }
3878   }//while end
3879}
3880
3881/* use for both MPUxxxx and third party compass */
3882void MPLSensor::buildCompassEvent(void)
3883{
3884    VHANDLER_LOG;
3885
3886    int done = 0;
3887
3888    // pthread_mutex_lock(&mMplMutex);
3889    // pthread_mutex_lock(&mHALMutex);
3890
3891    done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp);
3892    if(mCompassSensor->isYasCompass()) {
3893        if (mCompassSensor->checkCoilsReset() == 1) {
3894           //Reset relevant compass settings
3895           resetCompass();
3896        }
3897    }
3898    if (done > 0) {
3899        int status = 0;
3900        if (mCompassSensor->providesCalibration()) {
3901            status = mCompassSensor->getAccuracy();
3902            status |= INV_CALIBRATED;
3903        }
3904        if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
3905            inv_build_compass(mCachedCompassData, status,
3906                              mCompassTimestamp);
3907            LOGV_IF(INPUT_DATA,
3908                    "HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld",
3909                    mCachedCompassData[0], mCachedCompassData[1],
3910                    mCachedCompassData[2], mCompassTimestamp);
3911        }
3912    }
3913
3914    // pthread_mutex_unlock(&mMplMutex);
3915    // pthread_mutex_unlock(&mHALMutex);
3916}
3917
3918int MPLSensor::resetCompass(void)
3919{
3920    VFUNC_LOG;
3921
3922    //Reset compass cal if enabled
3923    if (mMplFeatureActiveMask & INV_COMPASS_CAL) {
3924       LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal");
3925       inv_init_vector_compass_cal();
3926    }
3927
3928    //Reset compass fit if enabled
3929    if (mMplFeatureActiveMask & INV_COMPASS_FIT) {
3930       LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit");
3931       inv_init_compass_fit();
3932    }
3933
3934    return 0;
3935}
3936
3937int MPLSensor::getFd(void) const
3938{
3939    VFUNC_LOG;
3940    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getFd returning %d", iio_fd);
3941    return iio_fd;
3942}
3943
3944int MPLSensor::getAccelFd(void) const
3945{
3946    VFUNC_LOG;
3947    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getAccelFd returning %d", accel_fd);
3948    return accel_fd;
3949}
3950
3951int MPLSensor::getCompassFd(void) const
3952{
3953    VFUNC_LOG;
3954    int fd = mCompassSensor->getFd();
3955    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getCompassFd returning %d", fd);
3956    return fd;
3957}
3958
3959int MPLSensor::turnOffAccelFifo(void)
3960{
3961    int i, res = 0, tempFd;
3962    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
3963                        0, mpu.accel_fifo_enable, getTimestamp());
3964    res += write_sysfs_int(mpu.accel_fifo_enable, 0);
3965    return res;
3966}
3967
3968int MPLSensor::turnOffGyroFifo(void)
3969{
3970    int i, res = 0, tempFd;
3971    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
3972                        0, mpu.gyro_fifo_enable, getTimestamp());
3973    res += write_sysfs_int(mpu.gyro_fifo_enable, 0);
3974    return res;
3975}
3976
3977int MPLSensor::enableDmpOrientation(int en)
3978{
3979    VFUNC_LOG;
3980    int res = 0;
3981    int enabled_sensors = mEnabled;
3982
3983    if (isMpuNonDmp())
3984        return res;
3985
3986    // reset master enable
3987    res = masterEnable(0);
3988    if (res < 0)
3989        return res;
3990
3991    if (en == 1) {
3992        //Enable DMP orientation
3993        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
3994                en, mpu.display_orientation_on, getTimestamp());
3995        if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
3996            LOGE("HAL:ERR can't enable Android orientation");
3997            res = -1;	// indicate an err
3998            return res;
3999        }
4000
4001        // enable DMP
4002        res = onDmp(1);
4003        if (res < 0)
4004            return res;
4005
4006        // set rate to 200Hz
4007        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
4008                200, mpu.accel_fifo_rate, getTimestamp());
4009        if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) {
4010            res = -1;
4011            LOGE("HAL:ERR can't set rate to 200Hz");
4012            return res;
4013        }
4014
4015        // enable accel engine
4016        res = enableAccel(1);
4017        if (res < 0)
4018            return res;
4019
4020        // disable accel FIFO
4021        if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) {
4022            res = turnOffAccelFifo();
4023            if (res < 0)
4024                return res;
4025        }
4026
4027        if (!mEnabled){
4028            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
4029                       1, mpu.dmp_event_int_on, getTimestamp());
4030            if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
4031                res = -1;
4032                LOGE("HAL:ERR can't enable DMP event interrupt");
4033            }
4034        }
4035
4036        mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION;
4037        LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
4038    } else {
4039        mFeatureActiveMask &= ~INV_DMP_DISPL_ORIENTATION;
4040        // disable DMP
4041        if (mFeatureActiveMask == 0) {
4042            res = onDmp(0);
4043            if (res < 0)
4044                return res;
4045
4046            // disable accel engine
4047            if (!(mLocalSensorMask & mMasterSensorMask
4048                    & INV_THREE_AXIS_ACCEL)) {
4049                res = enableAccel(0);
4050                if (res < 0)
4051                    return res;
4052            }
4053        }
4054
4055        if (mEnabled){
4056            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
4057                       en, mpu.dmp_event_int_on, getTimestamp());
4058            if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
4059                res = -1;
4060                LOGE("HAL:ERR can't enable DMP event interrupt");
4061            }
4062        }
4063        LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
4064    }
4065
4066    if (en || mEnabled || mFeatureActiveMask) {
4067        res = masterEnable(1);
4068    }
4069    return res;
4070}
4071
4072int MPLSensor::openDmpOrientFd(void)
4073{
4074    VFUNC_LOG;
4075
4076    if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) {
4077        LOGV_IF(PROCESS_VERBOSE,
4078                "HAL:DMP display orientation disabled or file desc opened");
4079        return 0;
4080    }
4081
4082    dmp_orient_fd = open(mpu.event_display_orientation, O_RDONLY| O_NONBLOCK);
4083    if (dmp_orient_fd < 0) {
4084        LOGE("HAL:ERR couldn't open dmpOrient node");
4085        return -1;
4086    } else {
4087        LOGV_IF(PROCESS_VERBOSE,
4088                "HAL:dmp_orient_fd opened : %d", dmp_orient_fd);
4089        return 0;
4090    }
4091}
4092
4093int MPLSensor::closeDmpOrientFd(void)
4094{
4095    VFUNC_LOG;
4096    if (dmp_orient_fd >= 0)
4097        close(dmp_orient_fd);
4098    return 0;
4099}
4100
4101int MPLSensor::dmpOrientHandler(int orient)
4102{
4103    VFUNC_LOG;
4104    LOGV_IF(PROCESS_VERBOSE, "HAL:orient %x", orient);
4105    return 0;
4106}
4107
4108int MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count)
4109{
4110    VFUNC_LOG;
4111
4112    char dummy[4];
4113    int screen_orientation = 0;
4114    FILE *fp;
4115
4116    fp = fopen(mpu.event_display_orientation, "r");
4117    if (fp == NULL) {
4118        LOGE("HAL:cannot open event_display_orientation");
4119        return 0;
4120    }
4121    fscanf(fp, "%d\n", &screen_orientation);
4122    fclose(fp);
4123
4124    int numEventReceived = 0;
4125
4126    if (mDmpOrientationEnabled && count > 0) {
4127        sensors_event_t temp;
4128
4129        temp.version = sizeof(sensors_event_t);
4130        temp.sensor = ID_SO;
4131        temp.acceleration.status
4132            = SENSOR_STATUS_UNRELIABLE;
4133#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
4134        temp.type = SENSOR_TYPE_SCREEN_ORIENTATION;
4135        temp.screen_orientation = screen_orientation;
4136#endif
4137        struct timespec ts;
4138        clock_gettime(CLOCK_MONOTONIC, &ts);
4139        temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
4140
4141        *data++ = temp;
4142        count--;
4143        numEventReceived++;
4144    }
4145
4146    // read dummy data per driver's request
4147    dmpOrientHandler(screen_orientation);
4148    read(dmp_orient_fd, dummy, 4);
4149
4150    return numEventReceived;
4151}
4152
4153int MPLSensor::getDmpOrientFd(void)
4154{
4155    VFUNC_LOG;
4156
4157    LOGV_IF(EXTRA_VERBOSE,
4158            "MPLSensor::getDmpOrientFd returning %d", dmp_orient_fd);
4159    return dmp_orient_fd;
4160
4161}
4162
4163int MPLSensor::checkDMPOrientation(void)
4164{
4165    VFUNC_LOG;
4166    return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0);
4167}
4168
4169int MPLSensor::getDmpRate(int64_t *wanted)
4170{
4171    VFUNC_LOG;
4172
4173      // set DMP output rate to FIFO
4174      if(mDmpOn == 1) {
4175        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
4176                int(1000000000.f / *wanted), mpu.three_axis_q_rate,
4177                getTimestamp());
4178        write_sysfs_int(mpu.three_axis_q_rate, 1000000000.f / *wanted);
4179        LOGV_IF(PROCESS_VERBOSE,
4180                "HAL:DMP three axis rate %.2f Hz", 1000000000.f / *wanted);
4181
4182        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
4183                int(1000000000.f / *wanted), mpu.six_axis_q_rate,
4184                getTimestamp());
4185        write_sysfs_int(mpu.six_axis_q_rate, 1000000000.f / *wanted);
4186        LOGV_IF(PROCESS_VERBOSE,
4187                "HAL:DMP six axis rate %.2f Hz", 1000000000.f / *wanted);
4188
4189        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
4190                int(1000000000.f / *wanted), mpu.ped_q_rate,
4191                getTimestamp());
4192        write_sysfs_int(mpu.ped_q_rate, 1000000000.f / *wanted);
4193        LOGV_IF(PROCESS_VERBOSE,
4194                "HAL:DMP ped quaternion rate %.2f Hz", 1000000000.f / *wanted);
4195
4196        //DMP running rate must be @ 200Hz
4197        *wanted= RATE_200HZ;
4198        LOGV_IF(PROCESS_VERBOSE,
4199                "HAL:DMP rate= %.2f Hz", 1000000000.f / *wanted);
4200    }
4201    return 0;
4202}
4203
4204int MPLSensor::getPollTime(void)
4205{
4206    VHANDLER_LOG;
4207    return mPollTime;
4208}
4209
4210int MPLSensor::getStepCountPollTime(void)
4211{
4212    VHANDLER_LOG;
4213    if (mDmpStepCountEnabled) {
4214    /* clamped to 1ms?, still rather large */
4215    LOGV_IF(0/*EXTRA_VERBOSE*/, "Step Count poll time = %lld ms",
4216            mStepCountPollTime / 1000000LL);
4217        return (mStepCountPollTime / 1000000LL);
4218    }
4219    return -1;
4220}
4221
4222bool MPLSensor::hasStepCountPendingEvents(void)
4223{
4224    VHANDLER_LOG;
4225    if (mDmpStepCountEnabled) {
4226        struct timespec t_now;
4227        int64_t interval = 0;
4228
4229        clock_gettime(CLOCK_MONOTONIC, &t_now);
4230        interval = ((int64_t(t_now.tv_sec) * 1000000000LL + t_now.tv_nsec) -
4231                    (int64_t(mt_pre.tv_sec) * 1000000000LL + mt_pre.tv_nsec));
4232
4233        if (interval < mStepCountPollTime) {
4234            LOGV_IF(0/*ENG_VERBOSE*/,
4235                    "Step Count interval elapsed: %lld, triggered: %d",
4236                    interval, mStepCountPollTime);
4237            return false;
4238        } else {
4239            clock_gettime(CLOCK_MONOTONIC, &mt_pre);
4240            LOGV_IF(0/*ENG_VERBOSE*/, "Step Count previous time: %ld ms",
4241                    mt_pre.tv_nsec / 1000);
4242            return true;
4243        }
4244    }
4245    return false;
4246}
4247
4248bool MPLSensor::hasPendingEvents(void) const
4249{
4250    VHANDLER_LOG;
4251    // if we are using the polling workaround, force the main
4252    // loop to check for data every time
4253    return (mPollTime != -1);
4254}
4255
4256/* TODO: support resume suspend when we gain more info about them*/
4257void MPLSensor::sleepEvent(void)
4258{
4259    VFUNC_LOG;
4260}
4261
4262void MPLSensor::wakeEvent(void)
4263{
4264    VFUNC_LOG;
4265}
4266
4267int MPLSensor::inv_float_to_q16(float *fdata, long *ldata)
4268{
4269    VHANDLER_LOG;
4270
4271    if (!fdata || !ldata)
4272        return -1;
4273    ldata[0] = (long)(fdata[0] * 65536.f);
4274    ldata[1] = (long)(fdata[1] * 65536.f);
4275    ldata[2] = (long)(fdata[2] * 65536.f);
4276    return 0;
4277}
4278
4279int MPLSensor::inv_long_to_q16(long *fdata, long *ldata)
4280{
4281    VHANDLER_LOG;
4282
4283    if (!fdata || !ldata)
4284        return -1;
4285    ldata[0] = (fdata[1] * 65536.f);
4286    ldata[1] = (fdata[2] * 65536.f);
4287    ldata[2] = (fdata[3] * 65536.f);
4288    return 0;
4289}
4290
4291int MPLSensor::inv_float_to_round(float *fdata, long *ldata)
4292{
4293    VHANDLER_LOG;
4294
4295    if (!fdata || !ldata)
4296            return -1;
4297    ldata[0] = (long)fdata[0];
4298    ldata[1] = (long)fdata[1];
4299    ldata[2] = (long)fdata[2];
4300    return 0;
4301}
4302
4303int MPLSensor::inv_float_to_round2(float *fdata, short *ldata)
4304{
4305    VHANDLER_LOG;
4306
4307    if (!fdata || !ldata)
4308        return -1;
4309    ldata[0] = (short)fdata[0];
4310    ldata[1] = (short)fdata[1];
4311    ldata[2] = (short)fdata[2];
4312    return 0;
4313}
4314
4315int MPLSensor::inv_long_to_float(long *ldata, float *fdata)
4316{
4317    VHANDLER_LOG;
4318
4319    if (!ldata || !fdata)
4320        return -1;
4321    fdata[0] = (float)ldata[0];
4322    fdata[1] = (float)ldata[1];
4323    fdata[2] = (float)ldata[2];
4324    return 0;
4325}
4326
4327int MPLSensor::inv_read_temperature(long long *data)
4328{
4329    VHANDLER_LOG;
4330
4331    int count = 0;
4332    char raw_buf[40];
4333    long raw = 0;
4334
4335    long long timestamp = 0;
4336
4337    memset(raw_buf, 0, sizeof(raw_buf));
4338    count = read_attribute_sensor(gyro_temperature_fd, raw_buf,
4339                                  sizeof(raw_buf));
4340    if(count < 1) {
4341        LOGE("HAL:error reading gyro temperature");
4342        return -1;
4343    }
4344
4345    count = sscanf(raw_buf, "%ld%lld", &raw, &timestamp);
4346
4347    if(count < 0) {
4348        return -1;
4349    }
4350
4351    LOGV_IF(ENG_VERBOSE,
4352            "HAL:temperature raw = %ld, timestamp = %lld, count = %d",
4353            raw, timestamp, count);
4354    data[0] = raw;
4355    data[1] = timestamp;
4356
4357    return 0;
4358}
4359
4360int MPLSensor::inv_read_dmp_state(int fd)
4361{
4362    VFUNC_LOG;
4363
4364    if(fd < 0)
4365        return -1;
4366
4367    int count = 0;
4368    char raw_buf[10];
4369    short raw = 0;
4370
4371    memset(raw_buf, 0, sizeof(raw_buf));
4372    count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf));
4373    if(count < 1) {
4374        LOGE("HAL:error reading dmp state");
4375        close(fd);
4376        return -1;
4377    }
4378    count = sscanf(raw_buf, "%hd", &raw);
4379    if(count < 0) {
4380        LOGE("HAL:dmp state data is invalid");
4381        close(fd);
4382        return -1;
4383    }
4384    LOGV_IF(EXTRA_VERBOSE, "HAL:dmp state = %d, count = %d", raw, count);
4385    close(fd);
4386    return (int)raw;
4387}
4388
4389int MPLSensor::inv_read_sensor_bias(int fd, long *data)
4390{
4391    VFUNC_LOG;
4392
4393    if(fd == -1) {
4394        return -1;
4395    }
4396
4397    char buf[50];
4398    char x[15], y[15], z[15];
4399
4400    memset(buf, 0, sizeof(buf));
4401    int count = read_attribute_sensor(fd, buf, sizeof(buf));
4402    if(count < 1) {
4403        LOGE("HAL:Error reading gyro bias");
4404        return -1;
4405    }
4406    count = sscanf(buf, "%[^','],%[^','],%[^',']", x, y, z);
4407    if(count) {
4408        /* scale appropriately for MPL */
4409        LOGV_IF(ENG_VERBOSE,
4410                "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)",
4411                atol(x), atol(y), atol(z));
4412
4413        data[0] = (long)(atol(x) / 10000 * (1L << 16));
4414        data[1] = (long)(atol(y) / 10000 * (1L << 16));
4415        data[2] = (long)(atol(z) / 10000 * (1L << 16));
4416
4417        LOGV_IF(ENG_VERBOSE,
4418                "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)",
4419                data[0], data[1], data[2]);
4420    }
4421    return 0;
4422}
4423
4424/** fill in the sensor list based on which sensors are configured.
4425 *  return the number of configured sensors.
4426 *  parameter list must point to a memory region of at least 7*sizeof(sensor_t)
4427 *  parameter len gives the length of the buffer pointed to by list
4428 */
4429int MPLSensor::populateSensorList(struct sensor_t *list, int len)
4430{
4431    VFUNC_LOG;
4432
4433    int numsensors;
4434
4435    if(len <
4436        (int)((sizeof(sSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) {
4437        LOGE("HAL:sensor list too small, not populating.");
4438        return -(sizeof(sSensorList) / sizeof(sensor_t));
4439    }
4440
4441    /* fill in the base values */
4442    memcpy(list, sSensorList,
4443           sizeof (struct sensor_t) * (sizeof(sSensorList) / sizeof(sensor_t)));
4444
4445    /* first add gyro, accel and compass to the list */
4446
4447    /* fill in gyro/accel values */
4448    if(chip_ID == NULL) {
4449        LOGE("HAL:Can not get gyro/accel id");
4450    }
4451    fillGyro(chip_ID, list);
4452    fillAccel(chip_ID, list);
4453
4454    // TODO: need fixes for unified HAL and 3rd-party solution
4455    mCompassSensor->fillList(&list[MagneticField]);
4456    mCompassSensor->fillList(&list[RawMagneticField]);
4457
4458    if(1) {
4459        numsensors = (sizeof(sSensorList) / sizeof(sensor_t));
4460        /* all sensors will be added to the list
4461           fill in orientation values */
4462        fillOrientation(list);
4463        /* fill in rotation vector values */
4464        fillRV(list);
4465        /* fill in game rotation vector values */
4466        fillGRV(list);
4467        /* fill in gravity values */
4468        fillGravity(list);
4469        /* fill in Linear accel values */
4470        fillLinearAccel(list);
4471        /* fill in Significant motion values */
4472        fillSignificantMotion(list);
4473#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
4474        /* fill in screen orientation values */
4475        fillScreenOrientation(list);
4476#endif
4477    } else {
4478        /* no 9-axis sensors, zero fill that part of the list */
4479        numsensors = 3;
4480        memset(list + 3, 0, 4 * sizeof(struct sensor_t));
4481    }
4482
4483    return numsensors;
4484}
4485
4486void MPLSensor::fillAccel(const char* accel, struct sensor_t *list)
4487{
4488    VFUNC_LOG;
4489
4490    if (accel) {
4491        if(accel != NULL && strcmp(accel, "BMA250") == 0) {
4492            list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
4493            list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
4494            list[Accelerometer].power = ACCEL_BMA250_POWER;
4495            list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
4496            return;
4497        } else if (accel != NULL && strcmp(accel, "MPU6050") == 0) {
4498            list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
4499            list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
4500            list[Accelerometer].power = ACCEL_MPU6050_POWER;
4501            list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY;
4502            return;
4503        } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
4504            list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
4505            list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
4506            list[Accelerometer].power = ACCEL_MPU6500_POWER;
4507            list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
4508            return;
4509         } else if (accel != NULL && strcmp(accel, "MPU6515") == 0) {
4510            list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
4511            list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
4512            list[Accelerometer].power = ACCEL_MPU6500_POWER;
4513            list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
4514            return;
4515        } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
4516            list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
4517            list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
4518            list[Accelerometer].power = ACCEL_MPU6500_POWER;
4519            list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
4520            return;
4521        } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
4522            list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
4523            list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
4524            list[Accelerometer].power = ACCEL_MPU6500_POWER;
4525            list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
4526            return;
4527        } else if (accel != NULL && strcmp(accel, "MPU9150") == 0) {
4528            list[Accelerometer].maxRange = ACCEL_MPU9150_RANGE;
4529            list[Accelerometer].resolution = ACCEL_MPU9150_RESOLUTION;
4530            list[Accelerometer].power = ACCEL_MPU9150_POWER;
4531            list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY;
4532            return;
4533        } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) {
4534            list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
4535            list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
4536            list[Accelerometer].power = ACCEL_BMA250_POWER;
4537            list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
4538            return;
4539        }
4540    }
4541
4542    LOGE("HAL:unknown accel id %s -- "
4543         "params default to bma250 and might be wrong.",
4544         accel);
4545    list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
4546    list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
4547    list[Accelerometer].power = ACCEL_BMA250_POWER;
4548    list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
4549}
4550
4551void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
4552{
4553    VFUNC_LOG;
4554
4555    if ( gyro != NULL && strcmp(gyro, "MPU3050") == 0) {
4556        list[Gyro].maxRange = GYRO_MPU3050_RANGE;
4557        list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
4558        list[Gyro].power = GYRO_MPU3050_POWER;
4559        list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
4560    } else if( gyro != NULL && strcmp(gyro, "MPU6050") == 0) {
4561        list[Gyro].maxRange = GYRO_MPU6050_RANGE;
4562        list[Gyro].resolution = GYRO_MPU6050_RESOLUTION;
4563        list[Gyro].power = GYRO_MPU6050_POWER;
4564        list[Gyro].minDelay = GYRO_MPU6050_MINDELAY;
4565    } else if( gyro != NULL && strcmp(gyro, "MPU6500") == 0) {
4566        list[Gyro].maxRange = GYRO_MPU6500_RANGE;
4567        list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
4568        list[Gyro].power = GYRO_MPU6500_POWER;
4569        list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
4570     } else if( gyro != NULL && strcmp(gyro, "MPU6515") == 0) {
4571        list[Gyro].maxRange = GYRO_MPU6500_RANGE;
4572        list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
4573        list[Gyro].power = GYRO_MPU6500_POWER;
4574        list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
4575    } else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) {
4576        list[Gyro].maxRange = GYRO_MPU9150_RANGE;
4577        list[Gyro].resolution = GYRO_MPU9150_RESOLUTION;
4578        list[Gyro].power = GYRO_MPU9150_POWER;
4579        list[Gyro].minDelay = GYRO_MPU9150_MINDELAY;
4580    } else {
4581        LOGE("HAL:unknown gyro id -- gyro params will be wrong.");
4582        LOGE("HAL:default to use mpu3050 params");
4583        list[Gyro].maxRange = GYRO_MPU3050_RANGE;
4584        list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
4585        list[Gyro].power = GYRO_MPU3050_POWER;
4586        list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
4587    }
4588
4589    list[RawGyro].maxRange = list[Gyro].maxRange;
4590    list[RawGyro].resolution = list[Gyro].resolution;
4591    list[RawGyro].power = list[Gyro].power;
4592    list[RawGyro].minDelay = list[Gyro].minDelay;
4593
4594    return;
4595}
4596
4597/* fillRV depends on values of gyro, accel and compass in the list */
4598void MPLSensor::fillRV(struct sensor_t *list)
4599{
4600    VFUNC_LOG;
4601
4602    /* compute power on the fly */
4603    list[RotationVector].power = list[Gyro].power +
4604                                 list[Accelerometer].power +
4605                                 list[MagneticField].power;
4606    list[RotationVector].resolution = .00001;
4607    list[RotationVector].maxRange = 1.0;
4608    list[RotationVector].minDelay = 5000;
4609
4610    return;
4611}
4612
4613/* fillGMRV depends on values of accel and mag in the list */
4614void MPLSensor::fillGMRV(struct sensor_t *list)
4615{
4616    VFUNC_LOG;
4617
4618    /* compute power on the fly */
4619    list[GeomagneticRotationVector].power = list[Accelerometer].power +
4620                                 list[MagneticField].power;
4621    list[GeomagneticRotationVector].resolution = .00001;
4622    list[GeomagneticRotationVector].maxRange = 1.0;
4623    list[GeomagneticRotationVector].minDelay = 5000;
4624
4625    return;
4626}
4627
4628/* fillGRV depends on values of gyro and accel in the list */
4629void MPLSensor::fillGRV(struct sensor_t *list)
4630{
4631    VFUNC_LOG;
4632
4633    /* compute power on the fly */
4634    list[GameRotationVector].power = list[Gyro].power +
4635                                 list[Accelerometer].power;
4636    list[GameRotationVector].resolution = .00001;
4637    list[GameRotationVector].maxRange = 1.0;
4638    list[GameRotationVector].minDelay = 5000;
4639
4640    return;
4641}
4642
4643void MPLSensor::fillOrientation(struct sensor_t *list)
4644{
4645    VFUNC_LOG;
4646
4647    list[Orientation].power = list[Gyro].power +
4648                              list[Accelerometer].power +
4649                              list[MagneticField].power;
4650    list[Orientation].resolution = .00001;
4651    list[Orientation].maxRange = 360.0;
4652    list[Orientation].minDelay = 5000;
4653
4654    return;
4655}
4656
4657void MPLSensor::fillGravity( struct sensor_t *list)
4658{
4659    VFUNC_LOG;
4660
4661    list[Gravity].power = list[Gyro].power +
4662                          list[Accelerometer].power +
4663                          list[MagneticField].power;
4664    list[Gravity].resolution = .00001;
4665    list[Gravity].maxRange = 9.81;
4666    list[Gravity].minDelay = 5000;
4667
4668    return;
4669}
4670
4671void MPLSensor::fillLinearAccel(struct sensor_t *list)
4672{
4673    VFUNC_LOG;
4674
4675    list[LinearAccel].power = list[Gyro].power +
4676                          list[Accelerometer].power +
4677                          list[MagneticField].power;
4678    list[LinearAccel].resolution = list[Accelerometer].resolution;
4679    list[LinearAccel].maxRange = list[Accelerometer].maxRange;
4680    list[LinearAccel].minDelay = 5000;
4681
4682    return;
4683}
4684
4685void MPLSensor::fillSignificantMotion(struct sensor_t *list)
4686{
4687    VFUNC_LOG;
4688
4689    list[SignificantMotion].power = list[Accelerometer].power;
4690    list[SignificantMotion].resolution = 1;
4691    list[SignificantMotion].maxRange = 1;
4692    list[SignificantMotion].minDelay = -1;
4693}
4694
4695#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
4696void MPLSensor::fillScreenOrientation(struct sensor_t *list)
4697{
4698    VFUNC_LOG;
4699
4700    list[NumSensors].power = list[Accelerometer].power;
4701    list[NumSensors].resolution = 1;
4702    list[NumSensors].maxRange = 3;
4703    list[NumSensors].minDelay = 0;
4704}
4705#endif
4706
4707int MPLSensor::inv_init_sysfs_attributes(void)
4708{
4709    VFUNC_LOG;
4710
4711    unsigned char i = 0;
4712    char sysfs_path[MAX_SYSFS_NAME_LEN];
4713    char tbuf[2];
4714    char *sptr;
4715    char **dptr;
4716    int num;
4717
4718    memset(sysfs_path, 0, sizeof(sysfs_path));
4719
4720    sysfs_names_ptr =
4721            (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
4722    sptr = sysfs_names_ptr;
4723    if (sptr != NULL) {
4724        dptr = (char**)&mpu;
4725        do {
4726            *dptr++ = sptr;
4727            memset(sptr, 0, sizeof(sptr));
4728            sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
4729        } while (++i < MAX_SYSFS_ATTRB);
4730    } else {
4731        LOGE("HAL:couldn't alloc mem for sysfs paths");
4732        return -1;
4733    }
4734
4735    // get proper (in absolute) IIO path & build MPU's sysfs paths
4736    inv_get_sysfs_path(sysfs_path);
4737
4738    if (strcmp(sysfs_path, "") == 0)
4739        return 0;
4740
4741    memcpy(mSysfsPath, sysfs_path, sizeof(sysfs_path));
4742    sprintf(mpu.key, "%s%s", sysfs_path, "/key");
4743    sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable");
4744    sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length");
4745    sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
4746
4747    sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path,
4748            "/scan_elements/in_timestamp_en");
4749    sprintf(mpu.in_timestamp_index, "%s%s", sysfs_path,
4750            "/scan_elements/in_timestamp_index");
4751    sprintf(mpu.in_timestamp_type, "%s%s", sysfs_path,
4752            "/scan_elements/in_timestamp_type");
4753
4754    sprintf(mpu.dmp_firmware, "%s%s", sysfs_path, "/dmp_firmware");
4755    sprintf(mpu.firmware_loaded, "%s%s", sysfs_path, "/firmware_loaded");
4756    sprintf(mpu.dmp_on, "%s%s", sysfs_path, "/dmp_on");
4757    sprintf(mpu.dmp_int_on, "%s%s", sysfs_path, "/dmp_int_on");
4758    sprintf(mpu.dmp_event_int_on, "%s%s", sysfs_path, "/dmp_event_int_on");
4759    sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
4760
4761    sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
4762
4763    sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature");
4764    sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable");
4765    sprintf(mpu.gyro_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
4766    sprintf(mpu.gyro_orient, "%s%s", sysfs_path, "/gyro_matrix");
4767    sprintf(mpu.gyro_fifo_enable, "%s%s", sysfs_path, "/gyro_fifo_enable");
4768    sprintf(mpu.gyro_fsr, "%s%s", sysfs_path, "/in_anglvel_scale");
4769    sprintf(mpu.gyro_fifo_enable, "%s%s", sysfs_path, "/gyro_fifo_enable");
4770    sprintf(mpu.gyro_rate, "%s%s", sysfs_path, "/gyro_rate");
4771
4772    sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accel_enable");
4773    sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
4774    sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accel_matrix");
4775    sprintf(mpu.accel_fifo_enable, "%s%s", sysfs_path, "/accel_fifo_enable");
4776    sprintf(mpu.accel_rate, "%s%s", sysfs_path, "/accel_rate");
4777
4778#ifndef THIRD_PARTY_ACCEL //MPUxxxx
4779    sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale");
4780
4781    // DMP uses these values
4782    sprintf(mpu.in_accel_x_dmp_bias, "%s%s", sysfs_path, "/in_accel_x_dmp_bias");
4783    sprintf(mpu.in_accel_y_dmp_bias, "%s%s", sysfs_path, "/in_accel_y_dmp_bias");
4784    sprintf(mpu.in_accel_z_dmp_bias, "%s%s", sysfs_path, "/in_accel_z_dmp_bias");
4785
4786    // MPU bias value is not set on purpose -
4787    // raw accel value with factory cal is not requested
4788#endif
4789
4790    // DMP uses these bias values
4791    sprintf(mpu.in_gyro_x_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_x_dmp_bias");
4792    sprintf(mpu.in_gyro_y_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_y_dmp_bias");
4793    sprintf(mpu.in_gyro_z_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_z_dmp_bias");
4794
4795    // MPU uses these bias values
4796    sprintf(mpu.in_gyro_x_offset, "%s%s", sysfs_path, "/in_anglvel_x_offset");
4797    sprintf(mpu.in_gyro_y_offset, "%s%s", sysfs_path, "/in_anglvel_y_offset");
4798    sprintf(mpu.in_gyro_z_offset, "%s%s", sysfs_path, "/in_anglvel_z_offset");
4799    sprintf(mpu.in_gyro_self_test_scale, "%s%s", sysfs_path, "/in_anglvel_self_test_scale");
4800
4801    sprintf(mpu.three_axis_q_on, "%s%s", sysfs_path, "/three_axes_q_on"); //formerly quaternion_on
4802    sprintf(mpu.three_axis_q_rate, "%s%s", sysfs_path, "/three_axes_q_rate");
4803
4804    sprintf(mpu.ped_q_on, "%s%s", sysfs_path, "/ped_q_on");
4805    sprintf(mpu.ped_q_rate, "%s%s", sysfs_path, "/ped_q_rate");
4806
4807    sprintf(mpu.six_axis_q_on, "%s%s", sysfs_path, "/six_axes_q_on");
4808    sprintf(mpu.six_axis_q_rate, "%s%s", sysfs_path, "/six_axes_q_rate");
4809
4810    sprintf(mpu.step_detector_on, "%s%s", sysfs_path, "/step_detector_on");
4811    sprintf(mpu.step_indicator_on, "%s%s", sysfs_path, "/step_indicator_on");
4812
4813    sprintf(mpu.display_orientation_on, "%s%s", sysfs_path,
4814            "/display_orientation_on");
4815    sprintf(mpu.event_display_orientation, "%s%s", sysfs_path,
4816            "/event_display_orientation");
4817
4818    sprintf(mpu.event_smd, "%s%s", sysfs_path,
4819            "/event_smd");
4820    sprintf(mpu.smd_enable, "%s%s", sysfs_path,
4821            "/smd_enable");
4822    sprintf(mpu.smd_delay_threshold, "%s%s", sysfs_path,
4823            "/smd_delay_threshold");
4824    sprintf(mpu.smd_delay_threshold2, "%s%s", sysfs_path,
4825            "/smd_delay_threshold2");
4826    sprintf(mpu.smd_threshold, "%s%s", sysfs_path,
4827            "/smd_threshold");
4828    sprintf(mpu.batchmode_timeout, "%s%s", sysfs_path,
4829            "/batchmode_timeout");
4830    sprintf(mpu.batchmode_wake_fifo_full_on, "%s%s", sysfs_path,
4831            "/batchmode_wake_fifo_full_on");
4832    sprintf(mpu.pedometer_on, "%s%s", sysfs_path,
4833            "/pedometer_on");
4834    sprintf(mpu.pedometer_int_on, "%s%s", sysfs_path,
4835            "/pedometer_int_on");
4836    sprintf(mpu.event_pedometer, "%s%s", sysfs_path,
4837            "/event_pedometer");
4838    sprintf(mpu.pedometer_steps, "%s%s", sysfs_path,
4839            "/pedometer_steps");
4840    return 0;
4841}
4842
4843bool MPLSensor::isMpuNonDmp(void)
4844{
4845    if (!strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050"))
4846        return true;
4847    else
4848        return false;
4849}
4850
4851int MPLSensor::isLowPowerQuatEnabled(void)
4852{
4853#ifdef ENABLE_LP_QUAT_FEAT
4854    return !isMpuNonDmp();
4855#else
4856    return 0;
4857#endif
4858}
4859
4860int MPLSensor::isDmpDisplayOrientationOn(void)
4861{
4862#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
4863    if (isMpuNonDmp())
4864        return 0;
4865    return 1;
4866#else
4867    return 0;
4868#endif
4869}
4870
4871/* these functions can be consolidated
4872with inv_convert_to_body_with_scale */
4873void MPLSensor::getCompassBias()
4874{
4875    VFUNC_LOG;
4876
4877
4878    long bias[3];
4879    long compassBias[3];
4880    unsigned short orient;
4881    signed char orientMtx[9];
4882    mCompassSensor->getOrientationMatrix(orientMtx);
4883    orient = inv_orientation_matrix_to_scalar(orientMtx);
4884    /* Get Values from MPL */
4885    inv_get_compass_bias(bias);
4886    //inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output);
4887    inv_convert_to_body(orient, bias, compassBias);
4888    LOGV_IF(HANDLER_DATA, "Mpl Compass Bias (HW unit) %ld %ld %ld", bias[0], bias[1], bias[2]);
4889    LOGV_IF(HANDLER_DATA, "Mpl Compass Bias (HW unit) (body) %ld %ld %ld", compassBias[0], compassBias[1], compassBias[2]);
4890    long compassSensitivity = inv_get_compass_sensitivity();
4891    if (compassSensitivity == 0) {
4892        compassSensitivity = mCompassScale;
4893    }
4894    for(int i=0; i<3; i++) {
4895        /* convert to uT */
4896        float temp = (float) compassSensitivity / (1L << 30);
4897        mCompassBias[i] =(float) (compassBias[i] * temp / 65536.f);
4898    }
4899
4900    return;
4901}
4902
4903void MPLSensor::getFactoryGyroBias()
4904{
4905    VFUNC_LOG;
4906
4907    //TODO: mllite needs to add this function
4908    //if(inv_factory_bias_available) {
4909        /* Get Values from MPL */
4910        inv_get_gyro_bias(mFactoryGyroBias);
4911        LOGV_IF(ENG_VERBOSE, "Factory Gyro Bias %ld %ld %ld", mFactoryGyroBias[0], mFactoryGyroBias[1], mFactoryGyroBias[2]);
4912        mFactoryGyroBiasAvailable = true;
4913    //}
4914
4915    return;
4916}
4917
4918/* set bias from factory cal file to MPU offset
4919/* x = values store in cal file --> (v/1000 * 2^16 / (2000/250))
4920/* offset = x/2^16 * (Gyro scale / self test scale used) * (-1) / offset scale
4921/* i.e. self test default scale = 250
4922/*       gyro scale default to = 2000
4923/*       offset scale = 4 //as spec by hardware
4924/*       offset = x/2^16 * (8) * (-1) / (4)
4925*/
4926
4927void MPLSensor::setFactoryGyroBias()
4928{
4929    VFUNC_LOG;
4930    int scaleRatio = mGyroScale / mGyroSelfTestScale;
4931    int offsetScale = 4;
4932    LOGV_IF(PROCESS_VERBOSE, "HAL: scaleRatio used =%d", scaleRatio);
4933    LOGV_IF(PROCESS_VERBOSE, "HAL: offsetScale used =%d", offsetScale);
4934
4935    /* Write to Driver */
4936    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
4937            (((int) (((float) mFactoryGyroBias[0]) / 65536.f * scaleRatio)) * -1 / offsetScale),
4938            mpu.in_gyro_x_offset, getTimestamp());
4939    if(write_attribute_sensor_continuous(gyro_x_offset_fd,
4940        (((int) (((float) mFactoryGyroBias[0]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0)
4941    {
4942        LOGE("HAL:Error writing to gyro_x_offset");
4943        return;
4944    }
4945    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
4946            (((int) (((float) mFactoryGyroBias[1]) / 65536.f * scaleRatio)) * -1 / offsetScale),
4947            mpu.in_gyro_y_offset, getTimestamp());
4948    if(write_attribute_sensor_continuous(gyro_y_offset_fd,
4949        (((int) (((float) mFactoryGyroBias[1]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0)
4950    {
4951        LOGE("HAL:Error writing to gyro_y_offset");
4952        return;
4953    }
4954    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
4955            (((int) (((float) mFactoryGyroBias[2]) / 65536.f * scaleRatio)) * -1 / offsetScale),
4956            mpu.in_gyro_z_offset, getTimestamp());
4957    if(write_attribute_sensor_continuous(gyro_z_offset_fd,
4958        (((int) (((float) mFactoryGyroBias[2]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0)
4959    {
4960        LOGE("HAL:Error writing to gyro_z_offset");
4961        return;
4962    }
4963    mFactoryGyroBiasAvailable = false;
4964    LOGV_IF(EXTRA_VERBOSE, "HAL:Factory Gyro Calibrated Bias Applied");
4965
4966    return;
4967}
4968
4969/* these functions can be consolidated
4970with inv_convert_to_body_with_scale */
4971void MPLSensor::getGyroBias()
4972{
4973    VFUNC_LOG;
4974
4975    long *temp = NULL;
4976    long chipBias[3];
4977    long bias[3];
4978    unsigned short orient;
4979
4980    /* Get Values from MPL */
4981    inv_get_mpl_gyro_bias(mGyroChipBias, temp);
4982    orient = inv_orientation_matrix_to_scalar(mGyroOrientation);
4983    //void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output);
4984    inv_convert_to_body(orient, mGyroChipBias, bias);
4985    LOGV_IF(ENG_VERBOSE, "Mpl Gyro Bias (HW unit) %ld %ld %ld", mGyroChipBias[0], mGyroChipBias[1], mGyroChipBias[2]);
4986    LOGV_IF(ENG_VERBOSE, "Mpl Gyro Bias (HW unit) (body) %ld %ld %ld", bias[0], bias[1], bias[2]);
4987    long gyroSensitivity = inv_get_gyro_sensitivity();
4988    if(gyroSensitivity == 0) {
4989        gyroSensitivity = mGyroScale;
4990    }
4991
4992    /* scale and convert to rad */
4993    for(int i=0; i<3; i++) {
4994        float temp = (float) gyroSensitivity / (1L << 30);
4995        mGyroBias[i] = (float) (bias[i] * temp / (1<<16) / 180 * M_PI);
4996        if (mGyroBias[i] != 0)
4997            mGyroBiasAvailable = true;
4998    }
4999
5000    return;
5001}
5002
5003void MPLSensor::setGyroBias()
5004{
5005    VFUNC_LOG;
5006
5007    if(mGyroBiasAvailable == false)
5008        return;
5009
5010    long bias[3];
5011    long gyroSensitivity = inv_get_gyro_sensitivity();
5012
5013    if(gyroSensitivity == 0) {
5014        gyroSensitivity = mGyroScale;
5015    }
5016
5017    inv_get_gyro_bias_dmp_units(bias);
5018
5019    /* Write to Driver */
5020    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
5021            bias[0], mpu.in_gyro_x_dmp_bias, getTimestamp());
5022    if(write_attribute_sensor_continuous(gyro_x_dmp_bias_fd, bias[0]) < 0)
5023    {
5024        LOGE("HAL:Error writing to gyro_x_dmp_bias");
5025        return;
5026    }
5027    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
5028            bias[1], mpu.in_gyro_y_dmp_bias, getTimestamp());
5029    if(write_attribute_sensor_continuous(gyro_y_dmp_bias_fd, bias[1]) < 0)
5030    {
5031        LOGE("HAL:Error writing to gyro_y_dmp_bias");
5032        return;
5033    }
5034    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
5035            bias[2], mpu.in_gyro_z_dmp_bias, getTimestamp());
5036    if(write_attribute_sensor_continuous(gyro_z_dmp_bias_fd, bias[2]) < 0)
5037    {
5038        LOGE("HAL:Error writing to gyro_z_dmp_bias");
5039        return;
5040    }
5041    mGyroBiasApplied = true;
5042    mGyroBiasAvailable = false;
5043    LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro DMP Calibrated Bias Applied");
5044
5045    return;
5046}
5047
5048void MPLSensor::getFactoryAccelBias()
5049{
5050    VFUNC_LOG;
5051
5052    long temp;
5053
5054    /* Get Values from MPL */
5055    inv_get_accel_bias(mAccelBias, &temp);
5056    LOGV_IF(HANDLER_DATA, "Factory Accel Bias (mg) %ld %ld %ld", mFactoryAccelBias[0], mFactoryAccelBias[1], mFactoryAccelBias[2]);
5057    mFactoryAccelBiasAvailable = true;
5058
5059    return;
5060}
5061
5062void MPLSensor::setFactoryAccelBias()
5063{
5064    if(mFactoryAccelBiasAvailable == false)
5065        return;
5066
5067    /* TODO: add scaling here - depends on self test parameters */
5068
5069    /* Write to Driver */
5070    if(write_attribute_sensor_continuous(accel_x_offset_fd, mFactoryAccelBias[0]) < 0)
5071    {
5072        LOGE("HAL:Error writing to accel_x_offset");
5073        return;
5074    }
5075    if(write_attribute_sensor_continuous(accel_y_offset_fd, mFactoryAccelBias[1]) < 0)
5076    {
5077        LOGE("HAL:Error writing to accel_y_offset");
5078        return;
5079    }
5080    if(write_attribute_sensor_continuous(accel_z_offset_fd, mFactoryAccelBias[2]) < 0)
5081    {
5082        LOGE("HAL:Error writing to accel_z_offset");
5083        return;
5084    }
5085    mFactoryAccelBiasAvailable = false;
5086    LOGV_IF(EXTRA_VERBOSE, "HAL:Factory Accel Calibrated Bias Applied");
5087
5088    return;
5089}
5090
5091void MPLSensor::getAccelBias()
5092{
5093    VFUNC_LOG;
5094    long temp;
5095
5096    /* Get Values from MPL */
5097    inv_get_accel_bias(mAccelBias, &temp);
5098    LOGV_IF(ENG_VERBOSE, "Accel Bias (mg) %ld %ld %ld", mAccelBias[0], mAccelBias[1], mAccelBias[2]);
5099    mAccelBiasAvailable = true;
5100
5101    return;
5102}
5103
5104void MPLSensor::setAccelBias()
5105{
5106    VFUNC_LOG;
5107
5108    if(mAccelBiasAvailable == false)
5109        return;
5110
5111    long bias[3];
5112    unsigned short orient = inv_orientation_matrix_to_scalar(mAccelOrientation);
5113    inv_convert_to_body(orient, mAccelBias, bias);
5114
5115    /* Write to Driver */
5116    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
5117           (long) (mAccelBias[0] / 65536.f / 2), mpu.in_accel_x_dmp_bias, getTimestamp());
5118    if(write_attribute_sensor_continuous(accel_x_dmp_bias_fd, (long)(mAccelBias[0] / 65536.f / 2)) < 0)
5119    {
5120        LOGE("HAL:Error writing to accel_x_dmp_bias");
5121        return;
5122    }
5123    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
5124            (long)(mAccelBias[1] / 65536.f / 2), mpu.in_accel_y_dmp_bias, getTimestamp());
5125    if(write_attribute_sensor_continuous(accel_y_dmp_bias_fd, (long)(mAccelBias[1] / 65536.f / 2)) < 0)
5126    {
5127        LOGE("HAL:Error writing to accel_y_dmp_bias");
5128        return;
5129    }
5130    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
5131            (long)(mAccelBias[2] / 65536 / 2), mpu.in_accel_z_dmp_bias, getTimestamp());
5132    if(write_attribute_sensor_continuous(accel_z_dmp_bias_fd, (long)(mAccelBias[2] / 65536 / 2)) < 0)
5133    {
5134        LOGE("HAL:Error writing to accel_z_dmp_bias");
5135        return;
5136    }
5137    mAccelBiasAvailable = false;
5138    mAccelBiasApplied = true;
5139    LOGV_IF(EXTRA_VERBOSE, "HAL:Accel DMP Calibrated Bias Applied");
5140
5141    return;
5142}
5143
5144int MPLSensor::isCompassDisabled(void)
5145{
5146    if(mCompassSensor->getFd() < 0 && !mCompassSensor->isIntegrated()) {
5147    LOGI_IF(EXTRA_VERBOSE, "HAL: Compass is disabled, Six-axis Sensor Fusion is used.");
5148        return 1;
5149    }
5150    return 0;
5151}
5152
5153/* precondition: framework disallows this case, ie enable continuous sensor, */
5154/* and enable batch sensor */
5155/* if one sensor is in continuous mode, HAL disallows enabling batch for this sensor */
5156/* or any other sensors */
5157#define DEBUG_BATCHING  (1)
5158int MPLSensor::batch(int handle, int flags, int64_t period_ns, int64_t timeout)
5159{
5160    VFUNC_LOG;
5161
5162    int res = 0;
5163
5164    if (isMpuNonDmp())
5165        return res;
5166
5167    /* Enables batch mode and sets timeout for the given sensor */
5168    /* enum SENSORS_BATCH_DRY_RUN, SENSORS_BATCH_WAKE_UPON_FIFO_FULL */
5169    bool dryRun = false;
5170    android::String8 sname;
5171    int what = -1;
5172    int enabled_sensors = mEnabled;
5173    int batchMode = timeout > 0 ? 1 : 0;
5174
5175    LOGI_IF(DEBUG_BATCHING || ENG_VERBOSE,
5176            "HAL:batch called - handle=%d, flags=%d, period=%lld, timeout=%lld",
5177            handle, flags, period_ns, timeout);
5178
5179    if(flags & (1 << SENSORS_BATCH_DRY_RUN)) {
5180        dryRun = true;
5181        LOGI_IF(PROCESS_VERBOSE,
5182                "HAL:batch - dry run mode is set (%d)", SENSORS_BATCH_DRY_RUN);
5183    }
5184
5185    getHandle(handle, what, sname);
5186    if(uint32_t(what) >= NumSensors) {
5187        LOGE("HAL:batch sensors %d not found", what);
5188        return -EINVAL;
5189    }
5190
5191    int tempBatch = 0;
5192    if (timeout > 0) {
5193        tempBatch = mBatchEnabled | (1 << what);
5194    } else {
5195        tempBatch = mBatchEnabled & ~(1 << what);
5196    }
5197
5198    if (!computeBatchSensorMask(mEnabled, tempBatch)) {
5199        batchMode = 0;
5200    } else {
5201        batchMode = 1;
5202    }
5203
5204    /* Supported sensors: Accel, Gyro, Raw Gyro, Compass, Raw Compass, GRV, Step Detector */
5205    switch (what) {
5206    case Orientation:
5207    case LinearAccel:
5208    case Gravity:
5209    case RotationVector:
5210    case GeomagneticRotationVector:
5211    case SignificantMotion:
5212    case ID_SC:
5213    case ID_SO:
5214        LOGE("HAL:batch - "
5215             "sensor (handle %d) is not supported in batch mode", handle);
5216        return -EINVAL;
5217    }
5218
5219    /* get maximum possible bytes to batch per sample */
5220    /* get minimum delay for each requested sensor    */
5221    ssize_t nBytes = 0;
5222    int64_t wanted = 1000000000LL, ns = 0;
5223    int64_t timeoutInMs = 0;
5224    for (int i = 0; i < NumSensors; i++) {
5225        if (batchMode == 1) {
5226            ns = mBatchDelays[i];
5227            LOGV_IF(DEBUG_BATCHING || EXTRA_VERBOSE,
5228                    "HAL:batch - requested sensor=0x%01x, batch delay=%lld", mEnabled & (1 << i), ns);
5229            // take the min delay ==> max rate
5230            wanted = (ns < wanted) ? ns : wanted;
5231            if (i <= RawMagneticField) {
5232                nBytes += 8;
5233            }
5234            if (i == Pressure) {
5235                nBytes += 6;
5236            }
5237            if ((i == StepDetector) || (i == GameRotationVector)) {
5238                nBytes += 16;
5239            }
5240        }
5241    }
5242
5243    /* check if we can support issuing interrupt before FIFO fills-up */
5244    /* in the given timeout.                                          */
5245    if ((flags & (1 << SENSORS_BATCH_WAKE_UPON_FIFO_FULL)) &&
5246            (batchMode == 1)) {
5247            LOGE("HAL: batch SENSORS_BATCH_WAKE_UPON_FIFO_FULL is not supported");
5248            return -EINVAL;
5249        /* provide messge if it exceeds hardware capability
5250        if (nSamples * nBytes >= 1024) {
5251            LOGE("HAL:batch - timeout - configuration is not supported, "
5252                 "cannot provide requested amount of buffering (%lld ms)",
5253                 timeout / 1000000LL);
5254        }*/
5255    }
5256
5257    if(dryRun == true)
5258        return 0;
5259
5260    /* starting from code below,  we will modify hardware */
5261    /* first edit global batch mode mask */
5262
5263    if (!timeout) {
5264        mBatchEnabled &= ~(1 << what);
5265        mBatchDelays[what] = 1000000000L;
5266        mBatchTimeouts[what] = 30000000000LL;
5267        if (mBatchEnabled == 0) {
5268            mFeatureActiveMask &= ~INV_DMP_BATCH_MODE;
5269        }
5270    } else {
5271        mBatchEnabled |= (1 << what);
5272        mBatchDelays[what] = period_ns;
5273        mBatchTimeouts[what] = timeout;
5274        mFeatureActiveMask |= INV_DMP_BATCH_MODE;
5275    }
5276
5277    /* For these sensors, switch to different data output */
5278    /* These steps should be optimized at some point */
5279    int featureMask = computeBatchDataOutput();
5280
5281    LOGV_IF(ENG_VERBOSE, "batchMode =%d, featureMask=0x%x, mEnabled=%d",
5282                batchMode, featureMask, mEnabled);
5283    if (DEBUG_BATCHING || EXTRA_VERBOSE) {
5284        LOGV("HAL:batch - sensor=0x%01x", mBatchEnabled);
5285        for (int d = 0; d < NumSensors; d++) {
5286            LOGV("HAL:batch - sensor status=0x%01x batch status=0x%01x timeout=%lld delay=%lld",
5287                            mEnabled & (1 << d), (mBatchEnabled & (1 << d)), mBatchTimeouts[d],
5288                            mBatchDelays[d]);
5289        }
5290    }
5291
5292    /* take the minimum batchmode timeout */
5293    if (batchMode == 1) {
5294        int64_t tempTimeout = 30000000000LL;
5295        for (int i = 0; i < NumSensors; i++) {
5296            if ((mEnabled & (1 << i) && mBatchEnabled & (1 << i)) ||
5297                    (((featureMask & INV_DMP_PED_STANDALONE) && (mBatchEnabled & (1 << StepDetector))))) {
5298                LOGV_IF(ENG_VERBOSE, "i=%d, timeout=%lld", i, mBatchTimeouts[i]);
5299                ns = mBatchTimeouts[i];
5300                tempTimeout = (ns < tempTimeout) ? ns : tempTimeout;
5301            }
5302        }
5303        timeout = tempTimeout;
5304        /* Convert ns to millisecond */
5305        timeoutInMs = timeout / 1000000;
5306
5307        /* remember last timeout value */
5308        mBatchTimeoutInMs = timeoutInMs;
5309
5310        /* TODO: Calculate nSamples */
5311        int nSamples = 0;
5312        nSamples = (unsigned long)(
5313            (1000000000.f / wanted) * ((float)timeout / 1000000000.f));
5314    } else {
5315        timeoutInMs = 0;
5316    }
5317
5318    LOGV_IF(DEBUG_BATCHING || EXTRA_VERBOSE,
5319                "HAL:batch - timeout - timeout=%lld ns, timeoutInMs=%lld, delay=%lld ns",
5320                timeout, timeoutInMs, wanted);
5321
5322    // reset master enable
5323    res = masterEnable(0);
5324    if (res < 0) {
5325        return res;
5326    }
5327
5328    /* case for Ped standalone */
5329    if ((batchMode == 1) && (featureMask & INV_DMP_PED_STANDALONE) &&
5330           (mFeatureActiveMask & INV_DMP_PEDOMETER)) {
5331        LOGI("ID_P only = 0x%x", mBatchEnabled);
5332        enablePedQuaternion(0);
5333        enablePedStandalone(1);
5334    } else {
5335        enablePedStandalone(0);
5336        if (featureMask & INV_DMP_PED_QUATERNION) {
5337            enableLPQuaternion(0);
5338            enablePedQuaternion(1);
5339        }
5340    }
5341
5342    /* case for Ped Quaternion */
5343    if ((batchMode == 1) && (featureMask & INV_DMP_PED_QUATERNION) &&
5344            (mEnabled & (1 << GameRotationVector)) &&
5345            (mFeatureActiveMask & INV_DMP_PEDOMETER)) {
5346        LOGI("ID_P and GRV or ALL = 0x%x", mBatchEnabled);
5347        LOGI("ID_P is enabled for batching, PED quat will be automatically enabled");
5348        enableLPQuaternion(0);
5349        enablePedQuaternion(1);
5350
5351        wanted = mBatchDelays[GameRotationVector];
5352        /* set pedq rate */
5353        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
5354                int(1000000000.f / wanted), mpu.ped_q_rate,
5355                getTimestamp());
5356        write_sysfs_int(mpu.ped_q_rate, 1000000000.f / wanted);
5357        LOGV_IF(PROCESS_VERBOSE,
5358                "HAL:DMP ped quaternion rate %.2f Hz", 1000000000.f / wanted);
5359    } else if (!(featureMask & INV_DMP_PED_STANDALONE)){
5360        LOGI("Toggle back to normal 6 axis");
5361        if (mEnabled & (1 << GameRotationVector)) {
5362            enableLPQuaternion(1);
5363        }
5364        enablePedQuaternion(0);
5365    }
5366
5367     /* case for Ped indicator */
5368    if ((batchMode == 1) && ((featureMask & INV_DMP_PED_INDICATOR))) {
5369        enablePedIndicator(1);
5370    } else {
5371        enablePedIndicator(0);
5372    }
5373
5374    /* case for Six Axis Quaternion */
5375    if ((batchMode == 1) && (featureMask & INV_DMP_6AXIS_QUATERNION) &&
5376            (mEnabled & (1 << GameRotationVector))) {
5377        LOGI("GRV = 0x%x", mBatchEnabled);
5378        enableLPQuaternion(0);
5379        enable6AxisQuaternion(1);
5380
5381        wanted = mBatchDelays[GameRotationVector];
5382        /* set sixaxis rate */
5383        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
5384                int(1000000000.f / wanted), mpu.six_axis_q_rate,
5385                getTimestamp());
5386        write_sysfs_int(mpu.six_axis_q_rate, 1000000000.f / wanted);
5387        LOGV_IF(PROCESS_VERBOSE,
5388                "HAL:DMP three axis rate %.2f Hz", 1000000000.f / wanted);
5389    } else if (!(featureMask & INV_DMP_PED_QUATERNION)){
5390        LOGI("Toggle back to normal 6 axis");
5391        if (mEnabled & (1 << GameRotationVector)) {
5392            enableLPQuaternion(1);
5393        }
5394        enable6AxisQuaternion(0);
5395    } else {
5396        enable6AxisQuaternion(0);
5397    }
5398
5399    /* TODO: This may make a come back some day */
5400    /* write not to overflow hardware FIFO if flag is set */
5401    /*if (flags & (1 << SENSORS_BATCH_WAKE_UPON_FIFO_FULL)) {
5402        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
5403                0, mpu.batchmode_wake_fifo_full_on, getTimestamp());
5404        if (write_sysfs_int(mpu.batchmode_wake_fifo_full_on, 0) < 0) {
5405            LOGE("HAL:ERR can't write batchmode_wake_fifo_full_on");
5406        }
5407    }*/
5408
5409    /* write required timeout to sysfs */
5410    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %lld > %s (%lld)",
5411            timeoutInMs, mpu.batchmode_timeout, getTimestamp());
5412    if (write_sysfs_int(mpu.batchmode_timeout, timeoutInMs) < 0) {
5413        LOGE("HAL:ERR can't write batchmode_timeout");
5414    }
5415
5416    if (batchMode == 1) {
5417        // enable DMP
5418        res = onDmp(1);
5419        if (res < 0) {
5420            return res;
5421        }
5422        // set batch rates
5423        if (setBatchDataRates() < 0) {
5424            LOGE("HAL:ERR can't set batch data rates");
5425        }
5426        // default fifo rate to 200Hz
5427        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
5428                200, mpu.gyro_fifo_rate, getTimestamp());
5429        if (write_sysfs_int(mpu.gyro_fifo_rate, 200) < 0) {
5430            res = -1;
5431            LOGE("HAL:ERR can't set DMP rate to 200Hz");
5432            return res;
5433        }
5434    } else {
5435        if ((mFeatureActiveMask == 0) && !(mEnabled & VIRTUAL_SENSOR_ALL_MASK)) {
5436            // disable DMP
5437            res = onDmp(0);
5438            if (res < 0) {
5439                return res;
5440            }
5441        }
5442        /* reset sensor rate */
5443        /*if (resetDataRates() < 0) {
5444            LOGE("HAL:ERR can't reset output rate back to original setting");
5445        }*/
5446    }
5447    if ((batchMode == 1) || enabled_sensors || mFeatureActiveMask) {
5448        masterEnable(1);
5449    }
5450    return res;
5451}
5452
5453int MPLSensor::computeBatchDataOutput()
5454{
5455    VFUNC_LOG;
5456
5457    int featureMask = 0;
5458    if (mBatchEnabled == 0)
5459        return 0;//h
5460
5461    uint32_t hardwareSensorMask = (1<<Gyro) | (1<<RawGyro) | (1<<Accelerometer) | (1<<MagneticField) |
5462                                        (1<<RawMagneticField) | (1<<Pressure);
5463    LOGV_IF(ENG_VERBOSE, "hardwareSensorMask = 0x%0x, mBatchEnabled = 0x%0x", hardwareSensorMask, mBatchEnabled);
5464
5465    if (mBatchEnabled & (1 << StepDetector)) {
5466        if (mBatchEnabled & (1 << GameRotationVector)) {
5467            if ((mBatchEnabled & hardwareSensorMask)) {
5468                featureMask |= INV_DMP_6AXIS_QUATERNION;//a
5469                featureMask |= INV_DMP_PED_INDICATOR;
5470            } else {
5471                featureMask |= INV_DMP_PED_QUATERNION; //b
5472                featureMask |= INV_DMP_PED_INDICATOR;  //always piggy back a bit
5473            }
5474        } else {
5475            if (mBatchEnabled & hardwareSensorMask) {
5476                featureMask |= INV_DMP_PED_INDICATOR; //c
5477            } else {
5478                featureMask |= INV_DMP_PED_STANDALONE; //d
5479                featureMask |= INV_DMP_PED_INDICATOR; //required for standalone
5480            }
5481        }
5482    } else if (mBatchEnabled & (1 << GameRotationVector)) {
5483        featureMask |= INV_DMP_6AXIS_QUATERNION; //e,f
5484    } else {
5485        LOGV_IF(ENG_VERBOSE, "HAL:computeBatchDataOutput: featuerMask=0x%x", featureMask);
5486        return 0; //g
5487    }
5488
5489    LOGV_IF(ENG_VERBOSE, "HAL:computeBatchDataOutput: featuerMask=0x%x", featureMask);
5490    return featureMask;
5491}
5492
5493int MPLSensor::getDmpPedometerFd()
5494{
5495    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getDmpPedometerFd returning %d",
5496             dmp_pedometer_fd);
5497    return dmp_pedometer_fd;
5498}
5499
5500/* @param [in] : outputType = 1 --derive from ped_q */
5501/* outputType = 0 --derive from IRQ          */
5502int MPLSensor::readDmpPedometerEvents(sensors_event_t* data, int count,
5503                                      int32_t id, int32_t type, int outputType)
5504{
5505    VFUNC_LOG;
5506
5507    int res = 0;
5508    char dummy[4];
5509    FILE *fp;
5510    uint64_t stepCount = 0;
5511    int numEventReceived = 0;
5512    int update = 0;
5513
5514    if((mDmpStepCountEnabled || mDmpPedometerEnabled) && count > 0) {
5515        /* handles return event */
5516        sensors_event_t temp;
5517
5518        LOGI_IF(EXTRA_VERBOSE, "HAL: Read Pedometer Event ID=%d", id);
5519        temp.version = sizeof(sensors_event_t);
5520        temp.sensor = id;
5521        temp.type = type;
5522        temp.acceleration.status
5523            = SENSOR_STATUS_UNRELIABLE;
5524        /* sensors.h specified to return 1.0 */
5525        if(id == ID_P) {
5526             temp.data[0] = 1;
5527             temp.data[1] = 0.f;
5528             temp.data[2] = 0.f;
5529        } else {
5530            fp = fopen(mpu.pedometer_steps, "r");
5531            if (fp == NULL) {
5532                LOGE("HAL:cannot open pedometer_steps");
5533            } else{
5534                if (fscanf(fp, "%lld\n", &stepCount) < 0 || fclose(fp) < 0) {
5535                    LOGE("HAL:cannot read pedometer_steps");
5536                }
5537            }
5538            /* return onChange only*/
5539            if (stepCount == mLastStepCount) {
5540                return 0;
5541            }
5542            temp.data[0] = stepCount;
5543            temp.data[1] = 0.f;
5544            temp.data[2] = 0.f;
5545            mLastStepCount = stepCount;
5546        }
5547
5548        if (!outputType) {
5549            struct timespec ts;
5550            clock_gettime(CLOCK_MONOTONIC, &ts) ;
5551            temp.timestamp = (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec;
5552        }
5553
5554        *data++ = temp;
5555        count--;
5556        numEventReceived++;
5557    }
5558
5559    if (!outputType) {
5560        // read dummy data per driver's request
5561        // only required if actual irq is issued
5562        read(dmp_pedometer_fd, dummy, 4);
5563    } else {
5564        return 1;
5565    }
5566
5567    return numEventReceived;
5568}
5569
5570int MPLSensor::getDmpSignificantMotionFd()
5571{
5572    LOGV_IF(EXTRA_VERBOSE,
5573            "MPLSensor::getDmpSignificantMotionFd returning %d",
5574            dmp_sign_motion_fd);
5575    return dmp_sign_motion_fd;
5576}
5577
5578int MPLSensor::readDmpSignificantMotionEvents(sensors_event_t* data, int count) {
5579    VFUNC_LOG;
5580
5581    int res = 0;
5582    char dummy[4];
5583    int significantMotion;
5584    FILE *fp;
5585    int sensors = mEnabled;
5586    int numEventReceived = 0;
5587    int update = 0;
5588
5589    /* Technically this step is not necessary for now  */
5590    /* In the future, we may have meaningful values */
5591    fp = fopen(mpu.event_smd, "r");
5592    if (fp == NULL) {
5593        LOGE("HAL:cannot open event_smd");
5594        return 0;
5595    }
5596    fscanf(fp, "%d\n", &significantMotion);
5597    fclose(fp);
5598
5599    if(mDmpSignificantMotionEnabled && count > 0) {
5600       /* By implementation, smd is disabled once an event is triggered */
5601        sensors_event_t temp;
5602
5603        /* Handles return event */
5604        LOGI("HAL: SMD detected");
5605        int update = smHandler(&temp);
5606        if (update && count > 0) {
5607            *data++ = temp;
5608            count--;
5609            numEventReceived++;
5610            mDmpSignificantMotionEnabled = 0;
5611            mFeatureActiveMask &= ~INV_DMP_SIGNIFICANT_MOTION;
5612            if(mFeatureActiveMask == 0) {
5613                LOGI("dmp off");
5614                // disable DMP
5615                masterEnable(0);
5616                res = onDmp(0);
5617                if (res < 0)
5618                    return res;
5619
5620                // disable accel engine
5621                if (!(mLocalSensorMask & mMasterSensorMask
5622                        & INV_THREE_AXIS_ACCEL)) {
5623                    res = enableAccel(0);
5624                    if (res < 0)
5625                        return res;
5626                }
5627            }
5628            if(sensors != 0) {
5629                 update_delay();
5630                 masterEnable(1);
5631            }
5632        }
5633    }
5634
5635    // read dummy data per driver's request
5636    read(dmp_sign_motion_fd, dummy, 4);
5637
5638    return numEventReceived;
5639}
5640
5641int MPLSensor::enableDmpSignificantMotion(int en)
5642{
5643    int res = 0;
5644    VFUNC_LOG;
5645
5646    int enabled_sensors = mEnabled;
5647
5648    if (isMpuNonDmp())
5649        return res;
5650
5651    // reset master enable
5652    res = masterEnable(0);
5653    if (res < 0)
5654        return res;
5655
5656    //Toggle significant montion detection
5657    if(en) {
5658        LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling Significant Motion");
5659        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
5660                1, mpu.smd_enable, getTimestamp());
5661        if (write_sysfs_int(mpu.smd_enable, 1) < 0) {
5662            LOGE("HAL:ERR can't write DMP smd_enable");
5663            res = -1;   //Indicate an err
5664        }
5665
5666        // enable DMP
5667        res = onDmp(1);
5668        if (res < 0)
5669            return res;
5670
5671        // set DMP rate to 200Hz
5672        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
5673                200, mpu.accel_fifo_rate, getTimestamp());
5674        if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) {
5675            res = -1;
5676            LOGE("HAL:ERR can't set rate to 200Hz");
5677            return res;
5678        }
5679
5680        // enable accel engine
5681        res = enableAccel(1);
5682        if (res < 0)
5683            return res;
5684
5685        // disable accel FIFO
5686        if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) {
5687            res = turnOffAccelFifo();
5688            if (res < 0)
5689                return res;
5690        }
5691        mFeatureActiveMask |= INV_DMP_SIGNIFICANT_MOTION;
5692    }
5693    else {
5694        LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling Significant Motion");
5695        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
5696                0, mpu.smd_enable, getTimestamp());
5697        if (write_sysfs_int(mpu.smd_enable, 0) < 0) {
5698            LOGE("HAL:ERR write DMP smd_enable");
5699        }
5700        mFeatureActiveMask &= ~INV_DMP_SIGNIFICANT_MOTION;
5701        // disable DMP
5702        if (mFeatureActiveMask == 0) {
5703            res = onDmp(0);
5704
5705            if (res < 0)
5706                return res;
5707
5708            // disable accel engine
5709            if (!(mLocalSensorMask & mMasterSensorMask
5710                    & INV_THREE_AXIS_ACCEL)) {
5711                res = enableAccel(0);
5712                if (res < 0)
5713                    return res;
5714            }
5715        }
5716        if(enabled_sensors) {
5717            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
5718                en, mpu.dmp_event_int_on, getTimestamp());
5719            if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
5720                res = -1;
5721                LOGE("HAL:ERR can't enable DMP event interrupt");
5722            }
5723         }
5724    }
5725    if(en || enabled_sensors || mFeatureActiveMask) {
5726        res = masterEnable(1);
5727    }
5728    return res;
5729}
5730
5731int MPLSensor::writeSignificantMotionParams(bool toggleEnable,
5732                                            uint32_t delayThreshold1,
5733                                            uint32_t delayThreshold2,
5734                                            uint32_t motionThreshold)
5735{
5736    int res = 0;
5737
5738    // Turn off enable
5739    if (toggleEnable) {
5740        masterEnable(0);
5741    }
5742
5743    // Write supplied values
5744    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
5745            delayThreshold1, mpu.smd_delay_threshold, getTimestamp());
5746    res = write_sysfs_int(mpu.smd_delay_threshold, delayThreshold1);
5747    if (res == 0) {
5748        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
5749                delayThreshold2, mpu.smd_delay_threshold2, getTimestamp());
5750        res = write_sysfs_int(mpu.smd_delay_threshold2, delayThreshold2);
5751    }
5752    if (res == 0) {
5753        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
5754                motionThreshold, mpu.smd_threshold, getTimestamp());
5755        res = write_sysfs_int(mpu.smd_threshold, motionThreshold);
5756    }
5757
5758    // Turn on enable
5759    if (toggleEnable) {
5760        masterEnable(1);
5761    }
5762    return res;
5763}
5764
5765/* set batch data rate */
5766/* this function should be optimized */
5767int MPLSensor::setBatchDataRates()
5768{
5769    VFUNC_LOG;
5770
5771    int res = 0;
5772    int tempFd = -1;
5773
5774    int64_t gyroRate;
5775    int64_t accelRate;
5776    int64_t compassRate;
5777    int64_t pressureRate;
5778
5779    int mplGyroRate;
5780    int mplAccelRate;
5781    int mplCompassRate;
5782
5783#ifdef ENABLE_MULTI_RATE
5784    /* take care of case where only one type of gyro sensors or compass sensors is turned on */
5785    gyroRate = mBatchDelays[Gyro] < mBatchDelays[RawGyro] ? mBatchDelays[Gyro] : mBatchDelays[RawGyro];
5786    accelRate = mBatchDelays[Accelerometer];
5787    compassRate = mBatchDelays[MagneticField] < mBatchDelays[RawMagneticField] ? mBatchDelays[MagneticField] : mBatchDelays[RawMagneticField];
5788    pressureRate = mBatchDelays[Pressure];
5789
5790    mplGyroRate = (int) gyroRate / 1000LL;
5791    mplAccelRate = (int) accelRate / 1000LL;
5792    mplCompassRate = (int) compassRate / 1000LL;
5793
5794     /* set rate in MPL */
5795     /* compass can only do 100Hz max */
5796    inv_set_gyro_sample_rate(mplGyroRate);
5797    inv_set_accel_sample_rate(mplAccelRate);
5798    inv_set_compass_sample_rate(mplCompassRate);
5799
5800    LOGV_IF(PROCESS_VERBOSE,
5801            "HAL:MPL gyro sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplGyroRate, 1000000000.f / gyroRate);
5802    LOGV_IF(PROCESS_VERBOSE,
5803            "HAL:MPL accel sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplAccelRate, 1000000000.f / accelRate);
5804    LOGV_IF(PROCESS_VERBOSE,
5805            "HAL:MPL compass sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplCompassRate, 1000000000.f / compassRate);
5806
5807#else
5808    /* search the minimum delay requested across all enabled sensors */
5809    int64_t wanted = 1000000000LL;
5810    for (int i = 1; i < NumSensors; i++) {
5811        if (mBatchEnabled & (1 << i)) {
5812            int64_t ns = mBatchDelays[i];
5813            wanted = wanted < ns ? wanted : ns;
5814        }
5815    }
5816    gyroRate = wanted;
5817    accelRate = wanted;
5818    compassRate = wanted;
5819    pressureRate = wanted;
5820#endif
5821
5822    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
5823            1000000000.f / gyroRate, mpu.gyro_rate,
5824            getTimestamp());
5825    tempFd = open(mpu.gyro_rate, O_RDWR);
5826    res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate);
5827    if(res < 0) {
5828        LOGE("HAL:GYRO update delay error");
5829    }
5830
5831    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
5832            1000000000.f / accelRate, mpu.accel_rate,
5833            getTimestamp());
5834    tempFd = open(mpu.accel_rate, O_RDWR);
5835    res = write_attribute_sensor(tempFd, 1000000000.f / accelRate);
5836    LOGE_IF(res < 0, "HAL:ACCEL update delay error");
5837
5838    if (compassRate < mCompassSensor->getMinDelay() * 1000LL) {
5839        compassRate = mCompassSensor->getMinDelay() * 1000LL;
5840    }
5841    mCompassSensor->setDelay(ID_M, compassRate);
5842
5843    mPressureSensor->setDelay(ID_PS, pressureRate);
5844
5845    return res;
5846}
5847
5848/* Set sensor rate */
5849/* this function should be optimized */
5850int MPLSensor::resetDataRates()
5851{
5852    VFUNC_LOG;
5853
5854    int res = 0;
5855    int tempFd = -1;
5856    int64_t wanted = 1000000000LL;
5857
5858    int64_t gyroRate;
5859    int64_t accelRate;
5860    int64_t compassRate;
5861    int64_t pressureRate;
5862
5863    /* enable this once driver supports multi-rate in dmp off mode */
5864
5865    /* search the minimum delay requested across all enabled sensors */
5866    for (int i = 1; i < NumSensors; i++) {
5867        if (mEnabled & (1 << i)) {
5868            int64_t ns = mDelays[i];
5869            wanted = wanted < ns ? wanted : ns;
5870        }
5871    }
5872
5873    if (mDmpOn == 1) {
5874        gyroRate = mDelays[Gyro] < mDelays[RawGyro] ? mDelays[Gyro] : mDelays[RawGyro];
5875        accelRate = mDelays[Accelerometer];
5876        compassRate = mDelays[MagneticField] < mDelays[RawMagneticField] ? mDelays[MagneticField] : mDelays[RawMagneticField];
5877        pressureRate = mDelays[Pressure];
5878
5879#ifndef ENABLE_MULTI_RATE
5880        gyroRate = wanted;
5881        accelRate = wanted;
5882        compassRate = wanted;
5883        pressureRate = wanted;
5884#endif
5885
5886    } else {
5887        gyroRate = wanted;
5888        accelRate = wanted;
5889        compassRate = wanted;
5890        pressureRate = wanted;
5891    }
5892
5893    /* set mpl data rate */
5894    inv_set_gyro_sample_rate((int)gyroRate/1000LL);
5895    inv_set_accel_sample_rate((int)accelRate/1000LL);
5896    inv_set_compass_sample_rate((int)compassRate/1000LL);
5897
5898   LOGV_IF(PROCESS_VERBOSE,
5899            "HAL:MPL gyro sample rate: (mpl)=%lld us (mpu)=%.2f Hz", gyroRate/1000LL, 1000000000.f / gyroRate);
5900   LOGV_IF(PROCESS_VERBOSE,
5901            "HAL:MPL accel sample rate: (mpl)=%lld us (mpu)=%.2f Hz", accelRate/1000LL, 1000000000.f / accelRate);
5902   LOGV_IF(PROCESS_VERBOSE,
5903            "HAL:MPL compass sample rate: (mpl)=%lld us (mpu)=%.2f Hz", compassRate/1000LL, 1000000000.f / compassRate);
5904
5905    /* reset dmp rate */
5906    getDmpRate (&wanted);
5907
5908    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
5909            1000000000.f / wanted, mpu.gyro_fifo_rate,
5910            getTimestamp());
5911    tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
5912    res = write_attribute_sensor(tempFd, 1000000000.f / wanted);
5913    LOGE_IF(res < 0, "HAL:sampling frequency update delay error");
5914
5915    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
5916            1000000000.f / gyroRate, mpu.gyro_rate,
5917            getTimestamp());
5918    tempFd = open(mpu.gyro_rate, O_RDWR);
5919    res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate);
5920    if(res < 0) {
5921        LOGE("HAL:GYRO update delay error");
5922    }
5923
5924    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
5925            1000000000.f / accelRate, mpu.accel_rate,
5926            getTimestamp());
5927    tempFd = open(mpu.accel_rate, O_RDWR);
5928    res = write_attribute_sensor(tempFd, 1000000000.f / accelRate);
5929    LOGE_IF(res < 0, "HAL:ACCEL update delay error");
5930
5931    if (compassRate < mCompassSensor->getMinDelay() * 1000LL) {
5932        compassRate = mCompassSensor->getMinDelay() * 1000LL;
5933    }
5934    mCompassSensor->setDelay(ID_M, compassRate);
5935
5936    mPressureSensor->setDelay(ID_PS, pressureRate);
5937
5938    return res;
5939}
5940
5941void MPLSensor::initBias()
5942{
5943    LOGV_IF(ENG_VERBOSE, "HAL:inititalize dmp and device offsets to 0");
5944
5945    if(write_attribute_sensor_continuous(accel_x_dmp_bias_fd, 0) < 0)
5946    {
5947        LOGE("HAL:Error writing to accel_x_dmp_bias");
5948    }
5949    if(write_attribute_sensor_continuous(accel_y_dmp_bias_fd, 0) < 0)
5950    {
5951        LOGE("HAL:Error writing to accel_y_dmp_bias");
5952    }
5953    if(write_attribute_sensor_continuous(accel_z_dmp_bias_fd, 0) < 0)
5954    {
5955        LOGE("HAL:Error writing to accel_z_dmp_bias");
5956    }
5957
5958    if(write_attribute_sensor_continuous(gyro_x_dmp_bias_fd, 0) < 0)
5959    {
5960        LOGE("HAL:Error writing to gyro_x_dmp_bias");
5961    }
5962    if(write_attribute_sensor_continuous(gyro_y_dmp_bias_fd, 0) < 0)
5963    {
5964        LOGE("HAL:Error writing to gyro_y_dmp_bias");
5965    }
5966     if(write_attribute_sensor_continuous(gyro_z_dmp_bias_fd, 0) < 0)
5967    {
5968        LOGE("HAL:Error writing to gyro_z_dmp_bias");
5969    }
5970
5971    if(write_attribute_sensor_continuous(gyro_x_offset_fd, 0) < 0)
5972    {
5973        LOGE("HAL:Error writing to gyro_x_offset");
5974    }
5975    if(write_attribute_sensor_continuous(gyro_y_offset_fd, 0) < 0)
5976    {
5977        LOGE("HAL:Error writing to gyro_y_offset");
5978    }
5979    if(write_attribute_sensor_continuous(gyro_z_offset_fd, 0) < 0)
5980    {
5981        LOGE("HAL:Error writing to gyro_z_offset");
5982    }
5983    return;
5984}
5985
5986/*TODO: reg_dump in a separate file*/
5987void MPLSensor::sys_dump(bool fileMode)
5988{
5989    VFUNC_LOG;
5990
5991    char sysfs_path[MAX_SYSFS_NAME_LEN];
5992    char scan_element_path[MAX_SYSFS_NAME_LEN];
5993
5994    memset(sysfs_path, 0, sizeof(sysfs_path));
5995    memset(scan_element_path, 0, sizeof(scan_element_path));
5996    inv_get_sysfs_path(sysfs_path);
5997    sprintf(scan_element_path, "%s%s", sysfs_path, "/scan_elements");
5998
5999    read_sysfs_dir(fileMode, sysfs_path);
6000    read_sysfs_dir(fileMode, scan_element_path);
6001
6002    dump_dmp_img("/data/local/read_img.h");
6003    return;
6004}
6005