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>

19 files changed, 866 insertions, 2185 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)
diff --git a/zephyr/include/emul/emul_bb_retimer.h b/zephyr/include/emul/emul_bb_retimer.h
index 2820c59934..c63cc651c0 100644
--- a/zephyr/include/emul/emul_bb_retimer.h
+++ b/zephyr/include/emul/emul_bb_retimer.h
@@ -28,24 +28,15 @@
  *
  * - define a Device Tree overlay file to set default vendor ID and which
  *   inadvisable driver behaviour should be treated as errors
- * - call @ref bb_emul_set_read_func and @ref bb_emul_set_write_func to setup
- *   custom handlers for I2C messages
  * - call @ref bb_emul_set_reg and @ref bb_emul_get_reg to set and get value
  *   of BB retimers registers
  * - call bb_emul_set_err_* to change emulator behaviour on inadvisable driver
  *   behaviour
- * - call @ref bb_emul_set_read_fail_reg and @ref bb_emul_set_write_fail_reg
- *   to configure emulator to fail on given register read or write
+ * - call functions from emul_common_i2c.h to setup custom handlers for I2C
+ *   messages
  */
 
 /**
- * Special register values used in @ref bb_emul_set_read_fail_reg and
- * @ref bb_emul_set_write_fail_reg
- */
-#define BB_EMUL_FAIL_ALL_REG	(-1)
-#define BB_EMUL_NO_FAIL_REG	(-2)
-
-/**
  * @brief Get pointer to BB retimer emulator using device tree order number.
  *
  * @param ord Device tree order number obtained from DT_DEP_ORD macro
@@ -55,78 +46,6 @@
 struct i2c_emul *bb_emul_get(int ord);
 
 /**
- * @brief Custom function type that is used as user-defined callback in read
- *        I2C messages handling.
- *
- * @param emul Pointer to BB retimer emulator
- * @param reg Address which is now accessed by read command
- * @param data Pointer to custom user data
- *
- * @return 0 on success. Value of @p reg should be set by @ref bb_emul_set_reg
- * @return 1 continue with normal BB retimer emulator handler
- * @return negative on error
- */
-typedef int (*bb_emul_read_func)(struct i2c_emul *emul, int reg, void *data);
-
-/**
- * @brief Custom function type that is used as user-defined callback in write
- *        I2C messages handling.
- *
- * @param emul Pointer to BB retimer emulator
- * @param reg Address which is now accessed by write command
- * @param val Value which is being written to @p reg
- * @param data Pointer to custom user data
- *
- * @return 0 on success
- * @return 1 continue with normal BB retimer emulator handler
- * @return negative on error
- */
-typedef int (*bb_emul_write_func)(struct i2c_emul *emul, int reg, uint32_t val,
-				  void *data);
-
-/**
- * @brief Lock access to BB retimer properties. After acquiring lock, user
- *        may change emulator behaviour in multi-thread setup.
- *
- * @param emul Pointer to BB retimer emulator
- * @param timeout Timeout in getting lock
- *
- * @return k_mutex_lock return code
- */
-int bb_emul_lock_data(struct i2c_emul *emul, k_timeout_t timeout);
-
-/**
- * @brief Unlock access to BB retimer properties.
- *
- * @param emul Pointer to BB retimer emulator
- *
- * @return k_mutex_unlock return code
- */
-int bb_emul_unlock_data(struct i2c_emul *emul);
-
-/**
- * @brief Set write handler for I2C messages. This function is called before
- *        generic handler.
- *
- * @param emul Pointer to BB retimer emulator
- * @param func Pointer to custom function
- * @param data User data passed on call of custom function
- */
-void bb_emul_set_write_func(struct i2c_emul *emul, bb_emul_write_func func,
-			    void *data);
-
-/**
- * @brief Set read handler for I2C messages. This function is called before
- *        generic handler.
- *
- * @param emul Pointer to BB retimer emulator
- * @param func Pointer to custom function
- * @param data User data passed on call of custom function
- */
-void bb_emul_set_read_func(struct i2c_emul *emul, bb_emul_read_func func,
-			   void *data);
-
-/**
  * @brief Set value of given register of BB retimer
  *
  * @param emul Pointer to BB retimer emulator
@@ -146,24 +65,6 @@ void bb_emul_set_reg(struct i2c_emul *emul, int reg, uint32_t val);
 uint32_t bb_emul_get_reg(struct i2c_emul *emul, int reg);
 
 /**
- * @brief Setup fail on read of given register of BB retimer
- *
- * @param emul Pointer to BB retimer emulator
- * @param reg Register address or one of special values (BB_EMUL_FAIL_ALL_REG,
- *            BB_EMUL_NO_FAIL_REG)
- */
-void bb_emul_set_read_fail_reg(struct i2c_emul *emul, int reg);
-
-/**
- * @brief Setup fail on write of given register of BB retimer
- *
- * @param emul Pointer to BB retimer emulator
- * @param reg Register address or one of special values (BB_EMUL_FAIL_ALL_REG,
- *            BB_EMUL_NO_FAIL_REG)
- */
-void bb_emul_set_write_fail_reg(struct i2c_emul *emul, int reg);
-
-/**
  * @brief Set if error should be generated when read only register is being
  *        written
  *
diff --git a/zephyr/include/emul/emul_bma255.h b/zephyr/include/emul/emul_bma255.h
index 1b40e06559..b2f71df88b 100644
--- a/zephyr/include/emul/emul_bma255.h
+++ b/zephyr/include/emul/emul_bma255.h
@@ -31,8 +31,6 @@
  * - define a Device Tree overlay file to set default NVM content, default
  *   static accelerometer value and which inadvisable driver behaviour should
  *   be treated as errors
- * - call @ref bma_emul_set_read_func and @ref bma_emul_set_write_func to setup
- *   custom handlers for I2C messages
  * - call @ref bma_emul_set_reg and @ref bma_emul_get_reg to set and get value
  *   of BMA255 registers
  * - call @ref bma_emul_set_off and @ref bma_emul_set_off to set and get
@@ -41,8 +39,8 @@
  *   accelerometer value
  * - call bma_emul_set_err_* to change emulator behaviour on inadvisable driver
  *   behaviour
- * - call @ref bma_emul_set_read_fail_reg and @ref bma_emul_set_write_fail_reg
- *   to configure emulator to fail on given register read or write
+ * - call functions from emul_common_i2c.h to setup custom handlers for I2C
+ *   messages
  */
 
 /**
@@ -61,13 +59,6 @@
 #define BMA_EMUL_1G		BIT(10)
 
 /**
- * Special register values used in @ref bma_emul_set_read_fail_reg and
- * @ref bma_emul_set_write_fail_reg
- */
-#define BMA_EMUL_FAIL_ALL_REG	(-1)
-#define BMA_EMUL_NO_FAIL_REG	(-2)
-
-/**
  * @brief Get pointer to BMA255 emulator using device tree order number.
  *
  * @param ord Device tree order number obtained from DT_DEP_ORD macro
@@ -77,78 +68,6 @@
 struct i2c_emul *bma_emul_get(int ord);
 
 /**
- * @brief Custom function type that is used as user-defined callback in read
- *        I2C messages handling.
- *
- * @param emul Pointer to BMA255 emulator
- * @param reg Address which is now accessed by read command
- * @param data Pointer to custom user data
- *
- * @return 0 on success. Value of @p reg should be set by @ref bma_emul_set_reg
- * @return 1 continue with normal BMA255 emulator handler
- * @return negative on error
- */
-typedef int (*bma_emul_read_func)(struct i2c_emul *emul, int reg, void *data);
-
-/**
- * @brief Custom function type that is used as user-defined callback in write
- *        I2C messages handling.
- *
- * @param emul Pointer to BMA255 emulator
- * @param reg Address which is now accessed by write command
- * @param val Value which is being written to @p reg
- * @param data Pointer to custom user data
- *
- * @return 0 on success
- * @return 1 continue with normal BMA255 emulator handler
- * @return negative on error
- */
-typedef int (*bma_emul_write_func)(struct i2c_emul *emul, int reg, uint8_t val,
-				   void *data);
-
-/**
- * @brief Lock access to BMA255 properties. After acquiring lock, user
- *        may change emulator behaviour in multi-thread setup.
- *
- * @param emul Pointer to BMA255 emulator
- * @param timeout Timeout in getting lock
- *
- * @return k_mutex_lock return code
- */
-int bma_emul_lock_data(struct i2c_emul *emul, k_timeout_t timeout);
-
-/**
- * @brief Unlock access to BMA255 properties.
- *
- * @param emul Pointer to BMA255 emulator
- *
- * @return k_mutex_unlock return code
- */
-int bma_emul_unlock_data(struct i2c_emul *emul);
-
-/**
- * @brief Set write handler for I2C messages. This function is called before
- *        generic handler.
- *
- * @param emul Pointer to BMA255 emulator
- * @param func Pointer to custom function
- * @param data User data passed on call of custom function
- */
-void bma_emul_set_write_func(struct i2c_emul *emul, bma_emul_write_func func,
-			     void *data);
-
-/**
- * @brief Set read handler for I2C messages. This function is called before
- *        generic handler.
- *
- * @param emul Pointer to BMA255 emulator
- * @param func Pointer to custom function
- * @param data User data passed on call of custom function
- */
-void bma_emul_set_read_func(struct i2c_emul *emul, bma_emul_read_func func,
-			    void *data);
-
-/**
  * @brief Set value of given register of BMA255
  *
  * @param emul Pointer to BMA255 emulator
@@ -168,24 +87,6 @@ void bma_emul_set_reg(struct i2c_emul *emul, int reg, uint8_t val);
 uint8_t bma_emul_get_reg(struct i2c_emul *emul, int reg);
 
 /**
- * @brief Setup fail on read of given register of BMA255
- *
- * @param emul Pointer to BMA255 emulator
- * @param reg Register address or one of special values (BMA_EMUL_FAIL_ALL_REG,
- *            BMA_EMUL_NO_FAIL_REG)
- */
-void bma_emul_set_read_fail_reg(struct i2c_emul *emul, int reg);
-
-/**
- * @brief Setup fail on write of given register of BMA255
- *
- * @param emul Pointer to BMA255 emulator
- * @param reg Register address or one of special values (BMA_EMUL_FAIL_ALL_REG,
- *            BMA_EMUL_NO_FAIL_REG)
- */
-void bma_emul_set_write_fail_reg(struct i2c_emul *emul, int reg);
-
-/**
  * @brief Get internal value of offset for given axis
  *
  * @param emul Pointer to BMA255 emulator
@@ -269,6 +170,21 @@ void bma_emul_set_err_on_rsvd_write(struct i2c_emul *emul, bool set);
 void bma_emul_set_err_on_msb_first(struct i2c_emul *emul, bool set);
 
 /**
+ * @brief Function calculate register that should be accessed when I2C message
+ *        started from @p reg register and now byte number @p bytes is handled.
+ *        This function is used in I2C common emulator code and can be used in
+ *        custom user functions.
+ *
+ * @param emul Pointer to BMA255 emulator
+ * @param reg Starting register
+ * @param bytes Number of bytes already processed in the I2C message handler
+ * @param read If current I2C message is read
+ *
+ * @retval Register address that should be accessed
+ */
+int bma_emul_access_reg(struct i2c_emul *emul, int reg, int bytes, bool read);
+
+/**
  * @}
  */
 
diff --git a/zephyr/include/emul/emul_bmi.h b/zephyr/include/emul/emul_bmi.h
index d70b975195..9eac9c3f85 100644
--- a/zephyr/include/emul/emul_bmi.h
+++ b/zephyr/include/emul/emul_bmi.h
@@ -29,8 +29,6 @@
  *
  * - define a Device Tree overlay file to set which inadvisable driver behaviour
  *   should be treated as errors and which model is emulated
- * - call @ref bmi_emul_set_read_func and @ref bmi_emul_set_write_func to setup
- *   custom handlers for I2C messages
  * - call @ref bmi_emul_set_reg and @ref bmi_emul_get_reg to set and get value
  *   of BMI registers
  * - call @ref bmi_emul_set_off and @ref bmi_emul_get_off to set and get
