#include <nds.h>

#include <stdio.h>

#define PROLOG(token)\
    "push {r0-r7}\n\t"\
    "ldr r0, ="token"\n\t"\
    "blx r0\n\t"\
    "b "token"_ret\n\t"\
    ".align 4\n\t"\
    ".arm\n\t"\
    token":\n\t"

#define EPILOG(token)\
    ".align 2\n\t"\
    ".thumb\n\t"\
    token"_ret:\n\t"\
    "pop {r0-r7}\n\t"\

long long var;
int r2;
int r2_new;

int main(void) {
    consoleDemoInit();
    puts("Testing ldmia...\n");
    var = 0x1122334455667788;

    // ldmia, base first in list, writeback
    puts("ldmia r2!, {r2, r3}");
    asm
    (
        PROLOG("ldmia1")
        "ldr r2, =var\n\t"
        "ldr r1, =r2\n\t"
        "str r2, [r1]\n\t"
        "ldmia r2!, {r2, r3}\n\t"
        "ldr r4, =r2_new\n\t"
        "str r2, [r4]\n\t"
        "bx lr\n\t"
        EPILOG("ldmia1")
    );
    printf("r2 (old) = 0x%x\n", r2);
    printf("r2 (new) = 0x%x\n", r2_new);
    if (r2_new == r2 + 8)
    {
        puts("[OK]\n");
    }
    else
    {
        puts("[FAIL]\n");
    }

    // ldmia, base second in list, writeback
    puts("ldmia r2!, {r1, r2}");
    asm
    (
        PROLOG("ldmia2")
        "ldr r2, =var\n\t"
        "ldr r1, =r2\n\t"
        "str r2, [r1]\n\t"
        "ldmia r2!, {r1, r2}\n\t"
        "ldr r3, =r2_new\n\t"
        "str r2, [r3]\n\t"
        "bx lr\n\t"
        EPILOG("ldmia2")
    );
    printf("r2 (old) = 0x%x\n", r2);
    printf("r2 (new) = 0x%x\n", r2_new);
    if (r2_new == 0x11223344)
    {
        puts("[OK]\n");
    }
    else
    {
        puts("[FAIL]\n");
    }

    while(1) {
        swiWaitForVBlank();
    }

    return 0;
}