path: root/zephyr/emul/emul_bmi.c

/* Copyright 2021 The Chromium OS Authors. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */

#define DT_DRV_COMPAT zephyr_bmi

#define LOG_LEVEL CONFIG_I2C_LOG_LEVEL
#include <logging/log.h>
LOG_MODULE_REGISTER(emul_bmi);

#include <device.h>
#include <emul.h>
#include <drivers/i2c.h>
#include <drivers/i2c_emul.h>

#include "emul/emul_common_i2c.h"
#include "emul/emul_bmi.h"

#include "driver/accelgyro_bmi160.h"
#include "driver/accelgyro_bmi260.h"
#include "driver/accelgyro_bmi_common.h"

#define BMI_DATA_FROM_I2C_EMUL(_emul)					     \
	CONTAINER_OF(CONTAINER_OF(_emul, struct i2c_common_emul_data, emul), \
		     struct bmi_emul_data, common)

/** Run-time data used by the emulator */
struct bmi_emul_data {
	/** Common I2C data */
	struct i2c_common_emul_data common;

	/** Current state of all emulated BMI registers */
	uint8_t reg[BMI_EMUL_MAX_REG];
	/** Internal offset values used in calculations */
	int16_t off_acc_x;
	int16_t off_acc_y;
	int16_t off_acc_z;
	int16_t off_gyr_x;
	int16_t off_gyr_y;
	int16_t off_gyr_z;
	/** Internal values of sensors */
	int32_t acc_x;
	int32_t acc_y;
	int32_t acc_z;
	int32_t gyr_x;
	int32_t gyr_y;
	int32_t gyr_z;
	/** Current state of NVM where offset and configuration can be saved */
	uint8_t nvm[BMI_EMUL_MAX_NVM_REGS];

	/** Return error when trying to write to RO register */
	bool error_on_ro_write;
	/** Return error when trying to write 1 to reserved bit */
	bool error_on_rsvd_write;
	/**
	 * If effect of command is vissable after simulated time from issuing
	 * command
	 */
	bool simulate_command_exec_time;
	/** Return error when trying to read WO register */
	bool error_on_wo_read;

	/** Value of data byte in ongoing write message */
	uint8_t write_byte;

	/** List of FIFO frames */
	struct bmi_emul_frame *fifo_frame;
	/** First FIFO frame in byte format */
	uint8_t fifo[21];
	/** Number of FIFO frames that were skipped */
	uint8_t fifo_skip;
	/** Currently accessed byte of first frame */
	int fifo_frame_byte;
	/** Length of first frame */
	int fifo_frame_len;

	/** Last time when emulator was resetted in sensor time units */
	int64_t zero_time;
	/** Time when current command should end */
	uint32_t cmd_end_time;

	/** Emulated model of BMI */
	int type;
	/** Pointer to data specific for emulated model of BMI */
	const struct bmi_emul_type_data *type_data;
};

/** Check description in emul_bmi.h */
void bmi_emul_set_reg(struct i2c_emul *emul, int reg, uint8_t val)
{
	struct bmi_emul_data *data;

	if (reg < 0 || reg > BMI_EMUL_MAX_REG) {
		return;
	}

	data = BMI_DATA_FROM_I2C_EMUL(emul);
	data->reg[reg] = val;
}

/** Check description in emul_bmi.h */
uint8_t bmi_emul_get_reg(struct i2c_emul *emul, int reg)
{
	struct bmi_emul_data *data;

	if (reg < 0 || reg > BMI_EMUL_MAX_REG) {
		return 0;
	}

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	return data->reg[reg];
}

/**
 * @brief Convert @p val to two's complement representation. It makes sure that
 *        bit representation is correct even on platforms which represent
 *        signed inteager in different format. Unsigned bit representation
 *        allows to use well defined bitwise operations on returned value.
 *
 * @param val Inteager that is converted
 *
 * @return two's complement representation of @p val
 */
static uint32_t bmi_emul_val_to_twos_comp(int32_t val)
{
	uint32_t twos_comp_val;

	/* Make sure that value is converted to twos compliment format */
	if (val < 0) {
		twos_comp_val = (uint32_t)(-val);
		twos_comp_val = ~twos_comp_val + 1;
	} else {
		twos_comp_val = (uint32_t)val;
	}

	return twos_comp_val;
}

/**
 * @brief Convert accelerometer value from NVM format (8bit, 0x01 == 3.9mg)
 *        to internal offset format (16bit, 0x01 == 0.061mg).
 *
 * @param nvm Value in NVM format (8bit, 0x01 == 3.9mg). This is binary
 *            representation of two's complement signed number.
 *
 * @return offset Internal representation of @p nvm (16bit, 0x01 == 0.061mg)
 */
