Prepare for commiting 6515 chip specifc drop.

Split /hardware/invensesnse code to chip specific 60xx and 65xx directories.
Moved 6050 chip specific stuff into 60xx directory. Manta uses that.

This will allow 6515 chip specific stuff go in 65xx directoryi which will be
used by hammerhead.

Change-Id: If7ae097cba5376bbc68c644f14fdb2689dee2da5

88 files changed, 15325 insertions, 0 deletions

diff --git a/60xx/libsensors_iio/Android.mk b/60xx/libsensors_iio/Android.mk
new file mode 100644
index 0000000..af52ea6
--- /dev/null
+++ b/60xx/libsensors_iio/Android.mk
@@ -0,0 +1,100 @@
+# 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\"
+# Comment out for ICS. Affects Android LOG macros.
+LOCAL_CFLAGS += -DANDROID_JELLYBEAN
+
+ifeq ($(ENG_BUILD),1)
+ifeq ($(COMPILE_INVENSENSE_COMPASS_CAL),1)
+LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL
+endif
+ifeq ($(COMPILE_THIRD_PARTY_ACCEL),1)
+LOCAL_CFLAGS += -DTHIRD_PARTY_ACCEL
+endif
+ifeq ($(COMPILE_COMPASS_YAS53x),1)
+LOCAL_CFLAGS += -DCOMPASS_YAS53x
+endif
+ifeq ($(COMPILE_COMPASS_AK8975),1)
+LOCAL_CFLAGS += -DCOMPASS_AK8975
+endif
+ifeq ($(COMPILE_COMPASS_AMI306),1)
+LOCAL_CFLAGS += -DCOMPASS_AMI306
+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 += CompassSensor.IIO.9150.cpp
+
+LOCAL_C_INCLUDES += $(LOCAL_PATH)
+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
+
+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)
+
+endif # !TARGET_SIMULATOR
diff --git a/60xx/libsensors_iio/CompassSensor.IIO.9150.cpp b/60xx/libsensors_iio/CompassSensor.IIO.9150.cpp
new file mode 100644
index 0000000..ce96564
--- /dev/null
+++ b/60xx/libsensors_iio/CompassSensor.IIO.9150.cpp
@@ -0,0 +1,390 @@
+/*
+ * 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"
+#include "local_log_def.h"
+
+#define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*)
+
+#if defined COMPASS_YAS53x
+#   warning "Invensense compass cal with YAS53x IIO on secondary bus"
+#   define USE_MPL_COMPASS_HAL          (1)
+#   define COMPASS_NAME                 "INV_YAS530"
+#elif defined COMPASS_AK8975
+#   warning "Invensense compass cal with AK8975 on primary bus"
+#   define USE_MPL_COMPASS_HAL          (1)
+#   define COMPASS_NAME                 "INV_AK8975"
+#elif defined INVENSENSE_COMPASS_CAL
+#   warning "Invensense compass cal with compass IIO on secondary bus"
+#   define USE_MPL_COMPASS_HAL          (1)
+#   define COMPASS_NAME                 "INV_COMPASS"
+#else
+#   warning "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(inv_init_sysfs_attributes()) {
+        LOGE("Error Instantiating Compass\n");
+        return;
+    }
+
+    if (!strcmp(COMPASS_NAME, "INV_COMPASS")) {
+        mI2CBus = COMPASS_BUS_SECONDARY;
+    } else {
+        mI2CBus = COMPASS_BUS_PRIMARY;
+    }
+
+    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];
+        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]);
+        fclose(fptr);
+
+        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];
+    } else {
+        LOGE("HAL:Couldn't read compass mounting matrix");
+    }
+
+    if (!isIntegrated()) {
+        enable(ID_M, 0);
+    }
+}
+
+CompassSensor::~CompassSensor()
+{
+    VFUNC_LOG;
+
+    free(pathP);
+    if( compass_fd > 0)
+        close(compass_fd);
+}
+
+int CompassSensor::getFd() const
+{
+    VHANDLER_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;
+
+    mEnable = en;
+    int res;
+
+    res = write_sysfs_int(compassSysFs.compass_enable, en);
+    LOGE_IF(res < 0, "HAL:enable compass failed");
+
+    if (en) {
+        res = write_sysfs_int(compassSysFs.compass_x_fifo_enable, en);
+        res = write_sysfs_int(compassSysFs.compass_y_fifo_enable, en);
+        res = write_sysfs_int(compassSysFs.compass_z_fifo_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;
+}
+
+
+/**
+    @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 = COMPASS_NAME;
+
+    if (compass) {
+        if(!strcmp(compass, "INV_COMPASS")) {
+            list->maxRange = COMPASS_MPU9150_RANGE;
+            /* since target platform would use AK8963 instead of AK8975,
+               need to adopt AK8963's resolution here */
+            // list->resolution = COMPASS_MPU9150_RESOLUTION;
+            list->resolution = COMPASS_AKM8963_RESOLUTION;
+            list->power = COMPASS_MPU9150_POWER;
+            list->minDelay = COMPASS_MPU9150_MINDELAY;
+            return;
+        }
+        if(!strcmp(compass, "compass")
+                || !strcmp(compass, "INV_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, "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], 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;
+
+    do {
+        *dptr++ = 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);
+    if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) {
+        ALOGE("CompassSensor failed get sysfs path");
+        return -1;
+    }
+
+    inv_get_iio_trigger_path(iio_trigger_path);
+
+#if defined COMPASS_AK8975
+    inv_get_input_number(COMPASS_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_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");
+#endif
+
+#if SYSFS_VERBOSE
+    // 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/60xx/libsensors_iio/CompassSensor.IIO.9150.h b/60xx/libsensors_iio/CompassSensor.IIO.9150.h
new file mode 100644
index 0000000..6a51338
--- /dev/null
+++ b/60xx/libsensors_iio/CompassSensor.IIO.9150.h
@@ -0,0 +1,93 @@
+/*
+ * 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);
+
+    // unnecessary for MPL
+    virtual int readEvents(sensors_event_t *data, int count) { return 0; }
+
+    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 (mI2CBus == COMPASS_BUS_SECONDARY); }
+
+private:
+    enum CompassBus {
+        COMPASS_BUS_PRIMARY = 0,
+        COMPASS_BUS_SECONDARY = 1
+    } mI2CBus;
+
+    struct sysfs_attrbs {
+       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;
+    } 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/60xx/libsensors_iio/InputEventReader.cpp b/60xx/libsensors_iio/InputEventReader.cpp
new file mode 100644
index 0000000..ca0a61a
--- /dev/null
+++ b/60xx/libsensors_iio/InputEventReader.cpp
@@ -0,0 +1,105 @@
+/*

+* 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"

+#include "local_log_def.h"

+

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

+

+struct input_event;

+

+InputEventCircularReader::InputEventCircularReader(size_t numEvents)

+    : mBuffer(new input_event[numEvents * 2]),

+      mBufferEnd(mBuffer + numEvents),

+      mHead(mBuffer),

+      mCurr(mBuffer),

+      mFreeSpace(numEvents)

+{

+}

+

+InputEventCircularReader::~InputEventCircularReader()

+{

+    delete [] mBuffer;

+}

+

+#define INPUT_EVENT_DEBUG (0)

+ssize_t InputEventCircularReader::fill(int fd)

+{

+    size_t numEventsRead = 0;

+    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\n", __PRETTY_FUNCTION__);

+    return numEventsRead;

+}

+

+ssize_t InputEventCircularReader::readEvent(input_event const** events)

+{

+    *events = mCurr;

+    ssize_t available = (mBufferEnd - mBuffer) - mFreeSpace;

+    return available ? 1 : 0;

+}

+

+void InputEventCircularReader::next()

+{

+    mCurr++;

+    mFreeSpace++;

+    if (mCurr >= mBufferEnd) {

+        mCurr = mBuffer;

+    }

+}

diff --git a/60xx/libsensors_iio/InputEventReader.h b/60xx/libsensors_iio/InputEventReader.h
new file mode 100644
index 0000000..5c752db
--- /dev/null
+++ b/60xx/libsensors_iio/InputEventReader.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 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;
+
+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/60xx/libsensors_iio/License.txt b/60xx/libsensors_iio/License.txt
new file mode 100644
index 0000000..930f931
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/MPLSensor.cpp b/60xx/libsensors_iio/MPLSensor.cpp
new file mode 100644
index 0000000..725a4fc
--- /dev/null
+++ b/60xx/libsensors_iio/MPLSensor.cpp
@@ -0,0 +1,2798 @@
+/*
+* 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 "MPLSupport.h"
+#include "sensor_params.h"
+#include "local_log_def.h"
+
+#include "invensense.h"
+#include "invensense_adv.h"
+#include "ml_stored_data.h"
+#include "ml_load_dmp.h"
+#include "ml_sysfs_helper.h"
+
+// #define TESTING
+
+#ifdef THIRD_PARTY_ACCEL
+#   warning "Third party accel"
+#   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 misc.                                                       */
+/******************************************************************************/
+static int 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
+
+static struct sensor_t sSensorList[] =
+{
+    {"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, 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 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 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, {}},
+
+#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
+};
+
+MPLSensor *MPLSensor::gMPLSensor = NULL;
+
+extern "C" {
+void procData_cb_wrapper()
+{
+    if(MPLSensor::gMPLSensor) {
+        MPLSensor::gMPLSensor->cbProcData();
+    }
+}
+
+void setCallbackObject(MPLSensor* gbpt)
+{
+    MPLSensor::gMPLSensor = gbpt;
+}
+
+MPLSensor* getCallbackObject() {
+    return MPLSensor::gMPLSensor;
+}
+
+} // end of extern C
+
+#ifdef INV_PLAYBACK_DBG
+static FILE *logfile = NULL;
+#endif
+
+pthread_mutex_t GlobalHalMutex = PTHREAD_MUTEX_INITIALIZER;
+
+/*******************************************************************************
+ * MPLSensor class implementation
+ ******************************************************************************/
+
+MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *))
+                       : SensorBase(NULL, NULL),
+                         mNewData(0),
+                         mMasterSensorMask(INV_ALL_SENSORS),
+                         mLocalSensorMask(0),
+                         mPollTime(-1),
+                         mHaveGoodMpuCal(0),
+                         mGyroAccuracy(0),
+                         mAccelAccuracy(0),
+                         mCompassAccuracy(0),
+                         mSampleCount(0),
+                         dmp_orient_fd(-1),
+                         mDmpOrientationEnabled(0),
+                         mEnabled(0),
+                         mOldEnabledMask(0),
+                         mAccelInputReader(4),
+                         mGyroInputReader(32),
+                         mTempScale(0),
+                         mTempOffset(0),
+                         mTempCurrentTime(0),
+                         mAccelScale(2),
+                         mPendingMask(0),
+                         mSensorMask(0),
+                         mFeatureActiveMask(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));
+
+#ifdef INV_PLAYBACK_DBG
+    LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging");
+    logfile = fopen("/data/playback.bin", "wb");
+    if (logfile)
+        inv_turn_on_data_logging(logfile);
+#endif
+
+    /* 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();
+
+    /* turn on power state */
+    onPower(1);
+
+    /* reset driver master enable */
+    masterEnable(0);
+
+    if (isLowPowerQuatEnabled() || isDmpDisplayOrientationOn()) {
+        /* Load DMP image if capable, ie. MPU6xxx/9xxx */
+        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 accel FSR to calcuate accel scale later */
+    if (!USE_THIRD_PARTY_ACCEL) {
+        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);
+        }
+    }
+
+    /* 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[LinearAccel].version = sizeof(sensors_event_t);
+    mPendingEvents[LinearAccel].sensor = ID_LA;
+    mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
+
+    mPendingEvents[Gravity].version = sizeof(sensors_event_t);
+    mPendingEvents[Gravity].sensor = ID_GR;
+    mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
+
+    mPendingEvents[Gyro].version = sizeof(sensors_event_t);
+    mPendingEvents[Gyro].sensor = ID_GY;
+    mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
+
+    mPendingEvents[RawGyro].version = sizeof(sensors_event_t);
+    mPendingEvents[RawGyro].sensor = ID_RG;
+    mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE;
+
+    mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
+    mPendingEvents[Accelerometer].sensor = ID_A;
+    mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
+
+    /* 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[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;
+
+    mHandlers[RotationVector] = &MPLSensor::rvHandler;
+    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[Orientation] = &MPLSensor::orienHandler;
+
+    for (int i = 0; i < numSensors; i++) {
+        mDelays[i] = 0;
+    }
+
+    (void)inv_get_version(&ver_str);
+    LOGV_IF(PROCESS_VERBOSE, "%s\n", ver_str);
+
+    /* setup MPL */
+    inv_constructor_init();
+
+    /* 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");
+    else
+        LOGE("HAL:Could not open or load MPL calibration file (%d)", rv);
+
+    /* Takes external Accel Calibration Load Method */
+    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_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(accel_offset, mAccelAccuracy);
+            inv_get_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 Accel Calibration Load Method */
+
+    inv_set_device_properties();
+
+    /* disable driver master enable the first sensor goes on */
+    masterEnable(0);
+    enableGyro(0);
+    enableAccel(0);
+    enableCompass(0);
+
+    if (isLowPowerQuatEnabled()) {
+        enableLPQuaternion(0);
+    }
+
+    onPower(0);
+
+    if (isDmpDisplayOrientationOn()) {
+        enableDmpOrientation(!isDmpScreenAutoRotationEnabled());
+    }
+
+}
+
+void MPLSensor::enable_iio_sysfs()
+{
+    VFUNC_LOG;
+
+    char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN];
+    FILE *tempFp = NULL;
+
+    /* ignore failures */
+    write_sysfs_int(mpu.in_timestamp_en, 1);
+
+    LOGV_IF(SYSFS_VERBOSE,
+            "HAL:sysfs:cat %s (%lld)",
+            mpu.trigger_name, getTimestamp());
+    tempFp = fopen(mpu.trigger_name, "r");
+    if (tempFp == NULL) {
+        LOGE("HAL:could not open trigger name");
+    } else {
+        if (fscanf(tempFp, "%s", iio_trigger_name) < 0) {
+            LOGE("HAL:could not read trigger name");
+        }
+        fclose(tempFp);
+    }
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %s > %s (%lld)",
+            iio_trigger_name, mpu.current_trigger, getTimestamp());
+    tempFp = fopen(mpu.current_trigger, "w");
+    if (tempFp == NULL) {
+        LOGE("HAL:could not open current trigger");
+    } else {
+        if (fprintf(tempFp, "%s", iio_trigger_name) < 0 || fclose(tempFp) < 0) {
+            LOGE("HAL:could not write current trigger %s err=%d", iio_trigger_name, errno);
+        }
+    }
+
+    write_sysfs_int(mpu.buffer_length, IIO_BUFFER_LENGTH);
+
+    if (inv_get_iio_device_node(iio_device_node) < 0) {
+        LOGE("HAL:could retrive the 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(PROCESS_VERBOSE, "HAL:iio iio_fd (%s) opened: %d", iio_device_node, iio_fd);
+    }
+}
+
+int MPLSensor::inv_constructor_init()
+{
+    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()
+{
+    VFUNC_LOG;
+
+    inv_error_t result;
+
+    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_motion_no_motion();
+    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(PROCESS_VERBOSE, "HAL:Invensense vector compass cal enabled");
+        result = inv_enable_vector_compass_cal();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        } else {
+            mFeatureActiveMask |= INV_COMPASS_CAL;
+        }
+
+        // specify MPL's trust weight, used by compass algorithms
+        inv_vector_compass_cal_sensitivity(3);
+
+        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;
+        }
+    }
+
+    result = inv_enable_9x_sensor_fusion();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    } else {
+        // 9x sensor fusion enables Compass fit
+        mFeatureActiveMask |= INV_COMPASS_FIT;
+    }
+
+    result = inv_enable_no_gyro_fusion();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    result = inv_enable_quat_accuracy_monitor();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return result;
+}
+
+/* TODO: create function pointers to calculate scale */
+void MPLSensor::inv_set_device_properties()
+{
+    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, 2000L << 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, 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);
+}
+
+void MPLSensor::loadDMP()
+{
+    int res, fd;
+    FILE *fptr;
+
+    if (isMpu3050()) {
+        //DMP support only for MPU6xxx/9xxx currently
+        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");
+        return;
+    }
+    if(inv_read_dmp_state(fd)) {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is already loaded");
+        return;
+    }
+
+    LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware);
+    fptr = fopen(mpu.dmp_firmware, "w");
+    if(!fptr) {
+        LOGE("HAL:could open %s for write. %s", mpu.dmp_firmware, strerror(errno));
+        return;
+    }
+    res = inv_load_dmp(fptr);
+    if(res < 0) {
+        LOGE("HAL:load DMP failed");
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded");
+    }
+    fclose(fptr);
+
+    // onDMP(1);                //Can't enable here. See note onDMP()
+}
+
+void MPLSensor::inv_get_sensors_orientation()
+{
+    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];
+        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]);
+        fclose(fptr);
+
+        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];
+    } else {
+        LOGE("HAL:Couldn't read gyro mounting matrix");
+    }
+
+    // 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];
+        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]);
+        fclose(fptr);
+
+        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];
+    } else {
+        LOGE("HAL:Couldn't read accel mounting matrix");
+    }
+}
+
+MPLSensor::~MPLSensor()
+{
+    VFUNC_LOG;
+
+    mCompassSensor = NULL;
+
+    /* 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);
+
+    if (isDmpDisplayOrientationOn()) {
+        closeDmpOrientFd();
+    }
+
+    /* Turn off Gyro master enable          */
+    /* A workaround until driver handles it */
+    /* TODO: Turn off and close all sensors */
+    if(write_sysfs_int(mpu.chip_enable, 0) < 0) {
+        LOGE("HAL:could not disable gyro master enable");
+    }
+
+#ifdef INV_PLAYBACK_DBG
+    inv_turn_off_data_logging();
+    fclose(logfile);
+#endif
+}
+
+#define GY_ENABLED (((1 << ID_GY) | (1 << ID_RG)) & enabled_sensors)
+#define A_ENABLED  ((1 << ID_A)  & enabled_sensors)
+#define M_ENABLED  ((1 << ID_M)  & 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)
+
+/* TODO: this step is optional, remove?  */
+int MPLSensor::setGyroInitialState()
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            HW_GYRO_RATE_HZ, mpu.gyro_fifo_rate, getTimestamp());
+    int fd = open(mpu.gyro_fifo_rate, O_RDWR);
+    res = errno;
+    if(fd < 0) {
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             mpu.gyro_fifo_rate, strerror(res), res);
+        return res;
+    }
+    res = write_attribute_sensor(fd, HW_GYRO_RATE_HZ);
+    if(res < 0) {
+        LOGE("HAL:write_attribute_sensor : error writing %s with %d",
+             mpu.gyro_fifo_rate, HW_GYRO_RATE_HZ);
+        return res;
+    }
+
+    // Setting LPF is deprecated
+
+    return 0;
+}
+
+/* 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::onPower(int en)
+{
+    VFUNC_LOG;
+
+    int res;
+
+    int count, curr_power_state;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.power_state, getTimestamp());
+    res = read_sysfs_int(mpu.power_state, &curr_power_state);
+    if (res < 0) {
+        LOGE("HAL:Error reading power state");
+        // will set power_state anyway
+        curr_power_state = -1;
+    }
+    if (en != curr_power_state) {
+        if((res = write_sysfs_int(mpu.power_state, en)) < 0) {
+                LOGE("HAL:Couldn't write power state");
+        }
+    } else {
+        LOGV_IF(EXTRA_VERBOSE,
+                "HAL:Power state already enable/disable curr=%d new=%d",
+                curr_power_state, en);
+    }
+    return res;
+}
+
+int MPLSensor::onDMP(int en)
+{
+    VFUNC_LOG;
+
+    int res = -1;
+    int status;
+
+    //Sequence to enable DMP
+    //1. Turn On power if not already on
+    //2. Load DMP image if not already loaded
+    //3. Either Gyro or Accel must be enabled/configured before next step
+    //4. Enable DMP
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
+            mpu.firmware_loaded, getTimestamp());
+    res = read_sysfs_int(mpu.firmware_loaded, &status);
+    if (res < 0){
+        LOGE("HAL:ERR can't get firmware_loaded status");
+        return res;
+    }
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs: %s status=%d", mpu.firmware_loaded, status);
+
+    if (status) {
+        //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) {
+            res = write_sysfs_int(mpu.dmp_on, en);
+            //Enable DMP interrupt
+            if (write_sysfs_int(mpu.dmp_int_on, en) < 0) {
+                LOGE("HAL:ERR can't en/dis DMP interrupt");
+            }
+        } else {
+            res = 0;    //DMP already set as requested
+        }
+    } else {
+        LOGE("HAL:ERR No DMP image");
+    }
+    return res;
+}
+
+int MPLSensor::checkLPQuaternion(void)
+{
+    VFUNC_LOG;
+
+    return ((mFeatureActiveMask & INV_DMP_QUATERNION)? 1:0);
+}
+
+int MPLSensor::enableLPQuaternion(int en)
+{
+    VFUNC_LOG;
+
+    if (!en) {
+        enableQuaternionData(0);
+        onDMP(0);
+        mFeatureActiveMask &= ~INV_DMP_QUATERNION;
+        LOGV_IF(PROCESS_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(PROCESS_VERBOSE, "HAL:LP Quat enabled");
+        }
+    }
+    return 0;
+}
+
+int MPLSensor::enableQuaternionData(int en)
+{
+    int res = 0;
+    VFUNC_LOG;
+
+    // Enable DMP quaternion
+    res = write_sysfs_int(mpu.quaternion_on, en);
+
+    if (!en) {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling quat scan elems");
+    } else {
+
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling quat scan elems");
+    }
+    write_sysfs_int(mpu.in_quat_r_en, en);
+    write_sysfs_int(mpu.in_quat_x_en, en);
+    write_sysfs_int(mpu.in_quat_y_en, en);
+    write_sysfs_int(mpu.in_quat_z_en, en);
+
+    LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off");
+
+    if (!en) {
+        inv_quaternion_sensor_was_turned_off();
+    }
+
+    return res;
+}
+
+int MPLSensor::enableTap(int en)
+{
+    VFUNC_LOG;
+
+    return 0;
+}
+
+int MPLSensor::enableFlick(int en)
+{
+    VFUNC_LOG;
+
+    return 0;
+}
+
+int MPLSensor::enablePedometer(int en)
+{
+    VFUNC_LOG;
+
+    return 0;
+}
+
+int MPLSensor::masterEnable(int en)
+{
+    VFUNC_LOG;
+    return write_sysfs_int(mpu.chip_enable, en);
+}
+
+int MPLSensor::enableGyro(int en)
+{
+    VFUNC_LOG;
+
+    /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
+    int res;
+
+    /* need to also turn on/off the master enable */
+    res = write_sysfs_int(mpu.gyro_enable, en);
+
+    if (!en) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off");
+        inv_gyro_was_turned_off();
+    } else {
+        write_sysfs_int(mpu.gyro_x_fifo_enable, en);
+        write_sysfs_int(mpu.gyro_y_fifo_enable, en);
+        res = write_sysfs_int(mpu.gyro_z_fifo_enable, en);
+    }
+
+    return res;
+}
+
+int MPLSensor::enableAccel(int en)
+{
+    VFUNC_LOG;
+
+    /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
+    int res;
+
+    /* need to also turn on/off the master enable */
+    res = write_sysfs_int(mpu.accel_enable, en);
+
+    if (!en) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off");
+        inv_accel_was_turned_off();
+    } else {
+        write_sysfs_int(mpu.accel_x_fifo_enable, en);
+        write_sysfs_int(mpu.accel_y_fifo_enable, en);
+        res = write_sysfs_int(mpu.accel_z_fifo_enable, en);
+    }
+
+    return res;
+}
+
+int MPLSensor::enableCompass(int en)
+{
+    VFUNC_LOG;
+
+    int res = mCompassSensor->enable(ID_M, en);
+    if (!en) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off");
+        inv_compass_was_turned_off();
+    }
+    return res;
+}
+
+void MPLSensor::computeLocalSensorMask(int enabled_sensors)
+{
+    VFUNC_LOG;
+
+    do {
+        if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "FUSION ENABLED");
+            mLocalSensorMask = ALL_MPL_SENSORS_NP;
+            break;
+        }
+
+        if(!A_ENABLED && !M_ENABLED && !GY_ENABLED) {
+            /* Invensense compass cal */
+            LOGV_IF(ENG_VERBOSE, "ALL DISABLED");
+            mLocalSensorMask = 0;
+            break;
+        }
+
+        if (GY_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) {
+            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::enableOneSensor(int en, const char *name, int (MPLSensor::*enabler)(int)) {
+    LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - %s %s", en ? "enabled" : "disable", name);
+    return (this->*enabler)(en);
+}
+
+int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) {
+    VFUNC_LOG;
+
+    inv_error_t res = -1;
+    bool store_cal = false;
+    bool ext_compass_changed = false;
+
+    // Sequence to enable or disable a sensor
+    // 1. enable Power state
+    // 2. reset master enable (=0)
+    // 3. enable or disable a sensor
+    // 4. set master enable (=1)
+
+    pthread_mutex_lock(&GlobalHalMutex);
+
+    uint32_t all_changeables = (1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer)
+            | (1 << MagneticField);
+    uint32_t all_integrated_changeables = all_changeables;
+
+    if (!mCompassSensor->isIntegrated()) {
+        ext_compass_changed = changed & (1 << MagneticField);
+        all_integrated_changeables = all_changeables & ~(1 << MagneticField);
+    }
+
+    if (isLowPowerQuatEnabled() || (changed & all_integrated_changeables)) {
+        /* ensure power state is on */
+        onPower(1);
+
+        /* reset master enable */
+        res = masterEnable(0);
+        if(res < 0) {
+            goto unlock_res;
+        }
+    }
+
+    LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", (unsigned int)sensors);
+
+    if (changed & ((1 << Gyro) | (1 << RawGyro))) {
+        res = enableOneSensor(sensors & INV_THREE_AXIS_GYRO, "gyro", &MPLSensor::enableGyro);
+        if(res < 0) {
+            goto unlock_res;
+        }
+    }
+
+    if (changed & (1 << Accelerometer)) {
+        res = enableOneSensor(sensors & INV_THREE_AXIS_ACCEL, "accel", &MPLSensor::enableAccel);
+        if(res < 0) {
+            goto unlock_res;
+        }
+    }
+
+    if (changed & (1 << MagneticField)) {
+        /* Invensense compass calibration */
+        res = enableOneSensor(sensors & INV_THREE_AXIS_COMPASS, "compass", &MPLSensor::enableCompass);
+        if(res < 0) {
+            goto unlock_res;
+        }
+    }
+
+    if ( isLowPowerQuatEnabled() ) {
+        // Enable LP Quat
+        if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) |
+                (1 << LinearAccel) | (1 << Gravity)))) {
+            if (!(changed & all_integrated_changeables)) {
+                /* ensure power state is on */
+                onPower(1);
+                /* reset master enable */
+                res = masterEnable(0);
+                if(res < 0) {
+                    goto unlock_res;
+                }
+            }
+            if (!checkLPQuaternion()) {
+                enableLPQuaternion(1);
+            } else {
+                LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled");
+            }
+        } else if (checkLPQuaternion()) {
+            enableLPQuaternion(0);
+        }
+    }
+
+    if (changed & all_integrated_changeables) {
+        if (sensors &
+            (INV_THREE_AXIS_GYRO
+                | INV_THREE_AXIS_ACCEL
+                | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
+            if ( isLowPowerQuatEnabled() ||
+                    (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) ) {
+                // disable DMP event interrupt only (w/ data interrupt)
+                if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
+                    res = -1;
+                    LOGE("HAL:ERR can't disable DMP event interrupt");
+                    goto unlock_res;
+                }
+            }
+
+            if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
+                // enable DMP
+                onDMP(1);
+
+                res = enableAccel(1);
+                if(res < 0) {
+                    goto unlock_res;
+                }
+
+                if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
+                    res = turnOffAccelFifo();
+                }
+                if(res < 0) {
+                    goto unlock_res;
+                }
+            }
+
+            res = masterEnable(1);
+            if(res < 0) {
+                goto unlock_res;
+            }
+        } else { // all sensors idle -> reduce power
+            if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
+                // enable DMP
+                onDMP(1);
+                // enable DMP event interrupt only (no data interrupt)
+                if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) {
+                    res = -1;
+                    LOGE("HAL:ERR can't enable DMP event interrupt");
+                }
+                res = enableAccel(1);
+                if(res < 0) {
+                    goto unlock_res;
+                }
+                if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
+                    res = turnOffAccelFifo();
+                }
+                if(res < 0) {
+                    goto unlock_res;
+                }
+                res = masterEnable(1);
+                if(res < 0) {
+                    goto unlock_res;
+                }
+            }
+            else {
+                res = onPower(0);
+                if(res < 0) {
+                    goto unlock_res;
+                }
+            }
+            store_cal = true;
+        }
+    } else if (ext_compass_changed &&
+            !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL
+                | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated()))))) {
+            store_cal = true;
+    }
+
+    if (store_cal || ((changed & all_changeables) != all_changeables)) {
+        storeCalibration();
+    }
+
+unlock_res:
+    pthread_mutex_unlock(&GlobalHalMutex);
+    return res;
+}
+
+/* Store calibration file */
+void MPLSensor::storeCalibration()
+{
+    if(mHaveGoodMpuCal || 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");
+       }
+    }
+}
+
+void MPLSensor::cbProcData()
+{
+    mNewData = 1;
+    mSampleCount++;
+    LOGV_IF(EXTRA_VERBOSE, "HAL:new data");
+}
+
+/*  these handlers transform mpl data into one of the Android sensor types */
+int MPLSensor::gyroHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int8_t status;
+    int update;
+    update = inv_get_sensor_type_gyroscope(s->gyro.v, &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;
+    int8_t status;
+    int update;
+    update = inv_get_sensor_type_gyroscope_raw(s->gyro.v, &status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:raw 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::accelHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int8_t status;
+    int update;
+    update = inv_get_sensor_type_accelerometer(
+        s->acceleration.v, &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 = 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::rvHandler(sensors_event_t* s)
+{
+    // rotation vector does not have an accuracy or status
+    VHANDLER_LOG;
+    int8_t status;
+    int update;
+    update = inv_get_sensor_type_rotation_vector(s->data, &status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f - %+lld - %d",
+            s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::laHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int8_t status;
+    int update;
+    update = inv_get_sensor_type_linear_acceleration(
+            s->gyro.v, &status, &s->timestamp);
+    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;
+    int8_t status;
+    int update;
+    update = inv_get_sensor_type_gravity(s->gyro.v, &status, &s->timestamp);
+    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);
+    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::enable(int32_t handle, int en)
+{
+    VFUNC_LOG;
+
+    android::String8 sname;
+    int what = -1, err = 0;
+
+    switch (handle) {
+    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());
+        /* TODO: stop manually testing/using 0 and 1 instead of
+         * false and true, but just use 0 and non-0.
+         * This allows  passing 0 and non-0 ints around instead of
+         * having to convert to 1 and test against 1.
+         */
+        mDmpOrientationEnabled = en;
+        return 0;
+    case ID_A:
+        what = Accelerometer;
+        sname = "Accelerometer";
+        break;
+    case ID_M:
+        what = MagneticField;
+        sname = "MagneticField";
+        break;
+    case ID_O:
+        what = Orientation;
+        sname = "Orientation";
+        break;
+    case ID_GY:
+        what = Gyro;
+        sname = "Gyro";
+        break;
+    case ID_RG:
+        what = RawGyro;
+        sname = "RawGyro";
+        break;
+    case ID_GR:
+        what = Gravity;
+        sname = "Gravity";
+        break;
+    case ID_RV:
+        what = RotationVector;
+        sname = "RotationVector";
+        break;
+    case ID_LA:
+        what = LinearAccel;
+        sname = "LinearAccel";
+        break;
+    default: //this takes care of all the gestures
+        what = handle;
+        sname = "Others";
+        break;
+    }
+
+    if (uint32_t(what) >= numSensors)
+        return -EINVAL;
+
+    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(PROCESS_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(PROCESS_VERBOSE, "HAL:handle = %d", handle);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:flags = %d", flags);
+        computeLocalSensorMask(mEnabled);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled);
+        sen_mask = mLocalSensorMask & mMasterSensorMask;
+        mSensorMask = sen_mask;
+        LOGV_IF(PROCESS_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask);
+
+        switch (what) {
+            case Gyro:
+            case RawGyro:
+            case Accelerometer:
+            case MagneticField:
+                if (!(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
+                        (1 << LinearAccel) | (1 << Gravity))) &&
+                        ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) {
+                    changed |= (1 << what);
+                }
+                break;
+
+            case Orientation:
+            case RotationVector:
+            case LinearAccel:
+            case Gravity:
+                if ((en && !(lastEnabled & ((1 << Orientation) | (1 << RotationVector) |
+                        (1 << LinearAccel) | (1 << Gravity)))) ||
+                        (!en && !(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
+                        (1 << LinearAccel) | (1 << Gravity))))) {
+                    for (int i = Gyro; i <= MagneticField; i++) {
+                        if (!(mEnabled & (1 << i))) {
+                            changed |= (1 << i);
+                        }
+                    }
+                }
+                break;
+        }
+        LOGV_IF(PROCESS_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 be go through this sequence
+       to properly flush the log file */
+    inv_turn_off_data_logging();
+    fclose(logfile);
+    logfile = fopen("/data/playback.bin", "ab");
+    if (logfile)
+        inv_turn_on_data_logging(logfile);
+#endif
+
+    return err;
+}
+
+int MPLSensor::setDelay(int32_t handle, int64_t ns)
+{
+    VFUNC_LOG;
+
+    android::String8 sname;
+    int what = -1;
+
+    switch (handle) {
+        case ID_SO:
+            return update_delay();
+        case ID_A:
+            what = Accelerometer;
+            sname = "Accelerometer";
+            break;
+        case ID_M:
+            what = MagneticField;
+            sname = "MagneticField";
+            break;
+        case ID_O:
+            what = Orientation;
+            sname = "Orientation";
+            break;
+        case ID_GY:
+            what = Gyro;
+            sname = "Gyro";
+            break;
+        case ID_RG:
+            what = RawGyro;
+            sname = "RawGyro";
+            break;
+        case ID_GR:
+            what = Gravity;
+            sname = "Gravity";
+            break;
+        case ID_RV:
+            what = RotationVector;
+            sname = "RotationVector";
+            break;
+        case ID_LA:
+            what = LinearAccel;
+            sname = "LinearAccel";
+            break;
+        default: // this takes care of all the gestures
+            what = handle;
+            sname = "Others";
+            break;
+    }
+
+#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
+
+    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 < 10000000LL) {
+        ns = 10000000LL;
+    }
+*/
+    if (ns < 5000000LL) {
+        ns = 5000000LL;
+    }
+
+    /* store request rate to mDelays arrary for each sensor */
+    mDelays[what] = ns;
+
+    switch (what) {
+        case Gyro:
+        case RawGyro:
+        case Accelerometer:
+            for (int i = Gyro; i <= Accelerometer + mCompassSensor->isIntegrated();
+                    i++) {
+                if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
+                    LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i);
+                    return 0;
+                }
+            }
+            break;
+
+        case MagneticField:
+            if (mCompassSensor->isIntegrated() &&
+                    (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) ||
+                    ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) ||
+                    ((mEnabled & (1 << Accelerometer)) && ns > mDelays[Accelerometer]))) {
+                 LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to gyro/accel");
+                 return 0;
+            }
+            break;
+
+        case Orientation:
+        case RotationVector:
+        case LinearAccel:
+        case Gravity:
+            if (isLowPowerQuatEnabled()) {
+                LOGV_IF(PROCESS_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(PROCESS_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() {
+    VHANDLER_LOG;
+
+    int res = 0;
+    int64_t got;
+
+    pthread_mutex_lock(&GlobalHalMutex);
+    if (mEnabled) {
+        int64_t wanted = 1000000000;
+        int64_t wanted_3rd_party_sensor = 1000000000;
+
+        // Sequence to change sensor's FIFO rate
+        // 1. enable Power state
+        // 2. reset master enable
+        // 3. Update delay
+        // 4. set master enable
+
+        // ensure power on
+        onPower(1);
+
+        // reset master enable
+        masterEnable(0);
+
+        /* 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;
+            }
+        }
+
+        // same delay for 3rd party Accel or Compass
+        wanted_3rd_party_sensor = wanted;
+
+        /* mpl rate in us in future maybe different for
+           gyro vs compass vs accel */
+        int rateInus = (int)wanted / 1000LL;
+        int mplGyroRate = rateInus;
+        int mplAccelRate = rateInus;
+        int mplCompassRate = rateInus;
+
+        LOGV_IF(PROCESS_VERBOSE, "HAL:wanted rate for all sensors : "
+             "%llu ns, mpl rate: %d us, (%.2f Hz)",
+             wanted, rateInus, 1000000000.f / wanted);
+
+        /* 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);
+
+        /* TODO: Test 200Hz */
+        // inv_set_gyro_sample_rate(5000);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:MPL gyro sample rate: %d", mplGyroRate);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:MPL accel sample rate: %d", mplAccelRate);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:MPL compass sample rate: %d", mplCompassRate);
+
+        int enabled_sensors = mEnabled;
+        int tempFd = -1;
+        if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
+            if (isLowPowerQuatEnabled() ||
+                    (isDmpDisplayOrientationOn() && mDmpOrientationEnabled)) {
+                bool setDMPrate= 0;
+                // Set LP Quaternion sample rate if enabled
+                if (checkLPQuaternion()) {
+                    if (wanted < RATE_200HZ) {
+                        enableLPQuaternion(0);
+                    } else {
+                        inv_set_quat_sample_rate(rateInus);
+                        setDMPrate= 1;
+                    }
+                }
+
+                if (checkDMPOrientation() || setDMPrate==1) {
+                    getDmpRate(&wanted);
+                }
+            }
+
+            int64_t tempRate = wanted;
+            LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion");
+            //nsToHz
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+                    1000000000.f / tempRate, mpu.gyro_fifo_rate,
+                    getTimestamp());
+            tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
+            res = write_attribute_sensor(tempFd, 1000000000.f / tempRate);
+            if(res < 0) {
+                LOGE("HAL:GYRO update delay error");
+            }
+
+            //nsToHz (BMA250)
+            if(USE_THIRD_PARTY_ACCEL) {
+                LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)",
+                        wanted_3rd_party_sensor / 1000000L, mpu.accel_fifo_rate,
+                        getTimestamp());
+                tempFd = open(mpu.accel_fifo_rate, O_RDWR);
+                res = write_attribute_sensor(tempFd, wanted_3rd_party_sensor / 1000000L);
+                LOGE_IF(res < 0, "HAL:ACCEL update delay error");
+            }
+
+            if (!mCompassSensor->isIntegrated()) {
+                LOGV_IF(PROCESS_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 {
+
+            if (GY_ENABLED) {
+                wanted = (mDelays[Gyro] <= mDelays[RawGyro]?
+                    (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]):
+                    (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro]));
+
+                if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
+                    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:GYRO update delay error");
+            }
+
+            if (A_ENABLED) { /* else if because there is only 1 fifo rate for MPUxxxx */
+                if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) {
+                    wanted = mDelays[Gyro];
+                }
+                else if (GY_ENABLED && mDelays[RawGyro] < mDelays[Accelerometer]) {
+                    wanted = mDelays[RawGyro];
+
+                } else {
+                    wanted = mDelays[Accelerometer];
+                }
+
+                if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
+                    getDmpRate(&wanted);
+                }
+
+                /* TODO: use function pointers to calculate delay value specific to vendor */
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+                        1000000000.f / wanted, mpu.accel_fifo_rate, getTimestamp());
+                tempFd = open(mpu.accel_fifo_rate, O_RDWR);
+                if(USE_THIRD_PARTY_ACCEL) {
+                    //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) {
+                if (!mCompassSensor->isIntegrated()) {
+                    wanted = mDelays[MagneticField];
+                } else {
+                    if (GY_ENABLED && mDelays[Gyro] < mDelays[MagneticField]) {
+                        wanted = mDelays[Gyro];
+                    }
+                    else if (GY_ENABLED && mDelays[RawGyro] < mDelays[MagneticField]) {
+                        wanted = mDelays[RawGyro];
+                    } else if (A_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) {
+                        wanted = mDelays[Accelerometer];
+                    } else {
+                        wanted = mDelays[MagneticField];
+                    }
+
+                    if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
+                        getDmpRate(&wanted);
+                    }
+                }
+
+                mCompassSensor->setDelay(ID_M, wanted);
+                got = mCompassSensor->getDelay(ID_M);
+                inv_set_compass_sample_rate(got / 1000);
+            }
+
+        }
+
+        unsigned long sensors = mLocalSensorMask & mMasterSensorMask;
+        if (sensors &
+            (INV_THREE_AXIS_GYRO
+                | INV_THREE_AXIS_ACCEL
+                | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
+            res = masterEnable(1);
+        } else { // all sensors idle -> reduce power
+            res = onPower(0);
+        }
+    }
+    pthread_mutex_unlock(&GlobalHalMutex);
+    return res;
+}
+
+/* For Third Party Accel Input Subsystem Drivers only */
+/* TODO: FIX! data is not used and count not decremented, results is hardcoded to 0 */
+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 && 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.
+ */
+/* TODO: This should probably be called "int readEvents(...)"
+ *  and readEvents() called "void cacheData(void)".
+ */
+int MPLSensor::executeOnData(sensors_event_t* data, int count)
+{
+    VFUNC_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;
+        }
+    }
+
+    // load up virtual sensors
+    for (int i = 0; i < numSensors; i++) {
+        int update;
+        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
+/* TODO: FIX! data and count are not used, results is hardcoded to 0 */
+/* TODO: This should probably be called "void cacheEvents(void)"
+ * And executeOnData() should be int readEvents(data,count)
+ */
+int MPLSensor::readEvents(sensors_event_t *data, int count) {
+
+
+    int lp_quaternion_on = 0, nbyte;
+    int i, nb, mask = 0, numEventReceived = 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);
+    char *rdata = mIIOBuffer;
+
+    nbyte= (8 * sensors + 8) * 1;
+
+    if (isLowPowerQuatEnabled()) {
+        lp_quaternion_on = checkLPQuaternion();
+        if (lp_quaternion_on) {
+            nbyte += sizeof(mCachedQuaternionData);             //currently 16 bytes for Q data
+        }
+    }
+
+    // pthread_mutex_lock(&mMplMutex);
+    // pthread_mutex_lock(&mHALMutex);
+
+    ssize_t rsize = read(iio_fd, rdata, nbyte);
+    if (sensors == 0) {
+        // read(iio_fd, rdata, nbyte);
+        rsize = read(iio_fd, rdata, sizeof(mIIOBuffer));
+    }
+
+#ifdef TESTING
+    LOGI("get one sample of IIO data with size: %d", rsize);
+    LOGI("sensors: %d", sensors);
+
+    LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_GYRO, "gyro x/y/z: %d/%d/%d",
+        *((short *) (rdata + 0)), *((short *) (rdata + 2)),
+        *((short *) (rdata + 4)));
+    LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_ACCEL, "accel x/y/z: %d/%d/%d",
+        *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))),
+        *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))),
+        *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0)));
+
+    LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_COMPASS &
+        mCompassSensor->isIntegrated(), "compass x/y/z: %d/%d/%d",
+        *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
+            ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
+        *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
+            ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
+        *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
+            ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0)));
+#endif
+
+    if (rsize < (nbyte - 8)) {
+        LOGE("HAL:ERR Full data packet was not read. rsize=%ld nbyte=%d sensors=%d errno=%d(%s)",
+             rsize, nbyte, sensors, errno, strerror(errno));
+        return -1;
+    }
+
+    if (isLowPowerQuatEnabled() && lp_quaternion_on) {
+
+        for (i=0; i< 4; i++) {
+            mCachedQuaternionData[i]= *(long*)rdata;
+            rdata += sizeof(long);
+        }
+    }
+
+    for (i = 0; i < 3; i++) {
+        if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
+            mCachedGyroData[i] = *((short *) (rdata + i * 2));
+        }
+        if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
+            mCachedAccelData[i] = *((short *) (rdata + i * 2 +
+                ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0)));
+        }
+        if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated()) {
+            mCachedCompassData[i] = *((short *) (rdata + i * 2 + 6 * (sensors - 1)));
+        }
+    }
+
+    mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 << Gyro: 0) +
+        ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 << Accelerometer: 0));
+    if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated() &&
+            (mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0 || mCachedCompassData[0] != 0)) {
+        mask |= 1 << MagneticField;
+    }
+
+    mSensorTimestamp = *((long long *) (rdata + 8 * sensors));
+    if (mCompassSensor->isIntegrated()) {
+        mCompassTimestamp = mSensorTimestamp;
+    }
+
+    if (mask & (1 << Gyro)) {
+        // send down temperature every 0.5 seconds
+        // with timestamp measured in "driver" layer
+        if(mSensorTimestamp - mTempCurrentTime >= 500000000LL) {
+            mTempCurrentTime = mSensorTimestamp;
+            long long temperature[2];
+            if(inv_read_temperature(temperature) == 0) {
+                LOGV_IF(INPUT_DATA,
+                        "HAL: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_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;
+
+        if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
+            inv_build_gyro(mCachedGyroData, mSensorTimestamp);
+            LOGV_IF(INPUT_DATA,
+                    "HAL:inv_build_gyro: %+8d %+8d %+8d - %lld",
+                    mCachedGyroData[0], mCachedGyroData[1],
+                    mCachedGyroData[2], mSensorTimestamp);
+        }
+    }
+
+    if (mask & (1 << Accelerometer)) {
+        mPendingMask |= 1 << Accelerometer;
+        if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
+            inv_build_accel(mCachedAccelData, 0, mSensorTimestamp);
+             LOGV_IF(INPUT_DATA,
+                    "HAL:inv_build_accel: %+8ld %+8ld %+8ld - %lld",
+                    mCachedAccelData[0], mCachedAccelData[1],
+                    mCachedAccelData[2], mSensorTimestamp);
+        }
+    }
+
+    if ((mask & (1 << MagneticField)) && mCompassSensor->isIntegrated()) {
+        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:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
+                    mCachedCompassData[0], mCachedCompassData[1],
+                    mCachedCompassData[2], mCompassTimestamp);
+        }
+    }
+
+    if (isLowPowerQuatEnabled() && lp_quaternion_on) {
+
+        inv_build_quat(mCachedQuaternionData, 32 /*default 32 for now (16/32bits)*/, mSensorTimestamp);
+        LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld",
+                    mCachedQuaternionData[0], mCachedQuaternionData[1],
+                    mCachedQuaternionData[2], mCachedQuaternionData[3], mSensorTimestamp);
+    }
+
+    // pthread_mutex_unlock(&mMplMutex);
+    // pthread_mutex_unlock(&mHALMutex);
+
+    return numEventReceived;
+}
+
+/* use for both MPUxxxx and third party compass */
+int MPLSensor::readCompassEvents(sensors_event_t *data, int count)
+{
+    VHANDLER_LOG;
+
+    if (count < 1)
+        return -EINVAL;
+
+    int numEventReceived = 0;
+    int done = 0;
+    int nb;
+
+    // pthread_mutex_lock(&mMplMutex);
+    // pthread_mutex_lock(&mHALMutex);
+
+    done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp);
+#ifdef COMPASS_YAS53x
+    if (mCompassSensor->checkCoilsReset()) {
+       //Reset relevant compass settings
+       resetCompass();
+    }
+#endif
+    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:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
+                    mCachedCompassData[0], mCachedCompassData[1],
+                    mCachedCompassData[2], mCompassTimestamp);
+        }
+    }
+
+    // pthread_mutex_unlock(&mMplMutex);
+    // pthread_mutex_unlock(&mHALMutex);
+
+    return numEventReceived;
+}
+
+#ifdef COMPASS_YAS53x
+int MPLSensor::resetCompass()
+{
+    VFUNC_LOG;
+
+    //Reset compass cal if enabled
+    if (mFeatureActiveMask & INV_COMPASS_CAL) {
+       LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal");
+       inv_init_vector_compass_cal();
+    }
+
+    //Reset compass fit if enabled
+    if (mFeatureActiveMask & INV_COMPASS_FIT) {
+       LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit");
+       inv_init_compass_fit();
+    }
+
+    return 0;
+}
+#endif
+
+int MPLSensor::getFd() const
+{
+    VFUNC_LOG;
+    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getFd returning %d", iio_fd);
+    return iio_fd;
+}
+
+int MPLSensor::getAccelFd() const
+{
+    VFUNC_LOG;
+    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getAccelFd returning %d", accel_fd);
+    return accel_fd;
+}
+
+int MPLSensor::getCompassFd() const
+{
+    VFUNC_LOG;
+    int fd = mCompassSensor->getFd();
+    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getCompassFd returning %d", fd);
+    return fd;
+}
+
+int MPLSensor::turnOffAccelFifo() {
+    int i, res, tempFd;
+    char *accel_fifo_enable[3] = {mpu.accel_x_fifo_enable,
+        mpu.accel_y_fifo_enable, mpu.accel_z_fifo_enable};
+
+    for (i = 0; i < 3; i++) {
+        res = write_sysfs_int(accel_fifo_enable[i], 0);
+        if (res < 0) {
+            return res;
+        }
+    }
+    return 0;
+}
+
+int MPLSensor::enableDmpOrientation(int en)
+{
+    VFUNC_LOG;
+    /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
+    int res = 0;
+    int enabled_sensors = mEnabled;
+
+    if (isMpu3050())
+        return res;
+
+    pthread_mutex_lock(&GlobalHalMutex);
+
+    // on power if not already On
+    res = onPower(1);
+    // reset master enable
+    res = masterEnable(0);
+
+    if (en) {
+        //Enable DMP orientation
+        if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
+            LOGE("HAL:ERR can't enable Android orientation");
+            res = -1;	// indicate an err
+        }
+
+        // open DMP Orient Fd
+        res = openDmpOrientFd();
+
+        // enable DMP
+        res = onDMP(1);
+
+        // default DMP output rate to FIFO
+        if (write_sysfs_int(mpu.dmp_output_rate, 5) < 0) {
+            LOGE("HAL:ERR can't default DMP output rate");
+        }
+
+        // set DMP rate to 200Hz
+        if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) {
+            res = -1;
+            LOGE("HAL:ERR can't set DMP rate to 200Hz");
+        }
+
+        // enable accel engine
+        res = enableAccel(1);
+
+        // disable accel FIFO
+        if (!A_ENABLED) {
+            res = turnOffAccelFifo();
+        }
+
+        mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION;
+    } else {
+        // disable DMP
+        res = onDMP(0);
+
+        // disable accel engine
+        if (!A_ENABLED) {
+            res = enableAccel(0);
+        }
+    }
+
+    res = masterEnable(1);
+    pthread_mutex_unlock(&GlobalHalMutex);
+    return res;
+}
+
+int MPLSensor::openDmpOrientFd()
+{
+    VFUNC_LOG;
+
+    if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:DMP display orientation disabled or file desc opened");
+        return -1;
+    }
+
+    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()
+{
+    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;
+    }
+    fscanf(fp, "%d\n", &screen_orientation);
+    fclose(fp);
+
+    int numEventReceived = 0;
+
+    if (mDmpOrientationEnabled && count > 0) {
+        sensors_event_t temp;
+
+        bzero(&temp, sizeof(temp));  /* Let's hope that SENSOR_TYPE_NONE is 0 */
+        temp.version = sizeof(sensors_event_t);
+        temp.sensor = ID_SO;
+#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()
+{
+    VFUNC_LOG;
+
+    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getDmpOrientFd returning %d", dmp_orient_fd);
+    return dmp_orient_fd;
+
+}
+
+int MPLSensor::checkDMPOrientation()
+{
+    VFUNC_LOG;
+    return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0);
+}
+
+int MPLSensor::getDmpRate(int64_t *wanted)
+{
+    if (checkDMPOrientation() || checkLPQuaternion()) {
+        // set DMP output rate to FIFO
+        write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / *wanted);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:DMP FIFO 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()
+{
+    VHANDLER_LOG;
+    return mPollTime;
+}
+
+bool MPLSensor::hasPendingEvents() const
+{
+    VHANDLER_LOG;
+    // if we are using the polling workaround, force the main
+    // loop to check for data every time
+    return (mPollTime != -1);
+}
+
+/* TODO: support resume suspend when we gain more info about them*/
+void MPLSensor::sleepEvent()
+{
+    VFUNC_LOG;
+}
+
+void MPLSensor::wakeEvent()
+{
+    VFUNC_LOG;
+}
+
+int MPLSensor::inv_float_to_q16(float *fdata, long *ldata)
+{
+    VHANDLER_LOG;
+
+    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 MPLSensor::inv_long_to_q16(long *fdata, long *ldata)
+{
+    VHANDLER_LOG;
+
+    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 MPLSensor::inv_float_to_round(float *fdata, long *ldata)
+{
+    VHANDLER_LOG;
+
+    if (!fdata || !ldata)
+            return -1;
+    ldata[0] = (long)fdata[0];
+    ldata[1] = (long)fdata[1];
+    ldata[2] = (long)fdata[2];
+    return 0;
+}
+
+int MPLSensor::inv_float_to_round2(float *fdata, short *ldata)
+{
+    VHANDLER_LOG;
+
+    if (!fdata || !ldata)
+        return -1;
+    ldata[0] = (short)fdata[0];
+    ldata[1] = (short)fdata[1];
+    ldata[2] = (short)fdata[2];
+    return 0;
+}
+
+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,
+            "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(sSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) {
+        LOGE("HAL:sensor list too small, not populating.");
+        return -(sizeof(sSensorList) / sizeof(sensor_t));
+    }
+
+    /* fill in the base values */
+    memcpy(list, sSensorList, sizeof (struct sensor_t) * (sizeof(sSensorList) / 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]);
+
+    if(1) {
+        numsensors = (sizeof(sSensorList) / 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 gravity values */
+        fillGravity(list);
+        /* fill in Linear accel values */
+        fillLinearAccel(list);
+    } 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, "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, "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 bma250 and might be wrong.",
+         accel);
+    list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
+    list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
+    list[Accelerometer].power = ACCEL_BMA250_POWER;
+    list[Accelerometer].minDelay = ACCEL_BMA250_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, "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 {
+        LOGE("HAL:unknown gyro id -- gyro params will be wrong.");
+        LOGE("HAL:default to use mpu3050 params");
+        list[Gyro].maxRange = GYRO_MPU3050_RANGE;
+        list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
+        list[Gyro].power = GYRO_MPU3050_POWER;
+        list[Gyro].minDelay = GYRO_MPU3050_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 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;
+}
+
+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;
+}
+
+int MPLSensor::inv_init_sysfs_attributes(void)
+{
+    VFUNC_LOG;
+
+    unsigned char i;
+    char sysfs_path[MAX_SYSFS_NAME_LEN], iio_trigger_path[MAX_SYSFS_NAME_LEN];
+    char *sptr;
+    char **dptr;
+    int num;
+
+    sysfs_names_ptr =
+            (char*)calloc(1, sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
+    sptr = sysfs_names_ptr;
+    if (sptr == NULL) {
+        LOGE("HAL:couldn't alloc mem for sysfs paths");
+        return -1;
+    }
+
+    dptr = (char**)&mpu;
+    i = 0;
+    do {
+        *dptr++ = sptr;
+        sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
+    } while (++i < MAX_SYSFS_ATTRB);
+
+    // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
+    // inv_get_sysfs_abs_path(sysfs_path);
+    if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) {
+        ALOGE("MPLSensor failed get sysfs path");
+        return -1;
+    }
+
+    if(INV_SUCCESS != inv_get_iio_trigger_path(iio_trigger_path)) {
+        ALOGE("MPLSensor failed get iio trigger path");
+        return -1;
+    }
+
+    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.power_state, "%s%s", sysfs_path, "/power_state");
+    sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path, "/scan_elements/in_timestamp_en");
+    sprintf(mpu.trigger_name, "%s%s", iio_trigger_path, "/name");
+    sprintf(mpu.current_trigger, "%s%s", sysfs_path, "/trigger/current_trigger");
+
+    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.dmp_output_rate,"%s%s", sysfs_path, "/dmp_output_rate");
+    sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
+
+    // TODO: for self test
+    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_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_x_en");
+    sprintf(mpu.gyro_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_y_en");
+    sprintf(mpu.gyro_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_z_en");
+
+    sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable");
+    sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
+    sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix");
+
+
+#ifndef THIRD_PARTY_ACCEL //MPUxxxx
+    sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale");
+    // TODO: for bias settings
+    sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/accl_bias");
+#endif
+
+    sprintf(mpu.accel_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_x_en");
+    sprintf(mpu.accel_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_y_en");
+    sprintf(mpu.accel_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_z_en");
+
+    sprintf(mpu.quaternion_on, "%s%s", sysfs_path, "/quaternion_on");
+    sprintf(mpu.in_quat_r_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_r_en");
+    sprintf(mpu.in_quat_x_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_x_en");
+    sprintf(mpu.in_quat_y_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_y_en");
+    sprintf(mpu.in_quat_z_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_z_en");
+
+    sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on");
+    sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation");
+
+#if SYSFS_VERBOSE
+    // test print sysfs paths
+    dptr = (char**)&mpu;
+    for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
+        LOGE("HAL:sysfs path: %s", *dptr++);
+    }
+#endif
+    return 0;
+}
+
+/* TODO: stop manually testing/using 0 and 1 instead of
+ * false and true, but just use 0 and non-0.
+ * This allows  passing 0 and non-0 ints around instead of
+ * having to convert to 1 and test against 1.
+ */
+bool MPLSensor::isMpu3050()
+{
+    return !strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050");
+}
+
+int MPLSensor::isLowPowerQuatEnabled()
+{
+#ifdef ENABLE_LP_QUAT_FEAT
+    return !isMpu3050();
+#else
+    return 0;
+#endif
+}
+
+int MPLSensor::isDmpDisplayOrientationOn()
+{
+#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
+    return !isMpu3050();
+#else
+    return 0;
+#endif
+}
diff --git a/60xx/libsensors_iio/MPLSensor.h b/60xx/libsensors_iio/MPLSensor.h
new file mode 100644
index 0000000..4162324
--- /dev/null
+++ b/60xx/libsensors_iio/MPLSensor.h
@@ -0,0 +1,346 @@
+/*

+* 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 <utils/Vector.h>

+#include <utils/KeyedVector.h>

+#include "sensors.h"

+#include "SensorBase.h"

+#include "InputEventReader.h"

+

+#ifdef INVENSENSE_COMPASS_CAL

+

+#ifdef COMPASS_YAS53x

+#warning "unified HAL for YAS53x"

+#include "CompassSensor.IIO.primary.h"

+#elif COMPASS_AMI306

+#warning "unified HAL for AMI306"

+#include "CompassSensor.IIO.primary.h"

+#else

+#warning "unified HAL for MPU9150"

+#include "CompassSensor.IIO.9150.h"

+#endif

+#else

+#warning "unified HAL for AKM"

+#include "CompassSensor.AKM.h"

+#endif

+

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

+/* 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_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

+

+// bit mask of current active features (mFeatureActiveMask)

+#define INV_COMPASS_CAL              0x01

+#define INV_COMPASS_FIT              0x02

+#define INV_DMP_QUATERNION           0x04

+#define INV_DMP_DISPL_ORIENTATION    0x08

+

+/* Uncomment to enable Low Power Quaternion */

+#define ENABLE_LP_QUAT_FEAT

+

+/* Uncomment to enable DMP display orientation 

+   (within the HAL, see below for Java framework) */

+// #define ENABLE_DMP_DISPL_ORIENT_FEAT

+

+#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT

+/* Uncomment following to expose the SENSOR_TYPE_SCREEN_ORIENTATION 

+   sensor type (DMP screen orientation) to the Java framework.

+   NOTE:

+       need Invensense customized 

+       'hardware/libhardware/include/hardware/sensors.h' to compile correctly.

+   NOTE:

+       need Invensense java framework changes to:

+       'frameworks/base/core/java/android/view/WindowOrientationListener.java'

+       'frameworks/base/core/java/android/hardware/Sensor.java'

+       'frameworks/base/core/java/android/hardware/SensorEvent.java'

+       for the 'Auto-rotate screen' to use this feature.

+*/

+#define ENABLE_DMP_SCREEN_AUTO_ROTATION

+#warning "ENABLE_DMP_DISPL_ORIENT_FEAT is defined, framework changes are necessary for HAL to work properly"

+#endif

+

+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:

+

+    enum {

+        Gyro = 0,

+        RawGyro,

+        Accelerometer,

+        MagneticField,

+        Orientation,

+        RotationVector,

+        LinearAccel,

+        Gravity,

+        numSensors

+    };

+

+    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);

+    int32_t getEnableMask() { return mEnabled; }

+

+    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 bool hasPendingEvents() const;

+    virtual void sleepEvent();

+    virtual void wakeEvent();

+    int populateSensorList(struct sensor_t *list, int len);

+    void cbProcData();

+

+    //static pointer to the object that will handle callbacks

+    static MPLSensor* gMPLSensor;

+

+    //AKM HAL Integration

+    //void set_compass(long ready, long x, long y, long z, long accuracy);

+    int executeOnData(sensors_event_t* data, int count);

+    int readAccelEvents(sensors_event_t* data, int count);

+    int readCompassEvents(sensors_event_t* data, int count);

+

+    int turnOffAccelFifo();

+    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();

+

+protected:

+    CompassSensor *mCompassSensor;

+

+    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 rvHandler(sensors_event_t *data);

+    int laHandler(sensors_event_t *data);

+    int gravHandler(sensors_event_t *data);

+    int orienHandler(sensors_event_t *data);

+    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 setGyroInitialState();

+    int setAccelInitialState();

+    int masterEnable(int en);

+    int onPower(int en);

+    int enableLPQuaternion(int);

+    int enableQuaternionData(int);

+    int onDMP(int);

+    int enableOneSensor(int en, const char *name, int (MPLSensor::*enabler)(int));

+    int enableGyro(int en);

+    int enableAccel(int en);

+    int enableCompass(int en);

+    void computeLocalSensorMask(int enabled_sensors);

+    int enableSensors(unsigned long sensors, int en, uint32_t changed);

+    int inv_read_gyro_buffer(int fd, short *data, long long *timestamp);

+    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_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);

+#ifdef COMPASS_YAS53x

+    int resetCompass(void);

+#endif

+    void setCompassDelay(int64_t ns);

+    void enable_iio_sysfs(void);

+    int enableTap(int);

+    int enableFlick(int);

+    int enablePedometer(int);

+    int checkLPQuaternion();

+

+    int mNewData;   // flag indicating that the MPL calculated new output values

+    int mDmpStarted;

+    long mMasterSensorMask;

+    long mLocalSensorMask;

+    int mPollTime;

+    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];

+    int mSampleCount;

+    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 dmp_orient_fd;

+

+    int mDmpOrientationEnabled;

+

+

+    uint32_t mEnabled;

+    uint32_t mOldEnabledMask;

+    sensors_event_t mPendingEvents[numSensors];

+    int64_t mDelays[numSensors];

+    hfunc_t mHandlers[numSensors];

+    short mCachedGyroData[3];

+    long mCachedAccelData[3];

+    long mCachedCompassData[3];

+    long mCachedQuaternionData[4];

+    android::KeyedVector<int, int> mIrqFds;

+

+    InputEventCircularReader mAccelInputReader;

+    InputEventCircularReader mGyroInputReader;

+

+    bool mFirstRead;

+    short mTempScale;

+    short mTempOffset;

+    int64_t mTempCurrentTime;

+    int mAccelScale;

+

+    uint32_t mPendingMask;

+    unsigned long mSensorMask;

+

+    char chip_ID[MAX_CHIP_ID_LEN];

+

+    signed char mGyroOrientation[9];

+    signed char mAccelOrientation[9];

+

+    int64_t mSensorTimestamp;

+    int64_t mCompassTimestamp;

+

+    struct sysfs_attrbs {

+       char *chip_enable;

+       char *power_state;

+       char *dmp_firmware;

+       char *firmware_loaded;

+       char *dmp_on;

+       char *dmp_int_on;

+       char *dmp_event_int_on;

+       char *dmp_output_rate;

+       char *tap_on;

+       char *key;

+       char *self_test;

+       char *temperature;

+

+       char *gyro_enable;

+       char *gyro_fifo_rate;

+       char *gyro_orient;

+       char *gyro_x_fifo_enable;

+       char *gyro_y_fifo_enable;

+       char *gyro_z_fifo_enable;

+

+       char *accel_enable;

+       char *accel_fifo_rate;

+       char *accel_fsr;

+       char *accel_bias;

+       char *accel_orient;

+       char *accel_x_fifo_enable;

+       char *accel_y_fifo_enable;

+       char *accel_z_fifo_enable;

+

+       char *quaternion_on;

+       char *in_quat_r_en;

+       char *in_quat_x_en;

+       char *in_quat_y_en;

+       char *in_quat_z_en;

+

+       char *in_timestamp_en;

+       char *trigger_name;

+       char *current_trigger;

+       char *buffer_length;

+

+       char *display_orientation_on;

+       char *event_display_orientation;

+    } mpu;

+

+    char *sysfs_names_ptr;

+    int mFeatureActiveMask;

+

+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 fillOrientation(struct sensor_t *list);

+    void fillGravity(struct sensor_t *list);

+    void fillLinearAccel(struct sensor_t *list);

+    void storeCalibration();

+    void loadDMP();

+    bool isMpu3050();

+    int isLowPowerQuatEnabled();

+    int isDmpDisplayOrientationOn();

+

+

+};

+

+extern "C" {

+    void setCallbackObject(MPLSensor*);

+    MPLSensor *getCallbackObject();

+}

+

+#endif  // ANDROID_MPL_SENSOR_H

diff --git a/60xx/libsensors_iio/MPLSupport.cpp b/60xx/libsensors_iio/MPLSupport.cpp
new file mode 100644
index 0000000..9773562
--- /dev/null
+++ b/60xx/libsensors_iio/MPLSupport.cpp
@@ -0,0 +1,190 @@
+/*
+* 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 <MPLSupport.h>
+#include <string.h>
+#include <stdio.h>
+#include <fcntl.h>
+
+#include "log.h"
+#include "SensorBase.h"
+
+#include "ml_sysfs_helper.h"
+#include "local_log_def.h"
+
+int64_t 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;
+}
+
+int64_t timevalToNano(timeval const& t) {
+    return t.tv_sec * 1000000000LL + t.tv_usec * 1000;
+}
+
+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;
+}
+
+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) < 1) {
+           LOGE("HAL:ERR failed to read an int from %s.", filename);
+           res = -EINVAL;
+        }
+        fclose(sysfsfp);
+    } else {
+        res = -errno;
+        LOGE("HAL:ERR open file %s to read with error %d", filename, res);
+    }
+    return res;
+}
+
+int write_sysfs_int(char *filename, int var)
+{
+    int res = 0;
+    FILE  *sysfsfp;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            var, filename, getTimestamp());
+    sysfsfp = fopen(filename, "w");
+    if (sysfsfp == NULL) {
+        res = -errno;
+        LOGE("HAL:ERR open file %s to write with error %d", filename, res);
+        return res;
+    }
+    int fpres, fcres = 0;
+    fpres = fprintf(sysfsfp, "%d\n", var);
+    /* fprintf() can succeed because it never actually writes to the
+     * underlying sysfs node.
+     */
+    if (fpres < 0) {
+       res = -errno;
+       fclose(sysfsfp);
+    } else {
+        fcres = fclose(sysfsfp);
+        /* Check for errors from: fflush(), write(), and close() */
+        if (fcres < 0) {
+            res = -errno;
+        }
+    }
+    if (fpres < 0 || fcres < 0) {
+        LOGE("HAL:ERR failed to write %d to %s (err=%d) print/close=%d/%d",
+            var, filename, res, fpres, fcres);
+    }
+    return res;
+}
diff --git a/60xx/libsensors_iio/MPLSupport.h b/60xx/libsensors_iio/MPLSupport.h
new file mode 100644
index 0000000..98e4497
--- /dev/null
+++ b/60xx/libsensors_iio/MPLSupport.h
@@ -0,0 +1,33 @@
+/*
+* 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>
+#include <time.h>
+
+int64_t getTimestamp();
+int64_t timevalToNano(timeval const& t);
+
+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 read_sysfs_int(char*, int*);
+int write_sysfs_int(char*, int);
+
+#endif //  ANDROID_MPL_SUPPORT_H
diff --git a/60xx/libsensors_iio/SensorBase.cpp b/60xx/libsensors_iio/SensorBase.cpp
new file mode 100644
index 0000000..21d0ed0
--- /dev/null
+++ b/60xx/libsensors_iio/SensorBase.cpp
@@ -0,0 +1,134 @@
+/*

+* 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 <dirent.h>

+#include <sys/select.h>

+#include <cutils/log.h>

+#include <linux/input.h>

+

+#include "SensorBase.h"

+#include "local_log_def.h"

+

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

+

+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);

+    }

+}

+

+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;

+}

+

+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;

+}

diff --git a/60xx/libsensors_iio/SensorBase.h b/60xx/libsensors_iio/SensorBase.h
new file mode 100644
index 0000000..fef47c7
--- /dev/null
+++ b/60xx/libsensors_iio/SensorBase.h
@@ -0,0 +1,61 @@
+/*
+* 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>
+
+#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 {
+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);
+
+    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;
+    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);
+};
+
+/*****************************************************************************/
+
+#endif  // ANDROID_SENSOR_BASE_H
diff --git a/60xx/libsensors_iio/libmllite.so b/60xx/libsensors_iio/libmllite.so
new file mode 100755
index 0000000..9bdd5bc
--- /dev/null
+++ b/60xx/libsensors_iio/libmllite.so
diff --git a/60xx/libsensors_iio/libmplmpu.so b/60xx/libsensors_iio/libmplmpu.so
new file mode 100644
index 0000000..205ab9a
--- /dev/null
+++ b/60xx/libsensors_iio/libmplmpu.so
diff --git a/60xx/libsensors_iio/local_log_def.h b/60xx/libsensors_iio/local_log_def.h
new file mode 100644
index 0000000..9135992
--- /dev/null
+++ b/60xx/libsensors_iio/local_log_def.h
@@ -0,0 +1,49 @@
+#ifndef LOCAL_LOG_DEF_H

+#define LOCAL_LOG_DEF_H

+

+/* Log enablers, each of these independent */

+

+#define PROCESS_VERBOSE (0) /* process log messages */

+#define EXTRA_VERBOSE   (0) /* verbose log messages */

+#define SYSFS_VERBOSE   (0) /* log sysfs interactions as cat/echo for repro

+                               purpose on a shell */

+#define FUNC_ENTRY      (0) /* log entry in all one-time functions */

+

+/* Note that enabling this logs may affect performance */

+#define HANDLER_ENTRY   (0) /* log entry in all handler functions */

+#define ENG_VERBOSE     (0) /* log some a lot more info about the internals */

+#define INPUT_DATA      (0) /* log the data input from the events */

+#define HANDLER_DATA    (0) /* log the data fetched from the handlers */

+

+#if defined ANDROID_JELLYBEAN

+#define LOGV            ALOGV

+#define LOGV_IF         ALOGV_IF

+#define LOGD            ALOGD

+#define LOGD_IF         ALOGD_IF

+#define LOGI            ALOGI

+#define LOGI_IF         ALOGI_IF

+#define LOGW            ALOGW

+#define LOGW_IF         ALOGW_IF

+#define LOGE            ALOGE

+#define LOGE_IF         ALOGE_IF

+#define IF_LOGV         IF_ALOGV

+#define IF_LOGD         IF_ALOGD

+#define IF_LOGI         IF_ALOGI

+#define IF_LOGW         IF_ALOGW

+#define IF_LOGE         IF_ALOGE

+#define LOG_ASSERT      ALOG_ASSERT

+#define LOG                     ALOG

+#define IF_LOG          IF_ALOG

+#else

+#warning "build for ICS or earlier version"

+#endif

+

+

+#define FUNC_LOG \

+            LOGV("%s", __PRETTY_FUNCTION__)

+#define VFUNC_LOG \

+            LOGV_IF(FUNC_ENTRY, "Entering function '%s'", __PRETTY_FUNCTION__)

+#define VHANDLER_LOG \

+            LOGV_IF(HANDLER_ENTRY, "Entering handler '%s'", __PRETTY_FUNCTION__)

+

+#endif /*ifndef LOCAL_LOG_DEF_H */

diff --git a/60xx/libsensors_iio/sensor_params.h b/60xx/libsensors_iio/sensor_params.h
new file mode 100644
index 0000000..39e3e5c
--- /dev/null
+++ b/60xx/libsensors_iio/sensor_params.h
@@ -0,0 +1,194 @@
+/*

+* 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

+

+/* Physical parameters of the sensors supported by Invensense MPL */

+#define SENSORS_ROTATION_VECTOR_HANDLE    (ID_RV)

+#define SENSORS_LINEAR_ACCEL_HANDLE       (ID_LA)

+#define SENSORS_GRAVITY_HANDLE            (ID_GR)

+#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_ORIENTATION_HANDLE        (ID_O)

+#define SENSORS_SCREEN_ORIENTATION_HANDLE (ID_SO)

+

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

+//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.f)

+#define ACCEL_MPU6500_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.f)

+#define ACCEL_MPU9250_MINDELAY          (1000)

+//ACCEL_ID_MPU9150

+#define ACCEL_MPU9150_RANGE             (2.f * GRAVITY_EARTH)

+#define ACCEL_MPU9150_RESOLUTION        (0.004f * GRAVITY_EARTH)

+#define ACCEL_MPU9150_POWER             (0.f)

+#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             (0.f)

+#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 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           (1000)

+//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           (1000)

+//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)

+

+#endif  /* INV_SENSOR_PARAMS_H */

+

diff --git a/60xx/libsensors_iio/sensors.h b/60xx/libsensors_iio/sensors.h
new file mode 100644
index 0000000..f698fc5
--- /dev/null
+++ b/60xx/libsensors_iio/sensors.h
@@ -0,0 +1,97 @@
+/*

+* 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

+

+enum {

+    ID_GY = 0,

+    ID_RG,

+    ID_A,

+    ID_M,

+    ID_O,

+    ID_RV,

+    ID_LA,

+    ID_GR,

+    ID_SO

+};

+

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

+

+/*

+ * 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)

+

+/* Compass AKM8975 */

+#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)

+

+__END_DECLS

+

+#endif  // ANDROID_SENSORS_H

diff --git a/60xx/libsensors_iio/sensors_mpl.cpp b/60xx/libsensors_iio/sensors_mpl.cpp
new file mode 100644
index 0000000..4aef514
--- /dev/null
+++ b/60xx/libsensors_iio/sensors_mpl.cpp
@@ -0,0 +1,260 @@
+/*

+* 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"

+

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

+/* The SENSORS Module */

+

+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION

+#define LOCAL_SENSORS (MPLSensor::numSensors + 1)

+#else

+#define LOCAL_SENSORS MPLSensor::numSensors

+#endif

+

+/* 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

+*/

+

+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,

+        },

+        get_sensors_list: sensors__get_sensors_list,

+};