@@ -41,11 +39,11 @@
  *   behaviour
  * - call @ref bmi_emul_simulate_cmd_exec_time to enable or disable simulation
  *   of command execution time
- * - call @ref bmi_emul_set_read_fail_reg and @ref bmi_emul_set_write_fail_reg
- *   to configure emulator to fail on given register read or write
  * - call @ref bmi_emul_append_frame to add frame to FIFO
  * - call @reg bmi_emul_set_skipped_frames to generate skip frame on next access
  *   to FIFO
+ * - call functions from emul_common_i2c.h to setup custom handlers for I2C
+ *   messages
  */
 
 /**
@@ -106,13 +104,6 @@ enum bmi_emul_axis {
  */
 #define BMI_EMUL_ACCESS_E	1
 
-/**
- * Special register values used in @ref bmi_emul_set_read_fail_reg and
- * @ref bmi_emul_set_write_fail_reg
- */
-#define BMI_EMUL_FAIL_ALL_REG	(-1)
-#define BMI_EMUL_NO_FAIL_REG	(-2)
-
 /** Structure used to describe single FIFO frame */
 struct bmi_emul_frame {
 	/** Type of frame */
@@ -145,14 +136,26 @@ struct bmi_emul_type_data {
 	bool sensortime_follow_config_frame;
 
 	/**
+	 * @brief Compute register address that acctually will be accessed, when
+	 *        selected register is @p reg and there was @p byte handled in
+	 *        the current I2C message
+	 *
+	 * @param emul Pointer to BMI emulator
+	 * @param reg Selected register
+	 * @param byte Number of handled bytes in the current I2C message
+	 * @param read If current I2C message is read
+	 *
+	 * @return Register address that will be accessed
+	 */
+	int (*access_reg)(struct i2c_emul *emul, int reg, int byte, bool read);
+
+	/**
 	 * @brief Model specific write function. It should modify state of
-	 *        emulator if required. @p reg value should be updated to
-	 *        register which is acctually accessed.
+	 *        emulator if required.
 	 *
 	 * @param regs Pointer to array of emulator's registers
 	 * @param emul Pointer to BMI emulator
-	 * @param reg Pointer to accessed reg. If different reg is accessed,
-	 *            this value should be modified.
+	 * @param reg Selected register
 	 * @param byte Number of handled bytes in this write command
 	 * @param val Value that is being written
 	 *
@@ -160,18 +163,15 @@ struct bmi_emul_type_data {
 	 * @return BMI_EMUL_ACCESS_E on RO register access
 	 * @return other on error
 	 */
-	int (*handle_write)(uint8_t *regs, struct i2c_emul *emul, int *reg,
+	int (*handle_write)(uint8_t *regs, struct i2c_emul *emul, int reg,
 			    int byte, uint8_t val);
 	/**
 	 * @brief Model specific read function. It should modify state of
-	 *        emulator if required. @p reg value should be updated to
-	 *        register which is acctually accessed. @p buf should be
-	 *        set to response value.
+	 *        emulator if required. @p buf should be set to response value.
 	 *
 	 * @param regs Pointer to array of emulator's registers
 	 * @param emul Pointer to BMI emulator
-	 * @param reg Pointer to accessed reg. If different reg is accessed,
-	 *            this value should be modified.
+	 * @param reg Selected register
 	 * @param byte Byte which is accessed during block read
 	 * @param buf Pointer where read byte should be stored
 	 *
@@ -179,7 +179,7 @@ struct bmi_emul_type_data {
 	 * @return BMI_EMUL_ACCESS_E on WO register access
 	 * @return other on error
 	 */
-	int (*handle_read)(uint8_t *regs, struct i2c_emul *emul, int *reg,
+	int (*handle_read)(uint8_t *regs, struct i2c_emul *emul, int reg,
 			   int byte, char *buf);
 	/**
 	 * @brief Model specific reset function. It should modify state of
@@ -229,82 +229,6 @@ const struct bmi_emul_type_data *get_bmi260_emul_type_data(void);
 struct i2c_emul *bmi_emul_get(int ord);
 
 /**
- * @brief Custom function type that is used as user-defined callback in read
- *        I2C messages handling.
- *
- * @param emul Pointer to BMI emulator
- * @param reg Address which is now accessed by read command
- * @param byte Byte which is accessed during block read
- * @param data Pointer to custom user data
- *
- * @return 0 on success. Value of @p reg should be set by @ref bmi_emul_set_reg
- * @return 1 continue with normal BMI emulator handler
- * @return negative on error
- */
-typedef int (*bmi_emul_read_func)(struct i2c_emul *emul, int reg, int byte,
-				  void *data);
-
-/**
- * @brief Custom function type that is used as user-defined callback in write
- *        I2C messages handling.
- *
- * @param emul Pointer to BMA255 emulator
- * @param reg Address which is now accessed by write command
- * @param byte Number of handled bytes in this write command. It does include
- *             first byte containing accessed register address.
- * @param val Value which is being written to @p reg
- * @param data Pointer to custom user data
- *
- * @return 0 on success
- * @return 1 continue with normal BMI emulator handler
- * @return negative on error
- */
-typedef int (*bmi_emul_write_func)(struct i2c_emul *emul, int reg, int byte,
-				   uint8_t val, void *data);
-
-/**
- * @brief Lock access to BMI properties. After acquiring lock, user
- *        may change emulator behaviour in multi-thread setup.
- *
- * @param emul Pointer to BMI emulator
- * @param timeout Timeout in getting lock
- *
- * @return k_mutex_lock return code
- */
-int bmi_emul_lock_data(struct i2c_emul *emul, k_timeout_t timeout);
-
-/**
- * @brief Unlock access to BMI properties.
- *
- * @param emul Pointer to BMI emulator
- *
- * @return k_mutex_unlock return code
- */
-int bmi_emul_unlock_data(struct i2c_emul *emul);
-
-/**
- * @brief Set write handler for I2C messages. This function is called before
- *        generic handler.
- *
- * @param emul Pointer to BMI emulator
- * @param func Pointer to custom function
- * @param data User data passed on call of custom function
- */
-void bmi_emul_set_write_func(struct i2c_emul *emul, bmi_emul_write_func func,
-			     void *data);
-
-/**
- * @brief Set read handler for I2C messages. This function is called before
- *        generic handler.
- *
- * @param emul Pointer to BMI emulator
- * @param func Pointer to custom function
- * @param data User data passed on call of custom function
- */
-void bmi_emul_set_read_func(struct i2c_emul *emul, bmi_emul_read_func func,
-			    void *data);
-
-/**
  * @brief Set value of given register of BMI
  *
  * @param emul Pointer to BMI emulator
@@ -324,24 +248,6 @@ void bmi_emul_set_reg(struct i2c_emul *emul, int reg, uint8_t val);
 uint8_t bmi_emul_get_reg(struct i2c_emul *emul, int reg);
 
 /**
- * @brief Setup fail on read of given register of BMI
- *
- * @param emul Pointer to BMI emulator
- * @param reg Register address or one of special values (BMI_EMUL_FAIL_ALL_REG,
- *            BMI_EMUL_NO_FAIL_REG)
- */
-void bmi_emul_set_read_fail_reg(struct i2c_emul *emul, int reg);
-
-/**
- * @brief Setup fail on write of given register of BMI
- *
- * @param emul Pointer to BMI emulator
- * @param reg Register address or one of special values (BMI_EMUL_FAIL_ALL_REG,
- *            BMI_EMUL_NO_FAIL_REG)
- */
-void bmi_emul_set_write_fail_reg(struct i2c_emul *emul, int reg);
-
-/**
  * @brief Get internal value of offset for given axis and sensor
  *
  * @param emul Pointer to BMI emulator
diff --git a/zephyr/include/emul/emul_smart_battery.h b/zephyr/include/emul/emul_smart_battery.h
index be5e7e4b16..b48f836d66 100644
--- a/zephyr/include/emul/emul_smart_battery.h
+++ b/zephyr/include/emul/emul_smart_battery.h
@@ -28,13 +28,12 @@
  * As-such, each application may
  *
  * - define a Device Tree overlay file to set the most of battery properties
- * - call @ref sbat_emul_set_custom_write_func and
- *   @ref sbat_emul_set_custom_read_func to setup custom handlers for SMBus
- *   messages.
  * - get battery properties calling @ref sbat_emul_get_bat_data Battery
  *   properties can be changed through obtained pointer. In multithread
  *   environment access to battery can be guarded by calling
  *   @ref sbat_emul_lock_bat_data and @ref sbat_emul_unlock_bat_data
+ * - call functions from emul_common_i2c.h to setup custom handlers for SMBus
+ *   messages
  */
 
 /* Value used to indicate that no command is selected */
@@ -123,26 +122,6 @@ struct sbat_emul_bat_data {
 struct i2c_emul *sbat_emul_get_ptr(int ord);
 
 /**
- * @brief Custom function type that is used as user defined callbacks in read
- *        and write SMBus messages handling.
- *
- * @param emul Pointer to smart battery emulator
- * @param buf Pointer to data from write command or to be filed by read command
- * @param len Pointer to number of bytes used for write command buffer. It may
- *            exceed MSG_BUF_LEN, indicating that some bytes from write command
- *            are not saved in @p buf. If read command is handled, than
- *            function should set how many bytes are written to @p buf
- * @param cmd Command that was recognized
- * @param data Pointer to custom user data
- *
- * @return 0 on success
- * @return 1 continue with normal smart battery emulator handler
- * @return negative on error
- */
-typedef int (*sbat_emul_custom_func)(struct i2c_emul *emul, uint8_t *buf,
-				     int *len, int cmd, void *data);
-
-/**
  * @brief Function which allows to get properties of emulated smart battery
  *
  * @param emul Pointer to smart battery emulator
@@ -152,48 +131,6 @@ typedef int (*sbat_emul_custom_func)(struct i2c_emul *emul, uint8_t *buf,
 struct sbat_emul_bat_data *sbat_emul_get_bat_data(struct i2c_emul *emul);
 
 /**
- * @brief Lock access to smart battery properties. After acquiring lock, user
- *        may change emulator behaviour in multi-thread setup.
- *
- * @param emul Pointer to smart battery emulator
- * @param timeout Timeout in getting lock
- *
- * @return k_mutex_lock return code
- */
-int sbat_emul_lock_bat_data(struct i2c_emul *emul, k_timeout_t timeout);
-
-/**
- * @brief Unlock access to smart battery properties.
- *
- * @param emul Pointer to smart battery emulator
- *
- * @return k_mutex_unlock return code
- */
-int sbat_emul_unlock_bat_dat(struct i2c_emul *emul);
-
-/**
- * @brief Set custom write SMBus message handler. This function is called before
- *        generic handler.
- *
- * @param emul Pointer to smart battery emulator
- * @param func Pointer to custom function
- * @param data User data passed on call of custom function
- */
-void sbat_emul_set_custom_write_func(struct i2c_emul *emul,
-				     sbat_emul_custom_func func, void *data);
-
-/**
- * @brief Set custom read SMBus message handler. This function is called before
- *        generic handler.
- *
- * @param emul Pointer to smart battery emulator
- * @param func Pointer to custom function
- * @param data User data passed on call of custom function
- */
-void sbat_emul_set_custom_read_func(struct i2c_emul *emul,
-				    sbat_emul_custom_func func, void *data);
-
-/**
  * @brief Convert date to format used by smart battery
  *
  * @param day Day
@@ -237,6 +174,19 @@ int sbat_emul_get_block_data(struct i2c_emul *emul, int cmd, uint8_t **blk,
 			     int *len);
 
 /**
+ * @brief Set next response of emulator. This function may be used in user
+ *        custom read callback to setup response with calculated PEC.
+ *
+ * @param emul Pointer to smart battery emulator
+ * @param cmd Read command
+ * @param buf Buffer with the response
+ * @param len Length of the response
+ * @param fail If emulator should fail to send response
+ */
+void sbat_emul_set_response(struct i2c_emul *emul, int cmd, uint8_t *buf,
+			    int len, bool fail);
+
+/**
  * @}
  */
 
diff --git a/zephyr/include/emul/emul_tcs3400.h b/zephyr/include/emul/emul_tcs3400.h
index 41f3577658..49ec382a66 100644
--- a/zephyr/include/emul/emul_tcs3400.h
+++ b/zephyr/include/emul/emul_tcs3400.h
@@ -27,16 +27,14 @@
  *
  * - define a devicetree overlay file to set which inadvisable driver behaviour
  *   should be treated as error and emulated device ID and revision
- * - call @ref tcs_emul_set_read_func and @ref tcs_emul_set_write_func to setup
- *   custom handlers for I2C messages
  * - call @ref tcs_emul_set_reg and @ref tcs_emul_get_reg to set and get value
  *   of TCS3400 registers
  * - call @ref tcs_emul_set_val and @ref tcs_emul_set_val to set and get
  *   light sensor value
  * - call tcs_emul_set_err_* to change emulator behaviour on inadvisable driver
  *   behaviour
- * - call @ref tcs_emul_set_read_fail_reg and @ref tcs_emul_set_write_fail_reg
- *   to configure emulator to fail on given register read or write
+ * - call functions from emul_common_i2c.h to setup custom handlers for I2C
+ *   messages
  */
 
 /**
@@ -77,13 +75,6 @@ enum tcs_emul_axis {
 #define TCS_EMUL_REG_COUNT	(TCS_EMUL_LAST_REG - TCS_EMUL_FIRST_REG + 1)
 
 /**
- * Special register values used in @ref tcs_emul_set_read_fail_reg and
- * @ref tcs_emul_set_write_fail_reg
- */
-#define TCS_EMUL_FAIL_ALL_REG	(-1)
-#define TCS_EMUL_NO_FAIL_REG	(-2)
-
-/**
  * @brief Get pointer to TCS3400 emulator using device tree order number.
  *
  * @param ord Device tree order number obtained from DT_DEP_ORD macro
@@ -93,78 +84,6 @@ enum tcs_emul_axis {
 struct i2c_emul *tcs_emul_get(int ord);
 
 /**
- * @brief Custom function type that is used as user-defined callback in read
- *        I2C messages handling.
- *
- * @param emul Pointer to TCS3400 emulator
- * @param reg Address which is now accessed by read command
- * @param data Pointer to custom user data
- *
- * @return 0 on success. Value of @p reg should be set by @ref tcs_emul_set_reg
- * @return 1 continue with normal TCS3400 emulator handler
- * @return negative on error
- */
-typedef int (*tcs_emul_read_func)(struct i2c_emul *emul, int reg, void *data);
-
-/**
- * @brief Custom function type that is used as user-defined callback in write
- *        I2C messages handling.
- *
- * @param emul Pointer to TCS3400 emulator
- * @param reg Address which is now accessed by write command
- * @param val Value which is being written to @p reg
- * @param data Pointer to custom user data
- *
- * @return 0 on success
- * @return 1 continue with normal TCS3400 emulator handler
- * @return negative on error
- */
-typedef int (*tcs_emul_write_func)(struct i2c_emul *emul, int reg, uint8_t val,
-				   void *data);
-
-/**
- * @brief Lock access to TCS3400 properties. After acquiring lock, user
- *        may change emulator behaviour in multi-thread setup.
- *
- * @param emul Pointer to TCS3400 emulator
- * @param timeout Timeout in getting lock
- *
- * @return k_mutex_lock return code
- */
-int tcs_emul_lock_data(struct i2c_emul *emul, k_timeout_t timeout);
-
-/**
- * @brief Unlock access to TCS3400 properties.
- *
- * @param emul Pointer to TCS3400 emulator
- *
- * @return k_mutex_unlock return code
- */
-int tcs_emul_unlock_data(struct i2c_emul *emul);
-
-/**
- * @brief Set write handler for I2C messages. This function is called before
- *        generic handler.
- *
- * @param emul Pointer to TCS3400 emulator
- * @param func Pointer to custom function
- * @param data User data passed on call of custom function
- */
-void tcs_emul_set_write_func(struct i2c_emul *emul, tcs_emul_write_func func,
-			     void *data);
-
-/**
- * @brief Set read handler for I2C messages. This function is called before
- *        generic handler.
- *
- * @param emul Pointer to TCS3400 emulator
- * @param func Pointer to custom function
- * @param data User data passed on call of custom function
- */
-void tcs_emul_set_read_func(struct i2c_emul *emul, tcs_emul_read_func func,
-			    void *data);
-
-/**
  * @brief Set value of given register of TCS3400
  *
  * @param emul Pointer to TCS3400 emulator
@@ -184,24 +103,6 @@ void tcs_emul_set_reg(struct i2c_emul *emul, int reg, uint8_t val);
 uint8_t tcs_emul_get_reg(struct i2c_emul *emul, int reg);
 
 /**
- * @brief Setup fail on read of given register of TCS3400
- *
- * @param emul Pointer to TCS3400 emulator
- * @param reg Register address or one of special values (TCS_EMUL_FAIL_ALL_REG,
- *            TCS_EMUL_NO_FAIL_REG)
- */
-void tcs_emul_set_read_fail_reg(struct i2c_emul *emul, int reg);
-
-/**
- * @brief Setup fail on write of given register of TCS3400
- *
- * @param emul Pointer to TCS3400 emulator
- * @param reg Register address or one of special values (TCS_EMUL_FAIL_ALL_REG,
- *            TCS_EMUL_NO_FAIL_REG)
- */
-void tcs_emul_set_write_fail_reg(struct i2c_emul *emul, int reg);
-
-/**
  * @brief Get internal value of light sensor for given axis
  *
  * @param emul Pointer to TCS3400 emulator
diff --git a/zephyr/test/drivers/src/bb_retimer.c b/zephyr/test/drivers/src/bb_retimer.c
index ac53df4779..c7e5fb7789 100644
--- a/zephyr/test/drivers/src/bb_retimer.c
+++ b/zephyr/test/drivers/src/bb_retimer.c
@@ -9,6 +9,7 @@
 #include "common.h"
 #include "ec_tasks.h"
 #include "emul/emul_bb_retimer.h"
+#include "emul/emul_common_i2c.h"
 #include "hooks.h"
 #include "i2c.h"
 #include "stubs.h"
@@ -41,7 +42,8 @@ static void test_bb_set_state(void)
 	set_test_runner_tid();
 
 	/* Setup emulator fail on write */
-	bb_emul_set_write_fail_reg(emul, BB_RETIMER_REG_CONNECTION_STATE);
+	i2c_common_emul_set_write_fail_reg(emul,
+					   BB_RETIMER_REG_CONNECTION_STATE);
 
 	/* Test fail on reset register write */
 	zassert_equal(-EIO, bb_usb_retimer.set(&usb_muxes[USBC_PORT_C1],
@@ -50,7 +52,7 @@ static void test_bb_set_state(void)
 	zassert_false(ack_required, "ACK is never required for BB retimer");
 
 	/* Do not fail on write */
-	bb_emul_set_write_fail_reg(emul, BB_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Set UFP role for whole test */
 	tc_set_data_role(USBC_PORT_C1, PD_ROLE_UFP);
diff --git a/zephyr/test/drivers/src/bma2x2.c b/zephyr/test/drivers/src/bma2x2.c
index 457aeeef5d..f655e0c99a 100644
--- a/zephyr/test/drivers/src/bma2x2.c
+++ b/zephyr/test/drivers/src/bma2x2.c
@@ -9,6 +9,7 @@
 #include "common.h"
 #include "i2c.h"
 #include "emul/emul_bma255.h"
+#include "emul/emul_common_i2c.h"
 
 #include "accelgyro.h"
 #include "motion_sense.h"
@@ -120,10 +121,12 @@ struct reset_func_data {
  * accessing register data.ok_before_fail times. Error is returned during next
  * data.fail_attempts times.
  */
-static int emul_read_reset(struct i2c_emul *emul, int reg, void *data)
+static int emul_read_reset(struct i2c_emul *emul, int reg, uint8_t *buf,
+			   int bytes, void *data)
 {
 	struct reset_func_data *d = data;
 
+	reg = bma_emul_access_reg(emul, reg, bytes, true /* = read */);
 	if (reg != BMA2x2_RST_ADDR) {
 		return 1;
 	}
@@ -161,15 +164,15 @@ static void test_bma_get_offset(void)
 	emul = bma_emul_get(BMA_ORD);
 
 	/* Test fail on each axis */
-	bma_emul_set_read_fail_reg(emul, BMA2x2_OFFSET_X_AXIS_ADDR);
+	i2c_common_emul_set_read_fail_reg(emul, BMA2x2_OFFSET_X_AXIS_ADDR);
 	zassert_equal(-EIO, ms.drv->get_offset(&ms, ret_offset, &temp), NULL);
-	bma_emul_set_read_fail_reg(emul, BMA2x2_OFFSET_Y_AXIS_ADDR);
+	i2c_common_emul_set_read_fail_reg(emul, BMA2x2_OFFSET_Y_AXIS_ADDR);
 	zassert_equal(-EIO, ms.drv->get_offset(&ms, ret_offset, &temp), NULL);
-	bma_emul_set_read_fail_reg(emul, BMA2x2_OFFSET_Z_AXIS_ADDR);
+	i2c_common_emul_set_read_fail_reg(emul, BMA2x2_OFFSET_Z_AXIS_ADDR);
 	zassert_equal(-EIO, ms.drv->get_offset(&ms, ret_offset, &temp), NULL);
 
 	/* Do not fail on read */
-	bma_emul_set_read_fail_reg(emul, BMA_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Set emulator offset */
 	exp_offset[0] = BMA_EMUL_1G / 10;
@@ -209,15 +212,15 @@ static void test_bma_set_offset(void)
 	emul = bma_emul_get(BMA_ORD);
 
 	/* Test fail on each axis */
-	bma_emul_set_write_fail_reg(emul, BMA2x2_OFFSET_X_AXIS_ADDR);
+	i2c_common_emul_set_write_fail_reg(emul, BMA2x2_OFFSET_X_AXIS_ADDR);
 	zassert_equal(-EIO, ms.drv->set_offset(&ms, exp_offset, temp), NULL);
-	bma_emul_set_write_fail_reg(emul, BMA2x2_OFFSET_Y_AXIS_ADDR);
+	i2c_common_emul_set_write_fail_reg(emul, BMA2x2_OFFSET_Y_AXIS_ADDR);
 	zassert_equal(-EIO, ms.drv->set_offset(&ms, exp_offset, temp), NULL);
-	bma_emul_set_write_fail_reg(emul, BMA2x2_OFFSET_Z_AXIS_ADDR);
+	i2c_common_emul_set_write_fail_reg(emul, BMA2x2_OFFSET_Z_AXIS_ADDR);
 	zassert_equal(-EIO, ms.drv->set_offset(&ms, exp_offset, temp), NULL);
 
 	/* Do not fail on write */
-	bma_emul_set_write_fail_reg(emul, BMA_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Set input offset */
 	exp_offset[0] = BMA_EMUL_1G / 10;
@@ -305,7 +308,7 @@ static void test_bma_set_range(void)
 	ms.current_range = start_range;
 	bma_emul_set_reg(emul, BMA2x2_RANGE_SELECT_ADDR, BMA2x2_RANGE_2G);
 	/* Setup emulator fail on read */
-	bma_emul_set_read_fail_reg(emul, BMA2x2_RANGE_SELECT_ADDR);
+	i2c_common_emul_set_read_fail_reg(emul, BMA2x2_RANGE_SELECT_ADDR);
 
 	/* Test fail on read */
 	zassert_equal(-EIO, ms.drv->set_range(&ms, 12, 0), NULL);
@@ -318,10 +321,10 @@ static void test_bma_set_range(void)
 		      bma_emul_get_reg(emul, BMA2x2_RANGE_SELECT_ADDR), NULL);
 
 	/* Do not fail on read */
-	bma_emul_set_read_fail_reg(emul, BMA_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Setup emulator fail on write */
-	bma_emul_set_write_fail_reg(emul, BMA2x2_RANGE_SELECT_ADDR);
+	i2c_common_emul_set_write_fail_reg(emul, BMA2x2_RANGE_SELECT_ADDR);
 
 	/* Test fail on write */
 	zassert_equal(-EIO, ms.drv->set_range(&ms, 12, 0), NULL);
@@ -334,7 +337,7 @@ static void test_bma_set_range(void)
 		      bma_emul_get_reg(emul, BMA2x2_RANGE_SELECT_ADDR), NULL);
 
 	/* Do not fail on write */
-	bma_emul_set_write_fail_reg(emul, BMA_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test setting range with rounding down */
 	check_set_range(emul, 1, 0, 2);
@@ -374,42 +377,42 @@ static void test_bma_init(void)
 	emul = bma_emul_get(BMA_ORD);
 
 	/* Setup emulator fail read function */
-	bma_emul_set_read_fail_reg(emul, BMA2x2_CHIP_ID_ADDR);
+	i2c_common_emul_set_read_fail_reg(emul, BMA2x2_CHIP_ID_ADDR);
 
 	/* Test fail on chip id read */
 	zassert_equal(EC_ERROR_UNKNOWN, ms.drv->init(&ms), NULL);
 
 	/* Disable failing on chip id read, but set wrong value */
-	bma_emul_set_read_fail_reg(emul, BMA_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 	bma_emul_set_reg(emul, BMA2x2_CHIP_ID_ADDR, 23);
 
 	/* Test wrong chip id */
 	zassert_equal(EC_ERROR_ACCESS_DENIED, ms.drv->init(&ms), NULL);
 
 	/* Set correct chip id, but fail on reset reg read */
-	bma_emul_set_read_fail_reg(emul, BMA2x2_RST_ADDR);
+	i2c_common_emul_set_read_fail_reg(emul, BMA2x2_RST_ADDR);
 	bma_emul_set_reg(emul, BMA2x2_CHIP_ID_ADDR, BMA255_CHIP_ID_MAJOR);
 
 	/* Test fail on reset register read */
 	zassert_equal(-EIO, ms.drv->init(&ms), NULL);
 
 	/* Do not fail on read */
-	bma_emul_set_read_fail_reg(emul, BMA_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Setup emulator fail on write */
-	bma_emul_set_write_fail_reg(emul, BMA2x2_RST_ADDR);
+	i2c_common_emul_set_write_fail_reg(emul, BMA2x2_RST_ADDR);
 
 	/* Test fail on reset register write */
 	zassert_equal(-EIO, ms.drv->init(&ms), NULL);
 
 	/* Do not fail on write */
-	bma_emul_set_write_fail_reg(emul, BMA_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Setup emulator fail reset read function */
 	reset_func_data.ok_before_fail = 1;
 	reset_func_data.fail_attempts = 100;
 	reset_func_data.reset_value = 0;
-	bma_emul_set_read_func(emul, emul_read_reset, &reset_func_data);
+	i2c_common_emul_set_read_func(emul, emul_read_reset, &reset_func_data);
 
 	/* Test fail on too many reset read errors */
 	zassert_equal(EC_ERROR_TIMEOUT, ms.drv->init(&ms), NULL);
@@ -434,7 +437,7 @@ static void test_bma_init(void)
 	zassert_equal(EC_RES_SUCCESS, ms.drv->init(&ms), NULL);
 
 	/* Remove custom emulator read function */
-	bma_emul_set_read_func(emul, NULL, NULL);
+	i2c_common_emul_set_read_func(emul, NULL, NULL);
 }
 
 /*
@@ -568,7 +571,7 @@ static void test_bma_rate(void)
 	reg_rate = bma_emul_get_reg(emul, BMA2x2_BW_SELECT_ADDR);
 
 	/* Setup emulator fail on read */
-	bma_emul_set_read_fail_reg(emul, BMA2x2_BW_SELECT_ADDR);
+	i2c_common_emul_set_read_fail_reg(emul, BMA2x2_BW_SELECT_ADDR);
 
 	/* Test fail on read */
 	zassert_equal(-EIO, ms.drv->set_data_rate(&ms, 15625, 0), NULL);
@@ -581,10 +584,10 @@ static void test_bma_rate(void)
 		      bma_emul_get_reg(emul, BMA2x2_BW_SELECT_ADDR), NULL);
 
 	/* Do not fail on read */
-	bma_emul_set_read_fail_reg(emul, BMA_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Setup emulator fail on write */
-	bma_emul_set_write_fail_reg(emul, BMA2x2_BW_SELECT_ADDR);
+	i2c_common_emul_set_write_fail_reg(emul, BMA2x2_BW_SELECT_ADDR);
 
 	/* Test fail on write */
 	zassert_equal(-EIO, ms.drv->set_data_rate(&ms, 15625, 0), NULL);
@@ -597,7 +600,7 @@ static void test_bma_rate(void)
 		      bma_emul_get_reg(emul, BMA2x2_BW_SELECT_ADDR), NULL);
 
 	/* Do not fail on write */
-	bma_emul_set_write_fail_reg(emul, BMA_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 }
 
 /** Test read with and without I2C errors. */
@@ -616,21 +619,21 @@ static void test_bma_read(void)
 	bma_emul_set_off(emul, BMA_EMUL_AXIS_Z, 0);
 
 	/* Test fail on each axis */
-	bma_emul_set_read_fail_reg(emul, BMA2x2_X_AXIS_LSB_ADDR);
+	i2c_common_emul_set_read_fail_reg(emul, BMA2x2_X_AXIS_LSB_ADDR);
 	zassert_equal(-EIO, ms.drv->read(&ms, ret_acc_v), NULL);
-	bma_emul_set_read_fail_reg(emul, BMA2x2_X_AXIS_MSB_ADDR);
+	i2c_common_emul_set_read_fail_reg(emul, BMA2x2_X_AXIS_MSB_ADDR);
 	zassert_equal(-EIO, ms.drv->read(&ms, ret_acc_v), NULL);
-	bma_emul_set_read_fail_reg(emul, BMA2x2_Y_AXIS_LSB_ADDR);
+	i2c_common_emul_set_read_fail_reg(emul, BMA2x2_Y_AXIS_LSB_ADDR);
 	zassert_equal(-EIO, ms.drv->read(&ms, ret_acc_v), NULL);
-	bma_emul_set_read_fail_reg(emul, BMA2x2_Y_AXIS_MSB_ADDR);
+	i2c_common_emul_set_read_fail_reg(emul, BMA2x2_Y_AXIS_MSB_ADDR);
 	zassert_equal(-EIO, ms.drv->read(&ms, ret_acc_v), NULL);
-	bma_emul_set_read_fail_reg(emul, BMA2x2_Z_AXIS_LSB_ADDR);
+	i2c_common_emul_set_read_fail_reg(emul, BMA2x2_Z_AXIS_LSB_ADDR);
 	zassert_equal(-EIO, ms.drv->read(&ms, ret_acc_v), NULL);
-	bma_emul_set_read_fail_reg(emul, BMA2x2_Z_AXIS_MSB_ADDR);
+	i2c_common_emul_set_read_fail_reg(emul, BMA2x2_Z_AXIS_MSB_ADDR);
 	zassert_equal(-EIO, ms.drv->read(&ms, ret_acc_v), NULL);
 
 	/* Do not fail on read */
-	bma_emul_set_read_fail_reg(emul, BMA_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Set input accelerometer values */
 	exp_acc[0] = BMA_EMUL_1G / 10;
@@ -692,12 +695,14 @@ struct calib_func_data {
  * error when offset control register is accessed when cal ready bit is not set
  * and data.read_fail is not zero.
  */
-static int emul_read_calib_func(struct i2c_emul *emul, int reg, void *data)
+static int emul_read_calib_func(struct i2c_emul *emul, int reg, uint8_t *val,
+				int bytes, void *data)
 {
 	struct calib_func_data *d = data;
 	uint8_t reg_val;
 	int cur_time;
 
+	reg = bma_emul_access_reg(emul, reg, bytes, true /* = read */);
 	if (reg != BMA2x2_OFFSET_CTRL_ADDR) {
 		return 1;
 	}
@@ -723,10 +728,11 @@ static int emul_read_calib_func(struct i2c_emul *emul, int reg, void *data)
  * triggerd.
  */
 static int emul_write_calib_func(struct i2c_emul *emul, int reg, uint8_t val,
-				 void *data)
+				 int bytes, void *data)
 {
 	struct calib_func_data *d = data;
 
+	reg = bma_emul_access_reg(emul, reg, bytes, false /* = read */);
 	if (reg != BMA2x2_OFFSET_CTRL_ADDR) {
 		return 1;
 	}
@@ -783,8 +789,8 @@ static void test_bma_perform_calib(void)
 	exp_off[2] = BMA_EMUL_1G - exp_off[2];
 
 	/* Setup emulator calibration functions */
-	bma_emul_set_read_func(emul, emul_read_calib_func, &func_data);
-	bma_emul_set_write_func(emul, emul_write_calib_func, &func_data);
+	i2c_common_emul_set_read_func(emul, emul_read_calib_func, &func_data);
+	i2c_common_emul_set_write_func(emul, emul_write_calib_func, &func_data);
 
 	/* Setup emulator to fail on first access to offset control register */
 	func_data.calib_start = k_uptime_get_32();
@@ -833,7 +839,6 @@ static void test_bma_perform_calib(void)
 	func_data.read_fail = 0;
 	func_data.time = 1000;
 
-	/* Test fail on too long offset compensation */
 	zassert_equal(EC_RES_TIMEOUT, ms.drv->perform_calib(&ms, 1), NULL);
 	zassert_equal(range, ms.current_range, NULL);
 	zassert_equal(rate, ms.drv->get_data_rate(&ms), NULL);
@@ -883,8 +888,8 @@ static void test_bma_perform_calib(void)
 	compare_int3v(exp_off, ret_off);
 
 	/* Remove custom emulator functions */
-	bma_emul_set_read_func(emul, NULL, NULL);
-	bma_emul_set_write_func(emul, NULL, NULL);
+	i2c_common_emul_set_read_func(emul, NULL, NULL);
+	i2c_common_emul_set_write_func(emul, NULL, NULL);
 }
 
 /** Test get resolution. */
diff --git a/zephyr/test/drivers/src/bmi160.c b/zephyr/test/drivers/src/bmi160.c
index d0cfed200d..030325674b 100644
--- a/zephyr/test/drivers/src/bmi160.c
+++ b/zephyr/test/drivers/src/bmi160.c
@@ -9,6 +9,7 @@
 #include "common.h"
 #include "i2c.h"
 #include "emul/emul_bmi.h"
+#include "emul/emul_common_i2c.h"
 
 #include "motion_sense_fifo.h"
 #include "driver/accelgyro_bmi160.h"
@@ -154,13 +155,13 @@ static void test_bmi_acc_get_offset(void)
 	exp_v[2] = -1000 / 30;
 
 	/* Test fail on offset read */
-	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_ACC70);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_OFFSET_ACC70);
 	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_ACC70 + 1);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_OFFSET_ACC70 + 1);
 	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_ACC70 + 2);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_OFFSET_ACC70 + 2);
 	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Disable rotation */
 	ms->rot_standard_ref = NULL;
@@ -198,7 +199,7 @@ static void test_bmi_gyr_get_offset(void)
 	ms = &motion_sensors[BMI_GYR_SENSOR_ID];
 
 	/* Do not fail on read */
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Set emulator offset */
 	exp_v[0] = BMI_EMUL_125_DEG_S / 100;
@@ -211,15 +212,15 @@ static void test_bmi_gyr_get_offset(void)
 	exp_v[2] = -125000 / 300;
 
 	/* Test fail on offset read */
-	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_GYR70);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_OFFSET_GYR70);
 	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_GYR70 + 1);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_OFFSET_GYR70 + 1);
 	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_GYR70 + 2);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_OFFSET_GYR70 + 2);
 	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_EN_GYR98);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_OFFSET_EN_GYR98);
 	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Disable rotation */
 	ms->rot_standard_ref = NULL;
@@ -260,21 +261,21 @@ static void test_bmi_acc_set_offset(void)
 	ms = &motion_sensors[BMI_ACC_SENSOR_ID];
 
 	/* Test fail on OFFSET EN GYR98 register read and write */
-	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_EN_GYR98);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_OFFSET_EN_GYR98);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
-	bmi_emul_set_write_fail_reg(emul, BMI160_OFFSET_EN_GYR98);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, BMI160_OFFSET_EN_GYR98);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test fail on offset write */
-	bmi_emul_set_write_fail_reg(emul, BMI160_OFFSET_ACC70);
+	i2c_common_emul_set_write_fail_reg(emul, BMI160_OFFSET_ACC70);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_write_fail_reg(emul, BMI160_OFFSET_ACC70 + 1);
+	i2c_common_emul_set_write_fail_reg(emul, BMI160_OFFSET_ACC70 + 1);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_write_fail_reg(emul, BMI160_OFFSET_ACC70 + 2);
+	i2c_common_emul_set_write_fail_reg(emul, BMI160_OFFSET_ACC70 + 2);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Set input offset */
 	exp_v[0] = BMI_EMUL_1G / 10;
