diff --git a/Makefile b/Makefile
index 7a2cf2d..6e0b709 100644
--- a/Makefile
+++ b/Makefile
@@ -14,7 +14,7 @@ INCDIR  = include
 OBJDIR  = build
 BINDIR  = .
 
-SOURCES  := $(wildcard $(SRCDIR)/*.c)
+SOURCES  := $(wildcard $(SRCDIR)/*.c) 
 INCLUDES := $(wildcard $(SRCDIR)/*.h)
 OBJECTS  := $(SOURCES:$(SRCDIR)/%.c=$(OBJDIR)/%.o)
 RM       = rm -f
diff --git a/include/mgos_i2c.h b/include/mgos_i2c.h
index 2b17bb5..bb2f2d1 100644
--- a/include/mgos_i2c.h
+++ b/include/mgos_i2c.h
@@ -12,6 +12,15 @@
 #pragma once
 
 #include "mgos.h"
+#include <stdio.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <fcntl.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <string.h>
+
 
 /* Each platform defines its own I2C connection parameters. */
 struct mgos_i2c;
@@ -113,5 +122,6 @@ 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/include/mgos_mock.h b/include/mgos_mock.h
index 8503000..5a633f0 100644
--- a/include/mgos_mock.h
+++ b/include/mgos_mock.h
@@ -30,6 +30,8 @@ int log_print_prefix(enum cs_log_level l, const char *func, const char *file);
   } while (0)
 
 
-float mg_time();
+double mg_time();
+
+void mgos_usleep(uint32_t usecs);
 
 #endif // __MGOS_MOCK_H
diff --git a/include/mgos_sht31.h b/include/mgos_sht31.h
new file mode 100644
index 0000000..646b964
--- /dev/null
+++ b/include/mgos_sht31.h
@@ -0,0 +1,57 @@
+#pragma once
+
+#include "mgos.h"
+#include "mgos_i2c.h"
+
+#define MGOS_SHT31_READ_DELAY (2)
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct mgos_sht31;
+
+/*
+ *
+ */
+struct mgos_sht31 *mgos_sht31_create(struct mgos_i2c *i2c, uint8_t i2caddr);
+
+/*
+ *
+ */
+void mgos_sht31_destroy(struct mgos_sht31 **sensor);
+
+/*
+ *
+ */
+bool mgos_sht31_read(struct mgos_sht31 *sensor);
+
+/*
+ *
+ */
+void mgos_sht31_setHeater(struct mgos_sht31 *sensor, bool enabled);
+
+/*
+ *
+ */
+float mgos_sht31_getTemperature(struct mgos_sht31 *sensor);
+
+/*
+ *
+ */
+float mgos_sht31_getHumidity(struct mgos_sht31 *sensor);
+
+
+/*
+ *
+ */
+uint16_t mgos_sht31_getStatus(struct mgos_sht31 *sensor);
+
+/*
+ *
+ */
+bool mgos_sht31_i2c_init(void);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/src/main.c b/src/main.c
index daae1ef..b810cb7 100644
--- a/src/main.c
+++ b/src/main.c
@@ -1,8 +1,61 @@
 #include "mgos.h"
 #include "mgos_i2c.h"
+#include "mgos_sht31.h"
+#include <fcntl.h>
+#include <sys/ioctl.h>
 
 #define I2CBUSNR 7
 
