[PATCH] Gateway profile

[event <event.riga@gmail.com>]

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

Hi all,

I've implemented gateway profile. I've tested basic things, like
place/cancel/answer call. Others are in development. Some could not be
tested as my carrier doesn't provide corresponding services (like
3-way call, etc.) so any help welcome.

Vale,
Leonid Movshovich

[-- Attachment #2: gateway.patch --]
[-- Type: application/octet-stream, Size: 48580 bytes --]

Index: audio/audio-api.txt
===================================================================
RCS file: /cvsroot/bluez/utils/audio/audio-api.txt,v
retrieving revision 1.30
diff -w -u -r1.30 audio-api.txt
--- audio/audio-api.txt	29 May 2008 10:41:44 -0000	1.30
+++ audio/audio-api.txt	15 Dec 2008 23:04:41 -0000
@@ -1,6 +1,7 @@
 Bluetooth audio service API description
 ***************************************
 
+Copyright (C) 2008  Leonid Movshovich <event.riga@gmail.com>
 Copyright (C) 2004-2007  Marcel Holtmann <marcel@holtmann.org>
 Copyright (C) 2005-2007  Johan Hedberg <johan.hedberg@nokia.com>
 Copyright (C) 2005-2006  Brad Midgley <bmidgley@xmission.com>
@@ -233,13 +234,102 @@
 org.bluez.audio.Gateway interface
 =================================
 
-[not yet implemented]
-
 This interface is available for remote devices which can function in the Audio
 Gateway role of the HSP and/or HFP profiles.
 
 Object path(s)	/org/bluez/audio/device*
 
+Methods		
+    void Connect()
+	Connect to the AG service on the remote device.
+
+    void Disconnect()
+    boolean IsConnected()
+
+    void AnswerCall()
+         It has to called only after Ring signal received.
+    
+    void CancelCurrentCall()
+         Cancel call which is running now. This one does nothing with any 3-way
+         situation incl. RaH. Just plain old PDH.
+         
+    void Call(string number)
+         Dial a number 'number'. No number processing is done thus if AG would 
+         reject to dial it don't blame me :)
+    
+    string GetOperatorName()
+    
+    void VoiceRecognitionActivate()
+    	in development
+    void VoiceRecognitionDeactivate()
+       	in development
+
+    string GetNumberForLastVoiceTag()
+         Ask AG to provide a phone number. It looks like AG shell in turn 
+         ask user to choose one. Idea behind this is VR on the HF side.
+
+    void SendDTMF(string digits)
+         Will send each digit in the 'digits' sequentially. Would send 
+         nothing if there is non-dtmf digit.
+
+    void SendMicrophoneGain(uint8 gain)
+    void SendSpeakerGain(uint8 gain)
+         Feel them useless but really easy to implement :)
+    
+    string[] GetSubscriberNumbers()
+         Get all the numbers of AG
+
+    void SendAudioData(uint8[] data)
+         Would only be effective when call or VR is running
+         (i.e. SCO link aka Audio Connection exists). Data should be 
+         plain 16 bit LE PCM (e.g. directly from ALSA)
+
+Signals
+    void Connected(uint32 ag_features) 
+         Connection successfully esteblished. 'ag_features' are
+         ORed features:
+              0x001 Three-way calling
+              0x002 Echo cancelling and/or noice reduction function
+              0x004 Voice recognition function
+              0x008 In-band ring tone capability
+              0x010 Attach a number to a voice tag
+              0x020 Ability to reject a call
+              0x040 Enhanced call status
+              0x080 Enhanced call control
+              0x100 Extended Error Result Codes
+
+    void Disconnected()
+
+    void Ring(string number)
+         Someone's calling from 'number'. 
+         Caller number is provided as received from AG.
+
+    void StatusUpdate(string indicator_name, uint32 indicator_value)
+         Indicator 'indicator_name' changed its value to 'indicator_value'. 
+         System (call, callsetup, etc.) statuses are not signalled.
+
+    void CallCancelled()
+         Call failed to set up. It means that we tried to call someone 
+         or someone tried to call us but call was not accepted.
+
+    void CallStart()
+         Call set up successfully.
+
+    void CallEnd()
+         Call was started and now ended. In contrast with CallCancelled 
+         where call didn't started
+
+    void VoiceRecognitionActive()
+    void VoiceRecognitionInactive()
+    	in development
+
+    void MicrophoneGain(uint8 gain) 
+    void SpeakerGain(uint8 gain)
+         AG sent us new gain.
+
+    void AudioData(uint8[] data) 
+         Array of audio data from AG. It is plain 16 bit PCM ready for ALSA.
+         
 
 org.bluez.audio.Sink interface
 ==============================
Index: audio/device.h
===================================================================
RCS file: /cvsroot/bluez/utils/audio/device.h,v
retrieving revision 1.18
diff -w -u -r1.18 device.h
--- audio/device.h	29 May 2008 08:05:16 -0000	1.18
+++ audio/device.h	15 Dec 2008 23:04:41 -0000
@@ -22,6 +22,11 @@
  *
  */
 
+#ifndef __DEVICE_H__
+#define __DEVICE_H__
+
+#include <bluetooth/bluetooth.h>
+
 #define AUDIO_DEVICE_INTERFACE	"org.bluez.audio.Device"
 
 #define GENERIC_AUDIO_UUID	"00001203-0000-1000-8000-00805F9B34FB"
@@ -81,3 +86,5 @@
 uint8_t device_get_state(struct audio_device *dev);
 
 gboolean device_is_connected(struct audio_device *dev, const char *interface);
+
+#endif /*__DEVICE_H__*/
Index: audio/gateway.c
===================================================================
RCS file: /cvsroot/bluez/utils/audio/gateway.c,v
retrieving revision 1.12
diff -w -u -r1.12 gateway.c
--- audio/gateway.c	2 Feb 2008 03:37:05 -0000	1.12
+++ audio/gateway.c	15 Dec 2008 23:04:46 -0000
@@ -4,6 +4,7 @@
  *
  *  Copyright (C) 2006-2007  Nokia Corporation
  *  Copyright (C) 2004-2008  Marcel Holtmann <marcel@holtmann.org>
+ *  Copyright (C) 2008  Leonid Movshovich <event.riga@gmail.org>
  *
  *
  *  This program is free software; you can redistribute it and/or modify
@@ -27,8 +28,945 @@
 #endif
 
 #include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
 
+#include <fcntl.h>
 #include <glib.h>
 #include <dbus/dbus.h>
+#include <gdbus.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
+#include <bluetooth/sco.h>
+#include <bluetooth/rfcomm.h>
+#include <bluetooth/sdp.h>
+#include <bluetooth/sdp_lib.h>
 
 #include "gateway.h"
