glib-helper.c

BlueZ源码

		uint8_t val[16];

		data0 = g_htonl(data0);
		data1 = g_htons(data1);
		data2 = g_htons(data2);
		data3 = g_htons(data3);
		data4 = g_htonl(data4);
		data5 = g_htons(data5);

		memcpy(&val[0], &data0, 4);
		memcpy(&val[4], &data1, 2);
		memcpy(&val[6], &data2, 2);
		memcpy(&val[8], &data3, 2);
		memcpy(&val[10], &data4, 4);
		memcpy(&val[14], &data5, 2);

		sdp_uuid128_create(uuid, val);

		return 0;
	} else {
		uint16_t class = bt_name2class(string);
		if (class) {
			sdp_uuid16_create(uuid, class);
			return 0;
		}
	}

	return -1;
}

gchar *bt_list2string(GSList *list)
{
	GSList *l;
	gchar *str, *tmp;

	if (!list)
		return NULL;

	str = g_strdup((const gchar *) list->data);

	for (l = list->next; l; l = l->next) {
		tmp = g_strconcat(str, " " , (const gchar *) l->data, NULL);
		g_free(str);
		str = tmp;
	}

	return str;
}

GSList *bt_string2list(const gchar *str)
{
	GSList *l = NULL;
	gchar **uuids;
	int i = 0;

	if (!str)
		return NULL;

	/* FIXME: eglib doesn't support g_strsplit */
	uuids = g_strsplit(str, " ", 0);
	if (!uuids)
		return NULL;

	while (uuids[i]) {
		l = g_slist_append(l, uuids[i]);
		i++;
	}

	g_free(uuids);

	return l;
}

static inline int resolve_names(int fd, struct sockaddr *host,
			struct sockaddr *peer, socklen_t len)
{
	int err;
	socklen_t namelen;

	namelen = len;
	memset(host, 0, len);
	err = getsockname(fd, host, &namelen);
	if (err < 0)
		return err;

	namelen = len;
	memset(peer, 0, len);
	err = getpeername(fd, peer, &namelen);
	if (err < 0)
		return err;

	return 0;
}

static int rfcomm_resolver(int fd, char *src, char *dst)
{
	struct sockaddr_rc host, peer;
	socklen_t len;
	int err;

	len = sizeof(host);
	err = resolve_names(fd, (struct sockaddr *) &host,
			(struct sockaddr *) &peer, len);
	if (err < 0)
		return err;

	ba2str(&host.rc_bdaddr, src);
	ba2str(&peer.rc_bdaddr, dst);

	return 0;
}

static int l2cap_resolver(int fd, char *src, char *dst)
{
	struct sockaddr_l2 host, peer;
	socklen_t len;
	int err;

	len = sizeof(host);
	err = resolve_names(fd, (struct sockaddr *) &host,
			(struct sockaddr *) &peer, len);
	if (err < 0)
		return err;

	ba2str(&host.l2_bdaddr, src);
	ba2str(&peer.l2_bdaddr, dst);

	return 0;
}

static int sco_resolver(int fd, char *src, char *dst)
{
	struct sockaddr_sco host, peer;
	socklen_t len;
	int err;

	len = sizeof(host);
	err = resolve_names(fd, (struct sockaddr *) &host,
			(struct sockaddr *) &peer, len);
	if (err < 0)
		return err;

	ba2str(&host.sco_bdaddr, src);
	ba2str(&peer.sco_bdaddr, dst);

	return 0;
}

static gboolean listen_cb(GIOChannel *chan, GIOCondition cond,
		gpointer user_data)
{
	BtIO *io = user_data;
	struct io_context *io_ctxt = io->io_ctxt;
	int fd, err = 0;
	GIOChannel *gio;
	struct sockaddr addr;
	socklen_t len;
	bdaddr_t src, dst;

	if (cond & G_IO_NVAL)
		return FALSE;

	if (cond & (G_IO_HUP | G_IO_ERR)) {
		g_io_channel_close(chan);
		g_io_channel_unref(chan);
		g_free(io_ctxt);
		return FALSE;
	}

	len = sizeof(addr);
	memset(&addr, 0, len);
	fd = accept(io_ctxt->fd, &addr, &len);
	if (fd < 0)
		goto drop;

	if (io_ctxt->resolver) {
		err = io_ctxt->resolver(fd, io->src, io->dst);
		if (err < 0) {
			close(fd);
			goto drop;
		}
	}

	gio = g_io_channel_unix_new(fd);
	if (!gio)
		err = -ENOMEM;

	if (io_ctxt->func)
		io_ctxt->func(io, err, gio, io_ctxt->user_data);
	if (io_ctxt->cb) {
		str2ba(io->src, &src);
		str2ba(io->dst, &dst);
		io_ctxt->cb(gio, err, &src, &dst, io_ctxt->user_data);
	}

	return TRUE;

drop:
	if (io_ctxt->func)
		io_ctxt->func(io, -errno, NULL, io_ctxt->user_data);
	if (io_ctxt->cb)
		io_ctxt->cb(NULL, err, NULL, NULL, io_ctxt->user_data);

	return TRUE;
}

