[merge] Integrated updates from 'next' branch

- Added ARM CoreSight STM and timestamping driver
- Added Xil_CpuMutexWait and Xil_CpuMutexPost methods
- Fixed GIC driver behavior when multiple cores want to claim an interrupt

CMakeLists.txt

@@ -17,6 +17,11 @@ find_package(
     REQUIRED
 )
 
+find_package(
+    toki-libc-repl 0.1
+    REQUIRED
+)
+
 
 get_target_property(
     XIL_CONFIG_DEFINES
@@ -43,6 +48,7 @@ target_link_libraries(
     xil
         #
         PUBLIC      xil-config
+        PRIVATE     toki-libc-repl
 )
 
 

include/arm_ca53/xil/xil_cpu.h

+#ifndef XIL_CPU_H
+#define XIL_CPU_H
+
+#include "xil/xil_types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define XCORE_MASK(x) ((uintptr_t)(1u << x))
+
+extern void __AMP_NUM_CORES;
+extern void __AMP_CORE_MASK;
+extern void __AMP_SLAVE_MASK;
+
+#define XAMP_NUM_CORES_U64 ((uintptr_t)(&__AMP_NUM_CORES))
+#define XAMP_SLAVE_MASK_U64 ((uintptr_t)(&__AMP_SLAVE_MASK))
+#define XAMP_CORE_MASK_U64 ((uintptr_t)(&__AMP_CORE_MASK))
+
+#define Xil_CpuDisableInterrupts()									\
+	__asm volatile ( "MSR DAIFSET, #2" ::: "memory" );				\
+	__asm volatile ( "DSB SY" );									\
+	__asm volatile ( "ISB SY" );
+
+#define Xil_CpuEnableInterrupts()									\
+	__asm volatile ( "MSR DAIFCLR, #2" ::: "memory" );				\
+	__asm volatile ( "DSB SY" );									\
+	__asm volatile ( "ISB SY" );
+
+#define Xil_CpuWaitForEvent()                                       \
+    __asm__("wfe");
+
+#define Xil_CpuSignalEvent()                                        \
+    __asm__("sev");
+
+
+u32 Xil_CpuId();
+void Xil_CpuBarrier();
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+#endif /* XIL_CPU_H */

include/common/xil/drivers/xaxidma_bdring.h

@@ -81,7 +81,7 @@ extern "C" {
 
 #include "xil/xstatus.h"
 #include "xil/drivers/xaxidma_bd.h"
-#include <stdlib.h>
+#include <string.h>
 
 /************************** Constant Definitions *****************************/
 /* State of a DMA channel

include/common/xil/drivers/xcoresight_stm_trace.h

+#ifndef XCORESIGHT_STM_TRACE_H
+#define XCORESIGHT_STM_TRACE_H
+
+#include "xil/xil_io.h"
+#include "xil/csparameters.h"
+
+#if !defined (__aarch64__)
+#error "Unsupported Processor Type"
+#endif
+
+/*******************************************************************************
+ * DEFINES
+ *******************************************************************************/
+
+/* \brief Macros to create trace records
+ *
+ * \param port A natural number indicating the number of the stimulus port (channel).
+ * \param type The trace record type. One of the fields in stim_port_t.
+ * \param data The data to write to the stimulus port.
+ *
+ * A possible use of these macros could look like this: STM_TRACE_8(0, G_DMTS, 42);
+ */
+#define XilCoresightSTM_Trace8(port, type, data) \
+    (Xil_Out8( &((XCoresightSTM_Axi *)( CS_STM_AXI_BASE ))->ports[port].type, data ))
+
+#define XilCoresightSTM_Trace16(port, type, data) \
+    (Xil_Out16( &((XCoresightSTM_Axi *)( CS_STM_AXI_BASE ))->ports[port].type, data ))
+
+#define XilCoresightSTM_Trace32(port, type, data) \
+    (Xil_Out32( &((XCoresightSTM_Axi *)( CS_STM_AXI_BASE ))->ports[port].type, data ))
+
+#define XilCoresightSTM_Trace64(port, type, data) \
+    (Xil_Out64( &((XCoresightSTM_Axi *)( CS_STM_AXI_BASE ))->ports[port].type, data ))
+
+
+/*******************************************************************************
+ * TYPEDEFS
+ *******************************************************************************/
+
+typedef volatile u64 XCoresightSTM_Stim;
+
+/**
+ * \typedef stim_port_t
+ * \brief A struct representing one stimulus port. The accessed field determines
+ * 	  the trace record type. The records types are explained in the ARM
+ * 	  System Trace Macrocell Programmer's Model Architecture Specification
+ * 	  Version 1.1, Chapter 3.1.
+ */
+typedef struct {
+    XCoresightSTM_Stim  G_DMTS;
+    XCoresightSTM_Stim  G_DM;
+    XCoresightSTM_Stim  G_DTS;
+    XCoresightSTM_Stim  G_D;
+    u8                  G_reserved[64];
+
+    XCoresightSTM_Stim  G_FLAGTS;
+    XCoresightSTM_Stim  G_FLAG;
+    XCoresightSTM_Stim  G_TRIGTS;
+    XCoresightSTM_Stim  G_TRIG;
+
+    XCoresightSTM_Stim  I_DMTS;
+    XCoresightSTM_Stim  I_DM;
+    XCoresightSTM_Stim  I_DTS;
+    XCoresightSTM_Stim  I_D;
+    u8                  I_reserved[64];
+
+    XCoresightSTM_Stim  I_FLAGTS;
+    XCoresightSTM_Stim  I_FLAG;
+    XCoresightSTM_Stim  I_TRIGTS;
+    XCoresightSTM_Stim  I_TRIG;
+} XCoresightSTM_StimPort;
+
+/**
+ * \brief Á struct that helps to access the STM stimulus ports.
+ */
+typedef struct {
+    XCoresightSTM_StimPort  ports[0];
+} XCoresightSTM_Axi;
+
+/**
+ * \brief A struct containing all parameters necessary to configure STM.
+ * 	  This includes all register values and the addresses of the interfaces.
+ */
+typedef struct {
+    void                *base;
+    XCoresightSTM_Axi   *axi;
+    u8                  traceid;
+    u32                 write_bytes;
+    u32                 stmsper;
+    u32                 stmspter;
+    u32                 stmspscr;
+    u32                 numsp;
+    u32                 stmheer;
+    u32                 stmheter;
+    u32                 stmhebsr;
+} XCoresightSTM_DrvData;
+
+
+/*******************************************************************************
+ * FUNCTIONS
+ *******************************************************************************/
+
+void XCoresight_STM_Initialize();
+
+#endif /* XCORESIGHT_STM_TRACE_H */

include/common/xil/drivers/xcoresight_timestamping.h

+#ifndef XCORESIGHT_TIMESTAMPING_H
+#define XCORESIGHT_TIMESTAMPING_H
+
+void XCoresight_Timestamping_Initialize();
+
+#endif /* XCORESIGHT_TIMESTAMPING_H */

include/common/xil/drivers/xemacps.h

@@ -433,6 +433,10 @@ extern "C" {
 #define XEMACPS_JUMBO_ENABLE_OPTION	0x00004000U
 #define XEMACPS_SGMII_ENABLE_OPTION	0x00008000U
 
+#define XEMACPS_EXTENDED_DESC_ENABLE_OPTION	 0x00010000U
+/**< Enable the extended buffer descriptors
+ *   This option defaults to disabled (clear) */
+
 #define XEMACPS_DEFAULT_OPTIONS                     \
     ((u32)XEMACPS_FLOW_CONTROL_OPTION |                  \
      (u32)XEMACPS_FCS_INSERT_OPTION |                    \
@@ -442,7 +446,8 @@ extern "C" {
      (u32)XEMACPS_TRANSMITTER_ENABLE_OPTION |            \
      (u32)XEMACPS_RECEIVER_ENABLE_OPTION |               \
      (u32)XEMACPS_RX_CHKSUM_ENABLE_OPTION |              \
-     (u32)XEMACPS_TX_CHKSUM_ENABLE_OPTION)
+     (u32)XEMACPS_TX_CHKSUM_ENABLE_OPTION |              \
+     (u32)XEMACPS_EXTENDED_DESC_ENABLE_OPTION)
 
 /**< Default options set when device is initialized or reset */
 /*@}*/

include/common/xil/drivers/xemacps_bd.h

@@ -82,7 +82,6 @@ extern "C" {
 
 /***************************** Include Files *********************************/
 
-#include <string.h>
 #include "xil/xil_types.h"
 #include "xil/xil_assert.h"
 
@@ -92,11 +91,11 @@ extern "C" {
 #ifdef __aarch64__
 /* Minimum BD alignment */
 #define XEMACPS_DMABD_MINIMUM_ALIGNMENT  64U
-#define XEMACPS_BD_NUM_WORDS 4U
+#define XEMACPS_BD_NUM_WORDS 6U
 #else
 /* Minimum BD alignment */
 #define XEMACPS_DMABD_MINIMUM_ALIGNMENT  4U
-#define XEMACPS_BD_NUM_WORDS 2U
+#define XEMACPS_BD_NUM_WORDS 4U
 #endif
 
 /**
@@ -401,11 +400,11 @@ typedef u32 XEmacPs_Bd[XEMACPS_BD_NUM_WORDS];
  *    void XEmacPs_BdSetRxWrap(XEmacPs_Bd* BdPtr)
  *
  *****************************************************************************/
-/*#define XEmacPs_BdSetRxWrap(BdPtr)                                 \
+#define XEmacPs_BdSetRxWrap(BdPtr)                                \
     (XEmacPs_BdWrite((BdPtr), XEMACPS_BD_ADDR_OFFSET,             \
     XEmacPs_BdRead((BdPtr), XEMACPS_BD_ADDR_OFFSET) |             \
     XEMACPS_RXBUF_WRAP_MASK))
-*/
+
 
 /*****************************************************************************/
 /**
@@ -436,11 +435,11 @@ typedef u32 XEmacPs_Bd[XEMACPS_BD_NUM_WORDS];
  *    void XEmacPs_BdSetTxWrap(XEmacPs_Bd* BdPtr)
  *
  *****************************************************************************/
-/*#define XEmacPs_BdSetTxWrap(BdPtr)                                 \
+#define XEmacPs_BdSetTxWrap(BdPtr)                                \
     (XEmacPs_BdWrite((BdPtr), XEMACPS_BD_STAT_OFFSET,             \
     XEmacPs_BdRead((BdPtr), XEMACPS_BD_STAT_OFFSET) |             \
     XEMACPS_TXBUF_WRAP_MASK))
-*/
+
 
 /*****************************************************************************/
 /**
@@ -773,6 +772,88 @@ typedef u32 XEmacPs_Bd[XEMACPS_BD_NUM_WORDS];
     ((XEmacPs_BdRead((BdPtr), XEMACPS_BD_STAT_OFFSET) &           \
     XEMACPS_RXBUF_SOF_MASK)!=0U ? TRUE : FALSE)
 
+/*****************************************************************************/
+/**
+ * Retrieve the BD's Packet timestamp nanoseconds part.
+ *
+ * @param  BdPtr is the BD pointer to operate on
+ *
+ * @return Nanoseconds of timestamp
+ *
+ * @note
+ * C-style signature:
+ *    u32 XEmacPs_GetRxNanoseconds(XEmacPs_Bd* BdPtr)
+ *
+ *****************************************************************************/
+#define XEmacPs_BdGetRxNanoseconds(BdPtr)                                         \
+    ((XEmacPs_BdRead((BdPtr), XEMACPS_BD_ADDR_OFFSET) &                         \
+        XEMACPS_RXBUF_TS_MASK) != 0U ?                                          \
+    (XEmacPs_BdRead((BdPtr), XEMACPS_BD_TIME_OFFSET) & XEMACPS_RXBUF_NSEC_MASK) \
+    : 0U )
+
+/*****************************************************************************/
+/**
+ * Retrieve the BD's Packet timestamp nanoseconds part.
+ *
+ * @param  BdPtr is the BD pointer to operate on
+ *
+ * @return Nanoseconds of timestamp
+ *
+ * @note
+ * C-style signature:
+ *    u32 XEmacPs_GetRxNanoseconds(XEmacPs_Bd* BdPtr)
+ *
+ *****************************************************************************/
+#define XEmacPs_BdGetTxNanoseconds(BdPtr)                                         \
+    ((XEmacPs_BdRead((BdPtr), XEMACPS_BD_STAT_OFFSET) &                         \
+        XEMACPS_TXBUF_TS_MASK) != 0U ?                                          \
+    (XEmacPs_BdRead((BdPtr), XEMACPS_BD_TIME_OFFSET) & XEMACPS_TXBUF_NSEC_MASK) \
+    : 0U )
+
+/*****************************************************************************/
+/**
+ * Retrieve the BD's Packet timestamp seconds part.
+ *
+ * @param  BdPtr is the BD pointer to operate on
+ *
+ * @return Nanoseconds of timestamp
+ *
+ * @note
+ * C-style signature:
+ *    u8 XEmacPs_GetRxNanoseconds(XEmacPs_Bd* BdPtr)
+ *
+ *****************************************************************************/
+#define XEmacPs_BdGetRxSeconds(BdPtr)                                                     \
+    ((XEmacPs_BdRead((BdPtr), XEMACPS_BD_ADDR_OFFSET) &                                 \
+        XEMACPS_RXBUF_TS_MASK) != 0U ?                                                  \
+    (((XEmacPs_BdRead((BdPtr), XEMACPS_BD_TIME_OFFSET) &                                \
+        XEMACPS_RXBUF_SEC_L_MASK) >> 30U) |                                             \
+        ((XEmacPs_BdRead((BdPtr), XEMACPS_BD_TIME_OFFSET + 1) &                         \
+        XEMACPS_RXBUF_SEC_H_MASK) << 2U))                                               \
+    : 0U )
+
+/*****************************************************************************/
+/**
+ * Retrieve the BD's Packet timestamp seconds part.
+ *
+ * @param  BdPtr is the BD pointer to operate on
+ *
+ * @return Nanoseconds of timestamp
+ *
+ * @note
+ * C-style signature:
+ *    u8 XEmacPs_GetRxNanoseconds(XEmacPs_Bd* BdPtr)
+ *
+ *****************************************************************************/
+#define XEmacPs_BdGetTxSeconds(BdPtr)                                                     \
+    ((XEmacPs_BdRead((BdPtr), XEMACPS_BD_STAT_OFFSET) &                                 \
+        XEMACPS_TXBUF_TS_MASK) != 0U ?                                                  \
+    (((XEmacPs_BdRead((BdPtr), XEMACPS_BD_TIME_OFFSET) &                                \
+        XEMACPS_TXBUF_SEC_L_MASK) >> 30U) |                                             \
+        ((XEmacPs_BdRead((BdPtr), XEMACPS_BD_TIME_OFFSET + 1) &                         \
+        XEMACPS_TXBUF_SEC_H_MASK) << 2U))                                               \
+    : 0U )
+
 
 /************************** Function Prototypes ******************************/
 

include/common/xil/drivers/xemacps_hw.h

@@ -422,6 +422,20 @@ typedef enum { MDC_DIV_8 = 0U, MDC_DIV_16, MDC_DIV_32, MDC_DIV_48,
 #define XEMACPS_DMACR_INCR16_AHB_BURST	0x00000010U /**< 16 bytes AHB bursts */
 /*@}*/
 
+/** @name Buffer descriptor control register offsets
+ * Bits 31:6 & 3:0 read 0 and ignore write
+ * Bits 5:4 determine which timestamps are inserted into the buffer descriptors
+ * - 0x00U : No timestamps
+ * - 0x01U : Only PTP event frames
+ * - 0x10U : Only PTP frames
+ * - 0x11U : All frames
+ * @{
+ */
+#define XEMACPS_BDCR_TX_OFFSET          0x000004CCU /**< TX buffer descriptor control register */
+#define XEMACPS_BDCR_RX_OFFSET          0x000004D0U /**< RX buffer descriptor control register */
+#define XEMACPS_BDCR_TS_MASK            0x00000030U /**< Mask for timestamping bits */
+/*@}*/
+
 /** @name transmit status register bit definitions
  * @{
  */
@@ -540,7 +554,11 @@ typedef enum { MDC_DIV_8 = 0U, MDC_DIV_16, MDC_DIV_32, MDC_DIV_48,
 #define XEMACPS_BD_ADDR_OFFSET  0x00000000U /**< word 0/addr of BDs */
 #define XEMACPS_BD_STAT_OFFSET  0x00000004U /**< word 1/status of BDs */
 #define XEMACPS_BD_ADDR_HI_OFFSET  0x00000008U /**< word 2/addr of BDs */
-
+#if defined(__aarch64__) || defined(__arch64__)
+#define XEMACPS_BD_TIME_OFFSET  0x0000000CU /**< word 3/time of BDs */
+#else
+#define XEMACPS_BD_TIME_OFFSET  0x00000008U /**< word 2/time of BDs */
+#endif
 /*
  * @}
  */
@@ -563,6 +581,13 @@ typedef enum { MDC_DIV_8 = 0U, MDC_DIV_16, MDC_DIV_32, MDC_DIV_48,
 #define XEMACPS_TXBUF_NOCRC_MASK 0x00010000U /**< No CRC */
 #define XEMACPS_TXBUF_LAST_MASK  0x00008000U /**< Last buffer */
 #define XEMACPS_TXBUF_LEN_MASK   0x00003FFFU /**< Mask for length field */
+#define XEMACPS_TXBUF_TS_MASK    0x00800000U /**< Timestamp bit */
+
+#define XEMACPS_TXBUF_NSEC_MASK  0x3FFFFFFFU /**< Mask for nanoseconds */
+#define XEMACPS_TXBUF_SEC_L_MASK 0xC0000000U /**< Mask for lower two bits
+                                                  of seconds */
+#define XEMACPS_TXBUF_SEC_H_MASK 0x00000004U /**< Mask for the higher bits
+                                                  of seconds */
 /*
  * @}
  */
@@ -595,9 +620,16 @@ typedef enum { MDC_DIV_8 = 0U, MDC_DIV_16, MDC_DIV_32, MDC_DIV_48,
 #define XEMACPS_RXBUF_LEN_MASK       0x00001FFFU /**< Mask for length field */
 #define XEMACPS_RXBUF_LEN_JUMBO_MASK 0x00003FFFU /**< Mask for jumbo length */
 
+#define XEMACPS_RXBUF_TS_MASK        0x00000004U /**< Timestamp bit */
 #define XEMACPS_RXBUF_WRAP_MASK      0x00000002U /**< Wrap bit, last BD */
 #define XEMACPS_RXBUF_NEW_MASK       0x00000001U /**< Used bit.. */
-#define XEMACPS_RXBUF_ADD_MASK       0xFFFFFFFCU /**< Mask for address */
+#define XEMACPS_RXBUF_ADD_MASK       0xFFFFFFFBU /**< Mask for address */
+
+#define XEMACPS_RXBUF_NSEC_MASK      0x3FFFFFFFU /**< Mask for nanoseconds */
+#define XEMACPS_RXBUF_SEC_L_MASK     0xC0000000U /**< Mask for lower two bits
+                                                      of seconds */
+#define XEMACPS_RXBUF_SEC_H_MASK     0x00000004U /**< Mask for the higher bits
+                                                      of seconds */
 /*
  * @}
  */

include/common/xil/drivers/xscugic.h

@@ -545,6 +545,9 @@ XSCUGIC_RDIST_OFFSET), (RegOffset)))
 *
 *****************************************************************************/
 #define XScuGic_Get_Rdist_Int_Trigger_Index(IntrId)  (((Int_Id%16) & 0x1f) << 2) +1
+
+/************************** Global Variables *********************************/
+extern XScuGic xInterruptController;
 /************************** Function Prototypes ******************************/
 
 /*
@@ -573,6 +576,7 @@ void XScuGic_UnmapAllInterruptsFromCpu(XScuGic *InstancePtr, u8 Cpu_Id);
 void XScuGic_Stop(XScuGic *InstancePtr);
 void XScuGic_SetCpuID(u32 CpuCoreId);
 u32 XScuGic_GetCpuID(void);
+XScuGic *XScuGic_GetInterruptController(void);
 /*
  * Initialization functions in xscugic_sinit.c
  */

include/common/xil/drivers/xsdps.h

@@ -165,7 +165,6 @@ extern "C" {
 #include "xil/xstatus.h"
 #include "xil/drivers/xsdps_hw.h"
 #include "xil/xplatform_info.h"
-#include <string.h>
 
 /************************** Constant Definitions *****************************/
 

include/common/xil/xil_printf.h

@@ -5,9 +5,8 @@
 extern "C" {
 #endif
 
-#include <ctype.h>
-#include <string.h>
 #include <stdarg.h>
+#include <stdio.h>
 #include "xil/xil_types.h"
 #include "xil/xparameters.h"
 #include "xil/bspconfig.h"
@@ -36,7 +35,7 @@ typedef s32 (*func_ptr)(int c);
 
 /*                                                   */
 
-void xil_printf( const char8 *ctrl1, ...);
+#define xil_printf printf_
 void print( const char8 *ptr);
 extern void outbyte (char8 c);
 extern char8 inbyte(void);

src/CMakeLists.txt

@@ -11,7 +11,6 @@ target_sources(
             "${CMAKE_CURRENT_LIST_DIR}/xil_exception.c"
             "${CMAKE_CURRENT_LIST_DIR}/xil_io.c"
             "${CMAKE_CURRENT_LIST_DIR}/xil_mem.c"
-            "${CMAKE_CURRENT_LIST_DIR}/xil_printf.c"
             "${CMAKE_CURRENT_LIST_DIR}/xil_sleepcommon.c"
             "${CMAKE_CURRENT_LIST_DIR}/xil_sleeptimer.c"
             "${CMAKE_CURRENT_LIST_DIR}/xil_testcache.c"

src/arch/arm_ca53/CMakeLists.txt

@@ -6,6 +6,7 @@ target_sources(
         PRIVATE
             "${CMAKE_CURRENT_LIST_DIR}/sleep.c"
             "${CMAKE_CURRENT_LIST_DIR}/xil_cache.c"
+            "${CMAKE_CURRENT_LIST_DIR}/xil_cpu.c"
             "${CMAKE_CURRENT_LIST_DIR}/xil_mmu.c"
             "${CMAKE_CURRENT_LIST_DIR}/xil_smc.c"
             "${CMAKE_CURRENT_LIST_DIR}/xtime_l.c"

src/arch/arm_ca53/boot/xil-crt0.S

@@ -110,9 +110,6 @@ _startup:
 	b	.Lloop_bss
 
 .Lenclbss:
-	/* run global constructors */
-	bl __libc_init_array
-
 	/* Reset and start Triple Timer Counter */
 	#if defined (SLEEP_TIMER_BASEADDR)
 	bl XTime_StartTTCTimer
@@ -128,12 +125,7 @@ _startup:
 
 	bl	main			/* Jump to main C code */
 
-	/* Cleanup global constructors */
-	bl __libc_fini_array
-
-	bl	exit
-
-.Lexit:	/* should never get here */
+.Lexit:	/* Will get here after toki-init eventually stops */
 	b .Lexit
 
 .Lstart:

src/arch/arm_ca53/xil_cpu.c

+#include "xil/xpseudo_asm.h"
+#include "xil/xil_cpu.h"
+
+#include <stdatomic.h>
+
+#ifdef __ICCARM__
+#define INLINE
+#else
+#define INLINE __inline
+#endif
+
+/* Multiprocessor Affinity Register - used to retrieve the CPU ID */
+#define MPIDR_EL1                   "MPIDR_EL1"
+
+/* Mask used for extracting the CPU_ID from the MPIDR_EL1 register */
+#define ARM_CA53_CPU_ID_MASK        ( (u64)( 0xFFu ) )
+
+extern volatile u64 XCpuBarrierMask1;
+extern volatile u64 XCpuBarrierMask2;
+
+INLINE u32 Xil_CpuId()
+{
+    u32 uxCpuId;
+
+    uxCpuId = mfcp( MPIDR_EL1 );
+    uxCpuId = uxCpuId & ARM_CA53_CPU_ID_MASK;
+
+    return uxCpuId;
+}
+
+void Xil_CpuBarrier()
+{
+    u64 mask1;
+    u64 mask2;
+
+    u32 uxCpuId = Xil_CpuId();
+
+    /* We disable interrupts here to ensure that nothing distracts the
+     * synchronization of the cores. */
+    Xil_CpuDisableInterrupts();
+
+    /* 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( &XCpuBarrierMask2 );
+
+    /* We mark our core as ready to sync by flipping the corresponding bit in
+     * the barrier 1 mask. */
+    atomic_fetch_xor( &XCpuBarrierMask1, XCORE_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( &XCpuBarrierMask1 ) ^ mask2 != 0 )
+        {
+            Xil_CpuWaitForEvent();
+        }
+        else
+        {
+            Xil_CpuSignalEvent();
+            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( &XCpuBarrierMask1 );
+
+    /* We mark our core as synced by flipping the corresponding bit in
+     * the barrier 1 mask. */
+    atomic_fetch_xor( &XCpuBarrierMask2, XCORE_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( &XCpuBarrierMask2 ) != ( XAMP_CORE_MASK_U64 ) )
+        {
+            Xil_CpuWaitForEvent();
+        }
+        else
+        {
+            Xil_CpuSignalEvent();
+            break;
+        }
+    }
+
+    Xil_CpuEnableInterrupts();
+}

src/drivers/CMakeLists.txt

@@ -6,6 +6,7 @@ add_subdirectory(axiethernet)
 add_subdirectory(axipmon)
 add_subdirectory(canps)
 add_subdirectory(clockps)
+add_subdirectory(coresight)
 add_subdirectory(coresightps_dcc)
 add_subdirectory(csudma)
 add_subdirectory(emaclite)

src/drivers/coresight/CMakeLists.txt

+cmake_minimum_required(VERSION 3.7 FATAL_ERROR)
+
+target_sources(
+    xil
+        #
+        PRIVATE
+            "${CMAKE_CURRENT_LIST_DIR}/xcoresight_stm_trace.c"
+            "${CMAKE_CURRENT_LIST_DIR}/xcoresight_timestamping.c"
+)

src/drivers/coresight/xcoresight_stm_trace.c

+/*
+ * This file is based on coresight-stm.c from the linux kernel source.
+ *