/*
 $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: mldl.c 5653 2011-06-16 21:06:55Z nroyer $
 *
 *****************************************************************************/

/**
 *  @defgroup   MLDL 
 *  @brief      Motion Library - Driver Layer.
 *              The Motion Library Driver Layer provides the intrface to the 
 *              system drivers that are used by the Motion Library.
 *
 *  @{
 *      @file   mldl.c
 *      @brief  The Motion Library Driver Layer.
 */

/* ------------------ */
/* - Include Files. - */
/* ------------------ */

#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 "mldl.h"
#include "mldl_cfg.h"
#include "compass.h"
#include "mlsl.h"
#include "mlos.h"
#include "mlinclude.h"
#include "ml.h"
#include "dmpKey.h"
#include "mlFIFOHW.h"
#include "compass.h"
#include "pressure.h"

#include "log.h"
#undef MPL_LOG_TAG
#define MPL_LOG_TAG "MPL-mldl"

#define _mldlDebug(x)           //{x}

/* --------------------- */
/* -    Variables.     - */
/* --------------------- */

#define MAX_LOAD_WRITE_SIZE (MPU_MEM_BANK_SIZE/2)   /* 128 */

/*---- structure containing control variables used by MLDL ----*/
static struct mldl_cfg mldlCfg;
struct ext_slave_descr gAccel;
struct ext_slave_descr gCompass;
struct ext_slave_descr gPressure;
struct mpu_platform_data gPdata;
static void *sMLSLHandle;
int_fast8_t intTrigger[NUM_OF_INTSOURCES];

/*******************************************************************************
 * Functions for accessing the DMP memory via keys
 ******************************************************************************/

unsigned short (*sGetAddress) (unsigned short key) = NULL;
static const unsigned char *localDmpMemory = NULL;
static unsigned short localDmpMemorySize = 0;

/**
 *  @internal
 *  @brief Sets the function to use to convert keys to addresses. This
 *         will changed for each DMP code loaded.
 *  @param func
 *              Function used to convert keys to addresses.
 *  @endif
 */
void inv_set_get_address(unsigned short (*func) (unsigned short key))
{
    INVENSENSE_FUNC_START;
    _mldlDebug(MPL_LOGV("setGetAddress %d", (int)func);
        )
        sGetAddress = func;
}

/**
 *  @internal
 *  @brief  Check if the feature is supported in the currently loaded
 *          DMP code basing on the fact that the key is assigned a
 *          value or not.
 *  @param  key     the DMP key
 *  @return whether the feature associated with the key is supported
 *          or not.
 */
uint_fast8_t inv_dmpkey_supported(unsigned short key)
{
    unsigned short memAddr;

    if (sGetAddress == NULL) {
        MPL_LOGE("%s : sGetAddress is NULL\n", __func__);
        return FALSE;
    }

    memAddr = sGetAddress(key);
    if (memAddr >= 0xffff) {
        MPL_LOGV("inv_set_mpu_memory unsupported key\n");
        return FALSE;
    }

    return TRUE;
}

/**
 *  @internal
 *  @brief  used to get the specified number of bytes from the original
 *          MPU memory location specified by the key.
 *          Reads the specified number of bytes from the MPU location
 *          that was used to program the MPU specified by the key. Each
 *          set of code specifies a function that changes keys into
 *          addresses. This function is set with setGetAddress().
 *
 *  @param  key     The key to use when looking up the address.
 *  @param  length  Number of bytes to read.
 *  @param  buffer  Result for data.
 *
 *  @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key
 *          not corresponding to a memory address will result in INV_ERROR.
 *  @endif
 */
inv_error_t inv_get_mpu_memory_original(unsigned short key,
                                        unsigned short length,
                                        unsigned char *buffer)
{
    unsigned short offset;

    if (sGetAddress == NULL) {
        return INV_ERROR_NOT_OPENED;
    }

    offset = sGetAddress(key);
    if (offset >= localDmpMemorySize || (offset + length) > localDmpMemorySize) {
        return INV_ERROR_INVALID_PARAMETER;
    }

    memcpy(buffer, &localDmpMemory[offset], length);

    return INV_SUCCESS;
}

