Home | History | Annotate | Download | only in libsensors_iio
      1 /*
      2 * Copyright (C) 2012 Invensense, Inc.
      3 *
      4 * Licensed under the Apache License, Version 2.0 (the "License");
      5 * you may not use this file except in compliance with the License.
      6 * You may obtain a copy of the License at
      7 *
      8 *      http://www.apache.org/licenses/LICENSE-2.0
      9 *
     10 * Unless required by applicable law or agreed to in writing, software
     11 * distributed under the License is distributed on an "AS IS" BASIS,
     12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
     13 * See the License for the specific language governing permissions and
     14 * limitations under the License.
     15 */
     16 
     17 #define LOG_NDEBUG 0
     18 //see also the EXTRA_VERBOSE define in the MPLSensor.h header file
     19 
     20 #include <fcntl.h>
     21 #include <errno.h>
     22 #include <math.h>
     23 #include <float.h>
     24 #include <poll.h>
     25 #include <unistd.h>
     26 #include <dirent.h>
     27 #include <stdlib.h>
     28 #include <sys/select.h>
     29 #include <sys/syscall.h>
     30 #include <dlfcn.h>
     31 #include <pthread.h>
     32 #include <cutils/log.h>
     33 #include <utils/KeyedVector.h>
     34 #include <utils/String8.h>
     35 #include <string.h>
     36 #include <linux/input.h>
     37 #include <utils/Atomic.h>
     38 
     39 #include "MPLSensor.h"
     40 #include "MPLSupport.h"
     41 #include "sensor_params.h"
     42 #include "local_log_def.h"
     43 
     44 #include "invensense.h"
     45 #include "invensense_adv.h"
     46 #include "ml_stored_data.h"
     47 #include "ml_load_dmp.h"
     48 #include "ml_sysfs_helper.h"
     49 
     50 // #define TESTING
     51 
     52 #ifdef THIRD_PARTY_ACCEL
     53 #   warning "Third party accel"
     54 #   define USE_THIRD_PARTY_ACCEL        (1)
     55 #else
     56 #   define USE_THIRD_PARTY_ACCEL        (0)
     57 #endif
     58 
     59 #define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))
     60 
     61 /******************************************************************************/
     62 /*  MPL interface misc.                                                       */
     63 /******************************************************************************/
     64 static int hertz_request = 200;
     65 
     66 #define DEFAULT_MPL_GYRO_RATE           (20000L)     //us
     67 #define DEFAULT_MPL_COMPASS_RATE        (20000L)     //us
     68 
     69 #define DEFAULT_HW_GYRO_RATE            (100)        //Hz
     70 #define DEFAULT_HW_ACCEL_RATE           (20)         //ms
     71 #define DEFAULT_HW_COMPASS_RATE         (20000000L)  //ns
     72 #define DEFAULT_HW_AKMD_COMPASS_RATE    (200000000L) //ns
     73 
     74 /* convert ns to hardware units */
     75 #define HW_GYRO_RATE_NS                 (1000000000LL / rate_request) // to Hz
     76 #define HW_ACCEL_RATE_NS                (rate_request / (1000000L))   // to ms
     77 #define HW_COMPASS_RATE_NS              (rate_request)                // to ns
     78 
     79 /* convert Hz to hardware units */
     80 #define HW_GYRO_RATE_HZ                 (hertz_request)
     81 #define HW_ACCEL_RATE_HZ                (1000 / hertz_request)
     82 #define HW_COMPASS_RATE_HZ              (1000000000LL / hertz_request)
     83 
     84 #define RATE_200HZ                      5000000LL
     85 #define RATE_15HZ                       66667000LL
     86 #define RATE_5HZ                        200000000LL
     87 
     88 static struct sensor_t sSensorList[] =
     89 {
     90     {"MPL Gyroscope", "Invensense", 1,
     91      SENSORS_GYROSCOPE_HANDLE,
     92      SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
     93     {"MPL Raw Gyroscope", "Invensense", 1,
     94      SENSORS_RAW_GYROSCOPE_HANDLE,
     95      SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
     96     {"MPL Accelerometer", "Invensense", 1,
     97      SENSORS_ACCELERATION_HANDLE,
     98      SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
     99     {"MPL Magnetic Field", "Invensense", 1,
    100      SENSORS_MAGNETIC_FIELD_HANDLE,
    101      SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
    102     {"MPL Orientation", "Invensense", 1,
    103      SENSORS_ORIENTATION_HANDLE,
    104      SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, {}},
    105     {"MPL Rotation Vector", "Invensense", 1,
    106      SENSORS_ROTATION_VECTOR_HANDLE,
    107      SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
    108     {"MPL Linear Acceleration", "Invensense", 1,
    109      SENSORS_LINEAR_ACCEL_HANDLE,
    110      SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
    111     {"MPL Gravity", "Invensense", 1,
    112      SENSORS_GRAVITY_HANDLE,
    113      SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
    114 
    115 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
    116     {"MPL Screen Orientation", "Invensense ", 1,
    117      SENSORS_SCREEN_ORIENTATION_HANDLE,
    118      SENSOR_TYPE_SCREEN_ORIENTATION, 100.0f, 1.0f, 1.1f, 0, 0, 0, {}},
    119 #endif
    120 };
    121 
    122 MPLSensor *MPLSensor::gMPLSensor = NULL;
    123 
    124 extern "C" {
    125 void procData_cb_wrapper()
    126 {
    127     if(MPLSensor::gMPLSensor) {
    128         MPLSensor::gMPLSensor->cbProcData();
    129     }
    130 }
    131 
    132 void setCallbackObject(MPLSensor* gbpt)
    133 {
    134     MPLSensor::gMPLSensor = gbpt;
    135 }
    136 
    137 MPLSensor* getCallbackObject() {
    138     return MPLSensor::gMPLSensor;
    139 }
    140 
    141 } // end of extern C
    142 
    143 #ifdef INV_PLAYBACK_DBG
    144 static FILE *logfile = NULL;
    145 #endif
    146 
    147 pthread_mutex_t GlobalHalMutex = PTHREAD_MUTEX_INITIALIZER;
    148 
    149 /*******************************************************************************
    150  * MPLSensor class implementation
    151  ******************************************************************************/
    152 
    153 MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *))
    154                        : SensorBase(NULL, NULL),
    155                          mNewData(0),
    156                          mMasterSensorMask(INV_ALL_SENSORS),
    157                          mLocalSensorMask(0),
    158                          mPollTime(-1),
    159                          mHaveGoodMpuCal(0),
    160                          mGyroAccuracy(0),
    161                          mAccelAccuracy(0),
    162                          mCompassAccuracy(0),
    163                          mSampleCount(0),
    164                          dmp_orient_fd(-1),
    165                          mDmpOrientationEnabled(0),
    166                          mEnabled(0),
    167                          mOldEnabledMask(0),
    168                          mAccelInputReader(4),
    169                          mGyroInputReader(32),
    170                          mTempScale(0),
    171                          mTempOffset(0),
    172                          mTempCurrentTime(0),
    173                          mAccelScale(2),
    174                          mPendingMask(0),
    175                          mSensorMask(0),
    176                          mFeatureActiveMask(0) {
    177     VFUNC_LOG;
    178 
    179     inv_error_t rv;
    180     int i, fd;
    181     char *port = NULL;
    182     char *ver_str;
    183     unsigned long mSensorMask;
    184     int res;
    185     FILE *fptr;
    186 
    187     mCompassSensor = compass;
    188 
    189     LOGV_IF(EXTRA_VERBOSE,
    190             "HAL:MPLSensor constructor : numSensors = %d", numSensors);
    191 
    192     pthread_mutex_init(&mMplMutex, NULL);
    193     pthread_mutex_init(&mHALMutex, NULL);
    194     memset(mGyroOrientation, 0, sizeof(mGyroOrientation));
    195     memset(mAccelOrientation, 0, sizeof(mAccelOrientation));
    196 
    197 #ifdef INV_PLAYBACK_DBG
    198     LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging");
    199     logfile = fopen("/data/playback.bin", "wb");
    200     if (logfile)
    201         inv_turn_on_data_logging(logfile);
    202 #endif
    203 
    204     /* setup sysfs paths */
    205     inv_init_sysfs_attributes();
    206 
    207     /* get chip name */
    208     if (inv_get_chip_name(chip_ID) != INV_SUCCESS) {
    209         LOGE("HAL:ERR- Failed to get chip ID\n");
    210     } else {
    211         LOGV_IF(PROCESS_VERBOSE, "HAL:Chip ID= %s\n", chip_ID);
    212     }
    213 
    214     enable_iio_sysfs();
    215 
    216     /* turn on power state */
    217     onPower(1);
    218 
    219     /* reset driver master enable */
    220     masterEnable(0);
    221 
    222     if (isLowPowerQuatEnabled() || isDmpDisplayOrientationOn()) {
    223         /* Load DMP image if capable, ie. MPU6xxx/9xxx */
    224         loadDMP();
    225     }
    226 
    227     /* open temperature fd for temp comp */
    228     LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature);
    229     gyro_temperature_fd = open(mpu.temperature, O_RDONLY);
    230     if (gyro_temperature_fd == -1) {
    231         LOGE("HAL:could not open temperature node");
    232     } else {
    233         LOGV_IF(EXTRA_VERBOSE,
    234                 "HAL:temperature_fd opened: %s", mpu.temperature);
    235     }
    236 
    237     /* read accel FSR to calcuate accel scale later */
    238     if (!USE_THIRD_PARTY_ACCEL) {
    239         char buf[3];
    240         int count = 0;
    241         LOGV_IF(SYSFS_VERBOSE,
    242                 "HAL:sysfs:cat %s (%lld)", mpu.accel_fsr, getTimestamp());
    243 
    244         fd = open(mpu.accel_fsr, O_RDONLY);
    245         if(fd < 0) {
    246             LOGE("HAL:Error opening accel FSR");
    247         } else {
    248            memset(buf, 0, sizeof(buf));
    249            count = read_attribute_sensor(fd, buf, sizeof(buf));
    250            if(count < 1) {
    251                LOGE("HAL:Error reading accel FSR");
    252            } else {
    253                count = sscanf(buf, "%d", &mAccelScale);
    254                if(count)
    255                    LOGV_IF(EXTRA_VERBOSE, "HAL:Accel FSR used %d", mAccelScale);
    256            }
    257            close(fd);
    258         }
    259     }
    260 
    261     /* initialize sensor data */
    262     memset(mPendingEvents, 0, sizeof(mPendingEvents));
    263 
    264     mPendingEvents[RotationVector].version = sizeof(sensors_event_t);
    265     mPendingEvents[RotationVector].sensor = ID_RV;
    266     mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR;
    267 
    268     mPendingEvents[LinearAccel].version = sizeof(sensors_event_t);
    269     mPendingEvents[LinearAccel].sensor = ID_LA;
    270     mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
    271 
    272     mPendingEvents[Gravity].version = sizeof(sensors_event_t);
    273     mPendingEvents[Gravity].sensor = ID_GR;
    274     mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
    275 
    276     mPendingEvents[Gyro].version = sizeof(sensors_event_t);
    277     mPendingEvents[Gyro].sensor = ID_GY;
    278     mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
    279 
    280     mPendingEvents[RawGyro].version = sizeof(sensors_event_t);
    281     mPendingEvents[RawGyro].sensor = ID_RG;
    282     mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE;
    283 
    284     mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
    285     mPendingEvents[Accelerometer].sensor = ID_A;
    286     mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
    287 
    288     /* Invensense compass calibration */
    289     mPendingEvents[MagneticField].version = sizeof(sensors_event_t);
    290     mPendingEvents[MagneticField].sensor = ID_M;
    291     mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD;
    292     mPendingEvents[MagneticField].magnetic.status =
    293         SENSOR_STATUS_ACCURACY_HIGH;
    294 
    295     mPendingEvents[Orientation].version = sizeof(sensors_event_t);
    296     mPendingEvents[Orientation].sensor = ID_O;
    297     mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION;
    298     mPendingEvents[Orientation].orientation.status
    299             = SENSOR_STATUS_ACCURACY_HIGH;
    300 
    301     mHandlers[RotationVector] = &MPLSensor::rvHandler;
    302     mHandlers[LinearAccel] = &MPLSensor::laHandler;
    303     mHandlers[Gravity] = &MPLSensor::gravHandler;
    304     mHandlers[Gyro] = &MPLSensor::gyroHandler;
    305     mHandlers[RawGyro] = &MPLSensor::rawGyroHandler;
    306     mHandlers[Accelerometer] = &MPLSensor::accelHandler;
    307     mHandlers[MagneticField] = &MPLSensor::compassHandler;
    308     mHandlers[Orientation] = &MPLSensor::orienHandler;
    309 
    310     for (int i = 0; i < numSensors; i++) {
    311         mDelays[i] = 0;
    312     }
    313 
    314     (void)inv_get_version(&ver_str);
    315     LOGV_IF(PROCESS_VERBOSE, "%s\n", ver_str);
    316 
    317     /* setup MPL */
    318     inv_constructor_init();
    319 
    320     /* load calibration file from /data/inv_cal_data.bin */
    321     rv = inv_load_calibration();
    322     if(rv == INV_SUCCESS)
    323         LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded");
    324     else
    325         LOGE("HAL:Could not open or load MPL calibration file (%d)", rv);
    326 
    327     /* Takes external Accel Calibration Load Method */
    328     if( m_pt2AccelCalLoadFunc != NULL)
    329     {
    330         long accel_offset[3];
    331         long tmp_offset[3];
    332         int result = m_pt2AccelCalLoadFunc(accel_offset);
    333         if(result)
    334             LOGW("HAL:Vendor accelerometer calibration file load failed %d\n", result);
    335         else
    336         {
    337             LOGW("HAL:Vendor accelerometer calibration file successfully loaded");
    338             inv_get_accel_bias(tmp_offset, NULL);
    339             LOGV_IF(PROCESS_VERBOSE, "HAL:Original accel offset, %ld, %ld, %ld\n",
    340                tmp_offset[0], tmp_offset[1], tmp_offset[2]);
    341             inv_set_accel_bias(accel_offset, mAccelAccuracy);
    342             inv_get_accel_bias(tmp_offset, NULL);
    343             LOGV_IF(PROCESS_VERBOSE, "HAL:Set accel offset, %ld, %ld, %ld\n",
    344                tmp_offset[0], tmp_offset[1], tmp_offset[2]);
    345         }
    346     }
    347     /* End of Accel Calibration Load Method */
    348 
    349     inv_set_device_properties();
    350 
    351     /* disable driver master enable the first sensor goes on */
    352     masterEnable(0);
    353     enableGyro(0);
    354     enableAccel(0);
    355     enableCompass(0);
    356 
    357     if (isLowPowerQuatEnabled()) {
    358         enableLPQuaternion(0);
    359     }
    360 
    361     onPower(0);
    362 
    363     if (isDmpDisplayOrientationOn()) {
    364         enableDmpOrientation(!isDmpScreenAutoRotationEnabled());
    365     }
    366 
    367 }
    368 
    369 void MPLSensor::enable_iio_sysfs()
    370 {
    371     VFUNC_LOG;
    372 
    373     char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN];
    374     FILE *tempFp = NULL;
    375 
    376     /* ignore failures */
    377     write_sysfs_int(mpu.in_timestamp_en, 1);
    378 
    379     LOGV_IF(SYSFS_VERBOSE,
    380             "HAL:sysfs:cat %s (%lld)",
    381             mpu.trigger_name, getTimestamp());
    382     tempFp = fopen(mpu.trigger_name, "r");
    383     if (tempFp == NULL) {
    384         LOGE("HAL:could not open trigger name");
    385     } else {
    386         if (fscanf(tempFp, "%s", iio_trigger_name) < 0) {
    387             LOGE("HAL:could not read trigger name");
    388         }
    389         fclose(tempFp);
    390     }
    391 
    392     LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %s > %s (%lld)",
    393             iio_trigger_name, mpu.current_trigger, getTimestamp());
    394     tempFp = fopen(mpu.current_trigger, "w");
    395     if (tempFp == NULL) {
    396         LOGE("HAL:could not open current trigger");
    397     } else {
    398         if (fprintf(tempFp, "%s", iio_trigger_name) < 0 || fclose(tempFp) < 0) {
    399             LOGE("HAL:could not write current trigger %s err=%d", iio_trigger_name, errno);
    400         }
    401     }
    402 
    403     write_sysfs_int(mpu.buffer_length, IIO_BUFFER_LENGTH);
    404 
    405     if (inv_get_iio_device_node(iio_device_node) < 0) {
    406         LOGE("HAL:could retrive the iio device node");
    407     }
    408     iio_fd = open(iio_device_node, O_RDONLY);
    409     if (iio_fd < 0) {
    410         LOGE("HAL:could not open iio device node");
    411     } else {
    412         LOGV_IF(PROCESS_VERBOSE, "HAL:iio iio_fd (%s) opened: %d", iio_device_node, iio_fd);
    413     }
    414 }
    415 
    416 int MPLSensor::inv_constructor_init()
    417 {
    418     VFUNC_LOG;
    419 
    420     inv_error_t result = inv_init_mpl();
    421     if (result) {
    422         LOGE("HAL:inv_init_mpl() failed");
    423         return result;
    424     }
    425     result = inv_constructor_default_enable();
    426     result = inv_start_mpl();
    427     if (result) {
    428         LOGE("HAL:inv_start_mpl() failed");
    429         LOG_RESULT_LOCATION(result);
    430         return result;
    431     }
    432 
    433     return result;
    434 }
    435 
    436 int MPLSensor::inv_constructor_default_enable()
    437 {
    438     VFUNC_LOG;
    439 
    440     inv_error_t result;
    441 
    442     result = inv_enable_quaternion();
    443     if (result) {
    444         LOGE("HAL:Cannot enable quaternion\n");
    445         return result;
    446     }
    447 
    448     result = inv_enable_in_use_auto_calibration();
    449     if (result) {
    450         return result;
    451     }
    452 
    453     // result = inv_enable_motion_no_motion();
    454     result = inv_enable_fast_nomot();
    455     if (result) {
    456         return result;
    457     }
    458 
    459     result = inv_enable_gyro_tc();
    460     if (result) {
    461         return result;
    462     }
    463 
    464     result = inv_enable_hal_outputs();
    465     if (result) {
    466         return result;
    467     }
    468 
    469     if (!mCompassSensor->providesCalibration()) {
    470         /* Invensense compass calibration */
    471         LOGV_IF(PROCESS_VERBOSE, "HAL:Invensense vector compass cal enabled");
    472         result = inv_enable_vector_compass_cal();
    473         if (result) {
    474             LOG_RESULT_LOCATION(result);
    475             return result;
    476         } else {
    477             mFeatureActiveMask |= INV_COMPASS_CAL;
    478         }
    479 
    480         // specify MPL's trust weight, used by compass algorithms
    481         inv_vector_compass_cal_sensitivity(3);
    482 
    483         result = inv_enable_compass_bias_w_gyro();
    484         if (result) {
    485             LOG_RESULT_LOCATION(result);
    486             return result;
    487         }
    488 
    489         result = inv_enable_heading_from_gyro();
    490         if (result) {
    491             LOG_RESULT_LOCATION(result);
    492             return result;
    493         }
    494 
    495         result = inv_enable_magnetic_disturbance();
    496         if (result) {
    497             LOG_RESULT_LOCATION(result);
    498             return result;
    499         }
    500     }
    501 
    502     result = inv_enable_9x_sensor_fusion();
    503     if (result) {
    504         LOG_RESULT_LOCATION(result);
    505         return result;
    506     } else {
    507         // 9x sensor fusion enables Compass fit
    508         mFeatureActiveMask |= INV_COMPASS_FIT;
    509     }
    510 
    511     result = inv_enable_no_gyro_fusion();
    512     if (result) {
    513         LOG_RESULT_LOCATION(result);
    514         return result;
    515     }
    516 
    517     result = inv_enable_quat_accuracy_monitor();
    518     if (result) {
    519         LOG_RESULT_LOCATION(result);
    520         return result;
    521     }
    522 
    523     return result;
    524 }
    525 
    526 /* TODO: create function pointers to calculate scale */
    527 void MPLSensor::inv_set_device_properties()
    528 {
    529     VFUNC_LOG;
    530 
    531     unsigned short orient;
    532 
    533     inv_get_sensors_orientation();
    534 
    535     inv_set_gyro_sample_rate(DEFAULT_MPL_GYRO_RATE);
    536     inv_set_compass_sample_rate(DEFAULT_MPL_COMPASS_RATE);
    537 
    538     /* gyro setup */
    539     orient = inv_orientation_matrix_to_scalar(mGyroOrientation);
    540     inv_set_gyro_orientation_and_scale(orient, 2000L << 15);
    541 
    542     /* accel setup */
    543     orient = inv_orientation_matrix_to_scalar(mAccelOrientation);
    544     /* use for third party accel input subsystem driver
    545     inv_set_accel_orientation_and_scale(orient, 1LL << 22);
    546     */
    547     inv_set_accel_orientation_and_scale(orient, mAccelScale << 15);
    548 
    549     /* compass setup */
    550     signed char orientMtx[9];
    551     mCompassSensor->getOrientationMatrix(orientMtx);
    552     orient =
    553         inv_orientation_matrix_to_scalar(orientMtx);
    554     long sensitivity;
    555     sensitivity = mCompassSensor->getSensitivity();
    556     inv_set_compass_orientation_and_scale(orient, sensitivity);
    557 }
    558 
    559 void MPLSensor::loadDMP()
    560 {
    561     int res, fd;
    562     FILE *fptr;
    563 
    564     if (isMpu3050()) {
    565         //DMP support only for MPU6xxx/9xxx currently
    566         return;
    567     }
    568 
    569     /* load DMP firmware */
    570     LOGV_IF(SYSFS_VERBOSE,
    571             "HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp());
    572     fd = open(mpu.firmware_loaded, O_RDONLY);
    573     if(fd < 0) {
    574         LOGE("HAL:could not open dmp state");
    575         return;
    576     }
    577     if(inv_read_dmp_state(fd)) {
    578         LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is already loaded");
    579         return;
    580     }
    581 
    582     LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware);
    583     fptr = fopen(mpu.dmp_firmware, "w");
    584     if(!fptr) {
    585         LOGE("HAL:could open %s for write. %s", mpu.dmp_firmware, strerror(errno));
    586         return;
    587     }
    588     res = inv_load_dmp(fptr);
    589     if(res < 0) {
    590         LOGE("HAL:load DMP failed");
    591     } else {
    592         LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded");
    593     }
    594     fclose(fptr);
    595 
    596     // onDMP(1);                //Can't enable here. See note onDMP()
    597 }
    598 
    599 void MPLSensor::inv_get_sensors_orientation()
    600 {
    601     FILE *fptr;
    602 
    603     // get gyro orientation
    604     LOGV_IF(SYSFS_VERBOSE,
    605             "HAL:sysfs:cat %s (%lld)", mpu.gyro_orient, getTimestamp());
    606     fptr = fopen(mpu.gyro_orient, "r");
    607     if (fptr != NULL) {
    608         int om[9];
    609         fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
    610                &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
    611                &om[6], &om[7], &om[8]);
    612         fclose(fptr);
    613 
    614         LOGV_IF(EXTRA_VERBOSE,
    615                 "HAL:gyro mounting matrix: "
    616                 "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
    617                 om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
    618 
    619         mGyroOrientation[0] = om[0];
    620         mGyroOrientation[1] = om[1];
    621         mGyroOrientation[2] = om[2];
    622         mGyroOrientation[3] = om[3];
    623         mGyroOrientation[4] = om[4];
    624         mGyroOrientation[5] = om[5];
    625         mGyroOrientation[6] = om[6];
    626         mGyroOrientation[7] = om[7];
    627         mGyroOrientation[8] = om[8];
    628     } else {
    629         LOGE("HAL:Couldn't read gyro mounting matrix");
    630     }
    631 
    632     // get accel orientation
    633     LOGV_IF(SYSFS_VERBOSE,
    634             "HAL:sysfs:cat %s (%lld)", mpu.accel_orient, getTimestamp());
    635     fptr = fopen(mpu.accel_orient, "r");
    636     if (fptr != NULL) {
    637         int om[9];
    638         fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
    639                &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
    640                &om[6], &om[7], &om[8]);
    641         fclose(fptr);
    642 
    643         LOGV_IF(EXTRA_VERBOSE,
    644                 "HAL:accel mounting matrix: "
    645                 "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
    646                 om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
    647 
    648         mAccelOrientation[0] = om[0];
    649         mAccelOrientation[1] = om[1];
    650         mAccelOrientation[2] = om[2];
    651         mAccelOrientation[3] = om[3];
    652         mAccelOrientation[4] = om[4];
    653         mAccelOrientation[5] = om[5];
    654         mAccelOrientation[6] = om[6];
    655         mAccelOrientation[7] = om[7];
    656         mAccelOrientation[8] = om[8];
    657     } else {
    658         LOGE("HAL:Couldn't read accel mounting matrix");
    659     }
    660 }
    661 
    662 MPLSensor::~MPLSensor()
    663 {
    664     VFUNC_LOG;
    665 
    666     mCompassSensor = NULL;
    667 
    668     /* Close open fds */
    669     if (iio_fd > 0)
    670         close(iio_fd);
    671     if( accel_fd > 0 )
    672         close(accel_fd );
    673     if (gyro_temperature_fd > 0)
    674         close(gyro_temperature_fd);
    675     if (sysfs_names_ptr)
    676         free(sysfs_names_ptr);
    677 
    678     if (isDmpDisplayOrientationOn()) {
    679         closeDmpOrientFd();
    680     }
    681 
    682     /* Turn off Gyro master enable          */
    683     /* A workaround until driver handles it */
    684     /* TODO: Turn off and close all sensors */
    685     if(write_sysfs_int(mpu.chip_enable, 0) < 0) {
    686         LOGE("HAL:could not disable gyro master enable");
    687     }
    688 
    689 #ifdef INV_PLAYBACK_DBG
    690     inv_turn_off_data_logging();
    691     fclose(logfile);
    692 #endif
    693 }
    694 
    695 #define GY_ENABLED (((1 << ID_GY) | (1 << ID_RG)) & enabled_sensors)
    696 #define A_ENABLED  ((1 << ID_A)  & enabled_sensors)
    697 #define M_ENABLED  ((1 << ID_M)  & enabled_sensors)
    698 #define O_ENABLED  ((1 << ID_O)  & enabled_sensors)
    699 #define LA_ENABLED ((1 << ID_LA) & enabled_sensors)
    700 #define GR_ENABLED ((1 << ID_GR) & enabled_sensors)
    701 #define RV_ENABLED ((1 << ID_RV) & enabled_sensors)
    702 
    703 /* TODO: this step is optional, remove?  */
    704 int MPLSensor::setGyroInitialState()
    705 {
    706     VFUNC_LOG;
    707 
    708     int res = 0;
    709 
    710     LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
    711             HW_GYRO_RATE_HZ, mpu.gyro_fifo_rate, getTimestamp());
    712     int fd = open(mpu.gyro_fifo_rate, O_RDWR);
    713     res = errno;
    714     if(fd < 0) {
    715         LOGE("HAL:open of %s failed with '%s' (%d)",
    716              mpu.gyro_fifo_rate, strerror(res), res);
    717         return res;
    718     }
    719     res = write_attribute_sensor(fd, HW_GYRO_RATE_HZ);
    720     if(res < 0) {
    721         LOGE("HAL:write_attribute_sensor : error writing %s with %d",
    722              mpu.gyro_fifo_rate, HW_GYRO_RATE_HZ);
    723         return res;
    724     }
    725 
    726     // Setting LPF is deprecated
    727 
    728     return 0;
    729 }
    730 
    731 /* this applies to BMA250 Input Subsystem Driver only */
    732 int MPLSensor::setAccelInitialState()
    733 {
    734     VFUNC_LOG;
    735 
    736     struct input_absinfo absinfo_x;
    737     struct input_absinfo absinfo_y;
    738     struct input_absinfo absinfo_z;
    739     float value;
    740     if (!ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo_x) &&
    741         !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo_y) &&
    742         !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo_z)) {
    743         value = absinfo_x.value;
    744         mPendingEvents[Accelerometer].data[0] = value * CONVERT_A_X;
    745         value = absinfo_y.value;
    746         mPendingEvents[Accelerometer].data[1] = value * CONVERT_A_Y;
    747         value = absinfo_z.value;
    748         mPendingEvents[Accelerometer].data[2] = value * CONVERT_A_Z;
    749         //mHasPendingEvent = true;
    750     }
    751     return 0;
    752 }
    753 
    754 int MPLSensor::onPower(int en)
    755 {
    756     VFUNC_LOG;
    757 
    758     int res;
    759 
    760     int count, curr_power_state;
    761 
    762     LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
    763             en, mpu.power_state, getTimestamp());
    764     res = read_sysfs_int(mpu.power_state, &curr_power_state);
    765     if (res < 0) {
    766         LOGE("HAL:Error reading power state");
    767         // will set power_state anyway
    768         curr_power_state = -1;
    769     }
    770     if (en != curr_power_state) {
    771         if((res = write_sysfs_int(mpu.power_state, en)) < 0) {
    772                 LOGE("HAL:Couldn't write power state");
    773         }
    774     } else {
    775         LOGV_IF(EXTRA_VERBOSE,
    776                 "HAL:Power state already enable/disable curr=%d new=%d",
    777                 curr_power_state, en);
    778     }
    779     return res;
    780 }
    781 
    782 int MPLSensor::onDMP(int en)
    783 {
    784     VFUNC_LOG;
    785 
    786     int res = -1;
    787     int status;
    788 
    789     //Sequence to enable DMP
    790     //1. Turn On power if not already on
    791     //2. Load DMP image if not already loaded
    792     //3. Either Gyro or Accel must be enabled/configured before next step
    793     //4. Enable DMP
    794 
    795     LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
    796             mpu.firmware_loaded, getTimestamp());
    797     res = read_sysfs_int(mpu.firmware_loaded, &status);
    798     if (res < 0){
    799         LOGE("HAL:ERR can't get firmware_loaded status");
    800         return res;
    801     }
    802     LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs: %s status=%d", mpu.firmware_loaded, status);
    803 
    804     if (status) {
    805         //Write only if curr DMP state <> request
    806         LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
    807                 mpu.dmp_on, getTimestamp());
    808         if (read_sysfs_int(mpu.dmp_on, &status) < 0) {
    809             LOGE("HAL:ERR can't read DMP state");
    810         } else if (status != en) {
    811             res = write_sysfs_int(mpu.dmp_on, en);
    812             //Enable DMP interrupt
    813             if (write_sysfs_int(mpu.dmp_int_on, en) < 0) {
    814                 LOGE("HAL:ERR can't en/dis DMP interrupt");
    815             }
    816         } else {
    817             res = 0;    //DMP already set as requested
    818         }
    819     } else {
    820         LOGE("HAL:ERR No DMP image");
    821     }
    822     return res;
    823 }
    824 
    825 int MPLSensor::checkLPQuaternion(void)
    826 {
    827     VFUNC_LOG;
    828 
    829     return ((mFeatureActiveMask & INV_DMP_QUATERNION)? 1:0);
    830 }
    831 
    832 int MPLSensor::enableLPQuaternion(int en)
    833 {
    834     VFUNC_LOG;
    835 
    836     if (!en) {
    837         enableQuaternionData(0);
    838         onDMP(0);
    839         mFeatureActiveMask &= ~INV_DMP_QUATERNION;
    840         LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat disabled");
    841     } else {
    842         if (enableQuaternionData(1) < 0 || onDMP(1) < 0) {
    843             LOGE("HAL:ERR can't enable LP Quaternion");
    844         } else {
    845             mFeatureActiveMask |= INV_DMP_QUATERNION;
    846             LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat enabled");
    847         }
    848     }
    849     return 0;
    850 }
    851 
    852 int MPLSensor::enableQuaternionData(int en)
    853 {
    854     int res = 0;
    855     VFUNC_LOG;
    856 
    857     // Enable DMP quaternion
    858     res = write_sysfs_int(mpu.quaternion_on, en);
    859 
    860     if (!en) {
    861         LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling quat scan elems");
    862     } else {
    863 
    864         LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling quat scan elems");
    865     }
    866     write_sysfs_int(mpu.in_quat_r_en, en);
    867     write_sysfs_int(mpu.in_quat_x_en, en);
    868     write_sysfs_int(mpu.in_quat_y_en, en);
    869     write_sysfs_int(mpu.in_quat_z_en, en);
    870 
    871     LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off");
    872 
    873     if (!en) {
    874         inv_quaternion_sensor_was_turned_off();
    875     }
    876 
    877     return res;
    878 }
    879 
    880 int MPLSensor::enableTap(int en)
    881 {
    882     VFUNC_LOG;
    883 
    884     return 0;
    885 }
    886 
    887 int MPLSensor::enableFlick(int en)
    888 {
    889     VFUNC_LOG;
    890 
    891     return 0;
    892 }
    893 
    894 int MPLSensor::enablePedometer(int en)
    895 {
    896     VFUNC_LOG;
    897 
    898     return 0;
    899 }
    900 
    901 int MPLSensor::masterEnable(int en)
    902 {
    903     VFUNC_LOG;
    904     return write_sysfs_int(mpu.chip_enable, en);
    905 }
    906 
    907 int MPLSensor::enableGyro(int en)
    908 {
    909     VFUNC_LOG;
    910 
    911     /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
    912     int res;
    913 
    914     /* need to also turn on/off the master enable */
    915     res = write_sysfs_int(mpu.gyro_enable, en);
    916 
    917     if (!en) {
    918         LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off");
    919         inv_gyro_was_turned_off();
    920     } else {
    921         write_sysfs_int(mpu.gyro_x_fifo_enable, en);
    922         write_sysfs_int(mpu.gyro_y_fifo_enable, en);
    923         res = write_sysfs_int(mpu.gyro_z_fifo_enable, en);
    924     }
    925 
    926     return res;
    927 }
    928 
    929 int MPLSensor::enableAccel(int en)
    930 {
    931     VFUNC_LOG;
    932 
    933     /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
    934     int res;
    935 
    936     /* need to also turn on/off the master enable */
    937     res = write_sysfs_int(mpu.accel_enable, en);
    938 
    939     if (!en) {
    940         LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off");
    941         inv_accel_was_turned_off();
    942     } else {
    943         write_sysfs_int(mpu.accel_x_fifo_enable, en);
    944         write_sysfs_int(mpu.accel_y_fifo_enable, en);
    945         res = write_sysfs_int(mpu.accel_z_fifo_enable, en);
    946     }
    947 
    948     return res;
    949 }
    950 
    951 int MPLSensor::enableCompass(int en)
    952 {
    953     VFUNC_LOG;
    954 
    955     int res = mCompassSensor->enable(ID_M, en);
    956     if (!en) {
    957         LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off");
    958         inv_compass_was_turned_off();
    959     }
    960     return res;
    961 }
    962 
    963 void MPLSensor::computeLocalSensorMask(int enabled_sensors)
    964 {
    965     VFUNC_LOG;
    966 
    967     do {
    968         if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
    969             LOGV_IF(ENG_VERBOSE, "FUSION ENABLED");
    970             mLocalSensorMask = ALL_MPL_SENSORS_NP;
    971             break;
    972         }
    973 
    974         if(!A_ENABLED && !M_ENABLED && !GY_ENABLED) {
    975             /* Invensense compass cal */
    976             LOGV_IF(ENG_VERBOSE, "ALL DISABLED");
    977             mLocalSensorMask = 0;
    978             break;
    979         }
    980 
    981         if (GY_ENABLED) {
    982             LOGV_IF(ENG_VERBOSE, "G ENABLED");
    983             mLocalSensorMask |= INV_THREE_AXIS_GYRO;
    984         } else {
    985             LOGV_IF(ENG_VERBOSE, "G DISABLED");
    986             mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
    987         }
    988 
    989         if (A_ENABLED) {
    990             LOGV_IF(ENG_VERBOSE, "A ENABLED");
    991             mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
    992         } else {
    993             LOGV_IF(ENG_VERBOSE, "A DISABLED");
    994             mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
    995         }
    996 
    997         /* Invensense compass calibration */
    998         if (M_ENABLED) {
    999             LOGV_IF(ENG_VERBOSE, "M ENABLED");
   1000             mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
   1001         } else {
   1002             LOGV_IF(ENG_VERBOSE, "M DISABLED");
   1003             mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS;
   1004         }
   1005     } while (0);
   1006 }
   1007 
   1008 int MPLSensor::enableOneSensor(int en, const char *name, int (MPLSensor::*enabler)(int)) {
   1009     LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - %s %s", en ? "enabled" : "disable", name);
   1010     return (this->*enabler)(en);
   1011 }
   1012 
   1013 int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) {
   1014     VFUNC_LOG;
   1015 
   1016     inv_error_t res = -1;
   1017     bool store_cal = false;
   1018     bool ext_compass_changed = false;
   1019 
   1020     // Sequence to enable or disable a sensor
   1021     // 1. enable Power state
   1022     // 2. reset master enable (=0)
   1023     // 3. enable or disable a sensor
   1024     // 4. set master enable (=1)
   1025 
   1026     pthread_mutex_lock(&GlobalHalMutex);
   1027 
   1028     uint32_t all_changeables = (1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer)
   1029             | (1 << MagneticField);
   1030     uint32_t all_integrated_changeables = all_changeables;
   1031 
   1032     if (!mCompassSensor->isIntegrated()) {
   1033         ext_compass_changed = changed & (1 << MagneticField);
   1034         all_integrated_changeables = all_changeables & ~(1 << MagneticField);
   1035     }
   1036 
   1037     if (isLowPowerQuatEnabled() || (changed & all_integrated_changeables)) {
   1038         /* ensure power state is on */
   1039         onPower(1);
   1040 
   1041         /* reset master enable */
   1042         res = masterEnable(0);
   1043         if(res < 0) {
   1044             goto unlock_res;
   1045         }
   1046     }
   1047 
   1048     LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", (unsigned int)sensors);
   1049 
   1050     if (changed & ((1 << Gyro) | (1 << RawGyro))) {
   1051         res = enableOneSensor(sensors & INV_THREE_AXIS_GYRO, "gyro", &MPLSensor::enableGyro);
   1052         if(res < 0) {
   1053             goto unlock_res;
   1054         }
   1055     }
   1056 
   1057     if (changed & (1 << Accelerometer)) {
   1058         res = enableOneSensor(sensors & INV_THREE_AXIS_ACCEL, "accel", &MPLSensor::enableAccel);
   1059         if(res < 0) {
   1060             goto unlock_res;
   1061         }
   1062     }
   1063 
   1064     if (changed & (1 << MagneticField)) {
   1065         /* Invensense compass calibration */
   1066         res = enableOneSensor(sensors & INV_THREE_AXIS_COMPASS, "compass", &MPLSensor::enableCompass);
   1067         if(res < 0) {
   1068             goto unlock_res;
   1069         }
   1070     }
   1071 
   1072     if ( isLowPowerQuatEnabled() ) {
   1073         // Enable LP Quat
   1074         if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) |
   1075                 (1 << LinearAccel) | (1 << Gravity)))) {
   1076             if (!(changed & all_integrated_changeables)) {
   1077                 /* ensure power state is on */
   1078                 onPower(1);
   1079                 /* reset master enable */
   1080                 res = masterEnable(0);
   1081                 if(res < 0) {
   1082                     goto unlock_res;
   1083                 }
   1084             }
   1085             if (!checkLPQuaternion()) {
   1086                 enableLPQuaternion(1);
   1087             } else {
   1088                 LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled");
   1089             }
   1090         } else if (checkLPQuaternion()) {
   1091             enableLPQuaternion(0);
   1092         }
   1093     }
   1094 
   1095     if (changed & all_integrated_changeables) {
   1096         if (sensors &
   1097             (INV_THREE_AXIS_GYRO
   1098                 | INV_THREE_AXIS_ACCEL
   1099                 | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
   1100             if ( isLowPowerQuatEnabled() ||
   1101                     (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) ) {
   1102                 // disable DMP event interrupt only (w/ data interrupt)
   1103                 if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
   1104                     res = -1;
   1105                     LOGE("HAL:ERR can't disable DMP event interrupt");
   1106                     goto unlock_res;
   1107                 }
   1108             }
   1109 
   1110             if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
   1111                 // enable DMP
   1112                 onDMP(1);
   1113 
   1114                 res = enableAccel(1);
   1115                 if(res < 0) {
   1116                     goto unlock_res;
   1117                 }
   1118 
   1119                 if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
   1120                     res = turnOffAccelFifo();
   1121                 }
   1122                 if(res < 0) {
   1123                     goto unlock_res;
   1124                 }
   1125             }
   1126 
   1127             res = masterEnable(1);
   1128             if(res < 0) {
   1129                 goto unlock_res;
   1130             }
   1131         } else { // all sensors idle -> reduce power
   1132             if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
   1133                 // enable DMP
   1134                 onDMP(1);
   1135                 // enable DMP event interrupt only (no data interrupt)
   1136                 if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) {
   1137                     res = -1;
   1138                     LOGE("HAL:ERR can't enable DMP event interrupt");
   1139                 }
   1140                 res = enableAccel(1);
   1141                 if(res < 0) {
   1142                     goto unlock_res;
   1143                 }
   1144                 if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
   1145                     res = turnOffAccelFifo();
   1146                 }
   1147                 if(res < 0) {
   1148                     goto unlock_res;
   1149                 }
   1150                 res = masterEnable(1);
   1151                 if(res < 0) {
   1152                     goto unlock_res;
   1153                 }
   1154             }
   1155             else {
   1156                 res = onPower(0);
   1157                 if(res < 0) {
   1158                     goto unlock_res;
   1159                 }
   1160             }
   1161             store_cal = true;
   1162         }
   1163     } else if (ext_compass_changed &&
   1164             !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL
   1165                 | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated()))))) {
   1166             store_cal = true;
   1167     }
   1168 
   1169     if (store_cal || ((changed & all_changeables) != all_changeables)) {
   1170         storeCalibration();
   1171     }
   1172 
   1173 unlock_res:
   1174     pthread_mutex_unlock(&GlobalHalMutex);
   1175     return res;
   1176 }
   1177 
   1178 /* Store calibration file */
   1179 void MPLSensor::storeCalibration()
   1180 {
   1181     if(mHaveGoodMpuCal || mAccelAccuracy >= 2 || mCompassAccuracy >= 3) {
   1182        int res = inv_store_calibration();
   1183        if (res) {
   1184            LOGE("HAL:Cannot store calibration on file");
   1185        } else {
   1186            LOGV_IF(PROCESS_VERBOSE, "HAL:Cal file updated");
   1187        }
   1188     }
   1189 }
   1190 
   1191 void MPLSensor::cbProcData()
   1192 {
   1193     mNewData = 1;
   1194     mSampleCount++;
   1195     LOGV_IF(EXTRA_VERBOSE, "HAL:new data");
   1196 }
   1197 
   1198 /*  these handlers transform mpl data into one of the Android sensor types */
   1199 int MPLSensor::gyroHandler(sensors_event_t* s)
   1200 {
   1201     VHANDLER_LOG;
   1202     int8_t status;
   1203     int update;
   1204     update = inv_get_sensor_type_gyroscope(s->gyro.v, &status, &s->timestamp);
   1205     LOGV_IF(HANDLER_DATA, "HAL:gyro data : %+f %+f %+f -- %lld - %d",
   1206             s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
   1207     return update;
   1208 }
   1209 
   1210 int MPLSensor::rawGyroHandler(sensors_event_t* s)
   1211 {
   1212     VHANDLER_LOG;
   1213     int8_t status;
   1214     int update;
   1215     update = inv_get_sensor_type_gyroscope_raw(s->gyro.v, &status, &s->timestamp);
   1216     LOGV_IF(HANDLER_DATA, "HAL:raw gyro data : %+f %+f %+f -- %lld - %d",
   1217             s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
   1218     return update;
   1219 }
   1220 
   1221 int MPLSensor::accelHandler(sensors_event_t* s)
   1222 {
   1223     VHANDLER_LOG;
   1224     int8_t status;
   1225     int update;
   1226     update = inv_get_sensor_type_accelerometer(
   1227         s->acceleration.v, &status, &s->timestamp);
   1228     LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d",
   1229             s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2],
   1230             s->timestamp, update);
   1231     mAccelAccuracy = status;
   1232     return update;
   1233 }
   1234 
   1235 int MPLSensor::compassHandler(sensors_event_t* s)
   1236 {
   1237     VHANDLER_LOG;
   1238     int update;
   1239     update = inv_get_sensor_type_magnetic_field(
   1240         s->magnetic.v, &s->magnetic.status, &s->timestamp);
   1241     LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d",
   1242             s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2], s->timestamp, update);
   1243     mCompassAccuracy = s->magnetic.status;
   1244     return update;
   1245 }
   1246 
   1247 int MPLSensor::rvHandler(sensors_event_t* s)
   1248 {
   1249     // rotation vector does not have an accuracy or status
   1250     VHANDLER_LOG;
   1251     int8_t status;
   1252     int update;
   1253     update = inv_get_sensor_type_rotation_vector(s->data, &status, &s->timestamp);
   1254     LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f - %+lld - %d",
   1255             s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update);
   1256     return update;
   1257 }
   1258 
   1259 int MPLSensor::laHandler(sensors_event_t* s)
   1260 {
   1261     VHANDLER_LOG;
   1262     int8_t status;
   1263     int update;
   1264     update = inv_get_sensor_type_linear_acceleration(
   1265             s->gyro.v, &status, &s->timestamp);
   1266     LOGV_IF(HANDLER_DATA, "HAL:la data: %+f %+f %+f - %lld - %d",
   1267             s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
   1268     return update;
   1269 }
   1270 
   1271 int MPLSensor::gravHandler(sensors_event_t* s)
   1272 {
   1273     VHANDLER_LOG;
   1274     int8_t status;
   1275     int update;
   1276     update = inv_get_sensor_type_gravity(s->gyro.v, &status, &s->timestamp);
   1277     LOGV_IF(HANDLER_DATA, "HAL:gr data: %+f %+f %+f - %lld - %d",
   1278             s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
   1279     return update;
   1280 }
   1281 
   1282 int MPLSensor::orienHandler(sensors_event_t* s)
   1283 {
   1284     VHANDLER_LOG;
   1285     int update;
   1286     update = inv_get_sensor_type_orientation(
   1287             s->orientation.v, &s->orientation.status, &s->timestamp);
   1288     LOGV_IF(HANDLER_DATA, "HAL:or data: %f %f %f - %lld - %d",
   1289             s->orientation.v[0], s->orientation.v[1], s->orientation.v[2], s->timestamp, update);
   1290     return update;
   1291 }
   1292 
   1293 int MPLSensor::enable(int32_t handle, int en)
   1294 {
   1295     VFUNC_LOG;
   1296 
   1297     android::String8 sname;
   1298     int what = -1, err = 0;
   1299 
   1300     switch (handle) {
   1301     case ID_SO:
   1302         sname = "Screen Orientation";
   1303         LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle,
   1304             (mDmpOrientationEnabled? "en": "dis"),
   1305             (en? "en" : "dis"));
   1306         enableDmpOrientation(en && isDmpDisplayOrientationOn());
   1307         /* TODO: stop manually testing/using 0 and 1 instead of
   1308          * false and true, but just use 0 and non-0.
   1309          * This allows  passing 0 and non-0 ints around instead of
   1310          * having to convert to 1 and test against 1.
   1311          */
   1312         mDmpOrientationEnabled = en;
   1313         return 0;
   1314     case ID_A:
   1315         what = Accelerometer;
   1316         sname = "Accelerometer";
   1317         break;
   1318     case ID_M:
   1319         what = MagneticField;
   1320         sname = "MagneticField";
   1321         break;
   1322     case ID_O:
   1323         what = Orientation;
   1324         sname = "Orientation";
   1325         break;
   1326     case ID_GY:
   1327         what = Gyro;
   1328         sname = "Gyro";
   1329         break;
   1330     case ID_RG:
   1331         what = RawGyro;
   1332         sname = "RawGyro";
   1333         break;
   1334     case ID_GR:
   1335         what = Gravity;
   1336         sname = "Gravity";
   1337         break;
   1338     case ID_RV:
   1339         what = RotationVector;
   1340         sname = "RotationVector";
   1341         break;
   1342     case ID_LA:
   1343         what = LinearAccel;
   1344         sname = "LinearAccel";
   1345         break;
   1346     default: //this takes care of all the gestures
   1347         what = handle;
   1348         sname = "Others";
   1349         break;
   1350     }
   1351 
   1352     if (uint32_t(what) >= numSensors)
   1353         return -EINVAL;
   1354 
   1355     int newState = en ? 1 : 0;
   1356     unsigned long sen_mask;
   1357 
   1358     LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle,
   1359             ((mEnabled & (1 << what)) ? "en" : "dis"),
   1360             ((uint32_t(newState) << what) ? "en" : "dis"));
   1361     LOGV_IF(PROCESS_VERBOSE,
   1362             "HAL:%s sensor state change what=%d", sname.string(), what);
   1363 
   1364     // pthread_mutex_lock(&mMplMutex);
   1365     // pthread_mutex_lock(&mHALMutex);
   1366 
   1367     if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) {
   1368         uint32_t sensor_type;
   1369         short flags = newState;
   1370         uint32_t lastEnabled = mEnabled, changed = 0;
   1371 
   1372         mEnabled &= ~(1 << what);
   1373         mEnabled |= (uint32_t(flags) << what);
   1374 
   1375         LOGV_IF(PROCESS_VERBOSE, "HAL:handle = %d", handle);
   1376         LOGV_IF(PROCESS_VERBOSE, "HAL:flags = %d", flags);
   1377         computeLocalSensorMask(mEnabled);
   1378         LOGV_IF(PROCESS_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled);
   1379         sen_mask = mLocalSensorMask & mMasterSensorMask;
   1380         mSensorMask = sen_mask;
   1381         LOGV_IF(PROCESS_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask);
   1382 
   1383         switch (what) {
   1384             case Gyro:
   1385             case RawGyro:
   1386             case Accelerometer:
   1387             case MagneticField:
   1388                 if (!(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
   1389                         (1 << LinearAccel) | (1 << Gravity))) &&
   1390                         ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) {
   1391                     changed |= (1 << what);
   1392                 }
   1393                 break;
   1394 
   1395             case Orientation:
   1396             case RotationVector:
   1397             case LinearAccel:
   1398             case Gravity:
   1399                 if ((en && !(lastEnabled & ((1 << Orientation) | (1 << RotationVector) |
   1400                         (1 << LinearAccel) | (1 << Gravity)))) ||
   1401                         (!en && !(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
   1402                         (1 << LinearAccel) | (1 << Gravity))))) {
   1403                     for (int i = Gyro; i <= MagneticField; i++) {
   1404                         if (!(mEnabled & (1 << i))) {
   1405                             changed |= (1 << i);
   1406                         }
   1407                     }
   1408                 }
   1409                 break;
   1410         }
   1411         LOGV_IF(PROCESS_VERBOSE, "HAL:changed = %d", changed);
   1412         enableSensors(sen_mask, flags, changed);
   1413     }
   1414 
   1415     // pthread_mutex_unlock(&mMplMutex);
   1416     // pthread_mutex_unlock(&mHALMutex);
   1417 
   1418 #ifdef INV_PLAYBACK_DBG
   1419     /* apparently the logging needs to be go through this sequence
   1420        to properly flush the log file */
   1421     inv_turn_off_data_logging();
   1422     fclose(logfile);
   1423     logfile = fopen("/data/playback.bin", "ab");
   1424     if (logfile)
   1425         inv_turn_on_data_logging(logfile);
   1426 #endif
   1427 
   1428     return err;
   1429 }
   1430 
   1431 int MPLSensor::setDelay(int32_t handle, int64_t ns)
   1432 {
   1433     VFUNC_LOG;
   1434 
   1435     android::String8 sname;
   1436     int what = -1;
   1437 
   1438     switch (handle) {
   1439         case ID_SO:
   1440             return update_delay();
   1441         case ID_A:
   1442             what = Accelerometer;
   1443             sname = "Accelerometer";
   1444             break;
   1445         case ID_M:
   1446             what = MagneticField;
   1447             sname = "MagneticField";
   1448             break;
   1449         case ID_O:
   1450             what = Orientation;
   1451             sname = "Orientation";
   1452             break;
   1453         case ID_GY:
   1454             what = Gyro;
   1455             sname = "Gyro";
   1456             break;
   1457         case ID_RG:
   1458             what = RawGyro;
   1459             sname = "RawGyro";
   1460             break;
   1461         case ID_GR:
   1462             what = Gravity;
   1463             sname = "Gravity";
   1464             break;
   1465         case ID_RV:
   1466             what = RotationVector;
   1467             sname = "RotationVector";
   1468             break;
   1469         case ID_LA:
   1470             what = LinearAccel;
   1471             sname = "LinearAccel";
   1472             break;
   1473         default: // this takes care of all the gestures
   1474             what = handle;
   1475             sname = "Others";
   1476             break;
   1477     }
   1478 
   1479 #if 0
   1480     // skip the 1st call for enalbing sensors called by ICS/JB sensor service
   1481     static int counter_delay = 0;
   1482     if (!(mEnabled & (1 << what))) {
   1483         counter_delay = 0;
   1484     } else {
   1485         if (++counter_delay == 1) {
   1486             return 0;
   1487         }
   1488         else {
   1489             counter_delay = 0;
   1490         }
   1491     }
   1492 #endif
   1493 
   1494     if (uint32_t(what) >= numSensors)
   1495         return -EINVAL;
   1496 
   1497     if (ns < 0)
   1498         return -EINVAL;
   1499 
   1500     LOGV_IF(PROCESS_VERBOSE, "setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns);
   1501 
   1502     // limit all rates to reasonable ones */
   1503 /*
   1504     if (ns < 10000000LL) {
   1505         ns = 10000000LL;
   1506     }
   1507 */
   1508     if (ns < 5000000LL) {
   1509         ns = 5000000LL;
   1510     }
   1511 
   1512     /* store request rate to mDelays arrary for each sensor */
   1513     mDelays[what] = ns;
   1514 
   1515     switch (what) {
   1516         case Gyro:
   1517         case RawGyro:
   1518         case Accelerometer:
   1519             for (int i = Gyro; i <= Accelerometer + mCompassSensor->isIntegrated();
   1520                     i++) {
   1521                 if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
   1522                     LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i);
   1523                     return 0;
   1524                 }
   1525             }
   1526             break;
   1527 
   1528         case MagneticField:
   1529             if (mCompassSensor->isIntegrated() &&
   1530                     (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) ||
   1531                     ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) ||
   1532                     ((mEnabled & (1 << Accelerometer)) && ns > mDelays[Accelerometer]))) {
   1533                  LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to gyro/accel");
   1534                  return 0;
   1535             }
   1536             break;
   1537 
   1538         case Orientation:
   1539         case RotationVector:
   1540         case LinearAccel:
   1541         case Gravity:
   1542             if (isLowPowerQuatEnabled()) {
   1543                 LOGV_IF(PROCESS_VERBOSE, "HAL:need to update delay due to LPQ");
   1544                 break;
   1545             }
   1546 
   1547             for (int i = 0; i < numSensors; i++) {
   1548                 if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
   1549                     LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i);
   1550                     return 0;
   1551                 }
   1552             }
   1553             break;
   1554     }
   1555 
   1556     // pthread_mutex_lock(&mHALMutex);
   1557     int res = update_delay();
   1558     // pthread_mutex_unlock(&mHALMutex);
   1559     return res;
   1560 }
   1561 
   1562 int MPLSensor::update_delay() {
   1563     VHANDLER_LOG;
   1564 
   1565     int res = 0;
   1566     int64_t got;
   1567 
   1568     pthread_mutex_lock(&GlobalHalMutex);
   1569     if (mEnabled) {
   1570         int64_t wanted = 1000000000;
   1571         int64_t wanted_3rd_party_sensor = 1000000000;
   1572 
   1573         // Sequence to change sensor's FIFO rate
   1574         // 1. enable Power state
   1575         // 2. reset master enable
   1576         // 3. Update delay
   1577         // 4. set master enable
   1578 
   1579         // ensure power on
   1580         onPower(1);
   1581 
   1582         // reset master enable
   1583         masterEnable(0);
   1584 
   1585         /* search the minimum delay requested across all enabled sensors */
   1586         for (int i = 0; i < numSensors; i++) {
   1587             if (mEnabled & (1 << i)) {
   1588                 int64_t ns = mDelays[i];
   1589                 wanted = wanted < ns ? wanted : ns;
   1590             }
   1591         }
   1592 
   1593         // same delay for 3rd party Accel or Compass
   1594         wanted_3rd_party_sensor = wanted;
   1595 
   1596         /* mpl rate in us in future maybe different for
   1597            gyro vs compass vs accel */
   1598         int rateInus = (int)wanted / 1000LL;
   1599         int mplGyroRate = rateInus;
   1600         int mplAccelRate = rateInus;
   1601         int mplCompassRate = rateInus;
   1602 
   1603         LOGV_IF(PROCESS_VERBOSE, "HAL:wanted rate for all sensors : "
   1604              "%llu ns, mpl rate: %d us, (%.2f Hz)",
   1605              wanted, rateInus, 1000000000.f / wanted);
   1606 
   1607         /* set rate in MPL */
   1608         /* compass can only do 100Hz max */
   1609         inv_set_gyro_sample_rate(mplGyroRate);
   1610         inv_set_accel_sample_rate(mplAccelRate);
   1611         inv_set_compass_sample_rate(mplCompassRate);
   1612 
   1613         /* TODO: Test 200Hz */
   1614         // inv_set_gyro_sample_rate(5000);
   1615         LOGV_IF(PROCESS_VERBOSE, "HAL:MPL gyro sample rate: %d", mplGyroRate);
   1616         LOGV_IF(PROCESS_VERBOSE, "HAL:MPL accel sample rate: %d", mplAccelRate);
   1617         LOGV_IF(PROCESS_VERBOSE, "HAL:MPL compass sample rate: %d", mplCompassRate);
   1618 
   1619         int enabled_sensors = mEnabled;
   1620         int tempFd = -1;
   1621         if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
   1622             if (isLowPowerQuatEnabled() ||
   1623                     (isDmpDisplayOrientationOn() && mDmpOrientationEnabled)) {
   1624                 bool setDMPrate= 0;
   1625                 // Set LP Quaternion sample rate if enabled
   1626                 if (checkLPQuaternion()) {
   1627                     if (wanted < RATE_200HZ) {
   1628                         enableLPQuaternion(0);
   1629                     } else {
   1630                         inv_set_quat_sample_rate(rateInus);
   1631                         setDMPrate= 1;
   1632                     }
   1633                 }
   1634 
   1635                 if (checkDMPOrientation() || setDMPrate==1) {
   1636                     getDmpRate(&wanted);
   1637                 }
   1638             }
   1639 
   1640             int64_t tempRate = wanted;
   1641             LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion");
   1642             //nsToHz
   1643             LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
   1644                     1000000000.f / tempRate, mpu.gyro_fifo_rate,
   1645                     getTimestamp());
   1646             tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
   1647             res = write_attribute_sensor(tempFd, 1000000000.f / tempRate);
   1648             if(res < 0) {
   1649                 LOGE("HAL:GYRO update delay error");
   1650             }
   1651 
   1652             //nsToHz (BMA250)
   1653             if(USE_THIRD_PARTY_ACCEL) {
   1654                 LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)",
   1655                         wanted_3rd_party_sensor / 1000000L, mpu.accel_fifo_rate,
   1656                         getTimestamp());
   1657                 tempFd = open(mpu.accel_fifo_rate, O_RDWR);
   1658                 res = write_attribute_sensor(tempFd, wanted_3rd_party_sensor / 1000000L);
   1659                 LOGE_IF(res < 0, "HAL:ACCEL update delay error");
   1660             }
   1661 
   1662             if (!mCompassSensor->isIntegrated()) {
   1663                 LOGV_IF(PROCESS_VERBOSE, "HAL:Ext compass rate %.2f Hz", 1000000000.f / wanted_3rd_party_sensor);
   1664                 mCompassSensor->setDelay(ID_M, wanted_3rd_party_sensor);
   1665                 got = mCompassSensor->getDelay(ID_M);
   1666                 inv_set_compass_sample_rate(got / 1000);
   1667             }
   1668 
   1669         } else {
   1670 
   1671             if (GY_ENABLED) {
   1672                 wanted = (mDelays[Gyro] <= mDelays[RawGyro]?
   1673                     (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]):
   1674                     (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro]));
   1675 
   1676                 if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
   1677                     getDmpRate(&wanted);
   1678                 }
   1679 
   1680                 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
   1681                         1000000000.f / wanted, mpu.gyro_fifo_rate, getTimestamp());
   1682                 tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
   1683                 res = write_attribute_sensor(tempFd, 1000000000.f / wanted);
   1684                 LOGE_IF(res < 0, "HAL:GYRO update delay error");
   1685             }
   1686 
   1687             if (A_ENABLED) { /* else if because there is only 1 fifo rate for MPUxxxx */
   1688                 if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) {
   1689                     wanted = mDelays[Gyro];
   1690                 }
   1691                 else if (GY_ENABLED && mDelays[RawGyro] < mDelays[Accelerometer]) {
   1692                     wanted = mDelays[RawGyro];
   1693 
   1694                 } else {
   1695                     wanted = mDelays[Accelerometer];
   1696                 }
   1697 
   1698                 if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
   1699                     getDmpRate(&wanted);
   1700                 }
   1701 
   1702                 /* TODO: use function pointers to calculate delay value specific to vendor */
   1703                 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
   1704                         1000000000.f / wanted, mpu.accel_fifo_rate, getTimestamp());
   1705                 tempFd = open(mpu.accel_fifo_rate, O_RDWR);
   1706                 if(USE_THIRD_PARTY_ACCEL) {
   1707                     //BMA250 in ms
   1708                     res = write_attribute_sensor(tempFd, wanted / 1000000L);
   1709                 }
   1710                 else {
   1711                     //MPUxxxx in hz
   1712                     res = write_attribute_sensor(tempFd, 1000000000.f/wanted);
   1713                 }
   1714                 LOGE_IF(res < 0, "HAL:ACCEL update delay error");
   1715             }
   1716 
   1717             /* Invensense compass calibration */
   1718             if (M_ENABLED) {
   1719                 if (!mCompassSensor->isIntegrated()) {
   1720                     wanted = mDelays[MagneticField];
   1721                 } else {
   1722                     if (GY_ENABLED && mDelays[Gyro] < mDelays[MagneticField]) {
   1723                         wanted = mDelays[Gyro];
   1724                     }
   1725                     else if (GY_ENABLED && mDelays[RawGyro] < mDelays[MagneticField]) {
   1726                         wanted = mDelays[RawGyro];
   1727                     } else if (A_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) {
   1728                         wanted = mDelays[Accelerometer];
   1729                     } else {
   1730                         wanted = mDelays[MagneticField];
   1731                     }
   1732 
   1733                     if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
   1734                         getDmpRate(&wanted);
   1735                     }
   1736                 }
   1737 
   1738                 mCompassSensor->setDelay(ID_M, wanted);
   1739                 got = mCompassSensor->getDelay(ID_M);
   1740                 inv_set_compass_sample_rate(got / 1000);
   1741             }
   1742 
   1743         }
   1744 
   1745         unsigned long sensors = mLocalSensorMask & mMasterSensorMask;
   1746         if (sensors &
   1747             (INV_THREE_AXIS_GYRO
   1748                 | INV_THREE_AXIS_ACCEL
   1749                 | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
   1750             res = masterEnable(1);
   1751         } else { // all sensors idle -> reduce power
   1752             res = onPower(0);
   1753         }
   1754     }
   1755     pthread_mutex_unlock(&GlobalHalMutex);
   1756     return res;
   1757 }
   1758 
   1759 /* For Third Party Accel Input Subsystem Drivers only */
   1760 /* TODO: FIX! data is not used and count not decremented, results is hardcoded to 0 */
   1761 int MPLSensor::readAccelEvents(sensors_event_t* data, int count)
   1762 {
   1763     VHANDLER_LOG;
   1764 
   1765     if (count < 1)
   1766         return -EINVAL;
   1767 
   1768     ssize_t n = mAccelInputReader.fill(accel_fd);
   1769     if (n < 0) {
   1770         LOGE("HAL:missed accel events, exit");
   1771         return n;
   1772     }
   1773 
   1774     int numEventReceived = 0;
   1775     input_event const* event;
   1776     int nb, done = 0;
   1777 
   1778     while (!done && count && mAccelInputReader.readEvent(&event)) {
   1779         int type = event->type;
   1780         if (type == EV_ABS) {
   1781             if (event->code == EVENT_TYPE_ACCEL_X) {
   1782                 mPendingMask |= 1 << Accelerometer;
   1783                 mCachedAccelData[0] = event->value;
   1784             } else if (event->code == EVENT_TYPE_ACCEL_Y) {
   1785                 mPendingMask |= 1 << Accelerometer;
   1786                 mCachedAccelData[1] = event->value;
   1787             } else if (event->code == EVENT_TYPE_ACCEL_Z) {
   1788                 mPendingMask |= 1 << Accelerometer;
   1789                 mCachedAccelData[2] =event-> value;
   1790             }
   1791         } else if (type == EV_SYN) {
   1792             done = 1;
   1793             if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
   1794                 inv_build_accel(mCachedAccelData, 0, getTimestamp());
   1795             }
   1796         } else {
   1797             LOGE("HAL:AccelSensor: unknown event (type=%d, code=%d)",
   1798                     type, event->code);
   1799         }
   1800         mAccelInputReader.next();
   1801     }
   1802 
   1803     return numEventReceived;
   1804 }
   1805 
   1806 /**
   1807  *  Should be called after reading at least one of gyro
   1808  *  compass or accel data. (Also okay for handling all of them).
   1809  *  @returns 0, if successful, error number if not.
   1810  */
   1811 /* TODO: This should probably be called "int readEvents(...)"
   1812  *  and readEvents() called "void cacheData(void)".
   1813  */
   1814 int MPLSensor::executeOnData(sensors_event_t* data, int count)
   1815 {
   1816     VFUNC_LOG;
   1817 
   1818     inv_execute_on_data();
   1819 
   1820     int numEventReceived = 0;
   1821 
   1822     long msg;
   1823     msg = inv_get_message_level_0(1);
   1824     if (msg) {
   1825         if (msg & INV_MSG_MOTION_EVENT) {
   1826             LOGV_IF(PROCESS_VERBOSE, "HAL:**** Motion ****\n");
   1827         }
   1828         if (msg & INV_MSG_NO_MOTION_EVENT) {
   1829             LOGV_IF(PROCESS_VERBOSE, "HAL:***** No Motion *****\n");
   1830             /* after the first no motion, the gyro should be
   1831                calibrated well */
   1832             mGyroAccuracy = SENSOR_STATUS_ACCURACY_HIGH;
   1833             /* if gyros are on and we got a no motion, set a flag
   1834                indicating that the cal file can be written. */
   1835             mHaveGoodMpuCal = true;
   1836         }
   1837     }
   1838 
   1839     // load up virtual sensors
   1840     for (int i = 0; i < numSensors; i++) {
   1841         int update;
   1842         if (mEnabled & (1 << i)) {
   1843             update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i);
   1844             mPendingMask |= (1 << i);
   1845 
   1846             if (update && (count > 0)) {
   1847                 *data++ = mPendingEvents[i];
   1848                 count--;
   1849                 numEventReceived++;
   1850             }
   1851         }
   1852     }
   1853 
   1854     return numEventReceived;
   1855 }
   1856 
   1857 // collect data for MPL (but NOT sensor service currently), from driver layer
   1858 /* TODO: FIX! data and count are not used, results is hardcoded to 0 */
   1859 /* TODO: This should probably be called "void cacheEvents(void)"
   1860  * And executeOnData() should be int readEvents(data,count)
   1861  */
   1862 int MPLSensor::readEvents(sensors_event_t *data, int count) {
   1863 
   1864 
   1865     int lp_quaternion_on = 0, nbyte;
   1866     int i, nb, mask = 0, numEventReceived = 0,
   1867         sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 : 0) +
   1868             ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 : 0) +
   1869             (((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated())? 1 : 0);
   1870     char *rdata = mIIOBuffer;
   1871 
   1872     nbyte= (8 * sensors + 8) * 1;
   1873 
   1874     if (isLowPowerQuatEnabled()) {
   1875         lp_quaternion_on = checkLPQuaternion();
   1876         if (lp_quaternion_on) {
   1877             nbyte += sizeof(mCachedQuaternionData);             //currently 16 bytes for Q data
   1878         }
   1879     }
   1880 
   1881     // pthread_mutex_lock(&mMplMutex);
   1882     // pthread_mutex_lock(&mHALMutex);
   1883 
   1884     ssize_t rsize = read(iio_fd, rdata, nbyte);
   1885     if (sensors == 0) {
   1886         // read(iio_fd, rdata, nbyte);
   1887         rsize = read(iio_fd, rdata, sizeof(mIIOBuffer));
   1888     }
   1889 
   1890 #ifdef TESTING
   1891     LOGI("get one sample of IIO data with size: %d", rsize);
   1892     LOGI("sensors: %d", sensors);
   1893 
   1894     LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_GYRO, "gyro x/y/z: %d/%d/%d",
   1895         *((short *) (rdata + 0)), *((short *) (rdata + 2)),
   1896         *((short *) (rdata + 4)));
   1897     LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_ACCEL, "accel x/y/z: %d/%d/%d",
   1898         *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))),
   1899         *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))),
   1900         *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0)));
   1901 
   1902     LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_COMPASS &
   1903         mCompassSensor->isIntegrated(), "compass x/y/z: %d/%d/%d",
   1904         *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
   1905             ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
   1906         *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
   1907             ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
   1908         *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
   1909             ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0)));
   1910 #endif
   1911 
   1912     if (rsize < (nbyte - 8)) {
   1913         LOGE("HAL:ERR Full data packet was not read. rsize=%ld nbyte=%d sensors=%d errno=%d(%s)",
   1914              rsize, nbyte, sensors, errno, strerror(errno));
   1915         return -1;
   1916     }
   1917 
   1918     if (isLowPowerQuatEnabled() && lp_quaternion_on) {
   1919 
   1920         for (i=0; i< 4; i++) {
   1921             mCachedQuaternionData[i]= *(long*)rdata;
   1922             rdata += sizeof(long);
   1923         }
   1924     }
   1925 
   1926     for (i = 0; i < 3; i++) {
   1927         if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
   1928             mCachedGyroData[i] = *((short *) (rdata + i * 2));
   1929         }
   1930         if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
   1931             mCachedAccelData[i] = *((short *) (rdata + i * 2 +
   1932                 ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0)));
   1933         }
   1934         if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated()) {
   1935             mCachedCompassData[i] = *((short *) (rdata + i * 2 + 6 * (sensors - 1)));
   1936         }
   1937     }
   1938 
   1939     mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 << Gyro: 0) +
   1940         ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 << Accelerometer: 0));
   1941     if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated() &&
   1942             (mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0 || mCachedCompassData[0] != 0)) {
   1943         mask |= 1 << MagneticField;
   1944     }
   1945 
   1946     mSensorTimestamp = *((long long *) (rdata + 8 * sensors));
   1947     if (mCompassSensor->isIntegrated()) {
   1948         mCompassTimestamp = mSensorTimestamp;
   1949     }
   1950 
   1951     if (mask & (1 << Gyro)) {
   1952         // send down temperature every 0.5 seconds
   1953         // with timestamp measured in "driver" layer
   1954         if(mSensorTimestamp - mTempCurrentTime >= 500000000LL) {
   1955             mTempCurrentTime = mSensorTimestamp;
   1956             long long temperature[2];
   1957             if(inv_read_temperature(temperature) == 0) {
   1958                 LOGV_IF(INPUT_DATA,
   1959                         "HAL:inv_read_temperature = %lld, timestamp= %lld",
   1960                         temperature[0], temperature[1]);
   1961                 inv_build_temp(temperature[0], temperature[1]);
   1962             }
   1963 #ifdef TESTING
   1964             long bias[3], temp, temp_slope[3];
   1965             inv_get_gyro_bias(bias, &temp);
   1966             inv_get_gyro_ts(temp_slope);
   1967 
   1968             LOGI("T: %.3f "
   1969                  "GB: %+13f %+13f %+13f "
   1970                  "TS: %+13f %+13f %+13f "
   1971                  "\n",
   1972                  (float)temperature[0] / 65536.f,
   1973                  (float)bias[0] / 65536.f / 16.384f,
   1974                  (float)bias[1] / 65536.f / 16.384f,
   1975                  (float)bias[2] / 65536.f / 16.384f,
   1976                  temp_slope[0] / 65536.f,
   1977                  temp_slope[1] / 65536.f,
   1978                  temp_slope[2] / 65536.f);
   1979 #endif
   1980         }
   1981 
   1982         mPendingMask |= 1 << Gyro;
   1983         mPendingMask |= 1 << RawGyro;
   1984 
   1985         if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
   1986             inv_build_gyro(mCachedGyroData, mSensorTimestamp);
   1987             LOGV_IF(INPUT_DATA,
   1988                     "HAL:inv_build_gyro: %+8d %+8d %+8d - %lld",
   1989                     mCachedGyroData[0], mCachedGyroData[1],
   1990                     mCachedGyroData[2], mSensorTimestamp);
   1991         }
   1992     }
   1993 
   1994     if (mask & (1 << Accelerometer)) {
   1995         mPendingMask |= 1 << Accelerometer;
   1996         if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
   1997             inv_build_accel(mCachedAccelData, 0, mSensorTimestamp);
   1998              LOGV_IF(INPUT_DATA,
   1999                     "HAL:inv_build_accel: %+8ld %+8ld %+8ld - %lld",
   2000                     mCachedAccelData[0], mCachedAccelData[1],
   2001                     mCachedAccelData[2], mSensorTimestamp);
   2002         }
   2003     }
   2004 
   2005     if ((mask & (1 << MagneticField)) && mCompassSensor->isIntegrated()) {
   2006         int status = 0;
   2007         if (mCompassSensor->providesCalibration()) {
   2008             status = mCompassSensor->getAccuracy();
   2009             status |= INV_CALIBRATED;
   2010         }
   2011         if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
   2012             inv_build_compass(mCachedCompassData, status,
   2013                               mCompassTimestamp);
   2014             LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
   2015                     mCachedCompassData[0], mCachedCompassData[1],
   2016                     mCachedCompassData[2], mCompassTimestamp);
   2017         }
   2018     }
   2019 
   2020     if (isLowPowerQuatEnabled() && lp_quaternion_on) {
   2021 
   2022         inv_build_quat(mCachedQuaternionData, 32 /*default 32 for now (16/32bits)*/, mSensorTimestamp);
   2023         LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld",
   2024                     mCachedQuaternionData[0], mCachedQuaternionData[1],
   2025                     mCachedQuaternionData[2], mCachedQuaternionData[3], mSensorTimestamp);
   2026     }
   2027 
   2028     // pthread_mutex_unlock(&mMplMutex);
   2029     // pthread_mutex_unlock(&mHALMutex);
   2030 
   2031     return numEventReceived;
   2032 }
   2033 
   2034 /* use for both MPUxxxx and third party compass */
   2035 int MPLSensor::readCompassEvents(sensors_event_t *data, int count)
   2036 {
   2037     VHANDLER_LOG;
   2038 
   2039     if (count < 1)
   2040         return -EINVAL;
   2041 
   2042     int numEventReceived = 0;
   2043     int done = 0;
   2044     int nb;
   2045 
   2046     // pthread_mutex_lock(&mMplMutex);
   2047     // pthread_mutex_lock(&mHALMutex);
   2048 
   2049     done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp);
   2050 #ifdef COMPASS_YAS53x
   2051     if (mCompassSensor->checkCoilsReset()) {
   2052        //Reset relevant compass settings
   2053        resetCompass();
   2054     }
   2055 #endif
   2056     if (done > 0) {
   2057         int status = 0;
   2058         if (mCompassSensor->providesCalibration()) {
   2059             status = mCompassSensor->getAccuracy();
   2060             status |= INV_CALIBRATED;
   2061         }
   2062         if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
   2063             inv_build_compass(mCachedCompassData, status,
   2064                               mCompassTimestamp);
   2065             LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
   2066                     mCachedCompassData[0], mCachedCompassData[1],
   2067                     mCachedCompassData[2], mCompassTimestamp);
   2068         }
   2069     }
   2070 
   2071     // pthread_mutex_unlock(&mMplMutex);
   2072     // pthread_mutex_unlock(&mHALMutex);
   2073 
   2074     return numEventReceived;
   2075 }
   2076 
   2077 #ifdef COMPASS_YAS53x
   2078 int MPLSensor::resetCompass()
   2079 {
   2080     VFUNC_LOG;
   2081 
   2082     //Reset compass cal if enabled
   2083     if (mFeatureActiveMask & INV_COMPASS_CAL) {
   2084        LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal");
   2085        inv_init_vector_compass_cal();
   2086     }
   2087 
   2088     //Reset compass fit if enabled
   2089     if (mFeatureActiveMask & INV_COMPASS_FIT) {
   2090        LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit");
   2091        inv_init_compass_fit();
   2092     }
   2093 
   2094     return 0;
   2095 }
   2096 #endif
   2097 
   2098 int MPLSensor::getFd() const
   2099 {
   2100     VFUNC_LOG;
   2101     LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getFd returning %d", iio_fd);
   2102     return iio_fd;
   2103 }
   2104 
   2105 int MPLSensor::getAccelFd() const
   2106 {
   2107     VFUNC_LOG;
   2108     LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getAccelFd returning %d", accel_fd);
   2109     return accel_fd;
   2110 }
   2111 
   2112 int MPLSensor::getCompassFd() const
   2113 {
   2114     VFUNC_LOG;
   2115     int fd = mCompassSensor->getFd();
   2116     LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getCompassFd returning %d", fd);
   2117     return fd;
   2118 }
   2119 
   2120 int MPLSensor::turnOffAccelFifo() {
   2121     int i, res, tempFd;
   2122     char *accel_fifo_enable[3] = {mpu.accel_x_fifo_enable,
   2123         mpu.accel_y_fifo_enable, mpu.accel_z_fifo_enable};
   2124 
   2125     for (i = 0; i < 3; i++) {
   2126         res = write_sysfs_int(accel_fifo_enable[i], 0);
   2127         if (res < 0) {
   2128             return res;
   2129         }
   2130     }
   2131     return 0;
   2132 }
   2133 
   2134 int MPLSensor::enableDmpOrientation(int en)
   2135 {
   2136     VFUNC_LOG;
   2137     /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
   2138     int res = 0;
   2139     int enabled_sensors = mEnabled;
   2140 
   2141     if (isMpu3050())
   2142         return res;
   2143 
   2144     pthread_mutex_lock(&GlobalHalMutex);
   2145 
   2146     // on power if not already On
   2147     res = onPower(1);
   2148     // reset master enable
   2149     res = masterEnable(0);
   2150 
   2151     if (en) {
   2152         //Enable DMP orientation
   2153         if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
   2154             LOGE("HAL:ERR can't enable Android orientation");
   2155             res = -1;	// indicate an err
   2156         }
   2157 
   2158         // open DMP Orient Fd
   2159         res = openDmpOrientFd();
   2160 
   2161         // enable DMP
   2162         res = onDMP(1);
   2163 
   2164         // default DMP output rate to FIFO
   2165         if (write_sysfs_int(mpu.dmp_output_rate, 5) < 0) {
   2166             LOGE("HAL:ERR can't default DMP output rate");
   2167         }
   2168 
   2169         // set DMP rate to 200Hz
   2170         if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) {
   2171             res = -1;
   2172             LOGE("HAL:ERR can't set DMP rate to 200Hz");
   2173         }
   2174 
   2175         // enable accel engine
   2176         res = enableAccel(1);
   2177 
   2178         // disable accel FIFO
   2179         if (!A_ENABLED) {
   2180             res = turnOffAccelFifo();
   2181         }
   2182 
   2183         mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION;
   2184     } else {
   2185         // disable DMP
   2186         res = onDMP(0);
   2187 
   2188         // disable accel engine
   2189         if (!A_ENABLED) {
   2190             res = enableAccel(0);
   2191         }
   2192     }
   2193 
   2194     res = masterEnable(1);
   2195     pthread_mutex_unlock(&GlobalHalMutex);
   2196     return res;
   2197 }
   2198 
   2199 int MPLSensor::openDmpOrientFd()
   2200 {
   2201     VFUNC_LOG;
   2202 
   2203     if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) {
   2204         LOGV_IF(PROCESS_VERBOSE, "HAL:DMP display orientation disabled or file desc opened");
   2205         return -1;
   2206     }
   2207 
   2208     dmp_orient_fd = open(mpu.event_display_orientation, O_RDONLY| O_NONBLOCK);
   2209     if (dmp_orient_fd < 0) {
   2210         LOGE("HAL:ERR couldn't open dmpOrient node");
   2211         return -1;
   2212     } else {
   2213         LOGV_IF(PROCESS_VERBOSE, "HAL:dmp_orient_fd opened : %d", dmp_orient_fd);
   2214         return 0;
   2215     }
   2216 }
   2217 
   2218 int MPLSensor::closeDmpOrientFd()
   2219 {
   2220     VFUNC_LOG;
   2221     if (dmp_orient_fd >= 0)
   2222         close(dmp_orient_fd);
   2223     return 0;
   2224 }
   2225 
   2226 int MPLSensor::dmpOrientHandler(int orient)
   2227 {
   2228     VFUNC_LOG;
   2229 
   2230     LOGV_IF(PROCESS_VERBOSE, "HAL:orient %x", orient);
   2231     return 0;
   2232 }
   2233 
   2234 int MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count) {
   2235     VFUNC_LOG;
   2236 
   2237     char dummy[4];
   2238     int screen_orientation = 0;
   2239     FILE *fp;
   2240 
   2241     fp = fopen(mpu.event_display_orientation, "r");
   2242     if (fp == NULL) {
   2243         LOGE("HAL:cannot open event_display_orientation");
   2244         return 0;
   2245     }
   2246     fscanf(fp, "%d\n", &screen_orientation);
   2247     fclose(fp);
   2248 
   2249     int numEventReceived = 0;
   2250 
   2251     if (mDmpOrientationEnabled && count > 0) {
   2252         sensors_event_t temp;
   2253 
   2254         bzero(&temp, sizeof(temp));  /* Let's hope that SENSOR_TYPE_NONE is 0 */
   2255         temp.version = sizeof(sensors_event_t);
   2256         temp.sensor = ID_SO;
   2257 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
   2258         temp.type = SENSOR_TYPE_SCREEN_ORIENTATION;
   2259         temp.screen_orientation = screen_orientation;
   2260 #endif
   2261         struct timespec ts;
   2262         clock_gettime(CLOCK_MONOTONIC, &ts);
   2263         temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
   2264 
   2265         *data++ = temp;
   2266         count--;
   2267         numEventReceived++;
   2268     }
   2269 
   2270     // read dummy data per driver's request
   2271     dmpOrientHandler(screen_orientation);
   2272     read(dmp_orient_fd, dummy, 4);
   2273 
   2274     return numEventReceived;
   2275 }
   2276 
   2277 int MPLSensor::getDmpOrientFd()
   2278 {
   2279     VFUNC_LOG;
   2280 
   2281     LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getDmpOrientFd returning %d", dmp_orient_fd);
   2282     return dmp_orient_fd;
   2283 
   2284 }
   2285 
   2286 int MPLSensor::checkDMPOrientation()
   2287 {
   2288     VFUNC_LOG;
   2289     return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0);
   2290 }
   2291 
   2292 int MPLSensor::getDmpRate(int64_t *wanted)
   2293 {
   2294     if (checkDMPOrientation() || checkLPQuaternion()) {
   2295         // set DMP output rate to FIFO
   2296         write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / *wanted);
   2297         LOGV_IF(PROCESS_VERBOSE, "HAL:DMP FIFO rate %.2f Hz", 1000000000.f / *wanted);
   2298 
   2299         //DMP running rate must be @ 200Hz
   2300         *wanted= RATE_200HZ;
   2301         LOGV_IF(PROCESS_VERBOSE, "HAL:DMP rate= %.2f Hz", 1000000000.f / *wanted);
   2302     }
   2303     return 0;
   2304 }
   2305 
   2306 int MPLSensor::getPollTime()
   2307 {
   2308     VHANDLER_LOG;
   2309     return mPollTime;
   2310 }
   2311 
   2312 bool MPLSensor::hasPendingEvents() const
   2313 {
   2314     VHANDLER_LOG;
   2315     // if we are using the polling workaround, force the main
   2316     // loop to check for data every time
   2317     return (mPollTime != -1);
   2318 }
   2319 
   2320 /* TODO: support resume suspend when we gain more info about them*/
   2321 void MPLSensor::sleepEvent()
   2322 {
   2323     VFUNC_LOG;
   2324 }
   2325 
   2326 void MPLSensor::wakeEvent()
   2327 {
   2328     VFUNC_LOG;
   2329 }
   2330 
   2331 int MPLSensor::inv_float_to_q16(float *fdata, long *ldata)
   2332 {
   2333     VHANDLER_LOG;
   2334 
   2335     if (!fdata || !ldata)
   2336         return -1;
   2337     ldata[0] = (long)(fdata[0] * 65536.f);
   2338     ldata[1] = (long)(fdata[1] * 65536.f);
   2339     ldata[2] = (long)(fdata[2] * 65536.f);
   2340     return 0;
   2341 }
   2342 
   2343 int MPLSensor::inv_long_to_q16(long *fdata, long *ldata)
   2344 {
   2345     VHANDLER_LOG;
   2346 
   2347     if (!fdata || !ldata)
   2348         return -1;
   2349     ldata[0] = (fdata[1] * 65536.f);
   2350     ldata[1] = (fdata[2] * 65536.f);
   2351     ldata[2] = (fdata[3] * 65536.f);
   2352     return 0;
   2353 }
   2354 
   2355 int MPLSensor::inv_float_to_round(float *fdata, long *ldata)
   2356 {
   2357     VHANDLER_LOG;
   2358 
   2359     if (!fdata || !ldata)
   2360             return -1;
   2361     ldata[0] = (long)fdata[0];
   2362     ldata[1] = (long)fdata[1];
   2363     ldata[2] = (long)fdata[2];
   2364     return 0;
   2365 }
   2366 
   2367 int MPLSensor::inv_float_to_round2(float *fdata, short *ldata)
   2368 {
   2369     VHANDLER_LOG;
   2370 
   2371     if (!fdata || !ldata)
   2372         return -1;
   2373     ldata[0] = (short)fdata[0];
   2374     ldata[1] = (short)fdata[1];
   2375     ldata[2] = (short)fdata[2];
   2376     return 0;
   2377 }
   2378 
   2379 int MPLSensor::inv_read_temperature(long long *data)
   2380 {
   2381     VHANDLER_LOG;
   2382 
   2383     int count = 0;
   2384     char raw_buf[40];
   2385     long raw = 0;
   2386 
   2387     long long timestamp = 0;
   2388 
   2389     memset(raw_buf, 0, sizeof(raw_buf));
   2390     count = read_attribute_sensor(gyro_temperature_fd, raw_buf,
   2391                                   sizeof(raw_buf));
   2392     if(count < 1) {
   2393         LOGE("HAL:error reading gyro temperature");
   2394         return -1;
   2395     }
   2396 
   2397     count = sscanf(raw_buf, "%ld%lld", &raw, &timestamp);
   2398 
   2399     if(count < 0) {
   2400         return -1;
   2401     }
   2402 
   2403     LOGV_IF(ENG_VERBOSE,
   2404             "HAL:temperature raw = %ld, timestamp = %lld, count = %d",
   2405             raw, timestamp, count);
   2406     data[0] = raw;
   2407     data[1] = timestamp;
   2408 
   2409     return 0;
   2410 }
   2411 
   2412 int MPLSensor::inv_read_dmp_state(int fd)
   2413 {
   2414     VFUNC_LOG;
   2415 
   2416     if(fd < 0)
   2417         return -1;
   2418 
   2419     int count = 0;
   2420     char raw_buf[10];
   2421     short raw = 0;
   2422 
   2423     memset(raw_buf, 0, sizeof(raw_buf));
   2424     count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf));
   2425     if(count < 1) {
   2426         LOGE("HAL:error reading dmp state");
   2427         close(fd);
   2428         return -1;
   2429     }
   2430     count = sscanf(raw_buf, "%hd", &raw);
   2431     if(count < 0) {
   2432         LOGE("HAL:dmp state data is invalid");
   2433         close(fd);
   2434         return -1;
   2435     }
   2436     LOGV_IF(EXTRA_VERBOSE, "HAL:dmp state = %d, count = %d", raw, count);
   2437     close(fd);
   2438     return (int)raw;
   2439 }
   2440 
   2441 int MPLSensor::inv_read_sensor_bias(int fd, long *data)
   2442 {
   2443     VFUNC_LOG;
   2444 
   2445     if(fd == -1) {
   2446         return -1;
   2447     }
   2448 
   2449     char buf[50];
   2450     char x[15], y[15], z[15];
   2451 
   2452     memset(buf, 0, sizeof(buf));
   2453     int count = read_attribute_sensor(fd, buf, sizeof(buf));
   2454     if(count < 1) {
   2455         LOGE("HAL:Error reading gyro bias");
   2456         return -1;
   2457     }
   2458     count = sscanf(buf, "%[^','],%[^','],%[^',']", x, y, z);
   2459     if(count) {
   2460         /* scale appropriately for MPL */
   2461         LOGV_IF(ENG_VERBOSE,
   2462                 "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)",
   2463                 atol(x), atol(y), atol(z));
   2464 
   2465         data[0] = (long)(atol(x) / 10000 * (1L << 16));
   2466         data[1] = (long)(atol(y) / 10000 * (1L << 16));
   2467         data[2] = (long)(atol(z) / 10000 * (1L << 16));
   2468 
   2469         LOGV_IF(ENG_VERBOSE,
   2470                 "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)",
   2471                 data[0], data[1], data[2]);
   2472     }
   2473     return 0;
   2474 }
   2475 
   2476 /** fill in the sensor list based on which sensors are configured.
   2477  *  return the number of configured sensors.
   2478  *  parameter list must point to a memory region of at least 7*sizeof(sensor_t)
   2479  *  parameter len gives the length of the buffer pointed to by list
   2480  */
   2481 int MPLSensor::populateSensorList(struct sensor_t *list, int len)
   2482 {
   2483     VFUNC_LOG;
   2484 
   2485     int numsensors;
   2486 
   2487     if(len < (int)((sizeof(sSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) {
   2488         LOGE("HAL:sensor list too small, not populating.");
   2489         return -(sizeof(sSensorList) / sizeof(sensor_t));
   2490     }
   2491 
   2492     /* fill in the base values */
   2493     memcpy(list, sSensorList, sizeof (struct sensor_t) * (sizeof(sSensorList) / sizeof(sensor_t)));
   2494 
   2495     /* first add gyro, accel and compass to the list */
   2496 
   2497     /* fill in gyro/accel values */
   2498     if(chip_ID == NULL) {
   2499         LOGE("HAL:Can not get gyro/accel id");
   2500     }
   2501     fillGyro(chip_ID, list);
   2502     fillAccel(chip_ID, list);
   2503 
   2504     // TODO: need fixes for unified HAL and 3rd-party solution
   2505     mCompassSensor->fillList(&list[MagneticField]);
   2506 
   2507     if(1) {
   2508         numsensors = (sizeof(sSensorList) / sizeof(sensor_t));
   2509         /* all sensors will be added to the list
   2510            fill in orientation values */
   2511         fillOrientation(list);
   2512         /* fill in rotation vector values */
   2513         fillRV(list);
   2514         /* fill in gravity values */
   2515         fillGravity(list);
   2516         /* fill in Linear accel values */
   2517         fillLinearAccel(list);
   2518     } else {
   2519         /* no 9-axis sensors, zero fill that part of the list */
   2520         numsensors = 3;
   2521         memset(list + 3, 0, 4 * sizeof(struct sensor_t));
   2522     }
   2523 
   2524     return numsensors;
   2525 }
   2526 
   2527 void MPLSensor::fillAccel(const char* accel, struct sensor_t *list)
   2528 {
   2529     VFUNC_LOG;
   2530 
   2531     if (accel) {
   2532         if(accel != NULL && strcmp(accel, "BMA250") == 0) {
   2533             list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
   2534             list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
   2535             list[Accelerometer].power = ACCEL_BMA250_POWER;
   2536             list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
   2537             return;
   2538         } else if (accel != NULL && strcmp(accel, "MPU6050") == 0) {
   2539             list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
   2540             list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
   2541             list[Accelerometer].power = ACCEL_MPU6050_POWER;
   2542             list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY;
   2543             return;
   2544         } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
   2545             list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
   2546             list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
   2547             list[Accelerometer].power = ACCEL_MPU6500_POWER;
   2548             list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
   2549 
   2550             return;
   2551         } else if (accel != NULL && strcmp(accel, "MPU9150") == 0) {
   2552             list[Accelerometer].maxRange = ACCEL_MPU9150_RANGE;
   2553             list[Accelerometer].resolution = ACCEL_MPU9150_RESOLUTION;
   2554             list[Accelerometer].power = ACCEL_MPU9150_POWER;
   2555             list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY;
   2556             return;
   2557         } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) {
   2558             list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
   2559             list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
   2560             list[Accelerometer].power = ACCEL_BMA250_POWER;
   2561             list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
   2562             return;
   2563         }
   2564     }
   2565 
   2566     LOGE("HAL:unknown accel id %s -- "
   2567          "params default to bma250 and might be wrong.",
   2568          accel);
   2569     list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
   2570     list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
   2571     list[Accelerometer].power = ACCEL_BMA250_POWER;
   2572     list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
   2573 }
   2574 
   2575 void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
   2576 {
   2577     VFUNC_LOG;
   2578 
   2579     if ( gyro != NULL && strcmp(gyro, "MPU3050") == 0) {
   2580         list[Gyro].maxRange = GYRO_MPU3050_RANGE;
   2581         list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
   2582         list[Gyro].power = GYRO_MPU3050_POWER;
   2583         list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
   2584     } else if( gyro != NULL && strcmp(gyro, "MPU6050") == 0) {
   2585         list[Gyro].maxRange = GYRO_MPU6050_RANGE;
   2586         list[Gyro].resolution = GYRO_MPU6050_RESOLUTION;
   2587         list[Gyro].power = GYRO_MPU6050_POWER;
   2588         list[Gyro].minDelay = GYRO_MPU6050_MINDELAY;
   2589     } else if( gyro != NULL && strcmp(gyro, "MPU6500") == 0) {
   2590         list[Gyro].maxRange = GYRO_MPU6500_RANGE;
   2591         list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
   2592         list[Gyro].power = GYRO_MPU6500_POWER;
   2593         list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
   2594     } else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) {
   2595         list[Gyro].maxRange = GYRO_MPU9150_RANGE;
   2596         list[Gyro].resolution = GYRO_MPU9150_RESOLUTION;
   2597         list[Gyro].power = GYRO_MPU9150_POWER;
   2598         list[Gyro].minDelay = GYRO_MPU9150_MINDELAY;
   2599     } else {
   2600         LOGE("HAL:unknown gyro id -- gyro params will be wrong.");
   2601         LOGE("HAL:default to use mpu3050 params");
   2602         list[Gyro].maxRange = GYRO_MPU3050_RANGE;
   2603         list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
   2604         list[Gyro].power = GYRO_MPU3050_POWER;
   2605         list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
   2606     }
   2607 
   2608     list[RawGyro].maxRange = list[Gyro].maxRange;
   2609     list[RawGyro].resolution = list[Gyro].resolution;
   2610     list[RawGyro].power = list[Gyro].power;
   2611     list[RawGyro].minDelay = list[Gyro].minDelay;
   2612 
   2613     return;
   2614 }
   2615 
   2616 /* fillRV depends on values of accel and compass in the list */
   2617 void MPLSensor::fillRV(struct sensor_t *list)
   2618 {
   2619     VFUNC_LOG;
   2620 
   2621     /* compute power on the fly */
   2622     list[RotationVector].power = list[Gyro].power +
   2623                                  list[Accelerometer].power +
   2624                                  list[MagneticField].power;
   2625     list[RotationVector].resolution = .00001;
   2626     list[RotationVector].maxRange = 1.0;
   2627     list[RotationVector].minDelay = 5000;
   2628 
   2629     return;
   2630 }
   2631 
   2632 void MPLSensor::fillOrientation(struct sensor_t *list)
   2633 {
   2634     VFUNC_LOG;
   2635 
   2636     list[Orientation].power = list[Gyro].power +
   2637                               list[Accelerometer].power +
   2638                               list[MagneticField].power;
   2639     list[Orientation].resolution = .00001;
   2640     list[Orientation].maxRange = 360.0;
   2641     list[Orientation].minDelay = 5000;
   2642 
   2643     return;
   2644 }
   2645 
   2646 void MPLSensor::fillGravity( struct sensor_t *list)
   2647 {
   2648     VFUNC_LOG;
   2649 
   2650     list[Gravity].power = list[Gyro].power +
   2651                           list[Accelerometer].power +
   2652                           list[MagneticField].power;
   2653     list[Gravity].resolution = .00001;
   2654     list[Gravity].maxRange = 9.81;
   2655     list[Gravity].minDelay = 5000;
   2656 
   2657     return;
   2658 }
   2659 
   2660 void MPLSensor::fillLinearAccel(struct sensor_t *list)
   2661 {
   2662     VFUNC_LOG;
   2663 
   2664     list[LinearAccel].power = list[Gyro].power +
   2665                           list[Accelerometer].power +
   2666                           list[MagneticField].power;
   2667     list[LinearAccel].resolution = list[Accelerometer].resolution;
   2668     list[LinearAccel].maxRange = list[Accelerometer].maxRange;
   2669     list[LinearAccel].minDelay = 5000;
   2670 
   2671     return;
   2672 }
   2673 
   2674 int MPLSensor::inv_init_sysfs_attributes(void)
   2675 {
   2676     VFUNC_LOG;
   2677 
   2678     unsigned char i;
   2679     char sysfs_path[MAX_SYSFS_NAME_LEN], iio_trigger_path[MAX_SYSFS_NAME_LEN];
   2680     char *sptr;
   2681     char **dptr;
   2682     int num;
   2683 
   2684     sysfs_names_ptr =
   2685             (char*)calloc(1, sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
   2686     sptr = sysfs_names_ptr;
   2687     if (sptr == NULL) {
   2688         LOGE("HAL:couldn't alloc mem for sysfs paths");
   2689         return -1;
   2690     }
   2691 
   2692     dptr = (char**)&mpu;
   2693     i = 0;
   2694     do {
   2695         *dptr++ = sptr;
   2696         sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
   2697     } while (++i < MAX_SYSFS_ATTRB);
   2698 
   2699     // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
   2700     // inv_get_sysfs_abs_path(sysfs_path);
   2701     if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) {
   2702         ALOGE("MPLSensor failed get sysfs path");
   2703         return -1;
   2704     }
   2705 
   2706     if(INV_SUCCESS != inv_get_iio_trigger_path(iio_trigger_path)) {
   2707         ALOGE("MPLSensor failed get iio trigger path");
   2708         return -1;
   2709     }
   2710 
   2711     sprintf(mpu.key, "%s%s", sysfs_path, "/key");
   2712     sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable");
   2713     sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length");
   2714     sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
   2715     sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path, "/scan_elements/in_timestamp_en");
   2716     sprintf(mpu.trigger_name, "%s%s", iio_trigger_path, "/name");
   2717     sprintf(mpu.current_trigger, "%s%s", sysfs_path, "/trigger/current_trigger");
   2718 
   2719     sprintf(mpu.dmp_firmware, "%s%s", sysfs_path,"/dmp_firmware");
   2720     sprintf(mpu.firmware_loaded,"%s%s", sysfs_path, "/firmware_loaded");
   2721     sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
   2722     sprintf(mpu.dmp_int_on,"%s%s", sysfs_path, "/dmp_int_on");
   2723     sprintf(mpu.dmp_event_int_on,"%s%s", sysfs_path, "/dmp_event_int_on");
   2724     sprintf(mpu.dmp_output_rate,"%s%s", sysfs_path, "/dmp_output_rate");
   2725     sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
   2726 
   2727     // TODO: for self test
   2728     sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
   2729 
   2730     sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature");
   2731     sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable");
   2732     sprintf(mpu.gyro_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
   2733     sprintf(mpu.gyro_orient, "%s%s", sysfs_path, "/gyro_matrix");
   2734     sprintf(mpu.gyro_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_x_en");
   2735     sprintf(mpu.gyro_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_y_en");
   2736     sprintf(mpu.gyro_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_z_en");
   2737 
   2738     sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable");
   2739     sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
   2740     sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix");
   2741 
   2742 
   2743 #ifndef THIRD_PARTY_ACCEL //MPUxxxx
   2744     sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale");
   2745     // TODO: for bias settings
   2746     sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/accl_bias");
   2747 #endif
   2748 
   2749     sprintf(mpu.accel_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_x_en");
   2750     sprintf(mpu.accel_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_y_en");
   2751     sprintf(mpu.accel_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_z_en");
   2752 
   2753     sprintf(mpu.quaternion_on, "%s%s", sysfs_path, "/quaternion_on");
   2754     sprintf(mpu.in_quat_r_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_r_en");
   2755     sprintf(mpu.in_quat_x_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_x_en");
   2756     sprintf(mpu.in_quat_y_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_y_en");
   2757     sprintf(mpu.in_quat_z_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_z_en");
   2758 
   2759     sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on");
   2760     sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation");
   2761 
   2762 #if SYSFS_VERBOSE
   2763     // test print sysfs paths
   2764     dptr = (char**)&mpu;
   2765     for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
   2766         LOGE("HAL:sysfs path: %s", *dptr++);
   2767     }
   2768 #endif
   2769     return 0;
   2770 }
   2771 
   2772 /* TODO: stop manually testing/using 0 and 1 instead of
   2773  * false and true, but just use 0 and non-0.
   2774  * This allows  passing 0 and non-0 ints around instead of
   2775  * having to convert to 1 and test against 1.
   2776  */
   2777 bool MPLSensor::isMpu3050()
   2778 {
   2779     return !strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050");
   2780 }
   2781 
   2782 int MPLSensor::isLowPowerQuatEnabled()
   2783 {
   2784 #ifdef ENABLE_LP_QUAT_FEAT
   2785     return !isMpu3050();
   2786 #else
   2787     return 0;
   2788 #endif
   2789 }
   2790 
   2791 int MPLSensor::isDmpDisplayOrientationOn()
   2792 {
   2793 #ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
   2794     return !isMpu3050();
   2795 #else
   2796     return 0;
   2797 #endif
   2798 }
   2799