[PATCH v3] HID: logitech-hidpp: add support for Logitech G533 headset

[Kamil DomaÅski]

Changelog:
  v2:
  - changed charging status parsing to account for invalid states
  v3:
  - rebased against Linux v5.7
  - changed variable naming in hidpp20_adc_map_status_voltage
    to camel case
  - corrected comment styling in hidpp_battery_get_property
  - dropped usage of test_bit macro in hidpp20_adc_map_status_voltage
    to avoid using `long` type
  - added bit flag definitions in hidpp20_adc_map_status_voltage

Signed-off-by: Kamil DomaÅski <kamil@xxxxxxxxxxx>
---
 drivers/hid/hid-logitech-hidpp.c | 197 ++++++++++++++++++++++++++++++-
 1 file changed, 196 insertions(+), 1 deletion(-)

diff --git a/drivers/hid/hid-logitech-hidpp.c b/drivers/hid/hid-logitech-hidpp.c
index 094f4f1b6555..2e2842aec05b 100644
--- a/drivers/hid/hid-logitech-hidpp.c
+++ b/drivers/hid/hid-logitech-hidpp.c
@@ -29,6 +29,7 @@
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Benjamin Tissoires <benjamin.tissoires@xxxxxxxxx>");
+MODULE_AUTHOR("Kamil DomaÅski <kamil@xxxxxxxxxxx>");
 MODULE_AUTHOR("Nestor Lopez Casado <nlopezcasad@xxxxxxxxxxxx>");
 
 static bool disable_raw_mode;
