perf_counter: add perf_print_counter_buffer & perf_iterate_all

These can be used to print the perf counters to a buffer.

src/modules/systemlib/perf_counter.c

@@ -488,6 +488,59 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 	}
 }
 
+
+int
+perf_print_counter_buffer(char *buffer, int length, perf_counter_t handle)
+{
+	int num_written = 0;
+
+	if (handle == NULL) {
+		return 0;
+	}
+
+	switch (handle->type) {
+	case PC_COUNT:
+		num_written = snprintf(buffer, length, "%s: %llu events",
+				       handle->name,
+				       (unsigned long long)((struct perf_ctr_count *)handle)->event_count);
+		break;
+
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+			float rms = sqrtf(pce->M2 / (pce->event_count - 1));
+			num_written = snprintf(buffer, length, "%s: %llu events, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms",
+					       handle->name,
+					       (unsigned long long)pce->event_count,
+					       (unsigned long long)pce->time_total,
+					       (pce->event_count == 0) ? 0 : (unsigned long long)pce->time_total / pce->event_count,
+					       (unsigned long long)pce->time_least,
+					       (unsigned long long)pce->time_most,
+					       (double)(1e6f * rms));
+			break;
+		}
+
+	case PC_INTERVAL: {
+			struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+			float rms = sqrtf(pci->M2 / (pci->event_count - 1));
+
+			num_written = snprintf(buffer, length, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3fus rms",
+					       handle->name,
+					       (unsigned long long)pci->event_count,
+					       (pci->event_count == 0) ? 0 : (unsigned long long)(pci->time_last - pci->time_first) / pci->event_count,
+					       (unsigned long long)pci->time_least,
+					       (unsigned long long)pci->time_most,
+					       (double)(1e6f * rms));
+			break;
+		}
+
+	default:
+		break;
+	}
+
+	buffer[length - 1] = 0; // ensure 0-termination
+	return num_written;
+}
+
 uint64_t
 perf_event_count(perf_counter_t handle)
 {
@@ -516,6 +569,20 @@ perf_event_count(perf_counter_t handle)
 	return 0;
 }
 
+void
+perf_iterate_all(perf_callback cb, void *user)
+{
+	pthread_mutex_lock(&perf_counters_mutex);
+	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
+
+	while (handle != NULL) {
+		cb(handle, user);
+		handle = (perf_counter_t)sq_next(&handle->link);
+	}
+
+	pthread_mutex_unlock(&perf_counters_mutex);
+}
+
 void
 perf_print_all(int fd)
 {

src/modules/systemlib/perf_counter.h

@@ -170,6 +170,16 @@ __EXPORT extern void		perf_print_counter(perf_counter_t handle);
  */
 __EXPORT extern void		perf_print_counter_fd(int fd, perf_counter_t handle);
 
+/**
+ * Print one performance counter to a buffer.
+ *
+ * @param buffer			buffer to write to
+ * @param length			buffer length
+ * @param handle			The counter to print.
+ * @param return			number of bytes written
+ */
+__EXPORT extern int		perf_print_counter_buffer(char *buffer, int length, perf_counter_t handle);
+
 /**
  * Print all of the performance counters.
  *
@@ -177,6 +187,21 @@ __EXPORT extern void		perf_print_counter_fd(int fd, perf_counter_t handle);
  */
 __EXPORT extern void		perf_print_all(int fd);
 
+
+typedef void (*perf_callback)(perf_counter_t handle, void *user);
+
+/**
+ * Iterate over all performance counters using a callback.
+ *
+ * Caution: This will aquire the mutex, so do not call any other perf_* method
+ * that aquire the mutex as well from the callback (If this is needed, configure
+ * the mutex to be reentrant).
+ *
+ * @param cb callback method
+ * @param user custom argument for the callback
+ */
+__EXPORT extern void	perf_iterate_all(perf_callback cb, void *user);
+
 /**
  * Print hrt latency counters.
  *

src/platforms/ros/perf_counter.cpp

@@ -144,6 +144,16 @@ void		perf_print_counter_fd(int fd, perf_counter_t handle)
 
 }
 
+int		perf_print_counter_buffer(char *buffer, int length, perf_counter_t handle)
+{
+	return 0;
+}
+
+void	perf_iterate_all(perf_callback cb, void *user)
+{
+
+}
+
 /**
  * Print all of the performance counters.
  *