1 files changed, 320 insertions, 0 deletions
diff --git a/libsensors/mlsdk/mllite/mlFIFOHW.c b/libsensors/mlsdk/mllite/mlFIFOHW.c
new file mode 100644
index 0000000..e806b95
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlFIFOHW.c
@@ -0,0 +1,320 @@
+/*
+ $License:
+   Copyright 2011 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.
+  $
+ */
+/*******************************************************************************
+ *
+ * $Id: mlFIFOHW.c 5653 2011-06-16 21:06:55Z nroyer $
+ *
+ *******************************************************************************/
+
+/**
+ *  @defgroup MLFIFO_HW
+ *  @brief  Motion Library - FIFO HW Driver.
+ *          Provides facilities to interact with the FIFO.
+ *
+ *  @{
+ *      @file   mlFIFOHW.c
+ *      @brief  The Motion Library Fifo Hardware Layer.
+ *
+ */
+
+#include <string.h>
+
+#include "mpu.h"
+#include "mpu3050.h"
+#include "mlFIFOHW.h"
+#include "ml.h"
+#include "mldl.h"
+#include "mldl_cfg.h"
+
+#include "mlsl.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-fifo"
+
+/*
+    Defines
+*/
+
+#define _fifoDebug(x)           //{x}
+
+/*
+    Typedefs
+*/
+
+struct fifo_hw_obj {
+    short fifoCount;
+    inv_error_t fifoError;
+    unsigned char fifoOverflow;
+    unsigned char fifoResetOnOverflow;
+};
+
+/*
+    Global variables
+*/
+const unsigned char gFifoFooter[FIFO_FOOTER_SIZE] = { 0xB2, 0x6A };
+
+/*
+    Static variables
+*/
+static struct fifo_hw_obj fifo_objHW;
+
+/*
+    Definitions
+*/
+
+/**
+ *  @brief  Initializes the internal FIFO data structure.
+ */
+void inv_init_fifo_hardare(void)
+{
+    memset(&fifo_objHW, 0, sizeof(fifo_objHW));
+    fifo_objHW.fifoResetOnOverflow = TRUE;
+}
+
+/**
+ *  @internal
+ *  @brief  used to get the FIFO data.
+ *  @param  length
+ *              Number of bytes to read from the FIFO.
+ *  @param  buffer
+ *              the bytes of FIFO data.
+ *              Note that this buffer <b>must</b> be large enough
+ *              to store and additional trailing FIFO footer when
+ *              expected.  The callers must make sure enough space
+ *              is allocated.
+ *  @return number of valid bytes of data.
+**/
+uint_fast16_t inv_get_fifo(uint_fast16_t length, unsigned char *buffer)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result;
+    uint_fast16_t inFifo;
+    uint_fast16_t toRead;
+    int_fast8_t kk;
+
+    toRead = length - FIFO_FOOTER_SIZE + fifo_objHW.fifoCount;
+    /*---- make sure length is correct ----*/
+    if (length > MAX_FIFO_LENGTH || toRead > length || NULL == buffer) {
+        fifo_objHW.fifoError = INV_ERROR_INVALID_PARAMETER;
+        return 0;
+    }
+
+    result = inv_get_fifo_length(&inFifo);
+    if (INV_SUCCESS != result) {
+        fifo_objHW.fifoError = result;
+        return 0;
+    }
+    // fifo_objHW.fifoCount is the footer size left in the buffer, or
+    //      0 if this is the first time reading the fifo since it was reset
+    if (inFifo < length + fifo_objHW.fifoCount) {
+        fifo_objHW.fifoError = INV_SUCCESS;
+        return 0;
+    }
+    // if a trailing fifo count is expected - start storing data 2 bytes before
+    result =
+        inv_read_fifo(fifo_objHW.fifoCount >
+                      0 ? buffer : buffer + FIFO_FOOTER_SIZE, toRead);
+    if (INV_SUCCESS != result) {
+        fifo_objHW.fifoError = result;
+        return 0;
+    }
+    // Make sure the fifo didn't overflow before or during the read
+    result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
+                             MPUREG_INT_STATUS, 1, &fifo_objHW.fifoOverflow);
+    if (INV_SUCCESS != result) {
+        fifo_objHW.fifoError = result;
+        return 0;
+    }
+
+    if (fifo_objHW.fifoOverflow & BIT_INT_STATUS_FIFO_OVERLOW) {
+        MPL_LOGV("Resetting Fifo : Overflow\n");
+        inv_reset_fifo();
+        fifo_objHW.fifoError = INV_ERROR_FIFO_OVERFLOW;
+        return 0;
+    }
+
+    /* Check the Footer value to give us a chance at making sure data
+     * didn't get corrupted */
+    for (kk = 0; kk < fifo_objHW.fifoCount; ++kk) {
+        if (buffer[kk] != gFifoFooter[kk]) {
+            MPL_LOGV("Resetting Fifo : Invalid footer : 0x%02x 0x%02x\n",
+                     buffer[0], buffer[1]);
+            _fifoDebug(char out[200];
+                       MPL_LOGW("fifoCount : %d\n", fifo_objHW.fifoCount);
+                       sprintf(out, "0x");
+                       for (kk = 0; kk < (int)toRead; kk++) {
+                       sprintf(out, "%s%02X", out, buffer[kk]);}
+                       MPL_LOGW("%s\n", out);)
+                inv_reset_fifo();
+            fifo_objHW.fifoError = INV_ERROR_FIFO_FOOTER;
+            return 0;
+        }
+    }
+
+    if (fifo_objHW.fifoCount == 0) {
+        fifo_objHW.fifoCount = FIFO_FOOTER_SIZE;
+    }
+
+    return length - FIFO_FOOTER_SIZE;
+}
+
+/**
+ *  @brief  Used to query the status of the FIFO.
+ *  @return INV_SUCCESS if the fifo is OK. An error code otherwise.
+**/
+inv_error_t inv_get_fifo_status(void)
+{
+    inv_error_t fifoError = fifo_objHW.fifoError;
+    fifo_objHW.fifoError = 0;
+    return fifoError;
+}
+
+/**
+ * @internal
+ * @brief   Get the length from the fifo
+ *
+ * @param[out] len amount of data currently stored in the fifo.
+ *
+ * @return INV_SUCCESS or non-zero error code.
+**/
+inv_error_t inv_get_fifo_length(uint_fast16_t * len)
+{
+    INVENSENSE_FUNC_START;
+    unsigned char fifoBuf[2];
+    inv_error_t result;
+
+    if (NULL == len)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    /*---- read the 2 'count' registers and
+      burst read the data from the FIFO ----*/
+    result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
+                             MPUREG_FIFO_COUNTH, 2, fifoBuf);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("ReadBurst failed %d\n", result);
+        inv_reset_fifo();
+        fifo_objHW.fifoError = INV_ERROR_FIFO_READ_COUNT;
+        *len = 0;
+        return result;
+    }
+
+    *len = (uint_fast16_t) (fifoBuf[0] << 8);
+    *len += (uint_fast16_t) (fifoBuf[1]);
+    return result;
+}
+
+/**
+ *  @brief  inv_get_fifo_count is used to get the number of bytes left in the FIFO.
+ *          This function returns the stored value and does not access the hardware.
+ *          See inv_get_fifo_length().
+ *  @return the number of bytes left in the FIFO
+**/
+short inv_get_fifo_count(void)
+{
+    return fifo_objHW.fifoCount;
+}
+
+/**
+ *  @internal
+ *  @brief  Read data from the fifo
+ *
+ *  @param[out] data Location to store the date read from the fifo
+ *  @param[in] len   Amount of data to read out of the fifo
+ *
+ *  @return INV_SUCCESS or non-zero error code
+**/
+inv_error_t inv_read_fifo(unsigned char *data, uint_fast16_t len)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result;
+    result = inv_serial_read_fifo(inv_get_serial_handle(),
+                                  inv_get_mpu_slave_addr(),
+                                  (unsigned short)len, data);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_serial_readBurst failed %d\n", result);
+        inv_reset_fifo();
+        fifo_objHW.fifoError = INV_ERROR_FIFO_READ_DATA;
+        return result;
+    }
+    return result;
+}
+
+/**
+ *  @brief  Clears the FIFO status and its content.
+ *  @note   Halt the DMP writing into the FIFO for the time
+ *          needed to reset the FIFO.
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_reset_fifo(void)
+{
+    INVENSENSE_FUNC_START;
+    int len = FIFO_HW_SIZE;
+    unsigned char fifoBuf[2];
+    unsigned char tries = 0;
+    unsigned char userCtrlReg;
+    inv_error_t result;
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+
+    fifo_objHW.fifoCount = 0;
+    if (mldl_cfg->gyro_is_suspended)
+        return INV_SUCCESS;
+
+    result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
+                             MPUREG_USER_CTRL, 1, &userCtrlReg);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    while (len != 0 && tries < 6) {
+        result =
+            inv_serial_single_write(inv_get_serial_handle(),
+                                    inv_get_mpu_slave_addr(), MPUREG_USER_CTRL,
+                                    ((userCtrlReg & (~BIT_FIFO_EN)) |
+                                     BIT_FIFO_RST));
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        result =
+            inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
+                            MPUREG_FIFO_COUNTH, 2, fifoBuf);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        len = (unsigned short)fifoBuf[0] * 256 + (unsigned short)fifoBuf[1];
+        tries++;
+    }
+
+    result =
+        inv_serial_single_write(inv_get_serial_handle(),
+                                inv_get_mpu_slave_addr(), MPUREG_USER_CTRL,
+                                userCtrlReg);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return INV_SUCCESS;
+}
+
+/**
+ * @}
+**/