// host_service_emulation.c
// HPI host service emulation for 6713/6416 DSK with HPI daughtercard

#include <c6x.h>
#include "c6x11dsk.h"
#include "hpi_services.h"
#include "hpi_services_funcs.h"

#include <stdio.h>
#include <string.h>

// DSP host API emulation
// only supports read and write packets

extern HPI_SVC_BLOCK HpiSvcBlock;

// packe protocol
#define PKT_MAX_SIZE			  256
#define PKT_PACKET_HEADER_FLAG    0xFA
#define PKT_HEADER_OFFSET         0
#define PKT_UNIT_ID_OFFSET        1
#define PKT_LENGTH_OFFSET         2
#define PKT_COMMAND_OFFSET        3
#define PKT_PAYLOAD_OFFSET        4

#define PKT_COMMAND_MEM_WR        1
#define PKT_COMMAND_MEM_RD        2
#define PKT_COMMAND_RESPONSE      0xFE

#define PKT_ID_MASTER             0
#define PKT_ID_ALL_SLAVE          0xFF

#define SERIAL_TIMEOUT_INTERVAL   250

#pragma DATA_ALIGN (PacketBuffer , 4); // ensure word alignment
static unsigned char PacketBuffer[PKT_MAX_SIZE];

void ProcessPacket_MemWr(unsigned char *pPayload)
{
	unsigned char length, i, *p8;
	unsigned int *p32, temp32;
	
	p32 = (unsigned int *)(*(unsigned int *)pPayload); 	// get address (uint32)
	pPayload += 4;
	length = *pPayload;						// get length (uint8)
	pPayload++;

	for(i = 0;i < length;i++) { 			// do iterations
		p8 = (unsigned char *)&temp32;		// buffer data is not word aligned!
		*p8++ = *pPayload++;
		*p8++ = *pPayload++;
		*p8++ = *pPayload++;
		*p8++ = *pPayload++;
		*p32++ = temp32;					// write to memory
	}
}

unsigned char ProcessPacket_MemRd(unsigned char *pPayload)
{
	unsigned char length, i;
	unsigned int *p32, *pbuf;
		
	pbuf = (unsigned int *)pPayload; 		// point to first payload location (word aligned)
	p32 = (unsigned int *)(*pbuf); 							// get address (uint32)
	length = *(pPayload + 4); 				// get length (uint8)

	for(i = 0;i <length;i++) { 				// length-1 iterations
		*pbuf++ = *p32++;
	}
	
	return length << 2; 					// length in bytes
}

unsigned int CreatePacket(unsigned int command, unsigned int payload_bytes)
{
	unsigned char i, sum1, sum2;
	unsigned char error_check0, error_check1;

	PacketBuffer[PKT_HEADER_OFFSET] = PKT_PACKET_HEADER_FLAG;
	PacketBuffer[PKT_UNIT_ID_OFFSET] = PKT_ID_MASTER;
	PacketBuffer[PKT_LENGTH_OFFSET] = payload_bytes + 3; // plus command & sums
	PacketBuffer[PKT_COMMAND_OFFSET] = command;
	sum1 = 0;
	sum2 = 0;
	for(i = 0;i <= PacketBuffer[PKT_LENGTH_OFFSET];i++) {
		sum1 += PacketBuffer[i];
		sum2 += sum1;
	}
	error_check0 = 0 - (sum1 + sum2);
	error_check1 = 0 - (sum1 + error_check0);
	PacketBuffer[payload_bytes + PKT_PAYLOAD_OFFSET] = error_check0;
	PacketBuffer[payload_bytes + PKT_PAYLOAD_OFFSET + 1] = error_check1;

	return payload_bytes + 6;
}

void DoHostEmulation(int UseUSB)
{
	static unsigned short last_flag = 0;
	static unsigned char SerialTimeoutCounter = 0, byte_count = 0, RxPacketPayloadCounter = 0;
	static unsigned char data_buffer[PKT_MAX_SIZE];
	unsigned char bytes_xfrd, *pbuf, c, PacketRcvdFlag = 0;
	unsigned char num_bytes, i, sum1, sum2, packet_size;
	
	if(last_flag == HpiSvcBlock.flag) { // has there been an update?
		return;							// if not, nothing to do
	}

	last_flag = HpiSvcBlock.flag;		// update flag
	if(SerialTimeoutCounter) {			// update timeout counter
		SerialTimeoutCounter--;
	}

	// get any new data
	if(UseUSB)
		bytes_xfrd = ReadUsb((char *)data_buffer, PKT_MAX_SIZE);
	else
		bytes_xfrd = ReadSerial((char *)data_buffer, PKT_MAX_SIZE);


	//process incoming data
	pbuf = data_buffer;						// init pointer

	while(bytes_xfrd--) { 					// go until buffer is empty
		c = *pbuf++; 						// get data from buffer

		// timeout within a packet
		if((byte_count != 0) && (SerialTimeoutCounter == 0)) {
			byte_count = 0;
		}
		PacketBuffer[byte_count] = c; 		// store byte

		switch(byte_count) {
		case 0: // start of packet
			if(c != PKT_PACKET_HEADER_FLAG)
				break;
			SerialTimeoutCounter = SERIAL_TIMEOUT_INTERVAL;	// restart timeout counter
			byte_count++;
			break;
		case 1: // unit id
			byte_count++;
			break;
		case 2: // length
			RxPacketPayloadCounter = c; // include checksum bytes
			byte_count++;
			break;
		default: // in command / payload / error check sections
			byte_count++;
			RxPacketPayloadCounter--;
			if(RxPacketPayloadCounter == 0) { // packet all rcvd
				PacketRcvdFlag = 1;
				byte_count = 0;
			}
			break;
		}
	}

	if(PacketRcvdFlag) {					// is there a complete packet yet?
		sum1 = 	sum2 = 0;
		num_bytes = PacketBuffer[PKT_LENGTH_OFFSET];
		for(i = 0;i <= num_bytes + 2;i++) {
			sum1 += PacketBuffer[i];
			sum2 += sum1;
		}

		if((sum1 == 0) && (sum2 == 0)) { // checksum is valid
			// packet is valid, so process
			switch(PacketBuffer[PKT_COMMAND_OFFSET]) {
			case PKT_COMMAND_MEM_WR:
				ProcessPacket_MemWr(&PacketBuffer[PKT_PAYLOAD_OFFSET]);
				packet_size = CreatePacket(PKT_COMMAND_RESPONSE, 0);
				break;
			case PKT_COMMAND_MEM_RD:
				num_bytes = ProcessPacket_MemRd(&PacketBuffer[PKT_PAYLOAD_OFFSET]);
				packet_size = CreatePacket(PKT_COMMAND_RESPONSE, num_bytes);
				break;
			default:
				packet_size = 0;
				break;
			}

			// send response packet
			if(UseUSB)
				bytes_xfrd = WriteUsb((char *)PacketBuffer, packet_size);
			else
				bytes_xfrd = WriteSerial((char *)PacketBuffer, packet_size);
				
			// need to verify that whole packet was sent
		}
	}
}

void DoHostEmulationUsb()
{
	DoHostEmulation(1);
}

void DoHostEmulationSerial()
{
	DoHostEmulation(0);
}