/* MSPDebug - debugging tool for MSP430 MCUs
 * Copyright (C) 2009-2012 Daniel Beer
 * Copyright (C) 2012-2015 Peter Bägel
 *
 * 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
 */

/* jtag functions are taken from TIs SLAA149–September 2002
 *
 * breakpoint implementation influenced by a posting of Ruisheng Lin
 * to Travis Goodspeed at 2012-09-20 found at:
 * http://sourceforge.net/p/goodfet/mailman/message/29860790/
 *
 * 2012-10-03 Peter Bägel (DF5EQ)
 * 2012-10-03   initial release              Peter Bägel (DF5EQ)
 * 2014-12-26   jtag_single_step added       Peter Bägel (DF5EQ)
 *              jtag_read_reg    corrected
 *              jtag_write_reg   corrected
 * 2015-02-21   jtag_set_breakpoint added    Peter Bägel (DF5EQ)
 *              jtag_cpu_state      added
 * 2020-06-01   jtag_read_reg    corrected   Gabor Mayer (HG5OAP)
 *              jtag_write_reg   corrected
 */

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

#include "jtaglib.h"
#include "jtaglib_defs.h"

#include "output.h"
#include "ctrlc.h"
#include "device.h"

/* Reset target JTAG interface and perform fuse-HW check */
static void jtag_default_reset_tap(struct jtdev *p)
{
	int loop_counter;

	/* TODO: replace with tms_sequence()? */
	jtag_tms_set(p);
	jtag_tck_set(p);

	/* Perform fuse check */
	jtag_tms_clr(p);
	jtag_tms_set(p);
	jtag_tms_clr(p);
	jtag_tms_set(p);

	/* Reset JTAG state machine */
	for (loop_counter = 6; loop_counter > 0; loop_counter--) {
		jtag_tck_clr(p);
		jtag_tck_set(p);

		if (p->failed)
			return;
	}

	/* Set JTAG state machine to Run-Test/IDLE */
	jtag_tck_clr(p);
	jtag_tms_clr(p);
	jtag_tck_set(p);
}

/* This function sets the target JTAG state machine
 * back into the Run-Test/Idle state after a shift access
 */
static void jtag_default_tclk_prep (struct jtdev *p)
{
	/* JTAG state = Exit-DR */
	jtag_tck_clr(p);
	jtag_tck_set(p);

	/* JTAG state = Update-DR */
	jtag_tms_clr(p);
	jtag_tck_clr(p);
	jtag_tck_set(p);

	/* JTAG state = Run-Test/Idle */
}

/* Shift a value into TDI (MSB first) and simultaneously
 * shift out a value from TDO (MSB first)
 * num_bits: number of bits to shift
 * data_out: data to be shifted out
 * return  : scanned TDO value
 */
static unsigned int jtag_default_shift( struct jtdev *p,
				unsigned char num_bits,
				unsigned int  data_out )
{
	unsigned int data_in;
	unsigned int mask;
	unsigned int tclk_save;

	tclk_save = p->f->jtdev_tclk_get(p);

	data_in = 0;
	for (mask = 0x0001U << (num_bits - 1); mask != 0; mask >>= 1) {
		if ((data_out & mask) != 0)
			jtag_tdi_set(p);
		else
			jtag_tdi_clr(p);

		if (mask == 1)
			jtag_tms_set(p);

		jtag_tck_clr(p);
		jtag_tck_set(p);

		if (p->f->jtdev_tdo_get(p) == 1)
			data_in |= mask;
	}

	p->f->jtdev_tclk(p, tclk_save);

	/* Set JTAG state back to Run-Test/Idle */
	jtag_default_tclk_prep(p);

	return data_in;
}

/* Shifts a new instruction into the JTAG instruction register through TDI
 * MSB first, with interchanged MSB/LSB, to use the shifting function
 * instruction: 8 bit instruction
 * return     : scanned TDO value
 */
