/*	PIC32MZ ICSP Serial Interface
**
**	Copyright (C) 2018-2021 Herbert Poetzl
**
**	This program is free software; you can redistribute it and/or modify
**    	it under the terms of the GNU General Public License 2 as published 
**	by the Free Software Foundation.
**
*/

#include <xc.h>
#include <stdint.h>
#include <stdbool.h>

	// DEVCFG0
#pragma config BOOTISA = MIPS32
#pragma config ICESEL = ICS_PGx1
#pragma config FECCCON = OFF_UNLOCKED
#pragma config FSLEEP = 0

	// DEVCFG1
#pragma	config FDMTEN = OFF
#pragma	config FWDTEN = OFF
#pragma config POSCMOD = OFF
#pragma config OSCIOFNC = ON
#pragma config FSOSCEN = OFF
#pragma config FNOSC = SPLL
#pragma config FCKSM = CSECMD

	// DEVCFG2
#pragma config FPLLICLK = PLL_FRC
#pragma config FPLLIDIV = DIV_2
#pragma config FPLLRNG = RANGE_5_10_MHZ
#pragma config FPLLMULT = MUL_100
#pragma config FPLLODIV = DIV_4
//#pragma config UPLLEN = OFF
#pragma config UPLLFSEL = FREQ_24MHZ

	// DEVCFG3
#pragma config USERID = 0xC0DE
#pragma config FMIIEN = OFF
#pragma config PGL1WAY = OFF
#pragma config PMDL1WAY = OFF
#pragma config IOL1WAY = OFF
#pragma config FUSBIDIO = OFF

	// DEVCP0
#pragma config CP = OFF


static bool sel = 0;
static bool dbg = 0;

static uint32_t hexval = 0;
static uint32_t hexdat = 0;


#define VERS_0_14

	/* RPC14/SOSCO */
#define ICSP_E_MCLR_O	LATCbits.LATC14
#define ICSP_E_MCLR_I	PORTCbits.RC14
#define ICSP_E_MCLR_T	TRISCbits.TRISC14

#ifdef	VERS_0_14

	/* RPF5/SCL5 */
#define	ICSP_E_PCLK_O	LATFbits.LATF5
#define	ICSP_E_PCLK_I	PORTFbits.RF5
#define	ICSP_E_PCLK_T	TRISFbits.TRISF5
#define	ICSP_E_PCLK_U	CNPUFbits.CNPUF5

	/* RPF4/SDA5 */
#define	ICSP_E_PDAT_O	LATFbits.LATF4
#define ICSP_E_PDAT_I	PORTFbits.RF4
#define ICSP_E_PDAT_T	TRISFbits.TRISF4
#define ICSP_E_PDAT_U	CNPUFbits.CNPUF4

#define	ICSP_E	5

#else
	/* RA2/SCL2 */
#define	ICSP_E_PCLK_O	LATAbits.LATA2
#define	ICSP_E_PCLK_I	PORTAbits.RA2
#define	ICSP_E_PCLK_T	TRISAbits.TRISA2
#define	ICSP_E_PCLK_U	CNPUAbits.CNPUA2

	/* RA3/SDA2 */
#define	ICSP_E_PDAT_O	LATAbits.LATA3
#define ICSP_E_PDAT_I	PORTAbits.RA3
#define ICSP_E_PDAT_T	TRISAbits.TRISA3
#define ICSP_E_PDAT_U	CNPUAbits.CNPUA3

#define	I2C_E	2
#endif

	/* RPC14/SOSCI */
#define ICSP_W_MCLR_O	LATCbits.LATC13
#define ICSP_W_MCLR_I	PORTCbits.RC13
#define ICSP_W_MCLR_T	TRISCbits.TRISC13

	/* RPF8/SCL3 */
#define	ICSP_W_PCLK_O	LATFbits.LATF8
#define	ICSP_W_PCLK_I	PORTFbits.RF8
#define	ICSP_W_PCLK_T	TRISFbits.TRISF8
#define ICSP_W_PCLK_U	CNPUFbits.CNPUF8

	/* RPF2/SDA3 */
#define	ICSP_W_PDAT_O	LATFbits.LATF2
#define ICSP_W_PDAT_I	PORTFbits.RF2
#define ICSP_W_PDAT_T	TRISFbits.TRISF2
#define ICSP_W_PDAT_U	CNPUFbits.CNPUF2

