#include <stddef.h>
#include <stdio.h>

#include "pio_sbw.h"
#include "tap.h"

#include "msp430dbg.h"

bool msp430_check_fuse_blown(void) {
	for (size_t i = 0; i < 3; ++i) {
		sbw_tap_shift_ir(msp430_ir_ctrl_sig_capture);
		uint16_t dr = sbw_tap_shift_dr(0xaaaa);
		//printf("fuse dr=%04x\n", dr);
		if (dr == 0x5555) return true;
	}

	return false;
}

bool msp430_insn_fetch(void) {
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_capture);
	// shouldn't it set the insn fetch bit here?
	for (size_t i = 0; i < 50; ++i) {
		uint16_t dr = sbw_tap_read_dr();
		//printf("[%zu] wait fetch dr=%04x\n", i, dr);
		if (dr & 0x0080) return true;

		sbw_clr_tclk();
		sbw_set_tclk();
	}

	return false;
}

void msp430_cpu_halt(void) {
	if (!msp430_insn_fetch()) ;//printf("aaa no fetch\n");
	sbw_tap_shift_ir(msp430_ir_data_16bit);
	sbw_tap_shift_dr(0x3fff); // "jmp ."
	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x2409); // JTAG halt
	sbw_set_tclk();
}
void msp430_cpu_release(void) {
	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x2401);
	sbw_tap_shift_ir(msp430_ir_addr_capture);
	sbw_set_tclk();
}

bool msp430_cpu_reset(void) {
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x2C01);
	sbw_tap_shift_dr(0x2401);
	sbw_clr_tclk();
	sbw_set_tclk();
	sbw_clr_tclk();
	sbw_set_tclk();
	sbw_clr_tclk();
	uint8_t jtagid = sbw_tap_shift_ir(msp430_ir_addr_capture);
	sbw_set_tclk();

	//printf("reset: watchdog disable\n");
	msp430_memory_write(16, 0x0120, 0x5A80); // disable watchdog

	return jtagid == MSP430_JTAG_TAP_ID;
}

void msp430_pc_set(uint16_t pc) {
	if (!msp430_insn_fetch()) ;//printf("aaa no fetch\n");

	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x3401);
	sbw_tap_shift_ir(msp430_ir_data_16bit);
	sbw_tap_shift_dr(0x4030); // "load pc"
	sbw_clr_tclk();
	sbw_set_tclk();
	sbw_tap_shift_dr(pc);
	sbw_clr_tclk();
	sbw_set_tclk();
	sbw_tap_shift_ir(msp430_ir_addr_capture);
	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x2401);
}

void msp430_device_release(uint16_t addr) {
	if (addr == 0xfffe) { // reset vector
		sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
		sbw_tap_shift_dr(0x2c01);
		sbw_tap_shift_dr(0x2401);
	} else {
		msp430_pc_set(addr);
	}

	sbw_tap_shift_ir(msp430_ir_ctrl_sig_release);
}

uint16_t msp430_memory_read16(uint16_t addr) {
	msp430_cpu_halt();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x2409); // 2419 for 8bit
	sbw_tap_shift_ir(msp430_ir_addr_16bit);
	sbw_tap_shift_dr(addr);
	sbw_tap_shift_ir(msp430_ir_data_to_addr);
	sbw_set_tclk();
	sbw_clr_tclk();
	uint16_t r = sbw_tap_read_dr();
	//printf("read16 result=%04x\n", r);

	msp430_cpu_release();

	return r;
}

uint8_t  msp430_memory_read8 (uint16_t addr) {
	msp430_cpu_halt();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x2419); // 2409 for 16bit
	sbw_tap_shift_ir(msp430_ir_addr_16bit);
	sbw_tap_shift_dr(addr);
	sbw_tap_shift_ir(msp430_ir_data_to_addr);
	sbw_set_tclk();
	sbw_clr_tclk();
	uint16_t r = sbw_tap_read_dr();

	msp430_cpu_release();

	return r;
}

void msp430_memory_write16(uint16_t addr, uint16_t value) {
	msp430_cpu_halt();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x2408); // 2418 for 8bit
	sbw_tap_shift_ir(msp430_ir_addr_16bit);
	sbw_tap_shift_dr(addr);
	sbw_tap_shift_ir(msp430_ir_data_to_addr);
	sbw_tap_shift_dr(value);
	sbw_set_tclk();

	msp430_cpu_release();
}

void msp430_memory_write8 (uint16_t addr, uint8_t  value) {
	msp430_cpu_halt();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x2418);
	sbw_tap_shift_ir(msp430_ir_addr_16bit);
	sbw_tap_shift_dr(addr);
	sbw_tap_shift_ir(msp430_ir_data_to_addr);
	sbw_tap_shift_dr(value);
	sbw_set_tclk();

	msp430_cpu_release();
}

