#include <sys/types.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <sys/errno.h>
#include <ctype.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <strings.h>
#include <termios.h>
#include <unistd.h>
#include "srecreader.h"

extern int errno;

extern char *target_ttydev;
extern int target_fd;
extern struct srecreader srimage;

#define	MAX_IMAGE_LEN	32768

static u_char codeimage[MAX_IMAGE_LEN];
static unsigned codeimage_len;
static uint32_t loadaddr;

static u_char beacon_cmd[3] = {0xAA, 0x01, 0xDD};
static u_char fluid_baudrate_cmd[2] = {0x00, 0x07};
static u_char fluid_version_cmd[1] = {'V'};
static u_char fluid_chipquery_cmd[2] = {'Q', 'C'};
static u_char fluid_download_cmd[7] = {'L'};

#define	BEACON_INTERVAL		13	/* ms between beacons */
#define	INTERMEDIATE_TIMEOUT	500	/* ms to wait for responses */
#define	SERIAL_FLUSH_DELAY	200	/* also in ms */

read_srec_image()
{
	u_char *writep;
	uint32_t endaddr;
	int i;

	if (open_srec_file(&srimage) < 0)
		exit(1);
	for (;;) {
		if (read_s_record(&srimage) < 0)
			exit(1);
		switch (srimage.record_type) {
		case '0':
			if (srimage.lineno == 1)
				continue;
			fprintf(stderr,
		"%s: S0 record found in line %d (expected in line 1 only)\n",
				srimage.filename, srimage.lineno);
			exit(1);
		case '3':
		case '7':
			if (s3s7_get_addr_data(&srimage) < 0)
				exit(1);
			break;
		default:
			fprintf(stderr,
				"%s line %d: S%c record type not supported\n",
				srimage.filename, srimage.lineno,
				srimage.record_type);
			exit(1);
		}
		if (srimage.record_type == '7')
			break;
		/* must be S3 */
		if (srimage.datalen < 1) {
			fprintf(stderr,
				"%s line %d: S3 record has zero data length\n",
				srimage.filename, srimage.lineno);
			exit(1);
		}
		if (srimage.datalen & 1) {
			fprintf(stderr,
				"%s line %d: S3 record has odd data length\n",
				srimage.filename, srimage.lineno);
			exit(1);
		}
		if (srimage.addr & 1) {
			fprintf(stderr,
				"%s line %d: S3 record has odd address\n",
				srimage.filename, srimage.lineno);
			exit(1);
		}
		/* handle first record */
		if (!codeimage_len) {
			endaddr = loadaddr = srimage.addr;
			writep = codeimage;
		}
		if (srimage.addr != endaddr) {
			fprintf(stderr, "%s line %d: address discontinuity\n",
				srimage.filename, srimage.lineno);
			exit(1);
		}
		if (codeimage_len + srimage.datalen > MAX_IMAGE_LEN) {
			fprintf(stderr,
				"%s line %d: max image length exceeded\n",
				srimage.filename, srimage.lineno);
			exit(1);
		}
		/* reverse byte order */
		for (i = 0; i < srimage.datalen; i += 2) {
			*writep++ = srimage.record[i+6];
			*writep++ = srimage.record[i+5];
		}
		endaddr += srimage.datalen;
		codeimage_len += srimage.datalen;
	}
	/* got S7 */
	fclose(srimage.openfile);
	if (!codeimage_len) {
		fprintf(stderr,
		"%s line %d: S7 without any preceding S3 data records\n",
			srimage.filename, srimage.lineno);
		exit(1);
	}
	if (srimage.addr != loadaddr) {
		fprintf(stderr,
		"%s line %d: S7 address differs from image load address\n",
			srimage.filename, srimage.lineno);
		exit(1);
	}
	/* all good */
	return(0);
}

static void
fill_download_cmd()
{
	unsigned nwords;

	nwords = codeimage_len >> 1;
	fluid_download_cmd[1] = loadaddr;
	fluid_download_cmd[2] = loadaddr >> 8;
	fluid_download_cmd[3] = loadaddr >> 16;
	fluid_download_cmd[4] = loadaddr >> 24;
	fluid_download_cmd[5] = nwords;
	fluid_download_cmd[6] = nwords >> 8;
}

