aboutsummaryrefslogtreecommitdiffstats
path: root/package/libs/libnl-tiny/src/object.c
blob: f91555137dcdf8c726ea0d83757a159eebb587d4 (plain)
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
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
/*
 * lib/object.c		Generic Cacheable Object
 *
 *	This library is free software; you can redistribute it and/or
 *	modify it under the terms of the GNU Lesser General Public
 *	License as published by the Free Software Foundation version 2.1
 *	of the License.
 *
 * Copyright (c) 2003-2008 Thomas Graf <tgraf@suug.ch>
 */

/**
 * @ingroup cache
 * @defgroup object Object
 * @{
 */

#include <netlink-local.h>
#include <netlink/netlink.h>
#include <netlink/cache.h>
#include <netlink/object.h>
#include <netlink/utils.h>

static inline struct nl_object_ops *obj_ops(struct nl_object *obj)
{
	if (!obj->ce_ops)
		BUG();

	return obj->ce_ops;
}

/**
 * @name Object Creation/Deletion
 * @{
 */

/**
 * Allocate a new object of kind specified by the operations handle
 * @arg ops		cache operations handle
 * @return The new object or NULL
 */
struct nl_object *nl_object_alloc(struct nl_object_ops *ops)
{
	struct nl_object *new;

	if (ops->oo_size < sizeof(*new))
		BUG();

	new = calloc(1, ops->oo_size);
	if (!new)
		return NULL;

	new->ce_refcnt = 1;
	nl_init_list_head(&new->ce_list);

	new->ce_ops = ops;
	if (ops->oo_constructor)
		ops->oo_constructor(new);

	NL_DBG(4, "Allocated new object %p\n", new);

	return new;
}

#ifdef disabled
/**
 * Allocate a new object of kind specified by the name
 * @arg kind		name of object type
 * @return The new object or nULL
 */
int nl_object_alloc_name(const char *kind, struct nl_object **result)
{
	struct nl_cache_ops *ops;

	ops = nl_cache_ops_lookup(kind);
	if (!ops)
		return -NLE_OPNOTSUPP;

	if (!(*result = nl_object_alloc(ops->co_obj_ops)))
		return -NLE_NOMEM;

	return 0;
}
#endif

struct nl_derived_object {
	NLHDR_COMMON
	char data;
};

/**
 * Allocate a new object and copy all data from an existing object
 * @arg obj		object to inherite data from
 * @return The new object or NULL.
 */
struct nl_object *nl_object_clone(struct nl_object *obj)
{
	struct nl_object *new;
	struct nl_object_ops *ops = obj_ops(obj);
	int doff = offsetof(struct nl_derived_object, data);
	int size;

	new = nl_object_alloc(ops);
	if (!new)
		return NULL;

	size = ops->oo_size - doff;
	if (size < 0)
		BUG();

	new->ce_ops = obj->ce_ops;
	new->ce_msgtype = obj->ce_msgtype;

	if (size)
		memcpy((void *)new + doff, (void *)obj + doff, size);

	if (ops->oo_clone) {
		if (ops->oo_clone(new, obj) < 0) {
			nl_object_free(new);
			return NULL;
		}
	} else if (size && ops->oo_free_data)
		BUG();

	return new;
}

/**
 * Free a cacheable object
 * @arg obj		object to free
 *
 * @return 0 or a negative error code.
 */
void nl_object_free(struct nl_object *obj)
{
	struct nl_object_ops *ops = obj_ops(obj);

	if (obj->ce_refcnt > 0)
		NL_DBG(1, "Warning: Freeing object in use...\n");

	if (obj->ce_cache)
		nl_cache_remove(obj);

	if (ops->oo_free_data)
		ops->oo_free_data(obj);

	free(obj);

	NL_DBG(4, "Freed object %p\n", obj);
}

/** @} */

/**
 * @name Reference Management
 * @{
 */

/** @} */

/**
 * @name Utillities
 * @{
 */

#ifdef disabled
/**
 * Dump this object according to the specified parameters
 * @arg obj		object to dump
 * @arg params		dumping parameters
 */
void nl_object_dump(struct nl_object *obj, struct nl_dump_params *params)
{
	dump_from_ops(obj, params);
}

/**
 * Check if the identifiers of two objects are identical 
 * @arg a		an object
 * @arg b		another object of same type
 *
 * @return true if both objects have equal identifiers, otherwise false.
 */
int nl_object_identical(struct nl_object *a, struct nl_object *b)
{
	struct nl_object_ops *ops = obj_ops(a);
	int req_attrs;

	/* Both objects must be of same type */
	if (ops != obj_ops(b))
		return 0;

	req_attrs = ops->oo_id_attrs;

	/* Both objects must provide all required attributes to uniquely
	 * identify an object */
	if ((a->ce_mask & req_attrs) != req_attrs ||
	    (b->ce_mask & req_attrs) != req_attrs)
		return 0;

	/* Can't judge unless we can compare */
	if (ops->oo_compare == NULL)
		return 0;

	return !(ops->oo_compare(a, b, req_attrs, 0));
}

/**
 * Compute bitmask representing difference in attribute values
 * @arg a		an object
 * @arg b		another object of same type
 *
 * The bitmask returned is specific to an object type, each bit set represents
 * an attribute which mismatches in either of the two objects. Unavailability
 * of an attribute in one object and presence in the other is regarded a
 * mismatch as well.
 *
 * @return Bitmask describing differences or 0 if they are completely identical.
 */
uint32_t nl_object_diff(struct nl_object *a, struct nl_object *b)
{
	struct nl_object_ops *ops = obj_ops(a);

	if (ops != obj_ops(b) || ops->oo_compare == NULL)
		return UINT_MAX;

	return ops->oo_compare(a, b, ~0, 0);
}

