#ifdef SCP11_ENABLE
#include "SCP11.h"
#include "tz_debug.h"

int32_t scp11_parseBERTLV(uint8_t isContainValue, uint8_t* data, uint32_t* offset, uint32_t dataLen, uint32_t tag, tlv_t* pTarget) {
    uint32_t curIndex = *offset;
    uint8_t tag_len = 0, len_len = 0;
    uint32_t valueLen = 0;
    uint8_t temp = 0;
    uint32_t tTag = 0;

    if (data == NULL || dataLen < 1){
        LOGE("ERROR : WRONG BERTLV data. data is NULL or empty : %u", dataLen);
        return -1;
    }
    // check tag
    if (tag <= 255) { // 1 byte tag
        tTag = data[curIndex++] & 0xff;
        tag_len = 1;
    } else  { // 2 bytes tag
        if (curIndex >= dataLen) {
            LOGE("ERROR length : WRONG BERTLV data. curIndex=0x%x, dataLen=0x%x", curIndex, dataLen);
            return -1;
        }
        tTag = data[curIndex++] & 0xff;
        tTag = (tTag << 8) | (data[curIndex++] & 0xff);
        tag_len = 2;
    }
    if ( (tTag & 0xFFFF) != tag) {
        LOGE("ERROR : WRONG TAG %04X : %04X", tag, tTag);
        return -2;
    }
    LOGD("TAG : %04X", tag);
    /* length */
    if (curIndex >= dataLen) {
        LOGE("ERROR length : WRONG BERTLV data. curIndex=0x%x, dataLen=0x%x", curIndex, dataLen);
        return -1;
    }

    temp = data[curIndex++] & 0xff;
    if (temp < 0x80) {
        valueLen = temp;
        len_len = 1;
    } else if (temp == 0x81) {
        if (curIndex >= dataLen) {
            LOGE("ERROR length : WRONG BERTLV data. curIndex=0x%x, dataLen=0x%x", curIndex, dataLen);
            return -1;
        }

        valueLen = data[curIndex++] & 0xff;
        len_len = 2;

        if (dataLen < valueLen + curIndex) {
            LOGE("ERROR length : NOT ENOUGH  BERTLV DATA PROVIDED . dataLen=0x%x, length=0x%x, curIndex0x%x", dataLen, valueLen, curIndex);
            return -1;
        }
    } else if (temp == 0x82) {
        if ((curIndex + 1) >= dataLen) {
            LOGE("ERROR length : WRONG BERTLV data. curIndex=0x%x, dataLen=0x%x", curIndex, dataLen);
            return -1;
        }

        valueLen = ((data[curIndex] & 0xff) << 8) | (data[curIndex +1] & 0xff);
        len_len = 3;
        curIndex += 2;

        if (dataLen < valueLen + curIndex) {
            LOGE("ERROR length : NOT ENOUGH  BERTLV DATA PROVIDED . dataLen=0x%x, length=0x%x, curIndex0x%x", dataLen, valueLen, curIndex);
            return -1;
        }
    } else if (temp == 0x83) {
        if ((curIndex + 2) >= dataLen) {
            LOGE("ERROR length : WRONG BERTLV data. curIndex=0x%x, dataLen=0x%x", curIndex, dataLen);
            return -1;
        }

        valueLen = ((data[curIndex] & 0xff) << 16) | ((data[curIndex + 1] & 0xff) << 8) | (data[curIndex + 2] & 0xff);
        len_len = 4;
        curIndex += 3;

        if (dataLen < valueLen + curIndex) {
            LOGE("ERROR length : NOT ENOUGH  BERTLV DATA PROVIDED . dataLen=0x%x, length=0x%x, curIndex0x%x", dataLen, valueLen, curIndex);
            return -1;
        }
    }
    pTarget->tag= tag;
    pTarget->dataLen= valueLen;
    pTarget->dataOffset = *offset + tag_len + len_len;
    if (isContainValue == TRUE) {
        *offset = pTarget->dataOffset + valueLen;
    }
    else {
        *offset = pTarget->dataOffset;
    }
    return 0;
}
#endif