1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
|
/*
* Copyright (c) 2017 Zodiac Inflight Innovation
* Author: Andrey Smirnov <andrew.smirnov@gmail.com>
*
* Based on analogous code from Linux kernel
*
* Copyright (C) 2011, 2012 Cavium, 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.
* This file is subject to the terms and conditions of the GNU General Public
* License. See the file "COPYING" in the main directory of this archive
* for more details.
*
*/
#include <common.h>
#include <driver.h>
#include <init.h>
#include <linux/mdio-mux.h>
#include <gpio.h>
#include <of_gpio.h>
struct mdio_mux_gpio_state {
struct gpio *gpios;
int gpios_num;
};
static void mdio_mux_gpio_set_active(struct mdio_mux_gpio_state *s,
unsigned int mask, int value)
{
while (mask) {
const int n = __ffs(mask);
mask >>= n + 1;
if (WARN_ON(n >= s->gpios_num))
break;
gpio_set_active(s->gpios[n].gpio, value);
}
}
static int mdio_mux_gpio_switch_fn(int current_child, int desired_child,
void *data)
{
if (current_child < 0)
current_child = 0;
if (current_child != desired_child) {
struct mdio_mux_gpio_state *s = data;
/*
* GPIOs that are set in both current and desired
* states can be left untouched. So we calculate them
* and clear corresponding bits in both states.
*/
const int unchanged = current_child & desired_child;
current_child &= ~unchanged;
desired_child &= ~unchanged;
/*
* First step: disable all of the currently selected
* bits that are no loger needed
*/
mdio_mux_gpio_set_active(s, current_child, 0);
/*
* Second step: enable all of the GPIOs that are
* desired to be selected
*/
mdio_mux_gpio_set_active(s, desired_child, 1);
}
return 0;
}
static int mdio_mux_gpio_probe(struct device_d *dev)
{
struct mdio_mux_gpio_state *s;
int i, r;
s = xzalloc(sizeof(*s));
s->gpios_num = of_gpio_count(dev->device_node);
if (s->gpios_num <= 0) {
dev_err(dev, "No GPIOs specified\n");
r = -EINVAL;
goto free_mem;
}
s->gpios = xzalloc(s->gpios_num * sizeof(s->gpios[0]));
for (i = 0; i < s->gpios_num; i++) {
enum of_gpio_flags flags;
r = of_get_gpio_flags(dev->device_node, i, &flags);
if (!gpio_is_valid(r)) {
r = (r < 0) ? r : -EINVAL;
goto free_mem;
}
s->gpios[i].gpio = r;
s->gpios[i].label = dev_name(dev);
s->gpios[i].flags = GPIOF_OUT_INIT_INACTIVE;
if (flags & OF_GPIO_ACTIVE_LOW)
s->gpios[i].flags |= GPIOF_ACTIVE_LOW;
}
r = gpio_request_array(s->gpios, s->gpios_num);
if (r < 0)
goto free_gpios;
r = mdio_mux_init(dev, dev->device_node,
mdio_mux_gpio_switch_fn, s, NULL);
if (r < 0)
goto free_gpios;
return 0;
free_gpios:
gpio_free_array(s->gpios, s->gpios_num);
free_mem:
free(s->gpios);
free(s);
return r;
}
static const struct of_device_id mdio_mux_gpio_match[] = {
{
.compatible = "mdio-mux-gpio",
},
{},
};
static struct driver_d mdio_mux_gpio_driver = {
.name = "mdio-mux-gpio",
.probe = mdio_mux_gpio_probe,
.of_compatible = mdio_mux_gpio_match,
};
device_platform_driver(mdio_mux_gpio_driver);
|
