334 lines
9.2 KiB
C
334 lines
9.2 KiB
C
#include "app_config.h"
|
||
#include "interface.h"
|
||
|
||
#include "app_frm_signal.h"
|
||
|
||
typedef struct
|
||
{
|
||
void *signals[MAX_SIGNALS];
|
||
uint32_t head;
|
||
uint32_t tail;
|
||
uint32_t count;
|
||
} SignalQueue;
|
||
|
||
typedef struct
|
||
{
|
||
void *signal_id;
|
||
CallbackFunc callbacks[MAX_CALLBACKS];
|
||
uint32_t callback_count;
|
||
} Subscriber;
|
||
|
||
// 优先级队列
|
||
static SignalQueue priority_queues[PRIORITY_LEVELS];
|
||
|
||
// 订阅者表
|
||
static Subscriber subscriber_table[MAX_SUBSCRIBERS] = {{NULL, {NULL}, 0}};
|
||
|
||
// 初始化队列
|
||
static void initQueue(SignalQueue *q)
|
||
{
|
||
q->head = 0;
|
||
q->tail = 0;
|
||
q->count = 0;
|
||
}
|
||
|
||
// 将信号请求添加到队列中（按优先级）
|
||
static int32_t enqueue(SignalQueue *q, void *signal_id)
|
||
{
|
||
if (q == NULL)
|
||
{
|
||
printf("Error: enqueue received NULL queue pointer\n");
|
||
return -1;
|
||
}
|
||
if (signal_id == NULL)
|
||
{
|
||
printf("Error: Cannot enqueue NULL signal_id\n");
|
||
return -1;
|
||
}
|
||
|
||
irq_state_t saved_state = enter_critical_section();
|
||
|
||
if (q->count >= MAX_SIGNALS)
|
||
{
|
||
// 队列已满，移除最前面的信号
|
||
q->signals[q->head] = NULL;
|
||
q->head = (q->head + 1) % MAX_SIGNALS;
|
||
q->count--;
|
||
printf("Error: Signal queue is full, remove the first signal\n");
|
||
}
|
||
|
||
// 添加新的信号到队列尾部
|
||
q->signals[q->tail] = signal_id;
|
||
q->tail = (q->tail + 1) % MAX_SIGNALS;
|
||
q->count++;
|
||
|
||
exit_critical_section(saved_state);
|
||
return 0;
|
||
}
|
||
|
||
|
||
// 从队列中取出信号请求（按优先级）
|
||
static int32_t dequeue(SignalQueue *q, void **signal_id)
|
||
{
|
||
if (q == NULL)
|
||
{
|
||
printf("Error: dequeue received NULL queue pointer\n");
|
||
return -1;
|
||
}
|
||
if (signal_id == NULL)
|
||
{
|
||
printf("Error: dequeue received NULL signal pointer\n");
|
||
return -1;
|
||
}
|
||
|
||
// 仅在修改共享资源时进入临界区
|
||
if (q->count > 0)
|
||
{
|
||
irq_state_t saved_state = enter_critical_section();
|
||
|
||
*signal_id = q->signals[q->head];
|
||
q->signals[q->head] = NULL; // 清除已取出的信号
|
||
q->head = (q->head + 1) % MAX_SIGNALS;
|
||
q->count--;
|
||
|
||
exit_critical_section(saved_state);
|
||
return 0;
|
||
}
|
||
// printf("Warning: dequeue attempted to remove signal from an empty queue\n");
|
||
return -1;
|
||
}
|
||
|
||
// 哈希函数
|
||
static uint32_t hash(void *ptr)
|
||
{
|
||
uintptr_t value = (uintptr_t)ptr;
|
||
uint32_t hash = 0;
|
||
while (value != 0)
|
||
{
|
||
hash += value & 0xFF;
|
||
hash += (hash << 10);
|
||
hash ^= (hash >> 6);
|
||
value >>= 8;
|
||
}
|
||
hash += (hash << 3);
|
||
hash ^= (hash >> 11);
|
||
hash += (hash << 15);
|
||
return hash;
|
||
}
|
||
|
||
// 订阅信号, 给每个信号指定回调函数
|
||
int32_t subscribe(void *signal_id, CallbackFunc callback)
|
||
{
|
||
if (signal_id == NULL || callback == NULL)
|
||
{
|
||
printf("Error: Invalid signal_id or callback\n");
|
||
return -1;
|
||
}
|
||
|
||
irq_state_t saved_state = enter_critical_section();
|
||
|
||
uint32_t index = hash(signal_id) % MAX_SUBSCRIBERS;
|
||
uint32_t original_index = index;
|
||
|
||
do
|
||
{
|
||
// if(signal_id == &un_remote_control_input)
|
||
// {
|
||
// printf("remote的地址：%p\n", (void*)&un_remote_control_input); // 输出示例：0x7ffd9a3c8b40[5](@ref)
|
||
// printf("signal_id的地址：%p\n", (void*)(subscriber_table[index].signal_id)); // 输出示例：0x7ffd9a3c8b40[5](@ref)
|
||
// }
|
||
//
|
||
if (subscriber_table[index].signal_id == NULL ||
|
||
subscriber_table[index].signal_id == signal_id)
|
||
{
|
||
if (subscriber_table[index].signal_id == NULL)
|
||
{
|
||
subscriber_table[index].signal_id = signal_id;
|
||
subscriber_table[index].callback_count = 0;
|
||
}
|
||
|
||
if (subscriber_table[index].callback_count < MAX_CALLBACKS)
|
||
{
|
||
subscriber_table[index].callbacks[subscriber_table[index].callback_count++] = callback;
|
||
|
||
exit_critical_section(saved_state);
|
||
return 0;
|
||
}
|
||
else
|
||
{
|
||
printf("Error: Maximum callbacks reached for this signal\n");
|
||
exit_critical_section(saved_state);
|
||
return -1;
|
||
}
|
||
}
|
||
|
||
index = (index + 1) % MAX_SUBSCRIBERS;
|
||
} while (index != original_index);
|
||
|
||
// printf("remote addrss：%p\n", (void*)signal_id); // 输出示例：0x7ffd9a3c8b40[5](@ref)
|
||
// printf("signal_id addrss：%p\n", (void*)(subscriber_table[index].signal_id)); // 输出示例：0x7ffd9a3c8b40[5](@ref)
|
||
|
||
printf("Error: Subscriber table is full\n");
|
||
exit_critical_section(saved_state);
|
||
return -1;
|
||
}
|
||
|
||
// 检查信号是否有订阅者
|
||
static unsigned char hasSubscribers(void *signal_id)
|
||
{
|
||
irq_state_t saved_state = enter_critical_section();
|
||
|
||
uint32_t index = hash(signal_id) % MAX_SUBSCRIBERS;
|
||
uint32_t original_index = index;
|
||
|
||
do
|
||
{
|
||
if (subscriber_table[index].signal_id == signal_id)
|
||
{
|
||
unsigned char result = subscriber_table[index].callback_count > 0;
|
||
exit_critical_section(saved_state);
|
||
return result;
|
||
}
|
||
if (subscriber_table[index].signal_id == NULL)
|
||
{
|
||
exit_critical_section(saved_state);
|
||
return 0; // 没有订阅者
|
||
}
|
||
index = (index + 1) % MAX_SUBSCRIBERS;
|
||
} while (index != original_index);
|
||
|
||
exit_critical_section(saved_state);
|
||
return 0; // 没有订阅者
|
||
}
|
||
|
||
// 内部一致性检查
|
||
static void internalConsistencyCheck(void)
|
||
{
|
||
for (uint32_t i = 0; i < MAX_SUBSCRIBERS; i++)
|
||
{
|
||
assert(subscriber_table[i].callback_count <= MAX_CALLBACKS);
|
||
}
|
||
}
|
||
|
||
// 处理队列中的信号, 调用所有匹配的回调函数
|
||
static void processSignals(void)
|
||
{
|
||
void *signal_id;
|
||
|
||
for (uint32_t priority = 0; priority < PRIORITY_LEVELS; priority++)
|
||
{
|
||
SignalQueue *q = &priority_queues[priority];
|
||
|
||
while (dequeue(q, &signal_id) == 0)
|
||
{
|
||
// 进入临界区保护 subscriber_table 的读取
|
||
irq_state_t saved_state = enter_critical_section();
|
||
|
||
uint32_t index = hash(signal_id) % MAX_SUBSCRIBERS;
|
||
uint32_t original_index = index;
|
||
unsigned char found = 0;
|
||
|
||
do
|
||
{
|
||
if (subscriber_table[index].signal_id == signal_id)
|
||
{
|
||
CallbackFunc *callbacks = subscriber_table[index].callbacks;
|
||
uint32_t callback_count = subscriber_table[index].callback_count;
|
||
|
||
// 复制回调函数指针, 避免在临界区外访问共享数据
|
||
CallbackFunc local_callbacks[MAX_CALLBACKS];
|
||
memcpy(local_callbacks, callbacks, sizeof(CallbackFunc) * callback_count);
|
||
|
||
exit_critical_section(saved_state);
|
||
|
||
// 在临界区外调用回调函数
|
||
for (uint32_t i = 0; i < callback_count; i++)
|
||
{
|
||
local_callbacks[i](signal_id);
|
||
}
|
||
found = 1;
|
||
break;
|
||
}
|
||
if (subscriber_table[index].signal_id == NULL)
|
||
{
|
||
exit_critical_section(saved_state);
|
||
break; // 没有订阅者
|
||
}
|
||
index = (index + 1) % MAX_SUBSCRIBERS;
|
||
} while (index != original_index);
|
||
|
||
if (!found)
|
||
{
|
||
printf("Warning: No subscribers found for signal %p\n", signal_id);
|
||
}
|
||
}
|
||
}
|
||
internalConsistencyCheck(); // 处理完所有信号后进行一致性检查
|
||
}
|
||
|
||
// 初始化框架
|
||
void initFramework(void)
|
||
{
|
||
irq_state_t saved_state = enter_critical_section();
|
||
|
||
for (uint32_t i = 0; i < PRIORITY_LEVELS; i++)
|
||
{
|
||
initQueue(&priority_queues[i]);
|
||
}
|
||
memset(subscriber_table, 0, sizeof(subscriber_table));
|
||
|
||
exit_critical_section(saved_state);
|
||
}
|
||
|
||
// 将信号请求添加到指定优先级的队列中
|
||
int32_t publishMessage(void *signal_id, uint8_t priority)
|
||
{
|
||
if ((uint32_t)priority >= PRIORITY_LEVELS)
|
||
{
|
||
printf("Error: Invalid priority\n");
|
||
return -1;
|
||
}
|
||
|
||
if (hasSubscribers(signal_id))
|
||
{
|
||
return enqueue(&priority_queues[priority], signal_id);
|
||
}
|
||
else return 1;
|
||
}
|
||
|
||
// 处理所有优先级的信号
|
||
void processMessages(void)
|
||
{
|
||
processSignals();
|
||
}
|
||
|
||
// 获取当前队列中的信号数量
|
||
int32_t getSignalCount(uint32_t priority)
|
||
{
|
||
if (priority >= PRIORITY_LEVELS)
|
||
{
|
||
printf("Error: Invalid priority\n");
|
||
return -1;
|
||
}
|
||
return priority_queues[priority].count;
|
||
}
|
||
|
||
// 获取订阅者数量
|
||
uint32_t getSubscriberCount(void)
|
||
{
|
||
uint32_t count = 0;
|
||
|
||
irq_state_t saved_state = enter_critical_section();
|
||
|
||
for (uint32_t i = 0; i < MAX_SUBSCRIBERS; i++)
|
||
{
|
||
if (subscriber_table[i].signal_id != NULL)
|
||
{
|
||
count++;
|
||
}
|
||
}
|
||
|
||
exit_critical_section(saved_state);
|
||
return count;
|
||
}
|