/********************************************************************************
 * Marvell GPL License Option
 *
 * If you received this File from Marvell, you may opt to use, redistribute and/or
 * modify this File in accordance with the terms and conditions of the General
 * Public License Version 2, June 1991 (the "GPL License"), a copy of which is
 * available along with the File in the license.txt file or by writing to the Free
 * Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 or
 * on the worldwide web at http://www.gnu.org/licenses/gpl.txt.
 *
 * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY
 * DISCLAIMED.  The GPL License provides additional details about this warranty
 * disclaimer.
 ******************************************************************************/

#include "drmdmx.h"

#include "memmap.h"
#include "customer_keystore_spec.h"
#include "basic_inc.h"
#include "nand_priv.h"
#include "common.h"
#include "mv_nand.h"

#define MEMPOLL_SIZE            (64 * 1024) /* 64kB. In fact, the real size should be less than 16kB */
#define MEMPOOL_ALIGNED_SIZE    (MEMPOLL_SIZE + 128)    /* make it (64 + 64) header/end aligned. */
#define CUSTK_IMAGE_MAX_SIZE 4096
#define PRELOAD_BUF_SIZE    (512 * 1024)
extern int __preload_buf_start;
#define PRELOAD_BUF_ADDR    ((unsigned int)(&__preload_buf_start))
#define VT_BUF_ADDR     (PRELOAD_BUF_ADDR)
#define FIGO_BUF_ADDR       (VT_BUF_ADDR + 4096)
#define IMG2_PART1_SIZE     52<<10
#define VT_OFFSET       (IMG2_PART1_SIZE)
#define VT_SIZE         4096
#define FIGO_OFFSET     ((VT_OFFSET) + (VT_SIZE))
#define FIGO_SIZE       4096
#define KEY_SIZE        4096
/*
 * Macro to remove unused variable warning for function args not used for
 * specific platform.
 */

#define UNUSED(var) do { (void)(var); } while(0)

static UNSG8 mempool[MEMPOOL_ALIGNED_SIZE];
static UNSG8 dtcmpool[MEMPOOL_ALIGNED_SIZE];
static UNSG8 gp_cust_figo_image[CUSTK_IMAGE_MAX_SIZE] ={0};
static unsigned char *gp_figo_buf  = (unsigned char *) FIGO_BUF_ADDR;

extern struct mv_nand_data nand_data;
extern void flush_all_dcache();
static void Wait_FIGO()
{
    //UNSG32 nRet = 0;
  T32SECSTATUS_CFG cfgSecStatus;
  //Waiting for FIGO Ready
  while(1) {
    cfgSecStatus.u32 = MV_FIGODRV_IO_RD32(DRMFIGOREG_SECSTAT);
    if ( cfgSecStatus.uCFG_flag  == SECSTATUS_CFG_flag_DISABLED )
    {
      continue;
    }
    if ( cfgSecStatus.uCFG_flag  == SECSTATUS_CFG_flag_FAILED )
    {
	//nRet    = MV_FIGODRV_ERR;
      break;
    }
    if ( cfgSecStatus.uCFG_flag  == SECSTATUS_CFG_flag_ENABLED )
    {
	//nRet    = MV_FIGODRV_OK;
      break;
    }
  }
  return;
}

static int parse_figo(char * figobuff)
{
  unsigned int figo_len, figo_off;
  unsigned int flags;

  flags = ((unsigned int *)gp_figo_buf)[0];
  figo_off = ((unsigned int *)gp_figo_buf)[1];
  figo_len = ((unsigned int *)gp_figo_buf)[2];
  printf("figo magic = %x, figo_off = %d, figo_len = %d\n", flags, figo_off, figo_len);

  if(figo_off > FIGO_SIZE || figo_len > FIGO_SIZE || (figo_off + figo_len) > FIGO_SIZE )
    return -1;

  UtilMemCpy(figobuff, &gp_figo_buf[figo_off], figo_len);

  return 0;
}

