/*
 * setup.c -- fmk setup
 *
 * Copyright (C) 2016-2017 Tomasz Kramkowski <tk@the-tk.com>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include <asm.h>
#include <reg.h>
#include <stddef.h>

#include "pit.h"
#include "uart.h"
#include "usb/usb.h"

extern unsigned char _ldata[], _sdata[], _edata[];
extern unsigned char _sbss[], _ebss[];

#define RANGE0_VAL 2 /* Very High Frequency */
#define HGO0_VAL 0 /* High Gain Mode Disabled */
#define FRDIV_VAL 4 /* Divide 16 MHz by 512 to produce 31.25 kHz FLL Reference */
#define PRDIV0_VAL 7 /* Divide 16 MHz by 8 to produce 2 MHz PLL Reference */
#define VDIV0_VAL 12 /* Multiply 2 MHz by 36 to produce 72 MHz resulting clock */

/* Option 2 Clock Configuration */
#define OUTDIV1_VAL 0 /* 72 MHz / 1 = 72 MHz Core / system clocks */
#define OUTDIV2_VAL 1 /* 72 MHz / 2 = 36 MHz Bus clock */
#define OUTDIV4_VAL 2 /* 72 MHz / 3 = 24 MHz Flash clock */

void setup(void);
void setup(void)
{
	/* Disable interrupts for setup */
	CLI();

	/* Disable Watchdog Timer */
	WDOG_UNLOCK = WDOG_UNLOCK_S1;
	WDOG_UNLOCK = WDOG_UNLOCK_S2;
	UNSET_BIT(WDOG_STCTRLH, STCTRLH_WDOGEN);

	/* Copy data and clear bss */
	for (size_t i = 0; i < (size_t)(_edata - _sdata); i++)
		_sdata[i] = _ldata[i];
	for (size_t i = 0; i < (size_t)(_ebss - _sbss); i++)
		_sbss[i] = 0;

	/* Enable port clocks */
	SIM_SCGC5 |= BV(SCGC5_PORTE) | BV(SCGC5_PORTD) | BV(SCGC5_PORTC)
		| BV(SCGC5_PORTB) | BV(SCGC5_PORTA);

	/* Change Clock Dividers */
	SIM_CLKDIV1 = OUTDIV1_VAL << CLKDIV1_OUTDIV1
		   | OUTDIV2_VAL << CLKDIV1_OUTDIV2
		   | OUTDIV4_VAL << CLKDIV1_OUTDIV4;

	/* Clear latched peripherals and I/O */
	if (GET_BIT(PMC_REGSC, REGSC_ACKISO))
		SET_BIT(PMC_REGSC, REGSC_ACKISO);

	/*
	 * CLOCKING SETUP
	 */

	/* FEI to FBE */
	SET_MASKED(MCG_C2, C2_RANGE0_M | BV(C2_HGO0) | BV(C2_EREFS0),
		   RANGE0_VAL << C2_RANGE0 | BV(C2_EREFS0));

	SET_MASKED(MCG_C1, C1_CLKS_M | C1_FRDIV_M | BV(C1_IREFS),
		   2 << C1_CLKS | FRDIV_VAL << C1_FRDIV);

	WAIT_BIT_SET(MCG_S, S_OSCINIT0);
	WAIT_BIT_UNSET(MCG_S, S_IREFST);
	do { } while (GET_BITS(MCG_S, S_CLKST) != 2);

	/* Enable Clock Monitor */
	SET_BIT(MCG_C6, C6_CME0);


	/* FBE to PBE */
	SET_MASKED(MCG_C5, C5_PRDIV0_M, PRDIV0_VAL << C5_PRDIV0);
	SET_MASKED(MCG_C6, BV(C6_PLLS) | C6_VDIV0_M, BV(C6_PLLS) | VDIV0_VAL << C6_VDIV0);

	WAIT_BIT_SET(MCG_S, S_PLLST);
	WAIT_BIT_SET(MCG_S, S_LOCK0);

	/* PBE to PEE */
	SET_MASKED(MCG_C1, C1_CLKS_M, 0);

	do { } while (GET_BITS(MCG_S, S_CLKST) != 3);

	/* Set PLL as the clock to use for things which can take PLL or FLL */
	SET_BIT(SIM_SOPT2, SOPT2_PLLFLLSEL);
	SET_MASKED(SIM_SOPT2, SOPT2_CLKOUTSEL_M, 4 << SOPT2_CLKOUTSEL);

	/* configure LED pin */
	PORTC_PCR(5) = 1 << PCR_MUX | BV(PCR_DSE);
	GPIOC_PDDR = BV(5);

	uart_setup();
	usb_setup();
	pit_setup();

	/* Enable interrupts for setup */
	STI();
}