/* MSPDebug - debugging tool for MSP430 MCUs
 * Copyright (C) 2009-2013 Daniel Beer
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include <string.h>
#include "util.h"
#include "output.h"
#include "hal_proto.h"

void hal_proto_init(struct hal_proto *p, transport_t trans,
		    hal_proto_flags_t flags)
{
	memset(p, 0, sizeof(*p));

	p->trans = trans;
	p->flags = flags;
	p->ref_id = 0;
}

static int hal_proto_send_common(struct hal_proto *p, hal_proto_type_t type,
		uint8_t* buf, size_t len)
{
	if (len & 1)
		buf[len++] = 0;

	if (p->flags & HAL_PROTO_CHECKSUM) {
		size_t i;
		uint8_t sum_l = 0xff;
		uint8_t sum_h = 0xff;

		for (i = 0; i < len; i += 2) {
			sum_l ^= buf[i];
			sum_h ^= buf[i + 1];
		}

		buf[len++] = sum_l;
		buf[len++] = sum_h;
	}

	if (p->trans->ops->send(p->trans, buf, len) < 0) {
		printc_err("hal_proto_send_common: type: 0x%02x\n", type);
		return -1;
	}

	return 0;
}


int hal_proto_send(struct hal_proto *p, hal_proto_type_t type,
		   const uint8_t *data, int length)
{
	uint8_t buf[512];
	size_t len = 0;

	if (length > HAL_MAX_PAYLOAD) {
		printc_err("hal_proto_send: payload too long: %d\n", length);
		return -1;
	}

	buf[len++] = length + 3;
	buf[len++] = type;
	buf[len++] = p->ref_id;
	buf[len++] = 0;

	p->ref_id = (p->ref_id + 1) & 0x7f;

	memcpy(buf + len, data, length);
	len += length;

	return hal_proto_send_common(p, type, buf, len);
}


static int hal_proto_send_ack(struct hal_proto *p, hal_proto_type_t type,
		const uint8_t *data, int length)
{
	uint8_t buf[512];
	size_t len = 0;

	if (length > HAL_MAX_PAYLOAD) {
		printc_err("hal_proto_send_ack: payload too long: %d\n", length);
		return -1;
	}

	buf[len++] = length + 3;
	buf[len++] = type;
	buf[len++] = (p->ref_id - 1) & 0xff;
	buf[len++] = 0;

	/*p->ref_id = (p->ref_id + 1) & 0x7f;*/

	memcpy(buf + len, data, length);
	len += length;

	return hal_proto_send_common(p, type, buf, len);
}


int hal_proto_receive(struct hal_proto *p, uint8_t *buf, int max_len)
{
	uint8_t rx_buf[512];
	uint8_t sum_h = 0xff;
	uint8_t sum_l = 0xff;
	int rx_len = 0;
	int len;
	int i;

	for (;;) {
		int r = p->trans->ops->recv(p->trans, rx_buf + rx_len,
					    sizeof(rx_buf) - rx_len);

		if (r <= 0) {
			printc_err("hal_proto_recv: read error\n");
			return -1;
		}

		rx_len += r;

		if (rx_len) {
			const size_t expect_len =
				rx_buf[0] + 4 - (rx_buf[0] & 1);

			if (rx_len == expect_len)
				break;

			if (rx_len > expect_len) {
				printc_err("hal_proto_recv: length "
					   "mismatch\n");
				return -1;
			}
		}
	}

	if (rx_len < 6) {
		printc_err("hal_proto_recv: short read: %d\n", rx_len);
		return -1;
	}

	for (i = 0; i < rx_len; i += 2) {
		sum_h ^= rx_buf[i];
		sum_l ^= rx_buf[i + 1];
	}

	if (sum_h || sum_l) {
		printc_err("hal_proto_recv: bad checksum\n");
		return -1;
	}

	len = rx_buf[0] - 3;
	p->type = rx_buf[1];
	p->ref = rx_buf[2];
	p->seq = rx_buf[3];

	if (len > max_len) {
		printc_err("hal_proto_recv: reply too long\n");
		return -1;
	}

	memcpy(buf, rx_buf + 4, len);
	return len;
}

int hal_proto_execute(struct hal_proto *p, uint8_t fid,
		      const uint8_t *data, int len)
{
	uint8_t fdata[HAL_MAX_PAYLOAD];

	if (len + 2 > HAL_MAX_PAYLOAD) {
		printc_err("hal_proto_execute: fid 0x%02x: payload too big: %d\n",
				fid, len);
		return -1;
	}

	fdata[0] = fid;
	fdata[1] = 0;
	memcpy(fdata + 2, data, len);

	if (hal_proto_send(p, HAL_PROTO_TYPE_CMD_EXECUTE, fdata, len + 2) < 0)
		goto fail;

	p->length = 0;

	do {
		int r = hal_proto_receive(p, p->payload + p->length,
					  sizeof(p->payload) - p->length);

		if (r < 0)
			goto fail;

		if ((p->type == HAL_PROTO_TYPE_EXCEPTION) && (r >= 2)) {
			printc_err("hal_proto_execute: fid 0x%02x: HAL exception: 0x%04x\n",
				   fid, LE_WORD(p->payload, p->length));
			goto fail;
		}

		if (p->type == HAL_PROTO_TYPE_ACKNOWLEDGE)
			break;

		if (p->type != HAL_PROTO_TYPE_DATA) {
			printc_err("hal_proto_execute: fid 0x%02x: no data "
				   "(got type 0x%02x)\n", fid, p->type);
			goto fail;
		}

		if (hal_proto_send_ack(p, HAL_PROTO_TYPE_ACKNOWLEDGE, NULL, 0) < 0)
			goto fail;

		p->length += r;
	} while (p->ref & 0x80);

	return 0;

fail:
	printc_err("hal_proto_execute: fid: 0x%02x: failed\n", fid);
	return -1;
}