/*Copyright (C) 2005 Michael Kasianowicz (xtravar@yahoo.com)

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 Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

/* OS X support thanks to sixten */
#ifdef __APPLE__
#include <malloc/malloc.h>
#include <machine/endian.h>
#else
#include <malloc.h>
#include <netinet/in.h>
#endif
#include <string.h>
#include <stdio.h>

#include "p2k.h"


static uint16_t _p2k_current_packet = 0;

p2k_packet * p2k_packet_from_buff(const uint8_t *buff, int *pos) {
	p2k_packet *original = (p2k_packet*)(buff+*pos);
	if(p2k_packet_get_result(original) < 0) {
		return NULL;
	}
	int sz = p2k_packet_get_data_size(original);
	if(sz > P2K_PACKET_DATA_MAX) {
		return NULL;
	}

	p2k_packet *retval = (p2k_packet*)malloc(P2K_HEADER_SIZE+sz);
	memcpy(retval, original, P2K_HEADER_SIZE+sz);
	*pos += sz;
	return retval;
}

/* get the packet's id */
int p2k_packet_get_id(p2k_packet *packet) {
	return ntohs(packet->id) & 0x0FFF;
}

/* if this is a received packet, it will have the result in  */
int p2k_packet_get_command(p2k_packet *packet) {
    return ntohs(packet->command) & 0x0FFF;
}

/* return the size of the data */
int p2k_packet_get_data_size(p2k_packet *packet) {
	return ntohs(packet->data_size);
}

/* returns the result of the packet
    -1 if failed */
int p2k_packet_get_result(p2k_packet *packet) {

    uint8_t com_result = ntohs(packet->command) >> 12;

    /* i don't know whether these values should always be the same
       but it seems as though that is a good integrity check */
	/* ok they aren't always the same, i think */
/*	if(id_result != com_result) {
		return -1;
	}
*/

    return com_result;
}

int p2k_packet_get_id_result(p2k_packet *packet) {
    uint8_t id_result = ntohs(packet->id) >> 12;
    return id_result;
}

/* reads */
int p2k_packet_get_buff(p2k_packet *packet, int *pos, uint8_t *buff, int len) {
	int ret = *pos + len;
	if(ret > p2k_packet_get_data_size(packet)) {
		return 0;
	}

	memcpy(buff, packet->data+*pos, len);
	*pos = ret;
	return len;
}

int p2k_packet_get_u8(p2k_packet *packet, int *pos, uint8_t *value) {
	return p2k_packet_get_buff(packet, pos, value, 1);
}

int p2k_packet_get_u16(p2k_packet *packet, int *pos, uint16_t *value) {
	int ret = p2k_packet_get_buff(packet, pos, (uint8_t*)value, 2);
	if(ret > 0) {
		*value = ntohs(*value);
	}
	return ret;
}

int p2k_packet_get_u32(p2k_packet *packet, int *pos, uint32_t *value) {
	int ret = p2k_packet_get_buff(packet, pos, (uint8_t*)value, 4);
	if(ret > 0) {
		*value = ntohl(*value);
	}
	return ret;
}

int p2k_packet_get_str(p2k_packet *packet, int *pos, char *str, int len) {
	int ret = p2k_packet_get_buff(packet, pos, (uint8_t*)str, len);
	if(ret > 0) {
		str[len] = 0;
	}
	return ret;
}

int p2k_packet_get_wstr(p2k_packet *packet, int *pos, wchar_t *str, int len) {
	/* we need to do extra fooling since wchar_t is different on different platforms */
	uint16_t *buff = (uint16_t*)malloc(len*2);
	int ret = p2k_packet_get_buff(packet, pos, (uint8_t*)buff, len*2);
	if(ret > 0) {
		str[len] = 0;
		int i;
		for(i = 0; i < len; i++) {
			str[i] = ntohs(buff[i]);
		}
	}
	free(buff);
	return ret;
}

