From 028ef7d7768398d871041d36fa85b5b799f5895a Mon Sep 17 00:00:00 2001
From: Martin Stein <martin.stein@genode-labs.com>
Date: Tue, 30 Apr 2013 13:41:34 +0200
Subject: [PATCH] mmio: fix bug in read/write array items

Fix #668
---
 base/include/util/mmio.h        | 29 +++++++++++++++++++++--------
 base/src/test/util_mmio/main.cc | 18 ++++++++++++++++++
 2 files changed, 39 insertions(+), 8 deletions(-)

diff --git a/base/include/util/mmio.h b/base/include/util/mmio.h
index 058439f094..39a4e01be6 100644
--- a/base/include/util/mmio.h
+++ b/base/include/util/mmio.h
@@ -207,9 +207,9 @@ namespace Genode
 				 *                targeted by 'offset'.
 				 * \param index   index of the targeted array item
 				 */
-				static inline void access_dest(off_t & offset,
-				                               unsigned long & shift,
-				                               unsigned long const index)
+				static inline void dst(off_t & offset,
+				                       unsigned long & shift,
+				                       unsigned long const index)
 				{
 					unsigned long const bit_off = index << ITEM_WIDTH_LOG2;
 					offset  = (off_t) ((bit_off >> BYTE_WIDTH_LOG2)
@@ -217,6 +217,20 @@ namespace Genode
 					shift   = bit_off - ( offset << BYTE_WIDTH_LOG2 );
 					offset += OFFSET;
 				}
+
+				/**
+				 * Calc destination of a simple array-item access without shift
+				 *
+				 * \param offset  gets overridden with the offset of the the
+				 *                access destination, relative to the MMIO base
+				 * \param index   index of the targeted array item
+				 */
+				static inline void simple_dst(off_t & offset,
+				                              unsigned long const index)
+				{
+					offset  = (index << ITEM_WIDTH_LOG2) >> BYTE_WIDTH_LOG2;
+					offset += OFFSET;
+				}
 			};
 
 			addr_t const base; /* base address of targeted MMIO region */
@@ -337,13 +351,13 @@ namespace Genode
 				/* if item width equals access width we optimize the access */
 				off_t offset;
 				if (Array::ITEM_WIDTH == Array::ACCESS_WIDTH) {
-					offset = Array::OFFSET + (index << Array::ITEM_WIDTH_LOG2);
+					Array::simple_dst(offset, index);
 					return _read<access_t>(offset);
 
 				/* access width and item width differ */
 				} else {
 					long unsigned shift;
-					Array::access_dest(offset, shift, index);
+					Array::dst(offset, shift, index);
 					return (_read<access_t>(offset) >> shift) &
 					       Array::ITEM_MASK;
 				}
@@ -369,14 +383,13 @@ namespace Genode
 				/* optimize the access if item width equals access width */
 				off_t offset;
 				if (Array::ITEM_WIDTH == Array::ACCESS_WIDTH) {
-					offset = Array::OFFSET +
-					         (index << Array::ITEM_WIDTH_LOG2);
+					Array::simple_dst(offset, index);
 					_write<access_t>(offset, value);
 
 				/* access width and item width differ */
 				} else {
 					long unsigned shift;
-					Array::access_dest(offset, shift, index);
+					Array::dst(offset, shift, index);
 
 					/* insert new value into old register value */
 					access_t write_value;
diff --git a/base/src/test/util_mmio/main.cc b/base/src/test/util_mmio/main.cc
index 4c5a17e083..e17a2a44a2 100644
--- a/base/src/test/util_mmio/main.cc
+++ b/base/src/test/util_mmio/main.cc
@@ -110,6 +110,10 @@ struct Test_mmio : public Mmio
 		struct B : Bitfield<2,4> { };
 	};
 
+	struct Simple_array_1 : Register_array<0x0, 32, 2, 32> { };
+
+	struct Simple_array_2 : Register_array<0x2, 16, 4, 16> { };
+
 	struct Strict_reg : Register<0x0, 32, true>
 	{
 		struct A : Bitfield<3,2> { };
@@ -400,6 +404,20 @@ int main()
 	if (compare_mem(mmio_mem, mmio_cmpr_15, sizeof(mmio_mem))) {
 		return test_failed(15); }
 
+	/**
+	 * Test 16, writing to simple register array
+	 */
+	zero_mem(mmio_mem, sizeof(mmio_mem));
+	*(uint8_t*)((addr_t)mmio_mem + sizeof(uint16_t)) = 0xaa;
+	mmio.write<Test_mmio::Simple_array_1>(0x12345678, 0);
+	mmio.write<Test_mmio::Simple_array_1>(0x87654321, 1);
+
+	mmio.write<Test_mmio::Simple_array_2>(0xfedc, 0);
+	mmio.write<Test_mmio::Simple_array_2>(0xabcd, 2);
+	static uint8_t mmio_cmpr_16[MMIO_SIZE] = {0x78,0x56,0xdc,0xfe,0x21,0x43,0xcd,0xab};
+	if (compare_mem(mmio_mem, mmio_cmpr_16, sizeof(mmio_mem))) {
+	  return test_failed(16); }
+
 	printf("Test done\n");
 	return 0;
 }