@@ -69,15 +88,173 @@ static const struct ethtool_ops ep_ethtool_ops = {
.get_drvinfo = ep_get_drvinfo,
};
+/*
+ * Parse the "tx-queues-config" child of the EP node. The logical queue
+ * index is taken from the "queue<N>" node name, so the mapping does not
+ * depend on the order the child nodes appear in the device tree.
+ */
+static int ep_parse_tx_queue_config(struct xlnx_tsn_ep *ep,
+ struct device_node *txcfg_np, u16 tx_present)
+{
+ DECLARE_BITMAP(queue_seen, TSN_MAX_TX_QUEUE) = {};
+ DECLARE_BITMAP(chan_seen, TSN_MAX_TX_QUEUE) = {};
+ unsigned int count = 0;
+ int ret;
+
+ for_each_child_of_node_scoped(txcfg_np, qnode) {
+ u32 chan, queue;
+
+ if (!str_has_prefix(qnode->name, "queue") ||
+ kstrtou32(qnode->name + strlen("queue"), 10, &queue) ||
+ queue >= ep->num_tx_queues)
+ return dev_err_probe(ep->dev, -EINVAL,
+ "tx-config: invalid queue node %pOFn (have %u queues)\n",
+ qnode, ep->num_tx_queues);
+
+ if (test_and_set_bit(queue, queue_seen))
+ return dev_err_probe(ep->dev, -EINVAL,
+ "tx-config: queue %u described twice\n",
+ queue);
+
+ ret = of_property_read_u32(qnode, "xlnx,dma-channel-num", &chan);
+ if (ret)
+ return dev_err_probe(ep->dev, ret,
+ "tx-config: queue %u missing xlnx,dma-channel-num\n",
+ queue);
+
+ if (chan >= TSN_MAX_TX_QUEUE || !(tx_present & BIT(chan)))
+ return dev_err_probe(ep->dev, -EINVAL,
+ "tx-config: queue %u maps to channel %u not present in dma-names\n",
+ queue, chan);
+
+ if (test_and_set_bit(chan, chan_seen))
+ return dev_err_probe(ep->dev, -EINVAL,
+ "tx-config: channel %u already assigned to another queue\n",
+ chan);
+
+ ep->tx_dma_chan_map[queue] = chan;
+ count++;
+ }
+
+ if (count != ep->num_tx_queues)
+ return dev_err_probe(ep->dev, -EINVAL,
+ "tx-config: described %u queues but expected %u\n",
+ count, ep->num_tx_queues);
+
+ return 0;
+}
+
+static int ep_count_dma_queues(struct device *dev, u32 *out_tx, u32 *out_rx,
+ u16 *tx_present, u32 *rx_chan_num)
+{
+ u32 tx = 0, rx = 0;
+ u16 rx_present = 0;
+ int n, i, ret;
+
+ n = of_property_count_strings(dev->of_node, "dma-names");
+ if (n < 0)
+ return dev_err_probe(dev, n, "failed to read dma-names\n");
+
+ for (i = 0; i < n; i++) {
+ const char *name;
+ size_t plen;
+ u32 idx;
+
+ ret = of_property_read_string_index(dev->of_node, "dma-names",
+ i, &name);
+ if (ret)
+ return dev_err_probe(dev, ret,
+ "failed to read dma-names[%d]\n", i);
+
+ plen = str_has_prefix(name, "tx_chan");
+ if (plen) {
+ if (kstrtou32(name + plen, 10, &idx) ||
+ idx >= TSN_MAX_TX_QUEUE)
+ return dev_err_probe(dev, -EINVAL,
+ "invalid TX channel name %s\n",
+ name);
+
+ if (*tx_present & BIT(idx))
+ return dev_err_probe(dev, -EINVAL,
+ "duplicate TX channel %s\n",
+ name);
+
+ *tx_present |= BIT(idx);
+ tx++;
+ continue;
+ }
+
+ plen = str_has_prefix(name, "rx_chan");
+ if (plen) {
+ if (kstrtou32(name + plen, 10, &idx) ||
+ idx >= TSN_MAX_RX_QUEUE)
+ return dev_err_probe(dev, -EINVAL,
+ "invalid RX channel name %s\n",
+ name);
+
+ if (rx_present & BIT(idx))
+ return dev_err_probe(dev, -EINVAL,
+ "duplicate RX channel %s\n",
+ name);
+
+ rx_present |= BIT(idx);
+ rx_chan_num[rx] = idx;
+ rx++;
+ continue;
+ }
+
+ return dev_err_probe(dev, -EINVAL,
+ "unrecognised dma-names entry %s\n", name);
+ }
+
+ if (!tx)
+ return dev_err_probe(dev, -EINVAL,
+ "no TX channels in dma-names\n");
+
+ if (!rx)
+ return dev_err_probe(dev, -EINVAL,
+ "no RX channels in dma-names\n");
+
+ *out_tx = tx;
+ *out_rx = rx;
+
+ return 0;
+}
+
static int xlnx_tsn_ep_probe(struct platform_device *pdev)
{
+ u32 rx_chan_num[TSN_MAX_RX_QUEUE];
struct device *dev = &pdev->dev;
+ struct device_node *txcfg_np;
+ u32 num_tx, num_rx, num_prio;
+ struct device_node *ip_np;
struct net_device *ndev;
struct xlnx_tsn_ep *ep;
u8 mac_addr[ETH_ALEN];
+ u16 tx_present = 0;
int ret;
+ int i;
+
+ ret = ep_count_dma_queues(dev, &num_tx, &num_rx, &tx_present, rx_chan_num);
+ if (ret)
+ return ret;
+
+ ip_np = of_get_parent(dev->of_node);
+ if (!ip_np)
+ return dev_err_probe(dev, -EINVAL, "missing parent IP node\n");
+
+ ret = of_property_read_u32(ip_np, "xlnx,num-priorities", &num_prio);
+ of_node_put(ip_np);
+ if (ret)
+ return dev_err_probe(dev, ret, "missing xlnx,num-priorities\n");
+
+ if (num_tx != num_prio)
+ return dev_err_probe(dev, -EINVAL,
+ "TX channel count %u must equal num-priorities %u\n",
+ num_tx, num_prio);
- ndev = alloc_netdev(sizeof(*ep), "ep%d", NET_NAME_ENUM, ether_setup);
+ ndev = alloc_netdev_mqs(sizeof(*ep), "ep%d", NET_NAME_ENUM,
+ ether_setup, num_tx, num_rx);
if (!ndev)
return -ENOMEM;