a2dpd_output_a2dp.c

linux蓝牙剖面实现

					if(psm>0) {
						a2dp->psm_cmd = psm;
						a2dp_set_state(AVDTP_STATEX_CTL_CONNECTING);
					} else {
						a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
					}
				} else {
					errno = errcode;
					DBG("SDP connection failed");
					a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
				}
			}
			break;
		case AVDTP_STATEX_CTL_CONNECTING:
			// Start async connection to channel
			a2dp->control_sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
			DBG("Socket is %d", a2dp->control_sk);

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

			memset(&addr, 0, sizeof(addr));
			addr.l2_family = AF_BLUETOOTH;
			bacpy(&addr.l2_bdaddr, &a2dp->dst);
			addr.l2_psm = htobs(a2dp->psm_cmd);

			if((connect(a2dp->control_sk, (struct sockaddr *)&addr, sizeof(addr)) < 0) && ((errno == EAGAIN) || (errno == EINPROGRESS))) {
				a2dp_set_state(AVDTP_STATEX_CTL_CONNECTING_WAIT);
			} else {
				DBG("Connection failed");
				a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
			}
			break;
		case AVDTP_STATEX_CTL_CONNECTING_WAIT:
			// Poll ctl socket
			pollfds[0].fd = a2dp->control_sk;
			pollfds[0].events = POLLOUT | POLLERR | POLLHUP;
			pollfds[0].revents = 0;
			if(poll(pollfds, 1, 0)>0) {
				if((getsockopt(a2dp->control_sk, SOL_SOCKET, SO_ERROR, &errcode, &opt_size) == 0)
				&& (errcode == 0)) {
					a2dp_set_state(AVDTP_STATE_IDLE);
				} else {
					errno = errcode;
					DBG("CTL connection failed");
					a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
				}
			}
			break;
		case AVDTP_STATE_IDLE:
			if(a2dp->role == A2DPD_INITIATOR) {
				a2dp_set_state(AVDTP_STATEX_DISCOVERING);
			} else if(a2dp->role == A2DPD_ACCEPTOR) {
				a2dp_handle_avdtp_message(a2dp, a2dp->control_sk, NULL, NULL, 0);
			} else {
				a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
			}
			break;
		case AVDTP_STATEX_DISCOVERING:
			// Start configuration
			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)) {
				a2dp->state_info.answer_count = 0;
				a2dp_set_state(AVDTP_STATEX_DISCOVERING_RESP);
			} else {
				DBG("Discovery failed %d (socket = %d)", errno, a2dp->control_sk);
				a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
			}
			break;
		case AVDTP_STATEX_DISCOVERING_RESP:
			if(a2dp->state_info.answer_count<MAXANSWERCOUNT) {
				memset(&discover_req, 0, sizeof(discover_req));
				init_request(&discover_req.header, AVDTP_DISCOVER);
				memset(&a2dp->state_info.discover.resp, 0, sizeof(a2dp->state_info.discover.resp));
				size = a2dp_handle_avdtp_message(a2dp, a2dp->control_sk, &discover_req.header, &a2dp->state_info.discover.resp.header, sizeof(a2dp->state_info.discover.resp));
				if (size > 0) {
					DBG("AVDTP_DISCOVER success");
					a2dp->state_info.discover.curr_seid = 0;
					a2dp->state_info.discover.nb_seid = (size - sizeof(a2dp->state_info.discover.resp.header)) / sizeof(struct acp_seid_info);
					a2dp_set_state(AVDTP_STATEX_GETTING_CAPABILITIES);
				} else {
					//DBG("AVDTP_DISCOVER got bad answer");
					a2dp->state_info.answer_count++;
				}
			} else {
				DBG("Too many answer for AVDTP_DISCOVER");
				a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
			}
			break;
		case AVDTP_STATEX_GETTING_CAPABILITIES:
			if(a2dp->state_info.discover.curr_seid<a2dp->state_info.discover.nb_seid) {
				memset(&cap_req, 0, sizeof(cap_req));
				init_request(&cap_req.header, AVDTP_GET_CAPABILITIES);
				cap_req.acp_seid = a2dp->state_info.discover.resp.infos[a2dp->state_info.discover.curr_seid].acp_seid;
				a2dp->state_info.discover.curr_seid_acp_seid = cap_req.acp_seid;
				if (write(a2dp->control_sk, &cap_req, sizeof(cap_req)) == sizeof(cap_req)) {
					DBG("Getting capabilities for SEID=%d", cap_req.acp_seid);
					a2dp->state_info.answer_count = 0;
					a2dp_set_state(AVDTP_STATEX_GETTING_CAPABILITIES_RESP);
				} else {
					DBG("AVDTP_GET_CAPABILITIES failed");
					a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
				}
			} else {
				DBG("No more seid to browse");
				a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
			}
			break;
		case AVDTP_STATEX_GETTING_CAPABILITIES_RESP:
			if(a2dp->state_info.answer_count<MAXANSWERCOUNT) {
				memset(&cap_req, 0, sizeof(cap_req));
				init_request(&cap_req.header, AVDTP_GET_CAPABILITIES);
				memset(&a2dp->state_info.discover.cap_resp, 0, sizeof(a2dp->state_info.discover.cap_resp));
				size = a2dp_handle_avdtp_message(a2dp, a2dp->control_sk, &cap_req.header, (void*)&a2dp->state_info.discover.cap_resp, sizeof(a2dp->state_info.discover.cap_resp));
				if (size > 0) {
					DBG("AVDTP_GET_CAPABILITIES success for seid %d",
						a2dp->state_info.discover.curr_seid_acp_seid);
					DBG("servcap_cap=%d, servcap_len=%d,",
						a2dp->state_info.discover.cap_resp.serv_cap,
						a2dp->state_info.discover.cap_resp.serv_cap_len);
					DBG("cap_type=%d, length=%d", 
						a2dp->state_info.discover.cap_resp.cap_type,
						a2dp->state_info.discover.cap_resp.length);
					DBG("media_type=%d, codec=%d",
						a2dp->state_info.discover.cap_resp.media_type,
						a2dp->state_info.discover.cap_resp.media_codec_type);
					
					//FIXME Check SEID is audio & SBC
					// Else next seid

					a2dp_set_state(AVDTP_STATEX_SETTING_CONFIGURATION);
				} else {
					//DBG("AVDTP_GET_CAPABILITIES got bad answer");
					a2dp->state_info.answer_count++;
				}
			} else {
				DBG("Too many answer for AVDTP_GET_CAPABILITIES");
				a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
			}
			break;
		case AVDTP_STATEX_SETTING_CONFIGURATION:
			memset(&cfg_req, 0, sizeof(cfg_req));
			init_request(&cfg_req.header, AVDTP_SET_CONFIGURATION);
			cfg_req.serv_cap = MEDIA_TRANSPORT_CATEGORY;
			cfg_req.acp_seid = a2dp->state_info.discover.curr_seid_acp_seid;
			cfg_req.int_seid = A2DPD_SEID;
			cfg_req.cap_type = MEDIA_CODEC;
			cfg_req.length = 6;
			cfg_req.media_type = AUDIO_MEDIA_TYPE;
			cfg_req.media_codec_type = SBC_MEDIA_CODEC_TYPE;

			switch (a2dp->sbc.channels) {
			case 1:
				v = 8;
				break;
			case 2:
			default:
				v = 2;
				break;
			}
			cfg_req.codec_elements.sbc_elements.channel_mode = v;
		
			switch (a2dp->sbc.rate) {
			case 16000:
				v = 8;
				break;
			case 32000:
				v = 4;
				break;
			case 48000:
				v = 1;
				break;
			case 44100:
			default:
				v = 2;
				break;
			}
			cfg_req.codec_elements.sbc_elements.frequency = v;
			cfg_req.codec_elements.sbc_elements.allocation_method = 1 << 1;
		
			switch (a2dp->sbc.subbands) {
			case 4:
				v = 2;
				break;
			case 8:
			default:
				v = 1;
				break;
			}
			cfg_req.codec_elements.sbc_elements.subbands = v;
		
			switch (a2dp->sbc.blocks) {
			case 4:
				v = 8;
				break;
			case 8:
				v = 4;
				break;
			case 12:
				v = 2;
				break;
			case 16:
			default:
				v = 1;
				break;
			}
			cfg_req.codec_elements.sbc_elements.block_length = v;
			cfg_req.codec_elements.sbc_elements.min_bitpool = a2dp->state_info.discover.cap_resp.codec_elements.sbc_elements.min_bitpool;
			cfg_req.codec_elements.sbc_elements.max_bitpool = a2dp->state_info.discover.cap_resp.codec_elements.sbc_elements.max_bitpool;
		
			if (!(a2dp->state_info.discover.cap_resp.codec_elements.sbc_elements.channel_mode & cfg_req.codec_elements.sbc_elements.channel_mode)) {
				DBG("headset does not support this channel mode");
			}
		
			if (!(a2dp->state_info.discover.cap_resp.codec_elements.sbc_elements.frequency & cfg_req.codec_elements.sbc_elements.frequency)) {
				DBG("headset does not support this frequency");
			}
		
			if (!(a2dp->state_info.discover.cap_resp.codec_elements.sbc_elements.allocation_method & cfg_req.codec_elements.sbc_elements.allocation_method)) {
				DBG("headset does not support this allocation_method");
			}
		
			if (!(a2dp->state_info.discover.cap_resp.codec_elements.sbc_elements.subbands & cfg_req.codec_elements.sbc_elements.subbands)) {
				DBG("headset does not support this subbands setting");
			}
			if(write(a2dp->control_sk, &cfg_req, sizeof(cfg_req)) == sizeof(cfg_req)) {
				a2dp->state_info.answer_count = 0;
				a2dp_set_state(AVDTP_STATEX_SETTING_CONFIGURATION_RESP);
			} else {
				a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
			}
			break;
		case AVDTP_STATEX_SETTING_CONFIGURATION_RESP:
			if(a2dp->state_info.answer_count<MAXANSWERCOUNT) {
				memset(&cfg_req, 0, sizeof(cfg_req));
				init_request(&cfg_req.header, AVDTP_SET_CONFIGURATION);
				memset(&cfg_resp, 0, sizeof(cfg_resp));
				size = a2dp_handle_avdtp_message(a2dp, a2dp->control_sk, &cfg_req.header, (void*)&cfg_resp, sizeof(cfg_resp));
				if (size > 0) {
					DBG("AVDTP_SET_CONFIGURATION success");
					a2dp->mtu = A2DPMAXIMUMTRANSFERUNITSIZE;
					a2dp->seid = a2dp->state_info.discover.curr_seid_acp_seid;
					a2dp->psm_stream = 25;
					a2dp_set_state(AVDTP_STATEX_STREAM_OPENING);
				} else {
					//DBG("AVDTP_SET_CONFIGURATION got bad answer");
					a2dp->state_info.answer_count++;
				}
			} else {
				DBG("Too many answer for AVDTP_SET_CONFIGURATION");
				a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
			}
			break;
		case AVDTP_STATEX_STREAM_OPENING:
			memset(&open_req, 0, sizeof(open_req));
			init_request(&open_req.header, AVDTP_OPEN);
			open_req.acp_seid = a2dp->seid;

			if (write(a2dp->control_sk, &open_req, sizeof(open_req)) == sizeof(open_req)) {
				a2dp->state_info.answer_count = 0;
				a2dp_set_state(AVDTP_STATEX_STREAM_OPENING_RESP);
			} else {
				a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
			}
			break;
		case AVDTP_STATEX_STREAM_OPENING_RESP:
			if(a2dp->state_info.answer_count<MAXANSWERCOUNT) {
				memset(&open_req, 0, sizeof(open_req));
				init_request(&open_req.header, AVDTP_OPEN);
				memset(&open_resp, 0, sizeof(open_resp));
				size = a2dp_handle_avdtp_message(a2dp, a2dp->control_sk, &open_req.header, (void*)&open_resp, sizeof(open_resp));
				if (size > 0) {
					DBG("AVDTP_OPEN success");
					a2dp_set_state(AVDTP_STATE_CONFIGURED);
				} else {
					//DBG("AVDTP_OPEN got bad answer");
					a2dp->state_info.answer_count++;
				}
			} else {
				DBG("Too many answer for AVDTP_OPEN");
				a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
			}
			break;
		case AVDTP_STATE_CONFIGURED:
			a2dp_set_state(AVDTP_STATEX_STREAM_CONNECTING);
			break;
		case AVDTP_STATEX_STREAM_CONNECTING:
			// Start async connection to channel
			a2dp->sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
			DBG("Socket is %d", a2dp->sk);

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

			memset(&addr, 0, sizeof(addr));
			addr.l2_family = AF_BLUETOOTH;
			bacpy(&addr.l2_bdaddr, &a2dp->dst);
			addr.l2_psm = htobs(a2dp->psm_stream);

			if((connect(a2dp->sk, (struct sockaddr *)&addr, sizeof(addr)) < 0) && ((errno == EAGAIN) || (errno == EINPROGRESS))) {
				a2dp_set_state(AVDTP_STATEX_STREAM_CONNECTING_WAIT);
			} else {
				DBG("Connection failed");
				a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
			}
			break;
		case AVDTP_STATEX_STREAM_CONNECTING_WAIT:
			// Poll ctl socket
			pollfds[0].fd = a2dp->sk;
			pollfds[0].events = POLLOUT | POLLERR | POLLHUP;
			pollfds[0].revents = 0;
			if(poll(pollfds, 1, 0)>0) {
				if((getsockopt(a2dp->sk, SOL_SOCKET, SO_ERROR, &errcode, &opt_size) == 0)
				&& (errcode == 0)) {
					DBG("Stream connected");
					a2dp->mtu = get_socket_omtu(a2dp->sk);
					a2dp_set_state(AVDTP_STATE_OPEN_START_STREAMING);
				} else {
					errno = errcode;
					DBG("STREAM connection failed");
					a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
				}
			}
			break;
		case AVDTP_STATE_OPEN_START_STREAMING:
			a2dp_set_state(AVDTP_STATEX_STREAM_STARTING);
			break;
		case AVDTP_STATE_OPEN:
			a2dp_handle_avdtp_message(a2dp, a2dp->control_sk, NULL, NULL, 0);
			break;
		case AVDTP_STATEX_STREAM_STARTING:
			memset(&start_req, 0, sizeof(start_req));
			init_request(&start_req.header, AVDTP_START);
			start_req.acp_seid = a2dp->seid;

			if (write(a2dp->control_sk, &start_req, sizeof(start_req)) == sizeof(start_req)) {
				a2dp->state_info.answer_count = 0;
				a2dp_set_state(AVDTP_STATEX_STREAM_STARTING_WAIT);
			} else {
				a2dp_set_state(AVDTP_STATEX_STREAM_CLOSING);
			}
			break;
		case AVDTP_STATEX_STREAM_STARTING_WAIT:
			if(a2dp->state_info.answer_count<MAXANSWERCOUNT) {
				memset(&start_req, 0, sizeof(start_req));
				init_request(&start_req.header, AVDTP_START);
				memset(&start_resp, 0, sizeof(start_resp));
				size = a2dp_handle_avdtp_message(a2dp, a2dp->control_sk, &start_req.header, (void*)&start_resp, sizeof(start_resp));
				if (size > 0) {
					DBG("AVDTP_START success");
					a2dp->pause_writing = 0;
					a2dp_set_state(AVDTP_STATE_STREAMING);
				} else {
					a2dp->state_info.answer_count++;
				}
			} else {
				DBG("Too many answer for AVDTP_START");
				a2dp_set_state(AVDTP_STATEX_STREAM_CLOSING);
			}
			break;
		case AVDTP_STATE_STREAMING:
			a2dp_handle_avdtp_message(a2dp, a2dp->control_sk, &cfg_req.header, (void*)&cfg_resp, sizeof(cfg_resp));
			break;
		case AVDTP_STATEX_STREAM_SUSPENDING:
			memset(&suspend_req, 0, sizeof(suspend_req));
			init_request(&suspend_req.header, AVDTP_SUSPEND);
			suspend_req.acp_seid = a2dp->seid;

			if (write(a2dp->control_sk, &suspend_req, sizeof(suspend_req)) == sizeof(suspend_req)) {
				a2dp->state_info.answer_count = 0;
				a2dp->pause_writing = 0;
				a2dp_set_state(AVDTP_STATEX_STREAM_SUSPENDING_WAIT);
			} else {
				a2dp_set_state(AVDTP_STATEX_STREAM_CLOSING);
			}
			break;
		case AVDTP_STATEX_STREAM_SUSPENDING_WAIT:
			if(a2dp->state_info.answer_count<MAXANSWERCOUNT) {
				memset(&suspend_req, 0, sizeof(suspend_req));
				init_request(&suspend_req.header, AVDTP_SUSPEND);
				memset(&suspend_resp, 0, sizeof(suspend_resp));
				size = a2dp_handle_avdtp_message(a2dp, a2dp->control_sk, &suspend_req.header, (void*)&suspend_resp, sizeof(suspend_resp));
				if (size > 0) {
					DBG("AVDTP_SUSPEND success");
					a2dp_set_state(AVDTP_STATE_OPEN);
				} else {
					a2dp->state_info.answer_count++;
				}
			} else {
				DBG("Too many answer for AVDTP_SUSPEND");
				a2dp_set_state(AVDTP_STATEX_STREAM_CLOSING);
			}
			break;
		case AVDTP_STATE_CLOSING:
		case AVDTP_STATEX_STREAM_CLOSING:
			memset(&close_req, 0, sizeof(close_req));
			init_request(&close_req.header, AVDTP_CLOSE);
			close_req.acp_seid = a2dp->seid;

			if (write(a2dp->control_sk, &close_req, sizeof(close_req)) == (ssize_t)sizeof(close_req)) {
				a2dp->state_info.answer_count = 0;
				a2dp->pause_writing = 1;
				a2dp_set_state(AVDTP_STATEX_STREAM_CLOSING_WAIT);
			} else {
				a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
			}
			break;
		case AVDTP_STATEX_STREAM_CLOSING_WAIT:
			if(a2dp->state_info.answer_count<MAXANSWERCOUNT) {
				memset(&close_req, 0, sizeof(close_req));
				init_request(&close_req.header, AVDTP_CLOSE);
				memset(&close_resp, 0, sizeof(close_resp));
				size = a2dp_handle_avdtp_message(a2dp, a2dp->control_sk, &close_req.header, (void*)&close_resp, sizeof(close_resp));
				if (size > 0) {
					DBG("AVDTP_CLOSE success");
					a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
				} else {
					a2dp->state_info.answer_count++;
				}
			} else {
				DBG("Too many answer for AVDTP_CLOSE");
				a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
			}
			break;
		case AVDTP_STATE_ABORTING:
			a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
			break;
		default:
			DBG("Unhandled state %d", a2dp->state);
			break;
	}
	return 0;
}