ap/lib/liblpa/lpa_tool.c

/***************************************************************/
//
//²Î¼û LPA½Ó¿ÚÎĵµV0.1 SGP.22, ·µ»ØAPDU
//   
/***************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <sys/time.h>
#include <termios.h>

#include "lpa_inner.h"


static const unsigned char base64_table[65] = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";

/**
 * @brief ¿É´òÓ¡×Ö·û´®×ª»»Îª×Ö½ÚÊý¾Ý £¬È磺
                "C8329BFD0E01" --> {0xC8, 0x32, 0x9B, 0xFD, 0x0E, 0x01}
 * @param pSrc Ô´×Ö·û´®Ö¸Õë
 * @param pDst Ä¿±êÊý¾ÝÖ¸Õë
 * @param nSrcLength Ô´×Ö·û´®³¤¶È
 * @return Ä¿±êÊý¾Ý³¤¶È
 * @note   
 * @warning 
 */
int string2bytes(const char* pSrc, unsigned char* pDst, int nSrcLength)
{
    int i=0;

    //УÑé²ÎÊý
    if(pSrc ==  NULL || pDst == NULL || nSrcLength < 0)
    {
        return -1;
    }

    for(i = 0; i < nSrcLength; i += 2)
    {
        // Êä³ö¸ß4λ
        if(*pSrc >= '0' && *pSrc <= '9')
        {
            *pDst = (*pSrc - '0') << 4;
        }
        else
        {
            *pDst = ((toupper(*pSrc) - 'A') + 10) << 4;
        }

        pSrc++;

        // Êä³öµÍ4λ
        if(*pSrc >= '0' && *pSrc <= '9')
        {
            *pDst |= *pSrc - '0';
        }
        else
        {
            *pDst |= (toupper(*pSrc) - 'A') + 10;
        }

        pSrc++;
        pDst++;
    }

    // ·µ»ØÄ¿±êÊý¾Ý³¤¶È
    return nSrcLength / 2;
}

/**
 * @brief ×Ö½ÚÊý¾Ýת»»Îª¿É´òÓ¡×Ö·û´® £¬È磺
                     {0xC8, 0x32, 0x9B, 0xFD, 0x0E, 0x01} --> "C8329BFD0E01"
 * @param pSrc Ô´Êý¾ÝÖ¸Õë
 * @param pDst Ä¿±ê×Ö·û´®Ö¸Õë
 * @param nSrcLength Ô´Êý¾Ý³¤¶È
 * @return Ä¿±êÊý¾Ý³¤¶È£»
 * @note   
 * @warning 
 */
int bytes2string(const unsigned char* pSrc, char* pDst, int nSrcLength)
{
    const char tab[]="0123456789ABCDEF";	// 0x0-0xfµÄ×Ö·û²éÕÒ±í
    int i = 0;

    //УÑé²ÎÊý
    if(pSrc ==  NULL || pDst == NULL || nSrcLength < 0)
    {
        return -1;
    }

    for(i=0; i<nSrcLength; i++)
    {
        *pDst++ = tab[*pSrc >> 4];		// Êä³öµÍ4λ
        *pDst++ = tab[*pSrc & 0x0f];	// Êä³ö¸ß4λ
        pSrc++;
    }

    // ·µ»ØÄ¿±ê×Ö·û´®³¤¶È
    return nSrcLength * 2;
}

//lower case
int bytes2string_lower(const unsigned char* pSrc, char* pDst, int nSrcLength)
{
    const char tab[]="0123456789abcdef";	// 0x0-0xfµÄ×Ö·û²éÕÒ±í
    int i = 0;

    //УÑé²ÎÊý
    if(pSrc ==  NULL || pDst == NULL || nSrcLength < 0)
    {
        return -1;
    }

    for(i=0; i<nSrcLength; i++)
    {
        *pDst++ = tab[*pSrc >> 4];		// Êä³öµÍ4λ
        *pDst++ = tab[*pSrc & 0x0f];	// Êä³ö¸ß4λ
        pSrc++;
    }

    // ·µ»ØÄ¿±ê×Ö·û´®³¤¶È
    return nSrcLength * 2;
}


