server.c

基于LINUX内核驱动的开发

	case BNEP_SVC_PANU:
		if (src_role == BNEP_SVC_PANU ||
			src_role == BNEP_SVC_GN ||
			src_role == BNEP_SVC_NAP)
			return 0;

		return BNEP_CONN_INVALID_SRC;
	}

	return BNEP_CONN_INVALID_DST;
}

static gboolean connect_setup_event(GIOChannel *chan,
					GIOCondition cond, gpointer data)
{
	struct setup_session *s = data;
	struct network_server *ns = NULL;
	struct bnep_setup_conn_req *req;
	unsigned char pkt[BNEP_MTU];
	char path[MAX_PATH_LENGTH];
	uint16_t response;
	uint8_t *pservice;
	ssize_t r;
	int sk;

	if (cond & G_IO_NVAL)
		return FALSE;

	if (cond & (G_IO_ERR | G_IO_HUP)) {
		error("Hangup or error on BNEP socket");
		/* If there is a pending authorization */
		if (s->attempts)
			cancel_authorization(s);
		return FALSE;
	}

	sk = g_io_channel_unix_get_fd(chan);
	memset(pkt, 0, sizeof(pkt));
	r = recv(sk, pkt, sizeof(pkt) - 1, 0);

	req = (struct bnep_setup_conn_req *) pkt;
	/*
	 * FIXME: According to BNEP SPEC the UUID size can be
	 * 2-16 bytes. Currently only 2 bytes size is supported
	 */
	if (req->uuid_size != 2 || r != (sizeof(*req) + req->uuid_size * 2)) {
		error("Invalid BNEP packet size");
		response = BNEP_CONN_INVALID_SVC;
		goto reply;
	}

	if (req->type != BNEP_CONTROL || req->ctrl != BNEP_SETUP_CONN_REQ) {
		error("Invalid BNEP control packet content");
		return FALSE;
	}

	pservice = req->service;
	/* Getting destination service: considering 2 bytes size */
	s->dst_role = ntohs(bt_get_unaligned((uint16_t *) pservice));
	pservice += req->uuid_size;
	/* Getting source service: considering 2 bytes size */
	s->src_role = ntohs(bt_get_unaligned((uint16_t *) pservice));

	response = chk_role(s->src_role, s->dst_role);
	if (response)
		goto reply;

	snprintf(path, MAX_PATH_LENGTH, NETWORK_PATH"/%s", bnep_name(s->dst_role));
	dbus_connection_get_object_user_data(connection, path, (void *) &ns);

	if (ns == NULL || ns->enable == FALSE) {
		response = BNEP_CONN_NOT_ALLOWED;
		goto reply;
	}

	if (s->timeout) {
		g_source_remove(s->timeout);
		s->timeout = 0;
	}

	if (++s->attempts > MAX_SETUP_ATTEMPTS) {
		/* Retransmission */
		response = BNEP_CONN_NOT_ALLOWED;
		goto reply;
	}

	/* Wait authorization before reply success */
	if (authorize_connection(s) < 0) {
		response = BNEP_CONN_NOT_ALLOWED;
		goto reply;
	}

	return TRUE;
reply:
	send_bnep_ctrl_rsp(sk, response);
	return FALSE;
}

static gboolean setup_timeout(void *data)
{
	setup_watch_destroy(data);
	return FALSE;
}

