From c5539950d9632a77d1faef6931f2f3a3f4934428 Mon Sep 17 00:00:00 2001
From: Andrea Fioraldi <andreafioraldi@gmail.com>
Date: Fri, 10 Jun 2022 17:07:51 +0200
Subject: [PATCH] libafl custom monitor commands in gdbstub

---
 gdbstub.c | 55 +++++++++++++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 53 insertions(+), 2 deletions(-)

diff --git a/gdbstub.c b/gdbstub.c
index a3ff8702ce..d3083fff61 100644
--- a/gdbstub.c
+++ b/gdbstub.c
@@ -2158,6 +2158,58 @@ static void handle_query_offsets(GArray *params, void *user_ctx)
                     ts->info->data_offset);
     put_strbuf();
 }
+
+//// --- Begin LibAFL code ---
+
+struct libafl_custom_gdb_cmd {
+    void (*callback)(uint8_t*, size_t, void*);
+    void* data;
+    struct libafl_custom_gdb_cmd* next;
+};
+
+struct libafl_custom_gdb_cmd* libafl_qemu_gdb_cmds = NULL;
+
+void libafl_qemu_add_gdb_cmd(void (*callback)(uint8_t*, size_t, void*), void* data);
+void libafl_qemu_add_gdb_cmd(void (*callback)(uint8_t*, size_t, void*), void* data)
+{
+    struct libafl_custom_gdb_cmd* c = malloc(sizeof(struct libafl_custom_gdb_cmd));
+    c->callback = callback;
+    c->data = data;
+    c->next = libafl_qemu_gdb_cmds;
+    libafl_qemu_gdb_cmds = c;
+}
+
+static void handle_query_rcmd(GArray *params, void *user_ctx)
+{
+    // const guint8 zero = 0;
+    int len;
+
+    if (!params->len) {
+        put_packet("E22");
+        return;
+    }
+
+    len = strlen(get_param(params, 0)->data);
+    if (len % 2) {
+        put_packet("E01");
+        return;
+    }
+
+    g_assert(gdbserver_state.mem_buf->len == 0);
+    len = len / 2;
+    hextomem(gdbserver_state.mem_buf, get_param(params, 0)->data, len);
+    // g_byte_array_append(gdbserver_state.mem_buf, &zero, 1);
+
+    struct libafl_custom_gdb_cmd** c = &libafl_qemu_gdb_cmds;
+    while (*c) {
+        (*c)->callback(gdbserver_state.mem_buf->data, gdbserver_state.mem_buf->len, (*c)->data);
+        c = &(*c)->next;
+    }
+
+    put_packet("OK");
+}
+
+//// --- End LibAFL code ---
 #else
 static void handle_query_rcmd(GArray *params, void *user_ctx)
 {
@@ -2394,14 +2446,13 @@ static const GdbCmdParseEntry gdb_gen_query_table[] = {
         .handler = handle_query_offsets,
         .cmd = "Offsets",
     },
-#else
+#endif
     {
         .handler = handle_query_rcmd,
         .cmd = "Rcmd,",
         .cmd_startswith = 1,
         .schema = "s0"
     },
-#endif
     {
         .handler = handle_query_supported,
         .cmd = "Supported:",