static int16_t bmi_emul_acc_nvm_to_off(uint8_t nvm)
{
	int16_t offset;
	int8_t sign;

	if (nvm & BIT(7)) {
		sign = -1;
		/* NVM value is in two's complement format */
		nvm = ~nvm + 1;
	} else {
		sign = 1;
	}

	offset = (int16_t)nvm;
	/* LSB in NVM is 3.9mg, while LSB in internal offset is 0.061mg */
	offset *= sign * 64;

	return offset;
}

/**
 * @brief Convert gyroscope value from NVM format (10bit, 0x01 == 0.061 °/s)
 *        to internal offset format (16bit, 0x01 == 0.0038 °/s)
 *
 * @param nvm Value in NVM format (10bit, 0x01 == 0.061 °/s). This is binary
 *            representation of two's complement signed number.
 *
 * @return offset Internal representation of @p nvm (16bit, 0x01 == 0.0038 °/s)
 */
static int16_t bmi_emul_gyr_nvm_to_off(uint16_t nvm)
{
	int16_t offset;
	int8_t sign;

	if (nvm & BIT(9)) {
		sign = -1;
		/* NVM value is in two's complement format */
		nvm = ~nvm + 1;
	} else {
		sign = 1;
	}

	/* Mask 10 bits which holds value */
	nvm &= 0x3ff;

	offset = (int16_t)nvm;
	/* LSB in NVM is 0.061°/s, while LSB in internal offset is 0.0038°/s */
	offset *= sign * 16;

	return offset;
}

/**
 * @brief Convert accelerometer value from internal offset format
 *        (16bit, 0x01 == 0.061mg) to NVM format (8bit, 0x01 == 7.8mg).
 *        Function makes sure that NVM value is representation of two's
 *        complement signed number.
 *
 * @param val Value in internal offset format (16bit, 0x01 == 0.061mg).
 *
 * @return nvm NVM format representation of @p val (8bit, 0x01 == 3.9mg)
 */
static uint8_t bmi_emul_acc_off_to_nvm(int16_t off)
{
	uint32_t twos_comp_val;
	uint8_t nvm = 0;

	twos_comp_val = bmi_emul_val_to_twos_comp(off);

	/*
	 * LSB in internal representation has value 0.061mg, while in NVM
	 * LSB is 3.9mg. Skip 0.06mg, 0.12mg, 0.24mg, 0.48mg, 0.97mg and
	 * 1.9mg bits.
	 */
	nvm |= (twos_comp_val >> 6) & 0x7f;
	/* Set sign bit */
	nvm |= (twos_comp_val & BIT(31)) ? BIT(7) : 0x00;

	return nvm;
}

/**
 * @brief Convert gyroscope value from internal offset format
 *        (16bit, 0x01 == 0.0038°/s) to NVM format (10bit, 0x01 == 0.061°/s).
 *        Function makes sure that NVM value is representation of two's
 *        complement signed number.
 *
 * @param val Value in internal offset format (16bit, 0x01 == 0.0038°/s).
 *
 * @return nvm NVM format representation of @p val (10bit, 0x01 == 0.061°/s)
 */
static uint16_t bmi_emul_gyr_off_to_nvm(int16_t off)
{
	uint32_t twos_comp_val;
	uint16_t nvm = 0;

	twos_comp_val = bmi_emul_val_to_twos_comp(off);

	/*
	 * LSB in internal representation has value 0.0038°/s, while in NVM
	 * LSB is 0.061°/s. Skip 0.0038°/s, 0.0076°/s, 0.015°/s, and
	 * 0.03°/s bits.
	 */
	nvm |= (twos_comp_val >> 4) & 0x1ff;
	/* Set sign bit */
	nvm |= (twos_comp_val & BIT(31)) ? BIT(9) : 0x00;

	return nvm;
}

/** Check description in emul_bmi.h */
int16_t bmi_emul_get_off(struct i2c_emul *emul, enum bmi_emul_axis axis)
{
	struct bmi_emul_data *data;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	switch (axis) {
	case BMI_EMUL_ACC_X:
		return data->off_acc_x;
	case BMI_EMUL_ACC_Y:
		return data->off_acc_y;
	case BMI_EMUL_ACC_Z:
		return data->off_acc_z;
	case BMI_EMUL_GYR_X:
		return data->off_gyr_x;
	case BMI_EMUL_GYR_Y:
		return data->off_gyr_y;
	case BMI_EMUL_GYR_Z:
		return data->off_gyr_z;
	}

	return 0;
}

