Attempt at string functions

// r0 = destination, r1 = source, r2 = length, r3 = op (b2: str2 is 16-bit, b1: str1 is 16-bit, b0: 0 = compare, 1 = copy)
// reads use r4 and return in r5, writes use r0 and r5

push {r4-r9, lr}
and r7, r3, #4
and r6, r3, #2
mov r6, r6 lsl #1
mov r3, r3, lsl #2
and r3, r3, #4
add r7, r7, r3 lsl #1
add r4, pc, #0xC
add r5, r4, #0x10
ldr r6, [r4, r6]
ldr r7, [r4, r7]
ldr r3, [r5, r3]
bx r3
.word read_char8
.word read_char16
.word write_char8
.word write_char16
.word str_compare
.word str_copy

str_copy:
push {r0}
mov r4, r1
loop_copy:
bl character_op_2
cmp r5, #0
moveq r2, #1
bl character_op_1
subs r2, r2, #1
bne loop_copy
pop {r0, r4-r9, pc}

str_compare:
mov r4, r0
bl character_op_1
push {r5}
mov r0, r4
mov r4, r1
bl character_op_2
mov r1, r4
pop {r3}
subs r3, r3, r5
bne done_compare
subs r2, r2, #1
beq done_compare
cmp r5, #0
bne str_compare
done_compare:
mov r0, r3
pop {r4-r9, pc}

character_op_1:
bx r6

character_op_2:
bx r7

read_char8:
ldrb r5, [r4], #1
eor r3, r5, #0xFF
clz r8, r3
subs r8, #25
bxle lr
rsb r8, r8, #6
mov r3, #1
rsb r3, r3, r3 lsl r8
and r5, r5, r3
mov r3, r8 lsl #1
add r3, r3, r8 lsl #2
mov r5, r5 lsl r3
loop_read8:
ldrb r8, [r4], #1
subs r3, r3, #6
and r8, r8, #0x3F
orr r5, r5, r8 lsl r3
bne loop_read8
bx lr

read_char16:
ldrh r5, [r4], #2
and r3, r5, #0xFC00
cmp r3, #0xD800
bxne lr
bic r5, r5, #0xFC00
mov r5, r5 lsl #10
add r5, r5, #0x10000
ldrh r3, [r4], #2
bic r3, r3, #0xFC00
add r5, r5, r3
bx lr

write_char8:
bics r3, r5, #0x7F
streqb r5, [r0], #1
bxeq lr
mov r8, #0
sub r3, r8, #0x200000
bic r5, r5, r3
mov r3, #26
loop_write8:
and r9, r5, #0x3F
orr r9, r9, #0x80
orr r8, r9, r8 lsl #8
mov r5, r5 lsr #6
clz r9, r5
add r3, r3, #1
cmp r9, r3
blt loop_write8
sub r3, r3, #2
mov r9, #0x80000000
orr r9, r5, r9 asr r3
strb r9, [r0], #1
loop_shiftout:
strb r8, [r0], #1
movs r8, r8 lsr #8
bne loop_shiftout
bx lr

write_char16:
tst r5, #0x1F0000
streqh r5, [r0], #2
bxeq lr
mov r3, r5 lsr #10
eor r5, r5, r3 lsl #10
orr r5, r5, #0xDC00
sub r3, r3, #0x40
orr r5, r5, #0xD800
strh r3, [r0], #2
strh r5. [r0], #2
bx lr