--- a/rvinterf/lowlevel/tfc139.c	Thu May 15 10:32:30 2014 +0000
+++ b/rvinterf/lowlevel/tfc139.c	Thu May 15 11:09:45 2014 +0000
@@ -50,6 +50,8 @@
 static unsigned iram_load_addr = 0x800000;
 static unsigned stack_smash_addr = 0x837C54;
 
+static u_char stack_smash_payload[4];
+
 static void
 send_compal_memwrite(addr, payload, payload_len)
 	unsigned addr;
@@ -81,11 +83,14 @@
 	int c;
 	fd_set fds;
 
-	while ((c = getopt(argc, argv, "l:")) != EOF)
+	while ((c = getopt(argc, argv, "l:w:")) != EOF)
 		switch (c) {
 		case 'l':
 			logfname = optarg;
 			continue;
+		case 'w':
+			wakeup_after_sec = strtoul(optarg, 0, 0);
+			continue;
 		case '?':
 		default:
 usage:			fprintf(stderr,
@@ -110,6 +115,10 @@
 	}
 	output_line("Sending IRAM payload");
 	send_compal_memwrite(iram_load_addr, iram_payload, sizeof iram_payload);
+	stack_smash_payload[0] = iram_load_addr;
+	stack_smash_payload[1] = iram_load_addr >> 8;
+	stack_smash_payload[2] = iram_load_addr >> 16;
+	stack_smash_payload[3] = iram_load_addr >> 24;
 	for (;;) {
 		FD_ZERO(&fds);
 		FD_SET(target_fd, &fds);
@@ -126,8 +135,27 @@
 	}
 }
 
+static void
+handle_etm_response()
+{
+	char msgbuf[80];
+
+	if (rxpkt_len != 4 || rxpkt[1] != 0x40 || rxpkt[2] || rxpkt[3] != 0x40){
+		output_line("ETM 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;
+}
+
 handle_rx_packet()
 {
+	if (rxpkt_len == 2 && rxpkt[0] == 'O' && rxpkt[1] == 'K') {
+		output_line("Success! Run fc-loadtool now!");
+		return;
+	}
 	switch (rxpkt[0]) {
 	case RVT_RV_HEADER:
 		if (rxpkt_len < 6)
@@ -142,6 +170,7 @@
 		return;
 	case RVT_TM_HEADER:
 		print_etm_output_raw();
+		handle_etm_response();
 		return;
 	default:
 	unknown: