#include <atomic.h>
#include "cmsis.h"

/*
 * 注意：以下实现依赖于 CMSIS 提供的 CMSIS_LDREXW, CMSIS_STREXW, CMSIS_CLREX 等内建函数。
 * 如果使用 ARMCC，请替换为 __ldrex, __strex, __clrex。
 * 如果使用 IAR，请替换为 __LDREX, __STREX, CMSIS_CLREX。
 */

/* ======================================================================== */
/* 内部辅助宏：编译器屏障                                                     */
/* ======================================================================== */
#if defined(__GNUC__)
#define COMPILER_BARRIER() __asm volatile ("" ::: "memory")
#elif defined(__CC_ARM)
#define COMPILER_BARRIER() __schedule_barrier()
#endif

/* ======================================================================== */
/* 32位 原子操作实现                                                         */
/* ======================================================================== */

uint32_t atomic_load_32(const atomic_uint32_t *obj) {
    // Cortex-M3 普通加载即为原子加载（对齐情况下），但为了语义明确和防止重排
    uint32_t val;
    do {
        val = CMSIS_LDREXW((os_uint_t*)obj);
    } while (CMSIS_STREXW(val, (os_uint_t*)obj)); // 实际上 load 不需要 STREX，直接读即可，但为了严格内存序可加屏障
    // 更高效的 Load:
    return *obj;
}

void atomic_store_32(atomic_uint32_t *obj, uint32_t desired) {
    // 简单存储在某些情况下可能不是原子的（如果涉及中断上下文竞争且非位带），
    // 但对于 M3，32位对齐写入是原子的。为了安全起见，使用独占序列确保完整性。
    uint32_t status;
    do {
        CMSIS_LDREXW((os_uint_t*)obj);
        status = CMSIS_STREXW(desired, (os_uint_t*)obj);
    } while (status != 0);
}

uint32_t atomic_exchange_32(atomic_uint32_t *obj, uint32_t desired) {
    uint32_t old_val;
    uint32_t status;
    do {
        old_val = CMSIS_LDREXW((os_uint_t*)obj);
        status = CMSIS_STREXW(desired, (os_uint_t*)obj);
    } while (status != 0);
    return old_val;
}

bool atomic_compare_exchange_strong_32(atomic_uint32_t *obj, uint32_t *expected, uint32_t desired) {
    uint32_t current;
    uint32_t status;

    do {
        current = CMSIS_LDREXW((os_uint_t*)obj);
        if (current != *expected) {
            CMSIS_CLREX(); // 清除独占标记，避免总线锁定
            *expected = current; // 更新 expected 为当前实际值
            return false;
        }
        status = CMSIS_STREXW(desired, (os_uint_t*)obj);
    } while (status != 0);

    return true;
}

bool atomic_compare_exchange_weak_32(atomic_uint32_t *obj, uint32_t *expected, uint32_t desired) {
    // 在 Cortex-M3 上，Weak 和 Strong 区别不大，因为硬件不支持虚假失败的优化循环通常由软件处理
    // 这里复用 Strong 逻辑，但在某些实现中 Weak 可能在第一次 STREX 失败时直接返回而不重试内部循环
    // 为了标准兼容性，我们提供标准的 CAS 行为
    return atomic_compare_exchange_strong_32(obj, expected, desired);
}

uint32_t atomic_fetch_add_32(atomic_uint32_t *obj, uint32_t operand) {
    uint32_t old_val;
    uint32_t new_val;
    uint32_t status;

    do {
        old_val = CMSIS_LDREXW((os_uint_t*)obj);
        new_val = old_val + operand;
        status = CMSIS_STREXW(new_val, (os_uint_t*)obj);
    } while (status != 0);

    return old_val;
}

uint32_t atomic_fetch_sub_32(atomic_uint32_t *obj, uint32_t operand) {
    uint32_t old_val;
    uint32_t new_val;
    uint32_t status;

    do {
        old_val = CMSIS_LDREXW((os_uint_t*)obj);
        new_val = old_val - operand;
        status = CMSIS_STREXW(new_val, (os_uint_t*)obj);
    } while (status != 0);

    return old_val;
}

uint32_t atomic_fetch_and_32(atomic_uint32_t *obj, uint32_t operand) {
    uint32_t old_val;
    uint32_t new_val;
    uint32_t status;

    do {
        old_val = CMSIS_LDREXW((os_uint_t*)obj);
        new_val = old_val & operand;
        status = CMSIS_STREXW(new_val, (os_uint_t*)obj);
    } while (status != 0);

    return old_val;
}

uint32_t atomic_fetch_or_32(atomic_uint32_t *obj, uint32_t operand) {
    uint32_t old_val;
    uint32_t new_val;
    uint32_t status;

    do {
        old_val = CMSIS_LDREXW((os_uint_t*)obj);
        new_val = old_val | operand;
        status = CMSIS_STREXW(new_val, (os_uint_t*)obj);
    } while (status != 0);

    return old_val;
}

