Rearrange test code
Add Stop capability to firmware loading
Work on protocol testing
Fix a few issues with firmware