1 files changed, 40 insertions, 32 deletions

diff --git a/drivers/net/wireless/microchip/wilc1000/netdev.c b/drivers/net/wireless/microchip/wilc1000/netdev.c
index 710e29bea560..9ecf3fb29b55 100644
--- a/drivers/net/wireless/microchip/wilc1000/netdev.c
+++ b/drivers/net/wireless/microchip/wilc1000/netdev.c
@@ -590,7 +590,6 @@ static int wilc_mac_open(struct net_device *ndev)
 	struct wilc *wl = vif->wilc;
 	int ret = 0;
 	struct mgmt_frame_regs mgmt_regs = {};
-	u8 addr[ETH_ALEN] __aligned(2);
 
 	if (!wl || !wl->dev) {
 		netdev_err(ndev, "device not ready\n");
@@ -609,25 +608,19 @@ static int wilc_mac_open(struct net_device *ndev)
 		return ret;
 	}
 
-	wilc_set_operation_mode(vif, wilc_get_vif_idx(vif), vif->iftype,
-				vif->idx);
-
-	if (is_valid_ether_addr(ndev->dev_addr)) {
-		ether_addr_copy(addr, ndev->dev_addr);
-		wilc_set_mac_address(vif, addr);
-	} else {
-		wilc_get_mac_address(vif, addr);
-		eth_hw_addr_set(ndev, addr);
-	}
 	netdev_dbg(ndev, "Mac address: %pM\n", ndev->dev_addr);
-
-	if (!is_valid_ether_addr(ndev->dev_addr)) {
-		netdev_err(ndev, "Wrong MAC address\n");
+	ret = wilc_set_mac_address(vif, ndev->dev_addr);
+	if (ret) {
+		netdev_err(ndev, "Failed to enforce MAC address in chip");
 		wilc_deinit_host_int(ndev);
-		wilc_wlan_deinitialize(ndev);
-		return -EINVAL;
+		if (!wl->open_ifcs)
+			wilc_wlan_deinitialize(ndev);
+		return ret;
 	}
 
+	wilc_set_operation_mode(vif, wilc_get_vif_idx(vif), vif->iftype,
+				vif->idx);
+
 	mgmt_regs.interface_stypes = vif->mgmt_reg_stypes;
 	/* so we detect a change */
 	vif->mgmt_reg_stypes = 0;
@@ -681,7 +674,7 @@ static int wilc_set_mac_addr(struct net_device *dev, void *p)
 	}
 	srcu_read_unlock(&wilc->srcu, srcu_idx);
 
-	result = wilc_set_mac_address(vif, (u8 *)addr->sa_data);
+	result = wilc_set_mac_address(vif, addr->sa_data);
 	if (result)
 		return result;
 
@@ -948,6 +941,7 @@ struct wilc_vif *wilc_netdev_ifc_init(struct wilc *wl, const char *name,
 				      int vif_type, enum nl80211_iftype type,
 				      bool rtnl_locked)
 {
+	u8 mac_address[ETH_ALEN];
 	struct net_device *ndev;
 	struct wilc_vif *vif;
 	int ret;
@@ -972,36 +966,50 @@ struct wilc_vif *wilc_netdev_ifc_init(struct wilc *wl, const char *name,
 	vif->priv.wdev.iftype = type;
 	vif->priv.dev = ndev;
 
-	if (rtnl_locked)
-		ret = cfg80211_register_netdevice(ndev);
-	else
-		ret = register_netdev(ndev);
-
-	if (ret) {
-		ret = -EFAULT;
-		goto error;
-	}
-
 	ndev->needs_free_netdev = true;
 	vif->iftype = vif_type;
 	vif->idx = wilc_get_available_idx(wl);
 	vif->mac_opened = 0;
+
+	memcpy(mac_address, wl->nv_mac_address, ETH_ALEN);
+	/* WILC firmware uses locally administered MAC address for the
+	 * second virtual interface (bit 1 of first byte set), but
+	 * since it is possibly not loaded/running yet, reproduce this behavior
+	 * in the driver during interface creation.
+	 */
+	if (vif->idx)
+		mac_address[0] |= 0x2;
+
+	eth_hw_addr_set(vif->ndev, mac_address);
+
 	mutex_lock(&wl->vif_mutex);
 	list_add_tail_rcu(&vif->list, &wl->vif_list);
 	wl->vif_num += 1;
 	mutex_unlock(&wl->vif_mutex);
 	synchronize_srcu(&wl->srcu);
 
-	return vif;
-
-error:
 	if (rtnl_locked)
-		cfg80211_unregister_netdevice(ndev);
+		ret = cfg80211_register_netdevice(ndev);
 	else
-		unregister_netdev(ndev);
+		ret = register_netdev(ndev);
+
+	if (ret) {
+		ret = -EFAULT;
+		goto error_remove_vif;
+	}
+
+	return vif;
+
+error_remove_vif:
+	mutex_lock(&wl->vif_mutex);
+	list_del_rcu(&vif->list);
+	wl->vif_num -= 1;
+	mutex_unlock(&wl->vif_mutex);
+	synchronize_srcu(&wl->srcu);
 	free_netdev(ndev);
 	return ERR_PTR(ret);
 }
+EXPORT_SYMBOL_GPL(wilc_netdev_ifc_init);
 
 MODULE_DESCRIPTION("Atmel WILC1000 core wireless driver");
 MODULE_LICENSE("GPL");