#include "general.h"
#include "typedefs.h"
#include "vsi.h"
#include <stdlib.h>
#include <string.h>
#include "sec_drv.h"
#include "sec_drv_prim.h"

#ifdef FIRSTBOOT_ENABLED
#include "sec_firstboot.h"
#endif


#define UNLOCKED(status) ((status==SEC_DRV_CAT_STAT_Unlocked)||(status==SEC_DRV_CAT_STAT_PermUnlocked))


/**
 * Resets the phone.
 */
static void reset(void)
{
    UINT16 i;
    
    TRACE("** RESET **");
    // Setup watchdog timer and let it timeout to make a reset
    *(volatile UINT16*) 0xfffff804 = 0xFFFF;  // Timer to watchdog
    *(volatile UINT16*) 0xfffff800 = 0x0080;  // Start timer
    // Apparently works it only if we read this register?
    i = *(volatile UINT16*) 0xfffff802;
    *(volatile UINT16*) 0xfffff802 = 0x0001;  // Load timer
}


/**
 * Check whether or not we have valid lock data in the flash-area
 * Run first_boot if we've got data, but its not initialized.
 *
 * @param pNumCategories Pointer to UINT8 where the number of
 *                         categories will be stored.
 * @return SEC_DRV_RET_Ok if theres valid data in the secure area.
 */
static T_SEC_DRV_RETURN check_hdr(UINT8 *pNumCategories)
{
    T_SEC_DRV_RETURN result = SEC_DRV_RET_Ok;
    T_SEC_DRV_GLOBAL_CONF hdr;
    
    TRACE("check_hdr");
    if (sec_prim_get_global_conf(&hdr))
    {
        switch (hdr.firstboot_pattern)
        {
            case SEC_PATTERN_INITIALIZED:
                /* Secure area seems to be initialized OK */
                result = SEC_DRV_RET_Ok;
                if (pNumCategories) 
                {
                    T_SEC_DRV_CONFIGURATION conf;
                    sec_prim_get_configuration(&conf);
                    *pNumCategories = conf.NumCategories;
                }
                TRACE("check_hdr - OK");
                break;
            case SEC_PATTERN_UNINITIALIZED:
                /* We've got new data in the secure area that needs to be processed */
#ifdef FIRSTBOOT_ENABLED
                first_boot();
#endif
                result = SEC_DRV_RET_NotPresent;
                TRACE("check_hdr - uninitialized");
                reset();
                break;
            default:
                /* Secure area seems to contain void data! */
                result = SEC_DRV_RET_NotPresent;
                if (pNumCategories) *pNumCategories = 0;
                TRACE("check_hdr - void data");
                break;
        }
    }
    else
    {
        result = SEC_DRV_RET_NotPresent;
        TRACE("check_hdr - void!!!");
    }
    return result;
}


/**
 * Check if a counter is exceeded.
 *
 * @param pCount Pointer to current value - max value must follow immediately before!
 * @return TRUE if the failure counter has been exceeded
 */
static BOOL Counter_Exceeded(UINT8 *pCount)
{
    pCount--;
    return ((pCount[0] != 0xFF) && (pCount[1] >= pCount[0]));
}


/** 
 * Increment a counter if needed.
 *
 * @param pCount Pointer to the counter
 */
static void Counter_Increment(UINT8 *pCount)
{
    if ((*pCount) < 0xFF)
    {
        (*pCount)++;
    }
}


/**
 * Check whether or not a given key-len is ok according to its category.
 *
 * @param rec_num The record number to check the key_length against.
 * @param key_len The key length to check.
 * @return TRUE if the key_len is within specifications for the given
 *           record (category) - otherwise FALSE.
 */
static BOOL check_key_len(int rec_num, UINT8 key_len)
{
    if (key_len > SEC_DRV_KEY_MAX_LEN) return FALSE;
    if ((rec_num == SEC_DRV_CAT_NUM_SIM) && (key_len < 6)) return FALSE;
    if ((rec_num != SEC_DRV_CAT_NUM_SIM) && (key_len < 8)) return FALSE;
    return TRUE;
}


/**
 * Transform an unlock key into a special lock key - for use by the
 * non-ETSI lock algorithm.
 *
 * @param pKey     Pointer to a key to transform. This also serves as the output
 *                buffer. Must be '\0' terminated if shorter than SEC_DRV_KEY_MAX_LEN
 */
