libc: add instrument api support

Add registration function instrumentation API,
which can achieve instrumentation of entering and
exiting functions through the compiler's functionality.

We can use CONFIG_ARCH_INSTRUMENT_ALL to add instrumentation for all
source, or add '-finstrument-functions' to CFLAGS for Part of the
source.

Notice:
1. use CONFIG_ARCH_INSTRUMENT_ALL must mark _start or entry noinstrument_function,
   becuase bss not set.
2. Make sure your callbacks are not instrumented recursively.

use instrument_register to register entry function and exit function.
They will be called by the instrumented function

Signed-off-by: anjiahao <anjiahao@xiaomi.com>

arch/Kconfig

@@ -335,6 +335,13 @@ config ARCH_COVERAGE_ALL
 		will get image size increased and performance decreased
 		significantly.
 
+config ARCH_INSTRUMENT_ALL
+	bool "Instrument All"
+	default n
+	---help---
+		Add instrument to all source files. we can use instrument_register
+		to register the instrument function.
+
 comment "Architecture Options"
 
 config ARCH_NOINTC

arch/arm/src/armv7-m/arm_stackcheck.c

@@ -48,6 +48,7 @@
 
 #include <stdint.h>
 
+#include <nuttx/instrument.h>
 #include "arm_internal.h"
 #include "nvic.h"
 
@@ -55,10 +56,19 @@
  * Private Functions
  ****************************************************************************/
 
-void __cyg_profile_func_enter(void *func, void *caller) naked_function;
-void __cyg_profile_func_exit(void *func, void *caller) naked_function;
+static void stack_check_enter(void *func, void *caller, void *arg)
+                              naked_function;
 void __stack_overflow_trap(void) naked_function;
 
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static struct instrument_s g_stack_check =
+{
+  .enter = stack_check_enter,
+};
+
 /****************************************************************************
  * Name: __stack_overflow_trap
  ****************************************************************************/
@@ -87,10 +97,10 @@ void __stack_overflow_trap(void)
 }
 
 /****************************************************************************
- * Name: __cyg_profile_func_enter
+ * Name: stack_check_enter
  ****************************************************************************/
 
-void __cyg_profile_func_enter(void *func, void *caller)
+static void stack_check_enter(void *func, void *caller, void *arg)
 {
     asm volatile (
             "   mrs r2, ipsr        \n" /* Check whether we are in interrupt mode */
@@ -112,12 +122,8 @@ void __cyg_profile_func_enter(void *func, void *caller)
     );
 }
 
-/****************************************************************************
- * Name: __cyg_profile_func_exit
- ****************************************************************************/
-
-void __cyg_profile_func_exit(void *func, void *caller)
+void noinstrument_function arm_stack_check_init(void)
 {
-    asm volatile("bx lr");
+  instrument_register(&g_stack_check);
 }
 #endif

arch/arm/src/armv8-m/arm_stackcheck.c

@@ -48,6 +48,7 @@
 
 #include <stdint.h>
 
+#include <nuttx/instrument.h>
 #include "arm_internal.h"
 #include "nvic.h"
 
@@ -55,10 +56,19 @@
  * Private Functions
  ****************************************************************************/
 
-void  __cyg_profile_func_enter(void *func, void *caller) naked_function;
-void  __cyg_profile_func_exit(void *func, void *caller) naked_function;
+static void stack_check_enter(void *func, void *caller, void *arg)
+                              naked_function;
 void __stack_overflow_trap(void) naked_function;
 
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static struct instrument_s g_stack_check =
+{
+  .enter = stack_check_enter,
+};
+
 /****************************************************************************
  * Name: __stack_overflow_trap
  ****************************************************************************/
@@ -87,10 +97,10 @@ void __stack_overflow_trap(void)
 }
 
 /****************************************************************************
- * Name: __cyg_profile_func_enter
+ * Name: stack_check_enter
  ****************************************************************************/
 
