diff options
| -rw-r--r-- | src/simutil.c | 115 | ||||
| -rw-r--r-- | src/simutil.h | 23 |
2 files changed, 138 insertions, 0 deletions
diff --git a/src/simutil.c b/src/simutil.c index a2d838d3..d9383b7a 100644 --- a/src/simutil.c +++ b/src/simutil.c @@ -198,6 +198,115 @@ const unsigned char *simple_tlv_iter_get_data(struct simple_tlv_iter *iter) return iter->data; } +void comprehension_tlv_iter_init(struct comprehension_tlv_iter *iter, + const unsigned char *pdu, + unsigned int len) +{ + iter->pdu = pdu; + iter->max = len; + iter->pos = 0; + iter->tag = 0; + iter->cr = FALSE; + iter->data = 0; +} + +/* Comprehension TLVs defined in Section 7 of ETSI TS 102.220 */ +gboolean comprehension_tlv_iter_next(struct comprehension_tlv_iter *iter) +{ + const unsigned char *pdu = iter->pdu + iter->pos; + const unsigned char *end = iter->pdu + iter->max; + unsigned short tag; + unsigned short len; + gboolean cr; + + if (pdu == end) + return FALSE; + + cr = bit_field(*pdu, 7, 1); + tag = bit_field(*pdu, 0, 7); + pdu++; + + if (tag == 0x00 || tag == 0xFF || tag == 0x80) + return FALSE; + + /* + * ETSI TS 102.220, Section 7.1.1.2 + * + * If byte 1 of the tag is equal to 0x7F, then the tag is encoded + * on the following two bytes, with bit 8 of the 2nd byte of the tag + * being the CR flag. + */ + if (tag == 0x7F) { + if ((pdu + 2) > end) + return FALSE; + + cr = bit_field(pdu[0], 7, 1); + tag = ((pdu[0] & 0x7f) << 7) | pdu[1]; + + if (tag < 0x0001 || tag > 0x7fff) + return FALSE; + + pdu += 2; + } + + if (pdu == end) + return FALSE; + + len = *pdu++; + + if (len >= 0x80) { + unsigned int extended_bytes = len - 0x80; + unsigned int i; + + if (extended_bytes == 0 || extended_bytes > 3) + return FALSE; + + if ((pdu + extended_bytes) > end) + return FALSE; + + if (pdu[0] == 0) + return FALSE; + + for (len = 0, i = 0; i < extended_bytes; i++) + len = (len << 8) | *pdu++; + } + + if (pdu + len > end) + return FALSE; + + iter->tag = tag; + iter->cr = cr; + iter->len = len; + iter->data = pdu; + + iter->pos = pdu + len - iter->pdu; + + return TRUE; +} + +unsigned short comprehension_tlv_iter_get_tag( + struct comprehension_tlv_iter *iter) +{ + return iter->tag; +} + +gboolean comprehension_tlv_get_cr(struct comprehension_tlv_iter *iter) +{ + return iter->cr; +} + +unsigned int comprehension_tlv_iter_get_length( + struct comprehension_tlv_iter *iter) +{ + return iter->len; +} + +const unsigned char *comprehension_tlv_iter_get_data( + struct comprehension_tlv_iter *iter) +{ + return iter->data; +} + void ber_tlv_iter_init(struct ber_tlv_iter *iter, const unsigned char *pdu, unsigned int len) { @@ -351,6 +460,12 @@ void ber_tlv_iter_recurse_simple(struct ber_tlv_iter *iter, simple_tlv_iter_init(container, iter->data, iter->len); } +void ber_tlv_iter_recurse_comprehension(struct ber_tlv_iter *iter, + struct comprehension_tlv_iter *recurse) +{ + comprehension_tlv_iter_init(recurse, iter->data, iter->len); +} + static const guint8 *ber_tlv_find_by_tag(const guint8 *pdu, guint8 in_tag, int in_len, int *out_len) { diff --git a/src/simutil.h b/src/simutil.h index 5cad1d78..043c21fc 100644 --- a/src/simutil.h +++ b/src/simutil.h @@ -93,6 +93,16 @@ struct simple_tlv_iter { const unsigned char *data; }; +struct comprehension_tlv_iter { + unsigned int max; + unsigned int pos; + const unsigned char *pdu; + unsigned short tag; + gboolean cr; + unsigned int len; + const unsigned char *data; +}; + struct ber_tlv_iter { unsigned int max; unsigned int pos; @@ -111,6 +121,17 @@ unsigned char simple_tlv_iter_get_tag(struct simple_tlv_iter *iter); unsigned short simple_tlv_iter_get_length(struct simple_tlv_iter *iter); const unsigned char *simple_tlv_iter_get_data(struct simple_tlv_iter *iter); +void comprehension_tlv_iter_init(struct comprehension_tlv_iter *iter, + const unsigned char *pdu, + unsigned int len); +gboolean comprehension_tlv_iter_next(struct comprehension_tlv_iter *iter); +unsigned short comprehension_tlv_iter_get_tag(struct comprehension_tlv_iter *i); +gboolean comprehension_tlv_get_cr(struct comprehension_tlv_iter *iter); +unsigned int comprehension_tlv_iter_get_length( + struct comprehension_tlv_iter *iter); +const unsigned char *comprehension_tlv_iter_get_data( + struct comprehension_tlv_iter *iter); + void ber_tlv_iter_init(struct ber_tlv_iter *iter, const unsigned char *pdu, unsigned int len); /* @@ -140,6 +161,8 @@ void ber_tlv_iter_recurse(struct ber_tlv_iter *iter, struct ber_tlv_iter *recurse); void ber_tlv_iter_recurse_simple(struct ber_tlv_iter *iter, struct simple_tlv_iter *container); +void ber_tlv_iter_recurse_comprehension(struct ber_tlv_iter *iter, + struct comprehension_tlv_iter *recurse); struct sim_eons *sim_eons_new(int pnn_records); void sim_eons_add_pnn_record(struct sim_eons *eons, int record, |