--- a/rvinterf/lowlevel/format_g23.c	Sat Apr 19 05:23:11 2014 +0000
+++ b/rvinterf/lowlevel/format_g23.c	Sun Apr 20 20:39:53 2014 +0000
@@ -1,14 +1,14 @@
 /*
  * This module implements the decoding of G23 trace packets into
- * human-readable form.  Because I have not yet reached the point
- * of integrating the target-side code that generates these packets,
- * I do not yet have the proper understanding of their format.
- * Therefore, the current decoding logic implemented here is based
- * on a heuristic examination of what the packets look like to an
- * uninitiated eye.
+ * human-readable form - or more precisely, traces, system primitives
+ * and other packets that can be emitted through GPF on targets.
+ * The decoding is based on my (Space Falcon's) understanding
+ * of the packet format, which is in turn based on the GPF sources
+ * available to us, now integrated under gsm-fw/gpf.
  */
 
 #include <sys/types.h>
+#include <ctype.h>
 #include <stdio.h>
 #include <string.h>
 #include <strings.h>
@@ -17,80 +17,58 @@
 
 extern u_char rxpkt[];
 extern size_t rxpkt_len;
+extern char fmtbuf[];
+
+static char *fmtbuf_ptr;
 
 static int
-is_well_formed()
+basic_checks()
 {
 	int i, c;
 
 	if (rxpkt_len < 17)
 		return(0);
+	/* check version bits in the header byte */
+	if ((rxpkt[1] & 0xC0) != 0x80)
+		return(0);
+	/* check the length */
+	c = rxpkt[2] | rxpkt[3] << 8;
+	if (c + 4 != rxpkt_len)
+		return(0);
+	/* ensure that the "from" and "to" are printable ASCII */
 	for (i = 8; i < 16; i++) {
 		c = rxpkt[i];
 		if (c < ' ' || c > '~')
 			return(0);
 	}
+	/* basic checks pass */
 	return(1);
 }
 
