From b10f9c4e1515dbce1a62b342c4c03bc276767cb6 Mon Sep 17 00:00:00 2001
From: Gwenhael Goavec-Merou <gwenhael.goavec-merou@trabucayre.com>
Date: Wed, 15 Jan 2020 08:15:15 +0100
Subject: [PATCH] ftdipp_mpsse: mpsse_store: simplify and improve method

---
 ftdipp_mpsse.cpp | 37 +++++++++++++++++++++++--------------
 1 file changed, 23 insertions(+), 14 deletions(-)

diff --git a/ftdipp_mpsse.cpp b/ftdipp_mpsse.cpp
index 47ad2d7..2c2b610 100644
--- a/ftdipp_mpsse.cpp
+++ b/ftdipp_mpsse.cpp
@@ -249,27 +249,36 @@ int FTDIpp_MPSSE::mpsse_store(unsigned char c)
 int FTDIpp_MPSSE::mpsse_store(unsigned char *buff, int len)
 {
 	unsigned char *ptr = buff;
-	/* this case theorically never happen */
-	if (len > _buffer_size) {
-		mpsse_write();
-		for (; len > _buffer_size; len -= _buffer_size) {
-			memcpy(_buffer, ptr, _buffer_size);
+	int store_size;
+	/* check if _buffer as space to store all */
+	if (_num + len > _buffer_size) {
+		/* flush buffer if already full */
+		if (_num == _buffer_size)
 			mpsse_write();
-			ptr += _buffer_size;
+		/* loop until loop < _buffer_size */
+		while (_num + len > _buffer_size) {
+			/* we now have len enough to fill
+			 * buffer -> just complete buffer
+			 */
+			store_size = _buffer_size - _num;
+			memcpy(_buffer + _num, ptr, store_size);
+			_num += store_size;
+			if (mpsse_write() < 0) {
+				cout << "write_data error in " << __func__ << endl;
+				return -1;
+			}
+			ptr += store_size;
+			len -= store_size;
 		}
-	}
 
-	if (_num + len + 1 >= _buffer_size) {
-		if (mpsse_write() < 0) {
-			cout << "write_data error in " << __func__ << endl;
-			return -1;
-		}
 	}
 #ifdef DEBUG
 	display("%s %d %d\n", __func__, _num, len);
 #endif
-	memcpy(_buffer + _num, ptr, len);
-	_num += len;
+	if (len > 0) {
+		memcpy(_buffer + _num, ptr, len);
+		_num += len;
+	}
 	return 0;
 }