RTX5: updated Memory Pool implementation
diff --git a/ARM.CMSIS.pdsc b/ARM.CMSIS.pdsc
index 7554eae..6491c41 100644
--- a/ARM.CMSIS.pdsc
+++ b/ARM.CMSIS.pdsc
@@ -2677,7 +2677,7 @@
</attributes>
</example>
- <example name="CMSIS-RTOS2 RTX5 Migration" doc="Abstract.txt" folder="CMSIS/RTOS2/RTX/Examples/Migration">
+ <example name="CMSIS-RTOS2 RTX5 Migration" doc="Abstract.txt" folder="CMSIS/RTOS2/RTX/Examples/Migration">
<description>CMSIS-RTOS2 mixed API v1 and v2</description>
<board name="uVision Simulator" vendor="Keil"/>
<project>
@@ -2691,8 +2691,7 @@
</attributes>
</example>
-
- <example name="Trustzone-M Example No-RTOS" doc="Abstract.txt" folder="CMSIS/RTOS2/RTX/Examples/TrustZoneV8M/NoRTOS">
+ <example name="Trustzone-M Example No-RTOS" doc="Abstract.txt" folder="CMSIS/RTOS2/RTX/Examples/TrustZoneV8M/NoRTOS">
<description>V8M Trustzone Example Bare-Metal</description>
<board name="uVision Simulator" vendor="Keil"/>
<project>
@@ -2706,7 +2705,7 @@
</attributes>
</example>
- <example name="Trustzone-M Example CMSIS-RTOS2" doc="Abstract.txt" folder="CMSIS/RTOS2/RTX/Examples/TrustZoneV8M/RTOS">
+ <example name="Trustzone-M Example CMSIS-RTOS2" doc="Abstract.txt" folder="CMSIS/RTOS2/RTX/Examples/TrustZoneV8M/RTOS">
<description>V8M Trustzone Example Using CMSIS-RTOS2</description>
<board name="uVision Simulator" vendor="Keil"/>
<project>
diff --git a/CMSIS/RTOS2/RTX/Source/core_cm.h b/CMSIS/RTOS2/RTX/Source/core_cm.h
index d79b24e..76a612b 100644
--- a/CMSIS/RTOS2/RTX/Source/core_cm.h
+++ b/CMSIS/RTOS2/RTX/Source/core_cm.h
@@ -565,6 +565,39 @@
return ret;
}
+/// Exclusive Access Operation: Increment (32-bit) if Less Than
+/// \param[in] mem Memory address
+/// \param[in] max Maximum value
+/// \return Previous value
+__STATIC_INLINE uint32_t os_exc_inc32_lt (uint32_t *mem, uint32_t max) {
+ register uint32_t val, res;
+ register uint32_t ret;
+
+ __ASM volatile (
+ ".syntax unified\n\t"
+ "1:\n\t"
+ "ldrex %[ret],[%[mem]]\n\t"
+ "cmp %[max],%[ret]\n\t"
+ "bhi 2f\n\t"
+ "clrex\n\t"
+ "b 3f\n\t"
+ "2:\n\t"
+ "adds %[val],%[ret],#1\n\t"
+ "strex %[res],%[val],[%[mem]]\n\t"
+ "cbz %[res],3f\n\t"
+ "b 1b\n\t"
+ "3:"
+ : [ret] "=&l" (ret),
+ [val] "=&l" (val),
+ [res] "=&l" (res)
+ : [mem] "l" (mem),
+ [max] "l" (max)
+ : "cc", "memory"
+ );
+
+ return ret;
+}
+
/// Exclusive Access Operation: Increment (16-bit) if Less Than
/// \param[in] mem Memory address
/// \param[in] max Maximum value
@@ -690,6 +723,65 @@
return ret;
}
+/// Exclusive Access Operation: Link Get
+/// \param[in] root Root address
+/// \return Link
+__STATIC_INLINE void *os_exc_link_get (void **root) {
+ register uint32_t val, res;
+ register void *ret;
+
+ __ASM volatile (
+ ".syntax unified\n\t"
+ "1:\n\t"
+ "ldrex %[ret],[%[root]]\n\t"
+ "cbnz %[ret],2f\n\t"
+ "clrex\n\t"
+ "b 3f\n\t"
+ "2:\n\t"
+ "ldr %[val],[%[ret]]\n\t"
+ "strex %[res],%[val],[%[root]]\n\t"
+ "cbz %[res],3f\n\t"
+ "b 1b\n\t"
+ "3:"
+ : [ret] "=&l" (ret),
+ [val] "=&l" (val),
+ [res] "=&l" (res)
+ : [root] "l" (root)
+ : "cc", "memory"
+ );
+
+ return ret;
+}
+
+/// Exclusive Access Operation: Link Put
+/// \param[in] root Root address
+/// \param[in] lnk Link
+__STATIC_INLINE void os_exc_link_put (void **root, void *link) {
+ register uint32_t val1, val2, res;
+
+ __ASM volatile (
+ ".syntax unified\n\t"
+ "1:\n\t"
+ "ldr %[val1],[%[root]]\n\t"
+ "str %[val1],[%[link]]\n\t"
+ "dmb\n\t"
+ "ldrex %[val1],[%[root]]\n\t"
+ "ldr %[val2],[%[link]]\n\t"
+ "cmp %[val2],%[val2]\n\t"
+ "bne 1b\n\t"
+ "strex %[res],%[link],[%[root]]\n\t"
+ "cbz %[res],2f\n\t"
+ "b 1b\n\t"
+ "2:"
+ : [val1] "=&l" (val1),
+ [val2] "=&l" (val2),
+ [res] "=&l" (res)
+ : [root] "l" (root),
+ [link] "l" (link)
+ : "cc", "memory"
+ );
+}
+
#endif // (__EXCLUSIVE_ACCESS == 1U)
diff --git a/CMSIS/RTOS2/RTX/Source/rtx_mempool.c b/CMSIS/RTOS2/RTX/Source/rtx_mempool.c
index e475e30..d35f743 100644
--- a/CMSIS/RTOS2/RTX/Source/rtx_mempool.c
+++ b/CMSIS/RTOS2/RTX/Source/rtx_mempool.c
@@ -80,46 +80,24 @@
#if (__EXCLUSIVE_ACCESS == 0U)
__disable_irq();
- block = mp_info->block_free;
- if (block != NULL) {
- mp_info->block_free = *((void **)block);
+ if (mp_info->used_blocks < mp_info->max_blocks) {
mp_info->used_blocks++;
+ block = mp_info->block_free;
+ if (block != NULL) {
+ mp_info->block_free = *((void **)block);
+ }
+ } else {
+ block = NULL;
}
if (primask == 0U) {
__enable_irq();
}
#else
- {
- register uint32_t val, res;
-
- __ASM volatile (
- ".syntax unified\n\t"
- "loop1%=:\n\t"
- "ldrex %[block],[%[mp_info],%[_block_free]]\n\t"
- "cbnz %[block],update%=\n\t"
- "clrex\n\t"
- "b exit%=\n\t"
- "update%=:\n\t"
- "ldr %[val],[%[block]]\n\t"
- "strex %[res],%[val],[%[mp_info],%[_block_free]]\n\t"
- "cbz %[res],loop2%=\n\t"
- "b loop1%=\n\t"
- "loop2%=:\n\t"
- "ldrex %[val],[%[mp_info],%[_used_blocks]]\n\t"
- "adds %[val],#1\n\t"
- "strex %[res],%[val],[%[mp_info],%[_used_blocks]]\n\t"
- "cbz %[res],exit%=\n\t"
- "b loop2%=\n\t"
- "exit%=:"
- : [block] "=&l" (block),
- [val] "=&l" (val),
- [res] "=&l" (res)
- : [mp_info] "l" (mp_info),
- [_block_free] "I" (offsetof(os_mp_info_t, block_free)),
- [_used_blocks] "I" (offsetof(os_mp_info_t, used_blocks))
- : "cc", "memory"
- );
+ if (os_exc_inc32_lt(&mp_info->used_blocks, mp_info->max_blocks) < mp_info->max_blocks) {
+ block = os_exc_link_get(&mp_info->block_free);
+ } else {
+ block = NULL;
}
#endif
@@ -134,6 +112,7 @@
#if (__EXCLUSIVE_ACCESS == 0U)
uint32_t primask = __get_PRIMASK();
#endif
+ osStatus_t status;
if (mp_info == NULL) {
return osErrorParameter;
@@ -145,50 +124,28 @@
#if (__EXCLUSIVE_ACCESS == 0U)
__disable_irq();
- *((void **)block) = mp_info->block_free;
- mp_info->block_free = block;
- mp_info->used_blocks--;
+ if (mp_info->used_blocks != 0U) {
+ mp_info->used_blocks--;
+ *((void **)block) = mp_info->block_free;
+ mp_info->block_free = block;
+ status = osOK;
+ } else {
+ status = osErrorResource;
+ }
if (primask == 0U) {
__enable_irq();
}
#else
- {
- register uint32_t val1, val2, res;
-
- __ASM volatile (
- ".syntax unified\n\t"
- "loop1%=:\n\t"
- "ldr %[val1],[%[mp_info],%[_block_free]]\n\t"
- "str %[val1],[%[block]]\n\t"
- "dmb\n\t"
- "ldrex %[val1],[%[mp_info],%[_block_free]]\n\t"
- "ldr %[val2],[%[block]]\n\t"
- "cmp %[val2],%[val1]\n\t"
- "bne loop1%=\n\t"
- "strex %[res],%[block],[%[mp_info],%[_block_free]]\n\t"
- "cbz %[res],loop2%=\n\t"
- "b loop1%=\n\t"
- "loop2%=:\n\t"
- "ldrex %[val1],[%[mp_info],%[_used_blocks]]\n\t"
- "subs %[val1],#1\n\t"
- "strex %[res],%[val1],[%[mp_info],%[_used_blocks]]\n\t"
- "cbz %[res],exit%=\n\t"
- "b loop2%=\n\t"
- "exit%=:"
- : [val1] "=&l" (val1),
- [val2] "=&l" (val2),
- [res] "=&l" (res)
- : [block] "l" (block),
- [mp_info] "l" (mp_info),
- [_block_free] "I" (offsetof(os_mp_info_t, block_free)),
- [_used_blocks] "I" (offsetof(os_mp_info_t, used_blocks))
- : "cc", "memory"
- );
+ if (os_exc_dec32_nz(&mp_info->used_blocks) != 0U) {
+ os_exc_link_put(&mp_info->block_free, block);
+ status = osOK;
+ } else {
+ status = osErrorResource;
}
#endif
- return osOK;
+ return status;
}
/// Memory Pool post ISR processing.