static int
hostmode(char const *arg, int brief)
{
struct hostent *he = NULL;
bdaddr_t ba;
char bastr[32];
int reverse;
if (bt_aton(arg, &ba) == 1) {
reverse = 1;
he = bt_gethostbyaddr((char const *) &ba, sizeof(ba),
AF_BLUETOOTH);
} else {
reverse = 0;
he = bt_gethostbyname(arg);
}
if (he == NULL) {
herror(reverse? bt_ntoa(&ba, bastr) : arg);
return (1);
}
if (brief)
printf("%s", reverse? he->h_name :
bt_ntoa((bdaddr_t *)(he->h_addr), bastr));
else
printf("Host %s has %s %s\n",
reverse? bt_ntoa(&ba, bastr) : arg,
reverse? "name" : "address",
reverse? he->h_name :
bt_ntoa((bdaddr_t *)(he->h_addr), bastr));
return (0);
}
/* Process Link_Key_Request event */
static int
process_link_key_request_event(int sock, struct sockaddr_hci *addr,
bdaddr_p bdaddr)
{
link_key_p key = NULL;
syslog(LOG_DEBUG, "Got Link_Key_Request event from '%s', " \
"remote bdaddr %s", addr->hci_node,
bt_ntoa(bdaddr, NULL));
if ((key = get_key(bdaddr, 0)) != NULL) {
syslog(LOG_DEBUG, "Found matching entry, " \
"remote bdaddr %s, name '%s', link key %s",
bt_ntoa(&key->bdaddr, NULL),
(key->name != NULL)? key->name : "No name",
(key->key != NULL)? "exists" : "doesn't exist");
return (send_link_key_reply(sock, addr, bdaddr, key->key));
}
syslog(LOG_DEBUG, "Could not find link key for remote bdaddr %s",
bt_ntoa(bdaddr, NULL));
return (send_link_key_reply(sock, addr, bdaddr, NULL));
}
/* Process Link_Key_Notification event */
static int
process_link_key_notification_event(int sock, struct sockaddr_hci *addr,
ng_hci_link_key_notification_ep *ep)
{
link_key_p key = NULL;
syslog(LOG_DEBUG, "Got Link_Key_Notification event from '%s', " \
"remote bdaddr %s", addr->hci_node,
bt_ntoa(&ep->bdaddr, NULL));
if ((key = get_key(&ep->bdaddr, 1)) == NULL) {
syslog(LOG_ERR, "Could not find entry for remote bdaddr %s",
bt_ntoa(&ep->bdaddr, NULL));
return (-1);
}
syslog(LOG_DEBUG, "Updating link key for the entry, " \
"remote bdaddr %s, name '%s', link key %s",
bt_ntoa(&key->bdaddr, NULL),
(key->name != NULL)? key->name : "No name",
(key->key != NULL)? "exists" : "doesn't exist");
if (key->key == NULL) {
key->key = (uint8_t *) malloc(NG_HCI_KEY_SIZE);
if (key->key == NULL) {
syslog(LOG_ERR, "Could not allocate link key");
exit(1);
}
}
memcpy(key->key, &ep->key, NG_HCI_KEY_SIZE);
return (0);
}
/* Send PIN_Code_[Negative]_Reply */
static int
send_pin_code_reply(int sock, struct sockaddr_hci *addr,
bdaddr_p bdaddr, char const *pin)
{
uint8_t buffer[HCSECD_BUFFER_SIZE];
ng_hci_cmd_pkt_t *cmd = NULL;
memset(buffer, 0, sizeof(buffer));
cmd = (ng_hci_cmd_pkt_t *) buffer;
cmd->type = NG_HCI_CMD_PKT;
if (pin != NULL) {
ng_hci_pin_code_rep_cp *cp = NULL;
cmd->opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
NG_HCI_OCF_PIN_CODE_REP));
cmd->length = sizeof(*cp);
cp = (ng_hci_pin_code_rep_cp *)(cmd + 1);
memcpy(&cp->bdaddr, bdaddr, sizeof(cp->bdaddr));
strncpy((char *) cp->pin, pin, sizeof(cp->pin));
cp->pin_size = strlen((char const *) cp->pin);
syslog(LOG_DEBUG, "Sending PIN_Code_Reply to '%s' " \
"for remote bdaddr %s",
addr->hci_node, bt_ntoa(bdaddr, NULL));
} else {
ng_hci_pin_code_neg_rep_cp *cp = NULL;
cmd->opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
NG_HCI_OCF_PIN_CODE_NEG_REP));
cmd->length = sizeof(*cp);
cp = (ng_hci_pin_code_neg_rep_cp *)(cmd + 1);
memcpy(&cp->bdaddr, bdaddr, sizeof(cp->bdaddr));
syslog(LOG_DEBUG, "Sending PIN_Code_Negative_Reply to '%s' " \
"for remote bdaddr %s",
addr->hci_node, bt_ntoa(bdaddr, NULL));
}
again:
if (sendto(sock, buffer, sizeof(*cmd) + cmd->length, 0,
(struct sockaddr *) addr, sizeof(*addr)) < 0) {
if (errno == EINTR)
goto again;
syslog(LOG_ERR, "Could not send PIN code reply to '%s' " \
"for remote bdaddr %s. %s (%d)",
addr->hci_node, bt_ntoa(bdaddr, NULL),
strerror(errno), errno);
return (-1);
}
return (0);
}
/* Send Link_Key_[Negative]_Reply */
static int
send_link_key_reply(int sock, struct sockaddr_hci *addr,
bdaddr_p bdaddr, uint8_t *key)
{
uint8_t buffer[HCSECD_BUFFER_SIZE];
ng_hci_cmd_pkt_t *cmd = NULL;
memset(buffer, 0, sizeof(buffer));
cmd = (ng_hci_cmd_pkt_t *) buffer;
cmd->type = NG_HCI_CMD_PKT;
if (key != NULL) {
ng_hci_link_key_rep_cp *cp = NULL;
cmd->opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
NG_HCI_OCF_LINK_KEY_REP));
cmd->length = sizeof(*cp);
cp = (ng_hci_link_key_rep_cp *)(cmd + 1);
memcpy(&cp->bdaddr, bdaddr, sizeof(cp->bdaddr));
memcpy(&cp->key, key, sizeof(cp->key));
syslog(LOG_DEBUG, "Sending Link_Key_Reply to '%s' " \
"for remote bdaddr %s",
addr->hci_node, bt_ntoa(bdaddr, NULL));
} else {
ng_hci_link_key_neg_rep_cp *cp = NULL;
cmd->opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
NG_HCI_OCF_LINK_KEY_NEG_REP));
cmd->length = sizeof(*cp);
cp = (ng_hci_link_key_neg_rep_cp *)(cmd + 1);
memcpy(&cp->bdaddr, bdaddr, sizeof(cp->bdaddr));
syslog(LOG_DEBUG, "Sending Link_Key_Negative_Reply to '%s' " \
"for remote bdaddr %s",
addr->hci_node, bt_ntoa(bdaddr, NULL));
}
again:
if (sendto(sock, buffer, sizeof(*cmd) + cmd->length, 0,
(struct sockaddr *) addr, sizeof(*addr)) < 0) {
if (errno == EINTR)
goto again;
syslog(LOG_ERR, "Could not send link key reply to '%s' " \
"for remote bdaddr %s. %s (%d)",
addr->hci_node, bt_ntoa(bdaddr, NULL),
strerror(errno), errno);
return (-1);
}
return (0);
}
int32_t
client_rescan(bthid_server_p srv)
{
static hid_device_p d;
bthid_session_p s;
assert(srv != NULL);
if (connect_in_progress)
return (0); /* another connect is still pending */
d = get_next_hid_device(d);
if (d == NULL)
return (0); /* XXX should not happen? empty config? */
if ((s = session_by_bdaddr(srv, &d->bdaddr)) != NULL)
return (0); /* session already active */
if (!d->new_device) {
if (d->reconnect_initiate)
return (0); /* device will initiate reconnect */
}
syslog(LOG_NOTICE, "Opening outbound session for %s " \
"(new_device=%d, reconnect_initiate=%d)",
bt_ntoa(&d->bdaddr, NULL), d->new_device, d->reconnect_initiate);
if ((s = session_open(srv, d)) == NULL) {
syslog(LOG_CRIT, "Could not create outbound session for %s",
bt_ntoa(&d->bdaddr, NULL));
return (-1);
}
/* Open control channel */
s->ctrl = client_socket(&s->bdaddr, d->control_psm);
if (s->ctrl < 0) {
syslog(LOG_ERR, "Could not open control channel to %s. %s (%d)",
bt_ntoa(&s->bdaddr, NULL), strerror(errno), errno);
session_close(s);
return (-1);
}
s->state = W4CTRL;
FD_SET(s->ctrl, &srv->wfdset);
if (s->ctrl > srv->maxfd)
srv->maxfd = s->ctrl;
connect_in_progress = 1;
return (0);
}
char const *
hci_bdaddr2str(bdaddr_t const *ba)
{
extern int numeric_bdaddr;
static char buffer[MAXHOSTNAMELEN];
struct hostent *he = NULL;
if (memcmp(ba, NG_HCI_BDADDR_ANY, sizeof(*ba)) == 0) {
buffer[0] = '*';
buffer[1] = 0;
return (buffer);
}
if (!numeric_bdaddr &&
(he = bt_gethostbyaddr((char *)ba, sizeof(*ba), AF_BLUETOOTH)) != NULL) {
strlcpy(buffer, he->h_name, sizeof(buffer));
return (buffer);
}
bt_ntoa(ba, buffer);
return (buffer);
} /* hci_bdaddr2str */
/* Print BDADDR */
static char *
bdaddrpr(bdaddr_t const *ba)
{
extern int numeric_bdaddr;
static char str[24];
struct hostent *he = NULL;
if (memcmp(ba, NG_HCI_BDADDR_ANY, sizeof(*ba)) == 0) {
str[0] = '*';
str[1] = 0;
return (str);
}
if (!numeric_bdaddr &&
(he = bt_gethostbyaddr((char *)ba, sizeof(*ba), AF_BLUETOOTH)) != NULL) {
strlcpy(str, he->h_name, sizeof(str));
return (str);
}
bt_ntoa(ba, str);
return (str);
} /* bdaddrpr */
static int
hid_known(bdaddr_t *bdaddr, int argc, char **argv)
{
struct hid_device *hd = NULL;
struct hostent *he = NULL;
int e = FAILED;
if (read_config_file() == 0) {
if (read_hids_file() == 0) {
e = OK;
for (hd = get_next_hid_device(hd);
hd != NULL;
hd = get_next_hid_device(hd)) {
if (hd->new_device)
continue;
he = bt_gethostbyaddr((char *) &hd->bdaddr,
sizeof(hd->bdaddr),
AF_BLUETOOTH);
fprintf(stdout,
"%s %s\n", bt_ntoa(&hd->bdaddr, NULL),
(he != NULL && he->h_name != NULL)?
he->h_name : "");
}
}
clean_config();
}
return (e);
}
static char *
bdaddrpr(bdaddr_p const ba, char *str, int len)
{
static char buffer[MAXHOSTNAMELEN];
struct hostent *he = NULL;
if (str == NULL) {
str = buffer;
len = sizeof(buffer);
}
if (memcmp(ba, NG_HCI_BDADDR_ANY, sizeof(*ba)) == 0) {
str[0] = '*';
str[1] = 0;
return (str);
}
if (!numeric_bdaddr &&
(he = bt_gethostbyaddr((char *)ba, sizeof(*ba), AF_BLUETOOTH)) != NULL) {
strlcpy(str, he->h_name, len);
return (str);
}
bt_ntoa(ba, str);
return (str);
} /* bdaddrpr */
int32_t
hid_control(bthid_session_p s, uint8_t *data, int32_t len)
{
assert(s != NULL);
assert(data != NULL);
assert(len > 0);
switch (data[0] >> 4) {
case 0: /* Handshake (response to command) */
if (data[0] & 0xf)
syslog(LOG_ERR, "Got handshake message with error " \
"response 0x%x from %s",
data[0], bt_ntoa(&s->bdaddr, NULL));
break;
case 1: /* HID Control */
switch (data[0] & 0xf) {
case 0: /* NOP */
break;
case 1: /* Hard reset */
case 2: /* Soft reset */
syslog(LOG_WARNING, "Device %s requested %s reset",
bt_ntoa(&s->bdaddr, NULL),
((data[0] & 0xf) == 1)? "hard" : "soft");
break;
case 3: /* Suspend */
syslog(LOG_NOTICE, "Device %s requested Suspend",
bt_ntoa(&s->bdaddr, NULL));
break;
case 4: /* Exit suspend */
syslog(LOG_NOTICE, "Device %s requested Exit Suspend",
bt_ntoa(&s->bdaddr, NULL));
break;
case 5: /* Virtual cable unplug */
syslog(LOG_NOTICE, "Device %s unplugged virtual cable",
bt_ntoa(&s->bdaddr, NULL));
session_close(s);
break;
default:
syslog(LOG_WARNING, "Device %s sent unknown " \
"HID_Control message 0x%x",
bt_ntoa(&s->bdaddr, NULL), data[0]);
break;
}
break;
default:
syslog(LOG_WARNING, "Got unexpected message 0x%x on Control " \
"channel from %s", data[0], bt_ntoa(&s->bdaddr, NULL));
break;
}
return (0);
}
/*
* Look up pin in keys file. We store a dictionary for each
* remote address, and inside that we have a data object for
* each local address containing the pin.
*/
uint8_t *
lookup_pin_conf(bdaddr_t *laddr, bdaddr_t *raddr)
{
link_key_p key = NULL;
syslog(LOG_DEBUG, "Got Link_Pin_Request event from '%s', " \
"remote bdaddr %s", bt_ntoa(laddr, NULL),
bt_ntoa(raddr, NULL));
if ((key = get_key(raddr, 0)) != NULL) {
syslog(LOG_DEBUG, "Found matching entry, " \
"remote bdaddr %s, name '%s', pin %s",
bt_ntoa(&key->bdaddr, NULL),
(key->name != NULL)? key->name : "No name",
(key->pin != NULL)? "exists" : "doesn't exist");
return key->pin;
}
syslog(LOG_DEBUG, "Could not find link key for remote bdaddr %s",
bt_ntoa(raddr, NULL));
return NULL;
}
/* Send Read_Node_BD_ADDR command to the node */
static int
hci_read_node_bd_addr(int s, int argc, char **argv)
{
struct ng_btsocket_hci_raw_node_bdaddr r;
memset(&r, 0, sizeof(r));
if (ioctl(s, SIOC_HCI_RAW_NODE_GET_BDADDR, &r, sizeof(r)) < 0)
return (ERROR);
fprintf(stdout, "BD_ADDR: %s\n", bt_ntoa(&r.bdaddr, NULL));
return (OK);
} /* hci_read_node_bd_addr */
void
save_key(bdaddr_t *laddr, bdaddr_t *raddr, uint8_t * key)
{
link_key_p lkey = NULL;
syslog(LOG_DEBUG, "Got Link_Key_Notification event from '%s', " \
"remote bdaddr %s", bt_ntoa(laddr, NULL),
bt_ntoa(raddr, NULL));
if ((lkey = get_key(raddr, 1)) == NULL) {
syslog(LOG_ERR, "Could not find entry for remote bdaddr %s",
bt_ntoa(raddr, NULL));
return;
}
syslog(LOG_DEBUG, "Updating link key for the entry, " \
"remote bdaddr %s, name '%s', link key %s",
bt_ntoa(&lkey->bdaddr, NULL),
(lkey->name != NULL)? lkey->name : "No name",
(lkey->key != NULL)? "exists" : "doesn't exist");
if (lkey->key == NULL) {
lkey->key = (uint8_t *) malloc(HCI_KEY_SIZE);
if (lkey->key == NULL) {
syslog(LOG_ERR, "Could not allocate link key");
exit(1);
}
}
memcpy(lkey->key, key, HCI_KEY_SIZE);
dump_keys_file();
read_config_file();
read_keys_file();
return;
}
bthid_session_p
session_open(bthid_server_p srv, hid_device_p const d)
{
bthid_session_p s;
assert(srv != NULL);
assert(d != NULL);
if ((s = (bthid_session_p) malloc(sizeof(*s))) == NULL)
return (NULL);
s->srv = srv;
memcpy(&s->bdaddr, &d->bdaddr, sizeof(s->bdaddr));
s->ctrl = -1;
s->intr = -1;
if (d->keyboard) {
/* Open /dev/vkbdctl */
s->vkbd = open("/dev/vkbdctl", O_RDWR);
if (s->vkbd < 0) {
syslog(LOG_ERR, "Could not open /dev/vkbdctl " \
"for %s. %s (%d)", bt_ntoa(&s->bdaddr, NULL),
strerror(errno), errno);
free(s);
return (NULL);
}
} else
s->vkbd = -1;
s->state = CLOSED;
s->keys1 = bit_alloc(kbd_maxkey());
if (s->keys1 == NULL) {
free(s);
return (NULL);
}
s->keys2 = bit_alloc(kbd_maxkey());
if (s->keys2 == NULL) {
free(s->keys1);
free(s);
return (NULL);
}
LIST_INSERT_HEAD(&srv->sessions, s, next);
return (s);
}
int le_connect_result(s)
{
char buffer[512];
ng_hci_event_pkt_t *e;
ng_hci_le_ep *lep;
ng_hci_le_connection_complete_ep *cep;
int n;
char addrstring[50];
int err;
e = (ng_hci_event_pkt_t *)buffer;
lep = (ng_hci_le_ep *)(((char *)e)+(sizeof(*e)));
cep = (ng_hci_le_connection_complete_ep *)(((char *)lep)+(sizeof(*lep)));
n = sizeof(buffer);
if((err = hci_recv(s, buffer, &n))==ERROR){
printf("RECV Error\n");
return 0;
}
if(n < sizeof(*e)){
errno = EMSGSIZE;
return 0;
}
if(e->type != NG_HCI_EVENT_PKT){
printf("Event%d\n", e->type);
errno = EIO;
return 0;
}
printf("%d\n", lep->subevent_code);
if(lep->subevent_code != NG_HCI_LEEV_CON_COMPL){
printf("SubEvent%d\n", lep->subevent_code);
errno = EIO;
return 0;
}
printf("Connection Event:Status%d, handle%d, role%d, address_type:%d\n",
cep->status, cep->handle, cep->role, cep->address_type);
bt_ntoa(&cep->address, addrstring);
printf("%s %d %d %d %d\n", addrstring, cep->interval, cep->latency,
cep->supervision_timeout, cep->master_clock_accracy);
if(cep->status != 0){
printf("REQUEST ERROR %d\n", cep->status);
return 0;
}
return cep->handle;
}
/* Perform SDP query */
static int32_t
hid_query(bdaddr_t *bdaddr, int argc, char **argv)
{
struct hid_device hd;
int e;
memcpy(&hd.bdaddr, bdaddr, sizeof(hd.bdaddr));
if (hid_sdp_query(NULL, &hd, &e) < 0) {
fprintf(stderr, "Could not perform SDP query on the " \
"device %s. %s (%d)\n", bt_ntoa(bdaddr, NULL),
strerror(e), e);
return (FAILED);
}
print_hid_device(&hd, stdout);
return (OK);
}
/**
Read BD_ADDR
Vol2/Part E/7.4.6
*/
static int
read_bd_addr(int s, bdaddr_t *bdaddr)
{
ng_hci_read_bdaddr_rp rp;
int n = sizeof(rp);
int e = hci_simple_request(s,
NG_HCI_OPCODE(NG_HCI_OGF_INFO,
NG_HCI_OCF_READ_BDADDR),
(void *)&rp, &n);
if(e == 0){
if(bdaddr)
memcpy(bdaddr, &(rp.bdaddr), sizeof(*bdaddr));
char buf[18];
bt_ntoa(&(rp.bdaddr), buf);
printf("* Read_BD_ADDR: %s\n", buf);
}
return e;
}
/* Send Read_BD_ADDR command to the unit */
static int
hci_read_bd_addr(int s, int argc, char **argv)
{
ng_hci_read_bdaddr_rp rp;
int n;
n = sizeof(rp);
if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_INFO,
NG_HCI_OCF_READ_BDADDR), (char *) &rp, &n) == ERROR)
return (ERROR);
if (rp.status != 0x00) {
fprintf(stdout, "Status: %s [%#02x]\n",
hci_status2str(rp.status), rp.status);
return (FAILED);
}
fprintf(stdout, "BD_ADDR: %s\n", bt_ntoa(&rp.bdaddr, NULL));
return (OK);
} /* hci_read_bd_addr */
int
main(int argc, char *argv[])
{
bdaddr_t src, dst;
struct hostent *he;
uint8_t *echo_data;
struct sockaddr_l2cap sa;
int32_t n, s, count, wait, flood, echo_size, numeric;
char *endp, *rname;
/* Set defaults */
memcpy(&src, NG_HCI_BDADDR_ANY, sizeof(src));
memcpy(&dst, NG_HCI_BDADDR_ANY, sizeof(dst));
echo_data = (uint8_t *) calloc(NG_L2CAP_MAX_ECHO_SIZE, sizeof(uint8_t));
if (echo_data == NULL) {
fprintf(stderr, "Failed to allocate echo data buffer");
exit(1);
}
/*
* Set default echo size to the NG_L2CAP_MTU_MINIMUM minus
* the size of the L2CAP signalling command header.
*/
echo_size = NG_L2CAP_MTU_MINIMUM - sizeof(ng_l2cap_cmd_hdr_t);
count = -1; /* unimited */
wait = 1; /* sec */
flood = 0;
numeric = 0;
/* Parse command line arguments */
while ((n = getopt(argc, argv, "a:c:fi:nS:s:h")) != -1) {
switch (n) {
case 'a':
if (!bt_aton(optarg, &dst)) {
if ((he = bt_gethostbyname(optarg)) == NULL)
errx(1, "%s: %s", optarg, hstrerror(h_errno));
memcpy(&dst, he->h_addr, sizeof(dst));
}
break;
case 'c':
count = strtol(optarg, &endp, 10);
if (count <= 0 || *endp != '\0')
usage();
break;
case 'f':
flood = 1;
break;
case 'i':
wait = strtol(optarg, &endp, 10);
if (wait <= 0 || *endp != '\0')
usage();
break;
case 'n':
numeric = 1;
break;
case 'S':
if (!bt_aton(optarg, &src)) {
if ((he = bt_gethostbyname(optarg)) == NULL)
errx(1, "%s: %s", optarg, hstrerror(h_errno));
memcpy(&src, he->h_addr, sizeof(src));
}
break;
case 's':
echo_size = strtol(optarg, &endp, 10);
if (echo_size < sizeof(int32_t) ||
echo_size > NG_L2CAP_MAX_ECHO_SIZE ||
*endp != '\0')
usage();
break;
case 'h':
default:
usage();
break;
}
}
if (memcmp(&dst, NG_HCI_BDADDR_ANY, sizeof(dst)) == 0)
usage();
he = bt_gethostbyaddr((const char *)&dst, sizeof(dst), AF_BLUETOOTH);
if (he == NULL || he->h_name == NULL || he->h_name[0] == '\0' || numeric)
asprintf(&rname, "%s", bt_ntoa(&dst, NULL));
else
rname = strdup(he->h_name);
if (rname == NULL)
errx(1, "Failed to create remote hostname");
s = socket(PF_BLUETOOTH, SOCK_RAW, BLUETOOTH_PROTO_L2CAP);
if (s < 0)
err(2, "Could not create socket");
memset(&sa, 0, sizeof(sa));
sa.l2cap_len = sizeof(sa);
sa.l2cap_family = AF_BLUETOOTH;
memcpy(&sa.l2cap_bdaddr, &src, sizeof(sa.l2cap_bdaddr));
if (bind(s, (struct sockaddr *) &sa, sizeof(sa)) < 0)
err(3,
"Could not bind socket, src bdaddr=%s", bt_ntoa(&sa.l2cap_bdaddr, NULL));
memset(&sa, 0, sizeof(sa));
sa.l2cap_len = sizeof(sa);
sa.l2cap_family = AF_BLUETOOTH;
memcpy(&sa.l2cap_bdaddr, &dst, sizeof(sa.l2cap_bdaddr));
if (connect(s, (struct sockaddr *) &sa, sizeof(sa)) < 0)
err(4,
"Could not connect socket, dst bdaddr=%s", bt_ntoa(&sa.l2cap_bdaddr, NULL));
/* Fill pattern */
for (n = 0; n < echo_size; ) {
int32_t avail = min(echo_size - n, PATTERN_SIZE);
memcpy(echo_data + n, pattern, avail);
n += avail;
}
/* Start ping'ing */
for (n = 0; count == -1 || count > 0; n ++) {
struct ng_btsocket_l2cap_raw_ping r;
struct timeval a, b;
int32_t fail;
if (gettimeofday(&a, NULL) < 0)
err(5, "Could not gettimeofday(a)");
fail = 0;
*((int32_t *) echo_data) = htonl(n);
r.result = 0;
r.echo_size = echo_size;
r.echo_data = echo_data;
if (ioctl(s, SIOC_L2CAP_L2CA_PING, &r, sizeof(r)) < 0) {
r.result = errno;
fail = 1;
/*
warn("Could not ping, dst bdaddr=%s",
bt_ntoa(&r.echo_dst, NULL));
*/
}
if (gettimeofday(&b, NULL) < 0)
err(7, "Could not gettimeofday(b)");
tv_sub(&b, &a);
fprintf(stdout,
"%d bytes from %s seq_no=%d time=%.3f ms result=%#x %s\n",
r.echo_size,
rname,
ntohl(*((int32_t *)(r.echo_data))),
tv2msec(&b), r.result,
((fail == 0)? "" : strerror(errno)));
if (!flood) {
/* Wait */
a.tv_sec = wait;
a.tv_usec = 0;
select(0, NULL, NULL, NULL, &a);
}
if (count != -1)
count --;
}
free(rname);
free(echo_data);
close(s);
return (0);
} /* main */
static void
client_query(void)
{
uint8_t buffer[512];
sdp_attr_t attr;
uint32_t range;
void *ss;
int rv;
uint8_t *seq0, *seq1;
attr.flags = SDP_ATTR_INVALID;
attr.attr = 0;
attr.vlen = sizeof(buffer);
attr.value = buffer;
range = SDP_ATTR_RANGE(SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST,
SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST);
ss = sdp_open(&local_bdaddr, &remote_bdaddr);
if (ss == NULL || (errno = sdp_error(ss)) != 0) {
log_err("%s: %m", service_name);
exit(EXIT_FAILURE);
}
log_info("Searching for %s service at %s",
service_name, bt_ntoa(&remote_bdaddr, NULL));
rv = sdp_search(ss, 1, &service_class, 1, &range, 1, &attr);
if (rv != 0) {
log_err("%s: %s", service_name, strerror(sdp_error(ss)));
exit(EXIT_FAILURE);
}
sdp_close(ss);
if (attr.flags != SDP_ATTR_OK
|| attr.attr != SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST) {
log_err("%s service not found", service_name);
exit(EXIT_FAILURE);
}
/*
* we expect the following protocol descriptor list
*
* seq len
* seq len
* uuid value == L2CAP
* uint16 value16 => PSM
* seq len
* uuid value == BNEP
*/
if (_sdp_get_seq(&attr.value, attr.value + attr.vlen, &seq0)
&& _sdp_get_seq(&seq0, attr.value, &seq1)
&& _sdp_match_uuid16(&seq1, seq0, SDP_UUID_PROTOCOL_L2CAP)
&& _sdp_get_uint16(&seq1, seq0, &l2cap_psm)
&& _sdp_get_seq(&seq0, attr.value, &seq1)
&& _sdp_match_uuid16(&seq1, seq0, SDP_UUID_PROTOCOL_BNEP)) {
log_info("Found PSM %d for service %s", l2cap_psm, service_name);
return;
}
log_err("%s query failed", service_name);
exit(EXIT_FAILURE);
}
/* Execute commands */
static int
do_l2cap_command(bdaddr_p bdaddr, int argc, char **argv)
{
char *cmd = argv[0];
struct l2cap_command *c = NULL;
struct sockaddr_l2cap sa;
int s, e, help;
help = 0;
if (strcasecmp(cmd, "help") == 0) {
argc --;
argv ++;
if (argc <= 0) {
fprintf(stdout, "Supported commands:\n");
print_l2cap_command(l2cap_commands);
fprintf(stdout, "\nFor more information use " \
"'help command'\n");
return (OK);
}
help = 1;
cmd = argv[0];
}
c = find_l2cap_command(cmd, l2cap_commands);
if (c == NULL) {
fprintf(stdout, "Unknown command: \"%s\"\n", cmd);
return (ERROR);
}
if (!help) {
if (memcmp(bdaddr, NG_HCI_BDADDR_ANY, sizeof(*bdaddr)) == 0)
usage();
memset(&sa, 0, sizeof(sa));
sa.l2cap_len = sizeof(sa);
sa.l2cap_family = AF_BLUETOOTH;
memcpy(&sa.l2cap_bdaddr, bdaddr, sizeof(sa.l2cap_bdaddr));
s = socket(PF_BLUETOOTH, SOCK_RAW, BLUETOOTH_PROTO_L2CAP);
if (s < 0)
err(1, "Could not create socket");
if (bind(s, (struct sockaddr *) &sa, sizeof(sa)) < 0)
err(2,
"Could not bind socket, bdaddr=%s", bt_ntoa(&sa.l2cap_bdaddr, NULL));
e = 0x0ffff;
if (setsockopt(s, SOL_SOCKET, SO_RCVBUF, &e, sizeof(e)) < 0)
err(3, "Coult not setsockopt(RCVBUF, %d)", e);
e = (c->handler)(s, -- argc, ++ argv);
close(s);
} else
e = USAGE;
switch (e) {
case OK:
case FAILED:
break;
case ERROR:
fprintf(stdout, "Could not execute command \"%s\". %s\n",
cmd, strerror(errno));
break;
case USAGE:
fprintf(stdout, "Usage: %s\n%s\n", c->command, c->description);
break;
default: assert(0); break;
}
return (e);
} /* do_l2cap_command */
void
client_init(void)
{
struct sockaddr_l2cap sa;
channel_t *chan;
socklen_t len;
int fd, n;
uint16_t mru, mtu;
if (bdaddr_any(&remote_bdaddr))
return;
if (service_name)
client_query();
fd = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BLUETOOTH_PROTO_L2CAP);
if (fd == -1) {
log_err("Could not open L2CAP socket: %m");
exit(EXIT_FAILURE);
}
memset(&sa, 0, sizeof(sa));
sa.l2cap_family = AF_BLUETOOTH;
sa.l2cap_len = sizeof(sa);
sa.l2cap_bdaddr_type = BDADDR_BREDR;
sa.l2cap_cid = 0;
bdaddr_copy(&sa.l2cap_bdaddr, &local_bdaddr);
if (bind(fd, (struct sockaddr *)&sa, sizeof(sa)) == -1) {
log_err("Could not bind client socket: %m");
exit(EXIT_FAILURE);
}
mru = BNEP_MTU_MIN;
if (setsockopt(fd, SOL_L2CAP, SO_L2CAP_IMTU, &mru, sizeof(mru)) == -1) {
log_err("Could not set L2CAP IMTU (%d): %m", mru);
exit(EXIT_FAILURE);
}
log_info("Opening connection to service 0x%4.4x at %s",
service_class, bt_ntoa(&remote_bdaddr, NULL));
sa.l2cap_psm = htole16(l2cap_psm);
bdaddr_copy(&sa.l2cap_bdaddr, &remote_bdaddr);
if (connect(fd, (struct sockaddr *)&sa, sizeof(sa)) == -1) {
log_err("Could not connect: %m");
exit(EXIT_FAILURE);
}
len = sizeof(mru);
if (getsockopt(fd, SOL_L2CAP, SO_L2CAP_IMTU, &mru, &len) == -1) {
log_err("Could not get IMTU: %m");
exit(EXIT_FAILURE);
}
if (mru < BNEP_MTU_MIN) {
log_err("L2CAP IMTU too small (%d)", mru);
exit(EXIT_FAILURE);
}
len = sizeof(n);
if (getsockopt(fd, SOL_SOCKET, SO_RCVBUF, &n, &len) == -1) {
log_err("Could not read SO_RCVBUF");
exit(EXIT_FAILURE);
}
if (n < (mru * 10)) {
n = mru * 10;
if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &n, sizeof(n)) == -1)
log_info("Could not increase SO_RCVBUF (from %d)", n);
}
len = sizeof(mtu);
if (getsockopt(fd, SOL_L2CAP, SO_L2CAP_OMTU, &mtu, &len) == -1) {
log_err("Could not get L2CAP OMTU: %m");
exit(EXIT_FAILURE);
}
if (mtu < BNEP_MTU_MIN) {
log_err("L2CAP OMTU too small (%d)", mtu);
exit(EXIT_FAILURE);
}
len = sizeof(n);
if (getsockopt(fd, SOL_SOCKET, SO_SNDBUF, &n, &len) == -1) {
log_err("Could not get socket send buffer size: %m");
close(fd);
return;
}
if (n < (mtu * 2)) {
n = mtu * 2;
if (setsockopt(fd, SOL_SOCKET, SO_SNDBUF, &n, sizeof(n)) == -1) {
log_err("Could not set socket send buffer size (%d): %m", n);
close(fd);
return;
}
}
n = mtu;
if (setsockopt(fd, SOL_SOCKET, SO_SNDLOWAT, &n, sizeof(n)) == -1) {
log_err("Could not set socket low water mark (%d): %m", n);
close(fd);
return;
}
chan = channel_alloc();
if (chan == NULL)
exit(EXIT_FAILURE);
chan->send = bnep_send;
chan->recv = bnep_recv;
chan->mru = mru;
chan->mtu = mtu;
b2eaddr(chan->raddr, &remote_bdaddr);
b2eaddr(chan->laddr, &local_bdaddr);
chan->state = CHANNEL_WAIT_CONNECT_RSP;
channel_timeout(chan, 10);
if (!channel_open(chan, fd))
exit(EXIT_FAILURE);
bnep_send_control(chan, BNEP_SETUP_CONNECTION_REQUEST,
2, service_class, SDP_SERVICE_CLASS_PANU);
}
int32_t
hid_interrupt(bthid_session_p s, uint8_t *data, int32_t len)
{
hid_device_p hid_device;
hid_data_t d;
hid_item_t h;
int32_t report_id, usage, page, val,
mouse_x, mouse_y, mouse_z, mouse_butt,
mevents, kevents, i;
assert(s != NULL);
assert(s->srv != NULL);
assert(data != NULL);
if (len < 3) {
syslog(LOG_ERR, "Got short message (%d bytes) on Interrupt " \
"channel from %s", len, bt_ntoa(&s->bdaddr, NULL));
return (-1);
}
if (data[0] != 0xa1) {
syslog(LOG_ERR, "Got unexpected message 0x%x on " \
"Interrupt channel from %s",
data[0], bt_ntoa(&s->bdaddr, NULL));
return (-1);
}
report_id = data[1];
data ++;
len --;
hid_device = get_hid_device(&s->bdaddr);
assert(hid_device != NULL);
mouse_x = mouse_y = mouse_z = mouse_butt = mevents = kevents = 0;
for (d = hid_start_parse(hid_device->desc, 1 << hid_input, -1);
hid_get_item(d, &h) > 0; ) {
if ((h.flags & HIO_CONST) || (h.report_ID != report_id) ||
(h.kind != hid_input))
continue;
page = HID_PAGE(h.usage);
val = hid_get_data(data, &h);
/*
* When the input field is an array and the usage is specified
* with a range instead of an ID, we have to derive the actual
* usage by using the item value as an index in the usage range
* list.
*/
if ((h.flags & HIO_VARIABLE)) {
usage = HID_USAGE(h.usage);
} else {
const uint32_t usage_offset = val - h.logical_minimum;
usage = HID_USAGE(h.usage_minimum + usage_offset);
}
switch (page) {
case HUP_GENERIC_DESKTOP:
switch (usage) {
case HUG_X:
mouse_x = val;
mevents ++;
break;
case HUG_Y:
mouse_y = val;
mevents ++;
break;
case HUG_WHEEL:
mouse_z = -val;
mevents ++;
break;
case HUG_SYSTEM_SLEEP:
if (val)
syslog(LOG_NOTICE, "Sleep button pressed");
break;
}
break;
case HUP_KEYBOARD:
kevents ++;
if (h.flags & HIO_VARIABLE) {
if (val && usage < kbd_maxkey())
bit_set(s->keys1, usage);
} else {
if (val && val < kbd_maxkey())
bit_set(s->keys1, val);
for (i = 1; i < h.report_count; i++) {
h.pos += h.report_size;
val = hid_get_data(data, &h);
if (val && val < kbd_maxkey())
bit_set(s->keys1, val);
}
}
break;
case HUP_BUTTON:
if (usage != 0) {
if (usage == 2)
usage = 3;
else if (usage == 3)
usage = 2;
mouse_butt |= (val << (usage - 1));
mevents ++;
}
break;
case HUP_CONSUMER:
if (!val)
break;
switch (usage) {
case HUC_AC_PAN:
/* Horizontal scroll */
if (val < 0)
mouse_butt |= (1 << 5);
else
mouse_butt |= (1 << 6);
mevents ++;
val = 0;
break;
case 0xb5: /* Scan Next Track */
val = 0x19;
break;
case 0xb6: /* Scan Previous Track */
val = 0x10;
break;
case 0xb7: /* Stop */
val = 0x24;
break;
case 0xcd: /* Play/Pause */
val = 0x22;
break;
case 0xe2: /* Mute */
val = 0x20;
break;
case 0xe9: /* Volume Up */
val = 0x30;
break;
case 0xea: /* Volume Down */
val = 0x2E;
break;
case 0x183: /* Media Select */
val = 0x6D;
break;
case 0x018a: /* Mail */
val = 0x6C;
break;
case 0x192: /* Calculator */
val = 0x21;
break;
case 0x194: /* My Computer */
val = 0x6B;
break;
case 0x221: /* WWW Search */
val = 0x65;
break;
case 0x223: /* WWW Home */
val = 0x32;
break;
case 0x224: /* WWW Back */
val = 0x6A;
break;
case 0x225: /* WWW Forward */
val = 0x69;
break;
case 0x226: /* WWW Stop */
val = 0x68;
break;
case 0x227: /* WWW Refresh */
val = 0x67;
break;
case 0x22a: /* WWW Favorites */
val = 0x66;
break;
default:
val = 0;
break;
}
/* XXX FIXME - UGLY HACK */
if (val != 0) {
if (hid_device->keyboard) {
int32_t buf[4] = { 0xe0, val,
0xe0, val|0x80 };
assert(s->vkbd != -1);
write(s->vkbd, buf, sizeof(buf));
} else
syslog(LOG_ERR, "Keyboard events " \
"received from non-keyboard " \
"device %s. Please report",
bt_ntoa(&s->bdaddr, NULL));
}
break;
case HUP_MICROSOFT:
switch (usage) {
case 0xfe01:
if (!hid_device->battery_power)
break;
switch (val) {
case 1:
syslog(LOG_INFO, "Battery is OK on %s",
bt_ntoa(&s->bdaddr, NULL));
break;
case 2:
syslog(LOG_NOTICE, "Low battery on %s",
bt_ntoa(&s->bdaddr, NULL));
break;
case 3:
syslog(LOG_WARNING, "Very low battery "\
"on %s",
bt_ntoa(&s->bdaddr, NULL));
break;
}
break;
}
break;
}
}
hid_end_parse(d);
/*
* XXX FIXME Feed keyboard events into kernel.
* The code below works, bit host also needs to track
* and handle repeat.
*
* Key repeat currently works in X, but not in console.
*/
if (kevents > 0) {
if (hid_device->keyboard) {
assert(s->vkbd != -1);
kbd_process_keys(s);
} else
syslog(LOG_ERR, "Keyboard events received from " \
"non-keyboard device %s. Please report",
bt_ntoa(&s->bdaddr, NULL));
}
/*
* XXX FIXME Feed mouse events into kernel.
* The code block below works, but it is not good enough.
* Need to track double-clicks etc.
*
* Double click currently works in X, but not in console.
*/
if (mevents > 0) {
struct mouse_info mi;
mi.operation = MOUSE_ACTION;
mi.u.data.x = mouse_x;
mi.u.data.y = mouse_y;
mi.u.data.z = mouse_z;
mi.u.data.buttons = mouse_butt;
if (ioctl(s->srv->cons, CONS_MOUSECTL, &mi) < 0)
syslog(LOG_ERR, "Could not process mouse events from " \
"%s. %s (%d)", bt_ntoa(&s->bdaddr, NULL),
strerror(errno), errno);
}
return (0);
}
int le_smpconnect(bdaddr_t *bd,int hci)
{
struct sockaddr_l2cap l2c;
int s;
unsigned char buf[40];
ssize_t len;
int i;
int count;
int handle = 0;
ng_l2cap_smp_pairinfo preq, pres;
fd_set rfds,wfds;
uint8_t k[16];
int conok = 0;
struct sockaddr_l2cap myname;
s = socket(PF_BLUETOOTH, SOCK_SEQPACKET|SOCK_NONBLOCK,
BLUETOOTH_PROTO_L2CAP);
l2c.l2cap_len = sizeof(l2c);
l2c.l2cap_family = AF_BLUETOOTH;
l2c.l2cap_psm = 0;
l2c.l2cap_cid = NG_L2CAP_SMP_CID;
l2c.l2cap_bdaddr_type = BDADDR_LE_PUBLIC;
bcopy(bd, &l2c.l2cap_bdaddr, sizeof(*bd));
printf("CONNECT\n");
if(connect(s, (struct sockaddr *) &l2c, sizeof(l2c)) == 0){
printf("CONNECTOK\n");
}else{
perror("connect");
}
#if 1
do{
handle = le_connect_result(hci);
}while(handle==0);
#endif
printf("handle%x\n", handle);
{
int fl;
fl = fcntl(s, F_GETFL, 0);
fcntl(s, F_SETFL, fl&~O_NONBLOCK);
}
printf("HOGEHOGE\n");
{
preq.code = NG_L2CAP_SMP_PAIRREQ;
preq.iocap = 4;
preq.oobflag = 0;
preq.authreq = 1;
preq.maxkeysize = 16;
preq.ikeydist = 1;
preq.rkeydist = 1;
write(s,&preq, sizeof(preq));
printf("A\n");
do {
int len;
printf("B\n");
len = read(s, &pres, sizeof(pres));
printf("%d, pi.code %d\n",len, pres.code);
}while(pres.code != NG_L2CAP_SMP_PAIRRES);
printf("C\n");
printf("%d %d %d %d %d %d %d(%d)\n", pres.code,pres.iocap ,
pres.oobflag, pres.authreq,
pres.maxkeysize, pres.ikeydist, pres.rkeydist, sizeof(pres));
}
{
socklen_t siz = sizeof(myname);
char bdastr[40];
if(getsockname(s, (struct sockaddr *)&myname,&siz)!=0){
perror("getsockname");
}
printf("%d\n", myname.l2cap_bdaddr_type);
printf("%s\n", bt_ntoa(&myname.l2cap_bdaddr, NULL));
}
{
ng_l2cap_smp_keyinfo mrand,mconfirm,srand,sconfirm;
ng_l2cap_smp_reqres failed;
int res;
uint8_t rval[16];
int ng = 0;
bzero(k, sizeof(k));
arc4random_buf(rval, sizeof(rval));
swap128(rval, mrand.body);
mconfirm.code = NG_L2CAP_SMP_PAIRCONF;
mrand.code = NG_L2CAP_SMP_PAIRRAND;
smp_c1(k, rval, (uint8_t *)&preq, (uint8_t *)&pres,
(myname.l2cap_bdaddr_type == BDADDR_LE_RANDOM)? 1:0,
&myname.l2cap_bdaddr, 0, bd);
swap128(rval, mconfirm.body);
write(s, &mconfirm, sizeof(mconfirm));
res = read(s, &sconfirm, sizeof(sconfirm));
printf("%d\n", res);
if(sconfirm.code != NG_L2CAP_SMP_PAIRCONF){
printf("sconfirm.code %d\n", sconfirm.code);
}
write(s, &mrand, sizeof(mrand));
res = read(s, &srand, sizeof(srand));
printf("%d\n", res);
if(srand.code != NG_L2CAP_SMP_PAIRRAND){
printf("srand.code %d\n", srand.code);
ng = 1;
goto fail;
}
swap128(srand.body, rval);
smp_c1(k, rval, (uint8_t *)&preq, (uint8_t *)&pres,
(myname.l2cap_bdaddr_type == BDADDR_LE_RANDOM)? 1:0,
&myname.l2cap_bdaddr, 0, bd);
for(i =0; i< 16; i++){
printf("%x:%x,", rval[i], sconfirm.body[15-i]);
if(rval[i] != sconfirm.body[15-i]){
ng = 1;
goto fail;
}
}
{
uint8_t mr[16], sr[16],stk[16];
ng_hci_le_start_encryption_cp cp;
ng_hci_status_rp rp;
uint8_t buf[128];
ng_hci_event_pkt_t *ep;
ng_hci_encryption_change_ep *eep;
int n;
swap128(mrand.body, mr);
swap128(srand.body, sr);
smp_s1(k, sr, mr, stk);
swap128(stk, cp.long_term_key);
#if 1
cp.connection_handle = handle;
cp.random_number = 0;
cp.encrypted_diversifier = 0;
n = sizeof(cp);
hci_request(hci, NG_HCI_OPCODE(NG_HCI_OGF_LE
,NG_HCI_OCF_LE_START_ENCRYPTION),
(char *)&cp, sizeof(cp), (char *)&rp, &n);
#endif
printf("LE_ENC OK\n");
{
ng_l2cap_smp_keyinfo ki;
ng_l2cap_smp_masterinfo mi;
ng_hci_le_start_encryption_cp cp;
ng_hci_status_rp rp;
uint8_t pkt[30];
int encok=0, mok=0;
while(encok==0||mok==0){
read(s, pkt, sizeof(pkt));
printf("%d\n", pkt[0]);
switch(pkt[0]){
case NG_L2CAP_SMP_MASTERINFO:
mok=1;
bcopy(pkt, &mi,sizeof(mi));
break;
case NG_L2CAP_SMP_ENCINFO:
encok=1;
bcopy(pkt,&ki, sizeof(ki));
break;
}
printf("%d %d\n", encok, mok);
}
printf("EDIV:%x \nRAND",mi.ediv);
cp.encrypted_diversifier = mi.ediv;
cp.random_number = 0;
for(i = 0; i < 8 ; i++){
printf("%02x ", mi.rand[i]);
cp.random_number
|= (((uint64_t)mi.rand[i])<<(i*8));
}
printf("\nKEY");
for(i = 0 ; i < 16; i++){
printf("%02x ", ki.body[i]);
cp.long_term_key[i] = ki.body[i];
}
printf("\n");
arc4random_buf(ki.body, sizeof(ki.body));
ki.code = NG_L2CAP_SMP_ENCINFO;
write(s, &ki, sizeof(ki));
mi.ediv = arc4random()&0xffff;
arc4random_buf(&mi.rand, sizeof(mi.rand));
mi.code = NG_L2CAP_SMP_MASTERINFO;
write(s, &mi, sizeof(mi));
sleep(4);
cp.connection_handle = handle;
n = sizeof(cp);
hci_request(hci, NG_HCI_OPCODE(NG_HCI_OGF_LE
,NG_HCI_OCF_LE_START_ENCRYPTION),
(char *)&cp, sizeof(cp), (char *)&rp, &n);
sleep(30);
}
}
fail:
if(ng){
failed.code = NG_L2CAP_SMP_PAIRFAIL;
failed.reqres = 4;
write(s, &failed, sizeof(failed));
}
}
return 0;
}
/*
* handle connection request
*/
static void
server_read(int s, short ev, void *arg)
{
struct sockaddr_l2cap ra, la;
channel_t *chan;
socklen_t len;
int fd, n;
uint16_t mru, mtu;
len = sizeof(ra);
fd = accept(s, (struct sockaddr *)&ra, &len);
if (fd == -1)
return;
n = 1;
if (ioctl(fd, FIONBIO, &n) == -1) {
log_err("Could not set NonBlocking IO: %m");
close(fd);
return;
}
len = sizeof(mru);
if (getsockopt(fd, SOL_L2CAP, SO_L2CAP_IMTU, &mru, &len) == -1) {
log_err("Could not get L2CAP IMTU: %m");
close(fd);
return;
}
if(mru < BNEP_MTU_MIN) {
log_err("L2CAP IMTU too small (%d)", mru);
close(fd);
return;
}
len = sizeof(n);
if (getsockopt(fd, SOL_SOCKET, SO_RCVBUF, &n, &len) == -1) {
log_err("Could not read SO_RCVBUF");
close(fd);
return;
}
if (n < (mru * 10)) {
n = mru * 10;
if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &n, sizeof(n)) == -1)
log_info("Could not increase SO_RCVBUF (from %d)", n);
}
len = sizeof(mtu);
if (getsockopt(fd, SOL_L2CAP, SO_L2CAP_OMTU, &mtu, &len) == -1) {
log_err("Could not get L2CAP OMTU: %m");
close(fd);
return;
}
if (mtu < BNEP_MTU_MIN) {
log_err("L2CAP OMTU too small (%d)", mtu);
close(fd);
return;
}
len = sizeof(n);
if (getsockopt(fd, SOL_SOCKET, SO_SNDBUF, &n, &len) == -1) {
log_err("Could not get socket send buffer size: %m");
close(fd);
return;
}
if (n < (mtu * 2)) {
n = mtu * 2;
if (setsockopt(fd, SOL_SOCKET, SO_SNDBUF, &n, sizeof(n)) == -1) {
log_err("Could not set socket send buffer size (%d): %m", n);
close(fd);
return;
}
}
n = mtu;
if (setsockopt(fd, SOL_SOCKET, SO_SNDLOWAT, &n, sizeof(n)) == -1) {
log_err("Could not set socket low water mark (%d): %m", n);
close(fd);
return;
}
len = sizeof(la);
if (getsockname(fd, (struct sockaddr *)&la, &len) == -1) {
log_err("Could not get socket address: %m");
close(fd);
return;
}
log_info("Accepted connection from %s", bt_ntoa(&ra.l2cap_bdaddr, NULL));
chan = channel_alloc();
if (chan == NULL) {
close(fd);
return;
}
chan->send = bnep_send;
chan->recv = bnep_recv;
chan->mru = mru;
chan->mtu = mtu;
b2eaddr(chan->raddr, &ra.l2cap_bdaddr);
b2eaddr(chan->laddr, &la.l2cap_bdaddr);
chan->state = CHANNEL_WAIT_CONNECT_REQ;
channel_timeout(chan, 10);
if (!channel_open(chan, fd)) {
chan->state = CHANNEL_CLOSED;
channel_free(chan);
close(fd);
return;
}
}
ATF_TC_BODY(check_bt_ntoa, tc)
{
bdaddr_t bdaddr = { { 0x00, 0x11, 0x22, 0x33, 0x44, 0x55 } };
ATF_CHECK_STREQ(bt_ntoa(&bdaddr, NULL), "55:44:33:22:11:00");
}
int32_t
kbd_status_changed(bthid_session_p s, uint8_t *data, int32_t len)
{
vkbd_status_t st;
uint8_t leds, report_id;
hid_device_p hid_device;
hid_data_t d;
hid_item_t h;
assert(s != NULL);
assert(len == sizeof(vkbd_status_t));
memcpy(&st, data, sizeof(st));
leds = 0;
report_id = NO_REPORT_ID;
hid_device = get_hid_device(&s->bdaddr);
assert(hid_device != NULL);
for (d = hid_start_parse(hid_device->desc, 1 << hid_output, -1);
hid_get_item(d, &h) > 0; ) {
if (HID_PAGE(h.usage) == HUP_LEDS) {
if (report_id == NO_REPORT_ID)
report_id = h.report_ID;
else if (h.report_ID != report_id)
syslog(LOG_WARNING, "Output HID report IDs " \
"for %s do not match: %d vs. %d. " \
"Please report",
bt_ntoa(&s->bdaddr, NULL),
h.report_ID, report_id);
switch(HID_USAGE(h.usage)) {
case 0x01: /* Num Lock LED */
if (st.leds & LED_NUM)
hid_set_data(&leds, &h, 1);
break;
case 0x02: /* Caps Lock LED */
if (st.leds & LED_CAP)
hid_set_data(&leds, &h, 1);
break;
case 0x03: /* Scroll Lock LED */
if (st.leds & LED_SCR)
hid_set_data(&leds, &h, 1);
break;
/* XXX add other LEDs ? */
}
}
}
hid_end_parse(d);
data[0] = 0xa2; /* DATA output (HID output report) */
if (report_id != NO_REPORT_ID) {
data[1] = report_id;
data[2] = leds;
len = 3;
} else {
data[1] = leds;
len = 2;
}
write(s->intr, data, len);
return (0);
}
int32_t
client_connect(bthid_server_p srv, int32_t fd)
{
bthid_session_p s;
hid_device_p d;
int32_t error;
socklen_t len;
assert(srv != NULL);
assert(fd >= 0);
s = session_by_fd(srv, fd);
assert(s != NULL);
d = get_hid_device(&s->bdaddr);
assert(d != NULL);
error = 0;
len = sizeof(error);
if (getsockopt(fd, SOL_SOCKET, SO_ERROR, &error, &len) < 0) {
syslog(LOG_ERR, "Could not get socket error for %s. %s (%d)",
bt_ntoa(&s->bdaddr, NULL), strerror(errno), errno);
session_close(s);
connect_in_progress = 0;
return (-1);
}
if (error != 0) {
syslog(LOG_ERR, "Could not connect to %s. %s (%d)",
bt_ntoa(&s->bdaddr, NULL), strerror(error), error);
session_close(s);
connect_in_progress = 0;
return (0);
}
switch (s->state) {
case W4CTRL: /* Control channel is open */
assert(s->ctrl == fd);
assert(s->intr == -1);
/* Open interrupt channel */
s->intr = client_socket(&s->bdaddr, d->interrupt_psm);
if (s->intr < 0) {
syslog(LOG_ERR, "Could not open interrupt channel " \
"to %s. %s (%d)", bt_ntoa(&s->bdaddr, NULL),
strerror(errno), errno);
session_close(s);
connect_in_progress = 0;
return (-1);
}
s->state = W4INTR;
FD_SET(s->intr, &srv->wfdset);
if (s->intr > srv->maxfd)
srv->maxfd = s->intr;
d->new_device = 0; /* reset new device flag */
write_hids_file();
break;
case W4INTR: /* Interrupt channel is open */
assert(s->ctrl != -1);
assert(s->intr == fd);
s->state = OPEN;
connect_in_progress = 0;
/* Register session's vkbd descriptor (if any) for read */
if (s->state == OPEN && d->keyboard) {
assert(s->vkbd != -1);
FD_SET(s->vkbd, &srv->rfdset);
if (s->vkbd > srv->maxfd)
srv->maxfd = s->vkbd;
}
break;
default:
assert(0);
break;
}
/* Move fd to from the write fd set into read fd set */
FD_CLR(fd, &srv->wfdset);
FD_SET(fd, &srv->rfdset);
return (0);
}