unsigned short inv_dl_get_address(unsigned short key)
{
    unsigned short offset;
    if (sGetAddress == NULL) {
        return INV_ERROR_NOT_OPENED;
    }

    offset = sGetAddress(key);
    return offset;
}

/* ---------------------- */
/* -  Static Functions. - */
/* ---------------------- */

/**
 *  @brief  Open the driver layer and resets the internal
 *          gyroscope, accelerometer, and compass data
 *          structures.
 *  @param  mlslHandle
 *              the serial handle.
 *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
 */
inv_error_t inv_dl_open(void *mlslHandle)
{
    inv_error_t result;
    memset(&mldlCfg, 0, sizeof(mldlCfg));
    memset(intTrigger, INT_CLEAR, sizeof(intTrigger));

    sMLSLHandle = mlslHandle;

    mldlCfg.addr  = 0x68; /* default incase the driver doesn't set it */
    mldlCfg.accel = &gAccel;
    mldlCfg.compass = &gCompass;
    mldlCfg.pressure = &gPressure;
    mldlCfg.pdata = &gPdata;

    result = (inv_error_t) inv_mpu_open(&mldlCfg, sMLSLHandle,
                                        sMLSLHandle, sMLSLHandle, sMLSLHandle);
    return result;
}

/**
 *  @brief  Closes/Cleans up the ML Driver Layer.
 *          Put the device in sleep mode.
 *  @return INV_SUCCESS or non-zero error code.
 */
inv_error_t inv_dl_close(void)
{
    INVENSENSE_FUNC_START;
    inv_error_t result = INV_SUCCESS;

    result = (inv_error_t) inv_mpu_suspend(&mldlCfg,
                                           sMLSLHandle,
                                           sMLSLHandle,
                                           sMLSLHandle,
                                           sMLSLHandle, 
                                           INV_ALL_SENSORS);

    result = (inv_error_t) inv_mpu_close(&mldlCfg, sMLSLHandle,
                                         sMLSLHandle, sMLSLHandle, sMLSLHandle);
    /* Clear all previous settings */
    memset(&mldlCfg, 0, sizeof(mldlCfg));
    sMLSLHandle = NULL;
    sGetAddress = NULL;
    return result;
}

/**
 * @brief Sets the requested_sensors
 *
 * Accessor to set the requested_sensors field of the mldl_cfg structure.
 * Typically set at initialization.
 *
 * @param sensors
 * Bitfield of the sensors that are going to be used.  Combination of the
 * following:
 *  - INV_X_GYRO
 *  - INV_Y_GYRO
 *  - INV_Z_GYRO
 *  - INV_DMP_PROCESSOR
 *  - INV_X_ACCEL
 *  - INV_Y_ACCEL
 *  - INV_Z_ACCEL
 *  - INV_X_COMPASS
 *  - INV_Y_COMPASS
 *  - INV_Z_COMPASS
 *  - INV_X_PRESSURE
 *  - INV_Y_PRESSURE
 *  - INV_Z_PRESSURE
 *  - INV_THREE_AXIS_GYRO
 *  - INV_THREE_AXIS_ACCEL
 *  - INV_THREE_AXIS_COMPASS
 *  - INV_THREE_AXIS_PRESSURE
 *
 * @return INV_SUCCESS or non-zero error code
 */
inv_error_t inv_init_requested_sensors(unsigned long sensors)
{
    mldlCfg.requested_sensors = sensors;

    return INV_SUCCESS;
}

/**
 * @brief Starts the DMP running
 *
 * Resumes the sensor if any of the sensor axis or components are requested
 *
 * @param sensors
 * Bitfield of the sensors to turn on.  Combination of the following:
 *  - INV_X_GYRO
 *  - INV_Y_GYRO
 *  - INV_Z_GYRO
 *  - INV_DMP_PROCESSOR
 *  - INV_X_ACCEL
 *  - INV_Y_ACCEL
 *  - INV_Z_ACCEL
 *  - INV_X_COMPASS
 *  - INV_Y_COMPASS
 *  - INV_Z_COMPASS
 *  - INV_X_PRESSURE
 *  - INV_Y_PRESSURE
 *  - INV_Z_PRESSURE
 *  - INV_THREE_AXIS_GYRO
 *  - INV_THREE_AXIS_ACCEL
 *  - INV_THREE_AXIS_COMPASS
 *  - INV_THREE_AXIS_PRESSURE
 *
 * @return INV_SUCCESS or non-zero error code
 */
