#include "app_fspi.h"

#include "app_mpu_impl.h"

mpu6500_ret_t app_mpu_impl_read_reg(void *handle, uint8_t reg, uint8_t *data, uint32_t len) {
    uint8_t read_addr = reg | 0x80;

    if(fspi_read_registers(read_addr, data, len) != kStatus_Success) {
        return MPU6500_RET_FAIL;
    }

    return MPU6500_RET_SUCCESS;
}

mpu6500_ret_t app_mpu_impl_write_reg(void *handle, uint8_t reg, uint8_t *data, uint32_t len) {
    uint8_t write_addr = reg & ~(0x80);

    if(fspi_write_registers(write_addr, data, len) != kStatus_Success) {
        return MPU6500_RET_FAIL;
    }

    return MPU6500_RET_SUCCESS;
}

mpu6500_ret_t app_mpu_impl_delay(void *handle, uint32_t msec) {
    SDK_DelayAtLeastUs(msec * 1000, CLOCK_GetCoreSysClkFreq());

    return MPU6500_RET_SUCCESS;
}