Merge "icnss2: Enable power supply framework for 5.4 kernel"

drivers/soc/qcom/icnss2/main.c

@@ -722,6 +722,9 @@ static int icnss_driver_event_server_arrive(struct icnss_priv *priv,
 	if (priv->vbatt_supported)
 		icnss_init_vph_monitor(priv);
 
+	if (priv->psf_supported)
+		queue_work(priv->soc_update_wq, &priv->soc_update_work);
+
 	return ret;
 
 device_info_failure:
@@ -745,6 +748,9 @@ static int icnss_driver_event_server_exit(struct icnss_priv *priv)
 		adc_tm_disable_chan_meas(priv->adc_tm_dev,
 					  &priv->vph_monitor_params);
 
+	if (priv->psf_supported)
+		priv->last_updated_voltage = 0;
+
 	return 0;
 }
 
@@ -3610,6 +3616,13 @@ static int icnss_resource_parse(struct icnss_priv *priv)
 		goto put_vreg;
 	}
 
+	if (of_property_read_bool(pdev->dev.of_node, "qcom,psf-supported")) {
+		ret = icnss_get_psf_info(priv);
+		if (ret < 0)
+			goto out;
+		priv->psf_supported = true;
+	}
+
 	if (priv->device_id == ADRASTEA_DEVICE_ID) {
 		res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
 						   "membase");
@@ -4145,6 +4158,18 @@ out_reset_drvdata:
 	return ret;
 }
 