#define	I2C_W	3

	/* AUX: RPB5 */
#define ICSP_D_MCLR_O	LATBbits.LATB5
#define ICSP_D_MCLR_I	PORTBbits.RB5
#define ICSP_D_MCLR_T	TRISBbits.TRISB5

	/* SCL: RPG8/SCL4 */
#define	ICSP_D_PCLK_O	LATGbits.LATG8
#define ICSP_D_PCLK_I	PORTGbits.RG8
#define ICSP_D_PCLK_T	TRISGbits.TRISG8
#define ICSP_D_PCLK_U	CNPUGbits.CNPUG8

	/* SDA: RPG7/SDA4 */
#define	ICSP_D_PDAT_O	LATGbits.LATG7
#define	ICSP_D_PDAT_I	PORTGbits.RG7
#define	ICSP_D_PDAT_T	TRISGbits.TRISG7
#define ICSP_D_PDAT_U	CNPUGbits.CNPUG7

#define	I2C_D	4


static inline
void	unlock(void)
{
	SYSKEY = 0xAA996655;
	SYSKEY = 0x556699AA;
}

static inline
void	lock(void)
{
	SYSKEY = 0x33333333;
}

static inline
void	irq_disable(void)
{
	asm volatile("di");
	asm volatile("ehb");
}

static inline
void	irq_enable(void)
{
	asm volatile("ei");
}


void	init_pbus(void)
{
	unlock();
	PB2DIVbits.PBDIV = 0b000001;	// divide by 2
	PB2DIVbits.ON = 1;

	PB7DIVbits.PBDIV = 0b000000;	// divide by 1
	PB7DIVbits.ON = 1;
	lock();
}


#define	INIT_ICSP(L, l, n)				\
void	init_icsp_ ## l(void)				\
{							\
	ICSP_ ## L ## _MCLR_T = 0;	/* MCLR out */	\
	ICSP_ ## L ## _PCLK_T = 0;	/* PCLK out */	\
	ICSP_ ## L ## _PDAT_T = 0;	/* PDAT out */	\
							\
	I2C ## n ## CONbits.ON = 0;	/* I2C off */	\
}

INIT_ICSP(E, e, 5)
INIT_ICSP(W, w, 3)
INIT_ICSP(D, d, 4)


#define	INIT_I2C(L, l, n)				\
void	init_i2c_ ## l(void)				\
{							\
	ICSP_ ## L ## _PCLK_U = 1;			\
	ICSP_ ## L ## _PDAT_U = 1;			\
							\
	I2C ## n ## ADD = 0xFF;				\
	I2C ## n ## MSK = 0xFF;				\
							\
	I2C ## n ## BRG = 128;				\
							\
	ICSP_ ## L ## _PDAT_O = 0;	/* SDA low */	\
	ICSP_ ## L ## _PDAT_T = 0;	/* SDA out */	\
	ICSP_ ## L ## _PCLK_O = 1;	/* SCL high */	\
	ICSP_ ## L ## _PCLK_T = 1;	/* SCL in */	\
							\
	I2C ## n ##CONbits.ON = 1;			\
}

#define	INIT_BB_I2C(L, l, n)				\
void	init_i2c_ ## l(void)				\
{							\
	ICSP_ ## L ## _PCLK_U = 1;			\
	ICSP_ ## L ## _PDAT_U = 1;			\
							\
	ICSP_ ## L ## _PDAT_O = 1;	/* SDA high */	\
	ICSP_ ## L ## _PDAT_T = 0;	/* SDA out */	\
	ICSP_ ## L ## _PCLK_O = 1;	/* SCL high */	\
	ICSP_ ## L ## _PCLK_T = 0;	/* SCL out */	\
}

INIT_I2C(E, e, 5)
INIT_BB_I2C(W, w, 3)
INIT_BB_I2C(D, d, 4)


#define	INIT_UART(l)					\
void	init_uart_ ## l(void)				\
{							\
}

INIT_UART(e)
INIT_UART(w)
INIT_UART(d)




