From 0543e989561335e6c5a9db1ff6a928b499b8a7f1 Mon Sep 17 00:00:00 2001
From: occheung <dc@m-labs.hk>
Date: Wed, 12 Aug 2020 11:50:24 +0800
Subject: [PATCH] cfg_reg: add setter function

---
 .cargo/config          |  2 +-
 src/config_register.rs | 27 +++++++++++++++++++++------
 src/main.rs            | 20 ++++++++++++++++++--
 3 files changed, 40 insertions(+), 9 deletions(-)

diff --git a/.cargo/config b/.cargo/config
index c0ce242..6c91033 100644
--- a/.cargo/config
+++ b/.cargo/config
@@ -1,5 +1,5 @@
 [target.thumbv7em-none-eabihf]
-runner = "gdb -q -x gdb_config/fpga_config.gdb"
+runner = "gdb -q -x gdb_config/debug.gdb"
 rustflags = [
   "-C", "link-arg=-Tlink.x",
 ]
diff --git a/src/config_register.rs b/src/config_register.rs
index d4a2f73..8871069 100644
--- a/src/config_register.rs
+++ b/src/config_register.rs
@@ -1,4 +1,5 @@
 use embedded_hal::blocking::spi::Transfer;
+use cortex_m_semihosting::hprintln;
 use crate::Error;
 
 /*
@@ -6,6 +7,7 @@ use crate::Error;
  */
 macro_rules! construct_bitmask {
 	($collection: ident; $($name: ident, $shift: expr, $width: expr),+) => {
+		#[derive(Debug, Copy, Clone)]
 		pub enum $collection {
 			$(
 				$name,
@@ -33,7 +35,7 @@ macro_rules! construct_bitmask {
 						$collection::$name => {
 							let mut mask: u32 = 0;
 							for bit in 0..$width {
-								mask != 1 << ($width + bit);
+								mask |= (1 << ($shift + bit));
 							}
 							mask
 						},
@@ -80,17 +82,30 @@ where
 		}
 	}
 
-	pub fn set_configuration(&mut self, config: u32) -> Result<u32, Error<E>> {
-		self.data = config & 0x00FFFFFF;
+	fn set_all_configurations(&mut self) -> Result<u32, Error<E>> {
 		match self.spi.transfer(&mut [
-			((config & 0x00FF0000) >> 16) as u8,
-			((config & 0x0000FF00) >>  8) as u8,
-			((config & 0x000000FF) >>  0) as u8,
+			((self.data & 0x00FF0000) >> 16) as u8,
+			((self.data & 0x0000FF00) >>  8) as u8,
+			((self.data & 0x000000FF) >>  0) as u8,
 		]).map_err(Error::SPI) {
 			Ok(arr) => Ok(((arr[0] as u32) << 16) | ((arr[1] as u32) << 8) | arr[2] as u32),
 			Err(e) => Err(e),
 		}
 	}
+
+	pub fn set_configurations(&mut self, configs: &mut[(CFGMask, u32)]) -> Result<u32, Error<E>> {
+		for config in configs.into_iter() {
+			// Erase the bits in the configuration region
+			self.data &= (!config.0.get_bitmask());
+			// Check validity of config data
+			let shifted_config: u32 = config.1 << config.0.get_shift();
+			assert_eq!(shifted_config | config.0.get_bitmask(), config.0.get_bitmask());
+			// Write the configuration onto local data
+			self.data |= shifted_config;
+		}
+		// Write all configurations at the same time
+		self.set_all_configurations()
+	}
 }
 
 impl<SPI, E> Transfer<u8> for ConfigRegister<SPI>
diff --git a/src/main.rs b/src/main.rs
index 7f39442..3581a00 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -21,7 +21,10 @@ use firmware;
 use firmware::{
 	CPLD,
 	attenuator::Attenuator,
-	config_register::ConfigRegister,
+	config_register::{
+		ConfigRegister,
+		CFGMask,
+	},
 };
 
 #[entry]
@@ -92,7 +95,20 @@ fn main() -> ! {
 	attenuator.set_channel_attenuation(2, 15.3);
 
 	let mut config = ConfigRegister::new(parts.spi1);
-	hprintln!("{}", config.set_configuration(0xDEADBEEF).unwrap()).unwrap();
+	// Target configuration: 0x000FF1CE
+	hprintln!("{}", config.set_configurations(&mut [
+		(CFGMask::RF_SW,		0xE),
+		(CFGMask::LED,			0xC),
+		(CFGMask::PROFILE,		0x1),
+		(CFGMask::IO_UPDATE,	0x1),
+		(CFGMask::MASK_NU,		0xF),
+		(CFGMask::CLK_SEL0,		0x1),
+		(CFGMask::SYNC_SEL,		0x1),
+		(CFGMask::RST,			0x1),
+		(CFGMask::IO_RST,		0x0),
+		(CFGMask::CLK_SEL1,		0x0),
+		(CFGMask::DIV,			0x0),
+	]).unwrap()).unwrap();
 
 	loop {
 		nop();