view rvinterf/etmsync/interf.c @ 40:7ecb70b0ac36

c139explore: vibe command added that works like buz
author Mychaela Falconia <falcon@freecalypso.org>
date Wed, 26 Oct 2016 01:59:32 +0000
parents e7502631a0f9
children
line wrap: on
line source

/*
 * In this module we implement our synchronous interface to the target
 * via rvinterf.
 */

#include <sys/types.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "limits.h"
#include "localsock.h"
#include "pktmux.h"
#include "exitcodes.h"

extern int sock;

int rx_enable_state;
u_char rvi_msg[LOCALSOCK_MAX_MSG];
int rvi_msg_len;

static void
collect_bytes_from_rvi(buf, nbytes)
	u_char *buf;
{
	int cc;

	while (nbytes) {
		cc = read(sock, buf, nbytes);
		if (cc <= 0) {
			perror("read from rvinterf socket");
			exit(ERROR_RVINTERF);
		}
		buf += cc;
		nbytes -= cc;
	}
}

collect_rvi_msg()
{
	u_char lenbuf[2];

	collect_bytes_from_rvi(lenbuf, 2);
	rvi_msg_len = lenbuf[0] << 8 | lenbuf[1];
	if (rvi_msg_len < 1 || rvi_msg_len > LOCALSOCK_MAX_MSG) {
		fprintf(stderr, "Invalid length from rvinterf: %02X%02X\n",
			lenbuf[0], lenbuf[1]);
		exit(ERROR_RVINTERF);
	}
	collect_bytes_from_rvi(rvi_msg, rvi_msg_len);
	return(0);
}

send_rvimisc_command(cmdpkt, cmdlen)
	u_char *cmdpkt;
{
	u_char lenbuf[2];

	lenbuf[0] = 0;
	lenbuf[1] = cmdlen;
	write(sock, lenbuf, 2);
	write(sock, cmdpkt, cmdlen);
}

rx_control(enable)
{
	u_char cmdpkt[2];
	int cmdlen;

	/* are we already in the desired state? */
	if (rx_enable_state == enable)
		return(0);
	/* no, do the work */
	if (enable) {
		cmdpkt[0] = CLI2RVI_WANT_MUXPROTO;
		cmdpkt[1] = RVT_TM_HEADER;
		cmdlen = 2;
	} else {
		cmdpkt[0] = CLI2RVI_RESET_PACKET_RX;
		cmdlen = 1;
	}
	send_rvimisc_command(cmdpkt, cmdlen);
	collect_rvi_msg();
	if (rvi_msg[0] != RVI2CLI_LOCAL_CMD_RESP || rvi_msg_len < 2) {
		fprintf(stderr,
		"error: unexpected response to rvinterf local command\n");
		exit(ERROR_RVINTERF);
	}
	if (rvi_msg[1] != '+') {
		fprintf(stderr, "Error from rvinterf: %.*s\n", rvi_msg_len - 1,
			rvi_msg + 1);
		exit(ERROR_RVINTERF);
	}
	rx_enable_state = enable;
	return(0);
}

send_pkt_to_target(pkt, pktlen)
	u_char *pkt;
{
	u_char hdrbuf[3];
	int len1;

	len1 = pktlen + 1;
	hdrbuf[0] = len1 >> 8;
	hdrbuf[1] = len1 & 0xFF;
	hdrbuf[2] = CLI2RVI_PKT_TO_TARGET;
	write(sock, hdrbuf, 3);
	write(sock, pkt, pktlen);
}

target_pkt_exch(outpkt, outpktlen)
	u_char *outpkt;
{
	rx_control(1);
	send_pkt_to_target(outpkt, outpktlen);
	collect_rvi_msg();
	if (rvi_msg[0] != RVI2CLI_PKT_FROM_TARGET) {
		fprintf(stderr,
			"error: unexpected response type from rvinterf\n");
		exit(ERROR_RVINTERF);
	}
	return(0);
}

etm_pkt_exch(outbuf, outlen)
	u_char *outbuf;
{
	int i, c;

	outbuf[0] = RVT_TM_HEADER;
	c = 0;
	for (i = 1; i <= outlen; i++)
		c ^= outbuf[i];
	outbuf[i] = c;
	target_pkt_exch(outbuf, outlen + 2);
	if (rvi_msg[1] != RVT_TM_HEADER) {
		printf("error: packet from target is not ETM!\n");
		return(ERROR_TARGET);
	}
	if (rvi_msg_len < 5) {
		printf("error: ETM response packet is too short\n");
		return(ERROR_TARGET);
	}
	c = 0;
	for (i = 2; i < rvi_msg_len; i++)
		c ^= rvi_msg[i];
	if (c) {
		printf("ETM response checksum error!\n");
		return(ERROR_TARGET);
	}
	if (rvi_msg[2] != outbuf[1]) {
		printf("error: target response is from wrong ETM component\n");
		return(ERROR_TARGET);
	}
	return(0);
}