@@ -331,21 +332,21 @@ static void test_bmi_gyr_set_offset(void)
 	ms = &motion_sensors[BMI_GYR_SENSOR_ID];
 
 	/* Test fail on OFFSET EN GYR98 register read and write */
-	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_EN_GYR98);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_OFFSET_EN_GYR98);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
-	bmi_emul_set_write_fail_reg(emul, BMI160_OFFSET_EN_GYR98);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, BMI160_OFFSET_EN_GYR98);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test fail on offset write */
-	bmi_emul_set_write_fail_reg(emul, BMI160_OFFSET_GYR70);
+	i2c_common_emul_set_write_fail_reg(emul, BMI160_OFFSET_GYR70);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_write_fail_reg(emul, BMI160_OFFSET_GYR70 + 1);
+	i2c_common_emul_set_write_fail_reg(emul, BMI160_OFFSET_GYR70 + 1);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_write_fail_reg(emul, BMI160_OFFSET_GYR70 + 2);
+	i2c_common_emul_set_write_fail_reg(emul, BMI160_OFFSET_GYR70 + 2);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Set input offset */
 	exp_v[0] = BMI_EMUL_125_DEG_S / 100;
@@ -441,7 +442,7 @@ static void test_bmi_acc_set_range(void)
 	ms->current_range = start_range;
 	bmi_emul_set_reg(emul, BMI160_ACC_RANGE, BMI160_GSEL_2G);
 	/* Setup emulator fail on write */
