tools: io: do aligned read/write mapping

Align the mmap by pagesize.

Tested: Verified alignment is correct and mmap succeeds where it would
fail on a mis-aligned call.
Signed-off-by: Patrick Venture <venture@google.com>
Change-Id: I76a4f94415c9337d8ca983e24a18e3e4fc701564

1 files changed, 23 insertions, 6 deletions

diff --git a/tools/io.cpp b/tools/io.cpp
index 75677f4..cc9e7d1 100644
--- a/tools/io.cpp
+++ b/tools/io.cpp
@@ -27,16 +27,24 @@ bool DevMemDevice::read(const std::size_t offset, const std::size_t length,
         opened = true;
     }
 
+    /* Map based on aligned addresses - behind the scenes. */
+    const std::size_t alignedDiff = offset % sys->getpagesize();
+    const std::size_t alignedOffset = offset - alignedDiff;
+    const std::size_t alignedSize = length + alignedDiff;
+
     // addr, length, prot, flags, fd, offset
-    devMemMapped =
-        sys->mmap(0, length, PROT_WRITE, MAP_SHARED, devMemFd, offset);
+    devMemMapped = sys->mmap(0, alignedSize, PROT_READ, MAP_SHARED, devMemFd,
+                             alignedOffset);
     if (devMemMapped == MAP_FAILED)
     {
         return false; /* but leave the file open. */
     }
 
+    void* alignedSource =
+        static_cast<std::uint8_t*>(devMemMapped) + alignedDiff;
+
     /* Copy the bytes. */
-    std::memcpy(destination, devMemMapped, length);
+    std::memcpy(destination, alignedSource, length);
 
     /* Close the map between reads for now. */
     sys->munmap(devMemMapped, length);
@@ -58,16 +66,25 @@ bool DevMemDevice::write(const std::size_t offset, const std::size_t length,
         opened = true;
     }
 
+    /* Map based on aligned addresses - behind the scenes. */
+    const std::size_t alignedDiff = offset % sys->getpagesize();
+    const std::size_t alignedOffset = offset - alignedDiff;
+    const std::size_t alignedSize = length + alignedDiff;
+
     // addr, length, prot, flags, fd, offset
-    devMemMapped =
-        sys->mmap(0, length, PROT_WRITE, MAP_SHARED, devMemFd, offset);
+    devMemMapped = sys->mmap(0, alignedSize, PROT_WRITE, MAP_SHARED, devMemFd,
+                             alignedOffset);
+
     if (devMemMapped == MAP_FAILED)
     {
         return false; /* but leave the file open. */
     }
 
+    void* alignedDestination =
+        static_cast<std::uint8_t*>(devMemMapped) + alignedDiff;
+
     /* Copy the bytes. */
-    std::memcpy(devMemMapped, source, length);
+    std::memcpy(alignedDestination, source, length);
 
     /* Close the map between writes for now. */
     sys->munmap(devMemMapped, length);