Added debug compile options

Hard coded to build debug builds for the moment.  Should make this
a flag like -DDEBUG
Seperated out test functions into test.c

Also commented out sw_init() code as this conflicts with JTAG
debugger.