inv_error_t inv_dl_start(unsigned long sensors)
{
    INVENSENSE_FUNC_START;
    inv_error_t result = INV_SUCCESS;

    mldlCfg.requested_sensors = sensors;
    result = inv_mpu_resume(&mldlCfg,
                            sMLSLHandle,
                            sMLSLHandle,
                            sMLSLHandle,
                            sMLSLHandle,
                            sensors);
    return result;
}

/**
 * @brief Stops the DMP running and puts it in low power as requested
 *
 * Suspends each sensor according to the bitfield, if all axis and components
 * of the sensor is off.
 *
 * @param sensors Bitfiled of the sensors to leave on.  Combination of the
 * following:
 *  - INV_X_GYRO
 *  - INV_Y_GYRO
 *  - INV_Z_GYRO
 *  - INV_X_ACCEL
 *  - INV_Y_ACCEL
 *  - INV_Z_ACCEL
 *  - INV_X_COMPASS
 *  - INV_Y_COMPASS
 *  - INV_Z_COMPASS
 *  - INV_X_PRESSURE
 *  - INV_Y_PRESSURE
 *  - INV_Z_PRESSURE
 *  - INV_THREE_AXIS_GYRO
 *  - INV_THREE_AXIS_ACCEL
 *  - INV_THREE_AXIS_COMPASS
 *  - INV_THREE_AXIS_PRESSURE
 *
 *
 * @return INV_SUCCESS or non-zero error code
 */
inv_error_t inv_dl_stop(unsigned long sensors)
{
    INVENSENSE_FUNC_START;
    inv_error_t result = INV_SUCCESS;

    result = inv_mpu_suspend(&mldlCfg,
                             sMLSLHandle,
                             sMLSLHandle,
                             sMLSLHandle,
                             sMLSLHandle,
                             sensors);
    return result;
}

/**
 *  @brief  Get a pointer to the internal data structure
 *          storing the configuration for the MPU, the accelerometer
 *          and the compass in use.
 *  @return a pointer to the data structure of type 'struct mldl_cfg'.
 */
struct mldl_cfg *inv_get_dl_config(void)
{
    return &mldlCfg;
}

/**
 *  @brief   Query the MPU slave address.
 *  @return  The 7-bit mpu slave address.
 */
unsigned char inv_get_mpu_slave_addr(void)
{
    INVENSENSE_FUNC_START;
    return mldlCfg.addr;
}

/**
 *  @internal
 * @brief   MLDLCfgDMP configures the Digital Motion Processor internal to
 *          the MPU. The DMP can be enabled or disabled and the start address
 *          can be set.
 *
 * @param   enableRun   Enables the DMP processing if set to TRUE.
 * @param   enableFIFO  Enables DMP output to the FIFO if set to TRUE.
 * @param   startAddress start address
 *
 * @return  Zero if the command is successful, an error code otherwise.
*/
inv_error_t inv_get_dl_ctrl_dmp(unsigned char enableRun,
                                unsigned char enableFIFO)
{
    INVENSENSE_FUNC_START;

    mldlCfg.dmp_enable = enableRun;
    mldlCfg.fifo_enable = enableFIFO;
    mldlCfg.gyro_needs_reset = TRUE;

    return INV_SUCCESS;
}

