android_kernel_samsung_heroqlte/drivers/thermal/lmh_interface.c

/* Copyright (c) 2014-2015, 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) "%s:%s " fmt, KBUILD_MODNAME, __func__

#include <linux/module.h>
#include <linux/device.h>
#include <linux/interrupt.h>
#include <linux/kernel.h>
#include <linux/io.h>
#include <linux/err.h>
#include <linux/of.h>
#include <linux/sysfs.h>
#include <linux/rwsem.h>
#include <linux/debugfs.h>
#include <linux/thermal.h>
#include <linux/slab.h>
#include "lmh_interface.h"
#include <linux/string.h>
#include <linux/uaccess.h>

#define LMH_MON_NAME			"lmh_monitor"
#define LMH_ISR_POLL_DELAY		"interrupt_poll_delay_msec"
#define LMH_TRACE_ENABLE		"hw_trace_enable"
#define LMH_TRACE_INTERVAL		"hw_trace_interval"
#define LMH_DBGFS_DIR			"debug"
#define LMH_DBGFS_READ			"data"
#define LMH_DBGFS_CONFIG_READ		"config"
#define LMH_DBGFS_READ_TYPES		"data_types"
#define LMH_DBGFS_CONFIG_TYPES		"config_types"
#define LMH_TRACE_INTERVAL_XO_TICKS	250

struct lmh_mon_threshold {
	long				value;
	bool				active;
};

struct lmh_device_data {
	char				device_name[LMH_NAME_MAX];
	struct lmh_device_ops		*device_ops;
	uint32_t			max_level;
	int				curr_level;
	int				*levels;
	struct dentry			*dev_parent;
	struct dentry			*max_lvl_fs;
	struct dentry			*curr_lvl_fs;
	struct dentry			*avail_lvl_fs;
	struct list_head		list_ptr;
	struct rw_semaphore		lock;
	struct device			dev;
};

struct lmh_mon_sensor_data {
	struct list_head		list_ptr;
	char				sensor_name[LMH_NAME_MAX];
	struct lmh_sensor_ops		*sensor_ops;
	struct rw_semaphore		lock;
	struct lmh_mon_threshold	trip[LMH_TRIP_MAX];
	struct thermal_zone_device	*tzdev;
	enum thermal_device_mode	mode;
};

struct lmh_mon_driver_data {
	struct dentry			*debugfs_parent;
	struct dentry			*poll_fs;
	struct dentry			*enable_hw_log;
	struct dentry			*hw_log_delay;
	uint32_t			hw_log_enable;
	uint64_t			hw_log_interval;
	struct dentry			*debug_dir;
	struct dentry			*debug_read;
	struct dentry			*debug_config;
	struct dentry			*debug_read_type;
	struct dentry			*debug_config_type;
	struct lmh_debug_ops		*debug_ops;
};

enum lmh_read_type {
	LMH_DEBUG_READ_TYPE,
	LMH_DEBUG_CONFIG_TYPE,
	LMH_PROFILES,
};

static struct lmh_mon_driver_data	*lmh_mon_data;
static struct class			lmh_class_info = {
	.name = "msm_limits",
};
static DECLARE_RWSEM(lmh_mon_access_lock);
static LIST_HEAD(lmh_sensor_list);
static DECLARE_RWSEM(lmh_dev_access_lock);
static LIST_HEAD(lmh_device_list);

#define LMH_CREATE_DEBUGFS_FILE(_node, _name, _mode, _parent, _data, _ops, \
	_ret) do { \
		_node = debugfs_create_file(_name, _mode, _parent, \
				_data, _ops); \
		if (IS_ERR(_node)) { \
			_ret = PTR_ERR(_node); \
			pr_err("Error creating debugfs file:%s. err:%d\n", \
					_name, _ret); \
		} \
	} while (0)

#define LMH_CREATE_DEBUGFS_DIR(_node, _name, _parent, _ret) \
	do { \
		_node = debugfs_create_dir(_name, _parent); \
		if (IS_ERR(_node)) { \
			_ret = PTR_ERR(_node); \
			pr_err("Error creating debugfs dir:%s. err:%d\n", \
					_name, _ret); \
		} \
	} while (0)

#define LMH_HW_LOG_FS(_name) \
static int _name##_get(void *data, u64 *val) \
{ \
	*val = lmh_mon_data->_name; \
	return 0; \
} \
static int _name##_set(void *data, u64 val) \
{ \
	struct lmh_mon_sensor_data *lmh_sensor = data; \
	int ret = 0; \
	lmh_mon_data->_name = val; \
	if (lmh_mon_data->hw_log_enable) \
		ret = lmh_sensor->sensor_ops->enable_hw_log( \
			lmh_mon_data->hw_log_interval \
				, lmh_mon_data->hw_log_enable); \
	else \
		ret = lmh_sensor->sensor_ops->disable_hw_log(); \
	return ret; \
} \
DEFINE_SIMPLE_ATTRIBUTE(_name##_fops, _name##_get, _name##_set, \
	"%llu\n");

#define LMH_DEV_GET(_name) \
static ssize_t _name##_get(struct device *dev, \
	struct device_attribute *attr, char *buf) \
{ \
	struct lmh_device_data *lmh_dev = container_of(dev, \
			struct lmh_device_data, dev); \
	return snprintf(buf, LMH_NAME_MAX, "%d", lmh_dev->_name); \
} \

LMH_HW_LOG_FS(hw_log_enable);
LMH_HW_LOG_FS(hw_log_interval);
LMH_DEV_GET(max_level);
LMH_DEV_GET(curr_level);

static ssize_t curr_level_set(struct device *dev,
	struct device_attribute *attr, const char *buf, size_t count)
{
	struct lmh_device_data *lmh_dev = container_of(dev,
		struct lmh_device_data, dev);
	int val = 0, ret = 0;

	ret = kstrtouint(buf, 0, &val);
	if (ret < 0) {
		pr_err("Invalid input [%s]. err:%d\n", buf, ret);
		return ret;
	}
	return lmh_set_dev_level(lmh_dev->device_name, val);
}

static ssize_t avail_level_get(struct device *dev,
	struct device_attribute *attr, char *buf)
{
	struct lmh_device_data *lmh_dev = container_of(dev,
		struct lmh_device_data, dev);
	uint32_t *type_list = NULL;
	int ret = 0, count = 0, lvl_buf_count = 0, idx = 0;
	char *lvl_buf = NULL;

	if (!lmh_dev || !lmh_dev->levels || !lmh_dev->max_level) {
		pr_err("Invalid input\n");
		return -EINVAL;
	}
	type_list = lmh_dev->levels;
	lvl_buf = kzalloc(PAGE_SIZE, GFP_KERNEL);
	if (!lvl_buf) {
		pr_err("Error allocating memory\n");
		return -ENOMEM;
	}
	for (idx = 0; (idx < lmh_dev->max_level) && (lvl_buf_count < PAGE_SIZE)
		; idx++) {
		count = snprintf(lvl_buf + lvl_buf_count,
				PAGE_SIZE - lvl_buf_count, "%d ",
				type_list[idx]);
		if (count + lvl_buf_count >= PAGE_SIZE) {
			pr_err("overflow.\n");
			break;
		}
		lvl_buf_count += count;
	}
	count = snprintf(lvl_buf + lvl_buf_count, PAGE_SIZE - lvl_buf_count,
			"\n");
	if (count + lvl_buf_count < PAGE_SIZE)
		lvl_buf_count += count;

	count = snprintf(buf, lvl_buf_count + 1, lvl_buf);
	if (count > PAGE_SIZE) {
		pr_err("copy to user buf failed\n");
		ret = -EFAULT;
		goto lvl_get_exit;
	}

lvl_get_exit:
	kfree(lvl_buf);
	return (ret) ? ret : count;
}

static int lmh_create_dev_sysfs(struct lmh_device_data *lmh_dev)
{
	int ret = 0;
	static DEVICE_ATTR(level, 0600, curr_level_get, curr_level_set);
	static DEVICE_ATTR(available_levels, 0400, avail_level_get, NULL);
	static DEVICE_ATTR(total_levels, 0400, max_level_get, NULL);

	lmh_dev->dev.class = &lmh_class_info;
	dev_set_name(&lmh_dev->dev, "%s", lmh_dev->device_name);
	ret = device_register(&lmh_dev->dev);
	if (ret) {
		pr_err("Error registering profile device. err:%d\n", ret);
		return ret;
	}
	ret = device_create_file(&lmh_dev->dev, &dev_attr_level);
	if (ret) {
		pr_err("Error creating profile level sysfs node. err:%d\n",
			ret);
		goto dev_sysfs_exit;
	}
	ret = device_create_file(&lmh_dev->dev, &dev_attr_total_levels);
	if (ret) {
		pr_err("Error creating total level sysfs node. err:%d\n",
			ret);
		goto dev_sysfs_exit;
	}
	ret = device_create_file(&lmh_dev->dev, &dev_attr_available_levels);
	if (ret) {
		pr_err("Error creating available level sysfs node. err:%d\n",
			ret);
		goto dev_sysfs_exit;
	}

dev_sysfs_exit:
	if (ret)
		device_unregister(&lmh_dev->dev);
	return ret;
}

static int lmh_create_debugfs_nodes(struct lmh_mon_sensor_data *lmh_sensor)
{
	int ret = 0;

	lmh_mon_data->hw_log_enable = 0;
	lmh_mon_data->hw_log_interval = LMH_TRACE_INTERVAL_XO_TICKS;
	LMH_CREATE_DEBUGFS_FILE(lmh_mon_data->enable_hw_log, LMH_TRACE_ENABLE,
		0600, lmh_mon_data->debugfs_parent, (void *)lmh_sensor,
		&hw_log_enable_fops, ret);
	if (ret)
		goto create_debugfs_exit;
	LMH_CREATE_DEBUGFS_FILE(lmh_mon_data->hw_log_delay, LMH_TRACE_INTERVAL,
		0600, lmh_mon_data->debugfs_parent, (void *)lmh_sensor,
		&hw_log_interval_fops, ret);
	if (ret)
		goto create_debugfs_exit;

create_debugfs_exit:
	if (ret)
		debugfs_remove_recursive(lmh_mon_data->debugfs_parent);
	return ret;
}

static struct lmh_mon_sensor_data *lmh_match_sensor_ops(
		struct lmh_sensor_ops *ops)
{
	struct lmh_mon_sensor_data *lmh_sensor = NULL;

	list_for_each_entry(lmh_sensor, &lmh_sensor_list, list_ptr) {
		if (lmh_sensor->sensor_ops == ops)
			return lmh_sensor;
	}

	return NULL;
}

static struct lmh_mon_sensor_data *lmh_match_sensor_name(char *sensor_name)
{
	struct lmh_mon_sensor_data *lmh_sensor = NULL;

	list_for_each_entry(lmh_sensor, &lmh_sensor_list, list_ptr) {
		if (!strnicmp(lmh_sensor->sensor_name, sensor_name,
			LMH_NAME_MAX))
			return lmh_sensor;
	}

	return NULL;
}

static void lmh_evaluate_and_notify(struct lmh_mon_sensor_data *lmh_sensor,
	       long val)
{
	int idx = 0, trip = 0;
	bool cond = false;

	for (idx = 0; idx < LMH_TRIP_MAX; idx++) {
		if (!lmh_sensor->trip[idx].active)
			continue;
		if (idx == LMH_HIGH_TRIP) {
			trip = THERMAL_TRIP_CONFIGURABLE_HI;
			cond = (val >= lmh_sensor->trip[idx].value);
		} else {
			trip = THERMAL_TRIP_CONFIGURABLE_LOW;
			cond = (val <= lmh_sensor->trip[idx].value);
		}
		if (cond) {
			lmh_sensor->trip[idx].active = false;
			thermal_sensor_trip(lmh_sensor->tzdev, trip, val);
		}
	}
}

void lmh_update_reading(struct lmh_sensor_ops *ops, long trip_val)
{
	struct lmh_mon_sensor_data *lmh_sensor = NULL;

	if (!ops) {
		pr_err("Invalid input\n");
		return;
	}

	down_read(&lmh_mon_access_lock);
	lmh_sensor = lmh_match_sensor_ops(ops);
	if (!lmh_sensor) {
		pr_err("Invalid ops\n");
		goto interrupt_exit;
	}
	down_write(&lmh_sensor->lock);
	pr_debug("Sensor:[%s] intensity:%ld\n", lmh_sensor->sensor_name,
		trip_val);
	lmh_evaluate_and_notify(lmh_sensor, trip_val);
interrupt_exit:
	if (lmh_sensor)
		up_write(&lmh_sensor->lock);
	up_read(&lmh_mon_access_lock);
	return;
}

static int lmh_sensor_read(struct thermal_zone_device *dev, unsigned long *val)
{
	int ret = 0;
	struct lmh_mon_sensor_data *lmh_sensor;

	if (!val || !dev || !dev->devdata) {
		pr_err("Invalid input\n");
		return -EINVAL;
	}
	lmh_sensor = dev->devdata;
	down_read(&lmh_mon_access_lock);
	down_read(&lmh_sensor->lock);
	ret = lmh_sensor->sensor_ops->read(lmh_sensor->sensor_ops, val);
	if (ret) {
		pr_err("Error reading sensor:%s. err:%d\n",
				lmh_sensor->sensor_name, ret);
		goto unlock_and_exit;
	}
unlock_and_exit:
	up_read(&lmh_sensor->lock);
	up_read(&lmh_mon_access_lock);

	return ret;
}

static int lmh_get_mode(struct thermal_zone_device *dev,
		enum thermal_device_mode *mode)
{
	struct lmh_mon_sensor_data *lmh_sensor;

	if (!dev || !dev->devdata || !mode) {
		pr_err("Invalid input\n");
		return -EINVAL;
	}
	lmh_sensor = dev->devdata;
	*mode = lmh_sensor->mode;

	return 0;
}

static int lmh_get_trip_type(struct thermal_zone_device *dev,
		int trip, enum thermal_trip_type *type)
{
	if (!type || !dev || !dev->devdata || trip < 0
		|| trip >= LMH_TRIP_MAX) {
		pr_err("Invalid input\n");
		return -EINVAL;
	}

	switch (trip) {
	case LMH_HIGH_TRIP:
		*type = THERMAL_TRIP_CONFIGURABLE_HI;
		break;
	case LMH_LOW_TRIP:
		*type = THERMAL_TRIP_CONFIGURABLE_LOW;
		break;
	default:
		return -EINVAL;
	}

	return 0;
}

static int lmh_activate_trip(struct thermal_zone_device *dev,
		int trip, enum thermal_trip_activation_mode mode)
{
	struct lmh_mon_sensor_data *lmh_sensor;

	if (!dev || !dev->devdata || trip < 0 || trip >= LMH_TRIP_MAX) {
		pr_err("Invalid input\n");
		return -EINVAL;
	}

	lmh_sensor = dev->devdata;
	down_read(&lmh_mon_access_lock);
	down_write(&lmh_sensor->lock);
	lmh_sensor->trip[trip].active = (mode ==
					THERMAL_TRIP_ACTIVATION_ENABLED);
	up_write(&lmh_sensor->lock);
	up_read(&lmh_mon_access_lock);

	return 0;
}

static int lmh_get_trip_value(struct thermal_zone_device *dev,
		int trip, unsigned long *value)
{
	struct lmh_mon_sensor_data *lmh_sensor;

	if (!dev || !dev->devdata || trip < 0 || trip >= LMH_TRIP_MAX
		|| !value) {
		pr_err("Invalid input\n");
		return -EINVAL;
	}

	lmh_sensor = dev->devdata;
	down_read(&lmh_mon_access_lock);
	down_read(&lmh_sensor->lock);
	*value = lmh_sensor->trip[trip].value;
	up_read(&lmh_sensor->lock);
	up_read(&lmh_mon_access_lock);

	return 0;
}

static int lmh_set_trip_value(struct thermal_zone_device *dev,
		int trip, unsigned long value)
{
	struct lmh_mon_sensor_data *lmh_sensor;

	if (!dev || !dev->devdata || trip < 0 || trip >= LMH_TRIP_MAX) {
		pr_err("Invalid input\n");
		return -EINVAL;
	}

	lmh_sensor = dev->devdata;
	down_read(&lmh_mon_access_lock);
	down_write(&lmh_sensor->lock);
	lmh_sensor->trip[trip].value = value;
	up_write(&lmh_sensor->lock);
	up_read(&lmh_mon_access_lock);

	return 0;
}

static struct thermal_zone_device_ops lmh_sens_ops = {
	.get_temp = lmh_sensor_read,
	.get_mode = lmh_get_mode,
	.get_trip_type = lmh_get_trip_type,
	.activate_trip_type = lmh_activate_trip,
	.get_trip_temp = lmh_get_trip_value,
	.set_trip_temp = lmh_set_trip_value,
};

static int lmh_register_sensor(struct lmh_mon_sensor_data *lmh_sensor)
{
	int ret = 0;

	lmh_sensor->tzdev = thermal_zone_device_register(
			lmh_sensor->sensor_name, LMH_TRIP_MAX,
			(1 << LMH_TRIP_MAX) - 1, lmh_sensor, &lmh_sens_ops,
			NULL, 0 , 0);
	if (IS_ERR_OR_NULL(lmh_sensor->tzdev)) {
		ret = PTR_ERR(lmh_sensor->tzdev);
		pr_err("Error registering sensor:[%s] with thermal. err:%d\n",
			lmh_sensor->sensor_name, ret);
		return ret;
	}

	return ret;
}

static int lmh_sensor_init(struct lmh_mon_sensor_data *lmh_sensor,
	       char *sensor_name, struct lmh_sensor_ops *ops)
{
	int idx = 0, ret = 0;

	strlcpy(lmh_sensor->sensor_name, sensor_name, LMH_NAME_MAX);
	lmh_sensor->sensor_ops = ops;
	ops->new_value_notify = lmh_update_reading;
	for (idx = 0; idx < LMH_TRIP_MAX; idx++) {
		lmh_sensor->trip[idx].value = 0;
		lmh_sensor->trip[idx].active = false;
	}
	init_rwsem(&lmh_sensor->lock);
	if (list_empty(&lmh_sensor_list)
		&& !lmh_mon_data->enable_hw_log)
		lmh_create_debugfs_nodes(lmh_sensor);
	list_add_tail(&lmh_sensor->list_ptr, &lmh_sensor_list);

	return ret;
}

int lmh_sensor_register(char *sensor_name, struct lmh_sensor_ops *ops)
{
	int ret = 0;
	struct lmh_mon_sensor_data *lmh_sensor = NULL;

	if (!sensor_name || !ops) {
		pr_err("Invalid input\n");
		return -EINVAL;
	}

	if (!ops->read || !ops->enable_hw_log || !ops->disable_hw_log) {
		pr_err("Invalid ops input for sensor:%s\n", sensor_name);
		return -EINVAL;
	}
	down_write(&lmh_mon_access_lock);
	if (lmh_match_sensor_name(sensor_name)
		|| lmh_match_sensor_ops(ops)) {
		ret = -EEXIST;
		pr_err("Sensor[%s] exists\n", sensor_name);
		goto register_exit;
	}
	lmh_sensor = kzalloc(sizeof(struct lmh_mon_sensor_data), GFP_KERNEL);
	if (!lmh_sensor) {
		pr_err("kzalloc failed\n");
		ret = -ENOMEM;
		goto register_exit;
	}
	ret = lmh_sensor_init(lmh_sensor, sensor_name, ops);
	if (ret) {
		pr_err("Error registering sensor:%s. err:%d\n", sensor_name,
			ret);
		kfree(lmh_sensor);
		goto register_exit;
	}

	pr_debug("Registered Sensor:[%s]\n", sensor_name);

register_exit:
	up_write(&lmh_mon_access_lock);
	if (ret)
		return ret;
	ret = lmh_register_sensor(lmh_sensor);
	if (ret) {
		pr_err("Thermal Zone register failed for Sensor:[%s]\n"
			, sensor_name);
		return ret;
	}
	pr_debug("Registered Sensor:[%s]\n", sensor_name);
	return ret;
}

static void lmh_sensor_remove(struct lmh_sensor_ops *ops)
{
	struct lmh_mon_sensor_data *lmh_sensor = NULL;

	lmh_sensor = lmh_match_sensor_ops(ops);
	if (!lmh_sensor) {
		pr_err("No match for the sensor\n");
		goto deregister_exit;
	}
	down_write(&lmh_sensor->lock);
	thermal_zone_device_unregister(lmh_sensor->tzdev);
	list_del(&lmh_sensor->list_ptr);
	up_write(&lmh_sensor->lock);
	pr_debug("Deregistered sensor:[%s]\n", lmh_sensor->sensor_name);
	kfree(lmh_sensor);

deregister_exit:
	return;
}

void lmh_sensor_deregister(struct lmh_sensor_ops *ops)
{
	if (!ops) {
		pr_err("Invalid input\n");
		return;
	}

	down_write(&lmh_mon_access_lock);
	lmh_sensor_remove(ops);
	up_write(&lmh_mon_access_lock);

	return;
}

static struct lmh_device_data *lmh_match_device_name(char *device_name)
{
	struct lmh_device_data *lmh_device = NULL;

	list_for_each_entry(lmh_device, &lmh_device_list, list_ptr) {
		if (!strnicmp(lmh_device->device_name, device_name,
			LMH_NAME_MAX))
			return lmh_device;
	}

	return NULL;
}

static struct lmh_device_data *lmh_match_device_ops(struct lmh_device_ops *ops)
{
	struct lmh_device_data *lmh_device = NULL;

	list_for_each_entry(lmh_device, &lmh_device_list, list_ptr) {
		if (lmh_device->device_ops == ops)
			return lmh_device;
	}

	return NULL;
}

static int lmh_device_init(struct lmh_device_data *lmh_device,
		char *device_name, struct lmh_device_ops *ops)
{
	int ret = 0;

	ret = ops->get_curr_level(ops, &lmh_device->curr_level);
	if (ret) {
		pr_err("Error getting curr level for Device:[%s]. err:%d\n",
			device_name, ret);
		goto dev_init_exit;
	}
	ret = ops->get_available_levels(ops, NULL);
	if (ret <= 0) {
		pr_err("Error getting max level for Device:[%s]. err:%d\n",
			device_name, ret);
		ret = (!ret) ? -EINVAL : ret;
		goto dev_init_exit;
	}
	lmh_device->max_level = ret;
	lmh_device->levels = kzalloc(lmh_device->max_level * sizeof(int),
				GFP_KERNEL);
	if (!lmh_device->levels) {
		pr_err("No memory\n");
		ret = -ENOMEM;
		goto dev_init_exit;
	}
	ret = ops->get_available_levels(ops, lmh_device->levels);
	if (ret) {
		pr_err("Error getting device:[%s] levels. err:%d\n",
			device_name, ret);
		goto dev_init_exit;
	}
	init_rwsem(&lmh_device->lock);
	lmh_device->device_ops = ops;
	strlcpy(lmh_device->device_name, device_name, LMH_NAME_MAX);
	list_add_tail(&lmh_device->list_ptr, &lmh_device_list);
	lmh_create_dev_sysfs(lmh_device);

dev_init_exit:
	if (ret)
		kfree(lmh_device->levels);
	return ret;
}

int lmh_get_all_dev_levels(char *device_name, int *val)
{
	int ret = 0;
	struct lmh_device_data *lmh_device = NULL;

	if (!device_name) {
		pr_err("Invalid input\n");
		return -EINVAL;
	}
	down_read(&lmh_dev_access_lock);
	lmh_device = lmh_match_device_name(device_name);
	if (!lmh_device) {
		pr_err("Invalid device:%s\n", device_name);
		ret = -EINVAL;
		goto get_all_lvl_exit;
	}
	down_read(&lmh_device->lock);
	if (!val) {
		ret = lmh_device->max_level;
		goto get_all_lvl_exit;
	}
	memcpy(val, lmh_device->levels,
		sizeof(int) * lmh_device->max_level);

get_all_lvl_exit:
	if (lmh_device)
		up_read(&lmh_device->lock);
	up_read(&lmh_dev_access_lock);
	return ret;
}

int lmh_set_dev_level(char *device_name, int curr_lvl)
{
	int ret = 0;
	struct lmh_device_data *lmh_device = NULL;

	if (!device_name) {
		pr_err("Invalid input\n");
		return -EINVAL;
	}
	down_read(&lmh_dev_access_lock);
	lmh_device = lmh_match_device_name(device_name);
	if (!lmh_device) {
		pr_err("Invalid device:%s\n", device_name);
		ret = -EINVAL;
		goto set_dev_exit;
	}
	down_write(&lmh_device->lock);
	curr_lvl = min(curr_lvl, lmh_device->levels[lmh_device->max_level - 1]);
	curr_lvl = max(curr_lvl, lmh_device->levels[0]);
	if (curr_lvl == lmh_device->curr_level)
		goto set_dev_exit;
	ret = lmh_device->device_ops->set_level(lmh_device->device_ops,
			curr_lvl);
	if (ret) {
		pr_err("Error setting current level%d for device[%s]. err:%d\n",
			curr_lvl, device_name, ret);
		goto set_dev_exit;
	}
	pr_debug("Device:[%s] configured to level %d\n", device_name, curr_lvl);
	lmh_device->curr_level = curr_lvl;

set_dev_exit:
	if (lmh_device)
		up_write(&lmh_device->lock);
	up_read(&lmh_dev_access_lock);
	return ret;
}

int lmh_get_curr_level(char *device_name, int *val)
{
	int ret = 0;
	struct lmh_device_data *lmh_device = NULL;

	if (!device_name || !val) {
		pr_err("Invalid input\n");
		return -EINVAL;
	}
	down_read(&lmh_dev_access_lock);
	lmh_device = lmh_match_device_name(device_name);
	if (!lmh_device) {
		pr_err("Invalid device:%s\n", device_name);
		ret = -EINVAL;
		goto get_curr_level;
	}
	down_read(&lmh_device->lock);
	ret = lmh_device->device_ops->get_curr_level(lmh_device->device_ops,
			&lmh_device->curr_level);
	if (ret) {
		pr_err("Error getting device[%s] current level. err:%d\n",
			device_name, ret);
		goto get_curr_level;
	}
	*val = lmh_device->curr_level;
	pr_debug("Device:%s current level:%d\n", device_name, *val);

get_curr_level:
	if (lmh_device)
		up_read(&lmh_device->lock);
	up_read(&lmh_dev_access_lock);
	return ret;
}

int lmh_device_register(char *device_name, struct lmh_device_ops *ops)
{
	int ret = 0;
	struct lmh_device_data *lmh_device = NULL;

	if (!device_name || !ops) {
		pr_err("Invalid input\n");
		return -EINVAL;
	}

	if (!ops->get_available_levels || !ops->get_curr_level
		|| !ops->set_level) {
		pr_err("Invalid ops input for device:%s\n", device_name);
		return -EINVAL;
	}

	down_write(&lmh_dev_access_lock);
	if (lmh_match_device_name(device_name)
		|| lmh_match_device_ops(ops)) {
		ret = -EEXIST;
		pr_err("Device[%s] allready exists\n", device_name);
		goto register_exit;
	}
	lmh_device = kzalloc(sizeof(struct lmh_device_data), GFP_KERNEL);
	if (!lmh_device) {
		pr_err("kzalloc failed\n");
		ret = -ENOMEM;
		goto register_exit;
	}
	ret = lmh_device_init(lmh_device, device_name, ops);
	if (ret) {
		pr_err("Error registering device:%s. err:%d\n", device_name,
			ret);
		kfree(lmh_device);
		goto register_exit;
	}

	pr_debug("Registered Device:[%s] with %d levels\n", device_name,
			lmh_device->max_level);

register_exit:
	up_write(&lmh_dev_access_lock);
	return ret;
}

static void lmh_device_remove(struct lmh_device_ops *ops)
{
	struct lmh_device_data *lmh_device = NULL;

	lmh_device = lmh_match_device_ops(ops);
	if (!lmh_device) {
		pr_err("No match for the device\n");
		goto deregister_exit;
	}
	down_write(&lmh_device->lock);
	list_del(&lmh_device->list_ptr);
	pr_debug("Deregistered device:[%s]\n", lmh_device->device_name);
	kfree(lmh_device->levels);
	up_write(&lmh_device->lock);
	kfree(lmh_device);

deregister_exit:
	return;
}

void lmh_device_deregister(struct lmh_device_ops *ops)
{
	if (!ops) {
		pr_err("Invalid input\n");
		return;
	}

	down_write(&lmh_dev_access_lock);
	lmh_device_remove(ops);
	up_write(&lmh_dev_access_lock);
	return;
}

static int lmh_parse_and_extract(const char __user *user_buf, size_t count,
	enum lmh_read_type type)
{
	char *local_buf = NULL, *token = NULL, *curr_ptr = NULL, *token1 = NULL;
	char *next_line = NULL;
	int ret = 0, data_ct = 0, i = 0, size = 0;
	uint32_t *config_buf = NULL;

	/* Allocate two extra space to add ';' character and NULL terminate */
	local_buf = kzalloc(count + 2, GFP_KERNEL);
	if (!local_buf) {
		ret = -ENOMEM;
		goto dfs_cfg_write_exit;
	}
	if (copy_from_user(local_buf, user_buf, count)) {
		pr_err("user buf error\n");
		ret = -EFAULT;
		goto dfs_cfg_write_exit;
	}
	size = count + (strnchr(local_buf, count, '\n') ? 1 :  2);
	local_buf[size - 2] = ';';
	local_buf[size - 1] = '\0';
	curr_ptr = next_line = local_buf;
	while ((token1 = strnchr(next_line, local_buf + size - next_line, ';'))
		!= NULL) {
		data_ct = 0;
		*token1 = '\0';
		curr_ptr = next_line;
		next_line = token1 + 1;
		for (token = (char *)curr_ptr; token &&
			((token = strnchr(token, next_line - token, ' '))
			 != NULL); token++)
			data_ct++;
		if (data_ct < 2) {
			pr_err("Invalid format string:[%s]\n", curr_ptr);
			ret = -EINVAL;
			goto dfs_cfg_write_exit;
		}
		config_buf = kzalloc((++data_ct) * sizeof(uint32_t),
				GFP_KERNEL);
		if (!config_buf) {
			ret = -ENOMEM;
			goto dfs_cfg_write_exit;
		}
		pr_debug("Input:%s data_ct:%d\n", curr_ptr, data_ct);
		for (i = 0, token = (char *)curr_ptr; token && (i < data_ct);
			i++) {
			token = strnchr(token, next_line - token, ' ');
			if (token)
				*token = '\0';
			ret = kstrtouint(curr_ptr, 0, &config_buf[i]);
			if (ret < 0) {
				pr_err("Data[%s] scan error. err:%d\n",
					curr_ptr, ret);
				kfree(config_buf);
				goto dfs_cfg_write_exit;
			}
			if (token)
				curr_ptr = ++token;
		}
		switch (type) {
		case LMH_DEBUG_READ_TYPE:
			ret = lmh_mon_data->debug_ops->debug_config_read(
				lmh_mon_data->debug_ops, config_buf, data_ct);
			break;
		case LMH_DEBUG_CONFIG_TYPE:
			ret = lmh_mon_data->debug_ops->debug_config_lmh(
				lmh_mon_data->debug_ops, config_buf, data_ct);
			break;
		default:
			ret = -EINVAL;
			break;
		}
		kfree(config_buf);
		if (ret) {
			pr_err("Config error. type:%d err:%d\n", type, ret);
			goto dfs_cfg_write_exit;
		}
	}

