[picodrive.git] / Pico / misc.s


@ vim:filetype=armasm


.global memcpy16 @ unsigned short *dest, unsigned short *src, int count

memcpy16:
    eor     r3, r0, r1
    tst     r3, #2
    bne     mcp16_cant_align

    tst     r0, #2
    ldrneh  r3, [r1], #2
    subne   r2, r2, #1
    strneh  r3, [r0], #2

    subs    r2, r2, #4
    bmi     mcp16_fin

mcp16_loop:
    ldmia   r1!, {r3,r12}
    subs    r2, r2, #4
    stmia   r0!, {r3,r12}
    bpl     mcp16_loop

mcp16_fin:
    tst     r2, #2
    ldrne   r3, [r1], #4
    strne   r3, [r0], #4
    ands    r2, r2, #1
    bxeq    lr

mcp16_cant_align:
    ldrh    r3, [r1], #2
    subs    r2, r2, #1
    strh    r3, [r0], #2
    bne     mcp16_cant_align

    bx      lr


@ 0x12345678 -> 0x34127856
@ r4=temp, lr=0x00ff00ff
.macro bswap reg
    and     r4,   \reg, lr
    and     \reg, lr,   \reg, lsr #8
    orr     \reg, \reg, r4,   lsl #8
.endm


@ dest must be halfword aligned, src can be unaligned
.global memcpy16bswap @ unsigned short *dest, void *src, int count

memcpy16bswap:
    tst     r1, #1
    bne     mcp16bs_cant_align2

    eor     r3, r0, r1
    tst     r3, #2
    bne     mcp16bs_cant_align

    tst     r0, #2
    beq     mcp16bs_aligned
    ldrh    r3, [r1], #2
    sub     r2, r2, #1
    orr     r3, r3, r3, lsl #16
    mov     r3, r3, lsr #8
    strh    r3, [r0], #2

mcp16bs_aligned:
    stmfd   sp!, {r4,lr}
    mov     lr, #0xff
    orr     lr, lr, lr, lsl #16

    subs    r2, r2, #4
    bmi     mcp16bs_fin4

mcp16bs_loop:
    ldmia   r1!, {r3,r12}
    subs    r2, r2, #4
    bswap   r3
    bswap   r12
    stmia   r0!, {r3,r12}
    bpl     mcp16bs_loop

mcp16bs_fin4:
    tst     r2, #2
    beq     mcp16bs_fin2
    ldr     r3, [r1], #4
    bswap   r3
    str     r3, [r0], #4

mcp16bs_fin2:
    ldmfd   sp!, {r4,lr}
    ands    r2, r2, #1
    bxeq    lr

mcp16bs_cant_align:
    ldrh    r3, [r1], #2
    subs    r2, r2, #1
    orr     r3, r3, r3, lsl #16
    mov     r3, r3, lsr #8
    strh    r3, [r0], #2
    bne     mcp16bs_cant_align
    bx      lr

    @ worst case
mcp16bs_cant_align2:
    ldrb    r3, [r1], #1
    ldrb    r12,[r1], #1
    subs    r2, r2, #1
    mov     r3, r3, lsl #8
    orr     r3, r3, r12
    strh    r3, [r0], #2
    bne     mcp16bs_cant_align2
    bx      lr


.global memcpy32 @ int *dest, int *src, int count

memcpy32:
    stmfd   sp!, {r4,lr}

    subs    r2, r2, #4
    bmi     mcp32_fin

mcp32_loop:
    ldmia   r1!, {r3,r4,r12,lr}
    subs    r2, r2, #4
    stmia   r0!, {r3,r4,r12,lr}
    bpl     mcp32_loop

mcp32_fin:
    tst     r2, #3
    ldmeqfd sp!, {r4,pc}
    tst     r2, #1
    ldrne   r3, [r1], #4
    strne   r3, [r0], #4

mcp32_no_unal1:
    tst     r2, #2
    ldmneia r1!, {r3,r12}
    ldmfd   sp!, {r4,lr}
    stmneia r0!, {r3,r12}
    bx      lr


.global memset32 @ int *dest, int c, int count

memset32:
    stmfd   sp!, {lr}

    mov     r3, r1
    subs    r2, r2, #4
    bmi     mst32_fin

    mov     r12,r1
    mov     lr, r1

mst32_loop:
    subs    r2, r2, #4
    stmia   r0!, {r1,r3,r12,lr}
    bpl     mst32_loop

mst32_fin:
    tst     r2, #1
    strne   r1, [r0], #4

    tst     r2, #2
    stmneia r0!, {r1,r3}

    ldmfd   sp!, {lr}
    bx      lr
