#![no_main]
#![no_std]

#[macro_use]
extern crate log;
use stm32h7xx_hal::hal::digital::v2::{
	InputPin,
	OutputPin,
};
use stm32h7xx_hal::{gpio::Speed, pac, prelude::*, spi};

use cortex_m;
use cortex_m::asm::nop;
use cortex_m_rt::entry;

use core::ptr;
use nb::block;

#[path = "util/logger.rs"]
mod logger;

#[entry]
fn main() -> ! {

	let mut cp = cortex_m::Peripherals::take().unwrap();
	let dp = pac::Peripherals::take().unwrap();

	let pwr = dp.PWR.constrain();
	let vos = pwr.freeze();

	let rcc = dp.RCC.constrain();
	let ccdr = rcc
		.sys_ck(400.mhz())
		.pll1_q_ck(48.mhz())
		.pll1_r_ck(400.mhz())   // for TRACECK
		.freeze(vos, &dp.SYSCFG);

	unsafe {
		logger::enable_itm(&dp.DBGMCU, &mut cp.DCB, &mut cp.ITM);
	}
	
	let mut delay = cp.SYST.delay(ccdr.clocks);

	let gpioa = dp.GPIOA.split(ccdr.peripheral.GPIOA);
	let gpiob = dp.GPIOB.split(ccdr.peripheral.GPIOB);
	let gpiod = dp.GPIOD.split(ccdr.peripheral.GPIOD);
	let gpiof = dp.GPIOF.split(ccdr.peripheral.GPIOF);

    // gpiob.pb3.into_alternate_af0().set_speed(Speed::VeryHigh);

	logger::init();
	
	debug!("Flashing configuration bitstream to iCE40 HX8K on Humpback.");

	// Using SPI_1 alternate functions (af5)
	let fpga_sck = gpiob.pb3.into_alternate_af5();
	let fpga_sdo = gpiob.pb4.into_alternate_af5();
	let fpga_sdi = gpiob.pb5.into_alternate_af5();

	// Setup SPI_SS_B and CRESET_B
	let mut fpga_ss = gpioa.pa4.into_push_pull_output();
	let mut fpga_creset = gpiof.pf3.into_open_drain_output();

	// Setup CDONE
	let fpga_cdone = gpiod.pd15.into_pull_up_input();

	// Setup SPI interface
	let mut fpga_cfg_spi = dp.SPI1.spi(
		(fpga_sck, fpga_sdo, fpga_sdi),
		spi::MODE_3,
		12.mhz(),
		ccdr.peripheral.SPI1,
		&ccdr.clocks,
	);

	// Data buffer setup
	let mut dummy_byte :[u8; 1] = [0x00];
	let mut dummy_13_bytes :[u8; 13] = [0x00; 13];

	// Drive CRESET_B low
	fpga_creset.set_low().unwrap();

	// Drive SPI_SS_B low
	fpga_ss.set_low().unwrap();

	// Wait at least 200ns
	delay.delay_us(1_u16);

	// Drive CRESET_B high
	fpga_creset.set_high().unwrap();

	// Wait at least another 1200us to clear internal config memory
	delay.delay_us(1200_u16);

	// Before data transmission starts, check if C_DONE is truly dine
	match fpga_cdone.is_high() {
		Ok(false)	=> debug!("Reset successful!"),
		Ok(_) 		=> debug!("Reset unsuccessful!"),
		Err(_) 		=> debug!("Reset error!"),
	};

	// Set SPI_SS_B high
	fpga_ss.set_high().unwrap();

	// Send 8 dummy clock, effectively 1 byte of 0x00
	fpga_cfg_spi.transfer(&mut dummy_byte).unwrap();

	// Drive SPI_SS_B low
	fpga_ss.set_low().unwrap();

	// Send the whole image without interruption
	let base_address = 0x08100000;
	let size = 135100;
	for index in 0..size {
		unsafe {
			let data :u8 = ptr::read_volatile((base_address + index) as *const u8);
			block!(fpga_cfg_spi.send(data)).unwrap();
			block!(fpga_cfg_spi.read()).unwrap();
		}
	}

	// Drive SPI_SS_B high
	fpga_ss.set_high().unwrap();

	// Send at another 100 dummy clocks (choosing 13 bytes)
	fpga_cfg_spi.transfer(&mut dummy_13_bytes).unwrap();

	// Check the CDONE output from FPGA
	if !(fpga_cdone.is_high().unwrap()) {
		debug!("ERROR!");
	}
	else {
		debug!("Configuration successful!");
		// Send at least another 49 clock cycles to activate IO pins (choosing same 13 bytes)
		fpga_cfg_spi.transfer(&mut dummy_13_bytes).unwrap();
		debug!("User I/O pins activated.");
	}

	loop {
		nop();
	}
}