a2dpd_output_a2dp.c

linux蓝牙剖面实现

		if (a2dp->len + a2dp->sbc.len >= a2dp->mtu) {

			// time to prepare and send the packet
			memset(&payload_header, 0, sizeof(payload_header));
			memset(&packet_header, 0, sizeof(packet_header));
			payload_header.frame_count = a2dp->frame_count;
			packet_header.v = 2;
			packet_header.pt = 1;
			packet_header.cc = 0;
			packet_header.sequence_number = htons(a2dp->seq_num);
			packet_header.timestamp = htonl(a2dp->timestamp);
			packet_header.ssrc = htonl(1);

			a2dp->timestamp += (a2dp->sbc.blocks + 1) * 4 * (a2dp->sbc.subbands + 1) * 4;
			memcpy(a2dp->buf, &packet_header, sizeof(packet_header));
			memcpy(a2dp->buf + sizeof(packet_header), &payload_header, sizeof(payload_header));
			if (a2dp->sk > 0) {
				// Pause?
				// The value 0 have finally been tested ;)
				// However, we may safely simulate a failed write
				if (!a2dp->pause_writing) {
					// Send our data
					if ((written = write(a2dp->sk, a2dp->buf, a2dp->len)) != a2dp->len) {
						// Error while sending data
						DBG("Wrote %d not %d bytes.", written, a2dp->len);
						/*
						if (errno == EAGAIN) {
							usleep(1);
							if ((written = write(a2dp->sk, a2dp->buf, a2dp->len)) != a2dp->len) {
								// Error while sending data
								DBG("Wrote %d not %d bytes. (2)", written, a2dp->len);
								// Return the error
								result = written;
							}
						}
						else
						{
						}
						*/
						// Return the error
						result = written;
					} else {
						// Measure bandwith usage
						struct timeval now = { 0, 0 };
						struct timeval interval = { 0, 0 };

						if(a2dp->bandwithtimestamp.tv_sec==0)
							gettimeofday(&a2dp->bandwithtimestamp, NULL);

						//DBG("Wrote %d bytes.", written);
						// See if we must wait again
						gettimeofday(&now, NULL);
						timersub(&now, &a2dp->bandwithtimestamp, &interval);
						if(interval.tv_sec>0) {
							if(a2dp->user_flags & A2DPD_FLAGS_DISPLAYBANDWITH) {
								DBG("Bandwith: %d (%d kbps) %d", a2dp->bandwithcount, a2dp->bandwithcount/128, a2dp->sbc.bitpool);
							}
							a2dp->bandwithtimestamp = now;
							a2dp->bandwithcount = 0;
						}

						a2dp->bandwithcount += written;
					}
					
					
				} else {
					// Make the upper layer believe we sent data
					result = a2dp->len;
				}
			} else {
				DBG("Invalid socket");
			}
			// Reset buffer of data to send
			a2dp->len = sizeof(struct media_packet_header) + sizeof(struct media_payload_header);
			a2dp->frame_count = 0;
			a2dp->seq_num++;
		}
		// Append sbc encoded data to buf, until buf reaches A2DPMAXIMUMTRANSFERUNITSIZE to send
		a2dp->frame_count++;
		memcpy(a2dp->buf + a2dp->len, a2dp->sbc.data, a2dp->sbc.len);
		a2dp->len += a2dp->sbc.len;
	}

	// Display reception delay
	if(lpHdr && lpHdr->pcm_buffer_size != 0) {
		struct timeval now;
		gettimeofday(&now, NULL);
		timersub(&now, &lpHdr->packet_date, &now);
		// DBG("delay after transmit: %d µs", (int)now.tv_usec);
	}

	return result;
}