dfs_cfg_write_exit:
	kfree(local_buf);
	return ret;
}

static ssize_t lmh_dbgfs_config_write(struct file *file,
	const char __user *user_buf, size_t count, loff_t *ppos)
{
	lmh_parse_and_extract(user_buf, count, LMH_DEBUG_CONFIG_TYPE);
	return count;
}

static int lmh_dbgfs_data_read(struct seq_file *seq_fp, void *data)
{
	uint32_t *read_buf = NULL;
	int ret = 0, i = 0;

	ret = lmh_mon_data->debug_ops->debug_read(lmh_mon_data->debug_ops,
		&read_buf);
	if (ret <= 0 || !read_buf)
		goto dfs_read_exit;

	do {
		seq_printf(seq_fp, "0x%x ", read_buf[i]);
		i++;
		if ((i % LMH_READ_LINE_LENGTH) == 0)
			seq_puts(seq_fp, "\n");
	} while (i < (ret / sizeof(uint32_t)));

dfs_read_exit:
	return (ret < 0) ? ret : 0;
}

static ssize_t lmh_dbgfs_data_write(struct file *file,
	const char __user *user_buf, size_t count, loff_t *ppos)
{
	lmh_parse_and_extract(user_buf, count, LMH_DEBUG_READ_TYPE);
	return count;
}

