drivers/devfreq/armbw-pm.c - kernel/msm.git - Git at Google

 /*
  * 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);
	/*
	* 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);