/*
 $License:
    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
 $
 */
/*******************************************************************************
 *
 * $Id: mlFIFOHW.c 5705 2011-06-28 19:32:29Z 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 <stdio.h>
#include <string.h>

#include "mpu.h"
#if defined CONFIG_MPU_SENSORS_MPU6050A2
#    include "mpu6050a2.h"
#elif defined CONFIG_MPU_SENSORS_MPU6050B1
#    include "mpu6050b1.h"
#elif defined CONFIG_MPU_SENSORS_MPU3050
#  include "mpu3050.h"
#else
#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
#endif
#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)           {}

/*
    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;
    }
    MPL_LOGD("inv_get_fifo: toRead=%d length=%d FIFO_FOOTER=%d fifoCount=%d\n",
	     toRead, length, FIFO_FOOTER_SIZE, fifo_objHW.fifoCount);
    // 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  s/b 0x%02x 0x%02x\n",
                     buffer[0], buffer[1], gFifoFooter[0], gFifoFooter[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->inv_mpu_state->status & MPU_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;
}

/**
 * @}
**/