325 lines
8.6 KiB
C
325 lines
8.6 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 (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("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;
|
|
}
|