[T106][ZXW-22]7520V3SCV2.01.01.02P42U09_VEC_V0.8_AP_VEC origin source commit
Change-Id: Ic6e05d89ecd62fc34f82b23dcf306c93764aec4b
diff --git a/ap/lib/liblpa/lpa_tool.c b/ap/lib/liblpa/lpa_tool.c
new file mode 100755
index 0000000..a13fbe9
--- /dev/null
+++ b/ap/lib/liblpa/lpa_tool.c
@@ -0,0 +1,424 @@
+/***************************************************************/
+//
+//²Î¼û 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;
+ }
+}
+