Support reflashing on bootloader version 8.

5 files changed, 331 insertions, 93 deletions

diff --git a/rmi4update/firmware_image.cpp b/rmi4update/firmware_image.cpp
index f1eec2a..303eb88 100644..100755
--- a/rmi4update/firmware_image.cpp
+++ b/rmi4update/firmware_image.cpp
@@ -77,6 +77,10 @@ void FirmwareImage::ParseHierarchicalImg()
 			m_firmwareData = content;
 			m_firmwareSize = length;
 			break;
+		case FLASH_CONFIG_CONTAINER:
+			m_flashConfigData = content;
+			m_flashConfigSize = length;
+			break;
 		case UI_CONFIG_CONTAINER:
 		case CORE_CONFIG_CONTAINER:
 			m_configData = content;
diff --git a/rmi4update/firmware_image.h b/rmi4update/firmware_image.h
index bba6e0d..3ce1c59 100644..100755
--- a/rmi4update/firmware_image.h
+++ b/rmi4update/firmware_image.h
@@ -101,9 +101,11 @@ public:
 					unsigned long deviceConfigSize);
 	unsigned char * GetFirmwareData() { return m_firmwareData; }
 	unsigned char * GetConfigData() { return m_configData; }
+	unsigned char * GetFlashConfigData() { return m_flashConfigData; }
 	unsigned char * GetLockdownData() { return m_lockdownData; }
 	unsigned long GetFirmwareSize() { return m_firmwareSize; }
 	unsigned long GetConfigSize() { return m_configSize; }
+	unsigned long GetFlashConfigSize() { return m_flashConfigSize; }
 	unsigned long GetLockdownSize() { return m_lockdownSize; }
 	unsigned long GetFirmwareID() { return m_firmwareBuildID; }
 	bool HasIO() { return m_io; }
@@ -118,6 +120,7 @@ private:
 	unsigned long m_checksum;
 	unsigned long m_firmwareSize;
 	unsigned long m_configSize;
+	unsigned long m_flashConfigSize;
 	unsigned long m_lockdownSize;
 	long m_imageSize;
 	unsigned long m_firmwareBuildID;
@@ -129,6 +132,7 @@ private:
 
 	unsigned char * m_firmwareData;
 	unsigned char * m_configData;
+	unsigned char * m_flashConfigData;
 	unsigned char * m_lockdownData;
 	unsigned char * m_memBlock;
 	unsigned long m_cntrAddr;	// BL_V7
diff --git a/rmi4update/main.cpp b/rmi4update/main.cpp
index bf30cb9..bf30cb9 100644..100755
--- a/rmi4update/main.cpp
+++ b/rmi4update/main.cpp
diff --git a/rmi4update/rmi4update.cpp b/rmi4update/rmi4update.cpp
index 7bb4f4b..3e7c965 100644..100755
--- a/rmi4update/rmi4update.cpp
+++ b/rmi4update/rmi4update.cpp
@@ -59,6 +59,7 @@
 
 #define RMI_F34_ENABLE_WAIT_MS 300
 #define RMI_F34_ERASE_WAIT_MS (5 * 1000)
+#define RMI_F34_ERASE_V8_WAIT_MS (10000)
 #define RMI_F34_IDLE_WAIT_MS 500
 
 /* Most recent device status event */
@@ -68,6 +69,9 @@
 /* The device has lost its configuration for some reason. */
 #define RMI_F01_STATUS_UNCONFIGURED(status)	(!!((status) & 0x80))
 
