Peripheral is not able to disconnect connection with peer.

Hey,

In the code below bt_conn_disconnect function is executed but the connection is still there. My goal is that peripheral is able to disconnect the connected device if there is no data received after a certain time. 

Code is here

/**************************************************************************//**
 * @file        : ble.c
 * @date        : Jan, 2023
 *
 * @brief BLE Api.
 *
 *****************************************************************************/
// ----------------------------------------------------------------------------
// Include files
// ----------------------------------------------------------------------------
#include <zephyr/logging/log.h>
#include <zephyr/types.h>
#include <zephyr/kernel.h>
#include <stdio.h>
#include <zephyr/dfu/mcuboot.h>
#include <math.h>
#include <soc.h>
#include <app_event_manager.h>
// BLE dependencies
#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/bluetooth/gatt.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/settings/settings.h>
#include <stdio.h>

/* modules */
#include "../events/app_module_event.h"
#include "../events/ble_module_event.h"
#include "../events/debug_module_event.h"
#include "../events/settings_module_event.h"
#include "../events/common_module_event.h"
#include "../events/message_module_event.h"
#include "../system/modules_common.h"
#include "ble.h"
#include "test_service.h"
#include "../utils/hardware.h"

// ----------------------------------------------------------------------------
// Private definitions
// ----------------------------------------------------------------------------
#define MODULE BLE_MODULE
LOG_MODULE_REGISTER(MODULE, CONFIG_BLE_MODULE_LOG);

/* Data module message queue. */
#define BLE_QUEUE_ENTRY_COUNT		    10
#define BLE_QUEUE_BYTE_ALIGNMENT	    4

#define STACKSIZE CONFIG_BT_TEST_THREAD_STACK_SIZE
#define PRIORITY 7

#define DEVICE_NAME CONFIG_BT_DEVICE_NAME
#define DEVICE_NAME_LEN	(sizeof(DEVICE_NAME) - 1)

#define KEY_PASSKEY_ACCEPT DK_BTN1_MSK
#define KEY_PASSKEY_REJECT DK_BTN2_MSK

/** initialize a space in the slab memory */
// #define BLE_RX_BLOCK_SIZE (CONFIG_BT_L2CAP_TX_MTU - 3)
// #define BLE_RX_BUF_COUNT 3
// #define BLE_SLAB_ALIGNMENT 4
// K_MEM_SLAB_DEFINE(ble_rx_slab, BLE_RX_BLOCK_SIZE, BLE_RX_BUF_COUNT, BLE_SLAB_ALIGNMENT);

static K_SEM_DEFINE(ble_init_ok, 0, 1);

static K_FIFO_DEFINE(fifo_ble_tx_data);
static K_FIFO_DEFINE(fifo_ble_rx_data);

/* BLE module super states. */
static enum state_type {
	STATE_BLE_INIT,
	STATE_BLE_READY,
	STATE_BLE_DISCONNECTED,
	STATE_BLE_CONNECTED,
	STATE_SHUTDOWN
} state;

// ----------------------------------------------------------------------------
// Private structs
// ----------------------------------------------------------------------------
struct ble_msg_data {
	union {
		struct app_module_event app;
		struct settings_module_event settings;
		struct debug_module_event debug;
		struct message_module_event message;
		struct ble_module_event ble;
	} module;
};

K_MSGQ_DEFINE(msgq_ble, sizeof(struct ble_msg_data),
	      BLE_QUEUE_ENTRY_COUNT, BLE_QUEUE_BYTE_ALIGNMENT);

typedef struct {
	struct module_data self;
	struct bt_conn *current_conn;
	struct bt_conn *auth_conn;
	configuration_t settings;
	string_t name;
	struct k_timer connection_timeout;
	struct k_sem conn_timeout_sema;
}ble_t;

// ----------------------------------------------------------------------------
// External declarations
// ----------------------------------------------------------------------------

// ----------------------------------------------------------------------------
// Private functions definitions
// ----------------------------------------------------------------------------
static void message_handler(struct ble_msg_data *msg);
static void ble_get_id(void);
static void send_message(uint8_t * data, size_t len, enum ble_module_event_type type);
static void connection_timeout_expirefun(struct k_timer *timer_id);
static void ble_connection_tmr_start(void);
static void ble_connection_tmr_stop(void);
static int ble_enable(void);
static void connection_thread_fn(void);

// ----------------------------------------------------------------------------
// Private declarations
// ----------------------------------------------------------------------------
static ble_t ble = {
	.self = {.name = "ble",
		.msg_q = &msgq_ble,
		.supports_shutdown = true
	},
	.settings = {0},
	.name = {0}
};

