diff --git a/cores/arduino/i2c.c b/cores/arduino/i2c.c
index 164a7562..6b1b0aff 100644
--- a/cores/arduino/i2c.c
+++ b/cores/arduino/i2c.c
@@ -48,7 +48,8 @@ static void ss_i2c_err(uint32_t dev_id)
 	i2c_err_source = dev_id;
 }
 
-static int wait_rx_or_err(bool no_stop){
+static int wait_rx_or_err()
+{
 	uint64_t timeout = TIMEOUT_MS * 200;
 	while(timeout--) {
 		if (i2c_err_detect) {
@@ -65,20 +66,17 @@ static int wait_rx_or_err(bool no_stop){
 				return I2C_ERROR_OTHER; // other error
 			}
 		}
-		if (!no_stop) {
-			if (i2c_rx_complete) {
-				return I2C_OK;
-			}
+		if (i2c_rx_complete) {
+			return I2C_OK;
 		}
 		delayMicroseconds(10);
 	}
-	if (!no_stop)
-		return I2C_TIMEOUT;
-	else
-		return I2C_OK;
+
+	return I2C_TIMEOUT;
 }
 
-static int wait_tx_or_err(bool no_stop){
+static int wait_tx_or_err()
+{
 	uint64_t timeout = TIMEOUT_MS * 200;
 	while(timeout--) {
 		if (i2c_err_detect) {
@@ -95,17 +93,12 @@ static int wait_tx_or_err(bool no_stop){
 				return I2C_ERROR_OTHER; // other error
 			}
 		}
-		if (!no_stop) {
-			if (i2c_tx_complete) {
+		if (i2c_tx_complete) {
 			return I2C_OK;
-			}
 		}
 		delayMicroseconds(10);
 	}
-	if (!no_stop)
-		return I2C_TIMEOUT;
-	else
-		return I2C_OK;
+	return I2C_TIMEOUT;
 }
 
 static int wait_dev_ready(I2C_CONTROLLER controller_id, bool no_stop){
@@ -116,9 +109,13 @@ static int wait_dev_ready(I2C_CONTROLLER controller_id, bool no_stop){
 		if (ret == I2C_OK) {
 			return I2C_OK;
 		}
-		if (ret == I2C_BUSY) {
+		else if (ret == I2C_BUSY) {
 			delayMicroseconds(10);
 		}
+		else
+		{
+			return I2C_TIMEOUT - ret;
+		}
 	}
 	return I2C_TIMEOUT - ret;
 }
@@ -202,7 +199,7 @@ int i2c_writebytes(uint8_t *bytes, uint8_t length, bool no_stop)
 	i2c_err_detect = 0;
 	i2c_err_source = 0;
 	ss_i2c_transfer(I2C_SENSING_0, bytes, length, 0, 0, i2c_slave, no_stop);
-	ret = wait_tx_or_err(no_stop);
+	ret = wait_tx_or_err();
 	if (ret)
 		return ret;
 	ret = wait_dev_ready(I2C_SENSING_0, no_stop);
@@ -219,7 +216,7 @@ int i2c_readbytes(uint8_t *buf, int length, bool no_stop)
 	i2c_err_detect = 0;
 	i2c_err_source = 0;
 	ss_i2c_transfer(I2C_SENSING_0, 0, 0, buf, length, i2c_slave, no_stop);
-	ret = wait_rx_or_err(no_stop);
+	ret = wait_rx_or_err();
 	if (ret)
 		return ret;
 	ret = wait_dev_ready(I2C_SENSING_0, no_stop);
diff --git a/libraries/Wire/examples/bus_scan/bus_scan.ino b/libraries/Wire/examples/bus_scan/bus_scan.ino
index 11e39373..e816ea79 100644
--- a/libraries/Wire/examples/bus_scan/bus_scan.ino
+++ b/libraries/Wire/examples/bus_scan/bus_scan.ino
@@ -42,6 +42,7 @@ void setup()
   Wire.begin();
 
   Serial.begin(115200);
+  while(!Serial);
 }
 
 boolean toggle = false;          // state of the LED
diff --git a/system/libarc32_arduino101/drivers/i2c_priv.h b/system/libarc32_arduino101/drivers/i2c_priv.h
index d6bb585f..ba6dc142 100644
--- a/system/libarc32_arduino101/drivers/i2c_priv.h
+++ b/system/libarc32_arduino101/drivers/i2c_priv.h
@@ -133,6 +133,7 @@ typedef struct i2c_info {
     uint8_t         *i2c_write_buff;
     uint8_t         *i2c_read_buff;
     uint8_t         restart;
+    uint8_t         send_stop;
 
     /* Callbacks */
     IO_CB_FUNC      tx_cb;
diff --git a/system/libarc32_arduino101/drivers/ss_dw_i2c.c b/system/libarc32_arduino101/drivers/ss_dw_i2c.c
index 7f4b3b03..6f43bc78 100644
--- a/system/libarc32_arduino101/drivers/ss_dw_i2c.c
+++ b/system/libarc32_arduino101/drivers/ss_dw_i2c.c
@@ -92,7 +92,7 @@ static void recv_data(i2c_info_pt dev)
 }
 
 
-void i2c_fill_fifo(i2c_info_pt dev, bool no_stop)
+void i2c_fill_fifo(i2c_info_pt dev)
 {
     uint32_t      i, tx_cnt, data;
 
@@ -114,7 +114,7 @@ void i2c_fill_fifo(i2c_info_pt dev, bool no_stop)
 
             if( dev->tx_len == 1)
             {  // last byte to write
-                if (! no_stop)
+                if (dev->send_stop)
                     data |= I2C_STOP_CMD;
             }
             dev->tx_len -= 1;
@@ -125,7 +125,7 @@ void i2c_fill_fifo(i2c_info_pt dev, bool no_stop)
             data = I2C_PUSH_DATA | I2C_READ_CMD;
             if (dev->rx_tx_len == 1)
             { // last dummy byte to write
-                if(! no_stop)
+                if(dev->send_stop)
                     data |= I2C_STOP_CMD;
                 if(dev->restart)
                 {
@@ -143,13 +143,17 @@ void i2c_fill_fifo(i2c_info_pt dev, bool no_stop)
 static void xmit_data(i2c_info_pt dev)
 {
     int mask;
-    i2c_fill_fifo(dev, false);
+    i2c_fill_fifo(dev);
     if (dev->rx_tx_len <= 0)
     {
         mask = REG_READ( I2C_INTR_MASK );
         mask &= ~(R_TX_EMPTY);
         mask |= R_STOP_DETECTED;
         REG_WRITE(I2C_INTR_MASK, mask);
+        if ((dev->rx_len == 0) && (!dev->send_stop))
+        {
+            end_data_transfer( dev );
+        }
     }
 }
 
diff --git a/system/libarc32_arduino101/drivers/ss_dw_i2c.h b/system/libarc32_arduino101/drivers/ss_dw_i2c.h
index b5dd100c..f40f822b 100644
--- a/system/libarc32_arduino101/drivers/ss_dw_i2c.h
+++ b/system/libarc32_arduino101/drivers/ss_dw_i2c.h
@@ -39,7 +39,7 @@
 #define SS_DW_I2C_H_
 #include <stdbool.h>
 void i2c_mst_err_ISR_proc(i2c_info_pt dev);
-void i2c_fill_fifo(i2c_info_pt dev, bool no_stop);
+void i2c_fill_fifo(i2c_info_pt dev);
 void i2c_mst_rx_avail_ISR_proc(i2c_info_pt dev);
 void i2c_mst_tx_req_ISR_proc(i2c_info_pt dev);
 void i2c_mst_stop_detected_ISR_proc(i2c_info_pt dev);
diff --git a/system/libarc32_arduino101/drivers/ss_i2c_iface.c b/system/libarc32_arduino101/drivers/ss_i2c_iface.c
index 200ed273..199bdc8f 100644
--- a/system/libarc32_arduino101/drivers/ss_i2c_iface.c
+++ b/system/libarc32_arduino101/drivers/ss_i2c_iface.c
@@ -347,6 +347,15 @@ DRIVER_API_RC ss_i2c_transfer(I2C_CONTROLLER controller_id, uint8_t *data_write,
         return DRV_RC_FAIL;
     }
 
+    if (!no_stop)
+    {
+        dev->send_stop = true;
+    }
+    else
+    {
+        dev->send_stop = false;
+    }
+
     if ((data_read_len == 0) && (data_write_len == 0))
     {
         //Workaround: we know that we are doing I2C bus scan.
@@ -376,7 +385,7 @@ DRIVER_API_RC ss_i2c_transfer(I2C_CONTROLLER controller_id, uint8_t *data_write,
     dev->i2c_read_buff = data_read;
     dev->total_read_bytes = 0;
     dev->total_write_bytes = 0;
-    i2c_fill_fifo(dev, no_stop);
+    i2c_fill_fifo(dev);
 
     interrupt_unlock(saved);