6 files changed, 200 insertions, 3 deletions

diff --git a/utils/hwstub/stub/SOURCES b/utils/hwstub/stub/SOURCES
index 1b0b56072d..c91580c966 100644
--- a/utils/hwstub/stub/SOURCES
+++ b/utils/hwstub/stub/SOURCES
@@ -2,9 +2,11 @@
 asm/arm/memcpy.S
 asm/arm/memmove.S
 asm/arm/memset.S
+asm/arm/atomic_rw.S
 #elif defined(CPU_MIPS)
 asm/mips/memcpy.S
 asm/mips/memset.S
+asm/mips/atomic_rw.S
 #else
 #error "Unimplemented ISA"
 #endif
diff --git a/utils/hwstub/stub/asm/arm/atomic_rw.S b/utils/hwstub/stub/asm/arm/atomic_rw.S
new file mode 100644
index 0000000000..cb6272b4dc
--- /dev/null
+++ b/utils/hwstub/stub/asm/arm/atomic_rw.S
@@ -0,0 +1,58 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ *
+ * Copyright (C) 2014 by Marcin Bukat
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+
+    .section   .text, "ax", %progbits
+    .global    target_read8
+    .type      target_read8, %function
+    .global    target_read16
+    .type      target_read16, %function
+    .global    target_read32
+    .type      target_read32, %function
+    .global    target_write8
+    .type      target_write8, %function
+    .global    target_write16
+    .type      target_write16, %function
+    .global    target_write32
+    .type      target_write32, %function
+
+target_read8:
+    ldrb r0, [r0]
+    bx lr
+
+target_read16:
+    ldrh r0, [r0]
+    bx lr
+
+target_read32:
+    ldr r0, [r0]
+    bx lr
+
+target_write8:
+    strb r1, [r0]
+    bx lr
+
+target_write16:
+    strh r1, [r0]
+    bx lr
+
+target_write32:
+    str r1, [r0]
+    bx lr
+
diff --git a/utils/hwstub/stub/asm/mips/atomic_rw.S b/utils/hwstub/stub/asm/mips/atomic_rw.S
new file mode 100644
index 0000000000..47c4213a5d
--- /dev/null
+++ b/utils/hwstub/stub/asm/mips/atomic_rw.S
@@ -0,0 +1,61 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ *
+ * Copyright (C) 2014 by Marcin Bukat
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#include "mips.h"
+
+    .set noreorder
+    .section   .text, "ax", %progbits
+    .global    target_read8
+    .type      target_read8, %function
+    .global    target_read16
+    .type      target_read16, %function
+    .global    target_read32
+    .type      target_read32, %function
+    .global    target_write8
+    .type      target_write8, %function
+    .global    target_write16
+    .type      target_write16, %function
+    .global    target_write32
+    .type      target_write32, %function
+
+target_read8:
+    jr ra
+    lbu v0, 0(a0)
+
+target_read16:
+    jr ra
+    lhu v0, 0(a0)
+
+target_read32:
+    jr ra
+    lw v0, 0(a0)
+
+target_write8:
+    jr ra
+    sb a1, 0(a0)
+
+target_write16:
+    jr ra
+    sh a1, 0(a0)
+
+target_write32:
+    jr ra
+    sw a1, 0(a0)
+
+    .set reorder
diff --git a/utils/hwstub/stub/main.c b/utils/hwstub/stub/main.c
index 4e54b8fd89..3604cfdae0 100644
--- a/utils/hwstub/stub/main.c
+++ b/utils/hwstub/stub/main.c
@@ -356,6 +356,49 @@ static void handle_get_log(struct usb_ctrlrequest *req)
     enable_logf(true);
 }
 