/** Check description in emul_bmi.h */
void bmi_emul_set_off(struct i2c_emul *emul, enum bmi_emul_axis axis,
		      int16_t val)
{
	struct bmi_emul_data *data;
	uint16_t gyr_off;
	uint8_t gyr98_shift;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	switch (axis) {
	case BMI_EMUL_ACC_X:
		data->off_acc_x = val;
		data->reg[data->type_data->acc_off_reg] =
				bmi_emul_acc_off_to_nvm(data->off_acc_x);
		break;
	case BMI_EMUL_ACC_Y:
		data->off_acc_y = val;
		data->reg[data->type_data->acc_off_reg + 1] =
				bmi_emul_acc_off_to_nvm(data->off_acc_y);
		break;
	case BMI_EMUL_ACC_Z:
		data->off_acc_z = val;
		data->reg[data->type_data->acc_off_reg + 2] =
				bmi_emul_acc_off_to_nvm(data->off_acc_z);
		break;
	case BMI_EMUL_GYR_X:
		data->off_gyr_x = val;
		gyr_off = bmi_emul_gyr_off_to_nvm(data->off_gyr_x);
		data->reg[data->type_data->gyr_off_reg] = gyr_off & 0xff;
		gyr98_shift = 0;
		data->reg[data->type_data->gyr98_off_reg] &=
				~(0x3 << gyr98_shift);
		data->reg[data->type_data->gyr98_off_reg] |=
				(gyr_off & 0x300) >> (8 - gyr98_shift);
		break;
	case BMI_EMUL_GYR_Y:
		data->off_gyr_y = val;
		gyr_off = bmi_emul_gyr_off_to_nvm(data->off_gyr_y);
		data->reg[data->type_data->gyr_off_reg + 1] = gyr_off & 0xff;
		gyr98_shift = 2;
		data->reg[data->type_data->gyr98_off_reg] &=
				~(0x3 << gyr98_shift);
		data->reg[data->type_data->gyr98_off_reg] |=
				(gyr_off & 0x300) >> (8 - gyr98_shift);
		break;
	case BMI_EMUL_GYR_Z:
		data->off_gyr_z = val;
		gyr_off = bmi_emul_gyr_off_to_nvm(data->off_gyr_z);
		data->reg[data->type_data->gyr_off_reg + 2] = gyr_off & 0xff;
		gyr98_shift = 4;
		data->reg[data->type_data->gyr98_off_reg] &=
				~(0x3 << gyr98_shift);
		data->reg[data->type_data->gyr98_off_reg] |=
				(gyr_off & 0x300) >> (8 - gyr98_shift);
		break;
	}
}

/** Check description in emul_bmi.h */
int32_t bmi_emul_get_value(struct i2c_emul *emul, enum bmi_emul_axis axis)
{
	struct bmi_emul_data *data;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	switch (axis) {
	case BMI_EMUL_ACC_X:
		return data->acc_x;
	case BMI_EMUL_ACC_Y:
		return data->acc_y;
	case BMI_EMUL_ACC_Z:
		return data->acc_z;
	case BMI_EMUL_GYR_X:
		return data->gyr_x;
	case BMI_EMUL_GYR_Y:
		return data->gyr_y;
	case BMI_EMUL_GYR_Z:
		return data->gyr_z;
	}

	return 0;
}

/** Check description in emul_bmi.h */
void bmi_emul_set_value(struct i2c_emul *emul, enum bmi_emul_axis axis,
			int32_t val)
{
	struct bmi_emul_data *data;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	switch (axis) {
	case BMI_EMUL_ACC_X:
		data->acc_x = val;
		break;
	case BMI_EMUL_ACC_Y:
		data->acc_y = val;
		break;
	case BMI_EMUL_ACC_Z:
		data->acc_z = val;
		break;
	case BMI_EMUL_GYR_X:
		data->gyr_x = val;
		break;
	case BMI_EMUL_GYR_Y:
		data->gyr_y = val;
		break;
	case BMI_EMUL_GYR_Z:
		data->gyr_z = val;
		break;
	}
}

/** Check description in emul_bmi.h */
void bmi_emul_set_err_on_ro_write(struct i2c_emul *emul, bool set)
{
	struct bmi_emul_data *data;

	data = BMI_DATA_FROM_I2C_EMUL(emul);
	data->error_on_ro_write = set;
}

/** Check description in emul_bmi.h */
void bmi_emul_set_err_on_rsvd_write(struct i2c_emul *emul, bool set)
{
	struct bmi_emul_data *data;

	data = BMI_DATA_FROM_I2C_EMUL(emul);
	data->error_on_rsvd_write = set;
}