static int get_figo_img(char * figobuff)
{
  int i;
  for(i=1; i<9;i++) {
    if(mv_nand_block_bad(i*nand_data.szofblk, 0))
      continue;
    if( mv_nand_read_block( (loff_t)(i*nand_data.szofblk + FIGO_OFFSET),
                            (char *)gp_figo_buf, FIGO_SIZE + KEY_SIZE) < 0 )
      continue;

    if (0 == parse_figo(figobuff))
      break;
  }
  if(i==9) {
    //      lgpl_printf("Read FIGO image and customer key error!\n");
    return -1;
  }
  return 0;
}

static SIGN32 MV_DRMLIB_PreProcess_Customer_KeyStore(UNSG8* pbCustKeyStore,
                                                     UNSG32 uKeyStoreSize,
                                                     UNSG32* uKeyNum)
{
  UNSG32 uAESKeyNum = 0;
  UNSG32 uRSAKeyNum = 0;
  UNSG32 uKeyId = 0;
  UNSG32 uSize = 0;
  UNSG32 uAESKeyCnt = 0;
  UNSG32 uRSAPubKeyCnt = 0;
  UNSG32 uRSAPrvKeyCnt = 0;
  UNSG32 i = 0;

  // Parse the keystore header
  uAESKeyNum = *((UNSG32*)pbCustKeyStore);
  uRSAKeyNum = *((UNSG32*)(pbCustKeyStore + 4));

  if(uAESKeyNum == 0x00 && uRSAKeyNum == 0x00)
  {
    //lgpl_printf("Invalid Customer Key Store: AES Key Num = 0x%x, RSA Key Num = 0x%x\n", uAESKeyNum,uRSAKeyNum);
    return -1;
  }
  else
  {
    //lgpl_printf("Valid Customer Key Store: AES Key Num = 0x%x, RSA Key Num = 0x%x\n", uAESKeyNum,uRSAKeyNum);
  }
  pbCustKeyStore = pbCustKeyStore + 8; // 8 is lenght of header

  for(i =0; i < uAESKeyNum + uRSAKeyNum; i ++)
  {
    uKeyId = *((UNSG32*)pbCustKeyStore);
    uSize = *((UNSG32*)(pbCustKeyStore + 4));
    if(uKeyId ==0x80 || uKeyId == 0x81 )
    {
      pbCustKeyStore = pbCustKeyStore + 8;

      //lgpl_printf("No.%d AES Customer Key Store Useful Info:\n", uAESKeyCnt + 1);
      //lgpl_printf("Enc_Root_ID = [0x%x], Sign_Key_ID = [0x%x] ", *(pbCustKeyStore + 0x0), *(pbCustKeyStore + 0x48));
      //lgpl_printf("Sign_Type = [0x%x], Sign_Len = [0x%x]\n", *(pbCustKeyStore + 0x4c), *(pbCustKeyStore + 0x4d));
      uAESKeyCnt++;
    }
    else if(uKeyId == 0x82)
    {
      pbCustKeyStore = pbCustKeyStore + 8;

      //lgpl_printf("No.%d RSA Public Customer Key Store Useful Info:\n", uRSAPubKeyCnt + 1);
      //lgpl_printf("Enc_Root_ID = [0x%x], Sign_Key_ID = [0x%x] ", *(pbCustKeyStore + 0x0), *(pbCustKeyStore + 0x48));
      //lgpl_printf("Sign_Type = [0x%x], Sign_Len = [0x%x]\n", *(pbCustKeyStore + 0x4c), *(pbCustKeyStore + 0x4d));
      uRSAPubKeyCnt++;
    }
    else if(uKeyId == 0x8e)
    {
      pbCustKeyStore = pbCustKeyStore + 8;

      //lgpl_printf("No.%d RSA Private Customer Key Store Useful Info:\n", uRSAPrvKeyCnt + 1);
      //lgpl_printf("Enc_Root_ID = [0x%x], Sign_Key_ID = [0x%x] ", *(pbCustKeyStore + 0x0), *(pbCustKeyStore + 0x48));
      //lgpl_printf("Sign_Type = [0x%x], Sign_Len = [0x%x]\n", *(pbCustKeyStore + 0x4c), *(pbCustKeyStore + 0x4d));
      uRSAPrvKeyCnt++;
    }
    else
    {
      //lgpl_printf("Contian Invalid Customer Key ID:0x%x, Pls check the key store file\n", uKeyId);
      return -1;
    }

    pbCustKeyStore = pbCustKeyStore + uSize;

    if(pbCustKeyStore >= (pbCustKeyStore + uKeyStoreSize))
      break;
  }

  *uKeyNum = uAESKeyNum + uRSAKeyNum;

  return 0;
}

