a2dpd_output_a2dp.c

linux蓝牙剖面实现

	setsockopt(sk, SOL_SOCKET, SO_SNDTIMEO, &t, sizeof(t));
	setsockopt(sk, SOL_SOCKET, SO_RCVTIMEO, &t, sizeof(t));
#endif
	memset(&addr, 0, sizeof(addr));
	addr.l2_family = AF_BLUETOOTH;
	bacpy(&addr.l2_bdaddr, src);
	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
		DBG("Can't bind socket.");
		return -1;
	}

	// Get default options
	opt = sizeof(opts);
	if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &opt) < 0) {
		DBG("Can't get default L2CAP options.");
		return -1;
	}

	// Set new options
	if (mtu && *mtu) {
		opts.omtu = *mtu;
		//opts.imtu = *mtu;
	}
	if (setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, opt) < 0) {
		DBG("Can't set L2CAP options.");
		return -1;
	}

	memset(&addr, 0, sizeof(addr));
	addr.l2_family = AF_BLUETOOTH;
	bacpy(&addr.l2_bdaddr, dst);
	addr.l2_psm = htobs(psm);

	tries = 0;
	while (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
		char* tmpaddr = batostr(&addr.l2_bdaddr);
		DBG("Can't connect to %s on psm %d.", tmpaddr, psm);
		free(tmpaddr);
		if (++tries > NBSDPRETRIESMAX) {
			close(sk);
			return -1;
		}
		sleep(1);
	}
	opt = sizeof(opts);
	if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &opt) < 0) {
		DBG("Can't get L2CAP options.");
		close(sk);
		return -1;
	}
	//DBG( "Connected psm=%d sk=%d [imtu %d, omtu %d, flush_to %d]", psm, sk, opts.imtu, opts.omtu, opts.flush_to);

	if (mtu)
		*mtu = opts.omtu;

	return sk;
}

// Detect whether A2DP Sink is present at the destination or not
int detect_a2dp(bdaddr_t * src, bdaddr_t * dst, unsigned short *psm, unsigned long *flags)
{
	sdp_session_t *sess;
	sdp_list_t *attrid, *search, *seq, *next;
	sdp_data_t *pdlist;
	uuid_t group;
	uint32_t range = 0x0000ffff;
	int err;

	if(flags)
		*flags = 0;

	if (psm)
		*psm = 25;

	sess = sdp_connect(src, dst, 0);
	if(!sess)
		RETURNERROR("Warning: failed to connect to SDP server");

	// 0x1108->all? 0x1101->rf sink 0x111e->handsfree 0x1108->headset
	sdp_uuid16_create(&group, 0x110d);
	search = sdp_list_append(0, &group);
	attrid = sdp_list_append(0, &range);
	err = sdp_service_search_attr_req(sess, search, SDP_ATTR_REQ_RANGE, attrid, &seq);
	sdp_list_free(search, 0);
	sdp_list_free(attrid, 0);

	if (err) {
		sdp_close(sess);
		RETURNERROR("Service Search failed.");
	}

	// Free sdp record
	for (; seq; seq = next) {
		sdp_record_t *rec = (sdp_record_t *) seq->data;
		next = seq->next;
		free(seq);
		sdp_record_free(rec);
	}
	
	DBG("Service Search OK");

	sdp_uuid16_create(&group, PNP_INFO_SVCLASS_ID);
	search = sdp_list_append(0, &group);
	attrid = sdp_list_append(0, &range);
	err = sdp_service_search_attr_req(sess, search, SDP_ATTR_REQ_RANGE, attrid, &seq);
	sdp_list_free(search, 0);
	sdp_list_free(attrid, 0);

	if (!err)
	{
		DBG("Checking non spec audio");

		for (; seq; seq = next) {
			sdp_record_t *rec = (sdp_record_t *) seq->data;
			uint16_t vendor, product, version;

			pdlist = sdp_data_get(rec, 0x0201);
			vendor = pdlist ? pdlist->val.uint16 : 0x0000;
	
			pdlist = sdp_data_get(rec, 0x0202);
			product = pdlist ? pdlist->val.uint16 : 0x0000;

			pdlist = sdp_data_get(rec, 0x0203);
			version = pdlist ? pdlist->val.uint16 : 0x0000;

			DBG("Product ID %04x:%04x:%04x", vendor, product, version);

			if (vendor == 0x1310 && product == 0x0100 && version == 0x0104) {
				DBG("Enabling GCT media payload workaround");
				if (flags)
					*flags |= A2DPD_FLAGS_NONSPECAUDIO;
			}

			next = seq->next;
			free(seq);
			sdp_record_free(rec);
		}
	}
	sdp_close(sess);

	return 0;
}
*/
int a2dp_ctl_use_socket(LPA2DP a2dp, int cmdfd)
{
	if(a2dp->state != AVDTP_STATE_DISCONNECTED)
		return -1;

	if(cmdfd < 0)
		return -1;

	if(a2dp->control_sk > 0)
		return -1;

	a2dp->control_sk = cmdfd;
	a2dp_set_state(AVDTP_STATE_IDLE);

	return 0;
}