+#include "error.h"
+#include "glib-helper.h"
+#include "logging.h"
+
+#define SCO_BUF_SIZE 1024
+#define RFCOMM_BUF_SIZE 256
+#define AG_INDICATOR_DESCR_SIZE 20 /* not-more-then-16 defined by GSM + 1 for NULL + padding */
+#define AG_CALLER_NUM_SIZE 64 /* size of number + type */
+
+/* commands */
+#define AG_FEATURES "AT+BRSF=26\r" // = 0x7F = All features supported
+#define AG_INDICATORS_SUPP "AT+CIND=?\r"
+#define AG_INDICATORS_VAL "AT+CIND?\r"
+#define AG_INDICATORS_ENABLE "AT+CMER=3,0,0,1\r"
+#define AG_HOLD_MPTY_SUPP "AT+CHLD=?\r"
+#define AG_CALLER_IDENT_ENABLE "AT+CLIP=1\r"
+#define AG_CARRIER_FORMAT "AT+COPS=3,0\r"
+#define AG_EXTENDED_RESULT_CODE "AT+CMEE=1\r"
+
+#define AG_FEATURE_3WAY 0x1
+#define AG_FEATURE_EXTENDED_RES_CODE 0x100
+/* Hold and multipary AG features. Comments below are copied from hands-free spec for reference */
+#define AG_CHLD_0 0x01 /* Releases all held calls or sets User Determined User Busy (UDUB) for a waiting call */
+#define AG_CHLD_1 0x02 /* Releases all active calls (if any exist) and accepts the other (held or waiting) call */
+#define AG_CHLD_1x 0x04 /* Releases specified active call only <x> */
+#define AG_CHLD_2 0x08 /* Places all active calls (if any exist) on hold and accepts the other (held or waiting) call */
+#define AG_CHLD_2x 0x10 /* Request private consultation mode with specified call <x> (Place all calls on hold EXCEPT the call <x>) */
+#define AG_CHLD_3 0x20 /* Adds a held call to the conversation */
+#define AG_CHLD_4 0x40 /* Connects the two calls and disconnects the subscriber from both calls (Explicit Call Transfer). 
+                          Support for this value and its associated functionality is optional for the HF. */
+#define OK_RESPONSE "\r\nOK\r\n"
+#define ERROR_RESPONSE "\r\nERROR\r\n"
+
+
+struct indicator {
+	gchar descr[AG_INDICATOR_DESCR_SIZE]; 
+	gint value;
+};
+
+struct gateway {
+	GIOChannel* rfcomm;
+	guint rfcomm_watch_id;
+	GIOChannel* sco;
+	GIOChannel* sco_server;
+	DBusMessage* connect_message;
+	guint ag_features;
+	guint hold_multiparty_features;
+	GSList* indies;
+	gboolean is_dialing;
+};
+
+
+static gboolean rfcomm_ag_data_cb(GIOChannel *chan, GIOCondition cond, struct audio_device *device);
+
+static void rfcomm_start_watch(struct audio_device* dev) 
+{
+	struct gateway* gw = dev->gateway;
+	gw->rfcomm_watch_id = g_io_add_watch(gw->rfcomm, G_IO_IN | G_IO_ERR | G_IO_HUP| G_IO_NVAL
+					     , (GIOFunc) rfcomm_ag_data_cb, dev);
+}
+
+static void rfcomm_stop_watch(struct audio_device* dev) 
+{
+	struct gateway* gw = dev->gateway;
+	g_source_remove(gw->rfcomm_watch_id);
+}
+
+static gboolean io_channel_write_all(GIOChannel* io, gchar* data, gsize count) 
+{
+	gsize written = 0;
+	GIOStatus status;
+	while (0 < count) {
+		status =  g_io_channel_write_chars(io, data, count, &written, NULL);
+		if (G_IO_STATUS_NORMAL != status) {
+			return FALSE;
+		}
+		data += written;
+		count -= written;
+	}
+	return TRUE;
+}
+
+/* it's worth to mention that data and response could be the same pointers */
+static gboolean rfcomm_send_and_read(struct gateway* gw, gchar* data, gchar* response, gsize count) 
+{
+	GIOChannel* rfcomm = gw->rfcomm;
+
+	if (!io_channel_write_all(rfcomm, data, count)) {
+		return FALSE;
+	}
+
+	gsize read = 0;
+	gboolean got_ok = FALSE;
+	gboolean got_error = FALSE;
+	gchar* resp_buf = response;
+	gsize toread = RFCOMM_BUF_SIZE - 1;
+	GIOStatus status;
+	while (! (got_ok || got_error)) {
+		status = g_io_channel_read_chars(rfcomm, resp_buf, toread, &read, NULL);
+		if (G_IO_STATUS_NORMAL == status) {
+			resp_buf[read] = '\0';
+		} else {
+			debug("rfcomm_send_and_read(): %m");
+			return FALSE;
+		}
+		got_ok = NULL != strstr(resp_buf, OK_RESPONSE);
+		got_error = NULL != strstr(resp_buf, ERROR_RESPONSE);
+		resp_buf += read;
+		toread -= read;
+	}
+	debug("!!! Data is '%s'", response);
+	return TRUE;
+}
+
+/* get <descr> from the names: (<descr>, (<values>)), (<descr>, (<values>)) ... */
+GSList* parse_indicator_names(gchar* names, GSList* indies) 
+{
+	gchar* current = names - 1;
+	GSList* result = indies;
+	gchar* next;
+	while (NULL != current) {
+		current += 2;
+		next = strstr(current, ",(");
+		struct indicator* ind = (struct indicator*) g_slice_new(struct indicator); 
+		strncpy(ind->descr, current, 20);
+		ind->descr[(gint)next - (gint)current] = '\0';
+		debug("found indicator '%s'", ind->descr);
+		result = g_slist_append(result, (gpointer)ind);
+		debug("parse_indicator_names(): append success");
+		current = strstr(next + 1, ",(");
+		debug("parse_indicator_names(): strstr() success");
+	}
+	debug("parse_indicator_names(): success");
+	return result;
+}
+
+/* get values from <val0>,<val1>,... */
+GSList* parse_indicator_values(gchar* values, GSList* indies) 
+{
+	gint val;
+	gchar* current = values - 1;
+	GSList* runner = indies;
+	while (NULL != current) {
+		current += 1;
+		sscanf(current, "%d", &val);
+		current = strchr(current, ',');
+		struct indicator* ind = (struct indicator*) g_slist_nth_data(runner, 0);
+		ind->value = val;
+		runner = g_slist_next(runner);
+	}
+	return indies;
+}
+
+/* get values from <val0>,<val1>,... */
+static guint get_hold_mpty_features(gchar* features) 
+{
+	guint result = 0;
+	if (NULL != strstr(features, "0")) {
+		result |= AG_CHLD_0;
+	}
+	if (NULL != strstr(features, "1")) {
+		result |= AG_CHLD_1;
+	}
+	if (NULL != strstr(features, "1x")) {
+		result |= AG_CHLD_1x;
+	}
+	if (NULL != strstr(features, "2")) {
+		result |= AG_CHLD_2;
+	}
+	if (NULL != strstr(features, "2x")) {
+		result |= AG_CHLD_2x;
+	}
+	if (NULL != strstr(features, "3")) {
+		result |= AG_CHLD_3;
+	}
+	if (NULL != strstr(features, "4")) {
+		result |= AG_CHLD_4;
+	}
+	return result;
+}
+
+static gboolean establish_service_level_conn(struct gateway* gw) 
+{
+	gchar buf[RFCOMM_BUF_SIZE];
+	gint read;
+	gboolean res;
+	res = rfcomm_send_and_read(gw, AG_FEATURES, buf, sizeof(AG_FEATURES) - 1);
+	if (! res 
+	    || 1 != sscanf(buf, "\r\n+BRSF:%d", &gw->ag_features)) {
+		return FALSE;
+	}
+	debug("features are 0x%X", gw->ag_features);
+	res = rfcomm_send_and_read(gw, AG_INDICATORS_SUPP, buf, sizeof(AG_INDICATORS_SUPP) - 1);
+	if (! res || !strstr(buf, "+CIND:")) {
+		return FALSE;
+	}
+	debug("Got indicators '%s'", buf);
+	gw->indies = parse_indicator_names(strchr(buf, '('), NULL);
+
+	res = rfcomm_send_and_read(gw, AG_INDICATORS_VAL, buf, sizeof(AG_INDICATORS_VAL) - 1);
+	if (! res || !strstr(buf, "+CIND:")) {
+		return FALSE;
+	}
+	debug("Got values for them '%s'", buf);
+	gw->indies = parse_indicator_values(strchr(buf, ':') + 1, gw->indies);
+    
+	res = rfcomm_send_and_read(gw, AG_INDICATORS_ENABLE, buf, sizeof(AG_INDICATORS_ENABLE) - 1);
+	if (! res || !strstr(buf, "OK")) {
+		return FALSE;
+	}
+	if (0 != gw->ag_features & AG_FEATURE_3WAY) {
+		res = rfcomm_send_and_read(gw, AG_HOLD_MPTY_SUPP, buf, sizeof(AG_HOLD_MPTY_SUPP) - 1);
+		if (! res || !strstr(buf, "+CHLD:")) {
+			g_slice_free1(RFCOMM_BUF_SIZE, buf);
+			return FALSE;
+		}
+		gw->hold_multiparty_features = get_hold_mpty_features(strchr(buf, '('));
+	} else {
+		gw->hold_multiparty_features = 0;
+	}
+	debug("CHLD is 0x%X", gw->hold_multiparty_features);
+	debug("Service layer connection successfully established!");
+	rfcomm_send_and_read(gw, AG_CALLER_IDENT_ENABLE, buf, sizeof(AG_CALLER_IDENT_ENABLE) - 1);
+	debug("Caller identification enabled");
+	rfcomm_send_and_read(gw, AG_CARRIER_FORMAT, buf, sizeof(AG_CARRIER_FORMAT) - 1);
+	debug("Carrier format set");
+	if (0 != gw->ag_features & AG_FEATURE_EXTENDED_RES_CODE) {
+		rfcomm_send_and_read(gw, AG_EXTENDED_RESULT_CODE, buf, sizeof(AG_EXTENDED_RESULT_CODE) - 1);
+		debug("Extended result code enabled");
+	}
+	return TRUE;
+}
+
+static void process_ind_change(struct audio_device* dev, gchar* name, gint value) 
+{
+	struct gateway* gw = dev->gateway;
+	if (0 == strcmp(name, "call")) {
+		if (0 < value) {
+			g_dbus_emit_signal(dev->conn, dev->path, AUDIO_GATEWAY_INTERFACE, "CallStart", DBUS_TYPE_INVALID);
+			gw->is_dialing = FALSE;
+		} else {
+			g_dbus_emit_signal(dev->conn, dev->path, AUDIO_GATEWAY_INTERFACE, "CallEnd", DBUS_TYPE_INVALID);
+		}
+	} else if (0 == strcmp(name, "callstatus") 
+		   && gw->is_dialing) {
+		if (0 == value) {
+			g_dbus_emit_signal(dev->conn, dev->path, AUDIO_GATEWAY_INTERFACE, "CallCancelled", DBUS_TYPE_INVALID);
+			gw->is_dialing = FALSE;
+		} else {
+			gw->is_dialing = TRUE;
+		}
+	} else if(0 == strcmp(name, "callheld")) {
+		/* FIXME: The following code is based on assumptions only. Has to be tested for interoperability
+		   I assume that callheld=2 would be sent when dial from HF failed in case of 3-way call 
+		   Unfortunately this path is not covered by the HF spec so the code has to be tested for interop
+		*/
+		if (2 == value) { /* '2' means: all calls held, no active calls */
+			if (gw->is_dialing) {
+				g_dbus_emit_signal(dev->conn, dev->path, AUDIO_GATEWAY_INTERFACE, "CallCancelled", DBUS_TYPE_INVALID);
+				gw->is_dialing == FALSE;
+			}
+		}
+	} else {
+		g_dbus_emit_signal(dev->conn, dev->path, AUDIO_GATEWAY_INTERFACE, "StatusUpdate"
+				   , DBUS_TYPE_STRING, name, DBUS_TYPE_INT32, value, DBUS_TYPE_INVALID);
+	}
+}
+
+static void process_ring(struct audio_device* device, GIOChannel* chan, gchar* buf) 
+{
+	gchar number[AG_CALLER_NUM_SIZE];
+	gchar* sep;
+	gint read;
+    
+	rfcomm_stop_watch(device);
+	g_io_channel_read_chars(chan, buf, RFCOMM_BUF_SIZE - 1, &read, NULL);
+	if (1 == sscanf(buf, "\r\n+CLIP:%s\r\n", number)) {
+		sep = strchr(number, ',');
+		sep[0] = '\0';
+		/* FIXME: signal will be emitted on each RING+CLIP. That's bad */
+		g_dbus_emit_signal(device->conn, device->path, AUDIO_GATEWAY_INTERFACE, "Ring"
+				   , DBUS_TYPE_STRING, number, DBUS_TYPE_INVALID);
+	} else {
+		debug("rfcomm_ag_data_cb(): '%s' in place of +CLIP after RING", buf);
+	}
+	rfcomm_start_watch(device);
+}
+
+static gboolean rfcomm_ag_data_cb(GIOChannel *chan, GIOCondition cond, struct audio_device *device)
+{
+	gchar buf[RFCOMM_BUF_SIZE];
+	struct gateway* gw;
+	gint read;
+	gchar indicator[AG_INDICATOR_DESCR_SIZE + 4]; /* some space for value */
+	gint value;
+	gint result;
+	gchar* sep;
+    
+	if (cond & G_IO_NVAL)
+		return FALSE;
+    
+	gw = device->gateway;
+    
+	if (cond & (G_IO_ERR | G_IO_HUP))
+		return FALSE;
+    
+    
+	if (G_IO_ERROR_NONE != g_io_channel_read_chars(chan, buf, sizeof(buf) - 1, &read, NULL))
+		return TRUE;
+
+	buf[read] = '\0';
+	debug("!!! Callback received: %s", buf);
+	if (1 == sscanf(buf, "\r\n+CIEV:%s\r\n", indicator)) {
+		sep = strchr(indicator, ',');
+		sep[0] = '\0';
+		sep += 1;
+		value = atoi(sep);
+		process_ind_change(device, indicator, value);
+	} else if (NULL != strstr(buf, "RING")){
+		process_ring(device, chan, buf);
+	} else if (1 == sscanf(buf, "\r\n+BVRA:%d\r\n", value)){
+		if (0 == value) {
+			g_dbus_emit_signal(device->conn, device->path, AUDIO_GATEWAY_INTERFACE
+					   , "VoiceRecognitionActive", DBUS_TYPE_INVALID);
+		} else {
+			g_dbus_emit_signal(device->conn, device->path, AUDIO_GATEWAY_INTERFACE
+					   , "VoiceRecognitionInactive", DBUS_TYPE_INVALID);
+		}
+	} else if (1 == sscanf(buf, "\r\n+VGS:%d\r\n", value)){
+		g_dbus_emit_signal(device->conn, device->path, AUDIO_GATEWAY_INTERFACE
+				   , "SpeakerGain", DBUS_TYPE_INT32, value, DBUS_TYPE_INVALID);
+        
+	} else if (1 == sscanf(buf, "\r\n+VGM:%d\r\n", value)){
+		g_dbus_emit_signal(device->conn, device->path, AUDIO_GATEWAY_INTERFACE
+				   , "MicGain", DBUS_TYPE_INT32, value, DBUS_TYPE_INVALID);
+	} else {
+		debug("rfcomm_ag_data_cb(): read wrong data '%s'", buf);
+	}
+	return TRUE;
+}
+
+static gboolean sco_io_cb(GIOChannel *chan, GIOCondition cond,
+                          struct audio_device *dev) 
+{
+	gchar buf[SCO_BUF_SIZE];
+	gint read;
+	struct gateway* gw = dev->gateway;
+
+	if (cond & G_IO_NVAL)
+		return FALSE;
+    
+	if (cond & (G_IO_ERR | G_IO_HUP)) {
+		GIOChannel* chan = gw->sco;
+		debug("SCO connection finished");
+		g_io_channel_unref(chan);
+		g_io_channel_close(chan);
+		gw->sco = NULL;
+		return FALSE;
+	}
+    
+	/* May be here some more data have to be read. Depends on result performance. 
+	   For Nokia E51 volume is 48 bytes per call*/
+	if (G_IO_ERROR_NONE == g_io_channel_read(chan, buf, sizeof(buf) , &read)) {
+//    if (G_IO_ERROR_NONE == g_io_channel_read_chars(chan, buf, sizeof(buf) , &read, NULL)) {
+		gchar* p_buf = buf;
+		g_dbus_emit_signal(dev->conn, dev->path, AUDIO_GATEWAY_INTERFACE, "AudioData"
+				   , DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, &p_buf, read, DBUS_TYPE_INVALID);
+	}
+	return TRUE;
+}
+
+
+static void sco_connect_cb(GIOChannel *chan, int err, const bdaddr_t *src,
+                           const bdaddr_t *dst, gpointer user_data)
+{
+	struct audio_device *dev = (struct audio_device*) user_data;
+	struct gateway* gw = dev->gateway;
+
+	if (0 > err) {
+		error("sco_connect_cb(): %s (%d)", strerror(-err), -err);
+		/* not sure, but from other point of view, 
+		   what is the reason to have headset which cannot play audio? */
+		gateway_close(gw); 
+		return;
+	}
+
+	debug("SCO socket opened for gateway %s", dev->path);
+	gw->sco = chan;
+	fcntl(g_io_channel_unix_get_fd(chan), F_SETFL, 0); /* why is this here? */
+	g_io_add_watch(gw->sco, G_IO_ERR | G_IO_HUP | G_IO_NVAL | G_IO_IN,
+		       (GIOFunc) sco_io_cb, dev);
+}
+
+
+static void rfcomm_connect_cb(GIOChannel *chan, int err, const bdaddr_t *src,
+                              const bdaddr_t *dst, gpointer user_data)
+{
+	struct audio_device* dev = user_data;
+	struct gateway* gw = dev->gateway;
+	DBusMessage* conn_mes = gw->connect_message;
+	gchar gw_addr[18];
+    
+	if (err < 0) {
+		error("connect(): %s (%d)", strerror(-err), -err);
+		return;
+	}
+    
+	ba2str(&dev->dst, gw_addr);
+	/* Blocking mode should be default, but just in case: */
+	GIOFlags flags = g_io_channel_get_flags(chan); 
+	flags &= ~G_IO_FLAG_NONBLOCK; 
+	flags &= G_IO_FLAG_MASK;
+	g_io_channel_set_flags(chan, flags, NULL); 
+	g_io_channel_set_encoding(chan, NULL, NULL);
+	g_io_channel_set_buffered(chan, FALSE);
+	gw->rfcomm = chan;
+	if (establish_service_level_conn(dev->gateway)){
+		GIOChannel* sco_server = bt_sco_listen(&dev->src, sco_connect_cb, dev);
+		if (NULL != sco_server) {
+			debug("%s: Connected to %s", dev->path, gw_addr);
+			rfcomm_start_watch(dev);
+			gw->sco_server = sco_server;
+			DBusMessage* reply = dbus_message_new_method_return(conn_mes); 
+			dbus_connection_send(dev->conn, reply, NULL);
+			dbus_message_unref(reply); 
+			dbus_message_unref(conn_mes); 
+			gw->connect_message = NULL; 
+
+			g_dbus_emit_signal(dev->conn, dev->path, AUDIO_GATEWAY_INTERFACE, "Connected"
+					   , DBUS_TYPE_UINT32, &gw->ag_features, DBUS_TYPE_INVALID);
+			return;
+		} else {
+			debug("%s: Failed to setup SCO server socket", dev->path);
+		}
+	} else {
+		debug("%s: Failed to establish service layer connection to %s", dev->path, gw_addr);
+	}
+	gateway_close(gw);
+}
+
+static void get_record_cb(sdp_list_t *recs, int err, gpointer user_data)
+{
+	struct audio_device *dev = user_data;
+	DBusMessage* conn_mes = dev->gateway->connect_message;
+	int ch = -1;
+	sdp_list_t *protos, *classes = NULL;
+	sdp_record_t *record = NULL;
+	uuid_t uuid;
+
+	if (err < 0) {
+		error("Unable to get service record: %s (%d)", strerror(-err), -err);
+	} else if (NULL == recs || NULL == recs->data) {
+		error("No records found");
+	} else if (sdp_get_service_classes(recs->data, &classes) < 0) {
+		error("Unable to get service classes from record");
+	} else {
+		memcpy(&uuid, classes->data, sizeof(uuid));
+            
+		if (0 == sdp_uuid128_to_uuid(&uuid) || uuid.type != SDP_UUID16) {
+			error("Not a 16 bit UUID");
+		} else if (uuid.value.uuid16 != HANDSFREE_AGW_SVCLASS_ID) {
+			error("Service record didn't contain the HFP UUID");
+		} else if (0 == sdp_get_access_protos(recs->data, &protos)) {
+			ch = sdp_get_proto_port(protos, RFCOMM_UUID);
+			sdp_list_foreach(protos, (sdp_list_func_t) sdp_list_free,
+					 NULL);
+			sdp_list_free(protos, NULL);
+			if (ch == -1) {
+				error("Unable to extract RFCOMM channel from service record");
+			}else { 
+				debug("Connecting on channel %d", ch);
+				if (bt_rfcomm_connect(&dev->src, &dev->dst, ch,
+						      rfcomm_connect_cb, dev) < 0){
+					error("Unable to connect: %s (%s)", strerror(-err), -err);
+					error_connection_attempt_failed(dev->conn, conn_mes, -err); 
+					gateway_close(dev->gateway);
+				} 
+				sdp_list_free(classes, free);
+				sdp_record_free(recs->data);
+				return; 
+			}                
+		}
+	}
+	sdp_list_free(classes, free);
+	sdp_record_free(recs->data);
+	error_not_supported(dev->conn, conn_mes);
+	dbus_message_unref(conn_mes);
+	dev->gateway->connect_message = NULL; 
+}
+
+static int get_records(struct audio_device *device)
+{
+	uuid_t uuid;
+
+	sdp_uuid16_create(&uuid, HANDSFREE_AGW_SVCLASS_ID);
+	return bt_search_service(&device->src, &device->dst, &uuid,
+				 get_record_cb, device, NULL);
+}
+
+
+static DBusMessage* ag_connect(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+	struct audio_device* au_dev = (struct audio_device*) data;
+	struct gateway* gw = au_dev->gateway;
+
+	if (NULL != gw->rfcomm) {
+		return g_dbus_create_error(msg, ERROR_INTERFACE
+					   ".AlreadyConnected",
+					   "Already Connected");
+	} 
+    
+	gw->connect_message = dbus_message_ref(msg); 
+	if (get_records(au_dev) < 0){
+		dbus_message_unref(gw->connect_message);
+		return g_dbus_create_error(msg, ERROR_INTERFACE
+					   ".ConnectAttemptFailed",
+					   "Connect Attempt Failed");
+	}
+	debug("!!! Connection established");
+	return NULL;
+}
+
+static DBusMessage* ag_disconnect(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+	struct audio_device *device = data;
+	struct gateway *gw = device->gateway;
+	DBusMessage* reply = NULL;
+	char gw_addr[18];
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply)
+		return NULL;
+
+	if (NULL == gw->rfcomm)
+		return g_dbus_create_error(msg, ERROR_INTERFACE
+					   ".NotConnected",
+					   "Device not Connected");
+
+	gateway_close(gw);
+	ba2str(&device->dst, gw_addr);
+	debug("Disconnected from %s, %s", gw_addr, device->path);
+
+	return reply;
+
+}
+
+static DBusMessage* ag_is_connected(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+	gboolean res = gateway_is_connected((struct audio_device*)data);
+	DBusMessage* reply = dbus_message_new_method_return(msg);
+	if (NULL != reply) {
+		dbus_message_append_args(reply, DBUS_TYPE_BOOLEAN, &res,
+					 DBUS_TYPE_INVALID);
+	}
+	return reply;
+}
+
+static DBusMessage* process_ag_reponse(DBusMessage* msg, gchar* response) 
+{
+	DBusMessage* reply;
+	if (NULL != strstr(response, OK_RESPONSE)) {
+		reply = dbus_message_new_method_return(msg);
+	} else {
+		/* FIXME: some code should be here to processes errors in better fasion */
+		debug("AG responded with '%s' to %s method call", response, dbus_message_get_member(msg));
+		reply = dbus_message_new_error(msg, ERROR_INTERFACE".OperationFailed", "Operation failed. See log for details");
+	}
+	return reply;
+}
+
+static DBusMessage* process_simple(DBusMessage* msg, struct audio_device* dev, gchar* data) 
+{
+	struct gateway *gw = dev->gateway;
+	gchar buf[RFCOMM_BUF_SIZE];
+
+	rfcomm_stop_watch(dev);
+	rfcomm_send_and_read(gw, data, buf, strlen(data));
+	rfcomm_start_watch(dev);
+	return process_ag_reponse(msg, buf);
+}
+
+#define AG_ANSWER "ATA\r"
+
+static DBusMessage* ag_answer(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+	return process_simple(msg, (struct audio_device*) data, AG_ANSWER);
+}
+
+#define AG_CANCEL_CUR "AT+CHUP\r"
+
+static DBusMessage* ag_cancel_cur(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+	return process_simple(msg, (struct audio_device*) data, AG_CANCEL_CUR);
+}
+
+#define ALLOWED_NUMBER_SYMBOLS "1234567890*#+ABC" /* according to GSM spec */
+#define AG_PLACE_CALL "ATD%s\r"
+/* dialing from memory is not supported as headset spec doesn't define a way
+   to retreive phone memory entries.
+*/
+static DBusMessage* ag_call(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+	struct audio_device *device = data;
+	struct gateway *gw = device->gateway;
+	gchar buf[RFCOMM_BUF_SIZE];
+	gchar* number;
+	gint atd_len;
+    
+	dbus_message_get_args(msg, NULL, DBUS_TYPE_STRING, &number,	DBUS_TYPE_INVALID);
+	if (strlen(number) != strspn(number, ALLOWED_NUMBER_SYMBOLS)) {
+		return dbus_message_new_error(msg, ERROR_INTERFACE".BadNumber"
+					      , "Number contains characters which are not allowed");
+	}
+	atd_len = sprintf(buf, AG_PLACE_CALL, number);
+	rfcomm_stop_watch(device);
+	rfcomm_send_and_read(gw, buf, buf, atd_len);
+	rfcomm_start_watch(device);
+	return process_ag_reponse(msg, buf);
+}
+
+#define AG_GET_CARRIER "AT+COPS?\r"
+
+static DBusMessage* ag_get_operator(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+	struct audio_device* dev = (struct audio_device*) data;
+	struct gateway* gw = dev->gateway;
+	gchar buf[RFCOMM_BUF_SIZE];
+	GIOChannel* rfcomm = gw->rfcomm;
+	gsize read;
+	gchar* result;
+	DBusMessage* reply;
+	GIOStatus status;
+
+	rfcomm_stop_watch(dev);
+	io_channel_write_all(rfcomm, AG_GET_CARRIER, strlen(AG_GET_CARRIER));
+    
+	status = g_io_channel_read_chars(rfcomm, buf, RFCOMM_BUF_SIZE - 1, &read, NULL);
+	rfcomm_start_watch(dev);
+	if (G_IO_STATUS_NORMAL == status) {
+		buf[read] = '\0';
+		if (NULL != strstr(buf, "+COPS")) {
+			result = strrchr(buf, ',') + 1;
+			reply = dbus_message_new_method_return(msg);
+			dbus_message_append_args(reply, DBUS_TYPE_STRING, &result, DBUS_TYPE_INVALID);
+		} else {
+			debug("ag_get_operator(): '+COPS' expected but '%s' received", buf);
+			reply = dbus_message_new_error(msg, ERROR_INTERFACE".Failed", "Unexpected response from AG");
+		}
+	} else {
+		debug("ag_get_operator(): %m");
+		reply = dbus_message_new_error(msg, ERROR_INTERFACE".ConnectionFailed", "Failed to receive response from AG");
+	}
+	return reply;
+}
+
+#define AG_VR_ACTIVATE "AT+BVRA=1\r"
+static DBusMessage* ag_vr_activate(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+	return process_simple(msg, (struct audio_device*)data, AG_VR_ACTIVATE);
+}
+
+#define AG_VR_DEACTIVATE "AT+BVRA=0\r"
+static DBusMessage* ag_vr_deactivate(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+	return process_simple(msg, (struct audio_device*)data, AG_VR_DEACTIVATE);
+}
+
+#define AG_REQUEST_NUMBER "AT+BINP=1\r"
+static DBusMessage* ag_get_number_for_voice(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+	struct audio_device *device = data;
+	struct gateway *gw = device->gateway;
+	gchar buf[RFCOMM_BUF_SIZE];
+	gchar number[AG_CALLER_NUM_SIZE];
+    
+	rfcomm_stop_watch(device);
+	rfcomm_send_and_read(gw, AG_REQUEST_NUMBER, buf, strlen(AG_REQUEST_NUMBER));
+	rfcomm_start_watch(device);
+	if (1 == sscanf(buf, "\r\n+BINP:%s\r\n", number)) {
+		DBusMessage* reply = dbus_message_new_method_return(msg);
+		dbus_message_append_args(reply, DBUS_TYPE_STRING, number, DBUS_TYPE_INVALID);
+		return reply;
+	} else {
+		debug("ag_get_number_for_voice(): AG returned '%s' in place of number", buf);
+		return dbus_message_new_error(msg, ERROR_INTERFACE".Failed", "AG didn't return a number");
+	}
+}
+
+#define AG_SEND_DTMF "AT+VTS:%c\r"
+static DBusMessage* ag_send_dtmf(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+	struct audio_device *device = data;
+	struct gateway *gw = device->gateway;
+	gchar buf[RFCOMM_BUF_SIZE];
+	gchar* number;
+	gint com_len;
+    
+	dbus_message_get_args(msg, NULL, DBUS_TYPE_STRING, &number,	DBUS_TYPE_INVALID);
+	if (strlen(number) != strspn(number, ALLOWED_NUMBER_SYMBOLS)) {
+		return dbus_message_new_error(msg, ERROR_INTERFACE".BadNumber"
+					      , "Number contains characters which are not allowed");
+	}
+	gboolean got_ok = TRUE;
+	gint num_len = strlen(number);
+	gint i = 0;
+	rfcomm_stop_watch(device);
+	while (i < num_len && got_ok) {
+		com_len = sprintf(buf, AG_SEND_DTMF, number[i]);
+		rfcomm_send_and_read(gw, buf, buf, com_len);
+		got_ok = NULL != strstr(buf, OK_RESPONSE);
+		i += 1;
+	}
+	rfcomm_start_watch(device);
+	return process_ag_reponse(msg, buf);
+    
+}
+
+#define AG_SET_MIC_GAIN "AT+VGM:%d\r"
+static DBusMessage* ag_send_mic_gain(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+	gint value;
+	gchar buf[RFCOMM_BUF_SIZE];
+	dbus_message_get_args(msg, NULL, DBUS_TYPE_INT32, &value, DBUS_TYPE_INVALID);
+	sprintf(buf, AG_SET_MIC_GAIN, value);
+	return process_simple(msg, (struct audio_device*)data, buf);
+}
+
+
+#define AG_SET_SPEAKER_GAIN "AT+VGS:%d\r"
+static DBusMessage* ag_send_speaker_gain(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+	gint value;
+	gchar buf[RFCOMM_BUF_SIZE];
+	dbus_message_get_args(msg, NULL, DBUS_TYPE_INT32, &value, DBUS_TYPE_INVALID);
+	sprintf(buf, AG_SET_SPEAKER_GAIN, value);
+	return process_simple(msg, (struct audio_device*)data, buf);
+}
+
+#define AG_GET_SUBSCRIBER_NUMS "AT+CNUM\r"
+#define CNUM_LEN 5 /* length of "+CNUM" string */
+#define MAX_NUMBER_CNT 16
+static DBusMessage* ag_get_subscriber_nums(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+	struct audio_device *device = data;
+	struct gateway *gw = device->gateway;
+	gchar buf[RFCOMM_BUF_SIZE];
+	gchar* cur_cnum;
+	gchar numbers[MAX_NUMBER_CNT][AG_CALLER_NUM_SIZE];
+	int number_cnt = 0;
+
+	rfcomm_stop_watch(device);
+	rfcomm_send_and_read(gw, AG_GET_SUBSCRIBER_NUMS, buf, strlen(AG_GET_SUBSCRIBER_NUMS));
+	rfcomm_start_watch(device);
+	cur_cnum = strstr(buf, "+CNUM");
+	while (NULL != cur_cnum && number_cnt < MAX_NUMBER_CNT) {
+		sscanf(cur_cnum, "+CNUM:%s,", numbers[number_cnt]);
+		number_cnt += 1;
+		cur_cnum = strstr(cur_cnum + CNUM_LEN, "+CNUM");
+	}
+	DBusMessage* reply = dbus_message_new_method_return(msg);
+	dbus_message_append_args(reply, DBUS_TYPE_ARRAY, DBUS_TYPE_STRING, &numbers, number_cnt, DBUS_TYPE_INVALID);
+	return reply;
+}
+
+static DBusMessage* ag_send_audio_data(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+	gchar* audio_data;
+	gint data_byte_cnt;
+	struct audio_device* dev = (struct audio_device*) data;
+	struct gateway* gw = dev->gateway;
+
+	dbus_message_get_args(msg, NULL, DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE
+			      , &audio_data, &data_byte_cnt, DBUS_TYPE_INVALID);
+    
+	if (io_channel_write_all(gw->sco, audio_data, data_byte_cnt)) {
+		return dbus_message_new_method_return(msg);
+	} else {
+		debug("ag_send_audio_data(): %m");
+		return dbus_message_new_error(msg, ERROR_INTERFACE".WriteFailed", "SCO data send failed");
+	}
+}
+
+static DBusMessage* ag_hold_all_but_this(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+
+}
+
+static DBusMessage* ag_cancel_all(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+
+}
+
+static DBusMessage* ag_cancel_spec(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+
+}
+
+static DBusMessage* ag_rah_put_on_hold(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+
+}
+
+static DBusMessage* ag_rah_answer_held_call(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+
+}
+
+static DBusMessage* ag_rah_cancel_held_call(DBusConnection* conn, DBusMessage* msg, void* data) 
+{
+
+}
+
+static GDBusMethodTable gateway_methods[] = {
+	{"Connect", "", "", ag_connect, G_DBUS_METHOD_FLAG_ASYNC}
+	, {"Disconnect", "", "", ag_disconnect}
+	, {"IsConnected", "", "b", ag_is_connected}
+	, {"AnswerCall", "", "", ag_answer}
+	, {"CancelCurrentCall", "", "", ag_cancel_cur}
+	, {"Call", "s", "", ag_call}
+	, {"GetOperatorName", "", "s", ag_get_operator}
+	, {"VoiceRecognitionActvate", "", "", ag_vr_activate}
+	, {"VoiceRecognitionDeactvate", "", "", ag_vr_deactivate}
+	, {"GetNumberForLastVoiceTag", "", "s", ag_get_number_for_voice}
+	, {"SendDTMF", "s", "", ag_send_dtmf}
+	, {"SendMicrophoneGain", "y", "", ag_send_mic_gain}
+	, {"SendSpeakerGain", "y", "", ag_send_speaker_gain}
+	, {"GetSubscriberNumbers", "", "as", ag_get_subscriber_nums}
+
+	, {"RAHPutOnHold", "", "", ag_rah_put_on_hold}
+	, {"RAHAnswerHeldCall", "", "", ag_rah_answer_held_call}
+	, {"RAHCancelHeldCall", "", "", ag_rah_cancel_held_call}
+
+	, {"CancelCallFromNumber", "s", "", ag_cancel_spec}
+	, {"CancelAllCalls", "", "", ag_cancel_all}
+	, {"HoldAllButThis", "s", "", ag_hold_all_but_this}
+
+	, {"SendAudioData", "ay", "", ag_send_audio_data}
+	, {NULL, NULL, NULL, NULL }
+};
+
+static GDBusSignalTable gateway_signals[] = {
+	{"Connected", "u"}
+	, {"Disconnected", ""}
+	, {"Ring", "s"}
+	, {"StatusUpdate", "si"}
+	, {"CallCancelled", ""}
+	, {"CallStart", ""}
+	, {"CallEnd", ""}
+	, {"VoiceRecognitionActive", ""}
+	, {"VoiceRecognitionInactive", ""}
+	, {"MicrophoneGain", "y"}
+	, {"SpeakerGain", "y"}
+	, {"RAHCallOnHold", ""}
+	, {"NoCallsHeld", ""}
+	, {"HoldCalls", ""}
+	, {"AllCallsHeld", ""}
+	, {"AudioData", "ay"}
+	, {NULL, NULL }
+};
+
+gboolean gateway_is_connected(struct audio_device* dev)
+{
+	return (NULL == dev) || (NULL == dev->gateway) || (NULL == dev->gateway->rfcomm);   
+}
+
+struct gateway* gateway_init(struct audio_device* dev)
+{
+	debug("!!! Initializing gateway");
+	if (!g_dbus_register_interface(dev->conn, dev->path,
+				       AUDIO_GATEWAY_INTERFACE,
+				       gateway_methods, gateway_signals, NULL,
+				       dev, NULL)) {
+		return NULL;
+	}
+	struct gateway* gw = g_new0(struct gateway, 1);
+	gw->indies = NULL;
+	gw->is_dialing = FALSE;
+	debug("!!! GW init success");
+	return gw;
+}
+
+
+int gateway_connect_rfcomm(struct audio_device *dev, GIOChannel *io)
+{
+	if (!io)
+		return -EINVAL;
+
+	dev->gateway->rfcomm = io;
+
+	return 0;
+}
+
+static void indicator_slice_free(gpointer mem) 
+{
+	g_slice_free(struct indicator, mem);
+}
+
+int gateway_close(struct gateway* gw)
+{
+	GIOChannel *rfcomm = gw->rfcomm;
+	GIOChannel *sco = gw->sco;
+	GIOChannel *sco_server = gw->sco_server;
+	g_slist_foreach(gw->indies, (GFunc)indicator_slice_free, NULL);
+	g_slist_free(gw->indies);
+	if (rfcomm) {
+		g_io_channel_close(rfcomm);
+		g_io_channel_unref(rfcomm);
+		gw->rfcomm = NULL;
+	}
+  
+	if (sco) {
+		g_io_channel_close(sco);
+		g_io_channel_unref(sco);
+		gw->sco = NULL;
+	}
+
+	if (sco_server) {
+		g_io_channel_close(sco_server);
+		g_io_channel_unref(sco_server);
+		gw->sco_server = NULL;
+	}
+	return 0;
+}
+
Index: audio/gateway.h
===================================================================
RCS file: /cvsroot/bluez/utils/audio/gateway.h,v
retrieving revision 1.9
diff -w -u -r1.9 gateway.h
--- audio/gateway.h	21 Apr 2008 12:49:16 -0000	1.9
+++ audio/gateway.h	15 Dec 2008 23:04:46 -0000
@@ -21,14 +21,19 @@
  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  *
  */