@@ -92,6 +93,7 @@ MODULE_PARM_DESC(disable_tap_to_click,
 #define HIDPP_CAPABILITY_BATTERY_MILEAGE	BIT(2)
 #define HIDPP_CAPABILITY_BATTERY_LEVEL_STATUS	BIT(3)
 #define HIDPP_CAPABILITY_BATTERY_VOLTAGE	BIT(4)
+#define HIDPP_CAPABILITY_ADC_MEASUREMENT	BIT(5)
 
 /*
  * There are two hidpp protocols in use, the first version hidpp10 is known
@@ -141,6 +143,7 @@ struct hidpp_battery {
 	u8 feature_index;
 	u8 solar_feature_index;
 	u8 voltage_feature_index;
+	u8 adc_measurement_feature_index;
 	struct power_supply_desc desc;
 	struct power_supply *ps;
 	char name[64];
@@ -215,6 +218,7 @@ struct hidpp_device {
 #define HIDPP_ERROR_INVALID_PARAM_VALUE		0x0b
 #define HIDPP_ERROR_WRONG_PIN_CODE		0x0c
 /* HID++ 2.0 error codes */
+#define HIDPP20_ERROR_DISCONNECTED	0x05
 #define HIDPP20_ERROR				0xff
 
 static void hidpp_connect_event(struct hidpp_device *hidpp_dev);
@@ -1378,6 +1382,179 @@ static int hidpp20_battery_voltage_event(struct hidpp_device *hidpp,
 	return 0;
 }
 
+/* -------------------------------------------------------------------------- */
+/* 0x1F20: Analog-digital converter measurement                               */
+/* -------------------------------------------------------------------------- */
+
+#define HIDPP_PAGE_ADC_MEASUREMENT 0x1F20
+
+#define CMD_ADC_MEASUREMENT_GET_VOLTAGE 0x01
+
+#define FLAG_ADC_MAP_STATUS_CONNECTED         0x01
+#define FLAG_ADC_MAP_STATUS_CHARGING          0x02
+#define FLAG_ADC_MAP_STATUS_CHARGING_COMPLETE 0x04
+#define FLAG_ADC_MAP_STATUS_CHARGING_FAULT    0x08
+
+/**
+ * hidpp20_adc_map_status_voltage() - convert HID++ code to power supply status
+ * @hidpp: HID++ device struct.
+ * @data: ADC report data.
+ * @voltage: Pointer to variable where the ADC voltage shall be written.
+ *
+ * This function decodes the ADC voltage and charge status
+ * of the device's battery.
+ *
+ * Return: Returns the power supply charge status code.
+ */
+static int hidpp20_adc_map_status_voltage(struct hidpp_device *hidpp,
+						u8 data[3], int *voltage)
+{
+	u8 flags = data[2];
+	*voltage = get_unaligned_be16(data);
+
+	if (!(flags & FLAG_ADC_MAP_STATUS_CONNECTED))
+		return POWER_SUPPLY_STATUS_UNKNOWN;
+
+	if (flags & FLAG_ADC_MAP_STATUS_CHARGING) {
+		if (flags & FLAG_ADC_MAP_STATUS_CHARGING_FAULT)
+			return POWER_SUPPLY_STATUS_NOT_CHARGING;
+
+		if (flags & FLAG_ADC_MAP_STATUS_CHARGING_COMPLETE)
+			return POWER_SUPPLY_STATUS_FULL;
+
+		return POWER_SUPPLY_STATUS_CHARGING;
+	}
+
+	return POWER_SUPPLY_STATUS_DISCHARGING;
+}
+
+/**
+ * hidpp20_get_adc_measurement() - retrieve ADC mesurement feature info
+ * @hidpp: HID++ device struct.
+ * @feature_index: The device's feature index for ADC measurement.
+ * @status: Pointer to variable where the charge status shall be written.
+ * @voltage: Pointer to variable where the ADC voltage shall be written.
+ *
+ * This function retrieves the ADC voltage and charge status
+ * of the device's battery.
+ *
+ * Return: Returns 0 on success.
+ */
+static int hidpp20_get_adc_measurement(struct hidpp_device *hidpp,
+						 u8 feature_index,
+						 int *status, int *voltage)
+{
+	struct hidpp_report response;
+	int ret;
+	u8 *params = (u8 *)response.fap.params;
+
+	ret = hidpp_send_fap_command_sync(hidpp, feature_index,
+					  CMD_ADC_MEASUREMENT_GET_VOLTAGE,
+					  NULL, 0, &response);
+
+	/* The dongle cannot reach a device. */
+	if (ret == HIDPP20_ERROR_DISCONNECTED) {
+		*status = POWER_SUPPLY_STATUS_UNKNOWN;
+		*voltage = 0;
+		return 0;
+	}
+
+	if (ret > 0) {
+		hid_err(hidpp->hid_dev, "%s: received protocol error 0x%02x\n",
+			__func__, ret);
+		return -EPROTO;
+	}
+	if (ret)
+		return ret;
+
+	*status = hidpp20_adc_map_status_voltage(hidpp, params, voltage);
+
+	return 0;
+}
+
+/**
+ * hidpp20_query_adc_measurement_info() - retrieve ADC mesurement feature info
+ * @hidpp: HID++ device struct.
+ *
+ * This function queries the device for the feature index
+ * of the analog-to-digital converter measurement feature.
+ *
+ * Subsequently it retrieves the measurement result.
+ *
+ * Return: Returns 0 on success.
+ */
+static int hidpp20_query_adc_measurement_info(struct hidpp_device *hidpp)
+{
+	u8 feature_type;
+	int ret;
+	int status, voltage;
+
+	/* If the current index is unspecified (0xFF)
+	 * then fetch it using the root feature. */
+	if (hidpp->battery.adc_measurement_feature_index == 0xff) {
+		ret = hidpp_root_get_feature(hidpp, HIDPP_PAGE_ADC_MEASUREMENT,
+					     &hidpp->battery.adc_measurement_feature_index,
+					     &feature_type);
+		if (ret)
+			return ret;
+
+		hidpp->capabilities |= HIDPP_CAPABILITY_ADC_MEASUREMENT;
+	}
+
+	ret = hidpp20_get_adc_measurement(hidpp,
+						  hidpp->battery.adc_measurement_feature_index,
+						  &status, &voltage);
+
+	if (ret)
+		return ret;
+
+	hidpp->battery.status = status;
+	hidpp->battery.voltage = voltage;
+	hidpp->battery.online = status == POWER_SUPPLY_STATUS_CHARGING ||
+							status == POWER_SUPPLY_STATUS_FULL;
+
+	return 0;
+}
+
+/**
+ * hidpp20_adc_measurement_event() - handle incoming ADC measurement event
+ * @hidpp: HID++ device struct.
+ * @data: Pointer to the incoming report data.
+ * @size: Size of the incoming report data.
+ *
+ * This function processes an incoming update in the device's battery
+ * ADC voltage and charge status. If there is a change in either,
+ * a power supply update is passed on to the kernel.
+ *
+ * Return: Returns 0 on success.
+ */
+static int hidpp20_adc_measurement_event(struct hidpp_device *hidpp,
+					    u8 *data, int size)
+{
+	struct hidpp_report *report = (struct hidpp_report *)data;
+	int status, voltage;
+
+	/* Ignore the report if it is not an ADC report. */
+	if (report->fap.feature_index != hidpp->battery.adc_measurement_feature_index ||
+		report->fap.funcindex_clientid != EVENT_BATTERY_VOLTAGE_STATUS_BROADCAST)
+		return 0;
+
+	status = hidpp20_adc_map_status_voltage(hidpp,
+		report->fap.params, &voltage);
+
+	hidpp->battery.online = status == POWER_SUPPLY_STATUS_CHARGING ||
+							status == POWER_SUPPLY_STATUS_FULL;
+
+	/* Update the PS only if charging state or voltage changed. */
+	if (voltage != hidpp->battery.voltage || status != hidpp->battery.status) {
+		hidpp->battery.voltage = voltage;
+		hidpp->battery.status = status;
+		if (hidpp->battery.ps)
+			power_supply_changed(hidpp->battery.ps);
+	}
+	return 0;
+}
+
 static enum power_supply_property hidpp_battery_props[] = {
 	POWER_SUPPLY_PROP_ONLINE,
 	POWER_SUPPLY_PROP_STATUS,
@@ -1426,6 +1603,13 @@ static int hidpp_battery_get_property(struct power_supply *psy,
 			val->strval = hidpp->hid_dev->uniq;
 			break;
 		case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+			/*
+			 * ADC feature doesn't automatically report the voltage
+			 * so we poll it explicitly when the property is read.
+			 */
+			if (hidpp->capabilities & HIDPP_CAPABILITY_ADC_MEASUREMENT)
+				hidpp20_query_adc_measurement_info(hidpp);
+
 			/* hardware reports voltage in in mV. sysfs expects uV */
 			val->intval = hidpp->battery.voltage * 1000;
 			break;
@@ -3291,6 +3475,9 @@ static int hidpp_raw_hidpp_event(struct hidpp_device *hidpp, u8 *data,
 		ret = hidpp20_battery_voltage_event(hidpp, data, size);
 		if (ret != 0)
 			return ret;
+		ret = hidpp20_adc_measurement_event(hidpp, data, size);
+		if (ret != 0)
+			return ret;
 	}
 
 	if (hidpp->capabilities & HIDPP_CAPABILITY_HIDPP10_BATTERY) {
@@ -3413,6 +3600,7 @@ static int hidpp_initialize_battery(struct hidpp_device *hidpp)
 	hidpp->battery.feature_index = 0xff;
 	hidpp->battery.solar_feature_index = 0xff;
 	hidpp->battery.voltage_feature_index = 0xff;
+	hidpp->battery.adc_measurement_feature_index = 0xff;
 
 	if (hidpp->protocol_major >= 2) {
 		if (hidpp->quirks & HIDPP_QUIRK_CLASS_K750)
@@ -3421,6 +3609,8 @@ static int hidpp_initialize_battery(struct hidpp_device *hidpp)
 			ret = hidpp20_query_battery_voltage_info(hidpp);
 			if (ret)
 				ret = hidpp20_query_battery_info(hidpp);
+			if (ret)
+				ret = hidpp20_query_adc_measurement_info(hidpp);
 		}
 
 		if (ret)
@@ -3456,7 +3646,8 @@ static int hidpp_initialize_battery(struct hidpp_device *hidpp)
 		battery_props[num_battery_props++] =
 				POWER_SUPPLY_PROP_CAPACITY_LEVEL;
 
-	if (hidpp->capabilities & HIDPP_CAPABILITY_BATTERY_VOLTAGE)
+	if (hidpp->capabilities & HIDPP_CAPABILITY_BATTERY_VOLTAGE ||
+		hidpp->capabilities & HIDPP_CAPABILITY_ADC_MEASUREMENT)
 		battery_props[num_battery_props++] =
 			POWER_SUPPLY_PROP_VOLTAGE_NOW;
 
@@ -3625,6 +3816,8 @@ static void hidpp_connect_event(struct hidpp_device *hidpp)
 	} else if (hidpp->capabilities & HIDPP_CAPABILITY_HIDPP20_BATTERY) {
 		if (hidpp->capabilities & HIDPP_CAPABILITY_BATTERY_VOLTAGE)
 			hidpp20_query_battery_voltage_info(hidpp);
+		else if (hidpp->capabilities & HIDPP_CAPABILITY_ADC_MEASUREMENT)
+			hidpp20_query_adc_measurement_info(hidpp);
 		else
 			hidpp20_query_battery_info(hidpp);
 	}
@@ -3994,6 +4187,8 @@ static const struct hid_device_id hidpp_devices[] = {
 
 	{ /* Logitech G403 Wireless Gaming Mouse over USB */
 	  HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, 0xC082) },
+	{ /* Logitech G533 Wireless Headset over USB */
+	  HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, 0x0A66) },
 	{ /* Logitech G703 Gaming Mouse over USB */
 	  HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, 0xC087) },
 	{ /* Logitech G703 Hero Gaming Mouse over USB */
-- 
2.27.0