int a2dp_state_use_socket(LPA2DP a2dp, int cmdfd)
{
	int ret = -1;

	if(cmdfd < 0)
		return -1;

	// First connection?
	if(a2dp->control_sk <= 0) {
		// It's a control socket
		DBG("Incoming socket %d : used as control socket", cmdfd);
		ret = a2dp_ctl_use_socket(a2dp, cmdfd);
		if(ret>=0) {
			// Passive Acceptor role
			a2dp->role = A2DPD_ACCEPTOR;
			DBG("Role is ACCEPTOR");
			// Skip sdp browse and connection intermediate state to go to idle state
			a2dp_set_state(AVDTP_STATE_IDLE);
		}
	} else if(a2dp->role == A2DPD_ACCEPTOR && a2dp->sk <= 0) {
		// Already connected, then it's a stream socket
		DBG("Incoming socket %d : used as stream socket", cmdfd);
		a2dp->sk = cmdfd;
		a2dp->mtu = get_socket_omtu(cmdfd);
		a2dp_set_state(AVDTP_STATE_OPEN);
		ret = 0;
	} else {
		DBG("Incoming socket %d : refused", cmdfd);
		ret = -1;
	}

	return ret;
}

int a2dp_state_connect(LPA2DP a2dp)
{
	if(strlen(a2dp->bdaddr)==0)
		return -1;

	switch(a2dp->state) {
	case AVDTP_STATE_DISCONNECTED:	if(a2dp->role==A2DPD_NOROLE)
						a2dp_set_state(AVDTP_STATEX_SDP_CONNECTING);
					break;
	case AVDTP_STATE_OPEN:		if(a2dp->role==A2DPD_INITIATOR)
						a2dp_set_state(AVDTP_STATEX_STREAM_STARTING);
					break;
	default:                       return -1;
	}

	return 0;
}

int a2dp_is_streaming(LPA2DP a2dp)
{
	// Connected to a device but not necessarily streaming
	return
		(a2dp->state == AVDTP_STATE_STREAMING)
		;
}

int a2dp_is_connected(LPA2DP a2dp)
{
	// Connected to a device but not necessarily streaming
	return
		(a2dp->state == AVDTP_STATE_OPEN) ||
		(a2dp->state == AVDTP_STATEX_STREAM_STARTING) ||
		(a2dp->state == AVDTP_STATEX_STREAM_STARTING_WAIT) ||
		(a2dp->state == AVDTP_STATE_STREAMING) ||
		(a2dp->state == AVDTP_STATEX_STREAM_SUSPENDING_WAIT) ||
		(a2dp->state == AVDTP_STATEX_STREAM_SUSPENDING)
		;
}

int a2dp_is_connecting(LPA2DP a2dp)
{
	// Include connected state
	return
		(a2dp->state != AVDTP_STATE_DISCONNECTED)
		;
}