+

+struct sensors_poll_context_t {

+    struct sensors_poll_device_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);

+

+private:

+    enum {

+        mpl = 0,

+        compass,

+        dmpOrient,

+        numSensorDrivers,   // wake pipe goes here

+        numFds,

+    };

+

+    struct pollfd mPollFds[numSensorDrivers];

+    SensorBase *mSensor;

+};

+

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

+

+sensors_poll_context_t::sensors_poll_context_t() {

+    VFUNC_LOG;

+

+    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);

+    */

+

+    // setup the callback object for handing mpl callbacks

+    setCallbackObject(mplSensor);

+

+    // 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;

+}

+

+sensors_poll_context_t::~sensors_poll_context_t() {

+    FUNC_LOG;

+    delete mSensor;

+}

+

+int sensors_poll_context_t::activate(int handle, int enabled) {

+    FUNC_LOG;

+    return mSensor->enable(handle, enabled);

+}

+

+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;

+

+    // look for new events

+    nb = poll(mPollFds, numSensorDrivers, polltime);

+

+    if (nb > 0) {

+        for (int i = 0; count && i < numSensorDrivers; i++) {

+            if (mPollFds[i].revents & (POLLIN | POLLPRI)) {

+                nb = 0;

+                if (i == mpl) {

+                    /* Ignore res */

+                    mSensor->readEvents(NULL, 0);

+                    mPollFds[i].revents = 0;

+                }

+                else if (i == compass) {

+                    /* Ignore res */

+                    ((MPLSensor*) mSensor)->readCompassEvents(NULL, count);

+                    mPollFds[i].revents = 0;

+                }

+            }

+        }

+        nb = ((MPLSensor*) mSensor)->executeOnData(data, count);

+        if (nb > 0) {

+            count -= nb;

+            nbEvents += nb;

+            data += nb;

+        }

+

+        if (mPollFds[dmpOrient].revents & (POLLIN | POLLPRI)) {

+            nb = ((MPLSensor*) mSensor)->readDmpOrientEvents(data, count);

+            mPollFds[dmpOrient].revents= 0;

+            if (isDmpScreenAutoRotationEnabled() && nb > 0) {

+                count -= nb;

+                nbEvents += nb;

+                data += nb;

+            }

+        }

+    }

