linux-bluetooth.vger.kernel.org archive mirror
 help / color / mirror / Atom feed
* [Bluez-devel] [patch]
@ 2006-11-07 16:20 Frédéric DALLEAU
  2006-11-08  3:41 ` Brad Midgley
  0 siblings, 1 reply; 10+ messages in thread
From: Frédéric DALLEAU @ 2006-11-07 16:20 UTC (permalink / raw)
  To: BlueZ development

[-- Attachment #1: Type: text/plain, Size: 503 bytes --]

Hi brad,

Attached is my latest patch from a2dp,
It features :
- tests for an integrated resampler mainly if you don't have alsa,
- better plugin that will finally let alsa do all the resampling which 
gives the best quality and should work with any incoming stream.
- removed the realtime priority (could lock computer if spinning)
- Some A2DP protocol mods (AVDTP_START and AVDTP_STOP handled and 
AVDTP_CLOSE)
- Longer connection timeout

Certainly other things that I don't remember!

BR, Frederic


[-- Attachment #2: patch_btsco_resample.patch --]
[-- Type: text/x-patch, Size: 71029 bytes --]

? .deps
? .libs
? Doxyfile
? Makefile
? Makefile.in
? a2play
? a2recv
? aclocal.m4
? autom4te.cache
? avrecv
? avsnd
? btsco
? btsco.kdevelop
? btsco.kdevelop.pcs
? btsco.kdevses
? btsco2
? compile
? config.guess
? config.h
? config.h.in
? config.log
? config.status
? config.sub
? configure
? depcomp
? install-sh
? libtool
? missing
? stamp-h1
? alsa-plugins/.deps
? alsa-plugins/.libs
? alsa-plugins/Makefile
? alsa-plugins/Makefile.in
? alsa-plugins/a2dp_ipc.lo
? alsa-plugins/a2dp_timer.lo
? alsa-plugins/a2dpd
? alsa-plugins/ctl_a2dpd.lo
? alsa-plugins/ctl_sco.lo
? alsa-plugins/libasound_module_ctl_a2dpd.la
? alsa-plugins/libasound_module_ctl_sco.la
? alsa-plugins/libasound_module_pcm_a2dp.la
? alsa-plugins/libasound_module_pcm_a2dpd.la
? alsa-plugins/libasound_module_pcm_sco.la
? alsa-plugins/pcm_a2dp.lo
? alsa-plugins/pcm_a2dpd.lo
? alsa-plugins/pcm_sco.lo
? alsa-plugins/headsetd/.deps
? alsa-plugins/headsetd/.libs
? alsa-plugins/headsetd/Makefile
? alsa-plugins/headsetd/Makefile.in
? alsa-plugins/headsetd/headsetd
? avdtp/.deps
? avdtp/.libs
? avdtp/Makefile
? avdtp/Makefile.in
? avdtp/avtest
? sbc/.deps
? sbc/.libs
? sbc/Makefile
? sbc/Makefile.in
? sbc/rcplay
? sbc/sbcdec
? sbc/sbcenc
? sbc/sbcinfo
Index: a2dp.h
===================================================================
RCS file: /cvsroot/bluetooth-alsa/btsco/a2dp.h,v
retrieving revision 1.10
diff -u -r1.10 a2dp.h
--- a2dp.h	5 Aug 2006 20:55:11 -0000	1.10
+++ a2dp.h	7 Nov 2006 15:41:23 -0000
@@ -220,10 +220,14 @@
 #define AVDTP_DISCOVER 1
 #define AVDTP_GET_CAPABILITIES 2
 #define AVDTP_SET_CONFIGURATION 3
+#define AVDTP_GET_CONFIGURATION 4
+#define AVDTP_RECONFIGURE 5
 #define AVDTP_OPEN 6
 #define AVDTP_START 7
 #define AVDTP_CLOSE 8
 #define AVDTP_SUSPEND 9
+#define AVDTP_ABORT 0xA
+#define AVDTP_SECURITY_CONTROL 0xB
 
 #define MEDIA_TRANSPORT_CATEGORY 1
 #define MEDIA_CODEC 7
Index: alsa-plugins/Makefile.am
===================================================================
RCS file: /cvsroot/bluetooth-alsa/btsco/alsa-plugins/Makefile.am,v
retrieving revision 1.9
diff -u -r1.9 Makefile.am
--- alsa-plugins/Makefile.am	26 Oct 2006 16:21:39 -0000	1.9
+++ alsa-plugins/Makefile.am	7 Nov 2006 15:41:23 -0000
@@ -27,7 +27,7 @@
 libasound_module_ctl_sco_la_LIBADD = @ALSA_LIBS@
 
 bin_PROGRAMS = a2dpd
-a2dpd_SOURCES = a2dpd.c a2dplib.c alsalib.c
+a2dpd_SOURCES = a2dpd.c a2dplib.c alsalib.c resample.c
 a2dpd_CFLAGS = $(AM_CFLAGS)
 #a2dp_timer.c a2dp_ipc.c
 a2dpd_LDADD = a2dp_timer.o a2dp_ipc.o @ALSA_LIBS@ @BLUEZ_LIBS@ -lsbc -lpthread -lrt
Index: alsa-plugins/a2dp_ipc.c
===================================================================
RCS file: /cvsroot/bluetooth-alsa/btsco/alsa-plugins/a2dp_ipc.c,v
retrieving revision 1.4
diff -u -r1.4 a2dp_ipc.c
--- alsa-plugins/a2dp_ipc.c	6 Sep 2006 02:59:43 -0000	1.4
+++ alsa-plugins/a2dp_ipc.c	7 Nov 2006 15:41:23 -0000
@@ -100,6 +100,8 @@
 
         if(sockfd>0)
         {
+        	int on = 1;
+        	setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on));
                 if(bind(sockfd, (struct sockaddr *)&my_addr, sizeof(struct sockaddr))==0)
                 {
                         if(listen(sockfd, 0)==0)
@@ -223,6 +225,7 @@
 {
         int found=0, error=0;
         FILE* hFile = fopen(filename, "rt");
+	returnbuffer[0] = 0;
         //printf("read_config_string: reading %s\n", filename);
         if(hFile)
         {
@@ -285,7 +288,7 @@
                 strncpy(returnbuffer, defvalue, buffersize);
                 returnbuffer[buffersize-1]=0;
         }
-        syslog(LOG_INFO, "%s [%s] '%s'='%s'", __FUNCTION__, section, key, returnbuffer);
+        //syslog(LOG_INFO, "%s [%s] '%s'='%s'", __FUNCTION__, section, key, returnbuffer);
 }
 
 int read_config_int(char* filename, char* section, char* key, int defvalue)
Index: alsa-plugins/a2dpd.c
===================================================================
RCS file: /cvsroot/bluetooth-alsa/btsco/alsa-plugins/a2dpd.c,v
retrieving revision 1.9
diff -u -r1.9 a2dpd.c
--- alsa-plugins/a2dpd.c	22 Sep 2006 17:34:38 -0000	1.9
+++ alsa-plugins/a2dpd.c	7 Nov 2006 15:41:24 -0000
@@ -42,15 +42,15 @@
 #include "a2dp_timer.h"
 #include "a2dp_ipc.h"
 #include "../avrcp.h"
+#include "resample.h"
 
-#define BLUETOOTHSOUNDFIFOSIZE (16*1024)
-#define MAXBLUETOOTHDEVICES    3
-#define MAXCLIENTSPERDEVICE    8
-#define MAXCLIENTSRINGSIZE     64
-#define POOLENTRYSIZE          A2DPD_BLOCK_SIZE
+#define MAXBLUETOOTHDEVICES    (3)
+#define MAXCLIENTSPERDEVICE    (8)
+#define MAXCLIENTSRINGSIZE     (32)
+#define POOLENTRYSIZE          (A2DPD_BLOCK_SIZE)
 #define PIDFILE                "/var/run/a2dp.pid"
 #define UINPUT_DEVICE          "/dev/input/uinput"
-#define A2DPD_CONFIG_FILE       ".a2dpdrc"
+#define A2DPD_CONFIG_FILE      ".a2dpdrc"
 
 static char g_sOutputFilename[512];
 static char g_srcfilename[512];
@@ -65,6 +65,55 @@
 static int g_brereadconfig = 0;
 static int g_breversestereo = 0;
 
+#define CHECKVAL ((uint32_t)0xFDFDFDFD)
+
+void* mymalloc(int size)
+{
+	char* buffer = malloc(size+8);
+	
+	if(buffer)
+	{
+		*((uint32_t*)buffer) = ((uint32_t)size);
+		buffer+=4;
+		*((uint32_t*)(buffer+size)) = CHECKVAL;
+	}
+	return buffer;
+}
+
+void myfree(void* p, int line)
+{
+	char* buffer = p;
+	if(buffer)
+	{
+		uint32_t size  = *((uint32_t*)(buffer-4));
+		uint32_t check = *((uint32_t*)(buffer+size));
+		if(check != CHECKVAL || size>2048)
+			printf("buffer overflow line %d (size=%d check=%X)\n", line, size, check);
+		buffer-=4;
+		free(buffer);
+	}
+}
+
+int checkbuffer__(void* p, int line)
+{
+	int result = 0;
+	char* buffer = p;
+	if(buffer)
+	{
+		uint32_t size  = *((uint32_t*)(buffer-4));
+		uint32_t check = *((uint32_t*)(buffer+size));
+		if(check != CHECKVAL || size>2048)
+		{
+			printf("buffer failed check line %d (size=%d check=%X)\n", line, size, check);
+			result = 1;
+		}
+	}
+	return result;
+}
+
+#define safefree(buf) do { if(buf) { myfree(buf, __LINE__); (buf) = NULL; } } while (0)
+#define checkbuffer(buf) checkbuffer__(buf, __LINE__)
+
 // This function is needed to destroy zombies processes
 // On Unix, any forked process which terminate before its parent create a zombie until parent call waitpid()
 // We do not want to wait as we just need to "fire and forget" processes
@@ -110,7 +159,7 @@
 			if (fd > 2)
 				(void) close(fd);
 		} else {
-			printf("a2dpd: Couldn't redirect output to '%s' (errno=%d:%s)", output_file_name, errno, strerror(errno));
+			perror("a2dpd: Couldn't redirect output");
 		}
 	}
 
@@ -142,15 +191,14 @@
 	header->packet_type = PACKET_TYPE_SINGLE;
 }
 
-
 static int init_uinput()
 {
 	int fd, i;
 	struct uinput_user_dev dev = {
 		.id = {
-		       .bustype = BUS_BLUETOOTH,
-		       .version = 0x0001,
-		       }
+		.bustype = BUS_BLUETOOTH,
+		.version = 0x0001,
+		}
 	};
 
 	if ((fd = open(UINPUT_DEVICE, O_WRONLY)) < 0) {
@@ -175,9 +223,9 @@
 	uinput_fd = fd;
 
 	return 0;
-      release:
+release:
 	ioctl(fd, UI_DEV_DESTROY);
-      shutdown:
+shutdown:
 	close(fd);
 	return 1;
 }
@@ -274,7 +322,7 @@
 		}
 	} else {
 		if (errno != EAGAIN)
-			printf("socket %d: Receive failed %d (error %d:%s)\n", sockfd, iReceived, errno, strerror(errno));
+			perror("AVRCP Receive failed");
 	}
 
 	return iReceived;
@@ -294,24 +342,27 @@
 
 #define max(x,y) ((x)>(y)?(x):(y))
 
-char *pool_pop()
-{
-	return malloc(POOLENTRYSIZE);
-}
+// Data used to mix audio
+typedef struct {
+	void* lpVoid;
+	uint32_t index_to_construct;
+	uint32_t index_0;
+	uint32_t size;
+} CONVERTBUFFER;
 
-void pool_push(char *pool)
-{
-	free(pool);
-}
+typedef struct {
+	int len;
+	char* buf;
+} RINGINFO;
 
 // Data used to mix audio
 typedef struct {
 	int lives;
 	pthread_mutex_t mutex;
+	CONVERTBUFFER conv;
 	int ring_in;
 	int ring_out;
-	int ring_len[MAXCLIENTSRINGSIZE];
-	char *ring[MAXCLIENTSRINGSIZE];
+	RINGINFO ring[MAXCLIENTSRINGSIZE];
 } BTA2DPPERCLIENTDATA;
 
 // Data to keep per Bluetooth device
@@ -324,6 +375,10 @@
 	AUDIOMIXERDATA mixer;
 	int nb_clients;
 	int bredirectalsa;
+	int a2dp_rate;
+	int a2dp_channels;
+	int a2dp_bitspersample;
+	int sbcbitpool;
 	BTA2DPPERCLIENTDATA clients[MAXCLIENTSPERDEVICE];
 } BTA2DPPERDEVICEDATA, *LPBTA2DPPERDEVICEDATA;
 
@@ -338,7 +393,7 @@
 LPBTA2DPPERDEVICEDATA bta2dpdevicenew(char *addr)
 {
 	int i = 0;
-	LPBTA2DPPERDEVICEDATA lpDevice = malloc(sizeof(BTA2DPPERDEVICEDATA));
+	LPBTA2DPPERDEVICEDATA lpDevice = mymalloc(sizeof(BTA2DPPERDEVICEDATA));
 	if (lpDevice) {
 		memset(lpDevice, 0, sizeof(BTA2DPPERDEVICEDATA));
 		strncpy(lpDevice->addr, addr, sizeof(lpDevice->addr));
@@ -364,7 +419,7 @@
 			pthread_mutex_destroy(&lpDevice->clients[i].mutex);
 		}
 		pthread_mutex_destroy(&lpDevice->mutex);
-		free(lpDevice);
+		safefree(lpDevice);
 	}
 }
 
@@ -385,141 +440,437 @@
 	}
 }
 
-// This function handles a client
-void *client_handler(void *param)
+// This function append data received from a client to the device ring buffer
+void append_to_ring_buffer(BTA2DPPERCLIENTDATA* lpClientData, CONVERTBUFFER* lpConvert)
 {
-	int bError = 0;
-	int client_index = -1;
-	int32_t client_type = INVALID_CLIENT_TYPE;
-	int result;
-	LPA2DPDCLIENT lpClient = (LPA2DPDCLIENT) param;
-	AUDIOMIXERDATA AudioMixerData = INVALIDAUDIOMIXERDATA;
+	if(lpConvert->lpVoid != NULL) {
+		// Enqueue in bluetooth headset if we can else loose packet
+		pthread_mutex_lock(&lpClientData->mutex);
+	
+		// Append data to ring
+		int this_ring = lpClientData->ring_in;
+		int next_ring = ((this_ring + 1) % MAXCLIENTSRINGSIZE);
+
+		if (next_ring != lpClientData->ring_out) {
+			lpClientData->ring[this_ring].buf = lpConvert->lpVoid;
+			lpClientData->ring[this_ring].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;
+		}
 
-	// We should not terminate the process if clients are still running
-	iThreadsRunning++;
+		pthread_mutex_unlock(&lpClientData->mutex);
+	}
+	// Reintegrate data in pool if not transmitted via bthandler thread
+	safefree(lpConvert->lpVoid);
+}
 
-	pthread_detach(lpClient->thread);
+// 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);
+		}
+	}
 
-	setup_socket(lpClient->sockfd);
+	// 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;
+		}
+	}
+}
 
-	// Receive type of client
-	result = recv_socket(lpClient->sockfd, &client_type, sizeof(client_type));
+// 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)
+{
+	// We need this structure accross calls
+	CONVERTBUFFER* lpConvert = &lpClientData->conv;
 
-	// This client wants to send us pcm control data
-	if (client_type == A2DPD_PLUGIN_CTL_WRITE) {
-		printf("CTL WRITE thread %d.%d started\n", client_index, lpClient->sockfd);
+	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);
+				lpConvert->size = POOLENTRYSIZE;
+				/*
+				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))
+				{
+					printf("Buffer overflow: %d,%d\n", lpConvert->index_0+lpConvert->index_to_construct, POOLENTRYSIZE/convert_frame_bytes);
+				}
+				int state=0;
+				int count=0;
+				int total=0;
+				for(i=0; i<lpConvert->size/2; i+=2) {
+					if(state==0) {
+						//printf("%08X | %08X   %d | %d\n", ((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 {
+							//printf("Gap in the data %d,%d\n", count, i);
+							state=0;
+							count=0;
+						}
+					}
+				}
+				if(state==1) {
+					printf("Gap in the data: %d, total=%d\n", count, total);
+				}
+				//exit(0);
+				*/
+
+				// Enqueue in ring buffer
+				append_to_ring_buffer(lpClientData, lpConvert);
 
-		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();
-			int i = send_socket(notifyfd, &AudioMixerData,
-					    sizeof(AudioMixerData));
-			printf("Notify return %d\n", i);
-			close_socket(notifyfd);
+				// Store next index to read
+				pcm_buffer_index_0 = pcm_buffer_index;
+				pcm_buffer_index = pcm_buffer_index_0;
+			}
 		}
+
+		safefree(lpConvertedSample);
 	}
-	// This client wants to read our control status
-	if (client_type == A2DPD_PLUGIN_CTL_READ) {
-		printf("CTL READ thread %d.%d started\n", client_index, lpClient->sockfd);
+}
 
-		pthread_mutex_lock(&lpClient->lpDevice->mutex);
-		AudioMixerData = lpClient->lpDevice->mixer;
-		pthread_mutex_unlock(&lpClient->lpDevice->mutex);
+// This function convert a buffer to sample rate and format needed for device
+void convert_rateX(BTA2DPPERDEVICEDATA* lpDevice, BTA2DPPERCLIENTDATA* lpClientData, void* pcm_buffer, int pcm_buffer_size, AUDIOSTREAMINFOS* lpStreamInfos)
+{
+	// We need this structure accross calls
+	CONVERTBUFFER* lpConvert = &lpClientData->conv;
 
-		send_socket(lpClient->sockfd, &AudioMixerData, sizeof(AudioMixerData));
-	}
-	// This client wants to send us pcm stream
-	if (client_type == A2DPD_PLUGIN_PCM_WRITE) {
-		// Find an index in clients table for the mixer
-		pthread_mutex_lock(&lpClient->lpDevice->mutex);
-		for (client_index = 0; client_index < MAXCLIENTSPERDEVICE; client_index++) {
-			if (lpClient->lpDevice->clients[client_index].lives == 0) {
-				lpClient->lpDevice->clients[client_index].lives = 1;
-				lpClient->lpDevice->clients[client_index].ring_in = 0;
-				lpClient->lpDevice->clients[client_index].ring_out = 0;
-				break;
+	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);
+		int convert_nframes = POOLENTRYSIZE/convert_frame_bytes;
+		ReSampleContext* ctx = audio_resample_init(lpDevice->a2dp_channels, lpStreamInfos->channels, lpDevice->a2dp_rate, lpStreamInfos->rate);
+
+		// We must convert pcm_buffer
+		while(pcm_buffer_index<pcm_buffer_nframes) {
+			printf("Converting: idx=%d, ctx=%d\n", pcm_buffer_index, lpConvert->index_0);
+			int nframes_to_convert = 0;
+			if(lpConvert->lpVoid==NULL) {
+				lpConvert->size = POOLENTRYSIZE;
+				lpConvert->lpVoid = mymalloc(POOLENTRYSIZE);
+				lpConvert->index_0 = 0;
+				lpConvert->index_to_construct = 0;
 			}
+			#define min(x,y) ((x)<(y)?(x):(y))
+			nframes_to_convert = min((convert_nframes-lpConvert->index_0),(pcm_buffer_nframes*lpDevice->a2dp_rate/lpStreamInfos->rate));
+			nframes_to_convert = nframes_to_convert*lpStreamInfos->rate/lpDevice->a2dp_rate;
+			
+			int converted = audio_resample(ctx, lpConvert->lpVoid+(lpConvert->index_0*convert_frame_bytes), pcm_buffer, nframes_to_convert);
+			printf("Converted: %d frames to %d (%d)\n", nframes_to_convert, converted, (nframes_to_convert*lpDevice->a2dp_rate/lpStreamInfos->rate));
+			lpConvert->index_0 += converted;
+			if(lpConvert->index_0 >= convert_nframes) {
+				append_to_ring_buffer(lpClientData, lpConvert);
+			}
+
+			pcm_buffer_index += nframes_to_convert;
 		}
+
+		audio_resample_close(ctx);
+	}
+}
+
+// This function manage volume change wanted by clients
+void a2dpd_plugin_ctl_write(LPA2DPDCLIENT lpClient)
+{
+	AUDIOMIXERDATA AudioMixerData = INVALIDAUDIOMIXERDATA;
+
+	printf("CTL WRITE thread %d started\n", 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);
+	}
+}
 
-		printf("PCM thread %d.%d started\n", client_index, lpClient->sockfd);
+// This function manage volume read for client
+void a2dpd_plugin_ctl_read(LPA2DPDCLIENT lpClient)
+{
+	AUDIOMIXERDATA AudioMixerData = INVALIDAUDIOMIXERDATA;
+	printf("CTL READ thread %d started\n", 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));
+}
+
+// This function manage pcm streams sent by clients
+int a2dpd_plugin_pcm_write(LPA2DPDCLIENT lpClient)
+{
+	int client_index = -1;
+	int bError = 0;
+	AUDIOSTREAMINFOS StreamInfos = INVALIDAUDIOSTREAMINFOS;
 
-		if (client_index >= MAXCLIENTSPERDEVICE) {
-			printf("Client thread %d cannot start (too many clients already connected)\n", client_index);
-			return 0;
+	// Find an index in clients table for the mixer
+	pthread_mutex_lock(&lpClient->lpDevice->mutex);
+	for (client_index = 0; client_index < MAXCLIENTSPERDEVICE; client_index++) {
+		if (lpClient->lpDevice->clients[client_index].lives == 0) {
+			// FIXME Not sure this is safe but this is very unlikely to happen
+			lpClient->lpDevice->clients[client_index].lives = 1;
+			lpClient->lpDevice->clients[client_index].ring_in = 0;
+			lpClient->lpDevice->clients[client_index].ring_out = 0;
+			break;
 		}
+	}
+
+	pthread_mutex_unlock(&lpClient->lpDevice->mutex);
+
+	if (client_index >= MAXCLIENTSPERDEVICE) {
+		perror("Too many clients");
+		return 0;
+	}
+
+	if(recv_socket(lpClient->sockfd, &StreamInfos, sizeof(StreamInfos))==sizeof(StreamInfos))
+	{
+		printf("PCM thread %d.%d started (%d Hz, %d channels, %d bits)\n", client_index, lpClient->sockfd, StreamInfos.rate, StreamInfos.channels, StreamInfos.bitspersample*8);
+
 		// Loop while we receive data
 		while (!bSigINTReceived && !bError) {
 			// Receive data
 			int32_t pcm_buffer_size = 0;
-			result = recv_socket(lpClient->sockfd, &pcm_buffer_size, sizeof(pcm_buffer_size));
-			if (result == sizeof(pcm_buffer_size)
-			    && pcm_buffer_size <= POOLENTRYSIZE) {
-				char *pcm_buffer = pool_pop();
-				result = recv_socket(lpClient->sockfd, pcm_buffer, pcm_buffer_size);
-
-				if (result == pcm_buffer_size) {
-					// Enqueue in bluetooth headset if we can else loose packet
-					pthread_mutex_lock(&lpClient->lpDevice->clients[client_index].mutex);
-
-					// Append data to ring
-					int this_ring = lpClient->lpDevice->clients[client_index].ring_in;
-					int next_ring = ((this_ring + 1) % MAXCLIENTSRINGSIZE);
-
-					if (next_ring != lpClient->lpDevice->clients[client_index].ring_out) {
-						lpClient->lpDevice->clients[client_index].ring[this_ring] = pcm_buffer;
-						lpClient->lpDevice->clients[client_index].ring_len[this_ring] = pcm_buffer_size;
-						lpClient->lpDevice->clients[client_index].ring_in = next_ring;
-						// We will not free that buffer, it's the bthandler thread which will do it
-						pcm_buffer = NULL;
+			int result = recv_socket(lpClient->sockfd, &pcm_buffer_size, sizeof(pcm_buffer_size));
+			if (result == sizeof(pcm_buffer_size) && pcm_buffer_size <= A2DPD_BLOCK_SIZE) {
+				char *pcm_buffer = mymalloc(pcm_buffer_size);
+				if(pcm_buffer) {
+					/*
+					int i;
+					for(i = 0; i<pcm_buffer_size; i++)
+					{
+						pcm_buffer[i]=0xFB;
 					}
-
-					pthread_mutex_unlock(&lpClient->lpDevice->clients[client_index].mutex);
-
-					// Reintegrate data in pool
-					if (pcm_buffer)
-						pool_push(pcm_buffer);
+					*/
+					result = recv_socket(lpClient->sockfd, pcm_buffer, pcm_buffer_size);
+					if (result <= pcm_buffer_size) {
+						// Rate conversion
+						convert_rate(lpClient->lpDevice, &lpClient->lpDevice->clients[client_index], pcm_buffer, result, &StreamInfos);
+					} else {
+						perror("Receiving failed on socket");
+						bError = 1;
+					}
+					/*
+					int state=0;
+					int count=0;
+					int total=0;
+					
+					for(i=0; i<pcm_buffer_size/2; i+=2) {
+						if(state==0) {
+							//printf("%08X | %08X   %d | %d\n", ((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*)pcm_buffer)[i]==(int16_t)0xFAFA) {
+								state=1;
+								count++;
+								total++;
+							} else {
+								state=0;
+							}
+						} else if(state==1) {
+							if(((int16_t*)pcm_buffer)[i]==(int16_t)0xFAFA) {
+								count++;
+								total++;
+							} else {
+								//printf("Gap in the data %d,%d\n", count, i);
+								state=0;
+								count=0;
+							}
+						}
+					}
+					if(state==1) {
+						//printf("Gap in the data: %d, total=%d\n", count, total);
+					}
+					*/
+					safefree(pcm_buffer);
 				} else {
-					printf("[2] Receiving failed on socket %d.%d error (%d/%d bytes)\n", client_index, lpClient->sockfd, result, pcm_buffer_size);
+					perror("Not enough memory");
 					bError = 1;
 				}
 			} else {
 				if (result == sizeof(pcm_buffer_size)) {
-					printf("[1] Receiving will not fit pool (poolentrysize=%d != pcm_buffer_size=%d)\n", POOLENTRYSIZE, pcm_buffer_size);
+					perror("Receiving will not fit pool");
 				} else {
-					printf("[1] Receiving failed on socket %d.%d error (%d/%d bytes) errno=%d:%s\n", client_index, lpClient->sockfd, result, sizeof(pcm_buffer_size), errno,
-					       strerror(errno));
+					perror("Receiving failed");
 				}
 				bError = 1;
 			}
 		}
+	} else {
+		perror("Receiving stream informations failed");
+	}
 
-		pthread_mutex_lock(&lpClient->lpDevice->mutex);
-		if (client_index >= 0)
-			lpClient->lpDevice->clients[client_index].lives = 0;
-		pthread_mutex_unlock(&lpClient->lpDevice->mutex);
+	safefree(lpClient->lpDevice->clients[client_index].conv.lpVoid);
+
+	pthread_mutex_lock(&lpClient->lpDevice->mutex);
+	if (client_index >= 0)
+		lpClient->lpDevice->clients[client_index].lives = 0;
+	pthread_mutex_unlock(&lpClient->lpDevice->mutex);
+
+	printf("Client thread %d ending: %s\n", lpClient->sockfd, (bError ? (errno == EAGAIN ? "timeout" : "error") : "no error"));
+
+	return 0;
+}
+
+// This function handles a client
+void *client_handler(void *param)
+{
+	int32_t client_type = INVALID_CLIENT_TYPE;
+	LPA2DPDCLIENT lpClient = (LPA2DPDCLIENT) param;
+
+	// We should not terminate the process if clients are still running
+	iThreadsRunning++;
+
+	pthread_detach(lpClient->thread);
+
+	setup_socket(lpClient->sockfd);
+
+	// Receive type of client
+	recv_socket(lpClient->sockfd, &client_type, sizeof(client_type));
+
+	// This client wants to send us pcm control data
+	if (client_type == A2DPD_PLUGIN_CTL_WRITE) {
+		a2dpd_plugin_ctl_write(lpClient);
+	}
+	// This client wants to read our control status
+	if (client_type == A2DPD_PLUGIN_CTL_READ) {
+		a2dpd_plugin_ctl_read(lpClient);
 	}
+	// This client wants to send us pcm stream
+	if (client_type == A2DPD_PLUGIN_PCM_WRITE) {
+		a2dpd_plugin_pcm_write(lpClient);
+	}
+
 	// Say goodbye
 	pthread_mutex_lock(&lpClient->lpDevice->mutex);
 	lpClient->lpDevice->nb_clients--;
 	pthread_mutex_unlock(&lpClient->lpDevice->mutex);
 
 	// Close socket
-	printf("Client thread %d.%d ending: %s\n", client_index, lpClient->sockfd, (bError ? (errno == EAGAIN ? "timeout" : "error") : "no error"));
 	close_socket(lpClient->sockfd);
 
 	// Free client data
-	free(lpClient);
+	safefree(lpClient);
 
 	// Decrease thread count
 	iThreadsRunning--;
@@ -587,8 +938,10 @@
 	return pcm_buffer_filed_size;
 }
 
+/////////////////////////////////
 // This function handle the bluetooth connection
 void *bt_handler(void *param)
+/////////////////////////////////
 {
 	int i;
 	// We should not terminate the process if clients are still running
@@ -601,19 +954,24 @@
 	while (!bSigINTReceived) {
 		int bError = 0;
 		int destroy_count = 0;
-		int ibytespersecond = 0;
 
 		// Connect to the A2DP device
 		void *lpA2dp = NULL;
-		char *pcm_buffer = pool_pop();
+		char *pcm_buffer = mymalloc(POOLENTRYSIZE);
 		enum { NOSOUND, SOUND };
 		int state_previous = NOSOUND;
 		TIMERINFO TimerInfos;
-		int rate = read_config_int(g_srcfilename, "a2dpd", "rate",
-					   A2DPD_FRAME_RATE);
+		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;//(read_config_int(g_srcfilename, "a2dpd", "bitspersample", 16))/8;
+		lpDevice->sbcbitpool = read_config_int(g_srcfilename, "a2dpd", "sbcbitpool", 32);
+		printf("New connection to bluetooth [%d hz, %d channels, %d bits]\n", lpDevice->a2dp_rate, lpDevice->a2dp_channels, lpDevice->a2dp_bitspersample*8);
+
+		// 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) rate) * ((float) A2DPD_FRAME_BYTES) / ((float) A2DPD_BLOCK_SIZE)) / 1.0);
-		printf("New connection to bluetooth [%d hz]\n", rate);
+		TimerInfos.fps = (float)(((float) (lpDevice->a2dp_rate*lpDevice->a2dp_channels*lpDevice->a2dp_bitspersample)/((float) POOLENTRYSIZE))/1.0);
 
 		// As long as we can send sound
 		while (!bSigINTReceived && !bError) {
@@ -637,11 +995,11 @@
 
 						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];
-							pcm_buffers_size[i] = lpDevice->clients[i].ring_len[lpDevice->clients[i].ring_out];
+							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;
 							// Tell client we got them
-							lpDevice->clients[i].ring[lpDevice->clients[i].ring_out] = NULL;
-							lpDevice->clients[i].ring_len[lpDevice->clients[i].ring_out] = 0;
+							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);
@@ -670,7 +1028,7 @@
 				for (i = 0; i < MAXCLIENTSPERDEVICE; i++) {
 					if (pcm_buffers[i]) {
 						// Reintegrate data where they come from
-						pool_push(pcm_buffers[i]);
+						safefree(pcm_buffers[i]);
 					}
 				}
 
@@ -679,9 +1037,9 @@
 				/////////////////////////////////
 
 				if (pcm_buffer && pcm_buffer_filed_size > 0) {
-					// Transfer takes place by A2DPD_BLOCK_SIZE bytes blocks
+					// Transfer takes place by POOLENTRYSIZE bytes blocks
 					int blockstart = 0;
-					int blocksize = A2DPD_BLOCK_SIZE;
+					int blocksize = POOLENTRYSIZE;
 
 					// Allocate A2DP if we are not connected
 					if (!lpA2dp) {
@@ -690,10 +1048,21 @@
 						read_config_string(g_srcfilename, "a2dpd", "address", lpDevice->addr, sizeof(lpDevice->addr), "");
 						read_config_string(g_srcfilename, "a2dpd", "alsaoutput", lpDevice->plug, sizeof(lpDevice->plug), "");
 						// Allocate it
-						if (lpDevice->bredirectalsa)
-							lpA2dp = alsa_new(lpDevice->plug, rate);
-						else
-							lpA2dp = a2dp_new(lpDevice->addr, rate);
+						if (lpDevice->bredirectalsa) {
+							lpA2dp = alsa_new(lpDevice->plug, lpDevice->a2dp_rate);
+						} else {
+							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;
+							lpA2dp = a2dp_new(&settings);
+						}
+						// Do not spin if connection failed, this appear if no bluetooth device is installed
+						if(!lpA2dp) {
+							sleep(1);
+						}
 						g_nbdeviceconnected++;
 						destroy_count = 0;
 					}
@@ -703,7 +1072,7 @@
 						while (!bError && blockstart < pcm_buffer_filed_size) {
 							int transfer;
 
-							blocksize = (pcm_buffer_filed_size < A2DPD_BLOCK_SIZE) ? pcm_buffer_filed_size : A2DPD_BLOCK_SIZE;
+							blocksize = (pcm_buffer_filed_size < POOLENTRYSIZE) ? pcm_buffer_filed_size : POOLENTRYSIZE;
 
 							if (lpDevice->bredirectalsa)
 								transfer = alsa_transfer_raw(lpA2dp, pcm_buffer + blockstart, blocksize);
@@ -712,8 +1081,7 @@
 
 							if (transfer >= 0) {
 								destroy_count = 0;
-								blockstart += blocksize;
-								ibytespersecond += transfer;
+								blockstart += transfer;
 								a2dp_timer_notifyframe(&TimerInfos);
 							} else {
 								printf("Error in a2dp_transfer_raw\n");
@@ -750,21 +1118,19 @@
 					}
 				}
 				/*
-				   char* lpszFormat = "A2DPD: [%d,%d|%d,%d] %s %s clients=%d freq=%d[%d b/s] sleep=%d satur=%d\n";
-				   if(satured==0) lpszFormat = "A2DPD: [%d,%d|%d,%d] %s %s clients=%d freq=%d[%d b/s]\n";
-				   printf(lpszFormat, 
-				   lpDevice->mixer.volume_speaker_left,
-				   lpDevice->mixer.volume_speaker_right,
-				   lpDevice->mixer.volume_micro_left,
-				   lpDevice->mixer.volume_micro_right,
-				   (state_current==SOUND)?"playing":"silent",
-				   lpA2dp?"connected":"disconnected", lpDevice->nb_clients, TimerInfos.display,
-				   ibytespersecond,
-				   satured);
-				   // Reset all variables used
-				   ibytespersecond=0;
-				   satured=0;
-				 */
+				char* lpszFormat = "A2DPD: [%d,%d|%d,%d] %s %s clients=%d freq=%d[%d b/s] sleep=%d satur=%d\n";
+				if(satured==0) lpszFormat = "A2DPD: [%d,%d|%d,%d] %s %s clients=%d freq=%d[%d b/s]\n";
+				printf(lpszFormat, 
+				lpDevice->mixer.volume_speaker_left,
+				lpDevice->mixer.volume_speaker_right,
+				lpDevice->mixer.volume_micro_left,
+				lpDevice->mixer.volume_micro_right,
+				(state_current==SOUND)?"playing":"silent",
+				lpA2dp?"connected":"disconnected", lpDevice->nb_clients, TimerInfos.display,
+				satured);
+				// Reset all variables used
+				satured=0;
+				*/
 			}
 
 			// Free the A2DP device if needed
@@ -783,7 +1149,7 @@
 
 			state_previous = state_current;
 		}
-		pool_push(pcm_buffer);
+		safefree(pcm_buffer);
 
 		// Sleep a little bit before retrying
 		if (!bSigINTReceived)
@@ -844,13 +1210,13 @@
 							break;
 						} else if (iReceived < 0) {
 							if (errno != EAGAIN)
-								printf("avdtp: socket %d: Received failed result=%d (errno=%d:%s)\n", new_fd, iReceived, errno, strerror(errno));
+								perror("avdtp: Received failed");
 						}
 						count++;
 					}
 					// AVDTP do not need to have a device connected, since it can establish device connection
 					while (!bSigINTReceived && (iReceived >= 0 || errno == EAGAIN)
-					       && count < 10);
+					&& count < 10);
 					printf("avdtp: socket %d: timed out\n", new_fd);
 					close_socket(new_fd);
 
@@ -859,7 +1225,7 @@
 					}
 				} else {
 					if (errno != EAGAIN) {
-						printf("a2dp_wait_connection failed (AVDTP socket) : %d (errno=%d:%s)\n", new_fd, errno, strerror(errno));
+						perror("avdtp: a2dp_wait_connection failed");
 						break;
 					}
 				}
@@ -896,8 +1262,8 @@
 				uint16_t iMTU = 0;
 
 				int new_fd = a2dp_wait_connection(sockfd, szRemote,
-								  sizeof(szRemote),
-								  &iMTU);
+								sizeof(szRemote),
+								&iMTU);
 
 				if (new_fd > 0) {
 					printf("avrcp: socket %d: Connection from %s, mtu=%d\n", new_fd, szRemote, iMTU);
@@ -905,16 +1271,14 @@
 					setup_socket(new_fd);
 					int iReceived = 0;
 					do {
-						printf("avrcp: socket %d: Reading from %s, mtu=%d\n", new_fd, szRemote, iMTU);
 						errno = 0;
 						iReceived = a2dp_handle_avrcp_message(new_fd);
 					}
-					// AVRCP need device connected
-					while (g_nbdeviceconnected && !bSigINTReceived && (iReceived > 0 || errno == EAGAIN));
+					while (!bSigINTReceived && (iReceived > 0 || errno == EAGAIN));
 					printf("avrcp: socket %d: timed out\n", new_fd);
 					close_socket(new_fd);
 				} else if (errno != EAGAIN) {
-					printf("a2dp_wait_connection failed (AVRCP socket) : %d (errno=%d:%s)\n", new_fd, errno, strerror(errno));
+					perror("avrcp: a2dp_wait_connection failed");
 					break;
 				}
 			}
@@ -953,13 +1317,11 @@
 
 			while (!bSigINTReceived) {
 				int new_fd = -1;
-				printf("main_thread:Accepting incoming tcp stream connection\n");
 				new_fd = accept_socket(sockfd);
-				printf("main_thread: Accepted %d\n", new_fd);
 
 				// Handle connection if it is not the final dummy client
 				if (!bSigINTReceived && new_fd > 0) {
-					LPA2DPDCLIENT lpClient = malloc(sizeof(A2DPDCLIENT));
+					LPA2DPDCLIENT lpClient = mymalloc(sizeof(A2DPDCLIENT));
 					lpClient->lpDevice = lpDevice;
 					lpClient->sockfd = new_fd;
 
@@ -981,7 +1343,7 @@
 			// But we Must wait all client termination
 			// We will pthread_join one day
 			int icount = 0;
-			while (iThreadsRunning > 0 && icount < 30) {
+			while (iThreadsRunning > 0 /*&& icount < 30*/) {
 				printf("A2DPD still %d clients running\n", iThreadsRunning);
 				icount++;
 				sleep(1);
@@ -991,7 +1353,7 @@
 			bta2dpdevicefree(lpDevice);
 			pthread_attr_destroy(&tattr);
 		} else {
-			printf("Error %d: cannot get the socket errno=%d (%s)\n", sockfd, errno, strerror(errno));
+			perror("a2dpd: Cannot get the socket");
 		}
 
 		sleep(1);
@@ -1011,7 +1373,7 @@
 	//char* iphono420= "C2:00:08:F4:30:07:64";
 	//char* hpheadphone= "00:0D:44:2A:17:C7";
 	struct sched_param schedparam = { sched_get_priority_max(SCHED_FIFO) };
-	int res = 0, bFork = 0, bVerbose = 1, bKill = 0, fd = 0;
+	int res = 0, bFork = 0, bVerbose = 1, bKill = 0, fd = 0, bRealtime = 0;
 	FILE *fp;
 	pid_t pid;
 
@@ -1048,6 +1410,8 @@
 			bFork = 0;
 		} else if (!strcmp(argv[i], "+v")) {
 			bVerbose = 0;
+		} else if (!strcmp(argv[i], "-r")) {
+			bRealtime = 1;
 		} else {
 			printf("Parameter not handled: %s\r\n", argv[i]);
 		}
@@ -1076,7 +1440,7 @@
 
 		pid = -1;
 		if ((fscanf(fp, "%d", &pid) != 1) || (pid == getpid())
-		    || (lock_fd(fileno(fp)) == 0)) {
+		|| (lock_fd(fileno(fp)) == 0)) {
 			unlink(PIDFILE);
 		} else {
 			if (bKill) {
@@ -1102,13 +1466,18 @@
 	fflush(fp);
 	fcntl(fd, F_SETFD, (long) 1);
 
-      post_lock:
+post_lock:
 	printf("%s addr=%s timer=%d us [%s %s]\n", argv[0], addr, (int) (timer_resolution.tv_nsec / 1000), __DATE__, __TIME__);
 
 	// If we can be realtime it will be better
-	res = sched_setscheduler(0, SCHED_FIFO, &schedparam);
-	printf("setscheduler returns %d (errno=%d:%s)\n", res, errno, strerror(errno));
-
+	if(bRealtime)
+	{
+		// After some trouble while developping, a2dpd started spining 100%cpu
+		// In realtime, this led me with the only option of rebooting my PC
+		res = sched_setscheduler(0, SCHED_FIFO, &schedparam);
+		if(res != 0)
+			perror("setscheduler failed");
+	}
 	// set up the handler
 	signal(SIGINT, sigint_handler);
 	signal(SIGTERM, sigint_handler);
@@ -1119,12 +1488,12 @@
 	// Run main loop
 	main_loop(addr);
 
-	// global free
+	// global termination
 	a2dp_exit();
 
 	kill_uinput();
 
-      shutdown:
+shutdown:
 	printf("A2DPD terminated succesfully\n");
 
 	return 0;
Index: alsa-plugins/a2dpd_protocol.h
===================================================================
RCS file: /cvsroot/bluetooth-alsa/btsco/alsa-plugins/a2dpd_protocol.h,v
retrieving revision 1.4
diff -u -r1.4 a2dpd_protocol.h
--- alsa-plugins/a2dpd_protocol.h	6 Sep 2006 02:59:43 -0000	1.4
+++ alsa-plugins/a2dpd_protocol.h	7 Nov 2006 15:41:24 -0000
@@ -24,17 +24,35 @@
 
 #include <stdint.h>
 
-
 // parameters used to describe device state
 typedef struct {
 	int16_t volume_speaker_right;
 	int16_t volume_speaker_left;
 	int16_t volume_micro_right;
 	int16_t volume_micro_left;
-} AUDIOMIXERDATA;
+} __attribute__ ((packed)) AUDIOMIXERDATA;
 
 #define INVALIDAUDIOMIXERDATA   { -1, -1, -1, -1 }
 
+// PCM formats defined in alsa, we will restrict our selves to 8 and 16 bits
+#define A2DPD_PCM_FORMAT_UNKNOWN 0x00000000
+#define A2DPD_PCM_FORMAT_S8      0x00000001
+#define A2DPD_PCM_FORMAT_U8      0x00000002
+#define A2DPD_PCM_FORMAT_S16_LE  0x00000003
+//#define A2DPD_FORMAT_S16_BE        0x00000004
+//#define A2DPD_FORMAT_U16_LE        0x00000005
+//#define A2DPD_FORMAT_U16_BE        0x00000006
+
+// parameters used to describe device state
+typedef struct {
+	uint32_t format;
+	uint16_t rate;
+	uint8_t channels;
+	uint16_t bitspersample;
+} __attribute__ ((packed)) AUDIOSTREAMINFOS;
+
+#define INVALIDAUDIOSTREAMINFOS   { 0, 0, 0 }
+
 // Different types of client plugin for the daemon
 #define INVALID_CLIENT_TYPE     0xFFFFFFFF
 #define A2DPD_PLUGIN_CTL_WRITE  0x00000001
Index: alsa-plugins/a2dplib.c
===================================================================
RCS file: /cvsroot/bluetooth-alsa/btsco/alsa-plugins/a2dplib.c,v
retrieving revision 1.6
diff -u -r1.6 a2dplib.c
--- alsa-plugins/a2dplib.c	6 Sep 2006 02:59:43 -0000	1.6
+++ alsa-plugins/a2dplib.c	7 Nov 2006 15:41:24 -0000
@@ -25,12 +25,15 @@
 #include <config.h>
 #endif
 
+// #define FASTTIMEOUTS 1
+
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
 #include <errno.h>
 #include <malloc.h>
 #include <signal.h>
+#include <time.h>
 #include <sys/ioctl.h>
 #include <sys/socket.h>
 #include <sys/time.h>
@@ -62,6 +65,7 @@
 // However some devices may have longer transfer unit up to I saw omtu=733?
 
 #define min(X, Y)  ((X) < (Y) ? (X) : (Y))
+#define max(X, Y)  ((X) > (Y) ? (X) : (Y))
 #define DBG(fmt, arg...) { if(errno!=0) printf("DEBUG: %s: (errno=%d:%s)" fmt "\n" , __FUNCTION__ , errno, strerror(errno), ## arg);\
         else printf("DEBUG: %s: " fmt "\n" , __FUNCTION__ , ## arg); errno=0;}
 
@@ -81,7 +85,7 @@
 
 static struct sigaction actions;
 /*
-sdp_record_t* a2dp_advertise_sdp(sdp_session_t* sdpSessionP)
+static sdp_record_t* a2dp_advertise_sdp(sdp_session_t* sdpSessionP)
 {
         sdp_record_t *recordP=NULL;
         sdp_list_t *svclass=NULL, *rootlist=NULL, *protolist=NULL, *l2caplist=NULL, *avdtplist=NULL, *profileslist=NULL;
@@ -152,9 +156,7 @@
 
         return recordP;
 }
-*/
 
-/*
 void a2dp_init(void) __attribute__ ((constructor));
 void a2dp_exit(void) __attribute__ ((destructor));
 */
@@ -344,26 +346,27 @@
 
 	sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
 	if (sk < 0) {
-		DBG("Can't create socket. %s(%d)", strerror(errno), errno);
+		DBG("Can't create socket.");
 		return -1;
 	}
+#ifdef FASTTIMEOUTS
 	// Set connection timeout
 	struct timeval t = { 3, 0 };
 	setsockopt(sk, SOL_SOCKET, SO_SNDTIMEO, &t, sizeof(t));
 	setsockopt(sk, SOL_SOCKET, SO_RCVTIMEO, &t, sizeof(t));
-
+#endif
 	memset(&addr, 0, sizeof(addr));
 	addr.l2_family = AF_BLUETOOTH;
 	bacpy(&addr.l2_bdaddr, src);
 	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
-		DBG("Can't bind socket. %s(%d)", strerror(errno), errno);
+		DBG("Can't bind socket.");
 		return -1;
 	}
 
 	/* Get default options */
 	opt = sizeof(opts);
 	if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &opt) < 0) {
-		DBG("Can't get default L2CAP options. %s(%d)", strerror(errno), errno);
+		DBG("Can't get default L2CAP options.");
 		return -1;
 	}
 
@@ -373,7 +376,7 @@
 		//opts.imtu = *mtu;
 	}
 	if (setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, opt) < 0) {
-		DBG("Can't set L2CAP options. %s(%d)", strerror(errno), errno);
+		DBG("Can't set L2CAP options.");
 		return -1;
 	}
 
@@ -384,7 +387,9 @@
 
 	tries = 0;
 	while (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
-		DBG("Can't connect to %s on psm %d. %s(%d)", batostr(&addr.l2_bdaddr), psm, strerror(errno), errno);
+		char* tmpaddr = batostr(&addr.l2_bdaddr);
+		DBG("Can't connect to %s on psm %d.", tmpaddr, psm);
+		free(tmpaddr);
 		if (++tries > NBSDPRETRIESMAX) {
 			close(sk);
 			return -1;
@@ -393,7 +398,7 @@
 	}
 	opt = sizeof(opts);
 	if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &opt) < 0) {
-		DBG("Can't get L2CAP options. %s(%d)", strerror(errno), errno);
+		DBG("Can't get L2CAP options.");
 		close(sk);
 		return -1;
 	}
@@ -416,6 +421,7 @@
 	int err;
 	int tries;
 
+#ifdef FASTTIMEOUTS
 	// Try to connect an L2CAP socket to the sdp psm with short timeout for user interaction
 	int tmpsk = do_connect(src, dst, 1, NULL);
 	if (tmpsk > 0) {
@@ -424,10 +430,10 @@
 		DBG("Warning: failed to connect to SDP server");
 		return -1;
 	}
-
+#endif
 	tries = 0;
 	while (!(sess = sdp_connect(src, dst, SDP_RETRY_IF_BUSY))) {
-		DBG("retrying sdp connect: %s", strerror(errno));
+		DBG("retrying sdp connect.");
 		if (++tries > NBSDPRETRIESMAX) {
 			break;
 		}
@@ -451,7 +457,7 @@
 	sdp_list_free(attrid, 0);
 
 	if (err) {
-		DBG("Service Search failed: %s", strerror(errno));
+		DBG("Service Search failed.");
 		sdp_close(sess);
 		return -1;
 	}
@@ -529,7 +535,9 @@
 	int tries, res;
 
 	if (detect_a2dp(src, dst, &psm_cmd, &flags) < 0) {
-		DBG("could not find A2DP services on device %s", batostr(dst));
+		char* tmpaddr=batostr(dst);
+		DBG("could not find A2DP services on device %s", tmpaddr);
+		free(tmpaddr);
 		return -1;
 	} else {
 		DBG("Found A2DP Sink at the destination (psm_cmd=%d)", psm_cmd);
@@ -582,6 +590,11 @@
 	}
 
 	seid = -1;
+	if(size<sizeof(discover_resp.header)) {
+		DBG("Received invalid capabilities (size=%d, wanted=%d)", size, sizeof(discover_resp.header));
+		return -1;
+	}
+
 	nb_seid = (size - sizeof(discover_resp.header)) / sizeof(struct acp_seid_info);
 
 	DBG("received %d capabilities", nb_seid);
@@ -598,6 +611,7 @@
 		DBG("couldn't locate the correct seid");
 		return -1;
 	}
+
 	// open the stream
 	streamfd = do_connect(src, dst, psm_stream, &mtu);
 	if (streamfd < 0) {
@@ -635,6 +649,7 @@
 }
 
 typedef struct snd_pcm_a2dp {
+	A2DPSETTINGS settings;
 	bdaddr_t src;
 	bdaddr_t dst;
 	int sk;
@@ -654,6 +669,10 @@
 	int mtu;		//=A2DPMAXIMUMTRANSFERUNITSIZE
 	int seid;
 
+	// Bluetooth bandwith used
+	int bandwithcount;
+	struct timeval bandwithtimestamp;
+
 	// Used to control stream from headset
 	int stop_writing;	// = 0;
 	int pause_writing;	// = 0;
@@ -680,7 +699,7 @@
 	codesize = a2dp->sbc.subbands * a2dp->sbc.blocks * a2dp->sbc.channels * 2;
 	// 44 bitpool?
 	//codesize=a2dp->sbc.bitpool*a2dp->sbc.subbands*a2dp->sbc.blocks/8;
-	datatoread = min(codesize, pcm_buffer_size);
+	datatoread = min((BUFS - a2dp->lenbufe), pcm_buffer_size);
 
 	// Enqueue data in bufe
 	if (a2dp->lenbufe + datatoread < BUFS) {
@@ -691,6 +710,7 @@
 		datatoread = 0;
 	}
 
+	result = datatoread;
 
 	// If bufe is full, encode
 	if (a2dp->lenbufe >= codesize) {
@@ -721,6 +741,7 @@
 			memcpy(a2dp->buf, &packet_header, sizeof(packet_header));
 			memcpy(a2dp->buf + sizeof(packet_header), &payload_header, sizeof(payload_header));
 			if (a2dp->sk > 0) {
+			/*
 				// Check if data are to be read
 				// Not seen a device showing this yet
 				fd_set readfds;
@@ -733,20 +754,55 @@
 						a2dp_handle_avdtp_message(a2dp, a2dp->sk, NULL, NULL, 0);
 					}
 				}
+			*/
 				// Pause?
-				// The value 0 have never been tested
+				// The value 0 have finally been tested ;)
 				// However, we may safely simulate a failed write
 				if (!a2dp->pause_writing) {
 					// Send our data
 					if ((written = write(a2dp->sk, a2dp->buf, a2dp->len)) != a2dp->len) {
 						// Error while sending data
-						DBG("Wrote %d not %d bytes; (errno=%d:%s)", written, a2dp->len, errno, strerror(errno));
+						DBG("Wrote %d not %d bytes.", written, a2dp->len);
+						/*
+						if (errno == EAGAIN) {
+							usleep(1);
+							if ((written = write(a2dp->sk, a2dp->buf, a2dp->len)) != a2dp->len) {
+								// Error while sending data
+								DBG("Wrote %d not %d bytes. (2)", written, a2dp->len);
+								// Return the error
+								result = written;
+							}
+						}
+						else
+						{
+						}
+						*/
+						// Return the error
 						result = written;
+					} else {
+						// Measure bandwith usage
+						struct timeval now = { 0, 0 };
+						struct timeval interval = { 0, 0 };
+
+						if(a2dp->bandwithtimestamp.tv_sec==0)
+							gettimeofday(&a2dp->bandwithtimestamp, NULL);
+
+						// See if we must wait again
+						gettimeofday(&now, NULL);
+						timersub(&now, &a2dp->bandwithtimestamp, &interval);
+						if(interval.tv_sec>0) {
+							DBG("Bandwith: %d (%d kbps) %d", a2dp->bandwithcount, a2dp->bandwithcount/128, a2dp->sbc.bitpool);
+							a2dp->bandwithtimestamp = now;
+							a2dp->bandwithcount = 0;
+						}
+
+						a2dp->bandwithcount += written;
 					}
-					result = written;
+					
+					
 				} else {
 					// Make the upper layer believe we sent data
-					result = a2dp->len;
+					//result = a2dp->len;
 				}
 			}
 			// Reset buffer of data to send
@@ -762,7 +818,7 @@
 
 	return result;
 }
-
+/*
 static void init_response(struct avdtp_header *header, int response_type)
 {
 	// leave signal_id and transaction label since we are reusing the request
@@ -772,7 +828,7 @@
 	// clear rfa bits
 	header->rfa0 = 0;
 }
-
+*/
 // monitor the control connection for pause/play signals from headset
 // note this signaling is in the avdtp core; avrcp signaling is different
 static void *listen_thread(void *param)
@@ -786,43 +842,22 @@
 
 	DBG("Listen thread running [control_sk=%d]", a2dp->control_sk);
 
+//#ifdef FASTTIMEOUTS
 	// Set a timeout to close thread
 	struct timeval t = { 1, 0 };
 	setsockopt(a2dp->control_sk, SOL_SOCKET, SO_SNDTIMEO, &t, sizeof(t));
 	setsockopt(a2dp->control_sk, SOL_SOCKET, SO_RCVTIMEO, &t, sizeof(t));
+//#endif
 
 	// Loop until end of writing
 	while (!a2dp->stop_writing) {
-		char szBuffer[A2DPMAXIMUMTRANSFERUNITSIZE];
-		struct stream_cmd *cmd = (struct stream_cmd *) szBuffer;
 		if (a2dp_handle_avdtp_message(a2dp, a2dp->control_sk, NULL, NULL, 0) < 0) {
 			// Error
+			//FIXME we must reconnect
 			usleep(100 * 1000);
 		}
-
-		int size = read(a2dp->control_sk, szBuffer, sizeof(szBuffer));
-		if (size > 0) {
-			if (cmd->header.signal_id == AVDTP_SUSPEND) {
-				DBG("Received signal AVDTP_SUSPEND(%d) from set", cmd->header.signal_id);
-				a2dp->pause_writing = 1;
-			} else if (cmd->header.signal_id == AVDTP_START) {
-				DBG("Received signal AVDTP_START(%d) from set", cmd->header.signal_id);
-				a2dp->pause_writing = 0;
-			} else {
-				DBG("Unexpected headset directive %d", cmd->header.signal_id);
-			}
-			// ack the command regardless
-			//FIXME take a shortcut and reuse the command struct (knock one byte off length)
-			init_response(&cmd->header, MESSAGE_TYPE_ACCEPT);
-			if (write(a2dp->control_sk, &cmd, sizeof(cmd) - 1) != sizeof(cmd) - 1) {
-				DBG("Couldn't ack %d", cmd->header.signal_id);
-			}
-		} else {
-			if (errno != EAGAIN)
-				DBG("Error while receiving %d (errno=%d:%s)", size, errno, strerror(errno));
-			if (errno != EINTR)
-				break;
-		}
+		// Make sure we do not spin in case of errors
+		usleep(10 * 1000);
 	}
 
 	return NULL;
@@ -953,41 +988,33 @@
 {
 }
 
-LPA2DP a2dp_new(char *addr, int framerate)
+LPA2DP a2dp_new(A2DPSETTINGS* settings)
 {
 	snd_pcm_a2dp_t *a2dp = NULL;
-	bdaddr_t src, dst;
-	int err;		//, pos = -1, use_rfcomm = 0;
 
-	DBG("%s, %d", addr, framerate);
+	if(settings) {
+		a2dp = a2dp_alloc();
 
-	bacpy(&src, BDADDR_ANY);
-	bacpy(&dst, BDADDR_ANY);
-	str2ba(addr, &dst);
-
-	a2dp = a2dp_alloc();
-	if (!a2dp) {
-		DBG("Can't allocate");
-		return NULL;
-	}
-	if (a2dp)
-		a2dp->sbc.rate = framerate;
+		DBG("%s, %d", settings->bdaddr, settings->framerate);
 
-	bacpy(&a2dp->src, &src);
-	bacpy(&a2dp->dst, &dst);
-	//a2dp->use_rfcomm = use_rfcomm;
-
-	err = a2dp_connect(a2dp);
-	if (err < 0) {
-		DBG("Can't connect");
-		goto error;
+		if (a2dp) {
+			memcpy(&a2dp->settings, settings, sizeof(a2dp->settings));
+			a2dp->sbc.rate = settings->framerate;
+			a2dp->sbc.channels = max(1, min(settings->channels, 2));
+			a2dp->sbc.bitpool = settings->sbcbitpool;
+			if(settings->channels==1)
+				a2dp->sbc.joint=1;
+			bacpy(&a2dp->src, BDADDR_ANY);
+			str2ba(settings->bdaddr, &a2dp->dst);
+
+			if (a2dp_connect(a2dp) < 0) {
+				DBG("Can't connect");
+				a2dp_free(a2dp);
+				a2dp=NULL;
+			}
+		}
 	}
-
 	return a2dp;
-
-      error:
-	a2dp_free(a2dp);
-	return NULL;
 }
 
 void a2dp_destroy(LPA2DP a2dp)
@@ -1005,15 +1032,23 @@
 
 	// the stream-close used to make the iTech headset lock up and require it to be powercycled
 	// should be tested again now that we drain the queue properly
+	//FIXME Should be tested again now that we read the answer, Sonorix used to do something similar and no longer does it!
 
 	init_request(&close_stream.header, AVDTP_CLOSE);
 	close_stream.acp_seid = a2dp->seid;
-	// Use control_sk if it is needed
-	if ((a2dp->control_sk > 0 && (write(a2dp->control_sk, &close_stream, sizeof(close_stream)) != sizeof(close_stream)))
-	    // Else use sk
-	    || (write(a2dp->sk, &close_stream, sizeof(close_stream)) != sizeof(close_stream))
-	    ) {
-		DBG("Couldn't send close_stream (errno=%d:%s)", errno, strerror(errno));
+	if (a2dp->control_sk > 0) {
+		if(write(a2dp->control_sk, &close_stream, sizeof(close_stream)) == sizeof(close_stream)) {
+			// Receive close stream answer if any?
+			int i, size;
+			DBG("Receiving answer to close stream");
+			size = recv(a2dp->control_sk, &close_stream, sizeof(close_stream), 0);
+			DBG("Received answer size=%d", size);
+			for (i = 0; i < size; i++)
+				printf("%02X", (int) (*(((char *) &close_stream) + i)));
+			printf("\n");
+		} else {
+			DBG("Couldn't send close_stream");
+		}
 	}
 
 	a2dp_free(a2dp);
@@ -1065,7 +1100,7 @@
 	}
 
 	if (lpszError) {
-		DBG("%s %s(%d)", lpszError, strerror(errno), errno);
+		DBG("%s", lpszError);
 		close(sockfd);
 		sockfd = -1;
 	}
@@ -1104,9 +1139,13 @@
 		//DBG("Connected [imtu %d, omtu %d, flush_to %d]", opts.imtu, opts.omtu, opts.flush_to);
 
 		if (szRemote) {
-			strncpy(szRemote, batostr(&addr.l2_bdaddr), iRemoteSize);
+			char* tmpaddr = batostr(&addr.l2_bdaddr);
+			strncpy(szRemote, tmpaddr, iRemoteSize);
+			free(tmpaddr);
 			szRemote[iRemoteSize - 1] = '\0';
 		}
+	} else {
+		sleep(1);
 	}
 	return new_fd;
 }
@@ -1135,8 +1174,7 @@
 			}
 			printf("\n");
 			result = 0;
-		} else if ((pkt_hdr->message_type == MESSAGE_TYPE_ACCEPT) && (pkt_hdr->signal_id == sent_packet->signal_id)
-		    ) {
+		} else if ((pkt_hdr->message_type == MESSAGE_TYPE_ACCEPT) && (pkt_hdr->signal_id == sent_packet->signal_id)) {
 			// Got expected answer
 			memcpy(answer, lpFrame, answer_size > iReceived ? answer_size : iReceived);
 			result = iReceived;
@@ -1148,15 +1186,35 @@
 		// Reply to the packet by rejecting it
 		if (pkt_hdr->message_type == MESSAGE_TYPE_COMMAND) {
 			int accepted = 0;
-			if (a2dp && pkt_hdr->signal_id == AVDTP_SUSPEND) {
-				DBG("Received signal AVDTP_SUSPEND(%d) from set", pkt_hdr->signal_id);
-				a2dp->pause_writing = 1;
-				accepted = 1;
-			} else if (a2dp && pkt_hdr->signal_id == AVDTP_START) {
+			if (pkt_hdr->signal_id == AVDTP_DISCOVER) {
+				DBG("Received signal AVDTP_DISCOVER(%d) from set", pkt_hdr->signal_id);
+			} else if (pkt_hdr->signal_id == AVDTP_GET_CAPABILITIES) {
+				DBG("Received signal AVDTP_GET_CAPABILITIES(%d) from set", pkt_hdr->signal_id);
+			} else if (pkt_hdr->signal_id == AVDTP_SET_CONFIGURATION) {
+				DBG("Received signal AVDTP_SET_CONFIGURATION(%d) from set", pkt_hdr->signal_id);
+			} else if (pkt_hdr->signal_id == AVDTP_GET_CONFIGURATION) {
+				DBG("Received signal AVDTP_GET_CONFIGURATION(%d) from set", pkt_hdr->signal_id);
+			} else if (pkt_hdr->signal_id == AVDTP_RECONFIGURE) {
+				DBG("Received signal AVDTP_RECONFIGURE(%d) from set", pkt_hdr->signal_id);
+			} else if (pkt_hdr->signal_id == AVDTP_OPEN) {
+				DBG("Received signal AVDTP_OPEN(%d) from set", pkt_hdr->signal_id);
+			} else if (pkt_hdr->signal_id == AVDTP_START) {
 				DBG("Received signal AVDTP_START(%d) from set", pkt_hdr->signal_id);
-				a2dp->pause_writing = 0;
+				if(a2dp)
+					a2dp->pause_writing = 0;
 				accepted = 1;
-			} else {
+			} else if (pkt_hdr->signal_id == AVDTP_CLOSE) {
+				DBG("Received signal AVDTP_CLOSE(%d) from set", pkt_hdr->signal_id);
+			} else if (pkt_hdr->signal_id == AVDTP_SUSPEND) {
+				DBG("Received signal AVDTP_SUSPEND(%d) from set", pkt_hdr->signal_id);
+				if(a2dp)
+					a2dp->pause_writing = 1;
+				accepted = 1;
+			} else if (pkt_hdr->signal_id == AVDTP_ABORT) {
+				DBG("Received signal AVDTP_ABORT(%d) from set", pkt_hdr->signal_id);
+			} else if (pkt_hdr->signal_id == AVDTP_SECURITY_CONTROL) {
+				DBG("Received signal AVDTP_SECURITY_CONTROL(%d) from set", pkt_hdr->signal_id);
+			}  else {
 				DBG("Unexpected headset directive %d", pkt_hdr->signal_id);
 			}
 
@@ -1167,8 +1225,8 @@
 			wrresult = write(sockfd, pkt_hdr, sizeof(*pkt_hdr));
 
 			if (wrresult != sizeof(*pkt_hdr)) {
-				DBG("FAILED Answering command packet (msgtype=%s,signal=%d) wrresult=%d/%d (errno=%d:%s)", accepted ? "MESSAGE_TYPE_ACCEPT" : "MESSAGE_TYPE_REJECT", pkt_hdr->signal_id,
-				    wrresult, sizeof(*pkt_hdr), errno, strerror(errno));
+				DBG("FAILED Answering command packet (msgtype=%s,signal=%d) wrresult=%d/%d", accepted ? "MESSAGE_TYPE_ACCEPT" : "MESSAGE_TYPE_REJECT", pkt_hdr->signal_id,
+				    wrresult, sizeof(*pkt_hdr));
 			}
 		} else {
 			DBG("Read non command packet (msgtype=%d,signal=%d)", pkt_hdr->message_type, pkt_hdr->signal_id);
@@ -1176,7 +1234,7 @@
 	} else {
 		result = iReceived;
 		if (errno != EAGAIN)
-			printf("socket %d: Receive failed %d (errno=%d:%s)\n", sockfd, iReceived, errno, strerror(errno));
+			printf("socket %d: Receive failed %d\n", sockfd, iReceived);
 	}
 
 	return result;
Index: alsa-plugins/a2dplib.h
===================================================================
RCS file: /cvsroot/bluetooth-alsa/btsco/alsa-plugins/a2dplib.h,v
retrieving revision 1.4
diff -u -r1.4 a2dplib.h
--- alsa-plugins/a2dplib.h	17 Aug 2006 14:06:27 -0000	1.4
+++ alsa-plugins/a2dplib.h	7 Nov 2006 15:41:24 -0000
@@ -33,7 +33,6 @@
 
 #define A2DPMAXIMUMTRANSFERUNITSIZE 610
 
-
 // To send one L2CAP packets of 678 bytes, 4 ACL packets are sent, 3 are 192 bytes long, 
 // 1 contains 49 bytes  => loss 192-49/4
 // To send one L2CAP packets of 610 bytes, 3 ACL packets are sent, 2 are 192 bytes long, 
@@ -46,7 +45,15 @@
 extern void a2dp_exit( void);
 
 // Connect to an a2dp provider
-extern LPA2DP a2dp_new( char* bdaddr, int framerate);
+typedef struct
+{
+	char bdaddr[32];
+	int framerate;
+	int channels;
+	int sbcbitpool;
+} A2DPSETTINGS;
+
+extern LPA2DP a2dp_new( A2DPSETTINGS* settings);
 extern void a2dp_destroy( LPA2DP a2dp);
 
 // compress and transfers data
Index: alsa-plugins/pcm_a2dpd.c
===================================================================
RCS file: /cvsroot/bluetooth-alsa/btsco/alsa-plugins/pcm_a2dpd.c,v
retrieving revision 1.5
diff -u -r1.5 pcm_a2dpd.c
--- alsa-plugins/pcm_a2dpd.c	6 Sep 2006 02:59:43 -0000	1.5
+++ alsa-plugins/pcm_a2dpd.c	7 Nov 2006 15:41:24 -0000
@@ -51,8 +51,6 @@
 #define DBG(fmt, arg...)  printf("DEBUG: %s: (errno=%d:%s)" fmt "\n" , __FUNCTION__ , errno, strerror(errno), ## arg)
 //#define DBG(D...)
 
-static char g_srcfilename[512];
-
 // Signal handler, there is a strange SIGPIPE when the daemon is not running
 // We catch it to not quit
 void sighand(int signo)
@@ -73,6 +71,7 @@
 
 static int a2dp_disconnect(snd_pcm_a2dp_t * a2dp)
 {
+	//syslog(LOG_INFO, "Disconnected a2dp %p, sk %d", a2dp, a2dp->sk);
 	close_socket(a2dp->sk);
 	a2dp->sk = -1;
 	return 0;
@@ -86,8 +85,24 @@
 		if (sockfd > 0) {
 			int32_t client_type = A2DPD_PLUGIN_PCM_WRITE;
 			if (send_socket(sockfd, &client_type, sizeof(client_type)) == sizeof(client_type)) {
-				a2dp->sk = sockfd;
-				syslog(LOG_INFO, "Connected a2dp %p, sk %d", a2dp, a2dp->sk);
+				// Fill stream informations
+				AUDIOSTREAMINFOS StreamInfos = INVALIDAUDIOSTREAMINFOS;
+				StreamInfos.rate = a2dp->rate;
+				StreamInfos.channels = a2dp->channels;
+				StreamInfos.bitspersample = a2dp->frame_bytes/a2dp->channels;
+				switch(a2dp->io.format) {
+				case SND_PCM_FORMAT_S8:     StreamInfos.format = A2DPD_PCM_FORMAT_S8; break;
+				case SND_PCM_FORMAT_U8:     StreamInfos.format = A2DPD_PCM_FORMAT_U8; break;
+				case SND_PCM_FORMAT_S16_LE: StreamInfos.format = A2DPD_PCM_FORMAT_S16_LE; break;
+				default: StreamInfos.format = A2DPD_PCM_FORMAT_UNKNOWN; break;
+				}
+				if (send_socket(sockfd, &StreamInfos, sizeof(StreamInfos)) == sizeof(StreamInfos)) {
+					a2dp->sk = sockfd;
+					syslog(LOG_INFO, "Connected a2dp %p, sk %d, fps %f", a2dp, a2dp->sk, a2dp->TimerInfos.fps);
+				} else {
+					syslog(LOG_WARNING, "Couldn't send stream informations");
+					a2dp_disconnect(a2dp);
+				}
 			} else {
 				close_socket(sockfd);
 				syslog(LOG_WARNING, "Connected a2dp %p, sk %d, Authorisation failed", a2dp, a2dp->sk);
@@ -102,28 +117,18 @@
 static inline snd_pcm_a2dp_t *a2dp_alloc(void)
 {
 	snd_pcm_a2dp_t *a2dp;
-	DBG("Init");
 	a2dp = malloc(sizeof(*a2dp));
-	if (!a2dp)
-		return NULL;
-	memset(a2dp, 0, sizeof(*a2dp));
-	a2dp->sk = -1;
-
-	{
-		get_config_filename(g_srcfilename, sizeof(g_srcfilename));
-		int rate = read_config_int(g_srcfilename, "a2dpd", "rate", A2DPD_FRAME_RATE);
-		a2dp->TimerInfos.fps = (float) ((((float) rate) * ((float) A2DPD_FRAME_BYTES) / ((float) A2DPD_BLOCK_SIZE)) / 1.0);
+	if (a2dp) {
+		memset(a2dp, 0, sizeof(*a2dp));
+		a2dp->sk = -1;
 	}
-	DBG("OK");
 	return a2dp;
 }
 
 static inline void a2dp_free(snd_pcm_a2dp_t * a2dp)
 {
-	DBG("Finishing");
 	a2dp_disconnect(a2dp);
 	free(a2dp);
-	DBG("OK");
 }
 
 static int a2dp_start(snd_pcm_ioplug_t * io)
@@ -180,14 +185,14 @@
 
 // also works but sleeps between transfers
 // This is the main transfer func which does the transfer and sleep job
-static snd_pcm_sframes_t a2dp_transfer_all(snd_pcm_ioplug_t * io, const snd_pcm_channel_area_t * areas, snd_pcm_uframes_t offset, snd_pcm_uframes_t size)
+static snd_pcm_sframes_t a2dp_transfer_all(snd_pcm_ioplug_t * io, const snd_pcm_channel_area_t * areas, snd_pcm_uframes_t offset, snd_pcm_uframes_t nframes)
 {
 	snd_pcm_a2dp_t *a2dp = io->private_data;
 	int i = 0;
 	snd_pcm_sframes_t totaltransfered = 0;
-	while (i++ < 1 && totaltransfered < size) {
+	while (i++ < 1 && totaltransfered < nframes) {
 		char *buf = (char *) areas->addr + (areas->first + areas->step * offset) / 8;
-		int datatoread = min(A2DPD_BLOCK_SIZE, size * a2dp->frame_bytes);
+		int datatoread = min(A2DPD_BLOCK_SIZE, nframes * a2dp->frame_bytes);
 		snd_pcm_sframes_t transfered = a2dp_transfer2(io, buf, datatoread);
 		if (transfered > 0) {
 			offset += transfered;
@@ -212,15 +217,13 @@
 	snd_pcm_a2dp_t *a2dp = io->private_data;
 	unsigned int period_bytes;
 
-	DBG("a2dp %p", a2dp);
-
 	a2dp->frame_bytes = (snd_pcm_format_physical_width(io->format) * io->channels) / 8;
 
 	period_bytes = io->period_size * a2dp->frame_bytes;
 
-	DBG("format %s rate %d channels %d", snd_pcm_format_name(io->format), io->rate, io->channels);
+//	DBG("format %s rate %d channels %d", snd_pcm_format_name(io->format), io->rate, io->channels);
 
-	DBG("frame_bytes %d period_bytes %d period_size %ld buffer_size %ld", a2dp->frame_bytes, period_bytes, io->period_size, io->buffer_size);
+//	DBG("frame_bytes %d period_bytes %d period_size %ld buffer_size %ld", a2dp->frame_bytes, period_bytes, io->period_size, io->buffer_size);
 
 	return 0;
 }
@@ -228,17 +231,20 @@
 static int a2dp_prepare(snd_pcm_ioplug_t * io)
 {
 	snd_pcm_a2dp_t *a2dp = io->private_data;
-	DBG("a2dp %p", a2dp);
+
 	a2dp->num = 0;
 	a2dp->rate = io->rate;
 	a2dp->channels = io->channels;
+
+	a2dp->TimerInfos.fps = (float) ((((float)a2dp->rate) * ((float) a2dp->frame_bytes) / ((float) A2DPD_BLOCK_SIZE)) / 1.0);
+
 	return 0;
 }
 
 static int a2dp_drain(snd_pcm_ioplug_t * io)
 {
-	snd_pcm_a2dp_t *a2dp = io->private_data;
-	DBG("a2dp %p", a2dp);
+//	snd_pcm_a2dp_t *a2dp = io->private_data;
+//	DBG("a2dp %p", a2dp);
 	return 0;
 }
 
@@ -250,7 +256,7 @@
 static int a2dp_descriptors(snd_pcm_ioplug_t * io, struct pollfd *pfds, unsigned int space)
 {
 	if (space < 1) {
-		DBG("Can't fill in descriptors");
+//		DBG("Can't fill in descriptors");
 		SNDERR("Can't fill in descriptors");
 		return 0;
 	}
@@ -291,42 +297,59 @@
 	.poll_revents = a2dp_poll,
 };
 
-// Force alsa to give use the 44100 hz sound
-// Or say alsa we will accept only 44100hz?
+// Alsa can convert about any format/channels/rate to any other rate
+// However, since we added some code in the daemon to convert, why not do it ourselves!!!
+// Moreover some player like aplay won't play a wav file if the device that do not natively support the requested format
+// If you want alsa to do the conversion, just remove the value you want to see converted
 static int a2dp_constraint(snd_pcm_a2dp_t * a2dp)
 {
 	snd_pcm_ioplug_t *io = &a2dp->io;
-	snd_pcm_access_t access_list[] = {
-		SND_PCM_ACCESS_RW_INTERLEAVED,
-		SND_PCM_ACCESS_MMAP_INTERLEAVED,
-	};
-	unsigned int format[2], channel[2], rate[2];
+	#define ARRAY_SIZE(x) (sizeof(x)/sizeof((x)[0]))
+	snd_pcm_access_t access_list[] = { SND_PCM_ACCESS_RW_INTERLEAVED };
+	unsigned int formats[] = { SND_PCM_FORMAT_U8, SND_PCM_FORMAT_S8, SND_PCM_FORMAT_S16_LE };
+	unsigned int channels[] = { 1, 2 };
+	unsigned int rates[] = { 8000, 11025, 22050, 32000, 44100, 48000 };
+	int formats_nb = ARRAY_SIZE(formats);
+	int channels_nb = ARRAY_SIZE(channels);
+	int rates_nb = ARRAY_SIZE(rates);
+	int rate_daemon = 0;
+	int rate_prefered = 0;
+	char srcfilename[512];
 	int err;
 
+	get_config_filename(srcfilename, sizeof(srcfilename));
+	// Default is same as the daemon
+	rate_daemon = read_config_int(srcfilename, "a2dpd", "rate", A2DPD_FRAME_RATE);
+	// If a value is specified, use it
+	rate_prefered = read_config_int(srcfilename, "a2dpd", "plugin-rate", rate_daemon);
+	// If this value is not 0, alsa will convert to plugin-rate
+	if(rate_prefered != 0) {
+		// use defaults settings the rate specified + 16 bits stereo
+		rates[0] = rate_prefered;
+		rates_nb = 1;
+		formats[0] = SND_PCM_FORMAT_S16_LE;
+		formats_nb = 1;
+		channels[0] = 2;
+		channels_nb = 1;
+	} else {
+		// If this value is 0, the daemon will do most conversions
+	}
+
 	syslog(LOG_INFO, "[build %s %s] a2dp %p", __DATE__, __TIME__, a2dp);
 
-	err = snd_pcm_ioplug_set_param_list(io, SND_PCM_IOPLUG_HW_ACCESS, 2, access_list);
+	err = snd_pcm_ioplug_set_param_list(io, SND_PCM_IOPLUG_HW_ACCESS, ARRAY_SIZE(access_list), access_list);
 	if (err < 0)
 		return err;
 
-	format[0] = SND_PCM_FORMAT_S16_LE;
-
-	err = snd_pcm_ioplug_set_param_list(io, SND_PCM_IOPLUG_HW_FORMAT, 1, format);
+	err = snd_pcm_ioplug_set_param_list(io, SND_PCM_IOPLUG_HW_FORMAT, formats_nb, formats);
 	if (err < 0)
 		return err;
 
-	channel[0] = 1;
-	channel[1] = 2;
-
-	err = snd_pcm_ioplug_set_param_list(io, SND_PCM_IOPLUG_HW_CHANNELS, 2, channel);
+	err = snd_pcm_ioplug_set_param_list(io, SND_PCM_IOPLUG_HW_CHANNELS, channels_nb, channels);
 	if (err < 0)
 		return err;
 
-	get_config_filename(g_srcfilename, sizeof(g_srcfilename));
-	rate[0] = read_config_int(g_srcfilename, "a2dpd", "rate", A2DPD_FRAME_RATE);
-	//rate[1] = 48000;
-
-	err = snd_pcm_ioplug_set_param_list(io, SND_PCM_IOPLUG_HW_RATE, 1, rate);
+	err = snd_pcm_ioplug_set_param_list(io, SND_PCM_IOPLUG_HW_RATE, rates_nb, rates);
 	if (err < 0)
 		return err;
 
@@ -347,7 +370,7 @@
 	snd_config_iterator_t i, next;
 	int err = 0;
 
-	DBG("name %s mode %d", name, mode);
+//	DBG("name %s mode %d", name, mode);
 
 	// set up thread signal handler
 	signal(SIGPIPE, sighand);
@@ -375,8 +398,6 @@
 		SNDERR("Can't allocate plugin data");
 		return -ENOMEM;
 	}
-	// Connect
-	a2dp_connect(a2dp);
 
 	// Notify plugin
 	a2dp->io.version = SND_PCM_IOPLUG_VERSION;
Index: alsa-plugins/sample.a2dprc
===================================================================
RCS file: /cvsroot/bluetooth-alsa/btsco/alsa-plugins/sample.a2dprc,v
retrieving revision 1.3
diff -u -r1.3 sample.a2dprc
--- alsa-plugins/sample.a2dprc	6 Sep 2006 02:59:43 -0000	1.3
+++ alsa-plugins/sample.a2dprc	7 Nov 2006 15:41:24 -0000
@@ -2,15 +2,26 @@
 #
 # Rate
 # use 32000 if your headset seems to not support 44100 (HP works well at 44100, Sonorix at 32000)
-# Alsa output may not work depending on your graphics card
-# Very few players supports it (xmms does, but not amarok/gxine engine)
+# However, 44100 is mandatory
 #
 rate=44100
 #rate=32000
+
+#
+# plugin-rate default is the rate used between the plugin and the daemon
+# if this value is 0 then 
+# if this value is not 0 then alsa will convert all stream to the specified rate and then send it to the daemon
+# if this value is 0, then alsa will do no conversion at all, the daemon will do it's own resampling.
+# This "features" is disabled because of the crappy quality of the daemon resampler
+# To test a2dpd resampling from 32000 to 44100 use plugin-rate=32000 and rate=44100
+#plugin-rate=32000
+
+# Recommended
 enablereversestereo=1
 
 #
 # AVRCP Commands to run
+# If these entries are emptied, then some keyboard entry will be sent to /dev/uinput
 #
 cmdplay=xmms --play
 cmdpause=xmms --pause

[-- Attachment #3: Type: text/plain, Size: 373 bytes --]

-------------------------------------------------------------------------
Using Tomcat but need to do more? Need to support web services, security?
Get stuff done quickly with pre-integrated technology to make your job easier
Download IBM WebSphere Application Server v.1.0.1 based on Apache Geronimo
http://sel.as-us.falkag.net/sel?cmd=lnk&kid=120709&bid=263057&dat=121642

[-- Attachment #4: Type: text/plain, Size: 164 bytes --]

_______________________________________________
Bluez-devel mailing list
Bluez-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/bluez-devel

^ permalink raw reply	[flat|nested] 10+ messages in thread
* [Bluez-devel] [PATCH]
@ 2005-12-20 13:54 Filippo Giunchedi
  2005-12-20 18:59 ` Marcel Holtmann
  0 siblings, 1 reply; 10+ messages in thread
