2 files changed, 54 insertions, 36 deletions

diff --git a/src/flash/nor/stm32l4x.c b/src/flash/nor/stm32l4x.c
index 3c05561..ef15013 100644
--- a/src/flash/nor/stm32l4x.c
+++ b/src/flash/nor/stm32l4x.c
@@ -1319,11 +1319,10 @@ static int stm32l4_write_block(struct flash_bank *bank, const uint8_t *buffer,
 {
 	struct target *target = bank->target;
 	struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
-	uint32_t buffer_size;
 	struct working_area *write_algorithm;
 	struct working_area *source;
 	uint32_t address = bank->base + offset;
-	struct reg_param reg_params[6];
+	struct reg_param reg_params[5];
 	struct armv7m_algorithm armv7m_info;
 	int retval = ERROR_OK;
 
@@ -1345,12 +1344,13 @@ static int stm32l4_write_block(struct flash_bank *bank, const uint8_t *buffer,
 		return retval;
 	}
 
-	/* memory buffer, size *must* be multiple of stm32l4_info->data_width
-	 * plus one dword for rp and one for wp */
-	/* FIXME, currently only STM32U5 devices do have a different data_width,
-	 * but STM32U5 device flash programming does not go through this function
-	 * so temporarily continue to consider the default data_width = 8 */
-	buffer_size = target_get_working_area_avail(target) & ~(2 * sizeof(uint32_t) - 1);
+	/* data_width should be multiple of double-word */
+	assert(stm32l4_info->data_width % 8 == 0);
+	const size_t extra_size = sizeof(struct stm32l4_work_area);
+	uint32_t buffer_size = target_get_working_area_avail(target) - extra_size;
+	/* buffer_size should be multiple of stm32l4_info->data_width */
+	buffer_size &= ~(stm32l4_info->data_width - 1);
+
 	if (buffer_size < 256) {
 		LOG_WARNING("large enough working area not available, can't do block memory writes");
 		target_free_working_area(target, write_algorithm);
@@ -1360,7 +1360,7 @@ static int stm32l4_write_block(struct flash_bank *bank, const uint8_t *buffer,
 		buffer_size = 16384;
 	}
 
-	if (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
+	if (target_alloc_working_area_try(target, buffer_size + extra_size, &source) != ERROR_OK) {
 		LOG_ERROR("allocating working area failed");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}
@@ -1371,28 +1371,46 @@ static int stm32l4_write_block(struct flash_bank *bank, const uint8_t *buffer,
 	init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);	/* buffer start, status (out) */
 	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);	/* buffer end */
 	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);	/* target address */
-	init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);	/* count (double word-64bit) */
-	init_reg_param(&reg_params[4], "r4", 32, PARAM_OUT);	/* flash status register */
-	init_reg_param(&reg_params[5], "r5", 32, PARAM_OUT);	/* flash control register */
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);	/* count (of stm32l4_info->data_width) */
+	init_reg_param(&reg_params[4], "sp", 32, PARAM_OUT);	/* write algo stack pointer */
 
 	buf_set_u32(reg_params[0].value, 0, 32, source->address);
 	buf_set_u32(reg_params[1].value, 0, 32, source->address + source->size);
 	buf_set_u32(reg_params[2].value, 0, 32, address);
 	buf_set_u32(reg_params[3].value, 0, 32, count);