+

+    return nbEvents;

+}

+

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

+

+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);

+}

+

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

+

+/** 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_t));

+

+    dev->device.common.tag = HARDWARE_DEVICE_TAG;

+    dev->device.common.version  = 0;

+    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;

+

+    *device = &dev->device.common;

+    status = 0;

+

+    return status;

+}

diff --git a/60xx/libsensors_iio/software/build/android/common.mk b/60xx/libsensors_iio/software/build/android/common.mk
new file mode 100644
index 0000000..84e7e9b
--- /dev/null
+++ b/60xx/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 = 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/60xx/libsensors_iio/software/build/android/shared.mk b/60xx/libsensors_iio/software/build/android/shared.mk
new file mode 100644
index 0000000..47dedfe
--- /dev/null
+++ b/60xx/libsensors_iio/software/build/android/shared.mk
@@ -0,0 +1,74 @@
+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)
+
+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/60xx/libsensors_iio/software/core/driver/include/linux/mpu.h b/60xx/libsensors_iio/software/core/driver/include/linux/mpu.h
new file mode 100644
index 0000000..4391226
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/driver/include/linux/mpu.h
@@ -0,0 +1,108 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* This software is licensed under the terms of the GNU General Public
+* License version 2, as published by the Free Software Foundation, and
+* may be copied, distributed, and modified under those terms.
+*
+* This program is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU General Public License for more details.
+*
+*/
+
+/**
+ *  @addtogroup  DRIVERS
+ *  @brief       Hardware drivers.
+ *
+ *  @{
+ *      @file    mpu.h
+ *      @brief   mpu definition
+ */
+
+#ifndef __MPU_H_
+#define __MPU_H_
+
+#ifdef __KERNEL__
+#include <linux/types.h>
+#include <linux/ioctl.h>
+#endif
+
+enum secondary_slave_type {
+	SECONDARY_SLAVE_TYPE_NONE,
+	SECONDARY_SLAVE_TYPE_ACCEL,
+	SECONDARY_SLAVE_TYPE_COMPASS,
+	SECONDARY_SLAVE_TYPE_PRESSURE,
+
+	SECONDARY_SLAVE_TYPE_TYPES
+};
+
+enum ext_slave_id {
+	ID_INVALID = 0,
+	GYRO_ID_MPU3050,
+	GYRO_ID_MPU6050A2,
+	GYRO_ID_MPU6050B1,
+	GYRO_ID_MPU6050B1_NO_ACCEL,
+	GYRO_ID_ITG3500,
+
+	ACCEL_ID_LIS331,
+	ACCEL_ID_LSM303DLX,
+	ACCEL_ID_LIS3DH,
+	ACCEL_ID_KXSD9,
+	ACCEL_ID_KXTF9,
+	ACCEL_ID_BMA150,
+	ACCEL_ID_BMA222,
+	ACCEL_ID_BMA250,
+	ACCEL_ID_ADXL34X,
+	ACCEL_ID_MMA8450,
+	ACCEL_ID_MMA845X,
+	ACCEL_ID_MPU6050,
+
+	COMPASS_ID_AK8963,
+	COMPASS_ID_AK8975,
+	COMPASS_ID_AK8972,
+	COMPASS_ID_AMI30X,
+	COMPASS_ID_AMI306,
+	COMPASS_ID_YAS529,
+	COMPASS_ID_YAS530,
+	COMPASS_ID_HMC5883,
+	COMPASS_ID_LSM303DLH,
+	COMPASS_ID_LSM303DLM,
+	COMPASS_ID_MMC314X,
+	COMPASS_ID_HSCDTD002B,
+	COMPASS_ID_HSCDTD004A,
+
+	PRESSURE_ID_BMA085,
+};
+
+#define INV_PROD_KEY(ver, rev) (ver * 100 + rev)
+/**
+ * struct mpu_platform_data - Platform data for the mpu driver
+ * @int_config:		Bits [7:3] of the int config register.
+ * @level_shifter:	0: VLogic, 1: VDD
+ * @orientation:	Orientation matrix of the gyroscope
+ * @sec_slave_type:     secondary slave device type, can be compass, accel, etc
+ * @sec_slave_id:       id of the secondary slave device
+ * @secondary_i2c_address: secondary device's i2c address
+ * @secondary_orientation: secondary device's orientation matrix
+ * @key:                key for MPL library.
+ *
+ * Contains platform specific information on how to configure the MPU3050 to
+ * work on this platform.  The orientation matricies are 3x3 rotation matricies
+ * that are applied to the data to rotate from the mounting orientation to the
+ * platform orientation.  The values must be one of 0, 1, or -1 and each row and
+ * column should have exactly 1 non-zero value.
+ */
+struct mpu_platform_data {
+	__u8 int_config;
+	__u8 level_shifter;
+	__s8 orientation[9];
+	enum secondary_slave_type sec_slave_type;
+	enum ext_slave_id sec_slave_id;
+	__u16 secondary_i2c_addr;
+	__s8 secondary_orientation[9];
+	__u8 key[16];
+};
+
+#endif	/* __MPU_H_ */
diff --git a/60xx/libsensors_iio/software/core/driver/include/log.h b/60xx/libsensors_iio/software/core/driver/include/log.h
new file mode 100644
index 0000000..c519691
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/driver/include/log.h
@@ -0,0 +1,364 @@
+/*
+ $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
+
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * 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, ...) \
+	ALOG(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/60xx/libsensors_iio/software/core/driver/include/mlinclude.h b/60xx/libsensors_iio/software/core/driver/include/mlinclude.h
new file mode 100644
index 0000000..9725199
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/driver/include/mlmath.h b/60xx/libsensors_iio/software/core/driver/include/mlmath.h
new file mode 100644
index 0000000..37194d6
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/driver/include/mlsl.h b/60xx/libsensors_iio/software/core/driver/include/mlsl.h
new file mode 100644
index 0000000..12f2901
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/driver/include/mltypes.h b/60xx/libsensors_iio/software/core/driver/include/mltypes.h
new file mode 100644
index 0000000..09eccce
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h b/60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h
new file mode 100644
index 0000000..b8c2511
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so b/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so
new file mode 100755
index 0000000..9bdd5bc
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so
diff --git a/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk b/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk
new file mode 100644
index 0000000..1418450
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/build/filelist.mk b/60xx/libsensors_iio/software/core/mllite/build/filelist.mk
new file mode 100644
index 0000000..011120c
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/data_builder.c b/60xx/libsensors_iio/software/core/mllite/data_builder.c
new file mode 100644
index 0000000..48993bc
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/data_builder.c
@@ -0,0 +1,1117 @@
+/*
+ $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 0 /* 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 "MPL"
+
+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_db_save_t {
+    /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */
+    long compass_bias[3];
+    /** Gyro Bias in Chip Frame in Hardware units scaled by 2^16 */
+    long gyro_bias[3];
+    /** Temperature when *gyro_bias was stored. */
+    long gyro_temp;
+    /** Accel Bias in Chip Frame in Hardware units scaled by 2^16 */
+    long accel_bias[3];
+    /** Temperature when accel bias was stored. */
+    long accel_temp;
+    long gyro_temp_slope[3];
+    /** Sensor Accuracy */
+    int gyro_accuracy;
+    int accel_accuracy;
+    int compass_accuracy;
+};
+
+struct inv_data_builder_t {
+    int num_cb;
+    struct process_t process[INV_MAX_DATA_CB];
+    struct inv_db_save_t save;
+    int compass_disturbance;
+#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;
+
+/** Change this key if the data being stored by this file changes */
+#define INV_DB_SAVE_KEY 53395
+
+#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
+
+/** 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.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;
+}
+
+/** 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));
+    return inv_register_load_store(inv_db_load_func, inv_db_save_func,
+                                   sizeof(inv_data_builder.save),
+                                   INV_DB_SAVE_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()
+{
+    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)
+{
+    sensor->sensitivity = sensitivity;
+    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);
+    }
+}
+
+/** 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;
+        }
+    }
+    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;
+    }
+    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));
+    }
+}
+
+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 accel bias.
+* @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.
+*/
+void inv_set_accel_bias(const long *bias, int accuracy)
+{
+    if (bias) {
+        if (memcmp(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias))) {
+            memcpy(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias));
+            inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias);
+        }
+    }
+    sensors.accel.accuracy = accuracy;
+    inv_data_builder.save.accel_accuracy = accuracy;
+    inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_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;
+    inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
+}
+
+/** 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_bias[0] = bias[0];
+        }
+        if (mask & 2){
+            inv_data_builder.save.accel_bias[1] = bias[1];
+        }
+        if (mask & 4){
+            inv_data_builder.save.accel_bias[2] = bias[2];
+        }
+
+        inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias);
+    }
+    sensors.accel.accuracy = accuracy;
+    inv_data_builder.save.accel_accuracy = accuracy;
+    inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
+}
+
+
+/** Sets the gyro bias
+* @param[in] bias Gyro bias in hardware units scaled by 2^16. In chip mounting frame.
+*            Length 3.
+* @param[in] accuracy Accuracy of bias. 0 = least accurate, 3 = most accurate.
+*/
+void inv_set_gyro_bias(const long *bias, int accuracy)
+{
+    if (bias != NULL) {
+        if (memcmp(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias))) {
+            memcpy(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias));
+            inv_apply_calibration(&sensors.gyro, inv_data_builder.save.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 = 1769472L;
+    inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0);
+}
+
+/* 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.
+ *  @param[in] temp
+ *              Tempearature in degrees C. 
+ */
+void inv_get_gyro_bias(long *bias, long *temp)
+{
+    if (bias != NULL)
+        memcpy(bias, inv_data_builder.save.gyro_bias,
+               sizeof(inv_data_builder.save.gyro_bias));
+    if (temp != NULL)
+        temp[0] = inv_data_builder.save.gyro_temp;
+}
+
+/** Get Accel Bias
+* @param[out] bias Accel bias where
+* @param[out] temp Temperature where 1 C = 2^16
+*/
+void inv_get_accel_bias(long *bias, long *temp)
+{
+    if (bias != NULL)
+        memcpy(bias, inv_data_builder.save.accel_bias,
+               sizeof(inv_data_builder.save.accel_bias));
+    if (temp != NULL)
+        temp[0] = inv_data_builder.save.accel_temp;
+}
+
+/** 
+ *  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_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.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) {
+        sensors.compass.raw[0] = (short)compass[0];
+        sensors.compass.raw[1] = (short)compass[1];
+        sensors.compass.raw[2] = (short)compass[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
+* @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
+    
+    memcpy(sensors.quat.raw, quat, sizeof(sensors.quat.raw));
+    sensors.quat.timestamp = timestamp;
+    sensors.quat.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
+    sensors.quat.status |= (INV_BIAS_APPLIED & status);
+
+    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()
+{
+    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()
+{
+    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)
+{
+    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()
+{
+    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;
+    int mode;
+
+#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
+    mode = 0;
+    if (sensors.gyro.status & INV_NEW_DATA)
+        mode |= INV_GYRO_NEW;
+    if (sensors.accel.status & INV_NEW_DATA)
+        mode |= INV_ACCEL_NEW;
+    if (sensors.compass.status & INV_NEW_DATA)
+        mode |= INV_MAG_NEW;
+    if (sensors.temp.status & INV_NEW_DATA)
+        mode |= INV_TEMP_NEW;
+    if (sensors.quat.status & INV_QUAT_NEW)
+        mode |= INV_QUAT_NEW;
+
+    first_error = INV_SUCCESS;
+
+    for (kk = 0; kk < inv_data_builder.num_cb; ++kk) {
+        if (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;
+}
+
+/** 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 = sensors.gyro.accuracy;
+    }
+}
+
+/** 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 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;
+}
+
+
+/**
+ * @}
+ */
diff --git a/60xx/libsensors_iio/software/core/mllite/data_builder.h b/60xx/libsensors_iio/software/core/mllite/data_builder.h
new file mode 100644
index 0000000..9aa46e6
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/data_builder.h
@@ -0,0 +1,233 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#include "mltypes.h"
+
+#ifndef INV_DATA_BUILDER_H__
+#define INV_DATA_BUILDER_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
+
+/** 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
+
+#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_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;
+    long sample_rate_us;
+    long sample_rate_ms;
+};
+
+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;
+    /** 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
+
+} inv_rd_dbg_states;
+
+/** Maximum number of data callbacks that are supported. Safe to increase if needed.*/
+#define INV_MAX_DATA_CB 20
+
+#ifdef INV_PLAYBACK_DBG
+#include <stdio.h>
+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_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, int accuracy);
+void inv_set_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_gyro_bias(long *bias, long *temp);
+void inv_get_accel_bias(long *bias, long *temp);
+
+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_gyro(long *gyro);
+
+int inv_get_gyro_accuracy(void);
+int inv_get_accel_accuracy(void);
+int inv_get_mag_accuracy(void);
+
+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 __cplusplus
+}
+#endif
+
+#endif  /* INV_DATA_BUILDER_H__ */
diff --git a/60xx/libsensors_iio/software/core/mllite/hal_outputs.c b/60xx/libsensors_iio/software/core/mllite/hal_outputs.c
new file mode 100644
index 0000000..7cdca59
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/hal_outputs.c
@@ -0,0 +1,490 @@
+/*

+ $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.

+ */

