HACK: libsensors: Initial attempt at MotionApps 5.1 for MPU6050 + AKM8975

The code in this patch still needs work.
But checking it in will allow other parts of the sensor stack to have
something to work with.

1. Have sensor HAL for IIO driver, which uses MPU6050 + AKM8975 (on 2nd bus).
2. Include MPL binaries (libmllite.so/libmplmpu.so).
3. Include necessary header files.
4. remove light sensor dependency.
5. add missing include file.
6. modify the module name into "manta".
7. remove mlsdk directory.
8. Fix some known issues.
9. Sync up to June.12.
10. Tweak slightly so that it can be used with other sensors on manta and
  doesn't break other devices that need a non IIO libsensors.

Change-Id: I0913a6df56fb0e99e9bae9ecc40ab03884d68124
Signed-off-by: Mars Lee <mlee@invensense.com>

120 files changed, 19581 insertions, 0 deletions

diff --git a/Android.mk b/Android.mk
new file mode 100644
index 0000000..427bbb4
--- /dev/null
+++ b/Android.mk
@@ -0,0 +1,8 @@
+# Can't have both manta and non-manta libsensors.
+ifneq ($(filter manta, $(TARGET_DEVICE)),)
+# libsensors_iio expects IIO drivers for an MPU6050+AK8963 which are only available on manta.
+include $(call all-named-subdir-makefiles,libsensors_iio)
+else
+# libsensors expects an non-IIO MPU3050.
+include $(call all-named-subdir-makefiles,mlsdk libsensors)
+endif
diff --git a/libsensors_iio/Android.mk b/libsensors_iio/Android.mk
new file mode 100644
index 0000000..c3b2003
--- /dev/null
+++ b/libsensors_iio/Android.mk
@@ -0,0 +1,95 @@
+# 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_CFLAGS := -DLOG_TAG=\"Sensors\"
+
+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_YAS530),1)
+LOCAL_CFLAGS += -DCOMPASS_YAS530
+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_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_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/libsensors_iio/CompassSensor.IIO.9150.cpp b/libsensors_iio/CompassSensor.IIO.9150.cpp
new file mode 100644
index 0000000..ce0df34
--- /dev/null
+++ b/libsensors_iio/CompassSensor.IIO.9150.cpp
@@ -0,0 +1,421 @@
+/*
+ * 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_YAS530
+#   warning "Invensense compass cal with YAS530 on primary 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 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),
+                    mCompassTimestamp(0),
+                    mCompassInputReader(8),
+                    compass_fd(-1)
+{
+    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");
+    }
+
+    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 tempFd;
+    int res;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+            en, compassSysFs.compass_enable, getTimestamp());
+    tempFd = open(compassSysFs.compass_enable, O_RDWR);
+    res = errno;
+    if(tempFd < 0) {
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             compassSysFs.compass_enable, strerror(res), res);
+        return res;
+    }
+    res = enable_sysfs_sensor(tempFd, en);
+    LOGE_IF(res < 0, "HAL:enable compass failed");
+    close(tempFd);
+
+    if (en) {
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+                en, compassSysFs.compass_x_fifo_enable, getTimestamp());
+        tempFd = open(compassSysFs.compass_x_fifo_enable, O_RDWR);
+        res = errno;
+        if (tempFd > 0) {
+            res = enable_sysfs_sensor(tempFd, en);
+            close(tempFd);
+        } else {
+            LOGE("HAL:open of %s failed with '%s' (%d)",
+                 compassSysFs.compass_x_fifo_enable, strerror(res), res);
+        }
+
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+                en, compassSysFs.compass_y_fifo_enable, getTimestamp());
+        tempFd = open(compassSysFs.compass_y_fifo_enable, O_RDWR);
+        res = errno;
+        if (tempFd > 0) {
+            res = enable_sysfs_sensor(tempFd, en);
+            close(tempFd);
+        } else {
+            LOGE("HAL:open of %s failed with '%s' (%d)",
+                 compassSysFs.compass_y_fifo_enable, strerror(res), res);
+        }
+
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+                en, compassSysFs.compass_z_fifo_enable, getTimestamp());
+        tempFd = open(compassSysFs.compass_z_fifo_enable, O_RDWR);
+        res = errno;
+        if (tempFd > 0) {
+            res = enable_sysfs_sensor(tempFd, en);
+            close(tempFd);
+        } else {
+            LOGE("HAL:open of %s failed with '%s' (%d)",
+                 compassSysFs.compass_z_fifo_enable, strerror(res), res);
+        }
+    }
+
+    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;
+            list->resolution = COMPASS_MPU9150_RESOLUTION;
+            list->power = COMPASS_MPU9150_POWER;
+            list->minDelay = COMPASS_MPU9150_MINDELAY;
+            return;
+        }
+        if(!strcmp(compass, "compass")
+                || !strcmp(compass, "INV_AK8975")
+                || !strcmp(compass, "INV_YAS530")) {
+            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_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);
+    inv_get_sysfs_path(sysfs_path);
+    inv_get_iio_trigger_path(iio_trigger_path);
+
+#if defined COMPASS_YAS530 || 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);
+#if defined COMPASS_YAS530
+    strcat(sysfs_path, "/yas530");
+#else
+    strcat(sysfs_path, "/ak8975");
+#endif
+    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 0
+    // test print sysfs paths   
+    dptr = (char**)&compassSysFs;
+    for (i = 0; i < COMPASS_MAX_SYSFS_ATTRB; i++) {
+        LOGE("HAL:sysfs path: %s", *dptr++);
+    }
+#endif
+    return 0;
+}
diff --git a/libsensors_iio/CompassSensor.IIO.9150.h b/libsensors_iio/CompassSensor.IIO.9150.h
new file mode 100644
index 0000000..6a51338
--- /dev/null
+++ b/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/libsensors_iio/InputEventReader.cpp b/libsensors_iio/InputEventReader.cpp
new file mode 100644
index 0000000..ca0a61a
--- /dev/null
+++ b/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/libsensors_iio/InputEventReader.h b/libsensors_iio/InputEventReader.h
new file mode 100644
index 0000000..11c4e73
--- /dev/null
+++ b/libsensors_iio/InputEventReader.h
@@ -0,0 +1,47 @@
+/*

+* 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>
+
+/*****************************************************************************/
+
+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/libsensors_iio/License.txt b/libsensors_iio/License.txt
new file mode 100644
index 0000000..930f931
--- /dev/null
+++ b/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/libsensors_iio/MPLSensor.cpp b/libsensors_iio/MPLSensor.cpp
new file mode 100644
index 0000000..ae82459
--- /dev/null
+++ b/libsensors_iio/MPLSensor.cpp
@@ -0,0 +1,2555 @@
+/*
+* 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 "MPLSensor.h"
+#include "MPLSupport.h"
+#include "sensor_params.h"
+
+#include "invensense.h"
+#include "invensense_adv.h"
+#include "ml_stored_data.h"
+#include "ml_load_dmp.h"
+#include "ml_sysfs_helper.h"
+
+// #define TESTING
+// #define ENABLE_LP_QUAT_FEAT /* Uncomment to enable Low Power Quaternion */
+
+#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)
+
+static struct sensor_t sSensorList[] =
+{
+    {"MPL Gyroscope", "Invensense", 1,
+     SENSORS_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, {}},
+};
+
+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
+
+/*******************************************************************************
+ * MPLSensor class implementation
+ ******************************************************************************/
+
+MPLSensor::MPLSensor(CompassSensor *compass)
+                       : SensorBase(NULL, NULL),
+                         mNewData(0),
+                         mMasterSensorMask(INV_ALL_SENSORS),
+                         mLocalSensorMask(ALL_MPL_SENSORS_NP),
+                         mPollTime(-1),
+                         mHaveGoodMpuCal(0),
+                         mGyroAccuracy(0),
+                         mAccelAccuracy(0),
+                         mSampleCount(0),
+                         mEnabled(0),
+                         mOldEnabledMask(0),
+                         mAccelInputReader(4),
+                         mGyroInputReader(32),
+                         mTempScale(0),
+                         mTempOffset(0),
+                         mTempCurrentTime(0),
+                         mAccelScale(2),
+                         mPendingMask(0),
+                         mSensorMask(0),
+                         mGyroOrientation{0},
+                         mAccelOrientation{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);
+
+#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);
+
+    /* Load DMP image if capable, ie. MPU6xxx/9xxx */
+    // TODO: disabled for GED tablet
+#ifdef ENABLE_LP_QUAT_FEAT
+    loadDMP();	
+#endif
+
+    /* 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 == 0) {
+        char buf[3];
+        int count = 0;
+        LOGV_IF(SYSFS_VERBOSE, 
+                "HAL:sysfs:cat %s (%lld)", mpu.accel_fsr, getTimestamp());
+
+        fd = open(mpu.accel_fsr, O_RDONLY);
+        if(fd < 0) {
+            LOGE("HAL:Error opening accel FSR");
+        } else {
+           memset(buf, 0, sizeof(buf));
+           count = read_attribute_sensor(fd, buf, sizeof(buf));
+           if(count < 1) {
+               LOGE("HAL:Error reading accel FSR");
+           } else {
+               count = sscanf(buf, "%d", &mAccelScale);
+               if(count)
+                   LOGV_IF(EXTRA_VERBOSE, "HAL:Accel FSR used %d", mAccelScale);
+           }
+           close(fd);
+        }
+    }
+
+    /* initialize sensor data */
+    memset(mPendingEvents, 0, sizeof(mPendingEvents));
+
+    mPendingEvents[RotationVector].version = sizeof(sensors_event_t);
+    mPendingEvents[RotationVector].sensor = ID_RV;
+    mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR;
+    mPendingEvents[RotationVector].acceleration.status
+            = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[LinearAccel].version = sizeof(sensors_event_t);
+    mPendingEvents[LinearAccel].sensor = ID_LA;
+    mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
+    mPendingEvents[LinearAccel].acceleration.status
+            = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[Gravity].version = sizeof(sensors_event_t);
+    mPendingEvents[Gravity].sensor = ID_GR;
+    mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
+    mPendingEvents[Gravity].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[Gyro].version = sizeof(sensors_event_t);
+    mPendingEvents[Gyro].sensor = ID_GY;
+    mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
+    mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
+    mPendingEvents[Accelerometer].sensor = ID_A;
+    mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
+    mPendingEvents[Accelerometer].acceleration.status
+            = SENSOR_STATUS_ACCURACY_HIGH;
+
+    /* Invensense compass calibration */
+    mPendingEvents[MagneticField].version = sizeof(sensors_event_t);
+    mPendingEvents[MagneticField].sensor = ID_M;
+    mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD;
+    mPendingEvents[MagneticField].magnetic.status =
+        SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[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[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/cal.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);
+
+    inv_set_device_properties();
+
+    /* disable driver master enable the first sensor goes on */
+    masterEnable(0);
+    enableGyro(0);
+    enableAccel(0);
+    enableCompass(0);
+    onPower(0);
+
+#ifdef INV_PLAYBACK_DBG
+    logfile = fopen("/data/playback.bin", "wb");
+    if (logfile)
+        inv_turn_on_data_logging(logfile);
+#endif
+}
+
+
+void MPLSensor::enable_iio_sysfs() {
+    VFUNC_LOG;
+
+    int tempFd = 0;
+    char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN];
+    FILE *tempFp = NULL;
+
+    LOGV_IF(SYSFS_VERBOSE, 
+            "HAL:sysfs:echo 1 > %s (%lld)", 
+            mpu.in_timestamp_en, getTimestamp());
+    tempFd = open(mpu.in_timestamp_en, O_RDWR);
+    if(tempFd < 0) {
+        LOGE("HAL:could not open timestamp enable");
+    } else if(enable_sysfs_sensor(tempFd, 1) < 0) {
+        LOGE("HAL:could not enable timestamp");
+    }
+
+    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) {
+        LOGE("HAL:could not write current trigger");
+    }
+    fclose(tempFp);
+
+    LOGV_IF(SYSFS_VERBOSE, 
+            "HAL:sysfs:echo %d > %s (%lld)", 
+            IIO_BUFFER_LENGTH, mpu.buffer_length, getTimestamp());
+    tempFp = fopen(mpu.buffer_length, "w");
+    if (tempFp == NULL) {
+        LOGE("HAL:could not open buffer length");
+    } else if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0) {
+        LOGE("HAL:could not write buffer length");
+    }
+    fclose(tempFp);
+
+    inv_get_iio_device_node(iio_device_node);
+    iio_fd = open(iio_device_node, O_RDONLY);
+    if (iio_fd < 0) {
+        LOGE("HAL:could not open iio device node");
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:iio iio_fd opened : %d", 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;
+    }
+
+    // TODO: double-check for GED tablet
+    // 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;
+        }
+
+        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;
+    }
+
+    result = inv_enable_no_gyro_fusion();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    // TODO: double-check for GED tablet
+    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);
+    // BMA250
+    //inv_set_accel_orientation_and_scale(orient, 1LL << 22);
+    // MPU6050
+    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 (!strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050")) {
+        //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");
+    } else {
+        if(inv_read_dmp_state(fd) == 0) {
+            LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware);
+            fptr = fopen(mpu.dmp_firmware, "w");
+            if(!fptr) {
+                LOGE("HAL:could not write to dmp");
+            } else {
+                int res = inv_load_dmp(fptr);
+                if(res < 0) {
+                    LOGE("HAL:load DMP failed");
+                } else {
+                    LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded");
+                }
+                fclose(fptr);
+            }
+        } else {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is already loaded");
+        }
+    }
+
+    // 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;
+
+    /* 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);
+
+    /* Turn off Gyro master enable          */
+    /* A workaround until driver handles it */
+    /* TODO: Turn off and close all sensors */
+    LOGV_IF(SYSFS_VERBOSE,
+            "HAL:sysfs:echo 0 > %s (%lld)", mpu.chip_enable, getTimestamp());
+    int fd = open(mpu.chip_enable, O_RDWR);
+    if(fd < 0) {
+        LOGE("HAL:could not open gyro chip enable");
+    } else {
+        if(enable_sysfs_sensor(fd, 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) & enabled_sensors) // ID_GY = 0
+#define A_ENABLED  ((1 << ID_A)  & enabled_sensors) // ID_A  = 1
+#ifdef INVENSENSE_COMPASS_CAL                       // ID_M  = 2
+#define M_ENABLED  ((1 << ID_M)  & enabled_sensors)
+#else
+// TODO: ID_M = 2 even for 3rd-party solution
+#define M_ENABLED  ((1 << ID_M)  & enabled_sensors)
+#endif
+#define O_ENABLED  ((1 << ID_O)  & enabled_sensors) // ID_O  = 3
+#define LA_ENABLED ((1 << ID_LA) & enabled_sensors) // ID_RV = 4
+#define GR_ENABLED ((1 << ID_GR) & enabled_sensors) // ID_LA = 5
+#define RV_ENABLED ((1 << ID_RV) & enabled_sensors) // ID_GR = 6
+
+/* 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 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 = 0;
+    char buf[sizeof(int)+1];
+    int count, curr_power_state;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+            en, mpu.power_state, getTimestamp());
+    int tempFd = open(mpu.power_state, O_RDWR);
+    res = errno;
+    if(tempFd < 0){
+        LOGE("HAL:Open of %s failed with '%s' (%d)",
+             mpu.power_state, strerror(res), res);
+    } else {
+        // check and set new power state
+        count = read_attribute_sensor(tempFd, buf, sizeof(buf));
+        if(count < 1) {
+            LOGE("HAL:Error reading power state");
+            // will set power_state anyway
+            curr_power_state= -1;
+        } else {
+            sscanf(buf, "%d", &curr_power_state);
+        }
+       
+        if (en!=curr_power_state) {    
+            if((res=enable_sysfs_sensor(tempFd, 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);
+            close(tempFd);   
+        }
+    }
+    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
+
+    if(read_sysfs_int(mpu.firmware_loaded, &status) < 0){
+        LOGE("HAL:ERR can't get firmware_loaded status");
+    } else if (status == 1) {
+        //Write only if curr DMP state <> request
+        if (read_sysfs_int(mpu.dmp_on, &status) < 0) {
+            LOGE("HAL:ERR can't read DMP state");
+        } else if (status != en) {
+            if (write_sysfs_int(mpu.dmp_on, en) < 0) {
+                LOGE("HAL:ERR can't write dmp_on");  
+            } else {
+                res= 0;	//Indicate write successful
+            }
+            //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
+    if (write_sysfs_int(mpu.quaternion_on, en) < 0) {
+        LOGE("HAL:ERR can't write DMP quaternion_on");
+        res= -1;	//Indicate an err
+    } 
+
+    if (!en) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off");
+        inv_quaternion_sensor_was_turned_off();
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling quat scan elems");
+        if (write_sysfs_int(mpu.in_quat_r_en, 1) < 0) {
+            LOGE("HAL:ERR write in_quat_r_en");
+        }
+        if (write_sysfs_int(mpu.in_quat_x_en, 1) < 0) {
+            LOGE("HAL:ERR write in_quat_x_en");
+        }
+        if (write_sysfs_int(mpu.in_quat_y_en, 1) < 0) {
+            LOGE("HAL:ERR write in_quat_y_en");
+        }
+        if (write_sysfs_int(mpu.in_quat_z_en, 1) < 0) {
+            LOGE("HAL:ERR write in_quat_z_en"); 
+        }
+    }
+
+    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;
+
+    int res = 0;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+            en, mpu.chip_enable, getTimestamp());
+    int tempFd = open(mpu.chip_enable, O_RDWR);
+    res = errno;
+    if(tempFd < 0){
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             mpu.chip_enable, strerror(res), res);
+        return res;
+    }
+    res = enable_sysfs_sensor(tempFd, en);
+    return res;
+}
+
+int MPLSensor::enableGyro(int en)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    /* need to also turn on/off the master enable */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+            en, mpu.gyro_enable, getTimestamp());
+    int tempFd = open(mpu.gyro_enable, O_RDWR);
+    res = errno;
+    if (tempFd > 0) {
+        res = enable_sysfs_sensor(tempFd, en);
+    } else {
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             mpu.gyro_enable, strerror(res), res);
+    }
+
+    if (!en) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off");
+        inv_gyro_was_turned_off();
+    } else {
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+                en, mpu.gyro_x_fifo_enable, getTimestamp());
+        tempFd = open(mpu.gyro_x_fifo_enable, O_RDWR);
+        res = errno;
+        if (tempFd > 0) {
+            res = enable_sysfs_sensor(tempFd, en);
+        } else {
+            LOGE("HAL:open of %s failed with '%s' (%d)",
+                 mpu.gyro_x_fifo_enable, strerror(res), res);
+        }
+
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+                en, mpu.gyro_y_fifo_enable, getTimestamp());
+        tempFd = open(mpu.gyro_y_fifo_enable, O_RDWR);
+        res = errno;
+        if (tempFd > 0) {
+            res = enable_sysfs_sensor(tempFd, en);
+        } else {
+            LOGE("HAL:open of %s failed with '%s' (%d)",
+                 mpu.gyro_y_fifo_enable, strerror(res), res);
+        }
+
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+                en, mpu.gyro_z_fifo_enable, getTimestamp());
+        tempFd = open(mpu.gyro_z_fifo_enable, O_RDWR);
+        res = errno;
+        if (tempFd > 0) {
+            res = enable_sysfs_sensor(tempFd, en);
+        } else {
+            LOGE("HAL:open of %s failed with '%s' (%d)",
+                 mpu.gyro_z_fifo_enable, strerror(res), res);
+        }
+    }
+
+    return res;
+}
+
+int MPLSensor::enableAccel(int en)
+{
+    VFUNC_LOG;
+
+    int res;
+
+   /* need to also turn on/off the master enable */
+   LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+           en, mpu.accel_enable, getTimestamp());
+   int tempFd = open(mpu.accel_enable, O_RDWR);
+   res = errno;
+   if (tempFd > 0) {
+       res = enable_sysfs_sensor(tempFd, en);
+   } else {
+       LOGE("HAL:open of %s failed with '%s' (%d)",
+            mpu.accel_enable, strerror(res), res);
+   }
+
+    if (!en) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off");
+        inv_accel_was_turned_off();
+    } else {
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+                en, mpu.accel_x_fifo_enable, getTimestamp());
+        tempFd = open(mpu.accel_x_fifo_enable, O_RDWR);
+        res = errno;
+        if (tempFd > 0) {
+            res = enable_sysfs_sensor(tempFd, en);
+        } else {
+            LOGE("HAL:open of %s failed with '%s' (%d)",
+                 mpu.accel_x_fifo_enable, strerror(res), res);
+        }
+
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+                en, mpu.accel_y_fifo_enable, getTimestamp());
+        tempFd = open(mpu.accel_y_fifo_enable, O_RDWR);
+        res = errno;
+        if (tempFd > 0) {
+            res = enable_sysfs_sensor(tempFd, en);
+        } else {
+            LOGE("HAL:open of %s failed with '%s' (%d)",
+                 mpu.accel_y_fifo_enable, strerror(res), res);
+        }
+
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+                en, mpu.accel_z_fifo_enable, getTimestamp());
+        tempFd = open(mpu.accel_z_fifo_enable, O_RDWR);
+        res = errno;
+        if (tempFd > 0) {
+            res = enable_sysfs_sensor(tempFd, en);
+        } else {
+            LOGE("HAL:open of %s failed with '%s' (%d)",
+                 mpu.accel_z_fifo_enable, strerror(res), res);
+        }
+    }
+
+    if (!en && USE_THIRD_PARTY_ACCEL == 0) {
+    } 
+
+    if(USE_THIRD_PARTY_ACCEL == 1 && en) {
+        setAccelInitialState(); // BMA250
+    }
+    return res;
+}
+
+int MPLSensor::enableCompass(int en)
+{
+    VFUNC_LOG;
+
+    int res = mCompassSensor->enable(ID_M, en);
+    if (en == 0) {
+        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::enableSensors(unsigned long sensors, int en, uint32_t changed) {
+    VFUNC_LOG;
+
+    inv_error_t res = -1;
+    int on = 1;
+    int off = 0;
+
+    // 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)
+
+    if (changed & ((1 << Gyro) | (1 << Accelerometer) |
+            (mCompassSensor->isIntegrated() << MagneticField))) {
+        /* ensure power state is on */
+        onPower(1);
+      
+        /* reset master enable */
+        res = masterEnable(0);
+        if(res < 0) {
+            return res;
+        }
+    }
+
+    LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", (unsigned int)sensors);
+
+    if (changed & (1 << Gyro)) {
+        if(sensors & INV_THREE_AXIS_GYRO) {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable gyro");
+            res = enableGyro(on);
+            if(res < 0) {
+                return res;
+            }
+        } else if((sensors & INV_THREE_AXIS_GYRO) == 0) {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable gyro");
+            res = enableGyro(off);
+            if(res < 0) {
+                return res;
+            }
+        }
+    }
+
+    if (changed & (1 << Accelerometer)) {
+        if(sensors & INV_THREE_AXIS_ACCEL) {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable accel");
+            res = enableAccel(on);
+            if(res < 0) {
+                return res;
+            }
+        } else if((sensors & INV_THREE_AXIS_ACCEL) == 0) {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable accel");
+            res = enableAccel(off);
+            if(res < 0) {
+                return res;
+            }
+        }
+    }
+
+    if (changed & (1 << MagneticField)) {
+        /* Invensense compass calibration */
+        if (sensors & INV_THREE_AXIS_COMPASS) {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable compass");
+            res = enableCompass(on);
+            if(res < 0) {
+                return res;
+            }
+        } else if ((sensors & INV_THREE_AXIS_COMPASS) == 0) {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable compass");
+            res = enableCompass(off);
+            if(res < 0) {
+                return res;
+            }
+        }
+    }
+
+// TODO: disabled for GED tablet
+#ifdef ENABLE_LP_QUAT_FEAT
+    // Enable LP Quat
+    if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) |
+            (1 << LinearAccel) | (1 << Gravity)))) {
+        if (!checkLPQuaternion()) {
+            enableLPQuaternion(1);
+        } else {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled");
+        }
+    } else if (checkLPQuaternion()) {
+        enableLPQuaternion(0);
+    }
+#endif
+
+    /*
+        if sensor & THREE_AXIS_GYRO
+            enable = 1
+        if sensor & THREE_AXIS_ACCEL
+            enable = 1
+        if compass_on_secondary
+            if sensor & THREE_AXIS_COMPASS
+                enable = 1
+        else
+            enable = 0
+    */
+    if (changed & ((1 << Gyro) | (1 << Accelerometer) |
+            (mCompassSensor->isIntegrated() << MagneticField))) {
+        if (sensors & 
+            (INV_THREE_AXIS_GYRO 
+                | INV_THREE_AXIS_ACCEL 
+                | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
+            res = masterEnable(1);
+            if(res < 0) {
+                return res;
+            }
+        } else { // all sensors idle -> reduce power
+            res = onPower(0);
+            if(res < 0) {
+                return res;
+            }
+            storeCalibration();
+        }
+    }
+
+    return res;
+}
+
+/* Store calibration file */
+void MPLSensor::storeCalibration()
+{
+    if(mHaveGoodMpuCal == true || mAccelAccuracy >= 2) {
+       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;
+    int update;
+    update = inv_get_sensor_type_gyroscope(s->gyro.v, &s->gyro.status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:gyro data : %+f %+f %+f -- %lld - %d",
+            s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::accelHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_accelerometer(
+        s->acceleration.v, &s->acceleration.status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d",
+            s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2], 
+            s->timestamp, update);
+    mAccelAccuracy = s->acceleration.status;
+    return update;
+}
+
+int MPLSensor::compassHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_magnetic_field(
+        s->magnetic.v, &s->magnetic.status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d",
+            s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2], s->timestamp, update);
+    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;
+    int update;
+    update = inv_get_sensor_type_linear_acceleration(
+            s->gyro.v, &s->gyro.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;
+    int update;
+    update = inv_get_sensor_type_gravity(s->gyro.v, &s->gyro.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;
+
+    switch (handle) {
+    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_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;
+    int err = 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);
+
+    // TODO: disabled for GED tablet
+    // 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 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_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_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;
+    }
+
+// TODO: disabled for GED tablet
+#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);
+
+    // TODO: for GED tablet
+    // 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 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 << 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:
+            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;
+    }
+
+    // TODO: disabled for GED tablet
+    // 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;
+
+    if (mEnabled) {
+        uint64_t wanted = -1LLU;
+        uint64_t wanted_ec = -1LLU;
+
+        // Sequence to change sensor's FIFO rate 
+        // 1. enable Power state 
+        // 2. reset master enable
+        // 3. Update delay
+        // 4. set master enable
+
+        // TODO: unnecessary for IIO
+        // ensure power on
+        // onPower(1);
+
+        // TODO: unnecessary for IIO
+        // 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)) {
+                uint64_t ns = mDelays[i];
+                wanted = wanted < ns ? wanted : ns;
+            }
+        }
+
+	      // same delay for ext compass
+        wanted_ec = 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: disabled for GED tablet
+#ifdef ENABLE_LP_QUAT_FEAT
+        // Set LP Quaternion sample rate if enabled
+        if (checkLPQuaternion()) {
+            // TODO: need to verify this part for LPQ
+            if (wanted < 5000000LL) {
+                enableLPQuaternion(0);
+            } else {
+                inv_set_quat_sample_rate(rateInus);
+                // set DMP output rate to FIFO 
+                write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / wanted);
+
+                //DMP running rate must be @ 200Hz 
+                wanted= 5000000LL;	
+                LOGV("HAL:DMP rate= %.2f Hz Fifo Rate= %d us", 1000000000.f / wanted, rateInus);
+            }
+        }
+#endif
+
+        /* 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) {
+            uint64_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 == 1) {
+                LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)",
+                        wanted / 1000000L, mpu.accel_fifo_rate, 
+                        getTimestamp());
+                tempFd = open(mpu.accel_fifo_rate, O_RDWR);
+                res = write_attribute_sensor(tempFd, wanted / 1000000L);
+                LOGE_IF(res < 0, "HAL:ACCEL update delay error");
+            }
+
+            if (!mCompassSensor->isIntegrated()) {
+                mCompassSensor->setDelay(ID_M, wanted_ec);
+                got = mCompassSensor->getDelay(ID_M);
+                inv_set_compass_sample_rate(got / 1000);
+            }
+
+            /*
+            //nsTons - nothing to be done
+            strcpy(&compass_sensor_sysfs_path[compass_sensor_sysfs_path_len],
+                   COMPASS_SENSOR_DELAY);
+            tempFd = open(compass_sensor_sysfs_path, O_RDWR);
+            LOGV_IF(PROCESS_VERBOSE, "setDelay - open path: %s", compass_sensor_sysfs_path);
+            wanted = 20000000LLU;
+            res = write_attribute_sensor(tempFd, wanted);
+            if(res < 0) {
+                LOGE("Compass update delay error");
+            }
+            */
+
+        } else {
+
+            if (GY_ENABLED) {
+                wanted = mDelays[Gyro];
+                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 {
+                    wanted = mDelays[Accelerometer];
+                }
+                /* 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);
+                //BMA250 in ms
+                //res = write_attribute_sensor(tempFd, wanted / 1000000L);
+                //MPU6050 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[Accelerometer] < mDelays[MagneticField]) {
+                        wanted = mDelays[Accelerometer];
+                    } else {
+                        wanted = mDelays[MagneticField];
+                    }
+                }
+                mCompassSensor->setDelay(ID_M, wanted);
+                got = mCompassSensor->getDelay(ID_M);
+                inv_set_compass_sample_rate(got / 1000);
+            }
+
+        }
+
+        /*
+            if sensor & THREE_AXIS_GYRO
+                enable = 1
+            if sensor & THREE_AXIS_ACCEL
+                enable = 1
+            if compass_on_secondary
+                if sensor & THREE_AXIS_COMPASS
+                    enable = 1
+            else
+                enable = 0
+        */
+        unsigned long sensors = mLocalSensorMask & mMasterSensorMask;
+        if (sensors & 
+            (INV_THREE_AXIS_GYRO 
+                | INV_THREE_AXIS_ACCEL 
+                | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
+// TODO: unnecessary for IIO
+#if 0
+            res = masterEnable(1);
+            if(res < 0) {
+                return res;
+            }
+#endif
+        } else { // all sensors idle -> reduce power
+// TODO: unnecessary for IIO
+#if 0
+            res = onPower(0);
+            if(res < 0) {
+                return res;
+            }
+#endif
+        }
+    }
+
+    return res;
+}
+
+/* use for third party accel */
+int MPLSensor::readAccelEvents(sensors_event_t* data, int count)
+{
+    VHANDLER_LOG;
+
+    if (count < 1)
+        return -EINVAL;
+
+    ssize_t n = mAccelInputReader.fill(accel_fd);
+    if (n < 0) {
+        LOGE("HAL:missed accel events, exit");
+        return n;
+    }
+
+    int numEventReceived = 0;
+    input_event const* event;
+    int nb, done = 0;
+
+    while (done == 0 && count && mAccelInputReader.readEvent(&event)) {
+        int type = event->type;
+        if (type == EV_ABS) {
+            if (event->code == EVENT_TYPE_ACCEL_X) {
+                mPendingMask |= 1 << Accelerometer;
+                mCachedAccelData[0] = event->value;
+            } else if (event->code == EVENT_TYPE_ACCEL_Y) {
+                mPendingMask |= 1 << Accelerometer;
+                mCachedAccelData[1] = event->value;
+            } else if (event->code == EVENT_TYPE_ACCEL_Z) {
+                mPendingMask |= 1 << Accelerometer;
+                mCachedAccelData[2] =event-> value;
+            }
+        } else if (type == EV_SYN) {
+            done = 1;
+            if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
+                inv_build_accel(mCachedAccelData, 0, getTimestamp());
+            }
+        } else {
+            LOGE("HAL:AccelSensor: unknown event (type=%d, code=%d)",
+                    type, event->code);
+        }
+        mAccelInputReader.next();
+    }
+
+    LOGV_IF(ENG_VERBOSE, "HAL:readAccelEvents - events read=%d", numEventReceived);
+
+    return numEventReceived;
+}
+
+/** 
+ *  Should be called after reading at least one of gyro
+ *  compass or accel data. You should only read 1 sample of
+ *  data and call this.
+ *  @returns 0, if successful, error number if not.
+ */
+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;
+}
+
+int MPLSensor::readEvents(sensors_event_t *data, int count) {
+
+
+    int lp_quaternion_on, 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;
+
+// TODO: disabled for GED tablet
+#ifdef ENABLE_LP_QUAT_FEAT
+    lp_quaternion_on = checkLPQuaternion();
+    if (lp_quaternion_on == 1) {
+        nbyte += sizeof(mCachedQuaternionData);		//currently 16 bytes for Q data
+    }
+#endif
+
+    // TODO: disabled for GED tablet
+    // pthread_mutex_lock(&mMplMutex);
+    // pthread_mutex_lock(&mHALMutex);
+
+    size_t rsize = read(iio_fd, rdata, nbyte);
+    if (sensors == 0) {
+        // read(iio_fd, rdata, nbyte);
+        read(iio_fd, rdata, (16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH);
+    }
+/*
+    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)));
+*/
+
+    // TODO: need to verify this for LPQ
+    if (rsize < (nbyte - 8)) {
+        LOGE("HAL:ERR Full data packet was not read");
+        // return -1;
+    }
+
+// TODO: disabled for GED tablet
+#ifdef ENABLE_LP_QUAT_FEAT
+    if (lp_quaternion_on == 1) {
+        for (i=0; i< 4; i++) {
+            mCachedQuaternionData[i]= *(long*)rdata;
+            rdata += sizeof(long);
+        }
+    }
+/*
+    LOGV("HAL:rdata= %x sensors= %d  lp_q_on= %d nbyte= %d rsize= %d",
+        rdata, sensors, lp_quaternion_on, nbyte, rsize);  //tbd
+*/
+#endif
+
+    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: 0) +
+        ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 2: 0));
+    if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated() &&
+            (mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0 || mCachedCompassData[0] != 0)) {
+        mask |= 4;
+    }
+
+    mSensorTimestamp = *((long long *) (rdata + 8 * sensors));
+    if (mCompassSensor->isIntegrated()) {
+        mCompassTimestamp = mSensorTimestamp;
+    }
+
+    // send down temperature every 0.5 seconds
+    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
+    }
+
+    if (mask & 1) {
+        mPendingMask |= 1 << Gyro;
+        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 & 2) {
+        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 & 4) && 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);
+        }
+    }
+
+// TODO: disabled for GED tablet
+#ifdef ENABLE_LP_QUAT_FEAT
+    if (lp_quaternion_on == 1) {
+        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 - %d",
+                    mCachedQuaternionData[0], mCachedQuaternionData[1], 
+                    mCachedQuaternionData[2], mCachedQuaternionData[3], mSensorTimestamp);
+    }
+#endif
+
+    // 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;
+
+    // TODO: disabled for GED tablet
+    // TODO: for AMI306
+    // pthread_mutex_lock(&mMplMutex);
+    // pthread_mutex_lock(&mHALMutex);
+
+    done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp);
+    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;
+}
+
+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::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)(7 * sizeof(sensor_t))) {
+        LOGE("HAL:sensor list too small, not populating.");
+        return 0;
+    }
+
+    /* fill in the base values */
+    memcpy(list, sSensorList, sizeof (struct sensor_t) * 7);
+
+    /* 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 = 7;
+        /* 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;
+
+            // TODO: for GED tablet
+            // list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY;
+            list[Accelerometer].minDelay = 5000;
+
+            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;
+
+        // TODO: for GED tablet
+        // list[Gyro].minDelay = GYRO_MPU6050_MINDELAY;
+        list[Gyro].minDelay = 5000;
+    } 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;
+    }
+    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;
+
+    // TODO: for GED tablet
+    // list[RotationVector].minDelay = 10000;
+    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;
+
+    // TODO: for GED tablet
+    // list[Orientation].minDelay = 10000;
+    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;
+
+    // TODO: for GED tablet
+    // list[Gravity].minDelay = 10000;
+    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;
+
+    // TODO: for GED tablet
+    // list[LinearAccel].minDelay = 10000;
+    list[LinearAccel].minDelay = 5000;
+
+    return;
+}
+
+int MPLSensor::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;
+
+    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 {
+        LOGE("HAL:couldn't alloc mem for sysfs paths");
+        return -1;
+    }
+
+    // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
+    // inv_get_sysfs_abs_path(sysfs_path);
+    inv_get_sysfs_path(sysfs_path);
+    inv_get_iio_trigger_path(iio_trigger_path);
+
+    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_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");
+
+#ifdef THIRD_PARTY_ACCEL    //BMA250
+    /* same as 'mpu.accel_enable' */
+    sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/enable");
+    sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/fifo_rate");
+    sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/NA");
+    sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/NA");
+    sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/NA");   
+#else
+    sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable");
+    sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
+    sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale");
+    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");
+
+    // TODO: for bias settings
+    sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/accl_bias");
+
+    sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix");
+#endif
+
+    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");
+
+#if 0
+    // test print sysfs paths
+    dptr = (char**)&mpu;
+    for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
+        LOGE("HAL:sysfs path: %s", *dptr++);
+    }
+#endif
+    return 0;
+}
diff --git a/libsensors_iio/MPLSensor.h b/libsensors_iio/MPLSensor.h
new file mode 100644
index 0000000..4c38c57
--- /dev/null
+++ b/libsensors_iio/MPLSensor.h
@@ -0,0 +1,282 @@
+/*

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

+

+// TODO: change to wanted vendor

+#ifdef INVENSENSE_COMPASS_CAL

+

+#ifdef COMPASS_AMI306

+#warning "unified HAL for AMI306"

+#include "CompassSensor.IIO.AMI.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 IIO_BUFFER_LENGTH           (100 * 2)

+

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

+// TODO: ID_M = 2 even for 3rd-party solution

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

+

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

+/** MPLSensor implementation which fits into the HAL example for crespo provided

+ *  by Google.

+ *  WARNING: there may only be one instance of MPLSensor, ever.

+ */

