diff --git a/ch32fun/ch32fun.h b/ch32fun/ch32fun.h
index 223b9305..0d63f271 100644
--- a/ch32fun/ch32fun.h
+++ b/ch32fun/ch32fun.h
@@ -875,6 +875,9 @@ extern "C" {
 #define DELAY_MS_TIME ((FUNCONF_SYSTEM_CORE_CLOCK)/8000)
 #endif
 
+#define DELAY_MSEC_COUNT(n) (DELAY_MS_TIME * n)
+#define DELAY_SEC_COUNT(n) (DELAY_MS_TIME * 1000 * n)
+
 #define Delay_Us(n) DelaySysTick( (n) * DELAY_US_TIME )
 #define Delay_Ms(n) DelaySysTick( (n) * DELAY_MS_TIME )
 
diff --git a/examples_x035/i2c_sensor_test/Makefile b/examples_x035/i2c_sensor_test/Makefile
new file mode 100644
index 00000000..25878641
--- /dev/null
+++ b/examples_x035/i2c_sensor_test/Makefile
@@ -0,0 +1,11 @@
+all : flash
+
+TARGET:=i2c_sensor_test
+TARGET_MCU:=CH32X033
+
+include ../../ch32fun/ch32fun.mk
+
+flash : cv_flash
+clean : cv_clean
+
+
diff --git a/examples_x035/i2c_sensor_test/README.md b/examples_x035/i2c_sensor_test/README.md
new file mode 100644
index 00000000..f7d53ce2
--- /dev/null
+++ b/examples_x035/i2c_sensor_test/README.md
@@ -0,0 +1,50 @@
+# I2C Master Communication Example
+
+This example demonstrates I2C communication with an I2C slave device.
+You can use this with (one or the other):
+- BH1750 light sensor (actual hardware)
+- Another CH32X035 running i2c_slave_test firmware
+
+if you are connecting a CH32X03X to a BH1750, the default slave address for the sensor is 0x23
+if you are connecting to another CH32X03X, the default slave address for the slave CH32X03X is 0x66
+For the CH32X03X slave, it needs to run the `i2c_slave_test` example
+
+Setup for BH1750 sensor:
+- Connect SCL to PA10 (I2C1 SCL)
+- Connect SDA to PA11 (I2C1 SDA)
+
+Setup for CH32X03X slave:
+- Connect both boards to the same I2C bus
+- Connect SCL (PA10) of both boards together
+- Connect SDA (PA11) of both boards together
+
+Expected output readings from CH32X03X slave:
+Read 1 byte (cmd 0x01): 0x11
+Read 2 bytes (cmd 0x13): 0x5D 0x66 
+Read 4 bytes (cmd 0x14): 0x77 0x88 0x99 0xAA 
+write buffer (cmd 0x31): successful
+Read buffer (cmd 0x30): 0xAA 0xBB 0xCC 0xFF 0xFF 
+
+0x3x Slave Command Set:
+0x01 - Minick BH1750 Power on command (returns 1 byte)
+0x23 - Minmick BH1750 Resolution command (returns 1 byte) 
+0x13 - Read 2 bytes from slave
+0x14 - Read 4 bytes from slave
+0x30 - Read from slave's writable buffer (32 bytes)
+0x31 - Write to slave's writable buffer (32 bytes)
+
+Command 0x30: Read from writable buffer
+Format: { 0x30, start_index }
+- start_index: Position to start reading from (0-31)
+Since buffer size is 32 bytes (0-31), reading from index 29:
+- Returns bytes 29, 30, 31 (3 valid bytes)
+- Remaining requested bytes return 0xFF (buffer boundary exceeded)
+
+Command 0x31: Write to writable buffer  
+Format: { 0x31, start_index, data0, data1, ... }
+- start_index: Position to start writing to (0-31)
+- dataX: Bytes to write (up to buffer boundary)
+Since buffer ends at index 31:
+- Writes 0xAA to index 29, 0xBB to index 30, 0xCC to index 31
+- 0xDD is not written (buffer full)
+- Returns success for written bytes only
diff --git a/examples_x035/i2c_sensor_test/funconfig.h b/examples_x035/i2c_sensor_test/funconfig.h
new file mode 100644
index 00000000..3500c105
--- /dev/null
+++ b/examples_x035/i2c_sensor_test/funconfig.h
@@ -0,0 +1,7 @@
+#ifndef _FUNCONFIG_H
+#define _FUNCONFIG_H
+
+#define FUNCONF_USE_DEBUGPRINTF 1
+
+#endif
+
diff --git a/examples_x035/i2c_sensor_test/i2c_sensor_test.c b/examples_x035/i2c_sensor_test/i2c_sensor_test.c
new file mode 100644
index 00000000..6cb3fa2d
--- /dev/null
+++ b/examples_x035/i2c_sensor_test/i2c_sensor_test.c
@@ -0,0 +1,143 @@
+#define FUNCONF_SYSTICK_USE_HCLK 1
+
+#include "ch32fun.h"
+#include <stdio.h>
+#include "lib_i2c.h"
+
+#define I2C_TARGET I2C1
+#define SYSTEM_CLOCK_HZ 48000000
+
+// #define I2C_ADDRESS 0x23		// use this for BH1750
+#define I2C_ADDRESS 0x66		// use this for i2c_slave_test
+
+u8 read_state = 0;
+
+void print_ch32_readings(I2C_TypeDef* I2Cx, u8 i2_addr) {
+	u8 rx_buf[8];
+	u8 read, err;
+
+	switch (read_state) {
+		case 0:
+			// read command 0x01: return 1 byte
+			err = i2c_readReg_buffer(I2Cx, i2_addr, 0x01, rx_buf, 1);
+			if (!err) {
+				printf("\nRead 1 byte (cmd 0x01): 0x%02X", rx_buf[0]);
+			} else {
+				printf("\nError 0x%02X", err);
+			}
+			break;
+
+		case 1:
+			// read command 0x10: return 2 bytes
+			err = i2c_readReg_buffer(I2Cx, i2_addr, 0x13, rx_buf, 2);
+			if (!err) {
+				printf("\nRead 2 bytes (cmd 0x13): ");
+				for (int i = 0; i < 2; i++) {
+					printf("0x%02X ", rx_buf[i]);
+				}
+			} else {
+				printf("\nError 0x%02X", err);
+			}
+
+			break;
+
+		case 2:
+			// read command 0x11: return 4 bytes
+			err = i2c_readReg_buffer(I2Cx, i2_addr, 0x14, rx_buf, 4);
+
+			if (!err) {
+				printf("\nRead 4 bytes (cmd 0x14): ");
+				for (int i = 0; i < 4; i++) {
+					printf("0x%02X ", rx_buf[i]);
+				}
+			} else {
+				printf("\nError 0x%02X", err);
+			}
+			break;
+
+		case 3:
+			{
+				// write command 0x31. write buffer
+				u8 write_request[] = { 0x31, 29, 0xAA, 0xBB, 0xCC, 0xDD };
+				err = i2c_sendBytes(I2Cx, i2_addr, &write_request, sizeof(write_request));
+
+				if (!err) {
+					printf("\nwrite buffer (cmd 0x%02X): successful", 0x31);
+				}
+				if (err) {
+					printf("\nError 0x%02X", err);
+				}
+				break;
+			}
+
+		case 4:
+			{
+				// read command 0x30: read buffer
+				u8 read_request[] = { 0x30, 29 };
+				err = i2c_readRegTx_buffer(I2Cx, i2_addr, &read_request, sizeof(read_request), &rx_buf, 5);
+
+				if (!err) {
+					printf("\nRead buffer (cmd 0x30): ");
+					for (int i = 0; i < 5; i++) {
+						printf("0x%02X ", rx_buf[i]);
+					}
+				} else {
+					printf("\nError 0x%02X", err);
+				}
+				printf("\n");
+				break;
+			}
+
+		default:
+			break;
+	}
+
+	read_state++;
+	if (read_state > 4) read_state = 0;
+}
+
+u16 get_bh1750_readings(I2C_TypeDef* I2Cx, u8 i2cAddress) {
+	u8 data[2];
+	i2c_readReg_buffer(I2Cx, i2cAddress, 0x13, data, 2);	// get Reading
+	u16 raw = (data[0] << 8) | data[1];
+	return raw * 12 / 10;  // Convert to lux
+}
+
+void onHandle_ping(u8 i2cAddress) {
+	printf("Found device: 0x%02X\n", i2cAddress);
+}
+
+int main() {
+	SystemInit();
+	funGpioInitAll(); // Enable GPIOs
+
+	printf("\n~ I2C sensors Example ~\n");
+	printf("Chip ID: %08lX\n", ESIG->UID0);
+	printf("Chip Capacity: %d KB\n", ESIG->CAP);
+
+	funPinMode(PA10, GPIO_CFGLR_OUT_50Mhz_AF_PP);	// I2C1 SCL
+	funPinMode(PA11, GPIO_CFGLR_OUT_50Mhz_AF_PP);  // I2C1 SDA
+
+	i2c_init(I2C_TARGET, FUNCONF_SYSTEM_CORE_CLOCK, 100000);
+	printf("Scanning I2C Bus...\n");
+	i2c_scan(I2C_TARGET, onHandle_ping);
+	printf("\nDone.\n");
+
+	i2c_sendByte(I2C_TARGET, I2C_ADDRESS, 0x01); // Power on
+	i2c_sendByte(I2C_TARGET, I2C_ADDRESS, 0x23); // resolution
+
+	u32 time_ref = 0;
+
+	while(1) {
+		if (TimeElapsed32(SysTick->CNT, time_ref) > DELAY_MSEC_COUNT(200)) {
+			time_ref = SysTick->CNT;
+
+			#if I2C_ADDRESS == 0x23
+				u16 lux = get_bh1750_readings(I2C_TARGET, I2C_ADDRESS);
+				printf("BH1750 Reading: %d lx\n", lux);
+			#else
+				print_ch32_readings(I2C_TARGET, I2C_ADDRESS);
+			#endif
+		}
+	}
+}
diff --git a/examples_x035/i2c_slave_test/Makefile b/examples_x035/i2c_slave_test/Makefile
new file mode 100644
index 00000000..c0f9a08d
--- /dev/null
+++ b/examples_x035/i2c_slave_test/Makefile
@@ -0,0 +1,11 @@
+all : flash
+
+TARGET:=i2c_slave_test
+TARGET_MCU:=CH32X033
+
+include ../../ch32fun/ch32fun.mk
+
+flash : cv_flash
+clean : cv_clean
+
+
diff --git a/examples_x035/i2c_slave_test/funconfig.h b/examples_x035/i2c_slave_test/funconfig.h
new file mode 100644
index 00000000..3500c105
--- /dev/null
+++ b/examples_x035/i2c_slave_test/funconfig.h
@@ -0,0 +1,7 @@
+#ifndef _FUNCONFIG_H
+#define _FUNCONFIG_H
+
+#define FUNCONF_USE_DEBUGPRINTF 1
+
+#endif
+
diff --git a/examples_x035/i2c_slave_test/i2c_slave_test.c b/examples_x035/i2c_slave_test/i2c_slave_test.c
new file mode 100644
index 00000000..9a25d2b3
--- /dev/null
+++ b/examples_x035/i2c_slave_test/i2c_slave_test.c
@@ -0,0 +1,213 @@
+/**
+ * I2C Slave Communication Example
+ * You can use this with the i2c_sensor_test example
+ * 
+ * 0x3x Slave Command Set:
+ * 0x01 - Minick BH1750 Power on command (returns 1 byte)
+ * 0x23 - Minmick BH1750 Resolution command (returns 1 byte) 
+ * 0x13 - Read 2 bytes from slave
+ * 0x14 - Read 4 bytes from slave
+ * 0x30 - Read from slave's writable buffer (32 bytes)
+ * 0x31 - Write to slave's writable buffer (32 bytes)
+ * 
+ * Command 0x30: Read from writable buffer
+ * Format: { 0x30, start_index }
+ * - start_index: Position to start reading from (0-31)
+ * Since buffer size is 32 bytes (0-31), reading from index 29:
+ * - Returns bytes 29, 30, 31 (3 valid bytes)
+ * - Remaining requested bytes return 0xFF (buffer boundary exceeded)
+ * 
+ * Command 0x31: Write to writable buffer  
+ * Format: { 0x31, start_index, data0, data1, ... }
+ * - start_index: Position to start writing to (0-31)
+ * - dataX: Bytes to write (up to buffer boundary)
+ * Since buffer ends at index 31:
+ * - Writes 0xAA to index 29, 0xBB to index 30, 0xCC to index 31
+ * - 0xDD is not written (buffer full)
+ * - Returns success for written bytes only
+ */
+
+
+#include "ch32fun.h"
+#include <stdio.h>
+#include "lib_i2c.h"
+
+#define SYSTEM_CLOCK_HZ 48000000
+#define I2C_SELF_ADDR 0x66
+
+#define I2C_SLAVE_READ_BYTE_CMD1 0x01
+#define I2C_SLAVE_READ_BYTE_CMD2 0x23
+#define I2C_SLAVE_READ_2BYTES_CMD 0x13
+#define I2C_SLAVE_READ_4BYTES_CMD 0x14
+#define I2C_SLAVE_READ_CMD 0x30
+#define I2C_SLAVE_WRITE_CMD 0x31
+
+#define I2C_WRITABLE_SIZE 32
+volatile u8 writable_buffer[I2C_WRITABLE_SIZE] = { 0 };
+volatile u8 readonly_buffer[] = { 0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xAA };
+
+int main() {
+	SystemInit();
+	funGpioInitAll(); // Enable GPIOs
+	
+	printf("\n~ I2C slave Example ~\n");
+	printf("Chip ID: %08lX\n", ESIG->UID0);
+	printf("Chip Capacity: %d KB\n", ESIG->CAP);
+
+	funPinMode(PA10, GPIO_CFGLR_OUT_50Mhz_AF_PP);	// I2C1 SCL
+	funPinMode(PA11, GPIO_CFGLR_OUT_50Mhz_AF_PP);  // I2C1 SDA
+
+	// Initialize I2C as slave
+	i2c_slave_init(I2C1, I2C_SELF_ADDR, SYSTEM_CLOCK_HZ, 100000);
+	printf("CTLR1: 0x%04X\n", I2C1->CTLR1);	
+	printf("CTLR2: 0x%04X\n", I2C1->CTLR2);	
+	printf("OADDR1: 0x%04X\n", I2C1->OADDR1);
+	printf("CKCFGR: 0x%04X\n", I2C1->CKCFGR);
+
+	// Update sensor data periodically
+	while(1) {
+		readonly_buffer[5] = readonly_buffer[5] + 1;  
+		Delay_Ms(1000);
+	}
+}
+
+
+volatile u8 SLAVE_CMD = 0;
+volatile u8 slave_tx_idx = 0;			// to be transmitted
+volatile u8 slave_rx_idx = 0;			// to be received
+volatile u8 slave_temp_value = 0xFF;	// 0xFF = invalid
+
+// Not in master/slave mode
+// if(!(I2C1->STAR2 & I2C_STAR2_MSL)) { return 0; }
+u8 i2c_isTransmitter() { return I2C1->STAR2 & I2C_STAR2_TRA; }
+
+void I2C1_EV_IRQHandler(void) __attribute__((interrupt));
+void I2C1_EV_IRQHandler(void) {
+	// printf("\nSLAVE ISR: STAR1=0x%04X\n", I2C1->STAR1);
+	
+	//# Master request address, slave address match
+	if(I2C1->STAR1 & I2C_STAR1_ADDR) {
+		printf("\n***SLAVE ADDR MATCH\n");
+		(void)I2C1->STAR1;
+		(void)I2C1->STAR2;
+
+		if (!i2c_isTransmitter()) {
+			// WRITE mode - reset for new command
+			SLAVE_CMD = 0;
+			slave_rx_idx = 0;
+			slave_tx_idx = 0;  // Also reset tx index for new command
+		}
+	}
+	
+	//# Master sends data, slave is ready to receive
+	if(I2C1->STAR1 & I2C_STAR1_RXNE) {
+		u8 received_byte = I2C1->DATAR;
+		printf("Receive [%d]: 0x%02X, cmd: 0x%02X", slave_rx_idx, received_byte, SLAVE_CMD);
+
+		if (SLAVE_CMD == 0) {
+			SLAVE_CMD = received_byte;
+			printf(" | SLAVE_CMD: 0x%02X", SLAVE_CMD);
+		}
+		else {
+			switch (SLAVE_CMD) {
+				case I2C_SLAVE_READ_CMD:
+					// set slave_tx_index to prepare for transfer
+					printf(" | slave_tx_index: %d", received_byte);
+					slave_tx_idx = received_byte;
+					break;
+
+				case I2C_SLAVE_WRITE_CMD:
+					// save received data
+					if (slave_temp_value == 0xFF) {
+						printf(" | slave_temp_value: 0x%02X", received_byte);
+						// slave_temp_value holds the starting index of the transmitting data
+						slave_temp_value = received_byte;
+					}
+					else {
+						u8 target_index = slave_rx_idx + slave_temp_value;
+
+						if (target_index < I2C_WRITABLE_SIZE) {
+							printf(" | saving 0x%02X", received_byte);
+							writable_buffer[target_index] = received_byte;
+							slave_rx_idx++;
+						} else {
+							// buffer is exceeded, reset SLAVE_CMD
+							SLAVE_CMD = 0;
+						}
+					}
+
+					break;
+				default:
+					SLAVE_CMD = 0;
+					break;
+			}
+		}
+
+		printf("\n");
+	}
+	
+	//# Master requests data, slave is ready to send
+	if(I2C1->STAR1 & I2C_STAR1_TXE && i2c_isTransmitter()) {
+		// printf("Prepared to Transmit: cmd 0x%02X, tx_idx: %d\n", SLAVE_CMD, slave_tx_idx);
+
+		switch(SLAVE_CMD) {
+			// return 1 byte
+			case I2C_SLAVE_READ_BYTE_CMD1: case I2C_SLAVE_READ_BYTE_CMD2:
+				I2C1->DATAR = readonly_buffer[1];
+				break;
+
+			// return 2 bytes
+			case I2C_SLAVE_READ_2BYTES_CMD:
+				if (slave_tx_idx < 2) {  // Hardcoded length
+					I2C1->DATAR = readonly_buffer[5 + slave_tx_idx++];
+				} else {
+					I2C1->DATAR = 0xFF;
+				}
+				break;
+
+			// return 4 bytes
+			case I2C_SLAVE_READ_4BYTES_CMD:
+				if (slave_tx_idx < 4) {  // Hardcoded length
+					I2C1->DATAR = readonly_buffer[7 + slave_tx_idx++];
+				} else {
+					I2C1->DATAR = 0xFF;
+				}
+				break;
+
+			// return writable buffer
+			case I2C_SLAVE_READ_CMD:
+				if (slave_tx_idx < I2C_WRITABLE_SIZE) {
+					// printf("\n return writable buffer %d: \n", slave_tx_idx);
+					// for (int i = 0; i < I2C_WRITABLE_SIZE; i++) {
+					// 	printf("0x%02X ", writable_buffer[i]);
+					// }
+					// printf("\n");
+
+					u8 data = writable_buffer[slave_tx_idx++];
+					I2C1->DATAR = data;
+					printf(" | transmitting 0x%02X\n", data);
+				} else {
+					printf(" | transmitting 0xFF\n");
+					I2C1->DATAR = 0xFF;
+				}
+				break;
+			default:
+				I2C1->DATAR = 0xFF;
+				break;
+		}
+	}
+
+	//# Master sends stop
+	if(I2C1->STAR1 & I2C_STAR1_STOPF) {
+		// Clear STOPF flag by reading SR1 and writing CR1
+		(void)I2C1->STAR1;
+		I2C1->CTLR1 |= 0;
+		printf("\n***STOP DETECTED\n");
+
+		// Reset state
+		SLAVE_CMD = 0;
+		slave_rx_idx = 0;
+		slave_tx_idx = 0;
+		slave_temp_value = 0xFF;
+	}
+}
\ No newline at end of file
diff --git a/extralibs/lib_i2c.h b/extralibs/lib_i2c.h
new file mode 100644
index 00000000..f4b52bfa
--- /dev/null
+++ b/extralibs/lib_i2c.h
@@ -0,0 +1,227 @@
+// MIT License
+// Copyright (c) 2025 UniTheCat
+// Tested with Ch32X03x and CH32V30x
+
+#define I2C_TIMEOUT_DEFAULT 100000
+
+//! ####################################
+//! I2C INIT FUNCTIONS
+//! ####################################
+
+void i2c_init(I2C_TypeDef* I2Cx, u32 PCLK, u32 i2cSpeed_Hz) {
+	// Enable I2C clock
+	if (I2Cx == I2C1) {
+		RCC->APB1PCENR |= RCC_APB1Periph_I2C1;
+	}
+	#ifdef I2C2
+	else if (I2Cx == I2C2) {
+		RCC->APB1PCENR |= RCC_APB1Periph_I2C2;
+	}
+	#endif
+
+	// Disable I2C before configuration
+	I2Cx->CTLR1 &= ~I2C_CTLR1_PE;
+
+	// configure I2C clock
+	I2Cx->CTLR2 = (PCLK / 1000000);
+	I2Cx->CKCFGR = PCLK / (i2cSpeed_Hz << 1);	// PeripheralClock / (100KHz * 2)
+
+	// Enable I2C
+	I2Cx->CTLR1 |= I2C_CTLR1_PE;
+
+	// Enable ACK
+	I2Cx->CTLR1 |= I2C_CTLR1_ACK;
+}
+
+u8 i2c_start(I2C_TypeDef* I2Cx, u8 i2cAddress, u8 isRead) {
+	//# Wait while BUSY, when BUSY is set to 0 then continue
+	u32 timeout = I2C_TIMEOUT_DEFAULT;
+	while((I2Cx->STAR2 & I2C_STAR2_BUSY) && --timeout);
+	// if (timeout == 0) { I2Cx->CTLR1 |= I2C_CTLR1_STOP; return 0x11; }
+
+	//# Generate START condition
+	I2Cx->CTLR1 |= I2C_CTLR1_START;
+
+	//# Wait while SB is 0, when SB is set to 1 then continue
+	timeout = I2C_TIMEOUT_DEFAULT;
+	while(!(I2Cx->STAR1 & I2C_STAR1_SB) && --timeout);
+	if (timeout == 0) { I2Cx->CTLR1 |= I2C_CTLR1_STOP; return 0x12; }
+	// printf("timeoutB: %d\n", I2C_TIMEOUT_DEFAULT - timeout);
+
+	//# Send address + read/write. Write = 0, Read = 1
+	I2Cx->DATAR = (i2cAddress << 1) | isRead;
+
+	//# Wait while ADDR is 0, if ADDR is set to 1 then continue
+	timeout = I2C_TIMEOUT_DEFAULT;
+	while(!(I2Cx->STAR1 & I2C_STAR1_ADDR) && --timeout);
+	if (timeout == 0) { I2Cx->CTLR1 |= I2C_CTLR1_STOP; return 0x13; }
+	// printf("timeoutC: %d\n", I2C_TIMEOUT_DEFAULT - timeout);
+
+	//! REQUIRED. Clear ADDR by reading STAR1 then STAR2
+	(void)I2Cx->STAR1;
+	(void)I2Cx->STAR2;
+	return 0;
+}
+
+void i2c_scan(I2C_TypeDef* I2Cx, void (*onPingFound)(u8 address)) {
+	// mininum 0x08 to 0x77 (0b1110111)
+	for (int i = 0x08; i < 0x77; i++) {
+		u8 ping = i2c_start(I2Cx, i, 1);
+		
+		//# Generate STOP condition
+		I2Cx->CTLR1 |= I2C_CTLR1_STOP;
+		if (ping == 0) onPingFound(i);
+	}
+}
+
+//! ####################################
+//! I2C SEND FUNCTION
+//! ####################################
+
+u8 i2c_sendBytes_noStop(I2C_TypeDef* I2Cx, u8 i2cAddress, u8* buffer, u8 len) {
+	u8 err = i2c_start(I2Cx, i2cAddress, 0); // Write mode
+	if (err) return err;
+	u32 timeout;
+
+	for(u8 i = 0; i < len; i++) {
+		//# Wait for register empty
+		timeout = I2C_TIMEOUT_DEFAULT;
+		while(!(I2Cx->STAR1 & I2C_STAR1_TXE) && --timeout);
+		if (timeout == 0) { I2Cx->CTLR1 |= I2C_CTLR1_STOP; return 0x21; }
+		I2Cx->DATAR = buffer[i];		// Send data
+	}
+
+	//# Wait for transmission complete. Wait while BTF is 0, when set to 1 continue
+	timeout = I2C_TIMEOUT_DEFAULT;
+	while(!(I2Cx->STAR1 & I2C_STAR1_BTF) && --timeout);
+	if (timeout == 0) { I2Cx->CTLR1 |= I2C_CTLR1_STOP; return 0x22; }
+	
+	return 0;
+}
+
+u8 i2c_sendBytes(I2C_TypeDef* I2Cx, u8 i2cAddress, u8* buffer, u8 len) {
+	u8 err = i2c_sendBytes_noStop(I2Cx, i2cAddress, buffer, len);
+	//# Generate STOP condition
+	I2Cx->CTLR1 |= I2C_CTLR1_STOP;
+	return err;
+}
+
+u8 i2c_sendByte(I2C_TypeDef* I2Cx, u8 i2cAddress, u8 data) {
+	return i2c_sendBytes(I2Cx, i2cAddress, &data, 1);
+}
+
+
+//! ####################################
+//! I2C RECEIVE FUNCTIONS
+//! ####################################
+
+u8 i2c_readBytes(I2C_TypeDef* I2Cx, u8 i2cAddress, u8* buffer, u8 len) {
+	u8 err = i2c_start(I2Cx, i2cAddress, 1); // Read mode
+	if (err) return err;
+
+	//# Enable ACK at the beginning
+	I2Cx->CTLR1 |= I2C_CTLR1_ACK;
+
+	for(u8 i = 0; i < len; i++) {
+		//# Before reading the last bytes, disable ACK to signal the slave to stop sending
+		if(i == len-1) I2Cx->CTLR1 &= ~I2C_CTLR1_ACK;
+		
+		//# Wait for data. Wait while RxNE is 0, when set to 1 continue
+		u32 timeout = I2C_TIMEOUT_DEFAULT;
+		while(!(I2Cx->STAR1 & I2C_STAR1_RXNE) && --timeout);
+		if (timeout == 0) { I2Cx->CTLR1 |= I2C_CTLR1_STOP; return 0x31; }
+
+		//# Read data
+		buffer[i] = I2Cx->DATAR;
+	}
+	
+	//# Generate STOP condition
+	I2Cx->CTLR1 |= I2C_CTLR1_STOP;
+	return 0;	
+}
+
+// Write to register and then do read data, no stop inbetween
+u8 i2c_readRegTx_buffer(I2C_TypeDef* I2Cx, u8 i2cAddress,
+						u8 *tx_buf, u8 tx_len, u8 *rx_buf, u8 rx_len
+) {
+	u8 err = i2c_sendBytes_noStop(I2Cx, i2cAddress, tx_buf, tx_len);	// Send register address
+	if (err) return err;
+	err = i2c_readBytes(I2Cx, i2cAddress, rx_buf, rx_len); 	// Read data
+	return err;
+}
+
+u8 i2c_readReg_buffer(I2C_TypeDef* I2Cx, u8 i2cAddress, u8 reg, u8 *rx_buf, u8 rx_len) {
+	return i2c_readRegTx_buffer(I2Cx, i2cAddress, &reg, 1, rx_buf, rx_len);
+}
+
+//! ####################################
+//! I2C SLAVE FUNCTIONS
+//! ####################################
+
+void i2c_slave_init(I2C_TypeDef* I2Cx, u16 self_addr, u32 PCLK, u32 i2cSpeed_Hz) {
+	i2c_init(I2Cx, PCLK, i2cSpeed_Hz);
+
+	// Configure the CH32 I2C slave address to make it an I2C slave
+	I2Cx->OADDR1  = (self_addr << 1);
+	I2Cx->OADDR2 = 0;
+
+	I2Cx->CTLR2 |= I2C_CTLR2_ITEVTEN | I2C_CTLR2_ITERREN | I2C_CTLR2_ITBUFEN;
+
+	// Enable Event and Error Interrupts
+	if (I2Cx == I2C1) {
+		NVIC_EnableIRQ(I2C1_EV_IRQn); // I2C Event interrupt
+		NVIC_EnableIRQ(I2C1_ER_IRQn); // I2C Error interrupt
+	}
+
+	#ifdef I2C2
+		else if (I2Cx == I2C2) {
+			NVIC_EnableIRQ(I2C2_EV_IRQn); // I2C Event interrupt
+			NVIC_EnableIRQ(I2C2_ER_IRQn); // I2C Error interrupt
+		}
+	#endif
+}
+
+void I2C1_ER_IRQHandler(void) __attribute__((interrupt));
+void I2C1_ER_IRQHandler(void) {
+	// get I2C status
+	uint16_t STAR1 = I2C1->STAR1;
+
+	// Obtain and clear Bus error
+	if (STAR1 & I2C_STAR1_BERR) {
+		I2C1->STAR1 &= ~(I2C_STAR1_BERR);
+	}
+
+	// Obtain and clear Arbitration lost error
+	if (STAR1 & I2C_STAR1_ARLO) {
+		I2C1->STAR1 &= ~(I2C_STAR1_ARLO);
+	}
+
+	// Obtain and clear Acknowledge failure error
+	if (STAR1 & I2C_STAR1_AF) {
+		I2C1->STAR1 &= ~(I2C_STAR1_AF);
+	}
+}
+
+
+#ifdef I2C2
+	void I2C2_ER_IRQHandler(void) __attribute__((interrupt));
+	void I2C2_ER_IRQHandler(void) {
+		// get I2C status
+		uint16_t STAR1 = I2C2->STAR1;
+
+		// Obtain and clear Bus error
+		if (STAR1 & I2C_STAR1_BERR) {
+			I2C2->STAR1 &= ~(I2C_STAR1_BERR);
+		}
+
+		// Obtain and clear Arbitration lost error
+		if (STAR1 & I2C_STAR1_ARLO) {
+			I2C2->STAR1 &= ~(I2C_STAR1_ARLO);
+		}
+
+		// Obtain and clear Acknowledge failure error
+		if (STAR1 & I2C_STAR1_AF) {
+			I2C2->STAR1 &= ~(I2C_STAR1_AF);
+		}
+	}
+#endif
\ No newline at end of file