uint8_t jtag_default_ir_shift(struct jtdev *p, uint8_t instruction)
{
	/* JTAG state = Run-Test/Idle */
	jtag_tms_set(p);
	jtag_tck_clr(p);
	jtag_tck_set(p);

	/* JTAG state = Select DR-Scan */
	jtag_tck_clr(p);
	jtag_tck_set(p);

	/* JTAG state = Select IR-Scan */
	jtag_tms_clr(p);
	jtag_tck_clr(p);
	jtag_tck_set(p);

	/* JTAG state = Capture-IR */
	jtag_tck_clr(p);
	jtag_tck_set(p);

	/* JTAG state = Shift-IR, Shift in TDI (8-bit) */
	return jtag_default_shift(p, 8, instruction);

	/* JTAG state = Run-Test/Idle */
}

/* Shifts a given 8-bit byte into the JTAG data register through TDI.
 * data  : 8 bit data
 * return: scanned TDO value
 */
uint8_t jtag_default_dr_shift_8(struct jtdev *p, uint8_t data)
{
	/* JTAG state = Run-Test/Idle */
	jtag_tms_set(p);
	jtag_tck_clr(p);
	jtag_tck_set(p);

	/* JTAG state = Select DR-Scan */
	jtag_tms_clr(p);
	jtag_tck_clr(p);
	jtag_tck_set(p);

	/* JTAG state = Capture-DR */
	jtag_tck_clr(p);
	jtag_tck_set(p);

	/* JTAG state = Shift-DR, Shift in TDI (16-bit) */
	return jtag_default_shift(p, 8, data);

	/* JTAG state = Run-Test/Idle */
}

/* Shifts a given 16-bit word into the JTAG data register through TDI.
 * data  : 16 bit data
 * return: scanned TDO value
 */
uint16_t jtag_default_dr_shift_16(struct jtdev *p, uint16_t data)
{
	/* JTAG state = Run-Test/Idle */
	jtag_tms_set(p);
	jtag_tck_clr(p);
	jtag_tck_set(p);

	/* JTAG state = Select DR-Scan */
	jtag_tms_clr(p);
	jtag_tck_clr(p);
	jtag_tck_set(p);

	/* JTAG state = Capture-DR */
	jtag_tck_clr(p);
	jtag_tck_set(p);

	/* JTAG state = Shift-DR, Shift in TDI (16-bit) */
	return jtag_default_shift(p, 16, data);

	/* JTAG state = Run-Test/Idle */
}


/* Shifts a given 20-bit word into the JTAG data register through TDI.
 * data  : 20 bit data
 * return: scanned TDO value
 */
uint32_t jtag_default_dr_shift_20(struct jtdev *p, uint32_t data)
{
	/* JTAG state = Run-Test/Idle */
	jtag_tms_set(p);
	jtag_tck_clr(p);
	jtag_tck_set(p);

	/* JTAG state = Select DR-Scan */
	jtag_tms_clr(p);
	jtag_tck_clr(p);
	jtag_tck_set(p);

	/* JTAG state = Capture-DR */
	jtag_tck_clr(p);
	jtag_tck_set(p);

	/* JTAG state = Shift-DR, Shift in TDI (20-bit) */
	return jtag_default_shift(p, 20, data);

	/* JTAG state = Run-Test/Idle */
}

void jtag_default_tms_sequence(struct jtdev *p, int bits, unsigned int value)
{
	for (int i = 0; i < bits; ++i) {
		jtag_tck_clr(p);
		if (value & (1u << i))
			jtag_tms_set(p);
		else
			jtag_tms_clr(p);
		jtag_tck_set(p);
	}
}

void jtag_default_init_dap(struct jtdev *p)
{
	jtag_rst_clr(p);
	p->f->jtdev_power_on(p);
	jtag_tdi_set(p);
	jtag_tms_set(p);
	jtag_tck_set(p);
	jtag_tclk_set(p);

	jtag_rst_set(p);
	jtag_tst_clr(p);

	jtag_tst_set(p);
	jtag_rst_clr(p);
	jtag_tst_clr(p);

	jtag_tst_set(p);

	p->f->jtdev_connect(p);
	jtag_rst_set(p);
	jtag_default_reset_tap(p);
}

/* ------------------------------------------------------------------------- */

