From db6ba047d9c430125ab4ef791e432ceab28ab3c3 Mon Sep 17 00:00:00 2001
From: Benjamin Vedder <benjamin@vedder.se>
Date: Tue, 17 Sep 2019 19:39:11 +0200
Subject: [PATCH] Added COMM_BM_READ_MEM command

---
 CHANGELOG          |  3 +++
 blackmagic/bm_if.c | 28 ++++++++++++++++++++++++++++
 blackmagic/bm_if.h |  1 +
 commands.c         | 20 ++++++++++++++++++++
 datatypes.h        |  1 +
 5 files changed, 53 insertions(+)

diff --git a/CHANGELOG b/CHANGELOG
index 5af4c9c5..7777622d 100644
--- a/CHANGELOG
+++ b/CHANGELOG
@@ -1,3 +1,6 @@
+=== FW 3.62 ===
+* Added COMM_BM_MEM_READ.
+
 === FW 3.61 ===
 * Added PPM_CTRL_TYPE_CURRENT_SMART_REV mode.
 
diff --git a/blackmagic/bm_if.c b/blackmagic/bm_if.c
index 9e9eaa9b..462fe516 100644
--- a/blackmagic/bm_if.c
+++ b/blackmagic/bm_if.c
@@ -459,6 +459,34 @@ int bm_write_flash(uint32_t addr, const void *data, uint32_t len) {
 	return ret;
 }
 
+/**
+ * Read target memory
+ *
+ * @param addr
+ * Address to read from
+ *
+ * @param data
+ * Store the data here
+ *
+ * @param len
+ * Length of the data
+ *
+ * @return
+ * -2: Read failed
+ * -1: Not connected
+ *  1: Success
+ */
+int bm_mem_read(uint32_t addr, void *data, uint32_t len) {
+	int ret = -1;
+
+	if (cur_target) {
+		target_print_en = false;
+		ret = target_mem_read(cur_target, data, addr, len) ? 1 : -2;
+	}
+
+	return ret;
+}
+
 /**
  * Reboot target.
  *
diff --git a/blackmagic/bm_if.h b/blackmagic/bm_if.h
index 156355ec..3576c58f 100644
--- a/blackmagic/bm_if.h
+++ b/blackmagic/bm_if.h
@@ -29,6 +29,7 @@ void bm_set_enabled(bool enabled);
 int bm_connect(void);
 int bm_erase_flash_all(void);
 int bm_write_flash(uint32_t addr, const void *data, uint32_t len);
+int bm_mem_read(uint32_t addr, void *data, uint32_t len);
 int bm_reboot(void);
 void bm_disconnect(void);
 void bm_change_swd_pins(stm32_gpio_t *swdio_port, int swdio_pin,
diff --git a/commands.c b/commands.c
index c7a4c286..504d77f8 100644
--- a/commands.c
+++ b/commands.c
@@ -938,6 +938,7 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 	case COMM_BM_DISCONNECT:
 	case COMM_BM_MAP_PINS_DEFAULT:
 	case COMM_BM_MAP_PINS_NRF5X:
+	case COMM_BM_MEM_READ:
 		if (!is_blocking) {
 			memcpy(blocking_thread_cmd_buffer, data - 1, len + 1);
 			blocking_thread_cmd_len = len;
@@ -1454,6 +1455,25 @@ static THD_FUNCTION(blocking_thread, arg) {
 		} break;
 #endif
 
+		case COMM_BM_MEM_READ: {
+			int32_t ind = 0;
+			uint32_t addr = buffer_get_uint32(data, &ind);
+			uint16_t read_len = buffer_get_uint16(data, &ind);
+
+			if (read_len > sizeof(send_buffer) - 3) {
+				read_len = sizeof(send_buffer) - 3;
+			}
+
+			int res = bm_mem_read(addr, send_buffer + 3, read_len);
+
+			ind = 0;
+			send_buffer[ind++] = packet_id;
+			buffer_append_int16(send_buffer, res, &ind);
+			if (send_func_blocking) {
+				send_func_blocking(send_buffer, ind);
+			}
+		} break;
+
 		default:
 			break;
 		}
diff --git a/datatypes.h b/datatypes.h
index 3601cadf..2d8abeed 100644
--- a/datatypes.h
+++ b/datatypes.h
@@ -660,6 +660,7 @@ typedef enum {
 	COMM_PLOT_DATA,
 	COMM_PLOT_ADD_GRAPH,
 	COMM_PLOT_SET_GRAPH,
+	COMM_BM_MEM_READ,
 } COMM_PACKET_ID;
 
 // CAN commands