static int
expect_beacon_response(timeout)
{
	u_char buf;
	fd_set fds;
	struct timeval tv;
	int cc;

	for (;;) {
		FD_ZERO(&fds);
		FD_SET(target_fd, &fds);
		tv.tv_sec = 0;
		tv.tv_usec = timeout * 1000;
		cc = select(target_fd+1, &fds, NULL, NULL, &tv);
		if (cc < 0) {
			if (errno == EINTR)
				continue;
			perror("select");
			exit(1);
		}
		if (cc < 1)
			return(0);
		cc = read(target_fd, &buf, 1);
		if (cc <= 0) {
			perror("read after successful select");
			exit(1);
		}
		if (buf == 'H')
			return(1);
	}
}

static
send_beacons()
{
	printf("Sending beacons to %s\n", target_ttydev);
	do
		write(target_fd, beacon_cmd, sizeof beacon_cmd);
	while (!expect_beacon_response(BEACON_INTERVAL));
	return 0;
}

static int
expect_add_response(buf, expect_bytes, timeout)
	u_char *buf;
{
	fd_set fds;
	struct timeval tv;
	int pass, cc;

	for (pass = 0; pass < expect_bytes; ) {
		FD_ZERO(&fds);
		FD_SET(target_fd, &fds);
		tv.tv_sec = 0;
		tv.tv_usec = timeout * 1000;
		cc = select(target_fd+1, &fds, NULL, NULL, &tv);
		if (cc < 0) {
			if (errno == EINTR)
				continue;
			perror("select");
			exit(1);
		}
		if (cc < 1)
			return(-1);
		cc = read(target_fd, buf + pass, expect_bytes - pass);
		if (cc <= 0) {
			perror("read after successful select");
			exit(1);
		}
		pass += cc;
	}
	return(0);
}

perform_frbl2_protocol()
{
	u_char respbuf[4];
	static int zero = 0;

	ioctl(target_fd, FIONBIO, &zero);
	send_beacons();
	printf("Got Hello response, attempting FLUID protocol\n");
	write(target_fd, fluid_baudrate_cmd, sizeof fluid_baudrate_cmd);
	usleep(SERIAL_FLUSH_DELAY * 1000);
	tcflush(target_fd, TCIFLUSH);
	write(target_fd, fluid_version_cmd, sizeof fluid_version_cmd);
	if (expect_add_response(respbuf, 1, INTERMEDIATE_TIMEOUT) < 0) {
		fprintf(stderr, "no response to version query\n");
		exit(1);
	}
	if (!isdigit(respbuf[0])) {
		fprintf(stderr, "bad response to version query: 0x%02X\n",
			respbuf[0]);
		exit(1);
	}
	printf("Version response: %d\n", respbuf[0] - '0');
	write(target_fd, fluid_chipquery_cmd, sizeof fluid_chipquery_cmd);
	if (expect_add_response(respbuf, 4, INTERMEDIATE_TIMEOUT) < 0) {
		fprintf(stderr, "no response to chip ID query\n");
		exit(1);
	}
	if (respbuf[2] || respbuf[3]) {
		fprintf(stderr,
			"bad response to chip ID query: %02X %02X %02X %02X\n",
			respbuf[0], respbuf[1], respbuf[2], respbuf[3]);
		exit(1);
	}
	printf("Chip ID response: %04X\n", (respbuf[1] << 8) | respbuf[0]);
	printf("Sending download command\n");
	write(target_fd, fluid_download_cmd, sizeof fluid_download_cmd);
	if (expect_add_response(respbuf, 1, INTERMEDIATE_TIMEOUT) < 0) {
		fprintf(stderr, "no response to download command\n");
		exit(1);
	}
	if (respbuf[0] != 'R') {
		fprintf(stderr, "bad response to download command: 0x%02X\n",
			respbuf[0]);
		exit(1);
	}
	if (write(target_fd, codeimage, codeimage_len) != codeimage_len) {
		perror("write of download image bulk");
		exit(1);
	}
	printf("Code image sent!\n");
}

frbl_test_main()
{
	read_srec_image();
	fill_download_cmd();
	perform_frbl2_protocol();
}