// SPDX-License-Identifier: GPL-2.0-or-later
/*
 * Copyright Gavin Shan, IBM Corporation 2016.
 */

#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/netdevice.h>
#include <linux/skbuff.h>
#include <linux/of.h>
#include <linux/platform_device.h>

#include <net/ncsi.h>
#include <net/net_namespace.h>
#include <net/sock.h>
#include <net/addrconf.h>
#include <net/ipv6.h>
#include <net/genetlink.h>

#include "internal.h"
#include "ncsi-pkt.h"
#include "ncsi-netlink.h"

LIST_HEAD(ncsi_dev_list);
DEFINE_SPINLOCK(ncsi_dev_lock);

bool ncsi_channel_has_link(struct ncsi_channel *channel)
{
        return !!(channel->modes[NCSI_MODE_LINK].data[2] & 0x1);
}

bool ncsi_channel_is_last(struct ncsi_dev_priv *ndp,
                          struct ncsi_channel *channel)
{
        struct ncsi_package *np;
        struct ncsi_channel *nc;

        NCSI_FOR_EACH_PACKAGE(ndp, np)
                NCSI_FOR_EACH_CHANNEL(np, nc) {
                        if (nc == channel)
                                continue;
                        if (nc->state == NCSI_CHANNEL_ACTIVE &&
                            ncsi_channel_has_link(nc))
                                return false;
                }

        return true;
}

static void ncsi_report_link(struct ncsi_dev_priv *ndp, bool force_down)
{
        struct ncsi_dev *nd = &ndp->ndev;
        struct ncsi_package *np;
        struct ncsi_channel *nc;
        unsigned long flags;

        nd->state = ncsi_dev_state_functional;
        if (force_down) {
                nd->link_up = 0;
                goto report;
        }

        nd->link_up = 0;
        NCSI_FOR_EACH_PACKAGE(ndp, np) {
                NCSI_FOR_EACH_CHANNEL(np, nc) {
                        spin_lock_irqsave(&nc->lock, flags);

                        if (!list_empty(&nc->link) ||
                            nc->state != NCSI_CHANNEL_ACTIVE) {
                                spin_unlock_irqrestore(&nc->lock, flags);
                                continue;
                        }

                        if (ncsi_channel_has_link(nc)) {
                                spin_unlock_irqrestore(&nc->lock, flags);
                                nd->link_up = 1;
                                goto report;
                        }

                        spin_unlock_irqrestore(&nc->lock, flags);
                }
        }

report:
        nd->handler(nd);
}

static void ncsi_channel_monitor(struct timer_list *t)
{
        struct ncsi_channel *nc = timer_container_of(nc, t, monitor.timer);
        struct ncsi_package *np = nc->package;
        struct ncsi_dev_priv *ndp = np->ndp;
        struct ncsi_channel_mode *ncm;
        struct ncsi_cmd_arg nca;
        bool enabled, chained;
        unsigned int monitor_state;
        unsigned long flags;
        int state, ret;

        spin_lock_irqsave(&nc->lock, flags);
        state = nc->state;
        chained = !list_empty(&nc->link);
        enabled = nc->monitor.enabled;
        monitor_state = nc->monitor.state;
        spin_unlock_irqrestore(&nc->lock, flags);

        if (!enabled)
                return;         /* expected race disabling timer */
        if (WARN_ON_ONCE(chained))
                goto bad_state;

        if (state != NCSI_CHANNEL_INACTIVE &&
            state != NCSI_CHANNEL_ACTIVE) {
bad_state:
                netdev_warn(ndp->ndev.dev,
                            "Bad NCSI monitor state channel %d 0x%x %s queue\n",
                            nc->id, state, chained ? "on" : "off");
                spin_lock_irqsave(&nc->lock, flags);
                nc->monitor.enabled = false;
                spin_unlock_irqrestore(&nc->lock, flags);
                return;
        }

        switch (monitor_state) {
        case NCSI_CHANNEL_MONITOR_START:
        case NCSI_CHANNEL_MONITOR_RETRY:
                nca.ndp = ndp;
                nca.package = np->id;
                nca.channel = nc->id;
                nca.type = NCSI_PKT_CMD_GLS;
                nca.req_flags = 0;
                ret = ncsi_xmit_cmd(&nca);
                if (ret)
                        netdev_err(ndp->ndev.dev, "Error %d sending GLS\n",
                                   ret);
                break;
        case NCSI_CHANNEL_MONITOR_WAIT ... NCSI_CHANNEL_MONITOR_WAIT_MAX:
                break;
        default:
                netdev_err(ndp->ndev.dev, "NCSI Channel %d timed out!\n",
                           nc->id);
                ncsi_report_link(ndp, true);
                ndp->flags |= NCSI_DEV_RESHUFFLE;

                ncm = &nc->modes[NCSI_MODE_LINK];
                spin_lock_irqsave(&nc->lock, flags);
                nc->monitor.enabled = false;
                nc->state = NCSI_CHANNEL_INVISIBLE;
                ncm->data[2] &= ~0x1;
                spin_unlock_irqrestore(&nc->lock, flags);

                spin_lock_irqsave(&ndp->lock, flags);
                nc->state = NCSI_CHANNEL_ACTIVE;
                list_add_tail_rcu(&nc->link, &ndp->channel_queue);
                spin_unlock_irqrestore(&ndp->lock, flags);
                ncsi_process_next_channel(ndp);
                return;
        }

        spin_lock_irqsave(&nc->lock, flags);
        nc->monitor.state++;
        spin_unlock_irqrestore(&nc->lock, flags);
        mod_timer(&nc->monitor.timer, jiffies + HZ);
}

void ncsi_start_channel_monitor(struct ncsi_channel *nc)
{
        unsigned long flags;

        spin_lock_irqsave(&nc->lock, flags);
        WARN_ON_ONCE(nc->monitor.enabled);
        nc->monitor.enabled = true;
        nc->monitor.state = NCSI_CHANNEL_MONITOR_START;
        spin_unlock_irqrestore(&nc->lock, flags);

        mod_timer(&nc->monitor.timer, jiffies + HZ);
}

void ncsi_stop_channel_monitor(struct ncsi_channel *nc)
{
        unsigned long flags;

        spin_lock_irqsave(&nc->lock, flags);
        if (!nc->monitor.enabled) {
                spin_unlock_irqrestore(&nc->lock, flags);
                return;
        }
        nc->monitor.enabled = false;
        spin_unlock_irqrestore(&nc->lock, flags);

        timer_delete_sync(&nc->monitor.timer);
}

struct ncsi_channel *ncsi_find_channel(struct ncsi_package *np,
                                       unsigned char id)
{
        struct ncsi_channel *nc;

        NCSI_FOR_EACH_CHANNEL(np, nc) {
                if (nc->id == id)
                        return nc;
        }

        return NULL;
}

struct ncsi_channel *ncsi_add_channel(struct ncsi_package *np, unsigned char id)
{
        struct ncsi_channel *nc, *tmp;
        int index;
        unsigned long flags;

        nc = kzalloc_obj(*nc, GFP_ATOMIC);
        if (!nc)
                return NULL;

        nc->id = id;
        nc->package = np;
        nc->state = NCSI_CHANNEL_INACTIVE;
        nc->monitor.enabled = false;
        timer_setup(&nc->monitor.timer, ncsi_channel_monitor, 0);
        spin_lock_init(&nc->lock);
        INIT_LIST_HEAD(&nc->link);
        for (index = 0; index < NCSI_CAP_MAX; index++)
                nc->caps[index].index = index;
        for (index = 0; index < NCSI_MODE_MAX; index++)
                nc->modes[index].index = index;

        spin_lock_irqsave(&np->lock, flags);
        tmp = ncsi_find_channel(np, id);
        if (tmp) {
                spin_unlock_irqrestore(&np->lock, flags);
                kfree(nc);
                return tmp;
        }

        list_add_tail_rcu(&nc->node, &np->channels);
        np->channel_num++;
        spin_unlock_irqrestore(&np->lock, flags);

        return nc;
}

static void ncsi_remove_channel(struct ncsi_channel *nc)
{
        struct ncsi_package *np = nc->package;
        unsigned long flags;

        spin_lock_irqsave(&nc->lock, flags);

        /* Release filters */
        kfree(nc->mac_filter.addrs);
        kfree(nc->vlan_filter.vids);

        nc->state = NCSI_CHANNEL_INACTIVE;
        spin_unlock_irqrestore(&nc->lock, flags);
        ncsi_stop_channel_monitor(nc);

        /* Remove and free channel */
        spin_lock_irqsave(&np->lock, flags);
        list_del_rcu(&nc->node);
        np->channel_num--;
        spin_unlock_irqrestore(&np->lock, flags);

        kfree(nc);
}