-	buf_set_u32(reg_params[4].value, 0, 32, stm32l4_get_flash_reg_by_index(bank, STM32_FLASH_SR_INDEX));
-	buf_set_u32(reg_params[5].value, 0, 32, stm32l4_get_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX));
+	buf_set_u32(reg_params[4].value, 0, 32, source->address +
+			offsetof(struct stm32l4_work_area, stack) + LDR_STACK_SIZE);
+
+	struct stm32l4_loader_params loader_extra_params;
+
+	target_buffer_set_u32(target, (uint8_t *) &loader_extra_params.flash_sr_addr,
+			stm32l4_get_flash_reg_by_index(bank, STM32_FLASH_SR_INDEX));
+	target_buffer_set_u32(target, (uint8_t *) &loader_extra_params.flash_cr_addr,
+			stm32l4_get_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX));
+	target_buffer_set_u32(target, (uint8_t *) &loader_extra_params.flash_word_size,
+			stm32l4_info->data_width);
+	target_buffer_set_u32(target, (uint8_t *) &loader_extra_params.flash_sr_bsy_mask,
+			stm32l4_info->sr_bsy_mask);
+
+	retval = target_write_buffer(target, source->address, sizeof(loader_extra_params),
+			(uint8_t *) &loader_extra_params);
+	if (retval != ERROR_OK)
+		return retval;
 
 	retval = target_run_flash_async_algorithm(target, buffer, count, stm32l4_info->data_width,
 			0, NULL,
 			ARRAY_SIZE(reg_params), reg_params,
-			source->address, source->size,
+			source->address + offsetof(struct stm32l4_work_area, fifo),
+			source->size - offsetof(struct stm32l4_work_area, fifo),
 			write_algorithm->address, 0,
 			&armv7m_info);
 
 	if (retval == ERROR_FLASH_OPERATION_FAILED) {
 		LOG_ERROR("error executing stm32l4 flash write algorithm");
 
-		uint32_t error = buf_get_u32(reg_params[0].value, 0, 32) & FLASH_ERROR;
+		uint32_t error;
+		stm32l4_read_flash_reg_by_index(bank, STM32_FLASH_SR_INDEX, &error);
+		error &= FLASH_ERROR;
 
 		if (error & FLASH_WRPERR)
 			LOG_ERROR("flash memory write protected");
@@ -1413,7 +1431,6 @@ static int stm32l4_write_block(struct flash_bank *bank, const uint8_t *buffer,
 	destroy_reg_param(&reg_params[2]);
 	destroy_reg_param(&reg_params[3]);
 	destroy_reg_param(&reg_params[4]);
-	destroy_reg_param(&reg_params[5]);
 
 	return retval;
 }
@@ -1538,24 +1555,7 @@ static int stm32l4_write(struct flash_bank *bank, const uint8_t *buffer,
 	if (retval != ERROR_OK)
 		goto err_lock;
 
-	/**
-	 * FIXME update the flash loader to use a custom FLASH_SR_BSY mask
-	 * Workaround for STM32G0Bx/G0Cx devices in dual bank mode,
-	 * as the flash loader does not use the SR_BSY2
-	 */
-	bool use_flashloader = stm32l4_info->use_flashloader;
-	if ((stm32l4_info->part_info->id == 0x467) && stm32l4_info->dual_bank_mode) {
-		LOG_INFO("Couldn't use the flash loader in dual-bank mode");
-		use_flashloader = false;
-	} else if (stm32l4_info->part_info->id == 0x482) {
-		/**
-		 * FIXME the current flashloader does not support writing in quad-words
-		 * which is required for STM32U5 devices.
-		 */
-		use_flashloader = false;
-	}
-
-	if (use_flashloader) {
+	if (stm32l4_info->use_flashloader) {
 		/* For TrustZone enabled devices, when TZEN is set and RDP level is 0.5,
 		 * the debug is possible only in non-secure state.
 		 * Thus means the flashloader will run in non-secure mode,
@@ -1567,7 +1567,7 @@ static int stm32l4_write(struct flash_bank *bank, const uint8_t *buffer,
 				count / stm32l4_info->data_width);
 	}
 
-	if (!use_flashloader || retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
+	if (!stm32l4_info->use_flashloader || retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
 		LOG_INFO("falling back to single memory accesses");
 		retval = stm32l4_write_block_without_loader(bank, buffer, offset,
 				count / stm32l4_info->data_width);
diff --git a/src/flash/nor/stm32l4x.h b/src/flash/nor/stm32l4x.h
index 7b9162b..2d19cff 100644
--- a/src/flash/nor/stm32l4x.h
+++ b/src/flash/nor/stm32l4x.h
@@ -92,4 +92,22 @@
 
 #define STM32L5_REGS_SEC_OFFSET 0x10000000
 
+/* 100 bytes as loader stack should be large enough for the loader to operate */
+#define LDR_STACK_SIZE			100
+
+struct stm32l4_work_area {
+	struct stm32l4_loader_params {
+		uint32_t flash_sr_addr;
+		uint32_t flash_cr_addr;
+		uint32_t flash_word_size;
+		uint32_t flash_sr_bsy_mask;
+	} params;
+	uint8_t stack[LDR_STACK_SIZE];
+	struct flash_async_algorithm_circbuf {
+		uint8_t *wp;
+		uint8_t *rp;
+		uint8_t *buf;
+	} fifo;
+};
+
 #endif