static int init_vdi_state(uint64_t oid, const char *wd, uint32_t epoch)
{
int ret;
struct sd_inode *inode = xzalloc(SD_INODE_HEADER_SIZE);
struct siocb iocb = {
.epoch = epoch,
.buf = inode,
.length = SD_INODE_HEADER_SIZE,
};
ret = default_read(oid, &iocb);
if (ret != SD_RES_SUCCESS) {
sd_err("failed to read inode header %016" PRIx64 " %" PRId32
"wat %s", oid, epoch, wd);
goto out;
}
add_vdi_state_unordered(oid_to_vid(oid), inode->nr_copies,
vdi_is_snapshot(inode), inode->copy_policy,
inode->block_size_shift, inode->parent_vdi_id);
if (inode->name[0] == '\0')
atomic_set_bit(oid_to_vid(oid), sys->vdi_deleted);
atomic_set_bit(oid_to_vid(oid), sys->vdi_inuse);
ret = SD_RES_SUCCESS;
out:
free(inode);
return ret;
}
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));
}
int bt_conn_auth_pincode_entry(struct bt_conn *conn, const char *pin)
{
size_t len;
if (!bt_auth) {
return -EINVAL;
}
if (conn->type != BT_CONN_TYPE_BR) {
return -EINVAL;
}
len = strlen(pin);
if (len > 16) {
return -EINVAL;
}
if (conn->required_sec_level == BT_SECURITY_HIGH && len < 16) {
BT_WARN("PIN code for %s is not 16 bytes wide",
bt_addr_str(&conn->br.dst));
return -EPERM;
}
/* Allow user send entered PIN to remote, then reset user state. */
if (!atomic_test_and_clear_bit(conn->flags, BT_CONN_USER)) {
return -EPERM;
}
if (len == 16) {
atomic_set_bit(conn->flags, BT_CONN_BR_LEGACY_SECURE);
}
return pin_code_reply(conn, pin, len);
}
static int bt_init(void)
{
#if NOT_USED_FOR_NOW
struct bt_driver *drv = bt_dev.drv;
#endif
int err = 0;
#if NOT_USED_FOR_NOW
err = drv->open();
if (err) {
BT_ERR("HCI driver open failed (%d)", err);
return err;
}
err = hci_init();
#endif
if (!err) {
err = bt_conn_init();
}
scan_dev_found_cb = NULL;
if (!err) {
atomic_set_bit(bt_dev.flags, BT_DEV_READY);
#if 0
bt_le_scan_update(false);
#endif
}
return err;
}
void device_busy_set(struct device *busy_dev)
{
#ifdef CONFIG_DEVICE_POWER_MANAGEMENT
atomic_set_bit((atomic_t *) __device_busy_start,
(busy_dev - __device_init_start));
#endif
}
void on_nble_gap_start_advertise_rsp(const struct nble_response *params)
{
if (params->status == 0)
atomic_set_bit(bt_dev.flags, BT_DEV_ADVERTISING);
else
BT_WARN("start advertise failed with %d", params->status);
}
static int init_vdi_state(uint64_t oid, const char *wd, uint32_t epoch)
{
int ret;
struct sd_inode *inode = xzalloc(SD_INODE_HEADER_SIZE);
struct siocb iocb = {
.epoch = epoch,
.buf = inode,
.length = SD_INODE_HEADER_SIZE,
};
char path[PATH_MAX];
if (epoch == 0)
get_store_path(oid, iocb.ec_index, path);
else
get_store_stale_path(oid, iocb.epoch, iocb.ec_index, path);
ret = default_read_from_path(oid, path, &iocb);
if (ret != SD_RES_SUCCESS) {
sd_err("failed to read inode header %" PRIx64 " %" PRId32
"at %s", oid, epoch, path);
goto out;
}
atomic_set_bit(oid_to_vid(oid), sys->vdi_inuse);
out:
free(inode);
return ret;
}
void bt_conn_pin_code_req(struct bt_conn *conn)
{
if (bt_auth && bt_auth->pincode_entry) {
bool secure = false;
if (conn->required_sec_level == BT_SECURITY_HIGH) {
secure = true;
}
atomic_set_bit(conn->flags, BT_CONN_USER);
atomic_set_bit(conn->flags, BT_CONN_BR_PAIRING);
bt_auth->pincode_entry(conn, secure);
} else {
pin_code_neg_reply(&conn->br.dst);
}
}
void on_nble_sm_pairing_request_evt(const struct nble_sm_pairing_request_evt *evt)
{
struct bt_conn *conn;
struct bt_smp *smp;
BT_DBG("");
conn = bt_conn_lookup_handle(evt->conn_handle);
if (!conn) {
BT_ERR("Unable to find conn for handle %u", evt->conn_handle);
return;
}
smp = smp_chan_get(conn);
if (!smp) {
BT_ERR("No smp");
bt_conn_unref(conn);
return;
}
atomic_set_bit(&smp->flags, SMP_FLAG_PAIRING);
legacy_pairing_req(smp, evt->sec_param.remote_io);
bt_conn_unref(conn);
}
uint8_t tester_init_gap(void)
{
if (bt_enable(NULL) < 0) {
return BTP_STATUS_FAILED;
}
atomic_clear(¤t_settings);
atomic_set_bit(¤t_settings, GAP_SETTINGS_POWERED);
atomic_set_bit(¤t_settings, GAP_SETTINGS_CONNECTABLE);
atomic_set_bit(¤t_settings, GAP_SETTINGS_BONDABLE);
atomic_set_bit(¤t_settings, GAP_SETTINGS_LE);
bt_conn_cb_register(&conn_callbacks);
return BTP_STATUS_SUCCESS;
}
static int init_vdi_state(uint64_t oid, char *wd, uint32_t epoch)
{
int ret;
struct sd_inode *inode = xzalloc(SD_INODE_HEADER_SIZE);
struct siocb iocb = {
.epoch = epoch,
.buf = inode,
.length = SD_INODE_HEADER_SIZE,
};
ret = default_read(oid, &iocb);
if (ret != SD_RES_SUCCESS) {
sd_err("failed to read inode header %" PRIx64 " %" PRId32, oid,
epoch);
goto out;
}
add_vdi_state(oid_to_vid(oid), inode->nr_copies,
vdi_is_snapshot(inode));
atomic_set_bit(oid_to_vid(oid), sys->vdi_inuse);
ret = SD_RES_SUCCESS;
out:
free(inode);
return SD_RES_SUCCESS;
}
void bt_conn_ssp_auth(struct bt_conn *conn, uint32_t passkey)
{
conn->br.pairing_method = ssp_pair_method(conn);
/*
* If local required security is HIGH then MITM is mandatory.
* MITM protection is no achievable when SSP 'justworks' is applied.
*/
if (conn->required_sec_level > BT_SECURITY_MEDIUM &&
conn->br.pairing_method == JUST_WORKS) {
BT_DBG("MITM protection infeasible for required security");
ssp_confirm_neg_reply(conn);
return;
}
switch (conn->br.pairing_method) {
case PASSKEY_CONFIRM:
atomic_set_bit(conn->flags, BT_CONN_USER);
bt_auth->passkey_confirm(conn, passkey);
break;
case PASSKEY_DISPLAY:
atomic_set_bit(conn->flags, BT_CONN_USER);
bt_auth->passkey_display(conn, passkey);
break;
case PASSKEY_INPUT:
atomic_set_bit(conn->flags, BT_CONN_USER);
bt_auth->passkey_entry(conn);
break;
case JUST_WORKS:
/*
* When local host works as pairing acceptor and 'justworks'
* model is applied then notify user about such pairing request.
* [BT Core 4.2 table 5.7, Vol 3, Part C, 5.2.2.6]
*/
if (bt_auth && bt_auth->pairing_confirm &&
!atomic_test_bit(conn->flags,
BT_CONN_BR_PAIRING_INITIATOR)) {
atomic_set_bit(conn->flags, BT_CONN_USER);
bt_auth->pairing_confirm(conn);
break;
}
ssp_confirm_reply(conn);
break;
default:
break;
}
}
static int read_mcumgr_byte(uint8_t byte)
{
bool frag_1;
bool frag_2;
bool pkt_1;
bool pkt_2;
pkt_1 = atomic_test_bit(&esc_state, ESC_MCUMGR_PKT_1);
pkt_2 = atomic_test_bit(&esc_state, ESC_MCUMGR_PKT_2);
frag_1 = atomic_test_bit(&esc_state, ESC_MCUMGR_FRAG_1);
frag_2 = atomic_test_bit(&esc_state, ESC_MCUMGR_FRAG_2);
if (pkt_2 || frag_2) {
/* Already fully framed. */
return CONSOLE_MCUMGR_STATE_PAYLOAD;
}
if (pkt_1) {
if (byte == MCUMGR_SERIAL_HDR_PKT_2) {
/* Final framing byte received. */
atomic_set_bit(&esc_state, ESC_MCUMGR_PKT_2);
return CONSOLE_MCUMGR_STATE_PAYLOAD;
}
} else if (frag_1) {
if (byte == MCUMGR_SERIAL_HDR_FRAG_2) {
/* Final framing byte received. */
atomic_set_bit(&esc_state, ESC_MCUMGR_FRAG_2);
return CONSOLE_MCUMGR_STATE_PAYLOAD;
}
} else {
if (byte == MCUMGR_SERIAL_HDR_PKT_1) {
/* First framing byte received. */
atomic_set_bit(&esc_state, ESC_MCUMGR_PKT_1);
return CONSOLE_MCUMGR_STATE_HEADER;
} else if (byte == MCUMGR_SERIAL_HDR_FRAG_1) {
/* First framing byte received. */
atomic_set_bit(&esc_state, ESC_MCUMGR_FRAG_1);
return CONSOLE_MCUMGR_STATE_HEADER;
}
}
/* Non-mcumgr byte received. */
return CONSOLE_MCUMGR_STATE_NONE;
}
static void tester_init_gap_cb(int err)
{
if (err) {
tester_rsp(BTP_SERVICE_ID_CORE, CORE_REGISTER_SERVICE,
BTP_INDEX_NONE, BTP_STATUS_FAILED);
return;
}
atomic_clear(¤t_settings);
atomic_set_bit(¤t_settings, GAP_SETTINGS_POWERED);
atomic_set_bit(¤t_settings, GAP_SETTINGS_CONNECTABLE);
atomic_set_bit(¤t_settings, GAP_SETTINGS_BONDABLE);
atomic_set_bit(¤t_settings, GAP_SETTINGS_LE);
bt_conn_cb_register(&conn_callbacks);
tester_rsp(BTP_SERVICE_ID_CORE, CORE_REGISTER_SERVICE, BTP_INDEX_NONE,
BTP_STATUS_SUCCESS);
}
void on_nble_sm_security_request_evt(const struct nble_sm_security_request_evt *evt)
{
struct bt_conn *conn;
struct bt_smp *smp;
BT_DBG("");
conn = bt_conn_lookup_handle(evt->conn_handle);
if (!conn) {
BT_ERR("Unable to find conn for handle %u", evt->conn_handle);
return;
}
smp = smp_chan_get(conn);
if (!smp) {
BT_ERR("No smp");
bt_conn_unref(conn);
return;
}
BT_DBG("conn %p remote_io %u mitm %u", conn, evt->sec_param.remote_io,
evt->sec_param.mitm);
smp->method = legacy_get_pair_method(smp, evt->sec_param.remote_io);
if (smp->method == JUST_WORKS &&
nble.auth && nble.auth->pairing_confirm) {
atomic_set_bit(&smp->flags, SMP_FLAG_USER);
nble.auth->pairing_confirm(smp->conn);
goto done;
}
if (evt->sec_param.mitm) {
bt_conn_security(conn, BT_SECURITY_HIGH);
} else {
bt_conn_security(conn, BT_SECURITY_MEDIUM);
}
done:
atomic_set_bit(&smp->flags, SMP_FLAG_SEC_REQ);
bt_conn_unref(conn);
}
static void start_advertising(const uint8_t *data, uint16_t len)
{
const struct gap_start_advertising_cmd *cmd = (void *) data;
struct gap_start_advertising_rp rp;
uint8_t adv_len, sd_len;
bool adv_conn;
int i;
for (i = 0, adv_len = 1; i < cmd->adv_data_len; adv_len++) {
if (adv_len >= ARRAY_SIZE(ad)) {
BTTESTER_DBG("ad[] Out of memory");
goto fail;
}
ad[adv_len].type = cmd->adv_data[i++];
ad[adv_len].data_len = cmd->adv_data[i++];
ad[adv_len].data = &cmd->adv_data[i];
i += ad[adv_len].data_len;
}
for (i = 0, sd_len = 0; i < cmd->scan_rsp_len; sd_len++) {
if (sd_len >= ARRAY_SIZE(sd)) {
BTTESTER_DBG("sd[] Out of memory");
goto fail;
}
sd[sd_len].type = cmd->scan_rsp[i++];
sd[sd_len].data_len = cmd->scan_rsp[i++];
sd[sd_len].data = &cmd->scan_rsp[i];
i += sd[sd_len].data_len;
}
adv_conn = atomic_test_bit(¤t_settings, GAP_SETTINGS_CONNECTABLE);
/* BTP API don't allow to set empty scan response data. */
if (bt_le_adv_start(adv_conn ? BT_LE_ADV_CONN : BT_LE_ADV_NCONN,
ad, adv_len, sd_len ? sd : NULL, sd_len) < 0) {
BTTESTER_DBG("Failed to start advertising");
goto fail;
}
atomic_set_bit(¤t_settings, GAP_SETTINGS_ADVERTISING);
rp.current_settings = sys_cpu_to_le32(current_settings);
tester_send(BTP_SERVICE_ID_GAP, GAP_START_ADVERTISING, CONTROLLER_INDEX,
(uint8_t *) &rp, sizeof(rp));
return;
fail:
tester_rsp(BTP_SERVICE_ID_GAP, GAP_START_ADVERTISING, CONTROLLER_INDEX,
BTP_STATUS_FAILED);
}
static int init_objlist_and_vdi_bitmap(uint64_t oid, const char *wd,
uint32_t epoch, uint8_t ec_index,
struct vnode_info *vinfo,
void *arg)
{
int ret = SD_RES_SUCCESS;
objlist_cache_insert(oid);
if (is_vdi_obj(oid)) {
sd_debug("found the VDI object %" PRIx64" epoch %"PRIu32
" at %s", oid, epoch, wd);
atomic_set_bit(oid_to_vid(oid), sys->vdi_inuse);
}
return ret;
}
static void bt_iface_init(struct net_if *iface)
{
struct bt_context *ctxt = net_if_get_device(iface)->driver_data;
NET_DBG("iface %p", iface);
ctxt->iface = iface;
#if defined(CONFIG_NET_L2_BLUETOOTH_ZEP1656)
/* Workaround Linux bug, see:
* https://jira.zephyrproject.org/browse/ZEP-1656
*/
atomic_set_bit(iface->flags, NET_IF_POINTOPOINT);
#endif
}
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));
}
static int conn_auth(struct bt_conn *conn)
{
struct bt_hci_cp_auth_requested *auth;
struct net_buf *buf;
BT_DBG("");
buf = bt_hci_cmd_create(BT_HCI_OP_AUTH_REQUESTED, sizeof(*auth));
if (!buf) {
return -ENOBUFS;
}
auth = net_buf_add(buf, sizeof(*auth));
auth->handle = sys_cpu_to_le16(conn->handle);
atomic_set_bit(conn->flags, BT_CONN_BR_PAIRING_INITIATOR);
return bt_hci_cmd_send_sync(BT_HCI_OP_AUTH_REQUESTED, buf, NULL);
}
static void start_advertising(const uint8_t *data, uint16_t len)
{
const struct gap_start_advertising_cmd *cmd = (void *) data;
struct gap_start_advertising_rp rp;
uint8_t adv_type, adv_len;
int i;
if (atomic_test_bit(¤t_settings, GAP_SETTINGS_CONNECTABLE)) {
adv_type = BT_LE_ADV_IND;
} else {
adv_type = BT_LE_ADV_NONCONN_IND;
}
for (i = 0, adv_len = 1; i < cmd->adv_data_len; adv_len++) {
if (adv_len >= ARRAY_SIZE(ad)) {
BTTESTER_DBG("ad[] Out of memory");
goto fail;
}
ad[adv_len].type = cmd->adv_data[i++];
ad[adv_len].data_len = cmd->adv_data[i++];
ad[adv_len].data = &cmd->adv_data[i];
i += ad[adv_len].data_len;
}
if (bt_le_adv_start(BT_LE_ADV(adv_type), ad, adv_len, NULL, 0) < 0) {
BTTESTER_DBG("Failed to start advertising");
goto fail;
}
atomic_set_bit(¤t_settings, GAP_SETTINGS_ADVERTISING);
rp.current_settings = sys_cpu_to_le32(current_settings);
tester_send(BTP_SERVICE_ID_GAP, GAP_START_ADVERTISING, CONTROLLER_INDEX,
(uint8_t *) &rp, sizeof(rp));
return;
fail:
tester_rsp(BTP_SERVICE_ID_GAP, GAP_START_ADVERTISING, CONTROLLER_INDEX,
BTP_STATUS_FAILED);
}
static uint8_t legacy_pairing_req(struct bt_smp *smp, uint8_t remote_io)
{
struct nble_sm_pairing_response_req params;
smp->method = legacy_get_pair_method(smp, remote_io);
BT_DBG("method %u remote_io %u", smp->method, remote_io);
/* ask for consent if pairing is not due to sending SecReq*/
if (smp->method == JUST_WORKS &&
!atomic_test_bit(&smp->flags, SMP_FLAG_SEC_REQ) &&
nble.auth && nble.auth->pairing_confirm) {
atomic_set_bit(&smp->flags, SMP_FLAG_USER);
nble.auth->pairing_confirm(smp->conn);
return 0;
}
/* TODO: io caps */
params.conn = smp->conn;
params.conn_handle = smp->conn->handle;
nble_sm_pairing_response_req(¶ms);
return 0;
}
void uart_console_isr(struct device *unused)
{
ARG_UNUSED(unused);
while (uart_irq_update(uart_console_dev) &&
uart_irq_is_pending(uart_console_dev)) {
static struct uart_console_input *cmd;
uint8_t byte;
int rx;
if (!uart_irq_rx_ready(uart_console_dev)) {
continue;
}
/* Character(s) have been received */
rx = read_uart(uart_console_dev, &byte, 1);
if (rx < 0) {
return;
}
if (uart_irq_input_hook(uart_console_dev, byte) != 0) {
/*
* The input hook indicates that no further processing
* should be done by this handler.
*/
return;
}
if (!cmd) {
cmd = nano_isr_fifo_get(avail_queue, TICKS_NONE);
if (!cmd)
return;
}
/* Handle ANSI escape mode */
if (atomic_test_bit(&esc_state, ESC_ANSI)) {
handle_ansi(byte);
continue;
}
/* Handle escape mode */
if (atomic_test_and_clear_bit(&esc_state, ESC_ESC)) {
switch (byte) {
case ANSI_ESC:
atomic_set_bit(&esc_state, ESC_ANSI);
atomic_set_bit(&esc_state, ESC_ANSI_FIRST);
break;
default:
break;
}
continue;
}
/* Handle special control characters */
if (!isprint(byte)) {
switch (byte) {
case DEL:
if (cur > 0) {
del_char(&cmd->line[--cur], end);
}
break;
case ESC:
atomic_set_bit(&esc_state, ESC_ESC);
break;
case '\r':
cmd->line[cur + end] = '\0';
uart_poll_out(uart_console_dev, '\r');
uart_poll_out(uart_console_dev, '\n');
cur = 0;
end = 0;
nano_isr_fifo_put(lines_queue, cmd);
cmd = NULL;
break;
case '\t':
if (completion_cb && !end) {
cur += completion_cb(cmd->line, cur);
}
break;
default:
break;
}
continue;
}
/* Ignore characters if there's no more buffer space */
if (cur + end < sizeof(cmd->line) - 1) {
insert_char(&cmd->line[cur++], byte, end);
}
}
}
static int start_le_scan(uint8_t scan_type, uint16_t interval, uint16_t window,
uint8_t filter_dup)
{
struct nble_gap_scan_params params = {
.interval = interval,
.window = window,
.scan_type = scan_type,
};
#ifdef NOT_USED_FOR_NOW
struct net_buf *buf, *rsp;
struct bt_hci_cp_le_set_scan_params *set_param;
struct bt_hci_cp_le_set_scan_enable *scan_enable;
int err;
buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_SCAN_PARAMS,
sizeof(*set_param));
if (!buf) {
return -ENOBUFS;
}
set_param = net_buf_add(buf, sizeof(*set_param));
memset(set_param, 0, sizeof(*set_param));
set_param->scan_type = scan_type;
/* for the rest parameters apply default values according to
* spec 4.2, vol2, part E, 7.8.10
*/
set_param->interval = sys_cpu_to_le16(interval);
set_param->window = sys_cpu_to_le16(window);
set_param->filter_policy = 0x00;
if (scan_type == BT_HCI_LE_SCAN_ACTIVE) {
err = le_set_nrpa();
if (err) {
net_buf_unref(buf);
return err;
}
set_param->addr_type = BT_ADDR_LE_RANDOM;
} else {
set_param->addr_type = BT_ADDR_LE_PUBLIC;
}
bt_hci_cmd_send(BT_HCI_OP_LE_SET_SCAN_PARAMS, buf);
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 = filter_dup;
scan_enable->enable = BT_HCI_LE_SCAN_ENABLE;
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_set_bit(bt_dev.flags, BT_DEV_SCANNING);
}
net_buf_unref(rsp);
#endif
nble_gap_start_scan_req(¶ms);
return 0;
}
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 uart_console_isr(struct device *unused)
{
ARG_UNUSED(unused);
while (uart_irq_update(uart_console_dev) &&
uart_irq_is_pending(uart_console_dev)) {
static struct console_input *cmd;
u8_t byte;
int rx;
if (!uart_irq_rx_ready(uart_console_dev)) {
continue;
}
/* Character(s) have been received */
rx = read_uart(uart_console_dev, &byte, 1);
if (rx < 0) {
return;
}
#ifdef CONFIG_UART_CONSOLE_DEBUG_SERVER_HOOKS
if (debug_hook_in != NULL && debug_hook_in(byte) != 0) {
/*
* The input hook indicates that no further processing
* should be done by this handler.
*/
return;
}
#endif
if (!cmd) {
cmd = k_fifo_get(avail_queue, K_NO_WAIT);
if (!cmd) {
return;
}
}
#ifdef CONFIG_UART_CONSOLE_MCUMGR
/* Divert this byte from normal console handling if it is part
* of an mcumgr frame.
*/
if (handle_mcumgr(cmd, byte)) {
continue;
}
#endif /* CONFIG_UART_CONSOLE_MCUMGR */
/* Handle ANSI escape mode */
if (atomic_test_bit(&esc_state, ESC_ANSI)) {
handle_ansi(byte, cmd->line);
continue;
}
/* Handle escape mode */
if (atomic_test_and_clear_bit(&esc_state, ESC_ESC)) {
if (byte == ANSI_ESC) {
atomic_set_bit(&esc_state, ESC_ANSI);
atomic_set_bit(&esc_state, ESC_ANSI_FIRST);
}
continue;
}
/* Handle special control characters */
if (!isprint(byte)) {
switch (byte) {
case DEL:
if (cur > 0) {
del_char(&cmd->line[--cur], end);
}
break;
case ESC:
atomic_set_bit(&esc_state, ESC_ESC);
break;
case '\r':
cmd->line[cur + end] = '\0';
uart_poll_out(uart_console_dev, '\r');
uart_poll_out(uart_console_dev, '\n');
cur = 0;
end = 0;
k_fifo_put(lines_queue, cmd);
cmd = NULL;
break;
case '\t':
if (completion_cb && !end) {
cur += completion_cb(cmd->line, cur);
}
break;
default:
break;
}
continue;
}
/* Ignore characters if there's no more buffer space */
if (cur + end < sizeof(cmd->line) - 1) {
insert_char(&cmd->line[cur++], byte, end);
}
}
}
int bt_le_adv_start(const struct bt_le_adv_param *param,
const struct bt_data *ad, size_t ad_len,
const struct bt_data *sd, size_t sd_len)
{
int err;
struct nble_gap_adv_params set_param = {0};
struct nble_gap_ad_data_params data;
if (!valid_adv_param(param)) {
return -EINVAL;
}
memset(&data, 0, sizeof(data));
if (atomic_test_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING)) {
return -EALREADY;
}
err = set_advertise_disable();
if (err) {
return err;
}
err = set_ad(&data.ad, ad, ad_len);
if (err) {
return err;
}
/*
* Don't bother with scan response if the advertising type isn't
* a scannable one.
*/
if (param->type == BT_LE_ADV_IND || param->type == BT_LE_ADV_SCAN_IND) {
err = set_ad(&data.sd, sd, sd_len);
if (err) {
return err;
}
}
nble_gap_set_adv_data_req(&data);
/* Timeout is handled by application timer */
set_param.timeout = 0;
/* forced to none currently (no whitelist support) */
set_param.filter_policy = 0;
set_param.interval_max = param->interval_max;
set_param.interval_min = param->interval_min;
set_param.type = param->type;
nble_gap_set_adv_params_req(&set_param);
#if 0
if (param->addr_type == BT_LE_ADV_ADDR_NRPA) {
err = le_set_nrpa();
if (err) {
net_buf_unref(buf);
return err;
}
set_param->own_addr_type = BT_ADDR_LE_RANDOM;
} else {
set_param->own_addr_type = BT_ADDR_LE_PUBLIC;
}
bt_hci_cmd_send(BT_HCI_OP_LE_SET_ADV_PARAMETERS, buf);
#endif
err = set_advertise_enable();
if (err) {
return err;
}
atomic_set_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING);
return 0;
}
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 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");
}
}
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);
}
