/*
 * commandhandler.c
 *
 *  Date: Mar 5, 2015
 *  Author: adowd
 *  Description:  This handled a complete UDP packet of information and returns a response
 */
#include <stdio.h>
#include <string.h>

#include "lwip/err.h"
#include "lwip/tcp.h"
#ifdef __arm__
#include "xil_printf.h"
#endif

// Only accept read/write values within these ranges (inclusive)
const u32_t valid_addr_range[2] = {0x44A10000,0x44A1FFFF};
char response[128];
u32_t uaddr;  //  Address to access data.

const char resp_ok[] = "OK\n";
//const char resp_err[] = "!Err\n";
const char ws_chars[] = " ,\t";

char *cmdEAddr(char * c);
int cmdWWrite(char *);
int cmdRRead(char *);
int cmdPPoke(char *);
int cmdKPeek(char *);

// Returns number of characters in response (or null if error)
u16_t command_handler(char *cptr,char **rptrptr)
{
	char *dptr;
	int   ntransactions;

	switch(cptr[0]) {
	case 'w': // Write
	case 'W':
		if( (dptr = cmdEAddr(cptr+1)) == NULL) return 0;
		ntransactions = cmdWWrite(dptr);
		if(ntransactions != 0) {
			*rptrptr = resp_ok;
			return sizeof(resp_ok)-1;
		}
		break;
	case 'r': // Read
	case 'R':
		if( (dptr = cmdEAddr(cptr+1)) == NULL) return 0;
		ntransactions = cmdRRead(dptr);
		if(ntransactions != 0) {
			*rptrptr = response;
			return strlen(response);
		}
		break;
	case 'p': // Poke
	case 'P':
		if( (dptr = cmdEAddr(cptr+1)) == NULL) return 0;
		ntransactions = cmdPPoke(dptr);
		if(ntransactions != 0) {
			*rptrptr = resp_ok;
			return sizeof(resp_ok)-1;
		}
		break;
	case 'k': // Peek
	case 'K':
		if( (dptr = cmdEAddr(cptr+1)) == NULL) return 0;
		ntransactions = cmdKPeek(dptr);
		if(ntransactions != 0) {
			*rptrptr = response;
			return strlen(response);
		}
		break;
	}
	return 0;
}
//---------------------------------------------------------------------------
// Extract Address from the command string
//   returns pointer to the start of data, if address is valid
//   returns NULL if address is invalid
//
char *cmdEAddr(char * c)
{
	char *endptr;
	u32_t _uaddr;
	while(*c != '\n' && *c != '\0' ) {
		if( strchr(ws_chars,(int)(*c)) == NULL ) {  // skip whitespaces
			_uaddr = (u32_t)strtoul(c,&(endptr),0);
			if ((_uaddr >= valid_addr_range[0]) && (_uaddr <= valid_addr_range[1])) {
				uaddr = _uaddr;
				return endptr;
			}
			return NULL;
		}
		c++;
	}
	return NULL;
}
//--------------------------------------------------------------------------
// UDP Write command:
// >W ADDR D1 D2 D3.... <nl>  - This will write the data starting at address "ADDR".
// It will write to consecutive memory locations.  Thus D1 => ADDR, D2 =>ADDR+1, etc.
// This command will respond with "OK" if the command is successful or "ERR" if it
// failed (for whatever reason).
//
// cmdWWrite function:
//   c = string which points at data string: "D1 D2..."
// return= the number of data values written, or 0 if failed
//
int cmdWWrite(char * c)
{
	char *pch;
	u32_t data;
	int ndata = 0;
	pch = strtok (c,ws_chars);
	if(pch == NULL) return 0;  // no data!
	do
	{
		if (uaddr > valid_addr_range[1]) {
			return 0;
		}
		data = (u32_t)strtoul(pch,NULL,0);
		*(u32_t *)uaddr = data;  // Write data to this location...
		ndata++;
		uaddr += sizeof(u32_t);
		pch = strtok (NULL,ws_chars);
	} while (pch != NULL);
	return ndata;
}
//--------------------------------------------------------------------------
// UDP Read command:
// >R ADDR N <cr>  - This will read N locations starting at address "ADDR".  It will
// return the values read in a string as: R ADDR Q1 Q2 ... QN.<cr>  or "ERR" if it failed.
// Data is read from consecutive 32 bit address locations, thus Q1 <= ADDR, Q2 <= ADDR+1, etc
//
// cmdRRead function:
//   c = string which points at N.   Takes address from global uaddr
// return = returns number of values read (N), or zero if it failed
//
int cmdRRead(char * c)
{
	u32_t data;
	int ndata;
	char *rptr = response;
	int rsize = sizeof(response);
	int rinc;
	int iloop;

	ndata = (u32_t)strtoul(c,NULL,0);
	if(ndata == 0) return 0;  // no data!

	rinc = snprintf(rptr,rsize,"R 0x%x ",uaddr);
	if(rinc > 0) {
		rsize -= rinc;
		rptr += rinc;
	}
	else return 0;
	for(iloop=0; iloop <ndata; iloop++)
	{
		if (uaddr > valid_addr_range[1]) {
			return 0;
		}
		data = *(u32_t *)uaddr;  // Read data from this location...
		rinc = snprintf(rptr,rsize,"0x%x ",data);
		if(rinc > 0) {
			rsize -= rinc;
			rptr += rinc;
		}
		else return 0;
		uaddr += sizeof(u32_t);
	}
	rinc = snprintf(rptr,rsize,"\n");
	return ndata;
}
//--------------------------------------------------------------------------
// UDP Poke command:
// >>P ADDR D1 D2 D3 .... <cr>  - This will poke the data to address "ADDR".  It will
// write repeatedly to the specified memory location.  Thus D1 => ADDR, D2 =>ADDR, etc.
// This command will respond with "OK" if the poke is successful.
//
// cmdPPoke function:
//   c = string which points at data string: "D1 D2..."
// return= the number of data values written, or 0 if failed
int cmdPPoke(char * c)
{
	char *pch;
	u32_t data;
	int ndata = 0;
	pch = strtok (c,ws_chars);
	if(pch == NULL) return 0;  // no data!
	do
	{
		//if ((uaddr >= valid_addr_range[0]) && (uaddr <= valid_addr_range[1])) {
		//	return 0;
		//}
		data = (u32_t)strtoul(pch,NULL,0);
		*(u32_t *)uaddr = data;  // Write data to this location...
		ndata++;
		//uaddr += sizeof(u32_t);  -- Poke...
		pch = strtok (NULL,ws_chars);
	} while (pch != NULL);
	return ndata;
}
//--------------------------------------------------------------------------
// UDP Peek command:
// >K ADDR N <cr>  - This will read N locations starting at address "ADDR".  It will
// return the values read in a string as: K ADDR Q1 Q2 ... QN.<cr>  or "ERR" if it failed.
// Data is read from the same 32 bit address locations, thus Q1 <= ADDR, Q2 <= ADDR, etc
//
// cmdKPeek function:
//   c = string which points at N.   Takes address from global uaddr
// return= the number of data values written, or 0 if failed
int cmdKPeek(char * c)
{
	u32_t data;
	int ndata;
	char *rptr = response;
	int rsize = sizeof(response);
	int rinc;
	int iloop;

	ndata = (u32_t)strtoul(c,NULL,0);
	if(ndata == 0) return 0;  // no data!

	rinc = snprintf(rptr,rsize,"K 0x%x ",uaddr);
	if(rinc > 0) {
		rsize -= rinc;
		rptr += rinc;
	}
	else return 0;
	for(iloop=0; iloop <ndata; iloop++)
	{
		data = *(u32_t *)uaddr;  // Read data from this location...
		rinc = snprintf(rptr,rsize,"0x%x ",data);
		if(rinc > 0) {
			rsize -= rinc;
			rptr += rinc;
		}
		else return 0;
	}
	rinc = snprintf(rptr,rsize,"\n");
	return ndata;
}