/**
 * @brief   inv_get_dl_cfg_int configures the interrupt function on the specified pin.
 *          The basic interrupt signal characteristics can be set
 *          (i.e. active high/low, open drain/push pull, etc.) and the
 *          triggers can be set.
 *          Currently only INTPIN_MPU is supported.
 *
 * @param   triggers
 *              bitmask of triggers to enable for interrupt.
 *              The available triggers are:
 *              - BIT_MPU_RDY_EN
 *              - BIT_DMP_INT_EN
 *              - BIT_RAW_RDY_EN
 *
 * @return  Zero if the command is successful, an error code otherwise.
*/
inv_error_t inv_get_dl_cfg_int(unsigned char triggers)
{
    inv_error_t result = INV_SUCCESS;

#if defined CONFIG_MPU_SENSORS_MPU3050
    /* Mantis has 8 bits of interrupt config bits */
    if (triggers & !(BIT_MPU_RDY_EN | BIT_DMP_INT_EN | BIT_RAW_RDY_EN)) {
        return INV_ERROR_INVALID_PARAMETER;
    }
#endif

    mldlCfg.int_config = triggers;
    if (!mldlCfg.gyro_is_suspended) {
        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
                                         MPUREG_INT_CFG,
                                         (mldlCfg.int_config | mldlCfg.pdata->
                                          int_config));
    } else {
        mldlCfg.gyro_needs_reset = TRUE;
    }

    return result;
}

/**
 * @brief   configures the output sampling rate on the MPU.
 *          Three parameters control the sampling:
 *
 *          1) Low pass filter bandwidth, and
 *          2) output sampling divider.
 *
 *          The output sampling rate is determined by the divider and the low
 *          pass filter setting. If the low pass filter is set to
 *          'MPUFILTER_256HZ_NOLPF2', then the sample rate going into the
 *          divider is 8kHz; for all other settings it is 1kHz.
 *          The 8-bit divider will divide this frequency to get the resulting
 *          sample frequency.
 *          For example, if the filter setting is not 256Hz and the divider is
 *          set to 7, then the sample rate is as follows:
 *          sample rate = internal sample rate / div = 1kHz / 8 = 125Hz (or 8ms).
 *
 *          The low pass filter selection codes control both the cutoff frequency of
 *          the internal low pass filter and internal analog sampling rate. The
 *          latter, in turn, affects the final output sampling rate according to the
 *          sample rate divider settig.
 *              0 -> 256 Hz  cutoff BW, 8 kHz analog sample rate,
 *              1 -> 188 Hz  cutoff BW, 1 kHz analog sample rate,
 *              2 ->  98 Hz  cutoff BW, 1 kHz analog sample rate,
 *              3 ->  42 Hz  cutoff BW, 1 kHz analog sample rate,
 *              4 ->  20 Hz  cutoff BW, 1 kHz analog sample rate,
 *              5 ->  10 Hz  cutoff BW, 1 kHz analog sample rate,
 *              6 ->   5 Hz  cutoff BW, 1 kHz analog sample rate,
 *              7 -> 2.1 kHz cutoff BW, 8 kHz analog sample rate.
 *
 * @param   lpf         low pass filter,   0 to 7.
 * @param   divider     Output sampling rate divider, 0 to 255.
 *
 * @return  ML_SUCESS if successful; a non-zero error code otherwise.
**/
inv_error_t inv_dl_cfg_sampling(unsigned char lpf, unsigned char divider)
{
    /*---- do range checking ----*/
    if (lpf >= NUM_MPU_FILTER) {
        return INV_ERROR_INVALID_PARAMETER;
    }

    mldlCfg.lpf = lpf;
    mldlCfg.divider = divider;
    mldlCfg.gyro_needs_reset = TRUE;

    return INV_SUCCESS;
}

/**
 *  @brief  set the full scale range for the gyros.
 *          The full scale selection codes correspond to:
 *              0 -> 250  dps,
 *              1 -> 500  dps,
 *              2 -> 1000 dps,
 *              3 -> 2000 dps.
 *          Full scale range affect the MPU's measurement
 *          sensitivity.
 *
 *  @param  fullScale
 *              the gyro full scale range in dps.
 *
 *  @return INV_SUCCESS or non-zero error code.
**/
inv_error_t inv_set_full_scale(float fullScale)
{
    if (fullScale == 250.f)
        mldlCfg.full_scale = MPU_FS_250DPS;
    else if (fullScale == 500.f)
        mldlCfg.full_scale = MPU_FS_500DPS;
    else if (fullScale == 1000.f)
        mldlCfg.full_scale = MPU_FS_1000DPS;
    else if (fullScale == 2000.f)
        mldlCfg.full_scale = MPU_FS_2000DPS;
    else {                      // not a valid setting
        MPL_LOGE("Invalid full scale range specification for gyros : %f\n",
                 fullScale);
        MPL_LOGE
            ("\tAvailable values : +/- 250 dps, +/- 500 dps, +/- 1000 dps, +/- 2000 dps\n");
        return INV_ERROR_INVALID_PARAMETER;
    }
    mldlCfg.gyro_needs_reset = TRUE;

    return INV_SUCCESS;
}

