2 files changed, 88 insertions, 9 deletions

diff --git a/Software/pinlock.asm b/Software/pinlock.asm
index b119723..ba12dc9 100644
--- a/Software/pinlock.asm
+++ b/Software/pinlock.asm
@@ -5,6 +5,8 @@
 .def cnt = r19
 .def tmp1 = r20
 .def tmp2 = r21
+.def ret0 = r22
+.def ret1 = r23
 
 init:
   ldi tmp0, HIGH(RAMEND)
@@ -18,19 +20,28 @@ init:
   out DDRA, tmp0
   ldi tmp0, 0b11111111
   out DDRB, tmp0
-  ldi tmp0, 0x31
-  mov cnt, tmp0
-  out PORTA, tmp0
-  ldi tmp0, 0x42
-  out PORTB, tmp0
-  ldi tmp0, 0b11000000
+  ldi tmp0, 0b11101100
   out DDRD, tmp0
   ldi tmp0, 0b00000000
   out PORTD, tmp0
 
+	ldi par0, 0
+	ldi par1, 0
+	rcall display
+	ldi par0, 1
+	ldi par1, 1
+	rcall display
+	ldi par0, 2
+	ldi par1, 2
+	rcall display
+	ldi par0, 3
+	ldi par1, 3
+	rcall display
+
 main:
-	ldi par0, 0b11000011
+	ldi par0, 0b11111111
 	rcall status
+	rjmp [tmp0]
 
 wait:
   mov tmp0, par0
@@ -41,12 +52,13 @@ wait:
   dec par1
   nop
   brne wait
-  inc cnt
-  out PORTA, cnt
   ret
 
 ; Output the status
+; Parameters:
 ; par0 = Status bits
+; Returns:
+; None
 status:
 	ldi tmp0, 1
 status_loop:
@@ -70,3 +82,70 @@ status_end:
 	cpi tmp0, 0
 	brne status_loop
 	ret
+
+; Display the digit on a given display.
+; Parameters:
+; par0 = Display number
+; par1 = Digit
+; Returns:
+; None
+display:
+	mov tmp0, par1
+	andi tmp0, 0x0F
+	cpi par0, 0
+	breq display_0
+	cpi par0, 1
+	breq display_1
+	cpi par0, 2
+	breq display_2
+	cpi par0, 3
+	breq display_3
+	ret
+display_0:
+	in tmp1, PORTA
+	andi tmp1, 0xF0
+	or tmp1, tmp0
+	out PORTA, tmp1
+	ret
+display_1:
+	in tmp1, PORTB
+	andi tmp1, 0xF0
+	or tmp1, tmp0
+	out PORTB, tmp1
+	ret
+display_2:
+	in tmp1, PORTA
+	andi tmp1, 0x0F
+	lsl tmp0
+	lsl tmp0
+	lsl tmp0
+	lsl tmp0
+	or tmp1, tmp0
+	out PORTA, tmp1
+	ret
+display_3:
+	in tmp1, PORTB
+	andi tmp1, 0x0F
+	lsl tmp0
+	lsl tmp0
+	lsl tmp0
+	lsl tmp0
+	or tmp1, tmp0
+	out PORTB, tmp1
+	ret
+
+; Read the input buttons.
+; Parameters:
+; None
+; Returns:
+; ret0 = 123A456B
+; ret1 = 789C*0#D
+input:
+	; Write selection rows
+	ldi tmp0, 1
+	ldi tmp1, 1
+input_loop:
+	mov tmp2, tmp0
+	and tmp2, tmp1
+	cp tmp1, tmp2
+	breq input_set
diff --git a/Software/pinlock.cof b/Software/pinlock.cof
deleted file mode 100644
index e69de29..0000000
--- a/Software/pinlock.cof
+++ /dev/null