avdtp.c

Linux的蓝牙操作工具。配合bluez-lib使用

	seid_count = 1 + size - sizeof(struct start_req);

	seid = &req->first_seid;

	for (i = 0; i < seid_count; i++, seid++) {
		failed_seid = seid->seid;

		sep = find_local_sep_by_seid(req->first_seid.seid);
		if (!sep || !sep->stream) {
			err = AVDTP_BAD_ACP_SEID;
			goto failed;
		}

		stream = sep->stream;

		if (sep->state != AVDTP_STATE_OPEN) {
			err = AVDTP_BAD_STATE;
			goto failed;
		}

		if (sep->ind && sep->ind->start) {
			if (!sep->ind->start(session, sep, stream, &err,
						sep->user_data))
				goto failed;
		}

		avdtp_sep_set_state(session, sep, AVDTP_STATE_STREAMING);
	}

	init_response(&rsp->header, &req->header, TRUE);

	return avdtp_send(session, rsp, sizeof(struct gen_resp));

failed:
	memset(&rej, 0, sizeof(rej));
	init_response(&rej.header, &req->header, FALSE);
	rej.acp_seid = failed_seid;
	rej.error = err;
	return avdtp_send(session, &rej, sizeof(rej));
}

static gboolean avdtp_close_cmd(struct avdtp *session, struct seid_req *req,
				int size)
{
	struct avdtp_local_sep *sep;
	struct avdtp_stream *stream;
	struct gen_resp *rsp = (struct gen_resp *) session->buf;
	struct seid_rej rej;
	uint8_t err;

	if (size < sizeof(struct seid_req)) {
		error("Too short close request");
		return FALSE;
	}

	sep = find_local_sep_by_seid(req->acp_seid);
	if (!sep || !sep->stream) {
		err = AVDTP_BAD_ACP_SEID;
		goto failed;
	}

	if (sep->state != AVDTP_STATE_OPEN &&
			sep->state != AVDTP_STATE_STREAMING) {
		err = AVDTP_BAD_STATE;
		goto failed;
	}

	stream = sep->stream;

	if (sep->ind && sep->ind->close) {
		if (!sep->ind->close(session, sep, stream, &err,
					sep->user_data))
			goto failed;
	}

	avdtp_sep_set_state(session, sep, AVDTP_STATE_CLOSING);

	init_response(&rsp->header, &req->header, TRUE);

	if (!avdtp_send(session, rsp, sizeof(struct gen_resp)))
		return FALSE;

	stream->timer = g_timeout_add(REQ_TIMEOUT, stream_close_timeout,
					stream);

	return TRUE;

failed:
	init_response(&rej.header, &req->header, FALSE);
	rej.error = err;
	return avdtp_send(session, &rej, sizeof(rej));
}

static gboolean avdtp_suspend_cmd(struct avdtp *session,
					struct suspend_req *req, int size)
{
	struct avdtp_local_sep *sep;
	struct avdtp_stream *stream;
	struct gen_resp *rsp = (struct gen_resp *) session->buf;
	struct stream_rej rej;
	struct seid *seid;
	uint8_t err, failed_seid;
	int seid_count, i;

	if (size < sizeof(struct suspend_req)) {
		error("Too short suspend request");
		return FALSE;
	}
	
	seid_count = 1 + size - sizeof(struct suspend_req);

	seid = &req->first_seid;

	for (i = 0; i < seid_count; i++, seid++) {
		failed_seid = seid->seid;

		sep = find_local_sep_by_seid(req->first_seid.seid);
		if (!sep || !sep->stream) {
			err = AVDTP_BAD_ACP_SEID;
			goto failed;
		}

		stream = sep->stream;

		if (sep->state != AVDTP_STATE_STREAMING) {
			err = AVDTP_BAD_STATE;
			goto failed;
		}

		if (sep->ind && sep->ind->suspend) {
			if (!sep->ind->suspend(session, sep, stream, &err,
						sep->user_data))
				goto failed;
		}

		avdtp_sep_set_state(session, sep, AVDTP_STATE_OPEN);
	}

	init_response(&rsp->header, &req->header, TRUE);

	return avdtp_send(session, rsp, sizeof(struct gen_resp));

failed:
	memset(&rej, 0, sizeof(rej));
	init_response(&rej.header, &req->header, FALSE);
	rej.acp_seid = failed_seid;
	rej.error = err;
	return avdtp_send(session, &rej, sizeof(rej));
}

