--- a/console.s	Fri May 07 03:15:44 2010 +0300
+++ b/console.s	Fri May 07 03:16:01 2010 +0300
@@ -31,9 +31,12 @@
 .endm
 
 ;; Interrupt Vector
-.org 0x00
+.org 0x0000
         rjmp        Main
 
+;; Libraries
+.include "div.inc"			; Division routines
+
 ;; Serial
 .set SERIAL_BAUD = 103		; 9.6k @ 16Mhz
 
@@ -90,23 +93,48 @@
 
 ;; Write nul-terminated string to serial port
 ; Input string in Z
-Serial_Write:
+Serial_Write_s:
 	; Load byte to r16
 		lpm			r16, Z+
 
 	; Quit if nul
 		tst			r16
-		breq		_serial_write_end
+		breq		_serial_writes_end
 
 	; Write it
 		rcall		Serial_Send
 
 	; Continue
-		rjmp		Serial_Write
+		rjmp		Serial_Write_s
 
-_serial_write_end:
+_serial_writes_end:
 		ret
 
+;; Write char to serial port
+; Input byte in r16
+Serial_Write_c:
+		; ASCII offset for '0'
+		ldi			r17, 48
+		mov			r4, r17	
+	
+	; Convert
+		ldi			r17, 100
+		call		div8u
+		add			r16, r4
+		rcall		Serial_Send
+		mov			r16, r15
+
+		ldi			r17, 10
+		call		div8u
+		add			r16, r4
+		rcall		Serial_Send
+		mov			r16, r15
+
+		add			r16, r4
+		rcall		Serial_Send
+		
+	ret
+
 ;; Program
 message:	.db "Hello World", 13, 10, 0
 
@@ -125,19 +153,42 @@
 		ldi			ZH, high(message * 2)
 		ldi			ZL, low(message * 2)
 		
-		rcall		Serial_Write
+		rcall		Serial_Write_s
 
 ; Echo out
 		sbi			DDRB, PORTB5
 		ldi			r20, 0
+		ldi			r21, 0
 
 Main_Echo:
+		; blink LED
 		out			PORTB, r20
-
+		
+		; read
 		rcall		Serial_Recv
+		push		r16
+		
+		; running counter
+		inc			r21
+		mov			r16, r21
+		rcall		Serial_Write_c
+		
+		; ' '
+		ldi			r16, 32
 		rcall		Serial_Send
-
+		
+		pop			r16
+		rcall		Serial_Send
+		
+		; '\r\n'
+		ldi			r16, 13
+		rcall		Serial_Send
+		ldi			r16, 10
+		rcall		Serial_Send
+		
+		; toggle
 		com			r20
-
+		
+		; continue
 		rjmp		Main_Echo