diff options
Diffstat (limited to 'ffd/tlv.c')
-rw-r--r-- | ffd/tlv.c | 73 |
1 files changed, 73 insertions, 0 deletions
diff --git a/ffd/tlv.c b/ffd/tlv.c new file mode 100644 index 0000000..684a0cc --- /dev/null +++ b/ffd/tlv.c @@ -0,0 +1,73 @@ +/* + Copyright (c) 2012, Matthias Schiffer <mschiffer@universe-factory.net> + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + + +#include "tlv.h" +#include "packet.h" + +#include <string.h> + +#include <arpa/inet.h> + + +bool ffd_tlv_parse(const ffd_packet_t *packet, ffd_tlv_cb cb) { + if (packet->version_magic != htons(FFD_VERSION_MAGIC)) + return false; + + const uint8_t *data = packet->tlv + 2, *end = ((uint8_t*)packet->tlv) + packet->len; + + while (data <= end) { + if (!data[-2]) { + /* Pad1 */ + data++; + continue; + } + + if (data + data[-1] > end) { + /* warn */ + break; + } + + cb(data[-2], data, data[-1]); + data += data[-1] + 2; + } + + return true; +} + +bool ffd_tlv_add(ffd_packet_t *packet, size_t max_len, ffd_tlv_type_t type, void *data, size_t len) { + if (ntohs(packet->len)+len+2 > max_len) + return false; + + packet->len = htons(ntohs(packet->len)+len+2); + + uint8_t *pkgdata = packet->tlv; + + pkgdata[0] = type; + pkgdata[1] = len; + memcpy(pkgdata+2, data, len); + + return true; +} |