diff --git a/src/omv/cambus.c b/src/omv/cambus.c index db6d01beb..5c1bb9eda 100644 --- a/src/omv/cambus.c +++ b/src/omv/cambus.c @@ -105,3 +105,29 @@ int cambus_writew(uint8_t slv_addr, uint8_t reg_addr, uint16_t reg_data) __enable_irq(); return ret; } + +int cambus_readw2(uint8_t slv_addr, uint16_t reg_addr, uint16_t *reg_data) +{ + int ret=0; + __disable_irq(); + if (HAL_I2C_Mem_Read(&I2CHandle, slv_addr, reg_addr, + I2C_MEMADD_SIZE_16BIT, (uint8_t*) reg_data, 2, I2C_TIMEOUT) != HAL_OK) { + ret = -1; + } + __enable_irq(); + *reg_data = (*reg_data >> 8) | (*reg_data << 8); + return ret; +} + +int cambus_writew2(uint8_t slv_addr, uint16_t reg_addr, uint16_t reg_data) +{ + int ret=0; + reg_data = (reg_data >> 8) | (reg_data << 8); + __disable_irq(); + if (HAL_I2C_Mem_Write(&I2CHandle, slv_addr, reg_addr, + I2C_MEMADD_SIZE_16BIT, (uint8_t*) ®_data, 2, I2C_TIMEOUT) != HAL_OK) { + ret = -1; + } + __enable_irq(); + return ret; +} diff --git a/src/omv/cambus.h b/src/omv/cambus.h index 17f6a471e..fde9d46c0 100644 --- a/src/omv/cambus.h +++ b/src/omv/cambus.h @@ -15,4 +15,6 @@ int cambus_readb(uint8_t slv_addr, uint8_t reg_addr, uint8_t *reg_data); int cambus_writeb(uint8_t slv_addr, uint8_t reg_addr, uint8_t reg_data); int cambus_readw(uint8_t slv_addr, uint8_t reg_addr, uint16_t *reg_data); int cambus_writew(uint8_t slv_addr, uint8_t reg_addr, uint16_t reg_data); +int cambus_readw2(uint8_t slv_addr, uint16_t reg_addr, uint16_t *reg_data); +int cambus_writew2(uint8_t slv_addr, uint16_t reg_addr, uint16_t reg_data); #endif // __CAMBUS_H__