From: Filippo Giunchedi @ 2005-12-20 13:54 UTC (permalink / raw)
  To: bluez-devel

Hi,
dbus 0.60 has removed the DBUS_NAME_FLAG_PROHIBIT_REPLACEMENT flag for
dbus_bus_request_name and now that is the default behaviour, the following
patch should work:

--- dbus.c      (revision 116)
+++ dbus.c      (working copy)
@@ -874,7 +874,7 @@
        dbus_connection_set_exit_on_disconnect(connection, FALSE);

        dbus_bus_request_name(connection, BASE_INTERFACE,
-                               DBUS_NAME_FLAG_PROHIBIT_REPLACEMENT, &error);
+                               0, &error);

        if (dbus_error_is_set(&error)) {
                syslog(LOG_ERR, "Can't get system message bus name: %s",


unfortunately I can't find a DBUS_VERSION #define to be backward
compatible with dbus < 0.60


filippo



-------------------------------------------------------
This SF.net email is sponsored by: Splunk Inc. Do you grep through log files
for problems?  Stop!  Download the new AJAX search engine that makes
searching your log files as easy as surfing the  web.  DOWNLOAD SPLUNK!
http://ads.osdn.com/?ad_id=7637&alloc_id=16865&op=click
_______________________________________________
Bluez-devel mailing list
Bluez-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/bluez-devel

^ permalink raw reply	[flat|nested] 10+ messages in thread
end of thread, other threads:[~2006-11-14  3:56 UTC | newest]

Thread overview: 10+ messages (download: mbox.gz follow: Atom feed
-- links below jump to the message on this page --
2006-11-07 16:20 [Bluez-devel] [patch] Frédéric DALLEAU
2006-11-08  3:41 ` Brad Midgley
2006-11-08 10:05   ` Frédéric DALLEAU
2006-11-10 13:18     ` Marcel Hilzinger
2006-11-10 13:34       ` Marcel Hilzinger
2006-11-10 13:49         ` Frédéric DALLEAU
2006-11-10 14:01           ` Marcel Hilzinger
2006-11-14  3:56     ` Brad Midgley
  -- strict thread matches above, loose matches on Subject: below --
2005-12-20 13:54 [Bluez-devel] [PATCH] Filippo Giunchedi
2005-12-20 18:59 ` Marcel Holtmann

This is a public inbox, see mirroring instructions
for how to clone and mirror all data and code used for this inbox;
as well as URLs for NNTP newsgroup(s).