/*
// monitor the control connection for pause/play signals from headset
// note this signaling is in the avdtp core; avrcp signaling is different
static void *listen_thread(void *param)
{
	snd_pcm_a2dp_t *a2dp = (snd_pcm_a2dp_t *) param;

	if (a2dp->control_sk < 0) {
		DBG("Listen thread not started [control_sk=%d]", a2dp->control_sk);
		return NULL;
	}

	DBG("Listen thread running [control_sk=%d]", a2dp->control_sk);

//#ifdef FASTTIMEOUTS
	// Set a timeout to close thread
	struct timeval t = { 1, 0 };
	setsockopt(a2dp->control_sk, SOL_SOCKET, SO_SNDTIMEO, &t, sizeof(t));
	setsockopt(a2dp->control_sk, SOL_SOCKET, SO_RCVTIMEO, &t, sizeof(t));
//#endif

	// Loop until end of writing
	while (!a2dp->stop_writing) {
		if (a2dp_handle_avdtp_message(a2dp, a2dp->control_sk, NULL, NULL, 0) < 0) {
			// Error
			//FIXME we must reconnect
			usleep(100 * 1000);
		}
		// Make sure we do not spin in case of errors
		usleep(10 * 1000);
	}

	return NULL;
}

int a2dp_connect(LPA2DP a2dp)
{
	if(a2dp->state == AVDTP_STATE_DISCONNECTED) {
		a2dp_ctl_connect(a2dp);
	}

	if(a2dp->state == AVDTP_STATE_IDLE) {
		a2dp_ctl_negociate(a2dp);
	}

	if(a2dp->state == AVDTP_STATE_CONFIGURED) {
		a2dp_stream_connect(a2dp);
	}

	if(a2dp->state == AVDTP_STATE_OPEN) {
		a2dp_stream_start(a2dp);
	}

	return (a2dp->state == AVDTP_STATE_STREAMING)?0:-1;
}

int a2dp_use_socket(LPA2DP a2dp, int cmdfd)
{
	if(a2dp->state == AVDTP_STATE_DISCONNECTED) {
		a2dp_ctl_use_socket(a2dp, cmdfd);
	}

	if(a2dp->state == AVDTP_STATE_IDLE) {
		a2dp_ctl_negociate(a2dp);
	}

	if(a2dp->state == AVDTP_STATE_CONFIGURED) {
		a2dp_stream_connect(a2dp);
	}

	if(a2dp->state == AVDTP_STATE_OPEN) {
		a2dp_stream_start(a2dp);
	}

	return (a2dp->state == AVDTP_STATE_STREAMING)?0:-1;
}
*/
void a2dp_stream_stop(snd_pcm_a2dp_t * a2dp)
{
	DBG("");
/*
	if (a2dp->control_sk > 0) {
		struct stream_cmd close_stream;
		struct close_stream_rsp close_resp;
	
		memset(&close_stream, 0, sizeof(close_stream));
		memset(&close_resp, 0, sizeof(close_resp));
	
		// the stream-close used to make the iTech headset lock up and require it to be powercycled
		// should be tested again now that we drain the queue properly
		//FIXME Should be tested again now that we read the answer, Sonorix used to do lock and no longer does it!
		a2dp_set_state(AVDTP_STATE_CLOSING);

		init_request(&close_stream.header, AVDTP_CLOSE);
		close_stream.acp_seid = a2dp->seid;
		if (a2dp->control_sk > 0) {
			if(write(a2dp->control_sk, &close_stream, sizeof(close_stream)) == sizeof(close_stream)) {
				// Receive close stream answer if any?
				//int i;
				int size;
				DBG("Receiving answer to close stream");
				size = recv(a2dp->control_sk, &close_stream, sizeof(close_stream), 0);
				DBG("Received answer size=%d", size);
			} else {
				DBG("Couldn't send close_stream");
			}
		}
	}
*/
	DBG("Closing stream socket %d", a2dp->sk);
	close_socket(&a2dp->sk);
	DBG("Closed");

	a2dp_set_state(AVDTP_STATE_IDLE);
	a2dp->sk = -1;
}

void a2dp_disconnect(snd_pcm_a2dp_t * a2dp)
{
	DBG("");
	a2dp_stream_stop(a2dp);

	if(a2dp->sdp_session != NULL) {
		sdp_close(a2dp->sdp_session);
		a2dp->sdp_session = NULL;
	}

	DBG("Closing ctl socket %d", a2dp->control_sk);
	close_socket(&a2dp->control_sk);
	DBG("Closed");

	a2dp_set_state(AVDTP_STATE_DISCONNECTED);
}

void a2dp_state_disconnect(LPA2DP a2dp)
{
	if(a2dp->state != AVDTP_STATE_DISCONNECTED) {
		switch(a2dp->state) {
		case AVDTP_STATE_OPEN:
		case AVDTP_STATEX_STREAM_STARTING:
		case AVDTP_STATEX_STREAM_STARTING_WAIT:
		case AVDTP_STATEX_STREAM_SUSPENDING:
		case AVDTP_STATEX_STREAM_SUSPENDING_WAIT:
		case AVDTP_STATEX_STREAM_CLOSING:
		case AVDTP_STATEX_STREAM_CLOSING_WAIT:
		case AVDTP_STATE_STREAMING:
		case AVDTP_STATE_CLOSING:
		case AVDTP_STATE_ABORTING:
			a2dp_set_state(AVDTP_STATEX_STREAM_CLOSING);
			break;
		default:
			a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
			break;
		}
	} else {
		DBG("Filtering state : already disconnected");
	}
}

void a2dp_set_dst_addr(LPA2DP a2dp, char* bdaddr)
{
	bacpy(&a2dp->src, BDADDR_ANY);

	if(bdaddr && bdaddr[0]) {
		strncpy(a2dp->bdaddr, bdaddr, sizeof(a2dp->bdaddr));
		a2dp->bdaddr[sizeof(a2dp->bdaddr)-1] = 0;
		str2ba(a2dp->bdaddr, &a2dp->dst);
	} else {
		a2dp->bdaddr[0] = 0;
	}

	// Disconnect so that we reconnect
	if(a2dp->state != AVDTP_STATE_DISCONNECTED)
		a2dp_state_disconnect(a2dp);
}

