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