From 9c24c32f3dc98fcaa06f13552c75b6d5a8d61c93 Mon Sep 17 00:00:00 2001
From: BlackMark <me@blackmark.me>
Date: Sat, 11 Apr 2020 17:48:13 +0200
Subject: [PATCH] Add reading of eeprom

---
 stk500v2/main.cpp | 47 +++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 47 insertions(+)

diff --git a/stk500v2/main.cpp b/stk500v2/main.cpp
index 67af69b..8daabee 100644
--- a/stk500v2/main.cpp
+++ b/stk500v2/main.cpp
@@ -4,6 +4,7 @@
 
 #include <avr/boot.h>
 #include <avr/interrupt.h>
+#include <avr/io.h>
 #include <avr/pgmspace.h>
 
 #include "command.hpp"
@@ -126,6 +127,13 @@ static inline bool isReadFlashIsp(const Message &msg)
 	return false;
 }
 
+static inline bool isReadEepromIsp(const Message &msg)
+{
+	if (msg.size == 4 && msg.body[0] == CMD_READ_EEPROM_ISP)
+		return true;
+	return false;
+}
+
 static inline bool isLeaveProgmodeIsp(const Message &msg)
 {
 	if (msg.size == 3 && msg.body[0] == CMD_LEAVE_PROGMODE_ISP)
@@ -264,6 +272,43 @@ static inline void formatReadFlashIspAnswer(Message &msg, uint32_t &addr)
 	msg.checksum = calcChecksum(msg);
 }
 
+namespace {
+
+bool isEepromReady()
+{
+	return (EECR & (1 << EEPE)) ? false : true;
+}
+
+void waitEepromReady()
+{
+	while (!isEepromReady())
+		;
+}
+
+uint8_t readEepromByte(const uint8_t *addr)
+{
+	EEAR = reinterpret_cast<uint16_t>(addr);
+	EECR |= (1 << EERE);
+	return EEDR;
+}
+
+} // namespace
+
+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));
+	}
+	addr += numBytes;
+	msg.body[numBytes + 2] = STATUS_CMD_OK;
+
+	msg.checksum = calcChecksum(msg);
+}
+
 static inline void formatLeaveProgmodeIspAnswer(Message &msg)
 {
 	msg.size = 2;
@@ -307,6 +352,8 @@ static inline void handleMessage(Message &msg)
 		formatLoadAddressAnswer(msg);
 	} else if (isReadFlashIsp(msg))
 		formatReadFlashIspAnswer(msg, s_address);
+	else if (isReadEepromIsp(msg))
+		formatReadEepromIspAnswer(msg, s_address);
 	else if (isLeaveProgmodeIsp(msg))
 		formatLeaveProgmodeIspAnswer(msg);
 	else