static int transport_listen(BtIO *io)
{
	struct io_context *io_ctxt = io->io_ctxt;
	int err;

	err = listen(io_ctxt->fd, 5);
	if (err < 0)
		return -errno;

	io_ctxt->io = g_io_channel_unix_new(io_ctxt->fd);
	if (!io_ctxt->io)
		return -ENOMEM;

	g_io_add_watch(io_ctxt->io, G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
			(GIOFunc) listen_cb, io);

	return 0;
}

static gboolean connect_cb(GIOChannel *gio, GIOCondition cond,
				gpointer user_data)
{
	BtIO *io = user_data;
	struct io_context *io_ctxt = io->io_ctxt;
	int err = 0, ret;
	socklen_t len;
	bdaddr_t src, dst;

	if (cond & G_IO_NVAL)
		return FALSE;

	len = sizeof(ret);
	if (getsockopt(io_ctxt->fd, SOL_SOCKET, SO_ERROR, &ret, &len) < 0) {
		err = -errno;
		goto done;
	}

	if (ret != 0) {
		err = -ret;
		goto done;
	}

	if (io_ctxt->resolver) {
		err = io_ctxt->resolver(io_ctxt->fd, io->src, io->dst);
		if (err < 0)
			goto done;
	}

	io_ctxt->io = NULL;

done:
	if (io_ctxt->func)
		io_ctxt->func(io, err, gio, io_ctxt->user_data);
	if (io_ctxt->cb) {
		str2ba(io->src, &src);
		str2ba(io->dst, &dst);
		io_ctxt->cb(gio, err, &src, &dst, io_ctxt->user_data);
	}
	if (io_ctxt->io) {
		g_io_channel_close(io_ctxt->io);
		g_io_channel_unref(io_ctxt->io);
	}
	g_free(io_ctxt);

	return FALSE;
}

static int transport_connect(BtIO *io, struct sockaddr *addr,
		socklen_t addrlen)
{
	struct io_context *io_ctxt = io->io_ctxt;
	int err;

	io_ctxt->io = g_io_channel_unix_new(io_ctxt->fd);
	if (!io_ctxt->io)
		return -ENOMEM;

	err = g_io_channel_set_flags(io_ctxt->io, G_IO_FLAG_NONBLOCK, NULL);
	if (err != G_IO_STATUS_NORMAL)
		return -EPERM;

	err = connect(io_ctxt->fd, addr, addrlen);
	if (err < 0 && !(errno == EAGAIN || errno == EINPROGRESS))
		return -errno;

	g_io_add_watch(io_ctxt->io, G_IO_OUT | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
			(GIOFunc) connect_cb, io);

	return 0;
}

static int sco_bind(struct io_context *io_ctxt, const char *address,
			uint16_t mtu, struct sockaddr_sco *addr)
{
	int err;
	struct sco_options sco_opt;

	io_ctxt->fd = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO);
	if (io_ctxt->fd < 0)
		return -errno;

	memset(addr, 0, sizeof(*addr));
	addr->sco_family = AF_BLUETOOTH;
	str2ba(address, &addr->sco_bdaddr);

	err = bind(io_ctxt->fd, (struct sockaddr *) addr, sizeof(*addr));
	if (err < 0) {
		close(io_ctxt->fd);
		return -errno;
	}

	if (mtu) {
		socklen_t olen = sizeof(sco_opt);
		memset(&sco_opt, 0, olen);
		getsockopt(io_ctxt->fd, SOL_SCO, SCO_OPTIONS, &sco_opt, &olen);
		sco_opt.mtu = mtu;
		setsockopt(io_ctxt->fd, SOL_SCO, SCO_OPTIONS, &sco_opt,
				sizeof(sco_opt));
	}

	return 0;
}

static BtIOError sco_connect(BtIO *io, BtIOFunc func)
{
	struct io_context *io_ctxt = io->io_ctxt;
	struct sockaddr_sco addr;
	int err;

	io_ctxt->func = func;

	err = sco_bind(io_ctxt, io->src, 0, &addr);
	if (err < 0)
		return BT_IO_FAILED;

	memset(&addr, 0, sizeof(addr));
	addr.sco_family = AF_BLUETOOTH;
	str2ba(io->dst, &addr.sco_bdaddr);

	err = transport_connect(io, (struct sockaddr *) &addr,
				sizeof(addr));
	if (err < 0) {
		close(io_ctxt->fd);
		return BT_IO_FAILED;
	}

	return BT_IO_SUCCESS;
}

static int l2cap_bind(struct io_context *io_ctxt, const char *address,
			uint16_t psm, uint16_t mtu, uint32_t flags,
			struct sockaddr_l2 *addr)
{
	int err;
	struct l2cap_options l2o;

	io_ctxt->fd = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
	if (io_ctxt->fd < 0)
		return -errno;

	memset(addr, 0, sizeof(*addr));
	addr->l2_family = AF_BLUETOOTH;
	str2ba(address, &addr->l2_bdaddr);
	addr->l2_psm = htobs(psm);

	err = bind(io_ctxt->fd, (struct sockaddr *) addr, sizeof(*addr));
	if (err < 0) {
		close(io_ctxt->fd);
		return -errno;
	}