+

+#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"

+

+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;

+    inv_biquad_filter_t lp_filter[3];

+    float compass_float[3];

+};

+

+static struct hal_output_t hal_out;

+

+/** 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;

+    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_get_accel_set(accel, accuracy, timestamp);

+    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 hal_out.nine_axis_status;

+}

+

+/** 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];

+    int status;

+

+    *accuracy = (int8_t) hal_out.accuracy_quat;

+    *timestamp = hal_out.nav_timestamp;

+    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;

+    if ((hal_out.accel_status & INV_NEW_DATA) || (hal_out.gyro_status & INV_NEW_DATA))

+        status = 1;

+    else

+        status = 0;

+    return status;

+}

+

+/* 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 4.

+* @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_rotation_vector(float *values, int8_t *accuracy,

+        inv_time_t * timestamp)

+{

+    *accuracy = (int8_t) hal_out.accuracy_quat;

+    *timestamp = hal_out.nav_timestamp;

+

+    if (hal_out.nav_quat[0] >= 0) {

+        values[0] = hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30;

+        values[1] = hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30;

+        values[2] = hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30;

+        values[3] = hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30;

+    } else {

+        values[0] = -hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30;

+        values[1] = -hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30;

+        values[2] = -hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30;

+        values[3] = -hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30;

+    }

+    values[4] = inv_get_heading_confidence_interval();

+

+    return hal_out.nine_axis_status;

+}

+

+

+/** 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;

+    /* Converts fixed point to uT. Fixed point has 1 uT = 2^16.

+     * So this is: 1 / 2^16*/

