Motion Apps 5.2.0 HAL Release

Add latest 5.2.0 version of InvenSense HAL for the mpu9250 into it's own
unique folder named 6515 so that Hammerhead still uses it's original 65xx HAL.

Change-Id: I90f1bbbc1de2283a8d337495b6af59261cd63f88
Signed-off-by: Nick Vaccaro <nvaccaro@google.com>

119 files changed, 29507 insertions, 0 deletions

diff --git a/6515/libsensors_iio/Android.mk b/6515/libsensors_iio/Android.mk
new file mode 100644
index 0000000..f011eff
--- /dev/null
+++ b/6515/libsensors_iio/Android.mk
@@ -0,0 +1,192 @@
+# Copyright (C) 2008 The Android Open Source Project
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+# Modified 2011 by InvenSense, Inc
+
+LOCAL_PATH := $(call my-dir)
+
+#ifneq ($(TARGET_SIMULATOR),true)
+
+# InvenSense fragment of the HAL
+include $(CLEAR_VARS)
+
+LOCAL_MODULE := libinvensense_hal
+LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := invensense
+
+LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\"
+
+# ANDROID version check
+MAJOR_VERSION :=$(shell echo $(PLATFORM_VERSION) | cut -f1 -d.)
+MINOR_VERSION :=$(shell echo $(PLATFORM_VERSION) | cut -f2 -d.)
+VERSION_KK :=$(shell test $(MAJOR_VERSION) -gt 4 -o $(MAJOR_VERSION) -eq 4 -a $(MINOR_VERSION) -gt 3 && echo true)
+#$(info MAJOR_VERSION=$(MAJOR_VERSION))
+#$(info MINOR_VERSION=$(MINOR_VERSION))
+#ANDROID version check END
+
+ifeq ($(VERSION_KK),true)
+LOCAL_CFLAGS += -DANDROID_KITKAT
+else
+LOCAL_CFLAGS += -DANDROID_JELLYBEAN
+endif
+
+ifneq (,$(filter $(TARGET_BUILD_VARIANT),eng userdebug))
+ifneq ($(COMPILE_INVENSENSE_COMPASS_CAL),0)
+LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL
+endif
+ifeq ($(COMPILE_THIRD_PARTY_ACCEL),1)
+LOCAL_CFLAGS += -DTHIRD_PARTY_ACCEL
+endif
+else # release builds, default
+LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL
+endif
+
+LOCAL_SRC_FILES += SensorBase.cpp
+LOCAL_SRC_FILES += MPLSensor.cpp
+LOCAL_SRC_FILES += MPLSupport.cpp
+LOCAL_SRC_FILES += InputEventReader.cpp
+LOCAL_SRC_FILES += PressureSensor.IIO.secondary.cpp
+
+ifneq (,$(filter $(TARGET_BUILD_VARIANT),eng userdebug))
+ifeq ($(COMPILE_INVENSENSE_COMPASS_CAL),0)
+LOCAL_SRC_FILES += AkmSensor.cpp
+LOCAL_SRC_FILES += CompassSensor.AKM.cpp
+else ifeq ($(COMPILE_INVENSENSE_SENSOR_ON_PRIMARY_BUS), 1)
+LOCAL_SRC_FILES += CompassSensor.IIO.primary.cpp
+LOCAL_CFLAGS += -DSENSOR_ON_PRIMARY_BUS
+else
+LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp
+endif
+else # release builds, default
+LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp
+endif #userdebug
+
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite/linux
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include/linux
+
+LOCAL_SHARED_LIBRARIES := liblog
+LOCAL_SHARED_LIBRARIES += libcutils
+LOCAL_SHARED_LIBRARIES += libutils
+LOCAL_SHARED_LIBRARIES += libdl
+LOCAL_SHARED_LIBRARIES += libmllite
+
+# Additions for SysPed
+LOCAL_SHARED_LIBRARIES += libmplmpu
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mpl
+LOCAL_CPPFLAGS += -DLINUX=1
+LOCAL_PRELINK_MODULE := false
+
+LOCAL_SHARED_LIBRARIES += libmllite
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite
+LOCAL_CPPFLAGS += -DLINUX=1
+LOCAL_PRELINK_MODULE := false
+
+include $(BUILD_SHARED_LIBRARY)
+
+#endif # !TARGET_SIMULATOR
+
+# Build a temporary HAL that links the InvenSense .so
+include $(CLEAR_VARS)
+ifeq ($(filter eng, userdebug, $(TARGET_BUILD_VARIANT)),)
+ifneq ($(filter manta full_grouper tilapia, $(TARGET_PRODUCT)),)
+LOCAL_MODULE := sensors.full_grouper
+else
+ifneq ($(filter aosp_hammerhead, $(TARGET_PRODUCT)),)
+LOCAL_MODULE := sensors.hammerhead
+endif
+ifneq ($(filter platina dory guppy, $(TARGET_PRODUCT)),)
+LOCAL_MODULE := sensors.invensense
+endif
+endif
+else    # eng & userdebug builds
+LOCAL_MODULE := sensors.invensense
+endif   # eng & userdebug builds
+LOCAL_MODULE_PATH := $(TARGET_OUT_SHARED_LIBRARIES)/hw
+
+LOCAL_SHARED_LIBRARIES += libmplmpu
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite/linux
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mpl
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include/linux
+
+LOCAL_PRELINK_MODULE := false
+LOCAL_MODULE_TAGS := optional
+LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\"
+
+ifeq ($(VERSION_KK),true)
+LOCAL_CFLAGS += -DANDROID_KITKAT
+else
+LOCAL_CFLAGS += -DANDROID_JELLYBEAN
+endif
+
+ifneq (,$(filter $(TARGET_BUILD_VARIANT),eng userdebug))
+ifneq ($(COMPILE_INVENSENSE_COMPASS_CAL),0)
+LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL
+endif
+ifeq ($(COMPILE_THIRD_PARTY_ACCEL),1)
+LOCAL_CFLAGS += -DTHIRD_PARTY_ACCEL
+endif
+ifeq ($(COMPILE_INVENSENSE_SENSOR_ON_PRIMARY_BUS), 1)
+LOCAL_SRC_FILES += CompassSensor.IIO.primary.cpp
+LOCAL_CFLAGS += -DSENSOR_ON_PRIMARY_BUS
+else
+LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp
+endif
+else # release builds, default
+LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp
+endif # userdebug
+
+ifeq (,$(filter $(TARGET_BUILD_VARIANT),eng userdebug))
+ifneq ($(filter manta grouper tilapia, $(TARGET_DEVICE)),)
+# it's already defined in some other Makefile for production builds
+#LOCAL_SRC_FILES := sensors_mpl.cpp
+else
+LOCAL_SRC_FILES := sensors_mpl.cpp
+endif
+else    # eng & userdebug builds
+LOCAL_SRC_FILES := sensors_mpl.cpp
+endif   # eng & userdebug builds
+
+LOCAL_SHARED_LIBRARIES := libinvensense_hal
+LOCAL_SHARED_LIBRARIES += libcutils
+LOCAL_SHARED_LIBRARIES += libutils
+LOCAL_SHARED_LIBRARIES += libdl
+LOCAL_SHARED_LIBRARIES += liblog
+LOCAL_SHARED_LIBRARIES += libmllite
+include $(BUILD_SHARED_LIBRARY)
+
+include $(CLEAR_VARS)
+LOCAL_MODULE := libmplmpu
+LOCAL_SRC_FILES := libmplmpu.so
+LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := invensense
+LOCAL_MODULE_SUFFIX := .so
+LOCAL_MODULE_CLASS := SHARED_LIBRARIES
+LOCAL_MODULE_PATH := $(TARGET_OUT)/lib
+OVERRIDE_BUILT_MODULE_PATH := $(TARGET_OUT_INTERMEDIATE_LIBRARIES)
+include $(BUILD_PREBUILT)
+
+include $(CLEAR_VARS)
+LOCAL_MODULE := libmllite
+LOCAL_SRC_FILES := libmllite.so
+LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := invensense
+LOCAL_MODULE_SUFFIX := .so
+LOCAL_MODULE_CLASS := SHARED_LIBRARIES
+LOCAL_MODULE_PATH := $(TARGET_OUT)/lib
+OVERRIDE_BUILT_MODULE_PATH := $(TARGET_OUT_INTERMEDIATE_LIBRARIES)
+include $(BUILD_PREBUILT)
+
diff --git a/6515/libsensors_iio/CompassSensor.AKM.cpp b/6515/libsensors_iio/CompassSensor.AKM.cpp
new file mode 100644
index 0000000..45699fe
--- /dev/null
+++ b/6515/libsensors_iio/CompassSensor.AKM.cpp
@@ -0,0 +1,203 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <sys/select.h>
+#include <cutils/log.h>
+#include <linux/input.h>
+
+#include "sensor_params.h"
+#include "MPLSupport.h"
+
+// TODO: include corresponding header file for 3rd party compass sensor
+#include "CompassSensor.AKM.h"
+
+// TODO: specify this for "fillList()" API
+#define COMPASS_NAME "AKM8963"
+
+/*****************************************************************************/
+
+CompassSensor::CompassSensor() 
+                    : SensorBase(NULL, NULL)
+{
+    VFUNC_LOG;
+
+    // TODO: initiate 3rd-party's class, and disable its funtionalities
+    //       proper commands
+    mCompassSensor = new AkmSensor();
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            0, "/sys/class/compass/akm8963/enable_mag", getTimestamp());
+    write_sysfs_int((char*)"/sys/class/compass/akm8963/enable_mag", 0);
+}
+
+CompassSensor::~CompassSensor()
+{
+    VFUNC_LOG;
+
+    // TODO: disable 3rd-party's funtionalities and delete the object
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            0, "/sys/class/compass/akm8963/enable_mag", getTimestamp());
+    write_sysfs_int((char*)"/sys/class/compass/akm8963/enable_mag", 0);
+    delete mCompassSensor;
+}
+
+int CompassSensor::getFd(void) const
+{
+    VFUNC_LOG;
+
+    // TODO: return 3rd-party's file descriptor
+    return mCompassSensor->getFd();
+}
+
+/**
+ *  @brief        This function will enable/disable sensor.
+ *  @param[in]    handle    which sensor to enable/disable.
+ *  @param[in]    en        en=1 enable, en=0 disable
+ *  @return       if the operation is successful.
+ */
+int CompassSensor::enable(int32_t handle, int en)
+{
+    VFUNC_LOG;
+
+    // TODO: called 3rd-party's "set enable/disable" function
+    return mCompassSensor->setEnable(handle, en);
+}
+
+int CompassSensor::setDelay(int32_t handle, int64_t ns)
+{
+    VFUNC_LOG;
+
+    // TODO: called 3rd-party's "set delay" function
+    return mCompassSensor->setDelay(handle, ns);
+}
+
+/**
+    @brief      This function will return the state of the sensor.
+    @return     1=enabled; 0=disabled
+**/
+int CompassSensor::getEnable(int32_t handle)
+{
+    VFUNC_LOG;
+
+    // TODO: return if 3rd-party compass is enabled
+    return mCompassSensor->getEnable(handle);
+}
+
+/**
+    @brief      This function will return the current delay for this sensor.
+    @return     delay in nanoseconds. 
+**/
+int64_t CompassSensor::getDelay(int32_t handle)
+{
+    VFUNC_LOG;
+
+    // TODO: return 3rd-party's delay time (should be in ns)
+    return mCompassSensor->getDelay(handle);
+}
+
+/**
+    @brief         Integrators need to implement this function per 3rd-party solution
+    @param[out]    data      sensor data is stored in this variable. Scaled such that
+                             1 uT = 2^16
+    @para[in]      timestamp data's timestamp
+    @return        1, if 1   sample read, 0, if not, negative if error
+**/
+int CompassSensor::readSample(long *data, int64_t *timestamp)
+{
+    VFUNC_LOG;
+
+    // TODO: need to implement "readSample()" for MPL in 3rd-party's .cpp file
+    return mCompassSensor->readSample(data, timestamp);
+}
+
+/**
+    @brief         Integrators need to implement this function per 3rd-party solution
+    @param[out]    data      sensor data is stored in this variable. Scaled such that
+                             1 uT = 2^16
+    @para[in]      timestamp data's timestamp
+    @return        1, if 1   sample read, 0, if not, negative if error
+**/
+int CompassSensor::readRawSample(float *data, int64_t *timestamp)
+{
+    VFUNC_LOG;
+    long ldata[3];
+
+    int res = mCompassSensor->readSample(ldata, timestamp);
+    for(int i=0; i<3; i++) {
+        data[i] = (float)ldata[i];
+    }
+    return res; 
+}
+
+void CompassSensor::fillList(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    const char *compass = COMPASS_NAME;
+
+    if (compass) {
+        if (!strcmp(compass, "AKM8963")) {
+            list->maxRange = COMPASS_AKM8963_RANGE;
+            list->resolution = COMPASS_AKM8963_RESOLUTION;
+            list->power = COMPASS_AKM8963_POWER;
+            list->minDelay = COMPASS_AKM8963_MINDELAY;
+            return;
+        }
+        if (!strcmp(compass, "AKM8975")) {
+            list->maxRange = COMPASS_AKM8975_RANGE;
+            list->resolution = COMPASS_AKM8975_RESOLUTION;
+            list->power = COMPASS_AKM8975_POWER;
+            list->minDelay = COMPASS_AKM8975_MINDELAY;
+            LOGW("HAL:support for AKM8975 is incomplete");
+        }
+    }
+
+    LOGE("HAL:unsupported compass id %s -- "
+         "this implementation only supports AKM compasses", compass);
+    list->maxRange = COMPASS_AKM8975_RANGE;
+    list->resolution = COMPASS_AKM8975_RESOLUTION;
+    list->power = COMPASS_AKM8975_POWER;
+    list->minDelay = COMPASS_AKM8975_MINDELAY;
+}
+
+// TODO: specify compass sensor's mounting matrix for MPL
+void CompassSensor::getOrientationMatrix(signed char *orient)
+{
+    VFUNC_LOG;
+
+    orient[0] = 1;
+    orient[1] = 0;
+    orient[2] = 0;
+    orient[3] = 0;
+    orient[4] = 1;
+    orient[5] = 0;
+    orient[6] = 0;
+    orient[7] = 0;
+    orient[8] = 1;
+}
+
+int CompassSensor::getAccuracy(void)
+{
+    VFUNC_LOG;
+
+    // TODO: need to implement "getAccuracy()" for MPL in 3rd-party's .cpp file
+    return mCompassSensor->getAccuracy();
+}
diff --git a/6515/libsensors_iio/CompassSensor.AKM.h b/6515/libsensors_iio/CompassSensor.AKM.h
new file mode 100644
index 0000000..3d215a4
--- /dev/null
+++ b/6515/libsensors_iio/CompassSensor.AKM.h
@@ -0,0 +1,84 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef COMPASS_SENSOR_H
+#define COMPASS_SENSOR_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#include "SensorBase.h"
+
+// TODO: include 3rd-party compass vendor's header file
+//       p.s.: before using unified HAL, make sure 3rd-party compass
+//       solution's driver/HAL work well by themselves
+#include "AkmSensor.h"
+
+/*****************************************************************************/
+
+class CompassSensor : public SensorBase {
+
+protected:
+
+public:
+            CompassSensor();
+    virtual ~CompassSensor();
+
+    // TODO: make sure either 3rd-party compass solution has following virtual
+    //       functions, or SensorBase.cpp could provide equal functionalities
+    virtual int getFd() const;
+    virtual int getRawFd() {return 0;};
+    virtual int enable(int32_t handle, int enabled);
+    virtual int setDelay(int32_t handle, int64_t ns);
+    virtual int getEnable(int32_t handle);
+    virtual int64_t getDelay(int32_t handle);
+    virtual int64_t getMinDelay() { return -1; } // stub
+
+    // TODO: unnecessary for MPL solution (override 3rd-party solution)
+    virtual int readEvents(sensors_event_t *data, int count) { return 0; }
+
+    // TODO: following four APIs need further implementation for MPL's
+    //       reference (look into .cpp for detailed information, also refer to
+    //       3rd-party's readEvents() for relevant APIs)
+    int readSample(long *data, int64_t *timestamp);
+    int readRawSample(float *data, int64_t *timestamp);
+    void fillList(struct sensor_t *list);
+    void getOrientationMatrix(signed char *orient);
+    int getAccuracy();
+    virtual void getCompassBias(long *bias) {return;};
+
+    // TODO: if 3rd-party provides calibrated compass data, just return 1
+    int providesCalibration() { return 1; }
+
+    // TODO: hard-coded for 3rd-party's sensitivity transformation
+    long getSensitivity() { return (1L << 30); }
+    
+    /* all 3rd pary solution have compasses on the primary bus, hence they
+       have no dependency on the MPU */
+    int isIntegrated() { return 0; }
+
+    int checkCoilsReset(void) { return 0; };
+    int isYasCompass(void) { return 0; };
+
+private:
+    AkmSensor *mCompassSensor;
+};
+
+/*****************************************************************************/
+
+#endif  // COMPASS_SENSOR_H
diff --git a/6515/libsensors_iio/CompassSensor.IIO.9150.cpp b/6515/libsensors_iio/CompassSensor.IIO.9150.cpp
new file mode 100644
index 0000000..cc9d38c
--- /dev/null
+++ b/6515/libsensors_iio/CompassSensor.IIO.9150.cpp
@@ -0,0 +1,416 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#define LOG_NDEBUG 0
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <sys/select.h>
+#include <cutils/log.h>
+#include <linux/input.h>
+#include <string.h>
+
+#include "CompassSensor.IIO.9150.h"
+#include "sensors.h"
+#include "MPLSupport.h"
+#include "sensor_params.h"
+#include "ml_sysfs_helper.h"
+
+#define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*)
+#define COMPASS_NAME "USE_SYSFS"
+
+#if defined COMPASS_YAS53x
+#pragma message("HAL:build Invensense compass cal with YAS53x IIO on secondary bus")
+#define USE_MPL_COMPASS_HAL (1)
+#define COMPASS_NAME        "INV_YAS530"
+
+#elif defined COMPASS_AK8975
+#pragma message("HAL:build Invensense compass cal with AK8975 on primary bus")
+#define USE_MPL_COMPASS_HAL (1)
+#define COMPASS_NAME        "INV_AK8975"
+
+#elif defined INVENSENSE_COMPASS_CAL
+#   define COMPASS_NAME                 "USE_SYSFS"
+#pragma message("HAL:build Invensense compass cal with compass IIO on secondary bus")
+#define USE_MPL_COMPASS_HAL (1)
+#else
+#pragma message("HAL:build third party compass cal HAL")
+#define USE_MPL_COMPASS_HAL (0)
+// TODO: change to vendor's name
+#define COMPASS_NAME        "AKM8975"
+
+#endif
+
+/*****************************************************************************/
+
+CompassSensor::CompassSensor()
+                  : SensorBase(NULL, NULL),
+                    compass_fd(-1),
+                    mCompassTimestamp(0),
+                    mCompassInputReader(8)
+{
+    VFUNC_LOG;
+
+    if(!strcmp(COMPASS_NAME, "USE_SYSFS")) {
+        int result = find_name_by_sensor_type("in_magn_scale", "iio:device", 
+                                              sensor_name);
+        if(result) {
+            LOGE("HAL:Cannot read secondary device name - (%d)", result);
+        }
+        dev_name = sensor_name;
+    }
+    LOGI_IF(PROCESS_VERBOSE, "HAL:Secondary Chip Id: %s", dev_name);
+
+    if(inv_init_sysfs_attributes()) {
+        LOGE("Error Instantiating Compass\n");
+        return;
+    }
+
+    memset(mCachedCompassData, 0, sizeof(mCachedCompassData));
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", 
+            compassSysFs.compass_orient, getTimestamp());
+    FILE *fptr;
+    fptr = fopen(compassSysFs.compass_orient, "r");
+    if (fptr != NULL) {
+        int om[9];
+        if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", 
+               &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
+               &om[6], &om[7], &om[8]) < 0 || fclose(fptr) < 0) {
+            LOGE("HAL:Could not read compass mounting matrix");
+        } else {
+            LOGV_IF(EXTRA_VERBOSE, "HAL:compass mounting matrix: "
+                    "%+d %+d %+d %+d %+d %+d %+d %+d %+d", om[0], om[1], om[2], 
+                    om[3], om[4], om[5], om[6], om[7], om[8]);
+            mCompassOrientation[0] = om[0];
+            mCompassOrientation[1] = om[1];
+            mCompassOrientation[2] = om[2];
+            mCompassOrientation[3] = om[3];
+            mCompassOrientation[4] = om[4];
+            mCompassOrientation[5] = om[5];
+            mCompassOrientation[6] = om[6];
+            mCompassOrientation[7] = om[7];
+            mCompassOrientation[8] = om[8];
+        }
+    }
+
+    if (!isIntegrated()) {
+        enable(ID_M, 0);
+    }
+}
+
+CompassSensor::~CompassSensor()
+{
+    VFUNC_LOG;
+    free(pathP);
+    if( compass_fd > 0)
+        close(compass_fd);
+}
+
+int CompassSensor::getFd() const
+{
+    VFUNC_LOG;
+    return compass_fd;
+}
+
+/**
+ *  @brief        This function will enable/disable sensor.
+ *  @param[in]    handle
+ *                  which sensor to enable/disable.
+ *  @param[in]    en
+ *                  en=1, enable; 
+ *                  en=0, disable
+ *  @return       if the operation is successful.
+ */
+int CompassSensor::enable(int32_t handle, int en) 
+{
+    VFUNC_LOG;
+    int res = 0;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, compassSysFs.compass_enable, getTimestamp());
+    res = write_sysfs_int(compassSysFs.compass_enable, en);
+    return res;
+}
+
+int CompassSensor::setDelay(int32_t handle, int64_t ns) 
+{
+    VFUNC_LOG;
+    int tempFd;
+    int res;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 
+            1000000000.f / ns, compassSysFs.compass_rate, getTimestamp());
+    mDelay = ns;
+    if (ns == 0)
+        return -1;
+    tempFd = open(compassSysFs.compass_rate, O_RDWR);
+    res = write_attribute_sensor(tempFd, 1000000000.f / ns);
+    if(res < 0) {
+        LOGE("HAL:Compass update delay error");
+    }
+    return res;
+}
+
+int CompassSensor::turnOffCompassFifo(void)
+{
+    int i, res = 0, tempFd;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        0, compassSysFs.compass_fifo_enable, getTimestamp());
+    res += write_sysfs_int(compassSysFs.compass_fifo_enable, 0);
+    return res;
+}
+
+int CompassSensor::turnOnCompassFifo(void)
+{
+    int i, res = 0, tempFd;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        1, compassSysFs.compass_fifo_enable, getTimestamp());
+    res += write_sysfs_int(compassSysFs.compass_fifo_enable, 1);
+    return res;
+}
+
+/**
+    @brief      This function will return the state of the sensor.
+    @return     1=enabled; 0=disabled
+**/
+int CompassSensor::getEnable(int32_t handle)
+{
+    VFUNC_LOG;
+    return mEnable;
+}
+
+/* use for Invensense compass calibration */
+#define COMPASS_EVENT_DEBUG (0)
+void CompassSensor::processCompassEvent(const input_event *event)
+{
+    VHANDLER_LOG;
+
+    switch (event->code) {
+    case EVENT_TYPE_ICOMPASS_X:
+        LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_X\n");
+        mCachedCompassData[0] = event->value;
+        break;
+    case EVENT_TYPE_ICOMPASS_Y:
+        LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Y\n");
+        mCachedCompassData[1] = event->value;
+        break;
+    case EVENT_TYPE_ICOMPASS_Z:
+        LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Z\n");
+        mCachedCompassData[2] = event->value;
+        break;
+    }
+    
+    mCompassTimestamp = 
+        (int64_t)event->time.tv_sec * 1000000000L + event->time.tv_usec * 1000L;
+}
+
+void CompassSensor::getOrientationMatrix(signed char *orient)
+{
+    VFUNC_LOG;
+    memcpy(orient, mCompassOrientation, sizeof(mCompassOrientation));
+}
+
+long CompassSensor::getSensitivity()
+{
+    VFUNC_LOG;
+
+    long sensitivity;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", 
+            compassSysFs.compass_scale, getTimestamp());
+    inv_read_data(compassSysFs.compass_scale, &sensitivity);
+    return sensitivity;
+}
+
+/**
+    @brief         This function is called by sensors_mpl.cpp
+                   to read sensor data from the driver.
+    @param[out]    data      sensor data is stored in this variable. Scaled such that
+                             1 uT = 2^16
+    @para[in]      timestamp data's timestamp
+    @return        1, if 1   sample read, 0, if not, negative if error
+ */
+int CompassSensor::readSample(long *data, int64_t *timestamp)
+{
+    VHANDLER_LOG;
+
+    int numEventReceived = 0, done = 0;
+
+    ssize_t n = mCompassInputReader.fill(compass_fd);
+    if (n < 0) {
+        LOGE("HAL:no compass events read");
+        return n;
+    }
+
+    input_event const* event;
+
+    while (done == 0 && mCompassInputReader.readEvent(&event)) {
+        int type = event->type;
+        if (type == EV_REL) {
+            processCompassEvent(event);
+        } else if (type == EV_SYN) {
+            *timestamp = mCompassTimestamp;
+            memcpy(data, mCachedCompassData, sizeof(mCachedCompassData));
+            done = 1;
+        } else {
+            LOGE("HAL:Compass Sensor: unknown event (type=%d, code=%d)",
+                 type, event->code);
+        }
+        mCompassInputReader.next();
+    }
+
+    return done;
+}
+
+/**
+ *  @brief  This function will return the current delay for this sensor.
+ *  @return delay in nanoseconds. 
+ */
+int64_t CompassSensor::getDelay(int32_t handle)
+{
+    VFUNC_LOG;
+    return mDelay;
+}
+
+void CompassSensor::fillList(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    const char *compass = sensor_name;
+
+    if (compass) {
+        if(!strcmp(compass, "INV_COMPASS")) {
+            list->maxRange = COMPASS_MPU9150_RANGE;
+            list->resolution = COMPASS_MPU9150_RESOLUTION;
+            list->power = COMPASS_MPU9150_POWER;
+            list->minDelay = COMPASS_MPU9150_MINDELAY;
+            return;
+        }
+        if(!strcmp(compass, "compass")
+                || !strcmp(compass, "INV_AK8975")
+                || !strcmp(compass, "AK8975")
+                || !strcmp(compass, "ak8975")) {
+            list->maxRange = COMPASS_AKM8975_RANGE;
+            list->resolution = COMPASS_AKM8975_RESOLUTION;
+            list->power = COMPASS_AKM8975_POWER;
+            list->minDelay = COMPASS_AKM8975_MINDELAY;
+            return;
+        }
+        if(!strcmp(compass, "compass")
+                || !strcmp(compass, "INV_AK8963")
+                || !strcmp(compass, "AK8963")
+                || !strcmp(compass, "ak8963")) {
+            list->maxRange = COMPASS_AKM8963_RANGE;
+            list->resolution = COMPASS_AKM8963_RESOLUTION;
+            list->power = COMPASS_AKM8963_POWER;
+            list->minDelay = COMPASS_AKM8963_MINDELAY;
+            return;
+        }
+        if(!strcmp(compass, "compass")
+                || !strncmp(compass, "mlx90399",3)
+                || !strncmp(compass, "MLX90399",3)) {
+            list->maxRange = COMPASS_MPU9350_RANGE;
+            list->resolution = COMPASS_MPU9350_RESOLUTION;
+            list->power = COMPASS_MPU9350_POWER;
+            list->minDelay = COMPASS_MPU9350_MINDELAY;
+            return;
+        }
+        if(!strcmp(compass, "INV_YAS530")) {
+            list->maxRange = COMPASS_YAS53x_RANGE;
+            list->resolution = COMPASS_YAS53x_RESOLUTION;
+            list->power = COMPASS_YAS53x_POWER;
+            list->minDelay = COMPASS_YAS53x_MINDELAY;
+            return;
+        }
+        if(!strcmp(compass, "INV_AMI306")) {
+            list->maxRange = COMPASS_AMI306_RANGE;
+            list->resolution = COMPASS_AMI306_RESOLUTION;
+            list->power = COMPASS_AMI306_POWER;
+            list->minDelay = COMPASS_AMI306_MINDELAY;
+            return;
+        }
+    }
+    LOGE("HAL:unknown compass id %s -- "
+         "params default to ak8975 and might be wrong.",
+         compass);
+    list->maxRange = COMPASS_AKM8975_RANGE;
+    list->resolution = COMPASS_AKM8975_RESOLUTION;
+    list->power = COMPASS_AKM8975_POWER;
+    list->minDelay = COMPASS_AKM8975_MINDELAY;
+}
+
+int CompassSensor::inv_init_sysfs_attributes(void)
+{
+    VFUNC_LOG;
+
+    unsigned char i = 0;
+    char sysfs_path[MAX_SYSFS_NAME_LEN];
+    char iio_trigger_path[MAX_SYSFS_NAME_LEN], tbuf[2];
+    char *sptr;
+    char **dptr;
+    int num;
+
+    pathP = (char*)malloc(
+                    sizeof(char[COMPASS_MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
+    sptr = pathP;
+    dptr = (char**)&compassSysFs;
+    if (sptr == NULL)
+        return -1;
+
+    memset(sysfs_path, 0, sizeof(sysfs_path));
+    memset(iio_trigger_path, 0, sizeof(iio_trigger_path));
+
+    do {
+        *dptr++ = sptr;
+        memset(sptr, 0, sizeof(sptr));
+        sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
+    } while (++i < COMPASS_MAX_SYSFS_ATTRB);
+
+    // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
+    // inv_get_sysfs_abs_path(sysfs_path);
+    inv_get_sysfs_path(sysfs_path);
+    inv_get_iio_trigger_path(iio_trigger_path);
+
+#if defined COMPASS_AK8975
+    inv_get_input_number(dev_name, &num);
+    tbuf[0] = num + 0x30;
+    tbuf[1] = 0;
+    sprintf(sysfs_path, "%s%s", "sys/class/input/input", tbuf);
+    strcat(sysfs_path, "/ak8975");
+
+    sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/enable");
+    sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/rate");
+    sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/scale");
+    sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix");
+#else
+    sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/compass_enable");
+    sprintf(compassSysFs.compass_fifo_enable, "%s%s", sysfs_path, "/compass_fifo_enable");
+    sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/compass_rate");
+    sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/in_magn_scale");
+    sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix");
+#endif
+
+#if 0
+    // test print sysfs paths   
+    dptr = (char**)&compassSysFs;
+    for (i = 0; i < COMPASS_MAX_SYSFS_ATTRB; i++) {
+        LOGE("HAL:sysfs path: %s", *dptr++);
+    }
+#endif
+    return 0;
+}
diff --git a/6515/libsensors_iio/CompassSensor.IIO.9150.h b/6515/libsensors_iio/CompassSensor.IIO.9150.h
new file mode 100644
index 0000000..f981cd7
--- /dev/null
+++ b/6515/libsensors_iio/CompassSensor.IIO.9150.h
@@ -0,0 +1,100 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef COMPASS_SENSOR_H
+#define COMPASS_SENSOR_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+// TODO fixme, need input_event
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <poll.h>
+#include <utils/Vector.h>
+#include <utils/KeyedVector.h>
+
+#include "sensors.h"
+#include "SensorBase.h"
+#include "InputEventReader.h"
+
+class CompassSensor : public SensorBase {
+
+public:
+    CompassSensor();
+    virtual ~CompassSensor();
+
+    virtual int getFd() const;
+    virtual int enable(int32_t handle, int enabled);
+    virtual int setDelay(int32_t handle, int64_t ns);
+    virtual int getEnable(int32_t handle);
+    virtual int64_t getDelay(int32_t handle);
+    virtual int64_t getMinDelay() { return -1; } // stub
+
+    // unnecessary for MPL
+    virtual int readEvents(sensors_event_t *data, int count) { return 0; }
+
+    int turnOffCompassFifo(void);
+    int turnOnCompassFifo(void);
+    int readSample(long *data, int64_t *timestamp);
+    int providesCalibration() { return 0; }
+    void getOrientationMatrix(signed char *orient);
+    long getSensitivity();
+    int getAccuracy() { return 0; }
+    void fillList(struct sensor_t *list);
+    int isIntegrated() { return (1); }
+    int isYasCompass(void) { return (0); }
+    int checkCoilsReset(void) { return(-1); }
+
+private:
+    char sensor_name[200];
+    enum CompassBus {
+        COMPASS_BUS_PRIMARY = 0,
+        COMPASS_BUS_SECONDARY = 1
+    } mI2CBus;
+
+    struct sysfs_attrbs {
+       char *compass_enable;
+       char *compass_fifo_enable;
+       char *compass_x_fifo_enable;
+       char *compass_y_fifo_enable;
+       char *compass_z_fifo_enable;
+       char *compass_rate;
+       char *compass_scale;
+       char *compass_orient;
+    } compassSysFs;
+
+    // implementation specific
+    signed char mCompassOrientation[9];
+    long mCachedCompassData[3];
+    int compass_fd;
+    int64_t mCompassTimestamp;
+    InputEventCircularReader mCompassInputReader;
+    int64_t mDelay;
+    int mEnable;
+    char *pathP;
+
+    void processCompassEvent(const input_event *event);
+    int inv_init_sysfs_attributes(void);
+};
+
+/*****************************************************************************/
+
+#endif  // COMPASS_SENSOR_H
diff --git a/6515/libsensors_iio/CompassSensor.IIO.primary.cpp b/6515/libsensors_iio/CompassSensor.IIO.primary.cpp
new file mode 100644
index 0000000..5d8d120
--- /dev/null
+++ b/6515/libsensors_iio/CompassSensor.IIO.primary.cpp
@@ -0,0 +1,563 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#define LOG_NDEBUG 0
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <sys/select.h>
+#include <cutils/log.h>
+#include <linux/input.h>
+#include <string.h>
+
+#include "CompassSensor.IIO.primary.h"
+#include "sensors.h"
+#include "MPLSupport.h"
+#include "sensor_params.h"
+#include "ml_sysfs_helper.h"
+
+#define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*)
+#define COMPASS_NAME "USE_SYSFS"
+
+#if defined COMPASS_AK8975
+#pragma message("HAL:build Invensense compass cal with AK8975 on primary bus")
+#define USE_MPL_COMPASS_HAL (1)
+#define COMPASS_NAME        "INV_AK8975"
+#endif
+
+/******************************************************************************/
+
+CompassSensor::CompassSensor() 
+                  : SensorBase(COMPASS_NAME, NULL),
+                    mCompassTimestamp(0),
+                    mCompassInputReader(8),
+                    mCoilsResetFd(0)
+{
+    FILE *fptr;
+
+    VFUNC_LOG;
+
+    mYasCompass = false;
+    if(!strcmp(dev_name, "USE_SYSFS")) {
+        char sensor_name[20]; 
+        find_name_by_sensor_type("in_magn_x_raw", "iio:device", sensor_name);
+        strncpy(dev_full_name, sensor_name,
+                sizeof(dev_full_name) / sizeof(dev_full_name[0]));
+        if(!strncmp(dev_full_name, "yas", 3)) {
+            mYasCompass = true;
+        }
+    } else {
+
+#ifdef COMPASS_YAS53x
+        /* for YAS53x compasses, dev_name is just a prefix, 
+           we need to find the actual name */
+        if (fill_dev_full_name_by_prefix(dev_name, 
+                dev_full_name, sizeof(dev_full_name) / sizeof(dev_full_name[0]))) {
+            LOGE("Cannot find Yamaha device with prefix name '%s' - "
+                 "magnetometer will likely not work.", dev_name);
+        } else {
+            mYasCompass = true;
+        }
+#else
+        strncpy(dev_full_name, dev_name,
+                sizeof(dev_full_name) / sizeof(dev_full_name[0]));
+#endif
+
+}
+
+    if (inv_init_sysfs_attributes()) {
+        LOGE("Error Instantiating Compass\n");
+        return;
+    }
+
+    if (!strcmp(dev_full_name, "INV_COMPASS")) {
+        mI2CBus = COMPASS_BUS_SECONDARY;
+    } else {
+        mI2CBus = COMPASS_BUS_PRIMARY;
+    }
+
+    memset(mCachedCompassData, 0, sizeof(mCachedCompassData));
+
+    if (!isIntegrated()) {
+        enable(ID_M, 0);
+    }
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:compass name: %s", dev_full_name);
+    enable_iio_sysfs();
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", 
+            compassSysFs.compass_orient, getTimestamp());
+    fptr = fopen(compassSysFs.compass_orient, "r");
+    if (fptr != NULL) {
+        int om[9];
+        if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", 
+               &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
+               &om[6], &om[7], &om[8]) < 0 || fclose(fptr)) {
+            LOGE("HAL:could not read compass mounting matrix");
+        } else {
+
+            LOGV_IF(EXTRA_VERBOSE,
+                    "HAL:compass mounting matrix: "
+                    "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
+                    om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
+
+            mCompassOrientation[0] = om[0];
+            mCompassOrientation[1] = om[1];
+            mCompassOrientation[2] = om[2];
+            mCompassOrientation[3] = om[3];
+            mCompassOrientation[4] = om[4];
+            mCompassOrientation[5] = om[5];
+            mCompassOrientation[6] = om[6];
+            mCompassOrientation[7] = om[7];
+            mCompassOrientation[8] = om[8];
+        }
+    }
+
+    if(mYasCompass) {
+        mCoilsResetFd = fopen(compassSysFs.compass_attr_1, "r+");
+        if (fptr == NULL) {
+            LOGE("HAL:Could not open compass overunderflow");
+        }
+    }
+}
+
+void CompassSensor::enable_iio_sysfs()
+{
+    VFUNC_LOG;
+
+    int tempFd = 0;
+    char iio_device_node[MAX_CHIP_ID_LEN];
+    FILE *tempFp = NULL;
+    const char* compass = dev_full_name;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            1, compassSysFs.in_timestamp_en, getTimestamp());
+    write_sysfs_int(compassSysFs.in_timestamp_en, 1);
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            IIO_BUFFER_LENGTH, compassSysFs.buffer_length, getTimestamp());
+    tempFp = fopen(compassSysFs.buffer_length, "w");
+    if (tempFp == NULL) {
+        LOGE("HAL:could not open buffer length");
+    } else {
+        if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0 || fclose(tempFp) < 0) {
+            LOGE("HAL:could not write buffer length");
+        }
+    }
+
+    sprintf(iio_device_node, "%s%d", "/dev/iio:device",
+            find_type_by_name(compass, "iio:device"));
+    compass_fd = open(iio_device_node, O_RDONLY);
+    int res = errno;
+    if (compass_fd < 0) {
+        LOGE("HAL:could not open '%s' iio device node in path '%s' - "
+             "error '%s' (%d)",
+             compass, iio_device_node, strerror(res), res);
+    } else {
+        LOGV_IF(EXTRA_VERBOSE, 
+                "HAL:iio %s, compass_fd opened : %d", compass, compass_fd);
+    }
+
+    /* TODO: need further tests for optimization to reduce context-switch
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)", 
+            compassSysFs.compass_x_fifo_enable, getTimestamp());
+    tempFd = open(compassSysFs.compass_x_fifo_enable, O_RDWR);
+    res = errno;
+    if (tempFd > 0) {
+        res = enable_sysfs_sensor(tempFd, 1);
+    } else {
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             compassSysFs.compass_x_fifo_enable, strerror(res), res);
+    }
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)", 
+            compassSysFs.compass_y_fifo_enable, getTimestamp());
+    tempFd = open(compassSysFs.compass_y_fifo_enable, O_RDWR);
+    res = errno;
+    if (tempFd > 0) {
+        res = enable_sysfs_sensor(tempFd, 1);
+    } else {
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             compassSysFs.compass_y_fifo_enable, strerror(res), res);
+    }
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)", 
+            compassSysFs.compass_z_fifo_enable, getTimestamp());
+    tempFd = open(compassSysFs.compass_z_fifo_enable, O_RDWR);
+    res = errno;
+    if (tempFd > 0) {
+        res = enable_sysfs_sensor(tempFd, 1);
+    } else {
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             compassSysFs.compass_z_fifo_enable, strerror(res), res);
+    }
+    */
+}
+
+CompassSensor::~CompassSensor()
+{
+    VFUNC_LOG;
+
+    free(pathP);
+    if( compass_fd > 0)
+        close(compass_fd);
+    if(mYasCompass) {
+        if( mCoilsResetFd != NULL )
+            fclose(mCoilsResetFd);
+    }
+}
+
+int CompassSensor::getFd(void) const
+{
+    VHANDLER_LOG;
+    LOGI_IF(0, "HAL:compass_fd=%d", compass_fd);
+    return compass_fd;
+}
+
+/**
+ *  @brief        This function will enable/disable sensor.
+ *  @param[in]    handle
+ *                  which sensor to enable/disable.
+ *  @param[in]    en
+ *                  en=1, enable; 
+ *                  en=0, disable
+ *  @return       if the operation is successful.
+ */
+int CompassSensor::enable(int32_t handle, int en) 
+{
+    VFUNC_LOG;
+
+    mEnable = en;
+    int tempFd;
+    int res = 0;
+
+    /* reset master enable */
+    res = masterEnable(0);
+    if (res < 0) {
+        return res;
+    }
+
+    if (en) {
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                en, compassSysFs.compass_x_fifo_enable, getTimestamp());
+        res = write_sysfs_int(compassSysFs.compass_x_fifo_enable, en);
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                en, compassSysFs.compass_y_fifo_enable, getTimestamp());
+        res += write_sysfs_int(compassSysFs.compass_y_fifo_enable, en);
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                en, compassSysFs.compass_z_fifo_enable, getTimestamp());
+        res += write_sysfs_int(compassSysFs.compass_z_fifo_enable, en);
+
+        res = masterEnable(en);
+        if (res < en) {
+            return res;
+        }
+    }
+
+    return res;
+}
+
+int CompassSensor::masterEnable(int en)
+{
+    VFUNC_LOG;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, compassSysFs.chip_enable, getTimestamp());
+    return write_sysfs_int(compassSysFs.chip_enable, en);
+}
+
+int CompassSensor::setDelay(int32_t handle, int64_t ns) 
+{
+    VFUNC_LOG;
+    int tempFd;
+    int res;
+
+    mDelay = ns;
+    if (ns == 0)
+        return -1;
+    tempFd = open(compassSysFs.compass_rate, O_RDWR);
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 
+            1000000000.f / ns, compassSysFs.compass_rate, getTimestamp());
+    res = write_attribute_sensor(tempFd, 1000000000.f / ns);
+    if(res < 0) {
+        LOGE("HAL:Compass update delay error");
+    }
+    return res;
+}
+
+/**
+    @brief      This function will return the state of the sensor.
+    @return     1=enabled; 0=disabled
+**/
+int CompassSensor::getEnable(int32_t handle)
+{
+    VFUNC_LOG;
+    return mEnable;
+}
+
+/* use for Invensense compass calibration */
+#define COMPASS_EVENT_DEBUG (0)
+void CompassSensor::processCompassEvent(const input_event *event)
+{
+    VHANDLER_LOG;
+
+    switch (event->code) {
+    case EVENT_TYPE_ICOMPASS_X:
+        LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_X\n");
+        mCachedCompassData[0] = event->value;
+        break;
+    case EVENT_TYPE_ICOMPASS_Y:
+        LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Y\n");
+        mCachedCompassData[1] = event->value;
+        break;
+    case EVENT_TYPE_ICOMPASS_Z:
+        LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Z\n");
+        mCachedCompassData[2] = event->value;
+        break;
+    }
+    
+    mCompassTimestamp = 
+        (int64_t)event->time.tv_sec * 1000000000L + event->time.tv_usec * 1000L;
+}
+
+void CompassSensor::getOrientationMatrix(signed char *orient)
+{
+    VFUNC_LOG;
+    memcpy(orient, mCompassOrientation, sizeof(mCompassOrientation));
+}
+
+long CompassSensor::getSensitivity()
+{
+    VFUNC_LOG;
+
+    long sensitivity;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", 
+            compassSysFs.compass_scale, getTimestamp());
+    inv_read_data(compassSysFs.compass_scale, &sensitivity);
+    return sensitivity;
+}
+
+/**
+    @brief         This function is called by sensors_mpl.cpp
+                   to read sensor data from the driver.
+    @param[out]    data      sensor data is stored in this variable. Scaled such that
+                             1 uT = 2^16
+    @para[in]      timestamp data's timestamp
+    @return        1, if 1   sample read, 0, if not, negative if error
+ */
+int CompassSensor::readSample(long *data, int64_t *timestamp) {
+    VFUNC_LOG;
+
+    int i;
+    char *rdata = mIIOBuffer;
+
+    size_t rsize = read(compass_fd, rdata, (8 * mEnable + 8) * 1);
+
+    if (!mEnable) {
+        rsize = read(compass_fd, rdata, (8 + 8) * IIO_BUFFER_LENGTH);
+        // LOGI("clear buffer with size: %d", rsize);
+    }
+/*
+    LOGI("get one sample of AMI IIO data with size: %d", rsize);
+    LOGI_IF(mEnable, "compass x/y/z: %d/%d/%d", *((short *) (rdata + 0)),
+        *((short *) (rdata + 2)), *((short *) (rdata + 4)));
+*/
+    if (mEnable) {
+        for (i = 0; i < 3; i++) {
+            data[i] = *((short *) (rdata + i * 2));
+        }
+        *timestamp = *((long long *) (rdata + 8 * mEnable));
+    }
+
+    return mEnable;
+}
+
+/**
+ *  @brief  This function will return the current delay for this sensor.
+ *  @return delay in nanoseconds. 
+ */
+int64_t CompassSensor::getDelay(int32_t handle)
+{
+    VFUNC_LOG;
+    return mDelay;
+}
+
+void CompassSensor::fillList(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    const char *compass = dev_full_name;
+
+    if (compass) {
+        if(!strcmp(compass, "INV_COMPASS")) {
+            list->maxRange = COMPASS_MPU9150_RANGE;
+            list->resolution = COMPASS_MPU9150_RESOLUTION;
+            list->power = COMPASS_MPU9150_POWER;
+            list->minDelay = COMPASS_MPU9150_MINDELAY;
+            mMinDelay = list->minDelay;
+            return;
+        }
+        if(!strcmp(compass, "compass")
+                || !strcmp(compass, "INV_AK8975")
+                || !strcmp(compass, "AK8975")
+                || !strcmp(compass, "ak8975")) {
+            list->maxRange = COMPASS_AKM8975_RANGE;
+            list->resolution = COMPASS_AKM8975_RESOLUTION;
+            list->power = COMPASS_AKM8975_POWER;
+            list->minDelay = COMPASS_AKM8975_MINDELAY;
+            mMinDelay = list->minDelay;
+            return;
+        }
+        if(!strcmp(compass, "compass")
+                || !strcmp(compass, "INV_AK8963")
+                || !strcmp(compass, "AK8963")
+                || !strcmp(compass, "ak8963")) {
+            list->maxRange = COMPASS_AKM8963_RANGE;
+            list->resolution = COMPASS_AKM8963_RESOLUTION;
+            list->power = COMPASS_AKM8963_POWER;
+            list->minDelay = COMPASS_AKM8963_MINDELAY;
+            mMinDelay = list->minDelay;
+            return;
+        }
+        if(!strcmp(compass, "ami306")) {
+            list->maxRange = COMPASS_AMI306_RANGE;
+            list->resolution = COMPASS_AMI306_RESOLUTION;
+            list->power = COMPASS_AMI306_POWER;
+            list->minDelay = COMPASS_AMI306_MINDELAY;
+            mMinDelay = list->minDelay;
+            return;
+        }
+        if(!strcmp(compass, "yas530") 
+                || !strcmp(compass, "yas532")
+                || !strcmp(compass, "yas533")) {
+            list->maxRange = COMPASS_YAS53x_RANGE;
+            list->resolution = COMPASS_YAS53x_RESOLUTION;
+            list->power = COMPASS_YAS53x_POWER;
+            list->minDelay = COMPASS_YAS53x_MINDELAY;
+            mMinDelay = list->minDelay;
+            return;
+        }
+    }
+
+    LOGE("HAL:unknown compass id %s -- "
+         "params default to ak8975 and might be wrong.",
+         compass);
+    list->maxRange = COMPASS_AKM8975_RANGE;
+    list->resolution = COMPASS_AKM8975_RESOLUTION;
+    list->power = COMPASS_AKM8975_POWER;
+    list->minDelay = COMPASS_AKM8975_MINDELAY;
+    mMinDelay = list->minDelay;
+}
+
+/* Read sysfs entry to determine whether overflow had happend
+   then write to sysfs to reset to zero */
+int CompassSensor::checkCoilsReset()
+{
+    int result=-1;
+    VFUNC_LOG;
+
+    if(mCoilsResetFd != NULL) {
+        int attr;
+        rewind(mCoilsResetFd);
+        fscanf(mCoilsResetFd, "%d", &attr);
+        if(attr == 0)
+            return 0;
+        else {
+            LOGV_IF(SYSFS_VERBOSE, "HAL:overflow detected");
+            rewind(mCoilsResetFd);
+            if(fprintf(mCoilsResetFd, "%d", 0) < 0)
+                LOGE("HAL:could not write overunderflow");
+            else
+                return 1;
+        }
+    } else {
+        LOGE("HAL:could not read overunderflow");
+    }
+    return result;
+}
+
+int CompassSensor::inv_init_sysfs_attributes(void)
+{
+    VFUNC_LOG;
+
+    unsigned char i = 0;
+    char sysfs_path[MAX_SYSFS_NAME_LEN], tbuf[2];
+    char *sptr;
+    char **dptr;
+    int num;
+    const char* compass = dev_full_name;
+
+    pathP = (char*)malloc(
+                    sizeof(char[COMPASS_MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
+    sptr = pathP;
+    dptr = (char**)&compassSysFs;
+    if (sptr == NULL)
+        return -1;
+
+    do {
+        *dptr++ = sptr;
+        sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
+    } while (++i < COMPASS_MAX_SYSFS_ATTRB);
+
+    // get proper (in absolute/relative) IIO path & build sysfs paths
+    sprintf(sysfs_path, "%s%d", "/sys/bus/iio/devices/iio:device",
+    find_type_by_name(compass, "iio:device"));
+
+#if defined COMPASS_AK8975
+    inv_get_input_number(compass, &num);
+    tbuf[0] = num + 0x30;
+    tbuf[1] = 0;
+    sprintf(sysfs_path, "%s%s", "sys/class/input/input", tbuf);
+    strcat(sysfs_path, "/ak8975");
+
+    sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/enable");
+    sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/rate");
+    sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/scale");
+    sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix");
+#else /* IIO */
+    sprintf(compassSysFs.chip_enable, "%s%s", sysfs_path, "/buffer/enable");
+    sprintf(compassSysFs.in_timestamp_en, "%s%s", sysfs_path, "/scan_elements/in_timestamp_en");
+    sprintf(compassSysFs.buffer_length, "%s%s", sysfs_path, "/buffer/length");
+
+    sprintf(compassSysFs.compass_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_x_en");
+    sprintf(compassSysFs.compass_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_y_en");
+    sprintf(compassSysFs.compass_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_z_en");
+    sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/sampling_frequency");
+    sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/in_magn_scale");
+    sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix");
+
+    if(mYasCompass) {
+        sprintf(compassSysFs.compass_attr_1, "%s%s", sysfs_path, "/overunderflow");
+    }
+#endif
+
+#if 0 
+    // test print sysfs paths   
+    dptr = (char**)&compassSysFs;
+    LOGI("sysfs path base: %s", sysfs_path);
+    for (i = 0; i < COMPASS_MAX_SYSFS_ATTRB; i++) {
+        LOGE("HAL:sysfs path: %s", *dptr++);
+    }
+#endif
+    return 0;
+}
+
+int CompassSensor::isYasCompass(void)
+{
+    return mYasCompass;
+}
diff --git a/6515/libsensors_iio/CompassSensor.IIO.primary.h b/6515/libsensors_iio/CompassSensor.IIO.primary.h
new file mode 100644
index 0000000..de64799
--- /dev/null
+++ b/6515/libsensors_iio/CompassSensor.IIO.primary.h
@@ -0,0 +1,114 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef COMPASS_SENSOR_H
+#define COMPASS_SENSOR_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+// TODO fixme, need input_event
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <poll.h>
+#include <utils/Vector.h>
+#include <utils/KeyedVector.h>
+
+#include "sensors.h"
+#include "SensorBase.h"
+#include "InputEventReader.h"
+
+#define MAX_CHIP_ID_LEN (20)
+#define COMPASS_ON_PRIMARY "in_magn_x_raw"
+
+class CompassSensor : public SensorBase {
+
+public:
+    CompassSensor();
+    virtual ~CompassSensor();
+
+    virtual int getFd() const;
+    virtual int enable(int32_t handle, int enabled);
+    virtual int setDelay(int32_t handle, int64_t ns);
+    virtual int getEnable(int32_t handle);
+    virtual int64_t getDelay(int32_t handle);
+    virtual int64_t getMinDelay() { return mMinDelay; }
+
+    // unnecessary for MPL
+    virtual int readEvents(sensors_event_t *data, int count) { return 0; }
+
+    int readSample(long *data, int64_t *timestamp);
+    int readRawSample(float *data, int64_t *timestamp);
+    int providesCalibration() { return 0; }
+    void getOrientationMatrix(signed char *orient);
+    long getSensitivity();
+    int getAccuracy() { return 0; }
+    void fillList(struct sensor_t *list);
+    int isIntegrated() { return (0); }
+    int checkCoilsReset(void);
+    int isYasCompass(void);
+
+private:
+    enum CompassBus {
+        COMPASS_BUS_PRIMARY = 0,
+        COMPASS_BUS_SECONDARY = 1
+    } mI2CBus;
+
+    struct sysfs_attrbs {
+       char *chip_enable;
+       char *in_timestamp_en;
+       char *buffer_length;
+
+       char *compass_enable;
+       char *compass_x_fifo_enable;
+       char *compass_y_fifo_enable;
+       char *compass_z_fifo_enable;
+       char *compass_rate;
+       char *compass_scale;
+       char *compass_orient;
+       char *compass_attr_1;
+    } compassSysFs;
+    
+    char dev_full_name[20];
+
+    // implementation specific
+    signed char mCompassOrientation[9];
+    long mCachedCompassData[3];
+    int64_t mCompassTimestamp;
+    InputEventCircularReader mCompassInputReader;
+    int compass_fd;
+    int64_t mDelay;
+    int64_t mMinDelay;
+    int mEnable;
+    char *pathP;
+
+    char mIIOBuffer[(8 + 8) * IIO_BUFFER_LENGTH];
+
+    int masterEnable(int en);
+    void enable_iio_sysfs(void);
+    void processCompassEvent(const input_event *event);
+    int inv_init_sysfs_attributes(void);
+    FILE *mCoilsResetFd;
+    bool mYasCompass;
+};
+
+/*****************************************************************************/
+
+#endif  // COMPASS_SENSOR_H
diff --git a/6515/libsensors_iio/InputEventReader.cpp b/6515/libsensors_iio/InputEventReader.cpp
new file mode 100644
index 0000000..968e32e
--- /dev/null
+++ b/6515/libsensors_iio/InputEventReader.cpp
@@ -0,0 +1,114 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#define LOG_NDEBUG 0
+
+#include <stdint.h>
+#include <errno.h>
+#include <unistd.h>
+#include <poll.h>
+
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#include <linux/input.h>
+
+#include <cutils/log.h>
+
+#include "InputEventReader.h"
+
+/*****************************************************************************/
+
+struct input_event;
+
+InputEventCircularReader::InputEventCircularReader(size_t numEvents)
+    : mBuffer(new input_event[numEvents * 2]),
+      mBufferEnd(mBuffer + numEvents),
+      mHead(mBuffer),
+      mCurr(mBuffer),
+      mFreeSpace(numEvents)
+{
+    mLastFd = -1;
+}
+
+InputEventCircularReader::~InputEventCircularReader()
+{
+    delete [] mBuffer;
+}
+
+#define INPUT_EVENT_DEBUG (0)
+ssize_t InputEventCircularReader::fill(int fd)
+{
+    size_t numEventsRead = 0;
+    mLastFd = fd;
+
+    LOGV_IF(INPUT_EVENT_DEBUG, 
+            "DEBUG:%s enter, fd=%d\n", __PRETTY_FUNCTION__, fd);
+    if (mFreeSpace) {
+        const ssize_t nread = read(fd, mHead, mFreeSpace * sizeof(input_event));
+        if (nread < 0 || nread % sizeof(input_event)) {
+            //LOGE("Partial event received nread=%d, required=%d", 
+            //     nread, sizeof(input_event));
+            //LOGE("FD trying to read is: %d");
+            // we got a partial event!!
+            if (INPUT_EVENT_DEBUG) {
+                LOGV_IF(nread < 0, "DEBUG:%s exit nread < 0\n", 
+                        __PRETTY_FUNCTION__);
+                LOGV_IF(nread % sizeof(input_event), 
+                        "DEBUG:%s exit nread %% sizeof(input_event)\n", 
+                        __PRETTY_FUNCTION__);
+            }
+            return (nread < 0 ? -errno : -EINVAL);
+        }
+
+        numEventsRead = nread / sizeof(input_event);
+        if (numEventsRead) {
+            mHead += numEventsRead;
+            mFreeSpace -= numEventsRead;
+            if (mHead > mBufferEnd) {
+                size_t s = mHead - mBufferEnd;
+                memcpy(mBuffer, mBufferEnd, s * sizeof(input_event));
+                mHead = mBuffer + s;
+            }
+        }
+    }
+
+    LOGV_IF(INPUT_EVENT_DEBUG, "DEBUG:%s exit, numEventsRead:%d\n", 
+            __PRETTY_FUNCTION__, numEventsRead);
+    return numEventsRead;
+}
+
+ssize_t InputEventCircularReader::readEvent(input_event const** events)
+{
+    *events = mCurr;
+    ssize_t available = (mBufferEnd - mBuffer) - mFreeSpace;
+    LOGV_IF(INPUT_EVENT_DEBUG, "DEBUG:%s fd:%d, available:%d\n", 
+            __PRETTY_FUNCTION__, mLastFd, (int)available);
+    return (available ? 1 : 0);
+}
+
+void InputEventCircularReader::next()
+{
+    mCurr++;
+    mFreeSpace++;
+    if (mCurr >= mBufferEnd) {
+        mCurr = mBuffer;
+    }
+    ssize_t available = (mBufferEnd - mBuffer) - mFreeSpace;
+    LOGV_IF(INPUT_EVENT_DEBUG, "DEBUG:%s fd:%d, still available:%d\n",
+            __PRETTY_FUNCTION__, mLastFd, (int)available);
+}
+
diff --git a/6515/libsensors_iio/InputEventReader.h b/6515/libsensors_iio/InputEventReader.h
new file mode 100644
index 0000000..4e33af3
--- /dev/null
+++ b/6515/libsensors_iio/InputEventReader.h
@@ -0,0 +1,50 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef ANDROID_INPUT_EVENT_READER_H
+#define ANDROID_INPUT_EVENT_READER_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#include "SensorBase.h"
+
+/*****************************************************************************/
+
+struct input_event;
+
+class InputEventCircularReader
+{
+    struct input_event* const mBuffer;
+    struct input_event* const mBufferEnd;
+    struct input_event* mHead;
+    struct input_event* mCurr;
+    ssize_t mFreeSpace;
+    int mLastFd;
+
+public:
+    InputEventCircularReader(size_t numEvents);
+    ~InputEventCircularReader();
+    ssize_t fill(int fd);
+    ssize_t readEvent(input_event const** events);
+    void next();
+};
+
+/*****************************************************************************/
+
+#endif  // ANDROID_INPUT_EVENT_READER_H
diff --git a/6515/libsensors_iio/License.txt b/6515/libsensors_iio/License.txt
new file mode 100644
index 0000000..930f931
--- /dev/null
+++ b/6515/libsensors_iio/License.txt
@@ -0,0 +1,217 @@
+SOFTWARE LICENSE AGREEMENT
+
+Unless you and InvenSense Corporation ("InvenSense") execute a separate written
+software license agreement governing use of the accompanying software, this
+software is licensed to you under the terms of this Software License
+Agreement ("Agreement").
+
+ANY USE, REPRODUCTION OR DISTRIBUTION OF THE SOFTWARE CONSTITUTES YOUR
+ACCEPTANCE OF THIS AGREEMENT.
+
+1.  DEFINITIONS.
+
+1.1.    "InvenSense Product" means any of the proprietary integrated circuit
+product(s) sold by InvenSense with which the Software was designed to be used,
+or their successors.
+
+1.2.    "Licensee" means you or if you are accepting on behalf of an entity
+then the entity and its affiliates exercising rights under, and complying
+with all of the terms of this Agreement.
+
+1.3.    "Software" shall mean that software made available by InvenSense to
+Licensee in binary code form with this Agreement.
+
+2.  LICENSE GRANT; OWNERSHIP
+
+2.1.    License Grants.  Subject to the terms and conditions of this Agreement,
+InvenSense hereby grants to Licensee a non-exclusive, non-transferable,
+royalty-free license (i) to use and integrate the Software in conjunction
+with any other software; and (ii) to reproduce and distribute the Software
+complete, unmodified and only for use with a InvenSense Product.
+
+2.2.    Restriction on Modification.  If and to the extent that the Software is
+designed to be compliant with any published communications standard
+(including, without limitation, DOCSIS, HomePNA, IEEE, and ITU standards),
+Licensee may not make any modifications to the Software that would cause the
+Software or the accompanying InvenSense Products to be incompatible with such
+standard.
+
+2.3.    Restriction on Distribution.  Licensee shall only distribute the
+Software (a) under the terms of this Agreement and a copy of this Agreement
+accompanies such distribution, and (b) agrees to defend and indemnify
+InvenSense and its licensors from and against any damages, costs, liabilities,
+settlement amounts and/or expenses (including attorneys' fees) incurred in
+connection with any claim, lawsuit or action by any third party that arises
+or results from the use or distribution of any and all Software by the
+Licensee except as contemplated herein.
+
+2.4.    Proprietary Notices.  Licensee shall not remove, efface or obscure any
+copyright or trademark notices from the Software.  Licensee shall include
+reproductions of the InvenSense copyright notice with each copy of the
+Software, except where such Software is embedded in a manner not readily
+accessible to the end user.  Licensee acknowledges that any symbols,
+trademarks, tradenames, and service marks adopted by InvenSense to identify the
+Software belong to InvenSense and that Licensee shall have no rights therein.
+
+2.5.    Ownership.  InvenSense shall retain all right, title and interest,
+including all intellectual property rights, in and to the Software.  Licensee
+hereby covenants that it will not assert any claim that the Software created
+by or for InvenSense infringe any intellectual property right owned or
+controlled by Licensee.
+
+2.6.    No Other Rights Granted; Restrictions.  Apart from the license rights
+expressly set forth in this Agreement, InvenSense does not grant and Licensee
+does not receive any ownership right, title or interest nor any security
+interest or other interest in any intellectual property rights relating to
+the Software, nor in any copy of any part of the foregoing.  No license is
+granted to Licensee in any human readable code of the Software (source code).
+Licensee shall not (i) use, license, sell or otherwise distribute the
+Software except as provided in this Agreement, (ii) attempt to reverse
+engineer, decompile or disassemble any portion of the Software; or (iii) use
+the Software or other material in violation of any applicable law or
+regulation, including but not limited to any regulatory agency, such as FCC,
+rules.
+
+3.  NO WARRANTY OR SUPPORT
+
+3.1.    No Warranty. THE SOFTWARE IS OFFERED "AS IS," AND INVENSENSE GRANTS AND
+LICENSEE RECEIVES NO WARRANTIES OF ANY KIND, EXPRESS OR IMPLIED, BY STATUTE,
+COMMUNICATION OR CONDUCT WITH LICENSEE, OR OTHERWISE.  INVENSENSE SPECIFICALLY
+DISCLAIMS ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A SPECIFIC
+PURPOSE OR NONINFRINGEMENT CONCERNING THE SOFTWARE OR ANY UPGRADES TO OR
+DOCUMENTATION FOR THE SOFTWARE.  WITHOUT LIMITATION OF THE ABOVE, INVENSENSE
+GRANTS NO WARRANTY THAT THE SOFTWARE IS ERROR-FREE OR WILL OPERATE WITHOUT
+INTERRUPTION, AND GRANTS NO WARRANTY REGARDING ITS USE OR THE RESULTS
+THEREFROM INCLUDING, WITHOUT LIMITATION, ITS CORRECTNESS, ACCURACY OR
+RELIABILITY.
+
+3.2.    No Support.  Nothing in this agreement shall obligate InvenSense to
+provide any support for the Software. InvenSense may, but shall be under no
+obligation to, correct any defects in the Software and/or provide updates to
+licensees of the Software.  Licensee shall make reasonable efforts to
+promptly report to InvenSense any defects it finds in the Software, as an aid
+to creating improved revisions of the Software.
+
+3.3.    Dangerous Applications.  The Software is not designed, intended, or
+certified for use in components of systems intended for the operation of
+weapons, weapons systems, nuclear installations, means of mass
+transportation, aviation, life-support computers or equipment (including
+resuscitation equipment and surgical implants), pollution control, hazardous
+substances management, or for any other dangerous application in which the
+failure of the Software could create a situation where personal injury or
+death may occur.  Licensee understands that use of the Software in such
+applications is fully at the risk of Licensee.
+
+4.  TERM AND TERMINATION
+
+4.1.    Termination.  This Agreement will automatically terminate if Licensee
+fails to comply with any of the terms and conditions hereof. In such event,
+Licensee must destroy all copies of the Software and all of its component
+parts.
+
+4.2.    Effect Of Termination.  Upon any termination of this Agreement, the
+rights and licenses granted to Licensee under this Agreement shall
+immediately terminate.
+
+4.3.    Survival.  The rights and obligations under this Agreement which by
+their nature should survive termination will remain in effect after
+expiration or termination of this Agreement.
+
+5.  CONFIDENTIALITY
+
+5.1.    Obligations.  Licensee acknowledges and agrees that any documentation
+relating to the Software, and any other information (if such other
+information is identified as confidential or should be recognized as
+confidential under the circumstances) provided to Licensee by InvenSense
+hereunder (collectively, "Confidential Information") constitute the
+confidential and proprietary information of InvenSense, and that Licensee's
+protection thereof is an essential condition to Licensee's use and possession
+of the Software.  Licensee shall retain all Confidential Information in
+strict confidence and not disclose it to any third party or use it in any way
+except under a written agreement with terms and conditions at least as
+protective as the terms of this Section.  Licensee will exercise at least the
+same amount of diligence in preserving the secrecy of the Confidential
+Information as it uses in preserving the secrecy of its own most valuable
+confidential information, but in no event less than reasonable diligence.
+Information shall not be considered Confidential Information if and to the
+extent that it: (i) was in the public domain at the time it was disclosed or
+has entered the public domain through no fault of Licensee; (ii) was known to
+Licensee, without restriction, at the time of disclosure as proven by the
+files of Licensee in existence at the time of disclosure; or (iii) becomes
+known to Licensee, without restriction, from a source other than InvenSense
+without breach of this Agreement by Licensee and otherwise not in violation
+of InvenSense's rights.
+
+5.2.    Return of Confidential Information.  Notwithstanding the foregoing, all
+documents and other tangible objects containing or representing InvenSense
+Confidential Information and all copies thereof which are in the possession
+of Licensee shall be and remain the property of InvenSense, and shall be
+promptly returned to InvenSense upon written request by InvenSense or upon
+termination of this Agreement.
+
+6.  LIMITATION OF LIABILITY
+TO THE MAXIMUM EXTENT PERMITTED BY LAW, IN NO EVENT SHALL INVENSENSE OR ANY OF
+INVENSENSE'S LICENSORS HAVE ANY LIABILITY FOR ANY INDIRECT, INCIDENTAL,
+SPECIAL, OR CONSEQUENTIAL DAMAGES, HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER FOR BREACH OF CONTRACT, TORT (INCLUDING NEGLIGENCE) OR
+OTHERWISE, ARISING OUT OF THIS AGREEMENT, INCLUDING BUT NOT LIMITED TO LOSS
+OF PROFITS, EVEN IF SUCH PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES. IN NO EVENT WILL INVENSENSE'S LIABILITY WHETHER IN CONTRACT, TORT
+(INCLUDING NEGLIGENCE), OR OTHERWISE, EXCEED THE AMOUNT PAID BY LICENSEE FOR
+SOFTWARE UNDER THIS AGREEMENT.  THESE LIMITATIONS SHALL APPLY NOTWITHSTANDING
+ANY FAILURE OF ESSENTIAL PURPOSE OF ANY LIMITED REMEDY.
+
+7.  MISCELLANEOUS
+
+7.1.    Export Regulations. YOU UNDERSTAND AND AGREE THAT THE SOFTWARE IS
+SUBJECT TO UNITED STATES AND OTHER APPLICABLE EXPORT-RELATED LAWS AND
+REGULATIONS AND THAT YOU MAY NOT EXPORT, RE-EXPORT OR TRANSFER THE SOFTWARE
+OR ANY DIRECT PRODUCT OF THE SOFTWARE EXCEPT AS PERMITTED UNDER THOSE LAWS.
+WITHOUT LIMITING THE FOREGOING, EXPORT, RE-EXPORT OR TRANSFER OF THE SOFTWARE
+TO CUBA, IRAN, NORTH KOREA, SUDAN AND SYRIA IS PROHIBITED.
+
+7.2 Assignment. This Agreement shall be binding upon and inure to the
+benefit of the parties and their respective successors and assigns, provided,
+however that Licensee may not assign this Agreement or any rights or
+obligation hereunder, directly or indirectly, by operation of law or
+otherwise, without the prior written consent of InvenSense, and any such
+attempted assignment shall be void.  Notwithstanding the foregoing, Licensee
+may assign this Agreement to a successor to all or substantially all of its
+business or assets to which this Agreement relates that is not a competitor
+of InvenSense.
+
+7.3.    Governing Law; Venue.  This Agreement shall be governed by the laws of
+California without regard to any conflict-of-laws rules, and the United
+Nations Convention on Contracts for the International Sale of Goods is hereby
+excluded.  The sole jurisdiction and venue for actions related to the subject
+matter hereof shall be the state and federal courts located in the County of
+Orange, California, and both parties hereby consent to such jurisdiction and
+venue.
+
+7.4.    Severability.  All terms and provisions of this Agreement shall, if
+possible, be construed in a manner which makes them valid, but in the event
+any term or provision of this Agreement is found by a court of competent
+jurisdiction to be illegal or unenforceable, the validity or enforceability
+of the remainder of this Agreement shall not be affected if the illegal or
+unenforceable provision does not materially affect the intent of this
+Agreement.  If the illegal or unenforceable provision materially affects the
+intent of the parties to this Agreement, this Agreement shall become
+terminated.
+
+7.5.    Equitable Relief.  Licensee hereby acknowledges that its breach of this
+Agreement would cause irreparable harm and significant injury to InvenSense
+that may be difficult to ascertain and that a remedy at law would be
+inadequate.  Accordingly, Licensee agrees that InvenSense shall have the right
+to seek and obtain immediate injunctive relief to enforce obligations under
+the Agreement in addition to any other rights and remedies it may have.
+
+7.6.    Waiver.  The waiver of, or failure to enforce, any breach or default
+hereunder shall not constitute the waiver of any other or subsequent breach
+or default.
+
+7.7.    Entire Agreement.  This Agreement sets forth the entire Agreement
+between the parties and supersedes any and all prior proposals, agreements
+and representations between them, whether written or oral concerning the
+Software.  This Agreement may be changed only by mutual agreement of the
+parties in writing.
+
diff --git a/6515/libsensors_iio/MPLSensor.cpp b/6515/libsensors_iio/MPLSensor.cpp
new file mode 100644
index 0000000..9721734
--- /dev/null
+++ b/6515/libsensors_iio/MPLSensor.cpp
@@ -0,0 +1,6444 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#define LOG_NDEBUG 0
+
+//see also the EXTRA_VERBOSE define in the MPLSensor.h header file
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <float.h>
+#include <poll.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <stdlib.h>
+#include <sys/select.h>
+#include <sys/syscall.h>
+#include <dlfcn.h>
+#include <pthread.h>
+#include <cutils/log.h>
+#include <utils/KeyedVector.h>
+#include <utils/String8.h>
+#include <string.h>
+#include <linux/input.h>
+#include <utils/Atomic.h>
+
+#include "MPLSensor.h"
+#include "PressureSensor.IIO.secondary.h"
+#include "MPLSupport.h"
+#include "sensor_params.h"
+
+#include "invensense.h"
+#include "invensense_adv.h"
+#include "ml_stored_data.h"
+#include "ml_load_dmp.h"
+#include "ml_sysfs_helper.h"
+
+#define ENABLE_MULTI_RATE
+// #define TESTING
+#define USE_LPQ_AT_FASTEST
+
+#ifdef THIRD_PARTY_ACCEL
+#pragma message("HAL:build third party accel support")
+#define USE_THIRD_PARTY_ACCEL (1)
+#else
+#define USE_THIRD_PARTY_ACCEL (0)
+#endif
+
+#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))
+
+/******************************************************************************/
+/*  MPL Interface                                                             */
+/******************************************************************************/
+
+//#define INV_PLAYBACK_DBG
+#ifdef INV_PLAYBACK_DBG
+static FILE *logfile = NULL;
+#endif
+
+/*******************************************************************************
+ * MPLSensor class implementation
+ ******************************************************************************/
+
+static struct timespec mt_pre;
+
+// following extended initializer list would only be available with -std=c++11
+//  or -std=gnu+11
+MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *))
+                       : SensorBase(NULL, NULL),
+                         mMasterSensorMask(INV_ALL_SENSORS),
+                         mLocalSensorMask(0),
+                         mPollTime(-1),
+                         mStepCountPollTime(-1),
+                         mHaveGoodMpuCal(0),
+                         mGyroAccuracy(0),
+                         mAccelAccuracy(0),
+                         mCompassAccuracy(0),
+                         dmp_orient_fd(-1),
+                         mDmpOrientationEnabled(0),
+                         dmp_sign_motion_fd(-1),
+                         mDmpSignificantMotionEnabled(0),
+                         dmp_pedometer_fd(-1),
+                         mDmpPedometerEnabled(0),
+                         mDmpStepCountEnabled(0),
+                         mEnabled(0),
+                         mBatchEnabled(0),
+                         mFlushSensorEnabled(-1),
+                         mOldBatchEnabledMask(0),
+                         mAccelInputReader(4),
+                         mGyroInputReader(32),
+                         mTempScale(0),
+                         mTempOffset(0),
+                         mTempCurrentTime(0),
+                         mAccelScale(2),
+                         mAccelSelfTestScale(2),
+                         mGyroScale(2000),
+                         mGyroSelfTestScale(2000),
+                         mCompassScale(0),
+                         mFactoryGyroBiasAvailable(false),
+                         mGyroBiasAvailable(false),
+                         mGyroBiasApplied(false),
+                         mFactoryAccelBiasAvailable(false),
+                         mAccelBiasAvailable(false),
+                         mAccelBiasApplied(false),
+                         mPendingMask(0),
+                         mSensorMask(0),
+                         mMplFeatureActiveMask(0),
+                         mFeatureActiveMask(0),
+                         mDmpOn(0),
+                         mPedUpdate(0),
+                         mPressureUpdate(0),
+                         mQuatSensorTimestamp(0),
+                         mStepSensorTimestamp(0),
+                         mLastStepCount(0),
+                         mLeftOverBufferSize(0),
+                         mInitial6QuatValueAvailable(0),
+                         mFlushBatchSet(0),
+                         mSkipReadEvents(0),
+                         mDataMarkerDetected(0),
+                         mEmptyDataMarkerDetected(0) {
+    VFUNC_LOG;
+
+    inv_error_t rv;
+    int i, fd;
+    char *port = NULL;
+    char *ver_str;
+    unsigned long mSensorMask;
+    int res;
+    FILE *fptr;
+
+    mCompassSensor = compass;
+
+    LOGV_IF(EXTRA_VERBOSE,
+            "HAL:MPLSensor constructor : NumSensors = %d", NumSensors);
+
+    pthread_mutex_init(&mMplMutex, NULL);
+    pthread_mutex_init(&mHALMutex, NULL);
+    memset(mGyroOrientation, 0, sizeof(mGyroOrientation));
+    memset(mAccelOrientation, 0, sizeof(mAccelOrientation));
+    memset(mInitial6QuatValue, 0, sizeof(mInitial6QuatValue));
+
+    /* setup sysfs paths */
+    inv_init_sysfs_attributes();
+
+    /* get chip name */
+    if (inv_get_chip_name(chip_ID) != INV_SUCCESS) {
+        LOGE("HAL:ERR- Failed to get chip ID\n");
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Chip ID= %s\n", chip_ID);
+    }
+
+    enable_iio_sysfs();
+    
+    /* instantiate pressure sensor on secondary bus */   
+    mPressureSensor = new PressureSensor((const char*)mSysfsPath);
+   
+    /* reset driver master enable */
+    masterEnable(0);
+
+    /* Load DMP image if capable, ie. MPU6515 */
+    loadDMP();
+
+    /* open temperature fd for temp comp */
+    LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature);
+    gyro_temperature_fd = open(mpu.temperature, O_RDONLY);
+    if (gyro_temperature_fd == -1) {
+        LOGE("HAL:could not open temperature node");
+    } else {
+        LOGV_IF(EXTRA_VERBOSE,
+                "HAL:temperature_fd opened: %s", mpu.temperature);
+    }
+
+    /* read gyro FSR to calculate accel scale later */
+    char gyroBuf[5];
+    int count = 0;
+         LOGV_IF(SYSFS_VERBOSE,
+             "HAL:sysfs:cat %s (%lld)", mpu.gyro_fsr, getTimestamp());
+
+    fd = open(mpu.gyro_fsr, O_RDONLY);
+    if(fd < 0) {
+        LOGE("HAL:Error opening gyro FSR");
+    } else {
+        memset(gyroBuf, 0, sizeof(gyroBuf));
+        count = read_attribute_sensor(fd, gyroBuf, sizeof(gyroBuf));
+        if(count < 1) {
+            LOGE("HAL:Error reading gyro FSR");
+        } else {
+            count = sscanf(gyroBuf, "%ld", &mGyroScale);
+            if(count)
+                LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro FSR used %ld", mGyroScale);
+        }
+        close(fd);
+    }
+    
+    /* read gyro self test scale used to calculate factory cal bias later */
+    char gyroScale[5]; 
+         LOGV_IF(SYSFS_VERBOSE,
+             "HAL:sysfs:cat %s (%lld)", mpu.in_gyro_self_test_scale, getTimestamp());
+    fd = open(mpu.in_gyro_self_test_scale, O_RDONLY);
+    if(fd < 0) {
+        LOGE("HAL:Error opening gyro self test scale");
+    } else {
+        memset(gyroBuf, 0, sizeof(gyroBuf));
+        count = read_attribute_sensor(fd, gyroScale, sizeof(gyroScale));
+        if(count < 1) {
+            LOGE("HAL:Error reading gyro self test scale");
+        } else {
+            count = sscanf(gyroScale, "%ld", &mGyroSelfTestScale);
+            if(count)
+                LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro self test scale used %ld", mGyroSelfTestScale);
+        }
+        close(fd);
+    }
+
+    /* open Factory Gyro Bias fd */
+    /* mFactoryGyBias contains bias values that will be used for device offset */
+    memset(mFactoryGyroBias, 0, sizeof(mFactoryGyroBias));
+    LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro x offset path: %s", mpu.in_gyro_x_offset);
+    LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro y offset path: %s", mpu.in_gyro_y_offset);
+    LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro z offset path: %s", mpu.in_gyro_z_offset);   
+    gyro_x_offset_fd = open(mpu.in_gyro_x_offset, O_RDWR);
+    gyro_y_offset_fd = open(mpu.in_gyro_y_offset, O_RDWR);
+    gyro_z_offset_fd = open(mpu.in_gyro_z_offset, O_RDWR);
+    if (gyro_x_offset_fd == -1 ||
+             gyro_y_offset_fd == -1 || gyro_z_offset_fd == -1) {
+            LOGE("HAL:could not open factory gyro calibrated bias");
+    } else {
+        LOGV_IF(EXTRA_VERBOSE,
+             "HAL:gyro_offset opened");
+    }
+
+    /* open Gyro Bias fd */
+    /* mGyroBias contains bias values that will be used for framework */
+    /* mGyroChipBias contains bias values that will be used for dmp */
+    memset(mGyroBias, 0, sizeof(mGyroBias));
+    memset(mGyroChipBias, 0, sizeof(mGyroChipBias));
+    LOGV_IF(EXTRA_VERBOSE, "HAL: gyro x dmp bias path: %s", mpu.in_gyro_x_dmp_bias);
+    LOGV_IF(EXTRA_VERBOSE, "HAL: gyro y dmp bias path: %s", mpu.in_gyro_y_dmp_bias);
+    LOGV_IF(EXTRA_VERBOSE, "HAL: gyro z dmp bias path: %s", mpu.in_gyro_z_dmp_bias);
+    gyro_x_dmp_bias_fd = open(mpu.in_gyro_x_dmp_bias, O_RDWR);
+    gyro_y_dmp_bias_fd = open(mpu.in_gyro_y_dmp_bias, O_RDWR);
+    gyro_z_dmp_bias_fd = open(mpu.in_gyro_z_dmp_bias, O_RDWR);
+    if (gyro_x_dmp_bias_fd == -1 ||
+             gyro_y_dmp_bias_fd == -1 || gyro_z_dmp_bias_fd == -1) {
+            LOGE("HAL:could not open gyro DMP calibrated bias");
+    } else {
+        LOGV_IF(EXTRA_VERBOSE,
+             "HAL:gyro_dmp_bias opened");
+    }
+
+    /* read accel FSR to calcuate accel scale later */
+    if (USE_THIRD_PARTY_ACCEL == 0) {
+        char buf[3];
+        int count = 0;
+        LOGV_IF(SYSFS_VERBOSE,
+                "HAL:sysfs:cat %s (%lld)", mpu.accel_fsr, getTimestamp());
+
+        fd = open(mpu.accel_fsr, O_RDONLY);
+        if(fd < 0) {
+            LOGE("HAL:Error opening accel FSR");
+        } else {
+           memset(buf, 0, sizeof(buf));
+           count = read_attribute_sensor(fd, buf, sizeof(buf));
+           if(count < 1) {
+               LOGE("HAL:Error reading accel FSR");
+           } else {
+               count = sscanf(buf, "%d", &mAccelScale);
+               if(count)
+                   LOGV_IF(EXTRA_VERBOSE, "HAL:Accel FSR used %d", mAccelScale);
+           }
+           close(fd);
+        }
+
+        /* read accel self test scale used to calculate factory cal bias later */
+        char accelScale[5]; 
+             LOGV_IF(SYSFS_VERBOSE,
+                 "HAL:sysfs:cat %s (%lld)", mpu.in_accel_self_test_scale, getTimestamp());
+        fd = open(mpu.in_accel_self_test_scale, O_RDONLY);
+        if(fd < 0) {
+            LOGE("HAL:Error opening gyro self test scale");
+        } else {
+            memset(buf, 0, sizeof(buf));
+            count = read_attribute_sensor(fd, accelScale, sizeof(accelScale));
+            if(count < 1) {
+                LOGE("HAL:Error reading accel self test scale");
+            } else {
+                count = sscanf(accelScale, "%ld", &mAccelSelfTestScale);
+                if(count)
+                    LOGV_IF(EXTRA_VERBOSE, "HAL:Accel self test scale used %ld", mAccelSelfTestScale);
+            }
+            close(fd);
+        }
+ 
+        /* open Factory Accel Bias fd */
+        /* mFactoryAccelBias contains bias values that will be used for device offset */
+        memset(mFactoryAccelBias, 0, sizeof(mFactoryAccelBias));
+        LOGV_IF(EXTRA_VERBOSE, "HAL:factory accel x offset path: %s", mpu.in_accel_x_offset);
+        LOGV_IF(EXTRA_VERBOSE, "HAL:factory accel y offset path: %s", mpu.in_accel_y_offset);
+        LOGV_IF(EXTRA_VERBOSE, "HAL:factory accel z offset path: %s", mpu.in_accel_z_offset);   
+        accel_x_offset_fd = open(mpu.in_accel_x_offset, O_RDWR);
+        accel_y_offset_fd = open(mpu.in_accel_y_offset, O_RDWR);
+        accel_z_offset_fd = open(mpu.in_accel_z_offset, O_RDWR);
+        if (accel_x_offset_fd == -1 ||
+                 accel_y_offset_fd == -1 || accel_z_offset_fd == -1) {
+                LOGE("HAL:could not open factory accel calibrated bias");
+        } else {
+            LOGV_IF(EXTRA_VERBOSE,
+                 "HAL:accel_offset opened");
+        }
+        
+        /* open Accel Bias fd */
+        /* mAccelBias contains bias that will be used for dmp */
+        memset(mAccelBias, 0, sizeof(mAccelBias));
+        LOGV_IF(EXTRA_VERBOSE, "HAL:accel x dmp bias path: %s", mpu.in_accel_x_dmp_bias);
+        LOGV_IF(EXTRA_VERBOSE, "HAL:accel y dmp bias path: %s", mpu.in_accel_y_dmp_bias);
+        LOGV_IF(EXTRA_VERBOSE, "HAL:accel z dmp bias path: %s", mpu.in_accel_z_dmp_bias);
+        accel_x_dmp_bias_fd = open(mpu.in_accel_x_dmp_bias, O_RDWR);
+        accel_y_dmp_bias_fd = open(mpu.in_accel_y_dmp_bias, O_RDWR);
+        accel_z_dmp_bias_fd = open(mpu.in_accel_z_dmp_bias, O_RDWR);
+        if (accel_x_dmp_bias_fd == -1 ||
+                 accel_y_dmp_bias_fd == -1 || accel_z_dmp_bias_fd == -1) {
+            LOGE("HAL:could not open accel DMP calibrated bias");
+        } else {
+            LOGV_IF(EXTRA_VERBOSE,
+                 "HAL:accel_dmp_bias opened");
+        }
+    }
+
+    dmp_sign_motion_fd = open(mpu.event_smd, O_RDONLY | O_NONBLOCK);
+    if (dmp_sign_motion_fd < 0) {
+        LOGE("HAL:ERR couldn't open dmp_sign_motion node");
+    } else {
+        LOGV_IF(ENG_VERBOSE, 
+                "HAL:dmp_sign_motion_fd opened : %d", dmp_sign_motion_fd);
+    }
+#if 1
+    /* the following threshold can be modified for SMD sensitivity */
+    int motionThreshold = 3000;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                motionThreshold, mpu.smd_threshold, getTimestamp());
+        res = write_sysfs_int(mpu.smd_threshold, motionThreshold);
+#endif
+    dmp_pedometer_fd = open(mpu.event_pedometer, O_RDONLY | O_NONBLOCK);
+    if (dmp_pedometer_fd < 0) {
+        LOGE("HAL:ERR couldn't open dmp_pedometer node");
+    } else {
+        LOGV_IF(ENG_VERBOSE, 
+                "HAL:dmp_pedometer_fd opened : %d", dmp_pedometer_fd);
+    }
+
+    initBias();
+
+    (void)inv_get_version(&ver_str);
+    LOGI("%s\n", ver_str);
+
+    /* setup MPL */
+    inv_constructor_init();
+
+#ifdef INV_PLAYBACK_DBG
+    LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging");
+    logfile = fopen("/data/playback.bin", "w+");
+    if (logfile)
+        inv_turn_on_data_logging(logfile);
+#endif
+
+    /* setup orientation matrix and scale */
+    inv_set_device_properties();
+
+    /* initialize sensor data */
+    memset(mPendingEvents, 0, sizeof(mPendingEvents));
+
+    mPendingEvents[RotationVector].version = sizeof(sensors_event_t);
+    mPendingEvents[RotationVector].sensor = ID_RV;
+    mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR;
+    mPendingEvents[RotationVector].acceleration.status
+            = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[GameRotationVector].version = sizeof(sensors_event_t);
+    mPendingEvents[GameRotationVector].sensor = ID_GRV;
+    mPendingEvents[GameRotationVector].type = SENSOR_TYPE_GAME_ROTATION_VECTOR;
+    mPendingEvents[GameRotationVector].acceleration.status
+            = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[LinearAccel].version = sizeof(sensors_event_t);
+    mPendingEvents[LinearAccel].sensor = ID_LA;
+    mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
+    mPendingEvents[LinearAccel].acceleration.status
+            = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[Gravity].version = sizeof(sensors_event_t);
+    mPendingEvents[Gravity].sensor = ID_GR;
+    mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
+    mPendingEvents[Gravity].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[Gyro].version = sizeof(sensors_event_t);
+    mPendingEvents[Gyro].sensor = ID_GY;
+    mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
+    mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[RawGyro].version = sizeof(sensors_event_t);
+    mPendingEvents[RawGyro].sensor = ID_RG;
+    mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED;
+    mPendingEvents[RawGyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
+    mPendingEvents[Accelerometer].sensor = ID_A;
+    mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
+    mPendingEvents[Accelerometer].acceleration.status
+            = SENSOR_STATUS_ACCURACY_HIGH;
+
+    /* Invensense compass calibration */
+    mPendingEvents[MagneticField].version = sizeof(sensors_event_t);
+    mPendingEvents[MagneticField].sensor = ID_M;
+    mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD;
+    mPendingEvents[MagneticField].magnetic.status =
+        SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[RawMagneticField].version = sizeof(sensors_event_t);
+    mPendingEvents[RawMagneticField].sensor = ID_RM;
+    mPendingEvents[RawMagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED;
+    mPendingEvents[RawMagneticField].magnetic.status =
+        SENSOR_STATUS_ACCURACY_HIGH;
+        
+    mPendingEvents[Pressure].version = sizeof(sensors_event_t);
+    mPendingEvents[Pressure].sensor = ID_PS;
+    mPendingEvents[Pressure].type = SENSOR_TYPE_PRESSURE;
+    mPendingEvents[Pressure].magnetic.status =
+        SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[Orientation].version = sizeof(sensors_event_t);
+    mPendingEvents[Orientation].sensor = ID_O;
+    mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION;
+    mPendingEvents[Orientation].orientation.status
+            = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[GeomagneticRotationVector].version = sizeof(sensors_event_t);
+    mPendingEvents[GeomagneticRotationVector].sensor = ID_GMRV;
+    mPendingEvents[GeomagneticRotationVector].type
+            = SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR;
+    mPendingEvents[GeomagneticRotationVector].acceleration.status
+            = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mSmEvents.version = sizeof(sensors_event_t);
+    mSmEvents.sensor = ID_SM;
+    mSmEvents.type = SENSOR_TYPE_SIGNIFICANT_MOTION;
+    mSmEvents.acceleration.status = SENSOR_STATUS_UNRELIABLE;
+
+    mSdEvents.version = sizeof(sensors_event_t);
+    mSdEvents.sensor = ID_P;
+    mSdEvents.type = SENSOR_TYPE_STEP_DETECTOR;
+    mSdEvents.acceleration.status = SENSOR_STATUS_UNRELIABLE;
+
+    mScEvents.version = sizeof(sensors_event_t);
+    mScEvents.sensor = ID_SC;
+    mScEvents.type = SENSOR_TYPE_STEP_COUNTER;
+    mScEvents.acceleration.status = SENSOR_STATUS_UNRELIABLE;
+
+    /* Event Handlers for HW and Virtual Sensors */
+    mHandlers[RotationVector] = &MPLSensor::rvHandler;
+    mHandlers[GameRotationVector] = &MPLSensor::grvHandler;
+    mHandlers[LinearAccel] = &MPLSensor::laHandler;
+    mHandlers[Gravity] = &MPLSensor::gravHandler;
+    mHandlers[Gyro] = &MPLSensor::gyroHandler;
+    mHandlers[RawGyro] = &MPLSensor::rawGyroHandler;
+    mHandlers[Accelerometer] = &MPLSensor::accelHandler;
+    mHandlers[MagneticField] = &MPLSensor::compassHandler;
+    mHandlers[RawMagneticField] = &MPLSensor::rawCompassHandler;
+    mHandlers[Orientation] = &MPLSensor::orienHandler;
+    mHandlers[GeomagneticRotationVector] = &MPLSensor::gmHandler;
+    mHandlers[Pressure] = &MPLSensor::psHandler;
+
+    /* initialize delays to reasonable values */
+    for (int i = 0; i < NumSensors; i++) {
+        mDelays[i] = 1000000000LL;
+        mBatchDelays[i] = 1000000000LL;
+        mBatchTimeouts[i] = 100000000000LL;
+    }
+
+    /* initialize Compass Bias */
+    memset(mCompassBias, 0, sizeof(mCompassBias));
+
+    /* initialize Factory Accel Bias */
+    memset(mFactoryAccelBias, 0, sizeof(mFactoryAccelBias));
+
+    /* initialize Gyro Bias */
+    memset(mGyroBias, 0, sizeof(mGyroBias));
+    memset(mGyroChipBias, 0, sizeof(mGyroChipBias));    
+
+    /* load calibration file from /data/inv_cal_data.bin */
+    rv = inv_load_calibration();    
+    if(rv == INV_SUCCESS) {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded");
+        /* Get initial values */
+        getCompassBias();
+        getGyroBias();
+        if (mGyroBiasAvailable) {
+            setGyroBias();
+        }
+        getAccelBias();
+        getFactoryGyroBias();
+        if (mFactoryGyroBiasAvailable) {
+            setFactoryGyroBias();
+        }
+        getFactoryAccelBias();
+        if (mFactoryAccelBiasAvailable) {
+            setFactoryAccelBias();
+        }
+    }
+    else
+        LOGE("HAL:Could not open or load MPL calibration file (%d)", rv);
+
+    /* takes external accel calibration load workflow */
+    if( m_pt2AccelCalLoadFunc != NULL) {
+        long accel_offset[3];
+        long tmp_offset[3];
+        int result = m_pt2AccelCalLoadFunc(accel_offset);
+        if(result)
+            LOGW("HAL:Vendor accelerometer calibration file load failed %d\n",
+                 result);
+        else {
+            LOGW("HAL:Vendor accelerometer calibration file successfully "
+                 "loaded");
+            inv_get_mpl_accel_bias(tmp_offset, NULL);
+            LOGV_IF(PROCESS_VERBOSE,
+                    "HAL:Original accel offset, %ld, %ld, %ld\n",
+               tmp_offset[0], tmp_offset[1], tmp_offset[2]);
+            inv_set_accel_bias_mask(accel_offset, mAccelAccuracy,4);
+            inv_get_mpl_accel_bias(tmp_offset, NULL);
+            LOGV_IF(PROCESS_VERBOSE, "HAL:Set accel offset, %ld, %ld, %ld\n",
+               tmp_offset[0], tmp_offset[1], tmp_offset[2]);
+        }
+    }
+    /* end of external accel calibration load workflow */
+
+    /* disable all sensors and features */
+    masterEnable(0);
+    enableGyro(0);
+    enableLowPowerAccel(0);
+    enableAccel(0);
+    enableCompass(0,0);
+    enablePressure(0);
+    enableBatch(0);
+
+    if (isLowPowerQuatEnabled()) {
+        enableLPQuaternion(0);
+    }
+
+    if (isDmpDisplayOrientationOn()) {
+        // open DMP Orient Fd
+        openDmpOrientFd();
+        enableDmpOrientation(!isDmpScreenAutoRotationEnabled());
+    }
+}
+
+void MPLSensor::enable_iio_sysfs(void)
+{
+    VFUNC_LOG;
+
+    char iio_device_node[MAX_CHIP_ID_LEN];
+    FILE *tempFp = NULL;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)",
+            mpu.in_timestamp_en, getTimestamp());
+    // Either fopen()/open() are okay for sysfs access
+    // developers could choose what they want
+    // with fopen(), the benefit is that fprintf()/fscanf() are available
+    tempFp = fopen(mpu.in_timestamp_en, "w");
+    if (tempFp == NULL) {
+        LOGE("HAL:could not open timestamp enable");
+    } else {
+        if(fprintf(tempFp, "%d", 1) < 0 || fclose(tempFp) < 0) {
+            LOGE("HAL:could not enable timestamp");
+        }
+    }
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            IIO_BUFFER_LENGTH, mpu.buffer_length, getTimestamp());
+    tempFp = fopen(mpu.buffer_length, "w");
+    if (tempFp == NULL) {
+        LOGE("HAL:could not open buffer length");
+    } else {
+        if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0 || fclose(tempFp) < 0) {
+            LOGE("HAL:could not write buffer length");
+        }
+    }
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            1, mpu.chip_enable, getTimestamp());
+    tempFp = fopen(mpu.chip_enable, "w");
+    if (tempFp == NULL) {
+        LOGE("HAL:could not open chip enable");
+    } else {
+        if (fprintf(tempFp, "%d", 1) < 0 || fclose(tempFp) < 0) {
+            LOGE("HAL:could not write chip enable");
+        }
+    }
+    
+    inv_get_iio_device_node(iio_device_node);
+    iio_fd = open(iio_device_node, O_RDONLY);
+    if (iio_fd < 0) {
+        LOGE("HAL:could not open iio device node");
+    } else {
+        LOGV_IF(ENG_VERBOSE, "HAL:iio iio_fd opened : %d", iio_fd);
+    }
+}
+
+int MPLSensor::inv_constructor_init(void)
+{
+    VFUNC_LOG;
+
+    inv_error_t result = inv_init_mpl();
+    if (result) {
+        LOGE("HAL:inv_init_mpl() failed");
+        return result;
+    }
+    result = inv_constructor_default_enable();
+    result = inv_start_mpl();
+    if (result) {
+        LOGE("HAL:inv_start_mpl() failed");
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return result;
+}
+
+int MPLSensor::inv_constructor_default_enable(void)
+{
+    VFUNC_LOG;
+
+    inv_error_t result;
+
+/*******************************************************************************
+
+********************************************************************************
+
+The InvenSense binary file (libmplmpu.so) is subject to Google's standard terms
+and conditions as accepted in the click-through agreement required to download
+this library.
+The library includes, but is not limited to the following function calls:
+inv_enable_quaternion().
+
+ANY VIOLATION OF SUCH TERMS AND CONDITIONS WILL BE STRICTLY ENFORCED.
+
+********************************************************************************
+
+*******************************************************************************/
+
+    result = inv_enable_quaternion();
+    if (result) {
+        LOGE("HAL:Cannot enable quaternion\n");
+        return result;
+    }
+
+    result = inv_enable_in_use_auto_calibration();
+    if (result) {
+        return result;
+    }
+
+    result = inv_enable_fast_nomot();
+    if (result) {
+        return result;
+    }
+
+    result = inv_enable_gyro_tc();
+    if (result) {
+        return result;
+    }
+
+    result = inv_enable_hal_outputs();
+    if (result) {
+        return result;
+    }
+
+    if (!mCompassSensor->providesCalibration()) {
+        /* Invensense compass calibration */
+        LOGV_IF(ENG_VERBOSE, "HAL:Invensense vector compass cal enabled");
+        result = inv_enable_vector_compass_cal();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        } else {
+            mMplFeatureActiveMask |= INV_COMPASS_CAL;
+        }
+        // specify MPL's trust weight, used by compass algorithms
+        inv_vector_compass_cal_sensitivity(3);
+
+        /* disabled by default
+        result = inv_enable_compass_bias_w_gyro();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        */
+
+        result = inv_enable_heading_from_gyro();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        result = inv_enable_magnetic_disturbance();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        //inv_enable_magnetic_disturbance_logging();
+    }
+
+    result = inv_enable_9x_sensor_fusion();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    } else {
+        // 9x sensor fusion enables Compass fit
+        mMplFeatureActiveMask |= INV_COMPASS_FIT;
+    }
+
+    result = inv_enable_no_gyro_fusion();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return result;
+}
+
+/* TODO: create function pointers to calculate scale */
+void MPLSensor::inv_set_device_properties(void)
+{
+    VFUNC_LOG;
+
+    unsigned short orient;
+
+    inv_get_sensors_orientation();
+
+    inv_set_gyro_sample_rate(DEFAULT_MPL_GYRO_RATE);
+    inv_set_compass_sample_rate(DEFAULT_MPL_COMPASS_RATE);
+
+    /* gyro setup */
+    orient = inv_orientation_matrix_to_scalar(mGyroOrientation);
+    inv_set_gyro_orientation_and_scale(orient, mGyroScale << 15);
+    LOGI_IF(EXTRA_VERBOSE, "HAL: Set MPL Gyro Scale %ld", mGyroScale << 15);
+
+    /* accel setup */
+    orient = inv_orientation_matrix_to_scalar(mAccelOrientation);
+    /* use for third party accel input subsystem driver
+    inv_set_accel_orientation_and_scale(orient, 1LL << 22);
+    */
+    inv_set_accel_orientation_and_scale(orient, (long)mAccelScale << 15);
+    LOGI_IF(EXTRA_VERBOSE,
+            "HAL: Set MPL Accel Scale %ld", (long)mAccelScale << 15);
+
+    /* compass setup */
+    signed char orientMtx[9];
+    mCompassSensor->getOrientationMatrix(orientMtx);
+    orient =
+        inv_orientation_matrix_to_scalar(orientMtx);
+    long sensitivity;
+    sensitivity = mCompassSensor->getSensitivity();
+    inv_set_compass_orientation_and_scale(orient, sensitivity);
+    mCompassScale = sensitivity;
+    LOGI_IF(EXTRA_VERBOSE,
+            "HAL: Set MPL Compass Scale %ld", mCompassScale);
+}
+
+void MPLSensor::loadDMP(void)
+{
+    VFUNC_LOG;
+
+    int res, fd;
+    FILE *fptr;
+
+    if (isMpuNonDmp()) {
+        return;
+    }
+
+    /* load DMP firmware */
+    LOGV_IF(SYSFS_VERBOSE,
+            "HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp());
+    fd = open(mpu.firmware_loaded, O_RDONLY);
+    if(fd < 0) {
+        LOGE("HAL:could not open dmp state");
+    } else {
+        if(inv_read_dmp_state(fd) == 0) {
+            LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware);
+            fptr = fopen(mpu.dmp_firmware, "w");
+            if(fptr == NULL) {
+                LOGE("HAL:could not open dmp_firmware");
+            } else {
+                if (inv_load_dmp(fptr) < 0 || fclose(fptr) < 0) {
+                    LOGE("HAL:load DMP failed");
+                } else {
+                    LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded");
+                }
+            }
+        } else {
+            LOGV_IF(ENG_VERBOSE, "HAL:DMP is already loaded");
+        }
+    }
+
+    // onDmp(1);    //Can't enable here. See note onDmp()
+}
+
+void MPLSensor::inv_get_sensors_orientation(void)
+{
+    VFUNC_LOG;
+
+    FILE *fptr;
+
+    // get gyro orientation
+    LOGV_IF(SYSFS_VERBOSE,
+            "HAL:sysfs:cat %s (%lld)", mpu.gyro_orient, getTimestamp());
+    fptr = fopen(mpu.gyro_orient, "r");
+    if (fptr != NULL) {
+        int om[9];
+        if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
+               &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
+               &om[6], &om[7], &om[8]) < 0 || fclose(fptr) < 0) {
+            LOGE("HAL:Could not read gyro mounting matrix");
+        } else {
+            LOGV_IF(EXTRA_VERBOSE,
+                    "HAL:gyro mounting matrix: "
+                    "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
+                    om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
+
+            mGyroOrientation[0] = om[0];
+            mGyroOrientation[1] = om[1];
+            mGyroOrientation[2] = om[2];
+            mGyroOrientation[3] = om[3];
+            mGyroOrientation[4] = om[4];
+            mGyroOrientation[5] = om[5];
+            mGyroOrientation[6] = om[6];
+            mGyroOrientation[7] = om[7];
+            mGyroOrientation[8] = om[8];
+        }
+    }
+
+    // get accel orientation
+    LOGV_IF(SYSFS_VERBOSE,
+            "HAL:sysfs:cat %s (%lld)", mpu.accel_orient, getTimestamp());
+    fptr = fopen(mpu.accel_orient, "r");
+    if (fptr != NULL) {
+        int om[9];
+        if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
+            &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
+            &om[6], &om[7], &om[8]) < 0 || fclose(fptr) < 0) {
+            LOGE("HAL:could not read accel mounting matrix");
+        } else {
+            LOGV_IF(EXTRA_VERBOSE,
+           "HAL:accel mounting matrix: "
+           "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
+           om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
+
+           mAccelOrientation[0] = om[0];
+           mAccelOrientation[1] = om[1];
+           mAccelOrientation[2] = om[2];
+           mAccelOrientation[3] = om[3];
+           mAccelOrientation[4] = om[4];
+           mAccelOrientation[5] = om[5];
+           mAccelOrientation[6] = om[6];
+           mAccelOrientation[7] = om[7];
+           mAccelOrientation[8] = om[8];
+        }
+    }
+}
+
+MPLSensor::~MPLSensor()
+{
+    VFUNC_LOG;
+
+    /* Close open fds */
+    if (iio_fd > 0)
+        close(iio_fd);
+    if( accel_fd > 0 )
+        close(accel_fd );
+    if (gyro_temperature_fd > 0)
+        close(gyro_temperature_fd);
+    if (sysfs_names_ptr)
+        free(sysfs_names_ptr);
+
+    closeDmpOrientFd();
+
+    if (accel_x_dmp_bias_fd > 0) {
+        close(accel_x_dmp_bias_fd);
+    }
+    if (accel_y_dmp_bias_fd > 0) {
+        close(accel_y_dmp_bias_fd);
+    }
+    if (accel_z_dmp_bias_fd > 0) {
+        close(accel_z_dmp_bias_fd);
+    }
+
+    if (gyro_x_dmp_bias_fd > 0) {
+        close(gyro_x_dmp_bias_fd);
+    }
+    if (gyro_y_dmp_bias_fd > 0) {
+        close(gyro_y_dmp_bias_fd);
+    }
+    if (gyro_z_dmp_bias_fd > 0) {
+        close(gyro_z_dmp_bias_fd);
+    }
+
+    if (gyro_x_offset_fd > 0) {
+        close(gyro_x_dmp_bias_fd);
+    }
+    if (gyro_y_offset_fd > 0) {
+        close(gyro_y_offset_fd);
+    }
+    if (gyro_z_offset_fd > 0) {
+        close(accel_z_offset_fd);
+    }
+
+    /* Turn off Gyro master enable          */
+    /* A workaround until driver handles it */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            0, mpu.master_enable, getTimestamp());
+    write_sysfs_int(mpu.master_enable, 0);
+
+#ifdef INV_PLAYBACK_DBG
+    inv_turn_off_data_logging();
+    if (fclose(logfile) < 0) {
+        LOGE("cannot close debug log file");
+    }
+#endif
+}
+
+#define GY_ENABLED  ((1 << ID_GY) & enabled_sensors)
+#define RGY_ENABLED ((1 << ID_RG) & enabled_sensors)
+#define A_ENABLED   ((1 << ID_A)  & enabled_sensors)
+#define M_ENABLED   ((1 << ID_M) & enabled_sensors)
+#define RM_ENABLED  ((1 << ID_RM) & enabled_sensors)
+#define PS_ENABLED  ((1 << ID_PS) & enabled_sensors)
+#define O_ENABLED   ((1 << ID_O)  & enabled_sensors)
+#define LA_ENABLED  ((1 << ID_LA) & enabled_sensors)
+#define GR_ENABLED  ((1 << ID_GR) & enabled_sensors)
+#define RV_ENABLED  ((1 << ID_RV) & enabled_sensors)
+#define GRV_ENABLED ((1 << ID_GRV) & enabled_sensors)
+#define GMRV_ENABLED ((1 << ID_GMRV) & enabled_sensors)
+
+/* this applies to BMA250 Input Subsystem Driver only */
+int MPLSensor::setAccelInitialState()
+{
+    VFUNC_LOG;
+
+    struct input_absinfo absinfo_x;
+    struct input_absinfo absinfo_y;
+    struct input_absinfo absinfo_z;
+    float value;
+    if (!ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo_x) &&
+        !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo_y) &&
+        !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo_z)) {
+        value = absinfo_x.value;
+        mPendingEvents[Accelerometer].data[0] = value * CONVERT_A_X;
+        value = absinfo_y.value;
+        mPendingEvents[Accelerometer].data[1] = value * CONVERT_A_Y;
+        value = absinfo_z.value;
+        mPendingEvents[Accelerometer].data[2] = value * CONVERT_A_Z;
+        //mHasPendingEvent = true;
+    }
+    return 0;
+}
+
+int MPLSensor::onDmp(int en)
+{
+    VFUNC_LOG;
+
+    int res = -1;
+    int status;
+    mDmpOn = en;
+
+    //Sequence to enable DMP
+    //1. Load DMP image if not already loaded
+    //2. Either Gyro or Accel must be enabled/configured before next step
+    //3. Enable DMP
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
+            mpu.firmware_loaded, getTimestamp());
+    if(read_sysfs_int(mpu.firmware_loaded, &status) < 0){
+        LOGE("HAL:ERR can't get firmware_loaded status");
+    } else if (status == 1) {
+        //Write only if curr DMP state <> request
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
+                mpu.dmp_on, getTimestamp());
+        if (read_sysfs_int(mpu.dmp_on, &status) < 0) {
+            LOGE("HAL:ERR can't read DMP state");
+        } else if (status != en) {
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    en, mpu.dmp_on, getTimestamp());
+            if (write_sysfs_int(mpu.dmp_on, en) < 0) {
+                LOGE("HAL:ERR can't write dmp_on");
+            } else {
+                mDmpOn = en;
+                res = 0;    //Indicate write successful
+                if(!en) {
+                    setAccelBias();
+                }
+            }
+            //Enable DMP interrupt
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    en, mpu.dmp_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.dmp_int_on, en) < 0) {
+                LOGE("HAL:ERR can't en/dis DMP interrupt");
+            }
+
+            // disable DMP event interrupt if needed
+            if (!en) {
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        en, mpu.dmp_event_int_on, getTimestamp());
+                if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
+                    res = -1;
+                    LOGE("HAL:ERR can't enable DMP event interrupt");
+                }
+            }
+        } else {
+            mDmpOn = en;
+            res = 0;    //DMP already set as requested
+            if(!en) {
+                setAccelBias();
+            }
+        }
+    } else {
+        LOGE("HAL:ERR No DMP image");
+    }
+    return res;
+}
+
+int MPLSensor::setDmpFeature(int en)
+{
+    int res = 0;
+ 
+    // set sensor engine and fifo
+    if ((mFeatureActiveMask & DMP_FEATURE_MASK) || en) {
+        if ((mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION) ||
+                (mFeatureActiveMask & INV_DMP_PED_QUATERNION) ||
+                (mFeatureActiveMask & INV_DMP_QUATERNION)) {
+            res = enableGyro(1);
+            if (res < 0) {
+                return res;
+            }
+            if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_GYRO)) {
+                res = turnOffGyroFifo();
+                if (res < 0) {
+                    return res;
+                }
+            }
+        }
+        res = enableAccel(1);
+        if (res < 0) {
+            return res;
+        }
+        if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) {
+            res = turnOffAccelFifo();
+            if (res < 0) {
+                return res;
+            }
+        }
+    } else {
+        if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_GYRO)) {
+            res = enableGyro(0);
+            if (res < 0) {
+                return res;
+            }
+        }
+        if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) {
+            res = enableAccel(0);
+            if (res < 0) {
+                return res;
+            }
+        }
+    }
+
+    // set sensor data interrupt
+    uint32_t dataInterrupt = (mEnabled || (mFeatureActiveMask & INV_DMP_BATCH_MODE));
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                !dataInterrupt, mpu.dmp_event_int_on, getTimestamp());
+    if (write_sysfs_int(mpu.dmp_event_int_on, !dataInterrupt) < 0) {
+        res = -1;
+        LOGE("HAL:ERR can't enable DMP event interrupt");
+    }
+    return res;
+}
+
+int MPLSensor::computeAndSetDmpState()
+{
+    int res = 0;
+    bool dmpState = 0;
+    
+    if (mFeatureActiveMask) {
+        dmpState = 1;
+        LOGV_IF(PROCESS_VERBOSE, "HAL:computeAndSetDmpState() mFeatureActiveMask = 1");
+    } else if ((mEnabled & VIRTUAL_SENSOR_9AXES_MASK)
+                        || (mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK)) {
+            if (checkLPQuaternion() && checkLPQRateSupported()) {
+                dmpState = 1;
+                LOGV_IF(PROCESS_VERBOSE, "HAL:computeAndSetDmpState() Sensor Fusion = 1");
+            }
+    } /*else {
+        unsigned long sensor = mLocalSensorMask & mMasterSensorMask;    
+        if (sensor & (INV_THREE_AXIS_ACCEL & INV_THREE_AXIS_GYRO)) {
+            dmpState = 1;
+            LOGV_IF(PROCESS_VERBOSE, "HAL:computeAndSetDmpState() accel and gyro = 1");
+        }
+    }*/
+
+    // set Dmp state
+    res = onDmp(dmpState);
+    if (res < 0)
+        return res;
+
+    if (dmpState) {
+        // set DMP rate to 200Hz
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                200, mpu.accel_fifo_rate, getTimestamp());
+        if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) {
+            res = -1;
+            LOGE("HAL:ERR can't set rate to 200Hz");
+            return res;
+        }
+    }        
+    LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is set %s", (dmpState ? "on" : "off"));
+    return dmpState;
+}
+
+/* called when batch and hw sensor enabled*/
+int MPLSensor::enablePedIndicator(int en)
+{
+    VFUNC_LOG;
+    
+    int res = 0;
+    if (en) {
+        if (!(mFeatureActiveMask & INV_DMP_PED_QUATERNION)) { 
+            //Disable DMP Pedometer Interrupt
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        0, mpu.pedometer_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) {
+               LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
+               res = -1;   // indicate an err
+               return res;
+            }
+        }
+    }
+    
+    LOGV_IF(ENG_VERBOSE, "HAL:Toggling step indicator to %d", en);
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                en, mpu.step_indicator_on, getTimestamp());
+    if (write_sysfs_int(mpu.step_indicator_on, en) < 0) {
+        res = -1;
+        LOGE("HAL:ERR can't write to DMP step_indicator_on");
+    }
+    return res;
+}
+
+int MPLSensor::checkPedStandaloneEnabled(void)
+{
+    VFUNC_LOG;
+    return ((mFeatureActiveMask & INV_DMP_PED_STANDALONE)? 1:0);
+}
+
+/* This feature is only used in batch mode */
+/* Stand-alone Step Detector */
+int MPLSensor::enablePedStandalone(int en)
+{
+    VFUNC_LOG;
+
+    if (!en) {
+        enablePedStandaloneData(0);
+        mFeatureActiveMask &= ~INV_DMP_PED_STANDALONE;
+        if (mFeatureActiveMask == 0) {
+            onDmp(0);
+        } else if(mFeatureActiveMask & INV_DMP_PEDOMETER) {
+             //Re-enable DMP Pedometer Interrupt
+             LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                     1, mpu.pedometer_int_on, getTimestamp());
+             if (write_sysfs_int(mpu.pedometer_int_on, 1) < 0) {
+                 LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
+                 return (-1);
+             }
+            //Disable data interrupt if no continuous data
+            if (mEnabled == 0) {
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                       1, mpu.dmp_event_int_on, getTimestamp());
+                if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) {
+                    LOGE("HAL:ERR can't enable DMP event interrupt");
+                    return (-1);
+                }
+            }
+        }
+        LOGV_IF(ENG_VERBOSE, "HAL:Ped Standalone disabled");
+    } else {
+        if (enablePedStandaloneData(1) < 0 || onDmp(1) < 0) {
+            LOGE("HAL:ERR can't enable Ped Standalone");
+        } else {
+            mFeatureActiveMask |= INV_DMP_PED_STANDALONE;
+            //Disable DMP Pedometer Interrupt
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    0, mpu.pedometer_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) {
+                LOGE("HAL:ERR can't disable Android Pedometer Interrupt");
+                return (-1);
+            }
+            //Enable Data Interrupt
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                       0, mpu.dmp_event_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
+                LOGE("HAL:ERR can't enable DMP event interrupt");
+                return (-1);
+            }
+            LOGV_IF(ENG_VERBOSE, "HAL:Ped Standalone enabled");
+        }
+    }
+    return 0;
+}
+
+int MPLSensor:: enablePedStandaloneData(int en)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    // Set DMP Ped standalone
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.step_detector_on, getTimestamp());
+    if (write_sysfs_int(mpu.step_detector_on, en) < 0) {
+        LOGE("HAL:ERR can't write DMP step_detector_on");
+        res = -1;   //Indicate an err
+    }
+
+    // Set DMP Step indicator
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.step_indicator_on, getTimestamp());
+    if (write_sysfs_int(mpu.step_indicator_on, en) < 0) {
+        LOGE("HAL:ERR can't write DMP step_indicator_on");
+        res = -1;   //Indicate an err
+    }
+
+    if (!en) {
+      LOGV_IF(ENG_VERBOSE, "HAL:Disabling ped standalone");
+      //Disable Accel if no sensor needs it
+      if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
+                               && (!(mLocalSensorMask & mMasterSensorMask
+                                                   & INV_THREE_AXIS_ACCEL))) {
+          res = enableAccel(0);
+          if (res < 0)
+              return res;
+      }
+      if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
+                               && (!(mLocalSensorMask & mMasterSensorMask
+                                                   & INV_THREE_AXIS_GYRO))) {
+          res = enableGyro(0);
+          if (res < 0)
+              return res;
+      }
+    } else {
+        LOGV_IF(ENG_VERBOSE, "HAL:Enabling ped standalone");
+        // enable accel engine
+        res = enableAccel(1);
+        if (res < 0)
+            return res;
+        LOGV_IF(EXTRA_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask);
+        // disable accel FIFO
+        if (!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_ACCEL)) {
+            res = turnOffAccelFifo();
+            if (res < 0)
+                return res;           
+        }
+    }
+
+    return res;
+}
+
+int MPLSensor::checkPedQuatEnabled(void)
+{
+    VFUNC_LOG;
+    return ((mFeatureActiveMask & INV_DMP_PED_QUATERNION)? 1:0);
+}
+
+/* This feature is only used in batch mode */
+/* Step Detector && Game Rotation Vector */
+int MPLSensor::enablePedQuaternion(int en)
+{
+    VFUNC_LOG;
+
+    if (!en) {
+        enablePedQuaternionData(0);
+        mFeatureActiveMask &= ~INV_DMP_PED_QUATERNION;
+        if (mFeatureActiveMask == 0) {
+            onDmp(0);
+        } else if(mFeatureActiveMask & INV_DMP_PEDOMETER) {
+             //Re-enable DMP Pedometer Interrupt
+             LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                     1, mpu.pedometer_int_on, getTimestamp());
+             if (write_sysfs_int(mpu.pedometer_int_on, 1) < 0) {
+                 LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
+                 return (-1);
+             }
+            //Disable data interrupt if no continuous data
+            if (mEnabled == 0) {
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                       1, mpu.dmp_event_int_on, getTimestamp());
+                if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
+                    LOGE("HAL:ERR can't enable DMP event interrupt");
+                    return (-1);
+                }
+            }
+        }
+        LOGV_IF(ENG_VERBOSE, "HAL:Ped Quat disabled");
+    } else {
+        if (enablePedQuaternionData(1) < 0 || onDmp(1) < 0) {
+            LOGE("HAL:ERR can't enable Ped Quaternion");
+        } else {
+            mFeatureActiveMask |= INV_DMP_PED_QUATERNION;
+            //Disable DMP Pedometer Interrupt
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    0, mpu.pedometer_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) {
+                LOGE("HAL:ERR can't disable Android Pedometer Interrupt");
+                return (-1);
+            }
+            //Enable Data Interrupt
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                       0, mpu.dmp_event_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
+                LOGE("HAL:ERR can't enable DMP event interrupt");
+                return (-1);
+            }
+            LOGV_IF(ENG_VERBOSE, "HAL:Ped Quat enabled");
+        }
+    }
+    return 0;
+}
+
+int MPLSensor::enablePedQuaternionData(int en)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    // Enable DMP quaternion
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.ped_q_on, getTimestamp());
+    if (write_sysfs_int(mpu.ped_q_on, en) < 0) {
+        LOGE("HAL:ERR can't write DMP ped_q_on");
+        res = -1;   //Indicate an err
+    }
+
+    if (!en) {
+        LOGV_IF(ENG_VERBOSE, "HAL:Disabling ped quat");
+        //Disable Accel if no sensor needs it
+        if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
+                               && (!(mLocalSensorMask & mMasterSensorMask
+                                                   & INV_THREE_AXIS_ACCEL))) {
+          res = enableAccel(0);
+          if (res < 0)
+              return res;
+        }
+        if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
+                               && (!(mLocalSensorMask & mMasterSensorMask
+                                                   & INV_THREE_AXIS_GYRO))) {
+            res = enableGyro(0);
+            if (res < 0)
+                return res;
+        }
+        if (mFeatureActiveMask & INV_DMP_QUATERNION) {
+            res = write_sysfs_int(mpu.gyro_fifo_enable, 1);
+            res += write_sysfs_int(mpu.accel_fifo_enable, 1);
+            if (res < 0)
+                return res;
+        }
+        // LOGV_IF(ENG_VERBOSE, "before mLocalSensorMask=0x%lx", mLocalSensorMask);
+        // reset global mask for buildMpuEvent()
+        if (mEnabled & (1 << GameRotationVector)) {
+            mLocalSensorMask |= INV_THREE_AXIS_GYRO;
+            mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
+        } else if (mEnabled & (1 << Accelerometer)) {
+            mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
+        } else if ((mEnabled & ( 1 << Gyro)) ||
+            (mEnabled & (1 << RawGyro))) {
+            mLocalSensorMask |= INV_THREE_AXIS_GYRO; 
+        }
+        //LOGV_IF(ENG_VERBOSE, "after mLocalSensorMask=0x%lx", mLocalSensorMask); 
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling ped quat");
+        // enable accel engine
+        res = enableAccel(1);
+        if (res < 0)
+            return res;
+
+        // enable gyro engine
+        res = enableGyro(1);
+        if (res < 0)
+            return res;
+        LOGV_IF(EXTRA_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask);
+        // disable accel FIFO
+        if ((!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_ACCEL)) ||
+                !(mBatchEnabled & (1 << Accelerometer))) {
+            res = turnOffAccelFifo();
+            if (res < 0)
+                return res;
+            mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
+        }
+
+        // disable gyro FIFO
+        if ((!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_GYRO)) ||
+                !((mBatchEnabled & (1 << Gyro)) || (mBatchEnabled & (1 << RawGyro)))) {
+            res = turnOffGyroFifo();
+            if (res < 0)
+                return res;
+            mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
+        }
+    }
+
+    return res;
+}
+
+int MPLSensor::check6AxisQuatEnabled(void)
+{
+    VFUNC_LOG;
+    return ((mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION)? 1:0);
+}
+
+/* This is used for batch mode only */
+/* GRV is batched but not along with ped */
+int MPLSensor::enable6AxisQuaternion(int en)
+{
+    VFUNC_LOG;
+
+    if (!en) {
+        enable6AxisQuaternionData(0);
+        mFeatureActiveMask &= ~INV_DMP_6AXIS_QUATERNION;
+        if (mFeatureActiveMask == 0) {
+            onDmp(0);
+        }
+        LOGV_IF(ENG_VERBOSE, "HAL:6 Axis Quat disabled");
+    } else {
+        if (enable6AxisQuaternionData(1) < 0 || onDmp(1) < 0) {
+            LOGE("HAL:ERR can't enable 6 Axis Quaternion");
+        } else {
+            mFeatureActiveMask |= INV_DMP_6AXIS_QUATERNION;
+            LOGV_IF(PROCESS_VERBOSE, "HAL:6 Axis Quat enabled");
+        }
+    }
+    return 0;
+}
+
+int MPLSensor::enable6AxisQuaternionData(int en)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    // Enable DMP quaternion
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.six_axis_q_on, getTimestamp());
+    if (write_sysfs_int(mpu.six_axis_q_on, en) < 0) {
+        LOGE("HAL:ERR can't write DMP six_axis_q_on");
+        res = -1;   //Indicate an err
+    }
+
+    if (!en) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:DMP six axis quaternion data was turned off");
+        inv_quaternion_sensor_was_turned_off();
+        if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
+                                 && (!(mLocalSensorMask & mMasterSensorMask
+                                                     & INV_THREE_AXIS_ACCEL))) {
+            res = enableAccel(0);
+            if (res < 0)
+                return res;
+        }
+        if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
+                                 && (!(mLocalSensorMask & mMasterSensorMask
+                                                     & INV_THREE_AXIS_GYRO))) {
+            res = enableGyro(0);
+            if (res < 0)
+                return res;
+        }
+        if (mFeatureActiveMask & INV_DMP_QUATERNION) {
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    1, mpu.gyro_fifo_enable, getTimestamp());
+            res = write_sysfs_int(mpu.gyro_fifo_enable, 1);
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    1, mpu.accel_fifo_enable, getTimestamp());
+            res += write_sysfs_int(mpu.accel_fifo_enable, 1);
+            if (res < 0)
+                return res;
+        }
+        LOGV_IF(ENG_VERBOSE, "  k=0x%lx", mLocalSensorMask);
+        // reset global mask for buildMpuEvent()
+        if (mEnabled & (1 << GameRotationVector)) {
+            if (!(mFeatureActiveMask & INV_DMP_PED_QUATERNION)) {
+                mLocalSensorMask |= INV_THREE_AXIS_GYRO;        
+                mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
+                res = write_sysfs_int(mpu.gyro_fifo_enable, 1);
+                res += write_sysfs_int(mpu.accel_fifo_enable, 1);
+                if (res < 0)
+                    return res;
+            }
+        } else if (mEnabled & (1 << Accelerometer)) {
+            mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
+        } else if ((mEnabled & ( 1 << Gyro)) ||
+                (mEnabled & (1 << RawGyro))) {
+            mLocalSensorMask |= INV_THREE_AXIS_GYRO; 
+        }
+        LOGV_IF(ENG_VERBOSE, "after mLocalSensorMask=0x%lx", mLocalSensorMask);
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling six axis quat");
+        if (mEnabled & ( 1 << GameRotationVector)) {
+            // enable accel engine
+            res = enableAccel(1);
+            if (res < 0)
+                return res;
+
+            // enable gyro engine
+            res = enableGyro(1);
+            if (res < 0)
+                return res;
+            LOGV_IF(EXTRA_VERBOSE, "before: mLocalSensorMask=0x%lx", mLocalSensorMask);
+            if ((!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) ||
+                   (!(mBatchEnabled & (1 << Accelerometer)) ||
+                       (!(mEnabled & (1 << Accelerometer))))) {
+                res = turnOffAccelFifo();
+                if (res < 0)
+                    return res;
+                mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
+            }
+            
+            if ((!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_GYRO)) ||
+                    (!(mBatchEnabled & (1 << Gyro)) ||
+                       (!(mEnabled & (1 << Gyro))))) {
+                if (!(mBatchEnabled & (1 << RawGyro)) ||
+                        (!(mEnabled & (1 << RawGyro)))) {
+                    res = turnOffGyroFifo();
+                    if (res < 0)
+                        return res;
+                     mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
+                     }
+            }
+            LOGV_IF(ENG_VERBOSE, "after: mLocalSensorMask=0x%lx", mLocalSensorMask);
+        }
+    }
+
+    return res;
+}
+
+/* this is for batch  mode only */
+int MPLSensor::checkLPQRateSupported(void)
+{
+    VFUNC_LOG;
+#ifndef USE_LPQ_AT_FASTEST
+    return ((mDelays[GameRotationVector] <= RATE_200HZ) ? 0 :1);
+#else
+    return 1;
+#endif
+}
+
+int MPLSensor::checkLPQuaternion(void)
+{
+    VFUNC_LOG;
+    return ((mFeatureActiveMask & INV_DMP_QUATERNION)? 1:0);
+}
+
+int MPLSensor::enableLPQuaternion(int en)
+{
+    VFUNC_LOG;
+
+    if (!en) {
+        enableQuaternionData(0);
+        mFeatureActiveMask &= ~INV_DMP_QUATERNION;
+        if (mFeatureActiveMask == 0) {
+            onDmp(0);
+        }
+        LOGV_IF(ENG_VERBOSE, "HAL:LP Quat disabled");
+    } else {
+        if (enableQuaternionData(1) < 0 || onDmp(1) < 0) {
+            LOGE("HAL:ERR can't enable LP Quaternion");
+        } else {
+            mFeatureActiveMask |= INV_DMP_QUATERNION;
+            LOGV_IF(ENG_VERBOSE, "HAL:LP Quat enabled");
+        }
+    }
+    return 0;
+}
+
+int MPLSensor::enableQuaternionData(int en)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    // Enable DMP quaternion
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.three_axis_q_on, getTimestamp());
+    if (write_sysfs_int(mpu.three_axis_q_on, en) < 0) {
+        LOGE("HAL:ERR can't write DMP three_axis_q__on");
+        res = -1;   //Indicates an err
+    }
+
+    if (!en) {
+        LOGV_IF(ENG_VERBOSE, "HAL:DMP quaternion data was turned off");
+        inv_quaternion_sensor_was_turned_off();
+    } else {
+        LOGV_IF(ENG_VERBOSE, "HAL:Enabling three axis quat");
+    }
+
+    return res;
+}
+
+int MPLSensor::enableDmpPedometer(int en, int interruptMode)
+{
+    VFUNC_LOG;
+    int res = 0;
+    int enabled_sensors = mEnabled;
+
+    if (isMpuNonDmp())
+        return res;
+
+    // reset master enable
+    res = masterEnable(0);
+    if (res < 0) {
+        return res;
+    }
+
+    if (en == 1) {
+        //Enable DMP Pedometer Function
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                en, mpu.pedometer_on, getTimestamp());
+        if (write_sysfs_int(mpu.pedometer_on, en) < 0) {
+            LOGE("HAL:ERR can't enable Android Pedometer");
+            res = -1;   // indicate an err
+            return res;
+        }
+
+        if (interruptMode || (mFeatureActiveMask & INV_DMP_PEDOMETER)) {
+            //Enable DMP Pedometer Interrupt
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    en, mpu.pedometer_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.pedometer_int_on, en) < 0) {
+                LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
+                res = -1;   // indicate an err
+                return res;
+            }
+        }
+        
+        if (interruptMode) {
+            mFeatureActiveMask |= INV_DMP_PEDOMETER;
+        }
+        else {
+            mFeatureActiveMask |= INV_DMP_PEDOMETER_STEP;
+            mStepCountPollTime = 100000000LL;
+        }
+
+        clock_gettime(CLOCK_MONOTONIC, &mt_pre);
+    } else {
+        if (interruptMode) {
+            mFeatureActiveMask &= ~INV_DMP_PEDOMETER;
+        }
+        else {
+            mFeatureActiveMask &= ~INV_DMP_PEDOMETER_STEP;
+            mStepCountPollTime = -1;
+        }
+
+        /* if neither step detector or step count is on */
+        if (!(mFeatureActiveMask & (INV_DMP_PEDOMETER | INV_DMP_PEDOMETER_STEP))) {
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    en, mpu.pedometer_on, getTimestamp());
+            if (write_sysfs_int(mpu.pedometer_on, en) < 0) {
+                LOGE("HAL:ERR can't enable Android Pedometer");
+                res = -1;
+                return res;
+            }        
+        } 
+
+        /* if feature is not step detector */
+        if (!(mFeatureActiveMask & INV_DMP_PEDOMETER)) {
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    en, mpu.pedometer_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.pedometer_int_on, en) < 0) {
+                LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
+                res = -1;
+                return res;
+            }
+        }       
+    }
+
+    if ((res = setDmpFeature(en)) < 0)
+        return res;
+
+    if ((res = computeAndSetDmpState()) < 0)
+        return res;
+  
+    if (resetDataRates() < 0)
+        return res;
+
+    if(en || enabled_sensors || mFeatureActiveMask) {
+        res = masterEnable(1);
+    }
+    return res;
+}
+
+int MPLSensor::masterEnable(int en)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.master_enable, getTimestamp());
+    res = write_sysfs_int(mpu.master_enable, en);
+    return res;
+}
+
+int MPLSensor::enableGyro(int en)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    /* need to also turn on/off the master enable */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.gyro_enable, getTimestamp());
+    res = write_sysfs_int(mpu.gyro_enable, en);
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.gyro_fifo_enable, getTimestamp());
+    res += write_sysfs_int(mpu.gyro_fifo_enable, en);
+
+    if (!en) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off");
+        inv_gyro_was_turned_off();
+    }
+
+    return res;
+}
+
+int MPLSensor::enableLowPowerAccel(int en)
+{
+    VFUNC_LOG;
+    
+    int res;
+    
+    /* need to also turn on/off the master enable */
+    res = write_sysfs_int(mpu.motion_lpa_on, en);
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        en, mpu.motion_lpa_on, getTimestamp());  
+    return res;  
+}
+
+int MPLSensor::enableAccel(int en)
+{
+    VFUNC_LOG;
+
+    int res;
+
+    /* need to also turn on/off the master enable */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.accel_enable, getTimestamp());
+    res = write_sysfs_int(mpu.accel_enable, en);
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.accel_fifo_enable, getTimestamp());
+    res += write_sysfs_int(mpu.accel_fifo_enable, en);
+
+    if (!en) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off");
+        inv_accel_was_turned_off();
+    }
+
+    return res;
+}
+
+int MPLSensor::enableCompass(int en, int rawSensorRequested)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+    /* TODO: handle ID_RM if third party compass cal is used */
+    if (rawSensorRequested && mCompassSensor->providesCalibration()) {
+        res = mCompassSensor->enable(ID_RM, en);
+    } else {
+        res = mCompassSensor->enable(ID_M, en);
+    }
+    if (en == 0 || res != 0) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off %d", res);
+        inv_compass_was_turned_off();
+    }
+
+    return res;
+}
+
+int MPLSensor::enablePressure(int en)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    if (mPressureSensor) 
+        res = mPressureSensor->enable(ID_PS, en);
+    
+    return res;
+}
+
+/* use this function for initialization */
+int MPLSensor::enableBatch(int64_t timeout)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    res = write_sysfs_int(mpu.batchmode_timeout, timeout);
+    if (timeout == 0) {
+        res = write_sysfs_int(mpu.six_axis_q_on, 0);
+        res = write_sysfs_int(mpu.ped_q_on, 0);
+        res = write_sysfs_int(mpu.step_detector_on, 0);
+        res = write_sysfs_int(mpu.step_indicator_on, 0);
+    }
+
+    if (timeout == 0) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:batchmode timeout is zero");      
+    }
+
+    return res;
+}
+
+void MPLSensor::computeLocalSensorMask(int enabled_sensors)
+{
+    VFUNC_LOG;
+
+    do {
+        /* Invensense Pressure on secondary bus */
+        if (PS_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "PS ENABLED");
+            mLocalSensorMask |= INV_ONE_AXIS_PRESSURE;
+        } else {
+            LOGV_IF(ENG_VERBOSE, "PS DISABLED");
+            mLocalSensorMask &= ~INV_ONE_AXIS_PRESSURE;
+        }
+        
+        if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED
+                       || (GRV_ENABLED && GMRV_ENABLED)) {
+            LOGV_IF(ENG_VERBOSE, "FUSION ENABLED");
+            mLocalSensorMask |= ALL_MPL_SENSORS_NP;
+            break;
+        }
+
+        if (GRV_ENABLED) {
+            if (!(mFeatureActiveMask & INV_DMP_BATCH_MODE) ||
+                !(mBatchEnabled & (1 << GameRotationVector))) {
+                LOGV_IF(ENG_VERBOSE, "6 Axis Fusion ENABLED");
+                mLocalSensorMask |= INV_THREE_AXIS_GYRO;
+                mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
+            } else {
+                if (GY_ENABLED || RGY_ENABLED) {
+                    LOGV_IF(ENG_VERBOSE, "G ENABLED");
+                    mLocalSensorMask |= INV_THREE_AXIS_GYRO;
+                } else {
+                    LOGV_IF(ENG_VERBOSE, "G DISABLED");
+                    mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
+                }
+                if (A_ENABLED) {
+                    LOGV_IF(ENG_VERBOSE, "A ENABLED");
+                    mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
+                } else {
+                    LOGV_IF(ENG_VERBOSE, "A DISABLED");
+                    mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
+                }
+            }
+            /* takes care of MAG case */
+            if (M_ENABLED || RM_ENABLED) {
+                LOGV_IF(1, "M ENABLED");
+                mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
+            } else {
+                LOGV_IF(1, "M DISABLED");
+                mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS;
+            }
+            break;
+        }
+
+        if (GMRV_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "6 Axis Geomagnetic Fusion ENABLED");
+            mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
+            mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
+
+            /* takes care of Gyro case */
+            if (GY_ENABLED || RGY_ENABLED) {
+                LOGV_IF(1, "G ENABLED");
+                mLocalSensorMask |= INV_THREE_AXIS_GYRO;
+            } else {
+                LOGV_IF(1, "G DISABLED");
+                mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
+            }
+            break;
+        }
+
+        if(!A_ENABLED && !M_ENABLED && !RM_ENABLED &&
+               !GRV_ENABLED && !GMRV_ENABLED && !GY_ENABLED && !RGY_ENABLED &&
+               !PS_ENABLED) {
+            /* Invensense compass cal */
+            LOGV_IF(ENG_VERBOSE, "ALL DISABLED");
+            mLocalSensorMask = 0;
+            break;
+        }
+
+        if (GY_ENABLED || RGY_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "G ENABLED");
+            mLocalSensorMask |= INV_THREE_AXIS_GYRO;
+        } else {
+            LOGV_IF(ENG_VERBOSE, "G DISABLED");
+            mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
+        }
+
+        if (A_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "A ENABLED");
+            mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
+        } else {
+            LOGV_IF(ENG_VERBOSE, "A DISABLED");
+            mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
+        }
+
+        /* Invensense compass calibration */
+        if (M_ENABLED || RM_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "M ENABLED");
+            mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
+        } else {
+            LOGV_IF(ENG_VERBOSE, "M DISABLED");
+            mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS;
+        }        
+    } while (0);
+}
+
+int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed)
+{
+    VFUNC_LOG;
+
+    inv_error_t res = -1;
+    int on = 1;
+    int off = 0;
+    int cal_stored = 0;
+
+    // Sequence to enable or disable a sensor
+    // 1. reset master enable (=0)
+    // 2. enable or disable a sensor
+    // 3. set master enable (=1)
+
+    if (isLowPowerQuatEnabled() ||
+        changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
+        (mCompassSensor->isIntegrated() << MagneticField) |
+        (mCompassSensor->isIntegrated() << RawMagneticField) |
+        (mPressureSensor->isIntegrated() << Pressure))) {
+
+        /* reset master enable */
+        res = masterEnable(0);
+        if(res < 0) {
+            return res;
+        }
+    }
+
+    LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - sensors: 0x%0x",
+            (unsigned int)sensors);
+
+    if (changed & ((1 << Gyro) | (1 << RawGyro))) {
+        LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - gyro %s",
+                (sensors & INV_THREE_AXIS_GYRO? "enable": "disable"));
+        res = enableGyro(!!(sensors & INV_THREE_AXIS_GYRO));
+        if(res < 0) {
+            return res;
+        }
+
+        if (!cal_stored && (!en && (changed & (1 << Gyro)))) {
+            storeCalibration();
+            cal_stored = 1;
+        }
+    }
+
+    if (changed & (1 << Accelerometer)) {
+        LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - accel %s",
+                (sensors & INV_THREE_AXIS_ACCEL? "enable": "disable"));
+        res = enableAccel(!!(sensors & INV_THREE_AXIS_ACCEL));
+        if(res < 0) {
+            return res;
+        }
+
+        if (!(sensors & INV_THREE_AXIS_ACCEL) && !cal_stored) {
+            storeCalibration();
+            cal_stored = 1;
+        }
+    }
+
+    if (changed & ((1 << MagneticField) | (1 << RawMagneticField))) {
+        LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - compass %s",
+                (sensors & INV_THREE_AXIS_COMPASS? "enable": "disable"));
+        res = enableCompass(!!(sensors & INV_THREE_AXIS_COMPASS), changed & (1 << RawMagneticField));
+        if(res < 0) {
+            return res;
+        }
+
+        if (!cal_stored && (!en && (changed & (1 << MagneticField)))) {
+            storeCalibration();
+            cal_stored = 1;
+        }
+    }
+    
+     if (changed & (1 << Pressure)) {
+        LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - pressure %s",
+                (sensors & INV_ONE_AXIS_PRESSURE? "enable": "disable"));
+        res = enablePressure(!!(sensors & INV_ONE_AXIS_PRESSURE));
+        if(res < 0) {
+            return res;
+        }
+    }
+
+    if (isLowPowerQuatEnabled()) {
+        // Enable LP Quat
+        if ((mEnabled & VIRTUAL_SENSOR_9AXES_MASK)
+                      || (mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK)) {
+            LOGV_IF(ENG_VERBOSE, "HAL: 9 axis or game rot enabled");
+            if (!(changed & ((1 << Gyro)
+                           | (1 << RawGyro)
+                           | (1 << Accelerometer)
+                           | (mCompassSensor->isIntegrated() << MagneticField)
+                           | (mCompassSensor->isIntegrated() << RawMagneticField)))
+            ) {
+                /* reset master enable */
+                res = masterEnable(0);
+                if(res < 0) {
+                    return res;
+                }
+            }
+            if (!checkLPQuaternion()) {
+                enableLPQuaternion(1);
+            } else {
+                LOGV_IF(ENG_VERBOSE, "HAL:LP Quat already enabled");
+            }
+        } else if (checkLPQuaternion()) {
+            enableLPQuaternion(0);
+        }
+    }
+
+    /* apply accel/gyro bias to DMP bias                        */
+    /* precondition: masterEnable(0), mGyroBiasAvailable=true   */
+    /* postcondition: bias is applied upon masterEnable(1)      */
+    if(!(sensors & INV_THREE_AXIS_GYRO)) {
+        setGyroBias();
+    }
+    if(!(sensors & INV_THREE_AXIS_ACCEL)) {
+        setAccelBias();
+    }
+
+    /* to batch or not to batch */
+    int batchMode = computeBatchSensorMask(mEnabled, mBatchEnabled);
+    /* skip setBatch if there is no need to */
+    if(((int)mOldBatchEnabledMask != batchMode) || batchMode) {
+        setBatch(batchMode,0);
+    } 
+    mOldBatchEnabledMask = batchMode;
+
+    /* check for invn hardware sensors change */
+    if (changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
+            (mCompassSensor->isIntegrated() << MagneticField) |
+            (mCompassSensor->isIntegrated() << RawMagneticField) |
+            (mPressureSensor->isIntegrated() << Pressure))) {
+        LOGV_IF(ENG_VERBOSE,
+                "HAL DEBUG: Gyro, Accel, Compass, Pressure changes");
+        if ((checkSmdSupport() == 1) || (checkPedometerSupport() == 1) || (sensors &
+            (INV_THREE_AXIS_GYRO
+                | INV_THREE_AXIS_ACCEL
+                | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated())
+                | (INV_ONE_AXIS_PRESSURE * mPressureSensor->isIntegrated())))) {
+            LOGV_IF(ENG_VERBOSE, "SMD or Hardware sensors enabled");
+            LOGV_IF(ENG_VERBOSE, 
+                    "mFeatureActiveMask=0x%llx", mFeatureActiveMask);
+                LOGV_IF(ENG_VERBOSE, "HAL DEBUG: LPQ, SMD, SO enabled");
+                // disable DMP event interrupt only (w/ data interrupt)
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        0, mpu.dmp_event_int_on, getTimestamp());
+                if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
+                    res = -1;
+                    LOGE("HAL:ERR can't disable DMP event interrupt");
+                    return res;
+                }
+            LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=0x%llx", mFeatureActiveMask);
+            LOGV_IF(ENG_VERBOSE, "DMP_FEATURE_MASK=0x%x", DMP_FEATURE_MASK);
+        if ((mFeatureActiveMask & (long long)DMP_FEATURE_MASK) &&
+                    !((mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION) ||
+                     (mFeatureActiveMask & INV_DMP_PED_STANDALONE) ||
+                     (mFeatureActiveMask & INV_DMP_PED_QUATERNION) ||
+                     (mFeatureActiveMask & INV_DMP_BATCH_MODE))) {
+                // enable DMP
+                onDmp(1);
+                res = enableAccel(on);
+                if(res < 0) {
+                    return res;
+                }
+                LOGV_IF(ENG_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask);
+                if (((sensors | mLocalSensorMask) & INV_THREE_AXIS_ACCEL) == 0) {
+                    res = turnOffAccelFifo();
+                }
+                if(res < 0) {
+                    return res;
+                }
+            }
+        } else { // all sensors idle
+            LOGV_IF(ENG_VERBOSE, "HAL DEBUG: not SMD or Hardware sensors");
+            if (isDmpDisplayOrientationOn()
+                    && (mDmpOrientationEnabled
+                            || !isDmpScreenAutoRotationEnabled())) {
+                enableDmpOrientation(1);
+            }
+
+            if (!cal_stored) {
+                storeCalibration();
+                cal_stored = 1;
+            }
+        }
+    } else if ((changed &
+                    ((!mCompassSensor->isIntegrated()) << MagneticField) ||
+                    ((!mCompassSensor->isIntegrated()) << RawMagneticField))
+                    &&
+              !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL
+                | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated()))))
+    ) {
+        LOGV_IF(ENG_VERBOSE, "HAL DEBUG: Gyro, Accel, Compass no change");
+        if (!cal_stored) {
+            storeCalibration();
+            cal_stored = 1;
+        }
+    } /*else {
+      LOGV_IF(ENG_VERBOSE, "HAL DEBUG: mEnabled");
+      if (sensors &
+            (INV_THREE_AXIS_GYRO
+                | INV_THREE_AXIS_ACCEL
+                | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
+            res = masterEnable(1);
+            if(res < 0)
+                return res;
+        }
+    }*/
+
+    if (!batchMode && (resetDataRates() < 0)) {
+        LOGE("HAL:ERR can't reset output rate back to original setting");
+    }
+
+    if(mFeatureActiveMask || sensors) {
+        res = masterEnable(1);
+        if(res < 0)
+            return res;
+    }
+    return res;
+}
+
+/* check if batch mode should be turned on or not */
+int MPLSensor::computeBatchSensorMask(int enableSensors, int tempBatchSensor)
+{
+    VFUNC_LOG;
+    int batchMode = 1;
+    mFeatureActiveMask &= ~INV_DMP_BATCH_MODE;
+
+    LOGV_IF(ENG_VERBOSE,
+            "HAL:computeBatchSensorMask: enableSensors=%d tempBatchSensor=%d",
+            enableSensors, tempBatchSensor);
+
+    // handle initialization case
+    if (enableSensors == 0 && tempBatchSensor == 0)
+        return 0;
+            
+    // check for possible continuous data mode
+    for(int i = 0; i <= Pressure; i++) {
+        // if any one of the hardware sensor is in continuous data mode
+        // turn off batch mode.
+        if ((enableSensors & (1 << i)) && !(tempBatchSensor & (1 << i))) {
+            LOGV_IF(ENG_VERBOSE, "HAL:computeBatchSensorMask: "
+                    "hardware sensor on continuous mode:%d", i);
+            return 0;
+        }
+        if ((enableSensors & (1 << i)) && (tempBatchSensor & (1 << i))) {
+            LOGV_IF(ENG_VERBOSE, 
+                    "HAL:computeBatchSensorMask: hardware sensor is batch:%d", 
+                    i);
+            // if hardware sensor is batched, check if virtual sensor is also batched
+            if ((enableSensors & (1 << GameRotationVector))
+                            && !(tempBatchSensor & (1 << GameRotationVector))) {
+            LOGV_IF(ENG_VERBOSE, 
+                    "HAL:computeBatchSensorMask: but virtual sensor is not:%d", 
+                    i);
+                return 0;
+            }
+        }
+    }
+
+    // if virtual sensors are on but not batched, turn off batch mode.
+    for(int i = Orientation; i <= GeomagneticRotationVector; i++) {
+        if ((enableSensors & (1 << i)) && !(tempBatchSensor & (1 << i))) {
+             LOGV_IF(ENG_VERBOSE, "HAL:computeBatchSensorMask: "
+                     "composite sensor on continuous mode:%d", i);
+            return 0;
+        }
+    }
+
+    if ((mFeatureActiveMask & INV_DMP_PEDOMETER) && !(tempBatchSensor & (1 << StepDetector))) {
+        LOGV("HAL:computeBatchSensorMask: step detector on continuous mode.");
+        return 0;
+    }
+    
+    mFeatureActiveMask |= INV_DMP_BATCH_MODE;
+    LOGV_IF(EXTRA_VERBOSE,
+            "HAL:computeBatchSensorMask: batchMode=%d, mBatchEnabled=%0x", 
+            batchMode, tempBatchSensor);
+    return (batchMode && tempBatchSensor);
+}
+
+/* This function is called by enable() */
+int MPLSensor::setBatch(int en, int toggleEnable)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+    int64_t wanted = 1000000000LL;
+    int64_t timeout = 0;
+    int timeoutInMs = 0;
+    int featureMask = computeBatchDataOutput();
+
+    // reset master enable
+    if (toggleEnable == 1) {
+        res = masterEnable(0);
+        if (res < 0) {
+            return res;
+        }
+    }
+
+    if (en) {
+        /* take the minimum batchmode timeout */
+        int64_t timeout = 100000000000LL;
+        int64_t ns;
+        for (int i = 0; i < NumSensors; i++) {
+            LOGV_IF(0, "mFeatureActiveMask=0x%016llx, mEnabled=0x%01x, mBatchEnabled=0x%x",
+                            mFeatureActiveMask, mEnabled, mBatchEnabled);
+            if (((mEnabled & (1 << i)) && (mBatchEnabled & (1 << i))) ||
+                    (((featureMask & INV_DMP_PED_STANDALONE) && (mBatchEnabled & (1 << StepDetector))))) {
+                LOGV_IF(ENG_VERBOSE, "sensor=%d, timeout=%lld", i, mBatchTimeouts[i]);
+                ns = mBatchTimeouts[i];
+                timeout = (ns < timeout) ? ns : timeout;
+            }
+        }
+        /* Convert ns to millisecond */
+        timeoutInMs = timeout / 1000000; 
+    } else {
+        timeoutInMs = 0;
+    }    
+
+    LOGV_IF(ENG_VERBOSE, "HAL: batch timeout set to %dms", timeoutInMs);
+
+    /* step detector is enabled and */
+    /* batch mode is standalone */
+    if (en && (mFeatureActiveMask & INV_DMP_PEDOMETER) &&
+            (featureMask & INV_DMP_PED_STANDALONE)) {
+        LOGV_IF(ENG_VERBOSE, "ID_P only = 0x%x", mBatchEnabled);
+        enablePedStandalone(1);
+    } else {
+        enablePedStandalone(0);
+    }
+    
+    /* step detector and GRV are enabled and */
+    /* batch mode is ped q */
+    if (en && (mFeatureActiveMask & INV_DMP_PEDOMETER) &&
+            (mEnabled & (1 << GameRotationVector)) &&
+            (featureMask & INV_DMP_PED_QUATERNION)) {
+        LOGV_IF(ENG_VERBOSE, "ID_P and GRV or ALL = 0x%x", mBatchEnabled);
+        LOGV_IF(ENG_VERBOSE, "ID_P is enabled for batching, "
+                "PED quat will be automatically enabled");
+        enableLPQuaternion(0);
+        enablePedQuaternion(1);
+    } else if (!(featureMask & INV_DMP_PED_STANDALONE)){
+        enablePedQuaternion(0);
+    } else {
+        enablePedQuaternion(0);
+    }
+
+    /* step detector and hardware sensors enabled */
+    if (en && (featureMask & INV_DMP_PED_INDICATOR) &&
+            ((mEnabled) ||
+            (mFeatureActiveMask & INV_DMP_PED_STANDALONE))) {
+        enablePedIndicator(1);
+    } else {
+        enablePedIndicator(0);
+    }
+    
+    /* GRV is enabled and */
+    /* batch mode is 6axis q */
+    if (en && (mEnabled & (1 << GameRotationVector)) &&
+            (featureMask & INV_DMP_6AXIS_QUATERNION)) {
+        LOGV_IF(ENG_VERBOSE, "GRV = 0x%x", mBatchEnabled);
+        enableLPQuaternion(0);
+        enable6AxisQuaternion(1);
+        setInitial6QuatValue();
+    } else if (!(featureMask & INV_DMP_PED_QUATERNION)){
+        LOGV_IF(ENG_VERBOSE, "Toggle back to normal 6 axis");
+        if (mEnabled & (1 << GameRotationVector)) {
+            enableLPQuaternion(checkLPQRateSupported());
+        }
+        enable6AxisQuaternion(0);
+    } else {
+        enable6AxisQuaternion(0);
+    }
+    
+    /* write required timeout to sysfs */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            timeoutInMs, mpu.batchmode_timeout, getTimestamp());
+    if (write_sysfs_int(mpu.batchmode_timeout, timeoutInMs) < 0) {
+        LOGE("HAL:ERR can't write batchmode_timeout");
+    }
+
+    if (en) {
+        // enable DMP
+        res = onDmp(1);
+        if (res < 0) {
+            return res;
+        }
+
+        // set batch rates
+        if (setBatchDataRates() < 0) {
+            LOGE("HAL:ERR can't set batch data rates");
+        }
+        
+        // default fifo rate to 200Hz
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                200, mpu.gyro_fifo_rate, getTimestamp());
+        if (write_sysfs_int(mpu.gyro_fifo_rate, 200) < 0) {
+            res = -1;
+            LOGE("HAL:ERR can't set rate to 200Hz");
+            return res;
+        }
+    } else {
+        if (mFeatureActiveMask == 0) {
+            // disable DMP
+            res = onDmp(0);
+            if (res < 0) {
+                return res;
+            }
+            /* reset sensor rate */
+            if (resetDataRates() < 0) {
+                LOGE("HAL:ERR can't reset output rate back to original setting");
+            }
+        }
+    }
+
+    // set sensor data interrupt
+    uint32_t dataInterrupt = (mEnabled || (mFeatureActiveMask & INV_DMP_BATCH_MODE));
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                !dataInterrupt, mpu.dmp_event_int_on, getTimestamp());
+    if (write_sysfs_int(mpu.dmp_event_int_on, !dataInterrupt) < 0) {
+        res = -1;
+        LOGE("HAL:ERR can't enable DMP event interrupt");
+    }
+
+    if (toggleEnable == 1) {
+        if (mFeatureActiveMask || mEnabled)
+            masterEnable(1);
+    }
+    return res;
+}
+
+/* Store calibration file */
+void MPLSensor::storeCalibration(void)
+{
+    VFUNC_LOG;
+
+    if(mHaveGoodMpuCal == true
+        || mAccelAccuracy >= 2
+        || mCompassAccuracy >= 3) {
+       int res = inv_store_calibration();
+       if (res) {
+           LOGE("HAL:Cannot store calibration on file");
+       } else {
+           LOGV_IF(PROCESS_VERBOSE, "HAL:Cal file updated");
+       }
+    }
+}
+
+/*  these handlers transform mpl data into one of the Android sensor types */
+int MPLSensor::gyroHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_gyroscope(s->gyro.v, &s->gyro.status,
+                                           &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:gyro data : %+f %+f %+f -- %lld - %d",
+            s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::rawGyroHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_gyroscope_raw(s->uncalibrated_gyro.uncalib,
+                                               &s->gyro.status, &s->timestamp);
+    if(update) {
+        memcpy(s->uncalibrated_gyro.bias, mGyroBias, sizeof(mGyroBias));
+        LOGV_IF(HANDLER_DATA,"HAL:gyro bias data : %+f %+f %+f -- %lld - %d",
+            s->uncalibrated_gyro.bias[0], s->uncalibrated_gyro.bias[1],
+            s->uncalibrated_gyro.bias[2], s->timestamp, update);
+    }
+    s->gyro.status = SENSOR_STATUS_UNRELIABLE;
+    LOGV_IF(HANDLER_DATA, "HAL:raw gyro data : %+f %+f %+f -- %lld - %d",
+            s->uncalibrated_gyro.uncalib[0], s->uncalibrated_gyro.uncalib[1],
+            s->uncalibrated_gyro.uncalib[2], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::accelHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_accelerometer(
+        s->acceleration.v, &s->acceleration.status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d",
+            s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2],
+            s->timestamp, update);
+    mAccelAccuracy = s->acceleration.status;
+    return update;
+}
+
+int MPLSensor::compassHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_magnetic_field(
+        s->magnetic.v, &s->magnetic.status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d",
+            s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2],
+            s->timestamp, update);
+    mCompassAccuracy = s->magnetic.status;
+    return update;
+}
+
+int MPLSensor::rawCompassHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    //TODO: need to handle uncalib data and bias for 3rd party compass
+    if(mCompassSensor->providesCalibration()) {
+        update = mCompassSensor->readRawSample(s->uncalibrated_magnetic.uncalib, &s->timestamp);
+    }
+    else {
+        update = inv_get_sensor_type_magnetic_field_raw(s->uncalibrated_magnetic.uncalib,
+                     &s->magnetic.status, &s->timestamp);
+    }
+    if(update) {
+        memcpy(s->uncalibrated_magnetic.bias, mCompassBias, sizeof(mCompassBias));
+        LOGV_IF(HANDLER_DATA, "HAL:compass bias data: %+f %+f %+f -- %lld - %d",
+                s->uncalibrated_magnetic.bias[0], s->uncalibrated_magnetic.bias[1],
+                s->uncalibrated_magnetic.bias[2], s->timestamp, update);
+    }
+    s->magnetic.status = SENSOR_STATUS_UNRELIABLE;
+    LOGV_IF(HANDLER_DATA, "HAL:compass raw data: %+f %+f %+f %d -- %lld - %d",
+        s->uncalibrated_magnetic.uncalib[0], s->uncalibrated_magnetic.uncalib[1],
+                    s->uncalibrated_magnetic.uncalib[2], s->magnetic.status, s->timestamp, update);
+    return update;
+}
+
+/*
+    Rotation Vector handler.
+    NOTE: rotation vector does not have an accuracy or status
+*/
+int MPLSensor::rvHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int8_t status;
+    int update;
+    update = inv_get_sensor_type_rotation_vector(s->data, &status,
+                                                 &s->timestamp);
+    update |= isCompassDisabled();
+    LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f %+f- %+lld - %d",
+            s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->timestamp,
+            update);
+
+    return update;
+}
+
+/*
+    Game Rotation Vector handler.
+    NOTE: rotation vector does not have an accuracy or status
+*/
+int MPLSensor::grvHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int8_t status;
+    int update;
+    update = inv_get_sensor_type_rotation_vector_6_axis(s->data, &status,
+                                                     &s->timestamp);    
+                                                  
+    LOGV_IF(HANDLER_DATA, "HAL:grv data: %+f %+f %+f %+f %+f - %+lld - %d",
+            s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->timestamp,
+            update);
+    return update;
+}
+
+int MPLSensor::laHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_linear_acceleration(
+            s->gyro.v, &s->gyro.status, &s->timestamp);
+    update |= isCompassDisabled();
+    LOGV_IF(HANDLER_DATA, "HAL:la data: %+f %+f %+f - %lld - %d",
+            s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::gravHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_gravity(s->gyro.v, &s->gyro.status,
+                                         &s->timestamp);
+    update |= isCompassDisabled();
+    LOGV_IF(HANDLER_DATA, "HAL:gr data: %+f %+f %+f - %lld - %d",
+            s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::orienHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_orientation(
+            s->orientation.v, &s->orientation.status, &s->timestamp);
+    update |= isCompassDisabled();
+    LOGV_IF(HANDLER_DATA, "HAL:or data: %f %f %f - %lld - %d",
+            s->orientation.v[0], s->orientation.v[1], s->orientation.v[2],
+            s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::smHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update = 1;
+
+    /* When event is triggered, set data to 1 */
+    s->data[0] = 1.f;
+    s->data[1] = 0.f;
+    s->data[2] = 0.f;
+
+    /* Capture timestamp in HAL */
+    struct timespec ts;
+    clock_gettime(CLOCK_MONOTONIC, &ts);
+    s->timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
+
+    LOGV_IF(HANDLER_DATA, "HAL:sm data: %f - %lld - %d",
+            s->data[0], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::gmHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int8_t status;
+    int update = 0;
+    update = inv_get_sensor_type_geomagnetic_rotation_vector(s->data, &status,
+                                                             &s->timestamp);
+
+    LOGV_IF(HANDLER_DATA, "HAL:gm data: %+f %+f %+f %+f %+f- %+lld - %d",
+            s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->timestamp, update);
+    return update < 1 ? 0 :1;
+
+}
+
+int MPLSensor::psHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int8_t status;
+    int update = 0;
+
+    s->pressure = mCachedPressureData / 100.f; //hpa (millibar)
+    s->data[1] = 0;
+    s->data[2] = 0;
+    s->timestamp = mPressureTimestamp;
+    s->magnetic.status = 0;
+    update = mPressureUpdate;
+    mPressureUpdate = 0;
+
+    LOGV_IF(HANDLER_DATA, "HAL:ps data: %+f %+f %+f %+f- %+lld - %d",
+            s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update);
+    return update < 1 ? 0 :1;
+
+}
+
+int MPLSensor::sdHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update = 1;
+
+    /* When event is triggered, set data to 1 */
+    s->data[0] = 1;
+    s->data[1] = 0.f;
+    s->data[2] = 0.f;
+        
+    /* get current timestamp */
+    struct timespec ts;
+    clock_gettime(CLOCK_MONOTONIC, &ts) ;
+    s->timestamp = (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec;
+
+    LOGV_IF(HANDLER_DATA, "HAL:sd data: %f - %lld - %d",
+            s->data[0], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::scHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update = 1;
+ 
+    /* Set step count */
+#if defined ANDROID_KITKAT
+    s->u64.step_counter = mLastStepCount;
+    LOGV_IF(HANDLER_DATA, "HAL:sc data: %lld - %lld - %d",
+            s->u64.step_counter, s->timestamp, update);
+#else
+    s->step_counter = mLastStepCount;
+    LOGV_IF(HANDLER_DATA, "HAL:sc data: %lld - %lld - %d",
+            s->step_counter, s->timestamp, update);
+#endif
+    return update;
+}
+
+int MPLSensor::metaHandler(sensors_event_t* s, int flags)
+{
+    VHANDLER_LOG;
+    int update = 0;
+
+#if defined ANDROID_KITKAT
+    /* initalize SENSOR_TYPE_META_DATA */
+    s->version = 0;
+    s->sensor = 0;
+    s->reserved0 = 0;
+    s->timestamp = 0LL;
+    
+    switch(flags) {
+    case META_DATA_FLUSH_COMPLETE:
+        update = mFlushBatchSet;
+        s->type = SENSOR_TYPE_META_DATA;
+        s->meta_data.what = flags;
+        s->meta_data.sensor = mFlushSensorEnabled;
+        mFlushBatchSet = 0;
+        mFlushSensorEnabled = -1;
+        LOGV_IF(HANDLER_DATA,
+                "HAL:flush complete data: type=%d what=%d, "
+                "sensor=%d - %lld - %d",
+                s->type, s->meta_data.what, s->meta_data.sensor,
+                s->timestamp, update);
+        break;
+    
+    default:
+        LOGW("HAL: Meta flags not supported");
+        break;
+    }
+#endif 
+    return update;
+}
+
+int MPLSensor::enable(int32_t handle, int en)
+{
+    VFUNC_LOG;
+
+    android::String8 sname;
+    int what = -1, err = 0;
+    int batchMode = 0;
+
+    if (uint32_t(handle) >= NumSensors)
+        return -EINVAL;
+
+    if (!en)
+        mBatchEnabled &= ~(1 << handle);
+
+    LOGV_IF(ENG_VERBOSE, "HAL:handle = %d", handle);
+
+    switch (handle) {
+    case ID_SC:
+         what = StepCounter;
+         sname = "Step Counter";
+         LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
+                sname.string(), handle,
+                (mDmpStepCountEnabled? "en": "dis"),
+                (en? "en" : "dis"));
+        enableDmpPedometer(en, 0);
+        mDmpStepCountEnabled = !!en;
+        return 0;
+    case ID_P:
+        sname = "StepDetector";
+        LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
+                sname.string(), handle,
+                (mDmpPedometerEnabled? "en": "dis"),
+                (en? "en" : "dis"));
+        enableDmpPedometer(en, 1);
+        mDmpPedometerEnabled = !!en;
+        batchMode = computeBatchSensorMask(mEnabled, mBatchEnabled);
+        /* skip setBatch if there is no need to */
+        if(((int)mOldBatchEnabledMask != batchMode) || batchMode) {
+            setBatch(batchMode,1);
+        }
+        mOldBatchEnabledMask = batchMode;
+        return 0;
+    case ID_SM:
+        sname = "Significant Motion";
+        LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
+                sname.string(), handle,
+                (mDmpSignificantMotionEnabled? "en": "dis"),
+                (en? "en" : "dis"));
+        enableDmpSignificantMotion(en);
+        mDmpSignificantMotionEnabled = !!en;
+        return 0;
+    case ID_SO:
+        sname = "Screen Orientation";
+        LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
+                sname.string(), handle,
+                (mDmpOrientationEnabled? "en": "dis"),
+                (en? "en" : "dis"));
+        enableDmpOrientation(en && isDmpDisplayOrientationOn());
+        mDmpOrientationEnabled = !!en;
+        return 0;
+    case ID_A:
+        what = Accelerometer;
+        sname = "Accelerometer";
+        break;
+    case ID_M:
+        what = MagneticField;
+        sname = "MagneticField";
+        break;
+    case ID_RM:
+        what = RawMagneticField;
+        sname = "MagneticField Uncalibrated";
+        break;
+    case ID_O:
+        what = Orientation;
+        sname = "Orientation";
+        break;
+    case ID_GY:
+        what = Gyro;
+        sname = "Gyro";
+        break;
+    case ID_RG:
+        what = RawGyro;
+        sname = "Gyro Uncalibrated";
+        break;
+    case ID_GR:
+        what = Gravity;
+        sname = "Gravity";
+        break;
+    case ID_RV:
+        what = RotationVector;
+        sname = "RotationVector";
+        break;
+    case ID_GRV:
+        what = GameRotationVector;
+        sname = "GameRotationVector";
+        break;
+    case ID_LA:
+        what = LinearAccel;
+        sname = "LinearAccel";
+        break;
+    case ID_GMRV:
+        what = GeomagneticRotationVector;
+        sname = "GeomagneticRotationVector";
+        break;
+    case ID_PS:
+        what = Pressure;
+        sname = "Pressure";
+        break;
+    default:
+        what = handle;
+        sname = "Others";
+        break;
+    }    
+
+    int newState = en ? 1 : 0;
+    unsigned long sen_mask;
+
+    LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
+            sname.string(), handle,
+            ((mEnabled & (1 << what)) ? "en" : "dis"),
+            ((uint32_t(newState) << what) ? "en" : "dis"));
+    LOGV_IF(ENG_VERBOSE,
+            "HAL:%s sensor state change what=%d", sname.string(), what);
+
+    // pthread_mutex_lock(&mMplMutex);
+    // pthread_mutex_lock(&mHALMutex);
+
+    if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) {
+        uint32_t sensor_type;
+        short flags = newState;
+        uint32_t lastEnabled = mEnabled, changed = 0;
+
+        mEnabled &= ~(1 << what);
+        mEnabled |= (uint32_t(flags) << what);
+
+        LOGV_IF(ENG_VERBOSE, "HAL:flags = %d", flags);
+        computeLocalSensorMask(mEnabled);
+        LOGV_IF(ENG_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled);
+        LOGV_IF(ENG_VERBOSE, "HAL:last enable : lastEnabled = %d", lastEnabled);
+        sen_mask = mLocalSensorMask & mMasterSensorMask;
+        mSensorMask = sen_mask;
+        LOGV_IF(ENG_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask);
+
+        switch (what) {
+            case Gyro:
+            case RawGyro:
+            case Accelerometer:
+                if ((!(mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK) &&
+                    !(mEnabled & VIRTUAL_SENSOR_9AXES_MASK)) &&
+                    ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) {
+                    changed |= (1 << what);
+                }
+                if (mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION) {
+                     changed |= (1 << what);
+                }
+                break;
+            case MagneticField:
+            case RawMagneticField:
+                if (!(mEnabled & VIRTUAL_SENSOR_9AXES_MASK) &&
+                    ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) {
+                    changed |= (1 << what);
+                }
+                break;
+            case Pressure:
+                if ((lastEnabled & (1 << what)) != (mEnabled & (1 << what))) {
+                    changed |= (1 << what);
+                }
+                break;
+            case GameRotationVector:
+                if (!en)
+                    storeCalibration();
+                if ((en && !(lastEnabled & VIRTUAL_SENSOR_ALL_MASK))
+                         ||
+                    (en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK))
+                         ||
+                    (!en && !(mEnabled & VIRTUAL_SENSOR_ALL_MASK))
+                         ||
+                    (!en && (mEnabled & VIRTUAL_SENSOR_MAG_6AXES_MASK))) {
+                    for (int i = Gyro; i <= RawMagneticField; i++) {
+                        if (!(mEnabled & (1 << i))) {
+                            changed |= (1 << i);
+                        }
+                    }
+                }
+                break;
+
+            case Orientation:
+            case RotationVector:
+            case LinearAccel:
+            case Gravity:
+                if (!en)
+                    storeCalibration();
+                if ((en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK))
+                         ||
+                    (!en && !(mEnabled & VIRTUAL_SENSOR_9AXES_MASK))) {
+                    for (int i = Gyro; i <= RawMagneticField; i++) {
+                        if (!(mEnabled & (1 << i))) {
+                            changed |= (1 << i);
+                        }
+                    }
+                }
+                break;
+            case GeomagneticRotationVector:
+                if (!en)
+                    storeCalibration();
+                if ((en && !(lastEnabled & VIRTUAL_SENSOR_ALL_MASK))
+                        ||
+                    (en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK))
+                         ||
+                   (!en && !(mEnabled & VIRTUAL_SENSOR_ALL_MASK))
+                         ||
+                   (!en && (mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK))) {
+                   for (int i = Accelerometer; i <= RawMagneticField; i++) {
+                       if (!(mEnabled & (1<<i))) {
+                          changed |= (1 << i);
+                       }
+                   }
+                }
+                break;
+        }
+        LOGV_IF(ENG_VERBOSE, "HAL:changed = %d", changed);
+        enableSensors(sen_mask, flags, changed);
+    }
+
+    // pthread_mutex_unlock(&mMplMutex);
+    // pthread_mutex_unlock(&mHALMutex);
+
+#ifdef INV_PLAYBACK_DBG
+    /* apparently the logging needs to go through this sequence
+       to properly flush the log file */
+    inv_turn_off_data_logging();
+    if (fclose(logfile) < 0) {
+         LOGE("cannot close debug log file");
+    }
+    logfile = fopen("/data/playback.bin", "ab");
+    if (logfile)
+        inv_turn_on_data_logging(logfile);
+#endif
+
+    return err;
+}
+
+void MPLSensor::getHandle(int32_t handle, int &what, android::String8 &sname)
+{
+   VFUNC_LOG;
+
+   what = -1;
+
+   switch (handle) {
+   case ID_P:
+        what = StepDetector;
+        sname = "StepDetector";
+        break;
+   case ID_SC:
+        what = StepCounter;
+        sname = "StepCounter";
+        break;
+   case ID_SM:
+        what = SignificantMotion;
+        sname = "SignificantMotion";
+        break;
+   case ID_SO:
+        what = handle;
+        sname = "ScreenOrienation";
+        break;
+   case ID_A:
+        what = Accelerometer;
+        sname = "Accelerometer";
+        break;
+   case ID_M:
+        what = MagneticField;
+        sname = "MagneticField";
+        break;
+   case ID_RM:
+        what = RawMagneticField;
+        sname = "MagneticField Uncalibrated";
+        break;
+   case ID_O:
+        what = Orientation;
+        sname = "Orientation";
+        break;
+   case ID_GY:
+        what = Gyro;
+        sname = "Gyro";
+        break;
+   case ID_RG:
+        what = RawGyro;
+        sname = "Gyro Uncalibrated";
+        break;
+   case ID_GR:
+        what = Gravity;
+        sname = "Gravity";
+        break;
+   case ID_RV:
+        what = RotationVector;
+        sname = "RotationVector";
+        break;
+   case ID_GRV:
+        what = GameRotationVector;
+        sname = "GameRotationVector";
+        break;
+   case ID_LA:
+        what = LinearAccel;
+        sname = "LinearAccel";
+        break;
+   case ID_PS:
+        what = Pressure;
+        sname = "Pressure";
+        break;
+   default: // this takes care of all the gestures
+        what = handle;
+        sname = "Others";
+        break;
+    }
+
+    LOGI_IF(EXTRA_VERBOSE, "HAL:getHandle - what=%d, sname=%s", what, sname.string());
+    return;
+}
+
+int MPLSensor::setDelay(int32_t handle, int64_t ns)
+{
+    VFUNC_LOG;
+
+    android::String8 sname;
+    int what = -1;
+
+#if 0
+    // skip the 1st call for enalbing sensors called by ICS/JB sensor service
+    static int counter_delay = 0;
+    if (!(mEnabled & (1 << what))) {
+        counter_delay = 0;
+    } else {
+        if (++counter_delay == 1) {
+            return 0;
+        }
+        else {
+            counter_delay = 0;
+        }
+    }
+#endif
+
+    getHandle(handle, what, sname);
+    if (uint32_t(what) >= NumSensors)
+        return -EINVAL;
+
+    if (ns < 0)
+        return -EINVAL;
+
+    LOGV_IF(PROCESS_VERBOSE,
+            "setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns);
+
+    // limit all rates to reasonable ones */
+    if (ns < 5000000LL) {
+        ns = 5000000LL;
+    }
+
+    /* store request rate to mDelays arrary for each sensor */
+    int64_t previousDelay = mDelays[what];
+    mDelays[what] = ns;
+    LOGV_IF(ENG_VERBOSE, "storing mDelays[%d] = %lld, previousDelay = %lld", what, ns, previousDelay);
+
+    switch (what) {
+        case StepCounter:
+            /* set limits of delivery rate of events */
+            mStepCountPollTime = ns;
+            LOGV_IF(ENG_VERBOSE, "step count rate =%lld ns", ns);
+            break; 
+        case StepDetector:
+        case SignificantMotion:
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+        case ID_SO:
+#endif
+            LOGV_IF(ENG_VERBOSE, "Step Detect, SMD, SO rate=%lld ns", ns);            
+            break;
+        case Gyro:
+        case RawGyro:
+        case Accelerometer:
+            // need to update delay since they are different
+            // resetDataRates was called earlier
+            // LOGV("what=%d mEnabled=%d mDelays[%d]=%lld previousDelay=%lld",
+            // what, mEnabled, what, mDelays[what], previousDelay);
+            if ((mEnabled & (1 << what)) && (previousDelay != mDelays[what])) {
+                LOGV_IF(ENG_VERBOSE,
+                    "HAL:need to update delay due to resetDataRates");
+                break;
+            }
+            for (int i = Gyro;
+                    i <= Accelerometer + mCompassSensor->isIntegrated();
+                    i++) {
+                if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
+                    LOGV_IF(ENG_VERBOSE,
+                            "HAL:ignore delay set due to sensor %d", i);
+                    return 0;
+                }
+            }
+            break;
+
+        case MagneticField:
+        case RawMagneticField:
+            // need to update delay since they are different
+            // resetDataRates was called earlier
+            if ((mEnabled & (1 << what)) && (previousDelay != mDelays[what])) {
+                LOGV_IF(ENG_VERBOSE,
+                    "HAL:need to update delay due to resetDataRates");
+                break;
+            }
+            if (mCompassSensor->isIntegrated() &&
+                    (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) ||
+                    ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) ||
+                    ((mEnabled & (1 << Accelerometer)) &&
+                        ns > mDelays[Accelerometer])) &&
+                        !checkBatchEnabled()) {
+                 /* if request is slower rate, ignore request */
+                 LOGV_IF(ENG_VERBOSE,
+                         "HAL:ignore delay set due to gyro/accel");
+                 return 0;
+            }
+            break;
+
+        case Orientation:
+        case RotationVector:
+        case GameRotationVector:
+        case GeomagneticRotationVector:
+        case LinearAccel:
+        case Gravity:
+            if (isLowPowerQuatEnabled()) {
+                LOGV_IF(ENG_VERBOSE,
+                        "HAL:need to update delay due to LPQ");
+                break;
+            }
+
+            for (int i = 0; i < NumSensors; i++) {
+                if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
+                    LOGV_IF(ENG_VERBOSE,
+                            "HAL:ignore delay set due to sensor %d", i);
+                    return 0;
+                }
+            }
+            break;
+    }
+
+    // pthread_mutex_lock(&mHALMutex);
+    int res = update_delay();
+    // pthread_mutex_unlock(&mHALMutex);
+    return res;
+}
+
+int MPLSensor::update_delay(void)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+    int64_t got;
+
+    if (mEnabled) {
+        int64_t wanted = 1000000000LL;
+        int64_t wanted_3rd_party_sensor = 1000000000LL;
+
+        // Sequence to change sensor's FIFO rate
+        // 1. reset master enable
+        // 2. Update delay
+        // 3. set master enable
+
+        // reset master enable
+        masterEnable(0);
+
+        int64_t gyroRate;
+        int64_t accelRate;
+        int64_t compassRate;
+        int64_t pressureRate;
+        
+        int rateInus;
+        int mplGyroRate;
+        int mplAccelRate;
+        int mplCompassRate;
+        int tempRate = wanted;
+
+        /* search the minimum delay requested across all enabled sensors */
+        for (int i = 0; i < NumSensors; i++) {
+            if (mEnabled & (1 << i)) {
+                int64_t ns = mDelays[i];
+                wanted = wanted < ns ? wanted : ns;
+            }
+        }
+        
+        /* initialize rate for each sensor */
+        gyroRate = wanted;
+        accelRate = wanted;
+        compassRate = wanted;
+        pressureRate = wanted;
+        // same delay for 3rd party Accel or Compass
+        wanted_3rd_party_sensor = wanted;
+
+        int enabled_sensors = mEnabled;
+        int tempFd = -1;
+
+        if(mFeatureActiveMask & INV_DMP_BATCH_MODE) {
+            // set batch rates
+            LOGV_IF(ENG_VERBOSE, "HAL: mFeatureActiveMask=%016llx", mFeatureActiveMask);
+            LOGV("HAL: batch mode is set, set batch data rates");
+            if (setBatchDataRates() < 0) {
+                LOGE("HAL:ERR can't set batch data rates");
+            }
+        } else {
+            /* set master sampling frequency */
+            int64_t tempWanted = wanted;
+            getDmpRate(&tempWanted);
+            /* driver only looks at sampling frequency if DMP is off */
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+                    1000000000.f / tempWanted, mpu.gyro_fifo_rate, getTimestamp());
+            tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
+            res = write_attribute_sensor(tempFd, 1000000000.f / tempWanted);
+            LOGE_IF(res < 0, "HAL:sampling frequency update delay error");
+
+        if (LA_ENABLED || GR_ENABLED || RV_ENABLED
+                       || GRV_ENABLED || O_ENABLED || GMRV_ENABLED) {
+            rateInus = (int)wanted / 1000LL;
+
+            /* set rate in MPL */
+            /* compass can only do 100Hz max */
+            inv_set_gyro_sample_rate(rateInus);
+            inv_set_accel_sample_rate(rateInus);
+            inv_set_compass_sample_rate(rateInus);
+            inv_set_linear_acceleration_sample_rate(rateInus);
+            inv_set_orientation_sample_rate(rateInus);
+            inv_set_rotation_vector_sample_rate(rateInus);
+            inv_set_gravity_sample_rate(rateInus);
+            inv_set_orientation_geomagnetic_sample_rate(rateInus);
+            inv_set_rotation_vector_6_axis_sample_rate(rateInus);
+            inv_set_geomagnetic_rotation_vector_sample_rate(rateInus);
+
+            LOGV_IF(ENG_VERBOSE,
+                    "HAL:MPL virtual sensor sample rate: (mpl)=%d us (mpu)=%.2f Hz",
+                    rateInus, 1000000000.f / wanted);
+            LOGV_IF(ENG_VERBOSE,
+                    "HAL:MPL gyro sample rate: (mpl)=%d us (mpu)=%.2f Hz",
+                    rateInus, 1000000000.f / gyroRate);
+            LOGV_IF(ENG_VERBOSE,
+                    "HAL:MPL accel sample rate: (mpl)=%d us (mpu)=%.2f Hz",
+                    rateInus, 1000000000.f / accelRate);
+            LOGV_IF(ENG_VERBOSE,
+                    "HAL:MPL compass sample rate: (mpl)=%d us (mpu)=%.2f Hz",
+                    rateInus, 1000000000.f / compassRate);
+
+            LOGV_IF(ENG_VERBOSE, 
+                    "mFeatureActiveMask=%016llx", mFeatureActiveMask);
+            if(mFeatureActiveMask & DMP_FEATURE_MASK) {
+                bool setDMPrate= 0;
+                gyroRate = wanted;
+                accelRate = wanted;
+                compassRate = wanted;
+                // same delay for 3rd party Accel or Compass
+                wanted_3rd_party_sensor = wanted;
+                rateInus = (int)wanted / 1000LL;
+                // Set LP Quaternion sample rate if enabled
+                if (checkLPQuaternion()) {
+                    if (wanted <= RATE_200HZ) {
+#ifndef USE_LPQ_AT_FASTEST 
+                        enableLPQuaternion(0);
+#endif
+                    } else {
+                        inv_set_quat_sample_rate(rateInus);
+                        LOGV_IF(ENG_VERBOSE, "HAL:MPL quat sample rate: "
+                                "(mpl)=%d us (mpu)=%.2f Hz", 
+                                rateInus, 1000000000.f / wanted);
+                        setDMPrate= 1;
+                    }
+                }
+            }
+
+            LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion");
+            //nsToHz
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+                    1000000000.f / gyroRate, mpu.gyro_rate,
+                    getTimestamp());
+            tempFd = open(mpu.gyro_rate, O_RDWR);
+            res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate);
+            if(res < 0) {
+                LOGE("HAL:GYRO update delay error");
+            }
+
+            if(USE_THIRD_PARTY_ACCEL == 1) {
+                // 3rd party accelerometer - if applicable
+                // nsToHz (BMA250)
+                LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)",
+                        wanted_3rd_party_sensor / 1000000L, mpu.accel_rate,
+                        getTimestamp());
+                tempFd = open(mpu.accel_rate, O_RDWR);
+                res = write_attribute_sensor(tempFd,
+                        wanted_3rd_party_sensor / 1000000L);
+                LOGE_IF(res < 0, "HAL:ACCEL update delay error");
+            } else {
+                // mpu accel
+               LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+                        1000000000.f / accelRate, mpu.accel_rate,
+                        getTimestamp());
+                tempFd = open(mpu.accel_rate, O_RDWR);
+                res = write_attribute_sensor(tempFd, 1000000000.f / accelRate);
+                LOGE_IF(res < 0, "HAL:ACCEL update delay error");
+            }
+
+            if (!mCompassSensor->isIntegrated()) {
+                // stand-alone compass - if applicable
+                LOGV_IF(ENG_VERBOSE,
+                        "HAL:Ext compass delay %lld", wanted_3rd_party_sensor);
+                LOGV_IF(ENG_VERBOSE, "HAL:Ext compass rate %.2f Hz",
+                        1000000000.f / wanted_3rd_party_sensor);
+                if (wanted_3rd_party_sensor <
+                        mCompassSensor->getMinDelay() * 1000LL) {
+                    wanted_3rd_party_sensor =
+                        mCompassSensor->getMinDelay() * 1000LL;
+                }
+                LOGV_IF(ENG_VERBOSE,
+                        "HAL:Ext compass delay %lld", wanted_3rd_party_sensor);
+                LOGV_IF(ENG_VERBOSE, "HAL:Ext compass rate %.2f Hz",
+                        1000000000.f / wanted_3rd_party_sensor);
+                mCompassSensor->setDelay(ID_M, wanted_3rd_party_sensor);
+                got = mCompassSensor->getDelay(ID_M);
+                inv_set_compass_sample_rate(got / 1000);
+            } else {
+                // compass on secondary bus
+                if (compassRate < mCompassSensor->getMinDelay() * 1000LL) {
+                    compassRate = mCompassSensor->getMinDelay() * 1000LL;
+                }
+                mCompassSensor->setDelay(ID_M, compassRate);
+            }
+        } else {
+
+            if (GY_ENABLED || RGY_ENABLED) {
+                wanted = (mDelays[Gyro] <= mDelays[RawGyro]?
+                    (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]):
+                    (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro]));
+                LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
+                inv_set_gyro_sample_rate((int)wanted/1000LL);
+                LOGV_IF(ENG_VERBOSE,
+                    "HAL:MPL gyro sample rate: (mpl)=%d us", int(wanted/1000LL));
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+                        1000000000.f / wanted, mpu.gyro_rate, getTimestamp());
+                tempFd = open(mpu.gyro_rate, O_RDWR);
+                res = write_attribute_sensor(tempFd, 1000000000.f / wanted);
+                LOGE_IF(res < 0, "HAL:GYRO update delay error");
+            }
+
+            if (A_ENABLED) { /* there is only 1 fifo rate for MPUxxxx */
+                if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) {
+                    wanted = mDelays[Gyro];
+                } else if (RGY_ENABLED && mDelays[RawGyro]
+                            < mDelays[Accelerometer]) {
+                    wanted = mDelays[RawGyro];
+                } else {
+                    wanted = mDelays[Accelerometer];
+                }
+                LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
+                inv_set_accel_sample_rate((int)wanted/1000LL);
+                LOGV_IF(ENG_VERBOSE, "HAL:MPL accel sample rate: (mpl)=%d us",
+                        int(wanted/1000LL));
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+                        1000000000.f / wanted, mpu.accel_rate,
+                        getTimestamp());
+                tempFd = open(mpu.accel_rate, O_RDWR);
+                if(USE_THIRD_PARTY_ACCEL == 1) {
+                    //BMA250 in ms
+                    res = write_attribute_sensor(tempFd, wanted / 1000000L);
+                }
+                else {
+                    //MPUxxxx in hz
+                    res = write_attribute_sensor(tempFd, 1000000000.f/wanted);
+                }
+                LOGE_IF(res < 0, "HAL:ACCEL update delay error");
+            }
+
+            /* Invensense compass calibration */
+            if (M_ENABLED || RM_ENABLED) {
+                int64_t compassWanted = (mDelays[MagneticField] <= mDelays[RawMagneticField]?
+                    (mEnabled & (1 << MagneticField)? mDelays[MagneticField]: mDelays[RawMagneticField]):
+                    (mEnabled & (1 << RawMagneticField)? mDelays[RawMagneticField]: mDelays[MagneticField]));
+                if (!mCompassSensor->isIntegrated()) {
+                    wanted = compassWanted;
+                } else {
+                    if (GY_ENABLED
+                        && (mDelays[Gyro] < compassWanted)) {
+                        wanted = mDelays[Gyro];
+                    } else if (RGY_ENABLED
+                               && mDelays[RawGyro] < compassWanted) {
+                        wanted = mDelays[RawGyro];
+                    } else if (A_ENABLED && mDelays[Accelerometer]
+                                < compassWanted) {
+                        wanted = mDelays[Accelerometer];
+                    } else {
+                        wanted = compassWanted;
+                    }
+                    LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
+                }
+
+                mCompassSensor->setDelay(ID_M, wanted);
+                got = mCompassSensor->getDelay(ID_M);
+                inv_set_compass_sample_rate(got / 1000);
+                LOGV_IF(ENG_VERBOSE, "HAL:MPL compass sample rate: (mpl)=%d us",
+                        int(got/1000LL));
+            }
+            
+            if (PS_ENABLED) {
+                int64_t pressureRate = mDelays[Pressure];
+                LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
+                mPressureSensor->setDelay(ID_PS, pressureRate);
+                LOGE_IF(res < 0, "HAL:PRESSURE update delay error");
+            }
+        }
+
+        } //end of non batch mode
+
+        unsigned long sensors = mLocalSensorMask & mMasterSensorMask;
+        if (sensors &
+            (INV_THREE_AXIS_GYRO
+                | INV_THREE_AXIS_ACCEL
+                | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()
+                | (INV_ONE_AXIS_PRESSURE * mPressureSensor->isIntegrated())))) {
+            LOGV_IF(ENG_VERBOSE, "sensors=%lu", sensors);
+            res = masterEnable(1);
+            if(res < 0)
+                return res;
+        } else { // all sensors idle -> reduce power, unless DMP is needed
+            LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
+            if(mFeatureActiveMask & DMP_FEATURE_MASK) {
+                res = masterEnable(1);
+                if(res < 0)
+                    return res;
+            }
+        }
+    }
+
+    return res;
+}
+
+/* For Third Party Accel Input Subsystem Drivers only */
+int MPLSensor::readAccelEvents(sensors_event_t* data, int count)
+{
+    VHANDLER_LOG;
+
+    if (count < 1)
+        return -EINVAL;
+
+    ssize_t n = mAccelInputReader.fill(accel_fd);
+    if (n < 0) {
+        LOGE("HAL:missed accel events, exit");
+        return n;
+    }
+
+    int numEventReceived = 0;
+    input_event const* event;
+    int nb, done = 0;
+
+    while (done == 0 && count && mAccelInputReader.readEvent(&event)) {
+        int type = event->type;
+        if (type == EV_ABS) {
+            if (event->code == EVENT_TYPE_ACCEL_X) {
+                mPendingMask |= 1 << Accelerometer;
+                mCachedAccelData[0] = event->value;
+            } else if (event->code == EVENT_TYPE_ACCEL_Y) {
+                mPendingMask |= 1 << Accelerometer;
+                mCachedAccelData[1] = event->value;
+            } else if (event->code == EVENT_TYPE_ACCEL_Z) {
+                mPendingMask |= 1 << Accelerometer;
+                mCachedAccelData[2] =event-> value;
+            }
+        } else if (type == EV_SYN) {
+            done = 1;
+            if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
+                inv_build_accel(mCachedAccelData, 0, getTimestamp());
+            }
+        } else {
+            LOGE("HAL:AccelSensor: unknown event (type=%d, code=%d)",
+                    type, event->code);
+        }
+        mAccelInputReader.next();
+    }
+
+    return numEventReceived;
+}
+
+/**
+ *  Should be called after reading at least one of gyro
+ *  compass or accel data. (Also okay for handling all of them).
+ *  @returns 0, if successful, error number if not.
+ */
+int MPLSensor::readEvents(sensors_event_t* data, int count)
+{
+    VHANDLER_LOG;
+ 
+    inv_execute_on_data();
+
+    int numEventReceived = 0;
+
+    long msg;
+    msg = inv_get_message_level_0(1);
+    if (msg) {
+        if (msg & INV_MSG_MOTION_EVENT) {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:**** Motion ****\n");
+        }
+        if (msg & INV_MSG_NO_MOTION_EVENT) {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:***** No Motion *****\n");
+            /* after the first no motion, the gyro should be
+               calibrated well */
+            mGyroAccuracy = SENSOR_STATUS_ACCURACY_HIGH;
+            /* if gyros are on and we got a no motion, set a flag
+               indicating that the cal file can be written. */
+            mHaveGoodMpuCal = true;
+        }
+        if(msg & INV_MSG_NEW_AB_EVENT) {
+            LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Accel Bias *****\n");
+            getAccelBias();
+            mAccelAccuracy = inv_get_accel_accuracy();
+        }
+        if(msg & INV_MSG_NEW_GB_EVENT) {
+            LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Gyro Bias *****\n");
+            getGyroBias();
+            setGyroBias();
+        }
+        if(msg & INV_MSG_NEW_FGB_EVENT) {
+            LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Factory Gyro Bias *****\n");
+            getFactoryGyroBias();
+        }
+        if(msg & INV_MSG_NEW_FAB_EVENT) {
+            LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Factory Accel Bias *****\n");
+            getFactoryAccelBias();
+        }
+        if(msg & INV_MSG_NEW_CB_EVENT) {
+            LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Compass Bias *****\n");
+            getCompassBias();
+            mCompassAccuracy = inv_get_mag_accuracy();
+        }
+    }    
+
+    // handle partial packet read and end marker
+    // skip readEvents from hal_outputs
+    if (mSkipReadEvents) {        
+        if(mDataMarkerDetected || mEmptyDataMarkerDetected) {            
+            if (!mEmptyDataMarkerDetected) {
+                // turn off sensors in data_builder
+                resetMplStates();
+            }
+            mEmptyDataMarkerDetected = 0;   
+            mDataMarkerDetected = 0;
+
+            // handle flush complete event
+            if(mFlushBatchSet && mFlushSensorEnabled != -1) {
+                sensors_event_t temp;
+#if defined ANDROID_KITKAT
+                int sendEvent = metaHandler(&temp, META_DATA_FLUSH_COMPLETE);
+#else
+                int sendEvent = metaHandler(&temp, 0);
+#endif
+
+                if(sendEvent == 1 && count > 0) {
+                    *data++ = temp;
+                    count--;
+                    numEventReceived++;
+                }
+            }
+        }
+        return numEventReceived;
+    }
+
+    for (int i = 0; i < NumSensors; i++) {
+        int update = 0;
+
+        // handle step detector when ped_q is enabled
+        if(mPedUpdate) {
+            if (i == StepDetector) {
+                update = readDmpPedometerEvents(data, count, ID_P, 1);
+                mPedUpdate = 0;
+                if(update == 1 && count > 0) {
+                    data->timestamp = mStepSensorTimestamp;
+                    count--;
+                    numEventReceived++;
+                    continue;
+                }
+            } else {
+                if (mPedUpdate == DATA_FORMAT_STEP) {
+                    continue;
+                }
+            }
+        }
+
+        // load up virtual sensors
+        if (mEnabled & (1 << i)) {
+            update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i);
+            mPendingMask |= (1 << i);
+
+            if (update && (count > 0)) {
+                *data++ = mPendingEvents[i];
+                count--;
+                numEventReceived++;
+            }
+        }
+    }
+
+    return numEventReceived;
+}
+
+// collect data for MPL (but NOT sensor service currently), from driver layer
+void MPLSensor::buildMpuEvent(void)
+{
+    VHANDLER_LOG;
+
+    mSkipReadEvents = 0;
+    int64_t mGyroSensorTimestamp=0, mAccelSensorTimestamp=0, latestTimestamp=0;
+    int lp_quaternion_on = 0, sixAxis_quaternion_on = 0,
+        ped_quaternion_on = 0, ped_standalone_on = 0;
+    size_t nbyte;
+    unsigned short data_format = 0;
+    int i, nb, mask = 0,
+        sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 : 0) +
+            ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 : 0) +
+            (((mLocalSensorMask & INV_THREE_AXIS_COMPASS)
+                && mCompassSensor->isIntegrated())? 1 : 0) +
+            ((mLocalSensorMask & INV_ONE_AXIS_PRESSURE)? 1 : 0);
+    //LOGV("mLocalSensorMask=0x%lx", mLocalSensorMask);
+    char *rdata = mIIOBuffer;
+    ssize_t rsize = 0;
+    ssize_t readCounter = 0;
+    char *rdataP = NULL;
+    bool doneFlag = 0;
+
+    if (isLowPowerQuatEnabled()) {
+        lp_quaternion_on = checkLPQuaternion();
+    }
+    sixAxis_quaternion_on = check6AxisQuatEnabled();
+    ped_quaternion_on = checkPedQuatEnabled();
+    ped_standalone_on = checkPedStandaloneEnabled();
+   
+    nbyte = MAX_READ_SIZE - mLeftOverBufferSize;
+
+    /* check previous copied buffer */
+    /* append with just read data */
+    if (mLeftOverBufferSize > 0) {
+        LOGV_IF(0, "append old buffer size=%d", mLeftOverBufferSize);
+        memset(rdata, 0, sizeof(rdata));
+        memcpy(rdata, mLeftOverBuffer, mLeftOverBufferSize);
+            LOGV_IF(0,
+            "HAL:input retrieve old buffer data=:%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, "
+            "%d, %d,%d, %d, %d, %d\n",           
+            rdata[0], rdata[1], rdata[2], rdata[3],
+            rdata[4], rdata[5], rdata[6], rdata[7],
+            rdata[8], rdata[9], rdata[10], rdata[11],
+            rdata[12], rdata[13], rdata[14], rdata[15]);
+    }
+    rdataP = rdata + mLeftOverBufferSize;
+   
+    /* read expected number of bytes */
+    rsize = read(iio_fd, rdataP, nbyte);
+    if(rsize < 0) {
+        /* IIO buffer might have old data.
+           Need to flush it if no sensor is on, to avoid infinite 
+           read loop.*/
+        LOGE("HAL:input data file descriptor not available - (%s)", 
+             strerror(errno));
+        if (sensors == 0) {
+            rsize = read(iio_fd, rdata, MAX_SUSPEND_BATCH_PACKET_SIZE);
+            if(rsize > 0) {
+                LOGV_IF(ENG_VERBOSE, "HAL:input data flush rsize=%d", (int)rsize);
+#ifdef TESTING
+                LOGV_IF(INPUT_DATA,
+                     "HAL:input rdata:r=%d, n=%d,"
+                     "%d, %d, %d, %d, %d, %d, %d, %d",
+                     (int)rsize, nbyte,
+                     rdataP[0], rdataP[1], rdataP[2], rdataP[3],
+                     rdataP[4], rdataP[5], rdataP[6], rdataP[7]);
+#endif
+                mLeftOverBufferSize = 0;                              
+            }
+        }
+        return;
+    }
+
+#ifdef TESTING
+LOGV_IF(INPUT_DATA,
+         "HAL:input just read rdataP:r=%d, n=%d,"
+         "%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d,"
+         "%d, %d, %d, %d,%d, %d, %d, %d\n",
+         (int)rsize, nbyte,
+         rdataP[0], rdataP[1], rdataP[2], rdataP[3],
+         rdataP[4], rdataP[5], rdataP[6], rdataP[7],
+         rdataP[8], rdataP[9], rdataP[10], rdataP[11],
+         rdataP[12], rdataP[13], rdataP[14], rdataP[15],
+         rdataP[16], rdataP[17], rdataP[18], rdataP[19],
+         rdataP[20], rdataP[21], rdataP[22], rdataP[23]);
+
+    LOGV_IF(INPUT_DATA,
+         "HAL:input rdata:r=%d, n=%d,"
+         "%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d,"
+         "%d, %d, %d, %d,%d, %d, %d, %d\n",
+         (int)rsize, nbyte,
+         rdata[0], rdata[1], rdata[2], rdata[3],
+         rdata[4], rdata[5], rdata[6], rdata[7],
+         rdata[8], rdata[9], rdata[10], rdata[11],
+         rdata[12], rdata[13], rdata[14], rdata[15],
+         rdata[16], rdata[17], rdata[18], rdata[19],
+         rdata[20], rdata[21], rdata[22], rdata[23]);
+#endif
+
+    if(rsize + mLeftOverBufferSize < MAX_READ_SIZE) {
+        /* store packet then return */
+        mLeftOverBufferSize += rsize;
+        memcpy(mLeftOverBuffer, rdata, mLeftOverBufferSize);
+
+#ifdef TESTING
+        LOGV_IF(1, "HAL:input data has batched partial packet");
+        LOGV_IF(1, "HAL:input data batched mLeftOverBufferSize=%d", mLeftOverBufferSize);
+        LOGV_IF(1,
+            "HAL:input catch up batched retrieve buffer=:%d, %d, %d, %d, %d, %d, %d, %d,"
+            "%d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+            mLeftOverBuffer[0], mLeftOverBuffer[1], mLeftOverBuffer[2], mLeftOverBuffer[3],
+            mLeftOverBuffer[4], mLeftOverBuffer[5], mLeftOverBuffer[6], mLeftOverBuffer[7],
+            mLeftOverBuffer[8], mLeftOverBuffer[9], mLeftOverBuffer[10], mLeftOverBuffer[11],
+            mLeftOverBuffer[12], mLeftOverBuffer[13], mLeftOverBuffer[14], mLeftOverBuffer[15]);
+#endif
+        mSkipReadEvents = 1;
+        return;
+    }
+
+    /* reset data and count pointer */
+    rdataP = rdata;  
+    readCounter = rsize + mLeftOverBufferSize;
+    LOGV_IF(0, "HAL:input readCounter set=%d", (int)readCounter);
+    
+    LOGV_IF(INPUT_DATA && ENG_VERBOSE, 
+            "HAL:input b=%d rdata= %d nbyte= %d rsize= %d readCounter= %d",
+            checkBatchEnabled(), *((short *) rdata), nbyte, (int)rsize, (int)readCounter);
+    LOGV_IF(INPUT_DATA && ENG_VERBOSE, 
+            "HAL:input sensors= %d, lp_q_on= %d, 6axis_q_on= %d, "
+            "ped_q_on= %d, ped_standalone_on= %d",
+            sensors, lp_quaternion_on, sixAxis_quaternion_on, ped_quaternion_on, 
+            ped_standalone_on); 
+ 
+    while (readCounter > 0) {
+        // since copied buffer is already accounted for, reset left over size
+        mLeftOverBufferSize = 0;        
+        // clear data format mask for parsing the next set of data
+        mask = 0;
+        data_format = *((short *)(rdata));
+        LOGV_IF(INPUT_DATA && ENG_VERBOSE,
+                "HAL:input data_format=%x", data_format);
+
+        if(checkValidHeader(data_format) == 0) {
+            LOGE("HAL:input invalid data_format 0x%02X", data_format);
+            return;
+        }
+
+        if (data_format & DATA_FORMAT_STEP) {
+            if (data_format == DATA_FORMAT_STEP) {
+                rdata += BYTES_PER_SENSOR;
+                latestTimestamp = *((long long*) (rdata));
+                LOGV_IF(ENG_VERBOSE && INPUT_DATA, "STEP DETECTED:0x%x - ts: %lld", data_format, latestTimestamp);
+                // readCounter is decrement by 24 because DATA_FORMAT_STEP only applies in batch  mode
+                readCounter -= BYTES_PER_SENSOR_PACKET;
+            } else {
+                LOGV_IF(0, "STEP DETECTED:0x%x", data_format);
+            }
+            mPedUpdate |= data_format;
+            mask |= DATA_FORMAT_STEP;
+            // cancels step bit
+            data_format &= (~DATA_FORMAT_STEP);
+        }
+
+        if (data_format & DATA_FORMAT_MARKER) {
+            LOGV_IF(ENG_VERBOSE && INPUT_DATA, "MARKER DETECTED:0x%x", data_format);
+            readCounter -= BYTES_PER_SENSOR;
+            if (mFlushSensorEnabled != -1) {
+                mFlushBatchSet = 1;
+            }
+            mDataMarkerDetected = 1;
+            mSkipReadEvents = 1; 
+        }
+        else if (data_format & DATA_FORMAT_EMPTY_MARKER) {
+            LOGV_IF(ENG_VERBOSE && INPUT_DATA, "EMPTY MARKER DETECTED:0x%x", data_format);
+            readCounter -= BYTES_PER_SENSOR;
+            if (mFlushSensorEnabled != -1) {
+                mFlushBatchSet = 1;
+            }
+            mEmptyDataMarkerDetected = 1;
+            mSkipReadEvents = 1;
+        }
+        else if (data_format == DATA_FORMAT_QUAT) {
+            LOGV_IF(ENG_VERBOSE && INPUT_DATA, "QUAT DETECTED:0x%x", data_format);
+            if (readCounter >= BYTES_QUAT_DATA) {
+                mCachedQuaternionData[0] = *((int *) (rdata + 4));
+                mCachedQuaternionData[1] = *((int *) (rdata + 8));
+                mCachedQuaternionData[2] = *((int *) (rdata + 12));
+                rdata += QUAT_ONLY_LAST_PACKET_OFFSET;
+                mQuatSensorTimestamp = *((long long*) (rdata));
+                mask |= DATA_FORMAT_QUAT;
+                readCounter -= BYTES_QUAT_DATA; 
+            }
+            else {
+                doneFlag = 1;
+            }
+        }
+        else if (data_format == DATA_FORMAT_6_AXIS) {
+            LOGV_IF(ENG_VERBOSE && INPUT_DATA, "6AXIS DETECTED:0x%x", data_format);
+            if (readCounter >= BYTES_QUAT_DATA) {
+                mCached6AxisQuaternionData[0] = *((int *) (rdata + 4));
+                mCached6AxisQuaternionData[1] = *((int *) (rdata + 8));
+                mCached6AxisQuaternionData[2] = *((int *) (rdata + 12));
+                rdata += QUAT_ONLY_LAST_PACKET_OFFSET;
+                mQuatSensorTimestamp = *((long long*) (rdata));
+                mask |= DATA_FORMAT_6_AXIS;
+                readCounter -= BYTES_QUAT_DATA;
+            }
+            else {
+                doneFlag = 1;
+            }
+        }
+        else if (data_format == DATA_FORMAT_PED_QUAT) {
+            LOGV_IF(ENG_VERBOSE && INPUT_DATA, "PED QUAT DETECTED:0x%x", data_format);
+            if (readCounter >= BYTES_PER_SENSOR_PACKET) {
+                mCachedPedQuaternionData[0] = *((short *) (rdata + 2));
+                mCachedPedQuaternionData[1] = *((short *) (rdata + 4));
+                mCachedPedQuaternionData[2] = *((short *) (rdata + 6));
+                rdata += BYTES_PER_SENSOR;
+                mQuatSensorTimestamp = *((long long*) (rdata));
+                mask |= DATA_FORMAT_PED_QUAT;
+                readCounter -= BYTES_PER_SENSOR_PACKET;
+            }
+            else {
+                doneFlag = 1;
+            }
+        }
+        else if (data_format == DATA_FORMAT_PED_STANDALONE) {
+            LOGV_IF(ENG_VERBOSE && INPUT_DATA, "STANDALONE STEP DETECTED:0x%x", data_format);
+            if (readCounter >= BYTES_PER_SENSOR_PACKET) {
+                rdata += BYTES_PER_SENSOR;
+                mStepSensorTimestamp = *((long long*) (rdata));
+                mask |= DATA_FORMAT_PED_STANDALONE;
+                readCounter -= BYTES_PER_SENSOR_PACKET;
+                mPedUpdate |= data_format;
+            }
+            else {
+                doneFlag = 1;
+            }
+        }
+        else if (data_format == DATA_FORMAT_GYRO) {
+            LOGV_IF(ENG_VERBOSE && INPUT_DATA, "GYRO DETECTED:0x%x", data_format);
+            if (readCounter >= BYTES_PER_SENSOR_PACKET) {
+                mCachedGyroData[0] = *((short *) (rdata + 2));
+                mCachedGyroData[1] = *((short *) (rdata + 4));
+                mCachedGyroData[2] = *((short *) (rdata + 6));
+                rdata += BYTES_PER_SENSOR;
+                mGyroSensorTimestamp = *((long long*) (rdata));
+                mask |= DATA_FORMAT_GYRO;
+                readCounter -= BYTES_PER_SENSOR_PACKET;
+            } else {
+                doneFlag = 1;
+            }
+        }
+        else if (data_format == DATA_FORMAT_ACCEL) {
+            LOGV_IF(ENG_VERBOSE && INPUT_DATA, "ACCEL DETECTED:0x%x", data_format);
+            if (readCounter >= BYTES_PER_SENSOR_PACKET) {
+                mCachedAccelData[0] = *((short *) (rdata + 2));
+                mCachedAccelData[1] = *((short *) (rdata + 4));
+                mCachedAccelData[2] = *((short *) (rdata + 6));
+                rdata += BYTES_PER_SENSOR;
+                mAccelSensorTimestamp = *((long long*) (rdata));
+                mask |= DATA_FORMAT_ACCEL;
+                readCounter -= BYTES_PER_SENSOR_PACKET;
+            }
+            else {
+                doneFlag = 1;
+            }
+        }
+        else if (data_format == DATA_FORMAT_COMPASS) {
+            LOGV_IF(ENG_VERBOSE && INPUT_DATA, "COMPASS DETECTED:0x%x", data_format);
+            if (readCounter >= BYTES_PER_SENSOR_PACKET) {
+                if (mCompassSensor->isIntegrated()) {
+                    mCachedCompassData[0] = *((short *) (rdata + 2));
+                    mCachedCompassData[1] = *((short *) (rdata + 4));
+                    mCachedCompassData[2] = *((short *) (rdata + 6));
+                    rdata += BYTES_PER_SENSOR;
+                    mCompassTimestamp = *((long long*) (rdata));
+                    mask |= DATA_FORMAT_COMPASS;
+                    readCounter -= BYTES_PER_SENSOR_PACKET;
+                }
+            }
+            else {
+                doneFlag = 1;
+            }
+        }
+        else if (data_format == DATA_FORMAT_PRESSURE) {
+            LOGV_IF(ENG_VERBOSE && INPUT_DATA, "PRESSURE DETECTED:0x%x", data_format);
+            if (readCounter >= BYTES_QUAT_DATA) {
+                if (mPressureSensor->isIntegrated()) {
+                    mCachedPressureData = 
+                        ((*((short *)(rdata + 4))) << 16) + 
+                        (*((unsigned short *) (rdata + 6)));
+                    rdata += BYTES_PER_SENSOR;
+                    mPressureTimestamp = *((long long*) (rdata));
+                    if (mCachedPressureData != 0) {
+                        mask |= DATA_FORMAT_PRESSURE;
+                    }
+                    readCounter -= BYTES_PER_SENSOR_PACKET;
+                }
+            } else{
+                doneFlag = 1;
+            }
+        }
+
+        if(doneFlag == 0) {
+            rdata += BYTES_PER_SENSOR;
+            LOGV_IF(ENG_VERBOSE && INPUT_DATA, "HAL: input data doneFlag is zero, readCounter=%d", (int)readCounter);
+        }
+        else {
+            LOGV_IF(ENG_VERBOSE && INPUT_DATA, "HAL: input data doneFlag is set, readCounter=%d", (int)readCounter);
+        }
+        
+        /* read ahead and store left over data if any */
+        if (readCounter != 0) {
+            int currentBufferCounter = 0;
+            LOGV_IF(0, "Not enough data readCounter=%d, expected nbyte=%d, rsize=%d", (int)readCounter, nbyte, (int)rsize);
+            memset(mLeftOverBuffer, 0, sizeof(mLeftOverBuffer));
+            memcpy(mLeftOverBuffer, rdata, readCounter);
+            LOGV_IF(0,
+                    "HAL:input store rdata=:%d, %d, %d, %d,%d, %d, %d, %d,%d, "
+                    "%d, %d, %d,%d, %d, %d, %d\n",
+                    mLeftOverBuffer[0], mLeftOverBuffer[1], mLeftOverBuffer[2], mLeftOverBuffer[3],
+                    mLeftOverBuffer[4], mLeftOverBuffer[5], mLeftOverBuffer[6], mLeftOverBuffer[7],
+                    mLeftOverBuffer[8], mLeftOverBuffer[9], mLeftOverBuffer[10], mLeftOverBuffer[11],
+                    mLeftOverBuffer[12],mLeftOverBuffer[13],mLeftOverBuffer[14], mLeftOverBuffer[15]);
+
+            mLeftOverBufferSize = readCounter;
+            readCounter = 0;
+            LOGV_IF(0, "Stored number of bytes:%d", mLeftOverBufferSize);            
+        } else {
+            /* reset count since this is the last packet for the data set */
+            readCounter = 0;
+            mLeftOverBufferSize = 0;
+        }
+
+        /* handle data read */
+        if (mask & DATA_FORMAT_GYRO) {
+            /* batch mode does not batch temperature */
+            /* disable temperature read */
+            if (!(mFeatureActiveMask & INV_DMP_BATCH_MODE)) {
+                // send down temperature every 0.5 seconds
+                // with timestamp measured in "driver" layer
+                if(mGyroSensorTimestamp - mTempCurrentTime >= 500000000LL) {
+                    mTempCurrentTime = mGyroSensorTimestamp;
+                    long long temperature[2];
+                    if(inv_read_temperature(temperature) == 0) {
+                        LOGV_IF(INPUT_DATA,
+                        "HAL:input inv_read_temperature = %lld, timestamp= %lld",
+                        temperature[0], temperature[1]);
+                        inv_build_temp(temperature[0], temperature[1]);
+                     }
+#ifdef TESTING
+                    long bias[3], temp, temp_slope[3];
+                    inv_get_mpl_gyro_bias(bias, &temp);
+                    inv_get_gyro_ts(temp_slope);
+                    LOGI("T: %.3f "
+                     "GB: %+13f %+13f %+13f "
+                     "TS: %+13f %+13f %+13f "
+                     "\n",
+                     (float)temperature[0] / 65536.f,
+                     (float)bias[0] / 65536.f / 16.384f,
+                     (float)bias[1] / 65536.f / 16.384f,
+                     (float)bias[2] / 65536.f / 16.384f,
+                     temp_slope[0] / 65536.f,
+                     temp_slope[1] / 65536.f,
+                     temp_slope[2] / 65536.f);
+#endif
+                }
+            }
+            mPendingMask |= 1 << Gyro;
+            mPendingMask |= 1 << RawGyro;
+ 
+            inv_build_gyro(mCachedGyroData, mGyroSensorTimestamp);
+            LOGV_IF(INPUT_DATA, 
+                   "HAL:input inv_build_gyro: %+8d %+8d %+8d - %lld",
+                    mCachedGyroData[0], mCachedGyroData[1],
+                    mCachedGyroData[2], mGyroSensorTimestamp);
+            latestTimestamp = mGyroSensorTimestamp;
+        }
+
+        if (mask & DATA_FORMAT_ACCEL) {
+            mPendingMask |= 1 << Accelerometer;
+            inv_build_accel(mCachedAccelData, 0, mAccelSensorTimestamp);
+            LOGV_IF(INPUT_DATA,
+               "HAL:input inv_build_accel: %+8ld %+8ld %+8ld - %lld",
+                mCachedAccelData[0], mCachedAccelData[1],
+                mCachedAccelData[2], mAccelSensorTimestamp);
+                /* remember inital 6 axis quaternion */
+                inv_time_t tempTimestamp;
+                inv_get_6axis_quaternion(mInitial6QuatValue, &tempTimestamp);
+                if (mInitial6QuatValue[0] != 0 && mInitial6QuatValue[1] != 0 && 
+                        mInitial6QuatValue[2] != 0 && mInitial6QuatValue[3] != 0) {
+                    mInitial6QuatValueAvailable = 1;
+                    LOGV_IF(INPUT_DATA && ENG_VERBOSE,
+                        "HAL:input build 6q init: %+8ld %+8ld %+8ld %+8ld",
+                        mInitial6QuatValue[0], mInitial6QuatValue[1],
+                        mInitial6QuatValue[2], mInitial6QuatValue[3]);
+                }
+            latestTimestamp = mAccelSensorTimestamp;
+        }
+
+        if ((mask & DATA_FORMAT_COMPASS) && mCompassSensor->isIntegrated()) {
+            int status = 0;
+            if (mCompassSensor->providesCalibration()) {
+                status = mCompassSensor->getAccuracy();
+                status |= INV_CALIBRATED;
+            }
+            inv_build_compass(mCachedCompassData, status,
+                              mCompassTimestamp);
+            LOGV_IF(INPUT_DATA,
+                    "HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld",
+                    mCachedCompassData[0], mCachedCompassData[1],
+                    mCachedCompassData[2], mCompassTimestamp);
+            latestTimestamp = mCompassTimestamp;
+        }
+
+        if (mask & DATA_FORMAT_QUAT) {
+            /* if bias was applied to DMP bias,
+               set status bits to disable gyro bias cal */
+            int status = 0;
+            if (mGyroBiasApplied == true) {
+                LOGV_IF(INPUT_DATA && ENG_VERBOSE, "HAL:input dmp bias is used");
+                status |= INV_BIAS_APPLIED;
+            }
+            status |= INV_CALIBRATED | INV_QUAT_3AXIS | INV_QUAT_3ELEMENT; /* default 32 (16/32bits) */
+            inv_build_quat(mCachedQuaternionData,
+                       status,
+                       mQuatSensorTimestamp);
+            LOGV_IF(INPUT_DATA,
+                    "HAL:input inv_build_quat-3x: %+8ld %+8ld %+8ld - %lld",
+                    mCachedQuaternionData[0], mCachedQuaternionData[1],
+                    mCachedQuaternionData[2],
+                    mQuatSensorTimestamp);
+            latestTimestamp = mQuatSensorTimestamp;
+        }
+
+        if (mask & DATA_FORMAT_6_AXIS) {
+            /* if bias was applied to DMP bias,
+               set status bits to disable gyro bias cal */
+            int status = 0;
+            if (mGyroBiasApplied == true) {
+                LOGV_IF(INPUT_DATA && ENG_VERBOSE, "HAL:input dmp bias is used");
+                status |= INV_QUAT_6AXIS;
+            }
+            status |= INV_CALIBRATED | INV_QUAT_6AXIS | INV_QUAT_3ELEMENT; /* default 32 (16/32bits) */
+            inv_build_quat(mCached6AxisQuaternionData,
+                       status,
+                       mQuatSensorTimestamp);
+            LOGV_IF(INPUT_DATA,
+                    "HAL:input inv_build_quat-6x: %+8ld %+8ld %+8ld - %lld",
+                    mCached6AxisQuaternionData[0], mCached6AxisQuaternionData[1],
+                    mCached6AxisQuaternionData[2], mQuatSensorTimestamp);
+            latestTimestamp = mQuatSensorTimestamp;
+        }
+
+        if (mask & DATA_FORMAT_PED_QUAT) {
+            /* if bias was applied to DMP bias,
+               set status bits to disable gyro bias cal */
+            int status = 0;
+            if (mGyroBiasApplied == true) {
+                LOGV_IF(INPUT_DATA && ENG_VERBOSE, 
+                        "HAL:input dmp bias is used");
+                status |= INV_QUAT_6AXIS;
+            }
+            status |= INV_CALIBRATED | INV_QUAT_6AXIS | INV_QUAT_3ELEMENT; /* default 32 (16/32bits) */
+            mCachedPedQuaternionData[0] = mCachedPedQuaternionData[0] << 16;
+            mCachedPedQuaternionData[1] = mCachedPedQuaternionData[1] << 16;
+            mCachedPedQuaternionData[2] = mCachedPedQuaternionData[2] << 16;
+            inv_build_quat(mCachedPedQuaternionData,
+                       status,
+                       mQuatSensorTimestamp);
+
+            LOGV_IF(INPUT_DATA,
+                    "HAL:HAL:input inv_build_quat-ped_6x: %+8ld %+8ld %+8ld - %lld",
+                    mCachedPedQuaternionData[0], mCachedPedQuaternionData[1],
+                    mCachedPedQuaternionData[2], mQuatSensorTimestamp);
+            latestTimestamp = mQuatSensorTimestamp;
+        }
+
+        if ((mask & DATA_FORMAT_PRESSURE) && mPressureSensor->isIntegrated()) {
+            int status = 0;
+            if (mLocalSensorMask & INV_ONE_AXIS_PRESSURE) {
+                latestTimestamp = mPressureTimestamp;
+                mPressureUpdate = 1;
+                inv_build_pressure(mCachedPressureData,
+                            status,
+                            mPressureTimestamp);           
+                LOGV_IF(INPUT_DATA,
+                    "HAL:input inv_build_pressure: %+8ld - %lld",
+                    mCachedPressureData, mPressureTimestamp);                            
+            }
+        }
+
+        /* take the latest timestamp */
+        if (mask & DATA_FORMAT_STEP) {
+        /* work around driver output duplicate step detector bit */
+            if (latestTimestamp > mStepSensorTimestamp) {
+                mStepSensorTimestamp = latestTimestamp;
+                LOGV_IF(INPUT_DATA,
+                    "HAL:input build step: 1 - %lld", mStepSensorTimestamp);                    
+            } else {
+                mPedUpdate = 0;
+            }
+        }
+   }    //while end
+}
+
+int MPLSensor::checkValidHeader(unsigned short data_format)
+{
+    if(data_format & DATA_FORMAT_STEP) {
+        data_format &= (~DATA_FORMAT_STEP);
+    }
+   
+    LOGV_IF(ENG_VERBOSE && INPUT_DATA, "check data_format=%x", data_format);
+    
+    if ((data_format == DATA_FORMAT_PED_STANDALONE) ||
+        (data_format == DATA_FORMAT_PED_QUAT) ||
+        (data_format == DATA_FORMAT_6_AXIS) ||
+        (data_format == DATA_FORMAT_QUAT) ||
+        (data_format == DATA_FORMAT_COMPASS) ||
+        (data_format == DATA_FORMAT_GYRO) ||
+        (data_format == DATA_FORMAT_ACCEL) ||
+        (data_format == DATA_FORMAT_PRESSURE) ||
+        (data_format == DATA_FORMAT_EMPTY_MARKER) ||
+        (data_format == DATA_FORMAT_MARKER))
+            return 1;
+     else {
+        LOGV_IF(ENG_VERBOSE, "bad data_format = %x", data_format);
+        return 0;
+    }
+}
+
+/* use for both MPUxxxx and third party compass */
+void MPLSensor::buildCompassEvent(void)
+{
+    VHANDLER_LOG;
+
+    int done = 0;
+
+    // pthread_mutex_lock(&mMplMutex);
+    // pthread_mutex_lock(&mHALMutex);
+
+    done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp);
+    if(mCompassSensor->isYasCompass()) {
+        if (mCompassSensor->checkCoilsReset() == 1) {
+           //Reset relevant compass settings
+           resetCompass();
+        }
+    }
+    if (done > 0) {
+        int status = 0;
+        if (mCompassSensor->providesCalibration()) {
+            status = mCompassSensor->getAccuracy();
+            status |= INV_CALIBRATED;
+        }
+        if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
+            inv_build_compass(mCachedCompassData, status,
+                              mCompassTimestamp);
+            LOGV_IF(INPUT_DATA,
+                    "HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld",
+                    mCachedCompassData[0], mCachedCompassData[1],
+                    mCachedCompassData[2], mCompassTimestamp);
+        }
+    }
+
+    // pthread_mutex_unlock(&mMplMutex);
+    // pthread_mutex_unlock(&mHALMutex);
+}
+
+int MPLSensor::resetCompass(void)
+{
+    VFUNC_LOG;
+
+    //Reset compass cal if enabled
+    if (mMplFeatureActiveMask & INV_COMPASS_CAL) {
+       LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal");
+       inv_init_vector_compass_cal();
+    }
+
+    //Reset compass fit if enabled
+    if (mMplFeatureActiveMask & INV_COMPASS_FIT) {
+       LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit");
+       inv_init_compass_fit();
+    }
+
+    return 0;
+}
+
+int MPLSensor::getFd(void) const
+{
+    VFUNC_LOG;
+    LOGV_IF(EXTRA_VERBOSE, "getFd returning %d", iio_fd);
+    return iio_fd;
+}
+
+int MPLSensor::getAccelFd(void) const
+{
+    VFUNC_LOG;
+    LOGV_IF(EXTRA_VERBOSE, "getAccelFd returning %d", accel_fd);
+    return accel_fd;
+}
+
+int MPLSensor::getCompassFd(void) const
+{
+    VFUNC_LOG;
+    int fd = mCompassSensor->getFd();
+    LOGV_IF(EXTRA_VERBOSE, "getCompassFd returning %d", fd);
+    return fd;
+}
+
+int MPLSensor::turnOffAccelFifo(void)
+{
+    VFUNC_LOG;
+    int i, res = 0, tempFd;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        0, mpu.accel_fifo_enable, getTimestamp());
+    res += write_sysfs_int(mpu.accel_fifo_enable, 0);
+    return res;
+}
+
+int MPLSensor::turnOffGyroFifo(void)
+{
+    VFUNC_LOG;
+    int i, res = 0, tempFd;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        0, mpu.gyro_fifo_enable, getTimestamp());
+    res += write_sysfs_int(mpu.gyro_fifo_enable, 0);
+    return res;
+}
+
+int MPLSensor::enableDmpOrientation(int en)
+{
+    VFUNC_LOG;
+    int res = 0;
+    int enabled_sensors = mEnabled;
+
+    if (isMpuNonDmp())
+        return res;
+
+    // reset master enable
+    res = masterEnable(0);
+    if (res < 0)
+        return res;
+
+    if (en == 1) {
+        // Enable DMP orientation
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                en, mpu.display_orientation_on, getTimestamp());
+        if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
+            LOGE("HAL:ERR can't enable Android orientation");
+            res = -1;	// indicate an err
+            return res;
+        }
+
+        // enable accel engine
+        res = enableAccel(1);
+        if (res < 0)
+            return res;
+
+        // disable accel FIFO
+        if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) {
+            res = turnOffAccelFifo();
+            if (res < 0)
+                return res;
+        }
+
+        if (!mEnabled){
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                       1, mpu.dmp_event_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
+                res = -1;
+                LOGE("HAL:ERR can't enable DMP event interrupt");
+            }
+        }
+
+        mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION;
+        LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
+    } else {
+        mFeatureActiveMask &= ~INV_DMP_DISPL_ORIENTATION;
+
+        if (mFeatureActiveMask == 0) {
+            // disable accel engine
+            if (!(mLocalSensorMask & mMasterSensorMask
+                    & INV_THREE_AXIS_ACCEL)) {
+                res = enableAccel(0);
+                if (res < 0)
+                    return res;
+            }
+        }
+        
+        if (mEnabled){
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                       en, mpu.dmp_event_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
+                res = -1;
+                LOGE("HAL:ERR can't enable DMP event interrupt");
+            }
+        }
+        LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
+    }
+
+    if ((res = computeAndSetDmpState()) < 0)
+        return res;
+
+    if (en || mEnabled || mFeatureActiveMask) {
+        res = masterEnable(1);
+    }
+    return res;
+}
+
+int MPLSensor::openDmpOrientFd(void)
+{
+    VFUNC_LOG;
+
+    if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) {
+        LOGV_IF(PROCESS_VERBOSE,
+                "HAL:DMP display orientation disabled or file desc opened");
+        return 0;
+    }
+
+    dmp_orient_fd = open(mpu.event_display_orientation, O_RDONLY| O_NONBLOCK);
+    if (dmp_orient_fd < 0) {
+        LOGE("HAL:ERR couldn't open dmpOrient node");
+        return -1;
+    } else {
+        LOGV_IF(PROCESS_VERBOSE,
+                "HAL:dmp_orient_fd opened : %d", dmp_orient_fd);
+        return 0;
+    }
+}
+
+int MPLSensor::closeDmpOrientFd(void)
+{
+    VFUNC_LOG;
+    if (dmp_orient_fd >= 0)
+        close(dmp_orient_fd);
+    return 0;
+}
+
+int MPLSensor::dmpOrientHandler(int orient)
+{
+    VFUNC_LOG;
+    LOGV_IF(PROCESS_VERBOSE, "HAL:orient %x", orient);
+    return 0;
+}
+
+int MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count)
+{
+    VFUNC_LOG;
+
+    char dummy[4];
+    int screen_orientation = 0;
+    FILE *fp;
+
+    fp = fopen(mpu.event_display_orientation, "r");
+    if (fp == NULL) {
+        LOGE("HAL:cannot open event_display_orientation");
+        return 0;
+    } else {
+        if (fscanf(fp, "%d\n", &screen_orientation) < 0 || fclose(fp) < 0)
+        {
+            LOGE("HAL:cannot write event_display_orientation");
+        }
+    }
+
+    int numEventReceived = 0;
+
+    if (mDmpOrientationEnabled && count > 0) {
+        sensors_event_t temp;
+
+        temp.acceleration.x = 0;
+        temp.acceleration.y = 0;
+        temp.acceleration.z = 0;
+        temp.version = sizeof(sensors_event_t);
+        temp.sensor = ID_SO;
+        temp.acceleration.status
+            = SENSOR_STATUS_UNRELIABLE;
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+        temp.type = SENSOR_TYPE_SCREEN_ORIENTATION;
+        temp.screen_orientation = screen_orientation;
+#endif
+        struct timespec ts;
+        clock_gettime(CLOCK_MONOTONIC, &ts);
+        temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
+
+        *data++ = temp;
+        count--;
+        numEventReceived++;
+    }
+
+    // read dummy data per driver's request
+    dmpOrientHandler(screen_orientation);
+    read(dmp_orient_fd, dummy, 4);
+
+    return numEventReceived;
+}
+
+int MPLSensor::getDmpOrientFd(void)
+{
+    VFUNC_LOG;
+
+    LOGV_IF(EXTRA_VERBOSE, "getDmpOrientFd returning %d", dmp_orient_fd);
+    return dmp_orient_fd;
+
+}
+
+int MPLSensor::checkDMPOrientation(void)
+{
+    VFUNC_LOG;
+    return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0);
+}
+
+int MPLSensor::getDmpRate(int64_t *wanted)
+{
+    VFUNC_LOG;
+
+      // set DMP output rate to FIFO
+      if(mDmpOn == 1) {
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                int(1000000000.f / *wanted), mpu.three_axis_q_rate,
+                getTimestamp());
+        write_sysfs_int(mpu.three_axis_q_rate, 1000000000.f / *wanted);
+        LOGV_IF(PROCESS_VERBOSE,
+                    "HAL:DMP three axis rate %.2f Hz", 1000000000.f / *wanted);
+        if (mFeatureActiveMask & INV_DMP_BATCH_MODE) {
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    int(1000000000.f / *wanted), mpu.six_axis_q_rate,
+                    getTimestamp());
+            write_sysfs_int(mpu.six_axis_q_rate, 1000000000.f / *wanted);
+            LOGV_IF(PROCESS_VERBOSE,
+                    "HAL:DMP six axis rate %.2f Hz", 1000000000.f / *wanted);
+                    
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    int(1000000000.f / *wanted), mpu.ped_q_rate,
+                    getTimestamp());
+            write_sysfs_int(mpu.ped_q_rate, 1000000000.f / *wanted);
+            LOGV_IF(PROCESS_VERBOSE,
+                    "HAL:DMP ped quaternion rate %.2f Hz", 1000000000.f / *wanted);
+        }
+        // DMP running rate must be @ 200Hz
+        *wanted= RATE_200HZ;
+        LOGV_IF(PROCESS_VERBOSE,
+                "HAL:DMP rate= %.2f Hz", 1000000000.f / *wanted);
+    }
+    return 0;
+}
+
+int MPLSensor::getPollTime(void)
+{
+    VFUNC_LOG;
+    return mPollTime;
+}
+
+int MPLSensor::getStepCountPollTime(void)
+{
+    VFUNC_LOG;
+    /* clamped to 1ms? as spec, still rather large */
+    return 1000;
+}
+
+bool MPLSensor::hasStepCountPendingEvents(void)
+{
+    VFUNC_LOG;
+    if (mDmpStepCountEnabled) {
+        struct timespec t_now;
+        int64_t interval = 0;
+
+        clock_gettime(CLOCK_MONOTONIC, &t_now);
+        interval = ((int64_t(t_now.tv_sec) * 1000000000LL + t_now.tv_nsec) -
+                    (int64_t(mt_pre.tv_sec) * 1000000000LL + mt_pre.tv_nsec));
+
+        if (interval < mStepCountPollTime) {
+            LOGV_IF(0,
+                    "Step Count interval elapsed: %lld, triggered: %lld",
+                    interval, mStepCountPollTime);
+            return false;
+        } else {
+            clock_gettime(CLOCK_MONOTONIC, &mt_pre);
+            LOGV_IF(0, "Step Count previous time: %ld ms",
+                    mt_pre.tv_nsec / 1000);
+            return true;
+        }
+    }
+    return false;
+}
+
+bool MPLSensor::hasPendingEvents(void) const
+{
+    VFUNC_LOG;
+    // if we are using the polling workaround, force the main
+    // loop to check for data every time
+    return (mPollTime != -1);
+}
+
+int MPLSensor::inv_read_temperature(long long *data)
+{
+    VHANDLER_LOG;
+
+    int count = 0;
+    char raw_buf[40];
+    long raw = 0;
+
+    long long timestamp = 0;
+
+    memset(raw_buf, 0, sizeof(raw_buf));
+    count = read_attribute_sensor(gyro_temperature_fd, raw_buf,
+                                  sizeof(raw_buf));
+    if(count < 1) {
+        LOGE("HAL:error reading gyro temperature");
+        return -1;
+    }
+
+    count = sscanf(raw_buf, "%ld%lld", &raw, &timestamp);
+
+    if(count < 0) {
+        return -1;
+    }
+
+    LOGV_IF(ENG_VERBOSE && INPUT_DATA,
+            "HAL:temperature raw = %ld, timestamp = %lld, count = %d",
+            raw, timestamp, count);
+    data[0] = raw;
+    data[1] = timestamp;
+
+    return 0;
+}
+
+int MPLSensor::inv_read_dmp_state(int fd)
+{
+    VFUNC_LOG;
+
+    if(fd < 0)
+        return -1;
+
+    int count = 0;
+    char raw_buf[10];
+    short raw = 0;
+
+    memset(raw_buf, 0, sizeof(raw_buf));
+    count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf));
+    if(count < 1) {
+        LOGE("HAL:error reading dmp state");
+        close(fd);
+        return -1;
+    }
+    count = sscanf(raw_buf, "%hd", &raw);
+    if(count < 0) {
+        LOGE("HAL:dmp state data is invalid");
+        close(fd);
+        return -1;
+    }
+    LOGV_IF(EXTRA_VERBOSE, "HAL:dmp state = %d, count = %d", raw, count);
+    close(fd);
+    return (int)raw;
+}
+
+int MPLSensor::inv_read_sensor_bias(int fd, long *data)
+{
+    VFUNC_LOG;
+
+    if(fd == -1) {
+        return -1;
+    }
+
+    char buf[50];
+    char x[15], y[15], z[15];
+
+    memset(buf, 0, sizeof(buf));
+    int count = read_attribute_sensor(fd, buf, sizeof(buf));
+    if(count < 1) {
+        LOGE("HAL:Error reading gyro bias");
+        return -1;
+    }
+    count = sscanf(buf, "%[^','],%[^','],%[^',']", x, y, z);
+    if(count) {
+        /* scale appropriately for MPL */
+        LOGV_IF(ENG_VERBOSE,
+                "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)",
+                atol(x), atol(y), atol(z));
+
+        data[0] = (long)(atol(x) / 10000 * (1L << 16));
+        data[1] = (long)(atol(y) / 10000 * (1L << 16));
+        data[2] = (long)(atol(z) / 10000 * (1L << 16));
+
+        LOGV_IF(ENG_VERBOSE,
+                "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)",
+                data[0], data[1], data[2]);
+    }
+    return 0;
+}
+
+/** fill in the sensor list based on which sensors are configured.
+ *  return the number of configured sensors.
+ *  parameter list must point to a memory region of at least 7*sizeof(sensor_t)
+ *  parameter len gives the length of the buffer pointed to by list
+ */
+int MPLSensor::populateSensorList(struct sensor_t *list, int len)
+{
+    VFUNC_LOG;
+
+    int numsensors;
+
+    if(len <
+        (int)((sizeof(sBaseSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) {
+        LOGE("HAL:sensor list too small, not populating.");
+        return -(sizeof(sBaseSensorList) / sizeof(sensor_t));
+    }
+
+    /* fill in the base values */
+    memcpy(list, sBaseSensorList,
+           sizeof (struct sensor_t) * (sizeof(sBaseSensorList) / sizeof(sensor_t)));
+
+    /* first add gyro, accel and compass to the list */
+
+    /* fill in gyro/accel values */
+    if(chip_ID == NULL) {
+        LOGE("HAL:Can not get gyro/accel id");
+    }
+    fillGyro(chip_ID, list);
+    fillAccel(chip_ID, list);
+
+    // TODO: need fixes for unified HAL and 3rd-party solution
+    mCompassSensor->fillList(&list[MagneticField]);
+    mCompassSensor->fillList(&list[RawMagneticField]);
+
+    if (mPressureSensor != NULL) {
+        mPressureSensor->fillList(&list[Pressure]);
+    }
+
+    if(1) {
+        numsensors = (sizeof(sBaseSensorList) / sizeof(sensor_t));
+        /* all sensors will be added to the list
+           fill in orientation values */
+        fillOrientation(list);
+        /* fill in rotation vector values */
+        fillRV(list);
+        /* fill in game rotation vector values */
+        fillGRV(list);
+        /* fill in gravity values */
+        fillGravity(list);
+        /* fill in Linear accel values */
+        fillLinearAccel(list);
+        /* fill in Significant motion values */
+        fillSignificantMotion(list);
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+        /* fill in screen orientation values */
+        fillScreenOrientation(list);
+#endif
+    } else {
+        /* no 9-axis sensors, zero fill that part of the list */
+        numsensors = 3;
+        memset(list + 3, 0, 4 * sizeof(struct sensor_t));
+    }
+
+    return numsensors;
+}
+
+void MPLSensor::fillAccel(const char* accel, struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    if (accel) {
+        if(accel != NULL && strcmp(accel, "BMA250") == 0) {
+            list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
+            list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
+            list[Accelerometer].power = ACCEL_BMA250_POWER;
+            list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU6050") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU6050_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY;
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU6500_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
+            return;
+         } else if (accel != NULL && strcmp(accel, "MPU6515") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU6500_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU6500_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU6500_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU9150") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU9150_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU9150_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU9150_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY;
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU9250") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU9250_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU9250_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU9250_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU9250_MINDELAY;
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU9350") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU9350_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU9350_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU9350_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU9350_MINDELAY;
+            return;
+        }  else if (accel != NULL && strcmp(accel, "MPU3050") == 0) {
+            list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
+            list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
+            list[Accelerometer].power = ACCEL_BMA250_POWER;
+            list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
+            return;
+        }
+    }
+
+    LOGE("HAL:unknown accel id %s -- "
+         "params default to mpu6515 and might be wrong.",
+         accel);
+    list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
+    list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
+    list[Accelerometer].power = ACCEL_MPU6500_POWER;
+    list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
+}
+
+void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    if ( gyro != NULL && strcmp(gyro, "MPU3050") == 0) {
+        list[Gyro].maxRange = GYRO_MPU3050_RANGE;
+        list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
+        list[Gyro].power = GYRO_MPU3050_POWER;
+        list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
+    } else if( gyro != NULL && strcmp(gyro, "MPU6050") == 0) {
+        list[Gyro].maxRange = GYRO_MPU6050_RANGE;
+        list[Gyro].resolution = GYRO_MPU6050_RESOLUTION;
+        list[Gyro].power = GYRO_MPU6050_POWER;
+        list[Gyro].minDelay = GYRO_MPU6050_MINDELAY;
+    } else if( gyro != NULL && strcmp(gyro, "MPU6500") == 0) {
+        list[Gyro].maxRange = GYRO_MPU6500_RANGE;
+        list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
+        list[Gyro].power = GYRO_MPU6500_POWER;
+        list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
+     } else if( gyro != NULL && strcmp(gyro, "MPU6515") == 0) {
+        list[Gyro].maxRange = GYRO_MPU6500_RANGE;
+        list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
+        list[Gyro].power = GYRO_MPU6500_POWER;
+        list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
+    } else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) {
+        list[Gyro].maxRange = GYRO_MPU9150_RANGE;
+        list[Gyro].resolution = GYRO_MPU9150_RESOLUTION;
+        list[Gyro].power = GYRO_MPU9150_POWER;
+        list[Gyro].minDelay = GYRO_MPU9150_MINDELAY;
+    } else if( gyro != NULL && strcmp(gyro, "MPU9250") == 0) {
+        list[Gyro].maxRange = GYRO_MPU9250_RANGE;
+        list[Gyro].resolution = GYRO_MPU9250_RESOLUTION;
+        list[Gyro].power = GYRO_MPU9250_POWER;
+        list[Gyro].minDelay = GYRO_MPU9250_MINDELAY;
+    } else if( gyro != NULL && strcmp(gyro, "MPU9350") == 0) {
+        list[Gyro].maxRange = GYRO_MPU9350_RANGE;
+        list[Gyro].resolution = GYRO_MPU9350_RESOLUTION;
+        list[Gyro].power = GYRO_MPU9350_POWER;
+        list[Gyro].minDelay = GYRO_MPU9350_MINDELAY;
+    } else {
+        LOGE("HAL:unknown gyro id -- gyro params will be wrong.");
+        LOGE("HAL:default to use mpu6515 params");
+        list[Gyro].maxRange = GYRO_MPU6500_RANGE;
+        list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
+        list[Gyro].power = GYRO_MPU6500_POWER;
+        list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
+    }
+
+    list[RawGyro].maxRange = list[Gyro].maxRange;
+    list[RawGyro].resolution = list[Gyro].resolution;
+    list[RawGyro].power = list[Gyro].power;
+    list[RawGyro].minDelay = list[Gyro].minDelay;
+
+    return;
+}
+
+/* fillRV depends on values of gyro, accel and compass in the list */
+void MPLSensor::fillRV(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    /* compute power on the fly */
+    list[RotationVector].power = list[Gyro].power +
+                                 list[Accelerometer].power +
+                                 list[MagneticField].power;
+    list[RotationVector].resolution = .00001;
+    list[RotationVector].maxRange = 1.0;
+    list[RotationVector].minDelay = 5000;
+
+    return;
+}
+
+/* fillGMRV depends on values of accel and mag in the list */
+void MPLSensor::fillGMRV(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    /* compute power on the fly */
+    list[GeomagneticRotationVector].power = list[Accelerometer].power +
+                                 list[MagneticField].power;
+    list[GeomagneticRotationVector].resolution = .00001;
+    list[GeomagneticRotationVector].maxRange = 1.0;
+    list[GeomagneticRotationVector].minDelay = 5000;
+
+    return;
+}
+
+/* fillGRV depends on values of gyro and accel in the list */
+void MPLSensor::fillGRV(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    /* compute power on the fly */
+    list[GameRotationVector].power = list[Gyro].power +
+                                 list[Accelerometer].power;
+    list[GameRotationVector].resolution = .00001;
+    list[GameRotationVector].maxRange = 1.0;
+    list[GameRotationVector].minDelay = 5000;
+
+    return;
+}
+
+void MPLSensor::fillOrientation(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    list[Orientation].power = list[Gyro].power +
+                              list[Accelerometer].power +
+                              list[MagneticField].power;
+    list[Orientation].resolution = .00001;
+    list[Orientation].maxRange = 360.0;
+    list[Orientation].minDelay = 5000;
+
+    return;
+}
+
+void MPLSensor::fillGravity( struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    list[Gravity].power = list[Gyro].power +
+                          list[Accelerometer].power +
+                          list[MagneticField].power;
+    list[Gravity].resolution = .00001;
+    list[Gravity].maxRange = 9.81;
+    list[Gravity].minDelay = 5000;
+
+    return;
+}
+
+void MPLSensor::fillLinearAccel(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    list[LinearAccel].power = list[Gyro].power +
+                          list[Accelerometer].power +
+                          list[MagneticField].power;
+    list[LinearAccel].resolution = list[Accelerometer].resolution;
+    list[LinearAccel].maxRange = list[Accelerometer].maxRange;
+    list[LinearAccel].minDelay = 5000;
+
+    return;
+}
+
+void MPLSensor::fillSignificantMotion(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    list[SignificantMotion].power = list[Accelerometer].power;
+    list[SignificantMotion].resolution = 1;
+    list[SignificantMotion].maxRange = 1;
+    list[SignificantMotion].minDelay = -1;
+}
+
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+void MPLSensor::fillScreenOrientation(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    list[NumSensors].power = list[Accelerometer].power;
+    list[NumSensors].resolution = 1;
+    list[NumSensors].maxRange = 3;
+    list[NumSensors].minDelay = 0;
+}
+#endif
+
+int MPLSensor::inv_init_sysfs_attributes(void)
+{
+    VFUNC_LOG;
+
+    unsigned char i = 0;
+    char sysfs_path[MAX_SYSFS_NAME_LEN];
+    char tbuf[2];
+    char *sptr;
+    char **dptr;
+    int num;
+
+    memset(sysfs_path, 0, sizeof(sysfs_path));
+
+    sysfs_names_ptr =
+            (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
+    sptr = sysfs_names_ptr;
+    if (sptr != NULL) {
+        dptr = (char**)&mpu;
+        do {
+            *dptr++ = sptr;
+            memset(sptr, 0, sizeof(sptr));
+            sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
+        } while (++i < MAX_SYSFS_ATTRB);
+    } else {
+        LOGE("HAL:couldn't alloc mem for sysfs paths");
+        return -1;
+    }
+
+    // get proper (in absolute) IIO path & build MPU's sysfs paths
+    inv_get_sysfs_path(sysfs_path);
+
+    memcpy(mSysfsPath, sysfs_path, sizeof(sysfs_path));
+    sprintf(mpu.key, "%s%s", sysfs_path, "/key");
+    sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable");
+    sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length");
+    sprintf(mpu.master_enable, "%s%s", sysfs_path, "/master_enable");
+    sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
+
+    sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path,
+            "/scan_elements/in_timestamp_en");
+    sprintf(mpu.in_timestamp_index, "%s%s", sysfs_path,
+            "/scan_elements/in_timestamp_index");
+    sprintf(mpu.in_timestamp_type, "%s%s", sysfs_path,
+            "/scan_elements/in_timestamp_type");
+
+    sprintf(mpu.dmp_firmware, "%s%s", sysfs_path, "/dmp_firmware");
+    sprintf(mpu.firmware_loaded, "%s%s", sysfs_path, "/firmware_loaded");
+    sprintf(mpu.dmp_on, "%s%s", sysfs_path, "/dmp_on");
+    sprintf(mpu.dmp_int_on, "%s%s", sysfs_path, "/dmp_int_on");
+    sprintf(mpu.dmp_event_int_on, "%s%s", sysfs_path, "/dmp_event_int_on");
+    sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
+
+    sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
+
+    sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature");
+    sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable");
+    sprintf(mpu.gyro_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
+    sprintf(mpu.gyro_orient, "%s%s", sysfs_path, "/gyro_matrix");
+    sprintf(mpu.gyro_fifo_enable, "%s%s", sysfs_path, "/gyro_fifo_enable");
+    sprintf(mpu.gyro_fsr, "%s%s", sysfs_path, "/in_anglvel_scale");
+    sprintf(mpu.gyro_fifo_enable, "%s%s", sysfs_path, "/gyro_fifo_enable");
+    sprintf(mpu.gyro_rate, "%s%s", sysfs_path, "/gyro_rate");
+
+    sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accel_enable");
+    sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
+    sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accel_matrix");
+    sprintf(mpu.accel_fifo_enable, "%s%s", sysfs_path, "/accel_fifo_enable");
+    sprintf(mpu.accel_rate, "%s%s", sysfs_path, "/accel_rate");
+
+#ifndef THIRD_PARTY_ACCEL //MPU3050
+    sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale");
+
+    // DMP uses these values
+    sprintf(mpu.in_accel_x_dmp_bias, "%s%s", sysfs_path, "/in_accel_x_dmp_bias");
+    sprintf(mpu.in_accel_y_dmp_bias, "%s%s", sysfs_path, "/in_accel_y_dmp_bias");
+    sprintf(mpu.in_accel_z_dmp_bias, "%s%s", sysfs_path, "/in_accel_z_dmp_bias");
+    
+    // MPU uses these values 
+    sprintf(mpu.in_accel_x_offset, "%s%s", sysfs_path, "/in_accel_x_offset");
+    sprintf(mpu.in_accel_y_offset, "%s%s", sysfs_path, "/in_accel_y_offset");
+    sprintf(mpu.in_accel_z_offset, "%s%s", sysfs_path, "/in_accel_z_offset");
+    sprintf(mpu.in_accel_self_test_scale, "%s%s", sysfs_path, "/in_accel_self_test_scale");
+#endif
+
+    // DMP uses these bias values
+    sprintf(mpu.in_gyro_x_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_x_dmp_bias");
+    sprintf(mpu.in_gyro_y_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_y_dmp_bias");
+    sprintf(mpu.in_gyro_z_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_z_dmp_bias");
+
+    // MPU uses these bias values
+    sprintf(mpu.in_gyro_x_offset, "%s%s", sysfs_path, "/in_anglvel_x_offset");
+    sprintf(mpu.in_gyro_y_offset, "%s%s", sysfs_path, "/in_anglvel_y_offset");
+    sprintf(mpu.in_gyro_z_offset, "%s%s", sysfs_path, "/in_anglvel_z_offset");
+    sprintf(mpu.in_gyro_self_test_scale, "%s%s", sysfs_path, "/in_anglvel_self_test_scale");
+
+    sprintf(mpu.three_axis_q_on, "%s%s", sysfs_path, "/three_axes_q_on"); //formerly quaternion_on
+    sprintf(mpu.three_axis_q_rate, "%s%s", sysfs_path, "/three_axes_q_rate");
+    
+    sprintf(mpu.ped_q_on, "%s%s", sysfs_path, "/ped_q_on");
+    sprintf(mpu.ped_q_rate, "%s%s", sysfs_path, "/ped_q_rate");
+    
+    sprintf(mpu.six_axis_q_on, "%s%s", sysfs_path, "/six_axes_q_on");
+    sprintf(mpu.six_axis_q_rate, "%s%s", sysfs_path, "/six_axes_q_rate");
+
+    sprintf(mpu.six_axis_q_value, "%s%s", sysfs_path, "/six_axes_q_value");
+
+    sprintf(mpu.step_detector_on, "%s%s", sysfs_path, "/step_detector_on");
+    sprintf(mpu.step_indicator_on, "%s%s", sysfs_path, "/step_indicator_on");
+
+    sprintf(mpu.display_orientation_on, "%s%s", sysfs_path,
+            "/display_orientation_on");
+    sprintf(mpu.event_display_orientation, "%s%s", sysfs_path,
+            "/event_display_orientation");
+
+    sprintf(mpu.event_smd, "%s%s", sysfs_path,
+            "/event_smd");
+    sprintf(mpu.smd_enable, "%s%s", sysfs_path,
+            "/smd_enable");
+    sprintf(mpu.smd_delay_threshold, "%s%s", sysfs_path,
+            "/smd_delay_threshold");
+    sprintf(mpu.smd_delay_threshold2, "%s%s", sysfs_path,
+            "/smd_delay_threshold2");
+    sprintf(mpu.smd_threshold, "%s%s", sysfs_path,
+            "/smd_threshold");
+    sprintf(mpu.batchmode_timeout, "%s%s", sysfs_path,
+            "/batchmode_timeout");
+    sprintf(mpu.batchmode_wake_fifo_full_on, "%s%s", sysfs_path,
+            "/batchmode_wake_fifo_full_on");
+    sprintf(mpu.flush_batch, "%s%s", sysfs_path,
+            "/flush_batch");
+    sprintf(mpu.pedometer_on, "%s%s", sysfs_path,
+            "/pedometer_on");
+    sprintf(mpu.pedometer_int_on, "%s%s", sysfs_path,
+            "/pedometer_int_on");
+    sprintf(mpu.event_pedometer, "%s%s", sysfs_path,
+            "/event_pedometer");
+    sprintf(mpu.pedometer_steps, "%s%s", sysfs_path,
+            "/pedometer_steps");
+    sprintf(mpu.pedometer_counter, "%s%s", sysfs_path,
+            "/pedometer_counter");
+    sprintf(mpu.motion_lpa_on, "%s%s", sysfs_path,
+            "/motion_lpa_on");
+    return 0;
+}
+
+//DMP support only for MPU6xxx/9xxx
+bool MPLSensor::isMpuNonDmp(void)
+{
+    VFUNC_LOG;
+    if (!strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050"))
+        return true;
+    else
+        return false;
+}
+
+int MPLSensor::isLowPowerQuatEnabled(void)
+{
+    VFUNC_LOG;
+#ifdef ENABLE_LP_QUAT_FEAT
+    return !isMpuNonDmp();
+#else
+    return 0;
+#endif
+}
+
+int MPLSensor::isDmpDisplayOrientationOn(void)
+{
+    VFUNC_LOG;
+#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
+    if (isMpuNonDmp())
+        return 0;
+    return 1;
+#else
+    return 0;
+#endif
+}
+
+/* these functions can be consolidated
+with inv_convert_to_body_with_scale */
+void MPLSensor::getCompassBias()
+{
+    VFUNC_LOG;
+
+    
+    long bias[3];
+    long compassBias[3];
+    unsigned short orient;
+    signed char orientMtx[9];
+    mCompassSensor->getOrientationMatrix(orientMtx);
+    orient = inv_orientation_matrix_to_scalar(orientMtx);
+
+    /* Get Values from MPL */
+    inv_get_compass_bias(bias);
+    inv_convert_to_body(orient, bias, compassBias);
+    LOGV_IF(HANDLER_DATA, "Mpl Compass Bias (HW unit) %ld %ld %ld", bias[0], bias[1], bias[2]);
+    LOGV_IF(HANDLER_DATA, "Mpl Compass Bias (HW unit) (body) %ld %ld %ld", compassBias[0], compassBias[1], compassBias[2]);
+    long compassSensitivity = inv_get_compass_sensitivity();
+    if (compassSensitivity == 0) {
+        compassSensitivity = mCompassScale;
+    }
+    for(int i=0; i<3; i++) {
+        /* convert to uT */
+        float temp = (float) compassSensitivity / (1L << 30);
+        mCompassBias[i] =(float) (compassBias[i] * temp / 65536.f);
+    }
+
+    return; 
+}
+
+void MPLSensor::getFactoryGyroBias()
+{
+    VFUNC_LOG;
+
+    /* Get Values from MPL */
+    inv_get_gyro_bias(mFactoryGyroBias);
+    LOGV_IF(ENG_VERBOSE, "Factory Gyro Bias %ld %ld %ld", mFactoryGyroBias[0], mFactoryGyroBias[1], mFactoryGyroBias[2]);
+    mFactoryGyroBiasAvailable = true;
+
+    return;
+}
+
+/* set bias from factory cal file to MPU offset (in chip frame)
+   x = values store in cal file --> (v/1000 * 2^16 / (2000/250))
+   offset = x/2^16 * (Gyro scale / self test scale used) * (-1) / offset scale
+   i.e. self test default scale = 250 
+         gyro scale default to = 2000
+         offset scale = 4 //as spec by hardware
+         offset = x/2^16 * (8) * (-1) / (4)
+*/
+void MPLSensor::setFactoryGyroBias()
+{
+    VFUNC_LOG;
+    int scaleRatio = mGyroScale / mGyroSelfTestScale;
+    int offsetScale = 4;
+    LOGV_IF(ENG_VERBOSE, "HAL: scaleRatio used =%d", scaleRatio);
+    LOGV_IF(ENG_VERBOSE, "HAL: offsetScale used =%d", offsetScale);
+
+    /* Write to Driver */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            (((int) (((float) mFactoryGyroBias[0]) / 65536.f * scaleRatio)) * -1 / offsetScale),
+            mpu.in_gyro_x_offset, getTimestamp());
+    if(write_attribute_sensor_continuous(gyro_x_offset_fd,
+        (((int) (((float) mFactoryGyroBias[0]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0)
+    {
+        LOGE("HAL:Error writing to gyro_x_offset");
+        return;
+    }
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            (((int) (((float) mFactoryGyroBias[1]) / 65536.f * scaleRatio)) * -1 / offsetScale),
+            mpu.in_gyro_y_offset, getTimestamp());
+    if(write_attribute_sensor_continuous(gyro_y_offset_fd,
+        (((int) (((float) mFactoryGyroBias[1]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0)
+    {
+        LOGE("HAL:Error writing to gyro_y_offset");
+        return;
+    }
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            (((int) (((float) mFactoryGyroBias[2]) / 65536.f * scaleRatio)) * -1 / offsetScale),
+            mpu.in_gyro_z_offset, getTimestamp());
+    if(write_attribute_sensor_continuous(gyro_z_offset_fd,
+        (((int) (((float) mFactoryGyroBias[2]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0)
+    {
+        LOGE("HAL:Error writing to gyro_z_offset");
+        return;
+    }
+    mFactoryGyroBiasAvailable = false;
+    LOGV_IF(EXTRA_VERBOSE, "HAL:Factory Gyro Calibrated Bias Applied");
+
+    return;
+}
+
+/* these functions can be consolidated
+with inv_convert_to_body_with_scale */
+void MPLSensor::getGyroBias()
+{
+    VFUNC_LOG;
+
+    long *temp = NULL;
+    long chipBias[3];
+    long bias[3];
+    unsigned short orient;
+
+    /* Get Values from MPL */
+    inv_get_mpl_gyro_bias(mGyroChipBias, temp);
+    orient = inv_orientation_matrix_to_scalar(mGyroOrientation);    
+    inv_convert_to_body(orient, mGyroChipBias, bias);
+    LOGV_IF(ENG_VERBOSE, "Mpl Gyro Bias (HW unit) %ld %ld %ld", mGyroChipBias[0], mGyroChipBias[1], mGyroChipBias[2]);
+    LOGV_IF(ENG_VERBOSE, "Mpl Gyro Bias (HW unit) (body) %ld %ld %ld", bias[0], bias[1], bias[2]);
+    long gyroSensitivity = inv_get_gyro_sensitivity();
+    if(gyroSensitivity == 0) {
+        gyroSensitivity = mGyroScale;
+    }
+
+    /* scale and convert to rad */
+    for(int i=0; i<3; i++) {
+        float temp = (float) gyroSensitivity / (1L << 30);
+        mGyroBias[i] = (float) (bias[i] * temp / (1<<16) / 180 * M_PI);
+        if (mGyroBias[i] != 0)
+            mGyroBiasAvailable = true;
+    }
+
+    return;
+}
+
+void MPLSensor::setGyroZeroBias()
+{
+    VFUNC_LOG;
+   
+    /* Write to Driver */
+    LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %d > %s (%lld)",
+            0, mpu.in_gyro_x_dmp_bias, getTimestamp());
+    if(write_attribute_sensor_continuous(gyro_x_dmp_bias_fd, 0) < 0) {
+        LOGE("HAL:Error writing to gyro_x_dmp_bias");
+        return;
+    }
+    LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %d > %s (%lld)",
+            0, mpu.in_gyro_y_dmp_bias, getTimestamp());
+    if(write_attribute_sensor_continuous(gyro_y_dmp_bias_fd, 0) < 0) {
+        LOGE("HAL:Error writing to gyro_y_dmp_bias");
+        return;
+    }
+    LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %d > %s (%lld)",
+            0, mpu.in_gyro_z_dmp_bias, getTimestamp());
+    if(write_attribute_sensor_continuous(gyro_z_dmp_bias_fd, 0) < 0) {
+        LOGE("HAL:Error writing to gyro_z_dmp_bias");
+        return;
+    }   
+    LOGV_IF(EXTRA_VERBOSE, "HAL:Zero Gyro DMP Calibrated Bias Applied");
+
+    return;
+}
+
+void MPLSensor::setGyroBias()
+{
+    VFUNC_LOG;
+
+    if(mGyroBiasAvailable == false)
+        return;
+
+    long bias[3];
+    long gyroSensitivity = inv_get_gyro_sensitivity();
+
+    if(gyroSensitivity == 0) {
+        gyroSensitivity = mGyroScale;
+    }
+    
+    inv_get_gyro_bias_dmp_units(bias);
+
+    /* Write to Driver */
+    LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)",
+            bias[0], mpu.in_gyro_x_dmp_bias, getTimestamp());
+    if(write_attribute_sensor_continuous(gyro_x_dmp_bias_fd, bias[0]) < 0) {
+        LOGE("HAL:Error writing to gyro_x_dmp_bias");
+        return;
+    }
+    LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)",
+            bias[1], mpu.in_gyro_y_dmp_bias, getTimestamp());
+    if(write_attribute_sensor_continuous(gyro_y_dmp_bias_fd, bias[1]) < 0) {
+        LOGE("HAL:Error writing to gyro_y_dmp_bias");
+        return;
+    }
+    LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)",
+            bias[2], mpu.in_gyro_z_dmp_bias, getTimestamp());
+    if(write_attribute_sensor_continuous(gyro_z_dmp_bias_fd, bias[2]) < 0) {
+        LOGE("HAL:Error writing to gyro_z_dmp_bias");
+        return;
+    }
+    mGyroBiasApplied = true;
+    mGyroBiasAvailable = false;
+    LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro DMP Calibrated Bias Applied");
+
+    return;
+}
+
+void MPLSensor::getFactoryAccelBias()
+{
+    VFUNC_LOG;
+
+    long temp;
+
+    /* Get Values from MPL */
+    inv_get_accel_bias(mFactoryAccelBias);
+    LOGV_IF(ENG_VERBOSE, "Factory Accel Bias (mg) %ld %ld %ld", mFactoryAccelBias[0], mFactoryAccelBias[1], mFactoryAccelBias[2]);
+    mFactoryAccelBiasAvailable = true;
+
+    return;
+}
+
+void MPLSensor::setFactoryAccelBias()
+{
+    VFUNC_LOG;
+
+    if(mFactoryAccelBiasAvailable == false)
+        return;
+
+    /* add scaling here - depends on self test parameters */
+    int scaleRatio = mAccelScale / mAccelSelfTestScale;
+    int offsetScale = 16;
+    long tempBias;
+
+    LOGV_IF(ENG_VERBOSE, "HAL: scaleRatio used =%d", scaleRatio);
+    LOGV_IF(ENG_VERBOSE, "HAL: offsetScale used =%d", offsetScale);
+
+    /* Write to Driver */
+    tempBias = -mFactoryAccelBias[0] / 65536.f * scaleRatio / offsetScale;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
+            tempBias, mpu.in_accel_x_offset, getTimestamp());
+    if(write_attribute_sensor_continuous(accel_x_offset_fd, tempBias) < 0) {
+        LOGE("HAL:Error writing to accel_x_offset");
+        return;
+    }
+    tempBias = -mFactoryAccelBias[1] / 65536.f * scaleRatio / offsetScale;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
+            tempBias, mpu.in_accel_y_offset, getTimestamp());
+    if(write_attribute_sensor_continuous(accel_y_offset_fd, tempBias) < 0) {
+        LOGE("HAL:Error writing to accel_y_offset");
+        return;
+    }
+    tempBias = -mFactoryAccelBias[2] / 65536.f * scaleRatio / offsetScale;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
+            tempBias, mpu.in_accel_z_offset, getTimestamp());
+    if(write_attribute_sensor_continuous(accel_z_offset_fd, tempBias) < 0) {
+        LOGE("HAL:Error writing to accel_z_offset");
+        return;
+    }
+    mFactoryAccelBiasAvailable = false;
+    LOGV_IF(EXTRA_VERBOSE, "HAL:Factory Accel Calibrated Bias Applied");
+
+    return;
+}
+
+void MPLSensor::getAccelBias()
+{
+    VFUNC_LOG;
+    long temp;
+
+    /* Get Values from MPL */
+    inv_get_mpl_accel_bias(mAccelBias, &temp);
+    LOGV_IF(ENG_VERBOSE, "Accel Bias (mg) %ld %ld %ld", 
+            mAccelBias[0], mAccelBias[1], mAccelBias[2]);
+    mAccelBiasAvailable = true;
+
+    return;
+}
+
+/*    set accel bias obtained from MPL
+      bias is scaled by 65536 from MPL
+      DMP expects: bias * 536870912 / 2^30 = bias / 2 (in body frame)
+*/
+void MPLSensor::setAccelBias()
+{
+    VFUNC_LOG;
+
+    if(mAccelBiasAvailable == false) {
+        LOGV_IF(ENG_VERBOSE, "HAL: setAccelBias - accel bias not available");
+        return;
+    }
+
+    /* write to driver */
+    LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)",
+            (long) (mAccelBias[0] / 65536.f / 2), 
+            mpu.in_accel_x_dmp_bias, getTimestamp());
+    if(write_attribute_sensor_continuous(
+            accel_x_dmp_bias_fd, (long)(mAccelBias[0] / 65536.f / 2)) < 0) {
+        LOGE("HAL:Error writing to accel_x_dmp_bias");
+        return;
+    }
+    LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)",
+            (long)(mAccelBias[1] / 65536.f / 2), 
+            mpu.in_accel_y_dmp_bias, getTimestamp());
+    if(write_attribute_sensor_continuous(
+            accel_y_dmp_bias_fd, (long)(mAccelBias[1] / 65536.f / 2)) < 0) {
+        LOGE("HAL:Error writing to accel_y_dmp_bias");
+        return;
+    }
+    LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)",
+            (long)(mAccelBias[2] / 65536 / 2), 
+            mpu.in_accel_z_dmp_bias, getTimestamp());
+    if(write_attribute_sensor_continuous(
+            accel_z_dmp_bias_fd, (long)(mAccelBias[2] / 65536 / 2)) < 0) {
+        LOGE("HAL:Error writing to accel_z_dmp_bias");
+        return;
+    }
+
+    mAccelBiasAvailable = false;
+    mAccelBiasApplied = true;
+    LOGV_IF(EXTRA_VERBOSE, "HAL:Accel DMP Calibrated Bias Applied");
+
+    return;
+}
+
+int MPLSensor::isCompassDisabled(void)
+{
+    VFUNC_LOG;
+    if(mCompassSensor->getFd() < 0 && !mCompassSensor->isIntegrated()) {
+    LOGI_IF(EXTRA_VERBOSE, "HAL: Compass is disabled, Six-axis Sensor Fusion is used.");
+        return 1;
+    }
+    return 0;
+}
+
+int MPLSensor::checkBatchEnabled(void)
+{
+    VFUNC_LOG;
+    return ((mFeatureActiveMask & INV_DMP_BATCH_MODE)? 1:0);
+}
+
+/* precondition: framework disallows this case, ie enable continuous sensor, */
+/* and enable batch sensor */
+/* if one sensor is in continuous mode, HAL disallows enabling batch for this sensor */
+/* or any other sensors */
+int MPLSensor::batch(int handle, int flags, int64_t period_ns, int64_t timeout)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    if (isMpuNonDmp())
+        return res;
+
+    /* Enables batch mode and sets timeout for the given sensor */
+    /* enum SENSORS_BATCH_DRY_RUN, SENSORS_BATCH_WAKE_UPON_FIFO_FULL */
+    bool dryRun = false;
+    android::String8 sname;
+    int what = -1;
+    int enabled_sensors = mEnabled;
+    int batchMode = timeout > 0 ? 1 : 0;
+
+    LOGI_IF(DEBUG_BATCHING || ENG_VERBOSE,
+            "HAL:batch called - handle=%d, flags=%d, period=%lld, timeout=%lld",
+            handle, flags, period_ns, timeout);
+
+    if(flags & SENSORS_BATCH_DRY_RUN) {
+        dryRun = true;
+        LOGI_IF(PROCESS_VERBOSE,
+                "HAL:batch - dry run mode is set (%d)", SENSORS_BATCH_DRY_RUN);
+    }
+
+    /* check if we can support issuing interrupt before FIFO fills-up */
+    /* in a given timeout.                                          */
+    if (flags & SENSORS_BATCH_WAKE_UPON_FIFO_FULL) {
+            LOGE("HAL: batch SENSORS_BATCH_WAKE_UPON_FIFO_FULL is not supported");
+            return -EINVAL;
+    }
+
+    getHandle(handle, what, sname);
+    if(uint32_t(what) >= NumSensors) {
+        LOGE("HAL:batch sensors %d not found", what);
+        return -EINVAL;
+    }
+
+    LOGV_IF(PROCESS_VERBOSE,
+            "HAL:batch : %llu ns, (%.2f Hz)", period_ns, 1000000000.f / period_ns);
+
+    // limit all rates to reasonable ones */
+    if (period_ns < 5000000LL) {
+        period_ns = 5000000LL;
+    }
+
+    switch (what) {
+    case Gyro:
+    case RawGyro:
+    case Accelerometer:
+    case MagneticField:
+    case RawMagneticField:
+    case Pressure:
+    case GameRotationVector:
+    case StepDetector:
+        LOGV_IF(PROCESS_VERBOSE, "HAL: batch - select sensor (handle %d)", handle);
+        break;
+    default:
+        if (timeout > 0) {
+            LOGE("sensor (handle %d) is not supported in batch mode", handle);
+            return -EINVAL;
+        }
+    }
+
+    if(dryRun == true) {
+        LOGI("HAL: batch Dry Run is complete");
+        return 0;
+    }
+
+    int tempBatch = 0;
+    if (timeout > 0) { 
+        tempBatch = mBatchEnabled | (1 << what);
+    } else {
+        tempBatch = mBatchEnabled & ~(1 << what);
+    }
+
+    if (!computeBatchSensorMask(mEnabled, tempBatch)) {
+        batchMode = 0;
+    } else {
+        batchMode = 1;
+    }
+    
+    /* get maximum possible bytes to batch per sample */
+    /* get minimum delay for each requested sensor    */
+    ssize_t nBytes = 0;
+    int64_t wanted = 1000000000LL, ns = 0;
+    int64_t timeoutInMs = 0;
+    for (int i = 0; i < NumSensors; i++) {
+        if (batchMode == 1) {
+            ns = mBatchDelays[i];
+            LOGV_IF(DEBUG_BATCHING && EXTRA_VERBOSE,
+                    "HAL:batch - requested sensor=0x%01x, batch delay=%lld", mEnabled & (1 << i), ns);
+            // take the min delay ==> max rate
+            wanted = (ns < wanted) ? ns : wanted;
+            if (i <= RawMagneticField) {
+                nBytes += 8;
+            }
+            if (i == Pressure) {
+                nBytes += 6;
+            }
+            if ((i == StepDetector) || (i == GameRotationVector)) {
+                nBytes += 16;
+            }
+        }
+    }
+    
+    /* starting from code below,  we will modify hardware */
+    /* first edit global batch mode mask */
+
+    if (!timeout) {
+        mBatchEnabled &= ~(1 << what);
+        mBatchDelays[what] = 1000000000L;
+        mDelays[what] = period_ns;        
+        mBatchTimeouts[what] = 100000000000LL;
+    } else {
+        mBatchEnabled |= (1 << what);
+        mBatchDelays[what] = period_ns;
+        mDelays[what] = period_ns;    
+        mBatchTimeouts[what] = timeout;
+    }
+
+    // reset master enable
+    res = masterEnable(0);
+    if (res < 0) {
+        return res;
+    }
+
+    if(((int)mOldBatchEnabledMask != batchMode) || batchMode) {
+  
+    /* remember batch mode that is set  */
+    mOldBatchEnabledMask = batchMode;
+
+    /* For these sensors, switch to different data output */
+    int featureMask = computeBatchDataOutput();
+
+    LOGV_IF(ENG_VERBOSE, "batchMode =%d, featureMask=0x%x, mEnabled=%d",
+                batchMode, featureMask, mEnabled);
+    if (DEBUG_BATCHING && EXTRA_VERBOSE) {
+        LOGV("HAL:batch - sensor=0x%01x", mBatchEnabled);
+        for (int d = 0; d < NumSensors; d++) {            
+            LOGV("HAL:batch - sensor status=0x%01x batch status=0x%01x timeout=%lld delay=%lld",
+                            mEnabled & (1 << d), (mBatchEnabled & (1 << d)), mBatchTimeouts[d],
+                            mBatchDelays[d]);
+        }
+    }
+
+    /* take the minimum batchmode timeout */
+    if (batchMode == 1) {
+        int64_t tempTimeout = 100000000000LL;
+        for (int i = 0; i < NumSensors; i++) {
+            if ((mEnabled & (1 << i) && mBatchEnabled & (1 << i)) ||
+                    (((featureMask & INV_DMP_PED_STANDALONE) && (mBatchEnabled & (1 << StepDetector))))) {
+                LOGV_IF(ENG_VERBOSE, "i=%d, timeout=%lld", i, mBatchTimeouts[i]);
+                ns = mBatchTimeouts[i];
+                tempTimeout = (ns < tempTimeout) ? ns : tempTimeout;
+            }
+        }
+        timeout = tempTimeout;
+        /* Convert ns to millisecond */
+        timeoutInMs = timeout / 1000000;
+
+        /* remember last timeout value */
+        mBatchTimeoutInMs = timeoutInMs;
+    } else {
+        timeoutInMs = 0;
+    }
+    
+    LOGV_IF(DEBUG_BATCHING || EXTRA_VERBOSE,
+                "HAL:batch - timeout - timeout=%lld ns, timeoutInMs=%lld, delay=%lld ns",
+                timeout, timeoutInMs, period_ns);
+
+    /* case for Ped standalone */
+    if ((batchMode == 1) && (featureMask & INV_DMP_PED_STANDALONE) &&
+           (mFeatureActiveMask & INV_DMP_PEDOMETER)) {
+        LOGI("ID_P only = 0x%x", mBatchEnabled);
+        enablePedQuaternion(0);
+        enablePedStandalone(1);
+    } else {
+        enablePedStandalone(0);
+        if (featureMask & INV_DMP_PED_QUATERNION) {
+            enableLPQuaternion(0);
+            enablePedQuaternion(1);
+        }
+    }
+
+    /* case for Ped Quaternion */
+    if ((batchMode == 1) && (featureMask & INV_DMP_PED_QUATERNION) &&
+            (mEnabled & (1 << GameRotationVector)) &&
+            (mFeatureActiveMask & INV_DMP_PEDOMETER)) {
+        LOGI("ID_P and GRV or ALL = 0x%x", mBatchEnabled);
+        LOGI("ID_P is enabled for batching, PED quat will be automatically enabled");
+        enableLPQuaternion(0);
+        enablePedQuaternion(1);
+        
+        wanted = mBatchDelays[GameRotationVector];
+        /* set pedq rate */
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                int(1000000000.f / wanted), mpu.ped_q_rate,
+                getTimestamp());
+        write_sysfs_int(mpu.ped_q_rate, 1000000000.f / wanted);
+        LOGV_IF(PROCESS_VERBOSE,
+                "HAL:DMP ped quaternion rate %.2f Hz", 1000000000.f / wanted);
+    } else if (!(featureMask & INV_DMP_PED_STANDALONE)){
+        LOGV_IF(ENG_VERBOSE, "batch - PedQ Toggle back to normal 6 axis");
+        if (mEnabled & (1 << GameRotationVector)) {
+            enableLPQuaternion(checkLPQRateSupported());
+        }
+        enablePedQuaternion(0);
+    } else {
+        enablePedQuaternion(0);
+    }
+
+     /* case for Ped indicator */
+    if ((batchMode == 1) && ((featureMask & INV_DMP_PED_INDICATOR))) {
+        enablePedIndicator(1);
+    } else {
+        enablePedIndicator(0);
+    }    
+
+    /* case for Six Axis Quaternion */
+    if ((batchMode == 1) && (featureMask & INV_DMP_6AXIS_QUATERNION) &&
+            (mEnabled & (1 << GameRotationVector))) {
+        LOGI("GRV = 0x%x", mBatchEnabled);
+        enableLPQuaternion(0);
+        enable6AxisQuaternion(1);
+        if (what == GameRotationVector) {
+            setInitial6QuatValue();
+        }
+
+        wanted = mBatchDelays[GameRotationVector];
+        /* set sixaxis rate */
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                int(1000000000.f / wanted), mpu.six_axis_q_rate,
+                getTimestamp());
+        write_sysfs_int(mpu.six_axis_q_rate, 1000000000.f / wanted);
+        LOGV_IF(PROCESS_VERBOSE,
+                "HAL:DMP six axis rate %.2f Hz", 1000000000.f / wanted);        
+    } else if (!(featureMask & INV_DMP_PED_QUATERNION)){
+        LOGV_IF(ENG_VERBOSE, "batch - 6Axis Toggle back to normal 6 axis");
+        if (mEnabled & (1 << GameRotationVector)) {
+            enableLPQuaternion(checkLPQRateSupported());
+        }
+        enable6AxisQuaternion(0);
+    } else {
+        enable6AxisQuaternion(0);
+    }
+
+    /* TODO: This may make a come back some day */
+    /* Not to overflow hardware FIFO if flag is set */
+    /*if (flags & (1 << SENSORS_BATCH_WAKE_UPON_FIFO_FULL)) {
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                0, mpu.batchmode_wake_fifo_full_on, getTimestamp());
+        if (write_sysfs_int(mpu.batchmode_wake_fifo_full_on, 0) < 0) {
+            LOGE("HAL:ERR can't write batchmode_wake_fifo_full_on");
+        }
+    }*/
+
+    /* write required timeout to sysfs */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %lld > %s (%lld)",
+            timeoutInMs, mpu.batchmode_timeout, getTimestamp());
+    if (write_sysfs_int(mpu.batchmode_timeout, timeoutInMs) < 0) {
+        LOGE("HAL:ERR can't write batchmode_timeout");
+    }
+
+    if (computeAndSetDmpState() < 0) {
+        LOGE("HAL:ERR can't compute dmp state");
+    }
+
+}//end of batch mode modify
+
+    if (batchMode == 1) {
+        /* set batch rates */
+        if (setBatchDataRates() < 0) {
+            LOGE("HAL:ERR can't set batch data rates");
+        }
+    } else {
+        /* reset sensor rate */
+        if (resetDataRates() < 0) {
+            LOGE("HAL:ERR can't reset output rate back to original setting");
+        }
+    }
+
+    // set sensor data interrupt
+    uint32_t dataInterrupt = (mEnabled || (mFeatureActiveMask & INV_DMP_BATCH_MODE));
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                !dataInterrupt, mpu.dmp_event_int_on, getTimestamp());
+    if (write_sysfs_int(mpu.dmp_event_int_on, !dataInterrupt) < 0) {
+        res = -1;
+        LOGE("HAL:ERR can't enable DMP event interrupt");
+    }
+
+    if (enabled_sensors || mFeatureActiveMask) {
+        masterEnable(1);
+    }
+    return res;
+}
+
+/* Send empty event when:        */
+/* 1. batch mode is not enabled  */
+/* 2. no data in HW FIFO         */
+/* return status zero if (2)     */
+int MPLSensor::flush(int handle)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+    int status = 0;
+    android::String8 sname;
+    int what = -1;
+
+    getHandle(handle, what, sname);
+    if (uint32_t(what) >= NumSensors) {
+        LOGE("HAL:flush - what=%d is invalid", what);
+        return -EINVAL;
+    }
+
+    switch (what) {
+    case Gyro:
+    case RawGyro:
+    case Accelerometer:
+    case MagneticField:
+    case RawMagneticField:
+    case Pressure:
+    case GameRotationVector:
+    case StepDetector:
+        LOGV_IF(PROCESS_VERBOSE, "HAL: flush - select sensor %s (handle %d)", sname.string(), handle);
+        break;
+    default:
+        LOGE("sensor (handle %d) is not supported in batch or flush mode", handle);
+        return -EINVAL;
+    }
+
+    if (((what != StepDetector) && (!(mEnabled & (1 << what)))) ||
+        ((what == StepDetector) && !(mFeatureActiveMask & INV_DMP_PEDOMETER))) {
+        LOGE("HAL: flush - sensor %s not enabled", sname.string());
+         return -EINVAL;
+    }
+
+    if(!(mBatchEnabled & (1 << what))) {
+         LOGV_IF(PROCESS_VERBOSE, "HAL:flush - batch mode not enabled for sensor %s (handle %d)", sname.string(), handle);
+    }
+
+    /*write sysfs */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
+            mpu.flush_batch, getTimestamp());
+
+    status = read_sysfs_int(mpu.flush_batch, &res);
+
+    if (status < 0)
+        return status;
+    
+    /* driver returns 0 if FIFO is empty */
+    /* ensure we return status zero      */
+    if (res == 0) {
+        LOGI("HAL: flush - no data in FIFO");
+        status = 0;
+    }
+
+    mFlushSensorEnabled = handle;
+    LOGV_IF(ENG_VERBOSE, "HAl:flush - mFlushSensorEnabled=%d", mFlushSensorEnabled);
+
+    mFlushBatchSet = 0;
+    return status;
+}
+
+int MPLSensor::selectAndSetQuaternion(int batchMode, int mEnabled, long long featureMask)
+{
+    VFUNC_LOG;
+    int res = 0;
+
+    int64_t wanted;
+
+    /* case for Ped Quaternion */
+    if (batchMode == 1) {
+        if ((featureMask & INV_DMP_PED_QUATERNION) &&
+            (mEnabled & (1 << GameRotationVector)) &&
+            (mFeatureActiveMask & INV_DMP_PEDOMETER)) {
+                enableLPQuaternion(0);
+                enable6AxisQuaternion(0);
+                setInitial6QuatValue();
+                enablePedQuaternion(1);
+        
+                wanted = mBatchDelays[GameRotationVector];
+                /* set pedq rate */
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        int(1000000000.f / wanted), mpu.ped_q_rate,
+                        getTimestamp());
+                write_sysfs_int(mpu.ped_q_rate, 1000000000.f / wanted);
+                LOGV_IF(PROCESS_VERBOSE,
+                        "HAL:DMP ped quaternion rate %.2f Hz", 1000000000.f / wanted);
+        } else if ((featureMask & INV_DMP_6AXIS_QUATERNION) &&
+                   (mEnabled & (1 << GameRotationVector))) {
+                enableLPQuaternion(0);
+                enablePedQuaternion(0);
+                setInitial6QuatValue();
+                enable6AxisQuaternion(1);
+
+                wanted = mBatchDelays[GameRotationVector];
+                /* set sixaxis rate */
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        int(1000000000.f / wanted), mpu.six_axis_q_rate,
+                        getTimestamp());
+                write_sysfs_int(mpu.six_axis_q_rate, 1000000000.f / wanted);
+                LOGV_IF(PROCESS_VERBOSE,
+                        "HAL:DMP six axis rate %.2f Hz", 1000000000.f / wanted);                
+        } else {
+            enablePedQuaternion(0);
+            enable6AxisQuaternion(0);
+        }           
+    } else {
+        if(mEnabled & (1 << GameRotationVector)) {
+            enablePedQuaternion(0);
+            enable6AxisQuaternion(0);
+            enableLPQuaternion(checkLPQRateSupported());
+        }
+        else {
+            enablePedQuaternion(0);
+            enable6AxisQuaternion(0);
+        }
+    }    
+
+    return res;
+}
+
+/*
+Select Quaternion and Options for Batching
+
+    ID_P    ID_GRV     HW Batch     Type
+a   1       1          1            PedQ, Ped Indicator, HW
+b   1       1          0            PedQ
+c   1       0          1            Ped Indicator, HW
+d   1       0          0            Ped Standalone, Ped Indicator
+e   0       1          1            6Q, HW
+f   0       1          0            6Q
+g   0       0          1            HW
+h   0       0          0            LPQ <defualt>
+*/
+int MPLSensor::computeBatchDataOutput()
+{
+    VFUNC_LOG;
+
+    int featureMask = 0;
+    if (mBatchEnabled == 0)
+        return 0;//h
+    
+    uint32_t hardwareSensorMask = (1 << Gyro)
+                                | (1 << RawGyro)
+                                | (1 << Accelerometer)
+                                | (1 << MagneticField)
+                                | (1 << RawMagneticField)
+                                | (1 << Pressure);
+    LOGV_IF(ENG_VERBOSE, "hardwareSensorMask = 0x%0x, mBatchEnabled = 0x%0x", 
+            hardwareSensorMask, mBatchEnabled);
+   
+    if (mBatchEnabled & (1 << StepDetector)) {         
+        if (mBatchEnabled & (1 << GameRotationVector)) {
+            if ((mBatchEnabled & hardwareSensorMask)) {
+                featureMask |= INV_DMP_6AXIS_QUATERNION;//a
+                featureMask |= INV_DMP_PED_INDICATOR;
+//LOGE("batch output: a");
+            } else {
+                featureMask |= INV_DMP_PED_QUATERNION;  //b
+                featureMask |= INV_DMP_PED_INDICATOR;   //always piggy back a bit
+//LOGE("batch output: b");
+            }
+        } else {
+            if (mBatchEnabled & hardwareSensorMask) {
+                featureMask |= INV_DMP_PED_INDICATOR;   //c
+//LOGE("batch output: c");
+            } else {
+                featureMask |= INV_DMP_PED_STANDALONE;  //d
+                featureMask |= INV_DMP_PED_INDICATOR;   //required for standalone
+//LOGE("batch output: d");
+            }
+        }
+    } else if (mBatchEnabled & (1 << GameRotationVector)) {
+        featureMask |= INV_DMP_6AXIS_QUATERNION;        //e,f
+//LOGE("batch output: e,f");
+    } else {
+        LOGV_IF(ENG_VERBOSE, 
+                "HAL:computeBatchDataOutput: featuerMask=0x%x", featureMask);
+//LOGE("batch output: g");
+        return 0; //g
+    }
+
+    LOGV_IF(ENG_VERBOSE, 
+            "HAL:computeBatchDataOutput: featuerMask=0x%x", featureMask);
+    return featureMask;
+}
+
+int MPLSensor::getDmpPedometerFd()
+{
+    VFUNC_LOG;
+    LOGV_IF(EXTRA_VERBOSE, "getDmpPedometerFd returning %d", dmp_pedometer_fd);
+    return dmp_pedometer_fd;
+}
+
+/* @param [in] : outputType = 1 --event is from PED_Q        */
+/*               outputType = 0 --event is from ID_SC, ID_P  */
+int MPLSensor::readDmpPedometerEvents(sensors_event_t* data, int count,
+                                      int32_t id, int outputType)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+    char dummy[4];   
+   
+    int numEventReceived = 0;
+    int update = 0;
+
+    LOGI_IF(0, "HAL: Read Pedometer Event ID=%d", id);    
+    switch (id) {
+    case ID_P:
+        if (mDmpPedometerEnabled && count > 0) {
+            /* Handles return event */
+            LOGI("HAL: Step detected");
+            update = sdHandler(&mSdEvents);
+        }
+
+        if (update && count > 0) {
+            *data++ = mSdEvents;
+            count--;
+            numEventReceived++;
+        } 
+        break;
+    case ID_SC:
+        FILE *fp;
+        uint64_t stepCount;
+        uint64_t stepCountTs;
+        
+        if (mDmpStepCountEnabled && count > 0) {
+            fp = fopen(mpu.pedometer_steps, "r");
+            if (fp == NULL) {
+                LOGE("HAL:cannot open pedometer_steps");
+            } else {
+                if (fscanf(fp, "%lld\n", &stepCount) < 0 || fclose(fp) < 0) {
+                    LOGE("HAL:cannot read pedometer_steps");
+                    return 0;
+                }
+            }
+
+            /* return event onChange only */
+            if (stepCount == mLastStepCount) {
+                return 0;
+            }
+
+            mLastStepCount = stepCount;
+
+            /* Read step count timestamp */
+            fp = fopen(mpu.pedometer_counter, "r");
+            if (fp == NULL) {
+                LOGE("HAL:cannot open pedometer_counter");
+            } else{
+                if (fscanf(fp, "%lld\n", &stepCountTs) < 0 || fclose(fp) < 0) {
+                    LOGE("HAL:cannot read pedometer_counter");
+                    return 0;
+                }
+            }
+            mScEvents.timestamp = stepCountTs;
+            
+            /* Handles return event */  
+            update = scHandler(&mScEvents);
+        }
+
+        if (update && count > 0) {
+            *data++ = mScEvents;
+            count--;
+            numEventReceived++;
+        }
+        break;
+    }
+
+    if (!outputType) {
+        // read dummy data per driver's request
+        // only required if actual irq is issued
+        read(dmp_pedometer_fd, dummy, 4);
+    } else {
+        return 1;
+    } 
+
+    return numEventReceived;
+}
+
+int MPLSensor::getDmpSignificantMotionFd()
+{
+    VFUNC_LOG;
+
+    LOGV_IF(EXTRA_VERBOSE, "getDmpSignificantMotionFd returning %d",
+            dmp_sign_motion_fd);
+    return dmp_sign_motion_fd;
+}
+
+int MPLSensor::readDmpSignificantMotionEvents(sensors_event_t* data, int count)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+    char dummy[4];
+    int significantMotion;
+    FILE *fp;
+    int sensors = mEnabled;
+    int numEventReceived = 0;
+    int update = 0;
+
+    /* Technically this step is not necessary for now  */
+    /* In the future, we may have meaningful values */
+    fp = fopen(mpu.event_smd, "r");
+    if (fp == NULL) {
+        LOGE("HAL:cannot open event_smd");
+        return 0;
+    } else {
+        if (fscanf(fp, "%d\n", &significantMotion) < 0 || fclose(fp) < 0) {
+            LOGE("HAL:cannot read event_smd");
+        }
+    }
+
+    if(mDmpSignificantMotionEnabled && count > 0) {
+       /* By implementation, smd is disabled once an event is triggered */
+        sensors_event_t temp;
+
+        /* Handles return event */
+        LOGI("HAL: SMD detected");
+        int update = smHandler(&mSmEvents);
+        if (update && count > 0) {
+            *data++ = mSmEvents;
+            count--;
+            numEventReceived++;
+            
+            /* reset smd state */
+            mDmpSignificantMotionEnabled = 0;
+            mFeatureActiveMask &= ~INV_DMP_SIGNIFICANT_MOTION;
+        }
+    }
+
+    // read dummy data per driver's request
+    read(dmp_sign_motion_fd, dummy, 4);
+
+    return numEventReceived;
+}
+
+int MPLSensor::enableDmpSignificantMotion(int en)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+    int enabled_sensors = mEnabled;
+
+    if (isMpuNonDmp())
+        return res;
+
+    // reset master enable
+    res = masterEnable(0);
+    if (res < 0)
+        return res;
+
+    //Toggle significant montion detection
+    if(en) {
+        LOGV_IF(ENG_VERBOSE, "HAL:Enabling Significant Motion");
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                1, mpu.smd_enable, getTimestamp());
+        if (write_sysfs_int(mpu.smd_enable, 1) < 0) {
+            LOGE("HAL:ERR can't write DMP smd_enable");
+            res = -1;   //Indicate an err
+        }    
+        mFeatureActiveMask |= INV_DMP_SIGNIFICANT_MOTION;
+    }
+    else {
+        LOGV_IF(ENG_VERBOSE, "HAL:Disabling Significant Motion");
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                0, mpu.smd_enable, getTimestamp());
+        if (write_sysfs_int(mpu.smd_enable, 0) < 0) {
+            LOGE("HAL:ERR write DMP smd_enable");
+        }
+        mFeatureActiveMask &= ~INV_DMP_SIGNIFICANT_MOTION;     
+    }
+    
+    if ((res = setDmpFeature(en)) < 0)
+        return res;
+
+    if ((res = computeAndSetDmpState()) < 0)
+        return res;
+    
+    if(en || enabled_sensors || mFeatureActiveMask) { 
+        res = masterEnable(1);
+    }
+    return res;
+}
+
+void MPLSensor::setInitial6QuatValue()
+{
+    VFUNC_LOG;
+
+    if (!mInitial6QuatValueAvailable)
+        return;
+
+    /* convert to unsigned char array */
+    size_t length = 16;
+    unsigned char quat[16];
+    convert_long_to_hex_char(mInitial6QuatValue, quat, 4);
+
+    /* write to sysfs */
+    LOGV_IF(EXTRA_VERBOSE, "HAL:six axis q value: %s", mpu.six_axis_q_value);
+    FILE* fptr = fopen(mpu.six_axis_q_value, "w");
+    if(fptr == NULL) {
+        LOGE("HAL:could not open six_axis_q_value");
+    } else {
+        if (fwrite(quat, 1, length, fptr) != length || fclose(fptr) < 0) {
+           LOGE("HAL:write six axis q value failed");
+        } else {
+            mInitial6QuatValueAvailable = 0;
+        }
+    }
+
+    return;
+}
+int MPLSensor::writeSignificantMotionParams(bool toggleEnable,
+                                            uint32_t delayThreshold1,
+                                            uint32_t delayThreshold2,
+                                            uint32_t motionThreshold)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    // Turn off enable
+    if (toggleEnable) {
+        masterEnable(0);
+    }
+
+    // Write supplied values
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            delayThreshold1, mpu.smd_delay_threshold, getTimestamp());
+    res = write_sysfs_int(mpu.smd_delay_threshold, delayThreshold1);
+    if (res == 0) {
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                delayThreshold2, mpu.smd_delay_threshold2, getTimestamp());
+        res = write_sysfs_int(mpu.smd_delay_threshold2, delayThreshold2);
+    }
+    if (res == 0) {
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                motionThreshold, mpu.smd_threshold, getTimestamp());
+        res = write_sysfs_int(mpu.smd_threshold, motionThreshold);
+    }
+
+    // Turn on enable
+    if (toggleEnable) {
+        masterEnable(1);
+    }
+    return res;
+}
+
+/* set batch data rate */
+/* this function should be optimized */
+int MPLSensor::setBatchDataRates()
+{
+    VFUNC_LOG;
+
+    int res = 0;
+    int tempFd = -1;
+
+    int64_t gyroRate;
+    int64_t accelRate;
+    int64_t compassRate;
+    int64_t pressureRate;
+    int64_t quatRate;
+    
+    int mplGyroRate;
+    int mplAccelRate;
+    int mplCompassRate;
+    int mplQuatRate;
+            
+#ifdef ENABLE_MULTI_RATE
+    gyroRate = mBatchDelays[Gyro];    
+    /* take care of case where only one type of gyro sensors or
+       compass sensors is turned on */
+    if (mBatchEnabled & (1 << Gyro) || mBatchEnabled & (1 << RawGyro)) {
+        gyroRate = (mBatchDelays[Gyro] <= mBatchDelays[RawGyro]) ?
+            (mBatchEnabled & (1 << Gyro) ? mBatchDelays[Gyro] : mBatchDelays[RawGyro]):
+            (mBatchEnabled & (1 << RawGyro) ? mBatchDelays[RawGyro] : mBatchDelays[Gyro]); 
+    }
+    compassRate = mBatchDelays[MagneticField];
+    if (mBatchEnabled & (1 << MagneticField) || mBatchEnabled & (1 << RawMagneticField)) {
+        compassRate = (mBatchDelays[MagneticField] <= mBatchDelays[RawMagneticField]) ?
+                (mBatchEnabled & (1 << MagneticField) ? mBatchDelays[MagneticField] :
+                    mBatchDelays[RawMagneticField]) :
+                (mBatchEnabled & (1 << RawMagneticField) ? mBatchDelays[RawMagneticField] :
+                    mBatchDelays[MagneticField]); 
+    }
+    accelRate = mBatchDelays[Accelerometer];
+    pressureRate = mBatchDelays[Pressure];
+
+    if ((mFeatureActiveMask & INV_DMP_PED_QUATERNION) || 
+            (mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION)) {
+        quatRate = mBatchDelays[GameRotationVector];
+        mplQuatRate = (int) quatRate / 1000LL;
+        inv_set_quat_sample_rate(mplQuatRate);
+        inv_set_rotation_vector_6_axis_sample_rate(mplQuatRate);
+        LOGV_IF(PROCESS_VERBOSE,
+            "HAL:MPL rv 6 axis sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplQuatRate,
+                1000000000.f / quatRate );
+        LOGV_IF(PROCESS_VERBOSE,
+            "HAL:MPL quat sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplQuatRate,
+                1000000000.f / quatRate );
+        getDmpRate(&quatRate);
+    }
+
+    mplGyroRate = (int) gyroRate / 1000LL;
+    mplAccelRate = (int) accelRate / 1000LL;
+    mplCompassRate = (int) compassRate / 1000LL;
+    
+     /* set rate in MPL */
+     /* compass can only do 100Hz max */
+    inv_set_gyro_sample_rate(mplGyroRate);
+    inv_set_accel_sample_rate(mplAccelRate);
+    inv_set_compass_sample_rate(mplCompassRate);
+           
+    LOGV_IF(PROCESS_VERBOSE,
+            "HAL:MPL gyro sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplGyroRate, 1000000000.f / gyroRate);
+    LOGV_IF(PROCESS_VERBOSE,
+            "HAL:MPL accel sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplAccelRate, 1000000000.f / accelRate);
+    LOGV_IF(PROCESS_VERBOSE,
+            "HAL:MPL compass sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplCompassRate, 1000000000.f / compassRate);
+
+#else
+    /* search the minimum delay requested across all enabled sensors */
+    int64_t wanted = 1000000000LL;
+    for (int i = 0; i < NumSensors; i++) {
+        if (mBatchEnabled & (1 << i)) {
+            int64_t ns = mBatchDelays[i];
+            wanted = wanted < ns ? wanted : ns;
+        }
+    }
+    gyroRate = wanted;
+    accelRate = wanted;
+    compassRate = wanted;
+    pressureRate = wanted;
+#endif
+
+    /* takes care of gyro rate */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+            1000000000.f / gyroRate, mpu.gyro_rate,
+            getTimestamp());
+    tempFd = open(mpu.gyro_rate, O_RDWR);
+    res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate);
+    if(res < 0) {
+        LOGE("HAL:GYRO update delay error");
+    }
+    
+    /* takes care of accel rate */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+            1000000000.f / accelRate, mpu.accel_rate,
+            getTimestamp());
+    tempFd = open(mpu.accel_rate, O_RDWR);
+    res = write_attribute_sensor(tempFd, 1000000000.f / accelRate);
+    LOGE_IF(res < 0, "HAL:ACCEL update delay error");
+   
+    /* takes care of compass rate */
+    if (compassRate < mCompassSensor->getMinDelay() * 1000LL) {
+        compassRate = mCompassSensor->getMinDelay() * 1000LL;
+    }
+    mCompassSensor->setDelay(ID_M, compassRate);
+
+    /* takes care of pressure rate */
+    mPressureSensor->setDelay(ID_PS, pressureRate);
+   
+    return res;
+}
+
+/* Set sensor rate */
+/* this function should be optimized */
+int MPLSensor::resetDataRates()
+{
+    VFUNC_LOG;
+    
+    int res = 0;
+    int tempFd = -1;
+    int64_t wanted = 1000000000LL;
+
+    int64_t resetRate;
+    int64_t gyroRate;
+    int64_t accelRate;
+    int64_t compassRate;
+    int64_t pressureRate;
+
+    if (!mEnabled) {
+        LOGV_IF(ENG_VERBOSE, "skip resetDataRates");
+        return 0;
+    }
+    LOGI("HAL:resetDataRates mEnabled=%d", mEnabled);
+    /* search the minimum delay requested across all enabled sensors */
+    /* skip setting rates if it is not changed */
+    for (int i = 0; i < NumSensors; i++) {
+        if (mEnabled & (1 << i)) {
+            int64_t ns = mDelays[i];
+            if ((wanted == ns) && (i != Pressure)) {
+                LOGV_IF(ENG_VERBOSE, "skip resetDataRates : same delay mDelays[%d]=%lld", i,mDelays[i]);
+                //return 0;
+            }
+            LOGV_IF(ENG_VERBOSE, "resetDataRates - mDelays[%d]=%lld", i, mDelays[i]);
+            wanted = wanted < ns ? wanted : ns;
+        }
+    }
+
+    resetRate = wanted;
+    gyroRate = wanted;
+    accelRate = wanted;
+    compassRate = wanted;
+    pressureRate = wanted;
+
+    /* set mpl data rate */
+   inv_set_gyro_sample_rate((int)gyroRate/1000LL);
+   inv_set_accel_sample_rate((int)accelRate/1000LL);
+   inv_set_compass_sample_rate((int)compassRate/1000LL);
+   inv_set_linear_acceleration_sample_rate((int)resetRate/1000LL);
+   inv_set_orientation_sample_rate((int)resetRate/1000LL);
+   inv_set_rotation_vector_sample_rate((int)resetRate/1000LL);
+   inv_set_gravity_sample_rate((int)resetRate/1000LL);
+   inv_set_orientation_geomagnetic_sample_rate((int)resetRate/1000LL);
+   inv_set_rotation_vector_6_axis_sample_rate((int)resetRate/1000LL);
+   inv_set_geomagnetic_rotation_vector_sample_rate((int)resetRate/1000LL);
+
+    LOGV_IF(PROCESS_VERBOSE,
+            "HAL:MPL gyro sample rate: (mpl)=%lld us (mpu)=%.2f Hz", 
+            gyroRate/1000LL, 1000000000.f / gyroRate);
+    LOGV_IF(PROCESS_VERBOSE,
+            "HAL:MPL accel sample rate: (mpl)=%lld us (mpu)=%.2f Hz", 
+            accelRate/1000LL, 1000000000.f / accelRate);
+    LOGV_IF(PROCESS_VERBOSE,
+            "HAL:MPL compass sample rate: (mpl)=%lld us (mpu)=%.2f Hz", 
+            compassRate/1000LL, 1000000000.f / compassRate);
+
+    /* reset dmp rate */
+    getDmpRate (&wanted);
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+            1000000000.f / wanted, mpu.gyro_fifo_rate,
+            getTimestamp());
+    tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
+    res = write_attribute_sensor(tempFd, 1000000000.f / wanted);
+    LOGE_IF(res < 0, "HAL:sampling frequency update delay error");
+            
+    /* takes care of gyro rate */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+            1000000000.f / gyroRate, mpu.gyro_rate,
+            getTimestamp());
+    tempFd = open(mpu.gyro_rate, O_RDWR);
+    res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate);
+    if(res < 0) {
+        LOGE("HAL:GYRO update delay error");
+    }
+
+    /* takes care of accel rate */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+            1000000000.f / accelRate, mpu.accel_rate,
+            getTimestamp());
+    tempFd = open(mpu.accel_rate, O_RDWR);
+    res = write_attribute_sensor(tempFd, 1000000000.f / accelRate);
+    LOGE_IF(res < 0, "HAL:ACCEL update delay error");
+
+    /* takes care of compass rate */
+    if (compassRate < mCompassSensor->getMinDelay() * 1000LL) {
+        compassRate = mCompassSensor->getMinDelay() * 1000LL;
+    }
+    mCompassSensor->setDelay(ID_M, compassRate);
+
+    /* takes care of pressure rate */
+    mPressureSensor->setDelay(ID_PS, pressureRate);
+  
+    /* takes care of lpq case for data rate at 200Hz */
+    if (checkLPQuaternion()) {
+        if (resetRate <= RATE_200HZ) {
+#ifndef USE_LPQ_AT_FASTEST
+            enableLPQuaternion(0); 
+#endif
+        }
+    }
+ 
+    return res;
+}
+
+void MPLSensor::resetMplStates()
+{
+    VFUNC_LOG;
+    LOGV_IF(ENG_VERBOSE, "HAL:resetMplStates()");
+
+    inv_gyro_was_turned_off();
+    inv_accel_was_turned_off();
+    inv_compass_was_turned_off();
+    inv_quaternion_sensor_was_turned_off();
+
+    return;
+}
+
+void MPLSensor::initBias()
+{
+    VFUNC_LOG;
+
+    LOGV_IF(ENG_VERBOSE, "HAL:inititalize dmp and device offsets to 0");
+    if(write_attribute_sensor_continuous(accel_x_dmp_bias_fd, 0) < 0) {
+        LOGE("HAL:Error writing to accel_x_dmp_bias");
+    }
+    if(write_attribute_sensor_continuous(accel_y_dmp_bias_fd, 0) < 0) {
+        LOGE("HAL:Error writing to accel_y_dmp_bias");
+    }
+    if(write_attribute_sensor_continuous(accel_z_dmp_bias_fd, 0) < 0) {
+        LOGE("HAL:Error writing to accel_z_dmp_bias");
+    }
+    
+    if(write_attribute_sensor_continuous(accel_x_offset_fd, 0) < 0) {
+        LOGE("HAL:Error writing to accel_x_offset");
+    }
+    if(write_attribute_sensor_continuous(accel_y_offset_fd, 0) < 0) {
+        LOGE("HAL:Error writing to accel_y_offset");
+    }
+    if(write_attribute_sensor_continuous(accel_z_offset_fd, 0) < 0) {
+        LOGE("HAL:Error writing to accel_z_offset");
+    }
+    
+    if(write_attribute_sensor_continuous(gyro_x_dmp_bias_fd, 0) < 0) {
+        LOGE("HAL:Error writing to gyro_x_dmp_bias");
+    }
+    if(write_attribute_sensor_continuous(gyro_y_dmp_bias_fd, 0) < 0) {
+        LOGE("HAL:Error writing to gyro_y_dmp_bias");
+    }
+    if(write_attribute_sensor_continuous(gyro_z_dmp_bias_fd, 0) < 0) {
+        LOGE("HAL:Error writing to gyro_z_dmp_bias");
+    }
+    
+    if(write_attribute_sensor_continuous(gyro_x_offset_fd, 0) < 0) {
+        LOGE("HAL:Error writing to gyro_x_offset");
+    }
+    if(write_attribute_sensor_continuous(gyro_y_offset_fd, 0) < 0) {
+        LOGE("HAL:Error writing to gyro_y_offset");
+    }
+    if(write_attribute_sensor_continuous(gyro_z_offset_fd, 0) < 0) {
+        LOGE("HAL:Error writing to gyro_z_offset");
+    }
+    return;
+}
+ 
+/*TODO: reg_dump in a separate file*/
+void MPLSensor::sys_dump(bool fileMode)
+{
+    VFUNC_LOG;
+
+    char sysfs_path[MAX_SYSFS_NAME_LEN];
+    char scan_element_path[MAX_SYSFS_NAME_LEN];
+
+    memset(sysfs_path, 0, sizeof(sysfs_path));
+    memset(scan_element_path, 0, sizeof(scan_element_path));
+    inv_get_sysfs_path(sysfs_path);
+    sprintf(scan_element_path, "%s%s", sysfs_path, "/scan_elements");
+
+    read_sysfs_dir(fileMode, sysfs_path);
+    read_sysfs_dir(fileMode, scan_element_path);
+
+    dump_dmp_img("/data/local/read_img.h");
+    return;
+}
diff --git a/6515/libsensors_iio/MPLSensor.h b/6515/libsensors_iio/MPLSensor.h
new file mode 100644
index 0000000..65e3b23
--- /dev/null
+++ b/6515/libsensors_iio/MPLSensor.h
@@ -0,0 +1,557 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef ANDROID_MPL_SENSOR_H
+#define ANDROID_MPL_SENSOR_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <poll.h>
+#include <time.h>
+#include <utils/Vector.h>
+#include <utils/KeyedVector.h>
+#include <utils/String8.h>
+#include "sensors.h"
+#include "SensorBase.h"
+#include "InputEventReader.h"
+
+#ifndef INVENSENSE_COMPASS_CAL
+#pragma message("unified HAL for AKM")
+#include "CompassSensor.AKM.h"
+#endif
+
+#ifdef SENSOR_ON_PRIMARY_BUS
+#pragma message("Sensor on Primary Bus")
+#include "CompassSensor.IIO.primary.h"
+#else
+#pragma message("Sensor on Secondary Bus")
+#include "CompassSensor.IIO.9150.h"
+#endif
+
+class PressureSensor;
+
+/*****************************************************************************/
+/* Sensors Enable/Disable Mask
+ *****************************************************************************/
+#define MAX_CHIP_ID_LEN             (20)
+
+#define INV_THREE_AXIS_GYRO         (0x000F)
+#define INV_THREE_AXIS_ACCEL        (0x0070)
+#define INV_THREE_AXIS_COMPASS      (0x0380)
+#define INV_ONE_AXIS_PRESSURE       (0x0400)
+#define INV_ALL_SENSORS             (0x7FFF)
+
+#ifdef INVENSENSE_COMPASS_CAL
+#define ALL_MPL_SENSORS_NP          (INV_THREE_AXIS_ACCEL \
+                                      | INV_THREE_AXIS_COMPASS \
+                                      | INV_THREE_AXIS_GYRO)
+#else
+#define ALL_MPL_SENSORS_NP          (INV_THREE_AXIS_ACCEL \
+                                      | INV_THREE_AXIS_COMPASS \
+                                      | INV_THREE_AXIS_GYRO)
+#endif
+
+// mask of virtual sensors that require gyro + accel + compass data
+#define VIRTUAL_SENSOR_9AXES_MASK (         \
+        (1 << Orientation)                  \
+        | (1 << RotationVector)             \
+        | (1 << LinearAccel)                \
+        | (1 << Gravity)                    \
+)
+// mask of virtual sensors that require gyro + accel data (but no compass data)
+#define VIRTUAL_SENSOR_GYRO_6AXES_MASK (    \
+        (1 << GameRotationVector)           \
+)
+// mask of virtual sensors that require mag + accel data (but no gyro data)
+#define VIRTUAL_SENSOR_MAG_6AXES_MASK (     \
+        (1 << GeomagneticRotationVector)    \
+)
+// mask of all virtual sensors
+#define VIRTUAL_SENSOR_ALL_MASK (           \
+        VIRTUAL_SENSOR_9AXES_MASK           \
+        | VIRTUAL_SENSOR_GYRO_6AXES_MASK    \
+        | VIRTUAL_SENSOR_MAG_6AXES_MASK     \
+)
+
+// bit mask of current MPL active features (mMplFeatureActiveMask)
+#define INV_COMPASS_CAL              0x01
+#define INV_COMPASS_FIT              0x02
+
+// bit mask of current DMP active features (mFeatureActiveMask)
+#define INV_DMP_QUATERNION           0x001 //3 elements without real part, 32 bit each
+#define INV_DMP_DISPL_ORIENTATION    0x002 //screen orientation
+#define INV_DMP_SIGNIFICANT_MOTION   0x004 //significant motion
+#define INV_DMP_PEDOMETER            0x008 //interrupt-based pedometer
+#define INV_DMP_PEDOMETER_STEP       0x010 //timer-based pedometer
+#define INV_DMP_PED_STANDALONE       0x020 //timestamps only
+#define INV_DMP_6AXIS_QUATERNION     0x040 //3 elements without real part, 32 bit each
+#define INV_DMP_PED_QUATERNION       0x080 //3 elements without real part, 16 bit each
+#define INV_DMP_PED_INDICATOR        0x100 //tag along header with step indciator
+#define INV_DMP_BATCH_MODE           0x200 //batch mode
+
+// bit mask of whether DMP should be turned on
+#define DMP_FEATURE_MASK (                           \
+        (INV_DMP_QUATERNION)                         \
+        | (INV_DMP_DISPL_ORIENTATION)                \
+        | (INV_DMP_SIGNIFICANT_MOTION)               \
+        | (INV_DMP_PEDOMETER)                        \
+        | (INV_DMP_PEDOMETER_STEP)                   \
+        | (INV_DMP_6AXIS_QUATERNION)                 \
+        | (INV_DMP_PED_QUATERNION)                   \
+        | (INV_DMP_BATCH_MODE)                       \
+)
+
+// bit mask of DMP features as sensors
+#define DMP_SENSOR_MASK (                            \
+        (INV_DMP_DISPL_ORIENTATION)                  \
+        | (INV_DMP_SIGNIFICANT_MOTION)               \
+        | (INV_DMP_PEDOMETER)                        \
+        | (INV_DMP_PEDOMETER_STEP)                   \
+        | (INV_DMP_6AXIS_QUATERNION)                 \
+)
+
+// data header format used by kernel driver.
+#define DATA_FORMAT_STEP           0x0001
+#define DATA_FORMAT_MARKER         0x0010
+#define DATA_FORMAT_EMPTY_MARKER   0x0020
+#define DATA_FORMAT_PED_STANDALONE 0x0100
+#define DATA_FORMAT_PED_QUAT       0x0200
+#define DATA_FORMAT_6_AXIS         0x0400
+#define DATA_FORMAT_QUAT           0x0800
+#define DATA_FORMAT_COMPASS        0x1000
+#define DATA_FORMAT_GYRO           0x2000
+#define DATA_FORMAT_ACCEL          0x4000
+#define DATA_FORMAT_PRESSURE       0x8000
+#define DATA_FORMAT_MASK           0xffff
+
+#define BYTES_PER_SENSOR                8
+#define BYTES_PER_SENSOR_PACKET         16
+#define QUAT_ONLY_LAST_PACKET_OFFSET    16
+#define BYTES_QUAT_DATA                 24
+#define MAX_READ_SIZE                   BYTES_QUAT_DATA
+#define MAX_SUSPEND_BATCH_PACKET_SIZE   1024
+#define MAX_PACKET_SIZE                 80 //8 * 4 + (2 * 24)
+
+/* Uncomment to enable Low Power Quaternion */
+#define ENABLE_LP_QUAT_FEAT
+
+/* Enable Pressure sensor support */
+#define ENABLE_PRESSURE
+
+/* Screen Orientation is not currently supported */
+int isDmpScreenAutoRotationEnabled()
+{
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+    return 1;
+#else
+    return 0;
+#endif
+}
+
+int (*m_pt2AccelCalLoadFunc)(long *bias) = NULL;
+/*****************************************************************************/
+/** MPLSensor implementation which fits into the HAL example for crespo provided
+ *  by Google.
+ *  WARNING: there may only be one instance of MPLSensor, ever.
+ */
+
+class MPLSensor: public SensorBase
+{
+    typedef int (MPLSensor::*hfunc_t)(sensors_event_t*);
+
+public:
+
+    MPLSensor(CompassSensor *, int (*m_pt2AccelCalLoadFunc)(long*) = 0);
+    virtual ~MPLSensor();
+
+    virtual int setDelay(int32_t handle, int64_t ns);
+    virtual int enable(int32_t handle, int enabled);
+    virtual int batch(int handle, int flags, int64_t period_ns, int64_t timeout);
+    virtual int flush(int handle);
+    int selectAndSetQuaternion(int batchMode, int mEnabled, long long featureMask);
+    int checkBatchEnabled();
+    int setBatch(int en, int toggleEnable);
+    int32_t getEnableMask() { return mEnabled; }
+    void getHandle(int32_t handle, int &what, android::String8 &sname);
+
+    virtual int readEvents(sensors_event_t *data, int count);
+    virtual int getFd() const;
+    virtual int getAccelFd() const;
+    virtual int getCompassFd() const;
+    virtual int getPollTime();
+    virtual int getStepCountPollTime();
+    virtual bool hasPendingEvents() const;
+    virtual bool hasStepCountPendingEvents();
+    int populateSensorList(struct sensor_t *list, int len);
+
+    int readAccelEvents(sensors_event_t* data, int count);
+    void buildCompassEvent();
+    void buildMpuEvent();
+    int checkValidHeader(unsigned short data_format);
+
+    int turnOffAccelFifo();
+    int turnOffGyroFifo();
+    int enableDmpOrientation(int);
+    int dmpOrientHandler(int);
+    int readDmpOrientEvents(sensors_event_t* data, int count);
+    int getDmpOrientFd();
+    int openDmpOrientFd();
+    int closeDmpOrientFd();
+
+    int getDmpRate(int64_t *);
+    int checkDMPOrientation();
+
+    int getDmpSignificantMotionFd();
+    int readDmpSignificantMotionEvents(sensors_event_t* data, int count);
+    int enableDmpSignificantMotion(int);
+    int significantMotionHandler(sensors_event_t* data);
+    bool checkSmdSupport(){return (mDmpSignificantMotionEnabled);};
+
+    int enableDmpPedometer(int, int);
+    int readDmpPedometerEvents(sensors_event_t* data, int count, int32_t id, int outputType);
+    int getDmpPedometerFd();
+    bool checkPedometerSupport() {return (mDmpPedometerEnabled || mDmpStepCountEnabled);};
+    bool checkOrientationSupport() {return ((isDmpDisplayOrientationOn()
+                                       && (mDmpOrientationEnabled
+                                       || !isDmpScreenAutoRotationEnabled())));};
+
+protected:
+    CompassSensor *mCompassSensor;
+    PressureSensor *mPressureSensor;
+
+    int gyroHandler(sensors_event_t *data);
+    int rawGyroHandler(sensors_event_t *data);
+    int accelHandler(sensors_event_t *data);
+    int compassHandler(sensors_event_t *data);
+    int rawCompassHandler(sensors_event_t *data);
+    int rvHandler(sensors_event_t *data);
+    int grvHandler(sensors_event_t *data);
+    int laHandler(sensors_event_t *data);
+    int gravHandler(sensors_event_t *data);
+    int orienHandler(sensors_event_t *data);
+    int smHandler(sensors_event_t *data);
+    int pHandler(sensors_event_t *data);
+    int gmHandler(sensors_event_t *data);
+    int psHandler(sensors_event_t *data);
+    int sdHandler(sensors_event_t *data);
+    int scHandler(sensors_event_t *data);
+    int metaHandler(sensors_event_t *data, int flags);
+    void calcOrientationSensor(float *Rx, float *Val);
+    virtual int update_delay();
+
+    void inv_set_device_properties();
+    int inv_constructor_init();
+    int inv_constructor_default_enable();
+    int setAccelInitialState();
+    int masterEnable(int en);
+    int enablePedStandalone(int en);
+    int enablePedStandaloneData(int en);
+    int enablePedQuaternion(int);
+    int enablePedQuaternionData(int);
+    int enable6AxisQuaternion(int);
+    int enable6AxisQuaternionData(int);
+    int enableLPQuaternion(int);
+    int enableQuaternionData(int);
+    int enableAccelPedometer(int);
+    int enableAccelPedData(int);
+    int onDmp(int);
+    int enableGyro(int en);
+    int enableLowPowerAccel(int en);
+    int enableAccel(int en);
+    int enableCompass(int en, int rawSensorOn);
+    int enablePressure(int en);
+    int enableBatch(int64_t timeout);
+    void computeLocalSensorMask(int enabled_sensors);
+    int computeBatchSensorMask(int enableSensor, int checkNewBatchSensor);
+    int computeBatchDataOutput();
+    int enableSensors(unsigned long sensors, int en, uint32_t changed);
+    int inv_read_temperature(long long *data);
+    int inv_read_dmp_state(int fd);
+    int inv_read_sensor_bias(int fd, long *data);
+    void inv_get_sensors_orientation(void);
+    int inv_init_sysfs_attributes(void);
+    int resetCompass(void);
+    void setCompassDelay(int64_t ns);
+    void enable_iio_sysfs(void);
+    int setDmpFeature(int en);
+    int computeAndSetDmpState(void);
+    int enablePedometer(int);
+    int enablePedIndicator(int en);
+    int checkPedStandaloneEnabled(void);
+    int checkPedQuatEnabled();
+    int check6AxisQuatEnabled();
+    int checkLPQRateSupported();
+    int checkLPQuaternion();
+    int checkAccelPed();
+    void setInitial6QuatValue();
+    int writeSignificantMotionParams(bool toggleEnable,
+                                     uint32_t delayThreshold1, uint32_t delayThreshold2,
+                                     uint32_t motionThreshold);
+    long mMasterSensorMask;
+    long mLocalSensorMask;
+    int mPollTime;
+    int64_t mStepCountPollTime;
+    bool mHaveGoodMpuCal;   // flag indicating that the cal file can be written
+    int mGyroAccuracy;      // value indicating the quality of the gyro calibr.
+    int mAccelAccuracy;     // value indicating the quality of the accel calibr.
+    int mCompassAccuracy;   // value indicating the quality of the compass calibr.
+    struct pollfd mPollFds[5];
+    pthread_mutex_t mMplMutex;
+    pthread_mutex_t mHALMutex;
+
+    char mIIOBuffer[(16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH];
+
+    int iio_fd;
+    int accel_fd;
+    int mpufifo_fd;
+    int gyro_temperature_fd;
+    int accel_x_offset_fd;
+    int accel_y_offset_fd;
+    int accel_z_offset_fd;
+
+    int accel_x_dmp_bias_fd;
+    int accel_y_dmp_bias_fd;
+    int accel_z_dmp_bias_fd;
+
+    int gyro_x_offset_fd;
+    int gyro_y_offset_fd;
+    int gyro_z_offset_fd;
+
+    int gyro_x_dmp_bias_fd;
+    int gyro_y_dmp_bias_fd;
+    int gyro_z_dmp_bias_fd;
+
+    int dmp_orient_fd;
+    int mDmpOrientationEnabled;
+
+    int dmp_sign_motion_fd;
+    int mDmpSignificantMotionEnabled;
+
+    int dmp_pedometer_fd;
+    int mDmpPedometerEnabled;
+    int mDmpStepCountEnabled;
+
+    uint32_t mEnabled;
+    uint32_t mBatchEnabled;
+    int32_t mFlushSensorEnabled;
+    uint32_t mOldBatchEnabledMask;
+    int64_t mBatchTimeoutInMs;
+    sensors_event_t mPendingEvents[NumSensors];
+    sensors_event_t mSmEvents;
+    sensors_event_t mSdEvents;
+    sensors_event_t mScEvents;
+    int64_t mDelays[NumSensors];
+    int64_t mBatchDelays[NumSensors];
+    int64_t mBatchTimeouts[NumSensors];
+    hfunc_t mHandlers[NumSensors];
+    short mCachedGyroData[3];
+    long mCachedAccelData[3];
+    long mCachedCompassData[3];
+    long mCachedQuaternionData[3];
+    long mCached6AxisQuaternionData[3];
+    long mCachedPedQuaternionData[3];
+    long mCachedPressureData;
+    android::KeyedVector<int, int> mIrqFds;
+
+    InputEventCircularReader mAccelInputReader;
+    InputEventCircularReader mGyroInputReader;
+
+    bool mFirstRead;
+    short mTempScale;
+    short mTempOffset;
+    int64_t mTempCurrentTime;
+    int mAccelScale;
+    long mAccelSelfTestScale;
+    long mGyroScale;
+    long mGyroSelfTestScale;
+    long mCompassScale;
+    float mCompassBias[3];
+    bool mFactoryGyroBiasAvailable;
+    long mFactoryGyroBias[3];
+    bool mGyroBiasAvailable;
+    bool mGyroBiasApplied;
+    float mGyroBias[3];    //in body frame
+    long mGyroChipBias[3]; //in chip frame
+    bool mFactoryAccelBiasAvailable;
+    long mFactoryAccelBias[3];
+    bool mAccelBiasAvailable;
+    bool mAccelBiasApplied;
+    long mAccelBias[3];    //in chip frame
+
+    uint32_t mPendingMask;
+    unsigned long mSensorMask;
+
+    char chip_ID[MAX_CHIP_ID_LEN];
+    char mSysfsPath[MAX_SYSFS_NAME_LEN];
+
+    signed char mGyroOrientation[9];
+    signed char mAccelOrientation[9];
+
+    int64_t mSensorTimestamp;
+    int64_t mCompassTimestamp;
+    int64_t mPressureTimestamp;
+
+    struct sysfs_attrbs {
+       char *chip_enable;
+       char *power_state;
+       char *master_enable;
+       char *dmp_firmware;
+       char *firmware_loaded;
+       char *dmp_on;
+       char *dmp_int_on;
+       char *dmp_event_int_on;
+       char *tap_on;
+       char *key;
+       char *self_test;
+       char *temperature;
+
+       char *gyro_enable;
+       char *gyro_fifo_rate;
+       char *gyro_fsr;
+       char *gyro_orient;
+       char *gyro_fifo_enable;
+       char *gyro_rate;
+
+       char *accel_enable;
+       char *accel_fifo_rate;
+       char *accel_fsr;
+       char *accel_bias;
+       char *accel_orient;
+       char *accel_fifo_enable;
+       char *accel_rate;
+
+       char *three_axis_q_on; //formerly quaternion_on
+       char *three_axis_q_rate;
+
+       char *six_axis_q_on;
+       char *six_axis_q_rate;
+
+       char *six_axis_q_value;
+
+       char *ped_q_on;
+       char *ped_q_rate;
+
+       char *step_detector_on;
+       char *step_indicator_on;
+
+       char *in_timestamp_en;
+       char *in_timestamp_index;
+       char *in_timestamp_type;
+
+       char *buffer_length;
+
+       char *display_orientation_on;
+       char *event_display_orientation;
+
+       char *in_accel_x_offset;
+       char *in_accel_y_offset;
+       char *in_accel_z_offset;
+       char *in_accel_self_test_scale;
+
+       char *in_accel_x_dmp_bias;
+       char *in_accel_y_dmp_bias;
+       char *in_accel_z_dmp_bias;
+
+       char *in_gyro_x_offset;
+       char *in_gyro_y_offset;
+       char *in_gyro_z_offset;
+       char *in_gyro_self_test_scale;
+
+       char *in_gyro_x_dmp_bias;
+       char *in_gyro_y_dmp_bias;
+       char *in_gyro_z_dmp_bias;
+
+       char *event_smd;
+       char *smd_enable;
+       char *smd_delay_threshold;
+       char *smd_delay_threshold2;
+       char *smd_threshold;
+       char *batchmode_timeout;
+       char *batchmode_wake_fifo_full_on;
+       char *flush_batch;
+
+       char *pedometer_on;
+       char *pedometer_int_on;
+       char *event_pedometer;
+       char *pedometer_steps;
+       char *pedometer_counter;
+       
+       char *motion_lpa_on;
+    } mpu;
+
+    char *sysfs_names_ptr;
+    int mMplFeatureActiveMask;
+    uint64_t mFeatureActiveMask;
+    bool mDmpOn;
+    int mPedUpdate;
+    int mPressureUpdate;
+    int64_t mQuatSensorTimestamp;
+    int64_t mStepSensorTimestamp;
+    uint64_t mLastStepCount;
+    int mLeftOverBufferSize;
+    char mLeftOverBuffer[1024];
+    bool mInitial6QuatValueAvailable;
+    long mInitial6QuatValue[4];
+    bool mFlushBatchSet;
+    uint32_t mSkipReadEvents;
+    bool mDataMarkerDetected;
+    bool mEmptyDataMarkerDetected;
+
+private:
+    /* added for dynamic get sensor list */
+    void fillAccel(const char* accel, struct sensor_t *list);
+    void fillGyro(const char* gyro, struct sensor_t *list);
+    void fillRV(struct sensor_t *list);
+    void fillGMRV(struct sensor_t *list);
+    void fillGRV(struct sensor_t *list);
+    void fillOrientation(struct sensor_t *list);
+    void fillGravity(struct sensor_t *list);
+    void fillLinearAccel(struct sensor_t *list);
+    void fillSignificantMotion(struct sensor_t *list);
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+    void fillScreenOrientation(struct sensor_t *list);
+#endif
+    void storeCalibration();
+    void loadDMP();
+    bool isMpuNonDmp();
+    int isLowPowerQuatEnabled();
+    int isDmpDisplayOrientationOn();
+    void getCompassBias();
+    void getFactoryGyroBias();
+    void setFactoryGyroBias();
+    void getGyroBias();
+    void setGyroZeroBias();
+    void setGyroBias();
+    void getFactoryAccelBias();
+    void setFactoryAccelBias();
+    void getAccelBias();
+    void setAccelBias();
+    int isCompassDisabled();
+    int setBatchDataRates();
+    int resetDataRates();
+    void initBias();
+    void resetMplStates();
+    void sys_dump(bool fileMode);
+};
+
+extern "C" {
+    void setCallbackObject(MPLSensor*);
+    MPLSensor *getCallbackObject();
+}
+
+#endif  // ANDROID_MPL_SENSOR_H
diff --git a/6515/libsensors_iio/MPLSensorSysApi.cpp b/6515/libsensors_iio/MPLSensorSysApi.cpp
new file mode 100644
index 0000000..92e8ea1
--- /dev/null
+++ b/6515/libsensors_iio/MPLSensorSysApi.cpp
@@ -0,0 +1,169 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#undef  NDEBUG
+#define NDEBUG 0
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <float.h>
+#include <poll.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <stdlib.h>
+#include <sys/select.h>
+#include <dlfcn.h>
+#include <pthread.h>
+
+#include <cutils/log.h>
+#include <utils/KeyedVector.h>
+#include <linux/input.h>
+
+#include "invensense.h"
+
+#include "MPLSensorSysApi.h"
+
+/*******************************************************************************/
+/* Gyro Driver Specific SYSFS Attribute                                        */
+/*******************************************************************************/
+#define GYRO_SENSOR_SYSFS_PATH  "/sys/class/input/input0/"
+#define GYRO_SENSOR_SELFTEST "device/inv_gyro/self_test"
+
+MplSys_Interface* getSysInterfaceObject()
+{
+    MPLSensorSysApi* s = static_cast<MPLSensorSysApi*>(MPLSensor::gMPLSensor);
+    return static_cast<MplSys_Interface*>(s);
+}
+
+MPLSensorSysApi::MPLSensorSysApi() : MPLSensor()
+{
+
+}
+
+MPLSensorSysApi::~MPLSensorSysApi()
+{
+
+}
+
+MPLSensorSysApi::MplSys_Interface::~MplSys_Interface()
+{
+
+}
+
+/* Should be getting from hardware self-test */
+int MPLSensorSysApi::getBiases(float *b)
+{
+    FUNC_LOG;
+    int rv = INV_SUCCESS;
+    long val[3];
+    long temp;
+    LOGV("get biases\n");
+    pthread_mutex_lock(&mMplMutex);
+    pthread_mutex_unlock(&mMplMutex);
+    return rv;
+}
+
+int MPLSensorSysApi::setBiases(float *b)
+{
+    FUNC_LOG;
+    int rv = INV_SUCCESS;
+    pthread_mutex_lock(&mMplMutex);
+    pthread_mutex_unlock(&mMplMutex);
+    return rv;
+}
+
+int MPLSensorSysApi::setBiasUpdateFunc(long f)
+{
+    FUNC_LOG;
+    LOGW("SysApi :: setBiasUpdateFunc is OBSOLETE and ineffective");
+    return 0;
+}
+
+int MPLSensorSysApi::setSensors(long s)
+{
+    FUNC_LOG;
+    int rv = INV_SUCCESS;
+
+    pthread_mutex_lock(&mMplMutex);
+    mMasterSensorMask = s;
+    pthread_mutex_unlock(&mMplMutex);
+    return rv;
+}
+
+int MPLSensorSysApi::getSensors(long* s)
+{
+    FUNC_LOG;
+    int rv = INV_SUCCESS;
+    pthread_mutex_lock(&mMplMutex);
+    pthread_mutex_unlock(&mMplMutex);
+    return rv;
+}
+
+int MPLSensorSysApi::resetCal()
+{
+    FUNC_LOG;
+    int rv = INV_SUCCESS;
+    LOGI("SysApi :: resetCal is OBSOLETE");
+    return rv;
+}
+
+int MPLSensorSysApi::selfTest()
+{
+    FUNC_LOG;
+    int rv = INV_SUCCESS;
+    char buf[50];
+
+    LOGV_IF(EXTRA_VERBOSE, "gyro set delay path: %s", GYRO_SENSOR_SELF_TEST);
+
+    int fd = open(mpu.self_test, O_RDONLY);
+    if( fd < 0 ) {
+        LOGE("Error opening gyro self-test");
+        return INV_ERROR;
+    }
+    pthread_mutex_lock(&mMplMutex);
+    char x[15], y[15], z[15];
+    char result[2];
+    do {
+        memset(buf, 0, sizeof(buf));
+        int count = read_attribute_sensor(fd, buf, sizeof(buf));
+        if( count < 1 ) {
+            LOGE("Error reading gyro self-test"); 
+			pthread_mutex_unlock(&mMplMutex);
+            return INV_ERROR;
+        }
+        sscanf(buf, "%[^','],%[^','],%[^','],%[^',']", x, y, z, result);
+        LOGI("Bias: X:Y:Z (%ld, %ld, %ld)", atol(x), atol(y), atol(z));
+        if ( atoi(result) ) {
+            LOGE("self test passed");
+        }
+        else {
+            LOGE("error self-test failed");
+            break;
+        }
+    } while (0);
+    pthread_mutex_unlock(&mMplMutex);
+    return rv;
+}
+
+int MPLSensorSysApi::setLocalMagField(float x, float y, float z)
+{
+    FUNC_LOG;
+    int rv = INV_SUCCESS;
+    LOGI("SysApi :: setLocalMagField is OBSOLETE");
+    return rv;
+}
+
diff --git a/6515/libsensors_iio/MPLSensorSysApi.h b/6515/libsensors_iio/MPLSensorSysApi.h
new file mode 100644
index 0000000..3c1c53e
--- /dev/null
+++ b/6515/libsensors_iio/MPLSensorSysApi.h
@@ -0,0 +1,49 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef INV_HAL_SYS_API_H
+#define INV_HAL_SYS_API_H
+
+#include "sensors.h"
+#include "MPLSensor.h"
+#include <gui/MplInterfaces.h>
+
+class MPLSensorSysApi : public MPLSensor, public MplSys_Interface {
+
+public:
+    MPLSensorSysApi();
+    virtual ~MPLSensorSysApi();
+
+    virtual int getBiases(float *b);
+    virtual int setBiases(float *b);
+    virtual int setBiasUpdateFunc(long f);
+    virtual int setSensors(long s);
+    virtual int getSensors(long* s);
+    virtual int resetCal();
+    virtual int selfTest();
+    virtual int setLocalMagField(float x, float y, float z);
+
+private:
+    int write_attribute_sensor(int fd, unsigned char* buf, size_t count);
+
+};
+
+extern "C" {
+MplSys_Interface* getSysInterfaceObject();
+}
+
+#endif
+
diff --git a/6515/libsensors_iio/MPLSupport.cpp b/6515/libsensors_iio/MPLSupport.cpp
new file mode 100644
index 0000000..08b48bb
--- /dev/null
+++ b/6515/libsensors_iio/MPLSupport.cpp
@@ -0,0 +1,387 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#include <MPLSupport.h>
+#include <dirent.h>
+#include <string.h>
+#include <stdio.h>
+#include "log.h"
+#include "SensorBase.h"
+#include <fcntl.h>
+
+#include "ml_sysfs_helper.h"
+#include "ml_load_dmp.h"
+
+int inv_read_data(char *fname, long *data)
+{
+    VFUNC_LOG;
+
+    char buf[sizeof(long) * 4];
+    int count, fd;
+
+    fd = open(fname, O_RDONLY);
+    if(fd < 0) {
+        LOGE("HAL:Error opening %s", fname);
+        return -1;
+    }
+    memset(buf, 0, sizeof(buf));
+    count = read_attribute_sensor(fd, buf, sizeof(buf));
+    if(count < 1) {
+        close(fd);
+        return -1;
+    } else {
+        count = sscanf(buf, "%ld", data);
+        if(count)
+            LOGV_IF(EXTRA_VERBOSE, "HAL:Data= %ld", *data);
+    }
+    close(fd);
+
+    return 0;
+}
+
+/* This one DOES NOT close FDs for you */
+int read_attribute_sensor(int fd, char* data, unsigned int size)
+{
+    VFUNC_LOG;
+
+    int count = 0;
+    if (fd > 0) {
+        count = pread(fd, data, size, 0);
+        if(count < 1) {
+            LOGE("HAL:read fails with error code=%d", count);
+        }
+    }
+    return count;
+}
+
+/**
+ *  @brief  Enable a sensor through the sysfs file descriptor
+ *          provided.
+ *  @note   this function one closes FD after the write
+ *  @param  fd
+ *              the file descriptor to write into
+ *  @param  en
+ *              the value to write, typically 1 or 0
+ *  @return the errno whenever applicable.
+ */
+int enable_sysfs_sensor(int fd, int en)
+{
+    VFUNC_LOG;
+
+    int nb;
+    int err = 0;
+
+    char c = en ? '1' : '0';
+    nb = write(fd, &c, 1);
+
+    if (nb <= 0) {
+        err = errno;
+        LOGE("HAL:enable_sysfs_sensor - write %c returned %d (%s / %d)",
+             c, nb, strerror(err), err);
+    }
+    close(fd);
+
+
+    return -err;
+}
+
+/* This one closes FDs for you */
+int write_attribute_sensor(int fd, long data)
+{
+    VFUNC_LOG;
+
+    int num_b = 0;
+
+    if (fd >= 0) {
+        char buf[80];
+        sprintf(buf, "%ld", data);
+        num_b = write(fd, buf, strlen(buf) + 1);
+        if (num_b <= 0) {
+            int err = errno;
+            LOGE("HAL:write fd %d returned '%s' (%d)", fd, strerror(err), err);
+        } else {
+            LOGV_IF(EXTRA_VERBOSE, "HAL:fd=%d write attribute to %ld", fd, data);
+        }
+        close(fd);
+    }
+
+    return num_b;
+}
+
+/* This one DOES NOT close FDs for you */
+int write_attribute_sensor_continuous(int fd, long data)
+{
+    VFUNC_LOG;
+
+    int num_b = 0;
+
+    if (fd >= 0) {
+        char buf[80];
+        sprintf(buf, "%ld", data);
+        num_b = write(fd, buf, strlen(buf) + 1);
+        if (num_b <= 0) {
+            int err = errno;
+            LOGE("HAL:write fd %d returned '%s' (%d)", fd, strerror(err), err);
+        } else {
+            LOGV_IF(EXTRA_VERBOSE, "HAL:fd=%d write attribute to %ld", fd, data);
+        }
+    }
+
+    return num_b;
+}
+
+int read_sysfs_int(char *filename, int *var)
+{
+    int res=0;
+    FILE  *sysfsfp;
+
+    sysfsfp = fopen(filename, "r");
+    if (sysfsfp != NULL) {
+        if (fscanf(sysfsfp, "%d\n", var) < 0 || fclose(sysfsfp) < 0) {    
+            res = errno;
+            LOGE("HAL:ERR open file %s to read with error %d", filename, res);
+        }
+    }
+    return -res;
+}
+
+int read_sysfs_int64(char *filename, int64_t *var)
+{
+    int res=0;
+    FILE  *sysfsfp;
+
+    sysfsfp = fopen(filename, "r");
+    if (sysfsfp != NULL) {
+        if (fscanf(sysfsfp, "%lld\n", var) < 0 || fclose(sysfsfp) < 0) {    
+            res = errno;
+            LOGE("HAL:ERR open file %s to read with error %d", filename, res);
+        }
+    }
+    return -res;
+}
+
+void convert_long_to_hex_char(long* quat, unsigned char* hex, int numElement)
+{
+    int bytePosition = 0;
+    for (int index = 0; index < numElement; index++) {
+        for (int i = 0; i < 4; i++) {
+            hex[bytePosition] = (int) ((quat[index] >> (4-1-i) * 8) & 0xFF);                                 
+            //LOGI("e%d quat[%d]: %x", index, bytePosition, hex[bytePosition]);
+            bytePosition++;
+        }
+    }
+    return;
+}
+
+int write_sysfs_int(char *filename, int var)
+{
+    int res=0;
+    FILE  *sysfsfp;
+
+    sysfsfp = fopen(filename, "w");
+    if (sysfsfp != NULL) {
+        if (fprintf(sysfsfp, "%d\n", var) < 0 || fclose(sysfsfp) < 0) {
+            res = errno;
+            LOGE("HAL:ERR open file %s to write with error %d", filename, res);
+        }
+    }
+    return -res;
+}
+
+int write_sysfs_longlong(char *filename, int64_t var)
+{
+    int res=0;
+    FILE  *sysfsfp;
+
+    sysfsfp = fopen(filename, "w");
+    if (sysfsfp != NULL) {
+        if (fprintf(sysfsfp, "%lld\n", var) < 0 || fclose(sysfsfp) < 0) {   
+            res = errno;
+            LOGE("HAL:ERR open file %s to write with error %d", filename, res);
+        }
+    }
+    return -res;
+}
+
+int fill_dev_full_name_by_prefix(const char* dev_prefix, 
+                                 char *dev_full_name, int len)
+{
+    char cand_name[20];
+    int prefix_len = strlen(dev_prefix);
+    strncpy(cand_name, dev_prefix, sizeof(cand_name) / sizeof(cand_name[0]));
+
+    // try adding a number, 0-9
+    for(int cand_postfix = 0; cand_postfix < 10; cand_postfix++) {
+        snprintf(&cand_name[prefix_len],
+                 sizeof(cand_name) / sizeof(cand_name[0]), 
+                 "%d", cand_postfix);
+        int dev_num = find_type_by_name(cand_name, "iio:device");
+        if (dev_num != -ENODEV) {
+            strncpy(dev_full_name, cand_name, len);
+            return 0; 
+        }
+    }
+    // try adding a small letter, a-z
+    for(char cand_postfix = 'a'; cand_postfix <= 'z'; cand_postfix++) {
+        snprintf(&cand_name[prefix_len], 
+                 sizeof(cand_name) / sizeof(cand_name[0]), 
+                 "%c", cand_postfix);
+        int dev_num = find_type_by_name(cand_name, "iio:device");
+        if (dev_num != -ENODEV) {
+            strncpy(dev_full_name, cand_name, len);
+            return 0; 
+        }
+    }
+    // try adding a capital letter, A-Z
+    for(char cand_postfix = 'A'; cand_postfix <= 'Z'; cand_postfix++) {
+        snprintf(&cand_name[prefix_len],
+                 sizeof(cand_name) / sizeof(cand_name[0]), 
+                 "%c", cand_postfix);
+        int dev_num = find_type_by_name(cand_name, "iio:device");
+        if (dev_num != -ENODEV) {
+            strncpy(dev_full_name, cand_name, len);
+            return 0; 
+        }
+    }
+    return 1;
+}
+
+void dump_dmp_img(const char *outFile)
+{
+    FILE *fp;
+    int i;
+
+    char sysfs_path[MAX_SYSFS_NAME_LEN];
+    char dmp_path[MAX_SYSFS_NAME_LEN];
+
+    inv_get_sysfs_path(sysfs_path);
+    sprintf(dmp_path, "%s%s", sysfs_path, "/dmp_firmware");
+
+    LOGI("HAL DEBUG:dump DMP image");
+    LOGI("HAL DEBUG:open %s\n", dmp_path);
+    LOGI("HAL DEBUG:write to %s", outFile);
+
+    read_dmp_img(dmp_path, (char *)outFile);
+}
+
+int read_sysfs_dir(bool fileMode, char *sysfs_path)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+    char full_path[MAX_SYSFS_NAME_LEN];
+    int fd;
+    char buf[sizeof(long) *4];
+    long data;
+  
+    DIR *dp;
+    struct dirent *ep;
+      
+    dp = opendir (sysfs_path);
+
+    if (dp != NULL)
+    {
+        LOGI("******************** System Sysfs Dump ***************************");
+        LOGV_IF(0,"HAL DEBUG: opened directory %s", sysfs_path);
+        while ((ep = readdir (dp))) {
+            if(ep != NULL) {
+                LOGV_IF(0,"file name %s", ep->d_name);
+                if(!strcmp(ep->d_name, ".") || !strcmp(ep->d_name, "..") ||
+                         !strcmp(ep->d_name, "uevent") || !strcmp(ep->d_name, "dev") ||
+                         !strcmp(ep->d_name, "self_test"))
+                    continue;
+                sprintf(full_path, "%s%s%s", sysfs_path, "/", ep->d_name);
+                LOGV_IF(0,"HAL DEBUG: reading %s", full_path);
+                fd = open(full_path, O_RDONLY);
+                if (fd > -1) {
+                    memset(buf, 0, sizeof(buf));
+                    res = read_attribute_sensor(fd, buf, sizeof(buf));
+                    close(fd);
+                    if (res > 0) {
+                        res = sscanf(buf, "%ld", &data);
+                        if (res)
+                            LOGI("HAL DEBUG:sysfs:cat %s = %ld", full_path, data);
+                    } else {
+                         LOGV_IF(0,"HAL DEBUG: error reading %s", full_path);
+                    } 
+                } else {
+                    LOGV_IF(0,"HAL DEBUG: error opening %s", full_path);
+                }
+                close(fd);
+            }
+        }
+        closedir(dp);
+    } else{
+        LOGI("HAL DEBUG: could not open directory %s", sysfs_path);
+    }
+
+    return res;
+}
+
+int inv_float_to_q16(float *fdata, long *ldata)
+{
+
+    if (!fdata || !ldata)
+        return -1;
+    ldata[0] = (long)(fdata[0] * 65536.f);
+    ldata[1] = (long)(fdata[1] * 65536.f);
+    ldata[2] = (long)(fdata[2] * 65536.f);
+    return 0;
+}
+
+int inv_long_to_q16(long *fdata, long *ldata)
+{
+
+    if (!fdata || !ldata)
+        return -1;
+    ldata[0] = (fdata[1] * 65536.f);
+    ldata[1] = (fdata[2] * 65536.f);
+    ldata[2] = (fdata[3] * 65536.f);
+    return 0;
+}
+
+int inv_float_to_round(float *fdata, long *ldata)
+{
+
+    if (!fdata || !ldata)
+            return -1;
+    ldata[0] = (long)fdata[0];
+    ldata[1] = (long)fdata[1];
+    ldata[2] = (long)fdata[2];
+    return 0;
+}
+
+int inv_float_to_round2(float *fdata, short *ldata)
+{
+
+    if (!fdata || !ldata)
+        return -1;
+    ldata[0] = (short)fdata[0];
+    ldata[1] = (short)fdata[1];
+    ldata[2] = (short)fdata[2];
+    return 0;
+}
+
+int inv_long_to_float(long *ldata, float *fdata)
+{
+
+    if (!ldata || !fdata)
+        return -1;
+    fdata[0] = (float)ldata[0];
+    fdata[1] = (float)ldata[1];
+    fdata[2] = (float)ldata[2];
+    return 0;
+}
diff --git a/6515/libsensors_iio/MPLSupport.h b/6515/libsensors_iio/MPLSupport.h
new file mode 100644
index 0000000..c7f8eff
--- /dev/null
+++ b/6515/libsensors_iio/MPLSupport.h
@@ -0,0 +1,43 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef ANDROID_MPL_SUPPORT_H
+#define ANDROID_MPL_SUPPORT_H
+
+#include <stdint.h>
+
+int inv_read_data(char *fname, long *data);
+int read_attribute_sensor(int fd, char* data, unsigned int size);
+int enable_sysfs_sensor(int fd, int en);
+int write_attribute_sensor(int fd, long data);
+int write_attribute_sensor_continuous(int fd, long data);
+int read_sysfs_int64(char*, int64_t*);
+int read_sysfs_int(char*, int*);
+int write_sysfs_int(char*, int);
+int write_sysfs_longlong(char*, long long);
+int fill_dev_full_name_by_prefix(const char* dev_prefix,
+                                 char* dev_full_name, int len);
+void dump_dmp_img(const char *out_file);
+int read_sysfs_dir(bool fileMode, char *sysfs_path);
+
+void convert_long_to_hex_char(long* quat, unsigned char* hex, int numElement);
+int inv_float_to_q16(float *fdata, long *ldata);
+int inv_long_to_q16(long *fdata, long *ldata);
+int inv_float_to_round(float *fdata, long *ldata);
+int inv_float_to_round2(float *fdata, short *sdata);
+int inv_long_to_float(long *ldata, float *fdata);
+
+#endif //  ANDROID_MPL_SUPPORT_H
diff --git a/6515/libsensors_iio/PressureSensor.IIO.secondary.cpp b/6515/libsensors_iio/PressureSensor.IIO.secondary.cpp
new file mode 100644
index 0000000..1a33c19
--- /dev/null
+++ b/6515/libsensors_iio/PressureSensor.IIO.secondary.cpp
@@ -0,0 +1,200 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#define LOG_NDEBUG 0
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <sys/select.h>
+#include <cutils/log.h>
+#include <linux/input.h>
+#include <string.h>
+
+#include "PressureSensor.IIO.secondary.h"
+#include "sensors.h"
+#include "MPLSupport.h"
+#include "sensor_params.h"
+#include "ml_sysfs_helper.h"
+
+#pragma message("HAL:build pressure sensor on Invensense MPU secondary bus")
+/* dynamically get this when driver supports it */
+#define CHIP_ID "BMP280"
+
+//#define TIMER (1)
+#define DEFAULT_POLL_TIME 300
+#define PRESSURE_MAX_SYSFS_ATTRB sizeof(pressureSysFs) / sizeof(char*)
+
+static int s_poll_time = -1;
+static int min_poll_time = 50;
+static struct timespec t_pre;
+
+/*****************************************************************************/
+
+PressureSensor::PressureSensor(const char *sysfs_path) 
+                  : SensorBase(NULL, NULL),
+                    pressure_fd(-1)
+{
+    VFUNC_LOG;
+
+    mSysfsPath = sysfs_path;
+    LOGV_IF(ENG_VERBOSE, "pressuresensor path: %s", mSysfsPath);
+    if(inv_init_sysfs_attributes()) {
+        LOGE("Error Instantiating Pressure Sensor\n");
+        return;
+    } else {
+        LOGI_IF(PROCESS_VERBOSE, "HAL:Secondary Chip Id: %s", CHIP_ID);
+    }
+}
+
+PressureSensor::~PressureSensor()
+{
+    VFUNC_LOG;
+
+    if( pressure_fd > 0)
+        close(pressure_fd);
+}
+
+int PressureSensor::getFd() const
+{
+    VHANDLER_LOG;
+    return pressure_fd;
+}
+
+/**
+ *  @brief        This function will enable/disable sensor.
+ *  @param[in]    handle
+ *                  which sensor to enable/disable.
+ *  @param[in]    en
+ *                  en=1, enable; 
+ *                  en=0, disable
+ *  @return       if the operation is successful.
+ */
+int PressureSensor::enable(int32_t handle, int en) 
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, pressureSysFs.pressure_enable, getTimestamp());
+    res = write_sysfs_int(pressureSysFs.pressure_enable, en);
+
+    return res;
+}
+
+int PressureSensor::setDelay(int32_t handle, int64_t ns) 
+{
+    VFUNC_LOG;
+    
+    int res = 0;
+
+    mDelay = int(1000000000.f / ns);
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %lld > %s (%lld)",
+            mDelay, pressureSysFs.pressure_rate, getTimestamp());
+    res = write_sysfs_int(pressureSysFs.pressure_rate, mDelay);
+     
+#ifdef TIMER
+    int t_poll_time = (int)(ns / 1000000LL);
+    if (t_poll_time > min_poll_time) {
+        s_poll_time = t_poll_time;
+    } else {
+        s_poll_time = min_poll_time;
+    }
+    LOGV_IF(PROCESS_VERBOSE,
+            "HAL:setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f/ns);
+#endif
+    return res;
+}
+
+
+/**
+    @brief      This function will return the state of the sensor.
+    @return     1=enabled; 0=disabled
+**/
+int PressureSensor::getEnable(int32_t handle)
+{
+    VFUNC_LOG;
+    return mEnable;
+}
+
+/**
+ *  @brief  This function will return the current delay for this sensor.
+ *  @return delay in nanoseconds. 
+ */
+int64_t PressureSensor::getDelay(int32_t handle)
+{
+    VFUNC_LOG;
+
+#ifdef TIMER
+    if (mEnable) {
+        return s_poll_time;
+    } else {
+        return -1;
+    }
+#endif
+    return mDelay;
+}
+
+void PressureSensor::fillList(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    const char *pressure = "BMP280";
+
+    if (pressure) {
+        if(!strcmp(pressure, "BMP280")) {
+            list->maxRange = PRESSURE_BMP280_RANGE;
+            list->resolution = PRESSURE_BMP280_RESOLUTION;
+            list->power = PRESSURE_BMP280_POWER;
+            list->minDelay = PRESSURE_BMP280_MINDELAY;
+            mMinDelay = list->minDelay;
+            return;
+        }      
+    }
+    LOGE("HAL:unknown pressure id %s -- "
+         "params default to bmp280 and might be wrong.",
+         pressure);
+    list->maxRange = PRESSURE_BMP280_RANGE;
+    list->resolution = PRESSURE_BMP280_RESOLUTION;
+    list->power = PRESSURE_BMP280_POWER;
+    list->minDelay = PRESSURE_BMP280_MINDELAY;
+    mMinDelay = list->minDelay;
+    return;
+}
+
+int PressureSensor::inv_init_sysfs_attributes(void)
+{
+    VFUNC_LOG;
+ 
+    pathP = (char*)malloc(sizeof(char[PRESSURE_MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
+    char *sptr = pathP;
+    char **dptr = (char**)&pressureSysFs;
+    if (sptr == NULL)
+        return -1;
+    unsigned char i = 0;
+    do {
+        *dptr++ = sptr;
+        memset(sptr, 0, sizeof(sptr));
+        sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
+    } while (++i < PRESSURE_MAX_SYSFS_ATTRB);
+ 
+    sprintf(pressureSysFs.pressure_enable, "%s%s", mSysfsPath, "/pressure_enable");
+    sprintf(pressureSysFs.pressure_rate, "%s%s", mSysfsPath, "/pressure_rate");
+    return 0;
+}
diff --git a/6515/libsensors_iio/PressureSensor.IIO.secondary.h b/6515/libsensors_iio/PressureSensor.IIO.secondary.h
new file mode 100644
index 0000000..3dae748
--- /dev/null
+++ b/6515/libsensors_iio/PressureSensor.IIO.secondary.h
@@ -0,0 +1,76 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef PRESSURE_SENSOR_H
+#define PRESSURE_SENSOR_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <poll.h>
+#include <utils/Vector.h>
+#include <utils/KeyedVector.h>
+
+#include "sensors.h"
+#include "SensorBase.h"
+#include "InputEventReader.h"
+
+class PressureSensor : public SensorBase {
+
+public:
+    PressureSensor(const char *sysfs_path);
+    virtual ~PressureSensor();
+
+    virtual int getFd() const;
+    virtual int enable(int32_t handle, int enabled);
+    virtual int setDelay(int32_t handle, int64_t ns);
+    virtual int getEnable(int32_t handle);
+    virtual int64_t getDelay(int32_t handle);
+    virtual int64_t getMinDelay() { return mMinDelay; }
+    // only applicable to primary
+    virtual int readEvents(sensors_event_t *data, int count) { return 0; }
+            
+    void fillList(struct sensor_t *list);    
+    // default is integrated for secondary bus
+    int isIntegrated() { return (1); }
+
+private:
+    char sensor_name[200];
+    
+    struct sysfs_attrbs {
+       char *pressure_enable;
+       char *pressure_rate;
+    } pressureSysFs;
+
+    int pressure_fd; 
+    int64_t mDelay;
+    int64_t mMinDelay;
+    int mEnable;
+    char* pathP;
+    const char* mSysfsPath;
+
+    int inv_init_sysfs_attributes(void);
+};
+
+/*****************************************************************************/
+
+#endif  // PRESSURE_SENSOR_H
diff --git a/6515/libsensors_iio/SensorBase.cpp b/6515/libsensors_iio/SensorBase.cpp
new file mode 100644
index 0000000..016d2df
--- /dev/null
+++ b/6515/libsensors_iio/SensorBase.cpp
@@ -0,0 +1,211 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <dirent.h>
+#include <sys/select.h>
+#include <cutils/log.h>
+#include <linux/input.h>
+
+#include <cutils/properties.h>
+
+#include "SensorBase.h"
+
+/*****************************************************************************/
+
+// static vars
+bool SensorBase::PROCESS_VERBOSE = false;
+bool SensorBase::EXTRA_VERBOSE = false;
+bool SensorBase::SYSFS_VERBOSE = false;
+
+bool SensorBase::FUNC_ENTRY = false;
+bool SensorBase::HANDLER_ENTRY = false;
+bool SensorBase::ENG_VERBOSE = false;
+bool SensorBase::INPUT_DATA = false;
+bool SensorBase::HANDLER_DATA = false;
+bool SensorBase::DEBUG_BATCHING = false;
+
+SensorBase::SensorBase(const char* dev_name,
+                       const char* data_name) 
+                        : dev_name(dev_name),
+                          data_name(data_name),
+                          dev_fd(-1),
+                          data_fd(-1)
+{
+    if (data_name) {
+        data_fd = openInput(data_name);
+    }
+
+    char value[PROPERTY_VALUE_MAX];
+    property_get("invn.hal.verbose.basic", value, "0");
+    if (atoi(value)) {
+        PROCESS_VERBOSE = true;
+    }
+    property_get("invn.hal.verbose.extra", value, "0");
+    if (atoi(value)) {
+        EXTRA_VERBOSE = true;
+    }
+    property_get("invn.hal.verbose.sysfs", value, "0");
+    if (atoi(value)) {
+        SYSFS_VERBOSE = true;
+    }
+    property_get("invn.hal.verbose.engineering", value, "0");
+    if (atoi(value)) {
+        ENG_VERBOSE = true;
+    }
+    property_get("invn.hal.entry.function", value, "0");
+    if (atoi(value)) {
+        FUNC_ENTRY = true;
+    }
+    property_get("invn.hal.entry.handler", value, "0");
+    if (atoi(value)) {
+        HANDLER_ENTRY = true;
+    }
+    property_get("invn.hal.data.input", value, "0");
+    if (atoi(value)) {
+        INPUT_DATA = true;
+    }
+    property_get("invn.hal.data.handler", value, "0");
+    if (atoi(value)) {
+        HANDLER_DATA = true;
+    }
+    property_get("invn.hal.debug.batching", value, "0");
+    if (atoi(value)) {
+        DEBUG_BATCHING = true;
+    }
+}
+
+SensorBase::~SensorBase()
+{
+    if (data_fd >= 0) {
+        close(data_fd);
+    }
+    if (dev_fd >= 0) {
+        close(dev_fd);
+    }
+}
+
+int SensorBase::open_device()
+{
+    if (dev_fd<0 && dev_name) {
+        dev_fd = open(dev_name, O_RDONLY);
+        LOGE_IF(dev_fd<0, "Couldn't open %s (%s)", dev_name, strerror(errno));
+    }
+    return 0;
+}
+
+int SensorBase::close_device()
+{
+    if (dev_fd >= 0) {
+        close(dev_fd);
+        dev_fd = -1;
+    }
+    return 0;
+}
+
+int SensorBase::getFd() const
+{
+    if (!data_name) {
+        return dev_fd;
+    }
+    return data_fd;
+}
+
+int SensorBase::setDelay(int32_t handle, int64_t ns)
+{
+    return 0;
+}
+
+bool SensorBase::hasPendingEvents() const
+{
+    return false;
+}
+
+int64_t SensorBase::getTimestamp()
+{
+    struct timespec t;
+    t.tv_sec = t.tv_nsec = 0;
+    clock_gettime(CLOCK_MONOTONIC, &t);
+    return int64_t(t.tv_sec) * 1000000000LL + t.tv_nsec;
+}
+
+int SensorBase::openInput(const char *inputName)
+{
+    int fd = -1;
+    const char *dirname = "/dev/input";
+    char devname[PATH_MAX];
+    char *filename;
+    DIR *dir;
+    struct dirent *de;
+    dir = opendir(dirname);
+    if(dir == NULL)
+        return -1;
+    strcpy(devname, dirname);
+    filename = devname + strlen(devname);
+    *filename++ = '/';
+    while((de = readdir(dir))) {
+        if(de->d_name[0] == '.' &&
+                (de->d_name[1] == '\0' ||
+                        (de->d_name[1] == '.' && de->d_name[2] == '\0')))
+            continue;
+        strcpy(filename, de->d_name);
+        fd = open(devname, O_RDONLY);
+        LOGV_IF(EXTRA_VERBOSE, "path open %s", devname);
+        LOGI("path open %s", devname);
+        if (fd >= 0) {
+            char name[80];
+            if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) {
+                name[0] = '\0';
+            }
+            LOGV_IF(EXTRA_VERBOSE, "name read %s", name);
+            if (!strcmp(name, inputName)) {
+                strcpy(input_name, filename);
+                break;
+            } else {
+                close(fd);
+                fd = -1;
+            }
+        }
+    }
+    closedir(dir);
+    LOGE_IF(fd < 0, "couldn't find '%s' input device", inputName);
+    return fd;
+}
+
+int SensorBase::enable(int32_t handle, int enabled)
+{
+    return 0;
+}
+
+int SensorBase::query(int what, int* value)
+{
+    return 0;
+}
+
+int SensorBase::batch(int handle, int flags, int64_t period_ns, int64_t timeout)
+{
+    return 0;
+}
+
+int SensorBase::flush(int handle)
+{
+    return 0;
+}
diff --git a/6515/libsensors_iio/SensorBase.h b/6515/libsensors_iio/SensorBase.h
new file mode 100644
index 0000000..3a30310
--- /dev/null
+++ b/6515/libsensors_iio/SensorBase.h
@@ -0,0 +1,104 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef ANDROID_SENSOR_BASE_H
+#define ANDROID_SENSOR_BASE_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#if defined ANDROID_JELLYBEAN || defined ANDROID_KITKAT
+//build for Jellybean or KitKat
+#define LOGV_IF ALOGV_IF
+#define LOGE_IF ALOGE_IF
+#define LOGI_IF ALOGI_IF
+#define LOGI    ALOGI
+#define LOGE    ALOGE
+#define LOGV    ALOGV
+#define LOGW    ALOGW
+#else
+//build for ICS or earlier version
+#endif
+
+#define FUNC_LOG \
+            LOGV("%s", __PRETTY_FUNCTION__)
+#define VFUNC_LOG \
+            LOGV_IF(SensorBase::FUNC_ENTRY, \
+                    "Entering function '%s'", __PRETTY_FUNCTION__)
+#define VHANDLER_LOG \
+            LOGV_IF(SensorBase::HANDLER_ENTRY, \
+                    "Entering handler '%s'", __PRETTY_FUNCTION__)
+#define CALL_MEMBER_FN(pobject, ptrToMember) ((pobject)->*(ptrToMember))
+
+#define MAX_SYSFS_NAME_LEN  (100)
+#define IIO_BUFFER_LENGTH   (480)
+
+/*****************************************************************************/
+
+struct sensors_event_t;
+
+class SensorBase {
+public:
+    /* Log enablers, each of these independent */
+    static bool PROCESS_VERBOSE;   /* process log messages */
+    static bool EXTRA_VERBOSE;     /* verbose log messages */
+    static bool SYSFS_VERBOSE;     /* log sysfs interactions as cat/echo for 
+                                      repro purpose on a shell */
+    /* Note that enabling this logs may affect performance */
+    static bool FUNC_ENTRY;        /* log entry in all one-time functions */
+    static bool HANDLER_ENTRY;     /* log entry in all handler functions */
+    static bool ENG_VERBOSE;       /* log a lot more info about the internals */
+    static bool INPUT_DATA;        /* log the data input from the events */
+    static bool HANDLER_DATA;      /* log the data fetched from the handlers */
+    static bool DEBUG_BATCHING;    /* log the data for debugging batching */
+
+protected:
+    const char *dev_name;
+    const char *data_name;
+    char input_name[PATH_MAX];
+    int dev_fd;
+    int data_fd;
+
+    int openInput(const char* inputName);
+    static int64_t getTimestamp();
+    static int64_t timevalToNano(timeval const& t) {
+        return t.tv_sec * 1000000000LL + t.tv_usec * 1000;
+    }
+
+    int open_device();
+    int close_device();
+
+public:
+    SensorBase(const char* dev_name, const char* data_name);
+    virtual ~SensorBase();
+
+    virtual int readEvents(sensors_event_t* data, int count) = 0;
+    int readSample(long *data, int64_t *timestamp);
+    int readRawSample(float *data, int64_t *timestamp);
+    virtual bool hasPendingEvents() const;
+    virtual int getFd() const;
+    virtual int setDelay(int32_t handle, int64_t ns);
+    virtual int enable(int32_t handle, int enabled);
+    virtual int query(int what, int* value);
+    virtual int batch(int handle, int flags, int64_t period_ns, int64_t timeout);
+    virtual int flush(int handle);
+};
+
+/*****************************************************************************/
+
+#endif  // ANDROID_SENSOR_BASE_H
diff --git a/6515/libsensors_iio/libmllite.so b/6515/libsensors_iio/libmllite.so
new file mode 100644
index 0000000..407d4f2
--- /dev/null
+++ b/6515/libsensors_iio/libmllite.so
diff --git a/6515/libsensors_iio/libmplmpu.so b/6515/libsensors_iio/libmplmpu.so
new file mode 100644
index 0000000..e1b0276
--- /dev/null
+++ b/6515/libsensors_iio/libmplmpu.so
diff --git a/6515/libsensors_iio/sensor_params.h b/6515/libsensors_iio/sensor_params.h
new file mode 100644
index 0000000..d9bd978
--- /dev/null
+++ b/6515/libsensors_iio/sensor_params.h
@@ -0,0 +1,204 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef INV_SENSOR_PARAMS_H
+#define INV_SENSOR_PARAMS_H
+
+/******************************************/
+/******************************************/
+//MPU9350 INV_COMPASS
+#define COMPASS_MPU9350_RANGE           (9830.f)
+#define COMPASS_MPU9350_RESOLUTION      (0.15f)
+#define COMPASS_MPU9350_POWER           (10.f)
+#define COMPASS_MPU9350_MINDELAY        (1000)
+//MPU9250 INV_COMPASS
+#define COMPASS_MPU9250_RANGE           (9830.f)
+#define COMPASS_MPU9250_RESOLUTION      (0.15f)
+#define COMPASS_MPU9250_POWER           (10.f)
+#define COMPASS_MPU9250_MINDELAY        (10000)
+//MPU9150 INV_COMPASS
+#define COMPASS_MPU9150_RANGE           (9830.f)
+#define COMPASS_MPU9150_RESOLUTION      (0.285f)
+#define COMPASS_MPU9150_POWER           (10.f)
+#define COMPASS_MPU9150_MINDELAY        (10000)
+//COMPASS_ID_AK8975
+#define COMPASS_AKM8975_RANGE           (9830.f)
+#define COMPASS_AKM8975_RESOLUTION      (0.285f)
+#define COMPASS_AKM8975_POWER           (10.f)
+#define COMPASS_AKM8975_MINDELAY        (10000)
+//COMPASS_ID_AK8963C
+#define COMPASS_AKM8963_RANGE           (9830.f)
+#define COMPASS_AKM8963_RESOLUTION      (0.15f)
+#define COMPASS_AKM8963_POWER           (10.f)
+#define COMPASS_AKM8963_MINDELAY        (10000)
+//COMPASS_ID_AMI30X
+#define COMPASS_AMI30X_RANGE            (5461.f)
+#define COMPASS_AMI30X_RESOLUTION       (0.9f)
+#define COMPASS_AMI30X_POWER            (0.15f)
+//COMPASS_ID_AMI306
+#define COMPASS_AMI306_RANGE            (5461.f)
+#define COMPASS_AMI306_RESOLUTION       (0.9f)
+#define COMPASS_AMI306_POWER            (0.15f)
+#define COMPASS_AMI306_MINDELAY         (10000)
+//COMPASS_ID_YAS529
+#define COMPASS_YAS529_RANGE            (19660.f)
+#define COMPASS_YAS529_RESOLUTION       (0.012f)
+#define COMPASS_YAS529_POWER            (4.f)
+//COMPASS_ID_YAS53x
+#define COMPASS_YAS53x_RANGE            (8001.f)
+#define COMPASS_YAS53x_RESOLUTION       (0.012f)
+#define COMPASS_YAS53x_POWER            (4.f)
+#define COMPASS_YAS53x_MINDELAY         (10000)
+//COMPASS_ID_HMC5883
+#define COMPASS_HMC5883_RANGE           (10673.f)
+#define COMPASS_HMC5883_RESOLUTION      (10.f)
+#define COMPASS_HMC5883_POWER           (0.24f)
+//COMPASS_ID_LSM303DLH
+#define COMPASS_LSM303DLH_RANGE         (10240.f)
+#define COMPASS_LSM303DLH_RESOLUTION    (1.f)
+#define COMPASS_LSM303DLH_POWER         (1.f)
+//COMPASS_ID_LSM303DLM
+#define COMPASS_LSM303DLM_RANGE         (10240.f)
+#define COMPASS_LSM303DLM_RESOLUTION    (1.f)
+#define COMPASS_LSM303DLM_POWER         (1.f)
+//COMPASS_ID_MMC314X
+#define COMPASS_MMC314X_RANGE           (400.f)
+#define COMPASS_MMC314X_RESOLUTION      (2.f)
+#define COMPASS_MMC314X_POWER           (0.55f)
+//COMPASS_ID_HSCDTD002B
+#define COMPASS_HSCDTD002B_RANGE        (9830.f)
+#define COMPASS_HSCDTD002B_RESOLUTION   (1.f)
+#define COMPASS_HSCDTD002B_POWER        (1.f)
+//COMPASS_ID_HSCDTD004A
+#define COMPASS_HSCDTD004A_RANGE        (9830.f)
+#define COMPASS_HSCDTD004A_RESOLUTION   (1.f)
+#define COMPASS_HSCDTD004A_POWER        (1.f)
+/*******************************************/
+//ACCEL_ID_MPU6500
+#define ACCEL_MPU6500_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MPU6500_RESOLUTION        (0.004f * GRAVITY_EARTH)
+#define ACCEL_MPU6500_POWER             (0.5f)
+#define ACCEL_MPU6500_MINDELAY          (5000)
+//ACCEL_ID_MPU9350
+#define ACCEL_MPU9350_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MPU9350_RESOLUTION        (0.004f * GRAVITY_EARTH)
+#define ACCEL_MPU9350_POWER             (0.5f)
+#define ACCEL_MPU9350_MINDELAY          (1000)
+//ACCEL_ID_MPU9250
+#define ACCEL_MPU9250_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MPU9250_RESOLUTION        (0.004f * GRAVITY_EARTH)
+#define ACCEL_MPU9250_POWER             (0.5f)
+#define ACCEL_MPU9250_MINDELAY          (5000)
+//ACCEL_ID_MPU9150
+#define ACCEL_MPU9150_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MPU9150_RESOLUTION        (0.004f * GRAVITY_EARTH)
+#define ACCEL_MPU9150_POWER             (0.5f)
+#define ACCEL_MPU9150_MINDELAY          (1000)
+//ACCEL_ID_LIS331
+#define ACCEL_LIS331_RANGE              (2.48f * GRAVITY_EARTH)
+#define ACCEL_LIS331_RESOLUTION         (0.001f * GRAVITY_EARTH)
+#define ACCEL_LIS331_POWER              (1.f)
+//ACCEL_ID_LSM303DLX
+#define ACCEL_LSM303DLX_RANGE           (2.48f * GRAVITY_EARTH)
+#define ACCEL_LSM303DLX_RESOLUTION      (0.001f * GRAVITY_EARTH)
+#define ACCEL_LSM303DLX_POWER           (1.f)
+//ACCEL_ID_LIS3DH
+#define ACCEL_LIS3DH_RANGE              (2.48f * GRAVITY_EARTH)
+#define ACCEL_LIS3DH_RESOLUTION         (0.001f * GRAVITY_EARTH)
+#define ACCEL_LIS3DH_POWER              (1.f)
+//ACCEL_ID_KXSD9
+#define ACCEL_KXSD9_RANGE               (2.5006f * GRAVITY_EARTH)
+#define ACCEL_KXSD9_RESOLUTION          (0.001f * GRAVITY_EARTH)
+#define ACCEL_KXSD9_POWER               (1.f)
+//ACCEL_ID_KXTF9
+#define ACCEL_KXTF9_RANGE               (1.f * GRAVITY_EARTH)
+#define ACCEL_KXTF9_RESOLUTION          (0.033f * GRAVITY_EARTH)
+#define ACCEL_KXTF9_POWER               (0.35f)
+//ACCEL_ID_BMA150
+#define ACCEL_BMA150_RANGE              (2.f * GRAVITY_EARTH)
+#define ACCEL_BMA150_RESOLUTION         (0.004f * GRAVITY_EARTH)
+#define ACCEL_BMA150_POWER              (0.2f)
+//ACCEL_ID_BMA222
+#define ACCEL_BMA222_RANGE              (2.f * GRAVITY_EARTH)
+#define ACCEL_BMA222_RESOLUTION         (0.001f * GRAVITY_EARTH)
+#define ACCEL_BMA222_POWER              (0.1f)
+//ACCEL_ID_BMA250
+#define ACCEL_BMA250_RANGE              (2.f * GRAVITY_EARTH)
+#define ACCEL_BMA250_RESOLUTION         (0.00391f * GRAVITY_EARTH)
+#define ACCEL_BMA250_POWER              (0.139f)
+#define ACCEL_BMA250_MINDELAY           (1000)
+//ACCEL_ID_ADXL34X
+#define ACCEL_ADXL34X_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_ADXL34X_RESOLUTION        (0.001f * GRAVITY_EARTH)
+#define ACCEL_ADXL34X_POWER             (1.f)
+//ACCEL_ID_MMA8450
+#define ACCEL_MMA8450_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MMA8450_RESOLUTION        (0.001f * GRAVITY_EARTH)
+#define ACCEL_MMA8450_POWER             (1.0f)
+//ACCEL_ID_MMA845X
+#define ACCEL_MMA845X_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MMA845X_RESOLUTION        (0.001f * GRAVITY_EARTH)
+#define ACCEL_MMA845X_POWER             (1.f)
+//ACCEL_ID_MPU6050
+#define ACCEL_MPU6050_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MPU6050_RESOLUTION        (0.004f * GRAVITY_EARTH)
+#define ACCEL_MPU6050_POWER             (5.5f)
+#define ACCEL_MPU6050_MINDELAY          (1000)
+/******************************************/
+//GYRO MPU3050
+#define RAD_P_DEG                       (3.14159f / 180.f)
+#define GYRO_MPU3050_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_MPU3050_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_MPU3050_POWER              (6.1f)
+#define GYRO_MPU3050_MINDELAY           (1000)
+//GYRO MPU6050
+#define GYRO_MPU6050_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_MPU6050_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_MPU6050_POWER              (5.5f)
+#define GYRO_MPU6050_MINDELAY           (1000)
+//GYRO MPU9150
+#define GYRO_MPU9150_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_MPU9150_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_MPU9150_POWER              (5.5f)
+#define GYRO_MPU9150_MINDELAY           (1000)
+//GYRO MPU9350
+#define GYRO_MPU9350_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_MPU9350_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_MPU9350_POWER              (5.5f)
+#define GYRO_MPU9350_MINDELAY           (1000)
+//GYRO MPU9250
+#define GYRO_MPU9250_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_MPU9250_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_MPU9250_POWER              (5.5f)
+#define GYRO_MPU9250_MINDELAY           (5000)
+//GYRO MPU6500
+#define GYRO_MPU6500_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_MPU6500_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_MPU6500_POWER              (5.5f)
+#define GYRO_MPU6500_MINDELAY           (5000)
+//GYRO ITG3500
+#define GYRO_ITG3500_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_ITG3500_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_ITG3500_POWER              (5.5f)
+#define GYRO_ITG3500_MINDELAY           (1000)
+/******************************************/
+//PRESSURE BMP280
+#define PRESSURE_BMP280_RANGE           (1100.f)   // hpa
+#define PRESSURE_BMP280_RESOLUTION      (0.009995f)// in psi
+#define PRESSURE_BMP280_POWER           (0.004f)   // 0.004mA
+#define PRESSURE_BMP280_MINDELAY        (33333)    // 30Hz unit in ns 
+#endif  /* INV_SENSOR_PARAMS_H */
+
diff --git a/6515/libsensors_iio/sensors.h b/6515/libsensors_iio/sensors.h
new file mode 100644
index 0000000..9920daf
--- /dev/null
+++ b/6515/libsensors_iio/sensors.h
@@ -0,0 +1,288 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef ANDROID_SENSORS_H
+#define ANDROID_SENSORS_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#include <linux/input.h>
+
+#include <hardware/hardware.h>
+#include <hardware/sensors.h>
+
+__BEGIN_DECLS
+
+/*****************************************************************************/
+
+#ifndef ARRAY_SIZE
+#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
+#endif
+
+#define ENABLE_PRESSURE
+
+enum {
+    ID_GY = 0,
+    ID_RG,
+    ID_A,
+    ID_M,
+    ID_RM,
+    ID_PS,
+    ID_O,
+    ID_RV,
+    ID_GRV,
+    ID_LA,
+    ID_GR,
+    ID_SM,
+    ID_P,
+    ID_SC,
+    ID_GMRV,
+    ID_SO
+};
+
+enum {
+    Gyro = 0,
+    RawGyro,
+    Accelerometer,
+    MagneticField,
+    RawMagneticField,
+    Pressure,
+    Orientation,
+    RotationVector,
+    GameRotationVector,
+    LinearAccel,
+    Gravity,
+    SignificantMotion,
+    StepDetector,
+    StepCounter,
+    GeomagneticRotationVector,
+    NumSensors
+};
+
+/* Physical parameters of the sensors supported by Invensense MPL */
+#define SENSORS_GYROSCOPE_HANDLE                   (ID_GY)
+#define SENSORS_RAW_GYROSCOPE_HANDLE               (ID_RG)
+#define SENSORS_ACCELERATION_HANDLE                (ID_A)
+#define SENSORS_MAGNETIC_FIELD_HANDLE              (ID_M)
+#define SENSORS_RAW_MAGNETIC_FIELD_HANDLE          (ID_RM)
+#define SENSORS_PRESSURE_HANDLE                    (ID_PS)
+#define SENSORS_ORIENTATION_HANDLE                 (ID_O)
+#define SENSORS_ROTATION_VECTOR_HANDLE             (ID_RV)
+#define SENSORS_GAME_ROTATION_VECTOR_HANDLE        (ID_GRV)
+#define SENSORS_LINEAR_ACCEL_HANDLE                (ID_LA)
+#define SENSORS_GRAVITY_HANDLE                     (ID_GR)
+#define SENSORS_SIGNIFICANT_MOTION_HANDLE          (ID_SM)
+#define SENSORS_PEDOMETER_HANDLE                   (ID_P)
+#define SENSORS_STEP_COUNTER_HANDLE                (ID_SC)
+#define SENSORS_GEOMAGNETIC_ROTATION_VECTOR_HANDLE (ID_GMRV)
+#define SENSORS_SCREEN_ORIENTATION_HANDLE          (ID_SO)
+
+/*****************************************************************************/
+
+/*
+   Android KitKat 
+   Populate sensor_t structure according to hardware sensors.h
+   {    name, vendor, version, handle, type, maxRange, resolution, power, minDelay,
+    fifoReservedEventCount, fifoMaxEventCount, reserved[]    }
+*/
+#if defined ANDROID_KITKAT
+static struct sensor_t sBaseSensorList[] =
+{
+    {"MPL Gyroscope", "Invensense", 1,
+     SENSORS_GYROSCOPE_HANDLE,
+     SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 128, {}},
+    {"MPL Raw Gyroscope", "Invensense", 1,
+     SENSORS_RAW_GYROSCOPE_HANDLE,
+     SENSOR_TYPE_GYROSCOPE_UNCALIBRATED, 2000.0f, 1.0f, 0.5f, 10000, 0, 128, {}},
+    {"MPL Accelerometer", "Invensense", 1,
+     SENSORS_ACCELERATION_HANDLE,
+     SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 128, {}},
+    {"MPL Magnetic Field", "Invensense", 1,
+     SENSORS_MAGNETIC_FIELD_HANDLE,
+     SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 128, {}},
+    {"MPL Raw Magnetic Field", "Invensense", 1,
+     SENSORS_RAW_MAGNETIC_FIELD_HANDLE,
+     SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED, 10240.0f, 1.0f, 0.5f, 10000, 0, 128, {}},
+#ifdef ENABLE_PRESSURE
+    {"MPL Pressure", "Invensense", 1,
+     SENSORS_PRESSURE_HANDLE,
+     SENSOR_TYPE_PRESSURE, 10240.0f, 1.0f, 0.5f, 10000, 0, 128, {}},
+#endif
+    {"MPL Orientation", "Invensense", 1,
+     SENSORS_ORIENTATION_HANDLE,
+     SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, {}},
+    {"MPL Rotation Vector", "Invensense", 1,
+     SENSORS_ROTATION_VECTOR_HANDLE,
+     SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
+    {"MPL Game Rotation Vector", "Invensense", 1,
+     SENSORS_GAME_ROTATION_VECTOR_HANDLE,
+     SENSOR_TYPE_GAME_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 64, {}},
+    {"MPL Linear Acceleration", "Invensense", 1,
+     SENSORS_LINEAR_ACCEL_HANDLE,
+     SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
+    {"MPL Gravity", "Invensense", 1,
+     SENSORS_GRAVITY_HANDLE,
+     SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
+    {"MPL Significant Motion", "Invensense", 1,
+     SENSORS_SIGNIFICANT_MOTION_HANDLE,
+     SENSOR_TYPE_SIGNIFICANT_MOTION, 100.0f, 1.0f, 1.1f, 0, 0, 0, {}},
+    {"MPL Step Detector", "Invensense", 1,
+     SENSORS_PEDOMETER_HANDLE,
+     SENSOR_TYPE_STEP_DETECTOR, 100.0f, 1.0f, 1.1f, 0, 0, 128, {}},
+    {"MPL Step Counter", "Invensense", 1,
+     SENSORS_STEP_COUNTER_HANDLE,
+     SENSOR_TYPE_STEP_COUNTER, 100.0f, 1.0f, 1.1f, 0, 0, 0, {}},
+    {"MPL Geomagnetic Rotation Vector", "Invensense", 1,
+     SENSORS_GEOMAGNETIC_ROTATION_VECTOR_HANDLE,
+     SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 5000, 0, 0, {}},
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+    {"MPL Screen Orientation", "Invensense ", 1,
+     SENSORS_SCREEN_ORIENTATION_HANDLE,
+     SENSOR_TYPE_SCREEN_ORIENTATION, 100.0f, 1.0f, 1.1f, 0, 0, 0, {}},
+#endif
+};
+#else  //ANDROID_KITKAT END
+static struct sensor_t sBaseSensorList[] =
+{
+    {"MPL Gyroscope", "Invensense", 1,
+     SENSORS_GYROSCOPE_HANDLE,
+     SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, {}},
+    {"MPL Raw Gyroscope", "Invensense", 1,
+     SENSORS_RAW_GYROSCOPE_HANDLE,
+     SENSOR_TYPE_GYROSCOPE_UNCALIBRATED, 2000.0f, 1.0f, 0.5f, 10000, {}},
+    {"MPL Accelerometer", "Invensense", 1,
+     SENSORS_ACCELERATION_HANDLE,
+     SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, {}},
+    {"MPL Magnetic Field", "Invensense", 1,
+     SENSORS_MAGNETIC_FIELD_HANDLE,
+     SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, {}},
+    {"MPL Raw Magnetic Field", "Invensense", 1,
+     SENSORS_RAW_MAGNETIC_FIELD_HANDLE,
+     SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED, 10240.0f, 1.0f, 0.5f, 10000, {}},
+#ifdef ENABLE_PRESSURE
+    {"MPL Pressure", "Invensense", 1,
+     SENSORS_PRESSURE_HANDLE,
+     SENSOR_TYPE_PRESSURE, 10240.0f, 1.0f, 0.5f, 10000, {}},
+#endif
+    {"MPL Orientation", "Invensense", 1,
+     SENSORS_ORIENTATION_HANDLE,
+     SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, {}},
+    {"MPL Rotation Vector", "Invensense", 1,
+     SENSORS_ROTATION_VECTOR_HANDLE,
+     SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, {}},
+    {"MPL Game Rotation Vector", "Invensense", 1,
+     SENSORS_GAME_ROTATION_VECTOR_HANDLE,
+     SENSOR_TYPE_GAME_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, {}},
+    {"MPL Linear Acceleration", "Invensense", 1,
+     SENSORS_LINEAR_ACCEL_HANDLE,
+     SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, {}},
+    {"MPL Gravity", "Invensense", 1,
+     SENSORS_GRAVITY_HANDLE,
+     SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, {}},
+    {"MPL Significant Motion", "Invensense", 1,
+     SENSORS_SIGNIFICANT_MOTION_HANDLE,
+     SENSOR_TYPE_SIGNIFICANT_MOTION, 100.0f, 1.0f, 1.1f, 0, {}},
+    {"MPL Step Detector", "Invensense", 1,
+     SENSORS_PEDOMETER_HANDLE,
+     SENSOR_TYPE_STEP_DETECTOR, 100.0f, 1.0f, 1.1f, 0, {}},
+    {"MPL Step Counter", "Invensense", 1,
+     SENSORS_STEP_COUNTER_HANDLE,
+     SENSOR_TYPE_STEP_COUNTER, 100.0f, 1.0f, 1.1f, 0, {}},
+    {"MPL Geomagnetic Rotation Vector", "Invensense", 1,
+     SENSORS_GEOMAGNETIC_ROTATION_VECTOR_HANDLE,
+     SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, {}},
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+    {"MPL Screen Orientation", "Invensense ", 1,
+     SENSORS_SCREEN_ORIENTATION_HANDLE,
+     SENSOR_TYPE_SCREEN_ORIENTATION, 100.0f, 1.0f, 1.1f, 0, {}},
+#endif
+};
+#endif //ANDROID_JELLYBEAN END
+
+/*****************************************************************************/
+
+/*
+ * The SENSORS Module
+ */
+
+/* ITG3500 */
+#define EVENT_TYPE_GYRO_X          REL_X
+#define EVENT_TYPE_GYRO_Y          REL_Y
+#define EVENT_TYPE_GYRO_Z          REL_Z
+/* MPU6050 MPU9150 */
+#define EVENT_TYPE_IACCEL_X        REL_RX
+#define EVENT_TYPE_IACCEL_Y        REL_RY
+#define EVENT_TYPE_IACCEL_Z        REL_RZ
+/* MPU6050 MPU9150 */
+#define EVENT_TYPE_ICOMPASS_X      REL_X
+#define EVENT_TYPE_ICOMPASS_Y      REL_Y
+#define EVENT_TYPE_ICOMPASS_Z      REL_Z
+/* MPUxxxx */
+#define EVENT_TYPE_TIMESTAMP_HI    REL_MISC
+#define EVENT_TYPE_TIMESTAMP_LO    REL_WHEEL
+
+/* Accel BMA250 */
+#define EVENT_TYPE_ACCEL_X          ABS_X
+#define EVENT_TYPE_ACCEL_Y          ABS_Y
+#define EVENT_TYPE_ACCEL_Z          ABS_Z
+#define LSG                         (1000.0f)
+
+// conversion of acceleration data to SI units (m/s^2)
+#define RANGE_A                     (4*GRAVITY_EARTH)
+#define RESOLUTION_A                (GRAVITY_EARTH / LSG)
+#define CONVERT_A                   (GRAVITY_EARTH / LSG)
+#define CONVERT_A_X                 (CONVERT_A)
+#define CONVERT_A_Y                 (CONVERT_A)
+#define CONVERT_A_Z                 (CONVERT_A)
+
+/* AKM  compasses */
+#define EVENT_TYPE_MAGV_X           ABS_RX
+#define EVENT_TYPE_MAGV_Y           ABS_RY
+#define EVENT_TYPE_MAGV_Z           ABS_RZ
+#define EVENT_TYPE_MAGV_STATUS      ABS_RUDDER
+
+/* conversion of magnetic data to uT units */
+#define CONVERT_M                   (0.06f)
+
+/* conversion of sensor rates */
+#define hertz_request = 200;
+#define DEFAULT_MPL_GYRO_RATE           (20000L)     //us
+#define DEFAULT_MPL_COMPASS_RATE        (20000L)     //us
+
+#define DEFAULT_HW_GYRO_RATE            (100)        //Hz
+#define DEFAULT_HW_ACCEL_RATE           (20)         //ms
+#define DEFAULT_HW_COMPASS_RATE         (20000000L)  //ns
+#define DEFAULT_HW_AKMD_COMPASS_RATE    (200000000L) //ns
+
+/* convert ns to hardware units */
+#define HW_GYRO_RATE_NS                 (1000000000LL / rate_request) // to Hz
+#define HW_ACCEL_RATE_NS                (rate_request / (1000000L))   // to ms
+#define HW_COMPASS_RATE_NS              (rate_request)                // to ns
+
+/* convert Hz to hardware units */
+#define HW_GYRO_RATE_HZ                 (hertz_request)
+#define HW_ACCEL_RATE_HZ                (1000 / hertz_request)
+#define HW_COMPASS_RATE_HZ              (1000000000LL / hertz_request)
+
+#define RATE_200HZ                      5000000LL
+#define RATE_15HZ                       66667000LL
+#define RATE_5HZ                        200000000LL
+__END_DECLS
+
+#endif  // ANDROID_SENSORS_H
diff --git a/6515/libsensors_iio/sensors_mpl.cpp b/6515/libsensors_iio/sensors_mpl.cpp
new file mode 100644
index 0000000..97a4009
--- /dev/null
+++ b/6515/libsensors_iio/sensors_mpl.cpp
@@ -0,0 +1,412 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#define FUNC_LOG LOGV("%s", __PRETTY_FUNCTION__)
+
+#include <hardware/sensors.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <dirent.h>
+#include <math.h>
+#include <poll.h>
+#include <pthread.h>
+#include <stdlib.h>
+
+#include <linux/input.h>
+
+#include <utils/Atomic.h>
+#include <utils/Log.h>
+
+#include "sensors.h"
+#include "MPLSensor.h"
+
+/* 
+ * Vendor-defined Accel Load Calibration File Method 
+ * @param[out] Accel bias, length 3.  In HW units scaled by 2^16 in body frame
+ * @return '0' for a successful load, '1' otherwise
+ * example: int AccelLoadConfig(long* offset);
+ * End of Vendor-defined Accel Load Cal Method 
+ */
+
+/*****************************************************************************/
+/* The SENSORS Module */
+
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+#define LOCAL_SENSORS (NumSensors + 1)
+#else
+#define LOCAL_SENSORS (NumSensors)
+#endif
+
+static struct sensor_t sSensorList[LOCAL_SENSORS];
+static int sensors = (sizeof(sSensorList) / sizeof(sensor_t));
+
+static int open_sensors(const struct hw_module_t* module, const char* id,
+                        struct hw_device_t** device);
+
+static int sensors__get_sensors_list(struct sensors_module_t* module,
+                                     struct sensor_t const** list)
+{
+    *list = sSensorList;
+    return sensors;
+}
+
+static struct hw_module_methods_t sensors_module_methods = {
+        open: open_sensors
+};
+
+struct sensors_module_t HAL_MODULE_INFO_SYM = {
+        common: {
+                tag: HARDWARE_MODULE_TAG,
+                version_major: 1,
+                version_minor: 0,
+                id: SENSORS_HARDWARE_MODULE_ID,
+                name: "Invensense module",
+                author: "Invensense Inc.",
+                methods: &sensors_module_methods,
+                dso: NULL,
+                reserved: {0}
+        },
+        get_sensors_list: sensors__get_sensors_list,
+};
+
+struct sensors_poll_context_t {
+    sensors_poll_device_1_t device; // must be first
+
+    sensors_poll_context_t();
+    ~sensors_poll_context_t();
+    int activate(int handle, int enabled);
+    int setDelay(int handle, int64_t ns);
+    int pollEvents(sensors_event_t* data, int count);
+    int query(int what, int *value);
+    int batch(int handle, int flags, int64_t period_ns, int64_t timeout);
+#if defined ANDROID_KITKAT
+    int flush(int handle);
+#endif
+
+private:
+    enum {
+        mpl = 0,
+        compass,
+        dmpOrient,
+        dmpSign,
+        dmpPed,
+        numSensorDrivers,   // wake pipe goes here
+        numFds,
+    };
+
+    struct pollfd mPollFds[numFds];
+    SensorBase *mSensor;
+    CompassSensor *mCompassSensor;
+
+    static const size_t wake = numSensorDrivers;
+    static const char WAKE_MESSAGE = 'W';
+    int mWritePipeFd;
+};
+
+/******************************************************************************/
+
+sensors_poll_context_t::sensors_poll_context_t() {
+    VFUNC_LOG;
+
+    /* TODO: Handle external pressure sensor */
+    mCompassSensor = new CompassSensor();
+    MPLSensor *mplSensor = new MPLSensor(mCompassSensor);
+
+   /* For Vendor-defined Accel Calibration File Load
+    * Use the Following Constructor and Pass Your Load Cal File Function
+    * 
+    * MPLSensor *mplSensor = new MPLSensor(mCompassSensor, AccelLoadConfig);
+    */
+
+    // populate the sensor list
+    sensors =
+            mplSensor->populateSensorList(sSensorList, sizeof(sSensorList));
+
+    mSensor = mplSensor;
+    mPollFds[mpl].fd = mSensor->getFd();
+    mPollFds[mpl].events = POLLIN;
+    mPollFds[mpl].revents = 0;
+
+    mPollFds[compass].fd = mCompassSensor->getFd();
+    mPollFds[compass].events = POLLIN;
+    mPollFds[compass].revents = 0;
+
+    mPollFds[dmpOrient].fd = ((MPLSensor*) mSensor)->getDmpOrientFd();
+    mPollFds[dmpOrient].events = POLLPRI;
+    mPollFds[dmpOrient].revents = 0;
+
+    mPollFds[dmpSign].fd = ((MPLSensor*) mSensor)->getDmpSignificantMotionFd();
+    mPollFds[dmpSign].events = POLLPRI;
+    mPollFds[dmpSign].revents = 0;
+
+    mPollFds[dmpPed].fd = ((MPLSensor*) mSensor)->getDmpPedometerFd();
+    mPollFds[dmpPed].events = POLLPRI;
+    mPollFds[dmpPed].revents = 0;
+
+    /* Timer based sensor initialization */
+    int wakeFds[2];
+    int result = pipe(wakeFds);
+    LOGE_IF(result<0, "error creating wake pipe (%s)", strerror(errno));
+    fcntl(wakeFds[0], F_SETFL, O_NONBLOCK);
+    fcntl(wakeFds[1], F_SETFL, O_NONBLOCK);
+    mWritePipeFd = wakeFds[1];
+
+    mPollFds[numSensorDrivers].fd = wakeFds[0];
+    mPollFds[numSensorDrivers].events = POLLIN;
+    mPollFds[numSensorDrivers].revents = 0;
+}
+
+sensors_poll_context_t::~sensors_poll_context_t() {
+    FUNC_LOG;
+    delete mSensor;
+    delete mCompassSensor;
+    for (int i = 0; i < numSensorDrivers; i++) {
+        close(mPollFds[i].fd);
+    }
+    close(mWritePipeFd);
+}
+
+int sensors_poll_context_t::activate(int handle, int enabled) {
+    FUNC_LOG;
+
+    int err;
+    err = mSensor->enable(handle, enabled);
+    if (!err) {
+        const char wakeMessage(WAKE_MESSAGE);
+        int result = write(mWritePipeFd, &wakeMessage, 1);
+        LOGE_IF(result < 0, 
+                "error sending wake message (%s)", strerror(errno));
+    }
+    return err;
+}
+
+int sensors_poll_context_t::setDelay(int handle, int64_t ns)
+{
+    FUNC_LOG;
+    return mSensor->setDelay(handle, ns);
+}
+
+int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count)
+{
+    VHANDLER_LOG;
+
+    int nbEvents = 0;
+    int nb, polltime = -1;
+
+    polltime = ((MPLSensor*) mSensor)->getStepCountPollTime();
+
+    // look for new events
+    nb = poll(mPollFds, numSensorDrivers, polltime);
+    LOGI_IF(0, "poll nb=%d, count=%d, pt=%d", nb, count, polltime);
+    if (nb > 0) {
+        for (int i = 0; count && i < numSensorDrivers; i++) {
+            if (mPollFds[i].revents & (POLLIN | POLLPRI)) {
+                nb = 0;
+                if (i == mpl) {
+                    ((MPLSensor*) mSensor)->buildMpuEvent();
+                    mPollFds[i].revents = 0;
+                } else if (i == compass) {
+                    ((MPLSensor*) mSensor)->buildCompassEvent();
+                    mPollFds[i].revents = 0;
+                } else if (i == dmpOrient) {
+                    nb = ((MPLSensor*)mSensor)->
+                                        readDmpOrientEvents(data, count);
+                    mPollFds[dmpOrient].revents= 0;
+                    if (isDmpScreenAutoRotationEnabled() && nb > 0) {
+                        count -= nb;
+                        nbEvents += nb;
+                        data += nb;
+                    }
+                } else if (i == dmpSign) {
+                    nb = ((MPLSensor*) mSensor)->
+                                    readDmpSignificantMotionEvents(data, count);
+                    mPollFds[i].revents = 0;
+                    count -= nb;
+                    nbEvents += nb;
+                    data += nb; 
+                } else if (i == dmpPed) {
+                    nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents(
+                            data, count, ID_P, 0);
+                    mPollFds[i].revents = 0;
+                    count -= nb;
+                    nbEvents += nb;
+                    data += nb;
+                }
+                if(nb == 0) {
+                    nb = ((MPLSensor*) mSensor)->readEvents(data, count);
+                    LOGI_IF(0, "sensors_mpl:readEvents() - "
+                            "i=%d, nb=%d, count=%d, nbEvents=%d, "
+                            "data->timestamp=%lld, data->data[0]=%f,",
+                            i, nb, count, nbEvents, data->timestamp, 
+                            data->data[0]);
+                    if (nb > 0) {
+                        count -= nb;
+                        nbEvents += nb;
+                        data += nb;
+                    }
+                }
+            }
+        }
+
+        /* to see if any step counter events */
+        if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) {
+            nb = 0;
+            nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents(
+                            data, count, ID_SC, 0);
+            LOGI_IF(SensorBase::HANDLER_DATA, "sensors_mpl:readStepCount() - "
+                    "nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, ",
+                    nb, count, nbEvents, data->timestamp);
+            if (nb > 0) {
+                count -= nb;
+                nbEvents += nb;
+                data += nb;
+            }
+        }
+    } else if(nb == 0) {
+        /* to see if any step counter events */
+        if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) {
+            nb = 0;
+            nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents(
+                            data, count, ID_SC, 0);
+            LOGI_IF(SensorBase::HANDLER_DATA, "sensors_mpl:readStepCount() - "
+                    "nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, ",
+                    nb, count, nbEvents, data->timestamp);
+            if (nb > 0) {
+                count -= nb;
+                nbEvents += nb;
+                data += nb;
+            }
+        }
+
+        if (mPollFds[numSensorDrivers].revents & POLLIN) {
+            char msg;
+            int result = read(mPollFds[numSensorDrivers].fd, &msg, 1);
+            LOGE_IF(result < 0, 
+                    "error reading from wake pipe (%s)", strerror(errno));
+            mPollFds[numSensorDrivers].revents = 0;
+        }
+    }
+    return nbEvents;
+}
+
+int sensors_poll_context_t::query(int what, int* value)
+{
+    FUNC_LOG;
+    return mSensor->query(what, value);
+}
+
+int sensors_poll_context_t::batch(int handle, int flags, int64_t period_ns, 
+                                  int64_t timeout)
+{
+    FUNC_LOG;
+    return mSensor->batch(handle, flags, period_ns, timeout);
+}
+
+#if defined ANDROID_KITKAT
+int sensors_poll_context_t::flush(int handle)
+{
+    FUNC_LOG;
+    return mSensor->flush(handle);
+}
+#endif
+
+/******************************************************************************/
+
+static int poll__close(struct hw_device_t *dev)
+{
+    FUNC_LOG;
+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+    if (ctx) {
+        delete ctx;
+    }
+    return 0;
+}
+
+static int poll__activate(struct sensors_poll_device_t *dev,
+                          int handle, int enabled)
+{
+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+    return ctx->activate(handle, enabled);
+}
+
+static int poll__setDelay(struct sensors_poll_device_t *dev,
+                          int handle, int64_t ns)
+{
+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+    int s= ctx->setDelay(handle, ns);
+    return s;
+}
+
+static int poll__poll(struct sensors_poll_device_t *dev,
+                      sensors_event_t* data, int count)
+{
+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+    return ctx->pollEvents(data, count);
+}
+
+static int poll__query(struct sensors_poll_device_1 *dev,
+                      int what, int *value)
+{
+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+    return ctx->query(what, value);
+}
+
+static int poll__batch(struct sensors_poll_device_1 *dev,
+                      int handle, int flags, int64_t period_ns, int64_t timeout)
+{
+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+    return ctx->batch(handle, flags, period_ns, timeout);
+}
+
+#if defined ANDROID_KITKAT
+static int poll__flush(struct sensors_poll_device_1 *dev,
+                      int handle)
+{
+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+    return ctx->flush(handle);
+}
+#endif
+/******************************************************************************/
+
+/** Open a new instance of a sensor device using name */
+static int open_sensors(const struct hw_module_t* module, const char* id,
+                        struct hw_device_t** device)
+{
+    FUNC_LOG;
+    int status = -EINVAL;
+    sensors_poll_context_t *dev = new sensors_poll_context_t();
+
+    memset(&dev->device, 0, sizeof(sensors_poll_device_1));
+
+    dev->device.common.tag = HARDWARE_DEVICE_TAG;
+#if defined ANDROID_KITKAT
+    dev->device.common.version  = SENSORS_DEVICE_API_VERSION_1_1;
+    dev->device.flush           = poll__flush;
+#else
+    dev->device.common.version  = SENSORS_DEVICE_API_VERSION_1_0;
+#endif
+    dev->device.common.module   = const_cast<hw_module_t*>(module);
+    dev->device.common.close    = poll__close;
+    dev->device.activate        = poll__activate;
+    dev->device.setDelay        = poll__setDelay;
+    dev->device.poll            = poll__poll;
+    dev->device.batch           = poll__batch; 
+
+    *device = &dev->device.common;
+    status = 0;
+
+    return status;
+}
diff --git a/6515/libsensors_iio/software/build/android/common.mk b/6515/libsensors_iio/software/build/android/common.mk
new file mode 100644
index 0000000..acd3deb
--- /dev/null
+++ b/6515/libsensors_iio/software/build/android/common.mk
@@ -0,0 +1,87 @@
+# Use bash for additional echo fancyness
+SHELL = /bin/bash
+
+####################################################################################################
+## defines
+
+# Build for Jellybean 
+BUILD_ANDROID_JELLYBEAN = $(shell test -d $(ANDROID_ROOT)/frameworks/native && echo 1)
+
+## libraries ##
+LIB_PREFIX = lib
+
+STATIC_LIB_EXT = a
+SHARED_LIB_EXT = so
+
+# normally, overridden from outside 
+# ?= assignment sets it only if not already defined
+TARGET ?= android
+
+MLLITE_LIB_NAME     ?= mllite
+MPL_LIB_NAME        ?= mplmpu
+
+## applications ##
+SHARED_APP_SUFFIX = -shared
+STATIC_APP_SUFFIX = -static
+
+####################################################################################################
+## compile, includes, and linker
+
+ifeq ($(BUILD_ANDROID_JELLYBEAN),1)
+ANDROID_COMPILE = -DANDROID_JELLYBEAN=1
+endif
+
+ANDROID_LINK  = -nostdlib
+ANDROID_LINK += -fpic
+ANDROID_LINK += -Wl,--gc-sections 
+ANDROID_LINK += -Wl,--no-whole-archive 
+ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib
+ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+ANDROID_LINK_EXECUTABLE  = $(ANDROID_LINK)
+ANDROID_LINK_EXECUTABLE += -Wl,-dynamic-linker,/system/bin/linker
+ifneq ($(BUILD_ANDROID_JELLYBEAN),1)
+ANDROID_LINK_EXECUTABLE += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x
+endif
+ANDROID_LINK_EXECUTABLE += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o
+ANDROID_LINK_EXECUTABLE += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o
+
+ANDROID_INCLUDES  = -I$(ANDROID_ROOT)/system/core/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/libhardware/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/ril/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/dalvik/libnativehelper/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/base/include   # ICS
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/native/include # Jellybean
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/external/skia/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/arch-arm/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libstdc++/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/kernel/common
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/kernel/arch-arm
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/include/arch/arm
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libthread_db/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/arm
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/SHARED_LIBRARIES/libm_intermediates
+
+KERNEL_INCLUDES  = -I$(KERNEL_ROOT)/include
+
+INV_INCLUDES  = -I$(INV_ROOT)/software/core/driver/include
+INV_INCLUDES += -I$(MLLITE_DIR)
+INV_INCLUDES += -I$(MLLITE_DIR)/linux
+
+INV_DEFINES += -DINV_CACHE_DMP=1
+
+####################################################################################################
+## macros
+
+ifndef echo_in_colors
+define echo_in_colors
+	echo -ne "\e[1;32m"$(1)"\e[0m"
+endef 
+endif
+
+
+
diff --git a/6515/libsensors_iio/software/build/android/shared.mk b/6515/libsensors_iio/software/build/android/shared.mk
new file mode 100644
index 0000000..ac225cb
--- /dev/null
+++ b/6515/libsensors_iio/software/build/android/shared.mk
@@ -0,0 +1,76 @@
+SHELL=/bin/bash
+
+TARGET       ?= android
+PRODUCT      ?= beagleboard
+ANDROID_ROOT ?= /Android/trunk/0xdroid/beagle-eclair
+KERNEL_ROOT  ?= /Android/trunk/0xdroid/kernel
+MLSDK_ROOT   ?= $(CURDIR)
+
+ifeq ($(VERBOSE),1)
+	DUMP=1>/dev/stdout
+else
+	DUMP=1>/dev/null
+endif
+
+include common.mk
+
+################################################################################
+## targets
+
+INV_ROOT = ../..
+LIB_FOLDERS  = $(INV_ROOT)/core/mllite/build/$(TARGET)
+ifeq ($(BUILD_MPL),1)
+	LIB_FOLDERS += $(INV_ROOT)/core/mpl/build/$(TARGET)
+endif
+APP_FOLDERS  = $(INV_ROOT)/simple_apps/mpu_iio/build/$(TARGET)
+APP_FOLDERS += $(INV_ROOT)/simple_apps/self_test/build/$(TARGET)
+APP_FOLDERS += $(INV_ROOT)/simple_apps/gesture_test/build/$(TARGET)
+APP_FOLDERS += $(INV_ROOT)/simple_apps/playback/linux/build/$(TARGET)
+APP_FOLDERS += $(INV_ROOT)/simple_apps/devnode_parser/build/$(TARGET)
+
+INSTALL_DIR = $(CURDIR)
+
+################################################################################
+## macros
+
+define echo_in_colors
+	echo -ne "\e[1;34m"$(1)"\e[0m"
+endef
+
+################################################################################
+## rules
+
+.PHONY : all mllite mpl clean
+
+all:
+	for DIR in $(LIB_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+	for DIR in $(APP_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+
+clean: 
+	for DIR in $(LIB_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+	for DIR in $(APP_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+
+cleanall:
+	for DIR in $(LIB_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+	for DIR in $(APP_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+
+install:
+	for DIR in $(LIB_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+	for DIR in $(APP_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+
diff --git a/6515/libsensors_iio/software/core/driver/include/log.h b/6515/libsensors_iio/software/core/driver/include/log.h
new file mode 100644
index 0000000..626b00e
--- /dev/null
+++ b/6515/libsensors_iio/software/core/driver/include/log.h
@@ -0,0 +1,368 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*
+ * This file incorporates work covered by the following copyright and
+ * permission notice:
+ *
+ * Copyright (C) 2005 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ * C/C++ logging functions.  See the logging documentation for API details.
+ *
+ * We'd like these to be available from C code (in case we import some from
+ * somewhere), so this has a C interface.
+ *
+ * The output will be correct when the log file is shared between multiple
+ * threads and/or multiple processes so long as the operating system
+ * supports O_APPEND.  These calls have mutex-protected data structures
+ * and so are NOT reentrant.  Do not use MPL_LOG in a signal handler.
+ */
+#ifndef _LIBS_CUTILS_MPL_LOG_H
+#define _LIBS_CUTILS_MPL_LOG_H
+
+#include <stdlib.h>
+#include <stdarg.h>
+
+#ifdef ANDROID
+#ifdef NDK_BUILD
+#include "log_macros.h"
+#else
+#include <utils/Log.h>		/* For the LOG macro */
+#endif
+#endif
+
+#ifdef __KERNEL__
+#include <linux/kernel.h>
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if defined ANDROID_JELLYBEAN || defined ANDROID_KITKAT
+#define LOG ALOG
+#define LOG_ERRROR ANDROID_LOG_ERROR
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Normally we strip MPL_LOGV (VERBOSE messages) from release builds.
+ * You can modify this (for example with "#define MPL_LOG_NDEBUG 0"
+ * at the top of your source file) to change that behavior.
+ */
+#ifndef MPL_LOG_NDEBUG
+#ifdef NDEBUG
+#define MPL_LOG_NDEBUG 1
+#else
+#define MPL_LOG_NDEBUG 0
+#endif
+#endif
+
+#ifdef __KERNEL__
+#define MPL_LOG_UNKNOWN MPL_LOG_VERBOSE
+#define MPL_LOG_DEFAULT KERN_DEFAULT
+#define MPL_LOG_VERBOSE KERN_CONT
+#define MPL_LOG_DEBUG   KERN_NOTICE
+#define MPL_LOG_INFO    KERN_INFO
+#define MPL_LOG_WARN    KERN_WARNING
+#define MPL_LOG_ERROR   KERN_ERR
+#define MPL_LOG_SILENT  MPL_LOG_VERBOSE
+
+#else
+	/* Based off the log priorities in android
+	   /system/core/include/android/log.h */
+#define MPL_LOG_UNKNOWN		(0)
+#define MPL_LOG_DEFAULT		(1)
+#define MPL_LOG_VERBOSE		(2)
+#define MPL_LOG_DEBUG		(3)
+#define MPL_LOG_INFO		(4)
+#define MPL_LOG_WARN		(5)
+#define MPL_LOG_ERROR		(6)
+#define MPL_LOG_SILENT		(8)
+#endif
+
+
+/*
+ * This is the local tag used for the following simplified
+ * logging macros.  You can change this preprocessor definition
+ * before using the other macros to change the tag.
+ */
+#ifndef MPL_LOG_TAG
+#ifdef __KERNEL__
+#define MPL_LOG_TAG
+#else
+#define MPL_LOG_TAG NULL
+#endif
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Simplified macro to send a verbose log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGV
+#if MPL_LOG_NDEBUG
+#ifdef _WIN32
+#define MPL_LOGV(fmt, ...)						\
+	do {								\
+        __pragma (warning(suppress : 4127 )) \
+		if (0)							\
+			MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\
+            __pragma (warning(suppress : 4127 )) \
+    } while (0)
+#else
+#define MPL_LOGV(fmt, ...)						\
+	do {								\
+		if (0)							\
+			MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\
+    } while (0)
+#endif
+
+#else
+#define MPL_LOGV(fmt, ...) MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef CONDITION
+#define CONDITION(cond)     ((cond) != 0)
+#endif
+
+#ifndef MPL_LOGV_IF
+#if MPL_LOG_NDEBUG
+#define MPL_LOGV_IF(cond, fmt, ...)  \
+	do { if (0) MPL_LOG(fmt, ##__VA_ARGS__); } while (0)
+#else
+#define MPL_LOGV_IF(cond, fmt, ...) \
+	((CONDITION(cond))						\
+		? MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \
+		: (void)0)
+#endif
+#endif
+
+/*
+ * Simplified macro to send a debug log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGD
+#define MPL_LOGD(fmt, ...) MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+
+#ifndef MPL_LOGD_IF
+#define MPL_LOGD_IF(cond, fmt, ...) \
+	((CONDITION(cond))					       \
+		? MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__)  \
+		: (void)0)
+#endif
+
+/*
+ * Simplified macro to send an info log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGI
+#ifdef __KERNEL__
+#define MPL_LOGI(fmt, ...) pr_info(KERN_INFO MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOGI(fmt, ...) MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef MPL_LOGI_IF
+#define MPL_LOGI_IF(cond, fmt, ...) \
+	((CONDITION(cond))                                              \
+		? MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__)   \
+		: (void)0)
+#endif
+
+/*
+ * Simplified macro to send a warning log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGW
+#ifdef __KERNEL__
+#define MPL_LOGW(fmt, ...) printk(KERN_WARNING MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOGW(fmt, ...) MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef MPL_LOGW_IF
+#define MPL_LOGW_IF(cond, fmt, ...) \
+	((CONDITION(cond))					       \
+		? MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__)   \
+		: (void)0)
+#endif
+
+/*
+ * Simplified macro to send an error log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGE
+#ifdef __KERNEL__
+#define MPL_LOGE(fmt, ...) printk(KERN_ERR MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOGE(fmt, ...) MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef MPL_LOGE_IF
+#define MPL_LOGE_IF(cond, fmt, ...) \
+	((CONDITION(cond))					       \
+		? MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__)  \
+		: (void)0)
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Log a fatal error.  If the given condition fails, this stops program
+ * execution like a normal assertion, but also generating the given message.
+ * It is NOT stripped from release builds.  Note that the condition test
+ * is -inverted- from the normal assert() semantics.
+ */
+#define MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ...) \
+	((CONDITION(cond))					   \
+		? ((void)android_printAssert(#cond, MPL_LOG_TAG,   \
+						fmt, ##__VA_ARGS__))	\
+		: (void)0)
+
+#define MPL_LOG_ALWAYS_FATAL(fmt, ...) \
+	(((void)android_printAssert(NULL, MPL_LOG_TAG, fmt, ##__VA_ARGS__)))
+
+/*
+ * Versions of MPL_LOG_ALWAYS_FATAL_IF and MPL_LOG_ALWAYS_FATAL that
+ * are stripped out of release builds.
+ */
+#if MPL_LOG_NDEBUG
+#define MPL_LOG_FATAL_IF(cond, fmt, ...)				\
+	do {								\
+		if (0)							\
+			MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__); \
+	} while (0)
+#define MPL_LOG_FATAL(fmt, ...)						\
+	do {								\
+		if (0)							\
+			MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__)	\
+	} while (0)
+#else
+#define MPL_LOG_FATAL_IF(cond, fmt, ...) \
+	MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__)
+#define MPL_LOG_FATAL(fmt, ...) \
+	MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__)
+#endif
+
+/*
+ * Assertion that generates a log message when the assertion fails.
+ * Stripped out of release builds.  Uses the current MPL_LOG_TAG.
+ */
+#define MPL_LOG_ASSERT(cond, fmt, ...)			\
+	MPL_LOG_FATAL_IF(!(cond), fmt, ##__VA_ARGS__)
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Basic log message macro.
+ *
+ * Example:
+ *  MPL_LOG(MPL_LOG_WARN, NULL, "Failed with error %d", errno);
+ *
+ * The second argument may be NULL or "" to indicate the "global" tag.
+ */
+#ifndef MPL_LOG
+#define MPL_LOG(priority, tag, fmt, ...)		\
+	MPL_LOG_PRI(priority, tag, fmt, ##__VA_ARGS__)
+#endif
+
+/*
+ * Log macro that allows you to specify a number for the priority.
+ */
+#ifndef MPL_LOG_PRI
+#ifdef ANDROID
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+	LOG(priority, tag, fmt, ##__VA_ARGS__)
+#elif defined __KERNEL__
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+	pr_debug(MPL_##priority tag fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+	_MLPrintLog(MPL_##priority, tag, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+/*
+ * Log macro that allows you to pass in a varargs ("args" is a va_list).
+ */
+#ifndef MPL_LOG_PRI_VA
+#ifdef ANDROID
+#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \
+	android_vprintLog(priority, NULL, tag, fmt, args)
+#elif defined __KERNEL__
+/* not allowed in the Kernel because there is no dev_dbg that takes a va_list */
+#else
+#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \
+	_MLPrintVaLog(priority, NULL, tag, fmt, args)
+#endif
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * ===========================================================================
+ *
+ * The stuff in the rest of this file should not be used directly.
+ */
+
+#ifndef ANDROID
+int _MLPrintLog(int priority, const char *tag, const char *fmt,	...);
+int _MLPrintVaLog(int priority, const char *tag, const char *fmt, va_list args);
+/* Final implementation of actual writing to a character device */
+int _MLWriteLog(const char *buf, int buflen);
+#endif
+
+static inline void __print_result_location(int result,
+					   const char *file,
+					   const char *func, int line)
+{
+	MPL_LOGE("%s|%s|%d returning %d\n", file, func, line, result);
+}
+
+#ifdef _WIN32
+/* The pragma removes warning about expression being constant */
+#define LOG_RESULT_LOCATION(condition) \
+    do {								\
+		__print_result_location((int)(condition), __FILE__,	\
+					__func__, __LINE__);		\
+        __pragma (warning(suppress : 4127 )) \
+	} while (0)
+#else
+#define LOG_RESULT_LOCATION(condition) \
+    do {								\
+		__print_result_location((int)(condition), __FILE__,	\
+					__func__, __LINE__);		\
+	} while (0)
+#endif
+
+
+#define INV_ERROR_CHECK(r_1329) \
+    if (r_1329) { \
+        LOG_RESULT_LOCATION(r_1329); \
+        return r_1329; \
+    }
+
+#ifdef __cplusplus
+}
+#endif
+#endif				/* _LIBS_CUTILS_MPL_LOG_H */
diff --git a/6515/libsensors_iio/software/core/driver/include/mlinclude.h b/6515/libsensors_iio/software/core/driver/include/mlinclude.h
new file mode 100644
index 0000000..9725199
--- /dev/null
+++ b/6515/libsensors_iio/software/core/driver/include/mlinclude.h
@@ -0,0 +1,38 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+#ifndef INV_INCLUDE_H__
+#define INV_INCLUDE_H__
+
+#define INVENSENSE_FUNC_START  typedef int invensensePutFunctionCallsHere
+
+#ifdef COVERAGE
+#include "utestCommon.h"
+#endif
+#ifdef PROFILE
+#include "profile.h"
+#endif
+
+#ifdef WIN32
+#ifdef COVERAGE
+
+extern int functionEnterLog(const char *file, const char *func);
+extern int functionExitLog(const char *file, const char *func);
+
+#undef INVENSENSE_FUNC_START
+#define INVENSENSE_FUNC_START  __pragma(message(__FILE__ "|"__FUNCTION__ )) \
+    int dslkQjDsd = functionEnterLog(__FILE__, __FUNCTION__)
+#endif // COVERAGE
+#endif // WIN32
+
+#ifdef PROFILE
+#undef INVENSENSE_FUNC_START
+#define INVENSENSE_FUNC_START int dslkQjDsd = profileEnter(__FILE__, __FUNCTION__)
+#define return if ( profileExit(__FILE__, __FUNCTION__) ) return
+#endif // PROFILE
+
+// #define return if ( functionExitLog(__FILE__, __FUNCTION__) ) return
+
+#endif //INV_INCLUDE_H__
diff --git a/6515/libsensors_iio/software/core/driver/include/mlmath.h b/6515/libsensors_iio/software/core/driver/include/mlmath.h
new file mode 100644
index 0000000..37194d6
--- /dev/null
+++ b/6515/libsensors_iio/software/core/driver/include/mlmath.h
@@ -0,0 +1,95 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/*******************************************************************************
+ *
+ * $Id: mlmath.h 5629 2011-06-11 03:13:08Z mcaramello $ 
+ * 
+ *******************************************************************************/
+
+#ifndef _ML_MATH_H_
+#define	_ML_MATH_H_
+
+#ifndef MLMATH
+// This define makes Microsoft pickup things like M_PI
+#define _USE_MATH_DEFINES
+#include <math.h>
+
+#ifdef WIN32
+// Microsoft doesn't follow standards
+#define  round(x)(((double)((long long)((x)>0?(x)+.5:(x)-.5))))
+#define roundf(x)(((float )((long long)((x)>0?(x)+.5f:(x)-.5f))))
+#endif
+
+#else  // MLMATH
+
+#ifdef __cplusplus 
+extern "C" {
+#endif
+/* MPL needs below functions */
+double	ml_asin(double);
+double	ml_atan(double);
+double	ml_atan2(double, double);
+double	ml_log(double);
+double	ml_sqrt(double);
+double	ml_ceil(double);
+double	ml_floor(double);
+double  ml_cos(double);
+double  ml_sin(double);
+double  ml_acos(double);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+
+/*
+ * We rename functions here to provide the hook for other 
+ * customized math functions.
+ */
+#define	sqrt(x)      ml_sqrt(x)
+#define	log(x)       ml_log(x)
+#define	asin(x)      ml_asin(x)
+#define	atan(x)      ml_atan(x)
+#define	atan2(x,y)   ml_atan2(x,y)
+#define	ceil(x)      ml_ceil(x)
+#define	floor(x)     ml_floor(x)
+#define fabs(x)      (((x)<0)?-(x):(x))
+#define round(x)     (((double)((long long)((x)>0?(x)+.5:(x)-.5))))
+#define roundf(x)    (((float )((long long)((x)>0?(x)+.5f:(x)-.5f))))
+#define cos(x)       ml_cos(x)
+#define sin(x)       ml_sin(x)
+#define acos(x)      ml_acos(x)
+
+#define pow(x,y)     ml_pow(x,y)
+
+#ifdef LINUX
+/* stubs for float version of math functions */
+#define cosf(x)      ml_cos(x)
+#define sinf(x)      ml_sin(x)
+#define atan2f(x,y)  ml_atan2(x,y)
+#define sqrtf(x)     ml_sqrt(x)
+#endif
+
+
+
+#endif // MLMATH
+
+#ifndef M_PI
+#define M_PI 3.14159265358979
+#endif
+
+#ifndef ABS
+#define ABS(x) (((x)>=0)?(x):-(x))
+#endif
+
+#ifndef MIN
+#define MIN(x,y) (((x)<(y))?(x):(y))
+#endif
+
+#ifndef MAX
+#define MAX(x,y) (((x)>(y))?(x):(y))
+#endif
+
+/*---------------------------*/
+#endif /* !_ML_MATH_H_ */
diff --git a/6515/libsensors_iio/software/core/driver/include/mlsl.h b/6515/libsensors_iio/software/core/driver/include/mlsl.h
new file mode 100644
index 0000000..12f2901
--- /dev/null
+++ b/6515/libsensors_iio/software/core/driver/include/mlsl.h
@@ -0,0 +1,283 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+#ifndef __MLSL_H__
+#define __MLSL_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ *  @defgroup   MLSL
+ *  @brief      Motion Library - Serial Layer.
+ *              The Motion Library System Layer provides the Motion Library
+ *              with the communication interface to the hardware.
+ *
+ *              The communication interface is assumed to support serial
+ *              transfers in burst of variable length up to
+ *              SERIAL_MAX_TRANSFER_SIZE.
+ *              The default value for SERIAL_MAX_TRANSFER_SIZE is 128 bytes.
+ *              Transfers of length greater than SERIAL_MAX_TRANSFER_SIZE, will
+ *              be subdivided in smaller transfers of length <=
+ *              SERIAL_MAX_TRANSFER_SIZE.
+ *              The SERIAL_MAX_TRANSFER_SIZE definition can be modified to
+ *              overcome any host processor transfer size limitation down to
+ *              1 B, the minimum.
+ *              An higher value for SERIAL_MAX_TRANSFER_SIZE will favor
+ *              performance and efficiency while requiring higher resource usage
+ *              (mostly buffering). A smaller value will increase overhead and
+ *              decrease efficiency but allows to operate with more resource
+ *              constrained processor and master serial controllers.
+ *              The SERIAL_MAX_TRANSFER_SIZE definition can be found in the
+ *              mlsl.h header file and master serial controllers.
+ *              The SERIAL_MAX_TRANSFER_SIZE definition can be found in the
+ *              mlsl.h header file.
+ *
+ *  @{
+ *      @file   mlsl.h
+ *      @brief  The Motion Library System Layer.
+ *
+ */
+
+/*
+ * NOTE : to properly support Yamaha compass reads,
+ *	  the max transfer size should be at least 9 B.
+ *	  Length in bytes, typically a power of 2 >= 2
+ */
+#define SERIAL_MAX_TRANSFER_SIZE 31
+
+#ifndef __KERNEL__
+/**
+ *  inv_serial_open() - used to open the serial port.
+ *  @port	The COM port specification associated with the device in use.
+ *  @sl_handle	a pointer to the file handle to the serial device to be open
+ *		for the communication.
+ *	This port is used to send and receive data to the device.
+ *
+ *	This function is called by inv_serial_start().
+ *	Unlike previous MPL Software releases, explicitly calling
+ *	inv_serial_start() is mandatory to instantiate the communication
+ *	with the device.
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+int inv_serial_open(char const *port, void **sl_handle);
+
+/**
+ *  inv_serial_close() - used to close the serial port.
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *
+ *	This port is used to send and receive data to the device.
+ *
+ *	This function is called by inv_serial_stop().
+ *	Unlike previous MPL Software releases, explicitly calling
+ *	inv_serial_stop() is mandatory to properly shut-down the
+ *	communication with the device.
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+int inv_serial_close(void *sl_handle);
+
+/**
+ *  inv_serial_reset() - used to reset any buffering the driver may be doing
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+int inv_serial_reset(void *sl_handle);
+#endif
+
+/**
+ *  inv_serial_single_write() - used to write a single byte of data.
+ *  @sl_handle		pointer to the serial device used for the communication.
+ *  @slave_addr		I2C slave address of device.
+ *  @register_addr	Register address to write.
+ *  @data		Single byte of data to write.
+ *
+ *	It is called by the MPL to write a single byte of data to the MPU.
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+int inv_serial_single_write(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned char register_addr,
+	unsigned char data);
+
+/**
+ *  inv_serial_write() - used to write multiple bytes of data to registers.
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *  @slave_addr	I2C slave address of device.
+ *  @register_addr	Register address to write.
+ *  @length	Length of burst of data.
+ *  @data	Pointer to block of data.
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+int inv_serial_write(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short length,
+	unsigned char const *data);
+
+/**
+ *  inv_serial_read() - used to read multiple bytes of data from registers.
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *  @slave_addr	I2C slave address of device.
+ *  @register_addr	Register address to read.
+ *  @length	Length of burst of data.
+ *  @data	Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+int inv_serial_read(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned char register_addr,
+	unsigned short length,
+	unsigned char *data);
+
+/**
+ *  inv_serial_read_mem() - used to read multiple bytes of data from the memory.
+ *	    This should be sent by I2C or SPI.
+ *
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *  @slave_addr	I2C slave address of device.
+ *  @mem_addr	The location in the memory to read from.
+ *  @length	Length of burst data.
+ *  @data	Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+int inv_serial_read_mem(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short mem_addr,
+	unsigned char bank_reg,
+	unsigned char addr_reg,
+	unsigned char mem_reg,
+	unsigned short length,
+	unsigned char *data);
+
+/**
+ *  inv_serial_write_mem() - used to write multiple bytes of data to the memory.
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *  @slave_addr	I2C slave address of device.
+ *  @mem_addr	The location in the memory to write to.
+ *  @length	Length of burst data.
+ *  @data	Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+int inv_serial_write_mem(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short mem_addr,
+	unsigned char bank_reg,
+	unsigned char addr_reg,
+	unsigned char mem_reg,
+	unsigned short length,
+	unsigned char *data);
+
+/**
+ *  inv_serial_read_fifo() - used to read multiple bytes of data from the fifo.
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *  @slave_addr	I2C slave address of device.
+ *  @length	Length of burst of data.
+ *  @data	Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+int inv_serial_read_fifo(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned char fifo_reg,
+	unsigned short length,
+	unsigned char *data);
+
+/**
+ *  inv_serial_write_fifo() - used to write multiple bytes of data to the fifo.
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *  @slave_addr	I2C slave address of device.
+ *  @length	Length of burst of data.
+ *  @data	Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+int inv_serial_write_fifo(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned char fifo_reg,
+	unsigned short length,
+	unsigned char const *data);
+
+#ifndef __KERNEL__
+/**
+ *  inv_serial_read_cfg() - used to get the configuration data.
+ *  @cfg	Pointer to the configuration data.
+ *  @len	Length of the configuration data.
+ *
+ *		Is called by the MPL to get the configuration data
+ *		used by the motion library.
+ *		This data would typically be saved in non-volatile memory.
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+int inv_serial_read_cfg(unsigned char *cfg, unsigned int len);
+
+/**
+ *  inv_serial_write_cfg() - used to save the configuration data.
+ *  @cfg	Pointer to the configuration data.
+ *  @len	Length of the configuration data.
+ *
+ *		Is called by the MPL to save the configuration data used by the
+ *		motion library.
+ *		This data would typically be saved in non-volatile memory.
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+int inv_serial_write_cfg(unsigned char *cfg, unsigned int len);
+
+/**
+ *  inv_serial_read_cal() - used to get the calibration data.
+ *  @cfg	Pointer to the calibration data.
+ *  @len	Length of the calibration data.
+ *
+ *		It is called by the MPL to get the calibration data used by the
+ *		motion library.
+ *		This data is typically be saved in non-volatile memory.
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+int inv_serial_read_cal(unsigned char *cal, unsigned int len);
+
+/**
+ *  inv_serial_write_cal() - used to save the calibration data.
+ *
+ *  @cfg	Pointer to the calibration data.
+ *  @len	Length of the calibration data.
+ *
+ *	    It is called by the MPL to save the calibration data used by the
+ *	    motion library.
+ *	    This data is typically be saved in non-volatile memory.
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+int inv_serial_write_cal(unsigned char *cal, unsigned int len);
+
+/**
+ *  inv_serial_get_cal_length() - Get the calibration length from the storage.
+ *  @len	lenght to be returned
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+int inv_serial_get_cal_length(unsigned int *len);
+#endif
+#ifdef __cplusplus
+}
+#endif
+/**
+ * @}
+ */
+#endif				/* __MLSL_H__ */
diff --git a/6515/libsensors_iio/software/core/driver/include/mltypes.h b/6515/libsensors_iio/software/core/driver/include/mltypes.h
new file mode 100644
index 0000000..09eccce
--- /dev/null
+++ b/6515/libsensors_iio/software/core/driver/include/mltypes.h
@@ -0,0 +1,235 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/**
+ *  @defgroup MLERROR
+ *  @brief  Motion Library - Error definitions.
+ *          Definition of the error codes used within the MPL and
+ *          returned to the user.
+ *          Every function tries to return a meaningful error code basing
+ *          on the occuring error condition. The error code is numeric.
+ *
+ *          The available error codes and their associated values are:
+ *          - (0)               INV_SUCCESS
+ *          - (32)              INV_ERROR
+ *          - (22 / EINVAL)     INV_ERROR_INVALID_PARAMETER
+ *          - (1  / EPERM)      INV_ERROR_FEATURE_NOT_ENABLED
+ *          - (36)              INV_ERROR_FEATURE_NOT_IMPLEMENTED
+ *          - (38)              INV_ERROR_DMP_NOT_STARTED
+ *          - (39)              INV_ERROR_DMP_STARTED
+ *          - (40)              INV_ERROR_NOT_OPENED
+ *          - (41)              INV_ERROR_OPENED
+ *          - (19 / ENODEV)     INV_ERROR_INVALID_MODULE
+ *          - (12 / ENOMEM)     INV_ERROR_MEMORY_EXAUSTED
+ *          - (44)              INV_ERROR_DIVIDE_BY_ZERO
+ *          - (45)              INV_ERROR_ASSERTION_FAILURE
+ *          - (46)              INV_ERROR_FILE_OPEN
+ *          - (47)              INV_ERROR_FILE_READ
+ *          - (48)              INV_ERROR_FILE_WRITE
+ *          - (49)              INV_ERROR_INVALID_CONFIGURATION
+ *          - (52)              INV_ERROR_SERIAL_CLOSED
+ *          - (53)              INV_ERROR_SERIAL_OPEN_ERROR
+ *          - (54)              INV_ERROR_SERIAL_READ
+ *          - (55)              INV_ERROR_SERIAL_WRITE
+ *          - (56)              INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED
+ *          - (57)              INV_ERROR_SM_TRANSITION
+ *          - (58)              INV_ERROR_SM_IMPROPER_STATE
+ *          - (62)              INV_ERROR_FIFO_OVERFLOW
+ *          - (63)              INV_ERROR_FIFO_FOOTER
+ *          - (64)              INV_ERROR_FIFO_READ_COUNT
+ *          - (65)              INV_ERROR_FIFO_READ_DATA
+ *          - (72)              INV_ERROR_MEMORY_SET
+ *          - (82)              INV_ERROR_LOG_MEMORY_ERROR
+ *          - (83)              INV_ERROR_LOG_OUTPUT_ERROR
+ *          - (92)              INV_ERROR_OS_BAD_PTR
+ *          - (93)              INV_ERROR_OS_BAD_HANDLE
+ *          - (94)              INV_ERROR_OS_CREATE_FAILED
+ *          - (95)              INV_ERROR_OS_LOCK_FAILED
+ *          - (102)             INV_ERROR_COMPASS_DATA_OVERFLOW
+ *          - (103)             INV_ERROR_COMPASS_DATA_UNDERFLOW
+ *          - (104)             INV_ERROR_COMPASS_DATA_NOT_READY
+ *          - (105)             INV_ERROR_COMPASS_DATA_ERROR
+ *          - (107)             INV_ERROR_CALIBRATION_LOAD
+ *          - (108)             INV_ERROR_CALIBRATION_STORE
+ *          - (109)             INV_ERROR_CALIBRATION_LEN
+ *          - (110)             INV_ERROR_CALIBRATION_CHECKSUM
+ *          - (111)             INV_ERROR_ACCEL_DATA_OVERFLOW
+ *          - (112)             INV_ERROR_ACCEL_DATA_UNDERFLOW
+ *          - (113)             INV_ERROR_ACCEL_DATA_NOT_READY
+ *          - (114)             INV_ERROR_ACCEL_DATA_ERROR
+ *
+ *          The available warning codes and their associated values are:
+ *          - (115)             INV_WARNING_MOTION_RACE
+ *          - (116)             INV_WARNING_QUAT_TRASHED
+ *
+ *  @{
+ *      @file mltypes.h
+ *  @}
+ */
+
+#ifndef MLTYPES_H
+#define MLTYPES_H
+
+#ifdef __KERNEL__
+#include <linux/types.h>
+#include <asm-generic/errno-base.h>
+#else
+#include "stdint_invensense.h"
+#include <errno.h>
+#endif
+#include <limits.h>
+
+#ifndef REMOVE_INV_ERROR_T
+/*---------------------------
+ *    ML Types
+ *--------------------------*/
+
+/**
+ *  @struct inv_error_t mltypes.h "mltypes"
+ *  @brief  The MPL Error Code return type.
+ *
+ *  @code
+ *      typedef unsigned char inv_error_t;
+ *  @endcode
+ */
+//typedef unsigned char inv_error_t;
+typedef int inv_error_t;
+#endif
+
+typedef long long inv_time_t;
+
+#if !defined __GNUC__ && !defined __KERNEL__
+typedef int8_t   __s8;
+typedef int16_t  __s16;
+typedef int32_t  __s32;
+typedef int32_t  __s64;
+
+typedef uint8_t   __u8;
+typedef uint16_t  __u16;
+typedef uint32_t  __u32;
+typedef uint64_t  __u64;
+#elif !defined __KERNEL__
+#include <sys/types.h>
+#endif
+
+#ifndef __cplusplus
+#ifndef __KERNEL__
+typedef int_fast8_t bool;
+
+#ifndef false
+#define false 0
+#endif
+
+#ifndef true
+#define true 1
+#endif
+
+#endif
+#endif
+
+/*---------------------------
+ *    ML Defines
+ *--------------------------*/
+
+#ifndef NULL
+#define NULL 0
+#endif
+
+#ifndef __KERNEL__
+#ifndef ARRAY_SIZE
+/* Dimension of an array */
+#define ARRAY_SIZE(array) (sizeof(array)/sizeof((array)[0]))
+#endif
+#endif
+/* - ML Errors. - */
+#define ERROR_NAME(x)   (#x)
+#define ERROR_CHECK_FIRST(first, x) \
+	{ if (INV_SUCCESS == first) first = x; }
+
+#define INV_SUCCESS                       (0)
+/* Generic Error code.  Proprietary Error Codes only */
+#define INV_ERROR_BASE                    (0x20)
+#define INV_ERROR                         (INV_ERROR_BASE)
+
+/* Compatibility and other generic error codes */
+#define INV_ERROR_INVALID_PARAMETER             (EINVAL)
+#define INV_ERROR_FEATURE_NOT_ENABLED           (EPERM)
+#define INV_ERROR_FEATURE_NOT_IMPLEMENTED       (INV_ERROR_BASE + 4)
+#define INV_ERROR_DMP_NOT_STARTED               (INV_ERROR_BASE + 6)
+#define INV_ERROR_DMP_STARTED                   (INV_ERROR_BASE + 7)
+#define INV_ERROR_NOT_OPENED                    (INV_ERROR_BASE + 8)
+#define INV_ERROR_OPENED                        (INV_ERROR_BASE + 9)
+#define INV_ERROR_INVALID_MODULE                (ENODEV)
+#define INV_ERROR_MEMORY_EXAUSTED               (ENOMEM)
+#define INV_ERROR_DIVIDE_BY_ZERO                (INV_ERROR_BASE + 12)
+#define INV_ERROR_ASSERTION_FAILURE             (INV_ERROR_BASE + 13)
+#define INV_ERROR_FILE_OPEN                     (INV_ERROR_BASE + 14)
+#define INV_ERROR_FILE_READ                     (INV_ERROR_BASE + 15)
+#define INV_ERROR_FILE_WRITE                    (INV_ERROR_BASE + 16)
+#define INV_ERROR_INVALID_CONFIGURATION         (INV_ERROR_BASE + 17)
+#define INV_ERROR_NOT_AUTHORIZED                (INV_ERROR_BASE + 18)
+
+/* Serial Communication */
+#define INV_ERROR_SERIAL_CLOSED                 (INV_ERROR_BASE + 20)
+#define INV_ERROR_SERIAL_OPEN_ERROR             (INV_ERROR_BASE + 21)
+#define INV_ERROR_SERIAL_READ                   (INV_ERROR_BASE + 22)
+#define INV_ERROR_SERIAL_WRITE                  (INV_ERROR_BASE + 23)
+#define INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED  (INV_ERROR_BASE + 24)
+
+/* SM = State Machine */
+#define INV_ERROR_SM_TRANSITION                 (INV_ERROR_BASE + 25)
+#define INV_ERROR_SM_IMPROPER_STATE             (INV_ERROR_BASE + 26)
+
+/* Fifo */
+#define INV_ERROR_FIFO_OVERFLOW                 (INV_ERROR_BASE + 30)
+#define INV_ERROR_FIFO_FOOTER                   (INV_ERROR_BASE + 31)
+#define INV_ERROR_FIFO_READ_COUNT               (INV_ERROR_BASE + 32)
+#define INV_ERROR_FIFO_READ_DATA                (INV_ERROR_BASE + 33)
+
+/* Memory & Registers, Set & Get */
+#define INV_ERROR_MEMORY_SET                    (INV_ERROR_BASE + 40)
+
+#define INV_ERROR_LOG_MEMORY_ERROR              (INV_ERROR_BASE + 50)
+#define INV_ERROR_LOG_OUTPUT_ERROR              (INV_ERROR_BASE + 51)
+
+/* OS interface errors */
+#define INV_ERROR_OS_BAD_PTR                    (INV_ERROR_BASE + 60)
+#define INV_ERROR_OS_BAD_HANDLE                 (INV_ERROR_BASE + 61)
+#define INV_ERROR_OS_CREATE_FAILED              (INV_ERROR_BASE + 62)
+#define INV_ERROR_OS_LOCK_FAILED                (INV_ERROR_BASE + 63)
+
+/* Compass errors */
+#define INV_ERROR_COMPASS_DATA_OVERFLOW         (INV_ERROR_BASE + 70)
+#define INV_ERROR_COMPASS_DATA_UNDERFLOW        (INV_ERROR_BASE + 71)
+#define INV_ERROR_COMPASS_DATA_NOT_READY        (INV_ERROR_BASE + 72)
+#define INV_ERROR_COMPASS_DATA_ERROR            (INV_ERROR_BASE + 73)
+
+/* Load/Store calibration */
+#define INV_ERROR_CALIBRATION_LOAD              (INV_ERROR_BASE + 75)
+#define INV_ERROR_CALIBRATION_STORE             (INV_ERROR_BASE + 76)
+#define INV_ERROR_CALIBRATION_LEN               (INV_ERROR_BASE + 77)
+#define INV_ERROR_CALIBRATION_CHECKSUM          (INV_ERROR_BASE + 78)
+
+/* Accel errors */
+#define INV_ERROR_ACCEL_DATA_OVERFLOW           (INV_ERROR_BASE + 79)
+#define INV_ERROR_ACCEL_DATA_UNDERFLOW          (INV_ERROR_BASE + 80)
+#define INV_ERROR_ACCEL_DATA_NOT_READY          (INV_ERROR_BASE + 81)
+#define INV_ERROR_ACCEL_DATA_ERROR              (INV_ERROR_BASE + 82)
+
+/* No Motion Warning States */
+#define INV_WARNING_MOTION_RACE                 (INV_ERROR_BASE + 83)
+#define INV_WARNING_QUAT_TRASHED                (INV_ERROR_BASE + 84)
+#define INV_WARNING_GYRO_MAG                    (INV_ERROR_BASE + 85)
+
+#define INV_WARNING_SEMAPHORE_TIMEOUT           (INV_ERROR_BASE + 86)
+
+
+/* For Linux coding compliance */
+#ifndef __KERNEL__
+#define EXPORT_SYMBOL(x)
+#endif
+
+#endif				/* MLTYPES_H */
diff --git a/6515/libsensors_iio/software/core/driver/include/stdint_invensense.h b/6515/libsensors_iio/software/core/driver/include/stdint_invensense.h
new file mode 100644
index 0000000..9440228
--- /dev/null
+++ b/6515/libsensors_iio/software/core/driver/include/stdint_invensense.h
@@ -0,0 +1,41 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+#ifndef STDINT_INVENSENSE_H
+#define STDINT_INVENSENSE_H
+
+#ifndef _WIN32
+
+#ifdef __KERNEL__
+#include <linux/types.h>
+#else
+#include <stdint.h>
+#endif
+
+#else
+
+#include <windows.h>
+
+typedef signed char int8_t;
+typedef short int16_t;
+typedef long int32_t;
+typedef long long int64_t;
+
+typedef unsigned char uint8_t;
+typedef unsigned short uint16_t;
+typedef unsigned long uint32_t;
+typedef unsigned long long uint64_t;
+
+typedef int int_fast8_t;
+typedef int int_fast16_t;
+typedef long int_fast32_t;
+
+typedef unsigned int uint_fast8_t;
+typedef unsigned int uint_fast16_t;
+typedef unsigned long uint_fast32_t;
+
+#endif
+
+#endif // STDINT_INVENSENSE_H
diff --git a/6515/libsensors_iio/software/core/mllite/build/android/libmllite.so b/6515/libsensors_iio/software/core/mllite/build/android/libmllite.so
new file mode 100755
index 0000000..407d4f2
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/build/android/libmllite.so
diff --git a/6515/libsensors_iio/software/core/mllite/build/android/shared.mk b/6515/libsensors_iio/software/core/mllite/build/android/shared.mk
new file mode 100644
index 0000000..1418450
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/build/android/shared.mk
@@ -0,0 +1,87 @@
+MLLITE_LIB_NAME = mllite
+LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
+COMP  ?= $(CROSS)gcc
+LINK  ?= $(CROSS)gcc 
+
+OBJFOLDER = $(CURDIR)/obj
+
+INV_ROOT = ../../../../..
+MLLITE_DIR  = $(INV_ROOT)/software/core/mllite
+
+include $(INV_ROOT)/software/build/android/common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += $(ANDROID_COMPILE)
+CFLAGS += -Wall
+CFLAGS += -fpic
+CFLAGS += -nostdlib
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT
+CFLAGS += -DLINUX
+CFLAGS += -DANDROID
+CFLAGS += -mthumb-interwork
+CFLAGS += -fno-exceptions
+CFLAGS += -ffunction-sections
+CFLAGS += -funwind-tables
+CFLAGS += -fstack-protector
+CFLAGS += -fno-short-enums
+CFLAGS += -fmessage-length=0
+CFLAGS += -I$(MLLITE_DIR)
+CFLAGS += -I$(INV_ROOT)/simple_apps/common
+CFLAGS += $(INV_INCLUDES)
+CFLAGS += $(INV_DEFINES)
+
+LLINK  = -lc 
+LLINK += -lm 
+LLINK += -lutils 
+LLINK += -lcutils 
+LLINK += -lgcc 
+LLINK += -ldl
+
+LFLAGS += $(CMDLINE_LFLAGS)
+LFLAGS += -shared 
+LFLAGS += -Wl,-soname,$(LIBRARY)
+LFLAGS += -Wl,-shared,-Bsymbolic
+LFLAGS += $(ANDROID_LINK)
+LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+####################################################################################################
+## sources
+
+#INV_SOURCES provided by Makefile.filelist
+include ../filelist.mk
+
+INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
+INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all mllite clean cleanall makefiles
+
+all: mllite
+
+mllite: $(LIBRARY) $(MK_NAME)
+
+$(LIBRARY) : $(OBJFOLDER) $(INV_OBJS_DST) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(INV_OBJS_DST)\n")
+	$(LINK) $(LFLAGS) -o $(LIBRARY) $(INV_OBJS_DST) $(LLINK) $(INV_LIBS) $(LLINK)
+
+$(OBJFOLDER) :
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(CFLAGS) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(LIBRARY) $(OBJFOLDER)
+
diff --git a/6515/libsensors_iio/software/core/mllite/build/filelist.mk b/6515/libsensors_iio/software/core/mllite/build/filelist.mk
new file mode 100644
index 0000000..011120c
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/build/filelist.mk
@@ -0,0 +1,42 @@
+#### filelist.mk for mllite ####
+
+# headers only
+HEADERS := $(MLLITE_DIR)/invensense.h
+
+# headers for sources
+HEADERS += $(MLLITE_DIR)/data_builder.h
+HEADERS += $(MLLITE_DIR)/hal_outputs.h
+HEADERS += $(MLLITE_DIR)/message_layer.h
+HEADERS += $(MLLITE_DIR)/ml_math_func.h
+HEADERS += $(MLLITE_DIR)/mpl.h
+HEADERS += $(MLLITE_DIR)/results_holder.h
+HEADERS += $(MLLITE_DIR)/start_manager.h
+HEADERS += $(MLLITE_DIR)/storage_manager.h
+
+# headers (linux specific)
+HEADERS += $(MLLITE_DIR)/linux/mlos.h
+HEADERS += $(MLLITE_DIR)/linux/ml_stored_data.h
+HEADERS += $(MLLITE_DIR)/linux/ml_load_dmp.h
+HEADERS += $(MLLITE_DIR)/linux/ml_sysfs_helper.h
+
+# sources
+SOURCES := $(MLLITE_DIR)/data_builder.c
+SOURCES += $(MLLITE_DIR)/hal_outputs.c
+SOURCES += $(MLLITE_DIR)/message_layer.c
+SOURCES += $(MLLITE_DIR)/ml_math_func.c
+SOURCES += $(MLLITE_DIR)/mpl.c
+SOURCES += $(MLLITE_DIR)/results_holder.c
+SOURCES += $(MLLITE_DIR)/start_manager.c
+SOURCES += $(MLLITE_DIR)/storage_manager.c
+
+# sources (linux specific)
+SOURCES += $(MLLITE_DIR)/linux/mlos_linux.c
+SOURCES += $(MLLITE_DIR)/linux/ml_stored_data.c
+SOURCES += $(MLLITE_DIR)/linux/ml_load_dmp.c
+SOURCES += $(MLLITE_DIR)/linux/ml_sysfs_helper.c
+
+
+INV_SOURCES += $(SOURCES)
+
+VPATH += $(MLLITE_DIR) $(MLLITE_DIR)/linux
+
diff --git a/6515/libsensors_iio/software/core/mllite/data_builder.c b/6515/libsensors_iio/software/core/mllite/data_builder.c
new file mode 100644
index 0000000..0c842a1
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/data_builder.c
@@ -0,0 +1,1711 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/**
+ *   @defgroup  Data_Builder data_builder
+ *   @brief     Motion Library - Data Builder
+ *              Constructs and Creates the data for MPL
+ *
+ *   @{
+ *       @file data_builder.c
+ *       @brief Data Builder.
+ */
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 1 /* Use 0 to turn on MPL_LOGV output */
+
+#include <string.h>
+
+#include "ml_math_func.h"
+#include "data_builder.h"
+#include "mlmath.h"
+#include "storage_manager.h"
+#include "message_layer.h"
+#include "results_holder.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MLLITE"
+
+typedef inv_error_t (*inv_process_cb_func)(struct inv_sensor_cal_t *data);
+
+struct process_t {
+    inv_process_cb_func func;
+    int priority;
+    int data_required;
+};
+
+struct inv_data_builder_t {
+    int num_cb;
+    struct process_t process[INV_MAX_DATA_CB];
+    struct inv_db_save_t save;
+    struct inv_db_save_mpl_t save_mpl;
+    struct inv_db_save_accel_mpl_t save_accel_mpl;
+    int compass_disturbance;
+    int mode;
+#ifdef INV_PLAYBACK_DBG
+    int debug_mode;
+    int last_mode;
+    FILE *file;
+#endif
+};
+
+void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias);
+static void inv_set_contiguous(void);
+
+static struct inv_data_builder_t inv_data_builder;
+static struct inv_sensor_cal_t sensors;
+
+#ifdef INV_PLAYBACK_DBG
+
+/** Turn on data logging to allow playback of same scenario at a later time.
+* @param[in] file File to write to, must be open.
+*/
+void inv_turn_on_data_logging(FILE *file)
+{
+    MPL_LOGV("input data logging started\n");
+    inv_data_builder.file = file;
+    inv_data_builder.debug_mode = RD_RECORD;
+}
+
+/** Turn off data logging to allow playback of same scenario at a later time.
+* File passed to inv_turn_on_data_logging() must be closed after calling this.
+*/
+void inv_turn_off_data_logging()
+{
+    MPL_LOGV("input data logging stopped\n");
+    inv_data_builder.debug_mode = RD_NO_DEBUG;
+    inv_data_builder.file = NULL;
+}
+#endif
+
+/** Gets last value of raw compass data.
+* @param[out] raw Raw compass data in mounting frame in hardware units. Length 3.
+*/
+void inv_get_raw_compass(short *raw)
+{
+    memcpy(raw, sensors.compass.raw, sizeof(sensors.compass.raw));
+}
+
+/** This function receives the data that was stored in non-volatile memory between power off */
+static inv_error_t inv_db_load_func(const unsigned char *data)
+{
+    memcpy(&inv_data_builder.save, data, sizeof(inv_data_builder.save));
+    // copy in the saved accuracy in the actual sensors accuracy
+    sensors.gyro.accuracy = inv_data_builder.save.gyro_accuracy;
+    sensors.accel.accuracy = inv_data_builder.save.accel_accuracy;
+    sensors.compass.accuracy = inv_data_builder.save.compass_accuracy;
+    // TODO
+    if (sensors.accel.accuracy == 3) {
+        inv_set_accel_bias_found(1);
+    }
+    if (sensors.compass.accuracy == 3) {
+        inv_set_compass_bias_found(1);
+    }
+    return INV_SUCCESS;
+}
+
+/** This function returns the data to be stored in non-volatile memory between power off */
+static inv_error_t inv_db_save_func(unsigned char *data)
+{
+    memcpy(data, &inv_data_builder.save, sizeof(inv_data_builder.save));
+    return INV_SUCCESS;
+}
+
+/** This function receives the data for mpl that was stored in non-volatile memory between power off */
+static inv_error_t inv_db_load_mpl_func(const unsigned char *data)
+{
+    memcpy(&inv_data_builder.save_mpl, data, sizeof(inv_data_builder.save_mpl));
+
+    return INV_SUCCESS;
+}
+
+/** This function returns the data for mpl to be stored in non-volatile memory between power off */
+static inv_error_t inv_db_save_mpl_func(unsigned char *data)
+{
+    memcpy(data, &inv_data_builder.save_mpl, sizeof(inv_data_builder.save_mpl));
+    return INV_SUCCESS;
+}
+
+/** This function receives the data for mpl that was stored in non-volatile memory between power off */
+static inv_error_t inv_db_load_accel_mpl_func(const unsigned char *data)
+{
+    memcpy(&inv_data_builder.save_accel_mpl, data, sizeof(inv_data_builder.save_accel_mpl));
+
+    return INV_SUCCESS;
+}
+
+/** This function returns the data for mpl to be stored in non-volatile memory between power off */
+static inv_error_t inv_db_save_accel_mpl_func(unsigned char *data)
+{
+    memcpy(data, &inv_data_builder.save_accel_mpl, sizeof(inv_data_builder.save_accel_mpl));
+    return INV_SUCCESS;
+}
+
+/** Initialize the data builder
+*/
+inv_error_t inv_init_data_builder(void)
+{
+    /* TODO: Hardcode temperature scale/offset here. */
+    memset(&inv_data_builder, 0, sizeof(inv_data_builder));
+    memset(&sensors, 0, sizeof(sensors));
+
+    // disable the soft iron transform process
+    inv_reset_compass_soft_iron_matrix();
+
+    return ((inv_register_load_store(inv_db_load_func, inv_db_save_func,
+                                   sizeof(inv_data_builder.save),
+                                   INV_DB_SAVE_KEY))
+          | (inv_register_load_store(inv_db_load_mpl_func, inv_db_save_mpl_func,
+                                   sizeof(inv_data_builder.save_mpl),
+                                   INV_DB_SAVE_MPL_KEY))
+          | (inv_register_load_store(inv_db_load_accel_mpl_func, inv_db_save_accel_mpl_func,
+                                   sizeof(inv_data_builder.save_accel_mpl),
+                                   INV_DB_SAVE_ACCEL_MPL_KEY)) );
+}
+
+/** Gyro sensitivity.
+* @return A scale factor to convert device units to degrees per second scaled by 2^16
+* such that degrees_per_second  = device_units * sensitivity / 2^30. Typically
+* it works out to be the maximum rate * 2^15.
+*/
+long inv_get_gyro_sensitivity(void)
+{
+    return sensors.gyro.sensitivity;
+}
+
+/** Accel sensitivity.
+* @return A scale factor to convert device units to g's scaled by 2^16
+* such that g_s  = device_units * sensitivity / 2^30. Typically
+* it works out to be the maximum accel value in g's * 2^15.
+*/
+long inv_get_accel_sensitivity(void)
+{
+    return sensors.accel.sensitivity;
+}
+
+/** Compass sensitivity.
+* @return A scale factor to convert device units to micro Tesla scaled by 2^16
+* such that uT  = device_units * sensitivity / 2^30. Typically
+* it works out to be the maximum uT * 2^15.
+*/
+long inv_get_compass_sensitivity(void)
+{
+    return sensors.compass.sensitivity;
+}
+
+/** Sets orientation and sensitivity field for a sensor.
+* @param[out] sensor Structure to apply settings to
+* @param[in] orientation Orientation description of how part is mounted.
+* @param[in] sensitivity A Scale factor to convert from hardware units to
+*            standard units (dps, uT, g).
+*/
+void set_sensor_orientation_and_scale(struct inv_single_sensor_t *sensor,
+                                 int orientation, long sensitivity)
+{
+    int error = 0;
+
+    if (!sensitivity) {
+        // Sensitivity can't be zero
+        sensitivity = 1L<<16;
+        MPL_LOGE("\n\nCritical error! Sensitivity is zero.\n\n");
+    }
+
+    sensor->sensitivity = sensitivity;
+    // Make sure we don't describe some impossible orientation
+    if ((orientation & 3) == 3) {
+        error = 1;
+    }
+    if ((orientation & 0x18) == 0x18) {
+        error = 1;
+    }
+    if ((orientation & 0xc0) == 0xc0) {
+        error = 1;
+    }
+    if (error) {
+        orientation = 0x88; // Identity
+        MPL_LOGE("\n\nCritical error! Impossible mounting orientation given. Using Identity instead\n\n");
+    }
+    sensor->orientation = orientation;
+}
+
+/** Sets the Orientation and Sensitivity of the gyro data.
+* @param[in] orientation A scalar defining the transformation from chip mounting
+*            to the body frame. The function inv_orientation_matrix_to_scalar()
+*            can convert the transformation matrix to this scalar and describes the
+*            scalar in further detail.
+* @param[in] sensitivity A scale factor to convert device units to degrees per second scaled by 2^16
+*            such that degrees_per_second  = device_units * sensitivity / 2^30. Typically
+*            it works out to be the maximum rate * 2^15.
+*/
+void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_G_ORIENT;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file);
+        fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file);
+    }
+#endif
+    set_sensor_orientation_and_scale(&sensors.gyro, orientation,
+                                     sensitivity);
+}
+
+/** Set Gyro Sample rate in micro seconds.
+* @param[in] sample_rate_us Set Gyro Sample rate in us
+*/
+void inv_set_gyro_sample_rate(long sample_rate_us)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_G_SAMPLE_RATE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.gyro.sample_rate_us = sample_rate_us;
+    sensors.gyro.sample_rate_ms = sample_rate_us / 1000;
+    if (sensors.gyro.bandwidth == 0) {
+        sensors.gyro.bandwidth = (int)(1000000L / sample_rate_us);
+    }
+}
+
+/** Set Accel Sample rate in micro seconds.
+* @param[in] sample_rate_us Set Accel Sample rate in us
+*/
+void inv_set_accel_sample_rate(long sample_rate_us)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_A_SAMPLE_RATE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.accel.sample_rate_us = sample_rate_us;
+    sensors.accel.sample_rate_ms = sample_rate_us / 1000;
+    if (sensors.accel.bandwidth == 0) {
+        sensors.accel.bandwidth = (int)(1000000L / sample_rate_us);
+    }
+}
+
+/** Pick the smallest non-negative number. Priority to td1 on equal
+* If both are negative, return the largest.
+*/
+static int inv_pick_best_time_difference(long td1, long td2)
+{
+    if (td1 >= 0) {
+        if (td2 >= 0) {
+            if (td1 <= td2) {
+                // td1
+                return 0;
+            } else {
+                // td2
+                return 1;
+            }
+        } else {
+            // td1
+            return 0;
+        }
+    } else if (td2 >= 0) {
+        // td2
+        return 1;
+    } else {
+        // Both are negative
+        if (td1 >= td2) {
+            // td1
+            return 0;
+        } else {
+            // td2
+            return 1;
+        }
+    }
+}
+
+/** Returns timestame based upon a raw sensor, and returns if that sample has a new piece of data.
+*/
+static int inv_raw_sensor_timestamp(int sensor_number, inv_time_t *ts)
+{
+    int status = 0;
+    switch (sensor_number) {
+    case 0: // Quat
+        *ts = sensors.quat.timestamp;
+        if (inv_data_builder.mode & INV_QUAT_NEW)
+            if (sensors.quat.timestamp_prev != sensors.quat.timestamp)
+                status = 1;
+        return status;
+    case 1: // Gyro
+        *ts = sensors.gyro.timestamp;
+        if (inv_data_builder.mode & INV_GYRO_NEW)
+            if (sensors.gyro.timestamp_prev != sensors.gyro.timestamp)
+                status = 1;
+        return status;
+    case 2: // Compass
+        *ts = sensors.compass.timestamp;
+        if (inv_data_builder.mode & INV_MAG_NEW)
+            if (sensors.compass.timestamp_prev != sensors.compass.timestamp)
+                status = 1;
+        return status;
+    case 3: // Accel
+        *ts = sensors.accel.timestamp;
+        if (inv_data_builder.mode & INV_ACCEL_NEW)
+            if (sensors.accel.timestamp_prev != sensors.accel.timestamp)
+                status = 1;
+        return status;
+    default:
+        *ts = 0;
+        return 0;
+    }
+    return 0;
+}
+
+/** Gets best timestamp and if there is a new piece of data for a 9-axis sensor combination.
+* It does this by finding a raw sensor that has the closest sample rate that is at least as
+* often desired. It also returns if that raw sensor has a new piece of data.
+* Priority is 9-axis quat, 6-axis quat, 3-axis quat, gyro, compass, accel on ties.
+* @return Returns 1, if the raw sensor being attached has new data, 0 otherwise.
+*/
+int inv_get_9_axis_timestamp(long sample_rate_us, inv_time_t *ts)
+{
+    int status = 0;
+    long td[3];
+    int idx,idx2;
+
+    if ((sensors.quat.status & (INV_QUAT_9AXIS | INV_SENSOR_ON)) == (INV_QUAT_9AXIS | INV_SENSOR_ON)) {
+        // 9-axis from DMP
+        *ts = sensors.quat.timestamp;
+        if (inv_data_builder.mode & INV_QUAT_NEW)
+            if (sensors.quat.timestamp_prev != sensors.quat.timestamp)
+                status = 1;
+        return status;
+    }
+    
+    if ((sensors.compass.status & INV_SENSOR_ON) == 0) {
+        return 0; // Compass must be on
+    }
+
+    // At this point, we know compass is on. Check if accel or 6-axis quat is on
+    td[0] = sample_rate_us - sensors.quat.sample_rate_us;
+    td[1] = sample_rate_us - sensors.compass.sample_rate_us;
+    if ((sensors.quat.status & (INV_QUAT_6AXIS | INV_SENSOR_ON)) == (INV_QUAT_6AXIS | INV_SENSOR_ON)) {
+        // Sensor tied to compass or 6-axis
+        idx = inv_pick_best_time_difference(td[0], td[1]);
+        idx *= 2; // Sensor number is 0 (Quat) or 2 (Compass)
+        return inv_raw_sensor_timestamp(idx, ts);
+    } else if ((sensors.accel.status & INV_SENSOR_ON) == 0) {
+        return 0; // Accel must be on or 6-axis quat must be on
+    }
+
+    // At this point, we know accel is on. Check if 3-axis quat is on
+    td[2] = sample_rate_us - sensors.accel.sample_rate_us;
+    if ((sensors.quat.status & (INV_QUAT_3AXIS | INV_SENSOR_ON)) == (INV_QUAT_3AXIS | INV_SENSOR_ON)) {
+        idx = inv_pick_best_time_difference(td[0], td[1]);
+        idx2 = inv_pick_best_time_difference(td[idx], td[2]);
+        if (idx2 == 1)
+            idx = 2;
+        if (idx > 0)
+            idx++; // Skip gyro sensor in next function call
+        // 0 = quat, 2 = compass, 3=accel
+        return inv_raw_sensor_timestamp(idx, ts);
+    }
+
+    // No Quaternion data from outside, Gyro must be on
+    if ((sensors.gyro.status & INV_SENSOR_ON) == 0) {
+        return 0; // Gyro must be on
+    }
+
+    td[0] = sample_rate_us - sensors.gyro.sample_rate_us;
+    idx = inv_pick_best_time_difference(td[0], td[1]);
+    idx2 = inv_pick_best_time_difference(td[idx], td[2]);
+    if (idx2 == 1)
+        idx = 2;
+    idx++;
+    // 1 = gyro, 2 = compass, 3=accel
+    return inv_raw_sensor_timestamp(idx, ts);
+}
+
+/** Gets best timestamp and if there is a new piece of data for a 9-axis sensor combination.
+* It does this by finding a raw sensor that has the closest sample rate that is at least as
+* often desired. It also returns if that raw sensor has a new piece of data.
+* Priority compass, accel on a tie
+* @return Returns 1, if the raw sensor being attached has new data, 0 otherwise.
+*/
+int inv_get_6_axis_compass_accel_timestamp(long sample_rate_us, inv_time_t *ts)
+{
+    long td[2];
+    int idx;
+
+    if ((sensors.compass.status & INV_SENSOR_ON) == 0) {
+        return 0; // Compass must be on
+    }
+    if ((sensors.accel.status & INV_SENSOR_ON) == 0) {
+        return 0; // Accel must be on
+    }
+
+    // At this point, we know compass & accel are both on.
+    td[0] = sample_rate_us - sensors.compass.sample_rate_us;
+    td[1] = sample_rate_us - sensors.accel.sample_rate_us;
+    idx = inv_pick_best_time_difference(td[0], td[1]);
+    idx += 2;
+    return inv_raw_sensor_timestamp(idx, ts);
+}
+
+/** Gets best timestamp and if there is a new piece of data for a 9-axis sensor combination.
+* It does this by finding a raw sensor that has the closest sample rate that is at least as
+* often desired. It also returns if that raw sensor has a new piece of data.
+* Priority is Quaternion-6axis, Quaternion 3-axis, Gyro, Accel
+* @return Returns 1, if the raw sensor being attached has new data, 0 otherwise.
+*/
+int inv_get_6_axis_gyro_accel_timestamp(long sample_rate_us, inv_time_t *ts)
+{
+    long td[2];
+    int idx;
+
+    if ((sensors.quat.status & (INV_QUAT_6AXIS | INV_SENSOR_ON)) == (INV_QUAT_6AXIS | INV_SENSOR_ON)) {
+        // Sensor number is 0 (Quat)
+        return inv_raw_sensor_timestamp(0, ts);
+    } else if ((sensors.accel.status & INV_SENSOR_ON) == 0) {
+        return 0; // Accel must be on or 6-axis quat must be on
+    }
+
+    // At this point, we know accel is on. Check if 3-axis quat is on
+    td[0] = sample_rate_us - sensors.quat.sample_rate_us;
+    td[1] = sample_rate_us - sensors.accel.sample_rate_us;
+    if ((sensors.quat.status & (INV_QUAT_3AXIS | INV_SENSOR_ON)) == (INV_QUAT_3AXIS | INV_SENSOR_ON)) {
+        idx = inv_pick_best_time_difference(td[0], td[1]);
+        idx *= 3;
+        // 0 = quat, 3=accel
+        return inv_raw_sensor_timestamp(idx, ts);
+    }
+
+    // No Quaternion data from outside, Gyro must be on
+    if ((sensors.gyro.status & INV_SENSOR_ON) == 0) {
+        return 0; // Gyro must be on
+    }
+
+    td[0] = sample_rate_us - sensors.gyro.sample_rate_us;
+    idx = inv_pick_best_time_difference(td[0], td[1]);
+    idx *= 2; // 0=gyro 2=accel
+    idx++;
+    // 1 = gyro, 3=accel
+    return inv_raw_sensor_timestamp(idx, ts);
+}
+
+/** Set Compass Sample rate in micro seconds.
+* @param[in] sample_rate_us Set Gyro Sample rate in micro seconds.
+*/
+void inv_set_compass_sample_rate(long sample_rate_us)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_C_SAMPLE_RATE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.compass.sample_rate_us = sample_rate_us;
+    sensors.compass.sample_rate_ms = sample_rate_us / 1000;
+    if (sensors.compass.bandwidth == 0) {
+        sensors.compass.bandwidth = (int)(1000000L / sample_rate_us);
+    }
+}
+
+void inv_get_gyro_sample_rate_ms(long *sample_rate_ms)
+{
+	*sample_rate_ms = sensors.gyro.sample_rate_ms;
+}
+
+void inv_get_accel_sample_rate_ms(long *sample_rate_ms)
+{
+	*sample_rate_ms = sensors.accel.sample_rate_ms;
+}
+
+void inv_get_compass_sample_rate_ms(long *sample_rate_ms)
+{
+	*sample_rate_ms = sensors.compass.sample_rate_ms;
+}
+
+/** Set Quat Sample rate in micro seconds.
+* @param[in] sample_rate_us Set Quat Sample rate in us
+*/
+void inv_set_quat_sample_rate(long sample_rate_us)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.quat.sample_rate_us = sample_rate_us;
+    sensors.quat.sample_rate_ms = sample_rate_us / 1000;
+}
+
+/** Set Gyro Bandwidth in Hz
+* @param[in] bandwidth_hz Gyro bandwidth in Hz
+*/
+void inv_set_gyro_bandwidth(int bandwidth_hz)
+{
+    sensors.gyro.bandwidth = bandwidth_hz;
+}
+
+/** Set Accel Bandwidth in Hz
+* @param[in] bandwidth_hz Gyro bandwidth in Hz
+*/
+void inv_set_accel_bandwidth(int bandwidth_hz)
+{
+    sensors.accel.bandwidth = bandwidth_hz;
+}
+
+/** Set Compass Bandwidth in Hz
+* @param[in]  bandwidth_hz Gyro bandwidth in Hz
+*/
+void inv_set_compass_bandwidth(int bandwidth_hz)
+{
+    sensors.compass.bandwidth = bandwidth_hz;
+}
+
+/** Helper function stating whether the compass is on or off.
+ * @return TRUE if compass if on, 0 if compass if off
+*/
+int inv_get_compass_on()
+{
+    return (sensors.compass.status & INV_SENSOR_ON) == INV_SENSOR_ON;
+}
+
+/** Helper function stating whether the gyro is on or off.
+ * @return TRUE if gyro if on, 0 if gyro if off
+*/
+int inv_get_gyro_on()
+{
+    return (sensors.gyro.status & INV_SENSOR_ON) == INV_SENSOR_ON;
+}
+
+/** Helper function stating whether the acceleromter is on or off.
+ * @return TRUE if accel if on, 0 if accel if off
+*/
+int inv_get_accel_on()
+{
+    return (sensors.accel.status & INV_SENSOR_ON) == INV_SENSOR_ON;
+}
+
+/** Get last timestamp across all 3 sensors that are on.
+* This find out which timestamp has the largest value for sensors that are on.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_time_t inv_get_last_timestamp()
+{
+    inv_time_t timestamp = 0;
+    if (sensors.accel.status & INV_SENSOR_ON) {
+        timestamp = sensors.accel.timestamp;
+    }
+    if (sensors.gyro.status & INV_SENSOR_ON) {
+        if (timestamp < sensors.gyro.timestamp) {
+            timestamp = sensors.gyro.timestamp;
+        }
+        MPL_LOGV("g ts: %lld", timestamp);
+    }
+    if (sensors.compass.status & INV_SENSOR_ON) {
+        if (timestamp < sensors.compass.timestamp) {
+            timestamp = sensors.compass.timestamp;
+        }
+    }
+    if (sensors.temp.status & INV_SENSOR_ON) {
+        if (timestamp < sensors.temp.timestamp) {
+            timestamp = sensors.temp.timestamp;
+        }
+    }
+    if (sensors.quat.status & INV_SENSOR_ON) {
+        if (timestamp < sensors.quat.timestamp) {
+            timestamp = sensors.quat.timestamp;
+        }
+        MPL_LOGV("q ts: %lld", timestamp);
+    }
+
+    return timestamp;
+}
+
+/** Sets the orientation and sensitivity of the gyro data.
+* @param[in] orientation A scalar defining the transformation from chip mounting
+*            to the body frame. The function inv_orientation_matrix_to_scalar()
+*            can convert the transformation matrix to this scalar and describes the
+*            scalar in further detail.
+* @param[in] sensitivity A scale factor to convert device units to g's
+*            such that g's = device_units * sensitivity / 2^30. Typically
+*            it works out to be the maximum g_value * 2^15.
+*/
+void inv_set_accel_orientation_and_scale(int orientation, long sensitivity)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_A_ORIENT;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file);
+        fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file);
+    }
+#endif
+    set_sensor_orientation_and_scale(&sensors.accel, orientation,
+                                     sensitivity);
+}
+
+/** Sets the Orientation and Sensitivity of the gyro data.
+* @param[in] orientation A scalar defining the transformation from chip mounting
+*            to the body frame. The function inv_orientation_matrix_to_scalar()
+*            can convert the transformation matrix to this scalar and describes the
+*            scalar in further detail.
+* @param[in] sensitivity A scale factor to convert device units to uT
+*            such that uT = device_units * sensitivity / 2^30. Typically
+*            it works out to be the maximum uT_value * 2^15.
+*/
+void inv_set_compass_orientation_and_scale(int orientation, long sensitivity)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_C_ORIENT;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file);
+        fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file);
+    }
+#endif
+    set_sensor_orientation_and_scale(&sensors.compass, orientation, sensitivity);
+}
+
+void inv_matrix_vector_mult(const long *A, const long *x, long *y)
+{
+    y[0] = inv_q30_mult(A[0], x[0]) + inv_q30_mult(A[1], x[1]) + inv_q30_mult(A[2], x[2]);
+    y[1] = inv_q30_mult(A[3], x[0]) + inv_q30_mult(A[4], x[1]) + inv_q30_mult(A[5], x[2]);
+    y[2] = inv_q30_mult(A[6], x[0]) + inv_q30_mult(A[7], x[1]) + inv_q30_mult(A[8], x[2]);
+}
+
+/** Takes raw data stored in the sensor, removes bias, and converts it to
+* calibrated data in the body frame. Also store raw data for body frame.
+* @param[in,out] sensor structure to modify
+* @param[in] bias bias in the mounting frame, in hardware units scaled by
+*                 2^16. Length 3.
+*/
+void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias)
+{
+    long raw32[3];
+
+    // Convert raw to calibrated
+    raw32[0] = (long)sensor->raw[0] << 15;
+    raw32[1] = (long)sensor->raw[1] << 15;
+    raw32[2] = (long)sensor->raw[2] << 15;
+
+    inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_scaled);
+
+    raw32[0] -= bias[0] >> 1;
+    raw32[1] -= bias[1] >> 1;
+    raw32[2] -= bias[2] >> 1;
+
+    inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->calibrated);
+
+    sensor->status |= INV_CALIBRATED;
+}
+
+/** Returns the current bias for the compass
+* @param[out] bias Compass bias in hardware units scaled by 2^16. In mounting frame.
+*             Length 3.
+*/
+void inv_get_compass_bias(long *bias)
+{
+    if (bias != NULL) {
+        memcpy(bias, inv_data_builder.save.compass_bias, sizeof(inv_data_builder.save.compass_bias));
+    }
+}
+
+/** Sets the compass bias
+* @param[in] bias Length 3, in body frame, in hardware units scaled by 2^16 to allow fractional bit correction.
+* @param[in] accuracy Accuracy of compass data, where 3=most accurate, and 0=least accurate.
+*/
+void inv_set_compass_bias(const long *bias, int accuracy)
+{
+    if (memcmp(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias))) {
+        memcpy(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias));
+        inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias);
+    }
+    sensors.compass.accuracy = accuracy;
+    inv_data_builder.save.compass_accuracy = accuracy;
+    inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0);
+}
+
+/** Set the state of a compass disturbance
+* @param[in] dist 1=disturbance, 0=no disturbance
+*/
+void inv_set_compass_disturbance(int dist)
+{
+    inv_data_builder.compass_disturbance = dist;
+}
+
+int inv_get_compass_disturbance(void) {
+    return inv_data_builder.compass_disturbance;
+}
+
+/** 
+ *  Sets the factory accel bias
+ *  @param[in] bias 
+ *               Accel bias in hardware units (+/- 2 gee full scale assumed) 
+ *               scaled by 2^16. In chip mounting frame. Length of 3.
+ */
+void inv_set_accel_bias(const long *bias)
+{
+    if (!bias)
+        return;
+
+    if (memcmp(inv_data_builder.save.factory_accel_bias, bias,
+               sizeof(inv_data_builder.save.factory_accel_bias))) {
+        memcpy(inv_data_builder.save.factory_accel_bias, bias,
+               sizeof(inv_data_builder.save.factory_accel_bias));
+        }
+    inv_set_message(INV_MSG_NEW_FAB_EVENT, INV_MSG_NEW_FAB_EVENT, 0);
+}
+
+/** Sets the accel accuracy.
+* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate.
+*/
+void inv_set_accel_accuracy(int accuracy)
+{
+    sensors.accel.accuracy = accuracy;
+    inv_data_builder.save.accel_accuracy = accuracy;
+}
+
+/** Sets the accel bias with control over which axis.
+* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame
+* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate.
+* @param[in] mask Mask to select axis to apply bias set.
+*/
+void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask)
+{
+    if (bias) {
+        if (mask & 1){
+            inv_data_builder.save_accel_mpl.accel_bias[0] = bias[0];
+        }
+        if (mask & 2){
+            inv_data_builder.save_accel_mpl.accel_bias[1] = bias[1];
+        }
+        if (mask & 4){
+            inv_data_builder.save_accel_mpl.accel_bias[2] = bias[2];
+        }
+
+        inv_apply_calibration(&sensors.accel, inv_data_builder.save_accel_mpl.accel_bias);
+    }
+    inv_set_accel_accuracy(accuracy);
+    inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
+}
+
+#ifdef WIN32
+/** Sends out a message to activate writing 9-axis quaternion to the DMP.
+*/
+void inv_overwrite_dmp_9quat(void)
+{
+    inv_set_message(INV_MSG_NEW_DMP_QUAT_WRITE_EVENT, INV_MSG_NEW_DMP_QUAT_WRITE_EVENT, 0);
+}
+#endif
+
+/** 
+ *  Sets the factory gyro bias
+ *  @param[in] bias 
+ *               Gyro bias in hardware units (+/- 2000 dps full scale assumed) 
+ *               scaled by 2^16. In chip mounting frame. Length of 3.
+ */
+void inv_set_gyro_bias(const long *bias)
+{
+    if (!bias)
+        return;
+
+    if (memcmp(inv_data_builder.save.factory_gyro_bias, bias,
+               sizeof(inv_data_builder.save.factory_gyro_bias))) {
+        memcpy(inv_data_builder.save.factory_gyro_bias, bias,
+               sizeof(inv_data_builder.save.factory_gyro_bias));
+    }
+    inv_set_message(INV_MSG_NEW_FGB_EVENT, INV_MSG_NEW_FGB_EVENT, 0);
+}
+
+/** 
+ *  Sets the mpl gyro bias
+ *  @param[in] bias
+ *              Gyro bias in hardware units scaled by 2^16  (+/- 2000 dps full
+ *              scale assumed). In chip mounting frame. Length 3.
+ *  @param[in] accuracy
+ *              Accuracy of bias. 0 = least accurate, 3 = most accurate.
+ */
+void inv_set_mpl_gyro_bias(const long *bias, int accuracy)
+{
+    if (bias != NULL) {
+        if (memcmp(inv_data_builder.save_mpl.gyro_bias, bias, 
+                   sizeof(inv_data_builder.save_mpl.gyro_bias))) {
+            memcpy(inv_data_builder.save_mpl.gyro_bias, bias, 
+                   sizeof(inv_data_builder.save_mpl.gyro_bias));
+            inv_apply_calibration(&sensors.gyro,
+                                  inv_data_builder.save_mpl.gyro_bias);
+        }
+    }
+    sensors.gyro.accuracy = accuracy;
+    inv_data_builder.save.gyro_accuracy = accuracy;
+
+    /* TODO: What should we do if there's no temperature data? */
+    if (sensors.temp.calibrated[0])
+        inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0];
+    else
+        /* Set to 27 deg C for now until we've got a better solution. */
+        inv_data_builder.save.gyro_temp = 27L << 16;
+    inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0);
+
+    /* TODO: this flag works around the synchronization problem seen with using
+       the user-exposed message layer to signal the temperature compensation
+       module that gyro biases were set.
+       A better, cleaner method is certainly needed. */
+    inv_data_builder.save.gyro_bias_tc_set = true;
+}
+
+/**
+ *  @internal
+ *  @brief  Return whether gyro biases were set to signal the temperature
+ *          compensation algorithm that it can collect a data point to build
+ *          the temperature slope while in no motion state.
+ *          The flag clear automatically after is read.
+ *  @return true if the flag was set, indicating gyro biases were set.
+ *          false if the flag was not set.
+ */
+int inv_get_gyro_bias_tc_set(void)
+{
+    int flag = (inv_data_builder.save.gyro_bias_tc_set == true);
+    inv_data_builder.save.gyro_bias_tc_set = false;
+    return flag;
+}
+
+
+/**
+ *  Get the mpl gyro biases
+ *  @param[in] bias
+ *              Gyro calibrated bias.
+ *              Length 3.
+ */
+void inv_get_mpl_gyro_bias(long *bias, long *temp)
+{
+    if (bias != NULL)
+        memcpy(bias, inv_data_builder.save_mpl.gyro_bias,
+               sizeof(inv_data_builder.save_mpl.gyro_bias));
+
+    if (temp != NULL)
+        temp[0] = inv_data_builder.save.gyro_temp;
+}
+
+/** Gyro Bias in the form used by the DMP.
+* @param[out] bias Gyro Bias in the form used by the DMP. It is scaled appropriately
+*             and is in the body frame as needed. If this bias is applied in the DMP
+*             then any quaternion must have the flag INV_BIAS_APPLIED set if it is a
+*             3-axis quaternion, or INV_QUAT_6AXIS if it is a 6-axis quaternion
+*/
+void inv_get_gyro_bias_dmp_units(long *bias)
+{
+    if (bias == NULL)
+        return;
+    inv_convert_to_body_with_scale(sensors.gyro.orientation, 46850825L,
+                                   inv_data_builder.save_mpl.gyro_bias, bias);
+}
+
+/* TODO: Add this information to inv_sensor_cal_t */
+/**
+ *  Get the gyro biases and temperature record from MPL
+ *  @param[in] bias
+ *              Gyro bias in hardware units scaled by 2^16.
+ *              In chip mounting frame.
+ *              Length 3.
+ */
+void inv_get_gyro_bias(long *bias)
+{
+    if (bias != NULL)
+        memcpy(bias, inv_data_builder.save.factory_gyro_bias,
+               sizeof(inv_data_builder.save.factory_gyro_bias));
+}
+
+/**
+ * Get factory accel bias mask
+ * @param[in] bias
+ *             Accel bias mask
+ *             1 is set, 0 is not set, Length 3 = x,y,z.
+ */
+int inv_get_factory_accel_bias_mask()
+{
+    long bias[3];
+	int bias_mask = 0;
+    inv_get_accel_bias(bias);
+    if (bias != NULL) {
+        int i;
+        for(i = 0; i < 3; i++) {
+            if(bias[i] != 0) {
+                bias_mask |= 1 << i;
+			} else {
+                bias_mask &= ~ (1 << i);
+			}
+        }
+    }
+	return bias_mask;
+}
+
+/**
+ * Get accel bias from MPL
+ * @param[in] bias
+ *             Accel bias in hardware units scaled by 2^16.
+ *             In chp mounting frame.
+ *             Length 3.
+ */
+void inv_get_accel_bias(long *bias)
+{
+    if (bias != NULL)
+        memcpy(bias, inv_data_builder.save.factory_accel_bias,
+               sizeof(inv_data_builder.save.factory_accel_bias));
+}
+
+/** Get Accel Bias
+* @param[out] bias Accel bias
+* @param[out] temp Temperature where 1 C = 2^16
+*/
+void inv_get_mpl_accel_bias(long *bias, long *temp)
+{
+    if (bias != NULL)
+        memcpy(bias, inv_data_builder.save_accel_mpl.accel_bias,
+               sizeof(inv_data_builder.save_accel_mpl.accel_bias));
+    if (temp != NULL)
+        temp[0] = inv_data_builder.save.accel_temp;
+}
+
+/** Accel Bias in the form used by the DMP.
+* @param[out] bias Accel Bias in the form used by the DMP. It is scaled appropriately
+*             and is in the body frame as needed.
+*/
+void inv_get_accel_bias_dmp_units(long *bias)
+{
+    if (bias == NULL)
+        return;
+    inv_convert_to_body_with_scale(sensors.accel.orientation, 536870912L,
+                                   inv_data_builder.save_accel_mpl.accel_bias, bias);
+}
+
+/**
+ *  Record new accel data for use when inv_execute_on_data() is called
+ *  @param[in]  accel 
+ *                accel data, length 3.
+ *                Calibrated data is in m/s^2 scaled by 2^16 in body frame.
+ *                Raw data is in device units in chip mounting frame.
+ *  @param[in]  status
+ *                Lower 2 bits are the accuracy, with 0 being inaccurate, and 3
+ *                being most accurate.
+ *                The upper bit INV_CALIBRATED, is set if the data was calibrated
+ *                outside MPL and it is not set if the data being passed is raw.
+ *                Raw data should be in device units, typically in a 16-bit range.
+ *  @param[in]  timestamp
+ *                Monotonic time stamp, for Android it's in nanoseconds.
+ *  @return     Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_build_accel(const long *accel, int status, inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_ACCEL;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(accel, sizeof(accel[0]), 3, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+
+    if ((status & INV_CALIBRATED) == 0) {
+        sensors.accel.raw[0] = (short)accel[0];
+        sensors.accel.raw[1] = (short)accel[1];
+        sensors.accel.raw[2] = (short)accel[2];
+        sensors.accel.status |= INV_RAW_DATA;
+        inv_apply_calibration(&sensors.accel, inv_data_builder.save_accel_mpl.accel_bias);
+    } else {
+        sensors.accel.calibrated[0] = accel[0];
+        sensors.accel.calibrated[1] = accel[1];
+        sensors.accel.calibrated[2] = accel[2];
+        sensors.accel.status |= INV_CALIBRATED;
+        sensors.accel.accuracy = status & 3;
+        inv_data_builder.save.accel_accuracy = status & 3;
+    }
+    sensors.accel.status |= INV_NEW_DATA | INV_SENSOR_ON;
+    sensors.accel.timestamp_prev = sensors.accel.timestamp;
+    sensors.accel.timestamp = timestamp;
+
+    return INV_SUCCESS;
+}
+
+/** Record new gyro data and calls inv_execute_on_data() if previous
+* sample has not been processed.
+* @param[in] gyro Data is in device units. Length 3.
+* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds.
+* @param[out] executed Set to 1 if data processing was done.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_GYRO;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(gyro, sizeof(gyro[0]), 3, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+
+    memcpy(sensors.gyro.raw, gyro, 3 * sizeof(short));
+    sensors.gyro.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
+    sensors.gyro.timestamp_prev = sensors.gyro.timestamp;
+    sensors.gyro.timestamp = timestamp;
+    inv_apply_calibration(&sensors.gyro, inv_data_builder.save_mpl.gyro_bias);
+
+    return INV_SUCCESS;
+}
+
+/** Record new compass data for use when inv_execute_on_data() is called
+* @param[in] compass Compass data, if it was calibrated outside MPL, the units are uT scaled by 2^16.
+*            Length 3.
+* @param[in] status Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 being most accurate.
+*            The upper bit INV_CALIBRATED, is set if the data was calibrated outside MPL and it is
+*            not set if the data being passed is raw. Raw data should be in device units, typically
+*            in a 16-bit range.
+* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds.
+* @param[out] executed Set to 1 if data processing was done.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_build_compass(const long *compass, int status,
+                              inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_COMPASS;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(compass, sizeof(compass[0]), 3, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+
+    if ((status & INV_CALIBRATED) == 0) {
+        long data[3];
+        inv_set_compass_soft_iron_input_data(compass);
+        inv_get_compass_soft_iron_output_data(data);
+        sensors.compass.raw[0] = (short)data[0];
+        sensors.compass.raw[1] = (short)data[1];
+        sensors.compass.raw[2] = (short)data[2];
+        inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias);
+        sensors.compass.status |= INV_RAW_DATA;
+    } else {
+        sensors.compass.calibrated[0] = compass[0];
+        sensors.compass.calibrated[1] = compass[1];
+        sensors.compass.calibrated[2] = compass[2];
+        sensors.compass.status |= INV_CALIBRATED;
+        sensors.compass.accuracy = status & 3;
+        inv_data_builder.save.compass_accuracy = status & 3;
+    }
+    sensors.compass.timestamp_prev = sensors.compass.timestamp;
+    sensors.compass.timestamp = timestamp;
+    sensors.compass.status |= INV_NEW_DATA | INV_SENSOR_ON;
+
+    return INV_SUCCESS;
+}
+
+/** Record new temperature data for use when inv_execute_on_data() is called.
+ *  @param[in]  temp Temperature data in q16 format.
+ *  @param[in]  timestamp   Monotonic time stamp; for Android it's in
+ *                          nanoseconds.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_build_temp(const long temp, inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_TEMPERATURE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&temp, sizeof(temp), 1, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.temp.calibrated[0] = temp;
+    sensors.temp.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
+    sensors.temp.timestamp_prev = sensors.temp.timestamp;
+    sensors.temp.timestamp = timestamp;
+    /* TODO: Apply scale, remove offset. */
+
+    return INV_SUCCESS;
+}
+/** quaternion data
+* @param[in] quat Quaternion data. 2^30 = 1.0 or 2^14=1 for 16-bit data.
+*                 Real part first. Length 4.
+* @param[in] status number of axis, 16-bit or 32-bit
+*            set INV_QUAT_3ELEMENT if input quaternion has only 3 elements (no scalar).
+*            inv_compute_scalar_part() assumes 32-bit data.  If using 16-bit quaternion,
+*            shift 16 bits first before calling this function.
+* @param[in] timestamp
+* @param[in]  timestamp   Monotonic time stamp; for Android it's in
+*                         nanoseconds.
+* @param[out] executed Set to 1 if data processing was done.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_QUAT;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(quat, sizeof(quat[0]), 4, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+
+    /* Android version DMP does not have scalar part */
+    if (status & INV_QUAT_3ELEMENT) {
+        long resultQuat[4];
+        MPL_LOGV("3q: %ld,%ld,%ld\n", quat[0], quat[1], quat[2]);
+        inv_compute_scalar_part(quat, resultQuat);
+        MPL_LOGV("4q: %ld,%ld,%ld,%ld\n", resultQuat[0], resultQuat[1], resultQuat[2], resultQuat[3]);
+        memcpy(sensors.quat.raw, resultQuat, sizeof(sensors.quat.raw));
+    } else {
+        memcpy(sensors.quat.raw, quat, sizeof(sensors.quat.raw));
+    }
+    sensors.quat.timestamp_prev = sensors.quat.timestamp;
+    sensors.quat.timestamp = timestamp;
+    sensors.quat.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
+    sensors.quat.status |= (INV_QUAT_3AXIS & status);
+    sensors.quat.status |= (INV_QUAT_6AXIS & status);
+    sensors.quat.status |= (INV_QUAT_9AXIS & status);
+    sensors.quat.status |= (INV_BIAS_APPLIED & status);
+    sensors.quat.status |= (INV_DMP_BIAS_APPLIED & status);
+    sensors.quat.status |= (INV_QUAT_3ELEMENT & status);
+    MPL_LOGV("quat.status: %d", sensors.quat.status);
+    if (sensors.quat.status & (INV_QUAT_9AXIS | INV_QUAT_6AXIS)) {
+        // Set quaternion
+        inv_store_gaming_quaternion(quat, timestamp);
+    }
+    if (sensors.quat.status & INV_QUAT_9AXIS) {
+        long identity[4] = {(1L<<30), 0, 0, 0};
+        inv_set_compass_correction(identity, timestamp);
+    }
+    return INV_SUCCESS;
+}
+
+inv_error_t inv_build_pressure(const long pressure, int status, inv_time_t timestamp)
+{
+    sensors.pressure.status |= INV_NEW_DATA;
+    return INV_SUCCESS;
+}
+
+/** This should be called when the accel has been turned off. This is so
+* that we will know if the data is contiguous.
+*/
+void inv_accel_was_turned_off()
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_COMPASS_OFF;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.accel.status = 0;
+}
+
+/** This should be called when the compass has been turned off. This is so
+* that we will know if the data is contiguous.
+*/
+void inv_compass_was_turned_off()
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_COMPASS_OFF;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.compass.status = 0;
+}
+
+/** This should be called when the quaternion data from the DMP has been turned off. This is so
+* that we will know if the data is contiguous.
+*/
+void inv_quaternion_sensor_was_turned_off(void)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_QUAT_OFF;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.quat.status = 0;
+}
+
+/** This should be called when the gyro has been turned off. This is so
+* that we will know if the data is contiguous.
+*/
+void inv_gyro_was_turned_off()
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_GYRO_OFF;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.gyro.status = 0;
+}
+
+/** This should be called when the temperature sensor has been turned off.
+ *  This is so that we will know if the data is contiguous.
+ */
+void inv_temperature_was_turned_off()
+{
+    sensors.temp.status = 0;
+}
+
+/** Registers to receive a callback when there is new sensor data.
+* @internal
+* @param[in] func Function pointer to receive callback when there is new sensor data
+* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority
+*            numbers must be unique.
+* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be
+*            a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW =
+*            gyro data, INV_MAG_NEW = compass data. So passing in
+*            INV_ACCEL_NEW | INV_MAG_NEW, a
+*            callback would be generated if there was new magnetomer data OR new accel data.
+*/
+inv_error_t inv_register_data_cb(
+    inv_error_t (*func)(struct inv_sensor_cal_t *data),
+    int priority, int sensor_type)
+{
+    inv_error_t result = INV_SUCCESS;
+    int kk, nn;
+
+    // Make sure we haven't registered this function already
+    // Or used the same priority
+    for (kk = 0; kk < inv_data_builder.num_cb; ++kk) {
+        if ((inv_data_builder.process[kk].func == func) ||
+                (inv_data_builder.process[kk].priority == priority)) {
+            return INV_ERROR_INVALID_PARAMETER;    //fixme give a warning
+        }
+    }
+
+    // Make sure we have not filled up our number of allowable callbacks
+    if (inv_data_builder.num_cb <= INV_MAX_DATA_CB - 1) {
+        kk = 0;
+        if (inv_data_builder.num_cb != 0) {
+            // set kk to be where this new callback goes in the array
+            while ((kk < inv_data_builder.num_cb) &&
+                    (inv_data_builder.process[kk].priority < priority)) {
+                kk++;
+            }
+            if (kk != inv_data_builder.num_cb) {
+                // We need to move the others
+                for (nn = inv_data_builder.num_cb; nn > kk; --nn) {
+                    inv_data_builder.process[nn] =
+                        inv_data_builder.process[nn - 1];
+                }
+            }
+        }
+        // Add new callback
+        inv_data_builder.process[kk].func = func;
+        inv_data_builder.process[kk].priority = priority;
+        inv_data_builder.process[kk].data_required = sensor_type;
+        inv_data_builder.num_cb++;
+    } else {
+        MPL_LOGE("Unable to add feature callback as too many were already registered\n");
+        result = INV_ERROR_MEMORY_EXAUSTED;
+    }
+
+    return result;
+}
+
+/** Unregisters the callback that happens when new sensor data is received.
+* @internal
+* @param[in] func Function pointer to receive callback when there is new sensor data
+* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority
+*            numbers must be unique.
+* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be
+*            a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW =
+*            gyro data, INV_MAG_NEW = compass data. So passing in
+*            INV_ACCEL_NEW | INV_MAG_NEW, a
+*            callback would be generated if there was new magnetomer data OR new accel data.
+*/
+inv_error_t inv_unregister_data_cb(
+    inv_error_t (*func)(struct inv_sensor_cal_t *data))
+{
+    int kk, nn;
+
+    for (kk = 0; kk < inv_data_builder.num_cb; ++kk) {
+        if (inv_data_builder.process[kk].func == func) {
+            // Delete this callback
+            for (nn = kk + 1; nn < inv_data_builder.num_cb; ++nn) {
+                inv_data_builder.process[nn - 1] =
+                    inv_data_builder.process[nn];
+            }
+            inv_data_builder.num_cb--;
+            return INV_SUCCESS;
+        }
+    }
+
+    return INV_SUCCESS;    // We did not find the callback
+}
+
+/** After at least one of inv_build_gyro(), inv_build_accel(), or
+* inv_build_compass() has been called, this function should be called.
+* It will process the data it has received and update all the internal states
+* and features that have been turned on.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_execute_on_data(void)
+{
+    inv_error_t result, first_error;
+    int kk;
+
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_EXECUTE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+    }
+#endif
+    // Determine what new data we have
+    inv_data_builder.mode = 0;
+    if (sensors.gyro.status & INV_NEW_DATA)
+        inv_data_builder.mode |= INV_GYRO_NEW;
+    if (sensors.accel.status & INV_NEW_DATA)
+        inv_data_builder.mode |= INV_ACCEL_NEW;
+    if (sensors.compass.status & INV_NEW_DATA)
+        inv_data_builder.mode |= INV_MAG_NEW;
+    if (sensors.temp.status & INV_NEW_DATA)
+        inv_data_builder.mode |= INV_TEMP_NEW;
+    if (sensors.quat.status & INV_NEW_DATA)
+        inv_data_builder.mode |= INV_QUAT_NEW;
+    if (sensors.pressure.status & INV_NEW_DATA)
+        inv_data_builder.mode |= INV_PRESSURE_NEW;
+
+    first_error = INV_SUCCESS;
+
+    for (kk = 0; kk < inv_data_builder.num_cb; ++kk) {
+        if (inv_data_builder.mode & inv_data_builder.process[kk].data_required) {
+            result = inv_data_builder.process[kk].func(&sensors);
+            if (result && !first_error) {
+                first_error = result;
+            }
+        }
+    }
+
+    inv_set_contiguous();
+
+    return first_error;
+}
+
+/** Cleans up status bits after running all the callbacks. It sets the contiguous flag.
+*
+*/
+static void inv_set_contiguous(void)
+{
+    inv_time_t current_time = 0;
+    if (sensors.gyro.status & INV_NEW_DATA) {
+        sensors.gyro.status |= INV_CONTIGUOUS;
+        current_time = sensors.gyro.timestamp;
+    }
+    if (sensors.accel.status & INV_NEW_DATA) {
+        sensors.accel.status |= INV_CONTIGUOUS;
+        current_time = MAX(current_time, sensors.accel.timestamp);
+    }
+    if (sensors.compass.status & INV_NEW_DATA) {
+        sensors.compass.status |= INV_CONTIGUOUS;
+        current_time = MAX(current_time, sensors.compass.timestamp);
+    }
+    if (sensors.temp.status & INV_NEW_DATA) {
+        sensors.temp.status |= INV_CONTIGUOUS;
+        current_time = MAX(current_time, sensors.temp.timestamp);
+    }
+    if (sensors.quat.status & INV_NEW_DATA) {
+        sensors.quat.status |= INV_CONTIGUOUS;
+        current_time = MAX(current_time, sensors.quat.timestamp);
+    }
+
+#if 0
+    /* See if sensors are still on. These should be turned off by inv_*_was_turned_off()
+     * type functions. This is just in case that breaks down. We make sure
+     * all the data is within 2 seconds of the newest piece of data*/
+    if (inv_delta_time_ms(current_time, sensors.gyro.timestamp) >= 2000)
+        inv_gyro_was_turned_off();
+    if (inv_delta_time_ms(current_time, sensors.accel.timestamp) >= 2000)
+        inv_accel_was_turned_off();
+    if (inv_delta_time_ms(current_time, sensors.compass.timestamp) >= 2000)
+        inv_compass_was_turned_off();
+    /* TODO: Temperature might not need to be read this quickly. */
+    if (inv_delta_time_ms(current_time, sensors.temp.timestamp) >= 2000)
+        inv_temperature_was_turned_off();
+#endif
+
+    /* clear bits */
+    sensors.gyro.status &= ~INV_NEW_DATA;
+    sensors.accel.status &= ~INV_NEW_DATA;
+    sensors.compass.status &= ~INV_NEW_DATA;
+    sensors.temp.status &= ~INV_NEW_DATA;
+    sensors.quat.status &= ~INV_NEW_DATA;
+    sensors.pressure.status &= ~INV_NEW_DATA;
+}
+
+/** Gets a whole set of accel data including data, accuracy and timestamp.
+ * @param[out] data Accel Data where 1g = 2^16
+ * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
+ * @param[out] timestamp The timestamp of the data sample.
+*/
+void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+    if (data != NULL) {
+        memcpy(data, sensors.accel.calibrated, sizeof(sensors.accel.calibrated));
+    }
+    if (timestamp != NULL) {
+        *timestamp = sensors.accel.timestamp;
+    }
+    if (accuracy != NULL) {
+        *accuracy = sensors.accel.accuracy;
+    }
+}
+
+/** Gets a whole set of gyro data including data, accuracy and timestamp.
+ * @param[out] data Gyro Data where 1 dps = 2^16
+ * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
+ * @param[out] timestamp The timestamp of the data sample.
+*/
+void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+    memcpy(data, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated));
+    if (timestamp != NULL) {
+        *timestamp = sensors.gyro.timestamp;
+    }
+    if (accuracy != NULL) {
+        *accuracy = sensors.gyro.accuracy;
+    }
+}
+
+/** Gets a whole set of gyro raw data including data, accuracy and timestamp.
+ * @param[out] data Gyro Data where 1 dps = 2^16
+ * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
+ * @param[out] timestamp The timestamp of the data sample.
+*/
+void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+    memcpy(data, sensors.gyro.raw_scaled, sizeof(sensors.gyro.raw_scaled));
+    if (timestamp != NULL) {
+        *timestamp = sensors.gyro.timestamp;
+    }
+    if (accuracy != NULL) {
+        *accuracy = 0;
+    }
+}
+
+/** Get's latest gyro data.
+* @param[out] gyro Gyro Data, Length 3. 1 dps = 2^16.
+*/
+void inv_get_gyro(long *gyro)
+{
+    memcpy(gyro, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated));
+}
+
+/** Gets a whole set of compass data including data, accuracy and timestamp.
+ * @param[out] data Compass Data where 1 uT = 2^16
+ * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
+ * @param[out] timestamp The timestamp of the data sample.
+*/
+void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+    memcpy(data, sensors.compass.calibrated, sizeof(sensors.compass.calibrated));
+    if (timestamp != NULL) {
+        *timestamp = sensors.compass.timestamp;
+    }
+    if (accuracy != NULL) {
+        if (inv_data_builder.compass_disturbance)
+            *accuracy = 0;
+        else
+            *accuracy = sensors.compass.accuracy;
+    }
+}
+
+/** Gets a whole set of compass raw data including data, accuracy and timestamp.
+ * @param[out] data Compass Data where 1 uT = 2^16
+ * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
+ * @param[out] timestamp The timestamp of the data sample.
+*/
+void inv_get_compass_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+    memcpy(data, sensors.compass.raw_scaled, sizeof(sensors.compass.raw_scaled));
+    if (timestamp != NULL) {
+        *timestamp = sensors.compass.timestamp;
+    }
+    //per Michele, since data is raw, accuracy should = 0
+    *accuracy = 0;
+}
+
+/** Gets a whole set of temperature data including data, accuracy and timestamp.
+ *  @param[out] data        Temperature data where 1 degree C = 2^16
+ *  @param[out] accuracy    0 to 3, where 3 is most accurate.
+ *  @param[out] timestamp   The timestamp of the data sample.
+ */
+void inv_get_temp_set(long *data, int *accuracy, inv_time_t *timestamp)
+{
+    data[0] = sensors.temp.calibrated[0];
+    if (timestamp)
+        *timestamp = sensors.temp.timestamp;
+    if (accuracy)
+        *accuracy = sensors.temp.accuracy;
+}
+
+/** Returns accuracy of gyro.
+ * @return Accuracy of gyro with 0 being not accurate, and 3 being most accurate.
+*/
+int inv_get_gyro_accuracy(void)
+{
+    return sensors.gyro.accuracy;
+}
+
+/** Returns accuracy of compass.
+ * @return Accuracy of compass with 0 being not accurate, and 3 being most accurate.
+*/
+int inv_get_mag_accuracy(void)
+{
+    if (inv_data_builder.compass_disturbance)
+        return 0;
+    return sensors.compass.accuracy;
+}
+
+/** Returns accuracy of accel.
+ * @return Accuracy of accel with 0 being not accurate, and 3 being most accurate.
+*/
+int inv_get_accel_accuracy(void)
+{
+    return sensors.accel.accuracy;
+}
+
+inv_error_t inv_get_gyro_orient(int *orient)
+{
+    *orient = sensors.gyro.orientation;
+    return 0;
+}
+
+inv_error_t inv_get_accel_orient(int *orient)
+{
+    *orient = sensors.accel.orientation;
+    return 0;
+}
+
+/*======================================================================*/
+/*   compass soft iron module                                           */
+/*======================================================================*/
+
+/** Gets the 3x3 compass transform matrix in 32 bit Q30 fixed point format.
+ * @param[out] the pointer of the 3x3 matrix in Q30 format
+*/
+void inv_get_compass_soft_iron_matrix_d(long *matrix) {
+    int i;
+    for (i=0; i<9; i++)  {
+        matrix[i] = sensors.soft_iron.matrix_d[i];
+    }
+}
+
+/** Sets the 3x3 compass transform matrix in 32 bit Q30 fixed point format.
+ * @param[in] the pointer of the 3x3 matrix in Q30 format
+*/
+void inv_set_compass_soft_iron_matrix_d(long *matrix)  {
+    int i;
+    for (i=0; i<9; i++)  {
+        // set the floating point matrix
+        sensors.soft_iron.matrix_d[i] = matrix[i];
+        // convert to Q30 format
+        sensors.soft_iron.matrix_f[i] = inv_q30_to_float(matrix[i]);
+    }
+}
+/** Gets the 3x3 compass transform matrix in 32 bit floating point format.
+ * @param[out] the pointer of the 3x3 matrix in floating point format
+*/
+void inv_get_compass_soft_iron_matrix_f(float *matrix)  {
+    int i;
+    for (i=0; i<9; i++)  {
+        matrix[i] = sensors.soft_iron.matrix_f[i];
+    }
+}
+/** Sets the 3x3 compass transform matrix in 32 bit floating point format.
+ * @param[in] the pointer of the 3x3 matrix in floating point format
+*/
+void inv_set_compass_soft_iron_matrix_f(float *matrix)   {
+    int i;
+    for (i=0; i<9; i++)  {
+        // set the floating point matrix
+        sensors.soft_iron.matrix_f[i] = matrix[i];
+        // convert to Q30 format
+        sensors.soft_iron.matrix_d[i] = (long )(matrix[i]*ROT_MATRIX_SCALE_LONG);
+    }
+}
+
+/** This subroutine gets the fixed point Q30 compass data after the soft iron transformation.
+ * @param[out] the pointer of the 3x1 vector compass data in MPL format
+*/
+void inv_get_compass_soft_iron_output_data(long *data) {
+    int i;
+    for (i=0; i<3; i++)  {
+        data[i] = sensors.soft_iron.trans[i];
+    }
+}
+/** This subroutine gets the fixed point Q30 compass data before the soft iron transformation.
+ * @param[out] the pointer of the 3x1 vector compass data in MPL format
+*/
+void inv_get_compass_soft_iron_input_data(long *data)  {
+    int i;
+    for (i=0; i<3; i++)  {
+        data[i] = sensors.soft_iron.raw[i];
+    }
+}
+/** This subroutine sets the compass raw data for the soft iron transformation.
+ * @param[int] the pointer of the 3x1 vector compass raw data in MPL format
+*/
+void inv_set_compass_soft_iron_input_data(const long *data)  {
+    int i;
+    for (i=0; i<3; i++)  {
+        sensors.soft_iron.raw[i] = data[i];
+    }
+    if (sensors.soft_iron.enable == 1)  {
+        mlMatrixVectorMult(sensors.soft_iron.matrix_d, data, sensors.soft_iron.trans);
+    } else {
+        for (i=0; i<3; i++)  {
+            sensors.soft_iron.trans[i] = data[i];
+        }
+    }
+}
+
+/** This subroutine resets the the soft iron transformation to unity matrix and
+ * disable the soft iron transformation process by default.
+*/
+void inv_reset_compass_soft_iron_matrix(void)  {
+    int i;
+    for (i=0; i<9; i++) {
+        sensors.soft_iron.matrix_f[i] = 0.0f;
+    }
+
+    memset(&sensors.soft_iron.matrix_d,0,sizeof(sensors.soft_iron.matrix_d));
+
+    for (i=0; i<3; i++)  {
+        // set the floating point matrix
+        sensors.soft_iron.matrix_f[i*4] = 1.0;
+        // set the fixed point matrix
+        sensors.soft_iron.matrix_d[i*4] = ROT_MATRIX_SCALE_LONG;
+    }
+
+    inv_disable_compass_soft_iron_matrix();
+}
+
+/** This subroutine enables the the soft iron transformation process.
+*/
+void inv_enable_compass_soft_iron_matrix(void)   {
+    sensors.soft_iron.enable = 1;
+}
+
+/** This subroutine disables the the soft iron transformation process.
+*/
+void inv_disable_compass_soft_iron_matrix(void)   {
+    sensors.soft_iron.enable = 0;
+}
+
+/**
+ * @}
+ */
diff --git a/6515/libsensors_iio/software/core/mllite/data_builder.h b/6515/libsensors_iio/software/core/mllite/data_builder.h
new file mode 100644
index 0000000..542f76b
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/data_builder.h
@@ -0,0 +1,333 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#ifndef INV_DATA_BUILDER_H__
+#define INV_DATA_BUILDER_H__
+
+#include <stdio.h>
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Uncomment this flag to enable playback debug and record or playback scenarios
+//#define INV_PLAYBACK_DBG
+
+/** This is a new sample of accel data */
+#define INV_ACCEL_NEW 1
+/** This is a new sample of gyro data */
+#define INV_GYRO_NEW 2
+/** This is a new sample of compass data */
+#define INV_MAG_NEW 4
+/** This is a new sample of temperature data */
+#define INV_TEMP_NEW 8
+/** This is a new sample of quaternion data */
+#define INV_QUAT_NEW 16
+/** This is a new sample of pressure data */
+#define INV_PRESSURE_NEW 32
+
+/** Set if the data is contiguous. Typically not set if a sample was skipped */
+#define INV_CONTIGUOUS 16
+/** Set if the calibrated data has been solved for */
+#define INV_CALIBRATED 32
+/** INV_NEW_DATA set for a new set of data, cleared if not available. */
+#define INV_NEW_DATA 64
+/** Set if raw data exists */
+#define INV_RAW_DATA 128
+/** Set if the sensor is on */
+#define INV_SENSOR_ON 256
+/** Set if quaternion has bias correction applied */
+#define INV_BIAS_APPLIED 512
+/** Set if quaternion is 6-axis from DMP */
+#define INV_QUAT_6AXIS 1024
+/** Set if quaternion is 9 axis from DMP */
+#define INV_QUAT_9AXIS 2048
+/** Set if quaternion is 3-axis from DMP */
+#define INV_QUAT_3AXIS 4096
+/** Set if DMP has applied bias */
+#define INV_DMP_BIAS_APPLIED 8192
+/** Set if quaternion is 3 elements (android only) */
+#define INV_QUAT_3ELEMENT 16384
+
+#define INV_PRIORITY_MOTION_NO_MOTION          100
+#define INV_PRIORITY_GYRO_TC                   150
+#define INV_PRIORITY_QUATERNION_GYRO_ACCEL     200
+#define INV_PRIORITY_QUATERNION_NO_GYRO        250
+#define INV_PRIORITY_MAGNETIC_DISTURBANCE      300
+#define INV_PRIORITY_HEADING_FROM_GYRO         350
+#define INV_PRIORITY_COMPASS_BIAS_W_GYRO       375
+#define INV_PRIORITY_COMPASS_VECTOR_CAL        400
+#define INV_PRIORITY_COMPASS_ADV_BIAS          500
+#define INV_PRIORITY_9_AXIS_FUSION             600
+#define INV_PRIORITY_9_AXIS_FUSION_LIGHT       650
+#define INV_PRIORITY_QUATERNION_ADJUST_9_AXIS  700
+#define INV_PRIORITY_QUATERNION_ACCURACY       750
+#define INV_PRIORITY_RESULTS_HOLDER            800
+#define INV_PRIORITY_INUSE_AUTO_CALIBRATION    850
+#define INV_PRIORITY_HAL_OUTPUTS               900
+#define INV_PRIORITY_GLYPH                     950
+#define INV_PRIORITY_SHAKE                     975
+#define INV_PRIORITY_SM                        1000
+
+struct inv_single_sensor_t {
+    /** Orientation Descriptor. Describes how to go from the mounting frame to the body frame when
+    * the rotation matrix could be thought of only having elements of 0,1,-1.
+    * 2 bits are used to describe the column of the 1 or -1 and the 3rd bit is used for the sign.
+    * Bit 8 is sign of +/- 1 in third row. Bit 6-7 is column of +/-1 in third row.
+    * Bit 5 is sign of +/- 1 in second row. Bit 3-4 is column of +/-1 in second row.
+    * Bit 2 is sign of +/- 1 in first row. Bit 0-1 is column of +/-1 in first row.
+    */
+    int orientation;
+    /** The raw data in raw data units in the mounting frame */
+    short raw[3];
+    /** Raw data in body frame */
+    long raw_scaled[3];
+    /** Calibrated data */
+    long calibrated[3];
+    long sensitivity;
+    /** Sample rate in microseconds */
+    long sample_rate_us;
+    long sample_rate_ms;
+    /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample
+    * skipped due to power savings turning off this sensor.
+    * INV_NEW_DATA set for a new set of data, cleared if not available.
+    * INV_CALIBRATED_SET if calibrated data has been solved for */
+    int status;
+    /** 0 to 3 for how well sensor data and biases are known. 3 is most accurate. */
+    int accuracy;
+    inv_time_t timestamp;
+    inv_time_t timestamp_prev;
+    /** Bandwidth in Hz */
+    int bandwidth;
+};
+
+struct inv_quat_sensor_t {
+    long raw[4];
+    /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample
+    * skipped due to power savings turning off this sensor.
+    * INV_NEW_DATA set for a new set of data, cleared if not available.
+    * INV_CALIBRATED_SET if calibrated data has been solved for */
+    int status;
+    inv_time_t timestamp;
+    inv_time_t timestamp_prev;
+    long sample_rate_us;
+    long sample_rate_ms;
+};
+
+struct inv_soft_iron_t {
+    long raw[3];
+    long trans[3];
+    long matrix_d[9];  // Q30 format fixed point. The dynamic range is (-2.0 to 2.0);
+    float matrix_f[9];
+
+    int enable;
+};
+
+struct inv_sensor_cal_t {
+    struct inv_single_sensor_t gyro;
+    struct inv_single_sensor_t accel;
+    struct inv_single_sensor_t compass;
+    struct inv_single_sensor_t temp;
+    struct inv_quat_sensor_t quat;
+    struct inv_single_sensor_t pressure;
+    struct inv_soft_iron_t soft_iron;
+    /** Combinations of INV_GYRO_NEW, INV_ACCEL_NEW, INV_MAG_NEW to indicate
+    * which data is a new sample as these data points may have different sample rates.
+    */
+    int status;
+};
+
+// Useful for debug record and playback
+typedef enum {
+    RD_NO_DEBUG,
+    RD_RECORD,
+    RD_PLAYBACK
+} rd_dbg_mode;
+
+typedef enum {
+    PLAYBACK_DBG_TYPE_GYRO,
+    PLAYBACK_DBG_TYPE_ACCEL,
+    PLAYBACK_DBG_TYPE_COMPASS,
+    PLAYBACK_DBG_TYPE_TEMPERATURE,
+    PLAYBACK_DBG_TYPE_EXECUTE,
+    PLAYBACK_DBG_TYPE_A_ORIENT,
+    PLAYBACK_DBG_TYPE_G_ORIENT,
+    PLAYBACK_DBG_TYPE_C_ORIENT,
+    PLAYBACK_DBG_TYPE_A_SAMPLE_RATE,
+    PLAYBACK_DBG_TYPE_C_SAMPLE_RATE,
+    PLAYBACK_DBG_TYPE_G_SAMPLE_RATE,
+    PLAYBACK_DBG_TYPE_GYRO_OFF,
+    PLAYBACK_DBG_TYPE_ACCEL_OFF,
+    PLAYBACK_DBG_TYPE_COMPASS_OFF,
+    PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE,
+    PLAYBACK_DBG_TYPE_QUAT,
+    PLAYBACK_DBG_TYPE_QUAT_OFF
+} inv_rd_dbg_states;
+
+/** Change this key if the definition of the struct inv_db_save_t changes.
+    Previous keys: 53394, 53395, 53396 */
+#define INV_DB_SAVE_KEY (53397)
+
+#define INV_DB_SAVE_MPL_KEY (50001)
+#define INV_DB_SAVE_ACCEL_MPL_KEY (50002)
+
+struct inv_db_save_t {
+    /** compass Bias in chip frame, hardware units scaled by 2^16. */
+    long compass_bias[3];
+    /** gyro factory bias in chip frame, hardware units scaled by 2^16,
+        +/- 2000 dps full scale. */
+    long factory_gyro_bias[3];
+    /** accel factory bias in chip frame, hardware units scaled by 2^16, 
+        +/- 2 gee full scale. */
+    long factory_accel_bias[3];
+    /** temperature when factory_gyro_bias was stored. */
+    long gyro_temp;
+    /** flag to indicate temperature compensation that biases where stored. */
+    int gyro_bias_tc_set;
+    /** temperature when accel bias was stored. */
+    long accel_temp;
+    long gyro_temp_slope[3];
+    /** sensor accuracies */
+    int gyro_accuracy;
+    int accel_accuracy;
+    int compass_accuracy;
+};
+
+struct inv_db_save_mpl_t {
+    /** gyro bias in chip frame, hardware units scaled by 2^16, +/- 2000 dps
+        full scale */
+    long gyro_bias[3];
+};
+
+struct inv_db_save_accel_mpl_t {
+    /** accel bias in chip frame, hardware units scaled by 2^16, +/- 2 gee
+        full scale */
+    long accel_bias[3];
+};
+
+/** Maximum number of data callbacks that are supported. Safe to increase if needed.*/
+#define INV_MAX_DATA_CB 20
+
+#ifdef INV_PLAYBACK_DBG
+void inv_turn_on_data_logging(FILE *file);
+void inv_turn_off_data_logging();
+#endif
+
+void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity);
+void inv_set_accel_orientation_and_scale(int orientation,
+        long sensitivity);
+void inv_set_compass_orientation_and_scale(int orientation,
+        long sensitivity);
+void inv_set_gyro_sample_rate(long sample_rate_us);
+void inv_set_compass_sample_rate(long sample_rate_us);
+void inv_set_quat_sample_rate(long sample_rate_us);
+void inv_set_accel_sample_rate(long sample_rate_us);
+void inv_set_gyro_bandwidth(int bandwidth_hz);
+void inv_set_accel_bandwidth(int bandwidth_hz);
+void inv_set_compass_bandwidth(int bandwidth_hz);
+
+void inv_get_gyro_sample_rate_ms(long *sample_rate_ms);
+void inv_get_accel_sample_rate_ms(long *sample_rate_ms);
+void inv_get_compass_sample_rate_ms(long *sample_rate_ms);
+
+inv_error_t inv_register_data_cb(inv_error_t (*func)
+                                 (struct inv_sensor_cal_t * data), int priority,
+                                 int sensor_type);
+inv_error_t inv_unregister_data_cb(inv_error_t (*func)
+                                   (struct inv_sensor_cal_t * data));
+
+inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp);
+inv_error_t inv_build_compass(const long *compass, int status,
+                                  inv_time_t timestamp);
+inv_error_t inv_build_accel(const long *accel, int status,
+                            inv_time_t timestamp);
+inv_error_t inv_build_temp(const long temp, inv_time_t timestamp);
+inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp);
+inv_error_t inv_build_pressure(const long pressure, int status, inv_time_t timestamp);
+inv_error_t inv_execute_on_data(void);
+
+void inv_get_compass_bias(long *bias);
+
+void inv_set_compass_bias(const long *bias, int accuracy);
+void inv_set_compass_disturbance(int dist);
+void inv_set_gyro_bias(const long *bias);
+void inv_set_mpl_gyro_bias(const long *bias, int accuracy);
+void inv_set_accel_bias(const long *bias);
+void inv_set_mpl_accel_bias(const long *bias, int accuracy);
+void inv_set_accel_accuracy(int accuracy);
+void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask);
+
+void inv_get_compass_soft_iron_matrix_d(long *matrix);
+void inv_set_compass_soft_iron_matrix_d(long *matrix);
+
+void inv_get_compass_soft_iron_matrix_f(float *matrix);
+void inv_set_compass_soft_iron_matrix_f(float *matrix);
+
+void inv_get_compass_soft_iron_output_data(long *data);
+void inv_get_compass_soft_iron_input_data(long *data);
+void inv_set_compass_soft_iron_input_data(const long *data);
+
+void inv_reset_compass_soft_iron_matrix(void);
+void inv_enable_compass_soft_iron_matrix(void);
+void inv_disable_compass_soft_iron_matrix(void);
+
+void inv_get_mpl_gyro_bias(long *bias, long *temp);
+void inv_get_gyro_bias(long *bias);
+void inv_get_gyro_bias_dmp_units(long *bias);
+int inv_get_factory_accel_bias_mask();
+void inv_get_mpl_accel_bias(long *bias, long *temp);
+void inv_get_accel_bias(long *bias);
+void inv_get_accel_bias_dmp_units(long *bias);
+
+void inv_gyro_was_turned_off(void);
+void inv_accel_was_turned_off(void);
+void inv_compass_was_turned_off(void);
+void inv_quaternion_sensor_was_turned_off(void);
+inv_error_t inv_init_data_builder(void);
+long inv_get_gyro_sensitivity(void);
+long inv_get_accel_sensitivity(void);
+long inv_get_compass_sensitivity(void);
+
+void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t * timestamp);
+void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t * timestamp);
+void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t * timestamp);
+void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t * timestamp);
+void inv_get_compass_set_raw(long *data, int8_t *accuracy, inv_time_t * timestamp);
+
+void inv_get_gyro(long *gyro);
+
+int inv_get_gyro_accuracy(void);
+int inv_get_accel_accuracy(void);
+int inv_get_mag_accuracy(void);
+void inv_get_raw_compass(short *raw);
+
+int inv_get_compass_on(void);
+int inv_get_gyro_on(void);
+int inv_get_accel_on(void);
+
+inv_time_t inv_get_last_timestamp(void);
+int inv_get_compass_disturbance(void);
+
+// new DMP cal functions
+inv_error_t inv_get_gyro_orient(int *orient);
+inv_error_t inv_get_accel_orient(int *orient);
+
+#ifdef WIN32
+void inv_overwrite_dmp_9quat(void);
+#endif
+
+// internal
+int inv_get_gyro_bias_tc_set(void);
+int inv_get_9_axis_timestamp(long sample_rate_us, inv_time_t *ts);
+int inv_get_6_axis_gyro_accel_timestamp(long sample_rate_us, inv_time_t *ts);
+int inv_get_6_axis_compass_accel_timestamp(long sample_rate_us, inv_time_t *ts);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* INV_DATA_BUILDER_H__ */
diff --git a/6515/libsensors_iio/software/core/mllite/hal_outputs.c b/6515/libsensors_iio/software/core/mllite/hal_outputs.c
new file mode 100644
index 0000000..1e3bb72
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/hal_outputs.c
@@ -0,0 +1,772 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/**
+ *   @defgroup  HAL_Outputs hal_outputs
+ *   @brief     Motion Library - HAL Outputs
+ *              Sets up common outputs for HAL
+ *
+ *   @{
+ *       @file  hal_outputs.c
+ *       @brief HAL Outputs.
+ */
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 1 /* Use 0 to turn on MPL_LOGV output */
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MLLITE"
+//#define MPL_LOG_9AXIS_DEBUG 1
+
+#include <string.h>
+
+#include "hal_outputs.h"
+#include "log.h"
+#include "ml_math_func.h"
+#include "mlmath.h"
+#include "start_manager.h"
+#include "data_builder.h"
+#include "results_holder.h"
+
+/* commenting this define out will bypass the low pass filter noise reduction
+   filter for compass data.
+   Disable this only for testing purpose (e.g. comparing the raw and calibrated
+   compass data, since the former is unfiltered and the latter is filtered,
+   leading to a small difference in the readings sample by sample).
+   Android specifications require this filter to be enabled to have the Magnetic
+   Field output's standard deviation fall below 0.5 uT.
+   */
+#define CALIB_COMPASS_NOISE_REDUCTION
+
+struct hal_output_t {
+    int accuracy_mag;    /**< Compass accuracy */
+    //int accuracy_gyro;   /**< Gyro Accuracy */
+    //int accuracy_accel;  /**< Accel Accuracy */
+    int accuracy_quat;   /**< quat Accuracy */
+
+    inv_time_t nav_timestamp;
+    inv_time_t gam_timestamp;
+    //inv_time_t accel_timestamp;
+    inv_time_t mag_timestamp;
+    long nav_quat[4];
+    int gyro_status;
+    int accel_status;
+    int compass_status;
+    int nine_axis_status;
+    int quat_status;
+    inv_biquad_filter_t lp_filter[3];
+    float compass_float[3];
+    long linear_acceleration_sample_rate_us;
+    long orientation_sample_rate_us;
+    long rotation_vector_sample_rate_us;
+    long gravity_sample_rate_us;
+    long orientation_6_axis_sample_rate_us;
+    long orientation_geomagnetic_sample_rate_us;
+    long rotation_vector_6_axis_sample_rate_us;
+    long geomagnetic_rotation_vector_sample_rate_us;
+};
+
+static struct hal_output_t hal_out;
+
+void inv_set_linear_acceleration_sample_rate(long sample_rate_us)
+{
+    hal_out.linear_acceleration_sample_rate_us = sample_rate_us;
+}
+
+void inv_set_orientation_sample_rate(long sample_rate_us)
+{
+    hal_out.orientation_sample_rate_us = sample_rate_us;
+}
+
+void inv_set_rotation_vector_sample_rate(long sample_rate_us)
+{
+    hal_out.rotation_vector_sample_rate_us = sample_rate_us;
+}
+
+void inv_set_gravity_sample_rate(long sample_rate_us)
+{
+    hal_out.gravity_sample_rate_us = sample_rate_us;
+}
+
+void inv_set_orientation_6_axis_sample_rate(long sample_rate_us)
+{
+    hal_out.orientation_6_axis_sample_rate_us = sample_rate_us;
+}
+
+void inv_set_orientation_geomagnetic_sample_rate(long sample_rate_us)
+{
+    hal_out.geomagnetic_rotation_vector_sample_rate_us = sample_rate_us;
+}
+
+void inv_set_rotation_vector_6_axis_sample_rate(long sample_rate_us)
+{
+    hal_out.rotation_vector_6_axis_sample_rate_us = sample_rate_us;
+}
+
+void inv_set_geomagnetic_rotation_vector_sample_rate(long sample_rate_us)
+{
+    hal_out.geomagnetic_rotation_vector_sample_rate_us = sample_rate_us;
+}
+
+/** Acceleration (m/s^2) in body frame.
+* @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it
+*             should return a vector of magnitude near 9.81 m/s^2
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
+*             inv_build_accel().
+* @return     Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy,
+                                       inv_time_t * timestamp)
+{
+    int status;
+    /* Converts fixed point to m/s^2. Fixed point has 1g = 2^16.
+     * So this 9.80665 / 2^16 */
+#define ACCEL_CONVERSION 0.000149637603759766f
+    long accel[3];
+    inv_get_accel_set(accel, accuracy, timestamp);
+    values[0] = accel[0] * ACCEL_CONVERSION;
+    values[1] = accel[1] * ACCEL_CONVERSION;
+    values[2] = accel[2] * ACCEL_CONVERSION;
+    if (hal_out.accel_status & INV_NEW_DATA)
+        status = 1;
+    else
+        status = 0;
+    MPL_LOGV("accel values:%f %f %f -%d -%lld", values[0], values[1],
+                              values[2], status, *timestamp);
+    return status;
+}
+
+/** Linear Acceleration (m/s^2) in Body Frame.
+* @param[out] values Linear Acceleration in body frame, length 3, (m/s^2). May show
+*             accel biases while at rest.
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
+*             inv_build_accel().
+* @return     Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy,
+        inv_time_t * timestamp)
+{
+    long gravity[3], accel[3];
+    inv_time_t timestamp1;
+
+    inv_get_accel_set(accel, accuracy, &timestamp1);
+    inv_get_gravity(gravity);
+    accel[0] -= gravity[0] >> 14;
+    accel[1] -= gravity[1] >> 14;
+    accel[2] -= gravity[2] >> 14;
+    values[0] = accel[0] * ACCEL_CONVERSION;
+    values[1] = accel[1] * ACCEL_CONVERSION;
+    values[2] = accel[2] * ACCEL_CONVERSION;
+
+    return inv_get_6_axis_gyro_accel_timestamp(hal_out.linear_acceleration_sample_rate_us, timestamp);
+}
+
+/** Gravity vector (m/s^2) in Body Frame.
+* @param[out] values Gravity vector in body frame, length 3, (m/s^2)
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
+*             inv_build_accel().
+* @return     Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,
+                                 inv_time_t * timestamp)
+{
+    long gravity[3];
+
+    *accuracy = (int8_t) hal_out.accuracy_quat;
+    inv_get_gravity(gravity);
+    values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION;
+    values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION;
+    values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION;
+
+    return inv_get_6_axis_gyro_accel_timestamp(hal_out.gravity_sample_rate_us, timestamp);
+}
+
+/* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16.
+ * So this is: pi / 2^16 / 180 */
+#define GYRO_CONVERSION 2.66316109007924e-007f
+
+/** Gyroscope calibrated data (rad/s) in body frame.
+* @param[out] values Rotation Rate in rad/sec.
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
+*             inv_build_gyro().
+* @return     Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,
+                                   inv_time_t * timestamp)
+{
+    long gyro[3];
+    int status;
+
+    inv_get_gyro_set(gyro, accuracy, timestamp);
+
+    values[0] = gyro[0] * GYRO_CONVERSION;
+    values[1] = gyro[1] * GYRO_CONVERSION;
+    values[2] = gyro[2] * GYRO_CONVERSION;
+    if (hal_out.gyro_status & INV_NEW_DATA)
+        status = 1;
+    else
+        status = 0;
+    return status;
+}
+
+/** Gyroscope raw data (rad/s) in body frame.
+* @param[out] values Rotation Rate in rad/sec.
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate,
+*                      while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor. Derived from the
+*                       timestamp sent to inv_build_gyro().
+* @return     Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy,
+                                      inv_time_t * timestamp)
+{
+    long gyro[3];
+    int status;
+
+    inv_get_gyro_set_raw(gyro, accuracy, timestamp);
+    values[0] = gyro[0] * GYRO_CONVERSION;
+    values[1] = gyro[1] * GYRO_CONVERSION;
+    values[2] = gyro[2] * GYRO_CONVERSION;
+    if (hal_out.gyro_status & INV_NEW_DATA)
+        status = 1;
+    else
+        status = 0;
+    return status;
+}
+
+/**
+* This corresponds to Sensor.TYPE_ROTATION_VECTOR.
+* The rotation vector represents the orientation of the device as a combination
+* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$
+* around an axis {x, y, z}. <br>
+* The three elements of the rotation vector are
+* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)}, such that the magnitude of the rotation
+* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is
+* equal to the direction of the axis of rotation.
+*
+* The three elements of the rotation vector are equal to the last three components of a unit quaternion
+* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)>. The 4th element is cos(@f$\theta@f$/2).
+*
+* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor.
+* The reference coordinate system is defined as a direct orthonormal basis, where:
+*
+*   -X is defined as the vector product Y.Z (It is tangential to the ground at the device's current location and roughly points East).
+*   -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole.
+*   -Z points towards the sky and is perpendicular to the ground.
+* @param[out] values 
+*               Length 5, 4th element being the w angle of the originating 4 
+*               elements quaternion and 5th element being the heading accuracy
+*               at 95%.
+* @param[out] accuracy Accuracy is not defined
+* @param[out] timestamp Timestamp. In (ns) for Android.
+* @return     Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy,
+        inv_time_t * timestamp)
+{
+    float quat_float[4];
+    *accuracy = (int8_t) hal_out.accuracy_quat;
+    inv_get_quaternion_float(quat_float);
+
+    if (quat_float[0] >= .0) {
+        values[0] = quat_float[1];
+        values[1] = quat_float[2];
+        values[2] = quat_float[3];
+        values[3] = quat_float[0];
+    } else {
+        values[0] = -quat_float[1];
+        values[1] = -quat_float[2];
+        values[2] = -quat_float[3];
+        values[3] = -quat_float[0];
+    }
+    values[4] = inv_get_heading_confidence_interval();
+    return inv_get_9_axis_timestamp(hal_out.rotation_vector_sample_rate_us, timestamp);
+}
+
+int inv_get_sensor_type_rotation_vector_6_axis(float *values, int8_t *accuracy,
+        inv_time_t * timestamp)
+{
+    int status;
+    long accel[3];
+    float quat_6_axis[4];
+    inv_time_t timestamp1;
+    inv_get_accel_set(accel, accuracy, &timestamp1);
+    inv_get_6axis_quaternion_float(quat_6_axis, &timestamp1);
+
+    if (quat_6_axis[0] >= .0) {
+        values[0] = quat_6_axis[1];
+        values[1] = quat_6_axis[2];
+        values[2] = quat_6_axis[3];
+        values[3] = quat_6_axis[0];
+    } else {
+        values[0] = -quat_6_axis[1];
+        values[1] = -quat_6_axis[2];
+        values[2] = -quat_6_axis[3];
+        values[3] = -quat_6_axis[0];
+    }
+    //This sensor does not report an estimated heading accuracy
+    values[4] = 0;
+    if (hal_out.quat_status & INV_QUAT_3AXIS)
+    {        
+        status = hal_out.quat_status & INV_NEW_DATA? 1 : 0;
+    }
+    else {
+        status = hal_out.accel_status & INV_NEW_DATA? 1 : 0;
+    }
+    MPL_LOGV("values:%f %f %f %f %f -%d -%lld", values[0], values[1],
+                              values[2], values[3], values[4], status, timestamp1);
+    return inv_get_6_axis_gyro_accel_timestamp(hal_out.rotation_vector_6_axis_sample_rate_us, timestamp);
+}
+
+/**
+* This corresponds to Sensor.TYPE_GEOMAGNETIC_ROTATION_VECTOR.
+* Similar to SENSOR_TYPE_ROTATION_VECTOR, but using a magnetometer
+* instead of using a gyroscope.
+* Fourth element = estimated_accuracy in radians (heading confidence).
+* @param[out] values Length 4.
+* @param[out] accuracy is not defined.
+* @param[out] timestamp in (ns) for Android.
+* @return     Returns 1 if the data was updated, 0 otherwise.
+*/
+int inv_get_sensor_type_geomagnetic_rotation_vector(float *values, int8_t *accuracy,
+        inv_time_t * timestamp)
+{
+    long compass[3];
+    float quat_geomagnetic[4];
+    int status;
+    inv_time_t timestamp1;
+    inv_get_compass_set(compass, accuracy, &timestamp1);
+    inv_get_geomagnetic_quaternion_float(quat_geomagnetic, &timestamp1);
+
+    if (quat_geomagnetic[0] >= .0) {
+        values[0] = quat_geomagnetic[1];
+        values[1] = quat_geomagnetic[2];
+        values[2] = quat_geomagnetic[3];
+        values[3] = quat_geomagnetic[0];
+    } else {
+        values[0] = -quat_geomagnetic[1];
+        values[1] = -quat_geomagnetic[2];
+        values[2] = -quat_geomagnetic[3];
+        values[3] = -quat_geomagnetic[0];
+    }
+    values[4] = inv_get_accel_compass_confidence_interval();
+    status = hal_out.accel_status & INV_NEW_DATA? 1 : 0;
+    MPL_LOGV("values:%f %f %f %f %f -%d", values[0], values[1],
+                              values[2], values[3], values[4], status);
+
+    return inv_get_6_axis_compass_accel_timestamp(hal_out.geomagnetic_rotation_vector_sample_rate_us, timestamp);
+}
+
+/** Compass data (uT) in body frame.
+* @param[out] values Compass data in (uT), length 3. May be calibrated by having
+*             biases removed and sensitivity adjusted
+* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate
+* @param[out] timestamp Timestamp. In (ns) for Android.
+* @return     Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy,
+                                        inv_time_t * timestamp)
+{
+    int status;
+    int i;
+    /* Converts fixed point to uT. Fixed point has 1 uT = 2^16.
+     * So this is: 1 / 2^16*/
+//#define COMPASS_CONVERSION 1.52587890625e-005f
+
+    *timestamp = hal_out.mag_timestamp;
+    *accuracy = (int8_t) hal_out.accuracy_mag;
+
+    for (i = 0; i < 3; i++)
+        values[i] = hal_out.compass_float[i];
+    if (hal_out.compass_status & INV_NEW_DATA)
+        status = 1;
+    else
+        status = 0;
+    return status;
+}
+
+/** Compass raw data (uT) in body frame.
+* @param[out] values Compass data in (uT), length 3. May be calibrated by having
+*             biases removed and sensitivity adjusted
+* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate
+* @param[out] timestamp Timestamp. In (ns) for Android.
+* @return     Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_magnetic_field_raw(float *values, int8_t *accuracy,
+                                           inv_time_t * timestamp)
+{
+    long mag[3];
+    int status;
+    int i;
+
+    inv_get_compass_set_raw(mag, accuracy, timestamp);
+
+    /* Converts fixed point to uT. Fixed point has 1 uT = 2^16.
+     * So this is: 1 / 2^16*/
+#define COMPASS_CONVERSION 1.52587890625e-005f
+
+    for (i = 0; i < 3; i++) {
+        values[i] = (float)mag[i] * COMPASS_CONVERSION;
+    }
+    if (hal_out.compass_status & INV_NEW_DATA)
+        status = 1;
+    else
+        status = 0;
+    return status;
+}
+static void inv_get_rotation_geomagnetic(float r[3][3])
+{
+    long rot[9], quat_geo[4];
+    float conv = 1.f / (1L<<30);
+    inv_time_t timestamp;
+    inv_get_geomagnetic_quaternion(quat_geo, &timestamp);
+    inv_quaternion_to_rotation(quat_geo, rot);
+    r[0][0] = rot[0]*conv;
+    r[0][1] = rot[1]*conv;
+    r[0][2] = rot[2]*conv;
+    r[1][0] = rot[3]*conv;
+    r[1][1] = rot[4]*conv;
+    r[1][2] = rot[5]*conv;
+    r[2][0] = rot[6]*conv;
+    r[2][1] = rot[7]*conv;
+    r[2][2] = rot[8]*conv;
+}
+static void google_orientation_geomagnetic(float *g)
+{
+    float rad2deg = (float)(180.0 / M_PI);
+    float R[3][3];
+    inv_get_rotation_geomagnetic(R);
+    g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg;
+    g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg;
+    g[2] = asinf ( R[2][0])          * rad2deg;
+    if (g[0] < 0)
+        g[0] += 360;
+}
+
+static void inv_get_rotation_6_axis(float r[3][3])
+{
+    long rot[9], quat_6_axis[4];
+    float conv = 1.f / (1L<<30);
+    inv_time_t timestamp;
+
+    inv_get_6axis_quaternion(quat_6_axis, &timestamp);
+    inv_quaternion_to_rotation(quat_6_axis, rot);
+    r[0][0] = rot[0]*conv;
+    r[0][1] = rot[1]*conv;
+    r[0][2] = rot[2]*conv;
+    r[1][0] = rot[3]*conv;
+    r[1][1] = rot[4]*conv;
+    r[1][2] = rot[5]*conv;
+    r[2][0] = rot[6]*conv;
+    r[2][1] = rot[7]*conv;
+    r[2][2] = rot[8]*conv;
+}
+
+static void google_orientation_6_axis(float *g)
+{
+    float rad2deg = (float)(180.0 / M_PI);
+    float R[3][3];
+
+    inv_get_rotation_6_axis(R);
+
+    g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg;
+    g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg;
+    g[2] = asinf ( R[2][0])          * rad2deg;
+    if (g[0] < 0)
+        g[0] += 360;
+}
+
+static void inv_get_rotation(float r[3][3])
+{
+    long rot[9];
+    float conv = 1.f / (1L<<30);
+
+    inv_quaternion_to_rotation(hal_out.nav_quat, rot);
+    r[0][0] = rot[0]*conv;
+    r[0][1] = rot[1]*conv;
+    r[0][2] = rot[2]*conv;
+    r[1][0] = rot[3]*conv;
+    r[1][1] = rot[4]*conv;
+    r[1][2] = rot[5]*conv;
+    r[2][0] = rot[6]*conv;
+    r[2][1] = rot[7]*conv;
+    r[2][2] = rot[8]*conv;
+}
+
+static void google_orientation(float *g)
+{
+    float rad2deg = (float)(180.0 / M_PI);
+    float R[3][3];
+
+    inv_get_rotation(R);
+
+    g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg;
+    g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg;
+    g[2] = asinf ( R[2][0])          * rad2deg;
+    if (g[0] < 0)
+        g[0] += 360;
+}
+
+/** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees.
+* @param[out] values Length 3, Degrees.<br>
+*        - values[0]: Azimuth, angle between the magnetic north direction
+*         and the y-axis, around the z-axis (0 to 359). 0=North, 90=East, 180=South, 270=West<br>
+*        - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values
+*         when the z-axis moves toward the y-axis.<br>
+*        - values[2]: Roll, rotation around y-axis (-90 to 90), with positive
+*          values when the x-axis moves toward the z-axis.<br>
+*
+* @note  This definition is different from yaw, pitch and roll used in aviation
+*        where the X axis is along the long side of the plane (tail to nose).
+*        Note: This sensor type exists for legacy reasons, please use getRotationMatrix()
+*        in conjunction with remapCoordinateSystem() and getOrientation() to compute
+*        these values instead.
+*        Important note: For historical reasons the roll angle is positive in the
+*        clockwise direction (mathematically speaking, it should be positive in
+*        the counter-clockwise direction).
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor.
+* @return     Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_orientation(float *values, int8_t *accuracy,
+                                     inv_time_t * timestamp)
+{
+    *accuracy = (int8_t) hal_out.accuracy_quat;
+    google_orientation(values);
+
+    return inv_get_9_axis_timestamp(hal_out.orientation_sample_rate_us, timestamp);
+}
+
+int inv_get_sensor_type_orientation_6_axis(float *values, int8_t *accuracy,
+                                     inv_time_t * timestamp)
+{
+    long accel[3];
+    inv_time_t timestamp1;
+    inv_get_accel_set(accel, accuracy, &timestamp1);
+
+    google_orientation_6_axis(values);
+
+    return inv_get_6_axis_gyro_accel_timestamp(hal_out.orientation_6_axis_sample_rate_us, timestamp);
+}
+
+int inv_get_sensor_type_orientation_geomagnetic(float *values, int8_t *accuracy,
+                                     inv_time_t * timestamp)
+{
+    long compass[3];
+    inv_time_t timestamp1;
+    inv_get_compass_set(compass, accuracy, &timestamp1);
+
+    google_orientation_geomagnetic(values);
+
+    return inv_get_6_axis_compass_accel_timestamp(hal_out.orientation_geomagnetic_sample_rate_us, timestamp);
+}
+
+/** Main callback to generate HAL outputs. Typically not called by library users.
+* @param[in] sensor_cal Input variable to take sensor data whenever there is new
+* sensor data.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal)
+{
+    int use_sensor = 0;
+    long sr = 1000;
+    long compass[3];
+    int8_t accuracy;
+    int i;
+    (void) sensor_cal;
+
+    inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_quat,
+                           &hal_out.nav_timestamp);
+    hal_out.gyro_status = sensor_cal->gyro.status;
+    hal_out.accel_status = sensor_cal->accel.status;
+    hal_out.compass_status = sensor_cal->compass.status;
+    hal_out.quat_status = sensor_cal->quat.status;
+#if MPL_LOG_9AXIS_DEBUG
+    MPL_LOGV("hal_out:g=%d", hal_out.gyro_status);
+#endif
+    // Find the highest sample rate and tie generating 9-axis to that one.
+    if (sensor_cal->gyro.status & INV_SENSOR_ON) {
+        sr = sensor_cal->gyro.sample_rate_ms;
+        use_sensor = 0;
+    }
+    if ((sensor_cal->accel.status & INV_SENSOR_ON) && (sr > sensor_cal->accel.sample_rate_ms)) {
+        sr = sensor_cal->accel.sample_rate_ms;
+        use_sensor = 1;
+    }
+    if ((sensor_cal->compass.status & INV_SENSOR_ON) && (sr > sensor_cal->compass.sample_rate_ms)) {
+        sr = sensor_cal->compass.sample_rate_ms;
+        use_sensor = 2;
+    }
+    if ((sensor_cal->quat.status & INV_SENSOR_ON) && (sr > sensor_cal->quat.sample_rate_ms)) {
+        sr = sensor_cal->quat.sample_rate_ms;
+        use_sensor = 3;
+    }
+
+    // If the timestamp did not change, remove the new data flag
+    if (sensor_cal->gyro.timestamp_prev == sensor_cal->gyro.timestamp) {
+        hal_out.gyro_status &= ~INV_NEW_DATA;
+    }
+    if (sensor_cal->accel.timestamp_prev == sensor_cal->accel.timestamp) {
+        hal_out.accel_status &= ~INV_NEW_DATA;
+    }
+    if (sensor_cal->compass.timestamp_prev == sensor_cal->compass.timestamp) {
+        hal_out.compass_status &= ~INV_NEW_DATA;
+    }
+    if (sensor_cal->quat.timestamp_prev == sensor_cal->quat.timestamp) {
+        hal_out.quat_status &= ~INV_NEW_DATA;
+    }
+
+    // Only output 9-axis if all 9 sensors are on.
+    if (sensor_cal->quat.status & INV_SENSOR_ON) {
+        // If quaternion sensor is on, gyros are not required as quaternion already has that part
+        if ((sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) {
+            use_sensor = -1;
+        }
+    } else {
+        if ((sensor_cal->gyro.status & sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) {
+            use_sensor = -1;
+        }
+    }
+#if MPL_LOG_9AXIS_DEBUG
+    MPL_LOGI("use_sensor=%d", use_sensor);
+#endif
+    switch (use_sensor) {
+    case 0:
+        hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0;
+        hal_out.nav_timestamp = sensor_cal->gyro.timestamp;
+        break;
+    case 1:
+        hal_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0;
+        hal_out.nav_timestamp = sensor_cal->accel.timestamp;
+        break;
+    case 2:
+        hal_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0;
+        hal_out.nav_timestamp = sensor_cal->compass.timestamp;
+        break;
+    case 3:
+        hal_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0;
+        hal_out.nav_timestamp = sensor_cal->quat.timestamp;
+        break;
+    default:
+        hal_out.nine_axis_status = 0; // Don't output quaternion related info
+        break;
+    }
+#if MPL_LOG_9AXIS_DEBUG
+    MPL_LOGI("nav ts: %lld", hal_out.nav_timestamp);    
+#endif
+    /* Converts fixed point to uT. Fixed point has 1 uT = 2^16.
+     * So this is: 1 / 2^16*/
+    #define COMPASS_CONVERSION 1.52587890625e-005f
+
+    inv_get_compass_set(compass, &accuracy, &(hal_out.mag_timestamp) );
+    hal_out.accuracy_mag = (int)accuracy;
+
+#ifndef CALIB_COMPASS_NOISE_REDUCTION
+    for (i = 0; i < 3; i++) {
+        hal_out.compass_float[i] = (float)compass[i] * COMPASS_CONVERSION;
+    }
+#else
+    for (i = 0; i < 3; i++) {
+        if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_CONTIGUOUS)) ==
+                                                             INV_NEW_DATA)  {
+            // set the state variables to match output with input
+            inv_calc_state_to_match_output(&hal_out.lp_filter[i],
+                                           (float)compass[i]);
+        }
+        if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_RAW_DATA)) ==
+                                          (INV_NEW_DATA | INV_RAW_DATA)) {
+            hal_out.compass_float[i] =
+                inv_biquad_filter_process(&hal_out.lp_filter[i],
+                                          (float)compass[i]) *
+                                          COMPASS_CONVERSION;
+        } else if ((sensor_cal->compass.status & INV_NEW_DATA) == INV_NEW_DATA) {
+            hal_out.compass_float[i] = (float)compass[i] * COMPASS_CONVERSION;
+        }
+    }
+#endif // CALIB_COMPASS_NOISE_REDUCTION
+    return INV_SUCCESS;
+}
+
+/** Turns off generation of HAL outputs.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_stop_hal_outputs(void)
+{
+    inv_error_t result;
+    result = inv_unregister_data_cb(inv_generate_hal_outputs);
+    return result;
+}
+
+/** Turns on generation of HAL outputs. This should be called after inv_stop_hal_outputs()
+* to turn generation of HAL outputs back on. It is automatically called by inv_enable_hal_outputs().
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_start_hal_outputs(void)
+{
+    inv_error_t result;
+    result =
+        inv_register_data_cb(inv_generate_hal_outputs,
+                             INV_PRIORITY_HAL_OUTPUTS,
+                             INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW | INV_QUAT_NEW | INV_PRESSURE_NEW);
+    return result;
+}
+
+/* file name: lowPassFilterCoeff_1_6.c */
+float compass_low_pass_filter_coeff[5] =
+{+2.000000000000f, +1.000000000000f, -1.279632424998f, +0.477592250073f, +0.049489956269f};
+
+/** Initializes hal outputs class. This is called automatically by the
+* enable function. It may be called any time the feature is enabled, but
+* is typically not needed to be called by outside callers.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_init_hal_outputs(void)
+{
+    int i;
+    memset(&hal_out, 0, sizeof(hal_out));
+    for (i=0; i<3; i++)  {
+        inv_init_biquad_filter(&hal_out.lp_filter[i], compass_low_pass_filter_coeff);
+    }
+
+    return INV_SUCCESS;
+}
+
+/** Turns on creation and storage of HAL type results.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_enable_hal_outputs(void)
+{
+    inv_error_t result;
+
+        // don't need to check the result for inv_init_hal_outputs
+        // since it's always INV_SUCCESS
+        inv_init_hal_outputs();
+
+    result = inv_register_mpl_start_notification(inv_start_hal_outputs);
+    return result;
+}
+
+/** Turns off creation and storage of HAL type results.
+*/
+inv_error_t inv_disable_hal_outputs(void)
+{
+    inv_error_t result;
+
+    inv_stop_hal_outputs(); // Ignore error if we have already stopped this
+    result = inv_unregister_mpl_start_notification(inv_start_hal_outputs);
+    return result;
+}
+
+/**
+ * @}
+ */
+ 
+ 
+ 
diff --git a/6515/libsensors_iio/software/core/mllite/hal_outputs.h b/6515/libsensors_iio/software/core/mllite/hal_outputs.h
new file mode 100644
index 0000000..797269c
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/hal_outputs.h
@@ -0,0 +1,66 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#include "mltypes.h"
+
+#ifndef INV_HAL_OUTPUTS_H__
+#define INV_HAL_OUTPUTS_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    int inv_get_sensor_type_orientation(float *values, int8_t *accuracy,
+                                         inv_time_t * timestamp);
+    int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy,
+                                           inv_time_t * timestamp);
+    int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,
+                                       inv_time_t * timestamp);
+    int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy,
+                                       inv_time_t * timestamp);
+    int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy,
+                                            inv_time_t * timestamp);
+	int inv_get_sensor_type_magnetic_field_raw(float *values, int8_t *accuracy,
+                                            inv_time_t * timestamp);
+    int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy,
+            inv_time_t * timestamp);
+
+    int inv_get_sensor_type_linear_acceleration(float *values,
+            int8_t *accuracy,
+            inv_time_t * timestamp);
+    int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,
+                                     inv_time_t * timestamp);
+
+    int inv_get_sensor_type_orientation_6_axis(float *values, int8_t *accuracy,
+                                         inv_time_t * timestamp);
+    int inv_get_sensor_type_orientation_geomagnetic(float *values, int8_t *accuracy,
+                                         inv_time_t * timestamp);
+    int inv_get_sensor_type_rotation_vector_6_axis(float *values, int8_t *accuracy,
+                                         inv_time_t * timestamp);
+    int inv_get_sensor_type_geomagnetic_rotation_vector(float *values, int8_t *accuracy,
+                                         inv_time_t * timestamp);
+
+    inv_error_t inv_enable_hal_outputs(void);
+    inv_error_t inv_disable_hal_outputs(void);
+    inv_error_t inv_init_hal_outputs(void);
+    inv_error_t inv_start_hal_outputs(void);
+    inv_error_t inv_stop_hal_outputs(void);
+
+    // Set data rates for virtual sensors
+    void inv_set_linear_acceleration_sample_rate(long sample_rate_us);
+    void inv_set_orientation_sample_rate(long sample_rate_us);
+    void inv_set_rotation_vector_sample_rate(long sample_rate_us);
+    void inv_set_gravity_sample_rate(long sample_rate_us);
+    void inv_set_orientation_6_axis_sample_rate(long sample_rate_us);
+    void inv_set_orientation_geomagnetic_sample_rate(long sample_rate_us);
+    void inv_set_rotation_vector_6_axis_sample_rate(long sample_rate_us);
+    void inv_set_geomagnetic_rotation_vector_sample_rate(long sample_rate_us);
+    
+#ifdef __cplusplus
+}
+#endif
+
+#endif                          // INV_HAL_OUTPUTS_H__
diff --git a/6515/libsensors_iio/software/core/mllite/invensense.h b/6515/libsensors_iio/software/core/mllite/invensense.h
new file mode 100644
index 0000000..fb8e12b
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/invensense.h
@@ -0,0 +1,28 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+/**
+ *  Main header file for Invensense's basic library.
+ */
+
+#include "data_builder.h"
+#include "hal_outputs.h"
+#include "message_layer.h"
+#include "mlmath.h"
+#include "ml_math_func.h"
+#include "mpl.h"
+#include "results_holder.h"
+#include "start_manager.h"
+#include "storage_manager.h"
+#include "log.h"
+#include "mlinclude.h"
+//#include "..\HAL\include\mlos.h"
diff --git a/6515/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c b/6515/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c
new file mode 100644
index 0000000..649b917
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c
@@ -0,0 +1,318 @@
+/**
+ *  @brief    Provides helpful file IO wrappers for use with sysfs.
+ *  @details  Based on Jonathan Cameron's @e iio_utils.h.
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <ctype.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <dirent.h>
+#include <errno.h>
+#include <unistd.h>
+#include "inv_sysfs_utils.h"
+
+/* General TODO list:
+ * Select more reasonable string lengths or use fseek and malloc.
+ */
+
+/**
+ *  inv_sysfs_write() - Write an integer to a file.
+ *  @filename:	Path to file.
+ *  @data:	Value to write to file.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_sysfs_write(char *filename, long data)
+{
+	FILE *fp;
+	int count;
+
+	if (!filename)
+		return -1;
+	fp = fopen(filename, "w");
+	if (!fp)
+		return -errno;
+	count = fprintf(fp, "%ld", data);
+	fclose(fp);
+	return count;
+}
+
+/**
+ *  inv_sysfs_read() - Read a string from a file.
+ *  @filename:	Path to file.
+ *  @num_bytes:	Number of bytes to read.
+ *  @data:	Data from file.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_sysfs_read(char *filename, long num_bytes, char *data)
+{
+	FILE *fp;
+	int count;
+
+	if (!filename)
+		return -1;
+	fp = fopen(filename, "r");
+	if (!fp)
+		return -errno;
+	count = fread(data, 1, num_bytes, fp);
+	fclose(fp);
+	return count;
+}
+
+/**
+ *  inv_read_buffer() - Read data from ring buffer.
+ *  @fd:	File descriptor for buffer file.
+ *  @data:	Data in hardware units.
+ *  @timestamp:	Time when data was read from device. Use NULL if unsupported.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_buffer(int fd, long *data, long long *timestamp)
+{
+	char str[35];
+	int count;
+
+	count = read(fd, str, sizeof(str));
+	if (!count)
+		return count;
+	if (!timestamp)
+		count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]);
+	else
+		count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1],
+			&data[2], timestamp);
+
+	if (count < (timestamp?4:3))
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_raw() - Read raw data.
+ *  @names:	Names of sysfs files.
+ *  @data:	Data in hardware units.
+ *  @timestamp:	Time when data was read from device. Use NULL if unsupported.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, 
+	long long *timestamp)
+{
+	char str[40];
+	int count;
+
+	count = inv_sysfs_read((char*)names->raw_data, sizeof(str), str);
+	if (count < 0)
+		return count;
+	if (!timestamp)
+		count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]);
+	else
+		count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1],
+			&data[2], timestamp);
+	if (count < (timestamp?4:3))
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_temperature_raw() - Read temperature.
+ *  @names:	Names of sysfs files.
+ *  @data:	Data in hardware units.
+ *  @timestamp:	Time when data was read from device.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data,
+	long long *timestamp)
+{
+	char str[25];
+	int count;
+
+	count = inv_sysfs_read((char*)names->temperature, sizeof(str), str);
+	if (count < 0)
+		return count;
+	count = sscanf(str, "%hd%lld", &data[0], timestamp);
+	if (count < 2)
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_fifo_rate() - Read fifo rate.
+ *  @names:	Names of sysfs files.
+ *  @data:	Fifo rate.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data)
+{
+	char str[8];
+	int count;
+
+	count = inv_sysfs_read((char*)names->fifo_rate, sizeof(str), str);
+	if (count < 0)
+		return count;
+	count = sscanf(str, "%hd", data);
+	if (count < 1)
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_power_state() - Read power state.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 if device is on.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data)
+{
+	char str[2];
+	int count;
+
+	count = inv_sysfs_read((char*)names->power_state, sizeof(str), str);
+	if (count < 0)
+		return count;
+	count = sscanf(str, "%hd", (short*)data);
+	if (count < 1)
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_scale() - Read scale.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 if device is on.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_scale(const struct inv_sysfs_names_s *names, float *data)
+{
+	char str[5];
+	int count;
+
+	count = inv_sysfs_read((char*)names->scale, sizeof(str), str);
+	if (count < 0)
+		return count;
+	count = sscanf(str, "%f", data);
+	if (count < 1)
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_temp_scale() - Read temperature scale.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 if device is on.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data)
+{
+	char str[4];
+	int count;
+
+	count = inv_sysfs_read((char*)names->temp_scale, sizeof(str), str);
+	if (count < 0)
+		return count;
+	count = sscanf(str, "%hd", data);
+	if (count < 1)
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_temp_offset() - Read temperature offset.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 if device is on.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data)
+{
+	char str[4];
+	int count;
+
+	count = inv_sysfs_read((char*)names->temp_offset, sizeof(str), str);
+	if (count < 0)
+		return count;
+	count = sscanf(str, "%hd", data);
+	if (count < 1)
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_q16() - Get data as q16 fixed point.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 if device is on.
+ *  @timestamp:	Time when data was read from device.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_q16(const struct inv_sysfs_names_s *names, long *data,
+	long long *timestamp)
+{
+	int count;
+	short raw[3];
+	float scale;
+	count = inv_read_raw(names, (long*)raw, timestamp);
+	count += inv_read_scale(names, &scale);
+	data[0] = (long)(raw[0] * (65536.f / scale));
+	data[1] = (long)(raw[1] * (65536.f / scale));
+	data[2] = (long)(raw[2] * (65536.f / scale));
+	return count;
+}
+
+/**
+ *  inv_read_q16() - Get temperature data as q16 fixed point.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 if device is on.
+ *  @timestamp:	Time when data was read from device.
+ *  Returns number of bytes read or a negative error code.
+ */
+int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data,
+	long long *timestamp)
+{
+	int count = 0;
+	short raw;
+	static short scale, offset;
+	static unsigned char first_read = 1;
+
+	if (first_read) {
+		count += inv_read_temp_scale(names, &scale);
+		count += inv_read_temp_offset(names, &offset);
+		first_read = 0;
+	}
+	count += inv_read_temperature_raw(names, &raw, timestamp);
+	data[0] = (long)((35 + ((float)(raw - offset) / scale)) * 65536.f);
+
+	return count;
+}
+
+/**
+ *  inv_write_fifo_rate() - Write fifo rate.
+ *  @names:	Names of sysfs files.
+ *  @data:	Fifo rate.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data)
+{
+	return inv_sysfs_write((char*)names->fifo_rate, (long)data);
+}
+
+/**
+ *  inv_write_buffer_enable() - Enable/disable buffer in /dev.
+ *  @names:	Names of sysfs files.
+ *  @data:	Fifo rate.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data)
+{
+	return inv_sysfs_write((char*)names->enable, (long)data);
+}
+
+/**
+ *  inv_write_power_state() - Turn device on/off.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 to turn on.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_write_power_state(const struct inv_sysfs_names_s *names, char data)
+{
+	return inv_sysfs_write((char*)names->power_state, (long)data);
+}
+
+
+
diff --git a/6515/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h b/6515/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h
new file mode 100644
index 0000000..45a35f9
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h
@@ -0,0 +1,84 @@
+/**
+ *  @brief    Provides helpful file IO wrappers for use with sysfs.
+ *  @details  Based on Jonathan Cameron's @e iio_utils.h.
+ */
+
+#ifndef _INV_SYSFS_UTILS_H_
+#define _INV_SYSFS_UTILS_H_
+
+/**
+ *  struct inv_sysfs_names_s - Files needed by user applications.
+ *  @buffer:		Ring buffer attached to FIFO.
+ *  @enable:		Turns on HW-to-ring buffer flow.
+ *  @raw_data:		Raw data from registers.
+ *  @temperature:	Temperature data from register.
+ *  @fifo_rate:		FIFO rate/ODR.
+ *  @power_state:	Power state (this is a five-star comment).
+ *  @fsr:		Full-scale range.
+ *  @lpf:		Digital low pass filter.
+ *  @scale:		LSBs / dps (or LSBs / Gs).
+ *  @temp_scale:	LSBs / degrees C.
+ *  @temp_offset:	Offset in LSBs.
+ */
+struct inv_sysfs_names_s {
+
+	//Sysfs for ITG3500 & MPU6050
+	const char *buffer;
+	const char *enable;
+	const char *raw_data;		//Raw Gyro data
+	const char *temperature;
+	const char *fifo_rate;
+	const char *power_state;
+	const char *fsr;
+	const char *lpf;
+	const char *scale;			//Gyro scale
+	const char *temp_scale;
+	const char *temp_offset;
+	const char *self_test;
+	//Starting Sysfs available for MPU6050 only
+	const char *accel_en;
+	const char *accel_fifo_en;
+	const char *accel_fs;
+	const char *clock_source;
+	const char *early_suspend_en;
+	const char *firmware_loaded;
+	const char *gyro_en;
+	const char *gyro_fifo_en;
+	const char *key;
+	const char *raw_accel;
+	const char *reg_dump;
+	const char *tap_on;
+	const char *dmp_firmware;
+};
+
+/* File IO. Typically won't be called directly by user application, but they'll
+ * be here for your enjoyment.
+ */
+int inv_sysfs_write(char *filename, long data);
+int inv_sysfs_read(char *filename, long num_bytes, char *data);
+
+/* Helper APIs to extract specific data. */
+int inv_read_buffer(int fd, long *data, long long *timestamp);
+int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, 
+		 long long *timestamp);
+int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data,
+			     long long *timestamp);
+int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data);
+int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data);
+int inv_read_scale(const struct inv_sysfs_names_s *names, float *data);
+int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data);
+int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data);
+int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data);
+int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data);
+int inv_write_power_state(const struct inv_sysfs_names_s *names, char data);
+
+/* Scaled data. */
+int inv_read_q16(const struct inv_sysfs_names_s *names, long *data,
+                 long long *timestamp);
+int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data,
+                      long long *timestamp);
+
+
+#endif  /* #ifndef _INV_SYSFS_UTILS_H_ */
+
+
diff --git a/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c b/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
new file mode 100644
index 0000000..14e0c9c
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
@@ -0,0 +1,271 @@
+/*
+ $License:
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id:$
+ *
+ *****************************************************************************/
+
+/**
+ * @defgroup ML_LOAD_DMP
+ *
+ * @{
+ *      @file     ml_load_dmp.c
+ *      @brief    functions for writing dmp firmware.
+ */
+#include <stdio.h>
+
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-loaddmp"
+
+#include "ml_load_dmp.h"
+#include "log.h"
+#include "mlos.h"
+
+#define LOADDMP_LOG MPL_LOGI
+#define LOADDMP_LOG MPL_LOGI
+
+#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0]))
+#define DMP_CODE_SIZE 2463
+
+static const unsigned char dmpMemory[DMP_CODE_SIZE] = {
+    /* bank # 0 */
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x7d, 0x00, 0x01, 
+    0x00, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x00, 0x83, 0x12, 0x6f, 0xff, 0xdf, 0x3b, 0x64, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, 
+    0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 
+    0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x07, 
+    0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00, 
+    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x07, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x00, 0x00, 0x09, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    /* bank # 1 */
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x28, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x58, 0x05, 0xdc, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc8, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0xff, 
+    0x08, 0x00, 0x02, 0x00, 0x04, 0x00, 0x01, 0x00, 0x80, 0x00, 0x10, 0x00, 0x20, 0x00, 0x40, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    /* bank # 2 */
+    0x00, 0x28, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x0b, 0xeb, 0xc2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x14, 
+    0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x04, 0x06, 0x31, 0x0a, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x05, 0x00, 0x05, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x05, 0x00, 0x05, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x05, 0x00, 0x05, 
+    0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    /* bank # 3 */
+    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x3c, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, 
+    0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+
+    /* bank # 4 */
+    0xd8, 0xb0, 0xb5, 0xb9, 0xf3, 0xa6, 0xf8, 0xf9, 0xd1, 0xd9, 0x81, 0x96, 0xf8, 0xf7, 0x3e, 0xd8, 
+    0xf3, 0xb1, 0x86, 0x96, 0xa3, 0x31, 0xd1, 0xda, 0xf1, 0xff, 0xd8, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e, 
+    0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, 
+    0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0xf1, 0xa9, 
+    0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0x8a, 0xa8, 0x96, 0x36, 
+    0x56, 0x76, 0xd8, 0xf1, 0xb0, 0x89, 0xb9, 0xa3, 0xc3, 0xc5, 0xc7, 0xb1, 0x81, 0xb4, 0x97, 0xa3, 
+    0x11, 0xb5, 0x93, 0xa1, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xf1, 0x93, 0x81, 0xa3, 0x2d, 
+    0x55, 0x3d, 0xf2, 0xa6, 0xfa, 0xf9, 0xd1, 0xd9, 0xf8, 0xf1, 0x83, 0xa2, 0xc3, 0xc5, 0xc7, 0xd8, 
+    0xf3, 0xa2, 0xde, 0xf1, 0x82, 0x93, 0xa3, 0x2d, 0x55, 0x7d, 0x83, 0x95, 0xf5, 0xa3, 0x30, 0xd9, 
+    0xf3, 0xa2, 0xf8, 0xd8, 0xf5, 0xa3, 0x50, 0xd9, 0xf3, 0xa2, 0xf8, 0xd8, 0xf5, 0xa3, 0x70, 0xd9, 
+    0xf3, 0xa2, 0xf8, 0xd8, 0xf2, 0xb9, 0xa2, 0xf8, 0xf9, 0xd1, 0xd9, 0xa6, 0xde, 0xdf, 0xf4, 0x1c, 
+    0xd8, 0xf2, 0xa6, 0xf8, 0xf9, 0xd1, 0xd9, 0xf4, 0x14, 0xd8, 0xf2, 0xf9, 0xd1, 0xd9, 0xf8, 0xf4, 
+    0x1e, 0xd8, 0xf2, 0xf9, 0xd1, 0xd9, 0xf8, 0xf8, 0xf4, 0x14, 0xd8, 0xf4, 0x10, 0xd8, 0xf3, 0xa2, 
+    0xf8, 0xf9, 0xd1, 0xda, 0xf2, 0xa6, 0xf8, 0xf1, 0xa5, 0xde, 0xd8, 0xf4, 0xa3, 0x14, 0x14, 0xd8, 
+    0xf1, 0xa5, 0xf8, 0xa3, 0x85, 0x95, 0x09, 0xd9, 0xf1, 0xa5, 0xde, 0xf2, 0xa6, 0xf8, 0xdf, 0xd8, 
+    0xf4, 0xa3, 0x09, 0xd8, 0xf3, 0xa2, 0xf8, 0xf9, 0xd1, 0xf4, 0xd9, 0x08, 0x17, 0xda, 0x42, 0xf2, 
+    /* bank # 5 */
+    0xa2, 0xde, 0xf4, 0x0b, 0xd8, 0xf1, 0xa5, 0xf8, 0xa3, 0x85, 0x95, 0x19, 0xda, 0xf4, 0x05, 0xd8, 
+    0xf2, 0xa6, 0xde, 0xdf, 0xd8, 0xf1, 0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d, 0xb2, 
+    0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xb8, 0xa8, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, 0xaa, 
+    0x88, 0x2c, 0x54, 0x7c, 0xd8, 0xf1, 0xb8, 0xb4, 0xb0, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 
+    0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0, 0x0c, 0x20, 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 
+    0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 
+    0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab, 0x88, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 
+    0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0, 0x88, 0xb4, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 
+    0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0, 0xb8, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 
+    0x29, 0x51, 0x8a, 0x24, 0x70, 0x59, 0x8b, 0x20, 0x58, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 
+    0x8a, 0x64, 0x48, 0x31, 0x8b, 0x30, 0x49, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 
+    0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 
+    0x46, 0x66, 0xf0, 0x89, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 
+    0x48, 0x31, 0xa9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 
+    0x48, 0x60, 0x8c, 0xa8, 0x3c, 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 
+    0x6e, 0x76, 0x7e, 0xa9, 0x99, 0x88, 0x2d, 0x55, 0x7d, 0xd8, 0xf1, 0xb3, 0x8b, 0xb4, 0x97, 0xbb, 
+    /* bank # 6 */
+    0xab, 0xf8, 0xf9, 0xb9, 0xa3, 0xda, 0xc3, 0xc5, 0xc7, 0xd9, 0x2d, 0x4d, 0x6d, 0xd8, 0xb0, 0x8c, 
+    0xb8, 0xac, 0xf8, 0xf9, 0xa8, 0xda, 0xc3, 0xc5, 0xc7, 0xd9, 0x2d, 0x4d, 0x6d, 0xd8, 0xf3, 0xb9, 
+    0xac, 0xde, 0xd8, 0xb1, 0xb6, 0xb9, 0xf1, 0xa8, 0xf8, 0xf3, 0xb9, 0xaa, 0xfa, 0xf9, 0xd1, 0xda, 
+    0xf2, 0x8a, 0xca, 0xf4, 0x0e, 0xf3, 0xb1, 0x87, 0xdd, 0xc2, 0xc2, 0xf1, 0x8c, 0xc4, 0xdc, 0xf3, 
+    0xb9, 0xac, 0xf8, 0xd8, 0xf2, 0xb2, 0xb6, 0xba, 0xad, 0xfa, 0x8d, 0x9d, 0xab, 0x39, 0xd9, 0xad, 
+    0xdf, 0xf4, 0x13, 0xdd, 0xf2, 0xb1, 0x87, 0xb5, 0x9a, 0x08, 0x08, 0xf1, 0xb1, 0x83, 0xc2, 0xc4, 
+    0xc6, 0xdc, 0xf3, 0xb9, 0xac, 0xf8, 0xd8, 0xf3, 0xb2, 0xb6, 0xba, 0xad, 0xf8, 0x8d, 0x9d, 0xab, 
+    0x11, 0xd9, 0xad, 0xde, 0xf4, 0x13, 0xdd, 0xf2, 0xb1, 0x87, 0xb5, 0x9a, 0x28, 0x28, 0xf1, 0xb0, 
+    0x88, 0xc2, 0xc4, 0xc6, 0xdc, 0xf3, 0xb9, 0xac, 0xf8, 0xd8, 0xf3, 0xb2, 0xb6, 0xba, 0xad, 0xfa, 
+    0x8d, 0x9d, 0xab, 0x39, 0xd9, 0xad, 0xdf, 0xf4, 0x12, 0xdd, 0xf3, 0xb1, 0x87, 0xb5, 0x9a, 0x08, 
+    0xf2, 0xf2, 0x88, 0xc2, 0xc4, 0xc6, 0xdc, 0xf3, 0xb9, 0xac, 0xf8, 0xd8, 0xf2, 0xb2, 0xb6, 0xba, 
+    0xae, 0xf8, 0x8e, 0x9e, 0xab, 0x11, 0xd9, 0xae, 0xde, 0xf4, 0x12, 0xdd, 0xf3, 0xb1, 0x87, 0xb5, 
+    0x9a, 0x68, 0xf2, 0xb0, 0x80, 0xc0, 0xc8, 0xc2, 0xdc, 0xf3, 0xb9, 0xac, 0xf8, 0xd8, 0xf2, 0xb2, 
+    0xb6, 0xba, 0xae, 0xfa, 0x8e, 0x9e, 0xab, 0x39, 0xd9, 0xae, 0xdf, 0xf4, 0x12, 0xdd, 0xf2, 0xb1, 
+    0x87, 0xb5, 0x9a, 0x68, 0xf2, 0xb0, 0x80, 0xc4, 0xcc, 0xc6, 0xdc, 0xf3, 0xb9, 0xac, 0xf8, 0xd8, 
+    0xf3, 0xb2, 0xb6, 0xba, 0xae, 0xf8, 0x8e, 0x9e, 0xab, 0x11, 0xd9, 0xae, 0xde, 0xf4, 0x12, 0xdd, 
+    /* bank # 7 */
+    0xf3, 0xb1, 0x87, 0xb5, 0x9a, 0x48, 0xf2, 0xb0, 0x81, 0xc0, 0xc8, 0xc2, 0xdc, 0xf3, 0xb9, 0xac, 
+    0xf8, 0xd8, 0xf3, 0xb2, 0xb6, 0xba, 0xae, 0xfa, 0x8e, 0x9e, 0xab, 0x39, 0xd9, 0xae, 0xdf, 0xf4, 
+    0x12, 0xdd, 0xf2, 0xb1, 0x87, 0xb5, 0x9a, 0x48, 0xf2, 0xb0, 0x81, 0xc4, 0xcc, 0xc6, 0xdc, 0xf3, 
+    0xb9, 0xac, 0xf8, 0xd8, 0xf2, 0xb0, 0x88, 0xb9, 0xa8, 0xc3, 0xc5, 0xc7, 0xb1, 0xb5, 0xb9, 0xf3, 
+    0xac, 0xfa, 0xf9, 0xd1, 0xda, 0xf4, 0x10, 0xd8, 0xf3, 0xac, 0xf8, 0xf9, 0xd1, 0xda, 0xf1, 0xb9, 
+    0xaa, 0xdf, 0xf4, 0x1c, 0xd9, 0xf4, 0x1a, 0xd8, 0xf3, 0xac, 0xf8, 0xf9, 0xd1, 0xd9, 0xf4, 0x1d, 
+    0xd8, 0xf2, 0x8c, 0xac, 0xf8, 0xf9, 0xd1, 0xd9, 0xc2, 0xd8, 0xf2, 0xf9, 0xd9, 0xde, 0xf4, 0x09, 
+    0x1d, 0x16, 0xda, 0xf2, 0xdd, 0xc6, 0xdc, 0xf4, 0x30, 0xf1, 0xb9, 0xaa, 0xdf, 0xd8, 0xf1, 0xab, 
+    0xfa, 0x8b, 0x9b, 0xa3, 0x69, 0xd9, 0xf4, 0x07, 0x06, 0xda, 0xf1, 0xb9, 0xab, 0xdf, 0xfe, 0xd8, 
+    0xf1, 0xbb, 0xb3, 0xb7, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28, 0xb4, 0x80, 0x98, 
+    0xa7, 0x20, 0xd8, 0xf1, 0xb3, 0xb7, 0xbb, 0x87, 0xaa, 0xd0, 0xc1, 0xa7, 0x81, 0x97, 0x62, 0x93, 
+    0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 
+    0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83, 0x90, 0x28, 0x4c, 0x6c, 0x8a, 0x6c, 0xf2, 0xb0, 
+    0xb4, 0xb8, 0x83, 0x93, 0xa5, 0x71, 0xdb, 0xf1, 0x8c, 0x01, 0xa3, 0xc0, 0xd8, 0xf2, 0x83, 0xa5, 
+    0x71, 0xd9, 0xa3, 0xd0, 0xf8, 0xf1, 0x9c, 0xa5, 0xdb, 0x21, 0x8c, 0xa3, 0xc1, 0xd8, 0xf2, 0x83, 
+    0xa5, 0xc7, 0xf9, 0xf1, 0x93, 0x25, 0xf2, 0xa5, 0x95, 0x49, 0xd9, 0xf3, 0xa3, 0xd0, 0xde, 0xf8, 
+    /* bank # 8 */
+    0xf1, 0x84, 0xa5, 0xd0, 0xdb, 0x55, 0xf3, 0xa3, 0xd0, 0xde, 0xd8, 0xf2, 0xa5, 0x83, 0x95, 0x49, 
+    0xdb, 0xf3, 0x8e, 0x93, 0x11, 0xf1, 0x85, 0x94, 0xa5, 0x79, 0x9e, 0x83, 0xf3, 0x69, 0xa3, 0xd0, 
+    0xf8, 0xd8, 0xf2, 0xa5, 0x83, 0x95, 0x49, 0xd9, 0xf3, 0xa3, 0xd0, 0xc5, 0xf2, 0xa3, 0xd0, 0xde, 
+    0xf1, 0xa3, 0xde, 0x87, 0xc5, 0xd8, 0xf3, 0x8e, 0x93, 0xa5, 0x11, 0xdb, 0xf2, 0x8d, 0xb7, 0x92, 
+    0x31, 0xbb, 0xa2, 0xd0, 0xc2, 0xa8, 0xb3, 0x82, 0xb4, 0x9d, 0x21, 0xb0, 0x8d, 0xb7, 0x92, 0x49, 
+    0xa2, 0xdf, 0xd8, 0xf3, 0xb4, 0x9e, 0xb0, 0x83, 0xb8, 0xa5, 0x49, 0xdb, 0xf2, 0xb3, 0x82, 0x9d, 
+    0x41, 0x21, 0xb0, 0x84, 0xb7, 0x92, 0x09, 0xd9, 0xb3, 0x82, 0xbb, 0xa8, 0xf1, 0xca, 0x96, 0x88, 
+    0xa6, 0x00, 0xd8, 0xf3, 0xb7, 0x90, 0xb0, 0x83, 0xb8, 0xa5, 0x49, 0xdb, 0xf2, 0xb3, 0x82, 0xb4, 
+    0x9d, 0x41, 0xb0, 0x8d, 0xbb, 0xa2, 0xd0, 0xc0, 0xb3, 0x82, 0xb4, 0x9d, 0xa8, 0x21, 0xa2, 0xdf, 
+    0xd8, 0xf3, 0xbb, 0xb3, 0xb7, 0x90, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19, 0x80, 0xa2, 
+    0xf2, 0xd9, 0x26, 0xf3, 0xa7, 0xd0, 0xdf, 0xd8, 0xf1, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 
+    0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80, 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0x85, 0xf5, 0x75, 0xda, 
+    0xff, 0xd8, 0x71, 0x80, 0xa9, 0xda, 0xf1, 0xff, 0xd8, 0xd8, 0xf1, 0xb2, 0xb7, 0xba, 0x83, 0x99, 
+    0xa4, 0x09, 0xda, 0xa3, 0xdf, 0xf9, 0xc7, 0xf4, 0x0b, 0xd8, 0xf1, 0xa3, 0xfb, 0xf8, 0xdb, 0xfb, 
+    0xd9, 0xde, 0xa3, 0xdf, 0xd8, 0xf1, 0xa3, 0xfa, 0xf9, 0xbb, 0xa9, 0xd0, 0x84, 0xda, 0xc2, 0xd9, 
+    0xc4, 0xd8, 0xb3, 0x82, 0xa7, 0xf3, 0xc1, 0xf2, 0x80, 0xc2, 0xf1, 0x97, 0x86, 0x49, 0x2e, 0xa6, 
+    /* bank # 9 */
+    0xd0, 0x50, 0x96, 0x86, 0xa8, 0x75, 0xd9, 0x88, 0xa2, 0xd0, 0xf3, 0xc4, 0xc7, 0xf1, 0xda, 0x88, 
+    0x96, 0xa2, 0xd0, 0xf3, 0xc2, 0xc3, 0x82, 0xb4, 0x94, 0x68, 0x68, 0xf1, 0xd8, 0x80, 0xb7, 0x90, 
+    0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89, 0x99, 0xa8, 0x31, 
+    0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0xa8, 0x82, 
+    0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22, 0xf1, 0xa9, 
+    0x22, 0x26, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2, 0xf2, 0xde, 0xf1, 0xa9, 0xdf, 
+    0x97, 0x8c, 0xa8, 0xf3, 0x79, 0xda, 0xf1, 0xf1, 0xf1, 0xf1, 0xb1, 0x88, 0xb9, 0xac, 0xd0, 0xc0, 
+    0xb3, 0xf3, 0xf3, 0xb9, 0xaa, 0xfa, 0xf1, 0xbb, 0xaa, 0xd0, 0xf8, 0xf4, 0x10, 0xd8, 0xf3, 0xa7, 
+    0xd0, 0xfa, 0x97, 0x8c, 0xa8, 0x79, 0xda, 0xf1, 0x87, 0x9a, 0xaa, 0xd0, 0x70, 0xd8, 0xf2, 0x82, 
+    0x92, 0xa8, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00, 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 
+    0xd0, 0xde, 0xf1, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10, 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 
+    0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88, 0xa6, 0xd9, 0x00, 0xd8, 0xff
+};
+
+#define DMP_VERSION (dmpMemory)
+
+inv_error_t inv_write_dmp_data(FILE *fd, const unsigned char *dmp, size_t len)
+{
+    inv_error_t result = INV_SUCCESS;
+    int bytesWritten = 0;
+   
+    if (len <= 0) {
+        MPL_LOGE("Nothing to write");
+        return INV_ERROR_FILE_WRITE;
+    }
+    else {
+        MPL_LOGI("dmp firmware size to write = %d", len);
+    }
+    if ( fd == NULL ) {
+        return INV_ERROR_FILE_OPEN;
+    }
+    bytesWritten = fwrite(dmp, 1, len, fd);
+    if (bytesWritten != len) {
+        MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
+                 bytesWritten, len);
+        result = INV_ERROR_FILE_WRITE;
+    }
+    else {
+        MPL_LOGI("Bytes written = %d", bytesWritten);
+    }
+    return result;
+}
+
+inv_error_t inv_load_dmp(FILE *fd)
+{
+    inv_error_t result = INV_SUCCESS;
+    result = inv_write_dmp_data(fd, DMP_VERSION, DMP_CODE_SIZE);
+    return result;
+}
+
+void read_dmp_img(char *dmp_path, char* out_file)
+{
+    MPL_LOGI("read_dmp_img"); 
+    FILE *fp;
+    int i;
+    int dmpSize = DMP_CODE_SIZE;
+    char dmp_img[dmpSize];
+
+    if ((fp = fopen(dmp_path, "rb")) < 0) {
+        perror("dmp fail");
+    }
+    i = fread(dmp_img, 1, dmpSize, fp);
+    MPL_LOGI("Result=%d", i);
+    fclose(fp);
+    fp = fopen(out_file, "wt");
+    if(fp == NULL) {
+    MPL_LOGE("error open out file:%s", out_file);
+        return;
+    }
+    fprintf(fp, "char rec[]={\n");
+    for(i = 0; i < dmpSize; i++) {
+        fprintf(fp, "0x%02x, ", dmp_img[i]);
+        if(((i + 1) % 16) == 0) {
+            fprintf(fp, "\n");
+        }
+    }
+    fprintf(fp, "};\n ");
+    fclose(fp);
+}
+
+/**
+ *  @}
+ */
diff --git a/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h b/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h
new file mode 100644
index 0000000..3369f37
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h
@@ -0,0 +1,34 @@
+/*
+ $License:
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+#ifndef INV_LOAD_DMP_H
+#define INV_LOAD_DMP_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+    Includes.
+*/
+#include "mltypes.h"
+
+/*
+    APIs
+*/
+inv_error_t inv_load_dmp(FILE  *fd);
+void read_dmp_img(char *dmp_path, char *out_file);
+
+#ifdef __cplusplus
+}
+#endif
+#endif  /* INV_LOAD_DMP_H */
diff --git a/6515/libsensors_iio/software/core/mllite/linux/ml_stored_data.c b/6515/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
new file mode 100644
index 0000000..bf4520d
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
@@ -0,0 +1,351 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id: ml_stored_data.c 6132 2011-10-01 03:17:27Z mcaramello $
+ *
+ *****************************************************************************/
+
+/**
+ * @defgroup ML_STORED_DATA
+ *
+ * @{
+ *      @file     ml_stored_data.c
+ *      @brief    functions for reading and writing stored data sets.
+ *                Typically, these functions process stored calibration data.
+ */
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */
+#undef MPL_LOG_TAG
+
+#include <stdio.h>
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-storeload"
+
+#include "ml_stored_data.h"
+#include "storage_manager.h"
+#include "mlos.h"
+
+#define LOADCAL_DEBUG    0
+#define STORECAL_DEBUG   0
+
+#define DEFAULT_KEY 29681
+
+#define STORECAL_LOG MPL_LOGI
+#define LOADCAL_LOG  MPL_LOGI
+
+inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead)
+{
+    FILE *fp;
+    inv_error_t result = INV_SUCCESS;
+    size_t fsize;
+
+    fp = fopen(MLCAL_FILE,"rb");
+    if (fp == NULL) {
+        MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
+        return INV_ERROR_FILE_OPEN;
+    }
+
+    // obtain file size
+    fseek (fp, 0 , SEEK_END);
+    fsize = ftell (fp);
+    rewind (fp);
+  
+    *calData = (unsigned char *)inv_malloc(fsize);
+    if (*calData==NULL) {
+        MPL_LOGE("Could not allocate buffer of %d bytes - "
+                 "aborting\n", fsize);
+        fclose(fp);
+        return INV_ERROR_MEMORY_EXAUSTED;
+    }
+
+    *bytesRead = fread(*calData, 1, fsize, fp);
+    if (*bytesRead != fsize) {
+        MPL_LOGE("bytes read (%d) don't match file size (%d)\n",
+                 *bytesRead, fsize);
+        result = INV_ERROR_FILE_READ;
+        goto read_cal_end;
+    } else {
+        MPL_LOGV("Bytes read = %d", *bytesRead);
+    }
+
+read_cal_end:
+    fclose(fp);
+    return result;
+}
+
+inv_error_t inv_write_cal(unsigned char *cal, size_t len)
+{
+    FILE *fp;
+    int bytesWritten;
+    inv_error_t result = INV_SUCCESS;
+   
+    if (len <= 0) {
+        MPL_LOGE("Nothing to write");
+        return INV_ERROR_FILE_WRITE;
+    } else {
+        MPL_LOGV("cal data size to write = %d", len);
+    }
+    fp = fopen(MLCAL_FILE,"wb");
+    if (fp == NULL) {
+        MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE);
+        return INV_ERROR_FILE_OPEN;
+    }
+    bytesWritten = fwrite(cal, 1, len, fp);
+    if (bytesWritten != len) {
+        MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
+                 bytesWritten, len);
+        result = INV_ERROR_FILE_WRITE;
+    } else {
+        MPL_LOGV("Bytes written = %d", bytesWritten);
+    }
+    fclose(fp);
+    return result;
+}
+
+/**
+ *  @brief  Loads a type 0 set of calibration data.
+ *          It parses a binary data set containing calibration data.
+ *          The binary data set is intended to be loaded from a file.
+ *          This calibrations data format stores values for (in order of
+ *          appearance) :
+ *          - temperature compensation : temperature data points,
+ *          - temperature compensation : gyro biases data points for X, Y,
+ *              and Z axes.
+ *          - accel biases for X, Y, Z axes.
+ *          This calibration data is produced internally by the MPL and its
+ *          size is 2777 bytes (header and checksum included).
+ *          Calibration format type 1 is currently used for ITG3500
+ *
+ *  @pre    inv_init_storage_manager()
+ *          must have been called.
+ *
+ *  @param  calData
+ *              A pointer to an array of bytes to be parsed.
+ *  @param  len
+ *              the length of the calibration
+ *
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len)
+{
+    inv_error_t result;
+
+    LOADCAL_LOG("Entering inv_load_cal_V0\n");
+
+    /*if (len != expLen) {
+        MPL_LOGE("Calibration data type 0 must be %d bytes long (got %d)\n",
+                 expLen, len);
+        return INV_ERROR_FILE_READ;
+    }*/
+
+    result = inv_load_mpl_states(calData, len);
+    return result;
+}
+
+/**
+ *  @brief  Loads a type 1 set of calibration data.
+ *          It parses a binary data set containing calibration data.
+ *          The binary data set is intended to be loaded from a file.
+ *          This calibrations data format stores values for (in order of
+ *          appearance) :
+ *          - temperature,
+ *          - gyro biases for X, Y, Z axes,
+ *          - accel biases for X, Y, Z axes.
+ *          This calibration data would normally be produced by the MPU Self
+ *          Test and its size is 36 bytes (header and checksum included).
+ *          Calibration format type 1 is produced by the MPU Self Test and
+ *          substitutes the type 0 : inv_load_cal_V0().
+ *
+ *  @pre    
+ *
+ *  @param  calData
+ *              A pointer to an array of bytes to be parsed.
+ *  @param  len
+ *              the length of the calibration
+ *
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len)
+{
+    return INV_SUCCESS;
+}
+
+/**
+ * @brief   Loads a set of calibration data.
+ *          It parses a binary data set containing calibration data.
+ *          The binary data set is intended to be loaded from a file.
+ *
+ * @pre     
+ *          
+ *
+ * @param   calData
+ *              A pointer to an array of bytes to be parsed.
+ *
+ * @return  INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_load_cal(unsigned char *calData)
+{
+    int calType = 0;
+    int len = 0;
+    //int ptr;
+    //uint32_t chk = 0;
+    //uint32_t cmp_chk = 0;
+
+    /*load_func_t loaders[] = {
+        inv_load_cal_V0,
+        inv_load_cal_V1,
+    };
+    */
+
+    inv_load_cal_V0(calData, len);
+
+    /* read the header (type and len)
+       len is the total record length including header and checksum */
+    len = 0;
+    len += 16777216L * ((int)calData[0]);
+    len += 65536L * ((int)calData[1]);
+    len += 256 * ((int)calData[2]);
+    len += (int)calData[3];
+
+    calType = ((int)calData[4]) * 256 + ((int)calData[5]);
+    if (calType > 5) {
+        MPL_LOGE("Unsupported calibration file format %d. "
+                 "Valid types 0..5\n", calType);
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    /* call the proper method to read in the data */
+    //return loaders[calType] (calData, len);
+    return 0;
+}
+
+/**
+ *  @brief  Stores a set of calibration data.
+ *          It generates a binary data set containing calibration data.
+ *          The binary data set is intended to be stored into a file.
+ *
+ *  @pre    inv_dmp_open()
+ *
+ *  @param  calData
+ *              A pointer to an array of bytes to be stored.
+ *  @param  length
+ *              The amount of bytes available in the array.
+ *
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_store_cal(unsigned char *calData, size_t length)
+{
+    inv_error_t res = 0;
+    size_t size;
+
+    STORECAL_LOG("Entering inv_store_cal\n");
+
+    inv_get_mpl_state_size(&size);
+
+    MPL_LOGV("inv_get_mpl_state_size() : size=%d", size);
+
+    /* store data */
+    res = inv_save_mpl_states(calData, size);
+    if(res != 0) {
+        MPL_LOGE("inv_save_mpl_states() failed");
+    }
+
+    STORECAL_LOG("Exiting inv_store_cal\n");
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  Load a calibration file.
+ *
+ *  @pre    Must be in INV_STATE_DMP_OPENED state.
+ *          inv_dmp_open() or inv_dmp_stop() must have been called.
+ *          inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
+ *          been called.
+ *
+ *  @return 0 or error code.
+ */
+inv_error_t inv_load_calibration(void)
+{
+    unsigned char *calData= NULL;
+    inv_error_t result = 0;
+    size_t bytesRead = 0;
+
+    result = inv_read_cal(&calData, &bytesRead);
+    if(result != INV_SUCCESS) {
+        MPL_LOGE("Could not load cal file - "
+                 "aborting\n");
+        goto free_mem_n_exit;
+    }
+
+    result = inv_load_mpl_states(calData, bytesRead);
+    if (result != INV_SUCCESS) {
+        MPL_LOGE("Could not load the calibration data - "
+                 "error %d - aborting\n", result);
+        goto free_mem_n_exit;
+    }
+
+free_mem_n_exit:
+    inv_free(calData);
+    return result;
+}
+
+/**
+ *  @brief  Store runtime calibration data to a file
+ *
+ *  @pre    Must be in INV_STATE_DMP_OPENED state.
+ *          inv_dmp_open() or inv_dmp_stop() must have been called.
+ *          inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
+ *          been called.
+ *
+ *  @return 0 or error code.
+ */
+inv_error_t inv_store_calibration(void)
+{
+    unsigned char *calData;
+    inv_error_t result;
+    size_t length;
+
+    result = inv_get_mpl_state_size(&length);
+    calData = (unsigned char *)inv_malloc(length);
+    if (!calData) {
+        MPL_LOGE("Could not allocate buffer of %d bytes - "
+                 "aborting\n", length);
+        return INV_ERROR_MEMORY_EXAUSTED;
+    } else {
+        MPL_LOGV("inv_get_mpl state size = %d", length);
+    }
+
+    result = inv_save_mpl_states(calData, length);
+    if (result != INV_SUCCESS) {
+        MPL_LOGE("Could not save mpl states - "
+                 "error %d - aborting\n", result);
+        goto free_mem_n_exit;
+    } else {
+        MPL_LOGV("calData from inv_save_mpl_states, size=%d", 
+                 strlen((char *)calData));
+    }
+
+    result = inv_write_cal(calData, length);
+    if (result != INV_SUCCESS) {
+        MPL_LOGE("Could not store calibrated data on file - "
+                 "error %d - aborting\n", result);
+        goto free_mem_n_exit;
+
+    }
+
+free_mem_n_exit:
+    inv_free(calData);
+    return result;
+}
+
+/**
+ *  @}
+ */
diff --git a/6515/libsensors_iio/software/core/mllite/linux/ml_stored_data.h b/6515/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
new file mode 100644
index 0000000..115b34c
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
@@ -0,0 +1,53 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id: ml_stored_data.h 5873 2011-08-11 03:13:48Z mcaramello $
+ *
+ ******************************************************************************/
+
+#ifndef INV_MPL_STORED_DATA_H
+#define INV_MPL_STORED_DATA_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+    Includes.
+*/
+#include "mltypes.h"
+
+/*
+    Defines
+*/
+#define MLCAL_FILE "/data/inv_cal_data.bin"
+
+/*
+    APIs
+*/
+inv_error_t inv_load_calibration(void);
+inv_error_t inv_store_calibration(void);
+
+/*
+    Internal APIs
+*/
+inv_error_t inv_read_cal(unsigned char **, size_t *);
+inv_error_t inv_write_cal(unsigned char *cal, size_t len);
+inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len);
+inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len);
+
+/*
+    Other prototypes
+*/
+inv_error_t inv_load_cal(unsigned char *calData);
+inv_error_t inv_store_cal(unsigned char *calData, size_t length);
+
+#ifdef __cplusplus
+}
+#endif
+#endif  /* INV_MPL_STORED_DATA_H */
diff --git a/6515/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c b/6515/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
new file mode 100644
index 0000000..cbf69f8
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
@@ -0,0 +1,526 @@
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MLLITE"
+
+#include <string.h>
+#include <stdio.h>
+#include "ml_sysfs_helper.h"
+#include <dirent.h>
+#include <ctype.h>
+#include "log.h"
+
+#define MPU_SYSFS_ABS_PATH "/sys/class/invensense/mpu"
+
+enum PROC_SYSFS_CMD {
+	CMD_GET_SYSFS_PATH,
+	CMD_GET_DMP_PATH,
+	CMD_GET_CHIP_NAME,
+	CMD_GET_SYSFS_KEY,
+	CMD_GET_TRIGGER_PATH,
+	CMD_GET_DEVICE_NODE
+};
+static char sysfs_path[100];
+static char *chip_name[] = {
+    "ITG3500", 
+    "MPU6050", 
+    "MPU9150", 
+    "MPU3050", 
+    "MPU6500",
+    "MPU9250",
+    "MPU6XXX",
+    "MPU9350",
+    "MPU6515",
+};
+static int chip_ind;
+static int initialized =0;
+static int status = 0;
+static int iio_initialized = 0;
+static int iio_dev_num = 0;
+
+#define IIO_MAX_NAME_LENGTH 30
+
+#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements"
+#define FORMAT_TYPE_FILE "%s_type"
+
+#define CHIP_NUM ARRAY_SIZE(chip_name)
+
+static const char *iio_dir = "/sys/bus/iio/devices/";
+
+/**
+ * find_type_by_name() - function to match top level types by name
+ * @name: top level type instance name
+ * @type: the type of top level instance being sort
+ *
+ * Typical types this is used for are device and trigger.
+ **/
+int find_type_by_name(const char *name, const char *type)
+{
+	const struct dirent *ent;
+	int number, numstrlen;
+
+	FILE *nameFile;
+	DIR *dp;
+	char thisname[IIO_MAX_NAME_LENGTH];
+	char *filename;
+
+	dp = opendir(iio_dir);
+	if (dp == NULL) {
+		MPL_LOGE("No industrialio devices available");
+		return -ENODEV;
+	}
+
+	while (ent = readdir(dp), ent != NULL) {
+		if (strcmp(ent->d_name, ".") != 0 &&
+			strcmp(ent->d_name, "..") != 0 &&
+			strlen(ent->d_name) > strlen(type) &&
+			strncmp(ent->d_name, type, strlen(type)) == 0) {
+			numstrlen = sscanf(ent->d_name + strlen(type),
+					   "%d",
+					   &number);
+			/* verify the next character is not a colon */
+			if (strncmp(ent->d_name + strlen(type) + numstrlen,
+					":",
+					1) != 0) {
+				filename = malloc(strlen(iio_dir)
+						+ strlen(type)
+						+ numstrlen
+						+ 6);
+				if (filename == NULL)
+					return -ENOMEM;
+				sprintf(filename, "%s%s%d/name",
+					iio_dir,
+					type,
+					number);
+				nameFile = fopen(filename, "r");
+				if (!nameFile)
+					continue;
+				free(filename);
+				fscanf(nameFile, "%s", thisname);
+				if (strcmp(name, thisname) == 0)
+					return number;
+				fclose(nameFile);
+			}
+		}
+	}
+	return -ENODEV;
+}
+
+/* mode 0: search for which chip in the system and fill sysfs path
+   mode 1: return event number
+ */
+static int parsing_proc_input(int mode, char *name){
+	const char input[] = "/proc/bus/input/devices";
+	char line[4096], d;
+	char tmp[100];
+	FILE *fp;
+	int i, j, result, find_flag;
+	int event_number = -1;
+	int input_number = -1;
+
+	if(NULL == (fp = fopen(input, "rt")) ){
+		return -1;
+	}
+	result = 1;
+	find_flag = 0;
+	while(result != 0 && find_flag < 2){
+		i = 0;
+		d = 0;
+		memset(line, 0, 100);
+		while(d != '\n'){		
+			result = fread(&d, 1, 1, fp);
+			if(result == 0){
+				line[0] = 0;
+				break;
+			}
+			sprintf(&line[i], "%c", d);		
+			i ++;
+		}
+		if(line[0] == 'N'){
+			i = 1;
+			while(line[i] != '"'){
+				i++;
+			}
+			i++;
+			j = 0;
+			find_flag = 0;
+			if (mode == 0){
+				while(j < CHIP_NUM){
+					if(!memcmp(&line[i], chip_name[j], strlen(chip_name[j]))){
+						find_flag = 1;
+						chip_ind = j;
+					}
+					j++;
+				}
+			} else if (mode  != 0){
+				if(!memcmp(&line[i], name, strlen(name))){
+					find_flag = 1;
+				}
+			}
+		}		
+		if(find_flag){
+			if(mode == 0){
+				if(line[0] == 'S'){
+					memset(tmp, 0, 100);
+					i =1;
+					while(line[i] != '=') i++;
+					i++;
+					j = 0;
+					while(line[i] != '\n'){
+						tmp[j] = line[i];
+						i ++; j++;
+					}	
+					sprintf(sysfs_path, "%s%s", "/sys", tmp);
+					find_flag++;
+				}
+			} else if(mode == 1){
+				if(line[0] == 'H') {
+					i = 2;
+					while(line[i] != '=') i++;
+					while(line[i] != 't') i++;	
+					i++;
+					event_number = 0;
+					while(line[i] != '\n'){
+						if(line[i] >= '0' && line[i] <= '9')
+							event_number = event_number*10 + line[i]-0x30;
+						i ++;
+					}
+					find_flag ++;
+				}
+			} else if (mode == 2) {
+				if(line[0] == 'S'){
+					memset(tmp, 0, 100);
+					i =1;
+					while(line[i] != '=') i++;
+					i++;
+					j = 0;
+					while(line[i] != '\n'){
+						tmp[j] = line[i];
+						i ++; j++;
+					}
+					input_number = 0;
+					if(tmp[j-2] >= '0' && tmp[j-2] <= '9') 
+						input_number += (tmp[j-2]-0x30)*10;
+					if(tmp[j-1] >= '0' && tmp[j-1] <= '9') 
+						input_number += (tmp[j-1]-0x30);
+					find_flag++;
+				}
+			}
+		}
+	}
+	fclose(fp);
+	if(find_flag == 0){
+		return -1;
+	}
+	if(0 == mode)
+		status = 1;
+	if (mode == 1)
+		return event_number;
+	if (mode == 2)
+		return input_number;
+	return 0;
+
+}
+static void init_iio() {
+	int i, j;
+	char iio_chip[10];
+	int dev_num;
+	for(j=0; j< CHIP_NUM; j++) {
+		for (i=0; i<strlen(chip_name[j]); i++) {
+			iio_chip[i] = tolower(chip_name[j][i]);
+		}
+		iio_chip[strlen(chip_name[j])] = '\0';
+		dev_num = find_type_by_name(iio_chip, "iio:device");
+		if(dev_num >= 0) {
+			iio_initialized = 1;
+			iio_dev_num = dev_num;
+			chip_ind = j;
+		}
+	}
+}
+
+static int process_sysfs_request(enum PROC_SYSFS_CMD cmd, char *data)
+{
+	char key_path[100];
+	FILE *fp;
+	int i, result;
+	if(initialized == 0){
+		parsing_proc_input(0, NULL);
+		initialized = 1;
+	}
+	if(initialized && status == 0) {
+		init_iio();
+		if (iio_initialized == 0)
+			return -1;
+	}
+
+	memset(key_path, 0, 100);
+	switch(cmd){
+	case CMD_GET_SYSFS_PATH:
+		if (iio_initialized == 1)
+			sprintf(data, "/sys/bus/iio/devices/iio:device%d", iio_dev_num);
+		else
+			sprintf(data, "%s%s", sysfs_path, "/device/invensense/mpu");
+		break;
+	case CMD_GET_DMP_PATH:
+		if (iio_initialized == 1)
+			sprintf(data, "/sys/bus/iio/devices/iio:device%d/dmp_firmware", iio_dev_num);
+		else
+			sprintf(data, "%s%s", sysfs_path, "/device/invensense/mpu/dmp_firmware");
+		break;
+	case CMD_GET_CHIP_NAME:
+		sprintf(data, "%s", chip_name[chip_ind]);
+		break;
+	case CMD_GET_TRIGGER_PATH:
+		sprintf(data, "/sys/bus/iio/devices/trigger%d", iio_dev_num);
+		break;
+	case CMD_GET_DEVICE_NODE:
+		sprintf(data, "/dev/iio:device%d", iio_dev_num);
+		break;
+	case CMD_GET_SYSFS_KEY:
+		memset(key_path, 0, 100);
+		if (iio_initialized == 1)
+			sprintf(key_path, "/sys/bus/iio/devices/iio:device%d/key", iio_dev_num);
+		else	
+			sprintf(key_path, "%s%s", sysfs_path, "/device/invensense/mpu/key");
+
+		if((fp = fopen(key_path, "rt")) == NULL)
+			return -1;
+		for(i=0;i<16;i++){
+			fscanf(fp, "%02x", &result);
+			data[i] = (char)result;
+		}
+		
+		fclose(fp);
+		break;
+	default:
+		break;
+	}
+	return 0;
+}
+
+int find_name_by_sensor_type(const char *sensor_type, const char *type, char *sensor_name)
+{
+    const struct dirent *ent;
+    int number, numstrlen;
+
+    FILE *nameFile;
+    DIR *dp;
+    char *filename;
+
+    dp = opendir(iio_dir);
+    if (dp == NULL) {
+        MPL_LOGE("No industrialio devices available");
+        return -ENODEV;
+    }
+
+    while (ent = readdir(dp), ent != NULL) {
+        if (strcmp(ent->d_name, ".") != 0 &&
+            strcmp(ent->d_name, "..") != 0 &&
+            strlen(ent->d_name) > strlen(type) &&
+            strncmp(ent->d_name, type, strlen(type)) == 0) {
+            numstrlen = sscanf(ent->d_name + strlen(type),
+                       "%d",
+                       &number);
+            /* verify the next character is not a colon */
+            if (strncmp(ent->d_name + strlen(type) + numstrlen,
+                    ":",
+                    1) != 0) {
+                filename = malloc(strlen(iio_dir)
+                        + strlen(type)
+                        + numstrlen
+                        + 6
+                        + strlen(sensor_type));
+                if (filename == NULL)
+                    return -ENOMEM;
+                sprintf(filename, "%s%s%d/%s",
+                    iio_dir,
+                    type,
+                    number,
+                    sensor_type);
+                nameFile = fopen(filename, "r");
+                MPL_LOGI("sensor type path: %s\n", filename);
+                free(filename);
+                //fscanf(nameFile, "%s", thisname);
+                //if (strcmp(name, thisname) == 0) {
+                if(nameFile == NULL) {
+                    MPL_LOGI("keeps searching");
+                    continue;
+                } else{
+                    MPL_LOGI("found directory");
+                }
+                filename = malloc(strlen(iio_dir)
+                        + strlen(type)
+                        + numstrlen
+                        + 6);
+                sprintf(filename, "%s%s%d/name",
+                    iio_dir,
+                    type,
+                    number);
+                    nameFile = fopen(filename, "r");
+                    MPL_LOGI("name path: %s\n", filename);
+                    free(filename);
+                    if (!nameFile)
+                        continue;
+                    fscanf(nameFile, "%s", sensor_name);
+                    MPL_LOGI("name found: %s now test for mpuxxxx", sensor_name);
+                    if( !strncmp("mpu",sensor_name, 3) ) {                      
+                        char secondaryFileName[200];                    
+                    sprintf(secondaryFileName, "%s%s%d/secondary_name",
+                        iio_dir,
+                        type,
+                        number);
+                        nameFile = fopen(secondaryFileName, "r");
+                        MPL_LOGI("name path: %s\n", secondaryFileName); 
+                        if(!nameFile)
+                            continue;
+                        fscanf(nameFile, "%s", sensor_name);
+                        MPL_LOGI("secondary name found: %s\n", sensor_name);
+                    }
+                    else {
+                        fscanf(nameFile, "%s", sensor_name);
+                        MPL_LOGI("name found: %s\n", sensor_name);
+                    }
+                    return 0;
+                //}
+                fclose(nameFile);
+            }
+        }
+    }
+    return -ENODEV;
+}
+
+/** 
+ *  @brief  return sysfs key. if the key is not available
+ *          return false. So the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the key
+ *           It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_sysfs_key(unsigned char *key)
+{
+	if (process_sysfs_request(CMD_GET_SYSFS_KEY, (char*)key) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+
+/** 
+ *  @brief  return the sysfs path. If the path is not 
+ *          found yet. return false. So the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the sysfs
+ *           path. It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_sysfs_path(char *name)
+{
+	if (process_sysfs_request(CMD_GET_SYSFS_PATH, name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+
+inv_error_t inv_get_sysfs_abs_path(char *name)
+{
+    strcpy(name, MPU_SYSFS_ABS_PATH);
+    return INV_SUCCESS;
+}
+
+/** 
+ *  @brief  return the dmp file path. If the path is not 
+ *          found yet. return false. So the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the dmp file
+ *           path. It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_dmpfile(char *name)
+{
+   	if (process_sysfs_request(CMD_GET_DMP_PATH, name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+/** 
+ *  @brief  return the chip name. If the chip is not 
+ *          found yet. return false. So the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the chip name
+ *           path(8 bytes). It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_chip_name(char *name)
+{
+   	if (process_sysfs_request(CMD_GET_CHIP_NAME, name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+/** 
+ *  @brief  return event handler number. If the handler number is not found
+ *          return false. the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the chip name
+ *           path(8 bytes). It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ *  @int *num: event number store
+ */
+inv_error_t  inv_get_handler_number(const char *name, int *num)
+{
+	initialized = 0;
+	if ((*num = parsing_proc_input(1, (char *)name)) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;	
+}
+
+/** 
+ *  @brief  return input number. If the handler number is not found
+ *          return false. the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the chip name
+ *           path(8 bytes). It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ *  @int *num: input number store
+ */
+inv_error_t  inv_get_input_number(const char *name, int *num)
+{
+	initialized = 0;
+	if ((*num = parsing_proc_input(2, (char *)name)) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else {
+		return INV_SUCCESS;
+	}	
+}
+
+/** 
+ *  @brief  return iio trigger name. If iio is not initialized, return false.
+ *          So the return must be checked to make sure the numeber is valid.
+ *  @unsigned char *name: This should be array big enough to hold the trigger
+ *           name. It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_iio_trigger_path(const char *name)
+{
+	if (process_sysfs_request(CMD_GET_TRIGGER_PATH, (char *)name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+
+/** 
+ *  @brief  return iio device node. If iio is not initialized, return false.
+ *          So the return must be checked to make sure the numeber is valid.
+ *  @unsigned char *name: This should be array big enough to hold the device
+ *           node. It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_iio_device_node(const char *name)
+{
+	if (process_sysfs_request(CMD_GET_DEVICE_NODE, (char *)name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
diff --git a/6515/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h b/6515/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h
new file mode 100644
index 0000000..184d3b2
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h
@@ -0,0 +1,37 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id$
+ *
+ ******************************************************************************/
+
+#ifndef MLDMP_SYSFS_HELPER_H__
+#define MLDMP_SYSFS_HELPER_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "invensense.h"
+
+int find_type_by_name(const char *name, const char *type);
+int find_name_by_sensor_type(const char *sensor_type, const char *type, char *sensor_name);
+inv_error_t inv_get_sysfs_path(char *name);
+inv_error_t inv_get_sysfs_abs_path(char *name);
+inv_error_t inv_get_dmpfile(char *name);
+inv_error_t inv_get_chip_name(char *name);
+inv_error_t inv_get_sysfs_key(unsigned char *key);
+inv_error_t inv_get_handler_number(const char *name, int *num);
+inv_error_t inv_get_input_number(const char *name, int *num);
+inv_error_t inv_get_iio_trigger_path(const char *name);
+inv_error_t inv_get_iio_device_node(const char *name);
+
+#ifdef __cplusplus
+}
+#endif
+#endif	/* MLDMP_SYSFS_HELPER_H__ */
diff --git a/6515/libsensors_iio/software/core/mllite/linux/mlos.h b/6515/libsensors_iio/software/core/mllite/linux/mlos.h
new file mode 100644
index 0000000..d4f8912
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/linux/mlos.h
@@ -0,0 +1,99 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+#ifndef _MLOS_H
+#define _MLOS_H
+
+#ifndef __KERNEL__
+#include <stdio.h>
+#endif
+#include <pthread.h>
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if defined(LINUX) || defined(__KERNEL__)
+typedef pthread_mutex_t* HANDLE;
+#endif
+
+	/* ------------ */
+	/* - Defines. - */
+	/* ------------ */
+
+	/* - MLOSCreateFile defines. - */
+
+#define MLOS_GENERIC_READ         ((unsigned int)0x80000000)
+#define MLOS_GENERIC_WRITE        ((unsigned int)0x40000000)
+#define MLOS_FILE_SHARE_READ      ((unsigned int)0x00000001)
+#define MLOS_FILE_SHARE_WRITE     ((unsigned int)0x00000002)
+#define MLOS_OPEN_EXISTING        ((unsigned int)0x00000003)
+
+	/* ---------- */
+	/* - Enums. - */
+	/* ---------- */
+
+	/* --------------- */
+	/* - Structures. - */
+	/* --------------- */
+
+	/* --------------------- */
+	/* - Function p-types. - */
+	/* --------------------- */
+
+#ifndef __KERNEL__
+#include <string.h>
+	void *inv_malloc(unsigned int numBytes);
+	inv_error_t inv_free(void *ptr);
+	inv_error_t inv_create_mutex(HANDLE *mutex);
+	inv_error_t inv_lock_mutex(HANDLE mutex);
+	inv_error_t inv_unlock_mutex(HANDLE mutex);
+	FILE *inv_fopen(char *filename);
+	void inv_fclose(FILE *fp);
+
+	inv_error_t inv_destroy_mutex(HANDLE handle);
+
+	void inv_sleep(int mSecs);
+	unsigned long inv_get_tick_count(void);
+
+	/* Kernel implmentations */
+#define GFP_KERNEL (0x70)
+	static inline void *kmalloc(size_t size,
+				    unsigned int gfp_flags)
+	{
+        (void)gfp_flags;
+		return inv_malloc((unsigned int)size);
+	}
+	static inline void *kzalloc(size_t size, unsigned int gfp_flags)
+	{
+		void *tmp = inv_malloc((unsigned int)size);
+        (void)gfp_flags;
+		if (tmp)
+			memset(tmp, 0, size);
+		return tmp;
+	}
+	static inline void kfree(void *ptr)
+	{
+		inv_free(ptr);
+	}
+	static inline void msleep(long msecs)
+	{
+		inv_sleep(msecs);
+	}
+	static inline void udelay(unsigned long usecs)
+	{
+		inv_sleep((usecs + 999) / 1000);
+	}
+#else
+#include <linux/delay.h>
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+#endif				/* _MLOS_H */
diff --git a/6515/libsensors_iio/software/core/mllite/linux/mlos_linux.c b/6515/libsensors_iio/software/core/mllite/linux/mlos_linux.c
new file mode 100644
index 0000000..5424508
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/linux/mlos_linux.c
@@ -0,0 +1,190 @@
+/*
+ $License:
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $
+ *
+ ******************************************************************************/
+
+/**
+ *  @defgroup MLOS
+ *  @brief OS Interface.
+ *
+ *  @{
+ *      @file mlos.c
+ *      @brief OS Interface.
+ */
+
+/* ------------- */
+/* - Includes. - */
+/* ------------- */
+
+#include <sys/time.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <stdlib.h>
+#include <errno.h>
+
+#include "stdint_invensense.h"
+#include "mlos.h"
+
+
+/* -------------- */
+/* - Functions. - */
+/* -------------- */
+
+/**
+ *  @brief  Allocate space
+ *  @param  num_bytes  number of bytes
+ *  @return pointer to allocated space
+ */
+void *inv_malloc(unsigned int num_bytes)
+{
+    // Allocate space.
+    void *alloc_ptr = malloc(num_bytes);
+    return alloc_ptr;
+}
+
+
+/**
+ *  @brief  Free allocated space
+ *  @param  ptr pointer to space to deallocate
+ *  @return error code.
+ */
+inv_error_t inv_free(void *ptr)
+{
+    if (ptr)
+        free(ptr);
+    return INV_SUCCESS;
+}
+
+
+/**
+ *  @brief  Mutex create function
+ *  @param  mutex   pointer to mutex handle
+ *  @return error code.
+ */
+inv_error_t inv_create_mutex(HANDLE *mutex)
+{
+    int res;
+    pthread_mutex_t *pm = malloc(sizeof(pthread_mutex_t));
+    if(pm == NULL) 
+        return INV_ERROR;
+
+    res = pthread_mutex_init(pm, NULL);
+    if(res == -1) {
+        free(pm);
+        return INV_ERROR_OS_CREATE_FAILED;
+    }
+
+    *mutex = (HANDLE)pm;
+
+    return INV_SUCCESS;
+}
+
+
+/**
+ *  @brief  Mutex lock function
+ *  @param  mutex   Mutex handle
+ *  @return error code.
+ */
+inv_error_t inv_lock_mutex(HANDLE mutex)
+{
+    int res;
+    pthread_mutex_t *pm = (pthread_mutex_t *)mutex;
+
+    res = pthread_mutex_lock(pm);
+    if(res == -1)
+        return INV_ERROR_OS_LOCK_FAILED;
+
+    return INV_SUCCESS;
+}
+
+
+/**
+ *  @brief  Mutex unlock function
+ *  @param  mutex   mutex handle
+ *  @return error code.
+ */
+inv_error_t inv_unlock_mutex(HANDLE mutex)
+{
+    int res;
+    pthread_mutex_t *pm = (pthread_mutex_t *)mutex;
+
+    res = pthread_mutex_unlock(pm);
+    if(res == -1) 
+        return INV_ERROR_OS_LOCK_FAILED;
+
+    return INV_SUCCESS;
+}
+
+
+/**
+ *  @brief  open file
+ *  @param  filename    name of the file to open.
+ *  @return error code.
+ */
+FILE *inv_fopen(char *filename)
+{
+    FILE *fp = fopen(filename, "r");
+    return fp;
+}
+
+
+/**
+ *  @brief  close the file.
+ *  @param  fp  handle to file to close.
+ *  @return error code.
+ */
+void inv_fclose(FILE *fp)
+{
+    fclose(fp);
+}
+
+/**
+ *  @brief  Close Handle
+ *  @param  handle  handle to the resource.
+ *  @return Zero if success, an error code otherwise.
+ */
+inv_error_t inv_destroy_mutex(HANDLE handle)
+{
+    int error;
+    pthread_mutex_t *pm = (pthread_mutex_t *)handle;
+    error = pthread_mutex_destroy(pm);
+    if (error)
+        return errno;
+    free((void*) handle);
+
+    return INV_SUCCESS;}
+
+
+/**
+ *  @brief  Sleep function.
+ */
+void inv_sleep(int m_secs)
+{
+    usleep(m_secs * 1000);
+}
+
+
+/**
+ *  @brief  get system's internal tick count.
+ *          Used for time reference.
+ *  @return current tick count.
+ */
+unsigned long inv_get_tick_count()
+{
+    struct timeval tv;
+
+    if (gettimeofday(&tv, NULL) != 0)
+        return 0;
+
+    return (long)((tv.tv_sec * 1000000LL + tv.tv_usec) / 1000LL);
+}
+
+/** @} */
+
diff --git a/6515/libsensors_iio/software/core/mllite/message_layer.c b/6515/libsensors_iio/software/core/mllite/message_layer.c
new file mode 100644
index 0000000..d266d80
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/message_layer.c
@@ -0,0 +1,59 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+/**
+ *   @defgroup  Message_Layer message_layer
+ *   @brief     Motion Library - Message Layer
+ *              Holds Low Occurance messages
+ *
+ *   @{
+ *       @file message_layer.c
+ *       @brief Holds Low Occurance Messages.
+ */
+#include "message_layer.h"
+#include "log.h"
+
+struct message_holder_t {
+    long message;
+};
+
+static struct message_holder_t mh;
+
+/** Sets a message.
+* @param[in] set The flags to set.
+* @param[in] clear Before setting anything this will clear these messages,
+*                  which is useful for mutually exclusive messages such
+*                  a motion or no motion message.
+* @param[in] level Level of the messages. It starts at 0, and may increase
+*            in the future to allow more messages if the bit storage runs out.
+*/
+void inv_set_message(long set, long clear, int level)
+{
+    if (level == 0) {
+        mh.message &= ~clear;
+        mh.message |= set;
+    }
+}
+
+/** Returns Message Flags for Level 0 Messages.
+* Levels are to allow expansion of more messages in the future.
+* @param[in] clear If set, will clear the message. Typically this will be set
+*  for one reader, so that you don't get the same message over and over.
+* @return bit field to corresponding message.
+*/
+long inv_get_message_level_0(int clear)
+{
+    long msg;
+    msg = mh.message;
+    if (clear) {
+        mh.message = 0;
+    }
+    return msg;
+}
+
+/**
+ * @}
+ */
diff --git a/6515/libsensors_iio/software/core/mllite/message_layer.h b/6515/libsensors_iio/software/core/mllite/message_layer.h
new file mode 100644
index 0000000..321b5ee
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/message_layer.h
@@ -0,0 +1,49 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#ifndef INV_MESSAGE_LAYER_H__
+#define INV_MESSAGE_LAYER_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Level 0 Type Messages */
+/** A motion event has occured */
+#define INV_MSG_MOTION_EVENT                (0x01)
+/** A no motion event has occured */
+#define INV_MSG_NO_MOTION_EVENT             (0x02)
+/** A setting of the gyro bias has occured */
+#define INV_MSG_NEW_GB_EVENT                (0x04)
+/** A setting of the compass bias has occured */
+#define INV_MSG_NEW_CB_EVENT                (0x08)
+/** A setting of the accel bias has occured */
+#define INV_MSG_NEW_AB_EVENT                (0x10)
+/** Sensor fusion has switched from 9- to 6-axes
+    because of a magnetic disturbance */
+#define INV_MSG_6X_SF_EVENT    (0x020)
+/** Compass accuracy has dropped has dropped to 0
+    because of a magnetic disturbance */
+#define INV_MSG_HEADING_NOT_ACCURATE_EVENT  (0x40)
+/** A setting of the factory gyro bias has occured */
+#define INV_MSG_NEW_FGB_EVENT            (0x80)
+/** A setting of the factory accel bias has occured */
+#define INV_MSG_NEW_FAB_EVENT            (0x100)
+
+#ifdef WIN32
+#define INV_MSG_NEW_DMP_QUAT_WRITE_EVENT    (0x200)
+#endif
+
+void inv_set_message(long set, long clear, int level);
+long inv_get_message_level_0(int clear);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // INV_MESSAGE_LAYER_H__
diff --git a/6515/libsensors_iio/software/core/mllite/ml_math_func.c b/6515/libsensors_iio/software/core/mllite/ml_math_func.c
new file mode 100644
index 0000000..88e9934
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/ml_math_func.c
@@ -0,0 +1,1078 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+/**
+ *   @defgroup  ML_MATH_FUNC ml_math_func
+ *   @brief     Motion Library - Math Functions
+ *              Common math functions the Motion Library
+ *
+ *   @{
+ *       @file ml_math_func.c
+ *       @brief Math Functions.
+ */
+
+#include "mlmath.h"
+#include "ml_math_func.h"
+#include "mlinclude.h"
+#include <string.h>
+
+/** @internal
+ * Does the cross product of compass by gravity, then converts that
+ * to the world frame using the quaternion, then computes the angle that
+ * is made.
+ *
+ * @param[in] compass Compass Vector (Body Frame), length 3
+ * @param[in] grav Gravity Vector (Body Frame), length 3
+ * @param[in] quat Quaternion, Length 4
+ * @return Angle Cross Product makes after quaternion rotation.
+ */
+float inv_compass_angle(const long *compass, const long *grav, const float *quat)
+{
+    float cgcross[4], q1[4], q2[4], qi[4];
+    float angW;
+
+    // Compass cross Gravity
+    cgcross[0] = 0.f;
+    cgcross[1] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1];
+    cgcross[2] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2];
+    cgcross[3] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0];
+
+    // Now convert cross product into world frame
+    inv_q_multf(quat, cgcross, q1);
+    inv_q_invertf(quat, qi);
+    inv_q_multf(q1, qi, q2);
+
+    // Protect against atan2 of 0,0
+    if ((q2[2] == 0.f) && (q2[1] == 0.f))
+        return 0.f;
+
+    // This is the unfiltered heading correction
+    angW = -atan2f(q2[2], q2[1]);
+    return angW;
+}
+
+/**
+ *  @brief  The gyro data magnitude squared :
+ *          (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT.
+ * @param[in] gyro Gyro data scaled with 1 dps = 2^16
+ *  @return the computed magnitude squared output of the gyroscope.
+ */
+unsigned long inv_get_gyro_sum_of_sqr(const long *gyro)
+{
+    unsigned long gmag = 0;
+    long temp;
+    int kk;
+
+    for (kk = 0; kk < 3; ++kk) {
+        temp = gyro[kk] >> (16 - (GYRO_MAG_SQR_SHIFT / 2));
+        gmag += temp * temp;
+    }
+
+    return gmag;
+}
+
+/** Performs a multiply and shift by 29. These are good functions to write in assembly on
+ * with devices with small memory where you want to get rid of the long long which some
+ * assemblers don't handle well
+ * @param[in] a
+ * @param[in] b
+ * @return ((long long)a*b)>>29
+*/
+long inv_q29_mult(long a, long b)
+{
+#ifdef UMPL_ELIMINATE_64BIT
+    long result;
+    result = (long)((float)a * b / (1L << 29));
+    return result;
+#else
+    long long temp;
+    long result;
+    temp = (long long)a * b;
+    result = (long)(temp >> 29);
+    return result;
+#endif
+}
+
+/** Performs a multiply and shift by 30. These are good functions to write in assembly on
+ * with devices with small memory where you want to get rid of the long long which some
+ * assemblers don't handle well
+ * @param[in] a
+ * @param[in] b
+ * @return ((long long)a*b)>>30
+*/
+long inv_q30_mult(long a, long b)
+{
+#ifdef UMPL_ELIMINATE_64BIT
+    long result;
+    result = (long)((float)a * b / (1L << 30));
+    return result;
+#else
+    long long temp;
+    long result;
+    temp = (long long)a * b;
+    result = (long)(temp >> 30);
+    return result;
+#endif
+}
+
+#ifndef UMPL_ELIMINATE_64BIT
+long inv_q30_div(long a, long b)
+{
+    long long temp;
+    long result;
+    temp = (((long long)a) << 30) / b;
+    result = (long)temp;
+    return result;
+}
+#endif
+
+/** Performs a multiply and shift by shift. These are good functions to write
+ * in assembly on with devices with small memory where you want to get rid of
+ * the long long which some assemblers don't handle well
+ * @param[in] a First multicand
+ * @param[in] b Second multicand
+ * @param[in] shift Shift amount after multiplying
+ * @return ((long long)a*b)<<shift
+*/
+#ifndef UMPL_ELIMINATE_64BIT
+long inv_q_shift_mult(long a, long b, int shift)
+{
+    long result;
+    result = (long)(((long long)a * b) >> shift);
+    return result;
+}
+#endif
+
+/** Performs a fixed point quaternion multiply.
+* @param[in] q1 First Quaternion Multicand, length 4. 1.0 scaled
+*            to 2^30
+* @param[in] q2 Second Quaternion Multicand, length 4. 1.0 scaled
+*            to 2^30
+* @param[out] qProd Product after quaternion multiply. Length 4.
+*             1.0 scaled to 2^30.
+*/
+void inv_q_mult(const long *q1, const long *q2, long *qProd)
+{
+    INVENSENSE_FUNC_START;
+    qProd[0] = inv_q30_mult(q1[0], q2[0]) - inv_q30_mult(q1[1], q2[1]) -
+               inv_q30_mult(q1[2], q2[2]) - inv_q30_mult(q1[3], q2[3]);
+
+    qProd[1] = inv_q30_mult(q1[0], q2[1]) + inv_q30_mult(q1[1], q2[0]) +
+               inv_q30_mult(q1[2], q2[3]) - inv_q30_mult(q1[3], q2[2]);
+
+    qProd[2] = inv_q30_mult(q1[0], q2[2]) - inv_q30_mult(q1[1], q2[3]) +
+               inv_q30_mult(q1[2], q2[0]) + inv_q30_mult(q1[3], q2[1]);
+
+    qProd[3] = inv_q30_mult(q1[0], q2[3]) + inv_q30_mult(q1[1], q2[2]) -
+               inv_q30_mult(q1[2], q2[1]) + inv_q30_mult(q1[3], q2[0]);
+}
+
+/** Performs a fixed point quaternion addition.
+* @param[in] q1 First Quaternion term, length 4. 1.0 scaled
+*            to 2^30
+* @param[in] q2 Second Quaternion term, length 4. 1.0 scaled
+*            to 2^30
+* @param[out] qSum Sum after quaternion summation. Length 4.
+*             1.0 scaled to 2^30.
+*/
+void inv_q_add(long *q1, long *q2, long *qSum)
+{
+    INVENSENSE_FUNC_START;
+    qSum[0] = q1[0] + q2[0];
+    qSum[1] = q1[1] + q2[1];
+    qSum[2] = q1[2] + q2[2];
+    qSum[3] = q1[3] + q2[3];
+}
+
+void inv_vector_normalize(long *vec, int length)
+{
+    INVENSENSE_FUNC_START;
+    double normSF = 0;
+    int ii;
+    for (ii = 0; ii < length; ii++) {
+        normSF +=
+            inv_q30_to_double(vec[ii]) * inv_q30_to_double(vec[ii]);
+    }
+    if (normSF > 0) {
+        normSF = 1 / sqrt(normSF);
+        for (ii = 0; ii < length; ii++) {
+            vec[ii] = (int)((double)vec[ii] * normSF);
+        }
+    } else {
+        vec[0] = 1073741824L;
+        for (ii = 1; ii < length; ii++) {
+            vec[ii] = 0;
+        }
+    }
+}
+
+void inv_q_normalize(long *q)
+{
+    INVENSENSE_FUNC_START;
+    inv_vector_normalize(q, 4);
+}
+
+void inv_q_invert(const long *q, long *qInverted)
+{
+    INVENSENSE_FUNC_START;
+    qInverted[0] = q[0];
+    qInverted[1] = -q[1];
+    qInverted[2] = -q[2];
+    qInverted[3] = -q[3];
+}
+
+double quaternion_to_rotation_angle(const long *quat) {
+    double quat0 = (double )quat[0] / 1073741824;
+    if (quat0 > 1.0f) {
+        quat0 = 1.0;
+    } else if (quat0 < -1.0f) {
+        quat0 = -1.0;
+    }
+
+    return acos(quat0)*2*180/M_PI;
+}
+
+/** Rotates a 3-element vector by Rotation defined by Q
+*/
+void inv_q_rotate(const long *q, const long *in, long *out)
+{
+    long q_temp1[4], q_temp2[4];
+    long in4[4], out4[4];
+
+    // Fixme optimize
+    in4[0] = 0;
+    memcpy(&in4[1], in, 3 * sizeof(long));
+    inv_q_mult(q, in4, q_temp1);
+    inv_q_invert(q, q_temp2);
+    inv_q_mult(q_temp1, q_temp2, out4);
+    memcpy(out, &out4[1], 3 * sizeof(long));
+}
+
+void inv_q_multf(const float *q1, const float *q2, float *qProd)
+{
+    INVENSENSE_FUNC_START;
+    qProd[0] =
+        (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]);
+    qProd[1] =
+        (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]);
+    qProd[2] =
+        (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]);
+    qProd[3] =
+        (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]);
+}
+
+void inv_q_addf(const float *q1, const float *q2, float *qSum)
+{
+    INVENSENSE_FUNC_START;
+    qSum[0] = q1[0] + q2[0];
+    qSum[1] = q1[1] + q2[1];
+    qSum[2] = q1[2] + q2[2];
+    qSum[3] = q1[3] + q2[3];
+}
+
+void inv_q_normalizef(float *q)
+{
+    INVENSENSE_FUNC_START;
+    float normSF = 0;
+    float xHalf = 0;
+    normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+    if (normSF < 2) {
+        xHalf = 0.5f * normSF;
+        normSF = normSF * (1.5f - xHalf * normSF * normSF);
+        normSF = normSF * (1.5f - xHalf * normSF * normSF);
+        normSF = normSF * (1.5f - xHalf * normSF * normSF);
+        normSF = normSF * (1.5f - xHalf * normSF * normSF);
+        q[0] *= normSF;
+        q[1] *= normSF;
+        q[2] *= normSF;
+        q[3] *= normSF;
+    } else {
+        q[0] = 1.0;
+        q[1] = 0.0;
+        q[2] = 0.0;
+        q[3] = 0.0;
+    }
+    normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+}
+
+/** Performs a length 4 vector normalization with a square root.
+* @param[in,out] q vector to normalize. Returns [1,0,0,0] is magnitude is zero.
+*/
+void inv_q_norm4(float *q)
+{
+    float mag;
+    mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+    if (mag) {
+        q[0] /= mag;
+        q[1] /= mag;
+        q[2] /= mag;
+        q[3] /= mag;
+    } else {
+        q[0] = 1.f;
+        q[1] = 0.f;
+        q[2] = 0.f;
+        q[3] = 0.f;
+    }
+}
+
+void inv_q_invertf(const float *q, float *qInverted)
+{
+    INVENSENSE_FUNC_START;
+    qInverted[0] = q[0];
+    qInverted[1] = -q[1];
+    qInverted[2] = -q[2];
+    qInverted[3] = -q[3];
+}
+
+/**
+ * Converts a quaternion to a rotation matrix.
+ * @param[in] quat 4-element quaternion in fixed point. One is 2^30.
+ * @param[out] rot Rotation matrix in fixed point. One is 2^30. The
+ *             First 3 elements of the rotation matrix, represent
+ *             the first row of the matrix. Rotation matrix multiplied
+ *             by a 3 element column vector transform a vector from Body
+ *             to World.
+ */
+void inv_quaternion_to_rotation(const long *quat, long *rot)
+{
+    rot[0] =
+        inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0],
+                quat[0]) -
+        1073741824L;
+    rot[1] =
+        inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]);
+    rot[2] =
+        inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]);
+    rot[3] =
+        inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]);
+    rot[4] =
+        inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0],
+                quat[0]) -
+        1073741824L;
+    rot[5] =
+        inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]);
+    rot[6] =
+        inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]);
+    rot[7] =
+        inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]);
+    rot[8] =
+        inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0],
+                quat[0]) -
+        1073741824L;
+}
+
+/**
+ * Converts a quaternion to a rotation vector. A rotation vector is
+ * a method to represent a 4-element quaternion vector in 3-elements.
+ * To get the quaternion from the 3-elements, The last 3-elements of
+ * the quaternion will be the given rotation vector. The first element
+ * of the quaternion will be the positive value that will be required
+ * to make the magnitude of the quaternion 1.0 or 2^30 in fixed point units.
+ * @param[in] quat 4-element quaternion in fixed point. One is 2^30.
+ * @param[out] rot Rotation vector in fixed point. One is 2^30.
+ */
+void inv_quaternion_to_rotation_vector(const long *quat, long *rot)
+{
+    rot[0] = quat[1];
+    rot[1] = quat[2];
+    rot[2] = quat[3];
+
+    if (quat[0] < 0.0) {
+        rot[0] = -rot[0];
+        rot[1] = -rot[1];
+        rot[2] = -rot[2];
+    }
+}
+
+/** Converts a 32-bit long to a big endian byte stream */
+unsigned char *inv_int32_to_big8(long x, unsigned char *big8)
+{
+    big8[0] = (unsigned char)((x >> 24) & 0xff);
+    big8[1] = (unsigned char)((x >> 16) & 0xff);
+    big8[2] = (unsigned char)((x >> 8) & 0xff);
+    big8[3] = (unsigned char)(x & 0xff);
+    return big8;
+}
+
+/** Converts a big endian byte stream into a 32-bit long */
+long inv_big8_to_int32(const unsigned char *big8)
+{
+    long x;
+    x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8)
+        | ((long)big8[3]);
+    return x;
+}
+
+/** Converts a big endian byte stream into a 16-bit integer (short) */
+short inv_big8_to_int16(const unsigned char *big8)
+{
+    short x;
+    x = ((short)big8[0] << 8) | ((short)big8[1]);
+    return x;
+}
+
+/** Converts a little endian byte stream into a 16-bit integer (short) */
+short inv_little8_to_int16(const unsigned char *little8)
+{
+    short x;
+    x = ((short)little8[1] << 8) | ((short)little8[0]);
+    return x;
+}
+
+/** Converts a 16-bit short to a big endian byte stream */
+unsigned char *inv_int16_to_big8(short x, unsigned char *big8)
+{
+    big8[0] = (unsigned char)((x >> 8) & 0xff);
+    big8[1] = (unsigned char)(x & 0xff);
+    return big8;
+}
+
+void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y)
+{
+    int k, l, i, j;
+    for (i = 0, k = 0; i < *n; i++, k++) {
+        for (j = 0, l = 0; j < *n; j++, l++) {
+            if (i == x)
+                i++;
+            if (j == y)
+                j++;
+            *(b + 6 * k + l) = *(a + 6 * i + j);
+        }
+    }
+    *n = *n - 1;
+}
+
+void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y)
+{
+    int k, l, i, j;
+    for (i = 0, k = 0; i < *n; i++, k++) {
+        for (j = 0, l = 0; j < *n; j++, l++) {
+            if (i == x)
+                i++;
+            if (j == y)
+                j++;
+            *(b + 6 * k + l) = *(a + 6 * i + j);
+        }
+    }
+    *n = *n - 1;
+}
+
+float inv_matrix_det(float *p, int *n)
+{
+    float d[6][6], sum = 0;
+    int i, j, m;
+    m = *n;
+    if (*n == 2)
+        return (*p ** (p + 7) - *(p + 1) ** (p + 6));
+    for (i = 0, j = 0; j < m; j++) {
+        *n = m;
+        inv_matrix_det_inc(p, &d[0][0], n, i, j);
+        sum =
+            sum + *(p + 6 * i + j) * SIGNM(i +
+                                            j) *
+            inv_matrix_det(&d[0][0], n);
+    }
+
+    return (sum);
+}
+
+double inv_matrix_detd(double *p, int *n)
+{
+    double d[6][6], sum = 0;
+    int i, j, m;
+    m = *n;
+    if (*n == 2)
+        return (*p ** (p + 7) - *(p + 1) ** (p + 6));
+    for (i = 0, j = 0; j < m; j++) {
+        *n = m;
+        inv_matrix_det_incd(p, &d[0][0], n, i, j);
+        sum =
+            sum + *(p + 6 * i + j) * SIGNM(i +
+                                            j) *
+            inv_matrix_detd(&d[0][0], n);
+    }
+
+    return (sum);
+}
+
+/** Wraps angle from (-M_PI,M_PI]
+ * @param[in] ang Angle in radians to wrap
+ * @return Wrapped angle from (-M_PI,M_PI]
+ */
+float inv_wrap_angle(float ang)
+{
+    if (ang > M_PI)
+        return ang - 2 * (float)M_PI;
+    else if (ang <= -(float)M_PI)
+        return ang + 2 * (float)M_PI;
+    else
+        return ang;
+}
+
+/** Finds the minimum angle difference ang1-ang2 such that difference
+ * is between [-M_PI,M_PI]
+ * @param[in] ang1
+ * @param[in] ang2
+ * @return angle difference ang1-ang2
+ */
+float inv_angle_diff(float ang1, float ang2)
+{
+    float d;
+    ang1 = inv_wrap_angle(ang1);
+    ang2 = inv_wrap_angle(ang2);
+    d = ang1 - ang2;
+    if (d > M_PI)
+        d -= 2 * (float)M_PI;
+    else if (d < -(float)M_PI)
+        d += 2 * (float)M_PI;
+    return d;
+}
+
+/** bernstein hash, derived from public domain source */
+uint32_t inv_checksum(const unsigned char *str, int len)
+{
+    uint32_t hash = 5381;
+    int i, c;
+
+    for (i = 0; i < len; i++) {
+        c = *(str + i);
+        hash = ((hash << 5) + hash) + c;    /* hash * 33 + c */
+    }
+
+    return hash;
+}
+
+static unsigned short inv_row_2_scale(const signed char *row)
+{
+    unsigned short b;
+
+    if (row[0] > 0)
+        b = 0;
+    else if (row[0] < 0)
+        b = 4;
+    else if (row[1] > 0)
+        b = 1;
+    else if (row[1] < 0)
+        b = 5;
+    else if (row[2] > 0)
+        b = 2;
+    else if (row[2] < 0)
+        b = 6;
+    else
+        b = 7;  // error
+    return b;
+}
+
+
+/** Converts an orientation matrix made up of 0,+1,and -1 to a scalar representation.
+* @param[in] mtx Orientation matrix to convert to a scalar.
+* @return Description of orientation matrix. The lowest 2 bits (0 and 1) represent the column the one is on for the
+* first row, with the bit number 2 being the sign. The next 2 bits (3 and 4) represent
+* the column the one is on for the second row with bit number 5 being the sign.
+* The next 2 bits (6 and 7) represent the column the one is on for the third row with
+* bit number 8 being the sign. In binary the identity matrix would therefor be:
+* 010_001_000 or 0x88 in hex.
+*/
+unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
+{
+
+    unsigned short scalar;
+
+    /*
+       XYZ  010_001_000 Identity Matrix
+       XZY  001_010_000
+       YXZ  010_000_001
+       YZX  000_010_001
+       ZXY  001_000_010
+       ZYX  000_001_010
+     */
+
+    scalar = inv_row_2_scale(mtx);
+    scalar |= inv_row_2_scale(mtx + 3) << 3;
+    scalar |= inv_row_2_scale(mtx + 6) << 6;
+
+    return scalar;
+}
+
+/** Uses the scalar orientation value to convert from chip frame to body frame
+* @param[in] orientation A scalar that represent how to go from chip to body frame
+* @param[in] input Input vector, length 3
+* @param[out] output Output vector, length 3
+*/
+void inv_convert_to_body(unsigned short orientation, const long *input, long *output)
+{
+    output[0] = input[orientation      & 0x03] * SIGNSET(orientation & 0x004);
+    output[1] = input[(orientation>>3) & 0x03] * SIGNSET(orientation & 0x020);
+    output[2] = input[(orientation>>6) & 0x03] * SIGNSET(orientation & 0x100);
+}
+
+/** Uses the scalar orientation value to convert from body frame to chip frame
+* @param[in] orientation A scalar that represent how to go from chip to body frame
+* @param[in] input Input vector, length 3
+* @param[out] output Output vector, length 3
+*/
+void inv_convert_to_chip(unsigned short orientation, const long *input, long *output)
+{
+    output[orientation & 0x03]      = input[0] * SIGNSET(orientation & 0x004);
+    output[(orientation>>3) & 0x03] = input[1] * SIGNSET(orientation & 0x020);
+    output[(orientation>>6) & 0x03] = input[2] * SIGNSET(orientation & 0x100);
+}
+
+
+/** Uses the scalar orientation value to convert from chip frame to body frame and
+* apply appropriate scaling.
+* @param[in] orientation A scalar that represent how to go from chip to body frame
+* @param[in] sensitivity Sensitivity scale
+* @param[in] input Input vector, length 3
+* @param[out] output Output vector, length 3
+*/
+void inv_convert_to_body_with_scale(unsigned short orientation,
+                                    long sensitivity,
+                                    const long *input, long *output)
+{
+    output[0] = inv_q30_mult(input[orientation & 0x03] *
+                             SIGNSET(orientation & 0x004), sensitivity);
+    output[1] = inv_q30_mult(input[(orientation>>3) & 0x03] *
+                             SIGNSET(orientation & 0x020), sensitivity);
+    output[2] = inv_q30_mult(input[(orientation>>6) & 0x03] *
+                             SIGNSET(orientation & 0x100), sensitivity);
+}
+
+/** find a norm for a vector
+* @param[in] a vector [3x1]
+* @param[out] output the norm of the input vector
+*/
+double inv_vector_norm(const float *x)
+{
+    return sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]);
+}
+
+void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff) {
+    int i;
+    // initial state to zero
+    pFilter->state[0] = 0;
+    pFilter->state[1] = 0;
+
+    // set up coefficients
+    for (i=0; i<5; i++) {
+        pFilter->c[i] = pBiquadCoeff[i];
+    }
+}
+
+void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input)
+{
+    pFilter->input = input;
+    pFilter->output = input;
+    pFilter->state[0] = input / (1 + pFilter->c[2] + pFilter->c[3]);
+    pFilter->state[1] = pFilter->state[0];
+}
+
+float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input)  {
+    float stateZero;
+
+    pFilter->input = input;
+    // calculate the new state;
+    stateZero = pFilter->input - pFilter->c[2]*pFilter->state[0]
+                               - pFilter->c[3]*pFilter->state[1];
+
+    pFilter->output = stateZero + pFilter->c[0]*pFilter->state[0]
+                                + pFilter->c[1]*pFilter->state[1];
+
+    // update the output and state
+    pFilter->output = pFilter->output * pFilter->c[4];
+    pFilter->state[1] = pFilter->state[0];
+    pFilter->state[0] = stateZero;
+    return pFilter->output;
+}
+
+void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3])  {
+
+    cgcross[0] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1];
+    cgcross[1] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2];
+    cgcross[2] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0];
+}
+
+void mlMatrixVectorMult(long matrix[9], const long vecIn[3], long *vecOut)  {
+        // matrix format
+        //  [ 0  3  6;
+        //    1  4  7;
+        //    2  5  8]
+
+        // vector format:  [0  1  2]^T;
+        int i, j;
+        long temp;
+
+        for (i=0; i<3; i++)	{
+                temp = 0;
+                for (j=0; j<3; j++)  {
+                        temp += inv_q30_mult(matrix[i+j*3], vecIn[j]);
+                }
+                vecOut[i] = temp;
+        }
+}
+
+//============== 1/sqrt(x), 1/x, sqrt(x) Functions ================================
+
+/** Calculates 1/square-root of a fixed-point number (30 bit mantissa, positive): Q1.30
+* Input must be a positive scaled (2^30) integer
+* The number is scaled to lie between a range in which a Newton-Raphson
+* iteration works best. Corresponding square root of the power of two is returned.
+*  Caller must scale final result by 2^rempow (while avoiding overflow).
+* @param[in] x0, length 1
+* @param[out] rempow, length 1
+* @return scaledSquareRoot on success or zero.
+*/
+long inv_inverse_sqrt(long x0, int*rempow)
+{
+	//% Inverse sqrt NR in the neighborhood of 1.3>x>=0.65
+	//% x(k+1) = x(k)*(3 - x0*x(k)^2)
+
+	//% Seed equals 1. Works best in this region.
+	//xx0 = int32(1*2^30);
+
+	long oneoversqrt2, oneandhalf, x0_2;
+	unsigned long xx;
+	int pow2, sq2scale, nr_iters;
+	//long upscale, sqrt_upscale, upsclimit;
+	//long downscale, sqrt_downscale, downsclimit;
+
+	// Precompute some constants for efficiency
+	//% int32(2^30*1/sqrt(2))
+	oneoversqrt2=759250125L;
+	//% int32(1.5*2^30);
+	oneandhalf=1610612736L;
+
+	//// Further scaling into optimal region saves one or more NR iterations. Maps into region (.9, 1.1)
+	//// int32(0.9/log(2)*2^30)
+	//upscale = 1394173804L;
+	//// int32(sqrt(0.9/log(2))*2^30)
+	//sqrt_upscale = 1223512453L;
+	// // int32(1.1*log(2)/.9*2^30)
+	//upsclimit = 909652478L;
+	//// int32(1.1/log(4)*2^30)
+	//downscale = 851995103L;
+	//// int32(sqrt(1.1/log(4))*2^30)
+	//sqrt_downscale = 956463682L;
+	// // int32(0.9*log(4)/1.1*2^30)
+	//downsclimit = 1217881829L;
+
+	nr_iters = test_limits_and_scale(&x0, &pow2);
+
+	sq2scale=pow2%2;  // Find remainder. Is it even or odd?
+
+	
+	// Further scaling to decrease NR iterations
+	// With the mapping below, 89% of calculations will require 2 NR iterations or less.
+	// TBD
+
+
+	x0_2 = x0 >>1; // This scaling incorporates factor of 2 in NR iteration below.
+	// Initial condition starts at 1: xx=(1L<<30);
+	// First iteration is simple: Instead of initializing xx=1, assign to result of first iteration:
+	// xx= (3/2-x0/2);
+	//% NR formula: xx=xx*(3/2-x0*xx*xx/2); = xx*(1.5 - (x0/2)*xx*xx)
+	// Initialize NR (first iteration). Note we are starting with xx=1, so the first iteration becomes an initialization.
+	xx = oneandhalf - x0_2;
+ 	if ( nr_iters>=2 ) {
+		// Second NR iteration
+		xx = inv_q30_mult( xx, ( oneandhalf - inv_q30_mult(x0_2, inv_q30_mult(xx,xx) ) ) );
+		if ( nr_iters==3 ) {
+			// Third NR iteration. 
+			xx = inv_q30_mult( xx, ( oneandhalf - inv_q30_mult(x0_2, inv_q30_mult(xx,xx) ) ) );
+			// Fourth NR iteration: Not needed due to single precision.
+		}
+	}
+	if (sq2scale) {
+		*rempow = (pow2>>1) + 1; // Account for sqrt(2) in denominator, note we multiply if s2scale is true
+		return (inv_q30_mult(xx,oneoversqrt2));
+	}
+	else {
+		*rempow = pow2>>1;
+		return xx;
+	}
+}
+
+
+/** Calculates square-root of a fixed-point number (30 bit mantissa, positive)
+* Input must be a positive scaled (2^30) integer
+* The number is scaled to lie between a range in which a Newton-Raphson
+* iteration works best.
+* @param[in] x0, length 1
+* @return scaledSquareRoot on success or zero. **/
+long inv_fast_sqrt(long x0)
+{
+
+	//% Square-Root with NR in the neighborhood of 1.3>x>=0.65 (log(2) <= x <= log(4) )
+    // Two-variable NR iteration:
+    // Initialize: a=x; c=x-1;  
+    // 1st Newton Step:  a=a-a*c/2; ( or: a = x - x*(x-1)/2  )
+    // Iterate: c = c*c*(c-3)/4
+    //          a = a - a*c/2    --> reevaluating c at this step gives error of approximation
+
+	//% Seed equals 1. Works best in this region.
+	//xx0 = int32(1*2^30);
+
+	long sqrt2, oneoversqrt2, one_pt5;
+	long xx, cc;
+	int pow2, sq2scale, nr_iters;
+
+	// Return if input is zero. Negative should really error out. 
+	if (x0 <= 0L) {
+		return 0L;
+	}
+
+	sqrt2 =1518500250L;
+	oneoversqrt2=759250125L;
+	one_pt5=1610612736L;
+
+	nr_iters = test_limits_and_scale(&x0, &pow2);
+	
+	sq2scale = 0;
+	if (pow2 > 0) 
+		sq2scale=pow2%2;  // Find remainder. Is it even or odd?
+	pow2 = pow2-sq2scale; // Now pow2 is even. Note we are adding because result is scaled with sqrt(2)
+
+	// Sqrt 1st NR iteration
+	cc = x0 - (1L<<30);
+	xx = x0 - (inv_q30_mult(x0, cc)>>1);
+ 	if ( nr_iters>=2 ) {
+		// Sqrt second NR iteration
+		// cc = cc*cc*(cc-3)/4; = cc*cc*(cc/2 - 3/2)/2;
+		// cc = ( cc*cc*((cc>>1) - onePt5) ) >> 1
+		cc = inv_q30_mult( cc, inv_q30_mult(cc, (cc>>1) - one_pt5) ) >> 1;
+		xx = xx - (inv_q30_mult(xx, cc)>>1);
+		if ( nr_iters==3 ) {
+			// Sqrt third NR iteration
+			cc = inv_q30_mult( cc, inv_q30_mult(cc, (cc>>1) - one_pt5) ) >> 1;
+			xx = xx - (inv_q30_mult(xx, cc)>>1);
+		}
+	}
+	if (sq2scale)
+		xx = inv_q30_mult(xx,oneoversqrt2);
+	// Scale the number with the half of the power of 2 scaling
+	if (pow2>0)
+		xx = (xx >> (pow2>>1)); 
+	else if (pow2 == -1)
+		xx = inv_q30_mult(xx,sqrt2);
+	return xx;
+}
+
+/** Calculates 1/x of a fixed-point number (30 bit mantissa)
+* Input must be a scaled (2^30) integer (+/-)
+* The number is scaled to lie between a range in which a Newton-Raphson
+* iteration works best. Corresponding multiplier power of two is returned.
+*  Caller must scale final result by 2^pow (while avoiding overflow).
+* @param[in] x, length 1
+* @param[out] pow, length 1
+* @return scaledOneOverX on success or zero.
+*/
+long inv_one_over_x(long x0, int*pow)
+{
+	//% NR for 1/x in the neighborhood of log(2) => x => log(4)
+	//%    y(k+1)=y(k)*(2 \ 96 x0*y(k))
+    //% with y(0) = 1 as the starting value for NR
+
+	long two, xx;
+	int numberwasnegative, nr_iters, did_upscale, did_downscale;
+
+	long upscale, downscale, upsclimit, downsclimit;
+
+	*pow = 0;
+	// Return if input is zero. 
+	if (x0 == 0L) {
+		return 0L;
+	}
+
+	// This is really (2^31-1), i.e. 1.99999... .
+	// Approximation error is 1e-9. Note 2^31 will overflow to sign bit, so it can't be used here.
+	two = 2147483647L; 
+
+	// int32(0.92/log(2)*2^30)
+	upscale = 1425155444L;
+	// int32(1.08/upscale*2^30) 
+	upsclimit = 873697834L;
+
+	// int32(1.08/log(4)*2^30)
+	downscale = 836504283L;
+	// int32(0.92/downscale*2^30) 
+	downsclimit = 1268000423L;
+
+	// Algorithm is intended to work with positive numbers only. Change sign:
+	numberwasnegative = 0;
+	if (x0 < 0L) {
+		numberwasnegative = 1;
+		x0 = -x0;
+	}
+
+	nr_iters = test_limits_and_scale(&x0, pow);
+
+	did_upscale=0;
+	did_downscale=0;
+	// Pre-scaling to reduce NR iterations and improve accuracy:
+	if (x0<=upsclimit) {
+		x0 = inv_q30_mult(x0, upscale);
+		did_upscale = 1;
+		// The scaling ALWAYS leaves the number in the 2-NR iterations region:
+		nr_iters = 2;
+		// Is x0 in the single NR iteration region (0.994, 1.006) ?
+		if (x0 > 1067299373 && x0 < 1080184275)
+			nr_iters = 1;
+	} else if (x0>=downsclimit) {
+		x0 = inv_q30_mult(x0, downscale);
+		did_downscale = 1;
+		// The scaling ALWAYS leaves the number in the 2-NR iterations region:
+		nr_iters = 2;
+		// Is x0 in the single NR iteration region (0.994, 1.006) ?
+		if (x0 > 1067299373 && x0 < 1080184275)
+			nr_iters = 1;
+	}
+
+	xx = (two - x0) + 1; // Note 2 will overflow so the computation (2-x) is done with "two" == (2^30-1)
+	// First NR iteration
+	xx = inv_q30_mult( xx, (two - inv_q30_mult(x0, xx)) + 1 );
+ 	if ( nr_iters>=2 ) {
+		// Second NR iteration
+		xx = inv_q30_mult( xx, (two - inv_q30_mult(x0, xx)) + 1 );
+		if ( nr_iters==3 ) {
+			// THird NR iteration. 
+			xx = inv_q30_mult( xx, (two - inv_q30_mult(x0, xx)) + 1 );
+			// Fourth NR iteration: Not needed due to single precision.
+		}
+	}
+
+	// Post-scaling
+	if (did_upscale)
+		xx = inv_q30_mult( xx, upscale);
+	else if (did_downscale)
+		xx = inv_q30_mult( xx, downscale);
+
+	if (numberwasnegative) 
+		xx = -xx;
+	return xx;
+}
+
+/** Auxiliary function used by inv_OneOverX(), inv_fastSquareRoot(), inv_inverseSqrt().
+* Finds the range of the argument, determines the optimal number of Newton-Raphson
+* iterations and . Corresponding square root of the power of two is returned.
+* Restrictions: Number is represented as Q1.30.
+*               Number is betweeen the range 2<x<=0
+* @param[in] x, length 1
+* @param[out] pow, length 1
+* @return # of NR iterations, x0 scaled between log(2) and log(4) and 2^N scaling (N=pow)
+*/
+int test_limits_and_scale(long *x0, int *pow)
+{
+	long lowerlimit, upperlimit, oneiterlothr, oneiterhithr, zeroiterlothr, zeroiterhithr;
+
+	// Lower Limit: ll = int32(log(2)*2^30);
+	lowerlimit = 744261118L;
+	//Upper Limit ul = int32(log(4)*2^30);
+	upperlimit = 1488522236L;
+	//  int32(0.9*2^30)
+	oneiterlothr = 966367642L;
+	// int32(1.1*2^30)
+	oneiterhithr = 1181116006L;
+	// int32(0.99*2^30)
+	zeroiterlothr=1063004406L;
+	//int32(1.01*2^30)
+	zeroiterhithr=1084479242L;
+
+	// Scale number such that Newton Raphson iteration works best:
+	// Find the power of two scaling that leaves the number in the optimal range,
+	// ll <= number <= ul. Note odd powers have special scaling further below
+	if (*x0 > upperlimit) {
+		// Halving the number will push it in the optimal range since largest value is 2
+		*x0 = *x0>>1;
+		*pow=-1;
+	} else if (*x0 < lowerlimit) {
+		// Find position of highest bit, counting from left, and scale number 
+		*pow=get_highest_bit_position((unsigned long*)x0);
+		if (*x0 >= upperlimit) {
+			// Halving the number will push it in the optimal range
+			*x0 = *x0>>1;
+			*pow=*pow-1;
+		}
+		else if (*x0 < lowerlimit) {
+			// Doubling the number will push it in the optimal range
+			*x0 = *x0<<1;
+			*pow=*pow+1;
+		}
+	} else {
+		*pow = 0;
+	}
+
+	if ( *x0<oneiterlothr || *x0>oneiterhithr )
+		return 3; // 3 NR iterations
+	if ( *x0<zeroiterlothr || *x0>zeroiterhithr )
+		return 2; // 2 NR iteration
+
+	return 1; // 1 NR iteration
+}
+
+/** Auxiliary function used by testLimitsAndScale()
+* Find the highest nonzero bit in an unsigned 32 bit integer:
+* @param[in] value, length 1.
+* @return highes bit position.
+**/int get_highest_bit_position(unsigned long *value)
+{
+    int position;
+    position = 0;
+    if (*value == 0) return 0;
+
+    if ((*value & 0xFFFF0000) == 0) {
+		position += 16;
+		*value=*value<<16;
+	}
+    if ((*value & 0xFF000000) == 0) {
+		position += 8;
+		*value=*value<<8;
+	}
+    if ((*value & 0xF0000000) == 0) {
+		position += 4;
+		*value=*value<<4;
+	}
+    if ((*value & 0xC0000000) == 0) {
+		position += 2;
+		*value=*value<<2;
+	}
+
+	// If we got too far into sign bit, shift back. Note we are using an
+	// unsigned long here, so right shift is going to shift all the bits.
+    if ((*value & 0x80000000)) { 
+		position -= 1;
+		*value=*value>>1;
+	}
+    return position;
+}
+
+/* compute real part of quaternion, element[0]
+@param[in]  inQuat, 3 elements gyro quaternion
+@param[out] outquat, 4 elements gyro quaternion
+*/
+int inv_compute_scalar_part(const long * inQuat, long* outQuat)
+{
+    long scalarPart = 0;
+
+    scalarPart = inv_fast_sqrt((1L<<30) - inv_q30_mult(inQuat[0], inQuat[0])
+                                        - inv_q30_mult(inQuat[1], inQuat[1])
+                                        - inv_q30_mult(inQuat[2], inQuat[2]) );
+                outQuat[0] = scalarPart;
+                outQuat[1] = inQuat[0];
+                outQuat[2] = inQuat[1];
+                outQuat[3] = inQuat[2];
+
+                return 0;
+}
+/**
+ * @}
+ */
diff --git a/6515/libsensors_iio/software/core/mllite/ml_math_func.h b/6515/libsensors_iio/software/core/mllite/ml_math_func.h
new file mode 100644
index 0000000..1540254
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/ml_math_func.h
@@ -0,0 +1,129 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#ifndef INVENSENSE_INV_MATH_FUNC_H__
+#define INVENSENSE_INV_MATH_FUNC_H__
+
+#include "mltypes.h"
+
+#define GYRO_MAG_SQR_SHIFT 6
+#define NUM_ROTATION_MATRIX_ELEMENTS (9)
+#define ROT_MATRIX_SCALE_LONG  (1073741824L)
+#define ROT_MATRIX_SCALE_FLOAT (1073741824.0f)
+#define ROT_MATRIX_LONG_TO_FLOAT( longval ) \
+    ((float) ((longval) / ROT_MATRIX_SCALE_FLOAT ))
+#define SIGNM(k)((int)(k)&1?-1:1)
+#define SIGNSET(x) ((x) ? -1 : +1)
+
+#define INV_TWO_POWER_NEG_30 9.313225746154785e-010f
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+     typedef struct {
+        float state[4];
+        float c[5];
+        float input;
+        float output;
+    }   inv_biquad_filter_t;
+
+    static inline float inv_q30_to_float(long q30)
+    {
+        return (float) q30 / ((float)(1L << 30));
+    }
+
+    static inline double inv_q30_to_double(long q30)
+    {
+        return (double) q30 / ((double)(1L << 30));
+    }
+
+    static inline float inv_q16_to_float(long q16)
+    {
+        return (float) q16 / (1L << 16);
+    }
+
+    static inline double inv_q16_to_double(long q16)
+    {
+        return (double) q16 / (1L << 16);
+    }
+
+
+
+
+    long inv_q29_mult(long a, long b);
+    long inv_q30_mult(long a, long b);
+
+    /* UMPL_ELIMINATE_64BIT Notes:
+     * An alternate implementation using float instead of long long accudoublemulators
+     * is provided for q29_mult and q30_mult.
+     * When long long accumulators are used and an alternate implementation is not
+     * available, we eliminate the entire function and header with a macro.
+     */
+#ifndef UMPL_ELIMINATE_64BIT
+    long inv_q30_div(long a, long b);
+    long inv_q_shift_mult(long a, long b, int shift);
+#endif
+
+    void inv_q_mult(const long *q1, const long *q2, long *qProd);
+    void inv_q_add(long *q1, long *q2, long *qSum);
+    void inv_q_normalize(long *q);
+    void inv_q_invert(const long *q, long *qInverted);
+    void inv_q_multf(const float *q1, const float *q2, float *qProd);
+    void inv_q_addf(const float *q1, const float *q2, float *qSum);
+    void inv_q_normalizef(float *q);
+    void inv_q_norm4(float *q);
+    void inv_q_invertf(const float *q, float *qInverted);
+    void inv_quaternion_to_rotation(const long *quat, long *rot);
+    unsigned char *inv_int32_to_big8(long x, unsigned char *big8);
+    long inv_big8_to_int32(const unsigned char *big8);
+    short inv_big8_to_int16(const unsigned char *big8);
+    short inv_little8_to_int16(const unsigned char *little8);
+    unsigned char *inv_int16_to_big8(short x, unsigned char *big8);
+    float inv_matrix_det(float *p, int *n);
+    void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y);
+    double inv_matrix_detd(double *p, int *n);
+    void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y);
+    float inv_wrap_angle(float ang);
+    float inv_angle_diff(float ang1, float ang2);
+    void inv_quaternion_to_rotation_vector(const long *quat, long *rot);
+    unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx);
+    void inv_convert_to_body(unsigned short orientation, const long *input, long *output);
+    void inv_convert_to_chip(unsigned short orientation, const long *input, long *output);
+    void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output);
+    void inv_q_rotate(const long *q, const long *in, long *out);
+	void inv_vector_normalize(long *vec, int length);
+    uint32_t inv_checksum(const unsigned char *str, int len);
+    float inv_compass_angle(const long *compass, const long *grav,
+                            const float *quat);
+    unsigned long inv_get_gyro_sum_of_sqr(const long *gyro);
+
+    static inline long inv_delta_time_ms(inv_time_t t1, inv_time_t t2)
+    {
+        return (long)((t1 - t2) / 1000000L);
+    }
+
+    double quaternion_to_rotation_angle(const long *quat);
+    double inv_vector_norm(const float *x);
+
+    void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff);
+    float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input);
+    void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input);
+    void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]);
+
+    void mlMatrixVectorMult(long matrix[9], const long vecIn[3], long *vecOut);
+
+	long inv_inverse_sqrt(long x0, int*rempow);
+	long inv_fast_sqrt(long x0);
+	long inv_one_over_x(long x0, int*pow);
+	int test_limits_and_scale(long *x0, int *pow);
+	int get_highest_bit_position(unsigned long *value);
+    int inv_compute_scalar_part(const long * inQuat, long* outQuat);
+
+#ifdef __cplusplus
+}
+#endif
+#endif // INVENSENSE_INV_MATH_FUNC_H__
diff --git a/6515/libsensors_iio/software/core/mllite/mpl.c b/6515/libsensors_iio/software/core/mllite/mpl.c
new file mode 100644
index 0000000..0aa2242
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/mpl.c
@@ -0,0 +1,77 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+/**
+ *   @defgroup  MPL mpl
+ *   @brief     Motion Library - Start Point
+ *              Initializes MPL.
+ *
+ *   @{
+ *       @file  mpl.c
+ *       @brief MPL start point.
+ */
+
+#include "storage_manager.h"
+#include "log.h"
+#include "mpl.h"
+#include "start_manager.h"
+#include "data_builder.h"
+#include "results_holder.h"
+#include "mlinclude.h"
+#include "message_layer.h"
+
+/**
+ * @brief  Initializes the MPL. Should be called first and once 
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_init_mpl(void)
+{
+    inv_init_storage_manager();
+
+    /* initialize the start callback manager */
+    INV_ERROR_CHECK(inv_init_start_manager());
+
+    /* initialize the data builder */
+    INV_ERROR_CHECK(inv_init_data_builder());
+
+    INV_ERROR_CHECK(inv_enable_results_holder());
+
+    // Get any left over messages and clear them. Throw message away as it is not
+    // initialized.
+    (void)inv_get_message_level_0(1);
+
+    return INV_SUCCESS;
+}
+
+const char ml_ver[] = "InvenSense MA 5.2.0 K";
+
+/**
+ *  @brief  used to get the MPL version.
+ *  @param  version     a string where the MPL version gets stored.
+ *  @return INV_SUCCESS if successful or a non-zero error code otherwise.
+ */
+inv_error_t inv_get_version(char **version)
+{
+    INVENSENSE_FUNC_START;
+    /* cast out the const */
+    *version = (char *)&ml_ver;
+    return INV_SUCCESS;
+}
+
+/** 
+ *  @brief  Starts the MPL. Typically called after inv_init_mpl() or after a
+ *          inv_stop_mpl() to start the MPL back up an running.
+ *  @return INV_SUCCESS if successful or a non-zero error code otherwise.
+ */
+inv_error_t inv_start_mpl(void)
+{
+    INV_ERROR_CHECK(inv_execute_mpl_start_notification());
+    return INV_SUCCESS;
+}
+
+/**
+ * @}
+ */
diff --git a/6515/libsensors_iio/software/core/mllite/mpl.h b/6515/libsensors_iio/software/core/mllite/mpl.h
new file mode 100644
index 0000000..a6b5ac7
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/mpl.h
@@ -0,0 +1,24 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#include "mltypes.h"
+
+#ifndef INV_MPL_H__
+#define INV_MPL_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_init_mpl(void);
+inv_error_t inv_start_mpl(void);
+inv_error_t inv_get_version(char **version);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_MPL_H__
diff --git a/6515/libsensors_iio/software/core/mllite/results_holder.c b/6515/libsensors_iio/software/core/mllite/results_holder.c
new file mode 100644
index 0000000..c59eb94
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/results_holder.c
@@ -0,0 +1,893 @@
+/*

+ $License:

+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.

+    See included License.txt for License information.

+ $

+ */

+/**

+ *   @defgroup  Results_Holder results_holder

+ *   @brief     Motion Library - Results Holder

+ *              Holds the data for MPL

+ *

+ *   @{

+ *       @file results_holder.c

+ *       @brief Results Holder for HAL.

+ */

+

+#include <string.h>

+

+#include "results_holder.h"

+#include "ml_math_func.h"

+#include "mlmath.h"

+#include "start_manager.h"

+#include "data_builder.h"

+#include "message_layer.h"

+#include "log.h"

+

+struct results_t {

+    float nav_quat[4];

+    float game_quat[4];

+    long gam_quat[4];

+    float geomag_quat[4];

+    long accel_quat[4];

+    inv_time_t nav_timestamp;

+    inv_time_t gam_timestamp;

+    inv_time_t geomag_timestamp;

+    long mag_scale[3]; /**< scale factor to apply to magnetic field reading */

+    long compass_correction[4]; /**< quaternion going from gyro,accel quaternion to 9 axis */

+    long geomag_compass_correction[4]; /**< quaternion going from accel quaternion to geomag sensor fusion */

+    int acc_state; /**< Describes accel state */

+    int got_accel_bias; /**< Flag describing if accel bias is known */

+    long compass_bias_error[3]; /**< Error Squared */

+    unsigned char motion_state;

+    unsigned int motion_state_counter; /**< Incremented for each no motion event in a row */

+    long compass_count; /**< compass state internal counter */

+    int got_compass_bias; /**< Flag describing if compass bias is known */

+    int large_mag_field; /**< Flag describing if there is a large magnetic field */

+    int compass_state; /**< Internal compass state */

+    long status;

+    struct inv_sensor_cal_t *sensor;

+    float quat_confidence_interval;

+    float geo_mag_confidence_interval;

+    struct local_field_t mag_local_field;

+    struct local_field_t mpl_compass_cal;

+    int quat_validity;

+#ifdef WIN32

+    long last_quat[4];

+#endif

+};

+static struct results_t rh;

+

+/** @internal

+* Store a quaternion more suitable for gaming. This quaternion is often determined

+* using only gyro and accel.

+* @param[in] quat Length 4, Quaternion scaled by 2^30

+* @param[in] timestamp Timestamp of the 6-axis quaternion

+*/

+void inv_store_gaming_quaternion(const long *quat, inv_time_t timestamp)

+{

+    rh.status |= INV_6_AXIS_QUAT_SET;

+    memcpy(&rh.gam_quat, quat, sizeof(rh.gam_quat));

+    rh.gam_timestamp = timestamp;

+}

+

+/** @internal

+* Store a 9-axis quaternion.

+* @param[in] quat Length 4 in floating-point numbers

+* @param[in] timestamp Timestamp of the 9-axis quaternion

+*/

+void inv_store_nav_quaternion(const float *quat, inv_time_t timestamp)

+{

+    memcpy(&rh.nav_quat, quat, sizeof(rh.nav_quat));

+    rh.nav_timestamp = timestamp;

+}

+

+/** @internal

+* Store a 6-axis geomagnetic quaternion.

+* @param[in] quat Length 4 in floating-point numbers

+* @param[in] timestamp Timestamp of the geomag quaternion

+*/

+void inv_store_geomag_quaternion(const float *quat, inv_time_t timestamp)

+{

+    memcpy(&rh.geomag_quat, quat, sizeof(rh.geomag_quat));

+    rh.geomag_timestamp = timestamp;

+}

+

+/** @internal

+* Store a floating-point quaternion more suitable for gaming.

+* @param[in] quat Length 4 in floating-point numbers

+* @param[in] timestamp Timestamp of the 6-axis quaternion

+*/

+void inv_store_game_quaternion(const float *quat, inv_time_t timestamp)

+{

+    rh.status |= INV_6_AXIS_QUAT_SET;

+    memcpy(&rh.game_quat, quat, sizeof(rh.game_quat));

+    rh.gam_timestamp = timestamp;

+}

+

+/** @internal

+* Store a quaternion computed from accelerometer correction. This quaternion is 

+* determined * using only accel, and used for geomagnetic fusion. 

+* @param[in] quat Length 4, Quaternion scaled by 2^30

+*/

+void inv_store_accel_quaternion(const long *quat, inv_time_t timestamp)

+{

+   // rh.status |= INV_6_AXIS_QUAT_SET;

+    memcpy(&rh.accel_quat, quat, sizeof(rh.accel_quat));

+    rh.geomag_timestamp = timestamp;

+}

+/** @internal

+* Sets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion.

+* @param[in] data Quaternion Adjustment

+* @param[in] timestamp Timestamp of when this is valid

+*/

+void inv_set_compass_correction(const long *data, inv_time_t timestamp)

+{

+    rh.status |= INV_COMPASS_CORRECTION_SET;

+    memcpy(rh.compass_correction, data, sizeof(rh.compass_correction));

+    rh.nav_timestamp = timestamp;

+}

+

+/** @internal

+* Sets the quaternion adjustment from 3 axis (accel) to 6 axis (with compass) quaternion.

+* @param[in] data Quaternion Adjustment

+* @param[in] timestamp Timestamp of when this is valid

+*/

+void inv_set_geomagnetic_compass_correction(const long *data, inv_time_t timestamp)

+{

+    rh.status |= INV_GEOMAGNETIC_CORRECTION_SET;

+    memcpy(rh.geomag_compass_correction, data, sizeof(rh.geomag_compass_correction));

+    rh.geomag_timestamp = timestamp;

+}

+

+/** @internal

+* Gets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion.

+* @param[out] data Quaternion Adjustment

+* @param[out] timestamp Timestamp of when this is valid

+*/

+void inv_get_compass_correction(long *data, inv_time_t *timestamp)

+{

+    memcpy(data, rh.compass_correction, sizeof(rh.compass_correction));

+    *timestamp = rh.nav_timestamp;

+}

+

+/** @internal

+* Gets the quaternion adjustment from 3 axis (accel) to 6 axis (with compass) quaternion.

+* @param[out] data Quaternion Adjustment

+* @param[out] timestamp Timestamp of when this is valid

+*/

+void inv_get_geomagnetic_compass_correction(long *data, inv_time_t *timestamp)

+{

+    memcpy(data, rh.geomag_compass_correction, sizeof(rh.geomag_compass_correction));

+    *timestamp = rh.geomag_timestamp;

+}

+

+/** Returns non-zero if there is a large magnetic field. See inv_set_large_mag_field() for setting this variable.

+ * @return Returns non-zero if there is a large magnetic field.

+ */

+int inv_get_large_mag_field()

+{

+    return rh.large_mag_field;

+}

+

+/** Set to non-zero if there as a large magnetic field. See inv_get_large_mag_field() for getting this variable.

+ * @param[in] state value to set for magnetic field strength. Should be non-zero if it is large.

+ */

+void inv_set_large_mag_field(int state)

+{

+    rh.large_mag_field = state;

+}

+

+/** Gets the accel state set by inv_set_acc_state()

+ * @return accel state.

+ */

+int inv_get_acc_state()

+{

+    return rh.acc_state;

+}

+

+/** Sets the accel state. See inv_get_acc_state() to get the value.

+ * @param[in] state value to set accel state to.

+ */

+void inv_set_acc_state(int state)

+{

+    rh.acc_state = state;

+    return;

+}

+

+/** Returns the motion state

+* @param[out] cntr Number of previous times a no motion event has occured in a row.

+* @return Returns INV_SUCCESS if successful or an error code if not.

+*/

+int inv_get_motion_state(unsigned int *cntr)

+{

+    *cntr = rh.motion_state_counter;

+    return rh.motion_state;

+}

+

+/** Sets the motion state

+ * @param[in] state motion state where INV_NO_MOTION is not moving

+ *            and INV_MOTION is moving.

+ */

+void inv_set_motion_state(unsigned char state)

+{

+    long set;

+    if (state == rh.motion_state) {

+        if (state == INV_NO_MOTION) {

+            rh.motion_state_counter++;

+        } else {

+            rh.motion_state_counter = 0;

+        }

+        return;

+    }

+    rh.motion_state_counter = 0;

+    rh.motion_state = state;

+    /* Equivalent to set = state, but #define's may change. */

+    if (state == INV_MOTION)

+        set = INV_MSG_MOTION_EVENT;

+    else

+        set = INV_MSG_NO_MOTION_EVENT;

+    inv_set_message(set, (INV_MSG_MOTION_EVENT | INV_MSG_NO_MOTION_EVENT), 0);

+}

+

+/** Sets the compass sensitivity

+ * @param[in] data Length 3, sensitivity for each compass axis

+ *  scaled such that 1.0 = 2^30.

+ */

+void inv_set_mag_scale(const long *data)

+{

+    memcpy(rh.mag_scale, data, sizeof(rh.mag_scale));

+}

+

+/** Gets the compass sensitivity

+ * @param[out] data Length 3, sensitivity for each compass axis

+ *  scaled such that 1.0 = 2^30.

+ */

+void inv_get_mag_scale(long *data)

+{

+    memcpy(data, rh.mag_scale, sizeof(rh.mag_scale));

+}

+

+/** Gets gravity vector

+ * @param[out] data gravity vector in body frame scaled such that 1.0 = 2^30.

+ * @return Returns INV_SUCCESS if successful or an error code if not.

+ */

+inv_error_t inv_get_gravity(long *data)

+{

+    long ldata[4];

+    inv_error_t result = inv_get_quaternion(ldata);

+

+    data[0] =

+        inv_q29_mult(ldata[1], ldata[3]) - inv_q29_mult(ldata[2], ldata[0]);

+    data[1] =

+        inv_q29_mult(ldata[2], ldata[3]) + inv_q29_mult(ldata[1], ldata[0]);

+    data[2] =

+        (inv_q29_mult(ldata[3], ldata[3]) + inv_q29_mult(ldata[0], ldata[0])) -

+        1073741824L;

+

+    return result;

+}

+

+/** Returns a quaternion based only on accel.

+ * @param[out] data 3-axis  accel quaternion scaled such that 1.0 = 2^30.

+ * @return Returns INV_SUCCESS if successful or an error code if not.

+ */

+inv_error_t inv_get_accel_quaternion(long *data)

+{

+    memcpy(data, rh.accel_quat, sizeof(rh.accel_quat));

+    return INV_SUCCESS;

+}

+inv_error_t inv_get_gravity_6x(long *data)

+{

+    data[0] =

+        inv_q29_mult(rh.gam_quat[1], rh.gam_quat[3]) - inv_q29_mult(rh.gam_quat[2], rh.gam_quat[0]);

+    data[1] =

+        inv_q29_mult(rh.gam_quat[2], rh.gam_quat[3]) + inv_q29_mult(rh.gam_quat[1], rh.gam_quat[0]);

+    data[2] =

+        (inv_q29_mult(rh.gam_quat[3], rh.gam_quat[3]) + inv_q29_mult(rh.gam_quat[0], rh.gam_quat[0])) -

+        1073741824L;

+    return INV_SUCCESS;

+}

+/** Returns a quaternion based only on gyro and accel.

+ * @param[out] data 6-axis  gyro and accel quaternion scaled such that 1.0 = 2^30.

+ * @return Returns INV_SUCCESS if successful or an error code if not.

+ */

+inv_error_t inv_get_6axis_quaternion(long *data, inv_time_t *timestamp)

+{

+    data[0] = (long)MIN(MAX(rh.game_quat[0] * ((float)(1L << 30)), -2147483648.), 2147483647.);

+    data[1] = (long)MIN(MAX(rh.game_quat[1] * ((float)(1L << 30)), -2147483648.), 2147483647.);

+    data[2] = (long)MIN(MAX(rh.game_quat[2] * ((float)(1L << 30)), -2147483648.), 2147483647.);

+    data[3] = (long)MIN(MAX(rh.game_quat[3] * ((float)(1L << 30)), -2147483648.), 2147483647.);

+    *timestamp = rh.gam_timestamp;

+    return INV_SUCCESS;

+}

+

+/** Returns a quaternion.

+ * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30.

+ * @return Returns INV_SUCCESS if successful or an error code if not.

+ */

+inv_error_t inv_get_quaternion(long *data)

+{

+    data[0] = (long)MIN(MAX(rh.nav_quat[0] * ((float)(1L << 30)), -2147483648.), 2147483647.);

+    data[1] = (long)MIN(MAX(rh.nav_quat[1] * ((float)(1L << 30)), -2147483648.), 2147483647.);

+    data[2] = (long)MIN(MAX(rh.nav_quat[2] * ((float)(1L << 30)), -2147483648.), 2147483647.);

+    data[3] = (long)MIN(MAX(rh.nav_quat[3] * ((float)(1L << 30)), -2147483648.), 2147483647.);

+

+    return INV_SUCCESS;

+}

+

+#ifdef WIN32

+/** Returns the last 9-axis quaternion genearted by MPL, so that

+ * it can be written to the DMP.

+ * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30.

+ * @return Returns INV_SUCCESS if successful or an error code if not.

+ */

+inv_error_t inv_get_last_quaternion(long *data)

+{

+    memcpy(data, rh.last_quat, sizeof(rh.last_quat));

+    return INV_SUCCESS;

+}

+

+/** Saves the last 9-axis quaternion generated by MPL.

+ * @param[in] data 9-axis quaternion data.

+ */

+inv_error_t inv_set_last_quaternion(long *data)

+{

+    memcpy(rh.last_quat, data, sizeof(rh.last_quat));

+    return INV_SUCCESS;

+}

+#endif

+

+/** Returns the status of the result holder.

+ * @param[out] rh_status Result holder status.

+ * @return Returns INV_SUCCESS if successful or an error code if not.

+ */

+inv_error_t inv_get_result_holder_status(long *rh_status)

+{

+    *rh_status = rh.status;

+    return INV_SUCCESS;

+}

+

+/** Set the status of the result holder.

+ * @return Returns INV_SUCCESS if successful or an error code if not.

+ */

+inv_error_t inv_set_result_holder_status(long rh_status)

+{

+    rh.status = rh_status;

+    return INV_SUCCESS;

+}

+

+/** Returns the status of the authenticity of the quaternion data.

+ * @param[out] value Authenticity of the quaternion data.

+ * @return Returns INV_SUCCESS if successful or an error code if not.

+ */

+inv_error_t inv_get_quaternion_validity(int *value)

+{

+    *value = rh.quat_validity;

+    return INV_SUCCESS;

+}

+

+/** Set the status of the authenticity of the quaternion data.

+ * @return Returns INV_SUCCESS if successful or an error code if not.

+ */

+inv_error_t inv_set_quaternion_validity(int value)

+{

+    rh.quat_validity = value;

+    return INV_SUCCESS;

+}

+

+/** Returns a quaternion based only on compass and accel.

+ * @param[out] data 6-axis  compass and accel quaternion scaled such that 1.0 = 2^30.

+ * @return Returns INV_SUCCESS if successful or an error code if not.

+ */

+inv_error_t inv_get_geomagnetic_quaternion(long *data, inv_time_t *timestamp)

+{

+    data[0] = (long)MIN(MAX(rh.geomag_quat[0] * ((float)(1L << 30)), -2147483648.), 2147483647.);

+    data[1] = (long)MIN(MAX(rh.geomag_quat[1] * ((float)(1L << 30)), -2147483648.), 2147483647.);

+    data[2] = (long)MIN(MAX(rh.geomag_quat[2] * ((float)(1L << 30)), -2147483648.), 2147483647.);

+    data[3] = (long)MIN(MAX(rh.geomag_quat[3] * ((float)(1L << 30)), -2147483648.), 2147483647.);

+    *timestamp = rh.geomag_timestamp;

+    return INV_SUCCESS;

+}

+

+/** Returns a quaternion.

+ * @param[out] data 9-axis quaternion.

+ * @return Returns INV_SUCCESS if successful or an error code if not.

+ */

+inv_error_t inv_get_quaternion_float(float *data)

+{

+    memcpy(data, rh.nav_quat, sizeof(rh.nav_quat));

+    return INV_SUCCESS;

+}

+

+/** Returns a quaternion based only on gyro and accel.

+ * @param[out] data 6-axis  gyro and accel quaternion.

+ * @return Returns INV_SUCCESS if successful or an error code if not.

+ */

+inv_error_t inv_get_6axis_quaternion_float(float *data, inv_time_t *timestamp)

+{

+    memcpy(data, rh.game_quat, sizeof(rh.game_quat));

+    *timestamp = rh.gam_timestamp;

+    return INV_SUCCESS;

+}

+

+/** Returns a quaternion based only on compass and accel.

+ * @param[out] data 6-axis  compass and accel quaternion.

+ * @return Returns INV_SUCCESS if successful or an error code if not.

+ */

+inv_error_t inv_get_geomagnetic_quaternion_float(float *data, inv_time_t *timestamp)

+{

+    memcpy(data, rh.geomag_quat, sizeof(rh.geomag_quat));

+    *timestamp = rh.geomag_timestamp;

+    return INV_SUCCESS;

+}

+

+/** Returns a quaternion with accuracy and timestamp.

+ * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30.

+ * @param[out] accuracy Accuracy of quaternion, 0-3, where 3 is most accurate.

+ * @param[out] timestamp Timestamp of this quaternion in nanoseconds

+ */

+void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp)

+{

+    inv_get_quaternion(data);

+    *timestamp = inv_get_last_timestamp();

+    if (inv_get_compass_on()) {

+        *accuracy = inv_get_mag_accuracy();

+    } else if (inv_get_gyro_on()) {

+        *accuracy = inv_get_gyro_accuracy();

+    }else if (inv_get_accel_on()) {

+        *accuracy = inv_get_accel_accuracy();

+    } else {

+        *accuracy = 0;

+    }

+}

+

+/** Callback that gets called everytime there is new data. It is 

+ * registered by inv_start_results_holder().

+ * @param[in] sensor_cal New sensor data to process.

+ * @return Returns INV_SUCCESS if successful or an error code if not.

+ */

+inv_error_t inv_generate_results(struct inv_sensor_cal_t *sensor_cal)

+{

+    rh.sensor = sensor_cal;

+    return INV_SUCCESS;

+}

+

+/** Function to turn on this module. This is automatically called by

+ *  inv_enable_results_holder(). Typically not called by users.

+ * @return Returns INV_SUCCESS if successful or an error code if not.

+ */

+inv_error_t inv_start_results_holder(void)

+{

+    inv_register_data_cb(inv_generate_results, INV_PRIORITY_RESULTS_HOLDER,

+        INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW);

+    return INV_SUCCESS;

+}

+

+/** Initializes results holder. This is called automatically by the

+* enable function inv_enable_results_holder(). It may be called any time the feature is enabled, but

+* is typically not needed to be called by outside callers.

+* @return Returns INV_SUCCESS if successful or an error code if not.

+*/

+inv_error_t inv_init_results_holder(void)

+{

+    memset(&rh, 0, sizeof(rh));

+    rh.mag_scale[0] = 1L<<30;

+    rh.mag_scale[1] = 1L<<30;

+    rh.mag_scale[2] = 1L<<30;

+    rh.compass_correction[0] = 1L<<30;

+    rh.gam_quat[0] = 1L<<30;

+    rh.nav_quat[0] = 1.;

+    rh.geomag_quat[0] = 1.;

+    rh.game_quat[0] = 1.;

+    rh.accel_quat[0] = 1L<<30;

+    rh.geomag_compass_correction[0] = 1L<<30;

+    rh.quat_confidence_interval = (float)M_PI;

+#ifdef WIN32

+    rh.last_quat[0] = 1L<<30;

+#endif

+

+

+#ifdef TEST

+    {

+        struct local_field_t local_field;

+        local_field.intensity = 48.0f;   // uT

+        local_field.inclination = 60.0f; // degree

+        local_field.declination = 0.0f;  // degree

+        inv_set_earth_magnetic_local_field_parameter(&local_field);

+    //    inv_set_local_field_status(LOCAL_FILED_NOT_SET_BY_USER);

+        inv_set_local_field_status(LOCAL_FILED_SET_BY_USER);

+   	    inv_set_local_magnetic_field(48.0f, 59.0f, 0.0f);

+

+        // set default mpl calibration result for testing

+        local_field.intensity = 50.0f;   // uT

+        local_field.inclination = 59.0f; // degree

+        local_field.declination = 0.0f;  // degree

+        inv_set_mpl_magnetic_local_field_parameter(&local_field);

+        inv_set_mpl_mag_field_status(LOCAL_FIELD_SET_BUT_NOT_MATCH_WITH_MPL);

+    }

+#endif

+

+    return INV_SUCCESS;

+}

+

+/** Turns on storage of results.

+*/

+inv_error_t inv_enable_results_holder()

+{

+    inv_error_t result;

+    result = inv_init_results_holder();

+    if ( result ) {

+        return result;

+    }

+

+    result = inv_register_mpl_start_notification(inv_start_results_holder);

+    return result;

+}

+

+/** Sets state of if we know the accel bias.

+ * @return return 1 if we know the accel bias, 0 if not.

+ *            it is set with inv_set_accel_bias_found()

+ */

+int inv_got_accel_bias()

+{

+    return rh.got_accel_bias;

+}

+

+/** Sets whether we know the accel bias

+ * @param[in] state Set to 1 if we know the accel bias. 

+ *            Can be retrieved with inv_got_accel_bias()

+ */

+void inv_set_accel_bias_found(int state)

+{

+    rh.got_accel_bias = state;

+}

+

+/** Sets state of if we know the compass bias.

+ * @return return 1 if we know the compass bias, 0 if not.

+ *            it is set with inv_set_compass_bias_found()

+ */

+int inv_got_compass_bias()

+{

+    return rh.got_compass_bias;

+}

+

+/** Sets whether we know the compass bias

+ * @param[in] state Set to 1 if we know the compass bias. 

+ *            Can be retrieved with inv_got_compass_bias()

+ */

+void inv_set_compass_bias_found(int state)

+{

+    rh.got_compass_bias = state;

+}

+

+/** Sets the compass state.

+ * @param[in] state Compass state. It can be retrieved with inv_get_compass_state().

+ */

+void inv_set_compass_state(int state)

+{

+    rh.compass_state = state;

+}

+

+/** Get's the compass state

+ * @return the compass state that was set with inv_set_compass_state()

+ */

+int inv_get_compass_state()

+{

+    return rh.compass_state;

+}

+

+/** Set compass bias error. See inv_get_compass_bias_error()

+ * @param[in] bias_error Set's how accurate we know the compass bias. It is the 

+ * error squared.

+ */

+void inv_set_compass_bias_error(const long *bias_error)

+{

+    memcpy(rh.compass_bias_error, bias_error, sizeof(rh.compass_bias_error));

+}

+

+/** Get's compass bias error. See inv_set_compass_bias_error() for setting.

+ * @param[out] bias_error Accuracy as to how well the compass bias is known. It is the error squared.

+ */

+void inv_get_compass_bias_error(long *bias_error)

+{

+    memcpy(bias_error, rh.compass_bias_error, sizeof(rh.compass_bias_error));

+}

+

+/**

+ *  @brief      Returns 3-element vector of accelerometer data in body frame

+ *                with gravity removed

+ *  @param[out] data    3-element vector of accelerometer data in body frame

+ *                with gravity removed

+ *  @return     INV_SUCCESS if successful

+ *              INV_ERROR_INVALID_PARAMETER if invalid input pointer

+ */

+inv_error_t inv_get_linear_accel(long *data)

+{

+    long gravity[3];

+

+    if (data != NULL)

+    {

+        inv_get_accel_set(data, NULL, NULL);

+        inv_get_gravity(gravity);

+        data[0] -= gravity[0] >> 14;

+        data[1] -= gravity[1] >> 14;

+        data[2] -= gravity[2] >> 14;

+        return INV_SUCCESS;

+    }

+    else {

+        return INV_ERROR_INVALID_PARAMETER;

+    }

+}

+

+/**

+ *  @brief      Returns 3-element vector of accelerometer data in body frame

+ *  @param[out] data    3-element vector of accelerometer data in body frame

+ *  @return     INV_SUCCESS if successful

+ *              INV_ERROR_INVALID_PARAMETER if invalid input pointer

+ */

+inv_error_t inv_get_accel(long *data)

+{

+    if (data != NULL) {

+        inv_get_accel_set(data, NULL, NULL);

+        return INV_SUCCESS;

+    }

+    else {

+        return INV_ERROR_INVALID_PARAMETER;

+    }

+}

+

+/**

+ *  @brief      Returns 3-element vector of accelerometer float data

+ *  @param[out] data    3-element vector of accelerometer float data

+ *  @return     INV_SUCCESS if successful

+ *              INV_ERROR_INVALID_PARAMETER if invalid input pointer

+ */

+inv_error_t inv_get_accel_float(float *data)

+{

+    long tdata[3];

+    unsigned char i;

+

+    if (data != NULL && !inv_get_accel(tdata)) {

+        for (i = 0; i < 3; ++i) {

+            data[i] = ((float)tdata[i] / (1L << 16));

+        }

+        return INV_SUCCESS;

+    }

+    else {

+        return INV_ERROR_INVALID_PARAMETER;

+    }

+}

+

+/**

+ *  @brief      Returns 3-element vector of gyro float data

+ *  @param[out] data    3-element vector of gyro float data

+ *  @return     INV_SUCCESS if successful

+ *              INV_ERROR_INVALID_PARAMETER if invalid input pointer

+ */

+inv_error_t inv_get_gyro_float(float *data)

+{

+    long tdata[3];

+    unsigned char i;

+

+    if (data != NULL) {

+        inv_get_gyro_set(tdata, NULL, NULL);

+        for (i = 0; i < 3; ++i) {

+            data[i] = ((float)tdata[i] / (1L << 16));

+        }

+        return INV_SUCCESS;

+    }

+    else {

+        return INV_ERROR_INVALID_PARAMETER;

+    }

+}

+

+/** Set 9 axis 95% heading confidence interval for quaternion

+* @param[in] ci Confidence interval in radians.

+*/

+void inv_set_heading_confidence_interval(float ci)

+{

+    rh.quat_confidence_interval = ci;

+}

+

+/** Get 9 axis 95% heading confidence interval for quaternion

+* @return Confidence interval in radians.

+*/

+float inv_get_heading_confidence_interval(void)

+{

+    return rh.quat_confidence_interval;

+}

+

+/** Set 6 axis (accel and compass) 95% heading confidence interval for quaternion

+* @param[in] ci Confidence interval in radians.

+*/

+void inv_set_accel_compass_confidence_interval(float ci)

+{

+    rh.geo_mag_confidence_interval = ci;

+}

+

+/** Get 6 axis (accel and compass) 95% heading confidence interval for quaternion

+* @return Confidence interval in radians.

+*/

+float inv_get_accel_compass_confidence_interval(void)

+{

+    return rh.geo_mag_confidence_interval;

+}

+

+/**

+ *  @brief      Returns 3-element vector of linear accel float data

+ *  @param[out] data    3-element vector of linear aceel float data

+ *  @return     INV_SUCCESS if successful

+ *              INV_ERROR_INVALID_PARAMETER if invalid input pointer

+ */

+inv_error_t inv_get_linear_accel_float(float *data)

+{

+    long tdata[3];

+    unsigned char i;

+

+    if (data != NULL && !inv_get_linear_accel(tdata)) {

+        for (i = 0; i < 3; ++i) {

+            data[i] = ((float)tdata[i] / (1L << 16));

+        }

+        return INV_SUCCESS;

+    }

+    else {

+        return INV_ERROR_INVALID_PARAMETER;

+    }

+}

+

+/**

+ *  @brief      Returns the status of earth magnetic field local field parameters

+ *  @param[out] N/A

+ *  @return     status of local field, defined in enum compass_local_field_e

+ */

+enum compass_local_field_e inv_get_local_field_status(void)

+{

+    return rh.mag_local_field.mpl_match_status;

+}

+

+/** Set the status of earth magnetic field local field parameters

+* @param[in] status of earth magnetic field local field parameters.

+*/

+void inv_set_local_field_status(enum compass_local_field_e status)

+{

+    rh.mag_local_field.mpl_match_status = status;

+}

+

+/** Set the parameters of earth magnetic field local field

+* @param[in] the earth magnetic field local field parameters.

+*/

+void inv_set_earth_magnetic_local_field_parameter(struct local_field_t *parameters)

+{

+    rh.mag_local_field.intensity = parameters->intensity;        // radius

+    rh.mag_local_field.inclination = parameters->inclination;    // dip angle

+    rh.mag_local_field.declination = parameters->declination;    // yaw deviation angle from true north

+    

+    inv_set_local_field_status(LOCAL_FILED_SET_BY_USER);

+}

+

+/**

+ *  @brief      Returns the parameters of earth magnetic field local field

+ *  @param[out] the parameters of earth magnetic field local field

+ *  @return     N/A

+ */

+void inv_get_earth_magnetic_local_field_parameter(struct local_field_t *parameters)

+{

+    parameters->intensity = rh.mag_local_field.intensity;        // radius

+    parameters->inclination = rh.mag_local_field.inclination;    // dip angle

+    parameters->declination = rh.mag_local_field.declination;    // yaw deviation angle from true north

+    parameters->mpl_match_status = rh.mag_local_field.mpl_match_status;

+}

+

+/**

+ *  @brief      Returns the status of mpl calibrated magnetic field local field parameters

+ *  @param[out] N/A

+ *  @return     status of local field, defined in enum compass_local_field_e

+ */

+enum compass_local_field_e inv_get_mpl_mag_field_status(void)

+{

+    return rh.mpl_compass_cal.mpl_match_status;

+}

+

+/** Set the status of mpl calibrated magnetic field local field parameters

+* @param[in] status of earth magnetic field local field parameters.

+*/

+void inv_set_mpl_mag_field_status(enum compass_local_field_e status)

+{

+    rh.mpl_compass_cal.mpl_match_status = status;

+}

+

+/** Set the magnetic field local field struct object

+* @param[in] status of earth magnetic field local field parameters.

+*/

+inv_error_t inv_set_local_magnetic_field(float intensity, float inclination, float declination)

+{

+	struct local_field_t local_field;

+	local_field.intensity = intensity;  // radius

+    local_field.inclination = inclination; // dip angle angle degree

+    local_field.declination = declination; // yaw deviation angle from true north, eastward as positive

+    local_field.mpl_match_status = LOCAL_FILED_SET_BY_USER;

+

+	inv_set_earth_magnetic_local_field_parameter(&local_field);

+	return 0;

+}

+

+/** Set the parameters of mpl calibrated magnetic field local field

+*   This API is used by mpl only.

+* @param[in] the earth magnetic field local field parameters.

+ *  @return     INV_SUCCESS if successful

+ *              INV_ERROR_INVALID_PARAMETER if invalid input pointer

+ */

+inv_error_t inv_set_mpl_magnetic_local_field_parameter(struct local_field_t *parameters)

+{

+    enum compass_local_field_e mpl_status;

+    struct local_field_t local_field;

+    inv_error_t status;

+

+    rh.mpl_compass_cal.intensity = parameters->intensity;        // radius

+    rh.mpl_compass_cal.inclination = parameters->inclination;    // dip angle

+    rh.mpl_compass_cal.declination = parameters->declination;    // yaw deviation angle from true north

+

+    mpl_status = inv_get_mpl_mag_field_status();

+    inv_get_earth_magnetic_local_field_parameter(&local_field);

+

+    status = INV_SUCCESS;

+

+    switch (inv_get_local_field_status())  {

+    case LOCAL_FILED_NOT_SET_BY_USER:

+        if (mpl_status == LOCAL_FILED_NOT_SET_BY_USER)  {

+            inv_set_mpl_mag_field_status(LOCAL_FILED_NOT_SET_BY_USER_BUT_SET_BY_MPL);

+        } else {

+            // illegal status

+            status = INV_ERROR_INVALID_PARAMETER;

+        }

+        break;

+    case LOCAL_FILED_SET_BY_USER:

+        switch (mpl_status) {

+            case LOCAL_FIELD_SET_BUT_NOT_MATCH_WITH_MPL:

+            case LOCAL_FIELD_SET_MATCH_WITH_MPL:

+                if  ( (ABS(local_field.intensity - parameters->intensity) < 5.0f) &&

+                      (ABS(local_field.intensity - parameters->intensity) < 5.0f)  )  {

+                    inv_set_mpl_mag_field_status(LOCAL_FIELD_SET_MATCH_WITH_MPL);

+                } else {

+                    inv_set_mpl_mag_field_status(LOCAL_FIELD_SET_BUT_NOT_MATCH_WITH_MPL);

+                }

+            break;

+            case LOCAL_FILED_NOT_SET_BY_USER_BUT_SET_BY_MPL:

+            // no status update            

+            break;

+            case LOCAL_FILED_NOT_SET_BY_USER:

+            case LOCAL_FILED_SET_BY_USER:

+            default:

+            // illegal status

+            status = INV_ERROR_INVALID_PARAMETER;

+            break;

+        }

+        break;

+    case LOCAL_FILED_NOT_SET_BY_USER_BUT_SET_BY_MPL:

+    case LOCAL_FIELD_SET_BUT_NOT_MATCH_WITH_MPL:

+    case LOCAL_FIELD_SET_MATCH_WITH_MPL:

+    default:

+        // illegal status

+        status = INV_ERROR_INVALID_PARAMETER;

+        break;

+    }

+    return status;

+}

+

+/**

+ *  @brief      Returns the parameters of mpl calibrated magnetic field local field

+ *  @param[out] the parameters of earth magnetic field local field

+ *  @return     N/A

+ */

+void inv_get_mpl_magnetic_local_field_parameter(struct local_field_t *parameters)

+{

+    parameters->intensity = rh.mpl_compass_cal.intensity;        // radius

+    parameters->inclination = rh.mpl_compass_cal.inclination;    // dip angle

+    parameters->declination = rh.mpl_compass_cal.declination;    // yaw deviation angle from true north

+    parameters->mpl_match_status = rh.mpl_compass_cal.mpl_match_status;

+}

+

+/**

+ * @}

+ */

diff --git a/6515/libsensors_iio/software/core/mllite/results_holder.h b/6515/libsensors_iio/software/core/mllite/results_holder.h
new file mode 100644
index 0000000..342080d
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/results_holder.h
@@ -0,0 +1,144 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#include "mltypes.h"
+
+#ifndef INV_RESULTS_HOLDER_H__
+#define INV_RESULTS_HOLDER_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define INV_MOTION                       0x0001
+#define INV_NO_MOTION                    0x0002
+
+
+
+    /**************************************************************************/
+    /*  The value of inv_get_gyro_sum_of_sqr is scaled such the (1 dps)^2 =   */
+    /*  2^GYRO_MAG_SQR_SHIFT. This number must be >=0 and even.               */
+    /*  The value of inv_accel_sum_of_sqr is scaled such that (1g)^2 =        */
+    /*  2^ACC_MAG_SQR_SHIFT                                                   */
+    /**************************************************************************/
+#define ACC_MAG_SQR_SHIFT 16
+
+enum compass_local_field_e {
+    // status for user input earth magnetic local field
+    LOCAL_FILED_NOT_SET_BY_USER                     = 0,
+    LOCAL_FILED_SET_BY_USER                         = 1,
+
+    // status for mpl calibrated based magnetical field
+    LOCAL_FILED_NOT_SET_BY_USER_BUT_SET_BY_MPL      = 2,
+    LOCAL_FIELD_SET_BUT_NOT_MATCH_WITH_MPL          = 3,
+    LOCAL_FIELD_SET_MATCH_WITH_MPL                  = 4,
+};
+
+struct local_field_t {
+    float intensity;  // radius

+    float inclination; // dip angle angle degree

+    float declination; // yaw deviation angle from true north, eastward as positive
+    enum compass_local_field_e mpl_match_status;
+};
+
+// earth magnetic field access API
+enum compass_local_field_e inv_get_local_field_status(void);
+void inv_set_local_field_status(enum compass_local_field_e status);
+
+void inv_set_earth_magnetic_local_field_parameter(struct local_field_t *parameters);
+void inv_get_earth_magnetic_local_field_parameter(struct local_field_t *parameters);
+
+// mpl calibrated magnetic field access API
+enum compass_local_field_e inv_get_mpl_mag_field_status(void);
+void inv_set_mpl_mag_field_status(enum compass_local_field_e status);
+
+inv_error_t inv_set_mpl_magnetic_local_field_parameter(struct local_field_t *parameters);
+void inv_get_mpl_magnetic_local_field_parameter(struct local_field_t *parameters);
+
+// quaternion store API
+void inv_store_gaming_quaternion(const long *quat, inv_time_t timestamp);
+void inv_store_accel_quaternion(const long *quat, inv_time_t timestamp);
+void inv_store_nav_quaternion(const float *quat, inv_time_t timestamp);
+void inv_store_game_quaternion(const float *quat, inv_time_t timestamp);
+void inv_store_geomag_quaternion(const float *quat, inv_time_t timestamp);
+
+// States
+#define SF_NORMAL 0
+#define SF_UNCALIBRATED 1
+#define SF_STARTUP_SETTLE 2
+#define SF_FAST_SETTLE 3
+#define SF_DISTURBANCE 4
+#define SF_SLOW_SETTLE 5
+
+// These 2 status bits are used to control when the 9 axis quaternion is updated
+#define INV_COMPASS_CORRECTION_SET 1
+#define INV_6_AXIS_QUAT_SET 2
+#define INV_GEOMAGNETIC_CORRECTION_SET 4
+
+int inv_get_acc_state();
+void inv_set_acc_state(int state);
+int inv_get_motion_state(unsigned int *cntr);
+void inv_set_motion_state(unsigned char state);
+inv_error_t inv_get_gravity(long *data);
+inv_error_t inv_get_gravity_6x(long *data);
+inv_error_t inv_get_6axis_quaternion(long *data, inv_time_t *timestamp);
+inv_error_t inv_get_quaternion(long *data);
+inv_error_t inv_get_quaternion_float(float *data);
+inv_error_t inv_get_6axis_quaternion_float(float *data, inv_time_t *timestamp);
+inv_error_t inv_get_geomagnetic_quaternion_float(float *data, inv_time_t *timestamp);
+#ifdef WIN32
+inv_error_t inv_get_last_quaternion(long *data);
+inv_error_t inv_set_last_quaternion(long *data);
+#endif
+void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp);
+inv_error_t inv_get_accel_quaternion(long *data);
+inv_error_t inv_get_geomagnetic_quaternion(long *data, inv_time_t *timestamp);
+void inv_set_geomagnetic_compass_correction(const long *data, inv_time_t timestamp);
+void inv_get_geomagnetic_compass_correction(long *data, inv_time_t *timestamp);
+inv_error_t inv_get_result_holder_status(long *rh_status);
+inv_error_t inv_set_result_holder_status(long rh_status);
+inv_error_t inv_get_quaternion_validity(int *value);
+inv_error_t inv_set_quaternion_validity(int value);
+
+// set magnetic field by location
+inv_error_t inv_set_local_magnetic_field(float intensity, float inclination, float declination);
+
+inv_error_t inv_enable_results_holder();
+inv_error_t inv_init_results_holder(void);
+
+/* Magnetic Field Parameters*/
+void inv_set_mag_scale(const long *data);
+void inv_get_mag_scale(long *data);
+void inv_set_compass_correction(const long *data, inv_time_t timestamp);
+void inv_get_compass_correction(long *data, inv_time_t *timestamp);
+int inv_got_compass_bias();
+void inv_set_compass_bias_found(int state);
+int inv_get_large_mag_field();
+void inv_set_large_mag_field(int state);
+void inv_set_compass_state(int state);
+int inv_get_compass_state();
+void inv_set_compass_bias_error(const long *bias_error);
+void inv_get_compass_bias_error(long *bias_error);
+inv_error_t inv_get_linear_accel(long *data);
+inv_error_t inv_get_accel(long *data);
+inv_error_t inv_get_accel_float(float *data);
+inv_error_t inv_get_gyro_float(float *data);
+inv_error_t inv_get_linear_accel_float(float *data);
+void inv_set_heading_confidence_interval(float ci);
+float inv_get_heading_confidence_interval(void);
+
+void inv_set_accel_compass_confidence_interval(float ci);
+float inv_get_accel_compass_confidence_interval(void);
+
+int inv_got_accel_bias();
+void inv_set_accel_bias_found(int state);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_RESULTS_HOLDER_H__
diff --git a/6515/libsensors_iio/software/core/mllite/start_manager.c b/6515/libsensors_iio/software/core/mllite/start_manager.c
new file mode 100644
index 0000000..3e082d4
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/start_manager.c
@@ -0,0 +1,105 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+/**
+ *   @defgroup  Start_Manager start_manager
+ *   @brief     Motion Library - Start Manager
+ *              Start Manager
+ *
+ *   @{
+ *       @file start_manager.c
+ *       @brief This handles all the callbacks when inv_start_mpl() is called.
+ */
+
+
+#include <string.h>
+#include "log.h"
+#include "start_manager.h"
+
+typedef inv_error_t (*inv_start_cb_func)();
+struct inv_start_cb_t {
+    int num_cb;
+    inv_start_cb_func start_cb[INV_MAX_START_CB];
+};
+
+static struct inv_start_cb_t inv_start_cb;
+
+/** Initilize the start manager. Typically called by inv_start_mpl();
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_init_start_manager(void)
+{
+    memset(&inv_start_cb, 0, sizeof(inv_start_cb));
+    return INV_SUCCESS;
+}
+
+/** Removes a callback from start notification
+* @param[in] start_cb function to remove from start notification
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_unregister_mpl_start_notification(inv_error_t (*start_cb)(void))
+{
+    int kk;
+
+    for (kk=0; kk<inv_start_cb.num_cb; ++kk) {
+        if (inv_start_cb.start_cb[kk] == start_cb) {
+            // Found the match
+            if (kk != (inv_start_cb.num_cb-1)) {
+                memmove(&inv_start_cb.start_cb[kk],
+                    &inv_start_cb.start_cb[kk+1],
+                    (inv_start_cb.num_cb-kk-1)*sizeof(inv_start_cb_func));
+            }
+            inv_start_cb.num_cb--;
+            return INV_SUCCESS;
+        }
+    }
+    return INV_ERROR_INVALID_PARAMETER;
+}
+
+/** Register a callback to receive when inv_start_mpl() is called.
+* @param[in] start_cb Function callback that will be called when inv_start_mpl() is
+*            called.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_register_mpl_start_notification(inv_error_t (*start_cb)(void))
+{
+    if (inv_start_cb.num_cb >= INV_MAX_START_CB)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    inv_start_cb.start_cb[inv_start_cb.num_cb] = start_cb;
+    inv_start_cb.num_cb++;
+    return INV_SUCCESS;
+}
+
+/** Callback all the functions that want to be notified when inv_start_mpl() was
+* called.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_execute_mpl_start_notification(void)
+{
+    inv_error_t result,first_error;
+    int kk;
+
+    first_error = INV_SUCCESS;
+
+    for (kk = 0; kk < inv_start_cb.num_cb; ++kk) {
+        result = inv_start_cb.start_cb[kk]();
+        if (result && (first_error == INV_SUCCESS)) {
+            first_error = result;
+        }
+    }
+    return first_error;
+}
+
+/**
+ * @}
+ */
diff --git a/6515/libsensors_iio/software/core/mllite/start_manager.h b/6515/libsensors_iio/software/core/mllite/start_manager.h
new file mode 100644
index 0000000..899e3f5
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/start_manager.h
@@ -0,0 +1,27 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#ifndef INV_START_MANAGER_H__
+#define INV_START_MANAGER_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "mltypes.h"
+
+/** Max number of start callbacks we can handle. */
+#define INV_MAX_START_CB 20
+
+inv_error_t inv_init_start_manager(void);
+inv_error_t inv_register_mpl_start_notification(inv_error_t (*start_cb)(void));
+inv_error_t inv_execute_mpl_start_notification(void);
+inv_error_t inv_unregister_mpl_start_notification(inv_error_t (*start_cb)(void));
+
+#ifdef __cplusplus
+}
+#endif
+#endif  // INV_START_MANAGER_H__
diff --git a/6515/libsensors_iio/software/core/mllite/storage_manager.c b/6515/libsensors_iio/software/core/mllite/storage_manager.c
new file mode 100644
index 0000000..419b9d0
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/storage_manager.c
@@ -0,0 +1,210 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/**
+ *   @defgroup  Storage_Manager storage_manager
+ *   @brief     Motion Library - Stores Data for functions.
+ *
+ *
+ *   @{
+ *       @file storage_manager.c
+ *       @brief Load and Store Manager.
+ */
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MLLITE"
+
+#include <string.h>
+
+#include "storage_manager.h"
+#include "log.h"
+#include "ml_math_func.h"
+#include "mlmath.h"
+
+/* Must be changed if the format of storage changes */
+#define DEFAULT_KEY 29681
+
+typedef inv_error_t (*load_func_t)(const unsigned char *data);
+typedef inv_error_t (*save_func_t)(unsigned char *data);
+/** Max number of entites that can be stored */
+#define NUM_STORAGE_BOXES 20
+
+struct data_header_t {
+    long size;
+    uint32_t checksum;
+    unsigned int key;
+};
+
+struct data_storage_t {
+    int num; /**< Number of differnt save entities */
+    size_t total_size; /**< Size in bytes to store non volatile data */
+    load_func_t load[NUM_STORAGE_BOXES]; /**< Callback to load data */
+    save_func_t save[NUM_STORAGE_BOXES]; /**< Callback to save data */
+    struct data_header_t hd[NUM_STORAGE_BOXES]; /**< Header info for each entity */
+};
+static struct data_storage_t ds;
+
+/** Should be called once before using any of the storage methods. Typically
+* called first by inv_init_mpl().*/
+void inv_init_storage_manager()
+{
+    memset(&ds, 0, sizeof(ds));
+    ds.total_size = sizeof(struct data_header_t);
+}
+
+/** Used to register your mechanism to load and store non-volative data. This should typical be
+* called during the enable function for your feature.
+* @param[in] load_func function pointer you will use to receive data that was stored for you.
+* @param[in] save_func function pointer you will use to save any data you want saved to
+*            non-volatile memory between runs.
+* @param[in] size The size in bytes of the amount of data you want loaded and saved.
+* @param[in] key The key associated with your data type should be unique across MPL.
+*                    The key should change when your type of data for storage changes.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_register_load_store(inv_error_t (*load_func)(const unsigned char *data),
+                                    inv_error_t (*save_func)(unsigned char *data), size_t size, unsigned int key)
+{
+    int kk;
+    // Check if this has been registered already
+    for (kk=0; kk<ds.num; ++kk) {
+        if (key == ds.hd[kk].key) {
+            return INV_ERROR_INVALID_PARAMETER;
+        }
+    }
+    // Make sure there is room
+    if (ds.num >= NUM_STORAGE_BOXES) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+    // Add to list
+    ds.hd[ds.num].key = key;
+    ds.hd[ds.num].size = size;
+    ds.load[ds.num] = load_func;
+    ds.save[ds.num] = save_func;
+    ds.total_size += size + sizeof(struct data_header_t);
+    ds.num++;
+
+    return INV_SUCCESS;
+}
+
+/** Returns the memory size needed to perform a store
+* @param[out] size Size in bytes of memory needed to store.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_get_mpl_state_size(size_t *size)
+{
+    *size = ds.total_size;
+    return INV_SUCCESS;
+}
+
+/** @internal
+ * Finds key in ds.hd[] array and returns location
+ * @return location where key exists in array, -1 if not found.
+ */
+static int inv_find_entry(unsigned int key)
+{
+    int kk;
+    for (kk=0; kk<ds.num; ++kk) {
+        if (key == ds.hd[kk].key) {
+            return kk;
+        }
+    }
+    return -1;
+}
+
+/** This function takes a block of data that has been saved in non-volatile memory and pushes
+* to the proper locations. Multiple error checks are performed on the data.
+* @param[in] data Data that was saved to be loaded up by MPL
+* @param[in] length Length of data vector in bytes
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_load_mpl_states(const unsigned char *data, size_t length)
+{
+    struct data_header_t *hd;
+    int entry;
+    uint32_t checksum;
+    long len;
+
+    len = length; // Important so we get negative numbers
+    if (data == NULL || len == 0)
+        return INV_SUCCESS;
+    if (len < sizeof(struct data_header_t))
+        return INV_ERROR_CALIBRATION_LOAD;  // No data
+    hd = (struct data_header_t *)data;
+    if (hd->key != DEFAULT_KEY)
+        return INV_ERROR_CALIBRATION_LOAD;  // Key changed or data corruption
+    len = MIN(hd->size, len);
+    len = hd->size;
+    len -= sizeof(struct data_header_t);
+    data += sizeof(struct data_header_t);
+    checksum = inv_checksum(data, len);
+    if (checksum != hd->checksum)
+        return INV_ERROR_CALIBRATION_LOAD;  // Data corruption
+
+    while (len > (long)sizeof(struct data_header_t)) {
+        hd = (struct data_header_t *)data;
+        entry = inv_find_entry(hd->key);
+        data += sizeof(struct data_header_t);
+        len -= sizeof(struct data_header_t);
+        if (entry >= 0 && len >= hd->size) {
+            if (hd->size != ds.hd[entry].size)
+                return INV_ERROR_CALIBRATION_LEN;
+            checksum = inv_checksum(data, hd->size);
+            if (checksum != hd->checksum)
+                return INV_ERROR_CALIBRATION_LOAD;
+            ds.load[entry](data);
+        }
+        len -= hd->size;
+        if (len >= 0)
+            data = data + hd->size;
+    }
+
+    return INV_SUCCESS;
+}
+
+/** This function fills up a block of memory to be stored in non-volatile memory.
+* @param[out] data Place to store data, size of sz, must be at least size
+*                  returned by inv_get_mpl_state_size()
+* @param[in] sz Size of data.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_save_mpl_states(unsigned char *data, size_t sz)
+{
+    unsigned char *cur;
+    int kk;
+    struct data_header_t *hd;
+
+    if (data == NULL || sz == 0)
+        return INV_ERROR_CALIBRATION_LOAD;
+    if (sz >= ds.total_size) {
+        cur = data + sizeof(struct data_header_t);
+        for (kk = 0; kk < ds.num; ++kk) {
+            hd = (struct data_header_t *)cur;
+            cur += sizeof(struct data_header_t);
+            ds.save[kk](cur);
+            hd->checksum = inv_checksum(cur, ds.hd[kk].size);
+            hd->size = ds.hd[kk].size;
+            hd->key = ds.hd[kk].key;
+            cur += ds.hd[kk].size;
+        }
+    } else {
+        return INV_ERROR_CALIBRATION_LOAD;
+    }
+
+    hd = (struct data_header_t *)data;
+    hd->checksum = inv_checksum(data + sizeof(struct data_header_t),
+                                ds.total_size - sizeof(struct data_header_t));
+    hd->key = DEFAULT_KEY;
+    hd->size = ds.total_size;
+
+    return INV_SUCCESS;
+}
+
+/**
+ * @}
+ */
diff --git a/6515/libsensors_iio/software/core/mllite/storage_manager.h b/6515/libsensors_iio/software/core/mllite/storage_manager.h
new file mode 100644
index 0000000..7255591
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mllite/storage_manager.h
@@ -0,0 +1,30 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#include "mltypes.h"
+
+#ifndef INV_STORAGE_MANAGER_H__
+#define INV_STORAGE_MANAGER_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_register_load_store(
+                           inv_error_t (*load_func)(const unsigned char *data),
+                           inv_error_t (*save_func)(unsigned char *data), 
+                           size_t size, unsigned int key);
+void inv_init_storage_manager(void);
+
+inv_error_t inv_get_mpl_state_size(size_t *size);
+inv_error_t inv_load_mpl_states(const unsigned char *data, size_t len);
+inv_error_t inv_save_mpl_states(unsigned char *data, size_t len);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* INV_STORAGE_MANAGER_H__ */
diff --git a/6515/libsensors_iio/software/core/mpl/accel_auto_cal.h b/6515/libsensors_iio/software/core/mpl/accel_auto_cal.h
new file mode 100644
index 0000000..6641a7f
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/accel_auto_cal.h
@@ -0,0 +1,38 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id$
+ *
+ ******************************************************************************/
+
+#ifndef MLDMP_ACCEL_AUTO_CALIBRATION_H__
+#define MLDMP_ACCEL_AUTO_CALIBRATION_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_enable_in_use_auto_calibration(void);
+inv_error_t inv_disable_in_use_auto_calibration(void);
+inv_error_t inv_stop_in_use_auto_calibration(void);
+inv_error_t inv_start_in_use_auto_calibration(void);
+inv_error_t inv_in_use_auto_calibration_is_enabled(unsigned char *is_enabled);
+inv_error_t inv_init_in_use_auto_calibration(void);
+void inv_init_accel_maxmin(void);
+void inv_record_good_accel_maxmin(void);
+int inv_get_accel_bias_stage();
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // MLDMP_ACCEL_AUTO_CALIBRATION_H__
+
diff --git a/6515/libsensors_iio/software/core/mpl/authenticate.h b/6515/libsensors_iio/software/core/mpl/authenticate.h
new file mode 100644
index 0000000..df3e54d
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/authenticate.h
@@ -0,0 +1,29 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id$
+ *
+ ******************************************************************************/
+
+#ifndef MLDMP_AUTHENTICATE_H__
+#define MLDMP_AUTHENTICATE_H__
+
+#include "invensense.h"
+
+inv_error_t inv_check_key(void);
+
+#define INV_9AXIS_QUAT_VALID              0x10
+#define INV_9AXIS_QUAT_FALSE              0x20
+#define INV_6AXIS_QUAT_VALID              0x40
+#define INV_6AXIS_QUAT_FALSE              0x80
+#define INV_6AXIS_QUAT_NO_DMP             0x100
+#define INV_6AXIS_GEOMAG_QUAT_VALID       0x200
+#define INV_6AXIS_GEOMAG_QUAT_FALSE       0x400
+
+#endif  /* MLDMP_AUTHENTICATE_H__ */
diff --git a/6515/libsensors_iio/software/core/mpl/build/android/libmplmpu.so b/6515/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
new file mode 100755
index 0000000..e1b0276
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
diff --git a/6515/libsensors_iio/software/core/mpl/build/android/shared.mk b/6515/libsensors_iio/software/core/mpl/build/android/shared.mk
new file mode 100644
index 0000000..1d6f904
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/build/android/shared.mk
@@ -0,0 +1,90 @@
+MPL_LIB_NAME ?= mplmpu
+LIBRARY = $(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
+COMP  ?= $(CROSS)gcc
+LINK  ?= $(CROSS)gcc
+STRIP ?= $(CROSS)strip -g
+
+OBJFOLDER = $(CURDIR)/obj
+
+INV_ROOT = ../../../../..
+MLLITE_DIR  = $(INV_ROOT)/software/core/mllite
+MPL_DIR  = $(INV_ROOT)/software/core/mpl
+
+include $(INV_ROOT)/software/build/android/common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += $(ANDROID_COMPILE)
+CFLAGS += -Wall
+CFLAGS += -fpic
+CFLAGS += -nostdlib
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT
+CFLAGS += -DLINUX
+CFLAGS += -DANDROID
+CFLAGS += -mthumb-interwork
+CFLAGS += -fno-exceptions
+CFLAGS += -ffunction-sections
+CFLAGS += -funwind-tables
+CFLAGS += -fstack-protector
+CFLAGS += -fno-short-enums
+CFLAGS += -fmessage-length=0
+CFLAGS += $(INV_INCLUDES)
+CFLAGS += $(INV_DEFINES)
+
+LLINK  = -lc 
+LLINK += -lm 
+LLINK += -lutils 
+LLINK += -lcutils 
+LLINK += -lgcc 
+LLINK += -ldl
+
+LFLAGS += $(CMDLINE_LFLAGS)
+LFLAGS += -shared 
+LFLAGS += -Wl,-soname,$(LIBRARY)
+LFLAGS += -Wl,-shared,-Bsymbolic 
+LFLAGS += $(ANDROID_LINK)
+LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+####################################################################################################
+## sources
+
+INV_LIBS = $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
+
+#INV_SOURCES, VPATH provided by Makefile.filelist
+include ../filelist.mk
+
+INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
+INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all mpl clean cleanall
+
+all: mpl
+
+mpl: $(LIBRARY) $(MK_NAME)
+
+$(LIBRARY) : $(OBJFOLDER) $(INV_OBJS_DST) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(INV_OBJS_DST)\n")
+	$(LINK) $(LFLAGS) -o $(LIBRARY) $(INV_OBJS_DST) $(LLINK) $(INV_LIBS) $(LLINK)
+	$(STRIP) -g $(LIBRARY)
+
+$(OBJFOLDER) :
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(CFLAGS) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(LIBRARY) $(OBJFOLDER)
+
diff --git a/6515/libsensors_iio/software/core/mpl/build/filelist.mk b/6515/libsensors_iio/software/core/mpl/build/filelist.mk
new file mode 100644
index 0000000..34ae4d1
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/build/filelist.mk
@@ -0,0 +1,41 @@
+#### filelist.mk for mpl ####
+
+# headers for sources
+HEADERS := $(MPL_DIR)/fast_no_motion.h
+HEADERS += $(MPL_DIR)/fusion_9axis.h
+HEADERS += $(MPL_DIR)/motion_no_motion.h
+HEADERS += $(MPL_DIR)/no_gyro_fusion.h
+HEADERS += $(MPL_DIR)/quaternion_supervisor.h
+HEADERS += $(MPL_DIR)/gyro_tc.h
+HEADERS += $(MPL_DIR)/authenticate.h
+HEADERS += $(MPL_DIR)/accel_auto_cal.h
+HEADERS += $(MPL_DIR)/accel_auto_cal_protected.h
+HEADERS += $(MPL_DIR)/compass_vec_cal.h
+HEADERS += $(MPL_DIR)/compass_vec_cal_protected.h
+HEADERS += $(MPL_DIR)/mag_disturb.h
+HEADERS += $(MPL_DIR)/mag_disturb_protected.h
+HEADERS += $(MPL_DIR)/compass_bias_w_gyro.h
+HEADERS += $(MPL_DIR)/heading_from_gyro.h
+HEADERS += $(MPL_DIR)/compass_fit.h
+HEADERS += $(MPL_DIR)/quat_accuracy_monitor.h
+
+# sources
+SOURCES := $(MPL_DIR)/fast_no_motion.c
+SOURCES += $(MPL_DIR)/fusion_9axis.c
+SOURCES += $(MPL_DIR)/motion_no_motion.c
+SOURCES += $(MPL_DIR)/no_gyro_fusion.c
+SOURCES += $(MPL_DIR)/quaternion_supervisor.c
+SOURCES += $(MPL_DIR)/gyro_tc.c
+SOURCES += $(MPL_DIR)/authenticate.c
+SOURCES += $(MPL_DIR)/accel_auto_cal.c
+SOURCES += $(MPL_DIR)/compass_vec_cal.c
+SOURCES += $(MPL_DIR)/mag_disturb.c
+SOURCES += $(MPL_DIR)/compass_bias_w_gyro.c
+SOURCES += $(MPL_DIR)/heading_from_gyro.c
+SOURCES += $(MPL_DIR)/compass_fit.c
+SOURCES += $(MPL_DIR)/quat_accuracy_monitor.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH = $(MPL_DIR)
+
diff --git a/6515/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h b/6515/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h
new file mode 100644
index 0000000..7669b31
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h
@@ -0,0 +1,31 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_COMPASSBIASWGYRO_H__
+#define MLDMP_COMPASSBIASWGYRO_H__
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    inv_error_t inv_enable_compass_bias_w_gyro();
+    inv_error_t inv_disable_compass_bias_w_gyro();
+    void inv_init_compass_bias_w_gyro();
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_COMPASSBIASWGYRO_H__
diff --git a/6515/libsensors_iio/software/core/mpl/compass_fit.h b/6515/libsensors_iio/software/core/mpl/compass_fit.h
new file mode 100644
index 0000000..be3cce8
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/compass_fit.h
@@ -0,0 +1,28 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+#ifndef INV_MLDMP_COMPASSFIT_H__
+#define INV_MLDMP_COMPASSFIT_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void inv_init_compass_fit(void);
+inv_error_t inv_enable_compass_fit(void);
+inv_error_t inv_disable_compass_fit(void);
+inv_error_t inv_start_compass_fit(void);
+inv_error_t inv_stop_compass_fit(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // INV_MLDMP_COMPASSFIT_H__
diff --git a/6515/libsensors_iio/software/core/mpl/compass_vec_cal.h b/6515/libsensors_iio/software/core/mpl/compass_vec_cal.h
new file mode 100644
index 0000000..003a312
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/compass_vec_cal.h
@@ -0,0 +1,36 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+#ifndef COMPASS_ONLY_CAL_H__
+#define COMPASS_ONLY_CAL_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_enable_vector_compass_cal();
+inv_error_t inv_disable_vector_compass_cal();
+inv_error_t inv_start_vector_compass_cal(void);
+inv_error_t inv_stop_vector_compass_cal(void);
+void inv_vector_compass_cal_sensitivity(float sens);
+inv_error_t inv_init_vector_compass_cal();
+
+inv_error_t inv_mcb_switch(int i);
+#ifdef __cplusplus
+}
+#endif
+
+#endif // COMPASS_ONLY_CAL_H__
+
diff --git a/6515/libsensors_iio/software/core/mpl/fast_no_motion.h b/6515/libsensors_iio/software/core/mpl/fast_no_motion.h
new file mode 100644
index 0000000..cb05d3b
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/fast_no_motion.h
@@ -0,0 +1,52 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_FAST_NO_MOTION_H__
+#define MLDMP_FAST_NO_MOTION_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    inv_error_t inv_enable_fast_nomot(void);
+    inv_error_t inv_disable_fast_nomot(void);
+    inv_error_t inv_start_fast_nomot(void);
+    inv_error_t inv_stop_fast_nomot(void);
+    inv_error_t inv_init_fast_nomot(void);
+    void inv_set_default_number_of_samples(int count);
+    inv_error_t inv_fast_nomot_is_enabled(unsigned char *is_enabled);
+    inv_error_t inv_update_fast_nomot(long *gyro);
+
+    void inv_get_fast_nomot_accel_param(long *cntr, long long *param);
+    void inv_get_fast_nomot_compass_param(long *cntr, long long *param);
+    void inv_set_fast_nomot_accel_threshold(long long thresh);
+    void inv_set_fast_nomot_compass_threshold(long long thresh);
+    void int_set_fast_nomot_gyro_threshold(long long thresh);
+
+    inv_time_t fast_nomot_get_gyro_bias_update_time(void);
+    void fast_nomot_set_gyro_bias_update_time(struct inv_sensor_cal_t *sensor_cal);
+
+    int fast_nomot_get_gyro_calibration_confidence_level(struct inv_sensor_cal_t *sensor_cal);
+    void fast_nomot_set_gyro_calibration_confidence_level_time_threshold(float time_seconds);
+
+    void inv_fnm_debug_print(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_FAST_NO_MOTION_H__
+
diff --git a/6515/libsensors_iio/software/core/mpl/fusion_9axis.h b/6515/libsensors_iio/software/core/mpl/fusion_9axis.h
new file mode 100644
index 0000000..3a18519
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/fusion_9axis.h
@@ -0,0 +1,39 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_FUSION9AXIS_H__
+#define MLDMP_FUSION9AXIS_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    void inv_init_9x_fusion(void);
+    inv_error_t inv_9x_fusion_state_change(unsigned char newState);
+    inv_error_t inv_enable_9x_sensor_fusion(void);
+    inv_error_t inv_disable_9x_sensor_fusion(void);
+    inv_error_t inv_start_9x_sensor_fusion(void);
+    inv_error_t inv_stop_9x_sensor_fusion(void);
+    inv_error_t inv_9x_fusion_set_mag_fb(double fb);
+    inv_error_t inv_9x_fusion_enable_jitter_reduction(int en);
+    int inv_verify_9x_fusion_data(float *data);
+
+    float inv_9x_sensor_fusion_get_correction_angle(void);
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_FUSION9AXIS_H__
diff --git a/6515/libsensors_iio/software/core/mpl/gyro_tc.h b/6515/libsensors_iio/software/core/mpl/gyro_tc.h
new file mode 100644
index 0000000..3347a14
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/gyro_tc.h
@@ -0,0 +1,43 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef _GYRO_TC_H
+#define _GYRO_TC_H_
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_enable_gyro_tc(void);
+inv_error_t inv_disable_gyro_tc(void);
+inv_error_t inv_start_gyro_tc(void);
+inv_error_t inv_stop_gyro_tc(void);
+
+inv_error_t inv_get_gyro_ts(long *data);
+inv_error_t inv_set_gyro_ts(long *data);
+
+inv_error_t inv_init_gyro_ts(void);
+
+inv_error_t inv_set_gtc_max_temp(long data);
+inv_error_t inv_set_gtc_min_temp(long data);
+
+inv_error_t inv_print_gtc_data(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* _GYRO_TC_H */
+
diff --git a/6515/libsensors_iio/software/core/mpl/heading_from_gyro.h b/6515/libsensors_iio/software/core/mpl/heading_from_gyro.h
new file mode 100644
index 0000000..09ecc42
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/heading_from_gyro.h
@@ -0,0 +1,33 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef _HEADING_FROM_GYRO_H_
+#define _HEADING_FROM_GYRO_H_
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    inv_error_t inv_enable_heading_from_gyro(void);
+    inv_error_t inv_disable_heading_from_gyro(void);
+    void inv_init_heading_from_gyro(void);
+    inv_error_t inv_start_heading_from_gyro(void);
+    inv_error_t inv_stop_heading_from_gyro(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif  /* _HEADING_FROM_GYRO_H_ */
diff --git a/6515/libsensors_iio/software/core/mpl/inv_math.h b/6515/libsensors_iio/software/core/mpl/inv_math.h
new file mode 100644
index 0000000..175511a
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/inv_math.h
@@ -0,0 +1,8 @@
+/* math.h has many functions and defines that are not consistent across 
+* platforms. This address that */
+
+#ifdef _WINDOWS
+#define _USE_MATH_DEFINES
+#endif
+
+#include <math.h>
diff --git a/6515/libsensors_iio/software/core/mpl/invensense_adv.h b/6515/libsensors_iio/software/core/mpl/invensense_adv.h
new file mode 100644
index 0000000..12932c9
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/invensense_adv.h
@@ -0,0 +1,31 @@
+/*
+ $License:
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+/*
+    Main header file for Invensense's Advanced library.
+*/
+
+#include "accel_auto_cal.h"
+#include "compass_bias_w_gyro.h"
+#include "compass_fit.h"
+#include "compass_vec_cal.h"
+#include "fast_no_motion.h"
+#include "fusion_9axis.h"
+#include "gyro_tc.h"
+#include "heading_from_gyro.h"
+#include "mag_disturb.h"
+#include "motion_no_motion.h"
+#include "no_gyro_fusion.h"
+#include "quaternion_supervisor.h"
+#include "mag_disturb.h"
+#include "quat_accuracy_monitor.h"
+#include "shake.h"
diff --git a/6515/libsensors_iio/software/core/mpl/mag_disturb.h b/6515/libsensors_iio/software/core/mpl/mag_disturb.h
new file mode 100644
index 0000000..33db531
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/mag_disturb.h
@@ -0,0 +1,113 @@
+/*

+ $License:

+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.

+    See included License.txt for License information.

+ $

+ */

+

+#ifndef MLDMP_MAGDISTURB_H__

+#define MLDMP_MAGDISTURB_H__

+

+#include "mltypes.h"

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+

+   // #define WIN_8

+    //#define RECORD_DATA_4_ANALYSIS

+

+    int inv_check_magnetic_disturbance(unsigned long delta_time, const long *quat,

+        const long *compass, const long *gravity);

+

+    void inv_track_world_yaw_angle_angle(int mode, float currdip);

+

+    inv_error_t inv_enable_magnetic_disturbance(void);

+    inv_error_t inv_disable_magnetic_disturbance(void);

+    int inv_get_magnetic_disturbance_state();

+

+    inv_error_t inv_set_magnetic_disturbance(int time_ms);

+    inv_error_t inv_init_magnetic_disturbance(void);

+

+    void inv_enable_magnetic_disturbance_logging(void);

+    void inv_disable_magnetic_disturbance_logging(void);

+

+    float Mag3ofNormalizedLong(const long *x);

+    float Mag2ofNormalizedLong(const long *x);

+    float Mag2ofNormalizedFloat(const float *x);

+    

+    int inv_mag_disturb_get_detect_status_3D(void);

+    void inv_mag_disturb_set_detect_status_3D(int status);

+

+    int inv_mag_disturb_get_drop_heading_accuracy_status(void);

+    void inv_mag_disturb_set_drop_heading_accuracy_status(int status);

+

+    int inv_mag_disturb_get_detect_weak_status_3D(void);

+    void inv_mag_disturb_set_detect_weak_status_3D(int status);

+

+    int inv_mag_disturb_get_detect_world_yaw_angle_status(void);

+    void inv_mag_disturb_set_detect_world_yaw_angle_status(int status);

+

+    int inv_mag_disturb_get_detect_world_yaw_angle_confirm_status(void);

+    void inv_mag_disturb_set_detect_world_yaw_angle_confirm_status(int status);

+

+    float inv_mag_disturb_get_vector_radius_3D(void);

+    void inv_mag_disturb_set_vector_radius_3D(float radius);

+

+    void inv_mag_disturb_world_yaw_angle_init(void);

+    void inv_mag_disturb_world_yaw_angle_process(struct inv_sensor_cal_t *obj);

+    //enum mag_distub_state_e inv_mag_disturb_get_mar_world_yaw_angle_state(void);

+    

+    char inv_mag_disturb_get_mar_world_yaw_angle_detection_status(void);

+

+    float inv_mag_disturb_world_yaw_angle_distortion_from_gyro_bias(struct inv_sensor_cal_t *obj);

+    int inv_mag_disturb_get_dip_compassNgravity(struct inv_sensor_cal_t *data);

+

+    float inv_mag_disturb_9x_quat_confidence_interval(struct inv_sensor_cal_t *obj);

+    float inv_mag_disturb_geo_mag_confidence_interval(struct inv_sensor_cal_t *obj);

+

+    float inv_mag_disturb_world_yaw_angle_distortion_from_accel_compass_bias(float accel_bias_error, float compass_bias_error);

+    float inv_mag_disturb_world_yaw_angle_distortion_from_accel_compass_only(float accel_bias_error, float compass_bias_error);

+

+    void inv_mag_disturb_all_confidence_interval_init(void);

+    void inv_mag_disturb_recover_compass_accuracy_to_3(void);

+

+#ifdef RECORD_DATA_4_ANALYSIS

+    void debug_file_init(void);

+    void debug_file_record(struct inv_sensor_cal_t *obj);

+    void debug_file_exit(void);

+#endif

+

+    /************************/

+    /* external API         */

+    /************************/

+    float inv_mag_disturb_get_magnitude_threshold(void);

+    void inv_mag_disturb_set_magnitude_threshold(float radius);

+

+    float inv_mag_disturb_get_magnitude_recover_normal_threshold(void);

+    void inv_mag_disturb_set_magnitude_recover_normal_threshold(float radius);

+

+    /* These API get/set the yaw angle based magnetic anomaly detection*/

+    float inv_mag_disturb_get_yaw_angle_weak_threshold(void);

+    void inv_mag_disturb_set_yaw_angle_weak_threshold(float angle);

+

+    /* These API get/set the yaw angle based strong magnetic anomaly detection*/

+    /* The strong magnetic anomaly detection will drop heading accuracy if gyro is not trustable */

+    float inv_mag_disturb_get_yaw_angle_strong_threshold(void);

+    void inv_mag_disturb_set_yaw_angle_strong_threshold(float angle);

+

+    float inv_mag_disturb_get_time_threshold_detect(void);

+    void inv_mag_disturb_set_time_threshold_detect(float time_seconds);

+

+    float inv_mag_disturb_get_local_field_radius(void);

+    void inv_mag_disturb_set_local_field_radius(float radius);

+

+    float inv_mag_disturb_get_local_field_dip(void);

+    void inv_mag_disturb_set_local_field_dip(float dip);

+

+#ifdef __cplusplus

+}

+#endif

+

+

+#endif // MLDMP_MAGDISTURB_H__

diff --git a/6515/libsensors_iio/software/core/mpl/motion_no_motion.h b/6515/libsensors_iio/software/core/mpl/motion_no_motion.h
new file mode 100644
index 0000000..a43392c
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/motion_no_motion.h
@@ -0,0 +1,31 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#ifndef INV_MOTION_NO_MOTION_H__
+#define INV_MOTION_NO_MOTION_H__
+
+#include "mltypes.h"
+#include "invensense.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_enable_motion_no_motion(void);
+inv_error_t inv_disable_motion_no_motion(void);
+inv_error_t inv_init_motion_no_motion(void);
+inv_error_t inv_start_motion_no_motion(void);
+inv_error_t inv_stop_motion_no_motion(void);
+
+inv_error_t inv_set_no_motion_time(long time_ms);
+inv_time_t motion_nomot_get_gyro_bias_update_time(void);
+void motion_nomot_set_gyro_bias_update_time(struct inv_sensor_cal_t *sensor_cal);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_MOTION_NO_MOTION_H__
diff --git a/6515/libsensors_iio/software/core/mpl/no_gyro_fusion.h b/6515/libsensors_iio/software/core/mpl/no_gyro_fusion.h
new file mode 100644
index 0000000..b1c23e0
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/no_gyro_fusion.h
@@ -0,0 +1,35 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_NOGYROFUSION_H__
+#define MLDMP_NOGYROFUSION_H__
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    inv_error_t inv_enable_no_gyro_fusion(void);
+    inv_error_t inv_disable_no_gyro_fusion(void);
+    inv_error_t inv_start_no_gyro_fusion(void);
+    inv_error_t inv_start_no_gyro_fusion(void);
+    inv_error_t inv_init_no_gyro_fusion(void);
+    int inv_verify_no_gyro_fusion_data(float *data);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_NOGYROFUSION_H__
diff --git a/6515/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h b/6515/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
new file mode 100644
index 0000000..5ee0573
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
@@ -0,0 +1,71 @@
+/*
+	quat_accuracy_monitor.h
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+#ifndef QUAT_ACCURARCY_MONITOR_H__
+#define QUAT_ACCURARCY_MONITOR_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+enum accuracy_signal_type_e {
+    TYPE_NAV_QUAT,
+    TYPE_GAM_QUAT,
+    TYPE_NAV_QUAT_ADVANCED,
+    TYPE_GAM_QUAT_ADVANCED,
+    TYPE_NAV_QUAT_BASIC,
+    TYPE_GAM_QUAT_BASIC,
+    TYPE_MAG,
+    TYPE_GYRO,
+    TYPE_ACCEL,
+};
+
+inv_error_t inv_init_quat_accuracy_monitor(void);
+
+void set_accuracy_threshold(enum accuracy_signal_type_e type, double threshold);
+double get_accuracy_threshold(enum accuracy_signal_type_e type);
+void set_accuracy_weight(enum accuracy_signal_type_e type, int weight);
+int get_accuracy_weight(enum accuracy_signal_type_e type);
+
+int8_t get_accuracy_accuracy(enum accuracy_signal_type_e type);
+
+void inv_reset_quat_accuracy(void);
+double get_6axis_correction_term(void);
+double get_9axis_correction_term(void);
+int get_9axis_accuracy_state();
+
+void set_6axis_error_average(double value);
+double get_6axis_error_bound(void);
+double get_compass_correction(void);
+double get_9axis_error_bound(void);
+
+float get_confidence_interval(void);
+void set_compass_uncertainty(float value);
+
+inv_error_t inv_enable_quat_accuracy_monitor(void);
+inv_error_t inv_disable_quat_accuracy_monitor(void);
+inv_error_t inv_start_quat_accuracy_monitor(void);
+inv_error_t inv_stop_quat_accuracy_monitor(void);
+
+double get_compassNgravity(void);
+double get_init_compassNgravity(void);
+
+float inv_heading_accuracy_check(float orient[3], float *heading, int8_t *accuracy);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // QUAT_ACCURARCY_MONITOR_H__
diff --git a/6515/libsensors_iio/software/core/mpl/quaternion_supervisor.h b/6515/libsensors_iio/software/core/mpl/quaternion_supervisor.h
new file mode 100644
index 0000000..a2d445b
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/quaternion_supervisor.h
@@ -0,0 +1,30 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#ifndef INV_QUATERNION_SUPERVISOR_H__
+#define INV_QUATERNION_SUPERVISOR_H__
+
+#include "mltypes.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define ACCELERATION_SQUARE_1P5G   9663676416LL
+#define ACCELERATION_SQUARE_1P2G   6184752906LL
+inv_error_t inv_enable_quaternion(void);
+inv_error_t inv_disable_quaternion(void);
+inv_error_t inv_init_quaternion(void);
+inv_error_t inv_start_quaternion(void);
+void inv_set_quaternion(long *quat);
+int inv_verify_6x_fusion_data(float *data);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_QUATERNION_SUPERVISOR_H__
diff --git a/6515/libsensors_iio/software/core/mpl/shake.h b/6515/libsensors_iio/software/core/mpl/shake.h
new file mode 100644
index 0000000..8775a4c
--- /dev/null
+++ b/6515/libsensors_iio/software/core/mpl/shake.h
@@ -0,0 +1,94 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#ifndef INV_SHAKE_H__
+#define INV_SHAKE_H__
+
+#include "mltypes.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+	/* ------------ */
+    /* - Defines. - */
+    /* ------------ */
+
+    #define STATE_ZERO                             0
+    #define STATE_INIT_1                           1
+    #define STATE_INIT_2                           2
+    #define STATE_DETECT                           3
+
+	struct t_shake_config_params {
+		long shake_time_min_ms;
+		long shake_time_max_ms;
+		long shake_time_min;
+        long shake_time_max;
+		unsigned char shake_time_set;
+		long shake_time_saved;
+        float shake_deriv_thr;
+        int zero_cross_thr;
+        float accel_delta_min;
+        float accel_delta_max;
+		unsigned char interp_enable;
+	};
+
+	struct t_shake_state_params {
+		unsigned char state;
+        float accel_peak_high;
+        float accel_peak_low;
+        float accel_range;
+        int num_zero_cross;
+        short curr_shake_time;
+		int deriv_major_change;
+		int deriv_major_sign;
+        float accel_buffer[200];
+        float delta_buffer[200];
+	};
+
+	struct t_shake_data_params {
+		float accel_prev;
+		float accel_curr;
+		float delta_prev;
+		float delta_curr;
+		float delta_prev_buffer;
+	};
+
+	struct t_shake_results {
+		//unsigned char shake_int;
+		int shake_number;
+	};
+
+	struct t_shake_cb {
+       void (*shake_callback)(struct t_shake_results *shake_results);            
+    };
+
+
+    /* --------------------- */
+    /* - Function p-types. - */
+    /* --------------------- */
+	inv_error_t inv_enable_shake(void);
+    inv_error_t inv_disable_shake(void);
+    inv_error_t inv_init_shake(void);
+    inv_error_t inv_start_shake(void);
+	int inv_set_shake_cb(void (*callback)(struct t_shake_results *shake_results));
+	void inv_config_shake_time_params(long sample_time_ms);
+	void inv_set_shake_accel_delta_min(float accel_g);
+	void inv_set_shake_accel_delta_max(float accel_g);
+	void inv_set_shake_zero_cross_thresh(int num_zero_cross);
+	void inv_set_shake_deriv_thresh(float shake_deriv_thresh);
+	void inv_set_shake_time_min_ms(long time_ms);
+	void inv_set_shake_time_max_ms(long time_ms);
+	void inv_enable_shake_data_interpolation(unsigned char en);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_SHAKE__
\ No newline at end of file
diff --git a/6515/libsensors_iio/software/simple_apps/common/console_helper.c b/6515/libsensors_iio/software/simple_apps/common/console_helper.c
new file mode 100644
index 0000000..2888627
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/common/console_helper.c
@@ -0,0 +1,54 @@
+/*
+ $License:
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id:$
+ *
+ *****************************************************************************/
+
+#include <stdio.h>
+#ifdef _WIN32
+#include <windows.h>
+#include <conio.h>
+#endif
+#ifdef LINUX
+#include <sys/select.h>
+#endif
+#include <time.h>
+#include <string.h>
+
+int ConsoleKbhit(void)
+{
+#ifdef _WIN32
+    return _kbhit();
+#else
+    struct timeval tv;
+    fd_set read_fd;
+
+    tv.tv_sec=0;
+    tv.tv_usec=0;
+    FD_ZERO(&read_fd);
+    FD_SET(0, &read_fd);
+
+    if(select(1, &read_fd, NULL, NULL, &tv) == -1)
+        return 0;
+
+    if(FD_ISSET(0, &read_fd))
+        return 1;
+
+    return 0;
+#endif
+}
+
+char ConsoleGetChar(void)
+{
+#ifdef _WIN32
+    return _getch();
+#else
+    return getchar();
+#endif
+}
diff --git a/6515/libsensors_iio/software/simple_apps/common/console_helper.h b/6515/libsensors_iio/software/simple_apps/common/console_helper.h
new file mode 100644
index 0000000..5251d1c
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/common/console_helper.h
@@ -0,0 +1,33 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id:$
+ *
+ *****************************************************************************/
+
+#ifndef _CONSOLE_HELPER_H_
+#define _CONSOLE_HELPER_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+//#include "mltypes.h"
+
+/*
+    Prototypes
+*/
+
+char ConsoleGetChar(void);
+int ConsoleKbhit(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // _CONSOLE_HELPER_H_
diff --git a/6515/libsensors_iio/software/simple_apps/common/mlerrorcode.c b/6515/libsensors_iio/software/simple_apps/common/mlerrorcode.c
new file mode 100644
index 0000000..25b0df6
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/common/mlerrorcode.c
@@ -0,0 +1,96 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id: mlerrorcode.c 5629 2011-06-11 03:13:08Z mcaramello $
+ *
+ *****************************************************************************/
+
+#include <stdio.h>
+#include <string.h>
+
+#include "mltypes.h"
+#include "mlerrorcode.h"
+
+#define ERROR_CODE_CASE(CODE)   \
+    case CODE:                  \
+        return #CODE            \
+
+/**
+ *  @brief  return a string containing the label assigned to the error code.
+ *  
+ *  @param  errorcode   
+ *              The errorcode value of which the label has to be returned.
+ *
+ *  @return A string containing the error code label.
+ */
+char* MLErrorCode(inv_error_t errorcode) 
+{
+    switch(errorcode) {
+        ERROR_CODE_CASE(INV_SUCCESS);
+        ERROR_CODE_CASE(INV_ERROR);
+        ERROR_CODE_CASE(INV_ERROR_INVALID_PARAMETER);
+        ERROR_CODE_CASE(INV_ERROR_FEATURE_NOT_ENABLED);
+        ERROR_CODE_CASE(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        ERROR_CODE_CASE(INV_ERROR_DMP_NOT_STARTED);
+        ERROR_CODE_CASE(INV_ERROR_DMP_STARTED);
+        ERROR_CODE_CASE(INV_ERROR_NOT_OPENED);
+        ERROR_CODE_CASE(INV_ERROR_OPENED);
+        ERROR_CODE_CASE(INV_ERROR_INVALID_MODULE);
+        ERROR_CODE_CASE(INV_ERROR_MEMORY_EXAUSTED);
+        ERROR_CODE_CASE(INV_ERROR_DIVIDE_BY_ZERO);
+        ERROR_CODE_CASE(INV_ERROR_ASSERTION_FAILURE);
+        ERROR_CODE_CASE(INV_ERROR_FILE_OPEN);
+        ERROR_CODE_CASE(INV_ERROR_FILE_READ);
+        ERROR_CODE_CASE(INV_ERROR_FILE_WRITE);
+
+        ERROR_CODE_CASE(INV_ERROR_SERIAL_CLOSED);
+        ERROR_CODE_CASE(INV_ERROR_SERIAL_OPEN_ERROR);
+        ERROR_CODE_CASE(INV_ERROR_SERIAL_READ);
+        ERROR_CODE_CASE(INV_ERROR_SERIAL_WRITE);
+        ERROR_CODE_CASE(INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED);
+
+        ERROR_CODE_CASE(INV_ERROR_SM_TRANSITION);
+        ERROR_CODE_CASE(INV_ERROR_SM_IMPROPER_STATE);
+
+        ERROR_CODE_CASE(INV_ERROR_FIFO_OVERFLOW);
+        ERROR_CODE_CASE(INV_ERROR_FIFO_FOOTER);
+        ERROR_CODE_CASE(INV_ERROR_FIFO_READ_COUNT);
+        ERROR_CODE_CASE(INV_ERROR_FIFO_READ_DATA);
+        ERROR_CODE_CASE(INV_ERROR_MEMORY_SET);
+
+        ERROR_CODE_CASE(INV_ERROR_LOG_MEMORY_ERROR);
+        ERROR_CODE_CASE(INV_ERROR_LOG_OUTPUT_ERROR);
+
+        ERROR_CODE_CASE(INV_ERROR_OS_BAD_PTR);
+        ERROR_CODE_CASE(INV_ERROR_OS_BAD_HANDLE);
+        ERROR_CODE_CASE(INV_ERROR_OS_CREATE_FAILED);
+        ERROR_CODE_CASE(INV_ERROR_OS_LOCK_FAILED);
+
+        ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_OVERFLOW);
+        ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_UNDERFLOW);
+        ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_NOT_READY);
+        ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_ERROR);
+
+        ERROR_CODE_CASE(INV_ERROR_CALIBRATION_LOAD);
+        ERROR_CODE_CASE(INV_ERROR_CALIBRATION_STORE);
+        ERROR_CODE_CASE(INV_ERROR_CALIBRATION_LEN);
+        ERROR_CODE_CASE(INV_ERROR_CALIBRATION_CHECKSUM);
+
+        default:
+        {
+            #define UNKNOWN_ERROR_CODE 1234
+            return ERROR_NAME(UNKNOWN_ERROR_CODE);
+            break;
+        }
+
+    }
+}
+
+/**
+ *  @}
+ */
diff --git a/6515/libsensors_iio/software/simple_apps/common/mlerrorcode.h b/6515/libsensors_iio/software/simple_apps/common/mlerrorcode.h
new file mode 100644
index 0000000..9a35792
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/common/mlerrorcode.h
@@ -0,0 +1,86 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/*******************************************************************************
+ *
+ * $Id: mltypes.h 3680 2010-09-04 03:13:32Z mcaramello $
+ *
+ *******************************************************************************/
+
+#ifndef _MLERRORCODE_H_
+#define _MLERRORCODE_H_
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+    Defines 
+*/
+#define CALL_N_CHECK(f) {                                                   \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                             \
+        MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+    }                                                                       \
+}
+
+#define CALL_CHECK_N_RETURN_ERROR(f) {                                      \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                             \
+        MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        return r35uLt;                                                      \
+    }                                                                       \
+}
+
+// for functions returning void
+#define CALL_CHECK_N_RETURN(f) do {                                         \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                             \
+        MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        return;                                                             \
+    }                                                                       \
+    } while(0)
+
+#define CALL_CHECK_N_EXIT(f) {                                              \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                             \
+        MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        exit (r35uLt);                                                      \
+    }                                                                       \
+}
+
+    
+#define CALL_CHECK_N_CALLBACK(f, cb) {                                      \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                             \
+        MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        cb;                                                                 \
+    }                                                                       \
+}
+
+#define CALL_CHECK_N_GOTO(f, label) {                                       \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                             \
+        MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        goto label;                                                         \
+    }                                                                       \
+}
+
+char* MLErrorCode(inv_error_t errorcode);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
diff --git a/6515/libsensors_iio/software/simple_apps/common/testsupport.h b/6515/libsensors_iio/software/simple_apps/common/testsupport.h
new file mode 100644
index 0000000..0cab781
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/common/testsupport.h
@@ -0,0 +1,57 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id: testsupport.h 5629 2011-06-11 03:13:08Z mcaramello $
+ *
+ ******************************************************************************/
+ 
+#ifndef _TESTSUPPORT_H_
+#define _TESTSUPPORT_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*---------------------------
+    Includes
+---------------------------*/
+
+#include "mltypes.h"
+#include "mlerrorcode.h"
+
+#include "mlsl.h"
+#include "log.h"
+
+/*---------------------------
+    Defines
+---------------------------*/
+
+/*---------------------------
+    p-Types
+---------------------------*/
+#ifdef TESTING_SUPPORT
+    void            SetHandle           (void *sl_handle);
+    void            CommandPrompt       (void *sl_handle);
+    void            RegisterMap         (void *sl_handle);
+    void            DataLogger          (const unsigned long flag);
+    void            DataLoggerSelector  (const unsigned long flag);
+    void            DataLoggerCb        (void);
+    unsigned short  KeyboardHandler     (unsigned char key);
+    char*           CompassStateName    (char* out, int state);
+#else
+#define DataLoggerSelector(x)           //
+#define DataLogger(x)                   //
+#define DataLoggerCb                    NULL
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // _TESTSUPPORT_H_
+
diff --git a/6515/libsensors_iio/software/simple_apps/devnode_parser/build/android/inv_devnode_parser-shared b/6515/libsensors_iio/software/simple_apps/devnode_parser/build/android/inv_devnode_parser-shared
new file mode 100755
index 0000000..092d7c4
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/devnode_parser/build/android/inv_devnode_parser-shared
diff --git a/6515/libsensors_iio/software/simple_apps/devnode_parser/build/android/shared.mk b/6515/libsensors_iio/software/simple_apps/devnode_parser/build/android/shared.mk
new file mode 100644
index 0000000..38d1fb4
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/devnode_parser/build/android/shared.mk
@@ -0,0 +1,92 @@
+EXEC = inv_devnode_parser$(SHARED_APP_SUFFIX)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
+COMP  ?= $(CROSS)gcc
+LINK  ?= $(CROSS)gcc
+
+OBJFOLDER = $(CURDIR)/obj
+
+INV_ROOT   = ../../../../..
+APP_DIR    = $(CURDIR)/../..
+MLLITE_DIR = $(INV_ROOT)/software/core/mllite
+
+include $(INV_ROOT)/software/build/android/common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += $(ANDROID_COMPILE)
+CFLAGS += -Wall
+CFLAGS += -fpic
+CFLAGS += -nostdlib
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT
+CFLAGS += -DLINUX
+CFLAGS += -DANDROID
+CFLAGS += -mthumb-interwork
+CFLAGS += -fno-exceptions
+CFLAGS += -ffunction-sections
+CFLAGS += -funwind-tables
+CFLAGS += -fstack-protector
+CFLAGS += -fno-short-enums
+CFLAGS += -fmessage-length=0
+CFLAGS += -I$(MLLITE_DIR)
+CFLAGS += -I$(MPL_DIR)
+CFLAGS += $(INV_INCLUDES)
+CFLAGS += $(INV_DEFINES)
+
+LLINK  = -lc
+LLINK += -lm
+LLINK += -lutils
+LLINK += -lcutils
+LLINK += -lgcc
+LLINK += -ldl
+LLINK += -lstdc++
+LLINK += -llog
+LLINK += -lz
+
+LFLAGS += $(CMDLINE_LFLAGS)
+LFLAGS += $(ANDROID_LINK_EXECUTABLE)
+
+LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+####################################################################################################
+## sources
+
+INV_LIBS = $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
+
+#INV_SOURCES and VPATH provided by Makefile.filelist
+include ../filelist.mk
+
+INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
+INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all clean cleanall install
+
+all: $(EXEC) $(MK_NAME)
+
+$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
+	$(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+
+$(OBJFOLDER) :
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(EXEC) $(OBJFOLDER)
+
+install : $(EXEC)
+	cp -f $(EXEC) $(INSTALL_DIR)
+
+
diff --git a/6515/libsensors_iio/software/simple_apps/devnode_parser/build/filelist.mk b/6515/libsensors_iio/software/simple_apps/devnode_parser/build/filelist.mk
new file mode 100644
index 0000000..fdecbc8
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/devnode_parser/build/filelist.mk
@@ -0,0 +1,11 @@
+#### filelist.mk for inv_devnode_parser ####
+
+# headers
+HEADERS += $(APP_DIR)/iio_utils.h
+
+# sources
+SOURCES := $(APP_DIR)/read_device_node.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux
diff --git a/6515/libsensors_iio/software/simple_apps/devnode_parser/iio_utils.h b/6515/libsensors_iio/software/simple_apps/devnode_parser/iio_utils.h
new file mode 100644
index 0000000..c3d4955
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/devnode_parser/iio_utils.h
@@ -0,0 +1,650 @@
+/* IIO - useful set of util functionality
+ *
+ * Copyright (c) 2008 Jonathan Cameron
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+/* Made up value to limit allocation sizes */
+#include <string.h>
+#include <stdlib.h>
+#include <ctype.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <dirent.h>
+
+#define IIO_MAX_NAME_LENGTH 30
+
+#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements"
+#define FORMAT_TYPE_FILE "%s_type"
+
+const char *iio_dir = "/sys/bus/iio/devices/";
+
+extern int verbose;
+
+/**
+ * iioutils_break_up_name() - extract generic name from full channel name
+ * @full_name: the full channel name
+ * @generic_name: the output generic channel name
+ **/
+static int iioutils_break_up_name(const char *full_name,
+				  char **generic_name)
+{
+	char *current;
+	char *w, *r;
+	char *working;
+	current = strdup(full_name);
+	working = strtok(current, "_\0");
+	w = working;
+	r = working;
+
+	while (*r != '\0') {
+		if (!isdigit(*r)) {
+			*w = *r;
+			w++;
+		}
+		r++;
+	}
+	*w = '\0';
+	*generic_name = strdup(working);
+	free(current);
+
+	return 0;
+}
+
+/**
+ * struct iio_channel_info - information about a given channel
+ * @name: channel name
+ * @generic_name: general name for channel type
+ * @scale: scale factor to be applied for conversion to si units
+ * @offset: offset to be applied for conversion to si units
+ * @index: the channel index in the buffer output
+ * @bytes: number of bytes occupied in buffer output
+ * @mask: a bit mask for the raw output
+ * @is_signed: is the raw value stored signed
+ * @enabled: is this channel enabled
+ **/
+struct iio_channel_info {
+	char *name;
+	char *generic_name;
+	float scale;
+	float offset;
+	unsigned index;
+	unsigned bytes;
+	unsigned bits_used;
+	unsigned shift;
+	uint64_t mask;
+	unsigned be;
+	unsigned is_signed;
+	unsigned enabled;
+	unsigned location;
+};
+
+/**
+ * iioutils_get_type() - find and process _type attribute data
+ * @is_signed: output whether channel is signed
+ * @bytes: output how many bytes the channel storage occupies
+ * @mask: output a bit mask for the raw data
+ * @be: big endian
+ * @device_dir: the iio device directory
+ * @name: the channel name
+ * @generic_name: the channel type name
+ **/
+inline int iioutils_get_type(unsigned *is_signed,
+			     unsigned *bytes,
+			     unsigned *bits_used,
+			     unsigned *shift,
+			     uint64_t *mask,
+			     unsigned *be,
+			     const char *device_dir,
+			     const char *name,
+			     const char *generic_name)
+{
+	FILE *sysfsfp;
+	int ret;
+	DIR *dp;
+	char *scan_el_dir, *builtname, *builtname_generic, *filename = 0;
+	char signchar, endianchar;
+	unsigned padint;
+	const struct dirent *ent;
+
+	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	ret = asprintf(&builtname, FORMAT_TYPE_FILE, name);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_free_scan_el_dir;
+	}
+	ret = asprintf(&builtname_generic, FORMAT_TYPE_FILE, generic_name);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_free_builtname;
+	}
+
+	dp = opendir(scan_el_dir);
+	if (dp == NULL) {
+		ret = -errno;
+		goto error_free_builtname_generic;
+	}
+	while (ent = readdir(dp), ent != NULL)
+		/*
+		 * Do we allow devices to override a generic name with
+		 * a specific one?
+		 */
+		if ((strcmp(builtname, ent->d_name) == 0) ||
+		    (strcmp(builtname_generic, ent->d_name) == 0)) {
+			ret = asprintf(&filename,
+				       "%s/%s", scan_el_dir, ent->d_name);
+			if (ret < 0) {
+				ret = -ENOMEM;
+				goto error_closedir;
+			}
+			sysfsfp = fopen(filename, "r");
+			if (sysfsfp == NULL) {
+				printf("failed to open %s\n", filename);
+				ret = -errno;
+				goto error_free_filename;
+			}
+
+			ret = fscanf(sysfsfp,
+				     "%ce:%c%u/%u>>%u",
+				     &endianchar,
+				     &signchar,
+				     bits_used,
+				     &padint, shift);
+			if (ret < 0) {
+				printf("failed to pass scan type description\n");
+				return ret;
+			}
+			*be = (endianchar == 'b');
+			*bytes = padint / 8;
+			if (*bits_used == 64)
+				*mask = ~0;
+			else
+				*mask = (1 << *bits_used) - 1;
+			if (signchar == 's')
+				*is_signed = 1;
+			else
+				*is_signed = 0;
+			fclose(sysfsfp);
+			free(filename);
+
+			filename = 0;
+		}
+error_free_filename:
+	if (filename)
+		free(filename);
+error_closedir:
+	closedir(dp);
+error_free_builtname_generic:
+	free(builtname_generic);
+error_free_builtname:
+	free(builtname);
+error_free_scan_el_dir:
+	free(scan_el_dir);
+error_ret:
+	return ret;
+}
+
+inline int iioutils_get_param_float(float *output,
+				    const char *param_name,
+				    const char *device_dir,
+				    const char *name,
+				    const char *generic_name)
+{
+	FILE *sysfsfp;
+	int ret;
+	DIR *dp;
+	char *builtname, *builtname_generic;
+	char *filename = NULL;
+	const struct dirent *ent;
+
+	ret = asprintf(&builtname, "%s_%s", name, param_name);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	ret = asprintf(&builtname_generic,
+		       "%s_%s", generic_name, param_name);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_free_builtname;
+	}
+	dp = opendir(device_dir);
+	if (dp == NULL) {
+		ret = -errno;
+		goto error_free_builtname_generic;
+	}
+	while (ent = readdir(dp), ent != NULL)
+		if ((strcmp(builtname, ent->d_name) == 0) ||
+		    (strcmp(builtname_generic, ent->d_name) == 0)) {
+			ret = asprintf(&filename,
+				       "%s/%s", device_dir, ent->d_name);
+			if (ret < 0) {
+				ret = -ENOMEM;
+				goto error_closedir;
+			}
+			sysfsfp = fopen(filename, "r");
+			if (!sysfsfp) {
+				ret = -errno;
+				goto error_free_filename;
+			}
+			fscanf(sysfsfp, "%f", output);
+			break;
+		}
+error_free_filename:
+	if (filename)
+		free(filename);
+error_closedir:
+	closedir(dp);
+error_free_builtname_generic:
+	free(builtname_generic);
+error_free_builtname:
+	free(builtname);
+error_ret:
+	return ret;
+}
+
+/**
+ * bsort_channel_array_by_index() - reorder so that the array is in index order
+ *
+ **/
+
+inline void bsort_channel_array_by_index(struct iio_channel_info **ci_array,
+					 int cnt)
+{
+	struct iio_channel_info temp;
+	int x, y;
+
+	for (x = 0; x < cnt; x++)
+		for (y = 0; y < (cnt - 1); y++)
+			if ((*ci_array)[y].index > (*ci_array)[y+1].index) {
+				temp = (*ci_array)[y + 1];
+				(*ci_array)[y + 1] = (*ci_array)[y];
+				(*ci_array)[y] = temp;
+			}
+}
+
+/**
+ * build_channel_array() - function to figure out what channels are present
+ * @device_dir: the IIO device directory in sysfs
+ * @
+ **/
+inline int build_channel_array(const char *device_dir,
+			      struct iio_channel_info **ci_array,
+			      int *counter)
+{
+	DIR *dp;
+	FILE *sysfsfp;
+	int count, i;
+	struct iio_channel_info *current;
+	int ret;
+	const struct dirent *ent;
+	char *scan_el_dir;
+	char *filename;
+
+	*counter = 0;
+	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	dp = opendir(scan_el_dir);
+	if (dp == NULL) {
+		ret = -errno;
+		goto error_free_name;
+	}
+	while (ent = readdir(dp), ent != NULL)
+		if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
+			   "_en") == 0) {
+			ret = asprintf(&filename,
+				       "%s/%s", scan_el_dir, ent->d_name);
+			if (ret < 0) {
+				ret = -ENOMEM;
+				goto error_close_dir;
+			}
+			sysfsfp = fopen(filename, "r");
+			if (sysfsfp == NULL) {
+				ret = -errno;
+				free(filename);
+				goto error_close_dir;
+			}
+			fscanf(sysfsfp, "%u", &ret);
+			//printf("%s, %d\n", filename, ret);
+			if (ret == 1)
+				(*counter)++;
+			fclose(sysfsfp);
+			free(filename);
+		}
+	*ci_array = malloc(sizeof(**ci_array) * (*counter));
+	if (*ci_array == NULL) {
+		ret = -ENOMEM;
+		goto error_close_dir;
+	}
+	closedir(dp);
+	dp = opendir(scan_el_dir);
+	//seekdir(dp, 0);
+	count = 0;
+	while (ent = readdir(dp), ent != NULL) {
+		if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
+			   "_en") == 0) {
+			current = &(*ci_array)[count++];
+			ret = asprintf(&filename,
+				       "%s/%s", scan_el_dir, ent->d_name);
+			if (ret < 0) {
+				ret = -ENOMEM;
+				/* decrement count to avoid freeing name */
+				count--;
+				goto error_cleanup_array;
+			}
+			sysfsfp = fopen(filename, "r");
+			if (sysfsfp == NULL) {
+				free(filename);
+				ret = -errno;
+				goto error_cleanup_array;
+			}
+			fscanf(sysfsfp, "%u", &current->enabled);
+			fclose(sysfsfp);
+
+			if (!current->enabled) {
+				free(filename);
+				count--;
+				continue;
+			}
+
+			current->scale = 1.0;
+			current->offset = 0;
+			current->name = strndup(ent->d_name,
+						strlen(ent->d_name) -
+						strlen("_en"));
+			if (current->name == NULL) {
+				free(filename);
+				ret = -ENOMEM;
+				goto error_cleanup_array;
+			}
+			/* Get the generic and specific name elements */
+			ret = iioutils_break_up_name(current->name,
+						     &current->generic_name);
+			if (ret) {
+				free(filename);
+				goto error_cleanup_array;
+			}
+			ret = asprintf(&filename,
+				       "%s/%s_index",
+				       scan_el_dir,
+				       current->name);
+			if (ret < 0) {
+				free(filename);
+				ret = -ENOMEM;
+				goto error_cleanup_array;
+			}
+			sysfsfp = fopen(filename, "r");
+			fscanf(sysfsfp, "%u", &current->index);
+			fclose(sysfsfp);
+			free(filename);
+			/* Find the scale */
+			ret = iioutils_get_param_float(&current->scale,
+						       "scale",
+						       device_dir,
+						       current->name,
+						       current->generic_name);
+			if (ret < 0)
+				goto error_cleanup_array;
+			ret = iioutils_get_param_float(&current->offset,
+						       "offset",
+						       device_dir,
+						       current->name,
+						       current->generic_name);
+			if (ret < 0)
+				goto error_cleanup_array;
+			ret = iioutils_get_type(&current->is_signed,
+						&current->bytes,
+						&current->bits_used,
+						&current->shift,
+						&current->mask,
+						&current->be,
+						device_dir,
+						current->name,
+						current->generic_name);
+		}
+	}
+
+	closedir(dp);
+	/* reorder so that the array is in index order */
+	bsort_channel_array_by_index(ci_array, *counter);
+
+	return 0;
+
+error_cleanup_array:
+	for (i = count - 1;  i >= 0; i--)
+		free((*ci_array)[i].name);
+	free(*ci_array);
+error_close_dir:
+	closedir(dp);
+error_free_name:
+	free(scan_el_dir);
+error_ret:
+	return ret;
+}
+
+inline int _write_sysfs_int(char *filename, char *basedir, int val, int verify)
+{
+    int ret = 0;
+	FILE *sysfsfp;
+	int test;
+	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+	if (temp == NULL)
+		return -ENOMEM;
+        
+	sprintf(temp, "%s/%s", basedir, filename);
+
+    if (verbose)
+        printf("VERB: echo %d > %s\n", val, temp);
+	sysfsfp = fopen(temp, "w");
+	if (sysfsfp == NULL) {
+		printf("failed to open %s\n", temp);
+		ret = -errno;
+		goto error_free;
+	}
+	fprintf(sysfsfp, "%d", val);
+	fclose(sysfsfp);
+
+	if (verify) {
+		sysfsfp = fopen(temp, "r");
+		if (sysfsfp == NULL) {
+			printf("failed to open %s\n", temp);
+			ret = -errno;
+			goto error_free;
+		}
+		fscanf(sysfsfp, "%d", &test);
+        fclose(sysfsfp);
+        if (verbose)
+            printf("VERB: cat %s = %d\n", temp, test);
+		if (test != val) {
+            printf("Possible failure in int write %d to %s\n",
+                   val, temp);
+			ret = -1;
+		}
+	}
+
+error_free:
+	free(temp);
+	return ret;
+}
+
+int write_sysfs_int(char *filename, char *basedir, int val)
+{
+	return _write_sysfs_int(filename, basedir, val, 0);
+}
+
+int write_sysfs_int_and_verify(char *filename, char *basedir, int val)
+{
+	return _write_sysfs_int(filename, basedir, val, 1);
+}
+
+int _write_sysfs_string(char *filename, char *basedir, char *val, int verify)
+{
+	int ret = 0;
+	FILE  *sysfsfp;
+	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+	if (temp == NULL) {
+		printf("Memory allocation failed\n");
+		return -ENOMEM;
+	}
+	sprintf(temp, "%s/%s", basedir, filename);
+
+	sysfsfp = fopen(temp, "w");
+	if (sysfsfp == NULL) {
+		printf("Could not open %s\n", temp);
+		ret = -errno;
+		goto error_free;
+	}
+	fprintf(sysfsfp, "%s", val);
+	fclose(sysfsfp);
+
+	if (verify) {
+		sysfsfp = fopen(temp, "r");
+		if (sysfsfp == NULL) {
+			printf("could not open file to verify\n");
+			ret = -errno;
+			goto error_free;
+		}
+		fscanf(sysfsfp, "%s", temp);
+        fclose(sysfsfp);
+        
+		if (strcmp(temp, val) != 0) {
+			printf("Possible failure in string write of %s "
+                   "Should be %s written to %s\%s\n",
+                   temp, val, basedir, filename);
+			ret = -1;
+		}
+	}
+
+error_free:
+	free(temp);
+
+	return ret;
+}
+
+/**
+ * write_sysfs_string_and_verify() - string write, readback and verify
+ * @filename: name of file to write to
+ * @basedir: the sysfs directory in which the file is to be found
+ * @val: the string to write
+ **/
+int write_sysfs_string_and_verify(char *filename, char *basedir, char *val)
+{
+	return _write_sysfs_string(filename, basedir, val, 1);
+}
+
+int write_sysfs_string(char *filename, char *basedir, char *val)
+{
+	return _write_sysfs_string(filename, basedir, val, 0);
+}
+
+int read_sysfs_posint(char *filename, char *basedir)
+{
+	int ret;
+	FILE  *sysfsfp;
+	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+	if (temp == NULL) {
+		printf("Memory allocation failed");
+		return -ENOMEM;
+	}
+	sprintf(temp, "%s/%s", basedir, filename);
+	sysfsfp = fopen(temp, "r");
+	if (sysfsfp == NULL) {
+		ret = -errno;
+		goto error_free;
+	}
+	fscanf(sysfsfp, "%d\n", &ret);
+	fclose(sysfsfp);
+error_free:
+	free(temp);
+	return ret;
+}
+
+int read_sysfs_float(char *filename, char *basedir, float *val)
+{
+	float ret = 0;
+	FILE  *sysfsfp;
+	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+	if (temp == NULL) {
+		printf("Memory allocation failed");
+		return -ENOMEM;
+	}
+	sprintf(temp, "%s/%s", basedir, filename);
+	sysfsfp = fopen(temp, "r");
+	if (sysfsfp == NULL) {
+		ret = -errno;
+		goto error_free;
+	}
+	fscanf(sysfsfp, "%f\n", val);
+	fclose(sysfsfp);
+
+error_free:
+	free(temp);
+	return ret;
+}
+
+int enable_se(const char *device_dir, struct iio_channel_info **ci_array, 
+              int *counter, char *sensor, int en)
+{
+	DIR *dp;
+	int ret;
+	const struct dirent *ent;
+	char *scan_el_dir;
+    char pattern[50] = "in_";
+
+	*counter = 0;
+	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	dp = opendir(scan_el_dir);
+	if (dp == NULL) {
+		ret = -errno;
+		goto error_free_name;
+	}
+    strcat(pattern, sensor);
+    while (ent = readdir(dp), ent != NULL) {
+        if (strncmp(ent->d_name, pattern, strlen(pattern)) == 0 &&
+            strncmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
+                    "_en", strlen("_en")) == 0) {
+            write_sysfs_int_and_verify((char *)ent->d_name, scan_el_dir, en);
+        }
+		}
+	return 0;
+
+error_ret:
+error_free_name:
+	return -1;
+}
+
+int enable_accel_se(const char *device_dir, 
+                         struct iio_channel_info **ci_array, int *counter, 
+                         int en)
+{
+    return enable_se(device_dir, ci_array, counter, "accel", en);
+}
+
+int enable_anglvel_se(const char *device_dir, 
+                         struct iio_channel_info **ci_array, int *counter, 
+                         int en)
+{
+    return enable_se(device_dir, ci_array, counter, "anglvel", en);
+}
+
+int enable_quaternion_se(const char *device_dir, 
+                         struct iio_channel_info **ci_array, int *counter, 
+                         int en)
+{
+    return enable_se(device_dir, ci_array, counter, "quaternion", en);
+}
+
diff --git a/6515/libsensors_iio/software/simple_apps/devnode_parser/read_device_node.c b/6515/libsensors_iio/software/simple_apps/devnode_parser/read_device_node.c
new file mode 100644
index 0000000..7b73d4a
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/devnode_parser/read_device_node.c
@@ -0,0 +1,284 @@
+/*
+ * Copyright (c) Invensense Inc. 2012
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <unistd.h>
+#include <dirent.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <errno.h>
+#include <sys/stat.h>
+#include <dirent.h>
+#include <linux/types.h>
+#include <string.h>
+#include <poll.h>
+#include <termios.h>
+
+#include "iio_utils.h"
+#include "ml_sysfs_helper.h"
+#include "mlos.h"
+
+#define POLL_TIME (2000) // 2sec
+
+// settings
+int verbose = false;
+
+// paths
+char *dev_dir_name;
+
+/**************************************************
+   This _kbhit() function is courtesy of the web
+***************************************************/
+int _kbhit(void)
+{
+    static const int STDIN = 0;
+    static bool initialized = false;
+
+    if (!initialized) {
+        // Use termios to turn off line buffering
+        struct termios term;
+        tcgetattr(STDIN, &term);
+        term.c_lflag &= ~ICANON;
+        tcsetattr(STDIN, TCSANOW, &term);
+        setbuf(stdin, NULL);
+        initialized = true;
+    }
+
+    int bytesWaiting;
+    ioctl(STDIN, FIONREAD, &bytesWaiting);
+    return bytesWaiting;
+}
+
+void get_sensor_data(char *d, short *sensor)
+{
+    int i;
+    for (i = 0; i < 3; i++)
+        sensor[i] = *(short *)(d + 2 + i * 2);
+}
+
+static int read_data(char *buffer_access)
+{
+#define PRESSURE_HDR             0x8000
+#define ACCEL_HDR                0x4000
+#define GYRO_HDR                 0x2000
+#define COMPASS_HDR              0x1000
+#define LPQUAT_HDR               0x0800
+#define SIXQUAT_HDR              0x0400
+#define PEDQUAT_HDR              0x0200
+#define STEP_DETECTOR_HDR        0x0100
+
+    static int left_over_size = 0;
+    char data[1048], *dptr, tmp[24];
+    short sensor[3];
+    int q[3];
+    int ret, i, ind, fp;
+    int buf_size, read_size;
+    unsigned short hdr;
+    bool done_flag;
+
+    fp = open(buffer_access, O_RDONLY | O_NONBLOCK);
+    if (fp == -1) { /* if it isn't there make the node */
+        printf("Failed to open %s\n", buffer_access);
+        ret = -errno;
+        goto error_read_data;
+    }
+    ind = 0;
+    
+    {
+        struct pollfd pfd = {
+            .fd = fp,
+            .events = POLLIN,
+        };
+        poll(&pfd, 1, -1);
+
+        if (left_over_size > 0)
+            memcpy(data, tmp, left_over_size);
+        dptr = data + left_over_size;
+
+        read_size = read(fp,  dptr, 1024);
+        if (read_size <= 0) {
+            printf("Wrong size=%d\n", read_size);
+            ret = -EINVAL;
+            goto error_read_data;
+        }
+
+        ind = read_size + left_over_size;
+        dptr = data;
+        buf_size = ind - (dptr - data);
+        done_flag = false;
+        while ((buf_size > 0) && (!done_flag)) {
+            hdr = *((short *)(dptr));
+            if (hdr & 1)
+                printf("STEP\n");
+
+            switch (hdr & (~1)) {
+            case PRESSURE_HDR:
+                if (buf_size >= 16) {
+                    get_sensor_data(dptr, sensor);
+                    dptr += 8;
+                    printf("PRESS, %d, %lld\n", (sensor[1] << 16) + (unsigned short)sensor[2], *(long long *)dptr);
+                } else
+                    done_flag = true;
+                break;
+            case ACCEL_HDR:
+                if (buf_size >= 16) {
+                    get_sensor_data(dptr, sensor);
+                    dptr += 8;
+                    printf("ACCEL, %d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr);
+                } else
+                    done_flag = true;
+                break;
+            case GYRO_HDR:
+                if (buf_size >= 16) {
+                    get_sensor_data(dptr, sensor);
+                    dptr += 8;
+                    printf("GYRO, %d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr);
+                } else
+                    done_flag = true;
+                break;
+            case COMPASS_HDR:
+                if (buf_size >= 16) {
+                    get_sensor_data(dptr, sensor);
+                    dptr += 8;
+                    printf("COMPASS, %d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr);
+                } else
+                    done_flag = true;
+                break;
+            case PEDQUAT_HDR:
+                if (buf_size >= 16) {
+                    get_sensor_data(dptr, sensor);
+                    dptr += 8;
+                    printf("LOW_RES_QUAT, %d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr);
+                }  else
+                    done_flag = true;
+                break;
+            case LPQUAT_HDR:
+                if (buf_size >= 24) {
+                    q[0] = *(int *)(dptr + 4);
+                    dptr += 8;
+                    q[1] = *(int *)(dptr);
+                    q[2] = *(int *)(dptr + 4);
+                    dptr += 8;
+                    printf("LPQ_3AXES, %d, %d, %d, %lld\n", q[0], q[1], q[2], *(long long *)dptr);
+                }  else
+                    done_flag = true;
+                break;
+            case SIXQUAT_HDR:
+                if (buf_size >= 24) {
+                    q[0] = *(int *)(dptr + 4);
+                    dptr += 8;
+                    q[1] = *(int *)(dptr);
+                    q[2] = *(int *)(dptr + 4);
+                    dptr += 8;
+                    printf("LPQ_6AXES, %d, %d, %d, %lld\n", q[0], q[1], q[2], *(long long *)dptr);
+                }  else
+                    done_flag = true;
+                break;
+            case STEP_DETECTOR_HDR:
+                if (buf_size >= 16) {
+                    printf("STEP_DETECTOR, ");
+                    dptr += 8;
+                    printf("%lld\n", *(long long *)dptr);
+                }  else
+                    done_flag = true;
+
+                break;
+            default:
+                printf("unknown, \n");
+                for (i = 0; i < 8; i++)
+                    printf("%02x, ", dptr[i]);
+                printf("\n");
+                break;
+            }
+            if (!done_flag)
+                dptr += 8;
+            buf_size = ind - (dptr - data);
+        }
+        if (ind - (dptr - data) > 0)
+            memcpy(tmp, dptr, ind - (dptr - data));
+        left_over_size = ind - (dptr - data);
+    }
+    close(fp);
+
+error_read_data:
+    return ret;
+}
+
+/*
+    Main
+*/
+
+int main(int argc, char **argv)
+{
+    unsigned long num_loops = -1;
+    int ret, c, i;
+
+    int dev_num;
+    char *buffer_access;
+    char chip_name[10];
+    char *dummy;
+    char device_name[10];
+    char sysfs[100];
+
+    // all output to stdout must be delivered immediately, no buffering
+    setvbuf(stdout, NULL, _IONBF, 0);
+
+    /* parse the command line parameters 
+       TODO description
+    */
+    while ((c = getopt(argc, argv, "c:vh")) != -1) {
+        switch (c) {
+        case 'c':
+            num_loops = strtoul(optarg, &dummy, 10);
+            break;
+        case 'v':
+            verbose = true;
+            break;
+        case 'h':
+            //print_help();
+            // TODO write print_help helper function
+            break;
+        case '?':
+            return -1;
+        }
+    }
+
+    // get info about the device and driver
+    inv_get_sysfs_path(sysfs);
+    if (inv_get_chip_name(chip_name) != INV_SUCCESS) {
+        printf("get chip name fail\n");
+        exit(0);
+    }
+    printf("INFO: chip_name=%s\n", chip_name);
+
+    for (i = 0; i < strlen(chip_name); i++)
+        device_name[i] = tolower(chip_name[i]);
+    device_name[strlen(chip_name)] = '\0';
+    printf("INFO: device name=%s\n", device_name);
+
+    /* Find the device requested */
+    dev_num = find_type_by_name(device_name, "iio:device");
+    if (dev_num < 0) {
+        printf("Failed to find the %s\n", device_name);
+        ret = -ENODEV;
+        goto error_ret;
+    }
+    printf("INFO: iio device number=%d\n", dev_num);
+    
+    /* attempt to open non blocking the access dev */
+    ret = asprintf(&buffer_access, "/dev/iio:device%d", dev_num);
+    if (ret < 0) {
+        ret = -ENOMEM;
+        goto error_ret;
+    }
+    while (num_loops == -1 || num_loops--)
+        read_data(buffer_access);
+    free(buffer_access);
+
+error_ret:
+    return ret;
+}
diff --git a/6515/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared b/6515/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared
new file mode 100755
index 0000000..be620fe
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared
diff --git a/6515/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk b/6515/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk
new file mode 100644
index 0000000..8591982
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk
@@ -0,0 +1,96 @@
+EXEC = inv_gesture_test$(SHARED_APP_SUFFIX)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
+COMP  ?= $(CROSS)gcc
+LINK  ?= $(CROSS)gcc
+
+OBJFOLDER = $(CURDIR)/obj
+
+INV_ROOT   = ../../../../..
+APP_DIR    = $(CURDIR)/../..
+MLLITE_DIR = $(INV_ROOT)/software/core/mllite
+MPL_DIR    = $(INV_ROOT)/software/core/mpl
+
+include $(INV_ROOT)/software/build/android/common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += $(ANDROID_COMPILE)
+CFLAGS += -Wall
+CFLAGS += -fpic
+CFLAGS += -nostdlib
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT
+CFLAGS += -DLINUX
+CFLAGS += -DANDROID
+CFLAGS += -mthumb-interwork
+CFLAGS += -fno-exceptions
+CFLAGS += -ffunction-sections
+CFLAGS += -funwind-tables
+CFLAGS += -fstack-protector
+CFLAGS += -fno-short-enums
+CFLAGS += -fmessage-length=0
+CFLAGS += -I$(MLLITE_DIR)
+CFLAGS += -I$(MPL_DIR)
+CFLAGS += -I$(COMMON_DIR)
+CFLAGS += -I$(HAL_DIR)/include
+CFLAGS += $(INV_INCLUDES)
+CFLAGS += $(INV_DEFINES)
+
+LLINK  = -lc
+LLINK += -lm
+LLINK += -lutils
+LLINK += -lcutils
+LLINK += -lgcc
+LLINK += -ldl
+LLINK += -lstdc++
+LLINK += -llog
+LLINK += -lz
+
+LFLAGS += $(CMDLINE_LFLAGS)
+LFLAGS += $(ANDROID_LINK_EXECUTABLE)
+
+LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+####################################################################################################
+## sources
+
+INV_LIBS  = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT)
+INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
+
+#INV_SOURCES and VPATH provided by Makefile.filelist
+include ../filelist.mk
+
+INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
+INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all clean cleanall install
+
+all: $(EXEC) $(MK_NAME)
+
+$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
+	$(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+
+$(OBJFOLDER) :
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(EXEC) $(OBJFOLDER)
+
+install : $(EXEC)
+	cp -f $(EXEC) $(INSTALL_DIR)
+
+
diff --git a/6515/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk b/6515/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk
new file mode 100644
index 0000000..75d93cf
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk
@@ -0,0 +1,11 @@
+#### filelist.mk for inv_gesture_test ####
+
+# headers
+#HEADERS += 
+
+# sources
+SOURCES := $(APP_DIR)/inv_gesture_test.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH += $(APP_DIR)
diff --git a/6515/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c b/6515/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c
new file mode 100644
index 0000000..ac0803f
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c
@@ -0,0 +1,720 @@
+/**
+ *  Gesture Test application for Invensense's MPU6/9xxx (w/ DMP).
+ */
+
+#include <unistd.h>
+#include <dirent.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <errno.h>
+#include <sys/stat.h>
+#include <stdlib.h>
+#include <features.h>
+#include <dirent.h>
+#include <string.h>
+#include <poll.h>
+#include <stddef.h>
+#include <linux/input.h>
+#include <time.h>
+#include <linux/time.h>
+#include <unistd.h>
+#include <termios.h>
+
+#include "invensense.h"
+#include "ml_math_func.h"
+#include "storage_manager.h"
+#include "ml_stored_data.h"
+#include "ml_sysfs_helper.h"
+#include "mlos.h"
+
+//#define DEBUG_PRINT   /* Uncomment to print Gyro & Accel read from Driver */
+
+#define SUPPORT_SCREEN_ORIENTATION
+//#define SUPPORT_TAP
+//#define SUPPORT_ORIENTATION
+#define SUPPORT_PEDOMETER
+#define SUPPORT_SMD
+
+#define MAX_SYSFS_NAME_LEN  (100)
+#define MAX_SYSFS_ATTRB     (sizeof(struct sysfs_attrbs) / sizeof(char*))
+#define IIO_SYSFS_PATH      "/sys/bus/iio/devices/iio:device0"
+#define IIO_HUB_NAME        "inv_hub"
+
+#define POLL_TIME           (2000) // 2sec
+
+struct sysfs_attrbs {
+    char *name;
+    char *enable;
+    char *power_state;
+    char *dmp_on;
+    char *dmp_int_on;
+    char *dmp_firmware;
+    char *firmware_loaded;
+#ifdef SUPPORT_SCREEN_ORIENTATION
+    char *event_display_orientation;
+    char *display_orientation_on;
+#endif
+#ifdef SUPPORT_ORIENTATION
+    char *event_orientation;
+    char *orientation_on;
+#endif
+#ifdef SUPPORT_TAP
+    char *event_tap;
+    char *tap_min_count;
+    char *tap_on;
+    char *tap_threshold;
+    char *tap_time;
+#endif
+#ifdef SUPPORT_PEDOMETER
+    char *pedometer_on;
+    char *pedometer_steps;
+    char *pedometer_time;
+#endif
+#ifdef SUPPORT_SMD
+    char *event_smd;
+    char *smd_enable;
+    char *smd_threshold;
+    char *smd_delay_threshold;
+    char *smd_delay_threshold2;
+#endif
+} mpu;
+
+enum {
+#ifdef SUPPORT_TAP
+    FEAT_TAP,
+#endif
+#ifdef SUPPORT_SCREEN_ORIENTATION
+    FEAT_SCREEN_ORIENTATION,
+#endif
+#ifdef SUPPORT_ORIENTATION
+    FEAT_ORIENTATION,
+#endif
+#ifdef SUPPORT_PEDOMETER
+    FEAT_PEDOMETER,
+#endif
+#ifdef SUPPORT_SMD
+    FEAT_SMD,
+#endif
+
+    NUM_DMP_FEATS
+};
+
+char *sysfs_names_ptr;
+#ifdef SUPPORT_PEDOMETER
+unsigned long last_pedometer_poll = 0L;
+unsigned long pedometer_poll_timeout = 500L; // .5 second
+#endif
+struct pollfd pfd[NUM_DMP_FEATS];
+bool android_hub = false;   // flag to indicate true=Hub, false=non-hub
+
+/*******************************************************************************
+ *                       DMP Feature Supported Functions
+ ******************************************************************************/
+
+int read_sysfs_int(char *filename, int *var)
+{
+    int res=0;
+    FILE *fp;
+
+    fp = fopen(filename, "r");
+    if (fp!=NULL) {
+        fscanf(fp, "%d\n", var);
+        fclose(fp);
+    } else {
+        printf("ERR open file to read: %s\n", filename);
+        res= -1;
+    }
+    return res;
+}
+
+int write_sysfs_int(char *filename, int data)
+{
+    int res=0;
+    FILE  *fp;
+
+#ifdef DEBUG_PRINT
+    printf("writing '%s' with '%d'\n", filename, data);
+#endif
+
+    fp = fopen(filename, "w");
+    if (fp != NULL) {
+        fprintf(fp, "%d\n", data);
+        fclose(fp);
+    } else {
+        printf("ERR open file to write: %s\n", filename);
+        res = -1;
+    }
+    return res;
+}
+
+/**************************************************
+    This _kbhit() function is courtesy of the web
+***************************************************/
+int _kbhit(void)
+{
+    static const int STDIN = 0;
+    static bool initialized = false;
+
+    if (! initialized) {
+        // Use termios to turn off line buffering
+        struct termios term;
+        tcgetattr(STDIN, &term);
+        term.c_lflag &= ~ICANON;
+        tcsetattr(STDIN, TCSANOW, &term);
+        setbuf(stdin, NULL);
+        initialized = true;
+    }
+
+    int bytesWaiting;
+    ioctl(STDIN, FIONREAD, &bytesWaiting);
+    return bytesWaiting;
+}
+
+int inv_init_sysfs_attributes(void)
+{
+    unsigned char i = 0;
+    char sysfs_path[MAX_SYSFS_NAME_LEN];
+    char *sptr;
+    char **dptr;
+
+    sysfs_names_ptr =
+            (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
+    sptr = sysfs_names_ptr;
+    if (sptr != NULL) {
+        dptr = (char**)&mpu;
+        do {
+            *dptr++ = sptr;
+            sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
+        } while (++i < MAX_SYSFS_ATTRB);
+    } else {
+        printf("couldn't alloc mem for sysfs paths\n");
+        return -1;
+    }
+
+    // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
+    inv_get_sysfs_path(sysfs_path);
+
+    sprintf(mpu.name, "%s%s", sysfs_path, "/name");
+    sprintf(mpu.enable, "%s%s", sysfs_path, "/buffer/enable");
+    sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
+    sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
+    sprintf(mpu.dmp_int_on, "%s%s", sysfs_path, "/dmp_int_on");
+    sprintf(mpu.dmp_firmware, "%s%s", sysfs_path, "/dmp_firmware");
+    sprintf(mpu.firmware_loaded, "%s%s", sysfs_path, "/firmware_loaded");
+
+#ifdef SUPPORT_SCREEN_ORIENTATION
+    sprintf(mpu.event_display_orientation, "%s%s", 
+            sysfs_path, "/event_display_orientation");
+    sprintf(mpu.display_orientation_on, "%s%s", 
+            sysfs_path, "/display_orientation_on");
+#endif
+#ifdef SUPPORT_ORIENTATION
+    sprintf(mpu.event_orientation, "%s%s", sysfs_path, "/event_orientation");
+    sprintf(mpu.orientation_on, "%s%s", sysfs_path, "/orientation_on");
+#endif
+#ifdef SUPPORT_TAP
+    sprintf(mpu.event_tap, "%s%s", sysfs_path, "/event_tap");
+    sprintf(mpu.tap_min_count, "%s%s", sysfs_path, "/tap_min_count");
+    sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
+    sprintf(mpu.tap_threshold, "%s%s", sysfs_path, "/tap_threshold");
+    sprintf(mpu.tap_time, "%s%s", sysfs_path, "/tap_time");
+#endif
+#ifdef SUPPORT_PEDOMETER
+    sprintf(mpu.pedometer_on, "%s%s", sysfs_path, "/dmp_on");
+    sprintf(mpu.pedometer_steps, "%s%s", sysfs_path, "/pedometer_steps");
+    sprintf(mpu.pedometer_time, "%s%s", sysfs_path, "/pedometer_time");
+#endif
+#ifdef SUPPORT_SMD
+    sprintf(mpu.event_smd, "%s%s", sysfs_path, "/event_smd");
+    sprintf(mpu.smd_enable, "%s%s", sysfs_path, "/smd_enable");
+    sprintf(mpu.smd_threshold, "%s%s", sysfs_path, "/smd_threshold");
+    sprintf(mpu.smd_delay_threshold, "%s%s", 
+            sysfs_path, "/smd_delay_threshold");
+    sprintf(mpu.smd_delay_threshold2, "%s%s", 
+            sysfs_path, "/smd_delay_threshold2");
+#endif
+
+#if 0
+    // test print sysfs paths
+    dptr = (char**)&mpu;
+    for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
+        MPL_LOGE("sysfs path: %s", *dptr++);
+    }
+#endif
+    return 0;
+}
+
+int dmp_fw_loaded(void)
+{
+    int fw_loaded;
+    if (read_sysfs_int(mpu.firmware_loaded, &fw_loaded) < 0)
+        fw_loaded= 0;
+    return fw_loaded;
+}
+
+int is_android_hub(void)
+{
+    char dev_name[8];
+    FILE *fp;
+
+    fp= fopen(mpu.name, "r");
+    fgets(dev_name, 8, fp);
+    fclose(fp);
+    
+    if (!strncmp(dev_name, IIO_HUB_NAME, sizeof(IIO_HUB_NAME))) {
+        android_hub = true;
+    }else {
+        android_hub = false;
+    }
+
+    return 0;
+}
+
+/*
+    Enablers for the gestures
+*/
+
+int master_enable(int en)
+{
+    if (write_sysfs_int(mpu.enable, en) < 0) {
+        printf("GT:ERR-can't write 'buffer/enable'");
+        return -1;
+    }
+    return 0;
+}
+
+#ifdef SUPPORT_TAP
+int enable_tap(int en)
+{
+    if (write_sysfs_int(mpu.tap_on, en) < 0) {
+        printf("GT:ERR-can't write 'tap_on'\n");
+        return -1;
+    }
+
+    return 0;
+}
+#endif
+
+/* Unnecessary: pedometer_on == dmp_on, which is always on 
+#ifdef SUPPORT_PEDOMETER
+int enable_pedometer(int en)
+{
+    if (write_sysfs_int(mpu.pedometer_on, en) < 0) {
+        printf("GT:ERR-can't write 'pedometer_on'\n");
+        return -1;
+    }
+
+    return 0;
+}
+#endif
+*/
+
+#ifdef SUPPORT_SCREEN_ORIENTATION
+int enable_display_orientation(int en)
+{
+    if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
+        printf("GT:ERR-can't write 'display_orientation_on'\n");
+        return -1;
+    }
+
+    return 0;
+}
+#endif
+
+#ifdef SUPPORT_ORIENTATION
+int enable_orientation(int en)
+{
+    if (write_sysfs_int(mpu.orientation_on, en) < 0) {
+        printf("GT:ERR-can't write 'orientation_on'\n");
+        return -1;
+    }
+
+    return 0;
+}
+#endif
+
+#ifdef SUPPORT_SMD
+int enable_smd(int en)
+{
+    if (write_sysfs_int(mpu.smd_enable, en) < 0) {
+        printf("GT:ERR-can't write 'smd_enable'\n");
+        return -1;
+    }
+    return 0;
+}
+#endif
+
+/*
+    Handlers for the gestures
+*/
+#ifdef SUPPORT_TAP
+int tap_handler(void)
+{
+    FILE *fp;
+    int tap, tap_dir, tap_num;
+
+    fp = fopen(mpu.event_tap, "rt");
+    fscanf(fp, "%d\n", &tap);
+    fclose(fp);
+
+    tap_dir = tap/8;
+    tap_num = tap%8 + 1;
+
+#ifdef DEBUG_PRINT
+    printf("GT:Tap Handler **\n");
+    printf("Tap= %x\n", tap);
+    printf("Tap Dir= %x\n", tap_dir);
+    printf("Tap Num= %x\n", tap_num);
+#endif
+
+    switch (tap_dir) {
+        case 1:
+            printf("Tap Axis->X Pos, ");
+            break;
+        case 2:
+            printf("Tap Axis->X Neg, ");
+            break;
+        case 3:
+            printf("Tap Axis->Y Pos, ");
+            break;
+        case 4:
+            printf("Tap Axis->Y Neg, ");
+            break;
+        case 5:
+            printf("Tap Axis->Z Pos, ");
+            break;
+        case 6:
+            printf("Tap Axis->Z Neg, ");
+            break;
+        default:
+            printf("Tap Axis->Unknown, ");
+            break;
+    }
+    printf("#%d\n", tap_num);
+
+    return 0;
+}
+#endif
+
+#ifdef SUPPORT_PEDOMETER
+int pedometer_handler(void)
+{
+    FILE *fp;
+    static int last_pedometer_steps = -1;
+    static long last_pedometer_time = -1;
+    int pedometer_steps;
+    long pedometer_time;
+
+#ifdef DEBUG_PRINT
+    printf("GT:Pedometer Handler\n");
+#endif
+
+    fp = fopen(mpu.pedometer_steps, "rt");
+    fscanf(fp, "%d\n", &pedometer_steps);
+    fclose(fp);
+
+    fp = fopen(mpu.pedometer_time, "rt");
+    fscanf(fp, "%ld\n", &pedometer_time);
+    fclose(fp);
+
+    if (last_pedometer_steps == -1 && last_pedometer_time == -1) {
+        printf("Pedometer Steps: %d Time: %ld ",
+               pedometer_steps, pedometer_time);
+        if (pedometer_steps > 10 
+                || pedometer_time > (pedometer_poll_timeout * 2))
+            printf("(resumed)\n");
+        else
+            printf("\n");
+    } else if (last_pedometer_steps != pedometer_steps
+                    || last_pedometer_time != pedometer_time) {
+    printf("Pedometer Steps: %d Time: %ld\n", 
+           pedometer_steps, pedometer_time);
+    }
+
+    last_pedometer_steps = pedometer_steps;
+    last_pedometer_time = pedometer_time;
+
+    return 0;
+}
+#endif
+
+#ifdef SUPPORT_SCREEN_ORIENTATION
+int display_orientation_handler(void)
+{
+    FILE *fp;
+    int orient;
+
+#ifdef DEBUG_PRINT
+    printf("GT:Screen Orient Handler\n");
+#endif
+
+    fp = fopen(mpu.event_display_orientation, "rt");
+    if (!fp) {
+        printf("GT:Cannot open '%s'\n", mpu.event_display_orientation);
+        return -1;
+    }
+    fscanf(fp, "%d\n", &orient);
+    fclose(fp);
+
+    printf("Screen Orient-> %d\n", orient);
+
+    return 0;
+}
+#endif
+
+#ifdef SUPPORT_ORIENTATION
+int host_orientation_handler(void)
+{
+    FILE *fp;
+    int orient;
+
+    fp = fopen(mpu.event_orientation, "rt");
+    fscanf(fp, "%d\n", &orient);
+    fclose(fp);
+
+#ifdef DEBUG_PRINT
+    printf("GT:Reg Orient Handler\n");
+#endif
+
+    if (orient & 0x01)
+        printf("Orient->X Up\n");
+    if (orient & 0x02)
+        printf("Orient->X Down\n");
+    if (orient & 0x04)
+        printf("Orient->Y Up\n");
+    if (orient & 0x08)
+        printf("Orient->Y Down\n");
+    if (orient & 0x10)
+        printf("Orient->Z Up\n");
+    if (orient & 0x20)
+        printf("Orient->Z Down\n");
+    if (orient & 0x40)
+        printf("Orient->Flip\n");
+
+    return 0;
+}
+#endif
+
+#ifdef SUPPORT_SMD
+int smd_handler(void)
+{
+    FILE *fp;
+    int smd;
+
+    fp = fopen(mpu.event_smd, "rt");
+    fscanf(fp, "%d\n", &smd);
+    fclose(fp);
+
+#ifdef DEBUG_PRINT
+    printf("GT:SMD Handler\n");
+#endif
+    printf("SMD (%d)\n", smd);
+
+    /* wait for the acceleration low pass filtered tail to die off -
+       this is to prevent that the tail end of a 2nd event of above threhsold 
+       motion be considered as also the 1st event for the next SM detection */
+    inv_sleep(1000);
+
+    /* re-enable to continue the detection */
+    master_enable(0);
+    enable_smd(1);
+    master_enable(1);
+
+    return 0;
+}
+#endif
+
+int enable_dmp_features(int en)
+{
+    int res= -1;
+
+    if (android_hub || dmp_fw_loaded()) {
+        /* Currently there's no info regarding DMP's supported features/capabilities
+           An error in enabling features below could be an indication of the feature
+           not supported in current loaded DMP firmware */
+
+        master_enable(0);
+#ifdef SUPPORT_TAP
+        enable_tap(en);
+#endif
+#ifdef SUPPORT_SCREEN_ORIENTATION
+        enable_display_orientation(en);
+#endif
+#ifdef SUPPORT_ORIENTATION
+        if (android_hub == false) {
+            // Android Hub does not support 'regular' orientation feature
+            enable_orientation(en);
+        }
+#endif
+#ifdef SUPPORT_SMD
+        enable_smd(en);
+#endif
+        master_enable(1);
+        res = 0;
+
+    } else {
+        printf("GT:ERR-No DMP firmware\n");
+        res= -1;
+    }
+
+    return res;
+}
+
+int init_fds(void)
+{
+    int i;
+
+    for (i = 0; i < NUM_DMP_FEATS; i++) {
+        switch(i) {
+#ifdef SUPPORT_TAP
+        case FEAT_TAP:
+            pfd[i].fd = open(mpu.event_tap, O_RDONLY | O_NONBLOCK);
+            break;
+#endif
+#ifdef SUPPORT_SCREEN_ORIENTATION
+        case FEAT_SCREEN_ORIENTATION:
+            pfd[i].fd = open(mpu.event_display_orientation,
+                             O_RDONLY | O_NONBLOCK);
+            break;
+#endif
+#ifdef SUPPORT_ORIENTATION
+        case FEAT_ORIENTATION:
+            pfd[i].fd = open(mpu.event_orientation, O_RDONLY | O_NONBLOCK);
+            break;
+#endif
+#ifdef SUPPORT_SMD
+        case FEAT_SMD:
+            pfd[i].fd = open(mpu.event_smd, O_RDONLY | O_NONBLOCK);
+            break;
+#endif
+        default:
+            pfd[i].fd = -1;
+        }
+
+        pfd[i].events = POLLPRI|POLLERR,
+        pfd[i].revents = 0;
+    }
+
+    return 0;
+}
+
+void parse_events(struct pollfd pfd[], int num_fds)
+{
+    int i;
+
+    for (i = 0; i < num_fds; i++) {
+        if(pfd[i].revents != 0) {
+            switch(i) {
+#ifdef SUPPORT_TAP
+            case FEAT_TAP:
+                tap_handler();
+                break;
+#endif
+#ifdef SUPPORT_SCREEN_ORIENTATION
+            case FEAT_SCREEN_ORIENTATION:
+                display_orientation_handler();
+                break;
+#endif
+#ifdef SUPPORT_ORIENTATION
+            case FEAT_ORIENTATION:
+                host_orientation_handler();
+                break;
+#endif
+#ifdef SUPPORT_SMD
+            case FEAT_SMD:
+                smd_handler();
+                break;
+#endif
+            default:
+                printf("GT:ERR-unhandled/unrecognized gesture event");
+                break;
+            }
+            pfd[i].revents = 0;   // no need: reset anyway
+        }
+    }
+
+#ifdef SUPPORT_PEDOMETER
+    {
+        unsigned long now;
+        // pedometer is not event based, therefore we poll using a timer every
+        //  pedometer_poll_timeout milliseconds
+        if ((now = inv_get_tick_count()) - last_pedometer_poll
+                > pedometer_poll_timeout) {
+            pedometer_handler();
+            last_pedometer_poll = now;
+        }
+    }
+#endif
+}
+
+int close_fds(void)
+{
+    int i;
+    for (i = 0; i < NUM_DMP_FEATS; i++) {
+        if (!pfd[i].fd)
+            close(pfd[i].fd);
+    }
+    return 0;
+}
+
+/*******************************************************************************
+ *                       M a i n
+ ******************************************************************************/
+
+int main(int argc, char **argv)
+{
+    char data[4];
+    int i, res= 0;
+
+    printf("\n"
+           "****************************************************************\n"
+           "*** NOTE:                                                    ***\n"
+           "***       the HAL must be compiled with Low power quaternion ***\n"
+           "***           and/or DMP screen orientation support.         ***\n"
+           "***       'At least' one of the 4 Android virtual sensors    ***\n"
+           "***           must be enabled.                               ***\n"
+           "***                                                          ***\n"
+           "*** Please perform gestures to see the output.               ***\n"
+           "*** Press any key to stop the program.                       ***\n"
+           "****************************************************************\n"
+           "\n");
+
+    res = inv_init_sysfs_attributes();
+    if (res) {
+        printf("GT:ERR-Can't allocate mem\n");
+        return -1;
+    }
+
+    /* check if Android Hub */
+    is_android_hub();
+
+    /* init Fds to poll for gesture data */
+    init_fds();
+
+    /* on Gesture/DMP supported features */
+    if (enable_dmp_features(1) < 0) {
+        printf("GT:ERR-Can't enable Gestures\n");
+        return -1;
+    }
+
+    do {
+        for (i = 0; i < NUM_DMP_FEATS; i++)
+            read(pfd[i].fd, data, 4);
+        poll(pfd, NUM_DMP_FEATS, POLL_TIME);
+        parse_events(pfd, NUM_DMP_FEATS);
+    } while (!_kbhit());
+
+    /* off Gesture/DMP supported features */
+    if (enable_dmp_features(0) < 0) {
+        printf("GT:ERR-Can't disable Gestures\n");
+        return -1;
+    }
+
+    /* release resources */
+    close_fds();
+    if (sysfs_names_ptr)
+        free(sysfs_names_ptr);
+
+    return res;
+}
+
diff --git a/6515/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared b/6515/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared
new file mode 100755
index 0000000..b524d37
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared
diff --git a/6515/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk b/6515/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk
new file mode 100644
index 0000000..2dda9e0
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk
@@ -0,0 +1,94 @@
+EXEC = inv_mpu_iio$(SHARED_APP_SUFFIX)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
+COMP  ?= $(CROSS)gcc
+LINK  ?= $(CROSS)gcc
+
+OBJFOLDER = $(CURDIR)/obj
+
+INV_ROOT   = ../../../../..
+APP_DIR    = $(CURDIR)/../..
+MLLITE_DIR = $(INV_ROOT)/software/core/mllite
+MPL_DIR    = $(INV_ROOT)/software/core/mpl
+
+include $(INV_ROOT)/software/build/android/common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += $(ANDROID_COMPILE)
+CFLAGS += -Wall
+CFLAGS += -fpic
+CFLAGS += -nostdlib
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT
+CFLAGS += -DLINUX
+CFLAGS += -DANDROID
+CFLAGS += -mthumb-interwork
+CFLAGS += -fno-exceptions
+CFLAGS += -ffunction-sections
+CFLAGS += -funwind-tables
+CFLAGS += -fstack-protector
+CFLAGS += -fno-short-enums
+CFLAGS += -fmessage-length=0
+CFLAGS += -I$(MLLITE_DIR)
+CFLAGS += -I$(MPL_DIR)
+CFLAGS += $(INV_INCLUDES)
+CFLAGS += $(INV_DEFINES)
+
+LLINK  = -lc
+LLINK += -lm
+LLINK += -lutils
+LLINK += -lcutils
+LLINK += -lgcc
+LLINK += -ldl
+LLINK += -lstdc++
+LLINK += -llog
+LLINK += -lz
+
+LFLAGS += $(CMDLINE_LFLAGS)
+LFLAGS += $(ANDROID_LINK_EXECUTABLE)
+
+LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+####################################################################################################
+## sources
+
+INV_LIBS  = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT)
+INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
+
+#INV_SOURCES and VPATH provided by Makefile.filelist
+include ../filelist.mk
+
+INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
+INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all clean cleanall install
+
+all: $(EXEC) $(MK_NAME)
+
+$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
+	$(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+
+$(OBJFOLDER) :
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(EXEC) $(OBJFOLDER)
+
+install : $(EXEC)
+	cp -f $(EXEC) $(INSTALL_DIR)
+
+
diff --git a/6515/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk b/6515/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk
new file mode 100644
index 0000000..8a3977a
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk
@@ -0,0 +1,12 @@
+#### filelist.mk for mpu_iio ####
+
+# headers
+#HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h
+HEADERS += $(APP_DIR)/iio_utils.h
+
+# sources
+SOURCES := $(APP_DIR)/mpu_iio.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux
diff --git a/6515/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h b/6515/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h
new file mode 100644
index 0000000..c3d4955
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h
@@ -0,0 +1,650 @@
+/* IIO - useful set of util functionality
+ *
+ * Copyright (c) 2008 Jonathan Cameron
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+/* Made up value to limit allocation sizes */
+#include <string.h>
+#include <stdlib.h>
+#include <ctype.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <dirent.h>
+
+#define IIO_MAX_NAME_LENGTH 30
+
+#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements"
+#define FORMAT_TYPE_FILE "%s_type"
+
+const char *iio_dir = "/sys/bus/iio/devices/";
+
+extern int verbose;
+
+/**
+ * iioutils_break_up_name() - extract generic name from full channel name
+ * @full_name: the full channel name
+ * @generic_name: the output generic channel name
+ **/
+static int iioutils_break_up_name(const char *full_name,
+				  char **generic_name)
+{
+	char *current;
+	char *w, *r;
+	char *working;
+	current = strdup(full_name);
+	working = strtok(current, "_\0");
+	w = working;
+	r = working;
+
+	while (*r != '\0') {
+		if (!isdigit(*r)) {
+			*w = *r;
+			w++;
+		}
+		r++;
+	}
+	*w = '\0';
+	*generic_name = strdup(working);
+	free(current);
+
+	return 0;
+}
+
+/**
+ * struct iio_channel_info - information about a given channel
+ * @name: channel name
+ * @generic_name: general name for channel type
+ * @scale: scale factor to be applied for conversion to si units
+ * @offset: offset to be applied for conversion to si units
+ * @index: the channel index in the buffer output
+ * @bytes: number of bytes occupied in buffer output
+ * @mask: a bit mask for the raw output
+ * @is_signed: is the raw value stored signed
+ * @enabled: is this channel enabled
+ **/
+struct iio_channel_info {
+	char *name;
+	char *generic_name;
+	float scale;
+	float offset;
+	unsigned index;
+	unsigned bytes;
+	unsigned bits_used;
+	unsigned shift;
+	uint64_t mask;
+	unsigned be;
+	unsigned is_signed;
+	unsigned enabled;
+	unsigned location;
+};
+
+/**
+ * iioutils_get_type() - find and process _type attribute data
+ * @is_signed: output whether channel is signed
+ * @bytes: output how many bytes the channel storage occupies
+ * @mask: output a bit mask for the raw data
+ * @be: big endian
+ * @device_dir: the iio device directory
+ * @name: the channel name
+ * @generic_name: the channel type name
+ **/
+inline int iioutils_get_type(unsigned *is_signed,
+			     unsigned *bytes,
+			     unsigned *bits_used,
+			     unsigned *shift,
+			     uint64_t *mask,
+			     unsigned *be,
+			     const char *device_dir,
+			     const char *name,
+			     const char *generic_name)
+{
+	FILE *sysfsfp;
+	int ret;
+	DIR *dp;
+	char *scan_el_dir, *builtname, *builtname_generic, *filename = 0;
+	char signchar, endianchar;
+	unsigned padint;
+	const struct dirent *ent;
+
+	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	ret = asprintf(&builtname, FORMAT_TYPE_FILE, name);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_free_scan_el_dir;
+	}
+	ret = asprintf(&builtname_generic, FORMAT_TYPE_FILE, generic_name);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_free_builtname;
+	}
+
+	dp = opendir(scan_el_dir);
+	if (dp == NULL) {
+		ret = -errno;
+		goto error_free_builtname_generic;
+	}
+	while (ent = readdir(dp), ent != NULL)
+		/*
+		 * Do we allow devices to override a generic name with
+		 * a specific one?
+		 */
+		if ((strcmp(builtname, ent->d_name) == 0) ||
+		    (strcmp(builtname_generic, ent->d_name) == 0)) {
+			ret = asprintf(&filename,
+				       "%s/%s", scan_el_dir, ent->d_name);
+			if (ret < 0) {
+				ret = -ENOMEM;
+				goto error_closedir;
+			}
+			sysfsfp = fopen(filename, "r");
+			if (sysfsfp == NULL) {
+				printf("failed to open %s\n", filename);
+				ret = -errno;
+				goto error_free_filename;
+			}
+
+			ret = fscanf(sysfsfp,
+				     "%ce:%c%u/%u>>%u",
+				     &endianchar,
+				     &signchar,
+				     bits_used,
+				     &padint, shift);
+			if (ret < 0) {
+				printf("failed to pass scan type description\n");
+				return ret;
+			}
+			*be = (endianchar == 'b');
+			*bytes = padint / 8;
+			if (*bits_used == 64)
+				*mask = ~0;
+			else
+				*mask = (1 << *bits_used) - 1;
+			if (signchar == 's')
+				*is_signed = 1;
+			else
+				*is_signed = 0;
+			fclose(sysfsfp);
+			free(filename);
+
+			filename = 0;
+		}
+error_free_filename:
+	if (filename)
+		free(filename);
+error_closedir:
+	closedir(dp);
+error_free_builtname_generic:
+	free(builtname_generic);
+error_free_builtname:
+	free(builtname);
+error_free_scan_el_dir:
+	free(scan_el_dir);
+error_ret:
+	return ret;
+}
+
+inline int iioutils_get_param_float(float *output,
+				    const char *param_name,
+				    const char *device_dir,
+				    const char *name,
+				    const char *generic_name)
+{
+	FILE *sysfsfp;
+	int ret;
+	DIR *dp;
+	char *builtname, *builtname_generic;
+	char *filename = NULL;
+	const struct dirent *ent;
+
+	ret = asprintf(&builtname, "%s_%s", name, param_name);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	ret = asprintf(&builtname_generic,
+		       "%s_%s", generic_name, param_name);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_free_builtname;
+	}
+	dp = opendir(device_dir);
+	if (dp == NULL) {
+		ret = -errno;
+		goto error_free_builtname_generic;
+	}
+	while (ent = readdir(dp), ent != NULL)
+		if ((strcmp(builtname, ent->d_name) == 0) ||
+		    (strcmp(builtname_generic, ent->d_name) == 0)) {
+			ret = asprintf(&filename,
+				       "%s/%s", device_dir, ent->d_name);
+			if (ret < 0) {
+				ret = -ENOMEM;
+				goto error_closedir;
+			}
+			sysfsfp = fopen(filename, "r");
+			if (!sysfsfp) {
+				ret = -errno;
+				goto error_free_filename;
+			}
+			fscanf(sysfsfp, "%f", output);
+			break;
+		}
+error_free_filename:
+	if (filename)
+		free(filename);
+error_closedir:
+	closedir(dp);
+error_free_builtname_generic:
+	free(builtname_generic);
+error_free_builtname:
+	free(builtname);
+error_ret:
+	return ret;
+}
+
+/**
+ * bsort_channel_array_by_index() - reorder so that the array is in index order
+ *
+ **/
+
+inline void bsort_channel_array_by_index(struct iio_channel_info **ci_array,
+					 int cnt)
+{
+	struct iio_channel_info temp;
+	int x, y;
+
+	for (x = 0; x < cnt; x++)
+		for (y = 0; y < (cnt - 1); y++)
+			if ((*ci_array)[y].index > (*ci_array)[y+1].index) {
+				temp = (*ci_array)[y + 1];
+				(*ci_array)[y + 1] = (*ci_array)[y];
+				(*ci_array)[y] = temp;
+			}
+}
+
+/**
+ * build_channel_array() - function to figure out what channels are present
+ * @device_dir: the IIO device directory in sysfs
+ * @
+ **/
+inline int build_channel_array(const char *device_dir,
+			      struct iio_channel_info **ci_array,
+			      int *counter)
+{
+	DIR *dp;
+	FILE *sysfsfp;
+	int count, i;
+	struct iio_channel_info *current;
+	int ret;
+	const struct dirent *ent;
+	char *scan_el_dir;
+	char *filename;
+
+	*counter = 0;
+	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	dp = opendir(scan_el_dir);
+	if (dp == NULL) {
+		ret = -errno;
+		goto error_free_name;
+	}
+	while (ent = readdir(dp), ent != NULL)
+		if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
+			   "_en") == 0) {
+			ret = asprintf(&filename,
+				       "%s/%s", scan_el_dir, ent->d_name);
+			if (ret < 0) {
+				ret = -ENOMEM;
+				goto error_close_dir;
+			}
+			sysfsfp = fopen(filename, "r");
+			if (sysfsfp == NULL) {
+				ret = -errno;
+				free(filename);
+				goto error_close_dir;
+			}
+			fscanf(sysfsfp, "%u", &ret);
+			//printf("%s, %d\n", filename, ret);
+			if (ret == 1)
+				(*counter)++;
+			fclose(sysfsfp);
+			free(filename);
+		}
+	*ci_array = malloc(sizeof(**ci_array) * (*counter));
+	if (*ci_array == NULL) {
+		ret = -ENOMEM;
+		goto error_close_dir;
+	}
+	closedir(dp);
+	dp = opendir(scan_el_dir);
+	//seekdir(dp, 0);
+	count = 0;
+	while (ent = readdir(dp), ent != NULL) {
+		if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
+			   "_en") == 0) {
+			current = &(*ci_array)[count++];
+			ret = asprintf(&filename,
+				       "%s/%s", scan_el_dir, ent->d_name);
+			if (ret < 0) {
+				ret = -ENOMEM;
+				/* decrement count to avoid freeing name */
+				count--;
+				goto error_cleanup_array;
+			}
+			sysfsfp = fopen(filename, "r");
+			if (sysfsfp == NULL) {
+				free(filename);
+				ret = -errno;
+				goto error_cleanup_array;
+			}
+			fscanf(sysfsfp, "%u", &current->enabled);
+			fclose(sysfsfp);
+
+			if (!current->enabled) {
+				free(filename);
+				count--;
+				continue;
+			}
+
+			current->scale = 1.0;
+			current->offset = 0;
+			current->name = strndup(ent->d_name,
+						strlen(ent->d_name) -
+						strlen("_en"));
+			if (current->name == NULL) {
+				free(filename);
+				ret = -ENOMEM;
+				goto error_cleanup_array;
+			}
+			/* Get the generic and specific name elements */
+			ret = iioutils_break_up_name(current->name,
+						     &current->generic_name);
+			if (ret) {
+				free(filename);
+				goto error_cleanup_array;
+			}
+			ret = asprintf(&filename,
+				       "%s/%s_index",
+				       scan_el_dir,
+				       current->name);
+			if (ret < 0) {
+				free(filename);
+				ret = -ENOMEM;
+				goto error_cleanup_array;
+			}
+			sysfsfp = fopen(filename, "r");
+			fscanf(sysfsfp, "%u", &current->index);
+			fclose(sysfsfp);
+			free(filename);
+			/* Find the scale */
+			ret = iioutils_get_param_float(&current->scale,
+						       "scale",
+						       device_dir,
+						       current->name,
+						       current->generic_name);
+			if (ret < 0)
+				goto error_cleanup_array;
+			ret = iioutils_get_param_float(&current->offset,
+						       "offset",
+						       device_dir,
+						       current->name,
+						       current->generic_name);
+			if (ret < 0)
+				goto error_cleanup_array;
+			ret = iioutils_get_type(&current->is_signed,
+						&current->bytes,
+						&current->bits_used,
+						&current->shift,
+						&current->mask,
+						&current->be,
+						device_dir,
+						current->name,
+						current->generic_name);
+		}
+	}
+
+	closedir(dp);
+	/* reorder so that the array is in index order */
+	bsort_channel_array_by_index(ci_array, *counter);
+
+	return 0;
+
+error_cleanup_array:
+	for (i = count - 1;  i >= 0; i--)
+		free((*ci_array)[i].name);
+	free(*ci_array);
+error_close_dir:
+	closedir(dp);
+error_free_name:
+	free(scan_el_dir);
+error_ret:
+	return ret;
+}
+
+inline int _write_sysfs_int(char *filename, char *basedir, int val, int verify)
+{
+    int ret = 0;
+	FILE *sysfsfp;
+	int test;
+	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+	if (temp == NULL)
+		return -ENOMEM;
+        
+	sprintf(temp, "%s/%s", basedir, filename);
+
+    if (verbose)
+        printf("VERB: echo %d > %s\n", val, temp);
+	sysfsfp = fopen(temp, "w");
+	if (sysfsfp == NULL) {
+		printf("failed to open %s\n", temp);
+		ret = -errno;
+		goto error_free;
+	}
+	fprintf(sysfsfp, "%d", val);
+	fclose(sysfsfp);
+
+	if (verify) {
+		sysfsfp = fopen(temp, "r");
+		if (sysfsfp == NULL) {
+			printf("failed to open %s\n", temp);
+			ret = -errno;
+			goto error_free;
+		}
+		fscanf(sysfsfp, "%d", &test);
+        fclose(sysfsfp);
+        if (verbose)
+            printf("VERB: cat %s = %d\n", temp, test);
+		if (test != val) {
+            printf("Possible failure in int write %d to %s\n",
+                   val, temp);
+			ret = -1;
+		}
+	}
+
+error_free:
+	free(temp);
+	return ret;
+}
+
+int write_sysfs_int(char *filename, char *basedir, int val)
+{
+	return _write_sysfs_int(filename, basedir, val, 0);
+}
+
+int write_sysfs_int_and_verify(char *filename, char *basedir, int val)
+{
+	return _write_sysfs_int(filename, basedir, val, 1);
+}
+
+int _write_sysfs_string(char *filename, char *basedir, char *val, int verify)
+{
+	int ret = 0;
+	FILE  *sysfsfp;
+	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+	if (temp == NULL) {
+		printf("Memory allocation failed\n");
+		return -ENOMEM;
+	}
+	sprintf(temp, "%s/%s", basedir, filename);
+
+	sysfsfp = fopen(temp, "w");
+	if (sysfsfp == NULL) {
+		printf("Could not open %s\n", temp);
+		ret = -errno;
+		goto error_free;
+	}
+	fprintf(sysfsfp, "%s", val);
+	fclose(sysfsfp);
+
+	if (verify) {
+		sysfsfp = fopen(temp, "r");
+		if (sysfsfp == NULL) {
+			printf("could not open file to verify\n");
+			ret = -errno;
+			goto error_free;
+		}
+		fscanf(sysfsfp, "%s", temp);
+        fclose(sysfsfp);
+        
+		if (strcmp(temp, val) != 0) {
+			printf("Possible failure in string write of %s "
+                   "Should be %s written to %s\%s\n",
+                   temp, val, basedir, filename);
+			ret = -1;
+		}
+	}
+
+error_free:
+	free(temp);
+
+	return ret;
+}
+
+/**
+ * write_sysfs_string_and_verify() - string write, readback and verify
+ * @filename: name of file to write to
+ * @basedir: the sysfs directory in which the file is to be found
+ * @val: the string to write
+ **/
+int write_sysfs_string_and_verify(char *filename, char *basedir, char *val)
+{
+	return _write_sysfs_string(filename, basedir, val, 1);
+}
+
+int write_sysfs_string(char *filename, char *basedir, char *val)
+{
+	return _write_sysfs_string(filename, basedir, val, 0);
+}
+
+int read_sysfs_posint(char *filename, char *basedir)
+{
+	int ret;
+	FILE  *sysfsfp;
+	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+	if (temp == NULL) {
+		printf("Memory allocation failed");
+		return -ENOMEM;
+	}
+	sprintf(temp, "%s/%s", basedir, filename);
+	sysfsfp = fopen(temp, "r");
+	if (sysfsfp == NULL) {
+		ret = -errno;
+		goto error_free;
+	}
+	fscanf(sysfsfp, "%d\n", &ret);
+	fclose(sysfsfp);
+error_free:
+	free(temp);
+	return ret;
+}
+
+int read_sysfs_float(char *filename, char *basedir, float *val)
+{
+	float ret = 0;
+	FILE  *sysfsfp;
+	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+	if (temp == NULL) {
+		printf("Memory allocation failed");
+		return -ENOMEM;
+	}
+	sprintf(temp, "%s/%s", basedir, filename);
+	sysfsfp = fopen(temp, "r");
+	if (sysfsfp == NULL) {
+		ret = -errno;
+		goto error_free;
+	}
+	fscanf(sysfsfp, "%f\n", val);
+	fclose(sysfsfp);
+
+error_free:
+	free(temp);
+	return ret;
+}
+
+int enable_se(const char *device_dir, struct iio_channel_info **ci_array, 
+              int *counter, char *sensor, int en)
+{
+	DIR *dp;
+	int ret;
+	const struct dirent *ent;
+	char *scan_el_dir;
+    char pattern[50] = "in_";
+
+	*counter = 0;
+	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	dp = opendir(scan_el_dir);
+	if (dp == NULL) {
+		ret = -errno;
+		goto error_free_name;
+	}
+    strcat(pattern, sensor);
+    while (ent = readdir(dp), ent != NULL) {
+        if (strncmp(ent->d_name, pattern, strlen(pattern)) == 0 &&
+            strncmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
+                    "_en", strlen("_en")) == 0) {
+            write_sysfs_int_and_verify((char *)ent->d_name, scan_el_dir, en);
+        }
+		}
+	return 0;
+
+error_ret:
+error_free_name:
+	return -1;
+}
+
+int enable_accel_se(const char *device_dir, 
+                         struct iio_channel_info **ci_array, int *counter, 
+                         int en)
+{
+    return enable_se(device_dir, ci_array, counter, "accel", en);
+}
+
+int enable_anglvel_se(const char *device_dir, 
+                         struct iio_channel_info **ci_array, int *counter, 
+                         int en)
+{
+    return enable_se(device_dir, ci_array, counter, "anglvel", en);
+}
+
+int enable_quaternion_se(const char *device_dir, 
+                         struct iio_channel_info **ci_array, int *counter, 
+                         int en)
+{
+    return enable_se(device_dir, ci_array, counter, "quaternion", en);
+}
+
diff --git a/6515/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c b/6515/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c
new file mode 100644
index 0000000..7a17392
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c
@@ -0,0 +1,942 @@
+/*
+ * Copyright (c) Invensense Inc. 2012
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <unistd.h>
+#include <dirent.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <errno.h>
+#include <sys/stat.h>
+#include <dirent.h>
+#include <linux/types.h>
+#include <string.h>
+#include <poll.h>
+#include <termios.h>
+
+#include "iio_utils.h"
+#include "ml_load_dmp.h"
+#include "ml_sysfs_helper.h"
+#include "authenticate.h"
+#include "mlos.h"
+
+#define DMP_CODE_SIZE (3060)
+#define POLL_TIME     (2000) // 2sec
+
+// settings
+static int accel_only = false;
+static int test_motion = false;
+static int test_flick = false;
+static int test_pedometer = false;
+static int test_orientation = false;
+int verbose = false;
+
+// paths
+char *dev_dir_name, *buf_dir_name;
+
+// all the DMP features supported
+enum {
+    FEAT_TAP = 0,
+    FEAT_ORIENTATION,
+    FEAT_DISPLAY_ORIENTATION,
+    FEAT_MOTION,
+    FEAT_FLICK,
+
+    FEAT_NUM,
+} features;
+
+typedef void (*handler_t) (int data);
+
+struct dmp_feat_t {
+    int enabled;
+    struct pollfd *pollfd;
+    char *sysfs_name;
+    handler_t phandler;
+};
+
+static struct dmp_feat_t dmp_feat[FEAT_NUM] = {{0}};
+static struct pollfd pollfds[FEAT_NUM];
+static int pollfds_used = 0;
+
+/**************************************************
+   This _kbhit() function is courtesy of the web
+***************************************************/
+int _kbhit(void)
+{
+    static const int STDIN = 0;
+    static bool initialized = false;
+
+    if (!initialized) {
+        // Use termios to turn off line buffering
+        struct termios term;
+        tcgetattr(STDIN, &term);
+        term.c_lflag &= ~ICANON;
+        tcsetattr(STDIN, TCSANOW, &term);
+        setbuf(stdin, NULL);
+        initialized = true;
+    }
+
+    int bytesWaiting;
+    ioctl(STDIN, FIONREAD, &bytesWaiting);
+    return bytesWaiting;
+}
+
+/**
+ * size_from_channelarray() - calculate the storage size of a scan
+ * @channels: the channel info array
+ * @num_channels: size of the channel info array
+ *
+ * Has the side effect of filling the channels[i].location values used
+ * in processing the buffer output.
+ */
+int size_from_channelarray(struct iio_channel_info *channels, int num_channels)
+{
+    int bytes = 0;
+    int i = 0;
+    while (i < num_channels) {
+        if (bytes % channels[i].bytes == 0)
+            channels[i].location = bytes;
+        else
+            channels[i].location = bytes - bytes%channels[i].bytes
+                + channels[i].bytes;
+        bytes = channels[i].location + channels[i].bytes;
+        i++;
+    }
+    return bytes;
+}
+
+void print2byte(int input, struct iio_channel_info *info)
+{
+    /* shift before conversion to avoid sign extension
+       of left aligned data */
+    input = input >> info->shift;
+    if (info->is_signed) {
+        int16_t val = input;
+        val &= (1 << info->bits_used) - 1;
+        val = (int16_t)(val << (16 - info->bits_used)) >>
+            (16 - info->bits_used);
+        /*printf("%d, %05f, scale=%05f", val,
+               (float)(val + info->offset)*info->scale, info->scale);*/
+        printf("%d, ", val);
+
+    } else {
+        uint16_t val = input;
+        val &= (1 << info->bits_used) - 1;
+        printf("%05f ", ((float)val + info->offset)*info->scale);
+    }
+}
+
+/**
+ * process_scan() - print out the values in SI units
+ * @data:        pointer to the start of the scan
+ * @infoarray:        information about the channels. Note
+ *  size_from_channelarray must have been called first to fill the
+ *  location offsets.
+ * @num_channels:    the number of active channels
+ */
+void process_scan(char *data, struct iio_channel_info *infoarray, 
+          int num_channels)
+{
+    int k;
+    //char *tmp;
+    for (k = 0; k < num_channels; k++) {
+        switch (infoarray[k].bytes) {
+            /* only a few cases implemented so far */
+        case 2:
+            print2byte(*(uint16_t *)(data + infoarray[k].location),
+                   &infoarray[k]);
+            //tmp = data + infoarray[k].location;
+            break;
+        case 4:
+            if (infoarray[k].is_signed) {
+                int32_t val = *(int32_t *)(data + infoarray[k].location);
+                if ((val >> infoarray[k].bits_used) & 1)
+                    val = (val & infoarray[k].mask) | ~infoarray[k].mask;
+                /* special case for timestamp */
+                printf(" %d ", val);
+            }
+            break;
+        case 8:
+            if (infoarray[k].is_signed) {
+                int64_t val = *(int64_t *)(data + infoarray[k].location);
+                if ((val >> infoarray[k].bits_used) & 1)
+                    val = (val & infoarray[k].mask) | ~infoarray[k].mask;
+                /* special case for timestamp */
+                if (infoarray[k].scale == 1.0f &&
+                    infoarray[k].offset == 0.0f)
+                    printf(" %lld", val);
+                else
+                    printf("%05f ", ((float)val + infoarray[k].offset)
+                            * infoarray[k].scale);
+            }
+            break;
+        default:
+            break;
+        }
+    }
+    printf("\n");
+}
+
+/*
+    Enablers for the gestures
+*/
+
+int enable_flick(char *p, int on)
+{
+    int ret;
+    printf("flick:%s\n", p);
+    ret = write_sysfs_int_and_verify("flick_int_on", p, on);
+    if (ret < 0)
+        return ret;
+    ret = write_sysfs_int_and_verify("flick_upper", p, 3147790);
+    if (ret < 0)
+        return ret;
+    ret = write_sysfs_int_and_verify("flick_lower", p, -3147790);
+    if (ret < 0)
+        return ret;
+    ret = write_sysfs_int_and_verify("flick_counter", p, 50);
+    if (ret < 0)
+        return ret;
+    ret = write_sysfs_int_and_verify("flick_message_on", p, 0);
+    if (ret < 0)
+        return ret;
+    ret = write_sysfs_int_and_verify("flick_axis", p, 0);
+    if (ret < 0)
+        return ret;
+
+    return 0;
+}
+
+void verify_img(char *dmp_path)
+{
+    FILE *fp;
+    int i;
+    char dmp_img[DMP_CODE_SIZE];
+
+    if ((fp = fopen(dmp_path, "rb")) < 0) {
+        perror("dmp fail");
+    }
+    i = fread(dmp_img, 1, DMP_CODE_SIZE, fp);
+    printf("Result=%d\n", i);
+    fclose(fp);
+    fp = fopen("/dev/read_img.h", "wt");
+    fprintf(fp, "char rec[]={\n");
+    for(i = 0; i < DMP_CODE_SIZE; i++) {
+        fprintf(fp, "0x%02x, ", dmp_img[i]);
+        if(((i + 1) % 16) == 0) {
+            fprintf(fp, "\n");
+        }
+    }
+    fprintf(fp, "};\n ");
+    fclose(fp);
+}
+
+int setup_dmp(char *dev_path, int p_event)
+{
+    char dmp_path[100];
+    int ret;
+    FILE *fd;
+
+    printf("INFO: sysfs path=%s\n", dev_path);
+
+    ret = write_sysfs_int_and_verify("power_state", dev_path, 1);
+    if (ret < 0)
+        return ret;
+
+    ret = write_sysfs_int("in_accel_scale", dev_path, 0);
+    if (ret < 0)
+        return ret;
+    ret = write_sysfs_int("in_anglvel_scale", dev_path, 3);
+    if (ret < 0)
+        return ret;
+    ret = write_sysfs_int("sampling_frequency", dev_path, 200);
+    if (ret < 0)
+        return ret;
+    ret = write_sysfs_int_and_verify("firmware_loaded", dev_path, 0);
+    if (ret < 0)
+        return ret;
+
+    sprintf(dmp_path, "%s/dmp_firmware", dev_path);
+    if ((fd = fopen(dmp_path, "wb")) < 0 ) {
+        perror("dmp fail");
+    }    
+    inv_load_dmp(fd);
+    fclose(fd);
+
+    printf("INFO: firmware_loaded=%d\n",
+           read_sysfs_posint("firmware_loaded", dev_path));
+
+    // set accel offsets
+    //ret = write_sysfs_int_and_verify("in_accel_x_offset", 
+    //                                 dev_path, 0xabcd0000);
+    //if (ret < 0)
+    //    return ret;
+    //ret = write_sysfs_int_and_verify("in_accel_y_offset", 
+    //                                 dev_path, 0xffff0000);
+    //if (ret < 0)
+    //    return ret;
+    //ret = write_sysfs_int_and_verify("in_accel_z_offset", 
+    //                                 dev_path, 0xcdef0000);
+    //if (ret < 0)
+    //    return ret;
+
+    ret = write_sysfs_int_and_verify("dmp_on", dev_path, 1);
+    if (ret < 0)
+        return ret;
+    ret = write_sysfs_int_and_verify("dmp_int_on", dev_path, 1);
+    if (ret < 0)
+        return ret;
+
+    /* select which event to enable and interrupt on/off here */
+    if (test_flick) {
+        ret = enable_flick(dev_path, 1);
+        if (ret < 0)
+            return ret;
+    }
+
+    /*
+    ret = write_sysfs_int_and_verify("tap_on", dev_path, 1);
+    if (ret < 0)
+        return ret;
+    */
+
+    /*ret = write_sysfs_int_and_verify("display_orientation_on",
+                                     dev_path, 1);
+    if (ret < 0)
+        return ret;*/
+    if (test_orientation) {
+        ret = write_sysfs_int_and_verify("orientation_on", dev_path, 1);
+        if (ret < 0)
+            return ret;
+    }
+   /* ret = write_sysfs_int_and_verify("dmp_output_rate", dev_path, 25);
+    if (ret < 0)
+        return ret;*/
+    ret = write_sysfs_int_and_verify("dmp_event_int_on", dev_path, p_event);
+    if (ret < 0)
+        return ret;
+
+    //verify_img(dmp_path);
+    return 0;
+}
+
+/*
+    Handlers for the gestures
+*/
+
+void handle_flick(int flick)
+{
+    printf("flick=%x\n", flick);
+}
+
+void handle_display_orientation(int orient)
+{
+    printf("display_orientation=%x\n", orient);
+}
+
+void handle_motion(int motion)
+{
+    printf("motion=%x\n", motion);
+}
+
+void handle_orientation(int orient)
+{
+    printf("orientation=");
+    if (orient & 0x01)
+        printf("+X, ");
+    if (orient & 0x02) 
+        printf("-X, ");
+    if (orient & 0x04) 
+        printf("+Y, ");
+    if (orient & 0x08) 
+        printf("-Y, ");
+    if (orient & 0x10) 
+        printf("+Z, ");
+    if (orient & 0x20) 
+        printf("-Z, ");
+    if (orient & 0x40)
+        printf("flip");
+    printf("\n");
+}
+
+void handle_tap(int tap)
+{
+    int tap_dir = tap / 8;
+    int tap_num = tap % 8 + 1;
+
+    printf("tap=");
+    switch (tap_dir) {
+        case 1:
+            printf("+X, ");
+            break;
+        case 2:
+            printf("-X, ");
+            break;
+        case 3:
+            printf("+Y, ");
+            break;
+        case 4:
+            printf("-Y, ");
+            break;
+        case 5:
+            printf("+Z, ");
+            break;
+        case 6:
+            printf("-Z, ");
+            break;
+        default:
+            break;
+    }
+    printf("#%d\n", tap_num);
+}
+
+int handle_pedometer(int *got_event)
+{
+    static int last_pedometer_steps = -1;
+    static long last_pedometer_time = -1;
+    static unsigned long last_pedometer_poll = 0L;
+    static unsigned long pedometer_poll_timeout = 500L; // .5 second
+
+    unsigned long now;
+    int pedometer_steps;
+    long pedometer_time;
+
+#ifdef DEBUG_PRINT
+    printf("GT:Pedometer Handler\n");
+#endif
+
+    if ((now = inv_get_tick_count()) - last_pedometer_poll
+            < pedometer_poll_timeout) {
+        return 0;
+    }
+    last_pedometer_poll = now;
+
+    pedometer_steps = read_sysfs_posint("pedometer_steps", dev_dir_name);
+    pedometer_time = read_sysfs_posint("pedometer_time", dev_dir_name);
+
+    if (last_pedometer_steps == -1 && last_pedometer_time == -1) {
+        if (!*got_event)
+            printf("\n");
+        printf("p> pedometer=%d, %ld ",
+               pedometer_steps, pedometer_time);
+        if (pedometer_steps > 10 
+                || pedometer_time > (pedometer_poll_timeout * 2))
+            printf("(resumed)\n");
+        else
+            printf("\n");
+        *got_event = true;
+    } else if (last_pedometer_steps != pedometer_steps
+                    || last_pedometer_time != pedometer_time) {
+        if (!*got_event)
+            printf("\n");
+        printf("p> pedometer=%d, %ld\n", 
+               pedometer_steps, pedometer_time);
+        *got_event = true;
+    }
+
+    last_pedometer_steps = pedometer_steps;
+    last_pedometer_time = pedometer_time;
+
+    return 0;
+}
+
+/*
+    Main processing functions
+*/
+
+void dump_dmp_event_struct(void)
+{
+#define VARVAL(f, v) printf("\t%s : " f "\n", #v, v);
+    int i;
+
+    printf("dmp_feat structure content:\n");
+    for (i = 0; i < FEAT_NUM; i++) {
+        printf("%d - ", i);
+        VARVAL("%d", dmp_feat[i].enabled);
+        VARVAL("%s", dmp_feat[i].sysfs_name);
+        VARVAL("%p", dmp_feat[i].phandler);
+        VARVAL("%p", dmp_feat[i].pollfd);
+        if (dmp_feat[i].pollfd) {
+            VARVAL("%d", dmp_feat[i].pollfd->events);
+            VARVAL("%d", dmp_feat[i].pollfd->revents);
+            VARVAL("%d", dmp_feat[i].pollfd->fd);
+        }
+    }
+    printf("dmp_feat structure content:\n");
+    for (i = 0; i < FEAT_NUM; i++) {
+        printf("%d - ", i);
+        VARVAL("%d", pollfds[i].fd);
+        VARVAL("%d", pollfds[i].events);
+        VARVAL("%d", pollfds[i].revents);
+    }
+    printf("end.\n");
+}
+
+void init_dmp_event_fds(void)
+{
+    int i, j = 0;
+    char file_name[100];
+
+    for (i = 0; i < FEAT_NUM; i++) {
+        if (!dmp_feat[i].enabled)
+            continue;
+        sprintf(file_name, "%s/%s", dev_dir_name, dmp_feat[i].sysfs_name);
+        pollfds[j].fd = open(file_name, O_RDONLY | O_NONBLOCK);
+        if (pollfds[j].fd < 0) {
+            printf("Err: cannot open requested event file '%s'\n", file_name);
+        } else {
+            printf("INFO: opened event node '%s'\n", file_name);
+        }
+        pollfds[j].events = POLLPRI | POLLERR;
+        pollfds[j].revents = 0;
+
+        dmp_feat[i].pollfd = &pollfds[j];
+        j++;
+    }
+}
+
+void close_dmp_event_fds(void)
+{
+    int i;
+    for (i = 0; i < pollfds_used; i++)
+        close(pollfds[i].fd);
+}
+
+void poll_dmp_event_fds(void)
+{
+    int i;
+    char d[4];
+    static int got_event = 1;
+
+    // read the pollable fds
+    for (i = 0; i < pollfds_used; i++)
+        read(pollfds[i].fd, d, 4);
+
+    // poll
+    if (got_event)
+        printf("e> ");
+    got_event = false;
+    poll(pollfds, pollfds_used, POLL_TIME);
+
+    for (i = 0; i < FEAT_NUM; i++) {
+        if (!dmp_feat[i].enabled)
+            continue;
+
+        if (dmp_feat[i].pollfd->revents != 0) {
+            char file_name[200];
+            int data;
+
+            sprintf(file_name, "%s/%s",
+                    dev_dir_name, dmp_feat[i].sysfs_name);
+            FILE *fp = fopen(file_name, "rt");
+            if (!fp) {
+                printf("Err:cannot open requested event file '%s'\n",
+                       dmp_feat[i].sysfs_name);
+                continue;
+            }
+            fscanf(fp, "%d\n", &data);
+            fclose(fp);
+            dmp_feat[i].pollfd->revents = 0;
+
+            dmp_feat[i].phandler(data);
+            got_event = true;
+        }
+    }
+
+    if (test_pedometer) {
+        /* pedometer is not event based, therefore we poll using a timer every
+           pedometer_poll_timeout milliseconds */
+        handle_pedometer(&got_event);
+    }
+}
+
+/*
+    Main
+*/
+
+int main(int argc, char **argv)
+{
+    unsigned long num_loops = 2;
+    unsigned long timedelay = 100000;
+    unsigned long buf_len = 128;
+
+    int ret, c, i, j, toread;
+    int fp;
+
+    int num_channels;
+    char *trigger_name = NULL;
+
+    int datardytrigger = 1;
+    char *data;
+    int read_size;
+    int dev_num, trig_num;
+    char *buffer_access;
+    int scan_size;
+    int noevents = 0;
+    int p_event = 0, nodmp = 0;
+    char *dummy;
+    char chip_name[10];
+    char device_name[10];
+    char sysfs[100];
+
+    struct iio_channel_info *infoarray;
+
+    // all output to stdout must be delivered immediately, no buffering
+    setvbuf(stdout, NULL, _IONBF, 0);
+
+    // get info about the device and driver
+    inv_get_sysfs_path(sysfs);
+    if (inv_get_chip_name(chip_name) != INV_SUCCESS) {
+        printf("get chip name fail\n");
+        exit(0);
+    }
+    printf("INFO: chip_name=%s\n", chip_name);
+
+    for (i = 0; i < strlen(chip_name); i++)
+        device_name[i] = tolower(chip_name[i]);
+    device_name[strlen(chip_name)] = '\0';
+    printf("INFO: device name=%s\n", device_name);
+
+    /* parse the command line parameters 
+      -r means no DMP is enabled (raw) -> should be used for mpu3050.
+       -p means no print of data 
+       when using -p, 1 means orientation, 2 means tap, 3 means flick */
+    while ((c = getopt(argc, argv, "l:w:c:premavt:")) != -1) {
+        switch (c) {
+        case 't':
+            trigger_name = optarg;
+            datardytrigger = 0;
+            break;
+        case 'e':
+            noevents = 1;
+            break;
+        case 'p':
+            p_event = 1;
+            break;
+        case 'r':
+            nodmp = 1;
+            break;
+        case 'c':
+            num_loops = strtoul(optarg, &dummy, 10);
+            break;
+        case 'w':
+            timedelay = strtoul(optarg, &dummy, 10);
+            break;
+        case 'l':
+            buf_len = strtoul(optarg, &dummy, 10);
+            break;
+        case 'm':
+            test_motion = true;
+            break;
+        case 'a':
+            accel_only = true;
+            break;
+        case 'v':
+            verbose = true;
+            break;
+        case '?':
+            return -1;
+        }
+    }
+
+    pollfds_used = 0;
+
+    // comment out/remove/if(0) the block corresponding to the feature 
+    //  that you want to disable
+
+    if (0) {
+        struct dmp_feat_t f = {
+            true, 
+            NULL, 
+            "event_tap",
+            handle_tap
+        };
+        dmp_feat[pollfds_used] = f;
+        pollfds_used++;
+    }
+    if (test_orientation) {
+        struct dmp_feat_t f = {
+            true, 
+            NULL, 
+            "event_orientation",
+            handle_orientation
+        };
+        dmp_feat[pollfds_used] = f;
+        pollfds_used++;
+    }
+    /*if (1) {
+        struct dmp_feat_t f = {
+            true, 
+            NULL, 
+            "event_display_orientation",
+            handle_display_orientation
+        };
+        dmp_feat[pollfds_used] = f;
+        pollfds_used++;
+    }*/
+    if (test_motion) {
+        struct dmp_feat_t f = {
+            true,
+            NULL,
+            "event_accel_motion",
+            handle_motion
+        };
+        dmp_feat[pollfds_used] = f;
+        pollfds_used++;
+    }
+    if (test_flick) {
+        struct dmp_feat_t f = {
+            true, 
+            NULL, 
+            "event_flick", 
+            handle_flick
+        };
+        dmp_feat[pollfds_used] = f;
+        pollfds_used++;
+    }
+
+    // debug
+    printf("INFO\n");
+    printf("INFO: Configured features:\n");
+    for (i = 0; i < pollfds_used; i++)
+        printf("INFO:   %d -> %s\n", i, dmp_feat[i].sysfs_name);
+    printf("INFO\n");
+
+    /* Find the device requested */
+    dev_num = find_type_by_name(device_name, "iio:device");
+    if (dev_num < 0) {
+        printf("Failed to find the %s\n", device_name);
+        ret = -ENODEV;
+        goto error_ret;
+    }
+    printf("INFO: iio device number=%d\n", dev_num);
+    asprintf(&dev_dir_name, "%siio:device%d", iio_dir, dev_num);
+    if (trigger_name == NULL) {
+        /*
+         * Build the trigger name. If it is device associated it's
+         * name is <device_name>_dev[n] where n matches the device
+         * number found above
+         */
+        ret = asprintf(&trigger_name, "%s-dev%d", device_name, dev_num);
+        if (ret < 0) {
+            ret = -ENOMEM;
+            goto error_ret;
+        }
+    }
+
+   ret = write_sysfs_int_and_verify("master_enable", dev_dir_name, 0);
+    if (ret < 0)
+        return ret;
+    ret = write_sysfs_int_and_verify("buffer/enable", dev_dir_name, 0);
+    if (ret < 0)
+        return ret;
+    ret = write_sysfs_int_and_verify("power_state", dev_dir_name, 1);
+
+    //
+    //  motion interrupt in low power accel mode
+    //
+    if (test_motion) {
+        ret = write_sysfs_int_and_verify("motion_lpa_on", dev_dir_name, 1);
+        if (ret < 0)
+            return ret;
+        // magnitude threshold - range [0, 1020] in 32 mg increments
+        ret = write_sysfs_int_and_verify("motion_lpa_threshold", dev_dir_name, 
+                                         3 * 32);
+        if (ret < 0)
+            return ret;
+        // duration in ms up to 2^16
+      //  ret = write_sysfs_int_and_verify("motion_lpa_dur", dev_dir_name, 
+        //                                 200 * 1);
+        //if (ret < 0)
+          //  return ret;
+        // motion_lpa_freq: 0 for 1.25, 1 for 5, 2 for 20, 3 for 40 Hz update rate 
+        //  of the low power accel mode. 
+        //  The higher the rate, the better responsiveness of the motion interrupt.
+        ret = write_sysfs_int("motion_lpa_freq", dev_dir_name, 2);
+        if (ret < 0)
+            return ret;
+    } else {
+        ret = write_sysfs_int_and_verify("motion_lpa_on", dev_dir_name, 0);
+        if (ret < 0)
+            return ret;
+    }
+
+    /* Verify the trigger exists */
+    trig_num = find_type_by_name(trigger_name, "trigger");
+    if (trig_num < 0) {
+        printf("Failed to find the trigger %s\n", trigger_name);
+        ret = -ENODEV;
+        goto error_free_triggername;
+    }
+    printf("INFO: iio trigger number=%d\n", trig_num);
+
+    if (!nodmp)
+        setup_dmp(dev_dir_name, p_event);
+
+    /*
+     * Construct the directory name for the associated buffer.
+     * As we know that the lis3l02dq has only one buffer this may
+     * be built rather than found.
+     */
+    ret = asprintf(&buf_dir_name, "%siio:device%d/buffer", iio_dir, dev_num);
+    if (ret < 0) {
+        ret = -ENOMEM;
+        goto error_free_triggername;
+    }
+
+    /* Set the device trigger to be the data rdy trigger found above */
+    ret = write_sysfs_string_and_verify("trigger/current_trigger",
+                    dev_dir_name,
+                    trigger_name);
+    if (ret < 0) {
+        printf("Failed to write current_trigger file\n");
+        goto error_free_buf_dir_name;
+    }
+
+    /* Setup ring buffer parameters
+       length must be even number because iio_store_to_sw_ring is expecting 
+       half pointer to be equal to the read pointer, which is impossible
+       when buflen is odd number. This is actually a bug in the code */
+    ret = write_sysfs_int("length", buf_dir_name, buf_len * 2);
+    if (ret < 0)
+        goto exit_here;
+
+    // gyro
+    if (accel_only) {
+        ret = enable_anglvel_se(dev_dir_name, &infoarray, &num_channels, 0);
+        if (ret < 0)
+            return ret;
+        ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 0);
+        if (ret < 0)
+            return ret;
+    } else {
+        ret = enable_anglvel_se(dev_dir_name, &infoarray, &num_channels, 1);
+        if (ret < 0)
+            return ret;
+        ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 1);
+        if (ret < 0)
+            return ret;
+    }
+
+    // accel
+    ret = enable_accel_se(dev_dir_name, &infoarray, &num_channels, 1);
+    if (ret < 0)
+        return ret;
+    ret = write_sysfs_int_and_verify("accel_enable", dev_dir_name, 1);
+    if (ret < 0)
+        return ret;
+
+    // quaternion
+    if (!nodmp) {
+        ret = enable_quaternion_se(dev_dir_name, &infoarray, &num_channels, 1);
+        if (ret < 0)
+            return ret;
+        ret = write_sysfs_int_and_verify("three_axes_q_on", dev_dir_name, 1);
+        if (ret < 0)
+            return ret;
+    } else {
+        ret = enable_quaternion_se(dev_dir_name, &infoarray, &num_channels, 0);
+        if (ret < 0)
+            return ret;
+        ret = write_sysfs_int_and_verify("dmp_on", dev_dir_name, 0);
+        if (ret < 0)
+            return ret;
+    }
+
+    //sprintf(dmp_path, "%s/dmp_firmware", dev_dir_name);
+    //verify_img(dmp_path);
+
+    ret = build_channel_array(dev_dir_name, &infoarray, &num_channels);
+    if (ret) {
+        printf("Problem reading scan element information\n");
+        goto exit_here;
+    }
+
+    /* enable the buffer */
+    ret = write_sysfs_int_and_verify("enable", buf_dir_name, 1);
+    if (ret < 0)
+        goto exit_here;
+    scan_size = size_from_channelarray(infoarray, num_channels);
+    data = malloc(scan_size * buf_len);
+    if (!data) {
+        ret = -ENOMEM;
+        goto exit_here;
+    }
+/*ADDED*/
+   ret = write_sysfs_int_and_verify("master_enable", dev_dir_name, 1);
+    if (ret < 0)
+        return ret;
+    if (p_event) {
+
+        /* polling events from the DMP */
+        init_dmp_event_fds();
+        while(!_kbhit())
+            poll_dmp_event_fds();
+        close_dmp_event_fds();
+
+    } else {
+
+        /* attempt to open non blocking the access dev */
+        ret = asprintf(&buffer_access, "/dev/iio:device%d", dev_num);
+        if (ret < 0) {
+            ret = -ENOMEM;
+            goto error_free_data;
+        }
+        fp = open(buffer_access, O_RDONLY | O_NONBLOCK);
+        if (fp == -1) { /*If it isn't there make the node */
+            printf("Failed to open %s\n", buffer_access);
+            ret = -errno;
+            goto error_free_buffer_access;
+        }
+        /* wait for events num_loops times */
+        for (j = 0; j < num_loops; j++) {
+            if (!noevents) {
+                struct pollfd pfd = {
+                    .fd = fp,
+                    .events = POLLIN,
+                };
+                poll(&pfd, 1, -1);
+                toread = 1;
+                if (j % 128 == 0)
+                    usleep(timedelay);
+
+            } else {
+                usleep(timedelay);
+                toread = 1;
+            }
+            read_size = read(fp, data, toread * scan_size);
+            if (read_size == -EAGAIN) {
+                printf("nothing available\n");
+                continue;
+            }
+            if (!p_event) {
+                for (i = 0; i < read_size / scan_size; i++)
+                    process_scan(data + scan_size * i, infoarray, num_channels);
+            }
+        }
+        close(fp);
+    }
+
+error_free_buffer_access:
+    free(buffer_access);
+error_free_data:
+    free(data);
+exit_here:
+    /* stop the ring buffer */
+    ret = write_sysfs_int_and_verify("enable", buf_dir_name, 0);
+    /* disable the dmp */
+    if (p_event)
+        ret = write_sysfs_int_and_verify("dmp_on", dev_dir_name, 0);
+
+error_free_buf_dir_name:
+    free(buf_dir_name);
+error_free_triggername:
+    if (datardytrigger)
+        free(trigger_name);
+error_ret:
+    return ret;
+}
diff --git a/6515/libsensors_iio/software/simple_apps/playback/linux/and_constructor.c b/6515/libsensors_iio/software/simple_apps/playback/linux/and_constructor.c
new file mode 100644
index 0000000..c4f09a6
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/playback/linux/and_constructor.c
@@ -0,0 +1,379 @@
+/*
+ $License:
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+/*
+    Includes, Defines, and Macros
+*/
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 0 /* turn to 0 to enable verbose logging */
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-playback"
+
+#include "and_constructor.h"
+#include "mlos.h"
+#include "invensense.h"
+#include "invensense_adv.h"
+
+/*
+    Typedef
+*/
+struct inv_construct_t {
+    int product; /**< Gyro Product Number */
+    int debug_mode;
+    int last_mode;
+    FILE *file;
+    int dmp_version;
+    int left_in_buffer;
+#define FIFO_READ_SIZE 100
+    unsigned char fifo_data[FIFO_READ_SIZE];
+    int gyro_enable;
+    int accel_enable;
+    int compass_enable;
+    int quat_enable;
+};
+
+/*
+    Globals
+*/
+static struct inv_construct_t inv_construct = {0};
+static void (*s_func_cb)(void);
+static char playback_filename[101] = "/data/playback.bin";
+struct fifo_dmp_config fifo_dmp_cfg = {0};
+
+/*
+    Functions
+*/
+void inv_set_playback_filename(char *filename, int length)
+{
+    if (length > 100) {
+        MPL_LOGE("Error : file name and path too long, 100 characters limit\n");
+        return;
+    }
+    strncpy(playback_filename, filename, length);
+}
+
+inv_error_t inv_constructor_setup(void)
+{
+    unsigned short orient;
+    extern signed char g_gyro_orientation[9];
+    extern signed char g_accel_orientation[9];
+    extern signed char g_compass_orientation[9];
+    float scale = 2.f;
+    long sens;
+
+    // gyro setup
+    orient = inv_orientation_matrix_to_scalar(g_gyro_orientation);
+    inv_set_gyro_orientation_and_scale(orient, 2000L << 15);
+
+    // accel setup
+    orient = inv_orientation_matrix_to_scalar(g_accel_orientation);
+    scale = 2.f;
+    sens = (long)(scale * (1L << 15));
+    inv_set_accel_orientation_and_scale(orient, sens);
+
+    // compass setup
+    orient = inv_orientation_matrix_to_scalar(g_compass_orientation);
+    // scale is the max value of the compass in micro Tesla.
+    scale = 5000.f;
+    sens = (long)(scale * (1L << 15));
+    inv_set_compass_orientation_and_scale(orient, sens);
+
+    return INV_SUCCESS;
+}
+
+inv_error_t inv_set_fifo_processed_callback(void (*func_cb)(void))
+{
+    s_func_cb = func_cb;
+    return INV_SUCCESS;
+}
+
+void int32_to_long(int32_t in[], long out[], int length)
+{
+    int ii;
+    for (ii = 0; ii < length; ii++)
+        out[ii] = (long)in[ii];
+}
+
+inv_error_t inv_playback(void)
+{
+    inv_rd_dbg_states type;
+    inv_time_t ts;
+    int32_t buffer[4];
+    short gyro[3];
+    size_t r = 1;
+    int32_t orientation;
+    int32_t sensitivity, sample_rate_us = 0;
+
+    // Check to make sure we were request to playback
+    if (inv_construct.debug_mode != RD_PLAYBACK) {
+        MPL_LOGE("%s|%s|%d error: debug_mode != RD_PLAYBACK\n",
+                 __FILE__, __func__, __LINE__);
+        return INV_ERROR;
+    }
+
+    if (inv_construct.file == NULL) {
+        inv_construct.file = fopen(playback_filename, "rb");
+        if (!inv_construct.file) {
+            MPL_LOGE("Error : cannot find or open playback file '%s'\n",
+                     playback_filename);
+            return INV_ERROR_FILE_OPEN;
+        }
+    }
+
+    while (1) {
+        r = fread(&type, sizeof(type), 1, inv_construct.file);
+        if (r == 0) {
+            MPL_LOGV("read 0 bytes, PLAYBACK file closed\n");
+            inv_construct.debug_mode = RD_NO_DEBUG;
+            fclose(inv_construct.file);
+            break;
+        }
+        //MPL_LOGV("TYPE : %d, %d\n", type);
+        switch (type) {
+        case PLAYBACK_DBG_TYPE_GYRO:
+            r = fread(gyro, sizeof(gyro[0]), 3, inv_construct.file);
+            r = fread(&ts, sizeof(ts), 1, inv_construct.file);
+            inv_build_gyro(gyro, ts);
+            MPL_LOGV("PLAYBACK_DBG_TYPE_GYRO, %+d, %+d, %+d, %+lld\n",
+                     gyro[0], gyro[1], gyro[2], ts);
+            break;
+        case PLAYBACK_DBG_TYPE_ACCEL:
+        {
+            long accel[3];
+            r = fread(buffer, sizeof(buffer[0]), 3, inv_construct.file);
+            r = fread(&ts, sizeof(ts), 1, inv_construct.file);
+            int32_to_long(buffer, accel, 3);
+            inv_build_accel(accel, 0, ts);
+            MPL_LOGV("PLAYBACK_DBG_TYPE_ACCEL, %+d, %+d, %+d, %lld\n",
+                     buffer[0], buffer[1], buffer[2], ts);
+            break;
+        }
+        case PLAYBACK_DBG_TYPE_COMPASS:
+        {
+            long compass[3];
+            r = fread(buffer, sizeof(buffer[0]), 3, inv_construct.file);
+            r = fread(&ts, sizeof(ts), 1, inv_construct.file);
+            int32_to_long(buffer, compass, 3);
+            inv_build_compass(compass, 0, ts);
+            MPL_LOGV("PLAYBACK_DBG_TYPE_COMPASS, %+d, %+d, %+d, %lld\n",
+                     buffer[0], buffer[1], buffer[2], ts);
+            break;
+        }
+        case PLAYBACK_DBG_TYPE_TEMPERATURE:
+            r = fread(buffer, sizeof(buffer[0]), 1, inv_construct.file);
+            r = fread(&ts, sizeof(ts), 1, inv_construct.file);
+            inv_build_temp(buffer[0], ts);
+            MPL_LOGV("PLAYBACK_DBG_TYPE_TEMPERATURE, %+d, %lld\n",
+                     buffer[0], ts);
+            break;
+        case PLAYBACK_DBG_TYPE_QUAT:
+        {
+            long quat[4];
+            r = fread(buffer, sizeof(buffer[0]), 4, inv_construct.file);
+            r = fread(&ts, sizeof(ts), 1, inv_construct.file);
+            int32_to_long(buffer, quat, 4);
+            inv_build_quat(quat, INV_BIAS_APPLIED, ts);
+            MPL_LOGV("PLAYBACK_DBG_TYPE_QUAT, %+d, %+d, %+d, %+d, %lld\n",
+                     buffer[0], buffer[1], buffer[2], buffer[3], ts);
+            break;
+        }
+        case PLAYBACK_DBG_TYPE_EXECUTE:
+            MPL_LOGV("PLAYBACK_DBG_TYPE_EXECUTE\n");
+            inv_execute_on_data();
+            if (s_func_cb)
+                s_func_cb();
+            //done = 1;
+            break;
+
+        case PLAYBACK_DBG_TYPE_G_ORIENT:
+            MPL_LOGV("PLAYBACK_DBG_TYPE_G_ORIENT\n");
+            r = fread(&orientation, sizeof(orientation), 1, inv_construct.file);
+            r = fread(&sensitivity, sizeof(sensitivity), 1, inv_construct.file);
+            inv_set_gyro_orientation_and_scale(orientation, sensitivity);
+            break;
+        case PLAYBACK_DBG_TYPE_A_ORIENT:
+            MPL_LOGV("PLAYBACK_DBG_TYPE_A_ORIENT\n");
+            r = fread(&orientation, sizeof(orientation), 1, inv_construct.file);
+            r = fread(&sensitivity, sizeof(sensitivity), 1, inv_construct.file);
+            inv_set_accel_orientation_and_scale(orientation, sensitivity);
+            break;
+        case PLAYBACK_DBG_TYPE_C_ORIENT:
+            MPL_LOGV("PLAYBACK_DBG_TYPE_C_ORIENT\n");
+            r = fread(&orientation, sizeof(orientation), 1, inv_construct.file);
+            r = fread(&sensitivity, sizeof(sensitivity), 1, inv_construct.file);
+            inv_set_compass_orientation_and_scale(orientation, sensitivity);
+            break;
+
+        case PLAYBACK_DBG_TYPE_G_SAMPLE_RATE:
+            r = fread(&sample_rate_us, sizeof(sample_rate_us), 
+                      1, inv_construct.file);
+            inv_set_gyro_sample_rate(sample_rate_us);
+            MPL_LOGV("PLAYBACK_DBG_TYPE_G_SAMPLE_RATE => %d\n",
+                     sample_rate_us);
+            break;
+        case PLAYBACK_DBG_TYPE_A_SAMPLE_RATE:
+            r = fread(&sample_rate_us, sizeof(sample_rate_us), 
+                      1, inv_construct.file);
+            inv_set_accel_sample_rate(sample_rate_us);
+            MPL_LOGV("PLAYBACK_DBG_TYPE_A_SAMPLE_RATE => %d\n",
+                     sample_rate_us);
+            break;
+        case PLAYBACK_DBG_TYPE_C_SAMPLE_RATE:
+            r = fread(&sample_rate_us, sizeof(sample_rate_us), 
+                      1, inv_construct.file);
+            inv_set_compass_sample_rate(sample_rate_us);
+            MPL_LOGV("PLAYBACK_DBG_TYPE_C_SAMPLE_RATE => %d\n",
+                     sample_rate_us);
+            break;
+
+        case PLAYBACK_DBG_TYPE_GYRO_OFF:
+            MPL_LOGV("PLAYBACK_DBG_TYPE_GYRO_OFF\n");
+            inv_gyro_was_turned_off();
+            break;
+        case PLAYBACK_DBG_TYPE_ACCEL_OFF:
+            MPL_LOGV("PLAYBACK_DBG_TYPE_ACCEL_OFF\n");
+            inv_accel_was_turned_off();
+            break;
+        case PLAYBACK_DBG_TYPE_COMPASS_OFF:
+            MPL_LOGV("PLAYBACK_DBG_TYPE_COMPASS_OFF\n");
+            inv_compass_was_turned_off();
+            break;
+        case PLAYBACK_DBG_TYPE_QUAT_OFF:
+            MPL_LOGV("PLAYBACK_DBG_TYPE_QUAT_OFF\n");
+            inv_quaternion_sensor_was_turned_off();
+            break;
+
+        case PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE:
+            MPL_LOGV("PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE\n");
+            r = fread(&sample_rate_us, sizeof(sample_rate_us), 
+                      1, inv_construct.file);
+            inv_set_quat_sample_rate(sample_rate_us);
+            break;
+        default:
+            //MPL_LOGV("PLAYBACK file closed\n");
+            fclose(inv_construct.file);
+            MPL_LOGE("%s|%s|%d error: unrecognized log type '%d', "
+                     "PLAYBACK file closed\n",
+                     __FILE__, __func__, __LINE__, type);
+            return INV_ERROR;
+        }
+    }
+    msleep(1);
+
+    inv_construct.debug_mode = RD_NO_DEBUG;
+    fclose(inv_construct.file);
+
+    return INV_SUCCESS;
+}
+
+/** Turns on/off playback and record modes
+* @param mode Turn on recording mode with RD_RECORD and turn off recording mode with
+*             RD_NO_DBG. Turn on playback mode with RD_PLAYBACK.
+*/
+void inv_set_debug_mode(rd_dbg_mode mode)
+{
+#ifdef INV_PLAYBACK_DBG
+    inv_construct.debug_mode = mode;
+#endif
+}
+
+inv_error_t inv_constructor_start(void)
+{
+    inv_error_t result;
+    unsigned char divider;
+    //int gest_enabled = inv_get_gesture_enable();
+
+    // start the software
+    result = inv_start_mpl();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    
+    /*
+    if (inv_construct.dmp_version == WIN8_DMP_VERSION) {
+        int fifo_divider;
+        divider = 4; // 4 means 200Hz DMP
+        fifo_divider = 3;
+        // Set Gyro Sample Rate in MPL in micro seconds
+        inv_set_gyro_sample_rate(1000L*(divider+1)*(fifo_divider+1));
+
+        // Set Gyro Sample Rate in MPL in micro seconds
+        inv_set_quat_sample_rate(1000L*(divider+1)*(fifo_divider+1));
+
+        // Set Compass Sample Rate in MPL in micro seconds
+        inv_set_compass_sample_rate(1000L*(divider+1)*(fifo_divider+1));
+
+        // Set Accel Sample Rate in MPL in micro seconds
+        inv_set_accel_sample_rate(1000L*(divider+1)*(fifo_divider+1));
+    } else if (gest_enabled) {
+        int fifo_divider;
+        unsigned char mpl_divider;
+
+        inv_send_interrupt_word();
+        inv_send_sensor_data(INV_ALL & INV_GYRO_ACC_MASK);
+        inv_send_quaternion();
+
+        divider = fifo_dmp_cfg.sample_divider;
+        mpl_divider = fifo_dmp_cfg.mpl_divider;
+
+        // Set Gyro Sample Rate in MPL in micro seconds
+        inv_set_gyro_sample_rate(1000L*(mpl_divider+1));
+
+        // Set Gyro Sample Rate in MPL in micro seconds
+        inv_set_quat_sample_rate(1000L*(mpl_divider+1));
+
+        // Set Compass Sample Rate in MPL in micro seconds
+        inv_set_compass_sample_rate(1000L*(mpl_divider+1));
+
+        // Set Accel Sample Rate in MPL in micro seconds
+        inv_set_accel_sample_rate(1000L*(mpl_divider+1));
+    } else 
+    */
+    {
+        divider = 9;
+        // set gyro sample sate in MPL in micro seconds
+        inv_set_gyro_sample_rate(1000L*(divider+1));
+        // set compass sample rate in MPL in micro seconds
+        inv_set_compass_sample_rate(1000L*(divider+1));
+        // set accel sample rate in MPL in micro seconds
+        inv_set_accel_sample_rate(1000L*(divider+1));
+    }
+
+    // setup the scale factors and orientations and other parameters
+    result = inv_constructor_setup();
+
+    return result;
+}
+
+inv_error_t inv_constructor_default_enable()
+{
+    INV_ERROR_CHECK(inv_enable_quaternion());
+    INV_ERROR_CHECK(inv_enable_fast_nomot());
+    INV_ERROR_CHECK(inv_enable_heading_from_gyro());
+    INV_ERROR_CHECK(inv_enable_compass_bias_w_gyro());
+    INV_ERROR_CHECK(inv_enable_hal_outputs());
+    INV_ERROR_CHECK(inv_enable_vector_compass_cal());
+    INV_ERROR_CHECK(inv_enable_9x_sensor_fusion());
+    INV_ERROR_CHECK(inv_enable_gyro_tc());
+    INV_ERROR_CHECK(inv_enable_no_gyro_fusion());
+    INV_ERROR_CHECK(inv_enable_in_use_auto_calibration());
+    INV_ERROR_CHECK(inv_enable_magnetic_disturbance());
+    return INV_SUCCESS;
+}
+
+/**
+ * @}
+ */
diff --git a/6515/libsensors_iio/software/simple_apps/playback/linux/and_constructor.h b/6515/libsensors_iio/software/simple_apps/playback/linux/and_constructor.h
new file mode 100644
index 0000000..5905037
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/playback/linux/and_constructor.h
@@ -0,0 +1,62 @@
+/*
+ $License:
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+#ifndef INV_CONSTRUCTOR_H__
+#define INV_CONSTRUCTOR_H__
+
+#include "mltypes.h"
+#include "data_builder.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define PRECISION 10000.f
+#define RANGE_FLOAT_TO_FIXEDPOINT(range, x) {               \
+    range.mantissa = (long)x;                               \
+    range.fraction = (long)((float)(x-(long)x)*PRECISION);  \
+}
+#define RANGE_FIXEDPOINT_TO_FLOAT(range, x) {   \
+    x = (float)(range.mantissa);                \
+    x += ((float)range.fraction/PRECISION);     \
+}
+
+struct fifo_dmp_config {
+    unsigned char sample_divider;
+    unsigned char fifo_divider;
+    unsigned char mpl_divider;
+};
+
+inv_error_t inv_construct_and_push_data();
+inv_error_t inv_set_fifo_processed_callback(void (*func_cb)(void));
+inv_error_t inv_constructor_setup();
+inv_error_t inv_constructor_start();
+inv_error_t inv_constructor_init();
+inv_error_t inv_constructor_default_enable();
+void inv_set_debug_mode(rd_dbg_mode mode);
+inv_error_t inv_playback();
+void inv_set_playback_filename(char *filename, int length);
+inv_error_t wait_for_and_process_interrupt();
+
+inv_error_t inv_set_interrupt_word(unsigned long word);
+inv_error_t inv_get_interrupt_word(unsigned long *data);
+inv_error_t inv_set_gesture_enable(int word);
+int inv_get_gesture_enable(void);
+inv_error_t inv_set_fifo_rate(unsigned long fifo_rate);
+inv_error_t inv_get_dmp_sample_divider(unsigned char *data);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INVENSENSE_INV_CONSTRUCTOR_H__
+
diff --git a/6515/libsensors_iio/software/simple_apps/playback/linux/build/android/inv_playback-shared b/6515/libsensors_iio/software/simple_apps/playback/linux/build/android/inv_playback-shared
new file mode 100755
index 0000000..9252ad1
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/playback/linux/build/android/inv_playback-shared
diff --git a/6515/libsensors_iio/software/simple_apps/playback/linux/build/android/shared.mk b/6515/libsensors_iio/software/simple_apps/playback/linux/build/android/shared.mk
new file mode 100644
index 0000000..dc33ee3
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/playback/linux/build/android/shared.mk
@@ -0,0 +1,96 @@
+EXEC = inv_playback$(SHARED_APP_SUFFIX)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
+COMP  ?= $(CROSS)gcc
+LINK  ?= $(CROSS)gcc
+
+OBJFOLDER = $(CURDIR)/obj
+
+INV_ROOT   = ../../../../../..
+APP_DIR    = $(CURDIR)/../..
+COMMON_DIR = $(INV_ROOT)/software/simple_apps/common
+MLLITE_DIR = $(INV_ROOT)/software/core/mllite
+MPL_DIR    = $(INV_ROOT)/software/core/mpl
+
+include $(INV_ROOT)/software/build/android/common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += $(ANDROID_COMPILE)
+CFLAGS += -Wall
+CFLAGS += -fpic
+CFLAGS += -nostdlib
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT
+CFLAGS += -DLINUX
+CFLAGS += -DANDROID
+CFLAGS += -mthumb-interwork
+CFLAGS += -fno-exceptions
+CFLAGS += -ffunction-sections
+CFLAGS += -funwind-tables
+CFLAGS += -fstack-protector
+CFLAGS += -fno-short-enums
+CFLAGS += -fmessage-length=0
+CFLAGS += -I$(MLLITE_DIR)
+CFLAGS += -I$(MPL_DIR)
+CFLAGS += -I$(COMMON_DIR)
+CFLAGS += $(INV_INCLUDES)
+CFLAGS += $(INV_DEFINES)
+
+LLINK  = -lc
+LLINK += -lm
+LLINK += -lutils
+LLINK += -lcutils
+LLINK += -lgcc
+LLINK += -ldl
+LLINK += -lstdc++
+LLINK += -llog
+LLINK += -lz
+
+LFLAGS += $(CMDLINE_LFLAGS)
+LFLAGS += $(ANDROID_LINK_EXECUTABLE)
+
+LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+####################################################################################################
+## sources
+
+INV_LIBS  = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT)
+INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
+
+#INV_SOURCES and VPATH provided by Makefile.filelist
+include ../filelist.mk
+
+INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
+INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all clean cleanall install
+
+all: $(EXEC) $(MK_NAME)
+
+$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
+	$(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+
+$(OBJFOLDER) :
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(EXEC) $(OBJFOLDER)
+
+install : $(EXEC)
+	cp -f $(EXEC) $(INSTALL_DIR)
+
+
diff --git a/6515/libsensors_iio/software/simple_apps/playback/linux/build/filelist.mk b/6515/libsensors_iio/software/simple_apps/playback/linux/build/filelist.mk
new file mode 100644
index 0000000..1d04fea
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/playback/linux/build/filelist.mk
@@ -0,0 +1,20 @@
+#### filelist.mk for mpu_iio ####
+
+# headers
+HEADERS += $(APP_DIR)/iio_utils.h
+HEADERS += $(APP_DIR)/and_constructor.h
+HEADERS += $(APP_DIR)/datalogger_outputs.h
+HEADERS += $(COMMON_DIR)/console_helper.h
+HEADERS += $(COMMON_DIR)/mlerrorcode.h
+HEADERS += $(COMMON_DIR)/testsupport.h
+
+# sources
+SOURCES := $(APP_DIR)/main.c
+SOURCES += $(APP_DIR)/and_constructor.c
+SOURCES += $(APP_DIR)/datalogger_outputs.c
+SOURCES += $(COMMON_DIR)/console_helper.c
+SOURCES += $(COMMON_DIR)/mlerrorcode.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH += $(APP_DIR) $(COMMON_DIR)
diff --git a/6515/libsensors_iio/software/simple_apps/playback/linux/datalogger_outputs.c b/6515/libsensors_iio/software/simple_apps/playback/linux/datalogger_outputs.c
new file mode 100644
index 0000000..7c81cbb
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/playback/linux/datalogger_outputs.c
@@ -0,0 +1,384 @@
+/**
+ *   @defgroup  HAL_Outputs
+ *   @brief     Motion Library - HAL Outputs
+ *              Sets up common outputs for HAL
+ *
+ *   @{
+ *       @file  datalogger_outputs.c
+ *       @brief Windows 8 HAL outputs.
+ */
+ 
+#include <string.h>
+
+#include "datalogger_outputs.h"
+#include "ml_math_func.h"
+#include "mlmath.h"
+#include "start_manager.h"
+#include "data_builder.h"
+#include "results_holder.h"
+
+/*
+    Defines
+*/
+#define ACCEL_CONVERSION (0.000149637603759766f)
+
+/*
+    Types
+*/
+struct datalogger_output_s {
+    int quat_accuracy;
+    inv_time_t quat_timestamp;
+    long quat[4];
+    struct inv_sensor_cal_t sc;
+};
+
+/*
+    Globals and Statics
+*/
+static struct datalogger_output_s dl_out;
+
+/*
+    Functions
+*/
+
+/**
+ *  Raw (uncompensated) angular velocity (LSB) in chip frame.
+ *  @param[out] values      raw angular velocity in LSB.
+ *  @param[out] timestamp   Time when sensor was sampled.
+ */
+void inv_get_sensor_type_gyro_raw_short(short *values, inv_time_t *timestamp)
+{
+    struct inv_single_sensor_t *pg = &dl_out.sc.gyro;
+
+    if (values)
+        memcpy(values, &pg->raw, sizeof(short) * 3);
+    if (timestamp)
+        *timestamp = pg->timestamp;
+}
+
+/**
+ *  Raw (uncompensated) angular velocity (degrees per second) in body frame.
+ *  @param[out] values      raw angular velocity in dps.
+ *  @param[out] timestamp   Time when sensor was sampled.
+ */
+void inv_get_sensor_type_gyro_raw_body_float(float *values,
+        inv_time_t *timestamp)
+{
+    struct inv_single_sensor_t *pg = &dl_out.sc.gyro;
+    long raw[3];
+    long raw_body[3];
+
+    raw[0] = (long) pg->raw[0] * (1L << 16);
+    raw[1] = (long) pg->raw[1] * (1L << 16);
+    raw[2] = (long) pg->raw[2] * (1L << 16);
+    inv_convert_to_body_with_scale(pg->orientation, pg->sensitivity,
+                                   raw, raw_body);
+    if (values) {
+        values[0] = inv_q16_to_float(raw_body[0]);
+        values[1] = inv_q16_to_float(raw_body[1]);
+        values[2] = inv_q16_to_float(raw_body[2]);
+    }
+    if (timestamp)
+        *timestamp = pg->timestamp;
+}
+
+/**
+ *  Angular velocity (degrees per second) in body frame.
+ *  @param[out] values      Angular velocity in dps.
+ *  @param[out] accuracy    0 (uncalibrated) to 3 (most accurate).
+ *  @param[out] timestamp   Time when sensor was sampled.
+ */
+void inv_get_sensor_type_gyro_float(float *values, int8_t *accuracy,
+        inv_time_t *timestamp)
+{
+    long gyro[3];
+    inv_get_gyro_set(gyro, accuracy, timestamp);
+
+    values[0] = (float)gyro[0] / 65536.f;
+    values[1] = (float)gyro[1] / 65536.f;
+    values[2] = (float)gyro[2] / 65536.f;
+}
+
+/**
+ *  Raw (uncompensated) acceleration (LSB) in chip frame.
+ *  @param[out] values      raw acceleration in LSB.
+ *  @param[out] timestamp   Time when sensor was sampled.
+ */
+void inv_get_sensor_type_accel_raw_short(short *values, inv_time_t *timestamp)
+{
+    struct inv_single_sensor_t *pa = &dl_out.sc.accel;
+
+    if (values)
+        memcpy(values, &pa->raw, sizeof(short) * 3);
+    if (timestamp)
+        *timestamp = pa->timestamp;
+}
+
+/**
+ *  Acceleration (g's) in body frame.
+ *  Microsoft defines gravity as positive acceleration pointing towards the
+ *  Earth.
+ *  @param[out] values      Acceleration in g's.
+ *  @param[out] accuracy    0 (uncalibrated) to 3 (most accurate).
+ *  @param[out] timestamp   Time when sensor was sampled.
+ */
+void inv_get_sensor_type_accel_float(float *values, int8_t *accuracy,
+        inv_time_t *timestamp)
+{
+    long accel[3];
+    inv_get_accel_set(accel, accuracy, timestamp);
+
+    values[0] = (float) -accel[0] / 65536.f;
+    values[1] = (float) -accel[1] / 65536.f;
+    values[2] = (float) -accel[2] / 65536.f;
+}
+
+/**
+ *  Raw (uncompensated) compass magnetic field  (LSB) in chip frame.
+ *  @param[out] values      raw magnetic field in LSB.
+ *  @param[out] timestamp   Time when sensor was sampled.
+ */
+void inv_get_sensor_type_compass_raw_short(short *values, inv_time_t *timestamp)
+{
+    struct inv_single_sensor_t *pc = &dl_out.sc.compass;
+
+    if (values)
+        memcpy(values, &pc->raw, sizeof(short) * 3);
+    if (timestamp)
+        *timestamp = pc->timestamp;
+}
+
+/**
+ *  Magnetic heading/field strength in body frame.
+ *  TODO: No difference between mag_north and true_north yet.
+ *  @param[out] mag_north   Heading relative to magnetic north in degrees.
+ *  @param[out] true_north  Heading relative to true north in degrees.
+ *  @param[out] values      Field strength in milligauss.
+ *  @param[out] accuracy    0 (uncalibrated) to 3 (most accurate).
+ *  @param[out] timestamp   Time when sensor was sampled.
+ */
+void inv_get_sensor_type_compass_float(float *mag_north, float *true_north,
+        float *values, int8_t *accuracy, inv_time_t *timestamp)
+{
+    long compass[3];
+    long q00, q12, q22, q03, t1, t2;
+
+    /* 1 uT = 10 milligauss. */
+#define COMPASS_CONVERSION  (10 / 65536.f)
+    inv_get_compass_set(compass, accuracy, timestamp);
+    if (values) {
+        values[0] = (float)compass[0]*COMPASS_CONVERSION;
+        values[1] = (float)compass[1]*COMPASS_CONVERSION;
+        values[2] = (float)compass[2]*COMPASS_CONVERSION;
+    }
+
+    /* TODO: Stolen from euler angle computation. Calculate this only once per
+     * callback.
+     */
+    q00 = inv_q29_mult(dl_out.quat[0], dl_out.quat[0]);
+    q12 = inv_q29_mult(dl_out.quat[1], dl_out.quat[2]);
+    q22 = inv_q29_mult(dl_out.quat[2], dl_out.quat[2]);
+    q03 = inv_q29_mult(dl_out.quat[0], dl_out.quat[3]);
+    t1 = q12 - q03;
+    t2 = q22 + q00 - (1L << 30);
+    if (mag_north) {
+        *mag_north = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI;
+        if (*mag_north < 0)
+            *mag_north += 360;
+    }
+    if (true_north) {
+        if (!mag_north) {
+            *true_north = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI;
+            if (*true_north < 0)
+                *true_north += 360;
+        } else {
+            *true_north = *mag_north;
+        }
+    }
+}
+
+#if 0
+// put it back when we handle raw temperature
+/**
+ *  Raw temperature (LSB).
+ *  @param[out] value       raw temperature in LSB (1 element).
+ *  @param[out] timestamp   Time when sensor was sampled.
+ */
+void inv_get_sensor_type_temp_raw_short(short *value, inv_time_t *timestamp)
+{
+    struct inv_single_sensor_t *pt = &dl_out.sc.temp;
+    if (value) {
+        /* no raw temperature, temperature is only handled calibrated
+        *value = pt->raw[0];
+        */
+        *value = pt->calibrated[0];
+    }
+    if (timestamp)
+        *timestamp = pt->timestamp;
+}
+#endif
+
+/**
+ *  Temperature (degree C).
+ *  @param[out] values      Temperature in degrees C.
+ *  @param[out] timestamp   Time when sensor was sampled.
+ */
+void inv_get_sensor_type_temperature_float(float *value, inv_time_t *timestamp)
+{
+    struct inv_single_sensor_t *pt = &dl_out.sc.temp;
+    long ltemp;
+    if (timestamp)
+        *timestamp = pt->timestamp;
+    if (value) {
+        /* no raw temperature, temperature is only handled calibrated
+        ltemp = pt->raw[0];
+        */
+        ltemp = pt->calibrated[0];
+        *value = (float) ltemp / (1L << 16);
+    }
+}
+
+/**
+ *  Quaternion in body frame.
+ *  @e inv_get_sensor_type_quaternion_float outputs the data in the following
+ *  order: X, Y, Z, W.
+ *  TODO: Windows expects a discontinuity at 180 degree rotations. Will our
+ *  convention be ok?
+ *  @param[out] values      Quaternion normalized to one.
+ *  @param[out] accuracy    0 (uncalibrated) to 3 (most accurate).
+ *  @param[out] timestamp   Time when sensor was sampled.
+ */
+void inv_get_sensor_type_quat_float(float *values, int *accuracy,
+                                    inv_time_t *timestamp)
+{
+    values[0] = dl_out.quat[0] / 1073741824.f;
+    values[1] = dl_out.quat[1] / 1073741824.f;
+    values[2] = dl_out.quat[2] / 1073741824.f;
+    values[3] = dl_out.quat[3] / 1073741824.f;
+    accuracy[0] = dl_out.quat_accuracy;
+    timestamp[0] = dl_out.quat_timestamp;
+}
+
+/** Gravity vector (gee) in body frame.
+* @param[out] values Gravity vector in body frame, length 3, (gee)
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate,
+                       while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor. Derived from the
+                        timestamp sent to inv_build_accel().
+*/
+void inv_get_sensor_type_gravity_float(float *values, int *accuracy,
+                                       inv_time_t * timestamp)
+{
+    struct inv_single_sensor_t *pa = &dl_out.sc.accel;
+
+    if (values) {
+        long lgravity[3];
+        (void)inv_get_gravity(lgravity);
+        values[0] = (float) lgravity[0] / (1L << 16);
+        values[1] = (float) lgravity[1] / (1L << 16);
+        values[2] = (float) lgravity[2] / (1L << 16);
+    }
+    if (accuracy)
+        *accuracy = pa->accuracy;
+    if (timestamp)
+        *timestamp = pa->timestamp;
+}
+
+/**
+* This corresponds to Sensor.TYPE_ROTATION_VECTOR.
+* The rotation vector represents the orientation of the device as a combination
+* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$
+* around an axis {x, y, z}. <br>
+* The three elements of the rotation vector are
+* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)}, such that the magnitude of the rotation
+* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is
+* equal to the direction of the axis of rotation.
+*
+* The three elements of the rotation vector are equal to the last three components of a unit quaternion
+* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)>.
+*
+* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor.
+* The reference coordinate system is defined as a direct orthonormal basis, where:
+
+    -X is defined as the vector product Y.Z (It is tangential to the ground at the device's current location and roughly points East).
+    -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole.
+    -Z points towards the sky and is perpendicular to the ground.
+* @param[out] values
+* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate
+* @param[out] timestamp Timestamp. In (ns) for Android.
+*/
+void inv_get_sensor_type_rotation_vector_float(float *values, int *accuracy,
+        inv_time_t * timestamp)
+{
+    if (accuracy)
+        *accuracy = dl_out.quat_accuracy;
+    if (timestamp)
+        *timestamp = dl_out.quat_timestamp;
+    if (values) {
+        if (dl_out.quat[0] >= 0) {
+            values[0] = dl_out.quat[1] * INV_TWO_POWER_NEG_30;
+            values[1] = dl_out.quat[2] * INV_TWO_POWER_NEG_30;
+            values[2] = dl_out.quat[3] * INV_TWO_POWER_NEG_30;
+        } else {
+            values[0] = -dl_out.quat[1] * INV_TWO_POWER_NEG_30;
+            values[1] = -dl_out.quat[2] * INV_TWO_POWER_NEG_30;
+            values[2] = -dl_out.quat[3] * INV_TWO_POWER_NEG_30;
+        }
+    }
+}
+
+/** Main callback to generate HAL outputs. Typically not called by library users. */
+inv_error_t inv_generate_datalogger_outputs(struct inv_sensor_cal_t *sensor_cal)
+{
+    memcpy(&dl_out.sc, sensor_cal, sizeof(struct inv_sensor_cal_t));
+    inv_get_quaternion_set(dl_out.quat, &dl_out.quat_accuracy,
+                           &dl_out.quat_timestamp);
+    return INV_SUCCESS;
+}
+
+/** Turns off generation of HAL outputs. */
+inv_error_t inv_stop_datalogger_outputs(void)
+{
+    return inv_unregister_data_cb(inv_generate_datalogger_outputs);
+}
+
+/** Turns on generation of HAL outputs. This should be called after inv_stop_dl_outputs()
+* to turn generation of HAL outputs back on. It is automatically called by inv_enable_dl_outputs().*/
+inv_error_t inv_start_datalogger_outputs(void)
+{
+    return inv_register_data_cb(inv_generate_datalogger_outputs,
+        INV_PRIORITY_HAL_OUTPUTS, INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW);
+}
+
+/** Initializes hal outputs class. This is called automatically by the
+* enable function. It may be called any time the feature is enabled, but
+* is typically not needed to be called by outside callers.
+*/
+inv_error_t inv_init_datalogger_outputs(void)
+{
+    memset(&dl_out, 0, sizeof(dl_out));
+    return INV_SUCCESS;
+}
+
+/** Turns on creation and storage of HAL type results.
+*/
+inv_error_t inv_enable_datalogger_outputs(void)
+{
+    inv_error_t result;
+    result = inv_init_datalogger_outputs();
+    if (result)
+        return result;
+    return inv_register_mpl_start_notification(inv_start_datalogger_outputs);
+}
+
+/** Turns off creation and storage of HAL type results.
+*/
+inv_error_t inv_disable_datalogger_outputs(void)
+{
+    inv_stop_datalogger_outputs();
+    return inv_unregister_mpl_start_notification(inv_start_datalogger_outputs);
+}
+
+/**
+ * @}
+ */
diff --git a/6515/libsensors_iio/software/simple_apps/playback/linux/datalogger_outputs.h b/6515/libsensors_iio/software/simple_apps/playback/linux/datalogger_outputs.h
new file mode 100644
index 0000000..0e50faf
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/playback/linux/datalogger_outputs.h
@@ -0,0 +1,61 @@
+/**
+ *   @defgroup  HAL_Outputs
+ *   @brief     Motion Library - HAL Outputs
+ *              Sets up common outputs for HAL
+ *
+ *   @{
+ *       @file  datalogger_outputs.h
+ *       @brief Windows 8 HAL outputs.
+ */
+
+#ifndef _DATALOGGER_OUTPUTS_H_
+#define _DATALOGGER_OUTPUTS_H_
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* calibrated data */
+void inv_get_sensor_type_temperature_float(float *value,
+        inv_time_t *timestamp);
+void inv_get_sensor_type_gyro_float(float *values, int8_t *accuracy,
+        inv_time_t *timestamp);
+void inv_get_sensor_type_accel_float(float *values, int8_t *accuracy,
+        inv_time_t *timestamp);
+void inv_get_sensor_type_compass_float(float *mag_north, float *true_north,
+        float *values, int8_t *accuracy, inv_time_t *timestamp);
+void inv_get_sensor_type_quat_float(float *values, int *accuracy,
+        inv_time_t *timestamp);
+void inv_get_sensor_type_gravity_float(float *values, int *accuracy,
+        inv_time_t * timestamp);
+void inv_get_sensor_type_rotation_vector_float(float *values, int *accuracy,
+        inv_time_t * timestamp);
+
+/* uncalibrated data */
+void inv_get_sensor_type_gyro_raw_short(short *values,
+        inv_time_t *timestamp);
+void inv_get_sensor_type_gyro_raw_body_float(float *values,
+        inv_time_t *timestamp);
+void inv_get_sensor_type_accel_raw_short(short *values,
+        inv_time_t *timestamp);
+void inv_get_sensor_type_compass_raw_short(short *values,
+        inv_time_t *timestamp);
+
+/* enabler/disabler APIs */
+inv_error_t inv_enable_datalogger_outputs(void);
+inv_error_t inv_disable_datalogger_outputs(void);
+inv_error_t inv_init_datalogger_outputs(void);
+inv_error_t inv_start_datalogger_outputs(void);
+inv_error_t inv_stop_datalogger_outputs(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* #ifndef _DATALOGGER_OUTPUTS_H_ */
+
+/**
+ *  @}
+ */
diff --git a/6515/libsensors_iio/software/simple_apps/playback/linux/main.c b/6515/libsensors_iio/software/simple_apps/playback/linux/main.c
new file mode 100644
index 0000000..3eb64f0
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/playback/linux/main.c
@@ -0,0 +1,898 @@
+/*******************************************************************************
+ * Copyright (c) 2012 InvenSense Corporation, All Rights Reserved.
+ ******************************************************************************/
+
+/*******************************************************************************
+ *
+ * $Id: main.c 6146 2011-10-04 18:33:51Z jcalizo $
+ *
+ ******************************************************************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <time.h>
+
+#include "invensense.h"
+#include "invensense_adv.h"
+#include "and_constructor.h"
+#include "ml_math_func.h"
+#include "datalogger_outputs.h"
+
+#include "console_helper.h"
+
+#include "mlos.h"
+#include "mlsl.h"
+
+#include "testsupport.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-playback"
+
+/*
+    Defines & Macros
+*/
+#define UNPACK_3ELM_ARRAY(a)    (a)[0], (a)[1], (a)[2]
+#define UNPACK_4ELM_ARRAY(a)    UNPACK_3ELM_ARRAY(a), (a)[3]
+#define COMPONENT_NAME_MAX_LEN  (30)
+#define DEF_NAME(x)             (#x)
+
+#define PRINT_ON_CONSOLE(...)                   \
+    if (print_on_screen)                        \
+        printf(__VA_ARGS__)
+#define PRINT_ON_FILE(...)                      \
+    if(stream_file)                             \
+        fprintf(stream_file, __VA_ARGS__)
+
+#define PRINT(...)                              \
+    PRINT_ON_CONSOLE(__VA_ARGS__);              \
+    PRINT_ON_FILE(__VA_ARGS__)
+#define PRINT_FLOAT(width, prec, data)          \
+    PRINT_ON_CONSOLE("%+*.*f",                  \
+                     width, prec, data);        \
+    PRINT_ON_FILE("%+f", data)
+#define PRINT_INT(width, data)                  \
+    PRINT_ON_CONSOLE("%+*d", width, data);      \
+    PRINT_ON_FILE("%+d", data);
+#define PRINT_LONG(width, data)                 \
+    PRINT_ON_CONSOLE("%+*ld", width, data);     \
+    PRINT_ON_FILE("%+ld", data);
+
+#define PRINT_3ELM_ARRAY_FLOAT(w, p, data)      \
+    PRINT_FLOAT(w, p, data[0]);                 \
+    PRINT(", ");                                \
+    PRINT_FLOAT(w, p, data[1]);                 \
+    PRINT(", ");                                \
+    PRINT_FLOAT(w, p, data[2]);                 \
+    PRINT(", ");
+#define PRINT_4ELM_ARRAY_FLOAT(w, p, data)      \
+    PRINT_3ELM_ARRAY_FLOAT(w, p, data);         \
+    PRINT_FLOAT(w, p, data[3]);                 \
+    PRINT(", ");
+
+#define PRINT_3ELM_ARRAY_LONG(w, data)          \
+    PRINT_LONG(w, data[0]);                     \
+    PRINT(", ");                                \
+    PRINT_LONG(w, data[1]);                     \
+    PRINT(", ");                                \
+    PRINT_LONG(w, data[2]);                     \
+    PRINT(", ");
+#define PRINT_4ELM_ARRAY_LONG(w, data)          \
+    PRINT_3ELM_ARRAY_LONG(w, data);             \
+    PRINT_LONG(w, data[3]);                     \
+    PRINT(", ");
+
+#define PRINT_3ELM_ARRAY_INT(w, data)           \
+    PRINT_INT(w, data[0]);                      \
+    PRINT(", ");                                \
+    PRINT_INT(w, data[1]);                      \
+    PRINT(", ");                                \
+    PRINT_INT(w, data[2]);                      \
+    PRINT(", ");
+#define PRINT_4ELM_ARRAY_INT(w, data)           \
+    PRINT_3ELM_ARRAY_LONG(w, data);             \
+    PRINT_INT(w, data[3]);                      \
+    PRINT(", ");
+
+
+#define CASE_NAME(CODE)                         \
+    case CODE:                                  \
+        return #CODE
+
+#define CALL_CHECK_N_PRINT(f) {                                     \
+    MPL_LOGI("\n");                                                 \
+    MPL_LOGI("################################################\n"); \
+    MPL_LOGI("# %s\n", #f);                                         \
+    MPL_LOGI("################################################\n"); \
+    MPL_LOGI("\n");                                                 \
+    CALL_N_CHECK(f);                                                \
+}
+
+/*
+    Types
+*/
+/* A badly named enum type to track state of user input for tracker menu. */
+typedef enum {
+    STATE_SELECT_A_TRACKER,
+    STATE_SET_TRACKER_STATE,    /* I'm running out of ideas here. */
+    STATE_COUNT
+} user_state_t;
+
+/* bias trackers. */
+typedef enum {
+    BIAS_FROM_NO_MOTION,
+    FAST_NO_MOTION,
+    BIAS_FROM_GRAVITY,
+    BIAS_FROM_TEMPERATURE,
+    BIAS_FROM_LPF,
+    DEAD_ZONE,
+    NUM_TRACKERS
+} bias_t;
+
+enum comp_ids {
+    TIME = 0,
+    CALIBRATED_GYROSCOPE,
+    CALIBRATED_ACCELEROMETER,
+    CALIBRATED_COMPASS,
+    RAW_GYROSCOPE,
+    RAW_GYROSCOPE_BODY,
+    RAW_ACCELEROMETER,
+    RAW_COMPASS,
+    QUATERNION_9_AXIS,
+    QUATERNION_6_AXIS,
+    GRAVITY,
+    HEADING,
+    COMPASS_BIAS_ERROR,
+    COMPASS_STATE,
+    TEMPERATURE,
+    TEMP_COMP_SLOPE,
+    LINEAR_ACCELERATION,
+    ROTATION_VECTOR,
+    MOTION_STATE,
+
+    NUM_OF_IDS
+};
+
+struct component_list {
+    char name[COMPONENT_NAME_MAX_LEN];
+    int order;
+};
+
+/*
+    Globals
+*/
+static int print_on_screen = true;
+static int one_time_print = true;
+static FILE *stream_file = NULL;
+static unsigned long sample_count = 0;
+static int enabled_9x = true;
+
+signed char g_gyro_orientation[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+signed char g_accel_orientation[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+signed char g_compass_orientation[9] = {-1, 0, 0, 0, 1, 0, 0, 0, -1};
+
+#ifdef WIN32
+static double pc_freq;
+static __int64 counter_start;
+#else
+static inv_time_t counter_start;
+#endif
+
+struct component_list components[NUM_OF_IDS];
+
+/*
+    Prototypes
+*/
+void print_tracker_states(bias_t tracker);
+
+/*
+    Callbacks
+*/
+/*--- motion / no motion callback function ---*/
+void check_motion_event(void)
+{
+    long msg = inv_get_message_level_0(1);
+    if (msg) {
+        if (msg & INV_MSG_MOTION_EVENT) {
+            MPL_LOGI("################################################\n");
+            MPL_LOGI("## Motion\n");
+            MPL_LOGI("################################################\n");
+        }
+        if (msg & INV_MSG_NO_MOTION_EVENT) {
+            MPL_LOGI("################################################\n");
+            MPL_LOGI("## No Motion\n");
+            MPL_LOGI("################################################\n");
+        }
+    }
+}
+
+/* number to string coversion */
+char *compass_state_name(char* out, int state)
+{
+    switch(state) {
+        CASE_NAME(SF_NORMAL);
+        CASE_NAME(SF_DISTURBANCE);
+        CASE_NAME(SF_FAST_SETTLE);
+        CASE_NAME(SF_SLOW_SETTLE);
+        CASE_NAME(SF_STARTUP_SETTLE);
+        CASE_NAME(SF_UNCALIBRATED);
+    }
+
+    #define UNKNOWN_ERROR_CODE 1234
+    return ERROR_NAME(UNKNOWN_ERROR_CODE);
+}
+
+/* component ID to name convertion */
+char *component_name(char *out, int comp_id)
+{
+    switch (comp_id) {
+    CASE_NAME(TIME);
+    CASE_NAME(CALIBRATED_GYROSCOPE);
+    CASE_NAME(CALIBRATED_ACCELEROMETER);
+    CASE_NAME(CALIBRATED_COMPASS);
+    CASE_NAME(RAW_GYROSCOPE);
+    CASE_NAME(RAW_GYROSCOPE_BODY);
+    CASE_NAME(RAW_ACCELEROMETER);
+    CASE_NAME(RAW_COMPASS);
+    CASE_NAME(QUATERNION_9_AXIS);
+    CASE_NAME(QUATERNION_6_AXIS);
+    CASE_NAME(GRAVITY);
+    CASE_NAME(HEADING);
+    CASE_NAME(COMPASS_BIAS_ERROR);
+    CASE_NAME(COMPASS_STATE);
+    CASE_NAME(TEMPERATURE);
+    CASE_NAME(TEMP_COMP_SLOPE);
+    CASE_NAME(LINEAR_ACCELERATION);
+    CASE_NAME(ROTATION_VECTOR);
+    CASE_NAME(MOTION_STATE);
+    }
+
+    return "UNKNOWN";
+}
+
+
+#ifdef WIN32
+
+/*
+  Karthik Implementation.
+  http://stackoverflow.com/questions/1739259/how-to-use-queryperformancecounter
+*/
+double get_counter(__int64 *counter_start, double *pc_freq)
+{
+    LARGE_INTEGER li;
+    double x;
+    QueryPerformanceCounter(&li);
+    x = (double) (li.QuadPart - (*counter_start));
+    x = x / (*pc_freq);
+    return(x);
+}
+
+void start_counter(double *pc_freq, __int64 *counter_start)
+{
+    LARGE_INTEGER li;
+    double x;
+    if(!QueryPerformanceFrequency(&li))
+        printf("QueryPerformanceFrequency failed!\n");
+    x = (double)(li.QuadPart);
+    *pc_freq = x / 1000.0;
+    QueryPerformanceCounter(&li);
+    *counter_start = li.QuadPart;
+}
+
+#else
+
+unsigned long get_counter(void)
+{
+    return (inv_get_tick_count() - counter_start);
+}
+
+void start_counter(void)
+{
+    counter_start = inv_get_tick_count();
+}
+
+#endif
+
+/* processed data callback */
+void fifo_callback(void)
+{
+    int print_on_screen_saved = print_on_screen;
+    int i;
+
+    /* one_time_print causes the data labels to be printed on screen */
+    if (one_time_print) {
+        print_on_screen = true;
+    }
+    for (i = 0; i < NUM_OF_IDS; i++) {
+        if (components[TIME].order == i) {
+            if (one_time_print) {
+                PRINT("TIME,");
+            } else {
+#ifdef WIN32
+                double time_ms;
+                static int first_value = 0;
+                if(first_value == 0){
+                    first_value = 1;
+                    start_counter(&pc_freq, &counter_start);
+                    time_ms = 0;
+                } else {
+                    time_ms = get_counter(&counter_start, &pc_freq);
+                }
+                PRINT("%6.0f,   ", time_ms);
+#else
+                unsigned long time_ms;
+                static int first_value = 0;
+                if(first_value == 0){
+                    first_value = 1;
+                    start_counter();
+                    time_ms = 0;
+                } else {
+                    time_ms = get_counter();
+                }
+                PRINT("%6ld,   ", time_ms);
+#endif
+            }
+        } else if (components[CALIBRATED_GYROSCOPE].order == i) {
+            if (one_time_print) {
+                PRINT("CALIBRATED_GYROSCOPE_X,"
+                      "CALIBRATED_GYROSCOPE_Y,"
+                      "CALIBRATED_GYROSCOPE_Z,");
+                /*
+                PRINT("CALIBRATED_GYROSCOPE_X_AVERAGE,"
+                      "CALIBRATED_GYROSCOPE_Y_AVERAGE,"
+                      "CALIBRATED_GYROSCOPE_Z_AVERAGE,");
+                */
+            } else {
+                /*
+                #define window 20
+                static int cnt = 0;
+                static int valid = 0;
+                static float gyro_keep[window][3];
+                int kk, jj;
+                float avg[3];
+                */
+                float gyro[3];
+                inv_get_gyro_float(gyro);
+                PRINT_3ELM_ARRAY_FLOAT(10, 5, gyro);
+                PRINT("  ");
+                /*
+                memcpy(gyro_keep[cnt], gyro, sizeof(float) * 3);
+                cnt= (cnt + 1) % window;
+                if (cnt == window - 1)
+                    valid = 1;
+                if (valid) {
+                    memset(avg, 0, sizeof(float) * 3);
+                    jj = (cnt + 1) % window;
+                    for (kk = 0; kk < window; kk++) {
+                        avg[0] += gyro_keep[jj][0] / window;
+                        avg[1] += gyro_keep[jj][1] / window;
+                        avg[2] += gyro_keep[jj][2] / window;
+                        jj = (jj + 1) % window;
+                    }
+                    PRINT("%+f, %+f, %+f,   ",
+                          UNPACK_3ELM_ARRAY(avg));
+                    PRINT_3ELM_ARRAY_FLOAT(10, 5, avg);
+                    PRINT("  ");
+                }
+                */
+            }
+        } else if (components[CALIBRATED_ACCELEROMETER].order == i) {
+            if (one_time_print) {
+                PRINT("CALIBRATED_ACCELEROMETER_X,"
+                      "CALIBRATED_ACCELEROMETER_Y,"
+                      "CALIBRATED_ACCELEROMETER_Z,");
+            } else {
+                float accel[3];
+                inv_get_accel_float(accel);
+                PRINT_3ELM_ARRAY_FLOAT(10, 5, accel);
+                PRINT("  ");
+            }
+        } else if (components[CALIBRATED_COMPASS].order == i) {
+            if (one_time_print) {
+                PRINT("CALIBRATED_COMPASS_X,"
+                      "CALIBRATED_COMPASS_Y,"
+                      "CALIBRATED_COMPASS_Z,");
+            } else {
+                long lcompass[3];
+                float compass[3];
+                inv_get_compass_set(lcompass, 0, 0);
+                compass[0] = inv_q16_to_float(lcompass[0]);
+                compass[1] = inv_q16_to_float(lcompass[1]);
+                compass[2] = inv_q16_to_float(lcompass[2]);
+                PRINT_3ELM_ARRAY_FLOAT(10, 5, compass);
+                PRINT("  ");
+            }
+        } else if (components[RAW_GYROSCOPE].order == i) {
+            if (one_time_print) {
+                PRINT("RAW_GYROSCOPE_X,"
+                      "RAW_GYROSCOPE_Y,"
+                      "RAW_GYROSCOPE_Z,");
+            } else {
+                short raw[3];
+                inv_get_sensor_type_gyro_raw_short(raw, NULL);
+                PRINT_3ELM_ARRAY_INT(6, raw);
+                PRINT("  ");
+            }
+        } else if (components[RAW_GYROSCOPE_BODY].order == i) {
+            if (one_time_print) {
+                PRINT("RAW_GYROSCOPE_BODY_X,"
+                      "RAW_GYROSCOPE_BODY_Y,"
+                      "RAW_GYROSCOPE_BODY_Z,");
+            } else {
+                float raw_body[3];
+                inv_get_sensor_type_gyro_raw_body_float(raw_body, NULL);
+                PRINT_3ELM_ARRAY_FLOAT(10, 5, raw_body);
+                PRINT("  ");
+            }
+        } else if (components[RAW_ACCELEROMETER].order == i) {
+            if (one_time_print) {
+                PRINT("RAW_ACCELEROMETER_X,"
+                      "RAW_ACCELEROMETER_Y,"
+                      "RAW_ACCELEROMETER_Z,");
+            } else {
+                short raw[3];
+                inv_get_sensor_type_accel_raw_short(raw, NULL);
+                PRINT_3ELM_ARRAY_INT(6, raw);
+                PRINT("  ");
+            }
+        } else if (components[RAW_COMPASS].order == i) {
+            if (one_time_print) {
+                PRINT("RAW_COMPASS_X,"
+                      "RAW_COMPASS_Y,"
+                      "RAW_COMPASS_Z,");
+            } else {
+                short raw[3];
+                inv_get_sensor_type_compass_raw_short(raw, NULL);
+                PRINT_3ELM_ARRAY_INT(6, raw);
+                PRINT("  ");
+            }
+        } else if (components[QUATERNION_9_AXIS].order == i) {
+            if (one_time_print) {
+                PRINT("QUATERNION_9_AXIS_X,"
+                      "QUATERNION_9_AXIS_Y,"
+                      "QUATERNION_9_AXIS_Z,"
+                      "QUATERNION_9_AXIS_w,");
+            } else {
+                float quat[4];
+                inv_get_quaternion_float(quat);
+                PRINT_4ELM_ARRAY_FLOAT(10, 5, quat);
+                PRINT("  ");
+            }
+        } else if (components[QUATERNION_6_AXIS].order == i) {
+            if (one_time_print) {
+                PRINT("QUATERNION_6_AXIS_X,"
+                      "QUATERNION_6_AXIS_Y,"
+                      "QUATERNION_6_AXIS_Z,"
+                      "QUATERNION_6_AXIS_w,");
+            } else {
+                float quat[4];
+                long temp[4];
+                int j;
+                inv_time_t timestamp;
+                inv_get_6axis_quaternion(temp, &timestamp);
+                for (j = 0; j < 4; j++)
+                    quat[j] = (float)temp[j] / (1 << 30);
+                PRINT_4ELM_ARRAY_FLOAT(10, 5, quat);
+                PRINT("  ");
+            }
+        } else if (components[HEADING].order == i) {
+            if (one_time_print) {
+                PRINT("HEADING,");
+            } else {
+                float heading[1];
+                inv_get_sensor_type_compass_float(heading, NULL, NULL,
+                                                  NULL, NULL);
+                PRINT_FLOAT(10, 5, heading[0]);
+                PRINT(",   ");
+            }
+        } else if (components[GRAVITY].order == i) {
+            if (one_time_print) {
+                PRINT("GRAVITY_X,"
+                      "GRAVITY_Y,"
+                      "GRAVITY_Z,");
+            } else {
+                float gravity[3];
+                inv_get_sensor_type_gravity_float(gravity, NULL, NULL);
+                PRINT_3ELM_ARRAY_FLOAT(10, 5, gravity);
+                PRINT("  ");
+            }
+        } else if (components[COMPASS_STATE].order == i) {
+            if (one_time_print) {
+                PRINT("COMPASS_STATE,"
+                      "GOT_COARSE_HEADING,");
+            } else {
+                PRINT_INT(1, inv_get_compass_state());
+                PRINT(", ");
+                PRINT_INT(1, inv_got_compass_bias());
+                PRINT(", ");
+            }
+        } else if (components[COMPASS_BIAS_ERROR].order == i) {
+            if (one_time_print) {
+                PRINT("COMPASS_BIAS_ERROR_X,"
+                      "COMPASS_BIAS_ERROR_Y,"
+                      "COMPASS_BIAS_ERROR_Z,");
+            } else {
+                long mbe[3];
+                inv_get_compass_bias_error(mbe);
+                PRINT_3ELM_ARRAY_LONG(6, mbe);
+            }
+        } else if (components[TEMPERATURE].order == i) {
+            if (one_time_print) {
+                PRINT("TEMPERATURE,");
+            } else {
+                float temp;
+                inv_get_sensor_type_temperature_float(&temp, NULL);
+                PRINT_FLOAT(8, 4, temp);
+                PRINT(",   ");
+            }
+        } else if (components[TEMP_COMP_SLOPE].order == i) {
+            if (one_time_print) {
+                PRINT("TEMP_COMP_SLOPE_X,"
+                      "TEMP_COMP_SLOPE_Y,"
+                      "TEMP_COMP_SLOPE_Z,");
+            } else {
+                long temp_slope[3];
+                float temp_slope_f[3];
+                (void)inv_get_gyro_ts(temp_slope);
+                temp_slope_f[0] = inv_q16_to_float(temp_slope[0]);
+                temp_slope_f[1] = inv_q16_to_float(temp_slope[1]);
+                temp_slope_f[2] = inv_q16_to_float(temp_slope[2]);
+                PRINT_3ELM_ARRAY_FLOAT(10, 5, temp_slope_f);
+                PRINT("  ");
+            }
+        } else if (components[LINEAR_ACCELERATION].order == i) {
+            if (one_time_print) {
+                PRINT("LINEAR_ACCELERATION_X,"
+                      "LINEAR_ACCELERATION_Y,"
+                      "LINEAR_ACCELERATION_Z,");
+            } else {
+                float lin_acc[3];
+                inv_get_linear_accel_float(lin_acc);
+                PRINT_3ELM_ARRAY_FLOAT(10, 5, lin_acc);
+                PRINT("  ");
+            }
+        } else if (components[ROTATION_VECTOR].order == i) {
+            if (one_time_print) {
+                PRINT("ROTATION_VECTOR_X,"
+                      "ROTATION_VECTOR_Y,"
+                      "ROTATION_VECTOR_Z,");
+            } else {
+                float rot_vec[3];
+                inv_get_sensor_type_rotation_vector_float(rot_vec, NULL, NULL);
+                PRINT_3ELM_ARRAY_FLOAT(10, 5, rot_vec);
+                PRINT("  ");
+            }
+        } else if (components[MOTION_STATE].order == i) {
+            if (one_time_print) {
+                PRINT("MOTION_STATE,");
+            } else {
+                unsigned int counter;
+                PRINT_INT(1, inv_get_motion_state(&counter));
+                PRINT(",   ");
+            }
+        } else {
+            ;
+        }
+    }
+    PRINT("\n");
+
+    if (one_time_print) {
+        one_time_print = false;
+        print_on_screen = print_on_screen_saved;
+    }
+    sample_count++;
+}
+
+void print_help(char *exename)
+{
+    printf(
+        "\n"
+        "Usage:\n"
+        "\t%s [options]\n"
+        "\n"
+        "Options:\n"
+        "        [-h|--help]          = shows this help\n"
+        "        [-o|--output PREFIX] = to dump data on csv file whose file\n"
+        "                               prefix is specified by the parameter,\n"
+        "                               e.g. '<PREFIX>-<timestamp>.csv'\n"
+        "        [-i|--input NAME]    = to read the provided playback.bin file\n"
+        "        [-c|--comp C]        = enable the following components in the\n"
+        "                               given order:\n"
+        "                                 t = TIME\n"
+        "                                 T = TEMPERATURE,\n"
+        "                                 s = TEMP_COMP_SLOPE,\n"
+        "                                 G = CALIBRATED_GYROSCOPE,\n"
+        "                                 A = CALIBRATED_ACCELEROMETER,\n"
+        "                                 C = CALIBRATED_COMPASS,\n"
+        "                                 g = RAW_GYROSCOPE,\n"
+        "                                 b = RAW_GYROSCOPE_BODY,\n"
+        "                                 a = RAW_ACCELEROMETER,\n"
+        "                                 c = RAW_COMPASS,\n"
+        "                                 Q = QUATERNION_9_AXIS,\n"
+        "                                 6 = QUATERNION_6_AXIS,\n"
+        "                                 V = GRAVITY,\n"
+        "                                 L = LINEAR_ACCELERATION,\n"
+        "                                 R = ROTATION_VECTOR,\n"
+        "                                 H = HEADING,\n"
+        "                                 E = COMPASS_BIAS_ERROR,\n"
+        "                                 S = COMPASS_STATE,\n"
+        "                                 M = MOTION_STATE.\n"
+        "\n"
+        "Note on compass state values:\n"
+        "    SF_NORMAL         = 0\n"
+        "    SF_DISTURBANCE    = 1\n"
+        "    SF_FAST_SETTLE    = 2\n"
+        "    SF_SLOW_SETTLE    = 3\n"
+        "    SF_STARTUP_SETTLE = 4\n"
+        "    SF_UNCALIBRATED   = 5\n"
+        "\n",
+        exename);
+}
+
+char *output_filename_datetimestamp(char *out)
+{
+    time_t t;
+    struct tm *now;
+    t = time(NULL);
+    now = localtime(&t);
+
+    sprintf(out,
+            "%02d%02d%02d_%02d%02d%02d.csv",
+            now->tm_year - 100, now->tm_mon + 1, now->tm_mday,
+            now->tm_hour, now->tm_min, now->tm_sec);
+    return out;
+}
+
+int components_parser(char pname[], char requested[], int length)
+{
+    int j;
+
+    /* forcibly disable all */
+    for (j = 0; j < NUM_OF_IDS; j++)
+        components[j].order = -1;
+
+    /* parse each character one a time */
+    for (j = 0; j < length; j++) {
+        switch (requested[j]) {
+        case 'T':
+            components[TEMPERATURE].order = j;
+            break;
+        case 'G':
+            components[CALIBRATED_GYROSCOPE].order = j;
+            break;
+        case 'A':
+            components[CALIBRATED_ACCELEROMETER].order = j;
+            break;
+        case 'g':
+            components[RAW_GYROSCOPE].order = j;
+            break;
+        case 'b':
+            components[RAW_GYROSCOPE_BODY].order = j;
+            break;
+        case 'a':
+            components[RAW_ACCELEROMETER].order = j;
+            break;
+        case 'Q':
+            components[QUATERNION_9_AXIS].order = j;
+            break;
+        case '6':
+            components[QUATERNION_6_AXIS].order = j;
+            break;
+        case 'V':
+            components[GRAVITY].order = j;
+            break;
+        case 'L':
+            components[LINEAR_ACCELERATION].order = j;
+            break;
+        case 'R':
+            components[ROTATION_VECTOR].order = j;
+            break;
+
+        /* these components don't need to be enabled */
+        case 't':
+            components[TIME].order = j;
+            break;
+        case 'C':
+            components[CALIBRATED_COMPASS].order = j;
+            break;
+        case 'c':
+            components[RAW_COMPASS].order = j;
+            break;
+        case 'H':
+            components[HEADING].order = j;
+            break;
+        case 'E':
+            components[COMPASS_BIAS_ERROR].order = j;
+            break;
+        case 'S':
+            components[COMPASS_STATE].order = j;
+            break;
+        case 'M':
+            components[MOTION_STATE].order = j;
+            break;
+
+        default:
+            MPL_LOGI("Error : unrecognized component '%c'\n",
+                     requested[j]);
+            print_help(pname);
+            return 1;
+            break;
+        }
+    }
+    return 0;
+}
+
+int main(int argc, char *argv[])
+{
+#ifndef INV_PLAYBACK_DBG
+    MPL_LOGI("Error : this application was not compiled with the "
+             "INV_PLAYBACK_DBG define and cannot work.\n");
+    MPL_LOGI("        Recompile the libmllite libraries with #define "
+             "INV_PLAYBACK_DBG uncommented\n");
+    MPL_LOGI("        in 'software/core/mllite/data_builder.h'.\n");
+    return INV_ERROR;
+#endif
+
+    inv_time_t start_time;
+    double total_time;
+    char req_component_list[50] = "tQGACH";
+    char input_filename[101] = "/data/playback.bin";
+    int i = 0;
+    char *ver_str;
+    /* flags */
+    int use_nm_detection = true;
+
+    /* make sure there is no buffering of the print messages */
+    setvbuf(stdout, NULL, _IONBF, 0);
+
+    /* forcibly disable all and populate the names */
+    for (i = 0; i < NUM_OF_IDS; i++) {
+        char str_tmp[COMPONENT_NAME_MAX_LEN];
+        strcpy(components[i].name, component_name(str_tmp, i));
+        components[i].order = -1;
+    }
+
+    CALL_N_CHECK( inv_get_version(&ver_str) );
+    MPL_LOGI("\n");
+    MPL_LOGI("%s\n", ver_str);
+    MPL_LOGI("\n");
+
+    for (i = 1; i < argc; i++) {
+        if(strcmp(argv[i], "-h") == 0
+            || strcmp(argv[i], "--help") == 0) {
+            print_help(argv[0]);
+            return INV_SUCCESS;
+
+        } else if(strcmp(argv[i], "-o") == 0
+            || strcmp(argv[i], "--output") == 0) {
+            char output_filename[200];
+            char end[50] = "";
+            i++;
+
+            snprintf(output_filename, sizeof(output_filename), "%s-%s",
+                    argv[i], output_filename_datetimestamp(end));
+            stream_file = fopen(output_filename, "w+");
+            if (!stream_file) {
+                printf("Unable to open file '%s'\n", output_filename);
+                return INV_ERROR;
+            }
+            MPL_LOGI("-- Output on file '%s'\n", output_filename);
+
+        } else if(strcmp(argv[i], "-i") == 0
+            || strcmp(argv[i], "--input") == 0) {
+            i++;
+            strncpy(input_filename, argv[i], sizeof(input_filename));
+            MPL_LOGI("-- Playing back file '%s'\n", input_filename);
+
+        } else if(strcmp(argv[i], "-n") == 0
+            || strcmp(argv[i], "--nm") == 0) {
+            i++;
+            if (!strcmp(argv[i], "none")) {
+                use_nm_detection = 0;
+            } else if (!strcmp(argv[i], "regular")) {
+                use_nm_detection = 1;
+            } else if (!strcmp(argv[i], "fast")) {
+                use_nm_detection = 2;
+            } else {
+                MPL_LOGI("Error : unrecognized no-motion type '%s'\n",
+                         argv[i]);
+                return INV_ERROR_INVALID_PARAMETER;
+            }
+            MPL_LOGI("-- No motion algorithm : '%s', %d\n",
+                     argv[i], use_nm_detection);
+
+        } else if(strcmp(argv[i], "-9") == 0
+            || strcmp(argv[i], "--nine") == 0) {
+            MPL_LOGI("-- using 9 axis sensor fusion by default\n");
+            enabled_9x = true;
+
+        } else if(strcmp(argv[i], "-c") == 0
+            || strcmp(argv[i], "--comp") == 0) {
+            i++;
+            strcpy(req_component_list, argv[i]);
+
+        } else {
+            MPL_LOGI("Unrecognized command-line parameter '%s'\n", argv[i]);
+            return INV_ERROR_INVALID_PARAMETER;
+        }
+    }
+
+    CALL_CHECK_N_RETURN_ERROR(
+        components_parser(
+            argv[0],
+            req_component_list, strlen(req_component_list)));
+
+    /* set up callbacks */
+    CALL_N_CHECK(inv_set_fifo_processed_callback(fifo_callback));
+
+    /* algorithm init */
+    CALL_N_CHECK(inv_enable_quaternion());
+    if (use_nm_detection == 1) {
+        CALL_N_CHECK(inv_enable_motion_no_motion());
+    } else if (use_nm_detection == 2) {
+        CALL_N_CHECK(inv_enable_fast_nomot());
+    }
+    CALL_N_CHECK(inv_enable_gyro_tc());
+    CALL_N_CHECK(inv_enable_in_use_auto_calibration());
+    CALL_N_CHECK(inv_enable_no_gyro_fusion());
+    CALL_N_CHECK(inv_enable_results_holder());
+    if (enabled_9x) {
+        CALL_N_CHECK(inv_enable_heading_from_gyro());
+        CALL_N_CHECK(inv_enable_compass_bias_w_gyro());
+        CALL_N_CHECK(inv_enable_vector_compass_cal());
+        CALL_N_CHECK(inv_enable_9x_sensor_fusion());
+    }
+
+    CALL_CHECK_N_RETURN_ERROR(inv_enable_datalogger_outputs());
+    CALL_CHECK_N_RETURN_ERROR(inv_constructor_start());
+
+    /* load persistent data */
+    {
+        FILE *fc = fopen("invcal.bin", "rb");
+        if (fc != NULL) {
+            size_t sz, rd;
+            inv_error_t result = 0;
+            // Read amount of memory we need to hold data
+            rd = fread(&sz, sizeof(size_t), 1, fc);
+            if (rd == sizeof(size_t)) {
+                unsigned char *cal_data = (unsigned char *)malloc(sizeof(sz));
+                unsigned char *cal_ptr;
+                cal_ptr = cal_data;
+                *(size_t *)cal_ptr = sz;
+                cal_ptr += sizeof(sz);
+                /* read rest of the file */
+                fread(cal_ptr, sz - sizeof(size_t), 1, fc);
+                //result = inv_load_mpl_states(cal_data, sz);
+                if (result) {
+                    MPL_LOGE("Cal Load error\n");
+                }
+                MPL_LOGI("inv_load_mpl_states()\n");
+                free(cal_data);
+            } else {
+                MPL_LOGE("Cal Load error with size read\n");
+            }
+            fclose(fc);
+        }
+    }
+
+    sample_count = 0;
+    start_time = inv_get_tick_count();
+
+    /* playback data that was recorded */
+    inv_set_playback_filename(input_filename, strlen(input_filename) + 1);
+    inv_set_debug_mode(RD_PLAYBACK);
+    CALL_N_CHECK(inv_playback());
+
+    total_time = (1.0 * inv_get_tick_count() - start_time) / 1000;
+    if (total_time > 0) {
+        MPL_LOGI("\nPlayed back %ld samples in %.2f s (%.1f Hz)\n",
+                 sample_count, total_time , 1.0 * sample_count / total_time);
+    }
+
+    if (stream_file)
+        fclose(stream_file);
+
+    return INV_SUCCESS;
+}
+
+
diff --git a/6515/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared b/6515/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared
new file mode 100755
index 0000000..f2c157d
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared
diff --git a/6515/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk b/6515/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk
new file mode 100644
index 0000000..ed5fbf6
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk
@@ -0,0 +1,98 @@
+EXEC = inv_self_test$(SHARED_APP_SUFFIX)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
+COMP  ?= $(CROSS)gcc
+LINK  ?= $(CROSS)gcc
+
+OBJFOLDER = $(CURDIR)/obj
+
+INV_ROOT   = ../../../../..
+APP_DIR    = $(CURDIR)/../..
+MLLITE_DIR = $(INV_ROOT)/software/core/mllite
+COMMON_DIR = $(INV_ROOT)/software/simple_apps/common
+MPL_DIR    = $(INV_ROOT)/software/core/mpl
+HAL_DIR    = $(INV_ROOT)/software/core/HAL
+
+include $(INV_ROOT)/software/build/android/common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += $(ANDROID_COMPILE)
+CFLAGS += -Wall
+CFLAGS += -fpic
+CFLAGS += -nostdlib
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT
+CFLAGS += -DLINUX
+CFLAGS += -DANDROID
+CFLAGS += -mthumb-interwork
+CFLAGS += -fno-exceptions
+CFLAGS += -ffunction-sections
+CFLAGS += -funwind-tables
+CFLAGS += -fstack-protector
+CFLAGS += -fno-short-enums
+CFLAGS += -fmessage-length=0
+CFLAGS += -I$(MLLITE_DIR)
+CFLAGS += -I$(MPL_DIR)
+CFLAGS += -I$(COMMON_DIR)
+CFLAGS += -I$(HAL_DIR)/include
+CFLAGS += $(INV_INCLUDES)
+CFLAGS += $(INV_DEFINES)
+
+LLINK  = -lc
+LLINK += -lm
+LLINK += -lutils
+LLINK += -lcutils
+LLINK += -lgcc
+LLINK += -ldl
+LLINK += -lstdc++
+LLINK += -llog
+LLINK += -lz
+
+LFLAGS += $(CMDLINE_LFLAGS)
+LFLAGS += $(ANDROID_LINK_EXECUTABLE)
+
+LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+####################################################################################################
+## sources
+
+INV_LIBS  = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT)
+INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
+
+#INV_SOURCES and VPATH provided by Makefile.filelist
+include ../filelist.mk
+
+INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
+INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all clean cleanall install
+
+all: $(EXEC) $(MK_NAME)
+
+$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
+	$(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+
+$(OBJFOLDER) :
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(EXEC) $(OBJFOLDER)
+
+install : $(EXEC)
+	cp -f $(EXEC) $(INSTALL_DIR)
+
+
diff --git a/6515/libsensors_iio/software/simple_apps/self_test/build/filelist.mk b/6515/libsensors_iio/software/simple_apps/self_test/build/filelist.mk
new file mode 100644
index 0000000..492f47e
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/self_test/build/filelist.mk
@@ -0,0 +1,11 @@
+#### filelist.mk for console_test ####
+
+# headers
+HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h
+
+# sources
+SOURCES := $(APP_DIR)/inv_self_test.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux
diff --git a/6515/libsensors_iio/software/simple_apps/self_test/inv_self_test.c b/6515/libsensors_iio/software/simple_apps/self_test/inv_self_test.c
new file mode 100644
index 0000000..e846499
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/self_test/inv_self_test.c
@@ -0,0 +1,646 @@
+/**
+ *  Self Test application for Invensense's MPU6xxx/MPU9xxx.
+ */
+
+#include <unistd.h>
+#include <dirent.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <errno.h>
+#include <sys/stat.h>
+#include <stdlib.h>
+#include <features.h>
+#include <dirent.h>
+#include <string.h>
+#include <poll.h>
+#include <stddef.h>
+#include <linux/input.h>
+#include <time.h>
+#include <linux/time.h>
+
+#include "invensense.h"
+#include "ml_math_func.h"
+#include "storage_manager.h"
+#include "ml_stored_data.h"
+#include "ml_sysfs_helper.h"
+#include "data_builder.h"
+
+#ifndef ABS
+#define ABS(x)(((x) >= 0) ? (x) : -(x))
+#endif
+
+#define DEBUG_PRINT     /* Uncomment to print Gyro & Accel read from Driver */
+
+#define MAX_SYSFS_NAME_LEN     (100)
+#define MAX_SYSFS_ATTRB        (sizeof(struct sysfs_attrbs) / sizeof(char*))
+#define IIO_SYSFS_PATH         "/sys/bus/iio/devices/iio:device0"
+#define IIO_HUB_NAME           "inv_hub"
+
+#define GYRO_PASS_STATUS_BIT    0x01
+#define ACCEL_PASS_STATUS_BIT   0x02
+#define COMPASS_PASS_STATUS_BIT 0x04
+
+typedef union {
+    long l; 
+    int i;
+} bias_dtype;
+
+char *sysfs_names_ptr;
+struct sysfs_attrbs {
+    char *name;
+    char *enable;
+    int enable_value;
+    char *power_state;
+    int power_state_value;
+    char *dmp_on;
+    int dmp_on_value;
+    char *self_test;
+    char *temperature;
+    char *gyro_enable;
+    int gyro_enable_value;
+    char *gyro_x_calibbias;
+    char *gyro_y_calibbias;
+    char *gyro_z_calibbias;
+    char *gyro_scale;
+    char *gyro_x_offset;
+    char *gyro_y_offset;
+    char *gyro_z_offset;
+    char *accel_enable;
+    int accel_enable_value;
+    char *accel_x_calibbias;
+    char *accel_y_calibbias;
+    char *accel_z_calibbias;
+    char *accel_scale;
+    char *accel_x_offset;
+    char *accel_y_offset;
+    char *accel_z_offset;
+    char *compass_enable;
+    int compass_enable_value;
+} mpu;
+
+static struct inv_db_save_t save_data;
+static bool write_biases_immediately = false;
+
+/** This function receives the data that was stored in non-volatile memory
+    between power off */
+static inv_error_t inv_db_load_func(const unsigned char *data)
+{
+    memcpy(&save_data, data, sizeof(save_data));
+    return INV_SUCCESS;
+}
+
+/** This function returns the data to be stored in non-volatile memory between
+    power off */
+static inv_error_t inv_db_save_func(unsigned char *data)
+{
+    memcpy(data, &save_data, sizeof(save_data));
+    return INV_SUCCESS;
+}
+
+/** read a sysfs entry that represents an integer */
+int read_sysfs_int(char *filename, int *var)
+{
+    int res=0;
+    FILE *fp;
+
+    fp = fopen(filename, "r");
+    if (fp != NULL) {
+        fscanf(fp, "%d\n", var);
+        fclose(fp);
+    } else {
+        MPL_LOGE("inv_self_test: ERR open file to read");
+        res= -1;
+    }
+    return res;
+}
+
+/** write a sysfs entry that represents an integer */
+int write_sysfs_int(char *filename, int data)
+{
+    int res = 0;
+    FILE *fp;
+
+    fp = fopen(filename, "w");
+    if (fp != NULL) {
+        fprintf(fp, "%d\n", data);
+        fclose(fp);
+    } else {
+        MPL_LOGE("inv_self_test: ERR open file to write");
+        res= -1;
+    }
+    return res;
+}
+
+int android_hub(void)
+{
+    char dev_name[8];
+    FILE *fp;
+
+    fp = fopen(mpu.name, "r");
+    fgets(dev_name, 8, fp);
+    fclose (fp);
+    
+    if (!strncmp(dev_name, IIO_HUB_NAME, sizeof(IIO_HUB_NAME)))
+        return true;
+    else
+        return false;
+}
+
+int save_n_write_sysfs_int(char *filename, int data, int *old_value)
+{
+    int res;
+    res = read_sysfs_int(filename, old_value);
+    if (res < 0) {
+        return res;
+    }
+    //printf("saved %s = %d\n", filename, *old_value);
+    res = write_sysfs_int(filename, data);
+    if (res < 0) {
+        return res;
+    }
+    return 0;
+}
+
+int inv_init_sysfs_attributes(void)
+{
+    unsigned char i = 0;
+    char sysfs_path[MAX_SYSFS_NAME_LEN];
+    char *sptr;
+    char **dptr;
+
+    sysfs_names_ptr =
+            (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
+    sptr = sysfs_names_ptr;
+    if (sptr != NULL) {
+        dptr = (char**)&mpu;
+        do {
+            *dptr++ = sptr;
+            sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
+        } while (++i < MAX_SYSFS_ATTRB);
+    } else {
+        MPL_LOGE("inv_self_test: couldn't alloc mem for sysfs paths");
+        return -1;
+    }
+
+    // Setup IIO sysfs path & build MPU's sysfs paths
+    sprintf(sysfs_path, "%s", IIO_SYSFS_PATH);
+
+    sprintf(mpu.name, "%s%s", sysfs_path, "/name");
+    sprintf(mpu.enable, "%s%s", sysfs_path, "/buffer/enable");
+    sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
+    sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
+    sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
+    sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature");
+
+    sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable");
+    sprintf(mpu.gyro_x_calibbias, "%s%s",
+            sysfs_path, "/in_anglvel_x_calibbias");
+    sprintf(mpu.gyro_y_calibbias, "%s%s",
+            sysfs_path, "/in_anglvel_y_calibbias");
+    sprintf(mpu.gyro_z_calibbias, "%s%s",
+            sysfs_path, "/in_anglvel_z_calibbias");
+    sprintf(mpu.gyro_scale,  "%s%s", sysfs_path, "/in_anglvel_self_test_scale");
+    sprintf(mpu.gyro_x_offset, "%s%s", sysfs_path, "/in_anglvel_x_offset");
+    sprintf(mpu.gyro_y_offset, "%s%s", sysfs_path, "/in_anglvel_y_offset");
+    sprintf(mpu.gyro_z_offset, "%s%s", sysfs_path, "/in_anglvel_z_offset");
+
+    sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accel_enable");
+    sprintf(mpu.accel_x_calibbias, "%s%s", sysfs_path, "/in_accel_x_calibbias");
+    sprintf(mpu.accel_y_calibbias, "%s%s", sysfs_path, "/in_accel_y_calibbias");
+    sprintf(mpu.accel_z_calibbias, "%s%s", sysfs_path, "/in_accel_z_calibbias");
+    sprintf(mpu.accel_scale,  "%s%s", sysfs_path, "/in_accel_self_test_scale");
+    sprintf(mpu.accel_x_offset, "%s%s", sysfs_path, "/in_accel_x_offset");
+    sprintf(mpu.accel_y_offset, "%s%s", sysfs_path, "/in_accel_y_offset");
+    sprintf(mpu.accel_z_offset, "%s%s", sysfs_path, "/in_accel_z_offset");
+
+    sprintf(mpu.compass_enable, "%s%s", sysfs_path, "/compass_enable");
+
+#if 0
+    // test print sysfs paths
+    dptr = (char**)&mpu;
+    for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
+        printf("inv_self_test: sysfs path: %s\n", *dptr++);
+    }
+#endif
+    return 0;
+}
+
+void print_cal_file_content(struct inv_db_save_t *data)
+{
+    printf("------------------------------\n");
+    printf("-- Calibration file content --\n");
+    printf("   factory_gyro_bias[3]  = %+ld %+ld %+ld\n",
+           data->factory_gyro_bias[0], data->factory_gyro_bias[1],
+           data->factory_gyro_bias[2]);
+    printf("   factory_accel_bias[3] = %+ld %+ld %+ld\n",
+           data->factory_accel_bias[0], data->factory_accel_bias[1],
+           data->factory_accel_bias[2]);
+    printf("   compass_bias[3]      = %+ld %+ld %+ld\n",
+           data->compass_bias[0], data->compass_bias[1], data->compass_bias[2]);
+    printf("   gyro_temp            = %+ld\n", data->gyro_temp);
+    printf("   gyro_bias_tc_set     = %+d\n", data->gyro_bias_tc_set);
+    printf("   accel_temp           = %+ld\n", data->accel_temp);
+    printf("   gyro_accuracy        = %d\n", data->gyro_accuracy);
+    printf("   accel_accuracy       = %d\n", data->accel_accuracy);
+    printf("   compass_accuracy     = %d\n", data->compass_accuracy);
+    printf("------------------------------\n");
+}
+
+/*******************************************************************************
+ *                       M a i n
+ ******************************************************************************/
+int main(int argc, char **argv)
+{
+    FILE *fptr;
+    int self_test_status = 0;
+    inv_error_t result;
+    bias_dtype gyro_bias[3];
+    bias_dtype gyro_scale;
+    bias_dtype accel_bias[3];
+    bias_dtype accel_scale;
+    int scale_ratio;
+    size_t packet_sz;
+    int axis_sign = 1;
+    unsigned char *buffer;
+    long timestamp;
+    long long temperature = 0;
+    bool compass_present = true;
+    int c;
+
+    result= inv_init_sysfs_attributes();
+    if (result)
+        return -1;
+
+    // Self-test for Non-Hub
+    inv_init_storage_manager();
+
+    // Register packet to be saved.
+    result = inv_register_load_store(
+                inv_db_load_func, inv_db_save_func,
+                sizeof(save_data), INV_DB_SAVE_KEY);
+
+    // Self-test for Android Hub
+    if (android_hub() == true) {
+        fptr = fopen(mpu.self_test, "r");
+        if (fptr) {
+            fscanf(fptr, "%d", &self_test_status);
+            printf("\nSelf-Test:Hub:Self test result - "
+                   "Gyro passed= %x, Accel passed= %x, Compass passed= %x\n",
+                   (self_test_status & GYRO_PASS_STATUS_BIT), 
+                   (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1,
+                   (self_test_status & COMPASS_PASS_STATUS_BIT) >> 2);
+            fclose(fptr);
+            result = 0;
+        } else {
+            printf("Hub-Self-Test:ERR-Couldn't invoke self-test\n");
+            result = -1;
+        }
+
+        free(sysfs_names_ptr);
+        return result;
+    } 
+
+    /* testing hook: if the command-line parameter is '-l' as in 'load',
+       the system will read out the MLCAL_FILE */
+    while ((c = getopt(argc, argv, "lw")) != -1) {
+        switch (c) {
+        case 'l':
+            inv_get_mpl_state_size(&packet_sz);
+
+            buffer = (unsigned char *)malloc(packet_sz + 10);
+            if (buffer == NULL) {
+                printf("Self-Test:Can't allocate buffer\n");
+                return -1;
+            }
+
+            fptr= fopen(MLCAL_FILE, "rb");
+            if (!fptr) {
+                printf("Self-Test:ERR- Can't open cal file to read - %s\n",
+                       MLCAL_FILE);
+                result = -1;
+            }
+            fread(buffer, 1, packet_sz, fptr);
+            fclose(fptr);
+
+            result = inv_load_mpl_states(buffer, packet_sz);
+            if (result) {
+                printf("Self-Test:ERR - "
+                       "Cannot load MPL states from cal file - error %d\n", 
+                       result);
+                free(buffer);
+                return -1;
+            }
+            free(buffer);
+
+            print_cal_file_content(&save_data);
+            return 0;
+            break;
+
+        case 'w':
+            write_biases_immediately = true;
+            break;
+
+        case '?':
+            return -1;
+        }
+    }
+
+    // Clear out data.
+    memset(&save_data, 0, sizeof(save_data));
+    memset(gyro_bias,0, sizeof(gyro_bias));
+    memset(accel_bias,0, sizeof(accel_bias));
+
+    // enable the device
+    if (save_n_write_sysfs_int(mpu.power_state, 1, 
+                               &mpu.power_state_value) < 0) {
+        printf("Self-Test:ERR-Failed to set power_state=1\n");
+    }
+    if (save_n_write_sysfs_int(mpu.enable, 0, &mpu.enable_value) < 0) {
+        printf("Self-Test:ERR-Failed to disable master enable\n");
+    }
+    if (save_n_write_sysfs_int(mpu.dmp_on, 0, &mpu.dmp_on_value) < 0) {
+        printf("Self-Test:ERR-Failed to disable DMP\n");
+    }
+    if (save_n_write_sysfs_int(mpu.accel_enable, 1, 
+                               &mpu.accel_enable_value) < 0) {
+        printf("Self-Test:ERR-Failed to enable accel\n");
+    }
+    if (save_n_write_sysfs_int(mpu.gyro_enable, 1, 
+                               &mpu.gyro_enable_value) < 0) {
+        printf("Self-Test:ERR-Failed to enable gyro\n");
+    }
+    if (save_n_write_sysfs_int(mpu.compass_enable, 1, 
+                               &mpu.compass_enable_value) < 0) {
+#ifdef DEBUG_PRINT
+        printf("Self-Test:ERR-Failed to enable compass\n");
+#endif
+        compass_present = false;
+    } 
+
+    fptr = fopen(mpu.self_test, "r");
+    if (!fptr) {
+        printf("Self-Test:ERR-Couldn't invoke self-test\n");
+        result = -1;
+        goto free_sysfs_storage;
+    }
+
+    // Invoke self-test 
+    fscanf(fptr, "%d", &self_test_status);
+    if (compass_present == true) {
+        printf("Self-Test:Self test result- "
+               "Gyro passed = %x, Accel passed = %x, Compass passed = %x\n",
+               (self_test_status & GYRO_PASS_STATUS_BIT), 
+               (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1,
+               (self_test_status & COMPASS_PASS_STATUS_BIT) >> 2);
+    } else {
+        printf("Self-Test:Self test result- "
+               "Gyro passed = %x, Accel passed = %x\n",
+               (self_test_status & GYRO_PASS_STATUS_BIT),
+               (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1);
+    }
+    fclose(fptr);
+
+    if (self_test_status & GYRO_PASS_STATUS_BIT) {
+        // Read Gyro Bias
+        if (read_sysfs_int(mpu.gyro_x_calibbias, &gyro_bias[0].i) < 0 ||
+            read_sysfs_int(mpu.gyro_y_calibbias, &gyro_bias[1].i) < 0 ||
+            read_sysfs_int(mpu.gyro_z_calibbias, &gyro_bias[2].i) < 0 ||
+            read_sysfs_int(mpu.gyro_scale, &gyro_scale.i) < 0) {
+            memset(gyro_bias, 0, sizeof(gyro_bias));
+            gyro_scale.i = 0;
+            printf("Self-Test:Failed to read Gyro bias\n");
+        } else {
+            save_data.gyro_accuracy = 3;
+#ifdef DEBUG_PRINT
+            printf("Self-Test:Gyro bias[0..2] = [%d %d %d]\n", 
+                   gyro_bias[0].i, gyro_bias[1].i, gyro_bias[2].i);
+#endif
+        }
+    } else {
+        printf("Self-Test:Failed Gyro self-test\n");
+    }
+
+    if (self_test_status & ACCEL_PASS_STATUS_BIT) {
+        // Read Accel Bias
+        if (read_sysfs_int(mpu.accel_x_calibbias, &accel_bias[0].i) < 0 ||
+            read_sysfs_int(mpu.accel_y_calibbias, &accel_bias[1].i) < 0 ||
+            read_sysfs_int(mpu.accel_z_calibbias, &accel_bias[2].i) < 0 ||
+            read_sysfs_int(mpu.accel_scale, &accel_scale.i) < 0) {
+            memset(accel_bias, 0, sizeof(accel_bias));
+            accel_scale.i = 0;
+            printf("Self-Test:Failed to read Accel bias\n");
+        } else {
+            save_data.accel_accuracy = 3;
+#ifdef DEBUG_PRINT
+            printf("Self-Test:Accel bias[0..2] = [%d %d %d]\n", 
+                   accel_bias[0].i, accel_bias[1].i, accel_bias[2].i);
+#endif
+       }
+    } else {
+        printf("Self-Test:Failed Accel self-test\n");
+    }
+
+    if (!(self_test_status & (GYRO_PASS_STATUS_BIT | ACCEL_PASS_STATUS_BIT))) {
+        printf("Self-Test:Failed Gyro and Accel self-test, "
+               "nothing left to do\n");
+        result = -1;
+        goto restore_settings;
+    }
+
+    // Read temperature
+    fptr = fopen(mpu.temperature, "r");
+    if (fptr != NULL) {
+        fscanf(fptr,"%lld %ld", &temperature, &timestamp);
+        fclose(fptr);
+    } else {
+        printf("Self-Test:ERR-Couldn't read temperature\n");
+    }
+
+    /*
+        When we read gyro bias from sysfs, the bias is in the raw units scaled by 
+        1000 at the full scale used during self-test 
+        (in_anglvel_self_test_scale).
+        We store the biases in raw units (+/- 2000 dps full scale assumed) 
+        scaled by 2^16 therefore the gyro_bias[] have to be divided by the 
+        ratio of 2000 / gyro_scale to comply.
+    */
+    // TODO read this from the regular full scale in sysfs
+    scale_ratio = 2000 / gyro_scale.i;
+    save_data.factory_gyro_bias[0] = 
+                        (long)(gyro_bias[0].l / 1000.f * 65536.f / scale_ratio);
+    save_data.factory_gyro_bias[1] = 
+                        (long)(gyro_bias[1].l / 1000.f * 65536.f / scale_ratio);
+    save_data.factory_gyro_bias[2] = 
+                        (long)(gyro_bias[2].l / 1000.f * 65536.f / scale_ratio);
+
+    // Save temperature @ time stored.
+    //  Temperature is in degrees Celsius scaled by 2^16
+    save_data.gyro_temp = temperature * (1L << 16);
+    save_data.gyro_bias_tc_set = true;
+    save_data.accel_temp = save_data.gyro_temp;
+
+    // When we read accel bias, the bias is in raw units scaled by 1000.
+    // and it contains the gravity vector.
+    // Find the orientation of the device, by looking for gravity
+    int axis = 0;
+    if (ABS(accel_bias[1].l) > ABS(accel_bias[0].l)) {
+        axis = 1;
+    }
+    if (ABS(accel_bias[2].l) > ABS(accel_bias[axis].l)) {
+        axis = 2;
+    }
+    if (accel_bias[axis].l < 0) {
+        axis_sign = -1;
+    }
+
+    // Convert scaling from raw units scaled by 1000 to raw scaled by 2^16
+    /*
+        When we read accel bias from sysfs, the bias is in the raw units scaled
+        by 1000 at the full scale used during self-test 
+        (in_accel_self_test_scale).
+        We store the biases in raw units (+/- 2 gee full scale assumed) 
+        scaled by 2^16 therefore the accel_bias[] have to be multipled by the 
+        ratio of accel_scale / 2 to comply.
+    */
+    // TODO read this from the regular full scale in sysfs
+    scale_ratio = accel_scale.i / 2;
+    save_data.factory_accel_bias[0] =
+                    (long)(accel_bias[0].l / 1000.f * 65536.f * scale_ratio);
+    save_data.factory_accel_bias[1] =
+                    (long)(accel_bias[1].l / 1000.f * 65536.f * scale_ratio);
+    save_data.factory_accel_bias[2] =
+                    (long)(accel_bias[2].l / 1000.f * 65536.f * scale_ratio);
+
+#ifdef DEBUG_PRINT
+    printf("Self-Test:Saved Accel bias[0..2] = [%ld %ld %ld]\n",
+           save_data.factory_accel_bias[0], save_data.factory_accel_bias[1],
+           save_data.factory_accel_bias[2]);
+#endif
+
+    /*
+        Remove gravity, gravity in raw units should be 16384 = 2^14 for a +/- 
+        2 gee setting. The data has been saved in Hw units scaled to 2^16,
+        so gravity needs to scale up accordingly.
+    */
+    /* gravity correction for accel_bias format */
+    long gravity = axis_sign * (32768L / accel_scale.i) * 1000L;
+    accel_bias[axis].l -= gravity; 
+    /* gravity correction for saved_data.accel_bias format */
+    gravity = axis_sign * (32768L / accel_scale.i) * 65536L;
+#ifdef DEBUG_PRINT
+    printf("Self-Test:Gravity = %ld\n", gravity);
+#endif
+    save_data.factory_accel_bias[axis] -= gravity;
+
+    printf("Self-Test:Saved Accel bias (gravity removed) [0..2] = "
+           "[%ld %ld %ld]\n",
+           save_data.factory_accel_bias[0], save_data.factory_accel_bias[1],
+           save_data.factory_accel_bias[2]);
+    /* write the accel biases down to the hardware now */
+    if (write_biases_immediately) {
+        int offsets[3] = {0};
+        /* NOTE: 16 is the gee scale of the dmp_bias settings */
+        scale_ratio = 16 / accel_scale.i;
+        /* NOTE: the 2 factor is to account the halved resolution for the 
+                 accel offset registers */
+        offsets[0] = -accel_bias[0].l / 1000 / 2 / scale_ratio;
+        if (write_sysfs_int(mpu.accel_x_offset, offsets[0]) < 0) {
+            printf("Self-Test:ERR-Failed to write %s\n", mpu.accel_x_offset);
+        }
+        offsets[1] = -accel_bias[1].l / 1000 / 2 / scale_ratio;
+        if (write_sysfs_int(mpu.accel_y_offset, offsets[1]) < 0) {
+            printf("Self-Test:ERR-Failed to write %s\n", mpu.accel_y_offset);
+        }
+        offsets[2] = -accel_bias[2].l / 1000 / 2 / scale_ratio;
+        if (write_sysfs_int(mpu.accel_z_offset, offsets[2]) < 0) {
+            printf("Self-Test:ERR-Failed to write %s\n", mpu.accel_z_offset);
+        }
+        printf("Self-Test:Written Accel offsets[0..2] = [%d %d %d]\n",
+               offsets[0], offsets[1], offsets[2]);
+    }
+
+    printf("Self-Test:Saved Gyro bias[0..2] = [%ld %ld %ld]\n",
+           save_data.factory_gyro_bias[0], save_data.factory_gyro_bias[1],
+           save_data.factory_gyro_bias[2]);
+    /* write the gyro biases down to the hardware now */
+    if (write_biases_immediately) {
+        int offsets[3] = {0};
+        /* NOTE: 1000 is the dps scale of the offset registers */
+        scale_ratio = 1000 / gyro_scale.i;
+        offsets[0] = -gyro_bias[0].l / 1000 / scale_ratio;
+        if (write_sysfs_int(mpu.gyro_x_offset, offsets[0]) < 0) {
+            printf("Self-Test:ERR-Failed to write %s\n", mpu.gyro_x_offset);
+        }
+        offsets[1] = -gyro_bias[1].l / 1000 / scale_ratio;
+        if (write_sysfs_int(mpu.gyro_y_offset, offsets[1]) < 0) {
+            printf("Self-Test:ERR-Failed to write %s\n", mpu.gyro_y_offset);
+        }
+        offsets[2] = -gyro_bias[2].l / 1000 / scale_ratio;
+        if (write_sysfs_int(mpu.gyro_z_offset, offsets[2]) < 0) {
+            printf("Self-Test:ERR-Failed to write %s\n", mpu.gyro_z_offset);
+        }
+        printf("Self-Test:Written Gyro offsets[0..2] = [%d %d %d]\n",
+               offsets[0], offsets[1], offsets[2]);
+    }
+
+    printf("Self-Test:Gyro temperature @ time stored %ld\n",
+           save_data.gyro_temp);
+    printf("Self-Test:Accel temperature @ time stored %ld\n",
+           save_data.accel_temp);
+
+    // Get size of packet to store.
+    inv_get_mpl_state_size(&packet_sz);
+
+    // Create place to store data
+    buffer = (unsigned char *)malloc(packet_sz + 10);
+    if (buffer == NULL) {
+        printf("Self-Test:Can't allocate buffer\n");
+        result = -1;
+        goto free_sysfs_storage;
+    }
+
+    // Store the data
+    result = inv_save_mpl_states(buffer, packet_sz);
+    if (result) {
+        result = -1;
+    } else {
+        fptr = fopen(MLCAL_FILE, "wb+");
+        if (fptr != NULL) {
+            fwrite(buffer, 1, packet_sz, fptr);
+            fclose(fptr);
+        } else {
+            printf("Self-Test:ERR- Can't open calibration file to write - %s\n", 
+                   MLCAL_FILE);
+            result= -1;
+        }
+    }
+
+    free(buffer);
+
+restore_settings:
+    if (write_sysfs_int(mpu.dmp_on, mpu.dmp_on_value) < 0) {
+        printf("Self-Test:ERR-Failed to restore dmp_on\n");
+    }
+    if (write_sysfs_int(mpu.accel_enable, mpu.accel_enable_value) < 0) {
+        printf("Self-Test:ERR-Failed to restore accel_enable\n");
+    }
+    if (write_sysfs_int(mpu.gyro_enable, mpu.gyro_enable_value) < 0) {
+        printf("Self-Test:ERR-Failed to restore gyro_enable\n");
+    }
+    if (compass_present) {
+        if (write_sysfs_int(mpu.compass_enable, mpu.compass_enable_value) < 0) {
+            printf("Self-Test:ERR-Failed to restore compass_enable\n");
+        }
+    }
+    if (write_sysfs_int(mpu.enable, mpu.enable_value) < 0) {
+        printf("Self-Test:ERR-Failed to restore buffer/enable\n");
+    }
+    if (write_sysfs_int(mpu.power_state, mpu.power_state_value) < 0) {
+        printf("Self-Test:ERR-Failed to restore power_state\n");
+    }
+
+free_sysfs_storage:
+    free(sysfs_names_ptr);
+    return result;
+}
+
diff --git a/6515/libsensors_iio/software/simple_apps/stress_iio/README b/6515/libsensors_iio/software/simple_apps/stress_iio/README
new file mode 100644
index 0000000..eea7588
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/stress_iio/README
@@ -0,0 +1,27 @@
+Usage of inv_stress_iio:
+
+> inv_stress_iio: 
+  this will run enable and disable sequence sequentially. 
+  In the enable sequence, this will enable all the engines, output, and all dmp
+  events. 
+  In the disable sequence, this will disable all but accel engine; all outputs 
+  are disabled, only events are allowed.
+
+> inv_stress_iio -e 10 -d 1: 
+  this set the enable sequence to 10 seconds and disable sequence for 1 seconds.
+
+> inv_stress_iio -c: 
+  this includes the compass in the enable sequence when you have 9150 or other
+  secondary bus compass. It won't work for compasses on the primary bus.
+
+> inv_stress_iio -r: 
+  this set the enable and disable sequences to last a random time. It is good 
+  for stress test.
+
+> inv_stress_iio -m: 
+  this enables motion interrupt during the disable sequence. 
+  When this is enabled, the driver enters low power accel mode and disables all 
+  other sensor output (including quaternion, gyro, accel, and compass.) in the 
+  disabled sequence:
+
+
diff --git a/6515/libsensors_iio/software/simple_apps/stress_iio/build/android/shared.mk b/6515/libsensors_iio/software/simple_apps/stress_iio/build/android/shared.mk
new file mode 100644
index 0000000..fe844a5
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/stress_iio/build/android/shared.mk
@@ -0,0 +1,94 @@
+EXEC = inv_stress_iio$(SHARED_APP_SUFFIX)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
+COMP  ?= $(CROSS)gcc
+LINK  ?= $(CROSS)gcc
+
+OBJFOLDER = $(CURDIR)/obj
+
+INV_ROOT   = ../../../../..
+APP_DIR    = $(CURDIR)/../..
+MLLITE_DIR = $(INV_ROOT)/software/core/mllite
+MPL_DIR    = $(INV_ROOT)/software/core/mpl
+
+include $(INV_ROOT)/software/build/android/common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += $(ANDROID_COMPILE)
+CFLAGS += -Wall
+CFLAGS += -fpic
+CFLAGS += -nostdlib
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT
+CFLAGS += -DLINUX
+CFLAGS += -DANDROID
+CFLAGS += -mthumb-interwork
+CFLAGS += -fno-exceptions
+CFLAGS += -ffunction-sections
+CFLAGS += -funwind-tables
+CFLAGS += -fstack-protector
+CFLAGS += -fno-short-enums
+CFLAGS += -fmessage-length=0
+CFLAGS += -I$(MLLITE_DIR)
+CFLAGS += -I$(MPL_DIR)
+CFLAGS += $(INV_INCLUDES)
+CFLAGS += $(INV_DEFINES)
+
+LLINK  = -lc
+LLINK += -lm
+LLINK += -lutils
+LLINK += -lcutils
+LLINK += -lgcc
+LLINK += -ldl
+LLINK += -lstdc++
+LLINK += -llog
+LLINK += -lz
+
+LFLAGS += $(CMDLINE_LFLAGS)
+LFLAGS += $(ANDROID_LINK_EXECUTABLE)
+
+LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+####################################################################################################
+## sources
+
+INV_LIBS  = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT)
+INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
+
+#INV_SOURCES and VPATH provided by Makefile.filelist
+include ../filelist.mk
+
+INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
+INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all clean cleanall install
+
+all: $(EXEC) $(MK_NAME)
+
+$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
+	$(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+
+$(OBJFOLDER) :
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(EXEC) $(OBJFOLDER)
+
+install : $(EXEC)
+	cp -f $(EXEC) $(INSTALL_DIR)
+
+
diff --git a/6515/libsensors_iio/software/simple_apps/stress_iio/build/filelist.mk b/6515/libsensors_iio/software/simple_apps/stress_iio/build/filelist.mk
new file mode 100644
index 0000000..f201fbb
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/stress_iio/build/filelist.mk
@@ -0,0 +1,12 @@
+#### filelist.mk for stress_iio ####
+
+# headers
+#HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h
+HEADERS += $(APP_DIR)/iio_utils.h
+
+# sources
+SOURCES := $(APP_DIR)/stress_iio.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux
diff --git a/6515/libsensors_iio/software/simple_apps/stress_iio/iio_utils.h b/6515/libsensors_iio/software/simple_apps/stress_iio/iio_utils.h
new file mode 100644
index 0000000..d62c573
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/stress_iio/iio_utils.h
@@ -0,0 +1,700 @@
+/* IIO - useful set of util functionality
+ *
+ * Copyright (c) 2008 Jonathan Cameron
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+/* Made up value to limit allocation sizes */
+#include <string.h>
+#include <stdlib.h>
+#include <ctype.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <dirent.h>
+
+#define IIO_MAX_NAME_LENGTH 30
+
+#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements"
+#define FORMAT_TYPE_FILE "%s_type"
+
+const char *iio_dir = "/sys/bus/iio/devices/";
+
+/**
+ * iioutils_break_up_name() - extract generic name from full channel name
+ * @full_name: the full channel name
+ * @generic_name: the output generic channel name
+ **/
+static int iioutils_break_up_name(const char *full_name,
+				  char **generic_name)
+{
+	char *current;
+	char *w, *r;
+	char *working;
+	current = strdup(full_name);
+	working = strtok(current, "_\0");
+	w = working;
+	r = working;
+
+	while (*r != '\0') {
+		if (!isdigit(*r)) {
+			*w = *r;
+			w++;
+		}
+		r++;
+	}
+	*w = '\0';
+	*generic_name = strdup(working);
+	free(current);
+
+	return 0;
+}
+
+/**
+ * struct iio_channel_info - information about a given channel
+ * @name: channel name
+ * @generic_name: general name for channel type
+ * @scale: scale factor to be applied for conversion to si units
+ * @offset: offset to be applied for conversion to si units
+ * @index: the channel index in the buffer output
+ * @bytes: number of bytes occupied in buffer output
+ * @mask: a bit mask for the raw output
+ * @is_signed: is the raw value stored signed
+ * @enabled: is this channel enabled
+ **/
+struct iio_channel_info {
+	char *name;
+	char *generic_name;
+	float scale;
+	float offset;
+	unsigned index;
+	unsigned bytes;
+	unsigned bits_used;
+	unsigned shift;
+	uint64_t mask;
+	unsigned be;
+	unsigned is_signed;
+	unsigned enabled;
+	unsigned location;
+};
+
+/**
+ * iioutils_get_type() - find and process _type attribute data
+ * @is_signed: output whether channel is signed
+ * @bytes: output how many bytes the channel storage occupies
+ * @mask: output a bit mask for the raw data
+ * @be: big endian
+ * @device_dir: the iio device directory
+ * @name: the channel name
+ * @generic_name: the channel type name
+ **/
+inline int iioutils_get_type(unsigned *is_signed,
+			     unsigned *bytes,
+			     unsigned *bits_used,
+			     unsigned *shift,
+			     uint64_t *mask,
+			     unsigned *be,
+			     const char *device_dir,
+			     const char *name,
+			     const char *generic_name)
+{
+	FILE *sysfsfp;
+	int ret;
+	DIR *dp;
+	char *scan_el_dir, *builtname, *builtname_generic, *filename = 0;
+	char signchar, endianchar;
+	unsigned padint;
+	const struct dirent *ent;
+
+	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	ret = asprintf(&builtname, FORMAT_TYPE_FILE, name);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_free_scan_el_dir;
+	}
+	ret = asprintf(&builtname_generic, FORMAT_TYPE_FILE, generic_name);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_free_builtname;
+	}
+
+	dp = opendir(scan_el_dir);
+	if (dp == NULL) {
+		ret = -errno;
+		goto error_free_builtname_generic;
+	}
+	while (ent = readdir(dp), ent != NULL)
+		/*
+		 * Do we allow devices to override a generic name with
+		 * a specific one?
+		 */
+		if ((strcmp(builtname, ent->d_name) == 0) ||
+		    (strcmp(builtname_generic, ent->d_name) == 0)) {
+			ret = asprintf(&filename,
+				       "%s/%s", scan_el_dir, ent->d_name);
+			if (ret < 0) {
+				ret = -ENOMEM;
+				goto error_closedir;
+			}
+			sysfsfp = fopen(filename, "r");
+			if (sysfsfp == NULL) {
+				printf("failed to open %s\n", filename);
+				ret = -errno;
+				goto error_free_filename;
+			}
+
+			ret = fscanf(sysfsfp,
+				     "%ce:%c%u/%u>>%u",
+				     &endianchar,
+				     &signchar,
+				     bits_used,
+				     &padint, shift);
+			if (ret < 0) {
+				printf("failed to pass scan type description\n");
+				return ret;
+			}
+			*be = (endianchar == 'b');
+			*bytes = padint / 8;
+			if (*bits_used == 64)
+				*mask = ~0;
+			else
+				*mask = (1 << *bits_used) - 1;
+			if (signchar == 's')
+				*is_signed = 1;
+			else
+				*is_signed = 0;
+			fclose(sysfsfp);
+			free(filename);
+
+			filename = 0;
+		}
+error_free_filename:
+	if (filename)
+		free(filename);
+error_closedir:
+	closedir(dp);
+error_free_builtname_generic:
+	free(builtname_generic);
+error_free_builtname:
+	free(builtname);
+error_free_scan_el_dir:
+	free(scan_el_dir);
+error_ret:
+	return ret;
+}
+
+inline int iioutils_get_param_float(float *output,
+				    const char *param_name,
+				    const char *device_dir,
+				    const char *name,
+				    const char *generic_name)
+{
+	FILE *sysfsfp;
+	int ret;
+	DIR *dp;
+	char *builtname, *builtname_generic;
+	char *filename = NULL;
+	const struct dirent *ent;
+
+	ret = asprintf(&builtname, "%s_%s", name, param_name);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	ret = asprintf(&builtname_generic,
+		       "%s_%s", generic_name, param_name);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_free_builtname;
+	}
+	dp = opendir(device_dir);
+	if (dp == NULL) {
+		ret = -errno;
+		goto error_free_builtname_generic;
+	}
+	while (ent = readdir(dp), ent != NULL)
+		if ((strcmp(builtname, ent->d_name) == 0) ||
+		    (strcmp(builtname_generic, ent->d_name) == 0)) {
+			ret = asprintf(&filename,
+				       "%s/%s", device_dir, ent->d_name);
+			if (ret < 0) {
+				ret = -ENOMEM;
+				goto error_closedir;
+			}
+			sysfsfp = fopen(filename, "r");
+			if (!sysfsfp) {
+				ret = -errno;
+				goto error_free_filename;
+			}
+			fscanf(sysfsfp, "%f", output);
+			break;
+		}
+error_free_filename:
+	if (filename)
+		free(filename);
+error_closedir:
+	closedir(dp);
+error_free_builtname_generic:
+	free(builtname_generic);
+error_free_builtname:
+	free(builtname);
+error_ret:
+	return ret;
+}
+
+/**
+ * bsort_channel_array_by_index() - reorder so that the array is in index order
+ *
+ **/
+
+inline void bsort_channel_array_by_index(struct iio_channel_info **ci_array,
+					 int cnt)
+{
+
+	struct iio_channel_info temp;
+	int x, y;
+
+	for (x = 0; x < cnt; x++)
+		for (y = 0; y < (cnt - 1); y++)
+			if ((*ci_array)[y].index > (*ci_array)[y+1].index) {
+				temp = (*ci_array)[y + 1];
+				(*ci_array)[y + 1] = (*ci_array)[y];
+				(*ci_array)[y] = temp;
+			}
+}
+
+/**
+ * build_channel_array() - function to figure out what channels are present
+ * @device_dir: the IIO device directory in sysfs
+ * @
+ **/
+inline int build_channel_array(const char *device_dir,
+			      struct iio_channel_info **ci_array,
+			      int *counter)
+{
+	DIR *dp;
+	FILE *sysfsfp;
+	int count, i;
+	struct iio_channel_info *current;
+	int ret;
+	const struct dirent *ent;
+	char *scan_el_dir;
+	char *filename;
+
+	*counter = 0;
+	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	dp = opendir(scan_el_dir);
+	if (dp == NULL) {
+		ret = -errno;
+		goto error_free_name;
+	}
+	while (ent = readdir(dp), ent != NULL)
+		if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
+			   "_en") == 0) {
+			ret = asprintf(&filename,
+				       "%s/%s", scan_el_dir, ent->d_name);
+			if (ret < 0) {
+				ret = -ENOMEM;
+				goto error_close_dir;
+			}
+			sysfsfp = fopen(filename, "r");
+			if (sysfsfp == NULL) {
+				ret = -errno;
+				free(filename);
+				goto error_close_dir;
+			}
+			fscanf(sysfsfp, "%u", &ret);
+			printf("%s, %d\n", filename, ret);
+			if (ret == 1)
+				(*counter)++;
+			fclose(sysfsfp);
+			free(filename);
+		}
+	*ci_array = malloc(sizeof(**ci_array) * (*counter));
+	if (*ci_array == NULL) {
+		ret = -ENOMEM;
+		goto error_close_dir;
+	}
+	closedir(dp);
+	dp = opendir(scan_el_dir);
+	//seekdir(dp, 0);
+	count = 0;
+	while (ent = readdir(dp), ent != NULL) {
+		if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
+			   "_en") == 0) {
+			current = &(*ci_array)[count++];
+			ret = asprintf(&filename,
+				       "%s/%s", scan_el_dir, ent->d_name);
+			if (ret < 0) {
+				ret = -ENOMEM;
+				/* decrement count to avoid freeing name */
+				count--;
+				goto error_cleanup_array;
+			}
+			sysfsfp = fopen(filename, "r");
+			if (sysfsfp == NULL) {
+				free(filename);
+				ret = -errno;
+				goto error_cleanup_array;
+			}
+			fscanf(sysfsfp, "%u", &current->enabled);
+			fclose(sysfsfp);
+
+			if (!current->enabled) {
+				free(filename);
+				count--;
+				continue;
+			}
+
+			current->scale = 1.0;
+			current->offset = 0;
+			current->name = strndup(ent->d_name,
+						strlen(ent->d_name) -
+						strlen("_en"));
+			if (current->name == NULL) {
+				free(filename);
+				ret = -ENOMEM;
+				goto error_cleanup_array;
+			}
+			/* Get the generic and specific name elements */
+			ret = iioutils_break_up_name(current->name,
+						     &current->generic_name);
+			if (ret) {
+				free(filename);
+				goto error_cleanup_array;
+			}
+			ret = asprintf(&filename,
+				       "%s/%s_index",
+				       scan_el_dir,
+				       current->name);
+			if (ret < 0) {
+				free(filename);
+				ret = -ENOMEM;
+				goto error_cleanup_array;
+			}
+			sysfsfp = fopen(filename, "r");
+			fscanf(sysfsfp, "%u", &current->index);
+			fclose(sysfsfp);
+			free(filename);
+			/* Find the scale */
+			ret = iioutils_get_param_float(&current->scale,
+						       "scale",
+						       device_dir,
+						       current->name,
+						       current->generic_name);
+			if (ret < 0)
+				goto error_cleanup_array;
+			ret = iioutils_get_param_float(&current->offset,
+						       "offset",
+						       device_dir,
+						       current->name,
+						       current->generic_name);
+			if (ret < 0)
+				goto error_cleanup_array;
+			ret = iioutils_get_type(&current->is_signed,
+						&current->bytes,
+						&current->bits_used,
+						&current->shift,
+						&current->mask,
+						&current->be,
+						device_dir,
+						current->name,
+						current->generic_name);
+		}
+	}
+
+	closedir(dp);
+	/* reorder so that the array is in index order */
+	bsort_channel_array_by_index(ci_array, *counter);
+
+	return 0;
+
+error_cleanup_array:
+	for (i = count - 1;  i >= 0; i--)
+		free((*ci_array)[i].name);
+	free(*ci_array);
+error_close_dir:
+	closedir(dp);
+error_free_name:
+	free(scan_el_dir);
+error_ret:
+	return ret;
+}
+
+inline int _write_sysfs_int(char *filename, char *basedir, int val, int verify)
+{
+	int ret;
+	FILE *sysfsfp;
+	int test;
+	//char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+	char temp[200];
+	ret = 0;
+	if (temp == NULL)
+		return -ENOMEM;
+	sprintf(temp, "%s/%s", basedir, filename);
+	sysfsfp = fopen(temp, "w");
+	if (sysfsfp == NULL) {
+		printf("failed to open write %s\n", temp);
+		ret = -errno;
+		printf("ERROR1=%d\n", ret);
+		while(1);
+		goto error_free;
+	}
+	fprintf(sysfsfp, "%d", val);
+	fclose(sysfsfp);
+	if (verify) {
+		sysfsfp = fopen(temp, "r");
+		if (sysfsfp == NULL) {
+			printf("failed to open read %s\n", temp);
+			ret = -errno;
+			printf("ERROR2=%d\n", ret);
+			while(1);
+			goto error_free;
+		}
+		fscanf(sysfsfp, "%d", &test);
+		if (test != val) {
+			printf("Possible failure in int write %d to %s%s\n",
+				val,
+				basedir,
+				filename);
+			ret = -1;
+		}
+	}
+error_free:
+	fclose(sysfsfp);
+	//free(temp);
+	return ret;
+}
+
+int write_sysfs_int(char *filename, char *basedir, int val)
+{
+	return _write_sysfs_int(filename, basedir, val, 0);
+}
+
+int write_sysfs_int_and_verify(char *filename, char *basedir, int val)
+{
+	printf("echo %d > %s/%s\n", val, basedir, filename);
+	return _write_sysfs_int(filename, basedir, val, 1);
+}
+
+inline int write_sysfs_int64(char *filename, char *basedir, long long val)
+{
+	int ret;
+	FILE *sysfsfp;
+	int test;
+	//char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+	char temp[200];
+	ret = 0;
+	if (temp == NULL)
+		return -ENOMEM;
+	sprintf(temp, "%s/%s", basedir, filename);
+	sysfsfp = fopen(temp, "w");
+	if (sysfsfp == NULL) {
+		printf("failed to open write %s\n", temp);
+		ret = -errno;
+		printf("ERROR1=%d\n", ret);
+		while(1);
+		goto error_free;
+	}
+	fprintf(sysfsfp, "%lld", val);
+	fclose(sysfsfp);
+error_free:
+	fclose(sysfsfp);
+	//free(temp);
+	return ret;
+}
+
+int _write_sysfs_string(char *filename, char *basedir, char *val, int verify)
+{
+	int ret = 0;
+	FILE  *sysfsfp;
+	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+	if (temp == NULL) {
+		printf("Memory allocation failed\n");
+		return -ENOMEM;
+	}
+	sprintf(temp, "%s/%s", basedir, filename);
+	sysfsfp = fopen(temp, "w");
+	if (sysfsfp == NULL) {
+		printf("Could not open %s\n", temp);
+		ret = -errno;
+		goto error_free;
+	}
+	fprintf(sysfsfp, "%s", val);
+	fclose(sysfsfp);
+	if (verify) {
+		sysfsfp = fopen(temp, "r");
+		if (sysfsfp == NULL) {
+			printf("could not open file to verify\n");
+			ret = -errno;
+			goto error_free;
+		}
+		fscanf(sysfsfp, "%s", temp);
+		if (strcmp(temp, val) != 0) {
+			printf("Possible failure in string write of %s "
+				"Should be %s "
+				"written to %s\%s\n",
+				temp,
+				val,
+				basedir,
+				filename);
+			ret = -1;
+		}
+	}
+error_free:
+	free(temp);
+
+	return ret;
+}
+
+/**
+ * write_sysfs_string_and_verify() - string write, readback and verify
+ * @filename: name of file to write to
+ * @basedir: the sysfs directory in which the file is to be found
+ * @val: the string to write
+ **/
+int write_sysfs_string_and_verify(char *filename, char *basedir, char *val)
+{
+	return _write_sysfs_string(filename, basedir, val, 1);
+}
+
+int write_sysfs_string(char *filename, char *basedir, char *val)
+{
+	return _write_sysfs_string(filename, basedir, val, 0);
+}
+
+int read_sysfs_posint(char *filename, char *basedir)
+{
+	int ret;
+	FILE  *sysfsfp;
+	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+	if (temp == NULL) {
+		printf("Memory allocation failed");
+		return -ENOMEM;
+	}
+	sprintf(temp, "%s/%s", basedir, filename);
+	sysfsfp = fopen(temp, "r");
+	if (sysfsfp == NULL) {
+		ret = -errno;
+		goto error_free;
+	}
+	fscanf(sysfsfp, "%d\n", &ret);
+	fclose(sysfsfp);
+error_free:
+	free(temp);
+	return ret;
+}
+
+long long read_sysfs_poslonglong(char *filename, char *basedir)
+{
+	long long ret;
+	FILE  *sysfsfp;
+	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+	if (temp == NULL) {
+		printf("Memory allocation failed");
+		return -ENOMEM;
+	}
+	sprintf(temp, "%s/%s", basedir, filename);
+	sysfsfp = fopen(temp, "r");
+	if (sysfsfp == NULL) {
+		ret = -errno;
+		goto error_free;
+	}
+	fscanf(sysfsfp, "%lld\n", &ret);
+	fclose(sysfsfp);
+error_free:
+	free(temp);
+	return ret;
+}
+
+int read_sysfs_float(char *filename, char *basedir, float *val)
+{
+	float ret = 0;
+	FILE  *sysfsfp;
+	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+	if (temp == NULL) {
+		printf("Memory allocation failed");
+		return -ENOMEM;
+	}
+	sprintf(temp, "%s/%s", basedir, filename);
+	sysfsfp = fopen(temp, "r");
+	if (sysfsfp == NULL) {
+		ret = -errno;
+		goto error_free;
+	}
+	fscanf(sysfsfp, "%f\n", val);
+	fclose(sysfsfp);
+error_free:
+	free(temp);
+	return ret;
+}
+int enable(const char *device_dir,
+			      struct iio_channel_info **ci_array,
+			      int *counter)
+{
+	DIR *dp;
+	int ret;
+	const struct dirent *ent;
+	char *scan_el_dir;
+
+	*counter = 0;
+	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	dp = opendir(scan_el_dir);
+	if (dp == NULL) {
+		ret = -errno;
+		goto error_free_name;
+	}
+	while (ent = readdir(dp), ent != NULL)
+		if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
+			   "_en") == 0) {
+			write_sysfs_int_and_verify((char *)ent->d_name, scan_el_dir, 1);
+		}
+	return 0;
+error_ret:
+error_free_name:
+	return -1;
+}
+int disable_q_out(const char *device_dir,
+			      struct iio_channel_info **ci_array,
+			      int *counter) {
+	DIR *dp;
+	int ret;
+	const struct dirent *ent;
+	char *scan_el_dir;
+
+	*counter = 0;
+	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	dp = opendir(scan_el_dir);
+	if (dp == NULL) {
+		ret = -errno;
+		goto error_free_name;
+	}
+	while (ent = readdir(dp), ent != NULL)
+		if (strncmp(ent->d_name, "in_quaternion", strlen("in_quaternion")) == 0) {
+			write_sysfs_int_and_verify((char *)ent->d_name, scan_el_dir, 0);
+		}
+	return 0;
+error_ret:
+error_free_name:
+	return -1;
+
+}
+
diff --git a/6515/libsensors_iio/software/simple_apps/stress_iio/stress_iio.c b/6515/libsensors_iio/software/simple_apps/stress_iio/stress_iio.c
new file mode 100644
index 0000000..32bb7ad
--- /dev/null
+++ b/6515/libsensors_iio/software/simple_apps/stress_iio/stress_iio.c
@@ -0,0 +1,1011 @@
+/* Industrialio buffer test code.
+ *
+ * Copyright (c) 2012 Invensense Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * Command line parameters
+ * stress_iio -d time1 -e time2
+ */
+
+#include <unistd.h>
+#include <dirent.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <errno.h>
+#include <sys/stat.h>
+#include <dirent.h>
+#include <linux/types.h>
+#include <string.h>
+#include <poll.h>
+#include <pthread.h>
+#include "iio_utils.h"
+#include "ml_load_dmp.h"
+#include "ml_sysfs_helper.h"
+#include "authenticate.h"
+
+pthread_mutex_t data_switch_lock = PTHREAD_MUTEX_INITIALIZER;
+
+static int has_compass = 0;
+static int has_pressure = 0;
+static int enable_random_delay = 0;
+static int enable_delay = 10;
+static int disable_delay = 10;
+static int enable_motion_on = 0;
+static int final_output_rate;
+static int first_flag;
+static char dmp_path[200];
+
+static int dev_num;
+static char *dev_dir_name;
+static char *buf_dir_name;
+static char *scan_el_dir;
+static int gyro_data_is_enabled, accel_data_is_enabled, compass_data_is_enabled, quaternion_data_is_enabled;
+static int accel_engine_is_on;
+
+struct dmp_struct {
+	char fname[100];
+	void (*action)(struct dmp_struct *, int);
+};
+
+static void HandleTap(struct dmp_struct *dmp, int tap);
+static void sipmle_print(struct dmp_struct *dmp, int d){
+	printf("%s:%d\n", dmp->fname, d);
+}
+
+static void handle_smd() {
+	printf("write wake lock for SMD\n");
+	//write_sysfs_string_and_verify("wake_lock", "/sys/power/", "hack");
+}
+
+static void pedo_print()
+{
+	printf("steps=%lld, time=%lld\n",
+		read_sysfs_poslonglong("pedometer_steps", dev_dir_name),
+		read_sysfs_poslonglong("pedometer_time", dev_dir_name));
+}
+
+struct dmp_struct event_file[] = {
+#if 1
+	{
+		.fname = "event_tap",
+		.action = HandleTap,
+	},
+#endif
+	{
+		.fname = "event_smd",
+		.action = handle_smd,
+	},
+	{
+		.fname = "event_accel_motion",
+		.action = sipmle_print,
+	},
+	{
+		.fname = "event_pedometer",
+		.action = pedo_print,
+	},
+};
+
+static void HandleTap(struct dmp_struct *dmp, int tap)
+{
+    int tap_dir = tap/8;
+    int tap_num = tap%8 + 1;
+
+    switch (tap_dir) {
+        case 1:
+            printf("INV_TAP_AXIS_X_POS\n");
+            break;
+        case 2:
+            printf("INV_TAP_AXIS_X_NEG\n");
+            break;
+        case 3:
+            printf("INV_TAP_AXIS_Y_POS\n");
+            break;
+        case 4:
+            printf("INV_TAP_AXIS_Y_NEG\n");
+            break;
+        case 5:
+            printf("INV_TAP_AXIS_Z_POS\n");
+            break;
+        case 6:
+            printf("INV_TAP_AXIS_Z_NEG\n");
+            break;
+        default:
+            break;
+    }
+    printf("Tap number: %d\n", tap_num);
+}
+#define DMP_CODE_SIZE 2799
+static char dmp_img[DMP_CODE_SIZE];
+static void verify_img(){
+    FILE *fp;
+    int i;
+    char dmp_path[] = "/sys/bus/iio/devices/iio:device0/dmp_firmware";
+
+    printf("saving image\n");
+    if ((fp = fopen(dmp_path, "rb")) < 0 ) {
+        perror("dmp fail");
+    }
+
+    i = fread(dmp_img, 1, DMP_CODE_SIZE, fp);
+    printf("Result=%d\n", i);
+    fclose(fp);
+    fp = fopen("/dev/read_img.h", "wt");
+    fprintf(fp, "unsigned char rec[]={\n");
+    for(i=0; i<DMP_CODE_SIZE; i++) {
+      fprintf(fp, "0x%02x, ", dmp_img[i]);
+      //printf( "0x%02x, ", dmp_img[i]);
+      if(((i+1)%16) == 0) {
+        fprintf(fp, "\n");
+        //printf("\n");
+      }
+    }
+    fprintf(fp, "};\n ");
+    fclose(fp);
+    printf("saving image Done\n");
+}
+
+static void inv_set_rate()
+{
+	int ret;
+
+	printf("set rate \n");
+	ret = write_sysfs_int_and_verify("accel_rate", dev_dir_name, 5);
+	ret = write_sysfs_int_and_verify("gyro_rate", dev_dir_name, 5);
+	if (has_compass)
+		ret = write_sysfs_int_and_verify("compass_rate", dev_dir_name, 50);
+	if (has_pressure)
+		ret = write_sysfs_int_and_verify("pressure_rate", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("ped_q_rate", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("six_axes_q_rate", dev_dir_name, 15);
+	ret = write_sysfs_int_and_verify("three_axes_q_rate", dev_dir_name, 5);
+}
+
+
+static int setup_offset_and_bias()
+{
+	int ret;
+
+	ret = write_sysfs_int_and_verify("in_accel_x_offset", dev_dir_name, 0);
+	if (ret < 0)
+		printf("write accel x offset failed.\n");
+	ret = write_sysfs_int_and_verify("in_accel_y_offset", dev_dir_name, 0);
+	if (ret < 0)
+		printf("write accel y offset failed.\n");
+	ret = write_sysfs_int_and_verify("in_accel_z_offset", dev_dir_name, 0);
+	if (ret < 0)
+		printf("write accel z offset failed.\n");
+
+	ret = write_sysfs_int_and_verify("in_anglvel_x_offset", dev_dir_name, 0);
+	if (ret < 0)
+		printf("write accel x offset failed.\n");
+	ret = write_sysfs_int_and_verify("in_anglvel_y_offset", dev_dir_name, 0);
+	if (ret < 0)
+		printf("write accel y offset failed.\n");
+	ret = write_sysfs_int_and_verify("in_anglvel_z_offset", dev_dir_name, 0);
+	if (ret < 0)
+		printf("write accel z offset failed.\n"); 
+
+	ret = write_sysfs_int_and_verify("in_accel_x_dmp_bias", dev_dir_name, 0);
+	if (ret < 0)
+		printf("write accel x offset failed.\n");
+	ret = write_sysfs_int_and_verify("in_accel_y_dmp_bias", dev_dir_name, 0);
+	if (ret < 0)
+		printf("write accel y offset failed.\n");
+	ret = write_sysfs_int_and_verify("in_accel_z_dmp_bias", dev_dir_name, 0);
+	if (ret < 0)
+		printf("write accel z offset failed.\n");
+
+	ret = write_sysfs_int_and_verify("in_anglvel_x_dmp_bias", dev_dir_name, 0);
+	if (ret < 0)
+		printf("write gyro x offset failed.\n");
+	ret = write_sysfs_int_and_verify("in_anglvel_y_dmp_bias", dev_dir_name, 0);
+	if (ret < 0)
+		printf("write gyro y offset failed.\n");
+	ret = write_sysfs_int_and_verify("in_anglvel_z_dmp_bias", dev_dir_name, 0);
+	if (ret < 0)
+		printf("write gyro z offset failed.\n");
+
+	return 0;
+}
+
+static void setup_dmp(char *dev_path){
+	char sysfs_path[200];
+	int  ret;
+	FILE *fd;
+	sprintf(sysfs_path, "%s", dev_path);
+	printf("sysfs: %s\n", sysfs_path);
+	ret = write_sysfs_int_and_verify("power_state", sysfs_path, 1);
+	if (ret < 0)
+		return;
+
+	ret = write_sysfs_int("in_accel_scale", dev_path, 0);
+	if (ret < 0)
+		return;
+	ret = write_sysfs_int("in_anglvel_scale", dev_path, 3);
+	if (ret < 0)
+		return;	
+	ret = write_sysfs_int("sampling_frequency", sysfs_path, 200);
+	if (ret < 0)
+		return;	
+	ret = write_sysfs_int_and_verify("firmware_loaded", sysfs_path, 0);
+	if (ret < 0)
+		return;
+	sprintf(dmp_path, "%s/dmp_firmware", dev_path);
+	if ((fd = fopen(dmp_path, "wb")) < 0 ) {
+		perror("dmp fail");
+	}	
+	inv_load_dmp(fd);
+	fclose(fd);
+	printf("firmware_loaded=%d\n", read_sysfs_posint("firmware_loaded", sysfs_path));
+	ret = write_sysfs_int_and_verify("dmp_on", sysfs_path, 1);
+	if (ret < 0)
+		return;
+	ret = write_sysfs_int_and_verify("dmp_int_on", sysfs_path, 1);
+	if (ret < 0)
+		return;
+	/* selelct which event to enable and interrupt on/off here */
+	//enable_glu(sysfs_path, 0);
+	ret = write_sysfs_int_and_verify("tap_on", sysfs_path, 0);
+	if (ret < 0)
+		return;
+	ret = write_sysfs_int_and_verify("pedometer_int_on", sysfs_path, 1);
+	ret = write_sysfs_int_and_verify("pedometer_on", sysfs_path, 1);
+
+	ret = write_sysfs_int_and_verify("dmp_event_int_on", sysfs_path, 1);
+		write_sysfs_int64("pedometer_steps", sysfs_path, 0x3ffffffff);
+		write_sysfs_int64("pedometer_time", sysfs_path, 0xffffffff);
+	if (ret < 0)
+		return;
+
+	ret = setup_offset_and_bias();
+
+	return;
+}
+
+#if 0
+static char reg_dump_arr[2000];
+static int inv_do_reg_dump(void)
+{
+	char reg_dump_name[100];
+	int fd, i;
+
+	sprintf(reg_dump_name, "%s/reg_dump", dev_dir_name);
+	printf("%s\n", reg_dump_name);
+	fd = open(reg_dump_name, O_RDONLY);
+	pread(fd, reg_dump_arr, 2000, 0);
+	close(fd);
+	for ( i = 0; i < 2000; i++) {
+		printf("%c", reg_dump_arr[i]);
+		//if((i+1)%16 == 0)
+			//printf("\n");
+	}
+	return 0;
+}
+#endif
+
+static void *get_dmp_event(void *param) {
+	char file_name[100];
+	int i;
+	int data;
+	char d[4];
+	FILE *fp;
+	struct pollfd pfd[ARRAY_SIZE(event_file)];
+
+	printf("get DMP event: %s\n", dev_dir_name);
+	while(1) {
+		for (i = 0; i < ARRAY_SIZE(event_file); i++) {
+			sprintf(file_name, "%s/%s", dev_dir_name, event_file[i].fname);
+			pfd[i].fd = open(file_name, O_RDONLY | O_NONBLOCK);
+			pfd[i].events = POLLPRI|POLLERR;
+			pfd[i].revents = 0;
+			read(pfd[i].fd, d, 4);
+		}
+
+		poll(pfd, ARRAY_SIZE(event_file), -1);
+		for (i = 0; i < ARRAY_SIZE(event_file); i++) {
+			close(pfd[i].fd);
+		}
+
+		for (i=0; i< ARRAY_SIZE(pfd); i++) {
+			if(pfd[i].revents != 0) {
+				sprintf(file_name, "%s/%s", dev_dir_name, event_file[i].fname);
+				fp = fopen(file_name, "rt");
+				fscanf(fp, "%d\n", &data);
+				event_file[i].action(&event_file[i], data);
+			}
+		}						
+	}
+	
+	return 0;
+}
+
+static int enable_gyro(int on){
+	int ret;
+	ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, on);
+	if (ret < 0)
+		printf("write gyro_enable failed\n");
+
+	return ret;
+}
+
+static int enable_gyro_output(int on){
+	int ret;
+	gyro_data_is_enabled = on;
+	ret = write_sysfs_int_and_verify("gyro_fifo_enable", dev_dir_name, on);
+	if (ret < 0)
+		printf("write gyro_fifo_enable failed\n");
+
+	return ret;
+}
+
+static int enable_compass(int on){
+	int ret;
+
+	compass_data_is_enabled = on;
+	ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, on);
+	if (ret < 0)
+		printf("write gyro_enable failed\n");
+
+	return ret;
+}
+
+static int enable_pressure(int on){
+	int ret;
+
+	ret = write_sysfs_int_and_verify("pressure_enable", dev_dir_name, on);
+	if (ret < 0)
+		printf("write pressure_enable failed\n");
+
+	return ret;
+}
+
+static int enable_quaternion(int on) {
+	int ret;
+	ret = write_sysfs_int_and_verify("ped_q_on", dev_dir_name, on);
+	if (ret < 0)
+		printf("write quaternion_on failed\n");
+	ret = write_sysfs_int_and_verify("six_axes_q_on", dev_dir_name, on);
+	if (ret < 0)
+		printf("write quaternion_on failed\n");
+	ret = write_sysfs_int_and_verify("three_axes_q_on", dev_dir_name, on);
+	if (ret < 0)
+		printf("write quaternion_on failed\n");
+
+	return ret;
+}
+static int enable_step_detector(int on) {
+	int ret;
+
+	ret = write_sysfs_int_and_verify("step_detector_on", dev_dir_name, on);
+	if (ret < 0)
+		printf("write step detector on failed\n");
+}
+static int enable_step_indicator(int on) {
+	int ret;
+
+	ret = write_sysfs_int_and_verify("step_indicator_on", dev_dir_name, on);
+	if (ret < 0)
+		printf("write step indicator on failed\n");
+}
+
+static int enable_accel(int on){
+	int ret;
+	accel_data_is_enabled = on;
+	accel_engine_is_on = on;
+	ret = write_sysfs_int_and_verify("accel_enable", dev_dir_name, on);
+	if (ret < 0)
+		printf("write accel_enable failed\n");
+	ret = write_sysfs_int_and_verify("accel_fifo_enable", dev_dir_name, on);
+	if (ret < 0)
+		printf("write accel_fifo_enable failed\n");
+
+	return ret;
+}
+static int enable_accel_output(int on) {
+	int ret;
+	accel_data_is_enabled = on;
+
+	ret = write_sysfs_int_and_verify("accel_fifo_enable", dev_dir_name, on);
+	if (ret < 0)
+		printf("write accel_fifo_enable failed\n");
+
+	return ret;
+}
+
+static int enable_enable(int on){
+	int ret;
+
+	if (0 == on) {
+		pthread_mutex_lock(&data_switch_lock);
+	}
+	ret = write_sysfs_int_and_verify("master_enable", dev_dir_name, on);
+	if (ret < 0)
+		printf("write enable failed\n");
+
+	if (on) {
+		pthread_mutex_unlock(&data_switch_lock);
+	}
+
+	return 0;
+}
+static int write_dmp_event(int on) {
+	int ret;
+	ret = write_sysfs_int_and_verify("dmp_event_int_on", dev_dir_name, on);
+	if (ret < 0)
+		printf("write dmp_event_int_on failed\n");
+	return 0;
+}
+
+static void random_delay(){
+	int i;
+	float bb;
+
+	i = rand();
+	bb = i * 200.0;
+	bb = i * 10.0;
+	i = 1 + (unsigned int)(bb/(RAND_MAX + 1.0));
+	i *= 2;
+	if (i%2) {
+		printf("sleep %d ms\n", i);
+		usleep(i*1000);
+	} else {
+		printf("sleep %d s\n", i);
+		sleep(i);
+	}
+
+}
+static void dmp_event_control(on){
+	int ret;
+
+	ret = 0;
+
+//	ret = write_sysfs_int_and_verify("tap_on", dev_dir_name, on);
+	ret = write_sysfs_int_and_verify("smd_enable", dev_dir_name, 1);
+	if (ret < 0)
+		return;
+	inv_set_rate();
+
+	//ret = write_sysfs_int_and_verify("batchmode_wake_fifo_full_on", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("batchmode_timeout", dev_dir_name, 5000);
+//	ret = write_sysfs_int_and_verify("batchmode_timeout", dev_dir_name, 0);
+	//ret = write_sysfs_int_and_verify("smd_delay_threshold", dev_dir_name, 10);
+	if (ret < 0)
+		return;
+	//ret = write_sysfs_int_and_verify("smd_threshold", dev_dir_name, 5000);
+	if (ret < 0)
+		return;
+	//write_sysfs_int_and_verify("motion_lpa_duration", dev_dir_name, 1000);
+	//write_sysfs_int_and_verify("motion_lpa_threshold", dev_dir_name, 200);
+	write_sysfs_int_and_verify("dmp_on", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("sampling_frequency", dev_dir_name, 200);
+	//write_sysfs_int_and_verify("motion_lpa_freq", dev_dir_name, 3);
+
+}
+void enable_motion(int on) {
+	int ret;
+
+	ret = write_sysfs_int_and_verify("motion_lpa_on", dev_dir_name, on);
+	if (on) {
+	        gyro_data_is_enabled = 0;
+		compass_data_is_enabled = 0;
+		quaternion_data_is_enabled = 0;
+	}
+}
+bool g, a;
+static int counter = 0;
+static unsigned char data_rate[] = {5, 10, 15, 50, 100, 200};
+static int run_enable_sequence()
+{
+	bool g, a, out;
+
+	counter++;
+	g = rand()%2;
+	a = rand()%2;
+	if (!g && !a)
+		a = true;
+
+	//g = true;
+	//a = true;
+	/*disable the master enable */
+	enable_enable(0);
+	if(g) {
+		enable_gyro(1);
+		if (rand()%2) {
+			out = rand()%2;
+			enable_quaternion(out);
+			enable_gyro_output(!out);
+		} else {
+			enable_quaternion(1);
+			enable_gyro_output(1);
+		}
+	//	enable_quaternion(0);
+	//	enable_gyro_output(0);
+
+	} else {
+		enable_gyro(0);
+		enable_gyro_output(0);
+		enable_quaternion(0);
+	}
+	if(a) {
+		enable_accel(1);
+		enable_accel_output(1);
+	} else {
+		enable_accel(0);
+		enable_accel_output(0);
+	}
+	if (has_compass) {
+		if(rand()%2)
+			enable_compass(1);
+		else
+			enable_compass(0);
+		enable_compass(counter%2);
+		//enable_compass(0);
+	}
+	if (has_pressure) {
+		if(rand()%2)
+			enable_pressure(1);
+		else
+			enable_pressure(0);
+		enable_pressure(counter%3);
+		//enable_pressure(0);
+	}
+	enable_step_detector(1);
+	enable_step_indicator(1);
+	//enable_step_detector(0);
+	//enable_step_indicator(0);
+
+	write_dmp_event(0);
+
+	enable_motion(0);
+	if (accel_engine_is_on)
+		dmp_event_control(1);
+	else
+		dmp_event_control(0);
+	first_flag = 1;
+	/*enable the master enable */
+	enable_enable(1);
+	//write_sysfs_string_and_verify("wake_unlock", "/sys/power/", "hack");
+	if (enable_random_delay)
+		random_delay();
+	else {
+		printf("sleep %ds\n", enable_delay);
+		sleep(enable_delay);
+	}
+
+	return 0;
+}
+
+static int run_disable_sequence() {
+	enable_enable(0);
+
+	enable_gyro(0);
+	enable_accel(1);
+	enable_quaternion(0);
+	enable_accel_output(0);
+	write_dmp_event(1);
+	enable_motion(enable_motion_on);
+	if (accel_engine_is_on)
+		dmp_event_control(1);
+	else
+		dmp_event_control(0);
+
+	enable_enable(1);
+	if (enable_random_delay)
+		random_delay();
+	else {
+		printf("sleep %ds\n", disable_delay);
+		sleep(disable_delay);
+	}
+
+	return 0;
+}
+static int run_dmp_off() {
+	bool g, a, out;
+
+	counter++;
+	g = rand()%2;
+	a = rand()%2;
+	if (!g && !a)
+		a = true;
+
+	g = true;
+	a = true;
+//	a = false;
+//	g = false;
+	/*disable the master enable */
+	enable_enable(0);
+	if(g) {
+		enable_gyro(1);
+		if (rand()%2) {
+			enable_gyro_output(!out);
+		} else {
+			enable_gyro_output(1);
+		}
+		enable_gyro_output(1);
+
+	} else {
+		enable_gyro(0);
+		enable_gyro_output(0);
+	}
+	if(a) {
+		enable_accel(1);
+		enable_accel_output(1);
+//		enable_accel_output(0);
+	} else {
+		enable_accel(0);
+		enable_accel_output(0);
+	}
+	if (has_compass) {
+		if(rand()%2)
+			enable_compass(1);
+		else
+			enable_compass(0);
+		enable_compass(counter%2);
+		enable_compass(1);
+	}
+	if (has_pressure) {
+		if(rand()%2)
+			enable_pressure(1);
+		else
+			enable_pressure(0);
+		enable_pressure(counter%3);
+		enable_pressure(1);
+	}
+
+	write_sysfs_int_and_verify("sampling_frequency", dev_dir_name,100);
+	first_flag = 1;
+	/*enable the master enable */
+	enable_enable(1);	
+	sleep(2);
+
+	return 0;
+}	
+static void *control_switch(void *param)
+{
+	while(1) {
+		run_enable_sequence();
+		run_dmp_off();
+		printf("sleeping\n");
+		sleep(1000);
+		run_disable_sequence();
+	}
+	return 0;
+}
+
+void get_sensor_data(char *d, short *sensor)
+{
+	int i;
+
+	for (i = 0; i < 3; i++)
+		sensor[i] = *(short *)(d + 2 + i * 2);
+}
+
+static void *read_data(void *param)
+{
+	char *buffer_access;
+	char data[1048], *dptr, tmp[24];
+	short sensor[3];
+	int q[3], i, ind, left_over_size, buf_size;
+	int ret, fp,read_size;
+	unsigned short hdr;
+	bool done_flag;
+
+#define PRESSURE_HDR             0x8000
+#define ACCEL_HDR                0x4000
+#define GYRO_HDR                 0x2000
+#define COMPASS_HDR              0x1000
+#define LPQUAT_HDR               0x0800
+#define SIXQUAT_HDR              0x0400
+#define PEDQUAT_HDR              0x0200
+#define STEP_DETECTOR_HDR        0x0100
+#define STEP_INDICATOR_HDR       0x0001
+#define END_MARKER               0x0010
+#define EMPTY_MARKER             0x0020
+
+	printf("read_data Thread: %s\n", dev_dir_name);
+	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, dev_dir_name);
+	if (ret < 0)
+		goto error_alloc_scan_el_dir;
+	ret = asprintf(&buffer_access, "/dev/iio:device%d", dev_num);
+	if (ret < 0)
+		goto error_alloc_buffer_access;
+
+	fp = open(buffer_access, O_RDONLY | O_NONBLOCK);
+	if (fp == -1) { /*If it isn't there make the node */
+		printf("Failed to open %s\n", buffer_access);
+		ret = -errno;
+		goto error_open_buffer_access;
+	}
+	ind = 0;
+	
+	while(1) {
+		struct pollfd pfd = {
+			.fd = fp,
+			.events = POLLIN,
+		};
+		poll(&pfd, 1, -1);
+
+		if (left_over_size > 0)
+			memcpy(data, tmp, left_over_size);
+		dptr = data + left_over_size;
+		read_size = read(fp,  dptr, 1024);
+		printf("readsize=%d, left_over_size=%d\n", read_size, left_over_size);
+		if (read_size <= 0) {
+			printf("Wrong size=%d\n", read_size);
+			pthread_mutex_unlock(&data_switch_lock);
+			continue;
+		}
+		ind = read_size + left_over_size;
+		dptr = data;
+		printf("ind=%d\n", ind);
+		buf_size = ind - (dptr - data);
+		done_flag = false;
+
+		while ((buf_size > 0) && (!done_flag)) {
+			hdr = *((short *)(dptr));
+			if ((hdr & 0xf) && (hdr != STEP_INDICATOR_HDR))
+				printf("STEP$$$$$$$$$$$$$$$=%x  ", hdr);
+			switch (hdr & (~0xf)) {
+			case PRESSURE_HDR:
+				if (buf_size >= 16) {
+					get_sensor_data(dptr, sensor);
+					dptr += 8;
+					printf("PRESSURE:%d, %lld\n", (sensor[1] << 16) + (unsigned short)sensor[2],  *(long long *)dptr);
+				} else
+					done_flag = true;
+				break;
+			case ACCEL_HDR:
+				if (buf_size >= 16) {
+					get_sensor_data(dptr, sensor);
+					dptr += 8;
+					printf("A:%d, %d, %d,  %lld\n", sensor[0], sensor[1], sensor[2],   *(long long *)dptr);
+				} else
+					done_flag = true;
+				break;
+			case GYRO_HDR:
+				if (buf_size >= 16) {
+					get_sensor_data(dptr, sensor);
+					dptr += 8;
+					printf("G:%d, %d, %d,  %lld\n", sensor[0], sensor[1], sensor[2],   *(long long *)dptr);
+				} else
+					done_flag = true;
+				break;
+			case COMPASS_HDR:
+				if (buf_size >= 16) {
+					get_sensor_data(dptr, sensor);
+					dptr += 8;
+					printf("M:%d, %d, %d,  %lld\n", sensor[0], sensor[1], sensor[2],   *(long long *)dptr);
+				} else
+					done_flag = true;
+				break;
+			case PEDQUAT_HDR:
+				if (buf_size >= 16) {
+					get_sensor_data(dptr, sensor);
+					dptr += 8;
+					printf("PED:%d, %d, %d,  %lld\n", sensor[0], sensor[1], sensor[2],   *(long long *)dptr);
+				}  else
+					done_flag = true;
+				break;
+			case LPQUAT_HDR:
+				if (buf_size >= 24) {
+					q[0] = *(int *)(dptr + 4);
+					dptr += 8;
+					q[1] = *(int *)(dptr);
+					q[2] = *(int *)(dptr + 4);
+					dptr += 8;
+					printf("LPQ:%d, %d, %d,  %lld\n", q[0], q[1], q[2],   *(long long *)dptr);
+				}  else
+					done_flag = true;
+				break;
+			case SIXQUAT_HDR:
+				if (buf_size >= 24) {
+					q[0] = *(int *)(dptr + 4);
+					dptr += 8;
+					q[1] = *(int *)(dptr);
+					q[2] = *(int *)(dptr + 4);
+					dptr += 8;
+					printf("SIXQ:%d, %d, %d,  %lld\n", q[0], q[1], q[2],   *(long long *)dptr);
+				}  else
+					done_flag = true;
+				break;
+			case STEP_DETECTOR_HDR:
+				if (buf_size >= 16) {
+					printf("STEP DETECTOR ");
+					dptr += 8;
+					printf(" %lld\n", *(long long *)dptr);
+				}  else
+					done_flag = true;
+
+				break;
+			default:
+				if (hdr == EMPTY_MARKER) {
+					printf("emptry marker !!!!!!!!!!!\n");
+				} else if (hdr == END_MARKER) {
+					printf("end marker !!!!!\n");
+				} else {
+					dptr +=8;
+					printf("%lld\n", *(long long *)dptr);
+				}
+				break;
+			}
+			if (!done_flag)
+				dptr += 8;
+			buf_size = ind - (dptr - data);
+		}
+		if (ind - (dptr - data) > 0)
+			memcpy(tmp, dptr, ind - (dptr - data));
+		left_over_size = ind - (dptr - data);
+	}
+	close(fp);
+
+error_open_buffer_access:
+	free(buffer_access);
+error_alloc_buffer_access:
+	free(scan_el_dir);
+error_alloc_scan_el_dir:
+
+	return 0;
+}
+
+static void inv_create_thread() {
+	pthread_t thread_dmp_event, thread_read_data, thread_control;
+
+	pthread_create(&thread_dmp_event, NULL, &get_dmp_event, (void *)dev_dir_name);
+	pthread_create(&thread_read_data, NULL, &read_data, (void *)dev_dir_name);
+	pthread_create(&thread_control, NULL, &control_switch, (void *)dev_dir_name);
+
+	pthread_join(thread_dmp_event, NULL);
+	pthread_join(thread_read_data, NULL);
+	pthread_join(thread_control, NULL);
+}
+
+static int enable_enable_main(int on){
+	int ret;
+
+	printf("enable_enable: %s=%d\n", dev_dir_name, on);
+	ret = write_sysfs_int_and_verify("enable", buf_dir_name, on);
+	if (ret < 0)
+		printf("write enable failed\n");
+
+	return 0;
+}
+
+int main(int argc, char **argv)
+{
+	unsigned long buf_len = 240;
+
+	int ret, c, i;
+
+	char *trigger_name = NULL;
+
+	int datardytrigger = 1;
+	int trig_num;
+	char *dummy;
+	char chip_name[10];
+	char device_name[10];
+	char sysfs[100];
+
+	gyro_data_is_enabled = 0;
+	accel_data_is_enabled = 0;
+	compass_data_is_enabled = 0;
+	quaternion_data_is_enabled = 0;
+
+	while ((c = getopt(argc, argv, "lcd:e:rmp")) != -1) {
+		switch (c) {
+		case 'c':
+			has_compass = 1;
+			break;
+		case 'p':
+			has_pressure = 1;
+			break;
+		case 'd':
+			disable_delay = strtoul(optarg, &dummy, 10);
+			break;
+		case 'e':
+			enable_delay = strtoul(optarg, &dummy, 10);
+			break;
+		case 'r':
+			enable_random_delay = 1;
+			break;
+		case 'm':
+			enable_motion_on = 1;
+			break;
+		case '?':
+			return -1;
+		}
+	}
+
+	inv_get_sysfs_path(sysfs);
+	printf("sss:::%s\n", sysfs);
+	if (inv_get_chip_name(chip_name) != INV_SUCCESS) {
+		printf("get chip name fail\n");
+		exit(0);
+	}
+	printf("chip_name=%s\n", chip_name);
+	if (INV_SUCCESS != inv_check_key())
+        	printf("key check fail\n");
+	else
+        	printf("key authenticated\n");
+
+	for (i=0; i<strlen(chip_name); i++) {
+		device_name[i] = tolower(chip_name[i]);
+	}
+	device_name[strlen(chip_name)] = '\0';
+	printf("device name: %s\n", device_name);
+
+	/* Find the device requested */
+	dev_num = find_type_by_name(device_name, "iio:device");
+	if (dev_num < 0) {
+		printf("Failed to find the %s\n", device_name);
+		ret = -ENODEV;
+		goto error_ret;
+	}
+	printf("iio device number being used is %d\n", dev_num);
+	asprintf(&dev_dir_name, "%siio:device%d", iio_dir, dev_num);
+	printf("allco=%x\n", (int)dev_dir_name);
+	if (trigger_name == NULL) {
+		/*
+		 * Build the trigger name. If it is device associated it's
+		 * name is <device_name>_dev[n] where n matches the device
+		 * number found above
+		 */
+		ret = asprintf(&trigger_name,
+			       "%s-dev%d", device_name, dev_num);
+		if (ret < 0) {
+			ret = -ENOMEM;
+			goto error_ret;
+		}
+	}
+	/* Verify the trigger exists */
+	trig_num = find_type_by_name(trigger_name, "trigger");
+	if (trig_num < 0) {
+		printf("Failed to find the trigger %s\n", trigger_name);
+		ret = -ENODEV;
+		goto error_free_triggername;
+	}
+	printf("iio trigger number being used is %d\n", trig_num);
+	ret = asprintf(&buf_dir_name, "%siio:device%d/buffer", iio_dir, dev_num);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_free_triggername;
+	}
+	enable_enable_main(0);
+	ret = write_sysfs_int_and_verify("power_state", dev_dir_name, 1);
+	/*
+	 * Parse the files in scan_elements to identify what channels are
+	 * present
+	 */
+	ret = 0;
+	setup_dmp(dev_dir_name);
+
+	printf("%s %s\n", dev_dir_name, trigger_name);
+
+	/* Set the device trigger to be the data rdy trigger found above */
+	ret = write_sysfs_string_and_verify("trigger/current_trigger",
+					dev_dir_name,
+					trigger_name);
+	if (ret < 0) {
+		printf("Failed to write current_trigger file\n");
+		goto error_free_buf_dir_name;
+	}
+	/* Setup ring buffer parameters */
+	/* length must be even number because iio_store_to_sw_ring is expecting 
+		half pointer to be equal to the read pointer, which is impossible
+		when buflen is odd number. This is actually a bug in the code */
+	ret = write_sysfs_int("length", buf_dir_name, buf_len*2);
+	if (ret < 0)
+		goto exit_here;
+	enable_enable_main(1);
+	inv_create_thread();
+exit_here:
+error_free_buf_dir_name:
+	free(buf_dir_name);
+error_free_triggername:
+	if (datardytrigger)
+		free(trigger_name);
+error_ret:
+	return ret;
+}