diff --git a/buspirate_spi.c b/buspirate_spi.c
index 62433d4..3006aa1 100644
--- a/buspirate_spi.c
+++ b/buspirate_spi.c
@@ -40,7 +40,7 @@
 #define serialport_shutdown(...) 0
 #define serialport_write(...) 0
 #define serialport_read(...) 0
-#define serialport_discard_read(...) 0
+#define sp_flush_incoming(...) 0
 #endif
 
 int buspirate_sendrecv(unsigned char *buf, unsigned int writecnt, unsigned int readcnt)
@@ -144,9 +144,7 @@
 		if (ret)
 			return ret;
 		/* Read any response and discard it. */
-		ret = serialport_discard_read();
-		if (ret)
-			return ret;
+		sp_flush_incoming();
 	}
 	/* Enter raw bitbang mode */
 	buf[0] = 0x00;
diff --git a/flash.h b/flash.h
index a80ec32..b47dcbc 100644
--- a/flash.h
+++ b/flash.h
@@ -598,7 +598,6 @@
 int serialport_shutdown(void);
 int serialport_write(unsigned char *buf, unsigned int writecnt);
 int serialport_read(unsigned char *buf, unsigned int readcnt);
-int serialport_discard_read(void);
 
 #include "chipdrivers.h"
 
diff --git a/serial.c b/serial.c
index be88166..754dff9 100644
--- a/serial.c
+++ b/serial.c
@@ -129,16 +129,7 @@
 
 void sp_flush_incoming(void)
 {
-	int i;
-	for (i=0;i<100;i++) { /* In case the device doesnt do EAGAIN, just read 0 */
-		unsigned char flush[16];
-		ssize_t rv;
-		rv = read(sp_fd, flush, sizeof(flush));
-		if ((rv == -1) && (errno == EAGAIN))
-			break;
-		if (rv == -1)
-			sp_die("flush read");
-	}
+	tcflush(sp_fd, TCIFLUSH);
 	return;
 }
 
@@ -152,12 +143,14 @@
 {
 	int tmp = 0;
 
-	while (tmp != writecnt) {
-		tmp = write(sp_fd, buf + tmp, writecnt - tmp);
+	while (writecnt > 0) {
+		tmp = write(sp_fd, buf, writecnt);
 		if (tmp == -1)
 			return 1;
 		if (!tmp)
 			printf_debug("Empty write\n");
+		writecnt -= tmp; 
+		buf += tmp;
 	}
 
 	return 0;
@@ -167,28 +160,15 @@
 {
 	int tmp = 0;
 
-	while (tmp != readcnt) {
-		tmp = read(sp_fd, buf + tmp, readcnt - tmp);
+	while (readcnt > 0) {
+		tmp = read(sp_fd, buf, readcnt);
 		if (tmp == -1)
 			return 1;
 		if (!tmp)
 			printf_debug("Empty read\n");
+		readcnt -= tmp;
+		buf += tmp;
 	}
 
 	return 0;
 }
-
-int serialport_discard_read(void)
-{
-	int flags;
-
-	printf_debug("%s\n", __func__);
-	flags = fcntl(sp_fd, F_GETFL);
-	flags |= O_NONBLOCK;
-	fcntl(sp_fd, F_SETFL, flags);
-	sp_flush_incoming();
-	flags &= ~O_NONBLOCK;
-	fcntl(sp_fd, F_SETFL, flags);
-
-	return 0;
-}