Lpc2000

When I enter debug mode through jtag I made following:
01. read R0 (arm or thuumb instruction)
02. read PC (arm or thumb instruction)
03. If THUMB detected switch to arm (bx)
04. read r1 to r14 (arm instructions)
05. read cpsr and spsr (arm instructions)
06. modify read cpsr to get proper information about mode (arm/thumb 
bit) - only in host software.

When I exit from debug state:
10. restore r1 to r15 (arm instruction - r15 is modified to reflect 
correct address after 12)
11. restore cpsr
12. restore r0 (arm or thumb instruction)

I have a problem in step 11-13 when I have to restore cpsr. I don't 
make a jump at the end of restore.
Do I have to restore cpsr with thumb mode depended on the state I 
entered the debug state? 
I don't know if the next instruction after cpsr resore should be also 
depended on the state I entered debug mode.
Sometimes when I exit in thumb (I have entered in thumb) I got 
exception (undefined instruction), but not always. 
What will happen if the processor receive interrupt durring debug mode -
 should I exit in the state I entered debug mode or should I check 
interrupt flag before exit?
Maybe I'm making something wrong - could someone describe how to do it 
properly?