|
#include <endian.h> |
|
#include <stdbool.h> |
|
#include <stddef.h> |
|
#include <stdlib.h> |
|
#include <stdio.h> |
|
#include <sys/socket.h> |
|
#include <unistd.h> |
|
|
|
#include "bluetooth/bluetooth.h" |
|
#include "bluetooth/hci.h" |
|
|
|
#define NEW_LE_AUTOCONN_TIMEOUT 16000 |
|
|
|
struct hci_event { |
|
uint16_t code; |
|
uint16_t controller_id; |
|
uint16_t size; |
|
uint16_t cmd_opcode; |
|
uint8_t cmd_status; |
|
}; |
|
|
|
#define EVT_CODE_CMD_COMPLETE 0x0001 |
|
#define EVT_CMD_STATUS_SUCCESS 0x00 |
|
|
|
#define PARAM_TYPE_LE_AUTOCONN_TIMEOUT 0x001b |
|
|
|
unsigned char read_buf[1024]; |
|
const unsigned char read_conf_cmd[6] = { |
|
/* cmd opcode */ 0x4b, 0x00, |
|
/* controller id */ 0x00, 0x00, |
|
/* data len */ 0x00, 0x00}; |
|
unsigned char set_le_autoconn_timeout_cmd[] = { |
|
/* cmd opcode */ 0x4c, 0x00, |
|
/* controller id */ 0x00, 0x00, |
|
/* data len */ 0x05, 0x00, |
|
/* data */ 0x1b, 0x00, 0x02, 0x00, 0x00 |
|
}; |
|
const int set_timeout_cmd_value_offset = 9; |
|
|
|
int get_autoconn_timeout_from_result(const unsigned char *buf, size_t data_size) |
|
{ |
|
const unsigned char *buf_end; |
|
/* Skip event header */ |
|
buf += offsetof(struct hci_event, cmd_opcode); |
|
buf_end = buf + data_size; |
|
|
|
while (1) { |
|
uint8_t param_data_size = *(uint8_t *)(buf + 2); |
|
uint16_t param_type = 0; |
|
memcpy(¶m_type, buf, sizeof(param_type)); |
|
param_type = le16toh(param_type); |
|
|
|
if (param_type == PARAM_TYPE_LE_AUTOCONN_TIMEOUT) { |
|
uint16_t value = 0; |
|
|
|
if (param_data_size > 2) { |
|
fputs("Unexpected value length", stderr); |
|
return -1; |
|
} |
|
|
|
memcpy(&value, buf + 3, param_data_size); |
|
value = le16toh(value); |
|
|
|
printf("LE Autoconnect Timeout=%d \n", value); |
|
|
|
return value; |
|
} |
|
|
|
buf += 2/*type*/ + 1/*len*/ + param_data_size; |
|
|
|
if (buf >= buf_end) { |
|
puts("Param type LE Autoconnect Timeout not found in result"); |
|
break; |
|
} |
|
} |
|
|
|
return -1; |
|
} |
|
|
|
void print_buf(const unsigned char *buf, size_t len) |
|
{ |
|
for (int i = 0; i < len; i++) { |
|
printf("%02X ", (unsigned)buf[i]); |
|
} |
|
puts(""); |
|
} |
|
|
|
bool is_read_only_param(char *param) |
|
{ |
|
static const char read_param[]= "--read"; |
|
|
|
return strncmp(param, read_param, sizeof(read_param)) == 0; |
|
} |
|
|
|
struct hci_event *get_event_from_buf(void *buf) |
|
{ |
|
struct hci_event *evt = malloc(sizeof(struct hci_event)); |
|
if (!evt) |
|
exit(EXIT_FAILURE); |
|
|
|
memcpy(evt, buf, sizeof(*evt)); |
|
|
|
evt->code = le16toh(evt->code); |
|
evt->controller_id = le16toh(evt->controller_id); |
|
evt->size = le16toh(evt->size); |
|
evt->cmd_opcode = le16toh(evt->cmd_opcode); |
|
|
|
return evt; |
|
} |
|
|
|
int set_le_autoconn_timeout(int sockfd, uint16_t new_val) |
|
{ |
|
int status, result; |
|
struct hci_event *event; |
|
|
|
new_val = htole16(new_val); |
|
|
|
memcpy( |
|
&set_le_autoconn_timeout_cmd[set_timeout_cmd_value_offset], &new_val, sizeof(new_val) |
|
); |
|
|
|
status = send(sockfd, set_le_autoconn_timeout_cmd, sizeof(set_le_autoconn_timeout_cmd), 0); |
|
if (status < 0) { |
|
perror("Failed to set params"); |
|
return -1; |
|
} |
|
|
|
memset(read_buf, 0, sizeof(read_buf)); |
|
|
|
status = recv(sockfd, read_buf, sizeof(read_buf), 0); |
|
if (status < 0) { |
|
perror("Failed to set params"); |
|
return -1; |
|
} |
|
|
|
event = get_event_from_buf(read_buf); |
|
result = (event->code == EVT_CODE_CMD_COMPLETE |
|
&& event->size >= 3 |
|
&& event->cmd_status == EVT_CMD_STATUS_SUCCESS |
|
) ? 0 : -1; |
|
free(event); |
|
|
|
if (result) |
|
print_buf(read_buf, status); |
|
|
|
return result; |
|
} |
|
|
|
int main(int argc, char *argv[static 1]) |
|
{ |
|
int status; |
|
struct sockaddr_hci addr = {0}; |
|
int sockfd = socket(AF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC, BTPROTO_HCI); |
|
|
|
if (sockfd < 0) { |
|
perror("socket create error"); |
|
return EXIT_FAILURE; |
|
} |
|
|
|
addr.hci_family = AF_BLUETOOTH; |
|
addr.hci_dev = HCI_DEV_NONE; |
|
addr.hci_channel = HCI_CHANNEL_CONTROL; |
|
|
|
status = bind(sockfd, (struct sockaddr *)&addr, sizeof(addr)); |
|
if (status < 0) { |
|
perror("socket bind failed"); |
|
close(sockfd); |
|
return EXIT_FAILURE; |
|
} |
|
|
|
status = send(sockfd, read_conf_cmd, sizeof(read_conf_cmd), 0); |
|
if (status < 0) { |
|
perror("socket write failed"); |
|
close(sockfd); |
|
return EXIT_FAILURE; |
|
} |
|
|
|
status = recv(sockfd, read_buf, 1024, 0); |
|
if (status <= 0) { |
|
perror("Didn't receive response"); |
|
close(sockfd); |
|
return EXIT_FAILURE; |
|
} |
|
|
|
struct hci_event *event = get_event_from_buf(read_buf); |
|
|
|
if (event->code == EVT_CODE_CMD_COMPLETE && event->cmd_status == EVT_CMD_STATUS_SUCCESS) { |
|
int autocon_timeout; |
|
|
|
printf("Command complete, result size: %hu.\nBinary response: \n", event->size); |
|
print_buf(read_buf, status); |
|
|
|
autocon_timeout = get_autoconn_timeout_from_result(read_buf, event->size); |
|
if (autocon_timeout < 0) { |
|
return EXIT_FAILURE; |
|
} |
|
|
|
if (argc < 2 || !is_read_only_param(argv[1])) { |
|
printf("Set new LE Autoconnect Timeout value to %d... ", NEW_LE_AUTOCONN_TIMEOUT); |
|
if (set_le_autoconn_timeout(sockfd, NEW_LE_AUTOCONN_TIMEOUT) == 0) { |
|
puts("OK"); |
|
} else { |
|
puts("ERROR"); |
|
} |
|
} |
|
} else { |
|
fputs("Could not read current settings\n", stderr); |
|
} |
|
|
|
free(event); |
|
close(sockfd); |
|
|
|
return EXIT_SUCCESS; |
|
} |
You need to move line 41 to 44.