#include "Pkcs11Exceptions.h"
#include "Util.h"

#ifdef __APPLE__
#include <wintypes.h>
#endif

//
// Extrae la etiqueta (tag), y longitudes de una estructura TLV
//
long CUtil::getTLV( byte * tlv, long * pTag, long * pTL_dataLen )
{
	long	tag;
	long	idx = 0;

	tag = tlv[idx++];
	if( (tag & 0x1f) == 0x1f )
		tag = (tag<<8) | tlv[idx++];

	if( pTag )
		*pTag = tag;

	if( tlv[idx] < 0x81 )
	{
		if( pTL_dataLen )
			*pTL_dataLen = tlv[idx];
		return idx + 1;
	}
	else if( tlv[idx] == 0x81 )
	{
		if( pTL_dataLen )
			*pTL_dataLen = tlv[idx+1];
		return idx + 2;
	}
	else if( tlv[idx] == 0x82 )
	{
		if( pTL_dataLen )
			*pTL_dataLen = (tlv[idx+1]<<8) | tlv[idx+2];
		return idx + 3;
	}
	else
		throw Pkcs11Exception( CKR_DATA_INVALID );

	return 0;
}


void CUtil::buildTLV( byte tag, byteBuffer & value )
{
	byte	tagLen[4];
	size_t	tlLen;

	tagLen[0] = tag;

	if( value.size() < 0x80 )
	{
		tagLen[1] = (byte)value.size();
		tlLen = 2;
	}
	else if( value.size() < 0x100 )
	{
		tagLen[1] = 0x81;
		tagLen[2] = (byte)value.size();
		tlLen = 3;
	}
	else
	{
		tagLen[1] = 0x82;
		tagLen[2] = (byte)(value.size()>>8);
		tagLen[3] = (byte)(value.size() & 0x00ff);
		tlLen = 4;
	}

	value.resize(value.size()+tlLen);
	memmove( &value[tlLen], &value[0], value.size()-tlLen );
	memcpy( &value[0], tagLen, tlLen );
}

void CUtil::pkcs1_5_Pad( byteBuffer &data, ulong ulPaddedDataLen )
{
	// Hay que comprobar los tamaños
	if(data.size()+4 > ulPaddedDataLen)
		throw Pkcs11Exception( CKR_DATA_INVALID );

	byteBuffer unpaddedData(data);

	data.resize(ulPaddedDataLen);
	memcpy(&data[ulPaddedDataLen - unpaddedData.size()], &unpaddedData[0], unpaddedData.size() );

	data[0] = 0;
	data[1] = 1;
	memset(&data[2], 0xFF, ulPaddedDataLen - unpaddedData.size()-3);
	data[ulPaddedDataLen - unpaddedData.size() - 1] = 0;
}

void CUtil::addPadding_7816( byteBuffer &data )
{
	data.push_back(0x80);
	while( data.size() % 8 )
		data.push_back(0x00);
}

void CUtil::unPad_7816( byteBuffer &data )
{
	int	offset = data.size()-1;

	while( offset > 0 && data[offset]==0 )
		offset--;

	if( offset<0 || data[offset]!=0x80 )
		throw Pkcs11Exception( CKR_DATA_INVALID );

	data.resize(offset);
}

void CUtil::pkcs1_5_Unpad( SecByteBlock & message )
{
	DWORD	idx;
	SecByteBlock	unPadded;
	static byteBuffer	sha1DigestInfo("3021300906052b0e03021a05000414");

	if( message.size()<40 || message[0] != 0 || message[1] != 1 )
		throw Pkcs11Exception( CKR_DATA_INVALID );

	for( idx=2; (message[idx] == 0xff) && (idx<message.size()-2); ++idx )
		;

	if( message[idx++] != 0 )
		throw Pkcs11Exception( CKR_DATA_INVALID );

	if( memcmp( &message[idx], &sha1DigestInfo[0], sha1DigestInfo.size() ) )
		throw Pkcs11Exception( CKR_DATA_INVALID );

	idx += sha1DigestInfo.size();
	unPadded.Assign(&message[idx],message.size()-idx);
	message.swap(unPadded);
}