static const struct jtaglib_funcs* get_jlf(struct jtdev *p)
{
	// FIXME: this function only looks at the chip ID, while the device info DB
	//        is more fine-grained about function mapping. so use that instead
	//        when possible

	static const struct jtaglib_funcs* lut[] = {
		NULL,
		&jlf_cpu16,
		&jlf_cpux,
		&jlf_cpuxv2
	};

	if (p->cpu_type != 0) {
		if (p->cpu_type < sizeof(lut)/sizeof(*lut)) {
			return lut[p->cpu_type];
		} else {
			printc_err("jtaglib: ERROR: bad CPU type %d\n", p->cpu_type);
			return NULL;
		}
	} else {
		if (p->jtag_id == 0) {
			printc_err("jtaglib: ERROR: no JTAG ID set!\n");
			__builtin_trap();
			return NULL;
		} else if (JTAG_ID_IS_XV2(p->jtag_id)) {
			return &jlf_cpuxv2;
		} else {
			// Here, it's hard to predict whether the target is CPUX or regular
			// from the JTAG ID alone. However, this is only needed relatively
			// little as the actual MCU ID should be read from info memory
			// after connecting, so let's just assume old here, and add some
			// extra care to the few functions that will be called before the
			// ID readout happens to make it all work
			return &jlf_cpu16;
		}
	}
}

unsigned int jtag_get_device(struct jtdev *p)
{
	// NOTE: this is one of the special functions that have to be called early
	//       on before chip ID stuff is done

	unsigned int r = get_jlf(p)->jlf_get_device(p);

	if (r != 0) jtag_led_green_on(p);

	return r;
}

/* Reads one byte/word from a given address */
uint16_t jtag_read_mem(struct jtdev *p, unsigned int format, address_t address)
{
	// NOTE: this is one of the special functions that have to be called early
	//       on before chip ID stuff is done

	return get_jlf(p)->jlf_read_mem(p, format, address);
}

/* Execute a Power-Up Clear (PUC) using JTAG CNTRL SIG register */
unsigned int jtag_execute_puc(struct jtdev *p)
{
	// NOTE: this is one of the special functions that have to be called early
	//       on before chip ID stuff is done

	return get_jlf(p)->jlf_execute_puc(p);
}

/* Reads an array of words from target memory */
void jtag_read_mem_quick(struct jtdev *p, address_t start_address,
			 unsigned int word_count, uint16_t *data)
{
	get_jlf(p)->jlf_read_mem_quick(p, start_address, word_count, data);
}

/* Writes one byte/word at a given address */
void jtag_write_mem(struct jtdev *p, unsigned int format, address_t address,
		    uint16_t data)
{
	get_jlf(p)->jlf_write_mem(p, format, address, data);
}

/* Writes an array of words into target memory */
void jtag_write_mem_quick(struct jtdev *p, address_t start_address,
			  unsigned int word_count, const uint16_t *data)
{
	get_jlf(p)->jlf_write_mem_quick(p, start_address, word_count, data);
}

/* Release the target device from JTAG control */
void jtag_release_device(struct jtdev *p, address_t address)
{
	jtag_led_green_off(p);

	get_jlf(p)->jlf_release_device(p, address);
}

/* Performs a verification over the given memory range
 * return: 1 -  verification was successful
 *         0 -  otherwise
 */
int jtag_verify_mem(struct jtdev *p, address_t start_address,
		    unsigned int length, const uint16_t *data)
{
	return get_jlf(p)->jlf_verify_mem(p, start_address, length, data);
}

/* Performs an erase check over the given memory range
 * return: 1 - erase check was successful
 *         0 - otherwise
 */
int jtag_erase_check(struct jtdev *p,
		     address_t start_address,
		     unsigned int length)
{
	return get_jlf(p)->jlf_erase_check(p, start_address, length);
}

/* Programs/verifies an array of words into a FLASH */
void jtag_write_flash(struct jtdev *p, address_t start_address,
		      unsigned int word_count, const uint16_t *data)
{
	jtag_led_red_on(p);

	get_jlf(p)->jlf_write_flash(p, start_address, word_count, data);

	jtag_led_red_off(p);
}