void msp430_memory_read_block(uint16_t srcaddr, uint16_t num, uint16_t* dest) {
	msp430_pc_set(srcaddr - 4);
	msp430_cpu_halt();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x2409);
	sbw_tap_shift_ir(msp430_ir_data_quick);

	for (size_t i = 0; i < num; ++i) {
		sbw_set_tclk();
		dest[i] = sbw_tap_read_dr();
		sbw_clr_tclk();
	}

	msp430_cpu_release();
}
void msp430_memory_write_block(uint16_t dstaddr, uint16_t num, const uint16_t* src) {
	msp430_pc_set(dstaddr - 4);
	msp430_cpu_halt();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x2408);
	sbw_tap_shift_ir(msp430_ir_data_quick);

	for (size_t i = 0; i < num; ++i) {
		sbw_tap_shift_dr(src[i]);
		sbw_set_tclk();
		sbw_clr_tclk();
	}

	msp430_cpu_release();
}
bool msp430_memory_verify(uint16_t startaddr, uint16_t length, const uint16_t* copy) {
	const uint16_t poly = 0x0805;

	msp430_cpu_reset();

	if (false/*device has enhanced verify*/) {
		msp430_pc_set(startaddr - 4);
		msp430_cpu_halt();
		sbw_clr_tclk();
		sbw_tap_shift_ir(msp430_ir_data_16bit);
		sbw_tap_shift_dr(startaddr - 2);
	} else {
		msp430_pc_set(startaddr - 2);
		sbw_set_tclk();
		sbw_clr_tclk();
	}

	sbw_tap_shift_ir(msp430_ir_data_psa);

	for (size_t i = 0; i < length; ++i) {
		sbw_set_tclk();

		const uint8_t tms_seq = 0x19; // 100110
		// go through drscan/capturedr/shiftdr/updatedr without actually shifting data
		sbw_tms_sequence(6, true, &tms_seq);

		sbw_clr_tclk();
	}

	sbw_tap_shift_ir(msp430_ir_shift_out_psa);
	uint16_t psa_out = sbw_tap_read_dr();
	sbw_set_tclk();

	if (false/*device has enhanced verify*/) {
		msp430_cpu_release();
	}
	msp430_cpu_reset();

	uint16_t psa_crc = startaddr - 2;
	for (size_t i = 0; i < length; ++i) {
		if (psa_crc & 0x8000) {
			psa_crc ^= poly;
			psa_crc <<= 1;
			psa_crc |= 1;
		} else psa_crc <<= 1;

		if (copy) psa_crc ^= copy[i];
		else psa_crc ^= 0xffff;
	}

	return psa_out == psa_crc;
}

uint32_t msp430_device_get(float freq) {
	uint8_t jtagid = 0;
	for (size_t i = 0; i < 7; ++i) {
		sbw_preinit();

		bool s = sbw_init();
		if (!s) return 0xff<<24;

		sbw_tap_reset();
		sbw_check_fuse();

		jtagid = sbw_tap_shift_ir(msp430_ir_bypass);
		//printf("[%zu] get jtag id= %02x\n", i, jtagid);

		if (jtagid == MSP430_JTAG_TAP_ID) break;

		sbw_deinit();
	}

	if (jtagid != MSP430_JTAG_TAP_ID) return 0<<24;
	if (msp430_check_fuse_blown()) return 1<<24;

	//sbw_set_freq(false, freq); // TODO: store freq? for programming stuff (needs different baudrate for tclk)
	sbw_set_freq(true, 350e3);

	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	uint16_t dr = sbw_tap_shift_dr(0x2401); // JTAG mode, read
	//printf("olddr=%04x\n", dr);

	if (sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit) != jtagid) {
		return 2<<24;
	}

	uint16_t devid = 0;
	for (size_t i = 0; i < 50; ++i) {
		dr = sbw_tap_read_dr();
		//printf("[%zu] wait jtag sync: dr=%04x\n", i, dr);
		if (dr & 0x0200) {
			devid = msp430_memory_read(16, 0x0FF0);
			break;
		}
	}

	if (devid == 0) return 3<<24;

	if (!msp430_cpu_reset()) return 4<<24;

	return ((uint32_t)jtagid << 24) | devid;
}

