lib/pud/wireformat/src/wireFormat.c

   1 /*
   2  * The olsr.org Optimized Link-State Routing daemon (olsrd)
   3  *
   4  * (c) by the OLSR project
   5  *
   6  * See our Git repository to find out who worked on this file
   7  * and thus is a copyright holder on it.
   8  *
   9  * All rights reserved.
  10  *
  11  * Redistribution and use in source and binary forms, with or without
  12  * modification, are permitted provided that the following conditions
  13  * are met:
  14  *
  15  * * Redistributions of source code must retain the above copyright
  16  *   notice, this list of conditions and the following disclaimer.
  17  * * Redistributions in binary form must reproduce the above copyright
  18  *   notice, this list of conditions and the following disclaimer in
  19  *   the documentation and/or other materials provided with the
  20  *   distribution.
  21  * * Neither the name of olsr.org, olsrd nor the names of its
  22  *   contributors may be used to endorse or promote products derived
  23  *   from this software without specific prior written permission.
  24  *
  25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  26  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  27  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  28  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  29  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  30  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  31  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  32  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  33  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  36  * POSSIBILITY OF SUCH DAMAGE.
  37  *
  38  * Visit http://www.olsr.org for more information.
  39  *
  40  * If you find this software useful feel free to make a donation
  41  * to the project. For more information see the website or contact
  42  * the copyright holders.
  43  *
  44  */
  45
  46 #include <OlsrdPudWireFormat/wireFormat.h>
  47
  48 #include <time.h>
  49
  50 /*
  51  * NodeIdType
  52  */
  53
  54 bool isValidNodeIdType(unsigned long long nodeIdType) {
  55         return
  56         (
  57                 (
  58                         (/* (nodeIdType >= PUD_NODEIDTYPE_GLOBAL_FIRST) && */ (nodeIdType <= PUD_NODEIDTYPE_GLOBAL_LAST)) ||
  59                         (   (nodeIdType >= PUD_NODEIDTYPE_LOCAL_FIRST ) &&    (nodeIdType <= PUD_NODEIDTYPE_LOCAL_LAST ))
  60                 )
  61         );
  62 }
  63
  64
  65 /*
  66  * Validity Time
  67  */
  68
  69 /** Determine the validity time in seconds from the OLSR wire format value */
  70 #define PUD_VALIDITY_TIME_FROM_OLSR(msn, lsn) ((((lsn) + 16) * (1 << (msn))) - 16)
  71
  72 /**
  73  Get the validity time from a message
  74
  75  @param validityTimeField
  76  A pointer to the validity time field
  77
  78  @return
  79  The validity time in seconds
  80  */
  81 unsigned long getValidityTime(uint8_t * validityTimeField) {
  82         return PUD_VALIDITY_TIME_FROM_OLSR(*validityTimeField >> 4, *validityTimeField % 16);
  83 }
  84
  85 /**
  86  Set the validity time of the position update message
  87
  88  @param validityTimeField
  89  A pointer to the validity time field
  90  @param validityTime
  91  The validity time in seconds
  92  */
  93 void setValidityTime(uint8_t * validityTimeField, unsigned long long validityTime) {
  94         unsigned int msn = 1;
  95         unsigned long long lsn = 0;
  96         unsigned long long upperBound;
  97
  98         upperBound = PUD_VALIDITY_TIME_FROM_OLSR(msn, 0);
  99         while ((msn < 16) && (validityTime >= upperBound)) {
 100                 msn++;
 101                 upperBound = PUD_VALIDITY_TIME_FROM_OLSR(msn, 0);
 102         }
 103         msn--;
 104
 105         if (unlikely(validityTime >= upperBound)) {
 106                 lsn = 15;
 107         } else {
 108                 unsigned long lowerBound = PUD_VALIDITY_TIME_FROM_OLSR(msn, 0);
 109                 unsigned long resolution = (1 << msn);
 110                 lsn = ((validityTime - lowerBound + (resolution >> 1)) / resolution);
 111         }
 112
 113         assert(msn <= 15);
 114         assert(lsn <= 15);
 115
 116         *validityTimeField = ((msn << 4) | lsn);
 117 }
 118
 119 /*
 120  * UplinkHeader
 121  */
 122
 123 /**
 124  Get the type of the uplink message
 125
 126  @param uplinkHeader
 127  A pointer to the uplink message
 128  @return
 129  The type of the uplink message
 130  */
 131 uint8_t getUplinkMessageType(UplinkHeader * uplinkHeader) {
 132         return uplinkHeader->type;
 133 }
 134
 135 /**
 136  Set the type of the uplink message
 137
 138  @param uplinkHeader
 139  A pointer to the uplink message
 140  @param type
 141  The type of the uplink message
 142  */
 143 void setUplinkMessageType(UplinkHeader * uplinkHeader,
 144                 uint8_t type) {
 145         uplinkHeader->type = type;
 146 }
 147
 148 /**
 149  Get the length of the uplink message
 150
 151  @param uplinkHeader
 152  A pointer to the uplink message
 153  @return
 154  The length of the uplink message
 155  */
 156 uint16_t getUplinkMessageLength(UplinkHeader * uplinkHeader) {
 157         return ntohs(uplinkHeader->length);
 158 }
 159
 160 /**
 161  Set the length of the uplink message
 162
 163  @param uplinkHeader
 164  A pointer to the uplink message
 165  @param length
 166  The length of the uplink message
 167  */
 168 void setUplinkMessageLength(UplinkHeader * uplinkHeader,
 169                 uint16_t length) {
 170         uplinkHeader->length = ntohs(length);
 171 }
 172
 173 /**
 174  Get the IPv6 status of the uplink message
 175
 176  @param uplinkHeader
 177  A pointer to the uplink message
 178  @return
 179  true when the uplink message is sent from an olsrd stack in IPv6 mode, false
 180  otherwise
 181  */
 182 bool getUplinkMessageIPv6(UplinkHeader * uplinkHeader) {
 183         return (uplinkHeader->ipv6 == 1);
 184 }
 185
 186 /**
 187  Set the IPv6 status of the uplink message
 188
 189  @param uplinkHeader
 190  A pointer to the uplink message
 191  @param ipv6
 192  The IPv6 status of the uplink message (true when the uplink message is sent
 193  from olsrd stack in IPv6 mode, false otherwise)
 194  */
 195 void setUplinkMessageIPv6(UplinkHeader * uplinkHeader,
 196                 bool ipv6) {
 197         uplinkHeader->ipv6 = ipv6 ? 1 : 0;
 198 }
 199
 200 /**
 201  Set the padding of the uplink message header
 202
 203  @param uplinkHeader
 204  A pointer to the uplink message
 205  @param pad
 206  The padding of the uplink message header
 207  */
 208 void setUplinkMessagePadding(UplinkHeader * uplinkHeader,
 209                 uint8_t pad) {
 210         uplinkHeader->pad = pad;
 211 }
 212
 213 /*
 214  * OLSR header
 215  */
 216
 217 /**
 218  Determine the size of an OLSR message
 219
 220  @param ipVersion
 221  The IP version
 222  @param olsrMessage
 223  A pointer to the OLSR message
 224  @return
 225  The size of the OLSR message
 226  */
 227 unsigned short getOlsrMessageSize(int ipVersion,
 228                 union olsr_message * olsrMessage) {
 229         if (ipVersion == AF_INET) {
 230                 return ntohs(olsrMessage->v4.olsr_msgsize);
 231         }
 232
 233         return ntohs(olsrMessage->v6.olsr_msgsize);
 234 }
 235
 236 /**
 237  Get the originator of an OLSR message
 238
 239  @param ipVersion
 240  The IP version (AF_INET or AF_INET6)
 241  @param olsrMessage
 242  A pointer to the OLSR message
 243  @return
 244  A pointer to the originator address
 245  */
 246 union olsr_ip_addr * getOlsrMessageOriginator(int ipVersion,
 247                 union olsr_message * olsrMessage) {
 248         if (ipVersion == AF_INET) {
 249                 return (union olsr_ip_addr *) &olsrMessage->v4.originator;
 250         }
 251
 252         return (union olsr_ip_addr *) &olsrMessage->v6.originator;
 253 }
 254
 255 /**
 256  Get the position update message in an OLSR message
 257
 258  @param ipVersion
 259  The IP version (AF_INET or AF_INET6)
 260  @param olsrMessage
 261  A pointer to the OLSR message
 262  @return
 263  A pointer to the position update message
 264  */
 265 PudOlsrPositionUpdate * getOlsrMessagePayload(int ipVersion,
 266                 union olsr_message * olsrMessage) {
 267         if (ipVersion == AF_INET) {
 268                 return (PudOlsrPositionUpdate *) &olsrMessage->v4.message;
 269         }
 270
 271         return (PudOlsrPositionUpdate *) &olsrMessage->v6.message;
 272 }
 273
 274 /*
 275  * PudOlsrPositionUpdate
 276  */
 277
 278 /**
 279  Get the version of the position update message
 280
 281  @param olsrGpsMessage
 282  A pointer to the position update message
 283  @return
 284  The version of the position update message
 285  */
 286 uint8_t getPositionUpdateVersion(
 287                 PudOlsrPositionUpdate * olsrGpsMessage) {
 288         return olsrGpsMessage->version;
 289 }
 290
 291 /**
 292  Set the version of the position update message
 293
 294  @param olsrGpsMessage
 295  A pointer to the position update message
 296  @param version
 297  The version of the position update message
 298  */
 299 void setPositionUpdateVersion(
 300                 PudOlsrPositionUpdate * olsrGpsMessage, uint8_t version) {
 301         olsrGpsMessage->version = version;
 302 }
 303
 304 /**
 305  Get the presence field of the position update message
 306
 307  @param olsrGpsMessage
 308  A pointer to the position update message
 309  @return
 310  The presence field of the position update message
 311  */
 312 uint32_t getPositionUpdatePresent(
 313                 PudOlsrPositionUpdate * olsrGpsMessage) {
 314         return ntohl(olsrGpsMessage->present);
 315 }
 316
 317 /**
 318  Set the presence field of the position update message
 319
 320  @param olsrGpsMessage
 321  A pointer to the position update message
 322  @param present
 323  The presence field of the position update message
 324  */
 325 void setPositionUpdatePresent(
 326                 PudOlsrPositionUpdate * olsrGpsMessage, uint32_t present) {
 327         olsrGpsMessage->present = htonl(present);
 328 }
 329
 330 /*
 331  * GpsInfo
 332  */
 333
 334 /**
 335  Convert the time of an OLSR message (the number of seconds after midnight) to
 336  a time structure, based on midnight of the current day.
 337
 338  @param olsrGpsMessage
 339  A pointer to the position update message
 340  @param baseDate
 341  The base date from which to determine the time (number of seconds since Epoch,
 342  UTC)
 343  @param nowStruct
 344  A pointer to the time structure into which to put the converted time
 345  */
 346 void getPositionUpdateTime(PudOlsrPositionUpdate * olsrGpsMessage,
 347                 time_t baseDate, struct tm *nowStruct) {
 348         uint32_t olsrTime = olsrGpsMessage->gpsInfo.time;
 349         unsigned int secNow;
 350
 351         time_t now = baseDate;
 352         gmtime_r(&now, nowStruct);
 353
 354         secNow = ((nowStruct->tm_hour * 60 * 60) + (nowStruct->tm_min * 60)
 355                         + nowStruct->tm_sec);
 356
 357         if (secNow <= (12 * 60 * 60)) {
 358                 /* we are now in the first 12h of the day */
 359                 if (unlikely(olsrTime > (secNow + (12 * 60 * 60)))) {
 360                         /* the message was sent more than 12h later in time:
 361                          the message was sent yesterday: adjust the date by -1 day */
 362                         now -= (24 * 60 * 60);
 363                         gmtime_r(&now, nowStruct);
 364                 }
 365         } else {
 366                 /* we are now in the last 12h of the day */
 367                 if (unlikely(olsrTime < (secNow - (12 * 60 * 60)))) {
 368                         /* the message was sent more than 12h earlier in time:
 369                          the message was sent tomorrow: adjust the date by +1 day */
 370                         now += (24 * 60 * 60);
 371                         gmtime_r(&now, nowStruct);
 372                 }
 373         }
 374
 375         nowStruct->tm_hour = ((olsrTime % (24 * 60 * 60)) / 3600);
 376         nowStruct->tm_min = ((olsrTime % (60 * 60)) / 60);
 377         nowStruct->tm_sec = (olsrTime % 60);
 378 }
 379
 380 /**
 381  Set the time of the position update message (the number of seconds after
 382  midnight)
 383
 384  @param olsrGpsMessage
 385  A pointer to the position update message
 386  @param hour
 387  The hours
 388  @param min
 389  The minutes
 390  @param sec
 391  The seconds
 392  */
 393 void setPositionUpdateTime(PudOlsrPositionUpdate * olsrGpsMessage,
 394                 int hour, int min, int sec) {
 395         olsrGpsMessage->gpsInfo.time = ((hour * 60 * 60) + (min * 60) + sec);
 396 }
 397
 398 /**
 399  Get the latitude of the position update message
 400
 401  @param olsrGpsMessage
 402  A pointer to the position update message
 403
 404  @return
 405  The latitude converted to degrees: [-90, 90>
 406  */
 407 double getPositionUpdateLatitude(
 408                 PudOlsrPositionUpdate * olsrGpsMessage) {
 409         uint32_t olsrLat = olsrGpsMessage->gpsInfo.lat;
 410         double lat = (double) olsrLat;
 411
 412         /* lat is in [0, 2^LATITUDE_BITS> */
 413
 414         /* take half of the rounding error */
 415         lat += 0.5;
 416
 417         lat /= (double) (1 << PUD_LATITUDE_BITS);
 418         /* lat is now in [0, 1> */
 419
 420         lat -= 0.5;
 421         /* lat is now in [-0.5, 0.5> */
 422
 423         lat *= 180.0;
 424         /* lat is now in [-90, 90> */
 425
 426         return lat;
 427 }
 428
 429 /**
 430  Set the latitude of the position update message
 431
 432  @param olsrGpsMessage
 433  A pointer to the position update message
 434  @param latitude
 435  The latitude in degrees: [-90, 90]
 436  */
 437 void setPositionUpdateLatitude(
 438                 PudOlsrPositionUpdate * olsrGpsMessage, double latitude) {
 439         double lat = latitude;
 440
 441         /* lat is in [-90, 90] */
 442         assert(lat >= -90.0);
 443         assert(lat <= 90.0);
 444
 445         lat /= 180.0;
 446         /* lat is now in [-0.5, 0.5] */
 447
 448         lat += 0.5;
 449         /* lat is now in [0, 1] */
 450
 451         lat *= (double) (1 << PUD_LATITUDE_BITS);
 452         /* lat is now in [0, LATITUDE_BITS] */
 453
 454         /* clip max */
 455         if (unlikely(lat > (double)((1 << PUD_LATITUDE_BITS) - 1))) {
 456                 lat = (double) ((1 << PUD_LATITUDE_BITS) - 1);
 457         }
 458         /* lat is now in [0, 2^LATITUDE_BITS> */
 459
 460         olsrGpsMessage->gpsInfo.lat = lrint(lat);
 461 }
 462
 463 /**
 464  Get the longitude of the position update message
 465
 466  @param olsrGpsMessage
 467  A pointer to the position update message
 468
 469  @return
 470  The longitude converted to degrees: [-180, 180>
 471  */
 472 double getPositionUpdateLongitude(
 473                 PudOlsrPositionUpdate * olsrGpsMessage) {
 474         uint32_t olsrLon = olsrGpsMessage->gpsInfo.lon;
 475         double lon = (double) olsrLon;
 476
 477         /* lon is in [0, 2^LONGITUDE_BITS> */
 478
 479         /* take half of the rounding error */
 480         lon += 0.5;
 481
 482         lon /= (1 << PUD_LONGITUDE_BITS);
 483         /* lon is now in [0, 1> */
 484
 485         lon -= 0.5;
 486         /* lon is now in [-0.5, 0.5> */
 487
 488         lon *= 360.0;
 489         /* lon is now in [-180, 180> */
 490
 491         return lon;
 492 }
 493
 494 /**
 495  Set the longitude of the position update message
 496
 497  @param olsrGpsMessage
 498  A pointer to the position update message
 499  @param longitude
 500  The longitude in degrees: [-90, 90]
 501  */
 502 void setPositionUpdateLongitude(
 503                 PudOlsrPositionUpdate * olsrGpsMessage, double longitude) {
 504         double lon = longitude;
 505
 506         /* lon is in [-180, 180] */
 507         assert(lon >= -180.0);
 508         assert(lon <= 180.0);
 509
 510         lon /= 360.0;
 511         /* lon is now in [-0.5, 0.5] */
 512
 513         lon += 0.5;
 514         /* lon is now in [0, 1] */
 515
 516         lon *= (double) (1 << PUD_LONGITUDE_BITS);
 517         /* lon is now in [0, LONGITUDE_BITS] */
 518
 519         /* clip max */
 520         if (unlikely(lon > (double)((1 << PUD_LATITUDE_BITS) - 1))) {
 521                 lon = (double) ((1 << PUD_LATITUDE_BITS) - 1);
 522         }
 523
 524         /* lon is now in [0, 2^LONGITUDE_BITS> */
 525
 526         olsrGpsMessage->gpsInfo.lon = lrint(lon);
 527 }
 528
 529 /**
 530  Get the altitude of the position update message
 531
 532  @param olsrGpsMessage
 533  A pointer to the position update message
 534
 535  @return
 536  The altitude in meters
 537  */
 538 long getPositionUpdateAltitude(
 539                 PudOlsrPositionUpdate * olsrGpsMessage) {
 540         return ((long) olsrGpsMessage->gpsInfo.alt + PUD_ALTITUDE_MIN);
 541 }
 542
 543 /**
 544  Set the altitude of the position update message
 545
 546  @param olsrGpsMessage
 547  A pointer to the position update message
 548  @param altitude
 549  The altitude in meters
 550  */
 551 void setPositionUpdateAltitude(
 552                 PudOlsrPositionUpdate * olsrGpsMessage, double altitude) {
 553         double alt = altitude;
 554
 555         if (unlikely(alt > PUD_ALTITUDE_MAX)) {
 556                 alt = PUD_ALTITUDE_MAX;
 557         } else if (unlikely(alt < PUD_ALTITUDE_MIN)) {
 558                 alt = PUD_ALTITUDE_MIN;
 559         }
 560
 561         alt -= PUD_ALTITUDE_MIN;
 562
 563         olsrGpsMessage->gpsInfo.alt = lrint(alt);
 564 }
 565
 566 /**
 567  Get the speed of the position update message
 568
 569  @param olsrGpsMessage
 570  A pointer to the position update message
 571
 572  @return
 573  The speed in kph
 574  */
 575 unsigned long getPositionUpdateSpeed(
 576                 PudOlsrPositionUpdate * olsrGpsMessage) {
 577         return olsrGpsMessage->gpsInfo.speed;
 578 }
 579
 580 /**
 581  Set the speed of the position update message
 582
 583  @param olsrGpsMessage
 584  A pointer to the position update message
 585  @param speed
 586  The speed in kph
 587  */
 588 void setPositionUpdateSpeed(
 589                 PudOlsrPositionUpdate * olsrGpsMessage, double speed) {
 590         double spd = speed;
 591
 592         if (unlikely(speed < 0)) {
 593                 spd = 0;
 594         } else if (unlikely(speed > PUD_SPEED_MAX)) {
 595                 spd = PUD_SPEED_MAX;
 596         }
 597
 598         olsrGpsMessage->gpsInfo.speed = lrint(spd);
 599 }
 600
 601 /**
 602  Get the track angle of the position update message
 603
 604  @param olsrGpsMessage
 605  A pointer to the position update message
 606
 607  @return
 608  The track angle in degrees
 609  */
 610 unsigned long getPositionUpdateTrack(
 611                 PudOlsrPositionUpdate * olsrGpsMessage) {
 612         return olsrGpsMessage->gpsInfo.track;
 613 }
 614
 615 /**
 616  Set the track angle of the position update message
 617
 618  @param olsrGpsMessage
 619  A pointer to the position update message
 620  @param track
 621  The track angle in degrees
 622  */
 623 void setPositionUpdateTrack(
 624                 PudOlsrPositionUpdate * olsrGpsMessage, double track) {
 625         olsrGpsMessage->gpsInfo.track = lrint(track);
 626 }
 627
 628 /**
 629  Get the HDOP of the position update message
 630
 631  @param olsrGpsMessage
 632  A pointer to the position update message
 633
 634  @return
 635  The HDOP
 636  */
 637 double getPositionUpdateHdop(
 638                 PudOlsrPositionUpdate * olsrGpsMessage) {
 639         return (olsrGpsMessage->gpsInfo.hdop * PUD_HDOP_RESOLUTION);
 640 }
 641
 642 /**
 643  Set the HDOP of the position update message
 644
 645  @param olsrGpsMessage
 646  A pointer to the position update message
 647  @param hdop
 648  The HDOP
 649  */
 650 void setPositionUpdateHdop(PudOlsrPositionUpdate * olsrGpsMessage,
 651                 double hdop) {
 652         double hdopInternal = hdop;
 653
 654         if (unlikely(hdopInternal > PUD_HDOP_MAX)) {
 655                 hdopInternal = PUD_HDOP_MAX;
 656         }
 657
 658         olsrGpsMessage->gpsInfo.hdop = lrint(hdopInternal / PUD_HDOP_RESOLUTION);
 659 }
 660
 661 /*
 662  * NodeInfo
 663  */
 664
 665 /**
 666  Get the nodeIdType of the position update message
 667
 668  @param ipVersion
 669  The IP version (AF_INET or AF_INET6)
 670  @param olsrGpsMessage
 671  A pointer to the position update message
 672
 673  @return
 674  The nodeIdType
 675  */
 676 NodeIdType getPositionUpdateNodeIdType(int ipVersion,
 677                 PudOlsrPositionUpdate * olsrGpsMessage) {
 678         if (getPositionUpdatePresent(olsrGpsMessage) & PUD_PRESENT_ID) {
 679                 return olsrGpsMessage->nodeInfo.nodeIdType;
 680         }
 681
 682         return ((ipVersion == AF_INET) ? PUD_NODEIDTYPE_IPV4 : PUD_NODEIDTYPE_IPV6);
 683 }
 684
 685 /**
 686  Set the nodeIdType of the position update message
 687
 688  @param olsrGpsMessage
 689  A pointer to the position update message
 690  @param nodeIdType
 691  The nodeIdType
 692  */
 693 void setPositionUpdateNodeIdType(
 694                 PudOlsrPositionUpdate * olsrGpsMessage, NodeIdType nodeIdType) {
 695         olsrGpsMessage->nodeInfo.nodeIdType = nodeIdType;
 696 }
 697
 698 /**
 699  Get the nodeId and its size, accounting for nodeId presence
 700
 701  @param ipVersion
 702  The IP version (AF_INET or AF_INET6)
 703  @param olsrMessage
 704  A pointer to the OLSR message
 705  @param nodeId
 706  A pointer to the location where a pointer to the nodeId (as contained in the
 707  olsrMessage) can be stored
 708  @param nodeIdSize
 709  A pointer to the location where the number of bytes in the nodeId can be
 710  stored
 711  */
 712 void getPositionUpdateNodeId(int ipVersion, union olsr_message * olsrMessage,
 713                 unsigned char ** nodeId, unsigned int * nodeIdSize) {
 714         PudOlsrPositionUpdate * olsrGpsMessage = getOlsrMessagePayload(ipVersion,
 715                         olsrMessage);
 716
 717         *nodeId = &olsrGpsMessage->nodeInfo.nodeId;
 718
 719         switch (getPositionUpdateNodeIdType(ipVersion, olsrGpsMessage)) {
 720         case PUD_NODEIDTYPE_MAC: /* hardware address */
 721                 *nodeIdSize = PUD_NODEIDTYPE_MAC_BYTES;
 722                 break;
 723
 724         case PUD_NODEIDTYPE_MSISDN: /* an MSISDN number */
 725                 *nodeIdSize = PUD_NODEIDTYPE_MSISDN_BYTES;
 726                 break;
 727
 728         case PUD_NODEIDTYPE_TETRA: /* a Tetra number */
 729                 *nodeIdSize = PUD_NODEIDTYPE_TETRA_BYTES;
 730                 break;
 731
 732         case PUD_NODEIDTYPE_DNS: /* DNS name */
 733           {
 734             unsigned int len = 0;
 735             unsigned char * idx = *nodeId;
 736             unsigned char * lastPayloadByte = &((unsigned char *)olsrMessage)[getOlsrMessageSize(ipVersion, olsrMessage) - 1];
 737             while ((*idx != '\0') && (idx <= lastPayloadByte)) {
 738               idx++;
 739               len++;
 740             }
 741             *nodeIdSize = len;
 742           }
 743                 break;
 744
 745         case PUD_NODEIDTYPE_UUID: /* a UUID number */
 746                 *nodeIdSize = PUD_NODEIDTYPE_UUID_BYTES;
 747                 break;
 748
 749         case PUD_NODEIDTYPE_MMSI: /* an AIS MMSI number */
 750                 *nodeIdSize = PUD_NODEIDTYPE_MMSI_BYTES;
 751                 break;
 752
 753         case PUD_NODEIDTYPE_URN: /* a URN number */
 754                 *nodeIdSize = PUD_NODEIDTYPE_URN_BYTES;
 755                 break;
 756
 757         case PUD_NODEIDTYPE_MIP: /* a MIP OID number */
 758                 *nodeIdSize = PUD_NODEIDTYPE_MIP_BYTES;
 759                 break;
 760
 761         case PUD_NODEIDTYPE_192:
 762                 *nodeIdSize = PUD_NODEIDTYPE_192_BYTES;
 763                 break;
 764
 765         case PUD_NODEIDTYPE_193:
 766                 *nodeIdSize = PUD_NODEIDTYPE_193_BYTES;
 767                 break;
 768
 769         case PUD_NODEIDTYPE_194:
 770                 *nodeIdSize = PUD_NODEIDTYPE_194_BYTES;
 771                 break;
 772
 773         case PUD_NODEIDTYPE_IPV4: /* IPv4 address */
 774         case PUD_NODEIDTYPE_IPV6: /* IPv6 address */
 775         default: /* unsupported */
 776         {
 777                 *nodeId = (unsigned char *) getOlsrMessageOriginator(ipVersion,
 778                                 olsrMessage);
 779                 *nodeIdSize =
 780                                 (ipVersion == AF_INET) ?
 781                                                 PUD_NODEIDTYPE_IPV4_BYTES : PUD_NODEIDTYPE_IPV6_BYTES;
 782         }
 783                 break;
 784         }
 785
 786         return;
 787 }
 788
 789 /**
 790  Set the nodeId of the position update message
 791
 792  @param olsrGpsMessage
 793  A pointer to the position update message
 794  @param nodeId
 795  The nodeId
 796  @param nodeIdSize
 797  The number of bytes in nodeId
 798  @param padWithNullByte
 799  When true then an extra '\0' byte will be added at the end
 800  */
 801 void setPositionUpdateNodeId(
 802                 PudOlsrPositionUpdate * olsrGpsMessage, unsigned char * nodeId,
 803                 unsigned int nodeIdSize, bool padWithNullByte) {
 804         memcpy(&olsrGpsMessage->nodeInfo.nodeId, nodeId, nodeIdSize);
 805         if (unlikely(padWithNullByte)) {
 806                 (&olsrGpsMessage->nodeInfo.nodeId)[nodeIdSize] = '\0';
 807         }
 808 }
 809
 810 /**
 811  Convert the node information to the node information for an OLSR message and
 812  put it in the PUD message in the OLSR message. Also updates the PUD message
 813  presence field to signal whether or not an ID is in the message.
 814
 815  @param ipVersion
 816  The IP version (AF_INET or AF_INET6)
 817  @param olsrGpsMessage
 818  A pointer to the PUD message in the OLSR message
 819  @param olsrMessageSize
 820  The maximum number of bytes available for the olsrMessage
 821  @param nodeIdType
 822  The nodeIdType
 823  @param nodeId
 824  The (configured) nodeId in binary/wireformat representation
 825  @param nodeIdLength
 826  The number of bytes in the nodeId
 827
 828  @return
 829  The number of bytes written in the PUD message in the OLSR message (for ALL
 830  the node information)
 831  */
 832 size_t setPositionUpdateNodeInfo(int ipVersion,
 833                 PudOlsrPositionUpdate * olsrGpsMessage, unsigned int olsrMessageSize,
 834                 NodeIdType nodeIdType, unsigned char * nodeId, size_t nodeIdLength) {
 835         unsigned int length = 0;
 836
 837         setPositionUpdateNodeIdType(olsrGpsMessage, nodeIdType);
 838         switch (nodeIdType) {
 839         case PUD_NODEIDTYPE_MAC: /* hardware address */
 840         case PUD_NODEIDTYPE_MSISDN: /* an MSISDN number */
 841         case PUD_NODEIDTYPE_TETRA: /* a Tetra number */
 842         case PUD_NODEIDTYPE_UUID: /* a UUID number */
 843         case PUD_NODEIDTYPE_MMSI: /* an AIS MMSI number */
 844         case PUD_NODEIDTYPE_URN: /* a URN number */
 845         case PUD_NODEIDTYPE_MIP: /* a MIP OID number */
 846         case PUD_NODEIDTYPE_192:
 847         case PUD_NODEIDTYPE_193:
 848         case PUD_NODEIDTYPE_194:
 849                 length = nodeIdLength;
 850                 setPositionUpdateNodeId(olsrGpsMessage, nodeId, nodeIdLength, false);
 851                 break;
 852
 853         case PUD_NODEIDTYPE_DNS: /* DNS name */
 854         {
 855                 long charsAvailable = olsrMessageSize
 856                                 - (PUD_OLSRWIREFORMATSIZE + sizeof(NodeInfo)
 857                                                 - sizeof(olsrGpsMessage->nodeInfo.nodeId)) - 1;
 858
 859                 length = nodeIdLength + 1;
 860                 if (unlikely((long) length > charsAvailable)) {
 861                         length = charsAvailable;
 862                 }
 863
 864                 // FIXME do not pad with a null byte (compatibility breaking change!)
 865                 setPositionUpdateNodeId(olsrGpsMessage, nodeId, length, true);
 866         }
 867                 break;
 868
 869         case PUD_NODEIDTYPE_IPV4: /* IPv4 address */
 870         case PUD_NODEIDTYPE_IPV6: /* IPv6 address */
 871                 /* explicit return: no nodeId information in message */
 872                 return 0;
 873
 874         default: /* unsupported */
 875                 /* fallback to IP address */
 876                 setPositionUpdateNodeIdType(olsrGpsMessage,
 877                                 (ipVersion == AF_INET) ? PUD_NODEIDTYPE_IPV4 :
 878                                 PUD_NODEIDTYPE_IPV6);
 879
 880                 /* explicit return: no nodeId information in message */
 881                 return 0;
 882         }
 883
 884         setPositionUpdatePresent(olsrGpsMessage,
 885                         getPositionUpdatePresent(olsrGpsMessage) | PUD_PRESENT_ID);
 886         return ((sizeof(NodeInfo)
 887                         - (sizeof(olsrGpsMessage->nodeInfo.nodeId) /* nodeId placeholder */))
 888                         + length);
 889 }
 890
 891 /*
 892  * UplinkClusterLeader
 893  */
 894
 895 /**
 896  Get the version of the cluster leader message
 897
 898  @param clusterLeaderMessage
 899  A pointer to the cluster leader message
 900  @return
 901  The version of the cluster leader message
 902  */
 903 uint8_t getClusterLeaderVersion(
 904                 UplinkClusterLeader * clusterLeaderMessage) {
 905         return clusterLeaderMessage->version;
 906 }
 907
 908 /**
 909  Set the version of the cluster leader message
 910
 911  @param clusterLeaderMessage
 912  A pointer to the cluster leader message
 913  @param version
 914  The version of the cluster leader message
 915  */
 916 void setClusterLeaderVersion(
 917                 UplinkClusterLeader * clusterLeaderMessage, uint8_t version) {
 918         clusterLeaderMessage->version = version;
 919 }
 920
 921 /**
 922  Get the originator of a cluster leader message
 923
 924  @param ipVersion
 925  The IP version (AF_INET or AF_INET6)
 926  @param clusterLeaderMessage
 927  A pointer to the cluster leader message
 928  @return
 929  A pointer to the originator address
 930  */
 931 union olsr_ip_addr * getClusterLeaderOriginator(int ipVersion,
 932                 UplinkClusterLeader * clusterLeaderMessage) {
 933         if (ipVersion == AF_INET) {
 934                 return (union olsr_ip_addr *) &clusterLeaderMessage->leader.v4.originator;
 935         }
 936
 937         return (union olsr_ip_addr *) &clusterLeaderMessage->leader.v6.originator;
 938 }
 939
 940 /**
 941  Get the cluster leader of a cluster leader message
 942
 943  @param ipVersion
 944  The IP version (AF_INET or AF_INET6)
 945  @param clusterLeaderMessage
 946  A pointer to the cluster leader message
 947  @return
 948  A pointer to the clust leader address
 949  */
 950 union olsr_ip_addr * getClusterLeaderClusterLeader(int ipVersion,
 951                 UplinkClusterLeader * clusterLeaderMessage) {
 952         if (ipVersion == AF_INET) {
 953                 return (union olsr_ip_addr *) &clusterLeaderMessage->leader.v4.clusterLeader;
 954         }
 955
 956         return (union olsr_ip_addr *) &clusterLeaderMessage->leader.v6.clusterLeader;
 957 }