From 8383633f51b6b3b51fff4047c2122f6eb0d9df89 Mon Sep 17 00:00:00 2001
From: longfangsong <longfangsong@icloud.com>
Date: Mon, 22 Feb 2021 07:25:12 +0800
Subject: [PATCH] use the right APB

---
 src/spi.rs    |  6 +++---
 src/sysctl.rs | 34 +++++++++++++++++++++++++++++++---
 2 files changed, 34 insertions(+), 6 deletions(-)

diff --git a/src/spi.rs b/src/spi.rs
index 3cab426..1acb955 100644
--- a/src/spi.rs
+++ b/src/spi.rs
@@ -3,7 +3,7 @@
 use crate::clock::Clocks;
 use crate::pac::spi0::ctrlr0::TMOD_A as transfer_mode;
 use crate::pac::SPI0;
-use crate::sysctl::{self, APB0};
+use crate::sysctl::{self, APB2};
 use core::convert::Infallible;
 pub use embedded_hal::spi::{Mode, Phase, Polarity};
 
@@ -23,7 +23,7 @@ impl Spi<SPI0> {
         frame_format: FrameFormat,
         endian: Endian,
         clock: &Clocks,
-        apb0: &mut APB0,
+        apb2: &mut APB2,
     ) -> Self {
         let work_mode = hal_mode_to_pac(mode);
         let frame_format = frame_format_to_pac(frame_format);
@@ -54,7 +54,7 @@ impl Spi<SPI0> {
             spi.endian.write(|w| w.bits(endian));
         }
         // enable APB0 bus
-        apb0.enable();
+        apb2.enable();
         // enable peripheral via sysctl
         sysctl::clk_en_peri().modify(|_r, w| w.spi0_clk_en().set_bit());
         Spi { spi, cs_id }
diff --git a/src/sysctl.rs b/src/sysctl.rs
index 2440436..8c36b95 100644
--- a/src/sysctl.rs
+++ b/src/sysctl.rs
@@ -135,9 +135,37 @@ impl APB0 {
 //     _ownership: ()
 // }
 
-// pub struct APB2 {
-//     _ownership: ()
-// }
+pub struct APB2 {
+    _ownership: (),
+}
+
+impl APB2 {
+    pub(crate) fn enable(&mut self) {
+        clk_en_cent().modify(|_r, w| w.apb2_clk_en().set_bit());
+    }
+
+    pub fn set_frequency(&mut self, expected_freq: impl Into<Hertz>) -> Hertz {
+        let aclk = ACLK::steal();
+        let aclk_frequency = aclk.get_frequency().0 as i64;
+        // apb2_frequency = aclk_frequency / (apb2_clk_sel + 1)
+        let apb2_clk_sel = (aclk_frequency / expected_freq.into().0 as i64 - 1)
+            .max(0)
+            .min(0b111) as u8;
+        unsafe {
+            sysctl()
+                .clk_sel0
+                .modify(|_, w| w.apb2_clk_sel().bits(apb2_clk_sel));
+        }
+        Hertz(aclk_frequency as u32 / (apb2_clk_sel as u32 + 1))
+    }
+
+    pub fn get_frequency(&self) -> Hertz {
+        let aclk = ACLK::steal();
+        let aclk_frequency = aclk.get_frequency().0 as i64;
+        let apb2_clk_sel = sysctl().clk_sel0.read().apb2_clk_sel().bits();
+        Hertz(aclk_frequency as u32 / (apb2_clk_sel as u32 + 1))
+    }
+}
 
 /// PLL0, which source is CLOCK_FREQ_IN0,
 /// and the output can be used on ACLK(CPU), SPIs, etc.