struct ncsi_package *ncsi_find_package(struct ncsi_dev_priv *ndp,
                                       unsigned char id)
{
        struct ncsi_package *np;

        NCSI_FOR_EACH_PACKAGE(ndp, np) {
                if (np->id == id)
                        return np;
        }

        return NULL;
}

struct ncsi_package *ncsi_add_package(struct ncsi_dev_priv *ndp,
                                      unsigned char id)
{
        struct ncsi_package *np, *tmp;
        unsigned long flags;

        np = kzalloc_obj(*np, GFP_ATOMIC);
        if (!np)
                return NULL;

        np->id = id;
        np->ndp = ndp;
        spin_lock_init(&np->lock);
        INIT_LIST_HEAD(&np->channels);
        np->channel_whitelist = UINT_MAX;

        spin_lock_irqsave(&ndp->lock, flags);
        tmp = ncsi_find_package(ndp, id);
        if (tmp) {
                spin_unlock_irqrestore(&ndp->lock, flags);
                kfree(np);
                return tmp;
        }

        list_add_tail_rcu(&np->node, &ndp->packages);
        ndp->package_num++;
        spin_unlock_irqrestore(&ndp->lock, flags);

        return np;
}

void ncsi_remove_package(struct ncsi_package *np)
{
        struct ncsi_dev_priv *ndp = np->ndp;
        struct ncsi_channel *nc, *tmp;
        unsigned long flags;

        /* Release all child channels */
        list_for_each_entry_safe(nc, tmp, &np->channels, node)
                ncsi_remove_channel(nc);

        /* Remove and free package */
        spin_lock_irqsave(&ndp->lock, flags);
        list_del_rcu(&np->node);
        ndp->package_num--;
        spin_unlock_irqrestore(&ndp->lock, flags);

        kfree(np);
}

void ncsi_find_package_and_channel(struct ncsi_dev_priv *ndp,
                                   unsigned char id,
                                   struct ncsi_package **np,
                                   struct ncsi_channel **nc)
{
        struct ncsi_package *p;
        struct ncsi_channel *c;

        p = ncsi_find_package(ndp, NCSI_PACKAGE_INDEX(id));
        c = p ? ncsi_find_channel(p, NCSI_CHANNEL_INDEX(id)) : NULL;

        if (np)
                *np = p;
        if (nc)
                *nc = c;
}

/* For two consecutive NCSI commands, the packet IDs shouldn't
 * be same. Otherwise, the bogus response might be replied. So
 * the available IDs are allocated in round-robin fashion.
 */
struct ncsi_request *ncsi_alloc_request(struct ncsi_dev_priv *ndp,
                                        unsigned int req_flags)
{
        struct ncsi_request *nr = NULL;
        int i, limit = ARRAY_SIZE(ndp->requests);
        unsigned long flags;

        /* Check if there is one available request until the ceiling */
        spin_lock_irqsave(&ndp->lock, flags);
        for (i = ndp->request_id; i < limit; i++) {
                if (ndp->requests[i].used)
                        continue;

                nr = &ndp->requests[i];
                nr->used = true;
                nr->flags = req_flags;
                ndp->request_id = i + 1;
                goto found;
        }

        /* Fail back to check from the starting cursor */
        for (i = NCSI_REQ_START_IDX; i < ndp->request_id; i++) {
                if (ndp->requests[i].used)
                        continue;

                nr = &ndp->requests[i];
                nr->used = true;
                nr->flags = req_flags;
                ndp->request_id = i + 1;
                goto found;
        }

found:
        spin_unlock_irqrestore(&ndp->lock, flags);
        return nr;
}

void ncsi_free_request(struct ncsi_request *nr)
{
        struct ncsi_dev_priv *ndp = nr->ndp;
        struct sk_buff *cmd, *rsp;
        unsigned long flags;
        bool driven;

        if (nr->enabled) {
                nr->enabled = false;
                timer_delete_sync(&nr->timer);
        }

        spin_lock_irqsave(&ndp->lock, flags);
        cmd = nr->cmd;
        rsp = nr->rsp;
        nr->cmd = NULL;
        nr->rsp = NULL;
        nr->used = false;
        driven = !!(nr->flags & NCSI_REQ_FLAG_EVENT_DRIVEN);
        spin_unlock_irqrestore(&ndp->lock, flags);

        if (driven && cmd && --ndp->pending_req_num == 0)
                schedule_work(&ndp->work);

        /* Release command and response */
        consume_skb(cmd);
        consume_skb(rsp);
}

struct ncsi_dev *ncsi_find_dev(struct net_device *dev)
{
        struct ncsi_dev_priv *ndp;

        NCSI_FOR_EACH_DEV(ndp) {
                if (ndp->ndev.dev == dev)
                        return &ndp->ndev;
        }

        return NULL;
}

static void ncsi_request_timeout(struct timer_list *t)
{
        struct ncsi_request *nr = timer_container_of(nr, t, timer);
        struct ncsi_dev_priv *ndp = nr->ndp;
        struct ncsi_cmd_pkt *cmd;
        struct ncsi_package *np;
        struct ncsi_channel *nc;
        unsigned long flags;

        /* If the request already had associated response,
         * let the response handler to release it.
         */
        spin_lock_irqsave(&ndp->lock, flags);
        nr->enabled = false;
        if (nr->rsp || !nr->cmd) {
                spin_unlock_irqrestore(&ndp->lock, flags);
                return;
        }
        spin_unlock_irqrestore(&ndp->lock, flags);

        if (nr->flags == NCSI_REQ_FLAG_NETLINK_DRIVEN) {
                if (nr->cmd) {
                        /* Find the package */
                        cmd = (struct ncsi_cmd_pkt *)
                              skb_network_header(nr->cmd);
                        ncsi_find_package_and_channel(ndp,
                                                      cmd->cmd.common.channel,
                                                      &np, &nc);
                        ncsi_send_netlink_timeout(nr, np, nc);
                }
        }

        /* Release the request */
        ncsi_free_request(nr);
}

static void ncsi_suspend_channel(struct ncsi_dev_priv *ndp)
{
        struct ncsi_dev *nd = &ndp->ndev;
        struct ncsi_package *np;
        struct ncsi_channel *nc, *tmp;
        struct ncsi_cmd_arg nca;
        unsigned long flags;
        int ret;

        np = ndp->active_package;
        nc = ndp->active_channel;
        nca.ndp = ndp;
        nca.req_flags = NCSI_REQ_FLAG_EVENT_DRIVEN;
        switch (nd->state) {
        case ncsi_dev_state_suspend:
                nd->state = ncsi_dev_state_suspend_select;
                fallthrough;
        case ncsi_dev_state_suspend_select:
                ndp->pending_req_num = 1;

                nca.type = NCSI_PKT_CMD_SP;
                nca.package = np->id;
                nca.channel = NCSI_RESERVED_CHANNEL;
                if (ndp->flags & NCSI_DEV_HWA)
                        nca.bytes[0] = 0;
                else
                        nca.bytes[0] = 1;

                /* To retrieve the last link states of channels in current
                 * package when current active channel needs fail over to
                 * another one. It means we will possibly select another
                 * channel as next active one. The link states of channels
                 * are most important factor of the selection. So we need
                 * accurate link states. Unfortunately, the link states on
                 * inactive channels can't be updated with LSC AEN in time.
                 */
                if (ndp->flags & NCSI_DEV_RESHUFFLE)
                        nd->state = ncsi_dev_state_suspend_gls;
                else
                        nd->state = ncsi_dev_state_suspend_dcnt;
                ret = ncsi_xmit_cmd(&nca);
                if (ret)
                        goto error;

                break;
        case ncsi_dev_state_suspend_gls:
                ndp->pending_req_num = 1;

                nca.type = NCSI_PKT_CMD_GLS;
                nca.package = np->id;
                nca.channel = ndp->channel_probe_id;
                ret = ncsi_xmit_cmd(&nca);
                if (ret)
                        goto error;
                ndp->channel_probe_id++;

                if (ndp->channel_probe_id == ndp->channel_count) {
                        ndp->channel_probe_id = 0;
                        nd->state = ncsi_dev_state_suspend_dcnt;
                }

                break;
        case ncsi_dev_state_suspend_dcnt:
                ndp->pending_req_num = 1;

                nca.type = NCSI_PKT_CMD_DCNT;
                nca.package = np->id;
                nca.channel = nc->id;

                nd->state = ncsi_dev_state_suspend_dc;
                ret = ncsi_xmit_cmd(&nca);
                if (ret)
                        goto error;

                break;
        case ncsi_dev_state_suspend_dc:
                ndp->pending_req_num = 1;

                nca.type = NCSI_PKT_CMD_DC;
                nca.package = np->id;
                nca.channel = nc->id;
                nca.bytes[0] = 1;

                nd->state = ncsi_dev_state_suspend_deselect;
                ret = ncsi_xmit_cmd(&nca);
                if (ret)
                        goto error;

                NCSI_FOR_EACH_CHANNEL(np, tmp) {
                        /* If there is another channel active on this package
                         * do not deselect the package.
                         */
                        if (tmp != nc && tmp->state == NCSI_CHANNEL_ACTIVE) {
                                nd->state = ncsi_dev_state_suspend_done;
                                break;
                        }
                }
                break;
        case ncsi_dev_state_suspend_deselect:
                ndp->pending_req_num = 1;

                nca.type = NCSI_PKT_CMD_DP;
                nca.package = np->id;
                nca.channel = NCSI_RESERVED_CHANNEL;

                nd->state = ncsi_dev_state_suspend_done;
                ret = ncsi_xmit_cmd(&nca);
                if (ret)
                        goto error;

                break;
        case ncsi_dev_state_suspend_done:
                spin_lock_irqsave(&nc->lock, flags);
                nc->state = NCSI_CHANNEL_INACTIVE;
                spin_unlock_irqrestore(&nc->lock, flags);
                if (ndp->flags & NCSI_DEV_RESET)
                        ncsi_reset_dev(nd);
                else
                        ncsi_process_next_channel(ndp);
                break;
        default:
                netdev_warn(nd->dev, "Wrong NCSI state 0x%x in suspend\n",
                            nd->state);
        }

        return;
error:
        nd->state = ncsi_dev_state_functional;
}

