1 files changed, 28 insertions, 25 deletions

diff --git a/rust/hw/char/pl011/src/device.rs b/rust/hw/char/pl011/src/device.rs
index bf88e0b..bb2a0f2 100644
--- a/rust/hw/char/pl011/src/device.rs
+++ b/rust/hw/char/pl011/src/device.rs
@@ -190,25 +190,7 @@ impl PL011Registers {
 
         let mut update = false;
         let result = match offset {
-            DR => {
-                self.flags.set_receive_fifo_full(false);
-                let c = self.read_fifo[self.read_pos];
-                if self.read_count > 0 {
-                    self.read_count -= 1;
-                    self.read_pos = (self.read_pos + 1) & (self.fifo_depth() - 1);
-                }
-                if self.read_count == 0 {
-                    self.flags.set_receive_fifo_empty(true);
-                }
-                if self.read_count + 1 == self.read_trigger {
-                    self.int_level &= !Interrupt::RX.0;
-                }
-                // Update error bits.
-                self.receive_status_error_clear.set_from_data(c);
-                // Must call qemu_chr_fe_accept_input
-                update = true;
-                u32::from(c)
-            }
+            DR => self.read_data_register(&mut update),
             RSR => u32::from(self.receive_status_error_clear),
             FR => u32::from(self.flags),
             FBRD => self.fbrd,
@@ -239,12 +221,7 @@ impl PL011Registers {
         // eprintln!("write offset {offset} value {value}");
         use RegisterOffset::*;
         match offset {
-            DR => {
-                // interrupts always checked
-                let _ = self.loopback_tx(value.into());
-                self.int_level |= Interrupt::TX.0;
-                return true;
-            }
+            DR => return self.write_data_register(value),
             RSR => {
                 self.receive_status_error_clear = 0.into();
             }
@@ -306,6 +283,32 @@ impl PL011Registers {
         false
     }
 
+    fn read_data_register(&mut self, update: &mut bool) -> u32 {
+        self.flags.set_receive_fifo_full(false);
+        let c = self.read_fifo[self.read_pos];
+
+        if self.read_count > 0 {
+            self.read_count -= 1;
+            self.read_pos = (self.read_pos + 1) & (self.fifo_depth() - 1);
+        }
+        if self.read_count == 0 {
+            self.flags.set_receive_fifo_empty(true);
+        }
+        if self.read_count + 1 == self.read_trigger {
+            self.int_level &= !Interrupt::RX.0;
+        }
+        self.receive_status_error_clear.set_from_data(c);
+        *update = true;
+        u32::from(c)
+    }
+
+    fn write_data_register(&mut self, value: u32) -> bool {
+        // interrupts always checked
+        let _ = self.loopback_tx(value.into());
+        self.int_level |= Interrupt::TX.0;
+        true
+    }
+
     #[inline]
     #[must_use]
     fn loopback_tx(&mut self, value: registers::Data) -> bool {