fixup! riscv32: Implemented support for non-native-width system bus m…

…emory reads

src/target/riscv32.c

@@ -317,26 +317,40 @@ static void riscv32_sysbus_mem_native_read(riscv_hart_s *const hart, void *const
 }
 
 static void riscv32_sysbus_mem_adjusted_read(riscv_hart_s *const hart, void *const dest, const target_addr_t src,
-	const uint8_t access_width, const uint8_t native_access_width, const uint8_t access_length)
+	const uint8_t access_length, const uint8_t access_width, const uint8_t native_access_length)
 {
-	const target_addr_t alignment = ~(access_length - 1U);
+	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.
 	 */
-	switch (native_access_width) {
+	switch (access_width) {
 	case RV_MEM_ACCESS_16_BIT: {
 		uint16_t value = 0;
+		/* Run the 16-bit native read, storing the result in `value` */
 		riscv32_sysbus_mem_native_read(
-			hart, &value, src & alignment, access_length, RV_MEM_ACCESS_16_BIT, access_length);
-		adiv5_unpack_data(dest, src & 1U, value, access_width);
+			hart, &value, src & alignment, native_access_length, RV_MEM_ACCESS_16_BIT, native_access_length);
+		/* Having completed the read, unpack the data - either 1 or 2 bytes */
+		adiv5_unpack_data(dest, src, value, access_length == 1U ? RV_MEM_ACCESS_8_BIT : RV_MEM_ACCESS_16_BIT);
 		break;
 	}
 	case RV_MEM_ACCESS_32_BIT: {
 		uint32_t value = 0;
+		/* Run the 32-bit native read, storing the result in `value` */
 		riscv32_sysbus_mem_native_read(
-			hart, &value, src & alignment, access_length, RV_MEM_ACCESS_32_BIT, access_length);
-		adiv5_unpack_data(dest, src & 3U, value, access_width);
+			hart, &value, src & alignment, native_access_length, RV_MEM_ACCESS_32_BIT, native_access_length);
+
+		char *data = (char *)dest;
+		/* Figure out from the access length the initial unpack and adjustment */
+		const uint8_t adjustment = access_length & (uint8_t)~1U;
+		/* Having completed the read, unpack the first part of the data - 2 or 4 bytes */
+		if (adjustment) {
+			const uint8_t unpack_alignment = access_length < 4 ? RV_MEM_ACCESS_16_BIT : RV_MEM_ACCESS_32_BIT;
+			data = adiv5_unpack_data(data, src, value, unpack_alignment);
+		}
+		/* Now unpack the remaining byte if necessary */
+		if (access_length & 1U)
+			adiv5_unpack_data(data, src + adjustment, value, RV_MEM_ACCESS_8_BIT);
 		break;
 	}
 	}
@@ -360,16 +374,25 @@ static void riscv32_sysbus_mem_read(
 		++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 read to make it aligned */
+	const target_addr_t length_adjustment = src & (native_access_length - 1U);
+	/*
+	 * Having done this, figure out how long the resulting read actually is so we can fill enough of the
+	 * destination buffer with a single read
+	 */
+	const uint8_t read_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 = src;
 	uint8_t *data = (uint8_t *)dest;
-	riscv32_sysbus_mem_adjusted_read(hart, data, address, access_width, native_access_width, native_access_length);
+	riscv32_sysbus_mem_adjusted_read(hart, data, address, read_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 -= access_length;
-	address += access_length;
-	data += access_length;
+	remainder -= read_length;
+	address += read_length;
+	data += read_length;
 
 	/*
 	 * Now we're aligned to the wider access width, do another set of reads if there's
@@ -386,7 +409,7 @@ static void riscv32_sysbus_mem_read(
 
 	/* If there's any data left to read, do another adjusted access to grab it */
 	if (remainder)
-		riscv32_sysbus_mem_adjusted_read(hart, data, address, access_width, native_access_width, native_access_length);
+		riscv32_sysbus_mem_adjusted_read(hart, data, address, remainder, native_access_width, native_access_length);
 }
 
 static void riscv32_sysbus_mem_native_write(riscv_hart_s *const hart, const target_addr_t dest, const void *const src,