-	bmi_emul_set_write_fail_reg(emul, BMI160_ACC_RANGE);
+	i2c_common_emul_set_write_fail_reg(emul, BMI160_ACC_RANGE);
 
 	/* Test fail on write */
 	zassert_equal(-EIO, ms->drv->set_range(ms, 12, 0), NULL);
@@ -454,7 +455,7 @@ static void test_bmi_acc_set_range(void)
 		      bmi_emul_get_reg(emul, BMI160_ACC_RANGE), NULL);
 
 	/* Do not fail on write */
-	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test setting range with rounding down */
 	check_set_acc_range(emul, ms, 1, 0, 2);
@@ -549,7 +550,7 @@ static void test_bmi_gyr_set_range(void)
 	ms->current_range = start_range;
 	bmi_emul_set_reg(emul, BMI160_GYR_RANGE, BMI160_DPS_SEL_250);
 	/* Setup emulator fail on write */
-	bmi_emul_set_write_fail_reg(emul, BMI160_GYR_RANGE);
+	i2c_common_emul_set_write_fail_reg(emul, BMI160_GYR_RANGE);
 
 	/* Test fail on write */
 	zassert_equal(-EIO, ms->drv->set_range(ms, 125, 0), NULL);
@@ -562,7 +563,7 @@ static void test_bmi_gyr_set_range(void)
 		      bmi_emul_get_reg(emul, BMI160_GYR_RANGE), NULL);
 
 	/* Do not fail on write */
