Commit 277961fa authored by Oliver Horst's avatar Oliver Horst
Browse files

[add] Added a core synchronization barrier (vHalCpuBarrier)

parent 96ce50f3
......@@ -3,6 +3,8 @@
#include "xil/xpseudo_asm.h"
#include <stdatomic.h>
/* Multiprocessor Affinity Register - used to retrieve the CPU ID */
#define MPIDR_EL1 "MPIDR_EL1"
......@@ -11,6 +13,20 @@
#define ARM_CA53_CPU_ID_MASK ( (UBaseType_t)( 0xFFu ) )
static volatile uint64_t __attribute__((section (".ocm_ram"))) ulCpuBarrierMask1 = ( (uint64_t)__AMP_CORE_MASK );
static volatile uint64_t __attribute__((section (".ocm_ram"))) ulCpuBarrierMask2 = 0;
inline void vHalCpuWaitForEvent()
{
__asm__("wfe");
}
inline void vHalCpuSignalEvent()
{
__asm__("sev");
}
UBaseType_t uxHalGetCpuId()
{
UBaseType_t uxCpuId;
......@@ -20,3 +36,73 @@ UBaseType_t uxCpuId;
return uxCpuId;
}
void vHalCpuBarrier()
{
uint64_t mask1;
uint64_t mask2;
UBaseType_t uxCpuId = uxHalGetCpuId();
/* We disable interrupts here to ensure that nothing distracts the
* synchronization of the cores. */
portDISABLE_INTERRUPTS()
/* The value of the second barrier mask will only be changed in the 2nd
* loop, thus we load it once here to speed up the following comparisons. */
mask2 = atomic_load( &ulCpuBarrierMask2 );
/* We mark our core as ready to sync by flipping the corresponding bit in
* the barrier 1 mask. */
atomic_fetch_xor( &ulCpuBarrierMask1, CORE_MASK( uxCpuId ) );
/* Test whether all cores are ready to sync. If not we are entering the low
* power state and stall the execution until the last core wakes all other
* cores again. */
while ( 1 )
{
if ( atomic_load( &ulCpuBarrierMask1 ) ^ mask2 != ( (uint64_t)__AMP_CORE_MASK ) )
{
vHalCpuWaitForEvent();
}
else
{
vHalCpuSignalEvent();
break;
}
}
/* All cores are synced right now, however, to be able to call
* vHalCpuBarrier again later on, we have to ensure that one of the barrier
* masks is set to 0 and the other to __AMP_CORE_MASK.
*
* At this point in time, both masks will have the same value, which can be
* either 0 or __AMP_CORE_MASK.
*
* Hence, we execute a second synchronization round. */
/* As the value of barrier mask 1 is fixed now, we load it once in advance. */
mask1 = atomic_load( &ulCpuBarrierMask1 );
/* We mark our core as synced by flipping the corresponding bit in
* the barrier 1 mask. */
atomic_fetch_xor( &ulCpuBarrierMask2, CORE_MASK( uxCpuId ) );
/* Test whether all cores are synced. If not we are entering the low power
* state and stall the execution until the last core wakes all other cores
* again. */
while ( 1 )
{
if ( mask1 ^ atomic_load( &ulCpuBarrierMask2 ) != ( (uint64_t)__AMP_CORE_MASK ) )
{
vHalCpuWaitForEvent();
}
else
{
vHalCpuSignalEvent();
break;
}
}
portENABLE_INTERRUPTS()
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment