humpback-dds/src/dds.rs

use embedded_hal::blocking::spi::Transfer;
use crate::urukul::Error;
use core::mem::size_of;
use core::convert::TryInto;
use heapless::Vec;
use heapless::consts::*;
use log::{ trace, debug, warn };

/*
 *  Bitmask for all configurations (Order: CFR3, CFR2, CFR1)
 */
construct_bitmask!(DDSCFRMask; u32;
    // CFR1 bitmasks
    LSB_FIRST,                      0,     1,
    SDIO_IN_ONLY,                   1,     1,
    EXT_POWER_DOWN_CTRL,            3,     1,
    AUX_DAC_POWER_DOWN,             4,     1,
    REFCLK_IN_POWER_DOWN,           5,     1,
    DAC_POWER_DOWN,                 6,     1,
    DIGITAL_POWER_DOWN,             7,     1,
    SEL_AUTO_OSK,                   8,     1,
    OSK_ENABLE,                     9,     1,
    LOAD_ARR_IO_UPDATE,             10,    1,
    CLEAR_PHASE_ACU,                11,    1,
    CLEAR_DIGITAL_RAMP_ACU,         12,    1,
    AUTOCLEAR_PHASE_ACU,            13,    1,
    AUTOCLEAR_DIGITAL_RAMP_ACU,     14,    1,
    LOAD_LRR_IO_UPDATE,             15,    1,
    SEL_DDS_SIN_OUT,                16,    1,
    PROFILE_CTRL,                   17,    4,
    INV_SINC_FILTER_ENABLE,         22,    1,
    MANUAL_OSK_EXT_CTRL,            23,    1,
    RAM_PLAYBACK_DST,               29,    2,
    RAM_ENABLE,                     31,    1,

    // CFR2 bitmasks
    FM_GAIN,                        0  +32,    4,
    PARALLEL_DATA_PORT_ENABLE,      4  +32,    1,
    SYNC_TIM_VALIDATION_DISABLE,    5  +32,    1,
    DATA_ASSEM_HOLD_LAST_VALUE,     6  +32,    1,
    MATCHED_LATENCY_ENABLE,         7  +32,    1,
    TXENABLE_INV,                   9  +32,    1,
    PDCLK_INV,                      10 +32,    1,
    PDCLK_ENABLE,                   11 +32,    1,
    IO_UPDATE_RATE_CTRL,            14 +32,    2,
    READ_EFFECTIVE_FTW,             16 +32,    1,
    DIGITAL_RAMP_NO_DWELL_LOW,      17 +32,    1,
    DIGITAL_RAMP_NO_DWELL_HIGH,     18 +32,    1,
    DIGITAL_RAMP_ENABLE,            19 +32,    1,
    DIGITAL_RAMP_DEST,              20 +32,    2,
    SYNC_CLK_ENABLE,                22 +32,    1,    
    INT_IO_UPDATE_ACTIVE,           23 +32,    1,
    EN_AMP_SCALE_SINGLE_TONE_PRO,   24 +32,    1,

    // CFR3 bitmasks
    N,                              1  +64,    7,
    PLL_ENABLE,                     8  +64,    1,
    PFD_RESET,                      10 +64,    1,
    REFCLK_IN_DIV_RESETB,           14 +64,    1,
    REFCLK_IN_DIV_BYPASS,           15 +64,    1,
    I_CP,                           19 +64,    3,
    VCO_SEL,                        24 +64,    3,
    DRV0,                           28 +64,    2
);

const WRITE_MASK   :u8 = 0x00;
const READ_MASK    :u8 = 0x80;

static mut RAM_VEC: Vec<u8, U8192> = Vec(heapless::i::Vec::new());

#[derive(Clone, PartialEq)]
pub enum RAMDestination {
    Frequency = 0,
    Phase = 1,
    Amplitude = 2,
    Polar = 3,
}

#[derive(Clone)]
pub enum RAMOperationMode {
    DirectSwitch = 0,
    RampUp = 1,
    BidirectionalRamp = 2,
    ContinuousBidirectionalRamp = 3,
    ContinuousRecirculate = 4,
}

pub struct DDS<SPI> {
    spi: SPI,
    f_ref_clk: f64,
    f_sys_clk: f64,
}

impl<SPI, E> DDS<SPI>
where
    SPI: Transfer<u8, Error = E>
{
    pub fn new(spi: SPI, f_ref_clk: f64) -> Self {
        DDS {
            spi,
            f_ref_clk,
            f_sys_clk: f_ref_clk,
        }
    }
}

