From 56613709443a00ff80ff6f4bf715d1fda121e237 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Mon, 12 Dec 2016 15:23:02 -1000
Subject: [PATCH] Changed to Upstream NuttX Instrumentation changes

---
 src/modules/systemlib/cpuload.c | 95 ++++++++++++++++++---------------
 1 file changed, 52 insertions(+), 43 deletions(-)

diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c
index 9197a072c5..20f4c00cc9 100644
--- a/src/modules/systemlib/cpuload.c
+++ b/src/modules/systemlib/cpuload.c
@@ -60,8 +60,11 @@
 
 #ifdef __PX4_NUTTX
 
-__EXPORT void sched_note_start(FAR struct tcb_s *tcb);
-__EXPORT void sched_note_stop(FAR struct tcb_s *tcb);
+# include <nuttx/sched_note.h>
+
+void sched_note_suspend(FAR struct tcb_s *tcb);
+void sched_note_resume(FAR struct tcb_s *tcb);
+
 __EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb);
 
 __EXPORT struct system_load_s system_load;
@@ -97,6 +100,8 @@ void cpuload_initialize_once()
 					system_load.total_count);	// it is assumed that these static threads have consecutive PIDs
 		system_load.tasks[system_load.total_count].valid = true;
 	}
+
+	system_load.initialized = true;
 }
 
 void sched_note_start(FAR struct tcb_s *tcb)
@@ -104,15 +109,17 @@ void sched_note_start(FAR struct tcb_s *tcb)
 	/* search first free slot */
 	int i;
 
-	for (i = 1; i < CONFIG_MAX_TASKS; i++) {
-		if (!system_load.tasks[i].valid) {
-			/* slot is available */
-			system_load.tasks[i].total_runtime = 0;
-			system_load.tasks[i].curr_start_time = 0;
-			system_load.tasks[i].tcb = tcb;
-			system_load.tasks[i].valid = true;
-			system_load.total_count++;
-			break;
+	if (system_load.initialized) {
+		for (i = 1; i < CONFIG_MAX_TASKS; i++) {
+			if (!system_load.tasks[i].valid) {
+				/* slot is available */
+				system_load.tasks[i].total_runtime = 0;
+				system_load.tasks[i].curr_start_time = 0;
+				system_load.tasks[i].tcb = tcb;
+				system_load.tasks[i].valid = true;
+				system_load.total_count++;
+				break;
+			}
 		}
 	}
 }
@@ -121,49 +128,51 @@ void sched_note_stop(FAR struct tcb_s *tcb)
 {
 	int i;
 
-	for (i = 1; i < CONFIG_MAX_TASKS; i++) {
-		if (system_load.tasks[i].tcb->pid == tcb->pid) {
-			/* mark slot as fee */
-			system_load.tasks[i].valid = false;
-			system_load.tasks[i].total_runtime = 0;
-			system_load.tasks[i].curr_start_time = 0;
-			system_load.tasks[i].tcb = NULL;
-			system_load.total_count--;
-			break;
+	if (system_load.initialized) {
+		for (i = 1; i < CONFIG_MAX_TASKS; i++) {
+			if (system_load.tasks[i].tcb != 0 && system_load.tasks[i].tcb->pid == tcb->pid) {
+				/* mark slot as fee */
+				system_load.tasks[i].valid = false;
+				system_load.tasks[i].total_runtime = 0;
+				system_load.tasks[i].curr_start_time = 0;
+				system_load.tasks[i].tcb = NULL;
+				system_load.total_count--;
+				break;
+			}
 		}
 	}
 }
 
-void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb)
+void sched_note_suspend(FAR struct tcb_s *tcb)
 {
 	uint64_t new_time = hrt_absolute_time();
 
-	/* Kind of inefficient: find both tasks and update times */
-	uint8_t both_found = 0;
-
-	for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
-		/* Check if task is initialized correctly */
-		if (system_load.tasks[i].tcb == NULL) {
-			continue;
-		}
-
-		/* Task ending its current scheduling run */
-		if (system_load.tasks[i].tcb->pid == pFromTcb->pid) {
-			system_load.tasks[i].total_runtime += new_time - system_load.tasks[i].curr_start_time;
-			both_found++;
-
-		} else if (system_load.tasks[i].tcb->pid == pToTcb->pid) {
-			system_load.tasks[i].curr_start_time = new_time;
-			both_found++;
-		}
-
-		/* Do only iterate as long as needed */
-		if (both_found == 2) {
-			break;
+	if (system_load.initialized) {
+		for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
+			/* Task ending its current scheduling run */
+			if (system_load.tasks[i].valid && system_load.tasks[i].tcb != 0 && system_load.tasks[i].tcb->pid == tcb->pid) {
+				system_load.tasks[i].total_runtime += new_time - system_load.tasks[i].curr_start_time;
+				break;
+			}
 		}
 	}
 }
 
+void sched_note_resume(FAR struct tcb_s *tcb)
+{
+	uint64_t new_time = hrt_absolute_time();
+
+	if (system_load.initialized) {
+		for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
+			if (system_load.tasks[i].valid && system_load.tasks[i].tcb->pid == tcb->pid) {
+				system_load.tasks[i].curr_start_time = new_time;
+				break;
+			}
+		}
+	}
+
+}
+
 #else
 __EXPORT struct system_load_s system_load;
 #endif