a2dpd.c

linux蓝牙剖面实现

			lpClientData->ring[lpClientData->ring_in].hdr = *lpHdr;
			lpClientData->ring[lpClientData->ring_in].len = lpConvert->size;
			lpClientData->ring_in = next_ring;
			// We will not free that buffer, it's the bthandler thread which will do it
			lpConvert->lpVoid = NULL;
			lpConvert->size = 0;
			lpConvert->index_to_construct = 0;
			lpConvert->index_0 = 0;
		} else {
			//DBG("Ring buffer is full");
		}

		pthread_mutex_unlock(&lpClientData->mutex);
	}
	// Reintegrate data in pool if not transmitted via bthandler thread
	safefree(lpConvert->lpVoid);
}

// Convert individual sample
void convert_sample(AUDIOSTREAMINFOS* lpStreamInfos, void* lpSample, void* lpConvertedSample, BTA2DPPERDEVICEDATA* lpDevice)
{
	// Signed 32bits pivot
	int32_t channel_1=0;
	int32_t channel_2=0;
	// Convert to pivot format
	if(lpStreamInfos->channels==1) {
		if(lpStreamInfos->format==A2DPD_PCM_FORMAT_S8) {
			channel_1 = (*(((int8_t*)lpSample)+0))*256;
			channel_2 = (*(((int8_t*)lpSample)+0))*256;
		} else if(lpStreamInfos->format==A2DPD_PCM_FORMAT_U8) {
			channel_1 = ((*(((int8_t*)lpSample)+0))-(int)128)*256;
			channel_2 = ((*(((int8_t*)lpSample)+0))-(int)128)*256;
		} else if(lpStreamInfos->format==A2DPD_PCM_FORMAT_S16_LE) {
			channel_1 = *(((int16_t*)lpSample)+0);
			channel_2 = *(((int16_t*)lpSample)+0);
		}
	} else if(lpStreamInfos->channels==2) {
		if(lpStreamInfos->format==A2DPD_PCM_FORMAT_S8) {
			channel_1 = (*(((int8_t*)lpSample)+0))*256;
			channel_2 = (*(((int8_t*)lpSample)+1))*256;
		} else if(lpStreamInfos->format==A2DPD_PCM_FORMAT_U8) {
			channel_1 = ((*(((int8_t*)lpSample)+0))-(int)128)*256;
			channel_2 = ((*(((int8_t*)lpSample)+1))-(int)128)*256;
		} else if(lpStreamInfos->format==A2DPD_PCM_FORMAT_S16_LE) {
			channel_1 = *(((int16_t*)lpSample)+0);
			channel_2 = *(((int16_t*)lpSample)+1);
		}
	} else {
		if(lpStreamInfos->format==A2DPD_PCM_FORMAT_S8) {
			channel_1 = (*(((int8_t*)lpSample)+0))*256;
			channel_2 = (*(((int8_t*)lpSample)+1))*256;
		} else if(lpStreamInfos->format==A2DPD_PCM_FORMAT_U8) {
			channel_1 = ((*(((int8_t*)lpSample)+0))-(int)128)*256;
			channel_2 = ((*(((int8_t*)lpSample)+1))-(int)128)*256;
		} else if(lpStreamInfos->format==A2DPD_PCM_FORMAT_S16_LE) {
			channel_1 = *(((int16_t*)lpSample)+0);
			channel_2 = *(((int16_t*)lpSample)+1);
		}
	}

	// Convert to destination format
	if(lpDevice->a2dp_channels==1) {
		if(lpDevice->a2dp_bitspersample==1) {
			*(int8_t*)lpConvertedSample=(channel_1+channel_2)/(2*256);
		} else if(lpDevice->a2dp_bitspersample==2) {
			*(int16_t*)lpConvertedSample=(channel_1+channel_2)/(2);
		}
	} else if(lpDevice->a2dp_channels==2) {
		if(lpDevice->a2dp_bitspersample==1) {
			*(((int8_t*)lpConvertedSample)+0)=channel_1/256;
			*(((int8_t*)lpConvertedSample)+1)=channel_2/256;
		} else if(lpDevice->a2dp_bitspersample==2) {
			*(((int16_t*)lpConvertedSample)+0)=channel_1;
			*(((int16_t*)lpConvertedSample)+1)=channel_2;
		}
	} else {
		memset(lpConvertedSample, 0, lpDevice->a2dp_bitspersample*lpDevice->a2dp_channels);
		if(lpDevice->a2dp_bitspersample==1) {
			*(((int8_t*)lpConvertedSample)+0)=channel_1/256;
			*(((int8_t*)lpConvertedSample)+1)=channel_2/256;
		} else if(lpDevice->a2dp_bitspersample==2) {
			*(((int16_t*)lpConvertedSample)+0)=channel_1;
			*(((int16_t*)lpConvertedSample)+1)=channel_2;
		}
	}
}

