int bnep_accept_connection(int sk, uint16_t role, char *dev)
{
struct bnep_setup_conn_req *req;
struct bnep_control_rsp *rsp;
unsigned char pkt[BNEP_MTU];
int r;
r = recv(sk, pkt, BNEP_MTU, 0);
if (r <= 0)
return -1;
errno = EPROTO;
if (r < sizeof(*req))
return -1;
req = (void *) pkt;
if (req->type != BNEP_CONTROL || req->ctrl != BNEP_SETUP_CONN_REQ)
return -1;
/* FIXME: Check role UUIDs */
rsp = (void *) pkt;
rsp->type = BNEP_CONTROL;
rsp->ctrl = BNEP_SETUP_CONN_RSP;
rsp->resp = htons(BNEP_SUCCESS);
if (send(sk, rsp, sizeof(*rsp), 0) < 0)
return -1;
return bnep_connadd(sk, role, dev);
}
int bnep_server_add(int sk, uint16_t dst, char *bridge, char *iface,
const bdaddr_t *addr)
{
if (!bridge || !iface || !addr)
return -EINVAL;
if (bnep_connadd(sk, dst, iface) < 0) {
error("Can't add connection to the bridge %s: %s(%d)",
bridge, strerror(errno), errno);
return -errno;
}
if (bnep_add_to_bridge(iface, bridge) < 0) {
error("Can't add %s to the bridge %s: %s(%d)",
iface, bridge, strerror(errno), errno);
bnep_conndel(addr);
return -errno;
}
if (bnep_if_up(iface) < 0) {
error("Can't up the interface %s: %s(%d)",
iface, strerror(errno), errno);
return -errno;
}
return 0;
}
static int server_connadd(struct network_server *ns,
struct network_session *session,
uint16_t dst_role)
{
char devname[16];
int err, nsk;
memset(devname, 0, sizeof(devname));
strcpy(devname, "bnep%d");
nsk = g_io_channel_unix_get_fd(session->io);
err = bnep_connadd(nsk, dst_role, devname);
if (err < 0)
return err;
info("Added new connection: %s", devname);
if (bnep_add_to_bridge(devname, ns->bridge) < 0) {
error("Can't add %s to the bridge %s: %s(%d)",
devname, ns->bridge, strerror(errno), errno);
return -EPERM;
}
bnep_if_up(devname);
ns->sessions = g_slist_append(ns->sessions, session);
return 0;
}
/* Create BNEP connection
* sk - Connect L2CAP socket
* role - Local role
* service - Remote service
* dev - Network device (contains actual dev name on return)
*/
int bnep_create_connection(int sk, uint16_t role, uint16_t svc, char *dev)
{
struct bnep_setup_conn_req *req;
struct bnep_control_rsp *rsp;
struct __service_16 *s;
unsigned char pkt[BNEP_MTU];
int r;
/* Send request */
req = (void *) pkt;
req->type = BNEP_CONTROL;
req->ctrl = BNEP_SETUP_CONN_REQ;
req->uuid_size = 2; /* 16bit UUID */
s = (void *) req->service;
s->dst = htons(svc);
s->src = htons(role);
if (send(sk, pkt, sizeof(*req) + sizeof(*s), 0) < 0)
return -1;
receive:
/* Get response */
r = recv(sk, pkt, BNEP_MTU, 0);
if (r <= 0)
return -1;
errno = EPROTO;
if (r < sizeof(*rsp))
return -1;
rsp = (void *) pkt;
if (rsp->type != BNEP_CONTROL)
return -1;
if (rsp->ctrl != BNEP_SETUP_CONN_RSP)
goto receive;
r = ntohs(rsp->resp);
switch (r) {
case BNEP_SUCCESS:
break;
case BNEP_CONN_INVALID_DST:
case BNEP_CONN_INVALID_SRC:
case BNEP_CONN_INVALID_SVC:
errno = EPROTO;
return -1;
case BNEP_CONN_NOT_ALLOWED:
errno = EACCES;
return -1;
}
return bnep_connadd(sk, role, dev);
}
static int server_connadd(struct network_server *ns,
struct network_session *session,
uint16_t dst_role)
{
char devname[16];
char address[18];
const char *paddr = address;
const char *pdevname = devname;
int err, nsk;
memset(devname, 0, sizeof(devname));
strcpy(devname, "bnep%d");
nsk = g_io_channel_unix_get_fd(session->io);
err = bnep_connadd(nsk, dst_role, devname);
if (err < 0)
return err;
info("Added new connection: %s", devname);
#ifndef ANDROID_NO_BRIDGE
if (bnep_add_to_bridge(devname, ns->bridge) < 0) {
error("Can't add %s to the bridge %s: %s(%d)",
devname, ns->bridge, strerror(errno), errno);
return -EPERM;
}
#endif
bnep_if_up(devname);
ns->sessions = g_slist_append(ns->sessions, session);
ba2str(&session->dst, address);
gboolean result = g_dbus_emit_signal(connection, adapter_get_path(ns->na->adapter),
ns->iface, "DeviceConnected",
DBUS_TYPE_STRING, &paddr,
DBUS_TYPE_STRING, &pdevname,
DBUS_TYPE_UINT16, &dst_role,
DBUS_TYPE_INVALID);
session->io_watch = g_io_add_watch(session->io, G_IO_ERR | G_IO_HUP,
(GIOFunc) bnep_watchdog_cb, ns);
return 0;
}
static gboolean bnep_setup_cb(GIOChannel *chan, GIOCondition cond,
gpointer data)
{
struct bnep *session = data;
struct bnep_control_rsp *rsp;
struct timeval timeo;
char pkt[BNEP_MTU];
ssize_t r;
int sk;
if (cond & G_IO_NVAL)
return FALSE;
if (session->setup_to > 0) {
g_source_remove(session->setup_to);
session->setup_to = 0;
}
if (cond & (G_IO_HUP | G_IO_ERR)) {
error("Hangup or error on l2cap server socket");
goto failed;
}
sk = g_io_channel_unix_get_fd(chan);
memset(pkt, 0, BNEP_MTU);
r = read(sk, pkt, sizeof(pkt) - 1);
if (r < 0) {
error("IO Channel read error");
goto failed;
}
if (r == 0) {
error("No packet received on l2cap socket");
goto failed;
}
errno = EPROTO;
if ((size_t) r < sizeof(*rsp)) {
error("Packet received is not bnep type");
goto failed;
}
rsp = (void *) pkt;
if (rsp->type != BNEP_CONTROL) {
error("Packet received is not bnep type");
goto failed;
}
if (rsp->ctrl != BNEP_SETUP_CONN_RSP)
return TRUE;
r = ntohs(rsp->resp);
if (r != BNEP_SUCCESS) {
error("bnep failed");
goto failed;
}
memset(&timeo, 0, sizeof(timeo));
timeo.tv_sec = 0;
setsockopt(sk, SOL_SOCKET, SO_RCVTIMEO, &timeo, sizeof(timeo));
sk = g_io_channel_unix_get_fd(session->io);
if (bnep_connadd(sk, session->src, session->iface)) {
error("bnep conn could not be added");
goto failed;
}
if (bnep_if_up(session->iface)) {
error("could not up %s", session->iface);
bnep_conndel(&session->dst_addr);
goto failed;
}
session->watch = g_io_add_watch(session->io,
G_IO_ERR | G_IO_HUP | G_IO_NVAL,
(GIOFunc) bnep_watchdog_cb, session);
g_io_channel_unref(session->io);
session->io = NULL;
session->conn_cb(session->iface, 0, session->conn_data);
return FALSE;
failed:
session->conn_cb(NULL, -EIO, session->conn_data);
return FALSE;
}
static gboolean bnep_setup_cb(GIOChannel *chan, GIOCondition cond,
gpointer data)
{
struct network_conn *nc = data;
struct bnep_control_rsp *rsp;
struct timeval timeo;
char pkt[BNEP_MTU];
ssize_t r;
int sk;
const char *pdev, *uuid;
gboolean connected;
if (cond & G_IO_NVAL)
return FALSE;
g_source_remove(nc->timeout_source);
nc->timeout_source = 0;
if (cond & (G_IO_HUP | G_IO_ERR)) {
error("Hangup or error on l2cap server socket");
goto failed;
}
sk = g_io_channel_unix_get_fd(chan);
memset(pkt, 0, BNEP_MTU);
r = read(sk, pkt, sizeof(pkt) -1);
if (r < 0) {
error("IO Channel read error");
goto failed;
}
if (r == 0) {
error("No packet received on l2cap socket");
goto failed;
}
errno = EPROTO;
if ((size_t) r < sizeof(*rsp)) {
error("Packet received is not bnep type");
goto failed;
}
rsp = (void *) pkt;
if (rsp->type != BNEP_CONTROL) {
error("Packet received is not bnep type");
goto failed;
}
if (rsp->ctrl != BNEP_SETUP_CONN_RSP)
return TRUE;
r = ntohs(rsp->resp);
if (r != BNEP_SUCCESS) {
error("bnep failed");
goto failed;
}
memset(&timeo, 0, sizeof(timeo));
timeo.tv_sec = 0;
setsockopt(sk, SOL_SOCKET, SO_RCVTIMEO, &timeo, sizeof(timeo));
if (bnep_connadd(sk, BNEP_SVC_PANU, nc->dev)) {
error("%s could not be added", nc->dev);
goto failed;
}
bnep_if_up(nc->dev);
pdev = nc->dev;
uuid = bnep_uuid(nc->id);
g_dbus_send_reply(connection, nc->msg,
DBUS_TYPE_STRING, &pdev,
DBUS_TYPE_INVALID);
connected = TRUE;
emit_property_changed(connection, nc->peer->path,
NETWORK_PEER_INTERFACE, "Connected",
DBUS_TYPE_BOOLEAN, &connected);
emit_property_changed(connection, nc->peer->path,
NETWORK_PEER_INTERFACE, "Interface",
DBUS_TYPE_STRING, &pdev);
emit_property_changed(connection, nc->peer->path,
NETWORK_PEER_INTERFACE, "UUID",
DBUS_TYPE_STRING, &uuid);
nc->state = CONNECTED;
nc->dc_id = device_add_disconnect_watch(nc->peer->device, disconnect_cb,
nc, NULL);
info("%s connected", nc->dev);
/* Start watchdog */
g_io_add_watch(chan, G_IO_ERR | G_IO_HUP | G_IO_NVAL,
(GIOFunc) bnep_watchdog_cb, nc);
g_io_channel_unref(nc->io);
nc->io = NULL;
return FALSE;
failed:
cancel_connection(nc, "bnep setup failed");
return FALSE;
}