118 lines · c
1// SPDX-License-Identifier: GPL-2.0+2/* MDIO bus multiplexer using kernel multiplexer subsystem3 *4 * Copyright 2019 NXP5 */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-mux20 * layer when it thinks the mdio bus21 * 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 function25 * 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 the29 * 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 return dev_err_probe(&pdev->dev, PTR_ERR(s->muxc),77 "Failed to get mux\n");78 79 platform_set_drvdata(pdev, s);80 81 ret = mdio_mux_init(&pdev->dev, pdev->dev.of_node,82 mdio_mux_multiplexer_switch_fn, &s->mux_handle,83 pdev, NULL);84 85 return ret;86}87 88static void mdio_mux_multiplexer_remove(struct platform_device *pdev)89{90 struct mdio_mux_multiplexer_state *s = platform_get_drvdata(pdev);91 92 mdio_mux_uninit(s->mux_handle);93 94 if (s->do_deselect)95 mux_control_deselect(s->muxc);96}97 98static const struct of_device_id mdio_mux_multiplexer_match[] = {99 { .compatible = "mdio-mux-multiplexer", },100 {},101};102MODULE_DEVICE_TABLE(of, mdio_mux_multiplexer_match);103 104static struct platform_driver mdio_mux_multiplexer_driver = {105 .driver = {106 .name = "mdio-mux-multiplexer",107 .of_match_table = mdio_mux_multiplexer_match,108 },109 .probe = mdio_mux_multiplexer_probe,110 .remove_new = mdio_mux_multiplexer_remove,111};112 113module_platform_driver(mdio_mux_multiplexer_driver);114 115MODULE_DESCRIPTION("MDIO bus multiplexer using kernel multiplexer subsystem");116MODULE_AUTHOR("Pankaj Bansal <pankaj.bansal@nxp.com>");117MODULE_LICENSE("GPL");118