/* MSPDebug - debugging tool for MSP430 MCUs
 * Copyright (C) 2009-2012 Daniel Beer
 *
 * 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 <stdlib.h>
#include <string.h>

#include "util.h"
#include "sport.h"
#include "output.h"
#include "goodfet.h"
#include "ctrlc.h"

/* GoodFET protocol definitions */
#define APP_JTAG430		0x11
#define APP_DEBUG		0xFF

#define GLOBAL_READ		0x00
#define GLOBAL_WRITE		0x01
#define GLOBAL_PEEK		0x02
#define GLOBAL_POKE		0x03
#define GLOBAL_SETUP		0x10
#define GLOBAL_START		0x20
#define GLOBAL_STOP		0x21
#define GLOBAL_CALL		0x30
#define GLOBAL_EXEC		0x31
#define GLOBAL_LIMIT		0x7B
#define GLOBAL_EXIST		0x7C
#define GLOBAL_NMEM		0x7D
#define GLOBAL_NOK		0x7E
#define GLOBAL_OK		0x7F
#define GLOBAL_DEBUG		0xFF

#define JTAG430_HALTCPU		0xA0
#define JTAG430_RELEASECPU	0xA1
#define JTAG430_SETINSTRFETCH	0xC1
#define JTAG430_SETPC		0xC2
#define JTAG430_SETREG		0xD2
#define JTAG430_GETREG		0xD3
#define JTAG430_WRITEMEM	0xE0
#define JTAG430_WRITEFLASH	0xE1
#define JTAG430_READMEM		0xE2
#define JTAG430_ERASEFLASH	0xE3
#define JTAG430_ERASECHECK	0xE4
#define JTAG430_VERIFYMEM	0xE5
#define JTAG430_BLOWFUSE	0xE6
#define JTAG430_ISFUSEBLOWN	0xE7
#define JTAG430_ERASEINFO	0xE8
#define JTAG430_COREIP_ID	0xF0
#define JTAG430_DEVICE_ID	0xF1

/* GoodFET packet transfer */
#define MAX_LEN			1024
#define MAX_MEM_BLOCK		128

/************************************************************************
 * GoodFET protocol handling
 */

struct packet {
	uint8_t		app;
	uint8_t		verb;
	uint16_t	len;
	uint8_t		data[MAX_LEN];
};

static int reset_sequence(sport_t fd)
{
	static const int states[] = {
		SPORT_MC_RTS,
		SPORT_MC_RTS | SPORT_MC_DTR,
		SPORT_MC_DTR
	};
	int i;

	printc_dbg("Resetting GoodFET...\n");

	for (i = 0; i < 3; i++) {
		if (sport_set_modem(fd, states[i]) < 0) {
			printc_err("goodfet: failed at step %d: %s\n",
				   i, last_error());
			return -1;
		}

		delay_ms(20);
	}

	return 0;
}

static int send_packet(sport_t fd,
		       uint8_t app, uint8_t verb, uint16_t len,
		       const uint8_t *data)
{
	uint8_t raw[MAX_LEN + 4];

	if (len > MAX_LEN) {
		printc_err("goodfet: send_packet: maximum length "
			   "exceeded (%d)\n", len);
		return -1;
	}

#ifdef DEBUG_GOODFET
	printc_dbg("SEND: %02x/%02x\n", app, verb);
	if (len)
		debug_hexdump("Data", data, len);
#endif

	raw[0] = app;
	raw[1] = verb;
	raw[2] = len & 0xff;
	raw[3] = len >> 8;

	memcpy(raw + 4, data, len);

	if (sport_write_all(fd, raw, len + 4) < 0) {
		printc_err("goodfet: send_packet: %s\n", last_error());
		return -1;
	}

	return 0;
}

static int recv_packet(sport_t fd, struct packet *pkt)
{
	uint8_t header[4];

	if (sport_read_all(fd, header, 4) < 0) {
		printc_err("goodfet: recv_packet (header): %s\n",
			   last_error());
		return -1;
	}

	pkt->app = header[0];
	pkt->verb = header[1];
	pkt->len = ((uint16_t)header[2]) | (((uint16_t)header[3]) << 8);

	if (pkt->len > MAX_LEN) {
		printc_err("goodfet: recv_packet: maximum length "
			   "exceeded (%d)\n", pkt->len);
		return -1;
	}

	if (sport_read_all(fd, pkt->data, pkt->len) < 0) {
		printc_err("goodfet: recv_packet (data): %s\n",
			   last_error());
		return -1;
	}

#ifdef DEBUG_GOODFET
	printc_dbg("RECV: %02x/%02x\n", pkt->app, pkt->verb);
	if (pkt->len)
		debug_hexdump("Data", pkt->data, pkt->len);
#endif

	return 0;
}

