pse/pse-class.c

/*
 * ./kernel_modules/pse/pse-class.c
 *
 * This file defines a device class for 802.3af/at/at+ PSE devices.
 *
 * Author: Cradlepoint Technology, Inc.  <source@cradlepoint.com>
 *		Kyle Swenson <kswenson@cradlepoint.com>
 *
 * Copyright 2017 Cradlepoint Technology, Inc.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License 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.
 *
 */
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/device.h>
#include <linux/err.h>
#include <linux/of.h>
#include <linux/slab.h>
#include <linux/sysfs.h>
#include <linux/idr.h>
#include <linux/spinlock.h>
#include <linux/workqueue.h>
#include <linux/list_sort.h>
#include <linux/sort.h>
#include "pse-port.h"
#include "pse-sysfs.h"
#include "pse-class.h"

#define DEFSTRLEN	32

static int determine_power_used(struct class *class, struct device *pdev);
static int power_port(struct device *dev);
static int request_power(struct device *dev);
static void pse_port_power_request_handler(struct work_struct *work);
static void pse_port_power_disconnect_handler(struct work_struct *work);



static struct class *g_pse_class;
static DEFINE_IDA(pse_port_ida);


/* match_port_priority: Return the first pse_port_device that has a priority
 * equal the priority passed in via data
 */
int match_port_priority(struct device *dev, const void *data)
{
	uint8_t priority;

	if (data) {
		priority = *((uint8_t *)data);
		if (to_pse_port_device(dev)->priority == priority)
			return 1;/*Match found */
	}
	return 0; /* no match */
}


int match_port_id(struct device *dev, const void *data)
{
	uint8_t port_id;

	if (data) {
		port_id = *((uint8_t *)data);
		if (to_ppd(dev)->id == port_id)
			return 1;/*Match found */
	}
	return 0; /* no match */
}


/* compare_port_priority: if p1 is a lower priority (which means a higher value
 * in port->priority) port than p2, return -1 If the ports have the same
 * priority, then look at port->id, lower id, higher priority
 *
 * P1 : priority 1
 * P2 : priority 3
 * P1 has a higher priority than P2
 * compare_port_priority(P1, P2) -> -1
 * compare_port_priority(P1, P1) -> 0
 * compare_port_priority(P2, P1) -> 1
 *
 *
 */

int compare_port_priority(struct pse_port_device *p1, struct pse_port_device *p2)
{
	if (p1->priority > p2->priority)
		return -1;
	if (p1->priority < p2->priority)
		return 1;

	/* (p1->priority == p2->priority) */
	if (p1->id > p2->id)
		return -1;
	if (p1->id < p2->id)
		return 1;

	/* (p1->priority == p2->priority and p1->id == p2->id) */
	return 0;
}

#define LT(p1, p2) (compare_port_priority((p1), (p2)) < 0)
#define GT(p1, p2) (compare_port_priority((p1), (p2)) > 0)
#define EQ(p1, p2) (compare_port_priority((p1), (p2)) == 0)
#define LEQ(p1, p2) (!GT((p1), (p2)))
#define GEQ(p1, p2) (!LT((p1), (p2)))

struct device *get_highest_priority_device(struct class *class)
{
	/* Caller is responsible for calling put_device   */
	struct class_dev_iter iter;
	struct device *dev;
	struct device *hpdev;

	class_dev_iter_init(&iter, class, NULL, NULL);
	dev = class_dev_iter_next(&iter);
	hpdev = get_device(dev);
	while (dev) {
		if (GT(to_ppd(dev), to_ppd(hpdev))) {
			put_device(hpdev);
			hpdev = get_device(dev);
		}
		dev = class_dev_iter_next(&iter);
	}
	class_dev_iter_exit(&iter);
	return hpdev;
}
EXPORT_SYMBOL(get_highest_priority_device);

struct device *get_lowest_priority_device(struct class *class)
{
	struct class_dev_iter iter;
	struct device *dev;
	struct device *lpdev;

	class_dev_iter_init(&iter, class, NULL, NULL);
	dev = class_dev_iter_next(&iter);
	lpdev = get_device(dev);
	while (dev) {
		if (LT(to_ppd(dev), to_ppd(lpdev))) {
			put_device(lpdev);
			lpdev = get_device(dev);
		}
		dev = class_dev_iter_next(&iter);
	}
	class_dev_iter_exit(&iter);
	return lpdev;
}
struct device *get_next_lowest_priority_device(struct class *class,
									struct device *compare_dev)
{
	struct class_dev_iter iter;
	struct device *current_dev;
	struct device *next_lowest_dev;

	dev_dbg(compare_dev, "Next_lowest_priority_device, finding next dev after %d %d",
		to_ppd(compare_dev)->id, to_ppd(compare_dev)->priority);