/* Check the VLAN filter bitmap for a set filter, and construct a
 * "Set VLAN Filter - Disable" packet if found.
 */
static int clear_one_vid(struct ncsi_dev_priv *ndp, struct ncsi_channel *nc,
                         struct ncsi_cmd_arg *nca)
{
        struct ncsi_channel_vlan_filter *ncf;
        unsigned long flags;
        void *bitmap;
        int index;
        u16 vid;

        ncf = &nc->vlan_filter;
        bitmap = &ncf->bitmap;

        spin_lock_irqsave(&nc->lock, flags);
        index = find_first_bit(bitmap, ncf->n_vids);
        if (index >= ncf->n_vids) {
                spin_unlock_irqrestore(&nc->lock, flags);
                return -1;
        }
        vid = ncf->vids[index];

        clear_bit(index, bitmap);
        ncf->vids[index] = 0;
        spin_unlock_irqrestore(&nc->lock, flags);

        nca->type = NCSI_PKT_CMD_SVF;
        nca->words[1] = vid;
        /* HW filter index starts at 1 */
        nca->bytes[6] = index + 1;
        nca->bytes[7] = 0x00;
        return 0;
}

/* Find an outstanding VLAN tag and construct a "Set VLAN Filter - Enable"
 * packet.
 */
static int set_one_vid(struct ncsi_dev_priv *ndp, struct ncsi_channel *nc,
                       struct ncsi_cmd_arg *nca)
{
        struct ncsi_channel_vlan_filter *ncf;
        struct vlan_vid *vlan = NULL;
        unsigned long flags;
        int i, index;
        void *bitmap;
        u16 vid;

        if (list_empty(&ndp->vlan_vids))
                return -1;

        ncf = &nc->vlan_filter;
        bitmap = &ncf->bitmap;

        spin_lock_irqsave(&nc->lock, flags);

        rcu_read_lock();
        list_for_each_entry_rcu(vlan, &ndp->vlan_vids, list) {
                vid = vlan->vid;
                for (i = 0; i < ncf->n_vids; i++)
                        if (ncf->vids[i] == vid) {
                                vid = 0;
                                break;
                        }
                if (vid)
                        break;
        }
        rcu_read_unlock();

        if (!vid) {
                /* No VLAN ID is not set */
                spin_unlock_irqrestore(&nc->lock, flags);
                return -1;
        }

        index = find_first_zero_bit(bitmap, ncf->n_vids);
        if (index < 0 || index >= ncf->n_vids) {
                netdev_err(ndp->ndev.dev,
                           "Channel %u already has all VLAN filters set\n",
                           nc->id);
                spin_unlock_irqrestore(&nc->lock, flags);
                return -1;
        }

        ncf->vids[index] = vid;
        set_bit(index, bitmap);
        spin_unlock_irqrestore(&nc->lock, flags);

        nca->type = NCSI_PKT_CMD_SVF;
        nca->words[1] = vid;
        /* HW filter index starts at 1 */
        nca->bytes[6] = index + 1;
        nca->bytes[7] = 0x01;

        return 0;
}

static int ncsi_oem_keep_phy_intel(struct ncsi_cmd_arg *nca)
{
        unsigned char data[NCSI_OEM_INTEL_CMD_KEEP_PHY_LEN];
        int ret = 0;

        nca->payload = NCSI_OEM_INTEL_CMD_KEEP_PHY_LEN;

        memset(data, 0, NCSI_OEM_INTEL_CMD_KEEP_PHY_LEN);
        *(unsigned int *)data = ntohl((__force __be32)NCSI_OEM_MFR_INTEL_ID);

        data[4] = NCSI_OEM_INTEL_CMD_KEEP_PHY;

        /* PHY Link up attribute */
        data[6] = 0x1;

        nca->data = data;

        ret = ncsi_xmit_cmd(nca);
        if (ret)
                netdev_err(nca->ndp->ndev.dev,
                           "NCSI: Failed to transmit cmd 0x%x during configure\n",
                           nca->type);
        return ret;
}

/* NCSI OEM Command APIs */
static int ncsi_oem_gma_handler_bcm(struct ncsi_cmd_arg *nca)
{
        unsigned char data[NCSI_OEM_BCM_CMD_GMA_LEN];
        int ret = 0;

        nca->payload = NCSI_OEM_BCM_CMD_GMA_LEN;

        memset(data, 0, NCSI_OEM_BCM_CMD_GMA_LEN);
        *(unsigned int *)data = ntohl((__force __be32)NCSI_OEM_MFR_BCM_ID);
        data[5] = NCSI_OEM_BCM_CMD_GMA;

        nca->data = data;

        ret = ncsi_xmit_cmd(nca);
        if (ret)
                netdev_err(nca->ndp->ndev.dev,
                           "NCSI: Failed to transmit cmd 0x%x during configure\n",
                           nca->type);
        return ret;
}

static int ncsi_oem_gma_handler_mlx(struct ncsi_cmd_arg *nca)
{
        union {
                u8 data_u8[NCSI_OEM_MLX_CMD_GMA_LEN];
                u32 data_u32[NCSI_OEM_MLX_CMD_GMA_LEN / sizeof(u32)];
        } u;
        int ret = 0;

        nca->payload = NCSI_OEM_MLX_CMD_GMA_LEN;

        memset(&u, 0, sizeof(u));
        u.data_u32[0] = ntohl((__force __be32)NCSI_OEM_MFR_MLX_ID);
        u.data_u8[5] = NCSI_OEM_MLX_CMD_GMA;
        u.data_u8[6] = NCSI_OEM_MLX_CMD_GMA_PARAM;

        nca->data = u.data_u8;

        ret = ncsi_xmit_cmd(nca);
        if (ret)
                netdev_err(nca->ndp->ndev.dev,
                           "NCSI: Failed to transmit cmd 0x%x during configure\n",
                           nca->type);
        return ret;
}

static int ncsi_oem_smaf_mlx(struct ncsi_cmd_arg *nca)
{
        union {
                u8 data_u8[NCSI_OEM_MLX_CMD_SMAF_LEN];
                u32 data_u32[NCSI_OEM_MLX_CMD_SMAF_LEN / sizeof(u32)];
        } u;
        int ret = 0;

        memset(&u, 0, sizeof(u));
        u.data_u32[0] = ntohl((__force __be32)NCSI_OEM_MFR_MLX_ID);
        u.data_u8[5] = NCSI_OEM_MLX_CMD_SMAF;
        u.data_u8[6] = NCSI_OEM_MLX_CMD_SMAF_PARAM;
        memcpy(&u.data_u8[MLX_SMAF_MAC_ADDR_OFFSET],
               nca->ndp->ndev.dev->dev_addr,    ETH_ALEN);
        u.data_u8[MLX_SMAF_MED_SUPPORT_OFFSET] =
                (MLX_MC_RBT_AVL | MLX_MC_RBT_SUPPORT);

        nca->payload = NCSI_OEM_MLX_CMD_SMAF_LEN;
        nca->data = u.data_u8;

        ret = ncsi_xmit_cmd(nca);
        if (ret)
                netdev_err(nca->ndp->ndev.dev,
                           "NCSI: Failed to transmit cmd 0x%x during probe\n",
                           nca->type);
        return ret;
}

