diff --git a/src/main.c b/src/main.c
index 8d65eab..6b970b2 100644
--- a/src/main.c
+++ b/src/main.c
@@ -38,10 +38,16 @@ volatile i16 countdown_tick = 0;
 // Light timer
 volatile i16 timer = 0;
 
-// volatile u08 exec = 0;
-volatile u08 cmd[BUFFER_LEN];
-volatile u08 cmdPos = 0;
-volatile u08 cmdLen = 0;
+// rx buffer params
+volatile u08 rx_buffer[BUFFER_LEN];
+volatile u08 rx_pos = 0;
+volatile u08 rx_len = 0;
+
+// tx buffer params
+volatile u08 tx_buffer[BUFFER_LEN];
+volatile u08 tx_pos = 0;
+volatile u08 tx_len = 0;
+
 volatile u08 state = 0;
 u08 led_state = 0;
 
@@ -63,10 +69,10 @@ void reboot(void) {
  * Start feeding cmd into UART (will process with interrupts)
  */
 void start_cmd(u08 len) {
-	cmdPos = 0;
-	cmdLen = len;
+	tx_pos = 0;
+	tx_len = len;
 	UART1.CR2 |= UART_CR2_TXE_IVT;
-	UART1.Data = cmd[0];
+	UART1.Data = tx_buffer[0];
 }
 
 /**
@@ -107,9 +113,9 @@ i16 elapsed(void) {
 }
 
 void send_some_command(void) {
-	cmd[0] = 'C';
-	cmd[1] = 'M';
-	cmd[2] = 'D';
+	tx_buffer[0] = 'C';
+	tx_buffer[1] = 'M';
+	tx_buffer[2] = 'D';
 	start_cmd(3);
 }
 
@@ -188,37 +194,63 @@ void tick_250ms(void) {
 	}
 }
 
+/**
+ * Check if rx buffer contains finished command
+ */
+int is_rx_finished(void) {
+	// head + len + cmd + ver + tail
+	if (rx_pos < 4 + 2 + 2 + 2 + 4) {
+		return false;
+	}
+	// Check head, tail and just in case - length
+	return rx_buffer[0] == 0xFD &&
+	       rx_buffer[1] == 0xFC &&
+		   rx_buffer[2] == 0xFB &&
+		   rx_buffer[3] == 0xFA &&
+		   rx_buffer[rx_pos - 1] == 0x01 &&
+		   rx_buffer[rx_pos - 2] == 0x02 &&
+		   rx_buffer[rx_pos - 3] == 0x03 &&
+		   rx_buffer[rx_pos - 4] == 0x04 &&
+		   (rx_buffer[4] | (rx_buffer[5] << 8)) == rx_pos - 12;
+}
+
 interrupt(IRQ_UART1_RX_F, uart_recv) {
 	u08 s = UART1.Status;
 	u08 c = UART1.Data;
-	// if  (exec == 1) {
-	// 	switch (c) {
-	// 		case 0x2A:
-	// 			timer = 1200;
-	// 			break;
-	// 		case 0x2B:
-	// 			timer = -1;
-	// 			break;
-	// 		case 0x2C:
-	// 			timer = 0;
-	// 			break;
-	// 	}
-	// }
-	// if (c == 0x24) {
-	// 	exec = 1;
-	// } else {
-	// 	exec = 0;
-	// }
+	if ((s & UART_RX_NOT_EMPTY) == 0) {
+		return;
+	}
+
+	if (rx_pos < 4) {
+		if (rx_pos == 0xFD - c) {
+			rx_buffer[rx_pos] = c;
+		} else {
+			// Something wrong - reset
+			rx_pos = 0;
+			memset(rx_buffer, 0, BUFFER_LEN);
+		}
+	} else {
+		rx_buffer[rx_pos] = c;
+		if (is_rx_finished()) {
+			// TODO: Process
+			rx_pos = 0;
+			memset(rx_buffer, 0, BUFFER_LEN);
+			return;
+		}
+	}
+	rx_pos++;
 }
 
 interrupt(IRQ_UART1_TX_C, uart_sent) {
-	cmdPos++;
-	if (cmdPos < cmdLen) {
-		UART1.Data = cmd[cmdPos];
+	tx_pos++;
+	// If we have more data - process
+	if (tx_pos < tx_len) {
+		UART1.Data = tx_buffer[tx_pos];
 	} else {
-		cmdPos = 0;
-		cmdLen = 0;
-		memset(cmd, '\0', BUFFER_LEN);
+		// Clean up
+		tx_pos = 0;
+		tx_len = 0;
+		memset(tx_buffer, '\0', BUFFER_LEN);
 		UART1.CR2 &= ~UART_CR2_TXE_IVT;
 	}
 }