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>
diff --git a/arch/Kconfig b/arch/Kconfig
index fd0103c..deccaf4 100644
--- a/arch/Kconfig
+++ b/arch/Kconfig
@@ -335,6 +335,13 @@
 		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
diff --git a/arch/arm/src/armv7-m/arm_stackcheck.c b/arch/arm/src/armv7-m/arm_stackcheck.c
index d7167a9..0dee260 100644
--- a/arch/arm/src/armv7-m/arm_stackcheck.c
+++ b/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,11 +56,20 @@
  * 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 @@
 }
 
 /****************************************************************************
- * 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 @@
     );
 }
 
-/****************************************************************************
- * 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
diff --git a/arch/arm/src/armv8-m/arm_stackcheck.c b/arch/arm/src/armv8-m/arm_stackcheck.c
index cbe107c..1d05fdd 100644
--- a/arch/arm/src/armv8-m/arm_stackcheck.c
+++ b/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,9 +56,18 @@
  * Private Functions
  ****************************************************************************/
 
-void  __cyg_profile_func_enter(void *func, void *caller) naked_function;
-void  __cyg_profile_func_exit(void *func, void *caller) naked_function;
-void  __stack_overflow_trap(void) 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 @@
 }
 
 /****************************************************************************
- * 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 @@
 }
 
 /****************************************************************************
- * 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
diff --git a/arch/arm/src/cmake/armv7-a.cmake b/arch/arm/src/cmake/armv7-a.cmake
index 8566b54..58a08fb 100644
--- a/arch/arm/src/cmake/armv7-a.cmake
+++ b/arch/arm/src/cmake/armv7-a.cmake
@@ -65,4 +65,8 @@
   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})
diff --git a/arch/arm/src/cmake/armv7-m.cmake b/arch/arm/src/cmake/armv7-m.cmake
index 74dcd06..f04f837 100644
--- a/arch/arm/src/cmake/armv7-m.cmake
+++ b/arch/arm/src/cmake/armv7-m.cmake
@@ -90,4 +90,8 @@
   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})
diff --git a/arch/arm/src/cmake/armv8-m.cmake b/arch/arm/src/cmake/armv8-m.cmake
index 5b1adf0..514006f 100644
--- a/arch/arm/src/cmake/armv8-m.cmake
+++ b/arch/arm/src/cmake/armv8-m.cmake
@@ -65,6 +65,10 @@
   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()
diff --git a/arch/arm/src/common/Toolchain.defs b/arch/arm/src/common/Toolchain.defs
index 991ce31..1e0184a 100644
--- a/arch/arm/src/common/Toolchain.defs
+++ b/arch/arm/src/common/Toolchain.defs
@@ -86,6 +86,12 @@
   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
diff --git a/arch/arm/src/common/arm_internal.h b/arch/arm/src/common/arm_internal.h
index 40b4b40..271aa05 100644
--- a/arch/arm/src/common/arm_internal.h
+++ b/arch/arm/src/common/arm_internal.h
@@ -531,6 +531,10 @@
 # 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
 }
diff --git a/arch/arm/src/eoss3/eoss3_start.c b/arch/arm/src/eoss3/eoss3_start.c
index b1bcd96..560a3fe 100644
--- a/arch/arm/src/eoss3/eoss3_start.c
+++ b/arch/arm/src/eoss3/eoss3_start.c
@@ -152,6 +152,10 @@
 
   showprogress('C');
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
 #ifdef CONFIG_ARMV7M_ITMSYSLOG
   /* Perform ARMv7-M ITM SYSLOG initialization */
 
diff --git a/arch/arm/src/gd32f4/gd32f4xx_start.c b/arch/arm/src/gd32f4/gd32f4xx_start.c
index 0ea5e0a..70a09d4 100644
--- a/arch/arm/src/gd32f4/gd32f4xx_start.c
+++ b/arch/arm/src/gd32f4/gd32f4xx_start.c
@@ -237,6 +237,10 @@
       *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();
diff --git a/arch/arm/src/imxrt/imxrt_start.c b/arch/arm/src/imxrt/imxrt_start.c
index b5aa9c7..cc51d70 100644
--- a/arch/arm/src/imxrt/imxrt_start.c
+++ b/arch/arm/src/imxrt/imxrt_start.c
@@ -210,6 +210,10 @@
     }
 #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();
diff --git a/arch/arm/src/kinetis/kinetis_start.c b/arch/arm/src/kinetis/kinetis_start.c
index 085adf9..c46abe0 100644
--- a/arch/arm/src/kinetis/kinetis_start.c
+++ b/arch/arm/src/kinetis/kinetis_start.c
@@ -153,6 +153,10 @@
     }
 #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).
    */
diff --git a/arch/arm/src/lpc17xx_40xx/lpc17_40_start.c b/arch/arm/src/lpc17xx_40xx/lpc17_40_start.c
index 0fb65df..eee92a2 100644
--- a/arch/arm/src/lpc17xx_40xx/lpc17_40_start.c
+++ b/arch/arm/src/lpc17xx_40xx/lpc17_40_start.c
@@ -153,6 +153,10 @@
 
   showprogress('C');
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
   /* Perform early serial initialization */
 
 #ifdef USE_EARLYSERIALINIT
