zsonet_driver.c

#include <linux/if_ether.h>
#include <linux/if_link.h>
#include <linux/module.h>
#include <linux/pci.h>
#include <linux/netdevice.h>
#include <linux/etherdevice.h>
#include <linux/dma-mapping.h>
    
#include "zsonet_driver.h"
#include "zsonet.h"

MODULE_LICENSE("GPL");

struct zsonet_data {
    void __iomem *bar;
    struct net_device *netdev;
};

struct zsonet_netdev_data {
    struct pci_dev *pdev;
    void __iomem *bar;
    struct rtnl_link_stats64 stats;

    spinlock_t dev_lock;
    
    spinlock_t tx_lock;
    u8 **tx_buf;
    dma_addr_t tx_dma_addr[4];
    /// The index of the back of the tx hardware queue. 
    int tx_qback;

    u8 *rx_buf;
    size_t rx_buf_sz;
    size_t rx_buf_off;
    dma_addr_t rx_buf_addr;
};


inline static void zsonet_iowrite32(u32 value, void __iomem *addr, struct zsonet_netdev_data *priv)
{
    iowrite32(value, addr);
}


/* Net device */

/// Cannot be called from within an interrupt context!
/// Assumes that the device sends packets synchronously from each buffer.
static int _reserve_tx_slot(struct zsonet_netdev_data *d)
{
    void __iomem *reg = d->bar;
    int status, slot;

    switch (d->tx_qback) {
    case 0:
        reg += ZSONET_REG_TX_STATUS_1;
        break;
    case 1:
        reg += ZSONET_REG_TX_STATUS_2;
        break;
    case 2:
        reg += ZSONET_REG_TX_STATUS_3;
        break;
    case 3:
        reg += ZSONET_REG_TX_STATUS_0;
        break;
    }

    status = ioread32(reg);
    //printk("register from slot %d: %d (status: %d len: %d)", d->bid, status, ZSONET_TX_STATUS(status), ZSONET_TX_SIZE(status));
    slot = -1;
    if ((ZSONET_TX_STATUS(status) & ZSONET_TX_STATUS_TX_FINISHED) || status == 0) {
        slot = d->tx_qback;
        d->tx_qback = (d->tx_qback + 1) % ZSONET_TX_BUF_CNT;
    }

    return slot;
}

// TODO: dodać ewentualnie ten padding
static int _cpy_to_buffer(void *buf, struct sk_buff *skb)
{
    u16 len = skb->len & 0xffff;
    BUG_ON(skb->len > ZSONET_TX_BUF_SIZE);

    skb_copy_and_csum_dev(skb, buf);
    
    return len;
}

inline static void _fire_transfer(int slot, dma_addr_t dma_addr, int len, struct zsonet_netdev_data *d)
{
    u32 tx_reg = len << 16;
    void __iomem *reg = d->bar;
    void __iomem *buf_reg = d->bar;
    
    switch (slot) {
    case 0:
        reg += ZSONET_REG_TX_STATUS_0;
        buf_reg += ZSONET_REG_TX_BUF_0;
        break;
    case 1:
        reg += ZSONET_REG_TX_STATUS_1;
        buf_reg += ZSONET_REG_TX_BUF_1;
        break;
    case 2:
        reg += ZSONET_REG_TX_STATUS_2;
        buf_reg += ZSONET_REG_TX_BUF_2;
        break;
    case 3:
        reg += ZSONET_REG_TX_STATUS_3;
        buf_reg += ZSONET_REG_TX_BUF_3;
        break;
    }

    zsonet_iowrite32(dma_addr, buf_reg, d);
    zsonet_iowrite32(tx_reg, reg, d);
}

static int _enqueue_skb(struct sk_buff *skb, struct net_device *dev)
{
    struct zsonet_netdev_data *priv = netdev_priv(dev);
    int slot;
    int len;
    int ret = NETDEV_TX_OK;
    unsigned long flags;

    spin_lock_irqsave(&priv->tx_lock, flags);
    slot = _reserve_tx_slot(priv);
    if (slot < 0) {
        if (net_ratelimit())
            printk("queue full - packet dropped");
        ret = NETDEV_TX_BUSY;
        goto out;
    }

    len = _cpy_to_buffer(priv->tx_buf[slot], skb);
    _fire_transfer(slot, priv->tx_dma_addr[slot], len, priv);

out:
    spin_unlock_irqrestore(&priv->tx_lock, flags);
    return ret;
}

