a2dpd.c

linux蓝牙剖面实现

							}
							break;
						default:
							DBG("Invalid state");
							break;
						}
					} else {
						// Increase client timeout counter
						//DBG("No data to read");
						bError = 0;
						lpDevice->clients[i].timeoutcount++;
					}

					// Reset client timeout
					if(!bError) {
						lpDevice->clients[i].timeoutcount=0;
					} else {
						lpDevice->clients[i].timeoutcount++;
					}

					// Remove client for poll at next iteration if an error is detected
					if ((lpDevice->clients[i].timeoutcount>100) || bError || (pollfds[pollfdidx].revents & POLLERR) || (pollfds[pollfdidx].revents & POLLHUP)) {
						DBG("Client %d: Error detected (err=%d, toc=%d, err=%d, hup=%d)", i, bError,
						lpDevice->clients[i].timeoutcount,
						(pollfds[pollfdidx].revents& POLLERR)?1:0,
						(pollfds[pollfdidx].revents& POLLHUP)?1:0
						);
						close_socket(&lpDevice->clients[i].socket);
						lpDevice->clients[i].state = CLIENT_STATE_DISCONNECTED;
						lpDevice->clients[i].lives = 0;
						lpDevice->nb_clients--;
					}
					pollfdidx++;
				}
			}
		}
	} else {
		usleep(delay);
		// DBG("No clients");
	}

	// pthread_mutex_unlock(&lpDevice->mutex);

	return 0;
}

int retrieve_data_from(LPBTA2DPPERDEVICEDATA lpDevice, char** pcm_buffers, int nbuffers, int* pcm_buffers_size, AUDIOPACKETHEADER* hdr)
{
	// If there are BT data, send them
	//FIXME Since we read nb_clients, we should lock mutex, but it may create timer issues
	// degrading sound
	//pthread_mutex_lock(&lpDevice->mutex);
	int i;
	int new_state = DEVICE_STATE_NOSOUND;

	//if (lpDevice->nb_clients > 0) {
		// Retrieve data for client where it is available
		for (i = 0; i < nbuffers; i++) {
			if (lpDevice->clients[i].lives) {
				pthread_mutex_lock(&lpDevice->clients[i].mutex);

				if (lpDevice->clients[i].ring_in != lpDevice->clients[i].ring_out) {
					// Get ring buffer
					pcm_buffers[i] = lpDevice->clients[i].ring[lpDevice->clients[i].ring_out].buf;
					pcm_buffers_size[i] = lpDevice->clients[i].ring[lpDevice->clients[i].ring_out].len;
					*hdr = lpDevice->clients[i].ring[lpDevice->clients[i].ring_out].hdr;
					// Tell client we got them
					lpDevice->clients[i].ring[lpDevice->clients[i].ring_out].buf = NULL;
					lpDevice->clients[i].ring[lpDevice->clients[i].ring_out].len = 0;

					// Move to next ring
					int next_ring = ((lpDevice->clients[i].ring_out + 1) % MAXCLIENTSRINGSIZE);

					//DBG("Reading pool %d[ %d] = %p", i, lpDevice->clients[i].ring_out, pcm_buffers[i]);

					lpDevice->clients[i].ring_out = next_ring;

					// Remember we got some sound
					new_state = DEVICE_STATE_SOUND;
				}

				pthread_mutex_unlock(&lpDevice->clients[i].mutex);
			}
		}
	//}

	//FIXME 
	//pthread_mutex_unlock(&lpDevice->mutex);

	return new_state;
}

int poll_incoming_avdtp_sockets(int avdtpfd, int* incoming_socket)
{
	int ret = -1;

	// Handle incoming connections
	if(poll_accept(avdtpfd, 0)) {
		char szRemote[20];
		DBG("AVDTP Incoming connection");
		if(*incoming_socket == -1) {
			// Accept the first
			*incoming_socket = a2dp_wait_connection(avdtpfd, szRemote, sizeof(szRemote), NULL);
			DBG("AVDTP Accepted %s on socket %d", szRemote, *incoming_socket);
			if (g_sCmdNew[0]) {
				async_run_process(g_sCmdNew, 0);
				ret = 0;
			}
		}
	}

	return ret;
}

