From 2f1bc6676e4ea4e5ef042457e50f487fad74c397 Mon Sep 17 00:00:00 2001
From: David Sidrane <David.Sidrane@NscDg.com>
Date: Tue, 28 Jan 2020 06:03:53 -0800
Subject: [PATCH] imxrt:Add critmon

---
 .../nxp/imxrt/board_critmon/board_critmon.c   | 29 ++++++++++++++-----
 1 file changed, 21 insertions(+), 8 deletions(-)

diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_critmon/board_critmon.c b/platforms/nuttx/src/px4/nxp/imxrt/board_critmon/board_critmon.c
index 14e7a3bb3b..927adea930 100644
--- a/platforms/nuttx/src/px4/nxp/imxrt/board_critmon/board_critmon.c
+++ b/platforms/nuttx/src/px4/nxp/imxrt/board_critmon/board_critmon.c
@@ -37,6 +37,15 @@
  ************************************************************************************/
 
 #include <nuttx/config.h>
+
+#include <time.h>
+#include <fixedmath.h>
+
+#include "dwt.h"
+#include "up_arch.h"
+
+#include <nuttx/clock.h>
+
 #include <arch/board/board.h>
 
 #if defined(CONFIG_SCHED_CRITMONITOR) || defined(CONFIG_SCHED_IRQMONITOR)
@@ -45,22 +54,26 @@
  * Public Functions
  ************************************************************************************/
 
-#error "missing implementation for up_critmon_gettime() and up_critmon_convert()"
-
 /************************************************************************************
  * Name: up_critmon_gettime
  ************************************************************************************/
 
-// uint32_t up_critmon_gettime(void)
-// {
-// }
+uint32_t up_critmon_gettime(void)
+{
+	return getreg32(DWT_CYCCNT);
+}
 
 /************************************************************************************
  * Name: up_critmon_convert
  ************************************************************************************/
 
-// void up_critmon_convert(uint32_t elapsed, FAR struct timespec *ts)
-// {
-// }
+void up_critmon_convert(uint32_t elapsed, FAR struct timespec *ts)
+{
+	b32_t b32elapsed;
+
+	b32elapsed  = itob32(elapsed) / BOARD_CPU_FREQUENCY;
+	ts->tv_sec  = b32toi(b32elapsed);
+	ts->tv_nsec = NSEC_PER_SEC * b32frac(b32elapsed) / b32ONE;
+}
 
 #endif /* CONFIG_SCHED_CRITMONITOR */