view rvinterf/libg23/fmtdispatch.c @ 823:d93ecff8544b

aci: psa_mms.c imported from TCS211 version
author Space Falcon <falcon@ivan.Harhan.ORG>
date Sun, 05 Apr 2015 20:45:18 +0000
parents 42c91c51ca7f
children ca65f5adf1af
line wrap: on
line source

/*
 * 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, "GPF 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);
	}
}