-	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test setting range with rounding down */
 	check_set_gyr_range(emul, ms, 1, 0, 125);
@@ -756,7 +757,7 @@ static void test_bmi_acc_rate(void)
 	reg_rate = bmi_emul_get_reg(emul, BMI160_ACC_CONF);
 
 	/* Setup emulator fail on read */
-	bmi_emul_set_read_fail_reg(emul, BMI160_ACC_CONF);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_ACC_CONF);
 
 	/* Test fail on read */
 	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 50000, 0), NULL);
@@ -767,10 +768,10 @@ static void test_bmi_acc_rate(void)
 	zassert_equal(reg_rate, bmi_emul_get_reg(emul, BMI160_ACC_CONF), NULL);
 
 	/* Do not fail on read */
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Setup emulator fail on write */
-	bmi_emul_set_write_fail_reg(emul, BMI160_ACC_CONF);
+	i2c_common_emul_set_write_fail_reg(emul, BMI160_ACC_CONF);
 
 	/* Test fail on write */
 	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 50000, 0), NULL);
@@ -781,7 +782,7 @@ static void test_bmi_acc_rate(void)
 	zassert_equal(reg_rate, bmi_emul_get_reg(emul, BMI160_ACC_CONF), NULL);
 
 	/* Do not fail on write */
-	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test disabling sensor */
 	pmu_status = BMI160_PMU_NORMAL << BMI160_PMU_ACC_OFFSET;
@@ -935,7 +936,7 @@ static void test_bmi_gyr_rate(void)
 	reg_rate = bmi_emul_get_reg(emul, BMI160_GYR_CONF);
 
 	/* Setup emulator fail on read */
-	bmi_emul_set_read_fail_reg(emul, BMI160_GYR_CONF);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_GYR_CONF);
 
 	/* Test fail on read */
 	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 50000, 0), NULL);
@@ -946,10 +947,10 @@ static void test_bmi_gyr_rate(void)
 	zassert_equal(reg_rate, bmi_emul_get_reg(emul, BMI160_GYR_CONF), NULL);
 
 	/* Do not fail on read */
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Setup emulator fail on write */
-	bmi_emul_set_write_fail_reg(emul, BMI160_GYR_CONF);
+	i2c_common_emul_set_write_fail_reg(emul, BMI160_GYR_CONF);
 
 	/* Test fail on write */
 	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 50000, 0), NULL);
@@ -960,7 +961,7 @@ static void test_bmi_gyr_rate(void)
 	zassert_equal(reg_rate, bmi_emul_get_reg(emul, BMI160_GYR_CONF), NULL);
 
 	/* Do not fail on write */
-	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test disabling sensor */
 	pmu_status = BMI160_PMU_NORMAL << BMI160_PMU_ACC_OFFSET;
@@ -1030,18 +1031,18 @@ static void test_bmi_read_temp(void)
 	ms_gyr = &motion_sensors[BMI_GYR_SENSOR_ID];
 
 	/* Setup emulator fail on read */
-	bmi_emul_set_read_fail_reg(emul, BMI160_TEMPERATURE_0);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_TEMPERATURE_0);
 	zassert_equal(EC_ERROR_NOT_POWERED,
 		      ms_acc->drv->read_temp(ms_acc, &ret_temp), NULL);
 	zassert_equal(EC_ERROR_NOT_POWERED,
 		      ms_gyr->drv->read_temp(ms_gyr, &ret_temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_TEMPERATURE_1);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_TEMPERATURE_1);
 	zassert_equal(EC_ERROR_NOT_POWERED,
 		      ms_acc->drv->read_temp(ms_acc, &ret_temp), NULL);
 	zassert_equal(EC_ERROR_NOT_POWERED,
 		      ms_gyr->drv->read_temp(ms_gyr, &ret_temp), NULL);
 	/* Do not fail on read */
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Fail on invalid temperature */
 	bmi_emul_set_reg(emul, BMI160_TEMPERATURE_0, 0x00);
@@ -1116,10 +1117,10 @@ static void test_bmi_acc_read(void)
 	bmi_emul_set_off(emul, BMI_EMUL_ACC_Z, 0);
 
 	/* Fail on read status */
-	bmi_emul_set_read_fail_reg(emul, BMI160_STATUS);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_STATUS);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
 
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* When not ready, driver should return saved raw value */
 	exp_v[0] = 100;
@@ -1187,20 +1188,20 @@ static void test_bmi_acc_read(void)
 	compare_int3v(exp_v, ret_v);
 
 	/* Fail on read of data registers */
-	bmi_emul_set_read_fail_reg(emul, BMI160_ACC_X_L_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_ACC_X_L_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_ACC_X_H_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_ACC_X_H_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_ACC_Y_L_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_ACC_Y_L_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_ACC_Y_H_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_ACC_Y_H_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_ACC_Z_L_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_ACC_Z_L_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_ACC_Z_H_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_ACC_Z_H_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
 
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 	ms->rot_standard_ref = NULL;
 }
 
@@ -1224,10 +1225,10 @@ static void test_bmi_gyr_read(void)
 	bmi_emul_set_off(emul, BMI_EMUL_GYR_Z, 0);
 
 	/* Fail on read status */
-	bmi_emul_set_read_fail_reg(emul, BMI160_STATUS);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_STATUS);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
 
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* When not ready, driver should return saved raw value */
 	exp_v[0] = 100;
@@ -1295,20 +1296,20 @@ static void test_bmi_gyr_read(void)
 	compare_int3v(exp_v, ret_v);
 
 	/* Fail on read of data registers */
-	bmi_emul_set_read_fail_reg(emul, BMI160_GYR_X_L_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_GYR_X_L_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_GYR_X_H_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_GYR_X_H_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_GYR_Y_L_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_GYR_Y_L_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_GYR_Y_H_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_GYR_Y_H_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_GYR_Z_L_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_GYR_Z_L_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_GYR_Z_H_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_GYR_Z_H_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
 
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 	ms->rot_standard_ref = NULL;
 }
 
@@ -1316,10 +1317,12 @@ static void test_bmi_gyr_read(void)
  * Custom emulatro read function which always return not ready STATUS register.
  * Used in calibration test.
  */
-static int emul_nrdy(struct i2c_emul *emul, int reg, int byte, void *data)
+static int emul_nrdy(struct i2c_emul *emul, int reg, uint8_t *val, int byte,
+		     void *data)
 {
 	if (reg == BMI160_STATUS) {
 		bmi_emul_set_reg(emul, BMI160_STATUS, 0);
+		*val = 0;
 
 		return 0;
 	}
@@ -1379,13 +1382,13 @@ static void test_bmi_acc_perform_calib(void)
 	exp_off[2] = BMI_EMUL_1G - exp_off[2];
 
 	/* Test fail on rate set */
-	bmi_emul_set_read_fail_reg(emul, BMI160_ACC_CONF);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_ACC_CONF);
 	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
 	zassert_equal(range, ms->current_range, NULL);
 	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
 
 	/* Test fail on status read */
-	bmi_emul_set_read_fail_reg(emul, BMI160_STATUS);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_STATUS);
 	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
 	zassert_equal(range, ms->current_range, NULL);
 	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
@@ -1393,13 +1396,13 @@ static void test_bmi_acc_perform_calib(void)
 	bmi_emul_set_reg(emul, BMI160_CMD_REG, BMI160_CMD_NOOP);
 
 	/* Test fail on data not ready */
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
-	bmi_emul_set_read_func(emul, emul_nrdy, NULL);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_func(emul, emul_nrdy, NULL);
 	zassert_equal(EC_RES_TIMEOUT, ms->drv->perform_calib(ms, 1), NULL);
 	zassert_equal(range, ms->current_range, NULL);
 	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
 	/* Remove custom emulator read function */
