a2dpd_output_a2dp.c

linux蓝牙剖面实现

		memset(lpFrame+sizeof(*pkt_hdr), 0, ans_size-sizeof(*pkt_hdr));
		accepted = 1;

		//Fill in the values of the structure
		discover_resp->infos[0].inuse0 = (a2dp&&a2dp_is_connected(a2dp))?1:0;
		discover_resp->infos[0].acp_seid = A2DPD_SEID;
		discover_resp->infos[0].media_type = AUDIO_MEDIA_TYPE;
		discover_resp->infos[0].tsep = 0; // 0 source / 1 sink

	} else if (pkt_hdr->signal_id == AVDTP_GET_CAPABILITIES) {
		struct getcap_resp* cap_resp = (struct getcap_resp*)lpFrame;
		DBG("Received signal AVDTP_GET_CAPABILITIES(%d) from set", pkt_hdr->signal_id);
		ans_size = sizeof(*cap_resp);
		memset(lpFrame+sizeof(*pkt_hdr), 0, ans_size-sizeof(*pkt_hdr));
		accepted = 1;

		//Fill in the values of the structure
		cap_resp->serv_cap=MEDIA_TRANSPORT_CATEGORY;
		cap_resp->serv_cap_len=0;
		cap_resp->cap_type=MEDIA_CODEC;
		cap_resp->media_type=AUDIO_MEDIA_TYPE;
		cap_resp->length=6;//sizeof(cap_resp->codec_elements.sbc_elements);
		cap_resp->media_codec_type=SBC_MEDIA_CODEC_TYPE;
		cap_resp->codec_elements.sbc_elements.channel_mode=(CHANNEL_MODE_MONO|/*CHANNEL_MODE_DUAL|*/CHANNEL_MODE_STEREO/*|CHANNEL_MODE_JOINT*/);
		cap_resp->codec_elements.sbc_elements.frequency=(FREQUENCY_44100);
		cap_resp->codec_elements.sbc_elements.allocation_method=(ALLOCATION_LOUDNESS/*|ALLOCATION_SNR*/);
		cap_resp->codec_elements.sbc_elements.subbands=(/*SUBBANDS_4|*/SUBBANDS_8);  
		cap_resp->codec_elements.sbc_elements.min_bitpool=2;
		cap_resp->codec_elements.sbc_elements.max_bitpool=250;
		cap_resp->codec_elements.sbc_elements.block_length=(/*BLOCK_LENGTH_4|BLOCK_LENGTH_8|BLOCK_LENGTH_12|*/BLOCK_LENGTH_16);

	} else if (pkt_hdr->signal_id == AVDTP_SET_CONFIGURATION) {
		struct set_config* cfg_req = (struct set_config*)lpFrame;
		struct set_config_resp* cfg_resp = (struct set_config_resp*)lpFrame;
		struct sbc_codec_specific_elements* sbc_elements = 0;
		int cfgok = 1;
		int v = 0;

		DBG("Received signal AVDTP_SET_CONFIGURATION(%d) from set", pkt_hdr->signal_id);

		if(a2dp) {
			// Check we support the settings proposed
			//cfgok = cfgok && (cfg_req->acp_seid == A2DPD_SEID);
			DBG("acp_seid: %d => %s", cfg_req->acp_seid, cfgok?"supported":"unsupported");
	
			cfgok = cfgok && (cfg_req->media_type == AUDIO_MEDIA_TYPE);
			DBG("media_type: %d => %s", cfgok, cfgok?"supported":"unsupported");
	
			cfgok = cfgok && (cfg_req->media_codec_type == SBC_MEDIA_CODEC_TYPE);
			DBG("media_codec_type: %d => %s", cfg_req->media_codec_type, cfgok?"supported":"unsupported");
	
			sbc_elements = &cfg_req->codec_elements.sbc_elements;
	
			// channel mode
			if(sbc_elements->channel_mode&CHANNEL_MODE_MONO) {
				if(a2dp) {
					a2dp->sbc.channels = 1;
					a2dp->sbc.joint = 0;
				}
			} else if(sbc_elements->channel_mode&CHANNEL_MODE_DUAL) {
				// Don't know how to set sbc
			} else if(sbc_elements->channel_mode&CHANNEL_MODE_STEREO) {
				if(a2dp) {
					a2dp->sbc.channels = 2;
					a2dp->sbc.joint = 0;
				}
			} else if(sbc_elements->channel_mode&CHANNEL_MODE_JOINT) {
				if(a2dp) {
					a2dp->sbc.channels = 2;
					a2dp->sbc.joint = 1;
				}
			} else {
				cfgok = 0;
			}
			DBG("channel_mode: %d, joint: %d => %s", a2dp->sbc.channels, a2dp->sbc.joint, cfgok?"supported":"unsupported");
	
			if(sbc_elements->frequency & FREQUENCY_16000) {
				v = 16000;
				if(a2dp->sbc.rate != 16000) {
					cfgok = 0;
				}
			} else if(sbc_elements->frequency & FREQUENCY_32000) {
				v = 32000;
				if(a2dp->sbc.rate != 32000) {
					cfgok = 0;
				}
			} else if(sbc_elements->frequency & FREQUENCY_44100) {
				v = 44100;
				if(a2dp->sbc.rate != 44100) {
					cfgok = 0;
				}
			} else if(sbc_elements->frequency & FREQUENCY_48000) {
				v = 48000;
				if(a2dp->sbc.rate != 48000) {
					cfgok = 0;
				}
			} else {
				v = -1;
				cfgok = 0;
			}
			DBG("frequency: %d => %s", v, cfgok?"supported":"unsupported");
	
			if(sbc_elements->allocation_method&ALLOCATION_SNR) {
				// SBC support SNR?
			} else if(sbc_elements->allocation_method&ALLOCATION_LOUDNESS) {
				// Mandatory
				cfgok = 1;
			} else {
				cfgok = 0;
			}
			DBG("allocation_method: %d => %s", sbc_elements->allocation_method, cfgok?"supported":"unsupported");
	
			if(sbc_elements->allocation_method&SUBBANDS_4) {
				a2dp->sbc.subbands = 4;
			} else if(sbc_elements->allocation_method&SUBBANDS_8) {
				a2dp->sbc.subbands = 8;
			} else {
				cfgok = 0;
			}
			DBG("subbands: %d => %s", sbc_elements->subbands, cfgok?"supported":"unsupported");
	
			if(sbc_elements->allocation_method&BLOCK_LENGTH_4) {
				a2dp->sbc.blocks = 4;
			} else if(sbc_elements->allocation_method&BLOCK_LENGTH_8) {
				a2dp->sbc.blocks = 8;
			} else if(sbc_elements->allocation_method&BLOCK_LENGTH_12) {
				a2dp->sbc.blocks = 12;
			}  else if(sbc_elements->allocation_method&BLOCK_LENGTH_16) {
				a2dp->sbc.blocks = 16;
			}  else {
				cfgok = 0;
			}
			DBG("block_length: %d => %s", sbc_elements->block_length, cfgok?"supported":"unsupported");

			// PTS sends min and max to the same value so use what is wanted
			if(sbc_elements->min_bitpool == sbc_elements->max_bitpool) {
				if((a2dp->sbc.bitpool < 2) || (a2dp->sbc.bitpool > 250)) {
					DBG("Invalid bitpool %d wanted", sbc_elements->min_bitpool);
					cfgok = 0;
				} else {
					a2dp->sbc.bitpool = sbc_elements->min_bitpool;
				}
			} else {
				// This dumb of HBH DS 970 send 2 as min and 250 as max so use ours
				// We just check ours is within the supported range
				if((a2dp->sbc.bitpool < sbc_elements->min_bitpool) || (a2dp->sbc.bitpool > sbc_elements->max_bitpool)) {
					DBG("bitpool %d not supported", a2dp->sbc.bitpool);
					cfgok = 0;
				}
			}
			DBG("min_bitpool: %d => %s", sbc_elements->min_bitpool, cfgok?"supported":"unsupported");
			DBG("max_bitpool: %d => %s", sbc_elements->max_bitpool, cfgok?"supported":"unsupported");
		}
		// If config is OK then we reply
		ans_size = sizeof(*cfg_resp);
		memset(lpFrame+sizeof(*pkt_hdr), 0, ans_size-sizeof(*pkt_hdr));
		accepted = cfgok;
		cfg_resp->serv_cat = MEDIA_TRANSPORT_CATEGORY;
		cfg_resp->error_code = 0;

	} else if (pkt_hdr->signal_id == AVDTP_GET_CONFIGURATION) {
		DBG("Received signal AVDTP_GET_CONFIGURATION(%d) from set", pkt_hdr->signal_id);
		// Not implemented
		//accepted = 1;
		
	} else if (pkt_hdr->signal_id == AVDTP_RECONFIGURE) {
		DBG("Received signal AVDTP_RECONFIGURE(%d) from set", pkt_hdr->signal_id);
		// Not implemented
		//accepted = 1;
	
	} else if (pkt_hdr->signal_id == AVDTP_OPEN) {
		struct open_stream_rsp* open_resp = (struct open_stream_rsp*)lpFrame;
		DBG("Received signal AVDTP_OPEN(%d) from set", pkt_hdr->signal_id);
		ans_size = sizeof(*open_resp);
		memset(lpFrame+sizeof(*pkt_hdr), 0, ans_size-sizeof(*pkt_hdr));
		accepted = 1;

		//Fill in the values of the structure
		open_resp->error = 0;

	} else if (pkt_hdr->signal_id == AVDTP_START) {
		DBG("Received signal AVDTP_START(%d) from set", pkt_hdr->signal_id);

		if(a2dp) {
			if(a2dp->state == AVDTP_STATE_OPEN) {
				a2dp->pause_writing = 0;
				a2dp_set_state(AVDTP_STATE_STREAMING);
				accepted = 1;
			} else {
				DBG("Filtering state : AVDTP_START received while not in AVDTP_STATE_OPEN");
			}
		}

	} else if (pkt_hdr->signal_id == AVDTP_CLOSE) {
		DBG("Received signal AVDTP_CLOSE(%d) from set", pkt_hdr->signal_id);
		if(a2dp) {
			a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
			accepted = 1;
		}

	} else if (pkt_hdr->signal_id == AVDTP_SUSPEND) {
		DBG("Received signal AVDTP_SUSPEND(%d) from set", pkt_hdr->signal_id);
		if(a2dp) {
			if(a2dp->state == AVDTP_STATE_STREAMING) {
				a2dp->pause_writing = 1;
				accepted = 1;
				a2dp_set_state(AVDTP_STATE_OPEN);
			} else {
				DBG("Filtering state : AVDTP_SUSPEND received while not in AVDTP_STATE_STREAMING");
			}
		}

	} else if (pkt_hdr->signal_id == AVDTP_ABORT) {
		DBG("Received signal AVDTP_ABORT(%d) from set", pkt_hdr->signal_id);
		if(a2dp) {
			if(a2dp->role == A2DPD_INITIATOR) {
				a2dp_set_state(AVDTP_STATE_IDLE);
			}
			if(a2dp->role == A2DPD_INITIATOR) {
				a2dp_set_state(AVDTP_STATE_IDLE);
			}
			accepted = 1;
		}
	} else if (pkt_hdr->signal_id == AVDTP_SECURITY_CONTROL) {
		DBG("Received signal AVDTP_SECURITY_CONTROL(%d) from set", pkt_hdr->signal_id);
		// Not implemented
		//accepted = 1;
	}  else {
		DBG("Unexpected headset directive %d", pkt_hdr->signal_id);
		//accepted = 1;
	}

	pkt_hdr->message_type = accepted ? MESSAGE_TYPE_ACCEPT : MESSAGE_TYPE_REJECT;
	DBG("Answering command packet (msgtype=%s,signal=%s)", get_msgtype(pkt_hdr->message_type), get_signal(pkt_hdr->signal_id));

	// Reply to command received
	wrresult = write(sockfd, pkt_hdr, ans_size);

	if (wrresult != ans_size) {
		DBG("FAILED Answering command packet (msgtype=%s,signal=%s) wrresult=%d/%d", get_msgtype(pkt_hdr->message_type), get_signal(pkt_hdr->signal_id),
		wrresult, ans_size);
	}
	return result;
}

