zephyr: emul: Use common I2C code in emulators

Align following emulators to use common I2C code:
- bb_retimer
- bma255
- bmi (both 160 and 260)
- smart_battery
- tcs3400

BUG=none
BRANCH=none
TEST=make configure --test zephyr/test/drivers

Signed-off-by: Tomasz Michalec <tm@semihalf.com>
Change-Id: I7515407b867487574a29dcae3456d96920a24979
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/3140202
Commit-Queue: Jeremy Bettis <jbettis@chromium.org>
Reviewed-by: Jeremy Bettis <jbettis@chromium.org>

8 files changed, 567 insertions, 1447 deletions

diff --git a/zephyr/emul/Kconfig b/zephyr/emul/Kconfig
index 4557117985..9686baf24e 100644
--- a/zephyr/emul/Kconfig
+++ b/zephyr/emul/Kconfig
@@ -11,11 +11,13 @@ config EMUL_COMMON_I2C
 
 config EMUL_SMART_BATTERY
 	bool "Smart Battery emulator"
+	select EMUL_COMMON_I2C
 	help
 	  Enable the Smart Battery emulator. This driver use emulated I2C bus.
 
 config EMUL_BMA255
 	bool "BMA255 emulator"
+	select EMUL_COMMON_I2C
 	help
 	  Enable the BMA255 emulator. This driver use emulated I2C bus.
 	  It is used to test bma2x2 driver. Emulators API is available in
@@ -37,6 +39,7 @@ config EMUL_PPC_SYV682X
 
 config EMUL_BMI
 	bool "BMI emulator"
+	select EMUL_COMMON_I2C
 	help
 	  Enable the BMI emulator. This driver use emulated I2C bus.
 	  It is used to test bmi 160 and 260 drivers. Emulators API is
@@ -44,6 +47,7 @@ config EMUL_BMI
 
 config EMUL_TCS3400
 	bool "TCS3400 emulator"
+	select EMUL_COMMON_I2C
 	help
 	  Enable the TCS3400 light sensor. This driver use emulated I2C bus.
 	  It is used to test als_tcs3400 driver. It supports reading sensor
@@ -56,6 +60,7 @@ config EMUL_TCS3400
 
 config EMUL_BB_RETIMER
 	bool "BB retimer emulator"
+	select EMUL_COMMON_I2C
 	help
 	  Enable the BB (Burnside Bridge) retimer emulator. This driver use
 	  emulated I2C bus. It is used to test bb_retimer driver. It supports
diff --git a/zephyr/emul/emul_bb_retimer.c b/zephyr/emul/emul_bb_retimer.c
index e016cf709e..51e60948c8 100644
--- a/zephyr/emul/emul_bb_retimer.c
+++ b/zephyr/emul/emul_bb_retimer.c
@@ -14,29 +14,19 @@ LOG_MODULE_REGISTER(emul_bb_retimer);
 #include <drivers/i2c.h>
 #include <drivers/i2c_emul.h>
 
+#include "emul/emul_common_i2c.h"
 #include "emul/emul_bb_retimer.h"
 
 #include "driver/retimer/bb_retimer.h"
 
-/**
- * Describe if there is no ongoing I2C message or if there is message handled
- * at the moment (last message doesn't ended with stop or write is not followed
- * by read).
- */
-enum bb_emul_msg_state {
-	BB_EMUL_NONE_MSG,
-	BB_EMUL_IN_WRITE,
-	BB_EMUL_IN_READ
-};
+#define BB_DATA_FROM_I2C_EMUL(_emul)					     \
+	CONTAINER_OF(CONTAINER_OF(_emul, struct i2c_common_emul_data, emul), \
+		     struct bb_emul_data, common)
 
 /** Run-time data used by the emulator */
 struct bb_emul_data {
-	/** I2C emulator detail */
-	struct i2c_emul emul;
-	/** BB retimer device being emulated */
-	const struct device *i2c;
-	/** Configuration information */
-	const struct bb_emul_cfg *cfg;
+	/** Common I2C data */
+	struct i2c_common_emul_data common;
 
 	/** Current state of all emulated BB retimer registers */
 	uint32_t reg[BB_RETIMER_REG_COUNT];
@@ -49,86 +39,11 @@ struct bb_emul_data {
 	/** Return error when trying to write 1 to reserved bit */
 	bool error_on_rsvd_write;
 
-	/** Current state of I2C bus (if emulator is handling message) */
-	enum bb_emul_msg_state msg_state;
-	/** Number of already handled bytes in ongoing message */
-	int msg_byte;
-	/** Register selected in last write command */
-	uint8_t cur_reg;
 	/** Value of data dword in ongoing i2c message */
 	uint32_t data_dword;
-
-	/** Custom write function called on I2C write opperation */
-	bb_emul_write_func write_func;
-	/** Data passed to custom write function */
-	void *write_func_data;
-	/** Custom read function called on I2C read opperation */
-	bb_emul_read_func read_func;
-	/** Data passed to custom read function */
-	void *read_func_data;
-
-	/** Control if read should fail on given register */
-	int read_fail_reg;
-	/** Control if write should fail on given register */
-	int write_fail_reg;
-
-	/** Mutex used to control access to emulator data */
-	struct k_mutex data_mtx;
-};
-
-/** Static configuration for the emulator */
-struct bb_emul_cfg {
-	/** Label of the I2C bus this emulator connects to */
-	const char *i2c_label;
-	/** Pointer to run-time data */
-	struct bb_emul_data *data;
-	/** Address of BB retimer on i2c bus */
-	uint16_t addr;
 };
 
 /** Check description in emul_bb_retimer.h */
-int bb_emul_lock_data(struct i2c_emul *emul, k_timeout_t timeout)
-{
-	struct bb_emul_data *data;
-
-	data = CONTAINER_OF(emul, struct bb_emul_data, emul);
-
-	return k_mutex_lock(&data->data_mtx, timeout);
-}
-
-/** Check description in emul_bb_retimer.h */
-int bb_emul_unlock_data(struct i2c_emul *emul)
-{
-	struct bb_emul_data *data;
-
-	data = CONTAINER_OF(emul, struct bb_emul_data, emul);
-
-	return k_mutex_unlock(&data->data_mtx);
-}
-
-/** Check description in emul_bb_retimer.h */
-void bb_emul_set_write_func(struct i2c_emul *emul,
-			    bb_emul_write_func func, void *data)
-{
-	struct bb_emul_data *emul_data;
-
-	emul_data = CONTAINER_OF(emul, struct bb_emul_data, emul);
-	emul_data->write_func = func;
-	emul_data->write_func_data = data;
-}
-
-/** Check description in emul_bb_retimer.h */
-void bb_emul_set_read_func(struct i2c_emul *emul,
-			   bb_emul_read_func func, void *data)
-{
-	struct bb_emul_data *emul_data;
-
-	emul_data = CONTAINER_OF(emul, struct bb_emul_data, emul);
-	emul_data->read_func = func;
-	emul_data->read_func_data = data;
-}
-
-/** Check description in emul_bb_retimer.h */
 void bb_emul_set_reg(struct i2c_emul *emul, int reg, uint32_t val)
 {
 	struct bb_emul_data *data;
@@ -137,7 +52,7 @@ void bb_emul_set_reg(struct i2c_emul *emul, int reg, uint32_t val)
 		return;
 	}
 
-	data = CONTAINER_OF(emul, struct bb_emul_data, emul);
+	data = BB_DATA_FROM_I2C_EMUL(emul);
 	data->reg[reg] = val;
 }
 
@@ -150,35 +65,17 @@ uint32_t bb_emul_get_reg(struct i2c_emul *emul, int reg)
 		return 0;
 	}
 
-	data = CONTAINER_OF(emul, struct bb_emul_data, emul);
+	data = BB_DATA_FROM_I2C_EMUL(emul);
 
 	return data->reg[reg];
 }
 
 /** Check description in emul_bb_retimer.h */
-void bb_emul_set_read_fail_reg(struct i2c_emul *emul, int reg)
-{
-	struct bb_emul_data *data;
-
-	data = CONTAINER_OF(emul, struct bb_emul_data, emul);
-	data->read_fail_reg = reg;
-}
-
-/** Check description in emul_bb_retimer.h */
-void bb_emul_set_write_fail_reg(struct i2c_emul *emul, int reg)
-{
-	struct bb_emul_data *data;
-
-	data = CONTAINER_OF(emul, struct bb_emul_data, emul);
-	data->write_fail_reg = reg;
-}
-
-/** Check description in emul_bb_retimer.h */
 void bb_emul_set_err_on_ro_write(struct i2c_emul *emul, bool set)
 {
 	struct bb_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bb_emul_data, emul);
+	data = BB_DATA_FROM_I2C_EMUL(emul);
 	data->error_on_ro_write = set;
 }
 
@@ -187,7 +84,7 @@ void bb_emul_set_err_on_rsvd_write(struct i2c_emul *emul, bool set)
 {
 	struct bb_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bb_emul_data, emul);
+	data = BB_DATA_FROM_I2C_EMUL(emul);
 	data->error_on_rsvd_write = set;
 }
 
@@ -212,7 +109,7 @@ static void bb_emul_reset(struct i2c_emul *emul)
 {
 	struct bb_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bb_emul_data, emul);
+	data = BB_DATA_FROM_I2C_EMUL(emul);
 
 	data->reg[BB_RETIMER_REG_VENDOR_ID]		= data->vendor_id;
 	data->reg[BB_RETIMER_REG_DEVICE_ID]		= BB_RETIMER_DEVICE_ID;
@@ -227,25 +124,28 @@ static void bb_emul_reset(struct i2c_emul *emul)
 /**
  * @brief Handle I2C write message. It is checked if accessed register isn't RO
  *        and reserved bits are set to 0. Write set value of reg field of BB
- *        retimer emulator data ignoring reserved bits and write only bits. Some
- *        commands are handled specialy. Before any handling, custom function
- *        is called if provided.
+ *        retimer emulator data ignoring reserved bits and write only bits.
  *
  * @param emul Pointer to BB retimer emulator
  * @param reg Register which is written
- * @param val Value being written to @p reg
  * @param msg_len Length of handled I2C message
  *
  * @return 0 on success
  * @return -EIO on error
  */