// This function convert a buffer to sample rate and format needed for device
void convert_rate(BTA2DPPERDEVICEDATA* lpDevice, BTA2DPPERCLIENTDATA* lpClientData, void* pcm_buffer, int pcm_buffer_size, AUDIOSTREAMINFOS* lpStreamInfos, AUDIOPACKETHEADER* lpHdr)
{
	// We need this structure accross calls
	CONVERTBUFFER* lpConvert = &lpClientData->conv;

	if(lpConvert && lpStreamInfos && lpStreamInfos->bitspersample) {
		unsigned int pcm_buffer_index = 0;
		unsigned int pcm_buffer_index_0 = 0;
		unsigned int pcm_buffer_frame_bytes = (lpStreamInfos->channels*lpStreamInfos->bitspersample);
		unsigned int pcm_buffer_nframes = pcm_buffer_size/pcm_buffer_frame_bytes;
		unsigned int rate_multiplier = ((unsigned int)lpStreamInfos->rate)*256 / ((unsigned int)lpDevice->a2dp_rate);
		unsigned int convert_frame_bytes = (lpDevice->a2dp_channels*lpDevice->a2dp_bitspersample);
		void* lpConvertedSample = mymalloc(convert_frame_bytes);
		void* lpSample = NULL;
		//int i;

		lpConvert->index_0 = lpConvert->index_to_construct;
		lpConvert->index_to_construct = 0;
		while(pcm_buffer_index<pcm_buffer_nframes) {
			// Allocate destination if needed
			if(lpConvert->lpVoid==NULL) {
				lpConvert->lpVoid = mymalloc(POOLENTRYSIZE_A2DP/*lpDevice->a2dp_framesize*/);
				lpConvert->size = lpDevice->a2dp_framesize;
				/*
				for(i=0; i<lpConvert->size; i++)
				{
					((char*)lpConvert->lpVoid)[i]=(char)0xFA;
				}
				*/
				lpConvert->index_to_construct = 0;
				lpConvert->index_0 = 0;
			}

			// Get pointer to sample to convert
			lpSample = pcm_buffer+(pcm_buffer_index*pcm_buffer_frame_bytes);

			// Conversion of individual samples
			convert_sample(lpStreamInfos, lpSample, lpConvertedSample, lpDevice);

			// Append converted sample to constructed blocks, Can be avoided by converting in destination buffer
			void* lpDest = lpConvert->lpVoid+((lpConvert->index_0+lpConvert->index_to_construct)*convert_frame_bytes);
			memcpy(lpDest, lpConvertedSample, convert_frame_bytes);

			// Fill next index
			lpConvert->index_to_construct++;

			// The index to fill will be mapped according to rates
			pcm_buffer_index = pcm_buffer_index_0 + ((lpConvert->index_to_construct*rate_multiplier)/256);

			// If constructed block is full, enqueue and allocate new
			if(((lpConvert->index_0+lpConvert->index_to_construct)*convert_frame_bytes)>=lpConvert->size) {
				/*
				if(checkbuffer(lpConvert->lpVoid))
				{
					DBG("Buffer overflow: %d,%d", lpConvert->index_0+lpConvert->index_to_construct, lpDevice->a2dp_framesize/convert_frame_bytes);
				}
				int state=0;
				int count=0;
				int total=0;
				for(i=0; i<lpConvert->size/2; i+=2) {
					if(state==0) {
						//DBG("%08X | %08X   %d | %d", ((int16_t*)lpConvert->lpVoid)[i], ((int16_t*)lpConvert->lpVoid)[i+1], ((int16_t*)lpConvert->lpVoid)[i], ((int16_t*)lpConvert->lpVoid)[i+1]);
						if(((int16_t*)lpConvert->lpVoid)[i]==(int16_t)0xFAFA) {
							state=1;
							count++;
							total++;
						} else {
							state=0;
						}
					} else if(state==1) {
						if(((int16_t*)lpConvert->lpVoid)[i]==(int16_t)0xFAFA) {
							count++;
							total++;
						} else {
							//DBG("Gap in the data %d,%d", count, i);
							state=0;
							count=0;
						}
					}
				}
				if(state==1) {
					DBG("Gap in the data: %d, total=%d", count, total);
				}
				//exit(0);
				*/

				// Enqueue in ring buffer
				append_to_ring_buffer(lpClientData, lpConvert, lpHdr);

				// Store next index to read
				pcm_buffer_index_0 = pcm_buffer_index;
				pcm_buffer_index = pcm_buffer_index_0;
			}
		}

		safefree(lpConvertedSample);
	}
}