int p2k_packet_get_wstr_a(p2k_packet *packet, int *pos, char *str, int len) {
	uint16_t *buff = malloc(len*2);
	int ret = p2k_packet_get_buff(packet, pos, (uint8_t*)buff, len*2);
	if(ret > 0) {
		str[len] = 0;
		int i;
		for(i = 0; i < len; i++) {
			wchar_t c = ntohs(buff[i]);
			if(c > 0x7F) {
				str[i] = '?';
			} else {
				str[i] = ntohs(buff[i]);
			}
		}
	}
	free(buff);
	return ret;
}


/* set the size of the data */
int p2k_packet_set_data_size(p2k_packet *packet, uint16_t size) {
	if(size > P2K_PACKET_DATA_MAX) {
		return -1;
	}

	packet->data_size = htons(size);
}



/* initialize the packet:
    set the id to a unique id
    set the command
    make data size 0
    make dummy 0
    returns the packet's id */
p2k_packet *p2k_packet_create(p2k_command command, int max_data_size) {
	if(max_data_size > P2K_PACKET_DATA_MAX) {
		return NULL;
	}
	p2k_packet *retval = (p2k_packet*)malloc(P2K_HEADER_SIZE + max_data_size);	
	
	p2k_packet_init(retval, command);
	return retval;
}

void p2k_packet_init(p2k_packet *packet, p2k_command command) {
	p2k_packet_newid(packet);

	packet->command = htons(command);
	packet->data_size = 0;
	packet->_dummy = 0;
}

void p2k_packet_newid(p2k_packet *packet) {
	if(_p2k_current_packet > P2K_PACKET_ID_MAX) {
		_p2k_current_packet = 0;
	}
	uint16_t id = _p2k_current_packet++;
	packet->id = htons(id);
}


/* copies a buffer into the packet's data buffer at pos
   also, updates pos to reflect the current position in the data buffer
   returns bytes copied, 0 on max breached */
int p2k_packet_put_buff(p2k_packet *packet, int *pos, const uint8_t *buff, uint16_t len) {
	int newpos = *pos+len;
	if(newpos > P2K_PACKET_DATA_MAX) {
		return 0;
	}
	if(packet->data_size < newpos) {
		packet->data_size = newpos;
	}
	packet->data[*pos];
	memcpy(packet->data+*pos, buff, len);
    *pos = newpos;
    return len;
}

/* simply wraps around p2k_packet_put_buff
   could be more specialized, but we're keeping the code easy */
int p2k_packet_put_u8(p2k_packet *packet, int *pos, uint8_t value) {
	return p2k_packet_put_buff(packet, pos, (const uint8_t*)&value, 1);
}

/* changes the integer to big endian and uses put_buff */
int p2k_packet_put_u16(p2k_packet *packet, int *pos, uint16_t value) {
	value = htons(value);
	return p2k_packet_put_buff(packet, pos, (const uint8_t*)&value, 2);
}

/* changes the integer to big endian and uses put_buff */
int p2k_packet_put_u32(p2k_packet *packet, int *pos, uint32_t value) {
	value = htonl(value);
	return p2k_packet_put_buff(packet, pos, (const uint8_t*)&value, 4);
}

/* writes the ascii string to the packet's data buffer */
int p2k_packet_put_str(p2k_packet *packet, int *pos, const char *str) {
	int len = strlen(str);
	return p2k_packet_put_buff(packet, pos, (const uint8_t*)str, len);
}

/* writes the ascii string to the packet's data buffer and pads the rest with 0s */
int p2k_packet_put_strpad(p2k_packet *packet, int *pos, const char *str, int total) {
	int len = strlen(str);
	if(len > total) {
		return 0;
	}
	uint8_t *buff = malloc(total);
	memset(buff, 0, total);
	memcpy(buff, str, len);

	int retval = p2k_packet_put_buff(packet, pos, buff, total);
	free(buff);
	return retval;
}