void calculate_spec_lock_key(char *pKey)
{
    int len=0;
    int index;
    
    while((len<SEC_DRV_KEY_MAX_LEN) && (pKey[len])) len++;
    len = len & 0xFE;
    for(index=0; index<len; index+=2)
    {
        char ch;
        ch = pKey[index];
        pKey[index] = pKey[index+1];
        pKey[index+1] = ch;
    }
}


/**
 * Compare 2 keys up to key_len characters, given the length constraints implied
 * by the category number and check_key_len().
 *
 * @param rec_num The category number to use when validating key_len.
 * @param pRefKey Pointer to the key to use as reference (origin).
 * @param pKey      Pointer to the key to compare against pRefKey.
 * @param key_len The maximum number of chars to use in the compare or 0 (to use all).
 * @return SEC_DRV_RET_KeyWrong if the length constraints are violated, SEC_DRV_RET_Ok
 *           if the keys match, otherwise SEC_DRV_RET_KeyMismatch.
 */
static T_SEC_DRV_RETURN compare_keys(int rec_num, const char *pRefKey, const char *pKey, UINT8 key_len)
{
    if (pRefKey[0] == '\0') 
    {
        TRACE("compare_keys - old key not present");
        return SEC_DRV_RET_Ok;
    }
    
    if (key_len == 0)
    {
        /* compare using full length of the keys ->
            to '\0' termination or max SEC_DRV_KEY_MAX_LEN chars... 
            whichever comes first */
        key_len = SEC_DRV_KEY_MAX_LEN;
    }
    /* compare using the specified number of chars */
    if (!check_key_len(rec_num, key_len) || (pKey==0L) || (pKey[0]=='\0')) return SEC_DRV_RET_KeyWrong;
    return (strncmp(pRefKey, pKey, key_len)==0)? SEC_DRV_RET_Ok : SEC_DRV_RET_KeyMismatch;
}


/**
 * Try to set a new key on a specific category. For this to happen, the new key must
 * meet the length constraints implied by the category, and the current category key
 * must match the given oldkey within the key_len chars (again, remember length
 * constraints). 
 *
 * @param rec_num The category number for which to set the key.
 * @param pOldKey Key to compare against current category key. The 2 must match
 *                  for the new key to be set.
 * @param pNewKey Key to set the category to, if possible.
 * @param key_len Length to use when comparing keys, see check_key_len().
 * @return SEC_DRV_RET_Ok if the key could be set.
 */
static T_SEC_DRV_RETURN set_key(T_SEC_DRV_CONFIGURATION *pConf, int rec_num, const char *pOldKey, const char *pNewKey, UINT8 key_len)
{
    T_SEC_DRV_RETURN result;
    T_SEC_DRV_KEY key;
    if (sec_prim_get_key(rec_num, &key))
    {
        result = compare_keys(rec_num, (char *)key.digit, pOldKey, key_len);
        if (result == SEC_DRV_RET_Ok)
        {
            int len = strlen(pNewKey);
            if (check_key_len(rec_num, len))
            {
                len++; /* convert len into a size! */
                if (len > SEC_DRV_KEY_MAX_LEN) len = SEC_DRV_KEY_MAX_LEN;
                memcpy(key.digit, pNewKey, len);
                sec_prim_set_key(rec_num, &key);
            }
            else
            {
                /* key must be within specified length according to category */
                result = SEC_DRV_RET_KeyWrong;
            }
        }
        else
        {
            Counter_Increment(&pConf->FC_Current);
            sec_prim_set_configuration(pConf);
        }
    }
    else
    {
        result = SEC_DRV_RET_NotPresent;
    }
    return result;
}


/**
 * Update all dependant categories. Scanning through a categories dependants,
 * set their status and key if needed.
 * 
 * @param pCatHdr Pointer to the category header for the parent category.
 * @param pCatKey Pointer to the key for the parent category.
 */
