1145 lines
34 KiB
C
1145 lines
34 KiB
C
/*
|
|
* This file is part of the rtap localisation project.
|
|
*/
|
|
|
|
|
|
#include "owlps-listener.h"
|
|
|
|
|
|
|
|
char *program_name = NULL ;
|
|
uint8_t my_mac_bytes[6] ; // AP MAC address
|
|
char my_ip[16] ; // AP IP address
|
|
|
|
int aggregation_sockfd ;
|
|
struct sockaddr_in aggregation_server ;
|
|
int autocalibration_send_sockfd ;
|
|
struct sockaddr_in autocalibration_send_server ;
|
|
|
|
#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[16] ;
|
|
uint_fast16_t aggregation_port ;
|
|
uint_fast16_t listening_port ;
|
|
#ifdef USE_PTHREAD
|
|
BOOL keep_monitor ;
|
|
#endif // USE_PTHREAD
|
|
char rtap_iface[IFNAMSIZ + 1] ;
|
|
char wifi_iface[IFNAMSIZ + 1] ;
|
|
#ifdef USE_PTHREAD
|
|
BOOL autocalibration ;
|
|
char autocalibration_ip[16] ;
|
|
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 ;
|
|
#endif // USE_PTHREAD
|
|
BOOL verbose ;
|
|
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,
|
|
#endif // USE_PTHREAD
|
|
FALSE,
|
|
FALSE
|
|
} ;
|
|
#endif // USE_CONFIG_FILE
|
|
|
|
|
|
|
|
int main(int argc, char *argv[])
|
|
{
|
|
struct sigaction action ; // Signal handler structure
|
|
char *mac_string ; // AP MAC (string value)
|
|
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) ;
|
|
|
|
run = TRUE ;
|
|
|
|
/* Set up signal handlers */
|
|
sigemptyset(&action.sa_mask) ;
|
|
action.sa_handler = owl_sigint_handler ;
|
|
sigaction(SIGINT, &action, NULL) ;
|
|
action.sa_handler = owl_sigterm_handler ;
|
|
sigaction(SIGTERM, &action, NULL) ;
|
|
|
|
get_mac_addr(GET_WIFI_IFACE(), my_mac_bytes) ;
|
|
mac_string = owl_mac_bytes_to_string(my_mac_bytes) ;
|
|
printf("My MAC address is: %s\n", mac_string) ;
|
|
free(mac_string) ;
|
|
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())
|
|
pthread_create(&keep_monitor_thread, NULL,
|
|
(void *) &keep_mode_monitor, GET_WIFI_IFACE()) ;
|
|
if (GET_AUTOCALIBRATION())
|
|
{
|
|
pthread_create(&autocalibration_thread, NULL,
|
|
(void *) &autocalibrate, NULL) ;
|
|
pthread_create(&autocalibration_hello_thread, NULL,
|
|
(void *) &autocalibrate_hello, NULL) ;
|
|
}
|
|
#endif // USE_PTHREAD
|
|
|
|
ret = capture() ; // Capture loop
|
|
|
|
#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),
|
|
#endif // USE_PTHREAD
|
|
// Be verbose, or not:
|
|
CFG_BOOL("verbose", cfg_false, 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
|
|
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)
|
|
{
|
|
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' :
|
|
SET_AUTOCALIBRATION_IP(optarg) ;
|
|
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' :
|
|
UNSET_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' :
|
|
SET_VERBOSE() ;
|
|
break ;
|
|
case 'V' : // Version
|
|
break ; // (already parsed)
|
|
case 'w' :
|
|
SET_WIFI_IFACE(optarg) ;
|
|
break ;
|
|
default :
|
|
print_usage() ;
|
|
exit(ERR_BAD_USAGE) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
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()) ;
|
|
}
|
|
|
|
if (GET_AUTOCALIBRATION() && 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") ;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
#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"
|
|
#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
|
|
BOOL_TO_STRING(GET_KEEP_MONITOR()),
|
|
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(),
|
|
#endif // USE_PTHREAD
|
|
BOOL_TO_STRING(GET_VERBOSE()),
|
|
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(char *iface)
|
|
{
|
|
if (GET_VERBOSE())
|
|
fprintf(stderr, "Thread for keeping monitor mode launched.\n") ;
|
|
|
|
while (run)
|
|
{
|
|
// Switch the interface to monitor mode:
|
|
owl_iface_mode_monitor(iface) ;
|
|
sleep(1) ; // Wait for 1 second
|
|
}
|
|
}
|
|
#endif // USE_PTHREAD
|
|
|
|
|
|
|
|
/*
|
|
* Captures packets using the radiotap interface.
|
|
* Captured data is transmitted to the aggregator.
|
|
*/
|
|
int capture()
|
|
{
|
|
pcap_t *handle ; // Packet capture descriptor
|
|
char errbuf[PCAP_ERRBUF_SIZE] ; // Error message
|
|
|
|
// Start capture:
|
|
handle = pcap_open_live(GET_RTAP_IFACE(), BUFSIZ, 1, 1000, errbuf) ;
|
|
if (handle == 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(run)
|
|
// Capture one packet at time, and call read_packet() on it:
|
|
pcap_loop(handle, 1, read_packet, NULL) ;
|
|
|
|
pcap_close(handle) ; // Stop capture
|
|
(void) 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 ;
|
|
couple_message couple ; // Message to send to the aggregator
|
|
ssize_t nsent ; // sendto return value
|
|
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):
|
|
uint8_t packet_type ;
|
|
BOOL is_explicit_packet = TRUE ; // Is the packet an explicit request?
|
|
// Is the packet an autocalibration positioning request?
|
|
BOOL uses_autocalibration_request_port = FALSE ;
|
|
int i ; // Iterator
|
|
|
|
memset(couple.mobile_ip_addr_bytes, 0, 4) ; // Blank the IP
|
|
|
|
/* 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 != ETH_P_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(couple.mobile_ip_addr_bytes, &packet_ip_header->saddr, 4) ;
|
|
|
|
if (GET_MODE() != MODE_PASSIVE) // If mode is active or mixed
|
|
{
|
|
// 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)] ;
|
|
|
|
if (GET_AUTOCALIBRATION() && ntohs(packet_udp_header->dest) ==
|
|
GET_AUTOCALIBRATION_REQUEST_PORT())
|
|
uses_autocalibration_request_port = TRUE ;
|
|
|
|
else if (ntohs(packet_udp_header->dest) != 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(couple.mobile_mac_addr_bytes, &packet[rtap_bytes+10], 6) ;
|
|
|
|
// Drop the packet if it comes from the AP itself:
|
|
if (owl_mac_equals(my_mac_bytes, couple.mobile_mac_addr_bytes))
|
|
return ;
|
|
|
|
memcpy(couple.ap_mac_addr_bytes, my_mac_bytes, 6) ; // Copy AP MAC
|
|
// Capture time is in the pcap header:
|
|
couple.start_time = owl_timeval_to_timestamp(header->ts) ;
|
|
// Transmission time on the mobile is unknown (unless the packet is
|
|
// an explicit request):
|
|
memset(&couple.request_time, 0, sizeof(TIMESTAMP)) ;
|
|
// Blank position data:
|
|
couple.direction = 0 ;
|
|
couple.x_position = 0 ;
|
|
couple.y_position = 0 ;
|
|
couple.z_position = 0 ;
|
|
|
|
/* 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))
|
|
{
|
|
packet_type =
|
|
packet[rtap_bytes + ieee80211_header_size + LLC_HEADER_SIZE
|
|
+ sizeof(struct iphdr) + sizeof(struct udphdr)] ;
|
|
switch(packet_type)
|
|
{
|
|
case PACKET_TYPE_NORMAL :
|
|
if (GET_VERBOSE())
|
|
printf("\nExplicit packet received.\n") ;
|
|
break ;
|
|
|
|
case PACKET_TYPE_CALIBRATION :
|
|
if (GET_VERBOSE())
|
|
printf("\nExplicit calibration packet received.\n") ;
|
|
couple.direction =
|
|
packet[rtap_bytes + ieee80211_header_size + LLC_HEADER_SIZE
|
|
+ sizeof(struct iphdr) + sizeof(struct udphdr) + 9];
|
|
memcpy(&couple.x_position,
|
|
&packet[rtap_bytes + ieee80211_header_size
|
|
+ LLC_HEADER_SIZE + sizeof(struct iphdr)
|
|
+ sizeof(struct udphdr) + 10], sizeof(float)) ;
|
|
memcpy(&couple.y_position,
|
|
&packet[rtap_bytes + ieee80211_header_size
|
|
+ LLC_HEADER_SIZE + sizeof(struct iphdr)
|
|
+ sizeof(struct udphdr) + 14], sizeof(float)) ;
|
|
memcpy(&couple.z_position,
|
|
&packet[rtap_bytes + ieee80211_header_size
|
|
+ LLC_HEADER_SIZE + sizeof(struct iphdr)
|
|
+ sizeof(struct udphdr) + 18], sizeof(float)) ;
|
|
break ;
|
|
|
|
case PACKET_TYPE_AUTOCALIBRATION :
|
|
if (GET_VERBOSE())
|
|
{
|
|
printf("\nAutocalibration packet received.") ;
|
|
if (! uses_autocalibration_request_port)
|
|
printf(".. on the wrong port!") ;
|
|
putchar('\n') ;
|
|
}
|
|
break ;
|
|
|
|
default :
|
|
if (GET_VERBOSE())
|
|
printf("\nStrange explicit packet received\n") ;
|
|
fprintf(stderr,
|
|
"Error! Unknown packet type (%d).\n", packet_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") ;
|
|
}
|
|
else
|
|
memcpy(&couple.request_time,
|
|
&packet[rtap_bytes + ieee80211_header_size +
|
|
LLC_HEADER_SIZE + sizeof(struct iphdr) +
|
|
sizeof(struct udphdr) + 1],
|
|
sizeof(TIMESTAMP)) ;
|
|
}
|
|
|
|
else if (GET_MODE() == MODE_PASSIVE || GET_MODE() == MODE_MIXED)
|
|
{
|
|
if (GET_VERBOSE())
|
|
printf("\nImplicit packet received.\n") ;
|
|
}
|
|
|
|
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(&(couple.antenna_signal_dbm),
|
|
&packet[rtap_position], RTAP_L_ANTENNASIGNALDBM) ;
|
|
check[RTAP_ANTENNASIGNALDBM] = TRUE;
|
|
if (GET_VERBOSE())
|
|
printf("Antenna signal: %d dBm\n",
|
|
couple.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 *ap_mac_string =
|
|
owl_mac_bytes_to_string(couple.ap_mac_addr_bytes) ;
|
|
char *mobile_mac_string =
|
|
owl_mac_bytes_to_string(couple.mobile_mac_addr_bytes) ;
|
|
printf("*** Couple to send ***\n"
|
|
"\tMAC AP: %s\n"
|
|
"\tMobile MAC: %s\n"
|
|
"\tSequence number (request time): %"PRIu64"\n"
|
|
"\tRequest arrival time on the AP: %"PRIu64"\n"
|
|
"\tSignal: %d dBm\n"
|
|
"\tPosition X: %f\n"
|
|
"\tPosition Y: %f\n"
|
|
"\tPosition Z: %f\n"
|
|
"\tDirection: %hhd\n"
|
|
,
|
|
ap_mac_string,
|
|
mobile_mac_string,
|
|
owl_timestamp_to_ms(couple.request_time),
|
|
owl_timestamp_to_ms(couple.start_time),
|
|
couple.antenna_signal_dbm - 0x100,
|
|
couple.x_position,
|
|
couple.y_position,
|
|
couple.z_position,
|
|
couple.direction
|
|
) ;
|
|
free(ap_mac_string) ;
|
|
free(mobile_mac_string) ;
|
|
}
|
|
|
|
/* Send couple to the aggregator */
|
|
nsent =
|
|
sendto(aggregation_sockfd, (void *) &couple, sizeof(couple), 0,
|
|
(struct sockaddr *) &aggregation_server,
|
|
(socklen_t) sizeof(aggregation_server)) ;
|
|
if (nsent != (ssize_t) sizeof(couple))
|
|
{
|
|
perror("Error sending couple 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[6])
|
|
{
|
|
struct ifreq ifr;
|
|
int sockfd ;
|
|
|
|
memset(mac_bytes, 0, sizeof(uint8_t) * 6) ; // Empty mac_bytes
|
|
|
|
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, 6) ;
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
* Get our own IP address and copy it to 'ip'.
|
|
*/
|
|
void get_ip_addr(char *eth, char ip[16])
|
|
{
|
|
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), 16) ;
|
|
}
|
|
|
|
|
|
|
|
/* *** Autocalibration functions *** */
|
|
#ifdef USE_PTHREAD
|
|
|
|
void autocalibrate_hello()
|
|
{
|
|
int send_sockfd ;
|
|
struct sockaddr_in serv;
|
|
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) ;
|
|
|
|
memcpy(&message.ap_mac_addr_bytes, my_mac_bytes, 6) ;
|
|
|
|
while (run)
|
|
{
|
|
owlclient_send_packet(send_sockfd, &serv,
|
|
&message, sizeof(message)) ;
|
|
sleep(GET_AUTOCALIBRATION_HELLO_DELAY()) ;
|
|
}
|
|
|
|
(void) close(send_sockfd) ;
|
|
}
|
|
|
|
|
|
|
|
void autocalibrate()
|
|
{
|
|
int nread ; // recvfrom return value
|
|
struct sockaddr_in client; // UDP client structure
|
|
socklen_t client_len = sizeof(client) ; // Size of clients
|
|
int listen_sockfd ;
|
|
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) ;
|
|
}
|
|
|
|
while (run)
|
|
{
|
|
nread = recvfrom(listen_sockfd, &message, sizeof(message), 0,
|
|
(struct sockaddr *) &client, &client_len) ;
|
|
|
|
if (nread <= 0 && run)
|
|
{
|
|
if (run)
|
|
fprintf(stderr, "No message received from aggregator!\n") ;
|
|
continue ;
|
|
}
|
|
|
|
if (ntohl(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) ;
|
|
}
|
|
|
|
(void) close(listen_sockfd) ;
|
|
}
|
|
|
|
|
|
|
|
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
|
|
TIMESTAMP request_time ;
|
|
|
|
owl_timestamp_now(&request_time) ;
|
|
|
|
if (GET_VERBOSE())
|
|
printf("Autocalibration time: %"PRIu64"\n",
|
|
owl_timestamp_to_ms(request_time)) ;
|
|
|
|
size = sizeof(char) + sizeof(TIMESTAMP) ;
|
|
pkt = malloc(size) ;
|
|
|
|
memset(&pkt[0], PACKET_TYPE_AUTOCALIBRATION, 1) ; // Packet type
|
|
memcpy(&pkt[1], &request_time, sizeof(request_time)) ;
|
|
|
|
*packet = pkt ;
|
|
return size ;
|
|
}
|
|
|
|
#endif // USE_PTHREAD
|
|
/* *** End of autocalibration functions *** */
|
|
|
|
|
|
|
|
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]\n"
|
|
"\t%s -h\n"
|
|
"\t%s -V\n"
|
|
|
|
"Main options:\n"
|
|
"\t-h\t\tPrint this help.\n"
|
|
"\t-V\t\tShow version.\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"
|
|
|
|
"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"
|
|
"\n"
|
|
"Compilation-time options:\n"
|
|
"\tSupport for POSIX threads: %s.\n"
|
|
"\tSupport for configuration file (libconfuse): %s.\n",
|
|
#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
|
|
) ;
|
|
}
|