Merge tag 'android-4.4_r1' into cm-11.0cm-11.0-XNPH44S-bacon-5fa8c79c0bcm-11.0-XNPH33R-bacon-3628510d76cm-11.0-XNPH30O-bacon-4f280f505acm-11.0-XNPH25R-bacon-d22b777afacm-11.0-XNPH22R-bacon-03d77315eacm-11.0-XNPH05Q-tomato-9828f8e9cccm-11.0-XNPH05Q-bacon-5229c4ef56stable/cm-11.0-XNG3Cstable/cm-11.0-XNG2Sstable/cm-11.0-XNF9Xstable/cm-11.0-XNF8Ystable/cm-11.0shipping/cm-11.0cm-11.0

Android 4.4 Release 1.0

242 files changed, 19001 insertions, 2631 deletions

diff --git a/60xx/Android.mk b/60xx/Android.mk
new file mode 100644
index 0000000..427bbb4
--- /dev/null
+++ b/60xx/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/Android.mk b/60xx/libsensors/Android.mk
index 06c01e3..a324dae 100644
--- a/libsensors/Android.mk
+++ b/60xx/libsensors/Android.mk
@@ -30,13 +30,13 @@ LOCAL_CFLAGS += -DCONFIG_MPU_SENSORS_MPU3050=1
 
 LOCAL_SRC_FILES := SensorBase.cpp MPLSensor.cpp 
 
-LOCAL_C_INCLUDES += hardware/invensense/mlsdk/platform/include
-LOCAL_C_INCLUDES += hardware/invensense/mlsdk/platform/include/linux
-LOCAL_C_INCLUDES += hardware/invensense/mlsdk/platform/linux
-LOCAL_C_INCLUDES += hardware/invensense/mlsdk/mllite
-LOCAL_C_INCLUDES += hardware/invensense/mlsdk/mldmp
-LOCAL_C_INCLUDES += hardware/invensense/mlsdk/external/aichi
-LOCAL_C_INCLUDES += hardware/invensense/mlsdk/external/akmd
+LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/platform/include
+LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/platform/include/linux
+LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/platform/linux
+LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/mllite
+LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/mldmp
+LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/external/aichi
+LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/external/akmd
 
 LOCAL_SHARED_LIBRARIES := liblog libcutils libutils libdl libmllite libmlplatform
 LOCAL_CPPFLAGS+=-DLINUX=1
diff --git a/libsensors/MPLSensor.cpp b/60xx/libsensors/MPLSensor.cpp
index e766a7f..d669bf4 100644
--- a/libsensors/MPLSensor.cpp
+++ b/60xx/libsensors/MPLSensor.cpp
@@ -73,25 +73,25 @@ extern "C" {
 static struct sensor_t sSensorList[] =
     { { "MPL Gyroscope", "Invensense", 1,
          SENSORS_GYROSCOPE_HANDLE,
-         SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, { } },
+         SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, { } },
       { "MPL Accelerometer", "Invensense", 1,
          SENSORS_ACCELERATION_HANDLE,
-         SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, { } },
+         SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, { } },
       { "MPL Magnetic Field", "Invensense", 1,
          SENSORS_MAGNETIC_FIELD_HANDLE,
-         SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, { } },
+         SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, { } },
       { "MPL Orientation", "Invensense", 1,
          SENSORS_ORIENTATION_HANDLE,
-         SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, { } },
+         SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, { } },
       { "MPL Rotation Vector", "Invensense", 1,
          SENSORS_ROTATION_VECTOR_HANDLE,
-         SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, { } },
+         SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, { } },
       { "MPL Linear Acceleration", "Invensense", 1,
          SENSORS_LINEAR_ACCEL_HANDLE,
-         SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, { } },
+         SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, { } },
       { "MPL Gravity", "Invensense", 1,
          SENSORS_GRAVITY_HANDLE,
-         SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, { } },
+         SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, { } },
 };
 
 static unsigned long long irq_timestamp = 0;
diff --git a/libsensors/MPLSensor.h b/60xx/libsensors/MPLSensor.h
index 5b5d121..5b5d121 100644
--- a/libsensors/MPLSensor.h
+++ b/60xx/libsensors/MPLSensor.h
diff --git a/libsensors/SensorBase.cpp b/60xx/libsensors/SensorBase.cpp
index 79b1ee2..79b1ee2 100644
--- a/libsensors/SensorBase.cpp
+++ b/60xx/libsensors/SensorBase.cpp
diff --git a/libsensors/SensorBase.h b/60xx/libsensors/SensorBase.h
index bb4d055..bb4d055 100644
--- a/libsensors/SensorBase.h
+++ b/60xx/libsensors/SensorBase.h
diff --git a/libsensors/sensor_params.h b/60xx/libsensors/sensor_params.h
index 4925ac4..4925ac4 100644
--- a/libsensors/sensor_params.h
+++ b/60xx/libsensors/sensor_params.h
diff --git a/libsensors/sensors.h b/60xx/libsensors/sensors.h
index 5c56da0..5c56da0 100644
--- a/libsensors/sensors.h
+++ b/60xx/libsensors/sensors.h
diff --git a/libsensors_iio/Android.mk b/60xx/libsensors_iio/Android.mk
index af52ea6..af52ea6 100644
--- a/libsensors_iio/Android.mk
+++ b/60xx/libsensors_iio/Android.mk
diff --git a/libsensors_iio/CompassSensor.IIO.9150.cpp b/60xx/libsensors_iio/CompassSensor.IIO.9150.cpp
index ce96564..ce96564 100644
--- a/libsensors_iio/CompassSensor.IIO.9150.cpp
+++ b/60xx/libsensors_iio/CompassSensor.IIO.9150.cpp
diff --git a/libsensors_iio/CompassSensor.IIO.9150.h b/60xx/libsensors_iio/CompassSensor.IIO.9150.h
index 6a51338..6a51338 100644
--- a/libsensors_iio/CompassSensor.IIO.9150.h
+++ b/60xx/libsensors_iio/CompassSensor.IIO.9150.h
diff --git a/libsensors_iio/InputEventReader.cpp b/60xx/libsensors_iio/InputEventReader.cpp
index ca0a61a..ca0a61a 100644
--- a/libsensors_iio/InputEventReader.cpp
+++ b/60xx/libsensors_iio/InputEventReader.cpp
diff --git a/libsensors_iio/InputEventReader.h b/60xx/libsensors_iio/InputEventReader.h
index 5c752db..5c752db 100644
--- a/libsensors_iio/InputEventReader.h
+++ b/60xx/libsensors_iio/InputEventReader.h
diff --git a/libsensors_iio/License.txt b/60xx/libsensors_iio/License.txt
index 930f931..930f931 100644
--- a/libsensors_iio/License.txt
+++ b/60xx/libsensors_iio/License.txt
diff --git a/libsensors_iio/MPLSensor.cpp b/60xx/libsensors_iio/MPLSensor.cpp
index 725a4fc..a80e385 100644
--- a/libsensors_iio/MPLSensor.cpp
+++ b/60xx/libsensors_iio/MPLSensor.cpp
@@ -89,33 +89,33 @@ static struct sensor_t sSensorList[] =
 {
     {"MPL Gyroscope", "Invensense", 1,
      SENSORS_GYROSCOPE_HANDLE,
-     SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, {}},
+     SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
     {"MPL Raw Gyroscope", "Invensense", 1,
      SENSORS_RAW_GYROSCOPE_HANDLE,
-     SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, {}},
+     SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
     {"MPL Accelerometer", "Invensense", 1,
      SENSORS_ACCELERATION_HANDLE,
-     SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, {}},
+     SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
     {"MPL Magnetic Field", "Invensense", 1,
      SENSORS_MAGNETIC_FIELD_HANDLE,
-     SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, {}},
+     SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
     {"MPL Orientation", "Invensense", 1,
      SENSORS_ORIENTATION_HANDLE,
-     SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, {}},
+     SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, {}},
     {"MPL Rotation Vector", "Invensense", 1,
      SENSORS_ROTATION_VECTOR_HANDLE,
-     SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, {}},
+     SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
     {"MPL Linear Acceleration", "Invensense", 1,
      SENSORS_LINEAR_ACCEL_HANDLE,
-     SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, {}},
+     SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
     {"MPL Gravity", "Invensense", 1,
      SENSORS_GRAVITY_HANDLE,
-     SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, {}},
+     SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
 
 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
     {"MPL Screen Orientation", "Invensense ", 1,
      SENSORS_SCREEN_ORIENTATION_HANDLE,
-     SENSOR_TYPE_SCREEN_ORIENTATION, 100.0f, 1.0f, 1.1f, 0, {}},
+     SENSOR_TYPE_SCREEN_ORIENTATION, 100.0f, 1.0f, 1.1f, 0, 0, 0, {}},
 #endif
 };
 
diff --git a/libsensors_iio/MPLSensor.h b/60xx/libsensors_iio/MPLSensor.h
index 4162324..4162324 100644
--- a/libsensors_iio/MPLSensor.h
+++ b/60xx/libsensors_iio/MPLSensor.h
diff --git a/libsensors_iio/MPLSupport.cpp b/60xx/libsensors_iio/MPLSupport.cpp
index 9773562..9773562 100644
--- a/libsensors_iio/MPLSupport.cpp
+++ b/60xx/libsensors_iio/MPLSupport.cpp
diff --git a/libsensors_iio/MPLSupport.h b/60xx/libsensors_iio/MPLSupport.h
index 98e4497..98e4497 100644
--- a/libsensors_iio/MPLSupport.h
+++ b/60xx/libsensors_iio/MPLSupport.h
diff --git a/libsensors_iio/SensorBase.cpp b/60xx/libsensors_iio/SensorBase.cpp
index 21d0ed0..21d0ed0 100644
--- a/libsensors_iio/SensorBase.cpp
+++ b/60xx/libsensors_iio/SensorBase.cpp
diff --git a/libsensors_iio/SensorBase.h b/60xx/libsensors_iio/SensorBase.h
index fef47c7..fef47c7 100644
--- a/libsensors_iio/SensorBase.h
+++ b/60xx/libsensors_iio/SensorBase.h
diff --git a/libsensors_iio/libmllite.so b/60xx/libsensors_iio/libmllite.so
index 9bdd5bc..9bdd5bc 100755
--- a/libsensors_iio/libmllite.so
+++ b/60xx/libsensors_iio/libmllite.so
diff --git a/libsensors_iio/libmplmpu.so b/60xx/libsensors_iio/libmplmpu.so
index 205ab9a..205ab9a 100644
--- a/libsensors_iio/libmplmpu.so
+++ b/60xx/libsensors_iio/libmplmpu.so
diff --git a/libsensors_iio/local_log_def.h b/60xx/libsensors_iio/local_log_def.h
index 9135992..9135992 100644
--- a/libsensors_iio/local_log_def.h
+++ b/60xx/libsensors_iio/local_log_def.h
diff --git a/libsensors_iio/sensor_params.h b/60xx/libsensors_iio/sensor_params.h
index 39e3e5c..39e3e5c 100644
--- a/libsensors_iio/sensor_params.h
+++ b/60xx/libsensors_iio/sensor_params.h
diff --git a/libsensors_iio/sensors.h b/60xx/libsensors_iio/sensors.h
index f698fc5..f698fc5 100644
--- a/libsensors_iio/sensors.h
+++ b/60xx/libsensors_iio/sensors.h
diff --git a/libsensors_iio/sensors_mpl.cpp b/60xx/libsensors_iio/sensors_mpl.cpp
index 4aef514..4aef514 100644
--- a/libsensors_iio/sensors_mpl.cpp
+++ b/60xx/libsensors_iio/sensors_mpl.cpp
diff --git a/libsensors_iio/software/build/android/common.mk b/60xx/libsensors_iio/software/build/android/common.mk
index 84e7e9b..84e7e9b 100644
--- a/libsensors_iio/software/build/android/common.mk
+++ b/60xx/libsensors_iio/software/build/android/common.mk
diff --git a/libsensors_iio/software/build/android/shared.mk b/60xx/libsensors_iio/software/build/android/shared.mk
index 47dedfe..47dedfe 100644
--- a/libsensors_iio/software/build/android/shared.mk
+++ b/60xx/libsensors_iio/software/build/android/shared.mk
diff --git a/libsensors_iio/software/core/driver/include/log.h b/60xx/libsensors_iio/software/core/driver/include/log.h
index c519691..c519691 100644
--- a/libsensors_iio/software/core/driver/include/log.h
+++ b/60xx/libsensors_iio/software/core/driver/include/log.h
diff --git a/libsensors_iio/software/core/driver/include/mlinclude.h b/60xx/libsensors_iio/software/core/driver/include/mlinclude.h
index 9725199..9725199 100644
--- a/libsensors_iio/software/core/driver/include/mlinclude.h
+++ b/60xx/libsensors_iio/software/core/driver/include/mlinclude.h
diff --git a/libsensors_iio/software/core/driver/include/mlmath.h b/60xx/libsensors_iio/software/core/driver/include/mlmath.h
index 37194d6..37194d6 100644
--- a/libsensors_iio/software/core/driver/include/mlmath.h
+++ b/60xx/libsensors_iio/software/core/driver/include/mlmath.h
diff --git a/libsensors_iio/software/core/driver/include/mlsl.h b/60xx/libsensors_iio/software/core/driver/include/mlsl.h
index 12f2901..12f2901 100644
--- a/libsensors_iio/software/core/driver/include/mlsl.h
+++ b/60xx/libsensors_iio/software/core/driver/include/mlsl.h
diff --git a/libsensors_iio/software/core/driver/include/mltypes.h b/60xx/libsensors_iio/software/core/driver/include/mltypes.h
index 09eccce..09eccce 100644
--- a/libsensors_iio/software/core/driver/include/mltypes.h
+++ b/60xx/libsensors_iio/software/core/driver/include/mltypes.h
diff --git a/libsensors_iio/software/core/driver/include/stdint_invensense.h b/60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h
index b8c2511..b8c2511 100644
--- a/libsensors_iio/software/core/driver/include/stdint_invensense.h
+++ b/60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h
diff --git a/libsensors_iio/software/core/mllite/build/android/libmllite.so b/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so
index 9bdd5bc..9bdd5bc 100755
--- a/libsensors_iio/software/core/mllite/build/android/libmllite.so
+++ b/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so
diff --git a/libsensors_iio/software/core/mllite/build/android/shared.mk b/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk
index 1418450..1418450 100644
--- a/libsensors_iio/software/core/mllite/build/android/shared.mk
+++ b/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk
diff --git a/libsensors_iio/software/core/mllite/build/filelist.mk b/60xx/libsensors_iio/software/core/mllite/build/filelist.mk
index 011120c..011120c 100644
--- a/libsensors_iio/software/core/mllite/build/filelist.mk
+++ b/60xx/libsensors_iio/software/core/mllite/build/filelist.mk
diff --git a/libsensors_iio/software/core/mllite/data_builder.c b/60xx/libsensors_iio/software/core/mllite/data_builder.c
index 48993bc..48993bc 100644
--- a/libsensors_iio/software/core/mllite/data_builder.c
+++ b/60xx/libsensors_iio/software/core/mllite/data_builder.c
diff --git a/libsensors_iio/software/core/mllite/data_builder.h b/60xx/libsensors_iio/software/core/mllite/data_builder.h
index 9aa46e6..9aa46e6 100644
--- a/libsensors_iio/software/core/mllite/data_builder.h
+++ b/60xx/libsensors_iio/software/core/mllite/data_builder.h
diff --git a/libsensors_iio/software/core/mllite/hal_outputs.c b/60xx/libsensors_iio/software/core/mllite/hal_outputs.c
index 7cdca59..7cdca59 100644
--- a/libsensors_iio/software/core/mllite/hal_outputs.c
+++ b/60xx/libsensors_iio/software/core/mllite/hal_outputs.c
diff --git a/libsensors_iio/software/core/mllite/hal_outputs.h b/60xx/libsensors_iio/software/core/mllite/hal_outputs.h
index 85e88f3..85e88f3 100644
--- a/libsensors_iio/software/core/mllite/hal_outputs.h
+++ b/60xx/libsensors_iio/software/core/mllite/hal_outputs.h
diff --git a/libsensors_iio/software/core/mllite/invensense.h b/60xx/libsensors_iio/software/core/mllite/invensense.h
index fb8e12b..fb8e12b 100644
--- a/libsensors_iio/software/core/mllite/invensense.h
+++ b/60xx/libsensors_iio/software/core/mllite/invensense.h
diff --git a/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
index 72940f7..72940f7 100644
--- a/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
+++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
diff --git a/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h
index 4d98692..4d98692 100644
--- a/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h
+++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h
diff --git a/libsensors_iio/software/core/mllite/linux/ml_stored_data.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
index b4c4249..b4c4249 100644
--- a/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
+++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
diff --git a/libsensors_iio/software/core/mllite/linux/ml_stored_data.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
index 115b34c..115b34c 100644
--- a/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
+++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
diff --git a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
index d0c4513..d0c4513 100644
--- a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
+++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
diff --git a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h
index 9470874..9470874 100644
--- a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h
+++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h
diff --git a/libsensors_iio/software/core/mllite/linux/mlos.h b/60xx/libsensors_iio/software/core/mllite/linux/mlos.h
index d4f8912..d4f8912 100644
--- a/libsensors_iio/software/core/mllite/linux/mlos.h
+++ b/60xx/libsensors_iio/software/core/mllite/linux/mlos.h
diff --git a/libsensors_iio/software/core/mllite/linux/mlos_linux.c b/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c
index 5424508..5424508 100644
--- a/libsensors_iio/software/core/mllite/linux/mlos_linux.c
+++ b/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c
diff --git a/libsensors_iio/software/core/mllite/message_layer.c b/60xx/libsensors_iio/software/core/mllite/message_layer.c
index 8317957..8317957 100644
--- a/libsensors_iio/software/core/mllite/message_layer.c
+++ b/60xx/libsensors_iio/software/core/mllite/message_layer.c
diff --git a/libsensors_iio/software/core/mllite/message_layer.h b/60xx/libsensors_iio/software/core/mllite/message_layer.h
index df0c0e2..df0c0e2 100644
--- a/libsensors_iio/software/core/mllite/message_layer.h
+++ b/60xx/libsensors_iio/software/core/mllite/message_layer.h
diff --git a/libsensors_iio/software/core/mllite/ml_math_func.c b/60xx/libsensors_iio/software/core/mllite/ml_math_func.c
index c30d693..c30d693 100644
--- a/libsensors_iio/software/core/mllite/ml_math_func.c
+++ b/60xx/libsensors_iio/software/core/mllite/ml_math_func.c
diff --git a/libsensors_iio/software/core/mllite/ml_math_func.h b/60xx/libsensors_iio/software/core/mllite/ml_math_func.h
index 535d849..535d849 100644
--- a/libsensors_iio/software/core/mllite/ml_math_func.h
+++ b/60xx/libsensors_iio/software/core/mllite/ml_math_func.h
diff --git a/libsensors_iio/software/core/mllite/mpl.c b/60xx/libsensors_iio/software/core/mllite/mpl.c
index 9141cc6..9141cc6 100644
--- a/libsensors_iio/software/core/mllite/mpl.c
+++ b/60xx/libsensors_iio/software/core/mllite/mpl.c
diff --git a/libsensors_iio/software/core/mllite/mpl.h b/60xx/libsensors_iio/software/core/mllite/mpl.h
index a6b5ac7..a6b5ac7 100644
--- a/libsensors_iio/software/core/mllite/mpl.h
+++ b/60xx/libsensors_iio/software/core/mllite/mpl.h
diff --git a/libsensors_iio/software/core/mllite/results_holder.c b/60xx/libsensors_iio/software/core/mllite/results_holder.c
index df58f40..df58f40 100644
--- a/libsensors_iio/software/core/mllite/results_holder.c
+++ b/60xx/libsensors_iio/software/core/mllite/results_holder.c
diff --git a/libsensors_iio/software/core/mllite/results_holder.h b/60xx/libsensors_iio/software/core/mllite/results_holder.h
index 09da24e..09da24e 100644
--- a/libsensors_iio/software/core/mllite/results_holder.h
+++ b/60xx/libsensors_iio/software/core/mllite/results_holder.h
diff --git a/libsensors_iio/software/core/mllite/start_manager.c b/60xx/libsensors_iio/software/core/mllite/start_manager.c
index fb758e7..fb758e7 100644
--- a/libsensors_iio/software/core/mllite/start_manager.c
+++ b/60xx/libsensors_iio/software/core/mllite/start_manager.c
diff --git a/libsensors_iio/software/core/mllite/start_manager.h b/60xx/libsensors_iio/software/core/mllite/start_manager.h
index 899e3f5..899e3f5 100644
--- a/libsensors_iio/software/core/mllite/start_manager.h
+++ b/60xx/libsensors_iio/software/core/mllite/start_manager.h
diff --git a/libsensors_iio/software/core/mllite/storage_manager.c b/60xx/libsensors_iio/software/core/mllite/storage_manager.c
index 3e4e619..3e4e619 100644
--- a/libsensors_iio/software/core/mllite/storage_manager.c
+++ b/60xx/libsensors_iio/software/core/mllite/storage_manager.c
diff --git a/libsensors_iio/software/core/mllite/storage_manager.h b/60xx/libsensors_iio/software/core/mllite/storage_manager.h
index 7255591..7255591 100644
--- a/libsensors_iio/software/core/mllite/storage_manager.h
+++ b/60xx/libsensors_iio/software/core/mllite/storage_manager.h
diff --git a/libsensors_iio/software/core/mpl/accel_auto_cal.h b/60xx/libsensors_iio/software/core/mpl/accel_auto_cal.h
index 5a53213..5a53213 100644
--- a/libsensors_iio/software/core/mpl/accel_auto_cal.h
+++ b/60xx/libsensors_iio/software/core/mpl/accel_auto_cal.h
diff --git a/libsensors_iio/software/core/mpl/authenticate.h b/60xx/libsensors_iio/software/core/mpl/authenticate.h
index d7c803b..d7c803b 100644
--- a/libsensors_iio/software/core/mpl/authenticate.h
+++ b/60xx/libsensors_iio/software/core/mpl/authenticate.h
diff --git a/libsensors_iio/software/core/mpl/build/android/libmplmpu.so b/60xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
index fbd346f..fbd346f 100644
--- a/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
+++ b/60xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
diff --git a/libsensors_iio/software/core/mpl/build/android/shared.mk b/60xx/libsensors_iio/software/core/mpl/build/android/shared.mk
index 2e15205..2e15205 100644
--- a/libsensors_iio/software/core/mpl/build/android/shared.mk
+++ b/60xx/libsensors_iio/software/core/mpl/build/android/shared.mk
diff --git a/libsensors_iio/software/core/mpl/build/filelist.mk b/60xx/libsensors_iio/software/core/mpl/build/filelist.mk
index e18003a..e18003a 100644
--- a/libsensors_iio/software/core/mpl/build/filelist.mk
+++ b/60xx/libsensors_iio/software/core/mpl/build/filelist.mk
diff --git a/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h b/60xx/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h
index 4f01fc2..4f01fc2 100644
--- a/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h
+++ b/60xx/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h
diff --git a/libsensors_iio/software/core/mpl/compass_fit.h b/60xx/libsensors_iio/software/core/mpl/compass_fit.h
index be3cce8..be3cce8 100644
--- a/libsensors_iio/software/core/mpl/compass_fit.h
+++ b/60xx/libsensors_iio/software/core/mpl/compass_fit.h
diff --git a/libsensors_iio/software/core/mpl/compass_vec_cal.h b/60xx/libsensors_iio/software/core/mpl/compass_vec_cal.h
index a3e044c..a3e044c 100644
--- a/libsensors_iio/software/core/mpl/compass_vec_cal.h
+++ b/60xx/libsensors_iio/software/core/mpl/compass_vec_cal.h
diff --git a/libsensors_iio/software/core/mpl/fast_no_motion.h b/60xx/libsensors_iio/software/core/mpl/fast_no_motion.h
index c553926..c553926 100644
--- a/libsensors_iio/software/core/mpl/fast_no_motion.h
+++ b/60xx/libsensors_iio/software/core/mpl/fast_no_motion.h
diff --git a/libsensors_iio/software/core/mpl/fusion_9axis.h b/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h
index 1ba1ebb..1ba1ebb 100644
--- a/libsensors_iio/software/core/mpl/fusion_9axis.h
+++ b/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h
diff --git a/libsensors_iio/software/core/mpl/gyro_tc.h b/60xx/libsensors_iio/software/core/mpl/gyro_tc.h
index 8f1d50e..8f1d50e 100644
--- a/libsensors_iio/software/core/mpl/gyro_tc.h
+++ b/60xx/libsensors_iio/software/core/mpl/gyro_tc.h
diff --git a/libsensors_iio/software/core/mpl/heading_from_gyro.h b/60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h
index 09ecc42..09ecc42 100644
--- a/libsensors_iio/software/core/mpl/heading_from_gyro.h
+++ b/60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h
diff --git a/libsensors_iio/software/core/mpl/inv_math.h b/60xx/libsensors_iio/software/core/mpl/inv_math.h
index 6620bbf..6620bbf 100644
--- a/libsensors_iio/software/core/mpl/inv_math.h
+++ b/60xx/libsensors_iio/software/core/mpl/inv_math.h
diff --git a/libsensors_iio/software/core/mpl/invensense_adv.h b/60xx/libsensors_iio/software/core/mpl/invensense_adv.h
index 12932c9..12932c9 100644
--- a/libsensors_iio/software/core/mpl/invensense_adv.h
+++ b/60xx/libsensors_iio/software/core/mpl/invensense_adv.h
diff --git a/libsensors_iio/software/core/mpl/mag_disturb.h b/60xx/libsensors_iio/software/core/mpl/mag_disturb.h
index 38df919..38df919 100644
--- a/libsensors_iio/software/core/mpl/mag_disturb.h
+++ b/60xx/libsensors_iio/software/core/mpl/mag_disturb.h
diff --git a/libsensors_iio/software/core/mpl/motion_no_motion.h b/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h
index 01cf1c0..01cf1c0 100644
--- a/libsensors_iio/software/core/mpl/motion_no_motion.h
+++ b/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h
diff --git a/libsensors_iio/software/core/mpl/no_gyro_fusion.h b/60xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h
index 38d5690..38d5690 100644
--- a/libsensors_iio/software/core/mpl/no_gyro_fusion.h
+++ b/60xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h
diff --git a/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h b/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
index 3c6a2c1..3c6a2c1 100644
--- a/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
+++ b/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
diff --git a/libsensors_iio/software/core/mpl/quaternion_supervisor.h b/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h
index 4fa36b0..4fa36b0 100644
--- a/libsensors_iio/software/core/mpl/quaternion_supervisor.h
+++ b/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h
diff --git a/libsensors_iio/software/core/mpl/shake.h b/60xx/libsensors_iio/software/core/mpl/shake.h
index 67acb7b..67acb7b 100644
--- a/libsensors_iio/software/core/mpl/shake.h
+++ b/60xx/libsensors_iio/software/core/mpl/shake.h
diff --git a/mlsdk/Android.mk b/60xx/mlsdk/Android.mk
index 41b1457..41b1457 100644
--- a/mlsdk/Android.mk
+++ b/60xx/mlsdk/Android.mk
diff --git a/mlsdk/mllite/accel.c b/60xx/mlsdk/mllite/accel.c
index 60b8d6c..60b8d6c 100644
--- a/mlsdk/mllite/accel.c
+++ b/60xx/mlsdk/mllite/accel.c
diff --git a/mlsdk/mllite/accel.h b/60xx/mlsdk/mllite/accel.h
index d3fbc6a..d3fbc6a 100644
--- a/mlsdk/mllite/accel.h
+++ b/60xx/mlsdk/mllite/accel.h
diff --git a/mlsdk/mllite/compass.c b/60xx/mlsdk/mllite/compass.c
index 798cb9f..798cb9f 100644
--- a/mlsdk/mllite/compass.c
+++ b/60xx/mlsdk/mllite/compass.c
diff --git a/mlsdk/mllite/compass.h b/60xx/mlsdk/mllite/compass.h
index f0bdb58..f0bdb58 100644
--- a/mlsdk/mllite/compass.h
+++ b/60xx/mlsdk/mllite/compass.h
diff --git a/mlsdk/mllite/dmpDefault.c b/60xx/mlsdk/mllite/dmpDefault.c
index 6620d14..6620d14 100644
--- a/mlsdk/mllite/dmpDefault.c
+++ b/60xx/mlsdk/mllite/dmpDefault.c
diff --git a/mlsdk/mllite/dmpDefault.h b/60xx/mlsdk/mllite/dmpDefault.h
index 0670977..0670977 100644
--- a/mlsdk/mllite/dmpDefault.h
+++ b/60xx/mlsdk/mllite/dmpDefault.h
diff --git a/mlsdk/mllite/dmpDefaultMantis.c b/60xx/mlsdk/mllite/dmpDefaultMantis.c
index f35aaca..f35aaca 100644
--- a/mlsdk/mllite/dmpDefaultMantis.c
+++ b/60xx/mlsdk/mllite/dmpDefaultMantis.c
diff --git a/mlsdk/mllite/dmpKey.h b/60xx/mlsdk/mllite/dmpKey.h
index f152644..f152644 100644
--- a/mlsdk/mllite/dmpKey.h
+++ b/60xx/mlsdk/mllite/dmpKey.h
diff --git a/mlsdk/mllite/dmpconfig.txt b/60xx/mlsdk/mllite/dmpconfig.txt
index 4643ed5..4643ed5 100644
--- a/mlsdk/mllite/dmpconfig.txt
+++ b/60xx/mlsdk/mllite/dmpconfig.txt
diff --git a/mlsdk/mllite/dmpmap.h b/60xx/mlsdk/mllite/dmpmap.h
index cb53c7c..cb53c7c 100644
--- a/mlsdk/mllite/dmpmap.h
+++ b/60xx/mlsdk/mllite/dmpmap.h
diff --git a/mlsdk/mllite/invensense.h b/60xx/mlsdk/mllite/invensense.h
index 586dd25..586dd25 100644
--- a/mlsdk/mllite/invensense.h
+++ b/60xx/mlsdk/mllite/invensense.h
diff --git a/mlsdk/mllite/ml.c b/60xx/mlsdk/mllite/ml.c
index 3328edd..3328edd 100644
--- a/mlsdk/mllite/ml.c
+++ b/60xx/mlsdk/mllite/ml.c
diff --git a/mlsdk/mllite/ml.h b/60xx/mlsdk/mllite/ml.h
index 838cd49..838cd49 100644
--- a/mlsdk/mllite/ml.h
+++ b/60xx/mlsdk/mllite/ml.h
diff --git a/mlsdk/mllite/mlBiasNoMotion.c b/60xx/mlsdk/mllite/mlBiasNoMotion.c
index 73321ff..73321ff 100644
--- a/mlsdk/mllite/mlBiasNoMotion.c
+++ b/60xx/mlsdk/mllite/mlBiasNoMotion.c
diff --git a/mlsdk/mllite/mlBiasNoMotion.h b/60xx/mlsdk/mllite/mlBiasNoMotion.h
index 030dbf9..030dbf9 100644
--- a/mlsdk/mllite/mlBiasNoMotion.h
+++ b/60xx/mlsdk/mllite/mlBiasNoMotion.h
diff --git a/mlsdk/mllite/mlFIFO.c b/60xx/mlsdk/mllite/mlFIFO.c
index 2c0d2f0..2c0d2f0 100644
--- a/mlsdk/mllite/mlFIFO.c
+++ b/60xx/mlsdk/mllite/mlFIFO.c
diff --git a/mlsdk/mllite/mlFIFO.h b/60xx/mlsdk/mllite/mlFIFO.h
index 2114eb3..2114eb3 100644
--- a/mlsdk/mllite/mlFIFO.h
+++ b/60xx/mlsdk/mllite/mlFIFO.h
diff --git a/mlsdk/mllite/mlFIFOHW.c b/60xx/mlsdk/mllite/mlFIFOHW.c
index 7a77e66..7a77e66 100644
--- a/mlsdk/mllite/mlFIFOHW.c
+++ b/60xx/mlsdk/mllite/mlFIFOHW.c
diff --git a/mlsdk/mllite/mlFIFOHW.h b/60xx/mlsdk/mllite/mlFIFOHW.h
index 6f70185..6f70185 100644
--- a/mlsdk/mllite/mlFIFOHW.h
+++ b/60xx/mlsdk/mllite/mlFIFOHW.h
diff --git a/mlsdk/mllite/mlMathFunc.c b/60xx/mlsdk/mllite/mlMathFunc.c
index 0d8e7ec..0d8e7ec 100644
--- a/mlsdk/mllite/mlMathFunc.c
+++ b/60xx/mlsdk/mllite/mlMathFunc.c
diff --git a/mlsdk/mllite/mlMathFunc.h b/60xx/mlsdk/mllite/mlMathFunc.h
index 70fa9f4..70fa9f4 100644
--- a/mlsdk/mllite/mlMathFunc.h
+++ b/60xx/mlsdk/mllite/mlMathFunc.h
diff --git a/mlsdk/mllite/mlSetGyroBias.c b/60xx/mlsdk/mllite/mlSetGyroBias.c
index bd14d2e..bd14d2e 100644
--- a/mlsdk/mllite/mlSetGyroBias.c
+++ b/60xx/mlsdk/mllite/mlSetGyroBias.c
diff --git a/mlsdk/mllite/mlSetGyroBias.h b/60xx/mlsdk/mllite/mlSetGyroBias.h
index e56f18b..e56f18b 100644
--- a/mlsdk/mllite/mlSetGyroBias.h
+++ b/60xx/mlsdk/mllite/mlSetGyroBias.h
diff --git a/mlsdk/mllite/ml_mputest.c b/60xx/mlsdk/mllite/ml_mputest.c
index d7fc608..d7fc608 100644
--- a/mlsdk/mllite/ml_mputest.c
+++ b/60xx/mlsdk/mllite/ml_mputest.c
diff --git a/mlsdk/mllite/ml_mputest.h b/60xx/mlsdk/mllite/ml_mputest.h
index 705d3cc..705d3cc 100644
--- a/mlsdk/mllite/ml_mputest.h
+++ b/60xx/mlsdk/mllite/ml_mputest.h
diff --git a/mlsdk/mllite/ml_stored_data.c b/60xx/mlsdk/mllite/ml_stored_data.c
index 9107fe2..9107fe2 100644
--- a/mlsdk/mllite/ml_stored_data.c
+++ b/60xx/mlsdk/mllite/ml_stored_data.c
diff --git a/mlsdk/mllite/ml_stored_data.h b/60xx/mlsdk/mllite/ml_stored_data.h
index 74c2b7c..74c2b7c 100644
--- a/mlsdk/mllite/ml_stored_data.h
+++ b/60xx/mlsdk/mllite/ml_stored_data.h
diff --git a/mlsdk/mllite/mlarray.c b/60xx/mlsdk/mllite/mlarray.c
index 6ae85e0..6ae85e0 100644
--- a/mlsdk/mllite/mlarray.c
+++ b/60xx/mlsdk/mllite/mlarray.c
diff --git a/mlsdk/mllite/mlarray_legacy.c b/60xx/mlsdk/mllite/mlarray_legacy.c
index 9dce8f3..9dce8f3 100644
--- a/mlsdk/mllite/mlarray_legacy.c
+++ b/60xx/mlsdk/mllite/mlarray_legacy.c
diff --git a/mlsdk/mllite/mlcontrol.c b/60xx/mlsdk/mllite/mlcontrol.c
index bd186fa..bd186fa 100644
--- a/mlsdk/mllite/mlcontrol.c
+++ b/60xx/mlsdk/mllite/mlcontrol.c
diff --git a/mlsdk/mllite/mlcontrol.h b/60xx/mlsdk/mllite/mlcontrol.h
index a834fc6..a834fc6 100644
--- a/mlsdk/mllite/mlcontrol.h
+++ b/60xx/mlsdk/mllite/mlcontrol.h
diff --git a/mlsdk/mllite/mldl.c b/60xx/mlsdk/mllite/mldl.c
index 7054532..7054532 100644
--- a/mlsdk/mllite/mldl.c
+++ b/60xx/mlsdk/mllite/mldl.c
diff --git a/mlsdk/mllite/mldl.h b/60xx/mlsdk/mllite/mldl.h
index 961d860..961d860 100644
--- a/mlsdk/mllite/mldl.h
+++ b/60xx/mlsdk/mllite/mldl.h
diff --git a/mlsdk/mllite/mldl_cfg.h b/60xx/mlsdk/mllite/mldl_cfg.h
index b4914e2..b4914e2 100644
--- a/mlsdk/mllite/mldl_cfg.h
+++ b/60xx/mlsdk/mllite/mldl_cfg.h
diff --git a/mlsdk/mllite/mldl_cfg_mpu.c b/60xx/mlsdk/mllite/mldl_cfg_mpu.c
index f89f94c..f89f94c 100644
--- a/mlsdk/mllite/mldl_cfg_mpu.c
+++ b/60xx/mlsdk/mllite/mldl_cfg_mpu.c
diff --git a/mlsdk/mllite/mldmp.c b/60xx/mlsdk/mllite/mldmp.c
index 7381dd4..7381dd4 100644
--- a/mlsdk/mllite/mldmp.c
+++ b/60xx/mlsdk/mllite/mldmp.c
diff --git a/mlsdk/mllite/mldmp.h b/60xx/mlsdk/mllite/mldmp.h
index f519798..f519798 100644
--- a/mlsdk/mllite/mldmp.h
+++ b/60xx/mlsdk/mllite/mldmp.h
diff --git a/mlsdk/mllite/mlinclude.h b/60xx/mlsdk/mllite/mlinclude.h
index dcbe904..dcbe904 100644
--- a/mlsdk/mllite/mlinclude.h
+++ b/60xx/mlsdk/mllite/mlinclude.h
diff --git a/mlsdk/mllite/mlstates.c b/60xx/mlsdk/mllite/mlstates.c
index 8ebb086..8ebb086 100644
--- a/mlsdk/mllite/mlstates.c
+++ b/60xx/mlsdk/mllite/mlstates.c
diff --git a/mlsdk/mllite/mlstates.h b/60xx/mlsdk/mllite/mlstates.h
index cbaa610..cbaa610 100644
--- a/mlsdk/mllite/mlstates.h
+++ b/60xx/mlsdk/mllite/mlstates.h
diff --git a/mlsdk/mllite/mlsupervisor.c b/60xx/mlsdk/mllite/mlsupervisor.c
index 139297f..139297f 100644
--- a/mlsdk/mllite/mlsupervisor.c
+++ b/60xx/mlsdk/mllite/mlsupervisor.c
diff --git a/mlsdk/mllite/mlsupervisor.h b/60xx/mlsdk/mllite/mlsupervisor.h
index 62b427e..62b427e 100644
--- a/mlsdk/mllite/mlsupervisor.h
+++ b/60xx/mlsdk/mllite/mlsupervisor.h
diff --git a/mlsdk/mllite/pressure.c b/60xx/mlsdk/mllite/pressure.c
index 9c162ec..9c162ec 100644
--- a/mlsdk/mllite/pressure.c
+++ b/60xx/mlsdk/mllite/pressure.c
diff --git a/mlsdk/mllite/pressure.h b/60xx/mlsdk/mllite/pressure.h
index 77c5d43..77c5d43 100644
--- a/mlsdk/mllite/pressure.h
+++ b/60xx/mlsdk/mllite/pressure.h
diff --git a/mlsdk/mlutils/checksum.c b/60xx/mlsdk/mlutils/checksum.c
index a97477d..a97477d 100644
--- a/mlsdk/mlutils/checksum.c
+++ b/60xx/mlsdk/mlutils/checksum.c
diff --git a/mlsdk/mlutils/checksum.h b/60xx/mlsdk/mlutils/checksum.h
index 4d3f046..4d3f046 100644
--- a/mlsdk/mlutils/checksum.h
+++ b/60xx/mlsdk/mlutils/checksum.h
diff --git a/mlsdk/mlutils/mputest.c b/60xx/mlsdk/mlutils/mputest.c
index ac0fa30..ac0fa30 100644
--- a/mlsdk/mlutils/mputest.c
+++ b/60xx/mlsdk/mlutils/mputest.c
diff --git a/mlsdk/mlutils/mputest.h b/60xx/mlsdk/mlutils/mputest.h
index d3347c5..d3347c5 100644
--- a/mlsdk/mlutils/mputest.h
+++ b/60xx/mlsdk/mlutils/mputest.h
diff --git a/mlsdk/mlutils/slave.h b/60xx/mlsdk/mlutils/slave.h
index 45449f6..45449f6 100644
--- a/mlsdk/mlutils/slave.h
+++ b/60xx/mlsdk/mlutils/slave.h
diff --git a/mlsdk/platform/include/i2c.h b/60xx/mlsdk/platform/include/i2c.h
index 39dd255..39dd255 100644
--- a/mlsdk/platform/include/i2c.h
+++ b/60xx/mlsdk/platform/include/i2c.h
diff --git a/mlsdk/platform/include/linux/mpu.h b/60xx/mlsdk/platform/include/linux/mpu.h
index 04fa7b6..04fa7b6 100644
--- a/mlsdk/platform/include/linux/mpu.h
+++ b/60xx/mlsdk/platform/include/linux/mpu.h
diff --git a/mlsdk/platform/include/log.h b/60xx/mlsdk/platform/include/log.h
index 8485074..8485074 100644
--- a/mlsdk/platform/include/log.h
+++ b/60xx/mlsdk/platform/include/log.h
diff --git a/mlsdk/platform/include/mlmath.h b/60xx/mlsdk/platform/include/mlmath.h
index bf54870..bf54870 100644
--- a/mlsdk/platform/include/mlmath.h
+++ b/60xx/mlsdk/platform/include/mlmath.h
diff --git a/mlsdk/platform/include/mlos.h b/60xx/mlsdk/platform/include/mlos.h
index 78f0a83..78f0a83 100644
--- a/mlsdk/platform/include/mlos.h
+++ b/60xx/mlsdk/platform/include/mlos.h
diff --git a/mlsdk/platform/include/mlsl.h b/60xx/mlsdk/platform/include/mlsl.h
index 535d117..535d117 100644
--- a/mlsdk/platform/include/mlsl.h
+++ b/60xx/mlsdk/platform/include/mlsl.h
diff --git a/mlsdk/platform/include/mltypes.h b/60xx/mlsdk/platform/include/mltypes.h
index 90a126b..90a126b 100644
--- a/mlsdk/platform/include/mltypes.h
+++ b/60xx/mlsdk/platform/include/mltypes.h
diff --git a/mlsdk/platform/include/mpu3050.h b/60xx/mlsdk/platform/include/mpu3050.h
index c363a00..c363a00 100644
--- a/mlsdk/platform/include/mpu3050.h
+++ b/60xx/mlsdk/platform/include/mpu3050.h
diff --git a/mlsdk/platform/include/stdint_invensense.h b/60xx/mlsdk/platform/include/stdint_invensense.h
index d238813..d238813 100644
--- a/mlsdk/platform/include/stdint_invensense.h
+++ b/60xx/mlsdk/platform/include/stdint_invensense.h
diff --git a/mlsdk/platform/linux/kernel/mpuirq.h b/60xx/mlsdk/platform/linux/kernel/mpuirq.h
index 352170b..352170b 100644
--- a/mlsdk/platform/linux/kernel/mpuirq.h
+++ b/60xx/mlsdk/platform/linux/kernel/mpuirq.h
diff --git a/mlsdk/platform/linux/kernel/slaveirq.h b/60xx/mlsdk/platform/linux/kernel/slaveirq.h
index beb352b..beb352b 100644
--- a/mlsdk/platform/linux/kernel/slaveirq.h
+++ b/60xx/mlsdk/platform/linux/kernel/slaveirq.h
diff --git a/mlsdk/platform/linux/kernel/timerirq.h b/60xx/mlsdk/platform/linux/kernel/timerirq.h
index dc3eea2..dc3eea2 100644
--- a/mlsdk/platform/linux/kernel/timerirq.h
+++ b/60xx/mlsdk/platform/linux/kernel/timerirq.h
diff --git a/mlsdk/platform/linux/log_linux.c b/60xx/mlsdk/platform/linux/log_linux.c
index 8e75753..8e75753 100644
--- a/mlsdk/platform/linux/log_linux.c
+++ b/60xx/mlsdk/platform/linux/log_linux.c
diff --git a/mlsdk/platform/linux/log_printf_linux.c b/60xx/mlsdk/platform/linux/log_printf_linux.c
index 744a223..744a223 100644
--- a/mlsdk/platform/linux/log_printf_linux.c
+++ b/60xx/mlsdk/platform/linux/log_printf_linux.c
diff --git a/mlsdk/platform/linux/mlos_linux.c b/60xx/mlsdk/platform/linux/mlos_linux.c
index fd3b002..fd3b002 100644
--- a/mlsdk/platform/linux/mlos_linux.c
+++ b/60xx/mlsdk/platform/linux/mlos_linux.c
diff --git a/mlsdk/platform/linux/mlsl_linux_mpu.c b/60xx/mlsdk/platform/linux/mlsl_linux_mpu.c
index 29930b3..29930b3 100644
--- a/mlsdk/platform/linux/mlsl_linux_mpu.c
+++ b/60xx/mlsdk/platform/linux/mlsl_linux_mpu.c
diff --git a/65xx/Android.mk b/65xx/Android.mk
new file mode 100644
index 0000000..b80cd55
--- /dev/null
+++ b/65xx/Android.mk
@@ -0,0 +1,2 @@
+# libsensors_iio expects IIO drivers for an MPU6515+AK8963
+include $(call all-named-subdir-makefiles,libsensors_iio)
diff --git a/65xx/libsensors_iio/Android.mk b/65xx/libsensors_iio/Android.mk
new file mode 100644
index 0000000..77d5ce3
--- /dev/null
+++ b/65xx/libsensors_iio/Android.mk
@@ -0,0 +1,147 @@
+# Copyright (C) 2008 The Android Open Source Project
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+# Modified 2011 by InvenSense, Inc
+
+LOCAL_PATH := $(call my-dir)
+
+#ifneq ($(TARGET_SIMULATOR),true)
+
+# InvenSense fragment of the HAL
+include $(CLEAR_VARS)
+
+LOCAL_MODULE := libinvensense_hal
+LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := invensense
+
+LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\"
+
+# ANDROID version check
+MAJOR_VERSION :=$(shell echo $(PLATFORM_VERSION) | cut -f1 -d.)
+MINOR_VERSION :=$(shell echo $(PLATFORM_VERSION) | cut -f2 -d.)
+VERSION_JB :=$(shell test $(MAJOR_VERSION) -gt 4 -o $(MAJOR_VERSION) -eq 4 -a $(MINOR_VERSION) -gt 0 && echo true)
+$(info MAJOR_VERSION=$(MAJOR_VERSION))
+$(info MINOR_VERSION=$(MINOR_VERSION))
+#ANDROID version check END
+VERSION_JB:=true
+ifeq ($(VERSION_JB),true)
+LOCAL_CFLAGS += -DANDROID_JELLYBEAN
+endif
+
+ifneq (,$(filter $(TARGET_BUILD_VARIANT),eng userdebug))
+ifneq ($(COMPILE_INVENSENSE_COMPASS_CAL),0)
+LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL
+endif
+ifeq ($(COMPILE_THIRD_PARTY_ACCEL),1)
+LOCAL_CFLAGS += -DTHIRD_PARTY_ACCEL
+endif
+else # release builds, default
+LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL
+endif
+
+LOCAL_SRC_FILES += SensorBase.cpp
+LOCAL_SRC_FILES += MPLSensor.cpp
+LOCAL_SRC_FILES += MPLSupport.cpp
+LOCAL_SRC_FILES += InputEventReader.cpp
+LOCAL_SRC_FILES += PressureSensor.IIO.secondary.cpp
+
+ifneq (,$(filter $(TARGET_BUILD_VARIANT),eng userdebug))
+ifeq ($(COMPILE_INVENSENSE_COMPASS_CAL),0)
+LOCAL_SRC_FILES += AkmSensor.cpp
+LOCAL_SRC_FILES += CompassSensor.AKM.cpp
+else ifeq ($(COMPILE_INVENSENSE_SENSOR_ON_PRIMARY_BUS), 1)
+LOCAL_SRC_FILES += CompassSensor.IIO.primary.cpp
+LOCAL_CFLAGS += -DSENSOR_ON_PRIMARY_BUS
+else
+LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp
+endif
+else # release builds, default
+LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp
+endif #userdebug
+
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite/linux
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include/linux
+
+LOCAL_SHARED_LIBRARIES := liblog
+LOCAL_SHARED_LIBRARIES += libcutils
+LOCAL_SHARED_LIBRARIES += libutils
+LOCAL_SHARED_LIBRARIES += libdl
+LOCAL_SHARED_LIBRARIES += libmllite
+
+# Additions for SysPed
+LOCAL_SHARED_LIBRARIES += libmplmpu
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mpl
+LOCAL_CPPFLAGS += -DLINUX=1
+LOCAL_PRELINK_MODULE := false
+
+LOCAL_SHARED_LIBRARIES += libmllite
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite
+LOCAL_CPPFLAGS += -DLINUX=1
+LOCAL_PRELINK_MODULE := false
+
+include $(BUILD_SHARED_LIBRARY)
+
+#endif # !TARGET_SIMULATOR
+
+# Build a temporary HAL that links the InvenSense .so
+include $(CLEAR_VARS)
+ifeq (,$(filter $(TARGET_BUILD_VARIANT),eng userdebug))
+ifneq ($(filter manta grouper tilapia, $(TARGET_DEVICE)),)
+#LOCAL_MODULE := sensors.invensense
+else
+LOCAL_MODULE := sensors.${TARGET_PRODUCT}
+endif
+else    # eng & userdebug builds
+LOCAL_MODULE := sensors.${TARGET_PRODUCT}
+endif   # eng & userdebug builds
+LOCAL_MODULE_PATH := $(TARGET_OUT_SHARED_LIBRARIES)/hw
+
+LOCAL_SHARED_LIBRARIES += libmplmpu
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite/linux
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mpl
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include/linux
+
+LOCAL_PRELINK_MODULE := false
+LOCAL_MODULE_TAGS := optional
+LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\"
+
+ifeq ($(VERSION_JB),true)
+LOCAL_CFLAGS += -DANDROID_JELLYBEAN
+endif
+
+include $(CLEAR_VARS)
+LOCAL_MODULE := libmplmpu
+LOCAL_SRC_FILES := libmplmpu.so
+LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := invensense
+LOCAL_MODULE_SUFFIX := .so
+LOCAL_MODULE_CLASS := SHARED_LIBRARIES
+LOCAL_MODULE_PATH := $(TARGET_OUT)/lib
+OVERRIDE_BUILT_MODULE_PATH := $(TARGET_OUT_INTERMEDIATE_LIBRARIES)
+include $(BUILD_PREBUILT)
+
+include $(CLEAR_VARS)
+LOCAL_MODULE := libmllite
+LOCAL_SRC_FILES := libmllite.so
+LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := invensense
+LOCAL_MODULE_SUFFIX := .so
+LOCAL_MODULE_CLASS := SHARED_LIBRARIES
+LOCAL_MODULE_PATH := $(TARGET_OUT)/lib
+OVERRIDE_BUILT_MODULE_PATH := $(TARGET_OUT_INTERMEDIATE_LIBRARIES)
+include $(BUILD_PREBUILT)
+
diff --git a/65xx/libsensors_iio/CompassSensor.AKM.cpp b/65xx/libsensors_iio/CompassSensor.AKM.cpp
new file mode 100644
index 0000000..45699fe
--- /dev/null
+++ b/65xx/libsensors_iio/CompassSensor.AKM.cpp
@@ -0,0 +1,203 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <sys/select.h>
+#include <cutils/log.h>
+#include <linux/input.h>
+
+#include "sensor_params.h"
+#include "MPLSupport.h"
+
+// TODO: include corresponding header file for 3rd party compass sensor
+#include "CompassSensor.AKM.h"
+
+// TODO: specify this for "fillList()" API
+#define COMPASS_NAME "AKM8963"
+
+/*****************************************************************************/
+
+CompassSensor::CompassSensor() 
+                    : SensorBase(NULL, NULL)
+{
+    VFUNC_LOG;
+
+    // TODO: initiate 3rd-party's class, and disable its funtionalities
+    //       proper commands
+    mCompassSensor = new AkmSensor();
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            0, "/sys/class/compass/akm8963/enable_mag", getTimestamp());
+    write_sysfs_int((char*)"/sys/class/compass/akm8963/enable_mag", 0);
+}
+
+CompassSensor::~CompassSensor()
+{
+    VFUNC_LOG;
+
+    // TODO: disable 3rd-party's funtionalities and delete the object
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            0, "/sys/class/compass/akm8963/enable_mag", getTimestamp());
+    write_sysfs_int((char*)"/sys/class/compass/akm8963/enable_mag", 0);
+    delete mCompassSensor;
+}
+
+int CompassSensor::getFd(void) const
+{
+    VFUNC_LOG;
+
+    // TODO: return 3rd-party's file descriptor
+    return mCompassSensor->getFd();
+}
+
+/**
+ *  @brief        This function will enable/disable sensor.
+ *  @param[in]    handle    which sensor to enable/disable.
+ *  @param[in]    en        en=1 enable, en=0 disable
+ *  @return       if the operation is successful.
+ */
+int CompassSensor::enable(int32_t handle, int en)
+{
+    VFUNC_LOG;
+
+    // TODO: called 3rd-party's "set enable/disable" function
+    return mCompassSensor->setEnable(handle, en);
+}
+
+int CompassSensor::setDelay(int32_t handle, int64_t ns)
+{
+    VFUNC_LOG;
+
+    // TODO: called 3rd-party's "set delay" function
+    return mCompassSensor->setDelay(handle, ns);
+}
+
+/**
+    @brief      This function will return the state of the sensor.
+    @return     1=enabled; 0=disabled
+**/
+int CompassSensor::getEnable(int32_t handle)
+{
+    VFUNC_LOG;
+
+    // TODO: return if 3rd-party compass is enabled
+    return mCompassSensor->getEnable(handle);
+}
+
+/**
+    @brief      This function will return the current delay for this sensor.
+    @return     delay in nanoseconds. 
+**/
+int64_t CompassSensor::getDelay(int32_t handle)
+{
+    VFUNC_LOG;
+
+    // TODO: return 3rd-party's delay time (should be in ns)
+    return mCompassSensor->getDelay(handle);
+}
+
+/**
+    @brief         Integrators need to implement this function per 3rd-party solution
+    @param[out]    data      sensor data is stored in this variable. Scaled such that
+                             1 uT = 2^16
+    @para[in]      timestamp data's timestamp
+    @return        1, if 1   sample read, 0, if not, negative if error
+**/
+int CompassSensor::readSample(long *data, int64_t *timestamp)
+{
+    VFUNC_LOG;
+
+    // TODO: need to implement "readSample()" for MPL in 3rd-party's .cpp file
+    return mCompassSensor->readSample(data, timestamp);
+}
+
+/**
+    @brief         Integrators need to implement this function per 3rd-party solution
+    @param[out]    data      sensor data is stored in this variable. Scaled such that
+                             1 uT = 2^16
+    @para[in]      timestamp data's timestamp
+    @return        1, if 1   sample read, 0, if not, negative if error
+**/
+int CompassSensor::readRawSample(float *data, int64_t *timestamp)
+{
+    VFUNC_LOG;
+    long ldata[3];
+
+    int res = mCompassSensor->readSample(ldata, timestamp);
+    for(int i=0; i<3; i++) {
+        data[i] = (float)ldata[i];
+    }
+    return res; 
+}
+
+void CompassSensor::fillList(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    const char *compass = COMPASS_NAME;
+
+    if (compass) {
+        if (!strcmp(compass, "AKM8963")) {
+            list->maxRange = COMPASS_AKM8963_RANGE;
+            list->resolution = COMPASS_AKM8963_RESOLUTION;
+            list->power = COMPASS_AKM8963_POWER;
+            list->minDelay = COMPASS_AKM8963_MINDELAY;
+            return;
+        }
+        if (!strcmp(compass, "AKM8975")) {
+            list->maxRange = COMPASS_AKM8975_RANGE;
+            list->resolution = COMPASS_AKM8975_RESOLUTION;
+            list->power = COMPASS_AKM8975_POWER;
+            list->minDelay = COMPASS_AKM8975_MINDELAY;
+            LOGW("HAL:support for AKM8975 is incomplete");
+        }
+    }
+
+    LOGE("HAL:unsupported compass id %s -- "
+         "this implementation only supports AKM compasses", compass);
+    list->maxRange = COMPASS_AKM8975_RANGE;
+    list->resolution = COMPASS_AKM8975_RESOLUTION;
+    list->power = COMPASS_AKM8975_POWER;
+    list->minDelay = COMPASS_AKM8975_MINDELAY;
+}
+
+// TODO: specify compass sensor's mounting matrix for MPL
+void CompassSensor::getOrientationMatrix(signed char *orient)
+{
+    VFUNC_LOG;
+
+    orient[0] = 1;
+    orient[1] = 0;
+    orient[2] = 0;
+    orient[3] = 0;
+    orient[4] = 1;
+    orient[5] = 0;
+    orient[6] = 0;
+    orient[7] = 0;
+    orient[8] = 1;
+}
+
+int CompassSensor::getAccuracy(void)
+{
+    VFUNC_LOG;
+
+    // TODO: need to implement "getAccuracy()" for MPL in 3rd-party's .cpp file
+    return mCompassSensor->getAccuracy();
+}
diff --git a/65xx/libsensors_iio/CompassSensor.AKM.h b/65xx/libsensors_iio/CompassSensor.AKM.h
new file mode 100644
index 0000000..3d215a4
--- /dev/null
+++ b/65xx/libsensors_iio/CompassSensor.AKM.h
@@ -0,0 +1,84 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef COMPASS_SENSOR_H
+#define COMPASS_SENSOR_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#include "SensorBase.h"
+
+// TODO: include 3rd-party compass vendor's header file
+//       p.s.: before using unified HAL, make sure 3rd-party compass
+//       solution's driver/HAL work well by themselves
+#include "AkmSensor.h"
+
+/*****************************************************************************/
+
+class CompassSensor : public SensorBase {
+
+protected:
+
+public:
+            CompassSensor();
+    virtual ~CompassSensor();
+
+    // TODO: make sure either 3rd-party compass solution has following virtual
+    //       functions, or SensorBase.cpp could provide equal functionalities
+    virtual int getFd() const;
+    virtual int getRawFd() {return 0;};
+    virtual int enable(int32_t handle, int enabled);
+    virtual int setDelay(int32_t handle, int64_t ns);
+    virtual int getEnable(int32_t handle);
+    virtual int64_t getDelay(int32_t handle);
+    virtual int64_t getMinDelay() { return -1; } // stub
+
+    // TODO: unnecessary for MPL solution (override 3rd-party solution)
+    virtual int readEvents(sensors_event_t *data, int count) { return 0; }
+
+    // TODO: following four APIs need further implementation for MPL's
+    //       reference (look into .cpp for detailed information, also refer to
+    //       3rd-party's readEvents() for relevant APIs)
+    int readSample(long *data, int64_t *timestamp);
+    int readRawSample(float *data, int64_t *timestamp);
+    void fillList(struct sensor_t *list);
+    void getOrientationMatrix(signed char *orient);
+    int getAccuracy();
+    virtual void getCompassBias(long *bias) {return;};
+
+    // TODO: if 3rd-party provides calibrated compass data, just return 1
+    int providesCalibration() { return 1; }
+
+    // TODO: hard-coded for 3rd-party's sensitivity transformation
+    long getSensitivity() { return (1L << 30); }
+    
+    /* all 3rd pary solution have compasses on the primary bus, hence they
+       have no dependency on the MPU */
+    int isIntegrated() { return 0; }
+
+    int checkCoilsReset(void) { return 0; };
+    int isYasCompass(void) { return 0; };
+
+private:
+    AkmSensor *mCompassSensor;
+};
+
+/*****************************************************************************/
+
+#endif  // COMPASS_SENSOR_H
diff --git a/65xx/libsensors_iio/CompassSensor.IIO.9150.cpp b/65xx/libsensors_iio/CompassSensor.IIO.9150.cpp
new file mode 100644
index 0000000..3c0af83
--- /dev/null
+++ b/65xx/libsensors_iio/CompassSensor.IIO.9150.cpp
@@ -0,0 +1,392 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#define LOG_NDEBUG 0
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <sys/select.h>
+#include <cutils/log.h>
+#include <linux/input.h>
+#include <string.h>
+
+#include "CompassSensor.IIO.9150.h"
+#include "sensors.h"
+#include "MPLSupport.h"
+#include "sensor_params.h"
+#include "ml_sysfs_helper.h"
+
+#define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*)
+#define COMPASS_NAME "USE_SYSFS"
+
+#if defined COMPASS_YAS53x
+#pragma message("HAL:build Invensense compass cal with YAS53x IIO on secondary bus")
+#define USE_MPL_COMPASS_HAL (1)
+#define COMPASS_NAME        "INV_YAS530"
+
+#elif defined COMPASS_AK8975
+#pragma message("HAL:build Invensense compass cal with AK8975 on primary bus")
+#define USE_MPL_COMPASS_HAL (1)
+#define COMPASS_NAME        "INV_AK8975"
+
+#elif defined INVENSENSE_COMPASS_CAL
+#   define COMPASS_NAME                 "USE_SYSFS"
+#pragma message("HAL:build Invensense compass cal with compass IIO on secondary bus")
+#define USE_MPL_COMPASS_HAL (1)
+#else
+#pragma message("HAL:build third party compass cal HAL")
+#define USE_MPL_COMPASS_HAL (0)
+// TODO: change to vendor's name
+#define COMPASS_NAME        "AKM8975"
+
+#endif
+
+/*****************************************************************************/
+
+CompassSensor::CompassSensor() 
+                  : SensorBase(NULL, NULL),
+                    compass_fd(-1),
+                    mCompassTimestamp(0),
+                    mCompassInputReader(8)
+{
+    VFUNC_LOG;
+
+    if(!strcmp(COMPASS_NAME, "USE_SYSFS")) {
+        int result = find_name_by_sensor_type("in_magn_scale", "iio:device", sensor_name);
+        if(result) {
+            LOGE("HAL:Cannot read secondary device name - (%d)", result);
+        }
+        dev_name = sensor_name;
+    }
+    LOGI("HAL:Secondary Chip Id: %s", dev_name);
+
+    if(inv_init_sysfs_attributes()) {
+        LOGE("Error Instantiating Compass\n");
+        return;
+    }
+
+    /*if (!strcmp(COMPASS_NAME, "INV_COMPASS")) {
+        mI2CBus = COMPASS_BUS_SECONDARY;
+    } else {
+        mI2CBus = COMPASS_BUS_PRIMARY;
+    }*/
+
+    memset(mCachedCompassData, 0, sizeof(mCachedCompassData));
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", 
+            compassSysFs.compass_orient, getTimestamp());
+    FILE *fptr;
+    fptr = fopen(compassSysFs.compass_orient, "r");
+    if (fptr != NULL) {
+        int om[9];
+        fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", 
+               &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
+               &om[6], &om[7], &om[8]);
+        fclose(fptr);
+
+        LOGV_IF(EXTRA_VERBOSE,
+                "HAL:compass mounting matrix: "
+                "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
+                om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
+
+        mCompassOrientation[0] = om[0];
+        mCompassOrientation[1] = om[1];
+        mCompassOrientation[2] = om[2];
+        mCompassOrientation[3] = om[3];
+        mCompassOrientation[4] = om[4];
+        mCompassOrientation[5] = om[5];
+        mCompassOrientation[6] = om[6];
+        mCompassOrientation[7] = om[7];
+        mCompassOrientation[8] = om[8];
+    } else {
+        LOGE("HAL:Couldn't read compass mounting matrix");
+    }
+
+    if (!isIntegrated()) {
+        enable(ID_M, 0);
+    }
+}
+
+CompassSensor::~CompassSensor()
+{
+    VFUNC_LOG;
+
+    free(pathP);
+    if( compass_fd > 0)
+        close(compass_fd);
+}
+
+int CompassSensor::getFd() const
+{
+    VHANDLER_LOG;
+    return compass_fd;
+}
+
+/**
+ *  @brief        This function will enable/disable sensor.
+ *  @param[in]    handle
+ *                  which sensor to enable/disable.
+ *  @param[in]    en
+ *                  en=1, enable; 
+ *                  en=0, disable
+ *  @return       if the operation is successful.
+ */
+int CompassSensor::enable(int32_t handle, int en) 
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    res = write_sysfs_int(compassSysFs.compass_enable, en);
+
+    return res;
+}
+
+int CompassSensor::setDelay(int32_t handle, int64_t ns) 
+{
+    VFUNC_LOG;
+    int tempFd;
+    int res;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 
+            1000000000.f / ns, compassSysFs.compass_rate, getTimestamp());
+    mDelay = ns;
+    if (ns == 0)
+        return -1;
+    tempFd = open(compassSysFs.compass_rate, O_RDWR);
+    res = write_attribute_sensor(tempFd, 1000000000.f / ns);
+    if(res < 0) {
+        LOGE("HAL:Compass update delay error");
+    }
+    return res;
+}
+
+
+/**
+    @brief      This function will return the state of the sensor.
+    @return     1=enabled; 0=disabled
+**/
+int CompassSensor::getEnable(int32_t handle)
+{
+    VFUNC_LOG;
+    return mEnable;
+}
+
+/* use for Invensense compass calibration */
+#define COMPASS_EVENT_DEBUG (0)
+void CompassSensor::processCompassEvent(const input_event *event)
+{
+    VHANDLER_LOG;
+
+    switch (event->code) {
+    case EVENT_TYPE_ICOMPASS_X:
+        LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_X\n");
+        mCachedCompassData[0] = event->value;
+        break;
+    case EVENT_TYPE_ICOMPASS_Y:
+        LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Y\n");
+        mCachedCompassData[1] = event->value;
+        break;
+    case EVENT_TYPE_ICOMPASS_Z:
+        LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Z\n");
+        mCachedCompassData[2] = event->value;
+        break;
+    }
+    
+    mCompassTimestamp = 
+        (int64_t)event->time.tv_sec * 1000000000L + event->time.tv_usec * 1000L;
+}
+
+void CompassSensor::getOrientationMatrix(signed char *orient)
+{
+    VFUNC_LOG;
+    memcpy(orient, mCompassOrientation, sizeof(mCompassOrientation));
+}
+
+long CompassSensor::getSensitivity()
+{
+    VFUNC_LOG;
+
+    long sensitivity;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", 
+            compassSysFs.compass_scale, getTimestamp());
+    inv_read_data(compassSysFs.compass_scale, &sensitivity);
+    return sensitivity;
+}
+
+/**
+    @brief         This function is called by sensors_mpl.cpp
+                   to read sensor data from the driver.
+    @param[out]    data      sensor data is stored in this variable. Scaled such that
+                             1 uT = 2^16
+    @para[in]      timestamp data's timestamp
+    @return        1, if 1   sample read, 0, if not, negative if error
+ */
+int CompassSensor::readSample(long *data, int64_t *timestamp)
+{
+    VHANDLER_LOG;
+
+    int numEventReceived = 0, done = 0;
+
+    ssize_t n = mCompassInputReader.fill(compass_fd);
+    if (n < 0) {
+        LOGE("HAL:no compass events read");
+        return n;
+    }
+
+    input_event const* event;
+
+    while (done == 0 && mCompassInputReader.readEvent(&event)) {
+        int type = event->type;
+        if (type == EV_REL) {
+            processCompassEvent(event);
+        } else if (type == EV_SYN) {
+            *timestamp = mCompassTimestamp;
+            memcpy(data, mCachedCompassData, sizeof(mCachedCompassData));
+            done = 1;
+        } else {
+            LOGE("HAL:Compass Sensor: unknown event (type=%d, code=%d)",
+                 type, event->code);
+        }
+        mCompassInputReader.next();
+    }
+
+    return done;
+}
+
+/**
+ *  @brief  This function will return the current delay for this sensor.
+ *  @return delay in nanoseconds. 
+ */
+int64_t CompassSensor::getDelay(int32_t handle)
+{
+    VFUNC_LOG;
+    return mDelay;
+}
+
+void CompassSensor::fillList(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    const char *compass = sensor_name;
+
+    if (compass) {
+        if(!strcmp(compass, "INV_COMPASS")) {
+            list->maxRange = COMPASS_MPU9150_RANGE;
+            list->resolution = COMPASS_MPU9150_RESOLUTION;
+            list->power = COMPASS_MPU9150_POWER;
+            list->minDelay = COMPASS_MPU9150_MINDELAY;
+            return;
+        }
+        if(!strcmp(compass, "compass")
+                || !strcmp(compass, "INV_AK8975")
+                || !strncmp(compass, "AK89xx",2)
+                || !strncmp(compass, "ak89xx",2)) {
+            list->maxRange = COMPASS_AKM8975_RANGE;
+            list->resolution = COMPASS_AKM8975_RESOLUTION;
+            list->power = COMPASS_AKM8975_POWER;
+            list->minDelay = COMPASS_AKM8975_MINDELAY;
+            return;
+        }
+        if(!strcmp(compass, "INV_YAS530")) {
+            list->maxRange = COMPASS_YAS53x_RANGE;
+            list->resolution = COMPASS_YAS53x_RESOLUTION;
+            list->power = COMPASS_YAS53x_POWER;
+            list->minDelay = COMPASS_YAS53x_MINDELAY;
+            return;
+        }
+        if(!strcmp(compass, "INV_AMI306")) {
+            list->maxRange = COMPASS_AMI306_RANGE;
+            list->resolution = COMPASS_AMI306_RESOLUTION;
+            list->power = COMPASS_AMI306_POWER;
+            list->minDelay = COMPASS_AMI306_MINDELAY;
+            return;
+        }
+    }
+    LOGE("HAL:unknown compass id %s -- "
+         "params default to ak8975 and might be wrong.",
+         compass);
+    list->maxRange = COMPASS_AKM8975_RANGE;
+    list->resolution = COMPASS_AKM8975_RESOLUTION;
+    list->power = COMPASS_AKM8975_POWER;
+    list->minDelay = COMPASS_AKM8975_MINDELAY;
+}
+
+int CompassSensor::inv_init_sysfs_attributes(void)
+{
+    VFUNC_LOG;
+
+    unsigned char i = 0;
+    char sysfs_path[MAX_SYSFS_NAME_LEN];
+    char iio_trigger_path[MAX_SYSFS_NAME_LEN], tbuf[2];
+    char *sptr;
+    char **dptr;
+    int num;
+
+    pathP = (char*)malloc(
+                    sizeof(char[COMPASS_MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
+    sptr = pathP;
+    dptr = (char**)&compassSysFs;
+    if (sptr == NULL)
+        return -1;
+
+    memset(sysfs_path, 0, sizeof(sysfs_path));
+    memset(iio_trigger_path, 0, sizeof(iio_trigger_path));
+
+    do {
+        *dptr++ = sptr;
+        memset(sptr, 0, sizeof(sptr));
+        sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
+    } while (++i < COMPASS_MAX_SYSFS_ATTRB);
+
+    // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
+    // inv_get_sysfs_abs_path(sysfs_path);
+    inv_get_sysfs_path(sysfs_path);
+    inv_get_iio_trigger_path(iio_trigger_path);
+
+    if (strcmp(sysfs_path, "") == 0  || strcmp(iio_trigger_path, "") == 0)
+        return 0;
+
+#if defined COMPASS_AK8975
+    inv_get_input_number(dev_name, &num);
+    tbuf[0] = num + 0x30;
+    tbuf[1] = 0;
+    sprintf(sysfs_path, "%s%s", "sys/class/input/input", tbuf);
+    strcat(sysfs_path, "/ak8975");
+
+    sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/enable");
+    sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/rate");
+    sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/scale");
+    sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix");
+#else
+    sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/compass_enable");
+    sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/compass_rate");
+    sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/in_magn_scale");
+    sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix");
+#endif
+
+#if 0
+    // test print sysfs paths   
+    dptr = (char**)&compassSysFs;
+    for (i = 0; i < COMPASS_MAX_SYSFS_ATTRB; i++) {
+        LOGE("HAL:sysfs path: %s", *dptr++);
+    }
+#endif
+    return 0;
+}
diff --git a/65xx/libsensors_iio/CompassSensor.IIO.9150.h b/65xx/libsensors_iio/CompassSensor.IIO.9150.h
new file mode 100644
index 0000000..a69911f
--- /dev/null
+++ b/65xx/libsensors_iio/CompassSensor.IIO.9150.h
@@ -0,0 +1,97 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef COMPASS_SENSOR_H
+#define COMPASS_SENSOR_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+// TODO fixme, need input_event
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <poll.h>
+#include <utils/Vector.h>
+#include <utils/KeyedVector.h>
+
+#include "sensors.h"
+#include "SensorBase.h"
+#include "InputEventReader.h"
+
+class CompassSensor : public SensorBase {
+
+public:
+    CompassSensor();
+    virtual ~CompassSensor();
+
+    virtual int getFd() const;
+    virtual int enable(int32_t handle, int enabled);
+    virtual int setDelay(int32_t handle, int64_t ns);
+    virtual int getEnable(int32_t handle);
+    virtual int64_t getDelay(int32_t handle);
+    virtual int64_t getMinDelay() { return -1; } // stub
+
+    // unnecessary for MPL
+    virtual int readEvents(sensors_event_t *data, int count) { return 0; }
+
+    int readSample(long *data, int64_t *timestamp);
+    int providesCalibration() { return 0; }
+    void getOrientationMatrix(signed char *orient);
+    long getSensitivity();
+    int getAccuracy() { return 0; }
+    void fillList(struct sensor_t *list);
+    int isIntegrated() { return (1); }
+    int isYasCompass(void) { return (0); }
+    int checkCoilsReset(void) { return(-1); }
+
+private:
+    char sensor_name[200];
+    enum CompassBus {
+        COMPASS_BUS_PRIMARY = 0,
+        COMPASS_BUS_SECONDARY = 1
+    } mI2CBus;
+
+    struct sysfs_attrbs {
+       char *compass_enable;
+       char *compass_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/65xx/libsensors_iio/CompassSensor.IIO.primary.cpp b/65xx/libsensors_iio/CompassSensor.IIO.primary.cpp
new file mode 100644
index 0000000..572fa55
--- /dev/null
+++ b/65xx/libsensors_iio/CompassSensor.IIO.primary.cpp
@@ -0,0 +1,572 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#define LOG_NDEBUG 0
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <sys/select.h>
+#include <cutils/log.h>
+#include <linux/input.h>
+#include <string.h>
+
+#include "CompassSensor.IIO.primary.h"
+#include "sensors.h"
+#include "MPLSupport.h"
+#include "sensor_params.h"
+#include "ml_sysfs_helper.h"
+
+#define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*)
+#define COMPASS_NAME "USE_SYSFS"
+
+#if defined COMPASS_AK8975
+#pragma message("HAL:build Invensense compass cal with AK8975 on primary bus")
+#define USE_MPL_COMPASS_HAL (1)
+#define COMPASS_NAME        "INV_AK8975"
+#endif
+
+/******************************************************************************/
+
+CompassSensor::CompassSensor() 
+                  : SensorBase(COMPASS_NAME, NULL),
+                    mCompassTimestamp(0),
+                    mCompassInputReader(8),
+                    mCoilsResetFd(0)
+{
+    FILE *fptr;
+
+    VFUNC_LOG;
+
+    mYasCompass = false;
+    if(!strcmp(dev_name, "USE_SYSFS")) {
+        char sensor_name[20]; 
+        find_name_by_sensor_type("in_magn_x_raw", "iio:device", sensor_name);
+        strncpy(dev_full_name, sensor_name,
+                sizeof(dev_full_name) / sizeof(dev_full_name[0]));
+        if(!strncmp(dev_full_name, "yas", 3)) {
+            mYasCompass = true;
+        }
+    } else {
+
+#ifdef COMPASS_YAS53x
+        /* for YAS53x compasses, dev_name is just a prefix, 
+           we need to find the actual name */
+        if (fill_dev_full_name_by_prefix(dev_name, 
+                dev_full_name, sizeof(dev_full_name) / sizeof(dev_full_name[0]))) {
+            LOGE("Cannot find Yamaha device with prefix name '%s' - "
+                 "magnetometer will likely not work.", dev_name);
+        } else {
+            mYasCompass = true;
+        }
+#else
+        strncpy(dev_full_name, dev_name,
+                sizeof(dev_full_name) / sizeof(dev_full_name[0]));
+#endif
+
+}
+
+    if (inv_init_sysfs_attributes()) {
+        LOGE("Error Instantiating Compass\n");
+        return;
+    }
+
+    if (!strcmp(dev_full_name, "INV_COMPASS")) {
+        mI2CBus = COMPASS_BUS_SECONDARY;
+    } else {
+        mI2CBus = COMPASS_BUS_PRIMARY;
+    }
+
+    memset(mCachedCompassData, 0, sizeof(mCachedCompassData));
+
+    if (!isIntegrated()) {
+        enable(ID_M, 0);
+    }
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:compass name: %s", dev_full_name);
+    enable_iio_sysfs();
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", 
+            compassSysFs.compass_orient, getTimestamp());
+    fptr = fopen(compassSysFs.compass_orient, "r");
+    if (fptr != NULL) {
+        int om[9];
+        fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", 
+               &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
+               &om[6], &om[7], &om[8]);
+        fclose(fptr);
+
+        LOGV_IF(EXTRA_VERBOSE,
+                "HAL:compass mounting matrix: "
+                "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
+                om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
+
+        mCompassOrientation[0] = om[0];
+        mCompassOrientation[1] = om[1];
+        mCompassOrientation[2] = om[2];
+        mCompassOrientation[3] = om[3];
+        mCompassOrientation[4] = om[4];
+        mCompassOrientation[5] = om[5];
+        mCompassOrientation[6] = om[6];
+        mCompassOrientation[7] = om[7];
+        mCompassOrientation[8] = om[8];
+    } else {
+        LOGE("HAL:Couldn't read compass mounting matrix");
+    }
+
+    if(mYasCompass) {
+        mCoilsResetFd = fopen(compassSysFs.compass_attr_1, "r+");
+        if (fptr == NULL) {
+            LOGE("HAL:Couldn't read compass overunderflow");
+        }
+    }
+}
+
+void CompassSensor::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;
+    const char* compass = dev_full_name;
+
+    write_sysfs_int(compassSysFs.in_timestamp_en, 1);
+
+    LOGV_IF(SYSFS_VERBOSE, 
+            "HAL:sysfs:cat %s (%lld)", 
+            compassSysFs.trigger_name, getTimestamp());
+    tempFp = fopen(compassSysFs.trigger_name, "r");
+    if (tempFp == NULL) {
+        LOGE("HAL:could not open %s trigger name", compass);
+    } 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, compassSysFs.current_trigger, getTimestamp());
+    tempFp = fopen(compassSysFs.current_trigger, "w");
+    if (tempFp == NULL) {
+        LOGE("HAL:could not open current trigger");
+    } else {
+        if (fprintf(tempFp, "%s", iio_trigger_name) < 0 || fclose(tempFp) < 0) {
+            LOGE("HAL:could not write current trigger");
+        }
+    }
+
+    LOGV_IF(SYSFS_VERBOSE, 
+            "HAL:sysfs:echo %d > %s (%lld)", 
+            IIO_BUFFER_LENGTH, compassSysFs.buffer_length, getTimestamp());
+    tempFp = fopen(compassSysFs.buffer_length, "w");
+    if (tempFp == NULL) {
+        LOGE("HAL:could not open buffer length");
+    } else {
+        if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0 || fclose(tempFp) < 0) {
+            LOGE("HAL:could not write buffer length");
+        }
+    }
+
+    sprintf(iio_device_node, "%s%d", "/dev/iio:device",
+            find_type_by_name(compass, "iio:device"));
+    compass_fd = open(iio_device_node, O_RDONLY);
+    int res = errno;
+    if (compass_fd < 0) {
+        LOGE("HAL:could not open '%s' iio device node in path '%s' - "
+             "error '%s' (%d)",
+             compass, iio_device_node, strerror(res), res);
+    } else {
+        LOGV_IF(EXTRA_VERBOSE, 
+                "HAL:iio %s, compass_fd opened : %d", compass, compass_fd);
+    }
+
+    /* TODO: need further tests for optimization to reduce context-switch
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)", 
+            compassSysFs.compass_x_fifo_enable, getTimestamp());
+    tempFd = open(compassSysFs.compass_x_fifo_enable, O_RDWR);
+    res = errno;
+    if (tempFd > 0) {
+        res = enable_sysfs_sensor(tempFd, 1);
+    } else {
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             compassSysFs.compass_x_fifo_enable, strerror(res), res);
+    }
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)", 
+            compassSysFs.compass_y_fifo_enable, getTimestamp());
+    tempFd = open(compassSysFs.compass_y_fifo_enable, O_RDWR);
+    res = errno;
+    if (tempFd > 0) {
+        res = enable_sysfs_sensor(tempFd, 1);
+    } else {
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             compassSysFs.compass_y_fifo_enable, strerror(res), res);
+    }
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)", 
+            compassSysFs.compass_z_fifo_enable, getTimestamp());
+    tempFd = open(compassSysFs.compass_z_fifo_enable, O_RDWR);
+    res = errno;
+    if (tempFd > 0) {
+        res = enable_sysfs_sensor(tempFd, 1);
+    } else {
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             compassSysFs.compass_z_fifo_enable, strerror(res), res);
+    }
+    */
+}
+
+CompassSensor::~CompassSensor()
+{
+    VFUNC_LOG;
+
+    free(pathP);
+    if( compass_fd > 0)
+        close(compass_fd);
+    if(mYasCompass) {
+        if( mCoilsResetFd != NULL )
+            fclose(mCoilsResetFd);
+    }
+}
+
+int CompassSensor::getFd(void) const
+{
+    VHANDLER_LOG;
+    LOGI_IF(0, "HAL:compass_fd=%d", compass_fd);
+    return compass_fd;
+}
+
+/**
+ *  @brief        This function will enable/disable sensor.
+ *  @param[in]    handle
+ *                  which sensor to enable/disable.
+ *  @param[in]    en
+ *                  en=1, enable; 
+ *                  en=0, disable
+ *  @return       if the operation is successful.
+ */
+int CompassSensor::enable(int32_t handle, int en) 
+{
+    VFUNC_LOG;
+
+    mEnable = en;
+    int tempFd;
+    int res = 0;
+
+    /* reset master enable */
+    res = masterEnable(0);
+    if (res < 0) {
+        return res;
+    }
+
+    if (en) {
+        res = write_sysfs_int(compassSysFs.compass_x_fifo_enable, en);
+        res += write_sysfs_int(compassSysFs.compass_y_fifo_enable, en);
+        res += write_sysfs_int(compassSysFs.compass_z_fifo_enable, en);
+
+        res = masterEnable(en);
+        if (res < en) {
+            return res;
+        }
+    }
+
+    return res;
+}
+
+int CompassSensor::masterEnable(int en) {
+    VFUNC_LOG;
+    return write_sysfs_int(compassSysFs.chip_enable, en);
+}
+
+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) {
+    VFUNC_LOG;
+
+    int i;
+    char *rdata = mIIOBuffer;
+
+    size_t rsize = read(compass_fd, rdata, (8 * mEnable + 8) * 1);
+
+    if (!mEnable) {
+        rsize = read(compass_fd, rdata, (8 + 8) * IIO_BUFFER_LENGTH);
+        // LOGI("clear buffer with size: %d", rsize);
+    }
+/*
+    LOGI("get one sample of AMI IIO data with size: %d", rsize);
+    LOGI_IF(mEnable, "compass x/y/z: %d/%d/%d", *((short *) (rdata + 0)),
+        *((short *) (rdata + 2)), *((short *) (rdata + 4)));
+*/
+    if (mEnable) {
+        for (i = 0; i < 3; i++) {
+            data[i] = *((short *) (rdata + i * 2));
+        }
+        *timestamp = *((long long *) (rdata + 8 * mEnable));
+    }
+
+    return mEnable;
+}
+
+/**
+ *  @brief  This function will return the current delay for this sensor.
+ *  @return delay in nanoseconds. 
+ */
+int64_t CompassSensor::getDelay(int32_t handle)
+{
+    VFUNC_LOG;
+    return mDelay;
+}
+
+void CompassSensor::fillList(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    const char *compass = dev_full_name;
+
+    if (compass) {
+        if(!strcmp(compass, "INV_COMPASS")) {
+            list->maxRange = COMPASS_MPU9150_RANGE;
+            list->resolution = COMPASS_MPU9150_RESOLUTION;
+            list->power = COMPASS_MPU9150_POWER;
+            list->minDelay = COMPASS_MPU9150_MINDELAY;
+            mMinDelay = list->minDelay;
+            return;
+        }
+        if(!strcmp(compass, "compass")
+                || !strcmp(compass, "INV_AK8975")
+                || !strncmp(compass, "ak89xx", 2)) {
+            list->maxRange = COMPASS_AKM8975_RANGE;
+            list->resolution = COMPASS_AKM8975_RESOLUTION;
+            list->power = COMPASS_AKM8975_POWER;
+            list->minDelay = COMPASS_AKM8975_MINDELAY;
+            mMinDelay = list->minDelay;
+            return;
+        }
+        if(!strcmp(compass, "ami306")) {
+            list->maxRange = COMPASS_AMI306_RANGE;
+            list->resolution = COMPASS_AMI306_RESOLUTION;
+            list->power = COMPASS_AMI306_POWER;
+            list->minDelay = COMPASS_AMI306_MINDELAY;
+            mMinDelay = list->minDelay;
+            return;
+        }
+        if(!strcmp(compass, "yas530") 
+                || !strcmp(compass, "yas532")
+                || !strcmp(compass, "yas533")) {
+            list->maxRange = COMPASS_YAS53x_RANGE;
+            list->resolution = COMPASS_YAS53x_RESOLUTION;
+            list->power = COMPASS_YAS53x_POWER;
+            list->minDelay = COMPASS_YAS53x_MINDELAY;
+            mMinDelay = list->minDelay;
+            return;
+        }
+    }
+
+    LOGE("HAL:unknown compass id %s -- "
+         "params default to ak8975 and might be wrong.",
+         compass);
+    list->maxRange = COMPASS_AKM8975_RANGE;
+    list->resolution = COMPASS_AKM8975_RESOLUTION;
+    list->power = COMPASS_AKM8975_POWER;
+    list->minDelay = COMPASS_AKM8975_MINDELAY;
+    mMinDelay = list->minDelay;
+}
+
+/* Read sysfs entry to determine whether overflow had happend
+   then write to sysfs to reset to zero */
+int CompassSensor::checkCoilsReset()
+{
+    int result=-1;
+    VFUNC_LOG;
+
+    if(mCoilsResetFd != NULL) {
+        int attr;
+        rewind(mCoilsResetFd);
+        fscanf(mCoilsResetFd, "%d", &attr);
+        if(attr == 0)
+            return 0;
+        else {
+            LOGV_IF(SYSFS_VERBOSE, "HAL:overflow detected");
+            rewind(mCoilsResetFd);
+            if(fprintf(mCoilsResetFd, "%d", 0) < 0)
+                LOGE("HAL:could not write overunderflow");
+            else
+                return 1;
+        }
+    } else {
+        LOGE("HAL:could not read overunderflow");
+    }
+    return result;
+}
+
+int CompassSensor::inv_init_sysfs_attributes(void)
+{
+    VFUNC_LOG;
+
+    unsigned char i = 0;
+    char sysfs_path[MAX_SYSFS_NAME_LEN], 
+         iio_trigger_path[MAX_SYSFS_NAME_LEN], tbuf[2];
+    char *sptr;
+    char **dptr;
+    int num;
+    const char* compass = dev_full_name;
+
+    pathP = (char*)malloc(
+                    sizeof(char[COMPASS_MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
+    sptr = pathP;
+    dptr = (char**)&compassSysFs;
+    if (sptr == NULL)
+        return -1;
+
+    do {
+        *dptr++ = sptr;
+        sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
+    } while (++i < COMPASS_MAX_SYSFS_ATTRB);
+
+    // get proper (in absolute/relative) IIO path & build sysfs paths
+    sprintf(sysfs_path, "%s%d", "/sys/bus/iio/devices/iio:device",
+        find_type_by_name(compass, "iio:device"));
+    sprintf(iio_trigger_path, "%s%d", "/sys/bus/iio/devices/trigger",
+        find_type_by_name(compass, "iio:device"));
+
+#if defined COMPASS_AK8975
+    inv_get_input_number(compass, &num);
+    tbuf[0] = num + 0x30;
+    tbuf[1] = 0;
+    sprintf(sysfs_path, "%s%s", "sys/class/input/input", tbuf);
+    strcat(sysfs_path, "/ak8975");
+
+    sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/enable");
+    sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/rate");
+    sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/scale");
+    sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix");
+#else /* IIO */
+    sprintf(compassSysFs.chip_enable, "%s%s", sysfs_path, "/buffer/enable");
+    sprintf(compassSysFs.in_timestamp_en, "%s%s", sysfs_path, "/scan_elements/in_timestamp_en");
+    sprintf(compassSysFs.trigger_name, "%s%s", iio_trigger_path, "/name");
+    sprintf(compassSysFs.current_trigger, "%s%s", sysfs_path, "/trigger/current_trigger");
+    sprintf(compassSysFs.buffer_length, "%s%s", sysfs_path, "/buffer/length");
+
+    sprintf(compassSysFs.compass_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_x_en");
+    sprintf(compassSysFs.compass_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_y_en");
+    sprintf(compassSysFs.compass_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_z_en");
+    sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/sampling_frequency");
+    sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/in_magn_scale");
+    sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix");
+
+    if(mYasCompass) {
+        sprintf(compassSysFs.compass_attr_1, "%s%s", sysfs_path, "/overunderflow");
+    }
+#endif
+
+#if 0 
+    // test print sysfs paths   
+    dptr = (char**)&compassSysFs;
+    LOGI("sysfs path base: %s", sysfs_path);
+    LOGI("trigger sysfs path base: %s", iio_trigger_path);
+    for (i = 0; i < COMPASS_MAX_SYSFS_ATTRB; i++) {
+        LOGE("HAL:sysfs path: %s", *dptr++);
+    }
+#endif
+    return 0;
+}
+
+int CompassSensor::isYasCompass(void)
+{
+    return mYasCompass;
+}
diff --git a/65xx/libsensors_iio/CompassSensor.IIO.primary.h b/65xx/libsensors_iio/CompassSensor.IIO.primary.h
new file mode 100644
index 0000000..9fcfabd
--- /dev/null
+++ b/65xx/libsensors_iio/CompassSensor.IIO.primary.h
@@ -0,0 +1,116 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef COMPASS_SENSOR_H
+#define COMPASS_SENSOR_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+// TODO fixme, need input_event
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <poll.h>
+#include <utils/Vector.h>
+#include <utils/KeyedVector.h>
+
+#include "sensors.h"
+#include "SensorBase.h"
+#include "InputEventReader.h"
+
+#define MAX_CHIP_ID_LEN (20)
+#define COMPASS_ON_PRIMARY "in_magn_x_raw"
+
+class CompassSensor : public SensorBase {
+
+public:
+    CompassSensor();
+    virtual ~CompassSensor();
+
+    virtual int getFd() const;
+    virtual int enable(int32_t handle, int enabled);
+    virtual int setDelay(int32_t handle, int64_t ns);
+    virtual int getEnable(int32_t handle);
+    virtual int64_t getDelay(int32_t handle);
+    virtual int64_t getMinDelay() { return mMinDelay; }
+
+    // unnecessary for MPL
+    virtual int readEvents(sensors_event_t *data, int count) { return 0; }
+
+    int readSample(long *data, int64_t *timestamp);
+    int readRawSample(float *data, int64_t *timestamp);
+    int providesCalibration() { return 0; }
+    void getOrientationMatrix(signed char *orient);
+    long getSensitivity();
+    int getAccuracy() { return 0; }
+    void fillList(struct sensor_t *list);
+    int isIntegrated() { return (0); }
+    int checkCoilsReset(void);
+    int isYasCompass(void);
+
+private:
+    enum CompassBus {
+        COMPASS_BUS_PRIMARY = 0,
+        COMPASS_BUS_SECONDARY = 1
+    } mI2CBus;
+
+    struct sysfs_attrbs {
+       char *chip_enable;
+       char *in_timestamp_en;
+       char *trigger_name;
+       char *current_trigger;
+       char *buffer_length;
+
+       char *compass_enable;
+       char *compass_x_fifo_enable;
+       char *compass_y_fifo_enable;
+       char *compass_z_fifo_enable;
+       char *compass_rate;
+       char *compass_scale;
+       char *compass_orient;
+       char *compass_attr_1;
+    } compassSysFs;
+    
+    char dev_full_name[20];
+
+    // implementation specific
+    signed char mCompassOrientation[9];
+    long mCachedCompassData[3];
+    int64_t mCompassTimestamp;
+    InputEventCircularReader mCompassInputReader;
+    int compass_fd;
+    int64_t mDelay;
+    int64_t mMinDelay;
+    int mEnable;
+    char *pathP;
+
+    char mIIOBuffer[(8 + 8) * IIO_BUFFER_LENGTH];
+
+    int masterEnable(int en);
+    void enable_iio_sysfs(void);
+    void processCompassEvent(const input_event *event);
+    int inv_init_sysfs_attributes(void);
+    FILE *mCoilsResetFd;
+    bool mYasCompass;
+};
+
+/*****************************************************************************/
+
+#endif  // COMPASS_SENSOR_H
diff --git a/65xx/libsensors_iio/InputEventReader.cpp b/65xx/libsensors_iio/InputEventReader.cpp
new file mode 100644
index 0000000..62afbf0
--- /dev/null
+++ b/65xx/libsensors_iio/InputEventReader.cpp
@@ -0,0 +1,114 @@
+/*

+* Copyright (C) 2012 Invensense, Inc.

+*

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

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

+* You may obtain a copy of the License at

+*

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

+*

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

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

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

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

+* limitations under the License.

+*/

+

+#define LOG_NDEBUG 0

+

+#include <stdint.h>

+#include <errno.h>

+#include <unistd.h>

+#include <poll.h>

+

+#include <sys/cdefs.h>

+#include <sys/types.h>

+

+#include <linux/input.h>

+

+#include <cutils/log.h>

+

+#include "InputEventReader.h"

+

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

+

+struct input_event;

+

+InputEventCircularReader::InputEventCircularReader(size_t numEvents)

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

+      mBufferEnd(mBuffer + numEvents),

+      mHead(mBuffer),

+      mCurr(mBuffer),

+      mFreeSpace(numEvents)

+{

+    mLastFd = -1;

+}

+

+InputEventCircularReader::~InputEventCircularReader()

+{

+    delete [] mBuffer;

+}

+

+#define INPUT_EVENT_DEBUG (0)

+ssize_t InputEventCircularReader::fill(int fd)

+{

+    size_t numEventsRead = 0;

+    mLastFd = fd;

+

+    LOGV_IF(INPUT_EVENT_DEBUG, 

+            "DEBUG:%s enter, fd=%d\n", __PRETTY_FUNCTION__, fd);

+    if (mFreeSpace) {

+        const ssize_t nread = read(fd, mHead, mFreeSpace * sizeof(input_event));

+        if (nread < 0 || nread % sizeof(input_event)) {

+            //LOGE("Partial event received nread=%d, required=%d", 

+            //     nread, sizeof(input_event));

+            //LOGE("FD trying to read is: %d");

+            // we got a partial event!!

+            if (INPUT_EVENT_DEBUG) {

+                LOGV_IF(nread < 0, "DEBUG:%s exit nread < 0\n", 

+                        __PRETTY_FUNCTION__);

+                LOGV_IF(nread % sizeof(input_event), 

+                        "DEBUG:%s exit nread %% sizeof(input_event)\n", 

+                        __PRETTY_FUNCTION__);

+            }

+            return (nread < 0 ? -errno : -EINVAL);

+        }

+

+        numEventsRead = nread / sizeof(input_event);

+        if (numEventsRead) {

+            mHead += numEventsRead;

+            mFreeSpace -= numEventsRead;

+            if (mHead > mBufferEnd) {

+                size_t s = mHead - mBufferEnd;

+                memcpy(mBuffer, mBufferEnd, s * sizeof(input_event));

+                mHead = mBuffer + s;

+            }

+        }

+    }

+

+    LOGV_IF(INPUT_EVENT_DEBUG, "DEBUG:%s exit, numEventsRead:%d\n", 

+            __PRETTY_FUNCTION__, numEventsRead);

+    return numEventsRead;

+}

+

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

+{

+    *events = mCurr;

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

+    LOGV_IF(INPUT_EVENT_DEBUG, "DEBUG:%s fd:%d, available:%d\n", 

+            __PRETTY_FUNCTION__, mLastFd, (int)available);

+    return (available ? 1 : 0);

+}

+

+void InputEventCircularReader::next()

+{

+    mCurr++;

+    mFreeSpace++;

+    if (mCurr >= mBufferEnd) {

+        mCurr = mBuffer;

+    }

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

+    LOGV_IF(INPUT_EVENT_DEBUG, "DEBUG:%s fd:%d, still available:%d\n",

+            __PRETTY_FUNCTION__, mLastFd, (int)available);

+}

+

diff --git a/65xx/libsensors_iio/InputEventReader.h b/65xx/libsensors_iio/InputEventReader.h
new file mode 100644
index 0000000..78d562f
--- /dev/null
+++ b/65xx/libsensors_iio/InputEventReader.h
@@ -0,0 +1,50 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef ANDROID_INPUT_EVENT_READER_H
+#define ANDROID_INPUT_EVENT_READER_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#include "SensorBase.h"
+
+/*****************************************************************************/
+
+struct input_event;
+
+class InputEventCircularReader
+{
+    struct input_event* const mBuffer;
+    struct input_event* const mBufferEnd;
+    struct input_event* mHead;
+    struct input_event* mCurr;
+    ssize_t mFreeSpace;
+    int mLastFd;

+
+public:
+    InputEventCircularReader(size_t numEvents);
+    ~InputEventCircularReader();
+    ssize_t fill(int fd);
+    ssize_t readEvent(input_event const** events);
+    void next();
+};
+
+/*****************************************************************************/
+
+#endif  // ANDROID_INPUT_EVENT_READER_H
diff --git a/65xx/libsensors_iio/MPLSensor.cpp b/65xx/libsensors_iio/MPLSensor.cpp
new file mode 100644
index 0000000..7754ac3
--- /dev/null
+++ b/65xx/libsensors_iio/MPLSensor.cpp
@@ -0,0 +1,6004 @@
+/*
+* 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
+
+//#define KLP 1 //Key Lime Pie Temporary Test Define
+//see also the EXTRA_VERBOSE define in the MPLSensor.h header file
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <float.h>
+#include <poll.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <stdlib.h>
+#include <sys/select.h>
+#include <sys/syscall.h>
+#include <dlfcn.h>
+#include <pthread.h>
+#include <cutils/log.h>
+#include <utils/KeyedVector.h>
+#include <utils/String8.h>
+#include <string.h>
+#include <linux/input.h>
+#include <utils/Atomic.h>
+
+#include "MPLSensor.h"
+#include "PressureSensor.IIO.secondary.h"
+#include "MPLSupport.h"
+#include "sensor_params.h"
+
+#include "invensense.h"
+#include "invensense_adv.h"
+#include "ml_stored_data.h"
+#include "ml_load_dmp.h"
+#include "ml_sysfs_helper.h"
+
+#define ENABLE_MULTI_RATE
+// #define TESTING
+// #define USE_LPQ_AT_FASTEST
+#define ENABLE_PRESSSURE
+
+#ifdef THIRD_PARTY_ACCEL
+#pragma message("HAL:build third party accel support")
+#define USE_THIRD_PARTY_ACCEL (1)
+#else
+#define USE_THIRD_PARTY_ACCEL (0)
+#endif
+
+#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))
+
+/******************************************************************************/
+/*  MPL interface misc.                                                       */
+/******************************************************************************/
+static int hertz_request = 200;
+
+#define DEFAULT_MPL_GYRO_RATE           (20000L)     //us
+#define DEFAULT_MPL_COMPASS_RATE        (20000L)     //us
+
+#define DEFAULT_HW_GYRO_RATE            (100)        //Hz
+#define DEFAULT_HW_ACCEL_RATE           (20)         //ms
+#define DEFAULT_HW_COMPASS_RATE         (20000000L)  //ns
+#define DEFAULT_HW_AKMD_COMPASS_RATE    (200000000L) //ns
+
+/* convert ns to hardware units */
+#define HW_GYRO_RATE_NS                 (1000000000LL / rate_request) // to Hz
+#define HW_ACCEL_RATE_NS                (rate_request / (1000000L))   // to ms
+#define HW_COMPASS_RATE_NS              (rate_request)                // to ns
+
+/* convert Hz to hardware units */
+#define HW_GYRO_RATE_HZ                 (hertz_request)
+#define HW_ACCEL_RATE_HZ                (1000 / hertz_request)
+#define HW_COMPASS_RATE_HZ              (1000000000LL / hertz_request)
+
+#define RATE_200HZ                      5000000LL
+#define RATE_15HZ                       66667000LL
+#define RATE_5HZ                        200000000LL
+
+// mask of virtual sensors that require gyro + accel + compass data
+#define VIRTUAL_SENSOR_9AXES_MASK ( \
+        (1 << Orientation)          \
+        | (1 << RotationVector)     \
+        | (1 << LinearAccel)        \
+        | (1 << Gravity)            \
+)
+// mask of virtual sensors that require gyro + accel data (but no compass data)
+#define VIRTUAL_SENSOR_GYRO_6AXES_MASK ( \
+        (1 << GameRotationVector)   \
+)
+// mask of virtual sensors that require mag + accel data (but no gyro data)
+#define VIRTUAL_SENSOR_MAG_6AXES_MASK (    \
+        (1 << GeomagneticRotationVector)   \
+)
+// mask of all virtual sensors
+#define VIRTUAL_SENSOR_ALL_MASK (       \
+        VIRTUAL_SENSOR_9AXES_MASK       \
+        | VIRTUAL_SENSOR_GYRO_6AXES_MASK     \
+        | VIRTUAL_SENSOR_MAG_6AXES_MASK \
+)
+
+static struct timespec mt_pre;
+static struct sensor_t sSensorList[] =
+{
+    {"MPL Gyroscope", "Invensense", 1,
+     SENSORS_GYROSCOPE_HANDLE,
+     SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
+    {"MPL Raw Gyroscope", "Invensense", 1,
+     SENSORS_RAW_GYROSCOPE_HANDLE,
+     SENSOR_TYPE_GYROSCOPE_UNCALIBRATED, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
+    {"MPL Accelerometer", "Invensense", 1,
+     SENSORS_ACCELERATION_HANDLE,
+     SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
+    {"MPL Magnetic Field", "Invensense", 1,
+     SENSORS_MAGNETIC_FIELD_HANDLE,
+     SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
+#ifdef ENABLE_PRESSURE
+     {"MPL Pressure", "Invensense", 1,
+     SENSORS_PRESSURE_HANDLE,
+     SENSOR_TYPE_PRESSURE, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
+#endif
+    {"MPL Raw Magnetic Field", "Invensense", 1,
+     SENSORS_RAW_MAGNETIC_FIELD_HANDLE,
+     SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
+    {"MPL Orientation", "Invensense", 1,
+     SENSORS_ORIENTATION_HANDLE,
+     SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, {}},
+    {"MPL Rotation Vector", "Invensense", 1,
+     SENSORS_ROTATION_VECTOR_HANDLE,
+     SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
+    {"MPL Game Rotation Vector", "Invensense", 1,
+     SENSORS_GAME_ROTATION_VECTOR_HANDLE,
+     SENSOR_TYPE_GAME_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
+    {"MPL Linear Acceleration", "Invensense", 1,
+     SENSORS_LINEAR_ACCEL_HANDLE,
+     SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
+    {"MPL Gravity", "Invensense", 1,
+     SENSORS_GRAVITY_HANDLE,
+     SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
+    {"MPL Significant Motion", "Invensense", 1,
+     SENSORS_SIGNIFICANT_MOTION_HANDLE,
+     SENSOR_TYPE_SIGNIFICANT_MOTION, 100.0f, 1.0f, 1.1f, 0, 0, 0, {}},
+    {"MPL Step Detector", "Invensense", 1,
+     SENSORS_PEDOMETER_HANDLE,
+     SENSOR_TYPE_STEP_DETECTOR, 100.0f, 1.0f, 1.1f, 0, 0, 0, {}},
+    {"MPL Step Counter", "Invensense", 1,
+     SENSORS_STEP_COUNTER_HANDLE,
+     SENSOR_TYPE_STEP_COUNTER, 100.0f, 1.0f, 1.1f, 0, 0, 0, {}},
+    {"MPL Geomagnetic Rotation Vector", "Invensense", 1,
+     SENSORS_GEOMAGNETIC_ROTATION_VECTOR_HANDLE,
+     SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}},
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+    {"MPL Screen Orientation", "Invensense ", 1,
+     SENSORS_SCREEN_ORIENTATION_HANDLE,
+     SENSOR_TYPE_SCREEN_ORIENTATION, 100.0f, 1.0f, 1.1f, 0, 0, 0, {}},
+#endif
+};
+
+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
+
+//#define IINV_PLAYBACK_DBG
+#ifdef INV_PLAYBACK_DBG
+static FILE *logfile = NULL;
+#endif
+
+/*******************************************************************************
+ * MPLSensor class implementation
+ ******************************************************************************/
+
+// following extended initializer list would only be available with -std=c++11
+//  or -std=gnu+11
+MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *))
+                       : SensorBase(NULL, NULL),
+                         mNewData(0),
+                         mMasterSensorMask(INV_ALL_SENSORS),
+                         mLocalSensorMask(0),
+                         mPollTime(-1),
+                         mStepCountPollTime(-1),
+                         mHaveGoodMpuCal(0),
+                         mGyroAccuracy(0),
+                         mAccelAccuracy(0),
+                         mCompassAccuracy(0),
+                         mSampleCount(0),
+                         dmp_orient_fd(-1),
+                         mDmpOrientationEnabled(0),
+                         dmp_sign_motion_fd(-1),
+                         mDmpSignificantMotionEnabled(0),
+                         dmp_pedometer_fd(-1),
+                         mDmpPedometerEnabled(0),
+                         mDmpStepCountEnabled(0),
+                         mEnabled(0),
+                         mBatchEnabled(0),
+                         mAccelInputReader(4),
+                         mGyroInputReader(32),
+                         mTempScale(0),
+                         mTempOffset(0),
+                         mTempCurrentTime(0),
+                         mAccelScale(2),
+                         mGyroScale(2000),
+                         mCompassScale(0),
+                         mFactoryGyroBiasAvailable(false),
+                         mGyroBiasAvailable(false),
+                         mGyroBiasApplied(false),
+                         mFactoryAccelBiasAvailable(false),
+                         mAccelBiasAvailable(false),
+                         mAccelBiasApplied(false),
+                         mPendingMask(0),
+                         mSensorMask(0),
+                         mMplFeatureActiveMask(0),
+                         mFeatureActiveMask(0),
+                         mDmpOn(0),
+                         mPedUpdate(0),
+                         mQuatSensorTimestamp(0),
+                         mStepSensorTimestamp(0),
+                         mLastStepCount(0),
+                         mLeftOverBufferSize(0) {
+    VFUNC_LOG;
+
+    inv_error_t rv;
+    int i, fd;
+    char *port = NULL;
+    char *ver_str;
+    unsigned long mSensorMask;
+    int res;
+    FILE *fptr;
+
+    mCompassSensor = compass;
+
+    LOGV_IF(EXTRA_VERBOSE,
+            "HAL:MPLSensor constructor : NumSensors = %d", NumSensors);
+
+    pthread_mutex_init(&mMplMutex, NULL);
+    pthread_mutex_init(&mHALMutex, NULL);
+    memset(mGyroOrientation, 0, sizeof(mGyroOrientation));
+    memset(mAccelOrientation, 0, sizeof(mAccelOrientation));
+
+    /* setup sysfs paths */
+    inv_init_sysfs_attributes();
+
+    /* get chip name */
+    if (inv_get_chip_name(chip_ID) != INV_SUCCESS) {
+        LOGE("HAL:ERR- Failed to get chip ID\n");
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Chip ID= %s\n", chip_ID);
+    }
+
+    enable_iio_sysfs();
+    
+    /* instantiate pressure sensor on secondary bus */
+    if (strcmp(mSysfsPath, "") != 0) {
+        mPressureSensor = new PressureSensor((const char*)mSysfsPath);
+    } else {
+        LOGE("HAL:ERR - Failed to instantiate pressure sensor class");
+    }
+
+    /* reset driver master enable */
+    masterEnable(0);
+
+    //Always load DMP for KLP
+    /* Load DMP image if capable, ie. MPU6xxx/9xxx */
+    loadDMP();
+
+    /* open temperature fd for temp comp */
+    LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature);
+    gyro_temperature_fd = open(mpu.temperature, O_RDONLY);
+    if (gyro_temperature_fd == -1) {
+        LOGE("HAL:could not open temperature node");
+    } else {
+        LOGV_IF(EXTRA_VERBOSE,
+                "HAL:temperature_fd opened: %s", mpu.temperature);
+    }
+
+    /* read gyro FSR to calculate accel scale later */
+    char gyroBuf[5];
+    int count = 0;
+         LOGV_IF(SYSFS_VERBOSE,
+             "HAL:sysfs:cat %s (%lld)", mpu.gyro_fsr, getTimestamp());
+
+    fd = open(mpu.gyro_fsr, O_RDONLY);
+    if(fd < 0) {
+        LOGE("HAL:Error opening gyro FSR");
+    } else {
+        memset(gyroBuf, 0, sizeof(gyroBuf));
+        count = read_attribute_sensor(fd, gyroBuf, sizeof(gyroBuf));
+        if(count < 1) {
+            LOGE("HAL:Error reading gyro FSR");
+        } else {
+            count = sscanf(gyroBuf, "%ld", &mGyroScale);
+            if(count)
+                LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro FSR used %ld", mGyroScale);
+        }
+        close(fd);
+    }
+    
+    /* read gyro self test scale used to calculate factory cal bias later */
+    char gyroScale[5]; 
+         LOGV_IF(SYSFS_VERBOSE,
+             "HAL:sysfs:cat %s (%lld)", mpu.in_gyro_self_test_scale, getTimestamp());
+    fd = open(mpu.in_gyro_self_test_scale, O_RDONLY);
+    if(fd < 0) {
+        LOGE("HAL:Error opening gyro self test scale");
+    } else {
+        memset(gyroBuf, 0, sizeof(gyroBuf));
+        count = read_attribute_sensor(fd, gyroScale, sizeof(gyroScale));
+        if(count < 1) {
+            LOGE("HAL:Error reading gyro self test scale");
+        } else {
+            count = sscanf(gyroScale, "%ld", &mGyroSelfTestScale);
+            if(count)
+                LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro self test scale used %ld", mGyroSelfTestScale);
+        }
+        close(fd);
+    }
+
+    /* open Factory Gyro Bias fd */
+    /* mFactoryGyBias contains bias values that will be used for device offset */
+    memset(mFactoryGyroBias, 0, sizeof(mFactoryGyroBias));
+    LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro x offset path: %s", mpu.in_gyro_x_offset);
+    LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro y offset path: %s", mpu.in_gyro_y_offset);
+    LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro z offset path: %s", mpu.in_gyro_z_offset);   
+    gyro_x_offset_fd = open(mpu.in_gyro_x_offset, O_RDWR);
+    gyro_y_offset_fd = open(mpu.in_gyro_y_offset, O_RDWR);
+    gyro_z_offset_fd = open(mpu.in_gyro_z_offset, O_RDWR);
+    if (gyro_x_offset_fd == -1 ||
+             gyro_y_offset_fd == -1 || gyro_z_offset_fd == -1) {
+            LOGE("HAL:could not open factory gyro calibrated bias");
+    } else {
+        LOGV_IF(EXTRA_VERBOSE,
+             "HAL:gyro_offset opened");
+    }
+
+    /* open Gyro Bias fd */
+    /* mGyroBias contains bias values that will be used for framework */
+    /* mGyroChipBias contains bias values that will be used for dmp */
+    memset(mGyroBias, 0, sizeof(mGyroBias));
+    memset(mGyroChipBias, 0, sizeof(mGyroChipBias));
+    LOGV_IF(EXTRA_VERBOSE, "HAL: gyro x dmp bias path: %s", mpu.in_gyro_x_dmp_bias);
+    LOGV_IF(EXTRA_VERBOSE, "HAL: gyro y dmp bias path: %s", mpu.in_gyro_y_dmp_bias);
+    LOGV_IF(EXTRA_VERBOSE, "HAL: gyro z dmp bias path: %s", mpu.in_gyro_z_dmp_bias);
+    gyro_x_dmp_bias_fd = open(mpu.in_gyro_x_dmp_bias, O_RDWR);
+    gyro_y_dmp_bias_fd = open(mpu.in_gyro_y_dmp_bias, O_RDWR);
+    gyro_z_dmp_bias_fd = open(mpu.in_gyro_z_dmp_bias, O_RDWR);
+    if (gyro_x_dmp_bias_fd == -1 ||
+             gyro_y_dmp_bias_fd == -1 || gyro_z_dmp_bias_fd == -1) {
+            LOGE("HAL:could not open gyro DMP calibrated bias");
+    } else {
+        LOGV_IF(EXTRA_VERBOSE,
+             "HAL:gyro_dmp_bias opened");
+    }
+
+    /* read accel FSR to calcuate accel scale later */
+    if (USE_THIRD_PARTY_ACCEL == 0) {
+        char buf[3];
+        int count = 0;
+        LOGV_IF(SYSFS_VERBOSE,
+                "HAL:sysfs:cat %s (%lld)", mpu.accel_fsr, getTimestamp());
+
+        fd = open(mpu.accel_fsr, O_RDONLY);
+        if(fd < 0) {
+            LOGE("HAL:Error opening accel FSR");
+        } else {
+           memset(buf, 0, sizeof(buf));
+           count = read_attribute_sensor(fd, buf, sizeof(buf));
+           if(count < 1) {
+               LOGE("HAL:Error reading accel FSR");
+           } else {
+               count = sscanf(buf, "%d", &mAccelScale);
+               if(count)
+                   LOGV_IF(EXTRA_VERBOSE, "HAL:Accel FSR used %d", mAccelScale);
+           }
+           close(fd);
+        }
+
+        /* open Accel Bias fd */
+        /* mAccelBias contains bias that will be used for dmp */
+        memset(mAccelBias, 0, sizeof(mAccelBias));
+        LOGV_IF(EXTRA_VERBOSE, "HAL:accel x dmp bias path: %s", mpu.in_accel_x_dmp_bias);
+        LOGV_IF(EXTRA_VERBOSE, "HAL:accel y dmp bias path: %s", mpu.in_accel_y_dmp_bias);
+        LOGV_IF(EXTRA_VERBOSE, "HAL:accel z dmp bias path: %s", mpu.in_accel_z_dmp_bias);
+        accel_x_dmp_bias_fd = open(mpu.in_accel_x_dmp_bias, O_RDWR);
+        accel_y_dmp_bias_fd = open(mpu.in_accel_y_dmp_bias, O_RDWR);
+        accel_z_dmp_bias_fd = open(mpu.in_accel_z_dmp_bias, O_RDWR);
+        if (accel_x_dmp_bias_fd == -1 ||
+                 accel_y_dmp_bias_fd == -1 || accel_z_dmp_bias_fd == -1) {
+            LOGE("HAL:could not open accel DMP calibrated bias");
+        } else {
+            LOGV_IF(EXTRA_VERBOSE,
+                 "HAL:accel_dmp_bias opened");
+        }
+    }
+
+    dmp_sign_motion_fd = open(mpu.event_smd, O_RDONLY | O_NONBLOCK);
+    if (dmp_sign_motion_fd < 0) {
+        LOGE("HAL:ERR couldn't open dmp_sign_motion node");
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:dmp_sign_motion_fd opened : %d", dmp_sign_motion_fd);
+    }
+
+    dmp_pedometer_fd = open(mpu.event_pedometer, O_RDONLY | O_NONBLOCK);
+    if (dmp_pedometer_fd < 0) {
+        LOGE("HAL:ERR couldn't open dmp_pedometer node");
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:dmp_pedometer_fd opened : %d", dmp_pedometer_fd);
+    }
+
+    initBias();
+
+    (void)inv_get_version(&ver_str);
+    LOGV_IF(PROCESS_VERBOSE, "%s\n", ver_str);
+
+    /* setup MPL */
+    inv_constructor_init();
+
+#ifdef INV_PLAYBACK_DBG
+    LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging");
+    logfile = fopen("/data/playback.bin", "w+");
+    if (logfile)
+        inv_turn_on_data_logging(logfile);
+#endif
+
+    /* setup orientation matrix and scale */
+    inv_set_device_properties();
+
+    /* initialize sensor data */
+    memset(mPendingEvents, 0, sizeof(mPendingEvents));
+
+    mPendingEvents[RotationVector].version = sizeof(sensors_event_t);
+    mPendingEvents[RotationVector].sensor = ID_RV;
+    mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR;
+    mPendingEvents[RotationVector].acceleration.status
+            = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[GameRotationVector].version = sizeof(sensors_event_t);
+    mPendingEvents[GameRotationVector].sensor = ID_GRV;
+    mPendingEvents[GameRotationVector].type = SENSOR_TYPE_GAME_ROTATION_VECTOR;
+    mPendingEvents[GameRotationVector].acceleration.status
+            = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[LinearAccel].version = sizeof(sensors_event_t);
+    mPendingEvents[LinearAccel].sensor = ID_LA;
+    mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
+    mPendingEvents[LinearAccel].acceleration.status
+            = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[Gravity].version = sizeof(sensors_event_t);
+    mPendingEvents[Gravity].sensor = ID_GR;
+    mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
+    mPendingEvents[Gravity].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[Gyro].version = sizeof(sensors_event_t);
+    mPendingEvents[Gyro].sensor = ID_GY;
+    mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
+    mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[RawGyro].version = sizeof(sensors_event_t);
+    mPendingEvents[RawGyro].sensor = ID_RG;
+    mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED;
+    mPendingEvents[RawGyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
+    mPendingEvents[Accelerometer].sensor = ID_A;
+    mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
+    mPendingEvents[Accelerometer].acceleration.status
+            = SENSOR_STATUS_ACCURACY_HIGH;
+
+    /* Invensense compass calibration */
+    mPendingEvents[MagneticField].version = sizeof(sensors_event_t);
+    mPendingEvents[MagneticField].sensor = ID_M;
+    mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD;
+    mPendingEvents[MagneticField].magnetic.status =
+        SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[RawMagneticField].version = sizeof(sensors_event_t);
+    mPendingEvents[RawMagneticField].sensor = ID_RM;
+    mPendingEvents[RawMagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED;
+    mPendingEvents[RawMagneticField].magnetic.status =
+        SENSOR_STATUS_ACCURACY_HIGH;
+        
+    mPendingEvents[Pressure].version = sizeof(sensors_event_t);
+    mPendingEvents[Pressure].sensor = ID_PS;
+    mPendingEvents[Pressure].type = SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED;
+    mPendingEvents[Pressure].magnetic.status =
+        SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[Orientation].version = sizeof(sensors_event_t);
+    mPendingEvents[Orientation].sensor = ID_O;
+    mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION;
+    mPendingEvents[Orientation].orientation.status
+            = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[GeomagneticRotationVector].version = sizeof(sensors_event_t);
+    mPendingEvents[GeomagneticRotationVector].sensor = ID_GMRV;
+    mPendingEvents[GeomagneticRotationVector].type
+            = SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR;
+    mPendingEvents[GeomagneticRotationVector].acceleration.status
+            = SENSOR_STATUS_ACCURACY_HIGH;
+
+#ifndef KLP
+    mHandlers[RotationVector] = &MPLSensor::rvHandler;
+#else
+    mHandlers[RotationVector] = &MPLSensor::grvHandler;
+#endif
+    mHandlers[GameRotationVector] = &MPLSensor::grvHandler;
+    mHandlers[LinearAccel] = &MPLSensor::laHandler;
+    mHandlers[Gravity] = &MPLSensor::gravHandler;
+#ifndef KLP
+    mHandlers[Gyro] = &MPLSensor::gyroHandler;
+#else
+    mHandlers[Gyro] = &MPLSensor::rawGyroHandler;
+#endif
+    mHandlers[RawGyro] = &MPLSensor::rawGyroHandler;
+    mHandlers[Accelerometer] = &MPLSensor::accelHandler;
+#ifndef KLP
+    mHandlers[MagneticField] = &MPLSensor::compassHandler;
+#else
+    mHandlers[MagneticField] = &MPLSensor::rawCompassHandler;
+#endif
+    mHandlers[RawMagneticField] = &MPLSensor::rawCompassHandler;
+    mHandlers[Orientation] = &MPLSensor::orienHandler;
+    mHandlers[GeomagneticRotationVector] = &MPLSensor::gmHandler;
+    mHandlers[Pressure] = &MPLSensor::psHandler;
+
+    for (int i = 0; i < NumSensors; i++) {
+        mDelays[i] = 1000000000LL;
+        mBatchDelays[i] = 1000000000LL;
+        mBatchTimeouts[i] = 30000000000LL;
+    }
+
+    /* initialize Compass Bias */
+    memset(mCompassBias, 0, sizeof(mCompassBias));
+
+    /* initialize Factory Accel Bias */
+    memset(mFactoryAccelBias, 0, sizeof(mFactoryAccelBias));
+
+    /* initialize Gyro Bias */
+    memset(mGyroBias, 0, sizeof(mGyroBias));
+    memset(mGyroChipBias, 0, sizeof(mGyroChipBias));    
+
+    /* load calibration file from /data/inv_cal_data.bin */
+    rv = inv_load_calibration();    
+    if(rv == INV_SUCCESS) {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded");        
+        /* Get initial values */
+        getCompassBias();
+        getGyroBias();
+        getAccelBias();
+        getFactoryGyroBias();
+        if (mFactoryGyroBiasAvailable) {
+            setFactoryGyroBias();
+        }
+        /* disabled because no request for factory cal accel data yet */
+        //getFactoryAccelBias();
+    }
+    else
+        LOGE("HAL:Could not open or load MPL calibration file (%d)", rv);
+
+    /* takes external accel calibration load workflow */
+    if( m_pt2AccelCalLoadFunc != NULL) {
+        long accel_offset[3];
+        long tmp_offset[3];
+        int result = m_pt2AccelCalLoadFunc(accel_offset);
+        if(result)
+            LOGW("HAL:Vendor accelerometer calibration file load failed %d\n",
+                 result);
+        else {
+            LOGW("HAL:Vendor accelerometer calibration file successfully "
+                 "loaded");
+            inv_get_accel_bias(tmp_offset, NULL);
+            LOGV_IF(PROCESS_VERBOSE,
+                    "HAL:Original accel offset, %ld, %ld, %ld\n",
+               tmp_offset[0], tmp_offset[1], tmp_offset[2]);
+            inv_set_accel_bias(accel_offset, mAccelAccuracy);
+            inv_get_accel_bias(tmp_offset, NULL);
+            LOGV_IF(PROCESS_VERBOSE, "HAL:Set accel offset, %ld, %ld, %ld\n",
+               tmp_offset[0], tmp_offset[1], tmp_offset[2]);
+        }
+    }
+    /* end of external accel calibration load workflow */   
+
+    /* disable driver master enable the first sensor goes on */
+    masterEnable(0);
+    enableGyro(0);
+    enableAccel(0);
+    enableCompass(0,0);
+    enablePressure(0);
+    enableBatch(0);
+
+    if (isLowPowerQuatEnabled()) {
+        enableLPQuaternion(0);
+    }
+
+    if (isDmpDisplayOrientationOn()) {
+        // open DMP Orient Fd
+        openDmpOrientFd();
+        enableDmpOrientation(!isDmpScreenAutoRotationEnabled());
+    }
+}
+
+void MPLSensor::enable_iio_sysfs(void)
+{
+    VFUNC_LOG;
+
+    char iio_device_node[MAX_CHIP_ID_LEN];
+    FILE *tempFp = NULL;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)",
+            mpu.in_timestamp_en, getTimestamp());
+    // Either fopen()/open() are okay for sysfs access
+    // developers could choose what they want
+    // with fopen(), the benefit is that fprintf()/fscanf() are available
+    tempFp = fopen(mpu.in_timestamp_en, "w");
+    if (tempFp == NULL) {
+        LOGE("HAL:could not open timestamp enable");
+    } else {
+        if(fprintf(tempFp, "%d", 1) < 0 || fclose(tempFp) < 0) {
+            LOGE("HAL:could not enable timestamp");
+        }
+    }
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            IIO_BUFFER_LENGTH, mpu.buffer_length, getTimestamp());
+    tempFp = fopen(mpu.buffer_length, "w");
+    if (tempFp == NULL) {
+        LOGE("HAL:could not open buffer length");
+    } else {
+        if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0 || fclose(tempFp) < 0) {
+            LOGE("HAL:could not write buffer length");
+        }
+    }
+
+    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(void)
+{
+    VFUNC_LOG;
+
+    inv_error_t result = inv_init_mpl();
+    if (result) {
+        LOGE("HAL:inv_init_mpl() failed");
+        return result;
+    }
+    result = inv_constructor_default_enable();
+    result = inv_start_mpl();
+    if (result) {
+        LOGE("HAL:inv_start_mpl() failed");
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return result;
+}
+
+int MPLSensor::inv_constructor_default_enable(void)
+{
+    VFUNC_LOG;
+
+    inv_error_t result;
+
+/*******************************************************************************
+
+********************************************************************************
+
+The InvenSense binary file (libmplmpu.so) is subject to Google's standard terms
+and conditions as accepted in the click-through agreement required to download
+this library.
+The library includes, but is not limited to the following function calls:
+inv_enable_quaternion().
+
+ANY VIOLATION OF SUCH TERMS AND CONDITIONS WILL BE STRICTLY ENFORCED.
+
+********************************************************************************
+
+*******************************************************************************/
+
+    result = inv_enable_quaternion();
+    if (result) {
+        LOGE("HAL:Cannot enable quaternion\n");
+        return result;
+    }
+
+    result = inv_enable_in_use_auto_calibration();
+    if (result) {
+        return result;
+    }
+
+    result = inv_enable_fast_nomot();
+    if (result) {
+        return result;
+    }
+
+    result = inv_enable_gyro_tc();
+    if (result) {
+        return result;
+    }
+
+    result = inv_enable_hal_outputs();
+    if (result) {
+        return result;
+    }
+
+    if (!mCompassSensor->providesCalibration()) {
+        /* Invensense compass calibration */
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Invensense vector compass cal enabled");
+        result = inv_enable_vector_compass_cal();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        } else {
+            mMplFeatureActiveMask |= INV_COMPASS_CAL;
+        }
+        // specify MPL's trust weight, used by compass algorithms
+        inv_vector_compass_cal_sensitivity(3);
+
+        /* disabled by default
+        result = inv_enable_compass_bias_w_gyro();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        */
+
+        result = inv_enable_heading_from_gyro();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        result = inv_enable_magnetic_disturbance();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        //inv_enable_magnetic_disturbance_logging();
+    }
+
+    result = inv_enable_9x_sensor_fusion();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    } else {
+        // 9x sensor fusion enables Compass fit
+        mMplFeatureActiveMask |= INV_COMPASS_FIT;
+    }
+
+    result = inv_enable_no_gyro_fusion();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return result;
+}
+
+/* TODO: create function pointers to calculate scale */
+void MPLSensor::inv_set_device_properties(void)
+{
+    VFUNC_LOG;
+
+    unsigned short orient;
+
+    inv_get_sensors_orientation();
+
+    inv_set_gyro_sample_rate(DEFAULT_MPL_GYRO_RATE);
+    inv_set_compass_sample_rate(DEFAULT_MPL_COMPASS_RATE);
+
+    /* gyro setup */
+    orient = inv_orientation_matrix_to_scalar(mGyroOrientation);
+    inv_set_gyro_orientation_and_scale(orient, mGyroScale << 15);
+    LOGI_IF(EXTRA_VERBOSE, "HAL: Set MPL Gyro Scale %ld", mGyroScale << 15);
+
+    /* accel setup */
+    orient = inv_orientation_matrix_to_scalar(mAccelOrientation);
+    /* use for third party accel input subsystem driver
+    inv_set_accel_orientation_and_scale(orient, 1LL << 22);
+    */
+    inv_set_accel_orientation_and_scale(orient, (long)mAccelScale << 15);
+    LOGI_IF(EXTRA_VERBOSE,
+            "HAL: Set MPL Accel Scale %ld", (long)mAccelScale << 15);
+
+    /* compass setup */
+    signed char orientMtx[9];
+    mCompassSensor->getOrientationMatrix(orientMtx);
+    orient =
+        inv_orientation_matrix_to_scalar(orientMtx);
+    long sensitivity;
+    sensitivity = mCompassSensor->getSensitivity();
+    inv_set_compass_orientation_and_scale(orient, sensitivity);
+    mCompassScale = sensitivity;
+    LOGI_IF(EXTRA_VERBOSE,
+            "HAL: Set MPL Compass Scale %ld", mCompassScale);
+}
+
+void MPLSensor::loadDMP(void)
+{
+    int res, fd;
+    FILE *fptr;
+
+    if (isMpuNonDmp()) {
+        //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 == NULL) {
+                LOGE("HAL:could not write to dmp");
+            } else {
+                if (inv_load_dmp(fptr) < 0 || fclose(fptr) < 0) {
+                    LOGE("HAL:load DMP failed");
+                } else {
+                    fclose(fptr);
+                    LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded");
+                };
+            }
+        } 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(void)
+{
+    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);
+
+    closeDmpOrientFd();
+
+    if (accel_x_dmp_bias_fd > 0) {
+        close(accel_x_dmp_bias_fd);
+    }
+    if (accel_y_dmp_bias_fd > 0) {
+        close(accel_y_dmp_bias_fd);
+    }
+    if (accel_z_dmp_bias_fd > 0) {
+        close(accel_z_dmp_bias_fd);
+    }
+
+    if (gyro_x_dmp_bias_fd > 0) {
+        close(gyro_x_dmp_bias_fd);
+    }
+    if (gyro_y_dmp_bias_fd > 0) {
+        close(gyro_y_dmp_bias_fd);
+    }
+    if (gyro_z_dmp_bias_fd > 0) {
+        close(gyro_z_dmp_bias_fd);
+    }
+
+    if (gyro_x_offset_fd > 0) {
+        close(gyro_x_dmp_bias_fd);
+    }
+    if (gyro_y_offset_fd > 0) {
+        close(gyro_y_offset_fd);
+    }
+    if (gyro_z_offset_fd > 0) {
+        close(accel_z_offset_fd);
+    }
+
+    /* Turn off Gyro master enable          */
+    /* A workaround until driver handles it */
+    /* TODO: Turn off and close all sensors */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            0, mpu.chip_enable, getTimestamp());
+    write_sysfs_int(mpu.chip_enable, 0);
+
+#ifdef INV_PLAYBACK_DBG
+    inv_turn_off_data_logging();
+    fclose(logfile);
+#endif
+}
+
+#define GY_ENABLED  ((1 << ID_GY) & enabled_sensors)
+#define RGY_ENABLED ((1 << ID_RG) & enabled_sensors)
+#define A_ENABLED   ((1 << ID_A)  & enabled_sensors)
+#define M_ENABLED   ((1 << ID_M) & enabled_sensors)
+#define RM_ENABLED  ((1 << ID_RM) & enabled_sensors)
+#define PS_ENABLED  ((1 << ID_PS) & enabled_sensors)
+#define O_ENABLED   ((1 << ID_O)  & enabled_sensors)
+#define LA_ENABLED  ((1 << ID_LA) & enabled_sensors)
+#define GR_ENABLED  ((1 << ID_GR) & enabled_sensors)
+#define RV_ENABLED  ((1 << ID_RV) & enabled_sensors)
+#define GRV_ENABLED ((1 << ID_GRV) & enabled_sensors)
+#define GMRV_ENABLED ((1 << ID_GMRV) & enabled_sensors)
+
+/* TODO: this step is optional, remove?  */
+int MPLSensor::setGyroInitialState(void)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            HW_GYRO_RATE_HZ, mpu.gyro_rate, getTimestamp());
+    int fd = open(mpu.gyro_rate, O_RDWR);
+    res = errno;
+    if(fd < 0) {
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             mpu.gyro_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_rate, HW_GYRO_RATE_HZ);
+        return res;
+    }
+
+    // Setting LPF is deprecated
+
+    return 0;
+}
+
+/* this applies to BMA250 Input Subsystem Driver only */
+int MPLSensor::setAccelInitialState()
+{
+    VFUNC_LOG;
+
+    struct input_absinfo absinfo_x;
+    struct input_absinfo absinfo_y;
+    struct input_absinfo absinfo_z;
+    float value;
+    if (!ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo_x) &&
+        !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo_y) &&
+        !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo_z)) {
+        value = absinfo_x.value;
+        mPendingEvents[Accelerometer].data[0] = value * CONVERT_A_X;
+        value = absinfo_y.value;
+        mPendingEvents[Accelerometer].data[1] = value * CONVERT_A_Y;
+        value = absinfo_z.value;
+        mPendingEvents[Accelerometer].data[2] = value * CONVERT_A_Z;
+        //mHasPendingEvent = true;
+    }
+    return 0;
+}
+
+int MPLSensor::onDmp(int en)
+{
+    VFUNC_LOG;
+
+    int res = -1;
+    int status;
+    mDmpOn = en;
+
+    //Sequence to enable DMP
+    //1. Load DMP image if not already loaded
+    //2. Either Gyro or Accel must be enabled/configured before next step
+    //3. Enable DMP
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
+            mpu.firmware_loaded, getTimestamp());
+    if(read_sysfs_int(mpu.firmware_loaded, &status) < 0){
+        LOGE("HAL:ERR can't get firmware_loaded status");
+    } else if (status == 1) {
+        //Write only if curr DMP state <> request
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
+                mpu.dmp_on, getTimestamp());
+        if (read_sysfs_int(mpu.dmp_on, &status) < 0) {
+            LOGE("HAL:ERR can't read DMP state");
+        } else if (status != en) {
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    en, mpu.dmp_on, getTimestamp());
+            if (write_sysfs_int(mpu.dmp_on, en) < 0) {
+                LOGE("HAL:ERR can't write dmp_on");
+            } else {
+                mDmpOn = en;
+                res = 0;	//Indicate write successful
+                if(!en) {
+                    setAccelBias();
+                }
+            }
+            //Enable DMP interrupt
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    en, mpu.dmp_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.dmp_int_on, en) < 0) {
+                LOGE("HAL:ERR can't en/dis DMP interrupt");
+            }
+
+            // disable DMP event interrupt if needed
+            if (!en) {
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        en, mpu.dmp_event_int_on, getTimestamp());
+                if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
+                    res = -1;
+                    LOGE("HAL:ERR can't enable DMP event interrupt");
+                }
+            }
+        } else {
+            mDmpOn = en;
+            res = 0;  	//DMP already set as requested
+            if(!en) {
+                setAccelBias();
+            }
+        }
+    } else {
+        LOGE("HAL:ERR No DMP image");
+    }
+    return res;
+}
+
+/* called when batch and hw sensor enabled*/
+int MPLSensor::enablePedIndicator(int en)
+{
+    VFUNC_LOG;
+    
+    int res = 0;
+    if (en) {
+        if (!(mFeatureActiveMask & INV_DMP_PED_QUATERNION)) { 
+            //Disable DMP Pedometer Interrupt
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        0, mpu.pedometer_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) {
+               LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
+               res = -1;   // indicate an err
+               return res;
+            }
+        }
+    }
+    
+    LOGV_IF(PROCESS_VERBOSE, "HAL:Toggling step indicator to %d", en);
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                en, mpu.step_indicator_on, getTimestamp());
+    if (write_sysfs_int(mpu.step_indicator_on, en) < 0) {
+        res = -1;
+        LOGE("HAL:ERR can't write to DMP step_indicator_on");
+    }
+    return res;
+}
+
+int MPLSensor::checkPedStandaloneEnabled(void)
+{
+    return ((mFeatureActiveMask & INV_DMP_PED_STANDALONE)? 1:0);
+
+}
+
+/* This feature is only used in batch mode */
+/* Stand-alone Step Detector */
+int MPLSensor::enablePedStandalone(int en)
+{
+    VFUNC_LOG;
+
+    if (!en) {
+        enablePedStandaloneData(0);
+        mFeatureActiveMask &= ~INV_DMP_PED_STANDALONE;
+        if (mFeatureActiveMask == 0) {
+            onDmp(0);
+        } else if(mFeatureActiveMask & INV_DMP_PEDOMETER) {
+             //Re-enable DMP Pedometer Interrupt
+             LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                     1, mpu.pedometer_int_on, getTimestamp());
+             if (write_sysfs_int(mpu.pedometer_int_on, 1) < 0) {
+                 LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
+                 return (-1);
+             }
+            //Disable data interrupt if no continuous data
+            if (mEnabled == 0) {
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                       1, mpu.dmp_event_int_on, getTimestamp());
+                if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) {
+                    LOGE("HAL:ERR can't enable DMP event interrupt");
+                    return (-1);
+                }
+            }
+        }
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Ped Standalone disabled");
+    } else {
+        if (enablePedStandaloneData(1) < 0 || onDmp(1) < 0) {
+            LOGE("HAL:ERR can't enable Ped Standalone");
+        } else {
+            mFeatureActiveMask |= INV_DMP_PED_STANDALONE;
+            //Disable DMP Pedometer Interrupt
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    0, mpu.pedometer_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) {
+                LOGE("HAL:ERR can't disable Android Pedometer Interrupt");
+                return (-1);
+            }
+            //Enable Data Interrupt
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                       0, mpu.dmp_event_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
+                LOGE("HAL:ERR can't enable DMP event interrupt");
+                return (-1);
+            }
+            LOGV_IF(PROCESS_VERBOSE, "HAL:Ped Standalone enabled");
+        }
+    }
+    return 0;
+}
+
+int MPLSensor:: enablePedStandaloneData(int en)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    // Enable DMP Ped standalone
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.step_detector_on, getTimestamp());
+    if (write_sysfs_int(mpu.step_detector_on, en) < 0) {
+        LOGE("HAL:ERR can't write DMP step_detector_on");
+        res = -1;   //Indicate an err
+    }
+
+    // Disable DMP Step indicator
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.step_indicator_on, getTimestamp());
+    if (write_sysfs_int(mpu.step_indicator_on, en) < 0) {
+        LOGE("HAL:ERR can't write DMP step_indicator_on");
+        res = -1;   //Indicate an err
+    }
+
+    if (!en) {
+      LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling ped standalone");
+      //Disable Accel if no sensor needs it
+      if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
+                               && (!(mLocalSensorMask & mMasterSensorMask
+                                                   & INV_THREE_AXIS_ACCEL))) {
+          res = enableAccel(0);
+          if (res < 0)
+              return res;
+      }
+      if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
+                               && (!(mLocalSensorMask & mMasterSensorMask
+                                                   & INV_THREE_AXIS_GYRO))) {
+          res = enableGyro(0);
+          if (res < 0)
+              return res;
+      }
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling ped standalone");
+        // enable accel engine
+        res = enableAccel(1);
+        if (res < 0)
+            return res;
+        LOGV_IF(EXTRA_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask);
+        // disable accel FIFO
+        if (!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_ACCEL)) {
+            res = turnOffAccelFifo();
+            if (res < 0)
+                return res;           
+        }
+    }
+
+    return res;
+}
+
+int MPLSensor::checkPedQuatEnabled(void)
+{
+    return ((mFeatureActiveMask & INV_DMP_PED_QUATERNION)? 1:0);
+}
+
+/* This feature is only used in batch mode */
+/* Step Detector && Game Rotation Vector */
+int MPLSensor::enablePedQuaternion(int en)
+{
+    VFUNC_LOG;
+
+    if (!en) {
+        enablePedQuaternionData(0);
+        mFeatureActiveMask &= ~INV_DMP_PED_QUATERNION;
+        if (mFeatureActiveMask == 0) {
+            onDmp(0);
+        } else if(mFeatureActiveMask & INV_DMP_PEDOMETER) {
+             //Re-enable DMP Pedometer Interrupt
+             LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                     1, mpu.pedometer_int_on, getTimestamp());
+             if (write_sysfs_int(mpu.pedometer_int_on, 1) < 0) {
+                 LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
+                 return (-1);
+             }
+            //Disable data interrupt if no continuous data
+            if (mEnabled == 0) {
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                       1, mpu.dmp_event_int_on, getTimestamp());
+                if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
+                    LOGE("HAL:ERR can't enable DMP event interrupt");
+                    return (-1);
+                }
+            }
+        }
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Ped Quat disabled");
+    } else {
+        if (enablePedQuaternionData(1) < 0 || onDmp(1) < 0) {
+            LOGE("HAL:ERR can't enable Ped Quaternion");
+        } else {
+            mFeatureActiveMask |= INV_DMP_PED_QUATERNION;
+            //Disable DMP Pedometer Interrupt
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    0, mpu.pedometer_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) {
+                LOGE("HAL:ERR can't disable Android Pedometer Interrupt");
+                return (-1);
+            }
+            //Enable Data Interrupt
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                       0, mpu.dmp_event_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
+                LOGE("HAL:ERR can't enable DMP event interrupt");
+                return (-1);
+            }
+            LOGV_IF(PROCESS_VERBOSE, "HAL:Ped Quat enabled");
+        }
+    }
+    return 0;
+}
+
+int MPLSensor::enablePedQuaternionData(int en)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    // Enable DMP quaternion
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.ped_q_on, getTimestamp());
+    if (write_sysfs_int(mpu.ped_q_on, en) < 0) {
+        LOGE("HAL:ERR can't write DMP ped_q_on");
+        res = -1;   //Indicate an err
+    }
+
+    // toggle DMP step indicator
+    /*LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.step_indicator_on, getTimestamp());
+    if (write_sysfs_int(mpu.step_indicator_on, en) < 0) {
+        LOGE("HAL:ERR can't write DMP step_indicator_on");
+        res = -1;   //Indicate an err
+    }*/
+
+    if (!en) {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling ped quat");
+        //Disable Accel if no sensor needs it
+        if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
+                               && (!(mLocalSensorMask & mMasterSensorMask
+                                                   & INV_THREE_AXIS_ACCEL))) {
+          res = enableAccel(0);
+          if (res < 0)
+              return res;
+        }
+        if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
+                               && (!(mLocalSensorMask & mMasterSensorMask
+                                                   & INV_THREE_AXIS_GYRO))) {
+            res = enableGyro(0);
+            if (res < 0)
+                return res;
+        }
+      if (mFeatureActiveMask & INV_DMP_QUATERNION) {
+            res = write_sysfs_int(mpu.gyro_fifo_enable, 1);
+            res += write_sysfs_int(mpu.accel_fifo_enable, 1);
+            if (res < 0)
+                return res;
+      }
+//LOGV("before mLocalSensorMask=0x%lx", mLocalSensorMask);
+      // reset global mask for buildMpuEvent()
+      if (mEnabled & (1 << GameRotationVector)) {
+          mLocalSensorMask |= INV_THREE_AXIS_GYRO;        
+          mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
+      } else if (mEnabled & (1 << Accelerometer)) {
+          mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
+      } else if ((mEnabled & ( 1 << Gyro)) ||
+          (mEnabled & (1 << RawGyro))) {
+          mLocalSensorMask |= INV_THREE_AXIS_GYRO; 
+      }
+//LOGV("after mLocalSensorMask=0x%lx", mLocalSensorMask); 
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling ped quat");
+        // enable accel engine
+        res = enableAccel(1);
+        if (res < 0)
+            return res;
+
+        // enable gyro engine
+        res = enableGyro(1);
+        if (res < 0)
+            return res;
+        LOGV_IF(EXTRA_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask);
+        // disable accel FIFO
+        if ((!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_ACCEL)) ||
+                !(mBatchEnabled & (1 << Accelerometer))) {
+            res = turnOffAccelFifo();
+            if (res < 0)
+                return res;
+            mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
+        }
+
+        // disable gyro FIFO
+        if ((!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_GYRO)) ||
+                !((mBatchEnabled & (1 << Gyro)) || (mBatchEnabled & (1 << RawGyro)))) {
+            res = turnOffGyroFifo();
+            if (res < 0)
+                return res;
+            mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
+        }
+    }
+
+    return res;
+}
+
+int MPLSensor::check6AxisQuatEnabled(void)
+{
+    return ((mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION)? 1:0);
+}
+
+/* This is used for batch mode only */
+/* GRV is batched but not along with ped */
+int MPLSensor::enable6AxisQuaternion(int en)
+{
+    VFUNC_LOG;
+
+    if (!en) {
+        enable6AxisQuaternionData(0);
+        mFeatureActiveMask &= ~INV_DMP_6AXIS_QUATERNION;
+        if (mFeatureActiveMask == 0) {
+            onDmp(0);
+        }
+        LOGV_IF(PROCESS_VERBOSE, "HAL:6 Axis Quat disabled");
+    } else {
+        if (enable6AxisQuaternionData(1) < 0 || onDmp(1) < 0) {
+            LOGE("HAL:ERR can't enable 6 Axis Quaternion");
+        } else {
+            mFeatureActiveMask |= INV_DMP_6AXIS_QUATERNION;
+            LOGV_IF(PROCESS_VERBOSE, "HAL:6 Axis Quat enabled");
+        }
+    }
+    return 0;
+}
+
+int MPLSensor::enable6AxisQuaternionData(int en)
+{
+    int res = 0;
+    VFUNC_LOG;
+
+    // Enable DMP quaternion
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.six_axis_q_on, getTimestamp());
+    if (write_sysfs_int(mpu.six_axis_q_on, en) < 0) {
+        LOGE("HAL:ERR can't write DMP six_axis_q_on");
+        res = -1;   //Indicate an err
+    }
+
+    if (!en) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:DMP six axis quaternion data was turned off");
+        if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
+                                 && (!(mLocalSensorMask & mMasterSensorMask
+                                                     & INV_THREE_AXIS_ACCEL))) {
+            res = enableAccel(0);
+            if (res < 0)
+                return res;
+        }
+        if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
+                                 && (!(mLocalSensorMask & mMasterSensorMask
+                                                     & INV_THREE_AXIS_GYRO))) {
+            res = enableGyro(0);
+            if (res < 0)
+                return res;
+        }
+        if (mFeatureActiveMask & INV_DMP_QUATERNION) {
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    1, mpu.gyro_fifo_enable, getTimestamp());
+            res = write_sysfs_int(mpu.gyro_fifo_enable, 1);
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    1, mpu.accel_fifo_enable, getTimestamp());
+            res += write_sysfs_int(mpu.accel_fifo_enable, 1);
+            if (res < 0)
+                return res;
+        }
+LOGV("before mLocalSensorMask=0x%lx", mLocalSensorMask);
+        // reset global mask for buildMpuEvent()
+        if (mEnabled & (1 << GameRotationVector)) {
+            if (!(mFeatureActiveMask & INV_DMP_PED_QUATERNION)) {
+                mLocalSensorMask |= INV_THREE_AXIS_GYRO;        
+                mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
+            }
+        } else if (mEnabled & (1 << Accelerometer)) {
+            mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
+        } else if ((mEnabled & ( 1 << Gyro)) ||
+                (mEnabled & (1 << RawGyro))) {
+            mLocalSensorMask |= INV_THREE_AXIS_GYRO; 
+        }
+LOGV("after mLocalSensorMask=0x%lx", mLocalSensorMask);        
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling six axis quat");
+        if (mEnabled & ( 1 << GameRotationVector)) {
+            // enable accel engine
+            res = enableAccel(1);
+            if (res < 0)
+                return res;
+
+            // enable gyro engine
+            res = enableGyro(1);
+            if (res < 0)
+                return res;
+            LOGV_IF(EXTRA_VERBOSE, "before: mLocalSensorMask=0x%lx", mLocalSensorMask);
+            if ((!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) ||
+                   (!(mBatchEnabled & (1 << Accelerometer)) ||
+                       (!(mEnabled & (1 << Accelerometer))))) {
+                res = turnOffAccelFifo();
+                if (res < 0)
+                    return res;
+                mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
+            }
+            
+            if ((!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_GYRO)) ||
+                    (!(mBatchEnabled & (1 << Gyro)) ||
+                       (!(mEnabled & (1 << Gyro))))) {
+                if (!(mBatchEnabled & (1 << RawGyro)) ||
+                        (!(mEnabled & (1 << RawGyro)))) {
+                    res = turnOffGyroFifo();
+                    if (res < 0)
+                        return res;
+                     mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
+                     }
+            }
+LOGV("after: mLocalSensorMask=0x%lx", mLocalSensorMask);
+        }
+    }
+
+    return res;
+}
+
+int MPLSensor::checkLPQuaternion(void)
+{
+    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
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.three_axis_q_on, getTimestamp());
+    if (write_sysfs_int(mpu.three_axis_q_on, en) < 0) {
+        LOGE("HAL:ERR can't write DMP three_axis_q__on");
+        res = -1;	//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 three axis quat");
+    }
+
+    return res;
+}
+
+int MPLSensor::enableDmpPedometer(int en, int interruptMode)
+{
+    VFUNC_LOG;
+    int res = 0;
+    int enabled_sensors = mEnabled;
+
+    if (isMpuNonDmp())
+        return res;
+
+    // reset master enable
+    res = masterEnable(0);
+    if (res < 0) {
+        return res;
+    }
+
+    if (en == 1) {
+        //Enable DMP Pedometer Function
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                en, mpu.pedometer_on, getTimestamp());
+        if (write_sysfs_int(mpu.pedometer_on, en) < 0) {
+            LOGE("HAL:ERR can't enable Android Pedometer");
+            res = -1;   // indicate an err
+            return res;
+        }
+
+        if (interruptMode || (mFeatureActiveMask & INV_DMP_PEDOMETER)) {
+            //Enable DMP Pedometer Interrupt
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    en, mpu.pedometer_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.pedometer_int_on, en) < 0) {
+                LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
+                res = -1;   // indicate an err
+                return res;
+            }
+        }
+        // enable DMP
+        res = onDmp(1);
+        if (res < 0) {
+            return res;
+        }  
+
+        // set DMP rate to 200Hz
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                200, mpu.accel_fifo_rate, getTimestamp());
+        if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) {
+            res = -1;
+            LOGE("HAL:ERR can't set rate to 200Hz");
+            return res;
+        }
+
+        // enable accel engine
+        res = enableAccel(1);
+        if (res < 0) {
+            return res;
+        }
+
+        // disable accel FIFO
+        if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) {
+            res = turnOffAccelFifo();
+            if (res < 0)
+                return res;
+        }
+        
+        // disable data interrupt      
+        //if (!batchPed && enabled_sensors == 0) {
+        if (enabled_sensors == 0) {
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        en, mpu.dmp_event_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
+                res = -1;
+                LOGE("HAL:ERR can't enable DMP event interrupt");
+            }
+        }
+        if (interruptMode) {
+            mFeatureActiveMask |= INV_DMP_PEDOMETER;
+            /*if (batchPed) {
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    en, mpu.step_detector_on, getTimestamp());
+                if (write_sysfs_int(mpu.step_detector_on, en) < 0) {
+                    LOGE("HAL:ERR can't write DMP step_detector_on");
+                    res = -1;   //Indicate an err
+                }
+                mFeatureActiveMask |= INV_DMP_PED_STANDALONE;
+            }*/
+        }
+        else {
+            mFeatureActiveMask |= INV_DMP_PEDOMETER_STEP;
+        }
+
+        clock_gettime(CLOCK_MONOTONIC, &mt_pre);
+    } else {
+        if (interruptMode) {
+            mFeatureActiveMask &= ~INV_DMP_PEDOMETER;
+        }
+        else {
+            mFeatureActiveMask &= ~INV_DMP_PEDOMETER_STEP;
+        }
+
+        /* if neither step detector or step count is on */
+        if (!(mFeatureActiveMask & (INV_DMP_PEDOMETER | INV_DMP_PEDOMETER_STEP))) {
+            //Disable DMP Pedometer Function
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    en, mpu.pedometer_on, getTimestamp());
+            if (write_sysfs_int(mpu.pedometer_on, en) < 0) {
+                LOGE("HAL:ERR can't enable Android Pedometer");
+                res = -1;   // indicate an err
+                return res;
+            }        
+        }       
+
+        if (mFeatureActiveMask == 0 ) {
+            // disable DMP
+            res = onDmp(0);
+            if (res < 0) {
+                return res;
+            }
+
+             // disable accel engine
+             if (!(mLocalSensorMask & mMasterSensorMask
+                        & INV_THREE_AXIS_ACCEL)) {
+                res = enableAccel(0);
+                if (res < 0) {
+                    return res;
+                }
+            }
+        }
+
+        /* if feature is not step detector */
+        if (!(mFeatureActiveMask & INV_DMP_PEDOMETER)) { 
+            //Disable DMP Pedometer Interrupt
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    en, mpu.pedometer_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.pedometer_int_on, en) < 0) {
+                LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
+                res = -1;   // indicate an err
+                return res;
+            }
+        }
+
+        //enable data interrupts if applicable
+        if (enabled_sensors) {
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        en, mpu.dmp_event_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
+                res = -1;
+                LOGE("HAL:ERR can't enable DMP event interrupt");
+            }
+        }
+    }
+    
+    if(en || enabled_sensors || mFeatureActiveMask) {
+        res = masterEnable(1);
+    }
+    return res;
+}
+
+int MPLSensor::masterEnable(int en)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.chip_enable, getTimestamp());
+    res = write_sysfs_int(mpu.chip_enable, en);
+    return res;
+}
+
+int MPLSensor::enableGyro(int en)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    /* need to also turn on/off the master enable */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.gyro_enable, getTimestamp());
+    res = write_sysfs_int(mpu.gyro_enable, en);
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.gyro_fifo_enable, getTimestamp());
+    res += write_sysfs_int(mpu.gyro_fifo_enable, en);
+
+    if (!en) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off");
+        inv_gyro_was_turned_off();
+    }
+
+    return res;
+}
+
+int MPLSensor::enableAccel(int en)
+{
+    VFUNC_LOG;
+
+    int res;
+
+    /* need to also turn on/off the master enable */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.accel_enable, getTimestamp());
+    res = write_sysfs_int(mpu.accel_enable, en);
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.accel_fifo_enable, getTimestamp());
+    res += write_sysfs_int(mpu.accel_fifo_enable, en);
+
+    if (!en) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off");
+        inv_accel_was_turned_off();
+    }
+
+    return res;
+}
+
+int MPLSensor::enableCompass(int en, int rawSensorRequested)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+    /* handle ID_RM if third party compass cal is used */
+    if (rawSensorRequested && mCompassSensor->providesCalibration()) {
+        res = mCompassSensor->enable(ID_RM, en);
+    } else {
+        res = mCompassSensor->enable(ID_M, en);
+    }
+    if (en == 0 || res != 0) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off %d", res);
+        inv_compass_was_turned_off();
+    }
+
+    return res;
+}
+
+int MPLSensor::enablePressure(int en)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    if (mPressureSensor) {
+        res = mPressureSensor->enable(ID_PS, en);
+    } else {
+        LOGV_IF(ENG_VERBOSE, "HAL:PRESSURE sensor not detected");
+    }
+    
+    return res;
+}
+
+/* use this function for initialization */
+int MPLSensor::enableBatch(int64_t timeout)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    res = write_sysfs_int(mpu.batchmode_timeout, timeout);
+    if (timeout == 0) {
+        res = write_sysfs_int(mpu.six_axis_q_on, 0);
+        res = write_sysfs_int(mpu.ped_q_on, 0);
+        res = write_sysfs_int(mpu.step_detector_on, 0);
+        res = write_sysfs_int(mpu.step_indicator_on, 0);
+    }
+
+    if (timeout == 0) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:batchmode timeout is zero");      
+    }
+
+    return res;
+}
+
+void MPLSensor::computeLocalSensorMask(int enabled_sensors)
+{
+    VFUNC_LOG;
+
+    do {
+        /* Invensense Pressure on secondary bus */
+        if (PS_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "PS ENABLED");
+            mLocalSensorMask |= INV_ONE_AXIS_PRESSURE;
+        } else {
+            LOGV_IF(ENG_VERBOSE, "PS DISABLED");
+            mLocalSensorMask &= ~INV_ONE_AXIS_PRESSURE;
+        }
+        
+        if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED
+                       || (GRV_ENABLED && GMRV_ENABLED)) {
+            LOGV_IF(ENG_VERBOSE, "FUSION ENABLED");
+            mLocalSensorMask = ALL_MPL_SENSORS_NP;
+            break;
+        }
+
+        if (GRV_ENABLED) {
+            if (!(mBatchEnabled & (1 << GameRotationVector))) {
+                LOGV_IF(ENG_VERBOSE, "6 Axis Fusion ENABLED");
+                mLocalSensorMask |= INV_THREE_AXIS_GYRO;
+                mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
+            } else {
+                if (GY_ENABLED || RGY_ENABLED) {
+                    LOGV_IF(ENG_VERBOSE, "G ENABLED");
+                    mLocalSensorMask |= INV_THREE_AXIS_GYRO;
+                } else {
+                    LOGV_IF(ENG_VERBOSE, "G DISABLED");
+                    mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
+                }
+                if (A_ENABLED) {
+                    LOGV_IF(ENG_VERBOSE, "A ENABLED");
+                    mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
+                } else {
+                    LOGV_IF(ENG_VERBOSE, "A DISABLED");
+                    mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
+                }
+            }
+            /* takes care of MAG case */
+            if (M_ENABLED || RM_ENABLED) {
+                LOGV_IF(1, "M ENABLED");
+                mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
+            } else {
+                LOGV_IF(1, "M DISABLED");
+                mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS;
+            }
+            break;
+        }
+
+        if (GMRV_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "6 Axis Geomagnetic Fusion ENABLED");
+            mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
+            mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
+
+            /* takes care of Gyro case */
+            if (GY_ENABLED || RGY_ENABLED) {
+                LOGV_IF(1, "G ENABLED");
+                mLocalSensorMask |= INV_THREE_AXIS_GYRO;
+            } else {
+                LOGV_IF(1, "G DISABLED");
+                mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
+            }
+            break;
+        }
+
+        if(!A_ENABLED && !M_ENABLED && !RM_ENABLED &&
+               !GRV_ENABLED && !GMRV_ENABLED && !GY_ENABLED && !RGY_ENABLED &&
+               !PS_ENABLED) {
+            /* Invensense compass cal */
+            LOGV_IF(ENG_VERBOSE, "ALL DISABLED");
+            mLocalSensorMask = 0;
+            break;
+        }
+
+        if (GY_ENABLED || RGY_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "G ENABLED");
+            mLocalSensorMask |= INV_THREE_AXIS_GYRO;
+        } else {
+            LOGV_IF(ENG_VERBOSE, "G DISABLED");
+            mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
+        }
+
+        if (A_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "A ENABLED");
+            mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
+        } else {
+            LOGV_IF(ENG_VERBOSE, "A DISABLED");
+            mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
+        }
+
+        /* Invensense compass calibration */
+        if (M_ENABLED || RM_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "M ENABLED");
+            mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
+        } else {
+            LOGV_IF(ENG_VERBOSE, "M DISABLED");
+            mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS;
+        }        
+    } while (0);
+}
+
+int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed)
+{
+    VFUNC_LOG;
+
+    inv_error_t res = -1;
+    int on = 1;
+    int off = 0;
+    int cal_stored = 0;
+
+    // Sequence to enable or disable a sensor
+    // 1. reset master enable (=0)
+    // 2. enable or disable a sensor
+    // 3. set master enable (=1)
+
+    if (isLowPowerQuatEnabled() ||
+        changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
+        (mCompassSensor->isIntegrated() << MagneticField) |
+        (mCompassSensor->isIntegrated() << RawMagneticField) |
+        (mPressureSensor->isIntegrated() << Pressure))) {
+
+        /* reset master enable */
+        res = masterEnable(0);
+        if(res < 0) {
+            return res;
+        }
+    }
+
+    LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x",
+            (unsigned int)sensors);
+
+    if (changed & ((1 << Gyro) | (1 << RawGyro))) {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - gyro %s",
+            (sensors & INV_THREE_AXIS_GYRO? "enable": "disable"));
+        res = enableGyro(!!(sensors & INV_THREE_AXIS_GYRO));
+        if(res < 0) {
+            return res;
+        }
+
+        if (!cal_stored && (!en && (changed & (1 << Gyro)))) {
+            storeCalibration();
+            cal_stored = 1;
+        }
+    }
+
+    if (changed & (1 << Accelerometer)) {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - accel %s",
+            (sensors & INV_THREE_AXIS_ACCEL? "enable": "disable"));
+        res = enableAccel(!!(sensors & INV_THREE_AXIS_ACCEL));
+        if(res < 0) {
+            return res;
+        }
+
+        if (!(sensors & INV_THREE_AXIS_ACCEL) && !cal_stored) {
+            storeCalibration();
+            cal_stored = 1;
+        }
+    }
+
+    if (changed & ((1 << MagneticField) | (1 << RawMagneticField))) {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - compass %s",
+            (sensors & INV_THREE_AXIS_COMPASS? "enable": "disable"));
+        res = enableCompass(!!(sensors & INV_THREE_AXIS_COMPASS), changed & (1 << RawMagneticField));
+        if(res < 0) {
+            return res;
+        }
+
+        if (!cal_stored && (!en && (changed & (1 << MagneticField)))) {
+            storeCalibration();
+            cal_stored = 1;
+        }
+    }
+    
+     if (changed & (1 << Pressure)) {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - pressure %s",
+            (sensors & INV_ONE_AXIS_PRESSURE? "enable": "disable"));
+        res = enablePressure(!!(sensors & INV_ONE_AXIS_PRESSURE));
+        if(res < 0) {
+            return res;
+        }
+    }
+
+    if (isLowPowerQuatEnabled()) {
+        // Enable LP Quat
+        if ((mEnabled & VIRTUAL_SENSOR_9AXES_MASK)
+                      || (mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK)) {
+  LOGI("HAL: 9 axis or game rot enabled");
+            if (!(changed & ((1 << Gyro)
+                           | (1 << RawGyro)
+                           | (1 << Accelerometer)
+                           | (mCompassSensor->isIntegrated() << MagneticField)
+                           | (mCompassSensor->isIntegrated() << RawMagneticField)))
+            ) {
+                /* reset master enable */
+                res = masterEnable(0);
+                if(res < 0) {
+                    return res;
+                }
+            }
+            if (!checkLPQuaternion()) {
+                enableLPQuaternion(1);
+            } else {
+                LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled");
+            }
+        } else if (checkLPQuaternion()) {
+            enableLPQuaternion(0);
+        }
+    }
+
+    /* apply accel/gyro bias to DMP bias                        */
+    /* precondition: masterEnable(0), mGyroBiasAvailable=true   */
+    /* postcondition: bias is applied upon masterEnable(1)      */
+    if(!(sensors & INV_THREE_AXIS_GYRO)) {
+        setGyroBias();
+    }
+    if(!(sensors & INV_THREE_AXIS_ACCEL)) {
+        setAccelBias();
+    }
+
+    /* to batch or not to batch */
+    int batchMode = computeBatchSensorMask(mEnabled, mBatchEnabled);
+    setBatch(batchMode,0);
+    
+    if (changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
+            (mCompassSensor->isIntegrated() << MagneticField) |
+            (mCompassSensor->isIntegrated() << RawMagneticField) |
+            (mPressureSensor->isIntegrated() << Pressure))) {
+        LOGV_IF(ENG_VERBOSE, "HAL DEBUG: Gyro, Accel, Compass, Pressure changes");
+        if ((checkSmdSupport() == 1) || (checkPedometerSupport() == 1) || (sensors &
+            (INV_THREE_AXIS_GYRO
+                | INV_THREE_AXIS_ACCEL
+                | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated())
+                | (INV_ONE_AXIS_PRESSURE * mPressureSensor->isIntegrated())))) {
+            LOGV_IF(ENG_VERBOSE, "SMD or Hardware sensors enabled");
+            LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
+            if (mFeatureActiveMask & DMP_FEATURE_MASK) {
+                LOGV_IF(ENG_VERBOSE, "HAL DEBUG: LPQ, SMD, SO enabled");
+                // disable DMP event interrupt only (w/ data interrupt)
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        0, mpu.dmp_event_int_on, getTimestamp());
+                if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
+                    res = -1;
+                    LOGE("HAL:ERR can't disable DMP event interrupt");
+                    return res;
+                }
+            }
+
+            if ((mFeatureActiveMask & DMP_FEATURE_MASK) &&
+                    !((mFeatureActiveMask & 0x240) ||
+                        (mFeatureActiveMask & 0x220) ||
+                        (mFeatureActiveMask & 0x280))) {
+LOGI("mFeatureActiveMask=%lld", mFeatureActiveMask);
+                // enable DMP
+                onDmp(1);
+                res = enableAccel(on);
+                if(res < 0) {
+                    return res;
+                }
+                if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
+                    res = turnOffAccelFifo();
+                }
+                if(res < 0) {
+                    return res;
+                }
+            }
+            res = masterEnable(1);
+            if(res < 0) {
+                return res;
+            }
+        } else { // all sensors idle -> reduce power
+            LOGV_IF(ENG_VERBOSE, "HAL DEBUG: not SMD or Hardware sensors");
+            if (isDmpDisplayOrientationOn()
+                    && (mDmpOrientationEnabled
+                            || !isDmpScreenAutoRotationEnabled())) {
+                enableDmpOrientation(1);
+            }
+
+            if (!cal_stored) {
+                storeCalibration();
+                cal_stored = 1;
+            }
+        }
+    } else if ((changed &
+                    ((!mCompassSensor->isIntegrated()) << MagneticField) ||
+                    ((!mCompassSensor->isIntegrated()) << RawMagneticField))
+                    &&
+              !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL
+                | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated()))))
+    ) {
+        LOGV_IF(ENG_VERBOSE, "HAL DEBUG: Gyro, Accel, Compass no change");
+        if (!cal_stored) {
+            storeCalibration();
+            cal_stored = 1;
+        }
+    } else {
+      LOGV_IF(ENG_VERBOSE, "HAL DEBUG: mEnabled");
+      if (sensors &
+            (INV_THREE_AXIS_GYRO
+                | INV_THREE_AXIS_ACCEL
+                | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
+            res = masterEnable(1);
+            if(res < 0)
+                return res;
+        }
+    }
+
+    return res;
+}
+
+/* check if batch mode should be turned on or not */
+int MPLSensor::computeBatchSensorMask(int enableSensors, int tempBatchSensor)
+{
+    VFUNC_LOG;
+    int batchMode = 1;
+    
+    LOGV("HAL:computeBatchSensorMask: enableSensors=%d tempBatchSensor=%d", enableSensors, tempBatchSensor);
+        
+    // check for possible continuous data mode
+    for(int i = 0; i <= Pressure; i++) {
+        if ((enableSensors & (1 << i)) && !(tempBatchSensor & (1 << i))) {
+            LOGV("HAL:computeBatchSensorMask: hardware sensor on continuous mode:%d", i);
+            // if any one of the hardware sensor is in continuous data mode
+            // turn off batch mode.
+            return 0;
+        }
+        if ((enableSensors & (1 << i)) && (tempBatchSensor & (1 << i))) {
+            LOGV("HAL:computeBatchSensorMask: hardware sensor is batch:%d", i);
+            // if hardware sensor is batched, check if virtual sensor is batched
+            if ((enableSensors & (1 << GameRotationVector))
+                            && !(tempBatchSensor & (1 << GameRotationVector))) {
+            LOGV("HAL:computeBatchSensorMask: but virtual sensor is not:%d", i);
+                return 0;
+            }
+        }
+    }
+
+    for(int i = Orientation; i <= GeomagneticRotationVector; i++) {
+        if ((enableSensors & (1 << i)) && !(tempBatchSensor & (1 << i))) {
+             LOGV("HAL:computeBatchSensorMask: composite sensor on continuous mode:%d", i);
+            // if composite sensors are on but not batched
+            // turn off batch mode.
+            return 0;
+        }
+    }
+
+    LOGV_IF(EXTRA_VERBOSE, "HAL:computeBatchSensorMask: batchMode=%d, mBatchEnabled=%0x", batchMode, tempBatchSensor);
+    return (batchMode && tempBatchSensor);
+}
+
+/* This function is called by enable() */
+int MPLSensor::setBatch(int en, int toggleEnable)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+    int64_t wanted = 1000000000LL;
+    int64_t timeout = 0;
+    int timeoutInMs = 0;
+    int featureMask = computeBatchDataOutput();
+
+    // reset master enable
+    res = masterEnable(0);
+    if (res < 0) {
+        return res;
+    }
+
+    if (en) {
+        /* take the minimum batchmode timeout */
+        int64_t timeout = 30000000000LL;
+        int64_t ns;
+        for (int i = 0; i < NumSensors; i++) {
+            LOGV_IF(0, "mFeatureActiveMask=0x%016llx, mEnabled=0x%01x, mBatchEnabled=0x%x",
+                            mFeatureActiveMask, mEnabled, mBatchEnabled);
+            if ((mEnabled & (1 << i)) && (mBatchEnabled & (1 << i)) ||
+                    (((featureMask & INV_DMP_PED_STANDALONE) && (mBatchEnabled & (1 << StepDetector))))) {
+                LOGV_IF(ENG_VERBOSE, "sensor=%d, timeout=%lld", i, mBatchTimeouts[i]);
+                ns = mBatchTimeouts[i];
+                timeout = (ns < timeout) ? ns : timeout;
+            }
+        }
+        /* Convert ns to millisecond */
+        timeoutInMs = timeout / 1000000; 
+    } else {
+        timeoutInMs = 0;
+    }    
+
+    LOGV_IF(PROCESS_VERBOSE, "HAL: batch timeout set to %dms", timeoutInMs);
+
+    /* step detector is enabled and */
+    /* batch mode is standalone */
+    if (en && (mFeatureActiveMask & INV_DMP_PEDOMETER) &&
+            (featureMask & INV_DMP_PED_STANDALONE)) {
+        LOGI("ID_P only = 0x%x", mBatchEnabled);
+        enablePedStandalone(1);
+    } else {
+        enablePedStandalone(0);
+    }
+    
+    /* step detector and GRV are enabled and */
+    /* batch mode is ped q */
+    if (en && (mFeatureActiveMask & INV_DMP_PEDOMETER) &&
+            (mEnabled & (1 << GameRotationVector)) &&
+            (featureMask & INV_DMP_PED_QUATERNION)) {
+        LOGI("ID_P and GRV or ALL = 0x%x", mBatchEnabled);        
+        LOGI("ID_P is enabled for batching, PED quat will be automatically enabled");
+        enableLPQuaternion(0);
+        enablePedQuaternion(1);
+    } else if (!(featureMask & INV_DMP_PED_STANDALONE)){
+        enablePedQuaternion(0);
+    }
+
+    /* step detector and hardware sensors enabled */
+    if (en && (featureMask & INV_DMP_PED_INDICATOR) &&
+            ((mEnabled) ||
+            (mFeatureActiveMask & INV_DMP_PED_STANDALONE))) {
+        enablePedIndicator(1);
+    } else {
+        enablePedIndicator(0);
+    }
+    
+    /* GRV is enabled and */
+    /* batch mode is 6axis q */
+    if (en && (mEnabled & (1 << GameRotationVector)) &&
+            (featureMask & INV_DMP_6AXIS_QUATERNION)) {
+        LOGI("GRV = 0x%x", mBatchEnabled);
+        enableLPQuaternion(0);
+        enable6AxisQuaternion(1);
+    } else if (!(featureMask & INV_DMP_PED_QUATERNION)){
+        LOGI("Toggle back to normal 6 axis");
+        if (mEnabled & (1 << GameRotationVector)) {
+            enableLPQuaternion(1);
+        }
+        enable6AxisQuaternion(0);
+    }
+    
+    /* write required timeout to sysfs */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            timeoutInMs, mpu.batchmode_timeout, getTimestamp());
+    if (write_sysfs_int(mpu.batchmode_timeout, timeoutInMs) < 0) {
+        LOGE("HAL:ERR can't write batchmode_timeout");
+    }
+
+    if (en) {
+        // enable DMP
+        res = onDmp(1);
+        if (res < 0) {
+            return res;
+        }
+
+        // set batch rates
+        if (setBatchDataRates() < 0) {
+            LOGE("HAL:ERR can't set batch data rates");
+        }
+        
+        // default fifo rate to 200Hz
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                200, mpu.gyro_fifo_rate, getTimestamp());
+        if (write_sysfs_int(mpu.gyro_fifo_rate, 200) < 0) {
+            res = -1;
+            LOGE("HAL:ERR can't set rate to 200Hz");
+            return res;
+        }
+    } else {
+        if (mFeatureActiveMask == 0) {
+            // disable DMP
+            res = onDmp(0);
+            if (res < 0) {
+                return res;
+            }
+        }
+        /* reset sensor rate */
+        /*if (resetDataRates() < 0) {
+            LOGE("HAL:ERR can't reset output rate back to original setting");
+        }*/
+    }
+    if (toggleEnable == 1) {
+        if (mFeatureActiveMask || mEnabled)
+            masterEnable(1);
+    }
+    return res;
+}
+
+/* Store calibration file */
+void MPLSensor::storeCalibration(void)
+{
+    if(mHaveGoodMpuCal == true
+        || mAccelAccuracy >= 2
+        || mCompassAccuracy >= 3) {
+       int res = inv_store_calibration();
+       if (res) {
+           LOGE("HAL:Cannot store calibration on file");
+       } else {
+           LOGV_IF(PROCESS_VERBOSE, "HAL:Cal file updated");
+       }
+    }
+}
+
+void MPLSensor::cbProcData(void)
+{
+    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::rawGyroHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_gyroscope_raw(s->uncalibrated_gyro.uncalib,
+                                               &s->gyro.status, &s->timestamp);
+    if(update) {
+        memcpy(s->uncalibrated_gyro.bias, mGyroBias, sizeof(mGyroBias));
+        LOGV_IF(HANDLER_DATA,"HAL:gyro bias data : %+f %+f %+f -- %lld - %d",
+            s->uncalibrated_gyro.bias[0], s->uncalibrated_gyro.bias[1],
+            s->uncalibrated_gyro.bias[2], s->timestamp, update);
+    }
+    s->gyro.status = SENSOR_STATUS_UNRELIABLE;
+    LOGV_IF(HANDLER_DATA, "HAL:raw gyro data : %+f %+f %+f -- %lld - %d",
+            s->uncalibrated_gyro.uncalib[0], s->uncalibrated_gyro.uncalib[1],
+            s->uncalibrated_gyro.uncalib[2], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::accelHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_accelerometer(
+        s->acceleration.v, &s->acceleration.status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d",
+            s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2],
+            s->timestamp, update);
+    mAccelAccuracy = s->acceleration.status;
+    return update;
+}
+
+int MPLSensor::compassHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_magnetic_field(
+        s->magnetic.v, &s->magnetic.status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d",
+            s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2],
+            s->timestamp, update);
+    mCompassAccuracy = s->magnetic.status;
+    return update;
+}
+
+int MPLSensor::rawCompassHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    //TODO:: need to handle uncalib data and bias for 3rd party compass
+    if(mCompassSensor->providesCalibration()) {
+        update = mCompassSensor->readRawSample(s->uncalibrated_magnetic.uncalib, &s->timestamp);
+    }
+    else {
+        update = inv_get_sensor_type_magnetic_field_raw(s->uncalibrated_magnetic.uncalib,
+                     &s->magnetic.status, &s->timestamp);
+    }
+    if(update) {
+        memcpy(s->uncalibrated_magnetic.bias, mCompassBias, sizeof(mCompassBias));
+        LOGV_IF(HANDLER_DATA, "HAL:compass bias data: %+f %+f %+f -- %lld - %d",
+                s->uncalibrated_magnetic.bias[0], s->uncalibrated_magnetic.bias[1],
+                s->uncalibrated_magnetic.bias[2], s->timestamp, update);
+    }
+    s->magnetic.status = SENSOR_STATUS_UNRELIABLE;
+    LOGV_IF(HANDLER_DATA, "HAL:compass raw data: %+f %+f %+f %d -- %lld - %d",
+        s->uncalibrated_magnetic.uncalib[0], s->uncalibrated_magnetic.uncalib[1],
+                    s->uncalibrated_magnetic.uncalib[2], s->magnetic.status, s->timestamp, update);
+    return update;
+}
+
+/*
+    Rotation Vector handler.
+    NOTE: rotation vector does not have an accuracy or status
+*/
+int MPLSensor::rvHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int8_t status;
+    int update;
+    update = inv_get_sensor_type_rotation_vector(s->data, &status,
+                                                 &s->timestamp);
+    update |= isCompassDisabled();
+    LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f %+f- %+lld - %d",
+            s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->timestamp,
+            update);
+
+    return update;
+}
+
+/*
+    Game Rotation Vector handler.
+    NOTE: rotation vector does not have an accuracy or status
+*/
+int MPLSensor::grvHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int8_t status;
+    int update;
+    update = inv_get_sensor_type_rotation_vector_6_axis(s->data, &status,
+                                                     &s->timestamp);
+    /*hack*/
+    /*s->data[0] =  mCached6AxisQuaternionData[0];
+    s->data[1] =  mCached6AxisQuaternionData[1];
+    s->data[2] =  mCached6AxisQuaternionData[2];
+    update = 1;*/
+    
+                                                     
+    LOGV_IF(HANDLER_DATA, "HAL:grv data: %+f %+f %+f %+f %+f - %+lld - %d",
+            s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->timestamp,
+            update);
+    return update;
+}
+
+int MPLSensor::laHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_linear_acceleration(
+            s->gyro.v, &s->gyro.status, &s->timestamp);
+    update |= isCompassDisabled();
+    LOGV_IF(HANDLER_DATA, "HAL:la data: %+f %+f %+f - %lld - %d",
+            s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::gravHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_gravity(s->gyro.v, &s->gyro.status,
+                                         &s->timestamp);
+    update |= isCompassDisabled();
+    LOGV_IF(HANDLER_DATA, "HAL:gr data: %+f %+f %+f - %lld - %d",
+            s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::orienHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_orientation(
+            s->orientation.v, &s->orientation.status, &s->timestamp);
+    update |= isCompassDisabled();
+    LOGV_IF(HANDLER_DATA, "HAL:or data: %f %f %f - %lld - %d",
+            s->orientation.v[0], s->orientation.v[1], s->orientation.v[2],
+            s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::smHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update = 1;
+
+    /* When event is triggered, set data to 1 */
+    s->data[0] = 1.f;
+    s->data[1] = 0.f;
+    s->data[2] = 0.f;
+    s->acceleration.status
+            = SENSOR_STATUS_UNRELIABLE;
+
+    /* Capture timestamp in HAL */
+    struct timespec ts;
+    clock_gettime(CLOCK_MONOTONIC, &ts);
+    s->timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
+
+    /* Identify which sensor this event is for */
+    s->version = sizeof(sensors_event_t);
+    s->sensor = ID_SM;
+    s->type = SENSOR_TYPE_SIGNIFICANT_MOTION;
+
+    LOGV_IF(HANDLER_DATA, "HAL:sm data: %f - %lld - %d",
+            s->data[0], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::scHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update = 0;
+
+    //update = readDmpPedometerEvents(s, 1);
+    LOGV_IF(HANDLER_DATA, "HAL:sc data: %f - %lld - %d",
+            s->data[0], s->timestamp, update);
+    return update < 1 ? 0 :1;
+}
+
+int MPLSensor::gmHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int8_t status;
+    int update = 0;
+    update = inv_get_sensor_type_geomagnetic_rotation_vector(s->data, &status,
+                                                             &s->timestamp);
+
+    LOGV_IF(HANDLER_DATA, "HAL:gm data: %+f %+f %+f %+f %+f- %+lld - %d",
+            s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->timestamp, update);
+    return update < 1 ? 0 :1;
+
+}
+
+int MPLSensor::psHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int8_t status;
+    int update = 0;
+    
+    s->data[0] = mCachedPressureData / 100; //hpa (millibar)
+    s->data[1] = 0;
+    s->data[2] = 0;
+    s->timestamp = mPressureTimestamp;
+    s->magnetic.status = 0;
+    update = 1;   
+
+    LOGV_IF(HANDLER_DATA, "HAL:ps data: %+f %+f %+f %+f- %+lld - %d",
+            s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update);
+    return update < 1 ? 0 :1;
+
+}
+
+int MPLSensor::enable(int32_t handle, int en)
+{
+    VFUNC_LOG;
+
+    android::String8 sname;
+    int what = -1, err = 0;
+    int batchMode = 0;
+
+    switch (handle) {
+    case ID_SC:
+         what = StepCounter;
+         sname = "Step Counter";
+         LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
+                sname.string(), handle,
+                (mDmpStepCountEnabled? "en": "dis"),
+                (en? "en" : "dis"));
+        enableDmpPedometer(en, 0);
+        mDmpStepCountEnabled = !!en;
+        return 0;
+    case ID_P:
+        sname = "StepDetector";
+        LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
+                sname.string(), handle,
+                (mDmpPedometerEnabled? "en": "dis"),
+                (en? "en" : "dis"));
+        enableDmpPedometer(en, 1);
+        mDmpPedometerEnabled = !!en;
+        batchMode = computeBatchSensorMask(mEnabled, mBatchEnabled);
+        setBatch(batchMode,1);
+        return 0;
+    case ID_SM:
+        sname = "Significant Motion";
+        LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
+                sname.string(), handle,
+                (mDmpSignificantMotionEnabled? "en": "dis"),
+                (en? "en" : "dis"));
+        enableDmpSignificantMotion(en);
+        mDmpSignificantMotionEnabled = !!en;
+        return 0;
+    case ID_SO:
+        sname = "Screen Orientation";
+        LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
+                sname.string(), handle,
+                (mDmpOrientationEnabled? "en": "dis"),
+                (en? "en" : "dis"));
+        enableDmpOrientation(en && isDmpDisplayOrientationOn());
+        mDmpOrientationEnabled = !!en;
+        return 0;
+    case ID_A:
+        what = Accelerometer;
+        sname = "Accelerometer";
+        break;
+    case ID_M:
+        what = MagneticField;
+        sname = "MagneticField";
+        break;
+    case ID_RM:
+        what = RawMagneticField;
+        sname = "MagneticField Uncalibrated";
+        break;
+    case ID_O:
+        what = Orientation;
+        sname = "Orientation";
+        break;
+    case ID_GY:
+        what = Gyro;
+        sname = "Gyro";
+        break;
+    case ID_RG:
+        what = RawGyro;
+        sname = "Gyro Uncalibrated";
+        break;
+    case ID_GR:
+        what = Gravity;
+        sname = "Gravity";
+        break;
+    case ID_RV:
+        what = RotationVector;
+        sname = "RotationVector";
+        break;
+    case ID_GRV:
+        what = GameRotationVector;
+        sname = "GameRotationVector";
+        break;
+    case ID_LA:
+        what = LinearAccel;
+        sname = "LinearAccel";
+        break;
+    case ID_GMRV:
+        what = GeomagneticRotationVector;
+        sname = "GeomagneticRotationVector";
+        break;
+    case ID_PS:
+        what = Pressure;
+        sname = "Pressure";
+        break;
+    default: //this takes care of all the gestures
+        what = handle;
+        sname = "Others";
+        break;
+    }
+
+    if (uint32_t(what) >= NumSensors)
+        return -EINVAL;
+
+    int newState = en ? 1 : 0;
+    unsigned long sen_mask;
+
+    LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
+            sname.string(), handle,
+            ((mEnabled & (1 << what)) ? "en" : "dis"),
+            ((uint32_t(newState) << what) ? "en" : "dis"));
+    LOGV_IF(PROCESS_VERBOSE,
+            "HAL:%s sensor state change what=%d", sname.string(), what);
+
+    // pthread_mutex_lock(&mMplMutex);
+    // pthread_mutex_lock(&mHALMutex);
+
+    if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) {
+        uint32_t sensor_type;
+        short flags = newState;
+        uint32_t lastEnabled = mEnabled, changed = 0;
+
+        mEnabled &= ~(1 << what);
+        mEnabled |= (uint32_t(flags) << what);
+
+        LOGV_IF(PROCESS_VERBOSE, "HAL:handle = %d", handle);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:flags = %d", flags);
+        computeLocalSensorMask(mEnabled);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled);
+        LOGV_IF(ENG_VERBOSE, "HAL:last enable : lastEnabled = %d", lastEnabled);
+        sen_mask = mLocalSensorMask & mMasterSensorMask;
+        mSensorMask = sen_mask;
+        LOGV_IF(PROCESS_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask);
+
+        switch (what) {
+            case Gyro:
+            case RawGyro:
+            case Accelerometer:
+                if ((!(mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK) &&
+                    !(mEnabled & VIRTUAL_SENSOR_9AXES_MASK)) &&
+                    ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) {
+                    changed |= (1 << what);
+                }
+                if (mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION) {
+                     changed |= (1 << what);
+                }
+                break;
+            case MagneticField:
+            case RawMagneticField:
+                if (!(mEnabled & VIRTUAL_SENSOR_9AXES_MASK) &&
+                    ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) {
+                    changed |= (1 << what);
+                }
+                break;
+            case Pressure:
+                if ((lastEnabled & (1 << what)) != (mEnabled & (1 << what))) {
+                    changed |= (1 << what);
+                }
+                break;
+            case GameRotationVector:
+                if (!en)
+                    storeCalibration();
+                if ((en && !(lastEnabled & VIRTUAL_SENSOR_ALL_MASK))
+                         ||
+                    (en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK))
+                         ||
+                    (!en && !(mEnabled & VIRTUAL_SENSOR_ALL_MASK))
+                         ||
+                    (!en && (mEnabled & VIRTUAL_SENSOR_MAG_6AXES_MASK))) {
+                    for (int i = Gyro; i <= RawMagneticField; i++) {
+                        if (!(mEnabled & (1 << i))) {
+                            changed |= (1 << i);
+                        }
+                    }
+                }
+                break;
+
+            case Orientation:
+            case RotationVector:
+            case LinearAccel:
+            case Gravity:
+                if (!en)
+                    storeCalibration();
+                if ((en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK))
+                         ||
+                    (!en && !(mEnabled & VIRTUAL_SENSOR_9AXES_MASK))) {
+                    for (int i = Gyro; i <= RawMagneticField; i++) {
+                        if (!(mEnabled & (1 << i))) {
+                            changed |= (1 << i);
+                        }
+                    }
+                }
+                break;
+            case GeomagneticRotationVector:
+                if (!en)
+                    storeCalibration();
+                if ((en && !(lastEnabled & VIRTUAL_SENSOR_ALL_MASK))
+                        ||
+                    (en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK))
+                         ||
+                   (!en && !(mEnabled & VIRTUAL_SENSOR_ALL_MASK))
+                         ||
+                   (!en && (mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK))) {
+                   for (int i = Accelerometer; i <= RawMagneticField; i++) {
+                       if (!(mEnabled & (1<<i))) {
+                          changed |= (1 << i);
+                       }
+                   }
+                }
+                break;
+        }
+        LOGV_IF(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 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;
+}
+
+void MPLSensor::getHandle(int32_t handle, int &what, android::String8 &sname)
+{
+   VFUNC_LOG;
+
+   what = -1;
+
+   switch (handle) {
+   case ID_P:
+        what = StepDetector;
+        sname = "StepDetector";
+        break;
+   case ID_SC:
+        what = StepCounter;
+        sname = "StepCounter";
+        break;
+   case ID_SM:
+        what = SignificantMotion;
+        sname = "SignificantMotion";
+        break;
+   case ID_SO:
+        what = handle;
+        sname = "ScreenOrienation";
+   case ID_A:
+        what = Accelerometer;
+        sname = "Accelerometer";
+        break;
+   case ID_M:
+        what = MagneticField;
+        sname = "MagneticField";
+        break;
+   case ID_RM:
+        what = RawMagneticField;
+        sname = "MagneticField Uncalibrated";
+        break;
+   case ID_O:
+        what = Orientation;
+        sname = "Orientation";
+        break;
+   case ID_GY:
+        what = Gyro;
+        sname = "Gyro";
+        break;
+   case ID_RG:
+        what = RawGyro;
+        sname = "Gyro Uncalibrated";
+        break;
+   case ID_GR:
+        what = Gravity;
+        sname = "Gravity";
+        break;
+   case ID_RV:
+        what = RotationVector;
+        sname = "RotationVector";
+        break;
+   case ID_GRV:
+        what = GameRotationVector;
+        sname = "GameRotationVector";
+        break;
+   case ID_LA:
+        what = LinearAccel;
+        sname = "LinearAccel";
+        break;
+   case ID_PS:
+        what = Pressure;
+        sname = "Pressure";
+        break;
+   default: // this takes care of all the gestures
+        what = handle;
+        sname = "Others";
+        break;
+    }
+
+    LOGI_IF(EXTRA_VERBOSE, "HAL:getHandle - what=%d, sname=%s", what, sname.string());
+    return;
+}
+
+int MPLSensor::setDelay(int32_t handle, int64_t ns)
+{
+    VFUNC_LOG;
+
+    android::String8 sname;
+    int what = -1;
+
+#if 0
+    // skip the 1st call for enalbing sensors called by ICS/JB sensor service
+    static int counter_delay = 0;
+    if (!(mEnabled & (1 << what))) {
+        counter_delay = 0;
+    } else {
+        if (++counter_delay == 1) {
+            return 0;
+        }
+        else {
+            counter_delay = 0;
+        }
+    }
+#endif
+
+    getHandle(handle, what, sname);
+    if (uint32_t(what) >= NumSensors)
+        return -EINVAL;
+
+    if (ns < 0)
+        return -EINVAL;
+
+    LOGV_IF(PROCESS_VERBOSE,
+            "setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns);
+
+    // limit all rates to reasonable ones */
+    if (ns < 5000000LL) {
+        ns = 5000000LL;
+    }
+
+    /* store request rate to mDelays arrary for each sensor */
+    mDelays[what] = ns;
+
+    switch (what) {
+        case ID_SC:
+            /* set limits of delivery rate of events */
+            mStepCountPollTime = ns;
+            LOGV_IF(ENG_VERBOSE, "step count rate =%lld ns", ns);
+            break; 
+        case ID_P:
+        case SignificantMotion:
+        case ID_SO:
+            update_delay();
+            break;
+        case Gyro:
+        case RawGyro:
+        case Accelerometer:
+            for (int i = Gyro;
+                    i <= Accelerometer + mCompassSensor->isIntegrated();
+                    i++) {
+                if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
+                    LOGV_IF(PROCESS_VERBOSE,
+                            "HAL:ignore delay set due to sensor %d", i);
+                    return 0;
+                }
+            }
+            break;
+
+        case MagneticField:
+        case RawMagneticField:
+            if (mCompassSensor->isIntegrated() &&
+                    (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) ||
+                    ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) ||
+                    ((mEnabled & (1 << Accelerometer)) &&
+                        ns > mDelays[Accelerometer]))) {
+                 LOGV_IF(PROCESS_VERBOSE,
+                         "HAL:ignore delay set due to gyro/accel");
+                 return 0;
+            }
+            break;
+
+        case Orientation:
+        case RotationVector:
+        case GameRotationVector:
+        case GeomagneticRotationVector:
+        case LinearAccel:
+        case Gravity:
+            if (isLowPowerQuatEnabled()) {
+                LOGV_IF(PROCESS_VERBOSE,
+                        "HAL:need to update delay due to LPQ");
+                break;
+            }
+
+            for (int i = 0; i < NumSensors; i++) {
+                if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
+                    LOGV_IF(PROCESS_VERBOSE,
+                            "HAL:ignore delay set due to sensor %d", i);
+                    return 0;
+                }
+            }
+            break;
+    }
+
+    // pthread_mutex_lock(&mHALMutex);
+    int res = update_delay();
+    // pthread_mutex_unlock(&mHALMutex);
+    return res;
+}
+
+int MPLSensor::update_delay(void)
+{
+    VHANDLER_LOG;
+
+    int res = 0;
+    int64_t got;
+
+    if (mEnabled) {
+        int64_t wanted = 1000000000LL;
+        int64_t wanted_3rd_party_sensor = 1000000000LL;
+
+        // Sequence to change sensor's FIFO rate
+        // 1. reset master enable
+        // 2. Update delay
+        // 3. set master enable
+
+        // reset master enable
+        masterEnable(0);
+
+        int64_t gyroRate;
+        int64_t accelRate;
+        int64_t compassRate;
+        int64_t pressureRate;
+        
+        int rateInus;
+        int mplGyroRate;
+        int mplAccelRate;
+        int mplCompassRate;
+        int tempRate = wanted;
+
+        /* search the minimum delay requested across all enabled sensors */
+        for (int i = 0; i < NumSensors; i++) {
+            if (mEnabled & (1 << i)) {
+                int64_t ns = mDelays[i];
+                wanted = wanted < ns ? wanted : ns;
+            }
+        }
+        
+        if (mDmpOn) {
+            gyroRate = mDelays[Gyro] < mDelays[RawGyro] ? mDelays[Gyro] : mDelays[RawGyro];
+            accelRate = mDelays[Accelerometer];
+            compassRate = mDelays[MagneticField] < mDelays[RawMagneticField] ? mDelays[MagneticField] : mDelays[RawMagneticField];
+            pressureRate = mDelays[Pressure];
+            
+#ifdef ENABLE_MULTI_RATE
+            gyroRate = wanted;
+            accelRate = wanted;
+            compassRate = wanted;
+            pressureRate = wanted;   
+            // same delay for 3rd party Accel or Compass
+            wanted_3rd_party_sensor = wanted;
+#endif
+ 
+        }
+        else {        
+            gyroRate = wanted;
+            accelRate = wanted;
+            compassRate = wanted;
+            pressureRate = wanted;   
+            // same delay for 3rd party Accel or Compass
+            wanted_3rd_party_sensor = wanted;
+        }
+        
+        /* mpl rate in us in future maybe different for
+           gyro vs compass vs accel */
+ /*       rateInus = (int)wanted / 1000LL;
+        mplGyroRate = (int)gyroRate / 1000LL;
+        mplAccelRate = (int)accelRate / 1000LL;
+        mplCompassRate = (int)compassRate / 1000LL;  
+        
+        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);
+
+        LOGV_IF(PROCESS_VERBOSE,
+                "HAL:MPL gyro sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplGyroRate, 1000000000.f / gyroRate);
+        LOGV_IF(PROCESS_VERBOSE,
+                "HAL:MPL accel sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplAccelRate, 1000000000.f / accelRate);
+        LOGV_IF(PROCESS_VERBOSE,
+                "HAL:MPL compass sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplCompassRate, 1000000000.f / compassRate);
+*/
+        int enabled_sensors = mEnabled;
+        int tempFd = -1;
+
+        if(mFeatureActiveMask & INV_DMP_BATCH_MODE) {
+            // set batch rates
+            LOGV_IF(ENG_VERBOSE, "HAL: mFeatureActiveMask=%016llx", mFeatureActiveMask);
+            LOGV("HAL: batch mode is set, set batch data rates");
+            if (setBatchDataRates() < 0) {                
+                LOGE("HAL:ERR can't set batch data rates");
+            }
+        } else {
+        if (LA_ENABLED || GR_ENABLED || RV_ENABLED
+                       || GRV_ENABLED || O_ENABLED || GMRV_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
+            //TODO: may be able to combine DMP_FEATURE_MASK, DMP_SENSOR_MASK in the future
+            if(mFeatureActiveMask & DMP_FEATURE_MASK) {
+                bool setDMPrate= 0;
+                gyroRate = wanted;
+                accelRate = wanted;
+                compassRate = wanted;      
+                // same delay for 3rd party Accel or Compass
+                wanted_3rd_party_sensor = wanted;
+                rateInus = (int)wanted / 1000LL;
+                
+                 /* set rate in MPL */
+                 /* compass can only do 100Hz max */
+                 inv_set_gyro_sample_rate(rateInus);
+                 inv_set_accel_sample_rate(rateInus);
+                 inv_set_compass_sample_rate(rateInus);
+                 
+                 LOGV_IF(PROCESS_VERBOSE,
+                "HAL:MPL gyro sample rate: (mpl)=%d us (mpu)=%.2f Hz", rateInus, 1000000000.f / gyroRate);
+                 LOGV_IF(PROCESS_VERBOSE,
+                "HAL:MPL accel sample rate: (mpl)=%d us (mpu)=%.2f Hz", rateInus, 1000000000.f / accelRate);
+                 LOGV_IF(PROCESS_VERBOSE,
+                "HAL:MPL compass sample rate: (mpl)=%d us (mpu)=%.2f Hz", rateInus, 1000000000.f / compassRate);
+                 
+                // Set LP Quaternion sample rate if enabled
+                if (checkLPQuaternion()) {
+                    if (wanted <= RATE_200HZ) {
+#ifndef USE_LPQ_AT_FASTEST 
+                        enableLPQuaternion(0);
+#endif
+                    } else {
+                        inv_set_quat_sample_rate(rateInus);
+                        setDMPrate= 1;
+                    }
+                }
+                //if((mFeatureActiveMask & DMP_SENSOR_MASK) || setDMPrate==1) {
+                //    getDmpRate(&wanted);
+                //}
+            }
+
+            LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion");
+            //nsToHz
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+                    1000000000.f / gyroRate, mpu.gyro_rate,
+                    getTimestamp());
+            tempFd = open(mpu.gyro_rate, O_RDWR);
+            res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate);
+            if(res < 0) {
+                LOGE("HAL:GYRO update delay error");
+            }
+
+            if(USE_THIRD_PARTY_ACCEL == 1) {
+            // 3rd party accelerometer - if applicable
+            // nsToHz (BMA250)
+                LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)",
+                        wanted_3rd_party_sensor / 1000000L, mpu.accel_rate,
+                        getTimestamp());
+                tempFd = open(mpu.accel_rate, O_RDWR);
+                res = write_attribute_sensor(tempFd,
+                        wanted_3rd_party_sensor / 1000000L);
+                LOGE_IF(res < 0, "HAL:ACCEL update delay error");
+            } else {
+            // mpu accel
+               LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+                        1000000000.f / accelRate, mpu.accel_rate,
+                        getTimestamp());
+                tempFd = open(mpu.accel_rate, O_RDWR);
+                res = write_attribute_sensor(tempFd, 1000000000.f / accelRate);
+                LOGE_IF(res < 0, "HAL:ACCEL update delay error");
+            }
+
+            if (!mCompassSensor->isIntegrated()) {
+            // stand-alone compass - if applicable
+                LOGV_IF(PROCESS_VERBOSE,
+                        "HAL:Ext compass delay %lld", wanted_3rd_party_sensor);
+                LOGV_IF(PROCESS_VERBOSE, "HAL:Ext compass rate %.2f Hz",
+                        1000000000.f / wanted_3rd_party_sensor);
+                if (wanted_3rd_party_sensor <
+                        mCompassSensor->getMinDelay() * 1000LL) {
+                    wanted_3rd_party_sensor =
+                        mCompassSensor->getMinDelay() * 1000LL;
+                }
+                LOGV_IF(PROCESS_VERBOSE,
+                        "HAL:Ext compass delay %lld", wanted_3rd_party_sensor);
+                LOGV_IF(PROCESS_VERBOSE, "HAL:Ext compass rate %.2f Hz",
+                        1000000000.f / wanted_3rd_party_sensor);
+                mCompassSensor->setDelay(ID_M, wanted_3rd_party_sensor);
+                got = mCompassSensor->getDelay(ID_M);
+                inv_set_compass_sample_rate(got / 1000);
+            } else {
+            // compass on secondary bus
+                if (compassRate < mCompassSensor->getMinDelay() * 1000LL) {
+                    compassRate = mCompassSensor->getMinDelay() * 1000LL;
+                }
+                mCompassSensor->setDelay(ID_M, compassRate);
+            }
+
+            /*
+            //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 || RGY_ENABLED) {
+                wanted = (mDelays[Gyro] <= mDelays[RawGyro]?
+                    (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]):
+                    (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro]));
+                LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
+                if (mFeatureActiveMask & DMP_FEATURE_MASK) {
+                    //int64_t tempWanted;
+                    //getDmpRate(&tempWanted);
+                }
+
+                inv_set_gyro_sample_rate((int)wanted/1000LL);
+                LOGV_IF(PROCESS_VERBOSE,
+                    "HAL:MPL gyro sample rate: (mpl)=%d us", int(wanted/1000LL));
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+                        1000000000.f / wanted, mpu.gyro_rate, getTimestamp());
+                tempFd = open(mpu.gyro_rate, O_RDWR);
+                res = write_attribute_sensor(tempFd, 1000000000.f / wanted);
+                LOGE_IF(res < 0, "HAL:GYRO update delay error");
+            }
+
+            if (A_ENABLED) { /* there is only 1 fifo rate for MPUxxxx */
+
+#if (0)
+                wanted = mDelays[Accelerometer];
+#else
+                if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) {
+                    wanted = mDelays[Gyro];
+                } else if (RGY_ENABLED && mDelays[RawGyro]
+                            < mDelays[Accelerometer]) {
+                    wanted = mDelays[RawGyro];
+                } else {
+                    wanted = mDelays[Accelerometer];
+                }
+#endif
+                LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
+                if (mFeatureActiveMask & DMP_FEATURE_MASK) {
+                    //int64_t tempWanted;
+                    //getDmpRate(&tempWanted);
+                }
+
+                inv_set_accel_sample_rate((int)wanted/1000LL);
+                LOGV_IF(PROCESS_VERBOSE,
+                    "HAL:MPL accel sample rate: (mpl)=%d us", int(wanted/1000LL));
+                /* 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_rate,
+                        getTimestamp());
+                tempFd = open(mpu.accel_rate, O_RDWR);
+                if(USE_THIRD_PARTY_ACCEL == 1) {
+                    //BMA250 in ms
+                    res = write_attribute_sensor(tempFd, wanted / 1000000L);
+                }
+                else {
+                    //MPUxxxx in hz
+                    res = write_attribute_sensor(tempFd, 1000000000.f/wanted);
+                }
+                LOGE_IF(res < 0, "HAL:ACCEL update delay error");
+            }
+
+            /* Invensense compass calibration */
+            if (M_ENABLED || RM_ENABLED) {
+                int64_t compassWanted = (mDelays[MagneticField] <= mDelays[RawMagneticField]?
+                    (mEnabled & (1 << MagneticField)? mDelays[MagneticField]: mDelays[RawMagneticField]):
+                    (mEnabled & (1 << RawMagneticField)? mDelays[RawMagneticField]: mDelays[MagneticField]));
+                if (!mCompassSensor->isIntegrated()) {
+                    wanted = compassWanted;
+                } else {
+#if (0)
+                    wanted = compassWanted;
+#else
+                    if (GY_ENABLED
+                        && (mDelays[Gyro] < compassWanted)) {
+                        wanted = mDelays[Gyro];
+                    } else if (RGY_ENABLED
+                               && mDelays[RawGyro] < compassWanted) {
+                        wanted = mDelays[RawGyro];
+                    } else if (A_ENABLED && mDelays[Accelerometer]
+                                < compassWanted) {
+                        wanted = mDelays[Accelerometer];
+                    } else {
+                        wanted = compassWanted;
+                    }
+#endif
+                    LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
+                    if (mFeatureActiveMask & DMP_FEATURE_MASK) {
+                        //int64_t tempWanted;
+                        //getDmpRate(&tempWanted);
+                    }
+                }
+
+                mCompassSensor->setDelay(ID_M, wanted);
+                got = mCompassSensor->getDelay(ID_M);
+                inv_set_compass_sample_rate(got / 1000);
+                LOGV_IF(PROCESS_VERBOSE,
+                    "HAL:MPL compass sample rate: (mpl)=%d us", int(got/1000LL));
+            }
+            
+            if (PS_ENABLED) {
+#if (0)
+                wanted = mDelays[Pressure];
+#else
+                wanted = mDelays[Pressure];
+#endif
+                LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
+                if (mFeatureActiveMask & DMP_FEATURE_MASK) {
+                    //int64_t tempWanted;
+                    //getDmpRate(&tempWanted);
+                }
+
+                if (mPressureSensor) {
+                    mPressureSensor->setDelay(ID_PS, wanted);
+                } else {
+                    LOGV_IF(ENG_VERBOSE, "HAL:PRESSURE sensor not detected");
+                }
+                LOGE_IF(res < 0, "HAL:PRESSURE update delay error");
+            }
+        }
+
+        /* set master sampling frequency */
+        /* only use for non multi rate */
+        int64_t tempWanted = wanted;
+        getDmpRate(&tempWanted);
+        /* driver only looks at sampling frequency if DMP is off */
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+                1000000000.f / tempWanted, mpu.gyro_fifo_rate, getTimestamp());
+        tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
+        res = write_attribute_sensor(tempFd, 1000000000.f / tempWanted);
+            LOGE_IF(res < 0, "HAL:sampling frequency update delay error");
+        } //end of non batch mode
+
+        unsigned long sensors = mLocalSensorMask & mMasterSensorMask;
+        if (sensors &
+            (INV_THREE_AXIS_GYRO
+                | INV_THREE_AXIS_ACCEL
+                | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()
+                | (INV_ONE_AXIS_PRESSURE * mPressureSensor->isIntegrated())))) {
+            LOGV_IF(ENG_VERBOSE, "sensors=%lu", sensors);
+            res = masterEnable(1);
+            if(res < 0)
+                return res;
+        } else { // all sensors idle -> reduce power, unless DMP is needed
+            LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
+            if(mFeatureActiveMask & DMP_FEATURE_MASK) {
+                res = masterEnable(1);
+                if(res < 0)
+                    return res;
+            }
+        }
+    }
+
+    return res;
+}
+
+/* For Third Party Accel Input Subsystem Drivers only */
+int MPLSensor::readAccelEvents(sensors_event_t* data, int count)
+{
+    VHANDLER_LOG;
+
+    if (count < 1)
+        return -EINVAL;
+
+    ssize_t n = mAccelInputReader.fill(accel_fd);
+    if (n < 0) {
+        LOGE("HAL:missed accel events, exit");
+        return n;
+    }
+
+    int numEventReceived = 0;
+    input_event const* event;
+    int nb, done = 0;
+
+    while (done == 0 && count && mAccelInputReader.readEvent(&event)) {
+        int type = event->type;
+        if (type == EV_ABS) {
+            if (event->code == EVENT_TYPE_ACCEL_X) {
+                mPendingMask |= 1 << Accelerometer;
+                mCachedAccelData[0] = event->value;
+            } else if (event->code == EVENT_TYPE_ACCEL_Y) {
+                mPendingMask |= 1 << Accelerometer;
+                mCachedAccelData[1] = event->value;
+            } else if (event->code == EVENT_TYPE_ACCEL_Z) {
+                mPendingMask |= 1 << Accelerometer;
+                mCachedAccelData[2] =event-> value;
+            }
+        } else if (type == EV_SYN) {
+            done = 1;
+            if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
+                inv_build_accel(mCachedAccelData, 0, getTimestamp());
+            }
+        } else {
+            LOGE("HAL:AccelSensor: unknown event (type=%d, code=%d)",
+                    type, event->code);
+        }
+        mAccelInputReader.next();
+    }
+
+    return numEventReceived;
+}
+
+/**
+ *  Should be called after reading at least one of gyro
+ *  compass or accel data. (Also okay for handling all of them).
+ *  @returns 0, if successful, error number if not.
+ */
+int MPLSensor::readEvents(sensors_event_t* data, int count)
+{
+    //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;
+        }
+        if(msg & INV_MSG_NEW_AB_EVENT) {
+            getAccelBias();
+            mAccelAccuracy = inv_get_accel_accuracy();
+        }
+        if(msg & INV_MSG_NEW_GB_EVENT) {
+            getGyroBias();
+        }
+        if(msg & INV_MSG_NEW_FGB_EVENT) {
+            getFactoryGyroBias();
+        }
+        if(msg & INV_MSG_NEW_CB_EVENT) {
+            getCompassBias();
+            mCompassAccuracy = inv_get_mag_accuracy();
+        }
+    }
+
+    for (int i = 0; i < NumSensors; i++) {
+        int update = 0;
+        // handle step detector when ped_q is enabled
+        if ((i == StepDetector) && (mPedUpdate == 1)) {
+            mPedUpdate = 0;
+            update = readDmpPedometerEvents(data, count, ID_P, SENSOR_TYPE_STEP_DETECTOR, 1);
+            if(update == 1 && count > 0) {
+                data->timestamp = mStepSensorTimestamp;
+                //LOGI("sensor=%d type=%d data=%d", data->sensor, data->type, data->data[0]);
+                count--;
+                numEventReceived++;
+                continue;
+            }
+        }
+
+        // load up virtual sensors
+        if (mEnabled & (1 << i)) {
+            update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i);
+            mPendingMask |= (1 << i);
+
+            if (update && (count > 0)) {
+                *data++ = mPendingEvents[i];
+                count--;
+                numEventReceived++;
+            }
+        }
+    }
+
+    return numEventReceived;
+}
+
+// collect data for MPL (but NOT sensor service currently), from driver layer
+void MPLSensor::buildMpuEvent(void)
+{
+    int64_t mGyroSensorTimestamp=0, mAccelSensorTimestamp=0, latestTimestamp=0;
+    int lp_quaternion_on = 0, sixAxis_quaternion_on = 0,
+        ped_quaternion_on = 0, ped_standalone_on = 0;
+    size_t nbyte;
+    unsigned short data_format = 0;
+    int i, nb, mask = 0,
+        sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 : 0) +
+            ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 : 0) +
+            (((mLocalSensorMask & INV_THREE_AXIS_COMPASS)
+                && mCompassSensor->isIntegrated())? 1 : 0) +
+            ((mLocalSensorMask & INV_ONE_AXIS_PRESSURE)? 1 : 0);
+    //LOGV("mLocalSensorMask=0x%lx", mLocalSensorMask);
+    char *rdata = mIIOBuffer;
+    ssize_t rsize = 0;
+    size_t readCounter = 0;
+    char *rdataP = NULL;
+
+    /* 2 Bytes header + 6 Bytes x,y,z data | 8 bytes timestamp */
+    nbyte= (BYTES_PER_SENSOR + 8) * sensors * 1;
+
+    /* special case for 6 Axis or LPQ */
+    /* 2 Bytes header + 4 Bytes x data + 2 Bytes n/a */
+    /* 4 Bytes y data | 4 Bytes z data */
+    /* 8 Bytes timestamp */
+    if (isLowPowerQuatEnabled()) {
+        lp_quaternion_on = checkLPQuaternion();
+        if (lp_quaternion_on == 1) {
+            nbyte += BYTES_QUAT_DATA;
+        }
+    }
+
+    if ((sixAxis_quaternion_on = check6AxisQuatEnabled())) {
+        // sixAxis is mutually exclusive to LPQ
+        // and it is also never enabled when continuous data is enabled
+        // mLocalSensorMask does not need to be accounted for here
+        // because accel/gyro are turned off anyway
+        nbyte += BYTES_QUAT_DATA;
+    }
+
+    if ((ped_quaternion_on = checkPedQuatEnabled())) {
+        nbyte += BYTES_PER_SENSOR_PACKET;
+    }
+
+    if ((ped_standalone_on = checkPedStandaloneEnabled())) {
+        nbyte += BYTES_PER_SENSOR_PACKET;
+    }
+
+    /* check previous copied buffer */
+    /* append with just read data */
+    if (mLeftOverBufferSize > 0) {
+        LOGV_IF(0, "append old buffer size=%d", mLeftOverBufferSize);
+        memcpy(rdata, mLeftOverBuffer, mLeftOverBufferSize);
+            LOGV_IF(0,
+            "HAL:input retrieve rdata=:%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, "
+            "%d, %d,%d, %d, %d, %d\n",           
+            rdata[0], rdata[1], rdata[2], rdata[3],
+            rdata[4], rdata[5], rdata[6], rdata[7],
+            rdata[8], rdata[9], rdata[10], rdata[11],
+            rdata[12], rdata[13], rdata[14], rdata[15]);
+    }
+    rdataP = rdata + mLeftOverBufferSize;
+    
+    /* read expected  number of bytes */
+    rsize =  read(iio_fd, rdataP, nbyte);
+    if(rsize < 0) {
+        /* IIO buffer might have old data.
+           Need to flush it when enabling no sensors, to avoid infinite 
+           read loop.*/
+        LOGE("HAL:input data file descriptor not available - (%s)", 
+             strerror(errno));
+        if (sensors == 0) {
+            rsize = read(iio_fd, rdata, MAX_SUSPEND_BATCH_PACKET_SIZE);
+        }
+        return;
+    }
+
+    /* reset data and count pointer */
+    rdataP = rdata;
+    readCounter = rsize + mLeftOverBufferSize;
+
+#ifdef TESTING
+    LOGV_IF(INPUT_DATA,
+         "HAL:input rdataP:r=%ld, n=%d,"
+         "%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d\n",
+         rsize, nbyte,
+         rdataP[0], rdataP[1], rdataP[2], rdataP[3],
+         rdataP[4], rdataP[5], rdataP[6], rdataP[7],
+         rdataP[8], rdataP[9], rdataP[10], rdataP[11],
+         rdataP[12], rdataP[13], rdataP[14], rdataP[15]);
+#endif
+
+    LOGV_IF(INPUT_DATA && ENG_VERBOSE, 
+            "HAL:input rdata= %d nbyte= %d rsize= %ld readCounter= %d",
+            *((short *) rdata), nbyte, rsize, readCounter);
+    LOGV_IF(INPUT_DATA && ENG_VERBOSE, 
+            "HAL:input sensors= %d, lp_q_on= %d, 6axis_q_on= %d, "
+            "ped_q_on= %d, ped_standalone_on= %d",
+            sensors, lp_quaternion_on, sixAxis_quaternion_on, 
+            ped_quaternion_on, ped_standalone_on); 
+ 
+    while (readCounter > 0) {
+        mLeftOverBufferSize = 0;
+        mask = 0;
+        data_format = *((short *)(rdata));
+        LOGV_IF(INPUT_DATA && ENG_VERBOSE, 
+                "HAL:input data_format=%x", data_format);
+
+        if ((data_format & ~DATA_FORMAT_MASK) || (data_format == 0)) {
+            LOGE("HAL:input invalid data_format 0x%02X", data_format);
+            return;
+        }
+
+        if (data_format & DATA_FORMAT_STEP) {
+            LOGV_IF(ENG_VERBOSE, "STEP DETECTED:0x%x", data_format);
+            mPedUpdate = 1;
+            mask |= DATA_FORMAT_STEP;
+            // cancels step bit
+            data_format &= (~DATA_FORMAT_STEP);
+        }
+        
+        if (data_format == DATA_FORMAT_QUAT) {
+            mCachedQuaternionData[0] = *((int *) (rdata + 4));
+            mCachedQuaternionData[1] = *((int *) (rdata + 8));
+            mCachedQuaternionData[2] = *((int *) (rdata + 12));
+            rdata += QUAT_ONLY_LAST_PACKET_OFFSET;
+            mQuatSensorTimestamp = *((long long*) (rdata));
+            mask |= DATA_FORMAT_QUAT;
+            readCounter -= BYTES_QUAT_DATA;
+        }
+        else if (data_format == DATA_FORMAT_6_AXIS) {
+            mCached6AxisQuaternionData[0] = *((int *) (rdata + 4));
+            mCached6AxisQuaternionData[1] = *((int *) (rdata + 8));
+            mCached6AxisQuaternionData[2] = *((int *) (rdata + 12));
+            rdata += QUAT_ONLY_LAST_PACKET_OFFSET;
+            mQuatSensorTimestamp = *((long long*) (rdata));
+            mask |= DATA_FORMAT_6_AXIS;
+            readCounter -= BYTES_QUAT_DATA;
+        }
+        else if (data_format == DATA_FORMAT_PED_QUAT) {
+            mCachedPedQuaternionData[0] = *((short *) (rdata + 2));
+            mCachedPedQuaternionData[1] = *((short *) (rdata + 4));
+            mCachedPedQuaternionData[2] = *((short *) (rdata + 6));
+            rdata += BYTES_PER_SENSOR;
+            mQuatSensorTimestamp = *((long long*) (rdata));
+            mask |= DATA_FORMAT_PED_QUAT;
+            readCounter -= BYTES_PER_SENSOR_PACKET;
+        }
+        else if (data_format == DATA_FORMAT_PED_STANDALONE) {
+            LOGV_IF(ENG_VERBOSE, "STEP DETECTED:0x%x", data_format);
+            rdata += BYTES_PER_SENSOR;
+            mStepSensorTimestamp = *((long long*) (rdata));
+            mask |= DATA_FORMAT_PED_STANDALONE;
+            readCounter -= BYTES_PER_SENSOR_PACKET;
+            mPedUpdate = 1;
+        }
+        else if (data_format == DATA_FORMAT_GYRO) {
+            mCachedGyroData[0] = *((short *) (rdata + 2));
+            mCachedGyroData[1] = *((short *) (rdata + 4));
+            mCachedGyroData[2] = *((short *) (rdata + 6));
+            rdata += BYTES_PER_SENSOR;
+            mGyroSensorTimestamp = *((long long*) (rdata));
+            mask |= DATA_FORMAT_GYRO;
+            readCounter -= BYTES_PER_SENSOR_PACKET;
+        }
+        else if (data_format == DATA_FORMAT_ACCEL) {
+            mCachedAccelData[0] = *((short *) (rdata + 2));
+            mCachedAccelData[1] = *((short *) (rdata + 4));
+            mCachedAccelData[2] = *((short *) (rdata + 6));
+            rdata += BYTES_PER_SENSOR;
+            mAccelSensorTimestamp = *((long long*) (rdata));
+            mask |= DATA_FORMAT_ACCEL;
+            readCounter -= BYTES_PER_SENSOR_PACKET;
+        }
+        else  if (data_format == DATA_FORMAT_COMPASS) {
+            if (mCompassSensor->isIntegrated()) {
+                mCachedCompassData[0] = *((short *) (rdata + 2));
+                mCachedCompassData[1] = *((short *) (rdata + 4));
+                mCachedCompassData[2] = *((short *) (rdata + 6));
+                rdata += BYTES_PER_SENSOR;
+                mCompassTimestamp = *((long long*) (rdata));
+                if (mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0
+                                               || mCachedCompassData[2] != 0) {
+                    mask |= DATA_FORMAT_COMPASS;
+                }
+                readCounter -= BYTES_PER_SENSOR_PACKET;
+            }
+        }
+        else if (data_format == DATA_FORMAT_PRESSURE) {
+            if (mPressureSensor->isIntegrated()) {
+                mCachedPressureData = 
+                    ((*((short *)(rdata + 4))) << 16) + 
+                    (*((unsigned short *) (rdata + 6)));
+                rdata += BYTES_PER_SENSOR;
+                mPressureTimestamp = *((long long*) (rdata));
+                if (mCachedPressureData!= 0) {
+                    mask |= DATA_FORMAT_PRESSURE;
+                }
+                readCounter -= BYTES_PER_SENSOR_PACKET;
+            }
+        }
+        rdata += BYTES_PER_SENSOR;
+
+        /* read ahead and store left over data if any */
+        if ((readCounter != 0) && (rsize != (ssize_t)nbyte) && (readCounter <= 24)) {
+            int counter = readCounter;
+            int currentBufferCounter = 0;
+            LOGV_IF(0, "!!! not enough data readCounter=%d, expected nbyte=%d", readCounter, nbyte);
+            memcpy(mLeftOverBuffer, rdata, readCounter);
+            LOGV_IF(0, 
+                    "HAL:input store rdata=:%d, %d, %d, %d,%d, %d, %d, %d,%d, "
+                    "%d, %d, %d,%d, %d, %d, %d\n",
+                    mLeftOverBuffer[0], mLeftOverBuffer[1], mLeftOverBuffer[2], mLeftOverBuffer[3],
+                    mLeftOverBuffer[4], mLeftOverBuffer[5], mLeftOverBuffer[6], mLeftOverBuffer[7],
+                    mLeftOverBuffer[8], mLeftOverBuffer[9], mLeftOverBuffer[10], mLeftOverBuffer[11],
+                    mLeftOverBuffer[12],mLeftOverBuffer[13],mLeftOverBuffer[14], mLeftOverBuffer[15]);
+
+            mLeftOverBufferSize = readCounter;
+            readCounter = 0;
+            LOGV_IF(0, "!!! stored number of bytes:%d", mLeftOverBufferSize);            
+        }
+
+        /* handle data read */
+        if (mask & DATA_FORMAT_GYRO) {
+            /* batch mode does not batch temperature */
+            /* disable temperature read */
+            if (!(mFeatureActiveMask & INV_DMP_BATCH_MODE)) {
+                // send down temperature every 0.5 seconds
+                // with timestamp measured in "driver" layer
+                if(mGyroSensorTimestamp - mTempCurrentTime >= 500000000LL) {
+                    mTempCurrentTime = mGyroSensorTimestamp;
+                    long long temperature[2];
+                    if(inv_read_temperature(temperature) == 0) {
+                        LOGV_IF(INPUT_DATA,
+                        "HAL:input inv_read_temperature = %lld, timestamp= %lld",
+                        temperature[0], temperature[1]);
+                        inv_build_temp(temperature[0], temperature[1]);
+                     }
+#ifdef TESTING
+                    long bias[3], temp, temp_slope[3];
+                    inv_get_mpl_gyro_bias(bias, &temp);
+                    inv_get_gyro_ts(temp_slope);
+                    LOGI("T: %.3f "
+                     "GB: %+13f %+13f %+13f "
+                     "TS: %+13f %+13f %+13f "
+                     "\n",
+                     (float)temperature[0] / 65536.f,
+                     (float)bias[0] / 65536.f / 16.384f,
+                     (float)bias[1] / 65536.f / 16.384f,
+                     (float)bias[2] / 65536.f / 16.384f,
+                     temp_slope[0] / 65536.f,
+                     temp_slope[1] / 65536.f,
+                     temp_slope[2] / 65536.f);
+#endif
+                }
+            }
+            mPendingMask |= 1 << Gyro;
+            mPendingMask |= 1 << RawGyro;
+
+            if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
+                inv_build_gyro(mCachedGyroData, mGyroSensorTimestamp);
+                LOGV_IF(INPUT_DATA, 
+                        "HAL:input inv_build_gyro: %+8d %+8d %+8d - %lld",
+                        mCachedGyroData[0], mCachedGyroData[1],
+                        mCachedGyroData[2], mGyroSensorTimestamp);
+           }
+           latestTimestamp = mGyroSensorTimestamp;
+        }
+
+        if (mask & DATA_FORMAT_ACCEL) {
+            mPendingMask |= 1 << Accelerometer;
+            if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
+                inv_build_accel(mCachedAccelData, 0, mAccelSensorTimestamp);
+                 LOGV_IF(INPUT_DATA,
+                    "HAL:input inv_build_accel: %+8ld %+8ld %+8ld - %lld",
+                    mCachedAccelData[0], mCachedAccelData[1],
+                    mCachedAccelData[2], mAccelSensorTimestamp);
+            }
+            latestTimestamp = mAccelSensorTimestamp;
+        }
+
+        if ((mask & DATA_FORMAT_COMPASS) && 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:input inv_build_compass: %+8ld %+8ld %+8ld - %lld",
+                    mCachedCompassData[0], mCachedCompassData[1],
+                    mCachedCompassData[2], mCompassTimestamp);
+            }
+            latestTimestamp = mCompassTimestamp;
+        }
+
+        if (isLowPowerQuatEnabled() && lp_quaternion_on == 1
+                                && (mask & DATA_FORMAT_QUAT)) {
+            /* if bias was applied to DMP bias,
+               set status bits to disable gyro bias cal */
+            int status = 0;
+            if (mGyroBiasApplied == true) {
+                LOGV_IF(INPUT_DATA, "HAL:input mpl bias not used");
+                status |= INV_BIAS_APPLIED;
+                mGyroBiasApplied = false;
+            }
+            status |= 32 | INV_QUAT_3AXIS; /* default 32 (16/32bits) */
+            inv_build_quat(mCachedQuaternionData,
+                       status,
+                       mQuatSensorTimestamp);
+            LOGV_IF(INPUT_DATA,
+                    "HAL:input inv_build_quat: %+8ld %+8ld %+8ld - %lld",
+                    mCachedQuaternionData[0], mCachedQuaternionData[1],
+                    mCachedQuaternionData[2],
+                    mQuatSensorTimestamp);
+            latestTimestamp = mQuatSensorTimestamp;
+        }
+
+        if ((mask & DATA_FORMAT_6_AXIS) && check6AxisQuatEnabled()
+                                    && (sixAxis_quaternion_on == 1)) {
+            /* if bias was applied to DMP bias,
+               set status bits to disable gyro bias cal */
+            int status = 0;
+            if (mGyroBiasApplied == true) {
+                status |= INV_QUAT_6AXIS;
+                mGyroBiasApplied = false;
+            }
+            status |= 32 | INV_QUAT_3AXIS; /* default 32 (16/32bits) */
+            inv_build_quat(mCached6AxisQuaternionData,
+                       status,
+                       mQuatSensorTimestamp);
+            LOGV_IF(INPUT_DATA,
+                "HAL:input 6 axis ped quat: %+8ld %+8ld %+8ld - %lld",
+                mCached6AxisQuaternionData[0], mCached6AxisQuaternionData[1],
+                mCached6AxisQuaternionData[2],
+                mQuatSensorTimestamp);
+            latestTimestamp = mQuatSensorTimestamp;
+        }
+
+        if ((mask & DATA_FORMAT_PED_QUAT) && checkPedQuatEnabled()
+                                          && (ped_quaternion_on == 1)) {
+            /* if bias was applied to DMP bias,
+               set status bits to disable gyro bias cal */
+            int status = 0;
+            if (mGyroBiasApplied == true) {
+                status |= INV_QUAT_6AXIS;
+                mGyroBiasApplied = false;
+            }
+            status |= 32 | INV_QUAT_3AXIS; /* default 32 (16/32bits) */
+            inv_build_quat(mCachedPedQuaternionData,
+                       status,
+                       mQuatSensorTimestamp);
+
+            LOGV_IF(INPUT_DATA,
+            "HAL:input ped quat: %+8ld %+8ld %+8ld - %lld",
+            mCachedPedQuaternionData[0], mCachedPedQuaternionData[1],
+            mCachedPedQuaternionData[2],
+            mQuatSensorTimestamp);
+            latestTimestamp = mQuatSensorTimestamp;            
+        }
+
+        if ((mask & DATA_FORMAT_PRESSURE) && mPressureSensor->isIntegrated()) {
+            int status = 0;
+            if (mLocalSensorMask & INV_ONE_AXIS_PRESSURE) {
+           
+            LOGV_IF(INPUT_DATA,
+                    "HAL:input inv_build_pressure: %+8ld - %lld",
+                    mCachedPressureData, mPressureTimestamp);
+            }
+            latestTimestamp = mPressureTimestamp;
+        }
+
+        /* take the latest timestamp */
+        if (mask & DATA_FORMAT_STEP) {
+            mStepSensorTimestamp = latestTimestamp;
+        }
+   }//while end  
+}
+
+/* use for both MPUxxxx and third party compass */
+void MPLSensor::buildCompassEvent(void)
+{
+    VHANDLER_LOG;
+
+    int done = 0;
+
+    // pthread_mutex_lock(&mMplMutex);
+    // pthread_mutex_lock(&mHALMutex);
+
+    done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp);
+    if(mCompassSensor->isYasCompass()) {
+        if (mCompassSensor->checkCoilsReset() == 1) {
+           //Reset relevant compass settings
+           resetCompass();
+        }
+    }
+    if (done > 0) {
+        int status = 0;
+        if (mCompassSensor->providesCalibration()) {
+            status = mCompassSensor->getAccuracy();
+            status |= INV_CALIBRATED;
+        }
+        if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
+            inv_build_compass(mCachedCompassData, status,
+                              mCompassTimestamp);
+            LOGV_IF(INPUT_DATA,
+                    "HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld",
+                    mCachedCompassData[0], mCachedCompassData[1],
+                    mCachedCompassData[2], mCompassTimestamp);
+        }
+    }
+
+    // pthread_mutex_unlock(&mMplMutex);
+    // pthread_mutex_unlock(&mHALMutex);
+}
+
+int MPLSensor::resetCompass(void)
+{
+    VFUNC_LOG;
+
+    //Reset compass cal if enabled
+    if (mMplFeatureActiveMask & INV_COMPASS_CAL) {
+       LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal");
+       inv_init_vector_compass_cal();
+    }
+
+    //Reset compass fit if enabled
+    if (mMplFeatureActiveMask & INV_COMPASS_FIT) {
+       LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit");
+       inv_init_compass_fit();
+    }
+
+    return 0;
+}
+
+int MPLSensor::getFd(void) const
+{
+    VFUNC_LOG;
+    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getFd returning %d", iio_fd);
+    return iio_fd;
+}
+
+int MPLSensor::getAccelFd(void) const
+{
+    VFUNC_LOG;
+    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getAccelFd returning %d", accel_fd);
+    return accel_fd;
+}
+
+int MPLSensor::getCompassFd(void) const
+{
+    VFUNC_LOG;
+    int fd = mCompassSensor->getFd();
+    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getCompassFd returning %d", fd);
+    return fd;
+}
+
+int MPLSensor::turnOffAccelFifo(void)
+{
+    int i, res = 0, tempFd;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        0, mpu.accel_fifo_enable, getTimestamp());
+    res += write_sysfs_int(mpu.accel_fifo_enable, 0);
+    return res;
+}
+
+int MPLSensor::turnOffGyroFifo(void)
+{
+    int i, res = 0, tempFd;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        0, mpu.gyro_fifo_enable, getTimestamp());
+    res += write_sysfs_int(mpu.gyro_fifo_enable, 0);
+    return res;
+}
+
+int MPLSensor::enableDmpOrientation(int en)
+{
+    VFUNC_LOG;
+    int res = 0;
+    int enabled_sensors = mEnabled;
+
+    if (isMpuNonDmp())
+        return res;
+
+    // reset master enable
+    res = masterEnable(0);
+    if (res < 0)
+        return res;
+
+    if (en == 1) {
+        //Enable DMP orientation
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                en, mpu.display_orientation_on, getTimestamp());
+        if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
+            LOGE("HAL:ERR can't enable Android orientation");
+            res = -1;	// indicate an err
+            return res;
+        }
+
+        // enable DMP
+        res = onDmp(1);
+        if (res < 0)
+            return res;
+
+        // set rate to 200Hz
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                200, mpu.accel_fifo_rate, getTimestamp());
+        if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) {
+            res = -1;
+            LOGE("HAL:ERR can't set rate to 200Hz");
+            return res;
+        }
+
+        // enable accel engine
+        res = enableAccel(1);
+        if (res < 0)
+            return res;
+
+        // disable accel FIFO
+        if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) {
+            res = turnOffAccelFifo();
+            if (res < 0)
+                return res;
+        }
+
+        if (!mEnabled){
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                       1, mpu.dmp_event_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
+                res = -1;
+                LOGE("HAL:ERR can't enable DMP event interrupt");
+            }
+        }
+
+        mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION;
+        LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
+    } else {
+        mFeatureActiveMask &= ~INV_DMP_DISPL_ORIENTATION;
+        // disable DMP
+        if (mFeatureActiveMask == 0) {
+            res = onDmp(0);
+            if (res < 0)
+                return res;
+
+            // disable accel engine
+            if (!(mLocalSensorMask & mMasterSensorMask
+                    & INV_THREE_AXIS_ACCEL)) {
+                res = enableAccel(0);
+                if (res < 0)
+                    return res;
+            }
+        }
+        
+        if (mEnabled){
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                       en, mpu.dmp_event_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
+                res = -1;
+                LOGE("HAL:ERR can't enable DMP event interrupt");
+            }
+        }
+        LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
+    }
+
+    if (en || mEnabled || mFeatureActiveMask) {
+        res = masterEnable(1);
+    }
+    return res;
+}
+
+int MPLSensor::openDmpOrientFd(void)
+{
+    VFUNC_LOG;
+
+    if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) {
+        LOGV_IF(PROCESS_VERBOSE,
+                "HAL:DMP display orientation disabled or file desc opened");
+        return 0;
+    }
+
+    dmp_orient_fd = open(mpu.event_display_orientation, O_RDONLY| O_NONBLOCK);
+    if (dmp_orient_fd < 0) {
+        LOGE("HAL:ERR couldn't open dmpOrient node");
+        return -1;
+    } else {
+        LOGV_IF(PROCESS_VERBOSE,
+                "HAL:dmp_orient_fd opened : %d", dmp_orient_fd);
+        return 0;
+    }
+}
+
+int MPLSensor::closeDmpOrientFd(void)
+{
+    VFUNC_LOG;
+    if (dmp_orient_fd >= 0)
+        close(dmp_orient_fd);
+    return 0;
+}
+
+int MPLSensor::dmpOrientHandler(int orient)
+{
+    VFUNC_LOG;
+    LOGV_IF(PROCESS_VERBOSE, "HAL:orient %x", orient);
+    return 0;
+}
+
+int MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count)
+{
+    VFUNC_LOG;
+
+    char dummy[4];
+    int screen_orientation = 0;
+    FILE *fp;
+
+    fp = fopen(mpu.event_display_orientation, "r");
+    if (fp == NULL) {
+        LOGE("HAL:cannot open event_display_orientation");
+        return 0;
+    }
+    fscanf(fp, "%d\n", &screen_orientation);
+    fclose(fp);
+
+    int numEventReceived = 0;
+
+    if (mDmpOrientationEnabled && count > 0) {
+        sensors_event_t temp;
+
+        temp.version = sizeof(sensors_event_t);
+        temp.sensor = ID_SO;
+        temp.acceleration.status
+            = SENSOR_STATUS_UNRELIABLE;
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+        temp.type = SENSOR_TYPE_SCREEN_ORIENTATION;
+        temp.screen_orientation = screen_orientation;
+#endif
+        struct timespec ts;
+        clock_gettime(CLOCK_MONOTONIC, &ts);
+        temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
+
+        *data++ = temp;
+        count--;
+        numEventReceived++;
+    }
+
+    // read dummy data per driver's request
+    dmpOrientHandler(screen_orientation);
+    read(dmp_orient_fd, dummy, 4);
+
+    return numEventReceived;
+}
+
+int MPLSensor::getDmpOrientFd(void)
+{
+    VFUNC_LOG;
+
+    LOGV_IF(EXTRA_VERBOSE,
+            "MPLSensor::getDmpOrientFd returning %d", dmp_orient_fd);
+    return dmp_orient_fd;
+
+}
+
+int MPLSensor::checkDMPOrientation(void)
+{
+    VFUNC_LOG;
+    return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0);
+}
+
+int MPLSensor::getDmpRate(int64_t *wanted)
+{
+    VFUNC_LOG;
+
+      // set DMP output rate to FIFO
+      if(mDmpOn == 1) {
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                int(1000000000.f / *wanted), mpu.three_axis_q_rate,
+                getTimestamp());
+        write_sysfs_int(mpu.three_axis_q_rate, 1000000000.f / *wanted);
+        LOGV_IF(PROCESS_VERBOSE,
+                "HAL:DMP three axis rate %.2f Hz", 1000000000.f / *wanted);
+                
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                int(1000000000.f / *wanted), mpu.six_axis_q_rate,
+                getTimestamp());
+        write_sysfs_int(mpu.six_axis_q_rate, 1000000000.f / *wanted);
+        LOGV_IF(PROCESS_VERBOSE,
+                "HAL:DMP six axis rate %.2f Hz", 1000000000.f / *wanted);
+                
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                int(1000000000.f / *wanted), mpu.ped_q_rate,
+                getTimestamp());
+        write_sysfs_int(mpu.ped_q_rate, 1000000000.f / *wanted);
+        LOGV_IF(PROCESS_VERBOSE,
+                "HAL:DMP ped quaternion rate %.2f Hz", 1000000000.f / *wanted);
+        
+        //DMP running rate must be @ 200Hz
+        *wanted= RATE_200HZ;
+        LOGV_IF(PROCESS_VERBOSE,
+                "HAL:DMP rate= %.2f Hz", 1000000000.f / *wanted);
+    }
+    return 0;
+}
+
+int MPLSensor::getPollTime(void)
+{
+    VHANDLER_LOG;
+    return mPollTime;
+}
+
+int MPLSensor::getStepCountPollTime(void)
+{
+    VHANDLER_LOG;
+    if (mDmpStepCountEnabled) {
+    /* clamped to 1ms?, still rather large */
+    LOGV_IF(0/*EXTRA_VERBOSE*/, "Step Count poll time = %lld ms",
+            mStepCountPollTime / 1000000LL);
+        return (mStepCountPollTime / 1000000LL);
+    }
+    return -1;
+}
+
+bool MPLSensor::hasStepCountPendingEvents(void)
+{
+    VHANDLER_LOG;
+    if (mDmpStepCountEnabled) {
+        struct timespec t_now;
+        int64_t interval = 0;
+
+        clock_gettime(CLOCK_MONOTONIC, &t_now);
+        interval = ((int64_t(t_now.tv_sec) * 1000000000LL + t_now.tv_nsec) -
+                    (int64_t(mt_pre.tv_sec) * 1000000000LL + mt_pre.tv_nsec));
+
+        if (interval < mStepCountPollTime) {
+            LOGV_IF(0/*ENG_VERBOSE*/,
+                    "Step Count interval elapsed: %lld, triggered: %d",
+                    interval, mStepCountPollTime);
+            return false;
+        } else {
+            clock_gettime(CLOCK_MONOTONIC, &mt_pre);
+            LOGV_IF(0/*ENG_VERBOSE*/, "Step Count previous time: %ld ms",
+                    mt_pre.tv_nsec / 1000);
+            return true;
+        }
+    }
+    return false;
+}
+
+bool MPLSensor::hasPendingEvents(void) 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(void)
+{
+    VFUNC_LOG;
+}
+
+void MPLSensor::wakeEvent(void)
+{
+    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_long_to_float(long *ldata, float *fdata)
+{
+    VHANDLER_LOG;
+
+    if (!ldata || !fdata)
+        return -1;
+    fdata[0] = (float)ldata[0];
+    fdata[1] = (float)ldata[1];
+    fdata[2] = (float)ldata[2];
+    return 0;
+}
+
+int MPLSensor::inv_read_temperature(long long *data)
+{
+    VHANDLER_LOG;
+
+    int count = 0;
+    char raw_buf[40];
+    long raw = 0;
+
+    long long timestamp = 0;
+
+    memset(raw_buf, 0, sizeof(raw_buf));
+    count = read_attribute_sensor(gyro_temperature_fd, raw_buf,
+                                  sizeof(raw_buf));
+    if(count < 1) {
+        LOGE("HAL:error reading gyro temperature");
+        return -1;
+    }
+
+    count = sscanf(raw_buf, "%ld%lld", &raw, &timestamp);
+
+    if(count < 0) {
+        return -1;
+    }
+
+    LOGV_IF(ENG_VERBOSE,
+            "HAL:temperature raw = %ld, timestamp = %lld, count = %d",
+            raw, timestamp, count);
+    data[0] = raw;
+    data[1] = timestamp;
+
+    return 0;
+}
+
+int MPLSensor::inv_read_dmp_state(int fd)
+{
+    VFUNC_LOG;
+
+    if(fd < 0)
+        return -1;
+
+    int count = 0;
+    char raw_buf[10];
+    short raw = 0;
+
+    memset(raw_buf, 0, sizeof(raw_buf));
+    count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf));
+    if(count < 1) {
+        LOGE("HAL:error reading dmp state");
+        close(fd);
+        return -1;
+    }
+    count = sscanf(raw_buf, "%hd", &raw);
+    if(count < 0) {
+        LOGE("HAL:dmp state data is invalid");
+        close(fd);
+        return -1;
+    }
+    LOGV_IF(EXTRA_VERBOSE, "HAL:dmp state = %d, count = %d", raw, count);
+    close(fd);
+    return (int)raw;
+}
+
+int MPLSensor::inv_read_sensor_bias(int fd, long *data)
+{
+    VFUNC_LOG;
+
+    if(fd == -1) {
+        return -1;
+    }
+
+    char buf[50];
+    char x[15], y[15], z[15];
+
+    memset(buf, 0, sizeof(buf));
+    int count = read_attribute_sensor(fd, buf, sizeof(buf));
+    if(count < 1) {
+        LOGE("HAL:Error reading gyro bias");
+        return -1;
+    }
+    count = sscanf(buf, "%[^','],%[^','],%[^',']", x, y, z);
+    if(count) {
+        /* scale appropriately for MPL */
+        LOGV_IF(ENG_VERBOSE,
+                "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)",
+                atol(x), atol(y), atol(z));
+
+        data[0] = (long)(atol(x) / 10000 * (1L << 16));
+        data[1] = (long)(atol(y) / 10000 * (1L << 16));
+        data[2] = (long)(atol(z) / 10000 * (1L << 16));
+
+        LOGV_IF(ENG_VERBOSE,
+                "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)",
+                data[0], data[1], data[2]);
+    }
+    return 0;
+}
+
+/** fill in the sensor list based on which sensors are configured.
+ *  return the number of configured sensors.
+ *  parameter list must point to a memory region of at least 7*sizeof(sensor_t)
+ *  parameter len gives the length of the buffer pointed to by list
+ */
+int MPLSensor::populateSensorList(struct sensor_t *list, int len)
+{
+    VFUNC_LOG;
+
+    int numsensors;
+
+    if(len <
+        (int)((sizeof(sSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) {
+        LOGE("HAL:sensor list too small, not populating.");
+        return -(sizeof(sSensorList) / sizeof(sensor_t));
+    }
+
+    /* fill in the base values */
+    memcpy(list, sSensorList,
+           sizeof (struct sensor_t) * (sizeof(sSensorList) / sizeof(sensor_t)));
+
+    /* first add gyro, accel and compass to the list */
+
+    /* fill in gyro/accel values */
+    if(chip_ID == NULL) {
+        LOGE("HAL:Can not get gyro/accel id");
+    }
+    fillGyro(chip_ID, list);
+    fillAccel(chip_ID, list);
+
+    // TODO: need fixes for unified HAL and 3rd-party solution
+    mCompassSensor->fillList(&list[MagneticField]);
+    mCompassSensor->fillList(&list[RawMagneticField]);
+
+    if(1) {
+        numsensors = (sizeof(sSensorList) / sizeof(sensor_t));
+        /* all sensors will be added to the list
+           fill in orientation values */
+        fillOrientation(list);
+        /* fill in rotation vector values */
+        fillRV(list);
+        /* fill in game rotation vector values */
+        fillGRV(list);
+        /* fill in gravity values */
+        fillGravity(list);
+        /* fill in Linear accel values */
+        fillLinearAccel(list);
+        /* fill in Significant motion values */
+        fillSignificantMotion(list);
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+        /* fill in screen orientation values */
+        fillScreenOrientation(list);
+#endif
+    } else {
+        /* no 9-axis sensors, zero fill that part of the list */
+        numsensors = 3;
+        memset(list + 3, 0, 4 * sizeof(struct sensor_t));
+    }
+
+    return numsensors;
+}
+
+void MPLSensor::fillAccel(const char* accel, struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    if (accel) {
+        if(accel != NULL && strcmp(accel, "BMA250") == 0) {
+            list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
+            list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
+            list[Accelerometer].power = ACCEL_BMA250_POWER;
+            list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU6050") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU6050_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY;
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU6500_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
+            return;
+         } else if (accel != NULL && strcmp(accel, "MPU6515") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU6500_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU6500_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU6500_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU9150") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU9150_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU9150_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU9150_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY;
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) {
+            list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
+            list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
+            list[Accelerometer].power = ACCEL_BMA250_POWER;
+            list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
+            return;
+        }
+    }
+
+    LOGE("HAL:unknown accel id %s -- "
+         "params default to bma250 and might be wrong.",
+         accel);
+    list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
+    list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
+    list[Accelerometer].power = ACCEL_BMA250_POWER;
+    list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
+}
+
+void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    if ( gyro != NULL && strcmp(gyro, "MPU3050") == 0) {
+        list[Gyro].maxRange = GYRO_MPU3050_RANGE;
+        list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
+        list[Gyro].power = GYRO_MPU3050_POWER;
+        list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
+    } else if( gyro != NULL && strcmp(gyro, "MPU6050") == 0) {
+        list[Gyro].maxRange = GYRO_MPU6050_RANGE;
+        list[Gyro].resolution = GYRO_MPU6050_RESOLUTION;
+        list[Gyro].power = GYRO_MPU6050_POWER;
+        list[Gyro].minDelay = GYRO_MPU6050_MINDELAY;
+    } else if( gyro != NULL && strcmp(gyro, "MPU6500") == 0) {
+        list[Gyro].maxRange = GYRO_MPU6500_RANGE;
+        list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
+        list[Gyro].power = GYRO_MPU6500_POWER;
+        list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
+     } else if( gyro != NULL && strcmp(gyro, "MPU6515") == 0) {
+        list[Gyro].maxRange = GYRO_MPU6500_RANGE;
+        list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
+        list[Gyro].power = GYRO_MPU6500_POWER;
+        list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
+    } else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) {
+        list[Gyro].maxRange = GYRO_MPU9150_RANGE;
+        list[Gyro].resolution = GYRO_MPU9150_RESOLUTION;
+        list[Gyro].power = GYRO_MPU9150_POWER;
+        list[Gyro].minDelay = GYRO_MPU9150_MINDELAY;
+    } else {
+        LOGE("HAL:unknown gyro id -- gyro params will be wrong.");
+        LOGE("HAL:default to use mpu3050 params");
+        list[Gyro].maxRange = GYRO_MPU3050_RANGE;
+        list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
+        list[Gyro].power = GYRO_MPU3050_POWER;
+        list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
+    }
+
+    list[RawGyro].maxRange = list[Gyro].maxRange;
+    list[RawGyro].resolution = list[Gyro].resolution;
+    list[RawGyro].power = list[Gyro].power;
+    list[RawGyro].minDelay = list[Gyro].minDelay;
+
+    return;
+}
+
+/* fillRV depends on values of gyro, accel and compass in the list */
+void MPLSensor::fillRV(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    /* compute power on the fly */
+    list[RotationVector].power = list[Gyro].power +
+                                 list[Accelerometer].power +
+                                 list[MagneticField].power;
+    list[RotationVector].resolution = .00001;
+    list[RotationVector].maxRange = 1.0;
+    list[RotationVector].minDelay = 5000;
+
+    return;
+}
+
+/* fillGMRV depends on values of accel and mag in the list */
+void MPLSensor::fillGMRV(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    /* compute power on the fly */
+    list[GeomagneticRotationVector].power = list[Accelerometer].power +
+                                 list[MagneticField].power;
+    list[GeomagneticRotationVector].resolution = .00001;
+    list[GeomagneticRotationVector].maxRange = 1.0;
+    list[GeomagneticRotationVector].minDelay = 5000;
+
+    return;
+}
+
+/* fillGRV depends on values of gyro and accel in the list */
+void MPLSensor::fillGRV(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    /* compute power on the fly */
+    list[GameRotationVector].power = list[Gyro].power +
+                                 list[Accelerometer].power;
+    list[GameRotationVector].resolution = .00001;
+    list[GameRotationVector].maxRange = 1.0;
+    list[GameRotationVector].minDelay = 5000;
+
+    return;
+}
+
+void MPLSensor::fillOrientation(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    list[Orientation].power = list[Gyro].power +
+                              list[Accelerometer].power +
+                              list[MagneticField].power;
+    list[Orientation].resolution = .00001;
+    list[Orientation].maxRange = 360.0;
+    list[Orientation].minDelay = 5000;
+
+    return;
+}
+
+void MPLSensor::fillGravity( struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    list[Gravity].power = list[Gyro].power +
+                          list[Accelerometer].power +
+                          list[MagneticField].power;
+    list[Gravity].resolution = .00001;
+    list[Gravity].maxRange = 9.81;
+    list[Gravity].minDelay = 5000;
+
+    return;
+}
+
+void MPLSensor::fillLinearAccel(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    list[LinearAccel].power = list[Gyro].power +
+                          list[Accelerometer].power +
+                          list[MagneticField].power;
+    list[LinearAccel].resolution = list[Accelerometer].resolution;
+    list[LinearAccel].maxRange = list[Accelerometer].maxRange;
+    list[LinearAccel].minDelay = 5000;
+
+    return;
+}
+
+void MPLSensor::fillSignificantMotion(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    list[SignificantMotion].power = list[Accelerometer].power;
+    list[SignificantMotion].resolution = 1;
+    list[SignificantMotion].maxRange = 1;
+    list[SignificantMotion].minDelay = -1;
+}
+
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+void MPLSensor::fillScreenOrientation(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    list[NumSensors].power = list[Accelerometer].power;
+    list[NumSensors].resolution = 1;
+    list[NumSensors].maxRange = 3;
+    list[NumSensors].minDelay = 0;
+}
+#endif
+
+int MPLSensor::inv_init_sysfs_attributes(void)
+{
+    VFUNC_LOG;
+
+    unsigned char i = 0;
+    char sysfs_path[MAX_SYSFS_NAME_LEN];
+    char tbuf[2];
+    char *sptr;
+    char **dptr;
+    int num;
+
+    memset(sysfs_path, 0, sizeof(sysfs_path));
+
+    sysfs_names_ptr =
+            (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
+    sptr = sysfs_names_ptr;
+    if (sptr != NULL) {
+        dptr = (char**)&mpu;
+        do {
+            *dptr++ = sptr;
+            memset(sptr, 0, sizeof(sptr));
+            sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
+        } while (++i < MAX_SYSFS_ATTRB);
+    } else {
+        LOGE("HAL:couldn't alloc mem for sysfs paths");
+        return -1;
+    }
+
+    // get proper (in absolute) IIO path & build MPU's sysfs paths
+    inv_get_sysfs_path(sysfs_path);
+
+    if (strcmp(sysfs_path, "") == 0)
+        return 0;
+
+    memcpy(mSysfsPath, sysfs_path, sizeof(sysfs_path));
+    sprintf(mpu.key, "%s%s", sysfs_path, "/key");
+    sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable");
+    sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length");
+    sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
+
+    sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path,
+            "/scan_elements/in_timestamp_en");
+    sprintf(mpu.in_timestamp_index, "%s%s", sysfs_path,
+            "/scan_elements/in_timestamp_index");
+    sprintf(mpu.in_timestamp_type, "%s%s", sysfs_path,
+            "/scan_elements/in_timestamp_type");
+
+    sprintf(mpu.dmp_firmware, "%s%s", sysfs_path, "/dmp_firmware");
+    sprintf(mpu.firmware_loaded, "%s%s", sysfs_path, "/firmware_loaded");
+    sprintf(mpu.dmp_on, "%s%s", sysfs_path, "/dmp_on");
+    sprintf(mpu.dmp_int_on, "%s%s", sysfs_path, "/dmp_int_on");
+    sprintf(mpu.dmp_event_int_on, "%s%s", sysfs_path, "/dmp_event_int_on");
+    sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
+
+    sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
+
+    sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature");
+    sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable");
+    sprintf(mpu.gyro_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
+    sprintf(mpu.gyro_orient, "%s%s", sysfs_path, "/gyro_matrix");
+    sprintf(mpu.gyro_fifo_enable, "%s%s", sysfs_path, "/gyro_fifo_enable");
+    sprintf(mpu.gyro_fsr, "%s%s", sysfs_path, "/in_anglvel_scale");
+    sprintf(mpu.gyro_fifo_enable, "%s%s", sysfs_path, "/gyro_fifo_enable");
+    sprintf(mpu.gyro_rate, "%s%s", sysfs_path, "/gyro_rate");
+
+    sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accel_enable");
+    sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
+    sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accel_matrix");
+    sprintf(mpu.accel_fifo_enable, "%s%s", sysfs_path, "/accel_fifo_enable");
+    sprintf(mpu.accel_rate, "%s%s", sysfs_path, "/accel_rate");
+
+#ifndef THIRD_PARTY_ACCEL //MPUxxxx
+    sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale");
+
+    // DMP uses these values
+    sprintf(mpu.in_accel_x_dmp_bias, "%s%s", sysfs_path, "/in_accel_x_dmp_bias");
+    sprintf(mpu.in_accel_y_dmp_bias, "%s%s", sysfs_path, "/in_accel_y_dmp_bias");
+    sprintf(mpu.in_accel_z_dmp_bias, "%s%s", sysfs_path, "/in_accel_z_dmp_bias");
+
+    // MPU bias value is not set on purpose -
+    // raw accel value with factory cal is not requested
+#endif
+
+    // DMP uses these bias values
+    sprintf(mpu.in_gyro_x_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_x_dmp_bias");
+    sprintf(mpu.in_gyro_y_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_y_dmp_bias");
+    sprintf(mpu.in_gyro_z_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_z_dmp_bias");
+
+    // MPU uses these bias values
+    sprintf(mpu.in_gyro_x_offset, "%s%s", sysfs_path, "/in_anglvel_x_offset");
+    sprintf(mpu.in_gyro_y_offset, "%s%s", sysfs_path, "/in_anglvel_y_offset");
+    sprintf(mpu.in_gyro_z_offset, "%s%s", sysfs_path, "/in_anglvel_z_offset");
+    sprintf(mpu.in_gyro_self_test_scale, "%s%s", sysfs_path, "/in_anglvel_self_test_scale");
+
+    sprintf(mpu.three_axis_q_on, "%s%s", sysfs_path, "/three_axes_q_on"); //formerly quaternion_on
+    sprintf(mpu.three_axis_q_rate, "%s%s", sysfs_path, "/three_axes_q_rate");
+    
+    sprintf(mpu.ped_q_on, "%s%s", sysfs_path, "/ped_q_on");
+    sprintf(mpu.ped_q_rate, "%s%s", sysfs_path, "/ped_q_rate");
+    
+    sprintf(mpu.six_axis_q_on, "%s%s", sysfs_path, "/six_axes_q_on");
+    sprintf(mpu.six_axis_q_rate, "%s%s", sysfs_path, "/six_axes_q_rate");
+
+    sprintf(mpu.step_detector_on, "%s%s", sysfs_path, "/step_detector_on");
+    sprintf(mpu.step_indicator_on, "%s%s", sysfs_path, "/step_indicator_on");
+
+    sprintf(mpu.display_orientation_on, "%s%s", sysfs_path,
+            "/display_orientation_on");
+    sprintf(mpu.event_display_orientation, "%s%s", sysfs_path,
+            "/event_display_orientation");
+
+    sprintf(mpu.event_smd, "%s%s", sysfs_path,
+            "/event_smd");
+    sprintf(mpu.smd_enable, "%s%s", sysfs_path,
+            "/smd_enable");
+    sprintf(mpu.smd_delay_threshold, "%s%s", sysfs_path,
+            "/smd_delay_threshold");
+    sprintf(mpu.smd_delay_threshold2, "%s%s", sysfs_path,
+            "/smd_delay_threshold2");
+    sprintf(mpu.smd_threshold, "%s%s", sysfs_path,
+            "/smd_threshold");
+    sprintf(mpu.batchmode_timeout, "%s%s", sysfs_path,
+            "/batchmode_timeout");
+    sprintf(mpu.batchmode_wake_fifo_full_on, "%s%s", sysfs_path,
+            "/batchmode_wake_fifo_full_on");
+    sprintf(mpu.pedometer_on, "%s%s", sysfs_path,
+            "/pedometer_on");
+    sprintf(mpu.pedometer_int_on, "%s%s", sysfs_path,
+            "/pedometer_int_on");
+    sprintf(mpu.event_pedometer, "%s%s", sysfs_path,
+            "/event_pedometer");
+    sprintf(mpu.pedometer_steps, "%s%s", sysfs_path,
+            "/pedometer_steps");
+    return 0;
+}
+
+bool MPLSensor::isMpuNonDmp(void)
+{
+    if (!strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050"))
+        return true;
+    else
+        return false;
+}
+
+int MPLSensor::isLowPowerQuatEnabled(void)
+{
+#ifdef ENABLE_LP_QUAT_FEAT
+    return !isMpuNonDmp();
+#else
+    return 0;
+#endif
+}
+
+int MPLSensor::isDmpDisplayOrientationOn(void)
+{
+#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
+    if (isMpuNonDmp())
+        return 0;
+    return 1;
+#else
+    return 0;
+#endif
+}
+
+/* these functions can be consolidated
+with inv_convert_to_body_with_scale */
+void MPLSensor::getCompassBias()
+{
+    VFUNC_LOG;
+
+    
+    long bias[3];
+    long compassBias[3];
+    unsigned short orient;
+    signed char orientMtx[9];
+    mCompassSensor->getOrientationMatrix(orientMtx);
+    orient = inv_orientation_matrix_to_scalar(orientMtx);
+    /* Get Values from MPL */
+    inv_get_compass_bias(bias);
+    //inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output);
+    inv_convert_to_body(orient, bias, compassBias);
+    LOGV_IF(HANDLER_DATA, "Mpl Compass Bias (HW unit) %ld %ld %ld", bias[0], bias[1], bias[2]);
+    LOGV_IF(HANDLER_DATA, "Mpl Compass Bias (HW unit) (body) %ld %ld %ld", compassBias[0], compassBias[1], compassBias[2]);
+    long compassSensitivity = inv_get_compass_sensitivity();
+    if (compassSensitivity == 0) {
+        compassSensitivity = mCompassScale;
+    }
+    for(int i=0; i<3; i++) {
+        /* convert to uT */
+        float temp = (float) compassSensitivity / (1L << 30);
+        mCompassBias[i] =(float) (compassBias[i] * temp / 65536.f);
+    }
+
+    return; 
+}
+
+void MPLSensor::getFactoryGyroBias()
+{
+    VFUNC_LOG;
+
+    //TODO: mllite needs to add this function
+    //if(inv_factory_bias_available) {
+        /* Get Values from MPL */
+        inv_get_gyro_bias(mFactoryGyroBias);
+        LOGV_IF(ENG_VERBOSE, "Factory Gyro Bias %ld %ld %ld", mFactoryGyroBias[0], mFactoryGyroBias[1], mFactoryGyroBias[2]);
+        mFactoryGyroBiasAvailable = true;
+    //}
+
+    return;
+}
+
+/* set bias from factory cal file to MPU offset
+/* x = values store in cal file --> (v/1000 * 2^16 / (2000/250))
+/* offset = x/2^16 * (Gyro scale / self test scale used) * (-1) / offset scale
+/* i.e. self test default scale = 250 
+/*       gyro scale default to = 2000
+/*       offset scale = 4 //as spec by hardware
+/*       offset = x/2^16 * (8) * (-1) / (4)
+*/
+
+void MPLSensor::setFactoryGyroBias()
+{
+    VFUNC_LOG;
+    int scaleRatio = mGyroScale / mGyroSelfTestScale;
+    int offsetScale = 4;
+    LOGV_IF(PROCESS_VERBOSE, "HAL: scaleRatio used =%d", scaleRatio);
+    LOGV_IF(PROCESS_VERBOSE, "HAL: offsetScale used =%d", offsetScale);
+
+    /* Write to Driver */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            (((int) (((float) mFactoryGyroBias[0]) / 65536.f * scaleRatio)) * -1 / offsetScale),
+            mpu.in_gyro_x_offset, getTimestamp());
+    if(write_attribute_sensor_continuous(gyro_x_offset_fd,
+        (((int) (((float) mFactoryGyroBias[0]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0)
+    {
+        LOGE("HAL:Error writing to gyro_x_offset");
+        return;
+    }
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            (((int) (((float) mFactoryGyroBias[1]) / 65536.f * scaleRatio)) * -1 / offsetScale),
+            mpu.in_gyro_y_offset, getTimestamp());
+    if(write_attribute_sensor_continuous(gyro_y_offset_fd,
+        (((int) (((float) mFactoryGyroBias[1]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0)
+    {
+        LOGE("HAL:Error writing to gyro_y_offset");
+        return;
+    }
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            (((int) (((float) mFactoryGyroBias[2]) / 65536.f * scaleRatio)) * -1 / offsetScale),
+            mpu.in_gyro_z_offset, getTimestamp());
+    if(write_attribute_sensor_continuous(gyro_z_offset_fd,
+        (((int) (((float) mFactoryGyroBias[2]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0)
+    {
+        LOGE("HAL:Error writing to gyro_z_offset");
+        return;
+    }
+    mFactoryGyroBiasAvailable = false;
+    LOGV_IF(EXTRA_VERBOSE, "HAL:Factory Gyro Calibrated Bias Applied");
+
+    return;
+}
+
+/* these functions can be consolidated
+with inv_convert_to_body_with_scale */
+void MPLSensor::getGyroBias()
+{
+    VFUNC_LOG;
+
+    long *temp = NULL;
+    long chipBias[3];
+    long bias[3];
+    unsigned short orient;
+
+    /* Get Values from MPL */
+    inv_get_mpl_gyro_bias(mGyroChipBias, temp);
+    orient = inv_orientation_matrix_to_scalar(mGyroOrientation);
+    //void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output);
+    inv_convert_to_body(orient, mGyroChipBias, bias);
+    LOGV_IF(ENG_VERBOSE, "Mpl Gyro Bias (HW unit) %ld %ld %ld", mGyroChipBias[0], mGyroChipBias[1], mGyroChipBias[2]);
+    LOGV_IF(ENG_VERBOSE, "Mpl Gyro Bias (HW unit) (body) %ld %ld %ld", bias[0], bias[1], bias[2]);
+    long gyroSensitivity = inv_get_gyro_sensitivity();
+    if(gyroSensitivity == 0) {
+        gyroSensitivity = mGyroScale;
+    }
+
+    /* scale and convert to rad */
+    for(int i=0; i<3; i++) {
+        float temp = (float) gyroSensitivity / (1L << 30);
+        mGyroBias[i] = (float) (bias[i] * temp / (1<<16) / 180 * M_PI);
+        if (mGyroBias[i] != 0)
+            mGyroBiasAvailable = true;
+    }
+
+    return;
+}
+
+void MPLSensor::setGyroBias()
+{
+    VFUNC_LOG;
+
+    if(mGyroBiasAvailable == false)
+        return;
+
+    long bias[3];
+    long gyroSensitivity = inv_get_gyro_sensitivity();
+
+    if(gyroSensitivity == 0) {
+        gyroSensitivity = mGyroScale;
+    }
+    
+    inv_get_gyro_bias_dmp_units(bias);
+
+    /* Write to Driver */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
+            bias[0], mpu.in_gyro_x_dmp_bias, getTimestamp());
+    if(write_attribute_sensor_continuous(gyro_x_dmp_bias_fd, bias[0]) < 0)
+    {
+        LOGE("HAL:Error writing to gyro_x_dmp_bias");
+        return;
+    }
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
+            bias[1], mpu.in_gyro_y_dmp_bias, getTimestamp());
+    if(write_attribute_sensor_continuous(gyro_y_dmp_bias_fd, bias[1]) < 0)
+    {
+        LOGE("HAL:Error writing to gyro_y_dmp_bias");
+        return;
+    }
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
+            bias[2], mpu.in_gyro_z_dmp_bias, getTimestamp());
+    if(write_attribute_sensor_continuous(gyro_z_dmp_bias_fd, bias[2]) < 0)
+    {
+        LOGE("HAL:Error writing to gyro_z_dmp_bias");
+        return;
+    }
+    mGyroBiasApplied = true;
+    mGyroBiasAvailable = false;
+    LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro DMP Calibrated Bias Applied");
+
+    return;
+}
+
+void MPLSensor::getFactoryAccelBias()
+{
+    VFUNC_LOG;
+
+    long temp;
+
+    /* Get Values from MPL */
+    inv_get_accel_bias(mAccelBias, &temp);
+    LOGV_IF(HANDLER_DATA, "Factory Accel Bias (mg) %ld %ld %ld", mFactoryAccelBias[0], mFactoryAccelBias[1], mFactoryAccelBias[2]);
+    mFactoryAccelBiasAvailable = true;
+    
+    return;
+}
+
+void MPLSensor::setFactoryAccelBias()
+{
+    if(mFactoryAccelBiasAvailable == false)
+        return;
+
+    /* TODO: add scaling here - depends on self test parameters */
+
+    /* Write to Driver */
+    if(write_attribute_sensor_continuous(accel_x_offset_fd, mFactoryAccelBias[0]) < 0)
+    {
+        LOGE("HAL:Error writing to accel_x_offset");
+        return;
+    }
+    if(write_attribute_sensor_continuous(accel_y_offset_fd, mFactoryAccelBias[1]) < 0)
+    {
+        LOGE("HAL:Error writing to accel_y_offset");
+        return;
+    }
+    if(write_attribute_sensor_continuous(accel_z_offset_fd, mFactoryAccelBias[2]) < 0)
+    {
+        LOGE("HAL:Error writing to accel_z_offset");
+        return;
+    }
+    mFactoryAccelBiasAvailable = false;
+    LOGV_IF(EXTRA_VERBOSE, "HAL:Factory Accel Calibrated Bias Applied");
+
+    return;
+}
+
+void MPLSensor::getAccelBias()
+{
+    VFUNC_LOG;
+    long temp;
+
+    /* Get Values from MPL */
+    inv_get_accel_bias(mAccelBias, &temp);
+    LOGV_IF(ENG_VERBOSE, "Accel Bias (mg) %ld %ld %ld", mAccelBias[0], mAccelBias[1], mAccelBias[2]);
+    mAccelBiasAvailable = true;
+
+    return;
+}
+
+void MPLSensor::setAccelBias()
+{
+    VFUNC_LOG;
+
+    if(mAccelBiasAvailable == false)
+        return;
+
+    long bias[3];
+    unsigned short orient = inv_orientation_matrix_to_scalar(mAccelOrientation);
+    inv_convert_to_body(orient, mAccelBias, bias);
+
+    /* Write to Driver */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
+           (long) (mAccelBias[0] / 65536.f / 2), mpu.in_accel_x_dmp_bias, getTimestamp());
+    if(write_attribute_sensor_continuous(accel_x_dmp_bias_fd, (long)(mAccelBias[0] / 65536.f / 2)) < 0)
+    {
+        LOGE("HAL:Error writing to accel_x_dmp_bias");
+        return;
+    }
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
+            (long)(mAccelBias[1] / 65536.f / 2), mpu.in_accel_y_dmp_bias, getTimestamp());
+    if(write_attribute_sensor_continuous(accel_y_dmp_bias_fd, (long)(mAccelBias[1] / 65536.f / 2)) < 0)
+    {
+        LOGE("HAL:Error writing to accel_y_dmp_bias");
+        return;
+    }
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
+            (long)(mAccelBias[2] / 65536 / 2), mpu.in_accel_z_dmp_bias, getTimestamp());
+    if(write_attribute_sensor_continuous(accel_z_dmp_bias_fd, (long)(mAccelBias[2] / 65536 / 2)) < 0)
+    {
+        LOGE("HAL:Error writing to accel_z_dmp_bias");
+        return;
+    }
+    mAccelBiasAvailable = false;
+    mAccelBiasApplied = true;
+    LOGV_IF(EXTRA_VERBOSE, "HAL:Accel DMP Calibrated Bias Applied");
+
+    return;
+}
+
+int MPLSensor::isCompassDisabled(void)
+{
+    if(mCompassSensor->getFd() < 0 && !mCompassSensor->isIntegrated()) {
+    LOGI_IF(EXTRA_VERBOSE, "HAL: Compass is disabled, Six-axis Sensor Fusion is used.");
+        return 1;
+    }
+    return 0;
+}
+
+/* precondition: framework disallows this case, ie enable continuous sensor, */
+/* and enable batch sensor */
+/* if one sensor is in continuous mode, HAL disallows enabling batch for this sensor */
+/* or any other sensors */
+#define DEBUG_BATCHING  (1)
+int MPLSensor::batch(int handle, int flags, int64_t period_ns, int64_t timeout)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    if (isMpuNonDmp())
+        return res;
+
+    /* Enables batch mode and sets timeout for the given sensor */
+    /* enum SENSORS_BATCH_DRY_RUN, SENSORS_BATCH_WAKE_UPON_FIFO_FULL */
+    bool dryRun = false;
+    android::String8 sname;
+    int what = -1;
+    int enabled_sensors = mEnabled;
+    int batchMode = timeout > 0 ? 1 : 0;
+
+    LOGI_IF(DEBUG_BATCHING || ENG_VERBOSE,
+            "HAL:batch called - handle=%d, flags=%d, period=%lld, timeout=%lld",
+            handle, flags, period_ns, timeout);
+
+    if(flags & (1 << SENSORS_BATCH_DRY_RUN)) {
+        dryRun = true;
+        LOGI_IF(PROCESS_VERBOSE,
+                "HAL:batch - dry run mode is set (%d)", SENSORS_BATCH_DRY_RUN);
+    }
+
+    getHandle(handle, what, sname);
+    if(uint32_t(what) >= NumSensors) {
+        LOGE("HAL:batch sensors %d not found", what);
+        return -EINVAL;
+    }
+
+    int tempBatch = 0;
+    if (timeout > 0) { 
+        tempBatch = mBatchEnabled | (1 << what);
+    } else {
+        tempBatch = mBatchEnabled & ~(1 << what);
+    }
+
+    if (!computeBatchSensorMask(mEnabled, tempBatch)) {
+        batchMode = 0;
+    } else {
+        batchMode = 1;
+    }
+    
+    /* Supported sensors: Accel, Gyro, Raw Gyro, Compass, Raw Compass, GRV, Step Detector */
+    switch (what) {
+    case Orientation:
+    case LinearAccel:
+    case Gravity:
+    case RotationVector:
+    case GeomagneticRotationVector:
+    case SignificantMotion:
+    case ID_SC:
+    case ID_SO:
+        LOGE("HAL:batch - "
+             "sensor (handle %d) is not supported in batch mode", handle);
+        return -EINVAL;
+    }
+    
+    /* get maximum possible bytes to batch per sample */
+    /* get minimum delay for each requested sensor    */
+    ssize_t nBytes = 0;
+    int64_t wanted = 1000000000LL, ns = 0;
+    int64_t timeoutInMs = 0;
+    for (int i = 0; i < NumSensors; i++) {
+        if (batchMode == 1) {
+            ns = mBatchDelays[i];
+            LOGV_IF(DEBUG_BATCHING || EXTRA_VERBOSE,
+                    "HAL:batch - requested sensor=0x%01x, batch delay=%lld", mEnabled & (1 << i), ns);
+            // take the min delay ==> max rate
+            wanted = (ns < wanted) ? ns : wanted;
+            if (i <= RawMagneticField) {
+                nBytes += 8;
+            }
+            if (i == Pressure) {
+                nBytes += 6;
+            }
+            if ((i == StepDetector) || (i == GameRotationVector)) {
+                nBytes += 16;
+            }
+        }
+    }
+    
+    /* check if we can support issuing interrupt before FIFO fills-up */
+    /* in the given timeout.                                          */
+    if ((flags & (1 << SENSORS_BATCH_WAKE_UPON_FIFO_FULL)) &&
+            (batchMode == 1)) {
+            LOGE("HAL: batch SENSORS_BATCH_WAKE_UPON_FIFO_FULL is not supported");
+            return -EINVAL;
+        /* provide messge if it exceeds hardware capability
+        if (nSamples * nBytes >= 1024) {
+            LOGE("HAL:batch - timeout - configuration is not supported, "
+                 "cannot provide requested amount of buffering (%lld ms)",
+                 timeout / 1000000LL);
+        }*/
+    }
+
+    if(dryRun == true)
+        return 0;
+
+    /* starting from code below,  we will modify hardware */
+    /* first edit global batch mode mask */
+
+    if (!timeout) {
+        mBatchEnabled &= ~(1 << what);
+        mBatchDelays[what] = 1000000000L;
+        mBatchTimeouts[what] = 30000000000LL;
+        if (mBatchEnabled == 0) {
+            mFeatureActiveMask &= ~INV_DMP_BATCH_MODE;
+        }
+    } else {
+        mBatchEnabled |= (1 << what);
+        mBatchDelays[what] = period_ns;
+        mBatchTimeouts[what] = timeout;
+        mFeatureActiveMask |= INV_DMP_BATCH_MODE;
+    }
+
+    /* For these sensors, switch to different data output */
+    /* These steps should be optimized at some point */
+    int featureMask = computeBatchDataOutput();
+
+    LOGV_IF(ENG_VERBOSE, "batchMode =%d, featureMask=0x%x, mEnabled=%d",
+                batchMode, featureMask, mEnabled);
+    if (DEBUG_BATCHING || EXTRA_VERBOSE) {
+        LOGV("HAL:batch - sensor=0x%01x", mBatchEnabled);
+        for (int d = 0; d < NumSensors; d++) {            
+            LOGV("HAL:batch - sensor status=0x%01x batch status=0x%01x timeout=%lld delay=%lld",
+                            mEnabled & (1 << d), (mBatchEnabled & (1 << d)), mBatchTimeouts[d],
+                            mBatchDelays[d]);
+        }
+    }
+
+    /* take the minimum batchmode timeout */
+    if (batchMode == 1) {
+        int64_t tempTimeout = 30000000000LL;
+        for (int i = 0; i < NumSensors; i++) {        
+            if ((mEnabled & (1 << i) && mBatchEnabled & (1 << i)) ||
+                    (((featureMask & INV_DMP_PED_STANDALONE) && (mBatchEnabled & (1 << StepDetector))))) {
+                LOGV_IF(ENG_VERBOSE, "i=%d, timeout=%lld", i, mBatchTimeouts[i]);
+                ns = mBatchTimeouts[i];
+                tempTimeout = (ns < tempTimeout) ? ns : tempTimeout;
+            }
+        }
+        timeout = tempTimeout;
+        /* Convert ns to millisecond */
+        timeoutInMs = timeout / 1000000;
+
+        /* remember last timeout value */
+        mBatchTimeoutInMs = timeoutInMs;
+        
+        /* TODO: Calculate nSamples */
+        int nSamples = 0;
+        nSamples = (unsigned long)(
+            (1000000000.f / wanted) * ((float)timeout / 1000000000.f));
+    } else {
+        timeoutInMs = 0;    
+    }
+    
+    LOGV_IF(DEBUG_BATCHING || EXTRA_VERBOSE,
+                "HAL:batch - timeout - timeout=%lld ns, timeoutInMs=%lld, delay=%lld ns",
+                timeout, timeoutInMs, wanted);
+
+    // reset master enable
+    res = masterEnable(0);
+    if (res < 0) {
+        return res;
+    }
+
+    /* case for Ped standalone */
+    if ((batchMode == 1) && (featureMask & INV_DMP_PED_STANDALONE) &&
+           (mFeatureActiveMask & INV_DMP_PEDOMETER)) {
+        LOGI("ID_P only = 0x%x", mBatchEnabled);
+        enablePedQuaternion(0);
+        enablePedStandalone(1);
+    } else {
+        enablePedStandalone(0);
+        if (featureMask & INV_DMP_PED_QUATERNION) {
+            enableLPQuaternion(0);
+            enablePedQuaternion(1);
+        }
+    }
+
+    /* case for Ped Quaternion */
+    if ((batchMode == 1) && (featureMask & INV_DMP_PED_QUATERNION) &&
+            (mEnabled & (1 << GameRotationVector)) &&
+            (mFeatureActiveMask & INV_DMP_PEDOMETER)) {
+        LOGI("ID_P and GRV or ALL = 0x%x", mBatchEnabled);        
+        LOGI("ID_P is enabled for batching, PED quat will be automatically enabled");
+        enableLPQuaternion(0);
+        enablePedQuaternion(1);
+        
+        wanted = mBatchDelays[GameRotationVector];
+        /* set pedq rate */               
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                int(1000000000.f / wanted), mpu.ped_q_rate,
+                getTimestamp());
+        write_sysfs_int(mpu.ped_q_rate, 1000000000.f / wanted);
+        LOGV_IF(PROCESS_VERBOSE,
+                "HAL:DMP ped quaternion rate %.2f Hz", 1000000000.f / wanted);
+    } else if (!(featureMask & INV_DMP_PED_STANDALONE)){
+        LOGI("Toggle back to normal 6 axis");
+        if (mEnabled & (1 << GameRotationVector)) {
+            enableLPQuaternion(1);
+        }        
+        enablePedQuaternion(0);
+    }
+
+     /* case for Ped indicator */
+    if ((batchMode == 1) && ((featureMask & INV_DMP_PED_INDICATOR))) {
+        enablePedIndicator(1);
+    } else {
+        enablePedIndicator(0);
+    }    
+
+    /* case for Six Axis Quaternion */
+    if ((batchMode == 1) && (featureMask & INV_DMP_6AXIS_QUATERNION) &&
+            (mEnabled & (1 << GameRotationVector))) {
+        LOGI("GRV = 0x%x", mBatchEnabled);
+        enableLPQuaternion(0);
+        enable6AxisQuaternion(1);
+
+        wanted = mBatchDelays[GameRotationVector];
+        /* set sixaxis rate */
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                int(1000000000.f / wanted), mpu.six_axis_q_rate,
+                getTimestamp());
+        write_sysfs_int(mpu.six_axis_q_rate, 1000000000.f / wanted);
+        LOGV_IF(PROCESS_VERBOSE,
+                "HAL:DMP three axis rate %.2f Hz", 1000000000.f / wanted);
+    } else if (!(featureMask & INV_DMP_PED_QUATERNION)){
+        LOGI("Toggle back to normal 6 axis");
+        if (mEnabled & (1 << GameRotationVector)) {
+            enableLPQuaternion(1);
+        }
+        enable6AxisQuaternion(0);
+    } else {
+        enable6AxisQuaternion(0);
+    }
+
+    /* TODO: This may make a come back some day */
+    /* write not to overflow hardware FIFO if flag is set */
+    /*if (flags & (1 << SENSORS_BATCH_WAKE_UPON_FIFO_FULL)) {
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                0, mpu.batchmode_wake_fifo_full_on, getTimestamp());
+        if (write_sysfs_int(mpu.batchmode_wake_fifo_full_on, 0) < 0) {
+            LOGE("HAL:ERR can't write batchmode_wake_fifo_full_on");
+        }
+    }*/
+
+    /* write required timeout to sysfs */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %lld > %s (%lld)",
+            timeoutInMs, mpu.batchmode_timeout, getTimestamp());
+    if (write_sysfs_int(mpu.batchmode_timeout, timeoutInMs) < 0) {
+        LOGE("HAL:ERR can't write batchmode_timeout");
+    }
+
+    if (batchMode == 1) {
+        // enable DMP
+        res = onDmp(1);
+        if (res < 0) {
+            return res;
+        }
+        // set batch rates
+        if (setBatchDataRates() < 0) {
+            LOGE("HAL:ERR can't set batch data rates");
+        }                  
+        // default fifo rate to 200Hz
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                200, mpu.gyro_fifo_rate, getTimestamp());
+        if (write_sysfs_int(mpu.gyro_fifo_rate, 200) < 0) {
+            res = -1;
+            LOGE("HAL:ERR can't set DMP rate to 200Hz");
+            return res;
+        }
+    } else {
+        if ((mFeatureActiveMask == 0) && !(mEnabled & VIRTUAL_SENSOR_ALL_MASK)) {
+            // disable DMP
+            res = onDmp(0);
+            if (res < 0) {
+                return res;
+            }
+        }
+        /* reset sensor rate */
+        /*if (resetDataRates() < 0) {
+            LOGE("HAL:ERR can't reset output rate back to original setting");
+        }*/
+    }
+    if ((batchMode == 1) || enabled_sensors || mFeatureActiveMask) {
+        masterEnable(1);
+    }
+    return res;
+}
+
+int MPLSensor::computeBatchDataOutput()
+{
+    VFUNC_LOG;
+    
+    int featureMask = 0;
+    if (mBatchEnabled == 0)
+        return 0;//h
+    
+    uint32_t hardwareSensorMask = (1<<Gyro) | (1<<RawGyro) | (1<<Accelerometer) | (1<<MagneticField) |
+                                        (1<<RawMagneticField) | (1<<Pressure);
+    LOGV_IF(ENG_VERBOSE, "hardwareSensorMask = 0x%0x, mBatchEnabled = 0x%0x", hardwareSensorMask, mBatchEnabled);
+   
+    if (mBatchEnabled & (1 << StepDetector)) {         
+        if (mBatchEnabled & (1 << GameRotationVector)) {
+            if ((mBatchEnabled & hardwareSensorMask)) {
+                featureMask |= INV_DMP_6AXIS_QUATERNION;//a
+                featureMask |= INV_DMP_PED_INDICATOR;
+            } else {
+                featureMask |= INV_DMP_PED_QUATERNION; //b
+                featureMask |= INV_DMP_PED_INDICATOR;  //always piggy back a bit
+            }
+        } else {
+            if (mBatchEnabled & hardwareSensorMask) {
+                featureMask |= INV_DMP_PED_INDICATOR; //c
+            } else {
+                featureMask |= INV_DMP_PED_STANDALONE; //d
+                featureMask |= INV_DMP_PED_INDICATOR; //required for standalone
+            }
+        }
+    } else if (mBatchEnabled & (1 << GameRotationVector)) {            
+        featureMask |= INV_DMP_6AXIS_QUATERNION; //e,f            
+    } else {
+        LOGV_IF(ENG_VERBOSE, "HAL:computeBatchDataOutput: featuerMask=0x%x", featureMask);
+        return 0; //g
+    }
+
+    LOGV_IF(ENG_VERBOSE, "HAL:computeBatchDataOutput: featuerMask=0x%x", featureMask);
+    return featureMask;
+}
+
+int MPLSensor::getDmpPedometerFd()
+{
+    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getDmpPedometerFd returning %d",
+             dmp_pedometer_fd);
+    return dmp_pedometer_fd;
+}
+
+/* @param [in] : outputType = 1 --derive from ped_q */
+/* outputType = 0 --derive from IRQ          */
+int MPLSensor::readDmpPedometerEvents(sensors_event_t* data, int count,
+                                      int32_t id, int32_t type, int outputType)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+    char dummy[4];
+    FILE *fp;
+    uint64_t stepCount = 0;
+    int numEventReceived = 0;
+    int update = 0;
+
+    if((mDmpStepCountEnabled || mDmpPedometerEnabled) && count > 0) {
+        /* handles return event */
+        sensors_event_t temp;
+
+        LOGI_IF(EXTRA_VERBOSE, "HAL: Read Pedometer Event ID=%d", id);
+        temp.version = sizeof(sensors_event_t);
+        temp.sensor = id;
+        temp.type = type;
+        temp.acceleration.status
+            = SENSOR_STATUS_UNRELIABLE;
+        /* sensors.h specified to return 1.0 */
+        if(id == ID_P) {
+             temp.data[0] = 1;
+             temp.data[1] = 0.f;
+             temp.data[2] = 0.f;
+        } else {
+            fp = fopen(mpu.pedometer_steps, "r");
+            if (fp == NULL) {
+                LOGE("HAL:cannot open pedometer_steps");
+            } else{
+                if (fscanf(fp, "%lld\n", &stepCount) < 0 || fclose(fp) < 0) {
+                    LOGE("HAL:cannot read pedometer_steps");
+                }
+            }
+            /* return onChange only*/
+            if (stepCount == mLastStepCount) {
+                return 0;
+            }
+            temp.data[0] = stepCount;
+            temp.data[1] = 0.f;
+            temp.data[2] = 0.f;
+            mLastStepCount = stepCount;
+        }
+
+        if (!outputType) {
+            struct timespec ts;
+            clock_gettime(CLOCK_MONOTONIC, &ts) ;
+            temp.timestamp = (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec;
+        }
+
+        *data++ = temp;
+        count--;
+        numEventReceived++;
+    }
+
+    if (!outputType) {
+        // read dummy data per driver's request
+        // only required if actual irq is issued
+        read(dmp_pedometer_fd, dummy, 4);
+    } else {
+        return 1;
+    } 
+
+    return numEventReceived;
+}
+
+int MPLSensor::getDmpSignificantMotionFd()
+{
+    LOGV_IF(EXTRA_VERBOSE,
+            "MPLSensor::getDmpSignificantMotionFd returning %d",
+            dmp_sign_motion_fd);
+    return dmp_sign_motion_fd;
+}
+
+int MPLSensor::readDmpSignificantMotionEvents(sensors_event_t* data, int count) {
+    VFUNC_LOG;
+
+    int res = 0;
+    char dummy[4];
+    int significantMotion;
+    FILE *fp;
+    int sensors = mEnabled;
+    int numEventReceived = 0;
+    int update = 0;
+
+    /* Technically this step is not necessary for now  */
+    /* In the future, we may have meaningful values */
+    fp = fopen(mpu.event_smd, "r");
+    if (fp == NULL) {
+        LOGE("HAL:cannot open event_smd");
+        return 0;
+    }
+    fscanf(fp, "%d\n", &significantMotion);
+    fclose(fp);
+
+    if(mDmpSignificantMotionEnabled && count > 0) {
+       /* By implementation, smd is disabled once an event is triggered */
+        sensors_event_t temp;
+
+        /* Handles return event */
+        LOGI("HAL: SMD detected");
+        int update = smHandler(&temp);
+        if (update && count > 0) {
+            *data++ = temp;
+            count--;
+            numEventReceived++;
+            mDmpSignificantMotionEnabled = 0;
+            mFeatureActiveMask &= ~INV_DMP_SIGNIFICANT_MOTION;
+            if(mFeatureActiveMask == 0) {
+                LOGI("dmp off");
+                // disable DMP
+                masterEnable(0);
+                res = onDmp(0);
+                if (res < 0)
+                    return res;
+
+                // disable accel engine
+                if (!(mLocalSensorMask & mMasterSensorMask
+                        & INV_THREE_AXIS_ACCEL)) {
+                    res = enableAccel(0);
+                    if (res < 0)
+                        return res;
+                }
+            }
+            if(sensors != 0) {
+                 update_delay();
+                 masterEnable(1);
+            }
+        }
+    }
+
+    // read dummy data per driver's request
+    read(dmp_sign_motion_fd, dummy, 4);
+
+    return numEventReceived;
+}
+
+int MPLSensor::enableDmpSignificantMotion(int en)
+{
+    int res = 0;
+    VFUNC_LOG;
+
+    int enabled_sensors = mEnabled;
+
+    if (isMpuNonDmp())
+        return res;
+
+    // reset master enable
+    res = masterEnable(0);
+    if (res < 0)
+        return res;
+
+    //Toggle significant montion detection
+    if(en) {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling Significant Motion");
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                1, mpu.smd_enable, getTimestamp());
+        if (write_sysfs_int(mpu.smd_enable, 1) < 0) {
+            LOGE("HAL:ERR can't write DMP smd_enable");
+            res = -1;   //Indicate an err
+        }
+
+        // enable DMP
+        res = onDmp(1);
+        if (res < 0)
+            return res;
+
+        // set DMP rate to 200Hz
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                200, mpu.accel_fifo_rate, getTimestamp());
+        if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) {
+            res = -1;
+            LOGE("HAL:ERR can't set rate to 200Hz");
+            return res;
+        }
+
+        // enable accel engine
+        res = enableAccel(1);
+        if (res < 0)
+            return res;
+
+        // disable accel FIFO
+        if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) {
+            res = turnOffAccelFifo();
+            if (res < 0)
+                return res;
+        }
+        mFeatureActiveMask |= INV_DMP_SIGNIFICANT_MOTION;
+    }
+    else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling Significant Motion");
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                0, mpu.smd_enable, getTimestamp());
+        if (write_sysfs_int(mpu.smd_enable, 0) < 0) {
+            LOGE("HAL:ERR write DMP smd_enable");
+        }
+        mFeatureActiveMask &= ~INV_DMP_SIGNIFICANT_MOTION;
+        // disable DMP
+        if (mFeatureActiveMask == 0) {
+            res = onDmp(0);
+
+            if (res < 0)
+                return res;
+
+            // disable accel engine
+            if (!(mLocalSensorMask & mMasterSensorMask
+                    & INV_THREE_AXIS_ACCEL)) {
+                res = enableAccel(0);
+                if (res < 0)
+                    return res;
+            }
+        }
+        if(enabled_sensors) {
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                en, mpu.dmp_event_int_on, getTimestamp());
+            if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
+                res = -1;
+                LOGE("HAL:ERR can't enable DMP event interrupt");
+            }
+         }
+    }
+    if(en || enabled_sensors || mFeatureActiveMask) { 
+        res = masterEnable(1);
+    }
+    return res;
+}
+
+int MPLSensor::writeSignificantMotionParams(bool toggleEnable,
+                                            uint32_t delayThreshold1,
+                                            uint32_t delayThreshold2,
+                                            uint32_t motionThreshold)
+{
+    int res = 0;
+
+    // Turn off enable
+    if (toggleEnable) {
+        masterEnable(0);
+    }
+
+    // Write supplied values
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            delayThreshold1, mpu.smd_delay_threshold, getTimestamp());
+    res = write_sysfs_int(mpu.smd_delay_threshold, delayThreshold1);
+    if (res == 0) {
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                delayThreshold2, mpu.smd_delay_threshold2, getTimestamp());
+        res = write_sysfs_int(mpu.smd_delay_threshold2, delayThreshold2);
+    }
+    if (res == 0) {
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                motionThreshold, mpu.smd_threshold, getTimestamp());
+        res = write_sysfs_int(mpu.smd_threshold, motionThreshold);
+    }
+
+    // Turn on enable
+    if (toggleEnable) {
+        masterEnable(1);
+    }
+    return res;
+}
+
+/* set batch data rate */
+/* this function should be optimized */
+int MPLSensor::setBatchDataRates()
+{
+    VFUNC_LOG;
+
+    int res = 0;
+    int tempFd = -1;
+
+    int64_t gyroRate;
+    int64_t accelRate;
+    int64_t compassRate;
+    int64_t pressureRate;
+    
+    int mplGyroRate;
+    int mplAccelRate;
+    int mplCompassRate;
+
+#ifdef ENABLE_MULTI_RATE
+    /* take care of case where only one type of gyro sensors or compass sensors is turned on */
+    gyroRate = mBatchDelays[Gyro] < mBatchDelays[RawGyro] ? mBatchDelays[Gyro] : mBatchDelays[RawGyro];
+    accelRate = mBatchDelays[Accelerometer];
+    compassRate = mBatchDelays[MagneticField] < mBatchDelays[RawMagneticField] ? mBatchDelays[MagneticField] : mBatchDelays[RawMagneticField];
+    pressureRate = mBatchDelays[Pressure];
+
+    mplGyroRate = (int) gyroRate / 1000LL;
+    mplAccelRate = (int) accelRate / 1000LL;
+    mplCompassRate = (int) compassRate / 1000LL;
+    
+     /* set rate in MPL */
+     /* compass can only do 100Hz max */
+    inv_set_gyro_sample_rate(mplGyroRate);
+    inv_set_accel_sample_rate(mplAccelRate);
+    inv_set_compass_sample_rate(mplCompassRate);
+           
+    LOGV_IF(PROCESS_VERBOSE,
+            "HAL:MPL gyro sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplGyroRate, 1000000000.f / gyroRate);
+    LOGV_IF(PROCESS_VERBOSE,
+            "HAL:MPL accel sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplAccelRate, 1000000000.f / accelRate);
+    LOGV_IF(PROCESS_VERBOSE,
+            "HAL:MPL compass sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplCompassRate, 1000000000.f / compassRate);
+
+#else
+    /* search the minimum delay requested across all enabled sensors */
+    int64_t wanted = 1000000000LL;
+    for (int i = 1; i < NumSensors; i++) {
+        if (mBatchEnabled & (1 << i)) {
+            int64_t ns = mBatchDelays[i];
+            wanted = wanted < ns ? wanted : ns;
+        }
+    }
+    gyroRate = wanted;
+    accelRate = wanted;
+    compassRate = wanted;
+    pressureRate = wanted;
+#endif
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+            1000000000.f / gyroRate, mpu.gyro_rate,
+            getTimestamp());
+    tempFd = open(mpu.gyro_rate, O_RDWR);
+    res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate);
+    if(res < 0) {
+        LOGE("HAL:GYRO update delay error");
+    }
+    
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+            1000000000.f / accelRate, mpu.accel_rate,
+            getTimestamp());
+    tempFd = open(mpu.accel_rate, O_RDWR);
+    res = write_attribute_sensor(tempFd, 1000000000.f / accelRate);
+    LOGE_IF(res < 0, "HAL:ACCEL update delay error");
+   
+    if (compassRate < mCompassSensor->getMinDelay() * 1000LL) {
+        compassRate = mCompassSensor->getMinDelay() * 1000LL;
+    }
+    mCompassSensor->setDelay(ID_M, compassRate);
+ 
+    mPressureSensor->setDelay(ID_PS, pressureRate);
+   
+    return res;
+}
+
+/* Set sensor rate */
+/* this function should be optimized */
+int MPLSensor::resetDataRates()
+{
+    VFUNC_LOG;
+    
+    int res = 0;
+    int tempFd = -1;
+    int64_t wanted = 1000000000LL;
+
+    int64_t gyroRate;
+    int64_t accelRate;
+    int64_t compassRate;
+    int64_t pressureRate;
+    
+    /* enable this once driver supports multi-rate in dmp off mode */
+     
+    /* search the minimum delay requested across all enabled sensors */    
+    for (int i = 1; i < NumSensors; i++) {
+        if (mEnabled & (1 << i)) {
+            int64_t ns = mDelays[i];
+            wanted = wanted < ns ? wanted : ns;
+        }
+    }
+
+    if (mDmpOn == 1) {
+        gyroRate = mDelays[Gyro] < mDelays[RawGyro] ? mDelays[Gyro] : mDelays[RawGyro];
+        accelRate = mDelays[Accelerometer];
+        compassRate = mDelays[MagneticField] < mDelays[RawMagneticField] ? mDelays[MagneticField] : mDelays[RawMagneticField];
+        pressureRate = mDelays[Pressure];     
+
+#ifndef ENABLE_MULTI_RATE
+        gyroRate = wanted;
+        accelRate = wanted;
+        compassRate = wanted;
+        pressureRate = wanted; 
+#endif
+
+    } else { 
+        gyroRate = wanted;
+        accelRate = wanted;
+        compassRate = wanted;
+        pressureRate = wanted;
+    }
+
+    /* set mpl data rate */
+    inv_set_gyro_sample_rate((int)gyroRate/1000LL);
+    inv_set_accel_sample_rate((int)accelRate/1000LL);
+    inv_set_compass_sample_rate((int)compassRate/1000LL);
+
+   LOGV_IF(PROCESS_VERBOSE,
+            "HAL:MPL gyro sample rate: (mpl)=%lld us (mpu)=%.2f Hz", gyroRate/1000LL, 1000000000.f / gyroRate);
+   LOGV_IF(PROCESS_VERBOSE,
+            "HAL:MPL accel sample rate: (mpl)=%lld us (mpu)=%.2f Hz", accelRate/1000LL, 1000000000.f / accelRate);
+   LOGV_IF(PROCESS_VERBOSE,
+            "HAL:MPL compass sample rate: (mpl)=%lld us (mpu)=%.2f Hz", compassRate/1000LL, 1000000000.f / compassRate);
+
+    /* reset dmp rate */
+    getDmpRate (&wanted);
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+            1000000000.f / wanted, mpu.gyro_fifo_rate,
+            getTimestamp());
+    tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
+    res = write_attribute_sensor(tempFd, 1000000000.f / wanted);
+    LOGE_IF(res < 0, "HAL:sampling frequency update delay error");
+            
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+            1000000000.f / gyroRate, mpu.gyro_rate,
+            getTimestamp());
+    tempFd = open(mpu.gyro_rate, O_RDWR);
+    res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate);
+    if(res < 0) {
+        LOGE("HAL:GYRO update delay error");
+    }
+    
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+            1000000000.f / accelRate, mpu.accel_rate,
+            getTimestamp());
+    tempFd = open(mpu.accel_rate, O_RDWR);
+    res = write_attribute_sensor(tempFd, 1000000000.f / accelRate);
+    LOGE_IF(res < 0, "HAL:ACCEL update delay error");
+   
+    if (compassRate < mCompassSensor->getMinDelay() * 1000LL) {
+        compassRate = mCompassSensor->getMinDelay() * 1000LL;
+    }
+    mCompassSensor->setDelay(ID_M, compassRate);
+ 
+    mPressureSensor->setDelay(ID_PS, pressureRate);
+   
+    return res;
+}
+
+void MPLSensor::initBias()
+{
+    LOGV_IF(ENG_VERBOSE, "HAL:inititalize dmp and device offsets to 0");
+    
+    if(write_attribute_sensor_continuous(accel_x_dmp_bias_fd, 0) < 0)
+    {
+        LOGE("HAL:Error writing to accel_x_dmp_bias");
+    }
+    if(write_attribute_sensor_continuous(accel_y_dmp_bias_fd, 0) < 0)
+    {
+        LOGE("HAL:Error writing to accel_y_dmp_bias");       
+    }
+    if(write_attribute_sensor_continuous(accel_z_dmp_bias_fd, 0) < 0)
+    {
+        LOGE("HAL:Error writing to accel_z_dmp_bias");       
+    }
+    
+    if(write_attribute_sensor_continuous(gyro_x_dmp_bias_fd, 0) < 0)
+    {
+        LOGE("HAL:Error writing to gyro_x_dmp_bias");       
+    }
+    if(write_attribute_sensor_continuous(gyro_y_dmp_bias_fd, 0) < 0)
+    {
+        LOGE("HAL:Error writing to gyro_y_dmp_bias");       
+    }
+     if(write_attribute_sensor_continuous(gyro_z_dmp_bias_fd, 0) < 0)
+    {
+        LOGE("HAL:Error writing to gyro_z_dmp_bias");       
+    }
+    
+    if(write_attribute_sensor_continuous(gyro_x_offset_fd, 0) < 0)
+    {
+        LOGE("HAL:Error writing to gyro_x_offset");       
+    }
+    if(write_attribute_sensor_continuous(gyro_y_offset_fd, 0) < 0)
+    {
+        LOGE("HAL:Error writing to gyro_y_offset");       
+    }
+    if(write_attribute_sensor_continuous(gyro_z_offset_fd, 0) < 0)
+    {
+        LOGE("HAL:Error writing to gyro_z_offset");       
+    }
+    return;
+}
+ 
+/*TODO: reg_dump in a separate file*/
+void MPLSensor::sys_dump(bool fileMode)
+{
+    VFUNC_LOG;
+
+    char sysfs_path[MAX_SYSFS_NAME_LEN];
+    char scan_element_path[MAX_SYSFS_NAME_LEN];
+
+    memset(sysfs_path, 0, sizeof(sysfs_path));
+    memset(scan_element_path, 0, sizeof(scan_element_path));
+    inv_get_sysfs_path(sysfs_path);
+    sprintf(scan_element_path, "%s%s", sysfs_path, "/scan_elements");
+
+    read_sysfs_dir(fileMode, sysfs_path);
+    read_sysfs_dir(fileMode, scan_element_path);
+
+    dump_dmp_img("/data/local/read_img.h");
+    return;
+}
diff --git a/65xx/libsensors_iio/MPLSensor.h b/65xx/libsensors_iio/MPLSensor.h
new file mode 100644
index 0000000..44c5a50
--- /dev/null
+++ b/65xx/libsensors_iio/MPLSensor.h
@@ -0,0 +1,558 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef ANDROID_MPL_SENSOR_H
+#define ANDROID_MPL_SENSOR_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <poll.h>
+#include <time.h>
+#include <utils/Vector.h>
+#include <utils/KeyedVector.h>
+#include <utils/String8.h>
+#include "sensors.h"
+#include "SensorBase.h"
+#include "InputEventReader.h"
+
+#ifndef INVENSENSE_COMPASS_CAL
+#pragma message("unified HAL for AKM")
+#include "CompassSensor.AKM.h"
+#endif
+
+#ifdef SENSOR_ON_PRIMARY_BUS
+#pragma message("Sensor on Primary Bus")
+#include "CompassSensor.IIO.primary.h"
+#else
+#pragma message("Sensor on Secondary Bus")
+#include "CompassSensor.IIO.9150.h"
+#endif
+
+class PressureSensor;
+
+/*****************************************************************************/
+/* Sensors Enable/Disable Mask
+ *****************************************************************************/
+#define MAX_CHIP_ID_LEN             (20)
+
+#define INV_THREE_AXIS_GYRO         (0x000F)
+#define INV_THREE_AXIS_ACCEL        (0x0070)
+#define INV_THREE_AXIS_COMPASS      (0x0380)
+#define INV_ONE_AXIS_PRESSURE       (0x0400)
+#define INV_ALL_SENSORS             (0x7FFF)
+
+#ifdef INVENSENSE_COMPASS_CAL
+#define ALL_MPL_SENSORS_NP          (INV_THREE_AXIS_ACCEL \
+                                      | INV_THREE_AXIS_COMPASS \
+                                      | INV_THREE_AXIS_GYRO)
+#else
+#define ALL_MPL_SENSORS_NP          (INV_THREE_AXIS_ACCEL \
+                                      | INV_THREE_AXIS_COMPASS \
+                                      | INV_THREE_AXIS_GYRO)
+#endif
+
+// bit mask of current active features (mMplFeatureActiveMask)
+#define INV_COMPASS_CAL              0x01
+#define INV_COMPASS_FIT              0x02
+// bit mask of current active features (mFeatureActiveMask)
+#define INV_DMP_QUATERNION           0x001 //3 elements without real part, 32 bit each
+#define INV_DMP_DISPL_ORIENTATION    0x002
+#define INV_DMP_SIGNIFICANT_MOTION   0x004
+#define INV_DMP_PEDOMETER            0x008
+#define INV_DMP_PEDOMETER_STEP       0x010
+#define INV_DMP_PED_STANDALONE       0x020 //timestamps only
+#define INV_DMP_6AXIS_QUATERNION     0x040 //3 elements without real part, 32 bit each
+#define INV_DMP_PED_QUATERNION       0x080 //3 elements without real part, 16 bit each
+#define INV_DMP_PED_INDICATOR        0x100 //tag along header with step indciator
+#define INV_DMP_BATCH_MODE           0x200
+#define INV_DMP_ACCEL_PED (0xffff)
+
+// bit mask of whether screen orientation is on
+/*#define SCREEN_ORIENTATION_MASK (                         \
+        (isDmpDisplayOrientationOn()                      \
+        && ((1 << INV_DMP_DISPL_ORIENTATION)              \
+             || !isDmpScreenAutoRotationEnabled()))       \
+)*/
+// bit mask of whether DMP should be turned on
+#define DMP_FEATURE_MASK (                           \
+        (INV_DMP_QUATERNION)                         \
+        | (INV_DMP_DISPL_ORIENTATION)                \
+        | (INV_DMP_SIGNIFICANT_MOTION)               \
+        | (INV_DMP_PEDOMETER)                        \
+        | (INV_DMP_PEDOMETER_STEP)                   \
+        | (INV_DMP_6AXIS_QUATERNION)                 \
+        | (INV_DMP_PED_QUATERNION)                   \
+        | (INV_DMP_BATCH_MODE)                       \
+)
+// bit mask of DMP features as sensors
+#define DMP_SENSOR_MASK (                            \
+        (INV_DMP_DISPL_ORIENTATION)                  \
+        | (INV_DMP_SIGNIFICANT_MOTION)               \
+        | (INV_DMP_PEDOMETER)                        \
+        | (INV_DMP_PEDOMETER_STEP)                   \
+        | (INV_DMP_6AXIS_QUATERNION)                 \
+)
+// data header format used by kernel driver.
+#define DATA_FORMAT_STEP           0x0001
+#define DATA_FORMAT_PED_STANDALONE 0x0100
+#define DATA_FORMAT_PED_QUAT       0x0200
+#define DATA_FORMAT_6_AXIS         0x0400
+#define DATA_FORMAT_QUAT           0x0800
+#define DATA_FORMAT_COMPASS        0x1000
+#define DATA_FORMAT_GYRO           0x2000
+#define DATA_FORMAT_ACCEL          0x4000
+#define DATA_FORMAT_PRESSURE       0x8000
+#define DATA_FORMAT_MASK           0xffff
+
+#define BYTES_PER_SENSOR                8
+#define BYTES_PER_SENSOR_PACKET         16
+#define QUAT_ONLY_LAST_PACKET_OFFSET    16
+#define BYTES_QUAT_DATA                 24
+#define MAX_SUSPEND_BATCH_PACKET_SIZE   1024
+#define MAX_PACKET_SIZE                 80 //8 * 4 + (2 * 24)
+
+/* Uncomment to enable Low Power Quaternion */
+#define ENABLE_LP_QUAT_FEAT
+
+/* Uncomment to enable DMP display orientation
+   (within the HAL, see below for Java framework) */
+//#define ENABLE_DMP_DISPL_ORIENT_FEAT
+
+#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
+/* Uncomment following to expose the SENSOR_TYPE_SCREEN_ORIENTATION
+   sensor type (DMP screen orientation) to the Java framework.
+   NOTE:
+       need Invensense customized
+       'hardware/libhardware/include/hardware/sensors.h' to compile correctly.
+   NOTE:
+       need Invensense java framework changes to:
+       'frameworks/base/core/java/android/view/WindowOrientationListener.java'
+       'frameworks/base/core/java/android/hardware/Sensor.java'
+       'frameworks/base/core/java/android/hardware/SensorEvent.java'
+       for the 'Auto-rotate screen' to use this feature.
+*/
+#define ENABLE_DMP_SCREEN_AUTO_ROTATION
+#pragma message("ENABLE_DMP_DISPL_ORIENT_FEAT is defined, framework changes are necessary for HAL to work properly")
+#endif
+
+/* Enable Pressure sensor support */
+#define ENABLE_PRESSURE
+
+int isDmpScreenAutoRotationEnabled()
+{
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+    return 1;
+#else
+    return 0;
+#endif
+}
+
+int (*m_pt2AccelCalLoadFunc)(long *bias) = NULL;
+/*****************************************************************************/
+/** MPLSensor implementation which fits into the HAL example for crespo provided
+ *  by Google.
+ *  WARNING: there may only be one instance of MPLSensor, ever.
+ */
+
+class MPLSensor: public SensorBase
+{
+    typedef int (MPLSensor::*hfunc_t)(sensors_event_t*);
+
+public:
+
+    enum {
+        Gyro = 0,
+        RawGyro,
+        Accelerometer,
+        MagneticField,
+        RawMagneticField,
+        Pressure,
+        Orientation,
+        RotationVector,
+        GameRotationVector,
+        LinearAccel,
+        Gravity,
+        SignificantMotion,
+        StepDetector,
+        StepCounter,
+        GeomagneticRotationVector,
+        NumSensors
+    };
+
+    MPLSensor(CompassSensor *, int (*m_pt2AccelCalLoadFunc)(long*) = 0);
+    virtual ~MPLSensor();
+
+    virtual int setDelay(int32_t handle, int64_t ns);
+    virtual int enable(int32_t handle, int enabled);
+    virtual int batch(int handle, int flags, int64_t period_ns, int64_t timeout);
+    int setBatch(int en, int toggleEnable);
+    int32_t getEnableMask() { return mEnabled; }
+    void getHandle(int32_t handle, int &what, android::String8 &sname);
+
+    virtual int readEvents(sensors_event_t *data, int count);
+    virtual int getFd() const;
+    virtual int getAccelFd() const;
+    virtual int getCompassFd() const;
+    virtual int getPollTime();
+    virtual int getStepCountPollTime();
+    virtual bool hasPendingEvents() const;
+    virtual bool hasStepCountPendingEvents();
+    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;
+
+    int readAccelEvents(sensors_event_t* data, int count);
+    void buildCompassEvent();
+    void buildMpuEvent();
+
+    int turnOffAccelFifo();
+    int turnOffGyroFifo();
+    int enableDmpOrientation(int);
+    int dmpOrientHandler(int);
+    int readDmpOrientEvents(sensors_event_t* data, int count);
+    int getDmpOrientFd();
+    int openDmpOrientFd();
+    int closeDmpOrientFd();
+
+    int getDmpRate(int64_t *);
+    int checkDMPOrientation();
+
+    int getDmpSignificantMotionFd();
+    int readDmpSignificantMotionEvents(sensors_event_t* data, int count);
+    int enableDmpSignificantMotion(int);
+    int significantMotionHandler(sensors_event_t* data);
+    bool checkSmdSupport(){return (mDmpSignificantMotionEnabled);};
+
+    int enableDmpPedometer(int, int);
+    int readDmpPedometerEvents(sensors_event_t* data, int count, int32_t id, int32_t type, int outputType);
+    int getDmpPedometerFd();
+    bool checkPedometerSupport() {return (mDmpPedometerEnabled || mDmpStepCountEnabled);};
+    bool checkOrientationSupport() {return ((isDmpDisplayOrientationOn()
+                                       && (mDmpOrientationEnabled
+                                       || !isDmpScreenAutoRotationEnabled())));};
+
+protected:
+    CompassSensor *mCompassSensor;
+    PressureSensor *mPressureSensor;
+
+    int gyroHandler(sensors_event_t *data);
+    int rawGyroHandler(sensors_event_t *data);
+    int accelHandler(sensors_event_t *data);
+    int compassHandler(sensors_event_t *data);
+    int rawCompassHandler(sensors_event_t *data);
+    int rvHandler(sensors_event_t *data);
+    int grvHandler(sensors_event_t *data);
+    int laHandler(sensors_event_t *data);
+    int gravHandler(sensors_event_t *data);
+    int orienHandler(sensors_event_t *data);
+    int smHandler(sensors_event_t *data);
+    int pHandler(sensors_event_t *data);
+    int scHandler(sensors_event_t *data);
+    int gmHandler(sensors_event_t *data);
+    int psHandler(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 enablePedStandalone(int en);
+    int enablePedStandaloneData(int en);
+    int enablePedQuaternion(int);
+    int enablePedQuaternionData(int);
+    int enable6AxisQuaternion(int);
+    int enable6AxisQuaternionData(int);
+    int enableLPQuaternion(int);
+    int enableQuaternionData(int);
+    int enableAccelPedometer(int);
+    int enableAccelPedData(int);
+    int onDmp(int);
+    int enableGyro(int en);
+    int enableAccel(int en);
+    int enableCompass(int en, int rawSensorOn);
+    int enablePressure(int en);
+    int enableBatch(int64_t timeout);
+    void computeLocalSensorMask(int enabled_sensors);
+    int computeBatchSensorMask(int enableSensor, int checkNewBatchSensor);
+    int computeBatchDataOutput();
+    int enableSensors(unsigned long sensors, int en, uint32_t changed);
+    int inv_read_gyro_buffer(int fd, short *data, long long *timestamp);
+    int inv_float_to_q16(float *fdata, long *ldata);
+    int inv_long_to_q16(long *fdata, long *ldata);
+    int inv_float_to_round(float *fdata, long *ldata);
+    int inv_float_to_round2(float *fdata, short *sdata);
+    int inv_long_to_float(long *ldata, float *fdata);
+    int inv_read_temperature(long long *data);
+    int inv_read_dmp_state(int fd);
+    int inv_read_sensor_bias(int fd, long *data);
+    void inv_get_sensors_orientation(void);
+    int inv_init_sysfs_attributes(void);
+    int resetCompass(void);
+    void setCompassDelay(int64_t ns);
+    void enable_iio_sysfs(void);
+    int enablePedometer(int);
+    int enablePedIndicator(int en);
+    int checkPedStandaloneEnabled(void);
+    int checkPedQuatEnabled();
+    int check6AxisQuatEnabled();
+    int checkLPQuaternion();
+    int checkAccelPed();
+    int writeSignificantMotionParams(bool toggleEnable,
+                                     uint32_t delayThreshold1, uint32_t delayThreshold2,
+                                     uint32_t motionThreshold);
+
+    int mNewData;   // flag indicating that the MPL calculated new output values
+    int mDmpStarted;
+    long mMasterSensorMask;
+    long mLocalSensorMask;
+    int mPollTime;
+    int mStepCountPollTime;
+    bool mHaveGoodMpuCal;   // flag indicating that the cal file can be written
+    int mGyroAccuracy;      // value indicating the quality of the gyro calibr.
+    int mAccelAccuracy;     // value indicating the quality of the accel calibr.
+    int mCompassAccuracy;     // value indicating the quality of the compass calibr.
+    struct pollfd mPollFds[5];
+    int mSampleCount;
+    pthread_mutex_t mMplMutex;
+    pthread_mutex_t mHALMutex;
+
+    char mIIOBuffer[(16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH];
+
+    int iio_fd;
+    int accel_fd;
+    int mpufifo_fd;
+    int gyro_temperature_fd;
+    int accel_x_offset_fd;
+    int accel_y_offset_fd;
+    int accel_z_offset_fd;
+
+    int accel_x_dmp_bias_fd;
+    int accel_y_dmp_bias_fd;
+    int accel_z_dmp_bias_fd;
+
+    int gyro_x_offset_fd;
+    int gyro_y_offset_fd;
+    int gyro_z_offset_fd;
+
+    int gyro_x_dmp_bias_fd;
+    int gyro_y_dmp_bias_fd;
+    int gyro_z_dmp_bias_fd;
+
+    int dmp_orient_fd;
+    int mDmpOrientationEnabled;
+
+    int dmp_sign_motion_fd;
+    int mDmpSignificantMotionEnabled;
+
+    int dmp_pedometer_fd;
+    int mDmpPedometerEnabled;
+    int mDmpStepCountEnabled;
+
+    uint32_t mEnabled;
+    uint32_t mBatchEnabled;
+    int64_t mBatchTimeoutInMs;
+    sensors_event_t mPendingEvents[NumSensors];
+    int64_t mDelays[NumSensors];
+    int64_t mBatchDelays[NumSensors];
+    int64_t mBatchTimeouts[NumSensors];
+    hfunc_t mHandlers[NumSensors];
+    short mCachedGyroData[3];
+    long mCachedAccelData[3];
+    long mCachedCompassData[3];
+    long mCachedQuaternionData[3];
+    long mCached6AxisQuaternionData[3];
+    long mCachedPedQuaternionData[3];
+    long mCachedPressureData;
+    android::KeyedVector<int, int> mIrqFds;
+
+    InputEventCircularReader mAccelInputReader;
+    InputEventCircularReader mGyroInputReader;
+
+    bool mFirstRead;
+    short mTempScale;
+    short mTempOffset;
+    int64_t mTempCurrentTime;
+    int mAccelScale;
+    long mGyroScale;
+    long mGyroSelfTestScale;
+    long mCompassScale;
+    float mCompassBias[3];
+    bool mFactoryGyroBiasAvailable;
+    long mFactoryGyroBias[3];
+    bool mGyroBiasAvailable;
+    bool mGyroBiasApplied;
+    float mGyroBias[3];    //in body frame
+    long mGyroChipBias[3]; //in chip frame
+    bool mFactoryAccelBiasAvailable;
+    long mFactoryAccelBias[3];
+    bool mAccelBiasAvailable;
+    bool mAccelBiasApplied;
+    long mAccelBias[3];    //in chip frame
+
+    uint32_t mPendingMask;
+    unsigned long mSensorMask;
+
+    char chip_ID[MAX_CHIP_ID_LEN];
+    char mSysfsPath[MAX_SYSFS_NAME_LEN];
+
+    signed char mGyroOrientation[9];
+    signed char mAccelOrientation[9];
+
+    int64_t mSensorTimestamp;
+    int64_t mCompassTimestamp;
+    int64_t mPressureTimestamp;
+
+    struct sysfs_attrbs {
+       char *chip_enable;
+       char *power_state;
+       char *dmp_firmware;
+       char *firmware_loaded;
+       char *dmp_on;
+       char *dmp_int_on;
+       char *dmp_event_int_on;
+       char *tap_on;
+       char *key;
+       char *self_test;
+       char *temperature;
+
+       char *gyro_enable;
+       char *gyro_fifo_rate;
+       char *gyro_fsr;
+       char *gyro_orient;
+       char *gyro_fifo_enable;
+       char *gyro_rate;
+
+       char *accel_enable;
+       char *accel_fifo_rate;
+       char *accel_fsr;
+       char *accel_bias;
+       char *accel_orient;
+       char *accel_fifo_enable;
+       char *accel_rate;
+
+       char *three_axis_q_on; //formerly quaternion_on
+       char *three_axis_q_rate;
+       
+       char *six_axis_q_on;
+       char *six_axis_q_rate;
+       
+       char *ped_q_on;
+       char *ped_q_rate;
+
+       char *step_detector_on;
+       char *step_indicator_on;
+
+       char *in_timestamp_en;
+       char *in_timestamp_index;
+       char *in_timestamp_type;
+
+       char *buffer_length;
+
+       char *display_orientation_on;
+       char *event_display_orientation;
+
+       char *in_accel_x_offset;
+       char *in_accel_y_offset;
+       char *in_accel_z_offset;
+
+       char *in_accel_x_dmp_bias;
+       char *in_accel_y_dmp_bias;
+       char *in_accel_z_dmp_bias;
+
+       char *in_gyro_x_offset;
+       char *in_gyro_y_offset;
+       char *in_gyro_z_offset;
+       char *in_gyro_self_test_scale;
+
+       char *in_gyro_x_dmp_bias;
+       char *in_gyro_y_dmp_bias;
+       char *in_gyro_z_dmp_bias;
+
+       char *event_smd;
+       char *smd_enable;
+       char *smd_delay_threshold;
+       char *smd_delay_threshold2;
+       char *smd_threshold;
+       char *batchmode_timeout;
+       char *batchmode_wake_fifo_full_on;
+
+       char *pedometer_on;
+       char *pedometer_int_on;
+       char *event_pedometer;
+       char *pedometer_steps;
+    } mpu;
+
+    char *sysfs_names_ptr;
+    int mMplFeatureActiveMask;
+    uint64_t mFeatureActiveMask;
+    bool mDmpOn;
+    int mPedUpdate;
+    int64_t mQuatSensorTimestamp;
+    int64_t mStepSensorTimestamp;
+    uint64_t mLastStepCount;
+    int mLeftOverBufferSize;
+    char mLeftOverBuffer[24];
+
+private:
+    /* added for dynamic get sensor list */
+    void fillAccel(const char* accel, struct sensor_t *list);
+    void fillGyro(const char* gyro, struct sensor_t *list);
+    void fillRV(struct sensor_t *list);
+    void fillGMRV(struct sensor_t *list);
+    void fillGRV(struct sensor_t *list);
+    void fillOrientation(struct sensor_t *list);
+    void fillGravity(struct sensor_t *list);
+    void fillLinearAccel(struct sensor_t *list);
+    void fillSignificantMotion(struct sensor_t *list);
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+    void fillScreenOrientation(struct sensor_t *list);
+#endif
+    void storeCalibration();
+    void loadDMP();
+    bool isMpuNonDmp();
+    int isLowPowerQuatEnabled();
+    int isDmpDisplayOrientationOn();
+    void getCompassBias();
+    void getFactoryGyroBias();
+    void setFactoryGyroBias();
+    void getGyroBias();
+    void setGyroBias();
+    void getFactoryAccelBias();
+    void setFactoryAccelBias();
+    void getAccelBias();
+    void setAccelBias();
+    int isCompassDisabled();
+    int setBatchDataRates();
+    int resetDataRates();
+    void initBias();
+    void sys_dump(bool fileMode);
+};
+
+extern "C" {
+    void setCallbackObject(MPLSensor*);
+    MPLSensor *getCallbackObject();
+}
+
+#endif  // ANDROID_MPL_SENSOR_H
diff --git a/65xx/libsensors_iio/MPLSupport.cpp b/65xx/libsensors_iio/MPLSupport.cpp
new file mode 100644
index 0000000..f499daf
--- /dev/null
+++ b/65xx/libsensors_iio/MPLSupport.cpp
@@ -0,0 +1,307 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#include <MPLSupport.h>
+#include <dirent.h>
+#include <string.h>
+#include <stdio.h>
+#include "log.h"
+#include "SensorBase.h"
+#include <fcntl.h>
+
+#include "ml_sysfs_helper.h"
+#include "ml_load_dmp.h"
+
+int inv_read_data(char *fname, long *data)
+{
+    VFUNC_LOG;
+
+    char buf[sizeof(long) * 4];
+    int count, fd;
+
+    fd = open(fname, O_RDONLY);
+    if(fd < 0) {
+        LOGE("HAL:Error opening %s", fname);
+        return -1;
+    }
+    memset(buf, 0, sizeof(buf));
+    count = read_attribute_sensor(fd, buf, sizeof(buf));
+    if(count < 1) {
+        close(fd);
+        return -1;
+    } else {
+        count = sscanf(buf, "%ld", data);
+        if(count)
+            LOGV_IF(EXTRA_VERBOSE, "HAL:Data= %ld", *data);
+    }
+    close(fd);
+
+    return 0;
+}
+
+/* This one DOES NOT close FDs for you */
+int read_attribute_sensor(int fd, char* data, unsigned int size)
+{
+    VFUNC_LOG;
+
+    int count = 0;
+    if (fd > 0) {
+        count = pread(fd, data, size, 0);
+        if(count < 1) {
+            LOGE("HAL:read fails with error code=%d", count);
+        }
+    }
+    return count;
+}
+
+/**
+ *  @brief  Enable a sensor through the sysfs file descriptor
+ *          provided.
+ *  @note   this function one closes FD after the write
+ *  @param  fd
+ *              the file descriptor to write into
+ *  @param  en
+ *              the value to write, typically 1 or 0
+ *  @return the errno whenever applicable.
+ */
+int enable_sysfs_sensor(int fd, int en)
+{
+    VFUNC_LOG;
+
+    int nb;
+    int err = 0;
+
+    char c = en ? '1' : '0';
+    nb = write(fd, &c, 1);
+
+    if (nb <= 0) {
+        err = errno;
+        LOGE("HAL:enable_sysfs_sensor - write %c returned %d (%s / %d)",
+             c, nb, strerror(err), err);
+    }
+    close(fd);
+
+
+    return -err;
+}
+
+/* This one closes FDs for you */
+int write_attribute_sensor(int fd, long data)
+{
+    VFUNC_LOG;
+
+    int num_b = 0;
+
+    if (fd >= 0) {
+        char buf[80];
+        sprintf(buf, "%ld", data);
+        num_b = write(fd, buf, strlen(buf) + 1);
+        if (num_b <= 0) {
+            int err = errno;
+            LOGE("HAL:write fd %d returned '%s' (%d)", fd, strerror(err), err);
+        } else {
+            LOGV_IF(EXTRA_VERBOSE, "HAL:fd=%d write attribute to %ld", fd, data);
+        }
+        close(fd);
+    }
+
+    return num_b;
+}
+
+/* This one DOES NOT close FDs for you */
+int write_attribute_sensor_continuous(int fd, long data)
+{
+    VFUNC_LOG;
+
+    int num_b = 0;
+
+    if (fd >= 0) {
+        char buf[80];
+        sprintf(buf, "%ld", data);
+        num_b = write(fd, buf, strlen(buf) + 1);
+        if (num_b <= 0) {
+            int err = errno;
+            LOGE("HAL:write fd %d returned '%s' (%d)", fd, strerror(err), err);
+        } else {
+            LOGV_IF(EXTRA_VERBOSE, "HAL:fd=%d write attribute to %ld", fd, data);
+        }
+    }
+
+    return num_b;
+}
+
+int read_sysfs_int(char *filename, int *var)
+{
+    int res=0;
+    FILE  *sysfsfp;
+
+    sysfsfp = fopen(filename, "r");
+    if (sysfsfp!=NULL) {
+        fscanf(sysfsfp, "%d\n", var);
+        fclose(sysfsfp);
+    } else {
+        res = errno;
+        LOGE("HAL:ERR open file %s to read with error %d", filename, res);
+    }
+    return -res;
+}
+
+int write_sysfs_int(char *filename, int var)
+{
+    int res=0;
+    FILE  *sysfsfp;
+
+    sysfsfp = fopen(filename, "w");
+    if (sysfsfp!=NULL) {
+        fprintf(sysfsfp, "%d\n", var);
+        fclose(sysfsfp);
+    } else {
+        res = errno;
+        LOGE("HAL:ERR open file %s to write with error %d", filename, res);
+    }
+    return -res;
+}
+
+int write_sysfs_longlong(char *filename, int64_t var)
+{
+    int res=0;
+    FILE  *sysfsfp;
+
+    sysfsfp = fopen(filename, "w");
+    if (sysfsfp!=NULL) {
+        fprintf(sysfsfp, "%lld\n", var);
+        fclose(sysfsfp);
+    } else {
+        res = errno;
+        LOGE("HAL:ERR open file %s to write with error %d", filename, res);
+    }
+    return -res;
+}
+
+int fill_dev_full_name_by_prefix(const char* dev_prefix, 
+                                 char *dev_full_name, int len)
+{
+    char cand_name[20];
+    int prefix_len = strlen(dev_prefix);
+    strncpy(cand_name, dev_prefix, sizeof(cand_name) / sizeof(cand_name[0]));
+
+    // try adding a number, 0-9
+    for(int cand_postfix = 0; cand_postfix < 10; cand_postfix++) {
+        snprintf(&cand_name[prefix_len],
+                 sizeof(cand_name) / sizeof(cand_name[0]), 
+                 "%d", cand_postfix);
+        int dev_num = find_type_by_name(cand_name, "iio:device");
+        if (dev_num != -ENODEV) {
+            strncpy(dev_full_name, cand_name, len);
+            return 0; 
+        }
+    }
+    // try adding a small letter, a-z
+    for(char cand_postfix = 'a'; cand_postfix <= 'z'; cand_postfix++) {
+        snprintf(&cand_name[prefix_len], 
+                 sizeof(cand_name) / sizeof(cand_name[0]), 
+                 "%c", cand_postfix);
+        int dev_num = find_type_by_name(cand_name, "iio:device");
+        if (dev_num != -ENODEV) {
+            strncpy(dev_full_name, cand_name, len);
+            return 0; 
+        }
+    }
+    // try adding a capital letter, A-Z
+    for(char cand_postfix = 'A'; cand_postfix <= 'Z'; cand_postfix++) {
+        snprintf(&cand_name[prefix_len],
+                 sizeof(cand_name) / sizeof(cand_name[0]), 
+                 "%c", cand_postfix);
+        int dev_num = find_type_by_name(cand_name, "iio:device");
+        if (dev_num != -ENODEV) {
+            strncpy(dev_full_name, cand_name, len);
+            return 0; 
+        }
+    }
+    return 1;
+}
+
+void dump_dmp_img(const char *outFile)
+{
+    FILE *fp;
+    int i;
+
+    char sysfs_path[MAX_SYSFS_NAME_LEN];
+    char dmp_path[MAX_SYSFS_NAME_LEN];
+
+    inv_get_sysfs_path(sysfs_path);
+    sprintf(dmp_path, "%s%s", sysfs_path, "/dmp_firmware");
+
+    LOGI("HAL DEBUG:dump DMP image");
+    LOGI("HAL DEBUG:open %s\n", dmp_path);
+    LOGI("HAL DEBUG:write to %s", outFile);
+
+    read_dmp_img(dmp_path, (char *)outFile);
+}
+
+int read_sysfs_dir(bool fileMode, char *sysfs_path)
+{
+    VFUNC_LOG;
+
+    int res = 0;
+    char full_path[MAX_SYSFS_NAME_LEN];
+    int fd;
+    char buf[sizeof(long) *4];
+    long data;
+  
+    DIR *dp;
+    struct dirent *ep;
+      
+    dp = opendir (sysfs_path);
+
+    if (dp != NULL)
+    {
+        LOGI("******************** System Sysfs Dump ***************************");
+        LOGV_IF(0,"HAL DEBUG: opened directory %s", sysfs_path);
+        while ((ep = readdir (dp))) {
+            if(ep != NULL) {
+                LOGV_IF(0,"file name %s", ep->d_name);
+                if(!strcmp(ep->d_name, ".") || !strcmp(ep->d_name, "..") ||
+                         !strcmp(ep->d_name, "uevent") || !strcmp(ep->d_name, "dev") ||
+                         !strcmp(ep->d_name, "self_test"))
+                    continue;
+                sprintf(full_path, "%s%s%s", sysfs_path, "/", ep->d_name);
+                LOGV_IF(0,"HAL DEBUG: reading %s", full_path);
+                fd = open(full_path, O_RDONLY);
+                if (fd > -1) {
+                    memset(buf, 0, sizeof(buf));
+                    res = read_attribute_sensor(fd, buf, sizeof(buf));
+                    close(fd);
+                    if (res > 0) {
+                        res = sscanf(buf, "%ld", &data);
+                        if (res)
+                            LOGI("HAL DEBUG:sysfs:cat %s = %ld", full_path, data);
+                    } else {
+                         LOGV_IF(0,"HAL DEBUG: error reading %s", full_path);
+                    } 
+                } else {
+                    LOGV_IF(0,"HAL DEBUG: error opening %s", full_path);
+                }
+                close(fd);
+            }
+        }
+        closedir(dp);
+    } else{
+        LOGI("HAL DEBUG: could not open directory %s", sysfs_path);
+    }
+
+    return res;
+}
diff --git a/65xx/libsensors_iio/MPLSupport.h b/65xx/libsensors_iio/MPLSupport.h
new file mode 100644
index 0000000..981e275
--- /dev/null
+++ b/65xx/libsensors_iio/MPLSupport.h
@@ -0,0 +1,35 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef ANDROID_MPL_SUPPORT_H
+#define ANDROID_MPL_SUPPORT_H
+
+#include <stdint.h>
+
+int inv_read_data(char *fname, long *data);
+int read_attribute_sensor(int fd, char* data, unsigned int size);
+int enable_sysfs_sensor(int fd, int en);
+int write_attribute_sensor(int fd, long data);
+int write_attribute_sensor_continuous(int fd, long data);
+int read_sysfs_int(char*, int*);
+int write_sysfs_int(char*, int);
+int write_sysfs_longlong(char*, long long);
+int fill_dev_full_name_by_prefix(const char* dev_prefix,
+                                 char* dev_full_name, int len);
+void dump_dmp_img(const char *out_file);
+int read_sysfs_dir(bool fileMode, char *sysfs_path);
+
+#endif //  ANDROID_MPL_SUPPORT_H
diff --git a/65xx/libsensors_iio/PressureSensor.IIO.secondary.cpp b/65xx/libsensors_iio/PressureSensor.IIO.secondary.cpp
new file mode 100644
index 0000000..b221e6a
--- /dev/null
+++ b/65xx/libsensors_iio/PressureSensor.IIO.secondary.cpp
@@ -0,0 +1,213 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#define LOG_NDEBUG 0
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <sys/select.h>
+#include <cutils/log.h>
+#include <linux/input.h>
+#include <string.h>
+
+#include "PressureSensor.IIO.secondary.h"
+#include "sensors.h"
+#include "MPLSupport.h"
+#include "sensor_params.h"
+#include "ml_sysfs_helper.h"
+
+#pragma message("HAL:build pressure sensor on Invensense MPU secondary bus")
+/* dynamically get this when driver supports it */
+#define CHIP_ID "BMP280"
+
+/* return the current time in nanoseconds */
+extern int64_t now_ns(void);
+
+inline int64_t now_ns(void)
+{
+    struct timespec ts;
+
+    clock_gettime(CLOCK_MONOTONIC, &ts);
+    LOGV_IF(EXTRA_VERBOSE, "Time %lld", (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec);
+    return (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
+}
+
+//#define TIMER (1)
+#define DEFAULT_POLL_TIME 300
+#define PRESSURE_MAX_SYSFS_ATTRB sizeof(pressureSysFs) / sizeof(char*)
+
+static int s_poll_time = -1;
+static int min_poll_time = 50;
+static struct timespec t_pre;
+
+/*****************************************************************************/
+
+PressureSensor::PressureSensor(const char *sysfs_path) 
+                  : SensorBase(NULL, NULL),
+                    pressure_fd(-1)
+{                                     
+    VFUNC_LOG;
+
+    mSysfsPath = sysfs_path;
+    LOGI("pressuresensor path: %s", mSysfsPath);
+    if(inv_init_sysfs_attributes()) {
+        LOGE("Error Instantiating Pressure Sensor\n");
+        return;
+    } else {
+        LOGI("HAL:Secondary Chip Id: %s", CHIP_ID);
+    }
+}
+
+PressureSensor::~PressureSensor()
+{
+    VFUNC_LOG;
+
+    if( pressure_fd > 0)
+        close(pressure_fd);
+}
+
+int PressureSensor::getFd() const
+{
+    VHANDLER_LOG;
+    return pressure_fd;
+}
+
+/**
+ *  @brief        This function will enable/disable sensor.
+ *  @param[in]    handle
+ *                  which sensor to enable/disable.
+ *  @param[in]    en
+ *                  en=1, enable; 
+ *                  en=0, disable
+ *  @return       if the operation is successful.
+ */
+int PressureSensor::enable(int32_t handle, int en) 
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs: echo %d > %s (%lld)",
+            en, pressureSysFs.pressure_enable, getTimestamp());
+    res = write_sysfs_int(pressureSysFs.pressure_enable, en);
+
+    return res;
+}
+
+int PressureSensor::setDelay(int32_t handle, int64_t ns) 
+{
+    VFUNC_LOG;
+    
+    int res = 0;
+
+    mDelay = int(1000000000.f / ns);
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs: echo %lld > %s (%lld)",
+            mDelay, pressureSysFs.pressure_rate, getTimestamp());
+    res = write_sysfs_int(pressureSysFs.pressure_rate, mDelay);
+     
+#ifdef TIMER
+    int t_poll_time = (int)(ns / 1000000LL);
+    if (t_poll_time > min_poll_time) {
+        s_poll_time = t_poll_time;
+    } else {
+        s_poll_time = min_poll_time;
+    }
+    LOGV_IF(PROCESS_VERBOSE,
+            "HAL:setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f/ns);
+#endif
+    return res;
+}
+
+
+/**
+    @brief      This function will return the state of the sensor.
+    @return     1=enabled; 0=disabled
+**/
+int PressureSensor::getEnable(int32_t handle)
+{
+    VFUNC_LOG;
+    return mEnable;
+}
+
+/**
+ *  @brief  This function will return the current delay for this sensor.
+ *  @return delay in nanoseconds. 
+ */
+int64_t PressureSensor::getDelay(int32_t handle)
+{
+    VFUNC_LOG;
+
+#ifdef TIMER
+    if (mEnable) {
+        return s_poll_time;
+    } else {
+        return -1;
+    }
+#endif
+    return mDelay;
+}
+
+void PressureSensor::fillList(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    const char *pressure = "BMP280";
+
+    if (pressure) {
+        if(!strcmp(pressure, "BMP280")) {
+            list->maxRange = PRESSURE_BMP280_RANGE;
+            list->resolution = PRESSURE_BMP280_RESOLUTION;
+            list->power = PRESSURE_BMP280_POWER;
+            list->minDelay = PRESSURE_BMP280_MINDELAY;
+            return;
+        }      
+    }
+    LOGE("HAL:unknown pressure id %s -- "
+         "params default to bmp280 and might be wrong.",
+         pressure);
+    list->maxRange = PRESSURE_BMP280_RANGE;
+    list->resolution = PRESSURE_BMP280_RESOLUTION;
+    list->power = PRESSURE_BMP280_POWER;
+    list->minDelay = PRESSURE_BMP280_MINDELAY;
+}
+
+int PressureSensor::inv_init_sysfs_attributes(void)
+{
+    VFUNC_LOG;
+ 
+    pathP = (char*)malloc(sizeof(char[PRESSURE_MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
+    char *sptr = pathP;
+    char **dptr = (char**)&pressureSysFs;
+    if (sptr == NULL)
+        return -1;
+    unsigned char i = 0;
+    do {
+        *dptr++ = sptr;
+        memset(sptr, 0, sizeof(sptr));
+        sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
+    } while (++i < PRESSURE_MAX_SYSFS_ATTRB);
+ 
+    
+    if (mSysfsPath == NULL)
+        return 0;
+
+    sprintf(pressureSysFs.pressure_enable, "%s%s", mSysfsPath, "/pressure_enable");
+    sprintf(pressureSysFs.pressure_rate, "%s%s", mSysfsPath, "/pressure_rate");
+    return 0;
+}
diff --git a/65xx/libsensors_iio/PressureSensor.IIO.secondary.h b/65xx/libsensors_iio/PressureSensor.IIO.secondary.h
new file mode 100644
index 0000000..899cf34
--- /dev/null
+++ b/65xx/libsensors_iio/PressureSensor.IIO.secondary.h
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef PRESSURE_SENSOR_H
+#define PRESSURE_SENSOR_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <poll.h>
+#include <utils/Vector.h>
+#include <utils/KeyedVector.h>
+
+#include "sensors.h"
+#include "SensorBase.h"
+#include "InputEventReader.h"
+
+class PressureSensor : public SensorBase {
+
+public:
+    PressureSensor(const char *sysfs_path);    
+    virtual ~PressureSensor();
+
+    virtual int getFd() const;
+    virtual int enable(int32_t handle, int enabled);
+    virtual int setDelay(int32_t handle, int64_t ns);
+    virtual int getEnable(int32_t handle);
+    virtual int64_t getDelay(int32_t handle);
+    virtual int64_t getMinDelay() { return -1; } // stub
+    // only applicable to primary
+    virtual int readEvents(sensors_event_t *data, int count) { return 0; }
+            
+    void fillList(struct sensor_t *list);    
+    // default is integrated for secondary bus
+    int isIntegrated() { return (1); }
+
+private:
+    char sensor_name[200];
+    
+    struct sysfs_attrbs {
+       char *pressure_enable;
+       char *pressure_rate;    
+    } pressureSysFs;
+
+    int pressure_fd; 
+    int64_t mDelay;
+    int mEnable;
+    char* pathP;
+    const char* mSysfsPath;
+
+    int inv_init_sysfs_attributes(void);
+};
+
+/*****************************************************************************/
+
+#endif  // PRESSURE_SENSOR_H
diff --git a/65xx/libsensors_iio/SensorBase.cpp b/65xx/libsensors_iio/SensorBase.cpp
new file mode 100644
index 0000000..33335be
--- /dev/null
+++ b/65xx/libsensors_iio/SensorBase.cpp
@@ -0,0 +1,152 @@
+/*

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

+

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

+

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

+}

+

+int SensorBase::query(int what, int* value)

+{

+    return 0;

+}

+

+int SensorBase::batch(int handle, int flags, int64_t period_ns, int64_t timeout)

+{

+    return 0;

+}

diff --git a/65xx/libsensors_iio/SensorBase.h b/65xx/libsensors_iio/SensorBase.h
new file mode 100644
index 0000000..eb5b9e2
--- /dev/null
+++ b/65xx/libsensors_iio/SensorBase.h
@@ -0,0 +1,102 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef ANDROID_SENSOR_BASE_H
+#define ANDROID_SENSOR_BASE_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#if defined ANDROID_JELLYBEAN
+//build for Jellybean
+#define LOGV_IF ALOGV_IF
+#define LOGE_IF ALOGE_IF
+#define LOGI_IF ALOGI_IF
+#define LOGI    ALOGI
+#define LOGE    ALOGE
+#define LOGV    ALOGV
+#define LOGW    ALOGW
+#else
+//build for ICS or earlier version
+#endif
+
+/* 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__)
+#define CALL_MEMBER_FN(pobject, ptrToMember) ((pobject)->*(ptrToMember))
+
+#define MAX_SYSFS_NAME_LEN  (100)
+#define IIO_BUFFER_LENGTH   (480)
+
+/*****************************************************************************/
+
+struct sensors_event_t;
+
+class SensorBase {
+protected:
+    const char *dev_name;
+    const char *data_name;
+    char input_name[PATH_MAX];
+    int dev_fd;
+    int data_fd;
+
+    int openInput(const char* inputName);
+    static int64_t getTimestamp();
+    static int64_t timevalToNano(timeval const& t) {
+        return t.tv_sec * 1000000000LL + t.tv_usec * 1000;
+    }
+
+    int open_device();
+    int close_device();
+
+public:
+    SensorBase(const char* dev_name, const char* data_name);
+
+    virtual ~SensorBase();
+
+    virtual int readEvents(sensors_event_t* data, int count) = 0;
+    int readSample(long *data, int64_t *timestamp);
+    int readRawSample(float *data, int64_t *timestamp);
+    virtual bool hasPendingEvents() const;
+    virtual int getFd() const;
+    virtual int setDelay(int32_t handle, int64_t ns);
+    virtual int enable(int32_t handle, int enabled);
+    virtual int query(int what, int* value);
+    virtual int batch(int handle, int flags, int64_t period_ns, int64_t timeout);
+};
+
+/*****************************************************************************/
+
+#endif  // ANDROID_SENSOR_BASE_H
diff --git a/65xx/libsensors_iio/libmllite.so b/65xx/libsensors_iio/libmllite.so
new file mode 100644
index 0000000..eba66ba
--- /dev/null
+++ b/65xx/libsensors_iio/libmllite.so
diff --git a/65xx/libsensors_iio/libmplmpu.so b/65xx/libsensors_iio/libmplmpu.so
new file mode 100644
index 0000000..5d9ac0e
--- /dev/null
+++ b/65xx/libsensors_iio/libmplmpu.so
diff --git a/65xx/libsensors_iio/sensor_params.h b/65xx/libsensors_iio/sensor_params.h
new file mode 100644
index 0000000..7d6f62d
--- /dev/null
+++ b/65xx/libsensors_iio/sensor_params.h
@@ -0,0 +1,206 @@
+/*
+* 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_GAME_ROTATION_VECTOR_HANDLE        (ID_GRV)
+#define SENSORS_LINEAR_ACCEL_HANDLE                (ID_LA)
+#define SENSORS_GRAVITY_HANDLE                     (ID_GR)
+#define SENSORS_GYROSCOPE_HANDLE                   (ID_GY)
+#define SENSORS_RAW_GYROSCOPE_HANDLE               (ID_RG)
+#define SENSORS_ACCELERATION_HANDLE                (ID_A)
+#define SENSORS_MAGNETIC_FIELD_HANDLE              (ID_M)
+#define SENSORS_RAW_MAGNETIC_FIELD_HANDLE          (ID_RM)
+#define SENSORS_ORIENTATION_HANDLE                 (ID_O)
+#define SENSORS_SIGNIFICANT_MOTION_HANDLE          (ID_SM)
+#define SENSORS_PEDOMETER_HANDLE                   (ID_P)
+#define SENSORS_STEP_COUNTER_HANDLE                (ID_SC)
+#define SENSORS_GEOMAGNETIC_ROTATION_VECTOR_HANDLE (ID_GMRV)
+#define SENSORS_PRESSURE_HANDLE                    (ID_PS)
+#define SENSORS_SCREEN_ORIENTATION_HANDLE          (ID_SO)
+
+/******************************************/
+//MPU9250 INV_COMPASS
+#define COMPASS_MPU9250_RANGE           (9830.f)
+#define COMPASS_MPU9250_RESOLUTION      (0.15f)
+#define COMPASS_MPU9250_POWER           (10.f)
+#define COMPASS_MPU9250_MINDELAY        (10000)
+//MPU9150 INV_COMPASS
+#define COMPASS_MPU9150_RANGE           (9830.f)
+#define COMPASS_MPU9150_RESOLUTION      (0.285f)
+#define COMPASS_MPU9150_POWER           (10.f)
+#define COMPASS_MPU9150_MINDELAY        (10000)
+//COMPASS_ID_AK8975
+#define COMPASS_AKM8975_RANGE           (9830.f)
+#define COMPASS_AKM8975_RESOLUTION      (0.285f)
+#define COMPASS_AKM8975_POWER           (10.f)
+#define COMPASS_AKM8975_MINDELAY        (10000)
+//COMPASS_ID_AK8963C
+#define COMPASS_AKM8963_RANGE           (9830.f)
+#define COMPASS_AKM8963_RESOLUTION      (0.15f)
+#define COMPASS_AKM8963_POWER           (10.f)
+#define COMPASS_AKM8963_MINDELAY        (10000)
+//COMPASS_ID_AMI30X
+#define COMPASS_AMI30X_RANGE            (5461.f)
+#define COMPASS_AMI30X_RESOLUTION       (0.9f)
+#define COMPASS_AMI30X_POWER            (0.15f)
+//COMPASS_ID_AMI306
+#define COMPASS_AMI306_RANGE            (5461.f)
+#define COMPASS_AMI306_RESOLUTION       (0.9f)
+#define COMPASS_AMI306_POWER            (0.15f)
+#define COMPASS_AMI306_MINDELAY         (10000)
+//COMPASS_ID_YAS529
+#define COMPASS_YAS529_RANGE            (19660.f)
+#define COMPASS_YAS529_RESOLUTION       (0.012f)
+#define COMPASS_YAS529_POWER            (4.f)
+//COMPASS_ID_YAS53x
+#define COMPASS_YAS53x_RANGE            (8001.f)
+#define COMPASS_YAS53x_RESOLUTION       (0.012f)
+#define COMPASS_YAS53x_POWER            (4.f)
+#define COMPASS_YAS53x_MINDELAY         (10000)
+//COMPASS_ID_HMC5883
+#define COMPASS_HMC5883_RANGE           (10673.f)
+#define COMPASS_HMC5883_RESOLUTION      (10.f)
+#define COMPASS_HMC5883_POWER           (0.24f)
+//COMPASS_ID_LSM303DLH
+#define COMPASS_LSM303DLH_RANGE         (10240.f)
+#define COMPASS_LSM303DLH_RESOLUTION    (1.f)
+#define COMPASS_LSM303DLH_POWER         (1.f)
+//COMPASS_ID_LSM303DLM
+#define COMPASS_LSM303DLM_RANGE         (10240.f)
+#define COMPASS_LSM303DLM_RESOLUTION    (1.f)
+#define COMPASS_LSM303DLM_POWER         (1.f)
+//COMPASS_ID_MMC314X
+#define COMPASS_MMC314X_RANGE           (400.f)
+#define COMPASS_MMC314X_RESOLUTION      (2.f)
+#define COMPASS_MMC314X_POWER           (0.55f)
+//COMPASS_ID_HSCDTD002B
+#define COMPASS_HSCDTD002B_RANGE        (9830.f)
+#define COMPASS_HSCDTD002B_RESOLUTION   (1.f)
+#define COMPASS_HSCDTD002B_POWER        (1.f)
+//COMPASS_ID_HSCDTD004A
+#define COMPASS_HSCDTD004A_RANGE        (9830.f)
+#define COMPASS_HSCDTD004A_RESOLUTION   (1.f)
+#define COMPASS_HSCDTD004A_POWER        (1.f)
+/*******************************************/
+//ACCEL_ID_MPU6500
+#define ACCEL_MPU6500_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MPU6500_RESOLUTION        (0.004f * GRAVITY_EARTH)
+#define ACCEL_MPU6500_POWER             (0.5f)
+#define ACCEL_MPU6500_MINDELAY          (1000)
+//ACCEL_ID_MPU9250
+#define ACCEL_MPU9250_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MPU9250_RESOLUTION        (0.004f * GRAVITY_EARTH)
+#define ACCEL_MPU9250_POWER             (0.5f)
+#define ACCEL_MPU9250_MINDELAY          (1000)
+//ACCEL_ID_MPU9150
+#define ACCEL_MPU9150_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MPU9150_RESOLUTION        (0.004f * GRAVITY_EARTH)
+#define ACCEL_MPU9150_POWER             (0.5f)
+#define ACCEL_MPU9150_MINDELAY          (1000)
+//ACCEL_ID_LIS331
+#define ACCEL_LIS331_RANGE              (2.48f * GRAVITY_EARTH)
+#define ACCEL_LIS331_RESOLUTION         (0.001f * GRAVITY_EARTH)
+#define ACCEL_LIS331_POWER              (1.f)
+//ACCEL_ID_LSM303DLX
+#define ACCEL_LSM303DLX_RANGE           (2.48f * GRAVITY_EARTH)
+#define ACCEL_LSM303DLX_RESOLUTION      (0.001f * GRAVITY_EARTH)
+#define ACCEL_LSM303DLX_POWER           (1.f)
+//ACCEL_ID_LIS3DH
+#define ACCEL_LIS3DH_RANGE              (2.48f * GRAVITY_EARTH)
+#define ACCEL_LIS3DH_RESOLUTION         (0.001f * GRAVITY_EARTH)
+#define ACCEL_LIS3DH_POWER              (1.f)
+//ACCEL_ID_KXSD9
+#define ACCEL_KXSD9_RANGE               (2.5006f * GRAVITY_EARTH)
+#define ACCEL_KXSD9_RESOLUTION          (0.001f * GRAVITY_EARTH)
+#define ACCEL_KXSD9_POWER               (1.f)
+//ACCEL_ID_KXTF9
+#define ACCEL_KXTF9_RANGE               (1.f * GRAVITY_EARTH)
+#define ACCEL_KXTF9_RESOLUTION          (0.033f * GRAVITY_EARTH)
+#define ACCEL_KXTF9_POWER               (0.35f)
+//ACCEL_ID_BMA150
+#define ACCEL_BMA150_RANGE              (2.f * GRAVITY_EARTH)
+#define ACCEL_BMA150_RESOLUTION         (0.004f * GRAVITY_EARTH)
+#define ACCEL_BMA150_POWER              (0.2f)
+//ACCEL_ID_BMA222
+#define ACCEL_BMA222_RANGE              (2.f * GRAVITY_EARTH)
+#define ACCEL_BMA222_RESOLUTION         (0.001f * GRAVITY_EARTH)
+#define ACCEL_BMA222_POWER              (0.1f)
+//ACCEL_ID_BMA250
+#define ACCEL_BMA250_RANGE              (2.f * GRAVITY_EARTH)
+#define ACCEL_BMA250_RESOLUTION         (0.00391f * GRAVITY_EARTH)
+#define ACCEL_BMA250_POWER              (0.139f)
+#define ACCEL_BMA250_MINDELAY           (1000)
+//ACCEL_ID_ADXL34X
+#define ACCEL_ADXL34X_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_ADXL34X_RESOLUTION        (0.001f * GRAVITY_EARTH)
+#define ACCEL_ADXL34X_POWER             (1.f)
+//ACCEL_ID_MMA8450
+#define ACCEL_MMA8450_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MMA8450_RESOLUTION        (0.001f * GRAVITY_EARTH)
+#define ACCEL_MMA8450_POWER             (1.0f)
+//ACCEL_ID_MMA845X
+#define ACCEL_MMA845X_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MMA845X_RESOLUTION        (0.001f * GRAVITY_EARTH)
+#define ACCEL_MMA845X_POWER             (1.f)
+//ACCEL_ID_MPU6050
+#define ACCEL_MPU6050_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MPU6050_RESOLUTION        (0.004f * GRAVITY_EARTH)
+#define ACCEL_MPU6050_POWER             (5.5f)
+#define ACCEL_MPU6050_MINDELAY          (1000)
+/******************************************/
+//GYRO MPU3050
+#define RAD_P_DEG                       (3.14159f / 180.f)
+#define GYRO_MPU3050_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_MPU3050_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_MPU3050_POWER              (6.1f)
+#define GYRO_MPU3050_MINDELAY           (1000)
+//GYRO MPU6050
+#define GYRO_MPU6050_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_MPU6050_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_MPU6050_POWER              (5.5f)
+#define GYRO_MPU6050_MINDELAY           (1000)
+//GYRO MPU9150
+#define GYRO_MPU9150_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_MPU9150_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_MPU9150_POWER              (5.5f)
+#define GYRO_MPU9150_MINDELAY           (1000)
+//GYRO MPU9250
+#define GYRO_MPU9250_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_MPU9250_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_MPU9250_POWER              (5.5f)
+#define GYRO_MPU9250_MINDELAY           (1000)
+//GYRO MPU6500
+#define GYRO_MPU6500_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_MPU6500_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_MPU6500_POWER              (5.5f)
+#define GYRO_MPU6500_MINDELAY           (1000)
+//GYRO ITG3500
+#define GYRO_ITG3500_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_ITG3500_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_ITG3500_POWER              (5.5f)
+#define GYRO_ITG3500_MINDELAY           (1000)
+/******************************************/
+//PRESSURE BMP280
+#define PRESSURE_BMP280_RANGE           (1100.f)   // hpa
+#define PRESSURE_BMP280_RESOLUTION      (0.0018f)  // in psi
+#define PRESSURE_BMP280_POWER           (0.0248f)  // 0.00248mA
+#define PRESSURE_BMP280_MINDELAY        (26700)    // 26.7Hz
+#endif  /* INV_SENSOR_PARAMS_H */
+
diff --git a/65xx/libsensors_iio/sensors.h b/65xx/libsensors_iio/sensors.h
new file mode 100644
index 0000000..aebca81
--- /dev/null
+++ b/65xx/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

+

+enum {

+    ID_GY = 0,

+    ID_RG,

+    ID_A,

+    ID_M,

+    ID_RM,

+    ID_PS,

+    ID_O,

+    ID_RV,

+    ID_GRV,

+    ID_LA,

+    ID_GR,

+    ID_SM,

+    ID_P,

+    ID_SC,

+    ID_GMRV,

+    ID_SO

+};

+

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

+

+/*

+ * The SENSORS Module

+ */

+

+/* ITG3500 */

+#define EVENT_TYPE_GYRO_X          REL_X

+#define EVENT_TYPE_GYRO_Y          REL_Y

+#define EVENT_TYPE_GYRO_Z          REL_Z

+/* MPU6050 MPU9150 */

+#define EVENT_TYPE_IACCEL_X        REL_RX

+#define EVENT_TYPE_IACCEL_Y        REL_RY

+#define EVENT_TYPE_IACCEL_Z        REL_RZ

+/* MPU6050 MPU9150 */

+#define EVENT_TYPE_ICOMPASS_X      REL_X

+#define EVENT_TYPE_ICOMPASS_Y      REL_Y

+#define EVENT_TYPE_ICOMPASS_Z      REL_Z

+/* MPUxxxx */

+#define EVENT_TYPE_TIMESTAMP_HI    REL_MISC

+#define EVENT_TYPE_TIMESTAMP_LO    REL_WHEEL

+

+/* Accel BMA250 */

+#define EVENT_TYPE_ACCEL_X          ABS_X

+#define EVENT_TYPE_ACCEL_Y          ABS_Y

+#define EVENT_TYPE_ACCEL_Z          ABS_Z

+#define LSG                         (1000.0f)

+

+// conversion of acceleration data to SI units (m/s^2)

+#define RANGE_A                     (4*GRAVITY_EARTH)

+#define RESOLUTION_A                (GRAVITY_EARTH / LSG)

+#define CONVERT_A                   (GRAVITY_EARTH / LSG)

+#define CONVERT_A_X                 (CONVERT_A)

+#define CONVERT_A_Y                 (CONVERT_A)

+#define CONVERT_A_Z                 (CONVERT_A)

+

+/* AKM  compasses */

+#define EVENT_TYPE_MAGV_X           ABS_RX

+#define EVENT_TYPE_MAGV_Y           ABS_RY

+#define EVENT_TYPE_MAGV_Z           ABS_RZ

+#define EVENT_TYPE_MAGV_STATUS      ABS_RUDDER

+

+// conversion of magnetic data to uT units

+#define CONVERT_M                   (0.06f)

+

+__END_DECLS

+

+#endif  // ANDROID_SENSORS_H

diff --git a/65xx/libsensors_iio/sensors_mpl.cpp b/65xx/libsensors_iio/sensors_mpl.cpp
new file mode 100644
index 0000000..b7291fa
--- /dev/null
+++ b/65xx/libsensors_iio/sensors_mpl.cpp
@@ -0,0 +1,391 @@
+/*

+* Copyright (C) 2012 Invensense, Inc.

+*

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

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

+* You may obtain a copy of the License at

+*

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

+*

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

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

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

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

+* limitations under the License.

+*/

+

+#define FUNC_LOG LOGV("%s", __PRETTY_FUNCTION__)

+

+#include <hardware/sensors.h>

+#include <fcntl.h>

+#include <errno.h>

+#include <dirent.h>

+#include <math.h>

+#include <poll.h>

+#include <pthread.h>

+#include <stdlib.h>

+

+#include <linux/input.h>

+

+#include <utils/Atomic.h>

+#include <utils/Log.h>

+

+#include "sensors.h"

+#include "MPLSensor.h"

+

+/* 

+ * Vendor-defined Accel Load Calibration File Method 

+ * @param[out] Accel bias, length 3.  In HW units scaled by 2^16 in body frame

+ * @return '0' for a successful load, '1' otherwise

+ * example: int AccelLoadConfig(long* offset);

+ * End of Vendor-defined Accel Load Cal Method 

+ */

+

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

+/* The SENSORS Module */

+

+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION

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

+#else

+#define LOCAL_SENSORS MPLSensor::NumSensors

+#endif

+

+static struct sensor_t sSensorList[LOCAL_SENSORS];

+static int sensors = (sizeof(sSensorList) / sizeof(sensor_t));

+

+static int open_sensors(const struct hw_module_t* module, const char* id,

+                        struct hw_device_t** device);

+

+static int sensors__get_sensors_list(struct sensors_module_t* module,

+                                     struct sensor_t const** list)

+{

+    *list = sSensorList;

+    return sensors;

+}

+

+static struct hw_module_methods_t sensors_module_methods = {

+        open: open_sensors

+};

+

+struct sensors_module_t HAL_MODULE_INFO_SYM = {

+        common: {

+                tag: HARDWARE_MODULE_TAG,

+                version_major: 1,

+                version_minor: 0,

+                id: SENSORS_HARDWARE_MODULE_ID,

+                name: "Invensense module",

+                author: "Invensense Inc.",

+                methods: &sensors_module_methods,

+                dso: NULL,

+                reserved: {0}

+        },

+        get_sensors_list: sensors__get_sensors_list,

+};

+

+struct sensors_poll_context_t {

+    sensors_poll_device_1_t device; // must be first

+

+    sensors_poll_context_t();

+    ~sensors_poll_context_t();

+    int activate(int handle, int enabled);

+    int setDelay(int handle, int64_t ns);

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

+    int query(int what, int *value);

+    int batch(int handle, int flags, int64_t period_ns, int64_t timeout);

+

+private:

+    enum {

+        mpl = 0,

+        compass,

+        dmpOrient,

+        dmpSign,

+        dmpPed,

+        numSensorDrivers,   // wake pipe goes here

+        numFds,

+    };

+

+    struct pollfd mPollFds[numFds];

+    SensorBase *mSensor;

+    CompassSensor *mCompassSensor;

+

+    static const size_t wake = numSensorDrivers;

+    static const char WAKE_MESSAGE = 'W';

+    int mWritePipeFd;

+};

+

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

+

+sensors_poll_context_t::sensors_poll_context_t() {

+    VFUNC_LOG;

+

+    /* TODO: Handle external pressure sensor */

+    mCompassSensor = new CompassSensor();

+    MPLSensor *mplSensor = new MPLSensor(mCompassSensor);

+

+   /* For Vendor-defined Accel Calibration File Load

+    * Use the Following Constructor and Pass Your Load Cal File Function

+    * 

+    * MPLSensor *mplSensor = new MPLSensor(mCompassSensor, AccelLoadConfig);

+    */

+

+    // setup the callback object for handing mpl callbacks

+    setCallbackObject(mplSensor);

+

+    // populate the sensor list

+    sensors =

+            mplSensor->populateSensorList(sSensorList, sizeof(sSensorList));

+

+    mSensor = mplSensor;

+    mPollFds[mpl].fd = mSensor->getFd();

+    mPollFds[mpl].events = POLLIN;

+    mPollFds[mpl].revents = 0;

+

+    mPollFds[compass].fd = mCompassSensor->getFd();

+    mPollFds[compass].events = POLLIN;

+    mPollFds[compass].revents = 0;

+

+    mPollFds[dmpOrient].fd = ((MPLSensor*) mSensor)->getDmpOrientFd();

+    mPollFds[dmpOrient].events = POLLPRI;

+    mPollFds[dmpOrient].revents = 0;

+

+    mPollFds[dmpSign].fd = ((MPLSensor*) mSensor)->getDmpSignificantMotionFd();

+    mPollFds[dmpSign].events = POLLPRI;

+    mPollFds[dmpSign].revents = 0;

+

+    mPollFds[dmpPed].fd = ((MPLSensor*) mSensor)->getDmpPedometerFd();

+    mPollFds[dmpPed].events = POLLPRI;

+    mPollFds[dmpPed].revents = 0;

+

+    /* Timer based sensor initialization */

+    int wakeFds[2];

+    int result = pipe(wakeFds);

+    LOGE_IF(result<0, "error creating wake pipe (%s)", strerror(errno));

+    fcntl(wakeFds[0], F_SETFL, O_NONBLOCK);

+    fcntl(wakeFds[1], F_SETFL, O_NONBLOCK);

+    mWritePipeFd = wakeFds[1];

+

+    mPollFds[numSensorDrivers].fd = wakeFds[0];

+    mPollFds[numSensorDrivers].events = POLLIN;

+    mPollFds[numSensorDrivers].revents = 0;

+}

+

+sensors_poll_context_t::~sensors_poll_context_t() {

+    FUNC_LOG;

+    delete mSensor;

+    delete mCompassSensor;

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

+        close(mPollFds[i].fd);

+    }

+    close(mWritePipeFd);

+}

+

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

+    FUNC_LOG;

+

+    int err;

+    err = mSensor->enable(handle, enabled);

+    if (!err) {

+        const char wakeMessage(WAKE_MESSAGE);

+        int result = write(mWritePipeFd, &wakeMessage, 1);

+        LOGE_IF(result < 0, 

+                "error sending wake message (%s)", strerror(errno));

+    }

+    return err;

+}

+

+int sensors_poll_context_t::setDelay(int handle, int64_t ns)

+{

+    FUNC_LOG;

+    return mSensor->setDelay(handle, ns);

+}

+

+int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count)

+{

+    VHANDLER_LOG;

+

+    int nbEvents = 0;

+    int nb, polltime = -1;

+

+    polltime = ((MPLSensor*) mSensor)->getStepCountPollTime();

+

+    // look for new events

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

+    LOGI_IF(0, "poll nb=%d, count=%d, pt=%d", nb, count, polltime);

+    if (nb > 0) {

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

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

+                LOGI_IF(0, "poll found=%d", i);

+                nb = 0;

+                if (i == mpl) {

+                    ((MPLSensor*) mSensor)->buildMpuEvent();

+                    mPollFds[i].revents = 0;

+                } else if (i == compass) {

+                    ((MPLSensor*) mSensor)->buildCompassEvent();

+                    mPollFds[i].revents = 0;

+                } else if (i == dmpOrient) {

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

+                    mPollFds[dmpOrient].revents= 0;

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

+                        count -= nb;

+                        nbEvents += nb;

+                        data += nb;

+                    }

+                } else if (i == dmpSign) {

+                    LOGI("HAL: dmpSign interrupt");

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

+                    mPollFds[i].revents = 0;

+                    count -= nb;

+                    nbEvents += nb;

+                    data += nb; 

+                } else if (i == dmpPed) {

+                    LOGI("HAL: dmpPed interrupt");

+                    nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents(data, count, ID_P, SENSOR_TYPE_STEP_DETECTOR, 0);

+                    mPollFds[i].revents = 0;

+                    count -= nb;

+                    nbEvents += nb;

+                    data += nb;

+                }

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

+                LOGI_IF(0, "sensors_mpl:readEvents() - i=%d, nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, data->data[0]=%f,",

+                             i, nb, count, nbEvents, data->timestamp, data->data[0]);

+                if (nb > 0) {

+                    count -= nb;

+                    nbEvents += nb;

+                    data += nb;

+                }

+            }

+        }

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

+//        LOGI_IF(0, "sensors_mpl:readEvents() - nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, data->data[0]=%f,",

+//                          nb, count, nbEvents, data->timestamp, data->data[0]);

+//        if (nb > 0) {

+//            count -= nb;

+//            nbEvents += nb;

+//            data += nb;

+//        }

+

+        /* to see if any step counter events */

+        if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) {

+            nb = 0;

+            nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents(data, count, ID_SC, SENSOR_TYPE_STEP_COUNTER, 0);

+            LOGI_IF(HANDLER_DATA, "sensors_mpl:readStepCount() - nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, data->data[0]=%f,",

+                          nb, count, nbEvents, data->timestamp, data->data[0]);

+            if (nb > 0) {

+                count -= nb;

+                nbEvents += nb;

+                data += nb;

+            }

+        }

+    } else if(nb == 0){

+

+        /* to see if any step counter events */

+        if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) {

+            nb = 0;

+            nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents(data, count, ID_SC, SENSOR_TYPE_STEP_COUNTER, 0);

+            LOGI_IF(HANDLER_DATA, "sensors_mpl:readStepCount() - nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, data->data[0]=%f,",

+                          nb, count, nbEvents, data->timestamp, data->data[0]);

+            if (nb > 0) {

+                count -= nb;

+                nbEvents += nb;

+                data += nb;

+            }

+        }

+

+        if (mPollFds[numSensorDrivers].revents & POLLIN) {

+            char msg;

+            int result = read(mPollFds[numSensorDrivers].fd, &msg, 1);

+            LOGE_IF(result < 0, "error reading from wake pipe (%s)", strerror(errno));

+            mPollFds[numSensorDrivers].revents = 0;

+        }

+    }

+    return nbEvents;

+}

+

+int sensors_poll_context_t::query(int what, int* value)

+{

+    FUNC_LOG;

+    return mSensor->query(what, value);

+}

+

+int sensors_poll_context_t::batch(int handle, int flags, int64_t period_ns, int64_t timeout)

+{

+    FUNC_LOG;

+    return mSensor->batch(handle, flags, period_ns, timeout);

+}

+

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

+

+static int poll__close(struct hw_device_t *dev)

+{

+    FUNC_LOG;

+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;

+    if (ctx) {

+        delete ctx;

+    }

+    return 0;

+}

+

+static int poll__activate(struct sensors_poll_device_t *dev,

+                          int handle, int enabled)

+{

+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;

+    return ctx->activate(handle, enabled);

+}

+

+static int poll__setDelay(struct sensors_poll_device_t *dev,

+                          int handle, int64_t ns)

+{

+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;

+    int s= ctx->setDelay(handle, ns);

+    return s;

+}

+

+static int poll__poll(struct sensors_poll_device_t *dev,

+                      sensors_event_t* data, int count)

+{

+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;

+    return ctx->pollEvents(data, count);

+}

+

+static int poll__query(struct sensors_poll_device_1 *dev,

+                      int what, int *value)

+{

+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;

+    return ctx->query(what, value);

+}

+

+static int poll__batch(struct sensors_poll_device_1 *dev,

+                      int handle, int flags, int64_t period_ns, int64_t timeout)

+{

+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;

+    return ctx->batch(handle, flags, period_ns, timeout);

+}

+

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

+

+/** Open a new instance of a sensor device using name */

+static int open_sensors(const struct hw_module_t* module, const char* id,

+                        struct hw_device_t** device)

+{

+    FUNC_LOG;

+    int status = -EINVAL;

+    sensors_poll_context_t *dev = new sensors_poll_context_t();

+

+    memset(&dev->device, 0, sizeof(sensors_poll_device_1));

+

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

+    dev->device.common.version  = SENSORS_DEVICE_API_VERSION_1_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;

+

+    /* Batch processing */

+    dev->device.batch           = poll__batch; 

+

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

+    status = 0;

+

+    return status;

+}

diff --git a/65xx/libsensors_iio/software/build/android/common.mk b/65xx/libsensors_iio/software/build/android/common.mk
new file mode 100644
index 0000000..acd3deb
--- /dev/null
+++ b/65xx/libsensors_iio/software/build/android/common.mk
@@ -0,0 +1,87 @@
+# Use bash for additional echo fancyness
+SHELL = /bin/bash
+
+####################################################################################################
+## defines
+
+# Build for Jellybean 
+BUILD_ANDROID_JELLYBEAN = $(shell test -d $(ANDROID_ROOT)/frameworks/native && echo 1)
+
+## libraries ##
+LIB_PREFIX = lib
+
+STATIC_LIB_EXT = a
+SHARED_LIB_EXT = so
+
+# normally, overridden from outside 
+# ?= assignment sets it only if not already defined
+TARGET ?= android
+
+MLLITE_LIB_NAME     ?= mllite
+MPL_LIB_NAME        ?= mplmpu
+
+## applications ##
+SHARED_APP_SUFFIX = -shared
+STATIC_APP_SUFFIX = -static
+
+####################################################################################################
+## compile, includes, and linker
+
+ifeq ($(BUILD_ANDROID_JELLYBEAN),1)
+ANDROID_COMPILE = -DANDROID_JELLYBEAN=1
+endif
+
+ANDROID_LINK  = -nostdlib
+ANDROID_LINK += -fpic
+ANDROID_LINK += -Wl,--gc-sections 
+ANDROID_LINK += -Wl,--no-whole-archive 
+ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib
+ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+ANDROID_LINK_EXECUTABLE  = $(ANDROID_LINK)
+ANDROID_LINK_EXECUTABLE += -Wl,-dynamic-linker,/system/bin/linker
+ifneq ($(BUILD_ANDROID_JELLYBEAN),1)
+ANDROID_LINK_EXECUTABLE += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x
+endif
+ANDROID_LINK_EXECUTABLE += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o
+ANDROID_LINK_EXECUTABLE += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o
+
+ANDROID_INCLUDES  = -I$(ANDROID_ROOT)/system/core/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/libhardware/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/ril/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/dalvik/libnativehelper/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/base/include   # ICS
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/native/include # Jellybean
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/external/skia/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/arch-arm/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libstdc++/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/kernel/common
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/kernel/arch-arm
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/include/arch/arm
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libthread_db/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/arm
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/SHARED_LIBRARIES/libm_intermediates
+
+KERNEL_INCLUDES  = -I$(KERNEL_ROOT)/include
+
+INV_INCLUDES  = -I$(INV_ROOT)/software/core/driver/include
+INV_INCLUDES += -I$(MLLITE_DIR)
+INV_INCLUDES += -I$(MLLITE_DIR)/linux
+
+INV_DEFINES += -DINV_CACHE_DMP=1
+
+####################################################################################################
+## macros
+
+ifndef echo_in_colors
+define echo_in_colors
+	echo -ne "\e[1;32m"$(1)"\e[0m"
+endef 
+endif
+
+
+
diff --git a/65xx/libsensors_iio/software/build/android/shared.mk b/65xx/libsensors_iio/software/build/android/shared.mk
new file mode 100644
index 0000000..ac225cb
--- /dev/null
+++ b/65xx/libsensors_iio/software/build/android/shared.mk
@@ -0,0 +1,76 @@
+SHELL=/bin/bash
+
+TARGET       ?= android
+PRODUCT      ?= beagleboard
+ANDROID_ROOT ?= /Android/trunk/0xdroid/beagle-eclair
+KERNEL_ROOT  ?= /Android/trunk/0xdroid/kernel
+MLSDK_ROOT   ?= $(CURDIR)
+
+ifeq ($(VERBOSE),1)
+	DUMP=1>/dev/stdout
+else
+	DUMP=1>/dev/null
+endif
+
+include common.mk
+
+################################################################################
+## targets
+
+INV_ROOT = ../..
+LIB_FOLDERS  = $(INV_ROOT)/core/mllite/build/$(TARGET)
+ifeq ($(BUILD_MPL),1)
+	LIB_FOLDERS += $(INV_ROOT)/core/mpl/build/$(TARGET)
+endif
+APP_FOLDERS  = $(INV_ROOT)/simple_apps/mpu_iio/build/$(TARGET)
+APP_FOLDERS += $(INV_ROOT)/simple_apps/self_test/build/$(TARGET)
+APP_FOLDERS += $(INV_ROOT)/simple_apps/gesture_test/build/$(TARGET)
+APP_FOLDERS += $(INV_ROOT)/simple_apps/playback/linux/build/$(TARGET)
+APP_FOLDERS += $(INV_ROOT)/simple_apps/devnode_parser/build/$(TARGET)
+
+INSTALL_DIR = $(CURDIR)
+
+################################################################################
+## macros
+
+define echo_in_colors
+	echo -ne "\e[1;34m"$(1)"\e[0m"
+endef
+
+################################################################################
+## rules
+
+.PHONY : all mllite mpl clean
+
+all:
+	for DIR in $(LIB_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+	for DIR in $(APP_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+
+clean: 
+	for DIR in $(LIB_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+	for DIR in $(APP_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+
+cleanall:
+	for DIR in $(LIB_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+	for DIR in $(APP_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+
+install:
+	for DIR in $(LIB_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+	for DIR in $(APP_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+
diff --git a/65xx/libsensors_iio/software/core/driver/include/log.h b/65xx/libsensors_iio/software/core/driver/include/log.h
new file mode 100755
index 0000000..4da1efd
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/driver/include/log.h
@@ -0,0 +1,368 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*
+ * This file incorporates work covered by the following copyright and
+ * permission notice:
+ *
+ * Copyright (C) 2005 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ * C/C++ logging functions.  See the logging documentation for API details.
+ *
+ * We'd like these to be available from C code (in case we import some from
+ * somewhere), so this has a C interface.
+ *
+ * The output will be correct when the log file is shared between multiple
+ * threads and/or multiple processes so long as the operating system
+ * supports O_APPEND.  These calls have mutex-protected data structures
+ * and so are NOT reentrant.  Do not use MPL_LOG in a signal handler.
+ */
+#ifndef _LIBS_CUTILS_MPL_LOG_H
+#define _LIBS_CUTILS_MPL_LOG_H
+
+#include <stdlib.h>
+#include <stdarg.h>
+
+#ifdef ANDROID
+#ifdef NDK_BUILD
+#include "log_macros.h"
+#else
+#include <utils/Log.h>		/* For the LOG macro */
+#endif
+#endif
+
+#ifdef __KERNEL__
+#include <linux/kernel.h>
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if defined ANDROID_JELLYBEAN
+#define LOG ALOG
+#define LOG_ERRROR ANDROID_LOG_ERROR
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Normally we strip MPL_LOGV (VERBOSE messages) from release builds.
+ * You can modify this (for example with "#define MPL_LOG_NDEBUG 0"
+ * at the top of your source file) to change that behavior.
+ */
+#ifndef MPL_LOG_NDEBUG
+#ifdef NDEBUG
+#define MPL_LOG_NDEBUG 1
+#else
+#define MPL_LOG_NDEBUG 0
+#endif
+#endif
+
+#ifdef __KERNEL__
+#define MPL_LOG_UNKNOWN MPL_LOG_VERBOSE
+#define MPL_LOG_DEFAULT KERN_DEFAULT
+#define MPL_LOG_VERBOSE KERN_CONT
+#define MPL_LOG_DEBUG   KERN_NOTICE
+#define MPL_LOG_INFO    KERN_INFO
+#define MPL_LOG_WARN    KERN_WARNING
+#define MPL_LOG_ERROR   KERN_ERR
+#define MPL_LOG_SILENT  MPL_LOG_VERBOSE
+
+#else
+	/* Based off the log priorities in android
+	   /system/core/include/android/log.h */
+#define MPL_LOG_UNKNOWN		(0)
+#define MPL_LOG_DEFAULT		(1)
+#define MPL_LOG_VERBOSE		(2)
+#define MPL_LOG_DEBUG		(3)
+#define MPL_LOG_INFO		(4)
+#define MPL_LOG_WARN		(5)
+#define MPL_LOG_ERROR		(6)
+#define MPL_LOG_SILENT		(8)
+#endif
+
+
+/*
+ * This is the local tag used for the following simplified
+ * logging macros.  You can change this preprocessor definition
+ * before using the other macros to change the tag.
+ */
+#ifndef MPL_LOG_TAG
+#ifdef __KERNEL__
+#define MPL_LOG_TAG
+#else
+#define MPL_LOG_TAG NULL
+#endif
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Simplified macro to send a verbose log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGV
+#if MPL_LOG_NDEBUG
+#ifdef _WIN32
+#define MPL_LOGV(fmt, ...)						\
+	do {								\
+        __pragma (warning(suppress : 4127 )) \
+		if (0)							\
+			MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\
+            __pragma (warning(suppress : 4127 )) \
+    } while (0)
+#else
+#define MPL_LOGV(fmt, ...)						\
+	do {								\
+		if (0)							\
+			MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\
+    } while (0)
+#endif
+
+#else
+#define MPL_LOGV(fmt, ...) MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef CONDITION
+#define CONDITION(cond)     ((cond) != 0)
+#endif
+
+#ifndef MPL_LOGV_IF
+#if MPL_LOG_NDEBUG
+#define MPL_LOGV_IF(cond, fmt, ...)  \
+	do { if (0) MPL_LOG(fmt, ##__VA_ARGS__); } while (0)
+#else
+#define MPL_LOGV_IF(cond, fmt, ...) \
+	((CONDITION(cond))						\
+		? MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \
+		: (void)0)
+#endif
+#endif
+
+/*
+ * Simplified macro to send a debug log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGD
+#define MPL_LOGD(fmt, ...) MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+
+#ifndef MPL_LOGD_IF
+#define MPL_LOGD_IF(cond, fmt, ...) \
+	((CONDITION(cond))					       \
+		? MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__)  \
+		: (void)0)
+#endif
+
+/*
+ * Simplified macro to send an info log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGI
+#ifdef __KERNEL__
+#define MPL_LOGI(fmt, ...) pr_info(KERN_INFO MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOGI(fmt, ...) MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef MPL_LOGI_IF
+#define MPL_LOGI_IF(cond, fmt, ...) \
+	((CONDITION(cond))                                              \
+		? MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__)   \
+		: (void)0)
+#endif
+
+/*
+ * Simplified macro to send a warning log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGW
+#ifdef __KERNEL__
+#define MPL_LOGW(fmt, ...) printk(KERN_WARNING MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOGW(fmt, ...) MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef MPL_LOGW_IF
+#define MPL_LOGW_IF(cond, fmt, ...) \
+	((CONDITION(cond))					       \
+		? MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__)   \
+		: (void)0)
+#endif
+
+/*
+ * Simplified macro to send an error log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGE
+#ifdef __KERNEL__
+#define MPL_LOGE(fmt, ...) printk(KERN_ERR MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOGE(fmt, ...) MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef MPL_LOGE_IF
+#define MPL_LOGE_IF(cond, fmt, ...) \
+	((CONDITION(cond))					       \
+		? MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__)  \
+		: (void)0)
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Log a fatal error.  If the given condition fails, this stops program
+ * execution like a normal assertion, but also generating the given message.
+ * It is NOT stripped from release builds.  Note that the condition test
+ * is -inverted- from the normal assert() semantics.
+ */
+#define MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ...) \
+	((CONDITION(cond))					   \
+		? ((void)android_printAssert(#cond, MPL_LOG_TAG,   \
+						fmt, ##__VA_ARGS__))	\
+		: (void)0)
+
+#define MPL_LOG_ALWAYS_FATAL(fmt, ...) \
+	(((void)android_printAssert(NULL, MPL_LOG_TAG, fmt, ##__VA_ARGS__)))
+
+/*
+ * Versions of MPL_LOG_ALWAYS_FATAL_IF and MPL_LOG_ALWAYS_FATAL that
+ * are stripped out of release builds.
+ */
+#if MPL_LOG_NDEBUG
+#define MPL_LOG_FATAL_IF(cond, fmt, ...)				\
+	do {								\
+		if (0)							\
+			MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__); \
+	} while (0)
+#define MPL_LOG_FATAL(fmt, ...)						\
+	do {								\
+		if (0)							\
+			MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__)	\
+	} while (0)
+#else
+#define MPL_LOG_FATAL_IF(cond, fmt, ...) \
+	MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__)
+#define MPL_LOG_FATAL(fmt, ...) \
+	MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__)
+#endif
+
+/*
+ * Assertion that generates a log message when the assertion fails.
+ * Stripped out of release builds.  Uses the current MPL_LOG_TAG.
+ */
+#define MPL_LOG_ASSERT(cond, fmt, ...)			\
+	MPL_LOG_FATAL_IF(!(cond), fmt, ##__VA_ARGS__)
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Basic log message macro.
+ *
+ * Example:
+ *  MPL_LOG(MPL_LOG_WARN, NULL, "Failed with error %d", errno);
+ *
+ * The second argument may be NULL or "" to indicate the "global" tag.
+ */
+#ifndef MPL_LOG
+#define MPL_LOG(priority, tag, fmt, ...)		\
+	MPL_LOG_PRI(priority, tag, fmt, ##__VA_ARGS__)
+#endif
+
+/*
+ * Log macro that allows you to specify a number for the priority.
+ */
+#ifndef MPL_LOG_PRI
+#ifdef ANDROID
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+	LOG(priority, tag, fmt, ##__VA_ARGS__)
+#elif defined __KERNEL__
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+	pr_debug(MPL_##priority tag fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+	_MLPrintLog(MPL_##priority, tag, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+/*
+ * Log macro that allows you to pass in a varargs ("args" is a va_list).
+ */
+#ifndef MPL_LOG_PRI_VA
+#ifdef ANDROID
+#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \
+	android_vprintLog(priority, NULL, tag, fmt, args)
+#elif defined __KERNEL__
+/* not allowed in the Kernel because there is no dev_dbg that takes a va_list */
+#else
+#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \
+	_MLPrintVaLog(priority, NULL, tag, fmt, args)
+#endif
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * ===========================================================================
+ *
+ * The stuff in the rest of this file should not be used directly.
+ */
+
+#ifndef ANDROID
+int _MLPrintLog(int priority, const char *tag, const char *fmt,	...);
+int _MLPrintVaLog(int priority, const char *tag, const char *fmt, va_list args);
+/* Final implementation of actual writing to a character device */
+int _MLWriteLog(const char *buf, int buflen);
+#endif
+
+static inline void __print_result_location(int result,
+					   const char *file,
+					   const char *func, int line)
+{
+	MPL_LOGE("%s|%s|%d returning %d\n", file, func, line, result);
+}
+
+#ifdef _WIN32
+/* The pragma removes warning about expression being constant */
+#define LOG_RESULT_LOCATION(condition) \
+    do {								\
+		__print_result_location((int)(condition), __FILE__,	\
+					__func__, __LINE__);		\
+        __pragma (warning(suppress : 4127 )) \
+	} while (0)
+#else
+#define LOG_RESULT_LOCATION(condition) \
+    do {								\
+		__print_result_location((int)(condition), __FILE__,	\
+					__func__, __LINE__);		\
+	} while (0)
+#endif
+
+
+#define INV_ERROR_CHECK(r_1329) \
+    if (r_1329) { \
+        LOG_RESULT_LOCATION(r_1329); \
+        return r_1329; \
+    }
+
+#ifdef __cplusplus
+}
+#endif
+#endif				/* _LIBS_CUTILS_MPL_LOG_H */
diff --git a/65xx/libsensors_iio/software/core/driver/include/mlinclude.h b/65xx/libsensors_iio/software/core/driver/include/mlinclude.h
new file mode 100755
index 0000000..9725199
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/driver/include/mlmath.h b/65xx/libsensors_iio/software/core/driver/include/mlmath.h
new file mode 100755
index 0000000..37194d6
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/driver/include/mlsl.h b/65xx/libsensors_iio/software/core/driver/include/mlsl.h
new file mode 100755
index 0000000..12f2901
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/driver/include/mltypes.h b/65xx/libsensors_iio/software/core/driver/include/mltypes.h
new file mode 100755
index 0000000..09eccce
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/driver/include/stdint_invensense.h b/65xx/libsensors_iio/software/core/driver/include/stdint_invensense.h
new file mode 100755
index 0000000..9440228
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/mllite/build/android/libmllite.so b/65xx/libsensors_iio/software/core/mllite/build/android/libmllite.so
new file mode 100755
index 0000000..eba66ba
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/build/android/libmllite.so
diff --git a/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk b/65xx/libsensors_iio/software/core/mllite/build/android/shared.mk
index 7655e4d..1418450 100644..100755
--- a/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk
+++ b/65xx/libsensors_iio/software/core/mllite/build/android/shared.mk
@@ -1,21 +1,21 @@
-EXEC = inv_gesture_test$(SHARED_APP_SUFFIX)
+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
+LINK  ?= $(CROSS)gcc 
 
 OBJFOLDER = $(CURDIR)/obj
 
-INV_ROOT   = ../../../../..
-APP_DIR    = $(CURDIR)/../..
-MLLITE_DIR = $(INV_ROOT)/software/core/mllite
-MPL_DIR    = $(INV_ROOT)/software/core/mpl
+INV_ROOT = ../../../../..
+MLLITE_DIR  = $(INV_ROOT)/software/core/mllite
 
 include $(INV_ROOT)/software/build/android/common.mk
 
 CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += $(ANDROID_COMPILE)
 CFLAGS += -Wall
 CFLAGS += -fpic
 CFLAGS += -nostdlib
@@ -31,34 +31,28 @@ 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 += -I$(INV_ROOT)/simple_apps/common
 CFLAGS += $(INV_INCLUDES)
 CFLAGS += $(INV_DEFINES)
 
-LLINK  = -lc
-LLINK += -lm
-LLINK += -lutils
-LLINK += -lcutils
-LLINK += -lgcc
+LLINK  = -lc 
+LLINK += -lm 
+LLINK += -lutils 
+LLINK += -lcutils 
+LLINK += -lgcc 
 LLINK += -ldl
-LLINK += -lstdc++
-LLINK += -llog
-LLINK += -lz
 
 LFLAGS += $(CMDLINE_LFLAGS)
-LFLAGS += $(ANDROID_LINK_EXECUTABLE)
-
-LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+LFLAGS += -shared 
+LFLAGS += -Wl,-soname,$(LIBRARY)
+LFLAGS += -Wl,-shared,-Bsymbolic
+LFLAGS += $(ANDROID_LINK)
+LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
 
 ####################################################################################################
 ## sources
 
-INV_LIBS  = $(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
+#INV_SOURCES provided by Makefile.filelist
 include ../filelist.mk
 
 INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
@@ -67,13 +61,15 @@ INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES)
 ####################################################################################################
 ## rules
 
-.PHONY: all clean cleanall install
+.PHONY: all mllite clean cleanall makefiles
+
+all: mllite
 
-all: $(EXEC) $(MK_NAME)
+mllite: $(LIBRARY) $(MK_NAME)
 
-$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
-	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
-	$(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+$(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")
@@ -81,15 +77,11 @@ $(OBJFOLDER) :
 
 $(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 $<
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(CFLAGS) -o $@ -c $<
 
 clean : 
 	rm -fR $(OBJFOLDER)
 
 cleanall : 
-	rm -fR $(EXEC) $(OBJFOLDER)
-
-install : $(EXEC)
-	cp -f $(EXEC) $(INSTALL_DIR)
-
+	rm -fR $(LIBRARY) $(OBJFOLDER)
 
diff --git a/65xx/libsensors_iio/software/core/mllite/build/filelist.mk b/65xx/libsensors_iio/software/core/mllite/build/filelist.mk
new file mode 100755
index 0000000..011120c
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/mllite/data_builder.c b/65xx/libsensors_iio/software/core/mllite/data_builder.c
new file mode 100755
index 0000000..36cf2c6
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/data_builder.c
@@ -0,0 +1,1411 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/**
+ *   @defgroup  Data_Builder data_builder
+ *   @brief     Motion Library - Data Builder
+ *              Constructs and Creates the data for MPL
+ *
+ *   @{
+ *       @file data_builder.c
+ *       @brief Data Builder.
+ */
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 1 /* Use 0 to turn on MPL_LOGV output */
+
+#include <string.h>
+
+#include "ml_math_func.h"
+#include "data_builder.h"
+#include "mlmath.h"
+#include "storage_manager.h"
+#include "message_layer.h"
+#include "results_holder.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MLLITE"
+
+typedef inv_error_t (*inv_process_cb_func)(struct inv_sensor_cal_t *data);
+
+struct process_t {
+    inv_process_cb_func func;
+    int priority;
+    int data_required;
+};
+
+struct inv_data_builder_t {
+    int num_cb;
+    struct process_t process[INV_MAX_DATA_CB];
+    struct inv_db_save_t save;
+    struct inv_db_save_mpl_t save_mpl;
+    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;
+
+#ifdef INV_PLAYBACK_DBG
+
+/** Turn on data logging to allow playback of same scenario at a later time.
+* @param[in] file File to write to, must be open.
+*/
+void inv_turn_on_data_logging(FILE *file)
+{
+    MPL_LOGV("input data logging started\n");
+    inv_data_builder.file = file;
+    inv_data_builder.debug_mode = RD_RECORD;
+}
+
+/** Turn off data logging to allow playback of same scenario at a later time.
+* File passed to inv_turn_on_data_logging() must be closed after calling this.
+*/
+void inv_turn_off_data_logging()
+{
+    MPL_LOGV("input data logging stopped\n");
+    inv_data_builder.debug_mode = RD_NO_DEBUG;
+    inv_data_builder.file = NULL;
+}
+#endif
+
+/** Gets last value of raw compass data.
+* @param[out] raw Raw compass data in mounting frame in hardware units. Length 3.
+*/
+void inv_get_raw_compass(short *raw)
+{
+    memcpy(raw, sensors.compass.raw, sizeof(sensors.compass.raw));
+}
+
+/** This function receives the data that was stored in non-volatile memory between power off */
+static inv_error_t inv_db_load_func(const unsigned char *data)
+{
+    memcpy(&inv_data_builder.save, data, sizeof(inv_data_builder.save));
+    // copy in the saved accuracy in the actual sensors accuracy
+    sensors.gyro.accuracy = inv_data_builder.save.gyro_accuracy;
+    sensors.accel.accuracy = inv_data_builder.save.accel_accuracy;
+    sensors.compass.accuracy = inv_data_builder.save.compass_accuracy;
+    // TODO
+    if (sensors.compass.accuracy == 3) {
+        inv_set_compass_bias_found(1);
+    }
+    return INV_SUCCESS;
+}
+
+/** This function returns the data to be stored in non-volatile memory between power off */
+static inv_error_t inv_db_save_func(unsigned char *data)
+{
+    memcpy(data, &inv_data_builder.save, sizeof(inv_data_builder.save));
+    return INV_SUCCESS;
+}
+
+/** This function receives the data for mpl that was stored in non-volatile memory between power off */
+static inv_error_t inv_db_load_mpl_func(const unsigned char *data)
+{
+    memcpy(&inv_data_builder.save_mpl, data, sizeof(inv_data_builder.save_mpl));
+
+    return INV_SUCCESS;
+}
+
+/** This function returns the data for mpl to be stored in non-volatile memory between power off */
+static inv_error_t inv_db_save_mpl_func(unsigned char *data)
+{
+    memcpy(data, &inv_data_builder.save_mpl, sizeof(inv_data_builder.save_mpl));
+    return INV_SUCCESS;
+}
+
+/** Initialize the data builder
+*/
+inv_error_t inv_init_data_builder(void)
+{
+    /* TODO: Hardcode temperature scale/offset here. */
+    memset(&inv_data_builder, 0, sizeof(inv_data_builder));
+    memset(&sensors, 0, sizeof(sensors));
+
+    // disable the soft iron transform process
+    inv_reset_compass_soft_iron_matrix();
+
+    return ((inv_register_load_store(inv_db_load_func, inv_db_save_func,
+                                   sizeof(inv_data_builder.save),
+                                   INV_DB_SAVE_KEY))
+          | (inv_register_load_store(inv_db_load_mpl_func, inv_db_save_mpl_func,
+                                   sizeof(inv_data_builder.save_mpl),
+                                   INV_DB_SAVE_MPL_KEY)) );
+}
+
+/** Gyro sensitivity.
+* @return A scale factor to convert device units to degrees per second scaled by 2^16
+* such that degrees_per_second  = device_units * sensitivity / 2^30. Typically
+* it works out to be the maximum rate * 2^15.
+*/
+long inv_get_gyro_sensitivity(void)
+{
+    return sensors.gyro.sensitivity;
+}
+
+/** Accel sensitivity.
+* @return A scale factor to convert device units to g's scaled by 2^16
+* such that g_s  = device_units * sensitivity / 2^30. Typically
+* it works out to be the maximum accel value in g's * 2^15.
+*/
+long inv_get_accel_sensitivity(void)
+{
+    return sensors.accel.sensitivity;
+}
+
+/** Compass sensitivity.
+* @return A scale factor to convert device units to micro Tesla scaled by 2^16
+* such that uT  = device_units * sensitivity / 2^30. Typically
+* it works out to be the maximum uT * 2^15.
+*/
+long inv_get_compass_sensitivity(void)
+{
+    return sensors.compass.sensitivity;
+}
+
+/** Sets orientation and sensitivity field for a sensor.
+* @param[out] sensor Structure to apply settings to
+* @param[in] orientation Orientation description of how part is mounted.
+* @param[in] sensitivity A Scale factor to convert from hardware units to
+*            standard units (dps, uT, g).
+*/
+void set_sensor_orientation_and_scale(struct inv_single_sensor_t *sensor,
+                                 int orientation, long sensitivity)
+{
+    int error = 0;
+
+    if (!sensitivity) {
+        // Sensitivity can't be zero
+        sensitivity = 1L<<16;
+        MPL_LOGE("\n\nCritical error! Sensitivity is zero.\n\n");
+    }
+
+    sensor->sensitivity = sensitivity;
+    // Make sure we don't describe some impossible orientation
+    if ((orientation & 3) == 3) {
+        error = 1;
+    }
+    if ((orientation & 0x18) == 0x18) {
+        error = 1;
+    }
+    if ((orientation & 0xc0) == 0xc0) {
+        error = 1;
+    }
+    if (error) {
+        orientation = 0x88; // Identity
+        MPL_LOGE("\n\nCritical error! Impossible mounting orientation given. Using Identity instead\n\n");
+    }
+    sensor->orientation = orientation;
+}
+
+/** Sets the Orientation and Sensitivity of the gyro data.
+* @param[in] orientation A scalar defining the transformation from chip mounting
+*            to the body frame. The function inv_orientation_matrix_to_scalar()
+*            can convert the transformation matrix to this scalar and describes the
+*            scalar in further detail.
+* @param[in] sensitivity A scale factor to convert device units to degrees per second scaled by 2^16
+*            such that degrees_per_second  = device_units * sensitivity / 2^30. Typically
+*            it works out to be the maximum rate * 2^15.
+*/
+void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_G_ORIENT;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file);
+        fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file);
+    }
+#endif
+    set_sensor_orientation_and_scale(&sensors.gyro, orientation,
+                                     sensitivity);
+}
+
+/** Set Gyro Sample rate in micro seconds.
+* @param[in] sample_rate_us Set Gyro Sample rate in us
+*/
+void inv_set_gyro_sample_rate(long sample_rate_us)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_G_SAMPLE_RATE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.gyro.sample_rate_us = sample_rate_us;
+    sensors.gyro.sample_rate_ms = sample_rate_us / 1000;
+    if (sensors.gyro.bandwidth == 0) {
+        sensors.gyro.bandwidth = (int)(1000000L / sample_rate_us);
+    }
+}
+
+/** Set Accel Sample rate in micro seconds.
+* @param[in] sample_rate_us Set Accel Sample rate in us
+*/
+void inv_set_accel_sample_rate(long sample_rate_us)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_A_SAMPLE_RATE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.accel.sample_rate_us = sample_rate_us;
+    sensors.accel.sample_rate_ms = sample_rate_us / 1000;
+    if (sensors.accel.bandwidth == 0) {
+        sensors.accel.bandwidth = (int)(1000000L / sample_rate_us);
+    }
+}
+
+/** Set Compass Sample rate in micro seconds.
+* @param[in] sample_rate_us Set Gyro Sample rate in micro seconds.
+*/
+void inv_set_compass_sample_rate(long sample_rate_us)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_C_SAMPLE_RATE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.compass.sample_rate_us = sample_rate_us;
+    sensors.compass.sample_rate_ms = sample_rate_us / 1000;
+    if (sensors.compass.bandwidth == 0) {
+        sensors.compass.bandwidth = (int)(1000000L / sample_rate_us);
+    }
+}
+
+void inv_get_gyro_sample_rate_ms(long *sample_rate_ms)
+{
+	*sample_rate_ms = sensors.gyro.sample_rate_ms;
+}
+
+void inv_get_accel_sample_rate_ms(long *sample_rate_ms)
+{
+	*sample_rate_ms = sensors.accel.sample_rate_ms;
+}
+
+void inv_get_compass_sample_rate_ms(long *sample_rate_ms)
+{
+	*sample_rate_ms = sensors.compass.sample_rate_ms;
+}
+
+/** Set Quat Sample rate in micro seconds.
+* @param[in] sample_rate_us Set Quat Sample rate in us
+*/
+void inv_set_quat_sample_rate(long sample_rate_us)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.quat.sample_rate_us = sample_rate_us;
+    sensors.quat.sample_rate_ms = sample_rate_us / 1000;
+}
+
+/** Set Gyro Bandwidth in Hz
+* @param[in] bandwidth_hz Gyro bandwidth in Hz
+*/
+void inv_set_gyro_bandwidth(int bandwidth_hz)
+{
+    sensors.gyro.bandwidth = bandwidth_hz;
+}
+
+/** Set Accel Bandwidth in Hz
+* @param[in] bandwidth_hz Gyro bandwidth in Hz
+*/
+void inv_set_accel_bandwidth(int bandwidth_hz)
+{
+    sensors.accel.bandwidth = bandwidth_hz;
+}
+
+/** Set Compass Bandwidth in Hz
+* @param[in]  bandwidth_hz Gyro bandwidth in Hz
+*/
+void inv_set_compass_bandwidth(int bandwidth_hz)
+{
+    sensors.compass.bandwidth = bandwidth_hz;
+}
+
+/** Helper function stating whether the compass is on or off.
+ * @return TRUE if compass if on, 0 if compass if off
+*/
+int inv_get_compass_on()
+{
+    return (sensors.compass.status & INV_SENSOR_ON) == INV_SENSOR_ON;
+}
+
+/** Helper function stating whether the gyro is on or off.
+ * @return TRUE if gyro if on, 0 if gyro if off
+*/
+int inv_get_gyro_on()
+{
+    return (sensors.gyro.status & INV_SENSOR_ON) == INV_SENSOR_ON;
+}
+
+/** Helper function stating whether the acceleromter is on or off.
+ * @return TRUE if accel if on, 0 if accel if off
+*/
+int inv_get_accel_on()
+{
+    return (sensors.accel.status & INV_SENSOR_ON) == INV_SENSOR_ON;
+}
+
+/** Get last timestamp across all 3 sensors that are on.
+* This find out which timestamp has the largest value for sensors that are on.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_time_t inv_get_last_timestamp()
+{
+    inv_time_t timestamp = 0;
+    if (sensors.accel.status & INV_SENSOR_ON) {
+        timestamp = sensors.accel.timestamp;
+    }
+    if (sensors.gyro.status & INV_SENSOR_ON) {
+        if (timestamp < sensors.gyro.timestamp) {
+            timestamp = sensors.gyro.timestamp;
+        }
+    }
+    if (sensors.compass.status & INV_SENSOR_ON) {
+        if (timestamp < sensors.compass.timestamp) {
+            timestamp = sensors.compass.timestamp;
+        }
+    }
+    if (sensors.temp.status & INV_SENSOR_ON) {
+        if (timestamp < sensors.temp.timestamp)
+            timestamp = sensors.temp.timestamp;
+    }
+    if (sensors.quat.status & INV_SENSOR_ON) {
+        if (timestamp < sensors.quat.timestamp)
+            timestamp = sensors.quat.timestamp;
+    }
+    MPL_LOGV("values: %lld", timestamp);
+    return timestamp;
+}
+
+/** Sets the orientation and sensitivity of the gyro data.
+* @param[in] orientation A scalar defining the transformation from chip mounting
+*            to the body frame. The function inv_orientation_matrix_to_scalar()
+*            can convert the transformation matrix to this scalar and describes the
+*            scalar in further detail.
+* @param[in] sensitivity A scale factor to convert device units to g's
+*            such that g's = device_units * sensitivity / 2^30. Typically
+*            it works out to be the maximum g_value * 2^15.
+*/
+void inv_set_accel_orientation_and_scale(int orientation, long sensitivity)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_A_ORIENT;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file);
+        fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file);
+    }
+#endif
+    set_sensor_orientation_and_scale(&sensors.accel, orientation,
+                                     sensitivity);
+}
+
+/** Sets the Orientation and Sensitivity of the gyro data.
+* @param[in] orientation A scalar defining the transformation from chip mounting
+*            to the body frame. The function inv_orientation_matrix_to_scalar()
+*            can convert the transformation matrix to this scalar and describes the
+*            scalar in further detail.
+* @param[in] sensitivity A scale factor to convert device units to uT
+*            such that uT = device_units * sensitivity / 2^30. Typically
+*            it works out to be the maximum uT_value * 2^15.
+*/
+void inv_set_compass_orientation_and_scale(int orientation, long sensitivity)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_C_ORIENT;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file);
+        fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file);
+    }
+#endif
+    set_sensor_orientation_and_scale(&sensors.compass, orientation, sensitivity);
+}
+
+void inv_matrix_vector_mult(const long *A, const long *x, long *y)
+{
+    y[0] = inv_q30_mult(A[0], x[0]) + inv_q30_mult(A[1], x[1]) + inv_q30_mult(A[2], x[2]);
+    y[1] = inv_q30_mult(A[3], x[0]) + inv_q30_mult(A[4], x[1]) + inv_q30_mult(A[5], x[2]);
+    y[2] = inv_q30_mult(A[6], x[0]) + inv_q30_mult(A[7], x[1]) + inv_q30_mult(A[8], x[2]);
+}
+
+/** Takes raw data stored in the sensor, removes bias, and converts it to
+* calibrated data in the body frame. Also store raw data for body frame.
+* @param[in,out] sensor structure to modify
+* @param[in] bias bias in the mounting frame, in hardware units scaled by
+*                 2^16. Length 3.
+*/
+void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias)
+{
+    long raw32[3];
+
+    // Convert raw to calibrated
+    raw32[0] = (long)sensor->raw[0] << 15;
+    raw32[1] = (long)sensor->raw[1] << 15;
+    raw32[2] = (long)sensor->raw[2] << 15;
+
+    inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_scaled);
+
+    raw32[0] -= bias[0] >> 1;
+    raw32[1] -= bias[1] >> 1;
+    raw32[2] -= bias[2] >> 1;
+
+    inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->calibrated);
+
+    sensor->status |= INV_CALIBRATED;
+}
+
+/** Returns the current bias for the compass
+* @param[out] bias Compass bias in hardware units scaled by 2^16. In mounting frame.
+*             Length 3.
+*/
+void inv_get_compass_bias(long *bias)
+{
+    if (bias != NULL) {
+        memcpy(bias, inv_data_builder.save.compass_bias, sizeof(inv_data_builder.save.compass_bias));
+    }
+}
+
+/** Sets the compass bias
+* @param[in] bias Length 3, in body frame, in hardware units scaled by 2^16 to allow fractional bit correction.
+* @param[in] accuracy Accuracy of compass data, where 3=most accurate, and 0=least accurate.
+*/
+void inv_set_compass_bias(const long *bias, int accuracy)
+{
+    if (memcmp(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias))) {
+        memcpy(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias));
+        inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias);
+    }
+    sensors.compass.accuracy = accuracy;
+    inv_data_builder.save.compass_accuracy = accuracy;
+    inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0);
+}
+
+/** Set the state of a compass disturbance
+* @param[in] dist 1=disturbance, 0=no disturbance
+*/
+void inv_set_compass_disturbance(int dist)
+{
+    inv_data_builder.compass_disturbance = dist;
+}
+
+int inv_get_compass_disturbance(void) {
+    return inv_data_builder.compass_disturbance;
+}
+/** Sets the accel bias.
+* @param[in] bias 
+*               Accel bias, length 3. In HW units (+/- 2 gee full scale assumed)
+                scaled by 2^16 in body frame.
+* @param[in] accuracy 
+*               Accuracy rating from 0 to 3, with 3 being most accurate.
+*/
+void inv_set_accel_bias(const long *bias, int accuracy)
+{
+    if (bias) {
+        if (memcmp(inv_data_builder.save.accel_bias, bias, 
+                   sizeof(inv_data_builder.save.accel_bias))) {
+            memcpy(inv_data_builder.save.accel_bias, bias, 
+                   sizeof(inv_data_builder.save.accel_bias));
+            inv_apply_calibration(&sensors.accel, 
+                                  inv_data_builder.save.accel_bias);
+        }
+    }
+    sensors.accel.accuracy = accuracy;
+    inv_data_builder.save.accel_accuracy = accuracy;
+    inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
+}
+
+/** Sets the accel accuracy.
+* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate.
+*/
+void inv_set_accel_accuracy(int accuracy)
+{
+    sensors.accel.accuracy = accuracy;
+    inv_data_builder.save.accel_accuracy = accuracy;
+    inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
+}
+
+/** Sets the accel bias with control over which axis.
+* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame
+* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate.
+* @param[in] mask Mask to select axis to apply bias set.
+*/
+void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask)
+{
+    if (bias) {
+        if (mask & 1){
+            inv_data_builder.save.accel_bias[0] = bias[0];
+        }
+        if (mask & 2){
+            inv_data_builder.save.accel_bias[1] = bias[1];
+        }
+        if (mask & 4){
+            inv_data_builder.save.accel_bias[2] = bias[2];
+        }
+
+        inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias);
+    }
+    sensors.accel.accuracy = accuracy;
+    inv_data_builder.save.accel_accuracy = accuracy;
+    inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
+}
+
+/** 
+ *  Sets the factory gyro bias
+ *  @param[in] bias 
+ *               Gyro bias in hardware units (+/- 2000 dps full scale assumed) 
+ *               scaled by 2^16. In chip mounting frame. Length of 3.
+ */
+void inv_set_gyro_bias(const long *bias)
+{
+    if (!bias)
+        return;
+
+    if (memcmp(inv_data_builder.save.factory_gyro_bias, bias,
+               sizeof(inv_data_builder.save.factory_gyro_bias))) {
+        memcpy(inv_data_builder.save.factory_gyro_bias, bias,
+               sizeof(inv_data_builder.save.factory_gyro_bias));
+    }
+    inv_set_message(INV_MSG_NEW_FGB_EVENT, INV_MSG_NEW_FGB_EVENT, 0);
+}
+
+/** 
+ *  Sets the mpl gyro bias
+ *  @param[in] bias
+ *              Gyro bias in hardware units scaled by 2^16  (+/- 2000 dps full
+ *              scale assumed). In chip mounting frame. Length 3.
+ *  @param[in] accuracy
+ *              Accuracy of bias. 0 = least accurate, 3 = most accurate.
+ */
+void inv_set_mpl_gyro_bias(const long *bias, int accuracy)
+{
+    if (bias != NULL) {
+        if (memcmp(inv_data_builder.save_mpl.gyro_bias, bias, 
+                   sizeof(inv_data_builder.save_mpl.gyro_bias))) {
+            memcpy(inv_data_builder.save_mpl.gyro_bias, bias, 
+                   sizeof(inv_data_builder.save_mpl.gyro_bias));
+            inv_apply_calibration(&sensors.gyro,
+                                  inv_data_builder.save_mpl.gyro_bias);
+        }
+    }
+    sensors.gyro.accuracy = accuracy;
+    inv_data_builder.save.gyro_accuracy = accuracy;
+
+    /* TODO: What should we do if there's no temperature data? */
+    if (sensors.temp.calibrated[0])
+        inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0];
+    else
+        /* Set to 27 deg C for now until we've got a better solution. */
+        inv_data_builder.save.gyro_temp = 27L << 16;
+    inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0);
+
+    /* TODO: this flag works around the synchronization problem seen with using
+       the user-exposed message layer to signal the temperature compensation
+       module that gyro biases were set.
+       A better, cleaner method is certainly needed. */
+    inv_data_builder.save.gyro_bias_tc_set = true;
+}
+
+/**
+ *  @internal
+ *  @brief  Return whether gyro biases were set to signal the temperature
+ *          compensation algorithm that it can collect a data point to build
+ *          the temperature slope while in no motion state.
+ *          The flag clear automatically after is read.
+ *  @return true if the flag was set, indicating gyro biases were set.
+ *          false if the flag was not set.
+ */
+int inv_get_gyro_bias_tc_set(void)
+{
+    int flag = (inv_data_builder.save.gyro_bias_tc_set == true);
+    inv_data_builder.save.gyro_bias_tc_set = false;
+    return flag;
+}
+
+
+/**
+ *  Get the mpl gyro biases
+ *  @param[in] bias
+ *              Gyro calibrated bias.
+ *              Length 3.
+ */
+void inv_get_mpl_gyro_bias(long *bias, long *temp)
+{
+    if (bias != NULL)
+        memcpy(bias, inv_data_builder.save_mpl.gyro_bias,
+               sizeof(inv_data_builder.save_mpl.gyro_bias));
+
+    if (temp != NULL)
+        temp[0] = inv_data_builder.save.gyro_temp;
+}
+
+/** Gyro Bias in the form used by the DMP.
+* @param[out] bias Gyro Bias in the form used by the DMP. It is scaled appropriately
+*             and is in the body frame as needed. If this bias is applied in the DMP
+*             then any quaternion must have the flag INV_BIAS_APPLIED set if it is a
+*             3-axis quaternion, or INV_QUAT_6AXIS if it is a 6-axis quaternion
+*/
+void inv_get_gyro_bias_dmp_units(long *bias)
+{
+    if (bias == NULL)
+        return;
+    inv_convert_to_body_with_scale(sensors.gyro.orientation,  46850825L, inv_data_builder.save_mpl.gyro_bias, bias);
+}
+
+/* TODO: Add this information to inv_sensor_cal_t */
+/**
+ *  Get the gyro biases and temperature record from MPL
+ *  @param[in] bias
+ *              Gyro bias in hardware units scaled by 2^16.
+ *              In chip mounting frame.
+ *              Length 3.
+ */
+void inv_get_gyro_bias(long *bias)
+{
+    if (bias != NULL)
+        memcpy(bias, inv_data_builder.save.factory_gyro_bias,
+               sizeof(inv_data_builder.save.factory_gyro_bias));
+}
+
+/** Get Accel Bias
+* @param[out] bias Accel bias
+* @param[out] temp Temperature where 1 C = 2^16
+*/
+void inv_get_accel_bias(long *bias, long *temp)
+{
+    if (bias != NULL)
+        memcpy(bias, inv_data_builder.save.accel_bias,
+               sizeof(inv_data_builder.save.accel_bias));
+    if (temp != NULL)
+        temp[0] = inv_data_builder.save.accel_temp;
+}
+
+/**
+ *  Record new accel data for use when inv_execute_on_data() is called
+ *  @param[in]  accel 
+ *                accel data, length 3.
+ *                Calibrated data is in m/s^2 scaled by 2^16 in body frame.
+ *                Raw data is in device units in chip mounting frame.
+ *  @param[in]  status
+ *                Lower 2 bits are the accuracy, with 0 being inaccurate, and 3
+ *                being most accurate.
+ *                The upper bit INV_CALIBRATED, is set if the data was calibrated
+ *                outside MPL and it is not set if the data being passed is raw.
+ *                Raw data should be in device units, typically in a 16-bit range.
+ *  @param[in]  timestamp
+ *                Monotonic time stamp, for Android it's in nanoseconds.
+ *  @return     Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_build_accel(const long *accel, int status, inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_ACCEL;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(accel, sizeof(accel[0]), 3, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+
+    if ((status & INV_CALIBRATED) == 0) {
+        sensors.accel.raw[0] = (short)accel[0];
+        sensors.accel.raw[1] = (short)accel[1];
+        sensors.accel.raw[2] = (short)accel[2];
+        sensors.accel.status |= INV_RAW_DATA;
+        inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias);
+    } else {
+        sensors.accel.calibrated[0] = accel[0];
+        sensors.accel.calibrated[1] = accel[1];
+        sensors.accel.calibrated[2] = accel[2];
+        sensors.accel.status |= INV_CALIBRATED;
+        sensors.accel.accuracy = status & 3;
+        inv_data_builder.save.accel_accuracy = status & 3;
+    }
+    sensors.accel.status |= INV_NEW_DATA | INV_SENSOR_ON;
+    sensors.accel.timestamp_prev = sensors.accel.timestamp;
+    sensors.accel.timestamp = timestamp;
+
+    return INV_SUCCESS;
+}
+
+/** Record new gyro data and calls inv_execute_on_data() if previous
+* sample has not been processed.
+* @param[in] gyro Data is in device units. Length 3.
+* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds.
+* @param[out] executed Set to 1 if data processing was done.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_GYRO;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(gyro, sizeof(gyro[0]), 3, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+
+    memcpy(sensors.gyro.raw, gyro, 3 * sizeof(short));
+    sensors.gyro.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
+    sensors.gyro.timestamp_prev = sensors.gyro.timestamp;
+    sensors.gyro.timestamp = timestamp;
+    inv_apply_calibration(&sensors.gyro, inv_data_builder.save_mpl.gyro_bias);
+
+    return INV_SUCCESS;
+}
+
+/** Record new compass data for use when inv_execute_on_data() is called
+* @param[in] compass Compass data, if it was calibrated outside MPL, the units are uT scaled by 2^16.
+*            Length 3.
+* @param[in] status Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 being most accurate.
+*            The upper bit INV_CALIBRATED, is set if the data was calibrated outside MPL and it is
+*            not set if the data being passed is raw. Raw data should be in device units, typically
+*            in a 16-bit range.
+* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds.
+* @param[out] executed Set to 1 if data processing was done.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_build_compass(const long *compass, int status,
+                              inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_COMPASS;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(compass, sizeof(compass[0]), 3, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+
+    if ((status & INV_CALIBRATED) == 0) {
+        long data[3];
+        inv_set_compass_soft_iron_input_data(compass);
+        inv_get_compass_soft_iron_output_data(data);
+        sensors.compass.raw[0] = (short)data[0];
+        sensors.compass.raw[1] = (short)data[1];
+        sensors.compass.raw[2] = (short)data[2];
+        inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias);
+        sensors.compass.status |= INV_RAW_DATA;
+    } else {
+        sensors.compass.calibrated[0] = compass[0];
+        sensors.compass.calibrated[1] = compass[1];
+        sensors.compass.calibrated[2] = compass[2];
+        sensors.compass.status |= INV_CALIBRATED;
+        sensors.compass.accuracy = status & 3;
+        inv_data_builder.save.compass_accuracy = status & 3;
+    }
+    sensors.compass.timestamp_prev = sensors.compass.timestamp;
+    sensors.compass.timestamp = timestamp;
+    sensors.compass.status |= INV_NEW_DATA | INV_SENSOR_ON;
+
+    return INV_SUCCESS;
+}
+
+/** Record new temperature data for use when inv_execute_on_data() is called.
+ *  @param[in]  temp Temperature data in q16 format.
+ *  @param[in]  timestamp   Monotonic time stamp; for Android it's in
+ *                          nanoseconds.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_build_temp(const long temp, inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_TEMPERATURE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&temp, sizeof(temp), 1, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.temp.calibrated[0] = temp;
+    sensors.temp.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
+    sensors.temp.timestamp_prev = sensors.temp.timestamp;
+    sensors.temp.timestamp = timestamp;
+    /* TODO: Apply scale, remove offset. */
+
+    return INV_SUCCESS;
+}
+/** quaternion data
+* @param[in] quat Quaternion data. 2^30 = 1.0 or 2^14=1 for 16-bit data.
+*                 Real part first. Length 4.
+* @param[in] status number of axis, 16-bit or 32-bit
+* @param[in] timestamp
+* @param[in]  timestamp   Monotonic time stamp; for Android it's in
+*                         nanoseconds.
+* @param[out] executed Set to 1 if data processing was done.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_QUAT;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(quat, sizeof(quat[0]), 4, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+
+    /* Android version DMP does not have scalar part */
+    if (status & INV_QUAT_3AXIS) {
+        long resultQuat[4];
+        MPL_LOGV("3q: %ld,%ld,%ld\n", quat[0], quat[1], quat[2]);
+        inv_compute_scalar_part(quat, resultQuat);
+        MPL_LOGV("4q: %ld,%ld,%ld,%ld\n", resultQuat[0], resultQuat[1], resultQuat[2], resultQuat[3]);
+        memcpy(sensors.quat.raw, resultQuat, sizeof(sensors.quat.raw));
+    } else {
+        memcpy(sensors.quat.raw, quat, sizeof(sensors.quat.raw));
+    }
+    sensors.quat.timestamp = timestamp;
+    sensors.quat.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
+    sensors.quat.status |= (INV_BIAS_APPLIED & status);
+    sensors.quat.status |= (INV_QUAT_9AXIS & status);
+    sensors.quat.status |= (INV_DMP_BIAS_APPLIED & status);
+    if (sensors.quat.status & (INV_QUAT_9AXIS | INV_QUAT_6AXIS)) {
+        // Set quaternion
+        inv_store_gaming_quaternion(quat, timestamp);
+    }
+    if (sensors.quat.status & INV_QUAT_9AXIS) {
+        long identity[4] = {(1L<<30), 0, 0, 0};
+        inv_set_compass_correction(identity, timestamp);
+    }
+    return INV_SUCCESS;
+}
+
+/** This should be called when the accel has been turned off. This is so
+* that we will know if the data is contiguous.
+*/
+void inv_accel_was_turned_off()
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_COMPASS_OFF;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.accel.status = 0;
+}
+
+/** This should be called when the compass has been turned off. This is so
+* that we will know if the data is contiguous.
+*/
+void inv_compass_was_turned_off()
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_COMPASS_OFF;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.compass.status = 0;
+}
+
+/** This should be called when the quaternion data from the DMP has been turned off. This is so
+* that we will know if the data is contiguous.
+*/
+void inv_quaternion_sensor_was_turned_off(void)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_QUAT_OFF;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.quat.status = 0;
+}
+
+/** This should be called when the gyro has been turned off. This is so
+* that we will know if the data is contiguous.
+*/
+void inv_gyro_was_turned_off()
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_GYRO_OFF;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.gyro.status = 0;
+}
+
+/** This should be called when the temperature sensor has been turned off.
+ *  This is so that we will know if the data is contiguous.
+ */
+void inv_temperature_was_turned_off()
+{
+    sensors.temp.status = 0;
+}
+
+/** Registers to receive a callback when there is new sensor data.
+* @internal
+* @param[in] func Function pointer to receive callback when there is new sensor data
+* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority
+*            numbers must be unique.
+* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be
+*            a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW =
+*            gyro data, INV_MAG_NEW = compass data. So passing in
+*            INV_ACCEL_NEW | INV_MAG_NEW, a
+*            callback would be generated if there was new magnetomer data OR new accel data.
+*/
+inv_error_t inv_register_data_cb(
+    inv_error_t (*func)(struct inv_sensor_cal_t *data),
+    int priority, int sensor_type)
+{
+    inv_error_t result = INV_SUCCESS;
+    int kk, nn;
+
+    // Make sure we haven't registered this function already
+    // Or used the same priority
+    for (kk = 0; kk < inv_data_builder.num_cb; ++kk) {
+        if ((inv_data_builder.process[kk].func == func) ||
+                (inv_data_builder.process[kk].priority == priority)) {
+            return INV_ERROR_INVALID_PARAMETER;    //fixme give a warning
+        }
+    }
+
+    // Make sure we have not filled up our number of allowable callbacks
+    if (inv_data_builder.num_cb <= INV_MAX_DATA_CB - 1) {
+        kk = 0;
+        if (inv_data_builder.num_cb != 0) {
+            // set kk to be where this new callback goes in the array
+            while ((kk < inv_data_builder.num_cb) &&
+                    (inv_data_builder.process[kk].priority < priority)) {
+                kk++;
+            }
+            if (kk != inv_data_builder.num_cb) {
+                // We need to move the others
+                for (nn = inv_data_builder.num_cb; nn > kk; --nn) {
+                    inv_data_builder.process[nn] =
+                        inv_data_builder.process[nn - 1];
+                }
+            }
+        }
+        // Add new callback
+        inv_data_builder.process[kk].func = func;
+        inv_data_builder.process[kk].priority = priority;
+        inv_data_builder.process[kk].data_required = sensor_type;
+        inv_data_builder.num_cb++;
+    } else {
+        MPL_LOGE("Unable to add feature callback as too many were already registered\n");
+        result = INV_ERROR_MEMORY_EXAUSTED;
+    }
+
+    return result;
+}
+
+/** Unregisters the callback that happens when new sensor data is received.
+* @internal
+* @param[in] func Function pointer to receive callback when there is new sensor data
+* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority
+*            numbers must be unique.
+* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be
+*            a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW =
+*            gyro data, INV_MAG_NEW = compass data. So passing in
+*            INV_ACCEL_NEW | INV_MAG_NEW, a
+*            callback would be generated if there was new magnetomer data OR new accel data.
+*/
+inv_error_t inv_unregister_data_cb(
+    inv_error_t (*func)(struct inv_sensor_cal_t *data))
+{
+    int kk, nn;
+
+    for (kk = 0; kk < inv_data_builder.num_cb; ++kk) {
+        if (inv_data_builder.process[kk].func == func) {
+            // Delete this callback
+            for (nn = kk + 1; nn < inv_data_builder.num_cb; ++nn) {
+                inv_data_builder.process[nn - 1] =
+                    inv_data_builder.process[nn];
+            }
+            inv_data_builder.num_cb--;
+            return INV_SUCCESS;
+        }
+    }
+
+    return INV_SUCCESS;    // We did not find the callback
+}
+
+/** After at least one of inv_build_gyro(), inv_build_accel(), or
+* inv_build_compass() has been called, this function should be called.
+* It will process the data it has received and update all the internal states
+* and features that have been turned on.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_execute_on_data(void)
+{
+    inv_error_t result, first_error;
+    int kk;
+    int mode;
+
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_EXECUTE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+    }
+#endif
+    // Determine what new data we have
+    mode = 0;
+    if (sensors.gyro.status & INV_NEW_DATA)
+        mode |= INV_GYRO_NEW;
+    if (sensors.accel.status & INV_NEW_DATA)
+        mode |= INV_ACCEL_NEW;
+    if (sensors.compass.status & INV_NEW_DATA)
+        mode |= INV_MAG_NEW;
+    if (sensors.temp.status & INV_NEW_DATA)
+        mode |= INV_TEMP_NEW;
+    if (sensors.quat.status & INV_QUAT_NEW)
+        mode |= INV_QUAT_NEW;
+
+    first_error = INV_SUCCESS;
+
+    for (kk = 0; kk < inv_data_builder.num_cb; ++kk) {
+        if (mode & inv_data_builder.process[kk].data_required) {
+            result = inv_data_builder.process[kk].func(&sensors);
+            if (result && !first_error) {
+                first_error = result;
+            }
+        }
+    }
+
+    inv_set_contiguous();
+
+    return first_error;
+}
+
+/** Cleans up status bits after running all the callbacks. It sets the contiguous flag.
+*
+*/
+static void inv_set_contiguous(void)
+{
+    inv_time_t current_time = 0;
+    if (sensors.gyro.status & INV_NEW_DATA) {
+        sensors.gyro.status |= INV_CONTIGUOUS;
+        current_time = sensors.gyro.timestamp;
+    }
+    if (sensors.accel.status & INV_NEW_DATA) {
+        sensors.accel.status |= INV_CONTIGUOUS;
+        current_time = MAX(current_time, sensors.accel.timestamp);
+    }
+    if (sensors.compass.status & INV_NEW_DATA) {
+        sensors.compass.status |= INV_CONTIGUOUS;
+        current_time = MAX(current_time, sensors.compass.timestamp);
+    }
+    if (sensors.temp.status & INV_NEW_DATA) {
+        sensors.temp.status |= INV_CONTIGUOUS;
+        current_time = MAX(current_time, sensors.temp.timestamp);
+    }
+    if (sensors.quat.status & INV_NEW_DATA) {
+        sensors.quat.status |= INV_CONTIGUOUS;
+        current_time = MAX(current_time, sensors.quat.timestamp);
+    }
+
+#if 0
+    /* See if sensors are still on. These should be turned off by inv_*_was_turned_off()
+     * type functions. This is just in case that breaks down. We make sure
+     * all the data is within 2 seconds of the newest piece of data*/
+    if (inv_delta_time_ms(current_time, sensors.gyro.timestamp) >= 2000)
+        inv_gyro_was_turned_off();
+    if (inv_delta_time_ms(current_time, sensors.accel.timestamp) >= 2000)
+        inv_accel_was_turned_off();
+    if (inv_delta_time_ms(current_time, sensors.compass.timestamp) >= 2000)
+        inv_compass_was_turned_off();
+    /* TODO: Temperature might not need to be read this quickly. */
+    if (inv_delta_time_ms(current_time, sensors.temp.timestamp) >= 2000)
+        inv_temperature_was_turned_off();
+#endif
+
+    /* clear bits */
+    sensors.gyro.status &= ~INV_NEW_DATA;
+    sensors.accel.status &= ~INV_NEW_DATA;
+    sensors.compass.status &= ~INV_NEW_DATA;
+    sensors.temp.status &= ~INV_NEW_DATA;
+    sensors.quat.status &= ~INV_NEW_DATA;
+}
+
+/** Gets a whole set of accel data including data, accuracy and timestamp.
+ * @param[out] data Accel Data where 1g = 2^16
+ * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
+ * @param[out] timestamp The timestamp of the data sample.
+*/
+void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+    if (data != NULL) {
+        memcpy(data, sensors.accel.calibrated, sizeof(sensors.accel.calibrated));
+    }
+    if (timestamp != NULL) {
+        *timestamp = sensors.accel.timestamp;
+    }
+    if (accuracy != NULL) {
+        *accuracy = sensors.accel.accuracy;
+    }
+}
+
+/** Gets a whole set of gyro data including data, accuracy and timestamp.
+ * @param[out] data Gyro Data where 1 dps = 2^16
+ * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
+ * @param[out] timestamp The timestamp of the data sample.
+*/
+void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+    memcpy(data, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated));
+    if (timestamp != NULL) {
+        *timestamp = sensors.gyro.timestamp;
+    }
+    if (accuracy != NULL) {
+        *accuracy = sensors.gyro.accuracy;
+    }
+}
+
+/** Gets a whole set of gyro raw data including data, accuracy and timestamp.
+ * @param[out] data Gyro Data where 1 dps = 2^16
+ * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
+ * @param[out] timestamp The timestamp of the data sample.
+*/
+void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+    memcpy(data, sensors.gyro.raw_scaled, sizeof(sensors.gyro.raw_scaled));
+    if (timestamp != NULL) {
+        *timestamp = sensors.gyro.timestamp;
+    }
+    if (accuracy != NULL) {
+        *accuracy = 0;
+    }
+}
+
+/** Get's latest gyro data.
+* @param[out] gyro Gyro Data, Length 3. 1 dps = 2^16.
+*/
+void inv_get_gyro(long *gyro)
+{
+    memcpy(gyro, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated));
+}
+
+/** Gets a whole set of compass data including data, accuracy and timestamp.
+ * @param[out] data Compass Data where 1 uT = 2^16
+ * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
+ * @param[out] timestamp The timestamp of the data sample.
+*/
+void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+    memcpy(data, sensors.compass.calibrated, sizeof(sensors.compass.calibrated));
+    if (timestamp != NULL) {
+        *timestamp = sensors.compass.timestamp;
+    }
+    if (accuracy != NULL) {
+        if (inv_data_builder.compass_disturbance)
+            *accuracy = 0;
+        else
+            *accuracy = sensors.compass.accuracy;
+    }
+}
+
+/** Gets a whole set of compass raw data including data, accuracy and timestamp.
+ * @param[out] data Compass Data where 1 uT = 2^16
+ * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
+ * @param[out] timestamp The timestamp of the data sample.
+*/
+void inv_get_compass_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+    memcpy(data, sensors.compass.raw_scaled, sizeof(sensors.compass.raw_scaled));
+    if (timestamp != NULL) {
+        *timestamp = sensors.compass.timestamp;
+    }
+    //per Michele, since data is raw, accuracy should = 0
+    *accuracy = 0;
+}
+
+/** Gets a whole set of temperature data including data, accuracy and timestamp.
+ *  @param[out] data        Temperature data where 1 degree C = 2^16
+ *  @param[out] accuracy    0 to 3, where 3 is most accurate.
+ *  @param[out] timestamp   The timestamp of the data sample.
+ */
+void inv_get_temp_set(long *data, int *accuracy, inv_time_t *timestamp)
+{
+    data[0] = sensors.temp.calibrated[0];
+    if (timestamp)
+        *timestamp = sensors.temp.timestamp;
+    if (accuracy)
+        *accuracy = sensors.temp.accuracy;
+}
+
+/** Returns accuracy of gyro.
+ * @return Accuracy of gyro with 0 being not accurate, and 3 being most accurate.
+*/
+int inv_get_gyro_accuracy(void)
+{
+    return sensors.gyro.accuracy;
+}
+
+/** Returns accuracy of compass.
+ * @return Accuracy of compass with 0 being not accurate, and 3 being most accurate.
+*/
+int inv_get_mag_accuracy(void)
+{
+    if (inv_data_builder.compass_disturbance)
+        return 0;
+    return sensors.compass.accuracy;
+}
+
+/** Returns accuracy of accel.
+ * @return Accuracy of accel with 0 being not accurate, and 3 being most accurate.
+*/
+int inv_get_accel_accuracy(void)
+{
+    return sensors.accel.accuracy;
+}
+
+inv_error_t inv_get_gyro_orient(int *orient)
+{
+    *orient = sensors.gyro.orientation;
+    return 0;
+}
+
+inv_error_t inv_get_accel_orient(int *orient)
+{
+    *orient = sensors.accel.orientation;
+    return 0;
+}
+
+/*======================================================================*/
+/*   compass soft iron module                                           */
+/*======================================================================*/
+
+/** Gets the 3x3 compass transform matrix in 32 bit Q30 fixed point format.
+ * @param[out] the pointer of the 3x3 matrix in Q30 format
+*/
+void inv_get_compass_soft_iron_matrix_d(long *matrix) {
+    int i;
+    for (i=0; i<9; i++)  {
+        matrix[i] = sensors.soft_iron.matrix_d[i];
+    }
+}
+
+/** Sets the 3x3 compass transform matrix in 32 bit Q30 fixed point format.
+ * @param[in] the pointer of the 3x3 matrix in Q30 format
+*/
+void inv_set_compass_soft_iron_matrix_d(long *matrix)  {
+    int i;
+    for (i=0; i<9; i++)  {
+        // set the floating point matrix
+        sensors.soft_iron.matrix_d[i] = matrix[i];
+        // convert to Q30 format
+        sensors.soft_iron.matrix_f[i] = inv_q30_to_float(matrix[i]);
+    }
+}
+/** Gets the 3x3 compass transform matrix in 32 bit floating point format.
+ * @param[out] the pointer of the 3x3 matrix in floating point format
+*/
+void inv_get_compass_soft_iron_matrix_f(float *matrix)  {
+    int i;
+    for (i=0; i<9; i++)  {
+        matrix[i] = sensors.soft_iron.matrix_f[i];
+    }
+}
+/** Sets the 3x3 compass transform matrix in 32 bit floating point format.
+ * @param[in] the pointer of the 3x3 matrix in floating point format
+*/
+void inv_set_compass_soft_iron_matrix_f(float *matrix)   {
+    int i;
+    for (i=0; i<9; i++)  {
+        // set the floating point matrix
+        sensors.soft_iron.matrix_f[i] = matrix[i];
+        // convert to Q30 format
+        sensors.soft_iron.matrix_d[i] = (long )(matrix[i]*ROT_MATRIX_SCALE_LONG);
+    }
+}
+
+/** This subroutine gets the fixed point Q30 compass data after the soft iron transformation.
+ * @param[out] the pointer of the 3x1 vector compass data in MPL format
+*/
+void inv_get_compass_soft_iron_output_data(long *data) {
+    int i;
+    for (i=0; i<3; i++)  {
+        data[i] = sensors.soft_iron.trans[i];
+    }
+}
+/** This subroutine gets the fixed point Q30 compass data before the soft iron transformation.
+ * @param[out] the pointer of the 3x1 vector compass data in MPL format
+*/
+void inv_get_compass_soft_iron_input_data(long *data)  {
+    int i;
+    for (i=0; i<3; i++)  {
+        data[i] = sensors.soft_iron.raw[i];
+    }
+}
+/** This subroutine sets the compass raw data for the soft iron transformation.
+ * @param[int] the pointer of the 3x1 vector compass raw data in MPL format
+*/
+void inv_set_compass_soft_iron_input_data(const long *data)  {
+    int i;
+    for (i=0; i<3; i++)  {
+        sensors.soft_iron.raw[i] = data[i];
+    }
+    if (sensors.soft_iron.enable == 1)  {
+        mlMatrixVectorMult(sensors.soft_iron.matrix_d, data, sensors.soft_iron.trans);
+    } else {
+        for (i=0; i<3; i++)  {
+            sensors.soft_iron.trans[i] = data[i];
+        }
+    }
+}
+
+/** This subroutine resets the the soft iron transformation to unity matrix and
+ * disable the soft iron transformation process by default.
+*/
+void inv_reset_compass_soft_iron_matrix(void)  {
+    int i;
+    for (i=0; i<9; i++) {
+        sensors.soft_iron.matrix_f[i] = 0.0f;
+    }
+
+    memset(&sensors.soft_iron.matrix_d,0,sizeof(sensors.soft_iron.matrix_d));
+
+    for (i=0; i<3; i++)  {
+        // set the floating point matrix
+        sensors.soft_iron.matrix_f[i*4] = 1.0;
+        // set the fixed point matrix
+        sensors.soft_iron.matrix_d[i*4] = ROT_MATRIX_SCALE_LONG;
+    }
+
+    inv_disable_compass_soft_iron_matrix();
+}
+
+/** This subroutine enables the the soft iron transformation process.
+*/
+void inv_enable_compass_soft_iron_matrix(void)   {
+    sensors.soft_iron.enable = 1;
+}
+
+/** This subroutine disables the the soft iron transformation process.
+*/
+void inv_disable_compass_soft_iron_matrix(void)   {
+    sensors.soft_iron.enable = 0;
+}
+
+/**
+ * @}
+ */
diff --git a/65xx/libsensors_iio/software/core/mllite/data_builder.h b/65xx/libsensors_iio/software/core/mllite/data_builder.h
new file mode 100755
index 0000000..b59be03
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/data_builder.h
@@ -0,0 +1,308 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#ifndef INV_DATA_BUILDER_H__
+#define INV_DATA_BUILDER_H__
+
+#include <stdio.h>
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Uncomment this flag to enable playback debug and record or playback scenarios
+//#define INV_PLAYBACK_DBG
+
+/** This is a new sample of accel data */
+#define INV_ACCEL_NEW 1
+/** This is a new sample of gyro data */
+#define INV_GYRO_NEW 2
+/** This is a new sample of compass data */
+#define INV_MAG_NEW 4
+/** This is a new sample of temperature data */
+#define INV_TEMP_NEW 8
+/** This is a new sample of quaternion data */
+#define INV_QUAT_NEW 16
+
+/** Set if the data is contiguous. Typically not set if a sample was skipped */
+#define INV_CONTIGUOUS 16
+/** Set if the calibrated data has been solved for */
+#define INV_CALIBRATED 32
+/** INV_NEW_DATA set for a new set of data, cleared if not available. */
+#define INV_NEW_DATA 64
+/** Set if raw data exists */
+#define INV_RAW_DATA 128
+/** Set if the sensor is on */
+#define INV_SENSOR_ON 256
+/** Set if quaternion has bias correction applied */
+#define INV_BIAS_APPLIED 512
+/** Set if quaternion is 6-axis */
+#define INV_QUAT_6AXIS 1024
+/** Set if quaternion is 9 axis */
+#define INV_QUAT_9AXIS 2048
+/** Set if quaternion is 3-axis, 3 elements (android only) */
+#define INV_QUAT_3AXIS 4096
+/** Set if DMP has applied bias */
+#define INV_DMP_BIAS_APPLIED 8192
+
+#define INV_PRIORITY_MOTION_NO_MOTION          100
+#define INV_PRIORITY_GYRO_TC                   150
+#define INV_PRIORITY_QUATERNION_GYRO_ACCEL     200
+#define INV_PRIORITY_QUATERNION_NO_GYRO        250
+#define INV_PRIORITY_MAGNETIC_DISTURBANCE      300
+#define INV_PRIORITY_HEADING_FROM_GYRO         350
+#define INV_PRIORITY_COMPASS_BIAS_W_GYRO       375
+#define INV_PRIORITY_COMPASS_VECTOR_CAL        400
+#define INV_PRIORITY_COMPASS_ADV_BIAS          500
+#define INV_PRIORITY_9_AXIS_FUSION             600
+#define INV_PRIORITY_9_AXIS_FUSION_LIGHT       650
+#define INV_PRIORITY_QUATERNION_ADJUST_9_AXIS  700
+#define INV_PRIORITY_QUATERNION_ACCURACY       750
+#define INV_PRIORITY_RESULTS_HOLDER            800
+#define INV_PRIORITY_INUSE_AUTO_CALIBRATION    850
+#define INV_PRIORITY_HAL_OUTPUTS               900
+#define INV_PRIORITY_GLYPH                     950
+#define INV_PRIORITY_SHAKE                     975
+#define INV_PRIORITY_SM                        1000
+
+struct inv_single_sensor_t {
+    /** Orientation Descriptor. Describes how to go from the mounting frame to the body frame when
+    * the rotation matrix could be thought of only having elements of 0,1,-1.
+    * 2 bits are used to describe the column of the 1 or -1 and the 3rd bit is used for the sign.
+    * Bit 8 is sign of +/- 1 in third row. Bit 6-7 is column of +/-1 in third row.
+    * Bit 5 is sign of +/- 1 in second row. Bit 3-4 is column of +/-1 in second row.
+    * Bit 2 is sign of +/- 1 in first row. Bit 0-1 is column of +/-1 in first row.
+    */
+    int orientation;
+    /** The raw data in raw data units in the mounting frame */
+    short raw[3];
+    /** Raw data in body frame */
+    long raw_scaled[3];
+    /** Calibrated data */
+    long calibrated[3];
+    long sensitivity;
+    /** Sample rate in microseconds */
+    long sample_rate_us;
+    long sample_rate_ms;
+    /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample
+    * skipped due to power savings turning off this sensor.
+    * INV_NEW_DATA set for a new set of data, cleared if not available.
+    * INV_CALIBRATED_SET if calibrated data has been solved for */
+    int status;
+    /** 0 to 3 for how well sensor data and biases are known. 3 is most accurate. */
+    int accuracy;
+    inv_time_t timestamp;
+    inv_time_t timestamp_prev;
+    /** Bandwidth in Hz */
+    int bandwidth;
+};
+
+struct inv_quat_sensor_t {
+    long raw[4];
+    /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample
+    * skipped due to power savings turning off this sensor.
+    * INV_NEW_DATA set for a new set of data, cleared if not available.
+    * INV_CALIBRATED_SET if calibrated data has been solved for */
+    int status;
+    inv_time_t timestamp;
+    long sample_rate_us;
+    long sample_rate_ms;
+};
+
+struct inv_soft_iron_t {
+    long raw[3];
+    long trans[3];
+    long matrix_d[9];  // Q30 format fixed point. The dynamic range is (-2.0 to 2.0);
+    float matrix_f[9];
+
+    int enable;
+};
+
+struct inv_sensor_cal_t {
+    struct inv_single_sensor_t gyro;
+    struct inv_single_sensor_t accel;
+    struct inv_single_sensor_t compass;
+    struct inv_single_sensor_t temp;
+    struct inv_quat_sensor_t quat;
+    struct inv_soft_iron_t soft_iron;
+    /** Combinations of INV_GYRO_NEW, INV_ACCEL_NEW, INV_MAG_NEW to indicate
+    * which data is a new sample as these data points may have different sample rates.
+    */
+    int status;
+};
+
+// Useful for debug record and playback
+typedef enum {
+    RD_NO_DEBUG,
+    RD_RECORD,
+    RD_PLAYBACK
+} rd_dbg_mode;
+
+typedef enum {
+    PLAYBACK_DBG_TYPE_GYRO,
+    PLAYBACK_DBG_TYPE_ACCEL,
+    PLAYBACK_DBG_TYPE_COMPASS,
+    PLAYBACK_DBG_TYPE_TEMPERATURE,
+    PLAYBACK_DBG_TYPE_EXECUTE,
+    PLAYBACK_DBG_TYPE_A_ORIENT,
+    PLAYBACK_DBG_TYPE_G_ORIENT,
+    PLAYBACK_DBG_TYPE_C_ORIENT,
+    PLAYBACK_DBG_TYPE_A_SAMPLE_RATE,
+    PLAYBACK_DBG_TYPE_C_SAMPLE_RATE,
+    PLAYBACK_DBG_TYPE_G_SAMPLE_RATE,
+    PLAYBACK_DBG_TYPE_GYRO_OFF,
+    PLAYBACK_DBG_TYPE_ACCEL_OFF,
+    PLAYBACK_DBG_TYPE_COMPASS_OFF,
+    PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE,
+    PLAYBACK_DBG_TYPE_QUAT,
+    PLAYBACK_DBG_TYPE_QUAT_OFF
+} inv_rd_dbg_states;
+
+/** Change this key if the definition of the struct inv_db_save_t changes.
+    Previous keys: 53394, 53395 */
+#define INV_DB_SAVE_KEY (53396)
+
+#define INV_DB_SAVE_MPL_KEY (50001)
+
+struct inv_db_save_t {
+    /** compass Bias in chip frame, hardware units scaled by 2^16. */
+    long compass_bias[3];
+    /** gyro factory bias in chip frame, hardware units scaled by 2^16,
+        +/- 2000 dps full scale. */
+    long factory_gyro_bias[3];
+    /** temperature when factory_gyro_bias was stored. */
+    long gyro_temp;
+    /** flag to indicate temperature compensation that biases where stored. */
+    int gyro_bias_tc_set;
+    /** accel bias in chip frame, hardware units scaled by 2^16, 
+        +/- 2 gee full scale. */
+    long accel_bias[3];
+    /** temperature when accel bias was stored. */
+    long accel_temp;
+    long gyro_temp_slope[3];
+    /** sensor accuracies */
+    int gyro_accuracy;
+    int accel_accuracy;
+    int compass_accuracy;
+};
+
+struct inv_db_save_mpl_t {
+    /** gyro bias in chip frame, hardware units scaled by 2^16, +/- 2000 dps
+        full scale */
+    long gyro_bias[3];
+};
+
+/** Maximum number of data callbacks that are supported. Safe to increase if needed.*/
+#define INV_MAX_DATA_CB 20
+
+#ifdef INV_PLAYBACK_DBG
+void inv_turn_on_data_logging(FILE *file);
+void inv_turn_off_data_logging();
+#endif
+
+void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity);
+void inv_set_accel_orientation_and_scale(int orientation,
+        long sensitivity);
+void inv_set_compass_orientation_and_scale(int orientation,
+        long sensitivity);
+void inv_set_gyro_sample_rate(long sample_rate_us);
+void inv_set_compass_sample_rate(long sample_rate_us);
+void inv_set_quat_sample_rate(long sample_rate_us);
+void inv_set_accel_sample_rate(long sample_rate_us);
+void inv_set_gyro_bandwidth(int bandwidth_hz);
+void inv_set_accel_bandwidth(int bandwidth_hz);
+void inv_set_compass_bandwidth(int bandwidth_hz);
+
+void inv_get_gyro_sample_rate_ms(long *sample_rate_ms);
+void inv_get_accel_sample_rate_ms(long *sample_rate_ms);
+void inv_get_compass_sample_rate_ms(long *sample_rate_ms);
+
+inv_error_t inv_register_data_cb(inv_error_t (*func)
+                                 (struct inv_sensor_cal_t * data), int priority,
+                                 int sensor_type);
+inv_error_t inv_unregister_data_cb(inv_error_t (*func)
+                                   (struct inv_sensor_cal_t * data));
+
+inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp);
+inv_error_t inv_build_compass(const long *compass, int status,
+                                  inv_time_t timestamp);
+inv_error_t inv_build_accel(const long *accel, int status,
+                            inv_time_t timestamp);
+inv_error_t inv_build_temp(const long temp, inv_time_t timestamp);
+inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp);
+inv_error_t inv_execute_on_data(void);
+
+void inv_get_compass_bias(long *bias);
+
+void inv_set_compass_bias(const long *bias, int accuracy);
+void inv_set_compass_disturbance(int dist);
+void inv_set_gyro_bias(const long *bias);
+void inv_set_mpl_gyro_bias(const long *bias, int accuracy);
+void inv_set_accel_bias(const long *bias, int accuracy);
+void inv_set_accel_accuracy(int accuracy);
+void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask);
+
+void inv_get_compass_soft_iron_matrix_d(long *matrix);
+void inv_set_compass_soft_iron_matrix_d(long *matrix);
+
+void inv_get_compass_soft_iron_matrix_f(float *matrix);
+void inv_set_compass_soft_iron_matrix_f(float *matrix);
+
+void inv_get_compass_soft_iron_output_data(long *data);
+void inv_get_compass_soft_iron_input_data(long *data);
+void inv_set_compass_soft_iron_input_data(const long *data);
+
+void inv_reset_compass_soft_iron_matrix(void);
+void inv_enable_compass_soft_iron_matrix(void);
+void inv_disable_compass_soft_iron_matrix(void);
+
+void inv_get_mpl_gyro_bias(long *bias, long *temp);
+void inv_get_gyro_bias(long *bias);
+void inv_get_gyro_bias_dmp_units(long *bias);
+void inv_get_accel_bias(long *bias, long *temp);
+
+void inv_gyro_was_turned_off(void);
+void inv_accel_was_turned_off(void);
+void inv_compass_was_turned_off(void);
+void inv_quaternion_sensor_was_turned_off(void);
+inv_error_t inv_init_data_builder(void);
+long inv_get_gyro_sensitivity(void);
+long inv_get_accel_sensitivity(void);
+long inv_get_compass_sensitivity(void);
+
+void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t * timestamp);
+void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t * timestamp);
+void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t * timestamp);
+void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t * timestamp);
+void inv_get_compass_set_raw(long *data, int8_t *accuracy, inv_time_t * timestamp);
+
+void inv_get_gyro(long *gyro);
+
+int inv_get_gyro_accuracy(void);
+int inv_get_accel_accuracy(void);
+int inv_get_mag_accuracy(void);
+void inv_get_raw_compass(short *raw);
+
+int inv_get_compass_on(void);
+int inv_get_gyro_on(void);
+int inv_get_accel_on(void);
+
+inv_time_t inv_get_last_timestamp(void);
+int inv_get_compass_disturbance(void);
+
+// new DMP cal functions
+inv_error_t inv_get_gyro_orient(int *orient);
+inv_error_t inv_get_accel_orient(int *orient);
+
+// internal
+int inv_get_gyro_bias_tc_set(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* INV_DATA_BUILDER_H__ */
diff --git a/65xx/libsensors_iio/software/core/mllite/hal_outputs.c b/65xx/libsensors_iio/software/core/mllite/hal_outputs.c
new file mode 100755
index 0000000..232e656
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/hal_outputs.c
@@ -0,0 +1,697 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */

+

+/**

+ *   @defgroup  HAL_Outputs hal_outputs

+ *   @brief     Motion Library - HAL Outputs

+ *              Sets up common outputs for HAL

+ *

+ *   @{

+ *       @file  hal_outputs.c

+ *       @brief HAL Outputs.

+ */

+

+#undef MPL_LOG_NDEBUG

+#define MPL_LOG_NDEBUG 1 /* Use 0 to turn on MPL_LOGV output */

+#undef MPL_LOG_TAG

+#define MPL_LOG_TAG "MLLITE"

+

+#include <string.h>

+

+#include "hal_outputs.h"

+#include "log.h"

+#include "ml_math_func.h"

+#include "mlmath.h"

+#include "start_manager.h"

+#include "data_builder.h"

+#include "results_holder.h"

+

+/* commenting this define out will bypass the low pass filter noise reduction

+   filter for compass data.

+   Disable this only for testing purpose (e.g. comparing the raw and calibrated

+   compass data, since the former is unfiltered and the latter is filtered,

+   leading to a small difference in the readings sample by sample).

+   Android specifications require this filter to be enabled to have the Magnetic

+   Field output's standard deviation fall below 0.5 uT.

+   */

+#define CALIB_COMPASS_NOISE_REDUCTION

+

+struct hal_output_t {

+    int accuracy_mag;    /**< Compass accuracy */

+    //int accuracy_gyro;   /**< Gyro Accuracy */

+    //int accuracy_accel;  /**< Accel Accuracy */

+    int accuracy_quat;   /**< quat Accuracy */

+

+    inv_time_t nav_timestamp;

+    inv_time_t gam_timestamp;

+    //inv_time_t accel_timestamp;

+    inv_time_t mag_timestamp;

+    long nav_quat[4];

+    int gyro_status;

+    int accel_status;

+    int compass_status;

+    int nine_axis_status;

+    int quat_status;

+    inv_biquad_filter_t lp_filter[3];

+    float compass_float[3];

+};

+

+static struct hal_output_t hal_out;

+

+/** Acceleration (m/s^2) in body frame.

+* @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it

+*             should return a vector of magnitude near 9.81 m/s^2

+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.

+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to

+*             inv_build_accel().

+* @return     Returns 1 if the data was updated or 0 if it was not updated.

+*/

+int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy,

+                                       inv_time_t * timestamp)

+{

+    int status;

+    /* Converts fixed point to m/s^2. Fixed point has 1g = 2^16.

+     * So this 9.80665 / 2^16 */

+#define ACCEL_CONVERSION 0.000149637603759766f

+    long accel[3];

+    inv_get_accel_set(accel, accuracy, timestamp);

+    values[0] = accel[0] * ACCEL_CONVERSION;

+    values[1] = accel[1] * ACCEL_CONVERSION;

+    values[2] = accel[2] * ACCEL_CONVERSION;

+    if (hal_out.accel_status & INV_NEW_DATA)

+        status = 1;

+    else

+        status = 0;

+    return status;

+}

+

+/** Linear Acceleration (m/s^2) in Body Frame.

+* @param[out] values Linear Acceleration in body frame, length 3, (m/s^2). May show

+*             accel biases while at rest.

+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.

+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to

+*             inv_build_accel().

+* @return     Returns 1 if the data was updated or 0 if it was not updated.

+*/

+int inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy,

+        inv_time_t * timestamp)

+{

+    long gravity[3], accel[3];

+

+    inv_get_accel_set(accel, accuracy, timestamp);

+    inv_get_gravity(gravity);

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

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

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

+    values[0] = accel[0] * ACCEL_CONVERSION;

+    values[1] = accel[1] * ACCEL_CONVERSION;

+    values[2] = accel[2] * ACCEL_CONVERSION;

+

+    return hal_out.nine_axis_status;

+}

+

+/** Gravity vector (m/s^2) in Body Frame.

+* @param[out] values Gravity vector in body frame, length 3, (m/s^2)

+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.

+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to

+*             inv_build_accel().

+* @return     Returns 1 if the data was updated or 0 if it was not updated.

+*/

+int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,

+                                 inv_time_t * timestamp)

+{

+    long gravity[3];

+    int status;

+

+    *accuracy = (int8_t) hal_out.accuracy_quat;

+    *timestamp = hal_out.nav_timestamp;

+    inv_get_gravity(gravity);

+    values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION;

+    values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION;

+    values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION;

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

+        status = 1;

+    else

+        status = 0;

+    return status;

+}

+

+/* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16.

+ * So this is: pi / 2^16 / 180 */

+#define GYRO_CONVERSION 2.66316109007924e-007f

+

+/** Gyroscope calibrated data (rad/s) in body frame.

+* @param[out] values Rotation Rate in rad/sec.

+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.

+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to

+*             inv_build_gyro().

+* @return     Returns 1 if the data was updated or 0 if it was not updated.

+*/

+int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,

+                                   inv_time_t * timestamp)

+{

+    long gyro[3];

+    int status;

+

+    inv_get_gyro_set(gyro, accuracy, timestamp);

+

+    values[0] = gyro[0] * GYRO_CONVERSION;

+    values[1] = gyro[1] * GYRO_CONVERSION;

+    values[2] = gyro[2] * GYRO_CONVERSION;

+    if (hal_out.gyro_status & INV_NEW_DATA)

+        status = 1;

+    else

+        status = 0;

+    return status;

+}

+

+/** Gyroscope raw data (rad/s) in body frame.

+* @param[out] values Rotation Rate in rad/sec.

+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate,

+*                      while 3 is most accurate.

+* @param[out] timestamp The timestamp for this sensor. Derived from the

+*                       timestamp sent to inv_build_gyro().

+* @return     Returns 1 if the data was updated or 0 if it was not updated.

+*/

+int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy,

+                                      inv_time_t * timestamp)

+{

+    long gyro[3];

+    int status;

+

+    inv_get_gyro_set_raw(gyro, accuracy, timestamp);

+    values[0] = gyro[0] * GYRO_CONVERSION;

+    values[1] = gyro[1] * GYRO_CONVERSION;

+    values[2] = gyro[2] * GYRO_CONVERSION;

+    if (hal_out.gyro_status & INV_NEW_DATA)

+        status = 1;

+    else

+        status = 0;

+    return status;

+}

+

+/**

+* This corresponds to Sensor.TYPE_ROTATION_VECTOR.

+* The rotation vector represents the orientation of the device as a combination

+* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$

+* around an axis {x, y, z}. <br>

+* The three elements of the rotation vector are

+* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)}, such that the magnitude of the rotation

+* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is

+* equal to the direction of the axis of rotation.

+*

+* The three elements of the rotation vector are equal to the last three components of a unit quaternion

+* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)>. The 4th element is cos(@f$\theta@f$/2).

+*

+* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor.

+* The reference coordinate system is defined as a direct orthonormal basis, where:

+

+    -X is defined as the vector product Y.Z (It is tangential to the ground at the device's current location and roughly points East).

+    -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole.

+    -Z points towards the sky and is perpendicular to the ground.

+* @param[out] values Length 4, 4th one being the heading accuracy at 95%.

+* @param[out] accuracy Accuracy is not defined

+* @param[out] timestamp Timestamp. In (ns) for Android.

+* @return     Returns 1 if the data was updated or 0 if it was not updated.

+*/

+int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy,

+        inv_time_t * timestamp)

+{

+    *accuracy = (int8_t) hal_out.accuracy_quat;

+    *timestamp = hal_out.nav_timestamp;

+

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

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

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

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

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

+    } else {

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

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

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

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

+    }

+    values[4] = inv_get_heading_confidence_interval();

+    return hal_out.nine_axis_status;

+}

+

+int inv_get_sensor_type_rotation_vector_6_axis(float *values, int8_t *accuracy,

+        inv_time_t * timestamp)

+{

+    int status;

+    long accel[3], quat_6_axis[4];

+    inv_get_accel_set(accel, accuracy, timestamp);

+

+    hal_out.gam_timestamp = hal_out.nav_timestamp;

+    *timestamp = hal_out.gam_timestamp;

+

+    inv_get_6axis_quaternion(quat_6_axis);

+

+    if (quat_6_axis[0] >= 0) {

+        values[0] = quat_6_axis[1] * INV_TWO_POWER_NEG_30;

+        values[1] = quat_6_axis[2] * INV_TWO_POWER_NEG_30;

+        values[2] = quat_6_axis[3] * INV_TWO_POWER_NEG_30;

+        values[3] = quat_6_axis[0] * INV_TWO_POWER_NEG_30;

+    } else {

+        values[0] = -quat_6_axis[1] * INV_TWO_POWER_NEG_30;

+        values[1] = -quat_6_axis[2] * INV_TWO_POWER_NEG_30;

+        values[2] = -quat_6_axis[3] * INV_TWO_POWER_NEG_30;

+        values[3] = -quat_6_axis[0] * INV_TWO_POWER_NEG_30;

+    }

+    //This sensor does not report an estimated heading accuracy

+    values[4] = 0;

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

+        status = 1;

+    else

+        status = 0;

+    MPL_LOGV("values:%f %f %f %f %f -%d -%lld", values[0], values[1],

+                              values[2], values[3], values[4], status, *timestamp);

+    return status;

+}

+

+/**

+* This corresponds to Sensor.TYPE_GEOMAGNETIC_ROTATION_VECTOR.

+* Similar to SENSOR_TYPE_ROTATION_VECTOR, but using a magnetometer

+* instead of using a gyroscope.

+* Fourth element = estimated_accuracy in radians (heading confidence).

+* @param[out] values Length 4.

+* @param[out] accuracy is not defined.

+* @param[out] timestamp in (ns) for Android.

+* @return     Returns 1 if the data was updated, 0 otherwise.

+*/

+int inv_get_sensor_type_geomagnetic_rotation_vector(float *values, int8_t *accuracy,

+        inv_time_t * timestamp)

+{

+    long compass[3], quat_geomagnetic[4];

+    int status;

+    inv_get_compass_set(compass, accuracy, timestamp);

+    inv_get_geomagnetic_quaternion(quat_geomagnetic, timestamp);

+    if (quat_geomagnetic[0] >= 0) {

+        values[0] = quat_geomagnetic[1] * INV_TWO_POWER_NEG_30;

+        values[1] = quat_geomagnetic[2] * INV_TWO_POWER_NEG_30;

+        values[2] = quat_geomagnetic[3] * INV_TWO_POWER_NEG_30;

+        values[3] = quat_geomagnetic[0] * INV_TWO_POWER_NEG_30;

+    } else {

+        values[0] = -quat_geomagnetic[1] * INV_TWO_POWER_NEG_30;

+        values[1] = -quat_geomagnetic[2] * INV_TWO_POWER_NEG_30;

+        values[2] = -quat_geomagnetic[3] * INV_TWO_POWER_NEG_30;

+        values[3] = -quat_geomagnetic[0] * INV_TWO_POWER_NEG_30;

+    }

+    values[4] = inv_get_accel_compass_confidence_interval();

+    status = hal_out.accel_status & INV_NEW_DATA? 1 : 0;

+    MPL_LOGV("values:%f %f %f %f %f -%d", values[0], values[1],

+                              values[2], values[3], values[4], status);

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

+    int i;

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

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

+//#define COMPASS_CONVERSION 1.52587890625e-005f

+

+    *timestamp = hal_out.mag_timestamp;

+    *accuracy = (int8_t) hal_out.accuracy_mag;

+

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

+        values[i] = hal_out.compass_float[i];

+    if (hal_out.compass_status & INV_NEW_DATA)

+        status = 1;

+    else

+        status = 0;

+    return status;

+}

+

+/** Compass raw data (uT) in body frame.

+* @param[out] values Compass data in (uT), length 3. May be calibrated by having

+*             biases removed and sensitivity adjusted

+* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate

+* @param[out] timestamp Timestamp. In (ns) for Android.

+* @return     Returns 1 if the data was updated or 0 if it was not updated.

+*/

+int inv_get_sensor_type_magnetic_field_raw(float *values, int8_t *accuracy,

+                                           inv_time_t * timestamp)

+{

+    long mag[3];

+    int status;

+    int i;

+

+    inv_get_compass_set_raw(mag, accuracy, timestamp);

+

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

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

+#define COMPASS_CONVERSION 1.52587890625e-005f

+

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

+        values[i] = (float)mag[i] * COMPASS_CONVERSION;

+    }

+    if (hal_out.compass_status & INV_NEW_DATA)

+        status = 1;

+    else

+        status = 0;

+    return status;

+}

+static void inv_get_rotation_geomagnetic(float r[3][3])

+{

+    long rot[9], quat_geo[4];

+    float conv = 1.f / (1L<<30);

+    inv_time_t timestamp;

+    inv_get_geomagnetic_quaternion(quat_geo, &timestamp);

+    inv_quaternion_to_rotation(quat_geo, rot);

+    r[0][0] = rot[0]*conv;

+    r[0][1] = rot[1]*conv;

+    r[0][2] = rot[2]*conv;

+    r[1][0] = rot[3]*conv;

+    r[1][1] = rot[4]*conv;

+    r[1][2] = rot[5]*conv;

+    r[2][0] = rot[6]*conv;

+    r[2][1] = rot[7]*conv;

+    r[2][2] = rot[8]*conv;

+}

+static void google_orientation_geomagnetic(float *g)

+{

+    float rad2deg = (float)(180.0 / M_PI);

+    float R[3][3];

+    inv_get_rotation_geomagnetic(R);

+    g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg;

+    g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg;

+    g[2] = asinf ( R[2][0])          * rad2deg;

+    if (g[0] < 0)

+        g[0] += 360;

+}

+

+static void inv_get_rotation_6_axis(float r[3][3])

+{

+    long rot[9], quat_6_axis[4];

+    float conv = 1.f / (1L<<30);

+

+    inv_get_6axis_quaternion(quat_6_axis);

+    inv_quaternion_to_rotation(quat_6_axis, rot);

+    r[0][0] = rot[0]*conv;

+    r[0][1] = rot[1]*conv;

+    r[0][2] = rot[2]*conv;

+    r[1][0] = rot[3]*conv;

+    r[1][1] = rot[4]*conv;

+    r[1][2] = rot[5]*conv;

+    r[2][0] = rot[6]*conv;

+    r[2][1] = rot[7]*conv;

+    r[2][2] = rot[8]*conv;

+}

+

+static void google_orientation_6_axis(float *g)

+{

+    float rad2deg = (float)(180.0 / M_PI);

+    float R[3][3];

+

+    inv_get_rotation_6_axis(R);

+

+    g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg;

+    g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg;

+    g[2] = asinf ( R[2][0])          * rad2deg;

+    if (g[0] < 0)

+        g[0] += 360;

+}

+

+static void inv_get_rotation(float r[3][3])

+{

+    long rot[9];

+    float conv = 1.f / (1L<<30);

+

+    inv_quaternion_to_rotation(hal_out.nav_quat, rot);

+    r[0][0] = rot[0]*conv;

+    r[0][1] = rot[1]*conv;

+    r[0][2] = rot[2]*conv;

+    r[1][0] = rot[3]*conv;

+    r[1][1] = rot[4]*conv;

+    r[1][2] = rot[5]*conv;

+    r[2][0] = rot[6]*conv;

+    r[2][1] = rot[7]*conv;

+    r[2][2] = rot[8]*conv;

+}

+

+static void google_orientation(float *g)

+{

+    float rad2deg = (float)(180.0 / M_PI);

+    float R[3][3];

+

+    inv_get_rotation(R);

+

+    g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg;

+    g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg;

+    g[2] = asinf ( R[2][0])          * rad2deg;

+    if (g[0] < 0)

+        g[0] += 360;

+}

+

+/** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees.

+* @param[out] values Length 3, Degrees.<br>

+*        - values[0]: Azimuth, angle between the magnetic north direction

+*         and the y-axis, around the z-axis (0 to 359). 0=North, 90=East, 180=South, 270=West<br>

+*        - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values

+*         when the z-axis moves toward the y-axis.<br>

+*        - values[2]: Roll, rotation around y-axis (-90 to 90), with positive

+*          values when the x-axis moves toward the z-axis.<br>

+*

+* @note  This definition is different from yaw, pitch and roll used in aviation

+*        where the X axis is along the long side of the plane (tail to nose).

+*        Note: This sensor type exists for legacy reasons, please use getRotationMatrix()

+*        in conjunction with remapCoordinateSystem() and getOrientation() to compute

+*        these values instead.

+*        Important note: For historical reasons the roll angle is positive in the

+*        clockwise direction (mathematically speaking, it should be positive in

+*        the counter-clockwise direction).

+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.

+* @param[out] timestamp The timestamp for this sensor.

+* @return     Returns 1 if the data was updated or 0 if it was not updated.

+*/

+int inv_get_sensor_type_orientation(float *values, int8_t *accuracy,

+                                     inv_time_t * timestamp)

+{

+    *accuracy = (int8_t) hal_out.accuracy_quat;

+    *timestamp = hal_out.nav_timestamp;

+

+    google_orientation(values);

+

+    return hal_out.nine_axis_status;

+}

+

+int inv_get_sensor_type_orientation_6_axis(float *values, int8_t *accuracy,

+                                     inv_time_t * timestamp)

+{

+    long accel[3];

+    inv_get_accel_set(accel, accuracy, timestamp);

+

+    hal_out.gam_timestamp = hal_out.nav_timestamp;

+    *timestamp = hal_out.gam_timestamp;

+

+    google_orientation_6_axis(values);

+

+    return hal_out.accel_status;

+}

+

+int inv_get_sensor_type_orientation_geomagnetic(float *values, int8_t *accuracy,

+                                     inv_time_t * timestamp)

+{

+    long compass[3], quat_geomagnetic[4];

+    inv_get_compass_set(compass, accuracy, timestamp);

+    inv_get_geomagnetic_quaternion(quat_geomagnetic, timestamp);

+

+    google_orientation_geomagnetic(values);

+    return hal_out.accel_status & hal_out.compass_status;

+}

+/** Main callback to generate HAL outputs. Typically not called by library users.

+* @param[in] sensor_cal Input variable to take sensor data whenever there is new

+* sensor data.

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

+*/

+inv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal)

+{

+    int use_sensor = 0;

+    long sr = 1000;

+    long compass[3];

+    int8_t accuracy;

+    int i;

+    (void) sensor_cal;

+

+    inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_quat,

+                           &hal_out.nav_timestamp);

+    hal_out.gyro_status = sensor_cal->gyro.status;

+    hal_out.accel_status = sensor_cal->accel.status;

+    hal_out.compass_status = sensor_cal->compass.status;

+    hal_out.quat_status = sensor_cal->quat.status;

+

+    // Find the highest sample rate and tie generating 9-axis to that one.

+    if (sensor_cal->gyro.status & INV_SENSOR_ON) {

+        sr = sensor_cal->gyro.sample_rate_ms;

+        use_sensor = 0;

+    }

+    if ((sensor_cal->accel.status & INV_SENSOR_ON) && (sr > sensor_cal->accel.sample_rate_ms)) {

+        sr = sensor_cal->accel.sample_rate_ms;

+        use_sensor = 1;

+    }

+    if ((sensor_cal->compass.status & INV_SENSOR_ON) && (sr > sensor_cal->compass.sample_rate_ms)) {

+        sr = sensor_cal->compass.sample_rate_ms;

+        use_sensor = 2;

+    }

+    if ((sensor_cal->quat.status & INV_SENSOR_ON) && (sr > sensor_cal->quat.sample_rate_ms)) {

+        sr = sensor_cal->quat.sample_rate_ms;

+        use_sensor = 3;

+    }

+

+    // Only output 9-axis if all 9 sensors are on.

+    if (sensor_cal->quat.status & INV_SENSOR_ON) {

+        // If quaternion sensor is on, gyros are not required as quaternion already has that part

+        if ((sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) {

+            use_sensor = -1;

+        }

+    } else {

+        if ((sensor_cal->gyro.status & sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) {

+            use_sensor = -1;

+        }

+    }

+

+    switch (use_sensor) {

+    case 0:

+        hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0;

+        hal_out.nav_timestamp = sensor_cal->gyro.timestamp;

+        break;

+    case 1:

+        hal_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0;

+        hal_out.nav_timestamp = sensor_cal->accel.timestamp;

+        break;

+    case 2:

+        hal_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0;

+        hal_out.nav_timestamp = sensor_cal->compass.timestamp;

+        break;

+    case 3:

+        hal_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0;

+        hal_out.nav_timestamp = sensor_cal->quat.timestamp;

+        break;

+    default:

+        hal_out.nine_axis_status = 0; // Don't output quaternion related info

+        break;

+    }

+

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

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

+    #define COMPASS_CONVERSION 1.52587890625e-005f

+

+    inv_get_compass_set(compass, &accuracy, &(hal_out.mag_timestamp) );

+    hal_out.accuracy_mag = (int)accuracy;

+

+#ifndef CALIB_COMPASS_NOISE_REDUCTION

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

+        hal_out.compass_float[i] = (float)compass[i] * COMPASS_CONVERSION;

+    }

+#else

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

+        if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_CONTIGUOUS)) ==

+                                                             INV_NEW_DATA)  {

+            // set the state variables to match output with input

+            inv_calc_state_to_match_output(&hal_out.lp_filter[i],

+                                           (float)compass[i]);

+        }

+        if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_RAW_DATA)) ==

+                                          (INV_NEW_DATA | INV_RAW_DATA)) {

+            hal_out.compass_float[i] =

+                inv_biquad_filter_process(&hal_out.lp_filter[i],

+                                          (float)compass[i]) *

+                                          COMPASS_CONVERSION;

+        } else if ((sensor_cal->compass.status & INV_NEW_DATA) == INV_NEW_DATA) {

+            hal_out.compass_float[i] = (float)compass[i] * COMPASS_CONVERSION;

+        }

+    }

+#endif // CALIB_COMPASS_NOISE_REDUCTION

+    return INV_SUCCESS;

+}

+

+/** Turns off generation of HAL outputs.

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

+ */

+inv_error_t inv_stop_hal_outputs(void)

+{

+    inv_error_t result;

+    result = inv_unregister_data_cb(inv_generate_hal_outputs);

+    return result;

+}

+

+/** Turns on generation of HAL outputs. This should be called after inv_stop_hal_outputs()

+* to turn generation of HAL outputs back on. It is automatically called by inv_enable_hal_outputs().

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

+*/

+inv_error_t inv_start_hal_outputs(void)

+{

+    inv_error_t result;

+    result =

+        inv_register_data_cb(inv_generate_hal_outputs,

+                             INV_PRIORITY_HAL_OUTPUTS,

+                             INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW | INV_QUAT_NEW);

+    return result;

+}

+

+/* file name: lowPassFilterCoeff_1_6.c */

+float compass_low_pass_filter_coeff[5] =

+{+2.000000000000f, +1.000000000000f, -1.279632424998f, +0.477592250073f, +0.049489956269f};

+

+/** Initializes hal outputs class. This is called automatically by the

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

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

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

+*/

+inv_error_t inv_init_hal_outputs(void)

+{

+    int i;

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

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

+        inv_init_biquad_filter(&hal_out.lp_filter[i], compass_low_pass_filter_coeff);

+    }

+

+    return INV_SUCCESS;

+}

+

+/** Turns on creation and storage of HAL type results.

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

+*/

+inv_error_t inv_enable_hal_outputs(void)

+{

+    inv_error_t result;

+

+        // don't need to check the result for inv_init_hal_outputs

+        // since it's always INV_SUCCESS

+        inv_init_hal_outputs();

+

+    result = inv_register_mpl_start_notification(inv_start_hal_outputs);

+    return result;

+}

+

+/** Turns off creation and storage of HAL type results.

+*/

+inv_error_t inv_disable_hal_outputs(void)

+{

+    inv_error_t result;

+

+    inv_stop_hal_outputs(); // Ignore error if we have already stopped this

+    result = inv_unregister_mpl_start_notification(inv_start_hal_outputs);

+    return result;

+}

+

+/**

+ * @}

+ */

+ 

+ 

+ 

diff --git a/65xx/libsensors_iio/software/core/mllite/hal_outputs.h b/65xx/libsensors_iio/software/core/mllite/hal_outputs.h
new file mode 100755
index 0000000..2875071
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/hal_outputs.h
@@ -0,0 +1,56 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#include "mltypes.h"
+
+#ifndef INV_HAL_OUTPUTS_H__
+#define INV_HAL_OUTPUTS_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    int inv_get_sensor_type_orientation(float *values, int8_t *accuracy,
+                                         inv_time_t * timestamp);
+    int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy,
+                                           inv_time_t * timestamp);
+    int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,
+                                       inv_time_t * timestamp);
+    int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy,
+                                       inv_time_t * timestamp);
+    int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy,
+                                            inv_time_t * timestamp);
+	int inv_get_sensor_type_magnetic_field_raw(float *values, int8_t *accuracy,
+                                            inv_time_t * timestamp);
+    int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy,
+            inv_time_t * timestamp);
+
+    int inv_get_sensor_type_linear_acceleration(float *values,
+            int8_t *accuracy,
+            inv_time_t * timestamp);
+    int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,
+                                     inv_time_t * timestamp);
+
+    int inv_get_sensor_type_orientation_6_axis(float *values, int8_t *accuracy,
+                                         inv_time_t * timestamp);
+    int inv_get_sensor_type_orientation_geomagnetic(float *values, int8_t *accuracy,
+                                         inv_time_t * timestamp);
+    int inv_get_sensor_type_rotation_vector_6_axis(float *values, int8_t *accuracy,
+                                         inv_time_t * timestamp);
+    int inv_get_sensor_type_geomagnetic_rotation_vector(float *values, int8_t *accuracy,
+                                         inv_time_t * timestamp);
+
+    inv_error_t inv_enable_hal_outputs(void);
+    inv_error_t inv_disable_hal_outputs(void);
+    inv_error_t inv_init_hal_outputs(void);
+    inv_error_t inv_start_hal_outputs(void);
+    inv_error_t inv_stop_hal_outputs(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif                          // INV_HAL_OUTPUTS_H__
diff --git a/65xx/libsensors_iio/software/core/mllite/invensense.h b/65xx/libsensors_iio/software/core/mllite/invensense.h
new file mode 100755
index 0000000..fb8e12b
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c b/65xx/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c
new file mode 100755
index 0000000..649b917
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h b/65xx/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h
new file mode 100755
index 0000000..45a35f9
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c b/65xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
new file mode 100755
index 0000000..ffa35a9
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
@@ -0,0 +1,283 @@
+/*
+ $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 2634
+
+static const unsigned char dmpMemory[DMP_CODE_SIZE] = {
+    /* bank # 0 */
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x20, 0x00, 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, 
+    0xff, 0xff, 0xff, 0xff, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, 
+    0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 
+    0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x07, 
+    0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00, 
+    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    /* bank # 1 */
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x05, 0xdc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x58, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc8, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x03, 0x00, 0x02, 0x00, 0x0f, 
+    0x08, 0x00, 0x02, 0x00, 0x04, 0x00, 0x00, 0x00, 0x80, 0x00, 0x10, 0x00, 0x20, 0x00, 0x40, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 
+    0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00, 
+    /* bank # 2 */
+    0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2, 
+    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, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x05, 0x00, 0x05, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x05, 0x00, 0x05, 
+    0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    /* bank # 3 */
+    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 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, 0xb0, 0xb5, 0xb9, 0xf3, 0xa6, 0xf8, 0xf9, 0xd1, 0xd9, 0x81, 0x96, 0xf8, 0xf7, 0x3e, 0xd8, 
+    0xf3, 0xb1, 0x86, 0x96, 0xa3, 0x31, 0xd1, 0xda, 0xf1, 0xff, 0xd8, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e, 
+    0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, 
+    0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0xf1, 0xa9, 
+    0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0x8a, 0xa8, 0x96, 0x36, 
+    0x56, 0x76, 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, 0xf1, 0xb9, 0xa3, 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda, 0xde, 
+    0xdf, 0xdb, 0x81, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf, 0xd9, 
+    0xba, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 0xa2, 0xd9, 0xba, 0xa2, 0xf8, 0xdf, 
+    0xb9, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf, 0xba, 0xa0, 0xdf, 0xdf, 0xdf, 
+    0xd8, 0xf1, 0xb9, 0xa1, 0xb2, 0x80, 0xc0, 0xb0, 0x89, 0xa0, 0xc3, 0xc5, 0xc7, 0xb1, 0x81, 0xb4, 
+    0x97, 0xa0, 0x11, 0xb5, 0x90, 0xa1, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xf1, 0x90, 0x81, 
+    0xa0, 0x2d, 0x55, 0x3d, 0xf2, 0xa5, 0xd0, 0xf8, 0xf9, 0xd1, 0xd9, 0xf8, 0xf1, 0x80, 0xa2, 0xc3, 
+    0xc5, 0xc7, 0xd8, 0xf3, 0xa2, 0xde, 0xf1, 0x82, 0x90, 0xa4, 0x2d, 0x55, 0x7d, 0x84, 0x94, 0xf5, 
+    /* bank # 5 */
+    0xa1, 0x20, 0xd9, 0xf3, 0xa2, 0xf8, 0xd8, 0xf5, 0xa1, 0x40, 0xd9, 0xf3, 0xa2, 0xf8, 0xd8, 0xf5, 
+    0xa1, 0x60, 0xd9, 0xf3, 0xa2, 0xf8, 0xd8, 0xf2, 0xb9, 0xa2, 0xf8, 0xf9, 0xd1, 0xd9, 0xa6, 0xde, 
+    0xa5, 0xd0, 0xde, 0xf4, 0x1a, 0xd8, 0xf2, 0x86, 0x96, 0xa1, 0x09, 0xd1, 0xdb, 0xf4, 0x11, 0xd8, 
+    0xf2, 0x11, 0xd1, 0xdb, 0xf4, 0x1c, 0xd8, 0xf2, 0x19, 0xd1, 0xdb, 0xf4, 0x14, 0xd8, 0xf4, 0x10, 
+    0xd8, 0xf3, 0xa2, 0xf8, 0xf9, 0xd1, 0xda, 0xf2, 0xa6, 0xf8, 0xf1, 0xa5, 0xde, 0xd8, 0xf4, 0xa1, 
+    0x16, 0x16, 0xd8, 0xf1, 0xa5, 0xf8, 0xa1, 0x85, 0x95, 0x09, 0xd9, 0xf1, 0xa5, 0xde, 0xf2, 0xa5, 
+    0xd0, 0xde, 0xa6, 0xf8, 0xd8, 0xf4, 0xa1, 0x09, 0xd8, 0xf3, 0xa2, 0xf8, 0xf9, 0xd1, 0xf4, 0xd9, 
+    0x08, 0x19, 0xda, 0x42, 0xf2, 0xa2, 0xde, 0xf4, 0x0b, 0xd8, 0xf1, 0xa5, 0xf8, 0xa1, 0x85, 0x95, 
+    0x19, 0xda, 0xf4, 0x07, 0xd8, 0xf2, 0xa6, 0xde, 0xa5, 0xd0, 0xde, 0xd8, 0xf1, 0xb8, 0xaa, 0xb3, 
+    0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xa7, 
+    0x0e, 0x16, 0x1e, 0xb4, 0x9a, 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xba, 0xa4, 0xb0, 0x8a, 0xb6, 
+    0x91, 0x32, 0x56, 0x76, 0xb2, 0x84, 0x94, 0xa4, 0xd0, 0xcd, 0xa4, 0xd0, 0xc8, 0x30, 0xd8, 0xb8, 
+    0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55, 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 
+    0x54, 0x7c, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88, 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x0c, 0xd8, 0xf1, 
+    0xa4, 0xd0, 0xb2, 0x86, 0x86, 0xb0, 0x88, 0xa8, 0xf4, 0x01, 0x17, 0xf1, 0xa2, 0xd0, 0xf8, 0xf9, 
+    0xa8, 0x84, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 0xdf, 0x83, 0xc1, 0xd8, 
+    /* bank # 6 */
+    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, 0x0b, 0xf1, 0xa4, 0xd0, 0xb2, 0x86, 0x86, 0xb0, 0x88, 0xa8, 0xf4, 0x01, 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, 0x8d, 0x9d, 
+    0xad, 0xf1, 0xc8, 0xf3, 0xad, 0xf8, 0xd1, 0xd9, 0xf9, 0xf4, 0x11, 0xd8, 0xf3, 0xf9, 0x84, 0xa8, 
+    0x01, 0xd1, 0xd9, 0xf4, 0x07, 0xd8, 0xf1, 0xa4, 0x8d, 0xc0, 0xf4, 0x41, 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, 
+    /* bank # 7 */
+    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, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9, 0x88, 
+    0x09, 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c, 0xa8, 
+    0x3c, 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e, 0xa9, 
+    0x99, 0x88, 0x2d, 0x55, 0x7d, 0xd8, 0xf1, 0xb3, 0x8b, 0xb4, 0x97, 0xbb, 0xab, 0xf8, 0xf9, 0xb9, 
+    0xa3, 0xda, 0xc3, 0xc5, 0xc7, 0xd9, 0x2d, 0x4d, 0x6d, 0xd8, 0xb0, 0x8c, 0xb8, 0xac, 0xf8, 0xf9, 
+    0xb9, 0xa9, 0xda, 0xc3, 0xc5, 0xc7, 0xd9, 0x2d, 0x4d, 0x6d, 0xd8, 0xf3, 0xb9, 0xac, 0xde, 0xd8, 
+    0xb1, 0xb6, 0xb9, 0xf1, 0xa8, 0xf8, 0xf3, 0xb9, 0xaa, 0xfa, 0xf9, 0xd1, 0xda, 0xf2, 0x8a, 0xca, 
+    0xf4, 0x0e, 0xf3, 0xb1, 0x88, 0xdd, 0xc2, 0xc2, 0xf1, 0x88, 0xc0, 0xdc, 0xf3, 0xb9, 0xac, 0xf8, 
+    0xd8, 0xf2, 0xb2, 0xb6, 0xba, 0xad, 0xfa, 0x8d, 0x9d, 0xab, 0x39, 0xd9, 0xad, 0xdf, 0xf4, 0x13, 
+    /* bank # 8 */
+    0xdd, 0xf2, 0xb1, 0x87, 0xb5, 0x9a, 0x08, 0x08, 0xf1, 0xb1, 0x83, 0xc2, 0xc4, 0xc6, 0xdc, 0xf3, 
+    0xb9, 0xac, 0xf8, 0xd8, 0xf3, 0xb2, 0xb6, 0xba, 0xad, 0xf8, 0x8d, 0x9d, 0xab, 0x11, 0xd9, 0xad, 
+    0xde, 0xf4, 0x13, 0xdd, 0xf2, 0xb1, 0x87, 0xb5, 0x9a, 0x28, 0x28, 0xf1, 0xb1, 0x89, 0xc2, 0xc4, 
+    0xc6, 0xdc, 0xf3, 0xb9, 0xac, 0xf8, 0xd8, 0xf3, 0xb2, 0xb6, 0xba, 0xad, 0xfa, 0x8d, 0x9d, 0xab, 
+    0x39, 0xd9, 0xad, 0xdf, 0xf4, 0x12, 0xdd, 0xf3, 0xb1, 0x87, 0xb5, 0x9a, 0x08, 0xf2, 0xf2, 0x88, 
+    0xc2, 0xc4, 0xc6, 0xdc, 0xf3, 0xb9, 0xac, 0xf8, 0xd8, 0xf2, 0xb2, 0xb6, 0xba, 0xae, 0xf8, 0x8e, 
+    0x9e, 0xab, 0x11, 0xd9, 0xae, 0xde, 0xf4, 0x12, 0xdd, 0xf3, 0xb1, 0x87, 0xb5, 0x9a, 0x68, 0xf2, 
+    0xb0, 0x80, 0xc0, 0xc8, 0xc2, 0xdc, 0xf3, 0xb9, 0xac, 0xf8, 0xd8, 0xf2, 0xb2, 0xb6, 0xba, 0xae, 
+    0xfa, 0x8e, 0x9e, 0xab, 0x39, 0xd9, 0xae, 0xdf, 0xf4, 0x12, 0xdd, 0xf2, 0xb1, 0x87, 0xb5, 0x9a, 
+    0x68, 0xf2, 0xb0, 0x80, 0xc4, 0xcc, 0xc6, 0xdc, 0xf3, 0xb9, 0xac, 0xf8, 0xd8, 0xf3, 0xb2, 0xb6, 
+    0xba, 0xae, 0xf8, 0x8e, 0x9e, 0xab, 0x11, 0xd9, 0xae, 0xde, 0xf4, 0x12, 0xdd, 0xf3, 0xb1, 0x87, 
+    0xb5, 0x9a, 0x48, 0xf2, 0xb0, 0x81, 0xc0, 0xc8, 0xc2, 0xdc, 0xf3, 0xb9, 0xac, 0xf8, 0xd8, 0xf3, 
+    0xb2, 0xb6, 0xba, 0xae, 0xfa, 0x8e, 0x9e, 0xab, 0x39, 0xd9, 0xae, 0xdf, 0xf4, 0x12, 0xdd, 0xf2, 
+    0xb1, 0x87, 0xb5, 0x9a, 0x48, 0xf2, 0xb0, 0x81, 0xc4, 0xcc, 0xc6, 0xdc, 0xf3, 0xb9, 0xac, 0xf8, 
+    0xd8, 0xf2, 0xb1, 0x89, 0xb9, 0xa8, 0xc3, 0xc5, 0xc7, 0xd8, 0xf3, 0xb9, 0xac, 0xf8, 0xf9, 0xd1, 
+    0xd9, 0xf4, 0x1f, 0xd8, 0xf1, 0xb9, 0xaa, 0xdf, 0xf3, 0xac, 0xfa, 0xf9, 0xd1, 0xd9, 0xf4, 0x1a, 
+    /* bank # 9 */
+    0xd8, 0xf2, 0xb1, 0xb5, 0x8c, 0xac, 0xf8, 0xf9, 0xd1, 0xd9, 0xc2, 0xd8, 0xf2, 0xf9, 0xd9, 0xde, 
+    0xf4, 0x09, 0x13, 0xda, 0xf2, 0xdd, 0xc6, 0xdc, 0xf4, 0x31, 0x11, 0xd8, 0xf2, 0xab, 0xfa, 0x8b, 
+    0x9b, 0xa3, 0x49, 0xd9, 0xf4, 0x07, 0x06, 0xda, 0xf2, 0xb9, 0xab, 0xdf, 0xfe, 0xd8, 0xf1, 0xbb, 
+    0xb3, 0xb7, 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, 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, 
+    0x85, 0xf5, 0x75, 0xda, 0xff, 0xd8, 0x71, 0x80, 0xa9, 0xda, 0xf1, 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, 
+    /* bank # 10 */
+    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, 0xf1, 0xf1, 0xf1, 0xac, 
+    0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xf3, 0xf3, 0xb9, 0xaa, 0xfa, 0xf1, 0xff, 0xd8, 
+    0x8c, 0x9c, 0xac, 0xd0, 0x10, 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 
+    0xf2, 0x35, 0xf1, 0x96, 0x88, 0xa6, 0xd9, 0x00, 0xd8, 0xff
+};
+
+#define DMP_VERSION (dmpMemory)
+
+inv_error_t inv_write_dmp_data(FILE *fd, const unsigned char *dmp, size_t len)
+{
+    inv_error_t result = INV_SUCCESS;
+    int bytesWritten = 0;
+   
+    if (len <= 0) {
+        MPL_LOGE("Nothing to write");
+        return INV_ERROR_FILE_WRITE;
+    }
+    else {
+        MPL_LOGI("dmp firmware size to write = %d", len);
+    }
+    if ( fd == NULL ) {
+        return INV_ERROR_FILE_OPEN;
+    }
+    bytesWritten = fwrite(dmp, 1, len, fd);
+    if (bytesWritten != len) {
+        MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
+                 bytesWritten, len);
+        result = INV_ERROR_FILE_WRITE;
+    }
+    else {
+        MPL_LOGI("Bytes written = %d", bytesWritten);
+    }
+    return result;
+}
+
+inv_error_t inv_load_dmp(FILE *fd)
+{
+    inv_error_t result = INV_SUCCESS;
+    result = inv_write_dmp_data(fd, DMP_VERSION, DMP_CODE_SIZE);
+    return result;
+}
+
+void read_dmp_img(char *dmp_path, char* out_file)
+{
+    MPL_LOGI("read_dmp_img"); 
+    FILE *fp;
+    int i;
+    int dmpSize = DMP_CODE_SIZE;
+    char dmp_img[dmpSize];
+
+    if ((fp = fopen(dmp_path, "rb")) < 0) {
+        perror("dmp fail");
+    }
+    i = fread(dmp_img, 1, dmpSize, fp);
+    MPL_LOGI("Result=%d", i);
+    fclose(fp);
+    fp = fopen(out_file, "wt");
+    if(fp == NULL) {
+    MPL_LOGE("error open out file:%s", out_file);
+        return;
+    }
+    fprintf(fp, "char rec[]={\n");
+    for(i = 0; i < dmpSize; i++) {
+        fprintf(fp, "0x%02x, ", dmp_img[i]);
+        if(((i + 1) % 16) == 0) {
+            fprintf(fp, "\n");
+        }
+    }
+    fprintf(fp, "};\n ");
+    fclose(fp);
+}
+
+/**
+ *  @}
+ */
diff --git a/65xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h b/65xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h
new file mode 100755
index 0000000..3369f37
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h
@@ -0,0 +1,34 @@
+/*
+ $License:
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+#ifndef INV_LOAD_DMP_H
+#define INV_LOAD_DMP_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+    Includes.
+*/
+#include "mltypes.h"
+
+/*
+    APIs
+*/
+inv_error_t inv_load_dmp(FILE  *fd);
+void read_dmp_img(char *dmp_path, char *out_file);
+
+#ifdef __cplusplus
+}
+#endif
+#endif  /* INV_LOAD_DMP_H */
diff --git a/65xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c b/65xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
new file mode 100755
index 0000000..8c053b5
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
@@ -0,0 +1,357 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id: ml_stored_data.c 6132 2011-10-01 03:17:27Z mcaramello $
+ *
+ *****************************************************************************/
+
+/**
+ * @defgroup ML_STORED_DATA
+ *
+ * @{
+ *      @file     ml_stored_data.c
+ *      @brief    functions for reading and writing stored data sets.
+ *                Typically, these functions process stored calibration data.
+ */
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */
+#undef MPL_LOG_TAG
+
+#include <stdio.h>
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-storeload"
+
+#include "ml_stored_data.h"
+#include "storage_manager.h"
+#include "mlos.h"
+
+#define LOADCAL_DEBUG    0
+#define STORECAL_DEBUG   0
+
+#define DEFAULT_KEY 29681
+
+#define STORECAL_LOG MPL_LOGI
+#define LOADCAL_LOG  MPL_LOGI
+
+inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead)
+{
+    FILE *fp;
+    inv_error_t result = INV_SUCCESS;
+    size_t fsize;
+
+    fp = fopen(MLCAL_FILE,"rb");
+    if (fp == NULL) {
+        MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
+        return INV_ERROR_FILE_OPEN;
+    }
+
+    // obtain file size
+    fseek (fp, 0 , SEEK_END);
+    fsize = ftell (fp);
+    rewind (fp);
+  
+    *calData = (unsigned char *)inv_malloc(fsize);
+    if (*calData==NULL) {
+        MPL_LOGE("Could not allocate buffer of %d bytes - "
+                 "aborting\n", fsize);
+        fclose(fp);
+        return INV_ERROR_MEMORY_EXAUSTED;
+    }
+
+    *bytesRead = fread(*calData, 1, fsize, fp);
+    if (*bytesRead != fsize) {
+        MPL_LOGE("bytes read (%d) don't match file size (%d)\n",
+                 *bytesRead, fsize);
+        result = INV_ERROR_FILE_READ;
+        goto read_cal_end;
+    }
+    else {
+        MPL_LOGI("Bytes read = %d", *bytesRead);
+    }
+
+read_cal_end:
+    fclose(fp);
+    return result;
+}
+
+inv_error_t inv_write_cal(unsigned char *cal, size_t len)
+{
+    FILE *fp;
+    int bytesWritten;
+    inv_error_t result = INV_SUCCESS;
+   
+    if (len <= 0) {
+        MPL_LOGE("Nothing to write");
+        return INV_ERROR_FILE_WRITE;
+    }
+    else {
+        MPL_LOGI("cal data size to write = %d", len);
+    }
+    fp = fopen(MLCAL_FILE,"wb");
+    if (fp == NULL) {
+        MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE);
+        return INV_ERROR_FILE_OPEN;
+    }
+    bytesWritten = fwrite(cal, 1, len, fp);
+    if (bytesWritten != len) {
+        MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
+                 bytesWritten, len);
+        result = INV_ERROR_FILE_WRITE;
+    }
+    else {
+        MPL_LOGI("Bytes written = %d", bytesWritten);
+    }
+    fclose(fp);
+    return result;
+}
+
+/**
+ *  @brief  Loads a type 0 set of calibration data.
+ *          It parses a binary data set containing calibration data.
+ *          The binary data set is intended to be loaded from a file.
+ *          This calibrations data format stores values for (in order of
+ *          appearance) :
+ *          - temperature compensation : temperature data points,
+ *          - temperature compensation : gyro biases data points for X, Y,
+ *              and Z axes.
+ *          - accel biases for X, Y, Z axes.
+ *          This calibration data is produced internally by the MPL and its
+ *          size is 2777 bytes (header and checksum included).
+ *          Calibration format type 1 is currently used for ITG3500
+ *
+ *  @pre    inv_init_storage_manager()
+ *          must have been called.
+ *
+ *  @param  calData
+ *              A pointer to an array of bytes to be parsed.
+ *  @param  len
+ *              the length of the calibration
+ *
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len)
+{
+    inv_error_t result;
+
+    LOADCAL_LOG("Entering inv_load_cal_V0\n");
+
+    /*if (len != expLen) {
+        MPL_LOGE("Calibration data type 0 must be %d bytes long (got %d)\n",
+                 expLen, len);
+        return INV_ERROR_FILE_READ;
+    }*/
+
+    result = inv_load_mpl_states(calData, len);
+    return result;
+}
+
+/**
+ *  @brief  Loads a type 1 set of calibration data.
+ *          It parses a binary data set containing calibration data.
+ *          The binary data set is intended to be loaded from a file.
+ *          This calibrations data format stores values for (in order of
+ *          appearance) :
+ *          - temperature,
+ *          - gyro biases for X, Y, Z axes,
+ *          - accel biases for X, Y, Z axes.
+ *          This calibration data would normally be produced by the MPU Self
+ *          Test and its size is 36 bytes (header and checksum included).
+ *          Calibration format type 1 is produced by the MPU Self Test and
+ *          substitutes the type 0 : inv_load_cal_V0().
+ *
+ *  @pre    
+ *
+ *  @param  calData
+ *              A pointer to an array of bytes to be parsed.
+ *  @param  len
+ *              the length of the calibration
+ *
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len)
+{
+    return INV_SUCCESS;
+}
+
+/**
+ * @brief   Loads a set of calibration data.
+ *          It parses a binary data set containing calibration data.
+ *          The binary data set is intended to be loaded from a file.
+ *
+ * @pre     
+ *          
+ *
+ * @param   calData
+ *              A pointer to an array of bytes to be parsed.
+ *
+ * @return  INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_load_cal(unsigned char *calData)
+{
+    int calType = 0;
+    int len = 0;
+    //int ptr;
+    //uint32_t chk = 0;
+    //uint32_t cmp_chk = 0;
+
+    /*load_func_t loaders[] = {
+        inv_load_cal_V0,
+        inv_load_cal_V1,
+    };
+    */
+
+    inv_load_cal_V0(calData, len);
+
+    /* read the header (type and len)
+       len is the total record length including header and checksum */
+    len = 0;
+    len += 16777216L * ((int)calData[0]);
+    len += 65536L * ((int)calData[1]);
+    len += 256 * ((int)calData[2]);
+    len += (int)calData[3];
+
+    calType = ((int)calData[4]) * 256 + ((int)calData[5]);
+    if (calType > 5) {
+        MPL_LOGE("Unsupported calibration file format %d. "
+                 "Valid types 0..5\n", calType);
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    /* call the proper method to read in the data */
+    //return loaders[calType] (calData, len);
+    return 0;
+}
+
+/**
+ *  @brief  Stores a set of calibration data.
+ *          It generates a binary data set containing calibration data.
+ *          The binary data set is intended to be stored into a file.
+ *
+ *  @pre    inv_dmp_open()
+ *
+ *  @param  calData
+ *              A pointer to an array of bytes to be stored.
+ *  @param  length
+ *              The amount of bytes available in the array.
+ *
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_store_cal(unsigned char *calData, size_t length)
+{
+    inv_error_t res = 0;
+    size_t size;
+
+    STORECAL_LOG("Entering inv_store_cal\n");
+
+    inv_get_mpl_state_size(&size);
+
+    MPL_LOGI("inv_get_mpl_state_size() : size=%d", size);
+
+    /* store data */
+    res = inv_save_mpl_states(calData, size);
+    if(res != 0)
+    {
+        MPL_LOGE("inv_save_mpl_states() failed");
+    }
+
+    STORECAL_LOG("Exiting inv_store_cal\n");
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  Load a calibration file.
+ *
+ *  @pre    Must be in INV_STATE_DMP_OPENED state.
+ *          inv_dmp_open() or inv_dmp_stop() must have been called.
+ *          inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
+ *          been called.
+ *
+ *  @return 0 or error code.
+ */
+inv_error_t inv_load_calibration(void)
+{
+    unsigned char *calData= NULL;
+    inv_error_t result = 0;
+    size_t bytesRead = 0;
+
+    result = inv_read_cal(&calData, &bytesRead);
+    if(result != INV_SUCCESS) {
+        MPL_LOGE("Could not load cal file - "
+                 "aborting\n");
+        goto free_mem_n_exit;
+    }
+
+    result = inv_load_mpl_states(calData, bytesRead);
+    if (result != INV_SUCCESS) {
+        MPL_LOGE("Could not load the calibration data - "
+                 "error %d - aborting\n", result);
+        goto free_mem_n_exit;
+    }
+
+free_mem_n_exit:
+    inv_free(calData);
+    return result;
+}
+
+/**
+ *  @brief  Store runtime calibration data to a file
+ *
+ *  @pre    Must be in INV_STATE_DMP_OPENED state.
+ *          inv_dmp_open() or inv_dmp_stop() must have been called.
+ *          inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
+ *          been called.
+ *
+ *  @return 0 or error code.
+ */
+inv_error_t inv_store_calibration(void)
+{
+    unsigned char *calData;
+    inv_error_t result;
+    size_t length;
+
+    result = inv_get_mpl_state_size(&length);
+    calData = (unsigned char *)inv_malloc(length);
+    if (!calData) {
+        MPL_LOGE("Could not allocate buffer of %d bytes - "
+                 "aborting\n", length);
+        return INV_ERROR_MEMORY_EXAUSTED;
+    }
+    else {
+        MPL_LOGI("inv_get_mpl state size = %d", length);
+    }
+
+    result = inv_save_mpl_states(calData, length);
+    if (result != INV_SUCCESS) {
+        MPL_LOGE("Could not save mpl states - "
+                 "error %d - aborting\n", result);
+        goto free_mem_n_exit;
+    }
+    else {
+        MPL_LOGE("calData from inv_save_mpl_states, size=%d", 
+                 strlen((char *)calData));
+    }
+
+    result = inv_write_cal(calData, length);
+    if (result != INV_SUCCESS) {
+        MPL_LOGE("Could not store calibrated data on file - "
+                 "error %d - aborting\n", result);
+        goto free_mem_n_exit;
+
+    }
+
+free_mem_n_exit:
+    inv_free(calData);
+    return result;
+}
+
+/**
+ *  @}
+ */
diff --git a/65xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h b/65xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
new file mode 100755
index 0000000..115b34c
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
@@ -0,0 +1,53 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id: ml_stored_data.h 5873 2011-08-11 03:13:48Z mcaramello $
+ *
+ ******************************************************************************/
+
+#ifndef INV_MPL_STORED_DATA_H
+#define INV_MPL_STORED_DATA_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+    Includes.
+*/
+#include "mltypes.h"
+
+/*
+    Defines
+*/
+#define MLCAL_FILE "/data/inv_cal_data.bin"
+
+/*
+    APIs
+*/
+inv_error_t inv_load_calibration(void);
+inv_error_t inv_store_calibration(void);
+
+/*
+    Internal APIs
+*/
+inv_error_t inv_read_cal(unsigned char **, size_t *);
+inv_error_t inv_write_cal(unsigned char *cal, size_t len);
+inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len);
+inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len);
+
+/*
+    Other prototypes
+*/
+inv_error_t inv_load_cal(unsigned char *calData);
+inv_error_t inv_store_cal(unsigned char *calData, size_t length);
+
+#ifdef __cplusplus
+}
+#endif
+#endif  /* INV_MPL_STORED_DATA_H */
diff --git a/65xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c b/65xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
new file mode 100755
index 0000000..cbf69f8
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
@@ -0,0 +1,526 @@
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MLLITE"
+
+#include <string.h>
+#include <stdio.h>
+#include "ml_sysfs_helper.h"
+#include <dirent.h>
+#include <ctype.h>
+#include "log.h"
+
+#define MPU_SYSFS_ABS_PATH "/sys/class/invensense/mpu"
+
+enum PROC_SYSFS_CMD {
+	CMD_GET_SYSFS_PATH,
+	CMD_GET_DMP_PATH,
+	CMD_GET_CHIP_NAME,
+	CMD_GET_SYSFS_KEY,
+	CMD_GET_TRIGGER_PATH,
+	CMD_GET_DEVICE_NODE
+};
+static char sysfs_path[100];
+static char *chip_name[] = {
+    "ITG3500", 
+    "MPU6050", 
+    "MPU9150", 
+    "MPU3050", 
+    "MPU6500",
+    "MPU9250",
+    "MPU6XXX",
+    "MPU9350",
+    "MPU6515",
+};
+static int chip_ind;
+static int initialized =0;
+static int status = 0;
+static int iio_initialized = 0;
+static int iio_dev_num = 0;
+
+#define IIO_MAX_NAME_LENGTH 30
+
+#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements"
+#define FORMAT_TYPE_FILE "%s_type"
+
+#define CHIP_NUM ARRAY_SIZE(chip_name)
+
+static const char *iio_dir = "/sys/bus/iio/devices/";
+
+/**
+ * find_type_by_name() - function to match top level types by name
+ * @name: top level type instance name
+ * @type: the type of top level instance being sort
+ *
+ * Typical types this is used for are device and trigger.
+ **/
+int find_type_by_name(const char *name, const char *type)
+{
+	const struct dirent *ent;
+	int number, numstrlen;
+
+	FILE *nameFile;
+	DIR *dp;
+	char thisname[IIO_MAX_NAME_LENGTH];
+	char *filename;
+
+	dp = opendir(iio_dir);
+	if (dp == NULL) {
+		MPL_LOGE("No industrialio devices available");
+		return -ENODEV;
+	}
+
+	while (ent = readdir(dp), ent != NULL) {
+		if (strcmp(ent->d_name, ".") != 0 &&
+			strcmp(ent->d_name, "..") != 0 &&
+			strlen(ent->d_name) > strlen(type) &&
+			strncmp(ent->d_name, type, strlen(type)) == 0) {
+			numstrlen = sscanf(ent->d_name + strlen(type),
+					   "%d",
+					   &number);
+			/* verify the next character is not a colon */
+			if (strncmp(ent->d_name + strlen(type) + numstrlen,
+					":",
+					1) != 0) {
+				filename = malloc(strlen(iio_dir)
+						+ strlen(type)
+						+ numstrlen
+						+ 6);
+				if (filename == NULL)
+					return -ENOMEM;
+				sprintf(filename, "%s%s%d/name",
+					iio_dir,
+					type,
+					number);
+				nameFile = fopen(filename, "r");
+				if (!nameFile)
+					continue;
+				free(filename);
+				fscanf(nameFile, "%s", thisname);
+				if (strcmp(name, thisname) == 0)
+					return number;
+				fclose(nameFile);
+			}
+		}
+	}
+	return -ENODEV;
+}
+
+/* mode 0: search for which chip in the system and fill sysfs path
+   mode 1: return event number
+ */
+static int parsing_proc_input(int mode, char *name){
+	const char input[] = "/proc/bus/input/devices";
+	char line[4096], d;
+	char tmp[100];
+	FILE *fp;
+	int i, j, result, find_flag;
+	int event_number = -1;
+	int input_number = -1;
+
+	if(NULL == (fp = fopen(input, "rt")) ){
+		return -1;
+	}
+	result = 1;
+	find_flag = 0;
+	while(result != 0 && find_flag < 2){
+		i = 0;
+		d = 0;
+		memset(line, 0, 100);
+		while(d != '\n'){		
+			result = fread(&d, 1, 1, fp);
+			if(result == 0){
+				line[0] = 0;
+				break;
+			}
+			sprintf(&line[i], "%c", d);		
+			i ++;
+		}
+		if(line[0] == 'N'){
+			i = 1;
+			while(line[i] != '"'){
+				i++;
+			}
+			i++;
+			j = 0;
+			find_flag = 0;
+			if (mode == 0){
+				while(j < CHIP_NUM){
+					if(!memcmp(&line[i], chip_name[j], strlen(chip_name[j]))){
+						find_flag = 1;
+						chip_ind = j;
+					}
+					j++;
+				}
+			} else if (mode  != 0){
+				if(!memcmp(&line[i], name, strlen(name))){
+					find_flag = 1;
+				}
+			}
+		}		
+		if(find_flag){
+			if(mode == 0){
+				if(line[0] == 'S'){
+					memset(tmp, 0, 100);
+					i =1;
+					while(line[i] != '=') i++;
+					i++;
+					j = 0;
+					while(line[i] != '\n'){
+						tmp[j] = line[i];
+						i ++; j++;
+					}	
+					sprintf(sysfs_path, "%s%s", "/sys", tmp);
+					find_flag++;
+				}
+			} else if(mode == 1){
+				if(line[0] == 'H') {
+					i = 2;
+					while(line[i] != '=') i++;
+					while(line[i] != 't') i++;	
+					i++;
+					event_number = 0;
+					while(line[i] != '\n'){
+						if(line[i] >= '0' && line[i] <= '9')
+							event_number = event_number*10 + line[i]-0x30;
+						i ++;
+					}
+					find_flag ++;
+				}
+			} else if (mode == 2) {
+				if(line[0] == 'S'){
+					memset(tmp, 0, 100);
+					i =1;
+					while(line[i] != '=') i++;
+					i++;
+					j = 0;
+					while(line[i] != '\n'){
+						tmp[j] = line[i];
+						i ++; j++;
+					}
+					input_number = 0;
+					if(tmp[j-2] >= '0' && tmp[j-2] <= '9') 
+						input_number += (tmp[j-2]-0x30)*10;
+					if(tmp[j-1] >= '0' && tmp[j-1] <= '9') 
+						input_number += (tmp[j-1]-0x30);
+					find_flag++;
+				}
+			}
+		}
+	}
+	fclose(fp);
+	if(find_flag == 0){
+		return -1;
+	}
+	if(0 == mode)
+		status = 1;
+	if (mode == 1)
+		return event_number;
+	if (mode == 2)
+		return input_number;
+	return 0;
+
+}
+static void init_iio() {
+	int i, j;
+	char iio_chip[10];
+	int dev_num;
+	for(j=0; j< CHIP_NUM; j++) {
+		for (i=0; i<strlen(chip_name[j]); i++) {
+			iio_chip[i] = tolower(chip_name[j][i]);
+		}
+		iio_chip[strlen(chip_name[j])] = '\0';
+		dev_num = find_type_by_name(iio_chip, "iio:device");
+		if(dev_num >= 0) {
+			iio_initialized = 1;
+			iio_dev_num = dev_num;
+			chip_ind = j;
+		}
+	}
+}
+
+static int process_sysfs_request(enum PROC_SYSFS_CMD cmd, char *data)
+{
+	char key_path[100];
+	FILE *fp;
+	int i, result;
+	if(initialized == 0){
+		parsing_proc_input(0, NULL);
+		initialized = 1;
+	}
+	if(initialized && status == 0) {
+		init_iio();
+		if (iio_initialized == 0)
+			return -1;
+	}
+
+	memset(key_path, 0, 100);
+	switch(cmd){
+	case CMD_GET_SYSFS_PATH:
+		if (iio_initialized == 1)
+			sprintf(data, "/sys/bus/iio/devices/iio:device%d", iio_dev_num);
+		else
+			sprintf(data, "%s%s", sysfs_path, "/device/invensense/mpu");
+		break;
+	case CMD_GET_DMP_PATH:
+		if (iio_initialized == 1)
+			sprintf(data, "/sys/bus/iio/devices/iio:device%d/dmp_firmware", iio_dev_num);
+		else
+			sprintf(data, "%s%s", sysfs_path, "/device/invensense/mpu/dmp_firmware");
+		break;
+	case CMD_GET_CHIP_NAME:
+		sprintf(data, "%s", chip_name[chip_ind]);
+		break;
+	case CMD_GET_TRIGGER_PATH:
+		sprintf(data, "/sys/bus/iio/devices/trigger%d", iio_dev_num);
+		break;
+	case CMD_GET_DEVICE_NODE:
+		sprintf(data, "/dev/iio:device%d", iio_dev_num);
+		break;
+	case CMD_GET_SYSFS_KEY:
+		memset(key_path, 0, 100);
+		if (iio_initialized == 1)
+			sprintf(key_path, "/sys/bus/iio/devices/iio:device%d/key", iio_dev_num);
+		else	
+			sprintf(key_path, "%s%s", sysfs_path, "/device/invensense/mpu/key");
+
+		if((fp = fopen(key_path, "rt")) == NULL)
+			return -1;
+		for(i=0;i<16;i++){
+			fscanf(fp, "%02x", &result);
+			data[i] = (char)result;
+		}
+		
+		fclose(fp);
+		break;
+	default:
+		break;
+	}
+	return 0;
+}
+
+int find_name_by_sensor_type(const char *sensor_type, const char *type, char *sensor_name)
+{
+    const struct dirent *ent;
+    int number, numstrlen;
+
+    FILE *nameFile;
+    DIR *dp;
+    char *filename;
+
+    dp = opendir(iio_dir);
+    if (dp == NULL) {
+        MPL_LOGE("No industrialio devices available");
+        return -ENODEV;
+    }
+
+    while (ent = readdir(dp), ent != NULL) {
+        if (strcmp(ent->d_name, ".") != 0 &&
+            strcmp(ent->d_name, "..") != 0 &&
+            strlen(ent->d_name) > strlen(type) &&
+            strncmp(ent->d_name, type, strlen(type)) == 0) {
+            numstrlen = sscanf(ent->d_name + strlen(type),
+                       "%d",
+                       &number);
+            /* verify the next character is not a colon */
+            if (strncmp(ent->d_name + strlen(type) + numstrlen,
+                    ":",
+                    1) != 0) {
+                filename = malloc(strlen(iio_dir)
+                        + strlen(type)
+                        + numstrlen
+                        + 6
+                        + strlen(sensor_type));
+                if (filename == NULL)
+                    return -ENOMEM;
+                sprintf(filename, "%s%s%d/%s",
+                    iio_dir,
+                    type,
+                    number,
+                    sensor_type);
+                nameFile = fopen(filename, "r");
+                MPL_LOGI("sensor type path: %s\n", filename);
+                free(filename);
+                //fscanf(nameFile, "%s", thisname);
+                //if (strcmp(name, thisname) == 0) {
+                if(nameFile == NULL) {
+                    MPL_LOGI("keeps searching");
+                    continue;
+                } else{
+                    MPL_LOGI("found directory");
+                }
+                filename = malloc(strlen(iio_dir)
+                        + strlen(type)
+                        + numstrlen
+                        + 6);
+                sprintf(filename, "%s%s%d/name",
+                    iio_dir,
+                    type,
+                    number);
+                    nameFile = fopen(filename, "r");
+                    MPL_LOGI("name path: %s\n", filename);
+                    free(filename);
+                    if (!nameFile)
+                        continue;
+                    fscanf(nameFile, "%s", sensor_name);
+                    MPL_LOGI("name found: %s now test for mpuxxxx", sensor_name);
+                    if( !strncmp("mpu",sensor_name, 3) ) {                      
+                        char secondaryFileName[200];                    
+                    sprintf(secondaryFileName, "%s%s%d/secondary_name",
+                        iio_dir,
+                        type,
+                        number);
+                        nameFile = fopen(secondaryFileName, "r");
+                        MPL_LOGI("name path: %s\n", secondaryFileName); 
+                        if(!nameFile)
+                            continue;
+                        fscanf(nameFile, "%s", sensor_name);
+                        MPL_LOGI("secondary name found: %s\n", sensor_name);
+                    }
+                    else {
+                        fscanf(nameFile, "%s", sensor_name);
+                        MPL_LOGI("name found: %s\n", sensor_name);
+                    }
+                    return 0;
+                //}
+                fclose(nameFile);
+            }
+        }
+    }
+    return -ENODEV;
+}
+
+/** 
+ *  @brief  return sysfs key. if the key is not available
+ *          return false. So the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the key
+ *           It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_sysfs_key(unsigned char *key)
+{
+	if (process_sysfs_request(CMD_GET_SYSFS_KEY, (char*)key) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+
+/** 
+ *  @brief  return the sysfs path. If the path is not 
+ *          found yet. return false. So the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the sysfs
+ *           path. It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_sysfs_path(char *name)
+{
+	if (process_sysfs_request(CMD_GET_SYSFS_PATH, name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+
+inv_error_t inv_get_sysfs_abs_path(char *name)
+{
+    strcpy(name, MPU_SYSFS_ABS_PATH);
+    return INV_SUCCESS;
+}
+
+/** 
+ *  @brief  return the dmp file path. If the path is not 
+ *          found yet. return false. So the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the dmp file
+ *           path. It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_dmpfile(char *name)
+{
+   	if (process_sysfs_request(CMD_GET_DMP_PATH, name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+/** 
+ *  @brief  return the chip name. If the chip is not 
+ *          found yet. return false. So the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the chip name
+ *           path(8 bytes). It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_chip_name(char *name)
+{
+   	if (process_sysfs_request(CMD_GET_CHIP_NAME, name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+/** 
+ *  @brief  return event handler number. If the handler number is not found
+ *          return false. the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the chip name
+ *           path(8 bytes). It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ *  @int *num: event number store
+ */
+inv_error_t  inv_get_handler_number(const char *name, int *num)
+{
+	initialized = 0;
+	if ((*num = parsing_proc_input(1, (char *)name)) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;	
+}
+
+/** 
+ *  @brief  return input number. If the handler number is not found
+ *          return false. the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the chip name
+ *           path(8 bytes). It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ *  @int *num: input number store
+ */
+inv_error_t  inv_get_input_number(const char *name, int *num)
+{
+	initialized = 0;
+	if ((*num = parsing_proc_input(2, (char *)name)) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else {
+		return INV_SUCCESS;
+	}	
+}
+
+/** 
+ *  @brief  return iio trigger name. If iio is not initialized, return false.
+ *          So the return must be checked to make sure the numeber is valid.
+ *  @unsigned char *name: This should be array big enough to hold the trigger
+ *           name. It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_iio_trigger_path(const char *name)
+{
+	if (process_sysfs_request(CMD_GET_TRIGGER_PATH, (char *)name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+
+/** 
+ *  @brief  return iio device node. If iio is not initialized, return false.
+ *          So the return must be checked to make sure the numeber is valid.
+ *  @unsigned char *name: This should be array big enough to hold the device
+ *           node. It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_iio_device_node(const char *name)
+{
+	if (process_sysfs_request(CMD_GET_DEVICE_NODE, (char *)name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
diff --git a/65xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h b/65xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h
new file mode 100755
index 0000000..184d3b2
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h
@@ -0,0 +1,37 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id$
+ *
+ ******************************************************************************/
+
+#ifndef MLDMP_SYSFS_HELPER_H__
+#define MLDMP_SYSFS_HELPER_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "invensense.h"
+
+int find_type_by_name(const char *name, const char *type);
+int find_name_by_sensor_type(const char *sensor_type, const char *type, char *sensor_name);
+inv_error_t inv_get_sysfs_path(char *name);
+inv_error_t inv_get_sysfs_abs_path(char *name);
+inv_error_t inv_get_dmpfile(char *name);
+inv_error_t inv_get_chip_name(char *name);
+inv_error_t inv_get_sysfs_key(unsigned char *key);
+inv_error_t inv_get_handler_number(const char *name, int *num);
+inv_error_t inv_get_input_number(const char *name, int *num);
+inv_error_t inv_get_iio_trigger_path(const char *name);
+inv_error_t inv_get_iio_device_node(const char *name);
+
+#ifdef __cplusplus
+}
+#endif
+#endif	/* MLDMP_SYSFS_HELPER_H__ */
diff --git a/65xx/libsensors_iio/software/core/mllite/linux/mlos.h b/65xx/libsensors_iio/software/core/mllite/linux/mlos.h
new file mode 100755
index 0000000..d4f8912
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/linux/mlos.h
@@ -0,0 +1,99 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+#ifndef _MLOS_H
+#define _MLOS_H
+
+#ifndef __KERNEL__
+#include <stdio.h>
+#endif
+#include <pthread.h>
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if defined(LINUX) || defined(__KERNEL__)
+typedef pthread_mutex_t* HANDLE;
+#endif
+
+	/* ------------ */
+	/* - Defines. - */
+	/* ------------ */
+
+	/* - MLOSCreateFile defines. - */
+
+#define MLOS_GENERIC_READ         ((unsigned int)0x80000000)
+#define MLOS_GENERIC_WRITE        ((unsigned int)0x40000000)
+#define MLOS_FILE_SHARE_READ      ((unsigned int)0x00000001)
+#define MLOS_FILE_SHARE_WRITE     ((unsigned int)0x00000002)
+#define MLOS_OPEN_EXISTING        ((unsigned int)0x00000003)
+
+	/* ---------- */
+	/* - Enums. - */
+	/* ---------- */
+
+	/* --------------- */
+	/* - Structures. - */
+	/* --------------- */
+
+	/* --------------------- */
+	/* - Function p-types. - */
+	/* --------------------- */
+
+#ifndef __KERNEL__
+#include <string.h>
+	void *inv_malloc(unsigned int numBytes);
+	inv_error_t inv_free(void *ptr);
+	inv_error_t inv_create_mutex(HANDLE *mutex);
+	inv_error_t inv_lock_mutex(HANDLE mutex);
+	inv_error_t inv_unlock_mutex(HANDLE mutex);
+	FILE *inv_fopen(char *filename);
+	void inv_fclose(FILE *fp);
+
+	inv_error_t inv_destroy_mutex(HANDLE handle);
+
+	void inv_sleep(int mSecs);
+	unsigned long inv_get_tick_count(void);
+
+	/* Kernel implmentations */
+#define GFP_KERNEL (0x70)
+	static inline void *kmalloc(size_t size,
+				    unsigned int gfp_flags)
+	{
+        (void)gfp_flags;
+		return inv_malloc((unsigned int)size);
+	}
+	static inline void *kzalloc(size_t size, unsigned int gfp_flags)
+	{
+		void *tmp = inv_malloc((unsigned int)size);
+        (void)gfp_flags;
+		if (tmp)
+			memset(tmp, 0, size);
+		return tmp;
+	}
+	static inline void kfree(void *ptr)
+	{
+		inv_free(ptr);
+	}
+	static inline void msleep(long msecs)
+	{
+		inv_sleep(msecs);
+	}
+	static inline void udelay(unsigned long usecs)
+	{
+		inv_sleep((usecs + 999) / 1000);
+	}
+#else
+#include <linux/delay.h>
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+#endif				/* _MLOS_H */
diff --git a/65xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c b/65xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c
new file mode 100755
index 0000000..5424508
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c
@@ -0,0 +1,190 @@
+/*
+ $License:
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $
+ *
+ ******************************************************************************/
+
+/**
+ *  @defgroup MLOS
+ *  @brief OS Interface.
+ *
+ *  @{
+ *      @file mlos.c
+ *      @brief OS Interface.
+ */
+
+/* ------------- */
+/* - Includes. - */
+/* ------------- */
+
+#include <sys/time.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <stdlib.h>
+#include <errno.h>
+
+#include "stdint_invensense.h"
+#include "mlos.h"
+
+
+/* -------------- */
+/* - Functions. - */
+/* -------------- */
+
+/**
+ *  @brief  Allocate space
+ *  @param  num_bytes  number of bytes
+ *  @return pointer to allocated space
+ */
+void *inv_malloc(unsigned int num_bytes)
+{
+    // Allocate space.
+    void *alloc_ptr = malloc(num_bytes);
+    return alloc_ptr;
+}
+
+
+/**
+ *  @brief  Free allocated space
+ *  @param  ptr pointer to space to deallocate
+ *  @return error code.
+ */
+inv_error_t inv_free(void *ptr)
+{
+    if (ptr)
+        free(ptr);
+    return INV_SUCCESS;
+}
+
+
+/**
+ *  @brief  Mutex create function
+ *  @param  mutex   pointer to mutex handle
+ *  @return error code.
+ */
+inv_error_t inv_create_mutex(HANDLE *mutex)
+{
+    int res;
+    pthread_mutex_t *pm = malloc(sizeof(pthread_mutex_t));
+    if(pm == NULL) 
+        return INV_ERROR;
+
+    res = pthread_mutex_init(pm, NULL);
+    if(res == -1) {
+        free(pm);
+        return INV_ERROR_OS_CREATE_FAILED;
+    }
+
+    *mutex = (HANDLE)pm;
+
+    return INV_SUCCESS;
+}
+
+
+/**
+ *  @brief  Mutex lock function
+ *  @param  mutex   Mutex handle
+ *  @return error code.
+ */
+inv_error_t inv_lock_mutex(HANDLE mutex)
+{
+    int res;
+    pthread_mutex_t *pm = (pthread_mutex_t *)mutex;
+
+    res = pthread_mutex_lock(pm);
+    if(res == -1)
+        return INV_ERROR_OS_LOCK_FAILED;
+
+    return INV_SUCCESS;
+}
+
+
+/**
+ *  @brief  Mutex unlock function
+ *  @param  mutex   mutex handle
+ *  @return error code.
+ */
+inv_error_t inv_unlock_mutex(HANDLE mutex)
+{
+    int res;
+    pthread_mutex_t *pm = (pthread_mutex_t *)mutex;
+
+    res = pthread_mutex_unlock(pm);
+    if(res == -1) 
+        return INV_ERROR_OS_LOCK_FAILED;
+
+    return INV_SUCCESS;
+}
+
+
+/**
+ *  @brief  open file
+ *  @param  filename    name of the file to open.
+ *  @return error code.
+ */
+FILE *inv_fopen(char *filename)
+{
+    FILE *fp = fopen(filename, "r");
+    return fp;
+}
+
+
+/**
+ *  @brief  close the file.
+ *  @param  fp  handle to file to close.
+ *  @return error code.
+ */
+void inv_fclose(FILE *fp)
+{
+    fclose(fp);
+}
+
+/**
+ *  @brief  Close Handle
+ *  @param  handle  handle to the resource.
+ *  @return Zero if success, an error code otherwise.
+ */
+inv_error_t inv_destroy_mutex(HANDLE handle)
+{
+    int error;
+    pthread_mutex_t *pm = (pthread_mutex_t *)handle;
+    error = pthread_mutex_destroy(pm);
+    if (error)
+        return errno;
+    free((void*) handle);
+
+    return INV_SUCCESS;}
+
+
+/**
+ *  @brief  Sleep function.
+ */
+void inv_sleep(int m_secs)
+{
+    usleep(m_secs * 1000);
+}
+
+
+/**
+ *  @brief  get system's internal tick count.
+ *          Used for time reference.
+ *  @return current tick count.
+ */
+unsigned long inv_get_tick_count()
+{
+    struct timeval tv;
+
+    if (gettimeofday(&tv, NULL) != 0)
+        return 0;
+
+    return (long)((tv.tv_sec * 1000000LL + tv.tv_usec) / 1000LL);
+}
+
+/** @} */
+
diff --git a/65xx/libsensors_iio/software/core/mllite/message_layer.c b/65xx/libsensors_iio/software/core/mllite/message_layer.c
new file mode 100755
index 0000000..8317957
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/mllite/message_layer.h b/65xx/libsensors_iio/software/core/mllite/message_layer.h
new file mode 100755
index 0000000..bd3ddc4
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/message_layer.h
@@ -0,0 +1,43 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */

+#ifndef INV_MESSAGE_LAYER_H__

+#define INV_MESSAGE_LAYER_H__

+

+#include "mltypes.h"

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+

+/* Level 0 Type Messages */

+/** A motion event has occured */

+#define INV_MSG_MOTION_EVENT                (0x01)

+/** A no motion event has occured */

+#define INV_MSG_NO_MOTION_EVENT             (0x02)

+/** A setting of the gyro bias has occured */

+#define INV_MSG_NEW_GB_EVENT                (0x04)

+/** A setting of the compass bias has occured */

+#define INV_MSG_NEW_CB_EVENT                (0x08)

+/** A setting of the accel bias has occured */

+#define INV_MSG_NEW_AB_EVENT                (0x10)

+/** Sensor fusion has switched from 9- to 6-axes

+    because of a magnetic disturbance */

+#define INV_MSG_6X_SF_EVENT    (0x020)

+/** Compass accuracy has dropped has dropped to 0

+    because of a magnetic disturbance */

+#define INV_MSG_HEADING_NOT_ACCURATE_EVENT  (0x40)

+/** A setting of the factory gyro bias has occured */

+#define INV_MSG_NEW_FGB_EVENT            (0x80)

+

+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/65xx/libsensors_iio/software/core/mllite/ml_math_func.c b/65xx/libsensors_iio/software/core/mllite/ml_math_func.c
new file mode 100755
index 0000000..ca85510
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/ml_math_func.c
@@ -0,0 +1,1077 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+/**
+ *   @defgroup  ML_MATH_FUNC ml_math_func
+ *   @brief     Motion Library - Math Functions
+ *              Common math functions the Motion Library
+ *
+ *   @{
+ *       @file ml_math_func.c
+ *       @brief Math Functions.
+ */
+
+#include "mlmath.h"
+#include "ml_math_func.h"
+#include "mlinclude.h"
+#include <string.h>
+
+/** @internal
+ * Does the cross product of compass by gravity, then converts that
+ * to the world frame using the quaternion, then computes the angle that
+ * is made.
+ *
+ * @param[in] compass Compass Vector (Body Frame), length 3
+ * @param[in] grav Gravity Vector (Body Frame), length 3
+ * @param[in] quat Quaternion, Length 4
+ * @return Angle Cross Product makes after quaternion rotation.
+ */
+float inv_compass_angle(const long *compass, const long *grav, const float *quat)
+{
+    float cgcross[4], q1[4], q2[4], qi[4];
+    float angW;
+
+    // Compass cross Gravity
+    cgcross[0] = 0.f;
+    cgcross[1] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1];
+    cgcross[2] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2];
+    cgcross[3] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0];
+
+    // Now convert cross product into world frame
+    inv_q_multf(quat, cgcross, q1);
+    inv_q_invertf(quat, qi);
+    inv_q_multf(q1, qi, q2);
+
+    // Protect against atan2 of 0,0
+    if ((q2[2] == 0.f) && (q2[1] == 0.f))
+        return 0.f;
+
+    // This is the unfiltered heading correction
+    angW = -atan2f(q2[2], q2[1]);
+    return angW;
+}
+
+/**
+ *  @brief  The gyro data magnitude squared :
+ *          (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT.
+ * @param[in] gyro Gyro data scaled with 1 dps = 2^16
+ *  @return the computed magnitude squared output of the gyroscope.
+ */
+unsigned long inv_get_gyro_sum_of_sqr(const long *gyro)
+{
+    unsigned long gmag = 0;
+    long temp;
+    int kk;
+
+    for (kk = 0; kk < 3; ++kk) {
+        temp = gyro[kk] >> (16 - (GYRO_MAG_SQR_SHIFT / 2));
+        gmag += temp * temp;
+    }
+
+    return gmag;
+}
+
+/** Performs a multiply and shift by 29. These are good functions to write in assembly on
+ * with devices with small memory where you want to get rid of the long long which some
+ * assemblers don't handle well
+ * @param[in] a
+ * @param[in] b
+ * @return ((long long)a*b)>>29
+*/
+long inv_q29_mult(long a, long b)
+{
+#ifdef UMPL_ELIMINATE_64BIT
+    long result;
+    result = (long)((float)a * b / (1L << 29));
+    return result;
+#else
+    long long temp;
+    long result;
+    temp = (long long)a * b;
+    result = (long)(temp >> 29);
+    return result;
+#endif
+}
+
+/** Performs a multiply and shift by 30. These are good functions to write in assembly on
+ * with devices with small memory where you want to get rid of the long long which some
+ * assemblers don't handle well
+ * @param[in] a
+ * @param[in] b
+ * @return ((long long)a*b)>>30
+*/
+long inv_q30_mult(long a, long b)
+{
+#ifdef UMPL_ELIMINATE_64BIT
+    long result;
+    result = (long)((float)a * b / (1L << 30));
+    return result;
+#else
+    long long temp;
+    long result;
+    temp = (long long)a * b;
+    result = (long)(temp >> 30);
+    return result;
+#endif
+}
+
+#ifndef UMPL_ELIMINATE_64BIT
+long inv_q30_div(long a, long b)
+{
+    long long temp;
+    long result;
+    temp = (((long long)a) << 30) / b;
+    result = (long)temp;
+    return result;
+}
+#endif
+
+/** Performs a multiply and shift by shift. These are good functions to write
+ * in assembly on with devices with small memory where you want to get rid of
+ * the long long which some assemblers don't handle well
+ * @param[in] a First multicand
+ * @param[in] b Second multicand
+ * @param[in] shift Shift amount after multiplying
+ * @return ((long long)a*b)<<shift
+*/
+#ifndef UMPL_ELIMINATE_64BIT
+long inv_q_shift_mult(long a, long b, int shift)
+{
+    long result;
+    result = (long)(((long long)a * b) >> shift);
+    return result;
+}
+#endif
+
+/** Performs a fixed point quaternion multiply.
+* @param[in] q1 First Quaternion Multicand, length 4. 1.0 scaled
+*            to 2^30
+* @param[in] q2 Second Quaternion Multicand, length 4. 1.0 scaled
+*            to 2^30
+* @param[out] qProd Product after quaternion multiply. Length 4.
+*             1.0 scaled to 2^30.
+*/
+void inv_q_mult(const long *q1, const long *q2, long *qProd)
+{
+    INVENSENSE_FUNC_START;
+    qProd[0] = inv_q30_mult(q1[0], q2[0]) - inv_q30_mult(q1[1], q2[1]) -
+               inv_q30_mult(q1[2], q2[2]) - inv_q30_mult(q1[3], q2[3]);
+
+    qProd[1] = inv_q30_mult(q1[0], q2[1]) + inv_q30_mult(q1[1], q2[0]) +
+               inv_q30_mult(q1[2], q2[3]) - inv_q30_mult(q1[3], q2[2]);
+
+    qProd[2] = inv_q30_mult(q1[0], q2[2]) - inv_q30_mult(q1[1], q2[3]) +
+               inv_q30_mult(q1[2], q2[0]) + inv_q30_mult(q1[3], q2[1]);
+
+    qProd[3] = inv_q30_mult(q1[0], q2[3]) + inv_q30_mult(q1[1], q2[2]) -
+               inv_q30_mult(q1[2], q2[1]) + inv_q30_mult(q1[3], q2[0]);
+}
+
+/** Performs a fixed point quaternion addition.
+* @param[in] q1 First Quaternion term, length 4. 1.0 scaled
+*            to 2^30
+* @param[in] q2 Second Quaternion term, length 4. 1.0 scaled
+*            to 2^30
+* @param[out] qSum Sum after quaternion summation. Length 4.
+*             1.0 scaled to 2^30.
+*/
+void inv_q_add(long *q1, long *q2, long *qSum)
+{
+    INVENSENSE_FUNC_START;
+    qSum[0] = q1[0] + q2[0];
+    qSum[1] = q1[1] + q2[1];
+    qSum[2] = q1[2] + q2[2];
+    qSum[3] = q1[3] + q2[3];
+}
+
+void inv_vector_normalize(long *vec, int length)
+{
+    INVENSENSE_FUNC_START;
+    double normSF = 0;
+    int ii;
+    for (ii = 0; ii < length; ii++) {
+        normSF +=
+            inv_q30_to_double(vec[ii]) * inv_q30_to_double(vec[ii]);
+    }
+    if (normSF > 0) {
+        normSF = 1 / sqrt(normSF);
+        for (ii = 0; ii < length; ii++) {
+            vec[ii] = (int)((double)vec[ii] * normSF);
+        }
+    } else {
+        vec[0] = 1073741824L;
+        for (ii = 1; ii < length; ii++) {
+            vec[ii] = 0;
+        }
+    }
+}
+
+void inv_q_normalize(long *q)
+{
+    INVENSENSE_FUNC_START;
+    inv_vector_normalize(q, 4);
+}
+
+void inv_q_invert(const long *q, long *qInverted)
+{
+    INVENSENSE_FUNC_START;
+    qInverted[0] = q[0];
+    qInverted[1] = -q[1];
+    qInverted[2] = -q[2];
+    qInverted[3] = -q[3];
+}
+
+double quaternion_to_rotation_angle(const long *quat) {
+    double quat0 = (double )quat[0] / 1073741824;
+    if (quat0 > 1.0f) {
+        quat0 = 1.0;
+    } else if (quat0 < -1.0f) {
+        quat0 = -1.0;
+    }
+
+    return acos(quat0)*2*180/M_PI;
+}
+
+/** Rotates a 3-element vector by Rotation defined by Q
+*/
+void inv_q_rotate(const long *q, const long *in, long *out)
+{
+    long q_temp1[4], q_temp2[4];
+    long in4[4], out4[4];
+
+    // Fixme optimize
+    in4[0] = 0;
+    memcpy(&in4[1], in, 3 * sizeof(long));
+    inv_q_mult(q, in4, q_temp1);
+    inv_q_invert(q, q_temp2);
+    inv_q_mult(q_temp1, q_temp2, out4);
+    memcpy(out, &out4[1], 3 * sizeof(long));
+}
+
+void inv_q_multf(const float *q1, const float *q2, float *qProd)
+{
+    INVENSENSE_FUNC_START;
+    qProd[0] =
+        (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]);
+    qProd[1] =
+        (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]);
+    qProd[2] =
+        (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]);
+    qProd[3] =
+        (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]);
+}
+
+void inv_q_addf(const float *q1, const float *q2, float *qSum)
+{
+    INVENSENSE_FUNC_START;
+    qSum[0] = q1[0] + q2[0];
+    qSum[1] = q1[1] + q2[1];
+    qSum[2] = q1[2] + q2[2];
+    qSum[3] = q1[3] + q2[3];
+}
+
+void inv_q_normalizef(float *q)
+{
+    INVENSENSE_FUNC_START;
+    float normSF = 0;
+    float xHalf = 0;
+    normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+    if (normSF < 2) {
+        xHalf = 0.5f * normSF;
+        normSF = normSF * (1.5f - xHalf * normSF * normSF);
+        normSF = normSF * (1.5f - xHalf * normSF * normSF);
+        normSF = normSF * (1.5f - xHalf * normSF * normSF);
+        normSF = normSF * (1.5f - xHalf * normSF * normSF);
+        q[0] *= normSF;
+        q[1] *= normSF;
+        q[2] *= normSF;
+        q[3] *= normSF;
+    } else {
+        q[0] = 1.0;
+        q[1] = 0.0;
+        q[2] = 0.0;
+        q[3] = 0.0;
+    }
+    normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+}
+
+/** Performs a length 4 vector normalization with a square root.
+* @param[in,out] q vector to normalize. Returns [1,0,0,0] is magnitude is zero.
+*/
+void inv_q_norm4(float *q)
+{
+    float mag;
+    mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+    if (mag) {
+        q[0] /= mag;
+        q[1] /= mag;
+        q[2] /= mag;
+        q[3] /= mag;
+    } else {
+        q[0] = 1.f;
+        q[1] = 0.f;
+        q[2] = 0.f;
+        q[3] = 0.f;
+    }
+}
+
+void inv_q_invertf(const float *q, float *qInverted)
+{
+    INVENSENSE_FUNC_START;
+    qInverted[0] = q[0];
+    qInverted[1] = -q[1];
+    qInverted[2] = -q[2];
+    qInverted[3] = -q[3];
+}
+
+/**
+ * Converts a quaternion to a rotation matrix.
+ * @param[in] quat 4-element quaternion in fixed point. One is 2^30.
+ * @param[out] rot Rotation matrix in fixed point. One is 2^30. The
+ *             First 3 elements of the rotation matrix, represent
+ *             the first row of the matrix. Rotation matrix multiplied
+ *             by a 3 element column vector transform a vector from Body
+ *             to World.
+ */
+void inv_quaternion_to_rotation(const long *quat, long *rot)
+{
+    rot[0] =
+        inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0],
+                quat[0]) -
+        1073741824L;
+    rot[1] =
+        inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]);
+    rot[2] =
+        inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]);
+    rot[3] =
+        inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]);
+    rot[4] =
+        inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0],
+                quat[0]) -
+        1073741824L;
+    rot[5] =
+        inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]);
+    rot[6] =
+        inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]);
+    rot[7] =
+        inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]);
+    rot[8] =
+        inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0],
+                quat[0]) -
+        1073741824L;
+}
+
+/**
+ * Converts a quaternion to a rotation vector. A rotation vector is
+ * a method to represent a 4-element quaternion vector in 3-elements.
+ * To get the quaternion from the 3-elements, The last 3-elements of
+ * the quaternion will be the given rotation vector. The first element
+ * of the quaternion will be the positive value that will be required
+ * to make the magnitude of the quaternion 1.0 or 2^30 in fixed point units.
+ * @param[in] quat 4-element quaternion in fixed point. One is 2^30.
+ * @param[out] rot Rotation vector in fixed point. One is 2^30.
+ */
+void inv_quaternion_to_rotation_vector(const long *quat, long *rot)
+{
+    rot[0] = quat[1];
+    rot[1] = quat[2];
+    rot[2] = quat[3];
+
+    if (quat[0] < 0.0) {
+        rot[0] = -rot[0];
+        rot[1] = -rot[1];
+        rot[2] = -rot[2];
+    }
+}
+
+/** Converts a 32-bit long to a big endian byte stream */
+unsigned char *inv_int32_to_big8(long x, unsigned char *big8)
+{
+    big8[0] = (unsigned char)((x >> 24) & 0xff);
+    big8[1] = (unsigned char)((x >> 16) & 0xff);
+    big8[2] = (unsigned char)((x >> 8) & 0xff);
+    big8[3] = (unsigned char)(x & 0xff);
+    return big8;
+}
+
+/** Converts a big endian byte stream into a 32-bit long */
+long inv_big8_to_int32(const unsigned char *big8)
+{
+    long x;
+    x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8)
+        | ((long)big8[3]);
+    return x;
+}
+
+/** Converts a big endian byte stream into a 16-bit integer (short) */
+short inv_big8_to_int16(const unsigned char *big8)
+{
+    short x;
+    x = ((short)big8[0] << 8) | ((short)big8[1]);
+    return x;
+}
+
+/** Converts a little endian byte stream into a 16-bit integer (short) */
+short inv_little8_to_int16(const unsigned char *little8)
+{
+    short x;
+    x = ((short)little8[1] << 8) | ((short)little8[0]);
+    return x;
+}
+
+/** Converts a 16-bit short to a big endian byte stream */
+unsigned char *inv_int16_to_big8(short x, unsigned char *big8)
+{
+    big8[0] = (unsigned char)((x >> 8) & 0xff);
+    big8[1] = (unsigned char)(x & 0xff);
+    return big8;
+}
+
+void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y)
+{
+    int k, l, i, j;
+    for (i = 0, k = 0; i < *n; i++, k++) {
+        for (j = 0, l = 0; j < *n; j++, l++) {
+            if (i == x)
+                i++;
+            if (j == y)
+                j++;
+            *(b + 6 * k + l) = *(a + 6 * i + j);
+        }
+    }
+    *n = *n - 1;
+}
+
+void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y)
+{
+    int k, l, i, j;
+    for (i = 0, k = 0; i < *n; i++, k++) {
+        for (j = 0, l = 0; j < *n; j++, l++) {
+            if (i == x)
+                i++;
+            if (j == y)
+                j++;
+            *(b + 6 * k + l) = *(a + 6 * i + j);
+        }
+    }
+    *n = *n - 1;
+}
+
+float inv_matrix_det(float *p, int *n)
+{
+    float d[6][6], sum = 0;
+    int i, j, m;
+    m = *n;
+    if (*n == 2)
+        return (*p ** (p + 7) - *(p + 1) ** (p + 6));
+    for (i = 0, j = 0; j < m; j++) {
+        *n = m;
+        inv_matrix_det_inc(p, &d[0][0], n, i, j);
+        sum =
+            sum + *(p + 6 * i + j) * SIGNM(i +
+                                            j) *
+            inv_matrix_det(&d[0][0], n);
+    }
+
+    return (sum);
+}
+
+double inv_matrix_detd(double *p, int *n)
+{
+    double d[6][6], sum = 0;
+    int i, j, m;
+    m = *n;
+    if (*n == 2)
+        return (*p ** (p + 7) - *(p + 1) ** (p + 6));
+    for (i = 0, j = 0; j < m; j++) {
+        *n = m;
+        inv_matrix_det_incd(p, &d[0][0], n, i, j);
+        sum =
+            sum + *(p + 6 * i + j) * SIGNM(i +
+                                            j) *
+            inv_matrix_detd(&d[0][0], n);
+    }
+
+    return (sum);
+}
+
+/** Wraps angle from (-M_PI,M_PI]
+ * @param[in] ang Angle in radians to wrap
+ * @return Wrapped angle from (-M_PI,M_PI]
+ */
+float inv_wrap_angle(float ang)
+{
+    if (ang > M_PI)
+        return ang - 2 * (float)M_PI;
+    else if (ang <= -(float)M_PI)
+        return ang + 2 * (float)M_PI;
+    else
+        return ang;
+}
+
+/** Finds the minimum angle difference ang1-ang2 such that difference
+ * is between [-M_PI,M_PI]
+ * @param[in] ang1
+ * @param[in] ang2
+ * @return angle difference ang1-ang2
+ */
+float inv_angle_diff(float ang1, float ang2)
+{
+    float d;
+    ang1 = inv_wrap_angle(ang1);
+    ang2 = inv_wrap_angle(ang2);
+    d = ang1 - ang2;
+    if (d > M_PI)
+        d -= 2 * (float)M_PI;
+    else if (d < -(float)M_PI)
+        d += 2 * (float)M_PI;
+    return d;
+}
+
+/** bernstein hash, derived from public domain source */
+uint32_t inv_checksum(const unsigned char *str, int len)
+{
+    uint32_t hash = 5381;
+    int i, c;
+
+    for (i = 0; i < len; i++) {
+        c = *(str + i);
+        hash = ((hash << 5) + hash) + c;	/* hash * 33 + c */
+    }
+
+    return hash;
+}
+
+static unsigned short inv_row_2_scale(const signed char *row)
+{
+    unsigned short b;
+
+    if (row[0] > 0)
+        b = 0;
+    else if (row[0] < 0)
+        b = 4;
+    else if (row[1] > 0)
+        b = 1;
+    else if (row[1] < 0)
+        b = 5;
+    else if (row[2] > 0)
+        b = 2;
+    else if (row[2] < 0)
+        b = 6;
+    else
+        b = 7;		// error
+    return b;
+}
+
+
+/** Converts an orientation matrix made up of 0,+1,and -1 to a scalar representation.
+* @param[in] mtx Orientation matrix to convert to a scalar.
+* @return Description of orientation matrix. The lowest 2 bits (0 and 1) represent the column the one is on for the
+* first row, with the bit number 2 being the sign. The next 2 bits (3 and 4) represent
+* the column the one is on for the second row with bit number 5 being the sign.
+* The next 2 bits (6 and 7) represent the column the one is on for the third row with
+* bit number 8 being the sign. In binary the identity matrix would therefor be:
+* 010_001_000 or 0x88 in hex.
+*/
+unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
+{
+
+    unsigned short scalar;
+
+    /*
+       XYZ  010_001_000 Identity Matrix
+       XZY  001_010_000
+       YXZ  010_000_001
+       YZX  000_010_001
+       ZXY  001_000_010
+       ZYX  000_001_010
+     */
+
+    scalar = inv_row_2_scale(mtx);
+    scalar |= inv_row_2_scale(mtx + 3) << 3;
+    scalar |= inv_row_2_scale(mtx + 6) << 6;
+
+
+    return scalar;
+}
+
+/** Uses the scalar orientation value to convert from chip frame to body frame
+* @param[in] orientation A scalar that represent how to go from chip to body frame
+* @param[in] input Input vector, length 3
+* @param[out] output Output vector, length 3
+*/
+void inv_convert_to_body(unsigned short orientation, const long *input, long *output)
+{
+    output[0] = input[orientation      & 0x03] * SIGNSET(orientation & 0x004);
+    output[1] = input[(orientation>>3) & 0x03] * SIGNSET(orientation & 0x020);
+    output[2] = input[(orientation>>6) & 0x03] * SIGNSET(orientation & 0x100);
+}
+
+/** Uses the scalar orientation value to convert from body frame to chip frame
+* @param[in] orientation A scalar that represent how to go from chip to body frame
+* @param[in] input Input vector, length 3
+* @param[out] output Output vector, length 3
+*/
+void inv_convert_to_chip(unsigned short orientation, const long *input, long *output)
+{
+    output[orientation & 0x03]      = input[0] * SIGNSET(orientation & 0x004);
+    output[(orientation>>3) & 0x03] = input[1] * SIGNSET(orientation & 0x020);
+    output[(orientation>>6) & 0x03] = input[2] * SIGNSET(orientation & 0x100);
+}
+
+
+/** Uses the scalar orientation value to convert from chip frame to body frame and
+* apply appropriate scaling.
+* @param[in] orientation A scalar that represent how to go from chip to body frame
+* @param[in] sensitivity Sensitivity scale
+* @param[in] input Input vector, length 3
+* @param[out] output Output vector, length 3
+*/
+void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output)
+{
+    output[0] = inv_q30_mult(input[orientation & 0x03] *
+                             SIGNSET(orientation & 0x004), sensitivity);
+    output[1] = inv_q30_mult(input[(orientation>>3) & 0x03] *
+                             SIGNSET(orientation & 0x020), sensitivity);
+    output[2] = inv_q30_mult(input[(orientation>>6) & 0x03] *
+                             SIGNSET(orientation & 0x100), sensitivity);
+}
+
+/** find a norm for a vector
+* @param[in] a vector [3x1]
+* @param[out] output the norm of the input vector
+*/
+double inv_vector_norm(const float *x)
+{
+    return sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]);
+}
+
+void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff) {
+    int i;
+    // initial state to zero
+    pFilter->state[0] = 0;
+    pFilter->state[1] = 0;
+
+    // set up coefficients
+    for (i=0; i<5; i++) {
+        pFilter->c[i] = pBiquadCoeff[i];
+    }
+}
+
+void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input)
+{
+    pFilter->input = input;
+    pFilter->output = input;
+    pFilter->state[0] = input / (1 + pFilter->c[2] + pFilter->c[3]);
+    pFilter->state[1] = pFilter->state[0];
+}
+
+float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input)  {
+    float stateZero;
+
+    pFilter->input = input;
+    // calculate the new state;
+    stateZero = pFilter->input - pFilter->c[2]*pFilter->state[0]
+                               - pFilter->c[3]*pFilter->state[1];
+
+    pFilter->output = stateZero + pFilter->c[0]*pFilter->state[0]
+                                + pFilter->c[1]*pFilter->state[1];
+
+    // update the output and state
+    pFilter->output = pFilter->output * pFilter->c[4];
+    pFilter->state[1] = pFilter->state[0];
+    pFilter->state[0] = stateZero;
+    return pFilter->output;
+}
+
+void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3])  {
+
+    cgcross[0] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1];
+    cgcross[1] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2];
+    cgcross[2] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0];
+}
+
+void mlMatrixVectorMult(long matrix[9], const long vecIn[3], long *vecOut)  {
+        // matrix format
+        //  [ 0  3  6;
+        //    1  4  7;
+        //    2  5  8]
+
+        // vector format:  [0  1  2]^T;
+        int i, j;
+        long temp;
+
+        for (i=0; i<3; i++)	{
+                temp = 0;
+                for (j=0; j<3; j++)  {
+                        temp += inv_q30_mult(matrix[i+j*3], vecIn[j]);
+                }
+                vecOut[i] = temp;
+        }
+}
+
+//============== 1/sqrt(x), 1/x, sqrt(x) Functions ================================
+
+/** Calculates 1/square-root of a fixed-point number (30 bit mantissa, positive): Q1.30
+* Input must be a positive scaled (2^30) integer
+* The number is scaled to lie between a range in which a Newton-Raphson
+* iteration works best. Corresponding square root of the power of two is returned.
+*  Caller must scale final result by 2^rempow (while avoiding overflow).
+* @param[in] x0, length 1
+* @param[out] rempow, length 1
+* @return scaledSquareRoot on success or zero.
+*/
+long inv_inverse_sqrt(long x0, int*rempow)

+{

+	//% Inverse sqrt NR in the neighborhood of 1.3>x>=0.65

+	//% x(k+1) = x(k)*(3 - x0*x(k)^2)

+

+	//% Seed equals 1. Works best in this region.

+	//xx0 = int32(1*2^30);

+

+	long oneoversqrt2, oneandhalf, x0_2;

+	unsigned long xx;

+	int pow2, sq2scale, nr_iters;

+	//long upscale, sqrt_upscale, upsclimit;

+	//long downscale, sqrt_downscale, downsclimit;

+
+	// Precompute some constants for efficiency
+	//% int32(2^30*1/sqrt(2))

+	oneoversqrt2=759250125L;

+	//% int32(1.5*2^30);

+	oneandhalf=1610612736L;

+

+	//// Further scaling into optimal region saves one or more NR iterations. Maps into region (.9, 1.1)

+	//// int32(0.9/log(2)*2^30)

+	//upscale = 1394173804L;

+	//// int32(sqrt(0.9/log(2))*2^30)

+	//sqrt_upscale = 1223512453L;

+	// // int32(1.1*log(2)/.9*2^30)

+	//upsclimit = 909652478L;

+	//// int32(1.1/log(4)*2^30)

+	//downscale = 851995103L;

+	//// int32(sqrt(1.1/log(4))*2^30)

+	//sqrt_downscale = 956463682L;

+	// // int32(0.9*log(4)/1.1*2^30)

+	//downsclimit = 1217881829L;

+

+	nr_iters = test_limits_and_scale(&x0, &pow2);

+

+	sq2scale=pow2%2;  // Find remainder. Is it even or odd?

+

+	

+	// Further scaling to decrease NR iterations

+	// With the mapping below, 89% of calculations will require 2 NR iterations or less.

+	// TBD

+

+

+	x0_2 = x0 >>1; // This scaling incorporates factor of 2 in NR iteration below.

+	// Initial condition starts at 1: xx=(1L<<30);

+	// First iteration is simple: Instead of initializing xx=1, assign to result of first iteration:

+	// xx= (3/2-x0/2);

+	//% NR formula: xx=xx*(3/2-x0*xx*xx/2); = xx*(1.5 - (x0/2)*xx*xx)

+	// Initialize NR (first iteration). Note we are starting with xx=1, so the first iteration becomes an initialization.

+	xx = oneandhalf - x0_2;

+ 	if ( nr_iters>=2 ) {

+		// Second NR iteration

+		xx = inv_q30_mult( xx, ( oneandhalf - inv_q30_mult(x0_2, inv_q30_mult(xx,xx) ) ) );
+		if ( nr_iters==3 ) {

+			// Third NR iteration. 

+			xx = inv_q30_mult( xx, ( oneandhalf - inv_q30_mult(x0_2, inv_q30_mult(xx,xx) ) ) );
+			// Fourth NR iteration: Not needed due to single precision.

+		}

+	}
+	if (sq2scale) {
+		*rempow = (pow2>>1) + 1; // Account for sqrt(2) in denominator, note we multiply if s2scale is true
+		return (inv_q30_mult(xx,oneoversqrt2));
+	}
+	else {
+		*rempow = pow2>>1;
+		return xx;
+	}
+}
+
+
+/** Calculates square-root of a fixed-point number (30 bit mantissa, positive)
+* Input must be a positive scaled (2^30) integer
+* The number is scaled to lie between a range in which a Newton-Raphson
+* iteration works best.
+* @param[in] x0, length 1
+* @return scaledSquareRoot on success or zero. **/
+long inv_fast_sqrt(long x0)
+{
+
+	//% Square-Root with NR in the neighborhood of 1.3>x>=0.65 (log(2) <= x <= log(4) )

+    // Two-variable NR iteration:

+    // Initialize: a=x; c=x-1;  

+    // 1st Newton Step:  a=a-a*c/2; ( or: a = x - x*(x-1)/2  )

+    // Iterate: c = c*c*(c-3)/4

+    //          a = a - a*c/2    --> reevaluating c at this step gives error of approximation

+

+	//% Seed equals 1. Works best in this region.

+	//xx0 = int32(1*2^30);

+

+	long sqrt2, oneoversqrt2, one_pt5;

+	long xx, cc;

+	int pow2, sq2scale, nr_iters;

+
+	// Return if input is zero. Negative should really error out. 
+	if (x0 <= 0L) {

+		return 0L;

+	}

+

+	sqrt2 =1518500250L;

+	oneoversqrt2=759250125L;

+	one_pt5=1610612736L;

+

+	nr_iters = test_limits_and_scale(&x0, &pow2);

+	

+	sq2scale = 0;

+	if (pow2 > 0) 

+		sq2scale=pow2%2;  // Find remainder. Is it even or odd?

+	pow2 = pow2-sq2scale; // Now pow2 is even. Note we are adding because result is scaled with sqrt(2)

+

+	// Sqrt 1st NR iteration

+	cc = x0 - (1L<<30);

+	xx = x0 - (inv_q30_mult(x0, cc)>>1);

+ 	if ( nr_iters>=2 ) {

+		// Sqrt second NR iteration

+		// cc = cc*cc*(cc-3)/4; = cc*cc*(cc/2 - 3/2)/2;

+		// cc = ( cc*cc*((cc>>1) - onePt5) ) >> 1

+		cc = inv_q30_mult( cc, inv_q30_mult(cc, (cc>>1) - one_pt5) ) >> 1;

+		xx = xx - (inv_q30_mult(xx, cc)>>1);
+		if ( nr_iters==3 ) {

+			// Sqrt third NR iteration

+			cc = inv_q30_mult( cc, inv_q30_mult(cc, (cc>>1) - one_pt5) ) >> 1;

+			xx = xx - (inv_q30_mult(xx, cc)>>1);
+		}

+	}
+	if (sq2scale)
+		xx = inv_q30_mult(xx,oneoversqrt2);
+	// Scale the number with the half of the power of 2 scaling
+	if (pow2>0)
+		xx = (xx >> (pow2>>1)); 
+	else if (pow2 == -1)
+		xx = inv_q30_mult(xx,sqrt2);
+	return xx;
+}
+
+/** Calculates 1/x of a fixed-point number (30 bit mantissa)
+* Input must be a scaled (2^30) integer (+/-)
+* The number is scaled to lie between a range in which a Newton-Raphson
+* iteration works best. Corresponding multiplier power of two is returned.
+*  Caller must scale final result by 2^pow (while avoiding overflow).
+* @param[in] x, length 1
+* @param[out] pow, length 1
+* @return scaledOneOverX on success or zero.
+*/
+long inv_one_over_x(long x0, int*pow)

+{

+	//% NR for 1/x in the neighborhood of log(2) => x => log(4)

+	//%    y(k+1)=y(k)*(2 – x0*y(k))

+    //% with y(0) = 1 as the starting value for NR

+

+	long two, xx;

+	int numberwasnegative, nr_iters, did_upscale, did_downscale;

+

+	long upscale, downscale, upsclimit, downsclimit;

+

+	*pow = 0;
+	// Return if input is zero. 
+	if (x0 == 0L) {

+		return 0L;

+	}

+

+	// This is really (2^31-1), i.e. 1.99999... .

+	// Approximation error is 1e-9. Note 2^31 will overflow to sign bit, so it can't be used here.

+	two = 2147483647L; 

+

+	// int32(0.92/log(2)*2^30)

+	upscale = 1425155444L;

+	// int32(1.08/upscale*2^30) 

+	upsclimit = 873697834L;

+

+	// int32(1.08/log(4)*2^30)

+	downscale = 836504283L;

+	// int32(0.92/downscale*2^30) 

+	downsclimit = 1268000423L;

+

+	// Algorithm is intended to work with positive numbers only. Change sign:

+	numberwasnegative = 0;

+	if (x0 < 0L) {

+		numberwasnegative = 1;

+		x0 = -x0;

+	}

+

+	nr_iters = test_limits_and_scale(&x0, pow);

+

+	did_upscale=0;

+	did_downscale=0;

+	// Pre-scaling to reduce NR iterations and improve accuracy:

+	if (x0<=upsclimit) {

+		x0 = inv_q30_mult(x0, upscale);

+		did_upscale = 1;

+		// The scaling ALWAYS leaves the number in the 2-NR iterations region:

+		nr_iters = 2;

+		// Is x0 in the single NR iteration region (0.994, 1.006) ?

+		if (x0 > 1067299373 && x0 < 1080184275)

+			nr_iters = 1;

+	} else if (x0>=downsclimit) {

+		x0 = inv_q30_mult(x0, downscale);

+		did_downscale = 1;

+		// The scaling ALWAYS leaves the number in the 2-NR iterations region:

+		nr_iters = 2;

+		// Is x0 in the single NR iteration region (0.994, 1.006) ?

+		if (x0 > 1067299373 && x0 < 1080184275)

+			nr_iters = 1;

+	}

+

+	xx = (two - x0) + 1; // Note 2 will overflow so the computation (2-x) is done with "two" == (2^30-1)

+	// First NR iteration

+	xx = inv_q30_mult( xx, (two - inv_q30_mult(x0, xx)) + 1 );
+ 	if ( nr_iters>=2 ) {

+		// Second NR iteration

+		xx = inv_q30_mult( xx, (two - inv_q30_mult(x0, xx)) + 1 );
+		if ( nr_iters==3 ) {

+			// THird NR iteration. 

+			xx = inv_q30_mult( xx, (two - inv_q30_mult(x0, xx)) + 1 );
+			// Fourth NR iteration: Not needed due to single precision.

+		}

+	}
+
+	// Post-scaling
+	if (did_upscale)
+		xx = inv_q30_mult( xx, upscale);
+	else if (did_downscale)
+		xx = inv_q30_mult( xx, downscale);
+
+	if (numberwasnegative) 
+		xx = -xx;
+	return xx;
+}
+
+/** Auxiliary function used by inv_OneOverX(), inv_fastSquareRoot(), inv_inverseSqrt().
+* Finds the range of the argument, determines the optimal number of Newton-Raphson
+* iterations and . Corresponding square root of the power of two is returned.
+* Restrictions: Number is represented as Q1.30.
+*               Number is betweeen the range 2<x<=0
+* @param[in] x, length 1
+* @param[out] pow, length 1
+* @return # of NR iterations, x0 scaled between log(2) and log(4) and 2^N scaling (N=pow)
+*/
+int test_limits_and_scale(long *x0, int *pow)
+{
+	long lowerlimit, upperlimit, oneiterlothr, oneiterhithr, zeroiterlothr, zeroiterhithr;

+
+	// Lower Limit: ll = int32(log(2)*2^30);

+	lowerlimit = 744261118L;

+	//Upper Limit ul = int32(log(4)*2^30);

+	upperlimit = 1488522236L;

+	//  int32(0.9*2^30)

+	oneiterlothr = 966367642L;

+	// int32(1.1*2^30)

+	oneiterhithr = 1181116006L;

+	// int32(0.99*2^30)

+	zeroiterlothr=1063004406L;

+	//int32(1.01*2^30)

+	zeroiterhithr=1084479242L;

+

+	// Scale number such that Newton Raphson iteration works best:
+	// Find the power of two scaling that leaves the number in the optimal range,
+	// ll <= number <= ul. Note odd powers have special scaling further below
+	if (*x0 > upperlimit) {

+		// Halving the number will push it in the optimal range since largest value is 2

+		*x0 = *x0>>1;

+		*pow=-1;

+	} else if (*x0 < lowerlimit) {

+		// Find position of highest bit, counting from left, and scale number 

+		*pow=get_highest_bit_position((unsigned long*)x0);

+		if (*x0 >= upperlimit) {

+			// Halving the number will push it in the optimal range

+			*x0 = *x0>>1;

+			*pow=*pow-1;

+		}

+		else if (*x0 < lowerlimit) {

+			// Doubling the number will push it in the optimal range

+			*x0 = *x0<<1;

+			*pow=*pow+1;

+		}

+	} else {

+		*pow = 0;

+	}

+
+	if ( *x0<oneiterlothr || *x0>oneiterhithr )

+		return 3; // 3 NR iterations

+	if ( *x0<zeroiterlothr || *x0>zeroiterhithr )

+		return 2; // 2 NR iteration
+
+	return 1; // 1 NR iteration
+}
+
+/** Auxiliary function used by testLimitsAndScale()
+* Find the highest nonzero bit in an unsigned 32 bit integer:
+* @param[in] value, length 1.
+* @return highes bit position.

+**/int get_highest_bit_position(unsigned long *value)

+{

+    int position;

+    position = 0;

+    if (*value == 0) return 0;

+

+    if ((*value & 0xFFFF0000) == 0) {

+		position += 16;

+		*value=*value<<16;

+	}

+    if ((*value & 0xFF000000) == 0) {

+		position += 8;

+		*value=*value<<8;

+	}

+    if ((*value & 0xF0000000) == 0) {

+		position += 4;

+		*value=*value<<4;

+	}

+    if ((*value & 0xC0000000) == 0) {

+		position += 2;

+		*value=*value<<2;

+	}

+

+	// If we got too far into sign bit, shift back. Note we are using an

+	// unsigned long here, so right shift is going to shift all the bits.

+    if ((*value & 0x80000000)) { 

+		position -= 1;

+		*value=*value>>1;

+	}

+    return position;

+}
+
+/* compute real part of quaternion, element[0]
+@param[in]  inQuat, 3 elements gyro quaternion
+@param[out] outquat, 4 elements gyro quaternion
+*/
+int inv_compute_scalar_part(const long * inQuat, long* outQuat)
+{
+    long scalarPart = 0;
+
+    scalarPart = inv_fast_sqrt((1L<<30) - inv_q30_mult(inQuat[0], inQuat[0])
+                                        - inv_q30_mult(inQuat[1], inQuat[1])
+                                        - inv_q30_mult(inQuat[2], inQuat[2]) );
+                outQuat[0] = scalarPart;
+                outQuat[1] = inQuat[0];
+                outQuat[2] = inQuat[1];
+                outQuat[3] = inQuat[2];
+
+                return 0;
+}
+/**
+ * @}
+ */
diff --git a/65xx/libsensors_iio/software/core/mllite/ml_math_func.h b/65xx/libsensors_iio/software/core/mllite/ml_math_func.h
new file mode 100755
index 0000000..1540254
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/ml_math_func.h
@@ -0,0 +1,129 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#ifndef INVENSENSE_INV_MATH_FUNC_H__
+#define INVENSENSE_INV_MATH_FUNC_H__
+
+#include "mltypes.h"
+
+#define GYRO_MAG_SQR_SHIFT 6
+#define NUM_ROTATION_MATRIX_ELEMENTS (9)
+#define ROT_MATRIX_SCALE_LONG  (1073741824L)
+#define ROT_MATRIX_SCALE_FLOAT (1073741824.0f)
+#define ROT_MATRIX_LONG_TO_FLOAT( longval ) \
+    ((float) ((longval) / ROT_MATRIX_SCALE_FLOAT ))
+#define SIGNM(k)((int)(k)&1?-1:1)
+#define SIGNSET(x) ((x) ? -1 : +1)
+
+#define INV_TWO_POWER_NEG_30 9.313225746154785e-010f
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+     typedef struct {
+        float state[4];
+        float c[5];
+        float input;
+        float output;
+    }   inv_biquad_filter_t;
+
+    static inline float inv_q30_to_float(long q30)
+    {
+        return (float) q30 / ((float)(1L << 30));
+    }
+
+    static inline double inv_q30_to_double(long q30)
+    {
+        return (double) q30 / ((double)(1L << 30));
+    }
+
+    static inline float inv_q16_to_float(long q16)
+    {
+        return (float) q16 / (1L << 16);
+    }
+
+    static inline double inv_q16_to_double(long q16)
+    {
+        return (double) q16 / (1L << 16);
+    }
+
+
+
+
+    long inv_q29_mult(long a, long b);
+    long inv_q30_mult(long a, long b);
+
+    /* UMPL_ELIMINATE_64BIT Notes:
+     * An alternate implementation using float instead of long long accudoublemulators
+     * is provided for q29_mult and q30_mult.
+     * When long long accumulators are used and an alternate implementation is not
+     * available, we eliminate the entire function and header with a macro.
+     */
+#ifndef UMPL_ELIMINATE_64BIT
+    long inv_q30_div(long a, long b);
+    long inv_q_shift_mult(long a, long b, int shift);
+#endif
+
+    void inv_q_mult(const long *q1, const long *q2, long *qProd);
+    void inv_q_add(long *q1, long *q2, long *qSum);
+    void inv_q_normalize(long *q);
+    void inv_q_invert(const long *q, long *qInverted);
+    void inv_q_multf(const float *q1, const float *q2, float *qProd);
+    void inv_q_addf(const float *q1, const float *q2, float *qSum);
+    void inv_q_normalizef(float *q);
+    void inv_q_norm4(float *q);
+    void inv_q_invertf(const float *q, float *qInverted);
+    void inv_quaternion_to_rotation(const long *quat, long *rot);
+    unsigned char *inv_int32_to_big8(long x, unsigned char *big8);
+    long inv_big8_to_int32(const unsigned char *big8);
+    short inv_big8_to_int16(const unsigned char *big8);
+    short inv_little8_to_int16(const unsigned char *little8);
+    unsigned char *inv_int16_to_big8(short x, unsigned char *big8);
+    float inv_matrix_det(float *p, int *n);
+    void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y);
+    double inv_matrix_detd(double *p, int *n);
+    void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y);
+    float inv_wrap_angle(float ang);
+    float inv_angle_diff(float ang1, float ang2);
+    void inv_quaternion_to_rotation_vector(const long *quat, long *rot);
+    unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx);
+    void inv_convert_to_body(unsigned short orientation, const long *input, long *output);
+    void inv_convert_to_chip(unsigned short orientation, const long *input, long *output);
+    void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output);
+    void inv_q_rotate(const long *q, const long *in, long *out);
+	void inv_vector_normalize(long *vec, int length);
+    uint32_t inv_checksum(const unsigned char *str, int len);
+    float inv_compass_angle(const long *compass, const long *grav,
+                            const float *quat);
+    unsigned long inv_get_gyro_sum_of_sqr(const long *gyro);
+
+    static inline long inv_delta_time_ms(inv_time_t t1, inv_time_t t2)
+    {
+        return (long)((t1 - t2) / 1000000L);
+    }
+
+    double quaternion_to_rotation_angle(const long *quat);
+    double inv_vector_norm(const float *x);
+
+    void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff);
+    float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input);
+    void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input);
+    void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]);
+
+    void mlMatrixVectorMult(long matrix[9], const long vecIn[3], long *vecOut);
+
+	long inv_inverse_sqrt(long x0, int*rempow);
+	long inv_fast_sqrt(long x0);
+	long inv_one_over_x(long x0, int*pow);
+	int test_limits_and_scale(long *x0, int *pow);
+	int get_highest_bit_position(unsigned long *value);
+    int inv_compute_scalar_part(const long * inQuat, long* outQuat);
+
+#ifdef __cplusplus
+}
+#endif
+#endif // INVENSENSE_INV_MATH_FUNC_H__
diff --git a/65xx/libsensors_iio/software/core/mllite/mpl.c b/65xx/libsensors_iio/software/core/mllite/mpl.c
new file mode 100755
index 0000000..0415e57
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/mpl.c
@@ -0,0 +1,77 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+/**
+ *   @defgroup  MPL mpl
+ *   @brief     Motion Library - Start Point
+ *              Initializes MPL.
+ *
+ *   @{
+ *       @file  mpl.c
+ *       @brief MPL start point.
+ */
+
+#include "storage_manager.h"
+#include "log.h"
+#include "mpl.h"
+#include "start_manager.h"
+#include "data_builder.h"
+#include "results_holder.h"
+#include "mlinclude.h"
+#include "message_layer.h"
+
+/**
+ * @brief  Initializes the MPL. Should be called first and once 
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_init_mpl(void)
+{
+    inv_init_storage_manager();
+
+    /* initialize the start callback manager */
+    INV_ERROR_CHECK(inv_init_start_manager());
+
+    /* initialize the data builder */
+    INV_ERROR_CHECK(inv_init_data_builder());
+
+    INV_ERROR_CHECK(inv_enable_results_holder());
+
+    // Get any left over messages and clear them. Throw message away as it is not
+    // initialized.
+    (void)inv_get_message_level_0(1);
+
+    return INV_SUCCESS;
+}
+
+const char ml_ver[] = "InvenSense MA 5.1.6 RC25";
+
+/**
+ *  @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/65xx/libsensors_iio/software/core/mllite/mpl.h b/65xx/libsensors_iio/software/core/mllite/mpl.h
new file mode 100755
index 0000000..a6b5ac7
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/mllite/results_holder.c b/65xx/libsensors_iio/software/core/mllite/results_holder.c
new file mode 100755
index 0000000..ec0a7f1
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/results_holder.c
@@ -0,0 +1,616 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+/**
+ *   @defgroup  Results_Holder results_holder
+ *   @brief     Motion Library - Results Holder
+ *              Holds the data for MPL
+ *
+ *   @{
+ *       @file results_holder.c
+ *       @brief Results Holder for HAL.
+ */
+
+#include <string.h>
+
+#include "results_holder.h"
+#include "ml_math_func.h"
+#include "mlmath.h"
+#include "start_manager.h"
+#include "data_builder.h"
+#include "message_layer.h"
+#include "log.h"
+
+// These 2 status bits are used to control when the 9 axis quaternion is updated
+#define INV_COMPASS_CORRECTION_SET 1
+#define INV_6_AXIS_QUAT_SET 2
+#define INV_GEOMAGNETIC_CORRECTION_SET 4
+
+struct results_t {
+    long nav_quat[4];
+    long gam_quat[4];
+    long geomag_quat[4];
+    long accel_quat[4];
+    inv_time_t nav_timestamp;
+    inv_time_t gam_timestamp;
+    inv_time_t geomag_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 */
+    long geomag_compass_correction[4]; /**< quaternion going from accel quaternion to geomag sensor fusion */
+    int acc_state; /**< Describes accel state */
+    int got_accel_bias; /**< Flag describing if accel bias is known */
+    long compass_bias_error[3]; /**< Error Squared */
+    unsigned char motion_state;
+    unsigned int motion_state_counter; /**< Incremented for each no motion event in a row */
+    long compass_count; /**< compass state internal counter */
+    int got_compass_bias; /**< Flag describing if compass bias is known */
+    int large_mag_field; /**< Flag describing if there is a large magnetic field */
+    int compass_state; /**< Internal compass state */
+    long status;
+    struct inv_sensor_cal_t *sensor;
+    float quat_confidence_interval;
+    float geo_mag_confidence_interval;
+};
+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
+* Store a quaternion computed from accelerometer correction. This quaternion is 
+* determined * using only accel, and used for geomagnetic fusion. 
+* @param[in] quat Length 4, Quaternion scaled by 2^30
+*/
+void inv_store_accel_quaternion(const long *quat, inv_time_t timestamp)
+{
+   // rh.status |= INV_6_AXIS_QUAT_SET;
+    memcpy(&rh.accel_quat, quat, sizeof(rh.accel_quat));
+    rh.geomag_timestamp = timestamp;
+}
+/** @internal
+* Sets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion.
+* @param[in] data Quaternion Adjustment
+* @param[in] timestamp Timestamp of when this is valid
+*/
+void inv_set_compass_correction(const long *data, inv_time_t timestamp)
+{
+    rh.status |= INV_COMPASS_CORRECTION_SET;
+    memcpy(rh.compass_correction, data, sizeof(rh.compass_correction));
+    rh.nav_timestamp = timestamp;
+}
+
+/** @internal
+* Sets the quaternion adjustment from 3 axis (accel) to 6 axis (with compass) quaternion.
+* @param[in] data Quaternion Adjustment
+* @param[in] timestamp Timestamp of when this is valid
+*/
+void inv_set_geomagnetic_compass_correction(const long *data, inv_time_t timestamp)
+{
+    rh.status |= INV_GEOMAGNETIC_CORRECTION_SET;
+    memcpy(rh.geomag_compass_correction, data, sizeof(rh.geomag_compass_correction));
+    rh.geomag_timestamp = timestamp;
+}
+
+/** @internal
+* Gets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion.
+* @param[out] data Quaternion Adjustment
+* @param[out] timestamp Timestamp of when this is valid
+*/
+void inv_get_compass_correction(long *data, inv_time_t *timestamp)
+{
+    memcpy(data, rh.compass_correction, sizeof(rh.compass_correction));
+    *timestamp = rh.nav_timestamp;
+}
+
+/** @internal
+* Gets the quaternion adjustment from 3 axis (accel) to 6 axis (with compass) quaternion.
+* @param[out] data Quaternion Adjustment
+* @param[out] timestamp Timestamp of when this is valid
+*/
+void inv_get_geomagnetic_compass_correction(long *data, inv_time_t *timestamp)
+{
+    memcpy(data, rh.geomag_compass_correction, sizeof(rh.geomag_compass_correction));
+    *timestamp = rh.geomag_timestamp;
+}
+
+/** Returns non-zero if there is a large magnetic field. See inv_set_large_mag_field() for setting this variable.
+ * @return Returns non-zero if there is a large magnetic field.
+ */
+int inv_get_large_mag_field()
+{
+    return rh.large_mag_field;
+}
+
+/** Set to non-zero if there as a large magnetic field. See inv_get_large_mag_field() for getting this variable.
+ * @param[in] state value to set for magnetic field strength. Should be non-zero if it is large.
+ */
+void inv_set_large_mag_field(int state)
+{
+    rh.large_mag_field = state;
+}
+
+/** Gets the accel state set by inv_set_acc_state()
+ * @return accel state.
+ */
+int inv_get_acc_state()
+{
+    return rh.acc_state;
+}
+
+/** Sets the accel state. See inv_get_acc_state() to get the value.
+ * @param[in] state value to set accel state to.
+ */
+void inv_set_acc_state(int state)
+{
+    rh.acc_state = state;
+    return;
+}
+
+/** Returns the motion state
+* @param[out] cntr Number of previous times a no motion event has occured in a row.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+int inv_get_motion_state(unsigned int *cntr)
+{
+    *cntr = rh.motion_state_counter;
+    return rh.motion_state;
+}
+
+/** Sets the motion state
+ * @param[in] state motion state where INV_NO_MOTION is not moving
+ *            and INV_MOTION is moving.
+ */
+void inv_set_motion_state(unsigned char state)
+{
+    long set;
+    if (state == rh.motion_state) {
+        if (state == INV_NO_MOTION) {
+            rh.motion_state_counter++;
+        } else {
+            rh.motion_state_counter = 0;
+        }
+        return;
+    }
+    rh.motion_state_counter = 0;
+    rh.motion_state = state;
+    /* Equivalent to set = state, but #define's may change. */
+    if (state == INV_MOTION)
+        set = INV_MSG_MOTION_EVENT;
+    else
+        set = INV_MSG_NO_MOTION_EVENT;
+    inv_set_message(set, (INV_MSG_MOTION_EVENT | INV_MSG_NO_MOTION_EVENT), 0);
+}
+
+/** Sets the 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 accel.
+ * @param[out] data 3-axis  accel quaternion scaled such that 1.0 = 2^30.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_accel_quaternion(long *data)
+{
+    memcpy(data, rh.accel_quat, sizeof(rh.accel_quat));
+    return INV_SUCCESS;
+}
+inv_error_t inv_get_gravity_6x(long *data)
+{
+    data[0] =
+        inv_q29_mult(rh.gam_quat[1], rh.gam_quat[3]) - inv_q29_mult(rh.gam_quat[2], rh.gam_quat[0]);
+    data[1] =
+        inv_q29_mult(rh.gam_quat[2], rh.gam_quat[3]) + inv_q29_mult(rh.gam_quat[1], rh.gam_quat[0]);
+    data[2] =
+        (inv_q29_mult(rh.gam_quat[3], rh.gam_quat[3]) + inv_q29_mult(rh.gam_quat[0], rh.gam_quat[0])) -
+        1073741824L;
+    return INV_SUCCESS;
+}
+/** Returns a quaternion based only on gyro and accel.
+ * @param[out] data 6-axis  gyro and accel quaternion scaled such that 1.0 = 2^30.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_6axis_quaternion(long *data)
+{
+    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 based only on compass and accel.
+ * @param[out] data 6-axis  compass and accel quaternion scaled such that 1.0 = 2^30.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_geomagnetic_quaternion(long *data, inv_time_t *timestamp)
+{
+   if (rh.status & INV_GEOMAGNETIC_CORRECTION_SET) {
+        inv_q_mult(rh.geomag_compass_correction, rh.accel_quat, rh.geomag_quat);
+        rh.status &= ~(INV_GEOMAGNETIC_CORRECTION_SET);
+    }
+    memcpy(data, rh.geomag_quat, sizeof(rh.geomag_quat));
+    *timestamp = rh.geomag_timestamp;
+    return INV_SUCCESS;
+}
+
+/** Returns a quaternion.
+ * @param[out] data 9-axis quaternion.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_quaternion_float(float *data)
+{
+    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.geomag_quat[0] = 1L<<30;
+    rh.accel_quat[0] = 1L<<30;
+    rh.geomag_compass_correction[0] = 1L<<30;
+    rh.quat_confidence_interval = (float)M_PI;
+    return INV_SUCCESS;
+}
+
+/** Turns on storage of results.
+*/
+inv_error_t inv_enable_results_holder()
+{
+    inv_error_t result;
+    result = inv_init_results_holder();
+    if ( result ) {
+        return result;
+    }
+
+    result = inv_register_mpl_start_notification(inv_start_results_holder);
+    return result;
+}
+
+/** Sets state of if we know the accel bias.
+ * @return return 1 if we know the accel bias, 0 if not.
+ *            it is set with inv_set_accel_bias_found()
+ */
+int inv_got_accel_bias()
+{
+    return rh.got_accel_bias;
+}
+
+/** Sets whether we know the accel bias
+ * @param[in] state Set to 1 if we know the accel bias. 
+ *            Can be retrieved with inv_got_accel_bias()
+ */
+void inv_set_accel_bias_found(int state)
+{
+    rh.got_accel_bias = state;
+}
+
+/** Sets state of if we know the compass bias.
+ * @return return 1 if we know the compass bias, 0 if not.
+ *            it is set with inv_set_compass_bias_found()
+ */
+int inv_got_compass_bias()
+{
+    return rh.got_compass_bias;
+}
+
+/** Sets whether we know the compass bias
+ * @param[in] state Set to 1 if we know the compass bias. 
+ *            Can be retrieved with inv_got_compass_bias()
+ */
+void inv_set_compass_bias_found(int state)
+{
+    rh.got_compass_bias = state;
+}
+
+/** Sets the compass state.
+ * @param[in] state Compass state. It can be retrieved with inv_get_compass_state().
+ */
+void inv_set_compass_state(int state)
+{
+    rh.compass_state = state;
+}
+
+/** Get's the compass state
+ * @return the compass state that was set with inv_set_compass_state()
+ */
+int inv_get_compass_state()
+{
+    return rh.compass_state;
+}
+
+/** Set compass bias error. See inv_get_compass_bias_error()
+ * @param[in] bias_error Set's how accurate we know the compass bias. It is the 
+ * error squared.
+ */
+void inv_set_compass_bias_error(const long *bias_error)
+{
+    memcpy(rh.compass_bias_error, bias_error, sizeof(rh.compass_bias_error));
+}
+
+/** Get's compass bias error. See inv_set_compass_bias_error() for setting.
+ * @param[out] bias_error Accuracy as to how well the compass bias is known. It is the error squared.
+ */
+void inv_get_compass_bias_error(long *bias_error)
+{
+    memcpy(bias_error, rh.compass_bias_error, sizeof(rh.compass_bias_error));
+}
+
+/**
+ *  @brief      Returns 3-element vector of accelerometer data in body frame
+ *                with gravity removed
+ *  @param[out] data    3-element vector of accelerometer data in body frame
+ *                with gravity removed
+ *  @return     INV_SUCCESS if successful
+ *              INV_ERROR_INVALID_PARAMETER if invalid input pointer
+ */
+inv_error_t inv_get_linear_accel(long *data)
+{
+    long gravity[3];
+
+    if (data != NULL)
+    {
+        inv_get_accel_set(data, NULL, NULL);
+        inv_get_gravity(gravity);
+        data[0] -= gravity[0] >> 14;
+        data[1] -= gravity[1] >> 14;
+        data[2] -= gravity[2] >> 14;
+        return INV_SUCCESS;
+    }
+    else {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+}
+
+/**
+ *  @brief      Returns 3-element vector of accelerometer data in body frame
+ *  @param[out] data    3-element vector of accelerometer data in body frame
+ *  @return     INV_SUCCESS if successful
+ *              INV_ERROR_INVALID_PARAMETER if invalid input pointer
+ */
+inv_error_t inv_get_accel(long *data)
+{
+    if (data != NULL) {
+        inv_get_accel_set(data, NULL, NULL);
+        return INV_SUCCESS;
+    }
+    else {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+}
+
+/**
+ *  @brief      Returns 3-element vector of accelerometer float data
+ *  @param[out] data    3-element vector of accelerometer float data
+ *  @return     INV_SUCCESS if successful
+ *              INV_ERROR_INVALID_PARAMETER if invalid input pointer
+ */
+inv_error_t inv_get_accel_float(float *data)
+{
+    long tdata[3];
+    unsigned char i;
+
+    if (data != NULL && !inv_get_accel(tdata)) {
+        for (i = 0; i < 3; ++i) {
+            data[i] = ((float)tdata[i] / (1L << 16));
+        }
+        return INV_SUCCESS;
+    }
+    else {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+}
+
+/**
+ *  @brief      Returns 3-element vector of gyro float data
+ *  @param[out] data    3-element vector of gyro float data
+ *  @return     INV_SUCCESS if successful
+ *              INV_ERROR_INVALID_PARAMETER if invalid input pointer
+ */
+inv_error_t inv_get_gyro_float(float *data)
+{
+    long tdata[3];
+    unsigned char i;
+
+    if (data != NULL) {
+        inv_get_gyro_set(tdata, NULL, NULL);
+        for (i = 0; i < 3; ++i) {
+            data[i] = ((float)tdata[i] / (1L << 16));
+        }
+        return INV_SUCCESS;
+    }
+    else {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+}
+
+/** Set 9 axis 95% heading confidence interval for quaternion
+* @param[in] ci Confidence interval in radians.
+*/
+void inv_set_heading_confidence_interval(float ci)
+{
+    rh.quat_confidence_interval = ci;
+}
+
+/** Get 9 axis 95% heading confidence interval for quaternion
+* @return Confidence interval in radians.
+*/
+float inv_get_heading_confidence_interval(void)
+{
+    return rh.quat_confidence_interval;
+}
+
+/** Set 6 axis (accel and compass) 95% heading confidence interval for quaternion
+* @param[in] ci Confidence interval in radians.
+*/
+void inv_set_accel_compass_confidence_interval(float ci)
+{
+    rh.geo_mag_confidence_interval = ci;
+}
+
+/** Get 6 axis (accel and compass) 95% heading confidence interval for quaternion
+* @return Confidence interval in radians.
+*/
+float inv_get_accel_compass_confidence_interval(void)
+{
+    return rh.geo_mag_confidence_interval;
+}
+
+/**
+ *  @brief      Returns 3-element vector of linear accel float data
+ *  @param[out] data    3-element vector of linear aceel float data
+ *  @return     INV_SUCCESS if successful
+ *              INV_ERROR_INVALID_PARAMETER if invalid input pointer
+ */
+inv_error_t inv_get_linear_accel_float(float *data)
+{
+    long tdata[3];
+    unsigned char i;
+
+    if (data != NULL && !inv_get_linear_accel(tdata)) {
+        for (i = 0; i < 3; ++i) {
+            data[i] = ((float)tdata[i] / (1L << 16));
+        }
+        return INV_SUCCESS;
+    }
+    else {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+}
+
+/**
+ * @}
+ */
diff --git a/65xx/libsensors_iio/software/core/mllite/results_holder.h b/65xx/libsensors_iio/software/core/mllite/results_holder.h
new file mode 100755
index 0000000..600c666
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/results_holder.h
@@ -0,0 +1,90 @@
+/*
+ $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);
+void inv_store_accel_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_gravity_6x(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_get_accel_quaternion(long *data);
+inv_error_t inv_get_geomagnetic_quaternion(long *data, inv_time_t *timestamp);
+void inv_set_geomagnetic_compass_correction(const long *data, inv_time_t timestamp);
+void inv_get_geomagnetic_compass_correction(long *data, inv_time_t *timestamp);
+
+inv_error_t inv_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);
+
+void inv_set_accel_compass_confidence_interval(float ci);
+float inv_get_accel_compass_confidence_interval(void);
+
+int inv_got_accel_bias();
+void inv_set_accel_bias_found(int state);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_RESULTS_HOLDER_H__
diff --git a/65xx/libsensors_iio/software/core/mllite/start_manager.c b/65xx/libsensors_iio/software/core/mllite/start_manager.c
new file mode 100755
index 0000000..fb758e7
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/mllite/start_manager.h b/65xx/libsensors_iio/software/core/mllite/start_manager.h
new file mode 100755
index 0000000..899e3f5
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/mllite/storage_manager.c b/65xx/libsensors_iio/software/core/mllite/storage_manager.c
new file mode 100755
index 0000000..419b9d0
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mllite/storage_manager.c
@@ -0,0 +1,210 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/**
+ *   @defgroup  Storage_Manager storage_manager
+ *   @brief     Motion Library - Stores Data for functions.
+ *
+ *
+ *   @{
+ *       @file storage_manager.c
+ *       @brief Load and Store Manager.
+ */
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MLLITE"
+
+#include <string.h>
+
+#include "storage_manager.h"
+#include "log.h"
+#include "ml_math_func.h"
+#include "mlmath.h"
+
+/* Must be changed if the format of storage changes */
+#define DEFAULT_KEY 29681
+
+typedef inv_error_t (*load_func_t)(const unsigned char *data);
+typedef inv_error_t (*save_func_t)(unsigned char *data);
+/** Max number of entites that can be stored */
+#define NUM_STORAGE_BOXES 20
+
+struct data_header_t {
+    long size;
+    uint32_t checksum;
+    unsigned int key;
+};
+
+struct data_storage_t {
+    int num; /**< Number of differnt save entities */
+    size_t total_size; /**< Size in bytes to store non volatile data */
+    load_func_t load[NUM_STORAGE_BOXES]; /**< Callback to load data */
+    save_func_t save[NUM_STORAGE_BOXES]; /**< Callback to save data */
+    struct data_header_t hd[NUM_STORAGE_BOXES]; /**< Header info for each entity */
+};
+static struct data_storage_t ds;
+
+/** Should be called once before using any of the storage methods. Typically
+* called first by inv_init_mpl().*/
+void inv_init_storage_manager()
+{
+    memset(&ds, 0, sizeof(ds));
+    ds.total_size = sizeof(struct data_header_t);
+}
+
+/** Used to register your mechanism to load and store non-volative data. This should typical be
+* called during the enable function for your feature.
+* @param[in] load_func function pointer you will use to receive data that was stored for you.
+* @param[in] save_func function pointer you will use to save any data you want saved to
+*            non-volatile memory between runs.
+* @param[in] size The size in bytes of the amount of data you want loaded and saved.
+* @param[in] key The key associated with your data type should be unique across MPL.
+*                    The key should change when your type of data for storage changes.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_register_load_store(inv_error_t (*load_func)(const unsigned char *data),
+                                    inv_error_t (*save_func)(unsigned char *data), size_t size, unsigned int key)
+{
+    int kk;
+    // Check if this has been registered already
+    for (kk=0; kk<ds.num; ++kk) {
+        if (key == ds.hd[kk].key) {
+            return INV_ERROR_INVALID_PARAMETER;
+        }
+    }
+    // Make sure there is room
+    if (ds.num >= NUM_STORAGE_BOXES) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+    // Add to list
+    ds.hd[ds.num].key = key;
+    ds.hd[ds.num].size = size;
+    ds.load[ds.num] = load_func;
+    ds.save[ds.num] = save_func;
+    ds.total_size += size + sizeof(struct data_header_t);
+    ds.num++;
+
+    return INV_SUCCESS;
+}
+
+/** Returns the memory size needed to perform a store
+* @param[out] size Size in bytes of memory needed to store.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_get_mpl_state_size(size_t *size)
+{
+    *size = ds.total_size;
+    return INV_SUCCESS;
+}
+
+/** @internal
+ * Finds key in ds.hd[] array and returns location
+ * @return location where key exists in array, -1 if not found.
+ */
+static int inv_find_entry(unsigned int key)
+{
+    int kk;
+    for (kk=0; kk<ds.num; ++kk) {
+        if (key == ds.hd[kk].key) {
+            return kk;
+        }
+    }
+    return -1;
+}
+
+/** This function takes a block of data that has been saved in non-volatile memory and pushes
+* to the proper locations. Multiple error checks are performed on the data.
+* @param[in] data Data that was saved to be loaded up by MPL
+* @param[in] length Length of data vector in bytes
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_load_mpl_states(const unsigned char *data, size_t length)
+{
+    struct data_header_t *hd;
+    int entry;
+    uint32_t checksum;
+    long len;
+
+    len = length; // Important so we get negative numbers
+    if (data == NULL || len == 0)
+        return INV_SUCCESS;
+    if (len < sizeof(struct data_header_t))
+        return INV_ERROR_CALIBRATION_LOAD;  // No data
+    hd = (struct data_header_t *)data;
+    if (hd->key != DEFAULT_KEY)
+        return INV_ERROR_CALIBRATION_LOAD;  // Key changed or data corruption
+    len = MIN(hd->size, len);
+    len = hd->size;
+    len -= sizeof(struct data_header_t);
+    data += sizeof(struct data_header_t);
+    checksum = inv_checksum(data, len);
+    if (checksum != hd->checksum)
+        return INV_ERROR_CALIBRATION_LOAD;  // Data corruption
+
+    while (len > (long)sizeof(struct data_header_t)) {
+        hd = (struct data_header_t *)data;
+        entry = inv_find_entry(hd->key);
+        data += sizeof(struct data_header_t);
+        len -= sizeof(struct data_header_t);
+        if (entry >= 0 && len >= hd->size) {
+            if (hd->size != ds.hd[entry].size)
+                return INV_ERROR_CALIBRATION_LEN;
+            checksum = inv_checksum(data, hd->size);
+            if (checksum != hd->checksum)
+                return INV_ERROR_CALIBRATION_LOAD;
+            ds.load[entry](data);
+        }
+        len -= hd->size;
+        if (len >= 0)
+            data = data + hd->size;
+    }
+
+    return INV_SUCCESS;
+}
+
+/** This function fills up a block of memory to be stored in non-volatile memory.
+* @param[out] data Place to store data, size of sz, must be at least size
+*                  returned by inv_get_mpl_state_size()
+* @param[in] sz Size of data.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_save_mpl_states(unsigned char *data, size_t sz)
+{
+    unsigned char *cur;
+    int kk;
+    struct data_header_t *hd;
+
+    if (data == NULL || sz == 0)
+        return INV_ERROR_CALIBRATION_LOAD;
+    if (sz >= ds.total_size) {
+        cur = data + sizeof(struct data_header_t);
+        for (kk = 0; kk < ds.num; ++kk) {
+            hd = (struct data_header_t *)cur;
+            cur += sizeof(struct data_header_t);
+            ds.save[kk](cur);
+            hd->checksum = inv_checksum(cur, ds.hd[kk].size);
+            hd->size = ds.hd[kk].size;
+            hd->key = ds.hd[kk].key;
+            cur += ds.hd[kk].size;
+        }
+    } else {
+        return INV_ERROR_CALIBRATION_LOAD;
+    }
+
+    hd = (struct data_header_t *)data;
+    hd->checksum = inv_checksum(data + sizeof(struct data_header_t),
+                                ds.total_size - sizeof(struct data_header_t));
+    hd->key = DEFAULT_KEY;
+    hd->size = ds.total_size;
+
+    return INV_SUCCESS;
+}
+
+/**
+ * @}
+ */
diff --git a/65xx/libsensors_iio/software/core/mllite/storage_manager.h b/65xx/libsensors_iio/software/core/mllite/storage_manager.h
new file mode 100755
index 0000000..7255591
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/mpl/accel_auto_cal.h b/65xx/libsensors_iio/software/core/mpl/accel_auto_cal.h
new file mode 100755
index 0000000..5a53213
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/mpl/authenticate.h b/65xx/libsensors_iio/software/core/mpl/authenticate.h
new file mode 100755
index 0000000..d7c803b
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so b/65xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
new file mode 100755
index 0000000..5d9ac0e
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
diff --git a/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk b/65xx/libsensors_iio/software/core/mpl/build/android/shared.mk
index ed5fbf6..1d6f904 100644..100755
--- a/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk
+++ b/65xx/libsensors_iio/software/core/mpl/build/android/shared.mk
@@ -1,19 +1,18 @@
-EXEC = inv_self_test$(SHARED_APP_SUFFIX)
+MPL_LIB_NAME ?= mplmpu
+LIBRARY = $(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT)
 
 MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
 
 CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
 COMP  ?= $(CROSS)gcc
 LINK  ?= $(CROSS)gcc
+STRIP ?= $(CROSS)strip -g
 
 OBJFOLDER = $(CURDIR)/obj
 
-INV_ROOT   = ../../../../..
-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
+INV_ROOT = ../../../../..
+MLLITE_DIR  = $(INV_ROOT)/software/core/mllite
+MPL_DIR  = $(INV_ROOT)/software/core/mpl
 
 include $(INV_ROOT)/software/build/android/common.mk
 
@@ -33,35 +32,29 @@ 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  = -lc 
+LLINK += -lm 
+LLINK += -lutils 
+LLINK += -lcutils 
+LLINK += -lgcc 
 LLINK += -ldl
-LLINK += -lstdc++
-LLINK += -llog
-LLINK += -lz
 
 LFLAGS += $(CMDLINE_LFLAGS)
-LFLAGS += $(ANDROID_LINK_EXECUTABLE)
-
-LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+LFLAGS += -shared 
+LFLAGS += -Wl,-soname,$(LIBRARY)
+LFLAGS += -Wl,-shared,-Bsymbolic 
+LFLAGS += $(ANDROID_LINK)
+LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
 
 ####################################################################################################
 ## sources
 
-INV_LIBS  = $(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_LIBS = $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
 
-#INV_SOURCES and VPATH provided by Makefile.filelist
+#INV_SOURCES, VPATH provided by Makefile.filelist
 include ../filelist.mk
 
 INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
@@ -70,13 +63,16 @@ INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES)
 ####################################################################################################
 ## rules
 
-.PHONY: all clean cleanall install
+.PHONY: all mpl clean cleanall
+
+all: mpl
 
-all: $(EXEC) $(MK_NAME)
+mpl: $(LIBRARY) $(MK_NAME)
 
-$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
-	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
-	$(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+$(LIBRARY) : $(OBJFOLDER) $(INV_OBJS_DST) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(INV_OBJS_DST)\n")
+	$(LINK) $(LFLAGS) -o $(LIBRARY) $(INV_OBJS_DST) $(LLINK) $(INV_LIBS) $(LLINK)
+	$(STRIP) -g $(LIBRARY)
 
 $(OBJFOLDER) :
 	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
@@ -84,15 +80,11 @@ $(OBJFOLDER) :
 
 $(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 $<
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(CFLAGS) -o $@ -c $<
 
 clean : 
 	rm -fR $(OBJFOLDER)
 
 cleanall : 
-	rm -fR $(EXEC) $(OBJFOLDER)
-
-install : $(EXEC)
-	cp -f $(EXEC) $(INSTALL_DIR)
-
+	rm -fR $(LIBRARY) $(OBJFOLDER)
 
diff --git a/65xx/libsensors_iio/software/core/mpl/build/filelist.mk b/65xx/libsensors_iio/software/core/mpl/build/filelist.mk
new file mode 100755
index 0000000..34ae4d1
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mpl/build/filelist.mk
@@ -0,0 +1,41 @@
+#### filelist.mk for mpl ####
+
+# headers for sources
+HEADERS := $(MPL_DIR)/fast_no_motion.h
+HEADERS += $(MPL_DIR)/fusion_9axis.h
+HEADERS += $(MPL_DIR)/motion_no_motion.h
+HEADERS += $(MPL_DIR)/no_gyro_fusion.h
+HEADERS += $(MPL_DIR)/quaternion_supervisor.h
+HEADERS += $(MPL_DIR)/gyro_tc.h
+HEADERS += $(MPL_DIR)/authenticate.h
+HEADERS += $(MPL_DIR)/accel_auto_cal.h
+HEADERS += $(MPL_DIR)/accel_auto_cal_protected.h
+HEADERS += $(MPL_DIR)/compass_vec_cal.h
+HEADERS += $(MPL_DIR)/compass_vec_cal_protected.h
+HEADERS += $(MPL_DIR)/mag_disturb.h
+HEADERS += $(MPL_DIR)/mag_disturb_protected.h
+HEADERS += $(MPL_DIR)/compass_bias_w_gyro.h
+HEADERS += $(MPL_DIR)/heading_from_gyro.h
+HEADERS += $(MPL_DIR)/compass_fit.h
+HEADERS += $(MPL_DIR)/quat_accuracy_monitor.h
+
+# sources
+SOURCES := $(MPL_DIR)/fast_no_motion.c
+SOURCES += $(MPL_DIR)/fusion_9axis.c
+SOURCES += $(MPL_DIR)/motion_no_motion.c
+SOURCES += $(MPL_DIR)/no_gyro_fusion.c
+SOURCES += $(MPL_DIR)/quaternion_supervisor.c
+SOURCES += $(MPL_DIR)/gyro_tc.c
+SOURCES += $(MPL_DIR)/authenticate.c
+SOURCES += $(MPL_DIR)/accel_auto_cal.c
+SOURCES += $(MPL_DIR)/compass_vec_cal.c
+SOURCES += $(MPL_DIR)/mag_disturb.c
+SOURCES += $(MPL_DIR)/compass_bias_w_gyro.c
+SOURCES += $(MPL_DIR)/heading_from_gyro.c
+SOURCES += $(MPL_DIR)/compass_fit.c
+SOURCES += $(MPL_DIR)/quat_accuracy_monitor.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH = $(MPL_DIR)
+
diff --git a/65xx/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h b/65xx/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h
new file mode 100755
index 0000000..4f01fc2
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/mpl/compass_fit.h b/65xx/libsensors_iio/software/core/mpl/compass_fit.h
new file mode 100755
index 0000000..be3cce8
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/mpl/compass_vec_cal.h b/65xx/libsensors_iio/software/core/mpl/compass_vec_cal.h
new file mode 100755
index 0000000..003a312
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mpl/compass_vec_cal.h
@@ -0,0 +1,36 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+#ifndef COMPASS_ONLY_CAL_H__
+#define COMPASS_ONLY_CAL_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_enable_vector_compass_cal();
+inv_error_t inv_disable_vector_compass_cal();
+inv_error_t inv_start_vector_compass_cal(void);
+inv_error_t inv_stop_vector_compass_cal(void);
+void inv_vector_compass_cal_sensitivity(float sens);
+inv_error_t inv_init_vector_compass_cal();
+
+inv_error_t inv_mcb_switch(int i);
+#ifdef __cplusplus
+}
+#endif
+
+#endif // COMPASS_ONLY_CAL_H__
+
diff --git a/65xx/libsensors_iio/software/core/mpl/fast_no_motion.h b/65xx/libsensors_iio/software/core/mpl/fast_no_motion.h
new file mode 100755
index 0000000..a6c1ed8
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mpl/fast_no_motion.h
@@ -0,0 +1,52 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */

+

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

+ *

+ * $Id$

+ *

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

+

+#ifndef MLDMP_FAST_NO_MOTION_H__

+#define MLDMP_FAST_NO_MOTION_H__

+

+#include "mltypes.h"

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+

+    inv_error_t inv_enable_fast_nomot(void);

+    inv_error_t inv_disable_fast_nomot(void);

+    inv_error_t inv_start_fast_nomot(void);

+    inv_error_t inv_stop_fast_nomot(void);

+    inv_error_t inv_init_fast_nomot(void);

+    void inv_set_default_number_of_samples(int count);

+    inv_error_t inv_fast_nomot_is_enabled(unsigned char *is_enabled);

+    inv_error_t inv_update_fast_nomot(long *gyro);

+

+    void inv_get_fast_nomot_accel_param(long *cntr, long long *param);

+    void inv_get_fast_nomot_compass_param(long *cntr, long long *param);

+    void inv_set_fast_nomot_accel_threshold(long long thresh);

+    void inv_set_fast_nomot_compass_threshold(long long thresh);

+    void int_set_fast_nomot_gyro_threshold(long long thresh);

+

+    inv_time_t fast_nomot_get_gyro_bias_update_time(void);

+    void fast_nomot_set_gyro_bias_update_time(struct inv_sensor_cal_t *sensor_cal);

+

+    int fast_nomot_get_gyro_calibration_confidence_level(struct inv_sensor_cal_t *sensor_cal);

+    void fast_nomot_set_gyro_calibration_confidence_level_time_threshold(float time_seconds);

+

+    void inv_fnm_debug_print(void);

+

+#ifdef __cplusplus

+}

+#endif

+

+

+#endif // MLDMP_FAST_NO_MOTION_H__

+

diff --git a/65xx/libsensors_iio/software/core/mpl/fusion_9axis.h b/65xx/libsensors_iio/software/core/mpl/fusion_9axis.h
new file mode 100755
index 0000000..1f429f1
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mpl/fusion_9axis.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_FUSION9AXIS_H__
+#define MLDMP_FUSION9AXIS_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    void inv_init_9x_fusion(void);
+    inv_error_t inv_9x_fusion_state_change(unsigned char newState);
+    inv_error_t inv_enable_9x_sensor_fusion(void);
+    inv_error_t inv_disable_9x_sensor_fusion(void);
+    inv_error_t inv_start_9x_sensor_fusion(void);
+    inv_error_t inv_stop_9x_sensor_fusion(void);
+    inv_error_t inv_9x_fusion_set_mag_fb(double fb);

+    inv_error_t inv_9x_fusion_enable_jitter_reduction(int en);

+
+    float inv_9x_sensor_fusion_get_correction_angle(void);

+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_FUSION9AXIS_H__
diff --git a/65xx/libsensors_iio/software/core/mpl/gyro_tc.h b/65xx/libsensors_iio/software/core/mpl/gyro_tc.h
new file mode 100755
index 0000000..8f1d50e
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/mpl/heading_from_gyro.h b/65xx/libsensors_iio/software/core/mpl/heading_from_gyro.h
new file mode 100755
index 0000000..09ecc42
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/mpl/inv_math.h b/65xx/libsensors_iio/software/core/mpl/inv_math.h
new file mode 100755
index 0000000..6620bbf
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/mpl/invensense_adv.h b/65xx/libsensors_iio/software/core/mpl/invensense_adv.h
new file mode 100755
index 0000000..12932c9
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mpl/invensense_adv.h
@@ -0,0 +1,31 @@
+/*
+ $License:
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+/*
+    Main header file for Invensense's Advanced library.
+*/
+
+#include "accel_auto_cal.h"
+#include "compass_bias_w_gyro.h"
+#include "compass_fit.h"
+#include "compass_vec_cal.h"
+#include "fast_no_motion.h"
+#include "fusion_9axis.h"
+#include "gyro_tc.h"
+#include "heading_from_gyro.h"
+#include "mag_disturb.h"
+#include "motion_no_motion.h"
+#include "no_gyro_fusion.h"
+#include "quaternion_supervisor.h"
+#include "mag_disturb.h"
+#include "quat_accuracy_monitor.h"
+#include "shake.h"
diff --git a/65xx/libsensors_iio/software/core/mpl/mag_disturb.h b/65xx/libsensors_iio/software/core/mpl/mag_disturb.h
new file mode 100755
index 0000000..bace502
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mpl/mag_disturb.h
@@ -0,0 +1,93 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */

+

+#ifndef MLDMP_MAGDISTURB_H__

+#define MLDMP_MAGDISTURB_H__

+

+#include "mltypes.h"

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+

+   // #define WIN_8

+

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

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

+

+    void inv_track_world_yaw_angle_angle(int mode, float currdip);

+

+    inv_error_t inv_enable_magnetic_disturbance(void);

+    inv_error_t inv_disable_magnetic_disturbance(void);

+    int inv_get_magnetic_disturbance_state();

+

+    inv_error_t inv_set_magnetic_disturbance(int time_ms);

+    inv_error_t inv_init_magnetic_disturbance(void);

+

+    void inv_enable_magnetic_disturbance_logging(void);

+    void inv_disable_magnetic_disturbance_logging(void);

+

+    float Mag3ofNormalizedLong(const long *x);

+    float Mag2ofNormalizedLong(const long *x);

+    float Mag2ofNormalizedFloat(const float *x);

+    

+    int inv_mag_disturb_get_detect_status_3D(void);

+    void inv_mag_disturb_set_detect_status_3D(int status);

+

+    int inv_mag_disturb_get_drop_heading_accuracy_status(void);

+    void inv_mag_disturb_set_drop_heading_accuracy_status(int status);

+

+    int inv_mag_disturb_get_detect_weak_status_3D(void);

+    void inv_mag_disturb_set_detect_weak_status_3D(int status);

+

+    int inv_mag_disturb_get_detect_world_yaw_angle_status(void);

+    void inv_mag_disturb_set_detect_world_yaw_angle_status(int status);

+

+    int inv_mag_disturb_get_detect_world_yaw_angle_confirm_status(void);

+    void inv_mag_disturb_set_detect_world_yaw_angle_confirm_status(int status);

+

+    float inv_mag_disturb_get_vector_radius_3D(void);

+    void inv_mag_disturb_set_vector_radius_3D(float radius);

+

+    void inv_mag_disturb_world_yaw_angle_init(void);

+    void inv_mag_disturb_world_yaw_angle_process(struct inv_sensor_cal_t *obj);

+    //enum mag_distub_state_e inv_mag_disturb_get_mar_world_yaw_angle_state(void);

+    

+    char inv_mag_disturb_get_mar_world_yaw_angle_detection_status(void);

+

+    float inv_mag_disturb_world_yaw_angle_distortion_from_gyro_bias(struct inv_sensor_cal_t *obj);

+    int inv_mag_disturb_get_dip_compassNgravity(struct inv_sensor_cal_t *data);

+

+    float inv_mag_disturb_9x_quat_confidence_interval(struct inv_sensor_cal_t *obj);

+    float inv_mag_disturb_geo_mag_confidence_interval(struct inv_sensor_cal_t *obj);

+

+    float inv_mag_disturb_world_yaw_angle_distortion_from_accel_compass_bias(float accel_bias_error, float compass_bias_error);

+    float inv_mag_disturb_world_yaw_angle_distortion_from_accel_compass_only(float accel_bias_error, float compass_bias_error);

+

+    void inv_mag_disturb_all_confidence_interval_init(void);

+

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

+    /* external API         */

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

+    float inv_mag_disturb_get_magnitude_threshold(void);

+    void inv_mag_disturb_set_magnitude_threshold(float radius);

+

+    float inv_mag_disturb_get_time_threshold_detect(void);

+    void inv_mag_disturb_set_time_threshold_detect(float time_seconds);

+

+    float inv_mag_disturb_get_local_field_radius(void);

+    void inv_mag_disturb_set_local_field_radius(float radius);

+

+    float inv_mag_disturb_get_local_field_dip(void);

+    void inv_mag_disturb_set_local_field_dip(float dip);

+

+#ifdef __cplusplus

+}

+#endif

+

+

+#endif // MLDMP_MAGDISTURB_H__

diff --git a/65xx/libsensors_iio/software/core/mpl/motion_no_motion.h b/65xx/libsensors_iio/software/core/mpl/motion_no_motion.h
new file mode 100755
index 0000000..6868acf
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mpl/motion_no_motion.h
@@ -0,0 +1,31 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */

+#ifndef INV_MOTION_NO_MOTION_H__

+#define INV_MOTION_NO_MOTION_H__

+

+#include "mltypes.h"

+#include "invensense.h"

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+

+inv_error_t inv_enable_motion_no_motion(void);

+inv_error_t inv_disable_motion_no_motion(void);

+inv_error_t inv_init_motion_no_motion(void);

+inv_error_t inv_start_motion_no_motion(void);

+inv_error_t inv_stop_motion_no_motion(void);

+

+inv_error_t inv_set_no_motion_time(long time_ms);

+inv_time_t motion_nomot_get_gyro_bias_update_time(void);

+void motion_nomot_set_gyro_bias_update_time(struct inv_sensor_cal_t *sensor_cal);

+

+#ifdef __cplusplus

+}

+#endif

+

+#endif // INV_MOTION_NO_MOTION_H__

diff --git a/65xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h b/65xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h
new file mode 100755
index 0000000..38d5690
--- /dev/null
+++ b/65xx/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/65xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h b/65xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
new file mode 100755
index 0000000..3c6a2c1
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
@@ -0,0 +1,71 @@
+/*

+	quat_accuracy_monitor.h

+ $License:

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

+    See included License.txt for License information.

+ $

+ */

+

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

+ *

+ * $Id:$

+ *

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

+

+#ifndef QUAT_ACCURARCY_MONITOR_H__

+#define QUAT_ACCURARCY_MONITOR_H__

+

+#include "mltypes.h"

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+enum accuracy_signal_type_e {

+    TYPE_NAV_QUAT,

+    TYPE_GAM_QUAT,

+    TYPE_NAV_QUAT_ADVANCED,

+    TYPE_GAM_QUAT_ADVANCED,

+    TYPE_NAV_QUAT_BASIC,

+    TYPE_GAM_QUAT_BASIC,

+    TYPE_MAG,

+    TYPE_GYRO,

+    TYPE_ACCEL,

+};

+

+inv_error_t inv_init_quat_accuracy_monitor(void);

+

+void set_accuracy_threshold(enum accuracy_signal_type_e type, double threshold);

+double get_accuracy_threshold(enum accuracy_signal_type_e type);

+void set_accuracy_weight(enum accuracy_signal_type_e type, int weight);

+int get_accuracy_weight(enum accuracy_signal_type_e type);

+

+int8_t get_accuracy_accuracy(enum accuracy_signal_type_e type);

+

+void inv_reset_quat_accuracy(void);

+double get_6axis_correction_term(void);

+double get_9axis_correction_term(void);

+int get_9axis_accuracy_state();

+

+void set_6axis_error_average(double value);

+double get_6axis_error_bound(void);

+double get_compass_correction(void);

+double get_9axis_error_bound(void);

+

+float get_confidence_interval(void);

+void set_compass_uncertainty(float value);

+

+inv_error_t inv_enable_quat_accuracy_monitor(void);

+inv_error_t inv_disable_quat_accuracy_monitor(void);

+inv_error_t inv_start_quat_accuracy_monitor(void);

+inv_error_t inv_stop_quat_accuracy_monitor(void);

+

+double get_compassNgravity(void);

+double get_init_compassNgravity(void);

+

+float inv_heading_accuracy_check(float orient[3], float *heading, int8_t *accuracy);

+

+#ifdef __cplusplus

+}

+#endif

+

+#endif // QUAT_ACCURARCY_MONITOR_H__

diff --git a/65xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h b/65xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h
new file mode 100755
index 0000000..4e5881c
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h
@@ -0,0 +1,29 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */

+#ifndef INV_QUATERNION_SUPERVISOR_H__

+#define INV_QUATERNION_SUPERVISOR_H__

+

+#include "mltypes.h"

+

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+

+#define ACCELERATION_SQUARE_1P5G   9663676416LL

+#define ACCELERATION_SQUARE_1P2G   6184752906LL

+inv_error_t inv_enable_quaternion(void);

+inv_error_t inv_disable_quaternion(void);

+inv_error_t inv_init_quaternion(void);

+inv_error_t inv_start_quaternion(void);

+void inv_set_quaternion(long *quat);
+

+#ifdef __cplusplus

+}

+#endif

+

+#endif // INV_QUATERNION_SUPERVISOR_H__

diff --git a/65xx/libsensors_iio/software/core/mpl/shake.h b/65xx/libsensors_iio/software/core/mpl/shake.h
new file mode 100755
index 0000000..67acb7b
--- /dev/null
+++ b/65xx/libsensors_iio/software/core/mpl/shake.h
@@ -0,0 +1,94 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */

+#ifndef INV_SHAKE_H__

+#define INV_SHAKE_H__

+

+#include "mltypes.h"

+

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+

+	/* ------------ */

+    /* - Defines. - */

+    /* ------------ */

+

+    #define STATE_ZERO                             0

+    #define STATE_INIT_1                           1

+    #define STATE_INIT_2                           2

+    #define STATE_DETECT                           3

+

+	struct t_shake_config_params {

+		long shake_time_min_ms;

+		long shake_time_max_ms;

+		long shake_time_min;

+        long shake_time_max;

+		unsigned char shake_time_set;

+		long shake_time_saved;

+        float shake_deriv_thr;

+        int zero_cross_thr;

+        float accel_delta_min;

+        float accel_delta_max;

+		unsigned char interp_enable;

+	};

+

+	struct t_shake_state_params {

+		unsigned char state;

+        float accel_peak_high;

+        float accel_peak_low;

+        float accel_range;

+        int num_zero_cross;

+        short curr_shake_time;

+		int deriv_major_change;

+		int deriv_major_sign;

+        float accel_buffer[200];

+        float delta_buffer[200];

+	};

+

+	struct t_shake_data_params {

+		float accel_prev;

+		float accel_curr;

+		float delta_prev;

+		float delta_curr;

+		float delta_prev_buffer;

+	};

+

+	struct t_shake_results {

+		//unsigned char shake_int;

+		int shake_number;

+	};

+

+	struct t_shake_cb {

+       void (*shake_callback)(struct t_shake_results *shake_results);            

+    };

+

+

+    /* --------------------- */

+    /* - Function p-types. - */

+    /* --------------------- */

+	inv_error_t inv_enable_shake(void);

+    inv_error_t inv_disable_shake(void);

+    inv_error_t inv_init_shake(void);

+    inv_error_t inv_start_shake(void);

+	int inv_set_shake_cb(void (*callback)(struct t_shake_results *shake_results));

+	void inv_config_shake_time_params(long sample_time_ms);

+	void inv_set_shake_accel_delta_min(float accel_g);

+	void inv_set_shake_accel_delta_max(float accel_g);

+	void inv_set_shake_zero_cross_thresh(int num_zero_cross);

+	void inv_set_shake_deriv_thresh(float shake_deriv_thresh);

+	void inv_set_shake_time_min_ms(long time_ms);

+	void inv_set_shake_time_max_ms(long time_ms);

+	void inv_enable_shake_data_interpolation(unsigned char en);

+

+

+

+#ifdef __cplusplus

+}

+#endif

+

+#endif // INV_SHAKE__
\ No newline at end of file
diff --git a/Android.mk b/Android.mk
index 427bbb4..8c8e86d 100644
--- a/Android.mk
+++ b/Android.mk
@@ -1,8 +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)
+# Can't have both 65xx and 60xx sensors.
+ifneq ($(filter hammerhead, $(TARGET_DEVICE)),)
+# hammerhead expects 65xx sensors.
+include $(call all-named-subdir-makefiles,65xx)
 else
-# libsensors expects an non-IIO MPU3050.
-include $(call all-named-subdir-makefiles,mlsdk libsensors)
+# manta expects 60xx sensors.
+include $(call all-named-subdir-makefiles,60xx)
 endif
diff --git a/libsensors_iio/software/core/driver/include/linux/mpu.h b/libsensors_iio/software/core/driver/include/linux/mpu.h
deleted file mode 100644
index 4391226..0000000
--- a/libsensors_iio/software/core/driver/include/linux/mpu.h
+++ /dev/null
@@ -1,108 +0,0 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* This software is licensed under the terms of the GNU General Public
-* License version 2, as published by the Free Software Foundation, and
-* may be copied, distributed, and modified under those terms.
-*
-* This program is distributed in the hope that it will be useful,
-* but WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-* GNU General Public License for more details.
-*
-*/
-
-/**
- *  @addtogroup  DRIVERS
- *  @brief       Hardware drivers.
- *
- *  @{
- *      @file    mpu.h
- *      @brief   mpu definition
- */
-
-#ifndef __MPU_H_
-#define __MPU_H_
-
-#ifdef __KERNEL__
-#include <linux/types.h>
-#include <linux/ioctl.h>
-#endif
-
-enum secondary_slave_type {
-	SECONDARY_SLAVE_TYPE_NONE,
-	SECONDARY_SLAVE_TYPE_ACCEL,
-	SECONDARY_SLAVE_TYPE_COMPASS,
-	SECONDARY_SLAVE_TYPE_PRESSURE,
-
-	SECONDARY_SLAVE_TYPE_TYPES
-};
-
-enum ext_slave_id {
-	ID_INVALID = 0,
-	GYRO_ID_MPU3050,
-	GYRO_ID_MPU6050A2,
-	GYRO_ID_MPU6050B1,
-	GYRO_ID_MPU6050B1_NO_ACCEL,
-	GYRO_ID_ITG3500,
-
-	ACCEL_ID_LIS331,
-	ACCEL_ID_LSM303DLX,
-	ACCEL_ID_LIS3DH,
-	ACCEL_ID_KXSD9,
-	ACCEL_ID_KXTF9,
-	ACCEL_ID_BMA150,
-	ACCEL_ID_BMA222,
-	ACCEL_ID_BMA250,
-	ACCEL_ID_ADXL34X,
-	ACCEL_ID_MMA8450,
-	ACCEL_ID_MMA845X,
-	ACCEL_ID_MPU6050,
-
-	COMPASS_ID_AK8963,
-	COMPASS_ID_AK8975,
-	COMPASS_ID_AK8972,
-	COMPASS_ID_AMI30X,
-	COMPASS_ID_AMI306,
-	COMPASS_ID_YAS529,
-	COMPASS_ID_YAS530,
-	COMPASS_ID_HMC5883,
-	COMPASS_ID_LSM303DLH,
-	COMPASS_ID_LSM303DLM,
-	COMPASS_ID_MMC314X,
-	COMPASS_ID_HSCDTD002B,
-	COMPASS_ID_HSCDTD004A,
-
-	PRESSURE_ID_BMA085,
-};
-
-#define INV_PROD_KEY(ver, rev) (ver * 100 + rev)
-/**
- * struct mpu_platform_data - Platform data for the mpu driver
- * @int_config:		Bits [7:3] of the int config register.
- * @level_shifter:	0: VLogic, 1: VDD
- * @orientation:	Orientation matrix of the gyroscope
- * @sec_slave_type:     secondary slave device type, can be compass, accel, etc
- * @sec_slave_id:       id of the secondary slave device
- * @secondary_i2c_address: secondary device's i2c address
- * @secondary_orientation: secondary device's orientation matrix
- * @key:                key for MPL library.
- *
- * Contains platform specific information on how to configure the MPU3050 to
- * work on this platform.  The orientation matricies are 3x3 rotation matricies
- * that are applied to the data to rotate from the mounting orientation to the
- * platform orientation.  The values must be one of 0, 1, or -1 and each row and
- * column should have exactly 1 non-zero value.
- */
-struct mpu_platform_data {
-	__u8 int_config;
-	__u8 level_shifter;
-	__s8 orientation[9];
-	enum secondary_slave_type sec_slave_type;
-	enum ext_slave_id sec_slave_id;
-	__u16 secondary_i2c_addr;
-	__s8 secondary_orientation[9];
-	__u8 key[16];
-};
-
-#endif	/* __MPU_H_ */
diff --git a/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared b/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared
deleted file mode 100644
index 7635cd8..0000000
--- a/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared
+++ /dev/null
diff --git a/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk b/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk
deleted file mode 100644
index 75d93cf..0000000
--- a/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk
+++ /dev/null
@@ -1,11 +0,0 @@
-#### filelist.mk for inv_gesture_test ####
-
-# headers
-#HEADERS += 
-
-# sources
-SOURCES := $(APP_DIR)/inv_gesture_test.c
-
-INV_SOURCES += $(SOURCES)
-
-VPATH += $(APP_DIR)
diff --git a/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c b/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c
deleted file mode 100644
index d38d478..0000000
--- a/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c
+++ /dev/null
@@ -1,535 +0,0 @@
-/**
- *  Gesture Test application for Invensense's MPU6/9xxx (w/ DMP).
- */
-
-#include <unistd.h>
-#include <dirent.h>
-#include <fcntl.h>
-#include <stdio.h>
-#include <errno.h>
-#include <sys/stat.h>
-#include <stdlib.h>
-#include <features.h>
-#include <dirent.h>
-#include <string.h>
-#include <poll.h>
-#include <stddef.h>
-#include <linux/input.h>
-#include <time.h>
-#include <linux/time.h>
-#include <unistd.h>
-#include <termios.h>
-
-#include "invensense.h"
-#include "ml_math_func.h"
-#include "storage_manager.h"
-#include "ml_stored_data.h"
-#include "ml_sysfs_helper.h"
-
-#define DEBUG_PRINT     /* Uncomment to print Gyro & Accel read from Driver */
-
-#define MAX_SYSFS_NAME_LEN  (100)
-#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))
-
-#define FLICK_UPPER_THRES       3147790
-#define FLICK_LOWER_THRES       -3147790
-#define FLICK_COUNTER           50
-#define POLL_TIME               2000 // 2sec
-
-#define FALSE   0
-#define TRUE    1
-
-char *sysfs_names_ptr;
-
-struct sysfs_attrbs {
-    char *enable;
-    char *power_state;
-    char *dmp_on;
-    char *dmp_int_on;
-    char *self_test;
-    char *dmp_firmware;
-    char *firmware_loaded;
-    char *display_orientation_on;
-    char *orientation_on;
-    char *event_flick;
-    char *event_display_orientation;
-    char *event_orientation;
-    char *event_tap;
-    char *flick_axis;
-    char *flick_counter;
-    char *flick_int_on;
-    char *flick_lower;
-    char *flick_upper;
-    char *flick_message_on;
-    char *tap_min_count;
-    char *tap_on;
-    char *tap_threshold;
-    char *tap_time;
-} mpu;
-
-enum {
-    tap,
-    flick,
-    gOrient,
-    orient,
-    numDMPFeatures
-};
-
-struct pollfd pfd[numDMPFeatures];
-
-/*******************************************************************************
- *                       DMP Feature Supported Functions
- ******************************************************************************/
-
-int read_sysfs_int(char *filename, int *var)
-{
-    int res=0;
-    FILE *fp;
-
-    fp = fopen(filename, "r");
-    if (fp!=NULL) {
-        fscanf(fp, "%d\n", var);
-	fclose(fp);
-    } else {
-        MPL_LOGE("ERR open file to read");
-        res= -1;
-    }
-    return res;
-}
-
-int write_sysfs_int(char *filename, int data)
-{
-    int res=0;
-    FILE  *fp;
-
-    fp = fopen(filename, "w");
-    if (fp!=NULL) {
-        fprintf(fp, "%d\n", data);
-	fclose(fp);
-    } else {
-        MPL_LOGE("ERR open file to write");
-        res= -1;
-    }
-    return res;
-}
-
-/**************************************************
-    This _kbhit() function is courtesy from Web
-***************************************************/
-int _kbhit() {
-    static const int STDIN = 0;
-    static bool initialized = false;
-
-    if (! initialized) {
-        // Use termios to turn off line buffering
-        struct termios term;
-        tcgetattr(STDIN, &term);
-        term.c_lflag &= ~ICANON;
-        tcsetattr(STDIN, TCSANOW, &term);
-        setbuf(stdin, NULL);
-        initialized = true;
-    }
-
-    int bytesWaiting;
-    ioctl(STDIN, FIONREAD, &bytesWaiting);
-    return bytesWaiting;
-}
-
-int inv_init_sysfs_attributes(void)
-{
-    unsigned char i = 0;
-    char sysfs_path[MAX_SYSFS_NAME_LEN];
-    char *sptr;
-    char **dptr;
-
-    sysfs_names_ptr =
-            (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
-    sptr = sysfs_names_ptr;
-    if (sptr != NULL) {
-        dptr = (char**)&mpu;
-        do {
-            *dptr++ = sptr;
-            sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
-        } while (++i < MAX_SYSFS_ATTRB);
-    } else {
-        MPL_LOGE("couldn't alloc mem for sysfs paths");
-        return -1;
-    }
-
-    // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
-    inv_get_sysfs_path(sysfs_path);
-
-    sprintf(mpu.enable, "%s%s", sysfs_path, "/buffer/enable");
-    sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
-    sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
-    sprintf(mpu.dmp_int_on, "%s%s", sysfs_path, "/dmp_int_on");
-    sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
-    sprintf(mpu.dmp_firmware, "%s%s", sysfs_path, "/dmp_firmware");
-    sprintf(mpu.firmware_loaded, "%s%s", sysfs_path, "/firmware_loaded");
-    sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on");
-    sprintf(mpu.orientation_on, "%s%s", sysfs_path, "/orientation_on");
-    sprintf(mpu.event_flick, "%s%s", sysfs_path, "/event_flick");
-    sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation");
-    sprintf(mpu.event_orientation, "%s%s", sysfs_path, "/event_orientation");
-    sprintf(mpu.event_tap, "%s%s", sysfs_path, "/event_tap");
-    sprintf(mpu.flick_axis, "%s%s", sysfs_path, "/flick_axis");
-    sprintf(mpu.flick_counter, "%s%s", sysfs_path, "/flick_counter");
-    sprintf(mpu.flick_int_on, "%s%s", sysfs_path, "/flick_int_on");
-    sprintf(mpu.flick_lower, "%s%s", sysfs_path, "/flick_lower");
-    sprintf(mpu.flick_upper, "%s%s", sysfs_path, "/flick_upper");
-    sprintf(mpu.flick_message_on, "%s%s", sysfs_path, "/flick_message_on");
-    sprintf(mpu.tap_min_count, "%s%s", sysfs_path, "/tap_min_count");
-    sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
-    sprintf(mpu.tap_threshold, "%s%s", sysfs_path, "/tap_threshold");
-    sprintf(mpu.tap_time, "%s%s", sysfs_path, "/tap_time");
-
-#if 0
-    // test print sysfs paths
-    dptr = (char**)&mpu;
-    for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
-        MPL_LOGE("sysfs path: %s", *dptr++);
-    }
-#endif
-    return 0;
-}
-
-int DmpFWloaded()
-{
-    int res;
-    read_sysfs_int(mpu.firmware_loaded, &res);
-    return res;
-}
-
-int enable_flick(int en)
-{
-   int res=0;
-   int flickUpper=0, flickLower=0, flickCounter=0;
-
-   if (write_sysfs_int(mpu.flick_int_on, en) < 0) {
-       printf("GT:ERR-can't write 'flick_int_on'");
-       res= -1;
-   }
-
-   if (en) {
-       flickUpper= FLICK_UPPER_THRES;
-       flickLower= FLICK_LOWER_THRES;
-       flickCounter= FLICK_COUNTER;
-   }
-
-   if (write_sysfs_int(mpu.flick_upper, flickUpper) < 0) {
-       printf("GT:ERR-can't write 'flick_upper'");
-       res= -1;
-   }
-
-   if (write_sysfs_int(mpu.flick_lower, flickLower) < 0) {
-       printf("GT:ERR-can't write 'flick_lower'");
-       res= -1;
-   }
-
-   if (write_sysfs_int(mpu.flick_counter, flickCounter) < 0) {
-       printf("GT:ERR-can't write 'flick_counter'");
-       res= -1;
-   }
-
-   if (write_sysfs_int(mpu.flick_message_on, 0) < 0) {
-       printf("GT:ERR-can't write 'flick_message_on'");
-       res= -1;
-   }
-
-   if (write_sysfs_int(mpu.flick_axis, 0) < 0) {
-       printf("GT:ERR_can't write 'flick_axis'");
-       res= -1;
-   }
-
-   return res;
-}
-
-int enable_tap(int en)
-{
-    if (write_sysfs_int(mpu.tap_on, en) < 0) {
-        printf("GT:ERR-can't write 'tap_on'");
-        return -1;
-    }
-
-    return 0;
-}
-
-int enable_displ_orient(int en)
-{
-    if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
-        printf("GT:ERR-can't write 'display_orientation_en'");
-        return -1;
-    }
-
-    return 0;
-}
-
-int enable_orient(int en)
-{
-    if (write_sysfs_int(mpu.orientation_on, en) < 0) {
-        printf("GT:ERR-can't write 'orientation_on'");
-        return -1;
-    }
-
-    return 0;
-}
-
-int flickHandler()
-{
-    FILE *fp;
-    int data;
-
-#ifdef DEBUG_PRINT
-    printf("GT:Flick Handler\n");
-#endif
-
-    fp = fopen(mpu.event_flick, "rt");
-    fscanf(fp, "%d\n", &data);
-    fclose (fp);
-
-    printf("Flick= %x\n", data);
-
-    return 0;
-}
-
-int tapHandler()
-{
-    FILE *fp;
-    int tap, tap_dir, tap_num;
-
-    fp = fopen(mpu.event_tap, "rt");
-    fscanf(fp, "%d\n", &tap);
-    fclose(fp);
-
-    tap_dir = tap/8;
-    tap_num = tap%8 + 1;
-
-#ifdef DEBUG_PRINT
-    printf("GT:Tap Handler **\n");
-    printf("Tap= %x\n", tap);
-    printf("Tap Dir= %x\n", tap_dir);
-    printf("Tap Num= %x\n", tap_num);
-#endif
-
-    switch (tap_dir) {
-        case 1:
-            printf("Tap Axis->X Pos\n");
-            break;
-        case 2:
-            printf("Tap Axis->X Neg\n");
-            break;
-        case 3:
-            printf("Tap Axis->Y Pos\n");
-            break;
-        case 4:
-            printf("Tap Axis->Y Neg\n");
-            break;
-        case 5:
-            printf("Tap Axis->Z Pos\n");
-            break;
-        case 6:
-            printf("Tap Axis->Z Neg\n");
-            break;
-        default:
-            printf("Tap Axis->Unknown\n");
-            break;
-    }
-
-    return 0;
-}
-
-int googleOrientHandler()
-{
-    FILE *fp;
-    int orient;
-
-#ifdef DEBUG_PRINT
-    printf("GT:Google Orient Handler\n");
-#endif
-
-    fp = fopen(mpu.event_display_orientation, "rt");
-    fscanf(fp, "%d\n", &orient);
-    fclose(fp);
-
-    printf("Google Orient-> %d\n", orient);
-
-    return 0;
-}
-
-int orientHandler()
-{
-    FILE *fp;
-    int orient;
-
-    fp = fopen(mpu.event_orientation, "rt");
-    fscanf(fp, "%d\n", &orient);
-    fclose(fp);
-
-#ifdef DEBUG_PRINT
-    printf("GT:Reg Orient Handler\n");
-#endif
-
-    if (orient & 0x01)
-	printf("Orient->X Up\n");
-
-    if (orient & 0x02)
-	printf("Orient->X Down\n");
-
-    if (orient & 0x04)
-	printf("Orient->Y Up\n");
-
-    if (orient & 0x08)
-	printf("Orient->Y Down\n");
-
-    if (orient & 0x10)
-	printf("Orient->Z Up\n");
-
-    if (orient & 0x20)
-	printf("Orient->Z Down\n");
-
-    if (orient & 0x40)
-	printf("Orient->Flip\n");
-
-    return 0;
-}
-
-int enableDMPFeatures(int en)
-{
-    int res= -1;
-
-    if (DmpFWloaded())
-    {
-        /* Currently there's no info regarding DMP's supported features/capabilities */
-        /* An error in enabling features below could be an indication of the feature */
-        /* not supported in current loaded DMP firmware */
-
-        enable_flick(en);
-        enable_tap(en);
-        enable_displ_orient(en);
-        enable_orient(en);
-        res= 0;
-    }
-
-    return res;
-}
-
-int initFds()
-{
-    int i;
-
-    for (i=0; i< numDMPFeatures; i++) {
-        switch(i) {
-            case tap:
-                pfd[i].fd = open(mpu.event_tap, O_RDONLY | O_NONBLOCK);
-                break;
-
-            case flick:
-                pfd[i].fd = open(mpu.event_flick, O_RDONLY | O_NONBLOCK);
-                break;
-
-            case gOrient:
-                pfd[i].fd = open(mpu.event_display_orientation, O_RDONLY | O_NONBLOCK);
-                break;
-
-            case orient:
-                pfd[i].fd = open(mpu.event_orientation, O_RDONLY | O_NONBLOCK);
-                break;
-
-            default:
-                pfd[i].fd = -1;
-         }
-
-        pfd[i].events = POLLPRI|POLLERR,
-        pfd[i].revents = 0;
-#ifdef DEBUG_PRINT
-        printf("GT:pfd[%d].fd= %d\n", i, pfd[i].fd);
-#endif
-    }
-
-    return 0;
-}
-
-int closeFds()
-{
-    int i;
-    for (i = 0; i < numDMPFeatures; i++) {
-        if (!pfd[i].fd)
-            close(pfd[i].fd);
-    }
-    return 0;
-}
-
-/*******************************************************************************
- *                       M a i n  S e l f  T e s t
- ******************************************************************************/
-
-int main(int argc, char **argv)
-{
-    char data[4];
-    int i, res= 0;
-
-    res = inv_init_sysfs_attributes();
-    if (res) {
-        printf("GT:ERR-Can't allocate mem");
-        return -1;
-    }
-
-    /* On Gesture/DMP supported features */
-    enableDMPFeatures(1);
-
-    /* init Fds to poll for Gesture data */
-    initFds();
-
-    /* prompt user to make gesture and how to stop program */
-    printf("\n**Please make Gesture to see data.  Press any key to stop Prog**\n\n");
-
-    do {
-        for (i=0; i< numDMPFeatures; i++) {
-            read(pfd[i].fd, data, 4);
-        }
-
-        poll(pfd, numDMPFeatures, POLL_TIME);
-
-        for (i=0; i< numDMPFeatures; i++) {
-           if(pfd[i].revents != 0) {
-               switch(i) {
-                   case tap:
-                       tapHandler();
-                       break;
-
-                   case flick:
-                       flickHandler();
-                       break;
-
-                   case gOrient:
-                       googleOrientHandler();
-                       break;
-
-                   case orient:
-                       orientHandler();
-                       break;
-
-                   default:
-                       printf("GT:ERR-Not supported");
-                       break;
-               }
-               pfd[i].revents= 0;	//no need. reset anyway
-           }
-        }
-
-    } while (!_kbhit());
-
-    /* Off DMP features */
-    enableDMPFeatures(0);
-
-    /* release resources */
-    closeFds();
-    if (sysfs_names_ptr) {
-        free(sysfs_names_ptr);
-    }
-
-    printf("\nThank You!\n");
-
-    return res;
-}
-
diff --git a/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared b/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared
deleted file mode 100644
index aab27bb..0000000
--- a/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared
+++ /dev/null
diff --git a/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk b/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk
deleted file mode 100644
index 234c1d9..0000000
--- a/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk
+++ /dev/null
@@ -1,109 +0,0 @@
-EXEC = inv_mpu_iio$(SHARED_APP_SUFFIX)
-
-MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
-
-CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
-COMP  ?= $(CROSS)gcc
-LINK  ?= $(CROSS)gcc
-
-OBJFOLDER = $(CURDIR)/obj
-
-INV_ROOT   = ../../../../..
-APP_DIR    = $(CURDIR)/../..
-MLLITE_DIR = $(INV_ROOT)/software/core/mllite
-COMMON_DIR = $(INV_ROOT)/software/simple_apps/common
-MPL_DIR    = $(INV_ROOT)/software/core/mpl
-HAL_DIR    = $(INV_ROOT)/software/core/HAL
-
-include $(INV_ROOT)/software/build/android/common.mk
-
-CFLAGS += $(CMDLINE_CFLAGS)
-CFLAGS += -Wall
-CFLAGS += -fpic
-CFLAGS += -nostdlib
-CFLAGS += -DNDEBUG
-CFLAGS += -D_REENTRANT
-CFLAGS += -DLINUX
-CFLAGS += -DANDROID
-CFLAGS += -mthumb-interwork
-CFLAGS += -fno-exceptions
-CFLAGS += -ffunction-sections
-CFLAGS += -funwind-tables
-CFLAGS += -fstack-protector
-CFLAGS += -fno-short-enums
-CFLAGS += -fmessage-length=0
-CFLAGS += -I$(MLLITE_DIR)
-CFLAGS += -I$(MPL_DIR)
-CFLAGS += -I$(COMMON_DIR)
-CFLAGS += -I$(HAL_DIR)/include
-CFLAGS += $(INV_INCLUDES)
-CFLAGS += $(INV_DEFINES)
-
-LLINK  = -lc
-LLINK += -lm
-LLINK += -lutils
-LLINK += -lcutils
-LLINK += -lgcc
-LLINK += -ldl
-LLINK += -lstdc++
-LLINK += -llog
-LLINK += -lz
-
-PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x
-PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o
-PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o
-
-LFLAGS += $(CMDLINE_LFLAGS)
-LFLAGS += -nostdlib
-LFLAGS += -fpic
-LFLAGS += -Wl,--gc-sections 
-LFLAGS += -Wl,--no-whole-archive 
-LFLAGS += -Wl,-dynamic-linker,/system/bin/linker
-LFLAGS += $(ANDROID_LINK)
-ifneq ($(PRODUCT),panda)
-LFLAGS += -rdynamic
-endif
-
-LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
-
-####################################################################################################
-## sources
-
-INV_LIBS  = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT)
-INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
-
-#INV_SOURCES and VPATH provided by Makefile.filelist
-include ../filelist.mk
-
-INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
-INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
-
-####################################################################################################
-## rules
-
-.PHONY: all clean cleanall install
-
-all: $(EXEC) $(MK_NAME)
-
-$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
-	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
-	$(LINK) $(PRE_LFLAGS) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
-
-$(OBJFOLDER) :
-	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
-	mkdir obj
-
-$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
-	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
-	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $<
-
-clean : 
-	rm -fR $(OBJFOLDER)
-
-cleanall : 
-	rm -fR $(EXEC) $(OBJFOLDER)
-
-install : $(EXEC)
-	cp -f $(EXEC) $(INSTALL_DIR)
-
-
diff --git a/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk b/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk
deleted file mode 100644
index 8a3977a..0000000
--- a/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk
+++ /dev/null
@@ -1,12 +0,0 @@
-#### filelist.mk for mpu_iio ####
-
-# headers
-#HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h
-HEADERS += $(APP_DIR)/iio_utils.h
-
-# sources
-SOURCES := $(APP_DIR)/mpu_iio.c
-
-INV_SOURCES += $(SOURCES)
-
-VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux
diff --git a/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h b/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h
deleted file mode 100644
index 773ff2c..0000000
--- a/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h
+++ /dev/null
@@ -1,643 +0,0 @@
-/* IIO - useful set of util functionality
- *
- * Copyright (c) 2008 Jonathan Cameron
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License version 2 as published by
- * the Free Software Foundation.
- */
-
-/* Made up value to limit allocation sizes */
-#include <string.h>
-#include <stdlib.h>
-#include <ctype.h>
-#include <stdio.h>
-#include <stdint.h>
-#include <dirent.h>
-
-#define IIO_MAX_NAME_LENGTH 30
-
-#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements"
-#define FORMAT_TYPE_FILE "%s_type"
-
-const char *iio_dir = "/sys/bus/iio/devices/";
-
-/**
- * iioutils_break_up_name() - extract generic name from full channel name
- * @full_name: the full channel name
- * @generic_name: the output generic channel name
- **/
-static int iioutils_break_up_name(const char *full_name,
-				  char **generic_name)
-{
-	char *current;
-	char *w, *r;
-	char *working;
-	current = strdup(full_name);
-	working = strtok(current, "_\0");
-	w = working;
-	r = working;
-
-	while (*r != '\0') {
-		if (!isdigit(*r)) {
-			*w = *r;
-			w++;
-		}
-		r++;
-	}
-	*w = '\0';
-	*generic_name = strdup(working);
-	free(current);
-
-	return 0;
-}
-
-/**
- * struct iio_channel_info - information about a given channel
- * @name: channel name
- * @generic_name: general name for channel type
- * @scale: scale factor to be applied for conversion to si units
- * @offset: offset to be applied for conversion to si units
- * @index: the channel index in the buffer output
- * @bytes: number of bytes occupied in buffer output
- * @mask: a bit mask for the raw output
- * @is_signed: is the raw value stored signed
- * @enabled: is this channel enabled
- **/
-struct iio_channel_info {
-	char *name;
-	char *generic_name;
-	float scale;
-	float offset;
-	unsigned index;
-	unsigned bytes;
-	unsigned bits_used;
-	unsigned shift;
-	uint64_t mask;
-	unsigned be;
-	unsigned is_signed;
-	unsigned enabled;
-	unsigned location;
-};
-
-/**
- * iioutils_get_type() - find and process _type attribute data
- * @is_signed: output whether channel is signed
- * @bytes: output how many bytes the channel storage occupies
- * @mask: output a bit mask for the raw data
- * @be: big endian
- * @device_dir: the iio device directory
- * @name: the channel name
- * @generic_name: the channel type name
- **/
-inline int iioutils_get_type(unsigned *is_signed,
-			     unsigned *bytes,
-			     unsigned *bits_used,
-			     unsigned *shift,
-			     uint64_t *mask,
-			     unsigned *be,
-			     const char *device_dir,
-			     const char *name,
-			     const char *generic_name)
-{
-	FILE *sysfsfp;
-	int ret;
-	DIR *dp;
-	char *scan_el_dir, *builtname, *builtname_generic, *filename = 0;
-	char signchar, endianchar;
-	unsigned padint;
-	const struct dirent *ent;
-
-	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
-	if (ret < 0) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	ret = asprintf(&builtname, FORMAT_TYPE_FILE, name);
-	if (ret < 0) {
-		ret = -ENOMEM;
-		goto error_free_scan_el_dir;
-	}
-	ret = asprintf(&builtname_generic, FORMAT_TYPE_FILE, generic_name);
-	if (ret < 0) {
-		ret = -ENOMEM;
-		goto error_free_builtname;
-	}
-
-	dp = opendir(scan_el_dir);
-	if (dp == NULL) {
-		ret = -errno;
-		goto error_free_builtname_generic;
-	}
-	while (ent = readdir(dp), ent != NULL)
-		/*
-		 * Do we allow devices to override a generic name with
-		 * a specific one?
-		 */
-		if ((strcmp(builtname, ent->d_name) == 0) ||
-		    (strcmp(builtname_generic, ent->d_name) == 0)) {
-			ret = asprintf(&filename,
-				       "%s/%s", scan_el_dir, ent->d_name);
-			if (ret < 0) {
-				ret = -ENOMEM;
-				goto error_closedir;
-			}
-			sysfsfp = fopen(filename, "r");
-			if (sysfsfp == NULL) {
-				printf("failed to open %s\n", filename);
-				ret = -errno;
-				goto error_free_filename;
-			}
-
-			ret = fscanf(sysfsfp,
-				     "%ce:%c%u/%u>>%u",
-				     &endianchar,
-				     &signchar,
-				     bits_used,
-				     &padint, shift);
-			if (ret < 0) {
-				printf("failed to pass scan type description\n");
-				return ret;
-			}
-			*be = (endianchar == 'b');
-			*bytes = padint / 8;
-			if (*bits_used == 64)
-				*mask = ~0;
-			else
-				*mask = (1 << *bits_used) - 1;
-			if (signchar == 's')
-				*is_signed = 1;
-			else
-				*is_signed = 0;
-			fclose(sysfsfp);
-			free(filename);
-
-			filename = 0;
-		}
-error_free_filename:
-	if (filename)
-		free(filename);
-error_closedir:
-	closedir(dp);
-error_free_builtname_generic:
-	free(builtname_generic);
-error_free_builtname:
-	free(builtname);
-error_free_scan_el_dir:
-	free(scan_el_dir);
-error_ret:
-	return ret;
-}
-
-inline int iioutils_get_param_float(float *output,
-				    const char *param_name,
-				    const char *device_dir,
-				    const char *name,
-				    const char *generic_name)
-{
-	FILE *sysfsfp;
-	int ret;
-	DIR *dp;
-	char *builtname, *builtname_generic;
-	char *filename = NULL;
-	const struct dirent *ent;
-
-	ret = asprintf(&builtname, "%s_%s", name, param_name);
-	if (ret < 0) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	ret = asprintf(&builtname_generic,
-		       "%s_%s", generic_name, param_name);
-	if (ret < 0) {
-		ret = -ENOMEM;
-		goto error_free_builtname;
-	}
-	dp = opendir(device_dir);
-	if (dp == NULL) {
-		ret = -errno;
-		goto error_free_builtname_generic;
-	}
-	while (ent = readdir(dp), ent != NULL)
-		if ((strcmp(builtname, ent->d_name) == 0) ||
-		    (strcmp(builtname_generic, ent->d_name) == 0)) {
-			ret = asprintf(&filename,
-				       "%s/%s", device_dir, ent->d_name);
-			if (ret < 0) {
-				ret = -ENOMEM;
-				goto error_closedir;
-			}
-			sysfsfp = fopen(filename, "r");
-			if (!sysfsfp) {
-				ret = -errno;
-				goto error_free_filename;
-			}
-			fscanf(sysfsfp, "%f", output);
-			break;
-		}
-error_free_filename:
-	if (filename)
-		free(filename);
-error_closedir:
-	closedir(dp);
-error_free_builtname_generic:
-	free(builtname_generic);
-error_free_builtname:
-	free(builtname);
-error_ret:
-	return ret;
-}
-
-/**
- * bsort_channel_array_by_index() - reorder so that the array is in index order
- *
- **/
-
-inline void bsort_channel_array_by_index(struct iio_channel_info **ci_array,
-					 int cnt)
-{
-
-	struct iio_channel_info temp;
-	int x, y;
-
-	for (x = 0; x < cnt; x++)
-		for (y = 0; y < (cnt - 1); y++)
-			if ((*ci_array)[y].index > (*ci_array)[y+1].index) {
-				temp = (*ci_array)[y + 1];
-				(*ci_array)[y + 1] = (*ci_array)[y];
-				(*ci_array)[y] = temp;
-			}
-}
-
-/**
- * build_channel_array() - function to figure out what channels are present
- * @device_dir: the IIO device directory in sysfs
- * @
- **/
-inline int build_channel_array(const char *device_dir,
-			      struct iio_channel_info **ci_array,
-			      int *counter)
-{
-	DIR *dp;
-	FILE *sysfsfp;
-	int count, i;
-	struct iio_channel_info *current;
-	int ret;
-	const struct dirent *ent;
-	char *scan_el_dir;
-	char *filename;
-
-	*counter = 0;
-	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
-	if (ret < 0) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	dp = opendir(scan_el_dir);
-	if (dp == NULL) {
-		ret = -errno;
-		goto error_free_name;
-	}
-	while (ent = readdir(dp), ent != NULL)
-		if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
-			   "_en") == 0) {
-			ret = asprintf(&filename,
-				       "%s/%s", scan_el_dir, ent->d_name);
-			if (ret < 0) {
-				ret = -ENOMEM;
-				goto error_close_dir;
-			}
-			sysfsfp = fopen(filename, "r");
-			if (sysfsfp == NULL) {
-				ret = -errno;
-				free(filename);
-				goto error_close_dir;
-			}
-			fscanf(sysfsfp, "%u", &ret);
-			printf("%s, %d\n", filename, ret);
-			if (ret == 1)
-				(*counter)++;
-			fclose(sysfsfp);
-			free(filename);
-		}
-	*ci_array = malloc(sizeof(**ci_array) * (*counter));
-	if (*ci_array == NULL) {
-		ret = -ENOMEM;
-		goto error_close_dir;
-	}
-	closedir(dp);
-	dp = opendir(scan_el_dir);
-	//seekdir(dp, 0);
-	count = 0;
-	while (ent = readdir(dp), ent != NULL) {
-		if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
-			   "_en") == 0) {
-			current = &(*ci_array)[count++];
-			ret = asprintf(&filename,
-				       "%s/%s", scan_el_dir, ent->d_name);
-			if (ret < 0) {
-				ret = -ENOMEM;
-				/* decrement count to avoid freeing name */
-				count--;
-				goto error_cleanup_array;
-			}
-			sysfsfp = fopen(filename, "r");
-			if (sysfsfp == NULL) {
-				free(filename);
-				ret = -errno;
-				goto error_cleanup_array;
-			}
-			fscanf(sysfsfp, "%u", &current->enabled);
-			fclose(sysfsfp);
-
-			if (!current->enabled) {
-				free(filename);
-				count--;
-				continue;
-			}
-
-			current->scale = 1.0;
-			current->offset = 0;
-			current->name = strndup(ent->d_name,
-						strlen(ent->d_name) -
-						strlen("_en"));
-			if (current->name == NULL) {
-				free(filename);
-				ret = -ENOMEM;
-				goto error_cleanup_array;
-			}
-			/* Get the generic and specific name elements */
-			ret = iioutils_break_up_name(current->name,
-						     &current->generic_name);
-			if (ret) {
-				free(filename);
-				goto error_cleanup_array;
-			}
-			ret = asprintf(&filename,
-				       "%s/%s_index",
-				       scan_el_dir,
-				       current->name);
-			if (ret < 0) {
-				free(filename);
-				ret = -ENOMEM;
-				goto error_cleanup_array;
-			}
-			sysfsfp = fopen(filename, "r");
-			fscanf(sysfsfp, "%u", &current->index);
-			fclose(sysfsfp);
-			free(filename);
-			/* Find the scale */
-			ret = iioutils_get_param_float(&current->scale,
-						       "scale",
-						       device_dir,
-						       current->name,
-						       current->generic_name);
-			if (ret < 0)
-				goto error_cleanup_array;
-			ret = iioutils_get_param_float(&current->offset,
-						       "offset",
-						       device_dir,
-						       current->name,
-						       current->generic_name);
-			if (ret < 0)
-				goto error_cleanup_array;
-			ret = iioutils_get_type(&current->is_signed,
-						&current->bytes,
-						&current->bits_used,
-						&current->shift,
-						&current->mask,
-						&current->be,
-						device_dir,
-						current->name,
-						current->generic_name);
-		}
-	}
-
-	closedir(dp);
-	/* reorder so that the array is in index order */
-	bsort_channel_array_by_index(ci_array, *counter);
-
-	return 0;
-
-error_cleanup_array:
-	for (i = count - 1;  i >= 0; i--)
-		free((*ci_array)[i].name);
-	free(*ci_array);
-error_close_dir:
-	closedir(dp);
-error_free_name:
-	free(scan_el_dir);
-error_ret:
-	return ret;
-}
-
-inline int _write_sysfs_int(char *filename, char *basedir, int val, int verify)
-{
-	int ret;
-	FILE *sysfsfp;
-	int test;
-	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
-	if (temp == NULL)
-		return -ENOMEM;
-	sprintf(temp, "%s/%s", basedir, filename);
-	sysfsfp = fopen(temp, "w");
-	if (sysfsfp == NULL) {
-		printf("failed to open %s\n", temp);
-		ret = -errno;
-		goto error_free;
-	}
-	fprintf(sysfsfp, "%d", val);
-	fclose(sysfsfp);
-	if (verify) {
-		sysfsfp = fopen(temp, "r");
-		if (sysfsfp == NULL) {
-			printf("failed to open %s\n", temp);
-			ret = -errno;
-			goto error_free;
-		}
-		fscanf(sysfsfp, "%d", &test);
-		if (test != val) {
-			printf("Possible failure in int write %d to %s%s\n",
-				val,
-				basedir,
-				filename);
-			ret = -1;
-		}
-	}
-error_free:
-	free(temp);
-	return ret;
-}
-
-int write_sysfs_int(char *filename, char *basedir, int val)
-{
-	return _write_sysfs_int(filename, basedir, val, 0);
-}
-
-int write_sysfs_int_and_verify(char *filename, char *basedir, int val)
-{
-	return _write_sysfs_int(filename, basedir, val, 1);
-}
-
-int _write_sysfs_string(char *filename, char *basedir, char *val, int verify)
-{
-	int ret = 0;
-	FILE  *sysfsfp;
-	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
-	if (temp == NULL) {
-		printf("Memory allocation failed\n");
-		return -ENOMEM;
-	}
-	sprintf(temp, "%s/%s", basedir, filename);
-	sysfsfp = fopen(temp, "w");
-	if (sysfsfp == NULL) {
-		printf("Could not open %s\n", temp);
-		ret = -errno;
-		goto error_free;
-	}
-	fprintf(sysfsfp, "%s", val);
-	fclose(sysfsfp);
-	if (verify) {
-		sysfsfp = fopen(temp, "r");
-		if (sysfsfp == NULL) {
-			printf("could not open file to verify\n");
-			ret = -errno;
-			goto error_free;
-		}
-		fscanf(sysfsfp, "%s", temp);
-		if (strcmp(temp, val) != 0) {
-			printf("Possible failure in string write of %s "
-				"Should be %s "
-				"written to %s\%s\n",
-				temp,
-				val,
-				basedir,
-				filename);
-			ret = -1;
-		}
-	}
-error_free:
-	free(temp);
-
-	return ret;
-}
-
-/**
- * write_sysfs_string_and_verify() - string write, readback and verify
- * @filename: name of file to write to
- * @basedir: the sysfs directory in which the file is to be found
- * @val: the string to write
- **/
-int write_sysfs_string_and_verify(char *filename, char *basedir, char *val)
-{
-	return _write_sysfs_string(filename, basedir, val, 1);
-}
-
-int write_sysfs_string(char *filename, char *basedir, char *val)
-{
-	return _write_sysfs_string(filename, basedir, val, 0);
-}
-
-int read_sysfs_posint(char *filename, char *basedir)
-{
-	int ret;
-	FILE  *sysfsfp;
-	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
-	if (temp == NULL) {
-		printf("Memory allocation failed");
-		return -ENOMEM;
-	}
-	sprintf(temp, "%s/%s", basedir, filename);
-	sysfsfp = fopen(temp, "r");
-	if (sysfsfp == NULL) {
-		ret = -errno;
-		goto error_free;
-	}
-	fscanf(sysfsfp, "%d\n", &ret);
-	fclose(sysfsfp);
-error_free:
-	free(temp);
-	return ret;
-}
-
-int read_sysfs_float(char *filename, char *basedir, float *val)
-{
-	float ret = 0;
-	FILE  *sysfsfp;
-	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
-	if (temp == NULL) {
-		printf("Memory allocation failed");
-		return -ENOMEM;
-	}
-	sprintf(temp, "%s/%s", basedir, filename);
-	sysfsfp = fopen(temp, "r");
-	if (sysfsfp == NULL) {
-		ret = -errno;
-		goto error_free;
-	}
-	fscanf(sysfsfp, "%f\n", val);
-	fclose(sysfsfp);
-error_free:
-	free(temp);
-	return ret;
-}
-int enable(const char *device_dir,
-			      struct iio_channel_info **ci_array,
-			      int *counter)
-{
-	DIR *dp;
-	int ret;
-	const struct dirent *ent;
-	char *scan_el_dir;
-
-	*counter = 0;
-	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
-	if (ret < 0) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	dp = opendir(scan_el_dir);
-	if (dp == NULL) {
-		ret = -errno;
-		goto error_free_name;
-	}
-	while (ent = readdir(dp), ent != NULL)
-		if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
-			   "_en") == 0) {
-			write_sysfs_int_and_verify((char *)ent->d_name, scan_el_dir, 1);
-		}
-	return 0;
-error_ret:
-error_free_name:
-	return -1;
-}
-int disable_q_out(const char *device_dir,
-			      struct iio_channel_info **ci_array,
-			      int *counter) {
-	DIR *dp;
-	int ret;
-	const struct dirent *ent;
-	char *scan_el_dir;
-
-	*counter = 0;
-	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
-	if (ret < 0) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	dp = opendir(scan_el_dir);
-	if (dp == NULL) {
-		ret = -errno;
-		goto error_free_name;
-	}
-	while (ent = readdir(dp), ent != NULL)
-		if (strncmp(ent->d_name, "in_quaternion", strlen("in_quaternion")) == 0) {
-			write_sysfs_int_and_verify((char *)ent->d_name, scan_el_dir, 0);
-		}
-	return 0;
-error_ret:
-error_free_name:
-	return -1;
-
-}
-
diff --git a/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c b/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c
deleted file mode 100644
index b3d323c..0000000
--- a/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c
+++ /dev/null
@@ -1,685 +0,0 @@
-/* Industrialio buffer test code.
- *
- * Copyright (c) 2008 Jonathan Cameron
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License version 2 as published by
- * the Free Software Foundation.
- *
- * This program is primarily intended as an example application.
- * Reads the current buffer setup from sysfs and starts a short capture
- * from the specified device, pretty printing the result after appropriate
- * conversion.
- *
- * Command line parameters
- * generic_buffer -n <device_name> -t <trigger_name>
- * If trigger name is not specified the program assumes you want a dataready
- * trigger associated with the device and goes looking for it.
- *
- */
-
-#include <unistd.h>
-#include <dirent.h>
-#include <fcntl.h>
-#include <stdio.h>
-#include <errno.h>
-#include <sys/stat.h>
-#include <dirent.h>
-#include <linux/types.h>
-#include <string.h>
-#include <poll.h>
-#include "iio_utils.h"
-#include "ml_load_dmp.h"
-#include "ml_sysfs_helper.h"
-#include "authenticate.h"
-
-#define FLICK_SUPPORTED (0)
-
-/**
- * size_from_channelarray() - calculate the storage size of a scan
- * @channels: the channel info array
- * @num_channels: size of the channel info array
- *
- * Has the side effect of filling the channels[i].location values used
- * in processing the buffer output.
- **/
-int size_from_channelarray(struct iio_channel_info *channels, int num_channels)
-{
-    int bytes = 0;
-    int i = 0;
-    while (i < num_channels) {
-        if (bytes % channels[i].bytes == 0)
-            channels[i].location = bytes;
-        else
-            channels[i].location = bytes - bytes%channels[i].bytes
-                + channels[i].bytes;
-        bytes = channels[i].location + channels[i].bytes;
-        i++;
-    }
-    return bytes;
-}
-
-void print2byte(int input, struct iio_channel_info *info)
-{
-    /* shift before conversion to avoid sign extension
-       of left aligned data */
-    input = input >> info->shift;
-    if (info->is_signed) {
-        int16_t val = input;
-        val &= (1 << info->bits_used) - 1;
-        val = (int16_t)(val << (16 - info->bits_used)) >>
-            (16 - info->bits_used);
-        /*printf("%d, %05f, scale=%05f", val,
-               (float)(val + info->offset)*info->scale, info->scale);*/
-        printf("%d, ", val);
-
-    } else {
-        uint16_t val = input;
-        val &= (1 << info->bits_used) - 1;
-        printf("%05f ", ((float)val + info->offset)*info->scale);
-    }
-}
-/**
- * process_scan() - print out the values in SI units
- * @data:        pointer to the start of the scan
- * @infoarray:        information about the channels. Note
- *  size_from_channelarray must have been called first to fill the
- *  location offsets.
- * @num_channels:    the number of active channels
- **/
-void process_scan(char *data,
-          struct iio_channel_info *infoarray,
-          int num_channels)
-{
-    int k;
-    //char *tmp;
-    for (k = 0; k < num_channels; k++) {
-        switch (infoarray[k].bytes) {
-            /* only a few cases implemented so far */
-        case 2:
-            print2byte(*(uint16_t *)(data + infoarray[k].location),
-                   &infoarray[k]);
-            //tmp = data + infoarray[k].location;
-            break;
-        case 4:
-            if (infoarray[k].is_signed) {
-                int32_t val = *(int32_t *)
-                    (data +
-                     infoarray[k].location);
-                if ((val >> infoarray[k].bits_used) & 1)
-                    val = (val & infoarray[k].mask) |
-                        ~infoarray[k].mask;
-                /* special case for timestamp */
-                printf(" %d ", val);
-            }
-            break;
-        case 8:
-            if (infoarray[k].is_signed) {
-                int64_t val = *(int64_t *)
-                    (data +
-                     infoarray[k].location);
-                if ((val >> infoarray[k].bits_used) & 1)
-                    val = (val & infoarray[k].mask) |
-                        ~infoarray[k].mask;
-                /* special case for timestamp */
-                if (infoarray[k].scale == 1.0f &&
-                    infoarray[k].offset == 0.0f)
-                    printf(" %lld", val);
-                else
-                    printf("%05f ", ((float)val +
-                             infoarray[k].offset)*
-                           infoarray[k].scale);
-            }
-            break;
-        default:
-            break;
-        }
-    }
-    printf("\n");
-}
-
-#if FLICK_SUPPORTED /* hide flick, not offially supported */
-void enable_flick(char *p, int on){
-    int ret;
-    printf("flick:%s\n", p);
-    ret = write_sysfs_int_and_verify("flick_int_on", p, on);
-    if (ret < 0)
-        return;
-    ret = write_sysfs_int_and_verify("flick_upper", p, 3147790);
-    if (ret < 0)
-        return;
-    ret = write_sysfs_int_and_verify("flick_lower", p, -3147790);
-    if (ret < 0)
-        return;
-
-    ret = write_sysfs_int_and_verify("flick_counter", p, 50);
-    if (ret < 0)
-        return;
-    ret = write_sysfs_int_and_verify("flick_message_on", p, 0);
-    if (ret < 0)
-        return;
-    ret = write_sysfs_int_and_verify("flick_axis", p, 0);
-}
-#endif
-
-void HandleOrient(int orient)
-{    
-    if (orient & 0x01)
-    printf("INV_X_UP\n");
-    if (orient & 0x02) 
-    printf("INV_X_DOWN\n");
-    if (orient & 0x04) 
-    printf("INV_Y_UP\n");
-    if (orient & 0x08) 
-    printf("INV_Y_DOWN\n");
-    if (orient & 0x10) 
-    printf("INV_Z_UP\n");
-    if (orient & 0x20) 
-    printf("INV_Z_DOWN\n");
-    if (orient & 0x40) 
-    printf("INV_ORIENTATION_FLIP\n");
-}
-
-void HandleTap(int tap)
-{
-    int tap_dir = tap/8;
-    int tap_num = tap%8 + 1;
-
-    switch (tap_dir) {
-        case 1:
-            printf("INV_TAP_AXIS_X_POS\n");
-            break;
-        case 2:
-            printf("INV_TAP_AXIS_X_NEG\n");
-            break;
-        case 3:
-            printf("INV_TAP_AXIS_Y_POS\n");
-            break;
-        case 4:
-            printf("INV_TAP_AXIS_Y_NEG\n");
-            break;
-        case 5:
-            printf("INV_TAP_AXIS_Z_POS\n");
-            break;
-        case 6:
-            printf("INV_TAP_AXIS_Z_NEG\n");
-            break;
-        default:
-            break;
-    }
-    printf("Tap number: %d\n", tap_num);
-}
-#define DMP_CODE_SIZE 3060
-void verify_img(char *dmp_path){
-    FILE *fp;
-    int i;
-    char dmp_img[DMP_CODE_SIZE];
-    if ((fp = fopen(dmp_path, "rb")) < 0 ) {
-        perror("dmp fail");
-    }
-    i = fread(dmp_img, 1, DMP_CODE_SIZE, fp);
-    printf("Result=%d\n", i);
-    fclose(fp);
-    fp = fopen("/dev/read_img.h", "wt");
-    fprintf(fp, "char rec[]={\n");
-    for(i=0; i<DMP_CODE_SIZE; i++) {
-      fprintf(fp, "0x%02x, ", dmp_img[i]);
-      if(((i+1)%16) == 0) {
-        fprintf(fp, "\n");
-      }
-    }
-    fprintf(fp, "};\n ");
-    fclose(fp);
-}
-
-void setup_dmp(char *dev_path, int p_event){
-    char sysfs_path[200];
-    char dmp_path[200];
-    int  ret;
-    FILE *fd;
-    sprintf(sysfs_path, "%s", dev_path);
-    printf("sysfs: %s\n", sysfs_path);
-    ret = write_sysfs_int_and_verify("power_state", sysfs_path, 1);
-    if (ret < 0)
-        return;
-
-	ret = write_sysfs_int("in_accel_scale", dev_path, 0);
-	if (ret < 0)
-		return;
-	ret = write_sysfs_int("in_anglvel_scale", dev_path, 2);
-	if (ret < 0)
-		return;	
-	ret = write_sysfs_int("sampling_frequency", sysfs_path, 200);
-	if (ret < 0)
-		return;	
-	ret = write_sysfs_int_and_verify("firmware_loaded", sysfs_path, 0);
-	if (ret < 0)
-		return;
-	sprintf(dmp_path, "%s/dmp_firmware", dev_path);
-	if ((fd = fopen(dmp_path, "wb")) < 0 ) {
-		perror("dmp fail");
-	}	
-	inv_load_dmp(fd);
-	fclose(fd);
-	printf("firmware_loaded=%d\n", read_sysfs_posint("firmware_loaded", sysfs_path));
-	ret = write_sysfs_int_and_verify("in_accel_x_offset", sysfs_path, 0xabcd0000);
-	ret = write_sysfs_int_and_verify("in_accel_y_offset", sysfs_path, 0xffff0000);
-	ret = write_sysfs_int_and_verify("in_accel_z_offset", sysfs_path, 0xcdef0000);
-
-	ret = write_sysfs_int_and_verify("dmp_on", sysfs_path, 1);
-	if (ret < 0)
-		return;
-	ret = write_sysfs_int_and_verify("dmp_int_on", sysfs_path, 1);
-	if (ret < 0)
-		return;
-	/* selelct which event to enable and interrupt on/off here */
-	//enable_flick(sysfs_path, 1);
-	ret = write_sysfs_int_and_verify("tap_on", sysfs_path, 1);
-	if (ret < 0)
-		return;
-	ret = write_sysfs_int_and_verify("display_orientation_on", sysfs_path, 1);
-	if (ret < 0)
-		return;
-	ret = write_sysfs_int_and_verify("orientation_on", sysfs_path, 1);
-	if (ret < 0)
-		return;
-	printf("rate\n");
-	ret = write_sysfs_int_and_verify("dmp_output_rate", sysfs_path, 25);
-	if (ret < 0)
-		return;
-	ret = write_sysfs_int_and_verify("dmp_event_int_on", sysfs_path, p_event);
-	if (ret < 0)
-		return;
-	//verify_img(dmp_path);
-}
-
-void get_dmp_event(char *dev_dir_name)
-{
-    char file_name[100];
-    int i;
-#if FLICK_SUPPORTED /* hide flick, not offially supported */
-    int fp_tap, fp_orient, fp_disp, fp_flick;
-    const int n_gest = 6;
-#else
-    int fp_tap, fp_orient, fp_disp, fp_motion;
-    //int fp_no_motion;
-    const int n_gest = 4;
-#endif
-    int data;
-    char d[6];
-    FILE *fp;
-    struct pollfd pfd[4];
-    printf("%s\n", dev_dir_name);
-    while(1) {
-        sprintf(file_name, "%s/event_tap", dev_dir_name);
-        fp_tap = open(file_name, O_RDONLY | O_NONBLOCK);
-        sprintf(file_name, "%s/event_orientation", dev_dir_name);
-        fp_orient = open(file_name, O_RDONLY | O_NONBLOCK);
-        sprintf(file_name, "%s/event_display_orientation", dev_dir_name);
-        fp_disp = open(file_name, O_RDONLY | O_NONBLOCK);
-
-        //sprintf(file_name, "%s/event_accel_motion", dev_dir_name);
-        sprintf(file_name, "%s/event_accel_wom", dev_dir_name);
-        fp_motion = open(file_name, O_RDONLY | O_NONBLOCK);
-        //sprintf(file_name, "%s/event_accel_no_motion", dev_dir_name);
-        //fp_no_motion = open(file_name, O_RDONLY | O_NONBLOCK);
-#if FLICK_SUPPORTED /* hide flick, not offially supported */
-        sprintf(file_name, "%s/event_flick", dev_dir_name);
-        fp_flick = open(file_name, O_RDONLY | O_NONBLOCK);
-#endif
-
-        pfd[0].fd = fp_tap;
-        pfd[0].events = POLLPRI|POLLERR,
-        pfd[0].revents = 0;
-
-        pfd[1].fd = fp_orient;
-        pfd[1].events = POLLPRI|POLLERR,
-        pfd[1].revents = 0;
-
-        pfd[2].fd = fp_disp;
-        pfd[2].events = POLLPRI|POLLERR,
-        pfd[2].revents = 0;
-
-        pfd[3].fd = fp_motion;
-        pfd[3].events = POLLPRI|POLLERR,
-        pfd[3].revents = 0;
-
-        //pfd[4].fd = fp_no_motion;
-        //pfd[4].events = POLLPRI|POLLERR,
-        //pfd[4].revents = 0;
-
-#if FLICK_SUPPORTED /* hide flick, not offially supported */
-        pfd[5].fd = fp_flick;
-        pfd[5].events = POLLPRI|POLLERR,
-        pfd[5].revents = 0;    
-#endif
-
-        read(fp_tap, d, 4);
-        read(fp_orient, d, 4);
-        read(fp_disp, d, 4);
-        read(fp_motion, d, 4);
-        //read(fp_no_motion, d, 4);
-#if FLICK_SUPPORTED /* hide flick, not offially supported */
-        read(fp_flick, d, 4);
-#endif
-
-        poll(pfd, n_gest, -1);
-        close(fp_tap);
-        close(fp_orient);
-        close(fp_disp);
-        close(fp_motion);
-        //close(fp_no_motion);
-#if FLICK_SUPPORTED /* hide flick, not offially supported */
-        close(fp_flick);
-#endif
-        for (i = 0; i < ARRAY_SIZE(pfd); i++) {
-            if(pfd[i].revents != 0) {
-                switch (i){
-                case 0:
-                    sprintf(file_name, "%s/event_tap", dev_dir_name);
-                    fp = fopen(file_name, "rt");
-                    fscanf(fp, "%d\n", &data);
-                    printf("tap=%x\n", data);
-                    HandleTap(data);
-                    fclose(fp);
-                break;
-                case 1:
-                    sprintf(file_name, "%s/event_orientation", dev_dir_name);
-                    fp = fopen(file_name, "rt");
-                    fscanf(fp, "%d\n", &data);
-                    printf("orient=%x\n", data);
-                    HandleOrient(data);
-                    fclose(fp);
-                break;
-                case 2:
-                    sprintf(file_name, "%s/event_display_orientation", dev_dir_name);
-                    fp = fopen(file_name, "rt");
-                    fscanf(fp, "%d\n", &data);
-                    printf("display_orient=%x\n", data);
-                    fclose(fp);
-                break;
-                case 3:
-                    sprintf(file_name, "%s/event_accel_wom", dev_dir_name);
-                    fp = fopen(file_name, "rt");
-                    fscanf(fp, "%d\n", &data);
-                    printf("motion=%x\n", data);
-                    fclose(fp);
-                break;
-                case 4:
-                    sprintf(file_name, "%s/event_accel_no_motion", dev_dir_name);
-                    fp = fopen(file_name, "rt");
-                    fscanf(fp, "%d\n", &data);
-                    printf("No motion=%x\n", data);
-                    fclose(fp);
-                break;
-
-#if FLICK_SUPPORTED /* hide flick, not offially supported */
-                case 5:
-                    sprintf(file_name, "%s/event_flick", dev_dir_name);
-                    fp = fopen(file_name, "rt");
-                    fscanf(fp, "%d\n", &data);
-                    printf("flick=%x\n", data);
-                    fclose(fp);
-                break;
-#endif
-                }
-            }
-        }                        
-    }
-}
-
-
-int main(int argc, char **argv)
-{
-    unsigned long num_loops = 2;
-    unsigned long timedelay = 100000;
-    unsigned long buf_len = 128;
-
-    int ret, c, i, j, toread;
-    int fp;
-
-    int num_channels;
-    char *trigger_name = NULL;
-    char *dev_dir_name, *buf_dir_name;
-
-    int datardytrigger = 1;
-    char *data;
-    int read_size;
-    int dev_num, trig_num;
-    char *buffer_access;
-    int scan_size;
-    int noevents = 0;
-    int p_event = 0, nodmp = 0;
-    char *dummy;
-    char chip_name[10];
-    char device_name[10];
-    char sysfs[100];
-
-    struct iio_channel_info *infoarray;
-    /* -r means no DMP is enabled (raw) -> should be used for mpu3050.
-       -p means no print of data */
-    /* when using -p, 1 means orientation, 2 means tap, 3 means flick */
-    while ((c = getopt(argc, argv, "l:w:c:pret:")) != -1) {
-        switch (c) {
-        case 't':
-            trigger_name = optarg;
-            datardytrigger = 0;
-            break;
-        case 'e':
-            noevents = 1;
-            break;
-        case 'p':
-            p_event = 1;
-            break;
-        case 'r':
-            nodmp = 1;
-            break;
-        case 'c':
-            num_loops = strtoul(optarg, &dummy, 10);
-            break;
-        case 'w':
-            timedelay = strtoul(optarg, &dummy, 10);
-            break;
-        case 'l':
-            buf_len = strtoul(optarg, &dummy, 10);
-            break;
-        case '?':
-            return -1;
-        }
-    }
-    inv_get_sysfs_path(sysfs);
-    printf("sss:::%s\n", sysfs);
-    if (inv_get_chip_name(chip_name) != INV_SUCCESS) {
-        printf("get chip name fail\n");
-        exit(0);
-    }
-    printf("chip_name=%s\n", chip_name);
-    if (INV_SUCCESS != inv_check_key())
-            printf("key check fail\n");
-    else
-            printf("key authenticated\n");
-
-    for (i=0; i<strlen(chip_name); i++) {
-        device_name[i] = tolower(chip_name[i]);
-    }
-    device_name[strlen(chip_name)] = '\0';
-    printf("device name: %s\n", device_name);
-
-    /* Find the device requested */
-    dev_num = find_type_by_name(device_name, "iio:device");
-    if (dev_num < 0) {
-        printf("Failed to find the %s\n", device_name);
-        ret = -ENODEV;
-        goto error_ret;
-    }
-    printf("iio device number being used is %d\n", dev_num);
-    asprintf(&dev_dir_name, "%siio:device%d", iio_dir, dev_num);
-    if (trigger_name == NULL) {
-        /*
-         * Build the trigger name. If it is device associated it's
-         * name is <device_name>_dev[n] where n matches the device
-         * number found above
-         */
-        ret = asprintf(&trigger_name,
-                   "%s-dev%d", device_name, dev_num);
-        if (ret < 0) {
-            ret = -ENOMEM;
-            goto error_ret;
-        }
-    }
-    ret = write_sysfs_int("buffer/enable", dev_dir_name, 0);
-
-	ret = write_sysfs_int_and_verify("power_state", dev_dir_name, 1);
-	ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 1);
-	ret = write_sysfs_int_and_verify("accl_enable", dev_dir_name, 1);
-	ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, 1);
-/*
-	ret = write_sysfs_int_and_verify("zero_motion_on", dev_dir_name, 1);
-	ret = write_sysfs_int_and_verify("zero_motion_dur", dev_dir_name, 12);
-	ret = write_sysfs_int_and_verify("zero_motion_threshold", dev_dir_name, 13);
-
-	ret = write_sysfs_int_and_verify("motion_on", dev_dir_name, 1);
-	ret = write_sysfs_int_and_verify("motion_dur", dev_dir_name, 1);
-	ret = write_sysfs_int_and_verify("motion_threshold", dev_dir_name, 1);
-*/
-	ret = write_sysfs_int_and_verify("accel_wom_on", dev_dir_name, 1);
-	ret = write_sysfs_int_and_verify("accel_wom_threshold", dev_dir_name, 100);
-    /* Verify the trigger exists */
-    trig_num = find_type_by_name(trigger_name, "trigger");
-    if (trig_num < 0) {
-        printf("Failed to find the trigger %s\n", trigger_name);
-        ret = -ENODEV;
-        goto error_free_triggername;
-    }
-    printf("iio trigger number being used is %d\n", trig_num);
-    /*
-     * Parse the files in scan_elements to identify what channels are
-     * present
-     */
-    ret = 0;
-    ret = enable(dev_dir_name, &infoarray, &num_channels);
-    if (ret) {
-        printf("error enable\n");
-        goto error_free_triggername;
-    }
-    if (!nodmp)
-        setup_dmp(dev_dir_name, p_event);
-
-    /*
-     * Construct the directory name for the associated buffer.
-     * As we know that the lis3l02dq has only one buffer this may
-     * be built rather than found.
-     */
-    ret = asprintf(&buf_dir_name, "%siio:device%d/buffer", iio_dir, dev_num);
-    if (ret < 0) {
-        ret = -ENOMEM;
-        goto error_free_triggername;
-    }
-    printf("%s %s\n", dev_dir_name, trigger_name);
-
-    /* Set the device trigger to be the data rdy trigger found above */
-    ret = write_sysfs_string_and_verify("trigger/current_trigger",
-                    dev_dir_name,
-                    trigger_name);
-    if (ret < 0) {
-        printf("Failed to write current_trigger file\n");
-        goto error_free_buf_dir_name;
-    }
-    /* Setup ring buffer parameters */
-    /* length must be even number because iio_store_to_sw_ring is expecting 
-        half pointer to be equal to the read pointer, which is impossible
-        when buflen is odd number. This is actually a bug in the code */
-    ret = write_sysfs_int("length", buf_dir_name, buf_len*2);
-    if (ret < 0)
-        goto exit_here;
-    ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 1);
-    ret = write_sysfs_int_and_verify("accl_enable", dev_dir_name, 1);
-    //ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, 0);
-    if (nodmp == 0) {
-        ret = write_sysfs_int_and_verify("quaternion_on", dev_dir_name, 1);
-    } else {
-        ret = disable_q_out(dev_dir_name, &infoarray, &num_channels);        
-        ret = write_sysfs_int_and_verify("dmp_on", dev_dir_name, 0);
-    }
-    ret = build_channel_array(dev_dir_name, &infoarray, &num_channels);
-    if (ret) {
-        printf("Problem reading scan element information\n");
-        goto exit_here;
-    }
-
-    /* Enable the buffer */
-    ret = write_sysfs_int("enable", buf_dir_name, 1);
-    if (ret < 0)
-        goto exit_here;
-    scan_size = size_from_channelarray(infoarray, num_channels);
-    data = malloc(scan_size*buf_len);
-    if (!data) {
-        ret = -ENOMEM;
-        goto exit_here;
-    }
-
-    ret = asprintf(&buffer_access,
-               "/dev/iio:device%d",
-               dev_num);
-    if (ret < 0) {
-        ret = -ENOMEM;
-        goto error_free_data;
-    }
-    if (p_event) {
-        get_dmp_event(dev_dir_name);
-        goto error_free_buffer_access;
-    }
-    /* Attempt to open non blocking the access dev */
-    fp = open(buffer_access, O_RDONLY | O_NONBLOCK);
-    if (fp == -1) { /*If it isn't there make the node */
-        printf("Failed to open %s\n", buffer_access);
-        ret = -errno;
-        goto error_free_buffer_access;
-    }
-    /* Wait for events 10 times */
-    for (j = 0; j < num_loops; j++) {
-        if (!noevents) {
-            struct pollfd pfd = {
-                .fd = fp,
-                .events = POLLIN,
-            };
-            poll(&pfd, 1, -1);
-            toread = 1;
-            if ((j%128)==0)
-                usleep(timedelay);
-
-        } else {
-            usleep(timedelay);
-            toread = 1;
-        }
-        read_size = read(fp,
-                 data,
-                 toread*scan_size);
-        if (read_size == -EAGAIN) {
-            printf("nothing available\n");
-            continue;
-        }
-        if (0 == p_event) {
-            for (i = 0; i < read_size/scan_size; i++)
-                process_scan(data + scan_size*i,
-                         infoarray,
-                         num_channels);
-        }
-    }
-    close(fp);
-error_free_buffer_access:
-    free(buffer_access);
-error_free_data:
-    free(data);
-exit_here:
-    /* Stop the ring buffer */
-    ret = write_sysfs_int("enable", buf_dir_name, 0);
-
-error_free_buf_dir_name:
-    free(buf_dir_name);
-error_free_triggername:
-    if (datardytrigger)
-        free(trigger_name);
-error_ret:
-    return ret;
-}
diff --git a/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
deleted file mode 100644
index 6dc08bd..0000000
--- a/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared
+++ /dev/null
diff --git a/libsensors_iio/software/simple_apps/self_test/build/filelist.mk b/libsensors_iio/software/simple_apps/self_test/build/filelist.mk
deleted file mode 100644
index 492f47e..0000000
--- a/libsensors_iio/software/simple_apps/self_test/build/filelist.mk
+++ /dev/null
@@ -1,11 +0,0 @@
-#### 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
deleted file mode 100644
index 87ed703..0000000
--- a/libsensors_iio/software/simple_apps/self_test/inv_self_test.c
+++ /dev/null
@@ -1,417 +0,0 @@
-/**
- *  Self Test application for Invensense's MPU6050/MPU6500/MPU9150.
- */
-
-#include <unistd.h>
-#include <dirent.h>
-#include <fcntl.h>
-#include <stdio.h>
-#include <errno.h>
-#include <sys/stat.h>
-#include <stdlib.h>
-#include <features.h>
-#include <dirent.h>
-#include <string.h>
-#include <poll.h>
-#include <stddef.h>
-#include <linux/input.h>
-#include <time.h>
-#include <linux/time.h>
-
-#include "invensense.h"
-#include "ml_math_func.h"
-#include "storage_manager.h"
-#include "ml_stored_data.h"
-#include "ml_sysfs_helper.h"
-
-#ifndef ABS
-#define ABS(x)(((x) >= 0) ? (x) : -(x))
-#endif
-
-//#define DEBUG_PRINT     /* Uncomment to print Gyro & Accel read from Driver */
-
-#define MAX_SYSFS_NAME_LEN  (100)
-#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))
-
-/** Change this key if the data being stored by this file changes */
-#define INV_DB_SAVE_KEY 53395
-
-#define FALSE   0
-#define TRUE    1
-
-#define GYRO_PASS_STATUS_BIT    0x01
-#define ACCEL_PASS_STATUS_BIT   0x02
-#define COMPASS_PASS_STATUS_BIT 0x04
-
-typedef union {
-    long l;
-    int i;
-} bias_dtype;
-
-char *sysfs_names_ptr;
-
-struct sysfs_attrbs {
-    char *enable;
-    char *power_state;
-    char *dmp_on;
-    char *dmp_int_on;
-    char *self_test;
-    char *temperature;
-    char *gyro_enable;
-    char *gyro_x_bias;
-    char *gyro_y_bias;
-    char *gyro_z_bias;
-    char *accel_enable;
-    char *accel_x_bias;
-    char *accel_y_bias;
-    char *accel_z_bias;
-    char *compass_enable;
-} mpu;
-
-struct inv_db_save_t {
-    /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */
-    long compass_bias[3];
-    /** Gyro Bias in Chip Frame in Hardware units scaled by 2^16 */
-    long gyro_bias[3];
-    /** Temperature when *gyro_bias was stored. */
-    long gyro_temp;
-    /** Accel Bias in Chip Frame in Hardware units scaled by 2^16 */
-    long accel_bias[3];
-    /** Temperature when accel bias was stored. */
-    long accel_temp;
-    long gyro_temp_slope[3];
-    /** Sensor Accuracy */
-    int gyro_accuracy;
-    int accel_accuracy;
-    int compass_accuracy;
-};
-
-static struct inv_db_save_t save_data;
-
-/** This function receives the data that was stored in non-volatile memory
-    between power off */
-static inv_error_t inv_db_load_func(const unsigned char *data)
-{
-    memcpy(&save_data, data, sizeof(save_data));
-    return INV_SUCCESS;
-}
-
-/** This function returns the data to be stored in non-volatile memory between
-    power off */
-static inv_error_t inv_db_save_func(unsigned char *data)
-{
-    memcpy(data, &save_data, sizeof(save_data));
-    return INV_SUCCESS;
-}
-
-/** read a sysfs entry that represents an integer */
-int read_sysfs_int(char *filename, int *var)
-{
-    int res=0;
-    FILE *fp;
-
-    fp = fopen(filename, "r");
-    if (fp != NULL) {
-        fscanf(fp, "%d\n", var);
-        fclose(fp);
-    } else {
-        MPL_LOGE("inv_self_test: ERR open file to read");
-        res= -1;   
-    }
-    return res;
-}
-
-/** write a sysfs entry that represents an integer */
-int write_sysfs_int(char *filename, int data)
-{
-    int res=0;
-    FILE  *fp;
-
-    fp = fopen(filename, "w");
-    if (fp!=NULL) {
-        fprintf(fp, "%d\n", data);
-        fclose(fp);
-    } else {
-        MPL_LOGE("inv_self_test: ERR open file to write");
-        res= -1;   
-    }
-    return res;
-}
-
-int inv_init_sysfs_attributes(void)
-{
-    unsigned char i = 0;
-    char sysfs_path[MAX_SYSFS_NAME_LEN];
-    char *sptr;
-    char **dptr;
-
-    sysfs_names_ptr = 
-            (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
-    sptr = sysfs_names_ptr;
-    if (sptr != NULL) {
-        dptr = (char**)&mpu;
-        do {
-            *dptr++ = sptr;
-            sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
-        } while (++i < MAX_SYSFS_ATTRB);
-    } else {
-        MPL_LOGE("inv_self_test: couldn't alloc mem for sysfs paths");
-        return -1;
-    }
-
-    // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
-    inv_get_sysfs_path(sysfs_path);
-
-    sprintf(mpu.enable, "%s%s", sysfs_path, "/buffer/enable");
-    sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
-    sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
-    sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
-    sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature");
-
-    sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable");
-    sprintf(mpu.gyro_x_bias, "%s%s", sysfs_path, "/in_anglvel_x_calibbias");
-    sprintf(mpu.gyro_y_bias, "%s%s", sysfs_path, "/in_anglvel_y_calibbias");
-    sprintf(mpu.gyro_z_bias, "%s%s", sysfs_path, "/in_anglvel_z_calibbias");
-
-    sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable");
-    sprintf(mpu.accel_x_bias, "%s%s", sysfs_path, "/in_accel_x_calibbias");
-    sprintf(mpu.accel_y_bias, "%s%s", sysfs_path, "/in_accel_y_calibbias");
-    sprintf(mpu.accel_z_bias, "%s%s", sysfs_path, "/in_accel_z_calibbias");
-
-    sprintf(mpu.compass_enable, "%s%s", sysfs_path, "/compass_enable");
-
-#if 0
-    // test print sysfs paths
-    dptr = (char**)&mpu;
-    for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
-        MPL_LOGE("inv_self_test: sysfs path: %s", *dptr++);
-    }
-#endif
-    return 0;
-}
-
-/*******************************************************************************
- *                       M a i n  S e l f  T e s t 
- ******************************************************************************/
-int main(int argc, char **argv)
-{
-    FILE *fptr;
-    int self_test_status = 0;
-    inv_error_t result;
-    bias_dtype gyro_bias[3];
-    bias_dtype accel_bias[3];
-    int axis = 0;
-    size_t packet_sz;
-    int axis_sign = 1;
-    unsigned char *buffer;
-    long timestamp;
-    int temperature = 0;
-    bool compass_present = TRUE;
-
-    result = inv_init_sysfs_attributes();
-    if (result)
-        return -1;
-
-    inv_init_storage_manager();
-
-    // Clear out data.
-    memset(&save_data, 0, sizeof(save_data));
-    memset(gyro_bias, 0, sizeof(gyro_bias));
-    memset(accel_bias, 0, sizeof(accel_bias));
-
-    // Register packet to be saved.
-    result = inv_register_load_store(
-                inv_db_load_func, inv_db_save_func,
-                sizeof(save_data), INV_DB_SAVE_KEY);
-
-    // Power ON MPUxxxx chip
-    if (write_sysfs_int(mpu.power_state, 1) < 0) {
-        printf("Self-Test:ERR-Failed to set power state=1\n");
-    } else {
-        // Note: Driver turns on power automatically when self-test invoked
-    }
-
-    // Disable Master enable 
-    if (write_sysfs_int(mpu.enable, 0) < 0) {
-        printf("Self-Test:ERR-Failed to disable master enable\n");
-    }
-
-    // Disable DMP
-    if (write_sysfs_int(mpu.dmp_on, 0) < 0) {
-        printf("Self-Test:ERR-Failed to disable DMP\n");
-    }
-
-    // Enable Accel
-    if (write_sysfs_int(mpu.accel_enable, 1) < 0) {
-        printf("Self-Test:ERR-Failed to enable accel\n");
-    }
-
-    // Enable Gyro
-    if (write_sysfs_int(mpu.gyro_enable, 1) < 0) {
-        printf("Self-Test:ERR-Failed to enable gyro\n");
-    }
-
-    // Enable Compass
-    if (write_sysfs_int(mpu.compass_enable, 1) < 0) {
-#ifdef DEBUG_PRINT
-        printf("Self-Test:ERR-Failed to enable compass\n");
-#endif
-        compass_present= FALSE;
-    }
-
-    fptr = fopen(mpu.self_test, "r");
-    if (!fptr) {
-        printf("Self-Test:ERR-Couldn't invoke self-test\n");
-        result = -1;
-        goto free_sysfs_storage;
-    }
-
-    // Invoke self-test 
-    fscanf(fptr, "%d", &self_test_status);
-	if (compass_present == TRUE) {
-        printf("Self-Test:Self test result- "
-           "Gyro passed= %x, Accel passed= %x, Compass passed= %x\n",
-           (self_test_status & GYRO_PASS_STATUS_BIT),
-           (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1,
-           (self_test_status & COMPASS_PASS_STATUS_BIT) >> 2);
-	} else {
-		printf("Self-Test:Self test result- "
-			   "Gyro passed= %x, Accel passed= %x\n",
-			   (self_test_status & GYRO_PASS_STATUS_BIT),
-			   (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1);
-	}
-    fclose(fptr);
-
-    if (self_test_status & GYRO_PASS_STATUS_BIT) {
-        // Read Gyro Bias
-        if (read_sysfs_int(mpu.gyro_x_bias, &gyro_bias[0].i) < 0 ||
-            read_sysfs_int(mpu.gyro_y_bias, &gyro_bias[1].i) < 0 ||
-            read_sysfs_int(mpu.gyro_z_bias, &gyro_bias[2].i) < 0) {
-            memset(gyro_bias, 0, sizeof(gyro_bias));
-            printf("Self-Test:Failed to read Gyro bias\n");
-        } else {
-            save_data.gyro_accuracy = 3;
-#ifdef DEBUG_PRINT
-            printf("Self-Test:Gyro bias[0..2]= [%d %d %d]\n", 
-                   gyro_bias[0].i, gyro_bias[1].i, gyro_bias[2].i);
-#endif
-        }
-    } else {
-        printf("Self-Test:Failed Gyro self-test\n");
-    }
-
-    if (self_test_status & ACCEL_PASS_STATUS_BIT) {
-        // Read Accel Bias
-        if (read_sysfs_int(mpu.accel_x_bias, &accel_bias[0].i) < 0 ||
-            read_sysfs_int(mpu.accel_y_bias, &accel_bias[1].i) < 0 ||
-            read_sysfs_int(mpu.accel_z_bias, &accel_bias[2].i) < 0) {
-            memset(accel_bias,0, sizeof(accel_bias));
-            printf("Self-Test:Failed to read Accel bias\n");
-        } else {
-            save_data.accel_accuracy = 3;
-#ifdef DEBUG_PRINT
-            printf("Self-Test:Accel bias[0..2]= [%d %d %d]\n",
-                   accel_bias[0].i, accel_bias[1].i, accel_bias[2].i);
-#endif
-       }
-    } else {
-        printf("Self-Test:Failed Accel self-test\n");
-    }
-
-    if (!(self_test_status & (GYRO_PASS_STATUS_BIT | ACCEL_PASS_STATUS_BIT))) {
-        printf("Self-Test:Failed Gyro and Accel self-test, "
-               "nothing left to do\n");
-        result = -1;
-        goto free_sysfs_storage;
-    }
-
-    // Read temperature
-    fptr= fopen(mpu.temperature, "r");
-    if (fptr != NULL) {
-        fscanf(fptr,"%d %ld", &temperature, &timestamp);
-        fclose(fptr);
-    } else {
-        printf("Self-Test:ERR-Couldn't read temperature\n");
-    }
-
-    // When we read gyro bias, the bias is in raw units scaled by 1000.
-    // We store the bias in raw units scaled by 2^16
-    save_data.gyro_bias[0] = (long)(gyro_bias[0].l * 65536.f / 8000.f);
-    save_data.gyro_bias[1] = (long)(gyro_bias[1].l * 65536.f / 8000.f);
-    save_data.gyro_bias[2] = (long)(gyro_bias[2].l * 65536.f / 8000.f);
-
-    // Save temperature @ time stored.
-    //  Temperature is in degrees Celsius scaled by 2^16
-    save_data.gyro_temp = temperature * (1L << 16);
-    save_data.accel_temp = save_data.gyro_temp; 
-
-    // When we read accel bias, the bias is in raw units scaled by 1000.
-    // and it contains the gravity vector.
-    
-    // Find the orientation of the device, by looking for gravity
-    if (ABS(accel_bias[1].l) > ABS(accel_bias[0].l)) {
-        axis = 1;
-    }
-    if (ABS(accel_bias[2].l) > ABS(accel_bias[axis].l)) {
-        axis = 2;
-    }
-    if (accel_bias[axis].l < 0) {
-        axis_sign = -1;
-    }
-    
-    // Remove gravity, gravity in raw units should be 16384 for a 2g setting.
-    // We read data scaled by 1000, so 
-    accel_bias[axis].l -= axis_sign * 4096L * 1000L;
-
-    // Convert scaling from raw units scaled by 1000 to raw scaled by 2^16
-    save_data.accel_bias[0] = (long)(accel_bias[0].l * 65536.f / 1000.f * 4.f);
-    save_data.accel_bias[1] = (long)(accel_bias[1].l * 65536.f / 1000.f * 4.f);
-    save_data.accel_bias[2] = (long)(accel_bias[2].l * 65536.f / 1000.f * 4.f);
-
-#if 1
-    printf("Self-Test:Saved Accel bias[0..2]= [%ld %ld %ld]\n",
-           save_data.accel_bias[0], save_data.accel_bias[1], 
-           save_data.accel_bias[2]);
-    printf("Self-Test:Saved Gyro bias[0..2]= [%ld %ld %ld]\n",
-           save_data.gyro_bias[0], save_data.gyro_bias[1], 
-           save_data.gyro_bias[2]);
-    printf("Self-Test:Gyro temperature @ time stored %ld\n", 
-           save_data.gyro_temp);
-    printf("Self-Test:Accel temperature @ time stored %ld\n", 
-           save_data.accel_temp);
-#endif
-
-    // Get size of packet to store.
-    inv_get_mpl_state_size(&packet_sz);
-
-    // Create place to store data
-    buffer = (unsigned char *)malloc(packet_sz + 10);
-    if (buffer == NULL) {
-        printf("Self-Test:Can't allocate buffer\n");
-        result = -1;
-        goto free_sysfs_storage;
-    }
-
-    // Store the data
-    result = inv_save_mpl_states(buffer, packet_sz);
-    if (result) {
-        result = -1;
-    } else {
-        fptr= fopen(MLCAL_FILE, "wb+");
-        if (fptr != NULL) {
-            fwrite(buffer, 1, packet_sz, fptr);
-            fclose(fptr);
-        } else {
-            printf("Self-Test:ERR- Can't open calibration file to write - %s\n",
-                   MLCAL_FILE);
-            result = -1;
-        }
-    }
-
-    free(buffer);
-
-free_sysfs_storage:
-    free(sysfs_names_ptr);
-    return result;
-}
-