// This function handle the bluetooth connection
int a2dp_handle_avdtp_message(LPA2DP a2dp, int sockfd, struct avdtp_header *sent_packet, struct avdtp_header *answer, size_t answer_size)
{
	int result = 0;
	char lpFrame[A2DPMAXIMUMTRANSFERUNITSIZE];

	// Data to read?
	if(poll_accept(sockfd, 0)) {
		ssize_t iReceived = recv(sockfd, lpFrame, sizeof(lpFrame), MSG_WAITALL|MSG_NOSIGNAL);
		struct avdtp_header *pkt_hdr = (struct avdtp_header *) lpFrame;
		if (iReceived > 0) {
			// Manage the packet
			if (sent_packet == NULL) {
				//int i;
				DBG("socket %d: Received %d bytes", sockfd, iReceived);
				// dump_raw(lpFrame, iReceived);
				result = 0;
			} else if ((pkt_hdr->message_type == MESSAGE_TYPE_ACCEPT) && (pkt_hdr->signal_id == sent_packet->signal_id) && answer) {
				// Got expected answer
				memcpy(answer, lpFrame, (ssize_t)answer_size > iReceived ? answer_size : iReceived);
				result = iReceived;
			} else {
				// Got bad answer
				result = 0;
			}
	
			// Handle the packet if the incoming message is a command
			if (pkt_hdr->message_type == MESSAGE_TYPE_COMMAND) {
				a2dp_ctl_handle_commands(a2dp, sockfd, lpFrame, iReceived);
			} else {
				DBG("Read non command packet (msgtype=%s,signal=%s)", get_msgtype(pkt_hdr->message_type), get_signal(pkt_hdr->signal_id));
			}
		} else {
			result = iReceived;
			if (iReceived < 0 && errno != EAGAIN) {
				DBG("socket %d: Receive failed %d", sockfd, iReceived);
				a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
			}
		}
	} else {
		if(errno != EAGAIN) {
			DBG("socket %d: poll_accept returned error", sockfd);
			a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
		}
	}

	return result;
}