+//#define COMPASS_CONVERSION 1.52587890625e-005f

+    int i;

+

+    *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;

+}

+

+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;

+    *timestamp = hal_out.nav_timestamp;

+

+    google_orientation(values);

+

+    return hal_out.nine_axis_status;

+}

+

+/** 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;

+

+    // 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;

+    }

+

+    // 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;

+        }

+    }

+

+    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;

+    }

+

+    /* 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;

+

+    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;

+        }

+

+    }

+    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);

+    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/60xx/libsensors_iio/software/core/mllite/hal_outputs.h b/60xx/libsensors_iio/software/core/mllite/hal_outputs.h
new file mode 100644
index 0000000..85e88f3
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/hal_outputs.h
@@ -0,0 +1,45 @@
+/*
+ $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_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);
+
+    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);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif                          // INV_HAL_OUTPUTS_H__
diff --git a/60xx/libsensors_iio/software/core/mllite/invensense.h b/60xx/libsensors_iio/software/core/mllite/invensense.h
new file mode 100644
index 0000000..fb8e12b
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
new file mode 100644
index 0000000..72940f7
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
@@ -0,0 +1,279 @@
+/*
+ $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>
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-loaddmp"
+
+#include "ml_load_dmp.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 3065
+
+static const unsigned char dmpMemory[DMP_CODE_SIZE] = {
+   /* bank # 0 */
+    0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 
+    0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, 
+    0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e, 
+    0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 
+    0x00, 0x00, 0x00, 0x01, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1, 
+    0xff, 0xff, 0x00, 0x00, 0x00, 0x14, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x12, 0x00, 0x02, 
+    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, 
+    0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa, 
+    0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, 
+    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00, 
+    0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65, 
+    0x3f, 0x84, 0x05, 0x99, 0xf2, 0xbc, 0x3a, 0x39, 0xf1, 0x53, 0x7a, 0x76, 0xd9, 0x0e, 0x9f, 0xc8, 
+    0x00, 0x00, 0x00, 0x00, 0xf2, 0xbc, 0x3a, 0x39, 0xf1, 0x53, 0x7a, 0x76, 0xd9, 0x0e, 0x9f, 0xc8, 
+    /* bank # 1 */
+    0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f, 
+    0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00, 
+    0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 
+    0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, 
+    0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00, 
+    0xff, 0x9b, 0xfc, 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, 0x10, 0x00, 
+    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00, 
+    0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28, 
+    0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 
+    0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00, 
+    /* bank # 2 */
+    0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 
+    0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, 
+    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 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, 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, 0x47, 0x78, 0xa2, 
+    0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e, 
+    0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c, 
+    0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, 
+    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, 0x06, 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, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19, 
+    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, 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, 0x00, 
+    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, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 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, 0x97, 0x97, 
+    0x97, 0xf1, 0xa9, 0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0x99, 
+    0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 
+    0x99, 0x99, 0xb0, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 
+    0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83, 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 
+    0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1, 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 
+    0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2, 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xb9, 0xf1, 0xa3, 
+    0x82, 0x93, 0x61, 0xba, 0xa2, 0xda, 0xde, 0xdf, 0xdb, 0x8b, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 
+    0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf, 0xd9, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 
+    0xa2, 0xd9, 0xf8, 0xdf, 0xa4, 0x83, 0xc2, 0xc5, 0xc7, 0x85, 0xc1, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf, 
+    0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xa8, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 
+    0x5d, 0xb8, 0xaa, 0x98, 0xb0, 0x87, 0x2d, 0x35, 0x3d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 
+    0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xd8, 0xb8, 
+    0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55, 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 
+    /* bank # 5 */
+    0x54, 0x7c, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88, 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 
+    0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 
+    0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 
+    0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 
+    0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29, 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 
+    0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a, 0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 
+    0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7, 0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 
+    0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 
+    0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 
+    0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 
+    0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88, 0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 
+    0xa4, 0xf0, 0x2c, 0x50, 0x78, 0xa8, 0x82, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94, 0x4a, 0x6e, 
+    0x98, 0xdb, 0x69, 0x31, 0xd9, 0x84, 0xc4, 0xcd, 0xfc, 0xdb, 0x6d, 0xd9, 0xa8, 0xfc, 0xdb, 0x25, 
+    0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0xf2, 0xa5, 0xf8, 0x8d, 0x94, 0xd1, 0xda, 0xf4, 0x19, 0xd8, 
+    0xa8, 0xf2, 0x05, 0xd1, 0xa4, 0xda, 0xc0, 0xa5, 0xf7, 0xde, 0xf9, 0xd8, 0xa5, 0xf8, 0x85, 0x95, 
+    0x18, 0xdf, 0xf1, 0xad, 0x8e, 0xc3, 0xc5, 0xc7, 0xd8, 0xf2, 0xa5, 0xf8, 0xd1, 0xd9, 0xf1, 0xad, 
+    /* bank # 6 */
+    0x8f, 0xc3, 0xc5, 0xc7, 0xd8, 0xa5, 0xf2, 0xf9, 0xf9, 0xa8, 0x8d, 0x9d, 0xf0, 0x28, 0x50, 0x78, 
+    0x84, 0x98, 0xf1, 0x26, 0x84, 0x95, 0x2d, 0x88, 0x98, 0x11, 0x52, 0x87, 0x12, 0x12, 0x88, 0x31, 
+    0xf9, 0xd9, 0xf1, 0x8d, 0x9d, 0x7a, 0xf5, 0x7c, 0xf1, 0x88, 0x7a, 0x98, 0x45, 0x8e, 0x0e, 0xf5, 
+    0x82, 0x92, 0x7c, 0x88, 0xf1, 0x5a, 0x98, 0x5a, 0x82, 0x92, 0x7e, 0x88, 0x94, 0x69, 0x98, 0x1e, 
+    0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02, 0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 
+    0xf0, 0x8d, 0x92, 0xa8, 0x49, 0x30, 0x2c, 0x50, 0x8e, 0xc9, 0x88, 0x98, 0x34, 0xf5, 0x04, 0xf1, 
+    0x61, 0xdb, 0xf9, 0xd9, 0xf2, 0xa5, 0xf8, 0xdb, 0xf9, 0xd9, 0xf3, 0xfa, 0xd8, 0xf2, 0xa5, 0xf8, 
+    0xf9, 0xd9, 0xf1, 0xaf, 0x8e, 0xc3, 0xc5, 0xc7, 0xae, 0x82, 0xc3, 0xc5, 0xc7, 0xd8, 0xa4, 0xf2, 
+    0xf8, 0xd1, 0xa5, 0xf3, 0xd9, 0xde, 0xf9, 0xdf, 0xd8, 0xa4, 0xf2, 0xf9, 0xa5, 0xf8, 0xf8, 0xd1, 
+    0xda, 0xf9, 0xf9, 0xf4, 0x18, 0xd8, 0xf2, 0xf9, 0xf9, 0xf3, 0xfb, 0xf9, 0xd1, 0xda, 0xf4, 0x1d, 
+    0xd9, 0xf3, 0xa4, 0x84, 0xc8, 0xa8, 0x9f, 0x01, 0xdb, 0xd1, 0xd8, 0xf4, 0x10, 0x1c, 0xd8, 0xbb, 
+    0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xb8, 0xd8, 0xf3, 0xaf, 
+    0x84, 0xc0, 0xa5, 0xfa, 0xf8, 0xf2, 0x85, 0xa5, 0xc6, 0xd8, 0xd8, 0xf2, 0xf9, 0xf6, 0xb5, 0xb9, 
+    0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8, 0xd8, 0x7c, 
+    0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1, 0x85, 0x30, 
+    0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99, 0xa3, 0x2d, 
+    /* bank # 7 */
+    0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb, 0xd8, 0xa0, 
+    0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9, 0xf9, 0xd8, 
+    0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9, 0xf9, 0xf9, 
+    0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac, 0xde, 0xf8, 
+    0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6, 0x9d, 0x2c, 
+    0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0, 0xde, 0xd9, 
+    0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf, 0xd9, 0xd0, 
+    0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30, 0xdb, 0x38, 
+    0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9, 0xdf, 0xd0, 
+    0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0, 0xdf, 0xdf, 
+    0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1, 0xda, 0xf2, 
+    0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6, 0xf9, 0xf9, 
+    0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 
+    0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda, 0xf2, 0xae, 
+    0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde, 0xfa, 0xf9, 
+    0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde, 0xdf, 0xa4, 
+    /* bank # 8 */
+    0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88, 0xc6, 0xa3, 
+    0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf, 0xd8, 0xd8, 
+    0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8, 0xf3, 0x84, 
+    0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5, 0xc7, 0xa3, 
+    0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8, 0xd8, 0xa3, 
+    0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7, 0xd8, 0xa1, 
+    0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 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, 0x79, 0x8a, 0x24, 0x70, 0x59, 0x8b, 0x20, 
+    0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31, 0x8b, 0x30, 
+    0x49, 0x60, 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, 
+    /* bank # 9 */
+    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, 0xf5, 0xb0, 0xb4, 0xb8, 0x88, 0x98, 0xad, 0x2c, 0x54, 0x7c, 0xb5, 0xb9, 
+    0x9e, 0xa3, 0xdf, 0xdf, 0xdf, 0xf6, 0xa3, 0x30, 0xd9, 0xfa, 0xdb, 0x35, 0xf8, 0xd8, 0xa3, 0x50, 
+    0xd0, 0xd9, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa3, 0x70, 0xd0, 0xd9, 0xfa, 0xdb, 0x75, 0xf8, 0xd8, 
+    0xa3, 0xb4, 0x8d, 0x9d, 0x30, 0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xd8, 0xa3, 0x48, 0xdb, 0x58, 
+    0xd9, 0xdf, 0xd0, 0xde, 0xd8, 0xa3, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xd8, 0xf1, 0xa3, 0xb1, 
+    0xb5, 0x82, 0xc0, 0x83, 0x93, 0x0e, 0x0a, 0x0a, 0x16, 0x12, 0x1e, 0x58, 0x38, 0x1a, 0xa3, 0x8f, 
+    0x7c, 0x83, 0x0e, 0xa3, 0x12, 0x86, 0x61, 0xd1, 0xd9, 0xc7, 0xc7, 0xd8, 0xa3, 0x6d, 0xd1, 0xd9, 
+    0xc7, 0xd8, 0xa3, 0x71, 0xd1, 0xa6, 0xd9, 0xc5, 0xda, 0xdf, 0xd8, 0x9f, 0x83, 0xf3, 0xa3, 0x65, 
+    0xd1, 0xaf, 0xc6, 0xd9, 0xfa, 0xda, 0xdf, 0xd8, 0xa3, 0x8f, 0xdf, 0x1d, 0xd1, 0xa3, 0xd9, 0xfa, 
+    0xd8, 0xa3, 0x31, 0xda, 0xfa, 0xd9, 0xaf, 0xdf, 0xd8, 0xa3, 0xfa, 0xf9, 0xd1, 0xd9, 0xaf, 0xd0, 
+    0x96, 0xc1, 0xae, 0xd0, 0x0c, 0x8e, 0x94, 0xa3, 0xf7, 0x72, 0xdf, 0xf3, 0x83, 0x93, 0xdb, 0x09, 
+    0xd9, 0xf2, 0xaa, 0xd0, 0xde, 0xd8, 0xd8, 0xd8, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 
+    0xaa, 0xf1, 0xdf, 0xdf, 0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 
+    /* bank # 10 */
+    0xdf, 0xb0, 0x84, 0xf2, 0xc8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83, 0x9a, 
+    0x08, 0x10, 0xb7, 0x9f, 0x10, 0xb5, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6, 
+    0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 
+    0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb, 
+    0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c, 
+    0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8, 
+    0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9, 
+    0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2, 
+    0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 
+    0xb4, 0xd9, 0xab, 0xde, 0xb0, 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 
+    0x8b, 0xb0, 0x87, 0xa3, 0xa3, 0xa3, 0xa3, 0xb4, 0x90, 0x80, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 
+    0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xf1, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 
+    0xac, 0xdf, 0xb9, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 
+    0xa3, 0xa3, 0xa3, 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 
+    0x9a, 0xaa, 0x28, 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 
+    0x51, 0xf1, 0x90, 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 
+    /* bank # 11 */
+    0x94, 0x01, 0x29, 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 
+    0x78, 0xa3, 0x83, 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 
+    0xa8, 0x92, 0x19, 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 
+    0x96, 0xa8, 0x39, 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 
+    0xda, 0x87, 0xa7, 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 
+    0x39, 0xa9, 0x80, 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 
+    0x31, 0x98, 0xd9, 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 
+    0xa9, 0xda, 0x26, 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 
+    0x2e, 0xd8, 0x89, 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 
+    0x2e, 0xd8, 0xa8, 0x87, 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, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 
+    0xff, 0xd8, 0xa2, 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 
+    0xf1, 0xd9, 0x00, 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 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, 0xf1, 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;
+}
+
+/**
+ *  @}
+ */
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h
new file mode 100644
index 0000000..4d98692
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h
@@ -0,0 +1,33 @@
+/*
+ $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);
+
+#ifdef __cplusplus
+}
+#endif
+#endif  /* INV_LOAD_DMP_H */
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
new file mode 100644
index 0000000..b4c4249
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
@@ -0,0 +1,353 @@
+/*
+ $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.
+ */
+
+#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_LOGI("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_LOGI("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_LOGI("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_LOGI("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_LOGI("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_LOGE("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/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
new file mode 100644
index 0000000..115b34c
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
new file mode 100644
index 0000000..d0c4513
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
@@ -0,0 +1,423 @@
+#include <string.h>
+#include <stdio.h>
+#include "ml_sysfs_helper.h"
+#include <dirent.h>
+#include <ctype.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"
+};
+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) {
+		printf("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[0])] = '\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;
+}
+/** 
+ *  @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(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(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/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h
new file mode 100644
index 0000000..9470874
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h
@@ -0,0 +1,36 @@
+/*
+ $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);
+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(char *name);
+inv_error_t inv_get_iio_device_node(char *name);
+
+#ifdef __cplusplus
+}
+#endif
+#endif	/* MLDMP_SYSFS_HELPER_H__ */
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/mlos.h b/60xx/libsensors_iio/software/core/mllite/linux/mlos.h
new file mode 100644
index 0000000..d4f8912
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c b/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c
new file mode 100644
index 0000000..5424508
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/message_layer.c b/60xx/libsensors_iio/software/core/mllite/message_layer.c
new file mode 100644
index 0000000..8317957
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/message_layer.h b/60xx/libsensors_iio/software/core/mllite/message_layer.h
new file mode 100644
index 0000000..df0c0e2
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/message_layer.h
@@ -0,0 +1,35 @@
+/*

+ $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)

+

+    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/60xx/libsensors_iio/software/core/mllite/ml_math_func.c b/60xx/libsensors_iio/software/core/mllite/ml_math_func.c
new file mode 100644
index 0000000..c30d693
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/ml_math_func.c
@@ -0,0 +1,706 @@
+/*
+ $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] x a vector [3x1]
+* @return the normalize 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];
+}
+
+/**
+ * @}
+ */
diff --git a/60xx/libsensors_iio/software/core/mllite/ml_math_func.h b/60xx/libsensors_iio/software/core/mllite/ml_math_func.h
new file mode 100644
index 0000000..535d849
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/ml_math_func.h
@@ -0,0 +1,120 @@
+/*
+ $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]);
+
+#ifdef __cplusplus
+}
+#endif
+#endif // INVENSENSE_INV_MATH_FUNC_H__
diff --git a/60xx/libsensors_iio/software/core/mllite/mpl.c b/60xx/libsensors_iio/software/core/mllite/mpl.c
new file mode 100644
index 0000000..9141cc6
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/mpl.c
@@ -0,0 +1,72 @@
+/*

+ $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"

+

+/**

+ * @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());

+

+    return INV_SUCCESS;

+}

+

+const char ml_ver[] = "InvenSense MPL 5.1.2 beta RC9";

+

+/**

+ *  @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/60xx/libsensors_iio/software/core/mllite/mpl.h b/60xx/libsensors_iio/software/core/mllite/mpl.h
new file mode 100644
index 0000000..a6b5ac7
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/results_holder.c b/60xx/libsensors_iio/software/core/mllite/results_holder.c
new file mode 100644
index 0000000..df58f40
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/results_holder.c
@@ -0,0 +1,522 @@
+/*
+ $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"
+
+// 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
+
+struct results_t {
+    long nav_quat[4];
+    long gam_quat[4];
+    inv_time_t nav_timestamp;
+    inv_time_t gam_timestamp;
+    long local_field[3]; /**< local earth's magnetic field */
+    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 */
+    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;
+};
+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
+*/
+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
+* 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
+* 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;
+}
+
+/** 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 local earth's magnetic field
+* @param[in] data Local earth's magnetic field in uT scaled by 2^16.
+*            Length = 3. Y typically points north, Z typically points down in
+*                        northern hemisphere and up in southern hemisphere.
+*/
+void inv_set_local_field(const long *data)
+{
+    memcpy(rh.local_field, data, sizeof(rh.local_field));
+}
+
+/** Gets the local earth's magnetic field
+* @param[out] data Local earth's magnetic field in uT scaled by 2^16.
+*            Length = 3. Y typically points north, Z typically points down in
+*                        northern hemisphere and up in southern hemisphere.
+*/
+void inv_get_local_field(long *data)
+{
+    memcpy(data, rh.local_field, sizeof(rh.local_field));
+}
+
+/** 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)
+{
+    data[0] =
+        inv_q29_mult(rh.nav_quat[1], rh.nav_quat[3]) - inv_q29_mult(rh.nav_quat[2], rh.nav_quat[0]);
+    data[1] =
+        inv_q29_mult(rh.nav_quat[2], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[1], rh.nav_quat[0]);
+    data[2] =
+        (inv_q29_mult(rh.nav_quat[3], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[0], rh.nav_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)
+{
+    memcpy(data, rh.gam_quat, sizeof(rh.gam_quat));
+    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)
+{
+    if (rh.status & (INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET)) {
+        inv_q_mult(rh.compass_correction, rh.gam_quat, rh.nav_quat);
+        rh.status &= ~(INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET);
+    }
+    memcpy(data, rh.nav_quat, sizeof(rh.nav_quat));
+    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)
+{
+    long ldata[4];
+    inv_error_t result = inv_get_quaternion(ldata);
+    data[0] = inv_q30_to_float(ldata[0]);
+    data[1] = inv_q30_to_float(ldata[1]);
+    data[2] = inv_q30_to_float(ldata[2]);
+    data[3] = inv_q30_to_float(ldata[3]);
+    return result;
+}
+
+/** 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] = 1L<<30;
+    rh.quat_confidence_interval = (float)M_PI;
+    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;
+}
+
+/**
+ *  @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;
+    }
+}
+
+/**
+ * @}
+ */
diff --git a/60xx/libsensors_iio/software/core/mllite/results_holder.h b/60xx/libsensors_iio/software/core/mllite/results_holder.h
new file mode 100644
index 0000000..09da24e
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/results_holder.h
@@ -0,0 +1,81 @@
+/*
+ $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
+
+void inv_store_gaming_quaternion(const long *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
+
+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_6axis_quaternion(long *data);
+inv_error_t inv_get_quaternion(long *data);
+inv_error_t inv_get_quaternion_float(float *data);
+void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp);
+
+inv_error_t inv_enable_results_holder();
+inv_error_t inv_init_results_holder(void);
+
+/* Magnetic Field Parameters*/
+void inv_set_local_field(const long *data);
+void inv_get_local_field(long *data);
+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);
+
+int inv_got_accel_bias();
+void inv_set_accel_bias_found(int state);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_RESULTS_HOLDER_H__
diff --git a/60xx/libsensors_iio/software/core/mllite/start_manager.c b/60xx/libsensors_iio/software/core/mllite/start_manager.c
new file mode 100644
index 0000000..fb758e7
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/start_manager.h b/60xx/libsensors_iio/software/core/mllite/start_manager.h
new file mode 100644
index 0000000..899e3f5
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/storage_manager.c b/60xx/libsensors_iio/software/core/mllite/storage_manager.c
new file mode 100644
index 0000000..3e4e619
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/storage_manager.c
@@ -0,0 +1,204 @@
+/*
+ $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.
+ */
+
+#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 (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) {
+                checksum = inv_checksum(data, hd->size);
+                if (checksum == hd->checksum) {
+                    ds.load[entry](data);
+                } else {
+                    return INV_ERROR_CALIBRATION_LOAD;
+                }
+            }
+        }
+        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 (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/60xx/libsensors_iio/software/core/mllite/storage_manager.h b/60xx/libsensors_iio/software/core/mllite/storage_manager.h
new file mode 100644
index 0000000..7255591
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/accel_auto_cal.h b/60xx/libsensors_iio/software/core/mpl/accel_auto_cal.h
new file mode 100644
index 0000000..5a53213
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/authenticate.h b/60xx/libsensors_iio/software/core/mpl/authenticate.h
new file mode 100644
index 0000000..d7c803b
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/authenticate.h
@@ -0,0 +1,21 @@
+/*
+ $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);
+
+#endif  /* MLDMP_AUTHENTICATE_H__ */
diff --git a/60xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so b/60xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
new file mode 100644
index 0000000..fbd346f
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
diff --git a/60xx/libsensors_iio/software/core/mpl/build/android/shared.mk b/60xx/libsensors_iio/software/core/mpl/build/android/shared.mk
new file mode 100644
index 0000000..2e15205
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/build/android/shared.mk
@@ -0,0 +1,88 @@
+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
+
+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)
+
+$(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/60xx/libsensors_iio/software/core/mpl/build/filelist.mk b/60xx/libsensors_iio/software/core/mpl/build/filelist.mk
new file mode 100644
index 0000000..e18003a
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/build/filelist.mk
@@ -0,0 +1,46 @@
+#### filelist.mk for mpl ####
+
+# headers only
+HEADERS := $(MPL_DIR)/mpu.h
+
+# 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
+#HEADERS += $(MPL_DIR)/auto_calibration.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
+#SOURCES += $(MPL_DIR)/auto_calibration.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH = $(MPL_DIR)
+
diff --git a/60xx/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h b/60xx/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h
new file mode 100644
index 0000000..4f01fc2
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/compass_fit.h b/60xx/libsensors_iio/software/core/mpl/compass_fit.h
new file mode 100644
index 0000000..be3cce8
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/compass_vec_cal.h b/60xx/libsensors_iio/software/core/mpl/compass_vec_cal.h
new file mode 100644
index 0000000..a3e044c
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/compass_vec_cal.h
@@ -0,0 +1,34 @@
+/*
+ $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();
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // COMPASS_ONLY_CAL_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/fast_no_motion.h b/60xx/libsensors_iio/software/core/mpl/fast_no_motion.h
new file mode 100644
index 0000000..c553926
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/fast_no_motion.h
@@ -0,0 +1,46 @@
+/*

+ $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 N);

+    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);

+

+    void inv_fnm_debug_print(void);

+

+#ifdef __cplusplus

+}

+#endif

+

+

+#endif // MLDMP_FAST_NO_MOTION_H__

+

diff --git a/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h b/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h
new file mode 100644
index 0000000..1ba1ebb
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h
@@ -0,0 +1,37 @@
+/*

+ $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);

+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_FUSION9AXIS_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/gyro_tc.h b/60xx/libsensors_iio/software/core/mpl/gyro_tc.h
new file mode 100644
index 0000000..8f1d50e
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h b/60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h
new file mode 100644
index 0000000..09ecc42
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/inv_math.h b/60xx/libsensors_iio/software/core/mpl/inv_math.h
new file mode 100644
index 0000000..6620bbf
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/invensense_adv.h b/60xx/libsensors_iio/software/core/mpl/invensense_adv.h
new file mode 100644
index 0000000..12932c9
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/mag_disturb.h b/60xx/libsensors_iio/software/core/mpl/mag_disturb.h
new file mode 100644
index 0000000..38df919
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/mag_disturb.h
@@ -0,0 +1,37 @@
+/*

+ $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
+
+    int inv_check_magnetic_disturbance(unsigned long delta_time, const long *quat,
+        const long *compass, const long *gravity);
+
+    void inv_track_dip_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_disable_dip_tracking(void);
+    inv_error_t inv_enable_dip_tracking(void);
+    inv_error_t inv_init_magnetic_disturbance(void);

+
+    float Mag3ofNormalizedLong(const long *x);

+

+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_MAGDISTURB_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h b/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h
new file mode 100644
index 0000000..01cf1c0
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h
@@ -0,0 +1,28 @@
+/*

+ $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"

+

+#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);

+

+#ifdef __cplusplus

+}

+#endif

+

+#endif // INV_MOTION_NO_MOTION_H__

diff --git a/60xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h b/60xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h
new file mode 100644
index 0000000..38d5690
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h
@@ -0,0 +1,34 @@
+/*

+ $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);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_NOGYROFUSION_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h b/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
new file mode 100644
index 0000000..3c6a2c1
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h b/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h
new file mode 100644
index 0000000..4fa36b0
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h
@@ -0,0 +1,27 @@
+/*

+ $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

+

+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);
+

+#ifdef __cplusplus

+}

+#endif

+

+#endif // INV_QUATERNION_SUPERVISOR_H__

diff --git a/60xx/libsensors_iio/software/core/mpl/shake.h b/60xx/libsensors_iio/software/core/mpl/shake.h
new file mode 100644
index 0000000..67acb7b
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared b/60xx/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared
new file mode 100644
index 0000000..7635cd8
--- /dev/null
+++ b/60xx/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared
diff --git a/60xx/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk b/60xx/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk
new file mode 100644
index 0000000..7655e4d
--- /dev/null
+++ b/60xx/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk
@@ -0,0 +1,95 @@
+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 += -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/60xx/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk b/60xx/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk
new file mode 100644
index 0000000..75d93cf
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c b/60xx/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c
new file mode 100644
index 0000000..d38d478
--- /dev/null
+++ b/60xx/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c
@@ -0,0 +1,535 @@
+/**
+ *  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"
+
+#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 FLICK_UPPER_THRES       3147790
+#define FLICK_LOWER_THRES       -3147790
+#define FLICK_COUNTER           50
+#define POLL_TIME               2000 // 2sec
+
+#define FALSE   0
+#define TRUE    1
+
+char *sysfs_names_ptr;
+
+struct sysfs_attrbs {
+    char *enable;
+    char *power_state;
+    char *dmp_on;
+    char *dmp_int_on;
+    char *self_test;
+    char *dmp_firmware;
+    char *firmware_loaded;
+    char *display_orientation_on;
+    char *orientation_on;
+    char *event_flick;
+    char *event_display_orientation;
+    char *event_orientation;
+    char *event_tap;
+    char *flick_axis;
+    char *flick_counter;
+    char *flick_int_on;
+    char *flick_lower;
+    char *flick_upper;
+    char *flick_message_on;
+    char *tap_min_count;
+    char *tap_on;
+    char *tap_threshold;
+    char *tap_time;
+} mpu;
+
+enum {
+    tap,
+    flick,
+    gOrient,
+    orient,
+    numDMPFeatures
+};
+
+struct pollfd pfd[numDMPFeatures];
+
+/*******************************************************************************
+ *                       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 {
+        MPL_LOGE("ERR open file to read");
+        res= -1;
+    }
+    return res;
+}
+
+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("ERR open file to write");
+        res= -1;
+    }
+    return res;
+}
+
+/**************************************************
+    This _kbhit() function is courtesy from Web
+***************************************************/
+int _kbhit() {
+    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 {
+        MPL_LOGE("couldn't alloc mem for sysfs paths");
+        return -1;
+    }
+
+    // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
+    inv_get_sysfs_path(sysfs_path);
+
+    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.self_test, "%s%s", sysfs_path, "/self_test");
+    sprintf(mpu.dmp_firmware, "%s%s", sysfs_path, "/dmp_firmware");
+    sprintf(mpu.firmware_loaded, "%s%s", sysfs_path, "/firmware_loaded");
+    sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on");
+    sprintf(mpu.orientation_on, "%s%s", sysfs_path, "/orientation_on");
+    sprintf(mpu.event_flick, "%s%s", sysfs_path, "/event_flick");
+    sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation");
+    sprintf(mpu.event_orientation, "%s%s", sysfs_path, "/event_orientation");
+    sprintf(mpu.event_tap, "%s%s", sysfs_path, "/event_tap");
+    sprintf(mpu.flick_axis, "%s%s", sysfs_path, "/flick_axis");
+    sprintf(mpu.flick_counter, "%s%s", sysfs_path, "/flick_counter");
+    sprintf(mpu.flick_int_on, "%s%s", sysfs_path, "/flick_int_on");
+    sprintf(mpu.flick_lower, "%s%s", sysfs_path, "/flick_lower");
+    sprintf(mpu.flick_upper, "%s%s", sysfs_path, "/flick_upper");
+    sprintf(mpu.flick_message_on, "%s%s", sysfs_path, "/flick_message_on");
+    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");
+
+#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 DmpFWloaded()
+{
+    int res;
+    read_sysfs_int(mpu.firmware_loaded, &res);
+    return res;
+}
+
+int enable_flick(int en)
+{
+   int res=0;
+   int flickUpper=0, flickLower=0, flickCounter=0;
+
+   if (write_sysfs_int(mpu.flick_int_on, en) < 0) {
+       printf("GT:ERR-can't write 'flick_int_on'");
+       res= -1;
+   }
+
+   if (en) {
+       flickUpper= FLICK_UPPER_THRES;
+       flickLower= FLICK_LOWER_THRES;
+       flickCounter= FLICK_COUNTER;
+   }
+
+   if (write_sysfs_int(mpu.flick_upper, flickUpper) < 0) {
+       printf("GT:ERR-can't write 'flick_upper'");
+       res= -1;
+   }
+
+   if (write_sysfs_int(mpu.flick_lower, flickLower) < 0) {
+       printf("GT:ERR-can't write 'flick_lower'");
+       res= -1;
+   }
+
+   if (write_sysfs_int(mpu.flick_counter, flickCounter) < 0) {
+       printf("GT:ERR-can't write 'flick_counter'");
+       res= -1;
+   }
+
+   if (write_sysfs_int(mpu.flick_message_on, 0) < 0) {
+       printf("GT:ERR-can't write 'flick_message_on'");
+       res= -1;
+   }
+
+   if (write_sysfs_int(mpu.flick_axis, 0) < 0) {
+       printf("GT:ERR_can't write 'flick_axis'");
+       res= -1;
+   }
+
+   return res;
+}
+
+int enable_tap(int en)
+{
+    if (write_sysfs_int(mpu.tap_on, en) < 0) {
+        printf("GT:ERR-can't write 'tap_on'");
+        return -1;
+    }
+
+    return 0;
+}
+
+int enable_displ_orient(int en)
+{
+    if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
+        printf("GT:ERR-can't write 'display_orientation_en'");
+        return -1;
+    }
+
+    return 0;
+}
+
+int enable_orient(int en)
+{
+    if (write_sysfs_int(mpu.orientation_on, en) < 0) {
+        printf("GT:ERR-can't write 'orientation_on'");
+        return -1;
+    }
+
+    return 0;
+}
+
+int flickHandler()
+{
+    FILE *fp;
+    int data;
+
+#ifdef DEBUG_PRINT
+    printf("GT:Flick Handler\n");
+#endif
+
+    fp = fopen(mpu.event_flick, "rt");
+    fscanf(fp, "%d\n", &data);
+    fclose (fp);
+
+    printf("Flick= %x\n", data);
+
+    return 0;
+}
+
+int tapHandler()
+{
+    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\n");
+            break;
+        case 2:
+            printf("Tap Axis->X Neg\n");
+            break;
+        case 3:
+            printf("Tap Axis->Y Pos\n");
+            break;
+        case 4:
+            printf("Tap Axis->Y Neg\n");
+            break;
+        case 5:
+            printf("Tap Axis->Z Pos\n");
+            break;
+        case 6:
+            printf("Tap Axis->Z Neg\n");
+            break;
+        default:
+            printf("Tap Axis->Unknown\n");
+            break;
+    }
+
+    return 0;
+}
+
+int googleOrientHandler()
+{
+    FILE *fp;
+    int orient;
+
+#ifdef DEBUG_PRINT
+    printf("GT:Google Orient Handler\n");
+#endif
+
+    fp = fopen(mpu.event_display_orientation, "rt");
+    fscanf(fp, "%d\n", &orient);
+    fclose(fp);
+
+    printf("Google Orient-> %d\n", orient);
+
+    return 0;
+}
+
+int orientHandler()
+{
+    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;
+}
+
+int enableDMPFeatures(int en)
+{
+    int res= -1;
+
+    if (DmpFWloaded())
+    {
+        /* 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 */
+
+        enable_flick(en);
+        enable_tap(en);
+        enable_displ_orient(en);
+        enable_orient(en);
+        res= 0;
+    }
+
+    return res;
+}
+
+int initFds()
+{
+    int i;
+
+    for (i=0; i< numDMPFeatures; i++) {
+        switch(i) {
+            case tap:
+                pfd[i].fd = open(mpu.event_tap, O_RDONLY | O_NONBLOCK);
+                break;
+
+            case flick:
+                pfd[i].fd = open(mpu.event_flick, O_RDONLY | O_NONBLOCK);
+                break;
+
+            case gOrient:
+                pfd[i].fd = open(mpu.event_display_orientation, O_RDONLY | O_NONBLOCK);
+                break;
+
+            case orient:
+                pfd[i].fd = open(mpu.event_orientation, O_RDONLY | O_NONBLOCK);
+                break;
+
+            default:
+                pfd[i].fd = -1;
+         }
+
+        pfd[i].events = POLLPRI|POLLERR,
+        pfd[i].revents = 0;
+#ifdef DEBUG_PRINT
+        printf("GT:pfd[%d].fd= %d\n", i, pfd[i].fd);
+#endif
+    }
+
+    return 0;
+}
+
+int closeFds()
+{
+    int i;
+    for (i = 0; i < numDMPFeatures; i++) {
+        if (!pfd[i].fd)
+            close(pfd[i].fd);
+    }
+    return 0;
+}
+
+/*******************************************************************************
+ *                       M a i n  S e l f  T e s t
+ ******************************************************************************/
+
+int main(int argc, char **argv)
+{
+    char data[4];
+    int i, res= 0;
+
+    res = inv_init_sysfs_attributes();
+    if (res) {
+        printf("GT:ERR-Can't allocate mem");
+        return -1;
+    }
+
+    /* On Gesture/DMP supported features */
+    enableDMPFeatures(1);
+
+    /* init Fds to poll for Gesture data */
+    initFds();
+
+    /* prompt user to make gesture and how to stop program */
+    printf("\n**Please make Gesture to see data.  Press any key to stop Prog**\n\n");
+
+    do {
+        for (i=0; i< numDMPFeatures; i++) {
+            read(pfd[i].fd, data, 4);
+        }
+
+        poll(pfd, numDMPFeatures, POLL_TIME);
+
+        for (i=0; i< numDMPFeatures; i++) {
+           if(pfd[i].revents != 0) {
+               switch(i) {
+                   case tap:
+                       tapHandler();
+                       break;
+
+                   case flick:
+                       flickHandler();
+                       break;
+
+                   case gOrient:
+                       googleOrientHandler();
+                       break;
+
+                   case orient:
+                       orientHandler();
+                       break;
+
+                   default:
+                       printf("GT:ERR-Not supported");
+                       break;
+               }
+               pfd[i].revents= 0;	//no need. reset anyway
+           }
+        }
+
+    } while (!_kbhit());
+
+    /* Off DMP features */
+    enableDMPFeatures(0);
+
+    /* release resources */
+    closeFds();
+    if (sysfs_names_ptr) {
+        free(sysfs_names_ptr);
+    }
+
+    printf("\nThank You!\n");
+
+    return res;
+}
+
diff --git a/60xx/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared b/60xx/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared
new file mode 100644
index 0000000..aab27bb
--- /dev/null
+++ b/60xx/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared
diff --git a/60xx/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk b/60xx/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk
new file mode 100644
index 0000000..234c1d9
--- /dev/null
+++ b/60xx/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk
@@ -0,0 +1,109 @@
+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
+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 += -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
+
+PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x
+PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o
+PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o
+
+LFLAGS += $(CMDLINE_LFLAGS)
+LFLAGS += -nostdlib
+LFLAGS += -fpic
+LFLAGS += -Wl,--gc-sections 
+LFLAGS += -Wl,--no-whole-archive 
+LFLAGS += -Wl,-dynamic-linker,/system/bin/linker
+LFLAGS += $(ANDROID_LINK)
+ifneq ($(PRODUCT),panda)
+LFLAGS += -rdynamic
+endif
+
+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) $(PRE_LFLAGS) $(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/60xx/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk b/60xx/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk
new file mode 100644
index 0000000..8a3977a
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h b/60xx/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h
new file mode 100644
index 0000000..773ff2c
--- /dev/null
+++ b/60xx/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h
@@ -0,0 +1,643 @@
+/* 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);
+	if (temp == NULL)
+		return -ENOMEM;
+	sprintf(temp, "%s/%s", basedir, filename);
+	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);
+		if (test != val) {
+			printf("Possible failure in int write %d to %s%s\n",
+				val,
+				basedir,
+				filename);
+			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);
+		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(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/60xx/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c b/60xx/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c
new file mode 100644
index 0000000..b3d323c
--- /dev/null
+++ b/60xx/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c
@@ -0,0 +1,685 @@
+/* Industrialio buffer test code.
+ *
+ * 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.
+ *
+ * This program is primarily intended as an example application.
+ * Reads the current buffer setup from sysfs and starts a short capture
+ * from the specified device, pretty printing the result after appropriate
+ * conversion.
+ *
+ * Command line parameters
+ * generic_buffer -n <device_name> -t <trigger_name>
+ * If trigger name is not specified the program assumes you want a dataready
+ * trigger associated with the device and goes looking for it.
+ *
+ */
+
+#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 "iio_utils.h"
+#include "ml_load_dmp.h"
+#include "ml_sysfs_helper.h"
+#include "authenticate.h"
+
+#define FLICK_SUPPORTED (0)
+
+/**
+ * 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");
+}
+
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+void 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 = write_sysfs_int_and_verify("flick_upper", p, 3147790);
+    if (ret < 0)
+        return;
+    ret = write_sysfs_int_and_verify("flick_lower", p, -3147790);
+    if (ret < 0)
+        return;
+
+    ret = write_sysfs_int_and_verify("flick_counter", p, 50);
+    if (ret < 0)
+        return;
+    ret = write_sysfs_int_and_verify("flick_message_on", p, 0);
+    if (ret < 0)
+        return;
+    ret = write_sysfs_int_and_verify("flick_axis", p, 0);
+}
+#endif
+
+void HandleOrient(int orient)
+{    
+    if (orient & 0x01)
+    printf("INV_X_UP\n");
+    if (orient & 0x02) 
+    printf("INV_X_DOWN\n");
+    if (orient & 0x04) 
+    printf("INV_Y_UP\n");
+    if (orient & 0x08) 
+    printf("INV_Y_DOWN\n");
+    if (orient & 0x10) 
+    printf("INV_Z_UP\n");
+    if (orient & 0x20) 
+    printf("INV_Z_DOWN\n");
+    if (orient & 0x40) 
+    printf("INV_ORIENTATION_FLIP\n");
+}
+
+void HandleTap(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 3060
+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);
+}
+
+void setup_dmp(char *dev_path, int p_event){
+    char sysfs_path[200];
+    char dmp_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, 2);
+	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("in_accel_x_offset", sysfs_path, 0xabcd0000);
+	ret = write_sysfs_int_and_verify("in_accel_y_offset", sysfs_path, 0xffff0000);
+	ret = write_sysfs_int_and_verify("in_accel_z_offset", sysfs_path, 0xcdef0000);
+
+	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_flick(sysfs_path, 1);
+	ret = write_sysfs_int_and_verify("tap_on", sysfs_path, 1);
+	if (ret < 0)
+		return;
+	ret = write_sysfs_int_and_verify("display_orientation_on", sysfs_path, 1);
+	if (ret < 0)
+		return;
+	ret = write_sysfs_int_and_verify("orientation_on", sysfs_path, 1);
+	if (ret < 0)
+		return;
+	printf("rate\n");
+	ret = write_sysfs_int_and_verify("dmp_output_rate", sysfs_path, 25);
+	if (ret < 0)
+		return;
+	ret = write_sysfs_int_and_verify("dmp_event_int_on", sysfs_path, p_event);
+	if (ret < 0)
+		return;
+	//verify_img(dmp_path);
+}
+
+void get_dmp_event(char *dev_dir_name)
+{
+    char file_name[100];
+    int i;
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+    int fp_tap, fp_orient, fp_disp, fp_flick;
+    const int n_gest = 6;
+#else
+    int fp_tap, fp_orient, fp_disp, fp_motion;
+    //int fp_no_motion;
+    const int n_gest = 4;
+#endif
+    int data;
+    char d[6];
+    FILE *fp;
+    struct pollfd pfd[4];
+    printf("%s\n", dev_dir_name);
+    while(1) {
+        sprintf(file_name, "%s/event_tap", dev_dir_name);
+        fp_tap = open(file_name, O_RDONLY | O_NONBLOCK);
+        sprintf(file_name, "%s/event_orientation", dev_dir_name);
+        fp_orient = open(file_name, O_RDONLY | O_NONBLOCK);
+        sprintf(file_name, "%s/event_display_orientation", dev_dir_name);
+        fp_disp = open(file_name, O_RDONLY | O_NONBLOCK);
+
+        //sprintf(file_name, "%s/event_accel_motion", dev_dir_name);
+        sprintf(file_name, "%s/event_accel_wom", dev_dir_name);
+        fp_motion = open(file_name, O_RDONLY | O_NONBLOCK);
+        //sprintf(file_name, "%s/event_accel_no_motion", dev_dir_name);
+        //fp_no_motion = open(file_name, O_RDONLY | O_NONBLOCK);
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+        sprintf(file_name, "%s/event_flick", dev_dir_name);
+        fp_flick = open(file_name, O_RDONLY | O_NONBLOCK);
+#endif
+
+        pfd[0].fd = fp_tap;
+        pfd[0].events = POLLPRI|POLLERR,
+        pfd[0].revents = 0;
+
+        pfd[1].fd = fp_orient;
+        pfd[1].events = POLLPRI|POLLERR,
+        pfd[1].revents = 0;
+
+        pfd[2].fd = fp_disp;
+        pfd[2].events = POLLPRI|POLLERR,
+        pfd[2].revents = 0;
+
+        pfd[3].fd = fp_motion;
+        pfd[3].events = POLLPRI|POLLERR,
+        pfd[3].revents = 0;
+
+        //pfd[4].fd = fp_no_motion;
+        //pfd[4].events = POLLPRI|POLLERR,
+        //pfd[4].revents = 0;
+
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+        pfd[5].fd = fp_flick;
+        pfd[5].events = POLLPRI|POLLERR,
+        pfd[5].revents = 0;    
+#endif
+
+        read(fp_tap, d, 4);
+        read(fp_orient, d, 4);
+        read(fp_disp, d, 4);
+        read(fp_motion, d, 4);
+        //read(fp_no_motion, d, 4);
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+        read(fp_flick, d, 4);
+#endif
+
+        poll(pfd, n_gest, -1);
+        close(fp_tap);
+        close(fp_orient);
+        close(fp_disp);
+        close(fp_motion);
+        //close(fp_no_motion);
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+        close(fp_flick);
+#endif
+        for (i = 0; i < ARRAY_SIZE(pfd); i++) {
+            if(pfd[i].revents != 0) {
+                switch (i){
+                case 0:
+                    sprintf(file_name, "%s/event_tap", dev_dir_name);
+                    fp = fopen(file_name, "rt");
+                    fscanf(fp, "%d\n", &data);
+                    printf("tap=%x\n", data);
+                    HandleTap(data);
+                    fclose(fp);
+                break;
+                case 1:
+                    sprintf(file_name, "%s/event_orientation", dev_dir_name);
+                    fp = fopen(file_name, "rt");
+                    fscanf(fp, "%d\n", &data);
+                    printf("orient=%x\n", data);
+                    HandleOrient(data);
+                    fclose(fp);
+                break;
+                case 2:
+                    sprintf(file_name, "%s/event_display_orientation", dev_dir_name);
+                    fp = fopen(file_name, "rt");
+                    fscanf(fp, "%d\n", &data);
+                    printf("display_orient=%x\n", data);
+                    fclose(fp);
+                break;
+                case 3:
+                    sprintf(file_name, "%s/event_accel_wom", dev_dir_name);
+                    fp = fopen(file_name, "rt");
+                    fscanf(fp, "%d\n", &data);
+                    printf("motion=%x\n", data);
+                    fclose(fp);
+                break;
+                case 4:
+                    sprintf(file_name, "%s/event_accel_no_motion", dev_dir_name);
+                    fp = fopen(file_name, "rt");
+                    fscanf(fp, "%d\n", &data);
+                    printf("No motion=%x\n", data);
+                    fclose(fp);
+                break;
+
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+                case 5:
+                    sprintf(file_name, "%s/event_flick", dev_dir_name);
+                    fp = fopen(file_name, "rt");
+                    fscanf(fp, "%d\n", &data);
+                    printf("flick=%x\n", data);
+                    fclose(fp);
+                break;
+#endif
+                }
+            }
+        }                        
+    }
+}
+
+
+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;
+    char *dev_dir_name, *buf_dir_name;
+
+    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;
+    /* -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:pret:")) != -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 '?':
+            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);
+    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("buffer/enable", dev_dir_name, 0);
+
+	ret = write_sysfs_int_and_verify("power_state", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("accl_enable", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, 1);
+/*
+	ret = write_sysfs_int_and_verify("zero_motion_on", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("zero_motion_dur", dev_dir_name, 12);
+	ret = write_sysfs_int_and_verify("zero_motion_threshold", dev_dir_name, 13);
+
+	ret = write_sysfs_int_and_verify("motion_on", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("motion_dur", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("motion_threshold", dev_dir_name, 1);
+*/
+	ret = write_sysfs_int_and_verify("accel_wom_on", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("accel_wom_threshold", dev_dir_name, 100);
+    /* 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);
+    /*
+     * Parse the files in scan_elements to identify what channels are
+     * present
+     */
+    ret = 0;
+    ret = enable(dev_dir_name, &infoarray, &num_channels);
+    if (ret) {
+        printf("error enable\n");
+        goto error_free_triggername;
+    }
+    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;
+    }
+    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;
+    ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 1);
+    ret = write_sysfs_int_and_verify("accl_enable", dev_dir_name, 1);
+    //ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, 0);
+    if (nodmp == 0) {
+        ret = write_sysfs_int_and_verify("quaternion_on", dev_dir_name, 1);
+    } else {
+        ret = disable_q_out(dev_dir_name, &infoarray, &num_channels);        
+        ret = write_sysfs_int_and_verify("dmp_on", dev_dir_name, 0);
+    }
+    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("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;
+    }
+
+    ret = asprintf(&buffer_access,
+               "/dev/iio:device%d",
+               dev_num);
+    if (ret < 0) {
+        ret = -ENOMEM;
+        goto error_free_data;
+    }
+    if (p_event) {
+        get_dmp_event(dev_dir_name);
+        goto error_free_buffer_access;
+    }
+    /* Attempt to open non blocking the access dev */
+    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 10 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 (0 == 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("enable", buf_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/60xx/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared b/60xx/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared
new file mode 100644
index 0000000..6dc08bd
--- /dev/null
+++ b/60xx/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared
diff --git a/60xx/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk b/60xx/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk
new file mode 100644
index 0000000..ed5fbf6
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/simple_apps/self_test/build/filelist.mk b/60xx/libsensors_iio/software/simple_apps/self_test/build/filelist.mk
new file mode 100644
index 0000000..492f47e
--- /dev/null
+++ b/60xx/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/60xx/libsensors_iio/software/simple_apps/self_test/inv_self_test.c b/60xx/libsensors_iio/software/simple_apps/self_test/inv_self_test.c
new file mode 100644
index 0000000..87ed703
--- /dev/null
+++ b/60xx/libsensors_iio/software/simple_apps/self_test/inv_self_test.c
@@ -0,0 +1,417 @@
+/**
+ *  Self Test application for Invensense's MPU6050/MPU6500/MPU9150.
+ */
+
+#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"
+
+#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*))
+
+/** Change this key if the data being stored by this file changes */
+#define INV_DB_SAVE_KEY 53395
+
+#define FALSE   0
+#define TRUE    1
+
+#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 *enable;
+    char *power_state;
+    char *dmp_on;
+    char *dmp_int_on;
+    char *self_test;
+    char *temperature;
+    char *gyro_enable;
+    char *gyro_x_bias;
+    char *gyro_y_bias;
+    char *gyro_z_bias;
+    char *accel_enable;
+    char *accel_x_bias;
+    char *accel_y_bias;
+    char *accel_z_bias;
+    char *compass_enable;
+} mpu;
+
+struct inv_db_save_t {
+    /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */
+    long compass_bias[3];
+    /** Gyro Bias in Chip Frame in Hardware units scaled by 2^16 */
+    long gyro_bias[3];
+    /** Temperature when *gyro_bias was stored. */
+    long gyro_temp;
+    /** Accel Bias in Chip Frame in Hardware units scaled by 2^16 */
+    long accel_bias[3];
+    /** Temperature when accel bias was stored. */
+    long accel_temp;
+    long gyro_temp_slope[3];
+    /** Sensor Accuracy */
+    int gyro_accuracy;
+    int accel_accuracy;
+    int compass_accuracy;
+};
+
+static struct inv_db_save_t save_data;
+
+/** 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 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;
+    }
+
+    // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
+    inv_get_sysfs_path(sysfs_path);
+
+    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_bias, "%s%s", sysfs_path, "/in_anglvel_x_calibbias");
+    sprintf(mpu.gyro_y_bias, "%s%s", sysfs_path, "/in_anglvel_y_calibbias");
+    sprintf(mpu.gyro_z_bias, "%s%s", sysfs_path, "/in_anglvel_z_calibbias");
+
+    sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable");
+    sprintf(mpu.accel_x_bias, "%s%s", sysfs_path, "/in_accel_x_calibbias");
+    sprintf(mpu.accel_y_bias, "%s%s", sysfs_path, "/in_accel_y_calibbias");
+    sprintf(mpu.accel_z_bias, "%s%s", sysfs_path, "/in_accel_z_calibbias");
+
+    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++) {
+        MPL_LOGE("inv_self_test: sysfs path: %s", *dptr++);
+    }
+#endif
+    return 0;
+}
+
+/*******************************************************************************
+ *                       M a i n  S e l f  T e s t 
+ ******************************************************************************/
+int main(int argc, char **argv)
+{
+    FILE *fptr;
+    int self_test_status = 0;
+    inv_error_t result;
+    bias_dtype gyro_bias[3];
+    bias_dtype accel_bias[3];
+    int axis = 0;
+    size_t packet_sz;
+    int axis_sign = 1;
+    unsigned char *buffer;
+    long timestamp;
+    int temperature = 0;
+    bool compass_present = TRUE;
+
+    result = inv_init_sysfs_attributes();
+    if (result)
+        return -1;
+
+    inv_init_storage_manager();
+
+    // Clear out data.
+    memset(&save_data, 0, sizeof(save_data));
+    memset(gyro_bias, 0, sizeof(gyro_bias));
+    memset(accel_bias, 0, sizeof(accel_bias));
+
+    // 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);
+
+    // Power ON MPUxxxx chip
+    if (write_sysfs_int(mpu.power_state, 1) < 0) {
+        printf("Self-Test:ERR-Failed to set power state=1\n");
+    } else {
+        // Note: Driver turns on power automatically when self-test invoked
+    }
+
+    // Disable Master enable 
+    if (write_sysfs_int(mpu.enable, 0) < 0) {
+        printf("Self-Test:ERR-Failed to disable master enable\n");
+    }
+
+    // Disable DMP
+    if (write_sysfs_int(mpu.dmp_on, 0) < 0) {
+        printf("Self-Test:ERR-Failed to disable DMP\n");
+    }
+
+    // Enable Accel
+    if (write_sysfs_int(mpu.accel_enable, 1) < 0) {
+        printf("Self-Test:ERR-Failed to enable accel\n");
+    }
+
+    // Enable Gyro
+    if (write_sysfs_int(mpu.gyro_enable, 1) < 0) {
+        printf("Self-Test:ERR-Failed to enable gyro\n");
+    }
+
+    // Enable Compass
+    if (write_sysfs_int(mpu.compass_enable, 1) < 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_bias, &gyro_bias[0].i) < 0 ||
+            read_sysfs_int(mpu.gyro_y_bias, &gyro_bias[1].i) < 0 ||
+            read_sysfs_int(mpu.gyro_z_bias, &gyro_bias[2].i) < 0) {
+            memset(gyro_bias, 0, sizeof(gyro_bias));
+            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_bias, &accel_bias[0].i) < 0 ||
+            read_sysfs_int(mpu.accel_y_bias, &accel_bias[1].i) < 0 ||
+            read_sysfs_int(mpu.accel_z_bias, &accel_bias[2].i) < 0) {
+            memset(accel_bias,0, sizeof(accel_bias));
+            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 free_sysfs_storage;
+    }
+
+    // Read temperature
+    fptr= fopen(mpu.temperature, "r");
+    if (fptr != NULL) {
+        fscanf(fptr,"%d %ld", &temperature, &timestamp);
+        fclose(fptr);
+    } else {
+        printf("Self-Test:ERR-Couldn't read temperature\n");
+    }
+
+    // When we read gyro bias, the bias is in raw units scaled by 1000.
+    // We store the bias in raw units scaled by 2^16
+    save_data.gyro_bias[0] = (long)(gyro_bias[0].l * 65536.f / 8000.f);
+    save_data.gyro_bias[1] = (long)(gyro_bias[1].l * 65536.f / 8000.f);
+    save_data.gyro_bias[2] = (long)(gyro_bias[2].l * 65536.f / 8000.f);
+
+    // Save temperature @ time stored.
+    //  Temperature is in degrees Celsius scaled by 2^16
+    save_data.gyro_temp = temperature * (1L << 16);
+    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
+    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;
+    }
+    
+    // Remove gravity, gravity in raw units should be 16384 for a 2g setting.
+    // We read data scaled by 1000, so 
+    accel_bias[axis].l -= axis_sign * 4096L * 1000L;
+
+    // Convert scaling from raw units scaled by 1000 to raw scaled by 2^16
+    save_data.accel_bias[0] = (long)(accel_bias[0].l * 65536.f / 1000.f * 4.f);
+    save_data.accel_bias[1] = (long)(accel_bias[1].l * 65536.f / 1000.f * 4.f);
+    save_data.accel_bias[2] = (long)(accel_bias[2].l * 65536.f / 1000.f * 4.f);
+
+#if 1
+    printf("Self-Test:Saved Accel bias[0..2]= [%ld %ld %ld]\n",
+           save_data.accel_bias[0], save_data.accel_bias[1], 
+           save_data.accel_bias[2]);
+    printf("Self-Test:Saved Gyro bias[0..2]= [%ld %ld %ld]\n",
+           save_data.gyro_bias[0], save_data.gyro_bias[1], 
+           save_data.gyro_bias[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);
+#endif
+
+    // 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);
+
+free_sysfs_storage:
+    free(sysfs_names_ptr);
+    return result;
+}
+