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