a2dpd_output_sco.c

linux蓝牙剖面实现

/*
*
*  BlueZ - Bluetooth protocol stack for Linux
*
*  Copyright (C) 2004-2005  Marcel Holtmann <[e-mail]>
*
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*
*/

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <malloc.h>
#include <signal.h>
#include <time.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/select.h>
#include <sys/poll.h>

#include <bluetooth/bluetooth.h>
#include <bluetooth/rfcomm.h>
#include <bluetooth/l2cap.h>
#include <bluetooth/sdp.h>
#include <bluetooth/sdp_lib.h>
#include <bluetooth/sco.h>

#include "a2dpd_output_sco.h"
#include "a2dpd_protocol.h"
#include "a2dpd_tools.h"
#include "a2dpd_ipc.h"

enum {
	SCO_STATE_DISCONNECTED,
	SCO_STATE_DISCONNECTING,
	SCO_STATEX_SDP_CONNECTING,
	SCO_STATEX_SDP_CONNECTING_WAIT,
	SCO_STATE_RFC_CONNECTING,
	SCO_STATE_RFC_CONNECTING_WAIT,
	SCO_STATE_CONNECTED,
	SCO_STATE_SCO_CONNECTING,
	SCO_STATE_SCO_CONNECTED,
};

#define sco_set_state(new_state) do { DBG("State %s", #new_state); sco->state = new_state; } while (0)

typedef struct sco {
	int state;
	int sk_rfc;
	int sk_sco;
	int sk_sdp;
	bdaddr_t dst;
	char bdaddr[18];
	sdp_session_t *sdp_session;
} sco_t;

static int bw_in = 0;
static int bw_out = 0;
static struct timeval bandwith_timer = {0};

int sco_transfer_raw(LPSCO sco, const char *pcm_buffer, int pcm_buffer_size)
{
	int result = -1;

	struct timeval now, interval;
	now.tv_sec = now.tv_usec = interval.tv_sec = interval.tv_usec = 0;
	if(bandwith_timer.tv_sec==0)
		(void)gettimeofday(&bandwith_timer, 0);
	(void)gettimeofday(&now, 0);
	timersub(&now, &bandwith_timer, &interval);
	if(interval.tv_sec>0) {
		//DBG("bw(in=%d, out=%d)", bw_in, bw_out);
		(void)gettimeofday(&bandwith_timer, 0);
		bw_out=0;
		bw_in=0;
	}
	bw_out += pcm_buffer_size;

	if(sco->state == SCO_STATE_SCO_CONNECTED) {
		result = send_socket(sco->sk_sco, (char*)pcm_buffer, pcm_buffer_size);

		switch (result) {
		default:
			//DBG("result(%d)", result);
			break;
		}
	} else {
		result = pcm_buffer_size;
		//DBG("SCO not connected");
	}

	return result;
}

static sco_t *sco_alloc(void)
{
	sco_t *sco;
	sco = malloc(sizeof(*sco));
	if (!sco)
		return NULL;

	memset(sco, 0, sizeof(*sco));
	return sco;
}

static void sco_disconnect(LPSCO sco)
{
	if(sco->state != SCO_STATE_DISCONNECTED) {
		DBG("Disconnecting");
		// Free data
		if(sco->sdp_session)
			(void)sdp_close(sco->sdp_session);
		sco->sdp_session = NULL;

		sco->sk_sdp = 0;
	
		close_socket(&sco->sk_rfc);
		close_socket(&sco->sk_sco);
	
		// Reset to init state
		sco_set_state(SCO_STATE_DISCONNECTED);
	}
}

static void sco_free(LPSCO sco)
{
	DBG("Disconnecting");
	sco_disconnect(sco);

	DBG("(sco = %p)", sco);
	free(sco);
}

void sco_init(void)
{
	bandwith_timer.tv_sec = bandwith_timer.tv_usec = 0;
}

void sco_exit(void)
{
}

static uint8_t get_rfcomm_channel(LPSCO sco)
{
	/* Retrieving sdp info */
	uint32_t range = 0x0000ffff;
	sdp_list_t *attrid=NULL, *search=NULL, *seq=NULL, *next=NULL;
	uuid_t group;
	int searchresult;
	uint8_t channel = 0;

	if(sco->sdp_session) {
		DBG("Service search");
		sdp_uuid16_create(&group, HEADSET_SVCLASS_ID);

		// Remove non blocking attribute
		(void)fcntl(sco->sdp_session->sock, F_SETFL, fcntl(sco->sdp_session->sock, F_GETFL, 0)&~O_NONBLOCK);

		search = sdp_list_append(0, &group);
		attrid = sdp_list_append(0, &range);
		searchresult = sdp_service_search_attr_req(sco->sdp_session, search, SDP_ATTR_REQ_RANGE, attrid, &seq);
		sdp_list_free(attrid, NULL);
		sdp_list_free(search, NULL);

		if ((searchresult==0) && (seq!=NULL)) {
			DBG("Parsing results");
			for (; seq; seq = next) {
				sdp_record_t *rec = (sdp_record_t *) seq->data;
				sdp_list_t *list = 0;
				DBG("Record");
				if (sdp_get_access_protos(rec, &list) == 0) {
					channel = (uint8_t)sdp_get_proto_port(list, RFCOMM_UUID);
					DBG("Found %d", (int)channel);
				}
				next = seq->next;
				free(seq);
				sdp_record_free(rec);
			}
		} else {
			DBG("Service search failed (result=%d, seq=%p)", searchresult, seq);
		}

		(void)sdp_close(sco->sdp_session);
		sco->sdp_session = NULL;
	} else {
		DBG("Invalid SDP Session");
	}

	return channel;
}

