icnss2: Enable power supply framework for 5.15 kernel

Icnss driver indicates vbatt voltage level to wlan fw
based on battery percentage. This change adds power
supply framework code to enable icnss2 driver to measure
battery percentage and update vbatt volatge level to
wlan fw.

Change-Id: I376805da38222a5e7a20ae4a1737bc978ee6a9ae
Signed-off-by: Sandeep Singh <[email protected]>

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__*/