From 02b2b1683f96b3a2b9cfc4776c8593caaedea395 Mon Sep 17 00:00:00 2001 From: dragonmux Date: Sat, 1 Apr 2023 05:43:28 +0100 Subject: [PATCH] riscv32: Implemented support for non-native-width system bus memory writes --- src/target/riscv32.c | 101 ++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 100 insertions(+), 1 deletion(-) diff --git a/src/target/riscv32.c b/src/target/riscv32.c index 4d0c4f4380b..4b3e30ae9fb 100644 --- a/src/target/riscv32.c +++ b/src/target/riscv32.c @@ -437,6 +437,59 @@ static void riscv32_sysbus_mem_native_write(riscv_hart_s *const hart, const targ riscv_sysbus_check(hart); } +static void riscv32_sysbus_mem_adjusted_write(riscv_hart_s *const hart, const target_addr_t dest, const void *const src, + const uint8_t access_length, const uint8_t access_width, const uint8_t native_access_length) +{ + const target_addr_t alignment = ~(native_access_length - 1U); + /* + * On a 32-bit target the only possible widths are 8- 16- and 32-bit, so after the adjustment loop, + * there are only and exactly 2 possible cases to handle here: 16- and 32-bit access. + * The basic premise here is that we have to read to correctly write - to do a N bit write with a + * wider access primitive, we first have to read back what's at the target aligned location, replace + * the correct set of bits in the target value, then write the new combined value back + */ + switch (access_width) { + case RV_MEM_ACCESS_16_BIT: { + uint16_t value = 0; + /* Start by reading 16 bits */ + riscv32_sysbus_mem_native_read( + hart, &value, dest & alignment, native_access_length, RV_MEM_ACCESS_16_BIT, native_access_length); + /* Now replace the part to write (must be done on the widened version of the value) */ + uint32_t widened_value = value; + /* + * Note that to get here we're doing a 2 byte access for 1 byte so we only care about a single byte + * replacement. We also have to constrain the replacement to only happen in the lower 16 bits. + */ + adiv5_pack_data(dest & ~2U, src, &widened_value, ALIGN_8BIT); + value = (uint16_t)widened_value; + /* And finally write the new value back */ + riscv32_sysbus_mem_native_write( + hart, dest & alignment, &value, native_access_length, RV_MEM_ACCESS_16_BIT, native_access_length); + break; + } + case RV_MEM_ACCESS_32_BIT: { + uint32_t value = 0; + /* Start by reading 32 bits */ + riscv32_sysbus_mem_native_read( + hart, &value, dest & alignment, native_access_length, RV_MEM_ACCESS_32_BIT, native_access_length); + + /* Now replace the part to write */ + const char *data = (const char *)src; + /* Figure out from the access length the initial pack and adjustment */ + const uint8_t adjustment = access_length & (uint8_t)~1U; + if (adjustment) + data = (const char *)adiv5_pack_data(dest, data, &value, ALIGN_16BIT); + /* Now pack the remaining byte if necessary */ + if (access_length & 1) + adiv5_pack_data(dest + adjustment, data, &value, ALIGN_8BIT); + /* And finally write the new value back */ + riscv32_sysbus_mem_native_write( + hart, dest & alignment, &value, native_access_length, RV_MEM_ACCESS_32_BIT, native_access_length); + break; + } + } +} + static void riscv32_sysbus_mem_write( riscv_hart_s *const hart, const target_addr_t dest, const void *const src, const size_t len) { @@ -444,7 +497,53 @@ static void riscv32_sysbus_mem_write( const uint8_t access_width = riscv_mem_access_width(hart, dest, len); const uint8_t access_length = 1U << access_width; /* Check if the access is a natural/native width */ - riscv32_sysbus_mem_native_write(hart, dest, src, len, access_width, access_length); + if (hart->flags & access_length) { + riscv32_sysbus_mem_native_write(hart, dest, src, len, access_width, access_length); + return; + } + + /* If we were unable to do this using a native access, find the next largest supported access width */ + uint8_t native_access_width = access_width; + while (!((hart->flags >> native_access_width) & 1U) && native_access_width < RV_MEM_ACCESS_32_BIT) + ++native_access_width; + const uint8_t native_access_length = (uint8_t)(1U << native_access_width); + + /* Figure out how much the length is getting adjusted by in the first write to make it aligned */ + const target_addr_t length_adjustment = dest & (native_access_length - 1U); + /* + * Having done this, figure out how long the resulting write actually is so we can fill enough of the + * destination buffer with a single write + */ + const uint8_t write_length = + len + length_adjustment <= native_access_length ? len : native_access_length - length_adjustment; + + /* Do the initial adjusted access */ + size_t remainder = len; + target_addr_t address = dest; + const uint8_t *data = (const uint8_t *)src; + riscv32_sysbus_mem_adjusted_write(hart, address, data, write_length, native_access_width, native_access_length); + + /* After doing the initial access, adjust the location of the next and do any follow-up accesses required */ + remainder -= write_length; + address += write_length; + data += write_length; + + /* + * Now we're aligned to the wider access width, do another set of writes if there's + * any remainder. Do this till we either reach nothing left, or we have another small left-over amount + */ + if (!remainder) + return; + const size_t amount = remainder & ~(native_access_length - 1U); + if (amount) + riscv32_sysbus_mem_native_write(hart, address, data, amount, native_access_width, native_access_length); + remainder -= amount; + address += (uint32_t)amount; + data += amount; + + /* If there's any data left to write, do another adjusted access to perform it */ + if (remainder) + riscv32_sysbus_mem_adjusted_write(hart, address, data, remainder, native_access_width, native_access_length); } void riscv32_mem_read(target_s *const target, void *const dest, const target_addr_t src, const size_t len)