LPSCO sco_new(char *bdaddr)
{
	LPSCO sco = NULL;
	DBG("");

	sco = sco_alloc();
	if (!sco) {
		DBG("Can't allocate");
		return NULL;
	}

	sco_set_state(SCO_STATE_DISCONNECTED);

	sco_set_dst_addr(sco, bdaddr);

	return sco;
}

void sco_destroy(LPSCO *sco)
{
	DBG("sco = %p", sco);
	sco_free(*sco);
	*sco = NULL;
}

int sco_state_connect( LPSCO sco)
{
	if(strlen(sco->bdaddr)==0)
		return -1;

	switch(sco->state) {
	case SCO_STATE_DISCONNECTED:	sco_set_state(SCO_STATEX_SDP_CONNECTING);
					break;
	default:                       return -1;
	}

	return 0;
}

int sco_is_connected(LPSCO sco)
{
	return(sco->state == SCO_STATE_SCO_CONNECTED);
}

int sco_is_connecting(LPSCO sco)
{
	return(sco->state != SCO_STATE_DISCONNECTED);
}

void sco_state_disconnect(LPSCO sco)
{
	if(sco->state != SCO_STATE_DISCONNECTED) {
		switch(sco->state) {
		case SCO_STATE_DISCONNECTED:
			break;
		default:
			DBG("Disconnection asked");
			sco_set_state(SCO_STATE_DISCONNECTING);
			break;
		}
	} else {
		DBG("Filtering state : already disconnected");
	}
}

void sco_state_startstream(LPSCO sco)
{
	sco=sco;
	//FIXME
}

void sco_state_suspend(LPSCO sco)
{
	sco=sco;
	//FIXME
}

void sco_set_dst_addr(LPSCO sco, char* bdaddr)
{
	if((bdaddr!=NULL) && (bdaddr[0]!='\0')) {
		strncpy(sco->bdaddr, bdaddr, sizeof(sco->bdaddr));
		sco->bdaddr[sizeof(sco->bdaddr)-1] = '\0';
		(void)str2ba(sco->bdaddr, &sco->dst);
	} else {
		sco->bdaddr[0] = '\0';
	}
	sco_state_disconnect(sco);
}

static int sco_connect_audio(LPSCO sco)
{
	struct sockaddr_sco addr;
	// Start async connection to channel
	sco->sk_sco = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO);
	(void)fcntl(sco->sk_sco, F_SETFL, O_NONBLOCK);

	memset(&addr, 0, sizeof(addr));
	addr.sco_family = AF_BLUETOOTH;
	bacpy(&addr.sco_bdaddr, &sco->dst);

	if((connect(sco->sk_sco, (struct sockaddr *)&addr, (int)sizeof(addr)) < 0)  && ((errno == EAGAIN) || (errno == EINPROGRESS))) {
		sco_set_state(SCO_STATE_SCO_CONNECTING);
	}
	return 0;
}

static int sco_handle_sco_stream(LPSCO sco, char* lpFrame, size_t iSize, ssize_t* received)
{
	if(poll_accept(sco->sk_sco, 0)) {
		*received = recv(sco->sk_sco, lpFrame, iSize, MSG_NOSIGNAL);
		if(*received>=0) {
			bw_in += *received;

			// DBG("Received %d bytes", received);
		} else {
			DBG("Received Failed");
		}
	}
	return 0;
}

static int sco_handle_rfcomm_commands(LPSCO sco)
{
	char lpFrame[512];
	if(poll_accept(sco->sk_rfc, 0)) {
		ssize_t received = recv(sco->sk_rfc, lpFrame, sizeof(lpFrame), MSG_NOSIGNAL);
		if(received>=0) {
			lpFrame[received] = '\0';
			DBG("Received %s", lpFrame);
			if(strncmp(lpFrame, "AT+CLIP=1", 9)==0) {
				(void)send_socket(sco->sk_rfc, "\r\nOK\r\n", 6);
			} else if(strncmp(lpFrame, "AT+VGS", 6)==0) {
				(void)send_socket(sco->sk_rfc, "\r\nOK\r\n", 6);
			} else if(strncmp(lpFrame, "AT+CKPD", 7)==0) {
				(void)send_socket(sco->sk_rfc, "\r\nOK\r\n", 6);
			} else {
				(void)send_socket(sco->sk_rfc, "\r\nERROR\r\n", 9);
			}
		} else {
			DBG("Received Failed");
		}
	}
	return 0;
}

