/* MSPDebug - debugging tool for MSP430 MCUs
 * Copyright (C) 2021 sys64738@disroot.org
 *
 * 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 2 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, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include <stddef.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>

#include "mehfet_xport.h"
#include "output.h"
#include "util.h"

#include "mehfet_proto.h"

// TODO: replace '64' by transport buf size

int mehfet_cmd_info(transport_t t, struct mehfet_info* info) {
	if (!info) return -1;

	uint8_t buf[64];
	int r;

	r = mehfet_send_raw(t, mehfet_info, 0, NULL);
	if (r < 0) return r;

	uint8_t stat = 0;
	int len = sizeof buf;
	r = mehfet_recv_raw(t, &stat, &len, buf);
	if (r < 0) return r;
	r = mehfet_err_on_stat("Info", stat, len, buf);
	if (r < 0) return r;

	if (len < 8) {
		printc_err("mehfet: Info response too short: %d < 8\n", len);
		return -1;
	}

	info->caps = buf[0] | ((uint32_t)buf[1] << 8)
		| ((uint32_t)buf[2] << 16) | ((uint32_t)buf[3] << 24);
	info->proto_version = buf[4] | ((uint16_t)buf[5] << 8);
	info->packet_buf_size = 1u << buf[6];
	// buf[7]: reserved

	if (len > 9) { // len includes the null terminator
		char* str = calloc(len - 8, 1);
		memcpy(str, &buf[8], len - 8);
		str[len - 9] = 0;

		info->devicename = str;
	}

#ifdef DEBUG_MEHFET_PROTO_DRIVER
	printc_dbg("mehfet: Info(): '%s' caps=0x%x, protover=0x%04x, pktsize=0x%x\n",
			info->devicename ? info->devicename : "<none>",
			info->caps, info->proto_version, info->packet_buf_size);
#endif

	return 0;
}
int mehfet_cmd_status(transport_t t, enum mehfet_conn* ret) {
	if (!ret) return -1;

	uint8_t buf[64];
	int r;

	r = mehfet_send_raw(t, mehfet_status, 0, NULL);
	if (r < 0) return r;

	uint8_t stat = 0;
	int len = sizeof buf;
	r = mehfet_recv_raw(t, &stat, &len, buf);
	if (r < 0) return r;
	r = mehfet_err_on_stat("Status", stat, len, buf);
	if (r < 0) return r;

	if (len != 1) {
		printc_err("mehfet: Status response wrong length: %d != 1\n", len);
		return -1;
	}

	*ret = buf[0];

#ifdef DEBUG_MEHFET_PROTO_DRIVER
	printc_dbg("mehfet: Status(): %hhu\n", *ret);
#endif

	return 0;
}
int mehfet_cmd_connect(transport_t t, enum mehfet_conn conn) {
	uint8_t buf[64];
	int r;

	buf[0] = conn;
	r = mehfet_send_raw(t, mehfet_connect, 1, buf);
	if (r < 0) return r;

	uint8_t stat = 0;
	int len = sizeof buf;
	r = mehfet_recv_raw(t, &stat, &len, buf);
	if (r < 0) return r;
	r = mehfet_err_on_stat("Connect", stat, len, buf);
	if (r < 0) return r;

	if (len != 0) {
		printc_err("mehfet: Connect response wrong length: %d != 0\n", len);
		return -1;
	}

#ifdef DEBUG_MEHFET_PROTO_DRIVER
	printc_dbg("mehfet: Connect(0x%x)\n", conn);
#endif

	return 0;
}
int mehfet_cmd_disconnect(transport_t t) {
	uint8_t buf[64];
	int r;

	r = mehfet_send_raw(t, mehfet_disconnect, 0, NULL);
	if (r < 0) return r;

	uint8_t stat = 0;
	int len = sizeof buf;
	r = mehfet_recv_raw(t, &stat, &len, buf);
	if (r < 0) return r;
	r = mehfet_err_on_stat("Disconnect", stat, len, buf);
	if (r < 0) return r;

	if (len != 0) {
		printc_err("mehfet: Disconnect response wrong length: %d != 0\n", len);
		return -1;
	}

#ifdef DEBUG_MEHFET_PROTO_DRIVER
	printc_dbg("mehfet: Disconnect()\n");
#endif

	return 0;
}
int mehfet_cmd_delay(transport_t t, bool us, bool exact, uint32_t time) {
	if (time >= (1u << 30)) return -1; // too large

	uint8_t buf[64];
	int r;

	for (size_t i = 0; i < 4; ++i) buf[i] = (time >> (i*8)) & 0xff;
	if (us) buf[3] |= 0x40;
	if (exact) buf[3] |= 0x80;

	r = mehfet_send_raw(t, mehfet_delay, 4, buf);
	if (r < 0) return r;

	uint8_t stat = 0;
	int len = sizeof buf;
	r = mehfet_recv_raw(t, &stat, &len, buf);
	if (r < 0) return r;
	r = mehfet_err_on_stat("Delay", stat, len, buf);
	if (r < 0) return r;

	if (len != 0) {
		printc_err("mehfet: Delay response wrong length: %d != 0\n", len);
		return -1;
	}

#ifdef DEBUG_MEHFET_PROTO_DRIVER
	printc_dbg("mehfet: Delay(us=%c exact=%c time=%u)\n",
			us?'t':'f', exact?'t':'f', time);
#endif

	return 0;
}
int mehfet_cmd_set_clkspeed(transport_t t, bool fast) {
	uint8_t buf[64];
	int r;

	buf[0] = fast ? 0xff : 0;
	r = mehfet_send_raw(t, mehfet_set_clkspeed, 1, buf);
	if (r < 0) return r;

	uint8_t stat = 0;
	int len = sizeof buf;
	r = mehfet_recv_raw(t, &stat, &len, buf);
	if (r < 0) return r;
	r = mehfet_err_on_stat("SetClkSpeed", stat, len, buf);
	if (r < 0) return r;

	if (len != 0) {
		printc_err("mehfet: SetClkSpeed response wrong length: %d != 0\n", len);
		return -1;
	}

#ifdef DEBUG_MEHFET_PROTO_DRIVER
	printc_dbg("mehfet: SetClkSpeed(%s)\n", fast?"fast":"slow");
#endif

	return 0;
}
int mehfet_cmd_get_old_lines(transport_t t, enum mehfet_lines* lines) {
	if (!lines) return -1;

	uint8_t buf[64];
	int r;

	r = mehfet_send_raw(t, mehfet_get_old_lines, 0, NULL);
	if (r < 0) return r;

	uint8_t stat = 0;
	int len = sizeof buf;
	r = mehfet_recv_raw(t, &stat, &len, buf);
	if (r < 0) return r;
	r = mehfet_err_on_stat("GetOldLines", stat, len, buf);
	if (r < 0) return r;

	if (len != 1) {
		printc_err("mehfet: GetOldLines response wrong length: %d != 1\n", len);
		return -1;
	}

	*lines = buf[0];

#ifdef DEBUG_MEHFET_PROTO_DRIVER
	printc_dbg("mehfet: GetOldLines(): 0x%x\n", *lines);
#endif

	return 0;
}

int mehfet_cmd_tdio_seq(transport_t t, uint32_t nbits, bool tms, const uint8_t* tdi, uint8_t* tdo) {
	if (!tdi || !tdo || !nbits) return -1;

	int r;
	uint32_t nbytes = (nbits + 7) >> 3;
	uint8_t buf[(nbytes + 5) < 64 ? 64 : (nbytes + 5)]; // need min for recv

	for (size_t i = 0; i < 4; ++i) buf[i] = (nbits >> (i*8)) & 0xff;
	buf[4] = tms ? 0xff : 0;
	memcpy(&buf[5], tdi, nbytes);

	r = mehfet_send_raw(t, mehfet_tdio_seq, nbytes + 5, buf);
	if (r < 0) return r;

	uint8_t stat = 0;
	int len = sizeof buf; // is a max len
	r = mehfet_recv_raw(t, &stat, &len, buf);
	if (r < 0) return r;
	r = mehfet_err_on_stat("TdioSequence", stat, len, buf);
	if (r < 0) return r;

	if (len != nbytes) {
		printc_err("mehfet: TdioSequence response wrong length: %d != %d\n",
				len, nbytes);
		return -1;
	}

	memcpy(tdo, buf, nbytes);

#ifdef DEBUG_MEHFET_PROTO_DRIVER
	printc_dbg("mehfet: TdioSequence(%u, TMS=%c):\n", nbits, tms?'1':'0');
	debug_hexdump("\tTDI", tdi, nbytes);
	debug_hexdump("\tTDO", tdo, nbytes);
#endif

	return 0;
}
int mehfet_cmd_tms_seq(transport_t t, uint32_t nbits, bool tdi, const uint8_t* tms) {
	if (!tms || !nbits) return -1;

	int r;
	uint32_t nbytes = (nbits + 7) >> 3;
	uint8_t buf[(nbytes + 5) < 64 ? 64 : (nbytes + 5)]; // need min for recv

	for (size_t i = 0; i < 4; ++i) buf[i] = (nbits >> (i*8)) & 0xff;
	buf[4] = tdi ? 0xff : 0;
	memcpy(&buf[5], tms, nbytes);

	r = mehfet_send_raw(t, mehfet_tdio_seq, sizeof buf, buf);
	if (r < 0) return r;

	uint8_t stat = 0;
	int len = sizeof buf; // is a max len
	r = mehfet_recv_raw(t, &stat, &len, buf);
	if (r < 0) return r;
	r = mehfet_err_on_stat("TmsSequence", stat, len, buf);
	if (r < 0) return r;

	if (len != 0) {
		printc_err("mehfet: TmsSequence response wrong length: %d != 0\n", len);
		return -1;
	}

#ifdef DEBUG_MEHFET_PROTO_DRIVER
	printc_dbg("mehfet: TmsSequence(%u, TDI=%c):\n", nbits, tdi?'1':'0');
	debug_hexdump("\tTMS", tms, nbytes);
#endif

	return 0;
}
int mehfet_cmd_tclk_edge(transport_t t, bool newtclk) {
	uint8_t buf[64];
	int r;

	buf[0] = newtclk ? 0xff : 0;
	r = mehfet_send_raw(t, mehfet_tclk_edge, 1, buf);
	if (r < 0) return r;

	uint8_t stat = 0;
	int len = sizeof buf;
	r = mehfet_recv_raw(t, &stat, &len, buf);
	if (r < 0) return r;
	r = mehfet_err_on_stat("TclkEdge", stat, len, buf);
	if (r < 0) return r;

	if (len != 0) {
		printc_err("mehfet: TclkEdge response wrong length: %d != 0\n", len);
		return -1;
	}

#ifdef DEBUG_MEHFET_PROTO_DRIVER
	printc_dbg("mehfet: TclkEdge(TCLK=%c)\n", newtclk?'H':'L');
#endif

	return 0;
}
int mehfet_cmd_tclk_burst(transport_t t, uint32_t ncyc) {
	uint8_t buf[64];
	int r;

	for (size_t i = 0; i < 4; ++i) buf[i] = (ncyc >> (i*8)) & 0xff;
	r = mehfet_send_raw(t, mehfet_tclk_burst, 4, buf);
	if (r < 0) return r;

	uint8_t stat = 0;
	int len = sizeof buf;
	r = mehfet_recv_raw(t, &stat, &len, buf);
	if (r < 0) return r;
	r = mehfet_err_on_stat("TclkBurst", stat, len, buf);
	if (r < 0) return r;

	if (len != 0) {
		printc_err("mehfet: TclkBurst response wrong length: %d != 0\n", len);
		return -1;
	}

#ifdef DEBUG_MEHFET_PROTO_DRIVER
	printc_dbg("mehfet: TclkBurst(ncyc=%u)\n", ncyc);
#endif

	return 0;
}

int mehfet_cmd_reset_tap(transport_t t, enum mehfet_resettap_flags flags,
		enum mehfet_resettap_status* tstat) {
	if (!tstat) return -1;

	uint8_t buf[64];
	int r;

	buf[0] = flags;
	r = mehfet_send_raw(t, mehfet_reset_tap, 1, buf);
	if (r < 0) return r;

	uint8_t stat = 0;
	int len = sizeof buf;
	r = mehfet_recv_raw(t, &stat, &len, buf);
	if (r < 0) return r;
	r = mehfet_err_on_stat("ResetTAP", stat, len, buf);
	if (r < 0) return r;

	if (len != 1) {
		printc_err("mehfet: ResetTAP response wrong length: %d != 1\n", len);
		return -1;
	}

	*tstat = buf[0];

#ifdef DEBUG_MEHFET_PROTO_DRIVER
	printc_dbg("mehfet: ResetTAP(flags=0x%x) = 0x%x\n", flags, *tstat);
#endif

	return 0;
}

static uint8_t bitswap_nyb(uint8_t in) {
	return ((in >> 3) & 1) | ((in >> 1) & 2) | ((in << 1) & 4) | ((in << 3) & 8);
}
static uint8_t bitswap(uint8_t in) {
	return (bitswap_nyb(in&0xf) << 4) | bitswap_nyb(in>>4);
}

#ifdef DEBUG_MEHFET_PROTO_DRIVER
static const char* get_ir_name(uint8_t ir) {
	switch (ir) {
	case 0x09: return "EMEX_DATA_EXCHANGE";
	case 0x0a: return "EMEX_READ_TRIGGER";
	case 0x0b: return "EMEX_READ_CONTROL";
	case 0x0c: return "EMEX_WRITE_CONTROL";
	case 0x0d: return "EMEX_READ_CONTROL_2";
	case 0x0e: return "EMEX_WRITE_CONTROL_2";

	case 0x11: return "CNTRL_SIG_HIGH_BYTE";
	case 0x12: return "CNTRL_SIG_LOW_BYTE";
	case 0x13: return "CNTRL_SIG_16BIT";
	case 0x14: return "CNTRL_SIG_CAPTURE";
	case 0x15: return "CNTRL_SIG_RELEASE";
	case 0x17: return "COREIP_ID";

	case 0x19: return "FLASH_16BIT_UPDATE";
	case 0x1a: return "FLASH_CAPTURE";
	case 0x1b: return "FLASH_16BIT_IN";
	case 0x1c: return "FLASH_UPDATE";

	case 0x21: return "CNTRL";
	case 0x22: return "PREPARE_BLOW";
	case 0x24: return "EX_BLOX";
	case 0x29: return "CONFIG_FUSES";
	case 0x2a: return "TEST_REG";
	case 0x2f: return "TEST_3V_REG";

	case 0x31: return "DUAL_8BIT";
	case 0x32: return "DUAL_CAPTURE";
	case 0x33: return "SELECT_MAIN";
	case 0x34: return "SELECT_ESP";

	case 0x41: return "DATA_16BIT";
	case 0x42: return "DATA_CAPTURE";
	case 0x43: return "DATA_QUICK";
	case 0x44: return "DATA_PSA";
	case 0x45: return "DATA_16BIT_OPT";
	case 0x46: return "SHIFT_OUT_PSA";
	case 0x47: return "DTA";

	case 0x59: return "ACCEPT_KEY";

	case 0x61: return "JMB_EXCHANGE";
	case 0x62: return "JSTATE_ID";
	case 0x64: return "TDO_EVENT";
	case 0x65: return "TDO_EVENT_CTL";

	case 0x81: return "ADDR_HIGH_BYTE";
	case 0x82: return "ADDR_LOW_BYTE";
	case 0x83: return "ADDR_16BIT";
	case 0x84: return "ADDR_CAPTURE";

	case 0x85: return "DATA_TO_ADDR";
	case 0x86: return "CAPTURE_CPU_REG";
	case 0x87: return "DEVICE_ID";
	case 0x88: return "JMB_WRITE_32BIT_MODE";

	case 0xFF: return "BYPASS";

	default: return NULL;
	}
}
#endif

int mehfet_cmd_irshift(transport_t t, uint8_t newir, uint8_t* oldir) {
	if (!oldir) return -1;

	// NOTE: jtaglib.c uses bitswapped IR values, while MehFET uses values from
	//       the SLAU320 PDF, so we need to perform a bitswap here
	newir = bitswap(newir);

	uint8_t buf[64];
	int r;

	buf[0] = newir;
	r = mehfet_send_raw(t, mehfet_irshift, 1, buf);
	if (r < 0) return r;

	uint8_t stat = 0;
	int len = sizeof buf;
	r = mehfet_recv_raw(t, &stat, &len, buf);
	if (r < 0) return r;
	r = mehfet_err_on_stat("IRshift", stat, len, buf);
	if (r < 0) return r;

	if (len != 1) {
		printc_err("mehfet: IRshift response wrong length: %d != 1\n", len);
		return -1;
	}

	*oldir = buf[0];

#ifdef DEBUG_MEHFET_PROTO_DRIVER
	const char* s = get_ir_name(newir);
	if (s) {
		printc_dbg("mehfet: IRshift(new=IR_%s) = 0x%02x\n", s, *oldir);
	} else {
		printc_dbg("mehfet: IRshift(new=0x%02x) = 0x%02x\n", newir, *oldir);
	}
#endif

	return 0;
}
int mehfet_cmd_drshift(transport_t t, uint32_t nbits, const uint8_t* newdr, uint8_t* olddr) {
	if (!newdr || !olddr) return -1;

	uint32_t nbytes = (nbits + 7) >> 3;
	uint8_t buf[(nbytes + 4) < 64 ? 64 : (nbytes + 4)];
	int r;

	for (size_t i = 0; i < 4; ++i) buf[i] = (nbits >> (i * 8)) & 0xff;
	memcpy(&buf[4], newdr, nbytes);
	r = mehfet_send_raw(t, mehfet_drshift, nbytes + 4, buf);
	if (r < 0) return r;

	uint8_t stat = 0;
	int len = sizeof buf;
	r = mehfet_recv_raw(t, &stat, &len, buf);
	if (r < 0) return r;
	r = mehfet_err_on_stat("DRshift", stat, len, buf);
	if (r < 0) return r;

	if (len != (int)nbytes) {
		printc_err("mehfet: DRshift response wrong length: %d != %u\n", len, nbytes);
		return -1;
	}

	memcpy(olddr, buf, nbytes);

#ifdef DEBUG_MEHFET_PROTO_DRIVER
	if (nbits == 16) {
		uint16_t in, out;
		in = newdr[0] | ((uint16_t)newdr[1] << 8);
		out= olddr[0] | ((uint16_t)olddr[1] << 8);
		printc_dbg("mehfet: DRshift(nbits=%u): in: %04x, out: %04x\n", nbits, in, out);
	} else if (nbits == 20) {
		uint32_t in, out;
		in = newdr[0] | ((uint32_t)newdr[1] << 8) | ((uint32_t)newdr[2] << 16);
		out= olddr[0] | ((uint32_t)olddr[1] << 8) | ((uint32_t)olddr[2] << 16);
		printc_dbg("mehfet: DRshift(nbits=%u): in: %05x, out: %05x\n", nbits, in, out);
	} else if (nbits == 32) {
		uint32_t in, out;
		in = newdr[0] | ((uint32_t)newdr[1] << 8) | ((uint32_t)newdr[2] << 16) | ((uint32_t)newdr[3] << 24);
		out= olddr[0] | ((uint32_t)olddr[1] << 8) | ((uint32_t)olddr[2] << 16) | ((uint32_t)olddr[3] << 24);
		printc_dbg("mehfet: DRshift(nbits=%u): in: %08x, out: %08x\n", nbits, in, out);
	} else {
		printc_dbg("mehfet: DRshift(nbits=%u):\n", nbits);
		debug_hexdump("\tin ", newdr, nbytes);
		debug_hexdump("\tout", olddr, nbytes);
	}
#endif

	return 0;
}