diff --git a/arch/arm/src/nrf52/nrf52_start.c b/arch/arm/src/nrf52/nrf52_start.c
index 2e3bf74..bc34a93 100644
--- a/arch/arm/src/nrf52/nrf52_start.c
+++ b/arch/arm/src/nrf52/nrf52_start.c
@@ -188,6 +188,10 @@
 
   showprogress('C');
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
 #if defined(CONFIG_ARCH_CHIP_NRF52832)
   /* Initialize the errdata work-around */
 
diff --git a/arch/arm/src/nrf53/nrf53_start.c b/arch/arm/src/nrf53/nrf53_start.c
index 07458a7..b58c911 100644
--- a/arch/arm/src/nrf53/nrf53_start.c
+++ b/arch/arm/src/nrf53/nrf53_start.c
@@ -236,6 +236,10 @@
 
   showprogress('C');
 
+#ifdef CONFIG_ARMV8M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
 #ifdef CONFIG_ARCH_HAVE_FPU
   /* Initialize the FPU (if available) */
 
diff --git a/arch/arm/src/rtl8720c/ameba_start.c b/arch/arm/src/rtl8720c/ameba_start.c
index 3cafd02..df0cd4a 100644
--- a/arch/arm/src/rtl8720c/ameba_start.c
+++ b/arch/arm/src/rtl8720c/ameba_start.c
@@ -77,6 +77,11 @@
 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(
diff --git a/arch/arm/src/sam34/sam_start.c b/arch/arm/src/sam34/sam_start.c
index 553f665..dac801e 100644
--- a/arch/arm/src/sam34/sam_start.c
+++ b/arch/arm/src/sam34/sam_start.c
@@ -164,6 +164,10 @@
     }
 #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();
diff --git a/arch/arm/src/samd5e5/sam_start.c b/arch/arm/src/samd5e5/sam_start.c
index 689108d..443d0a7 100644
--- a/arch/arm/src/samd5e5/sam_start.c
+++ b/arch/arm/src/samd5e5/sam_start.c
@@ -167,6 +167,10 @@
     }
 #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.
    */
diff --git a/arch/arm/src/samv7/sam_start.c b/arch/arm/src/samv7/sam_start.c
index e2c4f1f..a6c1c61 100644
--- a/arch/arm/src/samv7/sam_start.c
+++ b/arch/arm/src/samv7/sam_start.c
@@ -199,6 +199,10 @@
     }
 #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();
diff --git a/arch/arm/src/stm32/stm32_start.c b/arch/arm/src/stm32/stm32_start.c
index c369d1c..973c752 100644
--- a/arch/arm/src/stm32/stm32_start.c
+++ b/arch/arm/src/stm32/stm32_start.c
@@ -158,6 +158,10 @@
 
   showprogress('C');
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
 #ifdef CONFIG_ARCH_PERF_EVENTS
   up_perf_init((void *)STM32_SYSCLK_FREQUENCY);
 #endif
diff --git a/arch/arm/src/stm32f7/stm32_start.c b/arch/arm/src/stm32f7/stm32_start.c
index e333385..a37e63e 100644
--- a/arch/arm/src/stm32f7/stm32_start.c
+++ b/arch/arm/src/stm32f7/stm32_start.c
@@ -223,6 +223,10 @@
     }
 #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();
diff --git a/arch/arm/src/stm32h7/stm32_start.c b/arch/arm/src/stm32h7/stm32_start.c
index 9959d49..7bd4013 100644
--- a/arch/arm/src/stm32h7/stm32_start.c
+++ b/arch/arm/src/stm32h7/stm32_start.c
@@ -239,6 +239,10 @@
     }
 #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();
diff --git a/arch/arm/src/stm32l4/stm32l4_start.c b/arch/arm/src/stm32l4/stm32l4_start.c
index c95e6e1..7b5b9a7 100644
--- a/arch/arm/src/stm32l4/stm32l4_start.c
+++ b/arch/arm/src/stm32l4/stm32l4_start.c
@@ -179,6 +179,10 @@
 
   showprogress('C');
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
 #ifdef CONFIG_ARCH_PERF_EVENTS
   up_perf_init((void *)STM32_SYSCLK_FREQUENCY);
 #endif
diff --git a/arch/arm/src/stm32l5/stm32l5_start.c b/arch/arm/src/stm32l5/stm32l5_start.c
index 1f09ed1..47efcf5 100644
--- a/arch/arm/src/stm32l5/stm32l5_start.c
+++ b/arch/arm/src/stm32l5/stm32l5_start.c
@@ -181,6 +181,10 @@
 
   showprogress('C');
 
+#ifdef CONFIG_ARMV8M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
 #ifdef CONFIG_ARCH_PERF_EVENTS
   up_perf_init((void *)STM32_SYSCLK_FREQUENCY);
 #endif