+void i2c_scanner(struct mgos_i2c *i2c) {
+  int i;
+  if (!i2c) {
+    LOG(LL_ERROR, ("No global I2C bus configured"));
+    return;
+  }
+
+  for(i=0x3; i<0x77; i++) {
+    bool ret;
+    ret=mgos_i2c_read(i2c, i, NULL, 0, true);
+    if (ret)
+      LOG(LL_INFO, ("I2C Address 0x%02x %s", i, ret?"true":"false"));
+  }
+}
+
+bool i2c_dumpregs(struct mgos_i2c *i2c, uint8_t i2caddr) {
+  uint8_t reg, value;
+
+  for(reg=0; reg<255; reg++) {
+    if (0 != get_i2c_register(i2c, i2caddr, reg, &value)) {
+      LOG(LL_ERROR, ("Could not read register"));
+    } else {
+      LOG(LL_INFO, ("reg=%u value=%u", reg, value));
+    }
+  }
+  return true;
+}
+
+bool sht31(struct mgos_i2c *i2c, uint8_t i2caddr) {
+  struct mgos_sht31 *sht31;
+  float temp, humid;
+  int num;
+
+  if (!(sht31 = mgos_sht31_create(i2c, i2caddr))) {
+    LOG(LL_ERROR, ("Cannot create SHT31 device"));
+    return false;
+  }
+
+  num=1000;
+  while (num--) {
+    temp=mgos_sht31_getTemperature(sht31);
+    humid=mgos_sht31_getHumidity(sht31);
+    LOG(LL_INFO, ("SHT31: temperature=%.2fC humidity=%.1f%%", temp, humid));
+    sleep(1);
+  }
+
+  mgos_sht31_destroy(&sht31);
+  return true;
+}
+
 int main() {
   struct mgos_i2c *i2c;
 
@@ -10,11 +63,13 @@ int main() {
     LOG(LL_ERROR, ("Cannot open I2C bus %u", I2CBUSNR));
     return -1;
   }
-  i2c = mgos_i2c_get_global();
-  if (!i2c) {
+  if (!(i2c = mgos_i2c_get_global())) {
     LOG(LL_ERROR, ("Cannot open I2C bus"));
     return -2;
   }
 
+  i2c_scanner(i2c);
+  sht31(i2c, 0x44);
+
   return 0;
 }
diff --git a/src/mgos_i2c.c b/src/mgos_i2c.c
index 9eecb8b..e40d2f5 100644
--- a/src/mgos_i2c.c
+++ b/src/mgos_i2c.c
@@ -6,16 +6,35 @@
 #include <i2c/smbus.h>
 #include <sys/ioctl.h>
 #include <unistd.h>
-
-
+#include <errno.h>
 
 struct mgos_i2c {
   int fd;
+  uint16_t read_timeout_ms; // in msec
   char *filename;
 };
 
 static struct mgos_i2c *s_global_i2c_bus = NULL;
 
+static size_t i2c_read_timeout(struct mgos_i2c *i2c, void *data, size_t len) {
+  uint16_t tries;
+  size_t ret=-1;
+
+  if (!i2c) return -1;
+
+  ret = read(i2c->fd, data, len);
+
+  for(tries=i2c->read_timeout_ms; tries && ret!=len && len>0; tries--) {
+    usleep(1000);
+    ret = read(i2c->fd, data, len);
+  }
+  if (ret!=len) {
+//    LOG(LL_ERROR, ("Read timeout on I2C"));
+    return -1;
+  }
+  return len;
+}
+
 bool mgos_i2c_read(struct mgos_i2c *i2c, uint16_t addr, void *data, size_t len, bool stop) {
   size_t ret;
 
@@ -28,8 +47,12 @@ bool mgos_i2c_read(struct mgos_i2c *i2c, uint16_t addr, void *data, size_t len,
     LOG(LL_ERROR, ("Cannot select slave 0x%04x on I2C bus", addr));
     return false;
   }
-  ret = read(i2c->fd, data, len);
-  return (ret == len);
+  ret = i2c_read_timeout(i2c, data, len);
+  if (ret != len) {
+//    LOG(LL_DEBUG, ("RECV %ld bytes (wanted %lu) from 0x%02x: %s", ret, len, addr, strerror(errno)));
+    return false;
+  }
+  return true;
 }
 
 bool mgos_i2c_write(struct mgos_i2c *i2c, uint16_t addr, const void *data, size_t len, bool stop) {
@@ -45,7 +68,11 @@ bool mgos_i2c_write(struct mgos_i2c *i2c, uint16_t addr, const void *data, size_
     return false;
   }
   ret = write(i2c->fd, data, len);
-  return (ret == len);
+  if (ret != len) {
+//    LOG(LL_DEBUG, ("XMIT %ld bytes (wanted %lu) from 0x%02x: %s", ret, len, addr, strerror(errno)));
+    return false;
+  }
+  return true;
 }
 
 void mgos_i2c_stop(struct mgos_i2c *i2c) {
@@ -70,15 +97,15 @@ int mgos_i2c_read_reg_b(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg) {
   }
 
   if (ioctl(i2c->fd,I2C_SLAVE,addr) < 0) {
-    LOG(LL_ERROR, ("Cannot select slave 0x%04x on I2C bus", addr));
+    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", reg, addr));
+    LOG(LL_ERROR, ("Cannot select register 0x%02x on device 0x%04x: %s", reg, addr, strerror(errno)));
     return -1;
   }
-  if (1 != read(i2c->fd, &data, 1)) {
-    LOG(LL_ERROR, ("Cannot read register 0x%02x on device 0x%04x", reg, addr));
+  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;
@@ -93,15 +120,15 @@ int mgos_i2c_read_reg_w(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg) {
   }
 
   if (ioctl(i2c->fd,I2C_SLAVE,addr) < 0) {
-    LOG(LL_ERROR, ("Cannot select slave 0x%04x on I2C bus", addr));
+    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", reg, addr));
+    LOG(LL_ERROR, ("Cannot select register 0x%02x on device 0x%04x: %s", reg, addr, strerror(errno)));
     return -1;
   }
-  if (2 != read(i2c->fd, &data, 2)) {
-    LOG(LL_ERROR, ("Cannot read register 0x%02x on device 0x%04x", reg, addr));
+  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;
@@ -114,15 +141,15 @@ bool mgos_i2c_read_reg_n(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg, size_
   }
 
   if (ioctl(i2c->fd,I2C_SLAVE,addr) < 0) {
-    LOG(LL_ERROR, ("Cannot select slave 0x%04x on I2C bus", addr));
+    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", reg, addr));
+    LOG(LL_ERROR, ("Cannot select register 0x%02x on device 0x%04x: %s", reg, addr, strerror(errno)));
     return false;
   }
-  if (n != read(i2c->fd, buf, n)) {
-    LOG(LL_ERROR, ("Cannot read %lu bytes at register 0x%02x on device 0x%04x", n, reg, addr));
+  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;
@@ -137,13 +164,13 @@ bool mgos_i2c_write_reg_b(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg, uint
   }
 
   if (ioctl(i2c->fd,I2C_SLAVE,addr) < 0) {
-    LOG(LL_ERROR, ("Cannot select slave 0x%04x on I2C bus", addr));
+    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", reg, addr));
+    LOG(LL_ERROR, ("Cannot write to register 0x%02x on device 0x%04x: %s", reg, addr, strerror(errno)));
     return false;
   }
   return true;
@@ -158,7 +185,7 @@ bool mgos_i2c_write_reg_w(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg, uint
   }
 
   if (ioctl(i2c->fd,I2C_SLAVE,addr) < 0) {
-    LOG(LL_ERROR, ("Cannot select slave 0x%04x on I2C bus", addr));
+    LOG(LL_ERROR, ("Cannot select slave 0x%04x on I2C bus: %s", addr, strerror(errno)));
     return false;
   }
   data[0]=reg;
@@ -166,7 +193,7 @@ bool mgos_i2c_write_reg_w(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg, uint
   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", reg, addr));
+    LOG(LL_ERROR, ("Cannot write to register 0x%02x on device 0x%04x: %s", reg, addr, strerror(errno)));
     return false;
   }
   return true;
@@ -179,16 +206,16 @@ bool mgos_i2c_write_reg_n(struct mgos_i2c *i2c, uint16_t addr, uint8_t reg, size
   }
 
   if (ioctl(i2c->fd,I2C_SLAVE,addr) < 0) {
-    LOG(LL_ERROR, ("Cannot select slave 0x%04x on I2C bus", addr));
+    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", reg, addr));
+    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", reg, addr));
+    LOG(LL_ERROR, ("Cannot write to register 0x%02x on device 0x%04x: %s", reg, addr, strerror(errno)));
     return false;
   }
   return true;
@@ -202,6 +229,42 @@ 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;
@@ -220,6 +283,7 @@ bool mgos_i2c_open(int busnr) {
   }
   i2c->fd=fd;
   i2c->filename=strdup(filename);
+  i2c->read_timeout_ms=1000;
   s_global_i2c_bus=i2c;
   LOG(LL_INFO, ("Opened I2C bus on %s", filename));
 
diff --git a/src/mgos_mock.c b/src/mgos_mock.c
index 8423b09..0a2dd8f 100644
--- a/src/mgos_mock.c
+++ b/src/mgos_mock.c
@@ -2,6 +2,7 @@
  */
 
 #include "mgos_mock.h"
+#include <unistd.h>
 
 int _mgos_timers = 0;
 
@@ -31,14 +32,18 @@ int log_print_prefix(enum cs_log_level l, const char *func, const char *file) {
   return 1;
 }
 
-float mg_time() {
-  struct timeval tod;
-  float time;
+double mg_time() {
+  struct timespec ts;
+  double ret;
 
-  gettimeofday(&tod, NULL);
+  clock_gettime(CLOCK_REALTIME_COARSE, &ts);
 
-  time = tod.tv_sec;
-  time += tod.tv_usec/1000000;
+  ret = (double) ts.tv_sec;
+  ret += ((double)ts.tv_nsec)/1000000000;
 
-  return time;
+  return ret;
+}
+
+void mgos_usleep(uint32_t usecs) {
+  usleep(usecs);
 }
diff --git a/src/mgos_sht31.c b/src/mgos_sht31.c
new file mode 100644
index 0000000..b2e8743
--- /dev/null
+++ b/src/mgos_sht31.c
@@ -0,0 +1,132 @@
+/*
+ * Copyright 2018 Google Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "mgos.h"
+#include "mgos_sht31_internal.h"
+#include "mgos_i2c.h"
+
+// Private functions follow
+static bool mgos_sht31_cmd(struct mgos_sht31 *sensor, uint16_t cmd) {
+  uint8_t data[2];
+
+  if (!sensor || !sensor->i2c)
+    return false;
+
+  data[0]=cmd>>8;
+  data[1]=cmd&0xFF;
+  if (!mgos_i2c_write(sensor->i2c, sensor->i2caddr, data, 2, true)) {
+    LOG(LL_ERROR, ("I2C=0x%02x cmd=%u (0x%04x) write error", sensor->i2caddr, cmd, cmd));
+    return false;
+  }
+  LOG(LL_DEBUG, ("I2C=0x%02x cmd=%u (0x%04x) write success", sensor->i2caddr, cmd, cmd));
+
+  return true;
+}
+
+static uint8_t crc8(const uint8_t *data, int len)
+{
+  const uint8_t POLYNOMIAL=0x31;
+  uint8_t crc=0xFF;
+
+  for (int j=len; j; --j ) {
+    crc ^= *data++;
+    for ( int i = 8; i; --i )
+      crc = ( crc & 0x80 ) ? (crc << 1) ^ POLYNOMIAL : (crc << 1);
+  }
+  return crc;
+}
+// Private functions end
+
+// Public functions follow
+struct mgos_sht31 *mgos_sht31_create(struct mgos_i2c *i2c, uint8_t i2caddr) {
+  struct mgos_sht31 *sensor;
+
+  if (!i2c) return NULL;
+
+  sensor=calloc(1, sizeof(struct mgos_sht31));
+  if (!sensor) return NULL;
+  sensor->i2caddr=i2caddr;
+  sensor->i2c=i2c;
+  sensor->last_read_time=0;
+
+  mgos_sht31_cmd(sensor, MGOS_SHT31_SOFTRESET);
+  return sensor;
+}
+
+void mgos_sht31_destroy(struct mgos_sht31 **sensor) {
+  if (!*sensor) return;
+  free (*sensor);
+  *sensor=NULL;
+  return;
+}
+
+bool mgos_sht31_read(struct mgos_sht31 *sensor) {
+  double now = mg_time();
+
+  if (!sensor || !sensor->i2c)
+    return false;
+
+  if (now - sensor->last_read_time < MGOS_SHT31_READ_DELAY) {
+    return true;
+  }
+  // Read out sensor data here 
+  //
+  uint8_t data[6];
+  float humidity, temperature;
+
+  mgos_sht31_cmd(sensor, MGOS_SHT31_MEAS_HIGHREP);
+
+  mgos_usleep(500);
+
+  if (!mgos_i2c_read(sensor->i2c, sensor->i2caddr, data, 6, true))
+    return false;
+
+  // Check CRC8 checksums
+  if ((data[2]!=crc8(data, 2)) || (data[5]!=crc8(data+3, 2)))
+    return false;
+
+  temperature = data[0]*256 + data[1];
+  temperature *= 175;
+  temperature /= 0xffff;
+  temperature -= 45;
+  sensor->temperature = temperature;
+
+  humidity = data[3] * 256 + data[4];
+  humidity *= 100;
+  humidity /= 0xFFFF;
+  sensor->humidity = humidity;
+
+  LOG(LL_DEBUG, ("temperature=%.2fC humidity=%.1f%%", sensor->temperature, sensor->humidity));
+  sensor->last_read_time=now;
+  return true;
+}
+
+float mgos_sht31_getTemperature(struct mgos_sht31 *sensor) {
+  if (!mgos_sht31_read(sensor)) return NAN;
+
+  return sensor->temperature;
+}
+
+float mgos_sht31_getHumidity(struct mgos_sht31 *sensor) {
+  if (!mgos_sht31_read(sensor)) return NAN;
+
+  return sensor->humidity;
+}
+
+bool mgos_sht31_i2c_init(void) {
+  return true;
+}
+// Public functions end
diff --git a/src/mgos_sht31_internal.h b/src/mgos_sht31_internal.h
new file mode 100644
index 0000000..7340dad
--- /dev/null
+++ b/src/mgos_sht31_internal.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include "mgos.h"
+#include "mgos_i2c.h"
+#include "mgos_sht31.h"
+#include <math.h>
+
+#define MGOS_SHT31_DEFAULT_I2CADDR       (0x44)
+
+#define MGOS_SHT31_MEAS_HIGHREP_STRETCH  (0x2C06)
+#define MGOS_SHT31_MEAS_MEDREP_STRETCH   (0x2C0D)
+#define MGOS_SHT31_MEAS_LOWREP_STRETCH   (0x2C10)
+#define MGOS_SHT31_MEAS_HIGHREP          (0x2400)
+#define MGOS_SHT31_MEAS_MEDREP           (0x240B)
+#define MGOS_SHT31_MEAS_LOWREP           (0x2416)
+#define MGOS_SHT31_READSTATUS            (0xF32D)
+#define MGOS_SHT31_CLEARSTATUS           (0x3041)
+#define MGOS_SHT31_SOFTRESET             (0x30A2)
+#define MGOS_SHT31_HEATEREN              (0x306D)
+#define MGOS_SHT31_HEATERDIS             (0x3066)
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct mgos_sht31 {
+  struct mgos_i2c *i2c;
+  uint8_t i2caddr;
+  double last_read_time;
+
+  float humidity, temperature;
+};
+
+#ifdef __cplusplus
+}
+#endif