diff --git a/Makefile b/Makefile
index 789a114..909f0ae 100644
--- a/Makefile
+++ b/Makefile
@@ -47,9 +47,9 @@ tests/%.o: tests/%.S tests/riscv_test.h tests/test_macros.h
 		-DTEST_FUNC_TXT='"$(notdir $(basename $<))"' -DTEST_FUNC_RET=$(notdir $(basename $<))_ret $<
 
 clean:
-	rm -vrf $(TEST_OBJS) firmware/firmware.elf firmware/firmware.bin firmware/firmware.hex \
-		firmware/firmware.map testbench*.exe testbench.vcd .Xil fsm_encoding.os \
-		synth_vivado.log synth_vivado_*.backup.log synth_vivado.v
+	rm -vrf $(FIRMWARE_OBJS) $(TEST_OBJS) \
+		firmware/firmware.{elf,bin,hex,map} \
+		testbench{,_sp,_axi}.exe testbench.vcd
 
 .PHONY: test test_sp test_axi clean
 
diff --git a/README.md b/README.md
index c4be655..eb15cf8 100644
--- a/README.md
+++ b/README.md
@@ -206,53 +206,51 @@ one bit is set in `q1`.
 Registers `q2` and `q3` are uninitialized and can be used as temporary storage
 when saving/restoring register values in the IRQ handler.
 
-All of the following instructions are encoded under the `custom0` opcode.
+All of the following instructions are encoded under the `custom0` opcode. The f3
+and rs2 fields are ignored in all this instructions.
+
+See [firmware/custom_ops.S](firmware/custom_ops.S) for GNU assembler macros that
+implement mnemonics for this instructions.
+
+See [firmware/start.S](firmware/start.S) for an example implementaion of an
+interrupt handler assember wrapper, and [firmware/irq.c](firmware/irq.c) for
+the actual interrupt handler.
 
 #### getq rd, qs
 
 This instruction copies the value from a q-register to a general-purpose
 register.
 
-    0000000 00000 000XX 000 XXXXX 0001011
+    0000000 ----- 000XX --- XXXXX 0001011
     f7      rs2   qs    f3  rd    opcode
 
-Example assembler code using the `custom0` mnemonic:
+Example:
 
-| Instruction       | Assember Code       |
-| ------------------| --------------------|
-| getq x5, q2       | custom0 5, 2, 0, 0  |
-| getq x3, q0       | custom0 3, 0, 0, 0  |
-| getq x1, q3       | custom0 1, 3, 0, 0  |
+    getq x5, q2
 
 #### setq qd, rs
 
 This instruction copies the value from a general-purpose register to a
 q-register.
 
-    0000001 00000 XXXXX 000 000XX 0001011
+    0000001 ----- XXXXX --- 000XX 0001011
     f7      rs2   rs    f3  qd    opcode
 
-Example assembler code using the `custom0` mnemonic:
+Example:
 
-| Instruction       | Assember Code       |
-| ------------------| --------------------|
-| setq q2, x5       | custom0 2, 5, 0, 1  |
-| setq q0, x3       | custom0 0, 3, 0, 1  |
-| setq q3, x1       | custom0 3, 1, 0, 1  |
+    setq q2, x5
 
 #### retirq
 
 Return from interrupt. This instruction copies the value from `q0`
 to the program counter and re-enables interrupts.
 
-    0000010 00000 00000 000 00000 0001011
+    0000010 ----- 00000 --- 00000 0001011
     f7      rs2   rs    f3  rd    opcode
 
-Example assembler code using the `custom0` mnemonic:
+Example:
 
-| Instruction       | Assember Code       |
-| ------------------| --------------------|
-| retirq            | custom0 0, 0, 0, 2  |
+    retirq
 
 #### maskirq
 
@@ -260,14 +258,12 @@ The "IRQ Mask" register contains a bitmask of masked (disabled) interrupts.
 This instruction writes a new value to the irq mask register and reads the old
 value.
 
-    0000011 00000 XXXXX 000 XXXXX 0001011
+    0000011 ----- XXXXX --- XXXXX 0001011
     f7      rs2   rs    f3  rd    opcode
 
