a2play.c

来自「蓝牙blue tooth sco协议栈」· C语言 代码 · 共 872 行 · 第 1/2 页

	// clear rfa bits
	header->rfa0 = 0;

	transaction = (transaction + 1) & 0xf;
}

static int process_seid(int s, struct acp_seid_info * get_seid_resp, unsigned short *psm)
{
	int seid = get_seid_resp->acp_seid;
	struct getcap_req put_req;
	struct getcap_resp cap_resp;
	struct set_config s_config;
	struct set_config_resp s_resp;
	struct open_stream_cmd open_stream;
	struct open_stream_rsp open_resp;

	printf("seid = %d\n", seid);

	memset(&put_req, 0, sizeof(put_req));
	init_request(&put_req.header, AVDTP_GET_CAPABILITIES);
	put_req.acp_seid = seid;

	if (write(s, &put_req, sizeof(put_req)) != sizeof(put_req)) {
		fprintf(stderr, "couldn't request caps for seid = %d\n", seid);
		return (-1);
	}

	if (read(s, &cap_resp, sizeof(cap_resp)) < sizeof(cap_resp) ||
			cap_resp.header.message_type == MESSAGE_TYPE_REJECT ||
			cap_resp.media_type != AUDIO_MEDIA_TYPE ||
			cap_resp.media_codec_type != SBC_MEDIA_CODEC_TYPE) {
		fprintf(stderr, "didn't receive sbc codec params (first) for seid = %d\n", seid);
		return (-1);
	}

	printf("got capabilities response\n");

	memset(&s_config, 0, sizeof(s_config));
	init_request(&s_config.header, AVDTP_SET_CONFIGURATION);
	s_config.serv_cap = MEDIA_TRANSPORT_CATEGORY;
	s_config.acp_seid = seid;
	s_config.int_seid = 1;	// how should I choose the int_seid??
	s_config.cap_type = MEDIA_CODEC;
	s_config.length = 6;
	s_config.media_type = AUDIO_MEDIA_TYPE;
	s_config.media_codec_type = SBC_MEDIA_CODEC_TYPE;
	s_config.sbc_elements.channel_mode = 8 >> sbc_info.channel_mode;
	s_config.sbc_elements.frequency = 8 >> sbc_info.sampling_frequency;
	s_config.sbc_elements.allocation_method = 1 << sbc_info.allocation_method;
	s_config.sbc_elements.subbands = 2 >> sbc_info.subbands;
	s_config.sbc_elements.block_length = 8 >> sbc_info.blocks;
	s_config.sbc_elements.min_bitpool = cap_resp.sbc_elements.min_bitpool;
	s_config.sbc_elements.max_bitpool = cap_resp.sbc_elements.max_bitpool;

        //dump_packet(&s_config.sbc_elements, sizeof(s_config.sbc_elements)); exit(0);

	if (!(cap_resp.sbc_elements.channel_mode & s_config.sbc_elements.channel_mode))
		printf("headset does not support this channel mode\n");

	if (!(cap_resp.sbc_elements.frequency & s_config.sbc_elements.frequency))
		printf("headset does not support this frequency\n");

	if (!(cap_resp.sbc_elements.allocation_method & s_config.sbc_elements.allocation_method))
		printf("headset does not support this allocation_method\n");

	if (!(cap_resp.sbc_elements.subbands & s_config.sbc_elements.subbands))
		printf("headset does not support this subbands setting\n");

	if (write(s, &s_config, sizeof(s_config)) != sizeof(s_config)) {
		fprintf(stderr, "couldn't set config seid = %d\n", seid);
		return (-1);
	}

	printf("sent set_config\n");

	int size = read(s, &s_resp, sizeof(s_resp));
	if (size == sizeof(s_resp) - 2) {
		printf("set config accepted %d\n", s_resp.header.message_type);
	} else {
		printf("set config rejected\n");
		return (-1);
	}

	memset(&open_stream, 0, sizeof(open_stream));
	init_request(&open_stream.header, AVDTP_OPEN);
	open_stream.acp_seid = seid;

	if (write(s, &open_stream, sizeof(open_stream)) != sizeof(open_stream)) {
		fprintf(stderr, "couldn't open stream seid = %d\n", seid);
		return (-1);
	}

	printf("sent open_stream\n");

	if (read(s, &open_resp, sizeof(open_resp)) < sizeof(open_resp) - 1 ||
			open_resp.header.message_type == MESSAGE_TYPE_REJECT) {
		fprintf(stderr, "didn't receive open_resp confirm for seid = %d\n", seid);
		return (-1);
	}

	printf("got open stream confirm\n");

	*psm = 25;
	return 0;

}