static int xfer(sport_t fd,
		uint8_t app, uint8_t verb, uint16_t len,
		const uint8_t *data, struct packet *pkt)
{
	if (send_packet(fd, app, verb, len, data) < 0)
		goto fail;

	while (recv_packet(fd, pkt) >= 0) {
		if (pkt->app == APP_DEBUG && pkt->verb == GLOBAL_DEBUG) {
			char text[MAX_LEN + 1];

			memcpy(text, pkt->data, pkt->len);
			text[pkt->len] = 0;
			printc_dbg("[GoodFET debug] %s\n", text);
		}

		if (pkt->app == app && pkt->verb == verb)
			return 0;
	}

fail:
	printc_err("goodfet: command 0x%02x/0x%02x "
		   "failed\n", app, verb);
	return -1;
}

/************************************************************************
 * GoodFET MSP430 JTAG operations
 */

/* Read a word-aligned block from any kind of memory. */
static int read_words(sport_t fd, address_t addr,
		      address_t len, uint8_t *data)
{
	struct packet pkt;
	uint8_t req[6];

	req[0] = addr;
	req[1] = addr >> 8;
	req[2] = addr >> 16;
	req[3] = addr >> 24;
	req[4] = len;
	req[5] = len >> 8;

	if (xfer(fd, APP_JTAG430, GLOBAL_PEEK, sizeof(req), req, &pkt) < 0) {
		printc_err("goodfet: read %d bytes from 0x%x failed\n",
			   len, addr);
		return -1;
	}

	if (pkt.len != len) {
		printc_err("goodfet: short memory read (got %d, "
			   "expected %d)\n", pkt.len, len);
		return -1;
	}

	memcpy(data, pkt.data, pkt.len);
	return 0;
}

/* Write a word to RAM. */
static int write_ram_word(sport_t fd, address_t addr, uint16_t value)
{
	uint8_t req[6];
	struct packet pkt;

	req[0] = addr;
	req[1] = addr >> 8;
	req[2] = 0;
	req[3] = 0;
	req[4] = value;
	req[5] = value >> 8;

	if (xfer(fd, APP_JTAG430, GLOBAL_POKE, sizeof(req), req, &pkt) < 0) {
		printc_err("goodfet: failed to write word at 0x%x\n", addr);
		return -1;
	}

	return 0;
}

/* Write a word-aligned flash block. The starting address must be within
 * the flash memory range.
 */
static int write_flash_block(sport_t fd, address_t addr,
			     address_t len, const uint8_t *data)
{
	uint8_t req[MAX_MEM_BLOCK + 4];
	struct packet pkt;

	req[0] = addr >> 0;
	req[1] = addr >> 8;
	req[2] = addr >> 16;
	req[3] = addr >> 24;
	memcpy(req + 4, data, len);

	if (xfer(fd, APP_JTAG430, JTAG430_WRITEFLASH,
		 len + 4, req, &pkt) < 0) {
		printc_err("goodfet: failed to write "
			   "flash block of size %d at 0x%x\n",
			   len, addr);
		return -1;
	}

	return 0;
}

/* Write a single byte by reading and rewriting a word. */
static int write_byte(sport_t fd, address_t addr, uint8_t value)
{
	address_t aligned = addr & ~1;
	uint8_t data[2];
	int r;

	if (read_words(fd, aligned, 2, data) < 0)
		goto fail;

	data[addr & 1] = value;

	if (addr < 0x8000)
		r = write_ram_word(fd, aligned,
				   ((uint16_t)data[0]) |
				   (((uint16_t)data[1]) << 8));
	else
		r = write_flash_block(fd, aligned, 2, data);

	if (r < 0)
		goto fail;

	return 0;
fail:
	printc_err("good_fet: write_byte at address 0x%x failed\n", addr);
	return -1;
}

