blob: a13fbe94d5376672c91311cec9cbb87da3313b76 [file] [log] [blame]
lh9ed821d2023-04-07 01:36:19 -07001/***************************************************************/
2//
3//²Î¼û LPA½Ó¿ÚÎĵµV0.1 SGP.22, ·µ»ØAPDU
4//
5/***************************************************************/
6
7#include <stdio.h>
8#include <stdlib.h>
9#include <string.h>
10#include <ctype.h>
11#include <sys/time.h>
12#include <termios.h>
13
14#include "lpa_inner.h"
15
16
17static const unsigned char base64_table[65] = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
18
19/**
20 * @brief ¿É´òÓ¡×Ö·û´®×ª»»Îª×Ö½ÚÊý¾Ý £¬È磺
21 "C8329BFD0E01" --> {0xC8, 0x32, 0x9B, 0xFD, 0x0E, 0x01}
22 * @param pSrc Ô´×Ö·û´®Ö¸Õë
23 * @param pDst Ä¿±êÊý¾ÝÖ¸Õë
24 * @param nSrcLength Ô´×Ö·û´®³¤¶È
25 * @return Ä¿±êÊý¾Ý³¤¶È
26 * @note
27 * @warning
28 */
29int string2bytes(const char* pSrc, unsigned char* pDst, int nSrcLength)
30{
31 int i=0;
32
33 //УÑé²ÎÊý
34 if(pSrc == NULL || pDst == NULL || nSrcLength < 0)
35 {
36 return -1;
37 }
38
39 for(i = 0; i < nSrcLength; i += 2)
40 {
41 // Êä³ö¸ß4λ
42 if(*pSrc >= '0' && *pSrc <= '9')
43 {
44 *pDst = (*pSrc - '0') << 4;
45 }
46 else
47 {
48 *pDst = ((toupper(*pSrc) - 'A') + 10) << 4;
49 }
50
51 pSrc++;
52
53 // Êä³öµÍ4λ
54 if(*pSrc >= '0' && *pSrc <= '9')
55 {
56 *pDst |= *pSrc - '0';
57 }
58 else
59 {
60 *pDst |= (toupper(*pSrc) - 'A') + 10;
61 }
62
63 pSrc++;
64 pDst++;
65 }
66
67 // ·µ»ØÄ¿±êÊý¾Ý³¤¶È
68 return nSrcLength / 2;
69}
70
71/**
72 * @brief ×Ö½ÚÊý¾Ýת»»Îª¿É´òÓ¡×Ö·û´® £¬È磺
73 {0xC8, 0x32, 0x9B, 0xFD, 0x0E, 0x01} --> "C8329BFD0E01"
74 * @param pSrc Ô´Êý¾ÝÖ¸Õë
75 * @param pDst Ä¿±ê×Ö·û´®Ö¸Õë
76 * @param nSrcLength Ô´Êý¾Ý³¤¶È
77 * @return Ä¿±êÊý¾Ý³¤¶È£»
78 * @note
79 * @warning
80 */
81int bytes2string(const unsigned char* pSrc, char* pDst, int nSrcLength)
82{
83 const char tab[]="0123456789ABCDEF"; // 0x0-0xfµÄ×Ö·û²éÕÒ±í
84 int i = 0;
85
86 //УÑé²ÎÊý
87 if(pSrc == NULL || pDst == NULL || nSrcLength < 0)
88 {
89 return -1;
90 }
91
92 for(i=0; i<nSrcLength; i++)
93 {
94 *pDst++ = tab[*pSrc >> 4]; // Êä³öµÍ4λ
95 *pDst++ = tab[*pSrc & 0x0f]; // Êä³ö¸ß4λ
96 pSrc++;
97 }
98
99 // ·µ»ØÄ¿±ê×Ö·û´®³¤¶È
100 return nSrcLength * 2;
101}
102
103//lower case
104int bytes2string_lower(const unsigned char* pSrc, char* pDst, int nSrcLength)
105{
106 const char tab[]="0123456789abcdef"; // 0x0-0xfµÄ×Ö·û²éÕÒ±í
107 int i = 0;
108
109 //УÑé²ÎÊý
110 if(pSrc == NULL || pDst == NULL || nSrcLength < 0)
111 {
112 return -1;
113 }
114
115 for(i=0; i<nSrcLength; i++)
116 {
117 *pDst++ = tab[*pSrc >> 4]; // Êä³öµÍ4λ
118 *pDst++ = tab[*pSrc & 0x0f]; // Êä³ö¸ß4λ
119 pSrc++;
120 }
121
122 // ·µ»ØÄ¿±ê×Ö·û´®³¤¶È
123 return nSrcLength * 2;
124}
125
126
127char *lpa_base64_encode(const char*data, int data_len)
128{
129 //int data_len = strlen(data);
130 int prepare = 0;
131 int ret_len;
132 int temp = 0;
133 char *ret = NULL;
134 char *f = NULL;
135 int tmp = 0;
136 char changed[4];
137 int i = 0;
138 const char base[] = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/=";
139
140 if(data == NULL)
141 {
142 return NULL;
143 }
144 if(data_len == 0)
145 {
146 return NULL;
147 }
148 ret_len = data_len / 3;
149 temp = data_len % 3;
150 if (temp > 0)
151 {
152 ret_len += 1;
153 }
154 ret_len = ret_len*4 + 1;
155 ret = (char *)malloc(ret_len);
156
157 if (ret == NULL)
158 {
159 printf("No enough memory.\n");
160 return NULL;
161 }
162 memset(ret, 0, ret_len);
163 f = ret;
164 while (tmp < data_len)
165 {
166 temp = 0;
167 prepare = 0;
168 memset(changed, '\0', 4);
169 while (temp < 3)
170 {
171 //printf("tmp = %d\n", tmp);
172 if (tmp >= data_len)
173 {
174 break;
175 }
176 prepare = ((prepare << 8) | (data[tmp] & 0xFF));
177 tmp++;
178 temp++;
179 }
180 prepare = (prepare<<((3-temp)*8));
181 //printf("before for : temp = %d, prepare = %d\n", temp, prepare);
182 for (i = 0; i < 4 ;i++ )
183 {
184 if (temp < i)
185 {
186 changed[i] = 0x40;
187 }
188 else
189 {
190 changed[i] = (prepare>>((3-i)*6)) & 0x3F;
191 }
192 *f = base[changed[i]];
193 //printf("%.2X", changed[i]);
194 f++;
195 }
196 }
197 *f = '\0';
198 return ret;
199}
200
201unsigned char *lpa_base64_decode(const unsigned char *src, int len, int *out_len)
202{
203 unsigned char dtable[256] = {0};//kw
204 unsigned char *out = NULL;
205 unsigned char *pos = NULL;
206 unsigned char in[4] = {0};
207 unsigned char block[4] = {0};
208 unsigned char tmp = 0;
209 int i = 0;
210 int count = 0;
211 int olen = 0;
212
213 memset(dtable, 0x80, 256);
214 for (i = 0; i < sizeof(base64_table) - 1; i++)
215 dtable[base64_table[i]] = (unsigned char) i;
216 dtable['='] = 0;
217
218 count = 0;
219 for (i = 0; i < len; i++) {
220 if (dtable[src[i]] != 0x80)
221 count++;
222 }
223
224 if (count == 0 || count % 4)
225 return NULL;
226
227 olen = count / 4 * 3;
228 pos = out = malloc(olen);
229 if (out == NULL)
230 return NULL;
231 memset(pos, 0, olen);
232
233 count = 0;
234 for (i = 0; i < len; i++) {
235 tmp = dtable[src[i]];
236 if (tmp == 0x80)
237 continue;
238
239 in[count] = src[i];
240 block[count] = tmp;
241 count++;
242 if (count == 4) {
243 *pos++ = (block[0] << 2) | (block[1] >> 4);
244 *pos++ = (block[1] << 4) | (block[2] >> 2);
245 *pos++ = (block[2] << 6) | block[3];
246 count = 0;
247 }
248 }
249
250 if (pos > out) {
251 if (in[2] == '=')
252 pos -= 2;
253 else if (in[3] == '=')
254 pos--;
255 }
256
257 *out_len = pos - out;
258 return out;
259}
260
261//lpa code
262//0 ok, -1 error, 9000 91XX 92XX and 61XX
263int lpa_csim_resp_code(char *atres_apdu, int *second_len)
264{
265 int pdu_len = 0;
266 char sw1_s[3] = {0};
267 char sw2_s[3] = {0};
268 int sw1 = 0;
269 int sw2 = 0;
270 int res = -1;
271
272 pdu_len = strlen(atres_apdu);//kw 1
273 if (pdu_len < 4) {
274 printf("lpa_csim_resp_code len err:%d\n", pdu_len);
275 return res;
276 }
277
278 strncpy(sw1_s, atres_apdu + pdu_len - 4, sizeof(sw1_s) - 1);
279 strncpy(sw2_s, atres_apdu + pdu_len - 2, sizeof(sw2_s) - 1);
280 errno = 0;
281 sw1 =strtol(sw1_s, NULL, 16);
282 if (errno == ERANGE)// kw ERRNO.NOT_CHECKED
283 {
284 printf("strtol errno %d: %s\n", errno, strerror(errno));
285 }
286 errno = 0;
287 sw2 =strtol(sw2_s, NULL, 16);
288 if (errno == ERANGE)// kw ERRNO.NOT_CHECKED
289 {
290 printf("strtol errno %d: %s\n", errno, strerror(errno));
291 }
292 if ((sw1 == 0x90 && sw2 == 0x00) || (sw1 == 0x91) || (sw1 == 0x92)) {
293 *second_len = 0;
294 res = 0;
295 }
296 else if(sw1 == 0x61) {
297 *second_len = sw2;
298 if (sw2 == 0x00) {//tianyu: bigger than 256, use 00c0000000
299 *second_len = 0xFF;
300 }
301 res = 0;
302 }
303
304 return res;
305}
306
307
308//0 ok, -1 error, 9000 91XX 92XX
309int lpa_csim_resp_normal(char *atres_apdu)
310{
311 int pdu_len = 0;
312 char sw1_s[3] = {0};
313 char sw2_s[3] = {0};
314 int sw1 = 0;
315 int sw2 = 0;
316
317 pdu_len = strlen(atres_apdu);//kw 1
318 if (pdu_len < 4) {
319 printf("lpa_csim_resp_normal len err:%d\n", pdu_len);
320 return -1;
321 }
322
323 strncpy(sw1_s, atres_apdu + pdu_len - 4, sizeof(sw1_s) - 1);
324 strncpy(sw2_s, atres_apdu + pdu_len - 2, sizeof(sw2_s) - 1);
325 errno = 0;
326 sw1 =strtol(sw1_s, NULL, 16);
327 if (errno == ERANGE)// kw ERRNO.NOT_CHECKED
328 {
329 printf("strtol errno %d: %s\n", errno, strerror(errno));
330 }
331 errno = 0;
332 sw2 =strtol(sw2_s, NULL, 16);
333 if (errno == ERANGE)// kw ERRNO.NOT_CHECKED
334 {
335 printf("strtol errno %d: %s\n", errno, strerror(errno));
336 }
337 //normal processing: 9000 91XX 92XX
338 if ((sw1 == 0x90 && sw2 == 0x00) || (sw1 == 0x91) || (sw1 == 0x92)) {
339 return 0;
340 }
341
342 return -1;
343}
344
345//delete sw1 sw2
346char *lpa_get_apdu_from_atresp(char *atres_apdu)
347{
348 int pdu_len = 0;
349 char *apdu = NULL;
350
351 pdu_len = strlen(atres_apdu);//kw
352 if (pdu_len < 4 || pdu_len >= APDU_RESP_LEN)
353 return NULL;
354
355 apdu = malloc(APDU_RESP_LEN);
356 if (apdu == NULL)
357 return NULL;
358 memset(apdu, 0, APDU_RESP_LEN);
359 strncpy(apdu, atres_apdu, pdu_len - 4);//len = 4 -> "\0"
360 printf("get apdu: %s\n", apdu);
361 return apdu;
362}
363
364
365//´ÓcsimµÄapduÖнâ³öÖ¸¶¨µÄtagµÄvalue(hexstring)
366//Èç¹ûÓжà×éͬÃûtag£¬Ö»Äܽâ³öµÚÒ»×é
367char *lpa_tag_apdu_from_atresp(char *atres_apdu, unsigned int tag)
368{
369 int pdu_len = 0;
370 char *apdu = NULL;
371 unsigned char *pdu_byte = NULL;
372 int nt_byte_len = 0;
373
374 pdu_len = strlen(atres_apdu);//kw
375 if (pdu_len <= 0) {
376 printf("atres_apdu len err:%d\n", pdu_len);
377 return NULL;
378 }
379
380 pdu_byte = malloc(pdu_len/2 + 1);
381 if (pdu_byte == NULL) {
382 printf("atres_apdu malloc fail\n");
383 return NULL;
384 }
385
386 memset(pdu_byte, 0, pdu_len/2 + 1);
387 nt_byte_len = string2bytes(atres_apdu, pdu_byte, pdu_len);
388 if (nt_byte_len < 2) {
389 free(pdu_byte);
390 printf("atres_apdu to byte err\n");
391 return NULL;
392 }
393
394 apdu = lpa_tlv_get_val_by_tag(pdu_byte, nt_byte_len, tag);
395
396 free(pdu_byte);
397
398 return apdu;
399}
400
401
402//9812.... to 8921
403void lpa_trans_iccid(char *iccid, int len)
404{
405 char low = 0;
406 char high = 0;
407 int i = 0;
408
409 if (iccid == NULL || len != ICCID_LEN) {
410 printf("iccid check fail\n");
411 return;
412 }
413
414 for(i = 0; i < len; i += 2)
415 {
416 low = *(iccid);
417 high = *(iccid + 1);
418 *(iccid) = high;
419 *(iccid + 1) = low;
420
421 iccid = iccid + 2;
422 }
423}
424