--- a/console.s	Fri May 07 02:04:37 2010 +0300
+++ b/console.s	Fri May 07 02:04:59 2010 +0300
@@ -55,7 +55,7 @@
 		; Enable RX
 		; Enable TX
 		; 8 bits
-		poke		[UCSR0B, r16, (0 << RXEN0) | (1 << TXEN0) | (0 << UCSZ02)]
+		poke		[UCSR0B, r16, (1 << RXEN0) | (1 << TXEN0) | (0 << UCSZ02)]
 		
 	; Done
 		ret
@@ -74,6 +74,20 @@
 	; Done
 		ret
 
+;; Read a single byte from serial port
+; Output byte in r16
+Serial_Recv:
+	; Wait for recv
+		lds			r0, UCSR0A
+		sbrs		r0, RXC0
+		rjmp		Serial_Recv
+
+	; Copy byte from buffer
+		lds			r16, UDR0
+
+	; Done
+		ret
+
 ;; Write nul-terminated string to serial port
 ; Input string in Z
 Serial_Write:
@@ -107,12 +121,23 @@
 	; Init
 		rcall		Serial_Init
 
-loop:
 ; Main program
 		ldi			ZH, high(message * 2)
 		ldi			ZL, low(message * 2)
 		
 		rcall		Serial_Write
-		
-		; repeat
-		rjmp		loop
+
+; Echo out
+		sbi			DDRB, PORTB5
+		ldi			r20, 0
+
+Main_Echo:
+		out			PORTB, r20
+
+		rcall		Serial_Recv
+		rcall		Serial_Send
+
+		com			r20
+
+		rjmp		Main_Echo
+