-Example assembler code using the `custom0` mnemonic:
+Example:
 
-| Instruction       | Assember Code       |
-| ------------------| --------------------|
-| maskirq x1, x2    | custom0 1, 2, 0, 3  |
+    maskirq x1, x2
 
 The processor starts with all interrupts disabled.
 
@@ -276,17 +272,15 @@ interrupt is disabled will cause the processor to halt.
 
 #### waitirq
 
-Pause execution until an interrupt triggers. The bitmask of pending IRQs is
-written to `rd`.
+Pause execution until an interrupt becomes pending. The bitmask of pending IRQs
+is written to `rd`.
 
-    0000100 00000 00000 000 XXXXX 0001011
+    0000100 ----- 00000 --- XXXXX 0001011
     f7      rs2   rs    f3  rd    opcode
 
-Example assembler code using the `custom0` mnemonic:
+Example:
 
-| Instruction       | Assember Code       |
-| ------------------| --------------------|
-| waitirq x1        | custom0 1, 0, 0, 4  |
+    waitirq x1
 
 #### timer
 
@@ -295,14 +289,12 @@ triggers the timer interrupt when transitioning from 1 to 0. Setting the
 counter to zero disables the timer. The old value of the counter is written to
 `rd`.
 
-    0000101 00000 XXXXX 000 XXXXX 0001011
+    0000101 ----- XXXXX --- XXXXX 0001011
     f7      rs2   rs    f3  rd    opcode
 
-Example assembler code using the `custom0` mnemonic:
+Example:
 
-| Instruction       | Assember Code       |
-| ------------------| --------------------|
-| timer x1, x2      | custom0 1, 2, 0, 5  |
+    timer x1, x2
 
 
 Building a pure RV32I Toolchain:
diff --git a/firmware/custom_ops.S b/firmware/custom_ops.S
new file mode 100644
index 0000000..b5ea7ba
--- /dev/null
+++ b/firmware/custom_ops.S
@@ -0,0 +1,36 @@
+// This is free and unencumbered software released into the public domain.
+// 
+// Anyone is free to copy, modify, publish, use, compile, sell, or
+// distribute this software, either in source code form or as a compiled
+// binary, for any purpose, commercial or non-commercial, and by any
+// means.
+
+#define q0 0
+#define q1 1
+#define q2 2
+#define q3 3
+
+.macro getq rd qs
+custom0 \rd,\qs,0,0
+.endm
+
+.macro setq qd rs
+custom0 \qd,\rs,0,1
+.endm
+
+.macro retirq
+custom0 0,0,0,2
+.endm
+
+.macro maskirq rd rs
+custom0 \rd,\rs,0,3
+.endm
+
+.macro waitirq rd
+custom0 \rd,0,0,4
+.endm
+
+.macro timer rd rs
+custom0 \rd,\rs,0,5
+.endm
+
diff --git a/firmware/firmware.h b/firmware/firmware.h
index b4fa4f8..3faf147 100644
--- a/firmware/firmware.h
+++ b/firmware/firmware.h
@@ -1,3 +1,10 @@
+// This is free and unencumbered software released into the public domain.
+// 
+// Anyone is free to copy, modify, publish, use, compile, sell, or
+// distribute this software, either in source code form or as a compiled
+// binary, for any purpose, commercial or non-commercial, and by any
+// means.
+
 #ifndef FIRMWARE_H
 #define FIRMWARE_H
 
diff --git a/firmware/irq.c b/firmware/irq.c
index e49ef47..997e439 100644
--- a/firmware/irq.c
+++ b/firmware/irq.c
@@ -1,3 +1,9 @@
+// This is free and unencumbered software released into the public domain.
+// 
+// Anyone is free to copy, modify, publish, use, compile, sell, or
+// distribute this software, either in source code form or as a compiled
+// binary, for any purpose, commercial or non-commercial, and by any
+// means.
 
 #include "firmware.h"
 
diff --git a/firmware/makehex.py b/firmware/makehex.py
index a5ae740..e1acdeb 100644
--- a/firmware/makehex.py
+++ b/firmware/makehex.py
@@ -1,4 +1,11 @@
 #!/usr/bin/python3
