/*
 * __________________________________________________________________
 *
 * Copyright (C) [2023] by InvenSense, Inc.
 * 
 * Permission to use, copy, modify, and/or distribute this software
 * for any purpose with or without fee is hereby granted.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
 * WARRANTIES  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
 * WARRANTIES OF MERCHANTABILITY  AND FITNESS. IN NO EVENT SHALL
 * THE AUTHOR BE LIABLE  FOR ANY SPECIAL, DIRECT, INDIRECT, OR
 * CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
 * LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
 * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR
 * IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 * __________________________________________________________________
 */

#include "Invn/Drivers/Ixm42xxx/Ixm42xxxExtFunc.h"
#include "Invn/Drivers/Ixm42xxx/Ixm42xxxDriver_HL_ois.h"

static void inv_ixm42xxx_format_data(const uint8_t endian, const uint8_t *in, uint16_t *out);
static int  inv_ixm42xxx_read_endianess(struct inv_ixm42xxx *s);

int inv_ixm42xxx_init_ois(struct inv_ixm42xxx *s, struct inv_ixm42xxx_serif *serif)
{
	memset(s, 0, sizeof(*s));

	s->transport.serif = *serif;

	inv_ixm42xxx_init_transport(s);

	// fifo is not accessible from aux interface so sensor_event_cb is not relevant
	s->sensor_event_cb = 0;

	/* Gyro power-off to power-on transition can cause ring down issue
	 * This variable keeps track of timestamp when gyro is power off. Set to UINT32_MAX at init
	 */
	s->gyro_power_off_tmst = UINT32_MAX;

	return 0;
}

int inv_ixm42xxx_get_who_am_i_ois(struct inv_ixm42xxx *s, uint8_t *who_am_i)
{
	return inv_ixm42xxx_read_reg(s, MPUREG_WHO_AM_I, 1, who_am_i);
}

int inv_ixm42xxx_enable_gyro_ois1(struct inv_ixm42xxx *s)
{
	int      status = 0;
	uint64_t current_time;
	uint8_t  int_status_ois, ois2_config1_reg, data;

	/* Powering the gyroscope on immediately after powering it off can result in device failure. 
	 * The gyroscope proof mass can continue vibrating after it has been powered off, 
	 * and powering it back on immediately can result in unpredictable proof mass movement.
	 * After powering the gyroscope off, a period of > 150ms should be allowed to elapse before it is powered back on. */
	status |= inv_ixm42xxx_read_reg(s, MPUREG_OIS2_CONFIG2_B2, 1, &ois2_config1_reg);

	if ((s->gyro_power_off_tmst != UINT32_MAX) &&
	    !(ois2_config1_reg & BIT_OIS2_CONFIG1_GYRO_EN_MASK)) {
		current_time = inv_ixm42xxx_get_time_us();
		/* Handle rollover */
		if (current_time <= s->gyro_power_off_tmst)
			current_time += UINT32_MAX;
		/* If 150 ms are not elapsed since power-off error is returned */
		if ((current_time - s->gyro_power_off_tmst) <= (150 * 1000))
			return INV_ERROR_HW;
	}

	status |= inv_ixm42xxx_read_endianess(s);

	/* Write ois1_accel_lp_clk_sel = 1 */
	status |= inv_ixm42xxx_read_reg(s, MPUREG_OIS1_CONFIG1_B2, 1, &data);
	data |= IXM42XXX_OIS1_CONFIG1_ACCEL_LP_CLK_SEL_RCOSC;
	status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS1_CONFIG1_B2, 1, &data);

	inv_ixm42xxx_sleep_us(80000U);

	/* Enable gyro ois1 */
	data |= IXM42XXX_OIS1_CONFIG1_GYRO_EN;
	status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS1_CONFIG1_B2, 1, &data);

	/* Check if Gyro UI is already on and do
	a 60ms sleep to discard Gyro sample the first 60 ms*/
	inv_ixm42xxx_sleep_us(60000U);

	do {
		status |= inv_ixm42xxx_read_reg(s, MPUREG_INT_STATUS_OIS1_B2, 1, &int_status_ois);
	} while ((status == 0) && !int_status_ois);

	return status;
}