static int lmh_dbgfs_data_open(struct inode *inode, struct file *file)
{
	return single_open(file, lmh_dbgfs_data_read, inode->i_private);
}

static int lmh_get_types(struct seq_file *seq_fp, enum lmh_read_type type)
{
	int ret = 0, idx = 0, size = 0;
	uint32_t *type_list = NULL;

	switch (type) {
	case LMH_DEBUG_READ_TYPE:
		ret = lmh_mon_data->debug_ops->debug_get_types(
			lmh_mon_data->debug_ops, true, &type_list);
		break;
	case LMH_DEBUG_CONFIG_TYPE:
		ret = lmh_mon_data->debug_ops->debug_get_types(
			lmh_mon_data->debug_ops, false, &type_list);
		break;
	default:
		return -EINVAL;
	}
	if (ret <= 0 || !type_list) {
		pr_err("No device information. err:%d\n", ret);
		return -ENODEV;
	}
	size = ret;
	for (idx = 0; idx < size; idx++)
		seq_printf(seq_fp, "0x%x ", type_list[idx]);
	seq_puts(seq_fp, "\n");

	return 0;
}

static int lmh_dbgfs_read_type(struct seq_file *seq_fp, void *data)
{
	return lmh_get_types(seq_fp, LMH_DEBUG_READ_TYPE);
}

