robot_h743/robot/library/common/rd_mempool.c

/******************************************************************************

                  版权所有 (C), 2018-2099, Radkil

 ******************************************************************************
  文 件 名   : rd_mempool.c
  版 本 号   : 初稿
  作    者   : Radkil
  生成日期   : 2026年3月10日星期二
  最近修改   :
  功能描述   : 内存池模块
  
  修改历史   :
  1.日    期   : 2026年3月10日星期二
    作    者   : Radkil
    修改内容   : 创建文件

******************************************************************************/
#include "rd_mempool.h"
#include "rd_thread.h"
/*----------------------------------------------*
 * 外部变量说明                                 *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 外部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 内部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 全局变量                                     *
 *----------------------------------------------*/
// 内存块头部结构 (隐藏在用户指针之前)
// 这个结构体的大小必须是 ALIGN_SIZE 的倍数
typedef struct MemBlockHeader {
    size_t size;          // 当前块的总大小 (包含 Header 本身)
    int is_free;          // 1 = 空闲, 0 = 已占用
    struct MemBlockHeader* next_free; // 指向下一个空闲块 (仅在 is_free=1 时有效)
    struct MemBlockHeader* prev_free; // 指向上一个空闲块 (双向链表便于删除节点)
} MemBlockHeader_t;
    
#define HEADER_SIZE RD_ALIGN(sizeof(MemBlockHeader_t), RD_ALIGN_SIZE)
#define MIN_BLOCK_SIZE (HEADER_SIZE + MIN_ALLOC_UNIT)
    
static uint8_t* g_pool_start = NULL;
static uint8_t* g_pool_end = NULL;
static MemBlockHeader_t* g_free_list_head = NULL;
static int g_initialized = 0;
    
#ifdef USE_HEAP_FOR_MEMPOOL
static uint8_t* g_heap_buf = NULL;
#endif

/*----------------------------------------------*
 * 模块级变量                                   *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 常量定义                                     *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 宏定义                                       *
 *----------------------------------------------*/

// 获取用户数据指针
rd_inline void* get_user_ptr(MemBlockHeader_t* block) 
{
    return (void*)((uint8_t*)block + HEADER_SIZE);
}

// 获取头部指针
rd_inline MemBlockHeader_t* get_header_ptr(void* ptr) 
{
    return (MemBlockHeader_t*)((uint8_t*)ptr - HEADER_SIZE);
}

// 检查指针是否在池内
rd_inline int is_valid_ptr(void* ptr) 
{
    if (!g_initialized || !ptr) return 0;
    return ((uint8_t*)ptr >= (g_pool_start + HEADER_SIZE) && 
            (uint8_t*)ptr < g_pool_end);
}

// 从空闲链表中移除节点
static void remove_from_free_list(MemBlockHeader_t* block) 
{
    if (!block->is_free) return;
    
    if (block->prev_free) 
    {
        block->prev_free->next_free = block->next_free;
    } 
    else 
    {
        g_free_list_head = block->next_free;
    }
    
    if (block->next_free) 
    {
        block->next_free->prev_free = block->prev_free;
    }
    
    block->next_free = NULL;
    block->prev_free = NULL;
}

// 添加节点到空闲链表头部 (简单高效)
static void add_to_free_list(MemBlockHeader_t* block) 
{
    block->is_free = 1;
    block->next_free = g_free_list_head;
    block->prev_free = NULL;
    
    if (g_free_list_head) 
    {
        g_free_list_head->prev_free = block;
    }
    g_free_list_head = block;
}

