--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rvinterf/etm/tmcore.c	Mon Nov 25 02:11:47 2013 +0000
@@ -0,0 +1,180 @@
+/*
+ * 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");
+	}
+}