/* writes a utf-16 string */
int p2k_packet_put_wstr(p2k_packet *packet, int *pos, const wchar_t *str) {
	int len = wcslen(str);
	int rlen = len*2;
	uint16_t *buff = malloc(rlen);
    int i;
	for(i = 0; i < len; i++) {
		buff[i] = htons(str[i]);
	}
	int retval = p2k_packet_put_buff(packet, pos, (const uint8_t*)buff, rlen);
	free(buff);
	return retval;
}

/* writes an ascii string as a utf-16 string */
int p2k_packet_put_wstr_a(p2k_packet *packet, int *pos, const char *str) {
	int len = strlen(str);
	int rlen = len*2;
	uint16_t *buff = malloc(rlen);
    int i;
	for(i = 0; i < len; i++) {
		buff[i] = htons(str[i]);
	}
	int retval = p2k_packet_put_buff(packet, pos, (const uint8_t*)buff, rlen);
	free(buff);
	return retval;
}

/* writes a padded utf-16 string */
int p2k_packet_put_wstrpad(p2k_packet *packet, int *pos, const wchar_t *str, int total) {
	int len = wcslen(str);
	int rlen = len*2;
	int blen = total*2;
	if(len > total) {
		return 0;
	}
	uint16_t *buff = malloc(blen);
	memset(buff, 0, blen);
	int i;
	for(i = 0; i < len; i++) {
		buff[i] = htons(str[i]);
	}
	int retval = p2k_packet_put_buff(packet, pos, (const uint8_t*)buff, blen);
	free(buff);
	return retval;
}

/* writes an ascii string as a 0-padded utf-16 string */
int p2k_packet_put_wstrpad_a(p2k_packet *packet, int *pos, const char *str, int total) {
	int len = strlen(str);
	int rlen = len*2;
	int blen = total*2;
	if(len > total ) {
		return 0;
	}
	uint16_t *buff = malloc(blen);
	memset(buff, 0, blen);
	int i;
	for(i = 0; i < len; i++) {
		buff[i] = htons(str[i]);
	}
	int retval = p2k_packet_put_buff(packet, pos, (const uint8_t*)buff, blen);
	free(buff);
	return retval;
}

int p2k_packet_destroy(p2k_packet *packet) {
	free(packet);
	return 0;
}



void p2k_packet_print(p2k_packet *packet) {
	printf("%10s: %10d\n", "ID", (int)p2k_packet_get_id(packet));
	p2k_command com = p2k_packet_get_command(packet);
	printf("%10s: %10s\n", "Type", p2k_command_str(com));
	if(com == P2K_FSAC) {
		p2k_fsac_command fsac;
		int pos = 0;
		if(p2k_packet_get_u32(packet, &pos, (uint32_t*)&fsac) > 0) {
			const char *fsac_str = p2k_fsac_command_str(fsac);
			if(fsac_str != NULL) {
				printf("%10s: %10s\n", "FSAC", fsac_str);
			}
		}
	}
	int size = p2k_packet_get_data_size(packet);
	printf("%10s: %10d\n", "Data size", size);
	printf("%10s: %10d\n", "Result", p2k_packet_get_result(packet));
	printf("%10s: %10d\n", "Result2", p2k_packet_get_id_result(packet));
	if(size == 0) {
		return;
	}

	printf("%10s:", "Data");

	int i;
	for(i = 0; i < size; i++) {
		if(i % 16 == 0) {
			printf("\n");
		}
		printf(" %2X", (int)packet->data[i]);
	}
	printf("\n");
}

void p2k_packet_dbgpr(p2k_packet *packet) {
	int size = p2k_packet_get_data_size(packet) + 8;
	char *buff = (char*)packet;
	int i;

	printf("Debug: ");
	for(i = 0; i < size; i++) {
		if(i % 16 == 0) {
			printf("\n");
		}
		printf(" %2X", (int)buff[i]);
	}
	printf("\n");
}