int get_avdtp_psm(LPA2DP a2dp)
{
	sdp_list_t *attrid, *search, *seq, *next;
	sdp_data_t *pdlist;
	uuid_t group;
	uint32_t range = 0x0000ffff;
	int err;
	int psm = 25;

	a2dp->flags = 0;

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

	/* 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(a2dp->sdp_session, search, SDP_ATTR_REQ_RANGE, attrid, &seq);
	sdp_list_free(search, 0);
	sdp_list_free(attrid, 0);

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

	if (!seq) {
		DBG("Service Search record empty.");
	} else {
		DBG("Parsing results");
	}

	// Free sdp record
	for (; seq; seq = next) {
		sdp_record_t *rec = (sdp_record_t *) seq->data;
		DBG("Record");
		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(a2dp->sdp_session, search, SDP_ATTR_REQ_RANGE, attrid, &seq);
	sdp_list_free(search, 0);
	sdp_list_free(attrid, 0);

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

	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");

			a2dp->flags |= A2DPD_FLAGS_NONSPECAUDIO;
		}

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

	return psm;
}

// This function handle the bluetooth connection
int a2dp_state_machine(LPA2DP a2dp)
{
	struct pollfd pollfds[1];
	int errcode = 0;
	unsigned int opt_size = sizeof(errcode);
	struct sockaddr_l2 addr;
	struct sepd_req discover_req;
	struct getcap_req cap_req;
	//struct getcap_resp cap_resp;
	struct set_config cfg_req;
	struct set_config_resp cfg_resp;
	struct stream_cmd open_req;
	struct open_stream_rsp open_resp;
	struct stream_cmd start_req;
	struct start_stream_rsp start_resp;
	struct stream_cmd suspend_req;
	struct start_stream_rsp suspend_resp;
	struct stream_cmd close_req;
	struct close_stream_rsp close_resp;

	int v,size;

	switch(a2dp->state) {
		case AVDTP_STATE_DISCONNECTED:
			// Delightly do nothing
			a2dp->role = A2DPD_NOROLE;
			break;
		case AVDTP_STATEX_DISCONNECTING:
			a2dp_disconnect(a2dp);
			break;
		case AVDTP_STATEX_SDP_CONNECTING:
			a2dp->role = A2DPD_INITIATOR;
			DBG("Role is INITIATOR");

			a2dp->sdp_session = sdp_connect_async(&a2dp->dst);

			if(a2dp->sdp_session != NULL) {
				a2dp->sk_sdp = sdp_get_socket(a2dp->sdp_session);
				a2dp_set_state(AVDTP_STATEX_SDP_CONNECTING_WAIT);
			} else  {
				a2dp_set_state(AVDTP_STATEX_DISCONNECTING);
			}
			break;
		case AVDTP_STATEX_SDP_CONNECTING_WAIT:
			// If SDP connection is established
			pollfds[0].fd = a2dp->sk_sdp;
			pollfds[0].events = POLLOUT | POLLERR | POLLHUP;
			pollfds[0].revents = 0;
			if(poll(pollfds, 1, 0)>0) {
				DBG("SDP connection terminated");

				if((getsockopt(a2dp->sk_sdp, SOL_SOCKET, SO_ERROR, &errcode, &opt_size) == 0)
				&& (errcode == 0)) {
					int psm = get_avdtp_psm(a2dp);
					DBG("Found psm %d", (int)psm);
					// Free sdp data
					if(a2dp->sdp_session)
						sdp_close(a2dp->sdp_session);
					a2dp->sdp_session = NULL;
					// Retrieve channel