-void __cyg_profile_func_enter(void *func, void *caller)
+static void stack_check_enter(void *func, void *caller, void *arg)
 {
     asm volatile (
             "   mrs r2, ipsr        \n" /* Check whether we are in interrupt mode */
@@ -113,11 +123,11 @@ void __cyg_profile_func_enter(void *func, void *caller)
 }
 
 /****************************************************************************
- * Name: __cyg_profile_func_exit
+ * Name: arm_stack_check_init
  ****************************************************************************/
 
-void __cyg_profile_func_exit(void *func, void *caller)
+void noinstrument_function arm_stack_check_init(void)
 {
-    asm volatile("bx lr");
+  instrument_register(&g_stack_check);
 }
 #endif

arch/arm/src/cmake/armv7-a.cmake

@@ -65,4 +65,8 @@ else()
   list(APPEND PLATFORM_FLAGS -mfloat-abi=soft)
 endif()
 
+if(CONFIG_ARCH_INSTRUMENT_ALL AND NOT CONFIG_ARMV8M_STACKCHECK)
+  list(APPEND PLATFORM_FLAGS -finstrument-functions)
+endif()
+
 add_compile_options(${PLATFORM_FLAGS})

arch/arm/src/cmake/armv7-m.cmake

@@ -90,4 +90,8 @@ if(CONFIG_ARMV7M_STACKCHECK)
   list(APPEND PLATFORM_FLAGS -finstrument-functions -ffixed-r10)
 endif()
 
+if(CONFIG_ARCH_INSTRUMENT_ALL AND NOT CONFIG_ARMV7M_STACKCHECK)
+  list(APPEND PLATFORM_FLAGS -finstrument-functions)
+endif()
+
 add_compile_options(${PLATFORM_FLAGS})

arch/arm/src/cmake/armv8-m.cmake

@@ -65,6 +65,10 @@ if(CONFIG_ARMV8M_STACKCHECK)
   list(APPEND PLATFORM_FLAGS -finstrument-functions -ffixed-r10)
 endif()
 
+if(CONFIG_ARCH_INSTRUMENT_ALL AND NOT CONFIG_ARMV8M_STACKCHECK)
+  list(APPEND PLATFORM_FLAGS -finstrument-functions)
+endif()
+
 if(CONFIG_ARMV8M_CMSE)
   list(APPEND PLATFORM_FLAGS -mcmse)
 endif()

arch/arm/src/common/Toolchain.defs

@@ -86,6 +86,12 @@ ifeq ($(CONFIG_MM_KASAN_DISABLE_WRITES_CHECK),y)
   ARCHOPTIMIZATION += --param asan-instrument-writes=0
 endif
 
+# Instrumentation options
+
+ifeq ($(CONFIG_ARCH_INSTRUMENT_ALL),y)
+  ARCHOPTIMIZATION += -finstrument-functions
+endif
+
 ifeq ($(CONFIG_UNWINDER_ARM),y)
   ARCHOPTIMIZATION += -funwind-tables -fasynchronous-unwind-tables
 endif

arch/arm/src/common/arm_internal.h

@@ -531,6 +531,10 @@ int arm_gen_nonsecurefault(int irq, uint32_t *regs);
 # define arm_gen_nonsecurefault(i, r)  (0)
 #endif
 
+#if defined(CONFIG_ARMV7M_STACKCHECK) || defined(CONFIG_ARMV8M_STACKCHECK)
+void arm_stack_check_init(void) noinstrument_function;
+#endif
+
 #undef EXTERN
 #ifdef __cplusplus
 }

arch/arm/src/eoss3/eoss3_start.c

@@ -152,6 +152,10 @@ void __start(void)
 
   showprogress('C');
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
 #ifdef CONFIG_ARMV7M_ITMSYSLOG
   /* Perform ARMv7-M ITM SYSLOG initialization */
 

arch/arm/src/gd32f4/gd32f4xx_start.c

@@ -237,6 +237,10 @@ void __start(void)
       *dest++ = *src++;
     }
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
   /* Configure the UART so that we can get debug output as soon as possible */
 
   gd32_clockconfig();

arch/arm/src/imxrt/imxrt_start.c

@@ -210,6 +210,10 @@ void __start(void)
     }
 #endif
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
   /* Configure the UART so that we can get debug output as soon as possible */
 
   imxrt_clockconfig();

