--- a/drivers/net/ethernet/lantiq_xrx200.c 2019-01-30 02:20:35.780993746 +0100 +++ b/drivers/net/ethernet/lantiq_xrx200.c 2019-02-08 12:59:08.067766387 +0100 @@ -36,16 +36,13 @@ #include "lantiq_pce.h" #include "lantiq_xrx200_sw.h" +#include //pc2005 time tests + #define SW_POLLING #define SW_ROUTING -#ifdef SW_ROUTING -#define XRX200_MAX_DEV 2 -#else -#define XRX200_MAX_DEV 1 -#endif - #define XRX200_MAX_VLAN 64 + #define XRX200_PCE_ACTVLAN_IDX 0x01 #define XRX200_PCE_VLANMAP_IDX 0x02 @@ -54,7 +51,7 @@ #define XRX200_HEADROOM 4 -#define XRX200_TX_TIMEOUT (10 * HZ) +#define XRX200_TX_TIMEOUT (30 * HZ) /* port type */ #define XRX200_PORT_TYPE_PHY 1 @@ -62,6 +59,8 @@ /* DMA */ #define XRX200_DMA_DATA_LEN 0x600 +#define XRX200_DMA_TX_ALIGN (32 - 1) + #define XRX200_DMA_IRQ INT_NUM_IM2_IRL0 #define XRX200_DMA_RX 0 #define XRX200_DMA_TX 1 @@ -69,6 +68,7 @@ #define XRX200_DMA_IS_TX(x) (x%2) #define XRX200_DMA_IS_RX(x) (!XRX200_DMA_IS_TX(x)) + /* fetch / store dma */ #define FDMA_PCTRL0 0x2A00 #define FDMA_PCTRLx(x) (FDMA_PCTRL0 + (x * 0x18)) @@ -205,48 +205,44 @@ struct xrx200_chan { int idx; int refcount; - int tx_free; - - struct net_device dummy_dev; - struct net_device *devs[XRX200_MAX_DEV]; + volatile int tx_free; struct tasklet_struct tasklet; struct napi_struct napi; struct ltq_dma_channel dma; struct sk_buff *skb[LTQ_DESC_NUM]; + struct xrx200_priv *priv; spinlock_t lock; }; -struct xrx200_hw { - struct clk *clk; - struct mii_bus *mii_bus; - - struct xrx200_chan chan[XRX200_MAX_DMA]; - u16 vlan_vid[XRX200_MAX_VLAN]; - u16 vlan_port_map[XRX200_MAX_VLAN]; - - struct net_device *devs[XRX200_MAX_DEV]; - int num_devs; - - int port_map[XRX200_MAX_PORT]; - unsigned short wan_map; - - struct switch_dev swdev; -}; - struct xrx200_priv { struct net_device_stats stats; int id; + struct clk *clk; + + struct xrx200_chan chan_tx; + struct xrx200_chan chan_rx; + struct net_device *net_dev; + struct device *dev; + struct xrx200_port port[XRX200_MAX_PORT]; int num_port; bool wan; bool sw; - unsigned short port_map; + unsigned short d_port_map; unsigned char mac[6]; - struct xrx200_hw *hw; + struct mii_bus *mii_bus; + + u16 vlan_vid[XRX200_MAX_VLAN]; + u16 vlan_port_map[XRX200_MAX_VLAN]; + + int port_map[XRX200_MAX_PORT]; + unsigned short wan_map; + + struct switch_dev swdev; }; static __iomem void *xrx200_switch_membase; @@ -470,14 +466,14 @@ } // swconfig interface -static void xrx200_hw_init(struct xrx200_hw *hw); +static void xrx200_hw_init(struct xrx200_priv *priv); // global static int xrx200sw_reset_switch(struct switch_dev *dev) { - struct xrx200_hw *hw = container_of(dev, struct xrx200_hw, swdev); + struct xrx200_priv *priv = container_of(dev, struct xrx200_priv, swdev); - xrx200_hw_init(hw); + xrx200_hw_init(priv); return 0; } @@ -523,7 +519,7 @@ static int xrx200sw_set_vlan_vid(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val) { - struct xrx200_hw *hw = container_of(dev, struct xrx200_hw, swdev); + struct xrx200_priv *priv = container_of(dev, struct xrx200_priv, swdev); int i; struct xrx200_pce_table_entry tev; struct xrx200_pce_table_entry tem; @@ -538,7 +534,7 @@ return -EINVAL; } - hw->vlan_vid[val->port_vlan] = val->value.i; + priv->vlan_vid[val->port_vlan] = val->value.i; tev.index = val->port_vlan; xrx200_pce_table_entry_read(&tev); @@ -571,7 +567,7 @@ static int xrx200sw_set_vlan_ports(struct switch_dev *dev, struct switch_val *val) { - struct xrx200_hw *hw = container_of(dev, struct xrx200_hw, swdev); + struct xrx200_priv *priv = container_of(dev, struct xrx200_priv, swdev); int i, portmap, tagmap, untagged; struct xrx200_pce_table_entry tem; @@ -624,7 +620,7 @@ ltq_switch_w32_mask(0, portmap, PCE_PMAP2); ltq_switch_w32_mask(0, portmap, PCE_PMAP3); - hw->vlan_port_map[val->port_vlan] = portmap; + priv->vlan_port_map[val->port_vlan] = portmap; xrx200sw_fixup_pvids(); @@ -722,8 +718,14 @@ link->duplex = xrx200sw_read_x(XRX200_MAC_PSTAT_FDUP, port); - link->rx_flow = !!(xrx200sw_read_x(XRX200_MAC_CTRL_0_FCON, port) && 0x0010); - link->tx_flow = !!(xrx200sw_read_x(XRX200_MAC_CTRL_0_FCON, port) && 0x0020); + +// TODO "&&" tohle je bug ne? +// link->rx_flow = !!(xrx200sw_read_x(XRX200_MAC_CTRL_0_FCON, port) && 0x0010); +// link->tx_flow = !!(xrx200sw_read_x(XRX200_MAC_CTRL_0_FCON, port) && 0x0020); + link->rx_flow = !!(xrx200sw_read_x(XRX200_MAC_CTRL_0_FCON, port) & 0x0010); + link->tx_flow = !!(xrx200sw_read_x(XRX200_MAC_CTRL_0_FCON, port) & 0x0020); + + link->aneg = !(xrx200sw_read_x(XRX200_MAC_CTRL_0_FCON, port)); link->speed = SWITCH_PORT_SPEED_10; @@ -834,19 +836,16 @@ // .get_port_stats = xrx200sw_get_port_stats, //TODO }; -static int xrx200sw_init(struct xrx200_hw *hw) +static int xrx200sw_init(struct xrx200_priv *priv) { - int netdev_num; - for (netdev_num = 0; netdev_num < hw->num_devs; netdev_num++) - { struct switch_dev *swdev; - struct net_device *dev = hw->devs[netdev_num]; - struct xrx200_priv *priv = netdev_priv(dev); - if (!priv->sw) - continue; + if (!priv->sw) { + pr_info("!!!! no switch\n"); + return -ENODEV; + } - swdev = &hw->swdev; + swdev = &priv->swdev; swdev->name = "Lantiq XRX200 Switch"; swdev->vlans = XRX200_MAX_VLAN; @@ -854,10 +853,26 @@ swdev->cpu_port = 6; swdev->ops = &xrx200sw_ops; - register_switch(swdev, dev); + register_switch(swdev, priv->net_dev); return 0; // enough switches +} + +/* drop all the packets from the DMA ring */ +static void xrx200_flush_dma(struct xrx200_chan *ch) +{ + int i; + + for (i = 0; i < LTQ_DESC_NUM; i++) { + struct ltq_dma_desc *desc = &ch->dma.desc_base[ch->dma.desc]; + + if ((desc->ctl & (LTQ_DMA_OWN | LTQ_DMA_C)) != LTQ_DMA_C) + break; + + desc->ctl = LTQ_DMA_OWN | LTQ_DMA_RX_OFFSET(NET_IP_ALIGN) | + XRX200_DMA_DATA_LEN; + ch->dma.desc++; + ch->dma.desc %= LTQ_DESC_NUM; } - return 0; } static int xrx200_open(struct net_device *dev) @@ -865,21 +880,29 @@ struct xrx200_priv *priv = netdev_priv(dev); int i; - for (i = 0; i < XRX200_MAX_DMA; i++) { - if (!priv->hw->chan[i].dma.irq) - continue; - spin_lock_bh(&priv->hw->chan[i].lock); - if (!priv->hw->chan[i].refcount) { - if (XRX200_DMA_IS_RX(i)) - napi_enable(&priv->hw->chan[i].napi); - ltq_dma_open(&priv->hw->chan[i].dma); - } - priv->hw->chan[i].refcount++; - spin_unlock_bh(&priv->hw->chan[i].lock); - } +// TODO DMA chan allocation seems to be more complex in openwrt version + + napi_enable(&priv->chan_tx.napi); + ltq_dma_open(&priv->chan_tx.dma); + ltq_dma_enable_irq(&priv->chan_tx.dma); + + napi_enable(&priv->chan_rx.napi); + ltq_dma_open(&priv->chan_rx.dma); + /* The boot loader does not always deactivate the receiving of frames + * on the ports and then some packets queue up in the PPE buffers. + * They already passed the PMAC so they do not have the tags + * configured here. Read the these packets here and drop them. + * The HW should have written them into memory after 10us + */ + usleep_range(20, 40); + xrx200_flush_dma(&priv->chan_rx); + + ltq_dma_enable_irq(&priv->chan_rx.dma); + for (i = 0; i < priv->num_port; i++) if (priv->port[i].phydev) phy_start(priv->port[i].phydev); + netif_wake_queue(dev); return 0; @@ -896,19 +919,11 @@ if (priv->port[i].phydev) phy_stop(priv->port[i].phydev); - for (i = 0; i < XRX200_MAX_DMA; i++) { - if (!priv->hw->chan[i].dma.irq) - continue; + napi_disable(&priv->chan_rx.napi); + ltq_dma_close(&priv->chan_rx.dma); - priv->hw->chan[i].refcount--; - if (!priv->hw->chan[i].refcount) { - if (XRX200_DMA_IS_RX(i)) - napi_disable(&priv->hw->chan[i].napi); - spin_lock_bh(&priv->hw->chan[i].lock); - ltq_dma_close(&priv->hw->chan[XRX200_DMA_RX].dma); - spin_unlock_bh(&priv->hw->chan[i].lock); - } - } + napi_disable(&priv->chan_tx.napi); + ltq_dma_close(&priv->chan_tx.dma); return 0; } @@ -916,12 +931,15 @@ static int xrx200_alloc_skb(struct xrx200_chan *ch) { #define DMA_PAD (NET_IP_ALIGN + NET_SKB_PAD) - ch->skb[ch->dma.desc] = dev_alloc_skb(XRX200_DMA_DATA_LEN + DMA_PAD); + ch->skb[ch->dma.desc] = netdev_alloc_skb(ch->priv->net_dev, XRX200_DMA_DATA_LEN + DMA_PAD); if (!ch->skb[ch->dma.desc]) goto skip; skb_reserve(ch->skb[ch->dma.desc], NET_SKB_PAD); - ch->dma.desc_base[ch->dma.desc].addr = dma_map_single(NULL, + + dma_unmap_single(ch->priv->dev, ch->dma.desc_base[ch->dma.desc].addr, XRX200_DMA_DATA_LEN, DMA_FROM_DEVICE); + + ch->dma.desc_base[ch->dma.desc].addr = dma_map_single(ch->priv->dev, ch->skb[ch->dma.desc]->data, XRX200_DMA_DATA_LEN, DMA_FROM_DEVICE); ch->dma.desc_base[ch->dma.desc].addr = @@ -938,8 +956,8 @@ static void xrx200_hw_receive(struct xrx200_chan *ch, int id) { - struct net_device *dev = ch->devs[id]; - struct xrx200_priv *priv = netdev_priv(dev); + struct xrx200_priv *priv = ch->priv; + struct net_device *dev = priv->net_dev; struct ltq_dma_desc *desc = &ch->dma.desc_base[ch->dma.desc]; struct sk_buff *skb = ch->skb[ch->dma.desc]; int len = (desc->ctl & LTQ_DMA_SIZE_MASK); @@ -960,18 +978,19 @@ #ifdef SW_ROUTING skb_pull(skb, 8); #endif + skb->dev = dev; skb->protocol = eth_type_trans(skb, dev); netif_receive_skb(skb); - priv->stats.rx_packets++; - priv->stats.rx_bytes+=len; + dev->stats.rx_packets++; + dev->stats.rx_bytes+=len; } static int xrx200_poll_rx(struct napi_struct *napi, int budget) { struct xrx200_chan *ch = container_of(napi, struct xrx200_chan, napi); - struct xrx200_priv *priv = netdev_priv(ch->devs[0]); + struct xrx200_priv *priv = ch->priv; int rx = 0; int complete = 0; @@ -982,7 +1001,7 @@ struct sk_buff *skb = ch->skb[ch->dma.desc]; u8 *special_tag = (u8*)skb->data; int port = (special_tag[7] >> SPPID_SHIFT) & SPPID_MASK; - xrx200_hw_receive(ch, priv->hw->port_map[port]); + xrx200_hw_receive(ch, priv->port_map[port]); #else xrx200_hw_receive(ch, 0); #endif @@ -993,95 +1012,130 @@ } if (complete || !rx) { - napi_complete(&ch->napi); + + if (napi_complete_done(&ch->napi,rx)) { +// ltq_dma_ack_irq(&ch->dma); ltq_dma_enable_irq(&ch->dma); + } } return rx; } -static void xrx200_tx_housekeeping(unsigned long ptr) + +static struct net_device_stats *xrx200_get_stats (struct net_device *dev) +{ + struct xrx200_priv *priv = netdev_priv(dev); + + return &priv->stats; +} + +static int xrx200_tx_housekeeping(struct napi_struct *napi, int budget) { - struct xrx200_chan *ch = (struct xrx200_chan *) ptr; + struct xrx200_chan *ch = container_of(napi, + struct xrx200_chan, napi); + struct net_device *net_dev = ch->priv->net_dev; int pkts = 0; - int i; + int bytes = 0; - spin_lock_bh(&ch->lock); - ltq_dma_ack_irq(&ch->dma); - while ((ch->dma.desc_base[ch->tx_free].ctl & (LTQ_DMA_OWN | LTQ_DMA_C)) == LTQ_DMA_C) { - struct sk_buff *skb = ch->skb[ch->tx_free]; + while (pkts < budget) { + struct ltq_dma_desc *desc = &ch->dma.desc_base[ch->tx_free]; - pkts++; - ch->skb[ch->tx_free] = NULL; - dev_kfree_skb(skb); - memset(&ch->dma.desc_base[ch->tx_free], 0, - sizeof(struct ltq_dma_desc)); - ch->tx_free++; - ch->tx_free %= LTQ_DESC_NUM; - } - ltq_dma_enable_irq(&ch->dma); - spin_unlock_bh(&ch->lock); + if ((desc->ctl & (LTQ_DMA_OWN | LTQ_DMA_C)) == LTQ_DMA_C) { + struct sk_buff *skb = ch->skb[ch->tx_free]; - if (!pkts) - return; + bytes += desc->ctl & LTQ_DMA_SIZE_MASK; + ch->skb[ch->tx_free] = NULL; - for (i = 0; i < XRX200_MAX_DEV && ch->devs[i]; i++) - netif_wake_queue(ch->devs[i]); -} +// TODO bad addr source (->addr ignores lowest 0x1f) +// TODO rewrite for frag indexes? (unstable??) + dma_unmap_single(ch->priv->dev, desc->addr & 0x1fffffff, desc->ctl & LTQ_DMA_SIZE_MASK , + DMA_TO_DEVICE); + + /* Consume skb only at last fragment */ + if (desc->ctl & LTQ_DMA_EOP) { + dev_consume_skb_irq(skb); + pkts++; + } -static struct net_device_stats *xrx200_get_stats (struct net_device *dev) -{ - struct xrx200_priv *priv = netdev_priv(dev); + memset(desc, 0, sizeof(struct ltq_dma_desc)); + ch->tx_free = (ch->tx_free + 1) % LTQ_DESC_NUM; + } else { + break; + } + } - return &priv->stats; + // TODO this is legacy + net_dev->stats.tx_packets += pkts; + net_dev->stats.tx_bytes += bytes; + + if (pkts < budget) { + if (napi_complete_done(&ch->napi, pkts)) { + ltq_dma_enable_irq(&ch->dma); + } + } + + if (netif_queue_stopped(net_dev)) { + // TODO hysteresis + if (ch->dma.desc != ch->tx_free) { + netif_wake_queue(net_dev); + } + } + + return pkts; } static void xrx200_tx_timeout(struct net_device *dev) { struct xrx200_priv *priv = netdev_priv(dev); - printk(KERN_ERR "%s: transmit timed out, disable the dma channel irq\n", dev->name); + pr_err("%s: transmit timed out!\n", dev->name); priv->stats.tx_errors++; - netif_wake_queue(dev); + + ltq_dma_enable_irq(&priv->chan_tx.dma); //TODO necessary? + + if (netif_queue_stopped(dev)) { + netif_wake_queue(dev); + } else { + pr_warn("%s: high transmit load\n", dev->name); + } } -static int xrx200_start_xmit(struct sk_buff *skb, struct net_device *dev) +static netdev_tx_t xrx200_start_xmit(struct sk_buff *skb, struct net_device *dev) { struct xrx200_priv *priv = netdev_priv(dev); - struct xrx200_chan *ch; + struct xrx200_chan *ch = &priv->chan_tx; + struct ltq_dma_desc *desc; - u32 byte_offset; int ret = NETDEV_TX_OK; int len; + int i; + dma_addr_t mapping; #ifdef SW_ROUTING u32 special_tag = (SPID_CPU_PORT << SPID_SHIFT) | DPID_ENABLE; #endif - if(priv->id) - ch = &priv->hw->chan[XRX200_DMA_TX_2]; - else - ch = &priv->hw->chan[XRX200_DMA_TX]; - desc = &ch->dma.desc_base[ch->dma.desc]; - - skb->dev = dev; - len = skb->len < ETH_ZLEN ? ETH_ZLEN : skb->len; + if (skb_put_padto(skb, ETH_ZLEN)) { + dev->stats.tx_dropped++; + return NETDEV_TX_OK; + } #ifdef SW_ROUTING if (is_multicast_ether_addr(eth_hdr(skb)->h_dest)) { - u16 port_map = priv->port_map; + u16 port_map = priv->d_port_map; if (priv->sw && skb->protocol == htons(ETH_P_8021Q)) { u16 vid; int i; - port_map = 0; + port_map = 0; if (!__vlan_get_tag(skb, &vid)) { for (i = 0; i < XRX200_MAX_VLAN; i++) { - if (priv->hw->vlan_vid[i] != vid) - continue; - port_map = priv->hw->vlan_port_map[i]; - break; + if (priv->vlan_vid[i] == vid) { + port_map = priv->vlan_port_map[i]; + break; + } } } } @@ -1089,108 +1143,193 @@ special_tag |= (port_map << PORT_MAP_SHIFT) | PORT_MAP_SEL | PORT_MAP_EN; } + if(priv->wan) special_tag |= (1 << DPID_SHIFT); + if(skb_headroom(skb) < 4) { struct sk_buff *tmp = skb_realloc_headroom(skb, 4); +pr_info("---slow realloc headroom?\n"); dev_kfree_skb_any(skb); skb = tmp; } + skb_push(skb, 4); memcpy(skb->data, &special_tag, sizeof(u32)); - len += 4; #endif - /* dma needs to start on a 16 byte aligned address */ - byte_offset = CPHYSADDR(skb->data) % 16; +// spin_lock_bh(&ch->lock); //only is multiple xmit queues + + if ((ch->tx_free != ch->dma.desc) && + (((((ch->tx_free - ch->dma.desc + LTQ_DESC_NUM) % LTQ_DESC_NUM)) + - skb_shinfo(skb)->nr_frags - 1) < 0 )) { + netdev_err(dev, "not enough TX ring space for frags\n"); + netif_stop_queue(dev); + ret = NETDEV_TX_BUSY; + goto out; + } - spin_lock_bh(&ch->lock); + desc = &ch->dma.desc_base[ch->dma.desc]; if ((desc->ctl & (LTQ_DMA_OWN | LTQ_DMA_C)) || ch->skb[ch->dma.desc]) { - netdev_err(dev, "tx ring full\n"); + netdev_err(dev, "tx ring full before send\n"); netif_stop_queue(dev); ret = NETDEV_TX_BUSY; goto out; } + + /* Send first fragment */ ch->skb[ch->dma.desc] = skb; + desc = &ch->dma.desc_base[ch->dma.desc]; + + if (skb_shinfo(skb)->nr_frags == 0) { + len = skb->len; + } else { + len = skb_headlen(skb); + } + + mapping = dma_map_single(priv->dev, skb->data, len, DMA_TO_DEVICE); + if (unlikely(dma_mapping_error(priv->dev, mapping))) { + netdev_err(dev, "DMA mapping failed\n"); + dev_kfree_skb(skb); + dev->stats.tx_dropped++; + dev->stats.tx_errors++; + ret = NETDEV_TX_OK; + goto out; + } + + desc->addr = (mapping & 0x1fffffe0) | (1<<31); + + /* Don't set OWN before filling all fragments descriptors */ + desc->ctl = LTQ_DMA_SOP | LTQ_DMA_TX_OFFSET(mapping & XRX200_DMA_TX_ALIGN) + | (len & LTQ_DMA_SIZE_MASK); + + if (!skb_shinfo(skb)->nr_frags) + desc->ctl |= LTQ_DMA_EOP; + + + /* Send rest of fragments */ + for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) { + + //TODO saving info for fragments + ch->skb[(ch->dma.desc + i + 1) % LTQ_DESC_NUM ] = skb; + desc = &ch->dma.desc_base[(ch->dma.desc + i + 1) % LTQ_DESC_NUM]; + + len = skb_frag_size(&skb_shinfo(skb)->frags[i]); + + mapping = dma_map_single(priv->dev, skb_frag_address(& skb_shinfo(skb)->frags[i]), len, DMA_TO_DEVICE); + if (unlikely(dma_mapping_error(priv->dev, mapping))) { + netdev_err(dev, "DMA mapping for fragment failed\n"); + dev_kfree_skb(skb); + dev->stats.tx_dropped++; + dev->stats.tx_errors++; + ret = NETDEV_TX_OK; + goto out; + } + + desc->addr = (mapping & 0x1fffffe0) | (1<<31); - netif_trans_update(dev); + desc->ctl = LTQ_DMA_OWN | + LTQ_DMA_TX_OFFSET(mapping & XRX200_DMA_TX_ALIGN) | (len & LTQ_DMA_SIZE_MASK); + + if (i == (skb_shinfo(skb)->nr_frags - 1)) + desc->ctl |= LTQ_DMA_EOP; + } + + desc = &ch->dma.desc_base[ch->dma.desc]; + + /* Increment TX ring index */ + ch->dma.desc = (ch->dma.desc + skb_shinfo(skb)->nr_frags + 1) % LTQ_DESC_NUM; - desc->addr = ((unsigned int) dma_map_single(NULL, skb->data, len, - DMA_TO_DEVICE)) - byte_offset; wmb(); - desc->ctl = LTQ_DMA_OWN | LTQ_DMA_SOP | LTQ_DMA_EOP | - LTQ_DMA_TX_OFFSET(byte_offset) | (len & LTQ_DMA_SIZE_MASK); - ch->dma.desc++; - ch->dma.desc %= LTQ_DESC_NUM; - if (ch->dma.desc == ch->tx_free) - netif_stop_queue(dev); + /* Start TX DMA */ + desc->ctl |= LTQ_DMA_OWN; + +// TODO stop queue sooner? +// if (ch->dma.desc == ch->tx_free) { - priv->stats.tx_packets++; - priv->stats.tx_bytes+=len; +if (((((ch->tx_free - ch->dma.desc + LTQ_DESC_NUM) % LTQ_DESC_NUM)) + - skb_shinfo(skb)->nr_frags - 1 - 32) < 0 ) { +pr_info("tx ring full after send\n"); + netif_stop_queue(dev); + } + + skb_tx_timestamp(skb); out: - spin_unlock_bh(&ch->lock); +// spin_unlock_bh(&ch->lock); return ret; } -static irqreturn_t xrx200_dma_irq(int irq, void *priv) +static irqreturn_t xrx200_dma_irq(int irq, void *ptr) { - struct xrx200_hw *hw = priv; - int chnr = irq - XRX200_DMA_IRQ; - struct xrx200_chan *ch = &hw->chan[chnr]; + struct xrx200_chan *ch = ptr; ltq_dma_disable_irq(&ch->dma); ltq_dma_ack_irq(&ch->dma); - - if (chnr % 2) - tasklet_schedule(&ch->tasklet); - else - napi_schedule(&ch->napi); + napi_schedule(&ch->napi); return IRQ_HANDLED; } -static int xrx200_dma_init(struct xrx200_hw *hw) +static int xrx200_dma_init(struct xrx200_priv *priv) { - int i, err = 0; + int i; + struct xrx200_chan *ch_rx = &priv->chan_rx; + struct xrx200_chan *ch_tx = &priv->chan_tx; + int ret; ltq_dma_init_port(DMA_PORT_ETOP); - for (i = 0; i < 8 && !err; i++) { - int irq = XRX200_DMA_IRQ + i; - struct xrx200_chan *ch = &hw->chan[i]; - - spin_lock_init(&ch->lock); - - ch->idx = ch->dma.nr = i; - - if (i == XRX200_DMA_TX) { - ltq_dma_alloc_tx(&ch->dma); - err = request_irq(irq, xrx200_dma_irq, 0, "vrx200_tx", hw); - } else if (i == XRX200_DMA_TX_2) { - ltq_dma_alloc_tx(&ch->dma); - err = request_irq(irq, xrx200_dma_irq, 0, "vrx200_tx_2", hw); - } else if (i == XRX200_DMA_RX) { - ltq_dma_alloc_rx(&ch->dma); - for (ch->dma.desc = 0; ch->dma.desc < LTQ_DESC_NUM; - ch->dma.desc++) - if (xrx200_alloc_skb(ch)) - err = -ENOMEM; - ch->dma.desc = 0; - err = request_irq(irq, xrx200_dma_irq, 0, "vrx200_rx", hw); - } else - continue; + ch_rx->dma.nr = XRX200_DMA_RX; + ch_rx->priv = priv; - if (!err) - ch->dma.irq = irq; - else - pr_err("net-xrx200: failed to request irq %d\n", irq); + ltq_dma_alloc_rx(&ch_rx->dma); + for (ch_rx->dma.desc = 0; ch_rx->dma.desc < LTQ_DESC_NUM; + ch_rx->dma.desc++) { + ret = xrx200_alloc_skb(ch_rx); + if (ret) + goto rx_free; } + ch_rx->dma.desc = 0; - return err; + ret = devm_request_irq(priv->dev, ch_rx->dma.irq, xrx200_dma_irq, 0, + "vrx200_rx", &priv->chan_rx); + if (ret) { + dev_err(priv->dev, "failed to request RX irq %d\n", + ch_rx->dma.irq); + goto rx_ring_free; + } + + ch_tx->dma.nr = XRX200_DMA_TX; + ch_tx->priv = priv; + + ltq_dma_alloc_tx(&ch_tx->dma); + ret = devm_request_irq(priv->dev, ch_tx->dma.irq, xrx200_dma_irq, 0, + "vrx200_tx", &priv->chan_tx); + if (ret) { + dev_err(priv->dev, "failed to request TX irq %d\n", + ch_tx->dma.irq); + goto tx_free; + } + + return ret; + +tx_free: + ltq_dma_free(&ch_tx->dma); + +rx_ring_free: + /* free the allocated RX ring */ + for (i = 0; i < LTQ_DESC_NUM; i++) { + if (priv->chan_rx.skb[i]) + dev_kfree_skb_any(priv->chan_rx.skb[i]); + } + +rx_free: + ltq_dma_free(&ch_rx->dma); + return ret; } #ifdef SW_POLLING @@ -1328,11 +1467,12 @@ { struct net_device *netdev = phydev->attached_dev; - if (do_carrier) + if (do_carrier) { if (up) netif_carrier_on(netdev); else if (!xrx200_phy_has_link(netdev)) netif_carrier_off(netdev); + } phydev->adjust_link(netdev); } @@ -1343,7 +1483,7 @@ struct phy_device *phydev = NULL; unsigned val; - phydev = mdiobus_get_phy(priv->hw->mii_bus, port->phy_addr); + phydev = mdiobus_get_phy(priv->mii_bus, port->phy_addr); if (!phydev) { netdev_err(dev, "no PHY found\n"); @@ -1376,10 +1516,10 @@ #ifdef SW_POLLING phy_read_status(phydev); - val = xrx200_mdio_rd(priv->hw->mii_bus, MDIO_DEVAD_NONE, MII_CTRL1000); + val = xrx200_mdio_rd(priv->mii_bus, MDIO_DEVAD_NONE, MII_CTRL1000); val |= ADVERTIZE_MPD; - xrx200_mdio_wr(priv->hw->mii_bus, MDIO_DEVAD_NONE, MII_CTRL1000, val); - xrx200_mdio_wr(priv->hw->mii_bus, 0, 0, 0x1040); + xrx200_mdio_wr(priv->mii_bus, MDIO_DEVAD_NONE, MII_CTRL1000, val); + xrx200_mdio_wr(priv->mii_bus, 0, 0, 0x1040); phy_start_aneg(phydev); #endif @@ -1522,12 +1662,12 @@ ltq_switch_w32_mask(0, BIT(3), PCE_GCTRL_REG(0)); } -static void xrx200_hw_init(struct xrx200_hw *hw) +static void xrx200_hw_init(struct xrx200_priv *priv) { int i; /* enable clock gate */ - clk_enable(hw->clk); + clk_enable(priv->clk); ltq_switch_w32(1, 0); mdelay(100); @@ -1595,49 +1735,45 @@ xrx200sw_write_x(1, XRX200_BM_QUEUE_GCTRL_GL_MOD, 0); for (i = 0; i < XRX200_MAX_VLAN; i++) - hw->vlan_vid[i] = i; + priv->vlan_vid[i] = i; } -static void xrx200_hw_cleanup(struct xrx200_hw *hw) +static void xrx200_hw_cleanup(struct xrx200_priv *priv) { int i; /* disable the switch */ ltq_mdio_w32_mask(MDIO_GLOB_ENABLE, 0, MDIO_GLOB); - /* free the channels and IRQs */ - for (i = 0; i < 2; i++) { - ltq_dma_free(&hw->chan[i].dma); - if (hw->chan[i].dma.irq) - free_irq(hw->chan[i].dma.irq, hw); - } + ltq_dma_free(&priv->chan_tx.dma); + ltq_dma_free(&priv->chan_rx.dma); /* free the allocated RX ring */ for (i = 0; i < LTQ_DESC_NUM; i++) - dev_kfree_skb_any(hw->chan[XRX200_DMA_RX].skb[i]); + dev_kfree_skb_any(priv->chan_rx.skb[i]); /* clear the mdio bus */ - mdiobus_unregister(hw->mii_bus); - mdiobus_free(hw->mii_bus); + mdiobus_unregister(priv->mii_bus); + mdiobus_free(priv->mii_bus); /* release the clock */ - clk_disable(hw->clk); - clk_put(hw->clk); + clk_disable(priv->clk); + clk_put(priv->clk); } -static int xrx200_of_mdio(struct xrx200_hw *hw, struct device_node *np) +static int xrx200_of_mdio(struct xrx200_priv *priv, struct device_node *np) { - hw->mii_bus = mdiobus_alloc(); - if (!hw->mii_bus) + priv->mii_bus = mdiobus_alloc(); + if (!priv->mii_bus) return -ENOMEM; - hw->mii_bus->read = xrx200_mdio_rd; - hw->mii_bus->write = xrx200_mdio_wr; - hw->mii_bus->name = "lantiq,xrx200-mdio"; - snprintf(hw->mii_bus->id, MII_BUS_ID_SIZE, "%x", 0); + priv->mii_bus->read = xrx200_mdio_rd; + priv->mii_bus->write = xrx200_mdio_wr; + priv->mii_bus->name = "lantiq,xrx200-mdio"; + snprintf(priv->mii_bus->id, MII_BUS_ID_SIZE, "%x", 0); - if (of_mdiobus_register(hw->mii_bus, np)) { - mdiobus_free(hw->mii_bus); + if (of_mdiobus_register(priv->mii_bus, np)) { + mdiobus_free(priv->mii_bus); return -ENXIO; } @@ -1655,6 +1791,7 @@ memset(p, 0, sizeof(struct xrx200_port)); p->phy_node = of_parse_phandle(port, "phy-handle", 0); addr = of_get_property(p->phy_node, "reg", NULL); + if (!addr) return; @@ -1665,6 +1802,7 @@ p->flags = XRX200_PORT_TYPE_MAC; else p->flags = XRX200_PORT_TYPE_PHY; + priv->num_port++; p->gpio = of_get_gpio_flags(port, 0, &p->gpio_flags); @@ -1677,12 +1815,12 @@ } /* is this port a wan port ? */ if (priv->wan) - priv->hw->wan_map |= BIT(p->num); + priv->wan_map |= BIT(p->num); - priv->port_map |= BIT(p->num); + priv->d_port_map |= BIT(p->num); /* store the port id in the hw struct so we can map ports -> devices */ - priv->hw->port_map[p->num] = priv->hw->num_devs; + priv->port_map[p->num] = 0; } static const struct net_device_ops xrx200_netdev_ops = { @@ -1696,29 +1834,21 @@ .ndo_tx_timeout = xrx200_tx_timeout, }; -static void xrx200_of_iface(struct xrx200_hw *hw, struct device_node *iface, struct device *dev) +static void xrx200_of_iface(struct xrx200_priv *priv, struct device_node *iface, struct device *dev) { - struct xrx200_priv *priv; struct device_node *port; const __be32 *wan; const u8 *mac; - /* alloc the network device */ - hw->devs[hw->num_devs] = alloc_etherdev(sizeof(struct xrx200_priv)); - if (!hw->devs[hw->num_devs]) - return; - /* setup the network device */ - strcpy(hw->devs[hw->num_devs]->name, "eth%d"); - hw->devs[hw->num_devs]->netdev_ops = &xrx200_netdev_ops; - hw->devs[hw->num_devs]->watchdog_timeo = XRX200_TX_TIMEOUT; - hw->devs[hw->num_devs]->needed_headroom = XRX200_HEADROOM; - SET_NETDEV_DEV(hw->devs[hw->num_devs], dev); + strcpy(priv->net_dev->name, "eth%d"); + priv->net_dev->netdev_ops = &xrx200_netdev_ops; + priv->net_dev->watchdog_timeo = XRX200_TX_TIMEOUT; + priv->net_dev->needed_headroom = XRX200_HEADROOM; + SET_NETDEV_DEV(priv->net_dev, dev); /* setup our private data */ - priv = netdev_priv(hw->devs[hw->num_devs]); - priv->hw = hw; - priv->id = hw->num_devs; + priv->id = 0; mac = of_get_mac_address(iface); if (mac) @@ -1738,20 +1868,34 @@ if (of_device_is_compatible(port, "lantiq,xrx200-pdi-port")) xrx200_of_port(priv, port); - /* register the actual device */ - if (!register_netdev(hw->devs[hw->num_devs])) - hw->num_devs++; } -static struct xrx200_hw xrx200_hw; - static int xrx200_probe(struct platform_device *pdev) { + struct device *dev = &pdev->dev; struct resource *res[4]; struct device_node *mdio_np, *iface_np, *phy_np; struct of_phandle_iterator it; int err; int i; + struct xrx200_priv *priv; + struct net_device *net_dev; + + + /* alloc the network device */ +// TODO add multiqueue? devm_alloc_etherdev_mqs + net_dev = devm_alloc_etherdev(dev, sizeof(struct xrx200_priv)); + if (!net_dev) + return -ENOMEM; + + priv = netdev_priv(net_dev); + priv->net_dev = net_dev; + priv->dev = dev; + + net_dev->netdev_ops = &xrx200_netdev_ops; + SET_NETDEV_DEV(net_dev, dev); + net_dev->min_mtu = ETH_ZLEN; + net_dev->max_mtu = XRX200_DMA_DATA_LEN; /* load the memory ranges */ for (i = 0; i < 4; i++) { @@ -1775,91 +1919,100 @@ phy_np = it.node; if (phy_np) { struct platform_device *phy = of_find_device_by_node(phy_np); - + of_node_put(phy_np); if (!platform_get_drvdata(phy)) return -EPROBE_DEFER; } } + priv->chan_rx.dma.irq = XRX200_DMA_IRQ + XRX200_DMA_RX; + priv->chan_tx.dma.irq = XRX200_DMA_IRQ + XRX200_DMA_TX; + priv->chan_rx.priv = priv; + priv->chan_tx.priv = priv; + /* get the clock */ - xrx200_hw.clk = clk_get(&pdev->dev, NULL); - if (IS_ERR(xrx200_hw.clk)) { + priv->clk = clk_get(&pdev->dev, NULL); + if (IS_ERR(priv->clk)) { dev_err(&pdev->dev, "failed to get clock\n"); - return PTR_ERR(xrx200_hw.clk); + return PTR_ERR(priv->clk); } /* bring up the dma engine and IP core */ - xrx200_dma_init(&xrx200_hw); - xrx200_hw_init(&xrx200_hw); - tasklet_init(&xrx200_hw.chan[XRX200_DMA_TX].tasklet, xrx200_tx_housekeeping, (u32) &xrx200_hw.chan[XRX200_DMA_TX]); - tasklet_init(&xrx200_hw.chan[XRX200_DMA_TX_2].tasklet, xrx200_tx_housekeeping, (u32) &xrx200_hw.chan[XRX200_DMA_TX_2]); + err = xrx200_dma_init(priv); + if (err) + return err; + + /* enable clock gate */ + err = clk_prepare_enable(priv->clk); + if (err) + goto err_uninit_dma; + + xrx200_hw_init(priv); /* bring up the mdio bus */ mdio_np = of_find_compatible_node(pdev->dev.of_node, NULL, "lantiq,xrx200-mdio"); if (mdio_np) - if (xrx200_of_mdio(&xrx200_hw, mdio_np)) + if (xrx200_of_mdio(priv, mdio_np)) dev_err(&pdev->dev, "mdio probe failed\n"); /* load the interfaces */ for_each_child_of_node(pdev->dev.of_node, iface_np) - if (of_device_is_compatible(iface_np, "lantiq,xrx200-pdi")) { - if (xrx200_hw.num_devs < XRX200_MAX_DEV) - xrx200_of_iface(&xrx200_hw, iface_np, &pdev->dev); - else - dev_err(&pdev->dev, - "only %d interfaces allowed\n", - XRX200_MAX_DEV); - } - - if (!xrx200_hw.num_devs) { - xrx200_hw_cleanup(&xrx200_hw); - dev_err(&pdev->dev, "failed to load interfaces\n"); - return -ENOENT; - } + if (of_device_is_compatible(iface_np, "lantiq,xrx200-pdi")) { + xrx200_of_iface(priv, iface_np, &pdev->dev); + break; //hack + } - xrx200sw_init(&xrx200_hw); + xrx200sw_init(priv); /* set wan port mask */ - ltq_pmac_w32(xrx200_hw.wan_map, PMAC_EWAN); - - for (i = 0; i < xrx200_hw.num_devs; i++) { - xrx200_hw.chan[XRX200_DMA_RX].devs[i] = xrx200_hw.devs[i]; - xrx200_hw.chan[XRX200_DMA_TX].devs[i] = xrx200_hw.devs[i]; - xrx200_hw.chan[XRX200_DMA_TX_2].devs[i] = xrx200_hw.devs[i]; - } + ltq_pmac_w32(priv->wan_map, PMAC_EWAN); /* setup NAPI */ - init_dummy_netdev(&xrx200_hw.chan[XRX200_DMA_RX].dummy_dev); - netif_napi_add(&xrx200_hw.chan[XRX200_DMA_RX].dummy_dev, - &xrx200_hw.chan[XRX200_DMA_RX].napi, xrx200_poll_rx, 32); + netif_napi_add(net_dev, &priv->chan_rx.napi, xrx200_poll_rx, 32); //32 + netif_tx_napi_add(net_dev, &priv->chan_tx.napi, xrx200_tx_housekeeping, 32); + + net_dev->features |= NETIF_F_SG ; + net_dev->hw_features |= NETIF_F_SG; + net_dev->vlan_features |= NETIF_F_SG; - platform_set_drvdata(pdev, &xrx200_hw); + platform_set_drvdata(pdev, priv); + + err = register_netdev(net_dev); + if (err) + goto err_unprepare_clk; return 0; + +err_unprepare_clk: + clk_disable_unprepare(priv->clk); + +err_uninit_dma: + xrx200_hw_cleanup(priv); + + return err; } static int xrx200_remove(struct platform_device *pdev) { - struct net_device *dev = platform_get_drvdata(pdev); - struct xrx200_priv *priv; - - if (!dev) - return 0; - priv = netdev_priv(dev); + struct xrx200_priv *priv = platform_get_drvdata(pdev); + struct net_device *net_dev = priv->net_dev; /* free stack related instances */ - netif_stop_queue(dev); - netif_napi_del(&xrx200_hw.chan[XRX200_DMA_RX].napi); - - /* shut down hardware */ - xrx200_hw_cleanup(&xrx200_hw); + netif_stop_queue(net_dev); + netif_napi_del(&priv->chan_tx.napi); + netif_napi_del(&priv->chan_rx.napi); /* remove the actual device */ - unregister_netdev(dev); - free_netdev(dev); + unregister_netdev(net_dev); + + /* release the clock */ + clk_disable_unprepare(priv->clk); + + /* shut down hardware */ + xrx200_hw_cleanup(priv); return 0; }