/**
Read an unsigned short number from a string
@param parameterName
The name of the parameter, used when reporting errors
@param str
The string to convert to a number
@param dst
A pointer to the location where to store the number upon successful conversion
Not touched when errors are reported.
@return
- true on success
- false otherwise
*/
bool readUS(const char * parameterName, const char * str, unsigned short * dst) {
char * endPtr = NULL;
unsigned long value;
assert(parameterName != NULL);
assert(str != NULL);
assert(dst != NULL);
errno = 0;
value = strtoul(str, &endPtr, 10);
if (!((endPtr != str) && (*str != '\0') && (*endPtr == '\0'))) {
/* invalid conversion */
pudError(false, "Value of parameter %s (%s) could not be converted to a number", parameterName, str);
return false;
}
if (value > 65535) {
pudError(false, "Value of parameter %s (%lu) is outside of valid range 0-65535", parameterName, value);
return false;
}
*dst = value;
return true;
}
/**
Get the IPv4 address of an interface
@param ifName
the name of the interface
@param ifr
the buffer in which to write the IPv4 address
@return
- the pointer to the IPv4 address (inside ifr)
- NULL on failure
*/
struct in_addr * getIPv4Address(const char * ifName, struct ifreq *ifr) {
int fd;
assert(ifName != NULL);
assert(strlen(ifName) <= sizeof(ifr->ifr_name));
assert(ifr != NULL);
fd = socket(AF_INET, SOCK_DGRAM, 0);
if (fd < 0) {
pudError(true, "%s@%u: socket error: %s", __FILE__, __LINE__, strerror(errno));
return NULL;
}
ifr->ifr_addr.sa_family = AF_INET;
memset(ifr->ifr_name, 0, sizeof(ifr->ifr_name));
strncpy(ifr->ifr_name, ifName, sizeof(ifr->ifr_name));
errno = 0;
if (ioctl(fd, SIOCGIFADDR, ifr) < 0) {
pudError(true, "%s@%u: ioctl(SIOCGIFADDR) error", __FILE__, __LINE__);
close(fd);
return NULL;
}
close(fd);
return &((struct sockaddr_in *)(void *) &ifr->ifr_addr)->sin_addr;
}
/**
Get the hardware address (MAC) of an interface
@param ifName
the name of the interface
@param family
the protocol family (AF_INET or AF_INET6)
@param ifr
the buffer in which to write the hardware address (MAC)
@return
- the pointer to the hardware address (inside ifr)
- NULL on failure
*/
unsigned char * getHardwareAddress(const char * ifName, int family,
struct ifreq *ifr) {
int fd;
assert(ifName != NULL);
assert(strlen(ifName) <= sizeof(ifr->ifr_name));
assert((family == AF_INET) || (family == AF_INET6));
assert(ifr != NULL);
fd = socket(family, SOCK_DGRAM, 0);
if (fd < 0) {
pudError(true, "%s@%u: socket error: %s", __FILE__, __LINE__, strerror(errno));
return NULL;
}
ifr->ifr_addr.sa_family = family;
memset(ifr->ifr_name, 0, sizeof(ifr->ifr_name));
strncpy(ifr->ifr_name, ifName, sizeof(ifr->ifr_name));
errno = 0;
if (ioctl(fd, SIOCGIFHWADDR, ifr) < 0) {
pudError(true, "%s@%u: ioctl(SIOCGIFHWADDR) error", __FILE__, __LINE__);
close(fd);
return NULL;
}
close(fd);
return (unsigned char *) &ifr->ifr_hwaddr.sa_data[0];
}
/**
Read a boolean from a string
@param parameterName
The name of the parameter, used when reporting errors
@param str
The string to convert to a boolean
@param dst
A pointer to the location where to store the boolean upon successful conversion
Not touched when errors are reported.
@return
- true on success
- false otherwise
*/
bool readBool(const char * parameterName, const char * str, bool * dst) {
bool retVal = true;
char * endPtr = NULL;
unsigned long value;
char * strDup = strdup(str);
char * c;
assert(parameterName != NULL);
assert(str != NULL);
assert(dst != NULL);
/* convert to lowercase */
for (c = strDup; *c != '\0'; c++) {
if (isalpha(*c) && !islower(*c)) {
*c = tolower(*c);
}
}
/* try true/yes/on */
if (!strcmp("true", strDup) || !strcmp("yes", strDup) || !strcmp("on", strDup)) {
*dst = true;
goto out;
}
/* try false/no/off */
if (!strcmp("false", strDup) || !strcmp("no", strDup) || !strcmp("off", strDup)) {
*dst = false;
goto out;
}
/* try a number */
errno = 0;
value = strtoul(str, &endPtr, 10);
if (!((endPtr != str) && (*str != '\0') && (*endPtr == '\0'))) {
/* invalid conversion */
pudError(false, "Value of parameter %s (%s) could not be converted to a number", parameterName, str);
retVal = false;
goto out;
}
if (value > 1) {
pudError(false, "Value of parameter %s (%lu) is outside of valid range 0-1", parameterName, value);
retVal = false;
goto out;
}
/* 0 = false, 1 = true */
*dst = (value == 1);
out: free(strDup);
return retVal;
}
/**
* @return
* the MAC address of the main IP address
*/
unsigned char * getMainIpMacAddress(void) {
if (!macSet) {
struct ifreq ifr;
unsigned char * macInIfr;
struct interface *mainInterface = if_ifwithaddr(&olsr_cnf->main_addr);
if (!mainInterface) {
pudError(true, "Could not get the main interface");
return NULL;
}
macInIfr = getHardwareAddress(mainInterface->int_name, olsr_cnf->ip_version, &ifr);
if (!macInIfr) {
pudError(true, "Could not get the MAC address of the main interface");
return NULL;
}
memcpy(&mac[0], &macInIfr[0], PUD_NODEIDTYPE_MAC_BYTES);
macSet = true;
}
return &mac[0];
}
/**
* Initialises the positionFile reader.
* @return true upon success, false otherwise
*/
bool startPositionFile(void) {
if (started) {
return true;
}
if (regcomp(®exComment, regexCommentString, REG_EXTENDED | REG_ICASE)) {
pudError(false, "Could not compile regex \"%s\"", regexCommentString);
return false;
}
if (regcomp(®exNameValue, regexNameValueString, REG_EXTENDED | REG_ICASE)) {
pudError(false, "Could not compile regex \"%s\"", regexNameValueString);
regfree(®exComment);
return false;
}
cachedStat.timeStamp = -1;
started = true;
return true;
}
static int setNodeIdType(const char *value, const char * valueName) {
unsigned long long nodeIdTypeNew;
if (!readULL(valueName, value, &nodeIdTypeNew, 10)) {
return true;
}
if (!isValidNodeIdType(nodeIdTypeNew)) {
pudError(false, "Value in parameter %s (%llu) is reserved", valueName,
nodeIdTypeNew);
return true;
}
nodeIdType = nodeIdTypeNew;
return false;
}
/**
* Performs a regex match
* @param regex the compiled regex to match against
* @param line the line to match
* @param nmatch the number of matches to produce
* @param pmatch the array with match information
* @return true upon success, false otherwise
*/
static bool regexMatch(regex_t * regex, char * line, size_t nmatch, regmatch_t pmatch[]) {
int result = regexec(regex, line, nmatch, pmatch, 0);
if (!result) {
return true;
}
if (result == REG_NOMATCH) {
return false;
}
{
char msgbuf[256];
regerror(result, regex, msgbuf, sizeof(msgbuf));
pudError(false, "Regex match failed: %s", msgbuf);
}
return false;
}
/**
Read a double number from a string
@param parameterName
The name of the parameter, used when reporting errors
@param str
The string to convert to a number
@param dst
A pointer to the location where to store the number upon successful conversion
Not touched when errors are reported.
@return
- true on success
- false otherwise
*/
bool readDouble(const char * parameterName, const char * str, double * dst) {
char * endPtr = NULL;
double value;
assert(parameterName != NULL);
assert(str != NULL);
assert(dst != NULL);
errno = 0;
value = strtod(str, &endPtr);
if (!((endPtr != str) && (*str != '\0') && (*endPtr == '\0'))) {
/* invalid conversion */
pudError(false, "Value of parameter %s (%s) could not be converted to a number", parameterName, str);
return false;
}
*dst = value;
return true;
}
/**
Read an (olsr_sockaddr) IP address from a string:
First tries to parse the value as an IPv4 address, and if not successful tries to parse it as an IPv6 address.
When the address wasn't set yet, the default port (portDefault) is set.
@param parameterName
The name of the parameter, used when reporting errors
@param str
The string to convert to an (olsr_sockaddr) IP address
@param portDefault
The default for the port (in host byte order, stored in network byte order)
@param dst
A pointer to the location where to store the (olsr_sockadd) IP address upon successful conversion.
Not touched when errors are reported.
@param dstSet
A pointer to the location where to store the flag that signals whether the IP address is set.
Not touched when errors are reported.
@return
- true on success
- false otherwise
*/
bool readIPAddress(const char * parameterName, const char * str, in_port_t portDefault,
union olsr_sockaddr * dst, bool * dstSet) {
union olsr_sockaddr ip;
int conversion;
assert(parameterName != NULL);
assert(str != NULL);
assert(dst != NULL);
assert(dstSet != NULL);
/* try IPv4 first */
memset(&ip, 0, sizeof(ip));
ip.in.sa_family = AF_INET;
conversion = inet_pton(ip.in.sa_family, str, &ip.in4.sin_addr);
/* now try IPv6 if IPv4 conversion was not successful */
if (conversion != 1) {
memset(&ip, 0, sizeof(ip));
ip.in.sa_family = AF_INET6;
conversion = inet_pton(ip.in.sa_family, str, &ip.in6.sin6_addr);
}
if (conversion != 1) {
pudError((conversion == -1) ? true : false,
"Value of parameter %s (%s) is not an IP address", parameterName, str);
return false;
}
if (!*dstSet) {
setOlsrSockaddrPort(&ip, htons(portDefault));
}
setOlsrSockaddrAddr(dst, &ip);
*dstSet = true;
return true;
}
/**
Sends a buffer out on all transmit interfaces
@param buffer
the buffer
@param bufferLength
the number of bytes in the buffer
*/
static void sendToAllTxInterfaces(unsigned char *buffer,
unsigned int bufferLength) {
union olsr_sockaddr * txAddress = getTxMcAddr();
void * addr;
socklen_t addrSize;
TRxTxNetworkInterface *txNetworkInterfaces = getTxNetworkInterfaces();
if (txAddress->in.sa_family == AF_INET) {
addr = &txAddress->in4;
addrSize = sizeof(struct sockaddr_in);
} else {
addr = &txAddress->in6;
addrSize = sizeof(struct sockaddr_in6);
}
while (txNetworkInterfaces != NULL) {
TRxTxNetworkInterface *networkInterface = txNetworkInterfaces;
errno = 0;
if (sendto(networkInterface->socketFd, buffer, bufferLength, 0, addr, addrSize) < 0) {
pudError(true, "Transmit error on interface %s", &networkInterface->name[0]);
}
txNetworkInterfaces = networkInterface->next;
}
}
/**
Create a transmit socket for a network interface
@param networkInterface
The network interface object. This function expects it to be filled with all
information, except for the socket descriptor.
@param txMcAddr
The transmit multicast address
@return
- the socket descriptor (>= 0)
- -1 if an error occurred
*/
static int createTxSocket(TRxTxNetworkInterface * networkInterface, union olsr_sockaddr * txMcAddr) {
int ipFamilySetting;
int ipProtoSetting;
int ipMcLoopSetting;
int ipMcIfSetting;
int ipTtlSetting;
unsigned int ifIndex;
union olsr_sockaddr address;
void * addr;
size_t addrSize;
int txSocket = -1;
int mcLoopValue = 1;
int txTtl = getTxTtl();
assert(networkInterface != NULL);
assert(strncmp((char *) &networkInterface->name[0], "",
sizeof(networkInterface->name)) != 0);
memset(&address, 0, sizeof(address));
if (txMcAddr->in.sa_family == AF_INET) {
struct ifreq ifr;
struct in_addr * ifAddr = getIPv4Address(networkInterface->name, &ifr);
if (!ifAddr) {
pudError(true, "Could not get interface address of %s", networkInterface->name);
goto bail;
}
assert(txMcAddr->in4.sin_addr.s_addr != INADDR_ANY);
ipFamilySetting = AF_INET;
ipProtoSetting = IPPROTO_IP;
ipMcLoopSetting = IP_MULTICAST_LOOP;
ipMcIfSetting = IP_MULTICAST_IF;
ipTtlSetting = IP_MULTICAST_TTL;
ifIndex = 0;
address.in4.sin_family = ipFamilySetting;
address.in4.sin_addr = *ifAddr;
address.in4.sin_port = getTxMcPort();
addr = &address.in4;
addrSize = sizeof(struct sockaddr_in);
} else {
assert(txMcAddr->in6.sin6_addr.s6_addr != in6addr_any.s6_addr);
ipFamilySetting = AF_INET6;
ipProtoSetting = IPPROTO_IPV6;
ipMcLoopSetting = IPV6_MULTICAST_LOOP;
ipMcIfSetting = IPV6_MULTICAST_IF;
ipTtlSetting = IPV6_MULTICAST_HOPS;
ifIndex = if_nametoindex(networkInterface->name);
addr = &ifIndex;
addrSize = sizeof(ifIndex);
}
/* Create a datagram socket on which to transmit */
errno = 0;
txSocket = socket(ipFamilySetting, SOCK_DGRAM, 0);
if (txSocket < 0) {
pudError(true, "Could not create a transmit socket for interface %s",
networkInterface->name);
goto bail;
}
/* Bind the socket to the desired interface */
errno = 0;
if (setsockopt(txSocket, ipProtoSetting, ipMcIfSetting, addr, addrSize) < 0) {
pudError(true, "Could not set the multicast interface on the"
" transmit socket to interface %s", networkInterface->name);
goto bail;
}
/* Disable multicast local loopback */
errno = 0;
if (setsockopt(txSocket, ipProtoSetting, ipMcLoopSetting, &mcLoopValue,
sizeof(mcLoopValue)) < 0) {
pudError(true, "Could not disable multicast loopback on the"
" transmit socket for interface %s", networkInterface->name);
goto bail;
}
/* Set the TTL on the socket */
errno = 0;
if (setsockopt(txSocket, ipProtoSetting, ipTtlSetting, &txTtl,
sizeof(txTtl)) < 0) {
pudError(true, "Could not set TTL on the transmit socket"
" for interface %s", networkInterface->name);
goto bail;
}
/* Set the no delay option on the socket */
errno = 0;
if (fcntl(txSocket, F_SETFL, O_NDELAY) < 0) {
pudError(true, "Could not set the no delay option on the"
" transmit socket for interface %s", networkInterface->name);
goto bail;
}
return txSocket;
bail: if (txSocket >= 0) {
close(txSocket);
}
return -1;
}
/**
Create an downlink socket
@param ipVersion
The IP version (AF_INET or AF_INET6) for the socket
@param rxSocketHandlerFunction
The socket handler function
@return
- the socket descriptor (>= 0)
- -1 if an error occurred
*/
static int createDownlinkSocket(int ipVersion, socket_handler_func rxSocketHandlerFunction) {
union olsr_sockaddr address;
void * addr;
size_t addrSize;
int downlinkSocket = -1;
int socketReuseFlagValue = 1;
memset(&address, 0, sizeof(address));
if (ipVersion == AF_INET) {
address.in4.sin_family = AF_INET;
address.in4.sin_addr.s_addr = INADDR_ANY;
address.in4.sin_port = getDownlinkPort();
addr = &address.in4;
addrSize = sizeof(struct sockaddr_in);
} else {
address.in6.sin6_family = AF_INET6;
address.in6.sin6_addr = in6addr_any;
address.in6.sin6_port = getDownlinkPort();
addr = &address.in6;
addrSize = sizeof(struct sockaddr_in6);
}
/* Create a datagram socket on which to receive */
errno = 0;
downlinkSocket = socket(ipVersion, SOCK_DGRAM, 0);
if (downlinkSocket < 0) {
pudError(true, "Could not create the downlink socket");
goto bail;
}
/* Enable SO_REUSEADDR to allow multiple applications to receive the same
* messages */
errno = 0;
if (setsockopt(downlinkSocket, SOL_SOCKET, SO_REUSEADDR, &socketReuseFlagValue,
sizeof(socketReuseFlagValue)) < 0) {
pudError(true, "Could not set REUSE option on the downlink socket");
goto bail;
}
/* Bind to the proper port number with the IP address INADDR_ANY
* (INADDR_ANY is really required here, do not change it) */
errno = 0;
if (bind(downlinkSocket, addr, addrSize)) {
pudError(true, "Could not bind downlink socket to port %d",
getDownlinkPort());
goto bail;
}
add_olsr_socket(downlinkSocket, rxSocketHandlerFunction, NULL, NULL,
SP_PR_READ);
downlinkHandler = rxSocketHandlerFunction;
return downlinkSocket;
bail: if (downlinkSocket >= 0) {
close(downlinkSocket);
}
return -1;
}
/**
Convert an OLSR message into a string to multicast on the LAN
@param olsrMessage
A pointer to the OLSR message
@param txGpsBuffer
A pointer to the buffer in which the transmit string can be written
@param txGpsBufferSize
The size of the txGpsBuffer
@return
- the length of the transmit string placed in the txGpsBuffer
- 0 (zero) in case of an error
*/
unsigned int gpsFromOlsr(union olsr_message *olsrMessage,
unsigned char * txGpsBuffer, unsigned int txGpsBufferSize) {
unsigned long validityTime;
struct tm timeStruct;
char latitudeString[PUD_TX_LATITUDE_DIGITS];
const char * latitudeHemisphere;
char longitudeString[PUD_TX_LONGITUDE_DIGITS];
const char * longitudeHemisphere;
char altitudeString[PUD_TX_ALTITUDE_DIGITS];
char speedString[PUD_TX_SPEED_DIGITS];
char trackString[PUD_TX_TRACK_DIGITS];
char hdopString[PUD_TX_HDOP_DIGITS];
uint32_t present;
char gateway[2] = { '0', '\0' };
char nodeIdTypeString[PUD_TX_NODEIDTYPE_DIGITS];
char nodeIdString[PUD_TX_NODEID_BUFFERSIZE];
const char * nodeId;
const void * ipAddr;
char originatorBuffer[64];
const char * originator;
unsigned int transmitStringLength;
PudOlsrPositionUpdate * olsrGpsMessage =
getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);
if (unlikely(getPositionUpdateVersion(olsrGpsMessage) != PUD_WIRE_FORMAT_VERSION)) {
/* currently we can only handle our own version */
pudError(false, "Can not handle version %u OLSR PUD messages"
" (only version %u): message ignored",
getPositionUpdateVersion(olsrGpsMessage), PUD_WIRE_FORMAT_VERSION);
return 0;
}
ipAddr = (olsr_cnf->ip_version == AF_INET) ?
(void *) &olsrMessage->v4.originator :
(void *) &olsrMessage->v6.originator;
originator = inet_ntop(olsr_cnf->ip_version, ipAddr, &originatorBuffer[0],
sizeof(originatorBuffer));
validityTime = getValidityTime(&olsrGpsMessage->validityTime);
present = getPositionUpdatePresent(olsrGpsMessage);
if (present & PUD_PRESENT_GATEWAY) {
gateway[0] = '1';
}
/* time is ALWAYS present so we can just use it */
getPositionUpdateTime(olsrGpsMessage, time(NULL), &timeStruct);
if (likely(nmea_INFO_is_present(present, LAT))) {
double latitude = getPositionUpdateLatitude(olsrGpsMessage);
if (latitude >= 0) {
latitudeHemisphere = "N";
} else {
latitudeHemisphere = "S";
latitude = -latitude;
}
latitude = nmea_degree2ndeg(latitude);
snprintf(&latitudeString[0], PUD_TX_LATITUDE_DIGITS, "%." PUD_TX_LATITUDE_DECIMALS "f", latitude);
} else {
latitudeHemisphere = "";
latitudeString[0] = '\0';
}
if (likely(nmea_INFO_is_present(present, LON))) {
double longitude = getPositionUpdateLongitude(olsrGpsMessage);
if (longitude >= 0) {
longitudeHemisphere = "E";
} else {
longitudeHemisphere = "W";
longitude = -longitude;
}
longitude = nmea_degree2ndeg(longitude);
snprintf(&longitudeString[0], PUD_TX_LONGITUDE_DIGITS, "%." PUD_TX_LONGITUDE_DECIMALS "f", longitude);
} else {
longitudeHemisphere = "";
longitudeString[0] = '\0';
}
if (likely(nmea_INFO_is_present(present, ELV))) {
snprintf(&altitudeString[0], PUD_TX_ALTITUDE_DIGITS, "%ld", getPositionUpdateAltitude(olsrGpsMessage));
} else {
altitudeString[0] = '\0';
}
if (likely(nmea_INFO_is_present(present, SPEED))) {
snprintf(&speedString[0], PUD_TX_SPEED_DIGITS, "%lu", getPositionUpdateSpeed(olsrGpsMessage));
} else {
speedString[0] = '\0';
}
if (likely(nmea_INFO_is_present(present, TRACK))) {
snprintf(&trackString[0], PUD_TX_TRACK_DIGITS, "%lu", getPositionUpdateTrack(olsrGpsMessage));
} else {
trackString[0] = '\0';
}
if (likely(nmea_INFO_is_present(present, HDOP))) {
snprintf(&hdopString[0], PUD_TX_HDOP_DIGITS, "%." PUD_TX_HDOP_DECIMALS "f",
nmea_meters2dop(getPositionUpdateHdop(olsrGpsMessage)));
} else {
hdopString[0] = '\0';
}
getNodeTypeStringFromOlsr(olsr_cnf->ip_version, olsrGpsMessage,
&nodeIdTypeString[0], sizeof(nodeIdTypeString));
getNodeIdStringFromOlsr(olsr_cnf->ip_version, olsrMessage, &nodeId,
&nodeIdString[0], sizeof(nodeIdString));
transmitStringLength = nmea_printf((char *) txGpsBuffer, txGpsBufferSize
- 1, "$P%s," /* prefix (always) */
"%u," /* sentence version (always) */
"%s," /* gateway flag (always) */
"%s," /* OLSR originator (always) */
"%s,%s," /* nodeIdType/nodeId (always) */
"%02u%02u%02u," /* date (always) */
"%02u%02u%02u," /* time (always) */
"%lu," /* validity time (always) */
"%s,%s," /* latitude (optional) */
"%s,%s," /* longitude (optional) */
"%s," /* altitude (optional) */
"%s," /* speed (optional) */
"%s," /* track (optional) */
"%s" /* hdop (optional) */
, getTxNmeaMessagePrefix(), PUD_TX_SENTENCE_VERSION, &gateway[0],
originator , &nodeIdTypeString[0],
nodeId, timeStruct.tm_mday, timeStruct.tm_mon + 1, (timeStruct.tm_year
% 100), timeStruct.tm_hour, timeStruct.tm_min,
timeStruct.tm_sec, validityTime, &latitudeString[0],
latitudeHemisphere, &longitudeString[0], longitudeHemisphere,
&altitudeString[0], &speedString[0], &trackString[0],
&hdopString[0]);
if (unlikely(transmitStringLength > (txGpsBufferSize - 1))) {
pudError(false, "String to transmit on non-OLSR is too large, need"
" at least %u bytes, skipped", transmitStringLength);
return 0;
}
if (unlikely(transmitStringLength == (txGpsBufferSize - 1))) {
txGpsBuffer[txGpsBufferSize - 1] = '\0';
} else {
txGpsBuffer[transmitStringLength] = '\0';
}
return transmitStringLength;
}
/**
Send the transmit buffer out over all designated interfaces, called as a
timer callback and also immediately on an external state change.
@param interfaces
a bitmap defining which interfaces to send over
*/
static void txToAllOlsrInterfaces(TimedTxInterface interfaces) {
/** txBuffer is used to concatenate the position update and cluster leader messages in */
unsigned char txBuffer[TX_BUFFER_SIZE_FOR_OLSR];
unsigned int txBufferBytesUsed = 0;
#define txBufferBytesFree (sizeof(txBuffer) - txBufferBytesUsed)
/*
* The first message in txBuffer is an OLSR position update.
*
* The position update is always present.
*
* The second message is the cluster leader message, but only when uplink
* was requested and correctly configured.
*/
UplinkMessage * pu_uplink = (UplinkMessage *) &txBuffer[0];
union olsr_message * pu = &pu_uplink->msg.olsrMessage;
unsigned int pu_size = 0;
union olsr_ip_addr gateway;
MovementState externalState;
nmeaINFO nmeaInfo;
externalState = getExternalState();
/* only fixup timestamp when the position is valid _and_ when the position was not updated */
if (positionValid(&transmitGpsInformation.txPosition) && !transmitGpsInformation.positionUpdated) {
nmea_time_now(&transmitGpsInformation.txPosition.nmeaInfo.utc, &transmitGpsInformation.txPosition.nmeaInfo.present);
}
nmeaInfo = transmitGpsInformation.txPosition.nmeaInfo;
transmitGpsInformation.positionUpdated = false;
gateway = transmitGpsInformation.txGateway;
/* convert nmeaINFO to wireformat olsr message */
txBufferBytesUsed += sizeof(UplinkHeader); /* keep before txBufferSpaceFree usage */
pu_size = gpsToOlsr(&nmeaInfo, pu, txBufferBytesFree,
((externalState == MOVEMENT_STATE_STATIONARY) ? getUpdateIntervalStationary() : getUpdateIntervalMoving()));
txBufferBytesUsed += pu_size;
/*
* push out to all OLSR interfaces
*/
if (((interfaces & TX_INTERFACE_OLSR) != 0) && getOlsrTtl() && (pu_size > 0)) {
int r;
struct interface *ifn;
for (ifn = ifnet; ifn; ifn = ifn->int_next) {
/* force the pending buffer out if there's not enough space for our message */
if ((int)pu_size > net_outbuffer_bytes_left(ifn)) {
net_output(ifn);
}
r = net_outbuffer_push(ifn, pu, pu_size);
if (r != (int) pu_size) {
pudError(
false,
"Could not send to OLSR interface %s: %s (size=%u, r=%d)",
ifn->int_name,
((r == -1) ? "no buffer was found" :
(r == 0) ? "there was not enough room in the buffer" : "unknown reason"), pu_size, r);
}
}
/* loopback to tx interface when so configured */
if (getUseLoopback()) {
(void) packetReceivedFromOlsr(pu, NULL, NULL);
}
}
/* push out over uplink when an uplink is configured */
if (((interfaces & TX_INTERFACE_UPLINK) != 0) && isUplinkAddrSet()) {
int fd = getDownlinkSocketFd();
if (fd != -1) {
union olsr_sockaddr * uplink_addr = getUplinkAddr();
void * addr;
socklen_t addrSize;
UplinkMessage * cl_uplink = (UplinkMessage *) &txBuffer[txBufferBytesUsed];
UplinkClusterLeader * cl = &cl_uplink->msg.clusterLeader;
union olsr_ip_addr * cl_originator = getClusterLeaderOriginator(olsr_cnf->ip_version, cl);
union olsr_ip_addr * cl_clusterLeader = getClusterLeaderClusterLeader(olsr_cnf->ip_version, cl);
unsigned int cl_size =
sizeof(UplinkClusterLeader) - sizeof(cl->leader)
+ ((olsr_cnf->ip_version == AF_INET) ? sizeof(cl->leader.v4) :
sizeof(cl->leader.v6));
unsigned long long uplinkUpdateInterval =
(externalState == MOVEMENT_STATE_STATIONARY) ? getUplinkUpdateIntervalStationary() : getUplinkUpdateIntervalMoving();
if (uplink_addr->in.sa_family == AF_INET) {
addr = &uplink_addr->in4;
addrSize = sizeof(struct sockaddr_in);
} else {
addr = &uplink_addr->in6;
addrSize = sizeof(struct sockaddr_in6);
}
/*
* position update message (pu)
*/
/* set header fields in position update uplink message and adjust
* the validity time to the uplink validity time */
if (pu_size > 0) {
PudOlsrPositionUpdate * pu_gpsMessage = getOlsrMessagePayload(olsr_cnf->ip_version, pu);
setUplinkMessageType(&pu_uplink->header, POSITION);
setUplinkMessageLength(&pu_uplink->header, pu_size);
setUplinkMessageIPv6(&pu_uplink->header, (olsr_cnf->ip_version != AF_INET));
setUplinkMessagePadding(&pu_uplink->header, 0);
/* fixup validity time */
setValidityTime(&pu_gpsMessage->validityTime, uplinkUpdateInterval);
}
/*
* cluster leader message (cl)
*/
/* set cl_uplink header fields */
setUplinkMessageType(&cl_uplink->header, CLUSTERLEADER);
setUplinkMessageLength(&cl_uplink->header, cl_size);
setUplinkMessageIPv6(&cl_uplink->header, (olsr_cnf->ip_version != AF_INET));
setUplinkMessagePadding(&cl_uplink->header, 0);
/* setup cl */
setClusterLeaderVersion(cl, PUD_WIRE_FORMAT_VERSION);
setValidityTime(&cl->validityTime, uplinkUpdateInterval);
/* really need 2 memcpy's here because of olsr_cnf->ipsize */
memcpy(cl_originator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
memcpy(cl_clusterLeader, &gateway, olsr_cnf->ipsize);
txBufferBytesUsed += sizeof(UplinkHeader);
txBufferBytesUsed += cl_size;
errno = 0;
if (sendto(fd, &txBuffer, txBufferBytesUsed, 0, addr, addrSize) < 0) {
pudError(true, "Could not send to uplink (size=%u)", txBufferBytesUsed);
}
}
}
}
/**
Create a receive socket for a network interface
@param networkInterface
The network interface object. This function expects it to be filled with all
information, except for the socket descriptor.
@param rxSocketHandlerFunction
The function that handles reception of data on the network interface
@param rxMcAddr
The receive multicast address
@return
- the socket descriptor (>= 0)
- -1 if an error occurred
*/
static int createRxSocket(TRxTxNetworkInterface * networkInterface,
socket_handler_func rxSocketHandlerFunction, union olsr_sockaddr * rxMcAddr) {
int ipFamilySetting;
int ipProtoSetting;
int ipMcLoopSetting;
int ipAddMembershipSetting;
union olsr_sockaddr address;
void * addr;
size_t addrSize;
int rxSocket = -1;
int socketReuseFlagValue = 1;
int mcLoopValue = 1;
assert(networkInterface != NULL);
assert(rxSocketHandlerFunction != NULL);
assert(strncmp((char *) &networkInterface->name[0], "",
sizeof(networkInterface->name)) != 0);
memset(&address, 0, sizeof(address));
if (rxMcAddr->in.sa_family == AF_INET) {
assert(rxMcAddr->in4.sin_addr.s_addr != INADDR_ANY);
ipFamilySetting = AF_INET;
ipProtoSetting = IPPROTO_IP;
ipMcLoopSetting = IP_MULTICAST_LOOP;
ipAddMembershipSetting = IP_ADD_MEMBERSHIP;
address.in4.sin_family = ipFamilySetting;
address.in4.sin_addr.s_addr = INADDR_ANY;
address.in4.sin_port = getRxMcPort();
addr = &address.in4;
addrSize = sizeof(struct sockaddr_in);
} else {
assert(rxMcAddr->in6.sin6_addr.s6_addr != in6addr_any.s6_addr);
ipFamilySetting = AF_INET6;
ipProtoSetting = IPPROTO_IPV6;
ipMcLoopSetting = IPV6_MULTICAST_LOOP;
ipAddMembershipSetting = IPV6_ADD_MEMBERSHIP;
address.in6.sin6_family = ipFamilySetting;
address.in6.sin6_addr = in6addr_any;
address.in6.sin6_port = getRxMcPort();
addr = &address.in6;
addrSize = sizeof(struct sockaddr_in6);
}
/* Create a datagram socket on which to receive. */
errno = 0;
rxSocket = socket(ipFamilySetting, SOCK_DGRAM, 0);
if (rxSocket < 0) {
pudError(true, "Could not create a receive socket for interface %s",
networkInterface->name);
goto bail;
}
/* Enable SO_REUSEADDR to allow multiple applications to receive the same
* multicast messages */
errno = 0;
if (setsockopt(rxSocket, SOL_SOCKET, SO_REUSEADDR, &socketReuseFlagValue,
sizeof(socketReuseFlagValue)) < 0) {
pudError(true, "Could not set the reuse flag on the receive socket for"
" interface %s", networkInterface->name);
goto bail;
}
/* Bind to the proper port number with the IP address INADDR_ANY
* (INADDR_ANY is really required here, do not change it) */
errno = 0;
if (bind(rxSocket, addr, addrSize) < 0) {
pudError(true, "Could not bind the receive socket for interface"
" %s to port %u", networkInterface->name, ntohs(getRxMcPort()));
goto bail;
}
/* Enable multicast local loopback */
errno = 0;
if (setsockopt(rxSocket, ipProtoSetting, ipMcLoopSetting, &mcLoopValue,
sizeof(mcLoopValue)) < 0) {
pudError(true, "Could not enable multicast loopback on the"
" receive socket for interface %s", networkInterface->name);
goto bail;
}
/* Join the multicast group on the local interface. Note that this
* ADD_MEMBERSHIP option must be called for each local interface over
* which the multicast datagrams are to be received. */
if (ipFamilySetting == AF_INET) {
struct ip_mreq mc_settings;
struct ifreq ifr;
struct in_addr * ifAddr = getIPv4Address(networkInterface->name, &ifr);
if (!ifAddr) {
pudError(true, "Could not get interface address of %s", networkInterface->name);
goto bail;
}
(void) memset(&mc_settings, 0, sizeof(mc_settings));
mc_settings.imr_multiaddr = rxMcAddr->in4.sin_addr;
mc_settings.imr_interface = *ifAddr;
errno = 0;
if (setsockopt(rxSocket, ipProtoSetting, ipAddMembershipSetting,
&mc_settings, sizeof(mc_settings)) < 0) {
pudError(true, "Could not subscribe interface %s to the configured"
" multicast group", networkInterface->name);
goto bail;
}
} else {
struct ipv6_mreq mc6_settings;
(void) memset(&mc6_settings, 0, sizeof(mc6_settings));
mc6_settings.ipv6mr_multiaddr = rxMcAddr->in6.sin6_addr;
mc6_settings.ipv6mr_interface = if_nametoindex(networkInterface->name);
errno = 0;
if (setsockopt(rxSocket, ipProtoSetting, ipAddMembershipSetting,
&mc6_settings, sizeof(mc6_settings)) < 0) {
pudError(true, "Could not subscribe interface %s to the configured"
" multicast group", networkInterface->name);
goto bail;
}
}
add_olsr_socket(rxSocket, rxSocketHandlerFunction, NULL, networkInterface,
SP_PR_READ);
return rxSocket;
bail: if (rxSocket >= 0) {
close(rxSocket);
}
return -1;
}
/**
* Read the position file
* @param fileName the filename
* @param nmeaInfo the NMEA data
*/
bool readPositionFile(char * fileName, nmeaINFO * nmeaInfo) {
bool retval = false;
struct stat statBuf;
nmeaINFO result;
FILE * fd = NULL;
unsigned int lineNumber = 0;
char * name = NULL;
char * value = NULL;
if (stat(fileName, &statBuf)) {
/* could not access the file */
goto out;
}
if (!memcmp(&cachedStat.timeStamp, &statBuf.st_mtime, sizeof(cachedStat.timeStamp))) {
/* file did not change since last read */
goto out;
}
fd = fopen(fileName, "r");
if (!fd) {
goto out;
}
nmea_zero_INFO(&result);
result.sig = POSFILE_DEFAULT_SIG;
result.fix = POSFILE_DEFAULT_FIX;
result.HDOP = POSFILE_DEFAULT_HDOP;
result.VDOP = POSFILE_CALCULATED_VDOP(result.HDOP);
result.PDOP = POSFILE_CALCULATED_PDOP(result.HDOP);
result.lat = POSFILE_DEFAULT_LAT;
result.lon = POSFILE_DEFAULT_LON;
result.elv = POSFILE_DEFAULT_ELV;
result.speed = POSFILE_DEFAULT_SPEED;
result.track = POSFILE_DEFAULT_TRACK;
result.mtrack = POSFILE_DEFAULT_MTRACK;
result.magvar = POSFILE_DEFAULT_MAGVAR;
memcpy(&cachedStat.timeStamp, &statBuf.st_mtime, sizeof(cachedStat.timeStamp));
while (fgets(line, LINE_LENGTH, fd)) {
regmatch_t pmatch[regexNameValuematchCount];
lineNumber++;
if (regexMatch(®exComment, line, 0, NULL)) {
continue;
}
if (!regexMatch(®exNameValue, line, regexNameValuematchCount, pmatch)) {
pudError(false, "Position file \"%s\", line %d uses invalid syntax: %s", fileName, lineNumber, line);
goto out;
}
/* determine name/value */
name = &line[pmatch[1].rm_so];
line[pmatch[1].rm_eo] = '\0';
value = &line[pmatch[2].rm_so];
line[pmatch[2].rm_eo] = '\0';
if (!strncasecmp(POSFILE_NAME_SIG, name, sizeof(line))) {
if (!strncasecmp(POSFILE_VALUE_SIG_BAD, value, sizeof(line))) {
result.sig = NMEA_SIG_BAD;
} else if (!strncasecmp(POSFILE_VALUE_SIG_LOW, value, sizeof(line))) {
result.sig = NMEA_SIG_LOW;
} else if (!strncasecmp(POSFILE_VALUE_SIG_MID, value, sizeof(line))) {
result.sig = NMEA_SIG_MID;
} else if (!strncasecmp(POSFILE_VALUE_SIG_HIGH, value, sizeof(line))) {
result.sig = NMEA_SIG_HIGH;
} else {
pudError(false, "Position file \"%s\", line %d uses an invalid value for \"%s\","
" valid values are [%s|%s\%s|%s]", fileName, lineNumber, POSFILE_NAME_SIG,
POSFILE_VALUE_SIG_BAD, POSFILE_VALUE_SIG_LOW, POSFILE_VALUE_SIG_MID, POSFILE_VALUE_SIG_HIGH);
goto out;
}
nmea_INFO_set_present(&result.present, SIG);
} else if (!strncasecmp(POSFILE_NAME_FIX, name, sizeof(line))) {
if (!strncasecmp(POSFILE_VALUE_FIX_BAD, value, sizeof(line))) {
result.fix = NMEA_FIX_BAD;
} else if (!strncasecmp(POSFILE_VALUE_FIX_2D, value, sizeof(line))) {
result.fix = NMEA_FIX_2D;
} else if (!strncasecmp(POSFILE_VALUE_FIX_3D, value, sizeof(line))) {
result.fix = NMEA_FIX_3D;
} else {
pudError(false, "Position file \"%s\", line %d uses an invalid value for \"%s\","
" valid values are [%s\%s|%s]", fileName, lineNumber, POSFILE_NAME_FIX, POSFILE_VALUE_FIX_BAD,
POSFILE_VALUE_FIX_2D, POSFILE_VALUE_FIX_3D);
goto out;
}
nmea_INFO_set_present(&result.present, FIX);
} else if (!strncasecmp(POSFILE_NAME_HDOP, name, sizeof(line))) {
