// SPDX-License-Identifier: GPL-2.0
|
/*
|
* net.c - Networking component for Mostcore
|
*
|
* Copyright (C) 2015, Microchip Technology Germany II GmbH & Co. KG
|
*/
|
|
#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
|
|
#include <linux/module.h>
|
#include <linux/netdevice.h>
|
#include <linux/etherdevice.h>
|
#include <linux/slab.h>
|
#include <linux/init.h>
|
#include <linux/list.h>
|
#include <linux/wait.h>
|
#include <linux/kobject.h>
|
#include <linux/most.h>
|
|
#define MEP_HDR_LEN 8
|
#define MDP_HDR_LEN 16
|
#define MAMAC_DATA_LEN (1024 - MDP_HDR_LEN)
|
|
#define PMHL 5
|
|
#define PMS_TELID_UNSEGM_MAMAC 0x0A
|
#define PMS_FIFONO_MDP 0x01
|
#define PMS_FIFONO_MEP 0x04
|
#define PMS_MSGTYPE_DATA 0x04
|
#define PMS_DEF_PRIO 0
|
#define MEP_DEF_RETRY 15
|
|
#define PMS_FIFONO_MASK 0x07
|
#define PMS_FIFONO_SHIFT 3
|
#define PMS_RETRY_SHIFT 4
|
#define PMS_TELID_MASK 0x0F
|
#define PMS_TELID_SHIFT 4
|
|
#define HB(value) ((u8)((u16)(value) >> 8))
|
#define LB(value) ((u8)(value))
|
|
#define EXTRACT_BIT_SET(bitset_name, value) \
|
(((value) >> bitset_name##_SHIFT) & bitset_name##_MASK)
|
|
#define PMS_IS_MEP(buf, len) \
|
((len) > MEP_HDR_LEN && \
|
EXTRACT_BIT_SET(PMS_FIFONO, (buf)[3]) == PMS_FIFONO_MEP)
|
|
static inline bool pms_is_mamac(char *buf, u32 len)
|
{
|
return (len > MDP_HDR_LEN &&
|
EXTRACT_BIT_SET(PMS_FIFONO, buf[3]) == PMS_FIFONO_MDP &&
|
EXTRACT_BIT_SET(PMS_TELID, buf[14]) == PMS_TELID_UNSEGM_MAMAC);
|
}
|
|
struct net_dev_channel {
|
bool linked;
|
int ch_id;
|
};
|
|
struct net_dev_context {
|
struct most_interface *iface;
|
bool is_mamac;
|
struct net_device *dev;
|
struct net_dev_channel rx;
|
struct net_dev_channel tx;
|
struct list_head list;
|
};
|
|
static struct list_head net_devices = LIST_HEAD_INIT(net_devices);
|
static struct mutex probe_disc_mt; /* ch->linked = true, most_nd_open */
|
static DEFINE_SPINLOCK(list_lock); /* list_head, ch->linked = false, dev_hold */
|
static struct most_component comp;
|
|
static int skb_to_mamac(const struct sk_buff *skb, struct mbo *mbo)
|
{
|
u8 *buff = mbo->virt_address;
|
static const u8 broadcast[] = { 0x03, 0xFF };
|
const u8 *dest_addr = skb->data + 4;
|
const u8 *eth_type = skb->data + 12;
|
unsigned int payload_len = skb->len - ETH_HLEN;
|
unsigned int mdp_len = payload_len + MDP_HDR_LEN;
|
|
if (mdp_len < skb->len) {
|
pr_err("drop: too large packet! (%u)\n", skb->len);
|
return -EINVAL;
|
}
|
|
if (mbo->buffer_length < mdp_len) {
|
pr_err("drop: too small buffer! (%d for %d)\n",
|
mbo->buffer_length, mdp_len);
|
return -EINVAL;
|
}
|
|
if (skb->len < ETH_HLEN) {
|
pr_err("drop: too small packet! (%d)\n", skb->len);
|
return -EINVAL;
|
}
|
|
if (dest_addr[0] == 0xFF && dest_addr[1] == 0xFF)
|
dest_addr = broadcast;
|
|
*buff++ = HB(mdp_len - 2);
|
*buff++ = LB(mdp_len - 2);
|
|
*buff++ = PMHL;
|
*buff++ = (PMS_FIFONO_MDP << PMS_FIFONO_SHIFT) | PMS_MSGTYPE_DATA;
|
*buff++ = PMS_DEF_PRIO;
|
*buff++ = dest_addr[0];
|
*buff++ = dest_addr[1];
|
*buff++ = 0x00;
|
|
*buff++ = HB(payload_len + 6);
|
*buff++ = LB(payload_len + 6);
|
|
/* end of FPH here */
|
|
*buff++ = eth_type[0];
|
*buff++ = eth_type[1];
|
*buff++ = 0;
|
*buff++ = 0;
|
|
*buff++ = PMS_TELID_UNSEGM_MAMAC << 4 | HB(payload_len);
|
*buff++ = LB(payload_len);
|
|
memcpy(buff, skb->data + ETH_HLEN, payload_len);
|
mbo->buffer_length = mdp_len;
|
return 0;
|
}
|
|
static int skb_to_mep(const struct sk_buff *skb, struct mbo *mbo)
|
{
|
u8 *buff = mbo->virt_address;
|
unsigned int mep_len = skb->len + MEP_HDR_LEN;
|
|
if (mep_len < skb->len) {
|
pr_err("drop: too large packet! (%u)\n", skb->len);
|
return -EINVAL;
|
}
|
|
if (mbo->buffer_length < mep_len) {
|
pr_err("drop: too small buffer! (%d for %d)\n",
|
mbo->buffer_length, mep_len);
|
return -EINVAL;
|
}
|
|
*buff++ = HB(mep_len - 2);
|
*buff++ = LB(mep_len - 2);
|
|
*buff++ = PMHL;
|
*buff++ = (PMS_FIFONO_MEP << PMS_FIFONO_SHIFT) | PMS_MSGTYPE_DATA;
|
*buff++ = (MEP_DEF_RETRY << PMS_RETRY_SHIFT) | PMS_DEF_PRIO;
|
*buff++ = 0;
|
*buff++ = 0;
|
*buff++ = 0;
|
|
memcpy(buff, skb->data, skb->len);
|
mbo->buffer_length = mep_len;
|
return 0;
|
}
|
|
static int most_nd_set_mac_address(struct net_device *dev, void *p)
|
{
|
struct net_dev_context *nd = netdev_priv(dev);
|
int err = eth_mac_addr(dev, p);
|
|
if (err)
|
return err;
|
|
nd->is_mamac =
|
(dev->dev_addr[0] == 0 && dev->dev_addr[1] == 0 &&
|
dev->dev_addr[2] == 0 && dev->dev_addr[3] == 0);
|
|
/*
|
* Set default MTU for the given packet type.
|
* It is still possible to change MTU using ip tools afterwards.
|
*/
|
dev->mtu = nd->is_mamac ? MAMAC_DATA_LEN : ETH_DATA_LEN;
|
|
return 0;
|
}
|
|
static void on_netinfo(struct most_interface *iface,
|
unsigned char link_stat, unsigned char *mac_addr);
|
|
static int most_nd_open(struct net_device *dev)
|
{
|
struct net_dev_context *nd = netdev_priv(dev);
|
int ret = 0;
|
|
mutex_lock(&probe_disc_mt);
|
|
if (most_start_channel(nd->iface, nd->rx.ch_id, &comp)) {
|
netdev_err(dev, "most_start_channel() failed\n");
|
ret = -EBUSY;
|
goto unlock;
|
}
|
|
if (most_start_channel(nd->iface, nd->tx.ch_id, &comp)) {
|
netdev_err(dev, "most_start_channel() failed\n");
|
most_stop_channel(nd->iface, nd->rx.ch_id, &comp);
|
ret = -EBUSY;
|
goto unlock;
|
}
|
|
netif_carrier_off(dev);
|
if (is_valid_ether_addr(dev->dev_addr))
|
netif_dormant_off(dev);
|
else
|
netif_dormant_on(dev);
|
netif_wake_queue(dev);
|
if (nd->iface->request_netinfo)
|
nd->iface->request_netinfo(nd->iface, nd->tx.ch_id, on_netinfo);
|
|
unlock:
|
mutex_unlock(&probe_disc_mt);
|
return ret;
|
}
|
|
static int most_nd_stop(struct net_device *dev)
|
{
|
struct net_dev_context *nd = netdev_priv(dev);
|
|
netif_stop_queue(dev);
|
if (nd->iface->request_netinfo)
|
nd->iface->request_netinfo(nd->iface, nd->tx.ch_id, NULL);
|
most_stop_channel(nd->iface, nd->rx.ch_id, &comp);
|
most_stop_channel(nd->iface, nd->tx.ch_id, &comp);
|
|
return 0;
|
}
|
|
static netdev_tx_t most_nd_start_xmit(struct sk_buff *skb,
|
struct net_device *dev)
|
{
|
struct net_dev_context *nd = netdev_priv(dev);
|
struct mbo *mbo;
|
int ret;
|
|
mbo = most_get_mbo(nd->iface, nd->tx.ch_id, &comp);
|
|
if (!mbo) {
|
netif_stop_queue(dev);
|
dev->stats.tx_fifo_errors++;
|
return NETDEV_TX_BUSY;
|
}
|
|
if (nd->is_mamac)
|
ret = skb_to_mamac(skb, mbo);
|
else
|
ret = skb_to_mep(skb, mbo);
|
|
if (ret) {
|
most_put_mbo(mbo);
|
dev->stats.tx_dropped++;
|
kfree_skb(skb);
|
return NETDEV_TX_OK;
|
}
|
|
most_submit_mbo(mbo);
|
dev->stats.tx_packets++;
|
dev->stats.tx_bytes += skb->len;
|
kfree_skb(skb);
|
return NETDEV_TX_OK;
|
}
|
|
static const struct net_device_ops most_nd_ops = {
|
.ndo_open = most_nd_open,
|
.ndo_stop = most_nd_stop,
|
.ndo_start_xmit = most_nd_start_xmit,
|
.ndo_set_mac_address = most_nd_set_mac_address,
|
};
|
|
static void most_nd_setup(struct net_device *dev)
|
{
|
ether_setup(dev);
|
dev->netdev_ops = &most_nd_ops;
|
}
|
|
static struct net_dev_context *get_net_dev(struct most_interface *iface)
|
{
|
struct net_dev_context *nd;
|
|
list_for_each_entry(nd, &net_devices, list)
|
if (nd->iface == iface)
|
return nd;
|
return NULL;
|
}
|
|
static struct net_dev_context *get_net_dev_hold(struct most_interface *iface)
|
{
|
struct net_dev_context *nd;
|
unsigned long flags;
|
|
spin_lock_irqsave(&list_lock, flags);
|
nd = get_net_dev(iface);
|
if (nd && nd->rx.linked && nd->tx.linked)
|
dev_hold(nd->dev);
|
else
|
nd = NULL;
|
spin_unlock_irqrestore(&list_lock, flags);
|
return nd;
|
}
|
|
static int comp_probe_channel(struct most_interface *iface, int channel_idx,
|
struct most_channel_config *ccfg, char *name,
|
char *args)
|
{
|
struct net_dev_context *nd;
|
struct net_dev_channel *ch;
|
struct net_device *dev;
|
unsigned long flags;
|
int ret = 0;
|
|
if (!iface)
|
return -EINVAL;
|
|
if (ccfg->data_type != MOST_CH_ASYNC)
|
return -EINVAL;
|
|
mutex_lock(&probe_disc_mt);
|
nd = get_net_dev(iface);
|
if (!nd) {
|
dev = alloc_netdev(sizeof(struct net_dev_context), "meth%d",
|
NET_NAME_UNKNOWN, most_nd_setup);
|
if (!dev) {
|
ret = -ENOMEM;
|
goto unlock;
|
}
|
|
nd = netdev_priv(dev);
|
nd->iface = iface;
|
nd->dev = dev;
|
|
spin_lock_irqsave(&list_lock, flags);
|
list_add(&nd->list, &net_devices);
|
spin_unlock_irqrestore(&list_lock, flags);
|
|
ch = ccfg->direction == MOST_CH_TX ? &nd->tx : &nd->rx;
|
} else {
|
ch = ccfg->direction == MOST_CH_TX ? &nd->tx : &nd->rx;
|
if (ch->linked) {
|
pr_err("direction is allocated\n");
|
ret = -EINVAL;
|
goto unlock;
|
}
|
|
if (register_netdev(nd->dev)) {
|
pr_err("register_netdev() failed\n");
|
ret = -EINVAL;
|
goto unlock;
|
}
|
}
|
ch->ch_id = channel_idx;
|
ch->linked = true;
|
|
unlock:
|
mutex_unlock(&probe_disc_mt);
|
return ret;
|
}
|
|
static int comp_disconnect_channel(struct most_interface *iface,
|
int channel_idx)
|
{
|
struct net_dev_context *nd;
|
struct net_dev_channel *ch;
|
unsigned long flags;
|
int ret = 0;
|
|
mutex_lock(&probe_disc_mt);
|
nd = get_net_dev(iface);
|
if (!nd) {
|
ret = -EINVAL;
|
goto unlock;
|
}
|
|
if (nd->rx.linked && channel_idx == nd->rx.ch_id) {
|
ch = &nd->rx;
|
} else if (nd->tx.linked && channel_idx == nd->tx.ch_id) {
|
ch = &nd->tx;
|
} else {
|
ret = -EINVAL;
|
goto unlock;
|
}
|
|
if (nd->rx.linked && nd->tx.linked) {
|
spin_lock_irqsave(&list_lock, flags);
|
ch->linked = false;
|
spin_unlock_irqrestore(&list_lock, flags);
|
|
/*
|
* do not call most_stop_channel() here, because channels are
|
* going to be closed in ndo_stop() after unregister_netdev()
|
*/
|
unregister_netdev(nd->dev);
|
} else {
|
spin_lock_irqsave(&list_lock, flags);
|
list_del(&nd->list);
|
spin_unlock_irqrestore(&list_lock, flags);
|
|
free_netdev(nd->dev);
|
}
|
|
unlock:
|
mutex_unlock(&probe_disc_mt);
|
return ret;
|
}
|
|
static int comp_resume_tx_channel(struct most_interface *iface,
|
int channel_idx)
|
{
|
struct net_dev_context *nd;
|
|
nd = get_net_dev_hold(iface);
|
if (!nd)
|
return 0;
|
|
if (nd->tx.ch_id != channel_idx)
|
goto put_nd;
|
|
netif_wake_queue(nd->dev);
|
|
put_nd:
|
dev_put(nd->dev);
|
return 0;
|
}
|
|
static int comp_rx_data(struct mbo *mbo)
|
{
|
const u32 zero = 0;
|
struct net_dev_context *nd;
|
char *buf = mbo->virt_address;
|
u32 len = mbo->processed_length;
|
struct sk_buff *skb;
|
struct net_device *dev;
|
unsigned int skb_len;
|
int ret = 0;
|
|
nd = get_net_dev_hold(mbo->ifp);
|
if (!nd)
|
return -EIO;
|
|
if (nd->rx.ch_id != mbo->hdm_channel_id) {
|
ret = -EIO;
|
goto put_nd;
|
}
|
|
dev = nd->dev;
|
|
if (nd->is_mamac) {
|
if (!pms_is_mamac(buf, len)) {
|
ret = -EIO;
|
goto put_nd;
|
}
|
|
skb = dev_alloc_skb(len - MDP_HDR_LEN + 2 * ETH_ALEN + 2);
|
} else {
|
if (!PMS_IS_MEP(buf, len)) {
|
ret = -EIO;
|
goto put_nd;
|
}
|
|
skb = dev_alloc_skb(len - MEP_HDR_LEN);
|
}
|
|
if (!skb) {
|
dev->stats.rx_dropped++;
|
pr_err_once("drop packet: no memory for skb\n");
|
goto out;
|
}
|
|
skb->dev = dev;
|
|
if (nd->is_mamac) {
|
/* dest */
|
ether_addr_copy(skb_put(skb, ETH_ALEN), dev->dev_addr);
|
|
/* src */
|
skb_put_data(skb, &zero, 4);
|
skb_put_data(skb, buf + 5, 2);
|
|
/* eth type */
|
skb_put_data(skb, buf + 10, 2);
|
|
buf += MDP_HDR_LEN;
|
len -= MDP_HDR_LEN;
|
} else {
|
buf += MEP_HDR_LEN;
|
len -= MEP_HDR_LEN;
|
}
|
|
skb_put_data(skb, buf, len);
|
skb->protocol = eth_type_trans(skb, dev);
|
skb_len = skb->len;
|
if (netif_rx(skb) == NET_RX_SUCCESS) {
|
dev->stats.rx_packets++;
|
dev->stats.rx_bytes += skb_len;
|
} else {
|
dev->stats.rx_dropped++;
|
}
|
|
out:
|
most_put_mbo(mbo);
|
|
put_nd:
|
dev_put(nd->dev);
|
return ret;
|
}
|
|
static struct most_component comp = {
|
.mod = THIS_MODULE,
|
.name = "net",
|
.probe_channel = comp_probe_channel,
|
.disconnect_channel = comp_disconnect_channel,
|
.tx_completion = comp_resume_tx_channel,
|
.rx_completion = comp_rx_data,
|
};
|
|
static int __init most_net_init(void)
|
{
|
int err;
|
|
mutex_init(&probe_disc_mt);
|
err = most_register_component(&comp);
|
if (err)
|
return err;
|
err = most_register_configfs_subsys(&comp);
|
if (err) {
|
most_deregister_component(&comp);
|
return err;
|
}
|
return 0;
|
}
|
|
static void __exit most_net_exit(void)
|
{
|
most_deregister_configfs_subsys(&comp);
|
most_deregister_component(&comp);
|
}
|
|
/**
|
* on_netinfo - callback for HDM to be informed about HW's MAC
|
* @param iface - most interface instance
|
* @param link_stat - link status
|
* @param mac_addr - MAC address
|
*/
|
static void on_netinfo(struct most_interface *iface,
|
unsigned char link_stat, unsigned char *mac_addr)
|
{
|
struct net_dev_context *nd;
|
struct net_device *dev;
|
const u8 *m = mac_addr;
|
|
nd = get_net_dev_hold(iface);
|
if (!nd)
|
return;
|
|
dev = nd->dev;
|
|
if (link_stat)
|
netif_carrier_on(dev);
|
else
|
netif_carrier_off(dev);
|
|
if (m && is_valid_ether_addr(m)) {
|
if (!is_valid_ether_addr(dev->dev_addr)) {
|
netdev_info(dev, "set mac %pM\n", m);
|
ether_addr_copy(dev->dev_addr, m);
|
netif_dormant_off(dev);
|
} else if (!ether_addr_equal(dev->dev_addr, m)) {
|
netdev_warn(dev, "reject mac %pM\n", m);
|
}
|
}
|
|
dev_put(nd->dev);
|
}
|
|
module_init(most_net_init);
|
module_exit(most_net_exit);
|
MODULE_LICENSE("GPL");
|
MODULE_AUTHOR("Andrey Shvetsov <andrey.shvetsov@k2l.de>");
|
MODULE_DESCRIPTION("Networking Component Module for Mostcore");
|
