diff --git a/src/uart/baud_rate_gen.rs b/src/uart/baud_rate_gen.rs
new file mode 100644
index 00000000..84bb60d3
--- /dev/null
+++ b/src/uart/baud_rate_gen.rs
@@ -0,0 +1,43 @@
+use crate::regs::*;
+use super::regs::{RegisterBlock, BaudRateGen, BaudRateDiv};
+
+const BDIV_MIN: u8 = 4;
+const BDIV_MAX: u8 = 255;
+const CD_MAX: u16 = 65535;
+
+/// Algorithm as in the Linux 5.1 driver
+pub fn configure(regs: &RegisterBlock, mut clk: u32, baud: u32) {
+    if regs.mode.read().clks() {
+        clk /= 8;
+    }
+
+    let mut best = None;
+    for bdiv in BDIV_MIN..=BDIV_MAX {
+        let bdiv: u32 = bdiv.into();
+        let cd = clk / (baud * (bdiv + 1));
+        if cd < 1 || cd > CD_MAX.into() {
+            continue;
+        }
+
+        let actual_baud = clk / (cd * (bdiv + 1));
+        let error = if baud > actual_baud {
+            baud - actual_baud
+        } else {
+            actual_baud - baud
+        };
+        let better = best
+            .map(|(_cd, _bdiv, best_error)| error < best_error)
+            .unwrap_or(true);
+        if better {
+            best = Some((cd as u16, bdiv as u8, error));
+        }
+    }
+
+    match best {
+        Some((cd, bdiv, error)) => {
+            regs.baud_rate_gen.write(BaudRateGen::zeroed().cd(cd));
+            regs.baud_rate_divider.write(BaudRateDiv::zeroed().bdiv(bdiv));
+        }
+        None => panic!("Cannot configure baud rate")
+    }
+}
diff --git a/src/uart/mod.rs b/src/uart/mod.rs
index dc690f06..a146e418 100644
--- a/src/uart/mod.rs
+++ b/src/uart/mod.rs
@@ -6,6 +6,7 @@ use volatile_register::RW;
 use crate::regs::*;
 
 mod regs;
+mod baud_rate_gen;
 
 pub struct Uart {
     regs: &'static mut regs::RegisterBlock,
@@ -49,12 +50,12 @@ impl Uart {
     }
 
     pub fn configure(&self) {
-        // Confiugre UART character frame
+        // Configure UART character frame
         // * Disable clock-divider
         // * 8-bit
         // * 1 stop bit
         // * Normal channel mode
-        // * no parity
+        // * No parity
         let parity_mode = regs::ParityMode::None;
         self.regs.mode.write(
             regs::Mode::zeroed()
@@ -66,12 +67,7 @@ impl Uart {
         self.disable_rx();
         self.disable_tx();
 
-        // 9,600 baud
-        self.regs.baud_rate_gen.write(regs::BaudRateGen::zeroed().cd(651));
-        self.regs.baud_rate_divider.write(regs::BaudRateDiv::zeroed().bdiv(7));
-        // // 115,200 baud
-        // self.regs.baud_rate_gen.write(regs::BaudRateGen::zeroed().cd(62));
-        // self.regs.baud_rate_divider.write(regs::BaudRateDiv::zeroed().bdiv(6));
+        baud_rate_gen::configure(&self.regs, 50_000_000, 9_600);
 
         // Enable controller
         self.reset_rx();