static inline void _buf_copy_wrapped(void *dest, size_t len, void *buf, size_t buf_sz, size_t offset)
{
    if (offset + len > buf_sz) {
        int wrapped_sz = (offset + len) % buf_sz;
        memcpy(dest, buf + offset, len - wrapped_sz);
        memcpy((u8*)dest + len - wrapped_sz, buf, wrapped_sz);
    } else {
        memcpy(dest, (u8*)buf + offset, len);
    }
}

static inline void _skb_copy_wrapped(struct sk_buff *skb, size_t len, u8 *buf, size_t buf_sz, size_t offset)
{
    if (offset + len > buf_sz) {
        int wrapped_sz = (offset + len) % buf_sz;
        skb_copy_to_linear_data(skb, (u8*) buf + offset, len - wrapped_sz);
        skb_copy_to_linear_data_offset(skb, len - wrapped_sz, buf, wrapped_sz);
    } else {
        skb_copy_to_linear_data(skb, (u8*)buf + offset, len);
    }
}

static void _zsonet_isr_handle_rx(struct net_device *netdev)
{
    struct zsonet_netdev_data *priv = netdev_priv(netdev);
    struct sk_buff *skb;
    int total_bytes = 0;
    u32 packet_size, crc;

    zsonet_iowrite32(ZSONET_INTR_RX_OK, priv->bar + ZSONET_REG_INTR_STATUS, priv);

    while (total_bytes < priv->rx_buf_sz
            && (ioread32(priv->bar + ZSONET_REG_RX_STATUS) & ZSONET_RX_STATUS_RX_HAS_DATA)) {
        _buf_copy_wrapped(&packet_size, 4, priv->rx_buf, priv->rx_buf_sz, priv->rx_buf_off);
        _buf_copy_wrapped(&crc, 4, priv->rx_buf, priv->rx_buf_sz, (priv->rx_buf_off + packet_size) % priv->rx_buf_sz);
        //printk("packet size %d (conv %d) offset %lu crc %u (conv %u)", packet_size, le32_to_cpu(packet_size), priv->rx_buf_off, crc, le32_to_cpu(crc));

        skb = netdev_alloc_skb(netdev, packet_size);
        if (unlikely(!skb)) {
            if (net_ratelimit())
                printk("rx isr out of memory (size: %d)", packet_size);
            priv->stats.rx_dropped++;
            break;
        }

        skb_put(skb, packet_size);
        _skb_copy_wrapped(skb, packet_size, priv->rx_buf, priv->rx_buf_sz, (priv->rx_buf_off + 4) % priv->rx_buf_sz);

        skb->protocol = eth_type_trans(skb, netdev);
        netif_rx(skb);
        //printk("netif status: %d (SUCCESS: %d DROP: %d)", res, NET_RX_SUCCESS, NET_RX_DROP);

        total_bytes += packet_size;
        priv->stats.rx_bytes += skb->len;
        priv->stats.rx_packets++;
        priv->rx_buf_off = (priv->rx_buf_off + packet_size + 4) % priv->rx_buf_sz;
        zsonet_iowrite32(priv->rx_buf_off, priv->bar + ZSONET_REG_RX_BUF_READ_OFFSET, priv);
    }
}

static irqreturn_t zsonet_xmit_isr(int irq, void *dev)
{
    struct net_device *netdev = dev;
    struct zsonet_netdev_data *priv = netdev_priv(netdev);
    int status = ioread32(priv->bar + ZSONET_REG_INTR_STATUS);

    //printk("received interrupt! irq %d status %d", irq, status);
    if (!status)
        return IRQ_NONE;

    if (status & ZSONET_INTR_RX_OK) {
        //printk("interrupt because of rx ok");
        _zsonet_isr_handle_rx(netdev);
    }

    if (status & ZSONET_INTR_TX_OK) {
        //printk("Interrupt because of tx ok");
        netif_wake_queue(netdev);
        zsonet_iowrite32(ZSONET_INTR_TX_OK, priv->bar + ZSONET_REG_INTR_STATUS, priv);
    }

    return IRQ_HANDLED;
}