static int calc_frame_len(struct sbc_frame_header *hdr)
{
	int tmp, nrof_subbands, nrof_blocks;

	nrof_subbands = (hdr->subbands + 1) * 4;
	nrof_blocks = (hdr->blocks + 1) * 4;

	switch (hdr->channel_mode) {
	case 0x00:
		nrof_subbands /= 2;
		tmp = nrof_blocks * hdr->bitpool;
		break;
	case 0x01:
		tmp = nrof_blocks * hdr->bitpool * 2;
		break;
	case 0x02:
		tmp = nrof_blocks * hdr->bitpool;
		break;
	case 0x03:
		tmp = nrof_blocks * hdr->bitpool + nrof_subbands;
		break;
	default:
		return 0;
	}

	return (nrof_subbands + ((tmp + 7) / 8));
}

static int read_header(int fd, struct sbc_frame_header *sbc_info) {
	if (read(fd, sbc_info, sizeof(*sbc_info)) < sizeof(*sbc_info)) {
		fprintf(stderr, "reached end of file?\n");
		return -1;
	}

	if (sbc_info->syncword != 0x9c) {
		printf("out of sync (0x%02x)\n", sbc_info->syncword);
		return -1;
	}

	return calc_frame_len(sbc_info);
}

int main(int argc, char *argv[])
{
	struct sigaction sa;
	int streamfd;
	struct sepd_req put_req;
	struct sepd_resp get_resp;
	struct start_stream_cmd start_stream;
	struct start_stream_rsp start_resp;
	struct media_packet_header packet_header;
	struct media_payload_header payload_header;
	struct close_stream_cmd close_stream;
	struct close_stream_rsp close_resp;

	bdaddr_t src, dst;
	unsigned short psm_cmd, psm_stream;
	unsigned long flags;
	int frame_len;
	time_t timestamp;
	uint16_t mtu, seq_num;
	int fd;

	char *addrstr;
	char *filename;

	bacpy(&src, BDADDR_ANY);

	switch (argc) {
	case 3:
		addrstr = argv[1];
		filename = argv[2];
		break;

	case 2:
		addrstr = argv[1];
		filename = "-";
		break;

	default:
		usage();
		exit(-1);
	}

	if(!strcmp(filename,"-")) {
		fd = 0;
	} else {
		fd = open(filename, 0);
		if (fd < 0) {
			fprintf(stderr, "couldn't open %s\n", filename);
			exit(-1);
		}
	}
	frame_len = read_header(fd, &sbc_info);
	if (frame_len < 0) {
		fprintf(stderr, "couldn't read header on %s\n", filename);
		exit(-1);
	}

	printf("subbands = %d\n"
		"allocation method = %d\n"
		"channel_mode = %d\n"
		"blocks = %d\n"
		"sampling frequency = %d\n"
		"bitpool = %d\n",
			sbc_info.subbands, sbc_info.allocation_method,
			sbc_info.channel_mode, sbc_info.blocks,
			sbc_info.sampling_frequency, sbc_info.bitpool);

	printf("frame len = %d\n", frame_len);

	printf("Using address: %s\n", addrstr);

	str2ba(addrstr, &dst);

	if (detect_a2dp(&src, &dst, &psm_cmd, &flags) < 0) {
		fprintf(stderr, "could not find A2DP services on device %s\n", addrstr);
		exit(-1);
	}

	/* setup sigterm handler. we must make sure to do a clean disconnect */
	memset(&sa, 0, sizeof(sa));
	sa.sa_flags = SA_NOCLDSTOP;
	sa.sa_handler = sig_term;
	sigaction(SIGTERM, &sa, NULL);
	sigaction(SIGINT, &sa, NULL);

	sa.sa_handler = SIG_IGN;
	sigaction(SIGCHLD, &sa, NULL);
	sigaction(SIGPIPE, &sa, NULL);

	cmdfd = do_connect(&src, &dst, psm_cmd, NULL);
	if (cmdfd < 0) {
		fprintf(stderr, "cannot open psm_cmd = %d\n", psm_cmd);
		exit(-1);
	}
	// avdt_discover_req
	memset(&put_req, 0, sizeof(put_req));
	init_request(&put_req.header, AVDTP_DISCOVER);

	if (write(cmdfd, &put_req, sizeof(put_req)) != sizeof(put_req)) {
		fprintf(stderr, "couldn't send avdtp_discover\n");
		close(cmdfd);
		exit(-1);
	}

	int size;
	size = read(cmdfd, &get_resp, sizeof(get_resp));
	if (size < sizeof(get_resp) - MAX_ADDITIONAL_CODEC_OCTETS) {
		fprintf(stderr, "couldn't get avdtp_discover\n");
		close(cmdfd);
		exit(-1);
	}

	int i;
	int seid = -1;
	int last_seid_index = MAX_ADDITIONAL_CODEC - ((sizeof(get_resp)-size)/sizeof(struct acp_seid_info));

	for(i=0; i <= last_seid_index; i++) {
		if (process_seid(cmdfd, &get_resp.infos[i], &psm_stream) == 0) {
			seid = get_resp.infos[i].acp_seid;
			break;
		}
	}

	if(seid == -1) {
		fprintf(stderr, "couldn't locate the correct seid\n");
		exit(-1);
	}

	// open the stream l2cap

	mtu = 48;
	streamfd = do_connect(&src, &dst, psm_stream, &mtu);
	if (streamfd < 0) {
		fprintf(stderr, "cannot open psm_stream = %d\n", psm_stream);
		exit(-1);
	}

	// start the stream

	memset(&start_stream, 0, sizeof(start_stream));
	init_request(&start_stream.header, AVDTP_START);
	start_stream.acp_seid = seid;

	if (write(cmdfd, &start_stream, sizeof(start_stream)) != sizeof(start_stream)) {
		fprintf(stderr, "couldn't send start_stream\n");
		close(streamfd);
		close(cmdfd);
		exit(-1);
	}

	printf("sent stream-start\n");

	if (read(cmdfd, &start_resp, sizeof(start_resp)) < sizeof(start_resp) - 2 ||
			start_resp.header.message_type == MESSAGE_TYPE_REJECT) {
		fprintf(stderr, "didn't receive start_resp confirm for seid = %d\n", seid);
		close(streamfd);
		close(cmdfd);
		return (-1);
	}

	printf("got start stream confirm\n");

	char buf[BUFS];
	int psize;

	if (mtu > BUFS)
		mtu = BUFS;

	terminate = 0;
	seq_num = 1;

	memset(&payload_header, 0, sizeof(payload_header));

	timestamp = 0;

	while (!terminate) {

		// a2dp headers: avdtp p.45
		memset(&packet_header, 0, sizeof(packet_header));
		packet_header.v = 2;
		packet_header.pt = 1;
		packet_header.sequence_number = htons(seq_num);
		packet_header.timestamp = htonl(timestamp);
		packet_header.ssrc = htonl(1);
		timestamp += (sbc_info.blocks + 1)*4 * (sbc_info.subbands + 1)*4;

		memcpy(buf, &packet_header, sizeof(packet_header));
		psize = sizeof(packet_header);

		// framing a2dp p.23
		payload_header.frame_count = 0;

		/* BEGIN: NONSPECAUDIO == TRUE */
		if (flags & NONSPECAUDIO) {
			
			buf[12] = 0xff;
			buf[13] = 0xff;
			
			memcpy(buf + MEDIA_PACKET_HEADER_LENGTH, &sbc_info, sizeof(sbc_info));
			size = read(fd, buf + sizeof(sbc_info) + MEDIA_PACKET_HEADER_LENGTH, frame_len);
			if (size != frame_len) {
				terminate = 1;
			} else {
				// framing, fragmenting, a2dp headers: avdtp p.45 and a2dp spec p.23
				write(streamfd, buf, size + sizeof(sbc_info) + MEDIA_PACKET_HEADER_LENGTH);
			}
			
			frame_len = read_header(fd, &sbc_info);
			if (frame_len < 0) {
				fprintf(stderr, "couldn't read header on %s\n", filename);
				break;
			}
			
			/* END: NONSPECAUDIO == TRUE */
		} else {
			/* BEGIN: NONSPECAUDIO == FALSE */
			
			// make room for the payload header but don't copy it yet (count up frames first)
			psize += sizeof(payload_header);
			
			do {
				payload_header.frame_count++;
				
				memcpy(buf + psize, &sbc_info, sizeof(sbc_info));
				psize += sizeof(sbc_info);
				
				size = read(fd, buf + psize, frame_len);
				psize += size;
				
				if (size != frame_len) {
					fprintf(stderr, "couldn't read frame of %d bytes (frame %d) on %s\n", 
						frame_len, payload_header.frame_count, filename);
					terminate = 1;
					break;
				}
				
				frame_len = read_header(fd, &sbc_info);
				if (frame_len < 0) {
					fprintf(stderr, "couldn't read header on %s\n", filename);
					terminate = 1;
					break;
				}
				
			} while(psize + frame_len + sizeof(sbc_info) < mtu);
			
			memcpy(buf + sizeof(packet_header), &payload_header, sizeof(payload_header));
			write(streamfd, buf, psize);
			
		}
		/* END: NONSPECAUDIO == FALSE */

		seq_num++;
	}
	close(fd);
	printf("sent %d packets\n", seq_num);

	// signal the stream close

	memset(&close_stream, 0, sizeof(close_stream));
	init_request(&close_stream.header, AVDTP_CLOSE);
	close_stream.acp_seid = seid;

	if (write(cmdfd, &close_stream, sizeof(close_stream)) != sizeof(close_stream)) {
		fprintf(stderr, "couldn't send close_stream\n");
		close(streamfd);
		close(cmdfd);
		exit(-1);
	}

	printf("sent stream-close\n");

	if (read(cmdfd, &close_resp, sizeof(close_resp)) < sizeof(close_resp) - 1 ||
			close_resp.header.message_type == MESSAGE_TYPE_REJECT) {
		fprintf(stderr, "didn't receive close_resp confirm for seid = %d\n", seid);
		close(streamfd);
		close(cmdfd);
		return (-1);
	}

	printf("got close stream confirm\n");

	close(streamfd);
	close(cmdfd);

	return 0;
}