#define SVC_0 0 #define SVC_1 1 #define SVC_2 2 void SVC_Handler( void ) { asm("TST LR, #4"); asm("ITE EQ"); asm("MRSEQ R0, MSP"); asm("MRSNE R0, PSP"); asm("B SVC_Handler_main"); } void Dummy0_Handler( unsigned int * svc_args ) { svc_args[6] += 4; // skip Dummy0() } void Dummy1_Handler( unsigned int * svc_args ) { svc_args[6] += 8; // skip Dummy0(), Dummy1() } void Dummy0( void ) { } void Dummy1( void ) { } // void Dummy2_Handler( unsigned int * svc_args ) { unsigned int a0 = svc_args[0]; unsigned int a1 = svc_args[1]; unsigned int a2 = svc_args[2]; unsigned int a3 = svc_args[3]; unsigned int a4 = svc_args[8]; unsigned int a5 = svc_args[9]; svc_args[0] = ( a0 + a1 + a2 ) * ( a3 + a4 + a5 ); // retValue svc_args[6] = svc_args[5] - 1; // return to Caller of Dummy2() } unsigned int Dummy2( unsigned int a0, unsigned int a1, unsigned int a2, unsigned int a3, unsigned int a4, unsigned int a5 ) { asm("SVC #2"); // <--- modify first instruct return ( a0 + a1 + a2 ) * ( a3 + a4 + a5 ); } void SVC_Demo( void ) { unsigned int a0, a1, a2, a3, a4, a5; a0 = 0; a1 = 1; a2 = 2; a3 = 3; a4 = 4; a5 = 5;
asm("SVC #0"); // Dummy0_handler() : svc_args[6] += 4 Bytes : modifyed PC ( return address ) Dummy0(); // <---- PC, skipped : 0xF7FF 0xFFCA : BL Dummy0
Dummy1();
asm("SVC #1"); // Dummy1_handler() : svc_args[6] += 8 bytes Dummy0(); // <---- PC, skipped : 0xF7FF 0xFFCA 0xF7FF 0xFFC4 Dummy1(); // <-------- skipped
Dummy2(a0, a1, a2, a3, a4, a5);
// Dummy2() : SVC #2 : FIRST INSTRUCTION be patched
// Dummy2_Handler() : svc_args[6] = svc_args[5] - 1 : return after caller of dummy2() :: Execute Dummy0()
Dummy0(); Dummy1(); } // // a0 a1 a2 a3 | a4 a5 // xSP : R0 R1 R2 R3 R12 R14 PC xPSR | XX XX XX XX XX XX XX XX // xSP : R0 R1 R2 R3 R12 R14 PC xPSR | // |svc_args[0] |svc_args[6] --> instruction after SVC XX // | // 00 DF|XX XX XX XX <-- instruction will be executed after reti // -2 -1|0 1 2 3 // | void SVC_Handler_main( unsigned int * svc_args ) // R0 = ( PSP or MSP ) { /* * Stack contains: * r0, r1, r2, r3, r12, r14, the return address and xPSR * First argument (r0) is svc_args[0] */ unsigned int svc_number; svc_number = ((char *)svc_args[6])[-2]; // ( SVC #0 ) 00 DF : XX XX XX XX <-- code will be executed after reti // ( SVC #1 ) 01 DF : \______________ PC : (char *)svc_args[6]) switch(svc_number) { case SVC_0: Dummy0_Handler( svc_args ); break; case SVC_1: Dummy1_Handler( svc_args ); break; case SVC_2: Dummy2_Handler( svc_args ); break; default: break; } }
void SVC_Demo( void ) { SVC_Demo: 0x20004920: 0xe92d 0x43f0 PUSH.W {R4-R9, LR} 0x20004924: 0xb083 SUB SP, SP, #0xc a0 = 0; 0x20004926: 0x2000 MOVS R0, #0 0x20004928: 0x0004 MOVS R4, R0 a1 = 1; 0x2000492a: 0x2001 MOVS R0, #1 0x2000492c: 0x0005 MOVS R5, R0 a2 = 2; 0x2000492e: 0x2002 MOVS R0, #2 0x20004930: 0x0006 MOVS R6, R0 a3 = 3; 0x20004932: 0x2003 MOVS R0, #3 0x20004934: 0x0007 MOVS R7, R0 a4 = 4; 0x20004936: 0x2004 MOVS R0, #4 0x20004938: 0x4680 MOV R8, R0 a5 = 5; 0x2000493a: 0x2005 MOVS R0, #5 0x2000493c: 0x4681 MOV R9, R0 asm("SVC #0"); 0x2000493e: 0xdf00 SVC #0x0 Dummy0(); // <---- PC, skipped 0x20004940: 0xf7ff 0xffca BL Dummy0 ; 0x200048d8 Dummy1(); 0x20004944: 0xf7ff 0xffc9 BL Dummy1 ; 0x200048da asm("SVC #1"); 0x20004948: 0xdf01 SVC #0x1 Dummy0(); // <---- PC, skipped 0x2000494a: 0xf7ff 0xffc5 BL Dummy0 ; 0x200048d8 Dummy1(); // <-------- skipped 0x2000494e: 0xf7ff 0xffc4 BL Dummy1 ; 0x200048da Dummy2(a0, a1, a2, a3, a4, a5); 0x20004952: 0xf8cd 0x9004 STR.W R9, [SP, #0x4] ; a5 0x20004956: 0xf8cd 0x8000 STR.W R8, [SP] ; a4 0x2000495a: 0x003b MOVS R3, R7 ; a3 0x2000495c: 0x0032 MOVS R2, R6 ; a2 0x2000495e: 0x0029 MOVS R1, R5 ; a1 0x20004960: 0x0020 MOVS R0, R4 ; a0 0x20004962: 0xf7ff 0xffd1 BL Dummy2 ; 0x20004908 -----------LR=0x20004966+1-----------------------------------------------+ Dummy0(); ; BL : Branch and Link : LR ---> 0x20004966+1 : BL Dummy0 | 0x20004966: 0xf7ff 0xffb7 BL Dummy0 ; 0x200048d8 -------------------------------------------------------------------------+ Dummy1(); | 0x2000496a: 0xf7ff 0xffb6 BL Dummy1 ; 0x200048da | } | 0x2000496e: 0xe8bd 0x83f7 POP.W {R0-R2, R4-R9, PC} | unsigned int Dymmy2( unsigned int a0, unsigned int a1, unsigned int a2, | unsigned int a3, unsigned int a4, unsigned int a5 ) ; modify first instruction : SVC #2 | { | Dummy2: | 0x20004908: 0xb430 PUSH {R4, R5} ; SP = 0x2000A3C0 : a4 a5 | 0x2000490a: 0x9c02 LDR R4, [SP, #0x8] ; a4 : SP = 0x2000A3B8 + 8 = 0x2000A3C0 | 0x2000490c: 0x9d03 LDR R5, [SP, #0xc] ; a5 : SP = 0x2000A3B8 + C = 0x2000A3C4 | asm("SVC #2"); ; <--- modify first instruct 0x20004908 to execute Dummy2_Handler() ----------------+
0x2000490e: 0xdf02 SVC #0x2 ; Force Execute this instruction <TEST>
svc_args[6] = svc_args[5] - 1; ; PC = LR - 1 : Dummy2_Handler() last instruction : LR = 0x20004966+1
else execute 0x2000490a after exit SVC_Handler_main() ; because svc_args[6] = 0x2000490a when execute SVC #0x02
old function : first instruction : SVC xx --> execute new function()
new function : last insturction : svc_args[6] = svc_args[5] - 1 : return to instruction after BL OLD_FUNCTION
return ( a0 + a1 + a2 ) * ( a3 + a4 + a5 ); ; SP = 0x2000A3A0 : R0 R1 R2 R3 R12 R14 PC xPSR a4 a5 0x20004910: 0x1808 ADDS R0, R1, R0 ; skipped 0x20004912: 0x1810 ADDS R0, R2, R0 0x20004914: 0x18e1 ADDS R1, R4, R3 0x20004916: 0x1869 ADDS R1, R5, R1 0x20004918: 0x4348 MULS R0, R1, R0 0x2000491a: 0xbc30 POP {R4, R5} 0x2000491c: 0x4770 BX LR 0x2000491e: 0x0000 MOVS R0, R0 ; skipped SVC_Handler: 0x200048b4: 0xf01e 0x0f04 TST.W LR, #4 ; SP = 0x2000A3A0 : R0 R1 R2 R3 R12 R14 PC xPSR a4 a5 asm("ITE EQ"); 0x200048b8: 0xbf0c ITE EQ asm("MRSEQ R0, MSP"); 0x200048ba: 0xf3ef 0x8008 MRSEQ R0, MSP asm("MRSNE R0, PSP"); 0x200048be: 0xf3ef 0x8009 MRSNE R0, PSP asm("B SVC_Handler_main"); 0x200048c2: 0xf000 0xb856 B.W SVC_Handler_main } void SVC_Handler_main( unsigned int * svc_args ) // R0 = ( PSP or MSP ) { SVC_Handler_main: 0x20004972: 0xb538 PUSH {R3-R5, LR} ; SP = 0x2000A3A0 : R0 R1 R2 R3 R12 R14 PC xPSR a4 a5 0x20004974: 0x0004 MOVS R4, R0 ; SP = 0x2000A390 : R3 R4 R5 LR : R0 R1 R2 R3 R12 R14 PC xPSR a4 a5 svc_number = ((char *)svc_args[6])[-2]; ; R0 = 0x2000A3A0 : ( svc_args ) 0x20004976: 0x69a0 LDR R0, [R4, #0x18] 0x20004978: 0xf810 0x0c02 LDRB.W R0, [R0, #-0x2] 0x2000497c: 0x0005 MOVS R5, R0 switch(svc_number) 0x2000497e: 0x2d00 CMP R5, #0 0x20004980: 0xd003 BEQ.N ??SVC_Handler_main_0 0x20004982: 0x2d02 CMP R5, #2 0x20004984: 0xd009 BEQ.N ??SVC_Handler_main_1 0x20004986: 0xd304 BCC.N ??SVC_Handler_main_2 0x20004988: 0xe00b B.N ??SVC_Handler_main_3 SVC_0_Handler( svc_args ); ??SVC_Handler_main_0: 0x2000498a: 0x0020 MOVS R0, R4 0x2000498c: 0xf7ff 0xff9c BL SVC_0_Handler break; 0x20004990: 0xe007 B.N ??SVC_Handler_main_3 SVC_1_Handler( svc_args ); ??SVC_Handler_main_2: 0x20004992: 0x0020 MOVS R0, R4 0x20004994: 0xf7ff 0xff9c BL SVC_1_Handler break; 0x20004998: 0xe003 B.N ??SVC_Handler_main_3 Dummy2_Handler( svc_args ); ??SVC_Handler_main_1: 0x2000499a: 0x0020 MOVS R0, R4 ; R0 = 0x2000A3A0 : ( svc_args ) 0x2000499c: 0xf7ff 0xff9e BL Dummy2_Handler ; 0x200048dc break; 0x200049a0: 0xe7ff B.N ??SVC_Handler_main_3 ; 0x200049a2 } ??SVC_Handler_main_3: ??SVC_Handler_main_4: 0x200049a2: 0xbd31 POP {R0, R4, R5, PC} ; PC <-- 0xFFFFFFF9, ( R0 : dont care ), Reti : restore registers ; R0 R1 R2 R3 R12 R14 PC xPSR : restore from stack void Dummy2_Handler( unsigned int * svc_args ) ; PC be modified : svc_args[6] = svc_args[5]-1 { Dummy2_Handler: ; SP(Dummy2.Entry) -------------------------------------------| 0x200048dc: 0xb4f0 PUSH {R4-R7} ; R0 = 0x2000A3A0 ----------------| | unsigned int a0 = svc_args[0]; ; SP = 0x2000A390 : R3 R4 R5 LR : R0 R1 R2 R3 R12 R14 PC xPSR a4 a5 0x200048de: 0x6801 LDR R1, [R0] ; SP = 0x2000A380 : R4 R5 R6 R7 : R3 R4 R5 LR : R0 R1 R2 R3 R12 R14 PC xPSR a4 a5 unsigned int a1 = svc_args[1]; ; svc_args[0] : R0 0x200048e0: 0x6842 LDR R2, [R0, #0x4] ; svc_args[1] : R1 unsigned int a2 = svc_args[2]; ; svc_args[8] : a4 0x200048e2: 0x6883 LDR R3, [R0, #0x8] ; svc_args[9] : a5 0x20004962 : BL Dummy2 unsigned int a3 = svc_args[3]; ; svc_args[5] : LR : 0x20004966+1 : BL Dummy0 -------+ 0x200048e4: 0x68c4 LDR R4, [R0, #0xc] ; | unsigned int a4 = svc_args[8]; ; 0x2000490E : SVC #2 | 0x200048e6: 0x6a05 LDR R5, [R0, #0x20] ; svc_args[6] : PC : 0x20004910 : XXXXXX | unsigned int a5 = svc_args[9]; ; svc_args[6] = svc_args[5]-1 -----------------------+ 0x200048e8: 0x6a46 LDR R6, [R0, #0x24] ; svc_args[0] = ( a0 + a1 + a2 ) * ( a3 + a4 + a5 ); ; svc_args[0] = result 0x200048ea: 0x1857 ADDS R7, R2, R1 0x200048ec: 0x19df ADDS R7, R3, R7 0x200048ee: 0xeb15 0x0c04 ADDS.W R12, R5, R4 0x200048f2: 0xeb16 0x0c0c ADDS.W R12, R6, R12 0x200048f6: 0xfb0c 0xf707 MUL R7, R12, R7 0x200048fa: 0x6007 STR R7, [R0] svc_args[6] = svc_args[5] - 1; ; svc_args[6] = svc_args[5]-1 0x200048fc: 0x6947 LDR R7, [R0, #0x14] ; return to Caller of Dummy2() after exit SVC_Handler_main() 0x200048fe: 0x1e7f SUBS R7, R7, #1 ; 0x20004900: 0x6187 STR R7, [R0, #0x18] ; } 0x20004902: 0xbcf0 POP {R4-R7} ; 0x20004904: 0x4770 BX LR ; return to SVC_Handler_main()| 0x20004906: 0x0000 MOVS R0, R0 ; | 0x2000499c: 0xf7ff 0xff9e BL Dummy2_Handler ; 0x200048dc | break; ; | 0x200049a0: 0xe7ff B.N ??SVC_Handler_main_3 ; 0x200049a2 <----------------+ } ??SVC_Handler_main_3: ??SVC_Handler_main_4: ; PC be modified : svc_args[6] = svc_args[5]-1 0x200049a2: 0xbd31 POP {R0, R4, R5, PC} ; return to Caller of Dummy2()| ; PC <-- 0xFFFFFFF9 | 0x20004962: 0xf7ff 0xffd1 BL Dummy2 ; | Dummy0(); ; | 0x20004966: 0xf7ff 0xffb7 BL Dummy0 ; 0x200048d8 <----------------+ Dummy1(); ; 0x2000496a: 0xf7ff 0xffb6 BL Dummy1 ; 0x200048da } 0x2000496e: 0xe8bd 0x83f7 POP.W {R0-R2, R4-R9, PC} ; return to caller of SVC_Demo()