cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

mdio-mux-multiplexer.c (3197B)

      1// SPDX-License-Identifier: GPL-2.0+
      2/* MDIO bus multiplexer using kernel multiplexer subsystem
      3 *
      4 * Copyright 2019 NXP
      5 */
      6
      7#include <linux/mdio-mux.h>
      8#include <linux/module.h>
      9#include <linux/mux/consumer.h>
     10#include <linux/platform_device.h>
     11
     12struct mdio_mux_multiplexer_state {
     13	struct mux_control *muxc;
     14	bool do_deselect;
     15	void *mux_handle;
     16};
     17
     18/**
     19 * mdio_mux_multiplexer_switch_fn - This function is called by the mdio-mux
     20 *                                  layer when it thinks the mdio bus
     21 *                                  multiplexer needs to switch.
     22 * @current_child:  current value of the mux register.
     23 * @desired_child: value of the 'reg' property of the target child MDIO node.
     24 * @data: Private data used by this switch_fn passed to mdio_mux_init function
     25 *        via mdio_mux_init(.., .., .., .., data, ..).
     26 *
     27 * The first time this function is called, current_child == -1.
     28 * If current_child == desired_child, then the mux is already set to the
     29 * correct bus.
     30 */
     31static int mdio_mux_multiplexer_switch_fn(int current_child, int desired_child,
     32					  void *data)
     33{
     34	struct platform_device *pdev;
     35	struct mdio_mux_multiplexer_state *s;
     36	int ret = 0;
     37
     38	pdev = (struct platform_device *)data;
     39	s = platform_get_drvdata(pdev);
     40
     41	if (!(current_child ^ desired_child))
     42		return 0;
     43
     44	if (s->do_deselect)
     45		ret = mux_control_deselect(s->muxc);
     46	if (ret) {
     47		dev_err(&pdev->dev, "mux_control_deselect failed in %s: %d\n",
     48			__func__, ret);
     49		return ret;
     50	}
     51
     52	ret =  mux_control_select(s->muxc, desired_child);
     53	if (!ret) {
     54		dev_dbg(&pdev->dev, "%s %d -> %d\n", __func__, current_child,
     55			desired_child);
     56		s->do_deselect = true;
     57	} else {
     58		s->do_deselect = false;
     59	}
     60
     61	return ret;
     62}
     63
     64static int mdio_mux_multiplexer_probe(struct platform_device *pdev)
     65{
     66	struct device *dev = &pdev->dev;
     67	struct mdio_mux_multiplexer_state *s;
     68	int ret = 0;
     69
     70	s = devm_kzalloc(&pdev->dev, sizeof(*s), GFP_KERNEL);
     71	if (!s)
     72		return -ENOMEM;
     73
     74	s->muxc = devm_mux_control_get(dev, NULL);
     75	if (IS_ERR(s->muxc)) {
     76		ret = PTR_ERR(s->muxc);
     77		if (ret != -EPROBE_DEFER)
     78			dev_err(&pdev->dev, "Failed to get mux: %d\n", ret);
     79		return ret;
     80	}
     81
     82	platform_set_drvdata(pdev, s);
     83
     84	ret = mdio_mux_init(&pdev->dev, pdev->dev.of_node,
     85			    mdio_mux_multiplexer_switch_fn, &s->mux_handle,
     86			    pdev, NULL);
     87
     88	return ret;
     89}
     90
     91static int mdio_mux_multiplexer_remove(struct platform_device *pdev)
     92{
     93	struct mdio_mux_multiplexer_state *s = platform_get_drvdata(pdev);
     94
     95	mdio_mux_uninit(s->mux_handle);
     96
     97	if (s->do_deselect)
     98		mux_control_deselect(s->muxc);
     99
    100	return 0;
    101}
    102
    103static const struct of_device_id mdio_mux_multiplexer_match[] = {
    104	{ .compatible = "mdio-mux-multiplexer", },
    105	{},
    106};
    107MODULE_DEVICE_TABLE(of, mdio_mux_multiplexer_match);
    108
    109static struct platform_driver mdio_mux_multiplexer_driver = {
    110	.driver = {
    111		.name		= "mdio-mux-multiplexer",
    112		.of_match_table	= mdio_mux_multiplexer_match,
    113	},
    114	.probe		= mdio_mux_multiplexer_probe,
    115	.remove		= mdio_mux_multiplexer_remove,
    116};
    117
    118module_platform_driver(mdio_mux_multiplexer_driver);
    119
    120MODULE_DESCRIPTION("MDIO bus multiplexer using kernel multiplexer subsystem");
    121MODULE_AUTHOR("Pankaj Bansal <pankaj.bansal@nxp.com>");
    122MODULE_LICENSE("GPL");