	if (mtu) {
		socklen_t olen = sizeof(l2o);
		memset(&l2o, 0, olen);
		getsockopt(io_ctxt->fd, SOL_L2CAP, L2CAP_OPTIONS, &l2o, &olen);
		l2o.imtu = l2o.omtu = mtu;
		setsockopt(io_ctxt->fd, SOL_L2CAP, L2CAP_OPTIONS, &l2o, sizeof(l2o));
	}

	if (flags) {
		int opt = flags;
		err = setsockopt(io_ctxt->fd, SOL_L2CAP, L2CAP_LM, &opt,
				sizeof(opt));
		if (err < 0) {
			close(io_ctxt->fd);
			return -errno;
		}
	}

	return 0;
}

static BtIOError l2cap_listen(BtIO *io, BtIOFunc func)
{
	struct io_context *io_ctxt = io->io_ctxt;
	struct sockaddr_l2 addr;
	BtIOError err;

	io_ctxt->func = func;

	err = l2cap_bind(io_ctxt, io->src, io->psm, io->mtu, io->flags, &addr);
	if (err < 0)
		return err;

	err = transport_listen(io);
	if (err < 0) {
		close(io_ctxt->fd);
		return err;
	}

	return BT_IO_SUCCESS;
}

static BtIOError l2cap_connect(BtIO *io, BtIOFunc func)
{
	struct io_context *io_ctxt = io->io_ctxt;
	struct sockaddr_l2 l2a;
	BtIOError err;

	io_ctxt->func = func;

	err = l2cap_bind(io_ctxt, io->src, 0, io->mtu, 0, &l2a);
	if (err < 0)
		return err;

	memset(&l2a, 0, sizeof(l2a));
	l2a.l2_family = AF_BLUETOOTH;
	str2ba(io->dst, &l2a.l2_bdaddr);
	l2a.l2_psm = htobs(io->psm);

	err = transport_connect(io, (struct sockaddr *) &l2a,
				sizeof(l2a));
	if (err < 0) {
		close(io_ctxt->fd);
		return err;
	}

	return BT_IO_SUCCESS;
}

static BtIOError rfcomm_bind(struct io_context *io_ctxt, const char *address,
				uint8_t channel, uint32_t flags,
				struct sockaddr_rc *addr)
{
	BtIOError err;

	io_ctxt->fd = socket(PF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
	if (io_ctxt->fd < 0)
		return BT_IO_FAILED;


	memset(addr, 0, sizeof(*addr));
	addr->rc_family = AF_BLUETOOTH;
	str2ba(address, &addr->rc_bdaddr);
	addr->rc_channel = channel;

	err = bind(io_ctxt->fd, (struct sockaddr *) addr, sizeof(*addr));
	if (err < 0) {
		close(io_ctxt->fd);
		return BT_IO_FAILED;
	}

	if (flags) {
		int opt = flags;
		err = setsockopt(io_ctxt->fd, SOL_RFCOMM, RFCOMM_LM, &opt,
				sizeof(opt));
		if (err < 0) {
			close(io_ctxt->fd);
			return BT_IO_FAILED;
		}
	}

	return BT_IO_SUCCESS;
}

static BtIOError rfcomm_listen(BtIO *io, BtIOFunc func)
{
	struct io_context *io_ctxt = io->io_ctxt;
	struct sockaddr_rc addr;
	socklen_t sa_len;
	BtIOError err;

	io_ctxt->func = func;

	err = rfcomm_bind(io_ctxt, io->src, io->channel, io->flags, &addr);
	if (err < 0)
		return err;

	err = transport_listen(io);
	if (err < 0) {
		close(io_ctxt->fd);
		return err;
	}

	sa_len = sizeof(struct sockaddr_rc);
	memset(&addr, 0, sizeof(addr));
	if (getsockname(io_ctxt->fd, (struct sockaddr *) &addr, &sa_len) < 0) {
		err = -errno;
		close(io_ctxt->fd);
		return err;
	}

	io->channel = addr.rc_channel;

	return BT_IO_SUCCESS;
}

static BtIOError rfcomm_connect(BtIO *io, BtIOFunc func)
{
	struct io_context *io_ctxt = io->io_ctxt;
	struct sockaddr_rc addr;
	BtIOError err;

	io_ctxt->func = func;

	err = rfcomm_bind(io_ctxt, io->src, 0, 0, &addr);
	if (err < 0)
		return err;

	memset(&addr, 0, sizeof(addr));
	addr.rc_family = AF_BLUETOOTH;
	str2ba(io->dst, &addr.rc_bdaddr);
	addr.rc_channel = io->channel;

	err = transport_connect(io, (struct sockaddr *) &addr,
				sizeof(addr));
	if (err < 0) {
		close(io_ctxt->fd);
		return err;
	}

	return BT_IO_SUCCESS;
}

static BtIOError sco_listen(BtIO *io, BtIOFunc func)
{
	struct io_context *io_ctxt = io->io_ctxt;
	struct sockaddr_sco addr;
	BtIOError err;

	io_ctxt->func = func;