diff --git a/arch/arm/src/stm32u5/stm32_start.c b/arch/arm/src/stm32u5/stm32_start.c
index cbb0c64..2f45723 100644
--- a/arch/arm/src/stm32u5/stm32_start.c
+++ b/arch/arm/src/stm32u5/stm32_start.c
@@ -181,6 +181,10 @@
 
   showprogress('C');
 
+#ifdef CONFIG_ARMV8M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
 #ifdef CONFIG_ARCH_PERF_EVENTS
   up_perf_init((void *)STM32_SYSCLK_FREQUENCY);
 #endif
diff --git a/arch/arm/src/stm32wb/stm32wb_start.c b/arch/arm/src/stm32wb/stm32wb_start.c
index 6ab5605..2f829f8 100644
--- a/arch/arm/src/stm32wb/stm32wb_start.c
+++ b/arch/arm/src/stm32wb/stm32wb_start.c
@@ -199,6 +199,10 @@
 
   showprogress('C');
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
 #ifdef CONFIG_ARCH_PERF_EVENTS
   up_perf_init((void *)STM32_SYSCLK_FREQUENCY);
 #endif
diff --git a/arch/arm/src/xmc4/xmc4_start.c b/arch/arm/src/xmc4/xmc4_start.c
index 4e55e7e..b40f550 100644
--- a/arch/arm/src/xmc4/xmc4_start.c
+++ b/arch/arm/src/xmc4/xmc4_start.c
@@ -220,6 +220,10 @@
     }
 #endif
 
+#ifdef CONFIG_ARMV7M_STACKCHECK
+  arm_stack_check_init();
+#endif
+
   /* Set FLASH wait states prior to the configuration of clocking */
 
   xmc4_flash_waitstates();
diff --git a/arch/arm64/src/Toolchain.defs b/arch/arm64/src/Toolchain.defs
index efebeb5..214b481 100644
--- a/arch/arm64/src/Toolchain.defs
+++ b/arch/arm64/src/Toolchain.defs
@@ -84,6 +84,12 @@
   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
diff --git a/arch/risc-v/src/common/Toolchain.defs b/arch/risc-v/src/common/Toolchain.defs
index 61d7f27..3d0e529 100644
--- a/arch/risc-v/src/common/Toolchain.defs
+++ b/arch/risc-v/src/common/Toolchain.defs
@@ -251,6 +251,12 @@
   ARCHOPTIMIZATION += -fsanitize-undefined-trap-on-error
 endif
 
+# Instrumentation options
+
+ifeq ($(CONFIG_ARCH_INSTRUMENT_ALL),y)
+  ARCHOPTIMIZATION += -finstrument-functions
+endif
+
 # Default toolchain
 
 CC      = $(CROSSDEV)gcc
diff --git a/arch/xtensa/src/lx6/Toolchain.defs b/arch/xtensa/src/lx6/Toolchain.defs
index 1be4627..686dc70 100644
--- a/arch/xtensa/src/lx6/Toolchain.defs
+++ b/arch/xtensa/src/lx6/Toolchain.defs
@@ -95,6 +95,12 @@
   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++
 
diff --git a/arch/xtensa/src/lx7/Toolchain.defs b/arch/xtensa/src/lx7/Toolchain.defs
index 710b196..6d5da90 100644
--- a/arch/xtensa/src/lx7/Toolchain.defs
+++ b/arch/xtensa/src/lx7/Toolchain.defs
@@ -99,6 +99,12 @@
   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++
 
diff --git a/boards/sim/sim/sim/scripts/Make.defs b/boards/sim/sim/sim/scripts/Make.defs
index 6c9a77b..efc9dd9 100644
--- a/boards/sim/sim/sim/scripts/Make.defs
+++ b/boards/sim/sim/sim/scripts/Make.defs
@@ -95,6 +95,10 @@
   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
diff --git a/drivers/note/note_driver.c b/drivers/note/note_driver.c
index ae1c979..758f1c5 100644
--- a/drivers/note/note_driver.c
+++ b/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 @@
  * 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 @@
 
 #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 @@
 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 @@
   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
diff --git a/include/nuttx/instrument.h b/include/nuttx/instrument.h
new file mode 100644
index 0000000..c39b946
--- /dev/null
+++ b/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 */
diff --git a/libs/libc/misc/CMakeLists.txt b/libs/libc/misc/CMakeLists.txt
index e24d3ef..dbbf69c 100644
--- a/libs/libc/misc/CMakeLists.txt
+++ b/libs/libc/misc/CMakeLists.txt
@@ -79,7 +79,8 @@
   lib_glob.c
   lib_execinfo.c
   lib_ftok.c
-  lib_err.c)
+  lib_err.c
+  lib_instrument.c)
 
 # Keyboard driver encoder/decoder
 
diff --git a/libs/libc/misc/Make.defs b/libs/libc/misc/Make.defs
index 5e71c06..605779e 100644
--- a/libs/libc/misc/Make.defs
+++ b/libs/libc/misc/Make.defs
@@ -40,7 +40,7 @@
 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
 
diff --git a/libs/libc/misc/lib_instrument.c b/libs/libc/misc/lib_instrument.c
new file mode 100644
index 0000000..e16af3b
--- /dev/null
+++ b/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;
+    }
+}