	class_dev_iter_init(&iter, class, NULL, NULL);
	current_dev = class_dev_iter_next(&iter);
	next_lowest_dev = NULL;
	while (current_dev) {
		if (LT(to_ppd(current_dev), to_ppd(compare_dev))) {
			if (!next_lowest_dev)
				next_lowest_dev = current_dev;

			if (GEQ(to_ppd(current_dev), to_ppd(next_lowest_dev))) {
				next_lowest_dev = get_device(current_dev);
				dev_dbg(next_lowest_dev, "is the next lowest priority device");
			}
		}
		current_dev = class_dev_iter_next(&iter);
	}
	class_dev_iter_exit(&iter);
	return next_lowest_dev;
}
EXPORT_SYMBOL(get_next_lowest_priority_device);


struct device *relink_priority_list(struct class *class)
{
	struct device *headdev;
	struct device *dev;
	struct device *ndev;

	headdev = get_highest_priority_device(class);
	if (!headdev)
		return NULL;

	dev = headdev;
	to_ppd(dev)->next_hp = NULL;
	ndev = get_next_lowest_priority_device(class, dev);
	while (ndev) {
		to_ppd(dev)->next_lp = ndev;
		to_ppd(ndev)->next_hp = dev;
		dev = ndev;
		ndev = get_next_lowest_priority_device(class, dev);
	}
	to_ppd(dev)->next_lp = NULL;
	return headdev;
}


/* Sums the power used by pdev and all lower-priority devices. To get the total power used, set pdev to NULL */
static int determine_power_used(struct class *class, struct device *pdev)
{
	int power_used = 0;
	struct pse_port_device *port;

	if (pdev == NULL)
		pdev = get_highest_priority_device(class);

	while (pdev) {
		port = to_ppd(pdev);
		power_used += port->power_allocation;
		pdev = get_next_lowest_priority_device(class, &port->dev);
	}
	return power_used;
}

static int power_port(struct device *dev)
{
	struct pse_data *pse_data = get_pse_data(dev->class);
	struct pse_port_device *port = to_pse_port_device(dev);
	uint16_t port_class = port->last_class_result;

	if (port_class >= CLASS_INVALID) {
		dev_warn(dev, "Attempt to power a port with an invalid class\n");
		return -EINVAL;
	}

	if (port->power_allocation == 0)
		port->ops->apply_power(&port->dev, 1);

	/* New power used is the difference between what we were allocated, and what we're requesting */
	pse_data->power_used += port->power_request - port->power_allocation;

	port->power_allocation = port->power_request;
	port->ops->set_power_limit(&port->dev, port->power_allocation);
	return 0;
}

void disconnect_port(struct device *dev)
{
	struct pse_port_device *port = to_pse_port_device(dev);
	struct pse_data *pse_data = get_pse_data(dev->class);

	if (port->power_allocation > 0)
		pse_data->power_used -= port->power_allocation;
	port->power_allocation = 0;
	port->power_request = 0;
	port->ops->reset(&port->dev);
}

/* request_power - potentially power a port.  This function really needs to be
 * called in port-priority order.  If this is not called in priority order then
 * when the get glitching, where a lower-priority port will be powered briefly
 * until the higher-priority port port requesting the power has a higher
 * priority than ports that are currently powered, those will likely be
 * unpowered.  NOTE: power_request should be a positive number indicating total
 * power wanted.
 */