/**
 * Match a filter against an object
 * @arg obj		object to check
 * @arg filter		object of same type acting as filter
 *
 * @return 1 if the object matches the filter or 0
 *           if no filter procedure is available or if the
 *           filter does not match.
 */
int nl_object_match_filter(struct nl_object *obj, struct nl_object *filter)
{
	struct nl_object_ops *ops = obj_ops(obj);

	if (ops != obj_ops(filter) || ops->oo_compare == NULL)
		return 0;
	
	return !(ops->oo_compare(obj, filter, filter->ce_mask,
				 LOOSE_COMPARISON));
}

/**
 * Convert bitmask of attributes to a character string
 * @arg obj		object of same type as attribute bitmask
 * @arg attrs		bitmask of attribute types
 * @arg buf		destination buffer
 * @arg len		length of destination buffer
 *
 * Converts the bitmask of attribute types into a list of attribute
 * names separated by comas.
 *
 * @return destination buffer.
 */
char *nl_object_attrs2str(struct nl_object *obj, uint32_t attrs,
			  char *buf, size_t len)
{
	struct nl_object_ops *ops = obj_ops(obj);

	if (ops->oo_attrs2str != NULL)
		return ops->oo_attrs2str(attrs, buf, len);
	else {
		memset(buf, 0, len);
		return buf;
	}
}

#endif

/** @} */

/** @} */
2200; background-color: #fff0f0 } /* Literal.String.Affix */ .highlight .sb { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Backtick */ .highlight .sc { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Char */ .highlight .dl { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Delimiter */ .highlight .sd { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Doc */ .highlight .s2 { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Double */ .highlight .se { color: #0044dd; background-color: #fff0f0 } /* Literal.String.Escape */ .highlight .sh { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Heredoc */ .highlight .si { color: #3333bb; background-color: #fff0f0 } /* Literal.String.Interpol */ .highlight .sx { color: #22bb22; background-color: #f0fff0 } /* Literal.String.Other */ .highlight .sr { color: #008800; background-color: #fff0ff } /* Literal.String.Regex */ .highlight .s1 { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Single */ .highlight .ss { color: #aa6600; background-color: #fff0f0 } /* Literal.String.Symbol */ .highlight .bp { color: #003388 } /* Name.Builtin.Pseudo */ .highlight .fm { color: #0066bb; font-weight: bold } /* Name.Function.Magic */ .highlight .vc { color: #336699 } /* Name.Variable.Class */ .highlight .vg { color: #dd7700 } /* Name.Variable.Global */ .highlight .vi { color: #3333bb } /* Name.Variable.Instance */ .highlight .vm { color: #336699 } /* Name.Variable.Magic */ .highlight .il { color: #0000DD; font-weight: bold } /* Literal.Number.Integer.Long */
/*
 * rtl8306.c: RTL8306S switch driver
 *
 * Copyright (C) 2009 Felix Fietkau <nbd@openwrt.org>
 *
 * 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.
 */

#include <linux/if.h>
#include <linux/module.h>
#include <linux/init.h>
#include <linux/list.h>
#include <linux/if_ether.h>
#include <linux/skbuff.h>
#include <linux/netdevice.h>
#include <linux/netlink.h>
#include <net/genetlink.h>
#include <linux/switch.h>
#include <linux/delay.h>
#include <linux/phy.h>

//#define DEBUG 1

/* Global (PHY0) */
#define RTL8306_REG_PAGE		16
#define RTL8306_REG_PAGE_LO		(1 << 15)
#define RTL8306_REG_PAGE_HI		(1 << 1) /* inverted */

#define RTL8306_NUM_VLANS		16
#define RTL8306_NUM_PORTS		6
#define RTL8306_PORT_CPU		5
#define RTL8306_NUM_PAGES		4
#define RTL8306_NUM_REGS		32

#define RTL_NAME_S          "RTL8306S"
#define RTL_NAME_SD         "RTL8306SD"
#define RTL_NAME_SDM        "RTL8306SDM"
#define RTL_NAME_UNKNOWN    "RTL8306(unknown)"

#define RTL8306_MAGIC	0x8306

static LIST_HEAD(phydevs);

struct rtl_priv {
	struct list_head list;
	struct switch_dev dev;
	int page;
	int type;
	int do_cpu;
	struct mii_bus *bus;
	char hwname[sizeof(RTL_NAME_UNKNOWN)];
	bool fixup;
};

struct rtl_phyregs {
	int nway;
	int speed;
	int duplex;
};

#define to_rtl(_dev) container_of(_dev, struct rtl_priv, dev)

enum {
	RTL_TYPE_S,
	RTL_TYPE_SD,
	RTL_TYPE_SDM,
};

struct rtl_reg {
	int page;
	int phy;
	int reg;
	int bits;
	int shift;
	int inverted;
};