static void update_dependants(const T_SEC_DRV_CAT_HDR *pCatHdr, const T_SEC_DRV_KEY *pCatKey, UINT16 dependMask)
{
    int dependentCat = 0;
    int dependentBit = 1;
    
    for (; dependentCat<(sizeof(pCatHdr->Dependency)*8); dependentCat++)
    {
        if (pCatHdr->Dependency & dependentBit & dependMask)
        {
            {
                /* set key on dependant iff not already done, and parent has a key */
                T_SEC_DRV_KEY dependentKey;
                if (sec_prim_get_key(dependentCat, &dependentKey))
                {
                    if (pCatKey->digit[0] != '\0' && dependentKey.digit[0] == '\0')
                    {
                        sec_prim_set_key(dependentCat, pCatKey);
                    }
                }
            }
            {
                /* update status on dependant */
                T_SEC_DRV_CAT_HDR dependentHdr;
                if (sec_prim_get_cat_header(dependentCat, &dependentHdr))
                {
                    if (dependentHdr.Status != SEC_DRV_CAT_STAT_PermUnlocked)
                    {
                        dependentHdr.Status = pCatHdr->Status;
                        sec_prim_set_cat_header(dependentCat, &dependentHdr);
                    }
                }
            }
        }
        dependentBit = dependentBit<<1;
    }
}


/**
 * Get the MEPD configuration.
 *
 * @param ppConfiguration Pointer to pointer where the configuration is stored.
 * @return SEC_DRV_RET_Ok if the configuration could be read.
 */
T_SEC_DRV_RETURN sec_get_CFG(T_SEC_DRV_CONFIGURATION **ppConfiguration)
{
    T_SEC_DRV_RETURN result = check_hdr(0L);
    if (result == SEC_DRV_RET_Ok)
    {
        *ppConfiguration = (T_SEC_DRV_CONFIGURATION *)M_ALLOC(sizeof(T_SEC_DRV_CONFIGURATION));
        sec_prim_get_configuration(*ppConfiguration);
    }
    else
    {
        *ppConfiguration = 0L;
    }
    return result;
}


/**
 * Compare a key against the one stored for a given category.
 *
 * @param rec_num The category number for which to check the key.
 * @param pKey      The key to compare.
 * @param key_len The length of the keys to use in the compare. Subject to constraints.
 * @return SEC_DRV_RET_Ok if the keys match
 */
T_SEC_DRV_RETURN sec_cmp_KEY (UINT8 rec_num, const char *pKey, UINT8 key_len)
{
    T_SEC_DRV_KEY refKey;
    T_SEC_DRV_CONFIGURATION conf;
    UINT8 numCategories=0;
    T_SEC_DRV_RETURN result = check_hdr(&numCategories);
    if (result != SEC_DRV_RET_Ok)
    {
        /* data not present in secure area! */
        return result;
    }

    sec_prim_get_configuration(&conf);
    if (Counter_Exceeded(&conf.FC_Current))
    {
        result = SEC_DRV_RET_FCExeeded;
    }
    else if (conf.Flags & SEC_DRV_HDR_FLAG_LAM_Unlock)
    {
        result = SEC_DRV_RET_Unknown;
    }
    else if (sec_prim_get_key(rec_num, &refKey))
    {
        result = compare_keys(rec_num, (char *)refKey.digit, pKey, key_len);
    }
    else 
    {
        /* requested data outside configured categories! */
        result = SEC_DRV_RET_NotPresent;
    }
    return result;
}


/**
 * Try to set a key for a given category. The category must be unlocked prior
 * to this attempt.
 *
 * @param rec_num Category number whoose key should be set.
 * @param pOldKey pointer to current key of the category (or rather what the 
 *                   client believes to be the current key).
 * @param pNewKey Pointer to the key that should be set.
 * @param key_len length to use during key-comparision.
 * @return SEC_DRV_RET_Ok if the key could be set.
 */