/* Performs a mass erase or a segment erase of a FLASH module */
void jtag_erase_flash(struct jtdev *p, unsigned int erase_mode, address_t erase_address)
{
	jtag_led_red_on(p);

	get_jlf(p)->jlf_erase_flash(p, erase_mode, erase_address);

	jtag_led_red_off(p);
}

/* Reads a register from the target CPU */
address_t jtag_read_reg(struct jtdev *p, int reg)
{
	return get_jlf(p)->jlf_read_reg(p, reg);
}

/* Writes a value into a register of the target CPU */
void jtag_write_reg(struct jtdev *p, int reg, address_t value)
{
	get_jlf(p)->jlf_write_reg(p, reg, value);
}
void jtag_single_step(struct jtdev *p)
{
	get_jlf(p)->jlf_single_step(p);
}
unsigned int jtag_set_breakpoint(struct jtdev *p, int bp_num, address_t bp_addr)
{
	return get_jlf(p)->jlf_set_breakpoint(p, bp_num, bp_addr);

}
unsigned int jtag_cpu_state(struct jtdev *p)
{
	return get_jlf(p)->jlf_cpu_state(p);
}
int jtag_get_config_fuses(struct jtdev *p)
{
	// NOTE: this is one of the special functions that have to be called early
	//       on before chip ID stuff is done

	return get_jlf(p)->jlf_get_config_fuses(p);
}

/* ------------------------------------------------------------------------- */

static uint8_t jtag_get_id(struct jtdev *p)
{
	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
	return jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
}

/* Take target device under JTAG control.
 * Disable the target watchdog.
 * return: 0 - fuse is blown
 *        >0 - jtag id
 */
unsigned int jtag_init(struct jtdev *p)
{
	unsigned int jtag_id, jtag_id_2;

	jtag_init_dap(p);

	/* Check fuse */
	if (jtag_is_fuse_blown(p)) {
		printc_err("jtag_init: fuse is blown\n");
		p->failed = 1;
		return 0;
	}

	jtag_id = jtag_get_id(p);
	p->jtag_id = jtag_id;

	/* Set device into JTAG mode */
	jtag_id_2 = jtag_get_device(p);
	if (jtag_id_2 == 0) {
		printc_err("jtag_init: invalid jtag_id: 0x%02x\n", jtag_id);
		p->failed = 1;
		return 0;
	}
	if (jtag_id != jtag_id_2) {
		printc_err("jtag_init: inconsistent jtag_id: 0x%02x vs 0x%02x\n",
				jtag_id, jtag_id_2);
		p->failed = 1;
		return 0;
	}

	/* Perform PUC, includes target watchdog disable */
	if (jtag_execute_puc(p) != jtag_id) {
		printc_err("jtag_init: PUC failed\n");
		p->failed = 1;
		return 0;
	}

	return jtag_id;
}

/* This function checks if the JTAG access security fuse is blown
 * return: 1 - fuse is blown
 *         0 - otherwise
 */
int jtag_is_fuse_blown (struct jtdev *p)
{
	unsigned int loop_counter;

	/* First trial could be wrong */
	for (loop_counter = 3; loop_counter > 0; loop_counter--) {
		jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
		if (jtag_dr_shift_16(p, 0xAAAA) == 0x5555)
			/* Fuse is blown */
			return 1;
	}

	/* Fuse is not blown */
	return 0;
}

/*----------------------------------------------------------------------------*/
int jtag_refresh_bps(device_t dev, struct jtdev *p)
{
	int i;
	int ret;
	struct device_breakpoint *bp;
	address_t addr;
	ret = 0;

	for (i = 0; i < dev->max_breakpoints; i++) {
		bp = &dev->breakpoints[i];

		printc_dbg("jtaglib: refresh breakpoint %d: type=%d "
			   "addr=%04x flags=%04x\n", i, bp->type, bp->addr, bp->flags);

		if ( (bp->flags &  DEVICE_BP_DIRTY) &&
		     (bp->type  == DEVICE_BPTYPE_BREAK) ) {
			addr = bp->addr;

			if ( !(bp->flags & DEVICE_BP_ENABLED) ) {
				addr = 0;
			}

			if ( jtag_set_breakpoint (p, i, addr) == 0) {
				printc_err("jtaglib: failed to refresh breakpoint #%d\n", i);
				ret = -1;
			} else {
				bp->flags &= ~DEVICE_BP_DIRTY;
			}
		}
	}

	return ret;
}

