From 602f09e7fb018fac56555ed1ed50407a8f60499f Mon Sep 17 00:00:00 2001
From: Michael S. Tsirkin <mst@redhat.com>
Date: Wed, 27 Jan 2010 12:19:45 -0200
Subject: [PATCH 2/5] qemu: add routines for atomic 16 bit accesses

RH-Author: Michael S. Tsirkin <mst@redhat.com>
Message-id: <20100127121945.GB7223@redhat.com>
Patchwork-id: 6702
O-Subject: [PATCHv3-repost 1/2] qemu: add routines for atomic 16 bit accesses
Bugzilla: 525323
RH-Acked-by: Markus Armbruster <armbru@redhat.com>
RH-Acked-by: Paolo Bonzini <pbonzini@redhat.com>
RH-Acked-by: Juan Quintela <quintela@redhat.com>

Signed-off-by: Michael S. Tsirkin <mst@redhat.com>
---
 qemu/cpu-all.h |    2 +
 qemu/exec.c    |   68 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 70 insertions(+), 0 deletions(-)

Signed-off-by: Eduardo Habkost <ehabkost@redhat.com>
---
 qemu/cpu-all.h |    2 +
 qemu/exec.c    |   68 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 70 insertions(+), 0 deletions(-)

diff --git a/qemu/cpu-all.h b/qemu/cpu-all.h
index 7a0539b..4c3a669 100644
--- a/qemu/cpu-all.h
+++ b/qemu/cpu-all.h
@@ -925,6 +925,7 @@ static inline void cpu_physical_memory_write(target_phys_addr_t addr,
     cpu_physical_memory_rw(addr, (uint8_t *)buf, len, 1);
 }
 uint32_t ldub_phys(target_phys_addr_t addr);
+uint32_t lduw_phys_atomic(target_phys_addr_t addr);
 uint32_t lduw_phys(target_phys_addr_t addr);
 uint32_t ldl_phys(target_phys_addr_t addr);
 uint64_t ldq_phys(target_phys_addr_t addr);
@@ -932,6 +933,7 @@ void stl_phys_notdirty(target_phys_addr_t addr, uint32_t val);
 void stq_phys_notdirty(target_phys_addr_t addr, uint64_t val);
 void stb_phys(target_phys_addr_t addr, uint32_t val);
 void stw_phys(target_phys_addr_t addr, uint32_t val);
+void stw_phys_atomic(target_phys_addr_t addr, uint32_t val);
 void stl_phys(target_phys_addr_t addr, uint32_t val);
 void stq_phys(target_phys_addr_t addr, uint64_t val);
 
diff --git a/qemu/exec.c b/qemu/exec.c
index 68e9fa8..becc8da 100644
--- a/qemu/exec.c
+++ b/qemu/exec.c
@@ -3175,6 +3175,38 @@ uint32_t lduw_phys(target_phys_addr_t addr)
     return tswap16(val);
 }
 
+/* warning: addr must be aligned */
+uint32_t lduw_phys_atomic(target_phys_addr_t addr)
+{
+    int io_index;
+    uint8_t *ptr;
+    uint64_t val;
+    unsigned long pd;
+    PhysPageDesc *p;
+
+    p = phys_page_find(addr >> TARGET_PAGE_BITS);
+    if (!p) {
+        pd = IO_MEM_UNASSIGNED;
+    } else {
+        pd = p->phys_offset;
+    }
+
+    if ((pd & ~TARGET_PAGE_MASK) > IO_MEM_ROM &&
+        !(pd & IO_MEM_ROMD)) {
+        /* I/O case */
+        io_index = (pd >> IO_MEM_SHIFT) & (IO_MEM_NB_ENTRIES - 1);
+        if (p)
+            addr = (addr & ~TARGET_PAGE_MASK) + p->region_offset;
+        val = io_mem_read[io_index][1](io_mem_opaque[io_index], addr);
+    } else {
+        /* RAM case */
+        ptr = phys_ram_base + (pd & TARGET_PAGE_MASK) +
+            (addr & ~TARGET_PAGE_MASK);
+        val = lduw_p(ptr);
+    }
+    return val;
+}
+
 #ifdef __GNUC__
 #define likely(x) __builtin_expect(!!(x), 1)
 #define unlikely(x) __builtin_expect(!!(x), 0)
@@ -3304,6 +3336,42 @@ void stw_phys(target_phys_addr_t addr, uint32_t val)
     cpu_physical_memory_write(addr, (const uint8_t *)&v, 2);
 }
 
+/* warning: addr must be aligned */
+void stw_phys_atomic(target_phys_addr_t addr, uint32_t val)
+{
+    int io_index;
+    uint8_t *ptr;
+    unsigned long pd;
+    PhysPageDesc *p;
+
+    p = phys_page_find(addr >> TARGET_PAGE_BITS);
+    if (!p) {
+        pd = IO_MEM_UNASSIGNED;
+    } else {
+        pd = p->phys_offset;
+    }
+
+    if ((pd & ~TARGET_PAGE_MASK) != IO_MEM_RAM) {
+        io_index = (pd >> IO_MEM_SHIFT) & (IO_MEM_NB_ENTRIES - 1);
+        if (p)
+            addr = (addr & ~TARGET_PAGE_MASK) + p->region_offset;
+        io_mem_write[io_index][1](io_mem_opaque[io_index], addr, val);
+    } else {
+        unsigned long addr1;
+        addr1 = (pd & TARGET_PAGE_MASK) + (addr & ~TARGET_PAGE_MASK);
+        /* RAM case */
+        ptr = phys_ram_base + addr1;
+        stw_p(ptr, val);
+        if (!cpu_physical_memory_is_dirty(addr1)) {
+            /* invalidate code */
+            tb_invalidate_phys_page_range(addr1, addr1 + 2, 0);
+            /* set dirty bit */
+            phys_ram_dirty[addr1 >> TARGET_PAGE_BITS] |=
+                (0xff & ~CODE_DIRTY_FLAG);
+        }
+    }
+}
+
 /* XXX: optimize */
 void stq_phys(target_phys_addr_t addr, uint64_t val)
 {
-- 
1.6.3.rc4.29.g8146