--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rvinterf/tmsh/tmcore.c	Wed Feb 05 04:02:13 2014 +0000
@@ -0,0 +1,448 @@
+/*
+ * 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"
+
+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");
+	}
+}
+
+void
+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;
+	}
+	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);
+}
+
+void
+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;
+	}
+	if (count < 1 || count > 126) {
+		printf("error: count argument outside valid range\n");
+		return;
+	}
+	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);
+}
+
+void
+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;
+	}
+	if (count < 1 || count > 63) {
+		printf("error: count argument outside valid range\n");
+		return;
+	}
+	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);
+}
+
+void
+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);
+}
+
+void
+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;
+	}
+	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);
+}
+
+void
+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;
+	}
+	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);
+}
+
+void
+cmd_dieid(argc, argv)
+	char **argv;
+{
+	u_char cmdpkt[4];
+
+	cmdpkt[1] = ETM_CORE;
+	cmdpkt[2] = TMCORE_OPC_DIEID;
+	send_etm_cmd(cmdpkt, 2);
+}
+
+void
+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;
+		}
+	} else
+		delay = 0;
+	if (argc > 2) {
+		size = strtoul(argv[2], 0, 0);
+		if (size > 240) {
+			printf("error: ping size argument too big\n");
+			return;
+		}
+	} 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);
+}
+
+void
+cmd_tgtreset(argc, argv)
+	char **argv;
+{
+	u_char cmdpkt[4];
+
+	cmdpkt[1] = ETM_CORE;
+	cmdpkt[2] = TMCORE_OPC_RESET;
+	send_etm_cmd(cmdpkt, 2);
+}
+
+void
+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);
+}