T_SEC_DRV_RETURN sec_set_KEY (UINT8 rec_num, const char *pOldKey, const char *pNewKey, UINT8 key_len)
{
    T_SEC_DRV_CONFIGURATION conf;
    UINT8 numCategories=0;
    T_SEC_DRV_CAT_HDR catHdr;
    T_SEC_DRV_RETURN result = check_hdr(&numCategories);
    if (result != SEC_DRV_RET_Ok)
    {
        /* data not present in secure area! */
        return result;
    }
    
    sec_prim_get_configuration(&conf);
    if (Counter_Exceeded(&conf.FC_Current))
    {
        result = SEC_DRV_RET_FCExeeded;
    }
    else if (sec_prim_get_cat_header(rec_num, &catHdr))
    {
        if (UNLOCKED(catHdr.Status) && !(conf.Flags & SEC_DRV_HDR_FLAG_LAM_Unlock))
        {
            result = set_key(&conf, rec_num, pOldKey, pNewKey, key_len);
        }
        else
        {
            /* category must be unlocked prior to setting a key */
            result = SEC_DRV_RET_Unknown;
        }
    }
    else
    {
        /* requested data outside configured categories! */
        result = SEC_DRV_RET_NotPresent;
    }
    return result;
}


/**
 * Set the key for tha failure counter.
 * 
 * @param pOldKey The current failure count key. Must match that stored in the
 *                  secure area for the new key to be set.
 * @param pNewKey The new key to set for the failure counter.
 * @return SEC_DRV_RET_Ok if the key could be set.
 */
T_SEC_DRV_RETURN sec_set_FC_KEY (const char *pOldKey, const char *pNewKey)
{
    T_SEC_DRV_CONFIGURATION conf;
    T_SEC_DRV_RETURN result = check_hdr(0L);
    if (result != SEC_DRV_RET_Ok)
    {
        /* data not present in secure area! */
        return result;
    }
    
    sec_prim_get_configuration(&conf);
    if (Counter_Exceeded(&conf.FC_Current))
    {
        result = SEC_DRV_RET_FCExeeded;
    }
    else
    {
        result = set_key(&conf, -1, pOldKey, pNewKey, 0);
    }
    return result;
}


/**
 * Get the record data (both header and body) for a given category.
 *
 * @param rec_num    The category number to get the data for.
 * @param ppCategory Pointer to where the result pointer should be stored.
 * @return SEC_DRV_RET_Ok if the record data could be read.
 */
T_SEC_DRV_RETURN sec_get_REC (UINT8 rec_num, T_SEC_DRV_CATEGORY **ppCategory)
{
    UINT8 numCategories=0;
    T_SEC_DRV_RETURN result = check_hdr(&numCategories);
    if (result != SEC_DRV_RET_Ok)
    {
        /* data not present in secure area! */
        *ppCategory = 0L;
        return result;
    }
    
    if (rec_num < numCategories)
    {
        *ppCategory = (T_SEC_DRV_CATEGORY *)M_ALLOC(sizeof(T_SEC_DRV_CATEGORY));
        sec_prim_get_cat_header(rec_num, &(*ppCategory)->Header);
        if ((*ppCategory)->Header.DataLen)
        {
            (*ppCategory)->pBody = M_ALLOC((*ppCategory)->Header.DataLen);
            sec_prim_get_cat_body(rec_num, (*ppCategory)->pBody, (*ppCategory)->Header.DataLen);
        }
        else
        {
            (*ppCategory)->pBody = 0L;
        }
        result = SEC_DRV_RET_Ok;
    }
    else
    {
        /* requested data outside configured categories! */
        *ppCategory = 0L;
        result = SEC_DRV_RET_NotPresent;
    }
    return result;
}


/**
 * Set the body part of a category's data. The data can only be set if the 
 * category is unlocked.
 * 
 * @param rec_num Category number whose body part should be set.
 * @param pBody   Pointer to the body data that should be stored.
 * @return SEC_DRV_RET_Ok if the data could be stored.
 */
T_SEC_DRV_RETURN sec_set_REC (UINT8 rec_num, const T_SEC_DRV_CATEGORY *pCategory)
{
    T_SEC_DRV_CAT_HDR header;
    UINT8 numCategories=0;
    T_SEC_DRV_RETURN result = check_hdr(&numCategories);
    if (result != SEC_DRV_RET_Ok)
    {
        /* data not present in secure area! */
        return result;
    }
    
    if (sec_prim_get_cat_header(rec_num, &header))
    {
        if (header.Status == SEC_DRV_CAT_STAT_Unlocked ||
            header.Status == SEC_DRV_CAT_STAT_PermUnlocked)
        {
            sec_prim_set_cat_body(rec_num, pCategory->pBody, pCategory->Header.DataLen);
            result = SEC_DRV_RET_Ok;
        }
        else
        {
            /* Category must be unlocked in order to write record data */
            result = SEC_DRV_RET_Unknown;
        }
    }
    else
    {
        /* requested data outside configured categories! */
        result = SEC_DRV_RET_NotPresent;
    }
    return result;
}