/* ========================================================================= */

static int write_flash_block(struct jtdev *p, address_t addr,
		address_t len, const uint8_t *data)
{
	uint16_t* word = malloc( len / 2 * sizeof(*word) );
	if (!word) {
		pr_error("jtaglib: failed to allocate memory");
		return -1;
	}

	for (unsigned int i = 0; i < len/2; i++) {
		word[i] = data[2*i] + (((uint16_t)data[2*i+1]) << 8);
	}
	jtag_write_flash(p, addr, len/2, word);

	free(word);

	return p->failed ? -1 : 0;
}

static int read_words(device_t dev_base, const struct chipinfo_memory *m,
		address_t addr, address_t len, uint8_t *data) {
	struct jtdev *p = (struct jtdev*)dev_base;

	printc_dbg("jtaglib: read_words: ram: len=%d addr=%06x\n", len, addr);
	if (len > 2 && !(len & 1)) {
		uint16_t* word = malloc((len>>1) * sizeof(*word));
		if (!word) {
			pr_error("jtaglib: failed to allocate memory");
			return -1;
		}

		jtag_read_mem_quick(p, addr, len >> 1, word);

		for (unsigned int i = 0; i < len/2; i++) {
			data[2*i+0] = word[i] & 0xff;
			data[2*i+1] = word[i] >> 8;
		}
		free(word);

		return p->failed ? -1 : len;
	}

	for (unsigned int index = 0; index < len; index += 2) {
		unsigned int word = jtag_read_mem(p, 16, addr+index);
		data[index  ] =  word       & 0x00ff;
		data[index+1] = (word >> 8) & 0x00ff;
	}

	return p->failed ? -1 : len;
}
static int write_words(device_t dev_base, const struct chipinfo_memory *m,
		address_t addr, address_t len, const uint8_t *data) {
	struct jtdev *p = (struct jtdev*)dev_base;
	int r;

	if (m->type != CHIPINFO_MEMTYPE_FLASH) {
		printc_dbg("jtaglib: write_words: ram: len=%d addr=%06x\n", len, addr);
		uint16_t value = data[0] | ((uint16_t)data[1] << 8);
		jtag_write_mem(p, 16, addr, value);
		r = p->failed ? -1 : 0;
	} else {
		r = write_flash_block(p, addr, len, data);
	}

	if (r < 0) {
		printc_err("jtaglib: write_words at address 0x%x failed\n", addr);
		return -1;
	}

	return len;}

int jtag_dev_readmem(device_t dev_base, address_t addr, uint8_t *mem, address_t len) {
	struct jtdev *p = (struct jtdev*)dev_base;
	p->failed = 0;
	return readmem(dev_base, addr, mem, len, read_words);
}
int jtag_dev_writemem(device_t dev_base, address_t addr, const uint8_t *mem, address_t len) {
	struct jtdev *p = (struct jtdev*)dev_base;
	p->failed = 0;
	return writemem(dev_base, addr, mem, len, write_words, read_words);
}
int jtag_dev_erase(device_t dev_base, device_erase_type_t type, address_t addr) {
	struct jtdev *p = (struct jtdev*)dev_base;

	p->failed = 0;

	switch (type) {
	case DEVICE_ERASE_MAIN:
		jtag_erase_flash(p, JTAG_ERASE_MAIN, addr);
		break;
	case DEVICE_ERASE_ALL:
		jtag_erase_flash(p, JTAG_ERASE_MASS, addr);
		break;
	case DEVICE_ERASE_SEGMENT:
		jtag_erase_flash(p, JTAG_ERASE_SGMT, addr);
		break;
	default: return -1;
	}

	return p->failed ? -1 : 0;
}

