// Activate Security
//

// CryptoMemory Library Include Files
#include "CM_LIB.h"
#include "CM_I2C.h"
#include "CM_I2C_L.h"
#include "CM_GPA.h"
#include <stdio.h>

// Local function prototypes
static uint8_t cm_AuthenEncrypt(uint8_t ucCmd1, uint8_t ucAddrCi, uint8_t * pucCi, uint8_t * pucG_Sk, uint8_t * pucRandom);

// Global Data
uint8_t ucCM_Ci[8], ucCM_G_Sk[8];
uint8_t ucCM_Q_Ch[16], ucCM_Ci2[8];

// Activate Security
//
// When called the function:
// ?reads the current cryptogram (Ci) of the key set, 
// ?computes the next cryptogram (Ci+1) based on the secret key pucKey (GCi) and the random number selected,
// ?sends the (Ci+1) and the random number to the CryptoMemory?device, 
// ?computes (Ci+2) and compares its computed value the new cryptogram of the key set.
// ?If (Ci+2) matches the new cryptogram of the key set, authentication was successful.
// In addition, if ucEncrypt is TRUE the function:
// ?computes the new session key (Ci+3) and a challenge, 
// ?sends the new session key and the challenge to the CryptoMemory?device, 
// ?If the new session key and the challenge are correctly related, encryption is activated.
//
uint8_t cm_ActiveSecurity(uint8_t ucKeySet, uint8_t * pucKey, uint8_t * pucRandom, uint8_t ucEncrypt)
{
    uint8_t i;
    uint8_t ucAddrCi;
	uint8_t ucReturn;
    // Read Ci for selected key set
    ucAddrCi = CM_Ci + (ucKeySet<<4);              // Ci blocks on 16 byte boundries
    if ((ucReturn = cm_ReadConfigZone(ucAddrCi, ucCM_Ci, 8)) != SUCCESS)
    {
        printf("---> cm_ActiveSecurity log1 ucReturn %d\n", ucReturn);
    	 return ucReturn;
    }
    // Try to activate authentication
    for (i = 0; i < 8; ++i) 
        ucCM_G_Sk[i] = pucKey[i];

    if ((ucReturn = cm_AuthenEncrypt(ucKeySet, ucAddrCi, ucCM_Ci, ucCM_G_Sk, pucRandom)) != SUCCESS) 
    {
        printf("---> cm_ActiveSecurity log2 ucReturn %d\n", ucReturn);
    	return ucReturn;
    }
    
    ucCM_Authenticate = TRUE;
        
    // If Encryption required, try to activate that too
    if (ucEncrypt) {
        if (pucRandom) pucRandom += 8;
        if ((ucReturn = cm_AuthenEncrypt(ucKeySet+0x10, ucAddrCi, ucCM_Ci, ucCM_G_Sk, pucRandom)) != SUCCESS) 
        {
            printf("---> cm_ActiveSecurity log3 ucReturn %d\n", ucReturn);
        	return ucReturn;
        }
        ucCM_Encrypt = TRUE;
    }
    // Done
    return SUCCESS;
}

// Common code for both activating authentication and encryption
static uint8_t cm_AuthenEncrypt(uint8_t ucCmd1, uint8_t ucAddrCi, uint8_t * pucCi, uint8_t * pucG_Sk, uint8_t * pucRandom)
{
    uint8_t i;
	uint8_t ucReturn;
    
    // Generate chalange data
    if (pucRandom) 
    	for (i = 0; i < 8; ++i) ucCM_Q_Ch[i] = pucRandom[i];
    else           
    	CM_LOW_LEVEL.RandomGen(ucCM_Q_Ch);

    for (i = 0; i < 8; ++i) 
    	ucCM_Q_Ch[i] = pucRandom[i];
    cm_AuthenEncryptCal(pucCi, pucG_Sk, ucCM_Q_Ch, &ucCM_Q_Ch[8]);
    
    // Send chalange
    ucCM_InsBuff[0] = 0xb8;
    ucCM_InsBuff[1] = ucCmd1;
    ucCM_InsBuff[2] = 0x00;
    ucCM_InsBuff[3] = 0x10;
    if ((ucReturn = cm_WriteCommand(ucCM_InsBuff, ucCM_Q_Ch, 16)) != SUCCESS) 
    {
        printf("cm_AuthenEncrypt log1 ucReturn %d\n", ucReturn);
    	return ucReturn;
    }
                    
    // Give chips some clocks to do calculations
    CM_LOW_LEVEL.WaitClock(30); //3
                             
    // Verify result
    if ((ucReturn = cm_ReadConfigZone(ucAddrCi, ucCM_Ci2, 8)) != SUCCESS)
    {
        printf("cm_AuthenEncrypt log2 ucReturn %d\n", ucReturn);
    	return ucReturn;
    }

    // for(i=0;i<8;i++)
    // {
    //     printf("i=%d, %#x : %#x\n", i, pucCi[i], ucCM_Ci2[i]);
    // }
    
    for(i=0; i<8; i++) 
    {
        
        if (pucCi[i]!=ucCM_Ci2[i]) 
        {
        	return FAILED;
        }
    }
    
    // Done
    return SUCCESS;
}