inline static void _zsonet_read_and_set_mac(struct net_device *netdev, void __iomem *bar)
{
    u8 addr[6];
    addr[0] = ioread8(bar + ZSONET_REG_MAC_0);
    addr[1] = ioread8(bar + ZSONET_REG_MAC_1);
    addr[2] = ioread8(bar + ZSONET_REG_MAC_2);
    addr[3] = ioread8(bar + ZSONET_REG_MAC_3);
    addr[4] = ioread8(bar + ZSONET_REG_MAC_4);
    addr[5] = ioread8(bar + ZSONET_REG_MAC_5);

    eth_hw_addr_set(netdev, (void*)&addr);
}

static inline void zsonet_reset(struct net_device *dev)
{
    struct zsonet_netdev_data *priv = netdev_priv(dev);

    priv->rx_buf_off = 0;
    priv->tx_qback = 0;
    memset(&priv->stats, 0, sizeof(priv->stats));
    
    zsonet_iowrite32(priv->rx_buf_addr, priv->bar + ZSONET_REG_RX_BUF, priv);
    zsonet_iowrite32(priv->rx_buf_sz, priv->bar + ZSONET_REG_RX_BUF_SIZE, priv);
    zsonet_iowrite32(ZSONET_INTR_RX_OK | ZSONET_INTR_TX_OK, priv->bar + ZSONET_REG_INTR_MASK, priv);

    _zsonet_read_and_set_mac(dev, priv->bar);
}

static int zsonet_ndo_open(struct net_device *dev)
{
    // printk("ndo_open");
    struct zsonet_netdev_data *priv = netdev_priv(dev);
    unsigned long flags;
    int err;
    
    spin_lock_irqsave(&priv->dev_lock, flags);

    if ((err = request_irq(priv->pdev->irq, &zsonet_xmit_isr, IRQF_SHARED, ZSONET_DRV_NAME, dev) != 0))
        return err;

    zsonet_iowrite32(ZSONET_ENABLED, priv->bar + ZSONET_REG_ENABLED, priv);
    netif_start_queue(dev);

    spin_unlock_irqrestore(&priv->dev_lock, flags);
    return 0;
}

static int zsonet_ndo_stop(struct net_device *dev)
{
    // printk("ndo_stop");
    struct zsonet_netdev_data *priv = netdev_priv(dev);
    unsigned long flags;
    
    spin_lock_irqsave(&priv->dev_lock, flags);

    netif_stop_queue(dev);
    zsonet_iowrite32(ZSONET_DISABLED, priv->bar + ZSONET_REG_ENABLED, priv);
    zsonet_reset(dev);
    free_irq(priv->pdev->irq, dev);

    spin_unlock_irqrestore(&priv->dev_lock, flags);
    return 0;
}

static void zsonet_ndo_get_stats64(struct net_device *dev,
	struct rtnl_link_stats64 *storage)
{
    struct zsonet_netdev_data *priv = netdev_priv(dev);

    priv->stats.rx_missed_errors += ioread32(priv->bar + ZSONET_REG_RX_MISSED);
    memcpy(storage, &priv->stats, sizeof(priv->stats));
}

static netdev_tx_t zsonet_ndo_start_xmit(struct sk_buff *skb,
    struct net_device *dev)
{
    struct zsonet_netdev_data *priv = netdev_priv(dev);
    int res;
    unsigned long flags;

    spin_lock_irqsave(&priv->dev_lock, flags);
    res = _enqueue_skb(skb, dev);
    //printk("ndo_start_xmit res %d", res);
    if (res != NETDEV_TX_OK) {
        priv->stats.tx_dropped++;
        dev_kfree_skb_any(skb);
        netif_stop_queue(dev);
    } else {
        priv->stats.tx_packets++;
        priv->stats.tx_bytes += skb->len;
        dev_consume_skb_any(skb);
    }

