memset的代碼實現

初學組合語言，寫了一個memset的實現代碼，記錄一下，呵呵

;**********************************************/
;函數MEMCPY
;
;**********************************************/
_MAIN
 LDR R4,=0x40000000
 LDR R5,=13
 LDR R6,=0xFF
 LDR SP,=0x40001000
 
 MOV R0,R4
 ADD R1,R5
 MOV R2,R6

 ADD LR,PC,#4
 STMFD SP!,{R0-R4, LR}
 B _MEMSET
     
 b .

_MEMSET;(address R0,size R1,content R2)  memset
 ADD LR,PC,#4
 STMFD SP!,{R0-R4, LR}
 B _MODE4
 MOV R4,R6  ;R4 STORE start addr's mode RESULT
 RSB R4,R4,#4
 CMP R4,#4
 MOVEQ R4,#0
 CMP R4,#0
 STRBNE  R2,[R0]  ;R4最多為3，故作3次判斷
 CMPNE R4,#1
 STRBNE  R2,[R0,#1]
 CMPNE R4,#2
 STRBNE  R2,[R0,#2]
 CMPNE R4,#3
 STRBNE  R2,[R0,#3]

 ADD R0,R0,R4   ;起始地址增加
 SUB R0,#4
 SUB R1,R1,R4   ;總大小減少R4的位元組數

 ADD LR,PC,#8
 STMFD SP!,{R0-R4, LR}
 MOV R5,#0
 B _DIV4
 MOV R4,R5 ;R4 STORE the DIV result
 ADD R2,R2,LSL #8
 ADD R2,R2,LSL #16

LOOP1
 SUBS R4,#1 
 STRPL  R2,[R0,#4]! 
 BPL  LOOP1

 ADD R0,#4 
 ADD LR,PC,#8
 STMFD SP!,{R0-R4, LR}
 MOV R0,R1
 B _MODE4
 MOV R4,R6  ;R4 STORE start addr's mode RESULT
 CMP R4,#0
 STRBNE  R2,[R0]  ;R4最多為3，故作3次判斷
 CMPNE R4,#1
 STRBNE  R2,[R0,#1]
 CMPNE R4,#2
 STRBNE  R2,[R0,#2]
 CMPNE R4,#3
 STRBNE  R2,[R0,#3]
 
 LDMFD SP!,{R0-R4,PC}

_DIV4;(R1) => R5 :R5 store result
 LSR  R1,#2
 MOV  R5,R1
 LDMFD  SP!,{R0-R4,PC}

_MODE4;(R0) => R6 :R6 store result
 AND  R6,R0,#0x03
 LDMFD   SP!,{R0-R4,PC}
 END