forked from M-Labs/zynq-rs
47 lines
1.2 KiB
Rust
47 lines
1.2 KiB
Rust
use crate::regs::*;
|
|
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")
|
|
}
|
|
}
|