374 lines
12 KiB
C
374 lines
12 KiB
C
|
|
||
|
|
|
||
|
|
#include "btstack_config.h"
|
||
|
|
|
||
|
|
#include <signal.h>
|
||
|
|
#include <stdint.h>
|
||
|
|
#include <stdio.h>
|
||
|
|
#include <stdlib.h>
|
||
|
|
#include <string.h>
|
||
|
|
|
||
|
|
#include "btstack_run_loop.h"
|
||
|
|
#include "hci_cmd.h"
|
||
|
|
|
||
|
|
#include "btstack.h"
|
||
|
|
|
||
|
|
#define NUM_SERVICES 20
|
||
|
|
|
||
|
|
int conf_iocap = SSP_IO_CAPABILITY_NO_INPUT_NO_OUTPUT;
|
||
|
|
int conf_auth_req = SSP_IO_AUTHREQ_MITM_PROTECTION_NOT_REQUIRED_GENERAL_BONDING;
|
||
|
|
int conf_enable_bounding = 1;
|
||
|
|
int conf_discoverable = 0;
|
||
|
|
char conf_pin[5] = {'0', '0', '0', '0', '\x00'};
|
||
|
|
|
||
|
|
int flag_bounding_start = 0;
|
||
|
|
int flag_bounding_done = 0;
|
||
|
|
int flag_rfcomm_connection_start = 0;
|
||
|
|
int flag_rfcomm_connection_done = 0;
|
||
|
|
|
||
|
|
int var_rfcomm_opened_channels = 0;
|
||
|
|
int var_rfcomm_closed_channels = 0;
|
||
|
|
uint16_t var_rfcom_cids[20];
|
||
|
|
|
||
|
|
static btstack_timer_source_t timer_rfcomm_query;
|
||
|
|
static btstack_timer_source_t timer_rfcomm_connection;
|
||
|
|
static btstack_timer_source_t timer_ssp_pairing;
|
||
|
|
static bd_addr_t remote_addr;
|
||
|
|
|
||
|
|
static struct
|
||
|
|
{
|
||
|
|
uint8_t channel_nr;
|
||
|
|
char service_name[SDP_SERVICE_NAME_LEN + 1];
|
||
|
|
} services[NUM_SERVICES];
|
||
|
|
|
||
|
|
static uint8_t service_index = 0;
|
||
|
|
static uint8_t current_service_index = 0;
|
||
|
|
|
||
|
|
static btstack_packet_callback_registration_t hci_event_callback_registration;
|
||
|
|
|
||
|
|
static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size);
|
||
|
|
|
||
|
|
static btstack_context_callback_registration_t handle_sdp_client_query_request;
|
||
|
|
|
||
|
|
static void handle_start_sdp_client_query(void *context)
|
||
|
|
{
|
||
|
|
UNUSED(context);
|
||
|
|
printf("Starting RFCOMM Query\n");
|
||
|
|
sdp_client_query_rfcomm_channel_and_name_for_uuid(&packet_handler, remote_addr, BLUETOOTH_ATTRIBUTE_PUBLIC_BROWSE_ROOT);
|
||
|
|
}
|
||
|
|
|
||
|
|
static void store_found_service(const char *name, uint8_t port)
|
||
|
|
{
|
||
|
|
printf("APP: Service name: '%s', RFCOMM port %u\n", name, port);
|
||
|
|
if (service_index < NUM_SERVICES) {
|
||
|
|
services[service_index].channel_nr = port;
|
||
|
|
strncpy(services[service_index].service_name, (char *)name, SDP_SERVICE_NAME_LEN);
|
||
|
|
services[service_index].service_name[SDP_SERVICE_NAME_LEN] = 0;
|
||
|
|
service_index++;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
printf("APP: list full - ignore\n");
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static void report_found_services(void)
|
||
|
|
{
|
||
|
|
printf("\n *** Client query response done. ");
|
||
|
|
if (service_index == 0) {
|
||
|
|
printf("No service found.\n\n");
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
printf("Found following %d services:\n", service_index);
|
||
|
|
}
|
||
|
|
int i;
|
||
|
|
for (i = 0; i < service_index; i++) {
|
||
|
|
printf(" Service name %s, RFCOMM port %u\n", services[i].service_name, services[i].channel_nr);
|
||
|
|
}
|
||
|
|
printf(" ***\n\n");
|
||
|
|
}
|
||
|
|
|
||
|
|
static void reset_vars()
|
||
|
|
{
|
||
|
|
flag_bounding_start = 0;
|
||
|
|
flag_bounding_done = 0;
|
||
|
|
flag_rfcomm_connection_start = 0;
|
||
|
|
flag_rfcomm_connection_done = 0;
|
||
|
|
var_rfcomm_closed_channels = 0;
|
||
|
|
var_rfcomm_opened_channels = 0;
|
||
|
|
service_index = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
static void start_ssp_pairing()
|
||
|
|
{
|
||
|
|
static int started = 0;
|
||
|
|
if (!started) {
|
||
|
|
started = 1;
|
||
|
|
// set one-shot timer
|
||
|
|
timer_ssp_pairing.process = &start_ssp_pairing;
|
||
|
|
btstack_run_loop_set_timer(&timer_ssp_pairing, 500);
|
||
|
|
btstack_run_loop_add_timer(&timer_ssp_pairing);
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
started = 0;
|
||
|
|
printf("Starting Bounding with IOCap = %02X, AuthReq=%02X\n", conf_iocap, conf_auth_req);
|
||
|
|
gap_dedicated_bonding(remote_addr, 0);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static void start_rfcomm_query()
|
||
|
|
{
|
||
|
|
static int started = 0;
|
||
|
|
static int created = 0;
|
||
|
|
if (!started) {
|
||
|
|
started = 1;
|
||
|
|
// set one-shot timer
|
||
|
|
timer_rfcomm_query.process = &start_rfcomm_query;
|
||
|
|
btstack_run_loop_set_timer(&timer_rfcomm_query, 1000);
|
||
|
|
btstack_run_loop_add_timer(&timer_rfcomm_query);
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
started = 0;
|
||
|
|
handle_sdp_client_query_request.callback = &handle_start_sdp_client_query;
|
||
|
|
(void)sdp_client_register_query_callback(&handle_sdp_client_query_request);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static void start_rfcomm_connection()
|
||
|
|
{
|
||
|
|
static int started = 0;
|
||
|
|
if (!started) {
|
||
|
|
started = 1;
|
||
|
|
// set one-shot timer
|
||
|
|
timer_rfcomm_connection.process = &start_rfcomm_connection;
|
||
|
|
btstack_run_loop_set_timer(&timer_rfcomm_connection, 500);
|
||
|
|
btstack_run_loop_add_timer(&timer_rfcomm_connection);
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
started = 0;
|
||
|
|
if (service_index == 0) {
|
||
|
|
// Try to force connection to devices without rfcomm service
|
||
|
|
printf("Fallback, connecting to %s, RFCOMM port %u\n", services[0].service_name, services[0].channel_nr);
|
||
|
|
rfcomm_create_channel(&packet_handler, remote_addr, services[0].channel_nr, &var_rfcom_cids[0]);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
while (var_rfcomm_opened_channels < service_index) {
|
||
|
|
int j = var_rfcomm_opened_channels;
|
||
|
|
printf("Connecting to %s, RFCOMM port %u\n", services[j].service_name, services[j].channel_nr);
|
||
|
|
int res = rfcomm_create_channel(&packet_handler, remote_addr, services[j].channel_nr, &var_rfcom_cids[j]);
|
||
|
|
|
||
|
|
if (!res) // Create channel ok
|
||
|
|
return;
|
||
|
|
else
|
||
|
|
printf("Failed with status=%d\n", res);
|
||
|
|
|
||
|
|
var_rfcomm_opened_channels++;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)
|
||
|
|
{
|
||
|
|
UNUSED(channel);
|
||
|
|
UNUSED(size);
|
||
|
|
|
||
|
|
// printf("type:%d\n", packet_type);
|
||
|
|
|
||
|
|
// HCI Events
|
||
|
|
if (packet_type == HCI_EVENT_PACKET) {
|
||
|
|
switch (hci_event_packet_get_type(packet)) {
|
||
|
|
|
||
|
|
case BTSTACK_EVENT_STATE:
|
||
|
|
// BTstack activated, get started
|
||
|
|
if (btstack_event_state_get_state(packet) == HCI_STATE_WORKING) {
|
||
|
|
// Start SDP Query
|
||
|
|
start_rfcomm_query();
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case HCI_EVENT_DISCONNECTION_COMPLETE:
|
||
|
|
puts("HCI Disconnection received");
|
||
|
|
printf("flag_bounding_start=%d,flag_rfcomm_connection_start=%d,flag_rfcomm_connection_done=%d\n", flag_bounding_start, flag_rfcomm_connection_start, flag_rfcomm_connection_done);
|
||
|
|
if (flag_rfcomm_connection_done) {
|
||
|
|
flag_rfcomm_connection_done = 0;
|
||
|
|
reset_vars();
|
||
|
|
start_rfcomm_query();
|
||
|
|
}
|
||
|
|
else if (flag_bounding_start) {
|
||
|
|
flag_bounding_start = 0;
|
||
|
|
start_ssp_pairing();
|
||
|
|
}
|
||
|
|
else if (flag_rfcomm_connection_start) {
|
||
|
|
flag_rfcomm_connection_start = 0;
|
||
|
|
start_rfcomm_connection();
|
||
|
|
}
|
||
|
|
else if (var_rfcomm_closed_channels < service_index) {
|
||
|
|
start_rfcomm_connection();
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
case HCI_EVENT_PIN_CODE_REQUEST:
|
||
|
|
// inform about pin code request
|
||
|
|
printf("Pin code request - using %s\n", remote_addr);
|
||
|
|
hci_event_pin_code_request_get_bd_addr(packet, remote_addr);
|
||
|
|
// baseband address, pin length, PIN: c-string
|
||
|
|
hci_send_cmd(&hci_pin_code_request_reply, &remote_addr, 4, conf_pin);
|
||
|
|
break;
|
||
|
|
case HCI_EVENT_SIMPLE_PAIRING_COMPLETE: {
|
||
|
|
flag_bounding_done = 1;
|
||
|
|
flag_rfcomm_connection_start = 1;
|
||
|
|
uint8_t res = hci_event_simple_pairing_complete_get_status(packet);
|
||
|
|
printf("Secure Simple Pairing complete status: %d\n", res);
|
||
|
|
// Disconnection will be triggered automatically, altough some slaves may continue.
|
||
|
|
if (res != 0) {
|
||
|
|
reset_vars();
|
||
|
|
flag_rfcomm_connection_done = 1;
|
||
|
|
hci_disconnect_all();
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
case RFCOMM_EVENT_CHANNEL_OPENED:
|
||
|
|
|
||
|
|
// data: event(8), len(8), status (8), address (48), server channel(8), rfcomm_cid(16), max frame size(16)
|
||
|
|
if (rfcomm_event_channel_opened_get_status(packet)) {
|
||
|
|
printf("RFCOMM channel open failed, status %u\n", rfcomm_event_channel_opened_get_status(packet));
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
int rfcomm_channel_id = rfcomm_event_channel_opened_get_rfcomm_cid(packet);
|
||
|
|
int mtu = rfcomm_event_channel_opened_get_max_frame_size(packet);
|
||
|
|
printf("RFCOMM channel open succeeded. New RFCOMM Channel ID %u, max frame size %u\n", rfcomm_channel_id, mtu);
|
||
|
|
}
|
||
|
|
var_rfcomm_opened_channels++;
|
||
|
|
// Open new channel, otherwise close all, service_index 0 will force connection
|
||
|
|
if ((var_rfcomm_opened_channels < service_index)) {
|
||
|
|
// Open all RFCOMM channels
|
||
|
|
start_rfcomm_connection();
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
// Disconnect from all RFCOMM channels
|
||
|
|
for (int i = 0; i < service_index; i++) {
|
||
|
|
rfcomm_disconnect(var_rfcom_cids[i]);
|
||
|
|
}
|
||
|
|
if (service_index == 0) {
|
||
|
|
flag_rfcomm_connection_done = 1;
|
||
|
|
hci_disconnect_all();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
case RFCOMM_EVENT_CHANNEL_CLOSED:
|
||
|
|
var_rfcomm_closed_channels++;
|
||
|
|
printf("RFCOMM channel closed, idx=%d/%d\n", var_rfcomm_closed_channels, service_index);
|
||
|
|
|
||
|
|
if (var_rfcomm_closed_channels >= service_index) {
|
||
|
|
printf("All done!\n");
|
||
|
|
flag_rfcomm_connection_done = 1;
|
||
|
|
hci_disconnect_all();
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
switch (packet[0]) {
|
||
|
|
case SDP_EVENT_QUERY_RFCOMM_SERVICE:
|
||
|
|
store_found_service(sdp_event_query_rfcomm_service_get_name(packet),
|
||
|
|
sdp_event_query_rfcomm_service_get_rfcomm_channel(packet));
|
||
|
|
break;
|
||
|
|
case SDP_EVENT_QUERY_COMPLETE:
|
||
|
|
if (sdp_event_query_complete_get_status(packet)) {
|
||
|
|
printf("SDP query failed 0x%02x, retrying...\n", sdp_event_query_complete_get_status(packet));
|
||
|
|
sdp_client_query_rfcomm_channel_and_name_for_uuid(&packet_handler, remote_addr, BLUETOOTH_ATTRIBUTE_PUBLIC_BROWSE_ROOT);
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
printf("SDP query done.\n");
|
||
|
|
report_found_services();
|
||
|
|
if (!flag_bounding_done) {
|
||
|
|
flag_bounding_start = 1;
|
||
|
|
hci_disconnect_all();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
#ifdef HAVE_POSIX_FILE_IO
|
||
|
|
static void usage(const char *name)
|
||
|
|
{
|
||
|
|
printf("\nUsage: %s -a|--address aa:bb:cc:dd:ee:ff\n", name);
|
||
|
|
printf("Use argument -a to connect to a specific device and dump the result of SDP query for L2CAP services.\n\n");
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
void signal_handler(int signal)
|
||
|
|
{
|
||
|
|
exit(0);
|
||
|
|
}
|
||
|
|
|
||
|
|
int btstack_main(int argc, const char *argv[]);
|
||
|
|
int btstack_main(int argc, const char *argv[])
|
||
|
|
{
|
||
|
|
(void)argc;
|
||
|
|
(void)argv;
|
||
|
|
|
||
|
|
int remote_addr_found = 0;
|
||
|
|
for (size_t i = 0; i < argc; i++) {
|
||
|
|
if (argc > i + 1) {
|
||
|
|
if (!strcmp(argv[i], "-a") || !strcmp(argv[i], "--address")) {
|
||
|
|
remote_addr_found = sscanf_bd_addr(argv[i + 1], remote_addr);
|
||
|
|
printf("address=%s\n", argv[i + 1]);
|
||
|
|
}
|
||
|
|
else if (!strcmp(argv[i], "--iocap")) {
|
||
|
|
conf_iocap = atoi(argv[i + 1]);
|
||
|
|
printf("iocap=%d\n", conf_iocap);
|
||
|
|
}
|
||
|
|
else if (!strcmp(argv[i], "--authreq")) {
|
||
|
|
conf_auth_req = atoi(argv[i + 1]);
|
||
|
|
printf("authreq=%d\n", conf_auth_req);
|
||
|
|
}
|
||
|
|
else if (!strcmp(argv[i], "--bounding")) {
|
||
|
|
conf_enable_bounding = atoi(argv[i + 1]);
|
||
|
|
printf("bouding=%d\n", conf_enable_bounding);
|
||
|
|
}
|
||
|
|
else if (!strcmp(argv[i], "--discoverable")) {
|
||
|
|
conf_discoverable = atoi(argv[i + 1]);
|
||
|
|
printf("discoverable=%d\n", conf_discoverable);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!remote_addr_found) {
|
||
|
|
usage(argv[0]);
|
||
|
|
exit(1);
|
||
|
|
}
|
||
|
|
|
||
|
|
signal(SIGTERM, signal_handler);
|
||
|
|
|
||
|
|
// init L2CAP
|
||
|
|
l2cap_init();
|
||
|
|
rfcomm_init();
|
||
|
|
|
||
|
|
// register for HCI events
|
||
|
|
hci_event_callback_registration.callback = &packet_handler;
|
||
|
|
hci_add_event_handler(&hci_event_callback_registration);
|
||
|
|
|
||
|
|
// GAP Config
|
||
|
|
gap_delete_all_link_keys();
|
||
|
|
gap_ssp_set_io_capability(conf_iocap);
|
||
|
|
gap_ssp_set_authentication_requirement(conf_auth_req);
|
||
|
|
gap_set_bondable_mode(conf_enable_bounding);
|
||
|
|
gap_discoverable_control(conf_discoverable);
|
||
|
|
gap_set_local_name("BT ESP32 00:00:00:00:00:00");
|
||
|
|
|
||
|
|
// Modes
|
||
|
|
hci_set_inquiry_mode(INQUIRY_MODE_STANDARD);
|
||
|
|
hci_set_master_slave_policy(HCI_ROLE_MASTER);
|
||
|
|
|
||
|
|
// turn on!
|
||
|
|
hci_power_control(HCI_POWER_ON);
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|