int sco_state_machine(LPSCO sco, char* lpFrame, size_t iSize, ssize_t* received)
{
	struct pollfd pollfds[1];
	int errcode = 0;
	size_t opt_size = sizeof(errcode);
	uint8_t channel = 0;

	switch(sco->state) {
		case SCO_STATE_DISCONNECTED:
			break;
		case SCO_STATE_DISCONNECTING:
			sco_disconnect(sco);
			break;
		case SCO_STATEX_SDP_CONNECTING:
			if((sco->bdaddr[0] != '\0') && (sco->sdp_session==NULL))
			{
				sco->sdp_session = sdp_connect_async(&sco->dst);

				if(sco->sdp_session != NULL) {
					sco->sk_sdp = sdp_get_socket(sco->sdp_session);
					sco_set_state(SCO_STATEX_SDP_CONNECTING_WAIT);
				} else {
					sco_set_state(SCO_STATE_DISCONNECTING);
				}
			} else {
				sco_set_state(SCO_STATE_DISCONNECTING);
			}
			break;
		case SCO_STATEX_SDP_CONNECTING_WAIT:
			// If SDP connection is established
			memset(pollfds, 0, sizeof(pollfds));
			pollfds[0].fd = sco->sk_sdp;
			pollfds[0].events = POLLOUT | POLLERR | POLLHUP;
			pollfds[0].revents = 0;
			if(poll(pollfds, 1, 0)>0) {

				DBG("SDP connection terminated");

				if((getsockopt(sco->sk_sdp, SOL_SOCKET, SO_ERROR, &errcode, &opt_size) == 0)
				&& (errcode == 0)) {
					sco_set_state(SCO_STATE_RFC_CONNECTING);
				} else {
					errno = errcode;
					DBG("SDP connection failed");
					sco_set_state(SCO_STATE_DISCONNECTING);
				}
			}
			break;
		case SCO_STATE_RFC_CONNECTING:
			channel = get_rfcomm_channel(sco);
			DBG("Found channel %d", (int)channel);
			// Free sdp data
			if(sco->sdp_session)
				(void)sdp_close(sco->sdp_session);
			sco->sdp_session = NULL;
			// Retrieve channel
			if(channel>0) {
				struct sockaddr_rc addr;
				// Start async connection to channel
				sco->sk_rfc = socket(PF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
				DBG("Socket is %d", sco->sk_rfc);

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

				memset(&addr, 0, sizeof(addr));
				addr.rc_family = AF_BLUETOOTH;
				bacpy(&addr.rc_bdaddr, &sco->dst);
				addr.rc_channel = channel;

				if((connect(sco->sk_rfc, (struct sockaddr *)&addr, (int)sizeof(addr)) < 0) && ((errno == EAGAIN) || (errno == EINPROGRESS))) {
					sco_set_state(SCO_STATE_RFC_CONNECTING_WAIT);
				} else {
					DBG("Connection failed");
					sco_set_state(SCO_STATE_DISCONNECTING);
				}
			} else {
				sco_set_state(SCO_STATE_DISCONNECTING);
			}
			break;
		case SCO_STATE_RFC_CONNECTING_WAIT:
			// Poll rfc socket
			memset(pollfds, 0, sizeof(pollfds));
			pollfds[0].fd = sco->sk_rfc;
			pollfds[0].events = POLLOUT | POLLERR | POLLHUP;
			pollfds[0].revents = 0;
			if(poll(pollfds, 1, 0)>0) {
				if((getsockopt(sco->sk_rfc, SOL_SOCKET, SO_ERROR, &errcode, &opt_size) == 0)
				&& (errcode == 0)) {
					sco_set_state(SCO_STATE_CONNECTED);
				} else {
					errno = errcode;
					DBG("RFC connection failed");
					sco_set_state(SCO_STATE_DISCONNECTING);
				}
			}
			break;
		case SCO_STATE_CONNECTED:
			// Start accepting SCO connections?
			(void)sco_connect_audio(sco);
			(void)sco_handle_rfcomm_commands(sco);
			break;
		case SCO_STATE_SCO_CONNECTING:
			memset(pollfds, 0, sizeof(pollfds));
			pollfds[0].fd = sco->sk_sco;
			// Poll rfc socket
			pollfds[0].events = POLLOUT | POLLERR | POLLHUP;
			pollfds[0].revents = 0;
			if(poll(pollfds, 1, 0)>0) {
				if((getsockopt(sco->sk_sco, SOL_SOCKET, SO_ERROR, &errcode, &opt_size) == 0)
				&& (errcode == 0)) {
					sco_set_state(SCO_STATE_SCO_CONNECTED);
				} else {
					errno = errcode;
					DBG("SCO connection failed");
					sco_set_state(SCO_STATE_CONNECTED);
				}
			}
			(void)sco_handle_rfcomm_commands(sco);
			break;
		case SCO_STATE_SCO_CONNECTED:
			(void)sco_handle_sco_stream(sco, lpFrame, iSize, received);
			(void)sco_handle_rfcomm_commands(sco);
			break;
		default:
			DBG("Unhandled state %d", sco->state);
			break;
	}

	return 0;
}