From b3555769e73225d3b16da800e6cbba1f849461a7 Mon Sep 17 00:00:00 2001
From: Giovanni Di Sirio <gdisirio@gmail.com>
Date: Tue, 21 Mar 2023 10:34:22 +0000
Subject: [PATCH] Tentative fixes for RT statistics.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@16175 27425a3e-05d8-49a3-a47f-9c15f0e5edd8
---
 os/common/ports/ARMv7-M-ALT/chcore.c | 3 +++
 os/rt/src/chinstances.c              | 5 +++++
 2 files changed, 8 insertions(+)

diff --git a/os/common/ports/ARMv7-M-ALT/chcore.c b/os/common/ports/ARMv7-M-ALT/chcore.c
index 02ac56a00..5e8c4a6c2 100644
--- a/os/common/ports/ARMv7-M-ALT/chcore.c
+++ b/os/common/ports/ARMv7-M-ALT/chcore.c
@@ -123,6 +123,9 @@ uint64_t __port_schedule_next(void) {
     mpuSetRegionAddress(PORT_USE_GUARD_MPU_REGION, ntp->wabase);
 #endif
 
+    __trace_switch(ntp, otp);
+    __stats_ctxswc(ntp, otp);
+
     return ((uint64_t)(uint32_t)otp << 32) | ((uint64_t)(uint32_t)ntp << 0);
   }
 
diff --git a/os/rt/src/chinstances.c b/os/rt/src/chinstances.c
index eff561d2d..31a351d3f 100644
--- a/os/rt/src/chinstances.c
+++ b/os/rt/src/chinstances.c
@@ -160,6 +160,11 @@ void chInstanceObjectInit(os_instance_t *oip,
   /* Setting up the caller as current thread.*/
   oip->rlist.current->state = CH_STATE_CURRENT;
 
+#if CH_DBG_STATISTICS == TRUE
+  /* Starting measurement for this thread.*/
+  chTMStartMeasurementX(&oip->rlist.current->stats);
+#endif
+
   /* User instance initialization hook.*/
   CH_CFG_OS_INSTANCE_INIT_HOOK(oip);