#define RTL_VLAN_REGOFS(name) \
	(RTL_REG_VLAN1_##name - RTL_REG_VLAN0_##name)

#define RTL_PORT_REGOFS(name) \
	(RTL_REG_PORT1_##name - RTL_REG_PORT0_##name)

#define RTL_PORT_REG(id, reg) \
	(RTL_REG_PORT0_##reg + (id * RTL_PORT_REGOFS(reg)))

#define RTL_VLAN_REG(id, reg) \
	(RTL_REG_VLAN0_##reg + (id * RTL_VLAN_REGOFS(reg)))

#define RTL_GLOBAL_REGATTR(reg) \
	.id = RTL_REG_##reg, \
	.type = SWITCH_TYPE_INT, \
	.ofs = 0, \
	.set = rtl_attr_set_int, \
	.get = rtl_attr_get_int

#define RTL_PORT_REGATTR(reg) \
	.id = RTL_REG_PORT0_##reg, \
	.type = SWITCH_TYPE_INT, \
	.ofs = RTL_PORT_REGOFS(reg), \
	.set = rtl_attr_set_port_int, \
	.get = rtl_attr_get_port_int

#define RTL_VLAN_REGATTR(reg) \
	.id = RTL_REG_VLAN0_##reg, \
	.type = SWITCH_TYPE_INT, \
	.ofs = RTL_VLAN_REGOFS(reg), \
	.set = rtl_attr_set_vlan_int, \
	.get = rtl_attr_get_vlan_int

enum rtl_regidx {
	RTL_REG_CHIPID,
	RTL_REG_CHIPVER,
	RTL_REG_CHIPTYPE,
	RTL_REG_CPUPORT,

	RTL_REG_EN_CPUPORT,
	RTL_REG_EN_TAG_OUT,
	RTL_REG_EN_TAG_CLR,
	RTL_REG_EN_TAG_IN,
	RTL_REG_TRAP_CPU,
	RTL_REG_TRUNK_PORTSEL,
	RTL_REG_EN_TRUNK,
	RTL_REG_RESET,

	RTL_REG_VLAN_ENABLE,
	RTL_REG_VLAN_FILTER,
	RTL_REG_VLAN_TAG_ONLY,
	RTL_REG_VLAN_TAG_AWARE,
#define RTL_VLAN_ENUM(id) \
	RTL_REG_VLAN##id##_VID, \
	RTL_REG_VLAN##id##_PORTMASK
	RTL_VLAN_ENUM(0),
	RTL_VLAN_ENUM(1),
	RTL_VLAN_ENUM(2),
	RTL_VLAN_ENUM(3),
	RTL_VLAN_ENUM(4),
	RTL_VLAN_ENUM(5),
	RTL_VLAN_ENUM(6),
	RTL_VLAN_ENUM(7),
	RTL_VLAN_ENUM(8),
	RTL_VLAN_ENUM(9),
	RTL_VLAN_ENUM(10),
	RTL_VLAN_ENUM(11),
	RTL_VLAN_ENUM(12),
	RTL_VLAN_ENUM(13),
	RTL_VLAN_ENUM(14),
	RTL_VLAN_ENUM(15),
#define RTL_PORT_ENUM(id) \
	RTL_REG_PORT##id##_PVID, \
	RTL_REG_PORT##id##_NULL_VID_REPLACE, \
	RTL_REG_PORT##id##_NON_PVID_DISCARD, \
	RTL_REG_PORT##id##_VID_INSERT, \
	RTL_REG_PORT##id##_TAG_INSERT, \
	RTL_REG_PORT##id##_LINK, \
	RTL_REG_PORT##id##_SPEED, \
	RTL_REG_PORT##id##_NWAY, \
	RTL_REG_PORT##id##_NRESTART, \
	RTL_REG_PORT##id##_DUPLEX, \
	RTL_REG_PORT##id##_RXEN, \
	RTL_REG_PORT##id##_TXEN
	RTL_PORT_ENUM(0),
	RTL_PORT_ENUM(1),
	RTL_PORT_ENUM(2),
	RTL_PORT_ENUM(3),
	RTL_PORT_ENUM(4),
	RTL_PORT_ENUM(5),
};

static const struct rtl_reg rtl_regs[] = {
	[RTL_REG_CHIPID]         = { 0, 4, 30, 16,  0, 0 },
	[RTL_REG_CHIPVER]        = { 0, 4, 31,  8,  0, 0 },
	[RTL_REG_CHIPTYPE]       = { 0, 4, 31,  2,  8, 0 },

	/* CPU port number */
	[RTL_REG_CPUPORT]        = { 2, 4, 21,  3,  0, 0 },
	/* Enable CPU port function */
	[RTL_REG_EN_CPUPORT]     = { 3, 2, 21,  1, 15, 1 },
	/* Enable CPU port tag insertion */
	[RTL_REG_EN_TAG_OUT]     = { 3, 2, 21,  1, 12, 0 },
	/* Enable CPU port tag removal */
	[RTL_REG_EN_TAG_CLR]     = { 3, 2, 21,  1, 11, 0 },
	/* Enable CPU port tag checking */
	[RTL_REG_EN_TAG_IN]      = { 0, 4, 21,  1,  7, 0 },
	[RTL_REG_EN_TRUNK]       = { 0, 0, 19,  1, 11, 1 },
	[RTL_REG_TRUNK_PORTSEL]  = { 0, 0, 16,  1,  6, 1 },
	[RTL_REG_RESET]          = { 0, 0, 16,  1, 12, 0 },

	[RTL_REG_TRAP_CPU]       = { 3, 2, 22,  1,  6, 0 },

	[RTL_REG_VLAN_TAG_ONLY]  = { 0, 0, 16,  1,  8, 1 },
	[RTL_REG_VLAN_FILTER]    = { 0, 0, 16,  1,  9, 1 },
	[RTL_REG_VLAN_TAG_AWARE] = { 0, 0, 16,  1, 10, 1 },
	[RTL_REG_VLAN_ENABLE]    = { 0, 0, 18,  1,  8, 1 },

#define RTL_VLAN_REGS(id, phy, page, regofs) \
	[RTL_REG_VLAN##id##_VID] = { page, phy, 25 + regofs, 12, 0, 0 }, \
	[RTL_REG_VLAN##id##_PORTMASK] = { page, phy, 24 + regofs, 6, 0, 0 }
	RTL_VLAN_REGS( 0, 0, 0, 0),
	RTL_VLAN_REGS( 1, 1, 0, 0),
	RTL_VLAN_REGS( 2, 2, 0, 0),
	RTL_VLAN_REGS( 3, 3, 0, 0),
	RTL_VLAN_REGS( 4, 4, 0, 0),
	RTL_VLAN_REGS( 5, 0, 1, 2),
	RTL_VLAN_REGS( 6, 1, 1, 2),
	RTL_VLAN_REGS( 7, 2, 1, 2),
	RTL_VLAN_REGS( 8, 3, 1, 2),
	RTL_VLAN_REGS( 9, 4, 1, 2),
	RTL_VLAN_REGS(10, 0, 1, 4),
	RTL_VLAN_REGS(11, 1, 1, 4),
	RTL_VLAN_REGS(12, 2, 1, 4),
	RTL_VLAN_REGS(13, 3, 1, 4),
	RTL_VLAN_REGS(14, 4, 1, 4),
	RTL_VLAN_REGS(15, 0, 1, 6),

#define REG_PORT_SETTING(port, phy) \
	[RTL_REG_PORT##port##_SPEED] = { 0, phy, 0, 1, 13, 0 }, \
	[RTL_REG_PORT##port##_NWAY] = { 0, phy, 0, 1, 12, 0 }, \
	[RTL_REG_PORT##port##_NRESTART] = { 0, phy, 0, 1, 9, 0 }, \
	[RTL_REG_PORT##port##_DUPLEX] = { 0, phy, 0, 1, 8, 0 }, \
	[RTL_REG_PORT##port##_TXEN] = { 0, phy, 24, 1, 11, 0 }, \
	[RTL_REG_PORT##port##_RXEN] = { 0, phy, 24, 1, 10, 0 }, \
	[RTL_REG_PORT##port##_LINK] = { 0, phy, 1, 1, 2, 0 }, \
	[RTL_REG_PORT##port##_NULL_VID_REPLACE] = { 0, phy, 22, 1, 12, 0 }, \
	[RTL_REG_PORT##port##_NON_PVID_DISCARD] = { 0, phy, 22, 1, 11, 0 }, \
	[RTL_REG_PORT##port##_VID_INSERT] = { 0, phy, 22, 2, 9, 0 }, \
	[RTL_REG_PORT##port##_TAG_INSERT] = { 0, phy, 22, 2, 0, 0 }

	REG_PORT_SETTING(0, 0),
	REG_PORT_SETTING(1, 1),
	REG_PORT_SETTING(2, 2),
	REG_PORT_SETTING(3, 3),
	REG_PORT_SETTING(4, 4),
	REG_PORT_SETTING(5, 6),

#define REG_PORT_PVID(phy, page, regofs) \
	{ page, phy, 24 + regofs, 4, 12, 0 }
	[RTL_REG_PORT0_PVID] = REG_PORT_PVID(0, 0, 0),
	[RTL_REG_PORT1_PVID] = REG_PORT_PVID(1, 0, 0),
	[RTL_REG_PORT2_PVID] = REG_PORT_PVID(2, 0, 0),
	[RTL_REG_PORT3_PVID] = REG_PORT_PVID(3, 0, 0),
	[RTL_REG_PORT4_PVID] = REG_PORT_PVID(4, 0, 0),
	[RTL_REG_PORT5_PVID] = REG_PORT_PVID(0, 1, 2),
};


static inline void
rtl_set_page(struct rtl_priv *priv, unsigned int page)
{
	struct mii_bus *bus = priv->bus;
	u16 pgsel;

	if (priv->fixup)
		return;

	if (priv->page == page)
		return;

	BUG_ON(page > RTL8306_NUM_PAGES);
	pgsel = bus->read(bus, 0, RTL8306_REG_PAGE);
	pgsel &= ~(RTL8306_REG_PAGE_LO | RTL8306_REG_PAGE_HI);
	if (page & (1 << 0))
		pgsel |= RTL8306_REG_PAGE_LO;
	if (!(page & (1 << 1))) /* bit is inverted */
		pgsel |= RTL8306_REG_PAGE_HI;
	bus->write(bus, 0, RTL8306_REG_PAGE, pgsel);
}

static inline int
rtl_w16(struct switch_dev *dev, unsigned int page, unsigned int phy, unsigned int reg, u16 val)
{
	struct rtl_priv *priv = to_rtl(dev);
	struct mii_bus *bus = priv->bus;

	rtl_set_page(priv, page);
	bus->write(bus, phy, reg, val);
	bus->read(bus, phy, reg); /* flush */
	return 0;
}

static inline int
rtl_r16(struct switch_dev *dev, unsigned int page, unsigned int phy, unsigned int reg)
{
	struct rtl_priv *priv = to_rtl(dev);
	struct mii_bus *bus = priv->bus;

	rtl_set_page(priv, page);
	return bus->read(bus, phy, reg);
}

static inline u16
rtl_rmw(struct switch_dev *dev, unsigned int page, unsigned int phy, unsigned int reg, u16 mask, u16 val)
{
	struct rtl_priv *priv = to_rtl(dev);
	struct mii_bus *bus = priv->bus;
	u16 r;

	rtl_set_page(priv, page);
	r = bus->read(bus, phy, reg);
	r &= ~mask;
	r |= val;
	bus->write(bus, phy, reg, r);
	return bus->read(bus, phy, reg); /* flush */
}


static inline int
rtl_get(struct switch_dev *dev, enum rtl_regidx s)
{
	const struct rtl_reg *r = &rtl_regs[s];
	u16 val;

	BUG_ON(s >= ARRAY_SIZE(rtl_regs));
	if (r->bits == 0) /* unimplemented */
		return 0;

	val = rtl_r16(dev, r->page, r->phy, r->reg);

	if (r->shift > 0)
		val >>= r->shift;

	if (r->inverted)
		val = ~val;

	val &= (1 << r->bits) - 1;

	return val;
}

static int
rtl_set(struct switch_dev *dev, enum rtl_regidx s, unsigned int val)
{
	const struct rtl_reg *r = &rtl_regs[s];
	u16 mask = 0xffff;

	BUG_ON(s >= ARRAY_SIZE(rtl_regs));

	if (r->bits == 0) /* unimplemented */
		return 0;

	if (r->shift > 0)
		val <<= r->shift;

	if (r->inverted)
		val = ~val;

	if (r->bits != 16) {
		mask = (1 << r->bits) - 1;
		mask <<= r->shift;
	}
	val &= mask;
	return rtl_rmw(dev, r->page, r->phy, r->reg, mask, val);
}

static void
rtl_phy_save(struct switch_dev *dev, int port, struct rtl_phyregs *regs)
{
	regs->nway = rtl_get(dev, RTL_PORT_REG(port, NWAY));
	regs->speed = rtl_get(dev, RTL_PORT_REG(port, SPEED));
	regs->duplex = rtl_get(dev, RTL_PORT_REG(port, DUPLEX));
}

static void
rtl_phy_restore(struct switch_dev *dev, int port, struct rtl_phyregs *regs)
{
	rtl_set(dev, RTL_PORT_REG(port, NWAY), regs->nway);
	rtl_set(dev, RTL_PORT_REG(port, SPEED), regs->speed);
	rtl_set(dev, RTL_PORT_REG(port, DUPLEX), regs->duplex);
}

static void
rtl_port_set_enable(struct switch_dev *dev, int port, int enabled)
{
	rtl_set(dev, RTL_PORT_REG(port, RXEN), enabled);
	rtl_set(dev, RTL_PORT_REG(port, TXEN), enabled);

	if ((port >= 5) || !enabled)
		return;

	/* restart autonegotiation if enabled */
	rtl_set(dev, RTL_PORT_REG(port, NRESTART), 1);
}

static int
rtl_hw_apply(struct switch_dev *dev)
{
	int i;
	int trunk_en, trunk_psel;
	struct rtl_phyregs port5;

	rtl_phy_save(dev, 5, &port5);

	/* disable rx/tx from PHYs */
	for (i = 0; i < RTL8306_NUM_PORTS - 1; i++) {
		rtl_port_set_enable(dev, i, 0);
	}

	/* save trunking status */
	trunk_en = rtl_get(dev, RTL_REG_EN_TRUNK);
	trunk_psel = rtl_get(dev, RTL_REG_TRUNK_PORTSEL);

	/* trunk port 3 and 4
	 * XXX: Big WTF, but RealTek seems to do it */
	rtl_set(dev, RTL_REG_EN_TRUNK, 1);
	rtl_set(dev, RTL_REG_TRUNK_PORTSEL, 1);

	/* execute the software reset */
	rtl_set(dev, RTL_REG_RESET, 1);

	/* wait for the reset to complete,
	 * but don't wait for too long */
	for (i = 0; i < 10; i++) {
		if (rtl_get(dev, RTL_REG_RESET) == 0)
			break;

		msleep(1);
	}

	/* enable rx/tx from PHYs */
	for (i = 0; i < RTL8306_NUM_PORTS - 1; i++) {
		rtl_port_set_enable(dev, i, 1);
	}

	/* restore trunking settings */
	rtl_set(dev, RTL_REG_EN_TRUNK, trunk_en);
	rtl_set(dev, RTL_REG_TRUNK_PORTSEL, trunk_psel);
	rtl_phy_restore(dev, 5, &port5);

	return 0;
}

static void
rtl_hw_init(struct switch_dev *dev)
{
	struct rtl_priv *priv = to_rtl(dev);
	int cpu_mask = 1 << dev->cpu_port;
	int i;

	rtl_set(dev, RTL_REG_VLAN_ENABLE, 0);
	rtl_set(dev, RTL_REG_VLAN_FILTER, 0);
	rtl_set(dev, RTL_REG_EN_TRUNK, 0);
	rtl_set(dev, RTL_REG_TRUNK_PORTSEL, 0);

	/* initialize cpu port settings */
	if (priv->do_cpu) {
		rtl_set(dev, RTL_REG_CPUPORT, dev->cpu_port);
		rtl_set(dev, RTL_REG_EN_CPUPORT, 1);
	} else {
		rtl_set(dev, RTL_REG_CPUPORT, 7);
		rtl_set(dev, RTL_REG_EN_CPUPORT, 0);
	}
	rtl_set(dev, RTL_REG_EN_TAG_OUT, 0);
	rtl_set(dev, RTL_REG_EN_TAG_IN, 0);
	rtl_set(dev, RTL_REG_EN_TAG_CLR, 0);

	/* reset all vlans */
	for (i = 0; i < RTL8306_NUM_VLANS; i++) {
		rtl_set(dev, RTL_VLAN_REG(i, VID), i);
		rtl_set(dev, RTL_VLAN_REG(i, PORTMASK), 0);
	}

	/* default to port isolation */
	for (i = 0; i < RTL8306_NUM_PORTS; i++) {
		unsigned long mask;

		if ((1 << i) == cpu_mask)
			mask = ((1 << RTL8306_NUM_PORTS) - 1) & ~cpu_mask; /* all bits set */
		else
			mask = cpu_mask | (1 << i);

		rtl_set(dev, RTL_VLAN_REG(i, PORTMASK), mask);
		rtl_set(dev, RTL_PORT_REG(i, PVID), i);
		rtl_set(dev, RTL_PORT_REG(i, NULL_VID_REPLACE), 1);
		rtl_set(dev, RTL_PORT_REG(i, VID_INSERT), 1);
		rtl_set(dev, RTL_PORT_REG(i, TAG_INSERT), 3);
	}
	rtl_hw_apply(dev);
}

#ifdef DEBUG
static int
rtl_set_use_cpuport(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
{
	struct rtl_priv *priv = to_rtl(dev);
	priv->do_cpu = val->value.i;
	rtl_hw_init(dev);
	return 0;
}

static int
rtl_get_use_cpuport(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
{
	struct rtl_priv *priv = to_rtl(dev);
	val->value.i = priv->do_cpu;
	return 0;
}

static int
rtl_set_cpuport(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
{
	dev->cpu_port = val->value.i;
	rtl_hw_init(dev);
	return 0;
}

static int
rtl_get_cpuport(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
{
	val->value.i = dev->cpu_port;
	return 0;
}
#endif

static int
rtl_reset(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
{
	rtl_hw_init(dev);
	return 0;
}

static int
rtl_attr_set_int(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
{
	int idx = attr->id + (val->port_vlan * attr->ofs);
	struct rtl_phyregs port;

	if (attr->id >= ARRAY_SIZE(rtl_regs))
		return -EINVAL;

	if ((attr->max > 0) && (val->value.i > attr->max))
		return -EINVAL;

	/* access to phy register 22 on port 4/5
	 * needs phy status save/restore */
	if ((val->port_vlan > 3) &&
		(rtl_regs[idx].reg == 22) &&
		(rtl_regs[idx].page == 0)) {

		rtl_phy_save(dev, val->port_vlan, &port);
		rtl_set(dev, idx, val->value.i);
		rtl_phy_restore(dev, val->port_vlan, &port);
	} else {
		rtl_set(dev, idx, val->value.i);
	}

	return 0;
}

static int
rtl_attr_get_int(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
{
	int idx = attr->id + (val->port_vlan * attr->ofs);

	if (idx >= ARRAY_SIZE(rtl_regs))
		return -EINVAL;

	val->value.i = rtl_get(dev, idx);
	return 0;
}

static int
rtl_attr_set_port_int(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
{
	if (val->port_vlan >= RTL8306_NUM_PORTS)
		return -EINVAL;

	return rtl_attr_set_int(dev, attr, val);
}

static int
rtl_attr_get_port_int(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
{
	if (val->port_vlan >= RTL8306_NUM_PORTS)
		return -EINVAL;
	return rtl_attr_get_int(dev, attr, val);
}

static int
rtl_attr_set_vlan_int(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
{
	if (val->port_vlan >= dev->vlans)
		return -EINVAL;

	return rtl_attr_set_int(dev, attr, val);
}

static int
rtl_attr_get_vlan_int(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
{
	if (val->port_vlan >= dev->vlans)
		return -EINVAL;

	return rtl_attr_get_int(dev, attr, val);
}

static int
rtl_get_ports(struct switch_dev *dev, struct switch_val *val)
{
	unsigned int i, mask;

	mask = rtl_get(dev, RTL_VLAN_REG(val->port_vlan, PORTMASK));
	for (i = 0; i < RTL8306_NUM_PORTS; i++) {
		struct switch_port *port;

		if (!(mask & (1 << i)))
			continue;

		port = &val->value.ports[val->len];
		port->id = i;
		port->flags = 0;
		val->len++;
	}

	return 0;
}

static int
rtl_set_vlan(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
{
	struct rtl_priv *priv = to_rtl(dev);
	struct rtl_phyregs port;
	int en = val->value.i;
	int i;

	rtl_set(dev, RTL_REG_EN_TAG_OUT, en && priv->do_cpu);
	rtl_set(dev, RTL_REG_EN_TAG_IN, en && priv->do_cpu);
	rtl_set(dev, RTL_REG_EN_TAG_CLR, en && priv->do_cpu);
	rtl_set(dev, RTL_REG_VLAN_TAG_AWARE, en);
	if (en)
		rtl_set(dev, RTL_REG_VLAN_FILTER, en);

	for (i = 0; i < RTL8306_NUM_PORTS; i++) {
		if (i > 3)
			rtl_phy_save(dev, val->port_vlan, &port);
		rtl_set(dev, RTL_PORT_REG(i, NULL_VID_REPLACE), 1);
		rtl_set(dev, RTL_PORT_REG(i, VID_INSERT), (en ? (i == dev->cpu_port ? 0 : 1) : 1));
		rtl_set(dev, RTL_PORT_REG(i, TAG_INSERT), (en ? (i == dev->cpu_port ? 2 : 1) : 3));
		if (i > 3)
			rtl_phy_restore(dev, val->port_vlan, &port);
	}
	rtl_set(dev, RTL_REG_VLAN_ENABLE, en);

	return 0;
}

static int
rtl_get_vlan(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
{
	return rtl_get(dev, RTL_REG_VLAN_ENABLE);
}

static int
rtl_set_ports(struct switch_dev *dev, struct switch_val *val)
{
	unsigned int mask = 0;
	unsigned int oldmask;
	int i;

	for(i = 0; i < val->len; i++)
	{
		struct switch_port *port = &val->value.ports[i];
		bool tagged = false;

		mask |= (1 << port->id);

		if (port->id == dev->cpu_port)
			continue;

		if ((i == dev->cpu_port) ||
			(port->flags & (1 << SWITCH_PORT_FLAG_TAGGED)))
			tagged = true;

		/* fix up PVIDs for added ports */
		if (!tagged)
			rtl_set(dev, RTL_PORT_REG(port->id, PVID), val->port_vlan);

		rtl_set(dev, RTL_PORT_REG(port->id, NON_PVID_DISCARD), (tagged ? 0 : 1));
		rtl_set(dev, RTL_PORT_REG(port->id, VID_INSERT), (tagged ? 0 : 1));
		rtl_set(dev, RTL_PORT_REG(port->id, TAG_INSERT), (tagged ? 2 : 1));
	}

	oldmask = rtl_get(dev, RTL_VLAN_REG(val->port_vlan, PORTMASK));
	rtl_set(dev, RTL_VLAN_REG(val->port_vlan, PORTMASK), mask);

	/* fix up PVIDs for removed ports, default to last vlan */
	oldmask &= ~mask;
	for (i = 0; i < RTL8306_NUM_PORTS; i++) {
		if (!(oldmask & (1 << i)))
			continue;

		if (i == dev->cpu_port)
			continue;

		if (rtl_get(dev, RTL_PORT_REG(i, PVID)) == val->port_vlan)
			rtl_set(dev, RTL_PORT_REG(i, PVID), dev->vlans - 1);
	}

	return 0;
}

static struct switch_attr rtl_globals[] = {
	{
		.type = SWITCH_TYPE_INT,
		.name = "reset",
		.description = "Reset the switch",
		.set = rtl_reset,
	},
	{
		.type = SWITCH_TYPE_INT,
		.name = "enable_vlan",
		.description = "Enable VLAN mode",
		.max = 1,
		.set = rtl_set_vlan,
		.get = rtl_get_vlan,
	},
	{
		RTL_GLOBAL_REGATTR(EN_TRUNK),
		.name = "trunk",
		.description = "Enable port trunking",
		.max = 1,
	},
	{
		RTL_GLOBAL_REGATTR(TRUNK_PORTSEL),
		.name = "trunk_sel",
		.description = "Select ports for trunking (0: 0,1 - 1: 3,4)",
		.max = 1,
	},
#ifdef DEBUG
	{
		RTL_GLOBAL_REGATTR(VLAN_FILTER),
		.name = "vlan_filter",
		.description = "Filter incoming packets for allowed VLANS",
		.max = 1,
	},
	{
		.type = SWITCH_TYPE_INT,
		.name = "cpuport",
		.description = "CPU Port",
		.set = rtl_set_cpuport,
		.get = rtl_get_cpuport,
		.max = RTL8306_NUM_PORTS,
	},
	{
		.type = SWITCH_TYPE_INT,
		.name = "use_cpuport",
		.description = "CPU Port handling flag",
		.set = rtl_set_use_cpuport,
		.get = rtl_get_use_cpuport,
		.max = RTL8306_NUM_PORTS,
	},
	{
		RTL_GLOBAL_REGATTR(TRAP_CPU),
		.name = "trap_cpu",
		.description = "VLAN trap to CPU",
		.max = 1,
	},
	{
		RTL_GLOBAL_REGATTR(VLAN_TAG_AWARE),
		.name = "vlan_tag_aware",
		.description = "Enable VLAN tag awareness",
		.max = 1,
	},
	{
		RTL_GLOBAL_REGATTR(VLAN_TAG_ONLY),
		.name = "tag_only",
		.description = "Only accept tagged packets",
		.max = 1,
	},
#endif
};
static struct switch_attr rtl_port[] = {
	{
		RTL_PORT_REGATTR(PVID),
		.name = "pvid",
		.description = "Port VLAN ID",
		.max = RTL8306_NUM_VLANS - 1,
	},
	{
		RTL_PORT_REGATTR(LINK),
		.name = "link",
		.description = "get the current link state",
		.max = 1,
		.set = NULL,
	},
#ifdef DEBUG
	{
		RTL_PORT_REGATTR(NULL_VID_REPLACE),
		.name = "null_vid",
		.description = "NULL VID gets replaced by port default vid",
		.max = 1,
	},
	{
		RTL_PORT_REGATTR(NON_PVID_DISCARD),
		.name = "non_pvid_discard",
		.description = "discard packets with VID != PVID",
		.max = 1,
	},
	{
		RTL_PORT_REGATTR(VID_INSERT),
		.name = "vid_insert_remove",
		.description = "how should the switch insert and remove vids ?",
		.max = 3,
	},
	{
		RTL_PORT_REGATTR(TAG_INSERT),
		.name = "tag_insert",
		.description = "tag insertion handling",
		.max = 3,
	},
#endif
	{
		RTL_PORT_REGATTR(SPEED),
		.name = "speed",
		.description = "current link speed",
		.max = 1,
	},
	{
		RTL_PORT_REGATTR(NWAY),
		.name = "nway",
		.description = "enable autonegotiation",
		.max = 1,
	},
};

static struct switch_attr rtl_vlan[] = {
	{
		RTL_VLAN_REGATTR(VID),
		.name = "vid",
		.description = "VLAN ID (1-4095)",
		.max = 4095,
	},
};

static const struct switch_dev_ops rtl8306_ops = {
	.attr_global = {
		.attr = rtl_globals,
		.n_attr = ARRAY_SIZE(rtl_globals),
	},
	.attr_port = {
		.attr = rtl_port,
		.n_attr = ARRAY_SIZE(rtl_port),
	},
	.attr_vlan = {
		.attr = rtl_vlan,
		.n_attr = ARRAY_SIZE(rtl_vlan),
	},

	.get_vlan_ports = rtl_get_ports,
	.set_vlan_ports = rtl_set_ports,
	.apply_config = rtl_hw_apply,
};

static int
rtl8306_config_init(struct phy_device *pdev)
{
	struct net_device *netdev = pdev->attached_dev;
	struct rtl_priv *priv = pdev->priv;
	struct switch_dev *dev = &priv->dev;
	struct switch_val val;
	unsigned int chipid, chipver, chiptype;
	int err;

	/* Only init the switch for the primary PHY */
	if (pdev->addr != 0)
		return 0;

	val.value.i = 1;
	priv->dev.cpu_port = RTL8306_PORT_CPU;
	priv->dev.ports = RTL8306_NUM_PORTS;
	priv->dev.vlans = RTL8306_NUM_VLANS;
	priv->dev.ops = &rtl8306_ops;
	priv->do_cpu = 0;
	priv->page = -1;
	priv->bus = pdev->bus;

	chipid = rtl_get(dev, RTL_REG_CHIPID);
	chipver = rtl_get(dev, RTL_REG_CHIPVER);
	chiptype = rtl_get(dev, RTL_REG_CHIPTYPE);
	switch(chiptype) {
	case 0:
	case 2:
		strncpy(priv->hwname, RTL_NAME_S, sizeof(priv->hwname));
		priv->type = RTL_TYPE_S;
		break;
	case 1:
		strncpy(priv->hwname, RTL_NAME_SD, sizeof(priv->hwname));
		priv->type = RTL_TYPE_SD;
		break;
	case 3:
		strncpy(priv->hwname, RTL_NAME_SDM, sizeof(priv->hwname));
		priv->type = RTL_TYPE_SDM;
		break;
	default:
		strncpy(priv->hwname, RTL_NAME_UNKNOWN, sizeof(priv->hwname));
		break;
	}

	dev->name = priv->hwname;
	rtl_hw_init(dev);

	printk(KERN_INFO "Registering %s switch with Chip ID: 0x%04x, version: 0x%04x\n", priv->hwname, chipid, chipver);

	err = register_switch(dev, netdev);
	if (err < 0) {
		kfree(priv);
		return err;
	}

	return 0;
}


static int
rtl8306_fixup(struct phy_device *pdev)
{
	struct rtl_priv priv;
	u16 chipid;

	/* Attach to primary LAN port and WAN port */
	if (pdev->addr != 0 && pdev->addr != 4)
		return 0;

	memset(&priv, 0, sizeof(priv));
	priv.fixup = true;
	priv.page = -1;
	priv.bus = pdev->bus;
	chipid = rtl_get(&priv.dev, RTL_REG_CHIPID);
	if (chipid == 0x5988)
		pdev->phy_id = RTL8306_MAGIC;

	return 0;
}

static int
rtl8306_probe(struct phy_device *pdev)
{
	struct rtl_priv *priv;

	list_for_each_entry(priv, &phydevs, list) {
		/*
		 * share one rtl_priv instance between virtual phy
		 * devices on the same bus
		 */
		if (priv->bus == pdev->bus)
			goto found;
	}
	priv = kzalloc(sizeof(struct rtl_priv), GFP_KERNEL);
	if (!priv)
		return -ENOMEM;

	priv->bus = pdev->bus;

found:
	pdev->priv = priv;
	return 0;
}

static void
rtl8306_remove(struct phy_device *pdev)
{
	struct rtl_priv *priv = pdev->priv;
	unregister_switch(&priv->dev);
	kfree(priv);
}

static int
rtl8306_config_aneg(struct phy_device *pdev)
{
	struct rtl_priv *priv = pdev->priv;

	/* Only for WAN */
	if (pdev->addr == 0)
		return 0;

	/* Restart autonegotiation */
	rtl_set(&priv->dev, RTL_PORT_REG(4, NWAY), 1);
	rtl_set(&priv->dev, RTL_PORT_REG(4, NRESTART), 1);

	return 0;
}

static int
rtl8306_read_status(struct phy_device *pdev)
{
	struct rtl_priv *priv = pdev->priv;
	struct switch_dev *dev = &priv->dev;

	if (pdev->addr == 4) {
		/* WAN */
		pdev->speed = rtl_get(dev, RTL_PORT_REG(4, SPEED)) ? SPEED_100 : SPEED_10;
		pdev->duplex = rtl_get(dev, RTL_PORT_REG(4, DUPLEX)) ? DUPLEX_FULL : DUPLEX_HALF;
		pdev->link = !!rtl_get(dev, RTL_PORT_REG(4, LINK));
	} else {
		/* LAN */
		pdev->speed = SPEED_100;
		pdev->duplex = DUPLEX_FULL;
		pdev->link = 1;
	}

	/*
	 * Bypass generic PHY status read,
	 * it doesn't work with this switch
	 */
	if (pdev->link) {
		pdev->state = PHY_RUNNING;
		netif_carrier_on(pdev->attached_dev);
		pdev->adjust_link(pdev->attached_dev);
	} else {
		pdev->state = PHY_NOLINK;
		netif_carrier_off(pdev->attached_dev);
		pdev->adjust_link(pdev->attached_dev);
	}

	return 0;
}


static struct phy_driver rtl8306_driver = {
	.name		= "Realtek RTL8306S",
	.flags		= PHY_HAS_MAGICANEG,
	.phy_id		= RTL8306_MAGIC,
	.phy_id_mask	= 0xffffffff,
	.features	= PHY_BASIC_FEATURES,
	.probe		= &rtl8306_probe,
	.remove		= &rtl8306_remove,
	.config_init	= &rtl8306_config_init,
	.config_aneg	= &rtl8306_config_aneg,
	.read_status	= &rtl8306_read_status,
	.driver		= { .owner = THIS_MODULE,},
};


static int __init
rtl_init(void)
{
	phy_register_fixup_for_id(PHY_ANY_ID, rtl8306_fixup);
	return phy_driver_register(&rtl8306_driver);
}

static void __exit
rtl_exit(void)
{
	phy_driver_unregister(&rtl8306_driver);
}

module_init(rtl_init);
module_exit(rtl_exit);
MODULE_LICENSE("GPL");