diff --git a/libfreefare/freefare.h b/libfreefare/freefare.h index cdb30e4..a13bfa5 100644 --- a/libfreefare/freefare.h +++ b/libfreefare/freefare.h @@ -144,6 +144,7 @@ MifareSectorNumber *mifare_application_find (Mad mad, MadAid aid); uint8_t *tlv_encode (const uint8_t type, const uint8_t *istream, uint16_t isize, size_t *osize); uint8_t *tlv_decode (const uint8_t *istream, uint8_t *type, uint16_t *size); +uint8_t *tlv_append (uint8_t *a, uint8_t *b); #ifdef __cplusplus } diff --git a/libfreefare/tlv.c b/libfreefare/tlv.c index cdd0d55..fe11be3 100644 --- a/libfreefare/tlv.c +++ b/libfreefare/tlv.c @@ -36,6 +36,10 @@ #define TLV_TERMINATOR 0xFE +size_t tlv_record_length (const uint8_t *stream, size_t *field_length_size, size_t *field_value_size); +uint8_t *tlv_next (uint8_t *stream); +size_t tlv_sequence_length (uint8_t *stream); + /* * TLV (Type Length Value) Manipulation Functions. */ @@ -85,28 +89,111 @@ tlv_encode (const uint8_t type, const uint8_t *istream, uint16_t isize, size_t * uint8_t * tlv_decode (const uint8_t *istream, uint8_t *type, uint16_t *size) { - size_t s; - off_t o = 1; + size_t fls = 0; + size_t fvs = 0; uint8_t *res = NULL; if (type) *type = istream[0]; - if (istream[1] == 0xff) { - uint16_t be_size; - memcpy (&be_size, istream + 2, sizeof (uint16_t)); - s = be16toh(be_size); - o += 3; - } else { - s = istream[1]; - o += 1; - } + tlv_record_length (istream, &fls, &fvs); + if (size) { - *size = s; + *size = fvs; } - if ((res = malloc (s))) { - memcpy (res, istream + o, s); + if ((res = malloc (fvs))) { + memcpy (res, istream + 1 + fls, fvs); } return res; } + +/* + * Length of a TLV field + */ +size_t +tlv_record_length (const uint8_t *stream, size_t *field_length_size, size_t *field_value_size) +{ + size_t fls = 0; + size_t fvs = 0; + + switch (stream[0]) { + case 0x00: + case 0xfe: + break; + case 0x01: + case 0x02: + case 0x03: + default: // FIXME Not supported. + if (stream[1] == 0xff) { + uint16_t be_size; + memcpy (&be_size, stream + 2, sizeof (uint16_t)); + fls = 3; + fvs = be16toh(be_size); + } else { + fls = 1; + fvs = stream[1]; + } + break; + } + + if (field_length_size) + *field_length_size = fls; + + if (field_value_size) + *field_value_size = fvs; + + return 1 + fls + fvs; +} + +/* + * Get a pointer to the next record in the provided TLV sequence. + * | 0x03 | 0x02 | 0xbe | 0xef | 0x00 | 0x00 | 0xfe | + * First call ---' | | | + * Second call ------------------------' | | + * Third call -------------------------------' | + * Fourth call --------------------------------------' + * Fifth call NULL + */ +uint8_t * +tlv_next (uint8_t *stream) +{ + uint8_t *res = NULL; + if (stream[0] != TLV_TERMINATOR) + res = stream + tlv_record_length (stream, NULL, NULL); + + return res; +} + +/* + * Full-length of all TLV fields. + */ +size_t +tlv_sequence_length (uint8_t *stream) +{ + size_t res = 0; + + do { + res += tlv_record_length (stream, NULL, NULL); + } while ((stream = tlv_next (stream))); + + return res; +} + + +/* + * Append two TLV. Acts like realloc(3). + */ +uint8_t * +tlv_append (uint8_t *a, uint8_t *b) +{ + size_t a_size = tlv_sequence_length (a); + size_t b_size = tlv_sequence_length (b); + size_t new_size = a_size + b_size - 1; + + if ((a = realloc (a, new_size))) { + memcpy (a + a_size - 1, b, b_size); + } + + return a; +} diff --git a/test/test_tlv.c b/test/test_tlv.c index 060dc68..f446b31 100644 --- a/test/test_tlv.c +++ b/test/test_tlv.c @@ -130,3 +130,20 @@ test_tlv_rfu (void) free (data); } + +void +test_tlv_append (void) +{ + const uint8_t a[] = { 0xde, 0xad, 0xbe, 0xef }; + const uint8_t b[] = { 0x42 }; + + uint8_t ndef_ab_ref[] = { 0x03, 0x04, 0xde, 0xad, 0xbe, 0xef, 0x03, 0x01, 0x42, 0xfe }; + + uint8_t *ndef_a = tlv_encode (3, a, 4, NULL); + uint8_t *ndef_b = tlv_encode (3, b, 1, NULL); + ndef_a = tlv_append (ndef_a, ndef_b); + cut_assert_equal_memory (ndef_ab_ref, sizeof (ndef_ab_ref), ndef_a, sizeof (ndef_ab_ref), cut_message ("Wrong appended data")); + + free (ndef_a); + free (ndef_b); +}