static gboolean connect_event(GIOChannel *chan,
				GIOCondition cond, gpointer data)
{
	struct sockaddr_l2 addr;
	struct setup_session *s;
	GIOChannel *io;
	socklen_t addrlen;
	char peer[18];
	bdaddr_t dst;
	unsigned short psm;
	int sk, nsk;

	if (cond & G_IO_NVAL)
		return FALSE;

	if (cond & (G_IO_ERR | G_IO_HUP)) {
		error("Hangup or error on L2CAP socket PSM 15");
		g_io_channel_close(chan);
		return FALSE;
	}

	sk = g_io_channel_unix_get_fd(chan);

	memset(&addr, 0, sizeof(addr));
	addrlen = sizeof(addr);

	nsk = accept(sk, (struct sockaddr *) &addr, &addrlen);
	if (nsk < 0)
		return TRUE;

	bacpy(&dst, &addr.l2_bdaddr);
	psm = btohs(addr.l2_psm);
	ba2str(&dst, peer);

	info("Connection from: %s on PSM %d", peer, psm);

	if (g_slist_find_custom(setup_sessions, peer,
				(GCompareFunc) setup_cmp)) {
		error("Pending connection setup session");
		close(nsk);
		return TRUE;
	}

	s = g_new0(struct setup_session, 1);
	s->address = g_strdup(peer);
	s->nsk = nsk;

	io = g_io_channel_unix_new(nsk);
	g_io_channel_set_close_on_unref(io, TRUE);
	/* New watch for BNEP setup */
	s->watch = g_io_add_watch_full(io, G_PRIORITY_DEFAULT,
			G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
			connect_setup_event, s, &setup_watch_destroy);
	g_io_channel_unref(io);

	/* Remove the timeout at the first valid msg */
	s->timeout = g_timeout_add(SETUP_TIMEOUT, setup_timeout, s);

	setup_sessions = g_slist_append(setup_sessions, s);

	return TRUE;
}

int server_init(DBusConnection *conn, const char *iface_prefix,
		gboolean secure)
{
	struct l2cap_options l2o;
	struct sockaddr_l2 l2a;
	socklen_t olen;
	int sk, lm, err;

	/* Create L2CAP socket and bind it to PSM BNEP */
	sk = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
	if (sk < 0) {
		err = errno;
		error("Cannot create L2CAP socket. %s(%d)",
					strerror(err), err);
		return -err;
	}

	memset(&l2a, 0, sizeof(l2a));
	l2a.l2_family = AF_BLUETOOTH;
	bacpy(&l2a.l2_bdaddr, BDADDR_ANY);
	l2a.l2_psm = htobs(BNEP_PSM);

	if (bind(sk, (struct sockaddr *) &l2a, sizeof(l2a))) {
		err = errno;
		error("Bind failed. %s(%d)", strerror(err), err);
		goto fail;
	}

	/* Setup L2CAP options according to BNEP spec */
	memset(&l2o, 0, sizeof(l2o));
	olen = sizeof(l2o);
	if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &l2o, &olen) < 0) {
		err = errno;
		error("Failed to get L2CAP options. %s(%d)",
					strerror(err), err);
		goto fail;
	}

	l2o.imtu = l2o.omtu = BNEP_MTU;
	if (setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &l2o, sizeof(l2o)) < 0) {
		err = errno;
		error("Failed to set L2CAP options. %s(%d)",
					strerror(err), err);
		goto fail;
	}

	lm = secure ? L2CAP_LM_SECURE : 0;
	if (lm && setsockopt(sk, SOL_L2CAP, L2CAP_LM, &lm, sizeof(lm)) < 0) {
		err = errno;
		error("Failed to set link mode. %s(%d)",
					strerror(err), err);
		goto fail;
	}
	security = secure;

	if (listen(sk, 1) < 0) {
		err = errno;
		error("Listen failed. %s(%d)", strerror(err), err);
		goto fail;
	}

	connection = dbus_connection_ref(conn);
	prefix = iface_prefix;

	bnep_io = g_io_channel_unix_new(sk);
	g_io_channel_set_close_on_unref(bnep_io, FALSE);
	g_io_add_watch(bnep_io, G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
							connect_event, NULL);

	if (bridge_create(BNEP_SVC_GN) < 0)
		error("Can't create GN bridge");

	return 0;
fail:

	close(sk);
	errno = err;
	return -err;
}

void server_exit()
{
	if (setup_sessions) {
		g_slist_foreach(setup_sessions, (GFunc) setup_free, NULL);
		g_slist_free(setup_sessions);
		setup_sessions = NULL;
	}

	if (bnep_io != NULL) {
		g_io_channel_close(bnep_io);
		g_io_channel_unref(bnep_io);
		bnep_io = NULL;
	}

	if (bridge_remove(BNEP_SVC_GN) < 0)
		error("Can't remove GN bridge");

	dbus_connection_unref(connection);
	connection = NULL;
}

