[picodrive.git] / Pico / Misc.s

@ vim:filetype=armasm

@ Generic memory routines.
@ (c) Copyright 2007, Grazvydas "notaz" Ignotas


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