1 files changed, 64 insertions, 7 deletions

diff --git a/drivers/thermal/tegra/tegra-bpmp-thermal.c b/drivers/thermal/tegra/tegra-bpmp-thermal.c
index 0b7a1a1948cb..997d77ce30d9 100644
--- a/drivers/thermal/tegra/tegra-bpmp-thermal.c
+++ b/drivers/thermal/tegra/tegra-bpmp-thermal.c
@@ -52,6 +52,8 @@ static int __tegra_bpmp_thermal_get_temp(struct tegra_bpmp_thermal_zone *zone,
 	err = tegra_bpmp_transfer(zone->tegra->bpmp, &msg);
 	if (err)
 		return err;
+	if (msg.rx.ret == -BPMP_EFAULT)
+		return -EAGAIN;
 	if (msg.rx.ret)
 		return -EINVAL;
 
@@ -62,12 +64,14 @@ static int __tegra_bpmp_thermal_get_temp(struct tegra_bpmp_thermal_zone *zone,
 
 static int tegra_bpmp_thermal_get_temp(struct thermal_zone_device *tz, int *out_temp)
 {
-	return __tegra_bpmp_thermal_get_temp(tz->devdata, out_temp);
+	struct tegra_bpmp_thermal_zone *zone = thermal_zone_device_priv(tz);
+
+	return __tegra_bpmp_thermal_get_temp(zone, out_temp);
 }
 
 static int tegra_bpmp_thermal_set_trips(struct thermal_zone_device *tz, int low, int high)
 {
-	struct tegra_bpmp_thermal_zone *zone = tz->devdata;
+	struct tegra_bpmp_thermal_zone *zone = thermal_zone_device_priv(tz);
 	struct mrq_thermal_host_to_bpmp_request req;
 	struct tegra_bpmp_message msg;
 	int err;
@@ -163,19 +167,69 @@ static int tegra_bpmp_thermal_get_num_zones(struct tegra_bpmp *bpmp,
 	return 0;
 }
 
+static int tegra_bpmp_thermal_trips_supported(struct tegra_bpmp *bpmp, bool *supported)
+{
+	struct mrq_thermal_host_to_bpmp_request req;
+	union mrq_thermal_bpmp_to_host_response reply;
+	struct tegra_bpmp_message msg;
+	int err;
+
+	memset(&req, 0, sizeof(req));
+	req.type = CMD_THERMAL_QUERY_ABI;
+	req.query_abi.type = CMD_THERMAL_SET_TRIP;
+
+	memset(&msg, 0, sizeof(msg));
+	msg.mrq = MRQ_THERMAL;
+	msg.tx.data = &req;
+	msg.tx.size = sizeof(req);
+	msg.rx.data = &reply;
+	msg.rx.size = sizeof(reply);
+
+	err = tegra_bpmp_transfer(bpmp, &msg);
+	if (err)
+		return err;
+
+	if (msg.rx.ret == 0) {
+		*supported = true;
+		return 0;
+	} else if (msg.rx.ret == -BPMP_ENODEV) {
+		*supported = false;
+		return 0;
+	} else {
+		return -EINVAL;
+	}
+}
+
 static const struct thermal_zone_device_ops tegra_bpmp_of_thermal_ops = {
 	.get_temp = tegra_bpmp_thermal_get_temp,
 	.set_trips = tegra_bpmp_thermal_set_trips,
 };
 
+static const struct thermal_zone_device_ops tegra_bpmp_of_thermal_ops_notrips = {
+	.get_temp = tegra_bpmp_thermal_get_temp,
+};
+
 static int tegra_bpmp_thermal_probe(struct platform_device *pdev)
 {
 	struct tegra_bpmp *bpmp = dev_get_drvdata(pdev->dev.parent);
+	const struct thermal_zone_device_ops *thermal_ops;
 	struct tegra_bpmp_thermal *tegra;
 	struct thermal_zone_device *tzd;
 	unsigned int i, max_num_zones;
+	bool supported;
 	int err;
 
+	err = tegra_bpmp_thermal_trips_supported(bpmp, &supported);
+	if (err) {
+		dev_err(&pdev->dev, "failed to determine if trip points are supported\n");
+		return err;
+	}
+
+	if (supported)
+		thermal_ops = &tegra_bpmp_of_thermal_ops;
+	else
+		thermal_ops = &tegra_bpmp_of_thermal_ops_notrips;
+
 	tegra = devm_kzalloc(&pdev->dev, sizeof(*tegra), GFP_KERNEL);
 	if (!tegra)
 		return -ENOMEM;
@@ -207,13 +261,18 @@ static int tegra_bpmp_thermal_probe(struct platform_device *pdev)
 		zone->tegra = tegra;
 
 		err = __tegra_bpmp_thermal_get_temp(zone, &temp);
-		if (err < 0) {
+
+		/*
+		 * Sensors in powergated domains may temporarily fail to be read
+		 * (-EAGAIN), but will become accessible when the domain is powered on.
+		 */
+		if (err < 0 && err != -EAGAIN) {
 			devm_kfree(&pdev->dev, zone);
 			continue;
 		}
 
 		tzd = devm_thermal_of_zone_register(
-			&pdev->dev, i, zone, &tegra_bpmp_of_thermal_ops);
+			&pdev->dev, i, zone, thermal_ops);
 		if (IS_ERR(tzd)) {
 			if (PTR_ERR(tzd) == -EPROBE_DEFER)
 				return -EPROBE_DEFER;
@@ -241,13 +300,11 @@ static int tegra_bpmp_thermal_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int tegra_bpmp_thermal_remove(struct platform_device *pdev)
+static void tegra_bpmp_thermal_remove(struct platform_device *pdev)
 {
 	struct tegra_bpmp_thermal *tegra = platform_get_drvdata(pdev);
 
 	tegra_bpmp_free_mrq(tegra->bpmp, MRQ_THERMAL, tegra);
-
-	return 0;
 }
 
 static const struct of_device_id tegra_bpmp_thermal_of_match[] = {