diff --git a/stk500v2/main.cpp b/stk500v2/main.cpp
index c40c950..4832936 100644
--- a/stk500v2/main.cpp
+++ b/stk500v2/main.cpp
@@ -31,11 +31,10 @@ static inline bool receiveByte(uint8_t &data, uint16_t timeout = 1000)
 static inline uint8_t calcChecksum(const Message &msg)
 {
 	uint8_t checksum = msg.start;
-	checksum ^= msg.number;
-	checksum ^= msg.size;
-	checksum ^= msg.token;
-	for (uint8_t i = 0; i < msg.size; ++i)
-		checksum ^= msg.body[i];
+
+	for (uint16_t i = 1; i < 5 + msg.size; ++i) {
+		checksum ^= *(reinterpret_cast<const uint8_t *>(&msg) + i);
+	}
 
 	return checksum;
 }