// Copyright (c) 2014-2017, The Lemon Man, All rights reserved. LGPLv3
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "kd.h"

#define KD_DBG if(false)

uint32_t kd_data_checksum(const uint8_t *buf, const uint64_t buf_len) {
	uint32_t i, acc;

	if (!buf || !buf_len) {
		return 0;
	}

	for (i = acc = 0; i < buf_len; i++) {
		acc += buf[i];
	}

	return acc;
}

int kd_send_ctrl_packet(io_desc_t *desc, const uint32_t type, const uint32_t id) {
	kd_packet_t pkt;

	pkt.leader = KD_PACKET_CTRL;
	pkt.length = 0;
	pkt.checksum = 0;
	pkt.id = id;
	pkt.type = type;

	if (iob_write (desc, (uint8_t *)&pkt, sizeof (kd_packet_t)) < 0) {
		return KD_E_IOERR;
	}

	return KD_E_OK;
}

int kd_send_data_packet(io_desc_t *desc, const uint32_t type, const uint32_t id, const uint8_t *req, const int req_len, const uint8_t *buf, const uint32_t buf_len) {
	kd_packet_t pkt;

	if (req_len + buf_len > KD_MAX_PAYLOAD) {
		return KD_E_MALFORMED;
	}

	// kd_req_t *r = (kd_req_t*) req;
	// R_LOG_DEBUG ("==== Send ==== %08x", r->req);

	pkt.leader = KD_PACKET_DATA;
	pkt.length = req_len + buf_len;
	pkt.checksum = kd_data_checksum (req, req_len) + kd_data_checksum (buf, buf_len);
	pkt.id = id;
	pkt.type = type;

	if (iob_write (desc, (uint8_t *)&pkt, sizeof (kd_packet_t)) < 0) {
		return KD_E_IOERR;
	}

	if (iob_write (desc, (uint8_t *)req, req_len) < 0) {
		return KD_E_IOERR;
	}

	if (buf && iob_write (desc, (uint8_t *)buf, buf_len) < 0) {
		return KD_E_IOERR;
	}

	if (desc->iob->type == KD_IO_PIPE) {
		if (iob_write (desc, (uint8_t *)"\xAA", 1) < 0) {
			return KD_E_IOERR;
		}
	}

	return KD_E_OK;
}

int kd_read_packet(io_desc_t *desc, kd_packet_t **p) {
	kd_packet_t pkt;
	uint8_t *buf;

	*p = NULL;

	if (iob_read (desc, (uint8_t *)&pkt, sizeof (kd_packet_t)) <= 0) {
		return KD_E_IOERR;
	}

	if (!kd_packet_is_valid (&pkt)) {
		KD_DBG R_LOG_DEBUG ("invalid leader %08x, trying to recover", pkt.leader);
		kd_send_ctrl_packet (desc, KD_PACKET_TYPE_RESEND, 0);
		// Slide a 4-byte window byte-by-byte until a valid leader appears.
		// Capped to KD_MAX_PAYLOAD bytes so a hostile peer can't make us spin forever.
		int scanned;
		for (scanned = 0; scanned < KD_MAX_PAYLOAD; scanned++) {
			ut8 b;
			if (iob_read (desc, &b, 1) <= 0) {
				return KD_E_IOERR;
			}
			pkt.leader = (pkt.leader >> 8) | ((ut32)b << 24);
			if (kd_packet_is_valid (&pkt)) {
				break;
			}
		}
		if (!kd_packet_is_valid (&pkt)) {
			return KD_E_MALFORMED;
		}
		if (iob_read (desc, (uint8_t *)&pkt + 4, sizeof (kd_packet_t) - 4) <= 0) {
			return KD_E_IOERR;
		}
	}

	if (pkt.length > KD_MAX_PAYLOAD) {
		KD_DBG R_LOG_DEBUG ("packet length %u exceeds KD_MAX_PAYLOAD", pkt.length);
		return KD_E_MALFORMED;
	}

	buf = malloc (sizeof (kd_packet_t) + pkt.length);
	if (!buf) {
		return KD_E_IOERR;
	}
	memcpy (buf, &pkt, sizeof (kd_packet_t));

	if (pkt.length) {
		if (iob_read (desc, buf + sizeof (kd_packet_t), pkt.length) != pkt.length) {
			free (buf);
			return KD_E_IOERR;
		}
	}

	if (pkt.checksum != kd_data_checksum (buf + sizeof (kd_packet_t), pkt.length)) {
		KD_DBG R_LOG_DEBUG ("Checksum mismatch");
		free (buf);
		return KD_E_MALFORMED;
	}

	if (pkt.leader == KD_PACKET_DATA) {
		if (desc->iob->type == KD_IO_PIPE) {
			uint8_t trailer;
			iob_read (desc, (uint8_t *)&trailer, 1);

			if (trailer != 0xAA) {
				KD_DBG R_LOG_DEBUG ("Missing trailer 0xAA");
				free (buf);
				return KD_E_MALFORMED;
			}
		}
		kd_send_ctrl_packet (desc, KD_PACKET_TYPE_ACKNOWLEDGE, ((kd_packet_t *)buf)->id & ~ (0x800));
	}

	*p = (kd_packet_t *)buf;

	return KD_E_OK;
}

bool kd_packet_is_valid(const kd_packet_t *p) {
	return p->leader == KD_PACKET_CTRL || p->leader == KD_PACKET_DATA || p->leader == KD_PACKET_UNUSED;
}

int kd_packet_is_ack(const kd_packet_t *p) {
	return p->leader == KD_PACKET_CTRL && p->type == KD_PACKET_TYPE_ACKNOWLEDGE;
}