-static int bb_emul_handle_write(struct i2c_emul *emul, int reg, uint32_t val,
-				int msg_len)
+static int bb_emul_handle_write(struct i2c_emul *emul, int reg, int msg_len)
 {
 	struct bb_emul_data *data;
-	int ret;
+	uint32_t val;
+
+	data = BB_DATA_FROM_I2C_EMUL(emul);
+
+	/* This write only selected register for I2C read message */
+	if (msg_len < 2) {
+		return 0;
+	}
 
-	data = CONTAINER_OF(emul, struct bb_emul_data, emul);
+	val = data->data_dword;
 
 	/*
 	 * BB retimer ignores data bytes above 4 and use zeros if there is less
@@ -255,20 +155,6 @@ static int bb_emul_handle_write(struct i2c_emul *emul, int reg, uint32_t val,
 		LOG_WRN("Got %d bytes of WR data, expected 4", msg_len - 2);
 	}
 
-	if (data->write_func) {
-		ret = data->write_func(emul, reg, val, data->write_func_data);
-		if (ret < 0) {
-			return -EIO;
-		} else if (ret == 0) {
-			return 0;
-		}
-	}
-
-	if (data->write_fail_reg == reg ||
-	    data->write_fail_reg == BB_EMUL_FAIL_ALL_REG) {
-		return -EIO;
-	}
-
 	if (reg <= BB_RETIMER_REG_DEVICE_ID ||
 	    reg >= BB_RETIMER_REG_COUNT ||
 	    reg == BB_RETIMER_REG_TBT_CONTROL) {
@@ -297,40 +183,19 @@ static int bb_emul_handle_write(struct i2c_emul *emul, int reg, uint32_t val,
 
 /**
  * @brief Handle I2C read message. Response is obtained from reg field of bb
- *        emul data. When accessing accelerometer value, register data is first
- *        computed using internal emulator state. Before default handler, custom
- *        user read function is called if provided.
+ *        emul data.
  *
  * @param emul Pointer to BB retimer emulator
  * @param reg Register address to read
- * @param buf Pointer where result should be stored
  *
  * @return 0 on success
  * @return -EIO on error
  */
-static int bb_emul_handle_read(struct i2c_emul *emul, int reg, uint32_t *buf)
+static int bb_emul_handle_read(struct i2c_emul *emul, int reg)
 {
 	struct bb_emul_data *data;
-	int ret;
-
-	data = CONTAINER_OF(emul, struct bb_emul_data, emul);
 
-	if (data->read_func) {
-		ret = data->read_func(emul, reg, data->read_func_data);
-		if (ret < 0) {
-			return -EIO;
-		} else if (ret == 0) {
-			/* Immediately return value set by custom function */
-			*buf = data->reg[reg];
-
-			return 0;
-		}
-	}
-
-	if (data->read_fail_reg == reg ||
-	    data->read_fail_reg == BB_EMUL_FAIL_ALL_REG) {
-		return -EIO;
-	}
+	data = BB_DATA_FROM_I2C_EMUL(emul);
 
 	if (reg >= BB_RETIMER_REG_COUNT) {
 		LOG_ERR("Read unknown register 0x%x", reg);
@@ -338,146 +203,86 @@ static int bb_emul_handle_read(struct i2c_emul *emul, int reg, uint32_t *buf)
 		return -EIO;
 	}
 
-	*buf = data->reg[reg];
+	data->data_dword = data->reg[reg];
 
 	return 0;
 }
 
 /**
- * @biref Emulate an I2C transfer to a BB retimer
+ * @brief Function called for each byte of write message. Data are stored
+ *        in data_dword field of bb_emul_data
  *
- * This handles simple reads and writes
- *
- * @param emul I2C emulation information
- * @param msgs List of messages to process
- * @param num_msgs Number of messages to process
- * @param addr Address of the I2C target device
+ * @param emul Pointer to BB retimer emulator
+ * @param reg First byte of write message
+ * @param val Received byte of write message
+ * @param bytes Number of bytes already received
  *
- * @retval 0 If successful
- * @retval -EIO General input / output error
+ * @return 0 on success
  */
-static int bb_emul_transfer(struct i2c_emul *emul, struct i2c_msg *msgs,
-			    int num_msgs, int addr)
+static int bb_emul_write_byte(struct i2c_emul *emul, int reg, uint8_t val,
+			      int bytes)
 {
-	const struct bb_emul_cfg *cfg;
 	struct bb_emul_data *data;
-	int ret, i;
-	bool read;
 
-	data = CONTAINER_OF(emul, struct bb_emul_data, emul);
-	cfg = data->cfg;
+	data = BB_DATA_FROM_I2C_EMUL(emul);
 
-	if (cfg->addr != addr) {
-		LOG_ERR("Address mismatch, expected %02x, got %02x", cfg->addr,
-			addr);
-		return -EIO;
+	if (bytes == 1) {
+		data->data_dword = 0;
+		if (val != 4) {
+			LOG_WRN("Invalid write size");
+		}
+	} else if (bytes < 6) {
+		data->data_dword |= val << (8 * (bytes - 2));
 	}
 
-	i2c_dump_msgs("emul", msgs, num_msgs, addr);
-
-	for (; num_msgs > 0; num_msgs--, msgs++) {
-		read = msgs->flags & I2C_MSG_READ;
-
-		switch (data->msg_state) {
-		case BB_EMUL_NONE_MSG:
-			data->data_dword = 0;
-			data->msg_byte = 0;
-			break;
-		case BB_EMUL_IN_WRITE:
-			if (read) {
-				/* Finish write command */
-				if (data->msg_byte >= 2) {
-					k_mutex_lock(&data->data_mtx,
-						     K_FOREVER);
-					ret = bb_emul_handle_write(emul,
-							data->cur_reg,
-							data->data_dword,
-							data->msg_byte);
-					k_mutex_unlock(&data->data_mtx);
-					if (ret) {
-						return -EIO;
-					}
-				}
-				data->data_dword = 0;
-				data->msg_byte = 0;
-			}
-			break;
-		case BB_EMUL_IN_READ:
-			if (!read) {
-				data->data_dword = 0;
-				data->msg_byte = 0;
-			}
-			break;
-		}
-		data->msg_state = read ? BB_EMUL_IN_READ : BB_EMUL_IN_WRITE;
+	return 0;
+}
 
-		if (msgs->flags & I2C_MSG_STOP) {
-			data->msg_state = BB_EMUL_NONE_MSG;
-		}
+/**
+ * @brief Function called for each byte of read message. data_dword is converted
+ *        to read message response.
+ *
+ * @param emul Pointer to BB retimer emulator
+ * @param reg First byte of last write message
+ * @param val Pointer where byte to read should be stored
+ * @param bytes Number of bytes already readed
+ *
+ * @return 0 on success
+ */
+static int bb_emul_read_byte(struct i2c_emul *emul, int reg, uint8_t *val,
+			     int bytes)
+{
+	struct bb_emul_data *data;
 
-		if (!read) {
-			/* Dispatch wrtie command */
-			for (i = 0; i < msgs->len; i++) {
-				switch (data->msg_byte) {
-				case 0:
-					data->cur_reg = msgs->buf[i];
-					break;
-				case 1:
-					/*
-					 * BB retimer ignores size, but it
-					 * should be 4, so emulator check this.
-					 */
-					if (msgs->buf[i] != 4) {
-						LOG_WRN("Invalid write size");
-					}
-					break;
-				default:
-					data->data_dword |=
-						(msgs->buf[i] & 0xff) <<
-						(8 * (data->msg_byte - 2));
-				}
-				data->msg_byte++;
-			}
-
-			/* Execute write command */
-			if (msgs->flags & I2C_MSG_STOP && data->msg_byte >= 2) {
-				k_mutex_lock(&data->data_mtx, K_FOREVER);
-				ret = bb_emul_handle_write(emul, data->cur_reg,
-							   data->data_dword,
-							   data->msg_byte);
-				k_mutex_unlock(&data->data_mtx);
-				if (ret) {
-					return -EIO;
-				}
-			}
-		} else {
-			/* Prepare response */
-			if (data->msg_byte == 0) {
-				k_mutex_lock(&data->data_mtx, K_FOREVER);
-				ret = bb_emul_handle_read(emul, data->cur_reg,
-							  &data->data_dword);
-				k_mutex_unlock(&data->data_mtx);
-				if (ret) {
-					return -EIO;
-				}
-			}
-
-			for (i = 0; i < msgs->len; i++) {
-				msgs->buf[i] = data->data_dword & 0xff;
-				data->data_dword >>= 8;
-
-				data->msg_byte++;
-			}
-		}
-	}
+	data = BB_DATA_FROM_I2C_EMUL(emul);
+
+	*val = data->data_dword & 0xff;
+	data->data_dword >>= 8;
 
 	return 0;
 }
 
+/**
+ * @brief Get currently accessed register, which always equals to selected
+ *        register.
+ *
+ * @param emul Pointer to BB retimer emulator
+ * @param reg First byte of last write message
+ * @param bytes Number of bytes already handled from current message
+ * @param read If currently handled is read message
+ *
+ * @return Currently accessed register
+ */
+static int bb_emul_access_reg(struct i2c_emul *emul, int reg, int bytes,
+			      bool read)
+{
+	return reg;
+}
+
 /* Device instantiation */
 
 static struct i2c_emul_api bb_emul_api = {
-	.transfer = bb_emul_transfer,
+	.transfer = i2c_common_emul_transfer,
 };
 
 /**
@@ -494,15 +299,15 @@ static struct i2c_emul_api bb_emul_api = {
 static int bb_emul_init(const struct emul *emul,
 			const struct device *parent)
 {
-	const struct bb_emul_cfg *cfg = emul->cfg;
-	struct bb_emul_data *data = cfg->data;
+	const struct i2c_common_emul_cfg *cfg = emul->cfg;
+	struct i2c_common_emul_data *data = cfg->data;
 	int ret;
 
 	data->emul.api = &bb_emul_api;
 	data->emul.addr = cfg->addr;
 	data->i2c = parent;
 	data->cfg = cfg;
-	k_mutex_init(&data->data_mtx);
+	i2c_common_emul_init(data);
 
 	ret = i2c_emul_register(parent, emul->dev_label, &data->emul);
 
@@ -517,17 +322,20 @@ static int bb_emul_init(const struct emul *emul,
 		.error_on_ro_write = DT_INST_PROP(n, error_on_ro_write),\
 		.error_on_rsvd_write = DT_INST_PROP(n,			\
 					error_on_reserved_bit_write),	\
-		.msg_state = BB_EMUL_NONE_MSG,				\
-		.cur_reg = 0,						\
-		.write_func = NULL,					\
-		.read_func = NULL,					\
-		.write_fail_reg = BB_EMUL_NO_FAIL_REG,			\
-		.read_fail_reg = BB_EMUL_NO_FAIL_REG,			\
+		.common = {						\
+			.start_write = NULL,				\
+			.write_byte = bb_emul_write_byte,		\
+			.finish_write = bb_emul_handle_write,		\
+			.start_read = bb_emul_handle_read,		\
+			.read_byte = bb_emul_read_byte,			\
+			.finish_read = NULL,				\
+			.access_reg = bb_emul_access_reg,		\
+		},							\
 	};								\
 									\
-	static const struct bb_emul_cfg bb_emul_cfg_##n = {		\
+	static const struct i2c_common_emul_cfg bb_emul_cfg_##n = {	\
 		.i2c_label = DT_INST_BUS_LABEL(n),			\
-		.data = &bb_emul_data_##n,				\
+		.data = &bb_emul_data_##n.common,			\
 		.addr = DT_INST_REG_ADDR(n),				\
 	};								\
 	EMUL_DEFINE(bb_emul_init, DT_DRV_INST(n), &bb_emul_cfg_##n)
@@ -535,7 +343,7 @@ static int bb_emul_init(const struct emul *emul,
 DT_INST_FOREACH_STATUS_OKAY(BB_RETIMER_EMUL)
 
 #define BB_RETIMER_EMUL_CASE(n)					\
-	case DT_INST_DEP_ORD(n): return &bb_emul_data_##n.emul;
+	case DT_INST_DEP_ORD(n): return &bb_emul_data_##n.common.emul;
 
 /** Check description in emul_bb_emulator.h */
 struct i2c_emul *bb_emul_get(int ord)
diff --git a/zephyr/emul/emul_bma255.c b/zephyr/emul/emul_bma255.c
index 073e196df8..9b6c401d0d 100644
--- a/zephyr/emul/emul_bma255.c
+++ b/zephyr/emul/emul_bma255.c
@@ -14,29 +14,22 @@ LOG_MODULE_REGISTER(emul_bma255);
 #include <drivers/i2c.h>
 #include <drivers/i2c_emul.h>
 
+#include "emul/emul_common_i2c.h"
 #include "emul/emul_bma255.h"
 
 #include "driver/accel_bma2x2.h"
 
-/**
- * Describe if there is no ongoing I2C message or if there is message handled
- * at the moment (last message doesn't ended with stop or write is not followed
- * by read).
- */
-enum bma_emul_msg_state {
-	BMA_EMUL_NONE_MSG,
-	BMA_EMUL_IN_WRITE,
-	BMA_EMUL_IN_READ
-};
+#define BMA_DATA_FROM_I2C_EMUL(_emul)					     \
+	CONTAINER_OF(CONTAINER_OF(_emul, struct i2c_common_emul_data, emul), \
+		     struct bma_emul_data, common)
 
 /** Run-time data used by the emulator */
 struct bma_emul_data {
-	/** I2C emulator detail */
-	struct i2c_emul emul;
-	/** BMA255 device being emulated */
-	const struct device *i2c;
-	/** Configuration information */
-	const struct bma_emul_cfg *cfg;
+	/** Common I2C data */
+	struct i2c_common_emul_data common;
+
+	/** Value of data byte in ongoing write message */
+	uint8_t write_byte;
 
 	/** Current state of all emulated BMA255 registers */
 	uint8_t reg[0x40];
@@ -78,87 +71,9 @@ struct bma_emul_data {
 	bool lsb_x_read;
 	bool lsb_y_read;
 	bool lsb_z_read;
-
-	/** Current state of I2C bus (if emulator is handling message) */
-	enum bma_emul_msg_state msg_state;
-	/** Number of already handled bytes in ongoing message */
-	int msg_byte;
-	/** Register selected in last write command */
-	uint8_t cur_reg;
-	/** Value of data byte in ongoing write message */
-	uint8_t write_byte;
-
-	/** Custom write function called on I2C write opperation */
-	bma_emul_write_func write_func;
-	/** Data passed to custom write function */
-	void *write_func_data;
-	/** Custom read function called on I2C read opperation */
-	bma_emul_read_func read_func;
-	/** Data passed to custom read function */
-	void *read_func_data;
-
-	/** Control if read should fail on given register */
-	int read_fail_reg;
-	/** Control if write should fail on given register */
-	int write_fail_reg;
-
-	/** Mutex used to control access to emulator data */
-	struct k_mutex data_mtx;
-};
-
-/** Static configuration for the emulator */
-struct bma_emul_cfg {
-	/** Label of the I2C bus this emulator connects to */
-	const char *i2c_label;
-	/** Pointer to run-time data */
-	struct bma_emul_data *data;
-	/** Address of BMA255 on i2c bus */
-	uint16_t addr;
 };
 
 /** Check description in emul_bma255.h */
-int bma_emul_lock_data(struct i2c_emul *emul, k_timeout_t timeout)
-{
-	struct bma_emul_data *data;
-
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
-
-	return k_mutex_lock(&data->data_mtx, timeout);
-}
-
-/** Check description in emul_bma255.h */
-int bma_emul_unlock_data(struct i2c_emul *emul)
-{
-	struct bma_emul_data *data;
-
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
-
-	return k_mutex_unlock(&data->data_mtx);
-}
-
-/** Check description in emul_bma255.h */
-void bma_emul_set_write_func(struct i2c_emul *emul,
-			     bma_emul_write_func func, void *data)
-{
-	struct bma_emul_data *emul_data;
-
-	emul_data = CONTAINER_OF(emul, struct bma_emul_data, emul);
-	emul_data->write_func = func;
-	emul_data->write_func_data = data;
-}
-
-/** Check description in emul_bma255.h */
-void bma_emul_set_read_func(struct i2c_emul *emul,
-			    bma_emul_read_func func, void *data)
-{
-	struct bma_emul_data *emul_data;
-
-	emul_data = CONTAINER_OF(emul, struct bma_emul_data, emul);
-	emul_data->read_func = func;
-	emul_data->read_func_data = data;
-}
-
-/** Check description in emul_bma255.h */
 void bma_emul_set_reg(struct i2c_emul *emul, int reg, uint8_t val)
 {
 	struct bma_emul_data *data;
@@ -167,7 +82,7 @@ void bma_emul_set_reg(struct i2c_emul *emul, int reg, uint8_t val)
 		return;
 	}
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 	data->reg[reg] = val;
 }
 
@@ -180,29 +95,11 @@ uint8_t bma_emul_get_reg(struct i2c_emul *emul, int reg)
 		return 0;
 	}
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 
 	return data->reg[reg];
 }
 
-/** Check description in emul_bma255.h */
-void bma_emul_set_read_fail_reg(struct i2c_emul *emul, int reg)
-{
-	struct bma_emul_data *data;
-
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
-	data->read_fail_reg = reg;
-}
-
-/** Check description in emul_bma255.h */
-void bma_emul_set_write_fail_reg(struct i2c_emul *emul, int reg)
-{
-	struct bma_emul_data *data;
-
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
-	data->write_fail_reg = reg;
-}
-
 /**
  * @brief Convert @p val to two's complement representation. It makes sure that
  *        bit representation is correct even on platforms which represent
@@ -289,7 +186,7 @@ int16_t bma_emul_get_off(struct i2c_emul *emul, int axis)
 {
 	struct bma_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 
 	switch (axis) {
 	case BMA_EMUL_AXIS_X:
@@ -308,7 +205,7 @@ void bma_emul_set_off(struct i2c_emul *emul, int axis, int16_t val)
 {
 	struct bma_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 
 	switch (axis) {
 	case BMA_EMUL_AXIS_X:
@@ -334,7 +231,7 @@ int16_t bma_emul_get_acc(struct i2c_emul *emul, int axis)
 {
 	struct bma_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 
 	switch (axis) {
 	case BMA_EMUL_AXIS_X:
@@ -353,7 +250,7 @@ void bma_emul_set_acc(struct i2c_emul *emul, int axis, int16_t val)
 {
 	struct bma_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 
 	switch (axis) {
 	case BMA_EMUL_AXIS_X:
@@ -373,7 +270,7 @@ void bma_emul_set_err_on_cal_nrdy(struct i2c_emul *emul, bool set)
 {
 	struct bma_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 	data->error_on_cal_trg_nrdy = set;
 }
 
@@ -382,7 +279,7 @@ void bma_emul_set_err_on_cal_bad_range(struct i2c_emul *emul, bool set)
 {
 	struct bma_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 	data->error_on_cal_trg_bad_range = set;
 }
 
@@ -391,7 +288,7 @@ void bma_emul_set_err_on_ro_write(struct i2c_emul *emul, bool set)
 {
 	struct bma_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 	data->error_on_ro_write = set;
 }
 
@@ -400,7 +297,7 @@ void bma_emul_set_err_on_rsvd_write(struct i2c_emul *emul, bool set)
 {
 	struct bma_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 	data->error_on_rsvd_write = set;
 }
 
@@ -409,7 +306,7 @@ void bma_emul_set_err_on_msb_first(struct i2c_emul *emul, bool set)
 {
 	struct bma_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 	data->error_on_msb_first = set;
 }
 
@@ -491,7 +388,7 @@ static void bma_emul_restore_nvm(struct i2c_emul *emul)
 {
 	struct bma_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 
 	/* Restore registers values */
 	data->reg[BMA2x2_OFFSET_X_AXIS_ADDR] = data->nvm_x;