static int ncsi_oem_gma_handler_intel(struct ncsi_cmd_arg *nca)
{
        unsigned char data[NCSI_OEM_INTEL_CMD_GMA_LEN];
        int ret = 0;

        nca->payload = NCSI_OEM_INTEL_CMD_GMA_LEN;

        memset(data, 0, NCSI_OEM_INTEL_CMD_GMA_LEN);
        *(unsigned int *)data = ntohl((__force __be32)NCSI_OEM_MFR_INTEL_ID);
        data[4] = NCSI_OEM_INTEL_CMD_GMA;

        nca->data = data;

        ret = ncsi_xmit_cmd(nca);
        if (ret)
                netdev_err(nca->ndp->ndev.dev,
                           "NCSI: Failed to transmit cmd 0x%x during configure\n",
                           nca->type);

        return ret;
}

/* OEM Command handlers initialization */
static struct ncsi_oem_gma_handler {
        unsigned int    mfr_id;
        int             (*handler)(struct ncsi_cmd_arg *nca);
} ncsi_oem_gma_handlers[] = {
        { NCSI_OEM_MFR_BCM_ID, ncsi_oem_gma_handler_bcm },
        { NCSI_OEM_MFR_MLX_ID, ncsi_oem_gma_handler_mlx },
        { NCSI_OEM_MFR_INTEL_ID, ncsi_oem_gma_handler_intel }
};

static int ncsi_gma_handler(struct ncsi_cmd_arg *nca, unsigned int mf_id)
{
        struct ncsi_oem_gma_handler *nch = NULL;
        int i;

        /* This function should only be called once, return if flag set */
        if (nca->ndp->gma_flag == 1)
                return -1;

        /* Find gma handler for given manufacturer id */
        for (i = 0; i < ARRAY_SIZE(ncsi_oem_gma_handlers); i++) {
                if (ncsi_oem_gma_handlers[i].mfr_id == mf_id) {
                        if (ncsi_oem_gma_handlers[i].handler)
                                nch = &ncsi_oem_gma_handlers[i];
                        break;
                        }
        }

        if (!nch) {
                netdev_err(nca->ndp->ndev.dev,
                           "NCSI: No GMA handler available for MFR-ID (0x%x)\n",
                           mf_id);
                return -1;
        }

        /* Get Mac address from NCSI device */
        return nch->handler(nca);
}

/* Determine if a given channel from the channel_queue should be used for Tx */
static bool ncsi_channel_is_tx(struct ncsi_dev_priv *ndp,
                               struct ncsi_channel *nc)
{
        struct ncsi_channel_mode *ncm;
        struct ncsi_channel *channel;
        struct ncsi_package *np;

        /* Check if any other channel has Tx enabled; a channel may have already
         * been configured and removed from the channel queue.
         */
        NCSI_FOR_EACH_PACKAGE(ndp, np) {
                if (!ndp->multi_package && np != nc->package)
                        continue;
                NCSI_FOR_EACH_CHANNEL(np, channel) {
                        ncm = &channel->modes[NCSI_MODE_TX_ENABLE];
                        if (ncm->enable)
                                return false;
                }
        }

        /* This channel is the preferred channel and has link */
        list_for_each_entry_rcu(channel, &ndp->channel_queue, link) {
                np = channel->package;
                if (np->preferred_channel &&
                    ncsi_channel_has_link(np->preferred_channel)) {
                        return np->preferred_channel == nc;
                }
        }

        /* This channel has link */
        if (ncsi_channel_has_link(nc))
                return true;

        list_for_each_entry_rcu(channel, &ndp->channel_queue, link)
                if (ncsi_channel_has_link(channel))
                        return false;

        /* No other channel has link; default to this one */
        return true;
}

/* Change the active Tx channel in a multi-channel setup */
int ncsi_update_tx_channel(struct ncsi_dev_priv *ndp,
                           struct ncsi_package *package,
                           struct ncsi_channel *disable,
                           struct ncsi_channel *enable)
{
        struct ncsi_cmd_arg nca;
        struct ncsi_channel *nc;
        struct ncsi_package *np;
        int ret = 0;

        if (!package->multi_channel && !ndp->multi_package)
                netdev_warn(ndp->ndev.dev,
                            "NCSI: Trying to update Tx channel in single-channel mode\n");
        nca.ndp = ndp;
        nca.req_flags = 0;

        /* Find current channel with Tx enabled */
        NCSI_FOR_EACH_PACKAGE(ndp, np) {
                if (disable)
                        break;
                if (!ndp->multi_package && np != package)
                        continue;

                NCSI_FOR_EACH_CHANNEL(np, nc)
                        if (nc->modes[NCSI_MODE_TX_ENABLE].enable) {
                                disable = nc;
                                break;
                        }
        }

        /* Find a suitable channel for Tx */
        NCSI_FOR_EACH_PACKAGE(ndp, np) {
                if (enable)
                        break;
                if (!ndp->multi_package && np != package)
                        continue;
                if (!(ndp->package_whitelist & (0x1 << np->id)))
                        continue;

                if (np->preferred_channel &&
                    ncsi_channel_has_link(np->preferred_channel)) {
                        enable = np->preferred_channel;
                        break;
                }

                NCSI_FOR_EACH_CHANNEL(np, nc) {
                        if (!(np->channel_whitelist & 0x1 << nc->id))
                                continue;
                        if (nc->state != NCSI_CHANNEL_ACTIVE)
                                continue;
                        if (ncsi_channel_has_link(nc)) {
                                enable = nc;
                                break;
                        }
                }
        }

        if (disable == enable)
                return -1;

        if (!enable)
                return -1;

        if (disable) {
                nca.channel = disable->id;
                nca.package = disable->package->id;
                nca.type = NCSI_PKT_CMD_DCNT;
                ret = ncsi_xmit_cmd(&nca);
                if (ret)
                        netdev_err(ndp->ndev.dev,
                                   "Error %d sending DCNT\n",
                                   ret);
        }

        netdev_info(ndp->ndev.dev, "NCSI: channel %u enables Tx\n", enable->id);

        nca.channel = enable->id;
        nca.package = enable->package->id;
        nca.type = NCSI_PKT_CMD_ECNT;
        ret = ncsi_xmit_cmd(&nca);
        if (ret)
                netdev_err(ndp->ndev.dev,
                           "Error %d sending ECNT\n",
                           ret);

        return ret;
}