/** Check description in emul_bmi.h */
void bmi_emul_set_err_on_wo_read(struct i2c_emul *emul, bool set)
{
	struct bmi_emul_data *data;

	data = BMI_DATA_FROM_I2C_EMUL(emul);
	data->error_on_wo_read = set;
}

/** Check description in emul_bmi.h */
void bmi_emul_simulate_cmd_exec_time(struct i2c_emul *emul, bool set)
{
	struct bmi_emul_data *data;

	data = BMI_DATA_FROM_I2C_EMUL(emul);
	data->simulate_command_exec_time = set;
}

/** Check description in emul_bmi.h */
void bmi_emul_set_skipped_frames(struct i2c_emul *emul, uint8_t skip)
{
	struct bmi_emul_data *data;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	data->fifo_skip = skip;
}

/**
 * @brief Convert current time to sensor time (39 us units)
 *
 * @return time in 39 us units
 */
static int64_t bmi_emul_get_sensortime(void)
{
	return k_uptime_ticks() * 1000000 / 39 / CONFIG_SYS_CLOCK_TICKS_PER_SEC;
}

/**
 * @brief Set registers at address @p reg with sensor time that elapsed since
 *        last reset of emulator
 *
 * @param emul Pointer to BMI emulator
 * @param reg Pointer to 3 byte array, where current sensor time should be
 *            stored
 */
static void bmi_emul_set_sensortime_reg(struct i2c_emul *emul, uint8_t *reg)
{
	struct bmi_emul_data *data;
	uint32_t twos_comp_val;
	int64_t time;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	time = bmi_emul_get_sensortime();

	twos_comp_val = bmi_emul_val_to_twos_comp(time - data->zero_time);

	*reg = twos_comp_val & 0xff;
	*(reg + 1) = (twos_comp_val >> 8) & 0xff;
	*(reg + 2) = (twos_comp_val >> 16) & 0xff;
}

/**
 * @brief Convert given sensor axis @p val from internal units to register
 *        units. It shifts value by @p shift bits to the right to account
 *        range set in emulator's registers. Result is saved at address @p reg
 *
 * @param emul Pointer to BMI emulator
 * @param val Accelerometer or gyroscope value in internal units
 * @param reg Pointer to 2 byte array, where sensor value should be stored
 * @param shift How many bits should be shift to the right
 */
static void bmi_emul_set_data_reg(struct i2c_emul *emul, int32_t val,
				  uint8_t *reg, int shift)
{
	struct bmi_emul_data *data;
	uint32_t twos_comp_val;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	twos_comp_val = bmi_emul_val_to_twos_comp(val);

	/* Shift unused bits because of selected range */
	twos_comp_val >>= shift;

	*reg = twos_comp_val & 0xff;
	*(reg + 1) = (twos_comp_val >> 8) & 0xff;
}

/**
 * @brief Compute length of given FIFO @p frame. If frame is null then length
 *        of empty frame is returned.
 *
 * @param emul Pointer to BMI emulator
 * @param frame Pointer to FIFO frame
 * @param tag_time Indicate if sensor time should be included in empty frame
 * @param header Indicate if header should be included in frame
 *
 * @return length of frame
 */
static uint8_t bmi_emul_get_frame_len(struct i2c_emul *emul,
				      struct bmi_emul_frame *frame,
				      bool tag_time, bool header)
{
	struct bmi_emul_data *data;
	int len;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	/* Empty FIFO frame */
	if (frame == NULL) {
		if (tag_time && header) {
			/* Header of sensortime + sensortime + empty FIFO */
			return 5;
		}

		/* Empty fifo */
		return 1;
	}

	/* Config FIFO frame */
	if (frame->type & BMI_EMUL_FRAME_CONFIG) {
		if (header) {
			/* Header + byte of data */
			len = 2;
			if (data->type_data->sensortime_follow_config_frame) {
				/* Sensortime data */
				len += 3;
			}

			return len;
		}

		/* This frame doesn't exist in headerless mode */
		return 0;
	}

	/* Sensor data FIFO frame */
	if (header) {
		len = 1;
	} else {
		len = 0;
	}

	if (frame->type & BMI_EMUL_FRAME_ACC) {
		len += 6;
	}
	if (frame->type & BMI_EMUL_FRAME_MAG) {
		len += 8;
	}
	if (frame->type & BMI_EMUL_FRAME_GYR) {
		len += 6;
	}

	return len;
}