/* todo: implement iBeacon */
// struct bt_le_adv_param adv_param_conn =
// 	BT_LE_ADV_PARAM_INIT(BT_LE_ADV_OPT_CONNECTABLE |
// 			     BT_LE_ADV_OPT_NOTIFY_SCAN_REQ,
// 			     BT_GAP_ADV_FAST_INT_MIN_1,
// 			     BT_GAP_ADV_FAST_INT_MAX_1,
// 			     NULL);

// static const struct bt_data ad[] = {
// 	BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
// 	BT_DATA(BT_DATA_NAME_COMPLETE, DEVICE_NAME, DEVICE_NAME_LEN),
// };

// static const struct bt_data sd[] = {
// 	BT_DATA_BYTES(BT_DATA_UUID128_ALL, BT_UUID_TEST_VAL),
// };

// ----------------------------------------------------------------------------
// Private functions
// ----------------------------------------------------------------------------
/* Convenience functions used in internal state handling. */
static char *state2str(enum state_type state)
{
	switch (state) {
	case STATE_BLE_INIT:
		return "STATE_BLE_INIT";
	case STATE_BLE_READY:
		return "STATE_BLE_READY";
	case STATE_BLE_DISCONNECTED:
		return "STATE_BLE_DISCONNECTED";
	case STATE_BLE_CONNECTED:
		return "STATE_BLE_CONNECTED";
	case STATE_SHUTDOWN:
		return "STATE_SHUTDOWN";
	default:
		return "Unknown";
	}
}

static void state_set(enum state_type new_state)
{
	if (new_state == state) {
		LOG_DBG("State: %s", state2str(state));
		return;
	}

	LOG_DBG("State transition %s --> %s",
		state2str(state),
		state2str(new_state));

	state = new_state;
}

static void connected(struct bt_conn *conn, uint8_t err)
{
	char addr[BT_ADDR_LE_STR_LEN];

	if (err) {
		LOG_ERR("Connection failed (err %u)", err);
		return;
	}

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
	LOG_INF("Connected %s", addr);
	ble.current_conn = bt_conn_ref(conn);
	state_set(STATE_BLE_CONNECTED);
	ble_connection_tmr_start();
}

static void disconnected(struct bt_conn *conn, uint8_t reason)
{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	LOG_INF("Disconnected: %s (reason %u)", addr, reason);
	// not needed since is nubonded in pro.conf
	// bt_unpair(BT_ID_DEFAULT, NULL);

	if (ble.auth_conn) {
		bt_conn_unref(ble.auth_conn);
		ble.auth_conn = NULL;
	}
	
	if (ble.current_conn) {
		bt_conn_unref(ble.current_conn);
		ble.current_conn = NULL;
	}
	 state_set(STATE_BLE_DISCONNECTED);
	
}

static void ble_get_id(void) {
	static struct bt_le_oob _oob = { .addr = 0 };
	(void)bt_le_oob_get_local(BT_ID_DEFAULT, &_oob);
	ble.settings.serial_number.len = CONFIG_BLE_MAC_STR_LEN;
	(void)bt_addr_le_to_str(&_oob.addr, ble.settings.serial_number.array, ble.settings.serial_number.len);
	memset(&ble.name, 0, sizeof(string_t));
	ble.name.len = snprintk(ble.name.array, sizeof(ble.name.array), "%s%s", get_hardware_name_prefix(ble.settings.hardware_settings.type), ble.settings.serial_number.array);
	LOG_INF("MAC: %s", ble.settings.serial_number.array);
	send_message(&ble.settings.serial_number, sizeof(string_t), BLE_EVT_SN_READY);
}

static bool le_param_req(struct bt_conn *conn, struct bt_le_conn_param *param)
{
	//If acceptable params, return true, otherwise return false.
	return true; 
}

static void le_param_updated(struct bt_conn *conn, uint16_t interval, uint16_t latency, uint16_t timeout)
{
	struct bt_conn_info info; 
	char addr[BT_ADDR_LE_STR_LEN];
	if(bt_conn_get_info(conn, &info))
	{
		LOG_INF("Could not parse connection info\n");
	}
	else
	{
		bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
	}
}


#ifdef CONFIG_BT_TEST_SECURITY_ENABLED
static void security_changed(struct bt_conn *conn, bt_security_t level,
			     enum bt_security_err err)
{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	if (!err) {
		LOG_INF("Security changed: %s level %u", addr, level);
	} else {
		LOG_WRN("Security failed: %s level %u err %d", addr,
			level, err);
	}
}
#endif

