1 files changed, 385 insertions, 54 deletions

diff --git a/drivers/net/ethernet/renesas/sh_eth.c b/drivers/net/ethernet/renesas/sh_eth.c
index 87b65013177..8615961c128 100644
--- a/drivers/net/ethernet/renesas/sh_eth.c
+++ b/drivers/net/ethernet/renesas/sh_eth.c
@@ -34,7 +34,6 @@
 #include <linux/phy.h>
 #include <linux/cache.h>
 #include <linux/io.h>
-#include <linux/interrupt.h>
 #include <linux/pm_runtime.h>
 #include <linux/slab.h>
 #include <linux/ethtool.h>
@@ -269,6 +268,7 @@ static struct sh_eth_cpu_data sh_eth_my_cpu_data_giga = {
 	.rpadir_value   = 2 << 16,
 	.no_trimd	= 1,
 	.no_ade		= 1,
+	.tsu		= 1,
 };
 
 static struct sh_eth_cpu_data *sh_eth_get_cpu_data(struct sh_eth_private *mdp)
@@ -654,13 +654,12 @@ static void sh_eth_ring_format(struct net_device *ndev)
 	for (i = 0; i < RX_RING_SIZE; i++) {
 		/* skb */
 		mdp->rx_skbuff[i] = NULL;
-		skb = dev_alloc_skb(mdp->rx_buf_sz);
+		skb = netdev_alloc_skb(ndev, mdp->rx_buf_sz);
 		mdp->rx_skbuff[i] = skb;
 		if (skb == NULL)
 			break;
-		dma_map_single(&ndev->dev, skb->tail, mdp->rx_buf_sz,
+		dma_map_single(&ndev->dev, skb->data, mdp->rx_buf_sz,
 				DMA_FROM_DEVICE);
-		skb->dev = ndev; /* Mark as being used by this device. */
 		sh_eth_set_receive_align(skb);
 
 		/* RX descriptor */
@@ -883,8 +882,8 @@ static int sh_eth_txfree(struct net_device *ndev)
 		if (entry >= TX_RING_SIZE - 1)
 			txdesc->status |= cpu_to_edmac(mdp, TD_TDLE);
 
-		mdp->stats.tx_packets++;
-		mdp->stats.tx_bytes += txdesc->buffer_length;
+		ndev->stats.tx_packets++;
+		ndev->stats.tx_bytes += txdesc->buffer_length;
 	}
 	return freeNum;
 }
@@ -910,23 +909,23 @@ static int sh_eth_rx(struct net_device *ndev)
 			break;
 
 		if (!(desc_status & RDFEND))