uint32_t atomic_fetch_xor_32(atomic_uint32_t *obj, uint32_t operand) {
    uint32_t old_val;
    uint32_t new_val;
    uint32_t status;

    do {
        old_val = CMSIS_LDREXW((os_uint_t*)obj);
        new_val = old_val ^ operand;
        status = CMSIS_STREXW(new_val, (os_uint_t*)obj);
    } while (status != 0);

    return old_val;
}

/* ======================================================================== */
/* 8位 原子操作实现 (基于 32位 LDREX/STREX + 掩码)                           */
/* ======================================================================== */

uint8_t atomic_load_8(const atomic_uint8_t *obj) {
    return *obj;
}

void atomic_store_8(atomic_uint8_t *obj, uint8_t desired) {
    // 需要读取包含该字节的整个32位字，修改后写回
    volatile uint32_t *word_addr = (volatile uint32_t *)((uint32_t)obj & ~0x3);
    uint32_t shift = ((uint32_t)obj & 0x3) * 8;
    uint32_t mask = 0xFF << shift;

    uint32_t old_word;
    uint32_t new_word;
    uint32_t status;

    do {
        old_word = CMSIS_LDREXW((os_uint_t*)word_addr);
        new_word = (old_word & ~mask) | (((uint32_t)desired << shift) & mask);
        status = CMSIS_STREXW(new_word, (os_uint_t*)word_addr);
    } while (status != 0);
}

uint8_t atomic_fetch_add_8(atomic_uint8_t *obj, uint8_t operand) {
    volatile uint32_t *word_addr = (volatile uint32_t *)((uint32_t)obj & ~0x3);
    uint32_t shift = ((uint32_t)obj & 0x3) * 8;
    uint32_t mask = 0xFF << shift;

    uint32_t old_word;
    uint32_t new_word;
    uint32_t status;
    uint8_t old_val;

    do {
        old_word = CMSIS_LDREXW((os_uint_t*)word_addr);
        old_val = (old_word >> shift) & 0xFF;
        uint8_t new_val = old_val + operand;
        new_word = (old_word & ~mask) | (((uint32_t)new_val << shift) & mask);
        status = CMSIS_STREXW(new_word, (os_uint_t*)word_addr);
    } while (status != 0);

    return old_val;
}

bool atomic_compare_exchange_strong_8(atomic_uint8_t *obj, uint8_t *expected, uint8_t desired) {
    volatile uint32_t *word_addr = (volatile uint32_t *)((uint32_t)obj & ~0x3);
    uint32_t shift = ((uint32_t)obj & 0x3) * 8;
    uint32_t mask = 0xFF << shift;

    uint32_t old_word;
    uint32_t new_word;
    uint32_t status;
    uint8_t current_val;

    do {
        old_word = CMSIS_LDREXW((os_uint_t*)word_addr);
        current_val = (old_word >> shift) & 0xFF;

        if (current_val != *expected) {
            CMSIS_CLREX();
            *expected = current_val;
            return false;
        }

        new_word = (old_word & ~mask) | (((uint32_t)desired << shift) & mask);
        status = CMSIS_STREXW(new_word, (os_uint_t*)word_addr);
    } while (status != 0);

    return true;
}

/* ======================================================================== */
/* 位带操作实现 (绝对原子，无需 LDREX/STREX 循环)                            */
/* ======================================================================== */

void atomic_bit_set(volatile void *addr, uint32_t bit) {
    // 向位带别名地址写入 1
    *BITBAND_PTR(addr, bit) = 1;
}

void atomic_bit_clear(volatile void *addr, uint32_t bit) {
    // 向位带别名地址写入 0
    *BITBAND_PTR(addr, bit) = 0;
}

uint32_t atomic_bit_read(volatile void *addr, uint32_t bit) {
    // 从位带别名地址读取，结果为 0 或 1
    return *BITBAND_PTR(addr, bit);
}

void atomic_bit_toggle(volatile void *addr, uint32_t bit) {
    // 位带不支持直接 toggle，需要读-改-写，但因为是单比特映射，可以这样实现：
    // 读取当前位，然后写入相反值。由于读写的是不同的别名地址（如果是不同位）或同一地址，
    // 对于同一位的 toggle，最安全的方式还是 LDREX/STREX 或者关中断。
    // 但如果只是简单的 Set/Clear，位带是原子的。Toggle 通常不推荐在高位并发下使用位带，除非保证单线程访问该位。
    // 这里提供一个基于位带的简单 Toggle（注意：这在多核或极高并发中断下可能不安全，但在 M3 单核中断模型中，
    // 如果中断优先级管理得当，通常是安全的，或者更推荐使用 BSRR 寄存器进行 GPIO Toggle）

    // 更推荐的 GPIO Toggle 是使用 BSRR:
    // 如果 addr 是 GPIO ODR，建议直接使用 BSRR 寄存器，而不是位带 Toggle。
    // 此处仅为演示位带读写能力，实际 Toggle 建议：
    uint32_t current = *BITBAND_PTR(addr, bit);
    *BITBAND_PTR(addr, bit) = !current;
}