/**
 * @brief   This function sets the external sync for the MPU sampling.
 *          It can be synchronized on the LSB of any of the gyros, any of the
 *          external accels, or on the temp readings.
 *
 * @param   extSync External sync selection, 0 to 7.
 * @return  Zero if the command is successful; an error code otherwise.
**/
inv_error_t inv_set_external_sync(unsigned char extSync)
{
    INVENSENSE_FUNC_START;

    /*---- do range checking ----*/
    if (extSync >= NUM_MPU_EXT_SYNC) {
        return INV_ERROR_INVALID_PARAMETER;
    }
    mldlCfg.ext_sync = extSync;
    mldlCfg.gyro_needs_reset = TRUE;

    return INV_SUCCESS;
}

inv_error_t inv_set_ignore_system_suspend(unsigned char ignore)
{
    INVENSENSE_FUNC_START;

    mldlCfg.ignore_system_suspend = ignore;

    return INV_SUCCESS;
}

/**
 * @brief   inv_clock_source function sets the clock source for the MPU gyro
 *          processing.
 *          The source can be any of the following:
 *          - Internal 8MHz oscillator,
 *          - PLL with X gyro as reference,
 *          - PLL with Y gyro as reference,
 *          - PLL with Z gyro as reference,
 *          - PLL with external 32.768Mhz reference, or
 *          - PLL with external 19.2MHz reference
 *
 *          For best accuracy and timing, it is highly recommended to use one
 *          of the gyros as the clock source; however this gyro must be
 *          enabled to use its clock (see 'MLDLPowerMgmtMPU()').
 *
 * @param   clkSource   Clock source selection.
 *                      Can be one of:
 *                      - CLK_INTERNAL,
 *                      - CLK_PLLGYROX,
 *                      - CLK_PLLGYROY,
 *                      - CLK_PLLGYROZ,
 *                      - CLK_PLLEXT32K, or
 *                      - CLK_PLLEXT19M.
 *
 * @return  Zero if the command is successful; an error code otherwise.
**/
inv_error_t inv_clock_source(unsigned char clkSource)
{
    INVENSENSE_FUNC_START;

    /*---- do range checking ----*/
    if (clkSource >= NUM_CLK_SEL) {
        return INV_ERROR_INVALID_PARAMETER;
    }

    mldlCfg.clk_src = clkSource;
    mldlCfg.gyro_needs_reset = TRUE;

    return INV_SUCCESS;
}

/**
 *  @brief  Set the Temperature Compensation offset.
 *  @param  tc
 *              a pointer to the temperature compensations offset
 *              for the 3 gyro axes.
 *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
 */
inv_error_t inv_set_offsetTC(const unsigned char *tc)
{
    int ii;
    inv_error_t result;

    for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset_tc); ii++) {
        mldlCfg.offset_tc[ii] = tc[ii];
    }

    if (!mldlCfg.gyro_is_suspended) {
#ifdef CONFIG_MPU_SENSORS_MPU3050
        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
                                         MPUREG_XG_OFFS_TC, tc[0]);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
                                         MPUREG_YG_OFFS_TC, tc[1]);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
                                         MPUREG_ZG_OFFS_TC, tc[2]);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
#else
        unsigned char reg;
        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
                                         MPUREG_XG_OFFS_TC,
                                         ((mldlCfg.offset_tc[0] << 1)
                                          & BITS_XG_OFFS_TC));
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        reg = ((mldlCfg.offset_tc[1] << 1) & BITS_YG_OFFS_TC);
#ifdef CONFIG_MPU_SENSORS_MPU6050B1
        if (mldlCfg.pdata->level_shifter)
            reg |= BIT_I2C_MST_VDDIO;
