moa_memops 0.1.4

use core::arch::naked_asm;

use moa_vmem::PAGE_SIZE;

#[unsafe(naked)]
#[unsafe(no_mangle)]
extern "C" fn clear_page(addr: usize) {
    naked_asm!(
        "mrs x1, dczid_el0",
        "and w1, w1, #0xf",
        "mov x2, #4",
        "lsl x1, x2, x1",
        "1: dc zva, x0",
        "add x0, x0, x1",
        "tst x0, #{page_mask}",
        "b.ne 1b",
        "ret",
        page_mask = const (PAGE_SIZE - 1),
    );
}

// 以下 mem* 用 GP 寄存器（非 SIMD，内核 FP/SIMD 被 trap）：ldp/stp 一次 16 字节 + 字节收尾。
// 硬件允许非对齐访问（SCTLR.A=0），故无需对齐前导。x0/a0 保留作返回值。

#[unsafe(naked)]
#[unsafe(no_mangle)]
extern "C" fn memcpy(dest: *mut u8, src: *const u8, n: usize) -> *mut u8 {
    naked_asm!(
        "mov  x3, x0", // x3 = 移动 dest，x0 保留为返回值
        "1:",          // 16 字节主循环
        "cmp  x2, #16",
        "b.lo 2f",
        "ldp  x4, x5, [x1], #16",
        "stp  x4, x5, [x3], #16",
        "sub  x2, x2, #16",
        "b    1b",
        "2:", // 8 字节
        "cmp  x2, #8",
        "b.lo 3f",
        "ldr  x4, [x1], #8",
        "str  x4, [x3], #8",
        "sub  x2, x2, #8",
        "3:", // 字节收尾
        "cbz  x2, 4f",
        "5:",
        "ldrb w4, [x1], #1",
        "strb w4, [x3], #1",
        "sub  x2, x2, #1",
        "cbnz x2, 5b",
        "4:",
        "ret",
    );
}

#[unsafe(naked)]
#[unsafe(no_mangle)]
extern "C" fn memmove(dest: *mut u8, src: *const u8, n: usize) -> *mut u8 {
    naked_asm!(
        "mov  x3, x0",
        "cmp  x0, x1",
        "b.ls 1f", // dest <= src：前向拷贝
        // 后向：从高地址往低拷
        "add  x3, x3, x2",
        "add  x1, x1, x2",
        "2:",
        "cmp  x2, #8",
        "b.lo 3f",
        "ldr  x4, [x1, #-8]!",
        "str  x4, [x3, #-8]!",
        "sub  x2, x2, #8",
        "b    2b",
        "3:",
        "cbz  x2, 4f",
        "5:",
        "ldrb w4, [x1, #-1]!",
        "strb w4, [x3, #-1]!",
        "sub  x2, x2, #1",
        "cbnz x2, 5b",
        "4:",
        "ret",
        "1:", // 前向
        "6:",
        "cmp  x2, #8",
        "b.lo 7f",
        "ldr  x4, [x1], #8",
        "str  x4, [x3], #8",
        "sub  x2, x2, #8",
        "b    6b",
        "7:",
        "cbz  x2, 8f",
        "9:",
        "ldrb w4, [x1], #1",
        "strb w4, [x3], #1",
        "sub  x2, x2, #1",
        "cbnz x2, 9b",
        "8:",
        "ret",
    );
}

#[unsafe(naked)]
#[unsafe(no_mangle)]
extern "C" fn memset(dest: *mut u8, c: i32, n: usize) -> *mut u8 {
    naked_asm!(
        "mov  x3, x0",
        "and  w1, w1, #0xff", // 把待填字节复制到 x1 的全部 8 字节
        "orr  x1, x1, x1, lsl #8",
        "orr  x1, x1, x1, lsl #16",
        "orr  x1, x1, x1, lsl #32",
        "1:",
        "cmp  x2, #16",
        "b.lo 2f",
        "stp  x1, x1, [x3], #16",
        "sub  x2, x2, #16",
        "b    1b",
        "2:",
        "cmp  x2, #8",
        "b.lo 3f",
        "str  x1, [x3], #8",
        "sub  x2, x2, #8",
        "3:",
        "cbz  x2, 4f",
        "5:",
        "strb w1, [x3], #1",
        "sub  x2, x2, #1",
        "cbnz x2, 5b",
        "4:",
        "ret",
    );
}

#[unsafe(naked)]
#[unsafe(no_mangle)]
extern "C" fn memcmp(s1: *const u8, s2: *const u8, n: usize) -> i32 {
    naked_asm!(
        "1:", // 按字跳过相等前缀
        "cmp  x2, #8",
        "b.lo 2f",
        "ldr  x3, [x0]",
        "ldr  x4, [x1]",
        "cmp  x3, x4",
        "b.ne 2f", // 该字不同：落到逐字节定位首个差异
        "add  x0, x0, #8",
        "add  x1, x1, #8",
        "sub  x2, x2, #8",
        "b    1b",
        "2:", // 逐字节比较
        "cbz  x2, 3f",
        "6:",
        "ldrb w3, [x0], #1",
        "ldrb w4, [x1], #1",
        "sub  x2, x2, #1",
        "subs w5, w3, w4",
        "b.eq 7f",
        "mov  w0, w5", // 返回首个差异字节的 (a - b)
        "ret",
        "7:",
        "cbnz x2, 6b",
        "3:",
        "mov  w0, #0",
        "ret",
    );
}