static uint32_t add_server_record(struct network_server *ns)
{
	DBusMessage *msg, *reply;
	DBusError derr;
	dbus_uint32_t rec_id;
	sdp_buf_t buf;

	msg = dbus_message_new_method_call("org.bluez", "/org/bluez",
				"org.bluez.Database", "AddServiceRecord");
	if (!msg) {
		error("Can't allocate new method call");
		return 0;
	}

	if (create_server_record(&buf, ns->name, ns->id) < 0) {
		error("Unable to allocate new service record");
		dbus_message_unref(msg);
		return 0;
	}

	dbus_message_append_args(msg, DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE,
				&buf.data, buf.data_size, DBUS_TYPE_INVALID);

	dbus_error_init(&derr);
	reply = dbus_connection_send_with_reply_and_block(connection,
			msg, -1, &derr);

	free(buf.data);
	dbus_message_unref(msg);

	if (dbus_error_is_set(&derr) || dbus_set_error_from_message(&derr, reply)) {
		error("Adding service record failed: %s", derr.message);
		dbus_error_free(&derr);
		return 0;
	}

	dbus_message_get_args(reply, &derr, DBUS_TYPE_UINT32, &rec_id,
				DBUS_TYPE_INVALID);

	if (dbus_error_is_set(&derr)) {
		error("Invalid arguments to AddServiceRecord reply: %s", derr.message);
		dbus_message_unref(reply);
		dbus_error_free(&derr);
		return 0;
	}

	dbus_message_unref(reply);

	debug("add_server_record: got record id 0x%x", rec_id);

	return rec_id;
}

static int update_server_record(struct network_server *ns)
{
	DBusMessage *msg, *reply;
	DBusError derr;
	sdp_buf_t buf;

	msg = dbus_message_new_method_call("org.bluez", "/org/bluez",
				"org.bluez.Database", "UpdateServiceRecord");
	if (!msg) {
		error("Can't allocate new method call");
		return -ENOMEM;
	}

	if (create_server_record(&buf, ns->name, ns->id) < 0) {
		error("Unable to allocate new service record");
		dbus_message_unref(msg);
		return -1;
	}

	dbus_message_append_args(msg,
			DBUS_TYPE_UINT32, &ns->record_id,
			DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE,
			&buf.data, buf.data_size, DBUS_TYPE_INVALID);

	dbus_error_init(&derr);
	reply = dbus_connection_send_with_reply_and_block(connection,
			msg, -1, &derr);

	free(buf.data);
	dbus_message_unref(msg);

	if (dbus_error_is_set(&derr) || dbus_set_error_from_message(&derr, reply)) {
		error("Update service record failed: %s", derr.message);
		dbus_error_free(&derr);
		return -1;
	}

	dbus_message_unref(reply);

	return 0;
}

static int remove_server_record(uint32_t rec_id)
{
	DBusMessage *msg, *reply;
	DBusError derr;

	msg = dbus_message_new_method_call("org.bluez", "/org/bluez",
				"org.bluez.Database", "RemoveServiceRecord");
	if (!msg) {
		error("Can't allocate new method call");
		return -ENOMEM;
	}

	dbus_message_append_args(msg,
			DBUS_TYPE_UINT32, &rec_id,
			DBUS_TYPE_INVALID);

	dbus_error_init(&derr);
	reply = dbus_connection_send_with_reply_and_block(connection,
			msg, -1, &derr);

	dbus_message_unref(msg);

	if (dbus_error_is_set(&derr)) {
		error("Removing service record 0x%x failed: %s",
						rec_id, derr.message);
		dbus_error_free(&derr);
		return -1;
	}

	dbus_message_unref(reply);

	return 0;
}

static DBusHandlerResult get_uuid(DBusConnection *conn,
					DBusMessage *msg, void *data)
{
	struct network_server *ns = data;
	DBusMessage *reply;
	const char *uuid;

	reply = dbus_message_new_method_return(msg);
	if (!reply)
		return DBUS_HANDLER_RESULT_NEED_MEMORY;

	uuid = bnep_uuid(ns->id);
	dbus_message_append_args(reply,
			DBUS_TYPE_STRING, &uuid,
			DBUS_TYPE_INVALID);

	return send_message_and_unref(conn, reply);
}

static DBusHandlerResult enable(DBusConnection *conn,
				DBusMessage *msg, void *data)
{
	struct network_server *ns = data;
	DBusMessage *reply;

	if (ns->enable)
		return error_already_exists(conn, msg, "Server already enabled");

	if (bacmp(&ns->src, BDADDR_ANY) == 0) {
		int dev_id;

		dev_id = hci_get_route(&ns->src);