PUD: introduce wireFormat.c
[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
11 /* OLSRD includes */
12 #include "olsr.h"
13 #include "cfgparser/olsrd_conf.h"
14 #include "ipcalc.h"
15 #include "nmea/parser.h"
16 #include "parser.h"
17
18 /* System includes */
19 #include <nmea/parser.h>
20 #include <stddef.h>
21 #include <string.h>
22 #include <errno.h>
23 #include <stdarg.h>
24 #include <assert.h>
25 #include <stdio.h>
26 #include <sys/socket.h>
27 #include <sys/types.h>
28 #include <unistd.h>
29
30 /** The size of the buffer in which the received NMEA string is stored */
31 #define BUFFER_SIZE_FOR_OLSR    2048
32
33 /** The size of the buffer in which the transmit NMEA string is assembled */
34 #define BUFFER_SIZE_FROM_OLSR   512
35
36 /** The transmit socket address */
37 static union olsr_sockaddr * txAddress;
38
39 /** The de-duplication list */
40 static DeDupList deDupList;
41
42 /**
43  Report a plugin error.
44
45  @param useErrno
46  when true then errno is used in the error message; the error reason is also
47  reported.
48  @param format
49  a pointer to the format string
50  @param ...
51  arguments to the format string
52  */
53 void pudError(bool useErrno, const char *format, ...) {
54         char strDesc[256];
55         char *stringErr = NULL;
56
57         if (useErrno) {
58                 stringErr = strerror(errno);
59         }
60
61         if ((format == NULL) || (*format == '\0')) {
62                 if (useErrno) {
63                         olsr_printf(0, "%s: %s\n", PUD_PLUGIN_ABBR, stringErr);
64                 } else {
65                         olsr_printf(0, "%s: Unknown error\n", PUD_PLUGIN_ABBR);
66                 }
67         } else {
68                 va_list arglist;
69
70                 va_start(arglist, format);
71                 vsnprintf(strDesc, sizeof(strDesc), format, arglist);
72                 va_end(arglist);
73
74                 strDesc[sizeof(strDesc) - 1] = '\0'; /* Ensures null termination */
75
76                 if (useErrno) {
77                         olsr_printf(0, "%s: %s: %s\n", PUD_PLUGIN_ABBR, strDesc, stringErr);
78                 } else {
79                         olsr_printf(0, "%s: %s\n", PUD_PLUGIN_ABBR, strDesc);
80                 }
81         }
82 }
83
84 /**
85  Sends a buffer out on all transmit interfaces
86
87  @param buffer
88  the buffer
89  @param bufferLength
90  the number of bytes in the buffer
91  */
92 static void sendToAllTxInterfaces(unsigned char *buffer,
93                 unsigned int bufferLength) {
94         TRxTxNetworkInterface *txNetworkInterfaces = getTxNetworkInterfaces();
95         while (txNetworkInterfaces != NULL) {
96                 TRxTxNetworkInterface *networkInterface = txNetworkInterfaces;
97
98 #ifdef PUD_DUMP_GPS_PACKETS_TX_NON_OLSR
99                 olsr_printf(0, "%s: packet sent to non-OLSR interface %s (%u bytes)\n",
100                                 PUD_PLUGIN_ABBR, &networkInterface->name[0], bufferLength);
101                 dump_packet(&buffer[0], bufferLength);
102 #endif
103
104                 errno = 0;
105                 if (sendto(networkInterface->socketFd, buffer, bufferLength, 0,
106                                 (struct sockaddr *) &txAddress->in, sizeof(txAddress->in)) < 0) {
107                         pudError(true, "Transmit error on interface %s",
108                                         (char *) &networkInterface->name);
109                 }
110                 txNetworkInterfaces = networkInterface->next;
111         }
112 }
113
114 /**
115  Called by OLSR core when a packet for the plugin is received from the OLSR
116  network. It converts the packet into an NMEA string and transmits it over all
117  transmit non-OLSR network interfaces.
118
119  @param olsrMessage
120  a pointer to the received OLSR message
121  @param in_if
122  a pointer to the OLSR network interface on which the packet was received
123  @param ipaddr
124  a pointer to the IP address of the sender
125
126  @return
127  - true when the packet was processed
128  - false otherwise
129  */
130 bool packetReceivedFromOlsr(union olsr_message *olsrMessage,
131                 struct interface *in_if __attribute__ ((unused)), union olsr_ip_addr *ipaddr __attribute__ ((unused))) {
132         const union olsr_ip_addr * originator;
133         unsigned char *olsrMessagePayload;
134         unsigned int transmitStringLength;
135         unsigned char buffer[BUFFER_SIZE_FROM_OLSR];
136
137 #ifdef PUD_DUMP_GPS_PACKETS_RX_OLSR
138         unsigned int olsrMessageSize;
139 #endif
140
141         /* determine the originator of the messsage */
142         if (olsr_cnf->ip_version == AF_INET) {
143                 originator = (const union olsr_ip_addr *) &olsrMessage->v4.originator;
144                 olsrMessagePayload = (unsigned char *) &olsrMessage->v4.message;
145 #ifdef PUD_DUMP_GPS_PACKETS_RX_OLSR
146                 olsrMessageSize = ntohs(olsrMessage->v4.olsr_msgsize);
147 #endif
148         } else {
149                 originator = (const union olsr_ip_addr *) &olsrMessage->v6.originator;
150                 olsrMessagePayload = (unsigned char *) &olsrMessage->v6.message;
151 #ifdef PUD_DUMP_GPS_PACKETS_RX_OLSR
152                 olsrMessageSize = ntohs(olsrMessage->v6.olsr_msgsize);
153 #endif
154         }
155
156         /* when we do not loopback then check if the message originated from this
157          * node: back off */
158         if (!getUseLoopback() && ipequal(originator, &olsr_cnf->main_addr)) {
159                 return false;
160         }
161
162         /* do deduplication: when we have already seen this message from the same
163          * originator then just back off */
164         if (likely(getUseDeDup())) {
165                 if (isInDeDupList(&deDupList, olsrMessage)) {
166                         return false;
167                 }
168
169                 addToDeDup(&deDupList, olsrMessage);
170         }
171
172 #ifdef PUD_DUMP_GPS_PACKETS_RX_OLSR
173         olsr_printf(0, "\n%s: packet received from OLSR interface %s (%u bytes)\n",
174                         PUD_PLUGIN_ABBR, in_if->int_name, olsrMessageSize);
175         dump_packet((unsigned char *) olsrMessage, olsrMessageSize);
176 #endif
177
178         transmitStringLength = gpsFromOlsr(olsrMessage, olsrMessagePayload,
179                         &buffer[0], sizeof(buffer));
180         if (unlikely(transmitStringLength == 0)) {
181                 return false;
182         }
183
184         sendToAllTxInterfaces(&buffer[0], transmitStringLength);
185
186         return true;
187 }
188
189 /**
190  Called by OLSR core when a packet for the plugin is received from the non-OLSR
191  network. It converts the packet into the internal OLSR wire format for a
192  position update and transmits it over all OLSR network interfaces.
193
194  @param skfd
195  the socket file descriptor on which the packet is received
196  @param data
197  a pointer to the network interface structure on which the packet was received
198  @param flags
199  unused
200  */
201 #ifdef PUD_DUMP_GPS_PACKETS_RX_NON_OLSR
202 static void packetReceivedForOlsr(int skfd, void *data, unsigned int flags __attribute__ ((unused))) {
203 #else
204 static void packetReceivedForOlsr(int skfd, void *data __attribute__ ((unused)), unsigned int flags __attribute__ ((unused))) {
205 #endif
206         if (skfd >= 0) {
207                 unsigned char rxBuffer[BUFFER_SIZE_FOR_OLSR];
208                 ssize_t rxCount;
209                 struct sockaddr sender;
210                 socklen_t senderSize = sizeof(sender);
211
212                 assert(data != NULL);
213
214                 /* Receive the captured Ethernet frame */
215                 memset(&sender, 0, senderSize);
216                 errno = 0;
217                 rxCount = recvfrom(skfd, &rxBuffer[0], (sizeof(rxBuffer) - 1), 0,
218                                 &sender, &senderSize);
219                 if (rxCount < 0) {
220                         pudError(true, "Receive error in %s, ignoring message.", __func__);
221                         return;
222                 }
223
224                 /* make sure the string is null-terminated */
225                 rxBuffer[rxCount] = '\0';
226
227                 /* only accept messages from configured IP addresses */
228                 if (!isRxAllowedSourceIpAddress(&sender)) {
229                         return;
230                 }
231
232 #ifdef PUD_DUMP_GPS_PACKETS_RX_NON_OLSR
233                 {
234                         TRxTxNetworkInterface * networkInterface = data;
235                         void * src;
236                         in_port_t port;
237                         char fromAddr[64];
238
239                         if (olsr_cnf->ip_version == AF_INET) {
240                                 src = &((struct sockaddr_in*) &sender)->sin_addr;
241                                 port = ntohs(((struct sockaddr_in*) &sender)->sin_port);
242                         } else {
243                                 src = &((struct sockaddr_in6*) &sender)->sin6_addr;
244                                 port = ntohs(((struct sockaddr_in6*) &sender)->sin6_port);
245                         }
246
247                         inet_ntop(olsr_cnf->ip_version, src, &fromAddr[0], sizeof(fromAddr));
248                         olsr_printf(0, "\n%s: packet received from %s, port %u on non-OLSR"
249                                         " interface %s (%lu bytes)\n", PUD_PLUGIN_ABBR, &fromAddr[0],
250                                         port, &networkInterface->name[0], (size_t) rxCount);
251
252                         dump_packet(&rxBuffer[0], (size_t)rxCount);
253                 }
254 #endif
255
256                 /* we have the received string in the rxBuffer now */
257
258                 /* hand the NMEA information to the receiver */
259                 (void) receiverUpdateGpsInformation(&rxBuffer[0], rxCount);
260         }
261 }
262
263 /**
264  Initialise the plugin: check the configuration, initialise the NMEA parser,
265  create network interface sockets, hookup the plugin to OLSR and setup data
266  that can be setup in advance.
267
268  @return
269  - false upon failure
270  - true otherwise
271  */
272 bool initPud(void) {
273         if (!checkConfig()) {
274                 pudError(false, "Invalid configuration");
275                 goto error;
276         }
277
278         initDeDupList(&deDupList, getDeDupDepth());
279
280         /* set global transmit socket config */
281         txAddress = getTxMcAddr();
282
283         if (!startReceiver()) {
284                 pudError(false, "Could not start receiver");
285                 goto error;
286         }
287
288         /*
289          * Creates receive and transmit sockets and register the receive sockets
290          * with the OLSR stack
291          */
292         if (!createNetworkInterfaces(&packetReceivedForOlsr)) {
293                 pudError(false, "Could not create require network interfaces");
294                 goto error;
295         }
296
297         if (!checkRunSetup()) {
298                 pudError(false, "Invalid configuration");
299                 goto error;
300         }
301
302         /*
303          * Tell OLSR to launch olsr_parser when the packets for this plugin
304          * arrive from the OLSR network
305          */
306         olsr_parser_add_function(&packetReceivedFromOlsr, PUD_OLSR_MSG_TYPE);
307
308         return true;
309
310         error: closePud();
311         return false;
312 }
313
314 /**
315  Stop the plugin: shut down all created network interface sockets and destroy
316  the NMEA parser.
317  */
318 void closePud(void) {
319         closeNetworkInterfaces();
320         stopReceiver();
321         destroyDeDupList(&deDupList);
322 }