void a2dpd_save_setup(LPBTA2DPPERDEVICEDATA lpDevice) {
	
	DBG("Saving configuration to %s", g_srcfilename);
	if(write_config_string(g_srcfilename, "a2dpd", "address", lpDevice->addr)==0) {
		DBG("Saved configuration sucessfully");
	} else {
		DBG("Failed saving configuration");
	}
}

/////////////////////////////////
// This function handle one stream
void *a2dp_handler(void *param)
/////////////////////////////////
{
	int i;
	LPBTA2DPPERDEVICEDATA lpDevice = (LPBTA2DPPERDEVICEDATA) param;

	// As long as daemon is running
	while (!bSigINTReceived) {
		int destroy_count = -1;

		char *pcm_buffer = mymalloc(lpDevice->a2dp_framesize);
		int state_previous = DEVICE_STATE_NOSOUND;
		TIMERINFO TimerInfos;

		int a2dpd_dbus_state = DISCONNECTED;
		a2dpd_signal_state(DISCONNECTED, "");

		// Connect to the A2DP device
		A2DPSETTINGS settings;
		memset(&settings, 0, sizeof(settings));
		strncpy(settings.bdaddr, lpDevice->addr, sizeof(settings.bdaddr)-1);
		settings.framerate = lpDevice->a2dp_rate;
		settings.channels = lpDevice->a2dp_channels;
		settings.sbcbitpool = lpDevice->sbcbitpool;
		settings.flags = read_config_int(g_srcfilename, "a2dpd", "flags", 0);
		LPA2DP a2dp = a2dp_new(&settings);
		LPALSA alsa = alsa_new(lpDevice->plug, lpDevice->a2dp_rate);
		LPSCO sco = sco_new(lpDevice->addr);

		DBG("Bluetooth Device Settings [%d hz, %d channels, %d bits]", lpDevice->a2dp_rate, lpDevice->a2dp_channels, lpDevice->a2dp_bitspersample*8);

		int avdtpfd = a2dp_make_listen_socket(25);

		// This timer is used to sync bluetooth sound emission
		// This is because not all device have a queue for incoming sample
		// And device who have a queue won't react correctly
		memset(&TimerInfos, 0, sizeof(TimerInfos));
		TimerInfos.fps = (float)(((float) (lpDevice->a2dp_rate*lpDevice->a2dp_channels*lpDevice->a2dp_bitspersample)/((float) lpDevice->a2dp_framesize))/1.0);

		// As long as we can send sound
		while (pcm_buffer && !bSigINTReceived) {
			int bError = 0;
			int pcm_buffer_filed_size = 0;
			char *pcm_buffers[MAXCLIENTSPERDEVICE];
			int pcm_buffers_size[MAXCLIENTSPERDEVICE];
			int state_current = DEVICE_STATE_NOSOUND;
			int delay = 0;
			AUDIOPACKETHEADER hdr;
			char scoFrame[512];
			ssize_t scoFrameSize = 0;

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

			memset(pcm_buffers, 0, sizeof(pcm_buffers));
			memset(pcm_buffers_size, 0, sizeof(pcm_buffers_size));

			// Incoming AVDTP socket
			int incoming_socket = -1;
			poll_incoming_avdtp_sockets(avdtpfd, &incoming_socket);

			// Give the socket to state machine if possible
			if(incoming_socket>0) {
				// A2DP is not already connected
				if(a2dp_state_use_socket(a2dp, incoming_socket)<0) {
					DBG("State machine is busy, closing socket %d", incoming_socket);
					close_socket(&incoming_socket);
				}
			}

			// Retrieve streams
			state_current = retrieve_data_from(lpDevice, pcm_buffers, MAXCLIENTSPERDEVICE, pcm_buffers_size, &hdr);

			// Manual connection with or without stream
			if(lpDevice->ctl_socket[READ_SIDE]>0) {
				if(poll_accept( lpDevice->ctl_socket[READ_SIDE], 0)) {
					unsigned char order, param;
					int lparam = 0;
					char addr[32];
					char* redirect = NULL;
					read(lpDevice->ctl_socket[READ_SIDE], &order, sizeof(order));
					DBG("Processing '%c' %d", order, order);
					switch(order) {
					case 'a':
						read(lpDevice->ctl_socket[READ_SIDE], &param, sizeof(param));
						if(param == 'a')
							g_autoconnect = !g_autoconnect;
						else
							g_autoconnect = (param=='1')?1:0;
						DBG("Autoconnect %s to %s", (param == 'a')?"swapped":"set", g_autoconnect?"on":"off");
						break;
					case 'c':
						if(lpDevice->bredirect == REDIRECT_A2DP)
							a2dp_state_connect(a2dp);
						if(lpDevice->bredirect == REDIRECT_SCO)
							sco_state_connect(sco);
						break;
					case 'd':
						a2dp_state_disconnect(a2dp);
						sco_state_disconnect(sco);
						break;
					case 'p':
						a2dp_state_suspend(a2dp);
						sco_state_suspend(sco);
						break;
					case 's':
						a2dp_state_startstream(a2dp);
						sco_state_startstream(sco);
						break;
					case 'w':
						a2dpd_save_setup(lpDevice);
						break;
					case 't':
						a2dp_state_disconnect(a2dp);
						sco_state_disconnect(sco);
						// FIXME disconnect sco or a2dpd when changing
						read(lpDevice->ctl_socket[READ_SIDE], &addr, sizeof(addr));
						if(strcasecmp(addr, "none")==0) {
							lpDevice->bredirect = REDIRECT_NONE;
							redirect = "None";
						} else if(strcasecmp(addr, "auto")==0) {
							lpDevice->bredirect = REDIRECT_AUTO;
							redirect = "Auto";
						} else if(strcasecmp(addr, "alsa")==0) {
							lpDevice->bredirect = REDIRECT_ALSA;
							redirect = "Alsa";
						} else if(strcasecmp(addr, "sco")==0) {
							lpDevice->bredirect = REDIRECT_SCO;
							redirect = "Sco";
						} else if(strcasecmp(addr, "a2dp")==0) {
							lpDevice->bredirect = REDIRECT_A2DP;
							redirect = "A2dp";
						} else {
							redirect = &addr[0];
							strcpy(lpDevice->addr, addr);
							a2dp_set_dst_addr(a2dp, addr);
							sco_set_dst_addr(sco, addr);
						}
						DBG("SetAddress to %s", redirect); destroy_count=lpDevice->a2dp_timeout+1;
						break;
					case 'v':
						lparam = A2DPD_VOLUME_MAX;
						read(lpDevice->ctl_socket[READ_SIDE], &lparam, sizeof(lparam));
						lparam = min(max(0, lparam), A2DPD_VOLUME_MAX);
						lpDevice->mixer.volume_speaker_left = lparam;
						lpDevice->mixer.volume_speaker_right = lparam;
						DBG("SetVolume to %d", lparam);
						break;
					case 'f':
						read(lpDevice->ctl_socket[READ_SIDE], &lparam, sizeof(lparam));
						a2dp_set_user_flags(a2dp, lparam);
						DBG("SetFlags to %d", lparam);
						break;
					case 'r':
						read(lpDevice->ctl_socket[READ_SIDE], &lparam, sizeof(lparam));
						g_brereadconfig = lparam;
						DBG("SetRereadConfig to %d", lparam);
						break;
					default:
						DBG("Invalid command [%c] [%d] on ctl_socket", order, order);
						break;
					}
				}
			}

			// Handle AVDTP incoming commands (AVDTP_SUSPEND, ...)
			a2dp_state_machine(a2dp);

			// Setup the resampler to use the correct rate
			if(lpDevice->bredirect != REDIRECT_SCO) {
				if(lpDevice->a2dp_framesize != POOLENTRYSIZE_A2DP) {
					lpDevice->a2dp_framesize = POOLENTRYSIZE_A2DP;
					lpDevice->a2dp_rate = read_config_int(g_srcfilename, "a2dpd", "rate", A2DPD_FRAME_RATE);
					lpDevice->a2dp_channels = read_config_int(g_srcfilename, "a2dpd", "channels", 2);
					lpDevice->a2dp_bitspersample = 16/8;
				}
			} else {
				if(lpDevice->a2dp_framesize != POOLENTRYSIZE_SCO) {
					lpDevice->a2dp_framesize = POOLENTRYSIZE_SCO;
					lpDevice->a2dp_rate = 8000;
					lpDevice->a2dp_channels = 1;
					lpDevice->a2dp_bitspersample = 2;
				}
			}

			{
				//FIXME Handle SCO incoming stream
				sco_state_machine(sco, scoFrame, sizeof(scoFrame), &scoFrameSize);
				if(scoFrameSize>0 && (lpDevice->bredirect == REDIRECT_SCO)) {
					char* tmp = mymalloc(scoFrameSize);
					if(tmp) {
						memcpy(tmp, scoFrame, scoFrameSize);
						pthread_mutex_lock(&lpDevice->capture_mutex);
						int next_ring = (lpDevice->ring_in+1)%MAXCLIENTSRINGSIZE;
						if(next_ring != lpDevice->ring_out) {
							//DBG("Enqueue sco incoming stream %d", next_ring);
							lpDevice->ring[lpDevice->ring_in].len = scoFrameSize;
							lpDevice->ring[lpDevice->ring_in].buf = tmp;
							lpDevice->ring_in = next_ring;
						} else {
							safefree(tmp);
						}
						pthread_mutex_unlock(&lpDevice->capture_mutex);
					}
				}
			}

			pcm_buffer_filed_size = audio_mixer_16bits(pcm_buffer, pcm_buffers, MAXCLIENTSPERDEVICE, pcm_buffers_size, lpDevice->mixer.volume_speaker_left, lpDevice->mixer.volume_speaker_right,
			A2DPD_VOLUME_MAX,
			g_breversestereo);

			// Free no longer used audio blocks
			for (i = 0; i < MAXCLIENTSPERDEVICE; i++) {
				safefree(pcm_buffers[i]);
			}

			// Send mixed audio stream to clients
			switch (state_current) {
			case DEVICE_STATE_SOUND:

				// Try to connect only when there is a stream
				if(g_autoconnect) {
					if(lpDevice->bredirect == REDIRECT_A2DP || lpDevice->bredirect == REDIRECT_AUTO)
						a2dp_state_connect(a2dp);
					if(lpDevice->bredirect == REDIRECT_SCO)
						sco_state_connect(sco);
				}

				/////////////////////////////////
				// Transfer data to bluetooth
				/////////////////////////////////

				if (pcm_buffer_filed_size > 0) {
					// Transfer takes place by lpDevice->a2dp_framesize bytes blocks
					int blockstart = 0;
					int blocksize = lpDevice->a2dp_framesize;

					// Send data to BT headset
					while (!bError && blockstart < pcm_buffer_filed_size) {
						int transfer = -1;

						blocksize = (pcm_buffer_filed_size < lpDevice->a2dp_framesize) ? pcm_buffer_filed_size : lpDevice->a2dp_framesize;

						if(lpDevice->bredirect == REDIRECT_ALSA) {
							transfer = alsa_transfer_raw(alsa, pcm_buffer + blockstart, blocksize);
						} else if(lpDevice->bredirect == REDIRECT_A2DP) {
							transfer = a2dp_transfer_raw(a2dp, pcm_buffer + blockstart, blocksize, &hdr);
						} else if(lpDevice->bredirect == REDIRECT_SCO) {
							transfer = sco_transfer_raw(sco, pcm_buffer + blockstart, blocksize);
						} else if(lpDevice->bredirect == REDIRECT_AUTO) {
							// transfer to a2dp if a2dp is connected, else transfer to alsa
							if(a2dp_is_connected(a2dp)) {
								transfer = a2dp_transfer_raw(a2dp, pcm_buffer + blockstart, blocksize, &hdr);
							} else {
								transfer = alsa_transfer_raw(alsa, pcm_buffer + blockstart, blocksize);
							}
						} else {
							// Fake transfer
							transfer = blocksize;
						}

						// If 0, then we must retry the encoding
						if (transfer >= 0) {
							destroy_count = 0;
							blockstart += transfer;
							//DBG("transfered");
							a2dp_timer_notifyframe(&TimerInfos);
						} else {
							DBG("Error in transfer (transfer=%d)", transfer);
							bError = 1;
						}
					}
				}
				break;
			case DEVICE_STATE_NOSOUND:
				if (state_previous == DEVICE_STATE_SOUND) {
					DBG("Sound stream dry");
				}
				// Try to remember why it is only when we sent sound that the counter will be incremented
				if(destroy_count>=0)
					destroy_count++;
				break;