PUD: initDeDupList can fail
[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                 bool addedToDedup = false;
188
189                 /* Receive the captured Ethernet frame */
190                 memset(&sender, 0, senderSize);
191                 errno = 0;
192                 rxCount = recvfrom(skfd, &rxBuffer[0], (sizeof(rxBuffer) - 1), 0,
193                                 &sender, &senderSize);
194                 if (rxCount < 0) {
195                         pudError(true, "Receive error in %s, ignoring message.", __func__);
196                         return;
197                 }
198
199 #ifdef PUD_DUMP_GPS_PACKETS_RX_DOWNLINK
200                 {
201                         void * src;
202                         in_port_t port;
203                         char fromAddr[64];
204
205                         if (olsr_cnf->ip_version == AF_INET) {
206                                 src = &((struct sockaddr_in*) &sender)->sin_addr;
207                                 port = ntohs(((struct sockaddr_in*) &sender)->sin_port);
208                         } else {
209                                 src = &((struct sockaddr_in6*) &sender)->sin6_addr;
210                                 port = ntohs(((struct sockaddr_in6*) &sender)->sin6_port);
211                         }
212
213                         inet_ntop(olsr_cnf->ip_version, src, &fromAddr[0], sizeof(fromAddr));
214                         olsr_printf(0, "\n%s: downlink packet received from %s, port %u (%lu bytes)\n",
215                                         PUD_PLUGIN_ABBR, &fromAddr[0],
216                                         port, (size_t) rxCount);
217                 }
218 #endif
219
220                 while (rxIndex < rxCount) {
221                         UplinkMessage * msg = (UplinkMessage *) &rxBuffer[rxIndex];
222                         uint8_t type;
223                         uint16_t uplinkMessageLength;
224                         uint16_t olsrMessageLength;
225                         bool ipv6;
226                         union olsr_message * olsrMessage;
227
228                         type = getUplinkMessageType(&msg->header);
229                         if (type != POSITION) {
230                                 pudError(false, "Received wrong type (%d) in %s,"
231                                                 " ignoring message.", type, __func__);
232                                 continue;
233                         }
234
235                         ipv6 = getUplinkMessageIPv6(&msg->header);
236                         if (unlikely(ipv6 && (olsr_cnf->ip_version != AF_INET6))) {
237                                 pudError(false, "Received wrong IPv6 status (%s) in %s,"
238                                                 " ignoring message.", (ipv6 ? "true" : "false"),
239                                                 __func__);
240                                 continue;
241                         }
242
243                         olsrMessageLength = getUplinkMessageLength(&msg->header);
244                         uplinkMessageLength = olsrMessageLength + sizeof(UplinkHeader);
245
246                         if (unlikely((rxIndex + uplinkMessageLength) > rxCount)) {
247                                 pudError(false, "Received wrong length (%d) in %s,"
248                                                 " ignoring the rest of the messages.", olsrMessageLength,
249                                                 __func__);
250                                 break;
251                         }
252
253                         olsrMessage = &msg->msg.olsrMessage;
254
255                         /* we now have a position update (olsrMessage) of a certain length
256                          * (length). this needs to be transmitted over OLSR and on the LAN */
257
258                         if (likely(getUseDeDup()) && !isInDeDupList(&deDupList, olsrMessage)) {
259                                 addToDeDup(&deDupList, olsrMessage);
260                                 addedToDedup = true;
261                         }
262
263                         if (likely(addedToDedup) || unlikely(!getUseDeDup())) {
264                                 /* send out over OLSR interfaces */
265                                 int r;
266                                 struct interface *ifn;
267                                 for (ifn = ifnet; ifn; ifn = ifn->int_next) {
268                                         r = net_outbuffer_push(ifn, olsrMessage, olsrMessageLength);
269                                         if (r != (int) olsrMessageLength) {
270                                                 pudError(
271                                                                 false,
272                                                                 "Could not send to OLSR interface %s: %s"
273                                                                                 " (length=%u, r=%d)",
274                                                                 ifn->int_name,
275                                                                 ((r == -1) ? "no buffer was found" :
276                                                                         (r == 0) ? "there was not enough room in the buffer" :
277                                                                                         "unknown reason"), olsrMessageLength, r);
278                                         }
279 #ifdef PUD_DUMP_GPS_PACKETS_TX_DOWNLINK
280                                         else {
281                                                 olsr_printf(0, "%s: downlink position update sent"
282                                                                 " to OLSR interface %s (%d bytes)\n",
283                                                                 PUD_PLUGIN_ABBR, ifn->int_name, olsrMessageLength);
284                                         }
285 #endif
286                                 }
287
288                                 /* send out over tx interfaces */
289                                 (void) packetReceivedFromOlsr(olsrMessage, NULL, NULL);
290 #ifdef PUD_DUMP_GPS_PACKETS_TX_DOWNLINK
291                                 olsr_printf(0, "%s: downlink position update sent"
292                                                 " to tx interfaces (%d bytes)\n", PUD_PLUGIN_ABBR,
293                                                 olsrMessageLength);
294 #endif
295
296 #ifdef PUD_DUMP_GPS_PACKETS_TX_DOWNLINK
297                         {
298                                 void * src;
299                                 char fromAddr[64];
300                                 union olsr_ip_addr * originator = getOlsrMessageOriginator(
301                                                 olsr_cnf->ip_version, olsrMessage);
302
303                                 if (olsr_cnf->ip_version == AF_INET) {
304                                         src = &originator->v4;
305                                 } else {
306                                         src = &originator->v6;
307                                 }
308
309                                 inet_ntop(olsr_cnf->ip_version, src, &fromAddr[0],
310                                                 sizeof(fromAddr));
311                                 olsr_printf(
312                                                 0,
313                                                 "%s: downlink position update from %s (%u bytes)\n",
314                                                 PUD_PLUGIN_ABBR, &fromAddr[0], uplinkMessageLength);
315
316                                 dump_packet((unsigned char *) msg, uplinkMessageLength);
317
318                                 olsr_printf(0, "\n");
319                         }
320 #endif
321                         }
322                         rxIndex += uplinkMessageLength;
323                 }
324         }
325 }
326
327 /**
328  Called by OLSR core when a packet for the plugin is received from the non-OLSR
329  network. It converts the packet into the internal OLSR wire format for a
330  position update and transmits it over all OLSR network interfaces.
331
332  @param skfd
333  the socket file descriptor on which the packet is received
334  @param data
335  a pointer to the network interface structure on which the packet was received
336  @param flags
337  unused
338  */
339 #ifdef PUD_DUMP_GPS_PACKETS_RX_NON_OLSR
340 static void packetReceivedForOlsr(int skfd, void *data, unsigned int flags __attribute__ ((unused))) {
341 #else
342 static void packetReceivedForOlsr(int skfd, void *data __attribute__ ((unused)), unsigned int flags __attribute__ ((unused))) {
343 #endif
344         if (skfd >= 0) {
345                 unsigned char rxBuffer[BUFFER_SIZE_RX_NMEA];
346                 ssize_t rxCount;
347                 struct sockaddr sender;
348                 socklen_t senderSize = sizeof(sender);
349
350                 assert(data != NULL);
351
352                 /* Receive the captured Ethernet frame */
353                 memset(&sender, 0, senderSize);
354                 errno = 0;
355                 rxCount = recvfrom(skfd, &rxBuffer[0], (sizeof(rxBuffer) - 1), 0,
356                                 &sender, &senderSize);
357                 if (rxCount < 0) {
358                         pudError(true, "Receive error in %s, ignoring message.", __func__);
359                         return;
360                 }
361
362                 /* make sure the string is null-terminated */
363                 rxBuffer[rxCount] = '\0';
364
365                 /* only accept messages from configured IP addresses */
366                 if (!isRxAllowedSourceIpAddress(&sender)) {
367                         return;
368                 }
369
370 #ifdef PUD_DUMP_GPS_PACKETS_RX_NON_OLSR
371                 {
372                         TRxTxNetworkInterface * networkInterface = data;
373                         void * src;
374                         in_port_t port;
375                         char fromAddr[64];
376
377                         if (olsr_cnf->ip_version == AF_INET) {
378                                 src = &((struct sockaddr_in*) &sender)->sin_addr;
379                                 port = ntohs(((struct sockaddr_in*) &sender)->sin_port);
380                         } else {
381                                 src = &((struct sockaddr_in6*) &sender)->sin6_addr;
382                                 port = ntohs(((struct sockaddr_in6*) &sender)->sin6_port);
383                         }
384
385                         inet_ntop(olsr_cnf->ip_version, src, &fromAddr[0], sizeof(fromAddr));
386                         olsr_printf(0, "\n%s: packet received from %s, port %u on non-OLSR"
387                                         " interface %s (%lu bytes)\n", PUD_PLUGIN_ABBR, &fromAddr[0],
388                                         port, &networkInterface->name[0], (size_t) rxCount);
389
390                         dump_packet(&rxBuffer[0], (size_t)rxCount);
391                 }
392 #endif
393
394                 /* we have the received string in the rxBuffer now */
395
396                 /* hand the NMEA information to the receiver */
397                 (void) receiverUpdateGpsInformation(&rxBuffer[0], rxCount);
398         }
399 }
400
401 /**
402  Initialise the plugin: check the configuration, initialise the NMEA parser,
403  create network interface sockets, hookup the plugin to OLSR and setup data
404  that can be setup in advance.
405
406  @return
407  - false upon failure
408  - true otherwise
409  */
410 bool initPud(void) {
411         if (!checkConfig()) {
412                 pudError(false, "Invalid configuration");
413                 goto error;
414         }
415
416         if (!initDeDupList(&deDupList, getDeDupDepth())) {
417                 pudError(false, "Could not initialise de-duplication list");
418                 goto error;
419         }
420
421         if (!startReceiver()) {
422                 pudError(false, "Could not start receiver");
423                 goto error;
424         }
425
426         /*
427          * Creates receive and transmit sockets and register the receive sockets
428          * with the OLSR stack
429          */
430         if (!createNetworkInterfaces(&packetReceivedForOlsr,
431                         &packetReceivedFromDownlink)) {
432                 pudError(false, "Could not create require network interfaces");
433                 goto error;
434         }
435
436         if (!checkRunSetup()) {
437                 pudError(false, "Invalid configuration");
438                 goto error;
439         }
440
441         /*
442          * Tell OLSR to call packetReceivedFromOlsr when the packets for this
443          * plugin arrive from the OLSR network
444          */
445         olsr_parser_add_function(&packetReceivedFromOlsr, PUD_OLSR_MSG_TYPE);
446
447         return true;
448
449         error: closePud();
450         return false;
451 }
452
453 /**
454  Stop the plugin: shut down all created network interface sockets and destroy
455  the NMEA parser.
456  */
457 void closePud(void) {
458         olsr_parser_remove_function(&packetReceivedFromOlsr, PUD_OLSR_MSG_TYPE);
459         closeNetworkInterfaces();
460         stopReceiver();
461         destroyDeDupList(&deDupList);
462 }