diff --git a/include/mgos_i2c.h b/include/mgos_i2c.h
index bb2f2d1..416f319 100644
--- a/include/mgos_i2c.h
+++ b/include/mgos_i2c.h
@@ -122,6 +122,5 @@ void mgos_i2c_close(struct mgos_i2c *conn);
 struct mgos_i2c *mgos_i2c_get_global(void);
 
 // User contributed!
-int get_i2c_register(struct mgos_i2c *i2c, uint8_t addr, uint8_t reg, uint8_t *val);
 bool mgos_i2c_open(int busnr);
 
diff --git a/src/main.c b/src/main.c
index eafe0ae..ea07823 100644
--- a/src/main.c
+++ b/src/main.c
@@ -23,15 +23,19 @@ void i2c_scanner(struct mgos_i2c *i2c) {
 }
 
 bool i2c_dumpregs(struct mgos_i2c *i2c, uint8_t i2caddr) {
-  uint8_t reg, value;
+  uint16_t reg;
+  int value;
 
-  for(reg=0; reg<255; reg++) {
-    if (0 != get_i2c_register(i2c, i2caddr, reg, &value)) {
-      LOG(LL_ERROR, ("Could not read register"));
+  for(reg=0; reg<256; reg++) {
+    value=mgos_i2c_read_reg_b(i2c, i2caddr, reg);
+    if (value<0) {
+      printf(" XX");
     } else {
-      LOG(LL_INFO, ("reg=%u value=%u", reg, value));
+      printf(" %02x", value);
     }
+    if (reg%16==15) printf("\n");
   }
+  printf("\n");
   return true;
 }
 
@@ -82,9 +86,12 @@ int main() {
   }
 
   i2c_scanner(i2c);
+//  i2c_dumpregs(i2c, 0x76);
   for (;;) {
     sht31(i2c, 0x44);
-    si7021(i2c, 0x44);
+
+//    i2c_dumpregs(i2c, 0x40);
+    si7021(i2c, 0x40);
     sleep(1);
   }
 
diff --git a/src/mgos_i2c.c b/src/mgos_i2c.c
index e40d2f5..ee2146d 100644
--- a/src/mgos_i2c.c
+++ b/src/mgos_i2c.c
@@ -89,117 +89,80 @@ bool mgos_i2c_set_freq(struct mgos_i2c *i2c, int freq) {
 }
 
 int mgos_i2c_read_reg_b(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg) {
-  uint8_t data;
+  uint8_t value;
 
-  if (!i2c) {
-    LOG(LL_ERROR, ("No I2C bus"));
+  if (!mgos_i2c_read_reg_n(i2c, addr, reg, 1, &value))
     return -1;
-  }
-
-  if (ioctl(i2c->fd,I2C_SLAVE,addr) < 0) {
-    LOG(LL_ERROR, ("Cannot select slave 0x%04x on I2C bus: %s", addr, strerror(errno)));
-    return -1;
-  }
-  if (1 != write(i2c->fd, &reg, 1)) {
-    LOG(LL_ERROR, ("Cannot select register 0x%02x on device 0x%04x: %s", reg, addr, strerror(errno)));
-    return -1;
-  }
-  if (1 != i2c_read_timeout(i2c, &data, 1)) {
-    LOG(LL_ERROR, ("Cannot read register 0x%02x on device 0x%04x: %s", reg, addr, strerror(errno)));
-    return -1;
-  }
-  return data;
+  return value;
 }
 
 int mgos_i2c_read_reg_w(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg) {
-  uint16_t data;
+  uint16_t value;
+  uint8_t data[2];
 
-  if (!i2c) {
-    LOG(LL_ERROR, ("No I2C bus"));
+  if (!mgos_i2c_read_reg_n(i2c, addr, reg, 2, data))
     return -1;
-  }
-
-  if (ioctl(i2c->fd,I2C_SLAVE,addr) < 0) {
-    LOG(LL_ERROR, ("Cannot select slave 0x%04x on I2C bus: %s", addr, strerror(errno)));
-    return -1;
-  }
-  if (1 != write(i2c->fd, &reg, 1)) {
-    LOG(LL_ERROR, ("Cannot select register 0x%02x on device 0x%04x: %s", reg, addr, strerror(errno)));
-    return -1;
-  }
-  if (2 != i2c_read_timeout(i2c, &data, 2)) {
-    LOG(LL_ERROR, ("Cannot read register 0x%02x on device 0x%04x: %s", reg, addr, strerror(errno)));
-    return -1;
-  }
-  return data;
+  value=(data[1]<<8)+data[0];
+  return value;
 }
 
 bool mgos_i2c_read_reg_n(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg, size_t n, uint8_t *buf) {
+  uint8_t outbuf;
+  struct i2c_rdwr_ioctl_data packets;
+  struct i2c_msg messages[2];
+
   if (!i2c) {
     LOG(LL_ERROR, ("No I2C bus"));
-    return false;
+    return -1;
   }
 
   if (ioctl(i2c->fd,I2C_SLAVE,addr) < 0) {
     LOG(LL_ERROR, ("Cannot select slave 0x%04x on I2C bus: %s", addr, strerror(errno)));
-    return false;
+    return -1;
   }
-  if (1 != write(i2c->fd, &reg, 1)) {
-    LOG(LL_ERROR, ("Cannot select register 0x%02x on device 0x%04x: %s", reg, addr, strerror(errno)));
-    return false;
-  }
-  if (n != i2c_read_timeout(i2c, buf, n)) {
-    LOG(LL_ERROR, ("Cannot read %lu bytes at register 0x%02x on device 0x%04x: %s", n, reg, addr, strerror(errno)));
-    return false;
-  }
-  return true;
-}
+  /*
+  * In order to read a register, we first do a "dummy write" by writing
+  * 0 bytes to the register we want to read from.  This is similar to
+  * the packet in set_i2c_register, except it's 1 byte rather than 2.
+  */
+  outbuf = reg;
+  messages[0].addr  = addr;
+  messages[0].flags = 0;
+  messages[0].len   = sizeof(outbuf);
+  messages[0].buf   = &outbuf;
 
-bool mgos_i2c_write_reg_b(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg, uint8_t value) {
-  uint8_t data[2];
+  /* The data will get returned in this structure */
+  messages[1].addr  = addr;
+  messages[1].flags = I2C_M_RD/* | I2C_M_NOSTART*/;
+  messages[1].len   = n;
+  messages[1].buf   = buf;
 
-  if (!i2c) {
-    LOG(LL_ERROR, ("No I2C bus"));
+  /* Send the request to the kernel and get the result back */
+  packets.msgs      = messages;
+  packets.nmsgs     = 2;
+  if(ioctl(i2c->fd, I2C_RDWR, &packets) < 0)
     return false;
-  }
 
-  if (ioctl(i2c->fd,I2C_SLAVE,addr) < 0) {
-    LOG(LL_ERROR, ("Cannot select slave 0x%04x on I2C bus: %s", addr, strerror(errno)));
-    return false;
-  }
-  data[0]=reg;
-  data[1]=value;
-  if (2 != write(i2c->fd, data, 2)) {
-    LOG(LL_ERROR, ("Cannot write to register 0x%02x on device 0x%04x: %s", reg, addr, strerror(errno)));
-    return false;
-  }
   return true;
 }
 
 bool mgos_i2c_write_reg_w(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg, uint16_t value) {
-  uint8_t data[3];
+  uint8_t data[2];
+  data[0]=value>>8;
+  data[1]=value&0xFF;
 
-  if (!i2c) {
-    LOG(LL_ERROR, ("No I2C bus"));
-    return false;
-  }
+  return mgos_i2c_write_reg_n(i2c, addr, reg, 2, data);
+}
 
-  if (ioctl(i2c->fd,I2C_SLAVE,addr) < 0) {
-    LOG(LL_ERROR, ("Cannot select slave 0x%04x on I2C bus: %s", addr, strerror(errno)));
-    return false;
-  }
-  data[0]=reg;
-  data[1]=(uint8_t)(0xFF & (value >> 0));
-  data[2]=(uint8_t)(0xFF & (value >> 8));
-
-  if (3 != write(i2c->fd, data, 3)) {
-    LOG(LL_ERROR, ("Cannot write to register 0x%02x on device 0x%04x: %s", reg, addr, strerror(errno)));
-    return false;
-  }
-  return true;
+bool mgos_i2c_write_reg_b(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg, uint8_t value) {
+  return mgos_i2c_write_reg_n(i2c, addr, reg, 1, &value);
 }
 
 bool mgos_i2c_write_reg_n(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg, size_t n, const uint8_t *buf) {
+  unsigned char outbuf[n+1];
+  struct i2c_rdwr_ioctl_data packets;
+  struct i2c_msg messages[1];
+
   if (!i2c) {
     LOG(LL_ERROR, ("No I2C bus"));
     return false;
@@ -209,13 +172,18 @@ bool mgos_i2c_write_reg_n(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg, size
     LOG(LL_ERROR, ("Cannot select slave 0x%04x on I2C bus: %s", addr, strerror(errno)));
     return false;
   }
-  if (1 != write(i2c->fd, &reg, 1)) {
-    LOG(LL_ERROR, ("Cannot select register 0x%02x on device 0x%04x: %s", reg, addr, strerror(errno)));
-    return false;
-  }
 
-  if (n != write(i2c->fd, buf, n)) {
-    LOG(LL_ERROR, ("Cannot write to register 0x%02x on device 0x%04x: %s", reg, addr, strerror(errno)));
+  messages[0].addr=addr;
+  messages[0].flags=0;
+  messages[0].len=sizeof(outbuf);
+  messages[0].buf=outbuf;
+
+  outbuf[0]=reg;
+  memcpy(outbuf+1, buf, n);
+
+  packets.msgs  = messages;
+  packets.nmsgs = 1;
+  if(ioctl(i2c->fd, I2C_RDWR, &packets) < 0) {
     return false;
   }
   return true;
@@ -229,42 +197,6 @@ struct mgos_i2c *mgos_i2c_get_global(void) {
   return s_global_i2c_bus;
 }
 
-int get_i2c_register(struct mgos_i2c *i2c, uint8_t addr, uint8_t reg, uint8_t *val) {
-    unsigned char inbuf, outbuf;
-    struct i2c_rdwr_ioctl_data packets;
-    struct i2c_msg messages[2];
-
-    /*
-     * In order to read a register, we first do a "dummy write" by writing
-     * 0 bytes to the register we want to read from.  This is similar to
-     * the packet in set_i2c_register, except it's 1 byte rather than 2.
-     */
-    outbuf = reg;
-    messages[0].addr  = addr;
-    messages[0].flags = 0;
-    messages[0].len   = sizeof(outbuf);
-    messages[0].buf   = &outbuf;
-
-    /* The data will get returned in this structure */
-    messages[1].addr  = addr;
-    messages[1].flags = I2C_M_RD/* | I2C_M_NOSTART*/;
-    messages[1].len   = sizeof(inbuf);
-    messages[1].buf   = &inbuf;
-
-    /* Send the request to the kernel and get the result back */
-    packets.msgs      = messages;
-    packets.nmsgs     = 2;
-    if(ioctl(i2c->fd, I2C_RDWR, &packets) < 0) {
-        LOG(LL_ERROR, ("Unable to send data"));
-        return 1;
-    }
-    *val = inbuf;
-
-    return 0;
-}
-
-
-
 // User provided function to interface with Linux I2C driver
 bool mgos_i2c_open(int busnr) {
   int fd;
diff --git a/src/mgos_si7021.c b/src/mgos_si7021.c
index fb0b7d1..2c54a0a 100644
--- a/src/mgos_si7021.c
+++ b/src/mgos_si7021.c
@@ -24,14 +24,24 @@
 // Public functions follow
 struct mgos_si7021 *mgos_si7021_create(struct mgos_i2c *i2c, uint8_t i2caddr) {
   struct mgos_si7021 *sensor;
+  int ret;
 
   if (!i2c) return NULL;
 
+  // Reset and query register
+  ret=mgos_i2c_read_reg_b(i2c, i2caddr, MGOS_SI7021_READRHT_REG_CMD);
+  if (ret!=0x3A) {
+    LOG(LL_ERROR, ("Chip ID register invalid, expected 0x3A got 0x%02x (%d)", ret, ret));
+    return NULL;
+  }
+  LOG(LL_INFO, ("SI7021 found at I2C 0x%02x", i2caddr));
+
   sensor=calloc(1, sizeof(struct mgos_si7021));
   if (!sensor) return NULL;
   sensor->i2caddr=i2caddr;
   sensor->i2c=i2c;
   sensor->last_read_time=0;
+
   return sensor;
 }