// This function manage volume change wanted by clients
void a2dpd_plugin_ctl_write(LPA2DPDCLIENT lpClient)
{
	AUDIOMIXERDATA AudioMixerData = INVALIDAUDIOMIXERDATA;

	DBG("CTL WRITE thread %d started", lpClient->sockfd);

	if (recv_socket(lpClient->sockfd, &AudioMixerData, sizeof(AudioMixerData)) == sizeof(AudioMixerData)) {
		pthread_mutex_lock(&lpClient->lpDevice->mutex);
		if (AudioMixerData.volume_speaker_left != -1)
			lpClient->lpDevice->mixer.volume_speaker_left = AudioMixerData.volume_speaker_left;
		if (AudioMixerData.volume_speaker_left != -1)
			lpClient->lpDevice->mixer.volume_speaker_right = AudioMixerData.volume_speaker_right;
		if (AudioMixerData.volume_micro_left != -1)
			lpClient->lpDevice->mixer.volume_micro_left = AudioMixerData.volume_micro_left;
		if (AudioMixerData.volume_micro_left != -1)
			lpClient->lpDevice->mixer.volume_micro_right = AudioMixerData.volume_micro_right;
		pthread_mutex_unlock(&lpClient->lpDevice->mutex);

		// Notify other clients
		int notifyfd = make_udp_socket();
		send_socket(notifyfd, &AudioMixerData, sizeof(AudioMixerData));
		close_socket(&notifyfd);
	}
}

// This function manage volume read for client
void a2dpd_plugin_ctl_read(LPA2DPDCLIENT lpClient)
{
	AUDIOMIXERDATA AudioMixerData = INVALIDAUDIOMIXERDATA;
	DBG("CTL READ thread %d started", lpClient->sockfd);

	pthread_mutex_lock(&lpClient->lpDevice->mutex);
	AudioMixerData = lpClient->lpDevice->mixer;
	pthread_mutex_unlock(&lpClient->lpDevice->mutex);

	send_socket(lpClient->sockfd, &AudioMixerData, sizeof(AudioMixerData));
}