// 物理相邻的块合并 (关键步骤：消除碎片)
static void merge_free_blocks(MemBlockHeader_t* block) 
{
    if (!block || !block->is_free) return;

    // 1. 尝试与【后一个】块合并
    // 计算下一个块的地址
    uint8_t* next_block_addr = (uint8_t*)block + block->size;
    
    // 检查是否越界
    if (next_block_addr < g_pool_end) 
    {
        MemBlockHeader_t* next_block = (MemBlockHeader_t*)next_block_addr;
        
        // 如果下一个块也是空闲的，合并！
        if (next_block->is_free) 
        {
            // 从链表中移除下一个块 (因为要合并进当前块)
            remove_from_free_list(next_block);
            
            // 扩大当前块尺寸
            block->size += next_block->size;
            
            // 注意：不需要重新设置 next_block 的 header，因为它已经被吞并了
        }
    }

    // 2. 尝试与【前一个】块合并
    // 由于单向无法直接得知前一个块，我们需要遍历或者维护额外信息。
    // 优化技巧：在 Free 列表中遍历时，我们无法直接知道物理前驱。
    // 简单做法：遍历整个空闲链表，看谁的 (addr + size) == 当前 addr。
    // 为了效率，通常我们在分配时保证顺序，或者接受 O(N) 的前向查找。
    // 这里为了代码简洁和稳健，采用遍历查找前驱 (在碎片不多时很快)。
    
    MemBlockHeader_t* prev_block = g_free_list_head;
    while (prev_block) {
        if (prev_block == block) {
            prev_block = prev_block->next_free;
            continue;
        }
        
        uint8_t* potential_next = (uint8_t*)prev_block + prev_block->size;
        if (potential_next == (uint8_t*)block) 
        {
            // 发现前驱是空闲的，合并：前驱吞并当前
            remove_from_free_list(block); // 先移除当前
            remove_from_free_list(prev_block); // 移除前驱准备重组
            
            prev_block->size += block->size;
            add_to_free_list(prev_block); // 重新加入链表
            return; // 合并完成
        }
        prev_block = prev_block->next_free;
    }
}

WEAK void Rd_MemDoInit(void) 
{
    if (g_initialized) return;

#ifdef USE_HEAP_FOR_MEMPOOL
    g_heap_buf = (uint8_t*)MP_MALLOC(MEM_POOL_TOTAL_SIZE);
    if (!g_heap_buf) 
    {
        log_e("MemPool Init Failed: Malloc error");
        return;
    }
    g_pool_start = g_heap_buf;
#else
    // 静态数组，确保对齐
    static uint8_t static_pool[MEM_POOL_TOTAL_SIZE] __attribute__((aligned(RD_ALIGN_SIZE)));
    g_pool_start = static_pool;
#endif

    g_pool_end = g_pool_start + MEM_POOL_TOTAL_SIZE;
    
    // 初始化整个池为一个巨大的空闲块
    MemBlockHeader_t* initial_block = (MemBlockHeader_t*)g_pool_start;
    initial_block->size = MEM_POOL_TOTAL_SIZE;
    initial_block->is_free = 1;
    initial_block->next_free = NULL;
    initial_block->prev_free = NULL;
    
    g_free_list_head = initial_block;
    g_initialized = 1;
    
    log_d("MemPool Init Success: Total %d Bytes", MEM_POOL_TOTAL_SIZE);
}

WEAK void Rd_MemDoDeinit(void) 
{
#ifdef USE_HEAP_FOR_MEMPOOL
    if (g_heap_buf) 
    {
        MP_FREE(g_heap_buf);
        g_heap_buf = NULL;
    }
#endif
    g_initialized = 0;
    g_free_list_head = NULL;
}

