avdtp.c

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

			lsep->cfm->set_configuration(session, lsep, stream,
							&err, lsep->user_data);
		goto failed;
	case AVDTP_DISCOVER:
		error("Discover request timed out");
		goto failed;
	case AVDTP_GET_CAPABILITIES:
		error("GetCapabilities request timed out");
		goto failed;
	case AVDTP_ABORT:
		error("Abort request timed out");
		goto failed;
	}

	memset(&sreq, 0, sizeof(sreq));
	init_request(&sreq.header, AVDTP_ABORT);
	sreq.acp_seid = seid;

	if (send_request(session, TRUE, stream, &sreq, sizeof(sreq)) < 0) {
		error("Unable to send abort request");
		goto failed;
	}

	goto done;

failed:
	connection_lost(session, -ETIMEDOUT);
done:
	pending_req_free(req);
	return FALSE;
}

static int send_req(struct avdtp *session, gboolean priority,
			struct pending_req *req)
{
	int err;

	if (session->state == AVDTP_SESSION_STATE_DISCONNECTED) {
		err = l2cap_connect(session);
		if (err < 0)
			goto failed;
	}

	if (session->state < AVDTP_SESSION_STATE_CONNECTED ||
			session->req != NULL) {
		queue_request(session, req, priority);
		return 0;
	}

	/* FIXME: Should we retry to send if the buffer
	was not totally sent or in case of EINTR? */
	if (!avdtp_send(session, req->msg, req->msg_size)) {
		err = -EIO;
		goto failed;
	}

	session->req = req;

	req->timeout = g_timeout_add(REQ_TIMEOUT, request_timeout,
					session);
	return 0;

failed:
	g_free(req->msg);
	g_free(req);
	return err;
}

static int send_request(struct avdtp *session, gboolean priority,
			struct avdtp_stream *stream, void *buffer, int size)
{
	struct pending_req *req;

	req = g_new0(struct pending_req, 1);
	req->msg = g_malloc(size);
	memcpy(req->msg, buffer, size);
	req->msg_size = size;
	req->stream = stream;

	return send_req(session, priority, req);
}

static gboolean avdtp_discover_resp(struct avdtp *session,
					struct discover_resp *resp, int size)
{
	int sep_count, i, isize = sizeof(struct seid_info);

	sep_count = (size - sizeof(struct avdtp_header)) / isize;

	for (i = 0; i < sep_count; i++) {
		struct avdtp_remote_sep *sep;
		struct avdtp_stream *stream;
		struct seid_req req;
		int ret;

		debug("seid %d type %d media %d in use %d",
				resp->seps[i].seid, resp->seps[i].type,
				resp->seps[i].media_type, resp->seps[i].inuse);

		stream = find_stream_by_rseid(session, resp->seps[i].seid);

		sep = find_remote_sep(session->seps, resp->seps[i].seid);
		if (!sep) {
			if (resp->seps[i].inuse && !stream)
				continue;
			sep = g_new0(struct avdtp_remote_sep, 1);
			session->seps = g_slist_append(session->seps, sep);
		}

		sep->stream = stream;
		sep->seid = resp->seps[i].seid;
		sep->type = resp->seps[i].type;
		sep->media_type = resp->seps[i].media_type;

		memset(&req, 0, sizeof(req));
		init_request(&req.header, AVDTP_GET_CAPABILITIES);
		req.acp_seid = sep->seid;

		ret = send_request(session, TRUE, NULL, &req, sizeof(req));
		if (ret < 0) {
			finalize_discovery(session, ret);
			break;
		}
	}

	return TRUE;
}

static gboolean avdtp_get_capabilities_resp(struct avdtp *session,
						struct getcap_resp *resp,
						int size)
{
	struct avdtp_remote_sep *sep;
	uint8_t seid;

	/* Check for minimum required packet size includes:
	 *   1. getcap resp header
	 *   2. media transport capability (2 bytes)
	 *   3. media codec capability type + length (2 bytes)
	 *   4. the actual media codec elements
	 * */
	if (size < (sizeof(struct getcap_resp) + 4 +
				sizeof(struct avdtp_media_codec_capability))) {
		error("Too short getcap resp packet");
		return FALSE;
	}

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

	sep = find_remote_sep(session->seps, seid);

	debug("seid %d type %d media %d", sep->seid,
					sep->type, sep->media_type);

	if (sep->caps) {
		g_slist_foreach(sep->caps, (GFunc) g_free, NULL);
		g_slist_free(sep->caps);
		sep->caps = NULL;
		sep->codec = NULL;
	}

	sep->caps = caps_to_list(resp->caps, size - sizeof(struct getcap_resp),
					&sep->codec);

	return TRUE;
}

