+/*************************************
+ * Multiqueue Tx port select override
+ *************************************/
+
+static int team_queue_override_init(struct team *team)
+{
+ struct list_head *listarr;
+ unsigned int queue_cnt = team->dev->num_tx_queues - 1;
+ unsigned int i;
+
+ if (!queue_cnt)
+ return 0;
+ listarr = kmalloc(sizeof(struct list_head) * queue_cnt, GFP_KERNEL);
+ if (!listarr)
+ return -ENOMEM;
+ team->qom_lists = listarr;
+ for (i = 0; i < queue_cnt; i++)
+ INIT_LIST_HEAD(listarr++);
+ return 0;
+}
+
+static void team_queue_override_fini(struct team *team)
+{
+ kfree(team->qom_lists);
+}
+
+static struct list_head *__team_get_qom_list(struct team *team, u16 queue_id)
+{
+ return &team->qom_lists[queue_id - 1];
+}
+
+/*
+ * note: already called with rcu_read_lock
+ */
+static bool team_queue_override_transmit(struct team *team, struct sk_buff *skb)
+{
+ struct list_head *qom_list;
+ struct team_port *port;
+
+ if (!team->queue_override_enabled || !skb->queue_mapping)
+ return false;
+ qom_list = __team_get_qom_list(team, skb->queue_mapping);
+ list_for_each_entry_rcu(port, qom_list, qom_list) {
+ if (!team_dev_queue_xmit(team, port, skb))
+ return true;
+ }
+ return false;
+}
+
+static void __team_queue_override_port_del(struct team *team,
+ struct team_port *port)
+{
+ list_del_rcu(&port->qom_list);
+ synchronize_rcu();
+ INIT_LIST_HEAD(&port->qom_list);
+}
+
+static bool team_queue_override_port_has_gt_prio_than(struct team_port *port,
+ struct team_port *cur)
+{
+ if (port->priority < cur->priority)
+ return true;
+ if (port->priority > cur->priority)
+ return false;
+ if (port->index < cur->index)
+ return true;
+ return false;
+}
+
+static void __team_queue_override_port_add(struct team *team,
+ struct team_port *port)
+{
+ struct team_port *cur;
+ struct list_head *qom_list;
+ struct list_head *node;
+
+ if (!port->queue_id || !team_port_enabled(port))
+ return;
+
+ qom_list = __team_get_qom_list(team, port->queue_id);
+ node = qom_list;
+ list_for_each_entry(cur, qom_list, qom_list) {
+ if (team_queue_override_port_has_gt_prio_than(port, cur))
+ break;
+ node = &cur->qom_list;
+ }
+ list_add_tail_rcu(&port->qom_list, node);
+}
+
+static void __team_queue_override_enabled_check(struct team *team)
+{
+ struct team_port *port;
+ bool enabled = false;
+
+ list_for_each_entry(port, &team->port_list, list) {
+ if (!list_empty(&port->qom_list)) {
+ enabled = true;
+ break;
+ }
+ }
+ if (enabled == team->queue_override_enabled)
+ return;
+ netdev_dbg(team->dev, "%s queue override\n",
+ enabled ? "Enabling" : "Disabling");
+ team->queue_override_enabled = enabled;
+}
+
+static void team_queue_override_port_refresh(struct team *team,
+ struct team_port *port)
+{
+ __team_queue_override_port_del(team, port);
+ __team_queue_override_port_add(team, port);
+ __team_queue_override_enabled_check(team);
+}
+
+