    spin_unlock_irqrestore(&priv->dev_lock, flags);
    
    return NETDEV_TX_OK;
}

static const struct net_device_ops zsonet_netdev_ops = {
    .ndo_open = zsonet_ndo_open,
    .ndo_stop = zsonet_ndo_stop,
    .ndo_start_xmit = zsonet_ndo_start_xmit,
    .ndo_get_stats64 = zsonet_ndo_get_stats64,
};

inline static void _zsonet_free_netdev_buffers(u8 **buf)
{
    for (int i = 0; i < ZSONET_TX_BUF_CNT; i++)
        kfree(buf[i]);
    kfree(buf);
}

static void _zsonet_free_netdev(struct net_device *netdev)
{
    struct zsonet_netdev_data *dnet = netdev_priv(netdev);

    u8 **tx_buf = dnet->tx_buf;
    // frees also the priv field!
    free_netdev(netdev);
    _zsonet_free_netdev_buffers(tx_buf);
}

/* PCI driver.  */
static int _zsonet_init_netdev_data(struct pci_dev *pdev, struct zsonet_data *dd,
    struct zsonet_netdev_data *dnet)
{
    dnet->bar = dd->bar;
    dnet->pdev = pdev;
    dnet->tx_qback = 0;
    spin_lock_init(&dnet->tx_lock);
    spin_lock_init(&dnet->dev_lock);
    dnet->rx_buf = NULL;
    dnet->rx_buf_sz = ZSONET_RX_BUF_SIZE;
    dnet->rx_buf_off = 0;

    memset(&dnet->stats, 0, sizeof(dnet->stats));

    dnet->tx_buf = kzalloc(ZSONET_TX_BUF_CNT * sizeof(u8 *), GFP_KERNEL);
    if (!dnet->tx_buf)
        return -ENOMEM;

    dnet->rx_buf = kzalloc(dnet->rx_buf_sz * sizeof(u8), GFP_KERNEL);
    if (!dnet->rx_buf)
        return -ENOMEM;

    for (int i = 0; i < ZSONET_TX_BUF_CNT; i++) {
        dnet->tx_buf[i] = kzalloc(sizeof(u8) * ZSONET_TX_BUF_SIZE, GFP_KERNEL);
        if (!dnet->tx_buf[i])
            goto err_nomem;
    }

    return 0;

err_nomem:
    _zsonet_free_netdev_buffers(dnet->tx_buf);
    dnet->tx_buf = NULL;
    return -ENOMEM;
}


inline static int _zsonet_setup_pci_device(struct pci_dev *pdev, struct zsonet_data *dd)
{
    int err;
    if ((err = pci_enable_device(pdev)) != 0)
        return err;
    if ((err = pci_request_regions(pdev, "zsonet_regions")) != 0)
        goto err_disable;

    void __iomem *bar = pci_iomap(pdev, ZSONET_BAR_DATA_NUMBER, ZSONET_BAR_SIZE);
    if (!bar) {
        err = -ENOMEM;
        goto err_bar;
    }

    // DMA
    if ((err = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32))) != 0)
        goto err_dma;
    pci_set_master(pdev);

    dd->bar = bar;
    return 0;

err_dma:
    pci_iounmap(pdev, bar);
err_bar:
    pci_release_regions(pdev);
err_disable:
    pci_disable_device(pdev);
    return err;
}

static int _zsonet_setup_dma(struct pci_dev *pdev, struct zsonet_netdev_data *d)
{
    d->rx_buf = dma_alloc_coherent(&pdev->dev, d->rx_buf_sz, &d->rx_buf_addr, GFP_KERNEL);
    for (int i = 0; i < ZSONET_TX_BUF_CNT; i++)
        d->tx_buf[i] = dma_alloc_coherent(&pdev->dev, ZSONET_TX_BUF_SIZE, &d->tx_dma_addr[i], GFP_KERNEL);
    return 0;
}