int jtag_dev_getregs(device_t dev_base, address_t *regs) {
	struct jtdev *p = (struct jtdev*)dev_base;

	p->failed = 0;

	for (int i = 0; i < DEVICE_NUM_REGS; i++) {
		regs[i] = jtag_read_reg(p, i);
	}

	return p->failed ? -1 : 0;
}
int jtag_dev_setregs(device_t dev_base, const address_t *regs) {
	struct jtdev *p = (struct jtdev*)dev_base;

	p->failed = 0;

	for (int i = 0; i < DEVICE_NUM_REGS; i++) {
		jtag_write_reg(p, i, regs[i]);
	}
	return p->failed ? -1 : 0;
}

int jtag_dev_ctl(device_t dev_base, device_ctl_t type) {
	struct jtdev *p = (struct jtdev*)dev_base;

	p->failed = 0;

	switch (type) {
	case DEVICE_CTL_RESET:
		/* perform soft reset */
		jtag_execute_puc(p);
		break;

	case DEVICE_CTL_RUN:
		/* transfer changed breakpoints to device */
		if (jtag_refresh_bps(&p->base, p) < 0)
			return -1;

		/* start program execution at current PC */
		jtag_release_device(p, 0xffff);
		break;

	case DEVICE_CTL_HALT:
		/* take device under JTAG control */
		jtag_get_device(p);
		break;

	case DEVICE_CTL_STEP:
		/* execute next instruction at current PC */
		jtag_single_step(p);
		break;

	default:
		printc_err("mehfet: unsupported operation %d\n", type);
		return -1;
	}

	return p->failed ? -1 : 0;
}
device_status_t jtag_dev_poll(device_t dev_base) {
	struct jtdev *p = (struct jtdev*)dev_base;

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

	int r = jtag_cpu_state(p);

	if (r == 1) return DEVICE_STATUS_HALTED;

	return DEVICE_STATUS_RUNNING;
}
int jtag_dev_getconfigfuses(device_t dev_base) {
	struct jtdev *p = (struct jtdev*)dev_base;

	return jtag_get_config_fuses(p);
}

/* ------------------------------------------------------------------------- */

static uint8_t jtag_ids_known[] = {
	0x89, 0x8D, 0x91, 0x95, 0x98, 0x99
};

static int idproc_89(struct jtdev *p, uint32_t id_data_addr, struct chipinfo_id *id)
{
	uint16_t iddata[8];

	printc_dbg("Identify (89)...\n");
	printc_dbg("Read device ID bytes at 0x%05x...\n", id_data_addr);
	memset(iddata, 0, sizeof iddata);

	// read 8 words starting at id_data_addr
	// don't use read_mem_quick because we want to minimize the number of
	// functions that need special care before the device ID is read
	for (int i = 0; i < sizeof(iddata)/sizeof(*iddata); ++i) {
		iddata[i] = jtag_read_mem(p, 16, id_data_addr + i*2);
	}

	id->ver_id = iddata[0];
	id->ver_sub_id = 0;
	id->revision = iddata[1];
	id->fab = iddata[1] >> 8;
	id->self = iddata[2];
	id->config = (iddata[6]>>8) & 0x7f;
	if (get_jlf(p)->jlf_get_config_fuses) {
		id->fuses = jtag_get_config_fuses(p);
	} else {
		id->fuses = 0x55;
	}

	return 0;
}

