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