/*
 * In this module we are going to implement commands which send requests
 * to ETM_CORE and the handling of responses from that target module.
 */

#include <sys/types.h>
#include <stdio.h>
#include <string.h>
#include <strings.h>
#include <stdlib.h>
#include "pktmux.h"
#include "limits.h"
#include "localtypes.h"
#include "etm.h"
#include "exitcodes.h"

extern u_char rvi_msg[];
extern int rvi_msg_len;

static void
rw8_response()
{
	char buf[MAX_PKT_FROM_TARGET*3+80], *dp;
	int num, i;

	if (rvi_msg[3]) {
		print_etm_pkt_raw("rw8 error");
		return;
	}
	num = rvi_msg_len - 7;
	if (!num) {
		async_msg_output("w8 OK");
		return;
	}
	strcpy(buf, "r8:");
	dp = buf + 3;
	for (i = 0; i < num; i++) {
		sprintf(dp, " %02X", rvi_msg[i+6]);
		dp += 3;
	}
	async_msg_output(buf);
}

static void
rw16_response()
{
	char buf[MAX_PKT_FROM_TARGET*3+80], *dp;
	int num, i, d, off;

	if (rvi_msg[3]) {
		print_etm_pkt_raw("rw16 error");
		return;
	}
	num = rvi_msg_len - 7;
	if (!num) {
		async_msg_output("w16 OK");
		return;
	}
	if (num & 1) {
		print_etm_pkt_raw("rw16 malformed resp");
		return;
	}
	num >>= 1;
	strcpy(buf, "r16:");
	dp = buf + 4;
	off = 6;
	for (i = 0; i < num; i++) {
		d = rvi_msg[off] | rvi_msg[off+1] << 8;
		off += 2;
		sprintf(dp, " %04X", d);
		dp += 5;
	}
	async_msg_output(buf);
}

static void
rw32_response()
{
	char buf[MAX_PKT_FROM_TARGET*3+80], *dp;
	int num, i, d, off;

	if (rvi_msg[3]) {
		print_etm_pkt_raw("rw32 error");
		return;
	}
	num = rvi_msg_len - 7;
	if (!num) {
		async_msg_output("w32 OK");
		return;
	}
	if (num & 3) {
		print_etm_pkt_raw("rw32 malformed resp");
		return;
	}
	num >>= 2;
	strcpy(buf, "r32:");
	dp = buf + 4;
	off = 6;
	for (i = 0; i < num; i++) {
		d = rvi_msg[off] | rvi_msg[off+1] << 8 | rvi_msg[off+2] << 16
			| rvi_msg[off+3] << 24;
		off += 4;
		sprintf(dp, " %08X", d);
		dp += 9;
	}
	async_msg_output(buf);
}

static void
dieid_response()
{
	char buf[MAX_PKT_FROM_TARGET*3+80], *dp;
	int num, i;

	if (rvi_msg[3]) {
		print_etm_pkt_raw("dieid error");
		return;
	}
	num = rvi_msg_len - 6;
	strcpy(buf, "dieid resp:");
	dp = buf + 11;
	for (i = 0; i < num; i++) {
		sprintf(dp, " %02X", rvi_msg[i+5]);
		dp += 3;
	}
	async_msg_output(buf);
}

static void
echo_response()
{
	if (rvi_msg[3])
		print_etm_pkt_raw("echo error");
	else
		print_etm_pkt_raw("echo resp");
}

static void
version_response()
{
	char buf[80];

	if (rvi_msg[3]) {
		print_etm_pkt_raw("version error");
		return;
	}
	if (rvi_msg_len != 10) {
		print_etm_pkt_raw("version malformed resp");
		return;
	}
	sprintf(buf, "version resp: %02X%02X%02X%02X", rvi_msg[8], rvi_msg[7],
		rvi_msg[6], rvi_msg[5]);
	async_msg_output(buf);
}

