Pic32mx: make row programming work with any offset

In function pic32mx_write_block() if the parameter
offset is not a multiple of row size the row offset
(offset % row_size) will be ignored by the flash
controller, shifting the code to the beginning of
the row.
Word programming gets it right.

Change-Id: I134913e3d533688f791bbcb0c6e8983524197f3c
Signed-off-by: Salvador Arroyo <sarroyofdez@yahoo.es>
Reviewed-on: http://openocd.zylin.com/796
Tested-by: jenkins
Reviewed-by: Spencer Oliver <spen@spen-soft.co.uk>
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>

1 files changed, 44 insertions, 8 deletions

diff --git a/src/flash/nor/pic32mx.c b/src/flash/nor/pic32mx.c
index b718c98..0b43c1b 100644
--- a/src/flash/nor/pic32mx.c
+++ b/src/flash/nor/pic32mx.c
@@ -420,6 +420,7 @@ static int pic32mx_write_block(struct flash_bank *bank, uint8_t *buffer,
 	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;
@@ -439,12 +440,14 @@ 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,
@@ -464,7 +467,7 @@ static int pic32mx_write_block(struct flash_bank *bank, uint8_t *buffer,
 			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,19 +476,46 @@ 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,
@@ -513,6 +543,10 @@ 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);
@@ -522,6 +556,8 @@ static int pic32mx_write_block(struct flash_bank *bank, uint8_t *buffer,
 	destroy_reg_param(&reg_params[1]);
 	destroy_reg_param(&reg_params[2]);
 
+	if (new_buffer != NULL)
+		free(new_buffer);
 	return retval;
 }