63c5c34519c74b9b43bc1f8d92713fc33cc3211d
[olsrd.git] / lib / pud / src / pud.c
1 #include "pud.h"
2
3 /* Plugin includes */
4 #include "configuration.h"
5 #include "networkInterfaces.h"
6 #include "dump.h"
7 #include "gpsConversion.h"
8 #include "receiver.h"
9 #include "dedup.h"
10 #include "compiler.h"
11
12 /* OLSRD includes */
13 #include "ipcalc.h"
14 #include "parser.h"
15 #include "olsr.h"
16 #include "net_olsr.h"
17
18 /* System includes */
19 #include <assert.h>
20 #include <sys/socket.h>
21
22 /** The size of the buffer in which the received NMEA string is stored */
23 #define BUFFER_SIZE_RX_NMEA             2048
24
25 /** The size of the buffer in which the received downlink message is stored */
26 #define BUFFER_SIZE_RX_DOWNLINK 2048
27
28 /** The size of the buffer in which the converted NMEA string is assembled for
29  * transmission over OSLR */
30 #define BUFFER_SIZE_TX_OLSR     512
31
32 /** The de-duplication list */
33 static DeDupList deDupList;
34
35 /**
36  Report a plugin error.
37
38  @param useErrno
39  when true then errno is used in the error message; the error reason is also
40  reported.
41  @param format
42  a pointer to the format string
43  @param ...
44  arguments to the format string
45  */
46 void pudError(bool useErrno, const char *format, ...) {
47         char strDesc[256];
48         char *stringErr = NULL;
49
50         if (useErrno) {
51                 stringErr = strerror(errno);
52         }
53
54         if ((format == NULL) || (*format == '\0')) {
55                 if (useErrno) {
56                         olsr_printf(0, "%s: %s\n", PUD_PLUGIN_ABBR, stringErr);
57                 } else {
58                         olsr_printf(0, "%s: Unknown error\n", PUD_PLUGIN_ABBR);
59                 }
60         } else {
61                 va_list arglist;
62
63                 va_start(arglist, format);
64                 vsnprintf(strDesc, sizeof(strDesc), format, arglist);
65                 va_end(arglist);
66
67                 strDesc[sizeof(strDesc) - 1] = '\0'; /* Ensures null termination */
68
69                 if (useErrno) {
70                         olsr_printf(0, "%s: %s: %s\n", PUD_PLUGIN_ABBR, strDesc, stringErr);
71                 } else {
72                         olsr_printf(0, "%s: %s\n", PUD_PLUGIN_ABBR, strDesc);
73                 }
74         }
75 }
76
77 /**
78  Sends a buffer out on all transmit interfaces
79
80  @param buffer
81  the buffer
82  @param bufferLength
83  the number of bytes in the buffer
84  */
85 static void sendToAllTxInterfaces(unsigned char *buffer,
86                 unsigned int bufferLength) {
87         union olsr_sockaddr * txAddress;
88         TRxTxNetworkInterface *txNetworkInterfaces = getTxNetworkInterfaces();
89         while (txNetworkInterfaces != NULL) {
90                 TRxTxNetworkInterface *networkInterface = txNetworkInterfaces;
91
92 #ifdef PUD_DUMP_GPS_PACKETS_TX_NON_OLSR
93                 olsr_printf(0, "%s: packet sent to non-OLSR interface %s (%u bytes)\n",
94                                 PUD_PLUGIN_ABBR, &networkInterface->name[0], bufferLength);
95                 dump_packet(&buffer[0], bufferLength);
96 #endif
97
98                 errno = 0;
99                 txAddress = getTxMcAddr();
100                 if (sendto(networkInterface->socketFd, buffer, bufferLength, 0,
101                                 (struct sockaddr *) &txAddress->in, sizeof(txAddress->in)) < 0) {
102                         pudError(true, "Transmit error on interface %s",
103                                         (char *) &networkInterface->name);
104                 }
105                 txNetworkInterfaces = networkInterface->next;
106         }
107 }
108
109 /**
110  Called by OLSR core when a packet for the plugin is received from the OLSR
111  network. It converts the packet into an NMEA string and transmits it over all
112  transmit non-OLSR network interfaces.
113
114  @param olsrMessage
115  a pointer to the received OLSR message
116  @param in_if
117  a pointer to the OLSR network interface on which the packet was received
118  @param ipaddr
119  a pointer to the IP address of the sender
120
121  @return
122  - true when the packet was processed
123  - false otherwise
124  */
125 bool packetReceivedFromOlsr(union olsr_message *olsrMessage,
126                 struct interface *in_if __attribute__ ((unused)), union olsr_ip_addr *ipaddr __attribute__ ((unused))) {
127         const union olsr_ip_addr * originator = getOlsrMessageOriginator(
128                         olsr_cnf->ip_version, olsrMessage);
129         unsigned int transmitStringLength;
130         unsigned char buffer[BUFFER_SIZE_TX_OLSR];
131
132 #ifdef PUD_DUMP_GPS_PACKETS_RX_OLSR
133         unsigned short olsrMessageSize =
134                         getOlsrMessageSize(olsr_cnf->ip_version, olsrMessage);
135 #endif
136
137         /* when we do not loopback then check if the message originated from this
138          * node: back off */
139         if (!getUseLoopback() && ipequal(originator, &olsr_cnf->main_addr)) {
140                 return false;
141         }
142
143         /* do deduplication: when we have already seen this message from the same
144          * originator then just back off */
145         if (likely(getUseDeDup())) {
146                 if (isInDeDupList(&deDupList, olsrMessage)) {
147                         return false;
148                 }
149
150                 addToDeDup(&deDupList, olsrMessage);
151         }
152
153 #ifdef PUD_DUMP_GPS_PACKETS_RX_OLSR
154         olsr_printf(0, "\n%s: packet received from OLSR interface %s (%u bytes)\n",
155                         PUD_PLUGIN_ABBR, in_if->int_name, olsrMessageSize);
156         dump_packet((unsigned char *) olsrMessage, olsrMessageSize);
157 #endif
158
159         transmitStringLength = gpsFromOlsr(olsrMessage, &buffer[0], sizeof(buffer));
160         if (unlikely(transmitStringLength == 0)) {
161                 return false;
162         }
163
164         sendToAllTxInterfaces(&buffer[0], transmitStringLength);
165
166         return true;
167 }
168
169 /**
170  Called by OLSR core when a packet for the plugin is received from the downlink.
171  It unpacks the messages and distributes them into OLSR and on the LAN.
172
173  @param skfd
174  the socket file descriptor on which the packet is received
175  @param data
176  a pointer to the network interface structure on which the packet was received
177  @param flags
178  unused
179  */
180 static void packetReceivedFromDownlink(int skfd, void *data __attribute__ ((unused)), unsigned int flags __attribute__ ((unused))) {
181         if (skfd >= 0) {
182                 unsigned char rxBuffer[BUFFER_SIZE_RX_DOWNLINK];
183                 ssize_t rxCount;
184                 ssize_t rxIndex = 0;
185                 struct sockaddr sender;
186                 socklen_t senderSize = sizeof(sender);
187
188                 /* Receive the captured Ethernet frame */
189                 memset(&sender, 0, senderSize);
190                 errno = 0;
191                 rxCount = recvfrom(skfd, &rxBuffer[0], (sizeof(rxBuffer) - 1), 0,
192                                 &sender, &senderSize);
193                 if (rxCount < 0) {
194                         pudError(true, "Receive error in %s, ignoring message.", __func__);
195                         return;
196                 }
197
198 #ifdef PUD_DUMP_GPS_PACKETS_RX_DOWNLINK
199                 {
200                         void * src;
201                         in_port_t port;
202                         char fromAddr[64];
203
204                         if (olsr_cnf->ip_version == AF_INET) {
205                                 src = &((struct sockaddr_in*) &sender)->sin_addr;
206                                 port = ntohs(((struct sockaddr_in*) &sender)->sin_port);
207                         } else {
208                                 src = &((struct sockaddr_in6*) &sender)->sin6_addr;
209                                 port = ntohs(((struct sockaddr_in6*) &sender)->sin6_port);
210                         }
211
212                         inet_ntop(olsr_cnf->ip_version, src, &fromAddr[0], sizeof(fromAddr));
213                         olsr_printf(0, "\n%s: downlink packet received from %s, port %u (%lu bytes)\n",
214                                         PUD_PLUGIN_ABBR, &fromAddr[0],
215                                         port, (size_t) rxCount);
216                 }
217 #endif
218
219                 while (rxIndex < rxCount) {
220                         UplinkMessage * msg = (UplinkMessage *) &rxBuffer[rxIndex];
221                         uint8_t type;
222                         uint16_t uplinkMessageLength;
223                         uint16_t olsrMessageLength;
224                         bool ipv6;
225                         union olsr_message * olsrMessage;
226
227                         type = getUplinkMessageType(&msg->header);
228                         if (type != POSITION) {
229                                 pudError(false, "Received wrong type (%d) in %s,"
230                                                 " ignoring message.", type, __func__);
231                                 continue;
232                         }
233
234                         ipv6 = getUplinkMessageIPv6(&msg->header);
235                         if (unlikely(ipv6 && (olsr_cnf->ip_version != AF_INET6))) {
236                                 pudError(false, "Received wrong IPv6 status (%s) in %s,"
237                                                 " ignoring message.", (ipv6 ? "true" : "false"),
238                                                 __func__);
239                                 continue;
240                         }
241
242                         olsrMessageLength = getUplinkMessageLength(&msg->header);
243                         uplinkMessageLength = olsrMessageLength + sizeof(UplinkHeader);
244
245                         if (unlikely((rxIndex + uplinkMessageLength) > rxCount)) {
246                                 pudError(false, "Received wrong length (%d) in %s,"
247                                                 " ignoring the rest of the messages.", olsrMessageLength,
248                                                 __func__);
249                                 break;
250                         }
251
252                         olsrMessage = &msg->msg.olsrMessage;
253
254                         /* we now have a position update (olsrMessage) of a certain length
255                          * (length). this needs to be transmitted over OLSR and on the LAN */
256
257                         if (!isInDeDupList(&deDupList, olsrMessage)) {
258                                 /* send out over OLSR interfaces */
259                                 int r;
260                                 struct interface *ifn;
261                                 for (ifn = ifnet; ifn; ifn = ifn->int_next) {
262                                         r = net_outbuffer_push(ifn, olsrMessage, olsrMessageLength);
263                                         if (r != (int) olsrMessageLength) {
264                                                 pudError(
265                                                                 false,
266                                                                 "Could not send to OLSR interface %s: %s"
267                                                                                 " (length=%u, r=%d)",
268                                                                 ifn->int_name,
269                                                                 ((r == -1) ? "no buffer was found" :
270                                                                         (r == 0) ? "there was not enough room in the buffer" :
271                                                                                         "unknown reason"), olsrMessageLength, r);
272                                         }
273 #ifdef PUD_DUMP_GPS_PACKETS_TX_DOWNLINK
274                                         else {
275                                                 olsr_printf(0, "%s: downlink position update sent"
276                                                                 " to OLSR interface %s (%d bytes)\n",
277                                                                 PUD_PLUGIN_ABBR, ifn->int_name, olsrMessageLength);
278                                         }
279 #endif
280                                 }
281
282                                 /* send out over tx interfaces */
283                                 (void) packetReceivedFromOlsr(olsrMessage, NULL, NULL);
284 #ifdef PUD_DUMP_GPS_PACKETS_TX_DOWNLINK
285                                 olsr_printf(0, "%s: downlink position update sent"
286                                                 " to tx interfaces (%d bytes)\n", PUD_PLUGIN_ABBR,
287                                                 olsrMessageLength);
288 #endif
289
290 #ifdef PUD_DUMP_GPS_PACKETS_TX_DOWNLINK
291                         {
292                                 void * src;
293                                 char fromAddr[64];
294                                 union olsr_ip_addr * originator = getOlsrMessageOriginator(
295                                                 olsr_cnf->ip_version, olsrMessage);
296
297                                 if (olsr_cnf->ip_version == AF_INET) {
298                                         src = &originator->v4;
299                                 } else {
300                                         src = &originator->v6;
301                                 }
302
303                                 inet_ntop(olsr_cnf->ip_version, src, &fromAddr[0],
304                                                 sizeof(fromAddr));
305                                 olsr_printf(
306                                                 0,
307                                                 "%s: downlink position update from %s (%u bytes)\n",
308                                                 PUD_PLUGIN_ABBR, &fromAddr[0], uplinkMessageLength);
309
310                                 dump_packet((unsigned char *) msg, uplinkMessageLength);
311
312                                 olsr_printf(0, "\n");
313                         }
314 #endif
315                         }
316                         rxIndex += uplinkMessageLength;
317                 }
318         }
319 }
320
321 /**
322  Called by OLSR core when a packet for the plugin is received from the non-OLSR
323  network. It converts the packet into the internal OLSR wire format for a
324  position update and transmits it over all OLSR network interfaces.
325
326  @param skfd
327  the socket file descriptor on which the packet is received
328  @param data
329  a pointer to the network interface structure on which the packet was received
330  @param flags
331  unused
332  */
333 #ifdef PUD_DUMP_GPS_PACKETS_RX_NON_OLSR
334 static void packetReceivedForOlsr(int skfd, void *data, unsigned int flags __attribute__ ((unused))) {
335 #else
336 static void packetReceivedForOlsr(int skfd, void *data __attribute__ ((unused)), unsigned int flags __attribute__ ((unused))) {
337 #endif
338         if (skfd >= 0) {
339                 unsigned char rxBuffer[BUFFER_SIZE_RX_NMEA];
340                 ssize_t rxCount;
341                 struct sockaddr sender;
342                 socklen_t senderSize = sizeof(sender);
343
344                 assert(data != NULL);
345
346                 /* Receive the captured Ethernet frame */
347                 memset(&sender, 0, senderSize);
348                 errno = 0;
349                 rxCount = recvfrom(skfd, &rxBuffer[0], (sizeof(rxBuffer) - 1), 0,
350                                 &sender, &senderSize);
351                 if (rxCount < 0) {
352                         pudError(true, "Receive error in %s, ignoring message.", __func__);
353                         return;
354                 }
355
356                 /* make sure the string is null-terminated */
357                 rxBuffer[rxCount] = '\0';
358
359                 /* only accept messages from configured IP addresses */
360                 if (!isRxAllowedSourceIpAddress(&sender)) {
361                         return;
362                 }
363
364 #ifdef PUD_DUMP_GPS_PACKETS_RX_NON_OLSR
365                 {
366                         TRxTxNetworkInterface * networkInterface = data;
367                         void * src;
368                         in_port_t port;
369                         char fromAddr[64];
370
371                         if (olsr_cnf->ip_version == AF_INET) {
372                                 src = &((struct sockaddr_in*) &sender)->sin_addr;
373                                 port = ntohs(((struct sockaddr_in*) &sender)->sin_port);
374                         } else {
375                                 src = &((struct sockaddr_in6*) &sender)->sin6_addr;
376                                 port = ntohs(((struct sockaddr_in6*) &sender)->sin6_port);
377                         }
378
379                         inet_ntop(olsr_cnf->ip_version, src, &fromAddr[0], sizeof(fromAddr));
380                         olsr_printf(0, "\n%s: packet received from %s, port %u on non-OLSR"
381                                         " interface %s (%lu bytes)\n", PUD_PLUGIN_ABBR, &fromAddr[0],
382                                         port, &networkInterface->name[0], (size_t) rxCount);
383
384                         dump_packet(&rxBuffer[0], (size_t)rxCount);
385                 }
386 #endif
387
388                 /* we have the received string in the rxBuffer now */
389
390                 /* hand the NMEA information to the receiver */
391                 (void) receiverUpdateGpsInformation(&rxBuffer[0], rxCount);
392         }
393 }
394
395 /**
396  Initialise the plugin: check the configuration, initialise the NMEA parser,
397  create network interface sockets, hookup the plugin to OLSR and setup data
398  that can be setup in advance.
399
400  @return
401  - false upon failure
402  - true otherwise
403  */
404 bool initPud(void) {
405         if (!checkConfig()) {
406                 pudError(false, "Invalid configuration");
407                 goto error;
408         }
409
410         initDeDupList(&deDupList, getDeDupDepth());
411
412         if (!startReceiver()) {
413                 pudError(false, "Could not start receiver");
414                 goto error;
415         }
416
417         /*
418          * Creates receive and transmit sockets and register the receive sockets
419          * with the OLSR stack
420          */
421         if (!createNetworkInterfaces(&packetReceivedForOlsr,
422                         &packetReceivedFromDownlink)) {
423                 pudError(false, "Could not create require network interfaces");
424                 goto error;
425         }
426
427         if (!checkRunSetup()) {
428                 pudError(false, "Invalid configuration");
429                 goto error;
430         }
431
432         /*
433          * Tell OLSR to call packetReceivedFromOlsr when the packets for this
434          * plugin arrive from the OLSR network
435          */
436         olsr_parser_add_function(&packetReceivedFromOlsr, PUD_OLSR_MSG_TYPE);
437
438         return true;
439
440         error: closePud();
441         return false;
442 }
443
444 /**
445  Stop the plugin: shut down all created network interface sockets and destroy
446  the NMEA parser.
447  */
448 void closePud(void) {
449         closeNetworkInterfaces();
450         stopReceiver();
451         destroyDeDupList(&deDupList);
452 }