char *lpa_base64_encode(const char*data, int data_len)
{ 
    //int data_len = strlen(data); 
    int prepare = 0; 
    int ret_len; 
    int temp = 0; 
    char *ret = NULL; 
    char *f = NULL; 
    int tmp = 0; 
    char changed[4]; 
    int i = 0; 
	const char base[] = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/="; 

	if(data == NULL)
	{
		return NULL;
	}
	if(data_len == 0)
	{
		return NULL;
	}
    ret_len = data_len / 3; 
    temp = data_len % 3; 
    if (temp > 0) 
    { 
        ret_len += 1; 
    } 
    ret_len = ret_len*4 + 1; 
    ret = (char *)malloc(ret_len); 
      
    if (ret == NULL) 
    { 
        printf("No enough memory.\n"); 
        return NULL;
    } 
    memset(ret, 0, ret_len); 
    f = ret; 
    while (tmp < data_len) 
    { 
        temp = 0; 
        prepare = 0; 
        memset(changed, '\0', 4); 
        while (temp < 3) 
        { 
            //printf("tmp = %d\n", tmp); 
            if (tmp >= data_len) 
            { 
                break; 
            } 
            prepare = ((prepare << 8) | (data[tmp] & 0xFF)); 
            tmp++; 
            temp++; 
        } 
        prepare = (prepare<<((3-temp)*8)); 
        //printf("before for : temp = %d, prepare = %d\n", temp, prepare); 
        for (i = 0; i < 4 ;i++ ) 
        { 
            if (temp < i) 
            { 
                changed[i] = 0x40; 
            } 
            else 
            { 
                changed[i] = (prepare>>((3-i)*6)) & 0x3F; 
            } 
            *f = base[changed[i]]; 
            //printf("%.2X", changed[i]); 
            f++; 
        } 
    } 
    *f = '\0'; 
    return ret; 
} 

unsigned char *lpa_base64_decode(const unsigned char *src, int len, int *out_len)
{
	unsigned char dtable[256] = {0};//kw
	unsigned char *out = NULL;
	unsigned char *pos = NULL;
	unsigned char in[4] = {0};
	unsigned char block[4] = {0};
	unsigned char tmp = 0;
	int i = 0;
	int count = 0;
	int olen = 0;

	memset(dtable, 0x80, 256);
	for (i = 0; i < sizeof(base64_table) - 1; i++)
		dtable[base64_table[i]] = (unsigned char) i;
	dtable['='] = 0;

	count = 0;
	for (i = 0; i < len; i++) {
		if (dtable[src[i]] != 0x80)
			count++;
	}

	if (count == 0 || count % 4)
		return NULL;

	olen = count / 4 * 3;
	pos = out = malloc(olen);
	if (out == NULL)
		return NULL;
	memset(pos, 0, olen);

	count = 0;
	for (i = 0; i < len; i++) {
		tmp = dtable[src[i]];
		if (tmp == 0x80)
			continue;

		in[count] = src[i];
		block[count] = tmp;
		count++;
		if (count == 4) {
			*pos++ = (block[0] << 2) | (block[1] >> 4);
			*pos++ = (block[1] << 4) | (block[2] >> 2);
			*pos++ = (block[2] << 6) | block[3];
			count = 0;
		}
	}

	if (pos > out) {
		if (in[2] == '=')
			pos -= 2;
		else if (in[3] == '=')
			pos--;
	}

	*out_len = pos - out;
	return out;
}

//lpa code
//0 ok, -1 error, 9000 91XX 92XX and 61XX
int lpa_csim_resp_code(char *atres_apdu, int *second_len)
{
	int pdu_len = 0;
	char sw1_s[3] = {0};
	char sw2_s[3] = {0};
	int sw1 = 0;
	int sw2 = 0;
	int res = -1;

	pdu_len = strlen(atres_apdu);//kw 1
	if (pdu_len < 4) {
		printf("lpa_csim_resp_code len err:%d\n", pdu_len);
		return res;
	}
	
	strncpy(sw1_s, atres_apdu + pdu_len - 4, sizeof(sw1_s) - 1);
	strncpy(sw2_s, atres_apdu + pdu_len - 2, sizeof(sw2_s) - 1);
	errno = 0;
	sw1 =strtol(sw1_s, NULL, 16);
	if (errno == ERANGE)// kw ERRNO.NOT_CHECKED
	{
		printf("strtol errno %d: %s\n", errno, strerror(errno));
	}
	errno = 0;
	sw2 =strtol(sw2_s, NULL, 16);
	if (errno == ERANGE)// kw ERRNO.NOT_CHECKED
	{
		printf("strtol errno %d: %s\n", errno, strerror(errno));
	}
	if ((sw1 == 0x90 && sw2 == 0x00) || (sw1 == 0x91) || (sw1 == 0x92)) {
		*second_len = 0;
		res = 0;
	}
	else if(sw1 == 0x61) {
		*second_len = sw2;
		if (sw2 == 0x00) {//tianyu: bigger than 256, use 00c0000000
			*second_len = 0xFF;
		}
		res = 0;
	}
	
	return res;
}


