memcpy.s

来自「SAMSUNG S3C6410 CPU BSP for winmobile6」· S 代码 · 共 660 行 · 第 1/2 页

;++
;
; Module Name:
;
;    memcpy.s
;

;--
;

        OPT     2       ; disable listing
        INCLUDE kxarm.h
;        INCLUDE addrtab_cfg.inc
        OPT     1       ; reenable listing
		
        TEXTAREA
	
	 	EXPORT	memcpy32
	
memcpy32
; Check for zero length or source and target being the same

	cmp		r2, #0
    cmpne	r0, r1
    addeq	r0, r0, r2
	moveq	pc, lr ; __JUMP(eq,lr);

;	If <16 bytes, just do byte moves

    cmp		r2,	#15
	bhi		large_copy

	cmp		r1, r0
	bhi		small_copy_fwd
	add		r0, r0, r2
	add		r1, r1, r2
	mov		r3, r0
	add		r12, pc, #small_copy_back_end-.-8
	sub		pc, r12, r2, lsl #3

	ldrb	r12, [r1, #-1]!
	strb	r12, [r3, #-1]!
	ldrb	r12, [r1, #-1]!
	strb	r12, [r3, #-1]!
	ldrb	r12, [r1, #-1]!
	strb	r12, [r3, #-1]!
	ldrb	r12, [r1, #-1]!
	strb	r12, [r3, #-1]!
	ldrb	r12, [r1, #-1]!
	strb	r12, [r3, #-1]!
	ldrb	r12, [r1, #-1]!
	strb	r12, [r3, #-1]!
	ldrb	r12, [r1, #-1]!
	strb	r12, [r3, #-1]!
	ldrb	r12, [r1, #-1]!
	strb	r12, [r3, #-1]!
	ldrb	r12, [r1, #-1]!
	strb	r12, [r3, #-1]!
	ldrb	r12, [r1, #-1]!
	strb	r12, [r3, #-1]!
	ldrb	r12, [r1, #-1]!
	strb	r12, [r3, #-1]!
	ldrb	r12, [r1, #-1]!
	strb	r12, [r3, #-1]!
	ldrb	r12, [r1, #-1]!
	strb	r12, [r3, #-1]!
	ldrb	r12, [r1, #-1]!
	strb	r12, [r3, #-1]!
	ldrb	r12, [r1, #-1]!
	strb	r12, [r3, #-1]!
	
small_copy_back_end
	mov		pc, lr ;__JUMP(,lr)

small_copy_fwd
	add		r12, pc, #small_copy_fwd_end-.-8
	sub		pc, r12, r2, lsl #3

	ldrb	r12, [r1], #1
	strb	r12, [r0], #1
	ldrb	r12, [r1], #1
	strb	r12, [r0], #1
	ldrb	r12, [r1], #1
	strb	r12, [r0], #1
	ldrb	r12, [r1], #1
	strb	r12, [r0], #1
	ldrb	r12, [r1], #1
	strb	r12, [r0], #1
	ldrb	r12, [r1], #1
	strb	r12, [r0], #1
	ldrb	r12, [r1], #1
	strb	r12, [r0], #1
	ldrb	r12, [r1], #1
	strb	r12, [r0], #1
	ldrb	r12, [r1], #1
	strb	r12, [r0], #1
	ldrb	r12, [r1], #1
	strb	r12, [r0], #1
	ldrb	r12, [r1], #1
	strb	r12, [r0], #1
	ldrb	r12, [r1], #1
	strb	r12, [r0], #1
	ldrb	r12, [r1], #1
	strb	r12, [r0], #1
	ldrb	r12, [r1], #1
	strb	r12, [r0], #1
	ldrb	r12, [r1], #1
	strb	r12, [r0], #1

small_copy_fwd_end
	mov 	pc, lr    ;__JUMP(,lr)

large_copy

; Check for possible overlap of source and target

    cmp		r1, r0
    bcc		copy_back
	stmfd	sp!, {r4-r11,lr}

; Do byte moves if necessary to word-align destination

    movs	r3, r0, lsl #30
	rsbne	r3, r3, #0			; r3=number of bytes to align destination
	msr		cpsr_f, r3
	ldrmib	lr, [r1], #1		; move bytes to align destination
	strmib	lr, [r0], #1
	ldrmib	lr, [r1], #1
	strmib	lr, [r0], #1
	ldreqb	lr, [r1], #1
	streqb	lr, [r0], #1
	sub		r2, r2, r3, lsr #30	; adjust length

; Normal copy forwards. r0 should point to end address on exit
; Destination now word-aligned; if source is also word-aligned, do aligned copy.

    ands	r12, r1, #3		; r12=alignment of source
    beq		same_alignment_copy_fore

	bic		r1, r1, #3		; align source
	ldr		r11, [r1], #4	; get first word

	cmp		r12, #2
	beq		start_copy_fwd_nonaligned_2
	bhi		start_copy_fwd_nonaligned_3
	
start_copy_fwd_nonaligned_1

	movs	lr, r2, lsr #5	; lr=number of 32-byte blocks
	beq		copy_fwd_smaller_1
copy_fwd_nonaligned_1
	mov		r3, r11, lsr #8
	ldmia	r1!, {r4-r11}
	orr		r3, r3, r4, lsl #24
	mov		r4, r4, lsr #8
	orr		r4, r4, r5, lsl #24
	mov		r5, r5, lsr #8
	orr		r5, r5, r6, lsl #24
	mov		r6, r6, lsr #8
	orr		r6, r6, r7, lsl #24
	mov		r7, r7, lsr #8
	orr		r7, r7, r8, lsl #24
	mov		r8, r8, lsr #8
	orr		r8, r8, r9, lsl #24
	mov		r9, r9, lsr #8
	orr		r9, r9, r10, lsl #24
	mov		r10, r10, lsr #8
	orr		r10, r10, r11, lsl #24
	stmia	r0!, {r3-r10}
	subs	lr, lr, #1
	bne		copy_fwd_nonaligned_1
	
copy_fwd_smaller_1
	mov		r2, r2, lsl #26
	msr		cpsr_f, r2
	moveq	r3, r11, lsr #8
	ldmeqia	r1!, {r4-r6,r11}
	orreq	r3, r3, r4, lsl #24
	moveq	r4, r4, lsr #8
	orreq	r4, r4, r5, lsl #24
	moveq	r5, r5, lsr #8
	orreq	r5, r5, r6, lsl #24
	moveq	r6, r6, lsr #8
	orreq	r6, r6, r11, lsl #24
	stmeqia	r0!, {r3-r6}
	movcs	r3, r11, lsr #8
	ldmcsia	r1!, {r4,r11}
	orrcs	r3, r3, r4, lsl #24
	movcs	r4, r4, lsr #8
	orrcs	r4, r4, r11, lsl #24
	stmcsia	r0!, {r3,r4}
	movvs	r3, r11, lsr #8
	ldmvsia	r1!, {r11}
	orrvs	r3, r3, r11, lsl #24
	stmvsia	r0!, {r3}
	b		copy_fwd_remainder
start_copy_fwd_nonaligned_2
	movs	lr, r2, lsr #5	; lr=number of 32-byte blocks
	beq		copy_fwd_smaller_2
copy_fwd_nonaligned_2
	mov		r3, r11, lsr #16
	ldmia	r1!, {r4-r11}
	orr		r3, r3, r4, lsl #16
	mov		r4, r4, lsr #16
	orr		r4, r4, r5, lsl #16
	mov		r5, r5, lsr #16
	orr		r5, r5, r6, lsl #16
	mov		r6, r6, lsr #16
	orr		r6, r6, r7, lsl #16
	mov		r7, r7, lsr #16
	orr		r7, r7, r8, lsl #16
	mov		r8, r8, lsr #16
	orr		r8, r8, r9, lsl #16
	mov		r9, r9, lsr #16
	orr		r9, r9, r10, lsl #16
	mov		r10, r10, lsr #16
	orr		r10, r10, r11, lsl #16
	stmia	r0!, {r3-r10}
	subs	lr, lr, #1
	bne		copy_fwd_nonaligned_2
	
copy_fwd_smaller_2
	mov		r2, r2, lsl #26
	msr		cpsr_f, r2
	moveq	r3, r11, lsr #16
	ldmeqia	r1!, {r4-r6,r11}
	orreq	r3, r3, r4, lsl #16
	moveq	r4, r4, lsr #16
	orreq	r4, r4, r5, lsl #16
	moveq	r5, r5, lsr #16
	orreq	r5, r5, r6, lsl #16
	moveq	r6, r6, lsr #16
	orreq	r6, r6, r11, lsl #16
	stmeqia	r0!, {r3-r6}
	movcs	r3, r11, lsr #16
	ldmcsia	r1!, {r4,r11}
	orrcs	r3, r3, r4, lsl #16
	movcs	r4, r4, lsr #16
	orrcs	r4, r4, r11, lsl #16
	stmcsia	r0!, {r3,r4}
	movvs	r3, r11, lsr #16
	ldmvsia	r1!, {r11}
	orrvs	r3, r3, r11, lsl #16
	stmvsia	r0!, {r3}
	b		copy_fwd_remainder
	
start_copy_fwd_nonaligned_3
	movs	lr, r2, lsr #5   ;lr=number of 32-byte blocks
	beq		copy_fwd_smaller_3 
copy_fwd_nonaligned_3
	mov		r3, r11, lsr #24
	ldmia	r1!, {r4-r11}
	orr		r3, r3, r4, lsl #8
	mov		r4, r4, lsr #24
	orr		r4, r4, r5, lsl #8
	mov		r5, r5, lsr #24
	orr		r5, r5, r6, lsl #8
	mov		r6, r6, lsr #24
	orr		r6, r6, r7, lsl #8
	mov		r7, r7, lsr #24
	orr		r7, r7, r8, lsl #8
	mov		r8, r8, lsr #24
	orr		r8, r8, r9, lsl #8
	mov		r9, r9, lsr #24
	orr		r9, r9, r10, lsl #8
	mov		r10, r10, lsr #24
	orr		r10, r10, r11, lsl #8
	stmia	r0!, {r3-r10}
	subs	lr, lr, #1
	bne		copy_fwd_nonaligned_3
	
copy_fwd_smaller_3
	mov		r2, r2, lsl #26
	msr		cpsr_f, r2
	moveq	r3, r11, lsr #24
	ldmeqia	r1!, {r4-r6,r11}
	orreq	r3, r3, r4, lsl #8
	moveq	r4, r4, lsr #24
	orreq	r4, r4, r5, lsl #8
	moveq	r5, r5, lsr #24
	orreq	r5, r5, r6, lsl #8
	moveq	r6, r6, lsr #24
	orreq	r6, r6, r11, lsl #8
	stmeqia	r0!, {r3-r6}
	movcs	r3, r11, lsr #24
	ldmcsia	r1!, {r4,r11}
	orrcs	r3, r3, r4, lsl #8
	movcs	r4, r4, lsr #24
	orrcs	r4, r4, r11, lsl #8
	stmcsia	r0!, {r3,r4}
	movvs	r3, r11, lsr #24
	ldmvsia	r1!, {r11}
	orrvs	r3, r3, r11, lsl #8
	stmvsia	r0!, {r3}

copy_fwd_remainder
	sub		r1, r1, #4
	mov		r2, r2, lsl #2
	msr		cpsr_f, r2
	add		r1, r1, r12		; true source address (nonaligned)
	ldrcsb	lr, [r1], #1		; do byte copies to finish
	strcsb	lr, [r0], #1 
	ldrcsb	lr, [r1], #1 
	strcsb	lr, [r0], #1 
	ldrvsb	lr, [r1], #1
	strvsb	lr, [r0], #1
	ldmfd	sp!, {r4-r11, pc} ;__POPRET("r4-r11,")
	

; Source is before target, so need to copy backwards incase they overlap

copy_back
	add		r0, r0, r2			; r0=last dest address+1
	add		r1, r1, r2				; r1=last source address+1
	stmfd	sp!, {r0,r4-r11,lr}		; save registers and return value
    mov		r3, r0, lsl #30			; destination alignment into r3
	msr		cpsr_f, r3			; and into N,Z flags
	ldrmib	lr, [r1, #-1]!			; do byte moves to align destination
	strmib	lr, [r0, #-1]!
	ldrmib	lr, [r1, #-1]!
	strmib	lr, [r0, #-1]!
	ldreqb	lr, [r1, #-1]!
	streqb	lr, [r0, #-1]!
	sub		r2, r2, r3, lsr #30	; adjust length

	ands	r12, r1, #3					; r12=alignment of source
    beq		same_alignment_copy_back	; if source also aligned, do aligned copy

	bic		r1, r1, #3		; align source