static int lmh_dbgfs_read_type_open(struct inode *inode, struct file *file)
{
	return single_open(file, lmh_dbgfs_read_type, inode->i_private);
}

static int lmh_dbgfs_config_type(struct seq_file *seq_fp, void *data)
{
	return lmh_get_types(seq_fp, LMH_DEBUG_CONFIG_TYPE);
}

static int lmh_dbgfs_config_type_open(struct inode *inode, struct file *file)
{
	return single_open(file, lmh_dbgfs_config_type, inode->i_private);
}

static const struct file_operations lmh_dbgfs_config_fops = {
	.write		= lmh_dbgfs_config_write,
};
static const struct file_operations lmh_dbgfs_read_fops = {
	.open		= lmh_dbgfs_data_open,
	.read		= seq_read,
	.write		= lmh_dbgfs_data_write,
	.llseek		= seq_lseek,
	.release	= single_release,
};
static const struct file_operations lmh_dbgfs_read_type_fops = {
	.open		= lmh_dbgfs_read_type_open,
	.read		= seq_read,
	.llseek		= seq_lseek,
	.release	= single_release,
};
static const struct file_operations lmh_dbgfs_config_type_fops = {
	.open		= lmh_dbgfs_config_type_open,
	.read		= seq_read,
	.llseek		= seq_lseek,
	.release	= single_release,
};