+ 

+class MPLSensor: public SensorBase

+{

+    typedef int (MPLSensor::*hfunc_t)(sensors_event_t*);

+

+public:

+    MPLSensor(CompassSensor *);

+    virtual ~MPLSensor();

+

+    enum

+    {

+        Gyro = 0,

+        Accelerometer,

+        MagneticField,

+        Orientation,

+        RotationVector,

+        LinearAccel,

+        Gravity,

+        numSensors

+    };

+

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

+

+protected:

+    CompassSensor *mCompassSensor;

+

+    int gyroHandler(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 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 update_delay_sysfs_sensor(int fd, uint64_t ns);

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

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

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

+

+    uint32_t mEnabled;

+    uint32_t mOldEnabledMask;

+    sensors_event_t mPendingEvents[numSensors];

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

+       char *key;

+       char *self_test;

+       char *temperature;

+       char *dmp_output_rate;

+

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

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

+};

+

+extern "C" {

+    void setCallbackObject(MPLSensor*);

+    MPLSensor *getCallbackObject();

+}

+

+#endif  // ANDROID_MPL_SENSOR_H

diff --git a/libsensors_iio/MPLSupport.cpp b/libsensors_iio/MPLSupport.cpp
new file mode 100644
index 0000000..a961d78
--- /dev/null
+++ b/libsensors_iio/MPLSupport.cpp
@@ -0,0 +1,144 @@
+#include <MPLSupport.h>
+#include <string.h>
+#include <stdio.h>
+#include "log.h"
+#include "SensorBase.h"
+#include <fcntl.h>
+
+int inv_read_data(char *fname, long *data)
+{
+    VFUNC_LOG;
+
+    char buf[sizeof(long) * 4];
+    int count, fd;
+
+    fd = open(fname, O_RDONLY);
+    if(fd < 0) {
+        LOGE("HAL:Error opening %s", fname);
+        return -1;
+    }
+    memset(buf, 0, sizeof(buf));
+    count = read_attribute_sensor(fd, buf, sizeof(buf));
+    if(count < 1) {
+        close(fd);
+        return -1;
+    } else {
+        count = sscanf(buf, "%ld", data);
+        if(count)
+            LOGV_IF(EXTRA_VERBOSE, "HAL:Data= %ld", *data);
+    }
+    close(fd);
+
+    return 0;
+}
+
+/* This one DOES NOT close FDs for you */
+int read_attribute_sensor(int fd, char* data, unsigned int size)
+{
+    VFUNC_LOG;
+
+    int count = 0;
+    if (fd > 0) {
+        count = pread(fd, data, size, 0);
+        if(count < 1) {
+            LOGE("HAL:read fails with error code=%d", count);
+        }
+    }
+    return count;
+}
+
+/**
+ *  @brief  Enable a sensor through the sysfs file descriptor
+ *          provided.
+ *  @note   this function one closes FD after the write
+ *  @param  fd
+ *              the file descriptor to write into
+ *  @param  en
+ *              the value to write, typically 1 or 0
+ *  @return the errno whenever applicable.
+ */
+int enable_sysfs_sensor(int fd, int en)
+{
+    VFUNC_LOG;
+
+    int nb = -1;
+    int err = 0;
+
+    if (fd >= 0) {
+        char buf[2];
+        if (en) {
+            buf[0] = '1';
+            nb = write(fd, buf, 1);
+        } else {
+            buf[0] = '0';
+            nb = write(fd, buf, 1);
+        }
+        buf[1] = '\0';
+
+        if (nb <= 0) {
+            err = errno;
+            LOGE("HAL:enable_sysfs_sensor - write %c returned %d (%s / %d)",
+                 buf[0], nb, strerror(err), err);
+        }
+        close(fd);
+    } else {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:enable_sysfs_sensor - fd<0");
+    }
+
+    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) {
+        fscanf(sysfsfp, "%d\n", var);
+	fclose(sysfsfp);
+    } else {
+        LOGE("HAL:ERR open file to read");
+        res= -1;   
+    }
+    return res;
+}
+
+int write_sysfs_int(char *filename, int var)
+{
+    int res=0;
+    FILE  *sysfsfp;
+
+    sysfsfp = fopen(filename, "w");
+    if (sysfsfp!=NULL) {
+        fprintf(sysfsfp, "%d\n", var);
+	fclose(sysfsfp);
+    } else {
+        LOGE("HAL:ERR open file to write");
+        res= -1;   
+    }
+    return res;
+}
diff --git a/libsensors_iio/MPLSupport.h b/libsensors_iio/MPLSupport.h
new file mode 100644
index 0000000..73604ba
--- /dev/null
+++ b/libsensors_iio/MPLSupport.h
@@ -0,0 +1,29 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef ANDROID_MPL_SUPPORT_H
+#define ANDROID_MPL_SUPPORT_H
+
+#include <stdint.h>
+
+    int inv_read_data(char *fname, long *data);
+    int read_attribute_sensor(int fd, char* data, unsigned int size);
+    int enable_sysfs_sensor(int fd, int en);
+    int write_attribute_sensor(int fd, long data);
+    int read_sysfs_int(char*, int*);
+    int write_sysfs_int(char*, int);
+
+#endif //  ANDROID_MPL_SUPPORT_H
diff --git a/libsensors_iio/SensorBase.cpp b/libsensors_iio/SensorBase.cpp
new file mode 100644
index 0000000..fd0e2ca
--- /dev/null
+++ b/libsensors_iio/SensorBase.cpp
@@ -0,0 +1,143 @@
+/*

+* Copyright (C) 2012 Invensense, Inc.

+*

+* Licensed under the Apache License, Version 2.0 (the "License");

+* you may not use this file except in compliance with the License.

+* You may obtain a copy of the License at

+*

+*      http://www.apache.org/licenses/LICENSE-2.0

+*

+* Unless required by applicable law or agreed to in writing, software

+* distributed under the License is distributed on an "AS IS" BASIS,

+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

+* See the License for the specific language governing permissions and

+* limitations under the License.

+*/

+

+#include <fcntl.h>

+#include <errno.h>

+#include <math.h>

+#include <poll.h>

+#include <unistd.h>

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

+}

+

+int64_t SensorBase::getTimestamp()

+{

+    struct timespec t;

+    t.tv_sec = t.tv_nsec = 0;

+    clock_gettime(CLOCK_MONOTONIC, &t);

+    return int64_t(t.tv_sec) * 1000000000LL + t.tv_nsec;

+}

+

+int SensorBase::openInput(const char *inputName)

+{

+    int fd = -1;

+    const char *dirname = "/dev/input";

+    char devname[PATH_MAX];

+    char *filename;

+    DIR *dir;

+    struct dirent *de;

+    dir = opendir(dirname);

+    if(dir == NULL)

+        return -1;

+    strcpy(devname, dirname);

+    filename = devname + strlen(devname);

+    *filename++ = '/';

+    while((de = readdir(dir))) {

+        if(de->d_name[0] == '.' &&

+                (de->d_name[1] == '\0' ||

+                        (de->d_name[1] == '.' && de->d_name[2] == '\0')))

+            continue;

+        strcpy(filename, de->d_name);

+        fd = open(devname, O_RDONLY);

+        LOGV_IF(EXTRA_VERBOSE, "path open %s", devname);

+        LOGI("path open %s", devname);

+        if (fd >= 0) {

+            char name[80];

+            if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) {

+                name[0] = '\0';

+            }

+            LOGV_IF(EXTRA_VERBOSE, "name read %s", name);

+            if (!strcmp(name, inputName)) {

+                strcpy(input_name, filename);

+                break;

+            } else {

+                close(fd);

+                fd = -1;

+            }

+        }

+    }

+    closedir(dir);

+    LOGE_IF(fd < 0, "couldn't find '%s' input device", inputName);

+    return fd;

+}

+

+int SensorBase::enable(int32_t handle, int enabled)

+{

+    return 0;

+}

diff --git a/libsensors_iio/SensorBase.h b/libsensors_iio/SensorBase.h
new file mode 100644
index 0000000..d9abe92
--- /dev/null
+++ b/libsensors_iio/SensorBase.h
@@ -0,0 +1,64 @@
+/*
+* 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)
+
+/*****************************************************************************/
+
+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);
+    static int64_t getTimestamp();
+    static int64_t timevalToNano(timeval const& t) {
+        return t.tv_sec * 1000000000LL + t.tv_usec * 1000;
+    }
+
+    int open_device();
+    int close_device();
+
+public:
+            SensorBase(const char* dev_name, const char* data_name);
+
+    virtual ~SensorBase();
+
+    virtual int readEvents(sensors_event_t* data, int count) = 0;
+    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/libsensors_iio/libmllite.so b/libsensors_iio/libmllite.so
new file mode 100644
index 0000000..ed13b13
--- /dev/null
+++ b/libsensors_iio/libmllite.so
diff --git a/libsensors_iio/libmplmpu.so b/libsensors_iio/libmplmpu.so
new file mode 100644
index 0000000..e2ab461
--- /dev/null
+++ b/libsensors_iio/libmplmpu.so
diff --git a/libsensors_iio/local_log_def.h b/libsensors_iio/local_log_def.h
new file mode 100644
index 0000000..b08fdf1
--- /dev/null
+++ b/libsensors_iio/local_log_def.h
@@ -0,0 +1,46 @@
+#ifndef LOCAL_LOG_DEF_H

+#define LOCAL_LOG_DEF_H

+

+// -- workaround for different LOG definition in 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

+// -- Workaround End --

+

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

+

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

diff --git a/libsensors_iio/sensor_params.h b/libsensors_iio/sensor_params.h
new file mode 100644
index 0000000..88d5ba0
--- /dev/null
+++ b/libsensors_iio/sensor_params.h
@@ -0,0 +1,162 @@
+/*

+* 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_ACCELERATION_HANDLE     (ID_A)

+#define SENSORS_MAGNETIC_FIELD_HANDLE   (ID_M)

+#define SENSORS_ORIENTATION_HANDLE      (ID_O)

+

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

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

+#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_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_YAS530

+#define COMPASS_YAS530_RANGE            (8001.f)

+#define COMPASS_YAS530_RESOLUTION       (0.012f)

+#define COMPASS_YAS530_POWER            (4.f)

+#define COMPASS_YAS530_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_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         (32.8f * 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         (16.4f * 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         (16.4f * RAD_P_DEG)

+#define GYRO_MPU9150_POWER              (5.5f)

+#define GYRO_MPU9150_MINDELAY           (1000)

+//GYRO ITG3500

+#define GYRO_ITG3500_RANGE              (2000.f * RAD_P_DEG)

+#define GYRO_ITG3500_RESOLUTION         (16.4f * RAD_P_DEG)

+#define GYRO_ITG3500_POWER              (5.5f)

+#define GYRO_ITG3500_MINDELAY           (1000)

+

+#endif  /* INV_SENSOR_PARAMS_H */

+

diff --git a/libsensors_iio/sensors.h b/libsensors_iio/sensors.h
new file mode 100644
index 0000000..0556c10
--- /dev/null
+++ b/libsensors_iio/sensors.h
@@ -0,0 +1,104 @@
+/*

+* Copyright (C) 2012 Invensense, Inc.

+*

+* Licensed under the Apache License, Version 2.0 (the "License");

+* you may not use this file except in compliance with the License.

+* You may obtain a copy of the License at

+*

+*      http://www.apache.org/licenses/LICENSE-2.0

+*

+* Unless required by applicable law or agreed to in writing, software

+* distributed under the License is distributed on an "AS IS" BASIS,

+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

+* See the License for the specific language governing permissions and

+* limitations under the License.

+*/

+

+#ifndef ANDROID_SENSORS_H

+#define ANDROID_SENSORS_H

+

+#include <stdint.h>

+#include <errno.h>

+#include <sys/cdefs.h>

+#include <sys/types.h>

+

+#include <linux/input.h>

+

+#include <hardware/hardware.h>

+#include <hardware/sensors.h>

+

+__BEGIN_DECLS

+

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

+

+#ifndef ARRAY_SIZE

+#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))

+#endif

+

+#define ID_MPL_BASE (0)

+#define ID_GY (ID_MPL_BASE)

+#define ID_A  (ID_GY + 1)

+#define ID_M  (ID_A + 1)

+#define ID_O  (ID_M + 1)

+#define ID_RV (ID_O + 1)

+#define ID_LA (ID_RV + 1)

+#define ID_GR (ID_LA + 1)

+

+/*#define ID_MPL_BASE (0)

+#define ID_RV (ID_MPL_BASE)

+#define ID_LA (ID_RV + 1)

+#define ID_GR (ID_LA + 1)

+#define ID_GY (ID_GR + 1)

+#define ID_A  (ID_GY + 1)

+#define ID_M  (ID_A + 1)

+#define ID_O  (ID_M + 1)

+*/

+

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

+

+/*

+ * 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/libsensors_iio/sensors_mpl.cpp b/libsensors_iio/sensors_mpl.cpp
new file mode 100644
index 0000000..990c5f5
--- /dev/null
+++ b/libsensors_iio/sensors_mpl.cpp
@@ -0,0 +1,236 @@
+/*

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

+#include "local_log_def.h"

+

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

+/* The SENSORS Module */

+#define LOCAL_SENSORS (7)

+

+static struct sensor_t sSensorList[7];

+static int numSensors = LOCAL_SENSORS;

+

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

+}

+

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

+        numSensorDrivers,   // wake pipe goes here

+        numFds,

+    };

+

+    struct pollfd mPollFds[numSensorDrivers];

+    SensorBase *mSensor;

+    CompassSensor *mCompassSensor;

+};

+

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

+

+sensors_poll_context_t::sensors_poll_context_t() {

+    VFUNC_LOG;

+

+    CompassSensor *mCompassSensor = new CompassSensor();

+    MPLSensor *mplSensor = new MPLSensor(mCompassSensor);

+

+    // setup the callback object for handing mpl callbacks

+    setCallbackObject(mplSensor);

+    

+    // populate the sensor list

+    numSensors =

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

+}

+

+sensors_poll_context_t::~sensors_poll_context_t() {

+    FUNC_LOG;

+    delete mSensor;

+    delete mCompassSensor;

+}

+

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

+                nb = 0;

+                if (i == mpl) {

+                    nb = mSensor->readEvents(data, count);

+                }

+                else if (i == compass) {

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

+                }

+/*

+                if (nb > 0) {

+                    count -= nb;

+                    nbEvents += nb;

+                    data += nb;

+                    mPollFds[i].revents = 0;

+                }

+ */

+            }

+        }

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

+        if (nb > 0) {

+            count -= nb;

+            nbEvents += nb;

+            data += nb;

+            mPollFds[mpl].revents = 0;

+            mPollFds[compass].revents = 0;

+        }

+    }

+

