From 8f37e02c59ea24d5e46c7ab44ae61e399c2da4da Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 May 2016 12:23:29 +0200
Subject: [PATCH] Darwin: Print the relative CPU load produced by each thread

---
 src/modules/systemlib/print_load_posix.c | 101 ++++++++++++++++++++++-
 1 file changed, 100 insertions(+), 1 deletion(-)

diff --git a/src/modules/systemlib/print_load_posix.c b/src/modules/systemlib/print_load_posix.c
index 5e12dc3323..b89f3412cf 100644
--- a/src/modules/systemlib/print_load_posix.c
+++ b/src/modules/systemlib/print_load_posix.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,13 +39,21 @@
  * @author Lorenz Meier <lorenz@px4.io>
  */
 
+#include <px4_posix.h>
+
+#include <unistd.h>
 #include <string.h>
 #include <stdio.h>
+#include <stdbool.h>
 
 #include <systemlib/cpuload.h>
 #include <systemlib/printload.h>
 #include <drivers/drv_hrt.h>
 
+#ifdef __PX4_DARWIN
+#include <mach/mach.h>
+#endif
+
 extern struct system_load_s system_load;
 
 #define CL "\033[K" // clear line
@@ -70,5 +78,96 @@ void init_print_load_s(uint64_t t, struct print_load_s *s)
 
 void print_load(uint64_t t, int fd, struct print_load_s *print_state)
 {
+	char *clear_line = "";
+
+	/* print system information */
+	if (fd == 1) {
+		dprintf(fd, "\033[H"); /* move cursor home and clear screen */
+		clear_line = CL;
+	}
+
+#ifdef __PX4_DARWIN
+	pid_t pid = getpid();   //-- this is the process id you need info for
+	task_t port;
+	task_for_pid(mach_task_self(), pid, &port);
+
+	task_info_data_t tinfo;
+	mach_msg_type_number_t task_info_count;
+
+	task_info_count = TASK_INFO_MAX;
+	kern_return_t kr = task_info(port, TASK_BASIC_INFO, (task_info_t)tinfo, &task_info_count);
+
+	if (kr != KERN_SUCCESS) {
+		return;
+	}
+
+	task_basic_info_t basic_info;
+	thread_array_t thread_list;
+	mach_msg_type_number_t thread_count;
+
+	thread_info_data_t th_info_data;
+	mach_msg_type_number_t thread_info_count;
+
+	thread_basic_info_t basic_info_th;
+	uint32_t stat_thread = 0;
+
+	basic_info = (task_basic_info_t)tinfo;
+
+	// get all threads of the PX4 main task
+	kr = task_threads(port, &thread_list, &thread_count);
+
+	if (kr != KERN_SUCCESS) {
+		PX4_WARN("ERROR getting thread list");
+		return;
+	}
+
+	if (thread_count > 0) {
+		stat_thread += thread_count;
+	}
+
+	long tot_sec = 0;
+	long tot_usec = 0;
+	long tot_cpu = 0;
+
+	dprintf(fd, "%sThreads: %d total\n",
+		clear_line,
+		thread_count);
+
+	for (int j = 0; j < thread_count; j++) {
+		thread_info_count = THREAD_INFO_MAX;
+		kr = thread_info(thread_list[j], THREAD_BASIC_INFO,
+				 (thread_info_t)th_info_data, &thread_info_count);
+
+		if (kr != KERN_SUCCESS) {
+			PX4_WARN("ERROR getting thread info");
+			continue;
+		}
+
+		basic_info_th = (thread_basic_info_t)th_info_data;
+
+
+		if (!(basic_info_th->flags & TH_FLAGS_IDLE)) {
+			tot_sec = tot_sec + basic_info_th->user_time.seconds + basic_info_th->system_time.seconds;
+			tot_usec = tot_usec + basic_info_th->system_time.microseconds + basic_info_th->system_time.microseconds;
+			tot_cpu = tot_cpu + basic_info_th->cpu_usage;
+		}
+
+		// char tname[128];
+
+		// int ret = pthread_getname_np(pthread_t *thread,
+		//                      const char *name, size_t len);
+
+		dprintf(fd, "thread %d\t\t %d\n", j, basic_info_th->cpu_usage);
+	}
+
+	kr = vm_deallocate(mach_task_self(), (vm_offset_t)thread_list,
+			   thread_count * sizeof(thread_t));
+
+	if (kr != KERN_SUCCESS) {
+		PX4_WARN("ERROR cleaning up thread info");
+		return;
+	}
+
+#endif
 }