drivers/devfreq/armbw-pm.c

/*
 * Copyright (c) 2014, 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
 * only version 2 as published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 */

#define pr_fmt(fmt) "armbw-pm: " fmt

#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/init.h>
#include <linux/io.h>
#include <linux/delay.h>
#include <linux/err.h>
#include <linux/errno.h>
#include <linux/interrupt.h>
#include <linux/platform_device.h>
#include <linux/of.h>
#include <linux/of_irq.h>
#include <linux/slab.h>
#include <linux/irq.h>
#include <linux/cpu_pm.h>
#include <linux/cpu.h>
#include "governor.h"
#include "governor_bw_hwmon.h"


#define DEFINE_CP15_READ(name, op1, n, m, op2)				\
static u32 read_##name(void)						\
{									\
	u32 val;							\
	asm volatile ("mrc p15, " #op1 ", %0, c" #n ", c" #m ", " #op2  \
			: "=r" (val));				        \
	return val;							\
}

#define DEFINE_CP15_WRITE(name, op1, n, m, op2)				\
static void write_##name(u32 val)					\
{									\
	asm volatile ("mcr p15, " #op1 ", %0, c" #n ", c" #m", "#op2	\
			: : "r" (val));					\
}

#define DEFINE_CP15_RW(name, op1, n, m, op2)	\
DEFINE_CP15_READ(name, op1, n, m, op2)	\
DEFINE_CP15_WRITE(name, op1, n, m, op2)

DEFINE_CP15_WRITE(pmselr, 0, 9, 12, 5)
DEFINE_CP15_WRITE(pmcntenset, 0, 9, 12, 1)
DEFINE_CP15_WRITE(pmcntenclr, 0, 9, 12, 2)
DEFINE_CP15_RW(pmovsr, 0, 9, 12, 3)
DEFINE_CP15_WRITE(pmxevtyper, 0, 9, 13, 1)
DEFINE_CP15_RW(pmxevcntr, 0, 9, 13, 2)
DEFINE_CP15_WRITE(pmintenset, 0, 9, 14, 1)
DEFINE_CP15_WRITE(pmintenclr, 0, 9, 14, 2)
DEFINE_CP15_WRITE(pmcr, 0, 9, 12, 0)

struct bwmon_data {
	int cpu;
	u32 saved_evcntr;
	unsigned long count;
	u32 prev_rw_start_val;
	u32 limit;
};

static DEFINE_SPINLOCK(bw_lock);
static struct bw_hwmon *globalhw;
static struct work_struct irqwork;
static int bw_irq;
static DEFINE_PER_CPU(struct bwmon_data, gov_data);
static int use_cnt;
static DEFINE_MUTEX(use_lock);
static struct workqueue_struct *bw_wq;
static u32 bytes_per_beat;

#define RW_NUM 0x19
#define RW_MON 0

static void mon_enable(void *info)
{
	/* Clear previous overflow state for given counter*/
	write_pmovsr(BIT(RW_MON));
	/* Enable event counter n */
	write_pmcntenset(BIT(RW_MON));
}

static void mon_disable(void *info)
{
	write_pmcntenclr(BIT(RW_MON));
}

static void mon_irq_enable(void *info)
{
	write_pmintenset(BIT(RW_MON));
}

static void mon_irq_disable(void *info)
{
	write_pmintenclr(BIT(RW_MON));
}

static void mon_set_counter(void *count)
{
	write_pmxevcntr(*(u32 *) count);
}

static void mon_bw_init(void *evcntrval)
{
	u32 count;

	if (!evcntrval)
		count = 0xFFFFFFFF;
	else
		count = *(u32 *) evcntrval;

	write_pmcr(BIT(0));
	write_pmselr(RW_MON);
	write_pmxevtyper(RW_NUM);
	write_pmxevcntr(count);
}

static void percpu_bwirq_enable(void *info)
{
	enable_percpu_irq(bw_irq, IRQ_TYPE_EDGE_RISING);
}

static void percpu_bwirq_disable(void *info)
{
	disable_percpu_irq(bw_irq);
}

static irqreturn_t mon_intr_handler(int irq, void *dev_id)
{
	queue_work(bw_wq, &irqwork);
	return IRQ_HANDLED;
}

static void bwmon_work(struct work_struct *work)
{
	update_bw_hwmon(globalhw);
}

static unsigned int beats_to_mbps(long long beats, unsigned int us)
{
	beats *= USEC_PER_SEC;
	beats *= bytes_per_beat;
	do_div(beats, us);
	beats = DIV_ROUND_UP_ULL(beats, SZ_1M);

	return beats;
}