static int init_device(sport_t fd)
{
	struct packet pkt;
	uint8_t chip_id[2];

	printc_dbg("Initializing...\n");

	if (xfer(fd, APP_JTAG430, GLOBAL_NOK, 0, NULL, &pkt) < 0) {
		printc_err("goodfet: comms test failed\n");
		return -1;
	}

	printc_dbg("Setting up JTAG pins\n");

	if (xfer(fd, APP_JTAG430, GLOBAL_SETUP, 0, NULL, &pkt) < 0) {
		printc_err("goodfet: SETUP command failed\n");
		return -1;
	}

	printc_dbg("Starting JTAG\n");

	if (xfer(fd, APP_JTAG430, GLOBAL_START, 0, NULL, &pkt) < 0) {
		printc_err("goodfet: START command failed\n");
		return -1;
	}

	if (pkt.len < 1) {
		printc_err("goodfet: bad response to JTAG START\n");
		return -1;
	}

	printc("JTAG ID: 0x%02x\n", pkt.data[0]);
	if (pkt.data[0] != 0x89 && pkt.data[0] != 0x91) {
		printc_err("goodfet: unexpected JTAG ID: 0x%02x\n",
			   pkt.data[0]);
		xfer(fd, APP_JTAG430, GLOBAL_STOP, 0, NULL, &pkt);
		return -1;
	}

	printc_dbg("Halting CPU\n");

	if (xfer(fd, APP_JTAG430, JTAG430_HALTCPU, 0, NULL, &pkt) < 0) {
		printc_err("goodfet: HALTCPU command failed\n");
		xfer(fd, APP_JTAG430, GLOBAL_STOP, 0, NULL, &pkt);
		return -1;
	}

	if (read_words(fd, 0xff0, 2, chip_id) < 0) {
		printc_err("goodfet: failed to read chip ID\n");
		xfer(fd, APP_JTAG430, GLOBAL_STOP, 0, NULL, &pkt);
		return -1;
	}

	printc_dbg("Chip ID: 0x%02x%02x\n", chip_id[0], chip_id[1]);

	return 0;
}

/************************************************************************
 * MSPDebug Device interface
 */

struct goodfet {
	struct device		base;

	sport_t			serial_fd;
};

static int goodfet_readmem(device_t dev_base, address_t addr,
			   uint8_t *mem, address_t len)
{
	struct goodfet *gc = (struct goodfet *)dev_base;

	if (!len)
		return 0;

	/* Handle unaligned start */
	if (addr & 1) {
		uint8_t data[2];

		if (read_words(gc->serial_fd, addr ^ 1, 2, data) < 0)
			goto fail;

		mem[0] = data[1];
		addr++;
		mem++;
		len--;
	}

	/* Read aligned blocks */
	while (len >= 2) {
		int plen = MAX_MEM_BLOCK;

		if (plen > len)
			plen = len;
		plen &= ~1;

		if (read_words(gc->serial_fd, addr, plen, mem) < 0)
			goto fail;

		addr += plen;
		mem += plen;
		len -= plen;
	}

	/* Handle unaligned end */
	if (len) {
		uint8_t data[2];

		if (read_words(gc->serial_fd, addr, 2, data) < 0)
			goto fail;

		mem[0] = data[0];
	}

	return 0;

fail:
	printc_err("goodfet: readmem failed at 0x%x\n", addr);
	return -1;
}

static int goodfet_writemem(device_t dev_base, address_t addr,
			    const uint8_t *mem, address_t len)
{
	struct goodfet *gc = (struct goodfet *)dev_base;

	if (!len)
		return 0;

	/* Handle unaligned start */
	if (addr & 1) {
		if (write_byte(gc->serial_fd, addr, mem[0]) < 0)
			goto fail;

		addr++;
		mem++;
		len--;
	}

	while (len >= 2) {
		if (addr < 0x8000) {
			if (write_ram_word(gc->serial_fd, addr,
					   ((uint16_t)mem[0]) |
					   (((uint16_t)mem[1]) << 8)) < 0)
				goto fail;

			addr += 2;
			mem += 2;
			len -= 2;
		} else {
			int plen = MAX_MEM_BLOCK;

			if (plen > len)
				plen = len;
			plen &= ~1;

			if (write_flash_block(gc->serial_fd, addr,
					      plen, mem) < 0)
				goto fail;

			addr += plen;
			mem += plen;
			len -= plen;
		}
	}

	/* Handle unaligned end */
	if (len && (write_byte(gc->serial_fd, addr, mem[0]) < 0))
		goto fail;

	return 0;

fail:
	printc_err("goodfet: writemem failed at 0x%x\n", addr);
	return -1;
}

static int goodfet_setregs(device_t dev_base, const address_t *regs)
{
	(void)dev_base;
	(void)regs;

	printc_err("goodfet: register write not implemented\n");
	return -1;
}

static int goodfet_getregs(device_t dev_base, address_t *regs)
{
	(void)dev_base;
	(void)regs;

	printc_err("goodfet: register read not implemented\n");
	return -1;
}

static int goodfet_reset(struct goodfet *gc)
{
	static const uint8_t cmd_seq[] = {
		JTAG430_RELEASECPU,
		GLOBAL_STOP,
		GLOBAL_START,
		JTAG430_HALTCPU
	};
	int i;

	/* We don't have a POR request, so just restart JTAG */
	for (i = 0; i < 4; i++) {
		struct packet pkt;

		if (xfer(gc->serial_fd, APP_JTAG430, cmd_seq[i],
			 0, NULL, &pkt) < 0) {
			printc_err("goodfet: reset: command 0x%02x failed\n",
				   cmd_seq[i]);
			return -1;
		}
	}

	return 0;
}

