static void clear_mcumgr(void)
{
atomic_clear_bit(&esc_state, ESC_MCUMGR_PKT_1);
atomic_clear_bit(&esc_state, ESC_MCUMGR_PKT_2);
atomic_clear_bit(&esc_state, ESC_MCUMGR_FRAG_1);
atomic_clear_bit(&esc_state, ESC_MCUMGR_FRAG_2);
}
void device_busy_clear(struct device *busy_dev)
{
#ifdef CONFIG_DEVICE_POWER_MANAGEMENT
atomic_clear_bit((atomic_t *) __device_busy_start,
(busy_dev - __device_init_start));
#endif
}
static void set_discoverable(uint8_t *data, uint16_t len)
{
const struct gap_set_discoverable_cmd *cmd = (void *) data;
struct gap_set_discoverable_rp rp;
switch (cmd->discoverable) {
case GAP_NON_DISCOVERABLE:
ad_flags &= ~(BT_LE_AD_GENERAL | BT_LE_AD_LIMITED);
atomic_clear_bit(¤t_settings, GAP_SETTINGS_DISCOVERABLE);
break;
case GAP_GENERAL_DISCOVERABLE:
ad_flags &= ~BT_LE_AD_LIMITED;
ad_flags |= BT_LE_AD_GENERAL;
atomic_set_bit(¤t_settings, GAP_SETTINGS_DISCOVERABLE);
break;
case GAP_LIMITED_DISCOVERABLE:
ad_flags &= ~BT_LE_AD_GENERAL;
ad_flags |= BT_LE_AD_LIMITED;
atomic_set_bit(¤t_settings, GAP_SETTINGS_DISCOVERABLE);
break;
default:
tester_rsp(BTP_SERVICE_ID_GAP, GAP_SET_DISCOVERABLE,
CONTROLLER_INDEX, BTP_STATUS_FAILED);
return;
}
rp.current_settings = sys_cpu_to_le32(current_settings);
tester_send(BTP_SERVICE_ID_GAP, GAP_SET_DISCOVERABLE, CONTROLLER_INDEX,
(uint8_t *) &rp, sizeof(rp));
}
static int set_advertise_disable(void)
{
#if 0
struct net_buf *buf;
int err;
#endif
if (!atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) {
return 0;
}
#if 0
buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_ADV_ENABLE, 1);
if (!buf) {
return -ENOBUFS;
}
net_buf_add_u8(buf, BT_HCI_LE_ADV_DISABLE);
err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_ADV_ENABLE, buf, NULL);
if (err) {
return err;
}
#endif
nble_gap_stop_adv_req(NULL);
atomic_clear_bit(bt_dev.flags, BT_DEV_ADVERTISING);
return 0;
}
void on_nble_gap_dir_adv_timeout_evt(const struct nble_gap_dir_adv_timeout_evt *p_evt)
{
struct bt_conn *conn = bt_conn_lookup_state_le(BT_ADDR_LE_ANY, BT_CONN_CONNECT);
if (conn) {
atomic_clear_bit(bt_dev.flags, BT_DEV_ADVERTISING);
conn->err = p_evt->error;
bt_conn_set_state(conn, BT_CONN_DISCONNECTED);
bt_conn_unref(conn);
}
}
int bt_le_scan_start(const struct bt_le_scan_param *param, bt_le_scan_cb_t cb)
{
int err;
/* Check that the parameters have valid values */
if (!valid_le_scan_param(param)) {
return -EINVAL;
}
#if NOT_USED_FOR_NOW
/* Return if active scan is already enabled */
if (atomic_test_and_set_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) {
return -EALREADY;
}
if (atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING)) {
err = bt_hci_stop_scanning();
if (err) {
atomic_clear_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN);
return err;
}
}
#endif
err = start_le_scan(param->type, param->interval, param->window,
param->filter_dup);
if (err) {
#if NOT_USED_FOR_NOW
atomic_clear_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN);
#endif
return err;
}
scan_dev_found_cb = cb;
return 0;
}
static int monitor_console_out(int c)
{
static char buf[128];
static size_t len;
if (atomic_test_and_set_bit(&flags, BT_CONSOLE_BUSY)) {
return c;
}
if (c != '\n' && len < sizeof(buf) - 1) {
buf[len++] = c;
atomic_clear_bit(&flags, BT_CONSOLE_BUSY);
return c;
}
buf[len++] = '\0';
bt_monitor_send(BT_MONITOR_SYSTEM_NOTE, buf, len);
len = 0;
atomic_clear_bit(&flags, BT_CONSOLE_BUSY);
return c;
}
static void stop_advertising(const uint8_t *data, uint16_t len)
{
struct gap_stop_advertising_rp rp;
if (bt_le_adv_stop() < 0) {
tester_rsp(BTP_SERVICE_ID_GAP, GAP_STOP_ADVERTISING,
CONTROLLER_INDEX, BTP_STATUS_FAILED);
return;
}
atomic_clear_bit(¤t_settings, GAP_SETTINGS_ADVERTISING);
rp.current_settings = sys_cpu_to_le32(current_settings);
tester_send(BTP_SERVICE_ID_GAP, GAP_STOP_ADVERTISING, CONTROLLER_INDEX,
(uint8_t *) &rp, sizeof(rp));
}
static void set_connectable(uint8_t *data, uint16_t len)
{
const struct gap_set_connectable_cmd *cmd = (void *) data;
struct gap_set_connectable_rp rp;
if (cmd->connectable) {
atomic_set_bit(¤t_settings, GAP_SETTINGS_CONNECTABLE);
} else {
atomic_clear_bit(¤t_settings, GAP_SETTINGS_CONNECTABLE);
}
rp.current_settings = sys_cpu_to_le32(current_settings);
tester_send(BTP_SERVICE_ID_GAP, GAP_SET_CONNECTABLE, CONTROLLER_INDEX,
(uint8_t *) &rp, sizeof(rp));
}
void bt_monitor_send(u16_t opcode, const void *data, size_t len)
{
struct bt_monitor_hdr hdr;
if (atomic_test_and_set_bit(&flags, BT_LOG_BUSY)) {
drop_add(opcode);
return;
}
encode_hdr(&hdr, opcode, len);
monitor_send(&hdr, BT_MONITOR_BASE_HDR_LEN + hdr.hdr_len);
monitor_send(data, len);
atomic_clear_bit(&flags, BT_LOG_BUSY);
}
int bt_le_adv_stop(void)
{
int err;
if (!atomic_test_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING)) {
return -EALREADY;
}
err = set_advertise_disable();
if (err) {
return err;
}
atomic_clear_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING);
return 0;
}
static int bt_hci_stop_scanning(void)
{
#ifdef NOT_USED_FOR_NOW
struct net_buf *buf, *rsp;
struct bt_hci_cp_le_set_scan_enable *scan_enable;
int err;
if (!atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING)) {
return -EALREADY;
}
buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_SCAN_ENABLE,
sizeof(*scan_enable));
if (!buf) {
return -ENOBUFS;
}
scan_enable = net_buf_add(buf, sizeof(*scan_enable));
memset(scan_enable, 0, sizeof(*scan_enable));
scan_enable->filter_dup = BT_HCI_LE_SCAN_FILTER_DUP_DISABLE;
scan_enable->enable = BT_HCI_LE_SCAN_DISABLE;
err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_SCAN_ENABLE, buf, &rsp);
if (err) {
return err;
}
/* Update scan state in case of success (0) status */
err = rsp->data[0];
if (!err) {
atomic_clear_bit(bt_dev.flags, BT_DEV_SCANNING);
}
net_buf_unref(rsp);
return err;
#endif
nble_gap_stop_scan_req();
return 0;
}
void bt_log(int prio, const char *fmt, ...)
{
struct bt_monitor_user_logging log;
struct bt_monitor_hdr hdr;
const char id[] = "bt";
va_list ap;
int len;
va_start(ap, fmt);
len = vsnprintk(NULL, 0, fmt, ap);
va_end(ap);
if (len < 0) {
return;
}
log.priority = prio;
log.ident_len = sizeof(id);
if (atomic_test_and_set_bit(&flags, BT_LOG_BUSY)) {
drop_add(BT_MONITOR_USER_LOGGING);
return;
}
encode_hdr(&hdr, BT_MONITOR_USER_LOGGING,
sizeof(log) + sizeof(id) + len + 1);
monitor_send(&hdr, BT_MONITOR_BASE_HDR_LEN + hdr.hdr_len);
monitor_send(&log, sizeof(log));
monitor_send(id, sizeof(id));
va_start(ap, fmt);
_vprintk(log_out, NULL, fmt, ap);
va_end(ap);
/* Terminate the string with null */
uart_poll_out(monitor_dev, '\0');
atomic_clear_bit(&flags, BT_LOG_BUSY);
}
void atomic_test(void)
{
int i;
atomic_t target, orig;
atomic_val_t value;
atomic_val_t oldvalue;
target = 4;
value = 5;
oldvalue = 6;
/* atomic_cas() */
zassert_true((atomic_cas(&target, oldvalue, value) == 0), "atomic_cas");
target = 6;
zassert_true((atomic_cas(&target, oldvalue, value) == 1), "atomic_cas");
zassert_true((target == value), "atomic_cas");
/* atomic_add() */
target = 1;
value = 2;
zassert_true((atomic_add(&target, value) == 1), "atomic_add");
zassert_true((target == 3), "atomic_add");
/* atomic_sub() */
target = 10;
value = 2;
zassert_true((atomic_sub(&target, value) == 10), "atomic_sub");
zassert_true((target == 8), "atomic_sub");
/* atomic_inc() */
target = 5;
zassert_true((atomic_inc(&target) == 5), "atomic_inc");
zassert_true((target == 6), "atomic_inc");
/* atomic_dec() */
target = 2;
zassert_true((atomic_dec(&target) == 2), "atomic_dec");
zassert_true((target == 1), "atomic_dec");
/* atomic_get() */
target = 50;
zassert_true((atomic_get(&target) == 50), "atomic_get");
/* atomic_set() */
target = 42;
value = 77;
zassert_true((atomic_set(&target, value) == 42), "atomic_set");
zassert_true((target == value), "atomic_set");
/* atomic_clear() */
target = 100;
zassert_true((atomic_clear(&target) == 100), "atomic_clear");
zassert_true((target == 0), "atomic_clear");
/* atomic_or() */
target = 0xFF00;
value = 0x0F0F;
zassert_true((atomic_or(&target, value) == 0xFF00), "atomic_or");
zassert_true((target == 0xFF0F), "atomic_or");
/* atomic_xor() */
target = 0xFF00;
value = 0x0F0F;
zassert_true((atomic_xor(&target, value) == 0xFF00), "atomic_xor");
zassert_true((target == 0xF00F), "atomic_xor");
/* atomic_and() */
target = 0xFF00;
value = 0x0F0F;
zassert_true((atomic_and(&target, value) == 0xFF00), "atomic_and");
zassert_true((target == 0x0F00), "atomic_and");
/* atomic_nand() */
target = 0xFF00;
value = 0x0F0F;
zassert_true((atomic_nand(&target, value) == 0xFF00), "atomic_nand");
zassert_true((target == 0xFFFFF0FF), "atomic_nand");
/* atomic_test_bit() */
for (i = 0; i < 32; i++) {
target = 0x0F0F0F0F;
zassert_true(!!(atomic_test_bit(&target, i) == !!(target & (1 << i))),
"atomic_test_bit");
}
/* atomic_test_and_clear_bit() */
for (i = 0; i < 32; i++) {
orig = 0x0F0F0F0F;
target = orig;
zassert_true(!!(atomic_test_and_clear_bit(&target, i)) == !!(orig & (1 << i)),
"atomic_test_and_clear_bit");
zassert_true(target == (orig & ~(1 << i)), "atomic_test_and_clear_bit");
}
/* atomic_test_and_set_bit() */
for (i = 0; i < 32; i++) {
orig = 0x0F0F0F0F;
target = orig;
zassert_true(!!(atomic_test_and_set_bit(&target, i)) == !!(orig & (1 << i)),
"atomic_test_and_set_bit");
zassert_true(target == (orig | (1 << i)), "atomic_test_and_set_bit");
}
/* atomic_clear_bit() */
for (i = 0; i < 32; i++) {
orig = 0x0F0F0F0F;
target = orig;
atomic_clear_bit(&target, i);
zassert_true(target == (orig & ~(1 << i)), "atomic_clear_bit");
}
/* atomic_set_bit() */
for (i = 0; i < 32; i++) {
orig = 0x0F0F0F0F;
target = orig;
atomic_set_bit(&target, i);
zassert_true(target == (orig | (1 << i)), "atomic_set_bit");
}
}
static void handle_ansi(u8_t byte, char *line)
{
if (atomic_test_and_clear_bit(&esc_state, ESC_ANSI_FIRST)) {
if (!isdigit(byte)) {
ansi_val = 1U;
goto ansi_cmd;
}
atomic_set_bit(&esc_state, ESC_ANSI_VAL);
ansi_val = byte - '0';
ansi_val_2 = 0U;
return;
}
if (atomic_test_bit(&esc_state, ESC_ANSI_VAL)) {
if (isdigit(byte)) {
if (atomic_test_bit(&esc_state, ESC_ANSI_VAL_2)) {
ansi_val_2 *= 10;
ansi_val_2 += byte - '0';
} else {
ansi_val *= 10;
ansi_val += byte - '0';
}
return;
}
/* Multi value sequence, e.g. Esc[Line;ColumnH */
if (byte == ';' &&
!atomic_test_and_set_bit(&esc_state, ESC_ANSI_VAL_2)) {
return;
}
atomic_clear_bit(&esc_state, ESC_ANSI_VAL);
atomic_clear_bit(&esc_state, ESC_ANSI_VAL_2);
}
ansi_cmd:
switch (byte) {
case ANSI_BACKWARD:
if (ansi_val > cur) {
break;
}
end += ansi_val;
cur -= ansi_val;
cursor_backward(ansi_val);
break;
case ANSI_FORWARD:
if (ansi_val > end) {
break;
}
end -= ansi_val;
cur += ansi_val;
cursor_forward(ansi_val);
break;
case ANSI_HOME:
if (!cur) {
break;
}
cursor_backward(cur);
end += cur;
cur = 0U;
break;
case ANSI_END:
if (!end) {
break;
}
cursor_forward(end);
cur += end;
end = 0U;
break;
case ANSI_DEL:
if (!end) {
break;
}
cursor_forward(1);
del_char(&line[cur], --end);
break;
default:
break;
}
atomic_clear_bit(&esc_state, ESC_ANSI);
}
void on_nble_gap_connect_evt(const struct nble_gap_connect_evt *evt)
{
struct bt_conn *conn;
/* Make lookup to check if there's a connection object in CONNECT state
* associated with passed peer LE address.
*/
conn = bt_conn_lookup_state_le(&evt->peer_bda, BT_CONN_CONNECT);
#if 0
/* Nordic has no connection error */
if (evt->status) {
if (!conn) {
return;
}
conn->err = BT_HCI_ERR_UNACCEPT_CONN_PARAMS;
bt_conn_set_state(conn, BT_CONN_DISCONNECTED);
/* Drop the reference got by lookup call in CONNECT state.
* We are now in DISCONNECTED state since no successful LE
* link been made.
*/
bt_conn_unref(conn);
return;
}
#endif
/*
* clear advertising even if we are not able to add connection object
* to keep host in sync with controller state
*/
if (evt->role_slave == BT_CONN_ROLE_SLAVE) {
atomic_clear_bit(bt_dev.flags, BT_DEV_ADVERTISING);
}
if (!conn) {
conn = bt_conn_add_le(&evt->peer_bda);
}
if (!conn) {
BT_DBG("Unable to add new conn for handle %u",
evt->conn_handle);
return;
}
conn->handle = evt->conn_handle;
bt_addr_le_copy(&conn->le.dst, &evt->peer_bda);
conn->le.interval = evt->conn_values.interval;
conn->le.latency = evt->conn_values.latency;
conn->le.timeout = evt->conn_values.supervision_to;
conn->role = evt->role_slave;
#if 0
src.type = BT_ADDR_LE_PUBLIC;
memcpy(src.val, bt_dev.bdaddr.val, sizeof(bt_dev.bdaddr.val));
/* use connection address (instead of identity address) as initiator
* or responder address
*/
if (conn->role == BT_HCI_ROLE_MASTER) {
bt_addr_le_copy(&conn->le.init_addr, &src);
bt_addr_le_copy(&conn->le.resp_addr, &evt->peer_addr);
} else {
bt_addr_le_copy(&conn->le.init_addr, &evt->peer_addr);
bt_addr_le_copy(&conn->le.resp_addr, &src);
}
#endif
bt_conn_set_state(conn, BT_CONN_CONNECTED);
/* Note: Connection update removed because Windows interop and BT spec recommendations */
bt_conn_unref(conn);
#if 0
bt_le_scan_update(false);
#endif
}
static void handle_ansi(uint8_t byte)
{
if (atomic_test_and_clear_bit(&esc_state, ESC_ANSI_FIRST)) {
if (!isdigit(byte)) {
ansi_val = 1;
goto ansi_cmd;
}
atomic_set_bit(&esc_state, ESC_ANSI_VAL);
ansi_val = byte - '0';
ansi_val_2 = 0;
return;
}
if (atomic_test_bit(&esc_state, ESC_ANSI_VAL)) {
if (isdigit(byte)) {
if (atomic_test_bit(&esc_state, ESC_ANSI_VAL_2)) {
ansi_val_2 *= 10;
ansi_val_2 += byte - '0';
} else {
ansi_val *= 10;
ansi_val += byte - '0';
}
return;
}
/* Multi value sequence, e.g. Esc[Line;ColumnH */
if (byte == ';' &&
!atomic_test_and_set_bit(&esc_state, ESC_ANSI_VAL_2)) {
return;
}
atomic_clear_bit(&esc_state, ESC_ANSI_VAL);
atomic_clear_bit(&esc_state, ESC_ANSI_VAL_2);
}
ansi_cmd:
switch (byte) {
case ANSI_BACKWARD:
if (ansi_val > cur) {
break;
}
end += ansi_val;
cur -= ansi_val;
cursor_backward(ansi_val);
break;
case ANSI_FORWARD:
if (ansi_val > end) {
break;
}
end -= ansi_val;
cur += ansi_val;
cursor_forward(ansi_val);
break;
default:
break;
}
atomic_clear_bit(&esc_state, ESC_ANSI);
}
void main(void)
{
int failed, rv, i;
atomic_t target, orig;
atomic_val_t value;
atomic_val_t oldvalue;
failed = 0;
TC_START("Test atomic operation primitives");
TC_PRINT("Test atomic_cas()\n");
target = 4;
value = 5;
oldvalue = 6;
CHECK_OUTPUT(atomic_cas(&target, oldvalue, value), 0);
target = 6;
CHECK_OUTPUT(atomic_cas(&target, oldvalue, value), 1);
CHECK_OUTPUT(target, value);
TC_PRINT("Test atomic_add()\n");
target = 1;
value = 2;
CHECK_OUTPUT(atomic_add(&target, value), 1);
CHECK_OUTPUT(target, 3);
TC_PRINT("Test atomic_sub()\n");
target = 10;
value = 2;
CHECK_OUTPUT(atomic_sub(&target, value), 10);
CHECK_OUTPUT(target, 8);
TC_PRINT("Test atomic_inc()\n");
target = 5;
CHECK_OUTPUT(atomic_inc(&target), 5);
CHECK_OUTPUT(target, 6);
TC_PRINT("Test atomic_dec()\n");
target = 2;
CHECK_OUTPUT(atomic_dec(&target), 2);
CHECK_OUTPUT(target, 1);
TC_PRINT("Test atomic_get()\n");
target = 50;
CHECK_OUTPUT(atomic_get(&target), 50);
TC_PRINT("Test atomic_set()\n");
target = 42;
value = 77;
CHECK_OUTPUT(atomic_set(&target, value), 42);
CHECK_OUTPUT(target, value);
TC_PRINT("Test atomic_clear()\n");
target = 100;
CHECK_OUTPUT(atomic_clear(&target), 100);
CHECK_OUTPUT(target, 0);
TC_PRINT("Test atomic_or()\n");
target = 0xFF00;
value = 0x0F0F;
CHECK_OUTPUT(atomic_or(&target, value), 0xFF00);
CHECK_OUTPUT(target, 0xFF0F);
TC_PRINT("Test atomic_xor()\n");
target = 0xFF00;
value = 0x0F0F;
CHECK_OUTPUT(atomic_xor(&target, value), 0xFF00);
CHECK_OUTPUT(target, 0xF00F);
TC_PRINT("Test atomic_and()\n");
target = 0xFF00;
value = 0x0F0F;
CHECK_OUTPUT(atomic_and(&target, value), 0xFF00);
CHECK_OUTPUT(target, 0x0F00);
TC_PRINT("Test atomic_nand()\n");
target = 0xFF00;
value = 0x0F0F;
CHECK_OUTPUT(atomic_nand(&target, value), 0xFF00);
CHECK_OUTPUT(target, 0xFFFFF0FF);
TC_PRINT("Test atomic_test_bit()\n");
for (i = 0; i < 32; i++) {
target = 0x0F0F0F0F;
CHECK_TRUTH(atomic_test_bit(&target, i),
(target & (1 << i)));
}
TC_PRINT("Test atomic_test_and_clear_bit()\n");
for (i = 0; i < 32; i++) {
orig = 0x0F0F0F0F;
target = orig;
CHECK_TRUTH(atomic_test_and_clear_bit(&target, i),
(orig & (1 << i)));
CHECK_OUTPUT(target, orig & ~(1 << i));
}
TC_PRINT("Test atomic_test_and_set_bit()\n");
for (i = 0; i < 32; i++) {
orig = 0x0F0F0F0F;
target = orig;
CHECK_TRUTH(atomic_test_and_set_bit(&target, i),
(orig & (1 << i)));
CHECK_OUTPUT(target, orig | (1 << i));
}
TC_PRINT("Test atomic_clear_bit()\n");
for (i = 0; i < 32; i++) {
orig = 0x0F0F0F0F;
target = orig;
atomic_clear_bit(&target, i);
CHECK_OUTPUT(target, orig & ~(1 << i));
}
TC_PRINT("Test atomic_set_bit()\n");
for (i = 0; i < 32; i++) {
orig = 0x0F0F0F0F;
target = orig;
atomic_set_bit(&target, i);
CHECK_OUTPUT(target, orig | (1 << i));
}
if (failed) {
TC_PRINT("%d tests failed\n", failed);
rv = TC_FAIL;
} else {
rv = TC_PASS;
}
TC_END_RESULT(rv);
TC_END_REPORT(rv);
}