static unsigned int mbps_to_beats(unsigned long mbps, unsigned int ms,
				  unsigned int tolerance_percent)
{
	mbps *= (100 + tolerance_percent) * ms;
	mbps /= 100;
	mbps = DIV_ROUND_UP(mbps, MSEC_PER_SEC);
	mbps = mult_frac(mbps, SZ_1M, bytes_per_beat);
	return mbps;
}

static long mon_get_bw_count(u32 start_val)
{
	u32 overflow, count;

	count = read_pmxevcntr();
	overflow = read_pmovsr();
	if (overflow & BIT(RW_MON))
		return 0xFFFFFFFF - start_val + count;
	else
		return count - start_val;
}

static void get_beat_count(void *arg)
{
	int cpu = smp_processor_id();
	struct bwmon_data *data = &per_cpu(gov_data, cpu);

	mon_disable(NULL);
	data->count = mon_get_bw_count(data->prev_rw_start_val);
}

static unsigned long measure_bw_and_set_irq(struct bw_hwmon *hw,
					unsigned int tol, unsigned int us)
{
	unsigned long bw = 0;
	unsigned long tempbw;
	int cpu;
	struct bwmon_data *data;
	unsigned int sample_ms = hw->df->profile->polling_ms;

	spin_lock(&bw_lock);
	on_each_cpu(get_beat_count, NULL, true);
	for_each_possible_cpu(cpu) {
		data = &per_cpu(gov_data, cpu);

		tempbw = beats_to_mbps(data->count, us);
		data->limit = mbps_to_beats(tempbw, sample_ms, tol);
		data->prev_rw_start_val = 0xFFFFFFFF - data->limit;
		if (cpu_online(cpu))
			smp_call_function_single(cpu, mon_set_counter,
					      &(data->prev_rw_start_val), true);
		bw += tempbw;
		data->count = 0;
	}
	on_each_cpu(mon_enable, NULL, true);
	spin_unlock(&bw_lock);
	return bw;
}

static void save_hotplugstate(void)
{
	int cpu = smp_processor_id();
	struct bwmon_data *data;

	data = &per_cpu(gov_data, cpu);
	percpu_bwirq_disable(NULL);
	mon_disable(NULL);
	data->saved_evcntr = read_pmxevcntr();
	data->count = mon_get_bw_count(data->prev_rw_start_val);
}

static void restore_hotplugstate(void)
{
	int cpu = smp_processor_id();
	u32 count;
	struct bwmon_data *data;

	data = &per_cpu(gov_data, cpu);
	percpu_bwirq_enable(NULL);
	if (data->count != 0)
		count = data->saved_evcntr;
	else
		count = data->prev_rw_start_val = 0xFFFFFFFF - data->limit;
	mon_bw_init(&count);
	mon_irq_enable(NULL);
	mon_enable(NULL);
}

static void save_pmstate(void)
{
	int cpu = smp_processor_id();
	struct bwmon_data *data;

	data = &per_cpu(gov_data, cpu);
	mon_disable(NULL);
	data->saved_evcntr = read_pmxevcntr();
}

static void restore_pmstate(void)
{
	int cpu = smp_processor_id();
	u32 count;
	struct bwmon_data *data;

	data = &per_cpu(gov_data, cpu);
	count = data->saved_evcntr;
	mon_bw_init(&count);
	mon_irq_enable(NULL);
	mon_enable(NULL);
}

static int pm_notif(struct notifier_block *nb, unsigned long action,
			void *data)
{
	switch (action) {
	case CPU_PM_ENTER:
		save_pmstate();
		break;
	case CPU_PM_ENTER_FAILED:
	case CPU_PM_EXIT:
		restore_pmstate();
		break;
	}

	return NOTIFY_OK;
}

static struct notifier_block bwmon_cpu_pm_nb = {
	.notifier_call = pm_notif,
};

static int hotplug_notif(struct notifier_block *nb, unsigned long action,
			void *data)
{
	switch (action) {
	case CPU_DYING:
		spin_lock(&bw_lock);
		save_hotplugstate();
		spin_unlock(&bw_lock);
		break;
	case CPU_STARTING:
		spin_lock(&bw_lock);
		restore_hotplugstate();
		spin_unlock(&bw_lock);
		break;
	}

	return NOTIFY_OK;
}

static struct notifier_block cpu_hotplug_nb = {
	.notifier_call = hotplug_notif,
};