WEAK void* Rd_MemPoolMalloc(size_t size) 
{
    if (!g_initialized || size == 0) return NULL;

    void *pvReturn = NULL;
    Rd_MutexLock(NULL, __func__, NULL);
    {

        // 对齐大小计算：用户数据 + 头部
        size_t aligned_size = RD_ALIGN(size, RD_ALIGN_SIZE);
        size_t total_needed = HEADER_SIZE + aligned_size;
        
        // 确保不小于最小块大小
        if (total_needed < MIN_BLOCK_SIZE) 
        {
            total_needed = MIN_BLOCK_SIZE;
        }

        // 首次适应算法 (First Fit)
        MemBlockHeader_t* current = g_free_list_head;
        while (current) 
        {
            if (current->size >= total_needed) 
            {
                // 找到合适的块
                
                // 检查是否可以切分 (剩下的部分是否足够形成一个新块)
                size_t remaining = current->size - total_needed;
                
                if (remaining >= MIN_BLOCK_SIZE) 
                {
                    // 【切分操作】
                    // 1. 创建新块 (在剩余空间)
                    MemBlockHeader_t* new_block = (MemBlockHeader_t*)((uint8_t*)current + total_needed);
                    new_block->size = remaining;
                    new_block->is_free = 0; // 暂时设为占用，稍后加入链表
                    
                    // 2. 缩小当前块
                    current->size = total_needed;
                    
                    // 3. 将新块加入空闲链表
                    add_to_free_list(new_block);
                } 
                else 
                {
                    // 不切分，整个块都给用户 (避免产生极小碎片)
                    // 剩余空间被浪费在当前块内，直到下次 Free
                }
                
                // 从空闲链表移除当前块，标记为占用
                remove_from_free_list(current);
                current->is_free = 0;
                current->next_free = NULL;
                current->prev_free = NULL;
                
                pvReturn = get_user_ptr(current);
            }
            current = current->next_free;
        }
        if (pvReturn == NULL) 
        {
            log_d("MemPool Malloc Failed: Request %zu, No Block Found", size);
        }
    }
    Rd_MutexUnlock(NULL, __func__, NULL);
    return pvReturn;
}

WEAK void Rd_MemPoolFree(void* ptr) 
{
    if (!is_valid_ptr(ptr)) 
    {
        log_d("MemPool Free Error: Invalid Pointer %p", ptr);
        return;
    }

    Rd_MutexLock(NULL, __func__, NULL);
    {
        MemBlockHeader_t* block = get_header_ptr(ptr);
        
        // 双重释放保护
        if (block->is_free) 
        {
            log_d("MemPool Free Error: Double Free detected at %p", ptr);
            return;
        }

        // 标记为空闲并加入链表
        add_to_free_list(block);
        
        // 【关键】尝试与相邻空闲块合并，减少碎片
        merge_free_blocks(block);
    }
    Rd_MutexUnlock(NULL, __func__, NULL);
}

WEAK void* Rd_MemPoolCalloc(size_t n, size_t size)
{
    size_t total = n * size;
    void* ptr = Rd_MemPoolMalloc(total);
    if (ptr) 
    {
        MP_MEMSET(ptr, 0, total);
    }
    return ptr;
}

WEAK void* Rd_MemPoolRealloc(void* ptr, size_t new_size) 
{
    if (!ptr) return Rd_MemPoolMalloc(new_size);
    if (new_size == 0) 
    {
        Rd_MemPoolFree(ptr);
        return NULL;
    }

    MemBlockHeader_t* block = get_header_ptr(ptr);
    size_t old_user_size = block->size - HEADER_SIZE;
    
    if (old_user_size >= new_size) 
    {
        // 现有块足够大，不需要移动 (可选：这里可以不做任何事，或者返回原指针)
        // 注意：标准 realloc 在缩小时行为未定义，通常不缩小块，除非为了合并
        return ptr;
    }

    // 需要扩大：分配新的，拷贝，释放旧的
    void* new_ptr = Rd_MemPoolMalloc(new_size);
    if (!new_ptr) return NULL;

    MP_MEMCPY(new_ptr, ptr, old_user_size);
    Rd_MemPoolFree(ptr);
    return new_ptr;
}

WEAK size_t Rd_MemPoolGetFreeCount(void)
{
    if (!g_initialized) return 0;
    size_t total_free = 0;
    MemBlockHeader_t* curr = g_free_list_head;
    while (curr) 
    {
        total_free += (curr->size - HEADER_SIZE);
        curr = curr->next_free;
    }
    return total_free;
}

WEAK size_t Rd_MemPoolGetTotalCount(void)
{
    return (size_t)MEM_POOL_TOTAL_SIZE;
}

void Rd_MemPoolInit_Auto(void) 
{ 
    Rd_MemDoInit(); 
}

__attribute__((destructor(102))) 
void Rd_MemPoolDeinit_Auto(void) 
{ 
    Rd_MemDoDeinit(); 
}