/**
 * @brief Set given FIFO @p frame as current frame in fifo field of emulator
 *        data structure
 *
 * @param emul Pointer to BMI emulator
 * @param frame Pointer to FIFO frame
 * @param tag_time Indicate if sensor time should be included in empty frame
 * @param header Indicate if header should be included in frame
 * @param acc_shift How many bits should be right shifted from accelerometer
 *                  data
 * @param gyr_shift How many bits should be right shifted from gyroscope data
 */
static void bmi_emul_set_current_frame(struct i2c_emul *emul,
				       struct bmi_emul_frame *frame,
				       bool tag_time, bool header,
				       int acc_shift, int gyr_shift)
{
	struct bmi_emul_data *data;
	int i = 0;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	data->fifo_frame_byte = 0;
	data->fifo_frame_len = bmi_emul_get_frame_len(emul, frame, tag_time,
						      header);
	/* Empty FIFO frame */
	if (frame == NULL) {
		if (tag_time && header) {
			/* Header */
			data->fifo[0] = BMI_EMUL_FIFO_HEAD_TIME;
			bmi_emul_set_sensortime_reg(emul, &(data->fifo[1]));
			i = 4;
		}

		/* Empty header */
		data->fifo[i] = BMI_EMUL_FIFO_HEAD_EMPTY;

		return;
	}

	/* Config FIFO frame */
	if (frame->type & BMI_EMUL_FRAME_CONFIG) {
		/* Header */
		data->fifo[0] = BMI_EMUL_FIFO_HEAD_CONFIG;
		data->fifo[1] = frame->config;
		if (data->type_data->sensortime_follow_config_frame) {
			bmi_emul_set_sensortime_reg(emul, &(data->fifo[2]));
		}

		return;
	}

	/* Sensor data FIFO frame */
	if (header) {
		data->fifo[0] = BMI_EMUL_FIFO_HEAD_DATA;
		data->fifo[0] |= frame->type & BMI_EMUL_FRAME_MAG ?
						BMI_EMUL_FIFO_HEAD_DATA_MAG : 0;
		data->fifo[0] |= frame->type & BMI_EMUL_FRAME_GYR ?
						BMI_EMUL_FIFO_HEAD_DATA_GYR : 0;
		data->fifo[0] |= frame->type & BMI_EMUL_FRAME_ACC ?
						BMI_EMUL_FIFO_HEAD_DATA_ACC : 0;
		data->fifo[0] |= frame->tag & BMI_EMUL_FIFO_HEAD_DATA_TAG_MASK;
		i = 1;
	}

	if (frame->type & BMI_EMUL_FRAME_MAG) {
		bmi_emul_set_data_reg(emul, frame->mag_x, &(data->fifo[i]), 0);
		i += 2;
		bmi_emul_set_data_reg(emul, frame->mag_y, &(data->fifo[i]), 0);
		i += 2;
		bmi_emul_set_data_reg(emul, frame->mag_z, &(data->fifo[i]), 0);
		i += 2;
		bmi_emul_set_data_reg(emul, frame->rhall, &(data->fifo[i]), 0);
		i += 2;
	}

	if (frame->type & BMI_EMUL_FRAME_GYR) {
		bmi_emul_set_data_reg(emul, frame->gyr_x, &(data->fifo[i]),
				      gyr_shift);
		i += 2;
		bmi_emul_set_data_reg(emul, frame->gyr_y, &(data->fifo[i]),
				      gyr_shift);
		i += 2;
		bmi_emul_set_data_reg(emul, frame->gyr_z, &(data->fifo[i]),
				      gyr_shift);
		i += 2;
	}

	if (frame->type & BMI_EMUL_FRAME_ACC) {
		bmi_emul_set_data_reg(emul, frame->acc_x, &(data->fifo[i]),
				      acc_shift);
		i += 2;
		bmi_emul_set_data_reg(emul, frame->acc_y, &(data->fifo[i]),
				      acc_shift);
		i += 2;
		bmi_emul_set_data_reg(emul, frame->acc_z, &(data->fifo[i]),
				      acc_shift);
		i += 2;
	}
}

/**
 * @brief Update internal sensors offset values using values from emulated
 *        registers.
 *
 * @param emul Pointer to BMI emulator
 */
