inline void wlan_tx_start() {

	//Start the PHY Tx immediately; this bypasses the mac_hw MPDU Tx state machine
	// This should only be used for debug - normal transmissions should use mac_hw
	REG_SET_BITS(WLAN_TX_REG_START, WLAN_TX_REG_START_VIA_RC);
	REG_CLEAR_BITS(WLAN_TX_REG_START, WLAN_TX_REG_START_VIA_RC);

	return;
}
void wlan_tx_config_ant_mode(u32 ant_mode) {
	return;
	/*OLD - DELETE WHEN v40 HW WORKS!*/
	REG_CLEAR_BITS(WLAN_TX_REG_CFG, (WLAN_TX_REG_CFG_ANT_A_TXEN | WLAN_TX_REG_CFG_ANT_B_TXEN | WLAN_TX_REG_CFG_ANT_C_TXEN | WLAN_TX_REG_CFG_ANT_D_TXEN));
	radio_controller_setCtrlSource(RC_BASEADDR, RC_ALL_RF, RC_REG0_TXEN_CTRLSRC, RC_CTRLSRC_REG);

	switch(ant_mode) {
		case TX_ANTMODE_SISO_ANTA:
			REG_SET_BITS(WLAN_TX_REG_CFG, WLAN_TX_REG_CFG_ANT_A_TXEN);
			radio_controller_setCtrlSource(RC_BASEADDR, RC_RFA, RC_REG0_TXEN_CTRLSRC, RC_CTRLSRC_HW);
			break;

		case TX_ANTMODE_SISO_ANTB:
			REG_SET_BITS(WLAN_TX_REG_CFG, WLAN_TX_REG_CFG_ANT_B_TXEN);
			radio_controller_setCtrlSource(RC_BASEADDR, RC_RFB, RC_REG0_TXEN_CTRLSRC, RC_CTRLSRC_HW);
			break;

		case TX_ANTMODE_SISO_ANTC:
			REG_SET_BITS(WLAN_TX_REG_CFG, WLAN_TX_REG_CFG_ANT_C_TXEN);
			radio_controller_setCtrlSource(RC_BASEADDR, RC_RFC, RC_REG0_TXEN_CTRLSRC, RC_CTRLSRC_HW);
			break;

		case TX_ANTMODE_SISO_ANTD:
			REG_SET_BITS(WLAN_TX_REG_CFG, WLAN_TX_REG_CFG_ANT_D_TXEN);
			radio_controller_setCtrlSource(RC_BASEADDR, RC_RFD, RC_REG0_TXEN_CTRLSRC, RC_CTRLSRC_HW);
			break;

		default:
			//Default to SISO on A if user provides invalid mode
			xil_printf("wlan_tx_config_ant_mode ERROR: Invalid Mode - Defaulting to SISO on A\n");
			REG_SET_BITS(WLAN_TX_REG_CFG, WLAN_TX_REG_CFG_ANT_A_TXEN);
			radio_controller_setCtrlSource(RC_BASEADDR, RC_RFA, RC_REG0_TXEN_CTRLSRC, RC_CTRLSRC_HW);
			break;
	}
	return;
}
int main(){

	wlan_mac_hw_info* hw_info;
	xil_printf("\f");
	xil_printf("----- Mango 802.11 Reference Design -----\n");
	xil_printf("----- v1.3 ------------------------------\n");
	xil_printf("----- wlan_mac_nomac --------------------\n");
	xil_printf("Compiled %s %s\n\n", __DATE__, __TIME__);

	xil_printf("Note: this UART is currently printing from CPU_LOW. To view prints from\n");
	xil_printf("and interact with CPU_HIGH, raise the right-most User I/O DIP switch bit.\n");
	xil_printf("This switch can be toggled live while the design is running.\n\n");

	wlan_tx_config_ant_mode(TX_ANTMODE_SISO_ANTA);

	red_led_index = 0;
	green_led_index = 0;
	userio_write_leds_green(USERIO_BASEADDR, (1<<green_led_index));
	userio_write_leds_red(USERIO_BASEADDR, (1<<red_led_index));

	wlan_mac_low_init(WARPNET_TYPE_80211_LOW);

	hw_info = wlan_mac_low_get_hw_info();
	memcpy(eeprom_addr,hw_info->hw_addr_wlan,6);


	wlan_mac_low_set_frame_rx_callback((void*)frame_receive);
	wlan_mac_low_set_frame_tx_callback((void*)frame_transmit);

	wlan_mac_low_finish_init();

	REG_SET_BITS(WLAN_MAC_REG_CONTROL, (WLAN_MAC_CTRL_MASK_CCA_IGNORE_PHY_CS | WLAN_MAC_CTRL_MASK_CCA_IGNORE_NAV));

    xil_printf("Initialization Finished\n");

	while(1){

		//Poll PHY RX start
		wlan_mac_low_poll_frame_rx();

		//Poll IPC rx
		wlan_mac_low_poll_ipc_rx();
	}
	return 0;
}
void wlan_phy_init() {

	//Assert Tx and Rx resets
	REG_SET_BITS(WLAN_RX_REG_CTRL, WLAN_RX_REG_CTRL_RESET);
	REG_SET_BITS(WLAN_TX_REG_CFG, WLAN_TX_REG_CFG_RESET);

/************ PHY Rx ************/

	//Enable DSSS Rx by default
	wlan_phy_DSSS_rx_enable();
	//wlan_phy_DSSS_rx_disable();

	//Set the max Tx/Rx packet sizes to 2KB (sane default for standard 802.11a/g links)
	wlan_phy_rx_set_max_pkt_len_kB(2);
	wlan_phy_tx_set_max_pkt_len_kB(2);

	//Configure the DSSS Rx pipeline
	// DSSS_rx_config(code_corr, despread_dly, sfd_timeout)
	wlan_phy_DSSS_rx_config(0x30, 5, 140);

	//Configure the DSSS auto-correlation packet detector
	// autoCorr_dsss_cfg(corr_thresh, energy_thresh, timeout_ones, timeout_count)
	wlan_phy_rx_pktDet_autoCorr_dsss_cfg(0x60, 400, 30, 40);
//	wlan_phy_rx_pktDet_autoCorr_dsss_cfg(0xFF, 0x3FF, 30, 40); //Effectively disable DSSS with high det thresholds

	//Configure DSSS Rx to wait for AGC lock, then hold AGC lock until Rx completes or times out
	REG_SET_BITS(WLAN_RX_REG_CFG, (WLAN_RX_REG_CFG_DSSS_RX_AGC_HOLD | WLAN_RX_REG_CFG_DSSS_RX_REQ_AGC));

	//Enable LTS-based CFO correction
	REG_CLEAR_BITS(WLAN_RX_REG_CFG, WLAN_RX_REG_CFG_CFO_EST_BYPASS);

	//Enable byte order swap for payloads and chan ests
	REG_SET_BITS(WLAN_RX_REG_CFG, WLAN_RX_REG_CFG_PKT_BUF_WEN_SWAP);
	REG_CLEAR_BITS(WLAN_RX_REG_CFG, WLAN_RX_REG_CFG_CHAN_EST_WEN_SWAP);

	//Enable writing OFDM chan ests to Rx pkt buffer
	REG_SET_BITS(WLAN_RX_REG_CFG, WLAN_RX_REG_CFG_RECORD_CHAN_EST);

	//Block Rx inputs during Tx
	REG_SET_BITS(WLAN_RX_REG_CFG, WLAN_RX_REG_CFG_USE_TX_SIG_BLOCK);

	//FFT config
	wlan_phy_rx_set_fft_window_offset(3);
	wlan_phy_rx_set_fft_scaling(5);

	//Set LTS correlation threshold and timeout
	// 1023 disables LTS threshold switch (one threshold worked across SNRs in our testing)
	// Timeout value is doubled in hardware (350/2 becomes a timeout of 350 sample periods)
	wlan_phy_rx_lts_corr_config(1023 * PHY_RX_RSSI_SUM_LEN, 350/2);

	//LTS correlation thresholds (low NSR, high SNR)
	wlan_phy_rx_lts_corr_thresholds(12500, 12500); //FIXME

	//Configure RSSI pkt det
 	// RSSI pkt det disabled by default (auto-corr detection worked across SNRs in our testing)
 	wlan_phy_rx_pktDet_RSSI_cfg(PHY_RX_RSSI_SUM_LEN, (PHY_RX_RSSI_SUM_LEN * 1023), 4);

	//Configure auto-corr pkt det autoCorr_ofdm_cfg(corr_thresh, energy_thresh, min_dur, post_wait)
	wlan_phy_rx_pktDet_autoCorr_ofdm_cfg(200, 50, 4, 0x3F);

	//Configure the default antenna selections as SISO Tx/Rx on RF A
	wlan_rx_config_ant_mode(RX_ANTMODE_SISO_ANTA);

	//Set physical carrier sensing threshold
	wlan_phy_rx_set_cca_thresh(PHY_RX_RSSI_SUM_LEN * 480); //-62dBm from 802.11-2012
	//wlan_phy_rx_set_cca_thresh(PHY_RX_RSSI_SUM_LEN * 750);
	//wlan_phy_rx_set_cca_thresh(PHY_RX_RSSI_SUM_LEN * 1023);

	//Set post Rx extension (number of sample periods post-Rx the PHY waits before asserting Rx END - must be long enough for decoding latency at 64QAM 3/4)
	wlan_phy_rx_set_extension(PHY_RX_SIG_EXT_USEC*20); //num samp periods post done to extend CCA BUSY

	//Configure channel estimate capture (64 subcarriers, 4 bytes each)
	// Chan ests start at sizeof(rx_frame_info) - sizeof(chan_est)
	wlan_phy_rx_pkt_buf_h_est_offset((PHY_RX_PKT_BUF_PHY_HDR_OFFSET - (64*4)));
	

/************ PHY Tx ************/
	
	//De-assert all starts
	REG_CLEAR_BITS(WLAN_TX_REG_START, 0xFFFFFFFF);

	//Set Tx duration extension, in units of sample periods
	wlan_phy_tx_set_extension(PHY_TX_SIG_EXT_USEC*20);

	//Set extension from last samp output to RF Tx -> Rx transition
	// This delay allows the Tx pipeline to finish driving samples into DACs
	//  and for DAC->RF frontend to finish output Tx waveform
	wlan_phy_tx_set_txen_extension(50);

	//Set extension from RF Rx -> Tx to un-blocking Rx samples
	wlan_phy_tx_set_rx_invalid_extension(150); //100

	//Set digital scaling of preamble/payload signals before DACs (UFix12_0)
	wlan_phy_tx_set_scaling(0x2000, 0x2000);

	//Enable the Tx PHY 4-bit TxEn port that captures the MAC's selection of active antennas per Tx
	REG_SET_BITS(WLAN_TX_REG_CFG, WLAN_TX_REG_CFG_USE_MAC_ANT_MASKS);

/*********** AGC ***************/
	wlan_agc_config(RX_ANTMODE_SISO_ANTA);

/************ Wrap Up ************/

	//Set MSB of RSSI_THRESH register to use summed RSSI for debug output
	Xil_Out32(XPAR_WLAN_PHY_RX_MEMMAP_RSSI_THRESH, ((1<<31) | (PHY_RX_RSSI_SUM_LEN * 150)));

	//De-assert resets
	REG_CLEAR_BITS(WLAN_RX_REG_CTRL, WLAN_RX_REG_CTRL_RESET);
	REG_CLEAR_BITS(WLAN_TX_REG_CFG, WLAN_TX_REG_CFG_RESET);

	//Let PHY Tx take control of radio TXEN/RXEN
	REG_CLEAR_BITS(WLAN_TX_REG_CFG, WLAN_TX_REG_CFG_SET_RC_RXEN);
	REG_SET_BITS(WLAN_TX_REG_CFG, WLAN_TX_REG_CFG_SET_RC_RXEN);

	return;
}
void wlan_rx_config_ant_mode(u32 ant_mode) {

	//Hold the Rx PHY in reset before changing any pkt det or radio enables
	REG_SET_BITS(WLAN_RX_REG_CTRL, WLAN_RX_REG_CTRL_RESET);

	//Disable all Rx modes first; selectively re-enabled in switch below
	REG_CLEAR_BITS(WLAN_RX_REG_CFG, (
			WLAN_RX_REG_CFG_PKT_DET_EN_ANT_A |
			WLAN_RX_REG_CFG_PKT_DET_EN_ANT_B |
			WLAN_RX_REG_CFG_PKT_DET_EN_ANT_C |
			WLAN_RX_REG_CFG_PKT_DET_EN_ANT_D |
			WLAN_RX_REG_CFG_SWITCHING_DIV_EN |
			WLAN_RX_REG_CFG_PKT_DET_EN_EXT |
			WLAN_RX_REG_CFG_ANT_SEL_MASK));

	//Disable PHY control of all RF interfaces - selected interfaces to re-enabled below
	radio_controller_setCtrlSource(RC_BASEADDR, RC_ALL_RF, RC_REG0_RXEN_CTRLSRC, RC_CTRLSRC_REG);

	switch(ant_mode) {
		case RX_ANTMODE_SISO_ANTA:

			//Enable packet detection on RF A
			REG_SET_BITS(WLAN_RX_REG_CFG, WLAN_RX_REG_CFG_PKT_DET_EN_ANT_A);

			//Select RF A I/Q stream for Rx PHY
			wlan_phy_select_rx_antenna(0);

			//Give PHY control of RF A Tx/Rx status
			radio_controller_setCtrlSource(RC_BASEADDR, RC_RFA, RC_REG0_RXEN_CTRLSRC, RC_CTRLSRC_HW);

			//Configure AGC for RF A
			wlan_agc_config(RX_ANTMODE_SISO_ANTA);

			break;

		case RX_ANTMODE_SISO_ANTB:
			REG_SET_BITS(WLAN_RX_REG_CFG, WLAN_RX_REG_CFG_PKT_DET_EN_ANT_B);
			wlan_phy_select_rx_antenna(1);
			radio_controller_setCtrlSource(RC_BASEADDR, RC_RFB, RC_REG0_RXEN_CTRLSRC, RC_CTRLSRC_HW);
			wlan_agc_config(RX_ANTMODE_SISO_ANTB);

			break;

		case RX_ANTMODE_SISO_ANTC:
			REG_SET_BITS(WLAN_RX_REG_CFG, WLAN_RX_REG_CFG_PKT_DET_EN_ANT_C);
			wlan_phy_select_rx_antenna(2);
			radio_controller_setCtrlSource(RC_BASEADDR, RC_RFC, RC_REG0_RXEN_CTRLSRC, RC_CTRLSRC_HW);
			wlan_agc_config(RX_ANTMODE_SISO_ANTC);

			break;

		case RX_ANTMODE_SISO_ANTD:
			REG_SET_BITS(WLAN_RX_REG_CFG, WLAN_RX_REG_CFG_PKT_DET_EN_ANT_D);
			wlan_phy_select_rx_antenna(3);
			radio_controller_setCtrlSource(RC_BASEADDR, RC_RFD, RC_REG0_RXEN_CTRLSRC, RC_CTRLSRC_HW);
			wlan_agc_config(RX_ANTMODE_SISO_ANTD);

			break;

		case RX_ANTMODE_SISO_SELDIV_2ANT:
			REG_SET_BITS(WLAN_RX_REG_CFG, (WLAN_RX_REG_CFG_PKT_DET_EN_ANT_A | WLAN_RX_REG_CFG_PKT_DET_EN_ANT_B | WLAN_RX_REG_CFG_SWITCHING_DIV_EN));
			radio_controller_setCtrlSource(RC_BASEADDR, (RC_RFA | RC_RFB), RC_REG0_RXEN_CTRLSRC, RC_CTRLSRC_HW);

			wlan_agc_config(RX_ANTMODE_SISO_SELDIV_2ANT);

			break;

		case RX_ANTMODE_SISO_SELDIV_4ANT:
			REG_SET_BITS(WLAN_RX_REG_CFG, (WLAN_RX_REG_CFG_PKT_DET_EN_ANT_A | WLAN_RX_REG_CFG_PKT_DET_EN_ANT_B | WLAN_RX_REG_CFG_PKT_DET_EN_ANT_C | WLAN_RX_REG_CFG_PKT_DET_EN_ANT_D | WLAN_RX_REG_CFG_SWITCHING_DIV_EN));
			radio_controller_setCtrlSource(RC_BASEADDR, RC_ALL_RF, RC_REG0_RXEN_CTRLSRC, RC_CTRLSRC_HW);

			wlan_agc_config(RX_ANTMODE_SISO_SELDIV_4ANT);

			break;

		default:
			//Default to SISO on A if user provides invalid mode
			xil_printf("wlan_rx_config_ant_mode ERROR: Invalid Mode - Defaulting to SISO on A\n");

			REG_SET_BITS(WLAN_RX_REG_CFG, WLAN_RX_REG_CFG_PKT_DET_EN_ANT_A);
			wlan_phy_select_rx_antenna(0);
			radio_controller_setCtrlSource(RC_BASEADDR, RC_RFA, RC_REG0_RXEN_CTRLSRC, RC_CTRLSRC_HW);
			wlan_agc_config(RX_ANTMODE_SISO_ANTA);
			break;
	}

	//Release the PHY Rx reset
	REG_CLEAR_BITS(WLAN_RX_REG_CTRL, WLAN_RX_REG_CTRL_RESET);

	return;
}