arch/arm/src/kinetis/kinetis_start.c

@@ -153,6 +153,10 @@ void __start(void)
     }
 #endif
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
   /* Perform clock and Kinetis module initialization (This depends on
    * RAM functions having been copied to RAM).
    */

arch/arm/src/lpc17xx_40xx/lpc17_40_start.c

@@ -153,6 +153,10 @@ void __start(void)
 
   showprogress('C');
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
   /* Perform early serial initialization */
 
 #ifdef USE_EARLYSERIALINIT

arch/arm/src/nrf52/nrf52_start.c

@@ -188,6 +188,10 @@ void __start(void)
 
   showprogress('C');
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
 #if defined(CONFIG_ARCH_CHIP_NRF52832)
   /* Initialize the errdata work-around */
 

arch/arm/src/nrf53/nrf53_start.c

@@ -236,6 +236,10 @@ void __start(void)
 
   showprogress('C');
 
+#ifdef CONFIG_ARMV8M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
 #ifdef CONFIG_ARCH_HAVE_FPU
   /* Initialize the FPU (if available) */
 

arch/arm/src/rtl8720c/ameba_start.c

@@ -77,6 +77,11 @@ int promisc_recv_lens_func(void *padapter, uint8_t *payload, uint8_t plen)
 void app_start(void)
 {
   __asm volatile("MSR msplim, %0" : : "r"(0));
+
+#ifdef CONFIG_ARMV8M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
   arm_earlyserialinit();
 #ifdef CONFIG_MBEDTLS240_AMEBAZ_HARDWARE_CRYPTO
   extern int mbedtls_platform_set_calloc_free(

arch/arm/src/sam34/sam_start.c

@@ -164,6 +164,10 @@ void __start(void)
     }
 #endif
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
   /* Configure the UART so that we can get debug output as soon as possible */
 
   sam_clockconfig();

arch/arm/src/samd5e5/sam_start.c

@@ -167,6 +167,10 @@ void __start(void)
     }
 #endif
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
   /* Initialize clocking and the FPU.  Configure the console UART so that
    * we can get debug output as soon as possible.
    */

arch/arm/src/samv7/sam_start.c

@@ -199,6 +199,10 @@ void __start(void)
     }
 #endif
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
   /* Configure the UART so that we can get debug output as soon as possible */
 
   sam_clockconfig();

arch/arm/src/stm32/stm32_start.c

@@ -158,6 +158,10 @@ void __start(void)
 
   showprogress('C');
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
 #ifdef CONFIG_ARCH_PERF_EVENTS
   up_perf_init((void *)STM32_SYSCLK_FREQUENCY);
 #endif

arch/arm/src/stm32f7/stm32_start.c

@@ -223,6 +223,10 @@ void __start(void)
     }
 #endif
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
   /* Configure the UART so that we can get debug output as soon as possible */
 
   stm32_clockconfig();

arch/arm/src/stm32h7/stm32_start.c

@@ -239,6 +239,10 @@ void __start(void)
     }
 #endif
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
   /* Configure the UART so that we can get debug output as soon as possible */
 
   stm32_clockconfig();

arch/arm/src/stm32l4/stm32l4_start.c

@@ -179,6 +179,10 @@ void __start(void)
 
   showprogress('C');
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
 #ifdef CONFIG_ARCH_PERF_EVENTS
   up_perf_init((void *)STM32_SYSCLK_FREQUENCY);
 #endif

arch/arm/src/stm32l5/stm32l5_start.c

@@ -181,6 +181,10 @@ void __start(void)
 
   showprogress('C');
 
+#ifdef CONFIG_ARMV8M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
 #ifdef CONFIG_ARCH_PERF_EVENTS
   up_perf_init((void *)STM32_SYSCLK_FREQUENCY);
 #endif

arch/arm/src/stm32u5/stm32_start.c

@@ -181,6 +181,10 @@ void __start(void)
 
   showprogress('C');
 
+#ifdef CONFIG_ARMV8M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
 #ifdef CONFIG_ARCH_PERF_EVENTS
   up_perf_init((void *)STM32_SYSCLK_FREQUENCY);
 #endif