-	bmi_emul_set_read_func(emul, NULL, NULL);
+	i2c_common_emul_set_read_func(emul, NULL, NULL);
 	/* Stop fast offset compensation before next test */
 	bmi_emul_set_reg(emul, BMI160_CMD_REG, BMI160_CMD_NOOP);
 
@@ -1503,13 +1506,13 @@ static void test_bmi_gyr_perform_calib(void)
 	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
 
 	/* Test fail on rate set */
-	bmi_emul_set_read_fail_reg(emul, BMI160_GYR_CONF);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_GYR_CONF);
 	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
 	zassert_equal(range, ms->current_range, NULL);
 	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
 
 	/* Test fail on status read */
-	bmi_emul_set_read_fail_reg(emul, BMI160_STATUS);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_STATUS);
 	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
 	zassert_equal(range, ms->current_range, NULL);
 	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
@@ -1517,13 +1520,13 @@ static void test_bmi_gyr_perform_calib(void)
 	bmi_emul_set_reg(emul, BMI160_CMD_REG, BMI160_CMD_NOOP);
 
 	/* Test fail on data not ready */
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
-	bmi_emul_set_read_func(emul, emul_nrdy, NULL);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_func(emul, emul_nrdy, NULL);
 	zassert_equal(EC_RES_TIMEOUT, ms->drv->perform_calib(ms, 1), NULL);
 	zassert_equal(range, ms->current_range, NULL);
 	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
 	/* Remove custom emulator read function */
-	bmi_emul_set_read_func(emul, NULL, NULL);
+	i2c_common_emul_set_read_func(emul, NULL, NULL);
 	/* Stop fast offset compensation before next test */
 	bmi_emul_set_reg(emul, BMI160_CMD_REG, BMI160_CMD_NOOP);
 
@@ -1568,7 +1571,8 @@ struct fifo_func_data {
  * to value passed as additional data. It sets interrupt registers to 0 after
  * access.
  */
-static int emul_fifo_func(struct i2c_emul *emul, int reg, int byte, void *data)
+static int emul_fifo_func(struct i2c_emul *emul, int reg, uint8_t *val,
+			  int byte, void *data)
 {
 	struct fifo_func_data *d = data;
 
@@ -1703,11 +1707,11 @@ static void test_bmi_acc_fifo(void)
 	event = BMI_INT_EVENT;
 
 	/* Test fail to read interrupt status registers */
-	bmi_emul_set_read_fail_reg(emul, BMI160_INT_STATUS_0);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_INT_STATUS_0);
 	zassert_equal(-EIO, ms->drv->irq_handler(ms, &event), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_INT_STATUS_1);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_INT_STATUS_1);
 	zassert_equal(-EIO, ms->drv->irq_handler(ms, &event), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test no interrupt */
 	bmi_emul_set_reg(emul, BMI160_INT_STATUS_0, 0);
@@ -1717,7 +1721,7 @@ static void test_bmi_acc_fifo(void)
 	check_fifo(ms, ms_gyr, NULL, acc_range, gyr_range);
 
 	/* Set custom function for FIFO test */
-	bmi_emul_set_read_func(emul, emul_fifo_func, &func_data);
+	i2c_common_emul_set_read_func(emul, emul_fifo_func, &func_data);
 	/* Enable sensor FIFO */
 	zassert_equal(EC_SUCCESS, ms->drv->set_data_rate(ms, 50000, 0), NULL);
 	/* Set range */
@@ -1802,7 +1806,7 @@ static void test_bmi_acc_fifo(void)
 	check_fifo(ms, ms_gyr, f, acc_range, gyr_range);
 
 	/* Remove custom emulator read function */
-	bmi_emul_set_read_func(emul, NULL, NULL);
+	i2c_common_emul_set_read_func(emul, NULL, NULL);
 }
 
 /** Test irq handler of gyroscope sensor */
diff --git a/zephyr/test/drivers/src/bmi260.c b/zephyr/test/drivers/src/bmi260.c
index 63b5da0784..b4bd9b540a 100644
--- a/zephyr/test/drivers/src/bmi260.c
+++ b/zephyr/test/drivers/src/bmi260.c
@@ -9,6 +9,7 @@
 #include "common.h"
 #include "i2c.h"
 #include "emul/emul_bmi.h"
+#include "emul/emul_common_i2c.h"
 
 #include "motion_sense_fifo.h"
 #include "driver/accelgyro_bmi260.h"
@@ -154,13 +155,13 @@ static void test_bmi_acc_get_offset(void)
 	exp_v[2] = -1000 / 30;
 
 	/* Test fail on offset read */
-	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_ACC70);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_OFFSET_ACC70);
 	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_ACC70 + 1);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_OFFSET_ACC70 + 1);
 	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_ACC70 + 2);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_OFFSET_ACC70 + 2);
 	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Disable rotation */
 	ms->rot_standard_ref = NULL;
@@ -198,7 +199,7 @@ static void test_bmi_gyr_get_offset(void)
 	ms = &motion_sensors[BMI_GYR_SENSOR_ID];
 
 	/* Do not fail on read */
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Set emulator offset */
 	exp_v[0] = BMI_EMUL_125_DEG_S / 100;
@@ -211,15 +212,15 @@ static void test_bmi_gyr_get_offset(void)
 	exp_v[2] = -125000 / 300;
 
 	/* Test fail on offset read */
-	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_GYR70);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_OFFSET_GYR70);
 	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_GYR70 + 1);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_OFFSET_GYR70 + 1);
 	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_GYR70 + 2);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_OFFSET_GYR70 + 2);
 	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI160_OFFSET_EN_GYR98);
+	i2c_common_emul_set_read_fail_reg(emul, BMI160_OFFSET_EN_GYR98);
 	zassert_equal(-EIO, ms->drv->get_offset(ms, ret, &temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Disable rotation */
 	ms->rot_standard_ref = NULL;
@@ -261,21 +262,21 @@ static void test_bmi_acc_set_offset(void)
 	ms = &motion_sensors[BMI_ACC_SENSOR_ID];
 
 	/* Test fail on NV CONF register read and write */
-	bmi_emul_set_read_fail_reg(emul, BMI260_NV_CONF);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_NV_CONF);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
-	bmi_emul_set_write_fail_reg(emul, BMI260_NV_CONF);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, BMI260_NV_CONF);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test fail on offset write */
-	bmi_emul_set_write_fail_reg(emul, BMI160_OFFSET_ACC70);
+	i2c_common_emul_set_write_fail_reg(emul, BMI160_OFFSET_ACC70);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_write_fail_reg(emul, BMI160_OFFSET_ACC70 + 1);
+	i2c_common_emul_set_write_fail_reg(emul, BMI160_OFFSET_ACC70 + 1);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_write_fail_reg(emul, BMI160_OFFSET_ACC70 + 2);
+	i2c_common_emul_set_write_fail_reg(emul, BMI160_OFFSET_ACC70 + 2);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Setup NV_CONF register value */
 	bmi_emul_set_reg(emul, BMI260_NV_CONF, 0x7);
@@ -339,21 +340,21 @@ static void test_bmi_gyr_set_offset(void)
 	ms = &motion_sensors[BMI_GYR_SENSOR_ID];
 
 	/* Test fail on OFFSET EN GYR98 register read and write */
-	bmi_emul_set_read_fail_reg(emul, BMI260_OFFSET_EN_GYR98);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_OFFSET_EN_GYR98);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
-	bmi_emul_set_write_fail_reg(emul, BMI260_OFFSET_EN_GYR98);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, BMI260_OFFSET_EN_GYR98);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test fail on offset write */
-	bmi_emul_set_write_fail_reg(emul, BMI260_OFFSET_GYR70);
+	i2c_common_emul_set_write_fail_reg(emul, BMI260_OFFSET_GYR70);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_write_fail_reg(emul, BMI260_OFFSET_GYR70 + 1);
+	i2c_common_emul_set_write_fail_reg(emul, BMI260_OFFSET_GYR70 + 1);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_write_fail_reg(emul, BMI260_OFFSET_GYR70 + 2);
+	i2c_common_emul_set_write_fail_reg(emul, BMI260_OFFSET_GYR70 + 2);
 	zassert_equal(-EIO, ms->drv->set_offset(ms, input_v, temp), NULL);
-	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Set input offset */
 	exp_v[0] = BMI_EMUL_125_DEG_S / 100;
@@ -453,7 +454,7 @@ static void test_bmi_acc_set_range(void)
 	ms->current_range = start_range;
 	bmi_emul_set_reg(emul, BMI260_ACC_RANGE, BMI260_GSEL_2G);
 	/* Setup emulator fail on write */
-	bmi_emul_set_write_fail_reg(emul, BMI260_ACC_RANGE);
+	i2c_common_emul_set_write_fail_reg(emul, BMI260_ACC_RANGE);
 
 	/* Test fail on write */
 	zassert_equal(-EIO, ms->drv->set_range(ms, 12, 0), NULL);
@@ -466,7 +467,7 @@ static void test_bmi_acc_set_range(void)
 		      bmi_emul_get_reg(emul, BMI260_ACC_RANGE), NULL);
 
 	/* Do not fail on write */
-	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test setting range with rounding down */
 	check_set_acc_range(emul, ms, 1, 0, 2);
@@ -561,7 +562,7 @@ static void test_bmi_gyr_set_range(void)
 	ms->current_range = start_range;
 	bmi_emul_set_reg(emul, BMI260_GYR_RANGE, BMI260_DPS_SEL_250);
 	/* Setup emulator fail on write */
-	bmi_emul_set_write_fail_reg(emul, BMI260_GYR_RANGE);
+	i2c_common_emul_set_write_fail_reg(emul, BMI260_GYR_RANGE);
 
 	/* Test fail on write */
 	zassert_equal(-EIO, ms->drv->set_range(ms, 125, 0), NULL);
@@ -574,7 +575,7 @@ static void test_bmi_gyr_set_range(void)
 		      bmi_emul_get_reg(emul, BMI260_GYR_RANGE), NULL);
 
 	/* Do not fail on write */
-	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test setting range with rounding down */
 	check_set_gyr_range(emul, ms, 1, 0, 125);
@@ -768,7 +769,7 @@ static void test_bmi_acc_rate(void)
 	reg_rate = bmi_emul_get_reg(emul, BMI260_ACC_CONF);
 
 	/* Setup emulator fail on read */
-	bmi_emul_set_read_fail_reg(emul, BMI260_ACC_CONF);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_ACC_CONF);
 
 	/* Test fail on read */
 	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 50000, 0), NULL);
@@ -779,10 +780,10 @@ static void test_bmi_acc_rate(void)
 	zassert_equal(reg_rate, bmi_emul_get_reg(emul, BMI260_ACC_CONF), NULL);
 
 	/* Do not fail on read */
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Setup emulator fail on write */
-	bmi_emul_set_write_fail_reg(emul, BMI260_ACC_CONF);
+	i2c_common_emul_set_write_fail_reg(emul, BMI260_ACC_CONF);
 
 	/* Test fail on write */
 	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 50000, 0), NULL);
@@ -793,7 +794,7 @@ static void test_bmi_acc_rate(void)
 	zassert_equal(reg_rate, bmi_emul_get_reg(emul, BMI260_ACC_CONF), NULL);
 
 	/* Do not fail on write */
-	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test disabling sensor */
 	bmi_emul_set_reg(emul, BMI260_PWR_CTRL,
@@ -948,7 +949,7 @@ static void test_bmi_gyr_rate(void)
 	reg_rate = bmi_emul_get_reg(emul, BMI260_GYR_CONF);
 
 	/* Setup emulator fail on read */
-	bmi_emul_set_read_fail_reg(emul, BMI260_GYR_CONF);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_GYR_CONF);
 
 	/* Test fail on read */
 	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 50000, 0), NULL);
