staging/mops_ext_lldp.c

   1 /*
   2  * Mausezahn - A fast versatile traffic generator
   3  * Copyright (C) 2008-2010 Herbert Haas
   4  *
   5  * This program is free software; you can redistribute it and/or modify it under
   6  * the terms of the GNU General Public License version 2 as published by the
   7  * Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  11  * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
  12  * details.
  13  *
  14  * You should have received a copy of the GNU General Public License along with
  15  * this program; if not, see http://www.gnu.org/licenses/gpl-2.0.html
  16  *
  17 */
  18
  19
  20 #include "mz.h"
  21 #include "mops.h"
  22 #include "cli.h"
  23
  24
  25
  26 // Initialization function - specify defaults here!
  27 //
  28 int mops_init_pdesc_lldp(struct mops *mp)
  29 {
  30         struct mops_ext_lldp * pd;
  31         int i=0;
  32
  33         if (mp->p_desc == NULL) return 1;  // p_desc not properly assigned
  34         pd = mp->p_desc;
  35
  36         mp->eth_type = 0x88cc;
  37         str2hex("01:80:c2:00:00:0e", mp->eth_dst, 6);
  38         mp->ndelay.tv_sec = 30;
  39         mp->ndelay.tv_nsec = 0;
  40
  41         // get interface index for that packet
  42         i = mops_get_device_index(mp->device);
  43
  44         pd->non_conform = 0;
  45         pd->chassis_id_subtype = 4; // MAC address
  46         if (pd->chassis_id==NULL) pd->chassis_id = malloc(255);
  47         if (pd->chassis_id==NULL) return 1;
  48         memcpy((void*) pd->chassis_id, (void*) device_list[i].mac_mops, 6);
  49         pd->chassis_id_len = 6;
  50         pd->port_id_subtype = 5; // interface name
  51         pd->port_id_len = strnlen(mp->device, 15);
  52         if (pd->port_id==NULL) pd->port_id = malloc(255);
  53         if (pd->port_id==NULL) return 1;
  54         memcpy((void*) pd->port_id, (void*) mp->device, pd->port_id_len);
  55         pd->TTL = 120;
  56         pd->optional_tlvs_s = 0;
  57         if (pd->optional_tlvs==NULL) pd->optional_tlvs = malloc(MAX_LLDP_OPT_TLVS);
  58         if (pd->optional_tlvs == NULL) return 1;
  59         return 0;
  60 }
  61
  62
  63
  64 int mops_update_lldp (struct mops * mp)
  65 {
  66         struct mops_ext_lldp * pd;
  67
  68         pd = mp->p_desc;
  69         if (pd==NULL) return 1;  // no valid pointer to a p_desc
  70         mp->msg_s = 0; // important! Otherwise the msg would get longer and longer after each call!
  71
  72         switch (pd->non_conform) {
  73
  74          case 0: // Derive mandatory TLVs from struct entries and insert optional_tlvs
  75                 mp->msg_s += mops_lldp_tlv_chassis(mp->msg,
  76                                                    pd->chassis_id_subtype,
  77                                                    pd->chassis_id_len,
  78                                                    pd->chassis_id);
  79                 mp->msg_s += mops_lldp_tlv_port(&mp->msg[mp->msg_s],
  80                                                 pd->port_id_subtype,
  81                                                 pd->port_id_len,
  82                                                 pd->port_id);
  83                 mp->msg_s += mops_lldp_tlv_TTL(&mp->msg[mp->msg_s],
  84                                                pd->TTL);
  85                 if (pd->optional_tlvs_s) {
  86                         memcpy((void*) &mp->msg[mp->msg_s],
  87                                (void*) pd->optional_tlvs,
  88                                pd->optional_tlvs_s);
  89                         mp->msg_s += pd->optional_tlvs_s;
  90                 }
  91                 mp->msg_s += mops_lldp_tlv_end(&mp->msg[mp->msg_s]);
  92                 break;
  93
  94          case 1: // User defined ALL TLVs (i. e. ignore struct entries)
  95                 if (pd->optional_tlvs_s) {
  96                         memcpy((void*) &mp->msg[mp->msg_s],
  97                                (void*) pd->optional_tlvs,
  98                                pd->optional_tlvs_s);
  99                         mp->msg_s += pd->optional_tlvs_s;
 100                 }
 101                 mp->msg_s += mops_lldp_tlv_end(&mp->msg[mp->msg_s]);
 102                 break;
 103          default:
 104                 return 1;
 105         }
 106         return 0;
 107 }
 108
 109
 110 ///////////////////////////////////////////////////////////////////////////////
 111 //                                                                           //
 112 // Below are utility functions to creade the LLDPU. From these, the          //
 113 // following can be used for the optional part:                              //
 114 //                                                                           //
 115 //                                                                           //
 116 //                                                                           //
 117 //                                                                           //
 118
 119 /*
 120
 121 int  mops_lldp_opt_tlv          (struct mops *mp, int type, int len, u_int8_t *value)
 122 int  mops_lldp_opt_tlv_chassis  (struct mops *mp, int subtype, int len, u_int8_t *cid)
 123 int  mops_lldp_opt_tlv_port     (struct mops *mp, int subtype, int len, u_int8_t *pid)
 124 int  mops_lldp_opt_tlv_TTL      (struct mops *mp, int ttl)
 125 int  mops_lldp_opt_tlv_vlan     (struct mops *mp, int vlan)
 126 int  mops_lldp_opt_tlv_end      (struct mops *mp)
 127 int  mops_lldp_opt_tlv_bad      (struct mops *mp, int type, int badlen, int len, u_int8_t *value)
 128 int  mops_lldp_opt_tlv_org      (struct mops *mp, int oui, int subtype, int len, u_int8_t *inf)
 129
 130 */
 131
 132
 133 //                                                                           //
 134 //                                                                           //
 135 //                                                                           //
 136 //                                                                           //
 137 //                                                                           //
 138 //                                                                           //
 139 //                                                                           //
 140 //                                                                           //
 141 ///////////////////////////////////////////////////////////////////////////////
 142
 143
 144
 145
 146
 147 // Creates a LLDP TLV for a given type number and value string. The result will
 148 // be written into 'tlv'.
 149 //
 150 // NOTE: len must be given and indicates the length of value.
 151 //
 152 // RETURN VALUE: - Total number of bytes of this tlv
 153 //               - 0 upon error
 154 //
 155 int mops_lldp_tlv (u_int8_t *tlv, int type, int len, u_int8_t *value)
 156 {
 157         u_int16_t tl=0, tln=0;
 158
 159         if ((type>127) || (len>511)) return 0;
 160
 161         tl = type << 9;
 162         tl |= len;
 163
 164         tln = htons(tl);
 165         memcpy((void*) tlv, (void*) &tln, 2);
 166         memcpy((void*) &tlv[2], (void*) value, len);
 167
 168         return len+2;
 169 }
 170
 171
 172 // Same as above but **adds the TLVs to the 'pd->optional_tlvs' string.**
 173 // It also checks if MAX_LLDP_OPT_TLVS is exceeded.
 174 //
 175 // NOTE: first argument is a pointer to that mops!
 176 //
 177 // RETURN VALUE: - 0 upon error (no more space)
 178 //               - Total number of bytes written
 179 //
 180 int mops_lldp_opt_tlv (struct mops * mp, int type, int len, u_int8_t *value)
 181 {
 182         struct mops_ext_lldp * pd;
 183         u_int8_t tmp[MAX_LLDP_OPT_TLVS]; // this *must* be sufficient in length
 184         int l;
 185
 186         if (mp->p_desc == NULL) return 1;  // p_desc not properly assigned
 187         pd = mp->p_desc;
 188
 189         l = mops_lldp_tlv (tmp, type, len, value);
 190
 191         if ((MAX_LLDP_OPT_TLVS - pd->optional_tlvs_s)< (l+1)) return -1; // not enough space
 192         memcpy((void*) (pd->optional_tlvs + pd->optional_tlvs_s), (void*) tmp, l);
 193         pd->optional_tlvs_s += l;
 194         return l;
 195 }
 196
 197
 198 ///////////////////////////////////////////////////////////////////////////////
 199 //                                                                           //
 200 //                                                                           //
 201 //                                                                           //
 202 ///////////////////////////////////////////////////////////////////////////////
 203
 204
 205
 206 // Creates a Chassis ID TLV -- the first mandatory TLV.
 207 // The result will be written into 'tlv'.
 208 //
 209 // RETURN VALUE: - Total number of bytes within tlv
 210 //               - 0 upon error
 211 //
 212 int mops_lldp_tlv_chassis (u_int8_t *tlv, int subtype, int len, u_int8_t *cid)
 213 {
 214         u_int8_t tmp[256];
 215
 216         if ((len>255) || (subtype>255)) return 0;
 217
 218         tmp[0] = (u_int8_t) subtype;
 219         memcpy((void*) (tmp+1), (void*) cid, len);
 220         return mops_lldp_tlv(tlv, 1, len+1, tmp);
 221
 222 }
 223
 224 // Same but for optional tlv string
 225 int mops_lldp_opt_tlv_chassis (struct mops *mp, int subtype, int len, u_int8_t *cid)
 226 {
 227         u_int8_t tmp[256];
 228
 229         if ((len>255) || (subtype>255)) return 0;
 230         tmp[0] = (u_int8_t) subtype;
 231         memcpy((void*) (tmp+1), (void*) cid, len);
 232         return mops_lldp_opt_tlv(mp, 1, len+1, tmp);
 233 }
 234
 235
 236
 237 ///////////////////////////////////////////////////////////////////////////////
 238 //                                                                           //
 239 //                                                                           //
 240 //                                                                           //
 241 ///////////////////////////////////////////////////////////////////////////////
 242
 243
 244
 245 // Creates a Port ID TLV -- the second mandatory TLV.
 246 // The result will be written into 'tlv'.
 247 //
 248 // RETURN VALUE: - Total number of bytes within tlv
 249 //               - 0 upon error
 250 //
 251 int mops_lldp_tlv_port (u_int8_t *tlv, int subtype, int len, u_int8_t *pid)
 252 {
 253         u_int8_t tmp[256];
 254
 255         if ((len>255) || (subtype>255)) return 0;
 256
 257         tmp[0] = (u_int8_t) subtype;
 258         memcpy((void*) (tmp+1), (void*) pid, len);
 259         return mops_lldp_tlv(tlv, 2, len+1, tmp);
 260 }
 261
 262 // Same but for optional tlv string
 263 int mops_lldp_opt_tlv_port (struct mops *mp, int subtype, int len, u_int8_t *pid)
 264 {
 265         u_int8_t tmp[256];
 266
 267         if ((len>255) || (subtype>255)) return 0;
 268         tmp[0] = (u_int8_t) subtype;
 269         memcpy((void*) (tmp+1), (void*) pid, len);
 270         return mops_lldp_opt_tlv(mp, 2, len+1, tmp);
 271 }
 272
 273
 274 ///////////////////////////////////////////////////////////////////////////////
 275 //                                                                           //
 276 //                                                                           //
 277 //                                                                           //
 278 ///////////////////////////////////////////////////////////////////////////////
 279
 280
 281 // Creates a TTL TLV -- the third mandatory TLV.
 282 // The result will be written into 'tlv'.
 283 //
 284 // RETURN VALUE: - Total number of bytes within tlv
 285 //               - 0 upon error
 286 //
 287 int mops_lldp_tlv_TTL (u_int8_t *tlv, int ttl)
 288 {
 289         u_int16_t ttlh=0, ttln=0;
 290
 291         if (ttl>0xffff) return 0;
 292
 293         ttlh = (u_int16_t) ttl;
 294         ttln = htons(ttlh);
 295
 296         return mops_lldp_tlv(tlv, 3, 2, (u_int8_t*) &ttln);
 297 }
 298
 299
 300 // Same but for optional tlv string
 301 int mops_lldp_opt_tlv_TTL (struct mops *mp, int ttl)
 302 {
 303         u_int16_t ttlh=0, ttln=0;
 304
 305         if (ttl>0xffff) return 0;
 306
 307         ttlh = (u_int16_t) ttl;
 308         ttln = htons(ttlh);
 309
 310         return mops_lldp_opt_tlv(mp, 3, 2, (u_int8_t*) &ttln);
 311 }
 312
 313 ///////////////////////////////////////////////////////////////////////////////
 314 //                                                                           //
 315 //                                                                           //
 316 //                                                                           //
 317 ///////////////////////////////////////////////////////////////////////////////
 318
 319
 320 // Creates an End of LLDPDU TLV -- the last mandatory TLV.
 321 // The result will be written into 'tlv'.
 322 //
 323 // RETURN VALUE: - Total number of bytes within tlv
 324 //               - 0 upon error
 325 //
 326 int mops_lldp_tlv_end (u_int8_t *tlv)
 327 {
 328         tlv[0] = 0x00;
 329         tlv[1] = 0x00;
 330         return 2;
 331 }
 332
 333 // Same but for optional tlv string
 334 int mops_lldp_opt_tlv_end (struct mops *mp)
 335 {
 336         struct mops_ext_lldp * pd;
 337
 338         if (mp->p_desc == NULL) return 1;  // p_desc not properly assigned
 339         pd = mp->p_desc;
 340
 341         if ((MAX_LLDP_OPT_TLVS - pd->optional_tlvs_s) > 2) {
 342                 pd->optional_tlvs[pd->optional_tlvs_s++] = 0x00;
 343                 pd->optional_tlvs[pd->optional_tlvs_s++] = 0x00;
 344                 return 2;
 345         } else
 346                 return 0;
 347 }
 348
 349
 350 ///////////////////////////////////////////////////////////////////////////////
 351 //                                                                           //
 352 //                                                                           //
 353 //                                                                           //
 354 ///////////////////////////////////////////////////////////////////////////////
 355
 356
 357 // Creates a 'bad' LLDP TLV for a given type number and value string.
 358 // The result will be appended into 'pd->optional_tlvs'
 359 //
 360 // NOTE: 'len' must be given and indicates the TRUE length of value.
 361 //       'badlen' can be any number and is used as official length within the TLV
 362 //
 363 // RETURN VALUE: - Total number of bytes within tlv
 364 //               - 0 upon error
 365 //
 366 int mops_lldp_opt_tlv_bad (struct mops *mp,
 367                        int type,
 368                        int badlen,
 369                        int len,
 370                        u_int8_t *value)
 371 {
 372         u_int16_t tl=0, tln=0;
 373         u_int8_t tlv[512];
 374         struct mops_ext_lldp * pd = mp->p_desc;
 375
 376         if ((type>127) || (len>511) || (badlen>511)) return 0;
 377         if ((MAX_LLDP_OPT_TLVS - pd->optional_tlvs_s) < (len+3)) return 0;
 378
 379         tl = type << 9;
 380         tl |= badlen;
 381
 382         tln = htons(tl);
 383         memcpy((void*) tlv, (void*) &tln, 2);
 384         memcpy((void*) &tlv[2], (void*) value, len);
 385         // this detour has historical reasons ;-)
 386         memcpy((void*) (pd->optional_tlvs + pd->optional_tlvs_s), (void*) tlv, len+2);
 387         pd->optional_tlvs += len+2;
 388
 389         return len+2;
 390 }
 391
 392
 393
 394 // Creates a Organisational-specific TLV -- the second mandatory TLV.
 395 // The result will be appended into 'pd->optional_tlvs'
 396 //
 397 // RETURN VALUE: - Total number of bytes within tlv
 398 //               - 0 upon error
 399 //
 400 int mops_lldp_opt_tlv_org (struct mops *mp,
 401                        int oui,
 402                        int subtype,
 403                        int len,
 404                        u_int8_t *inf)
 405 {
 406         u_int8_t tmp[512];
 407         u_int8_t *x;
 408         u_int32_t oui_n = (u_int32_t) oui;
 409
 410         if ((len>507) || (subtype>255) || (oui_n>0xffffff)) return 0;
 411
 412         x = (u_int8_t *) &oui_n;
 413         tmp[0] = *(x+2);
 414         tmp[1] = *(x+1);
 415         tmp[2] = *x;
 416         tmp[3] = (u_int8_t) subtype;
 417         memcpy((void*) (tmp+4), (void*) inf, len);
 418         return mops_lldp_opt_tlv(mp, 127, len+4, tmp);
 419 }
 420
 421
 422 int mops_lldp_opt_tlv_vlan (struct mops *mp,
 423                          int vlan)
 424 {
 425         u_int16_t vid;
 426         if (vlan>0xffff) return 0; // yes, we also allow VLAN IDs > 4095
 427         vid = htons(vlan);
 428         return mops_lldp_opt_tlv_org (mp, 0x80c2, 1, 2, (u_int8_t*) &vid);
 429 }
 430