void bu_hci::_set_hci_filter()
{
TRACE(__FUNCTION__);
struct _PAACK8
{
uint32_t filter0;
uint32_t filter1;
uint32_t filter2;
uint16_t filter3;
} filter =
{
btohl((1 << HCI_EVENT_PKT)| (1 << HCI_ACLDATA_PKT)),
#ifdef ACL_MTU_FRAG
btohl( (1 << EVT_NUM_COMP_PKTS)|
(1 << EVT_DISCONN_COMPLETE) |
(1 << EVT_ENCRYPT_CHANGE) |
(1 << EVT_CMD_COMPLETE) |
(1 << EVT_CMD_STATUS)),
#else
btohl( (1 << EVT_DISCONN_COMPLETE) |
(1 << EVT_ENCRYPT_CHANGE) |
(1 << EVT_CMD_COMPLETE) |
(1 << EVT_CMD_STATUS)),
#endif
btohs(1 << (EVT_LE_META_EVENT - 32)),
0
};
#ifdef DEBUG
bybuff by((const uint8_t*)&filter, sizeof(filter));
TRACE("[]<=" << by.to_string());
#endif
_socket->set_filter((const uint8_t*)&filter, sizeof(filter));
}
static int check_capabilities(struct a2dp_preset *preset,
struct avdtp_media_codec_capability *codec,
uint8_t codec_len)
{
a2dp_vendor_codec_t *vndcodec;
/* Codec specific */
switch (codec->media_codec_type) {
case A2DP_CODEC_SBC:
return sbc_check_config(codec->data, codec_len, preset->data,
preset->len);
case A2DP_CODEC_MPEG24:
return aac_check_config(codec->data, codec_len, preset->data,
preset->len);
case A2DP_CODEC_VENDOR:
vndcodec = (void *) codec->data;
if (btohl(vndcodec->vendor_id) == APTX_VENDOR_ID &&
btohs(vndcodec->codec_id) == APTX_CODEC_ID)
return aptx_check_config(codec->data, codec_len,
preset->data, preset->len);
return -EINVAL;
default:
return -EINVAL;
}
}
static uint8_t register_endpoint(const uint8_t *uuid, uint8_t codec,
GSList *presets)
{
struct a2dp_endpoint *endpoint;
/* FIXME: Add proper check for uuid */
endpoint = g_new0(struct a2dp_endpoint, 1);
endpoint->id = g_slist_length(endpoints) + 1;
endpoint->codec = codec;
endpoint->sep = avdtp_register_sep(AVDTP_SEP_TYPE_SOURCE,
AVDTP_MEDIA_TYPE_AUDIO,
codec, FALSE, &sep_ind,
&sep_cfm, endpoint);
endpoint->caps = presets->data;
endpoint->presets = g_slist_copy(g_slist_nth(presets, 1));
if (endpoint->codec == A2DP_CODEC_VENDOR) {
a2dp_vendor_codec_t *vndcodec = (void *) endpoint->caps->data;
avdtp_sep_set_vendor_codec(endpoint->sep,
btohl(vndcodec->vendor_id),
btohs(vndcodec->codec_id));
}
endpoints = g_slist_append(endpoints, endpoint);
return endpoint->id;
}
tBleStatus aci_updater_calc_crc(uint32_t address,
uint8_t num_sectors,
uint32_t *crc)
{
struct hci_request rq;
updater_calc_crc_cp cp;
updater_calc_crc_rp resp;
memset(&resp, 0, sizeof(resp));
cp.address = htobl(address);
cp.num_sectors = num_sectors;
memset(&rq, 0, sizeof(rq));
rq.ogf = OGF_VENDOR_CMD;
rq.ocf = OCF_UPDATER_CALC_CRC;
rq.cparam = &cp;
rq.clen = UPDATER_CALC_CRC_CP_SIZE;
rq.rparam = &resp;
rq.rlen = UPDATER_CALC_CRC_RP_SIZE;
if (hci_send_req(&rq) < 0)
return -1;
*crc = btohl(resp.crc);
return resp.status;
}
static uint32_t get_val(uint8_t *ptr, uint8_t len)
{
switch (len) {
case 1:
return *ptr;
case 2:
return btohs(bt_get_unaligned((uint16_t *) ptr));
case 4:
return btohl(bt_get_unaligned((uint32_t *) ptr));
}
return 0;
}
u8* LaichMeta_CompileU8(U8* input, u32* size)
{
u8archive* arc;
u32 offset = 0, data_offset, i;
u8* outbuf;
arc = calloc(1, sizeof(u8archive));
arc->string_table = calloc(1, 1); //Initial NULL
arc->stringsz = 1; //Initial NULL
arc->data = NULL;
arc->datasz = 0;
arc->nodes = NULL;
arc->numnodes = 0;
arc = U8_packdir(arc, input->rootdir, 0);
arc->rootnode.type = htobs(0x0100);
arc->rootnode.string_offset = 0;
arc->rootnode.data_offset = 0;
arc->rootnode.size = htobl(arc->numnodes);
arc->header.tag = htobl(0x55AA382D);
arc->header.root_offset = htobl(0x20);
arc->header.header_size = htobl((sizeof(u8node) * arc->numnodes) + arc->stringsz);
data_offset = inlinepad(0x20 + (sizeof(u8node) * arc->numnodes) + arc->stringsz, 0x40);
arc->header.data_offset = htobl(data_offset);
memset(arc->header.zeroes, 0, 16);
for(i = 0; i < arc->numnodes; i++) // fix the data offset
if (htobs(arc->nodes[i].type) == 0) // Checking if it is a file
arc->nodes[i].data_offset = htobl(btohl(arc->nodes[i].data_offset) + data_offset);
outbuf = calloc(1, sizeof(u8header) + (sizeof(u8node) * (arc->numnodes + 1)) + arc->stringsz + arc->datasz);
memcpy(outbuf + offset, &arc->header, sizeof(u8header));
offset += sizeof(u8header);
memcpy(outbuf + offset, &arc->rootnode, sizeof(u8node));
offset += sizeof(u8node);
memcpy(outbuf + offset, arc->nodes, sizeof(u8node) * arc->numnodes);
offset += sizeof(u8node) * arc->numnodes;
memcpy(outbuf + offset, arc->string_table, arc->stringsz);
offset += arc->stringsz;
memcpy(outbuf + offset, arc->data, arc->datasz);
offset += arc->datasz;
*size = offset;
free(arc);
return outbuf;
}
static void read_info_callback(uint8_t status, uint16_t length,
const void *param, void *user_data)
{
struct test_data *data = tester_get_data();
const struct mgmt_rp_read_info *rp = param;
char addr[18];
uint16_t manufacturer;
uint32_t supported_settings, current_settings;
tester_print("Read Info callback");
tester_print(" Status: 0x%02x", status);
if (status || !param) {
tester_pre_setup_failed();
return;
}
ba2str(&rp->bdaddr, addr);
manufacturer = btohs(rp->manufacturer);
supported_settings = btohl(rp->supported_settings);
current_settings = btohl(rp->current_settings);
tester_print(" Address: %s", addr);
tester_print(" Version: 0x%02x", rp->version);
tester_print(" Manufacturer: 0x%04x", manufacturer);
tester_print(" Supported settings: 0x%08x", supported_settings);
tester_print(" Current settings: 0x%08x", current_settings);
tester_print(" Class: 0x%02x%02x%02x",
rp->dev_class[2], rp->dev_class[1], rp->dev_class[0]);
tester_print(" Name: %s", rp->name);
tester_print(" Short name: %s", rp->short_name);
if (strcmp(hciemu_get_address(data->hciemu), addr)) {
tester_pre_setup_failed();
return;
}
tester_pre_setup_complete();
}
static void print_vendor(a2dp_vendor_codec_t *vendor)
{
uint32_t vendor_id = btohl(vendor->vendor_id);
uint16_t codec_id = btohs(vendor->codec_id);
printf("\tMedia Codec: Vendor Specific A2DP Codec");
printf("\n\t\tVendor ID 0x%08x", vendor_id);
printf("\n\t\tVendor Specific Codec ID 0x%04x\n", codec_id);
if (vendor_id == APTX_VENDOR_ID && codec_id == APTX_CODEC_ID)
print_aptx((void *) vendor);
}
static void mgmt_user_confirm_request(int sk, uint16_t index, void *buf,
size_t len)
{
struct mgmt_ev_user_confirm_request *ev = buf;
struct controller_info *info;
char addr[18];
int err;
if (len < sizeof(*ev)) {
error("Too small user_confirm_request event");
return;
}
ba2str(&ev->bdaddr, addr);
DBG("hci%u %s confirm_hint %u", index, addr, ev->confirm_hint);
if (index > max_index) {
error("Unexpected index %u in user_confirm_request event",
index);
return;
}
if (ev->confirm_hint) {
struct confirm_data *data;
data = g_new0(struct confirm_data, 1);
data->index = index;
bacpy(&data->bdaddr, &ev->bdaddr);
g_timeout_add_seconds_full(G_PRIORITY_DEFAULT, 1,
confirm_accept, data, g_free);
return;
}
info = &controllers[index];
err = btd_event_user_confirm(&info->bdaddr, &ev->bdaddr,
btohl(ev->value));
if (err < 0) {
error("btd_event_user_confirm: %s", strerror(-err));
mgmt_confirm_reply(index, &ev->bdaddr, FALSE);
}
}
static void mgmt_device_found(uint16_t len, const void *buf)
{
const struct mgmt_ev_device_found *ev = buf;
uint32_t flags;
char str[18];
if (len < sizeof(*ev)) {
printf("* Malformed Device Found control\n");
return;
}
flags = btohl(ev->flags);
ba2str(&ev->addr.bdaddr, str);
printf("@ Device Found: %s (%d) rssi %d flags 0x%4.4x\n",
str, ev->addr.type, ev->rssi, flags);
buf += sizeof(*ev);
len -= sizeof(*ev);
packet_hexdump(buf, len);
}
static void mgmt_passkey_notify(uint16_t len, const void *buf)
{
const struct mgmt_ev_passkey_notify *ev = buf;
uint32_t passkey;
char str[18];
if (len < sizeof(*ev)) {
printf("* Malformed Passkey Notify control\n");
return;
}
ba2str(&ev->addr.bdaddr, str);
passkey = btohl(ev->passkey);
printf("@ Passkey Notify: %s (%d) passkey %06u entered %u\n",
str, ev->addr.type, passkey, ev->entered);
buf += sizeof(*ev);
len -= sizeof(*ev);
packet_hexdump(buf, len);
}
static void recv_mode(int sk)
{
struct timeval tv_beg, tv_end, tv_diff;
char ts[30];
long total;
syslog(LOG_INFO, "Receiving ...");
memset(ts, 0, sizeof(ts));
while (1) {
gettimeofday(&tv_beg,NULL);
total = 0;
while (total < data_size) {
//uint32_t sq;
//uint16_t l;
int r;
if ((r = recv(sk, buf, data_size, 0)) < 0) {
if (r < 0)
syslog(LOG_ERR, "Read failed: %s (%d)",
strerror(errno), errno);
return;
}
if (timestamp) {
struct timeval tv;
if (ioctl(sk, SIOCGSTAMP, &tv) < 0) {
timestamp = 0;
memset(ts, 0, sizeof(ts));
} else {
sprintf(ts, "[%ld.%ld] ",
tv.tv_sec, tv.tv_usec);
}
}
#if 0
/* Check sequence */
sq = btohl(*(uint32_t *) buf);
if (seq != sq) {
syslog(LOG_INFO, "seq missmatch: %d -> %d", seq, sq);
seq = sq;
}
seq++;
/* Check length */
l = btohs(*(uint16_t *) (buf + 4));
if (r != l) {
syslog(LOG_INFO, "size missmatch: %d -> %d", r, l);
continue;
}
/* Verify data */
for (i = 6; i < r; i++) {
if (buf[i] != 0x7f)
syslog(LOG_INFO, "data missmatch: byte %d 0x%2.2x", i, buf[i]);
}
#endif
total += r;
}
gettimeofday(&tv_end,NULL);
timersub(&tv_end,&tv_beg,&tv_diff);
syslog(LOG_INFO,"%s%ld bytes in %.2f sec, %.2f kB/s", ts, total,
tv2fl(tv_diff), (float)(total / tv2fl(tv_diff) ) / 1024.0);
}
}
static int do_connect(const char *svr)
{
struct sockaddr_rc addr;
struct rfcomm_conninfo conn;
socklen_t optlen;
int sk, opt;
if (uuid != 0x0000)
channel = get_channel(svr, uuid);
if (channel == 0) {
syslog(LOG_ERR, "Can't get channel number");
return -1;
}
/* Create socket */
sk = socket(PF_BLUETOOTH, socktype, BTPROTO_RFCOMM);
if (sk < 0) {
syslog(LOG_ERR, "Can't create socket: %s (%d)",
strerror(errno), errno);
return -1;
}
/* Bind to local address */
memset(&addr, 0, sizeof(addr));
addr.rc_family = AF_BLUETOOTH;
bacpy(&addr.rc_bdaddr, &bdaddr);
if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
syslog(LOG_ERR, "Can't bind socket: %s (%d)",
strerror(errno), errno);
goto error;
}
#if 0
/* Enable SO_TIMESTAMP */
if (timestamp) {
int t = 1;
if (setsockopt(sk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) {
syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)",
strerror(errno), errno);
goto error;
}
}
#endif
/* Enable SO_LINGER */
if (linger) {
struct linger l = { .l_onoff = 1, .l_linger = linger };
if (setsockopt(sk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)",
strerror(errno), errno);
goto error;
}
}
/* Set link mode */
opt = 0;
if (master)
opt |= RFCOMM_LM_MASTER;
if (auth)
opt |= RFCOMM_LM_AUTH;
if (encrypt)
opt |= RFCOMM_LM_ENCRYPT;
if (secure)
opt |= RFCOMM_LM_SECURE;
if (opt && setsockopt(sk, SOL_RFCOMM, RFCOMM_LM, &opt, sizeof(opt)) < 0) {
syslog(LOG_ERR, "Can't set RFCOMM link mode: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Connect to remote device */
memset(&addr, 0, sizeof(addr));
addr.rc_family = AF_BLUETOOTH;
str2ba(svr, &addr.rc_bdaddr);
addr.rc_channel = channel;
if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
syslog(LOG_ERR, "Can't connect: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Get connection information */
memset(&conn, 0, sizeof(conn));
optlen = sizeof(conn);
if (getsockopt(sk, SOL_RFCOMM, RFCOMM_CONNINFO, &conn, &optlen) < 0) {
syslog(LOG_ERR, "Can't get RFCOMM connection information: %s (%d)",
strerror(errno), errno);
//goto error;
}
syslog(LOG_INFO, "Connected [handle %d, class 0x%02x%02x%02x]",
conn.hci_handle,
conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);
return sk;
error:
close(sk);
return -1;
}
static void do_listen(void (*handler)(int sk))
{
struct sockaddr_rc addr;
struct rfcomm_conninfo conn;
socklen_t optlen;
int sk, nsk, opt;
char ba[18];
/* Create socket */
sk = socket(PF_BLUETOOTH, socktype, BTPROTO_RFCOMM);
if (sk < 0) {
syslog(LOG_ERR, "Can't create socket: %s (%d)",
strerror(errno), errno);
exit(1);
}
/* Bind to local address */
memset(&addr, 0, sizeof(addr));
addr.rc_family = AF_BLUETOOTH;
bacpy(&addr.rc_bdaddr, &bdaddr);
addr.rc_channel = channel;
if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
syslog(LOG_ERR, "Can't bind socket: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Set link mode */
opt = 0;
if (master)
opt |= RFCOMM_LM_MASTER;
if (auth)
opt |= RFCOMM_LM_AUTH;
if (encrypt)
opt |= RFCOMM_LM_ENCRYPT;
if (secure)
opt |= RFCOMM_LM_SECURE;
if (opt && setsockopt(sk, SOL_RFCOMM, RFCOMM_LM, &opt, sizeof(opt)) < 0) {
syslog(LOG_ERR, "Can't set RFCOMM link mode: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Enable deferred setup */
opt = defer_setup;
if (opt && setsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP,
&opt, sizeof(opt)) < 0) {
syslog(LOG_ERR, "Can't enable deferred setup : %s (%d)",
strerror(errno), errno);
goto error;
}
/* Listen for connections */
if (listen(sk, 10)) {
syslog(LOG_ERR,"Can not listen on the socket: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Check for socket address */
memset(&addr, 0, sizeof(addr));
optlen = sizeof(addr);
if (getsockname(sk, (struct sockaddr *) &addr, &optlen) < 0) {
syslog(LOG_ERR, "Can't get socket name: %s (%d)",
strerror(errno), errno);
goto error;
}
channel = addr.rc_channel;
syslog(LOG_INFO, "Waiting for connection on channel %d ...", channel);
while (1) {
memset(&addr, 0, sizeof(addr));
optlen = sizeof(addr);
nsk = accept(sk, (struct sockaddr *) &addr, &optlen);
if (nsk < 0) {
syslog(LOG_ERR,"Accept failed: %s (%d)",
strerror(errno), errno);
goto error;
}
if (fork()) {
/* Parent */
close(nsk);
continue;
}
/* Child */
close(sk);
/* Get connection information */
memset(&conn, 0, sizeof(conn));
optlen = sizeof(conn);
if (getsockopt(nsk, SOL_RFCOMM, RFCOMM_CONNINFO, &conn, &optlen) < 0) {
syslog(LOG_ERR, "Can't get RFCOMM connection information: %s (%d)",
strerror(errno), errno);
//close(nsk);
//goto error;
}
ba2str(&addr.rc_bdaddr, ba);
syslog(LOG_INFO, "Connect from %s [handle %d, class 0x%02x%02x%02x]",
ba, conn.hci_handle,
conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);
#if 0
/* Enable SO_TIMESTAMP */
if (timestamp) {
int t = 1;
if (setsockopt(nsk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) {
syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)",
strerror(errno), errno);
goto error;
}
}
#endif
/* Enable SO_LINGER */
if (linger) {
struct linger l = { .l_onoff = 1, .l_linger = linger };
if (setsockopt(nsk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)",
strerror(errno), errno);
close(nsk);
goto error;
}
}
/* Handle deferred setup */
if (defer_setup) {
syslog(LOG_INFO, "Waiting for %d seconds",
abs(defer_setup) - 1);
sleep(abs(defer_setup) - 1);
if (defer_setup < 0) {
close(nsk);
goto error;
}
}
handler(nsk);
syslog(LOG_INFO, "Disconnect: %m");
exit(0);
}
return;
error:
close(sk);
exit(1);
}
static void dump_mode(int sk)
{
int len;
syslog(LOG_INFO, "Receiving ...");
while ((len = read(sk, buf, data_size)) > 0)
syslog(LOG_INFO, "Recevied %d bytes", len);
}
static void recv_mode(int sk)
{
struct timeval tv_beg, tv_end, tv_diff;
char ts[30];
long total;
uint32_t seq;
syslog(LOG_INFO, "Receiving ...");
memset(ts, 0, sizeof(ts));
seq = 0;
while (1) {
gettimeofday(&tv_beg,NULL);
total = 0;
while (total < data_size) {
//uint32_t sq;
//uint16_t l;
int r;
if ((r = recv(sk, buf, data_size, 0)) < 0) {
if (r < 0)
syslog(LOG_ERR, "Read failed: %s (%d)",
strerror(errno), errno);
return;
}
if (timestamp) {
struct timeval tv;
if (ioctl(sk, SIOCGSTAMP, &tv) < 0) {
timestamp = 0;
memset(ts, 0, sizeof(ts));
} else {
sprintf(ts, "[%ld.%ld] ",
tv.tv_sec, tv.tv_usec);
}
}
#if 0
/* Check sequence */
sq = btohl(*(uint32_t *) buf);
if (seq != sq) {
syslog(LOG_INFO, "seq missmatch: %d -> %d", seq, sq);
seq = sq;
}
seq++;
/* Check length */
l = btohs(*(uint16_t *) (buf + 4));
if (r != l) {
syslog(LOG_INFO, "size missmatch: %d -> %d", r, l);
continue;
}
/* Verify data */
for (i = 6; i < r; i++) {
if (buf[i] != 0x7f)
syslog(LOG_INFO, "data missmatch: byte %d 0x%2.2x", i, buf[i]);
}
#endif
total += r;
}
gettimeofday(&tv_end,NULL);
timersub(&tv_end,&tv_beg,&tv_diff);
syslog(LOG_INFO,"%s%ld bytes in %.2f sec, %.2f kB/s", ts, total,
tv2fl(tv_diff), (float)(total / tv2fl(tv_diff) ) / 1024.0);
}
}
static void do_send(int sk)
{
uint32_t seq;
int i, fd, len;
syslog(LOG_INFO,"Sending ...");
if (filename) {
fd = open(filename, O_RDONLY);
if (fd < 0) {
syslog(LOG_ERR, "Open failed: %s (%d)",
strerror(errno), errno);
exit(1);
}
len = read(fd, buf, data_size);
send(sk, buf, len, 0);
return;
} else {
for (i = 6; i < data_size; i++)
buf[i] = 0x7f;
}
seq = 0;
while ((num_frames == -1) || (num_frames-- > 0)) {
*(uint32_t *) buf = htobl(seq);
*(uint16_t *) (buf + 4) = htobs(data_size);
seq++;
if (send(sk, buf, data_size, 0) <= 0) {
syslog(LOG_ERR, "Send failed: %s (%d)",
strerror(errno), errno);
exit(1);
}
if (num_frames && delay && count && !(seq % count))
usleep(delay);
}
}