int inv_ixm42xxx_enable_gyro_ois2(struct inv_ixm42xxx *s)
{
	int      status = 0;
	uint64_t current_time;
	uint8_t  int_status_ois, ois1_config1_reg, data;

	/* Powering the gyroscope on immediately after powering it off can result in device failure. 
	 * The gyroscope proof mass can continue vibrating after it has been powered off, 
	 * and powering it back on immediately can result in unpredictable proof mass movement.
	 * After powering the gyroscope off, a period of > 150ms should be allowed to elapse before it is powered back on. */
	status |= inv_ixm42xxx_read_reg(s, MPUREG_OIS1_CONFIG1_B2, 1, &ois1_config1_reg);

	if ((s->gyro_power_off_tmst != UINT32_MAX) &&
	    !(ois1_config1_reg & BIT_OIS1_CONFIG1_GYRO_EN_MASK)) {
		current_time = inv_ixm42xxx_get_time_us();
		/* Handle rollover */
		if (current_time <= s->gyro_power_off_tmst)
			current_time += UINT32_MAX;
		/* If 150 ms are not elapsed since power-off error is returned */
		if ((current_time - s->gyro_power_off_tmst) <= (150 * 1000))
			return INV_ERROR;
	}

	status |= inv_ixm42xxx_read_endianess(s);

	/* Write ois2_accel_lp_clk_sel = 1 */
	status |= inv_ixm42xxx_read_reg(s, MPUREG_OIS2_CONFIG1_B2, 1, &data);
	data |= IXM42XXX_OIS2_CONFIG1_ACCEL_LP_CLK_SEL_RCOSC;
	status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS2_CONFIG1_B2, 1, &data);

	inv_ixm42xxx_sleep_us(80000U);

	/* Enable gyro ois2 */
	data |= IXM42XXX_OIS2_CONFIG1_GYRO_EN;
	status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS2_CONFIG1_B2, 1, &data);

	/* Check if Gyro UI is already on and do
	a 60ms sleep to discard Gyro sample the first 60 ms*/
	inv_ixm42xxx_sleep_us(60000U);

	do {
		status |= inv_ixm42xxx_read_reg(s, MPUREG_INT_STATUS_OIS2_B2, 1, &int_status_ois);
	} while ((status == 0) && !int_status_ois);

	return status;
}

int inv_ixm42xxx_enable_accel_ois1(struct inv_ixm42xxx *s)
{
	int     status = 0;
	uint8_t int_status_ois, data;

	status |= inv_ixm42xxx_read_endianess(s);

	/* Write ois1_accel_lp_clk_sel = 1 */
	status |= inv_ixm42xxx_read_reg(s, MPUREG_OIS1_CONFIG1_B2, 1, &data);
	data |= IXM42XXX_OIS1_CONFIG1_ACCEL_LP_CLK_SEL_RCOSC;
	status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS1_CONFIG1_B2, 1, &data);

	inv_ixm42xxx_sleep_us(80000U);

	/* Enable accel ois1 */
	data |= IXM42XXX_OIS1_CONFIG1_ACCEL_EN;
	status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS1_CONFIG1_B2, 1, &data);

	do {
		status |= inv_ixm42xxx_read_reg(s, MPUREG_INT_STATUS_OIS1_B2, 1, &int_status_ois);
	} while ((status == 0) && !int_status_ois);

	return status;
}

int inv_ixm42xxx_enable_accel_ois2(struct inv_ixm42xxx *s)
{
	int     status = 0;
	uint8_t int_status_ois, data;

	status |= inv_ixm42xxx_read_endianess(s);

	/* Write ois2_accel_lp_clk_sel = 1 */
	status |= inv_ixm42xxx_read_reg(s, MPUREG_OIS2_CONFIG1_B2, 1, &data);
	data |= IXM42XXX_OIS2_CONFIG1_ACCEL_LP_CLK_SEL_RCOSC;
	status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS2_CONFIG1_B2, 1, &data);

	inv_ixm42xxx_sleep_us(80000U);

	/* Enable accel ois2 */
	data |= IXM42XXX_OIS2_CONFIG1_ACCEL_EN;
	status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS2_CONFIG1_B2, 1, &data);

	do {
		status |= inv_ixm42xxx_read_reg(s, MPUREG_INT_STATUS_OIS2_B2, 1, &int_status_ois);
	} while ((status == 0) && !int_status_ois);

	return status;
}

