initial commit of private island ARM test suiteHEADmaster

17 files changed, 1644 insertions, 0 deletions

diff --git a/source/cmds.c b/source/cmds.c
new file mode 100644
index 0000000..f0f9124
--- /dev/null
+++ b/source/cmds.c
@@ -0,0 +1,48 @@
+/*
+ *       cmds.c
+ *
+ *       Copyright (C) 2018, 2019 Mind Chasers Inc.
+ *
+ *   Licensed under the Apache License, Version 2.0 (the "License");
+ *   you may not use this file except in compliance with the License.
+ *   You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ *   function: convenience structs for sending commands to FPGA internal controller
+ *
+ */
+
+
+
+
+#include "cmds.h"
+
+
+struct gen0_config i_init = {.cmd = 'i', .term = '\r'};
+
+struct gen0_config pcs_s = {.cmd = 'p', .term = '\r'};
+struct gen0_config link_s = {.cmd = 'l', .term = '\r'};
+
+struct gen1_config m0 = {.cmd = 'm', .b0 = 0x0, .term = '\r'};
+struct gen1_config m1 = {.cmd = 'm', .b0 = 0x1, .term = '\r'};
+
+struct gen0_config test_tx_pkt = {.cmd = 't', .term = '\r'};
+
+struct x_config x_reset = {.cmd = 'x', .ch0 = 0x3, .ch1 = 0x3, .ch2 = 0x3, .ch3 = 0x3, .term = '\r'};
+struct x_config x_init0 = {.cmd = 'x', .ch0 = 0x1, .ch1 = 0x1, .ch2 = 0x1, .ch3 = 0x1, .term = '\r'};
+struct x_config x_init1 = {.cmd = 'x', .ch0 = 0x0, .ch1 = 0x0, .ch2 = 0x0, .ch3 = 0x0, .term = '\r'};
+
+struct u_config u_reset = {.cmd = 'u', .dcu0 = 0x7, .dcu1 = 0x7, .term = '\r'};
+struct u_config u_init = {.cmd = 'u', .dcu0 = 0x0, .dcu1 = 0x0, .term = '\r'};
+
+struct c_config c_reset = {.cmd = 'c', .ch0 = 0x7, .ch1 = 0x7, .ch2 = 0x7, .ch3 = 0x7, .term = '\r'};
+struct c_config c_init0 = {.cmd = 'c', .ch0 = 0x5, .ch1 = 0x5, .ch2 = 0x5, .ch3 = 0x5, .term = '\r'};
+struct c_config c_init1 = {.cmd = 'c', .ch0 = 0x0, .ch1 = 0x0, .ch2 = 0x0, .ch3 = 0x0, .term = '\r'};
+struct c_config c_init_pcsr = {.cmd = 'c', .ch0 = 0x0, .ch1 = 0x0, .ch2 = 0x1, .ch3 = 0x1, .term = '\r'};
diff --git a/source/cmds.h b/source/cmds.h
new file mode 100644
index 0000000..f03f8e4
--- /dev/null
+++ b/source/cmds.h
@@ -0,0 +1,100 @@
+/*
+ *       cmds.h
+ *
+ *       Copyright (C) 2018, 2019 Mind Chasers Inc.
+ *
+ *   Licensed under the Apache License, Version 2.0 (the "License");
+ *   you may not use this file except in compliance with the License.
+ *   You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ *
+ */
+
+#ifndef CMDS_H_
+#define CMDS_H_
+
+#include "stdint.h"
+
+// PCS Bit Map
+#define	RX_CDR_LOL_0	0x01
+#define	RX_CDR_LOL_1	0x02
+#define	RX_CDR_LOL_2	0x04
+#define	PLL_LOL_0		0x08
+#define	PLL_LOL_1		0x10
+
+// Link Status Bit Map
+#define	PORT_UP_0		0x1
+#define PORT_UP_1		0x2
+
+// SCI Address Definitions
+
+#define SCI_SEL_CH0		0x00
+#define SCI_SEL_CH1		0x40
+#define SCI_SEL_AUX		0xC0
+
+struct x_config {
+	char cmd;
+	uint8_t ch3;
+	uint8_t ch2;
+	uint8_t ch1;
+	uint8_t ch0;
+	char term;
+};
+
+struct c_config {
+	char cmd;
+	uint8_t ch3;
+	uint8_t ch2;
+	uint8_t ch1;
+	uint8_t ch0;
+	char term;
+};
+
+struct u_config {
+	char cmd;
+	uint8_t dcu1;
+	uint8_t dcu0;
+	char term;
+};
+
+struct gen0_config {
+	char cmd;
+	char term;
+};
+
+struct gen1_config {
+	char cmd;
+	uint8_t b0;
+	char term;
+};
+
+extern struct gen0_config i_init;
+extern struct gen0_config pcs_s;
+extern struct gen0_config link_s;
+
+extern struct gen1_config m0;
+extern struct gen1_config m1;
+
+extern struct gen0_config test_tx_pkt;
+
+extern struct x_config x_reset;
+extern struct x_config x_init0;
+extern struct x_config x_init1;
+
+extern struct u_config u_reset;
+extern struct u_config u_init;
+
+extern struct c_config c_reset;
+extern struct c_config c_init0;
+extern struct c_config c_init1;
+extern struct c_config c_init_pcsr;
+
+#endif /* CMDS_H_ */
diff --git a/source/ecp5_driver.c b/source/ecp5_driver.c
new file mode 100644
index 0000000..99373cd
--- /dev/null
+++ b/source/ecp5_driver.c
@@ -0,0 +1,123 @@
+/*
+ *       cmds.c
+ *
+ *       Copyright (C) 2018, 2019 Mind Chasers Inc.
+ *
+ *   Licensed under the Apache License, Version 2.0 (the "License");
+ *   you may not use this file except in compliance with the License.
+ *   You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ *   function: Initialize ECP PCS block
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "board.h"
+#include "fsl_debug_console.h"
+#include "fsl_i2c.h"
+#include "ecp5_driver.h"
+#include "cmds.h"
+#include "utils.h"
+#include "main.h"
+
+
+/* set PCS/SERDES block to reset */
+void ecp5_pcs_reset(i2c_master_transfer_t* pXfer) {
+	// reset our sgmii_cont modules, but leave the PHYs out of reset
+	i2c_write(pXfer,  I2C_ECP5_ADDR, (uint8_t*) &x_init0, sizeof(x_init0));
+
+	// assert all channel resets
+	i2c_write(pXfer,  I2C_ECP5_ADDR, (uint8_t*) &c_reset, sizeof(c_reset));
+
+	// assert all DCU resets
+	i2c_write(pXfer,  I2C_ECP5_ADDR, (uint8_t*) &u_reset, sizeof(u_reset));
+}
+
+
+int ecp5_pcs_init(i2c_master_transfer_t* pXfer, uint8_t* pBuff) {
+	uint8_t errors = 0;
+
+
+	// clear the I2C buffer of anything in the read FIFO
+	i2c_read(pXfer, I2C_ECP5_ADDR, (uint8_t*) pBuff, 8);
+
+	/*
+	 * Check RX and TX status for health
+	 * If PLL isn't locked, then we need a reset.
+	 */
+	PRINTF("Check PCS Status (p) \r\n");
+	i2c_write(pXfer,I2C_ECP5_ADDR, (uint8_t*) &pcs_s, sizeof(pcs_s));
+	i2c_read(pXfer, I2C_ECP5_ADDR, (uint8_t*) pBuff, 8);
+	dump_data((uint8_t*) pBuff, 8, true);
+	errors = ascii_to_bin(pBuff[2]) << 4 | ascii_to_bin(pBuff[3]);
+
+	if (errors == 0 ) {
+		return 0;
+	}
+
+	PRINTF("Channel 1, DCU1 Init (u00) \r\n");
+	i2c_write(pXfer,  I2C_ECP5_ADDR, (uint8_t*) &u_init, sizeof(u_init));
+	i2c_read(pXfer,   I2C_ECP5_ADDR, (uint8_t*) pBuff, 8);
+	dump_data((uint8_t*) pBuff, 8, true);
+
+	PRINTF("Remove SERDES Channel Resets (c555) :\r\n");
+	i2c_write(pXfer,  I2C_ECP5_ADDR, (uint8_t*) &c_init0, sizeof(c_init0));
+	i2c_read(pXfer,   I2C_ECP5_ADDR, (uint8_t*) pBuff, 8);
+	dump_data((uint8_t*) pBuff, 8, true);
+
+    for (int i=0; i<20; i++) {
+            PRINTF("Check PCS Status (p) \r\n");
+            i2c_write(pXfer, I2C_ECP5_ADDR, (uint8_t*) &pcs_s, sizeof(pcs_s));
+            i2c_read(pXfer, I2C_ECP5_ADDR, (uint8_t*) pBuff, 8);
+            dump_data((uint8_t*) pBuff, 8, true);
+
+            if ( pBuff[3] & PLL_LOL_0 ) {
+                    PRINTF("Keep Testing Until PLL Lock\r\n");
+                    if (i==19) {
+                            // give up
+                            ecp5_pcs_reset(pXfer);
+                            return -1;
+                    }
+            }
+            else {
+                    break;
+            }
+    }
+
+
+	PRINTF("Remove PCS Channel Resets (c000) :\r\n");
+	i2c_write(pXfer,  I2C_ECP5_ADDR, (uint8_t*) &c_init1, sizeof(c_init1));
+	i2c_read(pXfer,   I2C_ECP5_ADDR, (uint8_t*) pBuff, 8);
+	dump_data((uint8_t*) pBuff, 8, true);
+
+	PRINTF("Remove SGMII Controller Resets (x000) :\r\n");
+	i2c_write(pXfer,  I2C_ECP5_ADDR, (uint8_t*) &x_init1, sizeof(x_init1));
+	i2c_read(pXfer,   I2C_ECP5_ADDR, (uint8_t*) pBuff, 8);
+	dump_data((uint8_t*) pBuff, 8, true);
+
+	PRINTF("Check PCS Status (p) \r\n");
+	i2c_write(pXfer,  I2C_ECP5_ADDR, (uint8_t*) &pcs_s, sizeof(pcs_s));
+	i2c_read(pXfer,   I2C_ECP5_ADDR, (uint8_t*) pBuff, 8);
+	dump_data((uint8_t*) pBuff, 8, true);
+	errors = ascii_to_bin(pBuff[2]) << 4 | ascii_to_bin(pBuff[3]);
+
+	if ( errors == 0) {
+		return 0;
+	}
+	else {
+		// configure resets back to default, so we can try again
+		ecp5_pcs_reset(pXfer);
+		return -1;
+	}
+
+	return 0;
+}
diff --git a/source/ecp5_driver.h b/source/ecp5_driver.h
new file mode 100644
index 0000000..887ebe3
--- /dev/null
+++ b/source/ecp5_driver.h
@@ -0,0 +1,34 @@
+/*
+ *       ecp5_driver.h
+ *
+ *       Copyright (C) 2018, 2019 Mind Chasers Inc.
+ *
+ *   Licensed under the Apache License, Version 2.0 (the "License");
+ *   you may not use this file except in compliance with the License.
+ *   You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ */
+
+#ifndef ECP5_DRIVER_H_
+#define ECP5_DRIVER_H_
+
+#define PCS_RX_ERROR_0 0x1
+#define PCS_RX_ERROR_1 0x2
+#define PCS_RX_ERROR_2 0x4
+#define PCS_RX_ERROR_3 0x8
+
+#define PCS_TX_PLL_LOL_0 0x10
+#define PCS_TX_PLL_LOL_1 0x20
+
+
+int ecp5_pcs_init(i2c_master_transfer_t*, uint8_t*);
+
+#endif /* ECP5_DRIVER_H_ */
diff --git a/source/flexbus.h b/source/flexbus.h
new file mode 100644
index 0000000..0f89564
--- /dev/null
+++ b/source/flexbus.h
@@ -0,0 +1,48 @@
+/*
+ *       flexbus.h
+ *
+ *       Copyright (C) 2018, 2019 Mind Chasers Inc.
+ *
+ *   Licensed under the Apache License, Version 2.0 (the "License");
+ *   you may not use this file except in compliance with the License.
+ *   You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ */
+
+#ifndef FLEXBUS_H_
+#define FLEXBUS_H_
+
+
+// CREGS Bit Definitions
+#define MEM_SEL_DCU		0x0
+#define	MEM_SEL_DPR		0x1
+#define RESET_HFIFO_PTRS 0x4
+
+
+// Flex Bus Memory Map
+#define MRAM_START_ADDRESS 0x60000000U
+#define CREG_MEM_SEL MRAM_START_ADDRESS
+#define CREG_IACK MRAM_START_ADDRESS+1
+
+// Flex Bus Base Offsets for DCU
+#define DCU_SCI_CH0		MRAM_START_ADDRESS+0x000
+#define DCU_SCI_CH1		MRAM_START_ADDRESS+0x040
+#define DCU_DUAL1		MRAM_START_ADDRESS+0x080
+#define DCU_SCI_CH2		MRAM_START_ADDRESS+0x100
+#define DCU_SCI_SPARE	MRAM_START_ADDRESS+0x140
+#define DCU_DUAL0		MRAM_START_ADDRESS+0x180
+
+// Flex Bus Base Offsets for DPRAM
+#define DPRAM_RX		MRAM_START_ADDRESS+0x000
+#define DPRAM_TX		MRAM_START_ADDRESS+0x040
+#define DPRAM_META		MRAM_START_ADDRESS+0x080
+
+#endif /* FLEXBUS_H_ */
diff --git a/source/main.c b/source/main.c
new file mode 100644
index 0000000..51b5daa
--- /dev/null
+++ b/source/main.c
@@ -0,0 +1,272 @@
+/*
+ *       main.c
+ *
+ *       Copyright (C) 2018, 2019 Mind Chasers Inc.
+ *
+ *   Licensed under the Apache License, Version 2.0 (the "License");
+ *   you may not use this file except in compliance with the License.
+ *   You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ */
+
+#include <stdio.h>
+#include "board.h"
+#include "peripherals.h"
+#include "pin_mux.h"
+#include "clock_config.h"
+#include "MK02F12810.h"
+#include "fsl_debug_console.h"
+#include "fsl_gpio.h"
+#include "fsl_i2c.h"
+#include "fsl_dspi.h"
+#include "pin_mux.h"
+#include "fsl_pit.h"
+
+#include "pmic.h"
+#include "utils.h"
+#include "cmds.h"
+#include "spi.h"
+#include "pwr_driver.h"
+#include "phy_driver.h"
+#include "ecp5_driver.h"
+
+#include "main.h"
+
+
+/*******************************************************************************
+ * Global Variables
+ ******************************************************************************/
+bool power = true;
+volatile int pit_cnt = 40;
+bool interactive = true;
+uint8_t cnt_scl_clk = 0;
+uint8_t cnt_bit = 0;
+uint8_t buf[100];
+i2c_master_config_t i2c_masterConfig;
+i2c_master_transfer_t i2c_masterXfer;
+
+/*
+ * 		Todo: Make sure the operations are atomic inside and outside the ISR
+ */
+void PIT0_IRQHandler(void) {
+	/* Clear interrupt flag.*/
+	PIT_ClearStatusFlags(PIT, kPIT_Chnl_0, kPIT_TimerFlag);
+	pit_cnt--;
+	if (pit_cnt == 0) {
+		pit_cnt = 40;
+	}
+}
+
+/*!
+ * @brief PHY Interrupt.  Handle PTD5 (fpga_int)
+ *
+ */
+void PORTD_IRQHandler(void) {
+	uint8_t data;
+	volatile uint32_t temp;
+
+	i2c_write(&i2c_masterXfer,  I2C_ECP5_ADDR, (uint8_t*) &c_init_pcsr, sizeof(c_init_pcsr));
+	i2c_write(&i2c_masterXfer,  I2C_ECP5_ADDR, (uint8_t*) &c_init1, sizeof(c_init1));
+
+	temp = GPIO_PinRead(GPIOD, 0x5);
+
+	// clear int src bits
+	data=0;
+	write_spi(SPI_DEV_AD_INT_SEL, 1, &data, 1);
+
+	/* Clear external interrupt flag. */
+	GPIO_PortClearInterruptFlags(GPIOD, 0x1<<5);
+
+	temp = GPIO_PortGetInterruptFlags(GPIOD);
+
+}
+
+
+/*
+ * @brief   Application entry point.
+ */
+int main(void) {
+	int size;
+	uint32_t sourceClock;
+	volatile static int i = 0;
+	volatile uint32_t temp;
+	pit_config_t pitConfig;				// Periodic Interrupt Timer
+
+
+	gpio_pin_config_t pb7;			// (10) PTE24
+	gpio_pin_config_t pb6;			// (11) PTE25
+
+	gpio_pin_config_t pmic_dcdc_e;	// (17) PTA18
+	gpio_pin_config_t pmic_ldo_e;	// (18) PTA19
+
+	gpio_pin_config_t fcfg_pgmn;	// (20) PTB0
+	gpio_pin_config_t fcfg_done;	// (21) PTB1
+
+	gpio_pin_config_t fpga_gpio;	// (22) PTC1
+	gpio_pin_config_t fpga_resetn;	// (29) PTD4
+	gpio_pin_config_t fpga_int;		// (30) PTD5
+
+    dspi_master_config_t spi_masterConfig;
+
+
+  	/* Init board hardware. */
+    BOARD_InitBootPins();
+    BOARD_InitBootClocks();
+    BOARD_InitBootPeripherals();
+	BOARD_InitDebugConsole();
+
+	/* Init GPIO */
+	pmic_ldo_e.pinDirection = kGPIO_DigitalOutput;
+	pmic_ldo_e.outputLogic = 0;
+	GPIO_PinInit(GPIOA, 19, &pmic_ldo_e);
+
+	// Negate FPGA_RESETN
+	GPIO_WritePinOutput(BOARD_INITPINS_FPGA_RESETN_GPIO, BOARD_INITPINS_FPGA_RESETN_PIN, 1);
+
+	/*
+	 * Initialize PIT module
+	 * pitConfig.enableRunInDebug = false;
+	 * Calls CLOCK_EnableCLock to enable PIT clock (gate control) in SIM_SCGC6
+	 * Configures PIT MCR register to enable clock for PIT timer and disable run during debug
+	 */
+	PIT_GetDefaultConfig(&pitConfig);
+	PIT_Init(PIT, &pitConfig);
+
+	/* Set timer period for channel 0,
+	 * calls CLOCK_GetFreq()
+	 * sets PIT_LDVAL0
+	 */
+	PIT_SetTimerPeriod(PIT, kPIT_Chnl_0, USEC_TO_COUNT(100U, CLOCK_GetFreq(kCLOCK_BusClk)));
+
+
+	/* Enable timer interrupts for channel 0 ,
+	 * set PIT_TCTRL0 = 2 (Timer Interrupt Enable )
+	 */
+	PIT_EnableInterrupts(PIT, kPIT_Chnl_0, kPIT_TimerInterruptEnable);
+
+	/* Start PIT channel 0 */
+	PIT_StartTimer(PIT, kPIT_Chnl_0);
+
+
+	printf("Initialize I2C0.\n");
+	I2C_MasterGetDefaultConfig(&i2c_masterConfig);
+	i2c_masterConfig.baudRate_Bps = I2C_BAUDRATE;
+
+	sourceClock = CLOCK_GetFreq(I2C_MASTER_CLK_SRC);
+
+	I2C_MasterInit(I2C_MASTER_BASEADDR, &i2c_masterConfig, sourceClock);
+
+	memset(&i2c_masterXfer, 0, sizeof(i2c_masterXfer));
+
+	// set up defaults applicable to all transfers
+	i2c_masterXfer.subaddress = 0;
+	i2c_masterXfer.subaddressSize = 0;
+	i2c_masterXfer.flags = kI2C_TransferDefaultFlag;
+
+	if (power) {
+		printf("Initialize Power\n\r");
+		power_init(&i2c_masterXfer, buf);
+	}
+
+	// wait for the FPGA to be ready
+	// TODO: add a robust handshake that also interrogates the FPGA image
+	while (i2c_read(&i2c_masterXfer, I2C_ECP5_ADDR, buf, 8) < 0) {
+		PRINTF(".");
+	}
+
+	spi_init(spi_masterConfig);
+
+
+
+
+	PRINTF("\r\n Darsena! \r\n");
+	i=0;
+	while (phy_init(&i2c_masterXfer, buf) < 0) {
+		i++;
+	}
+
+	i=0;
+	while (ecp5_pcs_init(&i2c_masterXfer, buf) < 0) {
+		i++;
+
+	}
+
+	/*
+    buf[0]=192;
+    buf[1]=168;
+    buf[2]=3;
+    buf[3]=1;
+
+	for (i = 0; i< 100; i++) {
+		PRINTF("\r\n test_spi() \r\n");
+		write_spi(SPI_DEV_AD_DPRAM_RX, 0, buf, 4);
+		read_spi(SPI_DEV_AD_DPRAM_RX, 0, buf, 4);
+
+		write_spi(SPI_DEV_AD_DPRAM_TX, 0, buf, 4);
+		read_spi(SPI_DEV_AD_DPRAM_TX, 0, buf, 4);
+
+		write_spi(SPI_DEV_AD_PKT_FILTER_01, 0, buf, 4);
+
+		read_spi(SPI_DEV_AD_SCI_SEL_CH_0, 0, buf, 4);
+	}
+	*/
+	temp = GPIO_PortGetInterruptFlags(GPIOD);
+	EnableIRQ(PIT0_IRQn);
+	EnableIRQ(PORTD_IRQn);
+
+	while(interactive) {
+
+			PRINTF("\r\n I2C Command Line.  Hit enter to continue \r\n");
+			DbgConsole_Flush();
+
+			do {
+					size = get_cmd(buf, 10);
+					PRINTF("\r\n received %d bytes \r\n", size);
+
+					if (size != 0) {
+						if (buf[0] == 'T') {
+							phy_compliance_test(&i2c_masterXfer, buf[1]);
+						}
+						else {
+							i2c_write(&i2c_masterXfer, I2C_ECP5_ADDR, buf, size);
+							i2c_read(&i2c_masterXfer, I2C_ECP5_ADDR, buf, 8);
+							dump_data((uint8_t*) buf, 4, true );
+						}
+					}
+
+
+
+			} while(size != 0);
+
+	}
+
+
+	/* Force the counter to be placed into memory. */
+
+    while(1) {
+    	PRINTF(".");
+    	i++;
+    }
+    return 0 ;
+}
+
+/*
+    	p_reg = (uint32_t*) 0x40037104;
+    	temp_reg = *p_reg;
+    	temp_reg = PIT_GetStatusFlags(PIT, kPIT_Chnl_0);
+    	if (temp_reg != 0) {
+    		// *pTFLG = 1;
+    		temp_reg = __get_PRIMASK();
+    		temp_reg = __get_IPSR();
+    		temp_reg = NVIC_GetActive(PIT0_IRQn);
+    		temp_reg = NVIC_GetEnableIRQ(PIT0_IRQn);
+    	}
+ */
diff --git a/source/main.h b/source/main.h
new file mode 100644
index 0000000..978e42c
--- /dev/null
+++ b/source/main.h
@@ -0,0 +1,42 @@
+/*
+ *       main.h
+ *
+ *       Copyright (C) 2018, 2019 Mind Chasers Inc.
+ *
+ *   Licensed under the Apache License, Version 2.0 (the "License");
+ *   you may not use this file except in compliance with the License.
+ *   You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ */
+
+
+#ifndef SOURCES_MAIN_H_
+#define SOURCES_MAIN_H_
+
+#include <stdint.h>
+
+/* board level I2C defines */
+#define I2C_MASTER_BASEADDR I2C0
+#define I2C_MASTER_CLK_SRC I2C0_CLK_SRC
+#define I2C_DATA_MAX_LENGTH 257U
+/* 200KHz */
+#define I2C_BAUDRATE 200000U
+
+#define I2C_ECP5_ADDR (uint8_t) 0x10U
+#define I2C_PMIC_ADDR (uint8_t) 0x48U
+
+void PIT0_IRQHandler(void) __attribute__ ((isr));
+void PORTD_IRQHandler(void) __attribute__ ((isr));
+
+status_t i2c_write(i2c_master_transfer_t* pxfer, uint8_t, uint8_t * pdata, size_t size);
+status_t i2c_read(i2c_master_transfer_t* pxfer, uint8_t, uint8_t * pdata, size_t size);
+
+#endif /* SOURCES_MAIN_H_ */
diff --git a/source/phy_driver.c b/source/phy_driver.c
new file mode 100644
index 0000000..fa0522f
--- /dev/null
+++ b/source/phy_driver.c
@@ -0,0 +1,234 @@
+/*
+ *       phy_driver.c
+ *
+ *       Copyright (C) 2018, 2019 Mind Chasers Inc.
+ *
+ *   Licensed under the Apache License, Version 2.0 (the "License");
+ *   you may not use this file except in compliance with the License.
+ *   You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "phy_driver.h"
+#include "board.h"
+#include "fsl_debug_console.h"
+#include "fsl_i2c.h"
+#include "ecp5_driver.h"
+#include "cmds.h"
+#include "utils.h"
+#include "main.h"
+
+
+/*
+ *	Test 0 resets the PHY
+ */
+
+void phy_compliance_test(i2c_master_transfer_t* pXfer, uint8_t test) {
+
+	PRINTF("PHY TEST %d\n\r", test);
+	if (test == 0) {
+		write_mdio(pXfer, 0x1F, 0x8000);	// reset PHY
+	}
+	else if (test == 1) {
+		write_mdio(pXfer, 0x1F, 0x8000);	// reset PHY
+		write_mdio(pXfer, 0x00, 0x0140);	// 1000 Base-T mode
+		write_mdio(pXfer, 0x10, 0x5008);	// forced MDI mode
+		write_mdio(pXfer, 0x09, 0x3B00);	// Test Mode 1
+		write_mdio(pXfer, 0x25, 0x0480);	// Output test mode to all channels
+		write_mdio(pXfer, 0x1D5, 0xF508);
+	}
+	else if (test == 2) {
+		write_mdio(pXfer, 0x1F, 0x8000);	// reset PHY
+		write_mdio(pXfer, 0x00, 0x0140);	// 1000 Base-T mode
+		write_mdio(pXfer, 0x10, 0x5008);	// forced MDI mode
+		write_mdio(pXfer, 0x09, 0x5B00);	// Test Mode 2
+		write_mdio(pXfer, 0x25, 0x0480);	// Output test mode to all channels
+	}
+	else if (test == 3) {
+		write_mdio(pXfer, 0x1F, 0x8000);	// reset PHY
+		write_mdio(pXfer, 0x00, 0x0140);	// 1000 Base-T mode
+		write_mdio(pXfer, 0x10, 0x5008);	// forced MDI mode
+		write_mdio(pXfer, 0x09, 0x7B00);	// Test Mode 3
+		write_mdio(pXfer, 0x25, 0x0480);	// Output test mode to all channels
+	}
+	else if (test == 4) {
+		write_mdio(pXfer, 0x1F, 0x8000);	// reset PHY
+		write_mdio(pXfer, 0x00, 0x0140);	// 1000 Base-T mode
+		write_mdio(pXfer, 0x10, 0x5008);	// forced MDI mode
+		write_mdio(pXfer, 0x09, 0x9B00);	// Test Mode 3
+		write_mdio(pXfer, 0x25, 0x0400);	// Channel A
+	}
+	else {
+		PRINTF("not supported\n\r");
+	}
+
+}
+
+
+/*
+ * 	Write using Extended Addressing
+ */
+void write_mdio(i2c_master_transfer_t* pXfer, uint16_t addr, uint16_t data) {
+	/* set the write address first with a read */
+	read_mdio(pXfer, addr);
+
+	uint8_t buf[16];
+	buf[0]='z';
+	buf[1] = (data & 0xf000) >> 12;
+	buf[2] = (data & 0x0f00) >> 8;
+	buf[3] = (data & 0x00f0) >> 4;
+	buf[4] = data & 0x000f;
+	buf[5]=0xd;
+
+	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 6);
+	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);
+}
+/*
+ * Read using Extended Addressing.
+ */
+uint16_t read_mdio(i2c_master_transfer_t* pXfer, uint16_t addr) {
+	uint16_t temp;
+	uint8_t buf[16];
+
+	buf[0]='y';
+	buf[1] = (addr & 0xf000) >> 12;
+	buf[2] = (addr & 0x0f00) >> 8;
+	buf[3] = (addr & 0x00f0) >> 4;
+	buf[4] = addr & 0x000f;
+	buf[5]=0xd;
+
+	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 6);
+	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);
+
+	temp = (buf[0] & 0xf) << 12 | (buf[1] & 0xf) << 8 | (buf[2] & 0xf) << 4 | (buf[3] & 0xf);
+	return temp;
+}
+
+int invert_sgmii_polarity(i2c_master_transfer_t* pXfer, uint8_t phy) {
+	uint8_t buf[16];
+
+	// set the right PHY mux_sel
+	buf[0]='m';
+	buf[1]=phy;
+	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 2);
+	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);
+
+	// Extended Write of 0x01D5
+
+	// Read 0xD
+	buf[0]='r';
+	buf[1]=0x0;
+	buf[2]=0xd;
+	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 3);
+	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);
+
+	// Write 0x001f to 0xD
+	buf[0]='w';
+	buf[1]=0x0;
+	buf[2]=0x0;
+	buf[3]=0x1;
+	buf[4]=0xf;
+	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 5);
+	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);
+
+	// Read 0xE to set address
+	buf[0]='r';
+	buf[1]=0x0;
+	buf[2]=0xe;
+	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 3);
+	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);
+
+	// Write the extended address 0x01D0 to 0xe
+	buf[0]='w';
+	buf[1]=0x0;
+	buf[2]=0x1;
+	buf[3]=0xD;
+	buf[4]=0x5;
+	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 5);
+	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);
+
+	// Read 0xD to set address
+	buf[0]='r';
+	buf[1]=0x0;
+	buf[2]=0xd;
+	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 3);
+	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);
+
+	// Write 0x401f to 0xD
+	buf[0]='w';
+	buf[1]=0x4;
+	buf[2]=0x0;
+	buf[3]=0x1;
+	buf[4]=0xf;
+	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 5);
+	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);
+
+	// Read 0xE to set address
+	buf[0]='r';
+	buf[1]=0x0;
+	buf[2]=0xe;
+	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 3);
+	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);
+
+	// Write inversion bits to 0xE
+	buf[0]='w';
+	buf[1]=0x0;
+	buf[2]=0x0;
+	buf[3]=0x0;
+	buf[4]=0x3;
+	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 5);
+	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);
+
+	// Perform a soft reset: bit 15 at 0x1f (CTRL Reg)
+
+	// Read 0x1F to set address
+	buf[0]='r';
+	buf[1]=0x1;
+	buf[2]=0xf;
+	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 3);
+	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);
+
+	// Write bit 14 (0x8000) (SW reset, not including registers)
+	buf[0]='w';
+	buf[1]=0x4;
+	buf[2]=0x0;
+	buf[3]=0x0;
+	buf[4]=0x0;
+	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 5);
+	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);
+
+	return(0);
+
+}
+
+
+
+/* The assumption is that the PHY is brought up before the FPGA / Switch */
+//TODO: Should read the other values, so we don't change them / affect other parts of the register */
+int phy_init(i2c_master_transfer_t* pXfer, uint8_t* pBuff) {
+
+	PRINTF("Take PHYs out of reset (x111) \r\n");
+	i2c_write(pXfer, I2C_ECP5_ADDR, (uint8_t*) &x_init0, sizeof(x_init0));
+	i2c_read(pXfer,  I2C_ECP5_ADDR, (uint8_t*) pBuff, 8);
+	dump_data((uint8_t*) pBuff, 8, true);
+
+	/*
+	invert_sgmii_polarity(0);
+	invert_sgmii_polarity(1);
+	*/
+
+	return(0);
+
+}
+
+
diff --git a/source/phy_driver.h b/source/phy_driver.h
new file mode 100644
index 0000000..df6d5bc
--- /dev/null
+++ b/source/phy_driver.h
@@ -0,0 +1,33 @@
+/*
+ *       phy_driver.h
+ *
+ *       Copyright (C) 2018, 2019 Mind Chasers Inc.
+ *
+ *   Licensed under the Apache License, Version 2.0 (the "License");
+ *   you may not use this file except in compliance with the License.
+ *   You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ */
+
+#ifndef PHY_DRIVER_H_
+#define PHY_DRIVER_H_
+
+#include <stdint.h>
+#include "fsl_i2c.h"
+
+void phy_compliance_test(i2c_master_transfer_t*, uint8_t);
+
+void write_mdio(i2c_master_transfer_t*, uint16_t, uint16_t );
+uint16_t read_mdio(i2c_master_transfer_t* , uint16_t );
+
+int phy_init(i2c_master_transfer_t*, uint8_t*);
+
+#endif /* PHY_DRIVER_H_ */
diff --git a/source/pmic.h b/source/pmic.h
new file mode 100644
index 0000000..06ac9fb
--- /dev/null
+++ b/source/pmic.h
@@ -0,0 +1,49 @@
+/*
+ *       pmic.h
+ *
+ *       Copyright (C) 2018, 2019 Mind Chasers Inc.
+ *
+ *   Licensed under the Apache License, Version 2.0 (the "License");
+ *   you may not use this file except in compliance with the License.
+ *   You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ */
+
+
+#ifndef SOURCES_PMIC_H_
+#define SOURCES_PMIC_H_
+
+#define PMIC_VERSION 	0
+#define PMIC_PGOODZ		1
+#define PMIC_MASK		2
+#define PMIC_REG_CTRL	3
+#define PMIC_CON_CTRL	4
+#define PMIC_CON_CTRL2	5
+#define PMIC_DEFCORE	6
+#define PMIC_DEFSLEW	7
+#define PMIC_LDO_CTRL	8
+
+// PMIC_PGOODZ
+#define PGOODZ_LDO1		2
+
+// PMIC_REG_CTRL
+#define LDO1_ENABLE		0x02
+#define LDO2_ENABLE		0x04
+#define VDDC3_ENABLE	0x08
+#define VDDC2_ENABLE	0x10
+#define VDDC1_ENABLE	0x20
+
+// PMIC_LDO_CTRL
+// LDO2 = 2.5V, LD01 = 1.1V
+#define LDO1_1_1V		1
+#define	LDO2_2_5V		4
+
+#endif /* SOURCES_PMIC_H_ */
diff --git a/source/pwr_driver.c b/source/pwr_driver.c
new file mode 100644
index 0000000..37abbd4
--- /dev/null
+++ b/source/pwr_driver.c
@@ -0,0 +1,113 @@
+/*
+ *       pwr_driver.c
+ *
+ *       Copyright (C) 2018, 2019 Mind Chasers Inc.
+ *
+ *   Licensed under the Apache License, Version 2.0 (the "License");
+ *   you may not use this file except in compliance with the License.
+ *   You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ */
+
+#include "board.h"
+#include "pmic.h"
+#include "fsl_debug_console.h"
+#include "fsl_common.h"
+#include "fsl_i2c.h"
+#include "fsl_gpio.h"
+#include "main.h"
+
+int power_init(i2c_master_transfer_t* pXfer, uint8_t* pBuff) {
+
+	bool repeat = false;
+	uint8_t addr,data;
+	uint8_t buf[2];	// write address followed by data
+_loop:
+
+	PRINTF("Power Init\r\n");
+
+	// Read version
+	addr=PMIC_VERSION;
+	i2c_write(pXfer,  I2C_PMIC_ADDR, &addr, 1);
+	i2c_read(pXfer,  I2C_PMIC_ADDR, (uint8_t*) pBuff, 1);
+	//PRINTF("Version: 0x%2x\r\n", *pBuff);
+
+	// Read PGOODZ
+	addr=PMIC_PGOODZ;
+	i2c_write(pXfer,  I2C_PMIC_ADDR, &addr, 1);
+	i2c_read(pXfer,  I2C_PMIC_ADDR, (uint8_t*) pBuff, 1);
+	//PRINTF("PGOODZ: 0x%2x\r\n", *pBuff);
+
+	// Read MASK
+	addr=PMIC_MASK;
+	i2c_write(pXfer,  I2C_PMIC_ADDR, &addr, 1);
+	i2c_read(pXfer,  I2C_PMIC_ADDR, (uint8_t*) pBuff, 1);
+	//PRINTF("MASK: 0x%2x\r\n", *pBuff);
+
+	// Read REG_CTRL
+	addr=PMIC_REG_CTRL;
+	i2c_write(pXfer,  I2C_PMIC_ADDR, &addr, 1);
+	i2c_read(pXfer,  I2C_PMIC_ADDR, (uint8_t*) pBuff, 1);
+	//PRINTF("REG_CTRL: 0x%2x\r\n", *pBuff);
+
+	// Read CON_CTRL
+	addr=PMIC_CON_CTRL;
+	i2c_write(pXfer,  I2C_PMIC_ADDR, &addr, 1);
+	i2c_read(pXfer,  I2C_PMIC_ADDR, (uint8_t*) pBuff, 1);
+	// PRINTF("CON_CTRL: 0x%2x\r\n", *pBuff);
+
+	// Read CON_CTRL2
+	addr=PMIC_CON_CTRL2;
+	i2c_write(pXfer,  I2C_PMIC_ADDR, &addr, 1);
+	i2c_read(pXfer,  I2C_PMIC_ADDR, (uint8_t*) pBuff, 1);
+	//PRINTF("CON_CTRL2: 0x%2x\r\n", *pBuff);
+
+	// Read DEFCORE
+	// We want 1.1V: 0xc
+	addr=PMIC_DEFCORE;
+	i2c_write(pXfer,  I2C_PMIC_ADDR, &addr, 1);
+	i2c_read(pXfer,  I2C_PMIC_ADDR, (uint8_t*) pBuff, 1);
+	//PRINTF("DEFSLEW: 0x%2x\r\n", *pBuff);
+
+	// Read DEFSLEW
+	addr=PMIC_DEFSLEW;
+	i2c_write(pXfer,  I2C_PMIC_ADDR, &addr, 1);
+	i2c_read(pXfer,  I2C_PMIC_ADDR, (uint8_t*) pBuff, 1);
+	//PRINTF("DEFSLEW: 0x%2x\r\n", *pBuff);
+
+	// Read LDO_CTRL
+	addr=PMIC_LDO_CTRL;
+	i2c_write(pXfer,  I2C_PMIC_ADDR, &addr, 1);
+	i2c_read(pXfer,  I2C_PMIC_ADDR, (uint8_t*) pBuff, 1);
+	//PRINTF("LDO_CTRL: 0x%2x\r\n", *pBuff);
+
+	// Write LDO_CTRL
+	addr=PMIC_LDO_CTRL;
+	data=LDO2_2_5V<<4|LDO1_1_1V;
+	buf[0]=addr;
+	buf[1]=data;
+	i2c_write(pXfer,  I2C_PMIC_ADDR, buf, 2);
+	i2c_read(pXfer,  I2C_PMIC_ADDR, (uint8_t*) pBuff, 1);
+	//PRINTF("LDO_CTRL: 0x%2x\r\n", *pBuff);
+
+	GPIO_WritePinOutput(GPIOA, 19, 1);
+
+	// Read PGOODZ
+	addr=PMIC_PGOODZ;
+	i2c_write(pXfer,  I2C_PMIC_ADDR, &addr, 1);
+	i2c_read(pXfer,  I2C_PMIC_ADDR, (uint8_t*) pBuff, 1);
+	//PRINTF("PGOODZ: 0x%2x\r\n", *pBuff);
+
+
+	if (repeat) goto _loop;
+
+	return 0;
+}
diff --git a/source/pwr_driver.h b/source/pwr_driver.h
new file mode 100644
index 0000000..4a63956
--- /dev/null
+++ b/source/pwr_driver.h
@@ -0,0 +1,26 @@
+/*
+ *   pwr_driver.h
+ *
+ *   Copyright (C) 2018, 2019 Mind Chasers Inc.
+ *
+ *   Licensed under the Apache License, Version 2.0 (the "License");
+ *   you may not use this file except in compliance with the License.
+ *   You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ */
+
+#ifndef SOURCES_PWR_DRIVER_H_
+#define SOURCES_PWR_DRIVER_H_
+
+int power_init(i2c_master_transfer_t*, uint8_t*);
+
+
+#endif /* SOURCES_PWR_DRIVER_H_ */
diff --git a/source/semihost_hardfault.c b/source/semihost_hardfault.c
new file mode 100644
index 0000000..dc8d3ae
--- /dev/null
+++ b/source/semihost_hardfault.c
@@ -0,0 +1,109 @@
+// ****************************************************************************
+// semihost_hardfault.c
+//                - Provides hard fault handler to allow semihosting code not
+//                  to hang application when debugger not connected.
+//
+// ****************************************************************************
+// Copyright(C) NXP Semiconductors, 2017-2018
+// All rights reserved.
+//
+// Software that is described herein is for illustrative purposes only
+// which provides customers with programming information regarding the
+// NXP Cortex-M based MCUs. This software is supplied "AS IS" without any
+// warranties of any kind, and NXP Semiconductors and its licensor disclaim any
+// and all warranties, express or implied, including all implied warranties of
+// merchantability, fitness for a particular purpose and non-infringement of
+// intellectual property rights.  NXP Semiconductors assumes no responsibility
+// or liability for the use of the software, conveys no license or rights under
+// any patent, copyright, mask work right, or any other intellectual property
+// rights in or to any products. NXP Semiconductors reserves the right to make
+// changes in the software without notification. NXP Semiconductors also makes
+// no representation or warranty that such application will be suitable for the
+// specified use without further testing or modification.
+//
+// Permission to use, copy, modify, and distribute this software and its
+// documentation is hereby granted, under NXP Semiconductors' and its
+// licensor's relevant copyrights in the software, without fee, provided that it
+// is used in conjunction with NXP Semiconductors microcontrollers.  This
+// copyright, permission, and disclaimer notice must appear in all copies of
+// this code.
+// ****************************************************************************
+//
+//                       ===== DESCRIPTION =====
+//
+// One of the issues with applications that make use of semihosting operations
+// (such as printf calls) is that the code will not execute correctly when the
+// debugger is not connected. Generally this will show up with the application
+// appearing to just hang. This may include the application running from reset
+// or powering up the board (with the application already in FLASH), and also
+// as the application failing to continue to execute after a debug session is
+// terminated.
+//
+// The problem here is that the "bottom layer" of the semihosted variants of
+// the C library, semihosting is implemented by a "BKPT 0xAB" instruction.
+// When the debug tools are not connected, this instruction triggers a hard
+// fault - and the default hard fault handler within an application will
+// typically just contains an infinite loop - causing the application to
+// appear to have hang when no debugger is connected.
+//
+// The below code provides an example hard fault handler which instead looks
+// to see what the instruction that caused the hard fault was - and if it
+// was a "BKPT 0xAB", then it instead returns back to the user application.
+//
+// In most cases this will allow applications containing semihosting
+// operations to execute (to some degree) when the debugger is not connected.
+//
+// == NOTE ==
+//
+// Correct execution of the application containing semihosted operations
+// which are vectored onto this hard fault handler cannot be guaranteed. This
+// is because the handler may not return data or return codes that the higher
+// level C library code or application code expects. This hard fault handler
+// is meant as a development aid, and it is not recommended to leave
+// semihosted code in a production build of your application!
+//
+// ****************************************************************************
+
+// Allow handler to be removed by setting a define (via command line)
+#if !defined (__SEMIHOST_HARDFAULT_DISABLE)
+
+__attribute__((naked))
+void HardFault_Handler(void){
+    __asm(  ".syntax unified\n"
+        // Check which stack is in use
+            "MOVS   R0, #4  \n"
+            "MOV    R1, LR  \n"
+            "TST    R0, R1  \n"
+            "BEQ    _MSP    \n"
+            "MRS    R0, PSP \n"
+            "B  _process      \n"
+            "_MSP:  \n"
+            "MRS    R0, MSP \n"
+        // Load the instruction that triggered hard fault
+        "_process:     \n"
+            "LDR    R1,[R0,#24] \n"
+            "LDRH    R2,[r1] \n"
+        // Semihosting instruction is "BKPT 0xAB" (0xBEAB)
+            "LDR    R3,=0xBEAB \n"
+            "CMP     R2,R3 \n"
+            "BEQ    _semihost_return \n"
+        // Wasn't semihosting instruction so enter infinite loop
+            "B . \n"
+        // Was semihosting instruction, so adjust location to
+        // return to by 1 instruction (2 bytes), then exit function
+        "_semihost_return: \n"
+            "ADDS    R1,#2 \n"
+            "STR    R1,[R0,#24] \n"
+    	// Set a return value from semihosting operation.
+    	// 32 is slightly arbitrary, but appears to allow most
+    	// C Library IO functions sitting on top of semihosting to
+    	// continue to operate to some degree
+    		    "MOVS   R1,#32 \n"
+    		    "STR R1,[ R0,#0 ] \n" // R0 is at location 0 on stack
+    	// Return from hard fault handler to application
+            "BX LR \n"
+        ".syntax divided\n") ;
+}
+
+#endif
+
diff --git a/source/spi.c b/source/spi.c
new file mode 100644
index 0000000..aa36877
--- /dev/null
+++ b/source/spi.c
@@ -0,0 +1,159 @@
+/*
+ *       spi.c
+ *
+ *       Copyright (C) 2018, 2019 Mind Chasers Inc.
+ *
+ *   Licensed under the Apache License, Version 2.0 (the "License");
+ *   you may not use this file except in compliance with the License.
+ *   You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ */
+
+
+#include "fsl_debug_console.h"
+#include "fsl_dspi.h"
+#include "stdint.h"
+#include "spi.h"
+
+int spi_init(dspi_master_config_t spi_masterConfig) {
+    uint32_t srcClock_Hz;
+    spi_masterConfig.whichCtar = kDSPI_Ctar0;
+    spi_masterConfig.ctarConfig.baudRate = DSPI_TRANSFER_BAUDRATE;
+    spi_masterConfig.ctarConfig.bitsPerFrame = 9U;
+    spi_masterConfig.ctarConfig.cpol = kDSPI_ClockPolarityActiveHigh;
+    spi_masterConfig.ctarConfig.cpha = kDSPI_ClockPhaseFirstEdge;
+    spi_masterConfig.ctarConfig.direction = kDSPI_MsbFirst;
+    spi_masterConfig.ctarConfig.pcsToSckDelayInNanoSec = 1000000000U / DSPI_TRANSFER_BAUDRATE;
+    spi_masterConfig.ctarConfig.lastSckToPcsDelayInNanoSec = 1000000000U / DSPI_TRANSFER_BAUDRATE;
+    spi_masterConfig.ctarConfig.betweenTransferDelayInNanoSec = 1000000000U / DSPI_TRANSFER_BAUDRATE;
+
+    spi_masterConfig.whichPcs = kDSPI_Pcs0;
+    spi_masterConfig.pcsActiveHighOrLow = kDSPI_PcsActiveHigh;
+
+    spi_masterConfig.enableContinuousSCK = false;
+    spi_masterConfig.enableRxFifoOverWrite = false;
+    spi_masterConfig.enableModifiedTimingFormat = false;
+    spi_masterConfig.samplePoint = kDSPI_SckToSin0Clock;
+
+    srcClock_Hz = DSPI_MASTER_CLK_FREQ;
+    DSPI_MasterInit(DSPI_MASTER_BASEADDR, &spi_masterConfig, srcClock_Hz);
+
+	return(0);
+}
+
+int write_spi(uint8_t dev_ad, uint8_t addr, uint8_t* pBuff, uint8_t sz) {
+	uint16_t transfer_cnt = (sz+2)*2;
+    uint16_t masterRxData[256] = {0U};
+    uint16_t masterTxData[256] = {0U};
+    dspi_transfer_t masterXfer;
+
+    // SPI_RWN = 0 for write, so no operation is required
+    masterTxData[0] = (dev_ad<<1);
+    masterTxData[1] = addr;			// addr
+
+    for (int i = 2; i < sz+2; i++ ) {
+    	masterTxData[i] = *pBuff++;
+    }
+
+	/* Start master transfer, send data to slave */
+	masterXfer.txData = (uint8_t*) masterTxData;
+	masterXfer.rxData = (uint8_t*) masterRxData;
+	masterXfer.dataSize = transfer_cnt;
+	masterXfer.configFlags = kDSPI_MasterCtar0 | kDSPI_MasterPcs0 | kDSPI_MasterPcsContinuous;
+	DSPI_MasterTransferBlocking(DSPI_MASTER_BASEADDR, &masterXfer);
+
+	return(0);
+
+}
+
+int read_spi(uint8_t dev_ad, uint8_t addr, uint8_t* pBuff, uint8_t sz) {
+	uint16_t transfer_cnt = (sz+2)*2;
+    uint16_t masterRxData[256] = {0U};
+    uint16_t masterTxData[256] = {0U};
+    dspi_transfer_t masterXfer;
+
+    masterTxData[0] = (dev_ad<<1) | SPI_READ;
+    masterTxData[1] = addr;
+
+	/* Start master transfer, send data to slave */
+	masterXfer.txData = (uint8_t*) masterTxData;
+	masterXfer.rxData = (uint8_t*) masterRxData;
+	masterXfer.dataSize = transfer_cnt;
+	masterXfer.configFlags = kDSPI_MasterCtar0 | kDSPI_MasterPcs0 | kDSPI_MasterPcsContinuous;
+	DSPI_MasterTransferBlocking(DSPI_MASTER_BASEADDR, &masterXfer);
+
+	return(0);
+
+}
+
+
+int test_spi(uint8_t rwn, uint8_t dev_ad) {
+
+	volatile static int i = 0;
+	uint16_t transfer_cnt = 10;
+    uint16_t masterRxData[256] = {0U};
+    uint16_t masterTxData[256] = {0U};
+    dspi_transfer_t masterXfer;
+
+    if (!rwn) {
+		// write TX buffer
+		/* Set up the transfer data */
+		masterTxData[0] = (dev_ad<<1) | rwn;	// dev_ad|RWN
+		masterTxData[1] = 0x01;			// addr
+		masterTxData[2] = 0x77;			// data
+		masterTxData[3] = 0x33;			// data
+		masterTxData[4] = 0xaa;			// data
+    }
+    else {
+    	// read TX buffer
+    	masterTxData[0] = (dev_ad<<1) | rwn;	// dev_ad|RWN
+    	masterTxData[1] = 0x01;			// addr
+    	masterTxData[2] = 0x00;			// data
+    	masterTxData[3] = 0x00;			// data
+    	masterTxData[4] = 0x00;			// data
+    }
+
+	/* Print out transmit buffer */
+	PRINTF("\r\n Master transmit:\r\n");
+	for (i = 0U; i < transfer_cnt; i++)
+	{
+		/* Print 16 numbers in a line */
+		if ((i & 0x0FU) == 0U)
+		{
+			PRINTF("\r\n");
+		}
+		PRINTF(" %02X", masterTxData[i]);
+	}
+	PRINTF("\r\n");
+
+	/* Start master transfer, send data to slave */
+	masterXfer.txData = (uint8_t*) masterTxData;
+	masterXfer.rxData = (uint8_t*) masterRxData;
+	masterXfer.dataSize = transfer_cnt;
+	masterXfer.configFlags = kDSPI_MasterCtar0 | kDSPI_MasterPcs0 | kDSPI_MasterPcsContinuous;
+	DSPI_MasterTransferBlocking(DSPI_MASTER_BASEADDR, &masterXfer);
+
+	/* Print out receive buffer */
+	PRINTF("\r\n Master transmit:\r\n");
+	for (i = 0U; i < transfer_cnt; i++)
+	{
+		/* Print 16 numbers in a line */
+		if ((i & 0x0FU) == 0U)
+		{
+			PRINTF("\r\n");
+		}
+		PRINTF(" %02X", masterRxData[i]);
+	}
+	PRINTF("\r\n");
+
+    return(0);
+
+}
diff --git a/source/spi.h b/source/spi.h
new file mode 100644
index 0000000..100a73c
--- /dev/null
+++ b/source/spi.h
@@ -0,0 +1,71 @@
+/*
+ *       spi.h
+ *
+ *       Copyright (C) 2018, 2019 Mind Chasers Inc.
+ *
+ *   Licensed under the Apache License, Version 2.0 (the "License");
+ *   you may not use this file except in compliance with the License.
+ *   You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ */
+
+#ifndef SPI_H_
+#define SPI_H_
+
+/*******************************************************************************
+* SPI Definitions
+******************************************************************************/
+#define DSPI_MASTER_BASEADDR SPI0
+#define DSPI_MASTER_CLK_SRC DSPI0_CLK_SRC
+#define DSPI_MASTER_CLK_FREQ CLOCK_GetFreq(DSPI0_CLK_SRC)
+#define DSPI_TRANSFER_BAUDRATE 1000000U /*! Transfer baudrate - 1M */
+
+/* Memory Address Map
+ * 9-bit words are transferred
+ * DEV_AD, RWN
+ * MEM_AD
+ */
+
+#define SPI_READ 1
+#define SPI_WRITE 0
+
+#define SPI_DEV_AD_SCI_SEL_CH_0		0x00
+#define SPI_DEV_AD_SCI_SEL_CH_1		0x01
+#define SPI_DEV_AD_DUAL_0			0x02
+#define SPI_DEV_AD_SCI_SEL_CH_2		0x04
+#define SPI_DEV_AD_SCI_SEL_CH_3		0x05
+#define SPI_DEV_AD_DUAL_1			0x06
+#define SPI_DEV_AD_DPRAM_RX			0x08
+#define SPI_DEV_AD_DPRAM_TX			0x0c
+#define SPI_DEV_AD_DPRAM_PTRS		0x10
+#define SPI_DEV_AD_INT_SEL			0X11
+#define SPI_DEV_AD_PARAM_0			0x20
+#define SPI_DEV_AD_PARAM_1			0x24
+#define SPI_DEV_AD_PARAM_2			0x28
+#define SPI_DEV_AD_PARAM_2			0x2C
+#define SPI_DEV_AD_PKT_FILTER_01	0X41
+#define SPI_DEV_AD_PKT_FILTER_02	0X42
+#define SPI_DEV_AD_PKT_FILTER_03	0X43
+#define SPI_DEV_AD_PKT_FILTER_10	0X48
+#define SPI_DEV_AD_PKT_FILTER_12	0X4a
+#define SPI_DEV_AD_PKT_FILTER_13	0X4b
+#define SPI_DEV_AD_PKT_FILTER_20	0x50
+#define SPI_DEV_AD_PKT_FILTER_21	0X51
+#define SPI_DEV_AD_PKT_FILTER_23	0X53
+
+
+
+int spi_init(dspi_master_config_t);
+int test_spi(uint8_t, uint8_t);
+int write_spi(uint8_t, uint8_t, uint8_t*, uint8_t);
+int read_spi(uint8_t, uint8_t, uint8_t*, uint8_t);
+
+#endif /* SPI_H_ */
diff --git a/source/utils.c b/source/utils.c
new file mode 100644
index 0000000..5cfce86
--- /dev/null
+++ b/source/utils.c
@@ -0,0 +1,156 @@
+/*
+ *   utils.c
+ *
+ *   Copyright (C) 2018, 2019 Mind Chasers Inc.
+ *
+ *   Licensed under the Apache License, Version 2.0 (the "License");
+ *   you may not use this file except in compliance with the License.
+ *   You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ */
+
+/*  Standard C Included Files */
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+
+/*  SDK Included Files */
+#include "board.h"
+#include "fsl_debug_console.h"
+#include "fsl_i2c.h"
+
+#include "main.h"
+
+/*
+ * Stop dumping the data when we msbit is set
+ */
+void dump_data(uint8_t * pdata, size_t size, bool i2c_data ) {
+
+	for (uint32_t i = 0U; i < size; i++) {
+		if (!i2c_data && (i % 8 == 0)) {
+			printf("\r\n");
+		}
+
+		// printf("0x%2x  ", *pdata++);
+		PUTCHAR( *pdata++ & 0x7f );
+
+		if (i2c_data && (*pdata & 0x80) ) {
+			// printf("0x%2x  ", *pdata);
+			PUTCHAR( *pdata & 0x7f );
+			break;
+		}
+
+	}
+
+	PRINTF("\r\n");
+}
+
+uint8_t get_cmd(uint8_t * pdata, size_t size) {
+	int i=0;
+	char buf;
+
+	if (size <= 1)
+		return 0;
+
+	buf = (char) GETCHAR();
+	PUTCHAR(buf);
+
+	if (buf == 13 ) {
+		PUTCHAR(10);	// LF
+		return 0;
+	}
+
+	*pdata = buf;		// capture the cmd
+
+	for(i=1; i<size; i++ ) {
+		buf = GETCHAR();
+		PUTCHAR(buf);
+
+		// Convert 0 through 9 ASCII to int
+		if (buf >= 48 && buf <= 57) {
+			buf = buf-48;
+		}
+
+		// Convert A through F ASCII to int base 16 (hex)
+		else if (buf >= 65 && buf <= 70) {
+			buf = buf-55;
+		}
+
+		// Convert a through f ASCII to int base 16 (hex)
+		else if (buf >= 97 && buf <= 102) {
+			buf = buf-87;
+		}
+
+		else if (buf == 13) {
+			pdata[i] = buf;
+			break;
+		}
+		else {
+			PUTCHAR(10);	// LF
+			return 0;		// error
+		}
+
+		pdata[i] = buf;
+
+	}
+
+	PUTCHAR(10);
+	return i+1;
+
+}
+
+/* convert ASCII hex characters to integers */
+uint8_t ascii_to_bin(uint8_t data) {
+	uint8_t temp = 0;
+	if (data > 0x39) {
+		temp = data - 0x57;		// a should map to 10 and so on
+	}
+	else {
+		temp = data - 0x30;
+	}
+	return temp;
+}
+
+status_t i2c_write(i2c_master_transfer_t* pxfer, uint8_t devaddr, uint8_t * pdata,
+		size_t size) {
+	status_t result = kStatus_Success;
+	pxfer->slaveAddress = devaddr;
+	pxfer->direction = kI2C_Write;
+	pxfer->data = pdata;
+	pxfer->dataSize = size;
+	return( I2C_MasterTransferBlocking(I2C_MASTER_BASEADDR, pxfer));
+	if (result!=kStatus_Success)	{
+		// printf("\r\n I2C Transfer Error \r\n");
+		return -1;
+	}
+	else {
+		return 0;
+	}
+}
+
+status_t i2c_read(i2c_master_transfer_t* pxfer, uint8_t devaddr, uint8_t * pdata,
+		size_t size) {
+	status_t result = kStatus_Success;
+	pxfer->slaveAddress = devaddr;
+	pxfer->direction = kI2C_Read;
+	pxfer->data = pdata;
+	pxfer->dataSize = size;
+	result = I2C_MasterTransferBlocking(I2C_MASTER_BASEADDR, pxfer);
+	if (result!=kStatus_Success)	{
+		// printf("\r\n I2C Transfer Error \r\n");
+		return -1;
+	}
+	else {
+		return 0;
+	}
+
+}
diff --git a/source/utils.h b/source/utils.h
new file mode 100644
index 0000000..626042e
--- /dev/null
+++ b/source/utils.h
@@ -0,0 +1,27 @@
+/*
+ *   utils.h
+ *
+ *   Copyright (C) 2018, 2019 Mind Chasers Inc.
+ *
+ *   Licensed under the Apache License, Version 2.0 (the "License");
+ *   you may not use this file except in compliance with the License.
+ *   You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ */
+
+#ifndef UTILS_H_
+#define UTILS_H_
+
+void dump_data(uint8_t *, size_t, bool );
+uint8_t get_cmd(uint8_t *, size_t);
+uint8_t ascii_to_bin(uint8_t);
+
+#endif /* UTILS_H_ */