Finally got this working, this was a nightmare to design and debug. Basically two program counters which are switched when an interrupt happens so that program memory can jump between the two. Also...