mirror of
https://github.com/estkme-group/lpac
synced 2024-09-19 15:30:40 +02:00
156 lines
3.5 KiB
C
156 lines
3.5 KiB
C
#include "derutil.h"
|
|
#include <stdio.h>
|
|
|
|
uint8_t *euicc_derutil_tag_leftpad(uint8_t *buffer, unsigned buffer_length, unsigned data_offset, uint16_t tag)
|
|
{
|
|
uint8_t *wp = NULL;
|
|
unsigned data_length;
|
|
uint8_t lengthtag = 0x00;
|
|
uint8_t havelengthtag = 0;
|
|
uint8_t lengthbytes = 0;
|
|
uint8_t totalpadlen = 0;
|
|
|
|
data_length = buffer_length - data_offset;
|
|
|
|
if (data_length > 65535)
|
|
{
|
|
lengthtag = 0x83;
|
|
havelengthtag = 1;
|
|
lengthbytes = 3;
|
|
}
|
|
else if (data_length > 255)
|
|
{
|
|
lengthtag = 0x82;
|
|
havelengthtag = 1;
|
|
lengthbytes = 2;
|
|
}
|
|
else if (data_length > 127)
|
|
{
|
|
lengthtag = 0x81;
|
|
havelengthtag = 1;
|
|
lengthbytes = 1;
|
|
}
|
|
else
|
|
{
|
|
havelengthtag = 0;
|
|
lengthbytes = 1;
|
|
}
|
|
|
|
totalpadlen = 2 + havelengthtag + lengthbytes;
|
|
if (data_offset < totalpadlen)
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
wp = buffer + (data_offset - totalpadlen);
|
|
wp[0] = tag >> 8;
|
|
wp[1] = tag & 0xFF;
|
|
wp += 2;
|
|
if (havelengthtag)
|
|
{
|
|
wp[0] = lengthtag;
|
|
wp += 1;
|
|
}
|
|
// lengthbytes, big endian
|
|
for (int i = 0; i < lengthbytes; i++)
|
|
{
|
|
wp[lengthbytes - i - 1] = (data_length >> (i * 8)) & 0xFF;
|
|
}
|
|
return (buffer + data_offset - totalpadlen);
|
|
}
|
|
|
|
int euicc_derutil_tag_find(uint8_t **rptr, uint8_t *buffer, uint32_t buffer_len, uint16_t *target_tag, uint8_t retfirst)
|
|
{
|
|
uint16_t current_tag;
|
|
uint16_t current_length;
|
|
uint8_t state = 0;
|
|
uint8_t *ptr;
|
|
ptr = buffer;
|
|
state = 0;
|
|
while (ptr <= (buffer + buffer_len))
|
|
{
|
|
switch (state)
|
|
{
|
|
case 0:
|
|
current_tag = *ptr;
|
|
if ((current_tag & 0b11111) == 0b11111)
|
|
{
|
|
state = 1;
|
|
}
|
|
else
|
|
{
|
|
state = 2;
|
|
}
|
|
break;
|
|
case 1:
|
|
current_tag = (current_tag << 8) | *ptr;
|
|
state = 2;
|
|
break;
|
|
case 2:
|
|
current_length = *ptr;
|
|
switch (current_length)
|
|
{
|
|
case 0x81:
|
|
state = 3;
|
|
break;
|
|
case 0x82:
|
|
state = 4;
|
|
break;
|
|
default:
|
|
state = 5;
|
|
break;
|
|
}
|
|
break;
|
|
case 3:
|
|
current_length = *ptr;
|
|
state = 5;
|
|
break;
|
|
case 4:
|
|
current_length = *ptr;
|
|
state = 41;
|
|
break;
|
|
case 41:
|
|
current_length = (current_length << 8) | *ptr;
|
|
state = 5;
|
|
break;
|
|
case 5:
|
|
if (retfirst)
|
|
{
|
|
if (target_tag)
|
|
{
|
|
*target_tag = current_tag;
|
|
}
|
|
if (rptr)
|
|
{
|
|
*rptr = ptr;
|
|
}
|
|
return current_length;
|
|
}
|
|
else
|
|
{
|
|
if (current_tag == *target_tag)
|
|
{
|
|
if (rptr)
|
|
{
|
|
*rptr = ptr;
|
|
}
|
|
return current_length;
|
|
}
|
|
if (current_length == 0)
|
|
{
|
|
state = 0;
|
|
continue;
|
|
}
|
|
current_length--;
|
|
if (current_length == 0)
|
|
{
|
|
state = 0;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
ptr++;
|
|
}
|
|
return -1;
|
|
}
|