1 files changed, 31 insertions, 9 deletions

diff --git a/extmod/modmachine.c b/extmod/modmachine.c
index 7219787965..b9ebee120e 100644
--- a/extmod/modmachine.c
+++ b/extmod/modmachine.c
@@ -29,22 +29,44 @@
 
 #include "py/nlr.h"
 #include "py/obj.h"
+#if MICROPY_PLAT_DEV_MEM
+#include <errno.h>
+#include <fcntl.h>
+#include <sys/mman.h>
+#endif
 
 #if MICROPY_PY_MACHINE
 
-STATIC mp_uint_t get_read_addr(mp_obj_t addr_o, uint align) {
+#define PAGE_SIZE 4096
+#define PAGE_MASK (PAGE_SIZE - 1)
+
+STATIC mp_uint_t get_addr(mp_obj_t addr_o, uint align) {
     mp_uint_t addr = mp_obj_int_get_truncated(addr_o);
     if ((addr & (align - 1)) != 0) {
         nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_ValueError, "address %08x is not aligned to %d bytes", addr, align));
     }
-    return addr;
-}
+    #if MICROPY_PLAT_DEV_MEM
+    {
+        // Not thread-safe
+        static int fd;
+        static mp_uint_t last_base = (mp_uint_t)-1;
+        static mp_uint_t map_page;
+        if (!fd) {
+            fd = open("/dev/mem", O_RDWR | O_SYNC);
+            if (fd == -1) {
+                nlr_raise(mp_obj_new_exception_arg1(&mp_type_OSError, MP_OBJ_NEW_SMALL_INT(errno)));
+            }
+        }
 
-STATIC mp_uint_t get_write_addr(mp_obj_t addr_o, uint align) {
-    mp_uint_t addr = mp_obj_int_get_truncated(addr_o);
-    if ((addr & (align - 1)) != 0) {
-        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_ValueError, "address %08x is not aligned to %d bytes", addr, align));
+        mp_uint_t cur_base = addr & ~PAGE_MASK;
+        if (cur_base != last_base) {
+            map_page = (mp_uint_t)mmap(NULL, PAGE_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, cur_base);
+            last_base = cur_base;
+        }
+        addr = map_page + (addr & PAGE_MASK);
     }
+    #endif
+
     return addr;
 }
 
@@ -67,7 +89,7 @@ STATIC mp_obj_t machine_mem_subscr(mp_obj_t self_in, mp_obj_t index, mp_obj_t va
         return MP_OBJ_NULL; // op not supported
     } else if (value == MP_OBJ_SENTINEL) {
         // load
-        mp_uint_t addr = get_read_addr(index, self->elem_size);
+        mp_uint_t addr = get_addr(index, self->elem_size);
         uint32_t val;
         switch (self->elem_size) {
             case 1: val = (*(uint8_t*)addr); break;
@@ -77,7 +99,7 @@ STATIC mp_obj_t machine_mem_subscr(mp_obj_t self_in, mp_obj_t index, mp_obj_t va
         return mp_obj_new_int(val);
     } else {
         // store
-        mp_uint_t addr = get_write_addr(index, self->elem_size);
+        mp_uint_t addr = get_addr(index, self->elem_size);
         uint32_t val = mp_obj_get_int(value);
         switch (self->elem_size) {
             case 1: (*(uint8_t*)addr) = val; break;