diff --git a/flowi2chelper.cpp b/flowi2chelper.cpp
index 77559b6..dfab90d 100644
--- a/flowi2chelper.cpp
+++ b/flowi2chelper.cpp
@@ -33,32 +33,19 @@
 bool I2CHelper::readFromI2C(uint8_t i2cAddress, const uint8_t* i2cCommand, uint8_t commandLength, uint8_t* data, uint8_t dataLength)
 {
   Wire.beginTransmission(i2cAddress);
-  for (int i = 0; i < commandLength; ++i) {
-    if (Wire.write(i2cCommand[i]) != 1) {
-      return false;
-    }
-  }  
-  if (Wire.endTransmission(false) != 0) {
-    Serial.println("NACK");
+  Wire.write(i2cCommand, commandLength);
+  if (Wire.endTransmission(0 == dataLength) != 0) {
     return false;
   }
 
-  Wire.requestFrom(i2cAddress, dataLength);
+  if (dataLength != 0) {
+    if (dataLength > Wire.requestFrom(i2cAddress, dataLength))
+        return false;
 
-  // there should be no reason for this to not be ready, since we're using clock stretching mode,
-  // but just in case we'll try a few times
-  uint8_t tries = 1;
-  while (Wire.available() < dataLength) {
-    delay(1);
-    if (tries++ >= MAX_READ_TRIES) {
-      return false;
+    for (int i = 0; i < dataLength; ++i) {
+      data[i] = Wire.read();
     }
   }
-  
-  for (int i = 0; i < dataLength; ++i) {
-    data[i] = Wire.read();
-  }
   return true;
 }
 
-
diff --git a/sensirionflow.cpp b/sensirionflow.cpp
index f045204..8c9a29f 100644
--- a/sensirionflow.cpp
+++ b/sensirionflow.cpp
@@ -42,32 +42,28 @@ SensirionFlow::SensirionFlow(uint8_t i2cAddress)
 {
 }
 
-void SensirionFlow::init()
+bool SensirionFlow::init()
 {
-  const uint8_t CMD_LENGTH = 1;
-  const uint8_t CMD_READ_USER_REGISTER[CMD_LENGTH] = { 0xE3 };
   const uint8_t DATA_LENGTH = 6; // 2 data, 1 crc
   uint8_t data[DATA_LENGTH];
   
   // - read user register
-  if (!I2CHelper::readFromI2C(mI2cAddress, CMD_READ_USER_REGISTER, CMD_LENGTH, data, 3)) {
-    Serial.print("Failed to read from I2C 1\n");
-    return;
+  register_value_t baseAddress;
+  if (!readRegister(user_reg, &baseAddress)) {
+      return false;
   }
-  
-  int16_t baseAddress = (data[0] << 8) + data[1];
+
   baseAddress &= 0x70; // EEPROM base address is bits <6..4>
   baseAddress >>= 4;
   baseAddress *= 0x300;
 
   // - read scale factor
-  int16_t scaleFactorAddress = (baseAddress + 0x02B6);
+  uint16_t scaleFactorAddress = (baseAddress + 0x02B6);
   scaleFactorAddress <<= 4;  // address is a left aligned 12-bit value
 
-  uint8_t cmdReadRegister[] = { 0xFA, (scaleFactorAddress >> 8), scaleFactorAddress & 0x00FF };
+  uint8_t cmdReadRegister[] = { 0xFA, (uint8_t)(scaleFactorAddress >> 8), (uint8_t)(scaleFactorAddress & 0x00FF) };
   if (!I2CHelper::readFromI2C(mI2cAddress, cmdReadRegister, 3, data, DATA_LENGTH)) {
-    Serial.print("Failed to read from I2C 2\n");
-    return;
+    return false;
   }
   mScaleFactor = (data[0] << 8) + data[1]; // data[2] = crc
 
@@ -75,6 +71,14 @@ void SensirionFlow::init()
   mDimension = measurementUnit & 0xF;
   mTimeBase = (measurementUnit >> 4) & 0xF;
   mVolumePressureUnit = (measurementUnit >> 8) & 0x1F;
+  return true;
+}
+
+void SensirionFlow::reset()
+{
+  const uint8_t CMD_LENGTH = 1;
+  const uint8_t CMD_RESET[CMD_LENGTH] = { 0xFE };
+  I2CHelper::readFromI2C(mI2cAddress, CMD_RESET, CMD_LENGTH, NULL, 0);
 }
 
 float SensirionFlow::readSample()
@@ -85,8 +89,7 @@ float SensirionFlow::readSample()
   uint8_t data[dataLength];
   
   command[0] = 0xF1;
-  if (!I2CHelper::readFromI2C(mI2cAddress, command, 1, data, dataLength)) {
-    Serial.print("Failed to read from I2C 4\n");
+  if (!I2CHelper::readFromI2C(mI2cAddress, command, cmdLength, data, dataLength)) {
     return 0;
   }
   
@@ -94,3 +97,50 @@ float SensirionFlow::readSample()
   return (measurementValue / mScaleFactor);
 }
 
+bool SensirionFlow::readRegister(register_id_t reg, register_value_t *buffer)
+{
+  const static uint8_t commands[] = { 0xE3, 0xE5, 0xE7, 0xE9 };
+  const uint8_t dataLength = 2;
+
+  if (reg >= 4) {
+    return false;
+  }
+
+  uint8_t data[dataLength];
+  
+  if (!I2CHelper::readFromI2C(mI2cAddress, &commands[reg], 1, data, dataLength)) {
+    return false;
+  }
+  
+  *buffer = (data[0] << 8) | data[1];
+  return true;
+}
+
+bool SensirionFlow::writeRegister(register_id_t reg, register_value_t data)
+{
+  const static uint8_t commands[] = { 0xE2, 0xE4 };
+  const uint8_t commandLength = 3;
+
+  if (reg >= 2){
+    return false;
+  }
+
+  uint8_t command[commandLength];
+  command[0] = commands[reg];
+  command[1] = data >> 8;
+  command[2] = data & 0x00FF;
+
+  return I2CHelper::readFromI2C(mI2cAddress, &command[0], commandLength, NULL, 0);
+}
+
+bool SensirionFlow::modifyRegister(register_id_t reg, register_value_t data, register_value_t mask)
+{
+  register_value_t value;
+  if (!readRegister(reg, &value)) {
+    return false;
+  }
+
+  value &= ~mask; // zero out bits to modify
+  value |= data & mask; // set 1-bits to modify
+  return writeRegister(reg, value);
+}
diff --git a/sensirionflow.h b/sensirionflow.h
index aa89eb5..d5d8a16 100644
--- a/sensirionflow.h
+++ b/sensirionflow.h
@@ -34,14 +34,29 @@ class SensirionFlow
 {
 public:
   SensirionFlow(uint8_t i2cAddress);
-  void init();
-  
+  bool init();
+  void reset();
+
   float readSample();
 
   uint8_t getDimension()          const { return mDimension;          };
   uint8_t getTimeBase()           const { return mTimeBase;           };
   uint8_t getVolumePressureUnit() const { return mVolumePressureUnit; };
-  
+
+  typedef enum
+  {
+	  user_reg = 0,
+	  adv_user_reg,
+	  readonly_1,
+	  readonly_2
+  } register_id_t;
+
+  typedef uint16_t register_value_t;
+
+  bool readRegister(register_id_t reg, register_value_t *buffer);
+  bool writeRegister(register_id_t reg, register_value_t data);
+  bool modifyRegister(register_id_t reg, register_value_t data, register_value_t mask);
+
 private:
   uint8_t mI2cAddress;
   int16_t mScaleFactor;