static int goodfet_run(struct goodfet *gc)
{
	struct packet pkt;

	if (xfer(gc->serial_fd, APP_JTAG430, JTAG430_RELEASECPU,
		 0, NULL, &pkt) < 0) {
		printc_err("goodfet: failed to release CPU\n");
		return -1;
	}

	return 0;
}

static int goodfet_halt(struct goodfet *gc)
{
	struct packet pkt;

	if (xfer(gc->serial_fd, APP_JTAG430, JTAG430_HALTCPU,
		 0, NULL, &pkt) < 0) {
		printc_err("goodfet: failed to release CPU\n");
		return -1;
	}

	return 0;
}

static int goodfet_ctl(device_t dev_base, device_ctl_t type)
{
	struct goodfet *gc = (struct goodfet *)dev_base;

	switch (type) {
	case DEVICE_CTL_RESET:
		return goodfet_reset(gc);

	case DEVICE_CTL_RUN:
		return goodfet_run(gc);

	case DEVICE_CTL_HALT:
		return goodfet_halt(gc);

	default:
		printc_err("goodfet: unsupported operation\n");
		return -1;
	}

	return 0;
}

static device_status_t goodfet_poll(device_t dev_base)
{
	(void)dev_base;

	if (delay_ms(100) < 0)
		return DEVICE_STATUS_INTR;

	return DEVICE_STATUS_RUNNING;
}

static int goodfet_erase(device_t dev_base, device_erase_type_t type,
			 address_t addr)
{
	struct goodfet *gc = (struct goodfet *)dev_base;
	struct packet pkt;

	if (type != DEVICE_ERASE_MAIN) {
		printc_err("goodfet: only main memory erase is supported\n");
		return -1;
	}

	if (xfer(gc->serial_fd, APP_JTAG430, JTAG430_ERASEFLASH,
		 0, NULL, &pkt) < 0) {
		printc_err("goodfet: erase failed\n");
		return -1;
	}

	return 0;
}

static device_t goodfet_open(const struct device_args *args)
{
	struct goodfet *gc;

	if (!(args->flags & DEVICE_FLAG_TTY)) {
		printc_err("goodfet: this driver does not support raw "
			   "USB access\n");
		return NULL;
	}

	if (!(args->flags & DEVICE_FLAG_JTAG)) {
		printc_err("goodfet: this driver does not support "
			   "Spy-Bi-Wire\n");
		return NULL;
	}

	gc = malloc(sizeof(*gc));
	if (!gc) {
		printc_err("goodfet: malloc: %s\n", last_error());
		return NULL;
	}

	gc->base.type = &device_goodfet;
	gc->base.max_breakpoints = 0;

	gc->serial_fd = sport_open(args->path, 115200, 0);
	if (SPORT_ISERR(gc->serial_fd)) {
		printc_err("goodfet: sport_open: %s: %s\n",
			   args->path, last_error());
		free(gc);
		return NULL;
	}

	if ((args->flags & DEVICE_FLAG_FORCE_RESET) &&
	    reset_sequence(gc->serial_fd) < 0)
		printc_err("warning: goodfet: reset failed\n");

	if (sport_flush(gc->serial_fd) < 0)
		printc_err("warning: goodfet: sport_flush: %s\n",
			   last_error());

	if (init_device(gc->serial_fd) < 0) {
		printc_err("goodfet: initialization failed\n");
		free(gc);
		return NULL;
	}

	return &gc->base;
}

static void goodfet_destroy(device_t dev_base)
{
	struct goodfet *gc = (struct goodfet *)dev_base;
	struct packet pkt;

	xfer(gc->serial_fd, APP_JTAG430, JTAG430_RELEASECPU, 0, NULL, &pkt);
	xfer(gc->serial_fd, APP_JTAG430, GLOBAL_STOP, 0, NULL, &pkt);
	sport_close(gc->serial_fd);
	free(gc);
}

const struct device_class device_goodfet = {
	.name		= "goodfet",
	.help		= "GoodFET MSP430 JTAG",
	.open		= goodfet_open,
	.destroy	= goodfet_destroy,
	.readmem	= goodfet_readmem,
	.writemem	= goodfet_writemem,
	.getregs	= goodfet_getregs,
	.setregs	= goodfet_setregs,
	.ctl		= goodfet_ctl,
	.poll		= goodfet_poll,
	.erase		= goodfet_erase
};