src/libsystemd-network/lldp-tlv.c

   1 /*-*- Mode: C; c-basic-offset: 8; indent-tabs-mode: nil -*-*/
   2
   3 /***
   4   This file is part of systemd.
   5
   6   Copyright (C) 2014 Tom Gundersen
   7   Copyright (C) 2014 Susant Sahani
   8
   9   systemd is free software; you can redistribute it and/or modify it
  10   under the terms of the GNU Lesser General Public License as published by
  11   the Free Software Foundation; either version 2.1 of the License, or
  12   (at your option) any later version.
  13
  14   systemd is distributed in the hope that it will be useful, but
  15   WITHOUT ANY WARRANTY; without even the implied warranty of
  16   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  17   Lesser General Public License for more details.
  18
  19   You should have received a copy of the GNU Lesser General Public License
  20   along with systemd; If not, see <http://www.gnu.org/licenses/>.
  21 ***/
  22
  23 #include <net/ethernet.h>
  24 #include <arpa/inet.h>
  25
  26 #include "macro.h"
  27 #include "lldp-tlv.h"
  28
  29 int tlv_section_new(tlv_section **ret) {
  30         tlv_section *s;
  31
  32         s = new0(tlv_section, 1);
  33         if (!s)
  34                 return -ENOMEM;
  35
  36         *ret = s;
  37
  38         return 0;
  39 }
  40
  41 void tlv_section_free(tlv_section *m) {
  42
  43         if (!m)
  44                 return;
  45
  46         free(m);
  47 }
  48
  49 int tlv_packet_new(tlv_packet **ret) {
  50         tlv_packet *m;
  51
  52         m = new0(tlv_packet, 1);
  53         if (!m)
  54                 return -ENOMEM;
  55
  56         LIST_HEAD_INIT(m->sections);
  57
  58         *ret = m;
  59
  60         return 0;
  61 }
  62
  63 void tlv_packet_free(tlv_packet *m) {
  64         tlv_section *s, *n;
  65
  66         if (!m)
  67                 return;
  68
  69         LIST_FOREACH_SAFE(section, s, n, m->sections)
  70                 tlv_section_free(s);
  71
  72         free(m);
  73 }
  74
  75 int tlv_packet_append_bytes(tlv_packet *m, const void *data, size_t data_length) {
  76         uint8_t *p;
  77
  78         assert_return(m, -EINVAL);
  79         assert_return(data, -EINVAL);
  80         assert_return(data_length, -EINVAL);
  81
  82         if (m->length + data_length > ETHER_MAX_LEN)
  83                 return -ENOMEM;
  84
  85         p = m->pdu + m->length;
  86         memcpy(p, data, data_length);
  87         m->length += data_length;
  88
  89         return 0;
  90 }
  91
  92 int tlv_packet_append_u8(tlv_packet *m, uint8_t data) {
  93
  94         assert_return(m, -EINVAL);
  95
  96         return tlv_packet_append_bytes(m, &data, sizeof(uint8_t));
  97 }
  98
  99 int tlv_packet_append_u16(tlv_packet *m, uint16_t data) {
 100         uint16_t type;
 101
 102         assert_return(m, -EINVAL);
 103
 104         type = htons(data);
 105
 106         return tlv_packet_append_bytes(m, &type, sizeof(uint16_t));
 107 }
 108
 109 int tlv_packet_append_u32(tlv_packet *m, uint32_t data) {
 110         uint32_t type;
 111
 112         assert_return(m, -EINVAL);
 113
 114         type = htonl(data);
 115
 116         return tlv_packet_append_bytes(m, &type, sizeof(uint32_t));
 117 }
 118
 119 int tlv_packet_append_string(tlv_packet *m, char *data, uint16_t size) {
 120
 121         assert_return(m, -EINVAL);
 122
 123         return tlv_packet_append_bytes(m, data, size);
 124 }
 125
 126 int lldp_tlv_packet_open_container(tlv_packet *m, uint16_t type) {
 127
 128         assert_return(m, -EINVAL);
 129
 130         m->container_pos = m->pdu + m->length;
 131
 132         return tlv_packet_append_u16(m, type << 9);
 133 }
 134
 135 int lldp_tlv_packet_close_container(tlv_packet *m) {
 136         uint16_t type;
 137
 138         assert_return(m, -EINVAL);
 139         assert_return(m->container_pos, -EINVAL);
 140
 141         memcpy(&type, m->container_pos, sizeof(uint16_t));
 142
 143         type |= htons(((m->pdu + m->length) - (m->container_pos + 2)) & 0x01ff);
 144         memcpy(m->container_pos, &type, sizeof(uint16_t));
 145
 146         return 0;
 147 }
 148
 149 static inline int tlv_packet_read_internal(tlv_section *m, void **data) {
 150
 151         assert_return(m->read_pos, -EINVAL);
 152
 153         *data = m->read_pos;
 154
 155         return 0;
 156 }
 157
 158 int tlv_packet_read_u8(tlv_packet *m, uint8_t *data) {
 159         void *val;
 160         int r;
 161
 162         assert_return(m, -EINVAL);
 163
 164         r = tlv_packet_read_internal(m->container,  &val);
 165         if (r < 0)
 166                 return r;
 167
 168         memcpy(data, val, sizeof(uint8_t));
 169
 170         m->container->read_pos ++;
 171
 172         return 0;
 173 }
 174
 175 int tlv_packet_read_u16(tlv_packet *m, uint16_t *data) {
 176         uint16_t t;
 177         void *val;
 178         int r;
 179
 180         assert_return(m, -EINVAL);
 181
 182         r = tlv_packet_read_internal(m->container, &val);
 183         if (r < 0)
 184                 return r;
 185
 186         memcpy(&t, val, sizeof(uint16_t));
 187         *data = ntohs(t);
 188
 189         m->container->read_pos += 2;
 190
 191         return 0;
 192 }
 193
 194 int tlv_packet_read_u32(tlv_packet *m, uint32_t *data) {
 195         uint32_t t;
 196         void *val;
 197         int r;
 198
 199         assert_return(m, -EINVAL);
 200
 201         r = tlv_packet_read_internal(m->container, &val);
 202         if (r < 0)
 203                 return r;
 204
 205         memcpy(&t, val, sizeof(uint32_t));
 206         *data = ntohl(t);
 207
 208         m->container->read_pos += 4;
 209
 210         return r;
 211 }
 212
 213 int tlv_packet_read_string(tlv_packet *m, char **data, uint16_t *data_length) {
 214         void *val;
 215         int r;
 216
 217         assert_return(m, -EINVAL);
 218
 219         r = tlv_packet_read_internal(m->container, &val);
 220         if (r < 0)
 221                 return r;
 222
 223         *data = (char *) val;
 224         *data_length =  m->container->length;
 225
 226         m->container->read_pos += m->container->length;
 227
 228         return 0;
 229 }
 230
 231 int tlv_packet_read_bytes(tlv_packet *m, uint8_t **data, uint16_t *data_length) {
 232         void *val;
 233         int r;
 234
 235         assert_return(m, -EINVAL);
 236
 237         r = tlv_packet_read_internal(m->container, &val);
 238         if (r < 0)
 239                 return r;
 240
 241         *data = (uint8_t *) val;
 242         *data_length = m->container->length;
 243
 244         m->container->read_pos += m->container->length;
 245
 246         return 0;
 247 }
 248
 249 /* parse raw TLV packet */
 250 int tlv_packet_parse_pdu(tlv_packet *m, uint16_t size) {
 251         tlv_section *section, *tail;
 252         uint16_t t, l;
 253         uint8_t *p;
 254         int r;
 255
 256         assert_return(m, -EINVAL);
 257         assert_return(size, -EINVAL);
 258
 259         p = m->pdu;
 260
 261         /* extract ethernet herader */
 262         memcpy(&m->mac, p, ETH_ALEN);
 263         p += sizeof(struct ether_header);
 264
 265         for (l = 0; l <= size; ) {
 266                 r = tlv_section_new(&section);
 267                 if (r < 0)
 268                         return r;
 269
 270                 memcpy(&t, p, sizeof(uint16_t));
 271
 272                 section->type = ntohs(t) >> 9;
 273                 section->length = ntohs(t) & 0x01ff;
 274
 275                 if (section->type == LLDP_TYPE_END || section->type >=_LLDP_TYPE_MAX) {
 276                         tlv_section_free(section);
 277                         break;
 278                 }
 279
 280                 p += 2;
 281                 section->data = p;
 282
 283                 LIST_FIND_TAIL(section, m->sections, tail);
 284                 LIST_INSERT_AFTER(section, m->sections, tail, section);
 285
 286                 p += section->length;
 287                 l += (section->length + 2);
 288         }
 289
 290         return 0;
 291 }
 292
 293 int lldp_tlv_packet_enter_container(tlv_packet *m, uint16_t type) {
 294         tlv_section *s;
 295
 296         assert_return(m, -EINVAL);
 297
 298         LIST_FOREACH(section, s, m->sections)
 299                 if (s->type == type)
 300                         break;
 301         if (!s)
 302                 return -1;
 303
 304         m->container = s;
 305
 306         m->container->read_pos = s->data;
 307         if (!m->container->read_pos) {
 308                 m->container = 0;
 309                 return -1;
 310         }
 311
 312         return 0;
 313 }
 314
 315 int lldp_tlv_packet_exit_container(tlv_packet *m) {
 316         assert_return(m, -EINVAL);
 317
 318         m->container = 0;
 319
 320         return 0;
 321 }