From 40e9e29075d17fc2b6818050611516f0b29007bd Mon Sep 17 00:00:00 2001
From: Benjamin Vedder <benjamin@vedder.se>
Date: Thu, 17 Nov 2022 11:21:45 +0100
Subject: [PATCH] Added function to unregister previously registered reply
 function

---
 comm/commands.c           | 15 +++++++++++++++
 comm/commands.h           |  1 +
 lispBM/c_libs/vesc_c_if.h |  3 +++
 lispBM/lispif_c_lib.c     |  3 +++
 4 files changed, 22 insertions(+)

diff --git a/comm/commands.c b/comm/commands.c
index 896f1b4c..d514cc08 100644
--- a/comm/commands.c
+++ b/comm/commands.c
@@ -165,6 +165,21 @@ void commands_send_packet_last_blocking(unsigned char *data, unsigned int len) {
 	}
 }
 
+void commands_unregister_reply_func(void(*reply_func)(unsigned char *data, unsigned int len)) {
+	if (send_func == reply_func) {
+		send_func = NULL;
+	}
+	if (send_func_blocking == reply_func) {
+		send_func_blocking = NULL;
+	}
+	if (send_func_nrf == reply_func) {
+		send_func_nrf = NULL;
+	}
+	if (send_func_can_fwd == reply_func) {
+		send_func_can_fwd = NULL;
+	}
+}
+
 /**
  * Process a received buffer with commands and data.
  *
diff --git a/comm/commands.h b/comm/commands.h
index b0241650..d07c7baf 100644
--- a/comm/commands.h
+++ b/comm/commands.h
@@ -29,6 +29,7 @@ void commands_send_packet(unsigned char *data, unsigned int len);
 void commands_send_packet_can_last(unsigned char *data, unsigned int len);
 void commands_send_packet_nrf(unsigned char *data, unsigned int len);
 void commands_send_packet_last_blocking(unsigned char *data, unsigned int len);
+void commands_unregister_reply_func(void(*reply_func)(unsigned char *data, unsigned int len));
 void commands_process_packet(unsigned char *data, unsigned int len,
 		void(*reply_func)(unsigned char *data, unsigned int len));
 int commands_printf(const char* format, ...);
diff --git a/lispBM/c_libs/vesc_c_if.h b/lispBM/c_libs/vesc_c_if.h
index c2119ad4..e5cd7b8d 100755
--- a/lispBM/c_libs/vesc_c_if.h
+++ b/lispBM/c_libs/vesc_c_if.h
@@ -495,6 +495,9 @@ typedef struct {
 	// System lock (with counting)
 	void (*sys_lock)(void);
 	void (*sys_unlock)(void);
+
+	// Unregister pointers to previously used reply function
+	void (*commands_unregister_reply_func)(void(*reply_func)(unsigned char *data, unsigned int len));
 } vesc_c_if;
 
 typedef struct {
diff --git a/lispBM/lispif_c_lib.c b/lispBM/lispif_c_lib.c
index d60e92f7..4dfeeeaa 100644
--- a/lispBM/lispif_c_lib.c
+++ b/lispBM/lispif_c_lib.c
@@ -780,6 +780,9 @@ lbm_value ext_load_native_lib(lbm_value *args, lbm_uint argn) {
 		cif.cif.sys_lock = utils_sys_lock_cnt;
 		cif.cif.sys_unlock = utils_sys_unlock_cnt;
 
+		// Unregister pointers to previously used reply function
+		cif.cif.commands_unregister_reply_func = commands_unregister_reply_func;
+
 		lib_init_done = true;
 	}