//0 ok, -1 error, 9000 91XX 92XX
int lpa_csim_resp_normal(char *atres_apdu)
{
	int pdu_len = 0;
	char sw1_s[3] = {0};
	char sw2_s[3] = {0};
	int sw1 = 0;
	int sw2 = 0;

	pdu_len = strlen(atres_apdu);//kw 1
	if (pdu_len < 4) {
		printf("lpa_csim_resp_normal len err:%d\n", pdu_len);
		return -1;
	}
	
	strncpy(sw1_s, atres_apdu + pdu_len - 4, sizeof(sw1_s) - 1);
	strncpy(sw2_s, atres_apdu + pdu_len - 2, sizeof(sw2_s) - 1);
	errno = 0;
	sw1 =strtol(sw1_s, NULL, 16);
	if (errno == ERANGE)// kw ERRNO.NOT_CHECKED
	{
		printf("strtol errno %d: %s\n", errno, strerror(errno));
	}
	errno = 0;
	sw2 =strtol(sw2_s, NULL, 16);
	if (errno == ERANGE)// kw ERRNO.NOT_CHECKED
	{
		printf("strtol errno %d: %s\n", errno, strerror(errno));
	}
	//normal processing: 9000 91XX 92XX
	if ((sw1 == 0x90 && sw2 == 0x00) || (sw1 == 0x91) || (sw1 == 0x92)) {
		return 0;
	}
	
	return -1;
}

//delete sw1 sw2
char *lpa_get_apdu_from_atresp(char *atres_apdu)
{
	int pdu_len = 0;
	char *apdu = NULL;

	pdu_len = strlen(atres_apdu);//kw
	if (pdu_len < 4 || pdu_len >= APDU_RESP_LEN)
		return NULL;
	
	apdu = malloc(APDU_RESP_LEN);
	if (apdu == NULL)
		return NULL;
	memset(apdu, 0, APDU_RESP_LEN);
	strncpy(apdu, atres_apdu, pdu_len - 4);//len = 4 -> "\0"
	printf("get apdu: %s\n", apdu);
	return apdu;
}


//´ÓcsimµÄapduÖнâ³öÖ¸¶¨µÄtagµÄvalue(hexstring)
//Èç¹ûÓжà×éͬÃûtag£¬Ö»Äܽâ³öµÚÒ»×é
char *lpa_tag_apdu_from_atresp(char *atres_apdu, unsigned int tag)
{
	int pdu_len = 0;
	char *apdu = NULL;
	unsigned char *pdu_byte = NULL;
	int nt_byte_len = 0;

	pdu_len = strlen(atres_apdu);//kw
	if (pdu_len <= 0) {
		printf("atres_apdu len err:%d\n", pdu_len);
		return NULL;
	}
	
	pdu_byte = malloc(pdu_len/2 + 1);
	if (pdu_byte == NULL) {
		printf("atres_apdu malloc fail\n");
		return NULL;
	}

	memset(pdu_byte, 0, pdu_len/2 + 1);
	nt_byte_len = string2bytes(atres_apdu, pdu_byte, pdu_len);
	if (nt_byte_len < 2) {
		free(pdu_byte);
		printf("atres_apdu to byte err\n");
		return NULL;
	}
	
	apdu = lpa_tlv_get_val_by_tag(pdu_byte, nt_byte_len, tag);
	
	free(pdu_byte);
	
	return apdu;
}


//9812.... to 8921
void lpa_trans_iccid(char *iccid, int len)
{
	char low = 0;
	char high = 0;
	int i = 0;

    if (iccid == NULL || len != ICCID_LEN) {
		printf("iccid check fail\n");
		return;
    }

	for(i = 0; i < len; i += 2)
	{
		low = *(iccid);
		high = *(iccid + 1);
		*(iccid) = high;
		*(iccid + 1) = low;

		iccid = iccid + 2;
	}
}