diff --git a/src/flash/nor/esp_flash.c b/src/flash/nor/esp_flash.c
index 5468ad4cb..cda0b6186 100644
--- a/src/flash/nor/esp_flash.c
+++ b/src/flash/nor/esp_flash.c
@@ -295,7 +295,7 @@ int esp_algo_flash_blank_check(struct flash_bank *bank)
 	if (ret != ERROR_OK)
 		return ret;
 
-	run.stack_size = 1300;
+	run.stack_size = 512 + (esp_info->stub_log_enabled ? 512 : 0);
 	struct mem_param mp;
 	init_mem_param(&mp, 3 /*3rd usr arg*/, bank->num_sectors /*size in bytes*/, PARAM_IN);
 	run.mem_args.params = ∓
@@ -335,7 +335,7 @@ static uint32_t esp_algo_flash_get_size(struct flash_bank *bank)
 	if (ret != ERROR_OK)
 		return ret;
 
-	run.stack_size = 1024;
+	run.stack_size = 256 + (esp_info->stub_log_enabled ? 256 : 0);
 	ret = esp_info->run_func_image(bank->target,
 		&run,
 		1,
@@ -363,7 +363,7 @@ static int esp_algo_flash_get_mappings(struct flash_bank *bank,
 	if (ret != ERROR_OK)
 		return ret;
 
-	run.stack_size = 1300;
+	run.stack_size = 512 + (esp_info->stub_log_enabled ? 512 : 0);
 
 	struct mem_param mp;
 	init_mem_param(&mp,
@@ -442,7 +442,7 @@ int esp_algo_flash_erase(struct flash_bank *bank, unsigned int first, unsigned i
 	if (ret != ERROR_OK)
 		return ret;
 
-	run.stack_size = 1024;
+	run.stack_size = 512 + (esp_info->stub_log_enabled ? 512 : 0);
 	run.timeout_ms = ESP_FLASH_ERASE_TMO;
 	ret = esp_info->run_func_image(bank->target,
 		&run,
@@ -1104,7 +1104,7 @@ int esp_algo_flash_breakpoint_add(struct target *target,
 	ret = esp_algo_flasher_algorithm_init(&run, esp_info->stub_hw, esp_info->get_stub(bank));
 	if (ret != ERROR_OK)
 		return ret;
-	run.stack_size = 1300;
+	run.stack_size = 512 + (esp_info->stub_log_enabled ? 512 : 0);
 	run.usr_func_arg = &op_state;
 	run.usr_func_init = (esp_algorithm_usr_func_init_t)esp_algo_flash_bp_op_state_init;
 	run.usr_func_done = (esp_algorithm_usr_func_done_t)esp_algo_flash_bp_op_state_cleanup;
@@ -1170,7 +1170,7 @@ int esp_algo_flash_breakpoint_remove(struct target *target,
 		return ret;
 
 	op_state.esp_info = esp_info;
-	run.stack_size = 1300;
+	run.stack_size = 512 + (esp_info->stub_log_enabled ? 512 : 0);
 	run.usr_func_arg = &op_state;
 	run.usr_func_init = (esp_algorithm_usr_func_init_t)esp_algo_flash_bp_op_state_init;
 	run.usr_func_done = (esp_algorithm_usr_func_done_t)esp_algo_flash_bp_op_state_cleanup;
@@ -1287,7 +1287,7 @@ static int esp_algo_flash_boost_clock_freq(struct flash_bank *bank, int boost)
 	if (boost == 0)
 		new_cpu_freq = esp_info->old_cpu_freq;
 
-	run.stack_size = 1024;
+	run.stack_size = 512 + (esp_info->stub_log_enabled ? 512 : 0);
 	ret = esp_info->run_func_image(bank->target,
 		&run,
 		2,
diff --git a/src/target/espressif/esp_algorithm.c b/src/target/espressif/esp_algorithm.c
index 51f677107..7fa9f06f6 100644
--- a/src/target/espressif/esp_algorithm.c
+++ b/src/target/espressif/esp_algorithm.c
@@ -43,6 +43,59 @@ static int esp_algorithm_read_stub_logs(struct target *target, struct esp_algori
 	return retval;
 }
 
+#ifdef ESP_STACK_HIGH_WATER_MARK
+char *hexdump(uint8_t *buf, unsigned int size)
+{
+	int lines = (size + 15) / 16;
+	int line_length = 8 + 1 + 3 * 16 + 1;
+	int str_size = lines * line_length + 1;
+	char *str = calloc(str_size, 1);
+
+	if (!str)
+		return NULL;
+
+	char *current = str;
+	for (int i = 0; i < lines; i++) {
+		/* Print the address line */
+		current += sprintf(current, "%08x: ", i * 16);
+		/* Print the hex bytes */
+		for (int j = 0; j < 16 && (i * 16 + j) < size; j++)
+			current += sprintf(current, "%02x ", buf[i * 16 + j]);
+		/* Add newline at the end of each line */
+		*current++ = '\n';
+	}
+
+	return str;
+}
+
+static int esp_algorithm_calculate_stack_usage(struct target *target, struct esp_algorithm_stub *stub)
+{
+	if (!stub || stub->stack->address == 0)
+		return ERROR_FAIL;
+
+	uint8_t *stack_content = calloc(1, stub->stack->size);
+	if (!stack_content) {
+		LOG_ERROR("Failed to allocate memory for the stack memory!");
+		return ERROR_FAIL;
+	}
+	int retval = target_read_memory(target, stub->stack->address, 1, stub->stack->size, stack_content);
+	if (retval == ERROR_OK) {
+		LOG_OUTPUT("=================================================\n");
+		//LOG_OUTPUT("%s", hexdump(stack_content, stub->stack->size));
+		/* find first non 0xA5 address */
+		for (size_t i = 0; i < stub->stack->size; ++i) {
+			if (stack_content[i] != 0xA5) {
+				LOG_OUTPUT("(%zu) bytes used in (%d) bytes stack\n", stub->stack->size - i, stub->stack->size);
+				LOG_OUTPUT("=================================================\n");
+				break;
+			}
+		}
+	}
+	free(stack_content);
+	return retval;
+}
+#endif
+
 static int esp_algorithm_run_image(struct target *target,
 	struct esp_algorithm_run_data *run,
 	uint32_t num_args,
@@ -127,6 +180,10 @@ static int esp_algorithm_run_image(struct target *target,
 	}
 	esp_algorithm_read_stub_logs(target, &run->stub);
 
+#ifdef ESP_STACK_HIGH_WATER_MARK
+	esp_algorithm_calculate_stack_usage(target, &run->stub);
+#endif
+
 	if (run->usr_func_done)
 		run->usr_func_done(target, run, run->usr_func_arg);
 
@@ -486,6 +543,15 @@ int esp_algorithm_load_func_image(struct target *target, struct esp_algorithm_ru
 			goto _on_error;
 		}
 		run->stub.stack_addr = run->stub.stack->address + run->stack_size;
+
+#ifdef ESP_STACK_HIGH_WATER_MARK
+		/* For development purpose only */
+		uint8_t buff[run->stack_size];
+		memset(buff, 0xA5, sizeof(buff));
+		retval = target_write_memory(target, run->stub.stack->address, 1, run->stack_size, buff);
+		if (retval != ERROR_OK)
+			return retval;
+#endif
 	}
 
 	if (duration_measure(&algo_time) != 0) {
diff --git a/src/target/espressif/esp_algorithm.h b/src/target/espressif/esp_algorithm.h
index 11d275777..152349b6a 100644
--- a/src/target/espressif/esp_algorithm.h
+++ b/src/target/espressif/esp_algorithm.h
@@ -131,6 +131,7 @@ struct esp_algorithm_image {
 	bool reverse;
 };
 
+//#define ESP_STACK_HIGH_WATER_MARK  /* Comment out to see the stack usage of each stub command */
 #define ESP_IMAGE_ELF_PHF_EXEC			0x1
 
 /**