/***************************************************************************
 *   Copyright (C) 2008 by Spencer Oliver                                  *
 *   spen@spen-soft.co.uk                                                  *
 *                                                                         *
 *   Copyright (C) 2008 by David T.L. Wong                                 *
 *                                                                         *
 *   Copyright (C) 2009 by David N. Claffey <dnclaffey@gmail.com>          *
 *                                                                         *
 *   Copyright (C) 2011 by Drasko DRASKOVIC                                *
 *   drasko.draskovic@gmail.com                                            *
 *                                                                         *
 *   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, see <http://www.gnu.org/licenses/>. *
 ***************************************************************************/

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include "breakpoints.h"
#include "qorivva.h"
#include "register.h"
#include "smp.h"

static void qorivva_set_instr(struct qorivva_common *qorivva, uint32_t new_instr)
{
	struct jtag_tap *tap = qorivva->tap;
	
	assert(tap != NULL);
	if (buf_get_u32(tap->cur_instr, 0, tap->ir_length) != new_instr) {

		struct scan_field field;
		field.num_bits = tap->ir_length;

		uint8_t t[4] = { 0 };
		field.out_value = t;
		buf_set_u32(t, 0, field.num_bits, new_instr);

		field.in_value = NULL;

		jtag_add_ir_scan(tap, &field, TAP_IDLE);
	}
}

static int qorivva_capture_ir(struct qorivva_common *qorivva, uint32_t *ir)
{
	struct jtag_tap *tap = qorivva->tap;
	uint8_t t[4];

	assert(tap != NULL);
	assert((size_t)tap->ir_length <= sizeof(t) * 8);
	buf_set_ones(t, tap->ir_length);
	struct scan_field field;
	field.num_bits = tap->ir_length;
	field.out_value = t;
	field.in_value = t;
	jtag_add_plain_ir_scan(field.num_bits, field.out_value, field.in_value, TAP_IDLE);
	keep_alive();
	int retval = jtag_execute_queue();
	if (retval != ERROR_OK) {
		LOG_ERROR("ir read failed");
		return retval;
	}
	*ir = buf_get_u32(t, 0, 32);
	return ERROR_OK;
}

static void qorivva_drscan_32_queued(struct qorivva_common *qorivva, uint32_t data_out, uint8_t *data_in)
{
	assert(qorivva->tap != NULL);
	struct jtag_tap *tap = qorivva->tap;

	struct scan_field field;
	field.num_bits = 32;

	uint8_t scan_out[4] = { 0 };
	field.out_value = scan_out;
	buf_set_u32(scan_out, 0, field.num_bits, data_out);

	field.in_value = data_in;
	jtag_add_dr_scan(tap, 1, &field, TAP_IDLE);

	keep_alive();
}

static int qorivva_drscan_32(struct qorivva_common *qorivva, uint32_t *data)
{
	uint8_t scan_in[4];
	qorivva_drscan_32_queued(qorivva, *data, scan_in);

	int retval = jtag_execute_queue();
	if (retval != ERROR_OK) {
		LOG_ERROR("register read failed");
		return retval;
	}

	*data = buf_get_u32(scan_in, 0, 32);
	return ERROR_OK;
}

static int qorivva_poll(struct target *target)
{
	struct qorivva_common *qorivva = target_to_qorivva(target);
	uint32_t ir;
	int retval = qorivva_capture_ir(qorivva, &ir);
	if (retval != ERROR_OK) {
		return retval;
	}
	//LOG_INFO("captured ir:0x%x", ir);
	return ERROR_OK;
}

static int qorivva_target_create(struct target *target, Jim_Interp *interp)
{
	struct qorivva_common *qorivva = calloc(1, sizeof(struct qorivva_common));

	qorivva->common_magic = QORIVVA_COMMON_MAGIC;
	qorivva->tap = target->tap;
	target->arch_info = qorivva;
	return ERROR_OK;
}

static int qorivva_init_target(struct command_context *cmd_ctx,
		struct target *target)
{
	return ERROR_OK;
}

static int qorivva_examine(struct target *target)
{
	struct qorivva_common *qorivva = target_to_qorivva(target);

	if (!target_was_examined(target)) {
		qorivva_set_instr(qorivva, QORIVVA_INST_IDCODE);
		qorivva->idcode = 0;
		int retval = qorivva_drscan_32(qorivva, &qorivva->idcode);
		if (retval != ERROR_OK) {
			LOG_ERROR("idcode read failed");
			return retval;
		}
		// TODO: check qorivva->idcode?
		// Let's talk to the first e200Z4 core, we could also do TAP_PARALLEL to talk
		// to both Z4 cores and the Z2 core.
		qorivva_set_instr(qorivva, QORIVVA_INST_AUX_TAP_Z4A);
		// OnCE has a 10-bit IR
		qorivva->tap->ir_length = 10;
		target_set_examined(target);
	}
	return ERROR_OK;
}

struct target_type qorivva_target = {
	.name = "qorivva",

	.poll = qorivva_poll,
	//.arch_state = qorivva_arch_state,

	//.halt = qorivva_halt,
	//.resume = qorivva_resume,
	//.step = qorivva_step,

	//.assert_reset = qorivva_assert_reset,
	//.deassert_reset = qorivva_deassert_reset,

	//.get_gdb_reg_list = qorivva_get_gdb_reg_list,

	//.read_memory = qorivva_read_memory,
	//.write_memory = qorivva_write_memory,
	//.checksum_memory = qorivva_checksum_memory,
	//.blank_check_memory = qorivva_blank_check_memory,

	//.run_algorithm = qorivva_run_algorithm,

	//.add_breakpoint = qorivva_add_breakpoint,
	//.remove_breakpoint = qorivva_remove_breakpoint,
	//.add_watchpoint = qorivva_add_watchpoint,
	//.remove_watchpoint = qorivva_remove_watchpoint,

	//.commands = qorivva_command_handlers,
	.target_create = qorivva_target_create,
	.init_target = qorivva_init_target,
	.examine = qorivva_examine,
};