@@ -17,19 +17,17 @@ MODULE_LICENSE("GPL");
MODULE_DESCRIPTION("FWNODE MDIO bus (Ethernet PHY) accessors");
static struct pse_control *
-fwnode_find_pse_control(struct fwnode_handle *fwnode)
+dev_find_pse_control(struct device *dev)
{
struct pse_control *psec;
- struct device_node *np;
if (!IS_ENABLED(CONFIG_PSE_CONTROLLER))
return NULL;
- np = to_of_node(fwnode);
- if (!np)
+ if (!dev->of_node)
return NULL;
- psec = of_pse_control_get(np);
+ psec = of_pse_control_get(dev, dev->of_node);
if (PTR_ERR(psec) == -ENOENT)
return NULL;
@@ -120,15 +118,9 @@ int fwnode_mdiobus_register_phy(struct mii_bus *bus,
u32 phy_id;
int rc;
- psec = fwnode_find_pse_control(child);
- if (IS_ERR(psec))
- return PTR_ERR(psec);
-
mii_ts = fwnode_find_mii_timestamper(child);
- if (IS_ERR(mii_ts)) {
- rc = PTR_ERR(mii_ts);
- goto clean_pse;
- }
+ if (IS_ERR(mii_ts))
+ return PTR_ERR(mii_ts);
is_c45 = fwnode_device_is_compatible(child, "ethernet-phy-ieee802.3-c45");
if (is_c45 || fwnode_get_phy_id(child, &phy_id))
@@ -161,6 +153,12 @@ int fwnode_mdiobus_register_phy(struct mii_bus *bus,
goto clean_phy;
}
+ psec = dev_find_pse_control(&phy->mdio.dev);
+ if (IS_ERR(psec)) {
+ rc = PTR_ERR(psec);
+ goto unregister_phy;
+ }
+
phy->psec = psec;
/* phy->mii_ts may already be defined by the PHY driver. A
@@ -172,12 +170,13 @@ int fwnode_mdiobus_register_phy(struct mii_bus *bus,
return 0;
+unregister_phy:
+ if (is_acpi_node(child) || is_of_node(child))
+ phy_device_remove(phy);
clean_phy:
phy_device_free(phy);
clean_mii_ts:
unregister_mii_timestamper(mii_ts);
-clean_pse:
- pse_control_put(psec);
return rc;
}
@@ -8,25 +8,93 @@
#include <linux/device.h>
#include <linux/of.h>
#include <linux/pse-pd/pse.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
static DEFINE_MUTEX(pse_list_mutex);
+static DEFINE_MUTEX(pse_nesting_mutex);
static LIST_HEAD(pse_controller_list);
/**
* struct pse_control - a PSE control
* @pcdev: a pointer to the PSE controller device
* this PSE control belongs to
+ * @ps: PSE PI supply of the PSE control
* @list: list entry for the pcdev's PSE controller list
* @id: ID of the PSE line in the PSE controller device
* @refcnt: Number of gets of this pse_control
*/
struct pse_control {
struct pse_controller_dev *pcdev;
+ struct regulator *ps;
struct list_head list;
unsigned int id;
struct kref refcnt;
};
+/**
+ * pse_lock - lock a single PSE
+ * @pcdev: PSE source
+ *
+ * This function can be called many times by one task on
+ * a single pse and its mutex will be locked only once.
+ * If a task, which is calling this function is other
+ * than the one, which initially locked the mutex, it will
+ * wait on mutex.
+ */
+static int pse_lock(struct pse_controller_dev *pcdev)
+{
+ bool lock = false;
+ int ret = 0;
+
+ mutex_lock(&pse_nesting_mutex);
+
+ if (!mutex_trylock(&pcdev->lock)) {
+ if (pcdev->lock_owner == current)
+ pcdev->ref_cnt++;
+ else
+ lock = true;
+
+ if (lock) {
+ mutex_unlock(&pse_nesting_mutex);
+ mutex_lock(&pcdev->lock);
+ mutex_lock(&pse_nesting_mutex);
+ }
+ } else {
+ lock = true;
+ }
+
+ if (lock) {
+ pcdev->ref_cnt++;
+ pcdev->lock_owner = current;
+ }
+
+ mutex_unlock(&pse_nesting_mutex);
+
+ return ret;
+}
+
+/**
+ * pse_unlock - unlock a single PSE
+ * @pcdev: PSE source
+ *
+ * This function unlocks the mutex when the
+ * reference counter reaches 0.
+ */
+static void pse_unlock(struct pse_controller_dev *pcdev)
+{
+ mutex_lock(&pse_nesting_mutex);
+
+ if (--pcdev->ref_cnt == 0) {
+ pcdev->lock_owner = NULL;
+ mutex_unlock(&pcdev->lock);
+ }
+
+ WARN_ON_ONCE(pcdev->ref_cnt < 0);
+
+ mutex_unlock(&pse_nesting_mutex);
+}
+
static int of_load_pse_pi_pairsets(struct device_node *node,
struct pse_pi *pi,
int npairsets)
@@ -160,13 +228,123 @@ static int of_load_pse_pis(struct pse_controller_dev *pcdev)
return ret;
}
+static int pse_pi_is_enabled(struct regulator_dev *rdev)
+{
+ struct pse_controller_dev *pcdev = rdev_get_drvdata(rdev);
+ const struct pse_controller_ops *ops;
+ int id, ret;
+
+ ops = pcdev->ops;
+ if (!ops->pi_is_enabled)
+ return -EOPNOTSUPP;
+
+ id = rdev_get_id(rdev);
+ pse_lock(pcdev);
+ ret = ops->pi_is_enabled(pcdev, id);
+ if (ret >= 0)
+ pcdev->pi[id].enabled = ret;
+ pse_unlock(pcdev);
+
+ return ret;
+}
+
+static int pse_pi_enable(struct regulator_dev *rdev)
+{
+ struct pse_controller_dev *pcdev = rdev_get_drvdata(rdev);
+ const struct pse_controller_ops *ops;
+ int id, ret;
+
+ ops = pcdev->ops;
+ if (!ops->pi_enable)
+ return -EOPNOTSUPP;
+
+ id = rdev_get_id(rdev);
+ pse_lock(pcdev);
+ ret = ops->pi_enable(pcdev, id);
+ if (!ret)
+ pcdev->pi[id].enabled = 1;
+ pse_unlock(pcdev);
+
+ return ret;
+}
+
+static int pse_pi_disable(struct regulator_dev *rdev)
+{
+ struct pse_controller_dev *pcdev = rdev_get_drvdata(rdev);
+ const struct pse_controller_ops *ops;
+ int id, ret;
+
+ ops = pcdev->ops;
+ if (!ops->pi_disable)
+ return -EOPNOTSUPP;
+
+ id = rdev_get_id(rdev);
+ pse_lock(pcdev);
+ ret = ops->pi_disable(pcdev, id);
+ if (!ret)
+ pcdev->pi[id].enabled = 0;
+ pse_unlock(pcdev);
+
+ return ret;
+}
+
+static const struct regulator_ops pse_pi_ops = {
+ .is_enabled = pse_pi_is_enabled,
+ .enable = pse_pi_enable,
+ .disable = pse_pi_disable,
+};
+
+static const struct regulator_init_data pse_pi_initdata = {
+ .constraints = {
+ .valid_ops_mask = REGULATOR_CHANGE_STATUS,
+ },
+};
+
+static int
+devm_pse_pi_regulator_register(struct pse_controller_dev *pcdev,
+ char *name, int id)
+{
+ struct regulator_config rconfig = {0};
+ struct regulator_desc *rdesc;
+ struct regulator_dev *rdev;
+
+ rdesc = devm_kzalloc(pcdev->dev, sizeof(*rdesc), GFP_KERNEL);
+ if (!rdesc)
+ return -ENOMEM;
+
+ /* Regulator descriptor id have to be the same as its associated
+ * PSE PI id for the well functioning of the PSE controls.
+ */
+ rdesc->id = id;
+ rdesc->name = name;
+ rdesc->type = REGULATOR_VOLTAGE;
+ rdesc->ops = &pse_pi_ops;
+ rdesc->owner = pcdev->owner;
+
+ rconfig.dev = pcdev->dev;
+ rconfig.driver_data = pcdev;
+ rconfig.init_data = &pse_pi_initdata;
+
+ rdev = devm_regulator_register(pcdev->dev, rdesc, &rconfig);
+ if (IS_ERR(rdev)) {
+ dev_err_probe(pcdev->dev, PTR_ERR(rdev),
+ "Failed to register regulator\n");
+ return PTR_ERR(rdev);
+ }
+
+ pcdev->pi[id].rdev = rdev;
+
+ return 0;
+}
+
/**
* pse_controller_register - register a PSE controller device
* @pcdev: a pointer to the initialized PSE controller device
*/
int pse_controller_register(struct pse_controller_dev *pcdev)
{
- int ret;
+ size_t reg_name_len;
+ int ret, i;
mutex_init(&pcdev->lock);
INIT_LIST_HEAD(&pcdev->pse_control_head);
@@ -184,6 +362,31 @@ int pse_controller_register(struct pse_controller_dev *pcdev)
return ret;
}
+ /* Each regulator name len is pcdev dev name + 7 char +
+ * int max digit number (10) + 1
+ */
+ reg_name_len = strlen(dev_name(pcdev->dev)) + 18;
+
+ /* Register PI regulators */
+ for (i = 0; i < pcdev->nr_lines; i++) {
+ char *reg_name;
+
+ /* Do not register regulator for PIs not described */
+ if (!pcdev->of_legacy && !pcdev->pi[i].np)
+ continue;
+
+ reg_name = devm_kzalloc(pcdev->dev, reg_name_len, GFP_KERNEL);
+ if (!reg_name)
+ return -ENOMEM;
+
+ snprintf(reg_name, reg_name_len, "pse-%s_pi%d",
+ dev_name(pcdev->dev), i);
+
+ ret = devm_pse_pi_regulator_register(pcdev, reg_name, i);
+ if (ret)
+ return ret;
+ }
+
mutex_lock(&pse_list_mutex);
list_add(&pcdev->list, &pse_controller_list);
mutex_unlock(&pse_list_mutex);
@@ -281,6 +484,9 @@ void pse_control_put(struct pse_control *psec)
if (IS_ERR_OR_NULL(psec))
return;
+ if (psec->pcdev->pi[psec->id].enabled)
+ regulator_disable(psec->ps);
+
mutex_lock(&pse_list_mutex);
__pse_control_put_internal(psec);
mutex_unlock(&pse_list_mutex);
@@ -288,9 +494,11 @@ void pse_control_put(struct pse_control *psec)
EXPORT_SYMBOL_GPL(pse_control_put);
static struct pse_control *
-pse_control_get_internal(struct pse_controller_dev *pcdev, unsigned int index)
+pse_control_get_internal(struct device *dev, struct pse_controller_dev *pcdev,
+ unsigned int index)
{
struct pse_control *psec;
+ int ret;
lockdep_assert_held(&pse_list_mutex);
@@ -310,6 +518,20 @@ pse_control_get_internal(struct pse_controller_dev *pcdev, unsigned int index)
return ERR_PTR(-ENODEV);
}
+ psec->ps = devm_regulator_get_exclusive(dev,
+ rdev_get_name(pcdev->pi[index].rdev));
+ if (IS_ERR(psec->ps)) {
+ kfree(psec);
+ return ERR_CAST(psec->ps);
+ }
+
+ ret = regulator_is_enabled(psec->ps);
+ if (ret < 0) {
+ kfree(psec);
+ return ERR_PTR(ret);
+ }
+ pcdev->pi[index].enabled = ret;
+
psec->pcdev = pcdev;
list_add(&psec->list, &pcdev->pse_control_head);
psec->id = index;
@@ -344,7 +566,8 @@ static int psec_id_legacy_xlate(struct pse_controller_dev *pcdev,
return pse_spec->args[0];
}
-struct pse_control *of_pse_control_get(struct device_node *node)
+struct pse_control *of_pse_control_get(struct device *dev,
+ struct device_node *node)
{
struct pse_controller_dev *r, *pcdev;
struct of_phandle_args args;
@@ -394,7 +617,7 @@ struct pse_control *of_pse_control_get(struct device_node *node)
}
/* pse_list_mutex also protects the pcdev's pse_control list */
- psec = pse_control_get_internal(pcdev, psec_id);
+ psec = pse_control_get_internal(dev, pcdev, psec_id);
out:
mutex_unlock(&pse_list_mutex);
@@ -425,9 +648,9 @@ int pse_ethtool_get_status(struct pse_control *psec,
return -EOPNOTSUPP;
}
- mutex_lock(&psec->pcdev->lock);
+ pse_lock(psec->pcdev);
err = ops->ethtool_get_status(psec->pcdev, psec->id, extack, status);
- mutex_unlock(&psec->pcdev->lock);
+ pse_unlock(psec->pcdev);
return err;
}
@@ -443,20 +666,25 @@ int pse_ethtool_set_config(struct pse_control *psec,
struct netlink_ext_ack *extack,
const struct pse_control_config *config)
{
- const struct pse_controller_ops *ops;
- int err;
+ int err = 0;
- ops = psec->pcdev->ops;
-
- if (!ops->ethtool_set_config) {
- NL_SET_ERR_MSG(extack,
- "PSE driver does not configuration");
- return -EOPNOTSUPP;
+ /* Look at enabled status to not call regulator_enable or
+ * regulator_disable twice creating a regulator counter mismatch
+ */
+ pse_lock(psec->pcdev);
+ switch (config->c33_admin_control) {
+ case ETHTOOL_C33_PSE_ADMIN_STATE_ENABLED:
+ if (!psec->pcdev->pi[psec->id].enabled)
+ err = regulator_enable(psec->ps);
+ break;
+ case ETHTOOL_C33_PSE_ADMIN_STATE_DISABLED:
+ if (psec->pcdev->pi[psec->id].enabled)
+ err = regulator_disable(psec->ps);
+ break;
+ default:
+ err = -EOPNOTSUPP;
}
-
- mutex_lock(&psec->pcdev->lock);
- err = ops->ethtool_set_config(psec->pcdev, psec->id, extack, config);
- mutex_unlock(&psec->pcdev->lock);
+ pse_unlock(psec->pcdev);
return err;
}
@@ -24,37 +24,41 @@ static struct pse_reg_priv *to_pse_reg(struct pse_controller_dev *pcdev)
}
static int
-pse_reg_ethtool_set_config(struct pse_controller_dev *pcdev, unsigned long id,
- struct netlink_ext_ack *extack,
- const struct pse_control_config *config)
+pse_reg_pi_enable(struct pse_controller_dev *pcdev, int id)
{
struct pse_reg_priv *priv = to_pse_reg(pcdev);
int ret;
- if (priv->admin_state == config->podl_admin_control)
- return 0;
-
- switch (config->podl_admin_control) {
- case ETHTOOL_PODL_PSE_ADMIN_STATE_ENABLED:
- ret = regulator_enable(priv->ps);
- break;
- case ETHTOOL_PODL_PSE_ADMIN_STATE_DISABLED:
- ret = regulator_disable(priv->ps);
- break;
- default:
- dev_err(pcdev->dev, "Unknown admin state %i\n",
- config->podl_admin_control);
- ret = -ENOTSUPP;
- }
-
+ ret = regulator_enable(priv->ps);
if (ret)
return ret;
- priv->admin_state = config->podl_admin_control;
+ priv->admin_state = ETHTOOL_PODL_PSE_ADMIN_STATE_ENABLED;
+ return 0;
+}
+
+static int
+pse_reg_pi_disable(struct pse_controller_dev *pcdev, int id)
+{
+ struct pse_reg_priv *priv = to_pse_reg(pcdev);
+ int ret;
+ ret = regulator_disable(priv->ps);
+ if (ret)
+ return ret;
+
+ priv->admin_state = ETHTOOL_PODL_PSE_ADMIN_STATE_DISABLED;
return 0;
}
+static int
+pse_reg_pi_is_enabled(struct pse_controller_dev *pcdev, int id)
+{
+ struct pse_reg_priv *priv = to_pse_reg(pcdev);
+
+ return regulator_is_enabled(priv->ps);
+}
+
static int
pse_reg_ethtool_get_status(struct pse_controller_dev *pcdev, unsigned long id,
struct netlink_ext_ack *extack,
@@ -80,7 +84,9 @@ pse_reg_ethtool_get_status(struct pse_controller_dev *pcdev, unsigned long id,
static const struct pse_controller_ops pse_reg_ops = {
.ethtool_get_status = pse_reg_ethtool_get_status,
- .ethtool_set_config = pse_reg_ethtool_set_config,
+ .pi_enable = pse_reg_pi_enable,
+ .pi_is_enabled = pse_reg_pi_is_enabled,
+ .pi_disable = pse_reg_pi_disable,
};
static int
@@ -56,10 +56,10 @@ struct pse_controller_ops {
int (*ethtool_get_status)(struct pse_controller_dev *pcdev,
unsigned long id, struct netlink_ext_ack *extack,
struct pse_control_status *status);
- int (*ethtool_set_config)(struct pse_controller_dev *pcdev,
- unsigned long id, struct netlink_ext_ack *extack,
- const struct pse_control_config *config);
int (*setup_pi_matrix)(struct pse_controller_dev *pcdev);
+ int (*pi_is_enabled)(struct pse_controller_dev *pcdev, int id);
+ int (*pi_enable)(struct pse_controller_dev *pcdev, int id);
+ int (*pi_disable)(struct pse_controller_dev *pcdev, int id);
};
struct module;
@@ -91,10 +91,14 @@ struct pse_pi_pairset {
*
* @pairset: table of the PSE PI pinout alternative for the two pairset
* @np: device node pointer of the PSE PI node
+ * @rdev: regulator represented by the PSE PI
+ * @enabled: PI enabled state
*/
struct pse_pi {
struct pse_pi_pairset pairset[2];
struct device_node *np;
+ struct regulator_dev *rdev;
+ bool enabled;
};
/**
@@ -108,6 +112,8 @@ struct pse_pi {
* @of_pse_n_cells: number of cells in PSE line specifiers
* @nr_lines: number of PSE controls in this controller device
* @lock: Mutex for serialization access to the PSE controller
+ * @lock_owner: current owner of the mutex
+ * @ref_cnt: mutex's reference count
* @types: types of the PSE controller
* @pi: table of PSE PIs described in this controller device
* @of_legacy: flag set if the pse_pis devicetree node is not used
@@ -121,6 +127,8 @@ struct pse_controller_dev {
int of_pse_n_cells;
unsigned int nr_lines;
struct mutex lock;
+ struct task_struct *lock_owner;
+ int ref_cnt;
u32 types;
struct pse_pi *pi;
bool of_legacy;
@@ -133,7 +141,8 @@ struct device;
int devm_pse_controller_register(struct device *dev,
struct pse_controller_dev *pcdev);
-struct pse_control *of_pse_control_get(struct device_node *node);
+struct pse_control *of_pse_control_get(struct device *dev,
+ struct device_node *node);
void pse_control_put(struct pse_control *psec);
int pse_ethtool_get_status(struct pse_control *psec,
@@ -148,7 +157,8 @@ bool pse_has_c33(struct pse_control *psec);
#else
-static inline struct pse_control *of_pse_control_get(struct device_node *node)
+static inline struct pse_control *of_pse_control_get(struct device *dev,
+ struct device_node *node)
{
return ERR_PTR(-ENOENT);
}