static void ncsi_configure_channel(struct ncsi_dev_priv *ndp)
{
        struct ncsi_package *np = ndp->active_package;
        struct ncsi_channel *nc = ndp->active_channel;
        struct ncsi_channel *hot_nc = NULL;
        struct ncsi_dev *nd = &ndp->ndev;
        struct net_device *dev = nd->dev;
        struct ncsi_cmd_arg nca;
        unsigned char index;
        unsigned long flags;
        int ret;

        nca.ndp = ndp;
        nca.req_flags = NCSI_REQ_FLAG_EVENT_DRIVEN;
        switch (nd->state) {
        case ncsi_dev_state_config:
        case ncsi_dev_state_config_sp:
                ndp->pending_req_num = 1;

                /* Select the specific package */
                nca.type = NCSI_PKT_CMD_SP;
                if (ndp->flags & NCSI_DEV_HWA)
                        nca.bytes[0] = 0;
                else
                        nca.bytes[0] = 1;
                nca.package = np->id;
                nca.channel = NCSI_RESERVED_CHANNEL;
                ret = ncsi_xmit_cmd(&nca);
                if (ret) {
                        netdev_err(ndp->ndev.dev,
                                   "NCSI: Failed to transmit CMD_SP\n");
                        goto error;
                }

                nd->state = ncsi_dev_state_config_cis;
                break;
        case ncsi_dev_state_config_cis:
                ndp->pending_req_num = 1;

                /* Clear initial state */
                nca.type = NCSI_PKT_CMD_CIS;
                nca.package = np->id;
                nca.channel = nc->id;
                ret = ncsi_xmit_cmd(&nca);
                if (ret) {
                        netdev_err(ndp->ndev.dev,
                                   "NCSI: Failed to transmit CMD_CIS\n");
                        goto error;
                }

                nd->state = IS_ENABLED(CONFIG_NCSI_OEM_CMD_GET_MAC)
                          ? ncsi_dev_state_config_oem_gma
                          : ncsi_dev_state_config_clear_vids;
                break;
        case ncsi_dev_state_config_oem_gma:
                nd->state = ncsi_dev_state_config_apply_mac;

                nca.package = np->id;
                nca.channel = nc->id;
                ndp->pending_req_num = 1;
                if (nc->version.major >= 1 && nc->version.minor >= 2) {
                        nca.type = NCSI_PKT_CMD_GMCMA;
                        ret = ncsi_xmit_cmd(&nca);
                } else {
                        nca.type = NCSI_PKT_CMD_OEM;
                        ret = ncsi_gma_handler(&nca, nc->version.mf_id);
                }
                if (ret < 0) {
                        nd->state = ncsi_dev_state_config_clear_vids;
                        schedule_work(&ndp->work);
                }

                break;
        case ncsi_dev_state_config_apply_mac:
                rtnl_lock();
                ret = dev_set_mac_address(dev, &ndp->pending_mac, NULL);
                rtnl_unlock();
                if (ret < 0)
                        netdev_warn(dev, "NCSI: 'Writing MAC address to device failed\n");

                nd->state = ncsi_dev_state_config_clear_vids;

                fallthrough;
        case ncsi_dev_state_config_clear_vids:
        case ncsi_dev_state_config_svf:
        case ncsi_dev_state_config_ev:
        case ncsi_dev_state_config_sma:
        case ncsi_dev_state_config_ebf:
        case ncsi_dev_state_config_dgmf:
        case ncsi_dev_state_config_ecnt:
        case ncsi_dev_state_config_ec:
        case ncsi_dev_state_config_ae:
        case ncsi_dev_state_config_gls:
                ndp->pending_req_num = 1;

                nca.package = np->id;
                nca.channel = nc->id;

                /* Clear any active filters on the channel before setting */
                if (nd->state == ncsi_dev_state_config_clear_vids) {
                        ret = clear_one_vid(ndp, nc, &nca);
                        if (ret) {
                                nd->state = ncsi_dev_state_config_svf;
                                schedule_work(&ndp->work);
                                break;
                        }
                        /* Repeat */
                        nd->state = ncsi_dev_state_config_clear_vids;
                /* Add known VLAN tags to the filter */
                } else if (nd->state == ncsi_dev_state_config_svf) {
                        ret = set_one_vid(ndp, nc, &nca);
                        if (ret) {
                                nd->state = ncsi_dev_state_config_ev;
                                schedule_work(&ndp->work);
                                break;
                        }
                        /* Repeat */
                        nd->state = ncsi_dev_state_config_svf;
                /* Enable/Disable the VLAN filter */
                } else if (nd->state == ncsi_dev_state_config_ev) {
                        if (list_empty(&ndp->vlan_vids)) {
                                nca.type = NCSI_PKT_CMD_DV;
                        } else {
                                nca.type = NCSI_PKT_CMD_EV;
                                nca.bytes[3] = NCSI_CAP_VLAN_NO;
                        }
                        nd->state = ncsi_dev_state_config_sma;
                } else if (nd->state == ncsi_dev_state_config_sma) {
                /* Use first entry in unicast filter table. Note that
                 * the MAC filter table starts from entry 1 instead of
                 * 0.
                 */
                        nca.type = NCSI_PKT_CMD_SMA;
                        for (index = 0; index < 6; index++)
                                nca.bytes[index] = dev->dev_addr[index];
                        nca.bytes[6] = 0x1;
                        nca.bytes[7] = 0x1;
                        nd->state = ncsi_dev_state_config_ebf;
                } else if (nd->state == ncsi_dev_state_config_ebf) {
                        nca.type = NCSI_PKT_CMD_EBF;
                        nca.dwords[0] = nc->caps[NCSI_CAP_BC].cap;
                        /* if multicast global filtering is supported then
                         * disable it so that all multicast packet will be
                         * forwarded to management controller
                         */
                        if (nc->caps[NCSI_CAP_GENERIC].cap &
                            NCSI_CAP_GENERIC_MC)
                                nd->state = ncsi_dev_state_config_dgmf;
                        else if (ncsi_channel_is_tx(ndp, nc))
                                nd->state = ncsi_dev_state_config_ecnt;
                        else
                                nd->state = ncsi_dev_state_config_ec;
                } else if (nd->state == ncsi_dev_state_config_dgmf) {
                        nca.type = NCSI_PKT_CMD_DGMF;
                        if (ncsi_channel_is_tx(ndp, nc))
                                nd->state = ncsi_dev_state_config_ecnt;
                        else
                                nd->state = ncsi_dev_state_config_ec;
                } else if (nd->state == ncsi_dev_state_config_ecnt) {
                        if (np->preferred_channel &&
                            nc != np->preferred_channel)
                                netdev_info(ndp->ndev.dev,
                                            "NCSI: Tx failed over to channel %u\n",
                                            nc->id);
                        nca.type = NCSI_PKT_CMD_ECNT;
                        nd->state = ncsi_dev_state_config_ec;
                } else if (nd->state == ncsi_dev_state_config_ec) {
                        /* Enable AEN if it's supported */
                        nca.type = NCSI_PKT_CMD_EC;
                        nd->state = ncsi_dev_state_config_ae;
                        if (!(nc->caps[NCSI_CAP_AEN].cap & NCSI_CAP_AEN_MASK))
                                nd->state = ncsi_dev_state_config_gls;
                } else if (nd->state == ncsi_dev_state_config_ae) {
                        nca.type = NCSI_PKT_CMD_AE;
                        nca.bytes[0] = 0;
                        nca.dwords[1] = nc->caps[NCSI_CAP_AEN].cap;
                        nd->state = ncsi_dev_state_config_gls;
                } else if (nd->state == ncsi_dev_state_config_gls) {
                        nca.type = NCSI_PKT_CMD_GLS;
                        nd->state = ncsi_dev_state_config_done;
                }

                ret = ncsi_xmit_cmd(&nca);
                if (ret) {
                        netdev_err(ndp->ndev.dev,
                                   "NCSI: Failed to transmit CMD %x\n",
                                   nca.type);
                        goto error;
                }
                break;
        case ncsi_dev_state_config_done:
                netdev_dbg(ndp->ndev.dev, "NCSI: channel %u config done\n",
                           nc->id);
                spin_lock_irqsave(&nc->lock, flags);
                nc->state = NCSI_CHANNEL_ACTIVE;

                if (ndp->flags & NCSI_DEV_RESET) {
                        /* A reset event happened during config, start it now */
                        nc->reconfigure_needed = false;
                        spin_unlock_irqrestore(&nc->lock, flags);
                        ncsi_reset_dev(nd);
                        break;
                }

                if (nc->reconfigure_needed) {
                        /* This channel's configuration has been updated
                         * part-way during the config state - start the
                         * channel configuration over
                         */
                        nc->reconfigure_needed = false;
                        nc->state = NCSI_CHANNEL_INACTIVE;
                        spin_unlock_irqrestore(&nc->lock, flags);

                        spin_lock_irqsave(&ndp->lock, flags);
                        list_add_tail_rcu(&nc->link, &ndp->channel_queue);
                        spin_unlock_irqrestore(&ndp->lock, flags);

                        netdev_dbg(dev, "Dirty NCSI channel state reset\n");
                        ncsi_process_next_channel(ndp);
                        break;
                }

                if (nc->modes[NCSI_MODE_LINK].data[2] & 0x1) {
                        hot_nc = nc;
                } else {
                        hot_nc = NULL;
                        netdev_dbg(ndp->ndev.dev,
                                   "NCSI: channel %u link down after config\n",
                                   nc->id);
                }
                spin_unlock_irqrestore(&nc->lock, flags);

                /* Update the hot channel */
                spin_lock_irqsave(&ndp->lock, flags);
                ndp->hot_channel = hot_nc;
                spin_unlock_irqrestore(&ndp->lock, flags);

                ncsi_start_channel_monitor(nc);
                ncsi_process_next_channel(ndp);
                break;
        default:
                netdev_alert(dev, "Wrong NCSI state 0x%x in config\n",
                             nd->state);
        }

        return;

error:
        ncsi_report_link(ndp, true);
}