static void MV_DRMLIB_Check_ReturnError(UNSG32 uErrorCode)
{
  switch(uErrorCode){
    case DRMDiag_CUSTOMER_KEYSTORE_CTX_uErrorCode_RETURN_OK:
      //lgpl_printf("Customer Key has been set successfully\n");
      break;
    case DRMDiag_CUSTOMER_KEYSTORE_CTX_uErrorCode_INVALID_ENC_KEYID:
      //lgpl_printf("Invalid encryption key id\n");
      break;
    case DRMDiag_CUSTOMER_KEYSTORE_CTX_uErrorCode_INVALID_SIGN_KEYID:
      //lgpl_printf("Invalid signed key id\n");
      break;
    case DRMDiag_CUSTOMER_KEYSTORE_CTX_uErrorCode_AESUNWRAP_FAILED:
      //lgpl_printf("SubKey AES Unwrap Failed\n");
      break;
    case DRMDiag_CUSTOMER_KEYSTORE_CTX_uErrorCode_CMAC_VERIFY_FAILED:
      //lgpl_printf("CMAC valud is not valid\n");
      break;
    default:
      //lgpl_printf("Invalid Return Error Code, Check the Image again, ErrorCode = [0x%x]\n", uErrorCode);
      break;
  }
}

SIGN32 MV_DRMLIB_Load_Customer_Key(UNSG8* pbCustKeyStore, UNSG32 uKeyStoreSize)
{
  UNSG32 i = 0;
  SIGN32 nRet = 0;
  UNSG8* pbSrcDDR = NULL;
  UNSG8* pbDstImg = NULL;
  SIE_DRMROM_CMD  drmCmd;
  SIE_DRMROM_CMD  drmRsp;
  T32SECSTATUS_CFG cfgSecStatus;
  UNSG32 uRspFlag = 0;
  UNSG32 uCmdFlag = 0;
  UNSG32 uErrorCode = 0;
  UNSG32 uKeyNum = 0;
  UNSG32 uKeyId = 0;
  UNSG32 uSize = 0;
  UNSG8 * pbTemp = NULL;

  get_figo_img((char *)gp_cust_figo_image);
  flush_all_dcache();
  nRet = MV_DRMLIB_PreProcess_Customer_KeyStore(pbCustKeyStore, uKeyStoreSize, &uKeyNum);
  if(0 != nRet) {
    lgpl_printf("Warning: Failed to preprocess keystore \n");
    return -1;
  }
  pbCustKeyStore = pbCustKeyStore + 8;    //Skip the key store header: 16 bytes
  uKeyStoreSize  = uKeyStoreSize - 8;

  //!Wait FIGO Ready
  Wait_FIGO();
  //!Prepare the Image
  {
    UNSG8* pbAlloc = NULL;
    UNSG32  uVSize = MEMPOLL_SIZE;
    unsigned int i = 0;
    pbAlloc = (UNSG8*)mempool;
    pbSrcDDR = (UNSG8*)(ALIGN32(pbAlloc) + 32);
    pbTemp = pbSrcDDR;
    for (i = 0; i< uVSize - 64; i = i + 4)
    {
      pbTemp = pbSrcDDR + i;
      *(UNSG32 *)(pbTemp) = 0;
    }
  }
  pbDstImg = pbSrcDDR;
  {
    unsigned int i = 0;
    for (i = 0; i < CUSTK_IMAGE_MAX_SIZE; i++)
      *(pbDstImg + i) = *((UNSG8*)gp_cust_figo_image + i);

  }
  //! Init DTCM
  {
    unsigned int i = 0;
    void* pDiagDtcm = NULL;
    UNSG32 uSize = 0;
    pDiagDtcm = (void *)(ALIGN32(dtcmpool) + 32);
    for(i = 0 ; i< (9688/4); i = i+1 )
      *(((UNSG32*)pDiagDtcm) + i) = 0;

    uSize = 9688 - RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_cmdFlag;
    //lgpl_printf("Before Init the DTCM, the Size = [0x%x]\n", uSize);
    for ( i = 0; i < uSize/4; i +=1 )
    {
      MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_cmdFlag + i*4, 0);
    }
  }
  flush_all_dcache();
  //Load FIGO Image
  {
    unsigned int i = 0;
    for (i = 0; i < sizeof(drmCmd)/8; i = i + 8)
      *((UNSG64 *)&drmCmd + i) = 0;
    cfgSecStatus.u32 = MV_FIGODRV_IO_RD32(DRMFIGOREG_SECSTAT);
    if ( cfgSecStatus.uCFG_flag  != SECSTATUS_CFG_flag_ENABLED )
    {
      return MV_FIGODRV_ERR_INVLAID_ARG;
    }
    //! Check figo command status
    drmCmd.u32DRMROM_CMD_STAT = MV_FIGODRV_IO_RD32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_STAT);
    if ( drmCmd.uSTAT_en )
    {
      return MV_FIGODRV_ERR_INVLAID_STATUS;
    }

    drmCmd.uCMD_CFG_tag     = DRMROM_CMD_TYPE_LD_FIGOIMG;
    drmCmd.uCMD_CFG_nonce   = ~0;

    drmCmd.uCMD_DAT0_crcCmd32   = 0;
    drmCmd.uCMD_DAT1_imgSz      = 0;
    drmCmd.uCMD_DAT2_imgSrcAddr = (UNSG32)pbDstImg;
    drmCmd.uSTAT_en = 1;
    //! Issue command
    MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_CMD_CFG,  drmCmd.u32DRMROM_CMD_CMD_CFG);
    MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_CMD_DAT0, drmCmd.u32DRMROM_CMD_CMD_DAT0);
    MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_CMD_DAT1, drmCmd.u32DRMROM_CMD_CMD_DAT1);
    MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_CMD_DAT2, drmCmd.u32DRMROM_CMD_CMD_DAT2);
    MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_CMD_DAT3, drmCmd.u32DRMROM_CMD_CMD_DAT3);

    MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_STAT,     drmCmd.u32DRMROM_CMD_STAT);
    //! Check figo command status
    while(1)
    {
      drmRsp.u32DRMROM_CMD_STAT = MV_FIGODRV_IO_RD32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_STAT);
      if ( drmRsp.uSTAT_en )
      {
        continue;
      }
      break;
    }
    //! Load Reponse
    drmRsp.u32DRMROM_CMD_RSP_CFG = MV_FIGODRV_IO_RD32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_RSP_CFG);
    drmRsp.u32DRMROM_CMD_RSP_DAT0 = MV_FIGODRV_IO_RD32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_RSP_DAT0);
    drmRsp.u32DRMROM_CMD_RSP_DAT1 = MV_FIGODRV_IO_RD32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_RSP_DAT1);

    if ( drmRsp.uRSP_CFG_tag != DRMROM_CMD_TYPE_LD_FIGOIMG)
    {
      return MV_FIGODRV_ERR;
    }
    //!Todo verify crc32 value with nonce
    uErrorCode = drmRsp.uRSP_DAT1_error;
    if (0 != uErrorCode)
      return MV_FIGODRV_ERR;

  }
  flush_all_dcache();
  // Load the customer keystore into DTCM
  {
    UNSG32 uAESKeyCnt = 0;
    UNSG32 uRSAPubKeyCnt = 0;
    UNSG32 uRSAPrvKeyCnt = 0;
    UNSG32 j = 0;
    for(i =0; i< uKeyNum; i++)
    {
      uKeyId = *(UNSG32*)pbCustKeyStore;      //! First Word is Key ID
      uSize = *((UNSG32*)(pbCustKeyStore + 4));   //! Second word is size
      //printf("%s, %d, key id == 0x%x\n", __FUNCTION__, __LINE__, uKeyId);
      if((uKeyId == 0x80 || uKeyId == 0x81)&& uSize == 0x90)
      {
        uAESKeyCnt++;
#if 0
        pbCustKeyStore = pbCustKeyStore + 8 + uSize;
        continue;
#endif
        lgpl_printf("Load No.%d AES Key into User Key DTCM Area\n", uAESKeyCnt);
      }
      else if(uKeyId == 0x82&& uSize == 0x140)
      {
        uRSAPubKeyCnt++;
        lgpl_printf("Load No.%d RSA Public Key into User Key DTCM Area\n", uRSAPubKeyCnt);
      }
      else if(uKeyId == 0x8e && uSize == 0x180)
      {
        uRSAPrvKeyCnt++;
        lgpl_printf("Load No.%d RSA Private Key into User Key DTCM Area\n", uRSAPrvKeyCnt);
      }
      else
      {
        lgpl_printf("Warning: Invalid key store!Key Id is:0x%x, keystore size is: 0x%x\n", uKeyId, uSize);
        uErrorCode = 0xff;
        break;
      }

      pbCustKeyStore = pbCustKeyStore + 8;
      for(j = 0; j < uSize/4; j++)
      {
        MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_custKeystore + j*4,  *(UNSG32*)(pbCustKeyStore + j*4));
      }
      lgpl_printf("Loading Secure Customer Key Store is finished\n");

      do{
        uRspFlag = MV_FIGODRV_IO_RD32(DRMFIGOREG_CMDSTAT + RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_rspFlag);
        uRspFlag = uRspFlag & 0xffff;
        MV_FIGODRV_SLEEP(1000);
      } while (uRspFlag != 0x3010);
      // Send Command to FIOG image
      {
        MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_uKeyId, uKeyId);
        uCmdFlag = 0xa5;
        MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_cmdFlag, uCmdFlag);
      }

      // Wait the Command is executed
      do{
        uCmdFlag = MV_FIGODRV_IO_RD32(DRMFIGOREG_CMDSTAT + RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_cmdFlag);
        uCmdFlag = uCmdFlag & 0xff;
        MV_FIGODRV_SLEEP(1000);
      } while (uCmdFlag != 0x30);
      uErrorCode = MV_FIGODRV_IO_RD32(DRMFIGOREG_CMDSTAT + BA_DRMDiag_CUSTOMER_KEYSTORE_CTX_uErrorCode);
      uErrorCode = uErrorCode&0xff;
      MV_DRMLIB_Check_ReturnError(uErrorCode);

      if(uErrorCode != 0)
      {
        break;
      }
      //lgpl_printf("******* Load Customer Key store successfully*********\n");
      pbCustKeyStore = pbCustKeyStore + uSize;
    }
  }
  uCmdFlag = 0x5a;
  MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_cmdFlag, uCmdFlag);

  do{
    uRspFlag = MV_FIGODRV_IO_RD32(DRMFIGOREG_CMDSTAT + RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_rspFlag);
    uRspFlag = uRspFlag & 0xffff;
    MV_FIGODRV_SLEEP(1000);
  } while (uRspFlag != 0x88de);

  //  uCmdFlag = 0xf3;
  //  MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_cmdFlag, uCmdFlag);
  lgpl_printf("Finish to load Customer Key store\n");
  uErrorCode = MV_FIGODRV_IO_RD32(DRMFIGOREG_CMDSTAT + BA_DRMDiag_CUSTOMER_KEYSTORE_CTX_uErrorCode);
  uErrorCode = uErrorCode&0xff;

  MV_DRMLIB_Check_ReturnError(uErrorCode);

  if(uErrorCode != 0)
  {
    lgpl_printf("uErrorCode = 0x%x\n", uErrorCode);
    //return -1;
  }

  uCmdFlag = 0xde;
  MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_cmdFlag, uCmdFlag);
  // Wait Figo ROM code ready
  Wait_FIGO();
  lgpl_printf("FIGO is running at FIGO ROM Code agian\n");

  if(uErrorCode != 0)
  {
    return -1;
  }
  return MV_FIGODRV_OK;
}