+    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/libsensors_iio/software/build/android/common.mk b/libsensors_iio/software/build/android/common.mk
new file mode 100644
index 0000000..b84a99c
--- /dev/null
+++ b/libsensors_iio/software/build/android/common.mk
@@ -0,0 +1,68 @@
+# Use bash for additional echo fancyness
+SHELL = /bin/bash
+
+####################################################################################################
+## defines
+
+## 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
+HALWRAPPER_LIB_NAME ?= androidhal
+
+## applications ##
+SHARED_APP_SUFFIX = -shared
+STATIC_APP_SUFFIX = -static
+
+####################################################################################################
+## includes and linker
+
+ANDROID_LINK  = -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib 
+
+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
+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/libsensors_iio/software/build/android/shared.mk b/libsensors_iio/software/build/android/shared.mk
new file mode 100644
index 0000000..bc8548c
--- /dev/null
+++ b/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/console/linux/build/$(TARGET)
+APP_FOLDERS += $(INV_ROOT)/simple_apps/input_sub/build/$(TARGET)
+APP_FOLDERS += $(INV_ROOT)/simple_apps/self_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/libsensors_iio/software/core/HAL/include/inv_sysfs_utils.h b/libsensors_iio/software/core/HAL/include/inv_sysfs_utils.h
new file mode 100644
index 0000000..fce28ae
--- /dev/null
+++ b/libsensors_iio/software/core/HAL/include/inv_sysfs_utils.h
@@ -0,0 +1,83 @@
+/**
+ *  @brief    Provides helpful file IO wrappers for use with sysfs.
+ *  @details  Based on Jonathan Cameron's @e iio_utils.h.
+ */
+
+#ifndef _INV_SYSFS_UTILS_H_
+#define _INV_SYSFS_UTILS_H_
+
+/**
+ *  struct inv_sysfs_names_s - Files needed by user applications.
+ *  @buffer:		Ring buffer attached to FIFO.
+ *  @enable:		Turns on HW-to-ring buffer flow.
+ *  @raw_data:		Raw data from registers.
+ *  @temperature:	Temperature data from register.
+ *  @fifo_rate:		FIFO rate/ODR.
+ *  @power_state:	Power state (this is a five-star comment).
+ *  @fsr:		Full-scale range.
+ *  @lpf:		Digital low pass filter.
+ *  @scale:		LSBs / dps (or LSBs / Gs).
+ *  @temp_scale:	LSBs / degrees C.
+ *  @temp_offset:	Offset in LSBs.
+ */
+struct inv_sysfs_names_s {
+
+	//Sysfs for ITG3500 & MPU6050
+	const char *buffer;
+	const char *enable;
+	const char *raw_data;		//Raw Gyro data
+	const char *temperature;
+	const char *fifo_rate;
+	const char *power_state;
+	const char *fsr;
+	const char *lpf;
+	const char *scale;			//Gyro scale
+	const char *temp_scale;
+	const char *temp_offset;
+	const char *self_test;
+	//Starting Sysfs available for MPU6050 only
+	const char *accel_en;
+	const char *accel_fifo_en;
+	const char *accel_fs;
+	const char *clock_source;
+	const char *early_suspend_en;
+	const char *firmware_loaded;
+	const char *gyro_en;
+	const char *gyro_fifo_en;
+	const char *key;
+	const char *raw_accel;
+	const char *reg_dump;
+	const char *tap_on;
+	const char *dmp_firmware;
+};
+
+/* File IO. Typically won't be called directly by user application, but they'll
+ * be here for your enjoyment.
+ */
+int inv_sysfs_write(char *filename, long data);
+int inv_sysfs_read(char *filename, long num_bytes, char *data);
+
+/* Helper APIs to extract specific data. */
+int inv_read_buffer(int fd, long *data, long long *timestamp);
+int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, 
+		 long long *timestamp);
+int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data,
+			     long long *timestamp);
+int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data);
+int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data);
+int inv_read_scale(const struct inv_sysfs_names_s *names, float *data);
+int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data);
+int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data);
+int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data);
+int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data);
+int inv_write_power_state(const struct inv_sysfs_names_s *names, char data);
+
+/* Scaled data. */
+int inv_read_q16(const struct inv_sysfs_names_s *names, long *data,
+		 long long *timestamp);
+int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data,
+                      long long *timestamp);
+
+#endif  /* #ifndef _INV_SYSFS_UTILS_H_ */
+
+
diff --git a/libsensors_iio/software/core/HAL/include/mlos.h b/libsensors_iio/software/core/HAL/include/mlos.h
new file mode 100644
index 0000000..ce06b07
--- /dev/null
+++ b/libsensors_iio/software/core/HAL/include/mlos.h
@@ -0,0 +1,104 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+#ifndef _MLOS_H
+#define _MLOS_H
+
+#ifndef __KERNEL__
+#include <stdio.h>
+#endif
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if defined(LINUX) || defined(__KERNEL__)
+typedef unsigned int 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);
+
+    // Binary Semaphore
+    inv_error_t inv_create_binary_semaphore(HANDLE *semaphore);
+    inv_error_t inv_destroy_binary_semaphore(HANDLE handle);
+    inv_error_t inv_increment_binary_semaphore(HANDLE handle);
+    inv_error_t inv_decrement_binary_semaphore(HANDLE handle, long timeout_ms);
+
+	inv_error_t inv_destroy_mutex(HANDLE handle);
+
+	void inv_sleep(int mSecs);
+	inv_time_t 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/libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c b/libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c
new file mode 100644
index 0000000..c45badd
--- /dev/null
+++ b/libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c
@@ -0,0 +1,317 @@
+/**
+ *  @brief    Provides helpful file IO wrappers for use with sysfs.
+ *  @details  Based on Jonathan Cameron's @e iio_utils.h.
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <ctype.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <dirent.h>
+#include <errno.h>
+#include <unistd.h>
+#include "inv_sysfs_utils.h"
+
+/* General TODO list:
+ * Select more reasonable string lengths or use fseek and malloc.
+ */
+
+/**
+ *  inv_sysfs_write() - Write an integer to a file.
+ *  @filename:	Path to file.
+ *  @data:	Value to write to file.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_sysfs_write(char *filename, long data)
+{
+	FILE *fp;
+	int count;
+
+	if (!filename)
+		return -1;
+	fp = fopen(filename, "w");
+	if (!fp)
+		return -errno;
+	count = fprintf(fp, "%ld", data);
+	fclose(fp);
+	return count;
+}
+
+/**
+ *  inv_sysfs_read() - Read a string from a file.
+ *  @filename:	Path to file.
+ *  @num_bytes:	Number of bytes to read.
+ *  @data:	Data from file.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_sysfs_read(char *filename, long num_bytes, char *data)
+{
+	FILE *fp;
+	int count;
+
+	if (!filename)
+		return -1;
+	fp = fopen(filename, "r");
+	if (!fp)
+		return -errno;
+	count = fread(data, 1, num_bytes, fp);
+	fclose(fp);
+	return count;
+}
+
+/**
+ *  inv_read_buffer() - Read data from ring buffer.
+ *  @fd:	File descriptor for buffer file.
+ *  @data:	Data in hardware units.
+ *  @timestamp:	Time when data was read from device. Use NULL if unsupported.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_buffer(int fd, long *data, long long *timestamp)
+{
+	char str[35];
+	int count;
+
+	count = read(fd, str, sizeof(str));
+	if (!count)
+		return count;
+	if (!timestamp)
+		count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]);
+	else
+		count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1],
+			&data[2], timestamp);
+
+	if (count < (timestamp?4:3))
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_raw() - Read raw data.
+ *  @names:	Names of sysfs files.
+ *  @data:	Data in hardware units.
+ *  @timestamp:	Time when data was read from device. Use NULL if unsupported.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, 
+	long long *timestamp)
+{
+	char str[40];
+	int count;
+
+	count = inv_sysfs_read((char*)names->raw_data, sizeof(str), str);
+	if (count < 0)
+		return count;
+	if (!timestamp)
+		count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]);
+	else
+		count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1],
+			&data[2], timestamp);
+	if (count < (timestamp?4:3))
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_temperature_raw() - Read temperature.
+ *  @names:	Names of sysfs files.
+ *  @data:	Data in hardware units.
+ *  @timestamp:	Time when data was read from device.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data,
+	long long *timestamp)
+{
+	char str[25];
+	int count;
+
+	count = inv_sysfs_read((char*)names->temperature, sizeof(str), str);
+	if (count < 0)
+		return count;
+	count = sscanf(str, "%hd%lld", &data[0], timestamp);
+	if (count < 2)
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_fifo_rate() - Read fifo rate.
+ *  @names:	Names of sysfs files.
+ *  @data:	Fifo rate.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data)
+{
+	char str[8];
+	int count;
+
+	count = inv_sysfs_read((char*)names->fifo_rate, sizeof(str), str);
+	if (count < 0)
+		return count;
+	count = sscanf(str, "%hd", data);
+	if (count < 1)
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_power_state() - Read power state.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 if device is on.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data)
+{
+	char str[2];
+	int count;
+
+	count = inv_sysfs_read((char*)names->power_state, sizeof(str), str);
+	if (count < 0)
+		return count;
+	count = sscanf(str, "%hd", (short*)data);
+	if (count < 1)
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_scale() - Read scale.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 if device is on.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_scale(const struct inv_sysfs_names_s *names, float *data)
+{
+	char str[5];
+	int count;
+
+	count = inv_sysfs_read((char*)names->scale, sizeof(str), str);
+	if (count < 0)
+		return count;
+	count = sscanf(str, "%f", data);
+	if (count < 1)
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_temp_scale() - Read temperature scale.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 if device is on.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data)
+{
+	char str[4];
+	int count;
+
+	count = inv_sysfs_read((char*)names->temp_scale, sizeof(str), str);
+	if (count < 0)
+		return count;
+	count = sscanf(str, "%hd", data);
+	if (count < 1)
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_temp_offset() - Read temperature offset.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 if device is on.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data)
+{
+	char str[4];
+	int count;
+
+	count = inv_sysfs_read((char*)names->temp_offset, sizeof(str), str);
+	if (count < 0)
+		return count;
+	count = sscanf(str, "%hd", data);
+	if (count < 1)
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_q16() - Get data as q16 fixed point.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 if device is on.
+ *  @timestamp:	Time when data was read from device.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_q16(const struct inv_sysfs_names_s *names, long *data,
+	long long *timestamp)
+{
+	int count;
+	short raw[3];
+	float scale;
+	count = inv_read_raw(names, (long*)raw, timestamp);
+	count += inv_read_scale(names, &scale);
+	data[0] = (long)(raw[0] * (65536.f / scale));
+	data[1] = (long)(raw[1] * (65536.f / scale));
+	data[2] = (long)(raw[2] * (65536.f / scale));
+	return count;
+}
+
+/**
+ *  inv_read_q16() - Get temperature data as q16 fixed point.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 if device is on.
+ *  @timestamp:	Time when data was read from device.
+ *  Returns number of bytes read or a negative error code.
+ */
+int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data,
+	long long *timestamp)
+{
+	int count = 0;
+	short raw;
+	static short scale, offset;
+	static unsigned char first_read = 1;
+
+	if (first_read) {
+		count += inv_read_temp_scale(names, &scale);
+		count += inv_read_temp_offset(names, &offset);
+		first_read = 0;
+	}
+	count += inv_read_temperature_raw(names, &raw, timestamp);
+	data[0] = (long)((35 + ((float)(raw - offset) / scale)) * 65536.f);
+
+	return count;
+}
+
+/**
+ *  inv_write_fifo_rate() - Write fifo rate.
+ *  @names:	Names of sysfs files.
+ *  @data:	Fifo rate.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data)
+{
+	return inv_sysfs_write((char*)names->fifo_rate, (long)data);
+}
+
+/**
+ *  inv_write_buffer_enable() - Enable/disable buffer in /dev.
+ *  @names:	Names of sysfs files.
+ *  @data:	Fifo rate.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data)
+{
+	return inv_sysfs_write((char*)names->enable, (long)data);
+}
+
+/**
+ *  inv_write_power_state() - Turn device on/off.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 to turn on.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_write_power_state(const struct inv_sysfs_names_s *names, char data)
+{
+	return inv_sysfs_write((char*)names->power_state, (long)data);
+}
+
+
diff --git a/libsensors_iio/software/core/HAL/linux/mlos_linux.c b/libsensors_iio/software/core/HAL/linux/mlos_linux.c
new file mode 100644
index 0000000..75f062e
--- /dev/null
+++ b/libsensors_iio/software/core/HAL/linux/mlos_linux.c
@@ -0,0 +1,194 @@
+/*
+ $License:
+    Copyright (C) 2011 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 "stdint_invensense.h"
+
+#include "mlos.h"
+#include <errno.h>
+
+
+/* -------------- */
+/* - Functions. - */
+/* -------------- */
+
+/**
+ *  @brief  Allocate space
+ *  @param  numBytes  number of bytes
+ *  @return pointer to allocated space
+**/
+void *inv_malloc(unsigned int numBytes)
+{
+    // Allocate space.
+    void *allocPtr = malloc(numBytes);
+    return allocPtr;
+}
+
+
+/**
+ *  @brief  Free allocated space
+ *  @param  ptr pointer to space to deallocate
+ *  @return error code.
+**/
+inv_error_t inv_free(void *ptr)
+{
+    // Deallocate space.
+    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 mSecs)
+{
+    usleep(mSecs*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);
+}
+
+  /**********************/
+ /** @} */ /* defgroup */
+/**********************/
+
diff --git a/libsensors_iio/software/core/driver/include/linux/mpu.h b/libsensors_iio/software/core/driver/include/linux/mpu.h
new file mode 100644
index 0000000..9b29695
--- /dev/null
+++ b/libsensors_iio/software/core/driver/include/linux/mpu.h
@@ -0,0 +1,355 @@
+/*
+* 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>
+#elif defined LINUX
+#include <sys/ioctl.h>
+#include <linux/types.h>
+#else
+#include "mltypes.h"
+#endif
+
+struct mpu_read_write {
+	/* Memory address or register address depending on ioctl */
+	__u16 address;
+	__u16 length;
+	__u8 *data;
+};
+
+enum mpuirq_data_type {
+	MPUIRQ_DATA_TYPE_MPU_DATA_READY_IRQ,
+	MPUIRQ_DATA_TYPE_MPU_FIFO_READY_IRQ,
+	MPUIRQ_DATA_TYPE_SLAVE_IRQ,
+	MPUIRQ_DATA_TYPE_PM_EVENT,
+	MPUIRQ_DATA_TYPE_NUM_TYPES,
+};
+
+/* User space PM event notification */
+#define MPU_PM_EVENT_SUSPEND_PREPARE (3)
+#define MPU_PM_EVENT_POST_SUSPEND    (4)
+
+/**
+ * struct mpuirq_data - structure to report what and when
+ * @interruptcount	: The number of times this IRQ has occured since open
+ * @irqtime		: monotonic time of the IRQ in ns
+ * @data_type		: The type of this IRQ enum mpuirq_data_type
+ * @data		: Data associated with this IRQ
+ */
+struct mpuirq_data {
+	__u32 interruptcount;
+	__s64 irqtime_ns;
+	__u32 data_type;
+	__s32 data;
+};
+
+enum ext_slave_config_key {
+	/* TODO: Remove these first six. */
+	MPU_SLAVE_CONFIG_ODR_SUSPEND,
+	MPU_SLAVE_CONFIG_ODR_RESUME,
+	MPU_SLAVE_CONFIG_FSR_SUSPEND,
+	MPU_SLAVE_CONFIG_FSR_RESUME,
+	MPU_SLAVE_CONFIG_IRQ_SUSPEND,
+	MPU_SLAVE_CONFIG_IRQ_RESUME,
+	MPU_SLAVE_CONFIG_ODR,
+	MPU_SLAVE_CONFIG_FSR,
+	MPU_SLAVE_CONFIG_MOT_THS,
+	MPU_SLAVE_CONFIG_NMOT_THS,
+	MPU_SLAVE_CONFIG_MOT_DUR,
+	MPU_SLAVE_CONFIG_NMOT_DUR,
+	MPU_SLAVE_CONFIG_IRQ,
+	MPU_SLAVE_WRITE_REGISTERS,
+	MPU_SLAVE_READ_REGISTERS,
+	MPU_SLAVE_CONFIG_INTERNAL_REFERENCE,
+	/* AMI 306 specific config keys */
+	MPU_SLAVE_PARAM,
+	MPU_SLAVE_WINDOW,
+	MPU_SLAVE_READWINPARAMS,
+	MPU_SLAVE_SEARCHOFFSET,
+	/* MPU3050 and MPU6050 Keys */
+	MPU_SLAVE_INT_CONFIG,
+	MPU_SLAVE_EXT_SYNC,
+	MPU_SLAVE_FULL_SCALE,
+	MPU_SLAVE_LPF,
+	MPU_SLAVE_CLK_SRC,
+	MPU_SLAVE_DIVIDER,
+	MPU_SLAVE_DMP_ENABLE,
+	MPU_SLAVE_FIFO_ENABLE,
+	MPU_SLAVE_DMP_CFG1,
+	MPU_SLAVE_DMP_CFG2,
+	MPU_SLAVE_TC,
+	MPU_SLAVE_GYRO,
+	MPU_SLAVE_ADDR,
+	MPU_SLAVE_PRODUCT_REVISION,
+	MPU_SLAVE_SILICON_REVISION,
+	MPU_SLAVE_PRODUCT_ID,
+	MPU_SLAVE_GYRO_SENS_TRIM,
+	MPU_SLAVE_ACCEL_SENS_TRIM,
+	MPU_SLAVE_RAM,
+	/* -------------------------- */
+	MPU_SLAVE_CONFIG_NUM_CONFIG_KEYS
+};
+
+/* For the MPU_SLAVE_CONFIG_IRQ_SUSPEND and MPU_SLAVE_CONFIG_IRQ_RESUME */
+enum ext_slave_config_irq_type {
+	MPU_SLAVE_IRQ_TYPE_NONE,
+	MPU_SLAVE_IRQ_TYPE_MOTION,
+	MPU_SLAVE_IRQ_TYPE_DATA_READY,
+};
+
+/* Structure for the following IOCTS's
+ * MPU_CONFIG_GYRO
+ * MPU_CONFIG_ACCEL
+ * MPU_CONFIG_COMPASS
+ * MPU_CONFIG_PRESSURE
+ * MPU_GET_CONFIG_GYRO
+ * MPU_GET_CONFIG_ACCEL
+ * MPU_GET_CONFIG_COMPASS
+ * MPU_GET_CONFIG_PRESSURE
+ *
+ * @key one of enum ext_slave_config_key
+ * @len length of data pointed to by data
+ * @apply zero if communication with the chip is not necessary, false otherwise
+ *        This flag can be used to select cached data or to refresh cashed data
+ *        cache data to be pushed later or push immediately.  If true and the
+ *        slave is on the secondary bus the MPU will first enger bypass mode
+ *        before calling the slaves .config or .get_config funcion
+ * @data pointer to the data to confgure or get
+ */
+struct ext_slave_config {
+	__u8 key;
+	__u16 len;
+	__u8 apply;
+	void *data;
+};
+
+enum ext_slave_type {
+	EXT_SLAVE_TYPE_GYROSCOPE,
+	EXT_SLAVE_TYPE_ACCEL,
+	EXT_SLAVE_TYPE_COMPASS,
+	EXT_SLAVE_TYPE_PRESSURE,
+	/*EXT_SLAVE_TYPE_TEMPERATURE */
+
+	EXT_SLAVE_NUM_TYPES
+};
+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)
+
+enum ext_slave_endian {
+	EXT_SLAVE_BIG_ENDIAN,
+	EXT_SLAVE_LITTLE_ENDIAN,
+	EXT_SLAVE_FS8_BIG_ENDIAN,
+	EXT_SLAVE_FS16_BIG_ENDIAN,
+};
+
+enum ext_slave_bus {
+	EXT_SLAVE_BUS_INVALID = -1,
+	EXT_SLAVE_BUS_PRIMARY = 0,
+	EXT_SLAVE_BUS_SECONDARY = 1
+};
+
+
+/**
+ *  struct ext_slave_platform_data - Platform data for mpu3050 and mpu6050
+ *  slave devices
+ *
+ *  @type: the type of slave device based on the enum ext_slave_type
+ *         definitions.
+ *  @irq: the irq number attached to the slave if any.
+ *  @adapt_num: the I2C adapter number.
+ *  @bus: the bus the slave is attached to: enum ext_slave_bus
+ *  @address: the I2C slave address of the slave device.
+ *  @orientation: the mounting matrix of the device relative to MPU.
+ *  @irq_data: private data for the slave irq handler
+ *  @private_data: additional data, user customizable.  Not touched by the MPU
+ *                 driver.
+ *
+ * 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 ext_slave_platform_data {
+	__u8 type;
+	__u32 irq;
+	__u32 adapt_num;
+	__u32 bus;
+	__u8 address;
+	__s8 orientation[9];
+	void *irq_data;
+	void *private_data;
+};
+
+struct fix_pnt_range {
+	__s32 mantissa;
+	__s32 fraction;
+};
+
+static inline long range_fixedpoint_to_long_mg(struct fix_pnt_range rng)
+{
+	return (long)(rng.mantissa * 1000 + rng.fraction / 10);
+}
+
+struct ext_slave_read_trigger {
+	__u8 reg;
+	__u8 value;
+};
+
+/**
+ *  struct ext_slave_descr - Description of the slave device for programming.
+ *
+ *  @suspend:	function pointer to put the device in suspended state
+ *  @resume:	function pointer to put the device in running state
+ *  @read:	function that reads the device data
+ *  @init:	function used to preallocate memory used by the driver
+ *  @exit:	function used to free memory allocated for the driver
+ *  @config:	function used to configure the device
+ *  @get_config:function used to get the device's configuration
+ *
+ *  @name:	text name of the device
+ *  @type:	device type. enum ext_slave_type
+ *  @id:	enum ext_slave_id
+ *  @read_reg:	starting register address to retrieve data.
+ *  @read_len:	length in bytes of the sensor data.  Typically  6.
+ *  @endian:	byte order of the data. enum ext_slave_endian
+ *  @range:	full scale range of the slave ouput: struct fix_pnt_range
+ *  @trigger:	If reading data first requires writing a register this is the
+ *		data to write.
+ *
+ *  Defines the functions and information about the slave the mpu3050 and
+ *  mpu6050 needs to use the slave device.
+ */
+struct ext_slave_descr {
+	int (*init) (void *mlsl_handle,
+		     struct ext_slave_descr *slave,
+		     struct ext_slave_platform_data *pdata);
+	int (*exit) (void *mlsl_handle,
+		     struct ext_slave_descr *slave,
+		     struct ext_slave_platform_data *pdata);
+	int (*suspend) (void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata);
+	int (*resume) (void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata);
+	int (*read) (void *mlsl_handle,
+		     struct ext_slave_descr *slave,
+		     struct ext_slave_platform_data *pdata,
+		     __u8 *data);
+	int (*config) (void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       struct ext_slave_config *config);
+	int (*get_config) (void *mlsl_handle,
+			   struct ext_slave_descr *slave,
+			   struct ext_slave_platform_data *pdata,
+			   struct ext_slave_config *config);
+
+	char *name;
+	__u8 type;
+	__u8 id;
+	__u8 read_reg;
+	__u8 read_len;
+	__u8 endian;
+	struct fix_pnt_range range;
+	struct ext_slave_read_trigger *trigger;
+};
+
+/**
+ * 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
+ *
+ * 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/libsensors_iio/software/core/driver/include/log.h b/libsensors_iio/software/core/driver/include/log.h
new file mode 100644
index 0000000..74c49f3
--- /dev/null
+++ b/libsensors_iio/software/core/driver/include/log.h
@@ -0,0 +1,363 @@
+/*
+ $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 <stdarg.h>
+#include "local_log_def.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, ...) \
+	LOG(priority, tag, fmt, ##__VA_ARGS__)
+#elif defined __KERNEL__
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+	pr_debug(MPL_##priority tag fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+	_MLPrintLog(MPL_##priority, tag, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+/*
+ * Log macro that allows you to pass in a varargs ("args" is a va_list).
+ */
+#ifndef MPL_LOG_PRI_VA
+#ifdef ANDROID
+#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \
+	android_vprintLog(priority, NULL, tag, fmt, args)
+#elif defined __KERNEL__
+/* not allowed in the Kernel because there is no dev_dbg that takes a va_list */
+#else
+#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \
+	_MLPrintVaLog(priority, NULL, tag, fmt, args)
+#endif
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * ===========================================================================
+ *
+ * The stuff in the rest of this file should not be used directly.
+ */
+
+#ifndef ANDROID
+int _MLPrintLog(int priority, const char *tag, const char *fmt,	...);
+int _MLPrintVaLog(int priority, const char *tag, const char *fmt, va_list args);
+/* Final implementation of actual writing to a character device */
+int _MLWriteLog(const char *buf, int buflen);
+#endif
+
+static inline void __print_result_location(int result,
+					   const char *file,
+					   const char *func, int line)
+{
+	MPL_LOGE("%s|%s|%d returning %d\n", file, func, line, result);
+}
+
+#ifdef _WIN32
+/* The pragma removes warning about expression being constant */
+#define LOG_RESULT_LOCATION(condition) \
+    do {								\
+		__print_result_location((int)(condition), __FILE__,	\
+					__func__, __LINE__);		\
+        __pragma (warning(suppress : 4127 )) \
+	} while (0)
+#else
+#define LOG_RESULT_LOCATION(condition) \
+    do {								\
+		__print_result_location((int)(condition), __FILE__,	\
+					__func__, __LINE__);		\
+	} while (0)
+#endif
+
+
+#define INV_ERROR_CHECK(r_1329) \
+    if (r_1329) { \

+        LOG_RESULT_LOCATION(r_1329); \

+        return r_1329; \

+    }
+
+#ifdef __cplusplus
+}
+#endif
+#endif				/* _LIBS_CUTILS_MPL_LOG_H */
diff --git a/libsensors_iio/software/core/driver/include/mlinclude.h b/libsensors_iio/software/core/driver/include/mlinclude.h
new file mode 100644
index 0000000..9725199
--- /dev/null
+++ b/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/libsensors_iio/software/core/driver/include/mlmath.h b/libsensors_iio/software/core/driver/include/mlmath.h
new file mode 100644
index 0000000..37194d6
--- /dev/null
+++ b/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/libsensors_iio/software/core/driver/include/mlsl.h b/libsensors_iio/software/core/driver/include/mlsl.h
new file mode 100644
index 0000000..12f2901
--- /dev/null
+++ b/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/libsensors_iio/software/core/driver/include/mltypes.h b/libsensors_iio/software/core/driver/include/mltypes.h
new file mode 100644
index 0000000..09eccce
--- /dev/null
+++ b/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/libsensors_iio/software/core/driver/include/stdint_invensense.h b/libsensors_iio/software/core/driver/include/stdint_invensense.h
new file mode 100644
index 0000000..b8c2511
--- /dev/null
+++ b/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/libsensors_iio/software/core/mllite/CMakeLists.txt b/libsensors_iio/software/core/mllite/CMakeLists.txt
new file mode 100644
index 0000000..8650553
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/CMakeLists.txt
@@ -0,0 +1,39 @@
+## CMakeLists for mlsdk/mldmp
+
+project(mldmp C)
+
+if (NOT CMAKE_BUILD_ENGINEERING)
+
+#    # just copy the library from mldmp/mpl/$PLATFORM to mldmp/
+#    if (CMAKE_SYSTEM_NAME MATCHES Windows)
+#        execute_process(
+#            COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/win32/mpl.lib ${CMAKE_CURRENT_BINARY_DIR}/mpl.lib
+#        )
+#    elseif (CMAKE_SYSTEM_NAME MATCHES Android)
+#        execute_process(
+#            COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/android/libmpl.a ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a
+#        )
+#    elseif (CMAKE_SYSTEM_NAME MATCHES Linux)
+#        message("copying ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/libmpl.a to ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a")
+#        execute_process(
+#            COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/libmpl.a ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a
+#        )
+#    elseif (CMAKE_SYSTEM_NAME MATCHES Catalina)
+#        execute_process(
+#            COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/mpl.lib ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a
+#        )
+#    endif()
+#    # better way that doesn't work for now
+#    # add_custom_command(
+#        # TARGET mpl
+#        # PRE_BUILD
+#        # COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/${DDF} ${CMAKE_CURRENT_BINARY_DIR}
+#        # COMMAND ${CMAKE_COMMAND} ARGS -E echo "copying ${DDF}"
+#    # )
+    
+else (NOT CMAKE_BUILD_ENGINEERING)
+
+  set (CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR})
+  include(Engineering)
+
+endif (NOT CMAKE_BUILD_ENGINEERING)
diff --git a/libsensors_iio/software/core/mllite/Engineering.cmake b/libsensors_iio/software/core/mllite/Engineering.cmake
new file mode 100644
index 0000000..42f2429
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/Engineering.cmake
@@ -0,0 +1,150 @@
+## Engineering CMakeLists for software/core/mllite
+
+include_directories(
+    .
+    ${SOLUTION_SOURCE_DIR}/core/mpl
+    ${SOLUTION_SOURCE_DIR}/core/HAL
+    ${SOLUTION_SOURCE_DIR}/driver/include
+)
+
+add_definitions (
+    -DINV_INCLUDE_LEGACY_HEADERS
+)
+
+if (CMAKE_SYSTEM_NAME MATCHES Android)
+
+    include_directories(
+        ${SOLUTION_SOURCE_DIR}/core/mllite/linux
+        ${SOLUTION_SOURCE_DIR}/driver/include/linux
+    )
+    add_definitions(
+        -DLINUX
+        -D_REENTRANT
+    )
+    set (HEADERS
+        ${HEADERS}
+        linux/mlos.h
+        linux/ml_stored_data.h
+        linux/ml_load_dmp.h
+        linux/ml_sysfs_helper.h
+    )
+    set (SOURCES
+        ${SOURCES}
+        linux/mlos_linux.c
+        linux/ml_stored_data.c
+        linux/ml_load_dmp.c
+        linux/ml_sysfs_helper.c
+    )
+
+elseif (CMAKE_SYSTEM_NAME MATCHES Linux)
+
+    add_definitions(
+        -DLINUX
+        -D_REENTRANT
+    )
+    set (HEADERS
+        ${HEADERS}
+    )
+    set (SOURCES
+        ${SOURCES}
+    )
+
+elseif (CMAKE_SYSTEM_NAME MATCHES Windows)
+
+    add_definitions(
+        -DAIO
+        -DUART_COM
+        -D_CRT_SECURE_NO_WARNINGS
+        -D_CRT_SECURE_NO_DEPRECATE
+    )
+    set (HEADERS
+        ${HEADERS}
+    )
+    set (SOURCES
+        ${SOURCES}
+    )
+
+endif ()
+
+set (HEADERS
+    ${HEADERS}
+    data_builder.h
+    hal_outputs.h
+    message_layer.h
+    ml_math_func.h
+    mpl.h
+    results_holder.h
+    start_manager.h
+    storage_manager.h
+)
+set (SOURCES
+    ${SOURCES}
+    data_builder.c
+    hal_outputs.c
+    message_layer.c
+    ml_math_func.c
+    mpl.c
+    results_holder.c
+    start_manager.c
+    storage_manager.c
+)
+
+# coveniently add this file to the resources for easy access within IDEs
+set (RESOURCES
+    Engineering.cmake
+)
+
+if (CMAKE_TESTING_SUPPORT)
+
+    message("Including Testing support")
+    include_directories(
+        ${SOLUTION_SOURCE_DIR}/mltools/debugsupport
+    )
+    add_definitions(
+        -DSELF_VERIFICATION
+    )
+    set(
+        HEADERS 
+        ${HEADERS}
+        ${SOLUTION_SOURCE_DIR}/mltools/debugsupport/testsupport.h
+    )
+    set(
+        SOURCES
+        ${SOURCES}
+        ${SOLUTION_SOURCE_DIR}/mltools/debugsupport/testsupport.c
+    )
+
+endif ()
+
+############
+## TARGET ##
+############
+add_library(
+    mllite STATIC
+    ${SOURCES}
+    ${HEADERS}
+    ${RESOURCES}
+)
+set_target_properties(
+    mllite
+    PROPERTIES CLEAN_DIRECT_OUTPUT 1
+)
+
+if (CMAKE_SYSTEM_NAME MATCHES "(Android|Linux)")
+
+    add_library(
+        mllite_shared SHARED
+        ${SOURCES}
+        ${HEADERS}
+        ${RESOURCES}
+    )
+    FIX_SHARED_LIBRARY_NAME("mllite_shared" "mllite")
+
+    install (
+        TARGETS mllite_shared
+        DESTINATION lib
+    )
+
+endif () 
+
+
diff --git a/libsensors_iio/software/core/mllite/build/android/shared.mk b/libsensors_iio/software/core/mllite/build/android/shared.mk
new file mode 100644
index 0000000..2ee2e20
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/build/android/shared.mk
@@ -0,0 +1,91 @@
+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 += -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 += -nostdlib 
+LFLAGS += -fpic 
+LFLAGS += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.xsc 
+LFLAGS += -Wl,--gc-sections 
+LFLAGS += -Wl,--no-whole-archive 
+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/libsensors_iio/software/core/mllite/build/android/static.mk b/libsensors_iio/software/core/mllite/build/android/static.mk
new file mode 100644
index 0000000..6ad45de
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/build/android/static.mk
@@ -0,0 +1,110 @@
+MLLITE_LIB_NAME = mllite
+LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(STATIC_LIB_EXT)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+OBJFOLDER = $(CURDIR)/obj
+
+CROSS = arm-none-linux-gnueabi-
+COMP= $(CROSS)gcc
+LINK= $(CROSS)ar cr
+
+MLLITE_DIR = $(MLSDK_ROOT)/mllite
+MPL_DIR = $(MLSDK_ROOT)/mldmp
+MLPLATFORM_DIR = $(MLSDK_ROOT)/platform
+
+include $(MLSDK_ROOT)/Android-common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += -Wall -fpic
+CFLAGS += -mthumb-interwork -fno-exceptions -ffunction-sections -funwind-tables -fstack-protector -fno-short-enums -fmessage-length=0
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT -DLINUX -DANDROID
+CFLAGS += -I$(MLLITE_DIR) 
+CFLAGS += -I$(MLPLATFORM_DIR)/include
+CFLAGS += -I$(MLSDK_ROOT)/mlutils 
+CFLAGS += -I$(MLSDK_ROOT)/mlapps/common
+CFLAGS += $(MLSDK_INCLUDES)
+CFLAGS += $(MLSDK_DEFINES)
+
+VPATH += $(MLLITE_DIR) 
+VPATH += $(MLSDK_ROOT)/mlutils
+VPATH += $(MLLITE_DIR)/accel 
+VPATH += $(MLLITE_DIR)/compass 
+VPATH += $(MLLITE_DIR)/pressure 
+VPATH += $(MLLITE_DIR)/mlapps/common
+
+####################################################################################################
+## sources
+
+ML_LIBS  = $(MLPLATFORM_DIR)/linux/$(LIB_PREFIX)$(MLPLATFORM_LIB_NAME).$(STATIC_LIB_EXT)
+
+ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_mpu.c
+ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_init_linux.c
+ML_SOURCES += $(MLLITE_DIR)/accel.c
+ML_SOURCES += $(MLLITE_DIR)/compass.c
+ML_SOURCES += $(MLLITE_DIR)/compass_supervisor.c
+ML_SOURCES += $(MLLITE_DIR)/compass_supervisor_adv_callbacks.c
+ML_SOURCES += $(MLLITE_DIR)/key0_96.c
+ML_SOURCES += $(MLLITE_DIR)/pressure.c
+ML_SOURCES += $(MLLITE_DIR)/ml.c
+ML_SOURCES += $(MLLITE_DIR)/ml_invobj.c
+ML_SOURCES += $(MLLITE_DIR)/ml_init.c
+ML_SOURCES += $(MLLITE_DIR)/mlarray_lite.c
+ML_SOURCES += $(MLLITE_DIR)/mlarray_adv.c
+ML_SOURCES += $(MLLITE_DIR)/mlarray_legacy.c
+ML_SOURCES += $(MLLITE_DIR)/mlBiasNoMotion.c
+ML_SOURCES += $(MLLITE_DIR)/mlFIFO.c
+ML_SOURCES += $(MLLITE_DIR)/mlFIFOHW.c
+ML_SOURCES += $(MLLITE_DIR)/mlMathFunc.c
+ML_SOURCES += $(MLLITE_DIR)/mlcontrol.c
+ML_SOURCES += $(MLLITE_DIR)/mldl.c
+ML_SOURCES += $(MLLITE_DIR)/mldmp.c
+ML_SOURCES += $(MLLITE_DIR)/dmpDefault.c
+ML_SOURCES += $(MLLITE_DIR)/mlstates.c
+ML_SOURCES += $(MLLITE_DIR)/mlsupervisor.c
+ML_SOURCES += $(MLLITE_DIR)/ml_stored_data.c
+ML_SOURCES += $(MLLITE_DIR)/ustore_manager.c
+ML_SOURCES += $(MLLITE_DIR)/ustore_mlsl_io.c
+ML_SOURCES += $(MLLITE_DIR)/ustore_adv_fusion_delegate.c
+ML_SOURCES += $(MLLITE_DIR)/ustore_lite_fusion_delegate.c
+ML_SOURCES += $(MLLITE_DIR)/temp_comp_legacy.c
+ML_SOURCES += $(MLLITE_DIR)/mlSetGyroBias.c
+ML_SOURCES += $(MLSDK_ROOT)/mlutils/checksum.c
+ML_SOURCES += $(MLSDK_ROOT)/mlutils/mputest.c
+ML_SOURCES += $(MLLITE_DIR)/mldl_print_cfg.c
+ifeq ($(MPU_NAME),MPU3050)
+ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu3050test.c
+else
+ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu6050test.c
+endif
+
+ML_OBJS := $(addsuffix .o,$(ML_SOURCES))
+ML_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(ML_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all clean cleanall
+
+all: $(LIBRARY) $(MK_NAME)
+
+$(LIBRARY) : $(OBJFOLDER) $(ML_OBJS_DST) $(ML_LIBS) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(ML_OBJS_DST)\n")
+	$(LINK) $(LIBRARY) $(ML_OBJS_DST)
+	$(CROSS)ranlib $(LIBRARY)
+
+$(OBJFOLDER) : 
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(ML_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(CFLAGS) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(MLSDK_INCLUDES) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(LIBRARY) $(OBJFOLDER)
+
diff --git a/libsensors_iio/software/core/mllite/build/filelist.mk b/libsensors_iio/software/core/mllite/build/filelist.mk
new file mode 100644
index 0000000..011120c
--- /dev/null
+++ b/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/libsensors_iio/software/core/mllite/data_builder.c b/libsensors_iio/software/core/mllite/data_builder.c
new file mode 100644
index 0000000..f70be7c
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/data_builder.c
@@ -0,0 +1,1162 @@
+/*
+ $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 "ml_math_func.h"
+#include "data_builder.h"
+#include "mlmath.h"
+#include "storage_manager.h"
+#include "message_layer.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];
+};
+
+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 53394
+
+#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));
+    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;
+}
+void inv_get_quaternion_transformation(int orientation, long *q)
+{
+    long s = 759250125; // sqrt(.5)*2^30
+
+    switch (orientation)
+    {
+    case 0x70:
+        q[0] = 759250125;
+        q[1] = 759250125;
+        q[2] = 0;
+        q[3] = 0;
+        break;
+    case 0x10a:
+        q[0] = 759250125;
+        q[1] = 0;
+        q[2] = 759250125;
+        q[3] = 0;
+        break;
+    case 0x85:
+        q[0] = 759250125;
+        q[1] = 0;
+        q[2] = 0;
+        q[3] = 759250125;
+        break;
+    case 0x181:
+        q[0] = 0;
+        q[1] = 759250125;
+        q[2] = 759250125;
+        q[3] = 0;
+        break;
+    case 0x2a:
+        q[0] = 0;
+        q[1] = 759250125;
+        q[2] = 0;
+        q[3] = 759250125;
+        break;
+    case 0x54:
+        q[0] = 0;
+        q[1] = 0;
+        q[2] = 759250125;
+        q[3] = 759250125;
+        break;
+    case 0x150:
+        q[0] = 759250125;
+        q[1] = -759250125;
+        q[2] = 0;
+        q[3] = 0;
+        break;
+    case 0xe:
+        q[0] = 759250125;
+        q[1] = 0;
+        q[2] = -759250125;
+        q[3] = 0;
+        break;
+    case 0xa1:
+        q[0] = 759250125;
+        q[1] = 0;
+        q[2] = 0;
+        q[3] = -759250125;
+        break;
+    case 0x1a5:
+        q[0] = 0;
+        q[1] = 759250125;
+        q[2] = -759250125;
+        q[3] = 0;
+        break;
+    case 0x12e:
+        q[0] = 0;
+        q[1] = 759250125;
+        q[2] = 0;
+        q[3] = -759250125;
+        break;
+    case 0x174:
+        q[0] = 0;
+        q[1] = 0;
+        q[2] = 759250125;
+        q[3] = -759250125;
+        break;
+
+
+    case 0x88:
+        q[0] = 1073741824;
+        q[1] = 0;
+        q[2] = 0;
+        q[3] = 0;
+        break;
+
+
+    case 0x1a8:
+        q[0] = 0;
+        q[1] = 1073741824;
+        q[2] = 0;
+        q[3] = 0;
+        break;
+
+    case 0x18c:
+        q[0] = 0;
+        q[1] = 0;
+        q[2] = 1073741824;
+        q[3] = 0;
+        break;
+
+    case 0xac:
+        q[0] = 0;
+        q[1] = 0;
+        q[2] = 0;
+        q[3] = 1073741824;
+        break;
+
+    default:
+        break;
+    }
+}
+/** 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);
+    }
+}
+/** 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.
+* @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] << 16;
+    raw32[1] = (long)sensor->raw[1] << 16;
+    raw32[2] = (long)sensor->raw[2] << 16;
+
+    raw32[0] -= bias[0];
+    raw32[1] -= bias[1];
+    raw32[2] -= bias[2];
+
+    inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity, 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_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_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;
+}
+
+
+/** 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;
+    /* 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;
+    }
+    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;
+    }
+    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;
+    }
+}
+
+/** 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/libsensors_iio/software/core/mllite/data_builder.h b/libsensors_iio/software/core/mllite/data_builder.h
new file mode 100644
index 0000000..b2d0881
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/data_builder.h
@@ -0,0 +1,224 @@
+/*
+ $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_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];
+    /** 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);
+
+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_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_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/libsensors_iio/software/core/mllite/dmpconfig.txt b/libsensors_iio/software/core/mllite/dmpconfig.txt
new file mode 100644
index 0000000..4643ed5
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/dmpconfig.txt
@@ -0,0 +1,3 @@
+major version    =  1
+minor version    =  0
+startAddr        =  0
diff --git a/libsensors_iio/software/core/mllite/hal_outputs.c b/libsensors_iio/software/core/mllite/hal_outputs.c
new file mode 100644
index 0000000..1cd3b81
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/hal_outputs.c
@@ -0,0 +1,425 @@
+/*
+ $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 "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 */
+    inv_time_t nav_timestamp;
+    inv_time_t gam_timestamp;
+    inv_time_t accel_timestamp;
+    long nav_quat[4];
+    int gyro_status;
+    int accel_status;
+    int compass_status;
+    int nine_axis_status;
+};
+
+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 = hal_out.accuracy_mag;
+    *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;
+}
+
+/** Gyroscope 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)
+{
+    /* 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
+    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;
+}
+
+/**
+* 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 = hal_out.accuracy_mag;
+    *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
+    long compass[3];
+    inv_get_compass_set(compass, accuracy, timestamp);
+    values[0] = compass[0] * COMPASS_CONVERSION;
+    values[1] = compass[1] * COMPASS_CONVERSION;
+    values[2] = compass[2] * COMPASS_CONVERSION;
+    if (hal_out.compass_status & INV_NEW_DATA)
+        status = 1;
+    else
+        status = 0;
+    return status;
+}
+
+
+/** 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)
+{
+    long t1, t2, t3;
+    long q00, q01, q02, q03, q11, q12, q13, q22, q23, q33;
+
+    *accuracy = hal_out.accuracy_mag;
+    *timestamp = hal_out.nav_timestamp;
+
+    q00 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[0]);
+    q01 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[1]);
+    q02 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[2]);
+    q03 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[3]);
+    q11 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[1]);
+    q12 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[2]);
+    q13 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[3]);
+    q22 = inv_q29_mult(hal_out.nav_quat[2], hal_out.nav_quat[2]);
+    q23 = inv_q29_mult(hal_out.nav_quat[2], hal_out.nav_quat[3]);
+    q33 = inv_q29_mult(hal_out.nav_quat[3], hal_out.nav_quat[3]);
+
+    /* X component of the Ybody axis in World frame */
+    t1 = q12 - q03;
+
+    /* Y component of the Ybody axis in World frame */
+    t2 = q22 + q00 - (1L << 30);
+    values[0] = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI;
+    if (values[0] < 0)
+        values[0] += 360;
+
+    /* Z component of the Ybody axis in World frame */
+    t3 = q23 + q01;
+    values[1] =
+        -atan2f((float) t3,
+                sqrtf((float) t1 * t1 +
+                      (float) t2 * t2)) * 180.f / (float) M_PI;
+    /* Z component of the Zbody axis in World frame */
+    t2 = q33 + q00 - (1L << 30);
+    if (t2 < 0) {
+        if (values[1] >= 0)
+            values[1] = 180.f - values[1];
+        else
+            values[1] = -180.f - values[1];
+    }
+
+    /* X component of the Xbody axis in World frame */
+    t1 = q11 + q00 - (1L << 30);
+    /* Y component of the Xbody axis in World frame */
+    t2 = q12 + q03;
+    /* Z component of the Xbody axis in World frame */
+    t3 = q13 - q02;
+    //values[2] = atan2f((float)t3,sqrtf((float)t1*t1+(float)t2*t2))*180.f/(float)M_PI;
+
+    values[2] =
+        -(atan2f((float)(q33 + q00 - (1L << 30)), (float)(q13 - q02)) *
+          180.f / (float) M_PI - 90);
+    if (values[2] >= 90)
+        values[2] = 180 - values[2];
+
+    if (values[2] < -90)
+        values[2] = -180 - values[2];
+
+    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;
+    (void) sensor_cal;
+    inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_mag,
+                           &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;
+    }
+
+    switch (use_sensor) {
+    default:
+    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;
+    }
+
+    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;
+}
+
+/** 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)
+{
+    memset(&hal_out, 0, sizeof(hal_out));
+    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/libsensors_iio/software/core/mllite/hal_outputs.h b/libsensors_iio/software/core/mllite/hal_outputs.h
new file mode 100644
index 0000000..ed4cb65
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/hal_outputs.h
@@ -0,0 +1,43 @@
+/*
+ $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_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/libsensors_iio/software/core/mllite/invensense.h b/libsensors_iio/software/core/mllite/invensense.h
new file mode 100644
index 0000000..fb8e12b
--- /dev/null
+++ b/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/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c b/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c
new file mode 100644
index 0000000..649b917
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c
@@ -0,0 +1,318 @@
+/**
+ *  @brief    Provides helpful file IO wrappers for use with sysfs.
+ *  @details  Based on Jonathan Cameron's @e iio_utils.h.
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <ctype.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <dirent.h>
+#include <errno.h>
+#include <unistd.h>
+#include "inv_sysfs_utils.h"
+
+/* General TODO list:
+ * Select more reasonable string lengths or use fseek and malloc.
+ */
+
+/**
+ *  inv_sysfs_write() - Write an integer to a file.
+ *  @filename:	Path to file.
+ *  @data:	Value to write to file.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_sysfs_write(char *filename, long data)
+{
+	FILE *fp;
+	int count;
+
+	if (!filename)
+		return -1;
+	fp = fopen(filename, "w");
+	if (!fp)
+		return -errno;
+	count = fprintf(fp, "%ld", data);
+	fclose(fp);
+	return count;
+}
+
+/**
+ *  inv_sysfs_read() - Read a string from a file.
+ *  @filename:	Path to file.
+ *  @num_bytes:	Number of bytes to read.
+ *  @data:	Data from file.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_sysfs_read(char *filename, long num_bytes, char *data)
+{
+	FILE *fp;
+	int count;
+
+	if (!filename)
+		return -1;
+	fp = fopen(filename, "r");
+	if (!fp)
+		return -errno;
+	count = fread(data, 1, num_bytes, fp);
+	fclose(fp);
+	return count;
+}
+
+/**
+ *  inv_read_buffer() - Read data from ring buffer.
+ *  @fd:	File descriptor for buffer file.
+ *  @data:	Data in hardware units.
+ *  @timestamp:	Time when data was read from device. Use NULL if unsupported.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_buffer(int fd, long *data, long long *timestamp)
+{
+	char str[35];
+	int count;
+
+	count = read(fd, str, sizeof(str));
+	if (!count)
+		return count;
+	if (!timestamp)
+		count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]);
+	else
+		count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1],
+			&data[2], timestamp);
+
+	if (count < (timestamp?4:3))
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_raw() - Read raw data.
+ *  @names:	Names of sysfs files.
+ *  @data:	Data in hardware units.
+ *  @timestamp:	Time when data was read from device. Use NULL if unsupported.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, 
+	long long *timestamp)
+{
+	char str[40];
+	int count;
+
+	count = inv_sysfs_read((char*)names->raw_data, sizeof(str), str);
+	if (count < 0)
+		return count;
+	if (!timestamp)
+		count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]);
+	else
+		count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1],
+			&data[2], timestamp);
+	if (count < (timestamp?4:3))
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_temperature_raw() - Read temperature.
+ *  @names:	Names of sysfs files.
+ *  @data:	Data in hardware units.
+ *  @timestamp:	Time when data was read from device.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data,
+	long long *timestamp)
+{
+	char str[25];
+	int count;
+
+	count = inv_sysfs_read((char*)names->temperature, sizeof(str), str);
+	if (count < 0)
+		return count;
+	count = sscanf(str, "%hd%lld", &data[0], timestamp);
+	if (count < 2)
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_fifo_rate() - Read fifo rate.
+ *  @names:	Names of sysfs files.
+ *  @data:	Fifo rate.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data)
+{
+	char str[8];
+	int count;
+
+	count = inv_sysfs_read((char*)names->fifo_rate, sizeof(str), str);
+	if (count < 0)
+		return count;
+	count = sscanf(str, "%hd", data);
+	if (count < 1)
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_power_state() - Read power state.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 if device is on.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data)
+{
+	char str[2];
+	int count;
+
+	count = inv_sysfs_read((char*)names->power_state, sizeof(str), str);
+	if (count < 0)
+		return count;
+	count = sscanf(str, "%hd", (short*)data);
+	if (count < 1)
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_scale() - Read scale.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 if device is on.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_scale(const struct inv_sysfs_names_s *names, float *data)
+{
+	char str[5];
+	int count;
+
+	count = inv_sysfs_read((char*)names->scale, sizeof(str), str);
+	if (count < 0)
+		return count;
+	count = sscanf(str, "%f", data);
+	if (count < 1)
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_temp_scale() - Read temperature scale.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 if device is on.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data)
+{
+	char str[4];
+	int count;
+
+	count = inv_sysfs_read((char*)names->temp_scale, sizeof(str), str);
+	if (count < 0)
+		return count;
+	count = sscanf(str, "%hd", data);
+	if (count < 1)
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_temp_offset() - Read temperature offset.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 if device is on.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data)
+{
+	char str[4];
+	int count;
+
+	count = inv_sysfs_read((char*)names->temp_offset, sizeof(str), str);
+	if (count < 0)
+		return count;
+	count = sscanf(str, "%hd", data);
+	if (count < 1)
+		return -EAGAIN;
+	return count;
+}
+
+/**
+ *  inv_read_q16() - Get data as q16 fixed point.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 if device is on.
+ *  @timestamp:	Time when data was read from device.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_read_q16(const struct inv_sysfs_names_s *names, long *data,
+	long long *timestamp)
+{
+	int count;
+	short raw[3];
+	float scale;
+	count = inv_read_raw(names, (long*)raw, timestamp);
+	count += inv_read_scale(names, &scale);
+	data[0] = (long)(raw[0] * (65536.f / scale));
+	data[1] = (long)(raw[1] * (65536.f / scale));
+	data[2] = (long)(raw[2] * (65536.f / scale));
+	return count;
+}
+
+/**
+ *  inv_read_q16() - Get temperature data as q16 fixed point.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 if device is on.
+ *  @timestamp:	Time when data was read from device.
+ *  Returns number of bytes read or a negative error code.
+ */
+int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data,
+	long long *timestamp)
+{
+	int count = 0;
+	short raw;
+	static short scale, offset;
+	static unsigned char first_read = 1;
+
+	if (first_read) {
+		count += inv_read_temp_scale(names, &scale);
+		count += inv_read_temp_offset(names, &offset);
+		first_read = 0;
+	}
+	count += inv_read_temperature_raw(names, &raw, timestamp);
+	data[0] = (long)((35 + ((float)(raw - offset) / scale)) * 65536.f);
+
+	return count;
+}
+
+/**
+ *  inv_write_fifo_rate() - Write fifo rate.
+ *  @names:	Names of sysfs files.
+ *  @data:	Fifo rate.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data)
+{
+	return inv_sysfs_write((char*)names->fifo_rate, (long)data);
+}
+
+/**
+ *  inv_write_buffer_enable() - Enable/disable buffer in /dev.
+ *  @names:	Names of sysfs files.
+ *  @data:	Fifo rate.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data)
+{
+	return inv_sysfs_write((char*)names->enable, (long)data);
+}
+
+/**
+ *  inv_write_power_state() - Turn device on/off.
+ *  @names:	Names of sysfs files.
+ *  @data:	1 to turn on.
+ *  Returns number of bytes written or a negative error code.
+ */
+int inv_write_power_state(const struct inv_sysfs_names_s *names, char data)
+{
+	return inv_sysfs_write((char*)names->power_state, (long)data);
+}
+
+
+
diff --git a/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h b/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h
new file mode 100644
index 0000000..45a35f9
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h
@@ -0,0 +1,84 @@
+/**
+ *  @brief    Provides helpful file IO wrappers for use with sysfs.
+ *  @details  Based on Jonathan Cameron's @e iio_utils.h.
+ */
+
+#ifndef _INV_SYSFS_UTILS_H_
+#define _INV_SYSFS_UTILS_H_
+
+/**
+ *  struct inv_sysfs_names_s - Files needed by user applications.
+ *  @buffer:		Ring buffer attached to FIFO.
+ *  @enable:		Turns on HW-to-ring buffer flow.
+ *  @raw_data:		Raw data from registers.
+ *  @temperature:	Temperature data from register.
+ *  @fifo_rate:		FIFO rate/ODR.
+ *  @power_state:	Power state (this is a five-star comment).
+ *  @fsr:		Full-scale range.
+ *  @lpf:		Digital low pass filter.
+ *  @scale:		LSBs / dps (or LSBs / Gs).
+ *  @temp_scale:	LSBs / degrees C.
+ *  @temp_offset:	Offset in LSBs.
+ */
+struct inv_sysfs_names_s {
+
+	//Sysfs for ITG3500 & MPU6050
+	const char *buffer;
+	const char *enable;
+	const char *raw_data;		//Raw Gyro data
+	const char *temperature;
+	const char *fifo_rate;
+	const char *power_state;
+	const char *fsr;
+	const char *lpf;
+	const char *scale;			//Gyro scale
+	const char *temp_scale;
+	const char *temp_offset;
+	const char *self_test;
+	//Starting Sysfs available for MPU6050 only
+	const char *accel_en;
+	const char *accel_fifo_en;
+	const char *accel_fs;
+	const char *clock_source;
+	const char *early_suspend_en;
+	const char *firmware_loaded;
+	const char *gyro_en;
+	const char *gyro_fifo_en;
+	const char *key;
+	const char *raw_accel;
+	const char *reg_dump;
+	const char *tap_on;
+	const char *dmp_firmware;
+};
+
+/* File IO. Typically won't be called directly by user application, but they'll
+ * be here for your enjoyment.
+ */
+int inv_sysfs_write(char *filename, long data);
+int inv_sysfs_read(char *filename, long num_bytes, char *data);
+
+/* Helper APIs to extract specific data. */
+int inv_read_buffer(int fd, long *data, long long *timestamp);
+int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, 
+		 long long *timestamp);
+int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data,
+			     long long *timestamp);
+int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data);
+int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data);
+int inv_read_scale(const struct inv_sysfs_names_s *names, float *data);
+int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data);
+int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data);
+int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data);
+int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data);
+int inv_write_power_state(const struct inv_sysfs_names_s *names, char data);
+
+/* Scaled data. */
+int inv_read_q16(const struct inv_sysfs_names_s *names, long *data,
+                 long long *timestamp);
+int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data,
+                      long long *timestamp);
+
+
+#endif  /* #ifndef _INV_SYSFS_UTILS_H_ */
+
+
diff --git a/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c b/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
new file mode 100644
index 0000000..f0f078c
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
@@ -0,0 +1,281 @@
+/*
+ $License:
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id:$
+ *
+ *****************************************************************************/
+
+/**
+ * @defgroup ML_LOAD_DMP
+ *
+ * @{
+ *      @file     ml_load_dmp.c
+ *      @brief    functions for writing dmp firmware.
+ */
+#include <stdio.h>
+
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-loaddmp"
+
+#include "ml_load_dmp.h"
+#include "log.h"
+#include "mlos.h"
+
+#define LOADDMP_LOG MPL_LOGI
+#define LOADDMP_LOG MPL_LOGI
+
+#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0]))
+#define DMP_CODE_SIZE 3060
+
+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, 0x01, 0x00, 0x44, 0x00, 0x05, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 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, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0xc0, 0x00, 0x00, 0x01, 0x3f, 0xff, 0xff, 0xff, 0x40, 0x00, 0x00, 0x00, 0x30, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 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, 0xb0, 0xb4, 0xba, 0x80, 0xac, 0xde, 0xf2, 0xca, 0xf1, 
+    0xb2, 0x8c, 0x02, 0xa9, 0xb6, 0x98, 0x00, 0x89, 0x0e, 0x16, 0x1e, 0xb8, 0xa9, 0xb4, 0x99, 0x2c, 
+    0x54, 0x7c, 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, 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, 0x94, 0x01, 0x29, 0x51, 0x79, 
+    /* bank # 11 */
+    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/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h b/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h
new file mode 100644
index 0000000..4d98692
--- /dev/null
+++ b/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/libsensors_iio/software/core/mllite/linux/ml_stored_data.c b/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
new file mode 100644
index 0000000..c5cf2e6
--- /dev/null
+++ b/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>
+
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-storeload"
+
+
+#include "ml_stored_data.h"
+#include "storage_manager.h"
+#include "log.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 *cal, size_t len)
+{
+    FILE *fp;
+    int bytesRead;
+    inv_error_t result = INV_SUCCESS;
+
+    fp = fopen(MLCAL_FILE,"rb");
+    if (fp == NULL) {
+        MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
+        return INV_ERROR_FILE_OPEN;
+    }
+    bytesRead = fread(cal, 1, len, fp);
+    if (bytesRead != len) {
+        MPL_LOGE("bytes read (%d) don't match requested length (%d)\n",
+                 bytesRead, len);
+        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;
+    inv_error_t result = 0;
+    size_t length;
+
+    inv_get_mpl_state_size(&length);
+    if (length <= 0) {
+        MPL_LOGE("Could not get file calibration length - "
+                 "error %d - aborting\n", result);
+        return result;
+    }
+
+    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;
+    }
+
+    result = inv_read_cal(calData, length);
+    if(result != INV_SUCCESS) {
+        MPL_LOGE("Could not load cal file - "
+                 "aborting\n");
+    }
+
+    result = inv_load_mpl_states(calData, length);
+    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 INV_SUCCESS;
+}
+
+/**
+ *  @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 INV_SUCCESS;
+}
+
+/**
+ *  @}
+ */
diff --git a/libsensors_iio/software/core/mllite/linux/ml_stored_data.h b/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
new file mode 100644
index 0000000..336f081
--- /dev/null
+++ b/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 *cal, size_t len);
+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/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
new file mode 100644
index 0000000..5636a02
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
@@ -0,0 +1,416 @@
+#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"
+
+#define CHIP_NUM 4
+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[CHIP_NUM] = {"ITG3500", "MPU6050", "MPU9150", "MPU3050"};
+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"
+
+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[100], 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(const char *name)
+{
+	if (process_sysfs_request(CMD_GET_TRIGGER_PATH, name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+
+/** 
+ *  @brief  return iio device node. If iio is not initialized, return false.
+ *          So the return must be checked to make sure the numeber is valid.
+ *  @unsigned char *name: This should be array big enough to hold the device
+ *           node. It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_iio_device_node(const char *name)
+{
+	if (process_sysfs_request(CMD_GET_DEVICE_NODE, name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
diff --git a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h
new file mode 100644
index 0000000..eb285c5
--- /dev/null
+++ b/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(const char *name);
+inv_error_t inv_get_iio_device_node(const char *name);
+
+#ifdef __cplusplus
+}
+#endif
+#endif	/* MLDMP_SYSFS_HELPER_H__ */
diff --git a/libsensors_iio/software/core/mllite/linux/mlos.h b/libsensors_iio/software/core/mllite/linux/mlos.h
new file mode 100644
index 0000000..287025f
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/linux/mlos.h
@@ -0,0 +1,98 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+#ifndef _MLOS_H
+#define _MLOS_H
+
+#ifndef __KERNEL__
+#include <stdio.h>
+#endif
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if defined(LINUX) || defined(__KERNEL__)
+typedef unsigned int 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/libsensors_iio/software/core/mllite/linux/mlos_linux.c b/libsensors_iio/software/core/mllite/linux/mlos_linux.c
new file mode 100644
index 0000000..75f062e
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/linux/mlos_linux.c
@@ -0,0 +1,194 @@
+/*
+ $License:
+    Copyright (C) 2011 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 "stdint_invensense.h"
+
+#include "mlos.h"
+#include <errno.h>
+
+
+/* -------------- */
+/* - Functions. - */
+/* -------------- */
+
+/**
+ *  @brief  Allocate space
+ *  @param  numBytes  number of bytes
+ *  @return pointer to allocated space
+**/
+void *inv_malloc(unsigned int numBytes)
+{
+    // Allocate space.
+    void *allocPtr = malloc(numBytes);
+    return allocPtr;
+}
+
+
+/**
+ *  @brief  Free allocated space
+ *  @param  ptr pointer to space to deallocate
+ *  @return error code.
+**/
+inv_error_t inv_free(void *ptr)
+{
+    // Deallocate space.
+    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 mSecs)
+{
+    usleep(mSecs*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);
+}
+
+  /**********************/
+ /** @} */ /* defgroup */
+/**********************/
+
diff --git a/libsensors_iio/software/core/mllite/message_layer.c b/libsensors_iio/software/core/mllite/message_layer.c
new file mode 100644
index 0000000..8317957
--- /dev/null
+++ b/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/libsensors_iio/software/core/mllite/message_layer.h b/libsensors_iio/software/core/mllite/message_layer.h
new file mode 100644
index 0000000..df0c0e2
--- /dev/null
+++ b/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/libsensors_iio/software/core/mllite/ml_math_func.c b/libsensors_iio/software/core/mllite/ml_math_func.c
new file mode 100644
index 0000000..86c4b41
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/ml_math_func.c
@@ -0,0 +1,660 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+/**
+ *   @defgroup  ML_MATH_FUNC ml_math_func
+ *   @brief     Motion Library - Math Functions
+ *              Common math functions the Motion Library
+ *
+ *   @{
+ *       @file ml_math_func.c
+ *       @brief Math Functions.
+ */
+
+#include "mlmath.h"
+#include "ml_math_func.h"
+#include "mlinclude.h"
+#include <string.h>
+
+/** @internal
+ * Does the cross product of compass by gravity, then converts that
+ * to the world frame using the quaternion, then computes the angle that
+ * is made.
+ *
+ * @param[in] compass Compass Vector (Body Frame), length 3
+ * @param[in] grav Gravity Vector (Body Frame), length 3
+ * @param[in] quat Quaternion, Length 4
+ * @return Angle Cross Product makes after quaternion rotation.
+ */
+float inv_compass_angle(const long *compass, const long *grav, const float *quat)
+{
+    float cgcross[4], q1[4], q2[4], qi[4];
+    float angW;
+
+    // Compass cross Gravity
+    cgcross[0] = 0.f;
+    cgcross[1] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1];
+    cgcross[2] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2];
+    cgcross[3] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0];
+
+    // Now convert cross product into world frame
+    inv_q_multf(quat, cgcross, q1);
+    inv_q_invertf(quat, qi);
+    inv_q_multf(q1, qi, q2);
+
+    // Protect against atan2 of 0,0
+    if ((q2[2] == 0.f) && (q2[1] == 0.f))
+        return 0.f;
+
+    // This is the unfiltered heading correction
+    angW = -atan2f(q2[2], q2[1]);
+    return angW;
+}
+
+/**
+ *  @brief  The gyro data magnitude squared :
+ *          (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT.
+ * @param[in] gyro Gyro data scaled with 1 dps = 2^16
+ *  @return the computed magnitude squared output of the gyroscope.
+ */
+unsigned long inv_get_gyro_sum_of_sqr(const long *gyro)
+{
+    unsigned long gmag = 0;
+    long temp;
+    int kk;
+
+    for (kk = 0; kk < 3; ++kk) {
+        temp = gyro[kk] >> (16 - (GYRO_MAG_SQR_SHIFT / 2));
+        gmag += temp * temp;
+    }
+
+    return gmag;
+}
+
+/** Performs a multiply and shift by 29. These are good functions to write in assembly on
+ * with devices with small memory where you want to get rid of the long long which some
+ * assemblers don't handle well
+ * @param[in] a
+ * @param[in] b
+ * @return ((long long)a*b)>>29
+*/
+long inv_q29_mult(long a, long b)
+{
+#ifdef UMPL_ELIMINATE_64BIT
+    long result;
+    result = (long)((float)a * b / (1L << 29));
+    return result;
+#else
+    long long temp;
+    long result;
+    temp = (long long)a * b;
+    result = (long)(temp >> 29);
+    return result;
+#endif
+}
+
+/** Performs a multiply and shift by 30. These are good functions to write in assembly on
+ * with devices with small memory where you want to get rid of the long long which some
+ * assemblers don't handle well
+ * @param[in] a
+ * @param[in] b
+ * @return ((long long)a*b)>>30
+*/
+long inv_q30_mult(long a, long b)
+{
+#ifdef UMPL_ELIMINATE_64BIT
+    long result;
+    result = (long)((float)a * b / (1L << 30));
+    return result;
+#else
+    long long temp;
+    long result;
+    temp = (long long)a * b;
+    result = (long)(temp >> 30);
+    return result;
+#endif
+}
+
+#ifndef UMPL_ELIMINATE_64BIT
+long inv_q30_div(long a, long b)
+{
+    long long temp;
+    long result;
+    temp = (((long long)a) << 30) / b;
+    result = (long)temp;
+    return result;
+}
+#endif
+
+/** Performs a multiply and shift by shift. These are good functions to write
+ * in assembly on with devices with small memory where you want to get rid of
+ * the long long which some assemblers don't handle well
+ * @param[in] a First multicand
+ * @param[in] b Second multicand
+ * @param[in] shift Shift amount after multiplying
+ * @return ((long long)a*b)<<shift
+*/
+#ifndef UMPL_ELIMINATE_64BIT
+long inv_q_shift_mult(long a, long b, int shift)
+{
+    long result;
+    result = (long)(((long long)a * b) >> shift);
+    return result;
+}
+#endif
+
+/** Performs a fixed point quaternion multiply.
+* @param[in] q1 First Quaternion Multicand, length 4. 1.0 scaled
+*            to 2^30
+* @param[in] q2 Second Quaternion Multicand, length 4. 1.0 scaled
+*            to 2^30
+* @param[out] qProd Product after quaternion multiply. Length 4.
+*             1.0 scaled to 2^30.
+*/
+void inv_q_mult(const long *q1, const long *q2, long *qProd)
+{
+    INVENSENSE_FUNC_START;
+    qProd[0] = inv_q30_mult(q1[0], q2[0]) - inv_q30_mult(q1[1], q2[1]) -
+               inv_q30_mult(q1[2], q2[2]) - inv_q30_mult(q1[3], q2[3]);
+
+    qProd[1] = inv_q30_mult(q1[0], q2[1]) + inv_q30_mult(q1[1], q2[0]) +
+               inv_q30_mult(q1[2], q2[3]) - inv_q30_mult(q1[3], q2[2]);
+
+    qProd[2] = inv_q30_mult(q1[0], q2[2]) - inv_q30_mult(q1[1], q2[3]) +
+               inv_q30_mult(q1[2], q2[0]) + inv_q30_mult(q1[3], q2[1]);
+
+    qProd[3] = inv_q30_mult(q1[0], q2[3]) + inv_q30_mult(q1[1], q2[2]) -
+               inv_q30_mult(q1[2], q2[1]) + inv_q30_mult(q1[3], q2[0]);
+}
+
+/** Performs a fixed point quaternion addition.
+* @param[in] q1 First Quaternion term, length 4. 1.0 scaled
+*            to 2^30
+* @param[in] q2 Second Quaternion term, length 4. 1.0 scaled
+*            to 2^30
+* @param[out] qSum Sum after quaternion summation. Length 4.
+*             1.0 scaled to 2^30.
+*/
+void inv_q_add(long *q1, long *q2, long *qSum)
+{
+    INVENSENSE_FUNC_START;
+    qSum[0] = q1[0] + q2[0];
+    qSum[1] = q1[1] + q2[1];
+    qSum[2] = q1[2] + q2[2];
+    qSum[3] = q1[3] + q2[3];
+}
+
+void inv_vector_normalize(long *vec, int length)
+{
+    INVENSENSE_FUNC_START;
+    double normSF = 0;
+    int ii;
+    for (ii = 0; ii < length; ii++) {
+        normSF +=
+            inv_q30_to_double(vec[ii]) * inv_q30_to_double(vec[ii]);
+    }
+    if (normSF > 0) {
+        normSF = 1 / sqrt(normSF);
+        for (ii = 0; ii < length; ii++) {
+            vec[ii] = (int)((double)vec[ii] * normSF);
+        }
+    } else {
+        vec[0] = 1073741824L;
+        for (ii = 1; ii < length; ii++) {
+            vec[ii] = 0;
+        }
+    }
+}
+
+void inv_q_normalize(long *q)
+{
+    INVENSENSE_FUNC_START;
+    inv_vector_normalize(q, 4);
+}
+
+void inv_q_invert(const long *q, long *qInverted)
+{
+    INVENSENSE_FUNC_START;
+    qInverted[0] = q[0];
+    qInverted[1] = -q[1];
+    qInverted[2] = -q[2];
+    qInverted[3] = -q[3];
+}
+
+double quaternion_to_rotation_angle(const long *quat) {
+    double quat0 = (double )quat[0] / 1073741824;
+    if (quat0 > 1.0f) {
+        quat0 = 1.0;
+    } else if (quat0 < -1.0f) {
+        quat0 = -1.0;
+    }
+
+    return acos(quat0)*2*180/M_PI;
+}
+
+/** Rotates a 3-element vector by Rotation defined by Q
+*/
+void inv_q_rotate(const long *q, const long *in, long *out)
+{
+    long q_temp1[4], q_temp2[4];
+    long in4[4], out4[4];
+
+    // Fixme optimize
+    in4[0] = 0;
+    memcpy(&in4[1], in, 3 * sizeof(long));
+    inv_q_mult(q, in4, q_temp1);
+    inv_q_invert(q, q_temp2);
+    inv_q_mult(q_temp1, q_temp2, out4);
+    memcpy(out, &out4[1], 3 * sizeof(long));
+}
+
+void inv_q_multf(const float *q1, const float *q2, float *qProd)
+{
+    INVENSENSE_FUNC_START;
+    qProd[0] =
+        (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]);
+    qProd[1] =
+        (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]);
+    qProd[2] =
+        (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]);
+    qProd[3] =
+        (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]);
+}
+
+void inv_q_addf(const float *q1, const float *q2, float *qSum)
+{
+    INVENSENSE_FUNC_START;
+    qSum[0] = q1[0] + q2[0];
+    qSum[1] = q1[1] + q2[1];
+    qSum[2] = q1[2] + q2[2];
+    qSum[3] = q1[3] + q2[3];
+}
+
+void inv_q_normalizef(float *q)
+{
+    INVENSENSE_FUNC_START;
+    float normSF = 0;
+    float xHalf = 0;
+    normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+    if (normSF < 2) {
+        xHalf = 0.5f * normSF;
+        normSF = normSF * (1.5f - xHalf * normSF * normSF);
+        normSF = normSF * (1.5f - xHalf * normSF * normSF);
+        normSF = normSF * (1.5f - xHalf * normSF * normSF);
+        normSF = normSF * (1.5f - xHalf * normSF * normSF);
+        q[0] *= normSF;
+        q[1] *= normSF;
+        q[2] *= normSF;
+        q[3] *= normSF;
+    } else {
+        q[0] = 1.0;
+        q[1] = 0.0;
+        q[2] = 0.0;
+        q[3] = 0.0;
+    }
+    normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+}
+
+/** Performs a length 4 vector normalization with a square root.
+* @param[in,out] q vector to normalize. Returns [1,0,0,0] is magnitude is zero.
+*/
+void inv_q_norm4(float *q)
+{
+    float mag;
+    mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+    if (mag) {
+        q[0] /= mag;
+        q[1] /= mag;
+        q[2] /= mag;
+        q[3] /= mag;
+    } else {
+        q[0] = 1.f;
+        q[1] = 0.f;
+        q[2] = 0.f;
+        q[3] = 0.f;
+    }
+}
+
+void inv_q_invertf(const float *q, float *qInverted)
+{
+    INVENSENSE_FUNC_START;
+    qInverted[0] = q[0];
+    qInverted[1] = -q[1];
+    qInverted[2] = -q[2];
+    qInverted[3] = -q[3];
+}
+
+/**
+ * Converts a quaternion to a rotation matrix.
+ * @param[in] quat 4-element quaternion in fixed point. One is 2^30.
+ * @param[out] rot Rotation matrix in fixed point. One is 2^30. The
+ *             First 3 elements of the rotation matrix, represent
+ *             the first row of the matrix. Rotation matrix multiplied
+ *             by a 3 element column vector transform a vector from Body
+ *             to World.
+ */
+void inv_quaternion_to_rotation(const long *quat, long *rot)
+{
+    rot[0] =
+        inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0],
+                quat[0]) -
+        1073741824L;
+    rot[1] =
+        inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]);
+    rot[2] =
+        inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]);
+    rot[3] =
+        inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]);
+    rot[4] =
+        inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0],
+                quat[0]) -
+        1073741824L;
+    rot[5] =
+        inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]);
+    rot[6] =
+        inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]);
+    rot[7] =
+        inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]);
+    rot[8] =
+        inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0],
+                quat[0]) -
+        1073741824L;
+}
+
+/**
+ * Converts a quaternion to a rotation vector. A rotation vector is
+ * a method to represent a 4-element quaternion vector in 3-elements.
+ * To get the quaternion from the 3-elements, The last 3-elements of
+ * the quaternion will be the given rotation vector. The first element
+ * of the quaternion will be the positive value that will be required
+ * to make the magnitude of the quaternion 1.0 or 2^30 in fixed point units.
+ * @param[in] quat 4-element quaternion in fixed point. One is 2^30.
+ * @param[out] rot Rotation vector in fixed point. One is 2^30.
+ */
+void inv_quaternion_to_rotation_vector(const long *quat, long *rot)
+{
+    rot[0] = quat[1];
+    rot[1] = quat[2];
+    rot[2] = quat[3];
+
+    if (quat[0] < 0.0) {
+        rot[0] = -rot[0];
+        rot[1] = -rot[1];
+        rot[2] = -rot[2];
+    }
+}
+
+/** Converts a 32-bit long to a big endian byte stream */
+unsigned char *inv_int32_to_big8(long x, unsigned char *big8)
+{
+    big8[0] = (unsigned char)((x >> 24) & 0xff);
+    big8[1] = (unsigned char)((x >> 16) & 0xff);
+    big8[2] = (unsigned char)((x >> 8) & 0xff);
+    big8[3] = (unsigned char)(x & 0xff);
+    return big8;
+}
+
+/** Converts a big endian byte stream into a 32-bit long */
+long inv_big8_to_int32(const unsigned char *big8)
+{
+    long x;
+    x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8)
+        | ((long)big8[3]);
+    return x;
+}
+
+/** Converts a big endian byte stream into a 16-bit integer (short) */
+short inv_big8_to_int16(const unsigned char *big8)
+{
+    short x;
+    x = ((short)big8[0] << 8) | ((short)big8[1]);
+    return x;
+}
+
+/** Converts a little endian byte stream into a 16-bit integer (short) */
+short inv_little8_to_int16(const unsigned char *little8)
+{
+    short x;
+    x = ((short)little8[1] << 8) | ((short)little8[0]);
+    return x;
+}
+
+/** Converts a 16-bit short to a big endian byte stream */
+unsigned char *inv_int16_to_big8(short x, unsigned char *big8)
+{
+    big8[0] = (unsigned char)((x >> 8) & 0xff);
+    big8[1] = (unsigned char)(x & 0xff);
+    return big8;
+}
+
+void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y)
+{
+    int k, l, i, j;
+    for (i = 0, k = 0; i < *n; i++, k++) {
+        for (j = 0, l = 0; j < *n; j++, l++) {
+            if (i == x)
+                i++;
+            if (j == y)
+                j++;
+            *(b + 6 * k + l) = *(a + 6 * i + j);
+        }
+    }
+    *n = *n - 1;
+}
+
+void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y)
+{
+    int k, l, i, j;
+    for (i = 0, k = 0; i < *n; i++, k++) {
+        for (j = 0, l = 0; j < *n; j++, l++) {
+            if (i == x)
+                i++;
+            if (j == y)
+                j++;
+            *(b + 6 * k + l) = *(a + 6 * i + j);
+        }
+    }
+    *n = *n - 1;
+}
+
+float inv_matrix_det(float *p, int *n)
+{
+    float d[6][6], sum = 0;
+    int i, j, m;
+    m = *n;
+    if (*n == 2)
+        return (*p ** (p + 7) - *(p + 1) ** (p + 6));
+    for (i = 0, j = 0; j < m; j++) {
+        *n = m;
+        inv_matrix_det_inc(p, &d[0][0], n, i, j);
+        sum =
+            sum + *(p + 6 * i + j) * SIGNM(i +
+                                            j) *
+            inv_matrix_det(&d[0][0], n);
+    }
+
+    return (sum);
+}
+
+double inv_matrix_detd(double *p, int *n)
+{
+    double d[6][6], sum = 0;
+    int i, j, m;
+    m = *n;
+    if (*n == 2)
+        return (*p ** (p + 7) - *(p + 1) ** (p + 6));
+    for (i = 0, j = 0; j < m; j++) {
+        *n = m;
+        inv_matrix_det_incd(p, &d[0][0], n, i, j);
+        sum =
+            sum + *(p + 6 * i + j) * SIGNM(i +
+                                            j) *
+            inv_matrix_detd(&d[0][0], n);
+    }
+
+    return (sum);
+}
+
+/** Wraps angle from (-M_PI,M_PI]
+ * @param[in] ang Angle in radians to wrap
+ * @return Wrapped angle from (-M_PI,M_PI]
+ */
+float inv_wrap_angle(float ang)
+{
+    if (ang > M_PI)
+        return ang - 2 * (float)M_PI;
+    else if (ang <= -(float)M_PI)
+        return ang + 2 * (float)M_PI;
+    else
+        return ang;
+}
+
+/** Finds the minimum angle difference ang1-ang2 such that difference
+ * is between [-M_PI,M_PI]
+ * @param[in] ang1
+ * @param[in] ang2
+ * @return angle difference ang1-ang2
+ */
+float inv_angle_diff(float ang1, float ang2)
+{
+    float d;
+    ang1 = inv_wrap_angle(ang1);
+    ang2 = inv_wrap_angle(ang2);
+    d = ang1 - ang2;
+    if (d > M_PI)
+        d -= 2 * (float)M_PI;
+    else if (d < -(float)M_PI)
+        d += 2 * (float)M_PI;
+    return d;
+}
+
+/** bernstein hash, derived from public domain source */
+uint32_t inv_checksum(const unsigned char *str, int len)
+{
+    uint32_t hash = 5381;
+    int i, c;
+
+    for (i = 0; i < len; i++) {
+        c = *(str + i);
+        hash = ((hash << 5) + hash) + c;	/* hash * 33 + c */
+    }
+
+    return hash;
+}
+
+static unsigned short inv_row_2_scale(const signed char *row)
+{
+    unsigned short b;
+
+    if (row[0] > 0)
+        b = 0;
+    else if (row[0] < 0)
+        b = 4;
+    else if (row[1] > 0)
+        b = 1;
+    else if (row[1] < 0)
+        b = 5;
+    else if (row[2] > 0)
+        b = 2;
+    else if (row[2] < 0)
+        b = 6;
+    else
+        b = 7;		// error
+    return b;
+}
+
+
+/** Converts an orientation matrix made up of 0,+1,and -1 to a scalar representation.
+* @param[in] mtx Orientation matrix to convert to a scalar.
+* @return Description of orientation matrix. The lowest 2 bits (0 and 1) represent the column the one is on for the
+* first row, with the bit number 2 being the sign. The next 2 bits (3 and 4) represent
+* the column the one is on for the second row with bit number 5 being the sign.
+* The next 2 bits (6 and 7) represent the column the one is on for the third row with
+* bit number 8 being the sign. In binary the identity matrix would therefor be:
+* 010_001_000 or 0x88 in hex.
+*/
+unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
+{
+
+    unsigned short scalar;
+
+    /*
+       XYZ  010_001_000 Identity Matrix
+       XZY  001_010_000
+       YXZ  010_000_001
+       YZX  000_010_001
+       ZXY  001_000_010
+       ZYX  000_001_010
+     */
+
+    scalar = inv_row_2_scale(mtx);
+    scalar |= inv_row_2_scale(mtx + 3) << 3;
+    scalar |= inv_row_2_scale(mtx + 6) << 6;
+
+
+    return scalar;
+}
+
+/** Uses the scalar orientation value to convert from chip frame to body frame
+* @param[in] orientation A scalar that represent how to go from chip to body frame
+* @param[in] input Input vector, length 3
+* @param[out] output Output vector, length 3
+*/
+void inv_convert_to_body(unsigned short orientation, const long *input, long *output)
+{
+    output[0] = input[orientation      & 0x03] * SIGNSET(orientation & 0x004);
+    output[1] = input[(orientation>>3) & 0x03] * SIGNSET(orientation & 0x020);
+    output[2] = input[(orientation>>6) & 0x03] * SIGNSET(orientation & 0x100);
+}
+
+/** Uses the scalar orientation value to convert from body frame to chip frame
+* @param[in] orientation A scalar that represent how to go from chip to body frame
+* @param[in] input Input vector, length 3
+* @param[out] output Output vector, length 3
+*/
+void inv_convert_to_chip(unsigned short orientation, const long *input, long *output)
+{
+    output[orientation & 0x03]      = input[0] * SIGNSET(orientation & 0x004);
+    output[(orientation>>3) & 0x03] = input[1] * SIGNSET(orientation & 0x020);
+    output[(orientation>>6) & 0x03] = input[2] * SIGNSET(orientation & 0x100);
+}
+
+
+/** Uses the scalar orientation value to convert from chip frame to body frame and
+* apply appropriate scaling.
+* @param[in] orientation A scalar that represent how to go from chip to body frame
+* @param[in] sensitivity Sensitivity scale
+* @param[in] input Input vector, length 3
+* @param[out] output Output vector, length 3
+*/
+void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output)
+{
+    output[0] = inv_q30_mult(input[orientation & 0x03] *
+                             SIGNSET(orientation & 0x004), sensitivity);
+    output[1] = inv_q30_mult(input[(orientation>>3) & 0x03] *
+                             SIGNSET(orientation & 0x020), sensitivity);
+    output[2] = inv_q30_mult(input[(orientation>>6) & 0x03] *
+                             SIGNSET(orientation & 0x100), sensitivity);
+}
+
+/** find a norm for a vector
+* @param[in] a vector [3x1]
+* @param[out] output the norm of the input vector
+*/
+double inv_vector_norm(const float *x)
+{
+    return sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]);
+}
+/**
+ * @}
+ */
diff --git a/libsensors_iio/software/core/mllite/ml_math_func.h b/libsensors_iio/software/core/mllite/ml_math_func.h
new file mode 100644
index 0000000..916de0a
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/ml_math_func.h
@@ -0,0 +1,108 @@
+/*
+ $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
+
+    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);
+
+#ifdef __cplusplus
+}
+#endif
+#endif // INVENSENSE_INV_MATH_FUNC_H__
diff --git a/libsensors_iio/software/core/mllite/mlmath.c b/libsensors_iio/software/core/mllite/mlmath.c
new file mode 100644
index 0000000..5eb4264
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/mlmath.c
@@ -0,0 +1,68 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+/*******************************************************************************
+ *
+ * $Id: mlmath.c 5629 2011-06-11 03:13:08Z mcaramello $
+ *
+ *******************************************************************************/
+
+#include <math.h>
+
+double ml_asin(double x)
+{
+    return asin(x);
+}
+
+double ml_atan(double x)
+{
+    return atan(x);
+}
+
+double ml_atan2(double x, double y)
+{
+    return atan2(x, y);
+}
+
+double ml_log(double x)
+{
+    return log(x);
+}
+
+double ml_sqrt(double x)
+{
+    return sqrt(x);
+}
+
+double ml_ceil(double x)
+{
+    return ceil(x);
+}
+
+double ml_floor(double x)
+{
+    return floor(x);
+}
+
+double ml_cos(double x)
+{
+    return cos(x);
+}
+
+double ml_sin(double x)
+{
+    return sin(x);
+}
+
+double ml_acos(double x)
+{
+    return acos(x);
+}
+
+double ml_pow(double x, double y)
+{
+    return pow(x, y);
+}
diff --git a/libsensors_iio/software/core/mllite/mpl.c b/libsensors_iio/software/core/mllite/mpl.c
new file mode 100644
index 0000000..231cbfd
--- /dev/null
+++ b/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.0.1";