@@ -515,7 +412,7 @@ static void bma_emul_reset(struct i2c_emul *emul)
 {
 	struct bma_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 
 	data->reg[BMA2x2_CHIP_ID_ADDR]			= 0xfa;
 	data->reg[0x01]					= 0x00; /* Reserved */
@@ -622,7 +519,7 @@ static int bma_emul_handle_nvm_write(struct i2c_emul *emul, uint8_t val)
 	struct bma_emul_data *data;
 	uint8_t writes_rem;
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 
 	/* NVM not ready, ignore write/load requests */
 	if (!(data->reg[BMA2x2_EEPROM_CTRL_ADDR] & BMA2x2_EEPROM_RDY)) {
@@ -665,7 +562,7 @@ static void bma_emul_clear_int(struct i2c_emul *emul)
 {
 	struct bma_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 
 	data->reg[BMA2x2_STAT1_ADDR]		= 0x00;
 	data->reg[BMA2x2_STAT2_ADDR]		= 0x00;
@@ -687,7 +584,7 @@ static int16_t bma_emul_get_target(struct i2c_emul *emul, int axis)
 	struct bma_emul_data *data;
 	uint8_t target;
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 
 	target = data->reg[BMA2x2_OFC_SETTING_ADDR] >>
 		 BMA2x2_OFC_TARGET_AXIS(axis);
@@ -723,7 +620,7 @@ static int bma_emul_handle_off_comp(struct i2c_emul *emul, uint8_t val)
 	uint8_t trigger;
 	int16_t target;
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 
 	if (val & BMA2x2_OFFSET_RESET) {
 		data->off_x = 0;
@@ -775,37 +672,35 @@ static int bma_emul_handle_off_comp(struct i2c_emul *emul, uint8_t val)
  * @brief Handle I2C write message. It is checked if accessed register isn't RO
  *        and reserved bits are set to 0. Write set value of reg field of bma
  *        emulator data ignoring reserved bits and write only bits. Some
- *        commands are handled specialy. Before any handling, custom function
- *        is called if provided.
+ *        commands are handled specialy.
  *
  * @param emul Pointer to BMA255 emulator
  * @param reg Register which is written
- * @param val Value being written to @p reg
+ * @param bytes Number of bytes in I2C write message
  *
  * @return 0 on success
  * @return -EIO on error
  */
-static int bma_emul_handle_write(struct i2c_emul *emul, int reg, uint8_t val)
+static int bma_emul_handle_write(struct i2c_emul *emul, int reg, int bytes)
 {
 	struct bma_emul_data *data;
+	uint8_t val;
 	int ret;
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 
-	if (data->write_func) {
-		ret = data->write_func(emul, reg, val, data->write_func_data);
-		if (ret < 0) {
-			return -EIO;
-		} else if (ret == 0) {
-			return 0;
-		}
-	}
+	val = data->write_byte;
 
-	if (data->write_fail_reg == reg ||
-	    data->write_fail_reg == BMA_EMUL_FAIL_ALL_REG) {
+	if (bytes > 2) {
+		LOG_ERR("Too long write command");
 		return -EIO;
 	}
 
+	/* This write only selected register for I2C read message */
+	if (bytes < 2) {
+		return 0;
+	}
+
 	if (reg <= BMA2x2_STAT_FIFO_ADDR ||
 	    reg >= BMA2x2_FIFO_DATA_OUTPUT_ADDR) {
 		if (data->error_on_ro_write) {
@@ -907,7 +802,7 @@ static int bma_emul_get_acc_val(struct i2c_emul *emul, int lsb_reg,
 	int msb_reg;
 	int shift;
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 
 	if (lsb) {
 		*lsb_read = 1;
@@ -941,46 +836,47 @@ static int bma_emul_get_acc_val(struct i2c_emul *emul, int lsb_reg,
 	return 0;
 }
 
+/** Check description in emul_bma255.h */
+int bma_emul_access_reg(struct i2c_emul *emul, int reg, int bytes, bool read)
+{
+	/*
+	 * Exclude first byte (select register) from total number of bytes
+	 * in I2C write message
+	 */
+	if (!read) {
+		bytes--;
+	}
+
+	if (reg <= BMA2x2_FIFO_DATA_OUTPUT_ADDR &&
+	    reg + bytes >= BMA2x2_FIFO_DATA_OUTPUT_ADDR) {
+		return BMA2x2_FIFO_DATA_OUTPUT_ADDR;
+	}
+
+	return reg + bytes;
+}
+
 /**
  * @brief Handle I2C read message. Response is obtained from reg field of bma
  *        emul data. When accessing accelerometer value, register data is first
- *        computed using internal emulator state. Before default handler, custom
- *        user read function is called if provided.
+ *        computed using internal emulator state.
  *
  * @param emul Pointer to BMA255 emulator
  * @param reg Register address to read
- * @param buf Pointer where resultat should be stored
+ * @param val Pointer where resultat should be stored
+ * @param bytes Number of bytes in I2C read message
  *
  * @return 0 on success
  * @return -EIO on error
  */
-static int bma_emul_handle_read(struct i2c_emul *emul, int reg, char *buf)
+static int bma_emul_handle_read(struct i2c_emul *emul, int reg, uint8_t *val,
+				int bytes)
 {
 	struct bma_emul_data *data;
 	int ret;
 
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 
-	if (reg > BMA2x2_FIFO_DATA_OUTPUT_ADDR) {
-		reg = BMA2x2_FIFO_DATA_OUTPUT_ADDR;
-	}
-
-	if (data->read_func) {
-		ret = data->read_func(emul, reg, data->read_func_data);
-		if (ret < 0) {
-			return -EIO;
-		} else if (ret == 0) {
-			/* Immediately return value set by custom function */
-			*buf = data->reg[reg];
-
-			return 0;
-		}
-	}
-
-	if (data->read_fail_reg == reg ||
-	    data->read_fail_reg == BMA_EMUL_FAIL_ALL_REG) {
-		return -EIO;
-	}
+	reg = bma_emul_access_reg(emul, reg, bytes, true /* = read */);
 
 	switch (reg) {
 	case BMA2x2_X_AXIS_LSB_ADDR:
@@ -1024,126 +920,30 @@ static int bma_emul_handle_read(struct i2c_emul *emul, int reg, char *buf)
 		break;
 	}
 
-	*buf = data->reg[reg];
+	*val = data->reg[reg];
 
 	return 0;
 }
 
 /**
- * @biref Emulate an I2C transfer to a BMA255 accelerometer
- *
- * This handles simple reads and writes
+ * @brief Handle I2C write message. Saves data that will be stored in register.
  *
- * @param emul I2C emulation information
- * @param msgs List of messages to process
- * @param num_msgs Number of messages to process
- * @param addr Address of the I2C target device
+ * @param emul Pointer to BMA emulator
+ * @param reg Register address that is accessed
+ * @param val Data to write to the register
+ * @param bytes Number of bytes already handled in this read message
  *
- * @retval 0 If successful
- * @retval -EIO General input / output error
+ * @return 0 on success
+ * @return -EIO on error
  */
-static int bma_emul_transfer(struct i2c_emul *emul, struct i2c_msg *msgs,
-			     int num_msgs, int addr)
+static int bma_emul_write_byte(struct i2c_emul *emul, int reg, uint8_t val,
+			       int bytes)
 {
-	const struct bma_emul_cfg *cfg;
 	struct bma_emul_data *data;
-	int ret, i, reg;
-	bool read;
-
-	data = CONTAINER_OF(emul, struct bma_emul_data, emul);
-	cfg = data->cfg;
 
-	if (cfg->addr != addr) {
-		LOG_ERR("Address mismatch, expected %02x, got %02x", cfg->addr,
-			addr);
-		return -EIO;
-	}
+	data = BMA_DATA_FROM_I2C_EMUL(emul);
 
-	i2c_dump_msgs("emul", msgs, num_msgs, addr);
-
-	for (; num_msgs > 0; num_msgs--, msgs++) {
-		read = msgs->flags & I2C_MSG_READ;
-
-		switch (data->msg_state) {
-		case BMA_EMUL_NONE_MSG:
-			data->msg_byte = 0;
-			break;
-		case BMA_EMUL_IN_WRITE:
-			if (read) {
-				/* Finish write command */
-				if (data->msg_byte == 2) {
-					k_mutex_lock(&data->data_mtx,
-						     K_FOREVER);
-					ret = bma_emul_handle_write(emul,
-							data->cur_reg,
-							data->write_byte);
-					k_mutex_unlock(&data->data_mtx);
-					if (ret) {
-						return -EIO;
-					}
-				}
-				data->msg_byte = 0;
-			}
-			break;
-		case BMA_EMUL_IN_READ:
-			if (!read) {
-				data->msg_byte = 0;
-			}
-			break;
-		}
-		data->msg_state = read ? BMA_EMUL_IN_READ : BMA_EMUL_IN_WRITE;
-
-		if (msgs->flags & I2C_MSG_STOP) {
-			data->msg_state = BMA_EMUL_NONE_MSG;
-		}
-
-		if (!read) {
-			/* Dispatch wrtie command */
-			for (i = 0; i < msgs->len; i++) {
-				switch (data->msg_byte) {
-				case 0:
-					data->cur_reg = msgs->buf[i];
-					break;
-				case 1:
-					data->write_byte = msgs->buf[i];
-					break;
-				default:
-					data->msg_state = BMA_EMUL_NONE_MSG;
-					LOG_ERR("Too long write command");
-					return -EIO;
-				}
-				data->msg_byte++;
-			}
-
-			/* Execute write command */
-			if (msgs->flags & I2C_MSG_STOP && data->msg_byte == 2) {
-				k_mutex_lock(&data->data_mtx, K_FOREVER);
-				ret = bma_emul_handle_write(emul, data->cur_reg,
-							    data->write_byte);
-				k_mutex_unlock(&data->data_mtx);
-				if (ret) {
-					return -EIO;
-				}
-			}
-		} else {
-			/* Dispatch read command */
-			for (i = 0; i < msgs->len; i++) {
-				reg = data->cur_reg + data->msg_byte;
-				if (reg > BMA2x2_FIFO_DATA_OUTPUT_ADDR) {
-					reg = BMA2x2_FIFO_DATA_OUTPUT_ADDR;
-				} else {
-					data->msg_byte++;
-				}
-				k_mutex_lock(&data->data_mtx, K_FOREVER);
-				ret = bma_emul_handle_read(emul, reg,
-							   &(msgs->buf[i]));
-				k_mutex_unlock(&data->data_mtx);
-				if (ret) {
-					return -EIO;
-				}
-			}
-		}
-	}
+	data->write_byte = val;
 
 	return 0;
 }
@@ -1151,7 +951,7 @@ static int bma_emul_transfer(struct i2c_emul *emul, struct i2c_msg *msgs,
 /* Device instantiation */
 
 static struct i2c_emul_api bma_emul_api = {
-	.transfer = bma_emul_transfer,
+	.transfer = i2c_common_emul_transfer,
 };
 
 /**
@@ -1168,15 +968,15 @@ static struct i2c_emul_api bma_emul_api = {
 static int bma_emul_init(const struct emul *emul,
 			 const struct device *parent)
 {
-	const struct bma_emul_cfg *cfg = emul->cfg;
-	struct bma_emul_data *data = cfg->data;
+	const struct i2c_common_emul_cfg *cfg = emul->cfg;
+	struct i2c_common_emul_data *data = cfg->data;
 	int ret;
 
 	data->emul.api = &bma_emul_api;
 	data->emul.addr = cfg->addr;
 	data->i2c = parent;
 	data->cfg = cfg;
-	k_mutex_init(&data->data_mtx);
+	i2c_common_emul_init(data);
 
 	ret = i2c_emul_register(parent, emul->dev_label, &data->emul);
 
@@ -1205,17 +1005,20 @@ static int bma_emul_init(const struct emul *emul,
 		.lsb_x_read = 0,					\
 		.lsb_y_read = 0,					\
 		.lsb_z_read = 0,					\
-		.msg_state = BMA_EMUL_NONE_MSG,				\
-		.cur_reg = 0,						\
-		.write_func = NULL,					\
-		.read_func = NULL,					\
-		.write_fail_reg = BMA_EMUL_NO_FAIL_REG,			\
-		.read_fail_reg = BMA_EMUL_NO_FAIL_REG,			\
+		.common = {						\
+			.start_write = NULL,				\
+			.write_byte = bma_emul_write_byte,		\
+			.finish_write = bma_emul_handle_write,		\
+			.start_read = NULL,				\
+			.read_byte = bma_emul_handle_read,		\
+			.finish_read = NULL,				\
+			.access_reg = bma_emul_access_reg,		\
+		},							\
 	};								\
 									\
-	static const struct bma_emul_cfg bma_emul_cfg_##n = {		\
+	static const struct i2c_common_emul_cfg bma_emul_cfg_##n = {	\
 		.i2c_label = DT_INST_BUS_LABEL(n),			\
-		.data = &bma_emul_data_##n,				\
+		.data = &bma_emul_data_##n.common,			\
 		.addr = DT_INST_REG_ADDR(n),				\
 	};								\
 	EMUL_DEFINE(bma_emul_init, DT_DRV_INST(n), &bma_emul_cfg_##n)
@@ -1223,7 +1026,7 @@ static int bma_emul_init(const struct emul *emul,
 DT_INST_FOREACH_STATUS_OKAY(BMA255_EMUL)
 
 #define BMA255_EMUL_CASE(n)					\
-	case DT_INST_DEP_ORD(n): return &bma_emul_data_##n.emul;
+	case DT_INST_DEP_ORD(n): return &bma_emul_data_##n.common.emul;
 
 /** Check description in emul_bma255.h */
 struct i2c_emul *bma_emul_get(int ord)
diff --git a/zephyr/emul/emul_bmi.c b/zephyr/emul/emul_bmi.c
index d6807d3961..be564fe625 100644
--- a/zephyr/emul/emul_bmi.c
+++ b/zephyr/emul/emul_bmi.c
@@ -14,31 +14,21 @@ LOG_MODULE_REGISTER(emul_bmi);
 #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"
 
-/**
- * Describe if there is no ongoing I2C message or if there is message handled
- * at the moment (last message doesn't ended with stop or write is not followed
- * by read).
- */
-enum bmi_emul_msg_state {
-	BMI_EMUL_NONE_MSG,
-	BMI_EMUL_IN_WRITE,
-	BMI_EMUL_IN_READ
-};
+#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 {
-	/** I2C emulator detail */
-	struct i2c_emul emul;
-	/** BMI device being emulated */
-	const struct device *i2c;
-	/** Configuration information */
-	const struct bmi_emul_cfg *cfg;
+	/** Common I2C data */
+	struct i2c_common_emul_data common;
 
 	/** Current state of all emulated BMI registers */
 	uint8_t reg[BMI_EMUL_MAX_REG];
@@ -71,29 +61,9 @@ struct bmi_emul_data {
 	/** Return error when trying to read WO register */
 	bool error_on_wo_read;
 
-	/** Current state of I2C bus (if emulator is handling message) */
-	enum bmi_emul_msg_state msg_state;
-	/** Number of already handled bytes in ongoing message */
-	int msg_byte;
-	/** Register selected in last write command */
-	uint8_t cur_reg;
 	/** Value of data byte in ongoing write message */
 	uint8_t write_byte;
 
-	/** Custom write function called on I2C write opperation */
-	bmi_emul_write_func write_func;
-	/** Data passed to custom write function */
-	void *write_func_data;
-	/** Custom read function called on I2C read opperation */
-	bmi_emul_read_func read_func;
-	/** Data passed to custom read function */
-	void *read_func_data;
-
-	/** Control if read should fail on given register */
-	int read_fail_reg;
-	/** Control if write should fail on given register */
-	int write_fail_reg;
-
 	/** List of FIFO frames */
 	struct bmi_emul_frame *fifo_frame;
 	/** First FIFO frame in byte format */
@@ -114,63 +84,8 @@ struct bmi_emul_data {
 	int type;
 	/** Pointer to data specific for emulated model of BMI */
 	const struct bmi_emul_type_data *type_data;
-
-	/** Mutex used to control access to emulator data */
-	struct k_mutex data_mtx;
 };
 
-/** Static configuration for the emulator */
-struct bmi_emul_cfg {
-	/** Label of the I2C bus this emulator connects to */
-	const char *i2c_label;
-	/** Pointer to run-time data */
-	struct bmi_emul_data *data;
-	/** Address of BMI on i2c bus */
-	uint16_t addr;
-};
-
-/** Check description in emul_bmi.h */
-int bmi_emul_lock_data(struct i2c_emul *emul, k_timeout_t timeout)
-{
-	struct bmi_emul_data *data;
-
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
-
-	return k_mutex_lock(&data->data_mtx, timeout);
-}
-
-/** Check description in emul_bmi.h */
-int bmi_emul_unlock_data(struct i2c_emul *emul)
-{
-	struct bmi_emul_data *data;
-
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
-
-	return k_mutex_unlock(&data->data_mtx);
-}
-
-/** Check description in emul_bmi.h */
-void bmi_emul_set_write_func(struct i2c_emul *emul,
-			     bmi_emul_write_func func, void *data)
-{
-	struct bmi_emul_data *emul_data;
-
-	emul_data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
-	emul_data->write_func = func;
-	emul_data->write_func_data = data;
-}
-
-/** Check description in emul_bmi.h */
-void bmi_emul_set_read_func(struct i2c_emul *emul,
-			    bmi_emul_read_func func, void *data)
-{
-	struct bmi_emul_data *emul_data;
-
-	emul_data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
-	emul_data->read_func = func;
-	emul_data->read_func_data = data;
-}
-
 /** Check description in emul_bmi.h */
 void bmi_emul_set_reg(struct i2c_emul *emul, int reg, uint8_t val)
 {
@@ -180,7 +95,7 @@ void bmi_emul_set_reg(struct i2c_emul *emul, int reg, uint8_t val)
 		return;
 	}
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 	data->reg[reg] = val;
 }
 
@@ -193,29 +108,11 @@ uint8_t bmi_emul_get_reg(struct i2c_emul *emul, int reg)
 		return 0;
 	}
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
 	return data->reg[reg];
 }
 
-/** Check description in emul_bmi.h */
-void bmi_emul_set_read_fail_reg(struct i2c_emul *emul, int reg)
-{
-	struct bmi_emul_data *data;
-
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
-	data->read_fail_reg = reg;
-}
-
-/** Check description in emul_bmi.h */
-void bmi_emul_set_write_fail_reg(struct i2c_emul *emul, int reg)
-{
-	struct bmi_emul_data *data;
-
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
-	data->write_fail_reg = reg;
-}
-
 /**
  * @brief Convert @p val to two's complement representation. It makes sure that
  *        bit representation is correct even on platforms which represent
@@ -365,7 +262,7 @@ int16_t bmi_emul_get_off(struct i2c_emul *emul, enum bmi_emul_axis axis)
 {
 	struct bmi_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
 	switch (axis) {
 	case BMI_EMUL_ACC_X:
@@ -393,7 +290,7 @@ void bmi_emul_set_off(struct i2c_emul *emul, enum bmi_emul_axis axis,
 	uint16_t gyr_off;
 	uint8_t gyr98_shift;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
 	switch (axis) {
 	case BMI_EMUL_ACC_X:
@@ -449,7 +346,7 @@ int32_t bmi_emul_get_value(struct i2c_emul *emul, enum bmi_emul_axis axis)
 {
 	struct bmi_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
 	switch (axis) {
 	case BMI_EMUL_ACC_X:
@@ -475,7 +372,7 @@ void bmi_emul_set_value(struct i2c_emul *emul, enum bmi_emul_axis axis,
 {
 	struct bmi_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
 	switch (axis) {
 	case BMI_EMUL_ACC_X:
@@ -504,7 +401,7 @@ void bmi_emul_set_err_on_ro_write(struct i2c_emul *emul, bool set)
 {
 	struct bmi_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 	data->error_on_ro_write = set;
 }
 
@@ -513,7 +410,7 @@ void bmi_emul_set_err_on_rsvd_write(struct i2c_emul *emul, bool set)
 {
 	struct bmi_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 	data->error_on_rsvd_write = set;
 }
 
@@ -522,7 +419,7 @@ void bmi_emul_set_err_on_wo_read(struct i2c_emul *emul, bool set)
 {
 	struct bmi_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 	data->error_on_wo_read = set;
 }
 
@@ -531,7 +428,7 @@ void bmi_emul_simulate_cmd_exec_time(struct i2c_emul *emul, bool set)
 {
 	struct bmi_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 	data->simulate_command_exec_time = set;
 }
 
@@ -540,7 +437,7 @@ void bmi_emul_set_skipped_frames(struct i2c_emul *emul, uint8_t skip)
 {
 	struct bmi_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
 	data->fifo_skip = skip;
 }
@@ -569,7 +466,7 @@ static void bmi_emul_set_sensortime_reg(struct i2c_emul *emul, uint8_t *reg)
 	uint32_t twos_comp_val;
 	int64_t time;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
 	time = bmi_emul_get_sensortime();
 
@@ -596,7 +493,7 @@ static void bmi_emul_set_data_reg(struct i2c_emul *emul, int32_t val,
 	struct bmi_emul_data *data;
 	uint32_t twos_comp_val;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
 	twos_comp_val = bmi_emul_val_to_twos_comp(val);
 
@@ -625,7 +522,7 @@ static uint8_t bmi_emul_get_frame_len(struct i2c_emul *emul,
 	struct bmi_emul_data *data;
 	int len;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
 	/* Empty FIFO frame */
 	if (frame == NULL) {
@@ -695,7 +592,7 @@ static void bmi_emul_set_current_frame(struct i2c_emul *emul,
 	struct bmi_emul_data *data;
 	int i = 0;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	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,
@@ -788,7 +685,7 @@ static void bmi_emul_updata_int_off(struct i2c_emul *emul)
 	uint16_t gyr_nvm;
 	uint8_t gyr98;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	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]);
@@ -821,7 +718,7 @@ static void bmi_emul_restore_nvm(struct i2c_emul *emul)
 	struct bmi_emul_data *data;
 	int i;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
 	ASSERT(data->type_data->nvm_len <= BMI_EMUL_MAX_NVM_REGS);
 
@@ -838,7 +735,7 @@ void bmi_emul_flush_fifo(struct i2c_emul *emul, bool tag_time, bool header)
 {
 	struct bmi_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
 	data->fifo_skip = 0;
 	data->fifo_frame = NULL;
@@ -854,7 +751,7 @@ void bmi_emul_reset_common(struct i2c_emul *emul, bool tag_time, bool header)
 {
 	struct bmi_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
 	/* Restore registers backed in NVM */
 	bmi_emul_restore_nvm(emul);
@@ -871,7 +768,7 @@ void bmi_emul_set_cmd_end_time(struct i2c_emul *emul, int time)
 {
 	struct bmi_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
 	data->cmd_end_time = k_uptime_get_32() + time;
 }
@@ -881,7 +778,7 @@ bool bmi_emul_is_cmd_end(struct i2c_emul *emul)
 {
 	struct bmi_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
 	/* We are simulating command execution time and it doesn't expired */
 	if (data->simulate_command_exec_time &&
@@ -893,42 +790,30 @@ bool bmi_emul_is_cmd_end(struct i2c_emul *emul)
 }
 
 /**
- * @brief Handle I2C write message. Before any handling, custom function
- *        is called if provided. Next 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. Emulator may be configured to fail on write to specific
- *        register.
+ * @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 byte Number of handled bytes in this write command
  * @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, int byte,
-				 uint8_t val)
+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 = CONTAINER_OF(emul, struct bmi_emul_data, emul);
-
-	if (data->write_func) {
-		ret = data->write_func(emul, reg, byte, val,
-				       data->write_func_data);
-		if (ret < 0) {
-			return -EIO;
-		} else if (ret == 0) {
-			return 0;
-		}
-	}
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
-	ret = data->type_data->handle_write(data->reg, emul, &reg, byte, val);
+	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) {
@@ -940,11 +825,6 @@ static int bmi_emul_handle_write(struct i2c_emul *emul, int reg, int byte,
 		return -EIO;
 	}
 
-	if (data->write_fail_reg == reg ||
-	    data->write_fail_reg == BMI_EMUL_FAIL_ALL_REG) {
-		return -EIO;
-	}
-
 	rsvd_mask = data->type_data->rsvd_mask[reg];
 
 	if (data->error_on_rsvd_write && rsvd_mask & val) {
@@ -984,7 +864,7 @@ void bmi_emul_state_to_reg(struct i2c_emul *emul, int acc_shift,
 	int32_t val[3];
 	int i;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
 	if (gyr_off_en) {
 		val[0] = data->gyr_x - data->off_gyr_x;
@@ -1025,7 +905,7 @@ 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 = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
 	if (data->fifo_frame == NULL) {
 		data->fifo_frame = frame;
@@ -1045,7 +925,7 @@ uint16_t bmi_emul_fifo_len(struct i2c_emul *emul, bool tag_time, bool header)
 	struct bmi_emul_data *data;
 	uint16_t len = 0;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
 	if (data->fifo_skip != 0 && header) {
 		len += 2;
@@ -1072,7 +952,7 @@ uint8_t bmi_emul_get_fifo_data(struct i2c_emul *emul, int byte,
 	struct bmi_emul_data *data;
 	int ret;
 
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
 	if (byte == 0) {
 		/* Repeat uncompleated read of frame */
@@ -1108,158 +988,40 @@ uint8_t bmi_emul_get_fifo_data(struct i2c_emul *emul, int byte,
 }
 
 /**
- * @brief Handle I2C read message. Before any handling, custom function
- *        is called if provided. Next BMI model specific read function is
- *        called. It is checked if accessed register isn't WO. Emulator may
- *        be configured to fail on given register read.
+ * @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 byte Byte which is accessed during block 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, int byte,
-				char *buf)
+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 = CONTAINER_OF(emul, struct bmi_emul_data, emul);
-
-	if (data->read_func) {
-		ret = data->read_func(emul, reg, byte, data->read_func_data);
-		if (ret < 0) {
-			return -EIO;
-		} else if (ret == 0) {
-			/* Immediately return value set by custom function */
-			*buf = data->reg[reg];
-
-			return 0;
-		}
-	}
+	data = BMI_DATA_FROM_I2C_EMUL(emul);
 
-	ret = data->type_data->handle_read(data->reg, emul, &reg, byte, buf);
+	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;
 	}
 
-	if (data->read_fail_reg == reg ||
-	    data->read_fail_reg == BMI_EMUL_FAIL_ALL_REG) {
-		return -EIO;
-	}
-
-	return 0;
-}
-
-/**
- * @biref Emulate an I2C transfer to a BMI accelerometer
- *
- * This handles simple reads and writes
- *
- * @param emul I2C emulation information
- * @param msgs List of messages to process
- * @param num_msgs Number of messages to process
- * @param addr Address of the I2C target device
- *
- * @retval 0 If successful
- * @retval -EIO General input / output error
- */
-static int bmi_emul_transfer(struct i2c_emul *emul, struct i2c_msg *msgs,
-			     int num_msgs, int addr)
-{
-	const struct bmi_emul_cfg *cfg;
-	struct bmi_emul_data *data;
-	int ret, i;
-	bool read;
-
-	data = CONTAINER_OF(emul, struct bmi_emul_data, emul);
-	cfg = data->cfg;
-
-	if (cfg->addr != addr) {
-		LOG_ERR("Address mismatch, expected %02x, got %02x", cfg->addr,
-			addr);
-		return -EIO;
-	}
-
-	i2c_dump_msgs("emul", msgs, num_msgs, addr);
-
-	for (; num_msgs > 0; num_msgs--, msgs++) {
-		read = msgs->flags & I2C_MSG_READ;
-
-		switch (data->msg_state) {
-		case BMI_EMUL_NONE_MSG:
-			data->msg_byte = 0;
-			break;
-		case BMI_EMUL_IN_WRITE:
-			if (read) {
-				data->msg_byte = 0;
-			}
-			break;
-		case BMI_EMUL_IN_READ:
-			if (!read) {
-				data->msg_byte = 0;
-			}
-			break;
-		}
-		data->msg_state = read ? BMI_EMUL_IN_READ : BMI_EMUL_IN_WRITE;
-
-		if (msgs->flags & I2C_MSG_STOP) {
-			data->msg_state = BMI_EMUL_NONE_MSG;
-		}
-
-		if (!read) {
-			/* Dispatch wrtie command */
-			i = 0;
-			/*
-			 * Save first byte of write command as currently
-			 * accessed register.
-			 */
-			if (data->msg_byte == 0 && msgs->len) {
-				data->cur_reg = msgs->buf[0];
-				i++;
-				data->msg_byte++;
-			}
-
-			/* Handle rest of write command bytes */
-			for (; i < msgs->len; i++) {
-				k_mutex_lock(&data->data_mtx, K_FOREVER);
-				ret = bmi_emul_handle_write(emul, data->cur_reg,
-							    data->msg_byte,
-							    msgs->buf[i]);
-				k_mutex_unlock(&data->data_mtx);
-				if (ret) {
-					return -EIO;
-				}
-				data->msg_byte++;
-			}
-		} else {
-			/* Dispatch read command */
-			for (i = 0; i < msgs->len; i++) {
-				k_mutex_lock(&data->data_mtx, K_FOREVER);
-				ret = bmi_emul_handle_read(emul, data->cur_reg,
-							   data->msg_byte,
-							   &(msgs->buf[i]));
-				k_mutex_unlock(&data->data_mtx);
-				if (ret) {
-					return -EIO;
-				}
-				data->msg_byte++;
-			}
-		}
-	}
-
 	return 0;
 }
 
 /* Device instantiation */
 
 static struct i2c_emul_api bmi_emul_api = {
-	.transfer = bmi_emul_transfer,
+	.transfer = i2c_common_emul_transfer,
 };
 
 /**
@@ -1276,28 +1038,34 @@ static struct i2c_emul_api bmi_emul_api = {
 static int bmi_emul_init(const struct emul *emul,
 			 const struct device *parent)
 {
-	const struct bmi_emul_cfg *cfg = emul->cfg;
-	struct bmi_emul_data *data = cfg->data;
+	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;
-	k_mutex_init(&data->data_mtx);
+	i2c_common_emul_init(data);
+
+	bmi_data = CONTAINER_OF(data, struct bmi_emul_data, common);
 
-	switch (data->type) {
+	switch (bmi_data->type) {
 	case BMI_EMUL_160:
-		data->type_data = get_bmi160_emul_type_data();
+		bmi_data->type_data = get_bmi160_emul_type_data();
 		break;
 	case BMI_EMUL_260:
-		data->type_data = get_bmi260_emul_type_data();
+		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);
 
-	data->type_data->reset(data->reg, &data->emul);
+	bmi_data->type_data->reset(bmi_data->reg, &data->emul);
 
 	return ret;
 }
@@ -1311,17 +1079,20 @@ static int bmi_emul_init(const struct emul *emul,
 		.simulate_command_exec_time = DT_INST_PROP(n,		\
 					simulate_command_exec_time),	\
 		.type = DT_ENUM_TOKEN(DT_DRV_INST(n), device_model),	\
-		.msg_state = BMI_EMUL_NONE_MSG,				\
-		.cur_reg = 0,						\
-		.write_func = NULL,					\
-		.read_func = NULL,					\
-		.write_fail_reg = BMI_EMUL_NO_FAIL_REG,			\
-		.read_fail_reg = BMI_EMUL_NO_FAIL_REG,			\
+		.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 bmi_emul_cfg bmi_emul_cfg_##n = {		\
+	static const struct i2c_common_emul_cfg bmi_emul_cfg_##n = {	\
 		.i2c_label = DT_INST_BUS_LABEL(n),			\
-		.data = &bmi_emul_data_##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)
@@ -1329,7 +1100,7 @@ static int bmi_emul_init(const struct emul *emul,
 DT_INST_FOREACH_STATUS_OKAY(BMI_EMUL)
 
 #define BMI_EMUL_CASE(n)					\
-	case DT_INST_DEP_ORD(n): return &bmi_emul_data_##n.emul;
+	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)
diff --git a/zephyr/emul/emul_bmi160.c b/zephyr/emul/emul_bmi160.c
index d080574de5..2a68b688ff 100644
--- a/zephyr/emul/emul_bmi160.c
+++ b/zephyr/emul/emul_bmi160.c
@@ -552,7 +552,7 @@ static void bmi160_emul_end_cmd(uint8_t *regs, struct i2c_emul *emul)
  *
  * @param regs Pointer to array of emulator's registers
  * @param emul Pointer to BMI emulator
- * @param reg Pointer to accessed reg
+ * @param reg Register address that is accessed
  * @param byte Number of handled bytes in this write command
  * @param val Value that is being written
  *
@@ -561,7 +561,7 @@ static void bmi160_emul_end_cmd(uint8_t *regs, struct i2c_emul *emul)
  * @return -EIO on error
  */
 static int bmi160_emul_handle_write(uint8_t *regs, struct i2c_emul *emul,
-				    int *reg, int byte, uint8_t val)
+				    int reg, int byte, uint8_t val)
 {
 	bool tag_time;
 	bool header;
@@ -571,8 +571,8 @@ static int bmi160_emul_handle_write(uint8_t *regs, struct i2c_emul *emul,
 		return -EIO;
 	}
 
-	if (*reg <= BMI160_FIFO_DATA ||
-	    (*reg >= BMI160_STEP_CNT_0 && *reg <= BMI160_STEP_CNT_1)) {
+	if (reg <= BMI160_FIFO_DATA ||
+	    (reg >= BMI160_STEP_CNT_0 && reg <= BMI160_STEP_CNT_1)) {
 		return BMI_EMUL_ACCESS_E;
 	}
 
@@ -582,7 +582,7 @@ static int bmi160_emul_handle_write(uint8_t *regs, struct i2c_emul *emul,
 		bmi160_emul_end_cmd(regs, emul);
 	}
 
-	switch (*reg) {
+	switch (reg) {
 	case BMI160_CMD_REG:
 		if (regs[BMI160_CMD_REG] != BMI160_CMD_NOOP) {
 			LOG_ERR("Issued command before previous end");
@@ -607,6 +607,36 @@ static int bmi160_emul_handle_write(uint8_t *regs, struct i2c_emul *emul,
 }
 
 /**
+ * @brief Get currently accessed register. It is first register plus number of
+ *        handled bytes for all registers except BMI160_FIFO_DATA for which
+ *        address incrementation is disabled.
+ *
+ * @param emul Pointer to BMI emulator
+ * @param reg First byte of last write message
+ * @param bytes Number of bytes already handled from current message
+ * @param read If currently handled is read message
+ *
+ * @return Currently accessed register
+ */
+static int bmi160_emul_access_reg(struct i2c_emul *emul, int reg, int byte,
+				  bool read)
+{
+	if (!read) {
+		return reg;
+	}
+
+	/*
+	 * If register is FIFO data, then read data from FIFO.
+	 * Else block read access subsequent registers.
+	 */
+	if (reg <= BMI160_FIFO_DATA && reg + byte >= BMI160_FIFO_DATA) {
+		return BMI160_FIFO_DATA;
+	}
+
+	return reg + byte;
+}
+
+/**
  * @brief BMI160 specific read function. It handle block reads but only if
  *        device is not suspended. FIFO data register is trap register, so
  *        after reaching it, register address is not increased on block reads.
@@ -616,7 +646,7 @@ static int bmi160_emul_handle_write(uint8_t *regs, struct i2c_emul *emul,
  *
  * @param regs Pointer to array of emulator's registers
  * @param emul Pointer to BMI emulator
- * @param reg Pointer to accessed reg
+ * @param reg Register address that is accessed
  * @param byte Byte which is accessed during block read
  * @param buf Pointer where read byte should be stored
  *
@@ -625,7 +655,7 @@ static int bmi160_emul_handle_write(uint8_t *regs, struct i2c_emul *emul,
  * @return -EIO on other error
  */
 static int bmi160_emul_handle_read(uint8_t *regs, struct i2c_emul *emul,
-				   int *reg, int byte, char *buf)
+				   int reg, int byte, char *buf)
 {
 	uint16_t fifo_len;
 	bool acc_off_en;
@@ -634,17 +664,12 @@ static int bmi160_emul_handle_read(uint8_t *regs, struct i2c_emul *emul,
 	bool header;
 	int gyr_shift;
 	int acc_shift;
+	int fifo_byte;
 
-	/*
-	 * If register is FIFO data, then read data from FIFO.
-	 * Else block read access subsequent registers.
-	 */
-	if (*reg <= BMI160_FIFO_DATA && *reg + byte >= BMI160_FIFO_DATA) {
-		byte -= *reg - BMI160_FIFO_DATA;
-		*reg = BMI160_FIFO_DATA;
-	} else {
-		*reg += byte;
-	}
+	/* Get number of bytes readed from FIFO */
+	fifo_byte = byte - (reg - BMI160_FIFO_DATA);
+
+	reg = bmi160_emul_access_reg(emul, reg, byte, true /* = read */);
 
 	/* Stop on going command if required */
 	if (regs[BMI160_CMD_REG] != BMI160_CMD_NOOP &&
@@ -665,7 +690,7 @@ static int bmi160_emul_handle_read(uint8_t *regs, struct i2c_emul *emul,
 	gyr_shift = bmi160_emul_gyr_range_to_shift(regs[BMI160_GYR_RANGE]);
 	acc_shift = bmi160_emul_acc_range_to_shift(regs[BMI160_ACC_RANGE]);
 
-	switch (*reg) {
+	switch (reg) {
 	case BMI160_GYR_X_L_G:
 	case BMI160_GYR_X_H_G:
 	case BMI160_GYR_Y_L_G:
@@ -702,13 +727,13 @@ static int bmi160_emul_handle_read(uint8_t *regs, struct i2c_emul *emul,
 		}
 		break;
 	case BMI160_FIFO_DATA:
-		regs[*reg] = bmi_emul_get_fifo_data(emul, byte, tag_time,
-						    header, acc_shift,
-						    gyr_shift);
+		regs[reg] = bmi_emul_get_fifo_data(emul, fifo_byte, tag_time,
+						   header, acc_shift,
+						   gyr_shift);
 		break;
 	}
 
-	*buf = regs[*reg];
+	*buf = regs[reg];
 
 	return 0;
 }
@@ -728,6 +753,7 @@ struct bmi_emul_type_data bmi160_emul = {
 	.sensortime_follow_config_frame = false,
 	.handle_write = bmi160_emul_handle_write,
 	.handle_read = bmi160_emul_handle_read,
+	.access_reg = bmi160_emul_access_reg,
 	.reset = bmi160_emul_reset,
 	.rsvd_mask = bmi_emul_160_rsvd_mask,
 	.nvm_reg = bmi160_nvm_reg,
diff --git a/zephyr/emul/emul_bmi260.c b/zephyr/emul/emul_bmi260.c
index 69b0e86871..235b1e219e 100644
--- a/zephyr/emul/emul_bmi260.c
+++ b/zephyr/emul/emul_bmi260.c
@@ -328,6 +328,41 @@ static void bmi260_emul_end_cmd(uint8_t *regs, struct i2c_emul *emul)
 }
 
 /**
+ * @brief Get currently accessed register. It is first register plus number of
+ *        handled bytes for all registers except BMI260_FIFO_DATA and
+ *        BMI260_INIT_DATA for which address incrementation is disabled.
+ *
+ * @param emul Pointer to BMI emulator
+ * @param reg First byte of last write message
+ * @param bytes Number of bytes already handled from current message
+ * @param read If currently handled is read message
+ *
+ * @return Currently accessed register
+ */
+static int bmi260_emul_access_reg(struct i2c_emul *emul, int reg, int byte,
+				  bool read)
+{
+	/* Ignore first byte which sets starting register */
+	if (!read) {
+		byte -= 1;
+	}
+
+	/*
+	 * If register is FIFO data, then read data from FIFO.
+	 * Init data is also block, but it is not implemented in emulator.
+	 * Else block read access subsequent registers.
+	 */
+	if (reg <= BMI260_FIFO_DATA && reg + byte >= BMI260_FIFO_DATA) {
+		return BMI260_FIFO_DATA;
+	} else if (reg <= BMI260_INIT_DATA &&
+		   reg + byte >= BMI260_INIT_DATA) {
+		return BMI260_INIT_DATA;
+	}
+
+	return reg + byte;
+}
+
+/**
  * @brief BMI260 specific write function. It handle block writes. Init data
  *        register is trap register, so after reaching it, register address
  *        is not increased on block writes. Check if read only register is not
@@ -338,7 +373,7 @@ static void bmi260_emul_end_cmd(uint8_t *regs, struct i2c_emul *emul)
  *
  * @param regs Pointer to array of emulator's registers
  * @param emul Pointer to BMI emulator
- * @param reg Pointer to accessed reg
+ * @param reg Register address that is accessed
  * @param byte Number of handled bytes in this write command
  * @param val Value that is being written
  *
@@ -347,24 +382,16 @@ static void bmi260_emul_end_cmd(uint8_t *regs, struct i2c_emul *emul)
  * @return -EIO on error
  */
 static int bmi260_emul_handle_write(uint8_t *regs, struct i2c_emul *emul,
-				    int *reg, int byte, uint8_t val)
+				    int reg, int byte, uint8_t val)
 {
 	uint8_t mask;
 	bool tag_time;
 	bool header;
 
-	/* Ignore first byte which sets starting register */
-	byte -= 1;
-
-	if (*reg <= BMI260_INIT_DATA && *reg + byte >= BMI260_INIT_DATA) {
-		byte -= *reg - BMI260_INIT_DATA;
-		*reg = BMI260_INIT_DATA;
-	} else {
-		*reg += byte;
-	}
+	reg = bmi260_emul_access_reg(emul, reg, byte, false /* = read */);
 
-	if (*reg <= BMI260_FIFO_DATA || *reg == BMI260_GYR_SELF_TEST_AXES ||
-	    *reg == BMI260_INTERNAL_ERROR || *reg == BMI260_SATURATION) {
+	if (reg <= BMI260_FIFO_DATA || reg == BMI260_GYR_SELF_TEST_AXES ||
+	    reg == BMI260_INTERNAL_ERROR || reg == BMI260_SATURATION) {
 		return BMI_EMUL_ACCESS_E;
 	}
 
@@ -377,7 +404,7 @@ static int bmi260_emul_handle_write(uint8_t *regs, struct i2c_emul *emul,
 	tag_time = regs[BMI260_FIFO_CONFIG_0] & BMI260_FIFO_TIME_EN;
 	header = regs[BMI260_FIFO_CONFIG_1] & BMI260_FIFO_HEADER_EN;
 
-	switch (*reg) {
+	switch (reg) {
 	case BMI260_CMD_REG:
 		if (regs[BMI260_CMD_REG] != 0) {
 			LOG_ERR("Issued command before previous end");
@@ -419,7 +446,7 @@ static int bmi260_emul_handle_write(uint8_t *regs, struct i2c_emul *emul,
  *
  * @param regs Pointer to array of emulator's registers
  * @param emul Pointer to BMI emulator
- * @param reg Pointer to accessed reg
+ * @param reg Register address that is accessed
  * @param byte Byte which is accessed during block read
  * @param buf Pointer where read byte should be stored
  *
@@ -428,7 +455,7 @@ static int bmi260_emul_handle_write(uint8_t *regs, struct i2c_emul *emul,
  * @return -EIO on other error
  */
 static int bmi260_emul_handle_read(uint8_t *regs, struct i2c_emul *emul,
-				   int *reg, int byte, char *buf)
+				   int reg, int byte, char *buf)
 {
 	uint16_t fifo_len;
 	bool acc_off_en;
@@ -437,24 +464,14 @@ static int bmi260_emul_handle_read(uint8_t *regs, struct i2c_emul *emul,
 	bool header;
 	int gyr_shift;
 	int acc_shift;
+	int fifo_byte;
 
-	/*
-	 * If register is FIFO data, then read data from FIFO.
-	 * Init data is also block, but it is not implemented in emulator.
-	 * Else block read access subsequent registers.
-	 */
-	if (*reg <= BMI260_FIFO_DATA && *reg + byte >= BMI260_FIFO_DATA) {
-		byte -= *reg - BMI260_FIFO_DATA;
-		*reg = BMI260_FIFO_DATA;
-	} else if (*reg <= BMI260_INIT_DATA &&
-		   *reg + byte >= BMI260_INIT_DATA) {
-		byte -= *reg - BMI260_INIT_DATA;
-		*reg = BMI260_INIT_DATA;
-	} else {
-		*reg += byte;
-	}
+	/* Get number of bytes readed from FIFO */
+	fifo_byte = byte - (reg - BMI260_FIFO_DATA);
+
+	reg = bmi260_emul_access_reg(emul, reg, byte, true /* = read */);
 
-	if (*reg == BMI260_CMD_REG) {
+	if (reg == BMI260_CMD_REG) {
 		*buf = 0;
 
 		return BMI_EMUL_ACCESS_E;
@@ -472,7 +489,7 @@ static int bmi260_emul_handle_read(uint8_t *regs, struct i2c_emul *emul,
 	gyr_shift = bmi260_emul_gyr_range_to_shift(regs[BMI260_GYR_RANGE]);
 	acc_shift = bmi260_emul_acc_range_to_shift(regs[BMI260_ACC_RANGE]);
 
-	switch (*reg) {
+	switch (reg) {
 	case BMI260_GYR_X_L_G:
 	case BMI260_GYR_X_H_G:
 	case BMI260_GYR_Y_L_G:
@@ -509,13 +526,13 @@ static int bmi260_emul_handle_read(uint8_t *regs, struct i2c_emul *emul,
 		}
 		break;
 	case BMI260_FIFO_DATA:
-		regs[*reg] = bmi_emul_get_fifo_data(emul, byte, tag_time,
-						    header, acc_shift,
-						    gyr_shift);
+		regs[reg] = bmi_emul_get_fifo_data(emul, fifo_byte, tag_time,
+						   header, acc_shift,
+						   gyr_shift);
 		break;
 	}
 
-	*buf = regs[*reg];
+	*buf = regs[reg];
 
 	return 0;
 }
@@ -537,6 +554,7 @@ struct bmi_emul_type_data bmi260_emul = {
 	.sensortime_follow_config_frame = true,
 	.handle_write = bmi260_emul_handle_write,
 	.handle_read = bmi260_emul_handle_read,
+	.access_reg = bmi260_emul_access_reg,
 	.reset = bmi260_emul_reset,
 	.rsvd_mask = bmi_emul_260_rsvd_mask,
 	.nvm_reg = bmi260_nvm_reg,
diff --git a/zephyr/emul/emul_smart_battery.c b/zephyr/emul/emul_smart_battery.c
index 392b9a292b..f7e13624cb 100644
--- a/zephyr/emul/emul_smart_battery.c
+++ b/zephyr/emul/emul_smart_battery.c
@@ -14,52 +14,29 @@ LOG_MODULE_REGISTER(smart_battery);
 #include <drivers/i2c.h>
 #include <drivers/i2c_emul.h>
 
+#include "emul/emul_common_i2c.h"
 #include "emul/emul_smart_battery.h"
 
 #include "crc8.h"
 #include "battery_smart.h"
 
+#define SBAT_DATA_FROM_I2C_EMUL(_emul)					     \
+	CONTAINER_OF(CONTAINER_OF(_emul, struct i2c_common_emul_data, emul), \
+		     struct sbat_emul_data, common)
+
 /** Run-time data used by the emulator */
 struct sbat_emul_data {
-	/** I2C emulator detail */
-	struct i2c_emul emul;
-	/** Smart battery device being emulated */
-	const struct device *i2c;
-	/** Configuration information */
-	const struct sbat_emul_cfg *cfg;
+	/** Common I2C data */
+	struct i2c_common_emul_data common;
+
 	/** Data required to simulate battery */
 	struct sbat_emul_bat_data bat;
 	/** Command that should be handled next */
 	int cur_cmd;
 	/** Message buffer which is used to handle smb transactions */
 	uint8_t msg_buf[MSG_BUF_LEN];
-	/** Position in msg_buf if there is on going smb write opperation */
-	int ong_write;
-	/** Position in msg_buf if there is on going smb read opperation */
-	int ong_read;
 	/** Total bytes that were generated in response to smb read operation */
 	int num_to_read;
-	/** Custom write function called on smb write opperation */
-	sbat_emul_custom_func write_custom_func;
-	/** Data passed to custom write function */
-	void *write_custom_func_data;
-	/** Custom read function called on smb read opperation */
-	sbat_emul_custom_func read_custom_func;
-	/** Data passed to custom read function */
-	void *read_custom_func_data;
-
-	/** Mutex used to control access to battery data */
-	struct k_mutex bat_mtx;
-};
-
-/** Static configuration for the emulator */
-struct sbat_emul_cfg {
-	/** Label of the I2C bus this emulator connects to */
-	const char *i2c_label;
-	/** Pointer to run-time data */
-	struct sbat_emul_data *data;
-	/** Address of smart battery on i2c bus */
-	uint16_t addr;
 };
 
 /** Check description in emul_smart_battery.h */
@@ -67,54 +44,12 @@ struct sbat_emul_bat_data *sbat_emul_get_bat_data(struct i2c_emul *emul)
 {
 	struct sbat_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct sbat_emul_data, emul);
+	data = SBAT_DATA_FROM_I2C_EMUL(emul);
 
 	return &data->bat;
 }
 
 /** Check description in emul_smart_battery.h */
-int sbat_emul_lock_bat_data(struct i2c_emul *emul, k_timeout_t timeout)
-{
-	struct sbat_emul_data *data;
-
-	data = CONTAINER_OF(emul, struct sbat_emul_data, emul);
-
-	return k_mutex_lock(&data->bat_mtx, timeout);
-}
-
-/** Check description in emul_smart_battery.h */
-int sbat_emul_unlock_bat_dat(struct i2c_emul *emul)
-{
-	struct sbat_emul_data *data;
-
-	data = CONTAINER_OF(emul, struct sbat_emul_data, emul);
-
-	return k_mutex_unlock(&data->bat_mtx);
-}
-
-/** Check description in emul_smart_battery.h */
-void sbat_emul_set_custom_write_func(struct i2c_emul *emul,
-				     sbat_emul_custom_func func, void *data)
-{
-	struct sbat_emul_data *emul_data;
-
-	emul_data = CONTAINER_OF(emul, struct sbat_emul_data, emul);
-	emul_data->write_custom_func = func;
-	emul_data->write_custom_func_data = data;
-}
-
-/** Check description in emul_smart_battery.h */
-void sbat_emul_set_custom_read_func(struct i2c_emul *emul,
-				    sbat_emul_custom_func func, void *data)
-{
-	struct sbat_emul_data *emul_data;
-
-	emul_data = CONTAINER_OF(emul, struct sbat_emul_data, emul);
-	emul_data->read_custom_func = func;
-	emul_data->read_custom_func_data = data;
-}
-
-/** Check description in emul_smart_battery.h */
 uint16_t sbat_emul_date_to_word(unsigned int day, unsigned int month,
 				unsigned int year)
 {
@@ -345,7 +280,7 @@ static uint16_t sbat_emul_read_status(struct i2c_emul *emul)
 	struct sbat_emul_bat_data *bat;
 	struct sbat_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct sbat_emul_data, emul);
+	data = SBAT_DATA_FROM_I2C_EMUL(emul);
 	bat = &data->bat;
 
 	status = bat->status;
@@ -401,7 +336,7 @@ int sbat_emul_get_word_val(struct i2c_emul *emul, int cmd, uint16_t *val)
 	int mode_mw;
 	int rate;
 
-	data = CONTAINER_OF(emul, struct sbat_emul_data, emul);
+	data = SBAT_DATA_FROM_I2C_EMUL(emul);
 	bat = &data->bat;
 	mode_mw = bat->mode & MODE_CAPACITY;
 
@@ -537,7 +472,7 @@ int sbat_emul_get_block_data(struct i2c_emul *emul, int cmd, uint8_t **blk,
 	struct sbat_emul_bat_data *bat;
 	struct sbat_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct sbat_emul_data, emul);
+	data = SBAT_DATA_FROM_I2C_EMUL(emul);
 	bat = &data->bat;
 
 	switch (cmd) {
@@ -567,20 +502,41 @@ int sbat_emul_get_block_data(struct i2c_emul *emul, int cmd, uint8_t **blk,
  * @brief Append PEC to read command response if battery support it
  *
  * @param data Pointer to smart battery emulator data
+ * @param cmd Command for which PEC is calculated
  */
-static void sbat_emul_append_pec(struct sbat_emul_data *data)
+static void sbat_emul_append_pec(struct sbat_emul_data *data, int cmd)
 {
 	uint8_t pec;
 
 	if (BATTERY_SPEC_VERSION(data->bat.spec_info) ==
 	    BATTERY_SPEC_VER_1_1_WITH_PEC) {
-		pec = sbat_emul_pec_head(data->cfg->addr, 1, data->cur_cmd);
+		pec = sbat_emul_pec_head(data->common.cfg->addr, 1, cmd);
 		pec = cros_crc8_arg(data->msg_buf, data->num_to_read, pec);
 		data->msg_buf[data->num_to_read] = pec;
 		data->num_to_read++;
 	}
 }
 
+/** Check description in emul_smart_battery.h */
+void sbat_emul_set_response(struct i2c_emul *emul, int cmd, uint8_t *buf,
+			    int len, bool fail)
+{
+	struct sbat_emul_data *data;
+
+	data = SBAT_DATA_FROM_I2C_EMUL(emul);
+
+	if (fail) {
+		data->bat.error_code = STATUS_CODE_UNKNOWN_ERROR;
+		data->num_to_read = 0;
+		return;
+	}
+
+	data->num_to_read = MIN(len, MSG_BUF_LEN - 1);
+	memcpy(data->msg_buf, buf, data->num_to_read);
+	data->bat.error_code = STATUS_CODE_OK;
+	sbat_emul_append_pec(data, cmd);
+}
+
 /**
  * @brief Function which handles read messages. It expects that data->cur_cmd
  *        is set to command number which should be handled. It guarantee that
@@ -589,45 +545,32 @@ static void sbat_emul_append_pec(struct sbat_emul_data *data)
  *        always set to 0.
  *
  * @param emul Pointer to smart battery emulator
+ * @param reg Command selected by last write message. If data->cur_cmd is
+ *            different than SBAT_EMUL_NO_CMD, then reg should equal to
+ *            data->cur_cmd
  *
  * @return 0 on success
  * @return -EIO on error
  */
-static int sbat_emul_handle_read_msg(struct i2c_emul *emul)
+static int sbat_emul_handle_read_msg(struct i2c_emul *emul, int reg)
 {
 	struct sbat_emul_data *data;
 	uint16_t word;
 	uint8_t *blk;
 	int ret, len;
 
-	data = CONTAINER_OF(emul, struct sbat_emul_data, emul);
-
-	data->num_to_read = 0;
-	if (data->read_custom_func != NULL) {
-		ret = data->read_custom_func(emul, data->msg_buf,
-					     &data->num_to_read, data->cur_cmd,
-					     data->read_custom_func_data);
-		if (ret < 0) {
-			data->bat.error_code = STATUS_CODE_UNKNOWN_ERROR;
-			data->num_to_read = 0;
-
-			return -EIO;
-		} else if (ret == 0) {
-			data->bat.error_code = STATUS_CODE_OK;
-			sbat_emul_append_pec(data);
-
-			return 0;
-		}
-	}
+	data = SBAT_DATA_FROM_I2C_EMUL(emul);
 
 	if (data->cur_cmd == SBAT_EMUL_NO_CMD) {
 		/* Unexpected read message without preceding command select */
 		data->bat.error_code = STATUS_CODE_UNKNOWN_ERROR;
 		return -EIO;
 	}
+	data->cur_cmd = SBAT_EMUL_NO_CMD;
+	data->num_to_read = 0;
 
 	/* Handle commands which return word */
-	ret = sbat_emul_get_word_val(emul, data->cur_cmd, &word);
+	ret = sbat_emul_get_word_val(emul, reg, &word);
 	if (ret < 0) {
 		return -EIO;
 	}
@@ -636,17 +579,17 @@ static int sbat_emul_handle_read_msg(struct i2c_emul *emul)
 		data->msg_buf[0] = word & 0xff;
 		data->msg_buf[1] = (word >> 8) & 0xff;
 		data->bat.error_code = STATUS_CODE_OK;
-		sbat_emul_append_pec(data);
+		sbat_emul_append_pec(data, reg);
 
 		return 0;
 	}
 
 	/* Handle commands which return block */
-	ret = sbat_emul_get_block_data(emul, data->cur_cmd, &blk, &len);
+	ret = sbat_emul_get_block_data(emul, reg, &blk, &len);
 	if (ret != 0) {
 		if (ret == 1) {
 			data->bat.error_code = STATUS_CODE_UNSUPPORTED;
-			LOG_ERR("Unknown read command (0x%x)", data->cur_cmd);
+			LOG_ERR("Unknown read command (0x%x)", reg);
 		}
 
 		return -EIO;
@@ -656,88 +599,65 @@ static int sbat_emul_handle_read_msg(struct i2c_emul *emul)
 	data->msg_buf[0] = len;
 	memcpy(&data->msg_buf[1], blk, len);
 	data->bat.error_code = STATUS_CODE_OK;
-	sbat_emul_append_pec(data);
+	sbat_emul_append_pec(data, reg);
 
 	return 0;
 }
 
 /**
- * @brief Function which finalize write messages. It expects that
- *        data->ong_write is set to number of bytes received in data->msg_buf.
- *        It guarantee that data->ong_write is set to 0.
+ * @brief Function which finalize write messages.
  *
  * @param emul Pointer to smart battery emulator
+ * @param reg First byte of write message, usually selected command
+ * @param bytes Number of bytes received in data->msg_buf
  *
  * @return 0 on success
  * @return -EIO on error
  */
-static int sbat_emul_finalize_write_msg(struct i2c_emul *emul)
+static int sbat_emul_finalize_write_msg(struct i2c_emul *emul, int reg,
+					int bytes)
 {
 	struct sbat_emul_bat_data *bat;
 	struct sbat_emul_data *data;
 	uint16_t word;
 	uint8_t pec;
-	int ret;
 
-	data = CONTAINER_OF(emul, struct sbat_emul_data, emul);
+	data = SBAT_DATA_FROM_I2C_EMUL(emul);
 	bat = &data->bat;
 
-	if (data->write_custom_func != NULL) {
-		ret = data->write_custom_func(emul, data->msg_buf,
-					      &data->ong_write,
-					      data->msg_buf[0],
-					      data->write_custom_func_data);
-		if (ret < 0) {
-			data->bat.error_code = STATUS_CODE_UNKNOWN_ERROR;
-			data->ong_write = 0;
-
-			return -EIO;
-		} else if (ret == 0) {
-			data->bat.error_code = STATUS_CODE_OK;
-			data->ong_write = 0;
-
-			return 0;
-		}
-	}
-
 	/*
 	 * Fail if:
 	 *  - there are no bytes to handle
 	 *  - there are too many bytes
 	 *  - there is command byte and only one data byte
 	 */
-	if (data->ong_write <= 0 ||
-	    data->ong_write > 4 ||
-	    data->ong_write == 2) {
+	if (bytes <= 0 || bytes > 4 || bytes == 2) {
 		data->bat.error_code = STATUS_CODE_BADSIZE;
-		data->ong_write = 0;
-		LOG_ERR("wrong write message size (%d)", data->ong_write);
+		LOG_ERR("wrong write message size (%d)", bytes);
 
 		return -EIO;
 	}
 
 	/* There is only command for read */
-	if (data->ong_write == 1) {
-		data->cur_cmd = data->msg_buf[0];
-		data->ong_write = 0;
+	if (bytes == 1) {
+		data->cur_cmd = reg;
 		return 0;
 	}
 
 	/* Handle PEC */
-	if (data->ong_write == 4) {
+	data->msg_buf[0] = reg;
+	if (bytes == 4) {
 		if (BATTERY_SPEC_VERSION(data->bat.spec_info) !=
 		    BATTERY_SPEC_VER_1_1_WITH_PEC) {
 			data->bat.error_code = STATUS_CODE_BADSIZE;
-			data->ong_write = 0;
 			LOG_ERR("Unexpected PEC; No support in this version");
 
 			return -EIO;
 		}
-		pec = sbat_emul_pec_head(data->cfg->addr, 0, 0);
+		pec = sbat_emul_pec_head(data->common.cfg->addr, 0, 0);
 		pec = cros_crc8_arg(data->msg_buf, 3, pec);
 		if (pec != data->msg_buf[3]) {
 			data->bat.error_code = STATUS_CODE_UNKNOWN_ERROR;
-			data->ong_write = 0;
 			LOG_ERR("Wrong PEC 0x%x != 0x%x",
 				pec, data->msg_buf[3]);
 
@@ -766,117 +686,88 @@ static int sbat_emul_finalize_write_msg(struct i2c_emul *emul)
 		bat->at_rate = word;
 		break;
 	default:
-		data->ong_write = 0;
 		data->bat.error_code = STATUS_CODE_ACCESS_DENIED;
 		LOG_ERR("Unknown write command (0x%x)", data->msg_buf[0]);
 
 		return -EIO;
 	}
 
-	data->ong_write = 0;
 	data->bat.error_code = STATUS_CODE_OK;
 
 	return 0;
 }
 
 /**
- * Emulator an I2C transfer to an smart battery
+ * @brief Function called for each byte of write message which is saved in
+ *        data->msg_buf
  *
- * This handles simple reads and writes
- *
- * @param emul I2C emulation information
- * @param msgs List of messages to process. For 'read' messages, this function
- *             updates the 'buf' member with the data that was read
- * @param num_msgs Number of messages to process
- * @param addr Address of the I2C target device.
+ * @param emul Pointer to smart battery emulator
+ * @param reg First byte of write message, usually selected command
+ * @param val Received byte of write message
+ * @param bytes Number of bytes already received
  *
- * @retval 0 If successful
- * @retval -EIO General input / output error
+ * @return 0 on success
  */
-static int sbat_emul_transfer(struct i2c_emul *emul, struct i2c_msg *msgs,
-			      int num_msgs, int addr)
+static int sbat_emul_write_byte(struct i2c_emul *emul, int reg, uint8_t val,
+				int bytes)
 {
-	const struct sbat_emul_cfg *cfg;
 	struct sbat_emul_data *data;
-	int ret = 0, i;
 
-	data = CONTAINER_OF(emul, struct sbat_emul_data, emul);
-	cfg = data->cfg;
+	data = SBAT_DATA_FROM_I2C_EMUL(emul);
 
-	if (cfg->addr != addr) {
-		LOG_ERR("Address mismatch, expected %02x, got %02x", cfg->addr,
-			addr);
-		return -EIO;
+	if (bytes < MSG_BUF_LEN) {
+		data->msg_buf[bytes] = val;
 	}
 
-	i2c_dump_msgs("emul", msgs, num_msgs, addr);
-
-	for (; num_msgs > 0; num_msgs--, msgs++) {
-		if (!(msgs->flags & I2C_MSG_READ)) {
-			/* Disscard any ongoing read */
-			data->num_to_read = 0;
-
-			/* Save incoming data to buffer */
-			for (i = 0; i < msgs->len; i++, data->ong_write++) {
-				if (data->ong_write < MSG_BUF_LEN) {
-					data->msg_buf[data->ong_write] =
-								msgs->buf[i];
-				}
-			}
-
-			/* Handle write message when we receive stop signal */
-			if (msgs->flags & I2C_MSG_STOP) {
-				k_mutex_lock(&data->bat_mtx, K_FOREVER);
-				ret = sbat_emul_finalize_write_msg(emul);
-				k_mutex_unlock(&data->bat_mtx);
-			}
-		} else {
-			/* Finalize any ongoing write message before read */
-			if (data->ong_write) {
-				k_mutex_lock(&data->bat_mtx, K_FOREVER);
-				ret = sbat_emul_finalize_write_msg(emul);
-				k_mutex_unlock(&data->bat_mtx);
-				if (ret) {
-					return -EIO;
-				}
-			}
-
-			/* Prepare read message */
-			if (!data->num_to_read) {
-				k_mutex_lock(&data->bat_mtx, K_FOREVER);
-				ret = sbat_emul_handle_read_msg(emul);
-				k_mutex_unlock(&data->bat_mtx);
-				data->cur_cmd = SBAT_EMUL_NO_CMD;
-				data->ong_read = 0;
-			}
-
-			for (i = 0; i < msgs->len; i++, data->ong_read++) {
-				if (data->ong_read >= data->num_to_read) {
-					/* We wrote everything */
-					data->num_to_read = 0;
-					break;
-				}
-				msgs->buf[i] = data->msg_buf[data->ong_read];
-			}
-
-			if (msgs->flags & I2C_MSG_STOP) {
-				/* Disscard any data that wern't read */
-				data->num_to_read = 0;
-			}
-		}
+	return 0;
+}
 
-		if (ret) {
-			return -EIO;
-		}
+/**
+ * @brief Function called for each byte of read message. Byte from data->msg_buf
+ *        is copied to read message response.
+ *
+ * @param emul Pointer to smart battery emulator
+ * @param reg First byte of last write message, usually selected command
+ * @param val Pointer where byte to read should be stored
+ * @param bytes Number of bytes already readed
+ *
+ * @return 0 on success
+ */
+static int sbat_emul_read_byte(struct i2c_emul *emul, int reg, uint8_t *val,
+			       int bytes)
+{
+	struct sbat_emul_data *data;
+
+	data = SBAT_DATA_FROM_I2C_EMUL(emul);
+
+	if (bytes < data->num_to_read) {
+		*val = data->msg_buf[bytes];
 	}
 
 	return 0;
 }
 
+/**
+ * @brief Get currently accessed register, which always equals to selected
+ *        command.
+ *
+ * @param emul Pointer to smart battery emulator
+ * @param reg First byte of last write message, usually selected command
+ * @param bytes Number of bytes already handled from current message
+ * @param read If currently handled is read message
+ *
+ * @return Currently accessed register
+ */
+static int sbat_emul_access_reg(struct i2c_emul *emul, int reg, int bytes,
+				bool read)
+{
+	return reg;
+}
+
 /* Device instantiation */
 
 static struct i2c_emul_api sbat_emul_api = {
-	.transfer = sbat_emul_transfer,
+	.transfer = i2c_common_emul_transfer,
 };
 
 /**
@@ -893,15 +784,15 @@ static struct i2c_emul_api sbat_emul_api = {
 static int sbat_emul_init(const struct emul *emul,
 			  const struct device *parent)
 {
-	const struct sbat_emul_cfg *cfg = emul->cfg;
-	struct sbat_emul_data *data = cfg->data;
+	const struct i2c_common_emul_cfg *cfg = emul->cfg;
+	struct i2c_common_emul_data *data = cfg->data;
 	int ret;
 
 	data->emul.api = &sbat_emul_api;
 	data->emul.addr = cfg->addr;
 	data->i2c = parent;
 	data->cfg = cfg;
-	k_mutex_init(&data->bat_mtx);
+	i2c_common_emul_init(data);
 
 	ret = i2c_emul_register(parent, emul->dev_label, &data->emul);
 
@@ -965,13 +856,20 @@ static int sbat_emul_init(const struct emul *emul,
 			.error_code = STATUS_CODE_OK,			\
 		},							\
 		.cur_cmd = SBAT_EMUL_NO_CMD,				\
-		.write_custom_func = NULL,				\
-		.read_custom_func = NULL,				\
+		.common = {						\
+			.start_write = NULL,				\
+			.write_byte = sbat_emul_write_byte,		\
+			.finish_write = sbat_emul_finalize_write_msg,	\
+			.start_read = sbat_emul_handle_read_msg,	\
+			.read_byte = sbat_emul_read_byte,		\
+			.finish_read = NULL,				\
+			.access_reg = sbat_emul_access_reg,		\
+		},							\
 	};								\
 									\
-	static const struct sbat_emul_cfg sbat_emul_cfg_##n = {		\
+	static const struct i2c_common_emul_cfg sbat_emul_cfg_##n = {	\
 		.i2c_label = DT_INST_BUS_LABEL(n),			\
-		.data = &sbat_emul_data_##n,				\
+		.data = &sbat_emul_data_##n.common,			\
 		.addr = DT_INST_REG_ADDR(n),				\
 	};								\
 	EMUL_DEFINE(sbat_emul_init, DT_DRV_INST(n), &sbat_emul_cfg_##n)
@@ -979,7 +877,7 @@ static int sbat_emul_init(const struct emul *emul,
 DT_INST_FOREACH_STATUS_OKAY(SMART_BATTERY_EMUL)
 
 #define SMART_BATTERY_EMUL_CASE(n)					\
-	case DT_INST_DEP_ORD(n): return &sbat_emul_data_##n.emul;
+	case DT_INST_DEP_ORD(n): return &sbat_emul_data_##n.common.emul;
 
 /** Check description in emul_smart_battery.h */
 struct i2c_emul *sbat_emul_get_ptr(int ord)
diff --git a/zephyr/emul/emul_tcs3400.c b/zephyr/emul/emul_tcs3400.c
index 874b343a6b..e1df26c990 100644
--- a/zephyr/emul/emul_tcs3400.c
+++ b/zephyr/emul/emul_tcs3400.c
@@ -14,29 +14,22 @@ LOG_MODULE_REGISTER(emul_tcs);
 #include <drivers/i2c.h>
 #include <drivers/i2c_emul.h>
 
+#include "emul/emul_common_i2c.h"
 #include "emul/emul_tcs3400.h"
 
 #include "driver/als_tcs3400.h"
 
-/**
- * Describe if there is no ongoing I2C message or if there is message handled
- * at the moment (last message doesn't ended with stop or write is not followed
- * by read).
- */
-enum tcs_emul_msg_state {
-	TCS_EMUL_NONE_MSG,
-	TCS_EMUL_IN_WRITE,
-	TCS_EMUL_IN_READ
-};
+#define TCS_DATA_FROM_I2C_EMUL(_emul)					     \
+	CONTAINER_OF(CONTAINER_OF(_emul, struct i2c_common_emul_data, emul), \
+		     struct tcs_emul_data, common)
 
 /** Run-time data used by the emulator */
 struct tcs_emul_data {
-	/** I2C emulator detail */
-	struct i2c_emul emul;
-	/** TCS3400 device being emulated */
-	const struct device *i2c;
-	/** Configuration information */
-	const struct tcs_emul_cfg *cfg;
+	/** Common I2C data */
+	struct i2c_common_emul_data common;
+
+	/** Value of data byte in ongoing write message */
+	uint8_t write_byte;
 
 	/** Current state of emulated TCS3400 registers */
 	uint8_t reg[TCS_EMUL_REG_COUNT];
@@ -67,86 +60,8 @@ struct tcs_emul_data {
 	bool lsb_g_read;
 	bool lsb_b_read;
 	bool lsb_c_ir_read;
-
-	/** Current state of I2C bus (if emulator is handling message) */
-	enum tcs_emul_msg_state msg_state;
-	/** Number of already handled bytes in ongoing message */
-	int msg_byte;
-	/** Register selected in last write command */
-	uint8_t cur_reg;
-	/** Value of data byte in ongoing write message */
-	uint8_t write_byte;
-
-	/** Custom write function called on I2C write opperation */
-	tcs_emul_write_func write_func;
-	/** Data passed to custom write function */
-	void *write_func_data;
-	/** Custom read function called on I2C read opperation */
-	tcs_emul_read_func read_func;
-	/** Data passed to custom read function */
-	void *read_func_data;
-
-	/** Control if read should fail on given register */
-	int read_fail_reg;
-	/** Control if write should fail on given register */
-	int write_fail_reg;
-
-	/** Mutex used to control access to emulator data */
-	struct k_mutex data_mtx;
 };
 
-/** Static configuration for the emulator */
-struct tcs_emul_cfg {
-	/** Label of the I2C bus this emulator connects to */
-	const char *i2c_label;
-	/** Pointer to run-time data */
-	struct tcs_emul_data *data;
-	/** Address of TCS3400 on i2c bus */
-	uint16_t addr;
-};
-
-/** Check description in emul_tcs3400.h */
-int tcs_emul_lock_data(struct i2c_emul *emul, k_timeout_t timeout)
-{
-	struct tcs_emul_data *data;
-
-	data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
-
-	return k_mutex_lock(&data->data_mtx, timeout);
-}
-
-/** Check description in emul_tcs3400.h */
-int tcs_emul_unlock_data(struct i2c_emul *emul)
-{
-	struct tcs_emul_data *data;
-
-	data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
-
-	return k_mutex_unlock(&data->data_mtx);
-}
-
-/** Check description in emul_tcs3400.h */
-void tcs_emul_set_write_func(struct i2c_emul *emul,
-			     tcs_emul_write_func func, void *data)
-{
-	struct tcs_emul_data *emul_data;
-
-	emul_data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
-	emul_data->write_func = func;
-	emul_data->write_func_data = data;
-}
-
-/** Check description in emul_tcs3400.h */
-void tcs_emul_set_read_func(struct i2c_emul *emul,
-			    tcs_emul_read_func func, void *data)
-{
-	struct tcs_emul_data *emul_data;
-
-	emul_data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
-	emul_data->read_func = func;
-	emul_data->read_func_data = data;
-}
-
 /** Check description in emul_tcs3400.h */
 void tcs_emul_set_reg(struct i2c_emul *emul, int reg, uint8_t val)
 {
@@ -157,7 +72,7 @@ void tcs_emul_set_reg(struct i2c_emul *emul, int reg, uint8_t val)
 	}
 
 	reg -= TCS_EMUL_FIRST_REG;
-	data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
+	data = TCS_DATA_FROM_I2C_EMUL(emul);
 	data->reg[reg] = val;
 }
 
@@ -170,36 +85,18 @@ uint8_t tcs_emul_get_reg(struct i2c_emul *emul, int reg)
 		return 0;
 	}
 
-	data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
+	data = TCS_DATA_FROM_I2C_EMUL(emul);
 	reg -= TCS_EMUL_FIRST_REG;
 
 	return data->reg[reg];
 }
 
 /** Check description in emul_tcs3400.h */
-void tcs_emul_set_read_fail_reg(struct i2c_emul *emul, int reg)
-{
-	struct tcs_emul_data *data;
-
-	data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
-	data->read_fail_reg = reg;
-}
-
-/** Check description in emul_tcs3400.h */
-void tcs_emul_set_write_fail_reg(struct i2c_emul *emul, int reg)
-{
-	struct tcs_emul_data *data;
-
-	data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
-	data->write_fail_reg = reg;
-}
-
-/** Check description in emul_tcs3400.h */
 int tcs_emul_get_val(struct i2c_emul *emul, enum tcs_emul_axis axis)
 {
 	struct tcs_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
+	data = TCS_DATA_FROM_I2C_EMUL(emul);
 
 	switch (axis) {
 	case TCS_EMUL_R:
@@ -222,7 +119,7 @@ void tcs_emul_set_val(struct i2c_emul *emul, enum tcs_emul_axis axis, int val)
 {
 	struct tcs_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
+	data = TCS_DATA_FROM_I2C_EMUL(emul);
 
 	switch (axis) {
 	case TCS_EMUL_R:
@@ -248,7 +145,7 @@ void tcs_emul_set_err_on_ro_write(struct i2c_emul *emul, bool set)
 {
 	struct tcs_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
+	data = TCS_DATA_FROM_I2C_EMUL(emul);
 	data->error_on_ro_write = set;
 }
 
@@ -257,7 +154,7 @@ void tcs_emul_set_err_on_rsvd_write(struct i2c_emul *emul, bool set)
 {
 	struct tcs_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
+	data = TCS_DATA_FROM_I2C_EMUL(emul);
 	data->error_on_rsvd_write = set;
 }
 
@@ -266,7 +163,7 @@ void tcs_emul_set_err_on_msb_first(struct i2c_emul *emul, bool set)
 {
 	struct tcs_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
+	data = TCS_DATA_FROM_I2C_EMUL(emul);
 	data->error_on_msb_first = set;
 }
 
@@ -308,7 +205,7 @@ static void tcs_emul_reset(struct i2c_emul *emul)
 {
 	struct tcs_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
+	data = TCS_DATA_FROM_I2C_EMUL(emul);
 
 	data->reg[TCS_I2C_ENABLE  - TCS_EMUL_FIRST_REG] = 0x00;
 	data->reg[TCS_I2C_ATIME   - TCS_EMUL_FIRST_REG] = 0xff;
@@ -380,7 +277,7 @@ static void tcs_emul_clear_int(struct i2c_emul *emul)
 {
 	struct tcs_emul_data *data;
 
-	data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
+	data = TCS_DATA_FROM_I2C_EMUL(emul);
 
 	data->reg[TCS_I2C_STATUS  - TCS_EMUL_FIRST_REG] = 0x00;
 }
@@ -389,36 +286,28 @@ static void tcs_emul_clear_int(struct i2c_emul *emul)
  * @brief Handle I2C write message. It is checked if accessed register isn't RO
  *        and reserved bits are set to 0. Write set value of reg field of TCS
  *        emulator data ignoring reserved bits and write only bits. Some
- *        commands are handled specialy. Before any handling, custom function
- *        is called if provided.
+ *        commands are handled specialy.
  *
  * @param emul Pointer to TCS3400 emulator
  * @param reg Register which is written
- * @param val Value being written to @p reg
+ * @param bytes Number of bytes received in this write message
  *
  * @return 0 on success
  * @return -EIO on error
  */
-static int tcs_emul_handle_write(struct i2c_emul *emul, int reg, uint8_t val)
+static int tcs_emul_handle_write(struct i2c_emul *emul, int reg, int bytes)
 {
 	struct tcs_emul_data *data;
-	int ret;
+	uint8_t val;
 
-	data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
+	data = TCS_DATA_FROM_I2C_EMUL(emul);
 
-	if (data->write_func) {
-		ret = data->write_func(emul, reg, val, data->write_func_data);
-		if (ret < 0) {
-			return -EIO;
-		} else if (ret == 0) {
-			return 0;
-		}
+	/* This write only selected register for I2C read message */
+	if (bytes < 2) {
+		return 0;
 	}
 
-	if (data->write_fail_reg == reg ||
-	    data->write_fail_reg == TCS_EMUL_FAIL_ALL_REG) {
-		return -EIO;
-	}
+	val = data->write_byte;
 
 	/* Register is in data->reg */
 	if (reg >= TCS_EMUL_FIRST_REG && reg <= TCS_EMUL_LAST_REG) {
@@ -508,7 +397,7 @@ static int tcs_emul_get_reg_val(struct i2c_emul *emul, int reg,
 	int cycles;
 	int gain;
 
-	data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
+	data = TCS_DATA_FROM_I2C_EMUL(emul);
 
 	if (lsb) {
 		*lsb_read = 1;
@@ -552,40 +441,26 @@ static int tcs_emul_get_reg_val(struct i2c_emul *emul, int reg,
 /**
  * @brief Handle I2C read message. Response is obtained from reg field of TCS
  *        emul data. When accessing light sensor value, register data is first
- *        computed using internal emulator state. Before default handler, custom
- *        user read function is called if provided.
+ *        computed using internal emulator state.
  *
  * @param emul Pointer to TCS3400 emulator
- * @param reg Register address to read
- * @param buf Pointer where resultat should be stored
+ * @param reg First register address that is accessed in this read message
+ * @param buf Pointer where result should be stored
+ * @param bytes Number of bytes already handled in this read message
  *
  * @return 0 on success
  * @return -EIO on error
  */
-static int tcs_emul_handle_read(struct i2c_emul *emul, int reg, char *buf)
+static int tcs_emul_handle_read(struct i2c_emul *emul, int reg, uint8_t *buf,
+				int bytes)
 {
 	struct tcs_emul_data *data;
 	unsigned int c_ir;
 	int ret;
 
-	data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
-
-	if (data->read_func) {
-		ret = data->read_func(emul, reg, data->read_func_data);
-		if (ret < 0) {
-			return -EIO;
-		} else if (ret == 0) {
-			/* Immediately return value set by custom function */
-			*buf = data->reg[reg - TCS_EMUL_FIRST_REG];
-
-			return 0;
-		}
-	}
+	data = TCS_DATA_FROM_I2C_EMUL(emul);
 
-	if (data->read_fail_reg == reg ||
-	    data->read_fail_reg == TCS_EMUL_FAIL_ALL_REG) {
-		return -EIO;
-	}
+	reg += bytes;
 
 	if ((reg < TCS_EMUL_FIRST_REG || reg > TCS_EMUL_LAST_REG) &&
 	    reg != TCS_I2C_IR) {
@@ -660,117 +535,30 @@ static int tcs_emul_handle_read(struct i2c_emul *emul, int reg, char *buf)
 }
 
 /**
- * @biref Emulate an I2C transfer to a TCS3400 light sensor
- *
- * This handles simple reads and writes
+ * @brief Handle I2C write message. Check if message is not too long and saves
+ *        data that will be stored in register
  *
- * @param emul I2C emulation information
- * @param msgs List of messages to process
- * @param num_msgs Number of messages to process
- * @param addr Address of the I2C target device
+ * @param emul Pointer to TCS3400 emulator
+ * @param reg Register address that is accessed
+ * @param val Data to write to the register
+ * @param bytes Number of bytes already handled in this read message
  *
- * @retval 0 If successful
- * @retval -EIO General input / output error
+ * @return 0 on success
+ * @return -EIO on error
  */
-static int tcs_emul_transfer(struct i2c_emul *emul, struct i2c_msg *msgs,
-			     int num_msgs, int addr)
+static int tcs_emul_write_byte(struct i2c_emul *emul, int reg, uint8_t val,
+			       int bytes)
 {
-	const struct tcs_emul_cfg *cfg;
 	struct tcs_emul_data *data;
-	int ret, i, reg;
-	bool read;
 
-	data = CONTAINER_OF(emul, struct tcs_emul_data, emul);
-	cfg = data->cfg;
+	data = TCS_DATA_FROM_I2C_EMUL(emul);
 
-	if (cfg->addr != addr) {
-		LOG_ERR("Address mismatch, expected %02x, got %02x", cfg->addr,
-			addr);
+	if (bytes > 1) {
+		LOG_ERR("Too long write command");
 		return -EIO;
 	}
 
-	i2c_dump_msgs("emul", msgs, num_msgs, addr);
-
-	for (; num_msgs > 0; num_msgs--, msgs++) {
-		read = msgs->flags & I2C_MSG_READ;
-
-		switch (data->msg_state) {
-		case TCS_EMUL_NONE_MSG:
-			data->msg_byte = 0;
-			break;
-		case TCS_EMUL_IN_WRITE:
-			if (read) {
-				/* Finish write command */
-				if (data->msg_byte == 2) {
-					k_mutex_lock(&data->data_mtx,
-						     K_FOREVER);
-					ret = tcs_emul_handle_write(emul,
-							data->cur_reg,
-							data->write_byte);
-					k_mutex_unlock(&data->data_mtx);
-					if (ret) {
-						return -EIO;
-					}
-				}
-				data->msg_byte = 0;
-			}
-			break;
-		case TCS_EMUL_IN_READ:
-			if (!read) {
-				data->msg_byte = 0;
-			}
-			break;
-		}
-		data->msg_state = read ? TCS_EMUL_IN_READ : TCS_EMUL_IN_WRITE;
-
-		if (msgs->flags & I2C_MSG_STOP) {
-			data->msg_state = TCS_EMUL_NONE_MSG;
-		}
-
-		if (!read) {
-			/* Dispatch write command */
-			for (i = 0; i < msgs->len; i++) {
-				switch (data->msg_byte) {
-				case 0:
-					data->cur_reg = msgs->buf[i];
-					break;
-				case 1:
-					data->write_byte = msgs->buf[i];
-					break;
-				default:
-					data->msg_state = TCS_EMUL_NONE_MSG;
-					LOG_ERR("Too long write command");
-					return -EIO;
-				}
-				data->msg_byte++;
-			}
-
-			/* Execute write command */
-			if (msgs->flags & I2C_MSG_STOP && data->msg_byte == 2) {
-				k_mutex_lock(&data->data_mtx, K_FOREVER);
-				ret = tcs_emul_handle_write(emul, data->cur_reg,
-							    data->write_byte);
-				k_mutex_unlock(&data->data_mtx);
-				if (ret) {
-					return -EIO;
-				}
-			}
-		} else {
-			/* Dispatch read command */
-			for (i = 0; i < msgs->len; i++) {
-				reg = data->cur_reg + data->msg_byte;
-				data->msg_byte++;
-
-				k_mutex_lock(&data->data_mtx, K_FOREVER);
-				ret = tcs_emul_handle_read(emul, reg,
-							   &(msgs->buf[i]));
-				k_mutex_unlock(&data->data_mtx);
-				if (ret) {
-					return -EIO;
-				}
-			}
-		}
-	}
+	data->write_byte = val;
 
 	return 0;
 }
@@ -778,7 +566,7 @@ static int tcs_emul_transfer(struct i2c_emul *emul, struct i2c_msg *msgs,
 /* Device instantiation */
 
 static struct i2c_emul_api tcs_emul_api = {
-	.transfer = tcs_emul_transfer,
+	.transfer = i2c_common_emul_transfer,
 };
 
 /**
@@ -795,15 +583,15 @@ static struct i2c_emul_api tcs_emul_api = {
 static int tcs_emul_init(const struct emul *emul,
 			 const struct device *parent)
 {
-	const struct tcs_emul_cfg *cfg = emul->cfg;
-	struct tcs_emul_data *data = cfg->data;
+	const struct i2c_common_emul_cfg *cfg = emul->cfg;
+	struct i2c_common_emul_data *data = cfg->data;
 	int ret;
 
 	data->emul.api = &tcs_emul_api;
 	data->emul.addr = cfg->addr;
 	data->i2c = parent;
 	data->cfg = cfg;
-	k_mutex_init(&data->data_mtx);
+	i2c_common_emul_init(data);
 
 	ret = i2c_emul_register(parent, emul->dev_label, &data->emul);
 
@@ -825,17 +613,20 @@ static int tcs_emul_init(const struct emul *emul,
 		.lsb_r_read = 0,					\
 		.lsb_g_read = 0,					\
 		.lsb_b_read = 0,					\
-		.msg_state = TCS_EMUL_NONE_MSG,				\
-		.cur_reg = 0,						\
-		.write_func = NULL,					\
-		.read_func = NULL,					\
-		.write_fail_reg = TCS_EMUL_NO_FAIL_REG,			\
-		.read_fail_reg = TCS_EMUL_NO_FAIL_REG,			\
+		.common = {						\
+			.start_write = NULL,				\
+			.write_byte = tcs_emul_write_byte,		\
+			.finish_write = tcs_emul_handle_write,		\
+			.start_read = NULL,				\
+			.read_byte = tcs_emul_handle_read,		\
+			.finish_read = NULL,				\
+			.access_reg = NULL,				\
+		},							\
 	};								\
 									\
-	static const struct tcs_emul_cfg tcs_emul_cfg_##n = {		\
+	static const struct i2c_common_emul_cfg tcs_emul_cfg_##n = {	\
 		.i2c_label = DT_INST_BUS_LABEL(n),			\
-		.data = &tcs_emul_data_##n,				\
+		.data = &tcs_emul_data_##n.common,			\
 		.addr = DT_INST_REG_ADDR(n),				\
 	};								\
 	EMUL_DEFINE(tcs_emul_init, DT_DRV_INST(n), &tcs_emul_cfg_##n)
@@ -843,7 +634,7 @@ static int tcs_emul_init(const struct emul *emul,
 DT_INST_FOREACH_STATUS_OKAY(TCS3400_EMUL)
 
 #define TCS3400_EMUL_CASE(n)					\
-	case DT_INST_DEP_ORD(n): return &tcs_emul_data_##n.emul;
+	case DT_INST_DEP_ORD(n): return &tcs_emul_data_##n.common.emul;
 
 /** Check description in emul_tcs3400.h */
 struct i2c_emul *tcs_emul_get(int ord)