pud: use the correct field for padding
[olsrd.git] / lib / pud / src / gpsConversion.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 "gpsConversion.h"
47
48 /* Plugin includes */
49 #include "pud.h"
50 #include "configuration.h"
51 #include "compiler.h"
52
53 /* OLSR includes */
54 #include "olsr.h"
55
56 /* System includes */
57 #include <nmealib/info.h>
58 #include <nmealib/nmath.h>
59 #include <nmealib/util.h>
60 #include <arpa/inet.h>
61 #include <OlsrdPudWireFormat/nodeIdConversion.h>
62
63 /* ************************************************************************
64  * OLSR --> External
65  * ************************************************************************ */
66
67 /**
68  Convert an OLSR message into a string to multicast on the LAN
69
70  @param olsrMessage
71  A pointer to the OLSR message
72  @param txGpsBuffer
73  A pointer to the buffer in which the transmit string can be written
74  @param txGpsBufferSize
75  The size of the txGpsBuffer
76
77  @return
78  - the length of the transmit string placed in the txGpsBuffer
79  - 0 (zero) in case of an error
80  */
81 unsigned int gpsFromOlsr(union olsr_message *olsrMessage,
82                 unsigned char * txGpsBuffer, unsigned int txGpsBufferSize) {
83         unsigned long validityTime;
84
85         struct tm timeStruct;
86         char latitudeString[PUD_TX_LATITUDE_DIGITS];
87         const char * latitudeHemisphere;
88         char longitudeString[PUD_TX_LONGITUDE_DIGITS];
89         const char * longitudeHemisphere;
90         char altitudeString[PUD_TX_ALTITUDE_DIGITS];
91         char speedString[PUD_TX_SPEED_DIGITS];
92         char trackString[PUD_TX_TRACK_DIGITS];
93         char hdopString[PUD_TX_HDOP_DIGITS];
94         uint32_t present;
95         char gateway[2] = { '0', '\0' };
96
97         char nodeIdTypeString[PUD_TX_NODEIDTYPE_DIGITS];
98         char nodeIdString[PUD_TX_NODEID_BUFFERSIZE];
99         const char * nodeId;
100         const void * ipAddr;
101         char originatorBuffer[64];
102         const char * originator;
103
104         unsigned int transmitStringLength;
105
106         PudOlsrPositionUpdate * olsrGpsMessage =
107                         getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);
108
109         if (unlikely(getPositionUpdateVersion(olsrGpsMessage) != PUD_WIRE_FORMAT_VERSION)) {
110                 /* currently we can only handle our own version */
111                 pudError(false, "Can not handle version %u OLSR PUD messages"
112                         " (only version %u): message ignored",
113                         getPositionUpdateVersion(olsrGpsMessage), PUD_WIRE_FORMAT_VERSION);
114                 return 0;
115         }
116
117         ipAddr = (olsr_cnf->ip_version == AF_INET) ?
118                                 (void *) &olsrMessage->v4.originator :
119                                 (void *) &olsrMessage->v6.originator;
120         originator = inet_ntop(olsr_cnf->ip_version, ipAddr, &originatorBuffer[0],
121                         sizeof(originatorBuffer));
122
123         validityTime = getValidityTime(&olsrGpsMessage->validityTime);
124
125         present = getPositionUpdatePresent(olsrGpsMessage);
126
127         if (present & PUD_PRESENT_GATEWAY) {
128                 gateway[0] = '1';
129         }
130
131         /* time is ALWAYS present so we can just use it */
132         getPositionUpdateTime(olsrGpsMessage, time(NULL), &timeStruct);
133
134         if (likely(nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_LAT))) {
135                 double latitude = getPositionUpdateLatitude(olsrGpsMessage);
136
137                 if (latitude >= 0) {
138                         latitudeHemisphere = "N";
139                 } else {
140                         latitudeHemisphere = "S";
141                         latitude = -latitude;
142                 }
143                 latitude = nmeaMathDegreeToNdeg(latitude);
144
145                 snprintf(&latitudeString[0], PUD_TX_LATITUDE_DIGITS, "%." PUD_TX_LATITUDE_DECIMALS "f", latitude);
146         } else {
147                 latitudeHemisphere = "";
148                 latitudeString[0] = '\0';
149         }
150
151         if (likely(nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_LON))) {
152                 double longitude = getPositionUpdateLongitude(olsrGpsMessage);
153
154                 if (longitude >= 0) {
155                         longitudeHemisphere = "E";
156                 } else {
157                         longitudeHemisphere = "W";
158                         longitude = -longitude;
159                 }
160                 longitude = nmeaMathDegreeToNdeg(longitude);
161
162                 snprintf(&longitudeString[0], PUD_TX_LONGITUDE_DIGITS, "%." PUD_TX_LONGITUDE_DECIMALS "f", longitude);
163         } else {
164                 longitudeHemisphere = "";
165                 longitudeString[0] = '\0';
166         }
167
168         if (likely(nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_ELV))) {
169                 snprintf(&altitudeString[0], PUD_TX_ALTITUDE_DIGITS, "%ld", getPositionUpdateAltitude(olsrGpsMessage));
170         } else {
171                 altitudeString[0] = '\0';
172         }
173
174         if (likely(nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_SPEED))) {
175                 snprintf(&speedString[0], PUD_TX_SPEED_DIGITS, "%lu", getPositionUpdateSpeed(olsrGpsMessage));
176         } else {
177                 speedString[0] = '\0';
178         }
179
180         if (likely(nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_TRACK))) {
181                 snprintf(&trackString[0], PUD_TX_TRACK_DIGITS, "%lu", getPositionUpdateTrack(olsrGpsMessage));
182         } else {
183                 trackString[0] = '\0';
184         }
185
186         if (likely(nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_HDOP))) {
187                 snprintf(&hdopString[0], PUD_TX_HDOP_DIGITS, "%." PUD_TX_HDOP_DECIMALS "f",
188                     nmeaMathMetersToDop(getPositionUpdateHdop(olsrGpsMessage)));
189         } else {
190                 hdopString[0] = '\0';
191         }
192
193         getNodeTypeStringFromOlsr(olsr_cnf->ip_version, olsrGpsMessage,
194                         &nodeIdTypeString[0], sizeof(nodeIdTypeString));
195         getNodeIdStringFromOlsr(olsr_cnf->ip_version, olsrMessage, &nodeId,
196                         &nodeIdString[0], sizeof(nodeIdString));
197
198         transmitStringLength = nmeaPrintf((char *) txGpsBuffer, txGpsBufferSize
199                         - 1, "$P%s," /* prefix (always) */
200                 "%u," /* sentence version (always) */
201                 "%s," /* gateway flag (always) */
202                 "%s," /* OLSR originator (always) */
203                 "%s,%s," /* nodeIdType/nodeId (always) */
204                 "%02u%02u%02u," /* date (always) */
205                 "%02u%02u%02u," /* time (always) */
206                 "%lu," /* validity time (always) */
207                 "%s,%s," /* latitude (optional) */
208                 "%s,%s," /* longitude (optional) */
209                 "%s," /* altitude (optional) */
210                 "%s," /* speed (optional) */
211                 "%s," /* track (optional) */
212                 "%s" /* hdop (optional) */
213         , getTxNmeaMessagePrefix(), PUD_TX_SENTENCE_VERSION, &gateway[0],
214                         originator , &nodeIdTypeString[0],
215                         nodeId, timeStruct.tm_mday, timeStruct.tm_mon + 1, (timeStruct.tm_year
216                                         % 100), timeStruct.tm_hour, timeStruct.tm_min,
217                         timeStruct.tm_sec, validityTime, &latitudeString[0],
218                         latitudeHemisphere, &longitudeString[0], longitudeHemisphere,
219                         &altitudeString[0], &speedString[0], &trackString[0],
220                         &hdopString[0]);
221
222         if (unlikely(transmitStringLength > (txGpsBufferSize - 1))) {
223                 pudError(false, "String to transmit on non-OLSR is too large, need"
224                         " at least %u bytes, skipped", transmitStringLength);
225                 return 0;
226         }
227
228         if (unlikely(transmitStringLength == (txGpsBufferSize - 1))) {
229                 txGpsBuffer[txGpsBufferSize - 1] = '\0';
230         } else {
231                 txGpsBuffer[transmitStringLength] = '\0';
232         }
233
234         return transmitStringLength;
235 }
236
237 /* ************************************************************************
238  * External --> OLSR
239  * ************************************************************************ */
240
241 /**
242  Convert a nmeaINFO structure into an OLSR message.
243
244  @param nmeaInfo
245  A pointer to a nmeaINFO structure
246  @param olsrMessage
247  A pointer to an OLSR message in which to place the converted information
248  @param olsrMessageSize
249  The maximum number of bytes available for the olsrMessage
250  @param validityTime
251  the validity time of the message in seconds
252
253  @return
254  - the aligned size of the converted information
255  - 0 (zero) in case of an error
256  */
257 unsigned int gpsToOlsr(NmeaInfo *nmeaInfo, union olsr_message *olsrMessage,
258                 unsigned int olsrMessageSize, unsigned long long validityTime) {
259         unsigned int aligned_size;
260         unsigned int aligned_size_remainder;
261         size_t nodeLength;
262         nodeIdBinaryType * nodeIdBinary = NULL;
263
264         PudOlsrPositionUpdate * olsrGpsMessage =
265                         getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);
266
267         /*
268          * Compose message contents
269          */
270         memset(olsrGpsMessage, 0, sizeof (PudOlsrPositionUpdate));
271
272         setPositionUpdateVersion(olsrGpsMessage, PUD_WIRE_FORMAT_VERSION);
273         setValidityTime(&olsrGpsMessage->validityTime, validityTime);
274         setPositionUpdatePresent(olsrGpsMessage, nmeaInfo->present & ~PUD_PRESENT_GATEWAY);
275
276         /* utc is always present, we make sure of that elsewhere, so just use it */
277         setPositionUpdateTime(olsrGpsMessage, nmeaInfo->utc.hour, nmeaInfo->utc.min,
278                         nmeaInfo->utc.sec);
279
280         if (likely(nmeaInfoIsPresentAll(nmeaInfo->present, NMEALIB_PRESENT_LAT))) {
281                 setPositionUpdateLatitude(olsrGpsMessage, nmeaInfo->latitude);
282         } else {
283                 setPositionUpdateLatitude(olsrGpsMessage, 0.0);
284         }
285
286         if (likely(nmeaInfoIsPresentAll(nmeaInfo->present, NMEALIB_PRESENT_LON))) {
287                 setPositionUpdateLongitude(olsrGpsMessage, nmeaInfo->longitude);
288         } else {
289                 setPositionUpdateLongitude(olsrGpsMessage, 0.0);
290         }
291
292         if (likely(nmeaInfoIsPresentAll(nmeaInfo->present, NMEALIB_PRESENT_ELV))) {
293                 setPositionUpdateAltitude(olsrGpsMessage, nmeaInfo->elevation);
294         } else {
295                 setPositionUpdateAltitude(olsrGpsMessage, 0.0);
296         }
297
298         if (likely(nmeaInfoIsPresentAll(nmeaInfo->present, NMEALIB_PRESENT_SPEED))) {
299                 setPositionUpdateSpeed(olsrGpsMessage, nmeaInfo->speed);
300         } else {
301                 setPositionUpdateSpeed(olsrGpsMessage, 0.0);
302         }
303
304         if (likely(nmeaInfoIsPresentAll(nmeaInfo->present, NMEALIB_PRESENT_TRACK))) {
305                 setPositionUpdateTrack(olsrGpsMessage, nmeaInfo->track);
306         } else {
307                 setPositionUpdateTrack(olsrGpsMessage, 0);
308         }
309
310         if (likely(nmeaInfoIsPresentAll(nmeaInfo->present, NMEALIB_PRESENT_HDOP))) {
311                 setPositionUpdateHdop(olsrGpsMessage, nmeaInfo->hdop);
312         } else {
313                 setPositionUpdateHdop(olsrGpsMessage, PUD_HDOP_MAX);
314         }
315
316         nodeIdBinary = getNodeIdBinary();
317         nodeLength = setPositionUpdateNodeInfo(olsr_cnf->ip_version, olsrGpsMessage,
318                         olsrMessageSize, getNodeIdTypeNumber(),
319                         (unsigned char *) &nodeIdBinary->buffer, nodeIdBinary->length);
320
321         /*
322          * Messages in OLSR are 4-byte aligned: align
323          */
324
325         /* size = type, string, string terminator */
326         aligned_size = PUD_OLSRWIREFORMATSIZE + nodeLength;
327         aligned_size_remainder = (aligned_size % 4);
328         if (aligned_size_remainder != 0) {
329                 aligned_size += (4 - aligned_size_remainder);
330         }
331
332         /*
333          * Fill message headers (fill ALL fields, except message)
334          * Note: olsr_vtime is currently unused, we use it for our validity time.
335          */
336
337         if (olsr_cnf->ip_version == AF_INET) {
338                 /* IPv4 */
339
340                 olsrMessage->v4.olsr_msgtype = PUD_OLSR_MSG_TYPE;
341                 olsrMessage->v4.olsr_vtime = reltime_to_me(validityTime * 1000);
342                 /* message->v4.olsr_msgsize at the end */
343                 olsrMessage->v4.originator = olsr_cnf->main_addr.v4.s_addr;
344                 olsrMessage->v4.ttl = getOlsrTtl();
345                 olsrMessage->v4.hopcnt = 0;
346                 olsrMessage->v4.seqno = htons(get_msg_seqno());
347
348                 /* add length of message->v4 fields */
349                 aligned_size += (sizeof(olsrMessage->v4)
350                                 - sizeof(olsrMessage->v4.message));
351                 olsrMessage->v4.olsr_msgsize = htons(aligned_size);
352         } else {
353                 /* IPv6 */
354
355                 olsrMessage->v6.olsr_msgtype = PUD_OLSR_MSG_TYPE;
356                 olsrMessage->v6.olsr_vtime = reltime_to_me(validityTime * 1000);
357                 /* message->v6.olsr_msgsize at the end */
358                 olsrMessage->v6.originator = olsr_cnf->main_addr.v6;
359                 olsrMessage->v6.ttl = getOlsrTtl();
360                 olsrMessage->v6.hopcnt = 0;
361                 olsrMessage->v6.seqno = htons(get_msg_seqno());
362
363                 /* add length of message->v6 fields */
364                 aligned_size += (sizeof(olsrMessage->v6)
365                                 - sizeof(olsrMessage->v6.message));
366                 olsrMessage->v6.olsr_msgsize = htons(aligned_size);
367         }
368
369         /* pad with zeroes */
370         if (aligned_size_remainder != 0) {
371                 memset(&(((char *) &olsrGpsMessage->nodeInfo.nodeId)[nodeLength]),
372                                 0, (4 - aligned_size_remainder));
373         }
374
375         return aligned_size;
376 }