static void bmi_emul_updata_int_off(struct i2c_emul *emul)
{
	struct bmi_emul_data *data;
	uint16_t gyr_nvm;
	uint8_t gyr98;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	data->off_acc_x = bmi_emul_acc_nvm_to_off(
				data->reg[data->type_data->acc_off_reg]);
	data->off_acc_y = bmi_emul_acc_nvm_to_off(
				data->reg[data->type_data->acc_off_reg + 1]);
	data->off_acc_z = bmi_emul_acc_nvm_to_off(
				data->reg[data->type_data->acc_off_reg + 2]);

	gyr98 = data->reg[data->type_data->gyr98_off_reg];

	gyr_nvm = data->reg[data->type_data->gyr_off_reg];
	gyr_nvm |= (gyr98 & 0x3) << 8;
	data->off_gyr_x = bmi_emul_gyr_nvm_to_off(gyr_nvm);
	gyr_nvm = data->reg[data->type_data->gyr_off_reg + 1];
	gyr_nvm |= (gyr98 & 0xc) << 6;
	data->off_gyr_y = bmi_emul_gyr_nvm_to_off(gyr_nvm);
	gyr_nvm = data->reg[data->type_data->gyr_off_reg + 2];
	gyr_nvm |= (gyr98 & 0x30) << 4;
	data->off_gyr_z = bmi_emul_gyr_nvm_to_off(gyr_nvm);
}

/**
 * @brief Restore registers backed in NVM to emulator's registers. Each model
 *        of BMI may have different set of NVM backed registers.
 *
 * @param emul Pointer to BMI emulator
 */
static void bmi_emul_restore_nvm(struct i2c_emul *emul)
{
	struct bmi_emul_data *data;
	int i;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	ASSERT(data->type_data->nvm_len <= BMI_EMUL_MAX_NVM_REGS);

	/* Restore registers values */
	for (i = 0; i < data->type_data->nvm_len; i++) {
		data->reg[data->type_data->nvm_reg[i]] = data->nvm[i];
	}

	bmi_emul_updata_int_off(emul);
}

/** Check description in emul_bmi.h */
void bmi_emul_flush_fifo(struct i2c_emul *emul, bool tag_time, bool header)
{
	struct bmi_emul_data *data;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	data->fifo_skip = 0;
	data->fifo_frame = NULL;
	/*
	 * Gyroscope and accelerometer shift (last two arguments)
	 * are not important for NULL (empty) FIFO frame.
	 */
	bmi_emul_set_current_frame(emul, NULL, tag_time, header, 0, 0);
}

/** Check description in emul_bmi.h */
void bmi_emul_reset_common(struct i2c_emul *emul, bool tag_time, bool header)
{
	struct bmi_emul_data *data;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	/* Restore registers backed in NVM */
	bmi_emul_restore_nvm(emul);

	/* Flush FIFO */
	bmi_emul_flush_fifo(emul, tag_time, header);

	/* Reset sensor timer */
	data->zero_time = bmi_emul_get_sensortime();
}

/** Check description in emul_bmi.h */
void bmi_emul_set_cmd_end_time(struct i2c_emul *emul, int time)
{
	struct bmi_emul_data *data;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	data->cmd_end_time = k_uptime_get_32() + time;
}

/** Check description in emul_bmi.h */
bool bmi_emul_is_cmd_end(struct i2c_emul *emul)
{
	struct bmi_emul_data *data;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	/* We are simulating command execution time and it doesn't expired */
	if (data->simulate_command_exec_time &&
	    data->cmd_end_time > k_uptime_get_32()) {
		return false;
	}

	return true;
}

/**
 * @brief Handle I2C write message. BMI model specific write function is called.
 *        It is checked if accessed register isn't RO and reserved bits are set
 *        to 0. Write set value of reg field of bmi emulator data ignoring
 *        reserved bits. If required internal sensor offset values are updated.
 *
 * @param emul Pointer to BMI emulator
 * @param reg Register which is written
 * @param val Value being written to @p reg
 * @param byte Number of handled bytes in this write command
 *
 * @return 0 on success
 * @return -EIO on error
 */
static int bmi_emul_handle_write(struct i2c_emul *emul, int reg, uint8_t val,
				 int byte)
{
	struct bmi_emul_data *data;
	uint8_t rsvd_mask;
	int ret;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	ret = data->type_data->handle_write(data->reg, emul, reg, byte, val);
	reg = data->type_data->access_reg(emul, reg, byte, false /* = read */);
	if (ret != 0) {
		if (ret == BMI_EMUL_ACCESS_E) {
			if (!data->error_on_ro_write) {
				return 0;
			}
			LOG_ERR("Writing to reg 0x%x which is RO", reg);
		}

		return -EIO;
	}

	rsvd_mask = data->type_data->rsvd_mask[reg];

	if (data->error_on_rsvd_write && rsvd_mask & val) {
		LOG_ERR("Writing 0x%x to reg 0x%x with rsvd bits mask 0x%x",
			val, reg, rsvd_mask);
		return -EIO;
	}

	/* Ignore all reserved bits */
	val &= ~rsvd_mask;
	val |= data->reg[reg] & rsvd_mask;

	data->reg[reg] = val;

	if ((reg >= data->type_data->acc_off_reg &&
	     reg <= data->type_data->acc_off_reg + 2) ||
	    (reg >= data->type_data->gyr_off_reg &&
	     reg <= data->type_data->gyr_off_reg + 2) ||
	    reg == data->type_data->gyr98_off_reg) {
		/*
		 * Internal offset value should be updated to new value of
		 * offset registers
		 */
		bmi_emul_updata_int_off(emul);
	}

	return 0;
}