-			mdp->stats.rx_length_errors++;
+			ndev->stats.rx_length_errors++;
 
 		if (desc_status & (RD_RFS1 | RD_RFS2 | RD_RFS3 | RD_RFS4 |
 				   RD_RFS5 | RD_RFS6 | RD_RFS10)) {
-			mdp->stats.rx_errors++;
+			ndev->stats.rx_errors++;
 			if (desc_status & RD_RFS1)
-				mdp->stats.rx_crc_errors++;
+				ndev->stats.rx_crc_errors++;
 			if (desc_status & RD_RFS2)
-				mdp->stats.rx_frame_errors++;
+				ndev->stats.rx_frame_errors++;
 			if (desc_status & RD_RFS3)
-				mdp->stats.rx_length_errors++;
+				ndev->stats.rx_length_errors++;
 			if (desc_status & RD_RFS4)
-				mdp->stats.rx_length_errors++;
+				ndev->stats.rx_length_errors++;
 			if (desc_status & RD_RFS6)
-				mdp->stats.rx_missed_errors++;
+				ndev->stats.rx_missed_errors++;
 			if (desc_status & RD_RFS10)
-				mdp->stats.rx_over_errors++;
+				ndev->stats.rx_over_errors++;
 		} else {
 			if (!mdp->cd->hw_swap)
 				sh_eth_soft_swap(
@@ -939,8 +938,8 @@ static int sh_eth_rx(struct net_device *ndev)
 			skb_put(skb, pkt_len);
 			skb->protocol = eth_type_trans(skb, ndev);
 			netif_rx(skb);
-			mdp->stats.rx_packets++;
-			mdp->stats.rx_bytes += pkt_len;
+			ndev->stats.rx_packets++;
+			ndev->stats.rx_bytes += pkt_len;
 		}
 		rxdesc->status |= cpu_to_edmac(mdp, RD_RACT);
 		entry = (++mdp->cur_rx) % RX_RING_SIZE;
@@ -955,13 +954,12 @@ static int sh_eth_rx(struct net_device *ndev)
 		rxdesc->buffer_length = ALIGN(mdp->rx_buf_sz, 16);
 
 		if (mdp->rx_skbuff[entry] == NULL) {
-			skb = dev_alloc_skb(mdp->rx_buf_sz);
+			skb = netdev_alloc_skb(ndev, mdp->rx_buf_sz);
 			mdp->rx_skbuff[entry] = skb;
 			if (skb == NULL)
 				break;	/* Better luck next round. */
-			dma_map_single(&ndev->dev, skb->tail, mdp->rx_buf_sz,
+			dma_map_single(&ndev->dev, skb->data, mdp->rx_buf_sz,
 					DMA_FROM_DEVICE);
-			skb->dev = ndev;
 			sh_eth_set_receive_align(skb);
 
 			skb_checksum_none_assert(skb);
@@ -1009,7 +1007,7 @@ static void sh_eth_error(struct net_device *ndev, int intr_status)
 		felic_stat = sh_eth_read(ndev, ECSR);
 		sh_eth_write(ndev, felic_stat, ECSR);	/* clear int */
 		if (felic_stat & ECSR_ICD)
-			mdp->stats.tx_carrier_errors++;
+			ndev->stats.tx_carrier_errors++;
 		if (felic_stat & ECSR_LCHNG) {
 			/* Link Changed */
 			if (mdp->cd->no_psr || mdp->no_ether_link) {
@@ -1042,7 +1040,7 @@ static void sh_eth_error(struct net_device *ndev, int intr_status)
 	if (intr_status & EESR_TWB) {
 		/* Write buck end. unused write back interrupt */
 		if (intr_status & EESR_TABT)	/* Transmit Abort int */
-			mdp->stats.tx_aborted_errors++;
+			ndev->stats.tx_aborted_errors++;
 			if (netif_msg_tx_err(mdp))
 				dev_err(&ndev->dev, "Transmit Abort\n");
 	}
@@ -1051,7 +1049,7 @@ static void sh_eth_error(struct net_device *ndev, int intr_status)
 		/* Receive Abort int */
 		if (intr_status & EESR_RFRMER) {
 			/* Receive Frame Overflow int */
-			mdp->stats.rx_frame_errors++;
+			ndev->stats.rx_frame_errors++;
 			if (netif_msg_rx_err(mdp))
 				dev_err(&ndev->dev, "Receive Abort\n");
 		}
@@ -1059,21 +1057,21 @@ static void sh_eth_error(struct net_device *ndev, int intr_status)
 
 	if (intr_status & EESR_TDE) {
 		/* Transmit Descriptor Empty int */
-		mdp->stats.tx_fifo_errors++;
+		ndev->stats.tx_fifo_errors++;
 		if (netif_msg_tx_err(mdp))
 			dev_err(&ndev->dev, "Transmit Descriptor Empty\n");
 	}
 
 	if (intr_status & EESR_TFE) {
 		/* FIFO under flow */
-		mdp->stats.tx_fifo_errors++;
+		ndev->stats.tx_fifo_errors++;
 		if (netif_msg_tx_err(mdp))
 			dev_err(&ndev->dev, "Transmit FIFO Under flow\n");
 	}
 
 	if (intr_status & EESR_RDE) {
 		/* Receive Descriptor Empty int */
-		mdp->stats.rx_over_errors++;
+		ndev->stats.rx_over_errors++;
 
 		if (sh_eth_read(ndev, EDRRR) ^ EDRRR_R)
 			sh_eth_write(ndev, EDRRR_R, EDRRR);
@@ -1083,14 +1081,14 @@ static void sh_eth_error(struct net_device *ndev, int intr_status)
 
 	if (intr_status & EESR_RFE) {
 		/* Receive FIFO Overflow int */
-		mdp->stats.rx_fifo_errors++;
+		ndev->stats.rx_fifo_errors++;
 		if (netif_msg_rx_err(mdp))
 			dev_err(&ndev->dev, "Receive FIFO Overflow\n");
 	}
 
 	if (!mdp->cd->no_ade && (intr_status & EESR_ADE)) {
 		/* Address Error */
-		mdp->stats.tx_fifo_errors++;
+		ndev->stats.tx_fifo_errors++;
 		if (netif_msg_tx_err(mdp))
 			dev_err(&ndev->dev, "Address Error\n");
 	}
@@ -1447,7 +1445,7 @@ static void sh_eth_tx_timeout(struct net_device *ndev)
 	       " resetting...\n", ndev->name, (int)sh_eth_read(ndev, EESR));
 
 	/* tx_errors count up */
-	mdp->stats.tx_errors++;
+	ndev->stats.tx_errors++;
 
 	/* timer off */
 	del_timer_sync(&mdp->timer);
@@ -1569,27 +1567,27 @@ static struct net_device_stats *sh_eth_get_stats(struct net_device *ndev)
 
 	pm_runtime_get_sync(&mdp->pdev->dev);
 
-	mdp->stats.tx_dropped += sh_eth_read(ndev, TROCR);
+	ndev->stats.tx_dropped += sh_eth_read(ndev, TROCR);
 	sh_eth_write(ndev, 0, TROCR);	/* (write clear) */
-	mdp->stats.collisions += sh_eth_read(ndev, CDCR);
+	ndev->stats.collisions += sh_eth_read(ndev, CDCR);
 	sh_eth_write(ndev, 0, CDCR);	/* (write clear) */
-	mdp->stats.tx_carrier_errors += sh_eth_read(ndev, LCCR);
+	ndev->stats.tx_carrier_errors += sh_eth_read(ndev, LCCR);
 	sh_eth_write(ndev, 0, LCCR);	/* (write clear) */
 	if (sh_eth_is_gether(mdp)) {
-		mdp->stats.tx_carrier_errors += sh_eth_read(ndev, CERCR);
+		ndev->stats.tx_carrier_errors += sh_eth_read(ndev, CERCR);
 		sh_eth_write(ndev, 0, CERCR);	/* (write clear) */
-		mdp->stats.tx_carrier_errors += sh_eth_read(ndev, CEECR);
+		ndev->stats.tx_carrier_errors += sh_eth_read(ndev, CEECR);
 		sh_eth_write(ndev, 0, CEECR);	/* (write clear) */
 	} else {
-		mdp->stats.tx_carrier_errors += sh_eth_read(ndev, CNDCR);
+		ndev->stats.tx_carrier_errors += sh_eth_read(ndev, CNDCR);
 		sh_eth_write(ndev, 0, CNDCR);	/* (write clear) */
 	}
 	pm_runtime_put_sync(&mdp->pdev->dev);
 
-	return &mdp->stats;
+	return &ndev->stats;
 }
 
-/* ioctl to device funciotn*/
+/* ioctl to device function */
 static int sh_eth_do_ioctl(struct net_device *ndev, struct ifreq *rq,
 				int cmd)
 {
@@ -1606,18 +1604,345 @@ static int sh_eth_do_ioctl(struct net_device *ndev, struct ifreq *rq,
 }
 
 #if defined(SH_ETH_HAS_TSU)
+/* For TSU_POSTn. Please refer to the manual about this (strange) bitfields */
+static void *sh_eth_tsu_get_post_reg_offset(struct sh_eth_private *mdp,
+					    int entry)
+{
+	return sh_eth_tsu_get_offset(mdp, TSU_POST1) + (entry / 8 * 4);
+}
+
+static u32 sh_eth_tsu_get_post_mask(int entry)
+{
+	return 0x0f << (28 - ((entry % 8) * 4));
+}
+
+static u32 sh_eth_tsu_get_post_bit(struct sh_eth_private *mdp, int entry)
+{
+	return (0x08 >> (mdp->port << 1)) << (28 - ((entry % 8) * 4));
+}
+
+static void sh_eth_tsu_enable_cam_entry_post(struct net_device *ndev,
+					     int entry)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	u32 tmp;
+	void *reg_offset;
+
+	reg_offset = sh_eth_tsu_get_post_reg_offset(mdp, entry);
+	tmp = ioread32(reg_offset);
+	iowrite32(tmp | sh_eth_tsu_get_post_bit(mdp, entry), reg_offset);
+}
+
+static bool sh_eth_tsu_disable_cam_entry_post(struct net_device *ndev,
+					      int entry)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	u32 post_mask, ref_mask, tmp;
+	void *reg_offset;
+
+	reg_offset = sh_eth_tsu_get_post_reg_offset(mdp, entry);
+	post_mask = sh_eth_tsu_get_post_mask(entry);
+	ref_mask = sh_eth_tsu_get_post_bit(mdp, entry) & ~post_mask;
+
+	tmp = ioread32(reg_offset);
+	iowrite32(tmp & ~post_mask, reg_offset);
+
+	/* If other port enables, the function returns "true" */
+	return tmp & ref_mask;
+}
+
+static int sh_eth_tsu_busy(struct net_device *ndev)
+{
+	int timeout = SH_ETH_TSU_TIMEOUT_MS * 100;
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+
+	while ((sh_eth_tsu_read(mdp, TSU_ADSBSY) & TSU_ADSBSY_0)) {
+		udelay(10);
+		timeout--;
+		if (timeout <= 0) {
+			dev_err(&ndev->dev, "%s: timeout\n", __func__);
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static int sh_eth_tsu_write_entry(struct net_device *ndev, void *reg,
+				  const u8 *addr)
+{
+	u32 val;
+
+	val = addr[0] << 24 | addr[1] << 16 | addr[2] << 8 | addr[3];
+	iowrite32(val, reg);
+	if (sh_eth_tsu_busy(ndev) < 0)
+		return -EBUSY;
+
+	val = addr[4] << 8 | addr[5];
+	iowrite32(val, reg + 4);
+	if (sh_eth_tsu_busy(ndev) < 0)
+		return -EBUSY;
+
+	return 0;
+}
+
+static void sh_eth_tsu_read_entry(void *reg, u8 *addr)
+{
+	u32 val;
+
+	val = ioread32(reg);
+	addr[0] = (val >> 24) & 0xff;
+	addr[1] = (val >> 16) & 0xff;
+	addr[2] = (val >> 8) & 0xff;
+	addr[3] = val & 0xff;
+	val = ioread32(reg + 4);
+	addr[4] = (val >> 8) & 0xff;
+	addr[5] = val & 0xff;
+}
+
+
+static int sh_eth_tsu_find_entry(struct net_device *ndev, const u8 *addr)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	void *reg_offset = sh_eth_tsu_get_offset(mdp, TSU_ADRH0);
+	int i;
+	u8 c_addr[ETH_ALEN];
+
+	for (i = 0; i < SH_ETH_TSU_CAM_ENTRIES; i++, reg_offset += 8) {
+		sh_eth_tsu_read_entry(reg_offset, c_addr);
+		if (memcmp(addr, c_addr, ETH_ALEN) == 0)
+			return i;
+	}
+
+	return -ENOENT;
+}
+
+static int sh_eth_tsu_find_empty(struct net_device *ndev)
+{
+	u8 blank[ETH_ALEN];
+	int entry;
+
+	memset(blank, 0, sizeof(blank));
+	entry = sh_eth_tsu_find_entry(ndev, blank);
+	return (entry < 0) ? -ENOMEM : entry;
+}
+
+static int sh_eth_tsu_disable_cam_entry_table(struct net_device *ndev,
+					      int entry)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	void *reg_offset = sh_eth_tsu_get_offset(mdp, TSU_ADRH0);
+	int ret;
+	u8 blank[ETH_ALEN];
+
+	sh_eth_tsu_write(mdp, sh_eth_tsu_read(mdp, TSU_TEN) &
+			 ~(1 << (31 - entry)), TSU_TEN);
+
+	memset(blank, 0, sizeof(blank));
+	ret = sh_eth_tsu_write_entry(ndev, reg_offset + entry * 8, blank);
+	if (ret < 0)
+		return ret;
+	return 0;
+}
+
+static int sh_eth_tsu_add_entry(struct net_device *ndev, const u8 *addr)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	void *reg_offset = sh_eth_tsu_get_offset(mdp, TSU_ADRH0);
+	int i, ret;
+
+	if (!mdp->cd->tsu)
+		return 0;
+
+	i = sh_eth_tsu_find_entry(ndev, addr);
+	if (i < 0) {
+		/* No entry found, create one */
+		i = sh_eth_tsu_find_empty(ndev);
+		if (i < 0)
+			return -ENOMEM;
+		ret = sh_eth_tsu_write_entry(ndev, reg_offset + i * 8, addr);
+		if (ret < 0)
+			return ret;
+
+		/* Enable the entry */
+		sh_eth_tsu_write(mdp, sh_eth_tsu_read(mdp, TSU_TEN) |
+				 (1 << (31 - i)), TSU_TEN);
+	}
+
+	/* Entry found or created, enable POST */
+	sh_eth_tsu_enable_cam_entry_post(ndev, i);
+
+	return 0;
+}
+
+static int sh_eth_tsu_del_entry(struct net_device *ndev, const u8 *addr)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	int i, ret;
+
+	if (!mdp->cd->tsu)
+		return 0;
+
+	i = sh_eth_tsu_find_entry(ndev, addr);
+	if (i) {
+		/* Entry found */
+		if (sh_eth_tsu_disable_cam_entry_post(ndev, i))
+			goto done;
+
+		/* Disable the entry if both ports was disabled */
+		ret = sh_eth_tsu_disable_cam_entry_table(ndev, i);
+		if (ret < 0)
+			return ret;
+	}
+done:
+	return 0;
+}
+
+static int sh_eth_tsu_purge_all(struct net_device *ndev)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	int i, ret;
+
+	if (unlikely(!mdp->cd->tsu))
+		return 0;
+
+	for (i = 0; i < SH_ETH_TSU_CAM_ENTRIES; i++) {
+		if (sh_eth_tsu_disable_cam_entry_post(ndev, i))
+			continue;
+
+		/* Disable the entry if both ports was disabled */
+		ret = sh_eth_tsu_disable_cam_entry_table(ndev, i);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static void sh_eth_tsu_purge_mcast(struct net_device *ndev)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	u8 addr[ETH_ALEN];
+	void *reg_offset = sh_eth_tsu_get_offset(mdp, TSU_ADRH0);
+	int i;
+
+	if (unlikely(!mdp->cd->tsu))
+		return;
+
+	for (i = 0; i < SH_ETH_TSU_CAM_ENTRIES; i++, reg_offset += 8) {
+		sh_eth_tsu_read_entry(reg_offset, addr);
+		if (is_multicast_ether_addr(addr))
+			sh_eth_tsu_del_entry(ndev, addr);
+	}
+}
+
 /* Multicast reception directions set */
 static void sh_eth_set_multicast_list(struct net_device *ndev)
 {
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	u32 ecmr_bits;
+	int mcast_all = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&mdp->lock, flags);
+	/*
+	 * Initial condition is MCT = 1, PRM = 0.
+	 * Depending on ndev->flags, set PRM or clear MCT
+	 */
+	ecmr_bits = (sh_eth_read(ndev, ECMR) & ~ECMR_PRM) | ECMR_MCT;
+
+	if (!(ndev->flags & IFF_MULTICAST)) {
+		sh_eth_tsu_purge_mcast(ndev);
+		mcast_all = 1;
+	}
+	if (ndev->flags & IFF_ALLMULTI) {
+		sh_eth_tsu_purge_mcast(ndev);
+		ecmr_bits &= ~ECMR_MCT;
+		mcast_all = 1;
+	}
+
 	if (ndev->flags & IFF_PROMISC) {
-		/* Set promiscuous. */
-		sh_eth_write(ndev, (sh_eth_read(ndev, ECMR) & ~ECMR_MCT) |
-				ECMR_PRM, ECMR);
+		sh_eth_tsu_purge_all(ndev);
+		ecmr_bits = (ecmr_bits & ~ECMR_MCT) | ECMR_PRM;
+	} else if (mdp->cd->tsu) {
+		struct netdev_hw_addr *ha;
+		netdev_for_each_mc_addr(ha, ndev) {
+			if (mcast_all && is_multicast_ether_addr(ha->addr))
+				continue;
+
+			if (sh_eth_tsu_add_entry(ndev, ha->addr) < 0) {
+				if (!mcast_all) {
+					sh_eth_tsu_purge_mcast(ndev);
+					ecmr_bits &= ~ECMR_MCT;
+					mcast_all = 1;
+				}
+			}
+		}
 	} else {
 		/* Normal, unicast/broadcast-only mode. */
-		sh_eth_write(ndev, (sh_eth_read(ndev, ECMR) & ~ECMR_PRM) |
-				ECMR_MCT, ECMR);
+		ecmr_bits = (ecmr_bits & ~ECMR_PRM) | ECMR_MCT;
 	}
+
+	/* update the ethernet mode */
+	sh_eth_write(ndev, ecmr_bits, ECMR);
+
+	spin_unlock_irqrestore(&mdp->lock, flags);
+}
+
+static int sh_eth_get_vtag_index(struct sh_eth_private *mdp)
+{
+	if (!mdp->port)
+		return TSU_VTAG0;
+	else
+		return TSU_VTAG1;
+}
+
+static int sh_eth_vlan_rx_add_vid(struct net_device *ndev, u16 vid)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	int vtag_reg_index = sh_eth_get_vtag_index(mdp);
+
+	if (unlikely(!mdp->cd->tsu))
+		return -EPERM;
+
+	/* No filtering if vid = 0 */
+	if (!vid)
+		return 0;
+
+	mdp->vlan_num_ids++;
+
+	/*
+	 * The controller has one VLAN tag HW filter. So, if the filter is
+	 * already enabled, the driver disables it and the filte
+	 */
+	if (mdp->vlan_num_ids > 1) {
+		/* disable VLAN filter */
+		sh_eth_tsu_write(mdp, 0, vtag_reg_index);
+		return 0;
+	}
+
+	sh_eth_tsu_write(mdp, TSU_VTAG_ENABLE | (vid & TSU_VTAG_VID_MASK),
+			 vtag_reg_index);
+
+	return 0;
+}
+
+static int sh_eth_vlan_rx_kill_vid(struct net_device *ndev, u16 vid)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	int vtag_reg_index = sh_eth_get_vtag_index(mdp);
+
+	if (unlikely(!mdp->cd->tsu))
+		return -EPERM;
+
+	/* No filtering if vid = 0 */
+	if (!vid)
+		return 0;
+
+	mdp->vlan_num_ids--;
+	sh_eth_tsu_write(mdp, 0, vtag_reg_index);
+
+	return 0;
 }
 #endif /* SH_ETH_HAS_TSU */
 
