#include <linux/bitfield.h>
#include <linux/etherdevice.h>
#include <linux/log2.h>
#include <linux/list.h>
#include <linux/net.h>
#include <linux/slab.h>
#include "tag.h"
#define KS8995_NAME "ks8995"
#define KS8995_TAG_LEN VLAN_HLEN
#define KS8995_NEEDED_HEADROOM (KS8995_TAG_LEN + VLAN_HLEN)
#define KS8995M_STPID_STD GENMASK(15, 4)
#define KS8995M_STPID_PORTMASK GENMASK(3, 0)
#define KS8995M_STPID(portmask) htons(ETH_P_8021Q | FIELD_PREP(KS8995M_STPID_PORTMASK, portmask))
static struct sk_buff *ks8995_xmit(struct sk_buff *skb, struct net_device *dev)
{
struct vlan_ethhdr *hdr;
u16 portmask;
if (unlikely(!pskb_may_pull(skb, ETH_HLEN))) {
kfree_skb(skb);
return NULL;
}
portmask = dsa_xmit_port_mask(skb, dev);
if (unlikely(skb_vlan_tag_present(skb))) {
skb = __vlan_hwaccel_push_inside(skb);
if (!skb)
return NULL;
}
skb = vlan_insert_tag(skb, KS8995M_STPID(portmask), 0);
if (!skb)
return NULL;
hdr = skb_vlan_eth_hdr(skb);
netdev_dbg(dev, "%s: inserted VLAN TAG %04x TCI %04x\n",
__func__, ntohs(hdr->h_vlan_proto),
ntohs(hdr->h_vlan_TCI));
return skb;
}
static struct sk_buff *ks8995_rcv(struct sk_buff *skb, struct net_device *dev)
{
int portmask;
int port;
u16 etype;
if (skb_vlan_tag_present(skb))
etype = ntohs(skb->vlan_proto);
else
etype = ntohs(*(__be16 *)dsa_etype_header_pos_rx(skb));
if ((etype & KS8995M_STPID_STD) != ETH_P_8021Q) {
netdev_dbg(dev, "%s: dropped ethertype 0x%04x\n",
__func__, etype);
kfree_skb(skb);
return NULL;
}
portmask = FIELD_GET(KS8995M_STPID_PORTMASK, etype);
if (unlikely(!is_power_of_2(portmask))) {
netdev_dbg(dev, "%s: dropped invalid port mask 0x%04x\n",
__func__, portmask);
kfree_skb(skb);
return NULL;
}
port = ilog2(portmask);
netdev_dbg(dev, "%s: received ethertype %04x\n",
__func__, etype);
if (!skb_vlan_tag_present(skb)) {
skb = skb_vlan_untag(skb);
if (!skb) {
if (net_ratelimit())
netdev_err(dev, "%s: unable to untag skb\n", __func__);
return NULL;
}
}
netdev_dbg(dev, "%s: etype %04x portmask %04x (%d)\n",
__func__, etype, portmask, port);
skb->dev = dsa_conduit_find_user(dev, 0, port);
if (!skb->dev) {
kfree_skb(skb);
return NULL;
}
if (!skb->vlan_tci) {
netdev_dbg(dev, "%s: clear VLAN tag from frame\n", __func__);
__vlan_hwaccel_clear_tag(skb);
} else {
skb->vlan_proto = htons(ETH_P_8021Q);
netdev_dbg(dev, "%s: vlan_tci = 0x%04x VLAN frame\n",
__func__, skb->vlan_tci);
}
dsa_default_offload_fwd_mark(skb);
return skb;
}
static void ks8995_flow_dissect(const struct sk_buff *skb, __be16 *proto,
int *offset)
{
const __be16 *encap_proto;
__be16 buffer;
*proto = 0;
*offset = 0;
encap_proto = skb_header_pointer(skb, KS8995_TAG_LEN - sizeof(buffer),
sizeof(buffer), &buffer);
if (!encap_proto)
return;
*proto = *encap_proto;
*offset = KS8995_TAG_LEN;
}
static const struct dsa_device_ops ks8995_netdev_ops = {
.name = KS8995_NAME,
.proto = DSA_TAG_PROTO_KS8995,
.xmit = ks8995_xmit,
.rcv = ks8995_rcv,
.flow_dissect = ks8995_flow_dissect,
.needed_headroom = KS8995_NEEDED_HEADROOM,
};
MODULE_DESCRIPTION("DSA tag driver for Micrel KS8995 family of switches");
MODULE_LICENSE("GPL");
MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_KS8995, KS8995_NAME);
module_dsa_tag_driver