void CUtil::pkcs1Unpadding( byteBuffer  paddedData,
						    byteBuffer  & unpaddedData)
{
	BYTE BT;

	unsigned int i;
	unsigned long ulPaddedDataLen = paddedData.size();

	// Los datos de entrada debe tener el siguiente formato
	//            paddedData = 00 || BT || PS || 00 || D
	if (paddedData[0] != 0x00)
		throw Pkcs11Exception( CKR_DATA_INVALID );

	BT = paddedData[1];	
	switch(BT)
	{
	case 0x00:
		// PS son todos ceros
		i = 2;
		while ((paddedData[i] == 0) && (i < (ulPaddedDataLen)))
			i++;

		unpaddedData.resize(ulPaddedDataLen-i);
		memcpy(unpaddedData, &paddedData[i], ulPaddedDataLen-i);
		break;	

	case 0x01:
		// PS son todos FF
		i = 2;
		while ((paddedData[i] == 0xFF) && (i < (ulPaddedDataLen)))
			i++;

		if (paddedData[i] != 0x00)
			throw Pkcs11Exception( CKR_DATA_INVALID );

		i++;

		unpaddedData.resize(ulPaddedDataLen-i);
		memcpy(unpaddedData, &paddedData[i], ulPaddedDataLen-i);
		break;

	case 0x02:
		// PS son numero aleatorios distintos de 00
		i = 2;
		while ((paddedData[i] != 0x00) && (i < (ulPaddedDataLen)))
			i++;

		if (paddedData[i] != 0x00)
			throw Pkcs11Exception( CKR_DATA_INVALID );

		i++;

		unpaddedData.resize(ulPaddedDataLen-i);
		memcpy(unpaddedData, &paddedData[i], ulPaddedDataLen-i);
		break;

	default:
		throw Pkcs11Exception( CKR_DATA_INVALID );
	}
}


bool CUtil::GetBit( BIT_STRING_t * bitString, ulong bit )
{
	ulong	octect = bit>>3;
	bit = 7 - (bit & 7);

	if( octect < (ulong)bitString->size )
	{
		return (bitString->buf[octect] & (1<<bit)) ? true : false;
	}
	return false;
}

int CUtil::GetInt32( INTEGER_t * pInteger )
{
	int	value = 0;

	if( pInteger->size > sizeof(int) )
		throw Pkcs11Exception( CKR_DATA_LEN_RANGE );

	for( int idx=0; idx<pInteger->size; ++idx )
		value = (value<<8) | pInteger->buf[idx];

	return value;
}

bool CUtil::SetBit( BIT_STRING_t * bitString, ulong bit, bool bValue )
{
	ulong	octect = bit>>3;
	bit = 7 - (bit & 7);

	int value = (int)bValue;

	if( octect < (ulong)bitString->size )
	{
		bitString->buf[octect] |= (value<<bit);

		return true;
	}

	return false;
}

void CUtil::PathASCII2bin( byte * pbASCII, ulong ulASCIILen, byteBuffer & bbBIN )
{
	ulong	val;

	bbBIN.resize(0);

	while( ulASCIILen )
	{
		while( ulASCIILen && (*pbASCII == '\\') )
		{
			pbASCII++;
			ulASCIILen--;
		}

		for( val=0; ulASCIILen && isxdigit(*pbASCII); )
		{
			val = val*16 + ((*pbASCII>'9') ?toupper(*pbASCII) -'A'+10:*pbASCII-'0');
			pbASCII++;
			ulASCIILen--;
		}

		if( !val )
			return;

		bbBIN.push_back((byte)(val>>8));
		bbBIN.push_back(val&0xff);
	}

}


void CUtil::Bin2ASCII( byte * pBin, ulong ulBinLen, char * pASCII )
{
	static char * pAlfa = "0123456789ABCDEF";
	ulong idx;

	for(idx=0; idx<ulBinLen; idx++)
	{
		*(pASCII++) = pAlfa[*pBin>>4];
		*(pASCII++) = pAlfa[*pBin&0x0f];
		pBin++;
	}
}