+

+/**

+ *  @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/libsensors_iio/software/core/mllite/mpl.h b/libsensors_iio/software/core/mllite/mpl.h
new file mode 100644
index 0000000..a6b5ac7
--- /dev/null
+++ b/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/libsensors_iio/software/core/mllite/results_holder.c b/libsensors_iio/software/core/mllite/results_holder.c
new file mode 100644
index 0000000..97ffdec
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/results_holder.c
@@ -0,0 +1,500 @@
+/*
+ $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 "results_holder.h"
+#include "log.h"
+#include "ml_math_func.h"
+#include "mlmath.h"
+#include "start_manager.h"
+#include "data_builder.h"
+#include "message_layer.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 */
+    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 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/libsensors_iio/software/core/mllite/results_holder.h b/libsensors_iio/software/core/mllite/results_holder.h
new file mode 100644
index 0000000..a60d7f0
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/results_holder.h
@@ -0,0 +1,77 @@
+/*
+ $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);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_RESULTS_HOLDER_H__
diff --git a/libsensors_iio/software/core/mllite/start_manager.c b/libsensors_iio/software/core/mllite/start_manager.c
new file mode 100644
index 0000000..fb758e7
--- /dev/null
+++ b/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/libsensors_iio/software/core/mllite/start_manager.h b/libsensors_iio/software/core/mllite/start_manager.h
new file mode 100644
index 0000000..899e3f5
--- /dev/null
+++ b/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/libsensors_iio/software/core/mllite/storage_manager.c b/libsensors_iio/software/core/mllite/storage_manager.c
new file mode 100644
index 0000000..4b92bfc
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/storage_manager.c
@@ -0,0 +1,200 @@
+/*
+ $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 "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/libsensors_iio/software/core/mllite/storage_manager.h b/libsensors_iio/software/core/mllite/storage_manager.h
new file mode 100644
index 0000000..7255591
--- /dev/null
+++ b/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/libsensors_iio/software/core/mpl/accel_auto_cal.h b/libsensors_iio/software/core/mpl/accel_auto_cal.h
new file mode 100644
index 0000000..5a53213
--- /dev/null
+++ b/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/libsensors_iio/software/core/mpl/adv_func.h b/libsensors_iio/software/core/mpl/adv_func.h
new file mode 100644
index 0000000..3f8595f
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/adv_func.h
@@ -0,0 +1,30 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_ADVFUNC_H__
+#define MLDMP_ADVFUNC_H__
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    float inv_compass_angle(const long *compass, const long *grav, const float *quat);
+    inv_error_t inv_set_dmp_quaternion(long *q);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_ADVFUNC_H__
diff --git a/libsensors_iio/software/core/mpl/authenticate.h b/libsensors_iio/software/core/mpl/authenticate.h
new file mode 100644
index 0000000..d7c803b
--- /dev/null
+++ b/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/libsensors_iio/software/core/mpl/auto_calibration.h b/libsensors_iio/software/core/mpl/auto_calibration.h
new file mode 100644
index 0000000..3dd9827
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/auto_calibration.h
@@ -0,0 +1,33 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_IN_USE_AUTO_CALIBRATION_H__
+#define MLDMP_IN_USE_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_in_use_auto_calibration_is_enabled(unsigned char *is_enabled);
+inv_error_t inv_init_in_use_auto_calibration(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_IN_USE_AUTO_CALIBRATION_H__
+
diff --git a/libsensors_iio/software/core/mpl/build/android/libmplmpu.so b/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
new file mode 100644
index 0000000..116b618
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
diff --git a/libsensors_iio/software/core/mpl/build/android/shared.mk b/libsensors_iio/software/core/mpl/build/android/shared.mk
new file mode 100644
index 0000000..79cf9c1
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/build/android/shared.mk
@@ -0,0 +1,92 @@
+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 += -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 += -nostdlib 
+LFLAGS += -fpic 
+LFLAGS += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.xsc 
+LFLAGS += -Wl,--gc-sections 
+LFLAGS += -Wl,--no-whole-archive 
+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/libsensors_iio/software/core/mpl/build/android/static.mk b/libsensors_iio/software/core/mpl/build/android/static.mk
new file mode 100644
index 0000000..6ad45de
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/build/android/static.mk
@@ -0,0 +1,110 @@
+MLLITE_LIB_NAME = mllite
+LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(STATIC_LIB_EXT)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+OBJFOLDER = $(CURDIR)/obj
+
+CROSS = arm-none-linux-gnueabi-
+COMP= $(CROSS)gcc
+LINK= $(CROSS)ar cr
+
+MLLITE_DIR = $(MLSDK_ROOT)/mllite
+MPL_DIR = $(MLSDK_ROOT)/mldmp
+MLPLATFORM_DIR = $(MLSDK_ROOT)/platform
+
+include $(MLSDK_ROOT)/Android-common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += -Wall -fpic
+CFLAGS += -mthumb-interwork -fno-exceptions -ffunction-sections -funwind-tables -fstack-protector -fno-short-enums -fmessage-length=0
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT -DLINUX -DANDROID
+CFLAGS += -I$(MLLITE_DIR) 
+CFLAGS += -I$(MLPLATFORM_DIR)/include
+CFLAGS += -I$(MLSDK_ROOT)/mlutils 
+CFLAGS += -I$(MLSDK_ROOT)/mlapps/common
+CFLAGS += $(MLSDK_INCLUDES)
+CFLAGS += $(MLSDK_DEFINES)
+
+VPATH += $(MLLITE_DIR) 
+VPATH += $(MLSDK_ROOT)/mlutils
+VPATH += $(MLLITE_DIR)/accel 
+VPATH += $(MLLITE_DIR)/compass 
+VPATH += $(MLLITE_DIR)/pressure 
+VPATH += $(MLLITE_DIR)/mlapps/common
+
+####################################################################################################
+## sources
+
+ML_LIBS  = $(MLPLATFORM_DIR)/linux/$(LIB_PREFIX)$(MLPLATFORM_LIB_NAME).$(STATIC_LIB_EXT)
+
+ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_mpu.c
+ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_init_linux.c
+ML_SOURCES += $(MLLITE_DIR)/accel.c
+ML_SOURCES += $(MLLITE_DIR)/compass.c
+ML_SOURCES += $(MLLITE_DIR)/compass_supervisor.c
+ML_SOURCES += $(MLLITE_DIR)/compass_supervisor_adv_callbacks.c
+ML_SOURCES += $(MLLITE_DIR)/key0_96.c
+ML_SOURCES += $(MLLITE_DIR)/pressure.c
+ML_SOURCES += $(MLLITE_DIR)/ml.c
+ML_SOURCES += $(MLLITE_DIR)/ml_invobj.c
+ML_SOURCES += $(MLLITE_DIR)/ml_init.c
+ML_SOURCES += $(MLLITE_DIR)/mlarray_lite.c
+ML_SOURCES += $(MLLITE_DIR)/mlarray_adv.c
+ML_SOURCES += $(MLLITE_DIR)/mlarray_legacy.c
+ML_SOURCES += $(MLLITE_DIR)/mlBiasNoMotion.c
+ML_SOURCES += $(MLLITE_DIR)/mlFIFO.c
+ML_SOURCES += $(MLLITE_DIR)/mlFIFOHW.c
+ML_SOURCES += $(MLLITE_DIR)/mlMathFunc.c
+ML_SOURCES += $(MLLITE_DIR)/mlcontrol.c
+ML_SOURCES += $(MLLITE_DIR)/mldl.c
+ML_SOURCES += $(MLLITE_DIR)/mldmp.c
+ML_SOURCES += $(MLLITE_DIR)/dmpDefault.c
+ML_SOURCES += $(MLLITE_DIR)/mlstates.c
+ML_SOURCES += $(MLLITE_DIR)/mlsupervisor.c
+ML_SOURCES += $(MLLITE_DIR)/ml_stored_data.c
+ML_SOURCES += $(MLLITE_DIR)/ustore_manager.c
+ML_SOURCES += $(MLLITE_DIR)/ustore_mlsl_io.c
+ML_SOURCES += $(MLLITE_DIR)/ustore_adv_fusion_delegate.c
+ML_SOURCES += $(MLLITE_DIR)/ustore_lite_fusion_delegate.c
+ML_SOURCES += $(MLLITE_DIR)/temp_comp_legacy.c
+ML_SOURCES += $(MLLITE_DIR)/mlSetGyroBias.c
+ML_SOURCES += $(MLSDK_ROOT)/mlutils/checksum.c
+ML_SOURCES += $(MLSDK_ROOT)/mlutils/mputest.c
+ML_SOURCES += $(MLLITE_DIR)/mldl_print_cfg.c
+ifeq ($(MPU_NAME),MPU3050)
+ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu3050test.c
+else
+ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu6050test.c
+endif
+
+ML_OBJS := $(addsuffix .o,$(ML_SOURCES))
+ML_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(ML_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all clean cleanall
+
+all: $(LIBRARY) $(MK_NAME)
+
+$(LIBRARY) : $(OBJFOLDER) $(ML_OBJS_DST) $(ML_LIBS) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(ML_OBJS_DST)\n")
+	$(LINK) $(LIBRARY) $(ML_OBJS_DST)
+	$(CROSS)ranlib $(LIBRARY)
+
+$(OBJFOLDER) : 
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(ML_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(CFLAGS) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(MLSDK_INCLUDES) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(LIBRARY) $(OBJFOLDER)
+
diff --git a/libsensors_iio/software/core/mpl/build/filelist.mk b/libsensors_iio/software/core/mpl/build/filelist.mk
new file mode 100644
index 0000000..e18003a
--- /dev/null
+++ b/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/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h b/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h
new file mode 100644
index 0000000..4f01fc2
--- /dev/null
+++ b/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/libsensors_iio/software/core/mpl/compass_fit.h b/libsensors_iio/software/core/mpl/compass_fit.h
new file mode 100644
index 0000000..be3cce8
--- /dev/null
+++ b/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/libsensors_iio/software/core/mpl/compass_vec_cal.h b/libsensors_iio/software/core/mpl/compass_vec_cal.h
new file mode 100644
index 0000000..a3e044c
--- /dev/null
+++ b/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/libsensors_iio/software/core/mpl/fast_no_motion.h b/libsensors_iio/software/core/mpl/fast_no_motion.h
new file mode 100644
index 0000000..2a33093
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/fast_no_motion.h
@@ -0,0 +1,44 @@
+/*

+ $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, float *param);
+    void inv_get_fast_nomot_compass_param(long *cntr, float *param);
+    void inv_set_fast_nomot_accel_threshold(float thresh);
+    void inv_set_fast_nomot_compass_threshold(float thresh);
+    void int_set_fast_nomot_gyro_threshold(float thresh);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_FAST_NO_MOTION_H__
+
diff --git a/libsensors_iio/software/core/mpl/fusion_9axis.h b/libsensors_iio/software/core/mpl/fusion_9axis.h
new file mode 100644
index 0000000..616694a
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/fusion_9axis.h
@@ -0,0 +1,35 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_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);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_FUSION9AXIS_H__
diff --git a/libsensors_iio/software/core/mpl/gyro_tc.h b/libsensors_iio/software/core/mpl/gyro_tc.h
new file mode 100644
index 0000000..8f1d50e
--- /dev/null
+++ b/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/libsensors_iio/software/core/mpl/heading_from_gyro.h b/libsensors_iio/software/core/mpl/heading_from_gyro.h
new file mode 100644
index 0000000..09ecc42
--- /dev/null
+++ b/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/libsensors_iio/software/core/mpl/interpolator.h b/libsensors_iio/software/core/mpl/interpolator.h
new file mode 100644
index 0000000..5eb571d
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/interpolator.h
@@ -0,0 +1,103 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+#ifndef INTERPOLATOR_H
+#define INTERPOLATOR_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+//#include "mltypes.h"
+
+    /* ------------ */
+    /* - Defines. - */
+    /* ------------ */
+
+#define MAX_INTERPOLATION (20)
+
+typedef struct  {
+    long x[2];
+    long y[4];
+} tInterpolate2;
+typedef struct  {
+    long x[2];
+    long y[6];
+} tInterpolate3;
+typedef struct  {
+    long x[5];
+    long y[10];
+    int idx1;
+} tInterpolate5;
+typedef struct {
+    tInterpolate2 state1;
+    tInterpolate2 state2;
+} tInterpolate4;
+typedef struct {
+    tInterpolate3 state1;
+    tInterpolate2 state2;
+} tInterpolate6;
+typedef struct {
+    tInterpolate2 state1;
+    tInterpolate4 state2;
+} tInterpolate8;
+typedef struct {
+    tInterpolate3 state1;
+    tInterpolate3 state2;
+} tInterpolate9;
+typedef struct {
+    tInterpolate5 state1;
+    tInterpolate2 state2;
+} tInterpolate10;
+typedef struct {
+    tInterpolate4 state1;
+    tInterpolate3 state2;
+} tInterpolate12;
+typedef struct {
+    tInterpolate5 state1;
+    tInterpolate3 state2;
+} tInterpolate15;
+typedef struct {
+    tInterpolate2 state1;
+    tInterpolate8 state2;
+} tInterpolate16;
+typedef struct {
+    tInterpolate9 state1;
+    tInterpolate2 state2;
+} tInterpolate18;
+typedef struct {
+    tInterpolate10 state1;
+    tInterpolate2 state2;
+} tInterpolate20;
+
+typedef  union {
+    tInterpolate2 u2;
+    tInterpolate3 u3;
+    tInterpolate4 u4;
+    tInterpolate5 u5;
+    tInterpolate6 u6;
+    tInterpolate8 u8;
+    tInterpolate9 u9;
+    tInterpolate10 u10;
+    tInterpolate12 u12;
+    tInterpolate15 u15;
+    tInterpolate16 u16;
+    tInterpolate18 u18;
+    tInterpolate20 u20;
+} tInterpolateState;
+
+    /* --------------------- */
+    /* - Function p-types. - */
+    /* --------------------- */
+int inv_get_interp_amount( int x );
+int inv_interpolate( int amount, long input, long *output, tInterpolateState *state );
+long inv_fxmult( long x, long y );
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* INTERPOLATOR_H */
diff --git a/libsensors_iio/software/core/mpl/inv_log.h b/libsensors_iio/software/core/mpl/inv_log.h
new file mode 100644
index 0000000..972844b
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/inv_log.h
@@ -0,0 +1,7 @@
+#include "mltypes.h"
+#ifndef INV_INV_LOG_H__
+#define INV_INV_LOG_H__
+
+#define INV_LOGE(s)
+
+#endif // INV_INV_LOG_H__
diff --git a/libsensors_iio/software/core/mpl/inv_math.h b/libsensors_iio/software/core/mpl/inv_math.h
new file mode 100644
index 0000000..6620bbf
--- /dev/null
+++ b/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/libsensors_iio/software/core/mpl/invensense_adv.h b/libsensors_iio/software/core/mpl/invensense_adv.h
new file mode 100644
index 0000000..9e59c18
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/invensense_adv.h
@@ -0,0 +1,30 @@
+/*
+ $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"
diff --git a/libsensors_iio/software/core/mpl/mag_disturb.h b/libsensors_iio/software/core/mpl/mag_disturb.h
new file mode 100644
index 0000000..38df919
--- /dev/null
+++ b/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/libsensors_iio/software/core/mpl/mlsetinterrupts.h b/libsensors_iio/software/core/mpl/mlsetinterrupts.h
new file mode 100644
index 0000000..a81dabb
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/mlsetinterrupts.h
@@ -0,0 +1,23 @@
+/*
+ $License:
+   Copyright (c) 2008 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+#ifndef MLSETINTERRUPT_H
+#define MLSETINTERRUPT_H
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    /* DEPRECATED - Scheduled for removal. Do not use */
+    inv_error_t MLSetInterrupts(unsigned short interrupts);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* MLSETINTERRUPT_H */
diff --git a/libsensors_iio/software/core/mpl/mlsupervisor_9axis.h b/libsensors_iio/software/core/mpl/mlsupervisor_9axis.h
new file mode 100644
index 0000000..3779381
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/mlsupervisor_9axis.h
@@ -0,0 +1,57 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id: mlsupervisor_9axis.h 6123 2011-09-30 18:21:11Z mcaramello $
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_MLSUPERVISOR_H__
+#define MLDMP_MLSUPERVISOR_H__
+
+#include "mltypes.h"
+
+//#include "temp_comp.h"
+
+struct inv_fusion_t {
+    int compassCount;
+    long quat[4];
+};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_enable_9x_fusion(void);
+inv_error_t inv_disable_9x_fusion(void);
+
+inv_error_t inv_enable_9x_fusion_legacy(void);
+inv_error_t inv_disable_9x_fusion_legacy(void);
+
+inv_error_t inv_enable_9x_fusion_new(void);
+inv_error_t inv_disable_9x_fusion_new(void);
+
+inv_error_t inv_enable_9x_fusion_basic(void);
+inv_error_t inv_disable_9x_fusion_basic(void);
+
+inv_error_t inv_enable_9x_fusion_external(void);
+inv_error_t inv_disable_9x_fusion_external(void);
+
+inv_error_t inv_enable_maintain_heading(void);
+inv_error_t inv_disable_maintain_heading(void);
+
+void inv_set_compass_state(long compassState, long accState, 
+                           unsigned long deltaTime,
+                           int magDisturb, int gotBias,
+                           int *new_state,
+                           int *new_accuracy);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // MLDMP_MLSUPERVISOR_H__
diff --git a/libsensors_iio/software/core/mpl/motion_no_motion.h b/libsensors_iio/software/core/mpl/motion_no_motion.h
new file mode 100644
index 0000000..01cf1c0
--- /dev/null
+++ b/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/libsensors_iio/software/core/mpl/no_gyro_fusion.h b/libsensors_iio/software/core/mpl/no_gyro_fusion.h
new file mode 100644
index 0000000..38d5690
--- /dev/null
+++ b/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/libsensors_iio/software/core/mpl/orientation.h b/libsensors_iio/software/core/mpl/orientation.h
new file mode 100644
index 0000000..ab4e45e
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/orientation.h
@@ -0,0 +1,42 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+#ifndef MLDMP_ORIENTATION_H__
+#define MLDMP_ORIENTATION_H__
+
+#include "mltypes.h"
+/*******************************************************************************/
+/*    Orientations                                                             */
+/*******************************************************************************/
+
+#define INV_X_UP                          0x01
+#define INV_X_DOWN                        0x02
+#define INV_Y_UP                          0x04
+#define INV_Y_DOWN                        0x08
+#define INV_Z_UP                          0x10
+#define INV_Z_DOWN                        0x20
+#define INV_ORIENTATION_ALL               0x3F
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    inv_error_t inv_enable_orientation(void);
+    inv_error_t inv_disable_orientation(void);
+    inv_error_t inv_set_orientation(int orientation);
+    inv_error_t inv_set_orientation_cb(void (*callback)(unsigned short));
+    inv_error_t inv_get_orientation(int *orientation);
+    inv_error_t inv_get_orientation_state(int * state);
+    inv_error_t inv_set_orientation_interrupt(unsigned char on);
+    inv_error_t inv_set_orientation_thresh(float angle,
+                                       float hysteresis,
+                                       unsigned long time, 
+                                       unsigned int axis);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // MLDMP_ORIENTATION_H__
diff --git a/libsensors_iio/software/core/mpl/progressive_no_motion.h b/libsensors_iio/software/core/mpl/progressive_no_motion.h
new file mode 100644
index 0000000..99333e3
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/progressive_no_motion.h
@@ -0,0 +1,39 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id:$
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_PROG_NO_MOTION_H__
+#define MLDMP_PROG_NO_MOTION_H__
+
+#include "mltypes.h"
+
+#define PROG_NO_MOTION  1
+#define PROG_MOTION     2
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* APIs */
+inv_error_t inv_enable_prog_no_motion(void);
+inv_error_t inv_disable_prog_no_motion(void);
+
+/* internal use */
+int inv_get_prog_no_motion_enabled(void);
+void inv_get_prog_no_motion_bias_changed(void);
+int inv_get_prog_no_motion_state(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_PROG_NO_MOTION_H__
diff --git a/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h b/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
new file mode 100644
index 0000000..2cf7a50
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
@@ -0,0 +1,70 @@
+/*

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

+

+

+#ifdef __cplusplus

+}

+#endif

+

+#endif // QUAT_ACCURARCY_MONITOR_H__

diff --git a/libsensors_iio/software/core/mpl/quaternion_supervisor.h b/libsensors_iio/software/core/mpl/quaternion_supervisor.h
new file mode 100644
index 0000000..532e8af
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/quaternion_supervisor.h
@@ -0,0 +1,26 @@
+/*

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

+

+#ifdef __cplusplus

+}

+#endif

+

+#endif // INV_QUATERNION_SUPERVISOR_H__

diff --git a/libsensors_iio/software/core/mpl/sensor_moments.h b/libsensors_iio/software/core/mpl/sensor_moments.h
new file mode 100644
index 0000000..73eb363
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/sensor_moments.h
@@ -0,0 +1,42 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_SENSOR_MOMENTS_H__
+#define MLDMP_SENSOR_MOMENTS_H__
+
+#include "mltypes.h"
+
+enum moment_ord {
+	SECOND_ORD=0,
+	THIRD_ORD,
+	FOURTH_ORD,
+	MAX_ORD
+};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+	inv_error_t inv_enable_sm(void);

+	inv_error_t inv_disable_sm(void);

+	inv_error_t inv_sm_record_data(float sample, void *sensor);

+	inv_error_t inv_sm_update_evt_act_state(int motion);

+	void *inv_init_sm(enum moment_ord);

+	float inv_sm_get_filtered_data(void *sensor);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // MLDMP_SENSOR_MOMENTS_H__
+
diff --git a/libsensors_iio/software/core/mpl/state_storage.h b/libsensors_iio/software/core/mpl/state_storage.h
new file mode 100644
index 0000000..c1eb47b
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/state_storage.h
@@ -0,0 +1,25 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#ifndef INV_STATE_STORAGE_H__
+#define INV_STATE_STORAGE_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_store_data(const void *data, size_t size, unsigned long module,
+                           unsigned long version);
+inv_error_t inv_load_data(void *data, size_t size, unsigned long module,
+                          unsigned long version);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_STATE_STORAGE_H__
diff --git a/libsensors_iio/software/simple_apps/common/external_hardware.h b/libsensors_iio/software/simple_apps/common/external_hardware.h
new file mode 100644
index 0000000..55e3b20
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/external_hardware.h
@@ -0,0 +1,156 @@
+/*
+    Accelerometer
+*/
+#define get_accel_slave_descr NULL
+
+#ifdef CONFIG_MPU_SENSORS_ADXL34X	/* ADI accelerometer */
+struct ext_slave_descr *adxl34x_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr adxl34x_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_BMA150	/* Bosch accelerometer */
+struct ext_slave_descr *bma150_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr bma150_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_BMA222	/* Bosch 222 accelerometer */
+struct ext_slave_descr *bma222_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr bma222_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_BMA250	/* Bosch accelerometer */
+struct ext_slave_descr *bma250_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr bma250_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_KXSD9	/* Kionix accelerometer */
+struct ext_slave_descr *kxsd9_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr kxsd9_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_KXTF9	/* Kionix accelerometer */
+struct ext_slave_descr *kxtf9_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr kxtf9_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_LIS331DLH	/* ST accelerometer */
+struct ext_slave_descr *lis331_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr lis331_get_slave_descr
+#endif
+
+
+#ifdef CONFIG_MPU_SENSORS_LIS3DH	/* ST accelerometer */
+struct ext_slave_descr *lis3dh_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr lis3dh_get_slave_descr
+#endif
+
+/* ST accelerometer in LSM303DLx combo */
+#if defined CONFIG_MPU_SENSORS_LSM303DLX_A
+struct ext_slave_descr *lsm303dlx_a_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr lsm303dlx_a_get_slave_descr
+#endif
+
+/* MPU6050 Accel */
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
+	defined CONFIG_MPU_SENSORS_MPU6050B1
+struct ext_slave_descr *mpu6050_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr mpu6050_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_MMA8450	/* Freescale accelerometer */
+struct ext_slave_descr *mma8450_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr mma8450_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_MMA845X	/* Freescale accelerometer */
+struct ext_slave_descr *mma845x_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr mma845x_get_slave_descr
+#endif
+
+
+/*
+    Compass
+*/
+#define get_compass_slave_descr NULL
+
+#ifdef CONFIG_MPU_SENSORS_AK8975	/* AKM compass */
+struct ext_slave_descr *ak8975_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr ak8975_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_AMI30X	/* AICHI Steel  AMI304/305 compass */
+struct ext_slave_descr *ami30x_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr ami30x_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_AMI306	/* AICHI Steel AMI306 compass */
+struct ext_slave_descr *ami306_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr ami306_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_HMC5883	/* Honeywell compass */
+struct ext_slave_descr *hmc5883_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr hmc5883_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_MMC314X	/* MEMSIC compass */
+struct ext_slave_descr *mmc314x_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr mmc314x_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_LSM303DLX_M	/* ST compass */
+struct ext_slave_descr *lsm303dlx_m_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr lsm303dlx_m_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_YAS529	/* Yamaha compass */
+struct ext_slave_descr *yas529_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr yas529_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_YAS530	/* Yamaha compass */
+struct ext_slave_descr *yas530_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr yas530_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_HSCDTD002B	/* Alps HSCDTD002B compass */
+struct ext_slave_descr *hscdtd002b_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr hscdtd002b_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_HSCDTD004A	/* Alps HSCDTD004A compass */
+struct ext_slave_descr *hscdtd004a_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr hscdtd004a_get_slave_descr
+#endif
+/*
+    Pressure
+*/
+#define get_pressure_slave_descr NULL
+
+#ifdef CONFIG_MPU_SENSORS_BMA085	/* BMA pressure */
+struct ext_slave_descr *bma085_get_slave_descr(void);
+#undef get_pressure_slave_descr
+#define get_pressure_slave_descr bma085_get_slave_descr
+#endif
diff --git a/libsensors_iio/software/simple_apps/common/fopenCMake.c b/libsensors_iio/software/simple_apps/common/fopenCMake.c
new file mode 100644
index 0000000..2936109
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/fopenCMake.c
@@ -0,0 +1,56 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/******************************************************************************
+ *
+ * $Id: fopenCMake.c 5629 2011-06-11 03:13:08Z mcaramello $
+ *
+ *****************************************************************************/
+
+#include <string.h>
+
+#include "fopenCMake.h"
+#include "path_configure.h"
+
+/**
+ *  @brief  Replacement for fopen that concatenates the location of the 
+ *          source tree onto the filename path. 
+ *          It looks in 3 locations:
+ *              - in the current directory, 
+ *              - then it looks in "..", 
+ *              - lastly in the define UNITTEST_SOURCE_DIR which 
+ *                gets defined by CMake.
+ * @param filename 
+ *              Filename relative to base of source directory.
+ * @param prop 
+ *              Second argument to fopen.
+ */
+FILE *fopenCMake(const char *filename, const char *prop)
+{
+    char path[150];
+    FILE *file;
+
+    // Look first in current directory
+    file = fopen(filename, prop);
+    if (file == NULL) {
+        // Now look in ".."
+#ifdef WIN32
+        strcpy(path, "..\\");
+#else
+        strcpy(path, "../");
+#endif
+        strcat(path, filename);
+        file = fopen(path, prop);
+        if (file == NULL) {
+            // Now look in definition by CMake
+            strcpy(path, PATH_SOURCE_DIR);
+            strcat(path, filename);
+            file = fopen(path, prop);
+        }
+    }
+    return file;
+}
+
+
diff --git a/libsensors_iio/software/simple_apps/common/fopenCMake.h b/libsensors_iio/software/simple_apps/common/fopenCMake.h
new file mode 100644
index 0000000..c5eba39
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/fopenCMake.h
@@ -0,0 +1,21 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+#ifndef FOPEN_CMAKE_H__
+#define FOPEN_CMAKE_H__
+
+#include <stdio.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+FILE *fopenCMake( const char *filename, const char *prop );
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // FOPEN_CMAKE_H__
diff --git a/libsensors_iio/software/simple_apps/common/gestureMenu.c b/libsensors_iio/software/simple_apps/common/gestureMenu.c
new file mode 100644
index 0000000..2a9487c
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/gestureMenu.c
@@ -0,0 +1,725 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ * $Id: gestureMenu.c 5705 2011-06-28 19:32:29Z nroyer $
+ *****************************************************************************/
+
+#define _USE_MATH_DEFINES
+#include <stdio.h>
+#include <stddef.h>
+#include <math.h>
+#include <string.h>
+
+#include "ml.h"
+#include "mlmath.h"
+#include "gesture.h"
+#include "orientation.h"
+#include "gestureMenu.h"
+#include "fifo.h"
+
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "gest"
+#include "log.h"
+#include "mldl_cfg.h"
+
+static unsigned long sensors[] = {
+    INV_NINE_AXIS,
+    INV_THREE_AXIS_GYRO,
+    INV_DMP_PROCESSOR | INV_THREE_AXIS_ACCEL,
+    INV_THREE_AXIS_ACCEL,
+    INV_DMP_PROCESSOR | INV_THREE_AXIS_COMPASS,
+    INV_THREE_AXIS_COMPASS,
+    INV_SIX_AXIS_GYRO_ACCEL,
+    INV_DMP_PROCESSOR | INV_SIX_AXIS_ACCEL_COMPASS,
+    INV_SIX_AXIS_ACCEL_COMPASS,
+};
+
+static char *sensors_string[] = {
+    "INV_NINE_AXIS",
+    "INV_THREE_AXIS_GYRO",
+    "INV_DMP_PROCESSOR | INV_THREE_AXIS_ACCEL",
+    "INV_THREE_AXIS_ACCEL",
+    "INV_DMP_PROCESSOR | INV_THREE_AXIS_COMPASS",
+    "INV_THREE_AXIS_COMPASS",
+    "INV_SIX_AXIS_GYRO_ACCEL",
+    "INV_DMP_PROCESSOR | INV_SIX_AXIS_ACCEL_COMPASS",
+    "INV_SIX_AXIS_ACCEL_COMPASS",
+};
+
+/**
+ * Prints the menu with the current thresholds
+ *
+ * @param params The parameters to print
+ */
+void PrintGestureMenu(tGestureMenuParams const * const params)
+{
+    MPL_LOGI("Press h at any time to re-display this menu\n");
+    MPL_LOGI("TAP PARAMETERS:\n");
+    MPL_LOGI("    Use LEFT and RIGHT arrows to adjust Tap Time     \n\n");
+    MPL_LOGI("    j          : Increase X threshold     : %5d\n",
+             params->xTapThreshold);
+    MPL_LOGI("    J (Shift-j): Decrease X threshold\n");
+    MPL_LOGI("    k          : Increase Y threshold     : %5d\n",
+             params->yTapThreshold);
+    MPL_LOGI("    K (Shift-k): Decrease Y threshold\n");
+    MPL_LOGI("    i          : Increase Z threshold     : %5d\n",
+             params->zTapThreshold);
+    MPL_LOGI("    I (Shift-i): Decrease Z threshold\n");
+    MPL_LOGI("    l          : Increase tap time        : %5d\n",
+             params->tapTime);
+    MPL_LOGI("    L (Shift-l): Decrease tap time\n");
+    MPL_LOGI("    o          : Increase next tap time   : %5d\n",
+             params->nextTapTime);
+    MPL_LOGI("    O (Shift-o): Increase next tap time\n");
+    MPL_LOGI("    u          : Increase max Taps        : %5d\n",
+             params->maxTaps);
+    MPL_LOGI("    U (Shift-u): Increase max Taps\n");
+
+    MPL_LOGI("SHAKE PARAMETERS:\n");
+    MPL_LOGI("    x          : Increase X threshold     : %5d\n",
+             params->xShakeThresh);
+    MPL_LOGI("    X (Shift-x): Decrease X threshold\n");
+    MPL_LOGI("    y          : Increase Y threshold     : %5d\n",
+             params->yShakeThresh);
+    MPL_LOGI("    Y (Shift-y): Decrease Y threshold\n");
+    MPL_LOGI("    z          : Increase Z threshold     : %5d\n",
+             params->zShakeThresh);
+    MPL_LOGI("    Z (Shift-z): Decrease Z threshold\n");
+    MPL_LOGI("    s          : Toggle Shake Function    : %5d\n",
+             params->shakeFunction);
+    MPL_LOGI("    t          : Increase Shake Time      : %5d\n",
+             params->shakeTime);
+    MPL_LOGI("    T (Shift-T): Decrease Shake Time\n");
+    MPL_LOGI("    n          : Increase Next Shake Time : %5d\n",
+             params->nextShakeTime);
+    MPL_LOGI("    N (Shift-n): Decrease Next Shake Time\n");
+    MPL_LOGI("    m          : Increase max Shakes      : %5d\n",
+             params->maxShakes);
+    MPL_LOGI("    M (Shift-m): Decrease max Shakes\n");
+    MPL_LOGI("SNAP  PARAMETERS:\n");
+    MPL_LOGI("    p          : Increase Pitch threshold : %5d\n",
+             params->xSnapThresh);
+    MPL_LOGI("    P (Shift-p): Decrease Pitch threshold\n");
+    MPL_LOGI("    r          : Increase Roll  threshold : %5d\n",
+             params->ySnapThresh);
+    MPL_LOGI("    R (Shift-r): Decrease Roll  threshold\n");
+    MPL_LOGI("    a          : Increase yAw   threshold : %5d\n",
+             params->zSnapThresh);
+    MPL_LOGI("    A (Shift-a): Decrease yAw   threshold\n");
+    MPL_LOGI("YAW ROTATION PARAMETERS:\n");
+    MPL_LOGI("    e          : Increase yaW Rotate time : %5d\n",
+             params->yawRotateTime);
+    MPL_LOGI("    E (Shift-r): Decrease yaW Rotate time\n");
+    MPL_LOGI("    w          : Increase yaW Rotate threshold : %5d\n",
+             params->yawRotateThreshold);
+    MPL_LOGI("    W (Shift-w): Decrease yaW Rotate threshold\n");
+    MPL_LOGI("ORIENTATION PARAMETER:\n");
+    MPL_LOGI("    d          : Increase orientation angle threshold : %5f\n",
+             params->orientationThreshold);
+    MPL_LOGI("    D (Shift-d): Decrease orientation angle threshold\n");
+    MPL_LOGI("FIFO RATE:\n");
+    MPL_LOGI("    f          : Increase fifo divider    : %5d\n",
+             inv_get_fifo_rate());
+    MPL_LOGI("    F (Shift-f): Decrease fifo divider\n");
+    MPL_LOGI("REQUESTED SENSORS:\n");
+    MPL_LOGI("    S (Shift-s): Toggle in use sensors : %s\n",
+             sensors_string[params->sensorsIndex]);
+    MPL_LOGI("    F (Shift-f): Decrease fifo divider\n");
+
+    /* V,v, B,b, Q,q, C,c, G,g, are available letters upper and lowercase */
+    /* S is available */
+
+    MPL_LOGI("\n\n");
+}
+
+/**
+ * Handles a keyboard input and updates an appropriate threshold, prints then
+ * menu or returns false if the character is not processed.
+ *
+ * @param params The parameters to modify if the thresholds are updated
+ * @param ch     The input character
+ *
+ * @return        true if the character was processed, false otherwise
+ */
+inv_error_t GestureMenuProcessChar(tGestureMenuParams * const params, char ch)
+{
+    int result = INV_SUCCESS;
+    /* Dynamic keyboard processing */
+
+    switch (ch) {
+    case 'j':
+        params->xTapThreshold += 20;
+        // Intentionally fall through
+    case 'J':  {
+        params->xTapThreshold -= 10;
+        if (params->xTapThreshold < 0) params->xTapThreshold = 0;
+        result = inv_set_tap_threshold(INV_TAP_AXIS_X, params->xTapThreshold);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("MLSetTapThresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_X, %d)\n",
+                 params->xTapThreshold);
+    } break;
+    case 'k':
+        params->yTapThreshold += 20;
+        // Intentionally fall through
+    case 'K':  {
+        params->yTapThreshold -= 10;
+        if (params->yTapThreshold < 0) params->yTapThreshold = 0;
+        result = inv_set_tap_threshold(INV_TAP_AXIS_Y, params->yTapThreshold);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("MLSetTapThresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_Y, %d)\n",
+                 params->yTapThreshold);
+    } break;
+    case 'i':
+        params->zTapThreshold += 20;
+        // Intentionally fall through
+    case 'I':  {
+        params->zTapThreshold -= 10;
+        if (params->zTapThreshold < 0) params->zTapThreshold = 0;
+        result = inv_set_tap_threshold(INV_TAP_AXIS_Z, params->zTapThreshold);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("MLSetTapThresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_Z, %d)\n",
+                 params->zTapThreshold);
+    } break;
+
+    case  'l':
+        params->tapTime += 20;
+        // Intentionally fall through
+    case 'L':  {
+        params->tapTime -= 10;
+        if (params->tapTime < 0) params->tapTime = 0;
+        result = inv_set_next_tap_time(params->tapTime);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_next_tap_time returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_next_tap_time(%d)\n", params->tapTime);
+    } break;
+    case  'o':
+        params->nextTapTime += 20;
+        // Intentionally fall through
+    case 'O':  {
+        params->nextTapTime -= 10;
+        if (params->nextTapTime < 0) params->nextTapTime = 0;
+        result = MLSetNextTapTime(params->nextTapTime);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("MLSetNextTapTime returned :%d\n", result);
+        }
+        MPL_LOGI("MLSetNextTapTime(%d)\n", params->nextTapTime);
+    } break;
+    case  'u':
+        params->maxTaps += 2;
+        // Intentionally fall through
+    case 'U':  {
+        params->maxTaps -= 1;
+        if (params->maxTaps < 0) params->maxTaps = 0;
+        result = inv_set_max_taps(params->maxTaps);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_max_taps returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_max_taps(%d)\n", params->maxTaps);
+    } break;
+    case 's': {
+        int shakeParam;
+        params->shakeFunction = (params->shakeFunction + 1) % 2;
+        switch (params->shakeFunction)
+        {
+        case 0:
+            shakeParam = INV_NO_RETRACTION;
+            MPL_LOGE("inv_set_shake_func(INV_NO_RETRACTION)\n");
+            break;
+        case 1:
+            shakeParam = INV_RETRACTION;
+            MPL_LOGI("inv_set_shake_func(INV_RETRACTION)\n");
+            break;
+        };
+        result = inv_set_shake_func(shakeParam);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_shake_func returned :%d\n", result);
+        }
+    } break;
+    case 'x':
+        params->xShakeThresh += 200;
+        // Intentionally fall through
+    case 'X': {
+        params->xShakeThresh -= 100;
+        result = inv_set_shake_thresh(INV_PITCH_SHAKE, params->xShakeThresh);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_shake_thresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_shake_thresh(INV_PITCH_SHAKE, %d)\n", params->xShakeThresh);
+    } break;
+    case 'y':
+        params->yShakeThresh += 200;
+        // Intentionally fall through
+    case 'Y': {
+        params->yShakeThresh -= 100;
+        result = inv_set_shake_thresh(INV_ROLL_SHAKE, params->yShakeThresh);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_shake_thresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_shake_thresh(INV_ROLL_SHAKE, %d)\n", params->yShakeThresh);
+    } break;
+    case 'z':
+        params->zShakeThresh += 200;
+        // Intentionally fall through
+    case 'Z':{
+        params->zShakeThresh -= 100;
+        result = inv_set_shake_thresh(INV_YAW_SHAKE, params->zShakeThresh);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_shake_thresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_shake_thresh(INV_YAW_SHAKE, %d)\n",params->zShakeThresh);
+    } break;
+    case 'r':
+        params->ySnapThresh += 20;
+        // Intentionally fall through
+    case 'R': {
+        params->ySnapThresh -= 10;
+        result = inv_set_hard_shake_thresh(INV_ROLL_SHAKE, params->ySnapThresh);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_hard_shake_thresh(INV_ROLL_SHAKE, %d)\n",params->ySnapThresh);
+    } break;
+    case 'p':
+        params->xSnapThresh += 20;
+        // Intentionally fall through
+    case 'P': {
+        params->xSnapThresh -= 10;
+        result = inv_set_hard_shake_thresh(INV_PITCH_SHAKE, params->xSnapThresh);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_hard_shake_thresh(INV_PITCH_SHAKE, %d)\n",
+                 params->xSnapThresh);
+    } break;
+    case 'a':
+        params->zSnapThresh += 20;
+    case 'A': {
+        params->zSnapThresh -= 10;
+        result = inv_set_hard_shake_thresh(INV_YAW_SHAKE, params->zSnapThresh);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_hard_shake_thresh(INV_YAW_SHAKE, %d)\n",params->zSnapThresh);
+    } break;
+
+    case 't':
+        params->shakeTime += 20;
+    case 'T':{
+        params->shakeTime -= 10;
+        result = inv_set_shake_time(params->shakeTime);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_shake_time returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_shake_time(%d)\n", params->shakeTime);
+    } break;
+    case 'n':
+        params->nextShakeTime += 20;
+    case 'N':{
+        params->nextShakeTime -= 10;
+        result = inv_set_next_shake_time(params->nextShakeTime);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_next_shake_time returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_next_shake_time(%d)\n", params->nextShakeTime);
+    } break;
+    case 'm':
+        params->maxShakes += 2;
+    case 'M':{
+        params->maxShakes -= 1;
+        result = inv_set_max_shakes(INV_SHAKE_ALL, params->maxShakes);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_max_shakes returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_max_shakes(%d)\n", params->maxShakes);
+    } break;
+    case 'e':
+        params->yawRotateTime += 20;
+    case 'E':{
+        params->yawRotateTime -= 10;
+        result = inv_set_yaw_rotate_time(params->yawRotateTime);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_yaw_rotate_time returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_yaw_rotate_time(%d)\n", params->yawRotateTime);
+    } break;
+    case 'w':
+        params->yawRotateThreshold += 2;
+    case 'W':{
+        params->yawRotateThreshold -= 1;
+        result = inv_set_yaw_rotate_thresh(params->yawRotateThreshold);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_yaw_rotate_thresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_yaw_rotate_thresh(%d)\n", params->yawRotateThreshold);
+    } break;
+    case 'c':
+        params->shakeRejectValue += 0.20f;
+    case 'C':{
+        params->shakeRejectValue -= 0.10f;
+        result = inv_set_tap_shake_reject(params->shakeRejectValue);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_tap_shake_reject returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_tap_shake_reject(%f)\n", params->shakeRejectValue);
+    } break;
+    case 'd':
+        params->orientationThreshold += 10;
+    case 'D':{
+        params->orientationThreshold -= 5;
+        if (params->orientationThreshold > 90) {
+            params->orientationThreshold = 90;
+        }
+
+        if (params->orientationThreshold < 0 ) {
+            params->orientationThreshold = 0;
+        }
+
+        result = inv_set_orientation_thresh(params->orientationThreshold,
+                                           5, 80,
+                                           INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_orientation_thresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_orientation_thresh(%f, %d, %d,"
+                 " INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS)\n",
+                 params->orientationThreshold, 5, 80);
+    } break;
+    case 'f':
+        result = inv_set_fifo_rate(inv_get_fifo_rate() + 1);
+        MPL_LOGI("inv_set_fifo_rate(%d)\n",inv_get_fifo_rate());
+        break;
+    case 'F':
+    {
+        unsigned short newRate = inv_get_fifo_rate();
+        if (newRate > 0)
+            newRate--;
+        result = inv_set_fifo_rate(newRate);
+        MPL_LOGI("inv_set_fifo_rate(%d)\n",inv_get_fifo_rate());
+        break;
+    }
+    case 'S':
+        params->sensorsIndex++;
+        if (params->sensorsIndex >= ARRAY_SIZE(sensors)) {
+            params->sensorsIndex = 0;
+        }
+        result = inv_set_mpu_sensors(
+            sensors[params->sensorsIndex] & params->available_sensors);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        MPL_LOGI("%d = inv_set_mpu_sensors(%s)\n", result,
+                 sensors_string[params->sensorsIndex]);
+        break;
+    case 'h':
+    case 'H': {
+        PrintGestureMenu(params);
+    } break;
+    default: {
+        result = INV_ERROR;
+    } break;
+    };
+    return result;
+}
+
+/**
+ * Initializes the tGestureMenuParams to a set of defaults.
+ *
+ * @param params The parameters to initialize.
+ */
+void GestureMenuSetDefaults(tGestureMenuParams * const params)
+{
+    params->xTapThreshold           = 100;
+    params->yTapThreshold           = 100;
+    params->zTapThreshold           = 100;
+    params->tapTime                 = 100;
+    params->nextTapTime             = 600;
+    params->maxTaps                 = 2;
+    params->shakeRejectValue        = 0.8f;
+    params->xShakeThresh            = 750;
+    params->yShakeThresh            = 750;
+    params->zShakeThresh            = 750;
+    params->xSnapThresh             = 160;
+    params->ySnapThresh             = 320;
+    params->zSnapThresh             = 160;
+    params->shakeTime               = 100;
+    params->nextShakeTime           = 1000;
+    params->shakeFunction           = 0;
+    params->maxShakes               = 3;
+    params->yawRotateTime           = 80;
+    params->yawRotateThreshold      = 70;
+    params->orientationThreshold    = 60;
+    params->sensorsIndex            = 0;
+    params->available_sensors       = INV_NINE_AXIS;
+}
+
+void GestureMenuSetAvailableSensors(tGestureMenuParams * const params,
+                                    unsigned long available_sensors)
+{
+    params->available_sensors       = available_sensors;
+}
+/**
+ * Call the appropriate MPL set threshold functions and checkes the error codes
+ *
+ * @param params The parametrs to use in setting the thresholds
+ *
+ * @return INV_SUCCESS or the first error code encountered.
+ */
+inv_error_t GestureMenuSetMpl(tGestureMenuParams const * const params)
+{
+    inv_error_t result = INV_SUCCESS;
+
+    result = inv_set_tap_threshold(INV_TAP_AXIS_X, params->xTapThreshold);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_tap_threshold returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_tap_threshold(INV_TAP_AXIS_Y, params->yTapThreshold);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_tap_threshold returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_tap_threshold(INV_TAP_AXIS_Z, params->zTapThreshold);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_tap_threshold returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_next_tap_time(params->tapTime);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_next_tap_time returned :%d\n", result);
+        return result;
+    }
+    result = MLSetNextTapTime(params->nextTapTime);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("MLSetNextTapTime returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_max_taps(params->maxTaps);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_max_taps returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_tap_shake_reject(params->shakeRejectValue);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_tap_shake_reject returned :%d\n", result);
+        return result;
+    }
+
+    //Set up shake gesture
+    result = inv_set_shake_func(params->shakeFunction);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_shake_func returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_shake_thresh(INV_ROLL_SHAKE, params->xShakeThresh);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_shake_thresh returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_shake_thresh(INV_PITCH_SHAKE, params->yShakeThresh);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_shake_thresh returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_shake_thresh(INV_YAW_SHAKE, params->zShakeThresh);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_shake_thresh returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_shake_time(params->shakeTime);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_shake_time returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_next_shake_time(params->nextShakeTime);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_next_shake_time returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_max_shakes(INV_SHAKE_ALL,params->maxShakes);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_max_shakes returned :%d\n", result);
+        return result;
+    }
+
+    // Yaw rotate settings
+    result = inv_set_yaw_rotate_time(params->yawRotateTime);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_yaw_rotate_time returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_yaw_rotate_thresh(params->yawRotateThreshold);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_yaw_rotate_thresh returned :%d\n", result);
+        return result;
+    }
+
+    // Orientation settings
+    result = inv_set_orientation_thresh(params->orientationThreshold, 5, 80,
+                                       INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_orientation_thresh returned: %d\n", result);
+        return result;
+    }
+
+    // Requested Sensors
+    result = inv_set_mpu_sensors(
+        sensors[params->sensorsIndex] & params->available_sensors);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("MLSetMPUSesnors returned: %d %lx\n", result,
+                 sensors[params->sensorsIndex] & params->available_sensors);
+        return result;
+    }
+
+    return INV_SUCCESS;
+}
+
+void PrintGesture(tGesture* gesture)
+{
+    float speed;
+    char type[1024];
+    switch (gesture->type)
+    {
+    case INV_TAP:
+    {
+        if (gesture->meta < 0) {
+            snprintf(type,sizeof(type),"-");
+        } else {
+            snprintf(type,sizeof(type),"+");
+        }
+
+        switch (ABS(gesture->meta))
+        {
+        case 1:
+            strcat(type,"X");
+            break;
+        case 2:
+            strcat(type,"Y");
+            break;
+        case 3:
+            strcat(type,"Z");
+            break;
+        default:
+            strcat(type,"ERROR");
+            break;
+        };
+        MPL_LOGI("TAP: %s  %2d, X: %6d Y: %6d Z: %6d XY: %6.2f, YZ: %6.2f, XZ: %6.2f\n",
+                 type,
+                 gesture->num,
+                 gesture->strength,
+                 gesture->speed,
+                 gesture->reserved,
+                 (180 / M_PI) * atan2(
+                    (float)gesture->strength, (float)gesture->speed),
+                 (180 / M_PI) * atan2(
+                    (float)gesture->speed, (float)gesture->reserved),
+                 (180 / M_PI) * atan2(
+                    (float)gesture->strength, (float)gesture->reserved)
+            );
+    }
+    break;
+    case INV_ROLL_SHAKE:
+    case INV_PITCH_SHAKE:
+    case INV_YAW_SHAKE:
+    {
+        if (gesture->strength){
+            snprintf(type, sizeof(type), "Snap : ");
+        } else {
+            snprintf(type, sizeof(type), "Shake: ");
+        }
+
+        if (gesture->meta==0) {
+            strcat(type, "+");
+        } else {
+            strcat(type, "-");
+        }
+
+        if (gesture->type == INV_ROLL_SHAKE) {
+            strcat(type, "Roll  ");
+        } else if (gesture->type == INV_PITCH_SHAKE) {
+            strcat(type, "Pitch ");
+        } else if (gesture->type == INV_YAW_SHAKE) {
+            strcat(type, "Yaw   ");
+        }
+
+        speed = (float)gesture->speed +
+            (float)(gesture->reserved / (float)(1 << 16));
+        MPL_LOGI("%s:%3d (speed: %8.2f)\n",type, gesture->num, speed);
+    }
+    break;
+    case INV_YAW_IMAGE_ROTATE:
+    {
+        if (gesture->meta == 0) {
+            snprintf(type, sizeof(type), "Positive ");
+        } else {
+            snprintf(type, sizeof(type), "Negative ");
+        }
+        MPL_LOGI("%s Yaw Image Rotation\n", type);
+    }
+    break;
+    default:
+        MPL_LOGE("Unknown Gesture received\n");
+        break;
+    }
+}
+
+/**
+ * Prints the new or current orientation using MPL_LOGI and remembers the last
+ * orientation to print orientation flips.
+ *
+ * @param orientation the new or current orientation.  0 to reset.
+ */
+void PrintOrientation(unsigned short orientation)
+{
+    // Determine if it was a flip
+    static int sLastOrientation = 0;
+    int flip = orientation | sLastOrientation;
+
+    if ((INV_X_UP | INV_X_DOWN) == flip) {
+        MPL_LOGI("Flip about the X Axis: \n");
+    } else if ((INV_Y_UP | INV_Y_DOWN) == flip) {
+        MPL_LOGI("Flip about the Y axis: \n");
+    } else if ((INV_Z_UP | INV_Z_DOWN) == flip) {
+        MPL_LOGI("Flip about the Z axis: \n");
+    }
+    sLastOrientation = orientation;
+
+    switch (orientation) {
+    case INV_X_UP:
+        MPL_LOGI("X Axis is up\n");
+        break;
+    case INV_X_DOWN:
+        MPL_LOGI("X Axis is down\n");
+        break;
+    case INV_Y_UP:
+        MPL_LOGI("Y Axis is up\n");
+        break;
+    case INV_Y_DOWN:
+        MPL_LOGI("Y Axis is down\n");
+        break;
+    case INV_Z_UP:
+        MPL_LOGI("Z Axis is up\n");
+        break;
+    case INV_Z_DOWN:
+        MPL_LOGI("Z Axis is down\n");
+        break;
+    case 0:
+        break; /* Not an error.  Resets sLastOrientation */
+    default:
+        MPL_LOGE("%s: Unreconized orientation %hx\n", __func__, orientation);
+        break;
+    }
+}
+
+
diff --git a/libsensors_iio/software/simple_apps/common/gestureMenu.h b/libsensors_iio/software/simple_apps/common/gestureMenu.h
new file mode 100644
index 0000000..8f804e1
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/gestureMenu.h
@@ -0,0 +1,75 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/***************************************************************************** *
+ * $Id: gestureMenu.h 5705 2011-06-28 19:32:29Z nroyer $ 
+ ******************************************************************************/
+/**
+ * @defgroup 
+ * @brief  
+ *
+ * @{
+ *      @file     gestureMenu.h
+ *      @brief    
+ *
+ *
+ */
+
+#ifndef __GESTUREMENU_H__
+#define __GESTUREMENU_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/******************************************************************************/
+    typedef struct sGestureMenuParams {
+        /* Tap Params */
+        int xTapThreshold;
+        int yTapThreshold;
+        int zTapThreshold;
+        int tapTime;
+        int nextTapTime;
+        int maxTaps;
+        float shakeRejectValue;
+
+        /* Shake Params */
+        int xShakeThresh;
+        int yShakeThresh;
+        int zShakeThresh;
+        int xSnapThresh;
+        int ySnapThresh;
+        int zSnapThresh;
+        int shakeTime;
+        int nextShakeTime;
+        int shakeFunction;
+        int maxShakes;
+
+        /* Yaw rotate params */
+        int yawRotateTime;
+        int yawRotateThreshold;
+
+        /* Orientation */
+        float orientationThreshold;
+        int sensorsIndex;
+        unsigned long available_sensors;
+    } tGestureMenuParams;
+
+    void     PrintGestureMenu(tGestureMenuParams const * const params) ;
+    inv_error_t GestureMenuProcessChar(tGestureMenuParams * const params,char ch);
+    void     PrintGesture(gesture_t* gesture);
+    void     PrintOrientation(unsigned short orientation);
+    void     GestureMenuSetDefaults(tGestureMenuParams * const params);
+    void GestureMenuSetAvailableSensors(tGestureMenuParams * const params,
+                                        unsigned long available_sensors);
+    inv_error_t GestureMenuSetMpl(tGestureMenuParams const * const params);
+
+/******************************************************************************/
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // __GESTUREMENU_H__
diff --git a/libsensors_iio/software/simple_apps/common/helper.c b/libsensors_iio/software/simple_apps/common/helper.c
new file mode 100644
index 0000000..4d634bd
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/helper.c
@@ -0,0 +1,110 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/*******************************************************************************
+ *
+ * $Id: helper.c 4367 2010-12-21 03:02:55Z prao $
+ *
+ *******************************************************************************/
+
+#include <stdio.h>
+#ifdef _WIN32
+#include <windows.h>
+#include <conio.h>
+#endif
+#ifdef LINUX
+#include <sys/select.h>
+#endif
+#include <time.h>
+#include <string.h>
+
+#include "ml.h"
+#include "slave.h"
+#include "mldl.h"
+#include "mltypes.h"
+#include "mlstates.h"
+#include "compass.h"
+
+#include "mlsl.h"
+#include "ml.h"
+
+#include "helper.h"
+#include "mlsetup.h"
+#include "fopenCMake.h"
+#include "int.h"
+#include "mlos.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-helper"
+
+#ifdef AIO
+extern inv_error_t MLSLSetYamahaCompassDataMode(unsigned char mode);
+#endif
+
+// Keyboard hit function
+int ConsoleKbhit(void)
+{
+#ifdef _WIN32
+    return _kbhit();
+#else
+    struct timeval tv;
+    fd_set read_fd;
+
+    tv.tv_sec=0;
+    tv.tv_usec=0;
+    FD_ZERO(&read_fd);
+    FD_SET(0,&read_fd);
+
+    if(select(1, &read_fd, NULL, NULL, &tv) == -1)
+        return 0;
+
+    if(FD_ISSET(0,&read_fd))
+        return 1;
+
+    return 0;
+#endif
+}
+
+char ConsoleGetChar(void) {
+#ifdef _WIN32
+    return _getch();
+#else
+    return getchar();
+#endif
+}
+struct mpuirq_data** InterruptPoll(int *handles, int numHandles, long tv_sec, long tv_usec)
+{
+    struct mpuirq_data **data;
+    void *tmp;
+    int ii;
+    const int irq_data_size = sizeof(**data) * numHandles + 
+        sizeof(*data) * numHandles;
+
+    tmp = (void *)inv_malloc(irq_data_size);
+    memset(tmp, 0, irq_data_size);
+    data = (struct mpuirq_data **)tmp;
+    for (ii = 0; ii < numHandles; ii++) {
+        data[ii] = (struct mpuirq_data *)((unsigned long)tmp +
+            (sizeof(*data) * numHandles) + sizeof(**data) * ii);
+    }
+
+    if (IntProcess(handles, numHandles, data, tv_sec, tv_usec) > 0) {
+        for (ii = 0; ii < numHandles; ii++) {
+            if (data[ii]->interruptcount) {
+                inv_interrupt_handler(ii);
+            }
+        }
+    }
+    
+    /* Return data incase the application needs to look at the timestamp or
+       other part of the data */
+    return data;
+}
+
+void InterruptPollDone(struct mpuirq_data ** data)
+{
+    inv_free(data);
+}
diff --git a/libsensors_iio/software/simple_apps/common/helper.h b/libsensors_iio/software/simple_apps/common/helper.h
new file mode 100644
index 0000000..b2da520
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/helper.h
@@ -0,0 +1,103 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id: helper-customer.h 5770 2011-07-14 01:34:10Z mcaramello $
+ *
+ *******************************************************************************/
+
+#ifndef HELPER_C_H
+#define HELPER_C_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "mltypes.h"
+#include "mlerrorcode.h"
+
+/*
+    Defines 
+*/
+
+#define CALL_N_CHECK(f) {                                                   \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                              \
+        printf("Error in file %s, line %d : %s returned code %s (#%d)\n",   \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+    }                                                                       \
+}
+
+#define CALL_CHECK_N_RETURN_ERROR(f) {                                      \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                              \
+        printf("Error in file %s, line %d : %s returned code %s (#%d)\n",   \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        return r35uLt;                                                      \
+    }                                                                       \
+}
+
+// for functions returning void
+#define CALL_CHECK_N_RETURN(f) {                                            \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                              \
+        printf("Error in file %s, line %d : %s returned code %s (#%d)\n",   \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        return;                                                             \
+    }                                                                       \
+}
+
+#define CALL_CHECK_N_EXIT(f) {                                              \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                              \
+        printf("Error in file %s, line %d : %s returned code %s (#%d)\n",   \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        exit (r35uLt);                                                      \
+    }                                                                       \
+}
+
+    
+#define CALL_CHECK_N_CALLBACK(f, cb) {                                      \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                              \
+        printf("Error in file %s, line %d : %s returned code %s (#%d)\n",   \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        cb;                                                                 \
+    }                                                                       \
+}
+
+#define CALL_CHECK_N_GOTO(f, label) {                                       \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                              \
+        printf("Error in file %s, line %d : %s returned code %s (#%d)\n",   \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        goto label;                                                         \
+    }                                                                       \
+}
+
+#define DEFAULT_PLATFORM        PLATFORM_ID_MSB_V2
+#define DEFAULT_ACCEL_ID        ACCEL_ID_KXTF9
+#define DEFAULT_COMPASS_ID      COMPASS_ID_AK8975
+
+#define DataLogger(x)           NULL
+#define DataLoggerSelector(x)   //
+#define DataLoggerCb(x)         NULL
+#define findComm()              (9)
+#define MenuHwChoice(p,a,c)     (*p = DEFAULT_PLATFORM, *a = DEFAULT_ACCEL_ID, \
+                                 *c = DEFAULT_COMPASS_ID, INV_ERROR)
+
+    char ConsoleGetChar(void);
+    int ConsoleKbhit(void);
+    struct mpuirq_data **InterruptPoll(
+        int *handles, int numHandles, long tv_sec, long tv_usec);
+    void InterruptPollDone(struct mpuirq_data ** data);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // HELPER_C_H
diff --git a/libsensors_iio/software/simple_apps/common/mlerrorcode.c b/libsensors_iio/software/simple_apps/common/mlerrorcode.c
new file mode 100644
index 0000000..25b0df6
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/mlerrorcode.c
@@ -0,0 +1,96 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id: mlerrorcode.c 5629 2011-06-11 03:13:08Z mcaramello $
+ *
+ *****************************************************************************/
+
+#include <stdio.h>
+#include <string.h>
+
+#include "mltypes.h"
+#include "mlerrorcode.h"
+
+#define ERROR_CODE_CASE(CODE)   \
+    case CODE:                  \
+        return #CODE            \
+
+/**
+ *  @brief  return a string containing the label assigned to the error code.
+ *  
+ *  @param  errorcode   
+ *              The errorcode value of which the label has to be returned.
+ *
+ *  @return A string containing the error code label.
+ */
+char* MLErrorCode(inv_error_t errorcode) 
+{
+    switch(errorcode) {
+        ERROR_CODE_CASE(INV_SUCCESS);
+        ERROR_CODE_CASE(INV_ERROR);
+        ERROR_CODE_CASE(INV_ERROR_INVALID_PARAMETER);
+        ERROR_CODE_CASE(INV_ERROR_FEATURE_NOT_ENABLED);
+        ERROR_CODE_CASE(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        ERROR_CODE_CASE(INV_ERROR_DMP_NOT_STARTED);
+        ERROR_CODE_CASE(INV_ERROR_DMP_STARTED);
+        ERROR_CODE_CASE(INV_ERROR_NOT_OPENED);
+        ERROR_CODE_CASE(INV_ERROR_OPENED);
+        ERROR_CODE_CASE(INV_ERROR_INVALID_MODULE);
+        ERROR_CODE_CASE(INV_ERROR_MEMORY_EXAUSTED);
+        ERROR_CODE_CASE(INV_ERROR_DIVIDE_BY_ZERO);
+        ERROR_CODE_CASE(INV_ERROR_ASSERTION_FAILURE);
+        ERROR_CODE_CASE(INV_ERROR_FILE_OPEN);
+        ERROR_CODE_CASE(INV_ERROR_FILE_READ);
+        ERROR_CODE_CASE(INV_ERROR_FILE_WRITE);
+
+        ERROR_CODE_CASE(INV_ERROR_SERIAL_CLOSED);
+        ERROR_CODE_CASE(INV_ERROR_SERIAL_OPEN_ERROR);
+        ERROR_CODE_CASE(INV_ERROR_SERIAL_READ);
+        ERROR_CODE_CASE(INV_ERROR_SERIAL_WRITE);
+        ERROR_CODE_CASE(INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED);
+
+        ERROR_CODE_CASE(INV_ERROR_SM_TRANSITION);
+        ERROR_CODE_CASE(INV_ERROR_SM_IMPROPER_STATE);
+
+        ERROR_CODE_CASE(INV_ERROR_FIFO_OVERFLOW);
+        ERROR_CODE_CASE(INV_ERROR_FIFO_FOOTER);
+        ERROR_CODE_CASE(INV_ERROR_FIFO_READ_COUNT);
+        ERROR_CODE_CASE(INV_ERROR_FIFO_READ_DATA);
+        ERROR_CODE_CASE(INV_ERROR_MEMORY_SET);
+
+        ERROR_CODE_CASE(INV_ERROR_LOG_MEMORY_ERROR);
+        ERROR_CODE_CASE(INV_ERROR_LOG_OUTPUT_ERROR);
+
+        ERROR_CODE_CASE(INV_ERROR_OS_BAD_PTR);
+        ERROR_CODE_CASE(INV_ERROR_OS_BAD_HANDLE);
+        ERROR_CODE_CASE(INV_ERROR_OS_CREATE_FAILED);
+        ERROR_CODE_CASE(INV_ERROR_OS_LOCK_FAILED);
+
+        ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_OVERFLOW);
+        ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_UNDERFLOW);
+        ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_NOT_READY);
+        ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_ERROR);
+
+        ERROR_CODE_CASE(INV_ERROR_CALIBRATION_LOAD);
+        ERROR_CODE_CASE(INV_ERROR_CALIBRATION_STORE);
+        ERROR_CODE_CASE(INV_ERROR_CALIBRATION_LEN);
+        ERROR_CODE_CASE(INV_ERROR_CALIBRATION_CHECKSUM);
+
+        default:
+        {
+            #define UNKNOWN_ERROR_CODE 1234
+            return ERROR_NAME(UNKNOWN_ERROR_CODE);
+            break;
+        }
+
+    }
+}
+
+/**
+ *  @}
+ */
diff --git a/libsensors_iio/software/simple_apps/common/mlerrorcode.h b/libsensors_iio/software/simple_apps/common/mlerrorcode.h
new file mode 100644
index 0000000..9a35792
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/mlerrorcode.h
@@ -0,0 +1,86 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/*******************************************************************************
+ *
+ * $Id: mltypes.h 3680 2010-09-04 03:13:32Z mcaramello $
+ *
+ *******************************************************************************/
+
+#ifndef _MLERRORCODE_H_
+#define _MLERRORCODE_H_
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+    Defines 
+*/
+#define CALL_N_CHECK(f) {                                                   \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                             \
+        MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+    }                                                                       \
+}
+
+#define CALL_CHECK_N_RETURN_ERROR(f) {                                      \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                             \
+        MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        return r35uLt;                                                      \
+    }                                                                       \
+}
+
+// for functions returning void
+#define CALL_CHECK_N_RETURN(f) do {                                         \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                             \
+        MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        return;                                                             \
+    }                                                                       \
+    } while(0)
+
+#define CALL_CHECK_N_EXIT(f) {                                              \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                             \
+        MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        exit (r35uLt);                                                      \
+    }                                                                       \
+}
+
+    
+#define CALL_CHECK_N_CALLBACK(f, cb) {                                      \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                             \
+        MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        cb;                                                                 \
+    }                                                                       \
+}
+
+#define CALL_CHECK_N_GOTO(f, label) {                                       \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                             \
+        MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        goto label;                                                         \
+    }                                                                       \
+}
+
+char* MLErrorCode(inv_error_t errorcode);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
diff --git a/libsensors_iio/software/simple_apps/common/mlsetup.c b/libsensors_iio/software/simple_apps/common/mlsetup.c
new file mode 100644
index 0000000..f11bce9
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/mlsetup.c
@@ -0,0 +1,1722 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/******************************************************************************
+ *
+ * $Id: mlsetup.c 6113 2011-09-29 23:40:55Z jcalizo $
+ *
+ *****************************************************************************/
+#undef MPL_LOG_NDEBUG
+#ifdef UNITTESTING
+#define MPL_LOG_NDEBUG 1
+#else
+#define MPL_LOG_NDEBUG 0
+#endif
+
+/**
+ *  @defgroup MLSETUP
+ *  @brief  The Motion Library external slaves setup override suite.
+ *
+ *          Use these APIs to override the kernel/default settings in the
+ *          corresponding data structures for gyros, accel, and compass.
+ *
+ *  @{
+ *      @file mlsetup.c
+ *      @brief The Motion Library external slaves setup override suite.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+/*
+    Defines
+*/
+/* these have to appear before inclusion of mpu.h */
+#define CONFIG_MPU_SENSORS_KXSD9         y   // Kionix accel
+#define CONFIG_MPU_SENSORS_KXTF9         y   // Kionix accel
+#define CONFIG_MPU_SENSORS_LIS331DLH     y   // ST accelerometer
+#define CONFIG_MPU_SENSORS_LSM303DLX_A   y   // ST accelerometer in LSM303DLx combo
+#define CONFIG_MPU_SENSORS_LIS3DH        y   // ST accelerometer
+#define CONFIG_MPU_SENSORS_BMA150        y   // Bosch 150 accelerometer
+#define CONFIG_MPU_SENSORS_BMA222        y   // Bosch 222 accelerometer
+#define CONFIG_MPU_SENSORS_BMA250        y   // Bosch 250 accelerometer
+#define CONFIG_MPU_SENSORS_ADXL34X       y   // AD 345 or 346 accelerometer
+#define CONFIG_MPU_SENSORS_MMA8450       y   // Freescale MMA8450 accelerometer
+#define CONFIG_MPU_SENSORS_MMA845X       y   // Freescale MMA845X accelerometer
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1
+#define CONFIG_MPU_SENSORS_MPU6050_ACCEL y   // Invensense MPU6050 built-in accelerometer
+#endif
+
+#define CONFIG_MPU_SENSORS_AK8975        y   // AKM compass
+#define CONFIG_MPU_SENSORS_AMI30X        y   // AICHI AMI304/305 compass
+#define CONFIG_MPU_SENSORS_AMI306        y   // AICHI AMI306 compass
+#define CONFIG_MPU_SENSORS_HMC5883       y   // Honeywell compass
+#define CONFIG_MPU_SENSORS_LSM303DLX_M   y   // ST compass in LSM303DLx combo
+#define CONFIG_MPU_SENSORS_YAS529        y   // Yamaha compass
+#define CONFIG_MPU_SENSORS_YAS530        y   // Yamaha compass
+#define CONFIG_MPU_SENSORS_MMC314X       y   // MEMSIC compass
+#define CONFIG_MPU_SENSORS_HSCDTD002B    y   // ALPS compass
+#define CONFIG_MPU_SENSORS_HSCDTD004A    y   // ALPS HSCDTD004A compass
+
+#define CONFIG_MPU_SENSORS_BMA085        y   // Bosch 085 pressure
+
+#include "external_hardware.h"
+
+#include <stdio.h>
+#include <string.h>
+
+#include "slave.h"
+#include "compass.h"
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-mlsetup"
+
+#include "linux/mpu.h"
+
+#include "mlsetup.h"
+
+#ifdef LINUX
+#include "errno.h"
+#endif
+
+/* Override these structures from mldl.c */
+extern struct ext_slave_descr          g_slave_accel;
+extern struct ext_slave_descr          g_slave_compass;
+//extern struct ext_slave_descr          g_slave_pressure;
+/* Platform Data */
+//extern struct mpu_platform_data        g_pdata;
+extern struct ext_slave_platform_data  g_pdata_slave_accel;
+extern struct ext_slave_platform_data  g_pdata_slave_compass;
+//extern struct ext_slave_platform_data  g_pdata_slave_pressure;
+signed char g_gyro_orientation[9];
+
+/*
+    Typedefs
+*/
+typedef void tSetupFuncAccel(void);
+typedef void tSetupFuncCompass(void);
+typedef void tSetupFuncPressure(void);
+
+#ifdef LINUX
+#include <sys/ioctl.h>
+#endif
+
+/*********************************************************************
+              Dragon - PLATFORM_ID_DRAGON_PROTOTYPE
+*********************************************************************/
+/**
+ * @internal
+ * @brief  performs a 180' rotation around Z axis to reflect
+ *         usage of the multi sensor board (MSB) with the
+ *         beagleboard
+ * @note   assumes well formed mounting matrix, with only
+ *         one 1 for each row.
+ */
+static void Rotate180DegAroundZAxis(signed char matrix[])
+{
+    int ii;
+    for(ii=0; ii<6; ii++) {
+        matrix[ii] = -matrix[ii];
+    }
+}
+
+/**
+ * @internal
+ * Sets the orientation based on the position of the mounting.  For different
+ * devices the relative position to pin 1 will be different.
+ *
+ * Positions are:
+ * - 0-3 are Z up
+ * - 4-7 are Z down
+ * - 8-11 are Z right
+ * - 12-15 are Z left
+ * - 16-19 are Z front
+ * - 20-23 are Z back
+ *
+ * @param position The position of the orientation
+ * @param orientation the location to store the new oreintation
+ */
+static inv_error_t SetupOrientation(unsigned int position,
+                                 signed char *orientation)
+{
+    memset(orientation, 0, 9);
+    switch (position){
+    case 0:
+        /*-------------------------*/
+        orientation[0] = +1;
+        orientation[4] =     +1;
+        orientation[8] =         +1;
+        break;
+    case 1:
+        /*-------------------------*/
+        orientation[1] =     +1;
+        orientation[3] = -1;
+        orientation[8] =         +1;
+        break;
+    case 2:
+        /*-------------------------*/
+        orientation[0] = -1;
+        orientation[4] =     -1;
+        orientation[8] =         +1;
+        break;
+    case 3:
+        /*-------------------------*/
+        orientation[1] =     -1;
+        orientation[3] = +1;
+        orientation[8] =         +1;
+        break;
+    case 4:
+        /*-------------------------*/
+        orientation[0] = -1;
+        orientation[4] =     +1;
+        orientation[8] =         -1;
+        break;
+    case 5:
+        /*-------------------------*/
+        orientation[1] =     -1;
+        orientation[3] = -1;
+        orientation[8] =         -1;
+        break;
+    case 6:
+        /*-------------------------*/
+        orientation[0] = +1;
+        orientation[4] =     -1;
+        orientation[8] =         -1;
+        break;
+    case 7:
+        /*-------------------------*/
+        orientation[1] =     +1;
+        orientation[3] = +1;
+        orientation[8] =         -1;
+        break;
+    case 8:
+        /*-------------------------*/
+        orientation[2] =         +1;
+        orientation[3] = +1;
+        orientation[7] =     +1;
+        break;
+    case 9:
+        /*-------------------------*/
+        orientation[2] = +1;
+        orientation[4] = +1;
+        orientation[6] = -1;
+        break;
+    case 10:
+        orientation[2] = +1;
+        orientation[3] = -1;
+        orientation[7] = -1;
+        break;
+    case 11:
+        orientation[2] = +1;
+        orientation[4] = -1;
+        orientation[6] = +1;
+        break;
+    case 12:
+        orientation[2] = -1;
+        orientation[3] = -1;
+        orientation[7] = +1;
+        break;
+    case 13:
+        orientation[2] = -1;
+        orientation[4] = -1;
+        orientation[6] = -1;
+        break;
+    case 14:
+        orientation[2] = -1;
+        orientation[3] = +1;
+        orientation[7] = -1;
+        break;
+    case 15:
+        orientation[2] = -1;
+        orientation[4] = +1;
+        orientation[6] = +1;
+        break;
+    case 16:
+        orientation[0] = -1;
+        orientation[5] = +1;
+        orientation[7] = +1;
+        break;
+    case 17:
+        orientation[1] = -1;
+        orientation[5] = +1;
+        orientation[6] = -1;
+        break;
+    case 18:
+        orientation[0] = +1;
+        orientation[5] = -1;
+        orientation[7] = -1;
+        break;
+    case 19:
+        orientation[1] = -1;
+        orientation[5] = +1;
+        orientation[6] = +1;
+        break;
+    case 20:
+        orientation[0] = +1;
+        orientation[5] = -1;
+        orientation[7] = +1;
+        break;
+    case 21:
+        orientation[1] = -1;
+        orientation[5] = -1;
+        orientation[6] = +1;
+        break;
+    case 22:
+        orientation[0] = -1;
+        orientation[5] = -1;
+        orientation[7] = -1;
+        break;
+    case 23:
+        orientation[1] = +1;
+        orientation[5] = -1;
+        orientation[6] = -1;
+        break;
+    default:
+        MPL_LOGE("Invalid position %d\n", position);
+        LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    return INV_SUCCESS;
+}
+
+static void PrintMountingOrientation(
+    const char * header, signed char *orientation)
+{
+    MPL_LOGV("%s:\n", header);
+    MPL_LOGV("\t[[%3d, %3d, %3d]\n",
+             orientation[0], orientation[1], orientation[2]);
+    MPL_LOGV("\t [%3d, %3d, %3d]\n",
+             orientation[3], orientation[4], orientation[5]);
+    MPL_LOGV("\t [%3d, %3d, %3d]]\n",
+             orientation[6], orientation[7], orientation[8]);
+}
+
+/*****************************
+ *   Accel Setup Functions   *
+ *****************************/
+
+static inv_error_t SetupAccelSTLIS331Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_SPIDER_PROTOTYPE:
+        position = 5;
+        break;
+    case PLATFORM_ID_ST_6AXIS:
+        position = 0;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *lis331_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_LIS331;
+    return INV_SUCCESS;
+}
+
+static inv_error_t SetupAccelSTLIS3DHCalibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_SPIDER_PROTOTYPE:
+        position = 1;
+        break;
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+        position = 3;
+        break;
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *lis3dh_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_LIS3DH;
+    return result;
+}
+
+static inv_error_t SetupAccelKionixKXSD9Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 7;
+        break;
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *kxsd9_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_KXSD9;
+    return result;
+}
+
+static inv_error_t SetupAccelKionixKXTF9Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB_EVB:
+        position =0;
+            break;
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_SPIDER_PROTOTYPE:
+        position = 7;
+        break;
+#ifdef WIN32
+    case PLATFORM_ID_DONGLE:
+        position = 1;
+        break;
+#endif
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+        position = 1;
+        break;
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *kxtf9_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_KXTF9;
+    return result;
+}
+
+static inv_error_t SetupAccelLSM303Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+        position = 3;
+        break;
+    case PLATFORM_ID_MSB_V2:
+        position = 1;
+        break;
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *lsm303dlx_a_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_LSM303;
+    return result;
+}
+
+static inv_error_t SetupAccelBMA150Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 6;
+        break;
+#ifdef WIN32
+    case PLATFORM_ID_DONGLE:
+        position = 3;
+        break;
+#endif
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *bma150_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_BMA150;
+    return result;
+}
+
+static inv_error_t SetupAccelBMA222Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 0;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *bma222_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_BMA222;
+    return result;
+}
+
+static inv_error_t SetupAccelBMA250Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+        position = 0;
+        break;
+    case PLATFORM_ID_SPIDER_PROTOTYPE:
+        position = 3;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *bma250_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_BMA250;
+    return result;
+}
+
+static inv_error_t SetupAccelADXL34XCalibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 6;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *adxl34x_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_ADXL34X;
+    return result;
+}
+
+
+static inv_error_t SetupAccelMMA8450Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 5;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *mma8450_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_MMA8450;
+    return result;
+}
+
+
+static inv_error_t SetupAccelMMA845XCalibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 5;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *mma845x_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_MMA845X;
+    return result;
+}
+
+
+/**
+ * @internal
+ * Sets up the orientation matrix according to how the gyro was
+ * mounted.
+ *
+ * @param platforId Platform identification for mounting information
+ * @return INV_SUCCESS or non-zero error code
+ */
+static inv_error_t SetupAccelMPU6050Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_MANTIS_MSB:
+        position = 6;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+	case PLATFORM_ID_DRAGON_USB_DONGLE:
+        position = 1;
+        break;
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    case PLATFORM_ID_MANTIS_EVB:
+        position = 0;
+        break;
+    case PLATFORM_ID_MSB_V3:
+        position = 2;
+        break;
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        return INV_ERROR_INVALID_PARAMETER;
+    };
+
+    SetupOrientation(position, g_pdata_slave_accel.orientation);
+    /* Interrupt */
+#ifndef LINUX
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1
+   // g_slave_accel = // fixme *mpu6050_get_slave_descr();
+#endif
+#endif
+    g_pdata_slave_accel.address         = 0x68;
+    return result;
+}
+
+/*****************************
+    Compass Setup Functions
+******************************/
+static inv_error_t SetupCompassAKM8975Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_MANTIS_MSB:
+        position = 2;
+        break;
+#ifdef WIN32
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+        position = 4;
+        break;
+#endif
+    case PLATFORM_ID_SPIDER_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+        position = 7;
+        break;
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V3:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+        position = 6;
+        break;
+    case PLATFORM_ID_DRAGON_USB_DONGLE:
+    case PLATFORM_ID_MSB_EVB:
+        position = 5;
+        break;
+    case PLATFORM_ID_MANTIS_EVB:
+        position = 4;
+        break;
+	 case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_compass = *ak8975_get_slave_descr();
+#endif
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_AKM;
+    return result;
+}
+
+static inv_error_t SetupCompassMMCCalibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 7;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_compass = *mmc314x_get_slave_descr();
+#endif
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_MMC314X;
+    return result;
+}
+
+static inv_error_t SetupCompassAMI304Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 4;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_AMI304;
+#ifndef LINUX
+    g_slave_compass = *ami30x_get_slave_descr();
+#endif
+    return result;
+}
+
+static inv_error_t SetupCompassAMI306Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_SPIDER_PROTOTYPE:
+        position = 3;
+        break;
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+        position = 1;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_compass = *ami306_get_slave_descr();
+#endif
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_AMI306;
+    return result;
+}
+
+static inv_error_t SetupCompassHMC5883Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 6;
+        break;
+#ifdef WIN32
+    case PLATFORM_ID_DONGLE:
+        position = 2;
+        break;
+#endif
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_compass = *hmc5883_get_slave_descr();
+#endif
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_HMC5883;
+    return result;
+}
+
+
+static inv_error_t SetupCompassLSM303DLHCalibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 1;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+    };
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+#ifndef LINUX
+    g_slave_compass = *lsm303dlx_m_get_slave_descr();
+    g_slave_compass.id = COMPASS_ID_LSM303DLH;
+#endif
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_HMC5883;
+    return result;
+}
+
+static inv_error_t SetupCompassLSM303DLMCalibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+        position = 8;
+        break;
+    case PLATFORM_ID_MSB_V2:
+        position = 12;
+        break;
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+    };
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_compass = *lsm303dlx_m_get_slave_descr();
+    g_slave_compass.id = COMPASS_ID_LSM303DLM;
+#endif
+    g_pdata_slave_compass.address       = COMPASS_SLAVEADDR_HMC5883;
+    return result;
+}
+
+static inv_error_t SetupCompassYAS530Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+        position = 1;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_compass = *yas530_get_slave_descr();
+#endif
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_YAS530;
+    return result;
+}
+
+static inv_error_t SetupCompassYAS529Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 6;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_compass = *yas529_get_slave_descr();
+#endif
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_YAS529;
+    return result;
+}
+
+
+static inv_error_t SetupCompassHSCDTD002BCalibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 2;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_compass = *hscdtd002b_get_slave_descr();
+#endif
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_HSCDTD00XX;
+    return result;
+}
+
+static inv_error_t SetupCompassHSCDTD004ACalibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 1;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_compass = *hscdtd004a_get_slave_descr();
+#endif
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_HSCDTD00XX;
+    return result;
+}
+
+
+/*****************************
+    Pressure Setup Functions
+******************************/
+#if 0
+static inv_error_t SetupPressureBMA085Calibration(unsigned short platformId)
+{
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    memset(g_pdata_slave_pressure.orientation, 0, sizeof(g_pdata_slave_pressure.orientation));
+
+    g_pdata_slave_pressure.bus             = EXT_SLAVE_BUS_PRIMARY;
+#ifndef LINUX
+    g_slave_pressure = *bma085_get_slave_descr();
+#endif
+    g_pdata_slave_pressure.address         = PRESSURE_SLAVEADDR_BMA085;
+    return INV_SUCCESS;
+}
+#endif
+/**
+ * @internal
+ * Sets up the orientation matrix according to how the part was
+ * mounted.
+ *
+ * @param platforId Platform identification for mounting information
+ * @return INV_SUCCESS or non-zero error code
+ */
+static inv_error_t SetupAccelCalibration(unsigned short platformId,
+                                      unsigned short accelId)
+{
+    /*----  setup the accels ----*/
+    switch(accelId) {
+    case ACCEL_ID_LSM303DLX:
+        SetupAccelLSM303Calibration(platformId);
+        break;
+    case ACCEL_ID_LIS331:
+        SetupAccelSTLIS331Calibration(platformId);
+        break;
+    case ACCEL_ID_KXSD9:
+        SetupAccelKionixKXSD9Calibration(platformId);
+        break;
+    case ACCEL_ID_KXTF9:
+        SetupAccelKionixKXTF9Calibration(platformId);
+        break;
+    case ACCEL_ID_BMA150:
+        SetupAccelBMA150Calibration(platformId);
+        break;
+    case ACCEL_ID_BMA222:
+        SetupAccelBMA222Calibration(platformId);
+        break;
+    case ACCEL_ID_BMA250:
+        SetupAccelBMA250Calibration(platformId);
+        break;
+    case ACCEL_ID_ADXL34X:
+        SetupAccelADXL34XCalibration(platformId);
+        break;
+    case ACCEL_ID_MMA8450:
+        SetupAccelMMA8450Calibration(platformId);
+        break;
+    case ACCEL_ID_MMA845X:
+        SetupAccelMMA845XCalibration(platformId);
+        break;
+    case ACCEL_ID_LIS3DH:
+        SetupAccelSTLIS3DHCalibration(platformId);
+        break;
+    case ACCEL_ID_MPU6050:
+        SetupAccelMPU6050Calibration(platformId);
+        break;
+    case ID_INVALID:
+        break;
+    default:
+        LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    if (accelId != ID_INVALID && accelId != ACCEL_ID_MPU6050) {
+        g_pdata_slave_accel.bus             = EXT_SLAVE_BUS_SECONDARY;
+    } else if (accelId != ACCEL_ID_MPU6050) {
+        g_pdata_slave_accel.bus             = EXT_SLAVE_BUS_PRIMARY;
+    }
+
+#ifndef WIN32
+    if (accelId != ID_INVALID)
+        Rotate180DegAroundZAxis(g_pdata_slave_accel.orientation);
+#endif
+
+    return INV_SUCCESS;
+}
+
+/**
+ * @internal
+ * Sets up the orientation matrix according to how the part was
+ * mounted.
+ *
+ * @param platforId Platform identification for mounting information
+ * @return INV_SUCCESS or non-zero error code
+ */
+inv_error_t SetupCompassCalibration(unsigned short platformId,
+                                 unsigned short compassId)
+{
+    /*----  setup the compass ----*/
+    switch(compassId) {
+    case COMPASS_ID_AK8975:
+        SetupCompassAKM8975Calibration(platformId);
+        break;
+    case COMPASS_ID_AMI30X:
+        SetupCompassAMI304Calibration(platformId);
+        break;
+    case COMPASS_ID_AMI306:
+        SetupCompassAMI306Calibration(platformId);
+        break;
+    case COMPASS_ID_LSM303DLH:
+        SetupCompassLSM303DLHCalibration(platformId);
+        break;
+    case COMPASS_ID_LSM303DLM:
+        SetupCompassLSM303DLMCalibration(platformId);
+        break;
+    case COMPASS_ID_HMC5883:
+        SetupCompassHMC5883Calibration(platformId);
+        break;
+    case COMPASS_ID_YAS529:
+        SetupCompassYAS529Calibration(platformId);
+        break;
+    case COMPASS_ID_YAS530:
+        SetupCompassYAS530Calibration(platformId);
+        break;
+    case COMPASS_ID_MMC314X:
+        SetupCompassMMCCalibration(platformId);
+        break;
+    case COMPASS_ID_HSCDTD002B:
+        SetupCompassHSCDTD002BCalibration(platformId);
+        break;
+    case COMPASS_ID_HSCDTD004A:
+        SetupCompassHSCDTD004ACalibration(platformId);
+        break;
+    case ID_INVALID:
+        break;
+    default:
+        if (INV_ERROR_INVALID_PARAMETER) {
+            LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+            return INV_ERROR_INVALID_PARAMETER;
+        }
+        break;
+    }
+
+    if (platformId == PLATFORM_ID_MSB_V2_MANTIS     ||
+        platformId == PLATFORM_ID_MANTIS_MSB        ||
+        platformId == PLATFORM_ID_MANTIS_USB_DONGLE ||
+        platformId == PLATFORM_ID_MANTIS_PROTOTYPE  ||
+        platformId == PLATFORM_ID_DRAGON_PROTOTYPE) {
+        switch (compassId) {
+        case ID_INVALID:
+            g_pdata_slave_compass.bus = EXT_SLAVE_BUS_INVALID;
+            break;
+        case COMPASS_ID_AK8975:
+        case COMPASS_ID_AMI306:
+            g_pdata_slave_compass.bus             = EXT_SLAVE_BUS_SECONDARY;
+            break;
+        default:
+            g_pdata_slave_compass.bus             = EXT_SLAVE_BUS_PRIMARY;
+        };
+    } else {
+        g_pdata_slave_compass.bus             = EXT_SLAVE_BUS_PRIMARY;
+    }
+
+#ifndef WIN32
+    if (compassId != ID_INVALID)
+        Rotate180DegAroundZAxis(g_pdata_slave_compass.orientation);
+#endif
+
+    return INV_SUCCESS;
+}
+
+/**
+ * @internal
+ * Sets up the orientation matrix according to how the part was
+ * mounted.
+ *
+ * @param platforId Platform identification for mounting information
+ * @return INV_SUCCESS or non-zero error code
+ */
+#if 0
+inv_error_t SetupPressureCalibration(unsigned short platformId,
+                                  unsigned short pressureId)
+{
+    inv_error_t result = INV_SUCCESS;
+    /*----  setup the compass ----*/
+    switch(pressureId) {
+        case PRESSURE_ID_BMA085:
+            result = SetupPressureBMA085Calibration(platformId);
+            break;
+        default:
+            if (INV_ERROR_INVALID_PARAMETER) {
+                LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                return INV_ERROR_INVALID_PARAMETER;
+            }
+    };
+
+    return result;
+}
+#endif
+/**
+ * @internal
+ * Sets up the orientation matrix according to how the gyro was
+ * mounted.
+ *
+ * @param platforId Platform identification for mounting information
+ * @return INV_SUCCESS or non-zero error code
+ */
+static inv_error_t SetupGyroCalibration(unsigned short platformId)
+{
+    int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_SPIDER_PROTOTYPE:
+        position = 2;
+        break;
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_MANTIS_MSB:
+#ifndef WIN32
+        position = 4;
+#else
+        position = 6;
+#endif
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+        position = 1;
+        break;
+    case PLATFORM_ID_DRAGON_USB_DONGLE:
+        position = 3;
+        break;
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+#ifndef WIN32
+        position = 2;
+#else
+        position = 0;
+#endif
+        break;
+    case PLATFORM_ID_MANTIS_EVB:
+    case PLATFORM_ID_MSB_EVB:
+        position = 0;
+        break;
+    case PLATFORM_ID_MSB_V3:
+        position = 2;
+        break;
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        return INV_ERROR_INVALID_PARAMETER;
+    };
+
+    SetupOrientation(position, g_gyro_orientation);
+
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  Setup the Hw orientation and full scale.
+ *  @param  platfromId
+ *              an user defined Id to distinguish the Hw platform in
+ *              use from others.
+ *  @param  accelId
+ *              the accelerometer specific id, as specified in the MPL.
+ *  @param  compassId
+ *              the compass specific id, as specified in the MPL.
+ *  @return INV_SUCCESS or a non-zero error code.
+ */
+inv_error_t SetupPlatform(
+                unsigned short platformId,
+                unsigned short accelId,
+                unsigned short compassId)
+{
+    int result;
+
+    memset(&g_slave_accel, 0, sizeof(g_slave_accel));
+    memset(&g_slave_compass, 0, sizeof(g_slave_compass));
+//    memset(&g_slave_pressure, 0, sizeof(g_slave_pressure));
+//    memset(&g_pdata, 0, sizeof(g_pdata));
+
+#ifdef LINUX
+    /* On Linux initialize the global platform data with the driver defaults */
+    {
+        void *mpu_handle;
+        int ii;
+
+        struct ext_slave_descr *slave[EXT_SLAVE_NUM_TYPES];
+        struct ext_slave_platform_data *pdata_slave[EXT_SLAVE_NUM_TYPES];
+        slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL;
+        slave[EXT_SLAVE_TYPE_ACCEL] = &g_slave_accel;
+        slave[EXT_SLAVE_TYPE_COMPASS] = &g_slave_compass;
+        //slave[EXT_SLAVE_TYPE_PRESSURE] = &g_slave_pressure;
+
+        pdata_slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL;
+        pdata_slave[EXT_SLAVE_TYPE_ACCEL] = &g_pdata_slave_accel;
+        pdata_slave[EXT_SLAVE_TYPE_COMPASS] = &g_pdata_slave_compass;
+        //pdata_slave[EXT_SLAVE_TYPE_PRESSURE] = &g_pdata_slave_pressure;
+
+        MPL_LOGI("Getting the MPU_GET_PLATFORM_DATA\n");
+        result = inv_serial_open("/dev/mpu",&mpu_handle);
+        if (result) {
+            MPL_LOGE("MPU_GET_PLATFORM_DATA failed %d\n", result);
+        }
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+            if (!slave[ii])
+                continue;
+            slave[ii]->type = ii;
+            result = ioctl((int)mpu_handle, MPU_GET_EXT_SLAVE_DESCR,
+                           slave[ii]);
+            if (result)
+                result = errno;
+            if(result == INV_ERROR_INVALID_MODULE) {
+                slave[ii] = NULL;
+                result = 0;
+            } else if (result) {
+                LOG_RESULT_LOCATION(result);
+                LOG_RESULT_LOCATION(INV_ERROR_INVALID_MODULE);
+                return result;
+            }
+        }
+        //result = ioctl((int)mpu_handle, MPU_GET_MPU_PLATFORM_DATA, &g_pdata);
+        if (result) {
+            result = errno;
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+            if (!pdata_slave[ii])
+                continue;
+            pdata_slave[ii]->type = ii;
+            result = ioctl(
+                (int)mpu_handle, MPU_GET_EXT_SLAVE_PLATFORM_DATA,
+                pdata_slave[ii]);
+            if (result)
+                result = errno;
+            if (result == INV_ERROR_INVALID_MODULE) {
+                pdata_slave[ii] = NULL;
+                result = 0;
+            } else if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        }
+        if (result) {
+            MPL_LOGE("MPU_GET_PLATFORM_DATA failed %d\n", result);
+        }
+        inv_serial_close(mpu_handle);
+    }
+#endif
+
+    result = SetupGyroCalibration(platformId);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    PrintMountingOrientation("Gyroscope", g_gyro_orientation);
+    result = SetupAccelCalibration(platformId, accelId);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    PrintMountingOrientation("Accelerometer", g_pdata_slave_accel.orientation);
+    result = SetupCompassCalibration(platformId, compassId);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    PrintMountingOrientation("Compass", g_pdata_slave_compass.orientation);
+#if 0
+    if (platformId == PLATFORM_ID_MSB_10AXIS) {
+        result = SetupPressureCalibration(platformId, PRESSURE_ID_BMA085);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        PrintMountingOrientation("Pressure", g_pdata_slave_pressure.orientation);
+    }
+#endif
+#ifdef LINUX
+    /* On Linux override the orientation, level shifter etc */
+    {
+        void *mpu_handle;
+        int ii;
+        struct ext_slave_descr *slave[EXT_SLAVE_NUM_TYPES];
+        struct ext_slave_platform_data *pdata_slave[EXT_SLAVE_NUM_TYPES];
+        slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL;
+        slave[EXT_SLAVE_TYPE_ACCEL] = &g_slave_accel;
+        slave[EXT_SLAVE_TYPE_COMPASS] = &g_slave_compass;
+        //slave[EXT_SLAVE_TYPE_PRESSURE] = &g_slave_pressure;
+
+        pdata_slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL;
+        pdata_slave[EXT_SLAVE_TYPE_ACCEL] = &g_pdata_slave_accel;
+        pdata_slave[EXT_SLAVE_TYPE_COMPASS] = &g_pdata_slave_compass;
+        //pdata_slave[EXT_SLAVE_TYPE_PRESSURE] = &g_pdata_slave_pressure;
+
+        MPL_LOGI("Setting the MPU_SET_PLATFORM_DATA\n");
+        result = inv_serial_open("/dev/mpu",&mpu_handle);
+        if (result) {
+            MPL_LOGE("MPU_SET_PLATFORM_DATA failed %d\n", result);
+        }
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+            if (!slave[ii])
+                continue;
+            slave[ii]->type = ii;
+            result = ioctl((int)mpu_handle, MPU_SET_EXT_SLAVE_PLATFORM_DATA,
+                           slave[ii]);
+            if (result)
+                result = errno;
+            if (result == INV_ERROR_INVALID_MODULE) {
+                slave[ii] = NULL;
+                result = 0;
+            } else if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        }
+        //result = ioctl((int)mpu_handle, MPU_SET_MPU_PLATFORM_DATA, &g_pdata);
+        if (result) {
+            result = errno;
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+            if (!pdata_slave[ii])
+                continue;
+            pdata_slave[ii]->type = ii;
+            result = ioctl((int)mpu_handle, MPU_SET_EXT_SLAVE_PLATFORM_DATA,
+                pdata_slave[ii]);
+            if (result)
+                result = errno;
+            if (result == INV_ERROR_INVALID_MODULE) {
+                pdata_slave[ii] = NULL;
+                result = 0;
+            } else if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        }
+        if (result) {
+            MPL_LOGE("MPU_SET_PLATFORM_DATA failed %d\n", result);
+        }
+        inv_serial_close(mpu_handle);
+    }
+#endif
+    return INV_SUCCESS;
+}
+
+/**
+ * @}
+ */
+
+
diff --git a/libsensors_iio/software/simple_apps/common/mlsetup.h b/libsensors_iio/software/simple_apps/common/mlsetup.h
new file mode 100644
index 0000000..06fa9f4
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/mlsetup.h
@@ -0,0 +1,52 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+ 
+/*******************************************************************************
+ *
+ * $Id: mlsetup.h 6101 2011-09-29 00:30:33Z kkatingari $
+ *
+ *******************************************************************************/
+
+#ifndef MLSETUP_H
+#define MLSETUP_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "linux/mpu.h"
+#include "mltypes.h"
+
+    enum mpu_platform_id {
+        PLATFORM_ID_INVALID = ID_INVALID, // 0
+        PLATFORM_ID_MSB,              // (0x0001) MSB (Multi sensors board)
+        PLATFORM_ID_ST_6AXIS,         // (0x0002) 6 Axis with ST accelerometer
+        PLATFORM_ID_DONGLE,           // (0x0003) 9 Axis USB dongle with
+        PLATFORM_ID_MANTIS_PROTOTYPE, // (0x0004) Mantis prototype board
+        PLATFORM_ID_MANTIS_MSB,       // (0x0005) MSB with Mantis
+        PLATFORM_ID_MANTIS_USB_DONGLE,// (0x0006) Mantis and AKM on USB dongle.
+        PLATFORM_ID_MSB_10AXIS,       // (0x0007) MSB with pressure sensor
+        PLATFORM_ID_DRAGON_PROTOTYPE, // (0x0008) Dragon prototype board
+        PLATFORM_ID_MSB_V2,           // (0x0009) Version 2 MSB
+        PLATFORM_ID_MSB_V2_MANTIS,    // (0x000A) Version 2 MSB with mantis
+        PLATFORM_ID_MANTIS_EVB,       // (0x000B) Mantis EVB (shipped to cust.)
+        PLATFORM_ID_DRAGON_USB_DONGLE,// (0x000C) Dragon USB Dongle with Mantis Rev C
+        PLATFORM_ID_MSB_EVB,          // (0X000D) MSB with 3050. 
+        PLATFORM_ID_SPIDER_PROTOTYPE,
+        PLATFORM_ID_MSB_V3,
+
+        NUM_PLATFORM_IDS
+    };
+    // Main entry APIs
+inv_error_t SetupPlatform(unsigned short platformId, 
+                       unsigned short accelSelection, 
+                       unsigned short compassSelection);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* MLSETUP_H */
diff --git a/libsensors_iio/software/simple_apps/common/slave.h b/libsensors_iio/software/simple_apps/common/slave.h
new file mode 100644
index 0000000..7b40a8c
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/slave.h
@@ -0,0 +1,176 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/*******************************************************************************
+ *
+ * $Id: slave.h 5732 2011-07-07 01:11:34Z vbhatt $
+ *
+ *******************************************************************************/
+
+#ifndef SLAVE_H
+#define SLAVE_H
+
+/**
+ *  @addtogroup SLAVEDL
+ *
+ *  @{
+ *      @file     slave.h
+ *      @brief    Top level descriptions for Accelerometer support
+ *
+ */
+
+#include "mltypes.h"
+#include "linux/mpu.h"
+
+    /* ------------ */
+    /* - Defines. - */
+    /* ------------ */
+
+/*--- default accel support - selection ---*/
+#define ACCEL_ST_LIS331                 0
+#define ACCEL_KIONIX_KXTF9              1
+#define ACCEL_BOSCH                     0
+#define ACCEL_ADI                       0
+
+#define ACCEL_SLAVEADDR_INVALID         0x00
+
+#define ACCEL_SLAVEADDR_LIS331          0x18
+#define ACCEL_SLAVEADDR_LSM303          0x18
+#define ACCEL_SLAVEADDR_LIS3DH          0x18
+#define ACCEL_SLAVEADDR_KXSD9           0x18
+#define ACCEL_SLAVEADDR_KXTF9           0x0F
+#define ACCEL_SLAVEADDR_BMA150          0x38
+#define ACCEL_SLAVEADDR_BMA222          0x08
+#define ACCEL_SLAVEADDR_BMA250          0x18
+#define ACCEL_SLAVEADDR_ADXL34X         0x53
+#define ACCEL_SLAVEADDR_ADXL34X_ALT     0x1D /* alternative addr */
+#define ACCEL_SLAVEADDR_MMA8450         0x1C
+#define ACCEL_SLAVEADDR_MMA845X         0x1C
+
+#define ACCEL_SLAVEADDR_INVENSENSE      0x68
+/*
+    Define default accelerometer to use if no selection is made
+*/
+#if ACCEL_ST_LIS331
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_LIS331
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_LIS331
+#endif
+
+#if ACCEL_ST_LSM303
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_LSM303
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_LSM303DLX
+#endif
+
+#if ACCEL_KIONIX_KXSD9
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_KXSD9
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_KXSD9
+#endif
+
+#if ACCEL_KIONIX_KXTF9
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_KXTF9
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_KXTF9
+#endif
+
+#if ACCEL_BOSCH
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_BMA150
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_BMA150
+#endif
+
+#if ACCEL_BMA222
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_BMA222
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_BMA222
+#endif
+
+#if ACCEL_BOSCH
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_BMA250
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_BMA250
+#endif
+
+#if ACCEL_ADI
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_ADXL34X
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_ADXL34X
+#endif
+
+#if ACCEL_MMA8450
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_MMA8450
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_MMA8450
+#endif
+
+#if ACCEL_MMA845X
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_MMA845X
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_MMA845X
+#endif
+
+/*--- if no default accelerometer was selected ---*/
+#ifndef DEFAULT_ACCEL_SLAVEADDR
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_INVALID
+#endif
+
+#define USE_COMPASS_AICHI                  0
+#define USE_COMPASS_AKM                    0
+#define USE_COMPASS_YAS529                 0
+#define USE_COMPASS_YAS530                 0
+#define USE_COMPASS_HMC5883                0
+#define USE_COMPASS_MMC314X                0
+#define USE_COMPASS_HSCDTD002B             0
+#define USE_COMPASS_HSCDTD004A             0
+
+#define COMPASS_SLAVEADDR_INVALID          0x00
+#define COMPASS_SLAVEADDR_AKM_BASE         0x0C
+#define COMPASS_SLAVEADDR_AKM              0x0E
+#define COMPASS_SLAVEADDR_AMI304           0x0E
+#define COMPASS_SLAVEADDR_AMI305           0x0F /*Slave address for AMI 305/306*/
+#define COMPASS_SLAVEADDR_AMI306           0x0E /*Slave address for AMI 305/306*/
+#define COMPASS_SLAVEADDR_YAS529           0x2E
+#define COMPASS_SLAVEADDR_YAS530           0x2E
+#define COMPASS_SLAVEADDR_HMC5883          0x1E
+#define COMPASS_SLAVEADDR_MMC314X          0x30
+#define COMPASS_SLAVEADDR_HSCDTD00XX       0x0C
+
+/*
+    Define default compass to use if no selection is made
+*/
+ #if USE_COMPASS_AKM
+ #define DEFAULT_COMPASS_TYPE              COMPASS_ID_AK8975
+ #endif
+
+ #if USE_COMPASS_AICHI
+ #define DEFAULT_COMPASS_TYPE              COMPASS_ID_AMI30X
+ #endif
+
+ #if USE_COMPASS_YAS529
+ #define DEFAULT_COMPASS_TYPE              COMPASS_ID_YAS529
+ #endif
+
+ #if USE_COMPASS_YAS530
+ #define DEFAULT_COMPASS_TYPE              COMPASS_ID_YAS530
+ #endif
+
+ #if USE_COMPASS_HMC5883
+ #define DEFAULT_COMPASS_TYPE              COMPASS_ID_HMC5883
+ #endif
+
+#if USE_COMPASS_MMC314X
+#define DEFAULT_COMPASS_TYPE              COMPASS_ID_MMC314X
+#endif
+
+#if USE_COMPASS_HSCDTD002B
+#define DEFAULT_COMPASS_TYPE              COMPASS_ID_HSCDTD002B
+#endif
+
+#if USE_COMPASS_HSCDTD004A
+#define DEFAULT_COMPASS_TYPE              COMPASS_ID_HSCDTD004A
+#endif
+
+#ifndef DEFAULT_COMPASS_TYPE
+#define DEFAULT_COMPASS_TYPE               ID_INVALID
+#endif
+
+
+#endif // SLAVE_H
+
+/**
+ *  @}
+ */
diff --git a/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared b/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared
new file mode 100644
index 0000000..228abf7
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared
diff --git a/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk b/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk
new file mode 100644
index 0000000..b1d881c
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk
@@ -0,0 +1,109 @@
+EXEC = consoledmp$(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/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk b/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk
new file mode 100644
index 0000000..b01fdfb
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk
@@ -0,0 +1,23 @@
+#### filelist.mk for console_test ####
+
+# helper headers
+HEADERS := $(COMMON_DIR)/external_hardware.h
+HEADERS += $(COMMON_DIR)/fopenCMake.h
+HEADERS += $(COMMON_DIR)/helper.h
+HEADERS += $(COMMON_DIR)/mlerrorcode.h
+HEADERS += $(COMMON_DIR)/mlsetup.h
+HEADERS += $(COMMON_DIR)/slave.h
+
+HEADERS += $(HAL_DIR)/include/mlos.h
+HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h
+
+# sources
+SOURCES := $(APP_DIR)/console_test.c
+
+# helper sources
+SOURCES += $(HAL_DIR)/linux/inv_sysfs_utils.c
+SOURCES += $(HAL_DIR)/linux/mlos_linux.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux
diff --git a/libsensors_iio/software/simple_apps/console/linux/console_test.c b/libsensors_iio/software/simple_apps/console/linux/console_test.c
new file mode 100644
index 0000000..e15b20d
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/console/linux/console_test.c
@@ -0,0 +1,742 @@
+/*******************************************************************************
+ * $Id: $
+ ******************************************************************************/
+ 
+/*******************************************************************************
+ *
+ * Copyright (c) 2011 InvenSense Corporation, All Rights Reserved.
+ *
+ ******************************************************************************/
+//#include <linux/conio.h>
+//#include <fcntl.h>
+//#include <termios.h>
+//#include <unistd.h>
+
+//#if 0
+#include <stdio.h>
+#include <time.h>
+#include <sys/select.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <stdlib.h>
+#include <linux/input.h>
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "console_test"
+
+#include "mlos.h"
+#include "invensense.h"
+#include "invensense_adv.h"
+#include "inv_sysfs_utils.h"
+#include "ml_stored_data.h"
+#include "ml_math_func.h"
+#include "ml_load_dmp.h"
+/*#else
+#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 "inv_sysfs_utils.h"
+#include "invensense.h"
+#include "invensense_adv.h"
+#include "ml_stored_data.h"
+#include "ml_math_func.h"
+#include "ml_load_dmp.h"
+#include "log.h"
+#endif*/
+
+/* TODO: add devices as needed. */
+#define ITG3500         (0)
+#define MPU6050         (1)
+#define BMA250          (2)
+#define NUM_DEVICES     (ITG3500 + MPU6050 + BMA250)
+
+#define DEVICE          MPU6050
+#define DMP_IMAGE       dmp_firmware_200_latest
+#define SIX_AXES        6
+#define NINE_AXES       9
+
+#if 0
+struct input_event {
+       struct timeval time;
+       __u16 type;
+       __u16 code;
+       __s32 value;
+};
+#endif
+
+/* TODO: Add paths to other attributes.
+ * TODO: Input device paths depend on the module installation order.
+ */
+const struct inv_sysfs_names_s filenames[NUM_DEVICES] = {
+    {        /* ITG3500 */
+        .buffer             = "/dev/input/event0",
+        .enable             = "/sys/bus/i2c/devices/4-0068/inv_gyro/fifo_enable",
+        .raw_data           = "/sys/bus/i2c/devices/4-0068/inv_gyro/raw_gyro",
+        .temperature        = "/sys/bus/i2c/devices/4-0068/inv_gyro/temperature",
+        .fifo_rate          = "/sys/bus/i2c/devices/4-0068/inv_gyro/fifo_rate",
+        .power_state        = "/sys/bus/i2c/devices/4-0068/inv_gyro/power_state",
+        .fsr                = "/sys/bus/i2c/devices/4-0068/inv_gyro/FSR",
+        .lpf                = "/sys/bus/i2c/devices/4-0068/inv_gyro/lpf",
+        .scale              = "/sys/bus/i2c/devices/4-0068/inv_gyro/gyro_scale",
+        .temp_scale         = "/sys/bus/i2c/devices/4-0068/inv_gyro/temp_scale",
+        .temp_offset        = "/sys/bus/i2c/devices/4-0068/inv_gyro/temp_offset",
+        .self_test          = "/sys/bus/i2c/devices/4-0068/inv_gyro/self_test",
+        .accel_en           = NULL,
+        .accel_fifo_en      = NULL,
+        .accel_fs           = NULL,
+        .clock_source       = NULL,
+        .early_suspend_en   = NULL,
+        .firmware_loaded    = NULL,
+        .gyro_en            = NULL,
+        .gyro_fifo_en       = NULL,
+        .key                = NULL,
+        .raw_accel          = NULL,
+        .reg_dump           = NULL,
+        .tap_on             = NULL,
+        .dmp_firmware       = NULL
+    },
+
+    {        /* MPU6050 */
+        .buffer             = "/dev/input/event0",
+        .enable             = "/sys/class/invensense/mpu/enable",
+        .raw_data           = "/sys/class/invensense/mpu/raw_gyro",
+        .temperature        = "/sys/class/invensense/mpu/temperature",
+        .fifo_rate          = "/sys/class/invensense/mpu/fifo_rate",
+        .power_state        = "/sys/class/invensense/mpu/power_state",
+        .fsr                = "/sys/class/invensense/mpu/FSR",
+        .lpf                = "/sys/class/invensense/mpu/lpf",
+        .scale              = "/sys/class/invensense/mpu/gyro_scale",
+        .temp_scale         = "/sys/class/invensense/mpu/temp_scale",
+        .temp_offset        = "/sys/class/invensense/mpu/temp_offset",
+        .self_test          = "/sys/class/invensense/mpu/self_test",
+        .accel_en           = "/sys/class/invensense/mpu/accl_enable",
+        .accel_fifo_en      = "/sys/class/invensense/mpu/accl_fifo_enable",
+        .accel_fs           = "/sys/class/invensense/mpu/accl_fs",
+        .clock_source       = "/sys/class/invensense/mpu/clock_source",
+        .early_suspend_en   = "/sys/class/invensense/mpu/early_suspend_enable",
+           .firmware_loaded = "/sys/class/invensense/mpu/firmware_loaded",
+        .gyro_en            = "/sys/class/invensense/mpu/gyro_enable",
+        .gyro_fifo_en       = "/sys/class/invensense/mpu/gyro_fifo_enable",
+        .key                = "/sys/class/invensense/mpu/key",
+        .raw_accel          = "/sys/class/invensense/mpu/raw_accl",
+        .reg_dump           = "/sys/class/invensense/mpu/reg_dump",
+        .tap_on             = "/sys/class/invensense/mpu/tap_on",
+         .dmp_firmware      = "/sys/class/invensense/mpu/dmp_firmware"
+    },
+
+    {        /* BMA250 */
+        .buffer             = "/dev/input/input/event1",
+        .enable             = "/sys/devices/virtual/input/input1/enable",
+        .raw_data           = "/sys/devices/virtual/input/input1/value",
+        .temperature        = NULL,
+        .fifo_rate          = NULL,
+        .power_state        = NULL,
+        .fsr                = NULL,
+        .lpf                = NULL,
+        .scale              = NULL,
+        .temp_scale         = NULL,
+        .temp_offset        = NULL,
+        .self_test          = NULL,
+        .accel_en           = NULL,
+        .accel_fifo_en      = NULL,
+        .accel_fs           = NULL,
+        .clock_source       = NULL,
+        .early_suspend_en   = NULL,
+        .firmware_loaded    = NULL,
+        .gyro_en            = NULL,
+        .gyro_fifo_en       = NULL,
+        .key                = NULL,
+        .raw_accel          = NULL,
+        .reg_dump           = NULL,
+        .tap_on             = NULL,
+        .dmp_firmware       = NULL
+    }
+};
+
+static void (*s_func_cb) (void);
+
+int inv_read_data(char *names, char *data)
+{
+    char str[8];
+    int count;
+    short s_data;
+
+    count = inv_sysfs_read((char*)names, sizeof(str), str);
+    if (count < 0)
+        return count;
+    count = sscanf(str, "%hd", &s_data);
+    *data = s_data;
+    if (count < 1)
+        return -EAGAIN;
+    return count;
+
+}
+
+void fifoCB(void)
+{
+    if (1) {
+        float gyro[3];
+        float accel[3];
+        float orient[3];
+        float rv[3];
+
+        int8_t accuracy;
+        inv_time_t timestamp;
+
+        printf("/*************************************************\n"); 
+        inv_get_sensor_type_gyroscope(gyro, &accuracy, &timestamp);
+        printf("Gyro %13.6f %13.4f %13.4f %5d %9lld\n", 
+                     gyro[0],
+                     gyro[1],
+                     gyro[2],
+                     accuracy,
+                     timestamp);
+
+        inv_get_sensor_type_accelerometer(accel, &accuracy, &timestamp);
+        printf("Accel %13.6f %13.4f %13.4f %5d %9lld\n", 
+                     accel[0],
+                     accel[1],
+                     accel[2],
+                     accuracy,
+                     timestamp);
+
+        inv_get_sensor_type_rotation_vector(rv, &accuracy, &timestamp);
+        printf("RV %7.3f %7.3f %7.3f %5d %9lld\n",
+                     rv[0],rv[1],rv[2],accuracy,timestamp);
+
+        inv_get_sensor_type_orientation(orient, &accuracy, &timestamp);
+        printf("Orientation %7.3f %7.3f %7.3f %5d %9lld\n",
+                     orient[0],orient[1],orient[2],accuracy,timestamp);
+        printf("/*************************************************\n"); 
+    }
+}
+
+unsigned short orient;
+signed char g_gyro_orientation[9] = {1, 0, 0,
+                                     0, 1, 0,
+                                     0, 0, 1};
+
+signed char g_accel_orientation[9] = {-1, 0, 0,
+                                      0, -1, 0,
+                                      0, 0, 1};
+float scale;
+float range;
+long sens;
+
+
+
+short mTempOffset = 0;
+short mTempScale = 0;
+bool mFirstRead = 1;
+
+/******************* FUNCTIONS *******************************/
+#if 0
+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;
+}
+#endif 
+
+inv_error_t inv_set_fifo_processed_callback(void (*func_cb)(void))
+{
+    s_func_cb = func_cb;
+    return INV_SUCCESS;
+}
+
+/**
+ * @brief Keyboard hit function.
+ */
+int kbhit(void)
+{
+#if 1
+    struct timeval tv;
+    fd_set read_fd;
+
+    tv.tv_sec=0;
+    tv.tv_usec=0;
+    FD_ZERO(&read_fd);
+    FD_SET(0,&read_fd);
+
+    if (select(1, &read_fd, NULL, NULL, &tv) == -1)
+        return 0;
+
+    if (FD_ISSET(0,&read_fd))
+        return 1;
+
+    return 0;
+#else
+    struct timeval tv;  
+    fd_set rdfs;   
+    
+    tv.tv_sec = 0;  
+    tv.tv_usec = 0;   
+    
+    FD_ZERO(&rdfs);  
+    FD_SET (STDIN_FILENO, &rdfs);   
+    
+    select(STDIN_FILENO+1, &rdfs, NULL, NULL, &tv);  
+    return FD_ISSET(STDIN_FILENO, &rdfs); 
+#endif
+}
+
+inv_error_t inv_constructor_default_enable()
+{
+    inv_error_t result;
+  
+    result = inv_enable_quaternion();
+    if (result) {
+        if (result == INV_ERROR_NOT_AUTHORIZED) {
+            LOGE("Enable Quaternion failed: not authorized");
+        }
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_enable_motion_no_motion();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_enable_gyro_tc();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_enable_hal_outputs();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_enable_9x_sensor_fusion();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    return result;
+}
+
+int read_attribute_sensor(int fd, char *data, unsigned int size)
+{
+    int count = 0;
+        if (fd >=0) {
+            count = read(fd, data, size);
+            if(count < 0) {
+                MPL_LOGE("read fails with error code=%d", count);
+            }
+            close(fd);
+        }
+        return count;
+}
+
+int inv_read_temperature(long long *data)
+{
+        int count = 0;
+        int fd;
+
+        if(mFirstRead) {
+            char buf[4];
+            fd = open(filenames[ITG3500].temp_scale, O_RDONLY);
+            if(fd < 0) {
+                MPL_LOGE("errors opening tempscale");
+                return -1;
+            }
+
+            memset(buf, 0, sizeof(buf));
+
+            count = read_attribute_sensor(fd, buf, sizeof(buf));
+            if(count < 0) {
+                MPL_LOGE("errors reading temp_scale");
+                return -1;
+            }
+
+            count = sscanf(buf, "%hd", &mTempScale);
+            if(count < 1)
+                return -1;
+            MPL_LOGI("temp scale = %d", mTempScale);
+ 
+            fd = open(filenames[ITG3500].temp_offset, O_RDONLY);
+            if(fd < 0) {
+                MPL_LOGE("errors opening tempoffset");
+                return -1;
+            }
+
+            memset(buf, 0, sizeof(buf));
+
+            count = read_attribute_sensor(fd, buf, sizeof(buf));
+            if(count < 0) {
+                MPL_LOGE("errors reading temp_offset");
+                return -1;
+            }
+
+            count = sscanf(buf, "%hd", &mTempOffset);
+            if(count < 1)
+                return -1;
+            MPL_LOGI("temp offset = %d", mTempOffset);
+
+            mFirstRead = false;
+        }
+
+        char raw_buf[25];
+        short raw;
+        long long timestamp;
+        fd = open(filenames[ITG3500].temperature, O_RDONLY);
+        if(fd < 0) {
+            MPL_LOGE("errors opening temperature");
+            return -1;
+        }
+
+        memset(raw_buf, 0, sizeof(raw_buf));
+
+        count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf));
+        if(count < 0) {
+            MPL_LOGE("errors reading temperature");
+            return -1;
+        }
+        count = sscanf(raw_buf, "%hd%lld", &raw, &timestamp);
+        if(count < -1)
+            return -1;
+        MPL_LOGI("temperature raw = %d, timestamp = %lld", raw, timestamp);
+        MPL_LOGI("temperature offset = %d", mTempOffset);
+        MPL_LOGI("temperature scale = %d", mTempScale);
+        int adjuster = 35 + ((raw-mTempOffset)/mTempScale);
+        MPL_LOGI("pre-scaled temperature = %d", adjuster);
+        MPL_LOGI("adjusted temperature = %d", adjuster*65536);
+        //data[0] = adjuster * 65536;
+        data[0] = (35 + ((raw - mTempOffset) / mTempScale)) * 65536.f;
+        data[1] = timestamp;
+        return 0;
+}
+
+int self_test(void)
+{
+        int err = 0;
+        char str[50];
+        char x[9], y[9], z[9];
+        char pass[2];
+        int fd;
+
+        fd = open((char*)filenames[DEVICE].self_test, O_RDONLY);
+        if(fd < 0) {
+            return fd;
+        }
+        memset(str, 0, sizeof(str));
+        err = read_attribute_sensor(fd, str, sizeof(str));
+        if(err < 0) {
+            return err;
+        }
+        MPL_LOGI("self_test result: %s", str);
+        printf("Self test result: %s ", str);
+        err = sscanf(str, "%[^','],%[^','],%[^','],%[^',']", x, y, z, pass);
+        if(err < 1) {
+            return err;
+        }
+        MPL_LOGI("Bias   : X:Y:Z (%ld, %ld, %ld)", atol(x), atol(y), atol(z));
+        //printf("Bias   : X:Y:Z (%ld, %ld, %ld)", atol(x), atol(y), atol(z));
+        if (atoi(pass)) {
+            MPL_LOGI("Result : PASS (1)");
+            printf("----> PASS (1)\n");
+        } else {
+            MPL_LOGI("Result : FAIL (0)");
+            printf("----> FAIL (0)\n");
+        }
+        return err;
+}
+
+/*******************************************************************************
+ ******************************* MAIN ******************************************
+ ******************************************************************************/
+
+/**
+ * @brief Main function
+ */
+int main(int argc, char *argv[])
+{
+    int key = 0;
+    int ready;
+    long accel[3];
+    short gyro[3];
+    long long timestamp = 0;
+    inv_error_t result;
+
+    char data;
+    unsigned char i;
+    int fd, bytes_read;
+    struct pollfd pfd;
+    unsigned long long time_stamp;
+    unsigned int time_H;
+    struct input_event ev[100];
+#ifdef INV_PLAYBACK_DBG
+    int logging = false;
+    FILE *logfile = NULL;
+#endif
+
+    result = inv_init_mpl();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    // Master Enabling.  This also turns on power_state
+    if (inv_sysfs_write((char *)filenames[DEVICE].enable, 1) < 0)
+       printf("ERR- Failed to enable event generation\n");
+    else {
+          inv_read_data((char *)filenames[DEVICE].enable, &data);
+       printf("Event enable= %d\n", data);
+    }
+
+    // Power ON - No need after master enable above but do it anyway
+    if (inv_sysfs_write((char *)filenames[DEVICE].power_state, 1) < 0)
+       printf("ERR- Failed to set power state=1\n");
+    else {
+          inv_read_data((char *)filenames[DEVICE].power_state, &data);
+       printf("Power state: %d\n", data);
+    }
+    
+    // Turn on tap
+    if (inv_sysfs_write((char *)filenames[DEVICE].tap_on, 1) < 0) {
+       printf("ERR- Failed to enable Tap On\n");
+    } 
+    else {
+          inv_read_data((char *)filenames[DEVICE].tap_on, &data);
+       printf("Tap-on: %d\n", data);
+    }
+
+    // Program DMP code.  No longer required to enable tap-on first
+    if ((result = 
+            inv_sysfs_write((char *)filenames[DEVICE].firmware_loaded, 0)) < 0) {
+        printf("ERR- Failed to initiate DMP re-programming %d\n",result);
+    } else {
+         if ((fd = open(filenames[DEVICE].dmp_firmware, O_WRONLY)) < 0 ) {
+            printf("ERR- Failed file open to write DMP\n");
+            close(fd);
+            exit(0);
+        } else {
+            // Program 200Hz version
+            //result = write(fd, DMP_IMAGE, sizeof(DMP_IMAGE));
+            //printf("Downloaded %d byte(s) to DMP\n", result);
+            result = inv_load_dmp(fd);
+            //LOG_RESULT_LOCATION(result);
+            close(fd);
+        }
+    }
+
+    // Query DMP running.  For now check by 'firmware_loaded' status 
+    if (inv_read_data((char *)filenames[DEVICE].firmware_loaded, &data) < 0) {
+        printf("ERR- Failed to read 'firmware_loaded'\n");
+    } else {
+        printf("Firmware Loaded/ DMP running: %d\n", data);
+    }
+
+    inv_set_fifo_processed_callback(fifoCB);
+    result = inv_constructor_default_enable();
+    result = inv_start_mpl();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    } else {
+        printf ("MPL started\n");
+    }
+
+    /* Gyro Setup */
+    orient = inv_orientation_matrix_to_scalar(g_gyro_orientation);
+    inv_set_gyro_orientation_and_scale(orient,2000L<<15);
+
+    /* Accel Setup */
+    orient = inv_orientation_matrix_to_scalar(g_accel_orientation);
+    /* NOTE: sens expected to be 2 (FSR) * 1L<<15 for 16 bit hardware data.
+     * The BMA250 only uses a 10 bit ADC, so we shift the data by 6 bits.
+     * 2 * 1L<<15 * 1<<6 ==  1LL<<22
+     */
+    inv_set_accel_orientation_and_scale(orient, 1LL<<22); 
+
+    // Enable Gyro
+    if (inv_sysfs_write((char *)filenames[DEVICE].gyro_en, 1) <0)
+        printf("ERR- Failed to enable Gyro\n");
+    else {
+        inv_read_data((char *)filenames[DEVICE].gyro_en, &data);
+        printf("Gyro enable: %d\n", data);
+    }
+
+    // Enable Accel
+    if (inv_sysfs_write((char *)filenames[DEVICE].accel_en, 1) <0)
+        printf("ERR- Failed to enable Accel\n");
+    else {
+        inv_read_data((char *)filenames[DEVICE].accel_en, &data);
+        printf("Accel enable: %d\n", data);
+    }
+
+    // polling for data
+    fd = open(filenames[DEVICE].buffer, O_RDONLY);
+    if(fd < 0) {
+         MPL_LOGE("Cannot open device event buffer");
+    }
+    
+    pfd.fd = fd;
+    pfd.events = POLLIN;
+
+    while (1) {
+
+       result = kbhit();
+        if (result) {
+           key = getchar();
+        } else {
+           key = 0;
+        }
+        if (key == 'l') {
+           MPL_LOGI(" 'l' - load calibration file");
+           inv_load_calibration();
+        }
+        if (key == 't') {
+           MPL_LOGI(" 't' - self test");
+           self_test();
+        }
+        if (key == 'q') {
+            MPL_LOGI(" 'q' - store calibration file");
+            inv_store_calibration();
+            break;
+        }
+#ifdef INV_PLAYBACK_DBG
+        if (key == 's') {
+            if (!logging) {
+                MPL_LOGI(" 's' - toggle logging on");
+                logfile = fopen("/data/playback.bin", "wb");
+                if (logfile) {
+                    inv_turn_on_data_logging(logfile);
+                    logging = true;
+                } else {
+                    MPL_LOGI("Error : "
+                             "cannot open log file '/data/playback.bin'");
+                }
+            } else {
+                MPL_LOGI(" 's' - toggle logging off");
+                inv_turn_off_data_logging();
+                fclose(logfile);
+                logging = false;
+            }
+            break;
+        }
+#endif
+
+        ready = poll(&pfd, 1, 100);
+        if (ready) {
+            bytes_read = read_attribute_sensor(fd, (char *)ev,
+                                      sizeof(struct input_event) * SIX_AXES);
+            //bytes_read= read(fd, &ev, sizeof(struct input_event) * SIX_AXES);
+            if (bytes_read > 0) {
+                int executed;
+
+                for (i = 0; i < bytes_read / sizeof(struct input_event); i++) {
+                    if (ev[i].type == EV_REL) {
+                        switch (ev[i].code) {
+                        case REL_X:
+                            printf("REL_X\n");
+                            gyro[0]= ev[i].value;                    //Gyro X
+                            printf("Gyro X:%5d ", gyro[0]);        
+                            break;
+                        case REL_Y:
+                            printf("REL_Y\n");
+                            gyro[1]= ev[i].value;                    //Gyro Y
+                            printf("Gyro Y:%5d ", gyro[1]);    
+                            break;
+                        case REL_Z:
+                            printf("REL_Z\n");
+                            gyro[2]= ev[i].value;                    //Gyro Z
+                            printf("Gyro Z:%5d ", gyro[2]);        
+                            break;
+                        case REL_RX:
+                            printf("REL_RX\n");
+                            accel[0]= ev[i].value;                   //Accel X
+                            printf("Accl X:%5ld ", accel[0]);
+                            break;
+                        case REL_RY:
+                            printf("REL_RY\n");
+                            accel[1]= ev[i].value;                   //Accel Y
+                            printf("Accl Y:%5ld ", accel[1]);    
+                            break;
+                        case REL_RZ:
+                            printf("REL_RZ\n");
+                            accel[2]= ev[i].value;                   //Accel Z
+                            printf("Accl Z:%5ld ", accel[2]);    
+                            break;
+                        case REL_MISC:
+                            time_H= ev[i].value;
+                            break;
+                        case REL_WHEEL:
+                            time_stamp = ((unsigned long long)(time_H) << 32) + 
+                                           (unsigned int)ev[i].value;
+                            break;
+                        default:
+                            printf("ERR- Un-recognized event code:  %5d ", ev[i].code);
+                            break;
+                        }
+                    } else {
+#if 0
+                        clock_gettime(CLOCK_MONOTONIC, &timer);
+                        curr_time= timer.tv_nsec + timer.tv_sec * 1000000000LL;                        
+                        printf("Curr time= %lld, Dev time stamp= %lld, Time diff= %d ms\n", curr_time, time_stamp, (curr_time-time_stamp)/1000000LL);
+#endif
+                    }
+                }
+
+                // build & process gyro + accel data
+                result = inv_build_gyro(gyro, (inv_time_t)timestamp, &executed);
+                if (result) {
+                    LOG_RESULT_LOCATION(result);
+                } else if ((result = inv_build_accel(accel, 0,
+                                                    (inv_time_t)timestamp,
+                                                    &executed))) {
+                    LOG_RESULT_LOCATION(result);
+                } 
+                if (executed) {
+                    printf("Exec on data Ok\n");
+                    s_func_cb();
+                }
+
+           } else {
+               //printf ("ERR- No data!\n");
+           }
+
+        } else { MPL_LOGV("Device not ready"); }
+    }
+    close(fd);
+    
+#ifdef INV_PLAYBACK_DBG
+    if (logging) {
+        inv_turn_off_data_logging();
+        fclose(logfile);
+    }
+#endif
+
+    return 0;
+}
diff --git a/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared b/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared
new file mode 100644
index 0000000..5d52b21
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared
diff --git a/libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk b/libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk
new file mode 100644
index 0000000..7f6cc43
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk
@@ -0,0 +1,109 @@
+EXEC = input_gyro$(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/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk b/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk
new file mode 100644
index 0000000..0936212
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk
@@ -0,0 +1,13 @@
+#### filelist.mk for input_gyro ####
+
+# helper headers
+HEADERS := $(MPL_DIR)/authenticate.h
+#HEADERS += 
+
+# sources
+SOURCES := $(APP_DIR)/test_input_gyro.c
+
+INV_SOURCES += $(SOURCES)
+
+#VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux
+VPATH += $(APP_DIR)
diff --git a/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c b/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c
new file mode 100644
index 0000000..6fa9aab
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c
@@ -0,0 +1,485 @@
+/*
+ * input interface testing
+ */
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <time.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <linux/input.h>
+#include <linux/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include "linux/ml_sysfs_helper.h"
+#include "authenticate.h"
+#include "ml_load_dmp.h"
+
+#if 0
+struct input_event {
+       struct timeval time;
+       __u16 type;
+       __u16 code;
+       __s32 value;
+};
+#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);
+}
+
+static void read_compass(int event_number) 
+{
+    int ev_size, ret_byte, ii;
+    int fd = -1;
+    struct input_event ev[10];
+    char name[64];
+    char file_name[64];
+    unsigned int RX;
+    unsigned long long time0, time1, time2;
+    struct timespec tsp;
+    ev_size = sizeof(struct input_event);
+    sprintf(file_name, "/dev/input/event%d", event_number);
+    if ((fd = open(file_name, O_RDONLY)) < 0 ) {
+        printf("fail to open compass\n");
+        return;
+    }
+
+    /* NOTE: Use this to pass device name to HAL. */
+    ioctl (fd, EVIOCGNAME (sizeof (name)), name);
+    printf ("Reading From : (%s)\n", name);
+    while (1) {
+    clock_gettime(CLOCK_MONOTONIC, &tsp);
+        /*read compass data here */
+        if(fd > 0){
+            ret_byte = read(fd, ev, ev_size);
+        } else {
+            ret_byte = -1;
+        }
+        time0 = tsp.tv_nsec/1000000 + tsp.tv_sec * 1000LL;
+        if (ret_byte < 0)
+            continue;
+        for (ii = 0; ii < ret_byte/ev_size; ii++) {
+            if(EV_REL != ev[ii].type) {
+                time2 = ev[ii].time.tv_usec/1000 + ev[ii].time.tv_sec * 1000LL;
+                printf("mono=%lldms, diff=%d\n", time2, (int)(time1-time0));
+                continue;
+            }
+            switch (ev[ii].code) {
+            case REL_X:
+                printf("CX:%5d ", ev[ii].value);
+                break;
+            case REL_Y:
+                printf("CY:%5d ", ev[ii].value);
+                break;
+            case REL_Z:
+                printf("CZ:%5d ", ev[ii].value);
+                break;
+            case REL_MISC:
+                RX = ev[ii].value;
+                break;
+            case REL_WHEEL:
+                time1 = ((unsigned long long)(RX)<<32) + (unsigned int)ev[ii].value;
+                time1 = time1/1000000;
+                printf("time1: %lld ", time1);
+                break;
+            default:
+                printf("GES?:  %5d ", ev[ii].code);
+                break;
+            }
+        }
+    }
+    close(fd);
+}
+
+static void read_gesture(int num)
+{
+    int ev_size, ret_byte, ii;
+    int fd = -1;
+    struct input_event ev[10];
+    char name[64];
+    char file_name[64];
+    unsigned long long time;
+    struct timespec tsp;
+    ev_size = sizeof(struct input_event);
+    sprintf(file_name, "/dev/input/event%d", num);
+    MPL_LOGI("%s\n", file_name);
+    if ((fd = open(file_name, O_RDONLY)) < 0 ) {
+        printf("fail to open gusture.\n");
+        return;
+    }
+
+    /* NOTE: Use this to pass device name to HAL. */
+    ioctl (fd, EVIOCGNAME (sizeof (name)), name);
+    printf ("Reading From : (%s)\n", name);
+    while(1){
+        clock_gettime(CLOCK_MONOTONIC, &tsp);
+        if(fd > 0){
+            ret_byte = read(fd, ev, ev_size);
+        } else {
+            ret_byte = -1;
+        }
+        time = tsp.tv_nsec + tsp.tv_sec * 1000000000LL;
+        //printf("retbyte=%d, ev3=%d\n", ret_byte, ev_size*3);
+        if (ret_byte < 0)
+            continue;
+        for (ii = 0; ii < ret_byte/ev_size; ii++) {
+            if(EV_REL != ev[ii].type) {
+                time = ev[ii].time.tv_usec + ev[ii].time.tv_sec * 1000000LL;
+                printf("mono=%lld\n", time);
+                continue;
+            }
+            switch (ev[ii].code) {
+            case REL_RX:
+                printf("GESX:%5x\n", ev[ii].value);
+                HandleTap(ev[ii].value);
+                break;
+            case REL_RY:
+                printf("GESY:%5x\n", ev[ii].value);
+                HandleOrient(ev[ii].value);
+                break;
+            case REL_RZ:
+                printf("FLICK:%5x\n", ev[ii].value);
+                break;
+            default:
+                printf("?:  %5d ", ev[ii].code);
+                break;
+            }
+        }
+        }
+}
+
+static void read_gyro_accel(int num)
+{
+    int ev_size, ret_byte, ii;
+    int fd = -1;
+    unsigned int RX;
+    struct input_event ev[10];
+    char name[64];
+    char file_name[64];
+    unsigned long long time0, time1, time2;
+    struct timespec tsp;
+    ev_size = sizeof(struct input_event);
+    sprintf(file_name, "/dev/input/event%d", num);
+    if ((fd = open(file_name, O_RDONLY)) < 0 ) {
+        printf("fail to open gyro/accel\n");
+        return;
+    }    
+        
+    /* NOTE: Use this to pass device name to HAL. */
+    ioctl (fd, EVIOCGNAME (sizeof (name)), name);
+    printf ("Reading From : (%s)\n", name);
+    while (1){
+        //usleep(20000);
+        ret_byte = read(fd, ev, ev_size);
+        if (ret_byte < 0)
+            continue;
+        //ret_byte = 0;
+
+        for (ii = 0; ii < ret_byte/ev_size; ii++) {
+            if(EV_REL != ev[ii].type) {
+                time0 = ev[ii].time.tv_usec/1000 + ev[ii].time.tv_sec * 1000LL;
+                printf("T: %lld diff=%d ", time0, (int)(time1 - time0));
+                clock_gettime(CLOCK_MONOTONIC, &tsp);
+                time2 = tsp.tv_nsec/1000000 + tsp.tv_sec * 1000LL;
+                printf("mono=%lld, diff2=%d\n", time2, (int)(time1 - time2));
+                continue;
+            }
+            switch (ev[ii].code) {
+            case REL_X:
+                printf("GX:%5d ", ev[ii].value);
+                break;
+            case REL_Y:
+                printf("GY:%5d ", ev[ii].value);
+                break;
+            case REL_Z:
+                printf("GZ:%5d ", ev[ii].value);
+                break;
+            case REL_RX:
+                printf("AX:%5d ", ev[ii].value);
+                break;
+            case REL_RY:
+                printf("AY:%5d ", ev[ii].value);
+                break;
+            case REL_RZ:
+                printf("AZ:%5d ", ev[ii].value);
+                break;
+            case REL_MISC:
+                RX = ev[ii].value;
+                break;
+            case REL_WHEEL:
+                time1 = ((unsigned long long)(RX)<<32) + (unsigned int)ev[ii].value;
+                time1 = time1/1000000;
+                printf("time1: %lld ", time1);
+                break;
+            default:
+                printf("?:  %5d ", ev[ii].code);
+                break;
+            }
+        }
+    }
+    close(fd);
+}
+int inv_sysfs_write(char *filename, long data)
+{
+    FILE *fp;
+    int count;
+
+    if (!filename)
+        return -1;
+    fp = fopen(filename, "w");
+    if (!fp)
+        return -errno;
+    count = fprintf(fp, "%ld", data);
+    fclose(fp);
+    return count;
+}
+int inv_sysfs_read(char *filename, long num_bytes, char *data)
+{
+    FILE *fp;
+    int count;
+
+    if (!filename)
+        return -1;
+    fp = fopen(filename, "r");
+    if (!fp)
+        return -errno;
+    count = fread(data, 1, num_bytes, fp);
+    fclose(fp);
+    return count;
+}
+
+void enable_flick(char *p)
+{
+    char sysfs_file[200];
+    printf("flick:%s\n", p);
+    sprintf(sysfs_file, "%s/flick_int_on", p);
+    inv_sysfs_write(sysfs_file, 1);
+    sprintf(sysfs_file, "%s/flick_upper", p);
+    inv_sysfs_write(sysfs_file, 3147790);
+    sprintf(sysfs_file, "%s/flick_lower", p);
+    inv_sysfs_write(sysfs_file, -3147790);
+    sprintf(sysfs_file, "%s/flick_counter", p);
+    inv_sysfs_write(sysfs_file, 50);
+    sprintf(sysfs_file, "%s/flick_message_on", p);
+    inv_sysfs_write(sysfs_file, 0);
+    sprintf(sysfs_file, "%s/flick_axis", p);
+    inv_sysfs_write(sysfs_file, 2);
+}
+
+void setup_dmp(char *sysfs_path)
+{
+    char sysfs_file[200];
+    char firmware_loaded[200], dmp_path[200];
+    char dd[10];
+    
+    inv_get_dmpfile(dmp_path);
+    sprintf(sysfs_file, "%s/fifo_rate", sysfs_path);
+    inv_sysfs_write(sysfs_file, 200);    
+    sprintf(sysfs_file, "%s/FSR", sysfs_path);
+    inv_sysfs_write(sysfs_file, 2000);    
+    sprintf(sysfs_file, "%s/accl_fs", sysfs_path);
+    inv_sysfs_write(sysfs_file, 4);
+    /*
+    sprintf(firmware_loaded, "%s/%s", sysfs_path, "firmware_loaded");
+    printf("%s\n", firmware_loaded);
+    inv_sysfs_write(firmware_loaded, 0);
+    inv_sysfs_read(firmware_loaded, 1, dd);
+    printf("beforefirmware_loaded=%c\n", dd[0]);
+
+    if ((fd = open(dmp_path, O_WRONLY)) < 0 ) {
+        perror("dmp fail");
+    }
+    inv_load_dmp(fd);
+    close(fd);
+    */
+    inv_sysfs_read(firmware_loaded, 1, dd);
+    printf("firmware_loaded=%c\n", dd[0]);
+}
+void read_pedometer(char *sysfs_path){
+    int steps;
+    char sysfs_file[200];
+    char dd[4];
+    sprintf(sysfs_file, "%s/pedometer_steps", sysfs_path);
+    inv_sysfs_read(sysfs_file, 4, dd);
+    steps = dd[0] << 8 | dd[1];
+    printf("fff=%d\n", steps);
+}
+/* The running sequence:
+    "input_gyro 2 &". 
+    This will setup the dmp firmware and let it run on background. 
+    tap and flick will work at this time. 
+    To see accelerometer data and gyro data.
+    type :
+    "input_gyro ". 
+    This will print out gyro data and accelerometer data
+    To see Compass data
+    type:
+    "input_gyro 1" */
+
+int main(int argc, char *argv[])
+{
+    unsigned int RX, i, sel;
+    unsigned char key[16];
+    struct timeval tv;
+    struct timespec tsp0, tsp1, tsp2, tsp3;
+    int event_num;
+    char sysfs_path[200];
+    char chip_name[20];
+    char sysfs_file[200];
+    if (INV_SUCCESS != inv_check_key()) {
+        printf("key check fail\n");
+        exit(0);
+    }else
+        printf("key authenticated\n");
+
+    for(i=0;i<16;i++){
+        key[i] = 0xff;
+    }
+    RX = inv_get_sysfs_key(key);
+    if(RX == INV_SUCCESS){
+        for(i=0;i<16;i++){
+            printf("%d, ", key[i]);
+        }
+        printf("\n");
+    }else{
+        printf("get key failed\n");
+    }
+    memset(sysfs_path, 0, 200);
+    memset(sysfs_file, 0, 200);
+    memset(chip_name, 0, 20);
+    inv_get_sysfs_path(sysfs_path);
+    inv_get_chip_name(chip_name);
+    printf("sysfs path: %s\n", sysfs_path);
+    printf("chip name: %s\n", chip_name);
+    /*set up driver*/
+    sprintf(sysfs_file, "%s/enable", sysfs_path);
+    inv_sysfs_write(sysfs_file, 0);
+    sprintf(sysfs_file, "%s/power_state", sysfs_path);
+    inv_sysfs_write(sysfs_file, 1);    
+    if ((getuid ()) != 0)
+        printf ("You are not root! This may not work...\n");
+    
+    if(argc ==2 )
+        sel = argv[1][0] - 0x30;
+    else
+        sel = 0;
+    switch(sel){
+    case 0:
+        printf("-------------------------------\n");
+        printf("--- log gyro and accel data ---\n");
+        printf("-------------------------------\n");
+        sprintf(sysfs_file, "%s/enable", sysfs_path);
+        inv_sysfs_write(sysfs_file, 1);
+        if(inv_get_handler_number(chip_name, &event_num) < 0)
+            printf("mpu not installed\n");
+        else
+            read_gyro_accel(event_num);
+         break;
+         
+    case 1:
+        printf("------------------------\n");
+        printf("--- log compass data ---\n");
+        printf("------------------------\n");
+        sprintf(sysfs_file, "%s/compass_enable", sysfs_path);
+        inv_sysfs_write(sysfs_file, 1);
+        sprintf(sysfs_file, "%s/enable", sysfs_path);
+        inv_sysfs_write(sysfs_file, 1);
+        if(inv_get_handler_number("INV_COMPASS", &event_num) < 0)
+            printf("compass is not enabled\n");
+        else
+            read_compass(event_num);
+        break;
+        
+    case 2:
+        printf("--------------------\n");
+        printf("--- log gestures ---\n");
+        printf("--------------------\n");
+        setup_dmp(sysfs_path);
+        enable_flick(sysfs_path);
+        sprintf(sysfs_file, "%s/tap_on", sysfs_path);
+        inv_sysfs_write(sysfs_file, 1);
+        sprintf(sysfs_file, "%s/enable", sysfs_path);
+        inv_sysfs_write(sysfs_file, 1);
+        if(inv_get_handler_number("INV_DMP", &event_num) < 0)
+            printf("DMP not enabled\n");
+        else
+            read_gesture(event_num);
+        break;
+        
+    case 3:
+        printf("-----------------\n");
+        printf("--- pedometer ---\n");
+        printf("-----------------\n");
+        read_pedometer(sysfs_path);
+        break;
+        
+    default:
+        printf("error choice\n");
+        break;
+    }
+    
+    gettimeofday(&tv, NULL);
+    clock_gettime(CLOCK_MONOTONIC, &tsp1);
+    clock_gettime(CLOCK_REALTIME, &tsp0);
+
+    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &tsp2);
+    clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tsp3);
+    //printf("id=%d, %d, %d, %d\n", CLOCK_MONOTONIC,  CLOCK_REALTIME,
+    //    CLOCK_PROCESS_CPUTIME_ID, CLOCK_THREAD_CPUTIME_ID);
+    //printf("sec0=%lu , nsec=%ld\n", tsp0.tv_sec, tsp0.tv_nsec);
+    //printf("sec1=%lu , nsec=%ld\n", tsp1.tv_sec, tsp1.tv_nsec);
+    //printf("sec=%lu , nsec=%ld\n", tsp2.tv_sec, tsp2.tv_nsec);
+    //printf("sec=%lu , nsec=%ld\n", tsp3.tv_sec, tsp3.tv_nsec);
+
+    //ioctl (fd, EVIOCGNAME (sizeof (name)), name);
+    //printf ("Reading From : %s (%s)\n", argv[1], name);
+
+
+    return 0;
+}
+
diff --git a/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared b/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared
new file mode 100644
index 0000000..33c9eef
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared
diff --git a/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk b/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk
new file mode 100644
index 0000000..3a055cc
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk
@@ -0,0 +1,109 @@
+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 += -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/libsensors_iio/software/simple_apps/self_test/build/filelist.mk b/libsensors_iio/software/simple_apps/self_test/build/filelist.mk
new file mode 100644
index 0000000..492f47e
--- /dev/null
+++ b/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/libsensors_iio/software/simple_apps/self_test/inv_self_test.c b/libsensors_iio/software/simple_apps/self_test/inv_self_test.c
new file mode 100644
index 0000000..4f9996c
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/self_test/inv_self_test.c
@@ -0,0 +1,264 @@
+/**
+ *  Self Test application for Invensense's MPU6050/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"
+
+#ifndef ABS
+#define ABS(x)(((x) >= 0) ? (x) : -(x))
+#endif
+
+/** Change this key if the data being stored by this file changes */
+#define INV_DB_SAVE_KEY 53394
+
+#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;
+
+struct inv_sysfs_names_s {
+    char *enable;
+    char *power_state;
+    char *self_test;
+    char *temperature;
+    char *accl_bias;
+};
+
+const struct inv_sysfs_names_s mpu= {
+     /* MPU6050 & MPU9150 */
+    .enable         = "/sys/class/invensense/mpu/enable",
+    .power_state    = "/sys/class/invensense/mpu/power_state",
+    .self_test      = "/sys/class/invensense/mpu/self_test",
+    .temperature    = "/sys/class/invensense/mpu/temperature",
+    .accl_bias      = "/sys/class/invensense/mpu/accl_bias"
+};
+
+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];
+};
+
+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;
+}
+
+int inv_sysfs_write(char *filename, long data)
+{
+    FILE *fp;
+    int count;
+
+    if (!filename)
+        return -1;
+    fp = fopen(filename, "w");
+    if (!fp)
+        return -errno;
+    count = fprintf(fp, "%ld", data);
+    fclose(fp);
+    return count;
+}
+
+/**
+ *  Main Self test 
+ */
+int main(int argc, char **argv)
+{
+    FILE *fptr;
+    int self_test_status;
+    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;
+
+    // Initialize storage manager
+    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 (inv_sysfs_write(mpu.power_state, 1) <0) {
+        printf("ERR- Failed to set power state=1\n");
+    } else {
+        // Note: Driver turns on power automatically when self-test invoked
+    }
+
+    fptr = fopen(mpu.self_test, "r");
+    if (fptr != NULL) {
+        // Invoke self-test and read gyro bias
+        fscanf(fptr, "%d,%d,%d,%d",
+               &gyro_bias[0].i, &gyro_bias[1].i, &gyro_bias[2].i, &self_test_status);
+
+        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);
+        printf("Self-Test:Gyro bias data[0..2] read from Driver= [%d %d %d]\n",gyro_bias[0].i, gyro_bias[1].i, gyro_bias[2].i);
+        fclose(fptr);
+        
+        if (!(self_test_status & GYRO_PASS_STATUS_BIT)) {
+            // Reset gyro bias data if gyro self-test failed
+            memset(gyro_bias,0, sizeof(gyro_bias));
+            printf("Self-Test:Failed Gyro self-test\n");
+        }
+
+        if (self_test_status & ACCEL_PASS_STATUS_BIT) {
+            // Read Accel Bias
+            fptr= fopen(mpu.accl_bias, "r");
+            if (fptr != NULL) {
+                fscanf(fptr, "%d,%d,%d", &accel_bias[0].i, &accel_bias[1].i, &accel_bias[2].i);
+                printf("Self-Test:Accel bias data[0..2] read from Driver= [%d %d %d]\n", accel_bias[0].i, accel_bias[1].i, accel_bias[2].i);
+                fclose(fptr);
+            } else {
+                printf("Self-Test:ERR-Couldn't read accel bias\n");
+            }
+        } else {
+            memset(accel_bias,0, sizeof(accel_bias));
+            printf("Self-Test:Failed Accel self-test\n");
+        }
+
+        // Read temperature
+        if (self_test_status & (GYRO_PASS_STATUS_BIT|ACCEL_PASS_STATUS_BIT))
+        {
+            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");
+            }
+        }
+        
+    } else {
+        printf("Self-Test:ERR-Couldn't invoke self-test\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");
+        return -1;
+    }
+
+    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);
+    return result;
+}
+