+/* Indicates that flash programming is enabled V7(bootloader mode). */
+#define RMI_F01_STATUS_BOOTLOADER_v7(status) (!!((status) & 0x80))
+
 /*
  * Sleep mode controls power management on the device and affects all
  * functions of the device.
@@ -92,7 +96,6 @@ int RMI4Update::UpdateFirmware(bool force, bool performLockdown)
 	long long int duration_us = 0;
 	int rc;
 	const unsigned char eraseAll = RMI_F34_ERASE_ALL;
-
 	rc = FindUpdateFunctions();
 	if (rc != UPDATE_SUCCESS)
 		return rc;
@@ -120,44 +123,59 @@ int RMI4Update::UpdateFirmware(bool force, bool performLockdown)
 	rc = ReadF34Queries();
 	if (rc != UPDATE_SUCCESS)
 		return rc;
-
 	rc = m_firmwareImage.VerifyImageMatchesDevice(GetFirmwareSize(), GetConfigSize());
 	if (rc != UPDATE_SUCCESS)
 		return rc;
 
 	if (m_f34.GetFunctionVersion() == 0x02) {
+		fprintf(stdout, "Enable Flash V7+...\n");
 		rc = EnterFlashProgrammingV7();
 		if (rc != UPDATE_SUCCESS) {
 			fprintf(stderr, "%s: %s\n", __func__, update_err_to_string(rc));
 			goto reset;
 		}
-
-		fprintf(stdout, "Erasing FW V7...\n");
-		rc = EraseFirmwareV7();
-		if (rc != UPDATE_SUCCESS) {
-			fprintf(stderr, "%s: %s\n", __func__, update_err_to_string(rc));
-			goto reset;
+		fprintf(stdout, "Enable Flash done V7+...\n");
+
+        if (!m_IsErased){
+            fprintf(stdout, "Erasing FW V7+...\n");
+            rc = EraseFirmwareV7();
+            if (rc != UPDATE_SUCCESS) {
+                fprintf(stderr, "%s: %s\n", __func__, update_err_to_string(rc));
+                goto reset;
+            }
+            fprintf(stdout, "Erasing FW done V7+...\n");
+        }
+		if(m_bootloaderID[1] == 8){
+			if (m_firmwareImage.GetFlashConfigData()) {
+				fprintf(stdout, "Writing flash configuration V8...\n");
+				rc = WriteFlashConfigV7();
+				if (rc != UPDATE_SUCCESS) {
+					fprintf(stderr, "%s: %s\n", __func__, update_err_to_string(rc));
+					goto reset;
+				}
+				fprintf(stdout, "Writing flash config done V8...\n");
+			}
 		}
-		fprintf(stdout, "Erasing FW done V7...\n");
 		if (m_firmwareImage.GetFirmwareData()) {
-			fprintf(stdout, "Writing firmware V7...\n");
+			fprintf(stdout, "Writing firmware V7+...\n");
 			rc = WriteFirmwareV7();
 			if (rc != UPDATE_SUCCESS) {
 				fprintf(stderr, "%s: %s\n", __func__, update_err_to_string(rc));
 				goto reset;
 			}
-			fprintf(stdout, "Writing firmware done V7...\n");
+			fprintf(stdout, "Writing firmware done V7+...\n");
 		}
 		if (m_firmwareImage.GetConfigData()) {
-			fprintf(stdout, "Writing configuration V7...\n");
-			rc = WriteConfigV7();
+			fprintf(stdout, "Writing core configuration V7+...\n");
+			rc = WriteCoreConfigV7();
 			if (rc != UPDATE_SUCCESS) {
 				fprintf(stderr, "%s: %s\n", __func__, update_err_to_string(rc));
 				goto reset;
 			}
-			fprintf(stdout, "Writing config done V7...\n");
+			fprintf(stdout, "Writing core config done V7+...\n");
 			goto reset;
 		}
+		
 	} else {
 		rc = EnterFlashProgramming();
 		if (rc != UPDATE_SUCCESS) {
@@ -249,6 +267,18 @@ rebind:
 	{
 		goto rebind;
 	}
+
+	// In order to print out new PR
+	rc = FindUpdateFunctions();
+	if (rc != UPDATE_SUCCESS)
+		return rc;
+
+	rc = m_device.QueryBasicProperties();
+	if (rc < 0)
+		return UPDATE_FAIL_QUERY_BASIC_PROPERTIES;
+	fprintf(stdout, "Device Properties:\n");
+	m_device.PrintProperties();
+
 	return rc;
 
 }
@@ -683,12 +713,21 @@ int RMI4Update::WriteFirmwareV7()
 			free(data_temp);
 		} while (left_bytes);
 
+		// Sleep 100 ms and wait for attention.
+		Sleep(100);
+		rc = WaitForIdle(RMI_F34_IDLE_WAIT_MS, false);
+		if (rc != UPDATE_SUCCESS) {
+			fprintf(stderr, "%s: %s\n", __func__, update_err_to_string(rc));
+			return UPDATE_FAIL_TIMEOUT_WAITING_FOR_ATTN;
+		}
+        
 		//Wait for completion
 		do {
 			Sleep(20);
 			rmi4update_poll();
 			if (m_flashStatus == SUCCESS){
 				break;
+
 			}
 			retry++;
 		} while(retry < 20);
@@ -702,7 +741,7 @@ int RMI4Update::WriteFirmwareV7()
 	return UPDATE_SUCCESS;
 }
 
-int RMI4Update::WriteConfigV7()
+int RMI4Update::WriteCoreConfigV7()
 {
 	int transaction_count, remain_block;
 	int transfer_leng = 0;
@@ -777,6 +816,119 @@ int RMI4Update::WriteConfigV7()
 			memcpy(data_temp, m_firmwareImage.GetConfigData() + offset, sizeof(char) * write_size);
 			rc = m_device.Write(dataAddr + 5, data_temp, sizeof(char) * write_size);
 			if (rc != ((ssize_t)sizeof(char) * write_size)) {
+				return UPDATE_FAIL_READ_F34_QUERIES;
+			}
+
+			offset += write_size;
+			left_bytes -= write_size;
+			free(data_temp);
+		} while (left_bytes);
+
+		// Wait for attention.
+		rc = WaitForIdle(RMI_F34_IDLE_WAIT_MS, false);
+		if (rc != UPDATE_SUCCESS) {
+			fprintf(stderr, "%s: %s\n", __func__, update_err_to_string(rc));
+			return UPDATE_FAIL_TIMEOUT_WAITING_FOR_ATTN;
+		}
+
+		//Wait for completion
+		do {
+			Sleep(20);
+			rmi4update_poll();
+			if (m_flashStatus == SUCCESS){
+				break;
+			}
+			retry++;
+		} while(retry < 20);
+
+		if (m_flashStatus != SUCCESS) {
+			fprintf(stdout, "err flash_status = %d\n", m_flashStatus);
+			return UPDATE_FAIL_WRITE_F01_CONTROL_0;
+		}
+
+	}
+	return UPDATE_SUCCESS;
+}
+
+int RMI4Update::WriteFlashConfigV7()
+{
+	int transaction_count, remain_block;
+	int transfer_leng = 0;
+	int offset = 0;
+	unsigned char trans_leng_buf[2];
+	unsigned char cmd_buf[1];
+	unsigned char off[2] = {0, 0};
+	unsigned char partition_id;
+	unsigned short dataAddr = m_f34.GetDataBase();
+	unsigned short left_bytes;
+	unsigned short write_size;
+	unsigned short max_write_size;
+	int rc;
+	int i;
+	int retry = 0;
+	unsigned char *data_temp;
+	unsigned short FlashConfigBlockCount;
+
+	/* calculate the count */
+	partition_id = FLASH_CONFIG_PARTITION;
+
+    FlashConfigBlockCount = m_firmwareImage.GetFlashConfigSize() / m_blockSize;
+
+	remain_block = (FlashConfigBlockCount % m_payloadLength);
+	transaction_count = (FlashConfigBlockCount / m_payloadLength);
+	if (remain_block > 0)
+		transaction_count++;
+
+	/* set partition id for bootloader 7 */
+	rc = m_device.Write(dataAddr + 1, &partition_id, sizeof(partition_id));
+	if (rc != sizeof(partition_id))
+		return UPDATE_FAIL_WRITE_FLASH_COMMAND;
+
+	rc = m_device.Write(dataAddr + 2, off, sizeof(off));
+	if (rc != sizeof(off))
+		return UPDATE_FAIL_WRITE_INITIAL_ZEROS;
+
+	for (i = 0; i < transaction_count; i++)
+	{
+		if ((i == (transaction_count -1)) && (remain_block > 0))
+			transfer_leng = remain_block;
+		else
+			transfer_leng = m_payloadLength;
+
+		// Set Transfer Length
+		trans_leng_buf[0] = (unsigned char)(transfer_leng & 0xFF);
+		trans_leng_buf[1] = (unsigned char)((transfer_leng & 0xFF00) >> 8);
+
+		rc = m_device.Write(dataAddr + 3, trans_leng_buf, sizeof(trans_leng_buf));
+		if (rc != sizeof(trans_leng_buf))
+			return UPDATE_FAIL_WRITE_FLASH_COMMAND;
+
+		// Set Command to Write
+		cmd_buf[0] = (unsigned char)CMD_V7_WRITE;
+		rc = m_device.Write(dataAddr + 4, cmd_buf, sizeof(cmd_buf));
+		if (rc != sizeof(cmd_buf))
+			return UPDATE_FAIL_WRITE_FLASH_COMMAND;
+
+		max_write_size = 16;
+		if (max_write_size >= transfer_leng * m_blockSize)
+			max_write_size = transfer_leng * m_blockSize;
+		else if (max_write_size > m_blockSize)
+			max_write_size -= max_write_size % m_blockSize;
+		else
+			max_write_size = m_blockSize;
+
+		left_bytes = transfer_leng * m_blockSize;
+
+		do {
+			if (left_bytes / max_write_size)
+				write_size = max_write_size;
+			else
+				write_size = left_bytes;
+
+			data_temp = (unsigned char *) malloc(sizeof(unsigned char) * write_size);
+			memcpy(data_temp, m_firmwareImage.GetFlashConfigData() + offset, sizeof(char) * write_size);
+			rc = m_device.Write(dataAddr + 5, data_temp, sizeof(char) * write_size);
+			if (rc != ((ssize_t)sizeof(char) * write_size)) {
 				fprintf(stdout, "err write_size = %d; rc = %d\n", write_size, rc);
 				return UPDATE_FAIL_READ_F34_QUERIES;
 			}
@@ -786,6 +938,13 @@ int RMI4Update::WriteConfigV7()
 			free(data_temp);
 		} while (left_bytes);
 
+		// Wair for attention.
+		rc = WaitForIdle(RMI_F34_IDLE_WAIT_MS, false);
+		if (rc != UPDATE_SUCCESS) {
+			fprintf(stderr, "%s: %s\n", __func__, update_err_to_string(rc));
+			return UPDATE_FAIL_TIMEOUT_WAITING_FOR_ATTN;
+		}
+
 		//Wait for completion
 		do {
 			Sleep(20);
@@ -816,20 +975,45 @@ int RMI4Update::EraseFirmwareV7()
 	/* write bootloader id */
 	erase_cmd[6] = m_bootloaderID[0];
 	erase_cmd[7] = m_bootloaderID[1];
-	/* Set Command to Erase */
-	erase_cmd[5] = (unsigned char)CMD_V7_ERASE;
+	if(m_bootloaderID[1] == 8){
+	    /* Set Command to Erase AP for BL8*/
+	    erase_cmd[5] = (unsigned char)CMD_V7_ERASE_AP;
+	} else {
+	    /* Set Command to Erase AP for BL7*/
+	    erase_cmd[5] = (unsigned char)CMD_V7_ERASE;
+	}
+	
+	fprintf(stdout, "Erase command : ");
+    for(int i = 0 ;i<8;i++){
+       fprintf(stdout, "%d ", erase_cmd[i]);
+	}
+	fprintf(stdout, "\n");
 
-	rmi4update_poll();
-	if (!m_inBLmode)
-		return UPDATE_FAIL_DEVICE_NOT_IN_BOOTLOADER;
+    rmi4update_poll();
+    if (!m_inBLmode)
+        return UPDATE_FAIL_DEVICE_NOT_IN_BOOTLOADER;
+    if(m_bootloaderID[1] == 8){
+		// For BL8 device, we need hold 1 seconds after querying
+		// F34 status to avoid not get attention by following giving 
+		// erase command.
+        Sleep(1000);
+    }
 
 	rc = m_device.Write(m_f34.GetDataBase() + 1, erase_cmd, sizeof(erase_cmd));
 	if (rc != sizeof(erase_cmd))
 		return UPDATE_FAIL_WRITE_F01_CONTROL_0;
 
-	Sleep(100);
+    Sleep(100);
 
 	//Wait from ATTN
+	if(m_bootloaderID[1] == 8){
+		// Wait for attention for BL8 device.
+	    rc = WaitForIdle(RMI_F34_ERASE_V8_WAIT_MS, false);
+        if (rc != UPDATE_SUCCESS) {
+	        fprintf(stderr, "%s: %s\n", __func__, update_err_to_string(rc));
+	        return UPDATE_FAIL_TIMEOUT_WAITING_FOR_ATTN;
+	    }
+    }
 	do {
 		Sleep(20);
 		rmi4update_poll();
@@ -843,39 +1027,47 @@ int RMI4Update::EraseFirmwareV7()
 		fprintf(stdout, "err flash_status = %d\n", m_flashStatus);
 		return UPDATE_FAIL_WRITE_F01_CONTROL_0;
 	}
+ 
+    if(m_bootloaderID[1] == 7){
+		// For BL7, we need erase config partition.
+		fprintf(stdout, "Start to erase config\n");
+		erase_cmd[0] = CORE_CONFIG_PARTITION;
+		erase_cmd[6] = m_bootloaderID[0];
+	    erase_cmd[7] = m_bootloaderID[1];
+	    erase_cmd[5] = (unsigned char)CMD_V7_ERASE;
+
+	    Sleep(100);
+	    rmi4update_poll();
+	    if (!m_inBLmode)
+		  return UPDATE_FAIL_DEVICE_NOT_IN_BOOTLOADER;
+
+	    rc = m_device.Write(m_f34.GetDataBase() + 1, erase_cmd, sizeof(erase_cmd));
+	    if (rc != sizeof(erase_cmd))
+		  return UPDATE_FAIL_WRITE_F01_CONTROL_0;
+
+		//Wait from ATTN
+		Sleep(100);
+
+		rc = WaitForIdle(RMI_F34_ERASE_WAIT_MS, true);
+		if (rc != UPDATE_SUCCESS) {
+			fprintf(stderr, "%s: %s\n", __func__, update_err_to_string(rc));
+			return UPDATE_FAIL_TIMEOUT_WAITING_FOR_ATTN;
+		}
 
-	fprintf(stdout, "Star to erase config\n");
-	/* set partition id for bootloader 7 */
-	erase_cmd[0] = CORE_CONFIG_PARTITION;
-	/* write bootloader id */
-	erase_cmd[6] = m_bootloaderID[0];
-	erase_cmd[7] = m_bootloaderID[1];
-	/* Set ¡¥Command¡Š to ¡¥Erase¡Š */
-	erase_cmd[5] = (unsigned char)CMD_V7_ERASE;
-
-	Sleep(100);
-	rmi4update_poll();
-	if (!m_inBLmode)
-		return UPDATE_FAIL_DEVICE_NOT_IN_BOOTLOADER;
-
-	rc = m_device.Write(m_f34.GetDataBase() + 1, erase_cmd, sizeof(erase_cmd));
-	if (rc != sizeof(erase_cmd))
-		return UPDATE_FAIL_WRITE_F01_CONTROL_0;
 
-	//Wait from ATTN
-	Sleep(100);
-	do {
-		Sleep(20);
-		rmi4update_poll();
-		if (m_flashStatus == SUCCESS){
-			break;
-		}
-		retry++;
-	} while(retry < 20);
+	    do {
+		    Sleep(20);
+		    rmi4update_poll();
+		    if (m_flashStatus == SUCCESS){
+			  break;
+		    }
+		    retry++;
+	    } while(retry < 20);
 
-	if (m_flashStatus != SUCCESS) {
-		fprintf(stdout, "err flash_status = %d\n", m_flashStatus);
-		return UPDATE_FAIL_WRITE_F01_CONTROL_0;
+	    if (m_flashStatus != SUCCESS) {
+		    fprintf(stdout, "err flash_status = %d\n", m_flashStatus);
+		    return UPDATE_FAIL_WRITE_F01_CONTROL_0;
+	    }
 	}
 
 	return UPDATE_SUCCESS;
@@ -884,45 +1076,67 @@ int RMI4Update::EraseFirmwareV7()
 int RMI4Update::EnterFlashProgrammingV7()
 {
 	int rc;
-	unsigned char EnterCmd[8] = {0, 0, 0, 0, 0, 0, 0, 0};
-	int retry = 0;
+	unsigned char f34_status;
+	rc = m_device.Read(m_f34.GetDataBase(), &f34_status, sizeof(unsigned char));
+	m_inBLmode = f34_status & 0x80;
+	if(!m_inBLmode){
+		fprintf(stdout, "Not in BL mode, going to BL mode...\n");
+		unsigned char EnterCmd[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+		int retry = 0;
 
-	/* set partition id for bootloader 7 */
-	EnterCmd[0] = BOOTLOADER_PARTITION;
+		/* set partition id for bootloader 7 */
+		EnterCmd[0] = BOOTLOADER_PARTITION;
 
-	/* write bootloader id */
-	EnterCmd[6] = m_bootloaderID[0];
-	EnterCmd[7] = m_bootloaderID[1];
+		/* write bootloader id */
+		EnterCmd[6] = m_bootloaderID[0];
+		EnterCmd[7] = m_bootloaderID[1];
 
-	// Set Command to EnterBL
-	EnterCmd[5] = (unsigned char)CMD_V7_ENTER_BL;
+		// Set Command to EnterBL
+		EnterCmd[5] = (unsigned char)CMD_V7_ENTER_BL;
 
-	rc = m_device.Write(m_f34.GetDataBase() + 1, EnterCmd, sizeof(EnterCmd));
-	if (rc != sizeof(EnterCmd))
-		return UPDATE_FAIL_WRITE_F01_CONTROL_0;
+		rc = m_device.Write(m_f34.GetDataBase() + 1, EnterCmd, sizeof(EnterCmd));
+		if (rc != sizeof(EnterCmd))
+			return UPDATE_FAIL_WRITE_F01_CONTROL_0;
 
-	//Wait from ATTN
-	do {
-		Sleep(20);
-		rmi4update_poll();
-		if (m_flashStatus == SUCCESS){
-			break;
+		rc = WaitForIdle(RMI_F34_ENABLE_WAIT_MS, false);
+		if (rc != UPDATE_SUCCESS) {
+			fprintf(stderr, "%s: %s\n", __func__, update_err_to_string(rc));
+			return UPDATE_FAIL_TIMEOUT_WAITING_FOR_ATTN;
 		}
-		retry++;
-	} while(retry < 20);
+      
+		//Wait from ATTN
+		do {
+			Sleep(20);
+			rmi4update_poll();
+			if (m_flashStatus == SUCCESS){
+				break;
+			}
+			retry++;
+		} while(retry < 20);
 
-	if (m_flashStatus != SUCCESS) {
-		fprintf(stdout, "err flash_status = %d\n", m_flashStatus);
-		return UPDATE_FAIL_WRITE_F01_CONTROL_0;
-	}
+		if (m_flashStatus != SUCCESS) {
+			fprintf(stdout, "err flash_status = %d\n", m_flashStatus);
+			return UPDATE_FAIL_WRITE_F01_CONTROL_0;
+		}
 
-	Sleep(RMI_F34_ENABLE_WAIT_MS);
+		Sleep(RMI_F34_ENABLE_WAIT_MS);
+
+		fprintf(stdout, "%s\n", __func__);
+		rmi4update_poll();
+		if (!m_inBLmode)
+			return UPDATE_FAIL_DEVICE_NOT_IN_BOOTLOADER;
 
-	fprintf(stdout, "%s\n", __func__);
-	rmi4update_poll();
-	if (!m_inBLmode)
-		return UPDATE_FAIL_DEVICE_NOT_IN_BOOTLOADER;
+		rc = FindUpdateFunctions();
+		if (rc < 0)
+			return UPDATE_FAIL_QUERY_BASIC_PROPERTIES;
 
+		rc = ReadF34QueriesV7();
+		if (rc < 0)
+			return UPDATE_FAIL_READ_F34_QUERIES;
+
+	} else
+		fprintf(stdout, "Already in BL mode, skip...\n");
+ 	
 	// workaround
 	fprintf(stdout, "Erase in BL mode\n");
 	rc = EraseFirmwareV7();
@@ -932,6 +1146,8 @@ int RMI4Update::EnterFlashProgrammingV7()
 	}
 	fprintf(stdout, "Erase in BL mode end\n");
 
+	m_IsErased = true;
+
 	m_device.RebindDriver();
 	Sleep(RMI_F34_ENABLE_WAIT_MS);
 
@@ -979,8 +1195,15 @@ int RMI4Update::EnterFlashProgramming()
 	if (rc != 1)
 		return UPDATE_FAIL_READ_DEVICE_STATUS;
 
-	if (!RMI_F01_STATUS_BOOTLOADER(m_deviceStatus))
-		return UPDATE_FAIL_DEVICE_NOT_IN_BOOTLOADER;
+	if(m_f34.GetFunctionVersion() > 0x1){
+		if (!RMI_F01_STATUS_BOOTLOADER_v7(m_deviceStatus))
+			return UPDATE_FAIL_DEVICE_NOT_IN_BOOTLOADER;
+		fprintf(stdout, "Already in BL mode V7\n");
+	} else {
+		if (!RMI_F01_STATUS_BOOTLOADER(m_deviceStatus))
+			return UPDATE_FAIL_DEVICE_NOT_IN_BOOTLOADER;
+		fprintf(stdout, "Already in BL mode\n");
+	}
 
 	rc = ReadF34Queries();
 	if (rc != UPDATE_SUCCESS)
@@ -1068,7 +1291,7 @@ int RMI4Update::WaitForIdle(int timeout_ms, bool readF34OnSucess)
 		tv.tv_usec = (timeout_ms % 1000) * 1000;
 
 		rc = m_device.WaitForAttention(&tv, m_f34.GetInterruptMask());
-		if (rc == -ETIMEDOUT)
+		if (rc == -ETIMEDOUT){
 			/*
 			 * If for some reason we are not getting attention reports for HID devices
 			 * then we can still continue after the timeout and read F34 status
@@ -1076,7 +1299,9 @@ int RMI4Update::WaitForIdle(int timeout_ms, bool readF34OnSucess)
 			 * will be slow. If this message shows up a lot then something is wrong
 			 * with receiving attention reports and that should be fixed.
 			 */
-			fprintf(stderr, "Timed out waiting for attn report\n");
+			fprintf(stderr, "RMI4Update::WaitForIdle Timed out waiting for attn report\n");
+		} else if(rc == 1)
+			fprintf(stdout, "RMI4Update::WaitForIdle Got attention!\n");
 	}
 
 	if (rc <= 0 || readF34OnSucess) {
@@ -1086,18 +1311,18 @@ int RMI4Update::WaitForIdle(int timeout_ms, bool readF34OnSucess)
 
 		if (!m_f34Status && !m_f34Command) {
 			if (!m_programEnabled) {
-				fprintf(stderr, "Bootloader is idle but program_enabled bit isn't set.\n");
+				fprintf(stderr, "RMI4Update::WaitForIdle Bootloader is idle but program_enabled bit isn't set.\n");
 				return UPDATE_FAIL_PROGRAMMING_NOT_ENABLED;
 			} else {
 				return UPDATE_SUCCESS;
 			}
 		}
-
-		fprintf(stderr, "ERROR: Waiting for idle status.\n");
-		fprintf(stderr, "Command: %#04x\n", m_f34Command);
-		fprintf(stderr, "Status:  %#04x\n", m_f34Status);
-		fprintf(stderr, "Enabled: %d\n", m_programEnabled);
-		fprintf(stderr, "Idle:    %d\n", !m_f34Command && !m_f34Status);
+		fprintf(stderr, "RMI4Update::WaitForIdle\n");
+		fprintf(stderr, "  ERROR: Waiting for idle status.\n");
+		fprintf(stderr, "  Command: %#04x\n", m_f34Command);
+		fprintf(stderr, "  Status:  %#04x\n", m_f34Status);
+		fprintf(stderr, "  Enabled: %d\n", m_programEnabled);
+		fprintf(stderr, "  Idle:    %d\n", !m_f34Command && !m_f34Status);
 
 		return UPDATE_FAIL_NOT_IN_IDLE_STATE;
 	}
diff --git a/rmi4update/rmi4update.h b/rmi4update/rmi4update.h
index f64028f..b9de4ec 100644..100755
--- a/rmi4update/rmi4update.h
+++ b/rmi4update/rmi4update.h
@@ -155,7 +155,9 @@ class RMI4Update
 public:
 	RMI4Update(RMIDevice & device, FirmwareImage & firmwareImage) : m_device(device), 
 			m_firmwareImage(firmwareImage), m_writeBlockWithCmd(true)
-	{}
+	{
+		m_IsErased = false;
+	}
 	int UpdateFirmware(bool force = false, bool performLockdown = false);
 
 private:
@@ -170,7 +172,8 @@ private:
 	int EnterFlashProgrammingV7();
 	int EraseFirmwareV7();
 	int WriteFirmwareV7();
-	int WriteConfigV7();
+	int WriteCoreConfigV7();
+	int WriteFlashConfigV7();
 	int EnterFlashProgramming();
 	int WriteBlocks(unsigned char *block, unsigned short count, unsigned char cmd);
 	int WaitForIdle(int timeout_ms, bool readF34OnSucess = true);
@@ -216,6 +219,8 @@ private:
 
 	unsigned short m_f34StatusAddr;
 	enum bl_version m_blVersion;
+
+	bool m_IsErased;
 };
 
 #endif // _RMI4UPDATE_H_