static gboolean avdtp_set_configuration_resp(struct avdtp *session,
						struct avdtp_stream *stream,
						struct avdtp_header *resp,
						int size)
{
	struct avdtp_local_sep *sep = stream->lsep;

	if (sep->cfm && sep->cfm->set_configuration)
		sep->cfm->set_configuration(session, sep, stream, NULL,
						sep->user_data);

	avdtp_sep_set_state(session, sep, AVDTP_STATE_CONFIGURED);

	return TRUE;
}

static gboolean avdtp_reconfigure_resp(struct avdtp *session,
					struct avdtp_stream *stream,
					struct avdtp_header *resp, int size)
{
	return TRUE;
}

static gboolean avdtp_open_resp(struct avdtp *session, struct avdtp_stream *stream,
				struct seid_rej *resp, int size)
{
	struct avdtp_local_sep *sep = stream->lsep;

	if (l2cap_connect(session) < 0) {
		avdtp_sep_set_state(session, sep, AVDTP_STATE_IDLE);
		return FALSE;
	}

	session->pending_open = stream;

	avdtp_sep_set_state(session, sep, AVDTP_STATE_OPEN);

	return TRUE;
}

static gboolean avdtp_start_resp(struct avdtp *session,
					struct avdtp_stream *stream,
					struct seid_rej *resp, int size)
{
	struct avdtp_local_sep *sep = stream->lsep;

	if (sep->cfm && sep->cfm->start)
		sep->cfm->start(session, sep, stream, NULL, sep->user_data);

	avdtp_sep_set_state(session, sep, AVDTP_STATE_STREAMING);

	return TRUE;
}

static gboolean avdtp_close_resp(struct avdtp *session,
					struct avdtp_stream *stream,
					struct seid_rej *resp, int size)
{
	struct avdtp_local_sep *sep = stream->lsep;

	avdtp_sep_set_state(session, sep, AVDTP_STATE_CLOSING);

	close(stream->sock);
	stream->sock = -1;

	return TRUE;
}

static gboolean avdtp_suspend_resp(struct avdtp *session,
					struct avdtp_stream *stream,
					struct gen_resp *resp,
					int size)
{
	struct avdtp_local_sep *sep = stream->lsep;

	avdtp_sep_set_state(session, sep, AVDTP_STATE_OPEN);

	stream->idle_timer = g_timeout_add(STREAM_TIMEOUT,
					(GSourceFunc) stream_timeout, stream);

	if (sep->cfm && sep->cfm->suspend)
		sep->cfm->suspend(session, sep, stream, NULL, sep->user_data);

	return TRUE;
}

static gboolean avdtp_abort_resp(struct avdtp *session,
					struct avdtp_stream *stream,
					struct seid_rej *resp, int size)
{
	struct avdtp_local_sep *sep = stream->lsep;

	if (sep->cfm && sep->cfm->abort)
		sep->cfm->abort(session, sep, stream, NULL, sep->user_data);

	avdtp_sep_set_state(session, sep, AVDTP_STATE_IDLE);

	return TRUE;
}

static gboolean avdtp_parse_resp(struct avdtp *session,
					struct avdtp_stream *stream,
					struct avdtp_header *header, int size)
{
	struct avdtp_header *next;

	if (session->prio_queue)
		next = ((struct pending_req *)
				(session->prio_queue->data))->msg;
	else if (session->req_queue)
		next = ((struct pending_req *)
				(session->req_queue->data))->msg;
	else
		next = NULL;

