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") } }