diff options
| -rw-r--r-- | src/flash/nor/pic32mx.c | 52 |
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(®_params[1], "a1", 32, PARAM_OUT); init_reg_param(®_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(®_params[1]); destroy_reg_param(®_params[2]); + if (new_buffer != NULL) + free(new_buffer); return retval; } |