//------------------------------------------------------------------------------
// File:          des_hardware.c
// Function:      Firmware encryption using DES hardware acceleration
// Supported chip(s):
//    - AT91SAM7XC128
//    - AT91SAM7XC256
// Supported toolchain(s):
//    - IAR Embedded Workbench
// Date created:  09 June 2006
// Created by:    JJo
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// Includes
//------------------------------------------------------------------------------

#include "des_hardware.h"

#if defined(USE_ENCRYPTION) && defined(ENCRYPTION_3DES_HARD)

//------------------------------------------------------------------------------
// Global variables
//------------------------------------------------------------------------------

#ifdef ENCRYPTION_CTR
  unsigned char CTR[ENCRYPTION_BLOCK_LENGTH];
#endif

//------------------------------------------------------------------------------
// Inline functions
//------------------------------------------------------------------------------

/**
 * Name:     ASCII2Hex
 * Purpose:  Converts an ASCII value to an hexadecimal one
 * Input(s):
 *  - ASCII string
 *  - Buffer to store integer value
 *  - Length of string
 */
static inline void ASCII2Hex(const unsigned char * ascii, unsigned char * binary, unsigned int length) {
  
  for (unsigned int i=0; i < length; i++) {

    if (ascii[i*2] >= 'A') {

      binary[i] = ascii[i*2] - 'A' + 10;
    }
    else {

      binary[i] = ascii[i*2] - '0';
    }

    binary[i] <<= 4;
  
    if (ascii[i*2+1] >= 'A') {

      binary[i] += ascii[i*2+1] - 'A' + 10;
    }
    else {

      binary[i] += ascii[i*2+1] - '0';
    }
  }
}

//------------------------------------------------------------------------------
// Functions
//------------------------------------------------------------------------------

/**
 * Name:     des_hard_init
 * Purpose:  Initializes the AES peripheral
 */
void des_hard_init(void) {
  
  // Local variables
  unsigned char key[ENCRYPTION_KEY_LENGTH];
  
  debug_printf("DES/HARD: Initializing ...\n");
  
  // Activate peripheral clock
  AT91F_TDES_CfgPMC();
  
  // Load mode
#if (ENCRYPTION_KEY_LENGTH == 24)
  AT91F_TDES_CfgModeReg(AT91C_BASE_TDES, AT91C_TDES_SMOD_MANUAL | TDES_MODE |
                        AT91C_TDES_TDESMOD | TDES_CIPHER);
#else
  AT91F_TDES_CfgModeReg(AT91C_BASE_TDES, AT91C_TDES_SMOD_MANUAL | TDES_MODE |
                        AT91C_TDES_KEYMOD | AT91C_TDES_TDESMOD | TDES_CIPHER);
#endif
  
  // Convert and load key
  ASCII2Hex(ENCRYPTION_KEY, key, ENCRYPTION_KEY_LENGTH);
  
  AT91C_BASE_TDES->TDES_KEY1WxR[0] = ((int *) key)[0];
  AT91C_BASE_TDES->TDES_KEY1WxR[1] = ((int *) key)[1];
  AT91C_BASE_TDES->TDES_KEY2WxR[0] = ((int *) key)[2];
  AT91C_BASE_TDES->TDES_KEY2WxR[1] = ((int *) key)[3];
  
#if (ENCRYPTION_KEY_LENGTH == 24)
  AT91C_BASE_TDES->TDES_KEY3WxR[0] = ((int *) key)[4];
  AT91C_BASE_TDES->TDES_KEY3WxR[1] = ((int *) key)[5];
#endif

#if defined(ENCRYPTION_CBC)
  unsigned char IV[8];
  ASCII2Hex(ENCRYPTION_IV, IV, ENCRYPTION_BLOCK_LENGTH);
  
  AT91C_BASE_TDES->TDES_IVxR[0] = ((int *) IV)[0];
  AT91C_BASE_TDES->TDES_IVxR[1] = ((int *) IV)[1];
  
#elif defined(ENCRYPTION_CTR)
  // Convert IV
  ASCII2Hex(ENCRYPTION_IV, CTR, ENCRYPTION_BLOCK_LENGTH);
#endif
  
  debug_printf("DES/HARD: Initialization done.\n");
}

