From 7a962949c4a1e462647be97e192ba0f517a60b8a Mon Sep 17 00:00:00 2001
From: Benjamin Vedder <benjamin@vedder.se>
Date: Tue, 1 Mar 2022 12:07:27 +0100
Subject: [PATCH] Added lisp repl

---
 commands.c      |  3 ++-
 datatypes.h     |  2 ++
 lispBM/lispif.c | 39 +++++++++++++++++++++++++++++++++++++++
 3 files changed, 43 insertions(+), 1 deletion(-)

diff --git a/commands.c b/commands.c
index c6232ef9..065fb17b 100644
--- a/commands.c
+++ b/commands.c
@@ -1571,7 +1571,8 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 	} break;
 
 	case COMM_LISP_SET_RUNNING:
-	case COMM_LISP_GET_STATS: {
+	case COMM_LISP_GET_STATS:
+	case COMM_LISP_REPL_CMD: {
 #ifdef USE_LISPBM
 		lispif_process_cmd(data - 1, len + 1, reply_func);
 #endif
diff --git a/datatypes.h b/datatypes.h
index 5780d50e..ef44f834 100644
--- a/datatypes.h
+++ b/datatypes.h
@@ -1066,6 +1066,8 @@ typedef enum {
 
 	COMM_BMS_SET_BATT_TYPE,
 	COMM_BMS_GET_BATT_TYPE,
+
+	COMM_LISP_REPL_CMD,
 } COMM_PACKET_ID;
 
 // CAN commands
diff --git a/lispBM/lispif.c b/lispBM/lispif.c
index c8b3b4d2..e183a7e7 100644
--- a/lispBM/lispif.c
+++ b/lispBM/lispif.c
@@ -50,6 +50,8 @@ static THD_FUNCTION(eval_thread, arg);
 static THD_WORKING_AREA(eval_thread_wa, 2048);
 static bool lisp_thd_running = false;
 
+static int repl_cid = -1;
+
 // Private functions
 static bool start_lisp(bool print);
 static uint32_t timestamp_callback(void);
@@ -178,11 +180,47 @@ void lispif_process_cmd(unsigned char *data, unsigned int len,
 		chMtxUnlock(&send_buffer_mutex);
 	} break;
 
+	case COMM_LISP_REPL_CMD: {
+		if (lisp_thd_running) {
+			bool ok = true;
+			int timeout_cnt = 1000;
+			lbm_pause_eval_with_gc(100);
+			while (lbm_get_eval_state() != EVAL_CPS_STATE_PAUSED && timeout_cnt > 0) {
+				chThdSleep(5);
+				timeout_cnt--;
+			}
+			ok = timeout_cnt > 0;
+
+			if (ok) {
+				lbm_create_char_stream_from_string(&string_tok_state, &string_tok, (char*)data);
+				repl_cid = lbm_load_and_eval_expression(&string_tok);
+				lbm_continue_eval();
+				lbm_wait_ctx(repl_cid, 500);
+				repl_cid = -1;
+			} else {
+				commands_printf_lisp("Could not pause");
+			}
+		} else {
+			commands_printf_lisp("LispBM is not running");
+		}
+	} break;
+
 	default:
 		break;
 	}
 }
 
+static void done_callback(eval_context_t *ctx) {
+	lbm_cid cid = ctx->id;
+	lbm_value t = ctx->r;
+
+	if (cid == repl_cid) {
+		char output[64];
+		lbm_print_value(output, 1024, t);
+		commands_printf_lisp("> %s", output);
+	}
+}
+
 static bool start_lisp(bool print) {
 	bool res = false;
 
@@ -202,6 +240,7 @@ static bool start_lisp(bool print) {
 			lbm_set_timestamp_us_callback(timestamp_callback);
 			lbm_set_usleep_callback(sleep_callback);
 			lbm_set_printf_callback(commands_printf_lisp);
+			lbm_set_ctx_done_callback(done_callback);
 			chThdCreateStatic(eval_thread_wa, sizeof(eval_thread_wa), NORMALPRIO, eval_thread, NULL);
 
 			lisp_thd_running = true;