/*
  Copyright (c) 2004,   Beijing Senselock Data Security Centre
  All rights reserved.
  
  File£ºrsa.c
  
  Briefs£ºThis code will use the private key inside the SenseLock EL device         
          to do the sign/verify/encrypt/decrypt operation for the input data
          according to PKCS#1.          
  
*/

#include <string.h>
#include "..\..\inc\hardware\ses_v3.h"
#include "..\inc\sample_27.h"

#define _DEBUG

/* global variable defination. */
unsigned char last_ses_error = 0; /* last SES error */
IO_PACKAGE out_pkg;  /* output package */
IO_PACKAGE *output = &out_pkg; 
IO_PACKAGE *input = (IO_PACKAGE *)pbInBuff; /* input package */
unsigned char tmp_buf[128];/* temporary buffer used in make_signature and verify_signature */

/*digest info:const value for PKCS#1 encoding*/
static unsigned char DIGEST_INFO_SHS[] = {
0x30,0x21,0x30,0x09,0x06,0x05,0x2b,0x0e,0x03,0x02,0x1a,0x05,0x00,0x04,0x14};
static unsigned char DIGEST_INFO_MD5[] = {
  0x30, 0x20, 0x30, 0x0c, 0x06, 0x08, 0x2a, 0x86, 0x48, 0x86, 0xf7, 0x0d, 0x02,
  0x05, 0x05, 0x00, 0x04, 0x10 };

/*
	Exit upon an error.
  
	Parameters:
		errcode[in]: error code
		msg[in]: additional error message
		msglen[in]: message length
  
	Return:
		none.

	Remarks:
		none.
*/
void error_exit(unsigned char errcode, unsigned char *msg, int msglen)
{
	output->tag = errcode;
	output->len = 0;
	
	/* for debug purpose only */
#ifdef  _DEBUG
	if (msglen != 0 && msg != NULL)
	{
		memcpy(output->buff, msg, msglen);
		output->len = msglen;
	}
#endif

	_set_response(2+output->len, (unsigned char *)output);
	_exit();
}


/*
	This function signs digest according to PKCS#1 .
	
	Parameters:
		fid[in]: file ID of private key
		dgst_alg[in]: digest alorithm
		in[in]: message digest 
		inlen[in]: digest length
		out[out]: signature 
		outlen[in,out]: [in]out buffer size &&[out]signature length 
	Return: 
		If the function succeeds, it will return ERR_SUCCESS, otherwise 
		it returns corresponding error code.

	Remarks:
		External buffer at least 128 bytes long required, defined as tmp_buf
*/
int make_signature(unsigned short fid, unsigned char dgst_alg, 
				   unsigned char *in, int inlen, 
				   unsigned char *out, int *outlen)
{
	int ret = 0;
	int i = 0;
	int r_blklen = 0; /* required block length for different hash algorithms */
	int dgst_infolen = 0; /* digest info length */
	unsigned char *p_dgst_info = NULL; /* pointer to digest info */

    if (*outlen < RSA_BLOCK_LEN)
	{
		return ERR_INVALID_PARAMETER;
	}

    switch (dgst_alg)
	{
		case DA_SHS:
			r_blklen = 	SHA1_BLOCK_LEN;
			dgst_infolen = sizeof(DIGEST_INFO_SHS);
			p_dgst_info = &DIGEST_INFO_SHS;
			break;

		case DA_MD5:
			r_blklen = 	MD5_BLOCK_LEN;
			dgst_infolen = sizeof(DIGEST_INFO_MD5);
			p_dgst_info = &DIGEST_INFO_MD5;
			break;

		default:
			return ERR_INVALID_PARAMETER;
	}

	if (r_blklen != inlen)
	{
		return ERR_INVALID_PARAMETER;
	}

	tmp_buf[0] = 0;

	/* block type 1 */
	tmp_buf[1] = 1;
	
	/* padding */
	for (i=2; i<RSA_BLOCK_LEN-1-dgst_infolen-r_blklen; i++)
	{
		tmp_buf[i]=0xff;
	}
    
	/* seperator */
	tmp_buf[i++] = 0;
	ret = _mem_copy(&tmp_buf[i], p_dgst_info, dgst_infolen);
	if (ret != SES_SUCCESS)
	{
		last_ses_error = ret;
		return ERR_SES;
	}
	
	i += dgst_infolen;
	ret = _mem_copy(&tmp_buf[i],in,inlen);
	
	if (ret != SES_SUCCESS)
	{
		last_ses_error = ret;
		return ERR_SES;
	}

	ret = _rsa_dec(RSA_CALC_NORMAL, fid, RSA_BLOCK_LEN, tmp_buf);
	if (ret != SES_SUCCESS)
	{
		last_ses_error = ret;
		return ERR_SES;
	}

	ret = _mem_copy(out, tmp_buf, RSA_BLOCK_LEN);
	if (ret != SES_SUCCESS)
	{
		last_ses_error = ret ;
		return ERR_SES;
	}

	*outlen = RSA_BLOCK_LEN;				
	return ERR_SUCCESS;
}

