static void le_disconn_rsp(struct bt_l2cap *l2cap, uint8_t ident,
struct net_buf *buf)
{
struct bt_conn *conn = l2cap->chan.chan.conn;
struct bt_l2cap_le_chan *chan;
struct bt_l2cap_disconn_rsp *rsp = (void *)buf->data;
uint16_t dcid, scid;
if (buf->len < sizeof(*rsp)) {
BT_ERR("Too small LE disconn rsp packet size");
return;
}
dcid = sys_le16_to_cpu(rsp->dcid);
scid = sys_le16_to_cpu(rsp->scid);
BT_DBG("dcid 0x%04x scid 0x%04x", dcid, scid);
chan = l2cap_remove_tx_cid(conn, dcid);
if (!chan) {
return;
}
l2cap_chan_del(&chan->chan);
}
static int apds9960_channel_get(struct device *dev,
enum sensor_channel chan,
struct sensor_value *val)
{
struct apds9960_data *data = dev->driver_data;
switch (chan) {
case SENSOR_CHAN_LIGHT:
val->val1 = sys_le16_to_cpu(data->sample_crgb[0]);
val->val2 = 0;
break;
case SENSOR_CHAN_RED:
val->val1 = sys_le16_to_cpu(data->sample_crgb[1]);
val->val2 = 0;
break;
case SENSOR_CHAN_GREEN:
val->val1 = sys_le16_to_cpu(data->sample_crgb[2]);
val->val2 = 0;
break;
case SENSOR_CHAN_BLUE:
val->val1 = sys_le16_to_cpu(data->sample_crgb[3]);
val->val2 = 0;
break;
case SENSOR_CHAN_PROX:
val->val1 = data->pdata;
val->val2 = 0;
break;
default:
return -ENOTSUP;
}
return 0;
}
static int ak8975_sample_fetch(struct device *dev, enum sensor_channel chan)
{
struct ak8975_data *drv_data = dev->driver_data;
u8_t buf[6];
__ASSERT_NO_MSG(chan == SENSOR_CHAN_ALL);
if (i2c_reg_write_byte(drv_data->i2c, CONFIG_AK8975_I2C_ADDR,
AK8975_REG_CNTL, AK8975_MODE_MEASURE) < 0) {
LOG_ERR("Failed to start measurement.");
return -EIO;
}
k_busy_wait(AK8975_MEASURE_TIME_US);
if (i2c_burst_read(drv_data->i2c, CONFIG_AK8975_I2C_ADDR,
AK8975_REG_DATA_START, buf, 6) < 0) {
LOG_ERR("Failed to read sample data.");
return -EIO;
}
drv_data->x_sample = sys_le16_to_cpu(buf[0] | (buf[1] << 8));
drv_data->y_sample = sys_le16_to_cpu(buf[2] | (buf[3] << 8));
drv_data->z_sample = sys_le16_to_cpu(buf[4] | (buf[5] << 8));
return 0;
}
static int bmg160_sample_fetch(struct device *dev, enum sensor_channel chan)
{
struct bmg160_device_data *bmg160 = dev->driver_data;
union {
u8_t raw[7];
struct {
u16_t x_axis;
u16_t y_axis;
u16_t z_axis;
u8_t temp;
};
} buf __aligned(2);
/* do a burst read, to fetch all axis data */
if (bmg160_read(dev, BMG160_REG_RATE_X, buf.raw, sizeof(buf)) < 0) {
return -EIO;
}
bmg160->raw_gyro_xyz[0] = sys_le16_to_cpu(buf.x_axis);
bmg160->raw_gyro_xyz[1] = sys_le16_to_cpu(buf.y_axis);
bmg160->raw_gyro_xyz[2] = sys_le16_to_cpu(buf.z_axis);
bmg160->raw_temp = buf.temp;
return 0;
}
static void le_conn_param_update_req(struct bt_l2cap *l2cap, uint8_t ident,
struct net_buf *buf)
{
struct bt_conn *conn = l2cap->chan.chan.conn;
const struct bt_le_conn_param *param;
uint16_t min, max, latency, timeout;
bool params_valid;
struct bt_l2cap_sig_hdr *hdr;
struct bt_l2cap_conn_param_rsp *rsp;
struct bt_l2cap_conn_param_req *req = (void *)buf->data;
if (buf->len < sizeof(*req)) {
BT_ERR("Too small LE conn update param req");
return;
}
if (conn->role != BT_HCI_ROLE_MASTER) {
l2cap_send_reject(conn, ident, BT_L2CAP_REJ_NOT_UNDERSTOOD,
NULL, 0);
return;
}
min = sys_le16_to_cpu(req->min_interval);
max = sys_le16_to_cpu(req->max_interval);
latency = sys_le16_to_cpu(req->latency);
timeout = sys_le16_to_cpu(req->timeout);
param = BT_LE_CONN_PARAM(min, max, latency, timeout);
BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x timeout: 0x%4.4x",
min, max, latency, timeout);
buf = bt_l2cap_create_pdu(&le_sig);
if (!buf) {
return;
}
params_valid = bt_le_conn_params_valid(min, max, latency, timeout);
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->code = BT_L2CAP_CONN_PARAM_RSP;
hdr->ident = ident;
hdr->len = sys_cpu_to_le16(sizeof(*rsp));
rsp = net_buf_add(buf, sizeof(*rsp));
if (params_valid) {
rsp->result = sys_cpu_to_le16(BT_L2CAP_CONN_PARAM_ACCEPTED);
} else {
rsp->result = sys_cpu_to_le16(BT_L2CAP_CONN_PARAM_REJECTED);
}
bt_l2cap_send(l2cap->chan.chan.conn, BT_L2CAP_CID_LE_SIG, buf);
if (params_valid) {
bt_conn_le_conn_update(conn, param);
}
}
static void le_conn_rsp(struct bt_l2cap *l2cap, uint8_t ident,
struct net_buf *buf)
{
struct bt_conn *conn = l2cap->chan.chan.conn;
struct bt_l2cap_le_chan *chan;
struct bt_l2cap_le_conn_rsp *rsp = (void *)buf->data;
uint16_t dcid, mtu, mps, credits, result;
if (buf->len < sizeof(*rsp)) {
BT_ERR("Too small LE conn rsp packet size");
return;
}
dcid = sys_le16_to_cpu(rsp->dcid);
mtu = sys_le16_to_cpu(rsp->mtu);
mps = sys_le16_to_cpu(rsp->mps);
credits = sys_le16_to_cpu(rsp->credits);
result = sys_le16_to_cpu(rsp->result);
BT_DBG("dcid 0x%04x mtu %u mps %u credits %u result 0x%04x", dcid,
mtu, mps, credits, result);
if (result == BT_L2CAP_SUCCESS) {
chan = l2cap_lookup_ident(conn, ident);
} else {
chan = l2cap_remove_ident(conn, ident);
}
if (!chan) {
BT_ERR("Cannot find channel for ident %u", ident);
return;
}
switch (result) {
case BT_L2CAP_SUCCESS:
/* Reset ident since it is no longer pending */
chan->ident = 0;
chan->tx.cid = dcid;
chan->tx.mtu = mtu;
chan->tx.mps = mps;
if (chan->chan.ops && chan->chan.ops->connected) {
chan->chan.ops->connected(&chan->chan);
}
/* Give credits */
l2cap_chan_tx_give_credits(chan, credits);
l2cap_chan_rx_give_credits(chan, L2CAP_LE_MAX_CREDITS);
break;
/* TODO: Retry on Authentication and Encryption errors */
default:
l2cap_chan_del(&chan->chan);
}
}
static uint8_t att_read_group_req(struct bt_att *att, struct net_buf *buf)
{
struct bt_conn *conn = att->chan.conn;
struct bt_att_read_group_req *req;
uint16_t start_handle, end_handle, err_handle;
union {
struct bt_uuid uuid;
struct bt_uuid_16 u16;
struct bt_uuid_128 u128;
} u;
/* Type can only be UUID16 or UUID128 */
if (buf->len != sizeof(*req) + 2 && buf->len != sizeof(*req) + 16) {
return BT_ATT_ERR_INVALID_PDU;
}
req = (void *)buf->data;
start_handle = sys_le16_to_cpu(req->start_handle);
end_handle = sys_le16_to_cpu(req->end_handle);
net_buf_pull(buf, sizeof(*req));
if (!uuid_create(&u.uuid, buf)) {
return BT_ATT_ERR_UNLIKELY;
}
BT_DBG("start_handle 0x%04x end_handle 0x%04x type %s",
start_handle, end_handle, bt_uuid_str(&u.uuid));
if (!range_is_valid(start_handle, end_handle, &err_handle)) {
send_err_rsp(conn, BT_ATT_OP_READ_GROUP_REQ, err_handle,
BT_ATT_ERR_INVALID_HANDLE);
return 0;
}
/* Core v4.2, Vol 3, sec 2.5.3 Attribute Grouping:
* Not all of the grouping attributes can be used in the ATT
* Read By Group Type Request. The «Primary Service» and «Secondary
* Service» grouping types may be used in the Read By Group Type
* Request. The «Characteristic» grouping type shall not be used in
* the ATT Read By Group Type Request.
*/
if (bt_uuid_cmp(&u.uuid, BT_UUID_GATT_PRIMARY) &&
bt_uuid_cmp(&u.uuid, BT_UUID_GATT_SECONDARY)) {
send_err_rsp(conn, BT_ATT_OP_READ_GROUP_REQ, start_handle,
BT_ATT_ERR_UNSUPPORTED_GROUP_TYPE);
return 0;
}
return att_read_group_rsp(att, &u.uuid, start_handle, end_handle);
}
static void le_disconn_req(struct bt_l2cap *l2cap, uint8_t ident,
struct net_buf *buf)
{
struct bt_conn *conn = l2cap->chan.chan.conn;
struct bt_l2cap_le_chan *chan;
struct bt_l2cap_disconn_req *req = (void *)buf->data;
struct bt_l2cap_disconn_rsp *rsp;
struct bt_l2cap_sig_hdr *hdr;
uint16_t scid, dcid;
if (buf->len < sizeof(*req)) {
BT_ERR("Too small LE conn req packet size");
return;
}
dcid = sys_le16_to_cpu(req->dcid);
scid = sys_le16_to_cpu(req->scid);
BT_DBG("scid 0x%04x dcid 0x%04x", dcid, scid);
chan = l2cap_remove_tx_cid(conn, scid);
if (!chan) {
struct bt_l2cap_cmd_reject_cid_data data;
data.scid = req->scid;
data.dcid = req->dcid;
l2cap_send_reject(conn, ident, BT_L2CAP_REJ_INVALID_CID, &data,
sizeof(data));
return;
}
buf = bt_l2cap_create_pdu(&le_sig);
if (!buf) {
return;
}
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->code = BT_L2CAP_DISCONN_RSP;
hdr->ident = ident;
hdr->len = sys_cpu_to_le16(sizeof(*rsp));
rsp = net_buf_add(buf, sizeof(*rsp));
rsp->dcid = sys_cpu_to_le16(chan->rx.cid);
rsp->scid = sys_cpu_to_le16(chan->tx.cid);
l2cap_chan_del(&chan->chan);
bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf);
}
static uint8_t att_read_blob_req(struct bt_att *att, struct net_buf *buf)
{
struct bt_att_read_blob_req *req;
uint16_t handle, offset;
req = (void *)buf->data;
handle = sys_le16_to_cpu(req->handle);
offset = sys_le16_to_cpu(req->offset);
BT_DBG("handle 0x%04x offset %u", handle, offset);
return att_read_rsp(att, BT_ATT_OP_READ_BLOB_REQ,
BT_ATT_OP_READ_BLOB_RSP, handle, offset);
}
static uint8_t att_mtu_rsp(struct bt_att *att, struct net_buf *buf)
{
struct bt_att_exchange_mtu_rsp *rsp;
uint16_t mtu;
if (!att) {
return 0;
}
rsp = (void *)buf->data;
mtu = sys_le16_to_cpu(rsp->mtu);
BT_DBG("Server MTU %u", mtu);
/* Check if MTU is valid */
if (mtu < BT_ATT_DEFAULT_LE_MTU) {
return att_handle_rsp(att, NULL, 0, BT_ATT_ERR_INVALID_PDU);
}
att->chan.rx.mtu = min(mtu, CONFIG_BLUETOOTH_ATT_MTU);
/* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part F] page 484:
*
* A device's Exchange MTU Request shall contain the same MTU as the
* device's Exchange MTU Response (i.e. the MTU shall be symmetric).
*/
att->chan.tx.mtu = att->chan.rx.mtu;
BT_DBG("Negotiated MTU %u", att->chan.rx.mtu);
return att_handle_rsp(att, rsp, buf->len, 0);
}
static uint8_t *recv_cb(uint8_t *buf, size_t *off)
{
struct btp_hdr *cmd = (void *) buf;
uint8_t *new_buf;
uint16_t len;
if (*off < sizeof(*cmd)) {
return buf;
}
len = sys_le16_to_cpu(cmd->len);
if (len > BTP_MTU - sizeof(*cmd)) {
printk("BT tester: invalid packet length\n");
*off = 0;
return buf;
}
if (*off < sizeof(*cmd) + len) {
return buf;
}
new_buf = nano_fifo_get(&avail_queue, TICKS_NONE);
if (!new_buf) {
printk("BT tester: RX overflow\n");
*off = 0;
return buf;
}
nano_fifo_put(&cmds_queue, buf);
*off = 0;
return new_buf;
}
void bt_l2cap_recv(struct bt_conn *conn, struct net_buf *buf)
{
struct bt_l2cap_hdr *hdr = (void *)buf->data;
struct bt_l2cap_chan *chan;
uint16_t cid;
if (IS_ENABLED(CONFIG_BLUETOOTH_BREDR) &&
conn->type == BT_CONN_TYPE_BR) {
bt_l2cap_br_recv(conn, buf);
return;
}
if (buf->len < sizeof(*hdr)) {
BT_ERR("Too small L2CAP PDU received");
net_buf_unref(buf);
return;
}
cid = sys_le16_to_cpu(hdr->cid);
net_buf_pull(buf, sizeof(*hdr));
BT_DBG("Packet for CID %u len %u", cid, buf->len);
chan = bt_l2cap_le_lookup_rx_cid(conn, cid);
if (!chan) {
BT_WARN("Ignoring data for unknown CID 0x%04x", cid);
net_buf_unref(buf);
return;
}
l2cap_chan_recv(chan, buf);
net_buf_unref(buf);
}
static uint8_t att_signed_write_cmd(struct bt_att *att, struct net_buf *buf)
{
struct bt_conn *conn = att->chan.conn;
struct bt_att_signed_write_cmd *req;
uint16_t handle;
int err;
req = (void *)buf->data;
handle = sys_le16_to_cpu(req->handle);
BT_DBG("handle 0x%04x", handle);
/* Verifying data requires full buffer including attribute header */
net_buf_push(buf, sizeof(struct bt_att_hdr));
err = bt_smp_sign_verify(conn, buf);
if (err) {
BT_ERR("Error verifying data");
/* No response for this command */
return 0;
}
net_buf_pull(buf, sizeof(struct bt_att_hdr));
net_buf_pull(buf, sizeof(*req));
return att_write_rsp(conn, 0, 0, handle, 0, buf->data,
buf->len - sizeof(struct bt_att_signature));
}
static u8_t *recv_cb(u8_t *buf, size_t *off)
{
struct btp_hdr *cmd = (void *) buf;
u8_t *new_buf;
u16_t len;
if (*off < sizeof(*cmd)) {
return buf;
}
len = sys_le16_to_cpu(cmd->len);
if (len > BTP_MTU - sizeof(*cmd)) {
SYS_LOG_ERR("BT tester: invalid packet length");
*off = 0;
return buf;
}
if (*off < sizeof(*cmd) + len) {
return buf;
}
new_buf = k_fifo_get(&avail_queue, K_NO_WAIT);
if (!new_buf) {
SYS_LOG_ERR("BT tester: RX overflow");
*off = 0;
return buf;
}
k_fifo_put(&cmds_queue, buf);
*off = 0;
return new_buf;
}
static void le_conn_param_update_req(struct bt_l2cap *l2cap, uint8_t ident,
struct net_buf *buf)
{
struct bt_conn *conn = l2cap->chan.chan.conn;
struct bt_le_conn_param param;
struct bt_l2cap_conn_param_rsp *rsp;
struct bt_l2cap_conn_param_req *req = (void *)buf->data;
bool accepted;
if (buf->len < sizeof(*req)) {
BT_ERR("Too small LE conn update param req");
return;
}
if (conn->role != BT_HCI_ROLE_MASTER) {
l2cap_send_reject(conn, ident, BT_L2CAP_REJ_NOT_UNDERSTOOD,
NULL, 0);
return;
}
param.interval_min = sys_le16_to_cpu(req->min_interval);
param.interval_max = sys_le16_to_cpu(req->max_interval);
param.latency = sys_le16_to_cpu(req->latency);
param.timeout = sys_le16_to_cpu(req->timeout);
BT_DBG("min 0x%04x max 0x%04x latency: 0x%04x timeout: 0x%04x",
param.interval_min, param.interval_max, param.latency,
param.timeout);
buf = l2cap_create_le_sig_pdu(BT_L2CAP_CONN_PARAM_RSP, ident,
sizeof(*rsp));
accepted = le_param_req(conn, ¶m);
rsp = net_buf_add(buf, sizeof(*rsp));
if (accepted) {
rsp->result = sys_cpu_to_le16(BT_L2CAP_CONN_PARAM_ACCEPTED);
} else {
rsp->result = sys_cpu_to_le16(BT_L2CAP_CONN_PARAM_REJECTED);
}
bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf);
if (accepted) {
bt_conn_le_conn_update(conn, ¶m);
}
}
static uint8_t att_prepare_write_req(struct bt_att *att, struct net_buf *buf)
{
struct bt_conn *conn = att->chan.conn;
struct bt_att_prepare_write_req *req;
uint16_t handle, offset;
req = (void *)buf->data;
handle = sys_le16_to_cpu(req->handle);
offset = sys_le16_to_cpu(req->offset);
net_buf_pull(buf, sizeof(*req));
BT_DBG("handle 0x%04x offset %u", handle, offset);
return att_write_rsp(conn, BT_ATT_OP_PREPARE_WRITE_REQ,
BT_ATT_OP_PREPARE_WRITE_RSP, handle, offset,
buf->data, buf->len);
}
int bt_gatt_attr_write_ccc(struct bt_conn *conn,
const struct bt_gatt_attr *attr, const void *buf,
uint16_t len, uint16_t offset)
{
struct _bt_gatt_ccc *ccc = attr->user_data;
const uint16_t *data = buf;
bool bonded;
size_t i;
if (offset > sizeof(*data)) {
return -EINVAL;
}
if (offset + len > sizeof(*data)) {
return -EFBIG;
}
if (bt_keys_find_addr(&conn->le.dst))
bonded = true;
else
bonded = false;
for (i = 0; i < ccc->cfg_len; i++) {
/* Check for existing configuration */
if (!bt_addr_le_cmp(&ccc->cfg[i].peer, &conn->le.dst)) {
break;
}
}
if (i == ccc->cfg_len) {
for (i = 0; i < ccc->cfg_len; i++) {
/* Check for unused configuration */
if (!ccc->cfg[i].valid) {
bt_addr_le_copy(&ccc->cfg[i].peer, &conn->le.dst);
/* Only set valid if bonded */
ccc->cfg[i].valid = bonded;
break;
}
}
if (i == ccc->cfg_len) {
BT_WARN("No space to store CCC cfg");
return -ENOMEM;
}
}
ccc->cfg[i].value = sys_le16_to_cpu(*data);
BT_DBG("handle 0x%04x value %u", attr->handle, ccc->cfg[i].value);
/* Update cfg if don't match */
if (ccc->cfg[i].value != ccc->value) {
gatt_ccc_changed(ccc);
}
return len;
}
static void le_conn_param_rsp(struct bt_l2cap *l2cap, struct net_buf *buf)
{
struct bt_l2cap_conn_param_rsp *rsp = (void *)buf->data;
if (buf->len < sizeof(*rsp)) {
BT_ERR("Too small LE conn param rsp");
return;
}
BT_DBG("LE conn param rsp result %u", sys_le16_to_cpu(rsp->result));
}
static void le_credits(struct bt_l2cap *l2cap, uint8_t ident,
struct net_buf *buf)
{
struct bt_conn *conn = l2cap->chan.chan.conn;
struct bt_l2cap_chan *chan;
struct bt_l2cap_le_credits *ev = (void *)buf->data;
struct bt_l2cap_le_chan *ch;
uint16_t credits, cid;
if (buf->len < sizeof(*ev)) {
BT_ERR("Too small LE Credits packet size");
return;
}
cid = sys_le16_to_cpu(ev->cid);
credits = sys_le16_to_cpu(ev->credits);
BT_DBG("cid 0x%04x credits %u", cid, credits);
chan = bt_l2cap_le_lookup_tx_cid(conn, cid);
if (!chan) {
BT_ERR("Unable to find channel of LE Credits packet");
return;
}
ch = BT_L2CAP_LE_CHAN(chan);
if (k_sem_count_get(&ch->tx.credits) + credits > UINT16_MAX) {
BT_ERR("Credits overflow");
bt_l2cap_chan_disconnect(chan);
return;
}
l2cap_chan_tx_give_credits(ch, credits);
BT_DBG("chan %p total credits %u", ch,
k_sem_count_get(&ch->tx.credits));
l2cap_chan_le_send_resume(ch);
}
static inline void get_acl_hdr(void)
{
struct bt_hci_acl_hdr *hdr = &rx.acl;
int to_read = sizeof(*hdr) - rx.remaining;
rx.remaining -= uart_fifo_read(h4_dev, (u8_t *)hdr + to_read,
rx.remaining);
if (!rx.remaining) {
rx.remaining = sys_le16_to_cpu(hdr->len);
BT_DBG("Got ACL header. Payload %u bytes", rx.remaining);
rx.have_hdr = true;
}
}
static uint8_t att_find_info_req(struct bt_att *att, struct net_buf *buf)
{
struct bt_conn *conn = att->chan.conn;
struct bt_att_find_info_req *req;
uint16_t start_handle, end_handle, err_handle;
req = (void *)buf->data;
start_handle = sys_le16_to_cpu(req->start_handle);
end_handle = sys_le16_to_cpu(req->end_handle);
BT_DBG("start_handle 0x%04x end_handle 0x%04x", start_handle,
end_handle);
if (!range_is_valid(start_handle, end_handle, &err_handle)) {
send_err_rsp(conn, BT_ATT_OP_FIND_INFO_REQ, err_handle,
BT_ATT_ERR_INVALID_HANDLE);
return 0;
}
return att_find_info_rsp(att, start_handle, end_handle);
}
static uint8_t att_read_req(struct bt_att *att, struct net_buf *buf)
{
struct bt_att_read_req *req;
uint16_t handle;
req = (void *)buf->data;
handle = sys_le16_to_cpu(req->handle);
BT_DBG("handle 0x%04x", handle);
return att_read_rsp(att, BT_ATT_OP_READ_REQ, BT_ATT_OP_READ_RSP,
handle, 0);
}
static uint8_t att_read_type_req(struct bt_att *att, struct net_buf *buf)
{
struct bt_conn *conn = att->chan.conn;
struct bt_att_read_type_req *req;
uint16_t start_handle, end_handle, err_handle;
union {
struct bt_uuid uuid;
struct bt_uuid_16 u16;
struct bt_uuid_128 u128;
} u;
/* Type can only be UUID16 or UUID128 */
if (buf->len != sizeof(*req) + 2 && buf->len != sizeof(*req) + 16) {
return BT_ATT_ERR_INVALID_PDU;
}
req = (void *)buf->data;
start_handle = sys_le16_to_cpu(req->start_handle);
end_handle = sys_le16_to_cpu(req->end_handle);
net_buf_pull(buf, sizeof(*req));
if (!uuid_create(&u.uuid, buf)) {
return BT_ATT_ERR_UNLIKELY;
}
BT_DBG("start_handle 0x%04x end_handle 0x%04x type %s",
start_handle, end_handle, bt_uuid_str(&u.uuid));
if (!range_is_valid(start_handle, end_handle, &err_handle)) {
send_err_rsp(conn, BT_ATT_OP_READ_TYPE_REQ, err_handle,
BT_ATT_ERR_INVALID_HANDLE);
return 0;
}
return att_read_type_rsp(att, &u.uuid, start_handle, end_handle);
}
static uint8_t att_find_type_req(struct bt_att *att, struct net_buf *buf)
{
struct bt_conn *conn = att->chan.conn;
struct bt_att_find_type_req *req;
uint16_t start_handle, end_handle, err_handle, type;
uint8_t *value;
req = (void *)buf->data;
start_handle = sys_le16_to_cpu(req->start_handle);
end_handle = sys_le16_to_cpu(req->end_handle);
type = sys_le16_to_cpu(req->type);
value = net_buf_pull(buf, sizeof(*req));
BT_DBG("start_handle 0x%04x end_handle 0x%04x type %u", start_handle,
end_handle, type);
if (!range_is_valid(start_handle, end_handle, &err_handle)) {
send_err_rsp(conn, BT_ATT_OP_FIND_TYPE_REQ, err_handle,
BT_ATT_ERR_INVALID_HANDLE);
return 0;
}
/* The Attribute Protocol Find By Type Value Request shall be used with
* the Attribute Type parameter set to the UUID for «Primary Service»
* and the Attribute Value set to the 16-bit Bluetooth UUID or 128-bit
* UUID for the specific primary service.
*/
if (type != BT_UUID_GATT_PRIMARY_VAL) {
send_err_rsp(conn, BT_ATT_OP_FIND_TYPE_REQ, start_handle,
BT_ATT_ERR_ATTRIBUTE_NOT_FOUND);
return 0;
}
return att_find_type_rsp(att, start_handle, end_handle, value,
buf->len);
}
static uint8_t att_mtu_req(struct bt_att *att, struct net_buf *buf)
{
struct bt_conn *conn = att->chan.conn;
struct bt_att_exchange_mtu_req *req;
struct bt_att_exchange_mtu_rsp *rsp;
struct net_buf *pdu;
uint16_t mtu_client, mtu_server;
req = (void *)buf->data;
mtu_client = sys_le16_to_cpu(req->mtu);
BT_DBG("Client MTU %u", mtu_client);
/* Check if MTU is valid */
if (mtu_client < BT_ATT_DEFAULT_LE_MTU) {
return BT_ATT_ERR_INVALID_PDU;
}
pdu = bt_att_create_pdu(conn, BT_ATT_OP_MTU_RSP, sizeof(*rsp));
if (!pdu) {
return BT_ATT_ERR_UNLIKELY;
}
mtu_server = CONFIG_BLUETOOTH_ATT_MTU;
BT_DBG("Server MTU %u", mtu_server);
rsp = net_buf_add(pdu, sizeof(*rsp));
rsp->mtu = sys_cpu_to_le16(mtu_server);
bt_l2cap_send(conn, BT_L2CAP_CID_ATT, pdu);
/* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part F] page 484:
*
* A device's Exchange MTU Request shall contain the same MTU as the
* device's Exchange MTU Response (i.e. the MTU shall be symmetric).
*/
att->chan.rx.mtu = min(mtu_client, mtu_server);
att->chan.tx.mtu = att->chan.rx.mtu;
BT_DBG("Negotiated MTU %u", att->chan.rx.mtu);
return 0;
}
static int bmi160_word_read(struct device *dev, uint8_t reg_addr,
uint16_t *word)
{
union {
uint8_t raw[3];
struct {
uint8_t dummy;
uint16_t word;
} __packed;
} buf;
if (bmi160_read(dev, reg_addr, buf.raw, 3) < 0) {
return -EIO;
}
*word = sys_le16_to_cpu(buf.word);
return 0;
}
static uint8_t att_write_cmd(struct bt_att *att, struct net_buf *buf)
{
struct bt_conn *conn = att->chan.conn;
struct bt_att_write_cmd *req;
uint16_t handle;
if (buf->len < sizeof(*req)) {
/* Commands don't have any response */
return 0;
}
req = (void *)buf->data;
handle = sys_le16_to_cpu(req->handle);
BT_DBG("handle 0x%04x", handle);
return att_write_rsp(conn, 0, 0, handle, 0, buf->data, buf->len);
}
static void cmd_handler(void *p1, void *p2, void *p3)
{
while (1) {
struct btp_hdr *cmd;
u16_t len;
cmd = k_fifo_get(&cmds_queue, K_FOREVER);
len = sys_le16_to_cpu(cmd->len);
/* TODO
* verify if service is registered before calling handler
*/
switch (cmd->service) {
case BTP_SERVICE_ID_CORE:
handle_core(cmd->opcode, cmd->index, cmd->data, len);
break;
case BTP_SERVICE_ID_GAP:
tester_handle_gap(cmd->opcode, cmd->index, cmd->data,
len);
break;
case BTP_SERVICE_ID_GATT:
tester_handle_gatt(cmd->opcode, cmd->index, cmd->data,
len);
break;
#if defined(CONFIG_BLUETOOTH_L2CAP_DYNAMIC_CHANNEL)
case BTP_SERVICE_ID_L2CAP:
tester_handle_l2cap(cmd->opcode, cmd->index, cmd->data,
len);
#endif /* CONFIG_BLUETOOTH_L2CAP_DYNAMIC_CHANNEL */
break;
default:
tester_rsp(cmd->service, cmd->opcode, cmd->index,
BTP_STATUS_FAILED);
break;
}
k_fifo_put(&avail_queue, cmd);
}
}
static struct bt_buf *bt_uart_acl_recv(int *remaining)
{
struct bt_hci_acl_hdr hdr;
struct bt_buf *buf;
/* We can ignore the return value since we pass len == min */
bt_uart_read(UART, (void *)&hdr, sizeof(hdr), sizeof(hdr));
buf = bt_buf_get(BT_ACL_IN, 0);
if (buf) {
memcpy(bt_buf_add(buf, sizeof(hdr)), &hdr, sizeof(hdr));
} else {
BT_ERR("No available ACL buffers!\n");
}
*remaining = sys_le16_to_cpu(hdr.len);
BT_DBG("len %u\n", *remaining);
return buf;
}
static struct net_buf *h4_acl_recv(int *remaining)
{
struct bt_hci_acl_hdr hdr;
struct net_buf *buf;
/* We can ignore the return value since we pass len == min */
h4_read(h4_dev, (void *)&hdr, sizeof(hdr), sizeof(hdr));
buf = bt_buf_get_acl();
if (buf) {
memcpy(net_buf_add(buf, sizeof(hdr)), &hdr, sizeof(hdr));
} else {
BT_ERR("No available ACL buffers!");
}
*remaining = sys_le16_to_cpu(hdr.len);
BT_DBG("len %u", *remaining);
return buf;
}