// TODO
void msp430_flash_write(uint16_t startaddr, uint16_t num, const uint16_t* src) {
	msp430_cpu_halt();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x2408);
	sbw_tap_shift_ir(msp430_ir_addr_16bit);
	sbw_tap_shift_dr(0x0128); // FCTL1
	sbw_tap_shift_ir(msp430_ir_data_to_addr);
	sbw_tap_shift_dr(0xa540); // enable flash write
	sbw_set_tclk();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_addr_16bit);
	sbw_tap_shift_dr(0x012a); // FCTL2
	sbw_tap_shift_ir(msp430_ir_data_to_addr);
	sbw_tap_shift_dr(0xa540); // MCLK source, DIV=1
	sbw_set_tclk();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_addr_16bit);
	sbw_tap_shift_dr(0x012c); // FCTL3
	sbw_tap_shift_ir(msp430_ir_data_to_addr);
	sbw_tap_shift_dr(0xa540); // unlock
	sbw_set_tclk();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);

	//sbw_set_freq(true, 350e3);

	for (uint16_t i = 0, addr = startaddr; i < num; ++i, addr += 2) {
		sbw_tap_shift_dr(0x2408);

		sbw_tap_shift_ir(msp430_ir_addr_16bit);
		sbw_tap_shift_dr(addr);
		sbw_tap_shift_ir(msp430_ir_data_to_addr);
		sbw_tap_shift_dr(src[i]);

		sbw_set_tclk();
		sbw_clr_tclk();

		sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
		sbw_tap_shift_dr(0x2409);

		sbw_tclk_burst(35*4);
		printf("%04x gets %04x\n", addr, src[i]);
	}

	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x2408);
	sbw_tap_shift_ir(msp430_ir_addr_16bit);
	sbw_tap_shift_dr(0x0128); // FCTL1
	sbw_tap_shift_ir(msp430_ir_data_to_addr);
	sbw_tap_shift_dr(0xa500); // disable flash write
	sbw_set_tclk();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_addr_16bit);
	sbw_tap_shift_dr(0x012c); // FCTL3
	sbw_tap_shift_ir(msp430_ir_data_to_addr);
	sbw_tap_shift_dr(0xa550); // set lock
	sbw_set_tclk();

	msp430_cpu_release();
}
void msp430_flash_erase_seg(uint16_t startaddr, bool is_infosegA) {
	(void)is_infosegA;

	msp430_cpu_halt();

	sbw_clr_tclk();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x2408);
	sbw_tap_shift_ir(msp430_ir_addr_16bit);
	sbw_tap_shift_dr(0x0128); // FCTL1
	sbw_tap_shift_ir(msp430_ir_data_to_addr);
	sbw_tap_shift_dr(0xa502); // enable flash erase, single segment
	sbw_set_tclk();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_addr_16bit);
	sbw_tap_shift_dr(0x012a); // FCTL2
	sbw_tap_shift_ir(msp430_ir_data_to_addr);
	sbw_tap_shift_dr(0xa540); // MCLK source, DIV=1
	sbw_set_tclk();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_addr_16bit);
	sbw_tap_shift_dr(0x012c); // FCTL3
	sbw_tap_shift_ir(msp430_ir_data_to_addr);
	sbw_tap_shift_dr(0xa500); // unlock
	sbw_set_tclk();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_addr_16bit);
	sbw_tap_shift_dr(startaddr);
	sbw_tap_shift_ir(msp430_ir_data_to_addr);
	sbw_tap_shift_dr(0x55aa);
	sbw_set_tclk();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x2409);

	sbw_tclk_burst(4820);

	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x2408);
	sbw_tap_shift_ir(msp430_ir_addr_16bit);
	sbw_tap_shift_dr(0x0128); // FCTL1
	sbw_tap_shift_ir(msp430_ir_data_to_addr);
	sbw_tap_shift_dr(0xa500); // lock flash
	sbw_set_tclk();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_addr_16bit);
	sbw_tap_shift_dr(0x012c); // FCTL3
	sbw_tap_shift_ir(msp430_ir_data_to_addr);
	sbw_tap_shift_dr(0xa510); // set lock
	sbw_set_tclk();

	msp430_cpu_release();
}
void msp430_flash_erase_mass(bool withinfomem) {
	uint16_t addr = withinfomem ? 0x1002 : 0xfffe;
	uint16_t mode = withinfomem ? 0xa506 : 0xa504;

	(void)addr; (void)mode; // nah, not yet
}

// mwhahahah

// TODO
/*uint16_t msp430_read_jmb(void) {
	return 0;
}
void msp430_write_jmb(uint16_t v) {

}*/

uint16_t msp430_cpureg_read(short regid) {
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x3401);

	sbw_tap_shift_ir(msp430_ir_data_16bit);
	sbw_tap_shift_dr((regid & 0xf) << 8 | 0x4082);

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_data_capture);
	sbw_set_tclk();
	sbw_tap_shift_ir(msp430_ir_data_16bit);
	sbw_tap_shift_dr(0x00fe);
	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_data_capture);
	sbw_set_tclk();
	sbw_clr_tclk();
	sbw_set_tclk();
	uint16_t reg = sbw_tap_read_dr();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x2401);
	sbw_set_tclk();

	return reg;
}
void msp430_cpureg_write(short regid, uint16_t value) {
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x3401);

	sbw_tap_shift_ir(msp430_ir_data_16bit);
	sbw_tap_shift_dr((regid & 0xf) | 0x4030);

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_data_capture);
	sbw_set_tclk();
	sbw_tap_shift_ir(msp430_ir_data_16bit);
	sbw_tap_shift_dr(value);
	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_data_capture);
	sbw_set_tclk();
	sbw_tap_shift_ir(msp430_ir_data_16bit);
	sbw_tap_shift_dr(0x3ffd);
	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_data_capture);
	sbw_set_tclk();

	sbw_clr_tclk();
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	sbw_tap_shift_dr(0x2401);
	sbw_set_tclk();
}