diff --git a/stk500v2/main.cpp b/stk500v2/main.cpp
index 65eebe2..dd56170 100644
--- a/stk500v2/main.cpp
+++ b/stk500v2/main.cpp
@@ -2,14 +2,275 @@
 
 #include "uart/uart.hpp"
 
+#include <avr/boot.h>
+
 #include "command.hpp"
 
 using uart_interface = uart::Hardware0<uart::Config<115200>, uart::Driven::BLOCKING>;
+uart::Uart<uart_interface> serial;
+
+struct Message {
+	uint8_t start;
+	uint8_t number;
+	uint16_t size;
+	uint8_t token;
+	uint8_t body[275];
+	uint8_t checksum;
+};
+
+static inline bool receiveByte(uint8_t &data, uint16_t timeout = 1000)
+{
+	for (uint16_t i = 0; i < timeout; ++i) {
+		if (serial.rxByte(data))
+			return true;
+	}
+
+	return false;
+}
+
+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];
+
+	return checksum;
+}
+
+static inline bool receiveMessage(Message &msg)
+{
+	if (!receiveByte(msg.start) || msg.start != MESSAGE_START)
+		return false;
+	if (!receiveByte(msg.number))
+		return false;
+	if (!receiveByte(*(reinterpret_cast<uint8_t *>(&msg.size) + 1)))
+		return false;
+	if (!receiveByte(*reinterpret_cast<uint8_t *>(&msg.size)) || msg.size > sizeof(msg.body))
+		return false;
+	if (!receiveByte(msg.token) || msg.token != TOKEN)
+		return false;
+	for (uint16_t i = 0; i < msg.size; ++i) {
+		if (!receiveByte(msg.body[i]))
+			return false;
+	}
+	if (!receiveByte(msg.checksum) || msg.checksum != calcChecksum(msg))
+		return false;
+
+	return true;
+}
+
+static inline bool isSignOn(const Message &msg)
+{
+	if (msg.size == 1 && msg.body[0] == CMD_SIGN_ON)
+		return true;
+	return false;
+}
+
+static inline bool isGetParameter(const Message &msg)
+{
+	if (msg.size == 2 && msg.body[0] == CMD_GET_PARAMETER)
+		return true;
+	return false;
+}
+
+static inline bool isSetParameter(const Message &msg)
+{
+	if (msg.size == 3 && msg.body[0] == CMD_SET_PARAMETER)
+		return true;
+	return false;
+}
+
+static inline bool isEnterProgmodeIsp(const Message &msg)
+{
+	if (msg.size == 12 && msg.body[0] == CMD_ENTER_PROGMODE_ISP)
+		return true;
+	return false;
+}
+
+static inline bool isReadSignatureIsp(const Message &msg)
+{
+	if (msg.size == 6 && msg.body[0] == CMD_READ_SIGNATURE_ISP)
+		return true;
+	return false;
+}
+
+static inline bool isReadFuseIsp(const Message &msg)
+{
+	if (msg.size == 6 && msg.body[0] == CMD_READ_FUSE_ISP)
+		return true;
+	return false;
+}
+
+static inline bool isLeaveProgmodeIsp(const Message &msg)
+{
+	if (msg.size == 3 && msg.body[0] == CMD_LEAVE_PROGMODE_ISP)
+		return true;
+	return false;
+}
+
+static inline void formatSignOnAnswer(Message &msg)
+{
+	msg.size = 3 + 8;
+	msg.body[1] = STATUS_CMD_OK;
+	msg.body[2] = 8;
+	msg.body[3] = 'S';
+	msg.body[4] = 'T';
+	msg.body[5] = 'K';
+	msg.body[6] = '5';
+	msg.body[7] = '0';
+	msg.body[8] = '0';
+	msg.body[9] = '_';
+	msg.body[10] = '2';
+	msg.checksum = calcChecksum(msg);
+}
+
+static inline void formatGetParameterAnswer(Message &msg)
+{
+	msg.size = 3;
+
+	if (msg.body[1] == PARAM_HW_VER) {
+		msg.body[2] = 1;
+	} else if (msg.body[1] == PARAM_SW_MAJOR) {
+		msg.body[2] = 0x02;
+	} else if (msg.body[1] == PARAM_SW_MINOR) {
+		msg.body[2] = 0x0a;
+	} else if (msg.body[1] == PARAM_SCK_DURATION) {
+		msg.body[2] = 2;
+	} else if (msg.body[1] == PARAM_VADJUST) {
+		msg.body[2] = 25;
+	} else if (msg.body[1] == PARAM_VTARGET) {
+		msg.body[2] = 49;
+	} else if (msg.body[1] == PARAM_OSC_PSCALE) {
+		msg.body[2] = 2;
+	} else if (msg.body[1] == PARAM_OSC_CMATCH) {
+		msg.body[2] = 127;
+	} else {
+		msg.size = 2;
+	}
+
+	if (msg.size == 2) {
+		msg.body[1] = STATUS_CMD_FAILED;
+	} else {
+		msg.body[1] = STATUS_CMD_OK;
+	}
+
+	msg.checksum = calcChecksum(msg);
+}
+
+static inline void formatSetParameterAnswer(Message &msg)
+{
+	msg.size = 2;
+	msg.body[1] = STATUS_CMD_OK;
+
+	msg.checksum = calcChecksum(msg);
+}
+
+static inline void formatEnterProgmodeIspAnswer(Message &msg)
+{
+	msg.size = 2;
+	msg.body[1] = STATUS_CMD_OK;
+
+	msg.checksum = calcChecksum(msg);
+}
+
+static inline void formatReadSignatureIspAnswer(Message &msg)
+{
+	msg.size = 4;
+	msg.body[2] = boot_signature_byte_get(msg.body[4] * 2);
+	msg.body[1] = STATUS_CMD_OK;
+	msg.body[3] = STATUS_CMD_OK;
+
+	msg.checksum = calcChecksum(msg);
+}
+
+static inline void formatReadFuseIspAnswer(Message &msg)
+{
+	constexpr auto READ_LOW_FUSE_BITS = 0x0050;
+	constexpr auto READ_HIGH_FUSE_BITS = 0x0858;
+	constexpr auto READ_EXTENDED_FUSE_BITS = 0x0850;
+
+	msg.size = 4;
+
+	if (*reinterpret_cast<uint16_t *>(msg.body + 2) == READ_EXTENDED_FUSE_BITS) {
+		msg.body[2] = boot_lock_fuse_bits_get(GET_EXTENDED_FUSE_BITS);
+	}
+	if (*reinterpret_cast<uint16_t *>(msg.body + 2) == READ_HIGH_FUSE_BITS) {
+		msg.body[2] = boot_lock_fuse_bits_get(GET_HIGH_FUSE_BITS);
+	}
+	if (*reinterpret_cast<uint16_t *>(msg.body + 2) == READ_LOW_FUSE_BITS) {
+		msg.body[2] = boot_lock_fuse_bits_get(GET_LOW_FUSE_BITS);
+	}
+
+	msg.body[1] = STATUS_CMD_OK;
+	msg.body[3] = STATUS_CMD_OK;
+
+	msg.checksum = calcChecksum(msg);
+}
+
+static inline void formatLeaveProgmodeIspAnswer(Message &msg)
+{
+	msg.size = 2;
+	msg.body[1] = STATUS_CMD_OK;
+
+	msg.checksum = calcChecksum(msg);
+}
+
+static inline void formatErrorAnswer(Message &msg)
+{
+	msg.start = MESSAGE_START;
+	msg.size = 1;
+	msg.token = TOKEN;
+	msg.body[0] = STATUS_CMD_UNKNOWN;
+	msg.checksum = calcChecksum(msg);
+}
+
+static inline void handleMessage(Message &msg)
+{
+	if (isSignOn(msg))
+		formatSignOnAnswer(msg);
+	else if (isGetParameter(msg))
+		formatGetParameterAnswer(msg);
+	else if (isSetParameter(msg))
+		formatSetParameterAnswer(msg);
+	else if (isEnterProgmodeIsp(msg))
+		formatEnterProgmodeIspAnswer(msg);
+	else if (isReadSignatureIsp(msg))
+		formatReadSignatureIspAnswer(msg);
+	else if (isReadFuseIsp(msg))
+		formatReadFuseIspAnswer(msg);
+	else if (isLeaveProgmodeIsp(msg))
+		formatLeaveProgmodeIspAnswer(msg);
+	else
+		formatErrorAnswer(msg);
+}
+
+static inline void transmitMessage(const Message &msg)
+{
+	serial.txByte(msg.start);
+	serial.txByte(msg.number);
+	serial.txByte(msg.size >> 8);
+	serial.txByte(msg.size & 0xFF);
+	serial.txByte(msg.token);
+	for (uint16_t i = 0; i < msg.size; ++i)
+		serial.txByte(msg.body[i]);
+	serial.txByte(msg.checksum);
+}
 
 int main()
 {
-	uart::Uart<uart_interface> serial;
 	serial.init();
 
+	Message msg;
+
+	while (true) {
+		if (receiveMessage(msg)) {
+			handleMessage(msg);
+			transmitMessage(msg);
+		}
+	}
+
 	return 0;
 }