static int request_power(struct device *dev)
{
	int power_remaining;
	int lpu = 0;
	int32_t power_request;
	struct device *lpdev;
	struct pse_data *pse_data = get_pse_data(dev->class);
	struct pse_port_device *port = to_pse_port_device(dev);

	dev_dbg(&port->dev, "Request_power begin:");
	if (port->last_class_result >= CLASS_INVALID) {
		dev_warn(&port->dev, "request_power called with invalid port classification result");
		port->ops->reset(&port->dev);
		return -EINVAL;
	}


	power_request = port->power_request - port->power_allocation;
	power_remaining = ((int32_t) pse_data->power_budget) - ((int32_t) pse_data->power_used);
	dev_dbg(&port->dev, "Is requesting %i mW", power_request);
	dev_dbg(&port->dev, "We have %i mW remaining in our budget", power_remaining);

	if (power_remaining >= power_request) {
		dev_info(&port->dev, "Powering port%d because: power_budget (%i) - power_used (%i) - power_requested (%i) >= 0", port->id, pse_data->power_budget, pse_data->power_used, power_request);
		return power_port(&port->dev);
	}

	/* If the total current potentially gained by unpowering all the ports _in
	 * addition to the power currently remaining_ will not give enough current
	 * to power this device, there's no point to unpowering all the other ports
	 */

	lpu = determine_power_used(&pse_data->cls, &port->dev);
	if ((power_remaining + lpu) < power_request) {
		dev_dbg(&port->dev, " No budget left, power used by lower-priority devices is %i, power requested is %i", lpu, power_request);
		return -EBUSY;
	}

	dev_info(&port->dev, "power_remaining (%i) + power used by lower priority devices (%i) will net us %i enough to power this request of %i", power_remaining, lpu, power_remaining+lpu, power_request);

	lpdev = get_lowest_priority_device(&pse_data->cls);
	while (lpdev && (power_remaining < power_request)) {
		if (to_ppd(lpdev)->power_allocation > 0) {
			dev_info(&port->dev, "port%d (priority %d) is being unpowered to power port%d (priority %d)", to_ppd(lpdev)->id, to_ppd(lpdev)->priority,  port->id, port->priority);
			disconnect_port(lpdev);
			power_remaining = pse_data->power_budget - pse_data->power_used;
		}
		lpdev = to_ppd(lpdev)->next_hp;
	}

	if (power_remaining >= power_request) {
		dev_info(&port->dev, "Powering port%d, after powering off lower priority ports\n", port->id);
		return power_port(&port->dev);
	}

	/* If we get here, then our algorithm is wrong or state is corrupt */
	dev_info(&port->dev, "Warning, power_remaining (%i) < power_request (%i), even after unpowering devices", power_remaining, power_request);
	return -EAGAIN;
}

/* This is called in response to userspace writing to pse/port_priority, or
 * pse/power_budget
 */
void redistribute_power(struct class *class)
{
	struct device *h;
	struct pse_port_device *p;

	h = get_highest_priority_device(class);
	while (h) {
		p = to_ppd(h);
		mutex_lock(&p->lock);
		queue_work(p->wq, &p->disconnect);
		mutex_unlock(&p->lock);
		h = get_next_lowest_priority_device(class, h);
	}
}
static void pse_port_power_request_handler(struct work_struct *work)
{
	struct pse_port_device *port = container_of(work, struct pse_port_device, request_power);
	struct pse_data *pse_data = get_pse_data(port->dev.class);

	mutex_lock(&pse_data->pse_lock);
	dev_dbg(&port->dev, "in %s\n", __func__);
	if (!pse_data->enabled) {
		/* No powering devices if we're disabled */
		goto unlock_class_exit;
	}

	mutex_lock(&port->lock);
	if (!port->enabled) {
		/* No powering devices if the port is disabled */
		goto unlock_port_exit;
	}

	if (port->power_request == port->power_allocation) {
		/* If the port is already powered, don't try to power it again*/
		goto unlock_port_exit;
	}

	if (port->last_class_result != CLASS_INVALID && port->last_detect_result == DETECT_POWERED_DEVICE) {
		if (request_power(&port->dev) == 0)
			dev_info(&port->dev, "Has been allocated %i mW, and will be powered", port->power_allocation);
	}

unlock_port_exit:
	mutex_unlock(&port->lock);
unlock_class_exit:
	mutex_unlock(&pse_data->pse_lock);

}

static void pse_port_power_disconnect_handler(struct work_struct *work)
{
	struct pse_port_device *p = container_of(work, struct pse_port_device, disconnect);
	struct pse_data *pse_data = get_pse_data(p->dev.class);

	mutex_lock(&pse_data->pse_lock);
	disconnect_port(&p->dev);
	mutex_unlock(&pse_data->pse_lock);
}


static void pse_port_device_release(struct device *dev)
{
	struct pse_port_device *port = to_ppd(dev);
	struct pse_data *pse_data = get_pse_data(dev->class);

	mutex_lock(&pse_data->pse_lock);
	dev_dbg(dev, "%s:%d\n", __func__, __LINE__);
	ida_simple_remove(&pse_port_ida, port->id);
	kfree(port);
	mutex_unlock(&pse_data->pse_lock);

}


struct pse_port_device *pse_port_device_create(struct device *dev, const char *name, const struct pse_port_ops *ops, struct module *owner)
{

	struct pse_port_device *pse_port;

	pse_port = devm_kzalloc(dev, sizeof(struct pse_port_device), GFP_KERNEL);
	if (pse_port == NULL)
		return NULL;

	pse_port->ops = ops;
	pse_port->owner = owner;
	pse_port->dev.parent = dev;
	strlcpy(pse_port->name, name, PSE_PORT_NAME_SIZE);
	return pse_port_device_register(pse_port);


}
EXPORT_SYMBOL(pse_port_device_create);

