Highlight keys when pressed

xylo.asm

@@ -1,6 +1,8 @@
 org 51000
 
 counter EQU inc_counter + 1
+attrs EQU &5800
+xylo EQU attrs + 40
 
 zero_counter:
 	xor a
@@ -24,39 +26,79 @@ read_keyboard:
 	jr read_keyboard
 
 key_1:
+	ld c, 0
+	call highlight_key
 	ld a, (counter)
 	call play
+	ld c, 0
+	call highlight_key
 	jr inc_counter
 
 key_2:
+	ld c, 3
+	call highlight_key
 	ld a, (counter)
 	inc a
 	call play
+	ld c, 3
+	call highlight_key
 	jr inc_counter
 
 key_3:
+	ld c, 6
+	call highlight_key
 	ld a, (counter)
 	inc a
 	inc a
 	call play
+	ld c, 6
+	call highlight_key
 	jr inc_counter
 
 key_4:
+	ld c, 9
+	call highlight_key
 	ld a, (counter)
 	add a, 3
 	call play
+	ld c, 9
+	call highlight_key
 	jr inc_counter
 
 key_5:
+	ld c, 12
+	call highlight_key
 	ld a, (counter)
 	add a, 4
 	call play
+	ld c, 12
+	call highlight_key
 
 inc_counter:
 	ld a, 0		;This value will be modified by the running code
 	add a, 5
 	cp 25
 	jr nz, set_counter
-	jr zero_counter
+	jp zero_counter
+
+highlight_key:
+	ld hl, xylo
+	ld b, 0
+	add hl, bc
+	ld d, 15
+highlight_row:
+	ld e, 3
+highlight_block:
+	ld a, (hl)
+	xor %01000000
+	ld (hl), a
+	inc hl
+	dec e
+	jr nz, highlight_block
+	ld c, 29
+	add hl, bc
+	dec d
+	jr nz, highlight_row
+	ret
 
 INCLUDE "sample.asm"