HRESULT MV_Gen_Random_Server(UINT8* pbOutDat, UINT32*uOutSize )
{
    HRESULT lRes = S_OK;

	UNSG32 i = 0;
	UNSG8* pbSrcDDR = NULL;
	UNSG8* pbDstImg = NULL;
	SIE_DRMROM_CMD	drmCmd;
	SIE_DRMROM_CMD	drmRsp;
	T32SECSTATUS_CFG cfgSecStatus;
	SIE_DRMDiag_CUSTOMER_KEYSTORE_CTX *pDiagDtcm = NULL;
	UNSG32 uRspFlag = 0;
	UNSG32 uCmdFlag = 0;
	UNSG32 uErrorCode = 0;
	UNSG32 uKeyId = 0;
	UNSG32 uOut_data = 0;
	UNSG8 * pbTemp = NULL;
        UINT32 uBufSizeAligned32 = (32 + 31)&(~(32-1));

    if((pbOutDat ==NULL) && (uOutSize == NULL) && (*uOutSize == 0))
    {
        lRes = MV_FIGODRV_ERR_INVLAID_ARG;
        printf("invalid lenght of randon number(0)\n");
        return lRes;
    }
    uBufSizeAligned32 = (*uOutSize + 15)&(~(16-1));
	#if 1
	{
	    get_figo_img((char *)gp_cust_figo_image);
	}
	#endif
	flush_all_dcache();

	//!Wait FIGO Ready
	Wait_FIGO();
	//!Prepare the Image
	{
		UNSG8* pbAlloc = NULL;
		UNSG32	uVSize = MEMPOLL_SIZE;
		unsigned int i = 0;
		pbAlloc = (UNSG8*)mempool;
		pbSrcDDR = (UNSG8*)(ALIGN32(pbAlloc) + 32);
		pbTemp = pbSrcDDR;
		for (i = 0; i< uVSize - 64; i = i + 4)
		{
			pbTemp = pbSrcDDR + i;
			*(UNSG32 *)(pbTemp) = 0;
		}
	}
	pbDstImg = pbSrcDDR;
	{
		unsigned int i = 0;
		for (i = 0; i < CUSTK_IMAGE_MAX_SIZE; i++)
			*(pbDstImg + i) = *((UNSG8*)gp_cust_figo_image + i);

	}
	//! Init DTCM
	{
		unsigned int i = 0;
		SIE_DRMDiag_CUSTOMER_KEYSTORE_CTX* pDiagDtcm = NULL;
		UNSG32 uSize = 0;
		pDiagDtcm = (SIE_DRMDiag_CUSTOMER_KEYSTORE_CTX *)(ALIGN32(dtcmpool) + 32);
		for(i = 0 ; i< sizeof(SIE_DRMDiag_CUSTOMER_KEYSTORE_CTX)/4; i = i+1 )
			*(((UNSG32*)pDiagDtcm) + i) = 0;

		uSize = sizeof(SIE_DRMDiag_CUSTOMER_KEYSTORE_CTX) - RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_cmdFlag;
		//lgpl_printf("Before Init the DTCM, the Size = [0x%x]\n", uSize);
		for ( i = 0; i < uSize/4; i +=1 )
		{
			MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_cmdFlag + i*4, 0);
		}
	}
	flush_all_dcache();
	//Load FIGO Image
	{
		unsigned int i = 0;
		for (i = 0; i < sizeof(drmCmd)/8; i = i + 8)
			*((UNSG64 *)&drmCmd + i) = 0;
		cfgSecStatus.u32 = MV_FIGODRV_IO_RD32(DRMFIGOREG_SECSTAT);
		if ( cfgSecStatus.uCFG_flag  != SECSTATUS_CFG_flag_ENABLED )
		{
			return MV_FIGODRV_ERR_INVLAID_ARG;
		}
		//! Check figo command status
		drmCmd.u32DRMROM_CMD_STAT = MV_FIGODRV_IO_RD32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_STAT);
		if ( drmCmd.uSTAT_en )
		{
			return MV_FIGODRV_ERR_INVLAID_STATUS;
		}

		drmCmd.uCMD_CFG_tag		= DRMROM_CMD_TYPE_LD_FIGOIMG;
		drmCmd.uCMD_CFG_nonce	= ~0;

		drmCmd.uCMD_DAT0_crcCmd32	= 0;
		drmCmd.uCMD_DAT1_imgSz		= 0;
		drmCmd.uCMD_DAT2_imgSrcAddr = (UNSG32)pbDstImg;
		drmCmd.uSTAT_en = 1;
		//! Issue command
		MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_CMD_CFG,	drmCmd.u32DRMROM_CMD_CMD_CFG);
		MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_CMD_DAT0, drmCmd.u32DRMROM_CMD_CMD_DAT0);
		MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_CMD_DAT1, drmCmd.u32DRMROM_CMD_CMD_DAT1);
		MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_CMD_DAT2, drmCmd.u32DRMROM_CMD_CMD_DAT2);
		MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_CMD_DAT3, drmCmd.u32DRMROM_CMD_CMD_DAT3);

		MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_STAT,		drmCmd.u32DRMROM_CMD_STAT);
		//! Check figo command status
		while(1)
		{
			drmRsp.u32DRMROM_CMD_STAT = MV_FIGODRV_IO_RD32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_STAT);
			if ( drmRsp.uSTAT_en )
			{
				continue;
			}
			break;
		}
		//! Load Reponse
		drmRsp.u32DRMROM_CMD_RSP_CFG = MV_FIGODRV_IO_RD32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_RSP_CFG);
		drmRsp.u32DRMROM_CMD_RSP_DAT0 = MV_FIGODRV_IO_RD32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_RSP_DAT0);
		drmRsp.u32DRMROM_CMD_RSP_DAT1 = MV_FIGODRV_IO_RD32(DRMFIGOREG_CMDSTAT + RA_DRMROM_CMD_RSP_DAT1);

		if ( drmRsp.uRSP_CFG_tag != DRMROM_CMD_TYPE_LD_FIGOIMG)
		{
			return MV_FIGODRV_ERR;
		}
		//!Todo verify crc32 value with nonce
		uErrorCode = drmRsp.uRSP_DAT1_error;
		if (0 != uErrorCode)
			return MV_FIGODRV_ERR;

	}
	flush_all_dcache();
	// Load the customer keystore into DTCM
	{
		//for(i =0; i< uKeyNum; i++)
		{
			uKeyId = uBufSizeAligned32;		//! First Word is Key ID

			do{
				uRspFlag = MV_FIGODRV_IO_RD32(DRMFIGOREG_CMDSTAT + RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_rspFlag);
				uRspFlag = uRspFlag & 0xffff;
				MV_FIGODRV_SLEEP(1000);
			} while (uRspFlag != 0x3010);
			// Send Command to FIOG image
			{
				MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_uKeyId, uKeyId);
				uCmdFlag = 0xbf;
				MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_cmdFlag, uCmdFlag);
			}
		}
	}
	do{
		uRspFlag = MV_FIGODRV_IO_RD32(DRMFIGOREG_CMDSTAT + RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_rspFlag);
		uRspFlag = uRspFlag & 0xffff;
		MV_FIGODRV_SLEEP(1000);
	} while (uRspFlag != 0x88de);
	for ( i = 0; i < uBufSizeAligned32/4; i +=1 )
	{
		uOut_data = MV_FIGODRV_IO_RD32(DRMFIGOREG_CMDSTAT + RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_custKeystore + i*4);
		memcpy(((UINT8 *)pDiagDtcm->ie_custKeystore + i*4), &uOut_data, sizeof(uOut_data));
	}
	memcpy(pbOutDat, pDiagDtcm->ie_custKeystore, *uOutSize);
//	uCmdFlag = 0xf3;
//	MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_cmdFlag, uCmdFlag);

	uCmdFlag = 0xde;
	MV_FIGODRV_IO_WR32(DRMFIGOREG_CMDSTAT + RA_DRMDiag_CUSTOMER_KEYSTORE_CTX_cmdFlag, uCmdFlag);
	// Wait Figo ROM code ready
	Wait_FIGO();
	return MV_FIGODRV_OK;
}