int inv_ixm42xxx_disable_gyro_ois1(struct inv_ixm42xxx *s)
{
	int     status = 0;
	uint8_t ois1_config1_reg;

	/* Disable gyro AUX1 */
	status |= inv_ixm42xxx_read_reg(s, MPUREG_OIS1_CONFIG1_B2, 1, &ois1_config1_reg);
	ois1_config1_reg &= (uint8_t)~BIT_OIS1_CONFIG1_GYRO_EN_MASK; // IXM42XXX_OIS1_CONFIG1_GYRO_DIS
	status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS1_CONFIG1_B2, 1, &ois1_config1_reg);

	/* keep track of gyro power-off time to check if gyro will be power-on after more than 150ms*/
	s->gyro_power_off_tmst = inv_ixm42xxx_get_time_us();

	/* Handle OIS1 accel state */
	if ((ois1_config1_reg & BIT_OIS1_CONFIG1_ACCEL_EN_MASK) == IXM42XXX_OIS1_CONFIG1_ACCEL_DIS) {
		/* Wait for 2 accel ODR */
		inv_ixm42xxx_sleep_us(160000U);

		/* Write ois1/2_accel_lp_clk_sel = 0*/
		ois1_config1_reg &=
		    (uint8_t)~BIT_OIS1_CONFIG1_ACCEL_LP_CLK_SEL_MASK; // == IXM42XXX_OIS1_CONFIG1_ACCEL_LP_CLK_SEL_WUOSC
		status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS1_CONFIG1_B2, 1, &ois1_config1_reg);
	}

	return status;
}

int inv_ixm42xxx_disable_gyro_ois2(struct inv_ixm42xxx *s)
{
	int     status = 0;
	uint8_t ois2_config1_reg;

	/* Disable gyro AUX2 */
	status |= inv_ixm42xxx_read_reg(s, MPUREG_OIS2_CONFIG1_B2, 1, &ois2_config1_reg);
	ois2_config1_reg &= (uint8_t)~BIT_OIS2_CONFIG1_GYRO_EN_MASK; // IXM42XXX_OIS2_CONFIG1_GYRO_DIS
	status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS2_CONFIG1_B2, 1, &ois2_config1_reg);

	/* keep track of gyro power-off time to check if gyro will be power-on after more than 150ms*/
	s->gyro_power_off_tmst = inv_ixm42xxx_get_time_us();

	/* Handle OIS2 accel state */
	if ((ois2_config1_reg & BIT_OIS2_CONFIG1_ACCEL_EN_MASK) == IXM42XXX_OIS2_CONFIG1_ACCEL_DIS) {
		/* Wait for 2 accel ODR */
		inv_ixm42xxx_sleep_us(160000U);

		/* Write ois2/2_accel_lp_clk_sel = 0*/
		ois2_config1_reg &=
		    (uint8_t)~BIT_OIS2_CONFIG1_ACCEL_LP_CLK_SEL_MASK; // == IXM42XXX_OIS2_CONFIG1_ACCEL_LP_CLK_SEL_WUOSC
		status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS2_CONFIG1_B2, 1, &ois2_config1_reg);
	}

	return status;
}

int inv_ixm42xxx_disable_accel_ois1(struct inv_ixm42xxx *s)
{
	int     status = 0;
	uint8_t ois1_config1_reg;

	/* Disable accel AUX1 */
	status |= inv_ixm42xxx_read_reg(s, MPUREG_OIS1_CONFIG1_B2, 1, &ois1_config1_reg);
	ois1_config1_reg &= (uint8_t)~BIT_OIS1_CONFIG1_ACCEL_EN_MASK; // IXM42XXX_OIS1_CONFIG1_ACCEL_DIS
	status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS1_CONFIG1_B2, 1, &ois1_config1_reg);

	/* Handle OIS1 gyro state */
	if ((ois1_config1_reg & BIT_OIS1_CONFIG1_GYRO_EN_MASK) == IXM42XXX_OIS1_CONFIG1_GYRO_DIS) {
		/* Wait for 2 accel ODR */
		inv_ixm42xxx_sleep_us(160000U);

		/* Write ois1/2_accel_lp_clk_sel = 0*/
		ois1_config1_reg &=
		    (uint8_t)~BIT_OIS1_CONFIG1_ACCEL_LP_CLK_SEL_MASK; // == IXM42XXX_OIS1_CONFIG1_ACCEL_LP_CLK_SEL_WUOSC
		status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS1_CONFIG1_B2, 1, &ois1_config1_reg);
	}

	return status;
}

