PUD: split downlink debugging in rx and tx parts
[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 transmit socket address */
33 static union olsr_sockaddr * txAddress;
34
35 /** The de-duplication list */
36 static DeDupList deDupList;
37
38 /**
39  Report a plugin error.
40
41  @param useErrno
42  when true then errno is used in the error message; the error reason is also
43  reported.
44  @param format
45  a pointer to the format string
46  @param ...
47  arguments to the format string
48  */
49 void pudError(bool useErrno, const char *format, ...) {
50         char strDesc[256];
51         char *stringErr = NULL;
52
53         if (useErrno) {
54                 stringErr = strerror(errno);
55         }
56
57         if ((format == NULL) || (*format == '\0')) {
58                 if (useErrno) {
59                         olsr_printf(0, "%s: %s\n", PUD_PLUGIN_ABBR, stringErr);
60                 } else {
61                         olsr_printf(0, "%s: Unknown error\n", PUD_PLUGIN_ABBR);
62                 }
63         } else {
64                 va_list arglist;
65
66                 va_start(arglist, format);
67                 vsnprintf(strDesc, sizeof(strDesc), format, arglist);
68                 va_end(arglist);
69
70                 strDesc[sizeof(strDesc) - 1] = '\0'; /* Ensures null termination */
71
72                 if (useErrno) {
73                         olsr_printf(0, "%s: %s: %s\n", PUD_PLUGIN_ABBR, strDesc, stringErr);
74                 } else {
75                         olsr_printf(0, "%s: %s\n", PUD_PLUGIN_ABBR, strDesc);
76                 }
77         }
78 }
79
80 /**
81  Sends a buffer out on all transmit interfaces
82
83  @param buffer
84  the buffer
85  @param bufferLength
86  the number of bytes in the buffer
87  */
88 static void sendToAllTxInterfaces(unsigned char *buffer,
89                 unsigned int bufferLength) {
90         TRxTxNetworkInterface *txNetworkInterfaces = getTxNetworkInterfaces();
91         while (txNetworkInterfaces != NULL) {
92                 TRxTxNetworkInterface *networkInterface = txNetworkInterfaces;
93
94 #ifdef PUD_DUMP_GPS_PACKETS_TX_NON_OLSR
95                 olsr_printf(0, "%s: packet sent to non-OLSR interface %s (%u bytes)\n",
96                                 PUD_PLUGIN_ABBR, &networkInterface->name[0], bufferLength);
97                 dump_packet(&buffer[0], bufferLength);
98 #endif
99
100                 errno = 0;
101                 if (sendto(networkInterface->socketFd, buffer, bufferLength, 0,
102                                 (struct sockaddr *) &txAddress->in, sizeof(txAddress->in)) < 0) {
103                         pudError(true, "Transmit error on interface %s",
104                                         (char *) &networkInterface->name);
105                 }
106                 txNetworkInterfaces = networkInterface->next;
107         }
108 }
109
110 /**
111  Called by OLSR core when a packet for the plugin is received from the OLSR
112  network. It converts the packet into an NMEA string and transmits it over all
113  transmit non-OLSR network interfaces.
114
115  @param olsrMessage
116  a pointer to the received OLSR message
117  @param in_if
118  a pointer to the OLSR network interface on which the packet was received
119  @param ipaddr
120  a pointer to the IP address of the sender
121
122  @return
123  - true when the packet was processed
124  - false otherwise
125  */
126 bool packetReceivedFromOlsr(union olsr_message *olsrMessage,
127                 struct interface *in_if __attribute__ ((unused)), union olsr_ip_addr *ipaddr __attribute__ ((unused))) {
128         const union olsr_ip_addr * originator = getOlsrMessageOriginator(
129                         olsr_cnf->ip_version, olsrMessage);
130         unsigned int transmitStringLength;
131         unsigned char buffer[BUFFER_SIZE_TX_OLSR];
132
133 #ifdef PUD_DUMP_GPS_PACKETS_RX_OLSR
134         unsigned short olsrMessageSize =
135                         getOlsrMessageSize(olsr_cnf->ip_version, olsrMessage);
136 #endif
137
138         /* when we do not loopback then check if the message originated from this
139          * node: back off */
140         if (!getUseLoopback() && ipequal(originator, &olsr_cnf->main_addr)) {
141                 return false;
142         }
143
144         /* do deduplication: when we have already seen this message from the same
145          * originator then just back off */
146         if (likely(getUseDeDup())) {
147                 if (isInDeDupList(&deDupList, olsrMessage)) {
148                         return false;
149                 }
150
151                 addToDeDup(&deDupList, olsrMessage);
152         }
153
154 #ifdef PUD_DUMP_GPS_PACKETS_RX_OLSR
155         olsr_printf(0, "\n%s: packet received from OLSR interface %s (%u bytes)\n",
156                         PUD_PLUGIN_ABBR, in_if->int_name, olsrMessageSize);
157         dump_packet((unsigned char *) olsrMessage, olsrMessageSize);
158 #endif
159
160         transmitStringLength = gpsFromOlsr(olsrMessage, &buffer[0], sizeof(buffer));
161         if (unlikely(transmitStringLength == 0)) {
162                 return false;
163         }
164
165         sendToAllTxInterfaces(&buffer[0], transmitStringLength);
166
167         return true;
168 }
169
170 /**
171  Called by OLSR core when a packet for the plugin is received from the downlink.
172  It unpacks the messages and distributes them into OLSR and on the LAN.
173
174  @param skfd
175  the socket file descriptor on which the packet is received
176  @param data
177  a pointer to the network interface structure on which the packet was received
178  @param flags
179  unused
180  */
181 static void packetReceivedFromDownlink(int skfd, void *data __attribute__ ((unused)), unsigned int flags __attribute__ ((unused))) {
182         if (skfd >= 0) {
183                 unsigned char rxBuffer[BUFFER_SIZE_RX_DOWNLINK];
184                 ssize_t rxCount;
185                 ssize_t rxIndex = 0;
186                 struct sockaddr sender;
187                 socklen_t senderSize = sizeof(sender);
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 (!isInDeDupList(&deDupList, olsrMessage)) {
259                                 /* send out over OLSR interfaces */
260                                 int r;
261                                 struct interface *ifn;
262                                 for (ifn = ifnet; ifn; ifn = ifn->int_next) {
263                                         r = net_outbuffer_push(ifn, olsrMessage, olsrMessageLength);
264                                         if (r != (int) olsrMessageLength) {
265                                                 pudError(
266                                                                 false,
267                                                                 "Could not send to OLSR interface %s: %s"
268                                                                                 " (length=%u, r=%d)",
269                                                                 ifn->int_name,
270                                                                 ((r == -1) ? "no buffer was found" :
271                                                                         (r == 0) ? "there was not enough room in the buffer" :
272                                                                                         "unknown reason"), olsrMessageLength, r);
273                                         }
274 #ifdef PUD_DUMP_GPS_PACKETS_TX_DOWNLINK
275                                         else {
276                                                 olsr_printf(0, "%s: downlink position update sent"
277                                                                 " to OLSR interface %s (%d bytes)\n",
278                                                                 PUD_PLUGIN_ABBR, ifn->int_name, olsrMessageLength);
279                                         }
280 #endif
281                                 }
282
283                                 /* send out over tx interfaces */
284                                 (void) packetReceivedFromOlsr(olsrMessage, NULL, NULL);
285 #ifdef PUD_DUMP_GPS_PACKETS_TX_DOWNLINK
286                                 olsr_printf(0, "%s: downlink position update sent"
287                                                 " to tx interfaces (%d bytes)\n", PUD_PLUGIN_ABBR,
288                                                 olsrMessageLength);
289 #endif
290
291 #ifdef PUD_DUMP_GPS_PACKETS_TX_DOWNLINK
292                         {
293                                 void * src;
294                                 char fromAddr[64];
295                                 union olsr_ip_addr * originator = getOlsrMessageOriginator(
296                                                 olsr_cnf->ip_version, olsrMessage);
297
298                                 if (olsr_cnf->ip_version == AF_INET) {
299                                         src = &originator->v4;
300                                 } else {
301                                         src = &originator->v6;
302                                 }
303
304                                 inet_ntop(olsr_cnf->ip_version, src, &fromAddr[0],
305                                                 sizeof(fromAddr));
306                                 olsr_printf(
307                                                 0,
308                                                 "%s: downlink position update from %s (%u bytes)\n",
309                                                 PUD_PLUGIN_ABBR, &fromAddr[0], uplinkMessageLength);
310
311                                 dump_packet((unsigned char *) msg, uplinkMessageLength);
312
313                                 olsr_printf(0, "\n");
314                         }
315 #endif
316                         }
317                         rxIndex += uplinkMessageLength;
318                 }
319         }
320 }
321
322 /**
323  Called by OLSR core when a packet for the plugin is received from the non-OLSR
324  network. It converts the packet into the internal OLSR wire format for a
325  position update and transmits it over all OLSR network interfaces.
326
327  @param skfd
328  the socket file descriptor on which the packet is received
329  @param data
330  a pointer to the network interface structure on which the packet was received
331  @param flags
332  unused
333  */
334 #ifdef PUD_DUMP_GPS_PACKETS_RX_NON_OLSR
335 static void packetReceivedForOlsr(int skfd, void *data, unsigned int flags __attribute__ ((unused))) {
336 #else
337 static void packetReceivedForOlsr(int skfd, void *data __attribute__ ((unused)), unsigned int flags __attribute__ ((unused))) {
338 #endif
339         if (skfd >= 0) {
340                 unsigned char rxBuffer[BUFFER_SIZE_RX_NMEA];
341                 ssize_t rxCount;
342                 struct sockaddr sender;
343                 socklen_t senderSize = sizeof(sender);
344
345                 assert(data != NULL);
346
347                 /* Receive the captured Ethernet frame */
348                 memset(&sender, 0, senderSize);
349                 errno = 0;
350                 rxCount = recvfrom(skfd, &rxBuffer[0], (sizeof(rxBuffer) - 1), 0,
351                                 &sender, &senderSize);
352                 if (rxCount < 0) {
353                         pudError(true, "Receive error in %s, ignoring message.", __func__);
354                         return;
355                 }
356
357                 /* make sure the string is null-terminated */
358                 rxBuffer[rxCount] = '\0';
359
360                 /* only accept messages from configured IP addresses */
361                 if (!isRxAllowedSourceIpAddress(&sender)) {
362                         return;
363                 }
364
365 #ifdef PUD_DUMP_GPS_PACKETS_RX_NON_OLSR
366                 {
367                         TRxTxNetworkInterface * networkInterface = data;
368                         void * src;
369                         in_port_t port;
370                         char fromAddr[64];
371
372                         if (olsr_cnf->ip_version == AF_INET) {
373                                 src = &((struct sockaddr_in*) &sender)->sin_addr;
374                                 port = ntohs(((struct sockaddr_in*) &sender)->sin_port);
375                         } else {
376                                 src = &((struct sockaddr_in6*) &sender)->sin6_addr;
377                                 port = ntohs(((struct sockaddr_in6*) &sender)->sin6_port);
378                         }
379
380                         inet_ntop(olsr_cnf->ip_version, src, &fromAddr[0], sizeof(fromAddr));
381                         olsr_printf(0, "\n%s: packet received from %s, port %u on non-OLSR"
382                                         " interface %s (%lu bytes)\n", PUD_PLUGIN_ABBR, &fromAddr[0],
383                                         port, &networkInterface->name[0], (size_t) rxCount);
384
385                         dump_packet(&rxBuffer[0], (size_t)rxCount);
386                 }
387 #endif
388
389                 /* we have the received string in the rxBuffer now */
390
391                 /* hand the NMEA information to the receiver */
392                 (void) receiverUpdateGpsInformation(&rxBuffer[0], rxCount);
393         }
394 }
395
396 /**
397  Initialise the plugin: check the configuration, initialise the NMEA parser,
398  create network interface sockets, hookup the plugin to OLSR and setup data
399  that can be setup in advance.
400
401  @return
402  - false upon failure
403  - true otherwise
404  */
405 bool initPud(void) {
406         if (!checkConfig()) {
407                 pudError(false, "Invalid configuration");
408                 goto error;
409         }
410
411         initDeDupList(&deDupList, getDeDupDepth());
412
413         /* set global transmit socket config */
414         txAddress = getTxMcAddr();
415
416         if (!startReceiver()) {
417                 pudError(false, "Could not start receiver");
418                 goto error;
419         }
420
421         /*
422          * Creates receive and transmit sockets and register the receive sockets
423          * with the OLSR stack
424          */
425         if (!createNetworkInterfaces(&packetReceivedForOlsr,
426                         &packetReceivedFromDownlink)) {
427                 pudError(false, "Could not create require network interfaces");
428                 goto error;
429         }
430
431         if (!checkRunSetup()) {
432                 pudError(false, "Invalid configuration");
433                 goto error;
434         }
435
436         /*
437          * Tell OLSR to launch olsr_parser when the packets for this plugin
438          * arrive from the OLSR network
439          */
440         olsr_parser_add_function(&packetReceivedFromOlsr, PUD_OLSR_MSG_TYPE);
441
442         return true;
443
444         error: closePud();
445         return false;
446 }
447
448 /**
449  Stop the plugin: shut down all created network interface sockets and destroy
450  the NMEA parser.
451  */
452 void closePud(void) {
453         closeNetworkInterfaces();
454         stopReceiver();
455         destroyDeDupList(&deDupList);
456 }