@@ -959,10 +960,10 @@ static void test_bmi_gyr_rate(void)
 	zassert_equal(reg_rate, bmi_emul_get_reg(emul, BMI260_GYR_CONF), NULL);
 
 	/* Do not fail on read */
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Setup emulator fail on write */
-	bmi_emul_set_write_fail_reg(emul, BMI260_GYR_CONF);
+	i2c_common_emul_set_write_fail_reg(emul, BMI260_GYR_CONF);
 
 	/* Test fail on write */
 	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 50000, 0), NULL);
@@ -973,7 +974,7 @@ static void test_bmi_gyr_rate(void)
 	zassert_equal(reg_rate, bmi_emul_get_reg(emul, BMI260_GYR_CONF), NULL);
 
 	/* Do not fail on write */
-	bmi_emul_set_write_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test disabling sensor */
 	bmi_emul_set_reg(emul, BMI260_PWR_CTRL,
@@ -1047,18 +1048,18 @@ static void test_bmi_read_temp(void)
 	ms_gyr = &motion_sensors[BMI_GYR_SENSOR_ID];
 
 	/* Setup emulator fail on read */
-	bmi_emul_set_read_fail_reg(emul, BMI260_TEMPERATURE_0);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_TEMPERATURE_0);
 	zassert_equal(EC_ERROR_NOT_POWERED,
 		      ms_acc->drv->read_temp(ms_acc, &ret_temp), NULL);
 	zassert_equal(EC_ERROR_NOT_POWERED,
 		      ms_gyr->drv->read_temp(ms_gyr, &ret_temp), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI260_TEMPERATURE_1);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_TEMPERATURE_1);
 	zassert_equal(EC_ERROR_NOT_POWERED,
 		      ms_acc->drv->read_temp(ms_acc, &ret_temp), NULL);
 	zassert_equal(EC_ERROR_NOT_POWERED,
 		      ms_gyr->drv->read_temp(ms_gyr, &ret_temp), NULL);
 	/* Do not fail on read */
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Fail on invalid temperature */
 	bmi_emul_set_reg(emul, BMI260_TEMPERATURE_0, 0x00);
@@ -1133,10 +1134,10 @@ static void test_bmi_acc_read(void)
 	bmi_emul_set_off(emul, BMI_EMUL_ACC_Z, 0);
 
 	/* Fail on read status */
-	bmi_emul_set_read_fail_reg(emul, BMI260_STATUS);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_STATUS);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
 
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* When not ready, driver should return saved raw value */
 	exp_v[0] = 100;
@@ -1204,20 +1205,20 @@ static void test_bmi_acc_read(void)
 	compare_int3v(exp_v, ret_v);
 
 	/* Fail on read of data registers */
-	bmi_emul_set_read_fail_reg(emul, BMI260_ACC_X_L_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_ACC_X_L_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI260_ACC_X_H_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_ACC_X_H_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI260_ACC_Y_L_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_ACC_Y_L_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI260_ACC_Y_H_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_ACC_Y_H_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI260_ACC_Z_L_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_ACC_Z_L_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI260_ACC_Z_H_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_ACC_Z_H_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
 
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 	ms->rot_standard_ref = NULL;
 }
 
@@ -1241,10 +1242,10 @@ static void test_bmi_gyr_read(void)
 	bmi_emul_set_off(emul, BMI_EMUL_GYR_Z, 0);
 
 	/* Fail on read status */
-	bmi_emul_set_read_fail_reg(emul, BMI260_STATUS);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_STATUS);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
 
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* When not ready, driver should return saved raw value */
 	exp_v[0] = 100;
@@ -1312,20 +1313,20 @@ static void test_bmi_gyr_read(void)
 	compare_int3v(exp_v, ret_v);
 
 	/* Fail on read of data registers */
-	bmi_emul_set_read_fail_reg(emul, BMI260_GYR_X_L_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_GYR_X_L_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI260_GYR_X_H_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_GYR_X_H_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI260_GYR_Y_L_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_GYR_Y_L_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI260_GYR_Y_H_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_GYR_Y_H_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI260_GYR_Z_L_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_GYR_Z_L_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI260_GYR_Z_H_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_GYR_Z_H_G);
 	zassert_equal(-EIO, ms->drv->read(ms, ret_v), NULL);
 
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 	ms->rot_standard_ref = NULL;
 }
 
@@ -1372,20 +1373,20 @@ static void test_bmi_acc_perform_calib(void)
 	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
 
 	/* Test fail on rate read */
-	bmi_emul_set_read_fail_reg(emul, BMI260_ACC_CONF);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_ACC_CONF);
 	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
 	zassert_equal(range, ms->current_range, NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
 
 	/* Test fail on status read */
-	bmi_emul_set_read_fail_reg(emul, BMI260_STATUS);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_STATUS);
 	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
 	zassert_equal(range, ms->current_range, NULL);
 	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
 
 	/* Test fail on data not ready */
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 	bmi_emul_set_reg(emul, BMI260_STATUS, 0);
 	zassert_equal(EC_ERROR_TIMEOUT, ms->drv->perform_calib(ms, 1), NULL);
 	zassert_equal(range, ms->current_range, NULL);
@@ -1395,18 +1396,18 @@ static void test_bmi_acc_perform_calib(void)
 	bmi_emul_set_reg(emul, BMI260_STATUS, BMI260_DRDY_ACC);
 
 	/* Test fail on data read */
-	bmi_emul_set_read_fail_reg(emul, BMI260_ACC_X_L_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_ACC_X_L_G);
 	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
 	zassert_equal(range, ms->current_range, NULL);
 	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
 
 	/* Test fail on setting offset */
-	bmi_emul_set_read_fail_reg(emul, BMI260_NV_CONF);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_NV_CONF);
 	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
 	zassert_equal(range, ms->current_range, NULL);
 	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
 
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test successful offset compenastion */
 	zassert_equal(EC_SUCCESS, ms->drv->perform_calib(ms, 1), NULL);
@@ -1463,20 +1464,20 @@ static void test_bmi_gyr_perform_calib(void)
 	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
 
 	/* Test fail on rate read */
-	bmi_emul_set_read_fail_reg(emul, BMI260_GYR_CONF);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_GYR_CONF);
 	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
 	zassert_equal(range, ms->current_range, NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
 
 	/* Test fail on status read */
-	bmi_emul_set_read_fail_reg(emul, BMI260_STATUS);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_STATUS);
 	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
 	zassert_equal(range, ms->current_range, NULL);
 	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
 
 	/* Test fail on data not ready */
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 	bmi_emul_set_reg(emul, BMI260_STATUS, 0);
 	zassert_equal(EC_ERROR_TIMEOUT, ms->drv->perform_calib(ms, 1), NULL);
 	zassert_equal(range, ms->current_range, NULL);
@@ -1490,18 +1491,18 @@ static void test_bmi_gyr_perform_calib(void)
 			 BMI260_DRDY_ACC | BMI260_DRDY_GYR);
 
 	/* Test fail on data read */
-	bmi_emul_set_read_fail_reg(emul, BMI260_GYR_X_L_G);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_GYR_X_L_G);
 	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
 	zassert_equal(range, ms->current_range, NULL);
 	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
 
 	/* Test fail on setting offset */
-	bmi_emul_set_read_fail_reg(emul, BMI260_OFFSET_EN_GYR98);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_OFFSET_EN_GYR98);
 	zassert_equal(-EIO, ms->drv->perform_calib(ms, 1), NULL);
 	zassert_equal(range, ms->current_range, NULL);
 	zassert_equal(rate, ms->drv->get_data_rate(ms), NULL);
 
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test successful offset compenastion */
 	zassert_equal(EC_SUCCESS, ms->drv->perform_calib(ms, 1), NULL);
@@ -1519,7 +1520,8 @@ static void test_bmi_gyr_perform_calib(void)
  * Custom emulatro read function which always return INIT OK status in
  * INTERNAL STATUS register. Used in init test.
  */
-static int emul_init_ok(struct i2c_emul *emul, int reg, int byte, void *data)
+static int emul_init_ok(struct i2c_emul *emul, int reg, uint8_t *val, int byte,
+			void *data)
 {
 	bmi_emul_set_reg(emul, BMI260_INTERNAL_STATUS, BMI260_INIT_OK);
 
@@ -1541,13 +1543,13 @@ static void test_bmi_init(void)
 	 * BMI260_INTERNAL_STATUS register, because init function triggers reset
 	 * which clears value set in this register before test.
 	 */
-	bmi_emul_set_read_func(emul, emul_init_ok, NULL);
+	i2c_common_emul_set_read_func(emul, emul_init_ok, NULL);
 	zassert_equal(EC_RES_SUCCESS, ms_acc->drv->init(ms_acc), NULL);
 
 	zassert_equal(EC_RES_SUCCESS, ms_gyr->drv->init(ms_gyr), NULL);
 
 	/* Remove custom emulator read function */
-	bmi_emul_set_read_func(emul, NULL, NULL);
+	i2c_common_emul_set_read_func(emul, NULL, NULL);
 }
 
 /** Data for custom emulator read function used in FIFO test */
@@ -1560,7 +1562,8 @@ struct fifo_func_data {
  * to value passed as additional data. It sets interrupt registers to 0 after
  * access.
  */
-static int emul_fifo_func(struct i2c_emul *emul, int reg, int byte, void *data)
+static int emul_fifo_func(struct i2c_emul *emul, int reg, uint8_t *val,
+			  int byte, void *data)
 {
 	struct fifo_func_data *d = data;
 
@@ -1695,11 +1698,11 @@ static void test_bmi_acc_fifo(void)
 	event = BMI_INT_EVENT;
 
 	/* Test fail to read interrupt status registers */
-	bmi_emul_set_read_fail_reg(emul, BMI260_INT_STATUS_0);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_INT_STATUS_0);
 	zassert_equal(-EIO, ms->drv->irq_handler(ms, &event), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI260_INT_STATUS_1);
+	i2c_common_emul_set_read_fail_reg(emul, BMI260_INT_STATUS_1);
 	zassert_equal(-EIO, ms->drv->irq_handler(ms, &event), NULL);
-	bmi_emul_set_read_fail_reg(emul, BMI_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test no interrupt */
 	bmi_emul_set_reg(emul, BMI260_INT_STATUS_0, 0);
@@ -1709,7 +1712,7 @@ static void test_bmi_acc_fifo(void)
 	check_fifo(ms, ms_gyr, NULL, acc_range, gyr_range);
 
 	/* Set custom function for FIFO test */
-	bmi_emul_set_read_func(emul, emul_fifo_func, &func_data);
+	i2c_common_emul_set_read_func(emul, emul_fifo_func, &func_data);
 	/* Enable sensor FIFO */
 	zassert_equal(EC_SUCCESS, ms->drv->set_data_rate(ms, 50000, 0), NULL);
 	/* Set range */
@@ -1794,7 +1797,7 @@ static void test_bmi_acc_fifo(void)
 	check_fifo(ms, ms_gyr, f, acc_range, gyr_range);
 
 	/* Remove custom emulator read function */
-	bmi_emul_set_read_func(emul, NULL, NULL);
+	i2c_common_emul_set_read_func(emul, NULL, NULL);
 }
 
 /** Test irq handler of gyroscope sensor */
diff --git a/zephyr/test/drivers/src/smart.c b/zephyr/test/drivers/src/smart.c
index 5f5c4f52fc..7c053f1c23 100644
--- a/zephyr/test/drivers/src/smart.c
+++ b/zephyr/test/drivers/src/smart.c
@@ -8,6 +8,7 @@
 
 #include "common.h"
 #include "i2c.h"
+#include "emul/emul_common_i2c.h"
 #include "emul/emul_smart_battery.h"
 
 #include "battery.h"
@@ -102,13 +103,6 @@ static void test_battery_status(void)
 	zassert_equal(expected, status, "%d != %d", expected, status);
 }
 
