mcy
/
owlps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
owlps/owlps-listener/owlps-listenerd.c

1484 lines
44 KiB
C
Raw Blame History

/*
* This file is part of the rtap localisation project.
*/
#include "owlps-listener.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <inttypes.h>
#include <string.h>
#include <errno.h>
#include <signal.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <net/ethernet.h>
#ifdef USE_CONFIG_FILE
# include <confuse.h>
#endif // USE_CONFIG_FILE
#ifdef USE_PTHREAD
# include <pthread.h>
# include <iwlib.h>
#endif // USE_PTHREAD
// Used by get_mac_addr():
#include <netinet/udp.h>
#include <netinet/ip.h>
#include <arpa/inet.h>
#include <assert.h>
char *program_name = NULL ;
uint8_t my_mac_bytes[ETHER_ADDR_LEN] ; // AP MAC address
char my_ip[INET_ADDRSTRLEN] ; // AP IP address
pcap_t *capture_handler = NULL ; // Packet capture descriptor
int aggregation_sockfd ;
struct sockaddr_in aggregation_server ;
#ifdef USE_PTHREAD
int autocalibration_send_sockfd ;
struct sockaddr_in autocalibration_send_server ;
// TRUE if the coordinates of the listener were provided by the user:
owl_bool coordinates_provided = FALSE ;
#endif // USE_PTHREAD
#ifdef USE_CONFIG_FILE
cfg_t *cfg ; // Configuration structure
#else // USE_CONFIG_FILE
/* If we do not use libconfuse, we declare a structure to store getopt
* options.
*/
struct {
char mode ;
char aggregation_ip[INET_ADDRSTRLEN] ;
uint_fast16_t aggregation_port ;
uint_fast16_t listening_port ;
#ifdef USE_PTHREAD
owl_bool keep_monitor ;
#endif // USE_PTHREAD
char rtap_iface[IFNAMSIZ + 1] ;
char wifi_iface[IFNAMSIZ + 1] ;
#ifdef USE_PTHREAD
owl_bool autocalibration ;
char autocalibration_ip[INET_ADDRSTRLEN] ;
uint_fast16_t autocalibration_request_port ;
uint_fast16_t autocalibration_port ;
uint_fast32_t autocalibration_hello_delay ;
uint_fast32_t autocalibration_delay ;
uint_fast16_t autocalibration_nb_packets ;
owl_direction my_direction ;
float my_position_x ;
float my_position_y ;
float my_position_z ;
#endif // USE_PTHREAD
uint_fast8_t verbose ;
owl_bool display_captured ;
} options = { // Initalise default options:
MODE_ACTIVE,
"127.0.0.1",
AGGREGATE_DEFAULT_PORT,
LOC_REQUEST_DEFAULT_PORT,
#ifdef USE_PTHREAD
FALSE,
#endif // USE_PTHREAD
"",
"",
#ifdef USE_PTHREAD
FALSE,
"",
DEFAULT_AUTOCALIBRATION_REQUEST_PORT,
DEFAULT_AUTOCALIBRATION_PORT,
DEFAULT_AUTOCALIBRATION_HELLO_DELAY,
DEFAULT_AUTOCALIBRATION_DELAY,
DEFAULT_AUTOCALIBRATION_NBPKT,
0, 0, 0, 0,
#endif // USE_PTHREAD
0,
FALSE
} ;
#endif // USE_CONFIG_FILE
int main(int argc, char *argv[])
{
struct sigaction action ; // Signal handler structure
int ret ; // Program return value
#ifdef USE_PTHREAD
pthread_t
keep_monitor_thread,
autocalibration_thread,
autocalibration_hello_thread ;
#endif // USE_PTHREAD
program_name = argv[0] ;
initialise_configuration(argc, argv) ;
owl_run = TRUE ;
/* Set up signal handlers */
sigemptyset(&action.sa_mask) ;
action.sa_handler = sigint_handler ;
sigaction(SIGINT, &action, NULL) ;
action.sa_handler = sigterm_handler ;
sigaction(SIGTERM, &action, NULL) ;
get_mac_addr(GET_WIFI_IFACE(), my_mac_bytes) ;
printf("My MAC address is: %s\n",
owl_mac_bytes_to_string(my_mac_bytes)) ;
get_ip_addr(GET_WIFI_IFACE(), my_ip) ;
printf("My IP address is: %s\n", my_ip) ;
#ifdef USE_PTHREAD
/* Set up threads */
if (GET_KEEP_MONITOR())
{
ret = pthread_create(&keep_monitor_thread, NULL,
&keep_mode_monitor, GET_WIFI_IFACE()) ;
if (ret != 0)
{
perror("Cannot create keep monitor thread") ;
ret = ERR_CREATING_THREAD ;
goto exit ;
}
}
if (GET_AUTOCALIBRATION())
{
ret = pthread_create(&autocalibration_thread, NULL,
&autocalibrate, NULL) ;
if (ret != 0)
{
perror("Cannot create autocalibration thread") ;
ret = ERR_CREATING_THREAD ;
goto exit ;
}
ret = pthread_create(&autocalibration_hello_thread, NULL,
&autocalibrate_hello, NULL) ;
if (ret != 0)
{
perror("Cannot create autocalibration hello thread") ;
ret = ERR_CREATING_THREAD ;
goto exit ;
}
}
#endif // USE_PTHREAD
ret = capture() ; // Capture loop
/* Wait for the threads to terminate */
#ifdef USE_PTHREAD
if (GET_KEEP_MONITOR())
{
fprintf(stderr, "Waiting for the keep mode monitor thread... ") ;
if (pthread_join(keep_monitor_thread, NULL) != 0)
perror("Cannot join keep mode monitor thread") ;
else
fprintf(stderr, "OK.\n") ;
}
if (GET_AUTOCALIBRATION())
{
// We must cancel this thread because it can be blocked on the
// recvfrom() call:
fprintf(stderr,
"Cancelling the autocalibration thread... ") ;
if (pthread_cancel(autocalibration_thread) != 0)
perror("Cannot cancel autocalibration thread") ;
else
fprintf(stderr, "OK.\n") ;
fprintf(stderr,
"Waiting for the autocalibration thread... ") ;
if (pthread_join(autocalibration_thread, NULL) != 0)
perror("Cannot join autocalibration thread") ;
else
fprintf(stderr, "OK.\n") ;
// We must cancel this thread if we do not want to wait
// autocalibration_hello_delay seconds (in the worst case):
fprintf(stderr,
"Cancelling the autocalibration hello thread... ") ;
if (pthread_cancel(autocalibration_hello_thread) != 0)
perror("Cannot cancel autocalibration hello thread") ;
else
fprintf(stderr, "OK.\n") ;
fprintf(stderr,
"Waiting for the autocalibration hello thread... ") ;
if (pthread_join(autocalibration_hello_thread, NULL) != 0)
perror("Cannot join autocalibration hello thread") ;
else
fprintf(stderr, "OK.\n") ;
}
#else // USE_PTHREAD
// Just to avoid a warning when compiling without threads' support:
goto exit ;
#endif // USE_PTHREAD
/* Last cleaning tasks */
exit:
#ifdef USE_CONFIG_FILE
cfg_free(cfg) ; // Clean configuration
#endif // USE_CONFIG_FILE
printf("%s: end.\n", program_name) ;
return ret ;
}
void initialise_configuration(int argc, char **argv)
{
parse_config_file(argc, argv) ;
parse_command_line(argc, argv) ;
check_configuration() ;
#ifdef DEBUG
print_configuration() ;
#endif // DEBUG
}
void parse_config_file(int argc, char **argv)
{
#ifdef USE_CONFIG_FILE
// If we use libconfuse, we declare options:
cfg_opt_t opts[] =
{
// Listening mode: a for active, p for passive, m for mixed
// (default: a):
CFG_INT("mode", MODE_ACTIVE, CFGF_NONE),
// IP address of the aggregator (default: loopback):
CFG_STR("aggregation_ip", "127.0.0.1", CFGF_NONE),
// Port on which the aggregator listens:
CFG_INT("aggregation_port", AGGREGATE_DEFAULT_PORT, CFGF_NONE),
// Port on which mobiles send active requests:
CFG_INT("listening_port", LOC_REQUEST_DEFAULT_PORT, CFGF_NONE),
#ifdef USE_PTHREAD
// Activate the active monitor mode keeping-up (read the code if
// you do not understand what I mean):
CFG_BOOL("keep_monitor", cfg_false, CFGF_NONE),
#endif // USE_PTHREAD
// Radiotap interface, used to capture:
CFG_STR("rtap_iface", "", CFGF_NONE),
// Physical interface corresponding to the radiotap interface (used
// to get the MAC address):
CFG_STR("wifi_iface", "", CFGF_NONE),
#ifdef USE_PTHREAD
// Autocalibration activated?
CFG_BOOL("autocalibration", cfg_false, CFGF_NONE),
// Destination IP address of the autocalibration requests (default:
// none, but will be set to aggregation_ip in the config check):
CFG_STR("autocalibration_ip", "", CFGF_NONE),
// Port on which autocalibration requests are sent:
CFG_INT("autocalibration_request_port",
DEFAULT_AUTOCALIBRATION_REQUEST_PORT, CFGF_NONE),
// Port on which autocalibration data are exchanged:
CFG_INT("autocalibration_port", DEFAULT_AUTOCALIBRATION_PORT,
CFGF_NONE),
// Delay between two hello messages:
CFG_INT("autocalibration_hello_delay",
DEFAULT_AUTOCALIBRATION_HELLO_DELAY,
CFGF_NONE),
// Delay between two calibration packet transmission:
CFG_INT("autocalibration_delay", DEFAULT_AUTOCALIBRATION_DELAY,
CFGF_NONE),
// Number of packets for a calibration request:
CFG_INT("autocalibration_nb_packets",
DEFAULT_AUTOCALIBRATION_NBPKT, CFGF_NONE),
// Direction
CFG_INT("my_direction", 0, CFGF_NONE),
// Position X
CFG_FLOAT("my_position_x", 0, CFGF_NONE),
// Position Y
CFG_FLOAT("my_position_y", 0, CFGF_NONE),
// Position Z
CFG_FLOAT("my_position_z", 0, CFGF_NONE),
#endif // USE_PTHREAD
// Verbose level:
CFG_INT("verbose", 0, CFGF_NONE),
// Display captured packets, or not:
CFG_BOOL("display_captured", cfg_false, CFGF_NONE),
CFG_END()
} ;
char *config_file = NULL ; // Configuration file name
#endif // USE_CONFIG_FILE
// Option -f specifies a config file, options -h and -V exit the
// program, so we search for them first
int opt ;
while ((opt = getopt(argc, argv, OPTIONS)) != -1)
{
switch (opt)
{
case 'f' :
#ifdef USE_CONFIG_FILE
config_file = malloc((strlen(optarg) + 1) * sizeof(char)) ;
strcpy(config_file, optarg) ;
#else // USE_CONFIG_FILE
fprintf(stderr, "Warning! Program was not compiled with"
" configuration file support, so -f is not available."
" You must specify all options on the command line,"
" or default value will be used.\n") ;
#endif // USE_CONFIG_FILE
break ;
case 'h' :
print_usage() ;
exit(0) ;
case 'V' :
print_version() ;
exit(0) ;
}
}
#ifdef USE_CONFIG_FILE
// If -f isn't found, we use the default config file
if (config_file == NULL)
{
config_file =
malloc((strlen(DEFAULT_CONFIG_FILE) + 1) * sizeof(char)) ;
strcpy(config_file, DEFAULT_CONFIG_FILE) ;
}
/* Parse config file */
cfg = cfg_init(opts, CFGF_NONE) ; // Initialise options
switch (cfg_parse(cfg, config_file))
{
case CFG_FILE_ERROR :
fprintf(stderr,
"Error! Cannot open configuration file « %s »: %s.\n",
config_file, strerror(errno)) ;
break ;
case CFG_PARSE_ERROR :
fprintf(stderr,
"Error! Parsing of configuration file « %s » failed!\n",
config_file) ;
free(config_file) ;
exit(ERR_PARSING_CONFIG_FILE) ;
}
free(config_file) ;
#endif // USE_CONFIG_FILE
}
void parse_command_line(int argc, char **argv)
{
parse_main_options(argc, argv) ;
#ifdef USE_PTHREAD
parse_calibration_data(argc, argv) ;
#endif // USE_PTHREAD
}
void parse_main_options(int argc, char **argv)
{
int opt ;
optind = 1 ; // Rewind argument parsing
while ((opt = getopt(argc, argv, OPTIONS)) != -1)
{
switch (opt)
{
case 'A' :
#ifdef USE_PTHREAD
SET_AUTOCALIBRATION() ;
#else // USE_PTHREAD
fprintf(stderr, "Warning! The program was compiled without"
" support of POSIX threads, so -A (autocalibration)"
" is not available and will be ignored. All other"
" autocalibration-related options will also be"
" ignored.\n") ;
#endif // USE_PTHREAD
break ;
case 'a' :
#ifdef USE_PTHREAD
SET_AUTOCALIBRATION_PORT(strtol(optarg, NULL, 0)) ;
#endif // USE_PTHREAD
break ;
case 'c' :
SET_DISPLAY_CAPTURED() ;
break ;
case 'd' :
SET_AGGREGATION_IP(optarg) ;
break ;
case 'D' :
#ifdef USE_PTHREAD
SET_AUTOCALIBRATION_IP(optarg) ;
#endif // USE_PTHREAD
break ;
case 'f' : // Config file
break ; // (already parsed)
case 'h' : // Usage
break ; // (already parsed)
case 'H' :
#ifdef USE_PTHREAD
SET_AUTOCALIBRATION_HELLO_DELAY(strtol(optarg, NULL, 0)) ;
#endif // USE_PTHREAD
break ;
case 'k' :
#ifdef USE_PTHREAD
SET_KEEP_MONITOR() ;
#else // USE_PTHREAD
fprintf(stderr, "Warning! The program was compiled without"
" support of POSIX threads, so -k (monitor mode"
" keeping-up) is not available and will be ignored."
"\n") ;
#endif // USE_PTHREAD
break ;
case 'l' :
SET_LISTENING_PORT(strtol(optarg, NULL, 0)) ;
break ;
case 'm' :
SET_MODE(optarg[0]) ;
break ;
case 'n' :
#ifdef USE_PTHREAD
SET_AUTOCALIBRATION_NBPKT(strtol(optarg, NULL, 0)) ;
#endif // USE_PTHREAD
break ;
case 'p' :
SET_AGGREGATION_PORT(strtol(optarg, NULL, 0)) ;
break ;
case 'P' :
#ifdef USE_PTHREAD
SET_AUTOCALIBRATION_REQUEST_PORT(strtol(optarg, NULL, 0)) ;
#endif // USE_PTHREAD
break ;
case 'q' :
RESET_VERBOSE() ;
break ;
case 'Q' :
UNSET_DISPLAY_CAPTURED() ;
break ;
case 'r' :
SET_RTAP_IFACE(optarg) ;
break ;
case 't' :
#ifdef USE_PTHREAD
SET_AUTOCALIBRATION_DELAY(strtol(optarg, NULL, 0)) ;
#endif // USE_PTHREAD
break ;
case 'v' :
INCREMENT_VERBOSE() ;
break ;
case 'V' : // Version
break ; // (already parsed)
case 'w' :
SET_WIFI_IFACE(optarg) ;
break ;
default :
print_usage() ;
exit(ERR_BAD_USAGE) ;
}
}
}
#ifdef USE_PTHREAD
void parse_calibration_data(int argc, char **argv)
{
/* Parse remaining arguments (possible calibration data) */
if (argc - optind != 0)
{
if (argc - optind == 4)
{
coordinates_provided = TRUE ;
SET_MY_DIRECTION(strtoul(argv[optind++], NULL, 0)) ;
SET_MY_POSITION_X(strtod(argv[optind++], NULL)) ;
SET_MY_POSITION_Y(strtod(argv[optind++], NULL)) ;
SET_MY_POSITION_Z(strtod(argv[optind], NULL)) ;
}
else // Bad number of arguments
{
print_usage() ;
exit(ERR_BAD_USAGE) ;
}
}
}
#endif // USE_PTHREAD
void check_configuration()
{
switch (GET_MODE())
{
case MODE_ACTIVE :
case MODE_MIXED :
case MODE_PASSIVE :
break ;
default :
fprintf(stderr, "Error! Unknown mode « %c ».\n", (char) GET_MODE()) ;
print_usage() ;
exit(ERR_BAD_USAGE) ;
}
if (GET_RTAP_IFACE()[0] == '\0')
{
fprintf(stderr, "Error! You must specify a radiotap interface"
" for the capture.\n") ;
print_usage() ;
exit(ERR_BAD_USAGE) ;
}
if (GET_WIFI_IFACE()[0] == '\0')
{
if (GET_VERBOSE())
fprintf(stderr, "No Wi-Fi interface was specified. Failing back"
" to the radiotap interface (%s) instead.\n",
GET_RTAP_IFACE()) ;
SET_WIFI_IFACE(GET_RTAP_IFACE()) ;
}
// Port numbers //
if (GET_AGGREGATION_PORT() < 1 || GET_AGGREGATION_PORT() > 65535)
{
if (GET_VERBOSE())
fprintf(stderr, "Warning! Bad aggregation_port:"
" failing back to the default value.\n") ;
SET_AGGREGATION_PORT(AGGREGATE_DEFAULT_PORT) ;
}
if (GET_LISTENING_PORT() < 1 || GET_LISTENING_PORT() > 65535)
{
if (GET_VERBOSE())
fprintf(stderr, "Warning! Bad listening_port:"
" failing back to the default value.\n") ;
SET_LISTENING_PORT(LOC_REQUEST_DEFAULT_PORT) ;
}
// Autocalibration stuff //
#ifdef USE_PTHREAD
if (GET_AUTOCALIBRATION())
{
if (GET_AUTOCALIBRATION_IP()[0] == '\0')
{
if (GET_VERBOSE())
fprintf(stderr, "No autocalibration IP specified, we will"
" use the aggregation IP as the destination of"
" autocalibration requests.\n") ;
SET_AUTOCALIBRATION_IP(GET_AGGREGATION_IP()) ;
}
if (GET_AUTOCALIBRATION_NBPKT() < 1)
fprintf(stderr, "Warning! autocalibration_nb_packets is null,"
" no autocalibration request will be sent!\n") ;
if (coordinates_provided)
if (GET_MY_DIRECTION() < OWL_DIRECTION_MIN ||
GET_MY_DIRECTION() > OWL_DIRECTION_MAX)
fprintf(stderr, "Warning! « %d » is not a valid"
" direction.\n", (int) GET_MY_DIRECTION()) ;
// Autocalibration port numbers
if (GET_AUTOCALIBRATION_REQUEST_PORT() < 1 ||
GET_AUTOCALIBRATION_REQUEST_PORT() > 65535)
{
if (GET_VERBOSE())
fprintf(stderr, "Warning! Bad autocalibration_request_port:"
" failing back to the default value.\n") ;
SET_AUTOCALIBRATION_REQUEST_PORT(DEFAULT_AUTOCALIBRATION_REQUEST_PORT) ;
}
if (GET_AUTOCALIBRATION_PORT() < 1 ||
GET_AUTOCALIBRATION_PORT() > 65535)
{
if (GET_VERBOSE())
fprintf(stderr, "Warning! Bad autocalibration_port:"
" failing back to the default value.\n") ;
SET_AUTOCALIBRATION_PORT(DEFAULT_AUTOCALIBRATION_PORT) ;
}
}
#endif // USE_PTHREAD
}
#ifdef DEBUG
void print_configuration()
{
fprintf(stderr, "Configuration:\n") ;
#ifdef USE_CONFIG_FILE
cfg_print(cfg, stderr) ;
#else // USE_CONFIG_FILE
fprintf(stderr,
"mode = %c\n"
"aggregation_ip = \"%s\"\n"
"aggregation_port = %"PRIuFAST16"\n"
"listening_port = %"PRIuFAST16"\n"
"rtap_iface = \"%s\"\n"
"wifi_iface = \"%s\"\n"
#ifdef USE_PTHREAD
"keep_monitor = %s\n"
"autocalibration = %s\n"
"autocalibration_ip = \"%s\"\n"
"autocalibration_request_port = %"PRIuFAST16"\n"
"autocalibration_port = %"PRIuFAST16"\n"
"autocalibration_hello_delay = %"PRIuFAST32"\n"
"autocalibration_delay = %"PRIuFAST32"\n"
"autocalibration_nb_packets = %"PRIuFAST16"\n"
"my_direction = %"PRIu8"\n"
"my_position_x = %f\n"
"my_position_y = %f\n"
"my_position_z = %f\n"
#endif // USE_PTHREAD
"verbose = %s\n"
"display_captured = %s\n"
,
GET_MODE(),
GET_AGGREGATION_IP(),
GET_AGGREGATION_PORT(),
GET_LISTENING_PORT(),
GET_RTAP_IFACE(),
GET_WIFI_IFACE(),
#ifdef USE_PTHREAD
OWL_BOOL_TO_STRING(GET_KEEP_MONITOR()),
OWL_BOOL_TO_STRING(GET_AUTOCALIBRATION()),
GET_AUTOCALIBRATION_IP(),
GET_AUTOCALIBRATION_REQUEST_PORT(),
GET_AUTOCALIBRATION_PORT(),
GET_AUTOCALIBRATION_HELLO_DELAY(),
GET_AUTOCALIBRATION_DELAY(),
GET_AUTOCALIBRATION_NBPKT(),
GET_MY_DIRECTION(),
GET_MY_POSITION_X(),
GET_MY_POSITION_Y(),
GET_MY_POSITION_Z(),
#endif // USE_PTHREAD
OWL_BOOL_TO_STRING(GET_VERBOSE()),
OWL_BOOL_TO_STRING(GET_DISPLAY_CAPTURED())
) ;
#endif // USE_CONFIG_FILE
}
#endif // DEBUG
#ifdef USE_PTHREAD
/*
* Thread function. Switches interface 'iface' to monitor mode every
* second.
*/
void* keep_mode_monitor(void *iface)
{
if (GET_VERBOSE())
fprintf(stderr, "Thread for keeping monitor mode launched.\n") ;
while (owl_run)
{
// Switch the interface to monitor mode:
iface_mode_monitor((char*) iface) ;
sleep(1) ; // Wait for 1 second
}
pthread_exit(NULL) ;
}
/*
* Switches the IEEE 802.11 interface 'iface' to Monitor mode.
*/
int iface_mode_monitor(const char *const iface)
{
struct iwreq wrq ;
int sockfd = iw_sockets_open() ;
strncpy((&wrq)->ifr_name, iface, IFNAMSIZ) ;
if (ioctl(sockfd, SIOCGIWMODE, &wrq) == -1) // Get current mode
{
perror("Error reading interface mode") ;
return ERR_READING_MODE ;
}
// If interface is not yet in Monitor mode
if (wrq.u.mode != IW_MODE_MONITOR)
{
wrq.u.mode = IW_MODE_MONITOR ;
if (ioctl(sockfd, SIOCSIWMODE, &wrq) == -1) // Set up Monitor mode
{
perror("Error setting up Monitor mode") ;
return ERR_SETTING_MODE ;
}
}
close(sockfd) ;
return 0 ;
}
#endif // USE_PTHREAD
/*
* Captures packets using the radiotap interface.
* Captured data is transmitted to the aggregator.
*/
int capture()
{
char errbuf[PCAP_ERRBUF_SIZE] ; // Error message
// Start capture:
capture_handler =
pcap_open_live(GET_RTAP_IFACE(), BUFSIZ, 1, 1000, errbuf) ;
if (capture_handler == NULL) // Capture starting failed
{
fprintf(stderr, "Cannot open capture interface %s\n", errbuf) ;
return ERR_OPENING_IFACE ;
}
/* Open UDP socket to the aggregator */
aggregation_sockfd =
owlclient_create_trx_socket(GET_AGGREGATION_IP(),
GET_AGGREGATION_PORT(),
&aggregation_server, NULL) ;
while (owl_run)
// Capture one packet at time, and call read_packet() on it:
pcap_loop(capture_handler, 1, read_packet, NULL) ;
pcap_close(capture_handler) ; // Stop capture
close(aggregation_sockfd) ; // Close socket
return 0 ;
}
/*
* Treats a packet and sends it to the aggregator.
*/
void read_packet(u_char *args, const struct pcap_pkthdr *header,
const u_char *packet)
{
uint16_t rtap_bytes ; // Received data size
uint32_t rtap_presentflags ;
uint_fast16_t rtap_position ;
owl_captured_request request ; // Message to send to the aggregator
ssize_t nsent ; // sendto return value
owl_bool check[15] ; // Present flags
uint8_t raw_packet_fc1 ; // First byte of the received frame's FC
uint8_t raw_packet_fc2 ; // Second byte of the received frame's FC
uint8_t raw_packet_flags ; // IEEE 802.11 header flags
// Size of the IEEE 802.11 header:
uint_fast8_t ieee80211_header_size = IEEE80211_HEADER_SIZE_DATA ;
uint16_t llc_packet_type = 0 ;
// Pointer to the (possible) IP header of the packet:
struct iphdr *packet_ip_header = NULL ;
// Pointer to the (possible) UDP header of the packet:
struct udphdr *packet_udp_header = NULL ;
// Localisation request type (request, calibration, autocalibration):
owl_bool is_explicit_packet = TRUE ; // Is the packet an explicit request?
// Is the packet an autocalibration positioning request?
owl_bool uses_autocalibration_request_port = FALSE ;
int i ; // Iterator
// Blank the request:
memset(&request, 0, sizeof(request)) ;
/* Common treatements */
// Copy 2 bytes from the 3rd packet byte, that is the size of the rtap
// header (changes with the flags):
memcpy(&rtap_bytes, &packet[2], sizeof(rtap_bytes)) ;
// Radiotap header is little-endian
rtap_bytes = le16toh(rtap_bytes) ;
// After the rtap header, there is the 802.11 header; the first byte
// is the first byte of the Frame Control (FC) field, which contains
// the type of the packet (Management, Control or Data) and its subtype
// (QoS, etc.):
raw_packet_fc1 = packet[rtap_bytes] ;
if (! IS_DATA_FRAME(raw_packet_fc1)) // Data frame?
goto not_explicit_packet ;
if (DATA_FRAME_IS_QOS(raw_packet_fc1)) // QoS Data frame?
ieee80211_header_size += 2 ; // 2 bytes of QoS information
// The second byte of the FC field contains the frame flags. The two
// first bits indicate the frame source and destination types: the
// first bit is "To DS" and the second is "From DS", so if the second
// bit is 0 the frame comes from a STA. That's what we want for an
// explicit packet:
raw_packet_fc2 = packet[rtap_bytes+1] ;
if (! IS_FRAME_FROM_STA(raw_packet_fc2))
goto not_explicit_packet ;
// Get the packet type (protocol, 2 bytes) from the LLC header:
memcpy(&llc_packet_type,
&packet[rtap_bytes + ieee80211_header_size + 6], 2) ;
llc_packet_type = ntohs(llc_packet_type) ;
if (llc_packet_type != ETHERTYPE_IP) // IP packet?
goto not_explicit_packet ;
packet_ip_header = (struct iphdr *)
&packet[rtap_bytes + ieee80211_header_size + LLC_HEADER_SIZE] ;
// Get the source IP:
memcpy(request.mobile_ip_addr_bytes, &packet_ip_header->saddr, 4) ;
if (GET_MODE() != MODE_PASSIVE) // If mode is active or mixed
{
uint_fast16_t dest_port ;
// Protocol for an explicit request is UDP
if (packet_ip_header->protocol != IPPROTO_UDP)
goto not_explicit_packet ;
// Check destination port:
packet_udp_header = (struct udphdr *)
&packet[rtap_bytes + ieee80211_header_size +
LLC_HEADER_SIZE + sizeof(struct iphdr)] ;
dest_port = ntohs(packet_udp_header->dest) ;
#ifdef USE_PTHREAD
if (GET_AUTOCALIBRATION() && dest_port ==
(uint_fast16_t) GET_AUTOCALIBRATION_REQUEST_PORT())
uses_autocalibration_request_port = TRUE ;
else
#endif // USE_PTHREAD
if (dest_port != (uint_fast16_t) GET_LISTENING_PORT())
goto not_explicit_packet ;
}
goto process_packet ;
not_explicit_packet :
if (GET_MODE() == MODE_ACTIVE)
return ;
is_explicit_packet = FALSE ;
process_packet :
// Get 802.11 flags from the 802.11 header:
raw_packet_flags = packet[rtap_bytes+1] ;
#ifdef DEBUG
if (IS_RETRY(raw_packet_flags))
printf("This packet is a Retry.\n") ;
#endif // DEBUG
// Source MAC address is 10 bytes after the 802.11 packet type:
memcpy(request.mobile_mac_addr_bytes, &packet[rtap_bytes+10],
ETHER_ADDR_LEN) ;
// Drop the packet if it comes from the AP itself:
if (owl_mac_equals(my_mac_bytes, request.mobile_mac_addr_bytes))
return ;
// Copy AP MAC :
memcpy(request.ap_mac_addr_bytes, my_mac_bytes, ETHER_ADDR_LEN) ;
// Capture time is in the pcap header (net-endian):
request.start_time = owl_timeval_to_timestamp(header->ts) ;
/* Active mode */
if (is_explicit_packet
&& (GET_MODE() == MODE_ACTIVE || GET_MODE() == MODE_MIXED)
// FIXME: should we really ignore Retries?
&& ! IS_RETRY(raw_packet_flags))
{
request.type =
packet[rtap_bytes + ieee80211_header_size + LLC_HEADER_SIZE
+ sizeof(struct iphdr) + sizeof(struct udphdr)] ;
switch(request.type)
{
case OWL_REQUEST_NORMAL :
if (GET_VERBOSE())
printf("\nExplicit packet received.\n") ;
break ;
case OWL_REQUEST_CALIBRATION :
if (GET_VERBOSE())
printf("\nExplicit calibration packet received.\n") ;
request.direction =
packet[rtap_bytes + ieee80211_header_size + LLC_HEADER_SIZE
+ sizeof(struct iphdr) + sizeof(struct udphdr) + 9];
memcpy(&request.x_position,
&packet[rtap_bytes + ieee80211_header_size
+ LLC_HEADER_SIZE + sizeof(struct iphdr)
+ sizeof(struct udphdr) + 10], sizeof(float)) ;
memcpy(&request.y_position,
&packet[rtap_bytes + ieee80211_header_size
+ LLC_HEADER_SIZE + sizeof(struct iphdr)
+ sizeof(struct udphdr) + 14], sizeof(float)) ;
memcpy(&request.z_position,
&packet[rtap_bytes + ieee80211_header_size
+ LLC_HEADER_SIZE + sizeof(struct iphdr)
+ sizeof(struct udphdr) + 18], sizeof(float)) ;
break ;
case OWL_REQUEST_AUTOCALIBRATION :
if (GET_VERBOSE())
{
printf("\nAutocalibration packet received.") ;
if (! uses_autocalibration_request_port)
printf(".. on the wrong port!") ;
putchar('\n') ;
}
request.direction =
packet[rtap_bytes + ieee80211_header_size + LLC_HEADER_SIZE
+ sizeof(struct iphdr) + sizeof(struct udphdr) + 9];
memcpy(&request.x_position,
&packet[rtap_bytes + ieee80211_header_size
+ LLC_HEADER_SIZE + sizeof(struct iphdr)
+ sizeof(struct udphdr) + 10], sizeof(float)) ;
memcpy(&request.y_position,
&packet[rtap_bytes + ieee80211_header_size
+ LLC_HEADER_SIZE + sizeof(struct iphdr)
+ sizeof(struct udphdr) + 14], sizeof(float)) ;
memcpy(&request.z_position,
&packet[rtap_bytes + ieee80211_header_size
+ LLC_HEADER_SIZE + sizeof(struct iphdr)
+ sizeof(struct udphdr) + 18], sizeof(float)) ;
break ;
default :
if (GET_VERBOSE())
printf("\nStrange explicit packet received\n") ;
fprintf(stderr,
"Error! Unknown request type (%d).\n", request.type) ;
is_explicit_packet = FALSE ;
}
if (! is_explicit_packet)
{
if (GET_MODE() == MODE_ACTIVE)
return ;
else
{
if (GET_VERBOSE())
printf("\nThis strange explicit packet will be handled"
" as an implicit one.\n") ;
request.type = OWL_REQUEST_IMPLICIT ;
}
}
else
// Copy the timestamp "as is" (i.e. without changing endianess)
// because it will return to the network soon:
memcpy(&request.request_time,
&packet[rtap_bytes + ieee80211_header_size +
LLC_HEADER_SIZE + sizeof(struct iphdr) +
sizeof(struct udphdr) + 1],
sizeof(owl_timestamp)) ;
}
else if (GET_MODE() == MODE_PASSIVE || GET_MODE() == MODE_MIXED)
{
if (GET_VERBOSE())
printf("\nImplicit packet received.\n") ;
request.type = OWL_REQUEST_IMPLICIT ;
}
else // Active mode, packet was not an explicit request
return ;
/* Radiotap header handling */
// Get rtap flags:
memcpy(&rtap_presentflags,
&packet[RTAP_P_PRESENTFLAGS], RTAP_L_PRESENTFLAGS) ;
// Radiotap header is little-endian
rtap_presentflags = le32toh(rtap_presentflags) ;
for (i = 0 ; i < 15 ; i++) // Initialise present flags structure
check[i] = FALSE ;
rtap_position = 8 ; // Begining of the present flags determined fields
// Test the first 15 bits of the flag field in order to check their
// presence and to copy them:
for (i = 0 ; i < 15 ; i++)
{
if ((rtap_presentflags % 2) == 1)
{
switch(i)
{
case RTAP_MACTS:
check[RTAP_MACTS] = TRUE ;
rtap_position += RTAP_L_MACTS ;
break ;
case RTAP_FLAGS:
check[RTAP_FLAGS] = TRUE;
rtap_position += RTAP_L_FLAGS ;
break ;
case RTAP_RATE:
check[RTAP_RATE] = TRUE;
rtap_position += RTAP_L_RATE ;
break ;
case RTAP_CHANNEL:
rtap_position += RTAP_L_CHANNEL ;
rtap_position += RTAP_L_CHANNELTYPE ;
break ;
case RTAP_FHSS:
check[RTAP_FHSS] = TRUE;
rtap_position += RTAP_L_FHSS ;
break ;
case RTAP_ANTENNASIGNALDBM:
memcpy(&(request.antenna_signal_dbm),
&packet[rtap_position], RTAP_L_ANTENNASIGNALDBM) ;
check[RTAP_ANTENNASIGNALDBM] = TRUE;
if (GET_VERBOSE())
printf("Antenna signal: %d dBm\n",
request.antenna_signal_dbm - 0x100);
rtap_position += RTAP_L_ANTENNASIGNALDBM ;
break ;
case RTAP_ANTENNANOISEDBM:
check[RTAP_ANTENNANOISEDBM] = TRUE;
rtap_position += RTAP_L_ANTENNANOISEDBM ;
break ;
case RTAP_LOCKQUALITY:
check[RTAP_LOCKQUALITY] = TRUE;
rtap_position += RTAP_L_LOCKQUALITY ;
break ;
case RTAP_TXATTENUATION:
check[RTAP_TXATTENUATION] = TRUE;
rtap_position += RTAP_L_TXATTENUATION ;
break ;
case RTAP_TXATTENUATIONDB:
check[RTAP_TXATTENUATIONDB] = TRUE;
rtap_position += RTAP_L_TXATTENUATIONDB ;
break ;
case RTAP_TXATTENUATIONDBM:
check[RTAP_TXATTENUATIONDBM] = TRUE;
rtap_position += RTAP_L_TXATTENUATIONDBM ;
break ;
case RTAP_ANTENNA:
check[RTAP_ANTENNA] = TRUE;
rtap_position += RTAP_L_ANTENNA ;
break ;
case RTAP_ANTENNASIGNALDB:
check[RTAP_ANTENNASIGNALDB] = TRUE;
rtap_position += RTAP_L_ANTENNASIGNALDB ;
break ;
case RTAP_ANTENNANOISEDB:
check[RTAP_ANTENNANOISEDB] = TRUE;
rtap_position += RTAP_L_ANTENNANOISEDB ;
break ;
case RTAP_FCS:
check[RTAP_FCS] = TRUE;
rtap_position += RTAP_L_FCS ;
break ;
}
}
rtap_presentflags /= 2 ;
}
/* Display the packet details */
if (GET_DISPLAY_CAPTURED())
{
char
request_time_str[OWL_TIMESTAMP_STR_LEN],
start_time_str[OWL_TIMESTAMP_STR_LEN] ;
owl_timestamp_to_string(request_time_str,
owl_ntoh_timestamp(request.request_time)) ;
owl_timestamp_to_string(start_time_str,
owl_ntoh_timestamp(request.start_time)) ;
printf("*** Request to send ***\n"
"\tType: %"PRIu8"\n"
"\tMAC AP: %s\n"
"\tMobile MAC: %s\n"
"\tSequence number (request time): %s\n"
"\tRequest arrival time on the AP: %s\n"
"\tSignal: %d dBm\n"
"\tPosition X: %f\n"
"\tPosition Y: %f\n"
"\tPosition Z: %f\n"
"\tDirection: %hhd\n"
,
request.type,
owl_mac_bytes_to_string(request.ap_mac_addr_bytes),
owl_mac_bytes_to_string(request.mobile_mac_addr_bytes),
request_time_str,
start_time_str,
check[RTAP_ANTENNASIGNALDBM] ?
request.antenna_signal_dbm - 0x100 : 0,
owl_ntohf(request.x_position),
owl_ntohf(request.y_position),
owl_ntohf(request.z_position),
request.direction
) ;
}
/* Send the request to the aggregator */
nsent =
sendto(aggregation_sockfd, &request, sizeof(request), 0,
(struct sockaddr *) &aggregation_server,
(socklen_t) sizeof(aggregation_server)) ;
if (nsent != (ssize_t) sizeof(request))
{
perror("Error sending request to the aggregation server") ;
return ;
}
}
/*
* Get our own MAC address and copy it to 'mac_bytes'.
*/
void get_mac_addr(char *eth, uint8_t mac_bytes[ETHER_ADDR_LEN])
{
struct ifreq ifr;
int sockfd ;
// Empty mac_bytes:
memset(mac_bytes, 0, sizeof(uint8_t) * ETHER_ADDR_LEN) ;
sockfd = socket(AF_INET, SOCK_DGRAM, 0) ;
if(sockfd < 0)
perror("Cannot open socket to read MAC address") ;
strncpy(ifr.ifr_name, eth, IFNAMSIZ) ;
if (ioctl(sockfd, SIOCGIFFLAGS, &ifr) < 0)
return ;
if (ioctl(sockfd, SIOCGIFHWADDR, &ifr) < 0)
return ;
memcpy(mac_bytes, ifr.ifr_hwaddr.sa_data, ETHER_ADDR_LEN) ;
}
/*
* Get our own IP address and copy it to 'ip'.
*/
void get_ip_addr(char *eth, char ip[INET_ADDRSTRLEN])
{
struct ifreq ifr;
int sockfd ;
struct sockaddr_in sa ;
struct in_addr ip_addr ;
sockfd = socket(AF_INET, SOCK_DGRAM, 0) ;
if(sockfd < 0)
perror("Cannot open socket to read IP address") ;
strncpy(ifr.ifr_name, eth, IFNAMSIZ) ;
if (ioctl(sockfd, SIOCGIFFLAGS, &ifr) < 0)
return ;
if (ioctl(sockfd, SIOCGIFADDR, &ifr) < 0)
return ;
memcpy(&ip_addr, &ifr.ifr_addr.sa_data[sizeof(sa.sin_port)],
sizeof(ip_addr)) ;
strncpy(ip, inet_ntoa(ip_addr), INET_ADDRSTRLEN) ;
}
/* *** Autocalibration functions *** */
#ifdef USE_PTHREAD
void* autocalibrate_hello(void *NULL_value)
{
int send_sockfd ;
struct sockaddr_in serv;
owl_autocalibration_hello message ;
if (GET_VERBOSE())
fprintf(stderr, "Autocalibration Hello thread launched.\n") ;
send_sockfd =
owlclient_create_trx_socket(GET_AGGREGATION_IP(),
GET_AUTOCALIBRATION_PORT(),
&serv, NULL) ;
pthread_cleanup_push(&owl_close_fd, &send_sockfd) ;
memcpy(&message.ap_mac_addr_bytes, my_mac_bytes, ETHER_ADDR_LEN) ;
while (owl_run)
{
owlclient_send_packet(send_sockfd, &serv,
&message, sizeof(message)) ;
sleep(GET_AUTOCALIBRATION_HELLO_DELAY()) ;
}
/* Close the socket */
pthread_cleanup_pop(1) ;
pthread_exit(NULL_value) ;
}
void* autocalibrate(void *NULL_value)
{
int nread ; // recvfrom return value
struct sockaddr_in client; // UDP client structure
socklen_t client_len = sizeof(client) ; // Size of clients
int listen_sockfd ;
owl_autocalibration_order message ;
if (GET_VERBOSE())
fprintf(stderr, "Autocalibration thread launched.\n") ;
// Socket to send autocalibration positioning requests
autocalibration_send_sockfd =
owlclient_create_trx_socket(GET_AUTOCALIBRATION_IP(),
GET_AUTOCALIBRATION_REQUEST_PORT(),
&autocalibration_send_server,
GET_WIFI_IFACE()) ;
// Socket to receive orders from the aggregator
listen_sockfd =
owl_create_udp_listening_socket(GET_AUTOCALIBRATION_PORT()) ;
if (listen_sockfd < 0)
{
perror("Error! Cannot create UDP listening socket from the"
" aggregation server") ;
exit(ERR_CREATING_SOCKET) ;
}
pthread_cleanup_push(&owl_close_fd, &listen_sockfd) ;
while (owl_run)
{
nread = recvfrom(listen_sockfd, &message, sizeof(message), 0,
(struct sockaddr *) &client, &client_len) ;
if (nread <= 0 && owl_run)
{
if (owl_run)
fprintf(stderr, "No message received from aggregator!\n") ;
continue ;
}
if (message.order == AUTOCALIBRATION_ORDER_SEND)
{
if (GET_VERBOSE())
fprintf(stderr, "I was just ordered to send an"
" autocalibration request...\n") ;
send_autocalibration_request() ;
}
else
fprintf(stderr,
"Autocalibration order unknown: %d.\n", message.order) ;
}
/* Close the socket */
pthread_cleanup_pop(1) ;
pthread_exit(NULL_value) ;
}
void send_autocalibration_request()
{
uint8_t *packet ;
uint_fast16_t packet_size = make_packet(&packet) ;
owlclient_send_request(autocalibration_send_sockfd,
&autocalibration_send_server,
packet, packet_size,
GET_AUTOCALIBRATION_NBPKT(),
GET_AUTOCALIBRATION_DELAY()) ;
free(packet) ;
}
/*
* Creates the calibration packet to send.
* The packet must be freed by the calling function.
* Returns the size of the packet.
*/
uint_fast16_t make_packet(uint8_t **packet)
{
uint8_t *pkt ;
uint_fast16_t size ; // Packet size
uint_fast16_t offset ; // Index used to create the packet
owl_timestamp request_time ;
float
my_position_x = owl_htonf(GET_MY_POSITION_X()),
my_position_y = owl_htonf(GET_MY_POSITION_Y()),
my_position_z = owl_htonf(GET_MY_POSITION_Z()) ;
owl_timestamp_now(&request_time) ;
if (GET_VERBOSE())
{
char request_time_str[OWL_TIMESTAMP_STR_LEN] ;
owl_timestamp_to_string(request_time_str, request_time) ;
printf("Autocalibration time: %s\n", request_time_str) ;
}
request_time = owl_hton_timestamp(request_time) ;
offset = 0 ;
size =
sizeof(uint8_t) * 2 + sizeof(owl_timestamp) + sizeof(float) * 3 ;
pkt = malloc(size) ;
// Request type:
memset(&pkt[offset], OWL_REQUEST_AUTOCALIBRATION, 1) ;
++offset ;
// Timestamp:
memcpy(&pkt[offset], &request_time, sizeof(request_time)) ;
offset += sizeof(request_time) ;
// Coordinates:
pkt[offset] = GET_MY_DIRECTION() ;
++offset ;
memcpy(&pkt[offset], &my_position_x, sizeof(float)) ;
offset += sizeof(float) ;
memcpy(&pkt[offset], &my_position_y, sizeof(float)) ;
offset += sizeof(float) ;
memcpy(&pkt[offset], &my_position_z, sizeof(float)) ;
#ifdef DEBUG
offset += sizeof(float) ;
assert(offset == size) ;
#endif // DEBUG
*packet = pkt ;
return size ;
}
#endif // USE_PTHREAD
/* *** End of autocalibration functions *** */
void sigint_handler(int num)
{
owl_sigint_handler(num) ;
pcap_breakloop(capture_handler) ;
}
void sigterm_handler(int num)
{
owl_sigterm_handler(num) ;
pcap_breakloop(capture_handler) ;
}
void print_usage()
{
printf("Usage :\n"
"\t%s [-f config_file] [-m mode] [-d aggregation_ip]"
" [-l listening_port] [-p aggregation_port] -r rtap_iface"
" [-w wifi_iface] [-k] [-v | -q] [-c | -Q] [-A]"
" [-D autocalibration_ip]"
" [-P autocalibration_request_port ] [-a autocalibration_port]"
" [-H autocalibration_hello_delay] [-t autocalibration_delay]"
" [-n autocalibration_nb_packets] [direction x y z]\n"
"\t%s -h\n"
"\t%s -V\n"
"Main options:\n"
"\t-h\t\tPrint this help.\n"
"\t-V\t\tShow version information.\n"
"\t-f config_file\tUse 'config_file' instead of the default"
" configuration file (%s). Available only if program was"
" compiled with libconfuse.\n"
"Capture options:\n"
"\t-m mode\t\t\tCapture mode: a(ctive), p(assive), m(ixed)"
" (default: a).\n"
"\t-l listening_port\tPort on which explicit positioning"
" requests are sent by mobiles (default: %d).\n"
"\t-d aggregation_ip\tIP address of the aggregation server"
" (default: 127.0.0.1)\n"
"\t-p aggregation_port\tRequests are transmitted to the"
" aggregation server on this port (default: %d).\n"
"\t-r rtap_iface\t\tRadiotap capture interface.\n"
"\t-w wifi_iface\t\tPhysical interface behind rtap_iface"
" (default: rtap_iface).\n"
"Autocalibration options:\n"
"(These options are available only if the program was compiled"
" with support of POSIX threads.)\n"
"\t-A\t\t\tEnable autocalibration (default: disabled).\n"
"\t-D autocalib_ip\t\tDestination IP of the autocalibration"
"requests (default: aggregation_ip).\n"
"\t-P autocalib_req_port\tPort on which autocalibration"
" positioning requests are sent (default: %d).\n"
"\t-a autocalib_port\tPort on which autocalibration data (hello"
" & orders) are exchanged with the aggregation server"
" (default: %d).\n"
"\t-H hello_delay\t\tTime between each hello"
" message sent to the aggregation server (default: %d s).\n"
"\t-t delay\t\tTime between each autocalibration"
" packet transmission (default: %d µs).\n"
"\t-n nb_packets\t\tNumber of packet transmitted"
" for one autocalibration request (default: %d).\n"
"\tdirection x y z\t\tThe coordinates of the listener"
" (direction is an integer; x, y, z are floats).\n"
"Other options:\n"
"\t-k\tKeep the monitor mode up on wifi_iface. Use it with buggy"
" drivers that disable monitor mode periodically. Available"
" only if the program was compiled with support of POSIX"
" threads.\n"
"\t-v\tVerbose mode (display what we do).\n"
"\t-q\tQuiet mode (default).\n"
"\t-c\tDisplay captured packets.\n"
"\t-Q\tDo not display captured packets (default).\n"
,
program_name,
program_name,
program_name,
DEFAULT_CONFIG_FILE,
LOC_REQUEST_DEFAULT_PORT,
AGGREGATE_DEFAULT_PORT,
DEFAULT_AUTOCALIBRATION_REQUEST_PORT,
DEFAULT_AUTOCALIBRATION_PORT,
DEFAULT_AUTOCALIBRATION_HELLO_DELAY,
DEFAULT_AUTOCALIBRATION_DELAY,
DEFAULT_AUTOCALIBRATION_NBPKT
) ;
}
void print_version()
{
printf("This is OwlPS Listener, part of the Open Wireless Positioning"
" System project.\n"
"Version: %s.\n"
"Compilation-time options:\n"
"\tSupport for POSIX threads: %s.\n"
"\tSupport for configuration file (libconfuse): %s.\n",
#ifdef OWLPS_VERSION
OWLPS_VERSION
#else // OWLPS_VERSION
"unknown version"
#endif // OWLPS_VERSION
,
#ifdef USE_PTHREAD
"YES"
#else // USE_PTHREAD
"NO"
#endif // USE_PTHREAD
,
#ifdef USE_CONFIG_FILE
"YES"
#else // USE_CONFIG_FILE
"NO"
#endif // USE_CONFIG_FILE
) ;
}
Reference in New Issue View Git Blame Copy Permalink