const char *p2k_result_str(p2k_result result) {
/*
	switch(result) {
		case P2K_ERROR_LENGTH:
			return "Invalid data length for command.";
		case P2K_ERROR_SECURITY:
				return "Inadequate security level for command/parameter.";
		case P2K_ERROR_COMMAND:
				return "Command/parameter not supported for current protocol.";
		case P2K_ERROR_OPCODE:
				return "Unsupported/invalid opcode.";
		case P2K_ERROR_PARAM:
				return "Unsupported/invalid parameter for opcode.";
		case P2K_ERROR_NODATA:
				return "Unexpected result - no data received.";
		case P2K_ERROR_GENERIC:
				return "Generic failure.";
		case P2K_ERROR_DATA:
			return "Unexpected result - data received.";
		case P2K_ERROR_UNKNOWN:
			return "Unknown error.";
		case P2K_ERROR_NOMEM:
			return "Couldn't allocate memory.";
		case P2K_ERROR_INTERR:
			return "Internal task error.";
		case P2K_ERROR_TIMEOUT:
			return "Test command task timed out waiting for response.";
		case P2K_ERROR_CDMAPARSE:
			return "CDMA parse error.";
		case P2K_ERROR_LESSDATA:
			return "Length specified in command header greater than received.";
		case P2K_ERROR_POWERDOWN:
			return "Irrecoverable error - phone is being powered down.";
		default:
			return NULL;
	}*/
return NULL;
}

const char *p2k_fsac_command_str(p2k_fsac_command com) {
	switch(com) {
	    case P2K_FSAC_OPEN:
			return "OPEN";
	    case P2K_FSAC_READ:
			return "READ";
	    case P2K_FSAC_WRITE:
			return "WRITE";
	    case P2K_FSAC_SEEK:
			return "SEEK";
	    case P2K_FSAC_CLOSE:
			return "CLOSE";
	    case P2K_FSAC_DELETE:
			return "DELETE";
	    case P2K_FSAC_CLEAR:
			return "CLEAR";
	    case P2K_FSAC_COUNT:
			return "COUNT";
	    case P2K_FSAC_LIST:
			return "LIST";
	    case P2K_FSAC_UNKNOWN:
			return "UNKNOWN";
	    case P2K_FSAC_VOLNAME:
			return "VOLNAME";
	    case P2K_FSAC_VOLSPACE:
			return "VOLSPACE";
		default:
			return NULL;
	}
}

/* eh, we need it for debugging purposes
    plus, it wasn't coded by hand */
