diff --git a/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-SEC/Makefile b/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-SEC/Makefile
index 570dfbd1f..12a653790 100755
--- a/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-SEC/Makefile
+++ b/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-SEC/Makefile
@@ -5,7 +5,7 @@
 
 # Compiler options here.
 ifeq ($(USE_OPT),)
-  USE_OPT = -O0 -ggdb -fomit-frame-pointer -falign-functions=16
+  USE_OPT = -Og -ggdb -fomit-frame-pointer -falign-functions=16
 endif
 
 # C specific options here (added to USE_OPT).
diff --git a/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-SEC/chconf.h b/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-SEC/chconf.h
index 8093b6f33..2e08d695b 100755
--- a/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-SEC/chconf.h
+++ b/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-SEC/chconf.h
@@ -603,13 +603,6 @@
 /* Port-specific settings (override port settings defaulted in chcore.h).    */
 /*===========================================================================*/
 
-/**
- * @brief   Trust zone configuration.
- * @details If enabled the kernel is configured for the secure world
- *          and can access specific devices.
- */
-#define CH_CFG_SEC_WORLD                    TRUE
-
 #endif  /* CHCONF_H */
 
 /** @} */
diff --git a/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-SEC/main.c b/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-SEC/main.c
index 25218594f..0d04b0a17 100755
--- a/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-SEC/main.c
+++ b/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-SEC/main.c
@@ -52,7 +52,7 @@ static const SerialConfig sdcfg = {
   UART_MR_PAR_NO
 };
 
-/*
+/**
  *  Dummy trust service thread.
  */
 static THD_WORKING_AREA(waDummyTrustedService, 512);
@@ -63,20 +63,16 @@ static THD_FUNCTION(DummyTrustedService, arg) {
   smc_service_t *svcp;
   chRegSetThreadName("DTS");
 
-  /*
-   * Register the trust service
-   */
+  /* Register the trust service.*/
   svcp = smcRegisterMeAsService("DummyTrustedService");
   if (svcp == NULL) {
-    /*
-     * Error: the service is already registered
-     * or memory is exhausted.
-     */
+
+    /* Error: the service is already registered
+     * or memory is exhausted.*/
     return;
   }
-  /*
-   * Wait and process requests
-   */
+
+  /* Wait and process requests.*/
   while (true) {
     msg = smcServiceWaitRequest(svcp, MSG_OK);
     if (msg == MSG_OK && svcp->svc_datalen > 0) {
@@ -186,7 +182,7 @@ int main(void) {
    * the secure world.
    */
   chprintf((BaseSequentialStream*)&SD1, "Jumping in the non secure world\n\r");
-  _ns_trampoline(NSEC_MEMORY_START);
+  _ns_trampoline(NSEC_IMAGE_START_ADDR);
   /*
    * It never goes here
    */