int lmh_debug_register(struct lmh_debug_ops *ops)
{
	int ret = 0;

	if (!ops || !ops->debug_read || !ops->debug_config_read
		       || !ops->debug_get_types) {
		pr_err("Invalid input");
		ret = -EINVAL;
		goto dbg_reg_exit;
	}

	lmh_mon_data->debug_ops = ops;
	LMH_CREATE_DEBUGFS_DIR(lmh_mon_data->debug_dir, LMH_DBGFS_DIR,
			lmh_mon_data->debugfs_parent, ret);
	if (ret)
		goto dbg_reg_exit;

	LMH_CREATE_DEBUGFS_FILE(lmh_mon_data->debug_read, LMH_DBGFS_READ, 0600,
		lmh_mon_data->debug_dir, NULL, &lmh_dbgfs_read_fops, ret);
	if (!lmh_mon_data->debug_read) {
		pr_err("Error creating" LMH_DBGFS_READ "entry.\n");
		ret = -ENODEV;
		goto dbg_reg_exit;
	}
	LMH_CREATE_DEBUGFS_FILE(lmh_mon_data->debug_config,
		LMH_DBGFS_CONFIG_READ, 0200, lmh_mon_data->debug_dir, NULL,
		&lmh_dbgfs_config_fops, ret);
	if (!lmh_mon_data->debug_config) {
		pr_err("Error creating" LMH_DBGFS_CONFIG_READ "entry\n");
		ret = -ENODEV;
		goto dbg_reg_exit;
	}
	LMH_CREATE_DEBUGFS_FILE(lmh_mon_data->debug_read_type,
		LMH_DBGFS_READ_TYPES, 0400, lmh_mon_data->debug_dir, NULL,
		&lmh_dbgfs_read_type_fops, ret);
	if (!lmh_mon_data->debug_read_type) {
		pr_err("Error creating" LMH_DBGFS_READ_TYPES "entry\n");
		ret = -ENODEV;
		goto dbg_reg_exit;
	}
	LMH_CREATE_DEBUGFS_FILE(lmh_mon_data->debug_config_type,
		LMH_DBGFS_CONFIG_TYPES, 0400, lmh_mon_data->debug_dir, NULL,
		&lmh_dbgfs_config_type_fops, ret);
	if (!lmh_mon_data->debug_config_type) {
		pr_err("Error creating" LMH_DBGFS_CONFIG_TYPES "entry\n");
		ret = -ENODEV;
		goto dbg_reg_exit;
	}

dbg_reg_exit:
	if (ret) {
		/*Clean up all the dbg nodes*/
		debugfs_remove_recursive(lmh_mon_data->debug_dir);
		lmh_mon_data->debug_ops = NULL;
	}

	return ret;
}

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

	lmh_mon_data = kzalloc(sizeof(struct lmh_mon_driver_data),
				GFP_KERNEL);
	if (!lmh_mon_data) {
		pr_err("No memory\n");
		return -ENOMEM;
	}

	LMH_CREATE_DEBUGFS_DIR(lmh_mon_data->debugfs_parent, LMH_MON_NAME,
				NULL, ret);
	if (ret)
		goto init_exit;
	lmh_mon_data->poll_fs = debugfs_create_u32(LMH_ISR_POLL_DELAY, 0600,
			lmh_mon_data->debugfs_parent, &lmh_poll_interval);
	if (IS_ERR(lmh_mon_data->poll_fs))
		pr_err("Error creating debugfs:[%s]. err:%ld\n",
			LMH_ISR_POLL_DELAY, PTR_ERR(lmh_mon_data->poll_fs));