#endif
        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
                                         MPUREG_YG_OFFS_TC, reg);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
                                         MPUREG_ZG_OFFS_TC,
                                         ((mldlCfg.offset_tc[2] << 1)
                                          & BITS_ZG_OFFS_TC));
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
#endif
    } else {
        mldlCfg.gyro_needs_reset = TRUE;
    }
    return INV_SUCCESS;
}

/**
 *  @brief  Set the gyro offset.
 *  @param  offset
 *              a pointer to the gyro offset for the 3 gyro axes. This is scaled
 *              as it would be written to the hardware registers.
 *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
 */
inv_error_t inv_set_offset(const short *offset)
{
    inv_error_t result;
    unsigned char regs[7];
    int ii;
    long sf;

    sf = (2000L * 131) / mldlCfg.gyro_sens_trim;
    for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset); ii++) {
        // Record the bias in the units the register uses
        mldlCfg.offset[ii] = offset[ii];
        // Convert the bias to 1 dps = 1<<16
        inv_obj.gyro_bias[ii] = -offset[ii] * sf;
        regs[1 + ii * 2] = (unsigned char)(offset[ii] >> 8) & 0xff;
        regs[1 + ii * 2 + 1] = (unsigned char)(offset[ii] & 0xff);
    }

    if (!mldlCfg.gyro_is_suspended) {
        regs[0] = MPUREG_X_OFFS_USRH;
        result = inv_serial_write(sMLSLHandle, mldlCfg.addr, 7, regs);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
    } else {
        mldlCfg.gyro_needs_reset = TRUE;
    }
    return INV_SUCCESS;
}

/**
 *  @internal
 *  @brief  used to get the specified number of bytes in the specified MPU
 *          memory bank.
 *          The memory bank is one of the following:
 *          - MPUMEM_RAM_BANK_0,
 *          - MPUMEM_RAM_BANK_1,
 *          - MPUMEM_RAM_BANK_2, or
 *          - MPUMEM_RAM_BANK_3.
 *
 *  @param  bank    Memory bank to write.
 *  @param  memAddr Starting address for write.
 *  @param  length  Number of bytes to write.
 *  @param  buffer  Result for data.
 *
 *  @return zero if the command is successful, an error code otherwise.
 *  @endif
 */
inv_error_t
inv_get_mpu_memory_one_bank(unsigned char bank,
                            unsigned char memAddr,
                            unsigned short length, unsigned char *buffer)
{
    inv_error_t result;

    if ((bank >= MPU_MEM_NUM_RAM_BANKS) ||
        //(memAddr >= MPU_MEM_BANK_SIZE) || always 0, memAddr is an u_char, therefore limited to 255
        ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) {
        return INV_ERROR_INVALID_PARAMETER;
    }

    if (mldlCfg.gyro_is_suspended) {
        memcpy(buffer, &mldlCfg.ram[bank][memAddr], length);
        result = INV_SUCCESS;
    } else {
        result = inv_serial_read_mem(sMLSLHandle, mldlCfg.addr,
                                     ((bank << 8) | memAddr), length, buffer);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
    }

    return result;
}

/**
 *  @internal
 *  @brief  used to set the specified number of bytes in the specified MPU
 *          memory bank.
 *          The memory bank is one of the following:
 *          - MPUMEM_RAM_BANK_0,
 *          - MPUMEM_RAM_BANK_1,
 *          - MPUMEM_RAM_BANK_2, or
 *          - MPUMEM_RAM_BANK_3.
 *
 *  @param  bank    Memory bank to write.
 *  @param  memAddr Starting address for write.
 *  @param  length  Number of bytes to write.
 *  @param  buffer  Result for data.
 *
 *  @return zero if the command is successful, an error code otherwise.
 *  @endif
 */
inv_error_t inv_set_mpu_memory_one_bank(unsigned char bank,
                                        unsigned short memAddr,
                                        unsigned short length,
                                        const unsigned char *buffer)
{
    inv_error_t result = INV_SUCCESS;
    int different;

    if ((bank >= MPU_MEM_NUM_RAM_BANKS) || (memAddr >= MPU_MEM_BANK_SIZE) ||
        ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) {
        return INV_ERROR_INVALID_PARAMETER;
    }

    different = memcmp(&mldlCfg.ram[bank][memAddr], buffer, length);
    memcpy(&mldlCfg.ram[bank][memAddr], buffer, length);
    if (!mldlCfg.gyro_is_suspended) {
        result = inv_serial_write_mem(sMLSLHandle, mldlCfg.addr,
                                      ((bank << 8) | memAddr), length, buffer);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
    } else if (different) {
        mldlCfg.gyro_needs_reset = TRUE;
    }

    return result;
}