-static void
-print_well_formed()
+static int
+psprim_extra_checks()
 {
-	char buf[MAX_PKT_FROM_TARGET*4];
 	int i, c;
-	char *dp;
 
-	dp = buf;
-	strcpy(dp, "G23:");
-	dp += 4;
-	for (i = 1; i <= 7; i++) {
-		sprintf(dp, " %02X", rxpkt[i]);
-		dp += 3;
-	}
-	sprintf(dp, " \"%.4s\" \"%.4s\" ", rxpkt+8, rxpkt+12);
-	dp += 15;
-	i = 16;
-	if (rxpkt[i] < 0x20) {
-		sprintf(dp, "%02X ", rxpkt[i]);
-		dp += 3;
-		i++;
+	if (rxpkt_len < 24)
+		return(0);
+	/* "original rcvr" field needs to be printable ASCII */
+	for (i = 16; i < 20; i++) {
+		c = rxpkt[i];
+		if (c < ' ' || c > '~')
+			return(0);
 	}
-	if (rxpkt_len - i == 5 && rxpkt[i] == '%' &&
-	    !rxpkt[i+3] && !rxpkt[i+4]) {
-		sprintf(dp, "%d", rxpkt[i+2] << 8 | rxpkt[i+1]);
-		output_line(buf);
-		return;
-	}
-	*dp++ = '\"';
-	for (; i < rxpkt_len; i++) {
-		c = rxpkt[i];
-		if (c & 0x80) {
-			*dp++ = 'M';
-			*dp++ = '-';
-			c &= 0x7F;
-		}
-		if (c < 0x20) {
-			*dp++ = '^';
-			*dp++ = c + '@';
-		} else if (c == 0x7F) {
-			*dp++ = '^';
-			*dp++ = '?';
-		} else
-			*dp++ = c;
-	}
-	*dp++ = '\"';
-	*dp = '\0';
-	output_line(buf);
+	/* checks pass */
+	return(1);
 }
 
 static void
 print_malformed()
 {
-	char buf[MAX_PKT_FROM_TARGET*3+6];
 	int i;
 	char *dp;
 
-	dp = buf;
+	dp = fmtbuf;
 	strcpy(dp, "G23 UNK:");
 	dp += 8;
 	for (i = 1; i < rxpkt_len; i++) {
@@ -98,14 +76,166 @@
 		dp += 3;
 	}
 	*dp = '\0';
-	output_line(buf);
+	output_line(fmtbuf);
+}
+
+static int
+entity_name_well_formed(p)
+	char *p;
+{
+	int i, len;
+
+	if (!isupper(p[0]))
+		return(0);
+	for (i = 0; i < 4; i++)
+		if (!isalnum(p[i]))
+			break;
+	len = i;
+	for (; i < 4; i++)
+		if (p[i] != ' ')
+			return(0);
+	return(len);
+}
+
+static void
+print_entity_name(raw)
+	char *raw;
+{
+	int len;
+
+	len = entity_name_well_formed(raw);
+	if (len)
+		sprintf(fmtbuf_ptr, "%.*s", len, raw);
+	else
+		sprintf(fmtbuf_ptr, "\"%.4s\"", raw);
+	fmtbuf_ptr = index(fmtbuf_ptr, '\0');
+}
+
+static void
+print_common_hdr(typestr)
+	char *typestr;
+{
+	sprintf(fmtbuf, "G23 %s id=%02X ts=%02X%02X%02X%02X ", typestr,
+		rxpkt[1], rxpkt[7], rxpkt[6], rxpkt[5], rxpkt[4]);
+	fmtbuf_ptr = index(fmtbuf, '\0');
+	print_entity_name(rxpkt + 8);
+	*fmtbuf_ptr++ = '-';
+	*fmtbuf_ptr++ = '>';
+	print_entity_name(rxpkt + 12);
+	*fmtbuf_ptr++ = ' ';
+}
+
+static void
+format_text(start_off)
+{
+	int i, c;
+
+	*fmtbuf_ptr++ = '\"';
+	for (i = start_off; i < rxpkt_len; i++) {
+		c = rxpkt[i];
+		if (c & 0x80) {
+			*fmtbuf_ptr++ = 'M';
+			*fmtbuf_ptr++ = '-';
+			c &= 0x7F;
+		}
+		if (c < 0x20) {
+			*fmtbuf_ptr++ = '^';
+			*fmtbuf_ptr++ = c + '@';
+		} else if (c == 0x7F) {
+			*fmtbuf_ptr++ = '^';
+			*fmtbuf_ptr++ = '?';
+		} else
+			*fmtbuf_ptr++ = c;
+	}
+	*fmtbuf_ptr++ = '\"';
+	*fmtbuf_ptr = '\0';
+	output_line(fmtbuf);
+}
+
+static void
+format_compressed_trace(start_off)
+{
+	int i;
+
+	i = start_off + 1;
+	sprintf(fmtbuf_ptr, "%d", rxpkt[i+1] << 8 | rxpkt[i]);
+	fmtbuf_ptr = index(fmtbuf_ptr, '\0');
+	i += 4;
+	for (; i < rxpkt_len; i++) {
+		sprintf(fmtbuf_ptr, " %02X", rxpkt[i]);
+		fmtbuf_ptr += 3;
+	}
+	*fmtbuf_ptr = '\0';
+	output_line(fmtbuf);
+}
+
+static void
+format_trace()
+{
+	int i;
+
+	i = 16;
+	if (rxpkt[i] < 0x20) {
+		sprintf(fmtbuf_ptr, "tc=%02X ", rxpkt[i]);
+		fmtbuf_ptr += 6;
+		i++;
+	}
+	if (rxpkt_len - i >= 5 && rxpkt[i] == '%' &&
+	    !rxpkt[i+3] && !rxpkt[i+4])
+		format_compressed_trace(i);
+	else
+		format_text(i);
+}
+
+static void
+format_psprim()
+{
+	int i;
+
+	/* original destination */
+	*fmtbuf_ptr++ = '(';
+	print_entity_name(rxpkt + 16);
+	*fmtbuf_ptr++ = ')';
+	/* opcode */
+	sprintf(fmtbuf_ptr, " %02X%02X%02X%02X",
+		rxpkt[23], rxpkt[22], rxpkt[21], rxpkt[20]);
+	fmtbuf_ptr += 9;
+	for (i = 24; i < rxpkt_len; i++) {
+		sprintf(fmtbuf_ptr, " %02X", rxpkt[i]);
+		fmtbuf_ptr += 3;
+	}
+	*fmtbuf_ptr = '\0';
+	output_line(fmtbuf);
 }
 
 void
 print_g23_trace()
 {
-	if (is_well_formed())
-		print_well_formed();
-	else
+	if (!basic_checks()) {
 		print_malformed();
+		return;
+	}
+	/* dispatch by type */
+	switch (rxpkt[1] & 0x30) {
+	case 0x10:
+		/* PS primitive */
+		if (psprim_extra_checks()) {
+			print_common_hdr("PSprim");
+			format_psprim();
+		} else
+			print_malformed();
+		return;
+	case 0x20:
+		/* trace */
+		print_common_hdr("trace");
+		format_trace();
+		return;
+	case 0x30:
+		/* system primitive */
+		print_common_hdr("sysprim");
+		format_text(16);
+		return;
+	default:
+		print_malformed();
+	}
 }