impl<SPI, E> Transfer<u8> for DDS<SPI>
where
    SPI: Transfer<u8, Error = E>
{
    type Error = Error<E>;

    fn transfer<'w>(&mut self, words: &'w mut [u8]) -> Result<&'w [u8], Self::Error> {
        self.spi.transfer(words).map_err(Error::SPI)
    }
}


impl<SPI, E> DDS<SPI>
where
    SPI: Transfer<u8, Error = E>
{
    /*
     *  Implement init: Set SDIO to be input only, using LSB first
     */
    pub fn init(&mut self) -> Result<(), Error<E>> {
        match self.write_register(0x00, &mut [
            0x00, 0x00, 0x00, 0x02
        ]) {
            Ok(_) => Ok(()),
            Err(e) => Err(e),
        }
    }

    /*
     *  Implement clock control
     */
    pub fn enable_divided_ref_clk(&mut self) -> Result<(), Error<E>> {
        self.set_configurations(&mut [
            // Disable PLL
            (DDSCFRMask::PLL_ENABLE, 0),
            // Take ref_clk source from divider
            (DDSCFRMask::REFCLK_IN_DIV_BYPASS, 0),
            // Ensure divider is not reset
            (DDSCFRMask::REFCLK_IN_DIV_RESETB, 1),
        ])?;
        self.f_sys_clk = self.f_ref_clk / 2.0;
        Ok(())
    }

    pub fn enable_normal_ref_clk(&mut self) -> Result<(), Error<E>> {
        self.set_configurations(&mut [
            // Disable PLL
            (DDSCFRMask::PLL_ENABLE, 0),
            // Take ref_clk source from divider bypass
            (DDSCFRMask::REFCLK_IN_DIV_BYPASS, 1),
            // Reset does not matter
            (DDSCFRMask::REFCLK_IN_DIV_RESETB, 1),
        ])?;
        self.f_sys_clk = self.f_ref_clk;
        Ok(())
    }

    pub fn enable_pll(&mut self, f_sys_clk: f64) -> Result<(), Error<E>> {
        // Get a divider
        let divider = (f_sys_clk / self.f_ref_clk) as u64;
        // Reject extreme divider values. However, accept no frequency division
        if (divider > 127 || divider < 12) && divider != 1 {
            // panic!("Invalid divider value for PLL!");
            return Err(Error::DDSCLKError);
        }
        let vco = self.get_VCO_no(f_sys_clk, divider as u8)?;

        self.set_configurations(&mut [
            // Enable PLL, set divider (valid or not) and VCO
            (DDSCFRMask::PLL_ENABLE, 1),
            (DDSCFRMask::N, divider as u32),
            (DDSCFRMask::VCO_SEL, vco.into()),
            // Reset PLL lock before re-enabling it
            (DDSCFRMask::PFD_RESET, 1),
        ])?;
        self.set_configurations(&mut [
            (DDSCFRMask::PFD_RESET, 0),
        ])?;
        self.f_sys_clk = self.f_ref_clk * (divider as f64);
        Ok(())
    }

    // Change external clock source (ref_clk)
    pub fn set_ref_clk_frequency(&mut self, f_ref_clk: f64) -> Result<(), Error<E>> {
        // Override old reference clock frequency (ref_clk)
        self.f_ref_clk = f_ref_clk;

        // Calculate the new system clock frequency, examine the clock tree
        let mut configuration_queries = [
            // Acquire PLL status
            (DDSCFRMask::PLL_ENABLE, 0),
            // Acquire N-divider, to adjust VCO if necessary
            (DDSCFRMask::N, 0),
            // Acquire REF_CLK divider bypass
            (DDSCFRMask::REFCLK_IN_DIV_BYPASS, 0)
        ];

        self.get_configurations(&mut configuration_queries)?;
        if configuration_queries[0].1 == 1 {
            // Recalculate sys_clk
            let divider :f64 = configuration_queries[1].1.into();
            let f_sys_clk = self.f_ref_clk * divider;
            // Adjust VCO
            match self.get_VCO_no(f_sys_clk, divider as u8) {
                Ok(vco_no) => {
                    self.set_configurations(&mut [
                        // Update VCO selection
                        (DDSCFRMask::VCO_SEL, vco_no.into()),
                        // Reset PLL lock before re-enabling it
                        (DDSCFRMask::PFD_RESET, 1),
                    ])?;
                    self.set_configurations(&mut [
                        (DDSCFRMask::PFD_RESET, 0),
                    ])?;
                    // Update f_sys_clk from recalculation
                    self.f_sys_clk = f_sys_clk;
                    Ok(())
                },
                Err(_) => {
                    // Forcibly turn off PLL, enable default clk tree (divide by 2)
                    self.enable_divided_ref_clk()
                }
            }
        }
        else if configuration_queries[2].1 == 0 {
            self.f_sys_clk = self.f_ref_clk / 2.0;
            Ok(())
        }
        else {
            self.f_sys_clk = self.f_ref_clk;
            Ok(())
        }
    }

    // Change sys_clk frequency, method to be determined
    pub fn set_sys_clk_frequency(&mut self, f_sys_clk: f64) -> Result<(), Error<E>> {
        // If f_sys_clk is exactly the same as f_ref_clk, then invoke enable_normal_ref_clk
        if f_sys_clk == self.f_ref_clk {
            self.enable_normal_ref_clk()
        }
        // Otherwise, if the requested sys_clk is half of ref_clk, invoke enable_divided_ref_clk
        else if f_sys_clk == (self.f_ref_clk / 2.0) {
            self.enable_divided_ref_clk()
        }
        // Finally, try enabling PLL
        else {
            self.enable_pll(f_sys_clk)
        }
    }

    #[allow(non_snake_case)]
    fn get_VCO_no(&mut self, f_sys_clk: f64, divider: u8) -> Result<u8, Error<E>> {
        // Select a VCO
        if divider == 1 {
            Ok(6)    // Bypass PLL if no frequency division needed
        } else if f_sys_clk > 1_150_000_000.0 {
            Err(Error::DDSCLKError)
        } else if f_sys_clk > 820_000_000.0 {
            Ok(5)
        } else if f_sys_clk > 700_000_000.0 {
            Ok(4)
        } else if f_sys_clk > 600_000_000.0 {
            Ok(3)
        } else if f_sys_clk > 500_000_000.0 {
            Ok(2)
        } else if f_sys_clk > 420_000_000.0 {
            Ok(1)
        } else if f_sys_clk > 370_000_000.0 {
            Ok(0)
        } else {
            Ok(7)    // Bypass PLL if f_sys_clk is too low
        }
    }

    /*
     *  Implement configurations registers I/O through bitmasks
     *
     *  Get all (cfr1, cfr2, cfr3) contents
     */
    fn get_all_configurations(&mut self) -> Result<[u32; 3], Error<E>> {
        let mut cfr_reg = [0; 12];
        self.read_register(0x00, &mut cfr_reg[0..4])?;
        self.read_register(0x01, &mut cfr_reg[4..8])?;
        self.read_register(0x02, &mut cfr_reg[8..12])?;
        Ok([
            (cfr_reg[0] as u32) << 24 | (cfr_reg[1] as u32) << 16 | (cfr_reg[2] as u32) << 8 | (cfr_reg[3] as u32),
            (cfr_reg[4] as u32) << 24 | (cfr_reg[5] as u32) << 16 | (cfr_reg[6] as u32) << 8 | (cfr_reg[7] as u32),
            (cfr_reg[8] as u32) << 24 | (cfr_reg[9] as u32) << 16 | (cfr_reg[10] as u32) << 8 | (cfr_reg[11] as u32)
        ])
    }

    /*
     *  Get a set of configurations using DDSCFRMask
     */
    pub fn get_configurations<'w>(&mut self, mask_pairs: &'w mut[(DDSCFRMask, u32)]) -> Result<&'w [(DDSCFRMask, u32)], Error<E>> {
        let data_array = self.get_all_configurations()?;
        for index in 0..mask_pairs.len() {
            mask_pairs[index].1 = match mask_pairs[index].0.get_shift() {
                0..=31 => mask_pairs[index].0.get_filtered_content(data_array[0]),
                32..=63 => mask_pairs[index].0.get_filtered_content(data_array[1]),
                64..=95 => mask_pairs[index].0.get_filtered_content(data_array[2]),
                _ => panic!("Invalid DDSCFRMask!"),
            }
        }
        Ok(mask_pairs)
    }

    /*
     *  Write (cfr1, cfr2, cfr3) contents
     */
    fn set_all_configurations(&mut self, data_array: [u32; 3]) -> Result<(), Error<E>> {
        for register in 0x00..=0x02 {
            self.write_register(register, &mut [
                ((data_array[register as usize] >> 24) & 0xFF) as u8,
                ((data_array[register as usize] >> 16) & 0xFF) as u8,
                ((data_array[register as usize] >> 8 ) & 0xFF) as u8,
                ((data_array[register as usize] >> 0 ) & 0xFF) as u8
            ])?;
        }
        Ok(())
    }

    /*
     *  Set a set of configurations using DDSCFRMask
     */
    pub fn set_configurations(&mut self, mask_pairs: &mut[(DDSCFRMask, u32)]) -> Result<(), Error<E>> {
        let mut data_array = self.get_all_configurations()?;
        for index in 0..mask_pairs.len() {
            // Reject any attempt to rewrite LSB_FIRST and SBIO_INPUT_ONLY
            if mask_pairs[index].0 == DDSCFRMask::LSB_FIRST || mask_pairs[index].0 == DDSCFRMask::SDIO_IN_ONLY {
                continue;
            }
            match mask_pairs[index].0.get_shift() {
                0..=31 => mask_pairs[index].0.set_data_by_arg(&mut data_array[0], mask_pairs[index].1),
                32..=63 => mask_pairs[index].0.set_data_by_arg(&mut data_array[1], mask_pairs[index].1),
                64..=95 => mask_pairs[index].0.set_data_by_arg(&mut data_array[2], mask_pairs[index].1),
                _ => panic!("Invalid DDSCFRMask!"),
            };
        }
        self.set_all_configurations(data_array.clone())
    }

    /*
     *  Setup a complete single tone profile
     *  Phase: Expressed in positive degree, i.e. [0.0, 360.0)
     *  Frequency: Must be non-negative
     *  Amplitude: In a scale from 0 to 1, taking float
     */
    pub fn set_single_tone_profile(&mut self, profile: u8, f_out: f64, phase_offset: f64, amp_scale_factor: f64) -> Result<(), Error<E>> {

        assert!(profile < 8);
        assert!(f_out >= 0.0);
        assert!(phase_offset >= 0.0 && phase_offset < 360.0);
        assert!(amp_scale_factor >=0.0 && amp_scale_factor <= 1.0);

        let ftw = self.frequency_to_ftw(f_out);
        let pow = self.degree_to_pow(phase_offset);
        let asf = self.amplitude_to_asf(amp_scale_factor);

        // Setup configuration registers before writing single tone register
        self.enable_single_tone_configuration()?;
        
        // Transfer single tone profile data
        self.write_register(0x0E + profile, &mut [
            ((asf >> 8 ) & 0xFF) as u8,
            ((asf >> 0 ) & 0xFF) as u8,
            ((pow >> 8 ) & 0xFF) as u8,
            ((pow >> 0 ) & 0xFF) as u8,
            ((ftw >> 24) & 0xFF) as u8,
            ((ftw >> 16) & 0xFF) as u8,
            ((ftw >> 8 ) & 0xFF) as u8,
            ((ftw >> 0 ) & 0xFF) as u8,
        ])
    }

    /*
     *  Set frequency of a single tone profile
     *  Frequency: Must be non-negative
     *    Keep other field unchanged in the register
     */
    pub fn set_single_tone_profile_frequency(&mut self, profile: u8, f_out: f64) -> Result<(), Error<E>> {

        // Setup configuration registers before writing single tone register
        self.enable_single_tone_configuration()?;

        let ftw = self.frequency_to_ftw(f_out);

        // Read existing amplitude/phase data
        let mut register: [u8; 8] = [0; 8];
        self.read_register(0x0E + profile, &mut register)?;

        // Overwrite FTW
        register[4] = ((ftw >> 24) & 0xFF) as u8;
        register[5] = ((ftw >> 16) & 0xFF) as u8;
        register[6] = ((ftw >>  8) & 0xFF) as u8;
        register[7] = ((ftw >>  0) & 0xFF) as u8;

        // Update FTW by writing back the register
        self.write_register(0x0E + profile, &mut register)
    }

    /*
     *  Set phase offset of a single tone profile
     *  Phase: Expressed in positive degree, i.e. [0.0, 360.0)
     *    Keep other field unchanged in the register
     */
    pub fn set_single_tone_profile_phase(&mut self, profile: u8, phase_offset: f64) -> Result<(), Error<E>> {

        // Setup configuration registers before writing single tone register
        self.enable_single_tone_configuration()?;

        let pow = self.degree_to_pow(phase_offset);

        // Read existing amplitude/frequency data
        let mut register: [u8; 8] = [0; 8];
        self.read_register(0x0E + profile, &mut register)?;

        // Overwrite POW
        register[2] = ((pow >>  8) & 0xFF) as u8;
        register[3] = ((pow >>  0) & 0xFF) as u8;

        // Update POW by writing back the register
        self.write_register(0x0E + profile, &mut register)
    }

    /*
     *  Set amplitude offset of a single tone profile
     *  Amplitude: In a scale from 0 to 1, taking float
     *    Keep other field unchanged in the register
     */
    pub fn set_single_tone_profile_amplitude(&mut self, profile: u8, amp_scale_factor: f64) -> Result<(), Error<E>> {

        // Setup configuration registers before writing single tone register
        self.enable_single_tone_configuration()?;

        // Calculate amplitude_scale_factor (ASF)
        let asf = self.amplitude_to_asf(amp_scale_factor);

        // Read existing frequency/phase data
        let mut register: [u8; 8] = [0; 8];
        self.read_register(0x0E + profile, &mut register)?;

        // Overwrite POW
        register[0] = ((asf >>  8) & 0xFF) as u8;
        register[1] = ((asf >>  0) & 0xFF) as u8;

        // Update POW by writing back the register
        self.write_register(0x0E + profile, &mut register)
    }

    // Helper function to switch into single tone mode
    // Need to setup configuration registers before writing single tone register
    fn enable_single_tone_configuration(&mut self) -> Result<(), Error<E>> {
        
        self.set_configurations(&mut [
            (DDSCFRMask::RAM_ENABLE, 0),
            (DDSCFRMask::DIGITAL_RAMP_ENABLE, 0),
            (DDSCFRMask::OSK_ENABLE, 0),
            (DDSCFRMask::PARALLEL_DATA_PORT_ENABLE, 0),
        ])?;
        self.set_configurations(&mut [
            (DDSCFRMask::EN_AMP_SCALE_SINGLE_TONE_PRO, 1),
        ])
    }

    // Helper function to switch into RAM mode
    // Need to setup configuration registers before writing into RAM profile register
    fn enable_ram_configuration(&mut self, ram_dst: RAMDestination) -> Result<(), Error<E>> {
        self.set_configurations(&mut [
            (DDSCFRMask::RAM_ENABLE, 1),
            (DDSCFRMask::RAM_PLAYBACK_DST, ram_dst as u32),
        ])
    }

    // Helper function to switch out of RAM mode
    // Need to setup configuration registers before writing into RAM profile register
    fn disable_ram_configuration(&mut self) -> Result<(), Error<E>> {
        self.set_configurations(&mut [
            (DDSCFRMask::RAM_ENABLE, 0),
        ])
    }

    /*
     *  Configure a RAM mode profile, wrt supplied frequency data
     *  This will setup the static RAM_VEC by converting frequency to ftw
     */
    pub unsafe fn set_frequency_ram_profile(&mut self, profile: u8, start_addr: u16, end_addr: u16,
        no_dwell_high: bool, zero_crossing: bool, op_mode: RAMOperationMode, playback_rate: f64,
        frequency_data: &[f64]
    ) -> Result<(), Error<E>> {

        // Check the legality of the profile setup
        assert!(profile <= 7);
        assert!(end_addr >= start_addr);
        assert!(end_addr < 1024);
        assert_eq!(frequency_data.len() as u16, end_addr - start_addr + 1);

        // Clear RAM vector, and add address byte
        RAM_VEC.clear();
        RAM_VEC.push(0x16)
            .map_err(|_| Error::DDSRAMError)?;

        // Convert frequency data into bytes recognized by DDS
        for freq in frequency_data.iter() {
            let ftw = self.frequency_to_ftw(*freq);
            RAM_VEC.push(((ftw >> 24) & 0xFF) as u8)
                .map_err(|_| Error::DDSRAMError)?;
            RAM_VEC.push(((ftw >> 16) & 0xFF) as u8)
                .map_err(|_| Error::DDSRAMError)?;
            RAM_VEC.push(((ftw >>  8) & 0xFF) as u8)
                .map_err(|_| Error::DDSRAMError)?;
            RAM_VEC.push(((ftw >>  0) & 0xFF) as u8)
                .map_err(|_| Error::DDSRAMError)?;
        }

        self.set_ram_profile(profile, start_addr, end_addr, RAMDestination::Frequency,
            no_dwell_high, zero_crossing, op_mode, playback_rate)
    }

    /*
     *    Configure a RAM mode profile, wrt supplied amplitude data
     *  This will setup the static RAM_VEC by converting amplitude to asf
     */
    pub unsafe fn set_amplitude_ram_profile(&mut self, profile: u8, start_addr: u16, end_addr: u16,
        no_dwell_high: bool, zero_crossing: bool, op_mode: RAMOperationMode, playback_rate: f64,
        amplitude_data: &[f64]
    ) -> Result<(), Error<E>> {

        // Check the legality of the profile setup
        assert!(profile <= 7);
        assert!(end_addr >= start_addr);
        assert!(end_addr < 1024);
        assert_eq!(amplitude_data.len() as u16, end_addr - start_addr + 1);

        // Clear RAM vector, and add address byte
        RAM_VEC.clear();
        RAM_VEC.push(0x16)
            .map_err(|_| Error::DDSRAMError)?;

        // Convert amplitude data into bytes recognized by DDS
        for amp in amplitude_data.iter() {
            let asf = self.amplitude_to_asf(*amp);
            RAM_VEC.push(((asf >>  8) & 0xFF) as u8)
                .map_err(|_| Error::DDSRAMError)?;
            RAM_VEC.push(((asf <<  2) & 0xFC) as u8)
                .map_err(|_| Error::DDSRAMError)?;
            RAM_VEC.push(0)
                .map_err(|_| Error::DDSRAMError)?;
            RAM_VEC.push(0)
                .map_err(|_| Error::DDSRAMError)?;
        }

        self.set_ram_profile(profile, start_addr, end_addr, RAMDestination::Amplitude,
            no_dwell_high, zero_crossing, op_mode, playback_rate)
    }

    /*
     *  Configure a RAM mode profile, wrt supplied phase data
     *  This will setup the static RAM_VEC by converting phase to ftw
     */
    pub unsafe fn set_phase_ram_profile(&mut self, profile: u8, start_addr: u16, end_addr: u16,
        no_dwell_high: bool, zero_crossing: bool, op_mode: RAMOperationMode, playback_rate: f64,
        phase_data: &[f64]
    ) -> Result<(), Error<E>> {

        // Check the legality of the profile setup
        assert!(profile <= 7);
        assert!(end_addr >= start_addr);
        assert!(end_addr < 1024);
        assert_eq!(phase_data.len() as u16, end_addr - start_addr + 1);

        // Clear RAM vector, and add address byte
        RAM_VEC.clear();
        RAM_VEC.push(0x16)
            .map_err(|_| Error::DDSRAMError)?;

        // Convert phase data into bytes recognized by DDS
        for deg in phase_data.iter() {
            let pow = self.degree_to_pow(*deg);
             RAM_VEC.push(((pow >>  8) & 0xFF) as u8)
                .map_err(|_| Error::DDSRAMError)?;
             RAM_VEC.push(((pow >>  0) & 0xFF) as u8)
                .map_err(|_| Error::DDSRAMError)?;
             RAM_VEC.push(0)
                .map_err(|_| Error::DDSRAMError)?;
             RAM_VEC.push(0)
                .map_err(|_| Error::DDSRAMError)?;
        }

        self.set_ram_profile(profile, start_addr, end_addr, RAMDestination::Phase,
            no_dwell_high, zero_crossing, op_mode, playback_rate)
    }

    /*
     *  Configure a RAM mode profile, wrt supplied phase data
     *  This will setup the static RAM_VEC by converting phase to ftw
     */
    pub unsafe fn set_polar_ram_profile(&mut self, profile: u8, start_addr: u16, end_addr: u16,
        no_dwell_high: bool, zero_crossing: bool, op_mode: RAMOperationMode, playback_rate: f64,
        polar_data: &[(f64, f64)]
    ) -> Result<(), Error<E>> {

        // Check the legality of the profile setup
        assert!(profile <= 7);
        assert!(end_addr >= start_addr);
        assert!(end_addr < 1024);
        assert_eq!(polar_data.len() as u16, end_addr - start_addr + 1);

        // Clear RAM vector, and add address byte
        RAM_VEC.clear();
        RAM_VEC.push(0x16)
            .map_err(|_| Error::DDSRAMError)?;

        // Convert amplitude data into bytes recognized by DDS
        for (deg, amp) in polar_data.iter() {
            let pow = self.degree_to_pow(*deg);
            let asf = self.amplitude_to_asf(*amp);
            RAM_VEC.push(((pow >>  8) & 0xFF) as u8)
                .map_err(|_| Error::DDSRAMError)?;
            RAM_VEC.push(((pow >>  0) & 0xFF) as u8)
                .map_err(|_| Error::DDSRAMError)?;
            RAM_VEC.push(((asf >>  8) & 0xFF) as u8)
                .map_err(|_| Error::DDSRAMError)?;
            RAM_VEC.push(((asf <<  2) & 0xFC) as u8)
                .map_err(|_| Error::DDSRAMError)?;
        }

        self.set_ram_profile(profile, start_addr, end_addr, RAMDestination::Phase,
            no_dwell_high, zero_crossing, op_mode, playback_rate)
	}
	
    /*
     *  Configure a frequency sweep RAM mode profile
     */
	pub unsafe fn set_frequency_sweep_profile(&mut self, profile: u8, start_addr: u16,
        lower_boundary: f64, upper_boundary: f64, f_resolution: f64,
        no_dwell_high: bool, op_mode: RAMOperationMode, playback_rate: f64
    ) -> Result<(), Error<E>> {

        // Check the legality of the profile setup
        assert!(profile <= 7);
        assert!(start_addr < 1024);

        // Find out the required RAM size
        // Frequencies may have to be repeated if the playback rate is too low
        // Reject impossible setups
        // E.g. Higher playback rate than f_dds_clk, insufficient RAM allocation
        let nominal_step_rate = self.f_sys_clk/(4.0 * playback_rate);
        if nominal_step_rate < 1.0 {
            return Err(Error::DDSRAMError);
        }

        // TODO: Handle unfortunate scenario: step_rate / 0xFFFF gives a round number
        // Current implmentation unnecessarily allocates 1 extra RAM space for each data
        let duplication = (nominal_step_rate / (0xFFFF as f64)) as u64 + 1;

        // Acquire the RAM size needed by multiplying duplication.
        // All data needs to be duplicated such that a desired step_rate can be reached
        // Return DDS RAM Error if it does not fix into the RAM
        let span = upper_boundary - lower_boundary;
        let data_size = if core::intrinsics::roundf64(span/f_resolution) == (span/f_resolution) {
                (span/f_resolution) as u64 + 1
        } else {
                (span/f_resolution) as u64
        };
        let ram_size = data_size * duplication;

        let end_addr = (start_addr as u64) + ram_size - 1;
        trace!("Required RAM size: {}", ram_size);
        if end_addr >= 1024 {
            warn!("RAM address out of bound");
            return Err(Error::DDSRAMError);
        }

        // Clear RAM vector, and add address byte
        RAM_VEC.clear();
        RAM_VEC.push(0x16)
            .map_err(|_| Error::DDSRAMError)?;

        // Drop in the data into RAM_VEC
        for data_index in 0..data_size {
            let freq = lower_boundary + f_resolution * (data_index as f64);
            for _ in 0..duplication {
                let ftw = self.frequency_to_ftw(freq);
                RAM_VEC.push(((ftw >> 24) & 0xFF) as u8)
                    .map_err(|_| Error::DDSRAMError)?;
                RAM_VEC.push(((ftw >> 16) & 0xFF) as u8)
                    .map_err(|_| Error::DDSRAMError)?;
                RAM_VEC.push(((ftw >>  8) & 0xFF) as u8)
                    .map_err(|_| Error::DDSRAMError)?;
                RAM_VEC.push(((ftw >>  0) & 0xFF) as u8)
                    .map_err(|_| Error::DDSRAMError)?;
            }
        }

        debug!("start_addr: {}\nend_addr: {}\n, duplication: {}\n, data_size: {}\n",
            start_addr, end_addr, duplication, data_size);

        self.set_ram_profile(profile, start_addr, end_addr.try_into().unwrap(), RAMDestination::Frequency,
            no_dwell_high, true, op_mode, playback_rate * (duplication as f64))
    }

    /*
     *  Configure a RAM mode profile, w.r.t static vector (RAM_VEC)
     */
    fn set_ram_profile(&mut self, profile: u8, start_addr: u16, end_addr: u16,
        ram_dst: RAMDestination, no_dwell_high: bool, zero_crossing: bool,
        op_mode: RAMOperationMode, playback_rate: f64
    ) -> Result<(), Error<E>> {

        // Check the legality of the profile setup
        assert!(profile <= 7);
        assert!(end_addr >= start_addr);
        assert!(end_addr < 1024);
        // assert_eq! RAM_VEC.len() as u16, ((end_addr - start_addr + 1) * 4) + 1);

        // Calculate address step rate, and check legality
        let step_rate = (self.f_sys_clk/(4.0 * playback_rate)) as u64;
        trace!("Setting up RAM profile, step_rate: {}", step_rate);
        if step_rate == 0 || step_rate > 0xFFFF {
            return Err(Error::DDSRAMError);
        }

        // Before setting up RAM, disable RAM_ENABLE
        self.enable_ram_configuration(ram_dst.clone())?;

        // Write a RAM profile, but include all data in RAM
        self.write_register(0x0E + profile, &mut [
            0x00,
            ((step_rate >> 8) & 0xFF).try_into().unwrap(),
            ((step_rate >> 0) & 0xFF).try_into().unwrap(),
            ((end_addr >> 2) & 0xFF).try_into().unwrap(),
            ((end_addr & 0x3) << 6).try_into().unwrap(),
            ((start_addr >> 2) & 0xFF).try_into().unwrap(),
            ((start_addr & 0x3) << 6).try_into().unwrap(),
            ((no_dwell_high as u8) << 5) | ((zero_crossing as u8) << 3) | (op_mode as u8)
        ])?;
        
        // Temporarily disable RAM mode while accessing into RAM
        self.disable_ram_configuration()?;
        unsafe {
            self.write_ram()?;
        }

        // Properly configure start_addr and end_addr
        self.enable_ram_configuration(ram_dst)

    }

    // Calculate ftw (frequency tuning word)
    fn frequency_to_ftw(&mut self, f_out: f64) -> u32 {
        let f_res: u64 = 1 << 32;
        ((f_res as f64) * f_out / self.f_sys_clk) as u32
    }

    // Calculate pow (Phase Offset Word)
    fn degree_to_pow(&mut self, phase_offset: f64) -> u16 {
        // Calculate phase offset word (POW)
        let phase_res: u64 = 1 << 16;
        ((phase_res as f64) * phase_offset / 360.0) as u16
    }

    // Calculate asf (Amplitude Scale Factor)
    fn amplitude_to_asf(&mut self, amplitude: f64) -> u16 {
        let amp_res: u64 = 0x3FFF;
        ((amp_res as f64) * amplitude) as u16
    }

    // Write data in RAM
    unsafe fn write_ram(&mut self) -> Result<(), Error<E>> {
        self.spi.transfer(&mut RAM_VEC)
                .map(|_| ())
                .map_err(Error::SPI)
    }

    /*
     *  Test method for DDS.
     *  Return the number of test failed.
     */
    pub fn test(&mut self) -> Result<u32, Error<E>> {
        // Test configuration register by getting SDIO_IN_ONLY and LSB_FIRST.
        let mut error_count = 0;
        let mut config_checks = [
            (DDSCFRMask::SDIO_IN_ONLY, 1),
            (DDSCFRMask::LSB_FIRST, 0)
        ];
        self.get_configurations(&mut config_checks)?;
        if config_checks[0].1 == 0 {
            error_count += 1;
        }
        if config_checks[1].1 == 1 {
            error_count += 1;
        }
        Ok(error_count)
    }

    // Setter function for f_sys_clk
    // Warning: This does not setup the chip to generate this actual f_sys_clk
    // pub(crate) fn set_f_sys_clk(&mut self, f_sys_clk: f64) {
    //     self.f_sys_clk = f_sys_clk;
    // }

    // Getter function for f_sys_clk
    pub fn get_f_sys_clk(&mut self) -> f64 {
        self.f_sys_clk
    }
}

