1 files changed, 50 insertions, 34 deletions

diff --git a/drivers/net/pcmcia/xirc2ps_cs.c b/drivers/net/pcmcia/xirc2ps_cs.c
index 4ace18a7115..4d1802e457b 100644
--- a/drivers/net/pcmcia/xirc2ps_cs.c
+++ b/drivers/net/pcmcia/xirc2ps_cs.c
@@ -1364,47 +1364,63 @@ do_start_xmit(struct sk_buff *skb, struct net_device *dev)
     return NETDEV_TX_OK;
 }
 
+struct set_address_info {
+	int reg_nr;
+	int page_nr;
+	int mohawk;
+	unsigned int ioaddr;
+};
+
+static void set_address(struct set_address_info *sa_info, char *addr)
+{
+	unsigned int ioaddr = sa_info->ioaddr;
+	int i;
+
+	for (i = 0; i < 6; i++) {
+		if (sa_info->reg_nr > 15) {
+			sa_info->reg_nr = 8;
+			sa_info->page_nr++;
+			SelectPage(sa_info->page_nr);
+		}
+		if (sa_info->mohawk)
+			PutByte(sa_info->reg_nr++, addr[5 - i]);
+		else
+			PutByte(sa_info->reg_nr++, addr[i]);
+	}
+}
+
 /****************
  * Set all addresses: This first one is the individual address,
  * the next 9 addresses are taken from the multicast list and
  * the rest is filled with the individual address.
  */
-static void
-set_addresses(struct net_device *dev)
+static void set_addresses(struct net_device *dev)
 {
-    unsigned int ioaddr = dev->base_addr;
-    local_info_t *lp = netdev_priv(dev);
-    struct dev_mc_list *dmi = dev->mc_list;
-    unsigned char *addr;
-    int i,j,k,n;
-
-    SelectPage(k=0x50);
-    for (i=0,j=8,n=0; ; i++, j++) {
-	if (i > 5) {
-	    if (++n > 9)
-		break;
-	    i = 0;
-	    if (n > 1 && n <= netdev_mc_count(dev) && dmi) {
-	   	 dmi = dmi->next;
-	    }
-	}
-	if (j > 15) {
-	    j = 8;
-	    k++;
-	    SelectPage(k);
-	}
-
-	if (n && n <= netdev_mc_count(dev) && dmi)
-	    addr = dmi->dmi_addr;
-	else
-	    addr = dev->dev_addr;
+	unsigned int ioaddr = dev->base_addr;
+	local_info_t *lp = netdev_priv(dev);
+	struct dev_mc_list *dmi;
+	struct set_address_info sa_info;
+	int i;
 
-	if (lp->mohawk)
-	    PutByte(j, addr[5-i]);
-	else
-	    PutByte(j, addr[i]);
-    }
-    SelectPage(0);
+	/*
+	 * Setup the info structure so that by first set_address call it will do
+	 * SelectPage with the right page number. Hence these ones here.
+	 */
+	sa_info.reg_nr = 15 + 1;
+	sa_info.page_nr = 0x50 - 1;
+	sa_info.mohawk = lp->mohawk;
+	sa_info.ioaddr = ioaddr;
+
+	set_address(&sa_info, dev->dev_addr);
+	i = 0;
+	netdev_for_each_mc_addr(dmi, dev) {
+		if (i++ == 9)
+			break;
+		set_address(&sa_info, dmi->dmi_addr);
+	}
+	while (i++ < 9)
+		set_address(&sa_info, dev->dev_addr);
+	SelectPage(0);
 }
 
 /****************