Commit	Line	Data
fa1e5e29	1
	2	@ vim:filetype=armasm
	3
	4
	5	.global memcpy16 @ unsigned short dest, unsigned short src, int count
	6
	7	memcpy16:
	8	eor r3, r0, r1
	9	tst r3, #2
	10	bne mcp16_cant_align
	11
	12	tst r0, #2
	13	ldrneh r3, [r1], #2
	14	subne r2, r2, #1
	15	strneh r3, [r0], #2
	16
	17	subs r2, r2, #4
	18	bmi mcp16_fin
	19
	20	mcp16_loop:
	21	ldmia r1!, {r3,r12}
	22	subs r2, r2, #4
	23	stmia r0!, {r3,r12}
	24	bpl mcp16_loop
	25
	26	mcp16_fin:
	27	tst r2, #2
	28	ldrne r3, [r1], #4
	29	strne r3, [r0], #4
	30	ands r2, r2, #1
	31	bxeq lr
	32
	33	mcp16_cant_align:
	34	ldrh r3, [r1], #2
	35	subs r2, r2, #1
	36	strh r3, [r0], #2
	37	bne mcp16_cant_align
	38
	39	bx lr
	40
	41
	42
0a051f55	43	@ 0x12345678 -> 0x34127856
	44	@ r4=temp, lr=0x00ff00ff
	45	.macro bswap reg
	46	and r4, \reg, lr
	47	and \reg, lr, \reg, lsr #8
	48	orr \reg, \reg, r4, lsl #8
	49	.endm
	50
	51
	52	@ dest must be halfword aligned, src can be unaligned
	53	.global memcpy16bswap @ unsigned short dest, void src, int count
	54
	55	memcpy16bswap:
	56	tst r1, #1
	57	bne mcp16bs_cant_align2
	58
	59	eor r3, r0, r1
	60	tst r3, #2
	61	bne mcp16bs_cant_align
	62
	63	tst r0, #2
	64	beq mcp16bs_aligned
	65	ldrh r3, [r1], #2
	66	sub r2, r2, #1
	67	orr r3, r3, r3, lsl #16
	68	mov r3, r3, lsr #8
	69	strh r3, [r0], #2
	70
	71	mcp16bs_aligned:
	72	stmfd sp!, {r4,lr}
	73	mov lr, #0xff
	74	orr lr, lr, lr, lsl #16
	75
	76	subs r2, r2, #4
	77	bmi mcp16bs_fin4
	78
	79	mcp16bs_loop:
	80	ldmia r1!, {r3,r12}
	81	subs r2, r2, #4
	82	bswap r3
	83	bswap r12
	84	stmia r0!, {r3,r12}
	85	bpl mcp16bs_loop
	86
	87	mcp16bs_fin4:
	88	tst r2, #2
	89	beq mcp16bs_fin2
	90	ldr r3, [r1], #4
	91	bswap r3
	92	str r3, [r0], #4
	93
	94	mcp16bs_fin2:
	95	ldmfd sp!, {r4,lr}
	96	ands r2, r2, #1
	97	bxeq lr
	98
	99	mcp16bs_cant_align:
	100	ldrh r3, [r1], #2
	101	subs r2, r2, #1
	102	orr r3, r3, r3, lsl #16
	103	mov r3, r3, lsr #8
	104	strh r3, [r0], #2
	105	bne mcp16bs_cant_align
	106	bx lr
107
108	@ worst case
109	mcp16bs_cant_align2:
110	ldrb r3, [r1], #1
111	ldrb r12,[r1], #1
112	subs r2, r2, #1
113	mov r3, r3, lsl #8
114	orr r3, r3, r12
115	strh r3, [r0], #2
116	bne mcp16bs_cant_align2
117	bx lr
118
119
120
fa1e5e29	121	.global memcpy32 @ int dest, int src, int count
	122
	123	memcpy32:
	124	stmfd sp!, {r4,lr}
	125
	126	subs r2, r2, #4
	127	bmi mcp32_fin
	128
	129	mcp32_loop:
	130	ldmia r1!, {r3,r4,r12,lr}
	131	subs r2, r2, #4
	132	stmia r0!, {r3,r4,r12,lr}
	133	bpl mcp32_loop
	134
	135	mcp32_fin:
	136	tst r2, #3
	137	ldmeqfd sp!, {r4,pc}
	138	tst r2, #1
	139	ldrne r3, [r1], #4
	140	strne r3, [r0], #4
	141
	142	mcp32_no_unal1:
	143	tst r2, #2
	144	ldmneia r1!, {r3,r12}
	145	ldmfd sp!, {r4,lr}
	146	stmneia r0!, {r3,r12}
	147	bx lr
	148
	149
	150
	151	.global memset32 @ int *dest, int c, int count
	152
	153	memset32:
	154	stmfd sp!, {lr}
	155
	156	mov r3, r1
	157	subs r2, r2, #4
	158	bmi mst32_fin
	159
	160	mov r12,r1
	161	mov lr, r1
	162
	163	mst32_loop:
	164	subs r2, r2, #4
	165	stmia r0!, {r1,r3,r12,lr}
	166	bpl mst32_loop
	167
	168	mst32_fin:
	169	tst r2, #1
	170	strne r1, [r0], #4
	171
	172	tst r2, #2
	173	stmneia r0!, {r1,r3}
	174
	175	ldmfd sp!, {lr}
	176	bx lr
	177
	178
	179