+#ifndef __GATEWAY_H__
+#define __GATEWAY_H__
 
+#include "device.h"
 #define AUDIO_GATEWAY_INTERFACE "org.bluez.audio.Gateway"
 
 #define DEFAULT_HSP_HS_CHANNEL 6
 #define DEFAULT_HFP_HS_CHANNEL 7
 
-int gateway_init(DBusConnection *conn, gboolean disable_hfp, gboolean sco_hci);
+struct gateway* gateway_init(struct audio_device *device);
 
-void gateway_exit(void);
+gboolean gateway_is_connected(struct audio_device* dev);
 
-gboolean gateway_is_enabled(uint16_t svc);
+int gateway_connect_rfcomm(struct audio_device *dev, GIOChannel *chan);
+
+#endif /*__GATEWAY_H__*/
Index: audio/manager.c
===================================================================
RCS file: /cvsroot/bluez/utils/audio/manager.c,v
retrieving revision 1.115
diff -w -u -r1.115 manager.c
--- audio/manager.c	17 Jun 2008 19:37:36 -0000	1.115
+++ audio/manager.c	15 Dec 2008 23:04:52 -0000
@@ -105,12 +105,12 @@
 static uint32_t hsp_ag_record_id = 0;
 static uint32_t hfp_ag_record_id = 0;
 
