Owl Positioning System: a Wi-Fi-based, infrastructure-centred indoor positioning system. http://owlps.pu-pm.univ-fcomte.fr/
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

629 lines
15KB

  1. /*
  2. * This file is part of the Owl Positioning System (OwlPS) project.
  3. * It is subject to the copyright notice and license terms in the
  4. * COPYRIGHT.t2t file found in the top-level directory of this
  5. * distribution and at
  6. * http://code.lm7.fr/p/owlps/source/tree/master/COPYRIGHT.t2t
  7. * No part of the OwlPS Project, including this file, may be copied,
  8. * modified, propagated, or distributed except according to the terms
  9. * contained in the COPYRIGHT.t2t file; the COPYRIGHT.t2t file must be
  10. * distributed along with this file, either separately or by replacing
  11. * this notice by the COPYRIGHT.t2t file's contents.
  12. *
  13. ***********************************************************************
  14. *
  15. * This is the main source file of libowlps.
  16. */
  17. #include "owlps.h"
  18. #include <stdio.h>
  19. #include <stdlib.h>
  20. #include <unistd.h>
  21. #include <inttypes.h>
  22. #include <string.h>
  23. #include <time.h>
  24. #include <sys/time.h>
  25. #include <signal.h>
  26. #include <arpa/inet.h>
  27. #include <netdb.h>
  28. #include <assert.h>
  29. bool owl_run = true ;
  30. /* *** Miscellaneous functions *** */
  31. /**
  32. * Converts a MAC address from bytes to character string.
  33. * The string is allocated in a static buffer, and will be overwritten
  34. * each time this function is called.
  35. * **This function is not thread-safe!** Please use the reentrant version
  36. * owl_mac_bytes_to_string_r() if concurrent calls are possible.
  37. *
  38. * @returns A pointer to the static buffer containing the character
  39. * string. This pointer must *not* be freed manually by the caller.
  40. */
  41. const char* owl_mac_bytes_to_string(const uint8_t *const mac_binary)
  42. {
  43. static char mac_str[OWL_ETHER_ADDR_STRLEN] ;
  44. owl_mac_bytes_to_string_r(mac_binary, mac_str) ;
  45. return mac_str ;
  46. }
  47. /**
  48. * Converts a MAC address from bytes to character string.
  49. * The `mac_str` buffer must be allocated by the caller and its length
  50. * must be of at least `OWL_ETHER_ADDR_STRLEN`.
  51. * This function is thread-safe.
  52. */
  53. void owl_mac_bytes_to_string_r(const uint8_t *const mac_binary,
  54. char mac_str[OWL_ETHER_ADDR_STRLEN])
  55. {
  56. snprintf(mac_str, OWL_ETHER_ADDR_STRLEN,
  57. "%02x:%02x:%02x:%02x:%02x:%02x",
  58. mac_binary[0], mac_binary[1], mac_binary[2],
  59. mac_binary[3], mac_binary[4], mac_binary[5]) ;
  60. }
  61. /**
  62. * Compares two MAC addresses.
  63. *
  64. * @returns `true` if the two addresses are identical, `false` otherwise.
  65. */
  66. bool owl_mac_equals(const uint8_t *const mac1,
  67. const uint8_t *const mac2)
  68. {
  69. int i ;
  70. for (i = ETHER_ADDR_LEN - 1 ; i >= 0 ; --i)
  71. if(mac1[i] != mac2[i])
  72. return false ;
  73. return true ;
  74. }
  75. /**
  76. * Converts a IEEE 802.11 frequency (in MHz) into a channel number.
  77. *
  78. * @returns The channel number corresponding to `frequency`.
  79. * @returns 0 if the frequency does not correspond to an official
  80. * channel.
  81. */
  82. uint_fast8_t owl_frequency_to_channel(const uint_fast16_t frequency)
  83. {
  84. switch (frequency)
  85. {
  86. case OWL_80211_MHZ_CHANNEL_1 :
  87. return 1 ;
  88. case OWL_80211_MHZ_CHANNEL_2 :
  89. return 2 ;
  90. case OWL_80211_MHZ_CHANNEL_3 :
  91. return 3 ;
  92. case OWL_80211_MHZ_CHANNEL_4 :
  93. return 4 ;
  94. case OWL_80211_MHZ_CHANNEL_5 :
  95. return 5 ;
  96. case OWL_80211_MHZ_CHANNEL_6 :
  97. return 6 ;
  98. case OWL_80211_MHZ_CHANNEL_7 :
  99. return 7 ;
  100. case OWL_80211_MHZ_CHANNEL_8 :
  101. return 8 ;
  102. case OWL_80211_MHZ_CHANNEL_9 :
  103. return 9 ;
  104. case OWL_80211_MHZ_CHANNEL_10 :
  105. return 10 ;
  106. case OWL_80211_MHZ_CHANNEL_11 :
  107. return 11 ;
  108. case OWL_80211_MHZ_CHANNEL_12 :
  109. return 12 ;
  110. case OWL_80211_MHZ_CHANNEL_13 :
  111. return 13 ;
  112. case OWL_80211_MHZ_CHANNEL_14 :
  113. return 14 ;
  114. }
  115. return 0 ;
  116. }
  117. /* *** Time *** */
  118. /**
  119. * Sleeps for a given amount of milliseconds.
  120. * `time_ms` is an unsigned value, so please be careful: passing a
  121. * negative value may not do what you think.
  122. * In case of error, a message is displayed on the standard error.
  123. *
  124. * @returns 0 if everything went well.
  125. * @returns A non-zero error code in case of error; if positive, it is
  126. * the number of non-slept seconds.
  127. */
  128. int owl_msleep(uint32_t time_ms)
  129. {
  130. int ret ;
  131. uint_fast32_t seconds, microseconds ;
  132. if (! time_ms)
  133. return 0 ;
  134. seconds = time_ms / 1000 ;
  135. microseconds = time_ms % 1000 * 1000 ;
  136. if ((ret = sleep(seconds)))
  137. {
  138. perror("Cannot sleep()") ;
  139. return ret ;
  140. }
  141. if ((ret = usleep(microseconds)))
  142. {
  143. perror("Cannot usleep()") ;
  144. return ret ;
  145. }
  146. return 0 ;
  147. }
  148. /**
  149. * Sets the `owl_timestamp` `now` to the current time.
  150. *
  151. * @returns 0 in case of success, non-zero otherwise.
  152. */
  153. int owl_timestamp_now(owl_timestamp *const now)
  154. {
  155. int ret ;
  156. #if _POSIX_TIMERS > 0
  157. struct timespec now_ts ;
  158. ret = clock_gettime(CLOCK_REALTIME, &now_ts) ;
  159. #else // _POSIX_TIMERS
  160. struct timeval now_ts ;
  161. ret = gettimeofday(&now_ts, NULL) ;
  162. #endif // _POSIX_TIMERS
  163. if (ret)
  164. {
  165. perror("Cannot get the current time") ;
  166. return ret ;
  167. }
  168. #if _POSIX_TIMERS > 0
  169. owl_timespec_to_timestamp(&now_ts, now) ;
  170. #else // _POSIX_TIMERS
  171. owl_timeval_to_timestamp(&now_ts, now) ;
  172. #endif // _POSIX_TIMERS
  173. return 0 ;
  174. }
  175. /**
  176. * Converts the `struct timespec` `src` into the `owl_timestamp` `dst`.
  177. * `src` and `dst` must not be `NULL`.
  178. */
  179. void owl_timespec_to_timestamp(const struct timespec *const src,
  180. owl_timestamp *const dst)
  181. {
  182. assert(src) ;
  183. assert(dst) ;
  184. dst->tv_sec = src->tv_sec ;
  185. dst->tv_nsec = src->tv_nsec ;
  186. }
  187. /**
  188. * Converts the `struct timeval` `src` into the `owl_timestamp` `dst`.
  189. * `src` and `dst` must not be `NULL`.
  190. */
  191. void owl_timeval_to_timestamp(const struct timeval *const src,
  192. owl_timestamp *const dst)
  193. {
  194. assert(src) ;
  195. assert(dst) ;
  196. dst->tv_sec = src->tv_sec ;
  197. dst->tv_nsec = src->tv_usec * 1000u ;
  198. }
  199. /**
  200. * Compares two `owl_timestamp` and returns `true` if they are equal,
  201. * `false` otherwise.
  202. * `d1` and `d2` must not be `NULL`.
  203. */
  204. bool owl_timestamp_equals(const owl_timestamp *const d1,
  205. const owl_timestamp *const d2)
  206. {
  207. assert(d1) ;
  208. assert(d2) ;
  209. return d1->tv_sec == d2->tv_sec && d1->tv_nsec == d2->tv_nsec ;
  210. }
  211. /**
  212. * Converts the `owl_timestamp` date value `d` into milliseconds.
  213. * `d` must not be `NULL`.
  214. *
  215. * @returns The millisecond value of `d`.
  216. */
  217. uint64_t owl_timestamp_to_ms(const owl_timestamp *const d)
  218. {
  219. assert(d) ;
  220. return
  221. (uint64_t) d->tv_sec * 1000u + (uint64_t) d->tv_nsec / 1000000lu ;
  222. }
  223. /**
  224. * Converts the `owl_timestamp` date value `src` into a printable string.
  225. * `src` must not be `NULL`.
  226. * `dst` must be an allocated array of at least `OWL_TIMESTAMP_STRLEN`
  227. * characters.
  228. */
  229. void owl_timestamp_to_string(const owl_timestamp *const src,
  230. char *const dst)
  231. {
  232. assert(src) ;
  233. assert(dst) ;
  234. snprintf(dst, OWL_TIMESTAMP_STRLEN, "%"PRIu32".%09"PRIu32,
  235. src->tv_sec, src->tv_nsec) ;
  236. }
  237. /**
  238. * Returns the time (in milliseconds) elapsed between two `owl_timestamp`
  239. * `d1` and `d2`. `d1` can either be lower (before) or greater (after)
  240. * `d2`, the returned value will always be the delay between the older
  241. * and the newer time.
  242. * `d1` and `d2` must not be NULL.
  243. */
  244. uint_fast32_t owl_time_elapsed_ms(const owl_timestamp *const d1,
  245. const owl_timestamp *const d2)
  246. {
  247. owl_timestamp elapsed ;
  248. owl_time_elapsed(d1, d2, &elapsed) ;
  249. return owl_timestamp_to_ms(&elapsed) ;
  250. }
  251. /**
  252. * Computes the time difference between two `owl_timestamp` `d1` and
  253. * `d2`. The result is stored in the `elapsed` parameter.
  254. * Note that it is a delay, not a simple substraction, therefore the
  255. * result is always positive.
  256. * NULL parameters are not accepted.
  257. */
  258. void owl_time_elapsed(const owl_timestamp *const d1,
  259. const owl_timestamp *const d2,
  260. owl_timestamp *const elapsed)
  261. {
  262. int_fast32_t sec, nsec ;
  263. assert(d1) ;
  264. assert(d2) ;
  265. assert(elapsed) ;
  266. sec = (int_fast64_t) d1->tv_sec - d2->tv_sec ;
  267. nsec = (int_fast64_t) d1->tv_nsec - d2->tv_nsec ;
  268. if (sec == 0)
  269. {
  270. elapsed->tv_sec = 0 ;
  271. elapsed->tv_nsec = abs(nsec) ;
  272. }
  273. else if (sec > 0)
  274. {
  275. if (nsec >= 0)
  276. {
  277. elapsed->tv_sec = sec ;
  278. elapsed->tv_nsec = nsec ;
  279. }
  280. else // nsec < 0
  281. {
  282. elapsed->tv_sec = sec - 1 ;
  283. elapsed->tv_nsec = nsec + 1000000000ul ;
  284. }
  285. }
  286. else // sec < 0
  287. {
  288. if (nsec > 0)
  289. {
  290. elapsed->tv_sec = abs(sec) - 1 ;
  291. elapsed->tv_nsec = 1000000000ul - nsec ;
  292. }
  293. else // nsec <= 0
  294. {
  295. elapsed->tv_sec = abs(sec) ;
  296. elapsed->tv_nsec = abs(nsec) ;
  297. }
  298. }
  299. }
  300. /* *** Endianness *** */
  301. /**
  302. * Converts an `owl_timestamp` from host endianness to network
  303. * endianness.
  304. * The conversion is done in-place. `d` must not be `NULL`.
  305. */
  306. void owl_hton_timestamp(owl_timestamp *const d)
  307. {
  308. assert(d) ;
  309. d->tv_sec = htonl(d->tv_sec) ;
  310. d->tv_nsec = htonl(d->tv_nsec) ;
  311. }
  312. /**
  313. * Converts an `owl_timestamp` from network endianness to host
  314. * endianness.
  315. * The conversion is done in-place. `d` must not be `NULL`.
  316. */
  317. void owl_ntoh_timestamp(owl_timestamp *const d)
  318. {
  319. assert(d) ;
  320. d->tv_sec = ntohl(d->tv_sec) ;
  321. d->tv_nsec = ntohl(d->tv_nsec) ;
  322. }
  323. /**
  324. * This function swaps the bytes composing a `float`, i.e. it changes
  325. * their order.
  326. * You probably want to use the owl_htonf() and owl_ntohf() macros
  327. * instead of this function.
  328. *
  329. * @returns The swapped `float`.
  330. */
  331. float owl_swap_float(const float f)
  332. {
  333. float ret ;
  334. char
  335. *f_bytes = (char*) &f,
  336. *ret_bytes = (char*) &ret ;
  337. assert(sizeof(float) == 4) ;
  338. ret_bytes[0] = f_bytes[3] ;
  339. ret_bytes[1] = f_bytes[2] ;
  340. ret_bytes[2] = f_bytes[1] ;
  341. ret_bytes[3] = f_bytes[0] ;
  342. return ret ;
  343. }
  344. /* *** Network *** */
  345. /**
  346. * Opens a UDP transmission socket and returns its descriptor.
  347. *
  348. * @param[in] server_address The server's IP address.
  349. * @param[in] server_port The listening port on the server.
  350. * @param[out] server_description The structure in which the server
  351. * description will be saved.
  352. *
  353. * @returns A file descriptor to the opened socket in case of success,
  354. * or a negative error code.
  355. */
  356. int owl_create_udp_trx_socket(const char *const server_address,
  357. const uint_fast16_t server_port,
  358. struct sockaddr *const server_description)
  359. {
  360. char server_port_str[6] ;
  361. struct addrinfo
  362. gai_hints,
  363. *gai_results = NULL,
  364. *gai_res = NULL ;
  365. int gai_ret ; // Return value of getaddrinfo()
  366. int sockfd = -1 ; // Socket descriptor
  367. /* Get the server information */
  368. sprintf(server_port_str, "%"PRIuFAST16, server_port) ;
  369. memset(&gai_hints, 0, sizeof(struct addrinfo)) ;
  370. gai_hints.ai_family = AF_INET ; // IPv4 only
  371. gai_hints.ai_socktype = SOCK_DGRAM ;
  372. gai_ret = getaddrinfo(server_address, server_port_str,
  373. &gai_hints, &gai_results) ;
  374. if (gai_ret)
  375. {
  376. fprintf(stderr, "UDP socket creation failed: getaddrinfo(): %s\n",
  377. gai_strerror(gai_ret)) ;
  378. return -OWL_ERR_SOCKET_CREATE ;
  379. }
  380. /* Create the UDP socket: loop until socket() succeeds */
  381. for (gai_res = gai_results ; gai_res != NULL ;
  382. gai_res = gai_res->ai_next)
  383. {
  384. sockfd = socket(gai_res->ai_family, gai_res->ai_socktype,
  385. gai_res->ai_protocol) ;
  386. if (sockfd != -1)
  387. break ;
  388. }
  389. if (gai_res == NULL)
  390. {
  391. fprintf(stderr,
  392. "UDP socket creation failed: socket().\n") ;
  393. return -OWL_ERR_SOCKET_CREATE ;
  394. }
  395. /* Copy the server description */
  396. memcpy(server_description, gai_res->ai_addr, gai_res->ai_addrlen) ;
  397. freeaddrinfo(gai_results) ;
  398. return sockfd ;
  399. }
  400. /**
  401. * Opens a UDP reception socket and returns its descriptor.
  402. * `port` is the port on which the socket will listen.
  403. *
  404. * @returns A file descriptor to the opened socket in case of success,
  405. * or a negative error code.
  406. */
  407. int owl_create_udp_listening_socket(const uint_fast16_t port)
  408. {
  409. char port_str[6] ;
  410. struct addrinfo
  411. gai_hints,
  412. *gai_results = NULL,
  413. *gai_res = NULL ;
  414. int gai_ret ; // Return value of getaddrinfo()
  415. int sockfd ; // Socket descriptor
  416. /* Get the server information */
  417. sprintf(port_str, "%"PRIuFAST16, port) ;
  418. memset(&gai_hints, 0, sizeof(struct addrinfo)) ;
  419. gai_hints.ai_family = AF_INET ; // IPv4 only
  420. gai_hints.ai_socktype = SOCK_DGRAM ;
  421. gai_hints.ai_flags = AI_PASSIVE ;
  422. gai_ret = getaddrinfo(NULL, port_str, &gai_hints, &gai_results) ;
  423. if (gai_ret)
  424. {
  425. fprintf(stderr, "UDP socket creation failed: getaddrinfo(): %s\n",
  426. gai_strerror(gai_ret)) ;
  427. return -OWL_ERR_SOCKET_CREATE ;
  428. }
  429. /* Create the UDP socket:
  430. * loop until both socket() and bind() succeed */
  431. for (gai_res = gai_results ; gai_res != NULL ;
  432. gai_res = gai_res->ai_next)
  433. {
  434. sockfd = socket(gai_res->ai_family, gai_res->ai_socktype,
  435. gai_res->ai_protocol) ;
  436. if (sockfd == -1)
  437. continue ;
  438. if (! bind(sockfd, gai_res->ai_addr, gai_res->ai_addrlen))
  439. break ; // Success!
  440. close(sockfd) ;
  441. }
  442. if (gai_res == NULL)
  443. {
  444. fprintf(stderr,
  445. "UDP socket creation failed: socket() or bind().\n") ;
  446. return -OWL_ERR_SOCKET_CREATE ;
  447. }
  448. return sockfd ;
  449. }
  450. /* *** Signals *** */
  451. /**
  452. * Generic signal handler for SIGINT.
  453. *
  454. * @param num The catched signal. This function will exit with
  455. * `OWL_ERR_BAD_SIGNAL` if `num != SIGINT`.
  456. */
  457. void owl_sigint_handler(const int num)
  458. {
  459. if (num != SIGINT)
  460. {
  461. fprintf(stderr, "Error! The SIGINT handler was called but the"
  462. " signal is not SIGINT.\n") ;
  463. exit(OWL_ERR_BAD_SIGNAL) ;
  464. }
  465. owl_run = false ;
  466. #ifndef NDEBUG
  467. fprintf(stderr, "\nSignal received: end.\n");
  468. #endif // NDEBUG
  469. fflush(NULL) ;
  470. }
  471. /**
  472. * Generic signal handler for SIGTERM.
  473. *
  474. * @param num The catched signal. This function will exit with
  475. * `OWL_ERR_BAD_SIGNAL` if `num != SIGTERM`.
  476. */
  477. void owl_sigterm_handler(const int num)
  478. {
  479. if (num != SIGTERM)
  480. {
  481. fprintf(stderr, "Error! The SIGTERM handler was called but the"
  482. " signal is not SIGTERM.\n") ;
  483. exit(OWL_ERR_BAD_SIGNAL) ;
  484. }
  485. owl_sigint_handler(SIGINT) ;
  486. }
  487. /* *** Thread-related functions *** */
  488. /**
  489. * Closes the file descriptor `fd`.
  490. * `fd` must be passed as an int pointer (`int*`). If `fd` is `NULL`,
  491. * nothing will be done. Uppon error, a message is displayed on the
  492. * standard error.
  493. */
  494. void owl_close_fd(void *const fd)
  495. {
  496. if (fd == NULL)
  497. return ;
  498. const int *const file_desc = fd ;
  499. if (close(*file_desc) != 0)
  500. perror("Error closing file descriptor") ;
  501. #ifndef NDEBUG
  502. else
  503. fprintf(stderr, "File descriptor %d closed successfully.\n",
  504. *file_desc) ;
  505. #endif // NDEBUG
  506. }
  507. /**
  508. * Closes the stream `file`.
  509. * `file` must be passed as a pointer on a pointer of FILE (FILE**).
  510. * If `*file` is either `stdout`, `stderr` or `stdin`, it will not be
  511. * closed.
  512. */
  513. void owl_close_file(void *const file)
  514. {
  515. if (file == NULL)
  516. return ;
  517. FILE **stream = file ;
  518. if (*stream == stdout || *stream == stderr || *stream == stdin)
  519. return ;
  520. if (fclose(*stream) != 0)
  521. perror("Error closing stream") ;
  522. #ifndef NDEBUG
  523. else
  524. fprintf(stderr, "Stream closed successfully.\n") ;
  525. #endif // NDEBUG
  526. }