int inv_ixm42xxx_disable_accel_ois2(struct inv_ixm42xxx *s)
{
	int     status = 0;
	uint8_t ois2_config1_reg;

	/* Disable accel AUX2 */
	status |= inv_ixm42xxx_read_reg(s, MPUREG_OIS2_CONFIG1_B2, 1, &ois2_config1_reg);
	ois2_config1_reg &= (uint8_t)~BIT_OIS2_CONFIG1_ACCEL_EN_MASK; // IXM42XXX_OIS2_CONFIG1_ACCEL_DIS
	status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS2_CONFIG1_B2, 1, &ois2_config1_reg);

	/* Handle OIS2 gyro state */
	if ((ois2_config1_reg & BIT_OIS2_CONFIG1_GYRO_EN_MASK) == IXM42XXX_OIS2_CONFIG1_GYRO_DIS) {
		/* Wait for 2 accel ODR */
		inv_ixm42xxx_sleep_us(160000U);

		/* Write ois1/2_accel_lp_clk_sel = 0*/
		ois2_config1_reg &=
		    (uint8_t)~BIT_OIS2_CONFIG1_ACCEL_LP_CLK_SEL_MASK; // == IXM42XXX_OIS2_CONFIG1_ACCEL_LP_CLK_SEL_WUOSC
		status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS2_CONFIG1_B2, 1, &ois2_config1_reg);
	}

	return status;
}

int inv_ixm42xxx_set_gyro_fsr_ois1(struct inv_ixm42xxx *               s,
                                   IXM42XXX_OIS1_CONFIG2_GYRO_FS_SEL_t gyro_fsr_dps)
{
	uint8_t data;
	int     status = inv_ixm42xxx_read_reg(s, MPUREG_OIS1_CONFIG2_B2, 1, &data);
	data &= (uint8_t)~BIT_OIS1_CONFIG2_GYRO_FS_SEL_MASK;
	data |= (uint8_t)gyro_fsr_dps;
	status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS1_CONFIG2_B2, 1, &data);
	return status;
}

int inv_ixm42xxx_set_gyro_fsr_ois2(struct inv_ixm42xxx *               s,
                                   IXM42XXX_OIS2_CONFIG2_GYRO_FS_SEL_t gyro_fsr_dps)
{
	uint8_t data;
	int     status = inv_ixm42xxx_read_reg(s, MPUREG_OIS2_CONFIG2_B2, 1, &data);
	data &= (uint8_t)~BIT_OIS2_CONFIG2_GYRO_FS_SEL_MASK;
	data |= (uint8_t)gyro_fsr_dps;
	status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS2_CONFIG2_B2, 1, &data);
	return status;
}

int inv_ixm42xxx_set_accel_fsr_ois1(struct inv_ixm42xxx *                s,
                                    IXM42XXX_OIS1_CONFIG2_ACCEL_FS_SEL_t accel_fsr_g)
{
	uint8_t data;
	int     status = inv_ixm42xxx_read_reg(s, MPUREG_OIS1_CONFIG2_B2, 1, &data);
	data &= (uint8_t)~BIT_OIS1_CONFIG2_ACCEL_FS_SEL_MASK;
	data |= (uint8_t)accel_fsr_g;
	status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS1_CONFIG2_B2, 1, &data);
	return status;
}

int inv_ixm42xxx_set_accel_fsr_ois2(struct inv_ixm42xxx *                s,
                                    IXM42XXX_OIS2_CONFIG2_ACCEL_FS_SEL_t accel_fsr_g)
{
	uint8_t data;
	int     status = inv_ixm42xxx_read_reg(s, MPUREG_OIS2_CONFIG2_B2, 1, &data);
	data &= (uint8_t)~BIT_OIS2_CONFIG2_ACCEL_FS_SEL_MASK;
	data |= (uint8_t)accel_fsr_g;
	status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS2_CONFIG2_B2, 1, &data);
	return status;
}

int inv_ixm42xxx_set_ois1_decimator(struct inv_ixm42xxx *             s,
                                    const IXM42XXX_OIS1_CONFIG1_DEC_t decimator)
{
	uint8_t data;
	int     status = inv_ixm42xxx_read_reg(s, MPUREG_OIS1_CONFIG1_B2, 1, &data);
	data &= (uint8_t)~BIT_OIS1_CONFIG1_DEC_MASK;
	data |= (uint8_t)decimator;
	status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS1_CONFIG1_B2, 1, &data);
	return status;
}

