X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Fflash%2Fnor%2Fpic32mx.c;h=842fd76bba7fc5193fca83ad3c6b3eaeb8a74820;hp=b718c98ffddbce613f934853940b76fa94783fc4;hb=4da4e1cfb7d93dcedc333c11c787b83b8baf7dfa;hpb=a25e61ecd364eae32e45d0903f241bfd62b82782

diff --git a/src/flash/nor/pic32mx.c b/src/flash/nor/pic32mx.c
index b718c98ffd..842fd76bba 100644
--- a/src/flash/nor/pic32mx.c
+++ b/src/flash/nor/pic32mx.c
@@ -96,7 +96,6 @@
 #define MX_1_2			1	/* PIC32mx1xx/2xx */
 
 struct pic32mx_flash_bank {
-	struct working_area *write_algorithm;
 	int probed;
 	int dev_type;		/* Default 0. 1 for Pic32MX1XX/2XX variant */
 };
@@ -193,7 +192,6 @@ FLASH_BANK_COMMAND_HANDLER(pic32mx_flash_bank_command)
 	pic32mx_info = malloc(sizeof(struct pic32mx_flash_bank));
 	bank->driver_priv = pic32mx_info;
 
-	pic32mx_info->write_algorithm = NULL;
 	pic32mx_info->probed = 0;
 	pic32mx_info->dev_type = 0;
 
@@ -417,9 +415,11 @@ static int pic32mx_write_block(struct flash_bank *bank, uint8_t *buffer,
 {
 	struct target *target = bank->target;
 	uint32_t buffer_size = 16384;
+	struct working_area *write_algorithm;
 	struct working_area *source;
 	uint32_t address = bank->base + offset;
 	struct reg_param reg_params[3];
+	uint32_t row_size;
 	int retval = ERROR_OK;
 
 	struct pic32mx_flash_bank *pic32mx_info = bank->driver_priv;
@@ -427,7 +427,7 @@ static int pic32mx_write_block(struct flash_bank *bank, uint8_t *buffer,
 
 	/* flash write code */
 	if (target_alloc_working_area(target, sizeof(pic32mx_flash_write_code),
-			&pic32mx_info->write_algorithm) != ERROR_OK) {
+			&write_algorithm) != ERROR_OK) {
 		LOG_WARNING("no working area available, can't do block memory writes");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	};
@@ -439,15 +439,17 @@ static int pic32mx_write_block(struct flash_bank *bank, uint8_t *buffer,
 		pic32mx_flash_write_code[14] = 0x24840080;
 		pic32mx_flash_write_code[15] = 0x24A50080;
 		pic32mx_flash_write_code[17] = 0x24C6FFE0;
+		row_size = 128;
 	} else {
 		/* 512 byte row */
 		pic32mx_flash_write_code[8] = 0x2CD30080;
 		pic32mx_flash_write_code[14] = 0x24840200;
 		pic32mx_flash_write_code[15] = 0x24A50200;
 		pic32mx_flash_write_code[17] = 0x24C6FF80;
+		row_size = 512;
 	}
 
-	retval = target_write_buffer(target, pic32mx_info->write_algorithm->address,
+	retval = target_write_buffer(target, write_algorithm->address,
 			sizeof(pic32mx_flash_write_code), (uint8_t *)pic32mx_flash_write_code);
 	if (retval != ERROR_OK)
 		return retval;
@@ -456,15 +458,14 @@ static int pic32mx_write_block(struct flash_bank *bank, uint8_t *buffer,
 	while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
 		buffer_size /= 2;
 		if (buffer_size <= 256) {
-			/* if we already allocated the writing code, but failed to get a
+			/* we already allocated the writing code, but failed to get a
 			 * buffer, free the algorithm */
-			if (pic32mx_info->write_algorithm)
-				target_free_working_area(target, pic32mx_info->write_algorithm);
+			target_free_working_area(target, write_algorithm);
 
 			LOG_WARNING("no large enough working area available, can't do block memory writes");
 			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 		}
-	};
+	}
 
 	mips32_info.common_magic = MIPS32_COMMON_MAGIC;
 	mips32_info.isa_mode = MIPS32_ISA_MIPS32;
@@ -473,22 +474,49 @@ static int pic32mx_write_block(struct flash_bank *bank, uint8_t *buffer,
 	init_reg_param(&reg_params[1], "a1", 32, PARAM_OUT);
 	init_reg_param(&reg_params[2], "a2", 32, PARAM_OUT);
 
+	int row_offset = offset % row_size;
+	uint8_t *new_buffer = NULL;
+	if (row_offset && (count >= (row_size / 4))) {
+		new_buffer = malloc(buffer_size);
+		if (new_buffer == NULL) {
+			LOG_ERROR("Out of memory");
+			return ERROR_FAIL;
+		}
+		memset(new_buffer,  0xff, row_offset);
+		address -= row_offset;
+	} else
+		row_offset = 0;
+
 	while (count > 0) {
 		uint32_t status;
-		uint32_t thisrun_count = (count > (buffer_size / 4)) ?
-				(buffer_size / 4) : count;
+		uint32_t thisrun_count;
 
-		retval = target_write_buffer(target, source->address,
-				thisrun_count * 4, buffer);
-		if (retval != ERROR_OK)
-			break;
+		if (row_offset) {
+			thisrun_count = (count > ((buffer_size - row_offset) / 4)) ?
+				((buffer_size - row_offset) / 4) : count;
+
+			memcpy(new_buffer + row_offset, buffer, thisrun_count * 4);
+
+			retval = target_write_buffer(target, source->address,
+				row_offset + thisrun_count * 4, new_buffer);
+			if (retval != ERROR_OK)
+				break;
+		} else {
+			thisrun_count = (count > (buffer_size / 4)) ?
+					(buffer_size / 4) : count;
+
+			retval = target_write_buffer(target, source->address,
+					thisrun_count * 4, buffer);
+			if (retval != ERROR_OK)
+				break;
+		}
 
 		buf_set_u32(reg_params[0].value, 0, 32, Virt2Phys(source->address));
 		buf_set_u32(reg_params[1].value, 0, 32, Virt2Phys(address));
-		buf_set_u32(reg_params[2].value, 0, 32, thisrun_count);
+		buf_set_u32(reg_params[2].value, 0, 32, thisrun_count + row_offset / 4);
 
 		retval = target_run_algorithm(target, 0, NULL, 3, reg_params,
-				pic32mx_info->write_algorithm->address,
+				write_algorithm->address,
 				0, 10000, &mips32_info);
 		if (retval != ERROR_OK) {
 			LOG_ERROR("error executing pic32mx flash write algorithm");
@@ -513,15 +541,21 @@ static int pic32mx_write_block(struct flash_bank *bank, uint8_t *buffer,
 		buffer += thisrun_count * 4;
 		address += thisrun_count * 4;
 		count -= thisrun_count;
+		if (row_offset) {
+			address += row_offset;
+			row_offset = 0;
+		}
 	}
 
 	target_free_working_area(target, source);
-	target_free_working_area(target, pic32mx_info->write_algorithm);
+	target_free_working_area(target, write_algorithm);
 
 	destroy_reg_param(&reg_params[0]);
 	destroy_reg_param(&reg_params[1]);
 	destroy_reg_param(&reg_params[2]);
 
+	if (new_buffer != NULL)
+		free(new_buffer);
 	return retval;
 }