	switch (header->signal_id) {
	case AVDTP_DISCOVER:
		debug("DISCOVER request succeeded");
		return avdtp_discover_resp(session, (void *) header, size);
	case AVDTP_GET_CAPABILITIES:
		debug("GET_CAPABILITIES request succeeded");
		if (!avdtp_get_capabilities_resp(session,
						(void *) header, size))
			return FALSE;
		if (!(next && next->signal_id == AVDTP_GET_CAPABILITIES))
			finalize_discovery(session, 0);
		return TRUE;
	case AVDTP_SET_CONFIGURATION:
		debug("SET_CONFIGURATION request succeeded");
		return avdtp_set_configuration_resp(session, stream,
							(void *) header, size);
	case AVDTP_RECONFIGURE:
		debug("RECONFIGURE request succeeded");
		return avdtp_reconfigure_resp(session, stream,
							(void *) header, size);
	case AVDTP_OPEN:
		debug("OPEN request succeeded");
		return avdtp_open_resp(session, stream, (void *) header, size);
	case AVDTP_SUSPEND:
		debug("SUSPEND request succeeded");
		return avdtp_suspend_resp(session, stream,
							(void *) header, size);
	case AVDTP_START:
		debug("START request succeeded");
		return avdtp_start_resp(session, stream, (void *) header, size);
	case AVDTP_CLOSE:
		debug("CLOSE request succeeded");
		return avdtp_close_resp(session, stream, (void *) header, size);
	case AVDTP_ABORT:
		debug("ABORT request succeeded");
		return avdtp_abort_resp(session, stream, (void *) header, size);
	}

	error("Unknown signal id in accept response: %u", header->signal_id);

	return TRUE;
}

static gboolean seid_rej_to_err(struct seid_rej *rej, int size,
					struct avdtp_error *err)
{
	if (size < sizeof(struct seid_rej)) {
		error("Too small packet for seid_rej");
		return FALSE;
	}

	avdtp_error_init(err, AVDTP_ERROR_ERROR_CODE, rej->error);

	return TRUE;
}

static gboolean conf_rej_to_err(struct conf_rej *rej, int size,
				struct avdtp_error *err, uint8_t *category)
{
	if (size < sizeof(struct conf_rej)) {
		error("Too small packet for conf_rej");
		return FALSE;
	}

	avdtp_error_init(err, AVDTP_ERROR_ERROR_CODE, rej->error);

	if (category)
		*category = rej->category;

	return TRUE;
}

static gboolean stream_rej_to_err(struct stream_rej *rej, int size,
					struct avdtp_error *err,
					uint8_t *acp_seid)
{
	if (size < sizeof(struct conf_rej)) {
		error("Too small packet for stream_rej");
		return FALSE;
	}

	avdtp_error_init(err, AVDTP_ERROR_ERROR_CODE, rej->error);

	if (acp_seid)
		*acp_seid = rej->acp_seid;

	return TRUE;
}

static gboolean avdtp_parse_rej(struct avdtp *session,
				struct avdtp_stream *stream,
				struct avdtp_header *header, int size)
{
	struct avdtp_error err;
	uint8_t acp_seid, category;
	struct avdtp_local_sep *sep = stream ? stream->lsep : NULL;

	switch (header->signal_id) {
	case AVDTP_DISCOVER:
		if (!seid_rej_to_err((void *) header, size, &err))
			return FALSE;
		error("DISCOVER request rejected: %s (%d)",
				avdtp_strerror(&err), err.err.error_code);
		return TRUE;
	case AVDTP_GET_CAPABILITIES:
		if (!seid_rej_to_err((void *) header, size, &err))
			return FALSE;
		error("GET_CAPABILITIES request rejected: %s (%d)",
				avdtp_strerror(&err), err.err.error_code);
		return TRUE;
	case AVDTP_OPEN:
		if (!seid_rej_to_err((void *) header, size, &err))
			return FALSE;
		error("OPEN request rejected: %s (%d)",
				avdtp_strerror(&err), err.err.error_code);
		if (sep && sep->cfm && sep->cfm->open)
			sep->cfm->open(session, sep, stream, &err,
					sep->user_data);
		return TRUE;
	case AVDTP_SET_CONFIGURATION:
		if (!conf_rej_to_err((void *) header, size, &err, &category))
			return FALSE;
		error("SET_CONFIGURATION request rejected: %s (%d)",
				avdtp_strerror(&err), err.err.error_code);
		if (sep && sep->cfm && sep->cfm->set_configuration)
			sep->cfm->set_configuration(session, sep, stream,
							&err, sep->user_data);
		return TRUE;
	case AVDTP_RECONFIGURE:
		if (!conf_rej_to_err((void *) header, size, &err, &category))
			return FALSE;
		error("RECONFIGURE request rejected: %s (%d)",
				avdtp_strerror(&err), err.err.error_code);
		if (sep && sep->cfm && sep->cfm->reconfigure)
			sep->cfm->reconfigure(session, sep, stream, &err,
						sep->user_data);
		return TRUE;
	case AVDTP_START:
		if (!stream_rej_to_err((void *) header, size, &err, &acp_seid))
			return FALSE;
		error("START request rejected: %s (%d)",
				avdtp_strerror(&err), err.err.error_code);
		if (sep && sep->cfm && sep->cfm->start)
			sep->cfm->start(session, sep, stream, &err,
					sep->user_data);
		return TRUE;
	case AVDTP_SUSPEND:
		if (!stream_rej_to_err((void *) header, size, &err, &acp_seid))
			return FALSE;
		error("SUSPEND request rejected: %s (%d)",
				avdtp_strerror(&err), err.err.error_code);
		if (sep && sep->cfm && sep->cfm->suspend)
			sep->cfm->suspend(session, sep, stream, &err,
						sep->user_data);
		return TRUE;
	case AVDTP_CLOSE:
		if (!stream_rej_to_err((void *) header, size, &err, &acp_seid))
			return FALSE;
		error("CLOSE request rejected: %s (%d)",
				avdtp_strerror(&err), err.err.error_code);
		if (sep && sep->cfm && sep->cfm->close)
			sep->cfm->close(session, sep, stream, &err,
					sep->user_data);
		return TRUE;
	case AVDTP_ABORT:
		if (!stream_rej_to_err((void *) header, size, &err, &acp_seid))
			return FALSE;
		error("ABORT request rejected: %s (%d)",
				avdtp_strerror(&err), err.err.error_code);
		if (sep && sep->cfm && sep->cfm->abort)
			sep->cfm->abort(session, sep, stream, &err,
					sep->user_data);
		return TRUE;
	default:
		error("Unknown reject response signal id: %u",
				header->signal_id);
		return TRUE;
	}
}