void a2dp_state_suspend(LPA2DP a2dp)
{
	if(a2dp->state == AVDTP_STATE_STREAMING) {
		a2dp_set_state(AVDTP_STATEX_STREAM_SUSPENDING);
	} else {
		DBG("Filtering state : State invalid for suspending");
	}
}

void a2dp_state_startstream(LPA2DP a2dp)
{
	if(a2dp->state == AVDTP_STATE_OPEN) {
		a2dp_set_state(AVDTP_STATEX_STREAM_STARTING);
	} else {
		DBG("State invalid for starting stream");
	}
}
/*
int a2dp_ctl_connect(LPA2DP a2dp)
{
	int cmdfd = -1;
	unsigned short psm_cmd;
	unsigned long flags = 0;

	DBG("Begin");

	if(a2dp->state != AVDTP_STATE_DISCONNECTED)
		return -1;

	if (detect_a2dp(&a2dp->src, &a2dp->dst, &psm_cmd, &flags) < 0)
		RETURNERROR("could not find A2DP services on device");

	DBG("Found A2DP Sink at the destination (psm_cmd=%d)", psm_cmd);

	cmdfd = do_connect(&a2dp->src, &a2dp->dst, psm_cmd, NULL);
	if (cmdfd < 0)
		RETURNERROR("cannot open psm_cmd = %d", psm_cmd);

	return a2dp_ctl_use_socket(a2dp, cmdfd);
}

int a2dp_ctl_negociate(LPA2DP a2dp)
{
	struct sepd_req discover_req;
	struct sepd_resp discover_resp;
	int seid, nb_seid;
	int size;
	int i;
	int tries;
	unsigned short psm_stream = 0;

	// avdt_discover_req
	memset(&discover_req, 0, sizeof(discover_req));
	init_request(&discover_req.header, AVDTP_DISCOVER);

	if(write(a2dp->control_sk, &discover_req, sizeof(discover_req)) != sizeof(discover_req))
		RETURNERROR("couldn't send avdtp_discover");

	tries = 0;
	memset(&discover_resp, 0, sizeof(discover_resp));

	// SONORIX sends us a discover signal we should answer but we will discard
	while (tries < 10) {
		size = a2dp_handle_avdtp_message(NULL, a2dp->control_sk, &discover_req.header, &discover_resp.header, sizeof(discover_resp));
		if (size > 0) {
			// Answer to what we send
			break;
		} else {
			// Not answer
			usleep(100);
			tries++;
		}
	}

	if (size < sizeof(discover_resp.header))
			RETURNERROR("couldn't get avdtp_discover (size=%d, min=%d, max=%d)", size, sizeof(discover_resp.header), sizeof(discover_resp));
		
	DBG("Got a Stream End Point Discovery (%d bytes) Response (msgtype=%s,pkttype=%d,lbl=%d,sig=%s,rfa=%d)",
		    size, get_msgtype(discover_resp.header.message_type), discover_resp.header.packet_type, discover_resp.header.transaction_label, get_signal(discover_resp.header.signal_id), discover_resp.header.rfa0);

	seid = -1;
	if(size<sizeof(discover_resp.header)) {
		DBG("Received invalid capabilities (size=%d, wanted=%d)", size, sizeof(discover_resp.header));
		return -1;
	}

	nb_seid = (size - sizeof(discover_resp.header)) / sizeof(struct acp_seid_info);

	DBG("received %d capabilities", nb_seid);

	for (i = 0; i < nb_seid; i++) {
		if (process_seid(a2dp->control_sk, &discover_resp.infos[i], &psm_stream, &a2dp->sbc) == 0) {
			seid = discover_resp.infos[i].acp_seid;
			break;
		}
	}

	if (seid == -1)
		RETURNERROR("couldn't locate the correct seid");

	DBG("Seid selected: %d, (psm=%d)", seid, (int)psm_stream);

	a2dp_set_state(AVDTP_STATE_CONFIGURED);

	a2dp->mtu = A2DPMAXIMUMTRANSFERUNITSIZE;
	a2dp->seid = seid;
	a2dp->psm_stream = psm_stream;
	return a2dp->control_sk;
}

int a2dp_stream_connect(LPA2DP a2dp)
{

	// Connect the stream
	a2dp->sk = do_connect(&a2dp->src, &a2dp->dst, a2dp->psm_stream, NULL);
	if (a2dp->sk < 0)
		RETURNERROR("Cannot open stream (psm=%d)", (int)a2dp->psm_stream);
	a2dp_set_state(AVDTP_STATE_OPEN);
	return 0;
}

int a2dp_stream_start(LPA2DP a2dp)
{
	int result = -1;
	struct stream_cmd start_stream;
	struct start_stream_rsp start_resp;

	// start the stream
	memset(&start_stream, 0, sizeof(start_stream));
	init_request(&start_stream.header, AVDTP_START);
	start_stream.acp_seid = a2dp->seid;

	if (write(a2dp->control_sk, &start_stream, sizeof(start_stream)) != sizeof(start_stream))
		RETURNERROR("couldn't send start_stream");

	if (read(a2dp->control_sk, &start_resp, sizeof(start_resp)) < sizeof(start_resp) - 2 || start_resp.header.message_type == MESSAGE_TYPE_REJECT)
		RETURNERROR("didn't receive start_resp confirm for seid = %d", a2dp->seid);

	// We should be streaming now
	a2dp_set_state(AVDTP_STATE_STREAMING);
	a2dp->pause_writing = 0;

	return result;
}
*/
// We have pcm data to send through bluetooth
int a2dp_transfer_raw(LPA2DP a2dp, const char *pcm_buffer, int pcm_buffer_size, AUDIOPACKETHEADER* lpHdr)
{
	// No error
	int result = 0;
	struct media_packet_header packet_header;
	struct media_payload_header payload_header;
	int codesize, datatoread;
	int written;

	// Check parameter
	if (a2dp == 0 || pcm_buffer == 0 || pcm_buffer_size == 0)
		RETURNERROR("Invalid Parameter");

	// How much data can be encoded by sbc at a time?
	// 16 bits * 2 channels * 16 blocks * 8 subbands = 4096bits = 512 o
	codesize = a2dp->sbc.subbands * a2dp->sbc.blocks * a2dp->sbc.channels * 2;
	// 44 bitpool?
	//codesize=a2dp->sbc.bitpool*a2dp->sbc.subbands*a2dp->sbc.blocks/8;
	datatoread = min((BUFS - a2dp->lenbufe), pcm_buffer_size);

	// If state is not streaming then return
	if(!a2dp_is_streaming(a2dp))
		return datatoread;

	// Enqueue data in bufe
	if (a2dp->lenbufe + datatoread < BUFS) {
		// Append data to bufe, for sbc encoding
		memcpy_changeendian(a2dp->bufe + a2dp->lenbufe, pcm_buffer, datatoread);
		//DBG("Enqueud pcm (codesize=%d, datatoread=%d, lenbufe=%d/%d, mtu=%d)", codesize, datatoread, a2dp->lenbufe, BUFS, a2dp->mtu);
		a2dp->lenbufe += datatoread;
	} else {
		//DBG("Cannot enqueue pcm (codesize=%d, datatoread=%d, lenbufe=%d/%d, mtu=%d)", codesize, datatoread, a2dp->lenbufe, BUFS, a2dp->mtu);
		// Let the encoder dequeue the buffer and retry later
		datatoread = 0;
	}

	result = datatoread;

	// If bufe is full, encode
	if (a2dp->lenbufe >= codesize) {
		// Enough data to encode (sbc wants 1k blocks)
		int encoded;
		encoded = sbc_encode(&(a2dp->sbc), a2dp->bufe, codesize);	//encode

		if (encoded <= 0)
			RETURNERROR("Error while encoding");

		memmove(a2dp->bufe, a2dp->bufe + encoded, a2dp->lenbufe - encoded);	// Shift the bufe
		a2dp->lenbufe -= encoded;

		//DBG("SBC encoding = %d,%d, total=%d bytes> lenbufe = %d", a2dp->len, encoded, a2dp->sbc.len, a2dp->lenbufe);

		// Send data through bluetooth