/** Check description in emul_bmi.h */
void bmi_emul_state_to_reg(struct i2c_emul *emul, int acc_shift,
			   int gyr_shift, int acc_reg, int gyr_reg,
			   int sensortime_reg, bool acc_off_en,
			   bool gyr_off_en)
{
	struct bmi_emul_data *data;
	int32_t val[3];
	int i;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	if (gyr_off_en) {
		val[0] = data->gyr_x - data->off_gyr_x;
		val[1] = data->gyr_y - data->off_gyr_y;
		val[2] = data->gyr_z - data->off_gyr_z;
	} else {
		val[0] = data->gyr_x;
		val[1] = data->gyr_y;
		val[2] = data->gyr_z;
	}

	for (i = 0; i < 3; i++) {
		bmi_emul_set_data_reg(emul, val[i],
				      &(data->reg[gyr_reg + i * 2]), gyr_shift);
	}

	if (acc_off_en) {
		val[0] = data->acc_x - data->off_acc_x;
		val[1] = data->acc_y - data->off_acc_y;
		val[2] = data->acc_z - data->off_acc_z;
	} else {
		val[0] = data->acc_x;
		val[1] = data->acc_y;
		val[2] = data->acc_z;
	}

	for (i = 0; i < 3; i++) {
		bmi_emul_set_data_reg(emul, val[i],
				      &(data->reg[acc_reg + i * 2]), acc_shift);
	}

	bmi_emul_set_sensortime_reg(emul, &(data->reg[sensortime_reg]));
}

/** Check description in emul_bmi.h */
void bmi_emul_append_frame(struct i2c_emul *emul, struct bmi_emul_frame *frame)
{
	struct bmi_emul_data *data;
	struct bmi_emul_frame *tmp_frame;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	if (data->fifo_frame == NULL) {
		data->fifo_frame = frame;
	} else {
		tmp_frame = data->fifo_frame;
		while (tmp_frame->next != NULL) {
			tmp_frame = tmp_frame->next;
		}
		tmp_frame->next = frame;
	}
}

/** Check description in emul_bmi.h */
uint16_t bmi_emul_fifo_len(struct i2c_emul *emul, bool tag_time, bool header)
{
	struct bmi_emul_frame *frame;
	struct bmi_emul_data *data;
	uint16_t len = 0;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	if (data->fifo_skip != 0 && header) {
		len += 2;
	}

	frame = data->fifo_frame;
	while (frame != NULL) {
		len += bmi_emul_get_frame_len(emul, frame, tag_time, header);
		frame = frame->next;
	}

	len += bmi_emul_get_frame_len(emul, NULL, tag_time, header);
	/* Do not count last empty frame byte */
	len--;

	return len;
}

/** Check description in emul_bmi.h */
uint8_t bmi_emul_get_fifo_data(struct i2c_emul *emul, int byte,
			       bool tag_time, bool header, int acc_shift,
			       int gyr_shift)
{
	struct bmi_emul_data *data;
	int ret;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	if (byte == 0) {
		/* Repeat uncompleated read of frame */
		bmi_emul_set_current_frame(emul, data->fifo_frame, tag_time,
					   header, acc_shift, gyr_shift);

		/* Return header for skip frame */
		if (data->fifo_skip != 0 && header) {
			return BMI_EMUL_FIFO_HEAD_SKIP;
		}
	}

	if (data->fifo_skip != 0 && byte == 1 && header) {
		/* Return number of skipped frames */
		ret = data->fifo_skip;
		data->fifo_skip = 0;

		return ret;
	}

	/* Get next valid frame */
	while (data->fifo_frame_byte >= data->fifo_frame_len) {
		/* No data */
		if (data->fifo_frame == NULL) {
			return 0;
		}
		data->fifo_frame = data->fifo_frame->next;
		bmi_emul_set_current_frame(emul, data->fifo_frame, tag_time,
					   header, acc_shift, gyr_shift);
	}

	return data->fifo[data->fifo_frame_byte++];
}

