From b631bc732641a30408eb028b2e4321a1f46b2586 Mon Sep 17 00:00:00 2001
From: Pim van Pelt <pim@ipng.nl>
Date: Wed, 9 Jan 2019 14:13:08 +0100
Subject: [PATCH] Add mgos_i2c_bitfields.c

---
 include/mgos_i2c.h       | 27 +++++++++++++
 src/mgos_i2c_bitfields.c | 87 ++++++++++++++++++++++++++++++++++++++++
 2 files changed, 114 insertions(+)
 create mode 100644 src/mgos_i2c_bitfields.c

diff --git a/include/mgos_i2c.h b/include/mgos_i2c.h
index 416f319..2ea8db0 100644
--- a/include/mgos_i2c.h
+++ b/include/mgos_i2c.h
@@ -115,6 +115,33 @@ bool mgos_i2c_write_reg_w(struct mgos_i2c *conn, uint16_t addr, uint8_t reg,
 bool mgos_i2c_write_reg_n(struct mgos_i2c *conn, uint16_t addr, uint8_t reg,
                           size_t n, const uint8_t *buf);
 
+/*
+ * Helper to set/get a number of bits in a register `reg` on a device at
+ * address `addr`.
+ * - bitoffset: 0..7 is the position at which to write `value`
+ * - bitlen   : number of bits to write
+ * - value    : the value to write there
+ *
+ * Invariants:
+ * - value must fit in `bitlen` (ie value < 2^bitlen)
+ * - bitlen+bitoffset <= register size (8 for reg_b, 16 for reg_w)
+ * - bitlen cannot be 0.
+ *
+ * The `setbits` call will write the bits to the register, the `getbits` call
+ * will return the value of those bits in *value.
+ *
+ * Returns `true` in case of success, `false` otherwise.
+ */
+bool mgos_i2c_setbits_reg_b(struct mgos_i2c *conn, uint16_t addr, uint8_t reg,
+                            uint8_t bitoffset, uint8_t bitlen, uint8_t value);
+bool mgos_i2c_getbits_reg_b(struct mgos_i2c *conn, uint16_t addr, uint8_t reg,
+                            uint8_t bitoffset, uint8_t bitlen, uint8_t *value);
+bool mgos_i2c_setbits_reg_w(struct mgos_i2c *conn, uint16_t addr, uint8_t reg,
+                            uint8_t bitoffset, uint8_t bitlen, uint16_t value);
+bool mgos_i2c_getbits_reg_w(struct mgos_i2c *conn, uint16_t addr, uint8_t reg,
+                            uint8_t bitoffset, uint8_t bitlen, uint16_t *value);
+
+
 /* Close i2c connection and free resources. */
 void mgos_i2c_close(struct mgos_i2c *conn);
 
diff --git a/src/mgos_i2c_bitfields.c b/src/mgos_i2c_bitfields.c
new file mode 100644
index 0000000..66f1180
--- /dev/null
+++ b/src/mgos_i2c_bitfields.c
@@ -0,0 +1,87 @@
+#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;
+
+  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, (uint8_t *)&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, 2, (uint8_t *)&new)) {
+    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;
+
+  if (!i2c || bitoffset + bitlen > 16 || bitlen == 0 || !value) {
+    return false;
+  }
+
+  if (!mgos_i2c_read_reg_n(i2c, addr, reg, 2, (uint8_t *)&val)) {
+    return false;
+  }
+
+  mask   = ((1 << bitlen) - 1);
+  mask <<= bitoffset;
+  val   &= mask;
+  val  >>= bitoffset;
+
+  *value = val;
+  return true;
+}