+/* default implementation, relying on the compiler to produce correct code,
+ * targets should reimplement this... */
+uint8_t __attribute__((weak)) target_read8(const void *addr)
+{
+    return *(volatile uint8_t *)addr;
+}
+
+uint16_t __attribute__((weak)) target_read16(const void *addr)
+{
+    return *(volatile uint16_t *)addr;
+}
+
+uint32_t __attribute__((weak)) target_read32(const void *addr)
+{
+    return *(volatile uint32_t *)addr;
+}
+
+void __attribute__((weak)) target_write8(void *addr, uint8_t val)
+{
+    *(volatile uint8_t *)addr = val;
+}
+
+void __attribute__((weak)) target_write16(void *addr, uint16_t val)
+{
+    *(volatile uint16_t *)addr = val;
+}
+
+void __attribute__((weak)) target_write32(void *addr, uint32_t val)
+{
+    *(volatile uint32_t *)addr = val;
+}
+
+static bool read_atomic(void *dst, void *src, size_t sz)
+{
+    switch(sz)
+    {
+        case 1: *(uint8_t *)dst = target_read8(src); return true;
+        case 2: *(uint16_t *)dst = target_read16(src); return true;
+        case 4: *(uint32_t *)dst = target_read32(src); return true;
+        default: return false;
+    }
+}
+
 static void handle_read(struct usb_ctrlrequest *req)
 {
     static uint32_t last_addr = 0;
@@ -377,13 +420,30 @@ static void handle_read(struct usb_ctrlrequest *req)
     {
         if(id != last_id)
             return usb_drv_stall(EP_CONTROL, true, true);
-        memcpy(usb_buffer, (void *)last_addr, req->wLength);
+        if(req->bRequest == HWSTUB_READ2_ATOMIC)
+        {
+            if(!read_atomic(usb_buffer, (void *)last_addr, req->wLength))
+                return usb_drv_stall(EP_CONTROL, true, true);
+        }
+        else
+            memcpy(usb_buffer, (void *)last_addr, req->wLength);
         asm volatile("nop" : : : "memory");
         usb_drv_send(EP_CONTROL, usb_buffer, req->wLength);
         usb_drv_recv(EP_CONTROL, NULL, 0);
     }
 };
 
+static bool write_atomic(void *dst, void *src, size_t sz)
+{
+    switch(sz)
+    {
+        case 1: target_write8(dst, *(uint8_t *)src); return true;
+        case 2: target_write16(dst, *(uint16_t *)src); return true;
+        case 4: target_write32(dst, *(uint32_t *)src); return true;
+        default: return false;
+    }
+}
+
 static void handle_write(struct usb_ctrlrequest *req)
 {
     int size = usb_drv_recv(EP_CONTROL, usb_buffer, req->wLength);
@@ -392,7 +452,13 @@ static void handle_write(struct usb_ctrlrequest *req)
     int sz_hdr = sizeof(struct hwstub_write_req_t);
     if(size < sz_hdr)
         return usb_drv_stall(EP_CONTROL, true, true);
-    memcpy((void *)write->dAddress, usb_buffer + sz_hdr, size - sz_hdr);
+    if(req->bRequest == HWSTUB_WRITE_ATOMIC)
+    {
+        if(!write_atomic((void *)write->dAddress, usb_buffer + sz_hdr, size - sz_hdr))
+            return usb_drv_stall(EP_CONTROL, true, true);
+    }
+    else
+        memcpy((void *)write->dAddress, usb_buffer + sz_hdr, size - sz_hdr);
     usb_drv_send(EP_CONTROL, NULL, 0);
 }
 
@@ -451,8 +517,10 @@ static void handle_class_intf_req(struct usb_ctrlrequest *req)
             return handle_get_log(req);
         case HWSTUB_READ:
         case HWSTUB_READ2:
+        case HWSTUB_READ2_ATOMIC:
             return handle_read(req);
         case HWSTUB_WRITE:
+        case HWSTUB_WRITE_ATOMIC:
             return handle_write(req);
         case HWSTUB_EXEC:
             return handle_exec(req);
diff --git a/utils/hwstub/stub/protocol.h b/utils/hwstub/stub/protocol.h
index e358bb0a36..d551342d4b 100644
--- a/utils/hwstub/stub/protocol.h
+++ b/utils/hwstub/stub/protocol.h
@@ -1,3 +1,3 @@
 #include "../hwstub_protocol.h"
 
-#define HWSTUB_VERSION_REV  1
\ No newline at end of file
+#define HWSTUB_VERSION_REV  0
\ No newline at end of file
diff --git a/utils/hwstub/stub/target.h b/utils/hwstub/stub/target.h
index cb17401a9c..5cd049d04f 100644
--- a/utils/hwstub/stub/target.h
+++ b/utils/hwstub/stub/target.h
@@ -33,6 +33,14 @@ void target_get_config_desc(void *buffer, int *size);
 void target_udelay(int us);
 /* Wait for a short time (ms <= 1000) */
 void target_mdelay(int ms);
+/* Read a n-bit word atomically */
+uint8_t target_read8(const void *addr);
+uint16_t target_read16(const void *addr);
+uint32_t target_read32(const void *addr);
+/* Write a n-bit word atomically */
+void target_write8(void *addr, uint8_t val);
+void target_write16(void *addr, uint16_t val);
+void target_write32(void *addr, uint32_t val);
 
 /* mandatory for all targets */
 extern struct hwstub_target_desc_t target_descriptor;