static gboolean avdtp_abort_cmd(struct avdtp *session, struct seid_req *req,
				int size)
{
	struct avdtp_local_sep *sep;
	struct gen_resp *rsp = (struct gen_resp *) session->buf;
	struct seid_rej rej;
	uint8_t err;
	gboolean ret;

	if (size < sizeof(struct seid_req)) {
		error("Too short abort request");
		return FALSE;
	}

	sep = find_local_sep_by_seid(req->acp_seid);
	if (!sep || !sep->stream) {
		err = AVDTP_BAD_ACP_SEID;
		goto failed;
	}

	if (sep->ind && sep->ind->abort) {
		if (!sep->ind->abort(session, sep, sep->stream, &err,
					sep->user_data))
			goto failed;
	}

	init_response(&rsp->header, &req->header, TRUE);
	ret = avdtp_send(session, rsp, sizeof(struct gen_resp));

	if (ret)
		avdtp_sep_set_state(session, sep, AVDTP_STATE_ABORTING);

	return ret;

failed:
	init_response(&rej.header, &req->header, FALSE);
	rej.error = err;
	return avdtp_send(session, &rej, sizeof(rej));
}

static gboolean avdtp_secctl_cmd(struct avdtp *session, struct seid_req *req,
					int size)
{
	return avdtp_unknown_cmd(session, (void *) req, size);
}

static gboolean avdtp_parse_cmd(struct avdtp *session,
				struct avdtp_header *header, int size)
{
	switch (header->signal_id) {
	case AVDTP_DISCOVER:
		debug("Received DISCOVER_CMD");
		return avdtp_discover_cmd(session, (void *) header, size);
	case AVDTP_GET_CAPABILITIES:
		debug("Received  GET_CAPABILITIES_CMD");
		return avdtp_getcap_cmd(session, (void *) header, size);
	case AVDTP_SET_CONFIGURATION:
		debug("Received SET_CONFIGURATION_CMD");
		return avdtp_setconf_cmd(session, (void *) header, size);
	case AVDTP_GET_CONFIGURATION:
		debug("Received GET_CONFIGURATION_CMD");
		return avdtp_getconf_cmd(session, (void *) header, size);
	case AVDTP_RECONFIGURE:
		debug("Received RECONFIGURE_CMD");
		return avdtp_reconf_cmd(session, (void *) header, size);
	case AVDTP_OPEN:
		debug("Received OPEN_CMD");
		return avdtp_open_cmd(session, (void *) header, size);
	case AVDTP_START:
		debug("Received START_CMD");
		return avdtp_start_cmd(session, (void *) header, size);
	case AVDTP_CLOSE:
		debug("Received CLOSE_CMD");
		return avdtp_close_cmd(session, (void *) header, size);
	case AVDTP_SUSPEND:
		debug("Received SUSPEND_CMD");
		return avdtp_suspend_cmd(session, (void *) header, size);
	case AVDTP_ABORT:
		debug("Received ABORT_CMD");
		return avdtp_abort_cmd(session, (void *) header, size);
	case AVDTP_SECURITY_CONTROL:
		debug("Received SECURITY_CONTROL_CMD");
		return avdtp_secctl_cmd(session, (void *) header, size);
	default:
		debug("Received unknown request id %u", header->signal_id);
		return avdtp_unknown_cmd(session, (void *) header, size);
	}
}

static void init_request(struct avdtp_header *header, int request_id)
{
	static int transaction = 0;

	header->packet_type = AVDTP_PKT_TYPE_SINGLE;
	header->message_type = AVDTP_MSG_TYPE_COMMAND;
	header->transaction = transaction;
	header->signal_id = request_id;

	/* clear rfa bits */
	header->rfa0 = 0;

	transaction = (transaction + 1) % 16;
}