/**
 *  @internal
 *  @brief  used to get the specified number of bytes from the MPU location
 *          specified by the key.
 *          Reads the specified number of bytes from the MPU location
 *          specified by the key. Each set of code specifies a function
 *          that changes keys into addresses. This function is set with
 *          setGetAddress().
 *
 *  @param  key     The key to use when looking up the address.
 *  @param  length  Number of bytes to read.
 *  @param  buffer  Result for data.
 *
 *  @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key
 *          not corresponding to a memory address will result in INV_ERROR.
 *  @endif
 */
inv_error_t inv_get_mpu_memory(unsigned short key,
                               unsigned short length, unsigned char *buffer)
{
    unsigned char bank;
    inv_error_t result;
    unsigned short memAddr;

    if (sGetAddress == NULL) {
        return INV_ERROR_NOT_OPENED;
    }

    memAddr = sGetAddress(key);
    if (memAddr >= 0xffff)
        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    bank = memAddr >> 8;        // Get Bank
    memAddr &= 0xff;

    while (memAddr + length > MPU_MEM_BANK_SIZE) {
        // We cross a bank in the middle
        unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr;
        result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr,
                                             sub_length, buffer);
        if (INV_SUCCESS != result)
            return result;
        bank++;
        length -= sub_length;
        buffer += sub_length;
        memAddr = 0;
    }
    result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr,
                                         length, buffer);

    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

    return result;
}

/**
 *  @internal
 *  @brief  used to set the specified number of bytes from the MPU location
 *          specified by the key.
 *          Set the specified number of bytes from the MPU location
 *          specified by the key. Each set of DMP code specifies a function
 *          that changes keys into addresses. This function is set with
 *          setGetAddress().
 *
 *  @param  key     The key to use when looking up the address.
 *  @param  length  Number of bytes to write.
 *  @param  buffer  Result for data.
 *
 *  @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key
 *          not corresponding to a memory address will result in INV_ERROR.
 *  @endif
 */
inv_error_t inv_set_mpu_memory(unsigned short key,
                               unsigned short length,
                               const unsigned char *buffer)
{
    inv_error_t result = INV_SUCCESS;
    unsigned short memAddr;
    unsigned char bank;

    if (sGetAddress == NULL) {
        MPL_LOGE("MLDSetMemoryMPU sGetAddress is NULL\n");
        return INV_ERROR_INVALID_MODULE;
    }
    memAddr = sGetAddress(key);

    if (memAddr >= 0xffff) {
        MPL_LOGE("inv_set_mpu_memory unsupported key\n");
        return INV_ERROR_INVALID_MODULE; // This key not supported
    }

    bank = (unsigned char)(memAddr >> 8);
    memAddr &= 0xff;

    while (memAddr + length > MPU_MEM_BANK_SIZE) {
        // We cross a bank in the middle
        unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr;

        result = inv_set_mpu_memory_one_bank(bank, memAddr, sub_length, buffer);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }

        bank++;
        length -= sub_length;
        buffer += sub_length;
        memAddr = 0;
    }
    result = inv_set_mpu_memory_one_bank(bank, memAddr, length, buffer);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }
    return result;
}

/**
 *  @brief  Load the DMP with the given code and configuration.
 *  @param  buffer
 *              the DMP data.
 *  @param  length
 *              the length in bytes of the DMP data.
 *  @param  config
 *              the DMP configuration.
 *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
 */