int inv_ixm42xxx_set_ois2_decimator(struct inv_ixm42xxx *             s,
                                    const IXM42XXX_OIS2_CONFIG1_DEC_t decimator)
{
	uint8_t data;
	int     status = inv_ixm42xxx_read_reg(s, MPUREG_OIS2_CONFIG1_B2, 1, &data);
	data &= (uint8_t)~BIT_OIS2_CONFIG1_DEC_MASK;
	data |= (uint8_t)decimator;
	status |= inv_ixm42xxx_write_reg(s, MPUREG_OIS2_CONFIG1_B2, 1, &data);
	return status;
}

int inv_ixm42xxx_get_gyro_data_ois1(struct inv_ixm42xxx *s, int16_t gyro_data[3])
{
	uint8_t data[6];

	if (inv_ixm42xxx_read_reg(s, MPUREG_GYRO_DATA_X0_OIS1_B2, GYRO_DATA_SIZE, data) == 0) {
		inv_ixm42xxx_format_data(s->endianess_data, &data[0], (uint16_t *)&gyro_data[0]);
		inv_ixm42xxx_format_data(s->endianess_data, &data[2], (uint16_t *)&gyro_data[1]);
		inv_ixm42xxx_format_data(s->endianess_data, &data[4], (uint16_t *)&gyro_data[2]);
		if (gyro_data[0] != INVALID_VALUE_FIFO && gyro_data[1] != INVALID_VALUE_FIFO &&
		    gyro_data[2] != INVALID_VALUE_FIFO)
			return 0;
	}

	return -1;
}

int inv_ixm42xxx_get_gyro_data_ois2(struct inv_ixm42xxx *s, int16_t gyro_data[3])
{
	uint8_t data[6];

	if (inv_ixm42xxx_read_reg(s, MPUREG_GYRO_DATA_X0_OIS2_B2, GYRO_DATA_SIZE, data) == 0) {
		inv_ixm42xxx_format_data(s->endianess_data, &data[0], (uint16_t *)&gyro_data[0]);
		inv_ixm42xxx_format_data(s->endianess_data, &data[2], (uint16_t *)&gyro_data[1]);
		inv_ixm42xxx_format_data(s->endianess_data, &data[4], (uint16_t *)&gyro_data[2]);
		if (gyro_data[0] != INVALID_VALUE_FIFO && gyro_data[1] != INVALID_VALUE_FIFO &&
		    gyro_data[2] != INVALID_VALUE_FIFO)
			return 0;
	}

	return -1;
}

int inv_ixm42xxx_get_accel_data_ois1(struct inv_ixm42xxx *s, int16_t accel_data[3])
{
	uint8_t data[6];

	if (inv_ixm42xxx_read_reg(s, MPUREG_ACCEL_DATA_X0_OIS1_B2, ACCEL_DATA_SIZE, data) == 0) {
		inv_ixm42xxx_format_data(s->endianess_data, &data[0], (uint16_t *)&accel_data[0]);
		inv_ixm42xxx_format_data(s->endianess_data, &data[2], (uint16_t *)&accel_data[1]);
		inv_ixm42xxx_format_data(s->endianess_data, &data[4], (uint16_t *)&accel_data[2]);
		if (accel_data[0] != INVALID_VALUE_FIFO && accel_data[1] != INVALID_VALUE_FIFO &&
		    accel_data[2] != INVALID_VALUE_FIFO)
			return 0;
	}

	return -1;
}

int inv_ixm42xxx_get_accel_data_ois2(struct inv_ixm42xxx *s, int16_t accel_data[3])
{
	uint8_t data[6];

	if (inv_ixm42xxx_read_reg(s, MPUREG_ACCEL_DATA_X0_OIS2_B2, ACCEL_DATA_SIZE, data) == 0) {
		inv_ixm42xxx_format_data(s->endianess_data, &data[0], (uint16_t *)&accel_data[0]);
		inv_ixm42xxx_format_data(s->endianess_data, &data[2], (uint16_t *)&accel_data[1]);
		inv_ixm42xxx_format_data(s->endianess_data, &data[4], (uint16_t *)&accel_data[2]);
		if (accel_data[0] != INVALID_VALUE_FIFO && accel_data[1] != INVALID_VALUE_FIFO &&
		    accel_data[2] != INVALID_VALUE_FIFO)
			return 0;
	}

	return -1;
}