-static uint32_t hsp_hs_record_id = 0;
+static uint32_t hfp_hs_record_id = 0;
 
 static GIOChannel *hsp_ag_server = NULL;
 static GIOChannel *hfp_ag_server = NULL;
 
-static GIOChannel *hsp_hs_server = NULL;
+static GIOChannel *hfp_hs_server = NULL;
 
 static struct enabled_interfaces enabled = {
 	.headset	= TRUE,
@@ -231,13 +231,13 @@
 		ret = (hsp_ag_server != NULL);
 		break;
 	case HEADSET_AGW_SVCLASS_ID:
-		ret = (hsp_hs_server != NULL);
+		ret = FALSE;
 		break;
 	case HANDSFREE_SVCLASS_ID:
 		ret = (hfp_ag_server != NULL);
 		break;
 	case HANDSFREE_AGW_SVCLASS_ID:
-		ret = FALSE;
+		ret = (hfp_hs_server != NULL);
 		break;
 	case AUDIO_SINK_SVCLASS_ID:
 		return enabled.sink;
@@ -284,6 +284,9 @@
 		break;
 	case HANDSFREE_AGW_SVCLASS_ID:
 		debug("Found Handsfree AG record");
+		if (device->gateway == NULL) {
+			device->gateway = gateway_init(device);
+		}
 		break;
 	case AUDIO_SINK_SVCLASS_ID:
 		debug("Found Audio Sink");
@@ -500,6 +503,15 @@
 			return NULL;
 
 		headset = TRUE;
+	} else if (!strcmp(uuid, HFP_HS_UUID)) {
+		if (device->gateway) {
+			return device;
+		}
+
+		device->gateway = gateway_init(device);
+		if (NULL == device->gateway) {
+			return NULL;
+		}
 	} else if (!strcmp(uuid, A2DP_SOURCE_UUID)) {
 		if (device->sink)
 			return device;
@@ -516,9 +528,9 @@
 
 		if (!device->control)
 			return NULL;
-	} else
+	} else {
 		return NULL;
-
+	}
 	path = device->path;
 
 	if (created) {
@@ -886,6 +898,8 @@
 
 	if (enabled.headset && strstr(value, "headset"))
 		device->headset = headset_init(device, NULL, 0);
+	if (enabled.gateway && strstr(value, "gateway"))
+		device->gateway = gateway_init(device);
 	if (enabled.sink && strstr(value, "sink"))
 		device->sink = sink_init(device);
 	if (enabled.control && strstr(value, "control"))
@@ -1029,7 +1043,7 @@
 	return record;
 }
 