void	init_uart2(void)
{
	irq_disable();

	U2MODEbits.ON = 0;

	TRISEbits.TRISE8 = 0;		// U2TX out
	TRISEbits.TRISE9 = 1;		// U2RX in
	ANSELEbits.ANSE8 = 0;		// digital
	ANSELEbits.ANSE9 = 0;		// digital

	CFGCONbits.IOLOCK = 0;
	RPE8Rbits.RPE8R = 0b0010;	// U2TX
	U2RXRbits.U2RXR = 0b1101;	// RPE9
	CFGCONbits.IOLOCK = 1;

	INTCONbits.MVEC = 1;		// Multi Vector Interrupts
	PRISSbits.SS0 = 0;		// Normal Register Set
	PRISSbits.PRI7SS = 7;		// Assign Shadow Register Set

	IPC36bits.U2TXIP = 7;		// Interrupt priority of 7
	IPC36bits.U2TXIS = 0;		// Interrupt sub-priority of 0
	IPC36bits.U2RXIP = 7;		// Interrupt priority of 7
	IPC36bits.U2RXIS = 0;		// Interrupt sub-priority of 0

	IEC4SET = _IEC4_U2RXIE_MASK;	// Rx INT Enable
	IFS4bits.U2TXIF = 0;		// Clear Tx flag
	IFS4bits.U2RXIF = 0;		// Clear Rx flag

	U2BRG = 24;			// 1MBaud @ 50MHz
	// U2BRG = 216;			// 115200 @ 50MHz
	U2STA = 0;
	
	U2MODEbits.BRGH = 1;
	U2MODEbits.PDSEL = 0b00;
	U2MODEbits.STSEL = 0;
	U2MODEbits.UEN = 0b00;
	U2MODEbits.ON = 1;
	U2STASET = 0x9400;		// Enable Transmit and Receive

	irq_enable();
}

static inline
void	i2c_bb_delay(unsigned cnt)
{
	unsigned i;
	while (cnt--)
	    for (i=0; i<200; i++)
		asm("nop");
}