// Strong check for bytes passed to a register
macro_rules! impl_register_io {
    ($($reg_addr: expr, $reg_byte_size: expr),+) => {
        impl<SPI, E> DDS<SPI>
        where
            SPI: Transfer<u8, Error = E>
        {
            pub fn write_register(&mut self, addr: u8, bytes: &mut[u8]) -> Result<(), Error<E>> {
                match addr {
                    $(
                        $reg_addr => {
                            assert_eq!(bytes.len(), $reg_byte_size);
                            let mut arr: [u8; $reg_byte_size + 1] = [0; ($reg_byte_size + 1)];
                            arr[0] = addr | WRITE_MASK;
                            for i in 0..$reg_byte_size {
                                arr[i+1] = bytes[i];
                            }
                            self.spi.transfer(&mut arr)
                                    .map(|_| ())
                                    .map_err(Error::SPI)
                        },
                    )*
                    _ => panic!("Bad address for DDS writing.")
                }
            }

            pub fn read_register<'w>(&mut self, addr: u8, bytes: &'w mut[u8]) -> Result<&'w [u8], Error<E>> {
                match addr {
                    $(
                        $reg_addr => {
                            assert_eq!(bytes.len(), $reg_byte_size);
                            let mut arr: [u8; $reg_byte_size + 1] = [0; ($reg_byte_size + 1)];
                            arr[0] = addr | READ_MASK;
                            match self.spi.transfer(&mut arr).map_err(Error::SPI) {
                                Ok(ret) => {
                                    assert_eq!(ret.len(), $reg_byte_size + 1);
                                    for i in 0..$reg_byte_size {
                                        bytes[i] = ret[i+1];
                                    }
                                    Ok(bytes)
                                },
                                Err(e) => Err(e),
                            }
                        },
                    )*
                    _ => panic!("Bad address for DDS reading.")
                }
            }
        }
    }
}

impl_register_io!(
    0x00,    4,
    0x01,    4,
    0x02,    4,
    0x03,    4,
    0x04,    4,
    0x07,    4,
    0x08,    2,
    0x09,    4,
    0x0A,    4,
    0x0B,    8,
    0x0C,    8,
    0x0D,    4,
    0x0E,    8,
    0x0F,    8,
    0x10,    8,
    0x11,    8,
    0x12,    8,
    0x13,    8,
    0x14,    8,
    0x15,    8
);