msm: tsens: Re-initialize TSENS controller (a46f4b04) · Commits · e / devices / android_kernel_fairphone_FP4

drivers/thermal/msm-tsens.c

+45 −0

Original line number	Diff line number	Diff line
		@@ -13,6 +13,7 @@
		#include <linux/slab.h>
		#include <linux/thermal.h>
		#include "tsens.h"
		#include "thermal_core.h"
		#include "qcom/qti_virtual_sensor.h"

		LIST_HEAD(tsens_device_list);
		@@ -199,6 +200,8 @@ static int get_device_tree_data(struct platform_device *pdev,
		else
		tmdev->min_temp_sensor_id = MIN_TEMP_DEF_OFFSET;

		tmdev->tsens_reinit_wa =
		of_property_read_bool(of_node, "tsens-reinit-wa");
		return rc;
		}

		@@ -253,6 +256,38 @@ static int tsens_tm_remove(struct platform_device *pdev)
		return 0;
		}

		static void tsens_therm_fwk_notify(struct work_struct *work)
		{
		int i, rc, temp;
		struct tsens_device *tmdev =
		container_of(work, struct tsens_device, therm_fwk_notify);

		TSENS_DBG(tmdev, "Controller %pK\n", &tmdev->phys_addr_tm);
		for (i = 0; i < TSENS_MAX_SENSORS; i++) {
		if (tmdev->ops->sensor_en(tmdev, i)) {
		rc = tsens_get_temp(&tmdev->sensor[i], &temp);
		if (rc) {
		pr_err("%s: Error:%d reading temp sensor:%d\n",
		__func__, rc, i);
		continue;
		}
		TSENS_DBG(tmdev, "Calling trip_temp for sensor %d\n",
		i);
		of_thermal_handle_trip_temp(tmdev->sensor[i].tzd, temp);
		}
		}
		if (tmdev->min_temp_sensor_id != MIN_TEMP_DEF_OFFSET) {
		rc = tsens_get_temp(&tmdev->min_temp, &temp);
		if (rc) {
		pr_err("%s: Error:%d reading temp sensor:%d\n",
		__func__, rc, i);
		return;
		}
		TSENS_DBG(tmdev, "Calling trip_temp for sensor %d\n", i);
		of_thermal_handle_trip_temp(tmdev->min_temp.tzd, temp);
		}
		}

		int tsens_tm_probe(struct platform_device *pdev)
		{
		struct tsens_device *tmdev = NULL;
		@@ -282,6 +317,16 @@ int tsens_tm_probe(struct platform_device *pdev)
		return rc;
		}

		snprintf(tsens_name, sizeof(tsens_name), "tsens_wq_%pa",
		&tmdev->phys_addr_tm);

		tmdev->tsens_reinit_work = alloc_workqueue(tsens_name,
		WQ_HIGHPRI, 0);
		if (!tmdev->tsens_reinit_work) {
		rc = -ENOMEM;
		return rc;
		}
		INIT_WORK(&tmdev->therm_fwk_notify, tsens_therm_fwk_notify);
		rc = tsens_thermal_zone_register(tmdev);
		if (rc) {
		pr_err("Error registering the thermal zone\n");

drivers/thermal/tsens.h

+4 −1

Original line number	Diff line number	Diff line
		@@ -211,6 +211,9 @@ struct tsens_device {
		int trdy_fail_ctr;
		struct tsens_sensor min_temp;
		u8 min_temp_sensor_id;
		struct workqueue_struct *tsens_reinit_work;
		struct work_struct therm_fwk_notify;
		bool tsens_reinit_wa;
		struct tsens_sensor sensor[0];
		};

drivers/thermal/tsens2xxx.c

+57 −9

Original line number	Diff line number	Diff line
		@@ -11,6 +11,7 @@
		#include <linux/vmalloc.h>
		#include "tsens.h"
		#include "thermal_core.h"
		#include <soc/qcom/scm.h>

		#define TSENS_DRIVER_NAME "msm-tsens"

		@@ -66,6 +67,9 @@
		#define TSENS_TM_0C_THRESHOLDS(n) ((n) + 0x1c)
		#define TSENS_MAX_READ_FAIL 50

		#define TSENS_INIT_ID 0x5
		#define TSENS_RECOVERY_LOOP_COUNT 5

		static void msm_tsens_convert_temp(int last_temp, int *temp)
		{
		int code_mask = ~TSENS_TM_CODE_BIT_MASK;
		@@ -80,10 +84,10 @@ static void msm_tsens_convert_temp(int last_temp, int *temp)

		static int tsens2xxx_get_temp(struct tsens_sensor sensor, int temp)
		{
		struct tsens_device *tmdev = NULL;
		unsigned int code;
		struct tsens_device tmdev = NULL, tmdev_itr;
		unsigned int code, ret, tsens_ret;
		void __iomem sensor_addr, trdy;
		int last_temp = 0, last_temp2 = 0, last_temp3 = 0;
		int last_temp = 0, last_temp2 = 0, last_temp3 = 0, count = 0;

		if (!sensor)
		return -EINVAL;
		@@ -93,21 +97,65 @@ static int tsens2xxx_get_temp(struct tsens_sensor sensor, int temp)
		trdy = TSENS_TM_TRDY(tmdev->tsens_tm_addr);

		code = readl_relaxed_no_log(trdy);

		if (!((code & TSENS_TM_TRDY_FIRST_ROUND_COMPLETE) >>
		TSENS_TM_TRDY_FIRST_ROUND_COMPLETE_SHIFT)) {
		pr_err("tsens device first round not complete0x%x, ctr is %d\n",
		code, tmdev->trdy_fail_ctr);
		tmdev->trdy_fail_ctr++;
		pr_err("%s: tsens device first round not complete0x%x\n",
		__func__, code);
		/* Wait for 2.5 ms for tsens controller to recover */
		do {
		udelay(500);
		code = readl_relaxed_no_log(trdy);
		if (code & TSENS_TM_TRDY_FIRST_ROUND_COMPLETE) {
		TSENS_DUMP(tmdev, "%s",
		"tsens controller recovered\n");
		goto sensor_read;
		}
		} while (++count < TSENS_RECOVERY_LOOP_COUNT);

		/*
		* TSENS controller did not recover,
		* proceed with SCM call to re-init it
		*/
		if (tmdev->tsens_reinit_wa) {
		struct scm_desc desc = { 0 };

		if (tmdev->trdy_fail_ctr >= TSENS_MAX_READ_FAIL) {
		if (tmdev->ops->dbg)
		tmdev->ops->dbg(tmdev, 0,
		TSENS_DBG_LOG_BUS_ID_DATA, NULL);
		/* Make an scm call to re-init TSENS */
		TSENS_DBG(tmdev, "%s",
		"Calling TZ to re-init TSENS\n");
		ret = scm_call2(SCM_SIP_FNID(SCM_SVC_TSENS,
		TSENS_INIT_ID), &desc);
		TSENS_DBG(tmdev, "%s",
		"return from scm call\n");
		if (ret) {
		pr_err("%s: scm call failed %d\n",
		__func__, ret);
		BUG();
		}
		tsens_ret = desc.ret[0];
		if (tsens_ret) {
		pr_err("%s: scm call failed to init tsens %d\n",
		__func__, tsens_ret);
		BUG();
		}
		/* Notify thermal fwk */
		list_for_each_entry(tmdev_itr,
		&tsens_device_list, list) {
		queue_work(tmdev_itr->tsens_reinit_work,
		&tmdev_itr->therm_fwk_notify);
		}

		return -ENODATA;
		} else {
		pr_err("%s: tsens controller got reset\n", __func__);
		BUG();
		}
		return -EAGAIN;
		}

		sensor_read:

		tmdev->trdy_fail_ctr = 0;

include/soc/qcom/scm.h

+2 −1

Original line number	Diff line number	Diff line
		/* SPDX-License-Identifier: GPL-2.0-only */
		/*
		* Copyright (c) 2010-2018, The Linux Foundation. All rights reserved.
		* Copyright (c) 2010-2019, The Linux Foundation. All rights reserved.
		*/

		#ifndef __MACH_SCM_H
		@@ -26,6 +26,7 @@
		#define SCM_SVC_SMMU_PROGRAM 0x15
		#define SCM_SVC_QDSS 0x16
		#define SCM_SVC_RTIC 0x19
		#define SCM_SVC_TSENS 0x1E
		#define SCM_SVC_TZSCHEDULER 0xFC

		#define SCM_FUSE_READ 0x7