/*
 * 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 "etm.h"

extern u_char rvi_msg[];
extern int rvi_msg_len;

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

	if (rvi_msg[3]) {
		print_etm_pkt_raw("r8 error");
		return;
	}
	num = rvi_msg_len - 7;
	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
r16_response()
{
	char buf[MAX_PKT_FROM_TARGET*3+80], *dp;
	int num, i, d, off;

	if (rvi_msg[3]) {
		print_etm_pkt_raw("r16 error");
		return;
	}
	num = rvi_msg_len - 7;
	if (num & 1) {
		print_etm_pkt_raw("r16 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
r32_response()
{
	char buf[MAX_PKT_FROM_TARGET*3+80], *dp;
	int num, i, d, off;

	if (rvi_msg[3]) {
		print_etm_pkt_raw("r32 error");
		return;
	}
	num = rvi_msg_len - 7;
	if (num & 3) {
		print_etm_pkt_raw("r32 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];
		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 + 3;
	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:
			r32_response();
			return;
		case 0x01:
			r8_response();
			return;
		case 0x02:
			r16_response();
			return;
		default:
			goto unknown;
		}
	case TMCORE_OPC_ECHO:
		echo_response();
		return;
	case TMCORE_OPC_VERSION:
		version_response();
		return;
	case TMCORE_OPC_CODEC_RD:
	case TMCORE_OPC_CODEC_WR:
		/* not yet implemented */
		goto unknown;
	case TMCORE_OPC_DIEID:
		dieid_response();
		return;
	default:
	unknown:
		print_etm_pkt_raw("ETM_CORE resp");
	}
}