Updated SAMA drivers (still incomplete)

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@12543 110e8d01-0319-4d1e-a829-52ad28d1bb01

12 files changed, 423 insertions, 100 deletions

diff --git a/os/common/ports/ARM/compilers/GCC/chtypes.h b/os/common/ports/ARM/compilers/GCC/chtypes.h
index 23c3b8e7b..048101666 100644
--- a/os/common/ports/ARM/compilers/GCC/chtypes.h
+++ b/os/common/ports/ARM/compilers/GCC/chtypes.h
@@ -33,6 +33,24 @@
 #include <stdbool.h>

 

 /**

+ * @name    Common constants

+ */

+/**

+ * @brief   Generic 'false' boolean constant.

+ */

+#if !defined(FALSE) || defined(__DOXYGEN__)

+#define FALSE               0

+#endif

+

+/**

+ * @brief   Generic 'true' boolean constant.

+ */

+#if !defined(TRUE) || defined(__DOXYGEN__)

+#define TRUE                1

+#endif

+/** @} */

+

+/**

  * @name    Kernel types

  * @{

  */

diff --git a/os/common/ports/ARMCAx-TZ/chtssi.c b/os/common/ports/ARMCAx-TZ/chtssi.c
index 4e37c7be5..9948aa75b 100644
--- a/os/common/ports/ARMCAx-TZ/chtssi.c
+++ b/os/common/ports/ARMCAx-TZ/chtssi.c
@@ -1,5 +1,5 @@
 /*

-    ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio

+    ChibiOS - Copyright (C) 2006..2018 Isidoro Orabona

 

     Licensed under the Apache License, Version 2.0 (the "License");

     you may not use this file except in compliance with the License.

@@ -37,8 +37,18 @@
 /*===========================================================================*/

 /* Module local definitions.                                                 */

 /*===========================================================================*/

+/* Granted timeslices to trusted service. DO NOT modify those values.

+   They have non secure world counterparts.*/

+typedef enum {

+  TS_TIMEINT_1000_US  = 1000,

+  TS_TIMEINT_10000_US = 10000

+} ts_timeint_t;

 

 #define LOWORD(in64) ((int64_t)in64 & 0x0FFFFFFFF)

+#define TS_TIME2I(tmo)                                                      \

+   (tmo == TS_TIMEINT_10000_US ? TIME_US2I(TS_TIMEINT_10000_US) :           \

+                                 TIME_US2I(TS_TIMEINT_1000_US))

+#define FDT_MAGIC 0xd00dfeed

 

 /*===========================================================================*/

 /* Module exported variables.                                                */

@@ -47,6 +57,8 @@
 /* If a services file is missing in the user application.*/

 CC_WEAK ts_state_t ts_state[TS_MAX_SVCS];

 CC_WEAK const thread_descriptor_t ts_configs[TS_MAX_SVCS];

+CC_WEAK const fc_descriptor_t ts_fc_configs[1];

+uint32_t ts_fc_configs_n;

 

 /* The reference to the suspended NSEC main thread.*/

 thread_reference_t _ns_thread = NULL;

@@ -54,10 +66,31 @@ thread_reference_t _ns_thread = NULL;
 /* The services may broadcast and listen event flags via this object.*/

 EVENTSOURCE_DECL(tsEventSource);

 

+extern uint32_t __ram0_start__;

+

 /*===========================================================================*/

 /* Module local types.                                                       */

 /*===========================================================================*/

 

+struct fdt_header {

+  uint32_t magic;           /* magic word FDT_MAGIC */

+  uint32_t totalsize;       /* total size of DT block */

+  uint32_t off_dt_struct;   /* offset to structure */

+  uint32_t off_dt_strings;  /* offset to strings */

+  uint32_t off_mem_rsvmap;  /* offset to memory reserve map */

+  uint32_t version;         /* format version */

+  uint32_t last_comp_version;   /* last compatible version */

+

+  /* version 2 fields below */

+  uint32_t boot_cpuid_phys; /* Which physical CPU id we're

+                               booting on */

+  /* version 3 fields below */

+  uint32_t size_dt_strings; /* size of the strings block */

+

+  /* version 17 fields below */

+  uint32_t size_dt_struct;  /* size of the structure block */

+};

+

 /*===========================================================================*/

 /* Module local variables.                                                   */

 /*===========================================================================*/

@@ -69,6 +102,14 @@ static event_listener_t tsEventListener;
 /* Module local functions.                                                   */

 /*===========================================================================*/

 

+static inline uint32_t uswap32(uint32_t v)

+{

+  uint32_t result;

+

+  __asm volatile ("rev %0, %1" : "=r" (result) : "r" (v));

+  return result;

+}

