lib/pud/src/pud.c

   1 #include "pud.h"
   2
   3 /* Plugin includes */
   4 #include "dedup.h"
   5 #include "networkInterfaces.h"
   6 #include "configuration.h"
   7 #include "gpsConversion.h"
   8 #include "receiver.h"
   9 #include "state.h"
  10 #include "compiler.h"
  11
  12 /* OLSRD includes */
  13 #include "olsr.h"
  14 #include "ipcalc.h"
  15 #include "net_olsr.h"
  16 #include "parser.h"
  17
  18 /* System includes */
  19
  20 /** The size of the buffer in which the received NMEA string is stored */
  21 #define BUFFER_SIZE_RX_NMEA             2048
  22
  23 /** The size of the buffer in which the received downlink message is stored */
  24 #define BUFFER_SIZE_RX_DOWNLINK 2048
  25
  26 /** The size of the buffer in which the converted NMEA string is assembled for
  27  * transmission over OSLR */
  28 #define BUFFER_SIZE_TX_OLSR     512
  29
  30 /** The de-duplication list */
  31 static DeDupList deDupList;
  32
  33 /**
  34  Report a plugin error.
  35
  36  @param useErrno
  37  when true then errno is used in the error message; the error reason is also
  38  reported.
  39  @param format
  40  a pointer to the format string
  41  @param ...
  42  arguments to the format string
  43  */
  44 void pudError(bool useErrno, const char *format, ...) {
  45         char strDesc[256];
  46         char *stringErr = NULL;
  47
  48         if (useErrno) {
  49                 stringErr = strerror(errno);
  50         }
  51
  52         if ((format == NULL) || (*format == '\0')) {
  53                 if (useErrno) {
  54                         olsr_printf(0, "%s: %s\n", PUD_PLUGIN_ABBR, stringErr);
  55                 } else {
  56                         olsr_printf(0, "%s: Unknown error\n", PUD_PLUGIN_ABBR);
  57                 }
  58         } else {
  59                 va_list arglist;
  60
  61                 va_start(arglist, format);
  62                 vsnprintf(strDesc, sizeof(strDesc), format, arglist);
  63                 va_end(arglist);
  64
  65                 strDesc[sizeof(strDesc) - 1] = '\0'; /* Ensures null termination */
  66
  67                 if (useErrno) {
  68                         olsr_printf(0, "%s: %s: %s\n", PUD_PLUGIN_ABBR, strDesc, stringErr);
  69                 } else {
  70                         olsr_printf(0, "%s: %s\n", PUD_PLUGIN_ABBR, strDesc);
  71                 }
  72         }
  73 }
  74
  75 /**
  76  Sends a buffer out on all transmit interfaces
  77
  78  @param buffer
  79  the buffer
  80  @param bufferLength
  81  the number of bytes in the buffer
  82  */
  83 static void sendToAllTxInterfaces(unsigned char *buffer,
  84                 unsigned int bufferLength) {
  85         union olsr_sockaddr * txAddress;
  86         TRxTxNetworkInterface *txNetworkInterfaces = getTxNetworkInterfaces();
  87         while (txNetworkInterfaces != NULL) {
  88                 TRxTxNetworkInterface *networkInterface = txNetworkInterfaces;
  89                 errno = 0;
  90                 txAddress = getTxMcAddr();
  91                 if (sendto(networkInterface->socketFd, buffer, bufferLength, 0,
  92                                 (struct sockaddr *) &txAddress->in, sizeof(txAddress->in)) < 0) {
  93                         pudError(true, "Transmit error on interface %s",
  94                                         (char *) &networkInterface->name);
  95                 }
  96                 txNetworkInterfaces = networkInterface->next;
  97         }
  98 }
  99
 100 /**
 101  Called by OLSR core when a packet for the plugin is received from the OLSR
 102  network. It converts the packet into an NMEA string and transmits it over all
 103  transmit non-OLSR network interfaces.
 104
 105  @param olsrMessage
 106  a pointer to the received OLSR message
 107  @param in_if
 108  a pointer to the OLSR network interface on which the packet was received
 109  @param ipaddr
 110  a pointer to the IP address of the sender
 111
 112  @return
 113  - true when the packet was processed
 114  - false otherwise
 115  */
 116 bool packetReceivedFromOlsr(union olsr_message *olsrMessage,
 117                 struct interface *in_if __attribute__ ((unused)), union olsr_ip_addr *ipaddr __attribute__ ((unused))) {
 118         const union olsr_ip_addr * originator = getOlsrMessageOriginator(
 119                         olsr_cnf->ip_version, olsrMessage);
 120         unsigned int transmitStringLength;
 121         unsigned char buffer[BUFFER_SIZE_TX_OLSR];
 122
 123         /* when we do not loopback then check if the message originated from this
 124          * node: back off */
 125         if (!getUseLoopback() && ipequal(originator, &olsr_cnf->main_addr)) {
 126                 return false;
 127         }
 128
 129         /* do deduplication: when we have already seen this message from the same
 130          * originator then just back off */
 131         if (likely(getUseDeDup())) {
 132                 if (isInDeDupList(&deDupList, olsrMessage)) {
 133                         return false;
 134                 }
 135
 136                 addToDeDup(&deDupList, olsrMessage);
 137         }
 138
 139         transmitStringLength = gpsFromOlsr(olsrMessage, &buffer[0], sizeof(buffer));
 140         if (unlikely(transmitStringLength == 0)) {
 141                 return false;
 142         }
 143
 144         sendToAllTxInterfaces(&buffer[0], transmitStringLength);
 145
 146         return true;
 147 }
 148
 149 /**
 150  Called by OLSR core when a packet for the plugin is received from the downlink.
 151  It unpacks the messages and distributes them into OLSR and on the LAN.
 152
 153  @param skfd
 154  the socket file descriptor on which the packet is received
 155  @param data
 156  a pointer to the network interface structure on which the packet was received
 157  @param flags
 158  unused
 159  */
 160 static void packetReceivedFromDownlink(int skfd, void *data __attribute__ ((unused)), unsigned int flags __attribute__ ((unused))) {
 161         if (skfd >= 0) {
 162                 unsigned char rxBuffer[BUFFER_SIZE_RX_DOWNLINK];
 163                 ssize_t rxCount;
 164                 ssize_t rxIndex = 0;
 165                 struct sockaddr sender;
 166                 socklen_t senderSize = sizeof(sender);
 167                 bool addedToDedup = false;
 168
 169                 /* Receive the captured Ethernet frame */
 170                 memset(&sender, 0, senderSize);
 171                 errno = 0;
 172                 rxCount = recvfrom(skfd, &rxBuffer[0], (sizeof(rxBuffer) - 1), 0,
 173                                 &sender, &senderSize);
 174                 if (rxCount < 0) {
 175                         pudError(true, "Receive error in %s, ignoring message.", __func__);
 176                         return;
 177                 }
 178
 179                 while (rxIndex < rxCount) {
 180                         UplinkMessage * msg = (UplinkMessage *) &rxBuffer[rxIndex];
 181                         uint8_t type;
 182                         uint16_t uplinkMessageLength;
 183                         uint16_t olsrMessageLength;
 184                         bool ipv6;
 185                         union olsr_message * olsrMessage;
 186
 187                         type = getUplinkMessageType(&msg->header);
 188                         if (type != POSITION) {
 189                                 pudError(false, "Received wrong type (%d) in %s,"
 190                                                 " ignoring message.", type, __func__);
 191                                 continue;
 192                         }
 193
 194                         ipv6 = getUplinkMessageIPv6(&msg->header);
 195                         if (unlikely(ipv6 && (olsr_cnf->ip_version != AF_INET6))) {
 196                                 pudError(false, "Received wrong IPv6 status (%s) in %s,"
 197                                                 " ignoring message.", (ipv6 ? "true" : "false"),
 198                                                 __func__);
 199                                 continue;
 200                         }
 201
 202                         olsrMessageLength = getUplinkMessageLength(&msg->header);
 203                         uplinkMessageLength = olsrMessageLength + sizeof(UplinkHeader);
 204
 205                         if (unlikely((rxIndex + uplinkMessageLength) > rxCount)) {
 206                                 pudError(false, "Received wrong length (%d) in %s,"
 207                                                 " ignoring the rest of the messages.", olsrMessageLength,
 208                                                 __func__);
 209                                 break;
 210                         }
 211
 212                         olsrMessage = &msg->msg.olsrMessage;
 213
 214                         /* we now have a position update (olsrMessage) of a certain length
 215                          * (length). this needs to be transmitted over OLSR and on the LAN */
 216
 217                         if (likely(getUseDeDup()) && !isInDeDupList(&deDupList, olsrMessage)) {
 218                                 addToDeDup(&deDupList, olsrMessage);
 219                                 addedToDedup = true;
 220                         }
 221
 222                         if (likely(addedToDedup) || unlikely(!getUseDeDup())) {
 223                                 /* send out over OLSR interfaces */
 224                                 int r;
 225                                 struct interface *ifn;
 226                                 for (ifn = ifnet; ifn; ifn = ifn->int_next) {
 227                                         r = net_outbuffer_push(ifn, olsrMessage, olsrMessageLength);
 228                                         if (r != (int) olsrMessageLength) {
 229                                                 pudError(
 230                                                                 false,
 231                                                                 "Could not send to OLSR interface %s: %s"
 232                                                                                 " (length=%u, r=%d)",
 233                                                                 ifn->int_name,
 234                                                                 ((r == -1) ? "no buffer was found" :
 235                                                                         (r == 0) ? "there was not enough room in the buffer" :
 236                                                                                         "unknown reason"), olsrMessageLength, r);
 237                                         }
 238                                 }
 239
 240                                 /* send out over tx interfaces */
 241                                 (void) packetReceivedFromOlsr(olsrMessage, NULL, NULL);
 242                         }
 243                         rxIndex += uplinkMessageLength;
 244                 }
 245         }
 246 }
 247
 248 /**
 249  Called by OLSR core when a packet for the plugin is received from the non-OLSR
 250  network. It converts the packet into the internal OLSR wire format for a
 251  position update and transmits it over all OLSR network interfaces.
 252
 253  @param skfd
 254  the socket file descriptor on which the packet is received
 255  @param data
 256  a pointer to the network interface structure on which the packet was received
 257  @param flags
 258  unused
 259  */
 260 static void packetReceivedForOlsr(int skfd, void *data __attribute__ ((unused)), unsigned int flags __attribute__ ((unused))) {
 261         if (skfd >= 0) {
 262                 unsigned char rxBuffer[BUFFER_SIZE_RX_NMEA];
 263                 ssize_t rxCount;
 264                 struct sockaddr sender;
 265                 socklen_t senderSize = sizeof(sender);
 266
 267                 assert(data != NULL);
 268
 269                 /* Receive the captured Ethernet frame */
 270                 memset(&sender, 0, senderSize);
 271                 errno = 0;
 272                 rxCount = recvfrom(skfd, &rxBuffer[0], (sizeof(rxBuffer) - 1), 0,
 273                                 &sender, &senderSize);
 274                 if (rxCount < 0) {
 275                         pudError(true, "Receive error in %s, ignoring message.", __func__);
 276                         return;
 277                 }
 278
 279                 /* make sure the string is null-terminated */
 280                 rxBuffer[rxCount] = '\0';
 281
 282                 /* only accept messages from configured IP addresses */
 283                 if (!isRxAllowedSourceIpAddress(&sender)) {
 284                         return;
 285                 }
 286
 287                 /* we have the received string in the rxBuffer now */
 288
 289                 /* hand the NMEA information to the receiver */
 290                 (void) receiverUpdateGpsInformation(&rxBuffer[0], rxCount);
 291         }
 292 }
 293
 294 /**
 295  Initialise the plugin: check the configuration, initialise the NMEA parser,
 296  create network interface sockets, hookup the plugin to OLSR and setup data
 297  that can be setup in advance.
 298
 299  @return
 300  - false upon failure
 301  - true otherwise
 302  */
 303 bool initPud(void) {
 304         if (!checkConfig()) {
 305                 pudError(false, "Invalid configuration");
 306                 goto error;
 307         }
 308
 309         if (!initState()) {
 310                 pudError(false, "Could not initialise state");
 311                 goto error;
 312         }
 313
 314         if (!initDeDupList(&deDupList, getDeDupDepth())) {
 315                 pudError(false, "Could not initialise de-duplication list");
 316                 goto error;
 317         }
 318
 319         if (!startReceiver()) {
 320                 pudError(false, "Could not start receiver");
 321                 goto error;
 322         }
 323
 324         /*
 325          * Creates receive and transmit sockets and register the receive sockets
 326          * with the OLSR stack
 327          */
 328         if (!createNetworkInterfaces(&packetReceivedForOlsr,
 329                         &packetReceivedFromDownlink)) {
 330                 pudError(false, "Could not create require network interfaces");
 331                 goto error;
 332         }
 333
 334         if (!checkRunSetup()) {
 335                 pudError(false, "Invalid configuration");
 336                 goto error;
 337         }
 338
 339         /*
 340          * Tell OLSR to call packetReceivedFromOlsr when the packets for this
 341          * plugin arrive from the OLSR network
 342          */
 343         olsr_parser_add_function(&packetReceivedFromOlsr, PUD_OLSR_MSG_TYPE);
 344
 345         return true;
 346
 347         error: closePud();
 348         return false;
 349 }
 350
 351 /**
 352  Stop the plugin: shut down all created network interface sockets and destroy
 353  the NMEA parser.
 354  */
 355 void closePud(void) {
 356         olsr_parser_remove_function(&packetReceivedFromOlsr, PUD_OLSR_MSG_TYPE);
 357         closeNetworkInterfaces();
 358         stopReceiver();
 359         destroyDeDupList(&deDupList);
 360         destroyState();
 361 }