/**
 * Try to unlock a category. Failure count must not have been exeeded, NAM unlock
 * must not be set and the category must not be linklocked. Also the given key
 * must match the stored key for the category before it is unlocked.
 * 
 * @param rec_num Category number that should be unlocked.
 * @param pKey    Key that should match the stored category key.
 * @param key_len The maximum number of chars to use in the compare or 0 (to use all).
 * @return SEC_DRV_RET_Ok if the category could be unlocked.
 */
T_SEC_DRV_RETURN sec_rec_Unlock (UINT8 rec_num, T_SEC_DRV_UNLOCK_TYPE unlockType, const char *pKey, UINT8 key_len, UINT16 dependMask)
{
    T_SEC_DRV_CONFIGURATION conf;
    T_SEC_DRV_CAT_HDR catHdr;
    UINT8 numCategories=0;
    T_SEC_DRV_RETURN result = check_hdr(&numCategories);
    if (result != SEC_DRV_RET_Ok)
    {
        /* data not present in secure area! */
        return result;
    }
    
    sec_prim_get_configuration(&conf);
    if (Counter_Exceeded(&conf.FC_Current))
    {
        result = SEC_DRV_RET_FCExeeded;
    }
    else if (sec_prim_get_cat_header(rec_num, &catHdr))
    {
        if (UNLOCKED(catHdr.Status) ||
            (conf.Flags & SEC_DRV_HDR_FLAG_LAM_Unlock) ||
            (catHdr.Flags & SEC_DRV_CAT_FLAG_LinkLocked))
        {
            /* NAM or LinkLock failure */
            result = SEC_DRV_RET_Unknown;
        }
        else
        {
            T_SEC_DRV_KEY catKey;
            sec_prim_get_key(rec_num, &catKey);
            result = compare_keys(rec_num, (char *)catKey.digit, pKey, key_len);
            if (result == SEC_DRV_RET_Ok)
            {
                /* update status */
                catHdr.Status = (unlockType == TEMPORARY_UNLOCK)? 
                    SEC_DRV_CAT_STAT_Unlocked : 
                    SEC_DRV_CAT_STAT_PermUnlocked;
                sec_prim_set_cat_header(rec_num, &catHdr);
                /* update dependants */
                update_dependants(&catHdr, &catKey, dependMask);
                /* reset failure counter */
                conf.FC_Current = 0;
                sec_prim_set_configuration(&conf);
            }
            else
            {
                /* Failure comparing keys! -
                   update failure counter */
                Counter_Increment(&conf.FC_Current);
                sec_prim_set_configuration(&conf);
            }
        }
    }
    else
    {
        /* requested data outside configured categories! */
        result = SEC_DRV_RET_NotPresent;
    }
    return result;
}


static T_SEC_DRV_RETURN lock_record(
    UINT8 rec_num,
    T_SEC_DRV_CAT_HDR *catHdr,
    UINT16 dependMask)
{
    T_SEC_DRV_KEY catKey;
    catHdr->Status = SEC_DRV_CAT_STAT_Locked;
    sec_prim_get_key(rec_num, &catKey);
    sec_prim_set_cat_header(rec_num, catHdr);
    update_dependants(catHdr, &catKey, dependMask);
    return SEC_DRV_RET_Ok;
}

                
/**
 * Try to lock a category. Actual algorith depends on flags set for the individual
 * category.
 * 
 * @param rec_num Category number that should be locked.
 * @param pKey    Key that should match (or be set in) the stored category key.
 * @param key_len The maximum number of chars to use in the compare or 0 (to use all).
 * @return SEC_DRV_RET_Ok if the category could be locked.
 */
