Merge branch 'stable' into pud
[olsrd.git] / lib / pud / src / gpsConversion.c
1 #include "gpsConversion.h"
2
3 /* Plugin includes */
4 #include "wireFormat.h"
5 #include "pud.h"
6 #include "nodeIdConversion.h"
7 #include "configuration.h"
8 #include "compiler.h"
9
10 /* OLSR includes */
11 #include "olsr.h"
12
13 /* System includes */
14 #include <nmea/gmath.h>
15 #include <nmea/tok.h>
16 #include <nmea/info.h>
17 #include <netinet/in.h>
18 #include <stdio.h>
19
20 /* ************************************************************************
21  * OLSR --> External
22  * ************************************************************************ */
23
24 /**
25  Convert an OLSR message into a string to multicast on the LAN
26
27  @param olsrMessage
28  A pointer to the OLSR message
29  @param txGpsBuffer
30  A pointer to the buffer in which the transmit string can be written
31  @param txGpsBufferSize
32  The size of the txGpsBuffer
33
34  @return
35  - the length of the transmit string placed in the txGpsBuffer
36  - 0 (zero) in case of an error
37  */
38 unsigned int gpsFromOlsr(union olsr_message *olsrMessage,
39                 unsigned char * txGpsBuffer, unsigned int txGpsBufferSize) {
40         unsigned long validityTime;
41
42         struct tm timeStruct;
43         char latitudeString[PUD_TX_LATITUDE_DIGITS + 1];
44         const char * latitudeHemisphere;
45         char longitudeString[PUD_TX_LONGITUDE_DIGITS + 1];
46         const char * longitudeHemisphere;
47         char altitudeString[PUD_TX_ALTITUDE_DIGITS + 1];
48         char speedString[PUD_TX_SPEED_DIGITS + 1];
49         char trackString[PUD_TX_TRACK_DIGITS + 1];
50         char hdopString[PUD_TX_HDOP_DIGITS + 1];
51
52         char nodeIdTypeString[PUD_TX_NODEIDTYPE_DIGITS + 1];
53         char nodeIdString[PUD_TX_NODEID_BUFFERSIZE + 1];
54         const char * nodeId;
55
56         unsigned int transmitStringLength;
57
58         GpsInfo* gpsMessage;
59         PudOlsrWireFormat * olsrGpsMessage =
60                         getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);
61
62         if (unlikely(olsrGpsMessage->version != PUD_WIRE_FORMAT_VERSION)) {
63                 /* currently we can only handle our own version */
64                 pudError(false, "Can not handle version %u OLSR PUD messages"
65                         " (only version %u): message ignored", olsrGpsMessage->version,
66                                 PUD_WIRE_FORMAT_VERSION);
67                 return 0;
68         }
69
70         validityTime = getValidityTimeFromOlsr(olsrGpsMessage->validityTime);
71
72         gpsMessage = &olsrGpsMessage->gpsInfo;
73
74         /* time is ALWAYS present so we can just use it */
75         getTimeFromOlsr(gpsMessage->time, &timeStruct);
76
77         if (likely(nmea_INFO_has_field(olsrGpsMessage->smask, LAT))) {
78                 int chars;
79                 double latitude = getLatitudeFromOlsr(gpsMessage->lat);
80
81                 if (latitude >= 0) {
82                         latitudeHemisphere = "N";
83                 } else {
84                         latitudeHemisphere = "S";
85                         latitude = -latitude;
86                 }
87                 latitude = nmea_degree2ndeg(latitude);
88
89                 chars = snprintf(&latitudeString[0], PUD_TX_LATITUDE_DIGITS,
90                                 "%." PUD_TX_LATITUDE_DECIMALS "f", latitude);
91                 if (likely(chars < PUD_TX_LATITUDE_DIGITS)) {
92                         latitudeString[chars] = '\0';
93                 } else {
94                         latitudeString[PUD_TX_LATITUDE_DIGITS] = '\0';
95                 }
96         } else {
97                 latitudeHemisphere = "";
98                 latitudeString[0] = '\0';
99         }
100
101         if (likely(nmea_INFO_has_field(olsrGpsMessage->smask, LON))) {
102                 int chars;
103                 double longitude = getLongitudeFromOlsr(gpsMessage->lon);
104
105                 if (longitude >= 0) {
106                         longitudeHemisphere = "E";
107                 } else {
108                         longitudeHemisphere = "W";
109                         longitude = -longitude;
110                 }
111                 longitude = nmea_degree2ndeg(longitude);
112
113                 chars = snprintf(&longitudeString[0], PUD_TX_LONGITUDE_DIGITS,
114                                 "%." PUD_TX_LONGITUDE_DECIMALS "f", longitude);
115                 if (likely(chars < PUD_TX_LONGITUDE_DIGITS)) {
116                         longitudeString[chars] = '\0';
117                 } else {
118                         longitudeString[PUD_TX_LONGITUDE_DIGITS] = '\0';
119                 }
120         } else {
121                 longitudeHemisphere = "";
122                 longitudeString[0] = '\0';
123         }
124
125         if (likely(nmea_INFO_has_field(olsrGpsMessage->smask, ELV))) {
126                 int chars = snprintf(&altitudeString[0], PUD_TX_ALTITUDE_DIGITS, "%ld",
127                                 getAltitudeFromOlsr(gpsMessage->alt));
128                 if (likely(chars < PUD_TX_ALTITUDE_DIGITS)) {
129                         altitudeString[chars] = '\0';
130                 } else {
131                         altitudeString[PUD_TX_ALTITUDE_DIGITS] = '\0';
132                 }
133         } else {
134                 altitudeString[0] = '\0';
135         }
136
137         if (likely(nmea_INFO_has_field(olsrGpsMessage->smask, SPEED))) {
138                 int chars = snprintf(&speedString[0], PUD_TX_SPEED_DIGITS, "%lu",
139                                 getSpeedFromOlsr(gpsMessage->speed));
140                 if (likely(chars < PUD_TX_SPEED_DIGITS)) {
141                         speedString[chars] = '\0';
142                 } else {
143                         speedString[PUD_TX_SPEED_DIGITS] = '\0';
144                 }
145         } else {
146                 speedString[0] = '\0';
147         }
148
149         if (likely(nmea_INFO_has_field(olsrGpsMessage->smask, DIRECTION))) {
150                 int chars = snprintf(&trackString[0], PUD_TX_TRACK_DIGITS, "%lu",
151                                 getTrackFromOlsr(gpsMessage->track));
152                 if (likely(chars < PUD_TX_TRACK_DIGITS)) {
153                         trackString[chars] = '\0';
154                 } else {
155                         trackString[PUD_TX_TRACK_DIGITS] = '\0';
156                 }
157         } else {
158                 trackString[0] = '\0';
159         }
160
161         if (likely(nmea_INFO_has_field(olsrGpsMessage->smask, HDOP))) {
162                 int chars = snprintf(&hdopString[0], PUD_TX_HDOP_DIGITS,
163                                 "%." PUD_TX_HDOP_DECIMALS "f", nmea_meters2dop(getHdopFromOlsr(
164                                                 gpsMessage->hdop)));
165                 if (likely(chars < PUD_TX_HDOP_DIGITS)) {
166                         hdopString[chars] = '\0';
167                 } else {
168                         hdopString[PUD_TX_HDOP_DIGITS] = '\0';
169                 }
170         } else {
171                 hdopString[0] = '\0';
172         }
173
174         getNodeTypeStringFromOlsr(olsr_cnf->ip_version, olsrMessage,
175                         &nodeIdTypeString[0], sizeof(nodeIdTypeString));
176         getNodeIdStringFromOlsr(olsr_cnf->ip_version, olsrMessage, &nodeId,
177                         &nodeIdString[0], sizeof(nodeIdString));
178
179         transmitStringLength = nmea_printf((char *) txGpsBuffer, txGpsBufferSize
180                         - 1, "$P%s," /* prefix (always) */
181                 "%u," /* sentence version (always) */
182                 "%s,%s," /* nodeIdType/nodeId (always) */
183                 "%02u%02u%02u," /* date (always) */
184                 "%02u%02u%02u," /* time (always) */
185                 "%lu," /* validity time (always) */
186                 "%s,%s," /* latitude (optional) */
187                 "%s,%s," /* longitude (optional) */
188                 "%s," /* altitude (optional) */
189                 "%s," /* speed (optional) */
190                 "%s," /* track (optional) */
191                 "%s" /* hdop (optional) */
192         , getTxNmeaMessagePrefix(), PUD_TX_SENTENCE_VERSION, &nodeIdTypeString[0],
193                         nodeId, timeStruct.tm_mday, timeStruct.tm_mon, (timeStruct.tm_year
194                                         % 100), timeStruct.tm_hour, timeStruct.tm_min,
195                         timeStruct.tm_sec, validityTime, &latitudeString[0],
196                         latitudeHemisphere, &longitudeString[0], longitudeHemisphere,
197                         &altitudeString[0], &speedString[0], &trackString[0],
198                         &hdopString[0]);
199
200         if (unlikely(transmitStringLength > (txGpsBufferSize - 1))) {
201                 pudError(false, "String to transmit on non-OLSR is too large, need"
202                         " at least %u bytes, skipped", transmitStringLength);
203                 return 0;
204         }
205
206         if (unlikely(transmitStringLength == (txGpsBufferSize - 1))) {
207                 txGpsBuffer[txGpsBufferSize - 1] = '\0';
208         } else {
209                 txGpsBuffer[transmitStringLength] = '\0';
210         }
211
212         return transmitStringLength;
213 }
214
215 /* ************************************************************************
216  * External --> OLSR
217  * ************************************************************************ */
218
219 /**
220  Convert the node information to the node information for an OLSR message and
221  put it in the PUD message in the OLSR message. Also updates the PUD message
222  smask.
223
224  @param olsrGpsMessage
225  A pointer to the PUD message in the OLSR message
226  @param olsrMessageSize
227  The maximum number of bytes available for the olsrMessage
228  @param nodeIdType
229  The nodeIdType
230
231  @return
232  The number of bytes written in the PUD message in the OLSR message (for ALL
233  the node information)
234  */
235 static size_t setupNodeInfoForOlsr(PudOlsrWireFormat * olsrGpsMessage,
236                 unsigned int olsrMessageSize, NodeIdType nodeIdType) {
237         unsigned char * buffer;
238         unsigned int length = 0;
239
240         olsrGpsMessage->nodeInfo.nodeIdType = nodeIdType;
241         switch (nodeIdType) {
242                 case PUD_NODEIDTYPE_MAC: /* hardware address */
243                         /* handled when the message is actually sent into OLSR, in the
244                          * pre-transmit hook */
245                         length = PUD_NODEIDTYPE_MAC_BYTES;
246                         break;
247
248                 case PUD_NODEIDTYPE_MSISDN: /* an MSISDN number */
249                 case PUD_NODEIDTYPE_TETRA: /* a Tetra number */
250                 case PUD_NODEIDTYPE_192:
251                 case PUD_NODEIDTYPE_193:
252                 case PUD_NODEIDTYPE_194:
253                         getNodeIdNumberForOlsrCache(&buffer, &length);
254                         memcpy(&olsrGpsMessage->nodeInfo.nodeId, buffer, length);
255                         break;
256
257                 case PUD_NODEIDTYPE_DNS: /* DNS name */
258                 {
259                         size_t nodeIdLength;
260                         unsigned char * nodeId = getNodeIdWithLength(&nodeIdLength);
261                         long charsAvailable = olsrMessageSize - (PUD_OLSRWIREFORMATSIZE
262                                         + sizeof(NodeInfo)
263                                         - sizeof(olsrGpsMessage->nodeInfo.nodeId)) - 1;
264
265                         length = nodeIdLength + 1;
266                         if (unlikely((long) length > charsAvailable)) {
267                                 length = charsAvailable;
268                         }
269
270                         memcpy(&olsrGpsMessage->nodeInfo.nodeId, nodeId, length);
271                         (&olsrGpsMessage->nodeInfo.nodeId)[length] = '\0';
272                 }
273                         break;
274
275                 case PUD_NODEIDTYPE_IPV4: /* IPv4 address */
276                 case PUD_NODEIDTYPE_IPV6: /* IPv6 address */
277                         /* explicit return: no nodeId information in message */
278                         return 0;
279
280                 default: /* unsupported */
281                         /* fallback to IP address */
282                         olsrGpsMessage->nodeInfo.nodeIdType = (olsr_cnf->ip_version
283                                         == AF_INET) ? PUD_NODEIDTYPE_IPV4 : PUD_NODEIDTYPE_IPV6;
284
285                         /* explicit return: no nodeId information in message */
286                         return 0;
287         }
288
289         olsrGpsMessage->smask |= PUD_FLAGS_ID;
290         return ((sizeof(NodeInfo)
291                         - (sizeof(olsrGpsMessage->nodeInfo.nodeId) /* nodeId placeholder */))
292                         + length);
293 }
294
295 /**
296  Convert a nmeaINFO structure into an OLSR message.
297
298  @param nmeaInfo
299  A pointer to a nmeaINFO structure
300  @param olsrMessage
301  A pointer to an OLSR message in which to place the converted information
302  @param olsrMessageSize
303  The maximum number of bytes available for the olsrMessage
304  @param validityTime
305  the validity time of the message
306
307  @return
308  - the aligned size of the converted information
309  - 0 (zero) in case of an error
310  */
311 unsigned int gpsToOlsr(nmeaINFO *nmeaInfo, union olsr_message *olsrMessage,
312                 unsigned int olsrMessageSize, unsigned long long validityTime) {
313         unsigned int aligned_size;
314         unsigned int aligned_size_remainder;
315         size_t nodeLength;
316         PudOlsrWireFormat * olsrGpsMessage =
317                         getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);
318
319         /*
320          * Compose message contents
321          */
322
323         olsrGpsMessage->version = PUD_WIRE_FORMAT_VERSION;
324         olsrGpsMessage->validityTime = getValidityTimeForOlsr(validityTime);
325         olsrGpsMessage->smask = nmeaInfo->smask;
326
327         /* utc is always present, we make sure of that, so just use it */
328         olsrGpsMessage->gpsInfo.time = getTimeForOlsr(nmeaInfo->utc.hour,
329                         nmeaInfo->utc.min, nmeaInfo->utc.sec);
330
331         if (likely(nmea_INFO_has_field(nmeaInfo->smask, LAT))) {
332                 olsrGpsMessage->gpsInfo.lat = getLatitudeForOlsr(nmeaInfo->lat);
333         } else {
334                 olsrGpsMessage->gpsInfo.lat = (1 << (PUD_LATITUDE_BITS - 1));
335         }
336
337         if (likely(nmea_INFO_has_field(nmeaInfo->smask, LON))) {
338                 olsrGpsMessage->gpsInfo.lon = getLongitudeForOlsr(nmeaInfo->lon);
339         } else {
340                 olsrGpsMessage->gpsInfo.lon = (1 << (PUD_LONGITUDE_BITS - 1));
341         }
342
343         if (likely(nmea_INFO_has_field(nmeaInfo->smask, ELV))) {
344                 olsrGpsMessage->gpsInfo.alt = getAltitudeForOlsr(nmeaInfo->elv);
345         } else {
346                 olsrGpsMessage->gpsInfo.alt = -PUD_ALTITUDE_MIN;
347         }
348
349         if (likely(nmea_INFO_has_field(nmeaInfo->smask, SPEED))) {
350                 olsrGpsMessage->gpsInfo.speed = getSpeedForOlsr(nmeaInfo->speed);
351         } else {
352                 olsrGpsMessage->gpsInfo.speed = 0;
353         }
354
355         if (likely(nmea_INFO_has_field(nmeaInfo->smask, DIRECTION))) {
356                 olsrGpsMessage->gpsInfo.track = getTrackForOlsr(nmeaInfo->direction);
357         } else {
358                 olsrGpsMessage->gpsInfo.track = 0;
359         }
360
361         if (likely(nmea_INFO_has_field(nmeaInfo->smask, HDOP))) {
362                 olsrGpsMessage->gpsInfo.hdop = getHdopForOlsr(nmeaInfo->HDOP);
363         } else {
364                 olsrGpsMessage->gpsInfo.hdop = PUD_HDOP_MAX;
365         }
366
367         nodeLength = setupNodeInfoForOlsr(olsrGpsMessage, olsrMessageSize,
368                         getNodeIdTypeNumber());
369
370         /*
371          * Messages in OLSR are 4-byte aligned: align
372          */
373
374         /* size = type, string, string terminator */
375         aligned_size = PUD_OLSRWIREFORMATSIZE + nodeLength;
376         aligned_size_remainder = (aligned_size % 4);
377         if (aligned_size_remainder != 0) {
378                 aligned_size += (4 - aligned_size_remainder);
379         }
380
381         /*
382          * Fill message headers (fill ALL fields, except message)
383          * Note: olsr_vtime is currently unused, we use it for our validity time.
384          */
385
386         if (olsr_cnf->ip_version == AF_INET) {
387                 /* IPv4 */
388
389                 olsrMessage->v4.olsr_msgtype = PUD_OLSR_MSG_TYPE;
390                 olsrMessage->v4.olsr_vtime = reltime_to_me(validityTime * 1000);
391                 /* message->v4.olsr_msgsize at the end */
392                 memcpy(&olsrMessage->v4.originator, &olsr_cnf->main_addr,
393                                 olsr_cnf->ipsize);
394                 olsrMessage->v4.ttl = getOlsrTtl();
395                 olsrMessage->v4.hopcnt = 0;
396                 olsrMessage->v4.seqno = htons(get_msg_seqno());
397
398                 /* add length of message->v4 fields */
399                 aligned_size += (sizeof(olsrMessage->v4)
400                                 - sizeof(olsrMessage->v4.message));
401                 olsrMessage->v4.olsr_msgsize = htons(aligned_size);
402         } else {
403                 /* IPv6 */
404
405                 olsrMessage->v6.olsr_msgtype = PUD_OLSR_MSG_TYPE;
406                 olsrMessage->v6.olsr_vtime = reltime_to_me(validityTime * 1000);
407                 /* message->v6.olsr_msgsize at the end */
408                 memcpy(&olsrMessage->v6.originator, &olsr_cnf->main_addr,
409                                 olsr_cnf->ipsize);
410                 olsrMessage->v6.ttl = getOlsrTtl();
411                 olsrMessage->v6.hopcnt = 0;
412                 olsrMessage->v6.seqno = htons(get_msg_seqno());
413
414                 /* add length of message->v6 fields */
415                 aligned_size += (sizeof(olsrMessage->v6)
416                                 - sizeof(olsrMessage->v6.message));
417                 olsrMessage->v6.olsr_msgsize = htons(aligned_size);
418         }
419
420         /* pad with zeroes */
421         if (aligned_size_remainder != 0) {
422                 memset(&(((char *) &olsrGpsMessage->nodeInfo.nodeIdType)[nodeLength]),
423                                 0, (4 - aligned_size_remainder));
424         }
425
426         return aligned_size;
427 }