Updated SAMA drivers (still incomplete)

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

4 files changed, 266 insertions, 37 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