-static sdp_record_t *hsp_hs_record(uint8_t ch)
+static sdp_record_t *hfp_hs_record(uint8_t ch)
 {
 	sdp_list_t *svclass_id, *pfseq, *apseq, *root;
 	uuid_t root_uuid, svclass_uuid, ga_svclass_uuid;
@@ -1047,13 +1061,13 @@
 	root = sdp_list_append(0, &root_uuid);
 	sdp_set_browse_groups(record, root);
 
-	sdp_uuid16_create(&svclass_uuid, HEADSET_SVCLASS_ID);
+	sdp_uuid16_create(&svclass_uuid, HANDSFREE_SVCLASS_ID);
 	svclass_id = sdp_list_append(0, &svclass_uuid);
 	sdp_uuid16_create(&ga_svclass_uuid, GENERIC_AUDIO_SVCLASS_ID);
 	svclass_id = sdp_list_append(svclass_id, &ga_svclass_uuid);
 	sdp_set_service_classes(record, svclass_id);
 
-	sdp_uuid16_create(&profile.uuid, HEADSET_PROFILE_ID);
+	sdp_uuid16_create(&profile.uuid, HANDSFREE_SVCLASS_ID);
 	profile.version = 0x0100;
 	pfseq = sdp_list_append(0, &profile);
 	sdp_set_profile_descs(record, pfseq);
@@ -1150,7 +1164,7 @@
 	return record;
 }
 
