diff --git a/lib/bosch/BHy1_driver_and_MCU_solution/driver/src/bhy_support.c b/lib/bosch/BHy1_driver_and_MCU_solution/driver/src/bhy_support.c
index a92e254c5150d524be285d90f7470f3620f26523..f4b319246c3c46a3b825a8f871cc344964fb7906 100644
--- a/lib/bosch/BHy1_driver_and_MCU_solution/driver/src/bhy_support.c
+++ b/lib/bosch/BHy1_driver_and_MCU_solution/driver/src/bhy_support.c
@@ -59,6 +59,7 @@
 #include "i2c.h"
 #include "tmr_utils.h"
 #include <stdio.h>
+#include <string.h>
 
 /********************************************************************************/
 /*                                STATIC VARIABLES                              */
@@ -66,17 +67,22 @@
 static struct bhy_t bhy;
 static char *version = BHY_MCU_REFERENCE_VERSION;
 
+#define I2C_DEVICE	    MXC_I2C0_BUS0
 /********************************************************************************/
 /*                         EXTERN FUNCTION DECLARATIONS                         */
 /********************************************************************************/
 static int8_t sensor_i2c_write(uint8_t addr, uint8_t reg, uint8_t *p_buf, uint16_t size)
 {
-    return 0;
+    uint8_t buf[size + 1];
+    buf[0] = reg;
+    memcpy(buf + 1, p_buf, size);
+    return I2C_MasterWrite(I2C_DEVICE, addr << 1, buf, size + 1, 0);
 }
 
 static int8_t sensor_i2c_read(uint8_t addr, uint8_t reg, uint8_t *p_buf, uint16_t size)
 {
-    return 0;
+    I2C_MasterWrite(I2C_DEVICE, addr << 1, &reg, 1, 1);
+    return I2C_MasterRead(I2C_DEVICE, addr << 1, p_buf, size, 0);
 }
 
 /********************************************************************************/