static int ncsi_choose_active_channel(struct ncsi_dev_priv *ndp)
{
        struct ncsi_channel *nc, *found, *hot_nc;
        struct ncsi_channel_mode *ncm;
        unsigned long flags, cflags;
        struct ncsi_package *np;
        bool with_link;

        spin_lock_irqsave(&ndp->lock, flags);
        hot_nc = ndp->hot_channel;
        spin_unlock_irqrestore(&ndp->lock, flags);

        /* By default the search is done once an inactive channel with up
         * link is found, unless a preferred channel is set.
         * If multi_package or multi_channel are configured all channels in the
         * whitelist are added to the channel queue.
         */
        found = NULL;
        with_link = false;
        NCSI_FOR_EACH_PACKAGE(ndp, np) {
                if (!(ndp->package_whitelist & (0x1 << np->id)))
                        continue;
                NCSI_FOR_EACH_CHANNEL(np, nc) {
                        if (!(np->channel_whitelist & (0x1 << nc->id)))
                                continue;

                        spin_lock_irqsave(&nc->lock, cflags);

                        if (!list_empty(&nc->link) ||
                            nc->state != NCSI_CHANNEL_INACTIVE) {
                                spin_unlock_irqrestore(&nc->lock, cflags);
                                continue;
                        }

                        if (!found)
                                found = nc;

                        if (nc == hot_nc)
                                found = nc;

                        ncm = &nc->modes[NCSI_MODE_LINK];
                        if (ncm->data[2] & 0x1) {
                                found = nc;
                                with_link = true;
                        }

                        /* If multi_channel is enabled configure all valid
                         * channels whether or not they currently have link
                         * so they will have AENs enabled.
                         */
                        if (with_link || np->multi_channel) {
                                spin_lock_irqsave(&ndp->lock, flags);
                                list_add_tail_rcu(&nc->link,
                                                  &ndp->channel_queue);
                                spin_unlock_irqrestore(&ndp->lock, flags);

                                netdev_dbg(ndp->ndev.dev,
                                           "NCSI: Channel %u added to queue (link %s)\n",
                                           nc->id,
                                           ncm->data[2] & 0x1 ? "up" : "down");
                        }

                        spin_unlock_irqrestore(&nc->lock, cflags);

                        if (with_link && !np->multi_channel)
                                break;
                }
                if (with_link && !ndp->multi_package)
                        break;
        }

        if (list_empty(&ndp->channel_queue) && found) {
                netdev_info(ndp->ndev.dev,
                            "NCSI: No channel with link found, configuring channel %u\n",
                            found->id);
                spin_lock_irqsave(&ndp->lock, flags);
                list_add_tail_rcu(&found->link, &ndp->channel_queue);
                spin_unlock_irqrestore(&ndp->lock, flags);
        } else if (!found) {
                netdev_warn(ndp->ndev.dev,
                            "NCSI: No channel found to configure!\n");
                ncsi_report_link(ndp, true);
                return -ENODEV;
        }

        return ncsi_process_next_channel(ndp);
}

static bool ncsi_check_hwa(struct ncsi_dev_priv *ndp)
{
        struct ncsi_package *np;
        struct ncsi_channel *nc;
        unsigned int cap;
        bool has_channel = false;

        /* The hardware arbitration is disabled if any one channel
         * doesn't support explicitly.
         */
        NCSI_FOR_EACH_PACKAGE(ndp, np) {
                NCSI_FOR_EACH_CHANNEL(np, nc) {
                        has_channel = true;

                        cap = nc->caps[NCSI_CAP_GENERIC].cap;
                        if (!(cap & NCSI_CAP_GENERIC_HWA) ||
                            (cap & NCSI_CAP_GENERIC_HWA_MASK) !=
                            NCSI_CAP_GENERIC_HWA_SUPPORT) {
                                ndp->flags &= ~NCSI_DEV_HWA;
                                return false;
                        }
                }
        }

        if (has_channel) {
                ndp->flags |= NCSI_DEV_HWA;
                return true;
        }

        ndp->flags &= ~NCSI_DEV_HWA;
        return false;
}

static void ncsi_probe_channel(struct ncsi_dev_priv *ndp)
{
        struct ncsi_dev *nd = &ndp->ndev;
        struct ncsi_package *np;
        struct ncsi_cmd_arg nca;
        unsigned char index;
        int ret;

        nca.ndp = ndp;
        nca.req_flags = NCSI_REQ_FLAG_EVENT_DRIVEN;
        switch (nd->state) {
        case ncsi_dev_state_probe:
                nd->state = ncsi_dev_state_probe_deselect;
                fallthrough;
        case ncsi_dev_state_probe_deselect:
                ndp->pending_req_num = 8;

                /* Deselect all possible packages */
                nca.type = NCSI_PKT_CMD_DP;
                nca.channel = NCSI_RESERVED_CHANNEL;
                for (index = 0; index < 8; index++) {
                        nca.package = index;
                        ret = ncsi_xmit_cmd(&nca);
                        if (ret)
                                goto error;
                }

                nd->state = ncsi_dev_state_probe_package;
                break;
        case ncsi_dev_state_probe_package:
                if (ndp->package_probe_id >= 8) {
                        /* Last package probed, finishing */
                        ndp->flags |= NCSI_DEV_PROBED;
                        break;
                }

                ndp->pending_req_num = 1;

                nca.type = NCSI_PKT_CMD_SP;
                nca.bytes[0] = 1;
                nca.package = ndp->package_probe_id;
                nca.channel = NCSI_RESERVED_CHANNEL;
                ret = ncsi_xmit_cmd(&nca);
                if (ret)
                        goto error;
                nd->state = ncsi_dev_state_probe_channel;
                break;
        case ncsi_dev_state_probe_channel:
                ndp->active_package = ncsi_find_package(ndp,
                                                        ndp->package_probe_id);
                if (!ndp->active_package) {
                        /* No response */
                        nd->state = ncsi_dev_state_probe_dp;
                        schedule_work(&ndp->work);
                        break;
                }
                nd->state = ncsi_dev_state_probe_cis;
                if (IS_ENABLED(CONFIG_NCSI_OEM_CMD_GET_MAC) &&
                    ndp->mlx_multi_host)
                        nd->state = ncsi_dev_state_probe_mlx_gma;

                schedule_work(&ndp->work);
                break;
        case ncsi_dev_state_probe_mlx_gma:
                ndp->pending_req_num = 1;

                nca.type = NCSI_PKT_CMD_OEM;
                nca.package = ndp->active_package->id;
                nca.channel = 0;
                ret = ncsi_oem_gma_handler_mlx(&nca);
                if (ret)
                        goto error;

                nd->state = ncsi_dev_state_probe_mlx_smaf;
                break;
        case ncsi_dev_state_probe_mlx_smaf:
                ndp->pending_req_num = 1;

                nca.type = NCSI_PKT_CMD_OEM;
                nca.package = ndp->active_package->id;
                nca.channel = 0;
                ret = ncsi_oem_smaf_mlx(&nca);
                if (ret)
                        goto error;

                nd->state = ncsi_dev_state_probe_cis;
                break;
        case ncsi_dev_state_probe_keep_phy:
                ndp->pending_req_num = 1;

                nca.type = NCSI_PKT_CMD_OEM;
                nca.package = ndp->active_package->id;
                nca.channel = 0;
                ret = ncsi_oem_keep_phy_intel(&nca);
                if (ret)
                        goto error;

                nd->state = ncsi_dev_state_probe_gvi;
                break;
        case ncsi_dev_state_probe_cis:
        case ncsi_dev_state_probe_gvi:
        case ncsi_dev_state_probe_gc:
        case ncsi_dev_state_probe_gls:
                np = ndp->active_package;
                ndp->pending_req_num = 1;

                /* Clear initial state Retrieve version, capability or link status */
                if (nd->state == ncsi_dev_state_probe_cis)
                        nca.type = NCSI_PKT_CMD_CIS;
                else if (nd->state == ncsi_dev_state_probe_gvi)
                        nca.type = NCSI_PKT_CMD_GVI;
                else if (nd->state == ncsi_dev_state_probe_gc)
                        nca.type = NCSI_PKT_CMD_GC;
                else
                        nca.type = NCSI_PKT_CMD_GLS;

                nca.package = np->id;
                nca.channel = ndp->channel_probe_id;

                ret = ncsi_xmit_cmd(&nca);
                if (ret)
                        goto error;

                if (nd->state == ncsi_dev_state_probe_cis) {
                        nd->state = ncsi_dev_state_probe_gvi;
                        if (IS_ENABLED(CONFIG_NCSI_OEM_CMD_KEEP_PHY) && ndp->channel_probe_id == 0)
                                nd->state = ncsi_dev_state_probe_keep_phy;
                } else if (nd->state == ncsi_dev_state_probe_gvi) {
                        nd->state = ncsi_dev_state_probe_gc;
                } else if (nd->state == ncsi_dev_state_probe_gc) {
                        nd->state = ncsi_dev_state_probe_gls;
                } else {
                        nd->state = ncsi_dev_state_probe_cis;
                        ndp->channel_probe_id++;
                }

                if (ndp->channel_probe_id == ndp->channel_count) {
                        ndp->channel_probe_id = 0;
                        nd->state = ncsi_dev_state_probe_dp;
                }
                break;
        case ncsi_dev_state_probe_dp:
                ndp->pending_req_num = 1;

                /* Deselect the current package */
                nca.type = NCSI_PKT_CMD_DP;
                nca.package = ndp->package_probe_id;
                nca.channel = NCSI_RESERVED_CHANNEL;
                ret = ncsi_xmit_cmd(&nca);
                if (ret)
                        goto error;

                /* Probe next package after receiving response */
                ndp->package_probe_id++;
                nd->state = ncsi_dev_state_probe_package;
                ndp->active_package = NULL;
                break;
        default:
                netdev_warn(nd->dev, "Wrong NCSI state 0x%0x in enumeration\n",
                            nd->state);
        }

        if (ndp->flags & NCSI_DEV_PROBED) {
                /* Check if all packages have HWA support */
                ncsi_check_hwa(ndp);
                ncsi_choose_active_channel(ndp);
        }

        return;
error:
        netdev_err(ndp->ndev.dev,
                   "NCSI: Failed to transmit cmd 0x%x during probe\n",
                   nca.type);
        ncsi_report_link(ndp, true);
}

