Pim van Pelt
95 lines
2.2 KiB
C
95 lines
2.2 KiB
C
#include "mgos_i2c.h"
|
|
|
|
bool mgos_i2c_setbits_reg_b(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg, uint8_t bitoffset, uint8_t bitlen, uint8_t value) {
|
|
uint8_t old, new;
|
|
|
|
if (!i2c || bitoffset + bitlen > 8 || bitlen == 0) {
|
|
return false;
|
|
}
|
|
if (value > (1 << bitlen) - 1) {
|
|
return false;
|
|
}
|
|
|
|
if (!mgos_i2c_read_reg_n(i2c, addr, reg, 1, &old)) {
|
|
return false;
|
|
}
|
|
new = old | (((1 << bitlen) - 1) << bitoffset);
|
|
new &= ~(((1 << bitlen) - 1) << bitoffset);
|
|
new |= (value) << bitoffset;
|
|
|
|
if (!mgos_i2c_write_reg_n(i2c, addr, reg, 1, &new)) {
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool mgos_i2c_getbits_reg_b(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg, uint8_t bitoffset, uint8_t bitlen, uint8_t *value) {
|
|
uint8_t val, mask;
|
|
|
|
if (!i2c || bitoffset + bitlen > 8 || bitlen == 0 || !value) {
|
|
return false;
|
|
}
|
|
|
|
if (!mgos_i2c_read_reg_n(i2c, addr, reg, 1, &val)) {
|
|
return false;
|
|
}
|
|
|
|
mask = ((1 << bitlen) - 1);
|
|
mask <<= bitoffset;
|
|
val &= mask;
|
|
val >>= bitoffset;
|
|
|
|
*value = val;
|
|
return true;
|
|
}
|
|
|
|
bool mgos_i2c_setbits_reg_w(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg, uint8_t bitoffset, uint8_t bitlen, uint16_t value) {
|
|
uint16_t old, new;
|
|
uint8_t d[2];
|
|
|
|
if (!i2c || bitoffset + bitlen > 16 || bitlen == 0) {
|
|
return false;
|
|
}
|
|
if (value > (1 << bitlen) - 1) {
|
|
return false;
|
|
}
|
|
|
|
if (!mgos_i2c_read_reg_n(i2c, addr, reg, 2, d)) {
|
|
return false;
|
|
}
|
|
old = (d[0] << 8) | d[1];
|
|
new = old | (((1 << bitlen) - 1) << bitoffset);
|
|
new &= ~(((1 << bitlen) - 1) << bitoffset);
|
|
new |= (value) << bitoffset;
|
|
|
|
d[0] = new >> 8;
|
|
d[1] = new & 0xFF;
|
|
|
|
if (!mgos_i2c_write_reg_n(i2c, addr, reg, 2, d)) {
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool mgos_i2c_getbits_reg_w(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg, uint8_t bitoffset, uint8_t bitlen, uint16_t *value) {
|
|
uint16_t val, mask;
|
|
uint8_t d[2];
|
|
|
|
if (!i2c || bitoffset + bitlen > 16 || bitlen == 0 || !value) {
|
|
return false;
|
|
}
|
|
|
|
if (!mgos_i2c_read_reg_n(i2c, addr, reg, 2, d)) {
|
|
return false;
|
|
}
|
|
val = (d[0] << 8) | d[1];
|
|
|
|
mask = ((1 << bitlen) - 1);
|
|
mask <<= bitoffset;
|
|
val &= mask;
|
|
val >>= bitoffset;
|
|
|
|
*value = val;
|
|
return true;
|
|
}
|