struct tlv *dol_process(const struct tlv *tlv, const struct tlvdb *tlvdb, tlv_tag_t tag)
{
size_t res_len;
if (!tlv || !(res_len = dol_calculate_len(tlv, 0))) {
struct tlv *res_tlv = malloc(sizeof(*res_tlv));
res_tlv->tag = tag;
res_tlv->len = 0;
res_tlv->value = NULL;
return res_tlv;
}
struct tlv *res_tlv = malloc(sizeof(*res_tlv) + res_len);
if (!res_tlv)
return NULL;
const unsigned char *buf = tlv->value;
size_t left = tlv->len;
unsigned char *res = (unsigned char *)(res_tlv + 1);
size_t pos = 0;
while (left) {
struct tlv cur_tlv;
if (!tlv_parse_tl(&buf, &left, &cur_tlv) || pos + cur_tlv.len > res_len) {
free(res_tlv);
return NULL;
}
const struct tlv *tag_tlv = tlvdb_get(tlvdb, cur_tlv.tag, NULL);
if (!tag_tlv) {
memset(res + pos, 0, cur_tlv.len);
} else if (tag_tlv->len > cur_tlv.len) {
memcpy(res + pos, tag_tlv->value, cur_tlv.len);
} else {
// FIXME: cn data should be padded with 0xFF !!!
memcpy(res + pos, tag_tlv->value, tag_tlv->len);
memset(res + pos + tag_tlv->len, 0, cur_tlv.len - tag_tlv->len);
}
pos += cur_tlv.len;
}
res_tlv->tag = tag;
res_tlv->len = res_len;
res_tlv->value = res;
return res_tlv;
}
unsigned char *dol_process(const struct tlv *tlv, const struct tlvdb *tlvdb, size_t *len)
{
if (!tlv) {
*len = 0;
return NULL;
}
const unsigned char *buf = tlv->value;
size_t left = tlv->len;
size_t res_len = dol_calculate_len(tlv, 0);
unsigned char *res;
size_t pos = 0;
if (!res_len) {
*len = 0;
return NULL;
}
res = malloc(res_len);
while (left) {
struct tlv cur_tlv;
if (!tlv_parse_tl(&buf, &left, &cur_tlv) || pos + cur_tlv.len > res_len) {
free(res);
return NULL;
}
const struct tlv *tag_tlv = tlvdb_get(tlvdb, cur_tlv.tag, NULL);
if (!tag_tlv) {
memset(res + pos, 0, cur_tlv.len);
} else if (tag_tlv->len > cur_tlv.len) {
memcpy(res + pos, tag_tlv->value, cur_tlv.len);
} else {
// FIXME: cn data should be padded with 0xFF !!!
memcpy(res + pos, tag_tlv->value, tag_tlv->len);
memset(res + pos + tag_tlv->len, 0, cur_tlv.len - tag_tlv->len);
}
pos += cur_tlv.len;
}
*len = pos;
return res;
}
struct tlvdb *dol_parse(const struct tlv *tlv, const unsigned char *data, size_t data_len)
{
if (!tlv)
return NULL;
const unsigned char *buf = tlv->value;
size_t left = tlv->len;
size_t res_len = dol_calculate_len(tlv, data_len);
size_t pos = 0;
struct tlvdb *db = NULL;
if (res_len != data_len)
return NULL;
while (left) {
struct tlv cur_tlv;
if (!tlv_parse_tl(&buf, &left, &cur_tlv) || pos + cur_tlv.len > res_len) {
tlvdb_free(db);
return NULL;
}
/* Last tag can be of variable length */
if (cur_tlv.len == 0 && left == 0)
cur_tlv.len = res_len - pos;
struct tlvdb *tag_db = tlvdb_fixed(cur_tlv.tag, cur_tlv.len, data + pos);
if (!db)
db = tag_db;
else
tlvdb_add(db, tag_db);
pos += cur_tlv.len;
}
return db;
}