static void ncsi_dev_work(struct work_struct *work)
{
        struct ncsi_dev_priv *ndp = container_of(work,
                        struct ncsi_dev_priv, work);
        struct ncsi_dev *nd = &ndp->ndev;

        switch (nd->state & ncsi_dev_state_major) {
        case ncsi_dev_state_probe:
                ncsi_probe_channel(ndp);
                break;
        case ncsi_dev_state_suspend:
                ncsi_suspend_channel(ndp);
                break;
        case ncsi_dev_state_config:
                ncsi_configure_channel(ndp);
                break;
        default:
                netdev_warn(nd->dev, "Wrong NCSI state 0x%x in workqueue\n",
                            nd->state);
        }
}

int ncsi_process_next_channel(struct ncsi_dev_priv *ndp)
{
        struct ncsi_channel *nc;
        int old_state;
        unsigned long flags;

        spin_lock_irqsave(&ndp->lock, flags);
        nc = list_first_or_null_rcu(&ndp->channel_queue,
                                    struct ncsi_channel, link);
        if (!nc) {
                spin_unlock_irqrestore(&ndp->lock, flags);
                goto out;
        }

        list_del_init(&nc->link);
        spin_unlock_irqrestore(&ndp->lock, flags);

        spin_lock_irqsave(&nc->lock, flags);
        old_state = nc->state;
        nc->state = NCSI_CHANNEL_INVISIBLE;
        spin_unlock_irqrestore(&nc->lock, flags);

        ndp->active_channel = nc;
        ndp->active_package = nc->package;

        switch (old_state) {
        case NCSI_CHANNEL_INACTIVE:
                ndp->ndev.state = ncsi_dev_state_config;
                netdev_dbg(ndp->ndev.dev, "NCSI: configuring channel %u\n",
                           nc->id);
                ncsi_configure_channel(ndp);
                break;
        case NCSI_CHANNEL_ACTIVE:
                ndp->ndev.state = ncsi_dev_state_suspend;
                netdev_dbg(ndp->ndev.dev, "NCSI: suspending channel %u\n",
                           nc->id);
                ncsi_suspend_channel(ndp);
                break;
        default:
                netdev_err(ndp->ndev.dev, "Invalid state 0x%x on %d:%d\n",
                           old_state, nc->package->id, nc->id);
                ncsi_report_link(ndp, false);
                return -EINVAL;
        }

        return 0;

out:
        ndp->active_channel = NULL;
        ndp->active_package = NULL;
        if (ndp->flags & NCSI_DEV_RESHUFFLE) {
                ndp->flags &= ~NCSI_DEV_RESHUFFLE;
                return ncsi_choose_active_channel(ndp);
        }

        ncsi_report_link(ndp, false);
        return -ENODEV;
}

static int ncsi_kick_channels(struct ncsi_dev_priv *ndp)
{
        struct ncsi_dev *nd = &ndp->ndev;
        struct ncsi_channel *nc;
        struct ncsi_package *np;
        unsigned long flags;
        unsigned int n = 0;

        NCSI_FOR_EACH_PACKAGE(ndp, np) {
                NCSI_FOR_EACH_CHANNEL(np, nc) {
                        spin_lock_irqsave(&nc->lock, flags);

                        /* Channels may be busy, mark dirty instead of
                         * kicking if;
                         * a) not ACTIVE (configured)
                         * b) in the channel_queue (to be configured)
                         * c) it's ndev is in the config state
                         */
                        if (nc->state != NCSI_CHANNEL_ACTIVE) {
                                if ((ndp->ndev.state & 0xff00) ==
                                                ncsi_dev_state_config ||
                                                !list_empty(&nc->link)) {
                                        netdev_dbg(nd->dev,
                                                   "NCSI: channel %p marked dirty\n",
                                                   nc);
                                        nc->reconfigure_needed = true;
                                }
                                spin_unlock_irqrestore(&nc->lock, flags);
                                continue;
                        }

                        spin_unlock_irqrestore(&nc->lock, flags);

                        ncsi_stop_channel_monitor(nc);
                        spin_lock_irqsave(&nc->lock, flags);
                        nc->state = NCSI_CHANNEL_INACTIVE;
                        spin_unlock_irqrestore(&nc->lock, flags);

                        spin_lock_irqsave(&ndp->lock, flags);
                        list_add_tail_rcu(&nc->link, &ndp->channel_queue);
                        spin_unlock_irqrestore(&ndp->lock, flags);

                        netdev_dbg(nd->dev, "NCSI: kicked channel %p\n", nc);
                        n++;
                }
        }

        return n;
}

int ncsi_vlan_rx_add_vid(struct net_device *dev, __be16 proto, u16 vid)
{
        struct ncsi_dev_priv *ndp;
        unsigned int n_vids = 0;
        struct vlan_vid *vlan;
        struct ncsi_dev *nd;
        bool found = false;

        if (vid == 0)
                return 0;

        nd = ncsi_find_dev(dev);
        if (!nd) {
                netdev_warn(dev, "NCSI: No net_device?\n");
                return 0;
        }

        ndp = TO_NCSI_DEV_PRIV(nd);

        /* Add the VLAN id to our internal list */
        list_for_each_entry_rcu(vlan, &ndp->vlan_vids, list) {
                n_vids++;
                if (vlan->vid == vid) {
                        netdev_dbg(dev, "NCSI: vid %u already registered\n",
                                   vid);
                        return 0;
                }
        }
        if (n_vids >= NCSI_MAX_VLAN_VIDS) {
                netdev_warn(dev,
                            "tried to add vlan id %u but NCSI max already registered (%u)\n",
                            vid, NCSI_MAX_VLAN_VIDS);
                return -ENOSPC;
        }

        vlan = kzalloc_obj(*vlan);
        if (!vlan)
                return -ENOMEM;

        vlan->proto = proto;
        vlan->vid = vid;
        list_add_rcu(&vlan->list, &ndp->vlan_vids);

        netdev_dbg(dev, "NCSI: Added new vid %u\n", vid);

        found = ncsi_kick_channels(ndp) != 0;

        return found ? ncsi_process_next_channel(ndp) : 0;
}
EXPORT_SYMBOL_GPL(ncsi_vlan_rx_add_vid);

int ncsi_vlan_rx_kill_vid(struct net_device *dev, __be16 proto, u16 vid)
{
        struct vlan_vid *vlan, *tmp;
        struct ncsi_dev_priv *ndp;
        struct ncsi_dev *nd;
        bool found = false;

        if (vid == 0)
                return 0;

        nd = ncsi_find_dev(dev);
        if (!nd) {
                netdev_warn(dev, "NCSI: no net_device?\n");
                return 0;
        }

        ndp = TO_NCSI_DEV_PRIV(nd);

        /* Remove the VLAN id from our internal list */
        list_for_each_entry_safe(vlan, tmp, &ndp->vlan_vids, list)
                if (vlan->vid == vid) {
                        netdev_dbg(dev, "NCSI: vid %u found, removing\n", vid);
                        list_del_rcu(&vlan->list);
                        found = true;
                        kfree(vlan);
                }

        if (!found) {
                netdev_err(dev, "NCSI: vid %u wasn't registered!\n", vid);
                return -EINVAL;
        }

        found = ncsi_kick_channels(ndp) != 0;

        return found ? ncsi_process_next_channel(ndp) : 0;
}
EXPORT_SYMBOL_GPL(ncsi_vlan_rx_kill_vid);

