diff --git a/driver/i2c_bb.c b/driver/i2c_bb.c
index b6bbe02e..a49f796b 100644
--- a/driver/i2c_bb.c
+++ b/driver/i2c_bb.c
@@ -35,8 +35,6 @@ static void i2c_start_cond(i2c_bb_state *s);
 static void i2c_stop_cond(i2c_bb_state *s);
 static void i2c_write_bit(i2c_bb_state *s, bool bit);
 static bool i2c_read_bit(i2c_bb_state *s);
-static bool i2c_write_byte(i2c_bb_state *s, bool send_start, bool send_stop, unsigned char byte);
-static unsigned char i2c_read_byte(i2c_bb_state *s, bool nack, bool send_stop);
 static bool clock_stretch_timeout(i2c_bb_state *s);
 static void i2c_delay(float seconds);
 
@@ -88,7 +86,7 @@ bool i2c_bb_tx_rx(i2c_bb_state *s, uint16_t addr, uint8_t *txbuf, size_t txbytes
 	chMtxLock(&s->mutex);
 
 	if (txbytes > 0 && txbuf) {
-		i2c_write_byte(s, true, false, addr << 1);
+		i2c_bb_write_byte(s, true, false, addr << 1);
 
 		if (s->has_error) {
 			chMtxUnlock(&s->mutex);
@@ -96,7 +94,7 @@ bool i2c_bb_tx_rx(i2c_bb_state *s, uint16_t addr, uint8_t *txbuf, size_t txbytes
 		}
 
 		for (unsigned int i = 0;i < txbytes;i++) {
-			i2c_write_byte(s, false, false, txbuf[i]);
+			i2c_bb_write_byte(s, false, false, txbuf[i]);
 
 			if (s->has_error) {
 				chMtxUnlock(&s->mutex);
@@ -106,7 +104,7 @@ bool i2c_bb_tx_rx(i2c_bb_state *s, uint16_t addr, uint8_t *txbuf, size_t txbytes
 	}
 
 	if (rxbytes > 0) {
-		i2c_write_byte(s, true, false, addr << 1 | 1);
+		i2c_bb_write_byte(s, true, false, addr << 1 | 1);
 
 		if (s->has_error) {
 			chMtxUnlock(&s->mutex);
@@ -114,7 +112,7 @@ bool i2c_bb_tx_rx(i2c_bb_state *s, uint16_t addr, uint8_t *txbuf, size_t txbytes
 		}
 
 		for (unsigned int i = 0;i < rxbytes;i++) {
-			rxbuf[i] = i2c_read_byte(s, i == (rxbytes - 1), false);
+			rxbuf[i] = i2c_bb_read_byte(s, i == (rxbytes - 1), false);
 			if (s->has_error) {
 				chMtxUnlock(&s->mutex);
 				return false;
@@ -129,6 +127,45 @@ bool i2c_bb_tx_rx(i2c_bb_state *s, uint16_t addr, uint8_t *txbuf, size_t txbytes
 	return !s->has_error;
 }
 
+bool i2c_bb_write_byte(i2c_bb_state *s, bool send_start, bool send_stop, unsigned char byte) {
+	unsigned bit;
+	bool nack;
+
+	if (send_start) {
+		i2c_start_cond(s);
+	}
+
+	for (bit = 0;bit < 8;bit++) {
+		i2c_write_bit(s, (byte & 0x80) != 0);
+		byte <<= 1;
+	}
+
+	nack = i2c_read_bit(s);
+
+	if (send_stop) {
+		i2c_stop_cond(s);
+	}
+
+	return nack;
+}
+
+unsigned char i2c_bb_read_byte(i2c_bb_state *s, bool nack, bool send_stop) {
+	unsigned char byte = 0;
+	unsigned char bit;
+
+	for (bit = 0;bit < 8;bit++) {
+		byte = (byte << 1) | i2c_read_bit(s);
+	}
+
+	i2c_write_bit(s, nack);
+
+	if (send_stop) {
+		i2c_stop_cond(s);
+	}
+
+	return byte;
+}
+
 static void i2c_start_cond(i2c_bb_state *s) {
 	if (s->has_started) {
 		// if started, do a restart condition
@@ -242,45 +279,6 @@ static bool i2c_read_bit(i2c_bb_state *s) {
 	return bit;
 }
 
-static bool i2c_write_byte(i2c_bb_state *s, bool send_start, bool send_stop, unsigned char byte) {
-	unsigned bit;
-	bool nack;
-
-	if (send_start) {
-		i2c_start_cond(s);
-	}
-
-	for (bit = 0;bit < 8;bit++) {
-		i2c_write_bit(s, (byte & 0x80) != 0);
-		byte <<= 1;
-	}
-
-	nack = i2c_read_bit(s);
-
-	if (send_stop) {
-		i2c_stop_cond(s);
-	}
-
-	return nack;
-}
-
-static unsigned char i2c_read_byte(i2c_bb_state *s, bool nack, bool send_stop) {
-	unsigned char byte = 0;
-	unsigned char bit;
-
-	for (bit = 0;bit < 8;bit++) {
-		byte = (byte << 1) | i2c_read_bit(s);
-	}
-
-	i2c_write_bit(s, nack);
-
-	if (send_stop) {
-		i2c_stop_cond(s);
-	}
-
-	return byte;
-}
-
 static bool clock_stretch_timeout(i2c_bb_state *s) {
 	uint32_t time_start = timer_time_now();
 
diff --git a/driver/i2c_bb.h b/driver/i2c_bb.h
index 5ab1f0e4..2825a0d1 100644
--- a/driver/i2c_bb.h
+++ b/driver/i2c_bb.h
@@ -44,5 +44,7 @@ typedef struct {
 void i2c_bb_init(i2c_bb_state *s);
 void i2c_bb_restore_bus(i2c_bb_state *s);
 bool i2c_bb_tx_rx(i2c_bb_state *s, uint16_t addr, uint8_t *txbuf, size_t txbytes, uint8_t *rxbuf, size_t rxbytes);
+bool i2c_bb_write_byte(i2c_bb_state *s, bool send_start, bool send_stop, unsigned char byte);
+unsigned char i2c_bb_read_byte(i2c_bb_state *s, bool nack, bool send_stop);
 
 #endif /* I2C_BB_H_ */
diff --git a/lispBM/README.md b/lispBM/README.md
index 8ce42f1c..f5018d3f 100644
--- a/lispBM/README.md
+++ b/lispBM/README.md
@@ -2975,6 +2975,20 @@ Sends a sequence of bits in an attempt to restore the i2c-bus. Can be used if an
 
 ---
 
+#### i2c-detect-addr
+
+| Platforms | Firmware |
+|---|---|
+| ESC, Express | 6.05+ |
+
+```clj
+(i2c-detect-addr address)
+```
+
+Test if address is present on the bus by writing to it and checking the nack-bit. Returns true if the address is present, nil otherwise. Can be used to detect if I2C-devices are plugged in and powered correctly.
+
+---
+
 #### imu-start-lsm6
 
 | Platforms | Firmware |
diff --git a/lispBM/lispif.c b/lispBM/lispif.c
index f7d67179..566b0092 100644
--- a/lispBM/lispif.c
+++ b/lispBM/lispif.c
@@ -41,7 +41,7 @@
 #define LISP_MEM_BITMAP_SIZE		LBM_MEMORY_BITMAP_SIZE_18K
 #define GC_STACK_SIZE				160
 #define PRINT_STACK_SIZE			128
-#define EXTENSION_STORAGE_SIZE		290
+#define EXTENSION_STORAGE_SIZE		295
 #define EXT_LOAD_CALLBACK_LEN		20
 #define PROF_DATA_NUM				30
 
diff --git a/lispBM/lispif_vesc_extensions.c b/lispBM/lispif_vesc_extensions.c
index 09bd4a31..d8d9218c 100644
--- a/lispBM/lispif_vesc_extensions.c
+++ b/lispBM/lispif_vesc_extensions.c
@@ -3059,13 +3059,17 @@ static lbm_value ext_i2c_start(lbm_value *args, lbm_uint argn) {
 	return ENC_SYM_TRUE;
 }
 
+static char *i2c_not_started_msg = "I2C not started";
+
 static lbm_value ext_i2c_tx_rx(lbm_value *args, lbm_uint argn) {
 	if (argn != 2 && argn != 3) {
-		return ENC_SYM_EERROR;
+		lbm_set_error_reason((char*)lbm_error_str_num_args);
+		return ENC_SYM_TERROR;
 	}
 
 	if (!i2c_started) {
-		return lbm_enc_i(0);
+		lbm_set_error_reason(i2c_not_started_msg);
+		return ENC_SYM_EERROR;
 	}
 
 	uint16_t addr = 0;
@@ -3073,39 +3077,40 @@ static lbm_value ext_i2c_tx_rx(lbm_value *args, lbm_uint argn) {
 	size_t rxlen = 0;
 	uint8_t *txbuf = 0;
 	uint8_t *rxbuf = 0;
-
-	const unsigned int max_len = 40;
-	uint8_t to_send[max_len];
+	bool is_arr = lbm_is_array_r(args[1]);
 
 	if (!lbm_is_number(args[0])) {
-		return ENC_SYM_EERROR;
+		return ENC_SYM_TERROR;
 	}
 	addr = lbm_dec_as_u32(args[0]);
 
-	if (lbm_is_array_r(args[1])) {
+	if (is_arr) {
 		lbm_array_header_t *array = (lbm_array_header_t *)lbm_car(args[1]);
 		txbuf = (uint8_t*)array->data;
 		txlen = array->size;
 	} else {
-		lbm_value curr = args[1];
-		while (lbm_is_cons(curr)) {
-			lbm_value  arg = lbm_car(curr);
-
-			if (lbm_is_number(arg)) {
-				to_send[txlen++] = lbm_dec_as_u32(arg);
-			} else {
-				return ENC_SYM_EERROR;
-			}
-
-			if (txlen == max_len) {
-				break;
-			}
-
-			curr = lbm_cdr(curr);
-		}
+		txlen = lbm_list_length(args[1]);
 
 		if (txlen > 0) {
-			txbuf = to_send;
+			txbuf = lbm_malloc(txlen);
+			if (!txbuf) {
+				return ENC_SYM_MERROR;
+			}
+
+			lbm_value curr = args[1];
+			int ind = 0;
+			while (lbm_is_cons(curr)) {
+				lbm_value  arg = lbm_car(curr);
+
+				if (lbm_is_number(arg)) {
+					txbuf[ind++] = lbm_dec_as_u32(arg);
+				} else {
+					lbm_free(txbuf);
+					return ENC_SYM_TERROR;
+				}
+
+				curr = lbm_cdr(curr);
+			}
 		}
 	}
 
@@ -3116,14 +3121,36 @@ static lbm_value ext_i2c_tx_rx(lbm_value *args, lbm_uint argn) {
 	}
 
 	i2c_cfg.has_error = false;
-	return lbm_enc_i(i2c_bb_tx_rx(&i2c_cfg, addr, txbuf, txlen, rxbuf, rxlen) ? 1 : 0);
+	bool res = i2c_bb_tx_rx(&i2c_cfg, addr, txbuf, txlen, rxbuf, rxlen);
+
+	if (!is_arr && txbuf) {
+		lbm_free(txbuf);
+	}
+
+	return lbm_enc_i(res ? 1 : 0);
+}
+
+static lbm_value ext_i2c_detect_addr(lbm_value *args, lbm_uint argn) {
+	LBM_CHECK_ARGN_NUMBER(1);
+
+	if (!i2c_started) {
+		lbm_set_error_reason(i2c_not_started_msg);
+		return ENC_SYM_EERROR;
+	}
+
+	uint8_t address = lbm_dec_as_u32(args[0]);
+
+	bool res = i2c_bb_write_byte(&i2c_cfg, true, true, address << 1);
+
+	return res ? ENC_SYM_NIL : ENC_SYM_TRUE;
 }
 
 static lbm_value ext_i2c_restore(lbm_value *args, lbm_uint argn) {
 	(void)args; (void)argn;
 
 	if (!i2c_started) {
-		return lbm_enc_i(0);
+		lbm_set_error_reason(i2c_not_started_msg);
+		return ENC_SYM_EERROR;
 	}
 
 	i2c_bb_restore_bus(&i2c_cfg);
@@ -5437,6 +5464,7 @@ void lispif_load_vesc_extensions(void) {
 	lbm_add_extension("i2c-start", ext_i2c_start);
 	lbm_add_extension("i2c-tx-rx", ext_i2c_tx_rx);
 	lbm_add_extension("i2c-restore", ext_i2c_restore);
+	lbm_add_extension("i2c-detect-addr", ext_i2c_detect_addr);
 
 	// GPIO
 	lbm_add_extension("gpio-configure", ext_gpio_configure);