+static void icnss_unregister_power_supply_notifier(struct icnss_priv *priv)
+{
+	if (priv->batt_psy)
+		power_supply_put(penv->batt_psy);
+
+	if (priv->psf_supported) {
+		flush_workqueue(priv->soc_update_wq);
+		destroy_workqueue(priv->soc_update_wq);
+		power_supply_unreg_notifier(&priv->psf_nb);
+	}
+}
+
 static int icnss_remove(struct platform_device *pdev)
 {
 	struct icnss_priv *priv = dev_get_drvdata(&pdev->dev);
@@ -4164,6 +4189,8 @@ static int icnss_remove(struct platform_device *pdev)
 
 	icnss_debugfs_destroy(priv);
 
+	icnss_unregister_power_supply_notifier(penv);
+
 	icnss_sysfs_destroy(priv);
 
 	complete_all(&priv->unblock_shutdown);

drivers/soc/qcom/icnss2/main.h

@@ -13,6 +13,7 @@
 #include <linux/kobject.h>
 #include <linux/platform_device.h>
 #include <linux/ipc_logging.h>
+#include <linux/power_supply.h>
 #include <dt-bindings/iio/qcom,spmi-vadc.h>
 #include <soc/qcom/icnss2.h>
 #include <soc/qcom/service-locator.h>
@@ -167,6 +168,11 @@ struct icnss_clk_cfg {
 	u32 required;
 };
 
+struct icnss_battery_level {
+	int lower_battery_threshold;
+	int ldo_voltage;
+};
+
 struct icnss_clk_info {
 	struct list_head list;
 	struct clk *clk;
@@ -466,6 +472,12 @@ struct icnss_priv {
 	struct icnss_dms_data dms;
 	u8 use_nv_mac;
 	u32 wlan_en_delay_ms;
+	bool psf_supported;
+	struct notifier_block psf_nb;
+	struct power_supply *batt_psy;
+	int last_updated_voltage;
+	struct work_struct soc_update_work;
+	struct workqueue_struct *soc_update_wq;
 	unsigned long device_config;
 	struct timer_list recovery_timer;
 };

drivers/soc/qcom/icnss2/power.c

@@ -29,6 +29,14 @@ static struct icnss_vreg_cfg icnss_adrestea_vreg_list[] = {
 	{"vdd-smps", 984000, 984000, 0, 0, 0, false, true},
 };
 
+static struct icnss_battery_level icnss_battery_level[] = {
+	{70, 3300000},
+	{60, 3200000},
+	{50, 3100000},
+	{25, 3000000},
+	{0, 2850000},
+};
+
 static struct icnss_clk_cfg icnss_clk_list[] = {
 	{"rf_clk", 0, 0},
 };
@@ -55,6 +63,9 @@ static struct icnss_clk_cfg icnss_adrestea_clk_list[] = {
 #define MAX_TCS_CMD_NUM			5
 #define BT_CXMX_VOLTAGE_MV		950
 
+#define ICNSS_BATTERY_LEVEL_COUNT	ARRAY_SIZE(icnss_battery_level)
+#define ICNSS_MAX_BATTERY_LEVEL		100
+
 static int icnss_get_vreg_single(struct icnss_priv *priv,
 				 struct icnss_vreg_info *vreg)
 {
@@ -821,6 +832,110 @@ out:
 	return ret;
 }
 
+static int icnss_get_battery_level(struct icnss_priv *priv)
+{
+	int err = 0, battery_percentage = 0;
+	union power_supply_propval psp = {0,};
+
+	if (!priv->batt_psy)
+		priv->batt_psy = power_supply_get_by_name("battery");
+
+	if (priv->batt_psy) {
+		err = power_supply_get_property(priv->batt_psy,
+						POWER_SUPPLY_PROP_CAPACITY,
+						&psp);
+		if (err) {
+			icnss_pr_err("battery percentage read error:%d\n", err);
+			goto out;
+		}
+		battery_percentage = psp.intval;
+	}
+
+	icnss_pr_info("Battery Percentage: %d\n", battery_percentage);
+out:
+	return battery_percentage;
+}
+
+static void icnss_update_soc_level(struct work_struct *work)
+{
+	int battery_percentage = 0, current_updated_voltage = 0, err = 0;
+	int level_count;
+	struct icnss_priv *priv = container_of(work, struct icnss_priv, soc_update_work);
+
+	battery_percentage = icnss_get_battery_level(priv);
+	if (!battery_percentage ||
+	    battery_percentage > ICNSS_MAX_BATTERY_LEVEL) {
+		icnss_pr_err("Battery percentage read failure\n");
+		return;
+	}
+
+	for (level_count = 0; level_count < ICNSS_BATTERY_LEVEL_COUNT;
+	     level_count++) {
+		if (battery_percentage >=
+		    icnss_battery_level[level_count].lower_battery_threshold) {
+			current_updated_voltage =
+				icnss_battery_level[level_count].ldo_voltage;
+			break;
+		}
+	}
+
+	if (level_count != ICNSS_BATTERY_LEVEL_COUNT &&
+	    priv->last_updated_voltage != current_updated_voltage) {
+		err = icnss_send_vbatt_update(priv, current_updated_voltage);
+		if (err < 0) {
+			icnss_pr_err("Unable to update ldo voltage");
+			return;
+		}
+		priv->last_updated_voltage = current_updated_voltage;
+	}
+}
+
+static int icnss_battery_supply_callback(struct notifier_block *nb,
+					 unsigned long event, void *data)
+{
+	struct power_supply *psy = data;
+	struct icnss_priv *priv = container_of(nb, struct icnss_priv,
+					       psf_nb);
+	if (strcmp(psy->desc->name, "battery"))
+		return NOTIFY_OK;
+
+	if (test_bit(ICNSS_WLFW_CONNECTED, &priv->state) &&
+	    !test_bit(ICNSS_FW_DOWN, &priv->state))
+		queue_work(priv->soc_update_wq, &priv->soc_update_work);
+
+	return NOTIFY_OK;
+}
+
+int icnss_get_psf_info(struct icnss_priv *priv)
+{
+	int ret = 0;
+
+	priv->soc_update_wq = alloc_workqueue("icnss_soc_update",
+					      WQ_UNBOUND, 1);
+	if (!priv->soc_update_wq) {
+		icnss_pr_err("Workqueue creation failed for soc update\n");
+		ret = -EFAULT;
+		goto out;
+	}
+
+	priv->psf_nb.notifier_call = icnss_battery_supply_callback;
+	ret = power_supply_reg_notifier(&priv->psf_nb);
+	if (ret < 0) {
+		icnss_pr_err("Power supply framework registration err: %d\n",
+			     ret);
+		goto err_psf_registration;
+	}
+
+	INIT_WORK(&priv->soc_update_work, icnss_update_soc_level);
+
+	return 0;
+
+err_psf_registration:
+	destroy_workqueue(priv->soc_update_wq);
+out:
+	return ret;
+}
+
 int icnss_get_cpr_info(struct icnss_priv *priv)
 {
 	struct platform_device *plat_dev = priv->pdev;

drivers/soc/qcom/icnss2/power.h

@@ -15,5 +15,5 @@ void icnss_put_resources(struct icnss_priv *priv);
 void icnss_put_vreg(struct icnss_priv *priv);
 void icnss_put_clk(struct icnss_priv *priv);
 int icnss_vreg_unvote(struct icnss_priv *priv);
-
+int icnss_get_psf_info(struct icnss_priv *priv);
 #endif