BT_CONN_CB_DEFINE(conn_callbacks) = {
	.connected          = connected,
	.disconnected       = disconnected,
	.le_param_req		= le_param_req,
	.le_param_updated	= le_param_updated,
#ifdef CONFIG_BT_TEST_SECURITY_ENABLED
	.security_changed = security_changed,
#endif
};

#if defined(CONFIG_BT_TEST_SECURITY_ENABLED)
static void auth_cancel(struct bt_conn *conn)
{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	LOG_INF("Pairing cancelled: %s", addr);
}

static void pairing_confirm(struct bt_conn *conn)
{
	bt_conn_auth_cancel(conn);
}

static void pairing_complete(struct bt_conn *conn, bool bonded)
{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	LOG_INF("Pairing completed: %s, bonded: %d", addr, bonded);
}


static void pairing_failed(struct bt_conn *conn, enum bt_security_err reason)
{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	LOG_INF("Pairing failed conn: %s, reason %d", addr, reason);
}


static struct bt_conn_auth_cb conn_auth_callbacks = {
	.passkey_display = NULL,
	.passkey_confirm = NULL,
	.cancel = auth_cancel,
};

static struct bt_conn_auth_info_cb conn_auth_info_callbacks = {
	.pairing_complete = pairing_complete,
	.pairing_failed = pairing_failed,
};
#else
static struct bt_conn_auth_cb conn_auth_callbacks;
static struct bt_conn_auth_info_cb conn_auth_info_callbacks;
#endif

static void send_message(uint8_t * data, size_t len, enum ble_module_event_type type) {
	struct ble_module_event *ble_module_event = new_ble_module_event();
	__ASSERT(ble_module_event, "Not enough heap left to allocate event");
	if (len > CONFIG_BLE_MAX_BUFFER)
	__ASSERT(ble_module_event, "Received messaged is much bigger than what is expected");
	ble_module_event->type = type;
	ble_module_event->data.data.len = len;
	memcpy(ble_module_event->data.data.ptr, data, len);
	APP_EVENT_SUBMIT(ble_module_event);
}

static void bt_receive_cb(struct bt_conn *conn, const uint8_t *const data,
			  uint16_t len)
{
	int err;
	char addr[BT_ADDR_LE_STR_LEN] = {0};
	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, ARRAY_SIZE(addr));
	LOG_HEXDUMP_INF(data, len, "received data:");
	send_message(data, len, BLE_EVT_DATA_RECEIVED);
	
}

static struct bt_test_cb test_cb = {
	.received = bt_receive_cb,
};

void error(void)
{
	while (true) {
		/* Spin for ever */
		k_sleep(K_MSEC(1000));
	}
}

static void connection_timeout_expirefun(struct k_timer *timer_id) {
	LOG_DBG("Connection BLE timeout timer is expired");
	k_sem_give(&ble.conn_timeout_sema);
}

static void ble_connection_tmr_start(void) {
	k_timer_start(&ble.connection_timeout, K_SECONDS(CONFIG_BLE_CONNECTION_TIMEOUT), K_NO_WAIT);
}

static void ble_connection_tmr_stop(void) {
	k_timer_stop(&ble.connection_timeout);
}
/**************************************************************************//**
 * @brief Initialize BLE.
 *****************************************************************************/
int ble_init(void) {
	k_timer_init(&ble.connection_timeout, connection_timeout_expirefun, NULL);
	return ble_enable();
}

/**************************************************************************//**
 * @brief Enable BLE.
 *****************************************************************************/
static int ble_enable() {
	
	int blink_status = 0;
	int err = 0;

	if (IS_ENABLED(CONFIG_BT_TEST_SECURITY_ENABLED)) {
		err = bt_conn_auth_cb_register(&conn_auth_callbacks);
		if (err) {
			LOG_ERR("Failed to register authorization callbacks.");
			return;
		}

		err = bt_conn_auth_info_cb_register(&conn_auth_info_callbacks);
		if (err) {
			LOG_ERR("Failed to register authorization info callbacks.");
			return;
		}
	}
	bt_set_bondable(false);
	bt_passkey_set(CONFIG_BLE_PASSKEY);

	err = bt_enable(NULL);
	if (err) {
		error();
	}
	
	ble_get_id();
	LOG_INF("Bluetooth Module started");
	k_sem_give(&ble_init_ok);
	if (IS_ENABLED(CONFIG_SETTINGS)) {
		settings_load();
	}
	err = bt_test_init(&test_cb);
	if (err) {
		LOG_ERR("Failed to initialize UART service (err: %d)", err);
		return;
	}
	const struct bt_data ad[] = {
		BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
		BT_DATA(BT_DATA_NAME_COMPLETE, ble.name.array, ble.name.len),
	};

	const struct bt_data sd[] = {
		BT_DATA_BYTES(BT_DATA_UUID128_ALL, BT_UUID_TEST_VAL),
	};
	err = bt_le_adv_start(BT_LE_ADV_CONN, ad, ARRAY_SIZE(ad), sd,
			      ARRAY_SIZE(sd));
	if (err) {
		LOG_ERR("Advertising failed to start (err %d)", err);
		return;
	}
	return 0;
}