byteBuffer CUtil::PathBin2ASCII( byteBuffer & bbBin )
{
	static char * pAlfa = "0123456789ABCDEF";

	byteBuffer bbASCII;
	
	// Si el número de bytes binarios son impares
	// completamos con un cero... (sólo para poder leer, pero el path será erróneo)
	if(bbBin.size()&1)
		bbBin.push_back(0);

	bbASCII.resize( (bbBin.size()/2) * 5);

	byte * pASCII = &bbASCII[0];
	byte * pBin = &bbBin[0];

	for(int idx = 0; idx<bbBin.size(); idx++)
	{
		if(!(idx&1))
			*(pASCII++) = '\\';

		*(pASCII++) = pAlfa[*pBin>>4];
		*(pASCII++) = pAlfa[*pBin&0x0f];
		pBin++;
	}

	return bbASCII;
}

#ifdef _DEBUG
#if !defined WIN32
    #include <syslog.h>
    #define OutputDebugString(S)    syslog(LOG_LOCAL0|LOG_LOCAL0,S)
#endif
//
// Funciones de trazas
//
void CUtil::DBG_trace( const char * format, ... )
{
	va_list args;
	char	* buffer = NULL;
	int		buffLen = 256;

	va_start(args, format);

	do
	{
		if( buffer )
			delete buffer;
		buffLen *= 2;
		buffer = new char[buffLen];
		strcpy(buffer, "FNMT_P11 ");
	} while( vsnprintf(&buffer[9], buffLen-10, format, args) == -1 );

	OutputDebugString(buffer);
	delete buffer;

	va_end(args);
};

void CUtil::DBG_dumpData( byte * pData, long lenData )
{
	long	i, j;
	int		pos;
	char	buffer[256];

	for (i=0; i<lenData; i+= 16)
	{
		pos = sprintf( buffer, "FNMT_P11 %6x - ",i);
		for (j = 0;j<16;j++)
		{
			if ((i +j) < lenData)
				pos += sprintf( &buffer[pos], "%02x ", pData[i+j] );
			else
				pos += sprintf( &buffer[pos], "   " );
		}

		pos += sprintf( &buffer[pos], "    " );
		for (j = 0;j<16;j++)
		{
			if ((i +j) < lenData)
				pos += sprintf( &buffer[pos], "%c", isalnum(pData[i+j]) ? pData[i+j] : '.' );
		}
		pos += sprintf( &buffer[pos], "\n" );
		OutputDebugString(buffer);
	}
}

void CUtil::DBG_dumpDirect(char* texto, byte * pData, long lenData )
{
	long	i,j;
	int		pos;
	char	buffer[2048];

	pos = sprintf( buffer, texto);
	pos += sprintf( &buffer[pos], "\n");
	OutputDebugString(buffer);

	for (i=0; i<lenData; i+= 16)
	{
		pos = sprintf( buffer, "\t");
		for (j=0; j<16; j++)
		{
			if ((i +j) < lenData)
				pos += sprintf( &buffer[pos], "%02x ", pData[i+j] );
			else
				pos += sprintf( &buffer[pos], "   " );
		}

		pos += sprintf( &buffer[pos], "\n" );
		OutputDebugString(buffer);
	}
}

void CUtil::DBG_dumpTemplate( CK_ATTRIBUTE_PTR pTemplate, CK_ULONG count )
{
	DBG_trace( "DBG_dumpTemplate(0x%08x,%d)\n", pTemplate, count );
	if( !pTemplate || !count )
		return;
	for( CK_ULONG idx = 0; idx<count; ++idx )
	{
		DBG_trace( "%3d - 0x%08x,0x%08x,%d\n", idx, pTemplate[idx].type, pTemplate[idx].pValue, pTemplate[idx].ulValueLen );
		if( pTemplate[idx].pValue && pTemplate[idx].ulValueLen )
			DBG_dumpData( (byte *)pTemplate[idx].pValue, (long)pTemplate[idx].ulValueLen );
	}
}


#endif