int inv_ixm42xxx_get_accel_and_gyro_data_ois1(struct inv_ixm42xxx *s, int16_t accel_data[3],
                                              int16_t gyro_data[3])
{
	int     rc = 0;
	uint8_t data[ACCEL_DATA_SIZE + GYRO_DATA_SIZE];

	if (inv_ixm42xxx_read_reg(s, MPUREG_ACCEL_DATA_X0_OIS1_B2, ACCEL_DATA_SIZE + GYRO_DATA_SIZE,
	                          data) == 0) {
		inv_ixm42xxx_format_data(s->endianess_data, &data[0], (uint16_t *)&accel_data[0]);
		inv_ixm42xxx_format_data(s->endianess_data, &data[2], (uint16_t *)&accel_data[1]);
		inv_ixm42xxx_format_data(s->endianess_data, &data[4], (uint16_t *)&accel_data[2]);
		if (accel_data[0] == INVALID_VALUE_FIFO || accel_data[1] == INVALID_VALUE_FIFO ||
		    accel_data[2] == INVALID_VALUE_FIFO)
			rc = -1;

		inv_ixm42xxx_format_data(s->endianess_data, &data[6], (uint16_t *)&gyro_data[0]);
		inv_ixm42xxx_format_data(s->endianess_data, &data[8], (uint16_t *)&gyro_data[1]);
		inv_ixm42xxx_format_data(s->endianess_data, &data[10], (uint16_t *)&gyro_data[2]);
		if (gyro_data[0] == INVALID_VALUE_FIFO || gyro_data[1] == INVALID_VALUE_FIFO ||
		    gyro_data[2] == INVALID_VALUE_FIFO)
			rc = -1;
	}

	return rc;
}

int inv_ixm42xxx_get_accel_and_gyro_data_ois2(struct inv_ixm42xxx *s, int16_t accel_data[3],
                                              int16_t gyro_data[3])
{
	int     rc = 0;
	uint8_t data[ACCEL_DATA_SIZE + GYRO_DATA_SIZE];

	if (inv_ixm42xxx_read_reg(s, MPUREG_ACCEL_DATA_X0_OIS2_B2, ACCEL_DATA_SIZE + GYRO_DATA_SIZE,
	                          data) == 0) {
		inv_ixm42xxx_format_data(s->endianess_data, &data[0], (uint16_t *)&accel_data[0]);
		inv_ixm42xxx_format_data(s->endianess_data, &data[2], (uint16_t *)&accel_data[1]);
		inv_ixm42xxx_format_data(s->endianess_data, &data[4], (uint16_t *)&accel_data[2]);
		if (accel_data[0] == INVALID_VALUE_FIFO || accel_data[1] == INVALID_VALUE_FIFO ||
		    accel_data[2] == INVALID_VALUE_FIFO)
			rc = -1;

		inv_ixm42xxx_format_data(s->endianess_data, &data[6], (uint16_t *)&gyro_data[0]);
		inv_ixm42xxx_format_data(s->endianess_data, &data[8], (uint16_t *)&gyro_data[1]);
		inv_ixm42xxx_format_data(s->endianess_data, &data[10], (uint16_t *)&gyro_data[2]);
		if (gyro_data[0] == INVALID_VALUE_FIFO || gyro_data[1] == INVALID_VALUE_FIFO ||
		    gyro_data[2] == INVALID_VALUE_FIFO)
			rc = -1;
	}

	return rc;
}

/* Default implementation converts ICM endian to little endian */
static void inv_ixm42xxx_format_data(const uint8_t endian, const uint8_t *in, uint16_t *out)
{
	if (endian == IXM42XXX_INTF_CONFIG0_DATA_BIG_ENDIAN)
		*out = (((uint16_t)in[0]) << 8) | in[1];
	else
		*out = (((uint16_t)in[1]) << 8) | in[0];
}

static int inv_ixm42xxx_read_endianess(struct inv_ixm42xxx *s)
{
	uint8_t data;
	int     status = 0;

	/* Read data endianess in order to poll ois data correctly */
	status |= inv_ixm42xxx_set_reg_bank(s, 0);
	status |= inv_ixm42xxx_read_reg(s, MPUREG_INTF_CONFIG0, 1, &data);
	s->endianess_data = (data & BIT_DATA_ENDIAN_MASK);
	status |= inv_ixm42xxx_set_reg_bank(s, 2);

	return status;
}
