--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rvinterf/libprint/gpf_tst.c	Wed May 24 06:29:13 2023 +0000
@@ -0,0 +1,283 @@
+/*
+ * This libprint module (one public function plus many internal subfunctions)
+ * implements printing/formatting of GPF TST interface packets, which can be
+ * traces, sysprims, protocol stack primitives or unrecognized packet format.
+ */
+
+#include <sys/types.h>
+#include <ctype.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <strings.h>
+
+static int
+basic_checks(rxpkt, rxpkt_len)
+	u_char *rxpkt;
+	unsigned rxpkt_len;
+{
+	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 int
+psprim_extra_checks(rxpkt, rxpkt_len)
+	u_char *rxpkt;
+	unsigned rxpkt_len;
+{
+	int i, c;
+
+	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);
+	}
+	/* checks pass */
+	return(1);
+}
+
+static void
+print_malformed(rxpkt, rxpkt_len, extra_prefix, outfunc_head, outfunc_cont)
+	u_char *rxpkt;
+	unsigned rxpkt_len;
+	char *extra_prefix;
+	void (*outfunc_head)(), (*outfunc_cont)();
+{
+	char headline[80];
+
+	sprintf(headline, "%sGPF unrecognized packet format", extra_prefix);
+	outfunc_head(headline);
+	packet_hex_dump(rxpkt, rxpkt_len, outfunc_cont);
+}
+
+static void
+print_malloc_fail(rxpkt, rxpkt_len, extra_prefix, outfunc_head, outfunc_cont)
+	u_char *rxpkt;
+	unsigned rxpkt_len;
+	char *extra_prefix;
+	void (*outfunc_head)(), (*outfunc_cont)();
+{
+	char headline[80];
+
+	sprintf(headline, "%sGPF packet, failed malloc in formatting",
+		extra_prefix);
+	outfunc_head(headline);
+	packet_hex_dump(rxpkt, rxpkt_len, outfunc_cont);
+}
+
+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, outp)
+	char *raw, **outp;
+{
+	int len;
+
+	len = entity_name_well_formed(raw);
+	if (len) {
+		sprintf(*outp, "%.*s", len, raw);
+		*outp += len;
+	} else {
+		sprintf(*outp, "\"%.4s\"", raw);
+		*outp += 6;
+	}
+}
+
+static void
+print_common_hdr(rxpkt, outp, typestr, extra_prefix)
+	u_char *rxpkt;
+	char **outp, *typestr, *extra_prefix;
+{
+	sprintf(*outp, "%sGPF %s id=%02X ts=%02X%02X%02X%02X ", extra_prefix,
+		typestr, rxpkt[1], rxpkt[7], rxpkt[6], rxpkt[5], rxpkt[4]);
+	*outp = index(*outp, '\0');
+	print_entity_name(rxpkt + 8, outp);
+	*(*outp)++ = '-';
+	*(*outp)++ = '>';
+	print_entity_name(rxpkt + 12, outp);
+	*(*outp)++ = ' ';
+}
+
+static void
+format_text(rxpkt, rxpkt_len, start_off, outp)
+	u_char *rxpkt;
+	unsigned rxpkt_len, start_off;
+	char *outp;
+{
+	*outp++ = '\"';
+	safe_print_trace(rxpkt + start_off, rxpkt_len - start_off, outp);
+	outp = index(outp, '\0');
+	*outp++ = '\"';
+	*outp = '\0';
+}
+
+static void
+format_compressed_trace(rxpkt, rxpkt_len, start_off, outp)
+	u_char *rxpkt;
+	unsigned rxpkt_len, start_off;
+	char *outp;
+{
+	int i;
+
+	i = start_off + 1;
+	sprintf(outp, "%d", rxpkt[i+1] << 8 | rxpkt[i]);
+	outp = index(outp, '\0');
+	i += 4;
+	for (; i < rxpkt_len; i++) {
+		sprintf(outp, " %02X", rxpkt[i]);
+		outp += 3;
+	}
+	*outp = '\0';
+}
+
+static void
+format_trace(rxpkt, rxpkt_len, extra_prefix, outfunc_head, outfunc_cont)
+	u_char *rxpkt;
+	unsigned rxpkt_len;
+	char *extra_prefix;
+	void (*outfunc_head)(), (*outfunc_cont)();
+{
+	char *outbuf, *outp;
+	int i;
+
+	outbuf = malloc(rxpkt_len * 4 + strlen(extra_prefix) + 1);
+	if (!outbuf) {
+		print_malloc_fail(rxpkt, rxpkt_len, extra_prefix, outfunc_head,
+				  outfunc_cont);
+		return;
+	}
+	outp = outbuf;
+	print_common_hdr(rxpkt, &outp, "trace", extra_prefix);
+	i = 16;
+	if (rxpkt[i] < 0x20) {
+		sprintf(outp, "tc=%02X ", rxpkt[i]);
+		outp += 6;
+		i++;
+	}
+	if (rxpkt_len - i >= 5 && rxpkt[i] == '%' &&
+	    !rxpkt[i+3] && !rxpkt[i+4])
+		format_compressed_trace(rxpkt, rxpkt_len, i, outp);
+	else
+		format_text(rxpkt, rxpkt_len, i, outp);
+	outfunc_head(outbuf);
+	free(outbuf);
+}
+
+static void
+format_sysprim(rxpkt, rxpkt_len, extra_prefix, outfunc_head, outfunc_cont)
+	u_char *rxpkt;
+	unsigned rxpkt_len;
+	char *extra_prefix;
+	void (*outfunc_head)(), (*outfunc_cont)();
+{
+	char *outbuf, *outp;
+
+	outbuf = malloc(rxpkt_len * 4 + strlen(extra_prefix) + 1);
+	if (!outbuf) {
+		print_malloc_fail(rxpkt, rxpkt_len, extra_prefix, outfunc_head,
+				  outfunc_cont);
+		return;
+	}
+	outp = outbuf;
+	print_common_hdr(rxpkt, &outp, "sysprim", extra_prefix);
+	format_text(rxpkt, rxpkt_len, 16, outp);
+	outfunc_head(outbuf);
+	free(outbuf);
+}
+
+static void
+format_psprim(rxpkt, rxpkt_len, extra_prefix, outfunc_head, outfunc_cont)
+	u_char *rxpkt;
+	unsigned rxpkt_len;
+	char *extra_prefix;
+	void (*outfunc_head)(), (*outfunc_cont)();
+{
+	char headline[80];
+	char *outp = headline;
+
+	print_common_hdr(rxpkt, &outp, "PSprim", extra_prefix);
+	/* original destination */
+	*outp++ = '(';
+	print_entity_name(rxpkt + 16, &outp);
+	*outp++ = ')';
+	/* opcode and data length */
+	sprintf(outp, " %02X%02X%02X%02X len=%u",
+		rxpkt[23], rxpkt[22], rxpkt[21], rxpkt[20], rxpkt_len - 24);
+	outfunc_head(headline);
+	packet_hex_dump(rxpkt + 24, rxpkt_len - 24, outfunc_cont);
+}
+
+void
+format_gpf_packet(rxpkt, rxpkt_len, extra_prefix, outfunc_head, outfunc_cont)
+	u_char *rxpkt;
+	unsigned rxpkt_len;
+	char *extra_prefix;
+	void (*outfunc_head)(), (*outfunc_cont)();
+{
+	if (!basic_checks(rxpkt, rxpkt_len)) {
+		print_malformed(rxpkt, rxpkt_len, extra_prefix, outfunc_head,
+				outfunc_cont);
+		return;
+	}
+	/* dispatch by type */
+	switch (rxpkt[1] & 0x30) {
+	case 0x10:
+		/* PS primitive */
+		if (psprim_extra_checks(rxpkt, rxpkt_len))
+			format_psprim(rxpkt, rxpkt_len, extra_prefix,
+					outfunc_head, outfunc_cont);
+		else
+			print_malformed(rxpkt, rxpkt_len, extra_prefix,
+					outfunc_head, outfunc_cont);
+		return;
+	case 0x20:
+		/* trace */
+		format_trace(rxpkt, rxpkt_len, extra_prefix, outfunc_head,
+				outfunc_cont);
+		return;
+	case 0x30:
+		/* system primitive */
+		format_sysprim(rxpkt, rxpkt_len, extra_prefix, outfunc_head,
+				outfunc_cont);
+		return;
+	default:
+		print_malformed(rxpkt, rxpkt_len, extra_prefix, outfunc_head,
+				outfunc_cont);
+	}
+}