594 lines
14 KiB
C
594 lines
14 KiB
C
/*
|
|
* This file is part of the Owl Positioning System (OwlPS) project.
|
|
* It is subject to the copyright notice and license terms in the
|
|
* COPYRIGHT.t2t file found in the top-level directory of this
|
|
* distribution and at
|
|
* http://code.lm7.fr/p/owlps/source/tree/master/COPYRIGHT.t2t
|
|
* No part of the OwlPS Project, including this file, may be copied,
|
|
* modified, propagated, or distributed except according to the terms
|
|
* contained in the COPYRIGHT.t2t file; the COPYRIGHT.t2t file must be
|
|
* distributed along with this file, either separately or by replacing
|
|
* this notice by the COPYRIGHT.t2t file's contents.
|
|
*
|
|
***********************************************************************
|
|
*
|
|
* This is the main source file of libowlps.
|
|
*/
|
|
|
|
|
|
#include "owlps.h"
|
|
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <unistd.h>
|
|
#include <inttypes.h>
|
|
#include <string.h>
|
|
#include <time.h>
|
|
#include <sys/time.h>
|
|
#include <signal.h>
|
|
|
|
#include <arpa/inet.h>
|
|
|
|
#include <assert.h>
|
|
|
|
|
|
|
|
bool owl_run = true ;
|
|
|
|
|
|
|
|
/* *** Miscellaneous functions *** */
|
|
|
|
|
|
/**
|
|
* Converts a MAC address from bytes to character string.
|
|
* The string is allocated in a static buffer, and will be overwritten
|
|
* each time this function is called.
|
|
* **This function is not thread-safe!**
|
|
*
|
|
* @returns A pointer to the static buffer containing the character
|
|
* string. This pointer must *not* be freed manually by the caller.
|
|
*/
|
|
const char* owl_mac_bytes_to_string(const uint8_t *const mac_binary)
|
|
{
|
|
static char mac_str[OWL_ETHER_ADDR_STRLEN] ;
|
|
owl_mac_bytes_to_string_r(mac_binary, mac_str) ;
|
|
return mac_str ;
|
|
}
|
|
|
|
|
|
/**
|
|
* Converts a MAC address from bytes to character string.
|
|
* The `mac_str` buffer must be allocated by the caller and its length
|
|
* must be of at least `OWL_ETHER_ADDR_STRLEN`.
|
|
* This function is thread-safe.
|
|
*/
|
|
void owl_mac_bytes_to_string_r(const uint8_t *const mac_binary,
|
|
char mac_str[OWL_ETHER_ADDR_STRLEN])
|
|
{
|
|
snprintf(mac_str, OWL_ETHER_ADDR_STRLEN,
|
|
"%02x:%02x:%02x:%02x:%02x:%02x",
|
|
mac_binary[0], mac_binary[1], mac_binary[2],
|
|
mac_binary[3], mac_binary[4], mac_binary[5]) ;
|
|
}
|
|
|
|
|
|
/**
|
|
* Compares two MAC addresses.
|
|
*
|
|
* @returns `true` if the two addresses are identical, `false` otherwise.
|
|
*/
|
|
bool owl_mac_equals(const uint8_t *const mac1,
|
|
const uint8_t *const mac2)
|
|
{
|
|
int i ;
|
|
for (i = ETHER_ADDR_LEN - 1 ; i >= 0 ; --i)
|
|
if(mac1[i] != mac2[i])
|
|
return false ;
|
|
return true ;
|
|
}
|
|
|
|
|
|
/**
|
|
* Converts a IEEE 802.11 frequency (in MHz) into a channel number.
|
|
*
|
|
* @returns The channel number corresponding to `frequency`.
|
|
* @returns 0 if the frequency does not correspond to an official
|
|
* channel.
|
|
*/
|
|
uint_fast8_t owl_frequency_to_channel(const uint_fast16_t frequency)
|
|
{
|
|
switch (frequency)
|
|
{
|
|
case OWL_80211_MHZ_CHANNEL_1 :
|
|
return 1 ;
|
|
case OWL_80211_MHZ_CHANNEL_2 :
|
|
return 2 ;
|
|
case OWL_80211_MHZ_CHANNEL_3 :
|
|
return 3 ;
|
|
case OWL_80211_MHZ_CHANNEL_4 :
|
|
return 4 ;
|
|
case OWL_80211_MHZ_CHANNEL_5 :
|
|
return 5 ;
|
|
case OWL_80211_MHZ_CHANNEL_6 :
|
|
return 6 ;
|
|
case OWL_80211_MHZ_CHANNEL_7 :
|
|
return 7 ;
|
|
case OWL_80211_MHZ_CHANNEL_8 :
|
|
return 8 ;
|
|
case OWL_80211_MHZ_CHANNEL_9 :
|
|
return 9 ;
|
|
case OWL_80211_MHZ_CHANNEL_10 :
|
|
return 10 ;
|
|
case OWL_80211_MHZ_CHANNEL_11 :
|
|
return 11 ;
|
|
case OWL_80211_MHZ_CHANNEL_12 :
|
|
return 12 ;
|
|
case OWL_80211_MHZ_CHANNEL_13 :
|
|
return 13 ;
|
|
case OWL_80211_MHZ_CHANNEL_14 :
|
|
return 14 ;
|
|
}
|
|
return 0 ;
|
|
}
|
|
|
|
|
|
|
|
/* *** Time *** */
|
|
|
|
|
|
/**
|
|
* Sleeps for a given amount of milliseconds.
|
|
* `time_ms` is an unsigned value, so please be careful: passing a
|
|
* negative value may not do what you think.
|
|
* In case of error, a message is displayed on the standard error.
|
|
*
|
|
* @returns 0 if everything went well.
|
|
* @returns A non-zero error code in case of error; if positive, it is
|
|
* the number of non-slept seconds.
|
|
*/
|
|
int owl_msleep(uint32_t time_ms)
|
|
{
|
|
int ret ;
|
|
uint_fast32_t seconds, microseconds ;
|
|
|
|
if (! time_ms)
|
|
return 0 ;
|
|
|
|
seconds = time_ms / 1000 ;
|
|
microseconds = time_ms % 1000 * 1000 ;
|
|
|
|
if ((ret = sleep(seconds)))
|
|
{
|
|
perror("Cannot sleep()") ;
|
|
return ret ;
|
|
}
|
|
|
|
if ((ret = usleep(microseconds)))
|
|
{
|
|
perror("Cannot usleep()") ;
|
|
return ret ;
|
|
}
|
|
|
|
return 0 ;
|
|
}
|
|
|
|
|
|
/**
|
|
* Sets the `owl_timestamp` `now` to the current time.
|
|
*
|
|
* @returns 0 in case of success, non-zero otherwise.
|
|
*/
|
|
int owl_timestamp_now(owl_timestamp *const now)
|
|
{
|
|
int ret ;
|
|
|
|
#if _POSIX_TIMERS > 0
|
|
struct timespec now_ts ;
|
|
ret = clock_gettime(CLOCK_REALTIME, &now_ts) ;
|
|
#else // _POSIX_TIMERS
|
|
struct timeval now_ts ;
|
|
ret = gettimeofday(&now_ts, NULL) ;
|
|
#endif // _POSIX_TIMERS
|
|
|
|
if (ret)
|
|
{
|
|
perror("Cannot get the current time") ;
|
|
return ret ;
|
|
}
|
|
|
|
#if _POSIX_TIMERS > 0
|
|
owl_timespec_to_timestamp(&now_ts, now) ;
|
|
#else // _POSIX_TIMERS
|
|
owl_timeval_to_timestamp(&now_ts, now) ;
|
|
#endif // _POSIX_TIMERS
|
|
|
|
return 0 ;
|
|
}
|
|
|
|
|
|
/**
|
|
* Converts the `struct timespec` `src` into the `owl_timestamp` `dst`.
|
|
* `src` and `dst` must not be `NULL`.
|
|
*/
|
|
void owl_timespec_to_timestamp(const struct timespec *const src,
|
|
owl_timestamp *const dst)
|
|
{
|
|
assert(src) ;
|
|
assert(dst) ;
|
|
dst->tv_sec = src->tv_sec ;
|
|
dst->tv_nsec = src->tv_nsec ;
|
|
}
|
|
|
|
|
|
/**
|
|
* Converts the `struct timeval` `src` into the `owl_timestamp` `dst`.
|
|
* `src` and `dst` must not be `NULL`.
|
|
*/
|
|
void owl_timeval_to_timestamp(const struct timeval *const src,
|
|
owl_timestamp *const dst)
|
|
{
|
|
assert(src) ;
|
|
assert(dst) ;
|
|
dst->tv_sec = src->tv_sec ;
|
|
dst->tv_nsec = src->tv_usec * 1000u ;
|
|
}
|
|
|
|
|
|
/**
|
|
* Compares two `owl_timestamp` and returns `true` if they are equal,
|
|
* `false` otherwise.
|
|
* `d1` and `d2` must not be `NULL`.
|
|
*/
|
|
bool owl_timestamp_equals(const owl_timestamp *const d1,
|
|
const owl_timestamp *const d2)
|
|
{
|
|
assert(d1) ;
|
|
assert(d2) ;
|
|
return d1->tv_sec == d2->tv_sec && d1->tv_nsec == d2->tv_nsec ;
|
|
}
|
|
|
|
|
|
/**
|
|
* Converts the `owl_timestamp` date value `d` into milliseconds.
|
|
* `d` must not be `NULL`.
|
|
*
|
|
* @returns The millisecond value of `d`.
|
|
*/
|
|
uint64_t owl_timestamp_to_ms(const owl_timestamp *const d)
|
|
{
|
|
assert(d) ;
|
|
return
|
|
(uint64_t) d->tv_sec * 1000u + (uint64_t) d->tv_nsec / 1000000lu ;
|
|
}
|
|
|
|
|
|
/**
|
|
* Converts the `owl_timestamp` date value `src` into a printable string.
|
|
* `src` must not be `NULL`.
|
|
* `dst` must be an allocated array of at least `OWL_TIMESTAMP_STRLEN`
|
|
* characters.
|
|
*/
|
|
void owl_timestamp_to_string(const owl_timestamp *const src,
|
|
char *const dst)
|
|
{
|
|
assert(src) ;
|
|
assert(dst) ;
|
|
snprintf(dst, OWL_TIMESTAMP_STRLEN, "%"PRIu32".%09"PRIu32,
|
|
src->tv_sec, src->tv_nsec) ;
|
|
}
|
|
|
|
|
|
/**
|
|
* Returns the time (in milliseconds) elapsed between two `owl_timestamp`
|
|
* `d1` and `d2`. `d1` can either be lower (before) or greater (after)
|
|
* `d2`, the returned value will always be the delay between the older
|
|
* and the newer time.
|
|
* `d1` and `d2` must not be NULL.
|
|
*/
|
|
uint_fast32_t owl_time_elapsed_ms(const owl_timestamp *const d1,
|
|
const owl_timestamp *const d2)
|
|
{
|
|
owl_timestamp elapsed ;
|
|
owl_time_elapsed(d1, d2, &elapsed) ;
|
|
return owl_timestamp_to_ms(&elapsed) ;
|
|
}
|
|
|
|
|
|
/**
|
|
* Computes the time difference between two `owl_timestamp` `d1` and
|
|
* `d2`. The result is stored in the `elapsed` parameter.
|
|
* Note that it is a delay, not a simple substraction, therefore the
|
|
* result is always positive.
|
|
* NULL parameters are not accepted.
|
|
*/
|
|
void owl_time_elapsed(const owl_timestamp *const d1,
|
|
const owl_timestamp *const d2,
|
|
owl_timestamp *const elapsed)
|
|
{
|
|
int_fast32_t sec, nsec ;
|
|
|
|
assert(d1) ;
|
|
assert(d2) ;
|
|
assert(elapsed) ;
|
|
|
|
sec = (int_fast64_t) d1->tv_sec - d2->tv_sec ;
|
|
nsec = (int_fast64_t) d1->tv_nsec - d2->tv_nsec ;
|
|
|
|
if (sec == 0)
|
|
{
|
|
elapsed->tv_sec = 0 ;
|
|
elapsed->tv_nsec = abs(nsec) ;
|
|
}
|
|
else if (sec > 0)
|
|
{
|
|
if (nsec >= 0)
|
|
{
|
|
elapsed->tv_sec = sec ;
|
|
elapsed->tv_nsec = nsec ;
|
|
}
|
|
else // nsec < 0
|
|
{
|
|
elapsed->tv_sec = sec - 1 ;
|
|
elapsed->tv_nsec = nsec + 1000000000ul ;
|
|
}
|
|
}
|
|
else // sec < 0
|
|
{
|
|
if (nsec > 0)
|
|
{
|
|
elapsed->tv_sec = abs(sec) - 1 ;
|
|
elapsed->tv_nsec = 1000000000ul - nsec ;
|
|
}
|
|
else // nsec <= 0
|
|
{
|
|
elapsed->tv_sec = abs(sec) ;
|
|
elapsed->tv_nsec = abs(nsec) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
/* *** Endianness *** */
|
|
|
|
|
|
/**
|
|
* Converts an `owl_timestamp` from host endianness to network
|
|
* endianness.
|
|
* The conversion is done in-place. `d` must not be `NULL`.
|
|
*/
|
|
void owl_hton_timestamp(owl_timestamp *const d)
|
|
{
|
|
assert(d) ;
|
|
d->tv_sec = htonl(d->tv_sec) ;
|
|
d->tv_nsec = htonl(d->tv_nsec) ;
|
|
}
|
|
|
|
|
|
/**
|
|
* Converts an `owl_timestamp` from network endianness to host
|
|
* endianness.
|
|
* The conversion is done in-place. `d` must not be `NULL`.
|
|
*/
|
|
void owl_ntoh_timestamp(owl_timestamp *const d)
|
|
{
|
|
assert(d) ;
|
|
d->tv_sec = ntohl(d->tv_sec) ;
|
|
d->tv_nsec = ntohl(d->tv_nsec) ;
|
|
}
|
|
|
|
|
|
/**
|
|
* This function swaps the bytes composing a `float`, i.e. it changes
|
|
* their order.
|
|
* You probably want to use the `owl_htonf()` and `owl_ntohf()` macros
|
|
* instead of this function.
|
|
*
|
|
* @returns The swapped `float`.
|
|
*/
|
|
float owl_swap_float(const float f)
|
|
{
|
|
float ret ;
|
|
char
|
|
*f_bytes = (char*) &f,
|
|
*ret_bytes = (char*) &ret ;
|
|
|
|
assert(sizeof(float) == 4) ;
|
|
|
|
ret_bytes[0] = f_bytes[3] ;
|
|
ret_bytes[1] = f_bytes[2] ;
|
|
ret_bytes[2] = f_bytes[1] ;
|
|
ret_bytes[3] = f_bytes[0] ;
|
|
|
|
return ret ;
|
|
}
|
|
|
|
|
|
|
|
/* *** Network *** */
|
|
|
|
|
|
/**
|
|
* Opens a UDP transmission socket and returns its descriptor.
|
|
*
|
|
* @param[in] server_address The server's IP address.
|
|
* @param[in] server_port The listening port on the server.
|
|
* @param[out] server_description The structure in which the server
|
|
* description will be saved.
|
|
* @param[out] client_description The structure in which the client
|
|
* description will be saved.
|
|
*
|
|
* @returns A file descriptor to the opened socket in case of success,
|
|
* or a negative error code.
|
|
*/
|
|
int owl_create_udp_trx_socket(const char *const server_address,
|
|
const uint_fast16_t server_port,
|
|
struct sockaddr_in *const server_description,
|
|
struct sockaddr_in *const client_description)
|
|
{
|
|
int sockfd ; // Socket descriptor
|
|
|
|
/* Create the UDP socket */
|
|
sockfd = socket(AF_INET, SOCK_DGRAM, 0) ;
|
|
if (sockfd < 0)
|
|
{
|
|
perror("UDP socket creation failed") ;
|
|
return -OWL_ERR_SOCKET_CREATE ;
|
|
}
|
|
|
|
/* Initialise the client structure */
|
|
memset(client_description, 0, sizeof(*client_description)) ;
|
|
client_description->sin_family = AF_INET ; // INET socket
|
|
client_description->sin_addr.s_addr = htonl(INADDR_ANY) ;
|
|
|
|
/* Initialise the server structure */
|
|
memset(server_description, 0, sizeof(*server_description)) ;
|
|
server_description->sin_family = AF_INET ; // INET socket
|
|
// Server IP address:
|
|
server_description->sin_addr.s_addr = inet_addr(server_address) ;
|
|
// Listening port on the server:
|
|
server_description->sin_port = htons(server_port) ;
|
|
|
|
return sockfd ;
|
|
}
|
|
|
|
|
|
/**
|
|
* Opens a UDP reception socket and returns its descriptor.
|
|
* `port` is the port on which the socket will listen.
|
|
*
|
|
* @returns A file descriptor to the opened socket in case of success,
|
|
* or a negative error code.
|
|
*/
|
|
int owl_create_udp_listening_socket(const uint_fast16_t port)
|
|
{
|
|
int sockfd ; // Socket descriptor
|
|
struct sockaddr_in server_description ; // Server structure
|
|
int ret = 0 ; // Return value
|
|
|
|
/* Create the UDP socket */
|
|
sockfd = socket(AF_INET, SOCK_DGRAM, 0) ;
|
|
if (sockfd < 0)
|
|
{
|
|
perror("UDP socket creation failed") ;
|
|
return -OWL_ERR_SOCKET_CREATE ;
|
|
}
|
|
|
|
/* Initialise the server structure */
|
|
memset(&server_description, 0, sizeof(server_description)) ;
|
|
server_description.sin_family = AF_INET ; // INET socket
|
|
// All the connections are accepted:
|
|
server_description.sin_addr.s_addr = htonl(INADDR_ANY) ;
|
|
server_description.sin_port = htons(port) ; // Listening port
|
|
|
|
/* Port reservation */
|
|
ret = bind(sockfd, (struct sockaddr*) &server_description,
|
|
sizeof(server_description)) ;
|
|
if (ret < 0)
|
|
{
|
|
perror("Cannot bind the UDP socket") ;
|
|
close(sockfd) ;
|
|
return -OWL_ERR_SOCKET_CREATE ;
|
|
}
|
|
|
|
return sockfd ;
|
|
}
|
|
|
|
|
|
|
|
/* *** Signals *** */
|
|
|
|
|
|
/**
|
|
* Generic signal handler for SIGINT.
|
|
*
|
|
* @param num The catched signal. This function will exit with
|
|
* `OWL_ERR_BAD_SIGNAL` if `num != SIGINT`.
|
|
*/
|
|
void owl_sigint_handler(const int num)
|
|
{
|
|
if (num != SIGINT)
|
|
{
|
|
fprintf(stderr, "Error! The SIGINT handler was called but the"
|
|
" signal is not SIGINT.\n") ;
|
|
exit(OWL_ERR_BAD_SIGNAL) ;
|
|
}
|
|
|
|
owl_run = false ;
|
|
|
|
#ifndef NDEBUG
|
|
fprintf(stderr, "\nSignal received: end.\n");
|
|
#endif // NDEBUG
|
|
fflush(NULL) ;
|
|
}
|
|
|
|
|
|
/**
|
|
* Generic signal handler for SIGTERM.
|
|
*
|
|
* @param num The catched signal. This function will exit with
|
|
* `OWL_ERR_BAD_SIGNAL` if `num != SIGTERM`.
|
|
*/
|
|
void owl_sigterm_handler(const int num)
|
|
{
|
|
if (num != SIGTERM)
|
|
{
|
|
fprintf(stderr, "Error! The SIGTERM handler was called but the"
|
|
" signal is not SIGTERM.\n") ;
|
|
exit(OWL_ERR_BAD_SIGNAL) ;
|
|
}
|
|
|
|
owl_sigint_handler(SIGINT) ;
|
|
}
|
|
|
|
|
|
|
|
/* *** Thread-related functions *** */
|
|
|
|
|
|
/**
|
|
* Closes the file descriptor `fd`.
|
|
* `fd` must be passed as an int pointer (`int*`). If `fd` is `NULL`,
|
|
* nothing will be done. Uppon error, a message is displayed on the
|
|
* standard error.
|
|
*/
|
|
void owl_close_fd(void *const fd)
|
|
{
|
|
if (fd == NULL)
|
|
return ;
|
|
|
|
const int *const file_desc = fd ;
|
|
if (close(*file_desc) != 0)
|
|
perror("Error closing file descriptor") ;
|
|
#ifndef NDEBUG
|
|
else
|
|
fprintf(stderr, "File descriptor %d closed successfully.\n",
|
|
*file_desc) ;
|
|
#endif // NDEBUG
|
|
}
|
|
|
|
|
|
/**
|
|
* Closes the stream `file`.
|
|
* `file` must be passed as a pointer on a pointer of FILE (FILE**).
|
|
* If `*file` is either `stdout`, `stderr` or `stdin`, it will not be
|
|
* closed.
|
|
*/
|
|
void owl_close_file(void *const file)
|
|
{
|
|
if (file == NULL)
|
|
return ;
|
|
|
|
FILE **stream = file ;
|
|
if (*stream == stdout || *stream == stderr || *stream == stdin)
|
|
return ;
|
|
|
|
if (fclose(*stream) != 0)
|
|
perror("Error closing stream") ;
|
|
#ifndef NDEBUG
|
|
else
|
|
fprintf(stderr, "Stream closed successfully.\n") ;
|
|
#endif // NDEBUG
|
|
}
|