static gboolean session_cb(GIOChannel *chan, GIOCondition cond,
				gpointer data)
{
	struct avdtp *session = data;
	struct avdtp_header *header;
	gsize size;

	debug("session_cb");

	if (cond & G_IO_NVAL)
		return FALSE;

	if (cond & (G_IO_HUP | G_IO_ERR))
		goto failed;

	if (g_io_channel_read(chan, session->buf, session->mtu,
				&size) != G_IO_ERROR_NONE) {
		error("IO Channel read error");
		goto failed;
	}

	if (size < sizeof(struct avdtp_header)) {
		error("Received too small packet (%d bytes)", size);
		goto failed;
	}

	header = (struct avdtp_header *) session->buf;

	if (header->message_type == AVDTP_MSG_TYPE_COMMAND) {
		if (!avdtp_parse_cmd(session, header, size)) {
			error("Unable to handle command. Disconnecting");
			goto failed;
		}

		if (session->ref == 1 && !session->streams)
			set_disconnect_timer(session);

		if (session->streams && session->dc_timer)
			remove_disconnect_timer(session);

		return TRUE;
	}

	if (session->req == NULL) {
		error("No pending request, rejecting message");
		return TRUE;
	}

	if (header->transaction != session->req->msg->transaction) {
		error("Transaction label doesn't match");
		return TRUE;
	}

	if (header->signal_id != session->req->msg->signal_id) {
		error("Reponse signal doesn't match");
		return TRUE;
	}

	g_source_remove(session->req->timeout);
	session->req->timeout = 0;

	switch(header->message_type) {
	case AVDTP_MSG_TYPE_ACCEPT:
		if (!avdtp_parse_resp(session, session->req->stream,
							header, size)) {
			error("Unable to parse accept response");
			goto failed;
		}
		break;
	case AVDTP_MSG_TYPE_REJECT:
		if (!avdtp_parse_rej(session, session->req->stream,
							header, size)) {
			error("Unable to parse reject response");
			goto failed;
		}
		break;
	default:
		error("Unknown message type");
		break;
	}

	pending_req_free(session->req);
	session->req = NULL;

	process_queue(session);

	return TRUE;

failed:
	if (session->pending_auth) {
		manager_cancel_authorize(&session->dst, ADVANCED_AUDIO_UUID,
						session->pending_auth);
		dbus_pending_call_unref(session->pending_auth);
		session->pending_auth = NULL;
	}

	connection_lost(session, -EIO);

	return FALSE;
}

static gboolean l2cap_connect_cb(GIOChannel *chan, GIOCondition cond,
					gpointer data)
{
	struct avdtp *session = data;
	struct l2cap_options l2o;
	socklen_t len;
	int ret, err, sk;
	char address[18];

	if (cond & G_IO_NVAL)
		return FALSE;

	if (!g_slist_find(sessions, session)) {
		debug("l2cap_connect_cb: session got removed");
		return FALSE;
	}

	sk = g_io_channel_unix_get_fd(chan);

	len = sizeof(ret);
	if (getsockopt(sk, SOL_SOCKET, SO_ERROR, &ret, &len) < 0) {
		err = errno;
		error("getsockopt(SO_ERROR): %s (%d)", strerror(err), err);
		goto failed;
	}

	if (ret != 0) {
		err = ret;
		error("connect(): %s (%d)", strerror(err), err);
		goto failed;
	}

	if (cond & G_IO_HUP) {
		err = EIO;
		goto failed;
	}

	ba2str(&session->dst, address);
	debug("AVDTP: connected %s channel to %s",
			session->pending_open ? "transport" : "signaling",
			address);

	memset(&l2o, 0, sizeof(l2o));
	len = sizeof(l2o);
	if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &l2o,
				&len) < 0) {
		err = errno;
		error("getsockopt(L2CAP_OPTIONS): %s (%d)", strerror(err),
				err);
		goto failed;
	}

	if (session->state == AVDTP_SESSION_STATE_CONNECTING) {
		struct device *dev;

		session->mtu = l2o.imtu;
		session->buf = g_malloc0(session->mtu);
		session->state = AVDTP_SESSION_STATE_CONNECTED;
		session->io = g_io_add_watch(chan,
						G_IO_IN | G_IO_ERR | G_IO_HUP
						| G_IO_NVAL,
						(GIOFunc) session_cb, session);

		dev = manager_find_device(&session->dst,
						AUDIO_CONTROL_INTERFACE, FALSE);
		if (dev)
			avrcp_connect(dev);
	}
	else if (session->pending_open)
		handle_transport_connect(session, sk, l2o.imtu, l2o.omtu);
	else {
		err = -EIO;
		goto failed;
	}

	process_queue(session);

	return FALSE;

failed:
	close(sk);

	if (session->pending_open) {
		avdtp_sep_set_state(session, session->pending_open->lsep,
					AVDTP_STATE_IDLE);
		session->pending_open = NULL;
	} else
		connection_lost(session, -err);

	return FALSE;
}

