PUD: add getOlsrMessageSize function
[olsrd.git] / lib / pud / src / wireFormat.c
1 #include "wireFormat.h"
2
3 /* Plugin includes */
4 #include "compiler.h"
5
6 /* System includes */
7 #include <stdlib.h>
8 #include <stdbool.h>
9 #include <math.h>
10 #include <assert.h>
11 #include <netinet/in.h>
12
13 /*
14  * GPS Information Conversion Functions For OLSR GPS Wire Format
15  */
16
17 /* ************************************************************************
18  * OLSR Header
19  * ************************************************************************ */
20
21 /**
22  Determine the originator of an OLSR message
23
24  @param ipVersion
25  The IP version
26  @param olsrMessage
27  A pointer to the OLSR message
28  @return
29  A pointer to the originator address
30  */
31 inline union olsr_ip_addr * getOlsrMessageOriginator(int ipVersion,
32                 union olsr_message * olsrMessage) {
33         if (ipVersion == AF_INET) {
34                 return (union olsr_ip_addr *) &olsrMessage->v4.originator;
35         }
36
37         return (union olsr_ip_addr *) &olsrMessage->v6.originator;
38 }
39
40 /**
41  Determine the size of an OLSR message
42
43  @param ipVersion
44  The IP version
45  @param olsrMessage
46  A pointer to the OLSR message
47  @return
48  The size of the OLSR message
49  */
50 inline unsigned short getOlsrMessageSize(int ipVersion,
51                 union olsr_message * olsrMessage) {
52         if (ipVersion == AF_INET) {
53                 return ntohs(olsrMessage->v4.olsr_msgsize);
54         }
55
56         return ntohs(olsrMessage->v6.olsr_msgsize);
57 }
58
59 /* ************************************************************************
60  * VALIDITY TIME
61  * ************************************************************************ */
62
63 /** Determine the validity time in seconds from the OLSR wire format value */
64 #define PUD_VALIDITY_TIME_FROM_OLSR(msn, lsn) ((((lsn) + 16) * (1 << (msn))) - 16)
65
66 static unsigned long long cachedValidityTimeMsn[16];
67
68 static bool cachedValidityTimeMsnValid = false;
69
70 /**
71  Setup of cache of calculated most significant nibble results of the validity
72  time calculation to speed up run-time calculations. This method has to be
73  called once upon first use of ValidityTime functions.
74  */
75 static void setupCachedValidityTimeMsn(void) {
76         unsigned int msn;
77         for (msn = 0; msn < 16; msn++) {
78                 cachedValidityTimeMsn[msn] = PUD_VALIDITY_TIME_FROM_OLSR(msn, 0);
79         }
80         cachedValidityTimeMsnValid = true;
81 }
82
83 /**
84  Convert the validity time to the validity time for an OLSR message
85
86  @param validityTime
87  The validity time (in seconds)
88
89  @return
90  The validity time converted to the format for the wire format
91  */
92 unsigned char getValidityTimeForOlsr(unsigned long long validityTime) {
93         unsigned int msn = 1;
94         unsigned long long lsn = 0;
95         unsigned long long upperBound;
96
97         if (!cachedValidityTimeMsnValid) {
98                 setupCachedValidityTimeMsn();
99         }
100         upperBound = cachedValidityTimeMsn[msn];
101         while ((msn < 16) && (validityTime >= upperBound)) {
102                 msn++;
103                 upperBound = cachedValidityTimeMsn[msn];
104         }
105         msn--;
106
107         if (unlikely(validityTime >= upperBound)) {
108                 lsn = 15;
109         } else {
110                 unsigned long lowerBound = PUD_VALIDITY_TIME_FROM_OLSR(msn, 0);
111                 unsigned long resolution = (1 << msn);
112                 lsn = ((validityTime - lowerBound + (resolution >> 1)) / resolution);
113         }
114
115         assert(msn <= 15);
116         assert(lsn <= 15);
117
118         return (msn << 4) | lsn;
119 }
120
121 /**
122  Convert the validity time of an OLSR message to the validity time for internal
123  use.
124
125  @param internal
126  The validity time as contained in the wire format
127
128  @return
129  The validity time converted to seconds
130  */
131 unsigned long getValidityTimeFromOlsr(unsigned char internal) {
132         return PUD_VALIDITY_TIME_FROM_OLSR(internal >> 4, internal % 16);
133 }
134
135 /* ************************************************************************
136  * TIME
137  * ************************************************************************ */
138
139 /**
140  Convert the time to the time for an OLSR message (the number of seconds after
141  midnight).
142
143  @param hour
144  The hours
145  @param min
146  The minutes
147  @param sec
148  The seconds
149
150  @return
151  The time converted to the format for the wire format
152  */
153 unsigned long getTimeForOlsr(int hour, int min, int sec) {
154         return ((hour * 60 * 60) + (min * 60) + sec);
155 }
156
157 /**
158  Convert the time of an OLSR message (the number of seconds after midnight) to
159  a time structure, based on midnight of the current day.
160
161  @param olsrTime
162  The time from the wire format
163  @param nowStruct
164  A pointer to the time structure into which to put the converted time
165  */
166 void getTimeFromOlsr(uint32_t olsrTime, struct tm *nowStruct) {
167         unsigned int secNow;
168
169         time_t now = time(NULL);
170         gmtime_r(&now, nowStruct);
171
172         secNow = ((nowStruct->tm_hour * 60 * 60) + (nowStruct->tm_min * 60)
173                         + nowStruct->tm_sec);
174
175         if (secNow <= (12 * 60 * 60)) {
176                 /* we are now in the first 12h of the day */
177                 if (unlikely(olsrTime > (secNow + (12 * 60 * 60)))) {
178                         /* the message was sent more than 12h later in time:
179                          the message was sent yesterday: adjust the date by -1 day */
180                         now -= (24 * 60 * 60);
181                         gmtime_r(&now, nowStruct);
182                 }
183         } else {
184                 /* we are now in the last 12h of the day */
185                 if (unlikely(olsrTime < (secNow - (12 * 60 * 60)))) {
186                         /* the message was sent more than 12h earlier in time:
187                          the message was sent tomorrow: adjust the date by +1 day */
188                         now += (24 * 60 * 60);
189                         gmtime_r(&now, nowStruct);
190                 }
191         }
192
193         nowStruct->tm_mon++;
194         nowStruct->tm_hour = ((olsrTime % (24 * 60 * 60)) / 3600);
195         nowStruct->tm_min = ((olsrTime % (60 * 60)) / 60);
196         nowStruct->tm_sec = (olsrTime % 60);
197 }
198
199 /* ************************************************************************
200  * LATITUDE
201  * ************************************************************************ */
202
203 /**
204  Convert the latitude of a nmeaINFO structure to the latitude for an OLSR
205  message
206
207  @param infoLat
208  The latitude as contained in an NMEA info structure (in degrees)
209
210  @return
211  The latitude converted to the format for the wire format
212  */
213 unsigned long getLatitudeForOlsr(double infoLat) {
214         double lat = infoLat;
215
216         /* lat is in [-90, 90] */
217         assert(lat >= -90.0);
218         assert(lat <= 90.0);
219
220         lat /= 180.0;
221         /* lat is now in [-0.5, 0.5] */
222
223         lat += 0.5;
224         /* lat is now in [0, 1] */
225
226         lat *= (double) (1 << PUD_LATITUDE_BITS);
227         /* lat is now in [0, LATITUDE_BITS] */
228
229         /* clip max */
230         if (unlikely(lat > (double)((1 << PUD_LATITUDE_BITS) - 1))) {
231                 lat = (double) ((1 << PUD_LATITUDE_BITS) - 1);
232         }
233         /* lat is now in [0, 2^LATITUDE_BITS> */
234
235         return lrint(lat);
236 }
237
238 /**
239  Convert the latitude of an OLSR message to the latitude for a nmeaINFO
240  structure
241
242  @param olsrLat
243  The latitude as contained in the wire format
244
245  @return
246  The latitude converted to the format for an NMEA info structure (in degrees)
247  */
248 double getLatitudeFromOlsr(uint32_t olsrLat) {
249         double lat = (double) olsrLat;
250
251         /* lat is in [0, 2^LATITUDE_BITS> */
252
253         /* take half of the rounding error */
254         lat += 0.5;
255
256         lat /= (double) (1 << PUD_LATITUDE_BITS);
257         /* lat is now in [0, 1> */
258
259         lat -= 0.5;
260         /* lat is now in [-0.5, 0.5> */
261
262         lat *= 180.0;
263         /* lat is now in [-90, 90> */
264
265         return lat;
266 }
267
268 /* ************************************************************************
269  * LONGITUDE
270  * ************************************************************************ */
271
272 /**
273  Convert the longitude of a nmeaINFO structure to the longitude for an OLSR
274  message
275
276  @param infoLon
277  The longitude as contained in an NMEA info structure (in degrees)
278
279  @return
280  The longitude converted to the format for the wire format
281  */
282 unsigned long getLongitudeForOlsr(double infoLon) {
283         double lon = infoLon;
284
285         /* lon is in [-180, 180] */
286         assert(lon >= -180.0);
287         assert(lon <= 180.0);
288
289         lon /= 360.0;
290         /* lon is now in [-0.5, 0.5] */
291
292         lon += 0.5;
293         /* lon is now in [0, 1] */
294
295         lon *= (double) (1 << PUD_LONGITUDE_BITS);
296         /* lon is now in [0, LONGITUDE_BITS] */
297
298         /* clip max */
299         if (unlikely(lon > (double)((1 << PUD_LATITUDE_BITS) - 1))) {
300                 lon = (double) ((1 << PUD_LATITUDE_BITS) - 1);
301         }
302
303         /* lon is now in [0, 2^LONGITUDE_BITS> */
304
305         return lrint(lon);
306 }
307
308 /**
309  Convert the longitude of an OLSR message to the longitude for a nmeaINFO
310  structure
311
312  @param olsrLon
313  The longitude as contained in the wire format
314
315  @return
316  The longitude converted to the format for an NMEA info structure (in degrees)
317  */
318 double getLongitudeFromOlsr(uint32_t olsrLon) {
319         double lon = (double) olsrLon;
320
321         /* lon is in [0, 2^LONGITUDE_BITS> */
322
323         /* take half of the rounding error */
324         lon += 0.5;
325
326         lon /= (1 << PUD_LONGITUDE_BITS);
327         /* lon is now in [0, 1> */
328
329         lon -= 0.5;
330         /* lon is now in [-0.5, 0.5> */
331
332         lon *= 360.0;
333         /* lon is now in [-180, 180> */
334
335         return lon;
336 }
337
338 /* ************************************************************************
339  * ALTITIDE
340  * ************************************************************************ */
341
342 /**
343  Convert the altitude of a nmeaINFO structure to the altitude for an OLSR
344  message
345
346  @param infoElv
347  The altitude as contained in an NMEA info structure
348
349  @return
350  The altitude converted to the format for the wire format
351  */
352 long getAltitudeForOlsr(double infoElv) {
353         double elv = infoElv;
354
355         if (unlikely(elv > PUD_ALTITUDE_MAX)) {
356                 elv = PUD_ALTITUDE_MAX;
357         } else if (unlikely(elv < PUD_ALTITUDE_MIN)) {
358                 elv = PUD_ALTITUDE_MIN;
359         }
360
361         elv -= PUD_ALTITUDE_MIN;
362
363         return lrint(elv);
364 }
365
366 /**
367  Convert the altitude of an OLSR message to the altitude for a nmeaINFO
368  structure
369
370  @param olsrAlt
371  The altitude as contained in the wire format
372
373  @return
374  The altitude converted to the format for an NMEA info structure
375  */
376 long getAltitudeFromOlsr(uint32_t olsrAlt) {
377         return (olsrAlt + PUD_ALTITUDE_MIN);
378 }
379
380 /* ************************************************************************
381  * SPEED
382  * ************************************************************************ */
383
384 /**
385  Convert the speed of a nmeaINFO structure to the speed for an OLSR message
386
387  @param infoSpeed
388  The speed as contained in an NMEA info structure
389
390  @return
391  The speed converted to the format for the wire format
392  */
393 long getSpeedForOlsr(double infoSpeed) {
394         if (unlikely(infoSpeed < 0)) {
395                 return 0;
396         }
397         if (unlikely(infoSpeed > PUD_SPEED_MAX)) {
398                 return PUD_SPEED_MAX;
399         }
400
401         return lrint(infoSpeed);
402 }
403
404 /**
405  Convert the speed of an OLSR message to the speed for a nmeaINFO structure
406
407  @param olsrSpeed
408  The speed as contained in the wire format
409
410  @return
411  The speed converted to the format for an NMEA info structure
412  */
413 unsigned long getSpeedFromOlsr(uint32_t olsrSpeed) {
414         return olsrSpeed;
415 }
416
417 /* ************************************************************************
418  * TRACK
419  * ************************************************************************ */
420
421 /**
422  Convert the track angle of a nmeaINFO structure to the track angle for an OLSR
423  message
424
425  @param infoTrack
426  The track angle as contained in an NMEA info structure
427
428  @return
429  The track angle converted to the format for the wire format
430  */
431 long getTrackForOlsr(double infoTrack) {
432         return lrint(infoTrack);
433 }
434
435 /**
436  Convert the track angle of an OLSR message to the track angle for a nmeaINFO
437  structure
438
439  @param olsrTrack
440  The track angle as contained in the wire format
441
442  @return
443  The track angle converted to the format for an NMEA info structure
444  */
445 unsigned long getTrackFromOlsr(uint32_t olsrTrack) {
446         return olsrTrack;
447 }
448
449 /* ************************************************************************
450  * HDOP
451  * ************************************************************************ */
452
453 /**
454  Convert the HDOP of a nmeaINFO structure to the HDOP for an OLSR message
455
456  @param infoHdop
457  The HDOP as contained in an NMEA info structure
458
459  @return
460  The HDOP converted to the format for the wire format
461  */
462 long getHdopForOlsr(double infoHdop) {
463         double infoHdopInternal = infoHdop;
464
465         if (unlikely(infoHdopInternal > PUD_HDOP_MAX)) {
466                 infoHdopInternal = PUD_HDOP_MAX;
467         }
468
469         return lrint(infoHdopInternal / PUD_HDOP_RESOLUTION);
470 }
471
472 /**
473  Convert the HDOP of an OLSR message to the HDOP for a nmeaINFO structure
474
475  @param olsrHdop
476  The HDOP as contained in the wire format
477
478  @return
479  The HDOP converted to the format for an NMEA info structure
480  */
481 double getHdopFromOlsr(uint32_t olsrHdop) {
482         return (olsrHdop * PUD_HDOP_RESOLUTION);
483 }
484
485 /* ************************************************************************
486  * NodeInfo
487  * ************************************************************************ */
488
489 /**
490  Get the nodeIdType, accounting for nodeId presence
491
492  @param ipVersion
493  The ip version, either AF_INET or AF_INET6
494  @param olsrMessage
495  A pointer to the OLSR message
496
497  @return
498  The nodeIdType
499  */
500 NodeIdType getNodeIdType(int ipVersion, union olsr_message * olsrMessage) {
501         NodeIdType nodeIdType;
502         PudOlsrWireFormat *olsrGpsMessage;
503
504         /* determine the originator of the message */
505         if (ipVersion == AF_INET) {
506                 olsrGpsMessage = (PudOlsrWireFormat *) &olsrMessage->v4.message;
507         } else {
508                 olsrGpsMessage = (PudOlsrWireFormat *) &olsrMessage->v6.message;
509         }
510
511         if (olsrGpsMessage->smask & PUD_FLAGS_ID) {
512                 nodeIdType = olsrGpsMessage->nodeInfo.nodeIdType;
513         } else {
514                 nodeIdType = ((ipVersion == AF_INET) ? PUD_NODEIDTYPE_IPV4 : PUD_NODEIDTYPE_IPV6);
515         }
516
517         return nodeIdType;
518 }