--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rvinterf/libg23/fmtdispatch.c	Tue Apr 22 05:03:39 2014 +0000
@@ -0,0 +1,103 @@
+/*
+ * This libg23 module exports the format_g23_packet() function, which
+ * validates the packet, then dispatches it to format_g23_trace(),
+ * format_g23_sysprim() or format_g23_psprim() as appropriate, or
+ * prints it in raw hex if malformed.
+ */
+
+#include <sys/types.h>
+#include <stdio.h>
+#include <string.h>
+#include <strings.h>
+
+static int
+basic_checks(rxpkt, rxpkt_len)
+	u_char *rxpkt;
+{
+	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;
+{
+	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, outbuf)
+	u_char *rxpkt;
+	char *outbuf;
+{
+	int i;
+	char *dp;
+
+	dp = outbuf;
+	strcpy(dp, "G23 UNK:");
+	dp += 8;
+	for (i = 1; i < rxpkt_len; i++) {
+		sprintf(dp, " %02X", rxpkt[i]);
+		dp += 3;
+	}
+	*dp = '\0';
+}
+
+void
+format_g23_packet(rxpkt, rxpkt_len, outbuf)
+	u_char *rxpkt;
+	char *outbuf;
+{
+	if (!basic_checks(rxpkt, rxpkt_len)) {
+		print_malformed(rxpkt, rxpkt_len, outbuf);
+		return;
+	}
+	/* dispatch by type */
+	switch (rxpkt[1] & 0x30) {
+	case 0x10:
+		/* PS primitive */
+		if (psprim_extra_checks(rxpkt, rxpkt_len))
+			format_g23_psprim(rxpkt, rxpkt_len, outbuf);
+		else
+			print_malformed(rxpkt, rxpkt_len, outbuf);
+		return;
+	case 0x20:
+		/* trace */
+		format_g23_trace(rxpkt, rxpkt_len, outbuf);
+		return;
+	case 0x30:
+		/* system primitive */
+		format_g23_sysprim(rxpkt, rxpkt_len, outbuf);
+		return;
+	default:
+		print_malformed(rxpkt, rxpkt_len, outbuf);
+	}
+}