static int idproc_9x(struct jtdev *p, uint32_t dev_id_ptr, struct chipinfo_id *id)
{
	uint16_t iddata[8];
	uint8_t info_len;
	int tlv_size;

	printc_dbg("Identify (9x)...\n");
	printc_dbg("Read device ID bytes at 0x%05x...\n", dev_id_ptr);
	memset(iddata, 0, sizeof iddata);

	// read 8 words starting at dev_id_ptr
	// need to use read_mem_quick because apparently it's the only one capable
	// of reading from TLV...
	jtag_read_mem_quick(p, dev_id_ptr, sizeof(iddata)/sizeof(*iddata), iddata);
	/*for (int i = 0; i < 8; ++i) {
		printc_dbg("TLV[%d] = %04x\n", i, iddata[i]);
	}*/

	info_len = iddata[0] & 0xff;
	id->ver_id = iddata[2];
	id->revision = iddata[3]&0xff;
	id->config = iddata[3]>>8;
	id->fab = 0x55;
	id->self = 0x5555;
	id->fuses = 0x55;

	if (info_len < 1 || info_len > 11) {
		printc_dbg("jtaglib: idproc_9x: invalid info length %d\n", info_len);
		return 0;
	}

	printc_dbg("Read TLV...\n");

	tlv_size = ((1 << info_len) - 2) << 2;
	uint8_t *tlvdata = (uint8_t*)malloc(tlv_size);
	if (!tlvdata) {
		printc_err("jtaglib: idproc_9x: can't allocate %d bytes of memory for TLV\n",
				tlv_size);
		return -1;
	}
	jtag_read_mem_quick(p, dev_id_ptr, tlv_size >> 1, (uint16_t*)tlvdata);

	/* search TLV for sub-ID */
	for (int i = 8; i + 3 < tlv_size; ) {
		uint8_t tag = tlvdata[i]; ++i;
		uint8_t len = tlvdata[i]; ++i;

		if (tag == 0xff) break;

		if (tag == 0x14 && len >= 2) {
			id->ver_sub_id = r16le(&tlvdata[i]);
			break;
		}

		i += len;
	}

	return 0;
}

int jtag_dev_init(struct jtdev *p) {
	unsigned int jtagid;
	uint32_t dev_id_addr;
	struct chipinfo_id id;
	const struct chipinfo *chip;

	jtagid = jtag_init(p);
	if (p->failed) return -1;

	memset(&id, 0, sizeof id);

	printc("JTAG ID: 0x%02x\n", jtagid);

	bool found = false;
	for (int i = 0; i < sizeof(jtag_ids_known)/sizeof(*jtag_ids_known); ++i) {
		if (jtagid == jtag_ids_known[i]) {
			found = true;
			break;
		}
	}

	if (!found) {
		printc_err("jtaglib: unknown JTAG ID: 0x%02x\n", jtagid);
		jtag_release_device(p, 0xfffe);
		return -1;
	}

	if (JTAG_ID_IS_XV2(jtagid)) {
		uint16_t core_ip_id;
		unsigned int device_id;

		jtag_ir_shift(p, IR_COREIP_ID);
		core_ip_id = jtag_dr_shift_16(p, 0);

		if (jtagid == 0x95) delay_ms(1500);

		jtag_ir_shift(p, IR_DEVICE_ID);
		device_id = jtag_dr_shift_20(p, 0);

		//device_id = ((device_id & 0xffff) << 4) | (device_id >> 16);
		printc_dbg("jtaglib: Xv2: core IP ID=%04x, device ID=%06x\n",
				core_ip_id, device_id);

		if (device_id != 0 && (device_id & 0xff) != 0x80)
			dev_id_addr = device_id + 4;
		else dev_id_addr = 0x1a00; // ???
	} else {
		dev_id_addr = 0x0ff0;
	}

	if (!JTAG_ID_IS_XV2(jtagid)) {
		if (idproc_89(p, dev_id_addr, &id) < 0)
			return -1;
	} else {
		if (idproc_9x(p, dev_id_addr, &id) < 0)
			return -1;
	}

	printc_dbg("  ver_id:         %04x\n", id.ver_id);
	printc_dbg("  ver_sub_id:     %04x\n", id.ver_sub_id);
	printc_dbg("  revision:       %02x\n", id.revision);
	printc_dbg("  fab:            %02x\n", id.fab);
	printc_dbg("  self:           %04x\n", id.self);
	printc_dbg("  config:         %02x\n", id.config);
	printc_dbg("  fuses:          %02x\n", id.fuses);
	//printc_dbg("  activation_key: %08x\n", id.activation_key);

	chip = chipinfo_find_by_id(&id);
	if (!chip) {
		printc_err("jtaglib: unknown chip ID\n");
	} else {
		p->base.chip = chip;
	}

	return 0;
}