#include "pbkdf2.h"

#include "hmac_sha256.h"

#include <stdint.h>
#include <string.h>

#include "log.h"

int PBKDF2_HMAC_SHA256( const uint8_t* password, unsigned int passLength, const uint8_t* salt,
						 unsigned int saltLength, uint8_t* key, unsigned int keyLength,
						 unsigned int rounds )
{
	uint8_t hmac_out_prev [ SHA256_HASH_LENGTH ];
	uint8_t hmac_out_next [ SHA256_HASH_LENGTH ];
	uint8_t output [ SHA256_HASH_LENGTH ];
	/*u8 * block_num_buf; 
	 u8 salt_buf[MAX_SALT_SIZE + 4]; */
	uint8_t block_num_buf [ 4 ];
	uint32_t block_count = 0, round_count = 0, i = 0, round_key_length = 0;
	HMAC_SHA256 hmacCtx; 
	/* int	res; */
	
	if( saltLength > MAX_SALT_SIZE)
	{
		return PBKDF_ERR_SALT_SIZE;
	}
	/*
	memcpy( salt_buf, salt, saltLength );
	block_num_buf = salt_buf + saltLength;
        */
	for( block_count = 1; keyLength > 0; ++block_count )
	{
		block_num_buf[ 0 ] = ( block_count >> 24 ) & 0xFF;
		block_num_buf[ 1 ] = ( block_count >> 16 ) & 0xFF;
		block_num_buf[ 2 ] = ( block_count >>  8 ) & 0xFF;
		block_num_buf[ 3 ] = ( block_count		 ) & 0xFF;
		
		init_HMAC_SHA256( &hmacCtx, password, passLength );
		update_HMAC_SHA256( &hmacCtx, salt, saltLength );
		update_HMAC_SHA256( &hmacCtx, block_num_buf, 4 );
		final_HMAC_SHA256( &hmacCtx, hmac_out_prev );
		

		/*
		res = qsee_hmac( QSEE_HMAC_SHA256, salt_buf, saltLength + 4, key, keyLength, hmac_out_prev );
                
                
   		if ( 0 != res )
      		return PBKDF_HMAC_ERR;
      		*/
		memcpy( output, hmac_out_prev, SHA256_HASH_LENGTH );

		for( round_count = 1; round_count < rounds; ++round_count )
		{
			init_HMAC_SHA256( &hmacCtx, password, passLength );
			update_HMAC_SHA256( &hmacCtx, hmac_out_prev, SHA256_HASH_LENGTH );
			final_HMAC_SHA256( &hmacCtx, hmac_out_next );
			
                        /*
			res = qsee_hmac( QSEE_HMAC_SHA256, hmac_out_prev, SHA256_HASH_LENGTH, key, keyLength, hmac_out_next );
			

   			if ( 0 != res )
      			return PBKDF_HMAC_ERR;			
			*/
			memcpy( hmac_out_prev, hmac_out_next, SHA256_HASH_LENGTH );

			for( i = 0; i < SHA256_HASH_LENGTH; ++i )
			{
				output[ i ] ^= hmac_out_prev[ i ];
			}
		}
		round_key_length = ( keyLength < SHA256_HASH_LENGTH ) ? keyLength : SHA256_HASH_LENGTH;

		memcpy( key, output, round_key_length );

		keyLength -= round_key_length;
		key += round_key_length;
	}

	memset( hmac_out_prev, 0, SHA256_HASH_LENGTH );
	memset( hmac_out_next, 0, SHA256_HASH_LENGTH );
	memset( output, 0, SHA256_HASH_LENGTH );
	return PBKDF_NOERROR;
}