static int l2cap_connect(struct avdtp *session)
{
	struct sockaddr_l2 l2a;
	GIOChannel *io;
	int sk;

	memset(&l2a, 0, sizeof(l2a));
	l2a.l2_family = AF_BLUETOOTH;
	bacpy(&l2a.l2_bdaddr, &session->src);

	sk = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
	if (sk < 0) {
		error("Cannot create L2CAP socket. %s(%d)", strerror(errno),
				errno);
		return -errno;
	}

	if (bind(sk, (struct sockaddr *) &l2a, sizeof(l2a)) < 0) {
		error("Bind failed. %s (%d)", strerror(errno), errno);
		return -errno;
	}

	memset(&l2a, 0, sizeof(l2a));
	l2a.l2_family = AF_BLUETOOTH;
	bacpy(&l2a.l2_bdaddr, &session->dst);
	l2a.l2_psm = htobs(AVDTP_PSM);

	if (set_nonblocking(sk) < 0) {
		error("Set non blocking: %s (%d)", strerror(errno), errno);
		return -errno;
	}

	io = g_io_channel_unix_new(sk);
	g_io_channel_set_close_on_unref(io, FALSE);

	if (connect(sk, (struct sockaddr *) &l2a, sizeof(l2a)) < 0) {
		if (!(errno == EAGAIN || errno == EINPROGRESS)) {
			error("Connect failed. %s(%d)", strerror(errno),
					errno);
			g_io_channel_close(io);
			g_io_channel_unref(io);
			return -errno;
		}

		g_io_add_watch(io, G_IO_OUT | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
				(GIOFunc) l2cap_connect_cb, session);

		if (session->state == AVDTP_SESSION_STATE_DISCONNECTED) {
			session->sock = sk;
			session->state = AVDTP_SESSION_STATE_CONNECTING;
		}


	} else
		l2cap_connect_cb(io, G_IO_OUT, session);

	g_io_channel_unref(io);

	return 0;
}

static void queue_request(struct avdtp *session, struct pending_req *req,
			gboolean priority)
{
	if (priority)
		session->prio_queue = g_slist_append(session->prio_queue, req);
	else
		session->req_queue = g_slist_append(session->req_queue, req);
}

static gboolean request_timeout(gpointer user_data)
{
	struct avdtp *session = user_data;
	struct pending_req *req;
	struct seid_req sreq;
	struct avdtp_local_sep *lsep;
	struct avdtp_stream *stream;
	uint8_t seid;
	struct avdtp_error err;

	req = session->req;
	session->req = NULL;

	avdtp_error_init(&err, AVDTP_ERROR_ERRNO, ETIMEDOUT);

	seid = ((struct seid_req *) (req->msg))->acp_seid;

	stream = find_stream_by_rseid(session, seid);

	if (stream)
		lsep = stream->lsep;
	else
		lsep = NULL;

	switch (req->msg->signal_id) {
	case AVDTP_RECONFIGURE:
		error("Reconfigure request timed out");
		if (lsep && lsep->cfm && lsep->cfm->reconfigure)
			lsep->cfm->reconfigure(session, lsep, stream, &err,
						lsep->user_data);
		break;
	case AVDTP_OPEN:
		error("Open request timed out");
		if (lsep && lsep->cfm && lsep->cfm->open)
			lsep->cfm->open(session, lsep, stream, &err,
					lsep->user_data);
		break;
	case AVDTP_START:
		error("Start request timed out");
		if (lsep && lsep->cfm && lsep->cfm->start)
			lsep->cfm->start(session, lsep, stream, &err,
						lsep->user_data);
		break;
	case AVDTP_SUSPEND:
		error("Suspend request timed out");
		if (lsep && lsep->cfm && lsep->cfm->suspend)
			lsep->cfm->suspend(session, lsep, stream, &err,
						lsep->user_data);
		break;
	case AVDTP_CLOSE:
		error("Close request timed out");
		if (lsep && lsep->cfm && lsep->cfm->close)
			lsep->cfm->close(session, lsep, stream, &err,
						lsep->user_data);
		break;
	case AVDTP_SET_CONFIGURATION:
		error("SetConfiguration request timed out");
		if (lsep && lsep->cfm && lsep->cfm->set_configuration)