static int register_notifier(void)
{
	int ret = 0;

	mutex_lock(&use_lock);
	if (use_cnt == 0) {
		ret = cpu_pm_register_notifier(&bwmon_cpu_pm_nb);
		if (ret)
			goto out;
		ret = register_cpu_notifier(&cpu_hotplug_nb);
		if (ret) {
			cpu_pm_unregister_notifier(&bwmon_cpu_pm_nb);
			goto out;
		}
	}
	use_cnt++;
out:
	mutex_unlock(&use_lock);
	return ret;
}

static void unregister_notifier(void)
{
	mutex_lock(&use_lock);
	if (use_cnt == 1) {
		unregister_cpu_notifier(&cpu_hotplug_nb);
		cpu_pm_unregister_notifier(&bwmon_cpu_pm_nb);
	} else if (use_cnt == 0) {
		pr_warn("Notifier ref count unbalanced\n");
		goto out;
	}
	use_cnt--;
out:
	mutex_unlock(&use_lock);
}

static void stop_bw_hwmon(struct bw_hwmon *hw)
{
	unregister_notifier();
	on_each_cpu(mon_disable, NULL, true);
	on_each_cpu(mon_irq_disable, NULL, true);
	on_each_cpu(percpu_bwirq_disable, NULL, true);
	free_percpu_irq(bw_irq, &gov_data);
}

static int start_bw_hwmon(struct bw_hwmon *hw, unsigned long mbps)
{
	u32 limit;
	int cpu;
	struct bwmon_data *data;
	struct device *dev = hw->df->dev.parent;
	int ret;

	ret = request_percpu_irq(bw_irq, mon_intr_handler,
				"bw_hwmon", &gov_data);
	if (ret) {
		dev_err(dev, "Unable to register interrupt handler!\n");
		return ret;
	}

	get_online_cpus();
	on_each_cpu(mon_bw_init, NULL, true);
	on_each_cpu(mon_disable, NULL, true);

	ret = register_notifier();
	if (ret) {
		pr_err("Unable to register notifier\n");
		return ret;
	}

	limit = mbps_to_beats(mbps, hw->df->profile->polling_ms, 0);
	limit /= num_online_cpus();

	for_each_possible_cpu(cpu) {
		data = &per_cpu(gov_data, cpu);
		data->limit = limit;
		data->prev_rw_start_val = 0xFFFFFFFF - data->limit;
	}

	INIT_WORK(&irqwork, bwmon_work);

	on_each_cpu(percpu_bwirq_enable, NULL, true);
	on_each_cpu(mon_irq_enable, NULL, true);
	on_each_cpu(mon_enable, NULL, true);
	put_online_cpus();
	return 0;
}

static int armbw_pm_driver_probe(struct platform_device *pdev)
{
	struct device *dev = &pdev->dev;
	struct bw_hwmon *bw;
	int ret;

	bw = devm_kzalloc(dev, sizeof(*bw), GFP_KERNEL);
	if (!bw)
		return -ENOMEM;
	bw->dev = dev;

	bw_irq = platform_get_irq(pdev, 0);
	if (bw_irq < 0) {
		pr_err("Unable to get IRQ number\n");
		return bw_irq;
	}

	ret = of_property_read_u32(dev->of_node, "qcom,bytes-per-beat",
					&bytes_per_beat);

	if (ret) {
		pr_err("Unable to read bytes per beat\n");
		return ret;
	}

	bw->start_hwmon = &start_bw_hwmon;
	bw->stop_hwmon = &stop_bw_hwmon;
	bw->meas_bw_and_set_irq = &measure_bw_and_set_irq;
	globalhw = bw;

	ret = register_bw_hwmon(dev, bw);
	if (ret) {
		pr_err("CPUBW hwmon registration failed\n");
		return ret;
	}
	return 0;
}

static struct of_device_id match_table[] = {
	{ .compatible = "qcom,armbw-pm" },
	{}
};

static struct platform_driver armbw_pm_driver = {
	.probe = armbw_pm_driver_probe,
	.driver = {
		.name = "armbw-pm",
		.of_match_table = match_table,
		.owner = THIS_MODULE,
	},
};

static int __init armbw_pm_init(void)
{
	bw_wq = alloc_workqueue("armbw-pm-bwmon", WQ_HIGHPRI, 2);
	return platform_driver_register(&armbw_pm_driver);
}
module_init(armbw_pm_init);

static void __exit armbw_pm_exit(void)
{
	platform_driver_unregister(&armbw_pm_driver);
	destroy_workqueue(bw_wq);
}
module_exit(armbw_pm_exit);