From 03fd10860f5d5d047494d7afdc82ad48f443d658 Mon Sep 17 00:00:00 2001
From: BlackMark <me@blackmark.me>
Date: Sat, 11 Apr 2020 19:19:23 +0200
Subject: [PATCH] Implement eeprom programming

---
 stk500v2/main.cpp | 36 +++++++++++++++++++++++++++++++++++-
 1 file changed, 35 insertions(+), 1 deletion(-)

diff --git a/stk500v2/main.cpp b/stk500v2/main.cpp
index 8daabee..8f9f6ad 100644
--- a/stk500v2/main.cpp
+++ b/stk500v2/main.cpp
@@ -134,6 +134,16 @@ static inline bool isReadEepromIsp(const Message &msg)
 	return false;
 }
 
+static inline bool isProgramEepromIsp(const Message &msg)
+{
+	if (msg.body[0] == CMD_PROGRAM_EEPROM_ISP) {
+		if (msg.size == (static_cast<uint16_t>(msg.body[1]) << 8 | msg.body[2]) + 10)
+			return true;
+	}
+
+	return false;
+}
+
 static inline bool isLeaveProgmodeIsp(const Message &msg)
 {
 	if (msg.size == 3 && msg.body[0] == CMD_LEAVE_PROGMODE_ISP)
@@ -292,6 +302,15 @@ uint8_t readEepromByte(const uint8_t *addr)
 	return EEDR;
 }
 
+void writeEepromByte(uint8_t *addr, uint8_t value)
+{
+	EECR = 0;
+	EEAR = reinterpret_cast<uint16_t>(addr);
+	EEDR = value;
+	EECR |= (1 << EEMPE);
+	EECR |= (1 << EEPE);
+}
+
 } // namespace
 
 static inline void formatReadEepromIspAnswer(Message &msg, uint32_t &addr)
@@ -299,7 +318,6 @@ static inline void formatReadEepromIspAnswer(Message &msg, uint32_t &addr)
 	const uint16_t numBytes = static_cast<uint16_t>(msg.body[1]) << 8 | msg.body[2];
 	msg.size = 3 + numBytes;
 	msg.body[1] = STATUS_CMD_OK;
-	waitEepromReady();
 	for (uint16_t i = 0; i < numBytes; ++i) {
 		msg.body[i + 2] = readEepromByte(reinterpret_cast<const uint8_t *>(addr + i));
 	}
@@ -309,6 +327,20 @@ static inline void formatReadEepromIspAnswer(Message &msg, uint32_t &addr)
 	msg.checksum = calcChecksum(msg);
 }
 
+static inline void formatProgramEepromIspAnswer(Message &msg, uint32_t &addr)
+{
+	const uint16_t numBytes = static_cast<uint16_t>(msg.body[1]) << 8 | msg.body[2];
+	for (uint16_t i = 0; i < numBytes; ++i) {
+		writeEepromByte(reinterpret_cast<uint8_t *>(addr + i), msg.body[10 + i]);
+		waitEepromReady();
+	}
+	addr += numBytes;
+	msg.size = 2;
+	msg.body[1] = STATUS_CMD_OK;
+
+	msg.checksum = calcChecksum(msg);
+}
+
 static inline void formatLeaveProgmodeIspAnswer(Message &msg)
 {
 	msg.size = 2;
@@ -354,6 +386,8 @@ static inline void handleMessage(Message &msg)
 		formatReadFlashIspAnswer(msg, s_address);
 	else if (isReadEepromIsp(msg))
 		formatReadEepromIspAnswer(msg, s_address);
+	else if (isProgramEepromIsp(msg))
+		formatProgramEepromIspAnswer(msg, s_address);
 	else if (isLeaveProgmodeIsp(msg))
 		formatLeaveProgmodeIspAnswer(msg);
 	else