static struct avdtp *find_session(bdaddr_t *src, bdaddr_t *dst)
{
	GSList *l;

	for (l = sessions; l != NULL; l = g_slist_next(l)) {
		struct avdtp *s = l->data;

		if (bacmp(src, &s->src) || bacmp(dst, &s->dst))
			continue;

		return s;
	}

	return NULL;
}

static struct avdtp *avdtp_get_internal(bdaddr_t *src, bdaddr_t *dst)
{
	struct avdtp *session;

	assert(src != NULL);
	assert(dst != NULL);

	session = find_session(src, dst);
	if (session) {
		if (session->pending_auth)
			return NULL;
		else
			return session;
	}

	session = g_new0(struct avdtp, 1);

	session->sock = -1;
	bacpy(&session->src, src);
	bacpy(&session->dst, dst);
	session->ref = 1;
	session->state = AVDTP_SESSION_STATE_DISCONNECTED;

	sessions = g_slist_append(sessions, session);

	return session;
}

struct avdtp *avdtp_get(bdaddr_t *src, bdaddr_t *dst)
{
	struct avdtp *session;

	session = avdtp_get_internal(src, dst);

	if (!session)
		return NULL;

	return avdtp_ref(session);
}

gboolean avdtp_is_connected(bdaddr_t *src, bdaddr_t *dst)
{
	struct avdtp *session;

	session = find_session(src, dst);

	if (!session)
		return FALSE;

	if (session->state != AVDTP_SESSION_STATE_DISCONNECTED)
		return TRUE;

	return FALSE;
}

gboolean avdtp_stream_has_capability(struct avdtp_stream *stream,
				struct avdtp_service_capability *cap)
{
	GSList *l;
	struct avdtp_service_capability *stream_cap;

	for (l = stream->caps; l; l = g_slist_next(l)) {
		stream_cap = l->data;
		if (stream_cap->category == cap->category &&
			stream_cap->length == cap->length) {
			if (!memcmp(stream_cap->data, cap->data, cap->length))
				return TRUE;
		}
	}

	return FALSE;
}

gboolean avdtp_stream_has_capabilities(struct avdtp_stream *stream,
					GSList *caps)
{
	GSList *l;

	for (l = caps; l; l = g_slist_next(l)) {
		struct avdtp_service_capability *cap = l->data;

		if (!avdtp_stream_has_capability(stream, cap))
			return FALSE;
	}

	return TRUE;
}

gboolean avdtp_stream_get_transport(struct avdtp_stream *stream, int *sock,
					uint16_t *imtu, uint16_t *omtu,
					GSList **caps)
{
	if (stream->sock < 0)
		return FALSE;

	if (sock)
		*sock = stream->sock;