
#include <unistd.h>

#include "serial.h"
#include "monitor.h"

#include "boot_cmds.h"

int boot_address_set(int fd, unsigned int address)
{
	unsigned char buf[5];

	buf[0] = BOOT_CMD_ADDRESS_SET;
	buf[1] = (address >> 24) & 0xff;
	buf[2] = (address >> 16) & 0xff;
	buf[3] = (address >> 8) & 0xff;
	buf[4] = (address >> 0) & 0xff;

	if (serial_write(fd, buf, sizeof(buf)) != sizeof(buf)) {
		errorstr = " - unable to write to serial port";
		return -1;
	}

	if (serial_read(fd, buf, 1) != 1) {
		errorstr = " - unable to read from serial port";
		return -1;
	}

	if (buf[0] != (BOOT_CMD_ADDRESS_SET | 0x80)) {
  		errorstr = " - bad reply";
		return -1;
	}

	return 0;
}

int boot_read_n(int fd, int width, unsigned char *data)
{
	unsigned char buf[1];
	int cmd, k, n;

	switch(width) {
	case 8: cmd = BOOT_CMD_READ_8; n = 1; break;
	case 16: cmd = BOOT_CMD_READ_16; n = 2; break;
	case 32: cmd = BOOT_CMD_READ_32; n = 4; break;
	default: 
		errorstr = " - bad buswidth";
		return -1;
	}

	buf[0] = cmd;

	if (serial_write(fd, buf, sizeof(buf)) != sizeof(buf)) {
		errorstr = " - unable to write to serial port";
		return -1;
	}

	if (serial_read(fd, buf, 1) != 1) {
		errorstr = " - unable to read from serial port";
		return -1;
	}

	if (buf[0] != (cmd | 0x80)) {
  		errorstr = " - bad reply";
		return -1;
	}

	while (n > 0) {
		k = serial_read(fd, data, n);

		if (k <= 0) {
			errorstr = " - unable to read from serial port";
			return -1;
		}

		data += k;
		n -= k;
	}
	return 0;
}

int boot_write_n(int fd, int width, unsigned char *data)
{
	unsigned char buf[1];
	int cmd, n;

	switch(width) {
	case 8: cmd = BOOT_CMD_WRITE_8; n = 1; break;
	case 16: cmd = BOOT_CMD_WRITE_16; n = 2; break;
	case 32: cmd = BOOT_CMD_WRITE_32; n = 4; break;
	default: return -1;
	}

	buf[0] = cmd;

	if (serial_write(fd, buf, sizeof(buf)) != sizeof(buf)) {
		errorstr = " - unable to write to serial port";
		return -1;
	}

	if (serial_write(fd, data, n) != n) {
		errorstr = " - unable to write to serial port";
		return -1;
	}

	if (serial_read(fd, buf, 1) != 1) {
		errorstr = " - unable to read from serial port";
		return -1;
	}

	if (buf[0] != (cmd | 0x80)) {
  		errorstr = " - bad reply";
		return -1;
	}

	return 0;
}

int boot_run(int fd)
{
	unsigned char buf[1];

	buf[0] = BOOT_CMD_RUN;

	if (serial_write(fd, buf, sizeof(buf)) != sizeof(buf)) {
		errorstr = " - unable to write to serial port";
		return -1;
	}

	if (serial_read(fd, buf, 1) != 1) {
		errorstr = " - unable to read from serial port";
		return -1;
	}

	if (buf[0] != (BOOT_CMD_RUN | 0x80)) {
  		errorstr = " - bad reply";
		return -1;
	}

	return 0;
}

int boot_hello(int fd)
{
	unsigned char buf[1];

	buf[0] = BOOT_CMD_HELLO;

	if (serial_write(fd, buf, sizeof(buf)) != sizeof(buf)) {
		errorstr = " - unable to write to serial port";
		return -1;
	}

	if (serial_read(fd, buf, 1) != 1) {
		errorstr = " - unable to read from serial port";
		return -1;
	}

	if (buf[0] != (BOOT_CMD_HELLO | 0x80)) {
  		errorstr = " - bad reply";
		return -1;
	}

	return 0;
}