snd_pcm_a2dp_t *a2dp_alloc(void)
{
	snd_pcm_a2dp_t *a2dp;
	DBG("");
	a2dp = malloc(sizeof(*a2dp));
	if (!a2dp)
		return NULL;

	memset(a2dp, 0, sizeof(*a2dp));
	a2dp->seq_num = 1;
	a2dp->mtu = A2DPMAXIMUMTRANSFERUNITSIZE;
	a2dp->len = sizeof(struct media_packet_header) + sizeof(struct media_payload_header);

	sbc_init(&a2dp->sbc, 0L);
	a2dp->sbc.rate = A2DPD_FRAME_RATE;
	a2dp->sbc.subbands = 8;	// safe default
	a2dp->sbc.blocks = 16;	// safe default
	a2dp->sbc.bitpool = 32;	// recommended value 53, safe default is 32
	DBG("(a2dp = %p)", a2dp);

	return a2dp;
}

void a2dp_free(LPA2DP a2dp)
{
	DBG("Disconnecting");
	a2dp_disconnect(a2dp);

	DBG("Freeing sbc");
	sbc_finish(&a2dp->sbc);

	DBG("(a2dp = %p)", a2dp);
	free(a2dp);

	DBG("OK");
}

void a2dp_init(void)
{
	a2dpd_register_sdp();
}

void a2dp_exit(void)
{
	a2dpd_unregister_sdp();
}

LPA2DP a2dp_new(A2DPSETTINGS* settings)
{
	snd_pcm_a2dp_t *a2dp = NULL;

	if(settings) {
		a2dp = a2dp_alloc();

		DBG("%s, %d", settings->bdaddr, settings->framerate);

		if (a2dp) {
			memcpy(&a2dp->settings, settings, sizeof(a2dp->settings));
			a2dp->sbc.rate = settings->framerate;
			a2dp->sbc.channels = max(1, min(settings->channels, 2));
			a2dp->sbc.bitpool = settings->sbcbitpool;
			a2dp->user_flags = settings->flags;
			if(settings->channels==1)
				a2dp->sbc.joint=1;

			a2dp_set_dst_addr(a2dp, settings->bdaddr);

			a2dp_set_state(AVDTP_STATE_DISCONNECTED);
		}
	}
	return a2dp;
}

void a2dp_destroy(LPA2DP *a2dp)
{
	DBG("a2dp = %p", a2dp);
	a2dp_free(*a2dp);
	*a2dp = NULL;
}

void a2dp_set_user_flags(LPA2DP a2dp, int flags)
{
	a2dp->user_flags = flags;
}

int a2dp_get_user_flags(LPA2DP a2dp)
{
	return a2dp->user_flags;
}

int a2dp_make_listen_socket(unsigned short psm)
{
	struct sockaddr_l2 addr;
	int on = 1;
	int sockfd = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);

	if (sockfd < 0)
		RETURNERROR("Cannot create socket for psm %d", (int)psm);

	(void)setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on));

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

	if (bind(sockfd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
		close(sockfd);
		RETURNERROR("Cannot bind socket %d for psm %d", sockfd, (int)psm);
	}

	if (listen(sockfd, 5) < 0) {
		close(sockfd);
		RETURNERROR("Cannot listen socket %d for psm %d", sockfd, (int)psm);
	}

	return sockfd;
}

int a2dp_wait_connection(int sockfd, char *szRemote, int iRemoteSize, uint16_t * mtu)
{
	// Wait client connection
	struct sockaddr_l2 addr;
	int new_fd = -1;
	socklen_t addrlen = sizeof(addr);

	// Timeouts each second to read variables
	struct timeval t = { 1, 0 };
	(void)setsockopt(sockfd, SOL_SOCKET, SO_SNDTIMEO, &t, sizeof(t));
	(void)setsockopt(sockfd, SOL_SOCKET, SO_RCVTIMEO, &t, sizeof(t));

	if (szRemote)
		*szRemote = '\0';

	if(poll_accept(sockfd, 1000)) {
		new_fd = accept(sockfd, (struct sockaddr *) &addr, &addrlen);

		if (new_fd >= 0) {
			if(mtu)
				*mtu = get_socket_omtu(sockfd);

			if(fcntl(new_fd, F_SETFL, O_NONBLOCK)<0)
				DBG("Failed to set non blocking socket %d", new_fd);

			if (szRemote) {
				char* tmpaddr = batostr(&addr.l2_bdaddr);
				strncpy(szRemote, tmpaddr, iRemoteSize);
				free(tmpaddr);
				szRemote[iRemoteSize - 1] = '\0';
			}
		}
	} else {
		sleep(1);
	}
	return new_fd;
}

// This function handle the bluetooth connection
int a2dp_ctl_handle_commands(LPA2DP a2dp, int sockfd, char* lpFrame, int lpFrameSize)
{
	int result = 0;
	int accepted = 0;
	int wrresult = 0;
	struct avdtp_header *pkt_hdr = (struct avdtp_header *) lpFrame;
	int ans_size = sizeof(*pkt_hdr);

	if (pkt_hdr->signal_id == AVDTP_DISCOVER) {
		struct sepd_resp* discover_resp = (struct sepd_resp*)lpFrame;
		DBG("Received signal AVDTP_DISCOVER(%d) from set", pkt_hdr->signal_id);
		ans_size = sizeof(*pkt_hdr)+sizeof(discover_resp->infos[0]);