+

 static bool isAddrSpaceValid(uint8_t *addr, size_t size)

 {

   if (size == 0)

@@ -134,12 +175,13 @@ static ts_state_t *findSvcsEntry(const char *name)
  * @notapi

  */

 int64_t smcEntry(ts_state_t *svc_handle, ts_params_area_t svc_data,

-               size_t svc_datalen, sysinterval_t svc_timeout) {

+               size_t svc_datalen, ts_timeint_t svc_timeout) {

   ts_state_t *tssp = NULL;

   msg_t r;

 

-  /* Internal query service.*/

   if (svc_handle == TS_HND_STQRY) {

+

+    /* Internal query status service.*/

     ts_state_t *tsqryd;

 

     /* svc_data is the handle of the service to whom 'query' the state.*/

@@ -159,7 +201,19 @@ int64_t smcEntry(ts_state_t *svc_handle, ts_params_area_t svc_data,
     if (!isAddrSpaceValid(svc_data, svc_datalen))

       return LOWORD(SMC_SVC_INVALID);

 

-    if (svc_handle == TS_HND_VERSION) {

+    uint32_t i = (uint32_t)svc_handle;

+

+    if ((i & TS_FASTCALL_MASK) == TS_FASTCALL_MASK) {

+

+      /* Fast call user service.*/

+      i &= ~TS_FASTCALL_MASK;

+

+      if (i >= ts_fc_configs_n)

+        return LOWORD(SMC_SVC_BADH);

+

+      return TS_FC_CONF_TABLE(i)->funcp(svc_data, svc_datalen);

+    }

+    else if (svc_handle == TS_HND_VERSION) {

 

       /* Internal get version service.*/

       return LOWORD(TSSI_VERSION);

@@ -181,13 +235,13 @@ int64_t smcEntry(ts_state_t *svc_handle, ts_params_area_t svc_data,
       if (!isHndlValid(svc_handle))

         return LOWORD(SMC_SVC_BADH);

       tssp = svc_handle;

-    }

 

-    /* If the service is not waiting requests, it's busy.*/

-    if (tssp->ts_thdp == NULL)

-      return LOWORD(SMC_SVC_BUSY);

-    tssp->ts_datap = svc_data;

-    tssp->ts_datalen = svc_datalen;

+      /* If the service is not waiting requests, it's busy.*/

+      if (tssp->ts_thdp == NULL)

+        return LOWORD(SMC_SVC_BUSY);

+      tssp->ts_datap = svc_data;

+      tssp->ts_datalen = svc_datalen;

+    }

   }

 

 #if (CH_DBG_SYSTEM_STATE_CHECK == TRUE)

@@ -200,7 +254,11 @@ int64_t smcEntry(ts_state_t *svc_handle, ts_params_area_t svc_data,
 

   if (tssp)

     chThdResumeS(&tssp->ts_thdp, MSG_OK);

-  r = chThdSuspendTimeoutS(&_ns_thread, TIME_US2I(svc_timeout));

+  r = chThdSuspendTimeoutS(&_ns_thread, TS_TIME2I(svc_timeout));

+

+  /* Map MSG_TIMEOUT to SMC_SVC_INTR.*/

+  if (r == MSG_TIMEOUT)

+    r = SMC_SVC_INTR;

 

   /* Get and clear any pending event flags.*/

   eventflags_t f = chEvtGetAndClearFlagsI(&tsEventListener);

@@ -233,7 +291,7 @@ msg_t tssiWaitRequest(ts_state_t *svcp)
   chSysLock();

   if (_ns_thread) {

     /* Ack a previous service invocation. Not schedule.*/

-    chThdResumeI(&_ns_thread, svcp->ts_status);

+    chThdResumeI(&_ns_thread, SMC_SVC_INTR);

   }

   r = chThdSuspendS(&svcp->ts_thdp);

   chSysUnlock();

@@ -266,26 +324,36 @@ CC_NO_RETURN void tssiInit(void)
   int32_t i;

   uint32_t d;

   uint32_t *tt;

+  struct fdt_header *pfdt = (struct fdt_header *)NSEC_MEMORY_START_ADDR;

+  void *moveto = NULL;

 

   /*

    * The main DDR memory, PORT0, is divided in 4 region, each 32MB large.

    * The last region is split in two areas, each 16MB large.

    * The first 3 region and the lower area of this last region is non secure.

    * All the rest of the regions space is secured.

-   * The same applies to AESB view of the DDR, PORT1

+   * The same applies to AESB view of the DDR, PORT1, and LCDC view.

    *

    * Those settings depend on the designed memory mapping.

    */

   mtxSetSlaveRegionSize(MATRIX0, H64MX_SLAVE_DDR_PORT0, MATRIX_AREA_SIZE_32M, REGION_0_MSK);

   mtxSetSlaveRegionSize(MATRIX0, H64MX_SLAVE_DDR_PORT1, MATRIX_AREA_SIZE_32M, REGION_0_MSK);

+  mtxSetSlaveRegionSize(MATRIX0, H64MX_SLAVE_DDR_PORT2, MATRIX_AREA_SIZE_32M, REGION_0_MSK);

+  mtxSetSlaveRegionSize(MATRIX0, H64MX_SLAVE_DDR_PORT3, MATRIX_AREA_SIZE_32M, REGION_0_MSK);

 

   mtxSetSlaveSplitAddr(MATRIX0, H64MX_SLAVE_DDR_PORT0, MATRIX_AREA_SIZE_32M,

       REGION_0_MSK | REGION_1_MSK | REGION_2_MSK);

   mtxSetSlaveSplitAddr(MATRIX0, H64MX_SLAVE_DDR_PORT1, MATRIX_AREA_SIZE_32M,

       REGION_0_MSK | REGION_1_MSK | REGION_2_MSK);

+  mtxSetSlaveSplitAddr(MATRIX0, H64MX_SLAVE_DDR_PORT2, MATRIX_AREA_SIZE_32M,

+      REGION_0_MSK | REGION_1_MSK | REGION_2_MSK);

+  mtxSetSlaveSplitAddr(MATRIX0, H64MX_SLAVE_DDR_PORT3, MATRIX_AREA_SIZE_32M,

+      REGION_0_MSK | REGION_1_MSK | REGION_2_MSK);

 

   mtxSetSlaveSplitAddr(MATRIX0, H64MX_SLAVE_DDR_PORT0, MATRIX_AREA_SIZE_16M, REGION_3_MSK);

   mtxSetSlaveSplitAddr(MATRIX0, H64MX_SLAVE_DDR_PORT1, MATRIX_AREA_SIZE_16M, REGION_3_MSK);

+  mtxSetSlaveSplitAddr(MATRIX0, H64MX_SLAVE_DDR_PORT2, MATRIX_AREA_SIZE_16M, REGION_3_MSK);

+  mtxSetSlaveSplitAddr(MATRIX0, H64MX_SLAVE_DDR_PORT3, MATRIX_AREA_SIZE_16M, REGION_3_MSK);

 

   mtxConfigSlaveSec(MATRIX0, H64MX_SLAVE_DDR_PORT0,

       mtxRegionLansech(REGION_0, UPPER_AREA_SECURABLE) |

@@ -311,7 +379,31 @@ CC_NO_RETURN void tssiInit(void)
       mtxRegionWrnsech(REGION_1, NOT_SECURE_WRITE) |

       mtxRegionWrnsech(REGION_2, NOT_SECURE_WRITE));

 

-#if !HAL_USE_SDMMC

+  mtxConfigSlaveSec(MATRIX0, H64MX_SLAVE_DDR_PORT2,

+      mtxRegionLansech(REGION_0, UPPER_AREA_SECURABLE) |

+      mtxRegionLansech(REGION_1, UPPER_AREA_SECURABLE) |

+      mtxRegionLansech(REGION_2, UPPER_AREA_SECURABLE) |

+      mtxRegionLansech(REGION_3, UPPER_AREA_SECURABLE),

+      mtxRegionRdnsech(REGION_0, NOT_SECURE_READ) |

+      mtxRegionRdnsech(REGION_1, NOT_SECURE_READ) |

+      mtxRegionRdnsech(REGION_2, NOT_SECURE_READ),

+      mtxRegionWrnsech(REGION_0, NOT_SECURE_WRITE) |

+      mtxRegionWrnsech(REGION_1, NOT_SECURE_WRITE) |

+      mtxRegionWrnsech(REGION_2, NOT_SECURE_WRITE));

+

+  mtxConfigSlaveSec(MATRIX0, H64MX_SLAVE_DDR_PORT3,

+      mtxRegionLansech(REGION_0, UPPER_AREA_SECURABLE) |

+      mtxRegionLansech(REGION_1, UPPER_AREA_SECURABLE) |

+      mtxRegionLansech(REGION_2, UPPER_AREA_SECURABLE) |

+      mtxRegionLansech(REGION_3, UPPER_AREA_SECURABLE),

+      mtxRegionRdnsech(REGION_0, NOT_SECURE_READ) |

+      mtxRegionRdnsech(REGION_1, NOT_SECURE_READ) |

+      mtxRegionRdnsech(REGION_2, NOT_SECURE_READ),

+      mtxRegionWrnsech(REGION_0, NOT_SECURE_WRITE) |

+      mtxRegionWrnsech(REGION_1, NOT_SECURE_WRITE) |

+      mtxRegionWrnsech(REGION_2, NOT_SECURE_WRITE));

+

+#if !SAMA_USE_SDMMC

 

   /* Configure the SDMMCx regions as non secure.*/

   mtxSetSlaveSplitAddr(MATRIX0, H64MX_SLAVE_SDMMC, MATRIX_AREA_SIZE_128M, REGION_1_MSK|REGION_2_MSK);

@@ -324,15 +416,31 @@ CC_NO_RETURN void tssiInit(void)
       mtxRegionWrnsech(REGION_2, NOT_SECURE_WRITE));

 #endif

 

-  /* Mark the whole non secure memory region as non executable

-     by the secure side.*/

+  /* Mark the whole non secure memory region non executable

+     by the secure side, and set the Non-Secure access bit, so that

+     any access to this region is in the non-secure physical

+     space. This ensures the coherence of the cache between

+     secure and non secure accesses.*/

   tt = (uint32_t *)(__get_TTBR0() & 0xFFFFC000);

   for (d = ((uint32_t)NSEC_MEMORY_START_ADDR >> 20);

        d < ((uint32_t)NSEC_MEMORY_END_ADDR >> 20); d += 1) {

+    MMU_SecureSection(tt + d, NON_SECURE);

+    MMU_XNSection(tt + d, NON_EXECUTE);

+  }

+

+  /* The same, but for the AESB view of the DDR memory region.*/

+  for (d = ((uint32_t)(NSEC_MEMORY_START_ADDR + 0x20000000) >> 20);

+       d < ((uint32_t)(NSEC_MEMORY_END_ADDR + 0x20000000) >> 20); d += 1) {

+    MMU_SecureSection(tt + d, NON_SECURE);

     MMU_XNSection(tt + d, NON_EXECUTE);

   }

   MMU_InvalidateTLB();

 

+  /* Flush the modified MMU table.*/

+  cacheCleanRegion(tt, d * sizeof (uint32_t));

+  __DSB();

+  __ISB();

+

   /* Make sure that prio is NORMALPRIO.*/

   chThdSetPriority(NORMALPRIO);

 

@@ -342,12 +450,12 @@ CC_NO_RETURN void tssiInit(void)
       continue;

 

     /* Check that the initialization of the TS_TABLE against TS_STATE_TABLE

-       was set right.*/

+       has been set right.*/

     if (TS_CONF_TABLE(i)->arg != TS_STATE(i)) {

       chSysHalt("Bad TS_STATE setting in the services configuration table.");

     }

 

-    /* Check that the service priority was set right.*/

+    /* Check that the service priority has been set right.*/

     if ((TS_CONF_TABLE(i)->prio <= NORMALPRIO) ||

         (TS_CONF_TABLE(i)->prio >= HIGHPRIO)) {

       chSysHalt("Bad prio setting in the services configuration table.");

@@ -357,25 +465,55 @@ CC_NO_RETURN void tssiInit(void)
     chThdCreate(TS_CONF_TABLE(i));

   }

 

+  /* Fast call services.*/

+  for (i = 0; TS_FC_CONF_TABLE(i)->name; ++i) {

+

+    /* Check that the 'code' field of the

+       fast call table has been set right.*/

+    if ((TS_FC_CONF_TABLE(i)->code &~ TS_FASTCALL_MASK) != (uint32_t)i) {

+      chSysHalt("Bad 'code' setting in the fast call configuration table.");

+    }

+  }

+  ts_fc_configs_n = i;

+

   /* Register to the daemon services events. All flags.*/

   chEvtRegister(&tsEventSource, &tsEventListener, EVT_DAEMON_REQ_ATN);

 

-  /* Now set the priority to the max.*/

+  /* Now set the priority at the max.*/

   chThdSetPriority(HIGHPRIO);

 

-  /* Remove write protection on PMC registers.*/

-  pmcDisableWP();

-

   /* Allow non secure access to CP10 and CP11.*/

   asm volatile (

-    "MRC p15, 0, r0, c1, c1, 2    \n"

-    "ORR r0, r0, #0b11<<10        \n"

-    "MCR p15, 0, r0, c1, c1, 2    \n"

+    "mrc p15, 0, r0, c1, c1, 2    \n"

+    "orr r0, r0, #0b11<<10        \n"

+    "mcr p15, 0, r0, c1, c1, 2    \n"

   );

 

+  /* Check if a fdt image exists at the start

+     of the non secure memory region.*/

+  if (uswap32(pfdt->magic) == FDT_MAGIC) {

+    uint32_t fdtsize;

+

+    /* Detected a fdt structure.

+       Move it to the end of non secure area.*/

+    fdtsize = uswap32(pfdt->totalsize);

+    fdtsize = (fdtsize + 4095) &~ 4095;

+    moveto = (void *)(SEC_MEMORY_START_ADDR - fdtsize);

+    memmove(moveto, pfdt, fdtsize);

+

+    /* Invalidate the original fdt image.*/

+    pfdt->magic = 0;

+  }

+

   /* Jump in the NON SECURE world.

      This thread becomes the non secure environment as view by

      the secure world.*/

+

+  /* r2 address of the moved fdt, if any.*/

+  asm volatile (

+    "mov r2, %0 \n" :: "r" (moveto)

+  );

+

   _ns_trampoline(NSEC_MEMORY_START_ADDR + NSEC_MEMORY_EXE_OFFSET);

 

   /* It never goes here.*/

diff --git a/os/common/ports/ARMCAx-TZ/chtssi.h b/os/common/ports/ARMCAx-TZ/chtssi.h
index ae9cec7af..ca3d7aa5f 100644
--- a/os/common/ports/ARMCAx-TZ/chtssi.h
+++ b/os/common/ports/ARMCAx-TZ/chtssi.h
@@ -1,5 +1,5 @@
 /*

-    ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio.

+    ChibiOS - Copyright (C) 2006..2018 Isidoro Orabona.

 

     This file is part of ChibiOS.

 

@@ -40,15 +40,16 @@
 #define TSSI_VERSION          0x01000000    /* 00 major, 000 minor, 000 build.*/

 

 /* Service registry errors as returned by smc.*/

-#define SMC_SVC_OK            MSG_OK        /* No error.*/

-#define SMC_SVC_INTR          (msg_t)-1     /* Service interrupted ( == MSG_TIMEOUT).*/

-#define SMC_SVC_NOENT         (msg_t)-2     /* No existent service.*/

-#define SMC_SVC_INVALID       (msg_t)-3     /* Invalid service parameter(s).*/

-#define SMC_SVC_BADH          (msg_t)-4     /* Invalid service handle.*/

-#define SMC_SVC_EXIST         (msg_t)-5     /* Service already exists.*/

-#define SMC_SVC_NHND          (msg_t)-6     /* No more services or

-                                               service resources.*/

-#define SMC_SVC_BUSY          (msg_t)-7     /* Service busy.*/

+#define SMC_SVC_OK            (int32_t)0      /* No error.*/

+#define SMC_SVC_INTR          (int32_t)-4     /* Service interrupted.*/

+#define SMC_SVC_NOENT         (int32_t)-2     /* No existent service.*/

+#define SMC_SVC_INVALID       (int32_t)-22    /* Invalid service

+                                                 parameter(s).*/

+#define SMC_SVC_BADH          (int32_t)-9     /* Invalid service handle.*/

+#define SMC_SVC_EXIST         (int32_t)-17    /* Service already exists.*/

+#define SMC_SVC_NHND          (int32_t)-23    /* No more services or

+                                                 service resources.*/

+#define SMC_SVC_BUSY          (int32_t)-16    /* Service busy.*/

 

 /* Special trusted service handles.*/

 #define TS_HND_TRAMP          ((ts_state_t *)0)  /* Trampoline service handle.*/

@@ -57,6 +58,11 @@
 #define TS_HND_IDLE           ((ts_state_t *)3)  /* Idle service handle.*/

 #define TS_HND_VERSION        ((ts_state_t *)4)  /* Get version service handle.*/

 

+/* Fast call service bitmask.

+   Service handles that contain this mask access to

+   the fast call table.*/

+#define TS_FASTCALL_MASK      0xFFFF0000

+

 /* Services events event mask.*/

 #define EVT_DAEMON_REQ_ATN    EVENT_MASK(0)

 

@@ -116,6 +122,30 @@ typedef struct tssi_service_state {
   uint32_t            ts_datalen;

 } ts_state_t;

 

+/**

+ * @brief   Fast call function.

+ */

+typedef msg_t (*fcfunc_t)(ts_params_area_t ts_datap, uint32_t ts_datalen);

+

+/**

+ * @brief   Type of a fast call descriptor.

+ */

+typedef struct {

+  /**

+   * @brief   Fast call service name.

+   */

+  const char  *name;

+

+  /* The code identifying the service.

+     Used for checking purpose, it must correspond to the

+     order that the service lists in the descriptor table.*/

+  uint32_t    code;

+  /**

+   * @brief   Fast call function pointer.

+   */

+  fcfunc_t    funcp;

+} fc_descriptor_t;

+

 /*===========================================================================*/

 /* Module macros.                                                            */

 /*===========================================================================*/

@@ -190,6 +220,48 @@ typedef struct tssi_service_state {
 

 /** @} */

 

+/**

+ * @name    Fast call table definition macros.

+ * @note    Fast call services run at max priority level, so it is

+ *          mandatory that they last less time as possible.

+ * @note    Fast call services should be invoked using

+ *          the tsInvoke0 function in order to optimize the

+ *          performances.

+ * @note    Fast call services don't have a runtime state, so

+ *          the response management is in charge to the higher levels.

+ * @{

+ */

+

+/**

+ * @brief   Start of user fast call service table.

+ */

+#define TS_FC_CONF_TABLE_BEGIN                                 \

+  const fc_descriptor_t ts_fc_configs[] = {

+

+/**

+ * @brief   Entry of user fast call services table.

+ */

+#define TS_FC_CONF_TABLE_ENTRY(name, code, funcp)               \

+  {name, code, funcp},

+

+/**

+ * @brief   End of user fast call services table.

+ */

+#define TS_FC_CONF_TABLE_END                                    \

+};

+

+/**

+ * @brief   Accessor to the fast call service table entry i.

+ */

+#define TS_FC_CONF_TABLE(i) (&ts_fc_configs[i])

+

+/**

+ * @brief   Number of entries in the fast call service table.

+ */

+#define TS_FC_CONF_TABLE_N (sizeof ts_fc_configs / sizeof ts_fc_configs[0])

+

+/** @} */

+

 /*===========================================================================*/

 /* External declarations.                                                    */

 /*===========================================================================*/

diff --git a/os/common/ports/ARMCAx-TZ/compilers/GCC/monitor.S b/os/common/ports/ARMCAx-TZ/compilers/GCC/monitor.S
index 647966b72..9a3faec34 100644
--- a/os/common/ports/ARMCAx-TZ/compilers/GCC/monitor.S
+++ b/os/common/ports/ARMCAx-TZ/compilers/GCC/monitor.S
@@ -66,7 +66,7 @@
                 .set    MON_S_SCR,      (SCR_IRQ)           // (SCR_EA|SCR_IRQ)
                 .set    MON_NS_SCR,     (SCR_FIQ|SCR_NS)
 
-                .set    SMC_SVC_INTR,   -1
+                .set    SMC_SVC_INTR,   -4
 
                 .comm   sm_secctx, 20*4, 4
                 .comm   sm_nsecctx, 20*4, 4
@@ -126,6 +126,7 @@
  * Monitor vectors
  */
 				.global _monitor_vectors
+				.balign 32
 _monitor_vectors:
                 b   .           // Reset vector, not used
                 b   .           // Undefined instruction, not used
@@ -281,7 +282,7 @@ _ns_trampoline:
                 cps     #MODE_SYS
 
                 ldr     r0, =#0
-                mov     r2, r0
+/*              mov     r2, r0  */              // r2 could contain the address of the fdt
                 mov     r3, r0
                 mov     r4, r0
                 mov     r5, r0
diff --git a/os/common/startup/ARM/compilers/GCC/ld/SAMA5D2bvddr.ld b/os/common/startup/ARM/compilers/GCC/ld/SAMA5D2bvddr.ld
deleted file mode 100755
index efa2f63d9..000000000
--- a/os/common/startup/ARM/compilers/GCC/ld/SAMA5D2bvddr.ld
+++ /dev/null
@@ -1,46 +0,0 @@
-/*

-    ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio

-

-    Licensed under the Apache License, Version 2.0 (the "License");

-    you may not use this file except in compliance with the License.

-    You may obtain a copy of the License at

-

-        http://www.apache.org/licenses/LICENSE-2.0

-

-    Unless required by applicable law or agreed to in writing, software

-    distributed under the License is distributed on an "AS IS" BASIS,

-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-    See the License for the specific language governing permissions and

-    limitations under the License.

-*/

-

-/*

- * SAMA5D2 memory setup in non trusted mode.

- */

-MEMORY

-{

-    flash : org = 0x21000000, len = 16M

-    ram0  : org = 0x22000000, len = 96M

-    ram1  : org = 0x00000000, len = 0

-    ram2  : org = 0x00000000, len = 0

-    ram3  : org = 0x00000000, len = 0

-    ram4  : org = 0x00000000, len = 0

-    ram5  : org = 0x00000000, len = 0

-    ram6  : org = 0x00000000, len = 0

-    ram7  : org = 0x00000000, len = 0

-}

-

-/* RAM region to be used for stacks. This stack accommodates the processing

-   of all exceptions and interrupts*/

-REGION_ALIAS("STACKS_RAM", ram0);

-

-/* RAM region to be used for data segment.*/

-REGION_ALIAS("DATA_RAM", ram0);

-

-/* RAM region to be used for BSS segment.*/

-REGION_ALIAS("BSS_RAM", ram0);

-

-INCLUDE rules.ld

-

-ENTRY(Boot_Handler);

-

diff --git a/os/common/startup/ARM/compilers/GCC/ld/SAMA5D2ddr.ld b/os/common/startup/ARM/compilers/GCC/ld/SAMA5D2ddr.ld
index e2bcb004a..087234cbf 100755
--- a/os/common/startup/ARM/compilers/GCC/ld/SAMA5D2ddr.ld
+++ b/os/common/startup/ARM/compilers/GCC/ld/SAMA5D2ddr.ld
@@ -19,10 +19,10 @@
  */

 MEMORY

 {

-    flash : org = 0x20000000, len = 16M

-    ram0  : org = 0x21000000, len = 96M

-    ram1  : org = 0x00000000, len = 0

-    ram2  : org = 0x00000000, len = 0

+    flash : org = 0x21000000, len = 16M

+    ram0  : org = 0x22000000, len = 78M

+    ram1  : org = 0x26E00000, len = 1M

+    ram2  : org = 0x26F00000, len = 1M

     ram3  : org = 0x00000000, len = 0

     ram4  : org = 0x00000000, len = 0

     ram5  : org = 0x00000000, len = 0

@@ -40,6 +40,37 @@ REGION_ALIAS("DATA_RAM", ram0);
 /* RAM region to be used for BSS segment.*/

 REGION_ALIAS("BSS_RAM", ram0);

 

+/* RAM region to be used for lcd frame buffer. */

+REGION_ALIAS("FB_RAM", ram1);

+

+/* RAM region to be used for no cache area. */

+REGION_ALIAS("NO_CACHE", ram2);

+

+SECTIONS

+{

+    /* Special section for frame buffer area.*/

+    .fbram (NOLOAD) : ALIGN(4)

+    {

+        __fbram_base__ = .;

+        *(.fbram)

+        *(.fbram.*)

+        *(.bss.__fbram_*)

+        . = ALIGN(4);

+        __fbram_end__ = .;

+    } > FB_RAM

+    

+    /* Special section for no cache area.*/

+    .nocache (NOLOAD) : ALIGN(4)

+    {

+        __nocache_base__ = .;

+        *(.nocache)

+        *(.nocache.*)

+        *(.bss.__nocache_*)

+        . = ALIGN(4);

+        __nocache_end__ = .;

+    } > NO_CACHE

+}

+

 INCLUDE rules.ld

 

 ENTRY(Boot_Handler);

diff --git a/os/common/startup/ARM/devices/SAMA5D2/mmu.c b/os/common/startup/ARM/devices/SAMA5D2/mmu.c
index a4f3b0ea0..449019526 100644
--- a/os/common/startup/ARM/devices/SAMA5D2/mmu.c
+++ b/os/common/startup/ARM/devices/SAMA5D2/mmu.c
@@ -48,9 +48,17 @@
 #define SAMA_L2CC_ENABLE 0

 #endif

 

+#if (SAMA_L2CC_ASSUME_ENABLED && SAMA_L2CC_ENABLE)

+#error "These macros are mutually exclusive"

+#endif

+

 /*===========================================================================*/

 /* Module local definitions.                                                 */

 /*===========================================================================*/

+/*

+ * @brief    No cacheable memory start address.

+ */

+#define NO_CACHE_MEMORY_START_ADDR      ((uint8_t *) 0x26F00000)

 

 /*===========================================================================*/

 /* Module exported variables.                                                */

@@ -77,7 +85,7 @@
  * +---------+--+-+--+-+-----+--------+-------+-+------+--+-+-+-+---+

  * | section |NS|0|nG|S|AP[2]|TEX[2:0]|AP[1:0]| |domain|XN|C|B|1|PXN|

  * +---------+--+-+--+-+-----+--------+-------+-+------+--+-+-+-+---+

- * |         |0 |0|0 |1|0    |111     |11     |0|0000  |0 |1|1|1|0  | == normal, cacheable

+ * |         |X |0|0 |0|0    |111     |11     |0|0000  |0 |1|1|1|0  | == normal, cacheable, write back, no write allocate

  * |         |0 |0|0 |1|0    |100     |11     |0|0000  |0 |0|0|1|0  | == normal, no-cacheable

  * |         |0 |0|0 |1|0    |000     |11     |0|0000  |0 |0|1|1|0  | == device

  * |         |0 |0|0 |1|0    |000     |11     |0|0000  |0 |0|0|1|0  | == strongly-ordered

@@ -121,8 +129,8 @@ void __core_init(void) {
      Invalidate a disabled L1 D Cache.*/

   pm = __get_SCTLR();

   if ((pm & SCTLR_C_Msk)) {

-    L1C_CleanInvalidateCache(DCISW_CLEAN);

     L1C_DisableCaches();

+    L1C_CleanInvalidateCache(DCISW_CLEAN);

   }

 

   /* Disable the MMU and invalidate TLB.*/

@@ -164,7 +172,7 @@ void __core_init(void) {
                   TTE_SECT_MEM_CACHEABLE |

                   TTE_SECT_RW_ACCESS |

                   TTE_SECT_DOM(0x00) |

-                  TTE_SECT_S | TTE_TYPE_SECT;

+                  TTE_TYPE_SECT;

   /*

    * UDPHS RAM region

    *

@@ -264,7 +272,7 @@ void __core_init(void) {
                     TTE_SECT_MEM_CACHEABLE |

                     TTE_SECT_RW_ACCESS |

                     TTE_SECT_DOM(0x00) |

-                    TTE_SECT_S | TTE_TYPE_SECT;

+                    TTE_TYPE_SECT;

   /*

    * DDR AESB regions

    *

@@ -275,7 +283,7 @@ void __core_init(void) {
                     TTE_SECT_MEM_CACHEABLE |

                     TTE_SECT_RW_ACCESS |

                     TTE_SECT_DOM(0x00) |

-                    TTE_SECT_S | TTE_TYPE_SECT;

+                    TTE_TYPE_SECT;

   /*

    * EBI 1, 2 and 3 regions

    *

@@ -360,6 +368,9 @@ void __core_init(void) {
                   TTE_SECT_EXE_NEVER |

                   TTE_SECT_S | TTE_TYPE_SECT;

 

+  /* Make a NO CACHE AREA */

+  MMU_MemorySection((mmuTable + ((uint32_t)NO_CACHE_MEMORY_START_ADDR >> 20)), NORMAL, NON_CACHEABLE, NON_CACHEABLE);

+

   /* Invalidate TLB and L1 I cache

      Enable caches and MMU.*/

   MMU_InvalidateTLB();

@@ -395,11 +406,17 @@ void __core_init(void) {
 

     /* Invalidate and enable L2 cache.*/

     L2C_InvAllByWay();

+    L2C_310->AUX_CNT = L2CC_ACR_DPEN | L2CC_ACR_IPEN;

+

+    /* Prefetch control register. Double linefeed, instr and data enabled.*/

+    *((uint32_t *)((char *)L2C_310+0x0F60)) = 0x75800001;

     L2C_Enable();

     __DSB();

     __ISB();

   }

 #endif

+

+

 #endif

 }

 

diff --git a/os/common/startup/ARMCAx-TZ/compilers/GCC/crt0.S b/os/common/startup/ARMCAx-TZ/compilers/GCC/crt0.S
index 4810ea0f6..016e23c58 100644
--- a/os/common/startup/ARMCAx-TZ/compilers/GCC/crt0.S
+++ b/os/common/startup/ARMCAx-TZ/compilers/GCC/crt0.S
@@ -22,6 +22,20 @@
  * @{

  */

 

+/**

+ * @brief   Constructors invocation switch.

+ */

+#if !defined(CRT0_CALL_CONSTRUCTORS) || defined(__DOXYGEN__)

+#define CRT0_CALL_CONSTRUCTORS              TRUE

+#endif

+

+/**

+ * @brief   Destructors invocation switch.

+ */

+#if !defined(CRT0_CALL_DESTRUCTORS) || defined(__DOXYGEN__)

+#define CRT0_CALL_DESTRUCTORS               TRUE

+#endif

+

 #if !defined(__DOXYGEN__)

 

                 .set    MODE_USR, 0x10

@@ -117,10 +131,38 @@ bssloop:
                  */

                 bl      __core_init

                 bl      __late_init

+#if 0 /* Constructors initialized after halInit() */

+#if CRT0_CALL_CONSTRUCTORS == TRUE

+                /* Constructors invocation.*/

+                ldr     r4, =__init_array_start

+                ldr     r5, =__init_array_end

+initloop:

+                cmp     r4, r5

+                bge     endinitloop

+                ldr     r1, [r4], #4

+                blx     r1

+                b       initloop

+endinitloop:

+#endif /* CRT0_CALL_CONSTRUCTORS */

+#endif /* if 0 */

                 /*

                  * Main program invocation.

                  */

                 bl      main

+

+#if CRT0_CALL_DESTRUCTORS == TRUE

+                /* Destructors invocation.*/

+                ldr     r4, =__fini_array_start

+                ldr     r5, =__fini_array_end

+finiloop:

+                cmp     r4, r5

+                bge     endfiniloop

+                ldr     r1, [r4], #4

+                blx     r1

+                b       finiloop

+endfiniloop:

+#endif

+

                 b       __default_exit

 

 #endif /* !defined(__DOXYGEN__) */

diff --git a/os/common/startup/ARMCAx-TZ/compilers/GCC/ld/SAMA5D2ddr.ld b/os/common/startup/ARMCAx-TZ/compilers/GCC/ld/SAMA5D2ddr.ld
index 225f64d74..cb016edc0 100644
--- a/os/common/startup/ARMCAx-TZ/compilers/GCC/ld/SAMA5D2ddr.ld
+++ b/os/common/startup/ARMCAx-TZ/compilers/GCC/ld/SAMA5D2ddr.ld
@@ -20,9 +20,9 @@
 MEMORY

 {

     flash : org = 0x27000000, len = 1M

-    ram0  : org = 0x27100000, len = 15M

-    ram1  : org = 0x00000000, len = 0

-    ram2  : org = 0x00000000, len = 0

+    ram0  : org = 0x27100000, len = 13M

+    ram1  : org = 0x27E00000, len = 1M

+    ram2  : org = 0x27F00000, len = 1M

     ram3  : org = 0x00000000, len = 0

     ram4  : org = 0x00000000, len = 0

     ram5  : org = 0x00000000, len = 0

@@ -40,6 +40,37 @@ REGION_ALIAS("DATA_RAM", ram0);
 /* RAM region to be used for BSS segment.*/

 REGION_ALIAS("BSS_RAM", ram0);

 

+/* RAM region to be used for lcd frame buffer. */

+REGION_ALIAS("FB_RAM", ram1);

+

+/* RAM region to be used for no cache area. */

+REGION_ALIAS("NO_CACHE", ram2);

+

+SECTIONS

+{

+    /* Special section for frame buffer area.*/

+    .fbram (NOLOAD) : ALIGN(4)

+    {

+        __fbram_base__ = .;

+        *(.fbram)

+        *(.fbram.*)

+        *(.bss.__fbram_*)

+        . = ALIGN(4);

+        __fbram_end__ = .;

+    } > FB_RAM

+    

+    /* Special section for no cache area.*/

+    .nocache (NOLOAD) : ALIGN(4)

+    {

+        __nocache_base__ = .;

+        *(.nocache)

+        *(.nocache.*)

+        *(.bss.__nocache_*)

+        . = ALIGN(4);

+        __nocache_end__ = .;

+    } > NO_CACHE

+}

+

 INCLUDE rules.ld

 

 ENTRY(Boot_Handler);

diff --git a/os/common/startup/ARMCAx-TZ/devices/SAMA5D2/mmu.c b/os/common/startup/ARMCAx-TZ/devices/SAMA5D2/mmu.c
index ab063dcfc..b4af836c0 100644
--- a/os/common/startup/ARMCAx-TZ/devices/SAMA5D2/mmu.c
+++ b/os/common/startup/ARMCAx-TZ/devices/SAMA5D2/mmu.c
@@ -48,9 +48,17 @@
 #define SAMA_L2CC_ENABLE 0

 #endif

 

+#if (SAMA_L2CC_ASSUME_ENABLED && SAMA_L2CC_ENABLE)

+#error "These macros are mutually exclusive"

+#endif

+

 /*===========================================================================*/

 /* Module local definitions.                                                 */

 /*===========================================================================*/

+/*

+ * @brief    No cacheable memory start address.

+ */

+#define NO_CACHE_MEMORY_START_ADDR      ((uint8_t *) 0x27F00000)

 

 /*===========================================================================*/

 /* Module exported variables.                                                */

@@ -77,7 +85,7 @@
  * +---------+--+-+--+-+-----+--------+-------+-+------+--+-+-+-+---+

  * | section |NS|0|nG|S|AP[2]|TEX[2:0]|AP[1:0]| |domain|XN|C|B|1|PXN|

  * +---------+--+-+--+-+-----+--------+-------+-+------+--+-+-+-+---+

- * |         |0 |0|0 |1|0    |111     |11     |0|0000  |0 |1|1|1|0  | == normal, cacheable

+ * |         |0 |0|0 |0|0    |111     |11     |0|0000  |0 |1|1|1|0  | == normal, cacheable, write back, no write allocate

  * |         |0 |0|0 |1|0    |100     |11     |0|0000  |0 |0|0|1|0  | == normal, no-cacheable

  * |         |0 |0|0 |1|0    |000     |11     |0|0000  |0 |0|1|1|0  | == device

  * |         |0 |0|0 |1|0    |000     |11     |0|0000  |0 |0|0|1|0  | == strongly-ordered

@@ -164,7 +172,7 @@ void __core_init(void) {
                   TTE_SECT_MEM_CACHEABLE |

                   TTE_SECT_RW_ACCESS |

                   TTE_SECT_DOM(0x00) |

-                  TTE_SECT_S | TTE_TYPE_SECT;

+                  TTE_TYPE_SECT;

   /*

    * UDPHS RAM region

    *

@@ -264,7 +272,7 @@ void __core_init(void) {
                     TTE_SECT_MEM_CACHEABLE |

                     TTE_SECT_RW_ACCESS |

                     TTE_SECT_DOM(0x00) |

-                    TTE_SECT_S | TTE_TYPE_SECT;

+                    TTE_TYPE_SECT;

   /*

    * DDR AESB regions

    *

@@ -275,7 +283,7 @@ void __core_init(void) {
                     TTE_SECT_MEM_CACHEABLE |

                     TTE_SECT_RW_ACCESS |

                     TTE_SECT_DOM(0x00) |

-                    TTE_SECT_S | TTE_TYPE_SECT;

+                    TTE_TYPE_SECT;

   /*

    * EBI 1, 2 and 3 regions

    *

@@ -360,6 +368,9 @@ void __core_init(void) {
                   TTE_SECT_EXE_NEVER |

                   TTE_SECT_S | TTE_TYPE_SECT;

 

+  /* Make a NO CACHE AREA */

+  MMU_MemorySection((mmuTable + ((uint32_t)NO_CACHE_MEMORY_START_ADDR >> 20)), NORMAL, NON_CACHEABLE, NON_CACHEABLE);

+

   /* Invalidate TLB and L1 I cache

      Enable caches and MMU.*/

   MMU_InvalidateTLB();

@@ -395,11 +406,17 @@ void __core_init(void) {
 

     /* Invalidate and enable L2 cache.*/

     L2C_InvAllByWay();

+    L2C_310->AUX_CNT = L2CC_ACR_DPEN | L2CC_ACR_IPEN;

+

+    /* Prefetch control register. Double linefeed, instr and data enabled.*/

+    *((uint32_t *)((char *)L2C_310+0x0F60)) = 0x75800001;

     L2C_Enable();

     __DSB();

     __ISB();

   }

 #endif

+

+

 #endif

 }

 

diff --git a/os/common/startup/ARMCAx-TZ/devices/SAMA5D2/mmu.h b/os/common/startup/ARMCAx-TZ/devices/SAMA5D2/mmu.h
index 95ef15854..4392fe222 100644
--- a/os/common/startup/ARMCAx-TZ/devices/SAMA5D2/mmu.h
+++ b/os/common/startup/ARMCAx-TZ/devices/SAMA5D2/mmu.h
@@ -31,7 +31,6 @@
 /*===========================================================================*/

 /* Module constants.                                                         */

 /*===========================================================================*/

-

 #define DCISW_INVALIDATE    0

 #define DCISW_CLEAN         1

 #define DCISW_CLEAN_AND_INV 2

@@ -44,6 +43,7 @@
 /* Derived constants and error checks.                                       */

 /*===========================================================================*/

 

+

 /*===========================================================================*/

 /* Module data structures and types.                                         */

 /*===========================================================================*/

diff --git a/os/common/startup/ARMCAx-TZ/devices/SAMA5D2/sama5d2x.h b/os/common/startup/ARMCAx-TZ/devices/SAMA5D2/sama5d2x.h
index 6aad3e035..1d711bf0b 100644
--- a/os/common/startup/ARMCAx-TZ/devices/SAMA5D2/sama5d2x.h
+++ b/os/common/startup/ARMCAx-TZ/devices/SAMA5D2/sama5d2x.h
@@ -64,8 +64,10 @@
 

 /**@} */

 

+#ifndef GBC_DISABLE_AES_REFERENCE

 /* TODO: to delete */

 #define Aes wc_Aes

+#endif

 

 /**

   * @brief SAMA5D2 Family