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