/**
 * @brief Handle I2C read message. BMI model specific read function is called.
 *        It is checked if accessed register isn't WO.
 *
 * @param emul Pointer to BMI emulator
 * @param reg Register address to read
 * @param buf Pointer where result should be stored
 * @param byte Byte which is accessed during block read
 *
 * @return 0 on success
 * @return -EIO on error
 */
static int bmi_emul_handle_read(struct i2c_emul *emul, int reg, uint8_t *buf,
				int byte)
{
	struct bmi_emul_data *data;
	int ret;

	data = BMI_DATA_FROM_I2C_EMUL(emul);

	ret = data->type_data->handle_read(data->reg, emul, reg, byte, buf);
	reg = data->type_data->access_reg(emul, reg, byte, true /* = read */);
	if (ret == BMI_EMUL_ACCESS_E && data->error_on_wo_read) {
		LOG_ERR("Reading reg 0x%x which is WO", reg);
	} else if (ret != 0) {
		return ret;
	}

	return 0;
}

/* Device instantiation */

static struct i2c_emul_api bmi_emul_api = {
	.transfer = i2c_common_emul_transfer,
};

/**
 * @brief Set up a new BMI emulator
 *
 * This should be called for each BMI device that needs to be
 * emulated. It registers it with the I2C emulation controller.
 *
 * @param emul Emulation information
 * @param parent Device to emulate
 *
 * @return 0 indicating success (always)
 */
static int bmi_emul_init(const struct emul *emul,
			 const struct device *parent)
{
	const struct i2c_common_emul_cfg *cfg = emul->cfg;
	struct i2c_common_emul_data *data = cfg->data;
	struct bmi_emul_data *bmi_data;
	int ret;

	data->emul.api = &bmi_emul_api;
	data->emul.addr = cfg->addr;
	data->i2c = parent;
	data->cfg = cfg;
	i2c_common_emul_init(data);

	bmi_data = CONTAINER_OF(data, struct bmi_emul_data, common);

	switch (bmi_data->type) {
	case BMI_EMUL_160:
		bmi_data->type_data = get_bmi160_emul_type_data();
		break;
	case BMI_EMUL_260:
		bmi_data->type_data = get_bmi260_emul_type_data();
		break;
	}

	/* Set callback access_reg to type specific function */
	data->access_reg = bmi_data->type_data->access_reg;

	ret = i2c_emul_register(parent, emul->dev_label, &data->emul);

	bmi_data->type_data->reset(bmi_data->reg, &data->emul);

	return ret;
}

#define BMI_EMUL(n)							\
	static struct bmi_emul_data bmi_emul_data_##n = {		\
		.error_on_ro_write = DT_INST_PROP(n, error_on_ro_write),\
		.error_on_wo_read = DT_INST_PROP(n, error_on_wo_read),	\
		.error_on_rsvd_write = DT_INST_PROP(n,			\
					error_on_reserved_bit_write),	\
		.simulate_command_exec_time = DT_INST_PROP(n,		\
					simulate_command_exec_time),	\
		.type = DT_STRING_TOKEN(DT_DRV_INST(n), device_model),	\
		.common = {						\
			.start_write = NULL,				\
			.write_byte = bmi_emul_handle_write,		\
			.finish_write = NULL,				\
			.start_read = NULL,				\
			.read_byte = bmi_emul_handle_read,		\
			.finish_read = NULL,				\
			.access_reg = NULL,				\
		},							\
	};								\
									\
	static const struct i2c_common_emul_cfg bmi_emul_cfg_##n = {	\
		.i2c_label = DT_INST_BUS_LABEL(n),			\
		.dev_label = DT_INST_LABEL(n),                          \
		.data = &bmi_emul_data_##n.common,			\
		.addr = DT_INST_REG_ADDR(n),				\
	};								\
	EMUL_DEFINE(bmi_emul_init, DT_DRV_INST(n), &bmi_emul_cfg_##n,	\
		    &bmi_emul_data_##n)

DT_INST_FOREACH_STATUS_OKAY(BMI_EMUL)

#define BMI_EMUL_CASE(n)					\
	case DT_INST_DEP_ORD(n): return &bmi_emul_data_##n.common.emul;

/** Check description in emul_bmi.h */
struct i2c_emul *bmi_emul_get(int ord)
{
	switch (ord) {
	DT_INST_FOREACH_STATUS_OKAY(BMI_EMUL_CASE)

	default:
		return NULL;
	}
}