/*
	This function verifies signature according to PKCS#1 .
	
	Parameters:
		fid[in]:       File ID used to verify;
		dgst_alg[in]:  Digest alorithm;
		dgst[in]:      Digest;   
		dgst_len[in]:  Digest length;
		sig[out]:       Signature; 
		sig_len[out]:   Signature length; 
	Return: 
		If verificataion succeeds ,it will return ERR_SUCCESS,otherwise 
		it returns corresponding error code.
	Remarks:
		External buffer at least 128 bytes long required, defined as tmp_buf
*/

int verify_signature(unsigned short fid, unsigned char dgst_alg, 
				   unsigned char *dgst, int dgst_len, 
				   unsigned char *sig, int sig_len)
{
	int ret = 0;
	int i = 0;
	
	int r_blklen = 0;
	int dgst_infolen = 0;
	unsigned char *p_dgst_info =NULL;

	if (sig_len != RSA_BLOCK_LEN)
	{
		return ERR_INVALID_PARAMETER;	
	}

    switch(dgst_alg)
	{
		case DA_SHS:
			r_blklen = 	SHA1_BLOCK_LEN;
			dgst_infolen = sizeof(DIGEST_INFO_SHS);
			p_dgst_info = &DIGEST_INFO_SHS;
			break;

		case DA_MD5:
			r_blklen = 	MD5_BLOCK_LEN;
			dgst_infolen = sizeof(DIGEST_INFO_MD5);
			p_dgst_info = &DIGEST_INFO_MD5;
			break;

		default:
			return ERR_INVALID_PARAMETER;
	}

	if (r_blklen != dgst_len)
	{
		return ERR_INVALID_PARAMETER;
	}

	ret = _mem_copy(tmp_buf, sig, sig_len);
	if (ret != SES_SUCCESS)
	{
		last_ses_error = ret;
		return ERR_SES;
	}

	ret = _rsa_enc(RSA_CALC_NORMAL, fid, RSA_BLOCK_LEN, tmp_buf);
	if (ret != SES_SUCCESS)
	{
		last_ses_error = ret ;
		return ERR_SES;
	}

	if ((tmp_buf[i++] != 0) || (tmp_buf[i++] != 1))
	{
		return ERR_INCORRECT_SIGNATURE;
	}

	for (i=2; i<RSA_BLOCK_LEN-1; i++)
	{
		if(tmp_buf[i] != 0xff) 
		{
			break;
		}
	}

	/*seperator*/
	if ((tmp_buf[i++]) != 0)
	{
		return ERR_INCORRECT_SIGNATURE;
	}

/*	if (i < 11)
	{
		return ERR_INCORRECT_SIGNATURE;
	}
*/
	if (RSA_BLOCK_LEN-i != dgst_infolen+dgst_len)
	{
		return ERR_INCORRECT_SIGNATURE;
	}

	if (_mem_cmp(&tmp_buf[i], p_dgst_info, dgst_infolen))
	{
		return ERR_INCORRECT_SIGNATURE;
	}

	i += dgst_infolen;
	if (_mem_cmp(&tmp_buf[i], dgst, dgst_len))
	{
		return ERR_INCORRECT_SIGNATURE;
	}

	return ERR_SUCCESS;
}