// Retrieve an index in client table for device
int get_index_for_client(LPBTA2DPPERDEVICEDATA lpDevice)
{
	int client_index = -1;

	// Find an index in clients table for the mixer
	//pthread_mutex_lock(&lpDevice->mutex);
	for (client_index = 0; client_index < MAXCLIENTSPERDEVICE; client_index++) {
		if (lpDevice->clients[client_index].lives == 0) {
			// FIXME Not sure this is safe but this is very unlikely to happen
			lpDevice->clients[client_index].lives = 1;
			lpDevice->clients[client_index].state = CLIENT_STATE_NEW;
			lpDevice->clients[client_index].ring_in = 0;
			lpDevice->clients[client_index].ring_out = 0;
			lpDevice->nb_clients++;
			break;
		}
	}

	//pthread_mutex_unlock(&lpDevice->mutex);

	return client_index;
}

int poll_incoming_clients_sockets(LPBTA2DPPERDEVICEDATA lpDevice, int delay)
{
	int ipoll = -1;
	int i;
	int pollfdcount = 0;
	int pollfdidx = 0;
	struct pollfd pollfds[MAXCLIENTSPERDEVICE];
	memset(pollfds, 0, sizeof(pollfds));

	// pthread_mutex_lock(&lpDevice->mutex);

	// Retrieve data for client where it is available
	for (i = 0; i < MAXCLIENTSPERDEVICE; i++) {
		if(lpDevice->clients[i].lives) {
			pollfds[pollfdcount].fd = lpDevice->clients[i].socket;
			pollfds[pollfdcount].events = POLLIN | POLLERR | POLLHUP;
			pollfds[pollfdcount].revents = 0;
			pollfdcount++;
		}
	}

	if(pollfdcount>0) {

		// DBG("Polling %d clients for %d ms", pollfdcount, (delay/1000));

		// Poll
		ipoll = poll(pollfds, pollfdcount, (delay/1000));

		if(ipoll>0) {
			// Retrieve data for client where it is available
			for (i = 0; i < MAXCLIENTSPERDEVICE; i++) {
				// Valid client
				if(lpDevice->clients[i].lives) {
					int bError = 1;

					// Some data to read?
					if (pollfds[pollfdidx].revents & POLLIN) {
						AUDIOPACKETHEADER hdr;
						int client_type = INVALID_CLIENT_TYPE;
						AUDIOSTREAMINFOS StreamInfos;
						int result;
						char* tmpcapture;
						int tmpcapturelen;
						uint32_t dummy;

						// Already streaming?
						switch(lpDevice->clients[i].state) {
						case CLIENT_STATE_NEW:
							bError = 1;
							// Receive type of client
							if(recv_socket(lpDevice->clients[i].socket, &client_type, sizeof(client_type))==sizeof(client_type)) {
								// This client wants to send us pcm control data
								if (client_type == A2DPD_PLUGIN_CTL_WRITE) {
									DBG("Plugin ctl write ignored");
									//a2dpd_plugin_ctl_write(&lpDevice->clients[i]);
								}
								// This client wants to read our control status
								if (client_type == A2DPD_PLUGIN_CTL_READ) {
									DBG("Plugin ctl read ignored");
									//a2dpd_plugin_ctl_read(&lpDevice->clients[i]);
								}
								// This client wants to send us pcm stream
								if (client_type == A2DPD_PLUGIN_PCM_WRITE) {
									lpDevice->clients[i].state = CLIENT_STATE_STREAMINGSETUP;
									bError = 0;
								}
								// This client wants to send us pcm stream
								if (client_type == A2DPD_PLUGIN_PCM_READ) {
									lpDevice->clients[i].state = CLIENT_STATE_CAPTURESETUP;
									bError = 0;
								}
							}
							break;
						case CLIENT_STATE_STREAMINGSETUP:
							if(recv_socket(lpDevice->clients[i].socket, &StreamInfos, sizeof(StreamInfos))==sizeof(StreamInfos)) {
								lpDevice->clients[i].StreamInfos = StreamInfos;
								lpDevice->clients[i].state = CLIENT_STATE_STREAMING;
								DBG("PLAYBACK thread client_index=%d, socket=%d, id=%d started (%d Hz, %d channels, %d bits)", i, lpDevice->clients[i].socket, StreamInfos.streamid, StreamInfos.rate, StreamInfos.channels, StreamInfos.bitspersample*8);
								bError = 0;
							} else {
								DBG("Playback stream setup failed");
							}
							break;
						case CLIENT_STATE_STREAMING:
							result = recv_socket(lpDevice->clients[i].socket, &hdr, sizeof(hdr));
							if (result == sizeof(hdr) && hdr.pcm_buffer_size <= A2DPD_BLOCK_SIZE) {
								// Display reception delay
								struct timeval now;
								gettimeofday(&now, NULL);
								timersub(&now, &hdr.packet_date, &now);
								//DBG("delay at recv_socket: %d µs", (int)now.tv_usec);

								char *pcm_buffer = mymalloc(hdr.pcm_buffer_size);
								if(pcm_buffer) {
									int result2 = recv_socket(lpDevice->clients[i].socket, pcm_buffer, hdr.pcm_buffer_size);
									if (result2 == hdr.pcm_buffer_size) {
										// Rate conversion
										convert_rate(lpDevice, &lpDevice->clients[i], pcm_buffer, result2, &lpDevice->clients[i].StreamInfos, &hdr);
										bError = 0;
									} else {
										DBG("Receive stream failed (%d/%d)", result2, hdr.pcm_buffer_size);
									}
									safefree(pcm_buffer);
								} else {
									DBG("Couldn't alloc buffer %d", hdr.pcm_buffer_size);
								}
							} else {
								DBG("Did not receive pkt_hdr");
							}
							break;
						case CLIENT_STATE_CAPTURESETUP:
							if(recv_socket(lpDevice->clients[i].socket, &StreamInfos, sizeof(StreamInfos))==sizeof(StreamInfos)) {
								lpDevice->clients[i].StreamInfos = StreamInfos;
								lpDevice->clients[i].state = CLIENT_STATE_CAPTURE;
								DBG("CAPTURE thread %d.%d started (%d Hz, %d channels, %d bits)", i, lpDevice->clients[i].socket, StreamInfos.rate, StreamInfos.channels, StreamInfos.bitspersample*8);
								bError = 0;
							} else {
								DBG("Capture stream setup failed");
							}
							break;
						case CLIENT_STATE_CAPTURE:
							// Read ring buffer data and write to client
							DBG("Capturing");
							tmpcapture = NULL;
							tmpcapturelen = 0;
							pthread_mutex_lock(&lpDevice->capture_mutex);

							if (lpDevice->ring_in != lpDevice->ring_out) {

								tmpcapture = lpDevice->ring[lpDevice->ring_out].buf;
								tmpcapturelen = lpDevice->ring[lpDevice->ring_out].len;

								DBG("Dequeue sco incoming stream %d", lpDevice->ring_out);

								// Move to next ring
								int next_ring = ((lpDevice->ring_out + 1) % MAXCLIENTSRINGSIZE);
								lpDevice->ring_out = next_ring;
							}
							pthread_mutex_unlock(&lpDevice->capture_mutex);

							if(recv_socket(lpDevice->clients[i].socket, &dummy, sizeof(dummy))==sizeof(dummy)) {
								DBG("RECEIVED sync from plugin");
								if(send_socket(lpDevice->clients[i].socket, &tmpcapturelen, sizeof(tmpcapturelen)) == sizeof(tmpcapturelen)) {
									if(tmpcapture != NULL) {
										if(send_socket(lpDevice->clients[i].socket, tmpcapture, tmpcapturelen) == tmpcapturelen) {
											bError = 0;
											DBG("Wrote stream %d", tmpcapturelen);
										} else {
											DBG("Couldn't write capture stream");
										}
										safefree(tmpcapture);
									} else {
										DBG("No capture sent");
									}
								} else {
									DBG("Couldn't send");
								}