bluez5: add support for HSP/HFP hardware volume control

Only native backend is fulfilled.

spa/plugins/bluez5/backend-native.c

@@ -77,6 +77,7 @@ struct impl {
 };
 
 struct transport_data {
+	struct rfcomm *rfcomm;
 	struct spa_source sco;
 };
 
@@ -86,10 +87,25 @@ enum hfp_hf_state {
 	hfp_hf_cind1,
 	hfp_hf_cind2,
 	hfp_hf_cmer,
-	hfp_hf_slc,
+	hfp_hf_slc1,
+	hfp_hf_slc2,
+	hfp_hf_vgs,
+	hfp_hf_vgm,
 	hfp_hf_bcs
 };
 
+enum hsp_hs_state {
+	hsp_hs_init1,
+	hsp_hs_init2,
+	hsp_hs_vgs,
+	hsp_hs_vgm,
+};
+
+struct rfcomm_volume {
+	bool active;
+	int hw_volume;
+};
+
 struct rfcomm {
 	struct spa_list link;
 	struct spa_source source;
@@ -101,6 +117,8 @@ struct rfcomm {
 	enum spa_bt_profile profile;
 	struct spa_source timer;
 	char* path;
+	bool has_volume;
+	struct rfcomm_volume volumes[SPA_BT_VOLUME_ID_TERM];
 #ifdef HAVE_BLUEZ_5_BACKEND_HFP_NATIVE
 	unsigned int slc_configured:1;
 	unsigned int codec_negotiation_supported:1;
@@ -109,6 +127,7 @@ struct rfcomm {
 	unsigned int hfp_ag_switching_codec:1;
 	unsigned int hfp_ag_initial_codec_setup:2;
 	enum hfp_hf_state hf_state;
+	enum hsp_hs_state hs_state;
 	unsigned int codec;
 #endif
 };
@@ -148,6 +167,7 @@ static struct spa_bt_transport *_transport_create(struct rfcomm *rfcomm)
 {
 	struct impl *backend = rfcomm->backend;
 	struct spa_bt_transport *t = NULL;
+	struct transport_data *td;
 	char* pathfd;
 
 	if ((pathfd = spa_aprintf("%s/fd%d", rfcomm->path, rfcomm->source.fd)) == NULL)
@@ -165,6 +185,23 @@ static struct spa_bt_transport *_transport_create(struct rfcomm *rfcomm)
 	t->n_channels = 1;
 	t->channels[0] = SPA_AUDIO_CHANNEL_MONO;
 
+	td = t->user_data;
+	td->rfcomm = rfcomm;
+
+	for (int i = 0; i < SPA_BT_VOLUME_ID_TERM ; ++i) {
+		rfcomm->volumes[i].hw_volume = SPA_BT_VOLUME_INVALID;
+		t->volumes[i].active = rfcomm->volumes[i].active;
+		t->volumes[i].hw_volume_max = SPA_BT_VOLUME_HS_MAX;
+	}
+
+	if (t->profile & SPA_BT_PROFILE_HEADSET_AUDIO_GATEWAY) {
+		t->volumes[SPA_BT_VOLUME_ID_RX].volume = DEFAULT_AG_VOLUME;
+		t->volumes[SPA_BT_VOLUME_ID_TX].volume = DEFAULT_AG_VOLUME;
+	} else {
+		t->volumes[SPA_BT_VOLUME_ID_RX].volume = DEFAULT_RX_VOLUME;
+		t->volumes[SPA_BT_VOLUME_ID_TX].volume = DEFAULT_TX_VOLUME;
+	}
+
 	spa_bt_transport_add_listener(t, &rfcomm->transport_listener, &transport_events, rfcomm);
 
 finish:
@@ -214,7 +251,7 @@ static void rfcomm_send_cmd(struct spa_source *source, char *data)
 		spa_log_error(backend->log, NAME": RFCOMM write error: %s", strerror(errno));
 }
 
-static int rfcomm_send_reply(struct spa_source *source, char *data)
+static int rfcomm_send_reply(struct spa_source *source, const char *data)
 {
 	struct rfcomm *rfcomm = source->data;
 	struct impl *backend = rfcomm->backend;
@@ -231,6 +268,30 @@ static int rfcomm_send_reply(struct spa_source *source, char *data)
 	return len;
 }
 
+static void rfcomm_emit_volume_changed(struct rfcomm *rfcomm, int id, int hw_volume)
+{
+	struct spa_bt_transport_volume *t_volume;
+
+	if ((id == SPA_BT_VOLUME_ID_RX || id == SPA_BT_VOLUME_ID_TX) && hw_volume >= 0) {
+		rfcomm->volumes[id].active = true;
+		rfcomm->volumes[id].hw_volume = hw_volume;
+	}
+
+	spa_log_debug(rfcomm->backend->log, NAME": volume changed %d", hw_volume);
+
+	if (rfcomm->transport == NULL || !rfcomm->has_volume)
+		return;
+
+	for (int i = 0; i < SPA_BT_VOLUME_ID_TERM ; ++i) {
+		t_volume = &rfcomm->transport->volumes[i];
+		t_volume->active = rfcomm->volumes[i].active;
+		t_volume->volume =
+			spa_bt_volume_hw_to_linear(rfcomm->volumes[i].hw_volume, t_volume->hw_volume_max);
+	}
+
+	spa_bt_transport_emit_volume_changed(rfcomm->transport);
+}
+
 #ifdef HAVE_BLUEZ_5_BACKEND_HSP_NATIVE
 static bool rfcomm_hsp_ag(struct spa_source *source, char* buf)
 {
@@ -243,16 +304,16 @@ static bool rfcomm_hsp_ag(struct spa_source *source, char* buf)
 	 * AT+VGM=value: value between 0 and 15, sent by the HS to AG to set the microphone gain.
 	 * AT+CKPD=200: Sent by HS when headset button is pressed. */
 	if (sscanf(buf, "AT+VGS=%d", &gain) == 1) {
-		if (gain <= 15) {
+		if (gain <= SPA_BT_VOLUME_HS_MAX) {
-			/* t->speaker_gain = gain; */
+			rfcomm_emit_volume_changed(rfcomm, SPA_BT_VOLUME_ID_TX, gain);
 			rfcomm_send_reply(source, "OK");
 		} else {
 			spa_log_debug(backend->log, NAME": RFCOMM receive unsupported VGS gain: %s", buf);
 			rfcomm_send_reply(source, "ERROR");
 		}
 	} else if (sscanf(buf, "AT+VGM=%d", &gain) == 1) {
-		if (gain <= 15) {
+		if (gain <= SPA_BT_VOLUME_HS_MAX) {
-			/* t->microphone_gain = gain; */
+			rfcomm_emit_volume_changed(rfcomm, SPA_BT_VOLUME_ID_RX, gain);
 			rfcomm_send_reply(source, "OK");
 		} else {
 			rfcomm_send_reply(source, "ERROR");
@@ -267,6 +328,33 @@ static bool rfcomm_hsp_ag(struct spa_source *source, char* buf)
 	return true;
 }
 
+static bool rfcomm_send_volume_cmd(struct spa_source *source, int id)
+{
+	struct rfcomm *rfcomm = source->data;
+	struct spa_bt_transport_volume *t_volume;
+	char *cmd;
+	int hw_volume;
+
+	t_volume = rfcomm->transport ? &rfcomm->transport->volumes[id] : NULL;
+
+	if (!(t_volume && t_volume->active))
+		return false;
+
+	hw_volume = spa_bt_volume_linear_to_hw(t_volume->volume, t_volume->hw_volume_max);
+	rfcomm->volumes[id].hw_volume = hw_volume;
+
+	if (id == SPA_BT_VOLUME_ID_TX)
+		cmd = spa_aprintf("AT+VGM=%u", hw_volume);
+	else if (id == SPA_BT_VOLUME_ID_RX)
+		cmd = spa_aprintf("AT+VGS=%u", hw_volume);
+	else
+	 	spa_assert_not_reached();
+
+	rfcomm_send_cmd(source, cmd);
+	free(cmd);
+	return true;
+}
+
 static bool rfcomm_hsp_hs(struct spa_source *source, char* buf)
 {
 	struct rfcomm *rfcomm = source->data;
@@ -281,17 +369,29 @@ static bool rfcomm_hsp_hs(struct spa_source *source, char* buf)
 	 * RING: Sent by AG to HS to notify of an incoming call. It can safely be ignored because
 	 *   it does not expect a reply. */
 	if (sscanf(buf, "\r\n+VGS=%d\r\n", &gain) == 1) {
-		if (gain <= 15) {
+		if (gain <= SPA_BT_VOLUME_HS_MAX) {
-			/* t->microphone_gain = gain; */
+			rfcomm_emit_volume_changed(rfcomm, SPA_BT_VOLUME_ID_RX, gain);
 		} else {
 			spa_log_debug(backend->log, NAME": RFCOMM receive unsupported VGS gain: %s", buf);
 		}
 	} else if (sscanf(buf, "\r\n+VGM=%d\r\n", &gain) == 1) {
-		if (gain <= 15) {
+		if (gain <= SPA_BT_VOLUME_HS_MAX) {
-			/* t->speaker_gain = gain; */
+			rfcomm_emit_volume_changed(rfcomm, SPA_BT_VOLUME_ID_TX, gain);
 		} else {
 			spa_log_debug(backend->log, NAME": RFCOMM receive unsupported VGM gain: %s", buf);
 		}
+	} if (strncmp(buf, "\r\nOK\r\n", 6) == 0) {
+		if (rfcomm->hs_state == hsp_hs_init2) {
+			if (rfcomm_send_volume_cmd(&rfcomm->source, SPA_BT_VOLUME_ID_RX))
+				rfcomm->hs_state = hsp_hs_vgs;
+			else
+				rfcomm->hs_state = hsp_hs_init1;
+		} else if (rfcomm->hs_state == hsp_hs_vgs) {
+			if (rfcomm_send_volume_cmd(&rfcomm->source, SPA_BT_VOLUME_ID_TX))
+				rfcomm->hs_state = hsp_hs_vgm;
+			else
+				rfcomm->hs_state = hsp_hs_init1;
+		}
 	}
 
 	return true;
@@ -363,10 +463,16 @@ static bool rfcomm_hfp_ag(struct spa_source *source, char* buf)
 	unsigned int selected_codec;
 
 	if (sscanf(buf, "AT+BRSF=%u", &features) == 1) {
-
 		unsigned int ag_features = SPA_BT_HFP_AG_FEATURE_NONE;
 		char *cmd;
 
+		/*
+		 * Determine device volume control. Some headsets only support control of
+		 * TX volume, but not RX, even if they have a microphone. Determine this
+		 * separately based on whether we also get AT+VGS/AT+VGM.
+		 */
+		rfcomm->has_volume = (features & SPA_BT_HFP_HF_FEATURE_REMOTE_VOLUME_CONTROL);
+
 		/* Decide if we want to signal that the computer supports mSBC negotiation
 		   This should be done when
 			 a) mSBC support is enabled in config file and
@@ -451,6 +557,7 @@ static bool rfcomm_hfp_ag(struct spa_source *source, char* buf)
 			} else {
 				rfcomm->transport->codec = HFP_AUDIO_CODEC_CVSD;
 				spa_bt_device_connect_profile(rfcomm->device, rfcomm->profile);
+				rfcomm_emit_volume_changed(rfcomm, -1, SPA_BT_VOLUME_INVALID);
 			}
 		}
 
@@ -492,21 +599,22 @@ static bool rfcomm_hfp_ag(struct spa_source *source, char* buf)
 		}
 		rfcomm->transport->codec = selected_codec;
 		spa_bt_device_connect_profile(rfcomm->device, rfcomm->profile);
+		rfcomm_emit_volume_changed(rfcomm, -1, SPA_BT_VOLUME_INVALID);
 
 		rfcomm_send_reply(source, "OK");
 		if (was_switching_codec)
 			spa_bt_device_emit_codec_switched(rfcomm->device, 0);
 	} else if (sscanf(buf, "AT+VGM=%u", &gain) == 1) {
-		if (gain <= 15) {
+		if (gain <= SPA_BT_VOLUME_HS_MAX) {
-			/* t->microphone_gain = gain; */
+			rfcomm_emit_volume_changed(rfcomm, SPA_BT_VOLUME_ID_RX, gain);
 			rfcomm_send_reply(source, "OK");
 		} else {
 			spa_log_debug(backend->log, NAME": RFCOMM receive unsupported VGM gain: %s", buf);
 			rfcomm_send_reply(source, "ERROR");
 		}
 	} else if (sscanf(buf, "AT+VGS=%u", &gain) == 1) {
-		if (gain <= 15) {
+		if (gain <= SPA_BT_VOLUME_HS_MAX) {
-			/* t->speaker_gain = gain; */
+			rfcomm_emit_volume_changed(rfcomm, SPA_BT_VOLUME_ID_TX, gain);
 			rfcomm_send_reply(source, "OK");
 		} else {
 			spa_log_debug(backend->log, NAME": RFCOMM receive unsupported VGS gain: %s", buf);
@@ -617,8 +725,8 @@ static bool rfcomm_hfp_hf(struct spa_source *source, char* buf)
 			token = strtok(NULL, separators);
 			gain = atoi(token);
 
-			if (gain <= 15) {
+			if (gain <= SPA_BT_VOLUME_HS_MAX) {
-				/* t->speaker_gain = gain; */
+				rfcomm_emit_volume_changed(rfcomm, SPA_BT_VOLUME_ID_TX, gain);
 			} else {
 				spa_log_debug(backend->log, NAME": RFCOMM receive unsupported VGM gain: %s", token);
 			}
@@ -627,8 +735,8 @@ static bool rfcomm_hfp_hf(struct spa_source *source, char* buf)
 			token = strtok(NULL, separators);
 			gain = atoi(token);
 
-			if (gain <= 15) {
+			if (gain <= SPA_BT_VOLUME_HS_MAX) {
-				/* t->microphone_gain = gain; */
+				rfcomm_emit_volume_changed(rfcomm, SPA_BT_VOLUME_ID_RX, gain);
 			} else {
 				spa_log_debug(backend->log, NAME": RFCOMM receive unsupported VGS gain: %s", token);
 			}
@@ -656,7 +764,7 @@ static bool rfcomm_hfp_hf(struct spa_source *source, char* buf)
 					rfcomm->hf_state = hfp_hf_cmer;
 					break;
 				case hfp_hf_cmer:
-					rfcomm->hf_state = hfp_hf_slc;
+					rfcomm->hf_state = hfp_hf_slc1;
 					rfcomm->slc_configured = true;
 					if (!rfcomm->codec_negotiation_supported) {
 						rfcomm->transport = _transport_create(rfcomm);
@@ -668,6 +776,18 @@ static bool rfcomm_hfp_hf(struct spa_source *source, char* buf)
 							spa_bt_device_connect_profile(rfcomm->device, rfcomm->profile);
 						}
 					}
+					/* Report volume on SLC establishment */
+					if (rfcomm_send_volume_cmd(source, SPA_BT_VOLUME_ID_RX))
+						rfcomm->hf_state = hfp_hf_vgs;
+					break;
+				case hfp_hf_slc2:
+					if (rfcomm_send_volume_cmd(source, SPA_BT_VOLUME_ID_RX))
+						rfcomm->hf_state = hfp_hf_vgs;
+					break;
+				case hfp_hf_vgs:
+					rfcomm->hf_state = hfp_hf_slc1;
+					if (rfcomm_send_volume_cmd(source, SPA_BT_VOLUME_ID_TX))
+						rfcomm->hf_state = hfp_hf_vgm;
 					break;
 				default:
 					break;
@@ -985,6 +1105,19 @@ static void sco_listen_event(struct spa_source *source)
 
 	spa_log_debug(backend->log, NAME": transport %p: audio connected", t);
 
+	/* Report initial volume to remote */
+	if (t->profile == SPA_BT_PROFILE_HSP_AG) {
+		if (rfcomm_send_volume_cmd(&rfcomm->source, SPA_BT_VOLUME_ID_RX))
+			rfcomm->hs_state = hsp_hs_vgs;
+		else
+			rfcomm->hs_state = hsp_hs_init1;
+	} else if (t->profile == SPA_BT_PROFILE_HFP_AG) {
+		if (rfcomm_send_volume_cmd(&rfcomm->source, SPA_BT_VOLUME_ID_RX))
+			rfcomm->hf_state = hfp_hf_vgs;
+		else
+			rfcomm->hf_state = hfp_hf_slc1;
+	}
+
 	spa_bt_transport_set_state(t, SPA_BT_TRANSPORT_STATE_PENDING);
 	return;
 
@@ -1043,10 +1176,47 @@ fail_close:
 	return -1;
 }
 
+static int sco_set_volume_cb(void *data, int id, float volume)
+{
+	struct spa_bt_transport *t = data;
+	struct spa_bt_transport_volume *t_volume = &t->volumes[id];
+	struct transport_data *td = t->user_data;
+	struct rfcomm *rfcomm = td->rfcomm;
+	char *msg;
+	int value;
+
+	if (!(rfcomm->profile & SPA_BT_PROFILE_HEADSET_HEAD_UNIT))
+		return -ENOTSUP;
+
+	if (!(rfcomm->has_volume && rfcomm->volumes[id].active))
+		return -ENOTSUP;
+
+	value = spa_bt_volume_linear_to_hw(volume, t_volume->hw_volume_max);
+	t_volume->volume = volume;
+
+	if (rfcomm->volumes[id].hw_volume == value)
+		return 0;
+	rfcomm->volumes[id].hw_volume = value;
+
+	if (id == SPA_BT_VOLUME_ID_RX)
+		msg = spa_aprintf("+VGM: %d", value);
+	else if (id == SPA_BT_VOLUME_ID_TX)
+		msg = spa_aprintf("+VGS: %d", value);
+	else
+		spa_assert_not_reached();
+
+	if (rfcomm->transport)
+		rfcomm_send_reply(&rfcomm->source, msg);
+
+	free(msg);
+	return 0;
+}
+
 static const struct spa_bt_transport_implementation sco_transport_impl = {
 	SPA_VERSION_BT_TRANSPORT_IMPLEMENTATION,
 	.acquire = sco_acquire_cb,
 	.release = sco_release_cb,
+	.set_volume = sco_set_volume_cb,
 };
 
 static struct rfcomm *device_find_rfcomm(struct impl *backend, struct spa_bt_device *device)
@@ -1279,6 +1449,12 @@ static DBusHandlerResult profile_new_connection(DBusConnection *conn, DBusMessag
 	rfcomm->source.mask = SPA_IO_IN;
 	rfcomm->source.rmask = 0;
 
+	for (int i = 0; i < SPA_BT_VOLUME_ID_TERM; ++i) {
+		if (rfcomm->profile & SPA_BT_PROFILE_HEADSET_AUDIO_GATEWAY)
+			rfcomm->volumes[i].active = true;
+		rfcomm->volumes[i].hw_volume = SPA_BT_VOLUME_INVALID;
+	}
+
 	spa_bt_device_add_listener(d, &rfcomm->device_listener, &device_events, rfcomm);
 	spa_loop_add_source(backend->main_loop, &rfcomm->source);
 	spa_list_append(&backend->rfcomm_list, &rfcomm->link);
@@ -1296,6 +1472,11 @@ static DBusHandlerResult profile_new_connection(DBusConnection *conn, DBusMessag
 		}
 		rfcomm->transport = t;
 
+		if (profile == SPA_BT_PROFILE_HSP_AG) {
+			rfcomm->has_volume = true;
+			rfcomm->hs_state = hsp_hs_init1;
+		}
+
 		spa_bt_device_connect_profile(t->device, profile);
 
 		spa_log_debug(backend->log, NAME": Transport %s available for profile %s", t->path, handler);
@@ -1319,6 +1500,9 @@ static DBusHandlerResult profile_new_connection(DBusConnection *conn, DBusMessag
 			rfcomm->codec_negotiation_supported = false;
 		}
 
+		rfcomm->has_volume = true;
+		hf_features |= SPA_BT_HFP_HF_FEATURE_REMOTE_VOLUME_CONTROL;
+
 		/* send command to AG with the features supported by Hands-Free */
 		cmd = spa_aprintf("AT+BRSF=%u", hf_features);
 		rfcomm_send_cmd(&rfcomm->source, cmd);