R2_UP/User/Algorithm/user_cmsis_os2.c

132 lines
2.3 KiB
C
Raw Normal View History

2025-03-12 10:46:02 +08:00
/*
cmsis_os2
*/
#include "user_cmsis_os2.h"
#if (__ARM_ARCH_7A__ == 1U)
/* CPSR mode bitmasks */
#define CPSR_MODE_USER 0x10U
#define CPSR_MODE_SYSTEM 0x1FU
#define IS_IRQ_MODE() ((__get_mode() != CPSR_MODE_USER) && (__get_mode() != CPSR_MODE_SYSTEM))
#else
#define IS_IRQ_MODE() (__get_IPSR() != 0U)
#endif
#define IS_IRQ() IS_IRQ_MODE()
osStatus_t os_user_QueuePeek(osMessageQueueId_t mq_id, void *msg_ptr, uint8_t *msg_prio, uint32_t timeout) {
QueueHandle_t hQueue = (QueueHandle_t)mq_id;
osStatus_t stat;
BaseType_t yield;
(void)msg_prio; /* Message priority is ignored */
stat = osOK;
if (IS_IRQ()) {
if ((hQueue == NULL) || (msg_ptr == NULL) || (timeout != 0U)) {
stat = osErrorParameter;
}
else {
yield = pdFALSE;
if (xQueuePeekFromISR (hQueue, msg_ptr) != pdPASS) {
stat = osErrorResource;
} else {
portYIELD_FROM_ISR (yield);
}
}
}
else {
if ((hQueue == NULL) || (msg_ptr == NULL)) {
stat = osErrorParameter;
}
else {
if (xQueuePeek (hQueue, msg_ptr, (TickType_t)timeout) != pdPASS) {
if (timeout != 0U) {
stat = osErrorTimeout;
} else {
stat = osErrorResource;
}
}
}
}
return (stat);
}
osStatus_t os_user_QueueWrite (osMessageQueueId_t mq_id, const void *msg_ptr, uint8_t msg_prio, uint32_t timeout) {
QueueHandle_t hQueue = (QueueHandle_t)mq_id;
osStatus_t stat;
BaseType_t yield;
(void)msg_prio; /* Message priority is ignored */
stat = osOK;
if (IS_IRQ()) {
if ((hQueue == NULL) || (msg_ptr == NULL) || (timeout != 0U)) {
stat = osErrorParameter;
}
else {
yield = pdFALSE;
if (xQueueOverwriteFromISR (hQueue, msg_ptr ,NULL) != pdTRUE) {
stat = osErrorResource;
} else {
portYIELD_FROM_ISR (yield);
}
}
}
else {
if ((hQueue == NULL) || (msg_ptr == NULL)) {
stat = osErrorParameter;
}
else {
if (xQueueOverwrite (hQueue, msg_ptr) != pdPASS) {
if (timeout != 0U) {
stat = osErrorTimeout;
} else {
stat = osErrorResource;
}
}
}
}
return (stat);
}