const char *p2k_command_str(p2k_command com) {
	switch(com) {
		case P2K_AUD_TN_LST:
			return "AUD_TN_LST";
		case P2K_AD_CONV:
			return "AD_CONV";
		case P2K_AUTOCYCLE:
			return "AUTOCYCLE";
		case P2K_AUD_CTRL:
			return "AUD_CTRL";
		case P2K_AUD_LPB:
			return "AUD_LPB";
		case P2K_AUD_LVL:
			return "AUD_LVL";
		case P2K_AUD_PATH:
			return "AUD_PATH";
		case P2K_CARRIER:
			return "CARRIER";
		case P2K_CDATA:
			return "CDATA";
		case P2K_COMPD:
			return "COMPD";
		case P2K_CP_MODE:
			return "CP_MODE";
		case P2K_DTMF:
			return "DTMF";
		case P2K_ERS_PANIC:
			return "ERS_PANIC";
		case P2K_FLASH:
			return "FLASH";
		case P2K_GET_PANIC:
			return "GET_PANIC";
		case P2K_HDW:
			return "HDW";
		case P2K_INIT:
			return "INIT";
		case P2K_INVM:
			return "INVM";
		case P2K_KEYS:
			return "KEYS";
		case P2K_LOAD_SYN:
			return "LOAD_SYN";
		case P2K_LONG_STAT:
			return "LONG_STAT";
		case P2K_MEMACS:
			return "MEMACS";
		case P2K_MDI:
			return "MDI";
		case P2K_PHASE:
			return "PHASE";
		case P2K_PWR_OFF:
			return "PWR_OFF";
		case P2K_PREKEY:
			return "PREKEY";
		case P2K_RDELEM:
			return "RDELEM";
		case P2K_RDWR_SPI:
			return "RDWR_SPI";
		case P2K_RESTART:
			return "RESTART";
		case P2K_RCVS1:
			return "RCVS1";
		case P2K_RCVS2:
			return "RCVS2";
		case P2K_RQ:
			return "RQ";
		case P2K_RSSI:
			return "RSSI";
		case P2K_RTCC:
			return "RTCC";
		case P2K_SAT:
			return "SAT";
		case P2K_SET_LNA:
			return "SET_LNA";
		case P2K_SET_RF_PWR:
			return "SET_RF_PWR";
		case P2K_SIGTONE:
			return "SIGTONE";
		case P2K_STELEM:
			return "STELEM";
		case P2K_PARM:
			return "PARM";
		case P2K_MSG_INJ:
			return "MSG_INJ";
		case P2K_MSG_RT:
			return "MSG_RT";
		case P2K_SBSDY_LCK:
			return "SBSDY_LCK";
		case P2K_SUSPEND:
			return "SUSPEND";
		case P2K_TST_DISP:
			return "TST_DISP";
		case P2K_XUPID:
			return "XUPID";
		case P2K_VERSION:
			return "VERSION";
		case P2K_WSTS:
			return "WSTS";
		case P2K_WRITE_DA:
			return "WRITE_DA";
		case P2K_SMARTCARD:
			return "SMARTCARD";
		case P2K_LEDS:
			return "LEDS";
		case P2K_KEY_TEST:
			return "KEY_TEST";
		case P2K_IRDA:
			return "IRDA";
		case P2K_GPIO_TEST:
			return "GPIO_TEST";
		case P2K_BT:
			return "BT";
		case P2K_FLIP:
			return "FLIP";
		case P2K_AUD_TN_GEN:
			return "AUD_TN_GEN";
		case P2K_KEY_PRESS:
			return "KEY_PRESS";
		case P2K_ICACS:
			return "ICACS";
		case P2K_SSI_LPB:
			return "SSI_LPB";
		case P2K_CEM_TEST:
			return "CEM_TEST";
		case P2K_FSAC:
			return "FSAC";
		case P2K_WR_PBK:
			return "WR_PBK";
		case P2K_PU_TSTMODE:
			return "PU_TSTMODE";
		case P2K_CALL:
			return "CALL";
		case P2K_PROT:
			return "PROT";
		case P2K_MEMORY:
			return "MEMORY";
		case P2K_CARRIER_SYN_ATTN:
			return "CARRIER_SYN_ATTN";
		case P2K_HIBERNATE:
			return "HIBERNATE";
		case P2K_KJAVA:
			return "KJAVA";
		case P2K_TRAC_ACTIVE:
			return "TRAC_ACTIVE";
		case P2K_IS_FR:
			return "IS_FR";
		case P2K_SWDL:
			return "SWDL";
		case P2K_SWUL:
			return "SWUL";
		case P2K_AUD_TN_TEST:
			return "AUD_TN_TEST";
		case P2K_EFSAC:
			return "EFSAC";
		case P2K_TST_CAMERA:
			return "TST_CAMERA";
		case P2K_ALERT:
			return "ALERT";
		case P2K_HS_LPB:
			return "HS_LPB";
		case P2K_AUD_PH_LPB:
			return "AUD_PH_LPB";
		case P2K_ALT_STOP_AUD_PH_LPB:
			return "ALT_STOP_AUD_PH_LPB";
		case P2K_VIB_STOP_ALERT:
			return "VIB_STOP_ALERT";
		case P2K_CIT_ELEM:
			return "CIT_ELEM";
		case P2K_DIGTS:
			return "DIGTS";
		case P2K_TDMA_INFO:
			return "TDMA_INFO";
		case P2K_TDMA_ON:
			return "TDMA_ON";
		case P2K_PA_FDBACK:
			return "PA_FDBACK";
		case P2K_TDMA_CFG:
			return "TDMA_CFG";
		case P2K_IR_STATUS:
			return "IR_STATUS";
		case P2K_TX_PWR_DET:
			return "TX_PWR_DET";
		case P2K_COMMIT:
			return "COMMIT";
		case P2K_CPY_ACTIVE:
			return "CPY_ACTIVE";
		case P2K_GENERIC_DSP:
			return "GENERIC_DSP";
		case P2K_RDWR_UART:
			return "RDWR_UART";
		case P2K_WSCMP:
			return "WSCMP";
		case P2K_RDWR_DSP_SPI:
			return "RDWR_DSP_SPI";
		case P2K_DSP_SPI_TRIG_CONFIG:
			return "DSP_SPI_TRIG_CONFIG";
		case P2K_L1T_TOLS:
			return "L1T_TOLS";
		case P2K_L1T_TFTS:
			return "L1T_TFTS";
		case P2K_L1T_THEG:
			return "L1T_THEG";
		case P2K_L1T_TWRC:
			return "L1T_TWRC";
		case P2K_L1T_TRRC:
			return "L1T_TRRC";
		case P2K_L1T_TCCT:
			return "L1T_TCCT";
		case P2K_WLOAD_SYN:
			return "WLOAD_SYN";
		case P2K_RSSI_RSCP:
			return "RSSI_RSCP";
		case P2K_W_CARRIER:
			return "W_CARRIER";
		case P2K_W_PHASE:
			return "W_PHASE";
		case P2K_WPC_VAR:
			return "WPC_VAR";
		case P2K_WPC_DTAR:
			return "WPC_DTAR";
		case P2K_WPC_TPC:
			return "WPC_TPC";
		case P2K_RDWR_HARM:
			return "RDWR_HARM";
		case P2K_RQBER:
			return "RQBER";
		case P2K_AUD_SAMP:
			return "AUD_SAMP";
		case P2K_WOPCHAN:
			return "WOPCHAN";
		case P2K_GPS:
			return "GPS";
		case P2K_SUSPEND_COMP:
			return "SUSPEND_COMP";
		case P2K_TOUCH_SCREEN:
			return "TOUCH_SCREEN";
		case P2K_CAMERA:
			return "CAMERA";
		case P2K_TST_MMC:
			return "TST_MMC";
		case P2K_WUPD_COMP:
			return "WUPD_COMP";
		case P2K_BAUD_RATE:
			return "BAUD_RATE";
		case P2K_PDLOG:
			return "PDLOG";
		case P2K_RD_IC:
			return "RD_IC";
		case P2K_OCD:
			return "OCD";
		case P2K_WLONG_STAT:
			return "WLONG_STAT";
		case P2K_RDWR_IO:
			return "RDWR_IO";
		case P2K_DIP_SWITCH:
			return "DIP_SWITCH";
		case P2K_READ_ADC:
			return "READ_ADC";
		case P2K_GPS_CHECK_AGC:
			return "GPS_CHECK_AGC";
		case P2K_CW_SIGNAL:
			return "CW_SIGNAL";
		case P2K_PING:
			return "PING";
		case P2K_SCMP:
			return "SCMP";
		case P2K_TLPB:
			return "TLPB";
		case P2K_GPRS_LPB:
			return "GPRS_LPB";
		case P2K_EGPM:
			return "EGPM";
		case P2K_GPRS_SEND:
			return "GPRS_SEND";
		case P2K_TXBD:
			return "TXBD";
		default:
			return NULL;
	}
}