diff --git a/Inc/i2c.h b/Inc/i2c.h
index 1e09da2..a11938b 100644
--- a/Inc/i2c.h
+++ b/Inc/i2c.h
@@ -93,7 +93,7 @@ void i2c_scan(void);
 /*!
     \brief      write 16-bit data to I2C device
     \param[in]  slave_addr: 7-bit slave address
-    \param[in]  reg_addr: register address
+    \param[in]  reg_addr: register address (16-bit, high byte first)
     \param[in]  data: pointer to 2-byte data array
     \param[out] none
     \retval     i2c_result_t
@@ -119,17 +119,17 @@ i2c_result_t i2c_read_16bits(uint8_t slave_addr, uint8_t reg_addr, uint8_t *data
     \param[out] none
     \retval     i2c_result_t: operation result
 */
-i2c_result_t i2c_write(uint8_t slave_addr, uint8_t reg_addr, uint8_t *data, uint8_t length);
+i2c_result_t i2c_write(uint8_t slave_addr, uint16_t reg_addr, uint8_t *data, uint8_t length);
 
 /*!
     \brief      read data from I2C device with configurable length
     \param[in]  slave_addr: slave device address (7-bit)
-    \param[in]  reg_addr: register address
+    \param[in]  reg_addr: register address (16-bit, high byte first)
     \param[out] data: pointer to data buffer
     \param[in]  length: number of bytes to read (1-255)
     \retval     i2c_result_t: operation result
 */
-i2c_result_t i2c_read(uint8_t slave_addr, uint8_t reg_addr, uint8_t *data, uint8_t length);
+i2c_result_t i2c_read(uint8_t slave_addr, uint16_t reg_addr, uint8_t *data, uint8_t length);
 
 /* Pure read response without sub-address phase (for commands that already sent read request parameters) */
 /*!
diff --git a/Src/i2c.c b/Src/i2c.c
index b3efc84..705070c 100644
--- a/Src/i2c.c
+++ b/Src/i2c.c
@@ -640,13 +640,13 @@ i2c_result_t i2c_read_16bits(uint8_t slave_addr, uint8_t reg_addr, uint8_t *data
 /*!
     \brief      write data to I2C device with configurable length
     \param[in]  slave_addr: slave device address (7-bit)
-    \param[in]  reg_addr: register address
+    \param[in]  reg_addr: register address (16-bit, high byte first)
     \param[in]  data: pointer to data buffer
     \param[in]  length: number of bytes to write (1-255)
     \param[out] none
     \retval     i2c_result_t: operation result
 */
-i2c_result_t i2c_write(uint8_t slave_addr, uint8_t reg_addr, uint8_t *data, uint8_t length) {
+i2c_result_t i2c_write(uint8_t slave_addr, uint16_t reg_addr, uint8_t *data, uint8_t length) {
     i2c_state_t state = I2C_STATE_START;
     uint16_t timeout = 0;
     uint8_t retry_count = 0;
@@ -727,8 +727,18 @@ i2c_result_t i2c_write(uint8_t slave_addr, uint8_t reg_addr, uint8_t *data, uint
                     break;
                 }
 
-                /* send register address */
-                i2c_data_transmit(I2C0, reg_addr);
+                /* send register address, high byte first */
+                if(_i2c_wait_flag_timeout(I2C_FLAG_TBE) != I2C_RESULT_SUCCESS) {
+                    state = I2C_STATE_ERROR;
+                    break;
+                }
+                i2c_data_transmit(I2C0, (uint8_t)(reg_addr >> 8));
+
+                if(_i2c_wait_flag_timeout(I2C_FLAG_TBE) != I2C_RESULT_SUCCESS) {
+                    state = I2C_STATE_ERROR;
+                    break;
+                }
+                i2c_data_transmit(I2C0, (uint8_t)(reg_addr & 0xFF));
                 state = I2C_STATE_TRANSMIT_DATA;
                 break;
 
@@ -814,12 +824,12 @@ i2c_result_t i2c_write(uint8_t slave_addr, uint8_t reg_addr, uint8_t *data, uint
 /*!
     \brief      read data from I2C device with configurable length
     \param[in]  slave_addr: slave device address (7-bit)
-    \param[in]  reg_addr: register address
+    \param[in]  reg_addr: register address (16-bit, high byte first)
     \param[out] data: pointer to data buffer
     \param[in]  length: number of bytes to read (1-255)
     \retval     i2c_result_t: operation result
 */
-i2c_result_t i2c_read(uint8_t slave_addr, uint8_t reg_addr, uint8_t *data, uint8_t length) {
+i2c_result_t i2c_read(uint8_t slave_addr, uint16_t reg_addr, uint8_t *data, uint8_t length) {
     i2c_state_t state = I2C_STATE_START;
     uint16_t timeout = 0;
     uint8_t retry_count = 0;
@@ -937,8 +947,18 @@ i2c_result_t i2c_read(uint8_t slave_addr, uint8_t reg_addr, uint8_t *data, uint8
                     break;
                 }
 
-                /* send register address */
-                i2c_data_transmit(I2C0, reg_addr);
+                /* send register address, high byte first */
+                if(_i2c_wait_flag_timeout(I2C_FLAG_TBE) != I2C_RESULT_SUCCESS) {
+                    state = I2C_STATE_ERROR;
+                    break;
+                }
+                i2c_data_transmit(I2C0, (uint8_t)(reg_addr >> 8));
+
+                if(_i2c_wait_flag_timeout(I2C_FLAG_TBE) != I2C_RESULT_SUCCESS) {
+                    state = I2C_STATE_ERROR;
+                    break;
+                }
+                i2c_data_transmit(I2C0, (uint8_t)(reg_addr & 0xFF));
                 state = I2C_STATE_RESTART;
                 break;