void
tmcore_msg_rx()
{
	switch (rvi_msg[4]) {
	case TMCORE_OPC_MEM:
		if (rvi_msg_len < 7)
			goto unknown;
		switch (rvi_msg[5]) {
		case 0x00:
		case 0x04:
			rw32_response();
			return;
		case 0x01:
			rw8_response();
			return;
		case 0x02:
			rw16_response();
			return;
		default:
			goto unknown;
		}
	case TMCORE_OPC_ECHO:
		echo_response();
		return;
	case TMCORE_OPC_VERSION:
		version_response();
		return;
	case TMCORE_OPC_CODEC_RD:
		abbr_response();
		return;
	case TMCORE_OPC_CODEC_WR:
		abbw_response();
		return;
	case TMCORE_OPC_DIEID:
		dieid_response();
		return;
	default:
	unknown:
		print_etm_pkt_raw("ETM_CORE resp");
	}
}

cmd_r8(argc, argv)
	char **argv;
{
	u32 addr;
	int count;
	u_char cmdpkt[10];

	addr = strtoul(argv[1], 0, 16);
	if (argv[2])
		count = strtoul(argv[2], 0, 0);
	else
		count = 1;
	if (count < 1 || count > 253) {
		printf("error: count argument outside valid range\n");
		return(ERROR_USAGE);
	}
	cmdpkt[1] = ETM_CORE;
	cmdpkt[2] = TMCORE_OPC_MEM;
	cmdpkt[3] = 0x01;
	cmdpkt[4] = count;
	cmdpkt[5] = addr;
	cmdpkt[6] = addr >> 8;
	cmdpkt[7] = addr >> 16;
	cmdpkt[8] = addr >> 24;
	send_etm_cmd(cmdpkt, 8);
	return(0);
}

cmd_r16(argc, argv)
	char **argv;
{
	u32 addr;
	int count;
	u_char cmdpkt[10];

	addr = strtoul(argv[1], 0, 16);
	if (argv[2])
		count = strtoul(argv[2], 0, 0);
	else
		count = 1;
	if (addr & 1) {
		printf("error: address not aligned\n");
		return(ERROR_USAGE);
	}
	if (count < 1 || count > 126) {
		printf("error: count argument outside valid range\n");
		return(ERROR_USAGE);
	}
	cmdpkt[1] = ETM_CORE;
	cmdpkt[2] = TMCORE_OPC_MEM;
	cmdpkt[3] = 0x02;
	cmdpkt[4] = count;
	cmdpkt[5] = addr;
	cmdpkt[6] = addr >> 8;
	cmdpkt[7] = addr >> 16;
	cmdpkt[8] = addr >> 24;
	send_etm_cmd(cmdpkt, 8);
	return(0);
}

cmd_r32(argc, argv)
	char **argv;
{
	u32 addr;
	int count;
	u_char cmdpkt[10];

	addr = strtoul(argv[1], 0, 16);
	if (argv[2])
		count = strtoul(argv[2], 0, 0);
	else
		count = 1;
	if (addr & 3) {
		printf("error: address not aligned\n");
		return(ERROR_USAGE);
	}
	if (count < 1 || count > 63) {
		printf("error: count argument outside valid range\n");
		return(ERROR_USAGE);
	}
	cmdpkt[1] = ETM_CORE;
	cmdpkt[2] = TMCORE_OPC_MEM;
	cmdpkt[3] = 0x04;
	cmdpkt[4] = count;
	cmdpkt[5] = addr;
	cmdpkt[6] = addr >> 8;
	cmdpkt[7] = addr >> 16;
	cmdpkt[8] = addr >> 24;
	send_etm_cmd(cmdpkt, 8);
	return(0);
}

cmd_w8(argc, argv)
	char **argv;
{
	u32 addr, v;
	u_char cmdpkt[MAX_PKT_TO_TARGET];
	int di;
	char **ap;

	addr = strtoul(argv[1], 0, 16);
	cmdpkt[1] = ETM_CORE;
	cmdpkt[2] = TMCORE_OPC_MEM;
	cmdpkt[3] = 0x11;
	cmdpkt[4] = argc - 2;
	cmdpkt[5] = addr;
	cmdpkt[6] = addr >> 8;
	cmdpkt[7] = addr >> 16;
	cmdpkt[8] = addr >> 24;
	di = 9;
	for (ap = argv + 2; *ap; ap++) {
		v = strtoul(*ap, 0, 16);
		cmdpkt[di++] = v;
	}
	send_etm_cmd(cmdpkt, di - 1);
	return(0);
}

