From 0053aeec97f639deb7163ef629b33aa558dd8c8d Mon Sep 17 00:00:00 2001
From: Peter van der Perk <peter.vanderperk@nxp.com>
Date: Wed, 11 May 2022 09:47:47 +0200
Subject: [PATCH] Cyphal restore O1Heap statistics

---
 src/drivers/cyphal/CanardHandle.cpp | 21 +++++++++++++--------
 src/drivers/cyphal/CanardHandle.hpp |  4 +++-
 src/drivers/cyphal/Cyphal.cpp       |  4 ++--
 3 files changed, 18 insertions(+), 11 deletions(-)

diff --git a/src/drivers/cyphal/CanardHandle.cpp b/src/drivers/cyphal/CanardHandle.cpp
index d27d34c1d2..a197712afe 100644
--- a/src/drivers/cyphal/CanardHandle.cpp
+++ b/src/drivers/cyphal/CanardHandle.cpp
@@ -52,18 +52,18 @@
 #endif // NuttX
 
 
-O1HeapInstance *uavcan_allocator{nullptr};
+O1HeapInstance *cyphal_allocator{nullptr};
 
-static void *memAllocate(CanardInstance *const ins, const size_t amount) { return o1heapAllocate(uavcan_allocator, amount); }
-static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree(uavcan_allocator, pointer); }
+static void *memAllocate(CanardInstance *const ins, const size_t amount) { return o1heapAllocate(cyphal_allocator, amount); }
+static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree(cyphal_allocator, pointer); }
 
 
 CanardHandle::CanardHandle(uint32_t node_id, const size_t capacity, const size_t mtu_bytes)
 {
-	_uavcan_heap = memalign(O1HEAP_ALIGNMENT, HeapSize);
-	uavcan_allocator = o1heapInit(_uavcan_heap, HeapSize, nullptr, nullptr);
+	_cyphal_heap = memalign(O1HEAP_ALIGNMENT, HeapSize);
+	cyphal_allocator = o1heapInit(_cyphal_heap, HeapSize, nullptr, nullptr);
 
-	if (uavcan_allocator == nullptr) {
+	if (cyphal_allocator == nullptr) {
 		PX4_ERR("o1heapInit failed with size %u", HeapSize);
 	}
 
@@ -89,8 +89,8 @@ CanardHandle::~CanardHandle()
 	delete _can_interface;
 	_can_interface = nullptr;
 
-	delete static_cast<uint8_t *>(_uavcan_heap);
-	_uavcan_heap = nullptr;
+	delete static_cast<uint8_t *>(_cyphal_heap);
+	_cyphal_heap = nullptr;
 
 }
 
@@ -200,6 +200,11 @@ CanardTreeNode *CanardHandle::getRxSubscriptions(CanardTransferKind kind)
 	return _canard_instance.rx_subscriptions[kind];
 }
 
+O1HeapDiagnostics CanardHandle::getO1HeapDiagnostics()
+{
+	return o1heapGetDiagnostics(cyphal_allocator);
+}
+
 int32_t CanardHandle::mtu()
 {
 	return _queue.mtu_bytes;
diff --git a/src/drivers/cyphal/CanardHandle.hpp b/src/drivers/cyphal/CanardHandle.hpp
index 6e99def8e6..1e6c787c31 100644
--- a/src/drivers/cyphal/CanardHandle.hpp
+++ b/src/drivers/cyphal/CanardHandle.hpp
@@ -34,6 +34,7 @@
 #pragma once
 
 #include <canard.h>
+#include "o1heap/o1heap.h"
 #include "CanardInterface.hpp"
 
 class CanardHandle
@@ -68,6 +69,7 @@ public:
 	int8_t RxUnsubscribe(const CanardTransferKind transfer_kind,
 			     const CanardPortID       port_id);
 	CanardTreeNode *getRxSubscriptions(CanardTransferKind kind);
+	O1HeapDiagnostics getO1HeapDiagnostics();
 
 	int32_t mtu();
 	CanardNodeID node_id();
@@ -80,6 +82,6 @@ private:
 
 	CanardTxQueue _queue;
 
-	void *_uavcan_heap{nullptr};
+	void *_cyphal_heap{nullptr};
 
 };
diff --git a/src/drivers/cyphal/Cyphal.cpp b/src/drivers/cyphal/Cyphal.cpp
index 2dd977f06f..dd6fd8b711 100644
--- a/src/drivers/cyphal/Cyphal.cpp
+++ b/src/drivers/cyphal/Cyphal.cpp
@@ -245,12 +245,12 @@ void CyphalNode::print_info()
 	perf_print_counter(_cycle_perf);
 	perf_print_counter(_interval_perf);
 
-	/*O1HeapDiagnostics heap_diagnostics = o1heapGetDiagnostics(uavcan_allocator);
+	O1HeapDiagnostics heap_diagnostics = _canard_handle.getO1HeapDiagnostics();
 
 	PX4_INFO("Heap status %zu/%zu Peak alloc %zu Peak req %zu OOM count %" PRIu64,
 		 heap_diagnostics.allocated, heap_diagnostics.capacity,
 		 heap_diagnostics.peak_allocated, heap_diagnostics.peak_request_size,
-		 heap_diagnostics.oom_count);*/
+		 heap_diagnostics.oom_count);
 
 	_pub_manager.printInfo();