/*
	This function encrypt a plaintext block according to PKCS#1 .
	
	Parameters:
		fid[in]: file ID of public key
		in[in]: plaintext
		inlen[in]: plaintext length,must < 117
		out[out]: ciphertext
		outlen[in,out]: [in]ciphertext buffer size &&[out]ciphertext length 
	Return: 
		If the function succeeds, it will return ERR_SUCCESS, otherwise 
		it returns corresponding error code.
	Remarks:
		External buffer at least 128 bytes long required, defined as tmp_buf.
*/
int rsa_encrypt(unsigned short fid,
				  unsigned char *in, int inlen, 
				  unsigned char *out, int *outlen)
{
	int ret = 0;

	if (inlen > RSA_BLOCK_LEN -11)
	{
		return ERR_INVALID_PARAMETER;
	}
	if (*outlen < RSA_BLOCK_LEN)/* insufficient buffer size */
	{
		return ERR_INVALID_PARAMETER;
	}	

	ret = _mem_copy(tmp_buf, in, inlen);
	if (ret != SES_SUCCESS)
	{
		last_ses_error = ret;
		return ERR_SES;
	}

	ret = _rsa_enc(RSA_CALC_PKCS, fid, inlen, tmp_buf);
	if (ret != SES_SUCCESS)
	{
		last_ses_error = ret ;
		return ERR_SES;
	}
		
	*outlen = RSA_BLOCK_LEN;
	ret = _mem_copy(out, tmp_buf, RSA_BLOCK_LEN);
	if (ret != SES_SUCCESS)
	{
		last_ses_error = ret ;
		return ERR_SES;
	}
	return ERR_SUCCESS;
}

/*
	This function decrypt a ciphertext block according to PKCS#1 .
	
	Parameters:
		fid[in]: file ID of private key
		in[in]: ciphertext
		inlen[in]: ciphertext length
		out[out]: plaintext
		outlen[in,out]: [in]plaintext buffer size &&[out]plaintext length 
	Return: 
		If the function succeeds, it will return ERR_SUCCESS, otherwise 
		it returns corresponding error code.
	Remarks:
		External buffer at least 128 bytes long required, defined as tmp_buf.
*/

int rsa_decrypt(unsigned short fid,
				  unsigned char *in, int inlen, 
				  unsigned char *out, int *outlen)
{ 
	int ret = 0;

	if (inlen != RSA_BLOCK_LEN)
	{
		return ERR_INVALID_PARAMETER;
	}
	ret = _mem_copy(tmp_buf, in, inlen);
	if (ret != SES_SUCCESS)
	{
		last_ses_error = ret;
		return ERR_SES;
	}
	
	ret = _rsa_dec(RSA_CALC_PKCS, fid, inlen, tmp_buf);
	if (ret != SES_SUCCESS)
	{
		last_ses_error = ret ;
		return ERR_SES;
	}		
	if (*outlen < tmp_buf[0])
	{
		return ERR_INVALID_PARAMETER;
	}
	*outlen = tmp_buf[0];
	ret = _mem_copy(out, tmp_buf+1, *outlen);
	if (ret != SES_SUCCESS)
	{
		last_ses_error = ret ;
		return ERR_SES;
	}		

	return ERR_SUCCESS;
}

void main()
{
	int ret = 0;
	int length = 0;
	RSA_BLOCK *rblk;
	RSA_VERIFY_BLOCK *rvblk;

	switch (input->tag)
	{
	case CMD_SIGN:
		rblk = (RSA_BLOCK *)input->buff;
		length = RSA_BLOCK_LEN;
		LE16_TO_CC(&rblk->fid); //invert WORD from Little-Endian to Big-Endian if necessary

		ret = make_signature(rblk->fid, rblk->dgst_alg, rblk->buff, rblk->len, output->buff, &length);
		break;

	case CMD_VERIFY:
		rvblk = (RSA_VERIFY_BLOCK *)input->buff;
		LE16_TO_CC(&rvblk->fid);
	  
		ret = verify_signature(rvblk->fid, rvblk->dgst_alg, rvblk->dgst, rvblk->dgst_len, rvblk->sig, rvblk->sig_len);
		length = 0;
		break;

	case CMD_ENCRYPT:
		rblk = (RSA_BLOCK *)input->buff;
		LE16_TO_CC(&rblk->fid);
		length = RSA_BLOCK_LEN;
		
		ret = rsa_encrypt(rblk->fid,rblk->buff,rblk->len,output->buff,&length);
		break;

	case CMD_DECRYPT:
		rblk = (RSA_BLOCK *)input->buff;
		LE16_TO_CC(&rblk->fid);
	    length = RSA_BLOCK_LEN;
		
		ret = rsa_decrypt(rblk->fid,rblk->buff,rblk->len,output->buff,&length);
		break;

	default:
		error_exit(ERR_INVALID_PARAMETER, NULL, 0);
		break;
	}

	if (ret != ERR_SUCCESS)
	{
		if (ret == ERR_SES)
		{
			error_exit(ret, &last_ses_error, 1);
		}
		else
		{
			error_exit(ret, NULL, 0);
		}
	}

	output->tag = ERR_SUCCESS;
	output->len = (unsigned char)length;
	_set_response(IO_PACKAGE_HEADER_SIZE+output->len, (unsigned char *)output);
	_exit();  
}
