// TODO : add the duplicate packets support
char *hci_get_RSSI(hci_socket_t *hci_socket, hci_controller_t *hci_controller,
int8_t *file_descriptor,
bt_address_t *mac, uint8_t duration, uint16_t max_rsp) {
if (!hci_controller) {
print_trace(TRACE_ERROR, "hci_get_RSSI : invalid controller reference.\n");
return NULL;
}
if (hci_controller->interrupted) {
hci_resolve_interruption(hci_socket, hci_controller);
}
if (hci_controller->state != HCI_STATE_OPEN) {
print_trace(TRACE_ERROR, "hci_get_RSSI : busy or closed controller.\n");
return NULL;
}
// Resulting string :
char *res = NULL;
// HCI_Filter structure :
struct hci_filter flt;
struct hci_filter old_flt;
// Command parameter for an "OCF_WRITE_INQUIRY_MODE" :
write_inquiry_mode_cp write_cp;
memset(&write_cp, 0, sizeof(write_cp));
// Command parameter for a standard inquiry : "OCF_INQUIRY" :
inquiry_cp cp;
memset (&cp, 0, sizeof(cp));
/* Events buffer : table of unsigned char (range 0-255) allowing us
to catch an HCI-event packet coming in response to one of our
commands packet (thanks to the "read()" function).
*/
unsigned char buf[HCI_MAX_EVENT_SIZE];
unsigned char *event_parameter;
hci_event_hdr event_header;
char new_socket = 0;
char socket_err = 0;
check_hci_socket_ptr(&hci_socket, hci_controller, &new_socket, &socket_err);
if (socket_err) {
return NULL;
}
// Creating our filter :
hci_compute_filter(&flt, EVT_CMD_COMPLETE,
EVT_INQUIRY_RESULT_WITH_RSSI,
EVT_INQUIRY_COMPLETE,
0);
/* Saving the old filter.
This can be helpful in a case where we want to use a same socket for different purposes
(even with multiple threads), we can replace the old filter and re-use it later.
*/
char saved_flt = 1;
if (!new_socket) {
if (get_hci_socket_filter(*hci_socket, &old_flt) < 0) {
saved_flt = 0;
}
}
// Applying the new filter :
if (set_hci_socket_filter(*hci_socket, &flt) < 0) {
goto end;
}
// Configuring the inquiry mode on the open HCI socket :
print_trace(TRACE_DEBUG, "Configuring the inquiry mode...");
write_cp.mode = htobs(0x01); // Inquiry Result format with RSSI (cf p878 spec').
pthread_mutex_lock(&hci_controller_mutex);
hci_change_state(hci_controller, HCI_STATE_WRITING);
if (hci_send_cmd(hci_socket->sock, OGF_HOST_CTL, OCF_WRITE_INQUIRY_MODE,
WRITE_INQUIRY_MODE_CP_SIZE, &write_cp) < 0) {
print_trace(TRACE_ERROR, " [ERROR]\n");
perror("Can't set inquiry mode");
goto end;
}
hci_change_state(hci_controller, HCI_STATE_OPEN);
pthread_mutex_unlock(&hci_controller_mutex);
if (check_cmd_complete(hci_socket, hci_controller)) {
print_trace(TRACE_DEBUG, " [DONE]\n");
} else {
print_trace(TRACE_DEBUG, " [ERROR]\n");
}
// Setting the command parameters for our rssi inquiry :
cp.lap[2] = htobs(0x9e);
cp.lap[1] = htobs(0x8b);
cp.lap[0] = htobs(0x33);
cp.num_rsp = max_rsp;
cp.length = duration;
print_trace(TRACE_INFO, "Starting inquiry with RSSI...");
/* Sending the inquiry's command to the HCI socket :
TRACE_WARNING : According to the spec (p 706), no Command Complete
event will be sent to the host by using this command.
This will only generate an Inquiry Complete event.
*/
pthread_mutex_lock(&hci_controller_mutex);
hci_change_state(hci_controller, HCI_STATE_SCANNING);
if (hci_send_cmd(hci_socket->sock, OGF_LINK_CTL, OCF_INQUIRY, INQUIRY_CP_SIZE, &cp) < 0) {
print_trace(TRACE_ERROR, " [ERROR]\n");
perror("Can't start inquiry");
goto end;
}
print_trace(TRACE_INFO, " [DONE]\n");
hci_change_state(hci_controller, HCI_STATE_OPEN);
pthread_mutex_unlock(&hci_controller_mutex);
// Connection poll structure, not initialized yet :
struct pollfd p;
// Initializing the connection poll on our HCI socket :
p.fd = hci_socket->sock;
p.events = POLLIN;
char canceled = 0;
int8_t n = 0;
int16_t len = 0;
res = calloc(6*max_rsp, sizeof(char));
res[0] = '\0';
while(!canceled) {
p.revents = 0;
n = 0;
len = 0;
// Polling the BT device for an event :
if ((n = poll(&p, 1, HCI_CONTROLLER_DEFAULT_TIMEOUT)) < 0) {
if (errno == EAGAIN || EINTR) {
continue;
}
perror("hci_get_RSSI : error while polling the socket");
canceled = 1;
goto end;
}
if (!n) {
errno = ETIMEDOUT;
perror("hci_get_RSSI : error while polling the socket");
break;
}
while ((len = read(hci_socket->sock, buf, sizeof(buf))) < 0) {
if (errno == EAGAIN || errno == EINTR)
continue;
perror("hci_get_RSSI : error while reading the socket");
canceled = 1;
goto end;
}
if (len == 0) {
print_trace(TRACE_WARNING, "hci_get_RSSI : nothing to read on the socket.\n");
break;
}
// WHAT IS buf[0] ? Maybe a parameter added by "read()" ???
event_header.evt = buf[1];
event_header.plen = buf[2];
event_parameter = buf + HCI_EVENT_HDR_SIZE + 1;
int num_results = event_parameter[0];
event_parameter = (void *)(event_parameter + 1); // We switch to the next byte.
switch (event_header.evt) {
case EVT_CMD_COMPLETE: // Corresponding to the last "hci_send_cmd"
print_trace(TRACE_WARNING, "hci_get_RSSI : untreated \"Command Complete\" event.\n");
break;
case EVT_INQUIRY_RESULT_WITH_RSSI: // Code 0x22
for (uint16_t i = 0; i < num_results; i++) {
bt_device_t bt_device;
bt_address_t *rsp_mac = (bt_address_t *)(event_parameter + 6*i); // @ on 6 bytes.
if (!bt_already_registered_device(*rsp_mac)) {
bt_device.mac = *rsp_mac;
hci_compute_device_name(hci_socket, hci_controller, &bt_device);
strcpy(bt_device.custom_name, "UNKNOWN");
bt_device.add_type = UNKNOWN_ADDRESS_TYPE;
bt_register_device(bt_device);
} else {
bt_device = bt_get_device(*rsp_mac);
}
if (mac != NULL && !(bt_compare_addresses(mac, rsp_mac))) {
continue;
}
/* To understand the meaning of the following offsets, please refer to
the spec' p.1002. The structure of the HCI Packet "inquiry result with
RSSI" is shown and explained. To sum up, during the inquiry period,
several devices may respond and one event packet may contain several
informations. But these informations are not stored device per device
but rather "feature by feature". That is to say that the event packet
contains first all of the mac@, then all of the
"page_scan_repetition_mode" etc...
*/
int8_t *rssi = (int8_t *)(event_parameter + (6+1+1+3+2)*num_results + i);
bt_device_display(bt_device);
if (file_descriptor && (*file_descriptor >= 0)) {
char rssi_string[RSSI_STRING_LENGTH] = {0};
snprintf(rssi_string, RSSI_STRING_LENGTH, "%i \n", *rssi);
if (write(*file_descriptor, rssi_string, RSSI_STRING_LENGTH) < RSSI_STRING_LENGTH) {
print_trace(TRACE_WARNING, "Unable to write rssi value into given fd\n");
}
}
char rssi_string_val[5] = {0};
snprintf(rssi_string_val, 5, "%i;", *rssi);
strcat(res, rssi_string_val);
}
break;
case EVT_INQUIRY_COMPLETE:
print_trace(TRACE_INFO, "Inquiry complete !\n");
canceled = 1;
break;
default:
print_trace(TRACE_WARNING, "hci_get_RSSI : an unknown event occurred : 0x%X\n", event_header.evt);
break;
}
}
end :
if (!hci_controller->interrupted) {
hci_change_state(hci_controller, HCI_STATE_OPEN);
}
pthread_mutex_trylock(&hci_controller_mutex);
pthread_mutex_unlock(&hci_controller_mutex);
if (new_socket) {
close_hci_socket(hci_socket);
free(hci_socket);
} else {
// Restoring the old filter :
if (saved_flt) {
set_hci_socket_filter(*hci_socket, &old_flt);
}
}
return res;
}
char *hci_LE_get_RSSI(hci_socket_t *hci_socket, hci_controller_t *hci_controller,
int8_t *file_descriptor,
bt_address_t *mac, uint16_t max_rsp, uint8_t scan_type, uint16_t scan_interval,
uint16_t scan_window, uint8_t own_add_type, uint8_t scan_filter_policy) {
if (!hci_controller) {
print_trace(TRACE_ERROR, "hci_LE_get_RSSI : invalid controller reference.\n");
return NULL;
}
if (hci_controller->interrupted) {
hci_resolve_interruption(hci_socket, hci_controller);
}
if (hci_controller->state != HCI_STATE_OPEN) {
print_trace(TRACE_ERROR, "hci_LE_get_RSSI : busy or closed controller.\n");
return NULL;
}
// Resulting string :
char *res = NULL;
FILE *file = NULL;
print_trace(TRACE_INFO, "1. Opening socket...");
char new_socket = 0;
char socket_err = 0;
check_hci_socket_ptr(&hci_socket, hci_controller, &new_socket, &socket_err);
if (socket_err) {
return NULL;
}
print_trace(TRACE_INFO, " [DONE]\n");
struct pollfd p;
// Initializing the connection poll on our HCI socket :
p.fd = hci_socket->sock;
p.events = POLLIN;
unsigned char buf[HCI_MAX_EVENT_SIZE];
hci_event_hdr event_header;
unsigned char *event_parameter;
struct hci_filter flt;
struct hci_filter old_flt;
// Creating our filter :
hci_compute_filter(&flt, EVT_CMD_COMPLETE,
EVT_LE_META_EVENT,
EVT_LE_ADVERTISING_REPORT,
0);
//hci_filter_all_events(&flt);
/* Saving the old filter.
This can be helpful in a case where we want to use a same socket for different purposes
(even with multiple threads), we can replace the old filter and re-use it later.
*/
print_trace(TRACE_INFO, "2. Saving old filter...");
char saved_flt = 1;
if (!new_socket) {
if (get_hci_socket_filter(*hci_socket, &old_flt) < 0) {
saved_flt = 0;
}
}
print_trace(TRACE_INFO, " [DONE]\n");
// Applying the new filter :
print_trace(TRACE_INFO, "3. Applying new filter...");
if (set_hci_socket_filter(*hci_socket, &flt) < 0) {
goto end;
}
print_trace(TRACE_INFO, " [DONE]\n");
print_trace(TRACE_INFO, "4. Setting scan parameters...");
hci_change_state(hci_controller, HCI_STATE_WRITING);
pthread_mutex_lock(&hci_controller_mutex);
if (hci_le_set_scan_parameters(hci_socket->sock, scan_type, scan_interval, // cf p 1066 spec
scan_window, own_add_type, scan_filter_policy, 2*HCI_CONTROLLER_DEFAULT_TIMEOUT) < 0) {
// last parameter is timeout (for reaching the controler) 0 = infinity.
print_trace(TRACE_ERROR, " [ERROR] \n");
perror("set_scan_parameters");
goto end;
}
print_trace(TRACE_INFO, " [DONE]\n");
hci_change_state(hci_controller, HCI_STATE_OPEN);
pthread_mutex_unlock(&hci_controller_mutex);
print_trace(TRACE_INFO, "5. Enabling scan...");
pthread_mutex_lock(&hci_controller_mutex);
hci_change_state(hci_controller, HCI_STATE_SCANNING);
if (hci_le_set_scan_enable(hci_socket->sock, htobs(0x01), htobs(0x00), 2*HCI_CONTROLLER_DEFAULT_TIMEOUT) < 0) { // Duplicate filtering ? (cf p1069)
print_trace(TRACE_ERROR, " [ERROR] \n");
perror("set_scan_enable");
goto end;
}
print_trace(TRACE_INFO, " [DONE]\n");
pthread_mutex_unlock(&hci_controller_mutex);
char canceled = 0;
int k = 0;
print_trace(TRACE_INFO, "6. Checking response events...\n");
int8_t n = 0;
int16_t len = 0;
res = calloc(6*max_rsp, sizeof(char));
res[0] = '\0';
while(!canceled && (!(max_rsp > 0) || (k < max_rsp))) {
p.revents = 0;
n = 0;
len = 0;
// Polling the BT device for an event :
while ((n = poll(&p, 1, 5000)) < 0) {
if (errno == EAGAIN || EINTR) {
continue;
}
perror("hci_LE_get_RSSI : error while polling the socket");
canceled = 1;
}
if (canceled) {
break;
}
if (!n) {
errno = ETIMEDOUT;
perror("hci_LE_get_RSSI : error while polling the socket");
break;
}
while ((len = read(hci_socket->sock, buf, sizeof(buf))) < 0) {
if (errno == EAGAIN || errno == EINTR)
continue;
perror("hci_LE_get_RSSI : error while reading the socket");
canceled = 1;
}
if (canceled) {
break;
}
if (len == 0) {
print_trace(TRACE_WARNING, "hci_LE_get_RSSI : nothing to read on the socket.\n");
break;
}
// WHAT IS buf[0] ? Maybe a parameter added by "read()" ???
event_header.evt = buf[1];
event_header.plen = buf[2];
event_parameter = (void *)(buf + HCI_EVENT_HDR_SIZE + 1);
uint8_t subevent_code = *(event_parameter);
event_parameter = (void *)(event_parameter + 1);
int num_reports = *event_parameter;
event_parameter = (void *)(event_parameter + 1);
switch (event_header.evt) {
case EVT_LE_META_EVENT: // Code 0x3E
switch (subevent_code) {
case EVT_LE_ADVERTISING_REPORT:
for (uint16_t i = 0; i < num_reports; i++) {
bt_device_t bt_device;
// Address field :
bt_address_t *rsp_mac = (bt_address_t *)(event_parameter +
num_reports*(1+1) +
6*i); // @ on 6 bytes.
if (!bt_already_registered_device(*rsp_mac)) {
uint8_t *address_type;
// Address type field :
address_type = (uint8_t *)(event_parameter + 1*num_reports + i);
bt_device.mac = *rsp_mac;
bt_device.add_type = *address_type;
strcpy(bt_device.custom_name, "UNKNOWN");
bt_register_device(bt_device);
} else {
bt_device = bt_get_device(*rsp_mac);
}
if (mac != NULL && !(bt_compare_addresses(mac, rsp_mac))) {
continue;
}
// Length_data field :
uint8_t *length_data = (uint8_t *)(event_parameter +
num_reports*(1+1+6) + i);
// RSSI field :
int8_t *rssi = (int8_t *)(event_parameter +
(1+1+6+1+(*length_data))*num_reports + i); // rssi on 1byte
if (*rssi == 127) {
print_trace(TRACE_WARNING, "hci_LE_get_rssi : RSSI measure unavailable.\n");
} else if (*rssi >= 21) {
print_trace(TRACE_ERROR, "hci_LE_get_rssi : error while reading RSSI measure.\n");
}
// Display info :
bt_device_display(bt_device);
if (file_descriptor && *file_descriptor >= 0) {
char rssi_string[RSSI_STRING_LENGTH] = {0};
snprintf(rssi_string, RSSI_STRING_LENGTH, "%i \n", *rssi);
if (write(*file_descriptor, rssi_string, RSSI_STRING_LENGTH) < RSSI_STRING_LENGTH) {
print_trace(TRACE_WARNING, "Unable to write rssi value into given fd\n");
}
}
if (file) {
fprintf(file, "%i\n", *rssi);
}
char rssi_string_val[5] = {0};
snprintf(rssi_string_val, 5, "%i ;", *rssi);
strcat(res, rssi_string_val);
k++;
}
break;
default:
print_trace(TRACE_WARNING, "hci_LE_get_rssi : an unknown LE sub-event occured : 0x%X \n",
subevent_code);
break;
}
break;
default:
print_trace(TRACE_WARNING, "hci_LE_get_rssi : an unknown LE sub-event occured : 0x%X \n",
subevent_code);
break;
}
}
print_trace(TRACE_INFO, "Scan complete !\n");
print_trace(TRACE_INFO, "7. Disabling scan...");
pthread_mutex_lock(&hci_controller_mutex);
if (hci_le_set_scan_enable(hci_socket->sock, htobs(0x00), htobs(0x00), 2*HCI_CONTROLLER_DEFAULT_TIMEOUT) < 0) {
print_trace(TRACE_ERROR, " [ERROR] \n");
perror("set_scan_disable");
hci_controller->interrupted = 1;
goto end;
}
print_trace(TRACE_INFO, " [DONE]\n");
hci_change_state(hci_controller, HCI_STATE_OPEN);
pthread_mutex_unlock(&hci_controller_mutex);
end :
if (!hci_controller->interrupted) {
hci_change_state(hci_controller, HCI_STATE_OPEN);
}
pthread_mutex_trylock(&hci_controller_mutex);
pthread_mutex_unlock(&hci_controller_mutex);
if (new_socket) {
close_hci_socket(hci_socket);
free(hci_socket);
} else {
// Restoring the old filter :
if (saved_flt) {
set_hci_socket_filter(*hci_socket, &old_flt);
}
}
if (file) {
fclose(file);
}
return res;
}
void bt_device_table_display(bt_device_table_t device_table) {
for (uint32_t i = 0; i < device_table.length; i++) {
bt_device_display(device_table.device[i]);
}
}
