msm: tsens: Re-initialize TSENS controller (da4e1017) · Commits · e / devices / android_kernel_xiaomi_sm6125

drivers/thermal/msm-tsens.c

+37 −1

Original line number	Diff line number	Diff line
		/* Copyright (c) 2017-2019, The Linux Foundation. All rights reserved.
		/* Copyright (c) 2017-2020, The Linux Foundation. All rights reserved.
		*
		* This program is free software; you can redistribute it and/or modify
		* it under the terms of the GNU General Public License version 2 and
		@@ -20,6 +20,7 @@
		#include <linux/io.h>
		#include <linux/slab.h>
		#include <linux/thermal.h>
		#include "thermal_core.h"
		#include "tsens.h"
		#include "qcom/qti_virtual_sensor.h"

		@@ -189,6 +190,8 @@ static int get_device_tree_data(struct platform_device *pdev,
		}
		}
		}
		tmdev->tsens_reinit_wa =
		of_property_read_bool(of_node, "tsens-reinit-wa");

		return rc;
		}
		@@ -233,6 +236,28 @@ 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);
		}
		}
		}

		int tsens_tm_probe(struct platform_device *pdev)
		{
		struct tsens_device *tmdev = NULL;
		@@ -262,6 +287,17 @@ 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
		/* Copyright (c) 2017-2019, The Linux Foundation. All rights reserved.
		/* Copyright (c) 2017-2020, The Linux Foundation. All rights reserved.
		*
		* This program is free software; you can redistribute it and/or modify
		* it under the terms of the GNU General Public License version 2 and
		@@ -228,6 +228,9 @@ struct tsens_device {
		const struct tsens_data *ctrl_data;
		struct tsens_mtc_sysfs mtcsys;
		int trdy_fail_ctr;
		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

+59 −11

Original line number	Diff line number	Diff line
		/* Copyright (c) 2012-2019, The Linux Foundation. All rights reserved.
		/* Copyright (c) 2012-2020, The Linux Foundation. All rights reserved.
		*
		* This program is free software; you can redistribute it and/or modify
		* it under the terms of the GNU General Public License version 2 and
		@@ -17,6 +17,7 @@
		#include <linux/err.h>
		#include <linux/of.h>
		#include <linux/vmalloc.h>
		#include <soc/qcom/scm.h>
		#include "tsens.h"
		#include "thermal_core.h"

		@@ -66,6 +67,8 @@
		#define TSENS_TM_TRDY(n) ((n) + 0xe4)
		#define TSENS_TM_TRDY_FIRST_ROUND_COMPLETE BIT(3)
		#define TSENS_TM_TRDY_FIRST_ROUND_COMPLETE_SHIFT 3
		#define TSENS_INIT_ID 0x5
		#define TSENS_RECOVERY_LOOP_COUNT 5

		static void msm_tsens_convert_temp(int last_temp, int *temp)
		{
		@@ -81,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;
		@@ -94,22 +97,67 @@ 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 >= 50) {
		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;

		code = readl_relaxed_no_log(sensor_addr +

include/soc/qcom/scm.h

+2 −1

Original line number	Diff line number	Diff line
		/* Copyright (c) 2010-2019, The Linux Foundation. All rights reserved.
		/* Copyright (c) 2010-2020, The Linux Foundation. All rights reserved.
		*
		* This program is free software; you can redistribute it and/or modify
		* it under the terms of the GNU General Public License version 2 and
		@@ -30,6 +30,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 TZ_SVC_QUP_FW_LOAD 0x1F