#include <stdio.h>

#include <stlink.h>
#include <string.h>

static void usage(void) {
    puts("test-usb --reset");
    puts("test-usb --no-reset");
}

int main(int ac, char** av) {
    stlink_t* sl;
    struct stlink_reg regs;
    int reset = 0;

    if (ac == 2) {
        if (strcmp(av[1], "--reset") == 0)
            reset = 2;

        if (strcmp(av[1], "--no-reset") == 0)
            reset = 1;
    }

    if (reset == 0) {
        usage();
        return(0);
    }

    sl = stlink_open_usb(10, reset, NULL, 0);

    if (sl != NULL) {
        printf("-- version\n");
        stlink_version(sl);

        printf("mode before doing anything: %d\n", stlink_current_mode(sl));

        if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) {
            printf("-- exit_dfu_mode\n");
            stlink_exit_dfu_mode(sl);
        }

        printf("-- enter_swd_mode\n");
        stlink_enter_swd_mode(sl);

        printf("-- mode after entering swd mode: %d\n", stlink_current_mode(sl));

        printf("-- chip id: %#x\n", sl->chip_id);
        printf("-- core_id: %#x\n", sl->core_id);

        cortex_m3_cpuid_t cpuid;

        if (stlink_cpu_id(sl, &cpuid)) {
            printf("Failed reading stlink_cpu_id\n");
        } else {
            printf("cpuid:impl_id = %0#x, variant = %#x\n", cpuid.implementer_id, cpuid.variant);
            printf("cpuid:part = %#x, rev = %#x\n", cpuid.part, cpuid.revision);
        }

        printf("-- read_sram\n");
        static const uint32_t sram_base = STM32_SRAM_BASE;
        uint32_t off;

        for (off = 0; off < 16; off += 4)
            stlink_read_mem32(sl, sram_base + off, 4);

        printf("FP_CTRL\n");
        stlink_read_mem32(sl, STLINK_REG_CM3_FP_CTRL, 4);

        // no idea what reg this is...
        // stlink_read_mem32(sl, 0xe000ed90, 4);
        // no idea what register this is...
        // stlink_read_mem32(sl, 0xe000edf0, 4);
        // offset 0xC into TIM11 register? TIMx_DIER?
        // stlink_read_mem32(sl, 0x4001100c, 4);

        /* Test 32 bit write */
        write_uint32(sl->q_buf, 0x01234567);
        stlink_write_mem32(sl, 0x200000a8, 4);
        write_uint32(sl->q_buf, 0x89abcdef);
        stlink_write_mem32(sl, 0x200000ac, 4);
        stlink_read_mem32(sl, 0x200000a8, 4);
        stlink_read_mem32(sl, 0x200000ac, 4);

        /* Test 8 bit write */
        write_uint32(sl->q_buf, 0x01234567);
        stlink_write_mem8(sl, 0x200001a8, 3);
        write_uint32(sl->q_buf, 0x89abcdef);
        stlink_write_mem8(sl, 0x200001ac, 3);
        stlink_read_mem32(sl, 0x200001a8, 4);
        stlink_read_mem32(sl, 0x200001ac, 4);

        printf("-- status\n");
        stlink_status(sl);

        printf("-- reset\n");
        stlink_reset(sl, RESET_AUTO);
        stlink_force_debug(sl);
        /* Test reg write */
        stlink_write_reg(sl, 0x01234567, 3);
        stlink_write_reg(sl, 0x89abcdef, 4);
        stlink_write_reg(sl, 0x12345678, 15);

        for (off = 0; off < 21; off += 1) stlink_read_reg(sl, off, &regs);

        stlink_read_all_regs(sl, &regs);

        printf("-- status\n");
        stlink_status(sl);

        printf("-- step\n");
        stlink_step(sl);

        printf("-- run\n");
        stlink_run(sl, RUN_NORMAL);

        printf("-- exit_debug_mode\n");
        stlink_exit_debug_mode(sl);

        stlink_close(sl);
    }

    return(0);
}