/* Message handler for STATE_BLE_INIT. */
static void on_state_init(struct ble_msg_data *msg)
{
	if (IS_EVENT(msg, settings, SETTINGS_EVT_CONFIG_READY)) {
		int err = ble_init();
		__ASSERT(err == 0, "setp() failed");
		memcpy(&ble.settings, &msg->module.settings.data.device.settings, sizeof(ble.settings));
		state_set(STATE_BLE_READY);
	}
}

static void connection_thread_fn(void) {
	k_sem_init(&ble.conn_timeout_sema, 0, 1);
	for (;;) {
		if (k_sem_take(&ble.conn_timeout_sema, K_FOREVER) == 0) {
			LOG_DBG("Start to disconnect the device");
			if (ble.current_conn != NULL) {
				bt_conn_disconnect(ble.current_conn, BT_HCI_ERR_CONN_TIMEOUT);
			}
		}
	}
}

static void module_thread_fn(void)
{
	int err;
	struct ble_msg_data msg = { 0 };

	ble.self.thread_id = k_current_get();

	err = module_start(&ble.self);
	if (err) {
		LOG_ERR("Failed starting module, error: %d", err);
		SEND_ERROR(ble, BLE_EVT_ERROR, err);
	}

	state_set(STATE_BLE_INIT);

	while (true) {
		module_get_next_msg(&ble.self, &msg);

		switch (state) {
		case STATE_BLE_INIT:
			on_state_init(&msg);
			break;
		case STATE_BLE_CONNECTED:
			break;
		case STATE_BLE_READY:
			break;
		case STATE_SHUTDOWN:
			/* The shutdown state has no transition. */
			break;
		default:
			LOG_ERR("Unknown state.");
			break;
		}

		message_handler(&msg);
	}
}

static void message_handler(struct ble_msg_data *msg)
{
	if (IS_EVENT(msg, message, MESSAGE_EVT_DATA_READY_TO_SEND)) {
		bt_test_send(NULL, msg->module.message.data.data.ptr, msg->module.message.data.data.len);
	}
}

/* Handlers */
static bool ble_event_handler(const struct app_event_header *aeh)
{
	struct ble_msg_data msg = {0};
	bool enqueue_msg = false, consume = false;

	if (is_app_module_event(aeh)) {
		struct app_module_event *evt = cast_app_module_event(aeh);
		msg.module.app = *evt;
		enqueue_msg = true;
		LOG_DBG("Receiving message from the app");
	}

	if (is_settings_module_event(aeh)) {
		struct settings_module_event *evt = cast_settings_module_event(aeh);
		msg.module.settings = *evt;
		enqueue_msg = true;
		LOG_DBG("Receiving message from the settings");
	}

	if (is_debug_module_event(aeh)) {
		struct debug_module_event *evt = cast_debug_module_event(aeh);
		msg.module.debug = *evt;
		enqueue_msg = true;
	}

	if (is_message_module_event(aeh)) {
		struct message_module_event *evt = cast_message_module_event(aeh);
		msg.module.message = *evt;
		enqueue_msg = true;
	}

	if (enqueue_msg) {
		int err = module_enqueue_msg(&ble.self, &msg);

		if (err) {
			LOG_ERR("Message could not be enqueued");
			SEND_ERROR(ble, BLE_EVT_ERROR, err);
		}
	}
	return false;
}

K_THREAD_DEFINE(ble_module_thread, CONFIG_BLE_THREAD_STACK_SIZE,
		module_thread_fn, NULL, NULL, NULL,
		K_LOWEST_APPLICATION_THREAD_PRIO, 0, 0);

K_THREAD_DEFINE(ble_connection_timeout_thread, CONFIG_BLE_CONNECTION_TIMEOUT_THREAD_STACK_SIZE,
		connection_thread_fn, NULL, NULL, NULL,
		K_LOWEST_APPLICATION_THREAD_PRIO, 0, 0);

APP_EVENT_LISTENER(MODULE, ble_event_handler);
APP_EVENT_SUBSCRIBE(MODULE, app_module_event);
APP_EVENT_SUBSCRIBE(MODULE, settings_module_event);
APP_EVENT_SUBSCRIBE_EARLY(MODULE, message_module_event);

Thanks in advance! 

Related