T_SEC_DRV_RETURN sec_rec_Lock (UINT8 rec_num, const char *pKey, UINT8 key_len, UINT16 dependMask)
{
    T_SEC_DRV_CAT_HDR catHdr;
    UINT8 numCategories=0;
    T_SEC_DRV_RETURN result = check_hdr(&numCategories);
    if (result != SEC_DRV_RET_Ok)
    {
        /* data not present in secure area! */
        return result;
    }
    
    if (sec_prim_get_cat_header(rec_num, &catHdr))
    {
        if (catHdr.Status != SEC_DRV_CAT_STAT_Unlocked)
        {
            /* Status must be unlocked before locking! */
            result = SEC_DRV_RET_Unknown;
        }
        else
        {
            if (rec_num == SEC_DRV_CAT_NUM_AP)
            {
                result = lock_record(rec_num, &catHdr, dependMask);
            }
            else
            {
                T_SEC_DRV_CONFIGURATION conf;
                sec_prim_get_configuration(&conf);
                if (conf.Flags & SEC_DRV_HDR_FLAG_ETSI_Flag)
                {
                    /* ETSI Mode - set the category key */
                    int len = strlen(pKey);
                    if (check_key_len(rec_num, len))
                    {
                        T_SEC_DRV_KEY key;
                        memset(key.digit, 0, SEC_DRV_KEY_MAX_LEN);
                        memcpy(key.digit, pKey, len);
                        sec_prim_set_key(rec_num, &key);
                        result = lock_record(rec_num, &catHdr, dependMask);
                    }
                    else
                    {
                        /* Key has wrong length! */
                        result = SEC_DRV_RET_KeyWrong;
                    }
                }
                else if (conf.Flags & SEC_DRV_HDR_FLAG_LAM_Unlock)
                {
                    /* Non-ETSI, but LAM_unlock is set */
                    result = SEC_DRV_RET_Unknown;
                }
                else
                {
                    /* Non-ETSI mode */
                    T_SEC_DRV_KEY key;
                    sec_prim_get_key(rec_num, &key);
                    if (conf.Flags & SEC_DRV_HDR_FLAG_Spec_Lock_Key)
                    {
                        /* Special lock key enabled */
                        calculate_spec_lock_key((char *)&key.digit[0]);
                    }
                    result = compare_keys(rec_num, (char *)key.digit, pKey, key_len);
                    if (result == SEC_DRV_RET_Ok)
                    {
                        result = lock_record(rec_num, &catHdr, dependMask);
                    }
                }
            }
        }
    }
    else
    {
        /* requested data outside configured categories! */
        result = SEC_DRV_RET_NotPresent;
    }
    return result;
}


/**
 * reset the failure counter. The correct key must of course be given.
 *
 * @param pKey The key used to try to reset the failure counter.
 * @key_len    Length of the key to use.
 * @return SEC_DRV_RET_Ok if the failure counter could be reset.
 */
T_SEC_DRV_RETURN sec_FC_Reset (const char *pKey, UINT8 key_len)
{
    T_SEC_DRV_KEY refKey;
    T_SEC_DRV_CONFIGURATION conf;
    T_SEC_DRV_RETURN result = check_hdr(0L);
    if (result != SEC_DRV_RET_Ok)
    {
        /* data not present in secure area! */
        return result;
    }
    
    sec_prim_get_configuration(&conf);
    if (Counter_Exceeded(&conf.FC_Reset_Fail_Current) || 
        Counter_Exceeded(&conf.FC_Reset_Success_Current))
    {
        result = SEC_DRV_RET_FCExeeded;
    }
    else
    {
        sec_prim_get_key(-1, &refKey);
        result = compare_keys(-1, (char *)refKey.digit, pKey, key_len);
        if (result == SEC_DRV_RET_Ok)
        {
            Counter_Increment(&conf.FC_Reset_Success_Current);
            conf.FC_Current = 0;
        }
        else
        {
            Counter_Increment(&conf.FC_Reset_Fail_Current);
        }
        sec_prim_set_configuration(&conf);
    }
    return result;
}


T_SEC_DRV_RETURN sec_FC_Increment(void)
{
    T_SEC_DRV_RETURN result = SEC_DRV_RET_Ok;
    T_SEC_DRV_CONFIGURATION conf;
    
    sec_prim_get_configuration(&conf);
    if (Counter_Exceeded(&conf.FC_Current))
    {
        result = SEC_DRV_RET_FCExeeded;
    }
    else
    {
        Counter_Increment(&conf.FC_Current);
        sec_prim_set_configuration(&conf);
    }
    return result;
}