-/** Custom battery function which always fail */
-int fail_func(struct i2c_emul *emul, uint8_t *buf, int *len, int cmd,
-	      void *data)
-{
-	return -EINVAL;
-}
-
 /** Test wait for stable function */
 static void test_battery_wait_for_stable(void)
 {
@@ -117,11 +111,11 @@ static void test_battery_wait_for_stable(void)
 	emul = sbat_emul_get_ptr(BATTERY_ORD);
 
 	/* Should fail when read function always fail */
-	sbat_emul_set_custom_read_func(emul, fail_func, NULL);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_FAIL_ALL_REG);
 	zassert_equal(EC_ERROR_NOT_POWERED, battery_wait_for_stable(), NULL);
 
 	/* Should be ok with default handler */
-	sbat_emul_set_custom_read_func(emul, NULL, NULL);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 	zassert_equal(EC_SUCCESS, battery_wait_for_stable(), NULL);
 }
 
@@ -179,29 +173,10 @@ static void test_battery_time_at_rate(void)
 	zassert_equal(expect_time, minutes, "%d != %d", expect_time, minutes);
 }
 
-/** Parameter for fail_cmd_func */
-struct fail_cmd_data {
-	int cmd;
-};
-
-/** Custom battery function which fail on specific command */
-int fail_cmd_func(struct i2c_emul *emul, uint8_t *buf, int *len, int cmd,
-		  void *data)
-{
-	struct fail_cmd_data *p = data;
-
-	if (p->cmd == cmd)
-		return -EINVAL;
-
-	/* Use default handler */
-	return 1;
-}
-
 /** Test battery get params */
 static void test_battery_get_params(void)
 {
 	struct sbat_emul_bat_data *bat;
-	struct fail_cmd_data func_data;
 	struct batt_params batt;
 	struct i2c_emul *emul;
 	int flags;
@@ -213,84 +188,81 @@ static void test_battery_get_params(void)
 	bat->desired_charg_cur = 1000;
 	bat->desired_charg_volt = 5000;
 
-	/* Use function which allows to fail for specific command */
-	sbat_emul_set_custom_read_func(emul, fail_cmd_func, &func_data);
-
 	/* Fail temperature read */
-	func_data.cmd = SB_TEMPERATURE;
+	i2c_common_emul_set_read_fail_reg(emul, SB_TEMPERATURE);
 	flags = BATT_FLAG_WANT_CHARGE | BATT_FLAG_RESPONSIVE |
 		BATT_FLAG_BAD_TEMPERATURE;
 	battery_get_params(&batt);
 	zassert_equal(flags, batt.flags, "0x%x != 0x%x", flags, batt.flags);
 
 	/* Fail state of charge read; want charge cannot be set */
-	func_data.cmd = SB_RELATIVE_STATE_OF_CHARGE;
+	i2c_common_emul_set_read_fail_reg(emul, SB_RELATIVE_STATE_OF_CHARGE);
 	flags = BATT_FLAG_RESPONSIVE | BATT_FLAG_BAD_STATE_OF_CHARGE;
 	battery_get_params(&batt);
 	zassert_equal(flags, batt.flags, "0x%x != 0x%x", flags, batt.flags);
 
 	/* Fail voltage read */
-	func_data.cmd = SB_VOLTAGE;
+	i2c_common_emul_set_read_fail_reg(emul, SB_VOLTAGE);
 	flags = BATT_FLAG_WANT_CHARGE | BATT_FLAG_RESPONSIVE |
 		BATT_FLAG_BAD_VOLTAGE;
 	battery_get_params(&batt);
 	zassert_equal(flags, batt.flags, "0x%x != 0x%x", flags, batt.flags);
 
 	/* Fail current read */
-	func_data.cmd = SB_CURRENT;
+	i2c_common_emul_set_read_fail_reg(emul, SB_CURRENT);
 	flags = BATT_FLAG_WANT_CHARGE | BATT_FLAG_RESPONSIVE |
 		BATT_FLAG_BAD_CURRENT;
 	battery_get_params(&batt);
 	zassert_equal(flags, batt.flags, "0x%x != 0x%x", flags, batt.flags);
 
 	/* Fail average current read */
-	func_data.cmd = SB_AVERAGE_CURRENT;
+	i2c_common_emul_set_read_fail_reg(emul, SB_AVERAGE_CURRENT);
 	flags = BATT_FLAG_WANT_CHARGE | BATT_FLAG_RESPONSIVE |
 		BATT_FLAG_BAD_AVERAGE_CURRENT;
 	battery_get_params(&batt);
 	zassert_equal(flags, batt.flags, "0x%x != 0x%x", flags, batt.flags);
 
 	/* Fail charging voltage read; want charge cannot be set */
-	func_data.cmd = SB_CHARGING_VOLTAGE;
+	i2c_common_emul_set_read_fail_reg(emul, SB_CHARGING_VOLTAGE);
 	flags = BATT_FLAG_RESPONSIVE | BATT_FLAG_BAD_DESIRED_VOLTAGE;
 	battery_get_params(&batt);
 	zassert_equal(flags, batt.flags, "0x%x != 0x%x", flags, batt.flags);
 
 	/* Fail charging voltage read; want charge cannot be set */
-	func_data.cmd = SB_CHARGING_CURRENT;
+	i2c_common_emul_set_read_fail_reg(emul, SB_CHARGING_CURRENT);
 	flags = BATT_FLAG_RESPONSIVE | BATT_FLAG_BAD_DESIRED_CURRENT;
 	battery_get_params(&batt);
 	zassert_equal(flags, batt.flags, "0x%x != 0x%x", flags, batt.flags);
 
 	/* Fail remaining capacity read */
-	func_data.cmd = SB_REMAINING_CAPACITY;
+	i2c_common_emul_set_read_fail_reg(emul, SB_REMAINING_CAPACITY);
 	flags = BATT_FLAG_WANT_CHARGE | BATT_FLAG_RESPONSIVE |
 		BATT_FLAG_BAD_REMAINING_CAPACITY;
 	battery_get_params(&batt);
 	zassert_equal(flags, batt.flags, "0x%x != 0x%x", flags, batt.flags);
 
 	/* Fail full capacity read */
-	func_data.cmd = SB_FULL_CHARGE_CAPACITY;
+	i2c_common_emul_set_read_fail_reg(emul, SB_FULL_CHARGE_CAPACITY);
 	flags = BATT_FLAG_WANT_CHARGE | BATT_FLAG_RESPONSIVE |
 		BATT_FLAG_BAD_FULL_CAPACITY;
 	battery_get_params(&batt);
 	zassert_equal(flags, batt.flags, "0x%x != 0x%x", flags, batt.flags);
 
 	/* Fail status read */
-	func_data.cmd = SB_BATTERY_STATUS;
+	i2c_common_emul_set_read_fail_reg(emul, SB_BATTERY_STATUS);
 	flags = BATT_FLAG_WANT_CHARGE | BATT_FLAG_RESPONSIVE |
 		BATT_FLAG_BAD_STATUS;
 	battery_get_params(&batt);
 	zassert_equal(flags, batt.flags, "0x%x != 0x%x", flags, batt.flags);
 
 	/* Fail all */
-	sbat_emul_set_custom_read_func(emul, fail_func, NULL);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_FAIL_ALL_REG);
 	flags = BATT_FLAG_BAD_ANY;
 	battery_get_params(&batt);
 	zassert_equal(flags, batt.flags, "0x%x != 0x%x", flags, batt.flags);
 
 	/* Use default handler, everything should be ok */
-	sbat_emul_set_custom_read_func(emul, NULL, NULL);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 	flags = BATT_FLAG_WANT_CHARGE | BATT_FLAG_RESPONSIVE;
 	battery_get_params(&batt);
 	zassert_equal(flags, batt.flags, "0x%x != 0x%x", flags, batt.flags);
diff --git a/zephyr/test/drivers/src/tcs3400.c b/zephyr/test/drivers/src/tcs3400.c
index 8aaa3c2f38..c9f650f497 100644
--- a/zephyr/test/drivers/src/tcs3400.c
+++ b/zephyr/test/drivers/src/tcs3400.c
@@ -9,6 +9,7 @@
 #include "common.h"
 #include "i2c.h"
 #include "emul/emul_tcs3400.h"
+#include "emul/emul_common_i2c.h"
 
 #include "motion_sense.h"
 #include "motion_sense_fifo.h"
@@ -37,9 +38,9 @@ static void test_tcs_init(void)
 	zassert_equal(EC_SUCCESS, ms_rgb->drv->init(ms_rgb), NULL);
 
 	/* Fail init on communication errors */
-	tcs_emul_set_read_fail_reg(emul, TCS_EMUL_FAIL_ALL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_FAIL_ALL_REG);
 	zassert_equal(-EIO, ms->drv->init(ms), NULL);
-	tcs_emul_set_read_fail_reg(emul, TCS_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Fail on bad ID */
 	tcs_emul_set_reg(emul, TCS_I2C_ID, 0);
@@ -68,13 +69,13 @@ static void test_tcs_read(void)
 	ms = &motion_sensors[TCS_CLR_SENSOR_ID];
 
 	/* Test error on writing registers */
-	tcs_emul_set_write_fail_reg(emul, TCS_I2C_ATIME);
+	i2c_common_emul_set_write_fail_reg(emul, TCS_I2C_ATIME);
 	zassert_equal(-EIO, ms->drv->read(ms, v), NULL);
-	tcs_emul_set_write_fail_reg(emul, TCS_I2C_CONTROL);
+	i2c_common_emul_set_write_fail_reg(emul, TCS_I2C_CONTROL);
 	zassert_equal(-EIO, ms->drv->read(ms, v), NULL);
-	tcs_emul_set_write_fail_reg(emul, TCS_I2C_ENABLE);
+	i2c_common_emul_set_write_fail_reg(emul, TCS_I2C_ENABLE);
 	zassert_equal(-EIO, ms->drv->read(ms, v), NULL);
-	tcs_emul_set_write_fail_reg(emul, TCS_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test starting read with calibration */
 	tcs_emul_set_reg(emul, TCS_I2C_ATIME, 0);
@@ -152,15 +153,15 @@ static void test_tcs_irq_handler_fail(void)
 
 	event = TCS_INT_EVENT;
 	/* Test error on reading status */
-	tcs_emul_set_read_fail_reg(emul, TCS_I2C_STATUS);
+	i2c_common_emul_set_read_fail_reg(emul, TCS_I2C_STATUS);
 	zassert_equal(-EIO, ms->drv->irq_handler(ms, &event), NULL);
-	tcs_emul_set_read_fail_reg(emul, TCS_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 	check_fifo_empty(ms, ms_rgb);
 
 	/* Test fail on changing device power state */
-	tcs_emul_set_write_fail_reg(emul, TCS_I2C_ENABLE);
+	i2c_common_emul_set_write_fail_reg(emul, TCS_I2C_ENABLE);
 	zassert_equal(-EIO, ms->drv->irq_handler(ms, &event), NULL);
-	tcs_emul_set_write_fail_reg(emul, TCS_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_write_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 	check_fifo_empty(ms, ms_rgb);
 
 	/* Test that no data is committed when status is 0 */
@@ -536,12 +537,12 @@ static void test_tcs_data_rate(void)
 	ms_rgb = &motion_sensors[TCS_RGB_SENSOR_ID];
 
 	/* Test fail on reading device power state */
-	tcs_emul_set_read_fail_reg(emul, TCS_I2C_ENABLE);
+	i2c_common_emul_set_read_fail_reg(emul, TCS_I2C_ENABLE);
 	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 0, 0), NULL);
 	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 0, 1), NULL);
 	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 100, 0), NULL);
 	zassert_equal(-EIO, ms->drv->set_data_rate(ms, 100, 1), NULL);
-	tcs_emul_set_read_fail_reg(emul, TCS_EMUL_NO_FAIL_REG);
+	i2c_common_emul_set_read_fail_reg(emul, I2C_COMMON_EMUL_NO_FAIL_REG);
 
 	/* Test setting 0 rate disables device */
 	zassert_equal(EC_SUCCESS, ms->drv->set_data_rate(ms, 0, 0), NULL);