#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()