arch/arm/src/stm32wb/stm32wb_start.c

@@ -199,6 +199,10 @@ void __start(void)
 
   showprogress('C');
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
 #ifdef CONFIG_ARCH_PERF_EVENTS
   up_perf_init((void *)STM32_SYSCLK_FREQUENCY);
 #endif

arch/arm/src/xmc4/xmc4_start.c

@@ -220,6 +220,10 @@ void __start(void)
     }
 #endif
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
   /* Set FLASH wait states prior to the configuration of clocking */
 
   xmc4_flash_waitstates();

arch/arm64/src/Toolchain.defs

@@ -84,6 +84,12 @@ ifeq ($(CONFIG_MM_KASAN_ALL),y)
   ARCHOPTIMIZATION += -fsanitize=kernel-address
 endif
 
+# Instrumentation options
+
+ifeq ($(CONFIG_ARCH_INSTRUMENT_ALL),y)
+  ARCHOPTIMIZATION += -finstrument-functions
+endif
+
 ifeq ($(CONFIG_ARCH_FPU),y)
   ARCHCXXFLAGS += -D_LDBL_EQ_DBL
   ARCHCFLAGS   += -D_LDBL_EQ_DBL

arch/risc-v/src/common/Toolchain.defs

@@ -251,6 +251,12 @@ ifeq ($(CONFIG_MM_UBSAN_TRAP_ON_ERROR),y)
   ARCHOPTIMIZATION += -fsanitize-undefined-trap-on-error
 endif
 
+# Instrumentation options
+
+ifeq ($(CONFIG_ARCH_INSTRUMENT_ALL),y)
+  ARCHOPTIMIZATION += -finstrument-functions
+endif
+
 # Default toolchain
 
 CC      = $(CROSSDEV)gcc

arch/xtensa/src/lx6/Toolchain.defs

@@ -95,6 +95,12 @@ ifeq ($(CONFIG_ARCH_COVERAGE_ALL),y)
   ARCHOPTIMIZATION += -fprofile-generate -ftest-coverage
 endif
 
+# Instrumentation options
+
+ifeq ($(CONFIG_ARCH_INSTRUMENT_ALL),y)
+  ARCHOPTIMIZATION += -finstrument-functions
+endif
+
 ARCHCFLAGS += -fno-common
 ARCHCXXFLAGS += -fno-common -nostdinc++
 

arch/xtensa/src/lx7/Toolchain.defs

@@ -99,6 +99,12 @@ ifeq ($(CONFIG_ARCH_COVERAGE_ALL),y)
   ARCHOPTIMIZATION += -fprofile-generate -ftest-coverage
 endif
 
+# Instrumentation options
+
+ifeq ($(CONFIG_ARCH_INSTRUMENT_ALL),y)
+  ARCHOPTIMIZATION += -finstrument-functions
+endif
+
 ARCHCFLAGS += -fno-common
 ARCHCXXFLAGS += -fno-common -nostdinc++
 

boards/sim/sim/sim/scripts/Make.defs

@@ -95,6 +95,10 @@ else
   endif
 endif
 
+ifeq ($(CONFIG_ARCH_INSTRUMENT_ALL),y)
+  ARCHOPTIMIZATION += -finstrument-functions
+endif
+
 # Add -fno-common because macOS "ld -r" doesn't seem to pick objects
 # for common symbols.
 ARCHCFLAGS += -fno-common

drivers/note/note_driver.c

@@ -40,9 +40,14 @@
 #include <nuttx/note/notelog_driver.h>
 #include <nuttx/spinlock.h>
 #include <nuttx/sched_note.h>
+#include <nuttx/instrument.h>
 
 #include "sched/sched.h"
 
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
 #if defined(CONFIG_DRIVERS_NOTERAM) +  defined(CONFIG_DRIVERS_NOTELOG) + \
     defined(CONFIG_DRIVERS_NOTESNAP) + defined(CONFIG_DRIVERS_NOTERTT) + \
     defined(CONFIG_SEGGER_SYSVIEW) > CONFIG_DRIVERS_NOTE_MAX