-static void auth_cb(DBusError *derr, void *user_data)
+static void headset_auth_cb(DBusError *derr, void *user_data)
 {
 	struct audio_device *device = user_data;
 	const char *uuid;
@@ -1217,7 +1231,7 @@
 		goto drop;
 	}
 
-	err = service_req_auth(&device->src, &device->dst, uuid, auth_cb,
+	err = service_req_auth(&device->src, &device->dst, uuid, headset_auth_cb,
 				device);
 	if (err < 0) {
 		debug("Authorization denied: %s", strerror(-err));
@@ -1235,10 +1249,57 @@
 	return;
 }
 
-static void hs_io_cb(GIOChannel *chan, int err, const bdaddr_t *src,
+static void gateway_auth_cb(DBusError *derr, void *user_data)
+{
+	struct audio_device *device = user_data;
+
+	if (derr && dbus_error_is_set(derr)) {
+		error("Access denied: %s", derr->message);
+		if (dbus_error_has_name(derr, DBUS_ERROR_NO_REPLY)) {
+			debug("Canceling authorization request");
+			service_cancel_auth(&device->src, &device->dst);
+		}
+	} else {
+		char hs_address[18];
+
+		headset_set_authorized(device);
+
+		ba2str(&device->dst, hs_address);
+
+		debug("Accepted headset connection from %s for %s", hs_address, device->path);
+	}
+}
+
+static void hf_io_cb(GIOChannel *chan, int err, const bdaddr_t *src,
 		const bdaddr_t *dst, void *data)
 {
-	/*Stub*/
+	const char *uuid;
+	struct audio_device *device;
+	gboolean hfp_active;
+
+	if (err < 0) {
+		error("accept: %s (%d)", strerror(-err), -err);
+		return;
+	}
+
+	uuid = HFP_HS_UUID;
+
+	device = manager_device_connected(dst, uuid);
+	if (!device) {
+		debug("Device creation failed");
+	} else if (gateway_is_connected(device)) {
+		debug("Refusing new connection since one already exists");
+	} else if (service_req_auth(&device->src, 
+				    &device->dst, uuid, gateway_auth_cb, device) < 0) {
+		debug("Authorization denied: %s", strerror(-err));
+	} else if (gateway_connect_rfcomm(device, chan) < 0){
+		error("Allocating new GIOChannel failed!");
+	} else {
+		return;
+	}
+
+	g_io_channel_close(chan);
+	g_io_channel_unref(chan);
 	return;
 }
 
@@ -1332,22 +1393,19 @@
 
 static int gateway_server_init(DBusConnection *conn, GKeyFile *config)
 {
-	uint8_t chan = DEFAULT_HSP_HS_CHANNEL;
+	uint8_t chan = DEFAULT_HFP_HS_CHANNEL;
 	sdp_record_t *record;
 	gboolean master = TRUE;
 	GError *err = NULL;
 	uint32_t flags;
 
-	if (!enabled.gateway)
-		return 0;
-
 	if (config) {
 		gboolean tmp;
 
 		tmp = g_key_file_get_boolean(config, "General", "Master",
 						&err);
 		if (err) {
-			debug("audio.conf: %s", err->message);
+			debug("Gateway: audio.conf: %s", err->message);
 			g_error_free(err);
 			err = NULL;
 		} else
@@ -1358,13 +1416,13 @@
 
 	if (master)
 		flags |= RFCOMM_LM_MASTER;
-
-	hsp_hs_server = bt_rfcomm_listen(BDADDR_ANY, chan, flags, hs_io_cb,
+	debug("!!! Start listen for rfcomm");
+	hfp_hs_server = bt_rfcomm_listen(BDADDR_ANY, chan, flags, hf_io_cb,
 				NULL);
-	if (!hsp_hs_server)
+	if (!hfp_hs_server)
 		return -1;
 
-	record = hsp_hs_record(chan);
+	record = hfp_hs_record(chan);
 	if (!record) {
 		error("Unable to allocate new service record");
 		return -1;
@@ -1373,12 +1431,12 @@
 	if (add_record_to_server(BDADDR_ANY, record) < 0) {
 		error("Unable to register HSP HS service record");
 		sdp_record_free(record);
-		g_io_channel_unref(hsp_hs_server);
-		hsp_hs_server = NULL;
+		g_io_channel_unref(hfp_hs_server);
+		hfp_hs_server = NULL;
 		return -1;
 	}
 
-	hsp_hs_record_id = record->handle;
+	hfp_hs_record_id = record->handle;
 
 	return 0;
 }
@@ -1395,14 +1453,14 @@
 		hsp_ag_server = NULL;
 	}
 
-	if (hsp_hs_record_id) {
-		remove_record_from_server(hsp_hs_record_id);
-		hsp_hs_record_id = 0;
+	if (hfp_hs_record_id) {
+		remove_record_from_server(hfp_hs_record_id);
+		hfp_hs_record_id = 0;
 	}
 
-	if (hsp_hs_server) {
-		g_io_channel_unref(hsp_hs_server);
-		hsp_hs_server = NULL;
+	if (hfp_hs_server) {
+		g_io_channel_unref(hfp_hs_server);
+		hfp_hs_server = NULL;
 	}
 
 	if (hfp_ag_record_id) {
@@ -1423,8 +1481,9 @@
 
 	connection = dbus_connection_ref(conn);
 
-	if (!config)
+	if (!config) {
 		goto proceed;
+	}
 
 	list = g_key_file_get_string_list(config, "General", "Enable",
 						NULL, NULL);
@@ -1463,7 +1522,6 @@
 		if (headset_server_init(conn, config) < 0)
 			goto failed;
 	}
-
 	if (enabled.gateway) {
 		if (gateway_server_init(conn, config) < 0)
 			goto failed;
Index: common/glib-helper.c
===================================================================
RCS file: /cvsroot/bluez/utils/common/glib-helper.c,v
retrieving revision 1.39
diff -w -u -r1.39 glib-helper.c
--- common/glib-helper.c	30 Jun 2008 14:05:38 -0000	1.39
+++ common/glib-helper.c	15 Dec 2008 23:04:57 -0000
@@ -41,6 +41,7 @@
 #include <glib.h>
 
 #include "glib-helper.h"
+#include "logging.h"
 
 typedef int (*resolver_t) (int fd, char *src, char *dst);
 typedef BtIOError (*connect_t) (BtIO *io, BtIOFunc func);
@@ -727,38 +728,68 @@
 	return 0;
 }
 
-static BtIOError sco_connect(BtIO *io, BtIOFunc func)
-{
-	struct io_context *io_ctxt = io->io_ctxt;
+static BtIOError sco_bind(struct io_context *io_ctxt, const char *address) {
 	struct sockaddr_sco addr;
-	int sk, err;
-
-	io_ctxt->func = func;
-
-	sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO);
-	if (sk < 0)
-		return -errno;
+	BtIOError err;
+        int sco_fd;
 
-	memset(&addr, 0, sizeof(addr));
+	sco_fd = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO);
+	if (sco_fd < 0) {
+                return BT_IO_FAILED;
+        }
+        debug("sco_bind(): socket %d created", sco_fd);
 	addr.sco_family = AF_BLUETOOTH;
-	str2ba(io->src, &addr.sco_bdaddr);
+	str2ba(address, &addr.sco_bdaddr);
 
-	err = bind(sk, (struct sockaddr *) &addr, sizeof(addr));
+	err = bind(sco_fd, (struct sockaddr *) &addr, sizeof(addr));
 	if (err < 0) {
-		close(sk);
+                close(sco_fd);
 		return BT_IO_FAILED;
 	}
+        debug("sco_bind(): socket %d bind", sco_fd);
+        io_ctxt->fd = sco_fd;
+	return BT_IO_SUCCESS;
+}
+
+static BtIOError sco_listen(BtIO *io, BtIOFunc func) {
+	struct io_context *io_ctxt = io->io_ctxt;
+	BtIOError err;
+
+	io_ctxt->func = func;
+	err = sco_bind(io_ctxt, io->src);
+	if (err != BT_IO_SUCCESS) {
+                debug("sco_listen(): %m");
+                return err;
+        }
 
-	io_ctxt->fd = sk;
+	err = transport_listen(io);
+        debug("sco_listen(): %s", strerror(-err));
+	if (err < 0) {
+                close(io_ctxt->fd);
+                return err;
+	}
+	return BT_IO_SUCCESS;
+}
+
+static BtIOError sco_connect(BtIO *io, BtIOFunc func)
+{
+	struct io_context *io_ctxt = io->io_ctxt;
+	struct sockaddr_sco addr;
+	int sk, err;
+
+	io_ctxt->func = func;
+
+	err = sco_bind(io_ctxt, io->src);
+	if (err < 0)
+		return err;
 
-	memset(&addr, 0, sizeof(addr));
 	addr.sco_family = AF_BLUETOOTH;
 	str2ba(io->dst, &addr.sco_bdaddr);
 
 	err = transport_connect(io, (struct sockaddr *) &addr,
 				sizeof(addr));
 	if (err < 0) {
-		close(sk);
+		close(io_ctxt->fd);
 		return BT_IO_FAILED;
 	}
 
@@ -1020,6 +1051,25 @@
 
 }
 
+GIOChannel* bt_sco_listen(const bdaddr_t *src, bt_io_callback_t cb, void *user_data)
+{
+	BtIO *io;
+	BtIOError err;
+        
+	io = bt_io_create(BT_IO_SCO, user_data, NULL);
+	if (!io)
+                return NULL;
+	ba2str(src, io->src);
+	io->io_ctxt->cb = cb;
+	err = bt_io_listen(io, NULL, NULL);
+	if (err != BT_IO_SUCCESS) {
+                bt_io_unref(io);
+                return NULL;
+	}
+        debug("Returning %p", io->io_ctxt->io);
+	return io->io_ctxt->io;
+}
+
 int bt_rfcomm_connect(const bdaddr_t *src, const bdaddr_t *dst,
 			uint8_t channel, bt_io_callback_t cb, void *user_data)
 {
@@ -1144,6 +1194,7 @@
 		err = create_io_context(&io->io_ctxt, NULL, NULL,
 				sco_resolver, user_data);
 		io->connect = sco_connect;
+		io->listen = sco_listen;
 		break;
 	default:
 		return NULL;
Index: common/glib-helper.h
===================================================================
RCS file: /cvsroot/bluez/utils/common/glib-helper.h,v
retrieving revision 1.21
diff -w -u -r1.21 glib-helper.h
--- common/glib-helper.h	30 Jun 2008 14:05:38 -0000	1.21
+++ common/glib-helper.h	15 Dec 2008 23:04:58 -0000
@@ -57,6 +57,7 @@
 int bt_sco_connect(const bdaddr_t *src, const bdaddr_t *dst,
 			bt_io_callback_t cb, void *user_data);
 
+GIOChannel* bt_sco_listen(const bdaddr_t *src, bt_io_callback_t cb, void *user_data);
 /* Experiemental bt_io API */
 
 typedef struct bt_io BtIO;