/**
 * Name:     des_hard_cleanup
 * Purpose:  Cleans up the AES peripheral
 */
void des_hard_cleanup(void) {
  
  debug_printf("DES/HARD: Cleaning up ...\n");
  
  AT91F_TDES_CfgModeReg(AT91C_BASE_TDES, 0);
  AT91F_PMC_DisablePeriphClock(AT91C_BASE_PMC, 1 << AT91C_ID_TDES);
  
  debug_printf("DES/HARD: Cleanup done.\n");
}

/**
 * Name:     des_hard_decrypt
 * Purpose:  Decrypts a variable-length cipher text
 * Input(s):
 *  - Cipher text to decrypt
 *  - Buffer to store plain text
 *  - Length of cipher text (in bytes)
 */
int des_hard_decrypt(const unsigned char * cipherText,
                     unsigned char * plainText,
                     unsigned int length) {

#if defined(ENCRYPTION_ECB) || defined(ENCRYPTION_CBC)
  for (unsigned int l=0; l < length;) {
 
    // Load counter and encrypt it
    AT91C_BASE_TDES->TDES_IDATAxR[0] = ((int *) &cipherText[l])[0];
    AT91C_BASE_TDES->TDES_IDATAxR[1] = ((int *) &cipherText[l])[1];
    
    AT91F_TDES_StartProcessing(AT91C_BASE_TDES);
    while (!AT91F_TDES_IsStatusSet(AT91C_BASE_TDES, AT91C_TDES_DATRDY));
    
    ((int *) &plainText[l])[0] = AT91C_BASE_TDES->TDES_ODATAxR[0];
    ((int *) &plainText[l])[1] = AT91C_BASE_TDES->TDES_ODATAxR[1];
    
    l += ENCRYPTION_BLOCK_LENGTH;
  }
  
#elif defined(ENCRYPTION_CTR)
  // Local variables
  unsigned int bytes;
  unsigned char block[ENCRYPTION_BLOCK_LENGTH];
  
  // Decrypt
  for (unsigned int e=0; e < length;) {
   
    // Load counter and encrypt it
    AT91C_BASE_TDES->TDES_IDATAxR[0] = ((int *) CTR)[0];
    AT91C_BASE_TDES->TDES_IDATAxR[1] = ((int *) CTR)[1];
    
    AT91F_TDES_StartProcessing(AT91C_BASE_TDES);
    while (!AT91F_TDES_IsStatusSet(AT91C_BASE_TDES, AT91C_TDES_DATRDY));
    
    ((int *) block)[0] = AT91C_BASE_TDES->TDES_ODATAxR[0];
    ((int *) block)[1] = AT91C_BASE_TDES->TDES_ODATAxR[1];
    
    // XOR current plain text block with encrypted counter
    if ((length-e) < ENCRYPTION_BLOCK_LENGTH) {
      
      bytes = length - e;
    }
    else {
      
      bytes = ENCRYPTION_BLOCK_LENGTH;
    }
    
    for (unsigned int i=0; i < bytes; i++) {
     
      plainText[e+i] = cipherText[e+i] ^ ((char *) block)[i];
    }

    // Increment counter (big-endian) and number of encrypted bytes
    for (int i=ENCRYPTION_BLOCK_LENGTH-1; i >= 0; i--) {
        
      if (++((char *) CTR)[i] != 0) {
        
        break;
      }
    }
    
    e += bytes;
  }
#endif // ENCRYPTION_CTR
  
  return OK;
}

#endif // defined(USE_ENCRYPTION) && defined(ENCRYPTION_3DES_HARD)