@@ -1768,6 +2093,8 @@ static const struct net_device_ops sh_eth_netdev_ops = {
 	.ndo_get_stats		= sh_eth_get_stats,
 #if defined(SH_ETH_HAS_TSU)
 	.ndo_set_rx_mode	= sh_eth_set_multicast_list,
+	.ndo_vlan_rx_add_vid	= sh_eth_vlan_rx_add_vid,
+	.ndo_vlan_rx_kill_vid	= sh_eth_vlan_rx_kill_vid,
 #endif
 	.ndo_tx_timeout		= sh_eth_tx_timeout,
 	.ndo_do_ioctl		= sh_eth_do_ioctl,
@@ -1794,7 +2121,6 @@ static int sh_eth_drv_probe(struct platform_device *pdev)
 
 	ndev = alloc_etherdev(sizeof(struct sh_eth_private));
 	if (!ndev) {
-		dev_err(&pdev->dev, "Could not allocate device.\n");
 		ret = -ENOMEM;
 		goto out;
 	}
@@ -1862,18 +2188,22 @@ static int sh_eth_drv_probe(struct platform_device *pdev)
 	/* read and set MAC address */
 	read_mac_address(ndev, pd->mac_addr);
 
-	/* First device only init */
-	if (!devno) {
-		if (mdp->cd->tsu) {
-			struct resource *rtsu;
-			rtsu = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-			if (!rtsu) {
-				dev_err(&pdev->dev, "Not found TSU resource\n");
-				goto out_release;
-			}
-			mdp->tsu_addr = ioremap(rtsu->start,
-						resource_size(rtsu));
+	/* ioremap the TSU registers */
+	if (mdp->cd->tsu) {
+		struct resource *rtsu;
+		rtsu = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+		if (!rtsu) {
+			dev_err(&pdev->dev, "Not found TSU resource\n");
+			goto out_release;
 		}
+		mdp->tsu_addr = ioremap(rtsu->start,
+					resource_size(rtsu));
+		mdp->port = devno % 2;
+		ndev->features = NETIF_F_HW_VLAN_FILTER;
+	}
+
+	/* initialize first or needed device */
+	if (!devno || pd->needs_init) {
 		if (mdp->cd->chip_reset)
 			mdp->cd->chip_reset(ndev);
 
@@ -1922,7 +2252,8 @@ static int sh_eth_drv_remove(struct platform_device *pdev)
 	struct net_device *ndev = platform_get_drvdata(pdev);
 	struct sh_eth_private *mdp = netdev_priv(ndev);
 
-	iounmap(mdp->tsu_addr);
+	if (mdp->cd->tsu)
+		iounmap(mdp->tsu_addr);
 	sh_mdio_release(ndev);
 	unregister_netdev(ndev);
 	pm_runtime_disable(&pdev->dev);