diff --git a/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-NSEC/smc.S b/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-NSEC/smc.S
new file mode 100644
index 000000000..8452dc190
--- /dev/null
+++ b/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-NSEC/smc.S
@@ -0,0 +1,42 @@
+/*
+    ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    smcclient.S
+ * @brief   SMC client module code.
+ *
+ * @addtogroup SMC
+ * @{
+ */
+                .section .text
+                .code   32
+                .balign 4
+/*
+ * r0 smc_service_t     handle
+ * r1 smc_params_area_t data
+ * r2 size_t            size
+ *
+ * r0 msg_t             result
+ */
+                .global smcInvoke
+smcInvoke:
+                smc     #0
+                bx      lr
+/** @} */
+
diff --git a/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-NSEC/smcclient.c b/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-NSEC/smcclient.c
index 0cf00e217..8b9f0edeb 100644
--- a/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-NSEC/smcclient.c
+++ b/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED-NSEC/smcclient.c
@@ -1,5 +1,5 @@
 /*
-    ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio.
+    ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio.
 
     This file is part of ChibiOS.
 
@@ -28,6 +28,8 @@
 #include "ch.h"
 #include "smcclient.h"
 
+msg_t smcInvoke(smc_service_t handle, smc_params_area_t data,
+                       size_t size);
 /*===========================================================================*/
 /* Module local definitions.                                                 */
 /*===========================================================================*/
@@ -57,7 +59,7 @@ msg_t smcInvokeService(smc_service_t handle, smc_params_area_t data,
   msg_t result;
 
   do {
-    asm volatile("smc #0" : "=r" (result) : "r" (handle), "r" (data), "r" (size));
+    result = smcInvoke(handle, data, size);
   } while (result == MSG_TIMEOUT);
   return result;
 }