struct pse_port_device *pse_port_device_register(struct pse_port_device *pse_port)
{
	int err;
	int id = -1;
	struct pse_data *pse_data = get_pse_data(g_pse_class);

	if (id < 0) {
		id = ida_simple_get(&pse_port_ida, 0, 0, GFP_KERNEL);
		if (id < 0) {
			err = id;
			goto exit;
		}
	}

	if (id >= PSE_MAX_PORTS) {
		dev_warn(&pse_port->dev, "Max number of PSE ports (%d) have been registered", PSE_MAX_PORTS);
		err = -ENOMEM;
		goto exit_ida;
	}

	pse_port->id = id;
	pse_port->enabled = 0;
	pse_port->power_allocation = 0;
	pse_port->priority = pse_data->port_priorities[id];
	pse_port->last_class_result = CLASS_INVALID;
	pse_port->last_detect_result = DETECT_NO_DEVICE;
	pse_port->dev.class = g_pse_class;
	pse_port->dev.release = pse_port_device_release;
	dev_set_name(&pse_port->dev, "port%d", id);
	pse_port->wq = alloc_ordered_workqueue("pse-port%d-wq", 0, pse_port->id);
	mutex_init(&pse_port->lock);

	INIT_WORK(&pse_port->request_power, pse_port_power_request_handler);
	INIT_WORK(&pse_port->disconnect, pse_port_power_disconnect_handler);

	err = device_register(&pse_port->dev);
	if (err) {
		put_device(&pse_port->dev);
		goto unlock_exit_ida;
	}

	mutex_lock(&pse_data->pse_lock);
	relink_priority_list(&pse_data->cls);
	mutex_unlock(&pse_data->pse_lock);

	redistribute_power(&pse_data->cls);

	dev_info(&pse_port->dev, "pse core: registered port %d as %s with priority %d\n", pse_port->id, dev_name(&pse_port->dev), pse_port->priority);
	return pse_port;


unlock_exit_ida:
	mutex_unlock(&pse_data->pse_lock);
exit_ida:
	ida_simple_remove(&pse_port_ida, id);
exit:
	dev_err(&pse_port->dev, "pse core: unable to register port %d, err = %d\n", id, err);
	return ERR_PTR(err);
}
EXPORT_SYMBOL(pse_port_device_register);

/* pse_port_device_unregister - unregister a pse_port_device
 *
 * @port: the pse port class device to destroy
 */
void pse_port_device_unregister(struct pse_port_device *port)
{
	int id;

	if (get_device(&port->dev) != NULL) {
		id = port->id;
		dev_info(&port->dev, "Unregistering port%d\n", id);
		device_unregister(&port->dev);
		ida_simple_remove(&pse_port_ida, id);
		put_device(&port->dev);
	}
}
EXPORT_SYMBOL(pse_port_device_unregister);

static int __init pse_init(void)
{
	int i = 0;
	int err = 0;

	struct pse_data *pse_data = kzalloc(sizeof(struct pse_data), GFP_KERNEL);

	if (!pse_data)
		return -ENOMEM;

	 /* Since kzalloc will zero out the member before returning
	  * the pointer, we need to set every potential port's priority
	  * to the lowest possible (highest numerical value)
	  */

	for (i = 0; i < PSE_MAX_PORTS; i++)
		pse_data->port_priorities[i] = 0xFF;

	mutex_init(&pse_data->pse_lock);

	pse_data->enabled = 0;
	pse_data->power_used = 0;
	pse_data->power_budget = 0;

	/* Now we should set the various class things and such */
	pse_data->cls.name =  PSE_CLASS_NAME_STR;
	pse_data->cls.owner = THIS_MODULE;
	pse_data->cls.dev_groups = pse_port_dev_attr_groups;

	/* With necessary fields filled out, lets register the pse class */
	err = class_register(&pse_data->cls);
	if (err) {
		pr_info("Failed to register the PSE class");
		kfree(pse_data);
		return err;
	}

	/* We can now save the pse_data pointer */
	g_pse_class = &pse_data->cls;

	/* Now we should create the sysfs files for enable, power_budget,ect */
	pse_class_sysfs_init(&pse_data->cls);


	return err;
}

static void __exit pse_exit(void)
{
	struct pse_data *pse_data = get_pse_data(g_pse_class);

	pse_class_sysfs_close(&pse_data->cls);
	mutex_destroy(&pse_data->pse_lock);
	class_destroy(&pse_data->cls);
	ida_destroy(&pse_port_ida);
	kfree(pse_data);
}

subsys_initcall(pse_init);
module_exit(pse_exit);

MODULE_AUTHOR("Kyle Swenson <kswenson@cradlepoint.com>");
MODULE_DESCRIPTION("PoE (PSE) sysfs class");
MODULE_LICENSE("GPL");