struct ncsi_dev *ncsi_register_dev(struct net_device *dev,
                                   void (*handler)(struct ncsi_dev *ndev))
{
        struct ncsi_dev_priv *ndp;
        struct ncsi_dev *nd;
        struct platform_device *pdev;
        struct device_node *np;
        unsigned long flags;
        int i;

        /* Check if the device has been registered or not */
        nd = ncsi_find_dev(dev);
        if (nd)
                return nd;

        /* Create NCSI device */
        ndp = kzalloc_obj(*ndp, GFP_ATOMIC);
        if (!ndp)
                return NULL;

        nd = &ndp->ndev;
        nd->state = ncsi_dev_state_registered;
        nd->dev = dev;
        nd->handler = handler;
        ndp->pending_req_num = 0;
        INIT_LIST_HEAD(&ndp->channel_queue);
        INIT_LIST_HEAD(&ndp->vlan_vids);
        INIT_WORK(&ndp->work, ncsi_dev_work);
        ndp->package_whitelist = UINT_MAX;

        /* Initialize private NCSI device */
        spin_lock_init(&ndp->lock);
        INIT_LIST_HEAD(&ndp->packages);
        ndp->request_id = NCSI_REQ_START_IDX;
        for (i = 0; i < ARRAY_SIZE(ndp->requests); i++) {
                ndp->requests[i].id = i;
                ndp->requests[i].ndp = ndp;
                timer_setup(&ndp->requests[i].timer, ncsi_request_timeout, 0);
        }
        ndp->channel_count = NCSI_RESERVED_CHANNEL;

        spin_lock_irqsave(&ncsi_dev_lock, flags);
        list_add_tail_rcu(&ndp->node, &ncsi_dev_list);
        spin_unlock_irqrestore(&ncsi_dev_lock, flags);

        /* Register NCSI packet Rx handler */
        ndp->ptype.type = cpu_to_be16(ETH_P_NCSI);
        ndp->ptype.func = ncsi_rcv_rsp;
        ndp->ptype.dev = dev;
        dev_add_pack(&ndp->ptype);

        pdev = to_platform_device(dev->dev.parent);
        if (pdev) {
                np = pdev->dev.of_node;
                if (np && (of_property_read_bool(np, "mellanox,multi-host") ||
                           of_property_read_bool(np, "mlx,multi-host")))
                        ndp->mlx_multi_host = true;
        }

        return nd;
}
EXPORT_SYMBOL_GPL(ncsi_register_dev);

int ncsi_start_dev(struct ncsi_dev *nd)
{
        struct ncsi_dev_priv *ndp = TO_NCSI_DEV_PRIV(nd);

        if (nd->state != ncsi_dev_state_registered &&
            nd->state != ncsi_dev_state_functional)
                return -ENOTTY;

        if (!(ndp->flags & NCSI_DEV_PROBED)) {
                ndp->package_probe_id = 0;
                ndp->channel_probe_id = 0;
                nd->state = ncsi_dev_state_probe;
                schedule_work(&ndp->work);
                return 0;
        }

        return ncsi_reset_dev(nd);
}
EXPORT_SYMBOL_GPL(ncsi_start_dev);

void ncsi_stop_dev(struct ncsi_dev *nd)
{
        struct ncsi_dev_priv *ndp = TO_NCSI_DEV_PRIV(nd);
        struct ncsi_package *np;
        struct ncsi_channel *nc;
        bool chained;
        int old_state;
        unsigned long flags;

        /* Stop the channel monitor on any active channels. Don't reset the
         * channel state so we know which were active when ncsi_start_dev()
         * is next called.
         */
        NCSI_FOR_EACH_PACKAGE(ndp, np) {
                NCSI_FOR_EACH_CHANNEL(np, nc) {
                        ncsi_stop_channel_monitor(nc);

                        spin_lock_irqsave(&nc->lock, flags);
                        chained = !list_empty(&nc->link);
                        old_state = nc->state;
                        spin_unlock_irqrestore(&nc->lock, flags);

                        WARN_ON_ONCE(chained ||
                                     old_state == NCSI_CHANNEL_INVISIBLE);
                }
        }

        netdev_dbg(ndp->ndev.dev, "NCSI: Stopping device\n");
        ncsi_report_link(ndp, true);
}
EXPORT_SYMBOL_GPL(ncsi_stop_dev);

int ncsi_reset_dev(struct ncsi_dev *nd)
{
        struct ncsi_dev_priv *ndp = TO_NCSI_DEV_PRIV(nd);
        struct ncsi_channel *nc, *active, *tmp;
        struct ncsi_package *np;
        unsigned long flags;

        spin_lock_irqsave(&ndp->lock, flags);

        if (!(ndp->flags & NCSI_DEV_RESET)) {
                /* Haven't been called yet, check states */
                switch (nd->state & ncsi_dev_state_major) {
                case ncsi_dev_state_registered:
                case ncsi_dev_state_probe:
                        /* Not even probed yet - do nothing */
                        spin_unlock_irqrestore(&ndp->lock, flags);
                        return 0;
                case ncsi_dev_state_suspend:
                case ncsi_dev_state_config:
                        /* Wait for the channel to finish its suspend/config
                         * operation; once it finishes it will check for
                         * NCSI_DEV_RESET and reset the state.
                         */
                        ndp->flags |= NCSI_DEV_RESET;
                        spin_unlock_irqrestore(&ndp->lock, flags);
                        return 0;
                }
        } else {
                switch (nd->state) {
                case ncsi_dev_state_suspend_done:
                case ncsi_dev_state_config_done:
                case ncsi_dev_state_functional:
                        /* Ok */
                        break;
                default:
                        /* Current reset operation happening */
                        spin_unlock_irqrestore(&ndp->lock, flags);
                        return 0;
                }
        }

        if (!list_empty(&ndp->channel_queue)) {
                /* Clear any channel queue we may have interrupted */
                list_for_each_entry_safe(nc, tmp, &ndp->channel_queue, link)
                        list_del_init(&nc->link);
        }
        spin_unlock_irqrestore(&ndp->lock, flags);

        active = NULL;
        NCSI_FOR_EACH_PACKAGE(ndp, np) {
                NCSI_FOR_EACH_CHANNEL(np, nc) {
                        spin_lock_irqsave(&nc->lock, flags);

                        if (nc->state == NCSI_CHANNEL_ACTIVE) {
                                active = nc;
                                nc->state = NCSI_CHANNEL_INVISIBLE;
                                spin_unlock_irqrestore(&nc->lock, flags);
                                ncsi_stop_channel_monitor(nc);
                                break;
                        }

                        spin_unlock_irqrestore(&nc->lock, flags);
                }
                if (active)
                        break;
        }

        if (!active) {
                /* Done */
                spin_lock_irqsave(&ndp->lock, flags);
                ndp->flags &= ~NCSI_DEV_RESET;
                spin_unlock_irqrestore(&ndp->lock, flags);
                return ncsi_choose_active_channel(ndp);
        }

        spin_lock_irqsave(&ndp->lock, flags);
        ndp->flags |= NCSI_DEV_RESET;
        ndp->active_channel = active;
        ndp->active_package = active->package;
        spin_unlock_irqrestore(&ndp->lock, flags);

        nd->state = ncsi_dev_state_suspend;
        schedule_work(&ndp->work);
        return 0;
}

void ncsi_unregister_dev(struct ncsi_dev *nd)
{
        struct ncsi_dev_priv *ndp = TO_NCSI_DEV_PRIV(nd);
        struct ncsi_package *np, *tmp;
        unsigned long flags;

        dev_remove_pack(&ndp->ptype);

        list_for_each_entry_safe(np, tmp, &ndp->packages, node)
                ncsi_remove_package(np);

        spin_lock_irqsave(&ncsi_dev_lock, flags);
        list_del_rcu(&ndp->node);
        spin_unlock_irqrestore(&ncsi_dev_lock, flags);

        disable_work_sync(&ndp->work);

        kfree(ndp);
}
EXPORT_SYMBOL_GPL(ncsi_unregister_dev);