+#
+# This is free and unencumbered software released into the public domain.
+# 
+# Anyone is free to copy, modify, publish, use, compile, sell, or
+# distribute this software, either in source code form or as a compiled
+# binary, for any purpose, commercial or non-commercial, and by any
+# means.
 
 from sys import argv
 
diff --git a/firmware/multest.c b/firmware/multest.c
index 3d41267..6acf1e3 100644
--- a/firmware/multest.c
+++ b/firmware/multest.c
@@ -1,3 +1,10 @@
+// This is free and unencumbered software released into the public domain.
+// 
+// Anyone is free to copy, modify, publish, use, compile, sell, or
+// distribute this software, either in source code form or as a compiled
+// binary, for any purpose, commercial or non-commercial, and by any
+// means.
+
 #include "firmware.h"
 
 uint32_t xorshift32() {
diff --git a/firmware/print.c b/firmware/print.c
index 03e9b09..2aef715 100644
--- a/firmware/print.c
+++ b/firmware/print.c
@@ -1,3 +1,9 @@
+// This is free and unencumbered software released into the public domain.
+// 
+// Anyone is free to copy, modify, publish, use, compile, sell, or
+// distribute this software, either in source code form or as a compiled
+// binary, for any purpose, commercial or non-commercial, and by any
+// means.
 
 #include "firmware.h"
 
diff --git a/firmware/sections.lds b/firmware/sections.lds
index 5e6e40c..f6d9a3b 100644
--- a/firmware/sections.lds
+++ b/firmware/sections.lds
@@ -1,3 +1,12 @@
+/*
+This is free and unencumbered software released into the public domain.
+
+Anyone is free to copy, modify, publish, use, compile, sell, or
+distribute this software, either in source code form or as a compiled
+binary, for any purpose, commercial or non-commercial, and by any
+means.
+*/
+
 SECTIONS {
 	.memory : {
 		. = 0x000000;
diff --git a/firmware/sieve.c b/firmware/sieve.c
index 10fd1e0..9f7f7a3 100644
--- a/firmware/sieve.c
+++ b/firmware/sieve.c
@@ -1,3 +1,10 @@
+// This is free and unencumbered software released into the public domain.
+// 
+// Anyone is free to copy, modify, publish, use, compile, sell, or
+// distribute this software, either in source code form or as a compiled
+// binary, for any purpose, commercial or non-commercial, and by any
+// means.
+
 // A simple Sieve of Eratosthenes
 
 #include "firmware.h"
diff --git a/firmware/start.S b/firmware/start.S
index a61f7fc..f39665e 100644
--- a/firmware/start.S
+++ b/firmware/start.S
@@ -1,8 +1,17 @@
+// This is free and unencumbered software released into the public domain.
+// 
+// Anyone is free to copy, modify, publish, use, compile, sell, or
+// distribute this software, either in source code form or as a compiled
+// binary, for any purpose, commercial or non-commercial, and by any
+// means.
+
 #define ENABLE_RVTST
 #define ENABLE_SIEVE
 #define ENABLE_MULTST
 #define ENABLE_STATS
 
+#include "custom_ops.S"
+
 	.section .text
 	.global irq
 	.global sieve
@@ -13,43 +22,33 @@
 	.global hard_mulhu
 	.global stats
 
-#ifdef ENABLE_RVTST
-#  define TEST(n) \
-	.global n; \
-	addi x1, zero, 1000; \
-	custom0 0,1,0,5; /* timer zero, x1 */ \
-	jal zero,n; \
-	.global n ## _ret; \
-	n ## _ret:
-#else
-#  define TEST(n) \
-	.global n ## _ret; \
-	n ## _ret:
-#endif
-
 reset_vec:
-	custom0 0,0,0,4 // waitirq zero
-	custom0 0,0,0,3 // maskirq zero, zero
+	// no more than 16 bytes here !
+	waitirq zero
+	maskirq zero, zero
 	j start
-	nop
-	nop
 
+
+/* Interrupt handler
+ **********************************/
+
+.balign 16
 irq_vec:
 	/* save registers */
 
-	custom0 2,1,0,1 // setq q2, x1
-	custom0 3,2,0,1 // setq q3, x2
+	setq q2, x1
+	setq q3, x2
 
 	lui x1, %hi(irq_regs)
 	addi x1, x1, %lo(irq_regs)
 
-	custom0 2,0,0,0 // getq x2, q0
+	getq x2, q0
 	sw x2,   0*4(x1)
 
-	custom0 2,2,0,0 // getq x2, q2
+	getq x2, q2
 	sw x2,   1*4(x1)
 
-	custom0 2,3,0,0 // getq x2, q3
+	getq x2, q3
 	sw x2,   2*4(x1)
 
 	sw x3,   3*4(x1)
@@ -82,19 +81,19 @@ irq_vec:
 	sw x30, 30*4(x1)
 	sw x31, 31*4(x1)
 
-	/* call interrupt handler */
+	/* call interrupt handler C function */
 
-	lui sp, %hi(irq_stack_top)
-	addi sp, sp, %lo(irq_stack_top)
+	lui sp, %hi(irq_stack)
+	addi sp, sp, %lo(irq_stack)
 
 	// arg0 = address of regs
 	lui a0, %hi(irq_regs)
 	addi a0, a0, %lo(irq_regs)
 
 	// arg1 = interrupt type
-	custom0 11,1,0,0 // getq x11, q1
+	getq x11, q1
 
-	// call to c function
+	// call to C function
 	jal ra, irq
 
 	/* restore registers */
@@ -103,13 +102,13 @@ irq_vec:
 	addi x1, x1, %lo(irq_regs)
 
 	lw x2,   0*4(x1)
-	custom0 0,2,0,1 // setq q0, x2
+	setq q0, x2
 
 	lw x2,   1*4(x1)
-	custom0 1,2,0,1 // setq q1, x2
+	setq q1, x2
 
 	lw x2,   2*4(x1)
-	custom0 2,2,0,1 // setq q2, x2
+	setq q2, x2
 
 	lw x3,   3*4(x1)
 	lw x4,   4*4(x1)
@@ -141,21 +140,27 @@ irq_vec:
 	lw x30, 30*4(x1)
 	lw x31, 31*4(x1)
 
-	custom0 1,1,0,0 // getq x1, q1
-	custom0 2,2,0,0 // getq x2, q2
+	getq x1, q1
+	getq x2, q2
 
-	custom0 0,0,0,2 // retirq
+	retirq
 
 irq_regs:
-	// registers are saved to this memory region
+	// registers are saved to this memory region during interrupt handling
 	// the program counter is saved as register 0
 	.fill 32,4
 
-irq_stack:
+	// stack for the interrupt handler
 	.fill 128,4
-irq_stack_top:
+irq_stack:
+
+
+/* Main program
+ **********************************/
 
 start:
+	/* zero-initialize all registers */
+
 	addi x1, zero, 0
 	addi x2, zero, 0
 	addi x3, zero, 0
@@ -188,6 +193,22 @@ start:
 	addi x30, zero, 0
 	addi x31, zero, 0
 
+	/* running tests from riscv-tests */
+
+#ifdef ENABLE_RVTST
+#  define TEST(n) \
+	.global n; \
+	addi x1, zero, 1000; \
+	timer zero, x1; \
+	jal zero,n; \
+	.global n ## _ret; \
+	n ## _ret:
+#else
+#  define TEST(n) \
+	.global n ## _ret; \
+	n ## _ret:
+#endif
+
 	TEST(lui)
 	TEST(auipc)
 	TEST(j)
diff --git a/firmware/stats.c b/firmware/stats.c
index 21f97d5..7c4434a 100644
--- a/firmware/stats.c
+++ b/firmware/stats.c
@@ -1,3 +1,10 @@
+// This is free and unencumbered software released into the public domain.
+// 
+// Anyone is free to copy, modify, publish, use, compile, sell, or
+// distribute this software, either in source code form or as a compiled
+// binary, for any purpose, commercial or non-commercial, and by any
+// means.
+
 #include "firmware.h"
 
 static void stats_print_dec(int val, int digits, bool zero_pad)