1 // SPDX-License-Identifier: GPL-2.0
2 
3 #include <linux/errno.h>
4 #include <linux/mdio.h>
5 #include <linux/module.h>
6 #include <linux/regmap.h>
7 
8 #define REGVAL_MASK		GENMASK(15, 0)
9 #define REGNUM_C22_MASK		GENMASK(4, 0)
10 /* Clause-45 mask includes the device type (5 bit) and actual register number (16 bit) */
11 #define REGNUM_C45_MASK		GENMASK(20, 0)
12 
13 static int regmap_mdio_c22_read(void *context, unsigned int reg, unsigned int *val)
14 {
15 	struct mdio_device *mdio_dev = context;
16 	int ret;
17 
18 	if (unlikely(reg & ~REGNUM_C22_MASK))
19 		return -ENXIO;
20 
21 	ret = mdiodev_read(mdio_dev, reg);
22 	if (ret < 0)
23 		return ret;
24 
25 	*val = ret & REGVAL_MASK;
26 
27 	return 0;
28 }
29 
30 static int regmap_mdio_c22_write(void *context, unsigned int reg, unsigned int val)
31 {
32 	struct mdio_device *mdio_dev = context;
33 
34 	if (unlikely(reg & ~REGNUM_C22_MASK))
35 		return -ENXIO;
36 
37 	return mdiodev_write(mdio_dev, reg, val);
38 }
39 
40 static const struct regmap_bus regmap_mdio_c22_bus = {
41 	.reg_write = regmap_mdio_c22_write,
42 	.reg_read = regmap_mdio_c22_read,
43 };
44 
45 static int regmap_mdio_c45_read(void *context, unsigned int reg, unsigned int *val)
46 {
47 	struct mdio_device *mdio_dev = context;
48 	unsigned int devad;
49 	int ret;
50 
51 	if (unlikely(reg & ~REGNUM_C45_MASK))
52 		return -ENXIO;
53 
54 	devad = reg >> REGMAP_MDIO_C45_DEVAD_SHIFT;
55 	reg = reg & REGMAP_MDIO_C45_REGNUM_MASK;
56 
57 	ret = mdiodev_c45_read(mdio_dev, devad, reg);
58 	if (ret < 0)
59 		return ret;
60 
61 	*val = ret & REGVAL_MASK;
62 
63 	return 0;
64 }
65 
66 static int regmap_mdio_c45_write(void *context, unsigned int reg, unsigned int val)
67 {
68 	struct mdio_device *mdio_dev = context;
69 	unsigned int devad;
70 
71 	if (unlikely(reg & ~REGNUM_C45_MASK))
72 		return -ENXIO;
73 
74 	devad = reg >> REGMAP_MDIO_C45_DEVAD_SHIFT;
75 	reg = reg & REGMAP_MDIO_C45_REGNUM_MASK;
76 
77 	return mdiodev_c45_write(mdio_dev, devad, reg, val);
78 }
79 
80 static const struct regmap_bus regmap_mdio_c45_bus = {
81 	.reg_write = regmap_mdio_c45_write,
82 	.reg_read = regmap_mdio_c45_read,
83 };
84 
85 struct regmap *__regmap_init_mdio(struct mdio_device *mdio_dev,
86 	const struct regmap_config *config, struct lock_class_key *lock_key,
87 	const char *lock_name)
88 {
89 	const struct regmap_bus *bus;
90 
91 	if (config->reg_bits == 5 && config->val_bits == 16)
92 		bus = &regmap_mdio_c22_bus;
93 	else if (config->reg_bits == 21 && config->val_bits == 16)
94 		bus = &regmap_mdio_c45_bus;
95 	else
96 		return ERR_PTR(-EOPNOTSUPP);
97 
98 	return __regmap_init(&mdio_dev->dev, bus, mdio_dev, config, lock_key, lock_name);
99 }
100 EXPORT_SYMBOL_GPL(__regmap_init_mdio);
101 
102 struct regmap *__devm_regmap_init_mdio(struct mdio_device *mdio_dev,
103 	const struct regmap_config *config, struct lock_class_key *lock_key,
104 	const char *lock_name)
105 {
106 	const struct regmap_bus *bus;
107 
108 	if (config->reg_bits == 5 && config->val_bits == 16)
109 		bus = &regmap_mdio_c22_bus;
110 	else if (config->reg_bits == 21 && config->val_bits == 16)
111 		bus = &regmap_mdio_c45_bus;
112 	else
113 		return ERR_PTR(-EOPNOTSUPP);
114 
115 	return __devm_regmap_init(&mdio_dev->dev, bus, mdio_dev, config, lock_key, lock_name);
116 }
117 EXPORT_SYMBOL_GPL(__devm_regmap_init_mdio);
118 
119 MODULE_AUTHOR("Sander Vanheule <sander@svanheule.net>");
120 MODULE_DESCRIPTION("Regmap MDIO Module");
121 MODULE_LICENSE("GPL v2");
122