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