static void _cleanup_dma(struct pci_dev *pdev, struct zsonet_netdev_data *d)
{
    dma_free_coherent(&pdev->dev, d->rx_buf_sz, d->rx_buf, d->rx_buf_addr);
    for (int i = 0; i < ZSONET_TX_BUF_CNT; i++)
        dma_free_coherent(&pdev->dev, ZSONET_TX_BUF_SIZE, d->tx_buf[i], d->tx_dma_addr[i]);
}

static void _cleanup_pci_device(struct pci_dev *pdev)
{
    struct zsonet_data *dd = pci_get_drvdata(pdev);
    pci_clear_master(pdev);
    pci_disable_device(pdev);
    pci_iounmap(pdev, dd->bar);
    pci_release_regions(pdev);

    kfree(dd);
}

static int zsonet_probe(struct pci_dev *pdev,
	const struct pci_device_id *pci_id)
{
    // printk("zsonet_probe dev id vendor=%d device=%d", pci_id->vendor, pci_id->device);
    struct net_device *netdev;
    struct zsonet_data *dd;
    struct zsonet_netdev_data *dnet;
    int err;
    u8 *hw_addr;

    dd = kzalloc(sizeof(struct zsonet_data), GFP_KERNEL);
    hw_addr = kzalloc(ETH_ALEN, GFP_KERNEL);
    if (!dd || !hw_addr) {
        err = -ENOMEM;
        goto err;
    }

    netdev = alloc_etherdev(sizeof(struct zsonet_netdev_data));
    if (!netdev) {
        err = -ENOMEM;
        goto err;
    }

    dd->netdev = netdev;
    pci_set_drvdata(pdev, dd);
    netdev->netdev_ops = &zsonet_netdev_ops;
    netdev->max_mtu = ZSONET_MAX_MTU;
    netdev->min_mtu = ZSONET_MIN_MTU;

    if ((err = _zsonet_setup_pci_device(pdev, dd)) != 0)
        goto err_free_mem;
    
    // on error, netdev buffers are cleaned up by _zsonet_free_netdev
    dnet = netdev_priv(netdev);
    if ((err = _zsonet_init_netdev_data(pdev, dd, dnet)) != 0)
        goto err_cleanup_pci;
    
    if ((err = _zsonet_setup_dma(pdev, dnet)) != 0)
        goto err_cleanup_pci;

    SET_NETDEV_DEV(netdev, &pdev->dev);
    zsonet_reset(netdev);

   if ((err = register_netdev(netdev)))
		goto err_cleanup_dma;

    return 0;

err_cleanup_dma:
    _cleanup_dma(pdev, dnet);
err_cleanup_pci:
    _cleanup_pci_device(pdev);
err_free_mem:
    _zsonet_free_netdev(netdev);
err:
    kfree(dd);
    kfree(hw_addr);
    return err;
}

static void zsonet_remove(struct pci_dev *pdev)
{
    // printk("zsonet_remove cleanup!");
    struct zsonet_data *dd = pci_get_drvdata(pdev);

    if (dd->netdev) {
        unregister_netdev(dd->netdev);
        _cleanup_dma(pdev, netdev_priv(dd->netdev));
        _zsonet_free_netdev(dd->netdev);
    }
    _cleanup_pci_device(pdev);
}

static struct pci_device_id zsonet_id_table[] = {
	{ ZSO_PCI_DEVICE(PCI_VENDOR_ID_ZSO, PCI_DEVICE_ID_ZSONET, PCI_CLASS_NETWORK_ETHERNET) },
	{ 0 }
};

static struct pci_driver zsonet_pci_driver = {
	.name = ZSONET_DRV_NAME,
	.id_table = zsonet_id_table,
	.probe = zsonet_probe,
	.remove = zsonet_remove,
};


/* Init & exit.  */

static int __init zsonet_init(void)
{
    // printk("ZSONET driver init: %s", zsonet_pci_driver.name);
    return pci_register_driver(&zsonet_pci_driver);
}

static void __exit zsonet_exit(void)
{
    // printk("ZSONET exit");
    // for (int i = 0; i < 10; i++)
    //     printk("flush %d", i);
    pci_unregister_driver(&zsonet_pci_driver);
}


module_init(zsonet_init);
module_exit(zsonet_exit);