zynq-rs/libboard_zynq/src/uart/baud_rate_gen.rs

47 lines
1.2 KiB
Rust

use libregister::*;
use super::regs::{RegisterBlock, BaudRateGen, BaudRateDiv};
const BDIV_MIN: u32 = 4;
const BDIV_MAX: u32 = 255;
const CD_MAX: u16 = 65535;
fn div_round_closest(q: u32, d: u32) -> u32 {
(q + (d / 2)) / d
}
/// Algorithm as in the Linux 5.1 driver
pub fn configure(regs: &mut 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 cd = div_round_closest(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")
}
}