diff --git a/subsys/nrf_compress/lzma/armthumb.c b/subsys/nrf_compress/lzma/armthumb.c index 33b8ce52abd6..4e25b4689f7b 100644 --- a/subsys/nrf_compress/lzma/armthumb.c +++ b/subsys/nrf_compress/lzma/armthumb.c @@ -7,17 +7,21 @@ /// // Authors: Igor Pavlov // Lasse Collin +// With changes by Nordic Semiconductor ASA // /////////////////////////////////////////////////////////////////////////////// #include #include "armthumb.h" -void arm_thumb_filter(uint8_t *buf, uint32_t buf_size, uint32_t pos, bool compress) +void arm_thumb_filter(uint8_t *buf, uint32_t buf_size, uint32_t pos, bool compress, + bool *end_part_match) { uint32_t i = 0; + uint32_t last_update_address = 0; while ((i + 4) <= buf_size) { + if ((buf[i + 1] & 0xF8) == 0xF0 && (buf[i + 3] & 0xF8) == 0xF8) { uint32_t dest; uint32_t src = (((uint32_t)(buf[i + 1]) & 7) << 19) @@ -26,6 +30,7 @@ void arm_thumb_filter(uint8_t *buf, uint32_t buf_size, uint32_t pos, bool compre | (uint32_t)(buf[i + 2]); src <<= 1; + last_update_address = i; if (compress) { dest = pos + (uint32_t)(i) + 4 + src; @@ -38,9 +43,18 @@ void arm_thumb_filter(uint8_t *buf, uint32_t buf_size, uint32_t pos, bool compre buf[i + 0] = (dest >> 11); buf[i + 3] = 0xF8 | ((dest >> 8) & 0x7); buf[i + 2] = (dest); + i += 2; } i += 2; } + + if (i == (buf_size - 2)) { + if (i > last_update_address && (buf[i + 1] & 0xF8) == 0xF0) { + *end_part_match = true; + } else { + *end_part_match = false; + } + } } diff --git a/subsys/nrf_compress/lzma/armthumb.h b/subsys/nrf_compress/lzma/armthumb.h index fa08f329c990..dd5d8ed20613 100644 --- a/subsys/nrf_compress/lzma/armthumb.h +++ b/subsys/nrf_compress/lzma/armthumb.h @@ -16,6 +16,7 @@ #include #include -void arm_thumb_filter(uint8_t *buf, uint32_t buf_size, uint32_t pos, bool compress); +void arm_thumb_filter(uint8_t *buf, uint32_t buf_size, uint32_t pos, bool compress, + bool *end_part_match); #endif diff --git a/subsys/nrf_compress/src/arm_thumb.c b/subsys/nrf_compress/src/arm_thumb.c index bf786fa23aa1..6786c554f6ae 100644 --- a/subsys/nrf_compress/src/arm_thumb.c +++ b/subsys/nrf_compress/src/arm_thumb.c @@ -16,12 +16,18 @@ LOG_MODULE_REGISTER(nrf_compress_arm_thumb, CONFIG_NRF_COMPRESS_LOG_LEVEL); BUILD_ASSERT((CONFIG_NRF_COMPRESS_CHUNK_SIZE % 4) == 0, "CONFIG_NRF_COMPRESS_CHUNK_SIZE must be multiple of 4"); -static uint8_t output_buffer[CONFIG_NRF_COMPRESS_CHUNK_SIZE]; +/* Requires 2 extra bytes to allow checking cross-chunk 16-bit aligned ARM thumb instructions */ +#define EXTRA_BUFFER_SIZE 2 + +static uint8_t output_buffer[CONFIG_NRF_COMPRESS_CHUNK_SIZE + EXTRA_BUFFER_SIZE]; +static uint8_t temp_extra_buffer[EXTRA_BUFFER_SIZE]; static uint32_t data_position = 0; +static bool has_extra_buffer_data; static int arm_thumb_init(void *inst) { data_position = 0; + has_extra_buffer_data = false; return 0; } @@ -38,6 +44,7 @@ static int arm_thumb_deinit(void *inst) static int arm_thumb_reset(void *inst) { data_position = 0; + has_extra_buffer_data = false; memset(output_buffer, 0x00, sizeof(output_buffer)); return 0; @@ -52,17 +59,47 @@ static int arm_thumb_decompress(void *inst, const uint8_t *input, size_t input_s bool last_part, uint32_t *offset, uint8_t **output, size_t *output_size) { + bool end_part_match = false; + bool extra_buffer_used = false; + if (input_size > CONFIG_NRF_COMPRESS_CHUNK_SIZE) { return -EINVAL; } - memcpy(output_buffer, input, input_size); - arm_thumb_filter(output_buffer, input_size, data_position, false); + if (has_extra_buffer_data == true) { + /* Copy bytes from temporary holding buffer */ + memcpy(output_buffer, temp_extra_buffer, sizeof(temp_extra_buffer)); + memcpy(&output_buffer[sizeof(temp_extra_buffer)], input, input_size); + end_part_match = true; + extra_buffer_used = true; + has_extra_buffer_data = false; + input_size += sizeof(temp_extra_buffer); + } else { + memcpy(output_buffer, input, input_size); + } + + arm_thumb_filter(output_buffer, input_size, data_position, false, &end_part_match); data_position += input_size; *offset = input_size; + + if (extra_buffer_used) { + *offset -= sizeof(temp_extra_buffer); + } + *output = output_buffer; *output_size = input_size; + if (end_part_match == true && !last_part) { + /* Partial match at end of input, need to cut the final 2 bytes off and stash + * them + */ + memcpy(temp_extra_buffer, &output_buffer[(input_size - sizeof(temp_extra_buffer))], + sizeof(temp_extra_buffer)); + has_extra_buffer_data = true; + *output_size -= sizeof(temp_extra_buffer); + data_position -= sizeof(temp_extra_buffer); + } + return 0; }