inv_error_t inv_load_dmp(const unsigned char *buffer,
                         unsigned short length, unsigned short config)
{
    INVENSENSE_FUNC_START;

    inv_error_t result = INV_SUCCESS;
    unsigned short toWrite;
    unsigned short memAddr = 0;
    localDmpMemory = buffer;
    localDmpMemorySize = length;

    mldlCfg.dmp_cfg1 = (config >> 8);
    mldlCfg.dmp_cfg2 = (config & 0xff);

    while (length > 0) {
        toWrite = length;
        if (toWrite > MAX_LOAD_WRITE_SIZE)
            toWrite = MAX_LOAD_WRITE_SIZE;

        result =
            inv_set_mpu_memory_one_bank(memAddr >> 8, memAddr & 0xff, toWrite,
                                        buffer);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }

        buffer += toWrite;
        memAddr += toWrite;
        length -= toWrite;
    }

    return result;
}

/**
 *  @brief  Get the silicon revision ID.
 *  @return The silicon revision ID
 *          (0 will be read if inv_mpu_open returned an error)
 */
unsigned char inv_get_silicon_rev(void)
{
    return mldlCfg.silicon_revision;
}

/**
 *  @brief  Get the product revision ID.
 *  @return The product revision ID
 *          (0 will be read if inv_mpu_open returned an error)
 */
unsigned char inv_get_product_rev(void)
{
    return mldlCfg.product_revision;
}

/*******************************************************************************
 *******************************************************************************
 *******************************************************************************
 * @todo these belong with an interface to the kernel driver layer
 *******************************************************************************
 *******************************************************************************
 ******************************************************************************/

/**
 * @brief   inv_get_interrupt_status returns the interrupt status from the specified
 *          interrupt pin.
 * @param   intPin
 *              Currently only the value INTPIN_MPU is supported.
 * @param   status
 *              The available statuses are:
 *              - BIT_MPU_RDY_EN
 *              - BIT_DMP_INT_EN
 *              - BIT_RAW_RDY_EN
 *
 * @return  INV_SUCCESS or a non-zero error code.
 */
inv_error_t inv_get_interrupt_status(unsigned char intPin,
                                     unsigned char *status)
{
    INVENSENSE_FUNC_START;

    inv_error_t result;

    switch (intPin) {

    case INTPIN_MPU:
            /*---- return the MPU interrupt status ----*/
        result = inv_serial_read(sMLSLHandle, mldlCfg.addr,
                                 MPUREG_INT_STATUS, 1, status);
        break;

    default:
        result = INV_ERROR_INVALID_PARAMETER;
        break;
    }

    return result;
}

/**
 *  @brief   query the current status of an interrupt source.
 *  @param   srcIndex
 *              index of the interrupt source.
 *              Currently the only source supported is INTPIN_MPU.
 *
 *  @return  1 if the interrupt has been triggered.
 */
unsigned char inv_get_interrupt_trigger(unsigned char srcIndex)
{
    INVENSENSE_FUNC_START;
    return intTrigger[srcIndex];
}

/**
 * @brief clear the 'triggered' status for an interrupt source.
 * @param srcIndex
 *          index of the interrupt source.
 *          Currently only INTPIN_MPU is supported.
 */
void inv_clear_interrupt_trigger(unsigned char srcIndex)
{
    INVENSENSE_FUNC_START;
    intTrigger[srcIndex] = 0;
}

/**
 * @brief   inv_interrupt_handler function should be called when an interrupt is
 *          received.  The source parameter identifies which interrupt source
 *          caused the interrupt. Note that this routine should not be called
 *          directly from the interrupt service routine.
 *
 * @param   intSource   MPU, AUX1, AUX2, or timer. Can be one of: INTSRC_MPU, INTSRC_AUX1,
 *                      INTSRC_AUX2, or INT_SRC_TIMER.
 *
 * @return  Zero if the command is successful; an error code otherwise.
 */
inv_error_t inv_interrupt_handler(unsigned char intSource)
{
    INVENSENSE_FUNC_START;
    /*---- range check ----*/
    if (intSource >= NUM_OF_INTSOURCES) {
        return INV_ERROR;
    }

    /*---- save source of interrupt ----*/
    intTrigger[intSource] = INT_TRIGGERED;

#ifdef ML_USE_DMP_SIM
    if (intSource == INTSRC_AUX1 || intSource == INTSRC_TIMER) {
        MLSimHWDataInput();
    }
#endif

    return INV_SUCCESS;
}

/***************************/
        /**@}*//* end of defgroup */
/***************************/