@@ -150,6 +155,18 @@ struct note_taskname_s
  * Private Data
  ****************************************************************************/
 
+#ifdef CONFIG_SCHED_INSTRUMENTATION_FUNCTION
+static void note_driver_instrument_enter(FAR void *this_fn,
+            FAR void *call_site, FAR void *arg) noinstrument_function;
+static void note_driver_instrument_leave(FAR void *this_fn,
+            FAR void *call_site, FAR void *arg) noinstrument_function;
+static struct instrument_s g_note_instrument =
+{
+  .entry = note_driver_instrument_enter,
+  .exit = note_driver_instrument_leave,
+};
+#endif
+
 #ifdef CONFIG_SCHED_INSTRUMENTATION_FILTER
 static struct note_filter_s g_note_filter =
 {
@@ -1924,6 +1941,22 @@ FAR const char *note_get_taskname(pid_t pid)
 
 #endif
 
+#ifdef CONFIG_SCHED_INSTRUMENTATION_FUNCTION
+static void note_driver_instrument_enter(FAR void *this_fn,
+                                         FAR void *call_site,
+                                         FAR void *arg)
+{
+  sched_note_string_ip(NOTE_TAG_ALWAYS, (uintptr_t)this_fn, "B");
+}
+
+static void note_driver_instrument_leave(FAR void *this_fn,
+                                         FAR void *call_site,
+                                         FAR void *arg)
+{
+  sched_note_string_ip(NOTE_TAG_ALWAYS, (uintptr_t)this_fn, "E");
+}
+#endif
+
 /****************************************************************************
  * Name: note_driver_register
  ****************************************************************************/
@@ -1931,8 +1964,17 @@ FAR const char *note_get_taskname(pid_t pid)
 int note_driver_register(FAR struct note_driver_s *driver)
 {
   int i;
-  DEBUGASSERT(driver);
+#ifdef CONFIG_SCHED_INSTRUMENTATION_FUNCTION
+  static bool initialized;
 
+  if (!initialized)
+    {
+      instrument_register(g_note_instrument)
+      initialized = true;
+    }
+#endif
+
+  DEBUGASSERT(driver);
   for (i = 0; i < CONFIG_DRIVERS_NOTE_MAX; i++)
     {
       if (g_note_drivers[i] == NULL)
@@ -1945,25 +1987,3 @@ int note_driver_register(FAR struct note_driver_s *driver)
   return -ENOMEM;
 }
 
-#ifdef CONFIG_SCHED_INSTRUMENTATION_FUNCTION
-
-/****************************************************************************
- * Name: __cyg_profile_func_enter
- ****************************************************************************/
-
-void noinstrument_function
-__cyg_profile_func_enter(void *this_fn, void *call_site)
-{
-  sched_note_string_ip(NOTE_TAG_ALWAYS, (uintptr_t)this_fn, "B");
-}
-
-/****************************************************************************
- * Name: __cyg_profile_func_exit
- ****************************************************************************/
-
-void noinstrument_function
-__cyg_profile_func_exit(void *this_fn, void *call_site)
-{
-  sched_note_string_ip(NOTE_TAG_ALWAYS, (uintptr_t)this_fn, "E");
-}
-#endif

include/nuttx/instrument.h

@@ -0,0 +1,68 @@
+/****************************************************************************
+ * include/nuttx/instrument.h
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you 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.
+ *
+ ****************************************************************************/
+
+#ifndef __INCLUDE_NUTTX_INSTRUMENT_H
+#define __INCLUDE_NUTTX_INSTRUMENT_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/compiler.h>
+#include <nuttx/queue.h>
+
+/****************************************************************************
+ * Public Types
+ ****************************************************************************/
+
+typedef CODE void (instrument_fun_t)(FAR void *this_fn,
+                                     FAR void *call_site,
+                                     FAR void *arg);
+
+struct instrument_s
+{
+  sq_entry_t entry;
+  FAR instrument_fun_t *enter;
+  FAR instrument_fun_t *leave;
+  FAR void *arg;
+};
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: instrument_register
+ *
+ * Description: register instrument, it will be called
+ *              when function enter or exit.
+ *
+ * Input Parameters:
+ *   entry - instrument entry structure.
+ * Notice:
+ *  use CONFIG_ARCH_INSTRUMENT_ALL must mark _start or entry
+ *  noinstrument_function, becuase bss not set.
+ *  Make sure your callbacks are not instrumented recursively.
+ *
+ ****************************************************************************/
+
+void instrument_register(FAR struct instrument_s *entry);
+
+#endif /* __INCLUDE_NUTTX_INSTRUMENT_H */

libs/libc/misc/CMakeLists.txt

@@ -79,7 +79,8 @@ list(
   lib_glob.c
   lib_execinfo.c
   lib_ftok.c
-  lib_err.c)
+  lib_err.c
+  lib_instrument.c)
 
 # Keyboard driver encoder/decoder
 

libs/libc/misc/Make.defs

@@ -40,7 +40,7 @@ endif
 CSRCS += lib_dumpbuffer.c lib_dumpvbuffer.c lib_fnmatch.c lib_debug.c
 CSRCS += lib_crc64.c lib_crc32.c lib_crc16.c lib_crc16ccitt.c lib_crc8.c
 CSRCS += lib_crc8ccitt.c lib_crc8table.c lib_glob.c lib_execinfo.c
-CSRCS += lib_ftok.c lib_err.c
+CSRCS += lib_ftok.c lib_err.c lib_instrument.c
 
 # Keyboard driver encoder/decoder
 

libs/libc/misc/lib_instrument.c

@@ -0,0 +1,121 @@
+/****************************************************************************
+ * libs/libc/misc/lib_instrument.c
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you 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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/instrument.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Avoid instrument bootstrap */
+
+#define MAIGC_NUMBMER 0x5a5a5a5a
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/* Use static to avoid instrument bootstrap */
+
+static volatile uint32_t g_magic;
+static sq_queue_t g_instrument_queue;
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: __cyg_profile_func_enter
+ ****************************************************************************/
+
+void noinstrument_function
+__cyg_profile_func_enter(FAR void *this_fn, FAR void *call_site)
+{
+  FAR struct instrument_s *instrument;
+  FAR sq_entry_t *entry;
+
+  if (g_magic != MAIGC_NUMBMER)
+    {
+      return;
+    }
+
+  sq_for_every(&g_instrument_queue, entry)
+    {
+      instrument = (FAR struct instrument_s *)entry;
+      if (instrument->enter)
+        {
+          instrument->enter(this_fn, call_site, instrument->arg);
+        }
+    }
+}
+
+/****************************************************************************
+ * Name: __cyg_profile_func_exit
+ ****************************************************************************/
+
+void noinstrument_function
+__cyg_profile_func_exit(FAR void *this_fn, FAR void *call_site)
+{
+  FAR struct instrument_s *instrument;
+  FAR sq_entry_t *entry;
+
+  if (g_magic != MAIGC_NUMBMER)
+    {
+      return;
+    }
+
+  sq_for_every(&g_instrument_queue, entry)
+    {
+      instrument = (FAR struct instrument_s *)entry;
+      if (instrument->leave)
+        {
+          instrument->leave(this_fn, call_site, instrument->arg);
+        }
+    }
+}
+
+/****************************************************************************
+ * Name: instrument_register
+ *
+ * Description: register instrument, it will be called
+ *              when function enter or exit.
+ *
+ * Input Parameters:
+ *   entry - instrument entry structure.
+ * Notice:
+ *  use CONFIG_ARCH_INSTRUMENT_ALL must mark _start or entry
+ *  noinstrument_function, becuase bss not set.
+ *  Make sure your callbacks are not instrumented recursively.
+ *
+ ****************************************************************************/
+
+void noinstrument_function
+instrument_register(FAR struct instrument_s *entry)
+{
+  if (entry != NULL)
+    {
+      sq_addlast((FAR sq_entry_t *)entry, &g_instrument_queue);
+      g_magic = MAIGC_NUMBMER;
+    }
+}