init_exit:
	if (ret == -ENODEV)
		ret = 0;
	return ret;
}

static int __init lmh_mon_init_call(void)
{
	int ret = 0;

	ret = lmh_mon_init_driver();
	if (ret) {
		pr_err("Error initializing the debugfs. err:%d\n", ret);
		goto lmh_init_exit;
	}
	ret = class_register(&lmh_class_info);
	if (ret)
		goto lmh_init_exit;

lmh_init_exit:
	if (ret)
		class_unregister(&lmh_class_info);
	return ret;
}

static void lmh_mon_cleanup(void)
{
	down_write(&lmh_mon_access_lock);
	while (!list_empty(&lmh_sensor_list)) {
		lmh_sensor_remove(list_first_entry(&lmh_sensor_list,
			struct lmh_mon_sensor_data, list_ptr)->sensor_ops);
	}
	up_write(&lmh_mon_access_lock);
	debugfs_remove_recursive(lmh_mon_data->debugfs_parent);
	kfree(lmh_mon_data);
}

static void lmh_device_cleanup(void)
{
	down_write(&lmh_dev_access_lock);
	while (!list_empty(&lmh_device_list)) {
		lmh_device_remove(list_first_entry(&lmh_device_list,
			struct lmh_device_data, list_ptr)->device_ops);
	}
	up_write(&lmh_dev_access_lock);
}

static void lmh_debug_cleanup(void)
{
	if (lmh_mon_data->debug_ops) {
		debugfs_remove_recursive(lmh_mon_data->debug_dir);
		lmh_mon_data->debug_ops = NULL;
	}
}

static void __exit lmh_mon_exit(void)
{
	lmh_mon_cleanup();
	lmh_device_cleanup();
	lmh_debug_cleanup();
	class_unregister(&lmh_class_info);
}

module_init(lmh_mon_init_call);
module_exit(lmh_mon_exit);

MODULE_LICENSE("GPL v2");
MODULE_DESCRIPTION("LMH monitor driver");
MODULE_ALIAS("platform:" LMH_MON_NAME);