
#include <stdio.h>
#include <unistd.h>
#include <errno.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#include <termios.h>
#include <string.h>

#include "boot_cmds.h"
#include "srec.h"

#define BYTES_PER_ROW 16

char *errorstr;

void monitor(int fd)
{
	int k, n, is_repeat;
	unsigned char buf[80], ucb[4], lastcmd;
	unsigned int address, length, data;
	int width = 8, offset, bad;

	lastcmd = 0;
	offset = 0;

	while (1) {

		write(1, "ufpb_tool> ", 11);

		n = read(0, buf, sizeof(buf));

		if (n < 0) {
			break;
		}

		buf[n] = '\0';
		is_repeat = 0;
		bad = 0;
		errorstr = "";

		address = 0;
		length = 0;

		if (strncmp(buf, "d ", 2) == 0) {
			offset = 2;
		}
		else if (strncmp(buf, "d.8 ", 4) == 0) {
			width = 8;
			offset = 4;
		}
		else if (strncmp(buf, "d.16 ", 5) == 0) {
			width = 16;
			offset = 5;
		}
		else if (strncmp(buf, "d.32 ", 5) == 0) {
			width = 32;
			offset = 5;
		}
		else if (strncmp(buf, "m ", 2) == 0) {
			offset = 2;
		}
		else if (strncmp(buf, "m.8 ", 4) == 0) {
			width = 8;
			offset = 4;
		}
		else if (strncmp(buf, "m.16 ", 5) == 0) {
			width = 16;
			offset = 5;
		}
		else if (strncmp(buf, "m.32 ", 5) == 0) {
			width = 32;
			offset = 5;
		}
		else if (strncmp(buf, "l ", 2) == 0) {
			offset = 2;
		}
		else if (strncmp(buf, "v ", 2) == 0) {
			offset = 2;
		}
		else if (strncmp(buf, "r ", 2) == 0) {
			offset = 2;
		}
		else if (strncmp(buf, "h", 1) == 0) {
			offset = 0;
		}
		else {
			if ((n == 1) && (buf[0] == '\n')) {
				buf[0] = lastcmd;
				is_repeat = 1;
			}
			else {
				errorstr = " - command not supported";
				bad = 1;
			}
		}

		if (buf[0] == 'd') {
			lastcmd = 'd';
			if (!is_repeat) {
				length = 1;
				sscanf(buf + offset, "%x %x",&address,&length);
			}
			{
				int start = address & (BYTES_PER_ROW-1);
				unsigned int left = BYTES_PER_ROW - start;
				unsigned int len = length;
				unsigned int addr = address;
				int ret;

				boot_address_set(fd, addr);
				
				addr = addr - start;

				/* print header with address info */

				printf("   address: ");
				for (k = 0; k < BYTES_PER_ROW; k++) {
					switch (width) {
					case 8:
						printf("%02x ", k);
						break;
					case 16:
						printf("%04x ", k);
						k++;
						break;
					case 32:
						printf("%08x ", k);
						k += 3;
						break;
					}
				}
				printf("\n-----------");
				for (k = 0; k < BYTES_PER_ROW; k++) {
					printf("---");
				}
				printf("\n");

				/* display the rest */

				while (len) {

					if (left > len) {
						left = len;
					}

					printf("0x%08x: ", addr);

					for (k = 0; 
					     k < (start * ((width/8) + 2));
					     k++) {
						printf(" ");
					}

					k = start;

					while(1) {

						ret = boot_read_n(fd, width, 
								  ucb);

						if (ret < 0) {
							bad = 1;
							left = 0;
							len = 0;
							address -= length;
							printf("\n");
							break;
						}

						switch (width) {
						case 8:
							printf("%02x ",
							       ucb[0]);
							break;

						case 16:
							printf("%04x ",
							       (ucb[0] << 8) +
								ucb[1]);
							break;

						case 32:
							printf("%08x ",
							       (ucb[0] << 24) +
							       (ucb[1] << 16) +
							       (ucb[2] << 8) +
								ucb[3]);
							break;
						}

						k += (width / 8);

						if (k >= (left + start)) {
							printf("\n");
							break;
						}
					}

					len -= left;
					addr += BYTES_PER_ROW;
					start = 0;
					left = BYTES_PER_ROW;
				}
				address += length;
			}
		}
		if (buf[0] == 'm') {
			lastcmd = 'm';
			if (!is_repeat) {
				length = 1;
				sscanf(buf + offset, "%x %x",&address,&data);
			}
			switch (width) {
			case 8:	
				ucb[0] = data; 
				break;

			case 16: 
				ucb[0] = data >> 8; 
				ucb[1] = data & 0xff;
				break;
			case 32: 
				ucb[0] = (data >> 24) & 0xff; 
				ucb[1] = (data >> 16) & 0xff; 
				ucb[2] = (data >> 8) & 0xff; 
				ucb[3] = data & 0xff;
				break;
			}

			while (1) {
				if (boot_address_set(fd, address) < 0) {
					bad = 1;
					break;
				}

				if (boot_write_n(fd, width, ucb) < 0) {
					bad = 1;
					break;
				}

				address += width/8;
				break;
			}
		}
		if (buf[0] == 'l') {
			char *file;
			int file_fd;

			lastcmd = 'l';

			file = buf + offset;

			if (file[0] != '\0') {
				
				for (k = 0; k < strlen(file); k++) {
					if (file[k] == '\n') {
						file[k] = '\0';
					}
				}

				file_fd = open(file, O_RDONLY);

				if (file_fd < 0) {
					perror("open");
					bad = 1;
					errorstr = NULL;
				}
				else {
					srec_parse(file_fd, fd, 0);
					close(file_fd);
				}
			}
		}
		if (buf[0] == 'v') {
			char *file;
			int file_fd;

			lastcmd = 'v';

			file = buf + offset;

			if (file[0] != '\0') {
				
				for (k = 0; k < strlen(file); k++) {
					if (file[k] == '\n') {
						file[k] = '\0';
					}
				}

				file_fd = open(file, O_RDONLY);

				if (file_fd < 0) {
					perror("open");
					bad = 1;
					errorstr = NULL;
				}
				else {
					srec_parse(file_fd, fd, 1);
					close(file_fd);
				}
			}
		}
		if (buf[0] == 'r') {
			lastcmd = 'r';

			sscanf(buf + offset, "%x", &address);

			printf("run at address 0x%08x\n", address);

			bad = 1;

			if (boot_address_set(fd, address) == 0) {
				if (boot_run(fd) == 0) {
					unsigned char ch;

					bad = 0;

					while(1) {
						if(read(fd, &ch, 1) == 1) {
							printf("%c", ch);
							fflush(stdout);
						}
					}
				}
			}
		}

		if (buf[0] == 'h') {
			lastcmd = 'h';
			printf("help:\n");
			printf("h - help\n");
			printf("d - display memory on target\n");
			printf("m - modify memory on target\n");
			printf("l - load s-record file to target\n");
			printf("v - verify s-record file to target\n");
			printf("r - run\n");
		}

		if (bad) {
			if (errorstr) {
				printf("error: %s.\n", errorstr);
			}
		}
		else {
			printf("ok.\n");
		}
	}
}