#define	I2C_START(n)				\
uint8_t	i2c ## n ## _start(void)		\
{						\
	I2C ## n ## CONbits.SEN = 1;		\
	while (I2C ## n ## CONbits.SEN);	\
	return I2C ## n ## STAT & 0xFF;		\
}						\

#define	I2C_BB_START(n, p)			\
uint8_t	i2c ## n ## _start(void)		\
{						\
	p ## _PDAT_O = 0;	/* SDA low */	\
	i2c_bb_delay(1);			\
	p ## _PCLK_O = 0;	/* SCL low */	\
	i2c_bb_delay(1);			\
	return 0xFF;				\
}

static inline I2C_BB_START(3, ICSP_W)
// static inline I2C_START(4)
static inline I2C_BB_START(4, ICSP_D)
static inline I2C_START(5)

#define	i2c_e_start	i2c5_start
#define	i2c_w_start	i2c3_start
#define	i2c_d_start	i2c4_start


#define	I2C_RESTART(n)				\
uint8_t	i2c ## n ## _restart(void)		\
{						\
	I2C ## n ## CONbits.RSEN = 1;		\
	while (I2C ## n ## CONbits.RSEN);	\
	return I2C ## n ## STAT & 0xFF;		\
}

#define	I2C_BB_RESTART(n, p)			\
uint8_t	i2c ## n ## _restart(void)		\
{						\
	p ## _PDAT_O = 1;	/* SDA high */	\
	i2c_bb_delay(1);			\
	p ## _PCLK_O = 1;	/* SCL high */	\
	i2c_bb_delay(1);			\
	p ## _PDAT_O = 0;	/* SDA low */	\
	i2c_bb_delay(1);			\
	p ## _PCLK_O = 0;	/* SCL low */	\
	i2c_bb_delay(1);			\
	return 0xFF;				\
}

static inline I2C_BB_RESTART(3, ICSP_W)
// static inline I2C_RESTART(4)
static inline I2C_BB_RESTART(4, ICSP_D)
static inline I2C_RESTART(5)

#define	i2c_e_restart	i2c5_restart
#define	i2c_w_restart	i2c3_restart
#define	i2c_d_restart	i2c4_restart


#define	I2C_STOP(n)				\
uint8_t	i2c ## n ## _stop(void)			\
{						\
	if ((I2C ## n ## CON & 0x1F) == 0)	\
	    I2C ## n ## CONbits.PEN = 1;	\
	return I2C ## 2 ## CON & 0x1F;		\
}

#define	I2C_BB_STOP(n, p)			\
uint8_t	i2c ## n ## _stop(void)			\
{						\
	p ## _PCLK_O = 1;	/* SCL high */	\
	i2c_bb_delay(1);			\
	p ## _PDAT_O = 1;	/* SDA high */	\
	i2c_bb_delay(1);			\
	return 0xFF;				\
}

static inline I2C_BB_STOP(3, ICSP_W)
// static inline I2C_STOP(4)
static inline I2C_BB_STOP(4, ICSP_D)
static inline I2C_STOP(5)

#define	i2c_e_stop	i2c5_stop
#define	i2c_w_stop	i2c3_stop
#define	i2c_d_stop	i2c4_stop



#define	I2C_WRITE(n)				\
bool	i2c ## n ## _write(uint8_t byte)	\
{						\
	I2C ## n ## TRN = byte;			\
	while (I2C ## n ## STATbits.TRSTAT);	\
	return I2C ## n ## STATbits.ACKSTAT;	\
}

#define	I2C_BB_WRITE(n, p)			\
bool	i2c ## n ## _write(uint8_t byte)	\
{						\
	bool r = 0;				\
						\
	for (int i=0; i<9; i++) {		\
	    if (i == 8)	{	/* ACK */	\
	    	p ## _PDAT_T = 1;		\
	    	p ## _PDAT_O = 1;		\
	    } else {				\
		bool bit = (byte & 0x80);	\
	    	p ## _PDAT_O = bit;		\
	    }					\
	    i2c_bb_delay(1);			\
	    p ## _PCLK_O = 1;	/* SCL high */	\
	    if (i == 8)	{	/* ACK */	\
		r = p ## _PDAT_I;		\
	    } else {				\
		byte <<= 1;			\
	    }					\
	    i2c_bb_delay(1);			\
	    p ## _PCLK_O = 0;	/* SCL low */	\
	}					\
	p ## _PDAT_T = 0;	/* Cleanup */	\
	return r;				\
}


static inline I2C_BB_WRITE(3, ICSP_W)
// static inline I2C_WRITE(4)
static inline I2C_BB_WRITE(4, ICSP_D)
static inline I2C_WRITE(5)

#define	i2c_e_write	i2c5_write
#define	i2c_w_write	i2c3_write
#define	i2c_d_write	i2c4_write


#define	I2C_READ(n)				\
uint8_t	i2c ## n ## _read(bool ackdt)		\
{						\
	while (I2C ## n ## STATbits.RBF)	\
	    (void)I2C ## n ## RCV;		\
	if (I2C ## n ## CON & 0x1F)		\
	    return 0xFF;			\
						\
	I2C ## n ## CONbits.RCEN = 1;		\
	while (!I2C ## n ## STATbits.RBF);	\
						\
	I2C ## n ## CONbits.ACKDT = ackdt;	\
	I2C ## n ## CONbits.ACKEN = 1;		\
	while (I2C ## n ## CONbits.ACKEN);	\
						\
	return I2C ## n ## RCV;			\
}

#define	I2C_BB_READ(n, p)			\
uint8_t	i2c ## n ## _read(bool ackdt)		\
{						\
	uint8_t byte = 0;			\
						\
	p ## _PDAT_T = 1;	/* Prep */	\
	for (int i=0; i<9; i++) {		\
	    if (i == 8)	{	/* ACK */	\
	    	p ## _PDAT_O = ackdt;		\
		p ## _PDAT_T = 0;		\
	    } else {				\
		byte <<= 1;			\
	    }					\
	    i2c_bb_delay(1);			\
	    p ## _PCLK_O = 1;	/* SCL high */	\
	    if (i < 8)	{			\
	    	byte |= (p ## _PDAT_I) ? 1 : 0;	\
	    }					\
	    i2c_bb_delay(1);			\
	    p ## _PCLK_O = 0;	/* SCL low */	\
	}					\
	return byte;				\
}

static inline I2C_BB_READ(3, ICSP_W)
// static inline I2C_READ(4)
static inline I2C_BB_READ(4, ICSP_D)
static inline I2C_READ(5)

#define	i2c_e_read	i2c5_read
#define	i2c_w_read	i2c3_read
#define	i2c_d_read	i2c4_read


#define	ICSP_MCLR(L, l)					\
uint8_t	icsp_ ## l ## _mclr(unsigned val)		\
{							\
	ICSP_ ## L ## _PDAT_T = 0;			\
	ICSP_ ## L ## _MCLR_O = (val & 1) ? 1 : 0;	\
	ICSP_ ## L ## _PCLK_O = (val & 2) ? 1 : 0;	\
	ICSP_ ## L ## _PDAT_O = (val & 4) ? 1 : 0;	\
							\
	return ((ICSP_ ## L ## _MCLR_I) ? 1 : 0) |	\
	       ((ICSP_ ## L ## _PCLK_I) ? 2 : 0) |	\
	       ((ICSP_ ## L ## _PDAT_I) ? 4 : 0);	\
}

static inline ICSP_MCLR(E, e)
static inline ICSP_MCLR(W, w)
static inline ICSP_MCLR(D, d)


#define	ICSP_OUT(L, l)					\
void	icsp_ ## l ## _out(uint32_t val, unsigned len)	\
{							\
	while (len) {					\
	    bool bit = val & 1;				\
							\
	    ICSP_ ## L ## _PCLK_O = 1;			\
	    ICSP_ ## L ## _PDAT_O = bit;		\
	    val >>= 1;					\
	    ICSP_ ## L ## _PCLK_O = 0;			\
	    len--;					\
	}						\
}

static inline ICSP_OUT(E, e)
static inline ICSP_OUT(W, w)
static inline ICSP_OUT(D, d)


#define	ICSP_IN(L, l)					\
uint32_t icsp_ ## l ## _in(unsigned len)		\
{							\
	uint32_t val = 0;				\
	unsigned shift = 32 - len;			\
							\
	ICSP_ ## L ## _PDAT_T = 1;			\
	while (len) {					\
	    ICSP_ ## L ## _PCLK_O = 1;			\
	    val >>= 1;					\
	    len--;					\
							\
	    bool bit = ICSP_ ## L ## _PDAT_I;		\
							\
	    ICSP_ ## L ## _PCLK_O = 0;			\
	    val |= bit ? (1<<31) : 0;			\
	}						\
	ICSP_ ## L ## _PDAT_T = 0;			\
	return val >> shift;				\
}

static inline ICSP_IN(E, e)
static inline ICSP_IN(W, w)
static inline ICSP_IN(D, d)



static inline
void	uart2_ch(char ch)
{
	while (U2STAbits.UTXBF);
	U2TXREG = ch;
}

static inline
void	uart2_hex(uint8_t hex)
{
	hex &= 0xF;
	if (hex > 9)
	    uart2_ch(hex + 'A' - 10);
	else
	    uart2_ch(hex + '0');
}

static inline
void	uart2_byte(uint8_t val)
{
	uart2_hex(val >> 4);
	uart2_hex(val);
}

static inline
void	uart2_word(uint16_t val)
{
	uart2_byte(val >> 8);
	uart2_byte(val);
}

static inline
void	uart2_long(uint32_t val)
{
	uart2_word(val >> 16);
	uart2_word(val);
}

void	uart2_str0(const char *str)
{
	while (*str)
	    uart2_ch(*str++);
}



void __attribute__((vector(_UART2_RX_VECTOR), interrupt(IPL7SRS))) uart2_isr(void)
{
	while (U2STAbits.URXDA) {	// process buffer 
	    char ch = U2RXREG;

	    switch (ch) {
		case '0' ... '9':
		    hexval <<= 4;
		    hexval |= ch - '0';
		    uart2_ch(ch);	// echo back
		    break;
		case 'A' ... 'F':
		    hexval <<= 4;
		    hexval |= ch - 'A' + 10;
		    uart2_ch(ch);	// echo back
		    break;
		case 'a' ... 'f':
		    hexval <<= 4;
		    hexval |= ch - 'a' + 10;
		    uart2_ch(ch);	// echo back
		    break;

		case '#':		// copy val
		    hexdat = hexval;
		    uart2_ch(ch);	// echo back
		    hexval = 0;
		    break;

		case '!':		// select E/W/D
		    sel = (hexval & 1);
		    dbg = (hexval & 2);
		    uart2_ch(ch);	// echo back
		    hexval = 0;
		    break;

		case '=':		// set mclr/pclk/pdat
		    if (dbg)
			hexdat = icsp_d_mclr(hexval);
		    else if (sel)
			hexdat = icsp_e_mclr(hexval);
		    else
			hexdat = icsp_w_mclr(hexval);
		    uart2_ch(ch);	// echo back
		    uart2_hex(hexdat);
		    hexval = 0;
		    break;

		case '>':		// out icsp seq
		    if (dbg)
			icsp_d_out(hexdat, hexval & 0x3F);
		    else if (sel)
			icsp_e_out(hexdat, hexval & 0x3F);
		    else
			icsp_w_out(hexdat, hexval & 0x3F);
		    uart2_ch(ch);	// echo back
		    hexval = 0;
		    break;

		case '<':		// in icsp seq
		    if (dbg)
			hexdat = icsp_d_in(hexval & 0x1F);
		    else if (sel)
			hexdat = icsp_e_in(hexval & 0x1F);
		    else
			hexdat = icsp_w_in(hexval & 0x1F);
		    uart2_ch(ch);	// echo back
		    if (hexval > 16)
			uart2_long(hexdat);
		    else if (hexval > 8)
			uart2_word(hexdat);
		    else 
			uart2_byte(hexdat);
		    hexval = 0;
		    break;

		case '.':		// ignore
		    break;

		case '[':		// switch to I2C
		    if (dbg)
			init_i2c_d();
		    else if (sel)
			init_i2c_e();
		    else
			init_i2c_w();
		    uart2_ch(ch);	// echo back
		    break;

		case ']':		// back to ICSP
		    if (dbg)
			init_icsp_d();
		    else if (sel)
			init_icsp_e();
		    else
			init_icsp_w();
		    uart2_ch(ch);	// echo back
		    break;

		case '|':		// switch to Serial
		    if (dbg)
			init_uart_d();
		    else if (sel)
			init_uart_e();
		    else
			init_uart_w();
		    uart2_ch(ch);	// echo back
		    break;

		case 'S':		// I2C Start
		    if (dbg)
			hexdat = i2c_d_start();
		    else if (sel)
			hexdat = i2c_e_start();
		    else
			hexdat = i2c_w_start();
		    uart2_ch(ch);	// echo back
		    uart2_byte(hexdat);
		    break;

		case 's':		// I2C Restart
		    if (dbg)
			hexdat = i2c_d_restart();
		    else if (sel)
			hexdat = i2c_e_restart();
		    else
			hexdat = i2c_w_restart();
		    uart2_ch(ch);	// echo back
		    uart2_byte(hexdat);
		    break;

		case 'P':		// I2C Stop
		    if (dbg)
			i2c_d_stop();
		    else if (sel)
			i2c_e_stop();
		    else
			i2c_w_stop();
		    uart2_ch(ch);	// echo back
		    break;

		case 'W':
		    if (dbg)
			hexdat = i2c_d_write(hexval);
		    else if (sel)
			hexdat = i2c_e_write(hexval);
		    else
			hexdat = i2c_w_write(hexval);
		    uart2_ch(ch);	// echo back
		    uart2_hex(hexdat);
		    hexval = 0;
		    break;

		case 'R':
		    if (dbg)
			hexdat = i2c_d_read(hexval);
		    else if (sel)
			hexdat = i2c_e_read(hexval);
		    else
			hexdat = i2c_w_read(hexval);
		    uart2_ch(ch);	// echo back
		    uart2_byte(hexdat);
		    hexval = 0;
		    break;

		default:
		    uart2_ch(ch);	// echo back
	    }
	}
       
	IFS4CLR = _IFS4_U2RXIF_MASK;	// clear UART2 Rx IRQ
}



int	main(void)
{
	ANSELGbits.ANSG7 = 0;		// digital
	ANSELGbits.ANSG8 = 0;		// digital
	
	init_pbus();
	init_icsp_e();
	init_icsp_w();
	init_icsp_d();
	init_uart2();

	uart2_str0("ICSP Ser\n\r");	
	// uart2_str0("PRID ");
	// uart2_long(_CP0_GET_PRID());
	uart2_str0("DEVID ");
	uart2_long(DEVID);
	uart2_str0("\n\r");	

	while (1)
	    asm volatile("wait");
}