cmd_w16(argc, argv)
	char **argv;
{
	u32 addr, v;
	u_char cmdpkt[MAX_PKT_TO_TARGET];
	int di;
	char **ap;

	addr = strtoul(argv[1], 0, 16);
	if (addr & 1) {
		printf("error: address not aligned\n");
		return(ERROR_USAGE);
	}
	cmdpkt[1] = ETM_CORE;
	cmdpkt[2] = TMCORE_OPC_MEM;
	cmdpkt[3] = 0x12;
	cmdpkt[4] = argc - 2;
	cmdpkt[5] = addr;
	cmdpkt[6] = addr >> 8;
	cmdpkt[7] = addr >> 16;
	cmdpkt[8] = addr >> 24;
	di = 9;
	for (ap = argv + 2; *ap; ap++) {
		v = strtoul(*ap, 0, 16);
		cmdpkt[di++] = v;
		cmdpkt[di++] = v >> 8;
	}
	send_etm_cmd(cmdpkt, di - 1);
	return(0);
}

cmd_w32(argc, argv)
	char **argv;
{
	u32 addr, v;
	u_char cmdpkt[MAX_PKT_TO_TARGET];
	int di;
	char **ap;

	addr = strtoul(argv[1], 0, 16);
	if (addr & 3) {
		printf("error: address not aligned\n");
		return(ERROR_USAGE);
	}
	cmdpkt[1] = ETM_CORE;
	cmdpkt[2] = TMCORE_OPC_MEM;
	cmdpkt[3] = 0x14;
	cmdpkt[4] = argc - 2;
	cmdpkt[5] = addr;
	cmdpkt[6] = addr >> 8;
	cmdpkt[7] = addr >> 16;
	cmdpkt[8] = addr >> 24;
	di = 9;
	for (ap = argv + 2; *ap; ap++) {
		v = strtoul(*ap, 0, 16);
		cmdpkt[di++] = v;
		cmdpkt[di++] = v >> 8;
		cmdpkt[di++] = v >> 16;
		cmdpkt[di++] = v >> 24;
	}
	send_etm_cmd(cmdpkt, di - 1);
	return(0);
}

cmd_dieid(argc, argv)
	char **argv;
{
	u_char cmdpkt[4];

	cmdpkt[1] = ETM_CORE;
	cmdpkt[2] = TMCORE_OPC_DIEID;
	send_etm_cmd(cmdpkt, 2);
	return(0);
}

cmd_ping(argc, argv)
	char **argv;
{
	int delay, size;
	u_char cmdpkt[8];

	if (argc > 1) {
		delay = strtoul(argv[1], 0, 0);
		if (delay > 65535) {
			printf("error: ping delay argument too big\n");
			return(ERROR_USAGE);
		}
	} else
		delay = 0;
	if (argc > 2) {
		size = strtoul(argv[2], 0, 0);
		if (size > 240) {
			printf("error: ping size argument too big\n");
			return(ERROR_USAGE);
		}
	} else
		size = 1;
	cmdpkt[1] = ETM_CORE;
	cmdpkt[2] = TMCORE_OPC_ECHO;
	cmdpkt[3] = delay;
	cmdpkt[4] = delay >> 8;
	cmdpkt[5] = size;
	cmdpkt[6] = size >> 8;
	send_etm_cmd(cmdpkt, 6);
	return(0);
}

cmd_tgtreset(argc, argv)
	char **argv;
{
	u_char cmdpkt[4];

	cmdpkt[1] = ETM_CORE;
	cmdpkt[2] = TMCORE_OPC_RESET;
	send_etm_cmd(cmdpkt, 2);
	return(0);
}

cmd_version(argc, argv)
	char **argv;
{
	u32 arg;
	u_char cmdpkt[8];

	arg = strtoul(argv[1], 0, 16);
	cmdpkt[1] = ETM_CORE;
	cmdpkt[2] = TMCORE_OPC_VERSION;
	cmdpkt[3] = arg;
	cmdpkt[4] = arg >> 8;
	cmdpkt[5] = arg >> 16;
	cmdpkt[6] = arg >> 24;
	send_etm_cmd(cmdpkt, 6);
	return(0);
}