Merge "icnss2: Enable power supply framework for 5.15 kernel" into wlan-platform.lnx.1.0

icnss2/main.c

@@ -922,6 +922,9 @@ static int icnss_driver_event_server_arrive(struct icnss_priv *priv,
 	if (!priv->fw_early_crash_irq)
 		register_early_crash_notifications(&priv->pdev->dev);
 
+	if (priv->psf_supported)
+		queue_work(priv->soc_update_wq, &priv->soc_update_work);
+
 	return ret;
 
 device_info_failure:
@@ -941,6 +944,9 @@ static int icnss_driver_event_server_exit(struct icnss_priv *priv)
 
 	icnss_clear_server(priv);
 
+	if (priv->psf_supported)
+		priv->last_updated_voltage = 0;
+
 	return 0;
 }
 
@@ -3802,6 +3808,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");
@@ -4339,6 +4352,18 @@ void icnss_destroy_ramdump_device(struct icnss_ramdump_info *ramdump_info)
 	kfree(ramdump_info);
 }
 
+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);
@@ -4349,6 +4374,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);

icnss2/main.h

@@ -10,6 +10,7 @@
 #include <linux/kobject.h>
 #include <linux/platform_device.h>
 #include <linux/ipc_logging.h>
+#include <linux/power_supply.h>
 #ifdef CONFIG_CNSS_OUT_OF_TREE
 #include "icnss2.h"
 #else
@@ -161,6 +162,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;
@@ -476,6 +482,12 @@ struct icnss_priv {
 	uint32_t fw_soc_wake_ack_irq;
 	char foundry_name;
 	bool bdf_download_support;
+	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;
 	bool wpss_supported;
 };

icnss2/power.c

@@ -30,6 +30,14 @@ static struct icnss_vreg_cfg icnss_adrestea_vreg_list[] = {
 	{"vdd-3.3-ch0", 3312000, 3312000, 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},
 };
@@ -50,6 +58,9 @@ static struct icnss_clk_cfg icnss_adrestea_clk_list[] = {
 #define ICNSS_MBOX_MSG_MAX_LEN 64
 #define ICNSS_MBOX_TIMEOUT_MS 1000
 
+#define ICNSS_BATTERY_LEVEL_COUNT	ARRAY_SIZE(icnss_battery_level)
+#define ICNSS_MAX_BATTERY_LEVEL		100
+
 /**
  * enum icnss_vreg_param: Voltage regulator TCS param
  * @ICNSS_VREG_VOLTAGE: Provides voltage level to be configured in TCS
@@ -808,3 +819,107 @@ int icnss_update_cpr_info(struct icnss_priv *priv)
 				       ICNSS_TCS_UP_SEQ,
 				       cpr_info->voltage);
 }
+
+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;
+}

icnss2/power.h

@@ -14,5 +14,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

icnss2/qmi.c

@@ -3383,3 +3383,69 @@ out:
 	penv->stats.restart_level_err++;
 	return ret;
 }
+
+int icnss_send_vbatt_update(struct icnss_priv *priv, uint64_t voltage_uv)
+{
+	int ret;
+	struct wlfw_vbatt_req_msg_v01 *req;
+	struct wlfw_vbatt_resp_msg_v01 *resp;
+	struct qmi_txn txn;
+
+	if (!priv)
+		return -ENODEV;
+
+	if (test_bit(ICNSS_FW_DOWN, &priv->state))
+		return -EINVAL;
+
+	icnss_pr_dbg("Sending Vbatt message, state: 0x%lx\n", priv->state);
+
+	req = kzalloc(sizeof(*req), GFP_KERNEL);
+	if (!req)
+		return -ENOMEM;
+
+	resp = kzalloc(sizeof(*resp), GFP_KERNEL);
+	if (!resp) {
+		kfree(req);
+		return -ENOMEM;
+	}
+
+	req->voltage_uv = voltage_uv;
+
+	ret = qmi_txn_init(&priv->qmi, &txn, wlfw_vbatt_resp_msg_v01_ei, resp);
+	if (ret < 0) {
+		icnss_pr_err("Fail to init txn for Vbatt message resp %d\n",
+			     ret);
+		goto out;
+	}
+
+	ret = qmi_send_request(&priv->qmi, NULL, &txn,
+			       QMI_WLFW_VBATT_REQ_V01,
+			       WLFW_VBATT_REQ_MSG_V01_MAX_MSG_LEN,
+			       wlfw_vbatt_req_msg_v01_ei, req);
+	if (ret < 0) {
+		qmi_txn_cancel(&txn);
+		icnss_pr_err("Fail to send Vbatt message req %d\n", ret);
+		goto out;
+	}
+
+	ret = qmi_txn_wait(&txn, priv->ctrl_params.qmi_timeout);
+	if (ret < 0) {
+		icnss_pr_err("VBATT message resp wait failed with ret %d\n",
+			     ret);
+		goto out;
+	} else if (resp->resp.result != QMI_RESULT_SUCCESS_V01) {
+		icnss_pr_err("QMI Vbatt message request rejected, result:%d error:%d\n",
+			     resp->resp.result, resp->resp.error);
+		ret = -resp->resp.result;
+		goto out;
+	}
+
+	kfree(resp);
+	kfree(req);
+	return 0;
+
+out:
+	kfree(resp);
+	kfree(req);
+	return ret;
+}

icnss2/qmi.h

@@ -198,6 +198,11 @@ static inline int wlfw_cal_report_req(struct icnss_priv *priv)
 {
 	return 0;
 }
+
+int icnss_send_vbatt_update(struct icnss_priv *priv, uint64_t voltage_uv)
+{
+	return 0;
+}
 #else
 int wlfw_ind_register_send_sync_msg(struct icnss_priv *priv);
 int icnss_connect_to_fw_server(struct icnss_priv *priv, void *data);
@@ -252,6 +257,7 @@ int icnss_dms_init(struct icnss_priv *priv);
 void icnss_dms_deinit(struct icnss_priv *priv);
 int wlfw_subsys_restart_level_msg(struct icnss_priv *penv, uint8_t restart_level);
 int wlfw_cal_report_req(struct icnss_priv *priv);
+int icnss_send_vbatt_update(struct icnss_priv *priv, uint64_t voltage_uv);
 #endif
 
 #endif /* __ICNSS_QMI_H__*/