From 541ca41d5211770b52ab3b422a939185b5d7348e Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Thu, 20 May 2010 11:15:46 +0200
Subject: [PATCH 01/34] Fixed compiler warning.

---
 devices/e100-2.6.31-ethercat.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/devices/e100-2.6.31-ethercat.c b/devices/e100-2.6.31-ethercat.c
index e5f01597..b48821ac 100644
--- a/devices/e100-2.6.31-ethercat.c
+++ b/devices/e100-2.6.31-ethercat.c
@@ -704,7 +704,7 @@ static void e100_enable_irq(struct nic *nic)
 
 static void e100_disable_irq(struct nic *nic)
 {
-	unsigned long flags;
+	unsigned long flags = 0;
 
 	if (!nic->ecdev)
 		spin_lock_irqsave(&nic->cmd_lock, flags);

From b351036b1e026f702e50408282619fc31a124326 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Thu, 20 May 2010 11:16:37 +0200
Subject: [PATCH 02/34] =?UTF-8?q?Added=20r8169=20port=20based=20on=202.6.3?=
 =?UTF-8?q?1.13=20by=20M.=20L=C3=BCscher.?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 devices/r8169-2.6.31-ethercat.c | 553 ++++++++++++++++++--------------
 devices/r8169-2.6.31-orig.c     | 538 +++++++++++++++++--------------
 2 files changed, 614 insertions(+), 477 deletions(-)

diff --git a/devices/r8169-2.6.31-ethercat.c b/devices/r8169-2.6.31-ethercat.c
index 347bba43..e1da8d9e 100644
--- a/devices/r8169-2.6.31-ethercat.c
+++ b/devices/r8169-2.6.31-ethercat.c
@@ -56,9 +56,6 @@
 #define TX_BUFFS_AVAIL(tp) \
 	(tp->dirty_tx + NUM_TX_DESC - tp->cur_tx - 1)
 
-/* Maximum events (Rx packets, etc.) to handle at each interrupt. */
-static const int max_interrupt_work = 20;
-
 /* Maximum number of multicast addresses to filter (vs. Rx-all-multicast).
    The RTL chips use a 64 element hash table based on the Ethernet CRC. */
 static const int multicast_filter_limit = 32;
@@ -71,7 +68,6 @@ static const int multicast_filter_limit = 32;
 #define RX_DMA_BURST	6	/* Maximum PCI burst, '6' is 1024 */
 #define TX_DMA_BURST	6	/* Maximum PCI burst, '6' is 1024 */
 #define EarlyTxThld	0x3F	/* 0x3F means NO early transmit */
-#define RxPacketMaxSize	0x3FE8	/* 16K - 1 - ETH_HLEN - VLAN - CRC... */
 #define SafeMtu		0x1c20	/* ... actually life sucks beyond ~7k */
 #define InterFrameGap	0x03	/* 3 means InterFrameGap = the shortest one */
 
@@ -99,6 +95,7 @@ static const int multicast_filter_limit = 32;
 #define RTL_R32(reg)		((unsigned long) readl (ioaddr + (reg)))
 
 enum mac_version {
+	RTL_GIGA_MAC_NONE   = 0x00,
 	RTL_GIGA_MAC_VER_01 = 0x01, // 8169
 	RTL_GIGA_MAC_VER_02 = 0x02, // 8169S
 	RTL_GIGA_MAC_VER_03 = 0x03, // 8110S
@@ -443,6 +440,22 @@ enum features {
 	RTL_FEATURE_GMII	= (1 << 2),
 };
 
+struct rtl8169_counters {
+	__le64	tx_packets;
+	__le64	rx_packets;
+	__le64	tx_errors;
+	__le32	rx_errors;
+	__le16	rx_missed;
+	__le16	align_errors;
+	__le32	tx_one_collision;
+	__le32	tx_multi_collision;
+	__le64	rx_unicast;
+	__le64	rx_broadcast;
+	__le32	rx_multicast;
+	__le16	tx_aborted;
+	__le16	tx_underun;
+};
+
 struct rtl8169_private {
 	void __iomem *mmio_addr;	/* memory map physical address */
 	struct pci_dev *pci_dev;	/* Index of PCI device */
@@ -469,7 +482,6 @@ struct rtl8169_private {
 	u16 intr_event;
 	u16 napi_event;
 	u16 intr_mask;
-	int phy_auto_nego_reg;
 	int phy_1000_ctrl_reg;
 #ifdef CONFIG_R8169_VLAN
 	struct vlan_group *vlgrp;
@@ -480,11 +492,13 @@ struct rtl8169_private {
 	void (*hw_start)(struct net_device *);
 	unsigned int (*phy_reset_pending)(void __iomem *);
 	unsigned int (*link_ok)(void __iomem *);
+	int (*do_ioctl)(struct rtl8169_private *tp, struct mii_ioctl_data *data, int cmd);
 	int pcie_cap;
 	struct delayed_work task;
 	unsigned features;
 
 	struct mii_if_info mii;
+	struct rtl8169_counters counters;
 
 	ec_device_t *ecdev;
 	unsigned long ec_watchdog_jiffies;
@@ -840,76 +854,81 @@ static int rtl8169_set_speed_xmii(struct net_device *dev,
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
 	void __iomem *ioaddr = tp->mmio_addr;
-	int auto_nego, giga_ctrl;
-
-	auto_nego = mdio_read(ioaddr, MII_ADVERTISE);
-	auto_nego &= ~(ADVERTISE_10HALF | ADVERTISE_10FULL |
-		       ADVERTISE_100HALF | ADVERTISE_100FULL);
-	giga_ctrl = mdio_read(ioaddr, MII_CTRL1000);
-	giga_ctrl &= ~(ADVERTISE_1000FULL | ADVERTISE_1000HALF);
+	int giga_ctrl, bmcr;
 
 	if (autoneg == AUTONEG_ENABLE) {
+		int auto_nego;
+
+		auto_nego = mdio_read(ioaddr, MII_ADVERTISE);
 		auto_nego |= (ADVERTISE_10HALF | ADVERTISE_10FULL |
 			      ADVERTISE_100HALF | ADVERTISE_100FULL);
-		giga_ctrl |= ADVERTISE_1000FULL | ADVERTISE_1000HALF;
-	} else {
-		if (speed == SPEED_10)
-			auto_nego |= ADVERTISE_10HALF | ADVERTISE_10FULL;
-		else if (speed == SPEED_100)
-			auto_nego |= ADVERTISE_100HALF | ADVERTISE_100FULL;
-		else if (speed == SPEED_1000)
+		auto_nego |= ADVERTISE_PAUSE_CAP | ADVERTISE_PAUSE_ASYM;
+
+		giga_ctrl = mdio_read(ioaddr, MII_CTRL1000);
+		giga_ctrl &= ~(ADVERTISE_1000FULL | ADVERTISE_1000HALF);
+
+		/* The 8100e/8101e/8102e do Fast Ethernet only. */
+		if ((tp->mac_version != RTL_GIGA_MAC_VER_07) &&
+		    (tp->mac_version != RTL_GIGA_MAC_VER_08) &&
+		    (tp->mac_version != RTL_GIGA_MAC_VER_09) &&
+		    (tp->mac_version != RTL_GIGA_MAC_VER_10) &&
+		    (tp->mac_version != RTL_GIGA_MAC_VER_13) &&
+		    (tp->mac_version != RTL_GIGA_MAC_VER_14) &&
+		    (tp->mac_version != RTL_GIGA_MAC_VER_15) &&
+		    (tp->mac_version != RTL_GIGA_MAC_VER_16)) {
 			giga_ctrl |= ADVERTISE_1000FULL | ADVERTISE_1000HALF;
-
-		if (duplex == DUPLEX_HALF)
-			auto_nego &= ~(ADVERTISE_10FULL | ADVERTISE_100FULL);
-
-		if (duplex == DUPLEX_FULL)
-			auto_nego &= ~(ADVERTISE_10HALF | ADVERTISE_100HALF);
-
-		/* This tweak comes straight from Realtek's driver. */
-		if ((speed == SPEED_100) && (duplex == DUPLEX_HALF) &&
-		    ((tp->mac_version == RTL_GIGA_MAC_VER_13) ||
-		     (tp->mac_version == RTL_GIGA_MAC_VER_16))) {
-			auto_nego = ADVERTISE_100HALF | ADVERTISE_CSMA;
-		}
-	}
-
-	/* The 8100e/8101e/8102e do Fast Ethernet only. */
-	if ((tp->mac_version == RTL_GIGA_MAC_VER_07) ||
-	    (tp->mac_version == RTL_GIGA_MAC_VER_08) ||
-	    (tp->mac_version == RTL_GIGA_MAC_VER_09) ||
-	    (tp->mac_version == RTL_GIGA_MAC_VER_10) ||
-	    (tp->mac_version == RTL_GIGA_MAC_VER_13) ||
-	    (tp->mac_version == RTL_GIGA_MAC_VER_14) ||
-	    (tp->mac_version == RTL_GIGA_MAC_VER_15) ||
-	    (tp->mac_version == RTL_GIGA_MAC_VER_16)) {
-		if ((giga_ctrl & (ADVERTISE_1000FULL | ADVERTISE_1000HALF)) &&
-		    netif_msg_link(tp)) {
+		} else if (netif_msg_link(tp)) {
 			printk(KERN_INFO "%s: PHY does not support 1000Mbps.\n",
 			       dev->name);
 		}
-		giga_ctrl &= ~(ADVERTISE_1000FULL | ADVERTISE_1000HALF);
-	}
 
-	auto_nego |= ADVERTISE_PAUSE_CAP | ADVERTISE_PAUSE_ASYM;
+		bmcr = BMCR_ANENABLE | BMCR_ANRESTART;
+
+		if ((tp->mac_version == RTL_GIGA_MAC_VER_11) ||
+		    (tp->mac_version == RTL_GIGA_MAC_VER_12) ||
+		    (tp->mac_version >= RTL_GIGA_MAC_VER_17)) {
+			/*
+			 * Wake up the PHY.
+			 * Vendor specific (0x1f) and reserved (0x0e) MII
+			 * registers.
+			 */
+			mdio_write(ioaddr, 0x1f, 0x0000);
+			mdio_write(ioaddr, 0x0e, 0x0000);
+		}
+
+		mdio_write(ioaddr, MII_ADVERTISE, auto_nego);
+		mdio_write(ioaddr, MII_CTRL1000, giga_ctrl);
+	} else {
+		giga_ctrl = 0;
+
+		if (speed == SPEED_10)
+			bmcr = 0;
+		else if (speed == SPEED_100)
+			bmcr = BMCR_SPEED100;
+		else
+			return -EINVAL;
+
+		if (duplex == DUPLEX_FULL)
+			bmcr |= BMCR_FULLDPLX;
 
-	if ((tp->mac_version == RTL_GIGA_MAC_VER_11) ||
-	    (tp->mac_version == RTL_GIGA_MAC_VER_12) ||
-	    (tp->mac_version >= RTL_GIGA_MAC_VER_17)) {
-		/*
-		 * Wake up the PHY.
-		 * Vendor specific (0x1f) and reserved (0x0e) MII registers.
-		 */
 		mdio_write(ioaddr, 0x1f, 0x0000);
-		mdio_write(ioaddr, 0x0e, 0x0000);
 	}
 
-	tp->phy_auto_nego_reg = auto_nego;
 	tp->phy_1000_ctrl_reg = giga_ctrl;
 
-	mdio_write(ioaddr, MII_ADVERTISE, auto_nego);
-	mdio_write(ioaddr, MII_CTRL1000, giga_ctrl);
-	mdio_write(ioaddr, MII_BMCR, BMCR_ANENABLE | BMCR_ANRESTART);
+	mdio_write(ioaddr, MII_BMCR, bmcr);
+
+	if ((tp->mac_version == RTL_GIGA_MAC_VER_02) ||
+	    (tp->mac_version == RTL_GIGA_MAC_VER_03)) {
+		if ((speed == SPEED_100) && (autoneg != AUTONEG_ENABLE)) {
+			mdio_write(ioaddr, 0x17, 0x2138);
+			mdio_write(ioaddr, 0x0e, 0x0260);
+		} else {
+			mdio_write(ioaddr, 0x17, 0x2108);
+			mdio_write(ioaddr, 0x0e, 0x0000);
+		}
+	}
+
 	return 0;
 }
 
@@ -1113,22 +1132,6 @@ static const char rtl8169_gstrings[][ETH_GSTRING_LEN] = {
 	"tx_underrun",
 };
 
-struct rtl8169_counters {
-	__le64	tx_packets;
-	__le64	rx_packets;
-	__le64	tx_errors;
-	__le32	rx_errors;
-	__le16	rx_missed;
-	__le16	align_errors;
-	__le32	tx_one_collision;
-	__le32	tx_multi_collision;
-	__le64	rx_unicast;
-	__le64	rx_broadcast;
-	__le32	rx_multicast;
-	__le16	tx_aborted;
-	__le16	tx_underun;
-};
-
 static int rtl8169_get_sset_count(struct net_device *dev, int sset)
 {
 	switch (sset) {
@@ -1139,51 +1142,70 @@ static int rtl8169_get_sset_count(struct net_device *dev, int sset)
 	}
 }
 
-static void rtl8169_get_ethtool_stats(struct net_device *dev,
-				      struct ethtool_stats *stats, u64 *data)
+static void rtl8169_update_counters(struct net_device *dev)
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
 	void __iomem *ioaddr = tp->mmio_addr;
 	struct rtl8169_counters *counters;
 	dma_addr_t paddr;
 	u32 cmd;
+	int wait = 1000;
 
-	ASSERT_RTNL();
+	/*
+	 * Some chips are unable to dump tally counters when the receiver
+	 * is disabled.
+	 */
+	if ((RTL_R8(ChipCmd) & CmdRxEnb) == 0)
+		return;
 
 	counters = pci_alloc_consistent(tp->pci_dev, sizeof(*counters), &paddr);
 	if (!counters)
 		return;
 
 	RTL_W32(CounterAddrHigh, (u64)paddr >> 32);
-	cmd = (u64)paddr & DMA_32BIT_MASK;
+	cmd = (u64)paddr & DMA_BIT_MASK(32);
 	RTL_W32(CounterAddrLow, cmd);
 	RTL_W32(CounterAddrLow, cmd | CounterDump);
 
-	while (RTL_R32(CounterAddrLow) & CounterDump) {
-		if (msleep_interruptible(1))
+	while (wait--) {
+		if ((RTL_R32(CounterAddrLow) & CounterDump) == 0) {
+			/* copy updated counters */
+			memcpy(&tp->counters, counters, sizeof(*counters));
 			break;
+		}
+		udelay(10);
 	}
 
 	RTL_W32(CounterAddrLow, 0);
 	RTL_W32(CounterAddrHigh, 0);
 
-	data[0] = le64_to_cpu(counters->tx_packets);
-	data[1] = le64_to_cpu(counters->rx_packets);
-	data[2] = le64_to_cpu(counters->tx_errors);
-	data[3] = le32_to_cpu(counters->rx_errors);
-	data[4] = le16_to_cpu(counters->rx_missed);
-	data[5] = le16_to_cpu(counters->align_errors);
-	data[6] = le32_to_cpu(counters->tx_one_collision);
-	data[7] = le32_to_cpu(counters->tx_multi_collision);
-	data[8] = le64_to_cpu(counters->rx_unicast);
-	data[9] = le64_to_cpu(counters->rx_broadcast);
-	data[10] = le32_to_cpu(counters->rx_multicast);
-	data[11] = le16_to_cpu(counters->tx_aborted);
-	data[12] = le16_to_cpu(counters->tx_underun);
-
 	pci_free_consistent(tp->pci_dev, sizeof(*counters), counters, paddr);
 }
 
+static void rtl8169_get_ethtool_stats(struct net_device *dev,
+				      struct ethtool_stats *stats, u64 *data)
+{
+	struct rtl8169_private *tp = netdev_priv(dev);
+
+	ASSERT_RTNL();
+
+	rtl8169_update_counters(dev);
+
+	data[0] = le64_to_cpu(tp->counters.tx_packets);
+	data[1] = le64_to_cpu(tp->counters.rx_packets);
+	data[2] = le64_to_cpu(tp->counters.tx_errors);
+	data[3] = le32_to_cpu(tp->counters.rx_errors);
+	data[4] = le16_to_cpu(tp->counters.rx_missed);
+	data[5] = le16_to_cpu(tp->counters.align_errors);
+	data[6] = le32_to_cpu(tp->counters.tx_one_collision);
+	data[7] = le32_to_cpu(tp->counters.tx_multi_collision);
+	data[8] = le64_to_cpu(tp->counters.rx_unicast);
+	data[9] = le64_to_cpu(tp->counters.rx_broadcast);
+	data[10] = le32_to_cpu(tp->counters.rx_multicast);
+	data[11] = le16_to_cpu(tp->counters.tx_aborted);
+	data[12] = le16_to_cpu(tp->counters.tx_underun);
+}
+
 static void rtl8169_get_strings(struct net_device *dev, u32 stringset, u8 *data)
 {
 	switch(stringset) {
@@ -1289,7 +1311,8 @@ static void rtl8169_get_mac_version(struct rtl8169_private *tp,
 		{ 0xfc800000, 0x00800000,	RTL_GIGA_MAC_VER_02 },
 		{ 0xfc800000, 0x00000000,	RTL_GIGA_MAC_VER_01 },
 
-		{ 0x00000000, 0x00000000,	RTL_GIGA_MAC_VER_01 }	/* Catch-all */
+		/* Catch-all */
+		{ 0x00000000, 0x00000000,	RTL_GIGA_MAC_NONE   }
 	}, *p = mac_info;
 	u32 reg;
 
@@ -1297,12 +1320,6 @@ static void rtl8169_get_mac_version(struct rtl8169_private *tp,
 	while ((reg & p->mask) != p->val)
 		p++;
 	tp->mac_version = p->mac_version;
-
-	if (p->mask == 0x00000000) {
-		struct pci_dev *pdev = tp->pci_dev;
-
-		dev_info(&pdev->dev, "unknown MAC (%08x)\n", reg);
-	}
 }
 
 static void rtl8169_print_mac_version(struct rtl8169_private *tp)
@@ -1846,9 +1863,11 @@ static int rtl8169_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 	struct rtl8169_private *tp = netdev_priv(dev);
 	struct mii_ioctl_data *data = if_mii(ifr);
 
-	if (!netif_running(dev))
-		return -ENODEV;
+	return netif_running(dev) ? tp->do_ioctl(tp, data, cmd) : -ENODEV;
+}
 
+static int rtl_xmii_ioctl(struct rtl8169_private *tp, struct mii_ioctl_data *data, int cmd)
+{
 	switch (cmd) {
 	case SIOCGMIIPHY:
 		data->phy_id = 32; /* Internal PHY */
@@ -1867,6 +1886,11 @@ static int rtl8169_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 	return -EOPNOTSUPP;
 }
 
+static int rtl_tbi_ioctl(struct rtl8169_private *tp, struct mii_ioctl_data *data, int cmd)
+{
+	return -EOPNOTSUPP;
+}
+
 static const struct rtl_cfg_info {
 	void (*hw_start)(struct net_device *);
 	unsigned int region;
@@ -1874,6 +1898,7 @@ static const struct rtl_cfg_info {
 	u16 intr_event;
 	u16 napi_event;
 	unsigned features;
+	u8 default_ver;
 } rtl_cfg_infos [] = {
 	[RTL_CFG_0] = {
 		.hw_start	= rtl_hw_start_8169,
@@ -1882,7 +1907,8 @@ static const struct rtl_cfg_info {
 		.intr_event	= SYSErr | LinkChg | RxOverflow |
 				  RxFIFOOver | TxErr | TxOK | RxOK | RxErr,
 		.napi_event	= RxFIFOOver | TxErr | TxOK | RxOK | RxOverflow,
-		.features	= RTL_FEATURE_GMII
+		.features	= RTL_FEATURE_GMII,
+		.default_ver	= RTL_GIGA_MAC_VER_01,
 	},
 	[RTL_CFG_1] = {
 		.hw_start	= rtl_hw_start_8168,
@@ -1891,7 +1917,8 @@ static const struct rtl_cfg_info {
 		.intr_event	= SYSErr | LinkChg | RxOverflow |
 				  TxErr | TxOK | RxOK | RxErr,
 		.napi_event	= TxErr | TxOK | RxOK | RxOverflow,
-		.features	= RTL_FEATURE_GMII | RTL_FEATURE_MSI
+		.features	= RTL_FEATURE_GMII | RTL_FEATURE_MSI,
+		.default_ver	= RTL_GIGA_MAC_VER_11,
 	},
 	[RTL_CFG_2] = {
 		.hw_start	= rtl_hw_start_8101,
@@ -1900,7 +1927,8 @@ static const struct rtl_cfg_info {
 		.intr_event	= SYSErr | LinkChg | RxOverflow | PCSTimeout |
 				  RxFIFOOver | TxErr | TxOK | RxOK | RxErr,
 		.napi_event	= RxFIFOOver | TxErr | TxOK | RxOK | RxOverflow,
-		.features	= RTL_FEATURE_MSI
+		.features	= RTL_FEATURE_MSI,
+		.default_ver	= RTL_GIGA_MAC_VER_13,
 	}
 };
 
@@ -1932,6 +1960,26 @@ static void rtl_disable_msi(struct pci_dev *pdev, struct rtl8169_private *tp)
 	}
 }
 
+static const struct net_device_ops rtl8169_netdev_ops = {
+	.ndo_open		= rtl8169_open,
+	.ndo_stop		= rtl8169_close,
+	.ndo_get_stats		= rtl8169_get_stats,
+	.ndo_start_xmit		= rtl8169_start_xmit,
+	.ndo_tx_timeout		= rtl8169_tx_timeout,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_change_mtu		= rtl8169_change_mtu,
+	.ndo_set_mac_address	= rtl_set_mac_address,
+	.ndo_do_ioctl		= rtl8169_ioctl,
+	.ndo_set_multicast_list	= rtl_set_rx_mode,
+#ifdef CONFIG_R8169_VLAN
+	.ndo_vlan_rx_register	= rtl8169_vlan_rx_register,
+#endif
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller	= rtl8169_netpoll,
+#endif
+
+};
+
 static int __devinit
 rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
@@ -1958,6 +2006,7 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	}
 
 	SET_NETDEV_DEV(dev, &pdev->dev);
+	dev->netdev_ops = &rtl8169_netdev_ops;
 	tp = netdev_priv(dev);
 	tp->dev = dev;
 	tp->pci_dev = pdev;
@@ -2014,11 +2063,11 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	tp->cp_cmd = PCIMulRW | RxChkSum;
 
 	if ((sizeof(dma_addr_t) > 4) &&
-	    !pci_set_dma_mask(pdev, DMA_64BIT_MASK) && use_dac) {
+	    !pci_set_dma_mask(pdev, DMA_BIT_MASK(64)) && use_dac) {
 		tp->cp_cmd |= PCIDAC;
 		dev->features |= NETIF_F_HIGHDMA;
 	} else {
-		rc = pci_set_dma_mask(pdev, DMA_32BIT_MASK);
+		rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
 		if (rc < 0) {
 			if (netif_msg_probe(tp)) {
 				dev_err(&pdev->dev,
@@ -2028,8 +2077,6 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		}
 	}
 
-	pci_set_master(pdev);
-
 	/* ioremap MMIO region */
 	ioaddr = ioremap(pci_resource_start(pdev, region), R8169_REGS_SIZE);
 	if (!ioaddr) {
@@ -2057,9 +2104,20 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	RTL_W16(IntrStatus, 0xffff);
 
+	pci_set_master(pdev);
+
 	/* Identify chip attached to board */
 	rtl8169_get_mac_version(tp, ioaddr);
 
+	/* Use appropriate default if unknown */
+	if (tp->mac_version == RTL_GIGA_MAC_NONE) {
+		if (netif_msg_probe(tp)) {
+			dev_notice(&pdev->dev,
+				   "unknown MAC, using family default\n");
+		}
+		tp->mac_version = cfg->default_ver;
+	}
+
 	rtl8169_print_mac_version(tp);
 
 	for (i = 0; i < ARRAY_SIZE(rtl_chip_info); i++) {
@@ -2067,13 +2125,9 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 			break;
 	}
 	if (i == ARRAY_SIZE(rtl_chip_info)) {
-		/* Unknown chip: assume array element #0, original RTL-8169 */
-		if (netif_msg_probe(tp)) {
-			dev_printk(KERN_DEBUG, &pdev->dev,
-				"unknown chip version, assuming %s\n",
-				rtl_chip_info[0].name);
-		}
-		i = 0;
+		dev_err(&pdev->dev,
+			"driver bug, MAC version not found in rtl_chip_info\n");
+		goto err_out_msi_5;
 	}
 	tp->chipset = i;
 
@@ -2094,6 +2148,7 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		tp->phy_reset_enable = rtl8169_tbi_reset_enable;
 		tp->phy_reset_pending = rtl8169_tbi_reset_pending;
 		tp->link_ok = rtl8169_tbi_link_ok;
+		tp->do_ioctl = rtl_tbi_ioctl;
 
 		tp->phy_1000_ctrl_reg = ADVERTISE_1000FULL; /* Implied by TBI */
 	} else {
@@ -2102,8 +2157,7 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		tp->phy_reset_enable = rtl8169_xmii_reset_enable;
 		tp->phy_reset_pending = rtl8169_xmii_reset_pending;
 		tp->link_ok = rtl8169_xmii_link_ok;
-
-		dev->do_ioctl = rtl8169_ioctl;
+		tp->do_ioctl = rtl_xmii_ioctl;
 	}
 
 	spin_lock_init(&tp->lock);
@@ -2115,28 +2169,15 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		dev->dev_addr[i] = RTL_R8(MAC0 + i);
 	memcpy(dev->perm_addr, dev->dev_addr, dev->addr_len);
 
-	dev->open = rtl8169_open;
-	dev->hard_start_xmit = rtl8169_start_xmit;
-	dev->get_stats = rtl8169_get_stats;
 	SET_ETHTOOL_OPS(dev, &rtl8169_ethtool_ops);
-	dev->stop = rtl8169_close;
-	dev->tx_timeout = rtl8169_tx_timeout;
-	dev->set_multicast_list = rtl_set_rx_mode;
 	dev->watchdog_timeo = RTL8169_TX_TIMEOUT;
 	dev->irq = pdev->irq;
 	dev->base_addr = (unsigned long) ioaddr;
-	dev->change_mtu = rtl8169_change_mtu;
-	dev->set_mac_address = rtl_set_mac_address;
 
 	netif_napi_add(dev, &tp->napi, rtl8169_poll, R8169_NAPI_WEIGHT);
 
 #ifdef CONFIG_R8169_VLAN
 	dev->features |= NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
-	dev->vlan_rx_register = rtl8169_vlan_rx_register;
-#endif
-
-#ifdef CONFIG_NET_POLL_CONTROLLER
-	dev->poll_controller = rtl8169_netpoll;
 #endif
 
 	tp->intr_mask = 0xffff;
@@ -2176,12 +2217,12 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	rtl8169_init_phy(dev, tp);
 	device_set_wakeup_enable(&pdev->dev, tp->features & RTL_FEATURE_WOL);
-
 	if (tp->ecdev && ecdev_open(tp->ecdev)) {
 		ecdev_withdraw(tp->ecdev);
 		goto err_out_msi_5;
 	}
 
+
 out:
 	return rc;
 
@@ -2264,8 +2305,8 @@ static int rtl8169_open(struct net_device *dev)
 			goto err_release_ring_2;
 
 		napi_enable(&tp->napi);
-	}
 
+	}
 	rtl_hw_start(dev);
 
 	rtl8169_request_timer(dev);
@@ -2342,9 +2383,9 @@ static void rtl_set_rx_tx_desc_registers(struct rtl8169_private *tp,
 	 * Switching from MMIO to I/O access fixes the issue as well.
 	 */
 	RTL_W32(TxDescStartAddrHigh, ((u64) tp->TxPhyAddr) >> 32);
-	RTL_W32(TxDescStartAddrLow, ((u64) tp->TxPhyAddr) & DMA_32BIT_MASK);
+	RTL_W32(TxDescStartAddrLow, ((u64) tp->TxPhyAddr) & DMA_BIT_MASK(32));
 	RTL_W32(RxDescAddrHigh, ((u64) tp->RxPhyAddr) >> 32);
-	RTL_W32(RxDescAddrLow, ((u64) tp->RxPhyAddr) & DMA_32BIT_MASK);
+	RTL_W32(RxDescAddrLow, ((u64) tp->RxPhyAddr) & DMA_BIT_MASK(32));
 }
 
 static u16 rtl_rw_cpluscmd(void __iomem *ioaddr)
@@ -2356,10 +2397,10 @@ static u16 rtl_rw_cpluscmd(void __iomem *ioaddr)
 	return cmd;
 }
 
-static void rtl_set_rx_max_size(void __iomem *ioaddr)
+static void rtl_set_rx_max_size(void __iomem *ioaddr, unsigned int rx_buf_sz)
 {
 	/* Low hurts. Let's disable the filtering. */
-	RTL_W16(RxMaxSize, 16383);
+	RTL_W16(RxMaxSize, rx_buf_sz);
 }
 
 static void rtl8169_set_magic_reg(void __iomem *ioaddr, unsigned mac_version)
@@ -2406,7 +2447,7 @@ static void rtl_hw_start_8169(struct net_device *dev)
 
 	RTL_W8(EarlyTxThres, EarlyTxThld);
 
-	rtl_set_rx_max_size(ioaddr);
+	rtl_set_rx_max_size(ioaddr, tp->rx_buf_sz);
 
 	if ((tp->mac_version == RTL_GIGA_MAC_VER_01) ||
 	    (tp->mac_version == RTL_GIGA_MAC_VER_02) ||
@@ -2668,7 +2709,7 @@ static void rtl_hw_start_8168(struct net_device *dev)
 
 	RTL_W8(EarlyTxThres, EarlyTxThld);
 
-	rtl_set_rx_max_size(ioaddr);
+	rtl_set_rx_max_size(ioaddr, tp->rx_buf_sz);
 
 	tp->cp_cmd |= RTL_R16(CPlusCmd) | PktCntrDisable | INTT_1;
 
@@ -2847,7 +2888,7 @@ static void rtl_hw_start_8101(struct net_device *dev)
 
 	RTL_W8(EarlyTxThres, EarlyTxThld);
 
-	rtl_set_rx_max_size(ioaddr);
+	rtl_set_rx_max_size(ioaddr, tp->rx_buf_sz);
 
 	tp->cp_cmd |= rtl_rw_cpluscmd(ioaddr) | PCIMulRW;
 
@@ -3260,13 +3301,6 @@ static int rtl8169_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		opts1 |= FirstFrag;
 	} else {
 		len = skb->len;
-
-		if (unlikely(len < ETH_ZLEN)) {
-			if (skb_padto(skb, ETH_ZLEN))
-				goto err_update_stats;
-			len = ETH_ZLEN;
-		}
-
 		opts1 |= FirstFrag | LastFrag;
 		tp->tx_skb[entry].skb = skb;
 	}
@@ -3283,8 +3317,6 @@ static int rtl8169_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	status = opts1 | len | (RingEnd * !((entry + 1) % NUM_TX_DESC));
 	txd->opts1 = cpu_to_le32(status);
 
-	dev->trans_start = jiffies;
-
 	tp->cur_tx += frags + 1;
 
 	smp_wmb();
@@ -3307,7 +3339,6 @@ err_stop:
 	if (!tp->ecdev)
 		netif_stop_queue(dev);
 	ret = NETDEV_TX_BUSY;
-err_update_stats:
 	dev->stats.tx_dropped++;
 	goto out;
 }
@@ -3390,7 +3421,7 @@ static void rtl8169_tx_interrupt(struct net_device *dev,
 
 		if (status & LastFrag) {
 			if (!tp->ecdev)
-				dev_kfree_skb_irq(tx_skb->skb);
+				dev_kfree_skb(tx_skb->skb);
 			tx_skb->skb = NULL;
 		}
 		dirty_tx++;
@@ -3547,7 +3578,6 @@ static int rtl8169_rx_interrupt(struct net_device *dev,
 					netif_receive_skb(skb);
 			}
 
-			dev->last_rx = jiffies;
 			dev->stats.rx_bytes += pkt_size;
 			dev->stats.rx_packets++;
 		}
@@ -3594,54 +3624,64 @@ static irqreturn_t rtl8169_interrupt(int irq, void *dev_instance)
 	int handled = 0;
 	int status;
 
+	/* loop handling interrupts until we have no new ones or
+	 * we hit a invalid/hotplug case.
+	 */
 	status = RTL_R16(IntrStatus);
+	while (status && status != 0xffff) {
+		handled = 1;
 
-	/* hotplug/major error/no more work/shared irq */
-	if ((status == 0xffff) || !status)
-		goto out;
-
-	handled = 1;
-
-	if (unlikely(!tp->ecdev && !netif_running(dev))) {
-		rtl8169_asic_down(ioaddr);
-		goto out;
-	}
-
-	status &= tp->intr_mask;
-	RTL_W16(IntrStatus,
-		(status & RxFIFOOver) ? (status | RxOverflow) : status);
-
-	if (!(status & tp->intr_event))
-		goto out;
-
-	/* Work around for rx fifo overflow */
-	if (unlikely(status & RxFIFOOver) &&
-	    (tp->mac_version == RTL_GIGA_MAC_VER_11)) {
-		netif_stop_queue(dev);
-		rtl8169_tx_timeout(dev);
-		goto out;
-	}
-
-	if (unlikely(status & SYSErr)) {
-		rtl8169_pcierr_interrupt(dev);
-		goto out;
-	}
-
-	if (status & LinkChg)
-		rtl8169_check_link_status(dev, tp, ioaddr);
-
-	if (status & tp->napi_event) {
-		RTL_W16(IntrMask, tp->intr_event & ~tp->napi_event);
-		tp->intr_mask = ~tp->napi_event;
-
-		if (likely(netif_rx_schedule_prep(dev, &tp->napi)))
-			__netif_rx_schedule(dev, &tp->napi);
-		else if (netif_msg_intr(tp)) {
-			printk(KERN_INFO "%s: interrupt %04x in poll\n",
-			       dev->name, status);
+		/* Handle all of the error cases first. These will reset
+		 * the chip, so just exit the loop.
+		 */
+		if (unlikely(!tp->ecdev && !netif_running(dev))) {
+			rtl8169_asic_down(ioaddr);
+			break;
 		}
+
+		/* Work around for rx fifo overflow */
+		if (unlikely(status & RxFIFOOver) &&
+		(tp->mac_version == RTL_GIGA_MAC_VER_11)) {
+			netif_stop_queue(dev);
+			rtl8169_tx_timeout(dev);
+			break;
+		}
+
+		if (unlikely(status & SYSErr)) {
+			rtl8169_pcierr_interrupt(dev);
+			break;
+		}
+
+		if (status & LinkChg)
+			rtl8169_check_link_status(dev, tp, ioaddr);
+
+		/* We need to see the lastest version of tp->intr_mask to
+		 * avoid ignoring an MSI interrupt and having to wait for
+		 * another event which may never come.
+		 */
+		smp_rmb();
+		if (status & tp->intr_mask & tp->napi_event) {
+			RTL_W16(IntrMask, tp->intr_event & ~tp->napi_event);
+			tp->intr_mask = ~tp->napi_event;
+
+			if (likely(napi_schedule_prep(&tp->napi)))
+				__napi_schedule(&tp->napi);
+			else if (netif_msg_intr(tp)) {
+				printk(KERN_INFO "%s: interrupt %04x in poll\n",
+				dev->name, status);
+			}
+		}
+
+		/* We only get a new MSI interrupt when all active irq
+		 * sources on the chip have been acknowledged. So, ack
+		 * everything we've seen and check if new sources have become
+		 * active to avoid blocking all interrupts from the chip.
+		 */
+		RTL_W16(IntrStatus,
+			(status & RxFIFOOver) ? (status | RxOverflow) : status);
+		status = RTL_R16(IntrStatus);
 	}
-out:
+
 	return IRQ_RETVAL(handled);
 }
 
@@ -3671,14 +3711,16 @@ static int rtl8169_poll(struct napi_struct *napi, int budget)
 	rtl8169_tx_interrupt(dev, tp, ioaddr);
 
 	if (work_done < budget) {
-		netif_rx_complete(dev, napi);
-		tp->intr_mask = 0xffff;
-		/*
-		 * 20040426: the barrier is not strictly required but the
-		 * behavior of the irq handler could be less predictable
-		 * without it. Btw, the lack of flush for the posted pci
-		 * write is safe - FR
+		napi_complete(napi);
+
+		/* We need for force the visibility of tp->intr_mask
+		 * for other CPUs, as we can loose an MSI interrupt
+		 * and potentially wait for a retransmit timeout if we don't.
+		 * The posted write to IntrMask is safe, as it will
+		 * eventually make it to the chip and we won't loose anything
+		 * until it does.
 		 */
+		tp->intr_mask = 0xffff;
 		smp_wmb();
 		RTL_W16(IntrMask, tp->intr_event);
 	}
@@ -3709,8 +3751,8 @@ static void rtl8169_down(struct net_device *dev)
 		netif_stop_queue(dev);
 
 		napi_disable(&tp->napi);
-	}
 
+	}
 core_down:
 	if (!tp->ecdev)
 		spin_lock_irq(&tp->lock);
@@ -3755,6 +3797,9 @@ static int rtl8169_close(struct net_device *dev)
 	struct rtl8169_private *tp = netdev_priv(dev);
 	struct pci_dev *pdev = tp->pci_dev;
 
+	/* update counters before going down */
+	rtl8169_update_counters(dev);
+
 	rtl8169_down(dev);
 
 	if (!tp->ecdev)
@@ -3849,52 +3894,40 @@ static struct net_device_stats *rtl8169_get_stats(struct net_device *dev)
 	return &dev->stats;
 }
 
-#ifdef CONFIG_PM
-
-static int rtl8169_suspend(struct pci_dev *pdev, pm_message_t state)
+static void rtl8169_net_suspend(struct net_device *dev)
 {
-	struct net_device *dev = pci_get_drvdata(pdev);
-	struct rtl8169_private *tp = netdev_priv(dev);
-	void __iomem *ioaddr = tp->mmio_addr;
-
-	if (tp->ecdev)
-		return -EBUSY;
-
 	if (!netif_running(dev))
-		goto out_pci_suspend;
+		return;
 
 	netif_device_detach(dev);
 	netif_stop_queue(dev);
+}
 
-	spin_lock_irq(&tp->lock);
+#ifdef CONFIG_PM
 
-	rtl8169_asic_down(ioaddr);
+static int rtl8169_suspend(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct net_device *dev = pci_get_drvdata(pdev);
+	struct rtl8169_private *tp = netdev_priv(dev);
+	
+	if (tp->ecdev)
+ 		return -EBUSY;
 
-	rtl8169_rx_missed(dev, ioaddr);
-
-	spin_unlock_irq(&tp->lock);
-
-out_pci_suspend:
-	pci_save_state(pdev);
-	pci_enable_wake(pdev, pci_choose_state(pdev, state),
-		(tp->features & RTL_FEATURE_WOL) ? 1 : 0);
-	pci_set_power_state(pdev, pci_choose_state(pdev, state));
+	rtl8169_net_suspend(dev);
 
 	return 0;
 }
 
-static int rtl8169_resume(struct pci_dev *pdev)
+static int rtl8169_resume(struct device *device)
 {
+	struct pci_dev *pdev = to_pci_dev(device);
 	struct net_device *dev = pci_get_drvdata(pdev);
 	struct rtl8169_private *tp = netdev_priv(dev);
-
+ 
 	if (tp->ecdev)
 		return -EBUSY;
 
-	pci_set_power_state(pdev, PCI_D0);
-	pci_restore_state(pdev);
-	pci_enable_wake(pdev, PCI_D0, 0);
-
 	if (!netif_running(dev))
 		goto out;
 
@@ -3905,23 +3938,59 @@ out:
 	return 0;
 }
 
+static struct dev_pm_ops rtl8169_pm_ops = {
+	.suspend = rtl8169_suspend,
+	.resume = rtl8169_resume,
+	.freeze = rtl8169_suspend,
+	.thaw = rtl8169_resume,
+	.poweroff = rtl8169_suspend,
+	.restore = rtl8169_resume,
+};
+
+#define RTL8169_PM_OPS	(&rtl8169_pm_ops)
+
+#else /* !CONFIG_PM */
+
+#define RTL8169_PM_OPS	NULL
+
+#endif /* !CONFIG_PM */
+
 static void rtl_shutdown(struct pci_dev *pdev)
 {
-	rtl8169_suspend(pdev, PMSG_SUSPEND);
-}
+	struct net_device *dev = pci_get_drvdata(pdev);
+	struct rtl8169_private *tp = netdev_priv(dev);
+	void __iomem *ioaddr = tp->mmio_addr;
 
-#endif /* CONFIG_PM */
+	rtl8169_net_suspend(dev);
+
+	spin_lock_irq(&tp->lock);
+
+	rtl8169_asic_down(ioaddr);
+
+	spin_unlock_irq(&tp->lock);
+
+	if (system_state == SYSTEM_POWER_OFF) {
+		/* WoL fails with some 8168 when the receiver is disabled. */
+		if (tp->features & RTL_FEATURE_WOL) {
+			pci_clear_master(pdev);
+
+			RTL_W8(ChipCmd, CmdRxEnb);
+			/* PCI commit */
+			RTL_R8(ChipCmd);
+		}
+
+		pci_wake_from_d3(pdev, true);
+		pci_set_power_state(pdev, PCI_D3hot);
+	}
+}
 
 static struct pci_driver rtl8169_pci_driver = {
 	.name		= MODULENAME,
 	.id_table	= rtl8169_pci_tbl,
 	.probe		= rtl8169_init_one,
 	.remove		= __devexit_p(rtl8169_remove_one),
-#ifdef CONFIG_PM
-	.suspend	= rtl8169_suspend,
-	.resume		= rtl8169_resume,
 	.shutdown	= rtl_shutdown,
-#endif
+	.driver.pm	= RTL8169_PM_OPS,
 };
 
 static int __init rtl8169_init_module(void)
diff --git a/devices/r8169-2.6.31-orig.c b/devices/r8169-2.6.31-orig.c
index 1aaa9436..b82780d8 100644
--- a/devices/r8169-2.6.31-orig.c
+++ b/devices/r8169-2.6.31-orig.c
@@ -51,9 +51,6 @@
 #define TX_BUFFS_AVAIL(tp) \
 	(tp->dirty_tx + NUM_TX_DESC - tp->cur_tx - 1)
 
-/* Maximum events (Rx packets, etc.) to handle at each interrupt. */
-static const int max_interrupt_work = 20;
-
 /* Maximum number of multicast addresses to filter (vs. Rx-all-multicast).
    The RTL chips use a 64 element hash table based on the Ethernet CRC. */
 static const int multicast_filter_limit = 32;
@@ -66,7 +63,6 @@ static const int multicast_filter_limit = 32;
 #define RX_DMA_BURST	6	/* Maximum PCI burst, '6' is 1024 */
 #define TX_DMA_BURST	6	/* Maximum PCI burst, '6' is 1024 */
 #define EarlyTxThld	0x3F	/* 0x3F means NO early transmit */
-#define RxPacketMaxSize	0x3FE8	/* 16K - 1 - ETH_HLEN - VLAN - CRC... */
 #define SafeMtu		0x1c20	/* ... actually life sucks beyond ~7k */
 #define InterFrameGap	0x03	/* 3 means InterFrameGap = the shortest one */
 
@@ -94,6 +90,7 @@ static const int multicast_filter_limit = 32;
 #define RTL_R32(reg)		((unsigned long) readl (ioaddr + (reg)))
 
 enum mac_version {
+	RTL_GIGA_MAC_NONE   = 0x00,
 	RTL_GIGA_MAC_VER_01 = 0x01, // 8169
 	RTL_GIGA_MAC_VER_02 = 0x02, // 8169S
 	RTL_GIGA_MAC_VER_03 = 0x03, // 8110S
@@ -437,6 +434,22 @@ enum features {
 	RTL_FEATURE_GMII	= (1 << 2),
 };
 
+struct rtl8169_counters {
+	__le64	tx_packets;
+	__le64	rx_packets;
+	__le64	tx_errors;
+	__le32	rx_errors;
+	__le16	rx_missed;
+	__le16	align_errors;
+	__le32	tx_one_collision;
+	__le32	tx_multi_collision;
+	__le64	rx_unicast;
+	__le64	rx_broadcast;
+	__le32	rx_multicast;
+	__le16	tx_aborted;
+	__le16	tx_underun;
+};
+
 struct rtl8169_private {
 	void __iomem *mmio_addr;	/* memory map physical address */
 	struct pci_dev *pci_dev;	/* Index of PCI device */
@@ -463,7 +476,6 @@ struct rtl8169_private {
 	u16 intr_event;
 	u16 napi_event;
 	u16 intr_mask;
-	int phy_auto_nego_reg;
 	int phy_1000_ctrl_reg;
 #ifdef CONFIG_R8169_VLAN
 	struct vlan_group *vlgrp;
@@ -474,11 +486,13 @@ struct rtl8169_private {
 	void (*hw_start)(struct net_device *);
 	unsigned int (*phy_reset_pending)(void __iomem *);
 	unsigned int (*link_ok)(void __iomem *);
+	int (*do_ioctl)(struct rtl8169_private *tp, struct mii_ioctl_data *data, int cmd);
 	int pcie_cap;
 	struct delayed_work task;
 	unsigned features;
 
 	struct mii_if_info mii;
+	struct rtl8169_counters counters;
 };
 
 MODULE_AUTHOR("Realtek and the Linux r8169 crew <netdev@vger.kernel.org>");
@@ -826,76 +840,81 @@ static int rtl8169_set_speed_xmii(struct net_device *dev,
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
 	void __iomem *ioaddr = tp->mmio_addr;
-	int auto_nego, giga_ctrl;
-
-	auto_nego = mdio_read(ioaddr, MII_ADVERTISE);
-	auto_nego &= ~(ADVERTISE_10HALF | ADVERTISE_10FULL |
-		       ADVERTISE_100HALF | ADVERTISE_100FULL);
-	giga_ctrl = mdio_read(ioaddr, MII_CTRL1000);
-	giga_ctrl &= ~(ADVERTISE_1000FULL | ADVERTISE_1000HALF);
+	int giga_ctrl, bmcr;
 
 	if (autoneg == AUTONEG_ENABLE) {
+		int auto_nego;
+
+		auto_nego = mdio_read(ioaddr, MII_ADVERTISE);
 		auto_nego |= (ADVERTISE_10HALF | ADVERTISE_10FULL |
 			      ADVERTISE_100HALF | ADVERTISE_100FULL);
-		giga_ctrl |= ADVERTISE_1000FULL | ADVERTISE_1000HALF;
-	} else {
-		if (speed == SPEED_10)
-			auto_nego |= ADVERTISE_10HALF | ADVERTISE_10FULL;
-		else if (speed == SPEED_100)
-			auto_nego |= ADVERTISE_100HALF | ADVERTISE_100FULL;
-		else if (speed == SPEED_1000)
+		auto_nego |= ADVERTISE_PAUSE_CAP | ADVERTISE_PAUSE_ASYM;
+
+		giga_ctrl = mdio_read(ioaddr, MII_CTRL1000);
+		giga_ctrl &= ~(ADVERTISE_1000FULL | ADVERTISE_1000HALF);
+
+		/* The 8100e/8101e/8102e do Fast Ethernet only. */
+		if ((tp->mac_version != RTL_GIGA_MAC_VER_07) &&
+		    (tp->mac_version != RTL_GIGA_MAC_VER_08) &&
+		    (tp->mac_version != RTL_GIGA_MAC_VER_09) &&
+		    (tp->mac_version != RTL_GIGA_MAC_VER_10) &&
+		    (tp->mac_version != RTL_GIGA_MAC_VER_13) &&
+		    (tp->mac_version != RTL_GIGA_MAC_VER_14) &&
+		    (tp->mac_version != RTL_GIGA_MAC_VER_15) &&
+		    (tp->mac_version != RTL_GIGA_MAC_VER_16)) {
 			giga_ctrl |= ADVERTISE_1000FULL | ADVERTISE_1000HALF;
-
-		if (duplex == DUPLEX_HALF)
-			auto_nego &= ~(ADVERTISE_10FULL | ADVERTISE_100FULL);
-
-		if (duplex == DUPLEX_FULL)
-			auto_nego &= ~(ADVERTISE_10HALF | ADVERTISE_100HALF);
-
-		/* This tweak comes straight from Realtek's driver. */
-		if ((speed == SPEED_100) && (duplex == DUPLEX_HALF) &&
-		    ((tp->mac_version == RTL_GIGA_MAC_VER_13) ||
-		     (tp->mac_version == RTL_GIGA_MAC_VER_16))) {
-			auto_nego = ADVERTISE_100HALF | ADVERTISE_CSMA;
-		}
-	}
-
-	/* The 8100e/8101e/8102e do Fast Ethernet only. */
-	if ((tp->mac_version == RTL_GIGA_MAC_VER_07) ||
-	    (tp->mac_version == RTL_GIGA_MAC_VER_08) ||
-	    (tp->mac_version == RTL_GIGA_MAC_VER_09) ||
-	    (tp->mac_version == RTL_GIGA_MAC_VER_10) ||
-	    (tp->mac_version == RTL_GIGA_MAC_VER_13) ||
-	    (tp->mac_version == RTL_GIGA_MAC_VER_14) ||
-	    (tp->mac_version == RTL_GIGA_MAC_VER_15) ||
-	    (tp->mac_version == RTL_GIGA_MAC_VER_16)) {
-		if ((giga_ctrl & (ADVERTISE_1000FULL | ADVERTISE_1000HALF)) &&
-		    netif_msg_link(tp)) {
+		} else if (netif_msg_link(tp)) {
 			printk(KERN_INFO "%s: PHY does not support 1000Mbps.\n",
 			       dev->name);
 		}
-		giga_ctrl &= ~(ADVERTISE_1000FULL | ADVERTISE_1000HALF);
-	}
 
-	auto_nego |= ADVERTISE_PAUSE_CAP | ADVERTISE_PAUSE_ASYM;
+		bmcr = BMCR_ANENABLE | BMCR_ANRESTART;
+
+		if ((tp->mac_version == RTL_GIGA_MAC_VER_11) ||
+		    (tp->mac_version == RTL_GIGA_MAC_VER_12) ||
+		    (tp->mac_version >= RTL_GIGA_MAC_VER_17)) {
+			/*
+			 * Wake up the PHY.
+			 * Vendor specific (0x1f) and reserved (0x0e) MII
+			 * registers.
+			 */
+			mdio_write(ioaddr, 0x1f, 0x0000);
+			mdio_write(ioaddr, 0x0e, 0x0000);
+		}
+
+		mdio_write(ioaddr, MII_ADVERTISE, auto_nego);
+		mdio_write(ioaddr, MII_CTRL1000, giga_ctrl);
+	} else {
+		giga_ctrl = 0;
+
+		if (speed == SPEED_10)
+			bmcr = 0;
+		else if (speed == SPEED_100)
+			bmcr = BMCR_SPEED100;
+		else
+			return -EINVAL;
+
+		if (duplex == DUPLEX_FULL)
+			bmcr |= BMCR_FULLDPLX;
 
-	if ((tp->mac_version == RTL_GIGA_MAC_VER_11) ||
-	    (tp->mac_version == RTL_GIGA_MAC_VER_12) ||
-	    (tp->mac_version >= RTL_GIGA_MAC_VER_17)) {
-		/*
-		 * Wake up the PHY.
-		 * Vendor specific (0x1f) and reserved (0x0e) MII registers.
-		 */
 		mdio_write(ioaddr, 0x1f, 0x0000);
-		mdio_write(ioaddr, 0x0e, 0x0000);
 	}
 
-	tp->phy_auto_nego_reg = auto_nego;
 	tp->phy_1000_ctrl_reg = giga_ctrl;
 
-	mdio_write(ioaddr, MII_ADVERTISE, auto_nego);
-	mdio_write(ioaddr, MII_CTRL1000, giga_ctrl);
-	mdio_write(ioaddr, MII_BMCR, BMCR_ANENABLE | BMCR_ANRESTART);
+	mdio_write(ioaddr, MII_BMCR, bmcr);
+
+	if ((tp->mac_version == RTL_GIGA_MAC_VER_02) ||
+	    (tp->mac_version == RTL_GIGA_MAC_VER_03)) {
+		if ((speed == SPEED_100) && (autoneg != AUTONEG_ENABLE)) {
+			mdio_write(ioaddr, 0x17, 0x2138);
+			mdio_write(ioaddr, 0x0e, 0x0260);
+		} else {
+			mdio_write(ioaddr, 0x17, 0x2108);
+			mdio_write(ioaddr, 0x0e, 0x0000);
+		}
+	}
+
 	return 0;
 }
 
@@ -1099,22 +1118,6 @@ static const char rtl8169_gstrings[][ETH_GSTRING_LEN] = {
 	"tx_underrun",
 };
 
-struct rtl8169_counters {
-	__le64	tx_packets;
-	__le64	rx_packets;
-	__le64	tx_errors;
-	__le32	rx_errors;
-	__le16	rx_missed;
-	__le16	align_errors;
-	__le32	tx_one_collision;
-	__le32	tx_multi_collision;
-	__le64	rx_unicast;
-	__le64	rx_broadcast;
-	__le32	rx_multicast;
-	__le16	tx_aborted;
-	__le16	tx_underun;
-};
-
 static int rtl8169_get_sset_count(struct net_device *dev, int sset)
 {
 	switch (sset) {
@@ -1125,51 +1128,70 @@ static int rtl8169_get_sset_count(struct net_device *dev, int sset)
 	}
 }
 
-static void rtl8169_get_ethtool_stats(struct net_device *dev,
-				      struct ethtool_stats *stats, u64 *data)
+static void rtl8169_update_counters(struct net_device *dev)
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
 	void __iomem *ioaddr = tp->mmio_addr;
 	struct rtl8169_counters *counters;
 	dma_addr_t paddr;
 	u32 cmd;
+	int wait = 1000;
 
-	ASSERT_RTNL();
+	/*
+	 * Some chips are unable to dump tally counters when the receiver
+	 * is disabled.
+	 */
+	if ((RTL_R8(ChipCmd) & CmdRxEnb) == 0)
+		return;
 
 	counters = pci_alloc_consistent(tp->pci_dev, sizeof(*counters), &paddr);
 	if (!counters)
 		return;
 
 	RTL_W32(CounterAddrHigh, (u64)paddr >> 32);
-	cmd = (u64)paddr & DMA_32BIT_MASK;
+	cmd = (u64)paddr & DMA_BIT_MASK(32);
 	RTL_W32(CounterAddrLow, cmd);
 	RTL_W32(CounterAddrLow, cmd | CounterDump);
 
-	while (RTL_R32(CounterAddrLow) & CounterDump) {
-		if (msleep_interruptible(1))
+	while (wait--) {
+		if ((RTL_R32(CounterAddrLow) & CounterDump) == 0) {
+			/* copy updated counters */
+			memcpy(&tp->counters, counters, sizeof(*counters));
 			break;
+		}
+		udelay(10);
 	}
 
 	RTL_W32(CounterAddrLow, 0);
 	RTL_W32(CounterAddrHigh, 0);
 
-	data[0] = le64_to_cpu(counters->tx_packets);
-	data[1] = le64_to_cpu(counters->rx_packets);
-	data[2] = le64_to_cpu(counters->tx_errors);
-	data[3] = le32_to_cpu(counters->rx_errors);
-	data[4] = le16_to_cpu(counters->rx_missed);
-	data[5] = le16_to_cpu(counters->align_errors);
-	data[6] = le32_to_cpu(counters->tx_one_collision);
-	data[7] = le32_to_cpu(counters->tx_multi_collision);
-	data[8] = le64_to_cpu(counters->rx_unicast);
-	data[9] = le64_to_cpu(counters->rx_broadcast);
-	data[10] = le32_to_cpu(counters->rx_multicast);
-	data[11] = le16_to_cpu(counters->tx_aborted);
-	data[12] = le16_to_cpu(counters->tx_underun);
-
 	pci_free_consistent(tp->pci_dev, sizeof(*counters), counters, paddr);
 }
 
+static void rtl8169_get_ethtool_stats(struct net_device *dev,
+				      struct ethtool_stats *stats, u64 *data)
+{
+	struct rtl8169_private *tp = netdev_priv(dev);
+
+	ASSERT_RTNL();
+
+	rtl8169_update_counters(dev);
+
+	data[0] = le64_to_cpu(tp->counters.tx_packets);
+	data[1] = le64_to_cpu(tp->counters.rx_packets);
+	data[2] = le64_to_cpu(tp->counters.tx_errors);
+	data[3] = le32_to_cpu(tp->counters.rx_errors);
+	data[4] = le16_to_cpu(tp->counters.rx_missed);
+	data[5] = le16_to_cpu(tp->counters.align_errors);
+	data[6] = le32_to_cpu(tp->counters.tx_one_collision);
+	data[7] = le32_to_cpu(tp->counters.tx_multi_collision);
+	data[8] = le64_to_cpu(tp->counters.rx_unicast);
+	data[9] = le64_to_cpu(tp->counters.rx_broadcast);
+	data[10] = le32_to_cpu(tp->counters.rx_multicast);
+	data[11] = le16_to_cpu(tp->counters.tx_aborted);
+	data[12] = le16_to_cpu(tp->counters.tx_underun);
+}
+
 static void rtl8169_get_strings(struct net_device *dev, u32 stringset, u8 *data)
 {
 	switch(stringset) {
@@ -1275,7 +1297,8 @@ static void rtl8169_get_mac_version(struct rtl8169_private *tp,
 		{ 0xfc800000, 0x00800000,	RTL_GIGA_MAC_VER_02 },
 		{ 0xfc800000, 0x00000000,	RTL_GIGA_MAC_VER_01 },
 
-		{ 0x00000000, 0x00000000,	RTL_GIGA_MAC_VER_01 }	/* Catch-all */
+		/* Catch-all */
+		{ 0x00000000, 0x00000000,	RTL_GIGA_MAC_NONE   }
 	}, *p = mac_info;
 	u32 reg;
 
@@ -1283,12 +1306,6 @@ static void rtl8169_get_mac_version(struct rtl8169_private *tp,
 	while ((reg & p->mask) != p->val)
 		p++;
 	tp->mac_version = p->mac_version;
-
-	if (p->mask == 0x00000000) {
-		struct pci_dev *pdev = tp->pci_dev;
-
-		dev_info(&pdev->dev, "unknown MAC (%08x)\n", reg);
-	}
 }
 
 static void rtl8169_print_mac_version(struct rtl8169_private *tp)
@@ -1829,9 +1846,11 @@ static int rtl8169_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 	struct rtl8169_private *tp = netdev_priv(dev);
 	struct mii_ioctl_data *data = if_mii(ifr);
 
-	if (!netif_running(dev))
-		return -ENODEV;
+	return netif_running(dev) ? tp->do_ioctl(tp, data, cmd) : -ENODEV;
+}
 
+static int rtl_xmii_ioctl(struct rtl8169_private *tp, struct mii_ioctl_data *data, int cmd)
+{
 	switch (cmd) {
 	case SIOCGMIIPHY:
 		data->phy_id = 32; /* Internal PHY */
@@ -1850,6 +1869,11 @@ static int rtl8169_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 	return -EOPNOTSUPP;
 }
 
+static int rtl_tbi_ioctl(struct rtl8169_private *tp, struct mii_ioctl_data *data, int cmd)
+{
+	return -EOPNOTSUPP;
+}
+
 static const struct rtl_cfg_info {
 	void (*hw_start)(struct net_device *);
 	unsigned int region;
@@ -1857,6 +1881,7 @@ static const struct rtl_cfg_info {
 	u16 intr_event;
 	u16 napi_event;
 	unsigned features;
+	u8 default_ver;
 } rtl_cfg_infos [] = {
 	[RTL_CFG_0] = {
 		.hw_start	= rtl_hw_start_8169,
@@ -1865,7 +1890,8 @@ static const struct rtl_cfg_info {
 		.intr_event	= SYSErr | LinkChg | RxOverflow |
 				  RxFIFOOver | TxErr | TxOK | RxOK | RxErr,
 		.napi_event	= RxFIFOOver | TxErr | TxOK | RxOK | RxOverflow,
-		.features	= RTL_FEATURE_GMII
+		.features	= RTL_FEATURE_GMII,
+		.default_ver	= RTL_GIGA_MAC_VER_01,
 	},
 	[RTL_CFG_1] = {
 		.hw_start	= rtl_hw_start_8168,
@@ -1874,7 +1900,8 @@ static const struct rtl_cfg_info {
 		.intr_event	= SYSErr | LinkChg | RxOverflow |
 				  TxErr | TxOK | RxOK | RxErr,
 		.napi_event	= TxErr | TxOK | RxOK | RxOverflow,
-		.features	= RTL_FEATURE_GMII | RTL_FEATURE_MSI
+		.features	= RTL_FEATURE_GMII | RTL_FEATURE_MSI,
+		.default_ver	= RTL_GIGA_MAC_VER_11,
 	},
 	[RTL_CFG_2] = {
 		.hw_start	= rtl_hw_start_8101,
@@ -1883,7 +1910,8 @@ static const struct rtl_cfg_info {
 		.intr_event	= SYSErr | LinkChg | RxOverflow | PCSTimeout |
 				  RxFIFOOver | TxErr | TxOK | RxOK | RxErr,
 		.napi_event	= RxFIFOOver | TxErr | TxOK | RxOK | RxOverflow,
-		.features	= RTL_FEATURE_MSI
+		.features	= RTL_FEATURE_MSI,
+		.default_ver	= RTL_GIGA_MAC_VER_13,
 	}
 };
 
@@ -1915,6 +1943,26 @@ static void rtl_disable_msi(struct pci_dev *pdev, struct rtl8169_private *tp)
 	}
 }
 
+static const struct net_device_ops rtl8169_netdev_ops = {
+	.ndo_open		= rtl8169_open,
+	.ndo_stop		= rtl8169_close,
+	.ndo_get_stats		= rtl8169_get_stats,
+	.ndo_start_xmit		= rtl8169_start_xmit,
+	.ndo_tx_timeout		= rtl8169_tx_timeout,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_change_mtu		= rtl8169_change_mtu,
+	.ndo_set_mac_address	= rtl_set_mac_address,
+	.ndo_do_ioctl		= rtl8169_ioctl,
+	.ndo_set_multicast_list	= rtl_set_rx_mode,
+#ifdef CONFIG_R8169_VLAN
+	.ndo_vlan_rx_register	= rtl8169_vlan_rx_register,
+#endif
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller	= rtl8169_netpoll,
+#endif
+
+};
+
 static int __devinit
 rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
@@ -1941,6 +1989,7 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	}
 
 	SET_NETDEV_DEV(dev, &pdev->dev);
+	dev->netdev_ops = &rtl8169_netdev_ops;
 	tp = netdev_priv(dev);
 	tp->dev = dev;
 	tp->pci_dev = pdev;
@@ -1997,11 +2046,11 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	tp->cp_cmd = PCIMulRW | RxChkSum;
 
 	if ((sizeof(dma_addr_t) > 4) &&
-	    !pci_set_dma_mask(pdev, DMA_64BIT_MASK) && use_dac) {
+	    !pci_set_dma_mask(pdev, DMA_BIT_MASK(64)) && use_dac) {
 		tp->cp_cmd |= PCIDAC;
 		dev->features |= NETIF_F_HIGHDMA;
 	} else {
-		rc = pci_set_dma_mask(pdev, DMA_32BIT_MASK);
+		rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
 		if (rc < 0) {
 			if (netif_msg_probe(tp)) {
 				dev_err(&pdev->dev,
@@ -2011,8 +2060,6 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		}
 	}
 
-	pci_set_master(pdev);
-
 	/* ioremap MMIO region */
 	ioaddr = ioremap(pci_resource_start(pdev, region), R8169_REGS_SIZE);
 	if (!ioaddr) {
@@ -2040,9 +2087,20 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	RTL_W16(IntrStatus, 0xffff);
 
+	pci_set_master(pdev);
+
 	/* Identify chip attached to board */
 	rtl8169_get_mac_version(tp, ioaddr);
 
+	/* Use appropriate default if unknown */
+	if (tp->mac_version == RTL_GIGA_MAC_NONE) {
+		if (netif_msg_probe(tp)) {
+			dev_notice(&pdev->dev,
+				   "unknown MAC, using family default\n");
+		}
+		tp->mac_version = cfg->default_ver;
+	}
+
 	rtl8169_print_mac_version(tp);
 
 	for (i = 0; i < ARRAY_SIZE(rtl_chip_info); i++) {
@@ -2050,13 +2108,9 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 			break;
 	}
 	if (i == ARRAY_SIZE(rtl_chip_info)) {
-		/* Unknown chip: assume array element #0, original RTL-8169 */
-		if (netif_msg_probe(tp)) {
-			dev_printk(KERN_DEBUG, &pdev->dev,
-				"unknown chip version, assuming %s\n",
-				rtl_chip_info[0].name);
-		}
-		i = 0;
+		dev_err(&pdev->dev,
+			"driver bug, MAC version not found in rtl_chip_info\n");
+		goto err_out_msi_5;
 	}
 	tp->chipset = i;
 
@@ -2077,6 +2131,7 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		tp->phy_reset_enable = rtl8169_tbi_reset_enable;
 		tp->phy_reset_pending = rtl8169_tbi_reset_pending;
 		tp->link_ok = rtl8169_tbi_link_ok;
+		tp->do_ioctl = rtl_tbi_ioctl;
 
 		tp->phy_1000_ctrl_reg = ADVERTISE_1000FULL; /* Implied by TBI */
 	} else {
@@ -2085,8 +2140,7 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		tp->phy_reset_enable = rtl8169_xmii_reset_enable;
 		tp->phy_reset_pending = rtl8169_xmii_reset_pending;
 		tp->link_ok = rtl8169_xmii_link_ok;
-
-		dev->do_ioctl = rtl8169_ioctl;
+		tp->do_ioctl = rtl_xmii_ioctl;
 	}
 
 	spin_lock_init(&tp->lock);
@@ -2098,28 +2152,15 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		dev->dev_addr[i] = RTL_R8(MAC0 + i);
 	memcpy(dev->perm_addr, dev->dev_addr, dev->addr_len);
 
-	dev->open = rtl8169_open;
-	dev->hard_start_xmit = rtl8169_start_xmit;
-	dev->get_stats = rtl8169_get_stats;
 	SET_ETHTOOL_OPS(dev, &rtl8169_ethtool_ops);
-	dev->stop = rtl8169_close;
-	dev->tx_timeout = rtl8169_tx_timeout;
-	dev->set_multicast_list = rtl_set_rx_mode;
 	dev->watchdog_timeo = RTL8169_TX_TIMEOUT;
 	dev->irq = pdev->irq;
 	dev->base_addr = (unsigned long) ioaddr;
-	dev->change_mtu = rtl8169_change_mtu;
-	dev->set_mac_address = rtl_set_mac_address;
 
 	netif_napi_add(dev, &tp->napi, rtl8169_poll, R8169_NAPI_WEIGHT);
 
 #ifdef CONFIG_R8169_VLAN
 	dev->features |= NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
-	dev->vlan_rx_register = rtl8169_vlan_rx_register;
-#endif
-
-#ifdef CONFIG_NET_POLL_CONTROLLER
-	dev->poll_controller = rtl8169_netpoll;
 #endif
 
 	tp->intr_mask = 0xffff;
@@ -2307,9 +2348,9 @@ static void rtl_set_rx_tx_desc_registers(struct rtl8169_private *tp,
 	 * Switching from MMIO to I/O access fixes the issue as well.
 	 */
 	RTL_W32(TxDescStartAddrHigh, ((u64) tp->TxPhyAddr) >> 32);
-	RTL_W32(TxDescStartAddrLow, ((u64) tp->TxPhyAddr) & DMA_32BIT_MASK);
+	RTL_W32(TxDescStartAddrLow, ((u64) tp->TxPhyAddr) & DMA_BIT_MASK(32));
 	RTL_W32(RxDescAddrHigh, ((u64) tp->RxPhyAddr) >> 32);
-	RTL_W32(RxDescAddrLow, ((u64) tp->RxPhyAddr) & DMA_32BIT_MASK);
+	RTL_W32(RxDescAddrLow, ((u64) tp->RxPhyAddr) & DMA_BIT_MASK(32));
 }
 
 static u16 rtl_rw_cpluscmd(void __iomem *ioaddr)
@@ -2321,10 +2362,10 @@ static u16 rtl_rw_cpluscmd(void __iomem *ioaddr)
 	return cmd;
 }
 
-static void rtl_set_rx_max_size(void __iomem *ioaddr)
+static void rtl_set_rx_max_size(void __iomem *ioaddr, unsigned int rx_buf_sz)
 {
 	/* Low hurts. Let's disable the filtering. */
-	RTL_W16(RxMaxSize, 16383);
+	RTL_W16(RxMaxSize, rx_buf_sz);
 }
 
 static void rtl8169_set_magic_reg(void __iomem *ioaddr, unsigned mac_version)
@@ -2371,7 +2412,7 @@ static void rtl_hw_start_8169(struct net_device *dev)
 
 	RTL_W8(EarlyTxThres, EarlyTxThld);
 
-	rtl_set_rx_max_size(ioaddr);
+	rtl_set_rx_max_size(ioaddr, tp->rx_buf_sz);
 
 	if ((tp->mac_version == RTL_GIGA_MAC_VER_01) ||
 	    (tp->mac_version == RTL_GIGA_MAC_VER_02) ||
@@ -2632,7 +2673,7 @@ static void rtl_hw_start_8168(struct net_device *dev)
 
 	RTL_W8(EarlyTxThres, EarlyTxThld);
 
-	rtl_set_rx_max_size(ioaddr);
+	rtl_set_rx_max_size(ioaddr, tp->rx_buf_sz);
 
 	tp->cp_cmd |= RTL_R16(CPlusCmd) | PktCntrDisable | INTT_1;
 
@@ -2810,7 +2851,7 @@ static void rtl_hw_start_8101(struct net_device *dev)
 
 	RTL_W8(EarlyTxThres, EarlyTxThld);
 
-	rtl_set_rx_max_size(ioaddr);
+	rtl_set_rx_max_size(ioaddr, tp->rx_buf_sz);
 
 	tp->cp_cmd |= rtl_rw_cpluscmd(ioaddr) | PCIMulRW;
 
@@ -3218,13 +3259,6 @@ static int rtl8169_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		opts1 |= FirstFrag;
 	} else {
 		len = skb->len;
-
-		if (unlikely(len < ETH_ZLEN)) {
-			if (skb_padto(skb, ETH_ZLEN))
-				goto err_update_stats;
-			len = ETH_ZLEN;
-		}
-
 		opts1 |= FirstFrag | LastFrag;
 		tp->tx_skb[entry].skb = skb;
 	}
@@ -3241,8 +3275,6 @@ static int rtl8169_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	status = opts1 | len | (RingEnd * !((entry + 1) % NUM_TX_DESC));
 	txd->opts1 = cpu_to_le32(status);
 
-	dev->trans_start = jiffies;
-
 	tp->cur_tx += frags + 1;
 
 	smp_wmb();
@@ -3262,7 +3294,6 @@ out:
 err_stop:
 	netif_stop_queue(dev);
 	ret = NETDEV_TX_BUSY;
-err_update_stats:
 	dev->stats.tx_dropped++;
 	goto out;
 }
@@ -3344,7 +3375,7 @@ static void rtl8169_tx_interrupt(struct net_device *dev,
 		rtl8169_unmap_tx_skb(tp->pci_dev, tx_skb, tp->TxDescArray + entry);
 
 		if (status & LastFrag) {
-			dev_kfree_skb_irq(tx_skb->skb);
+			dev_kfree_skb(tx_skb->skb);
 			tx_skb->skb = NULL;
 		}
 		dirty_tx++;
@@ -3485,7 +3516,6 @@ static int rtl8169_rx_interrupt(struct net_device *dev,
 			if (rtl8169_rx_vlan_skb(tp, desc, skb) < 0)
 				netif_receive_skb(skb);
 
-			dev->last_rx = jiffies;
 			dev->stats.rx_bytes += pkt_size;
 			dev->stats.rx_packets++;
 		}
@@ -3527,54 +3557,64 @@ static irqreturn_t rtl8169_interrupt(int irq, void *dev_instance)
 	int handled = 0;
 	int status;
 
+	/* loop handling interrupts until we have no new ones or
+	 * we hit a invalid/hotplug case.
+	 */
 	status = RTL_R16(IntrStatus);
+	while (status && status != 0xffff) {
+		handled = 1;
 
-	/* hotplug/major error/no more work/shared irq */
-	if ((status == 0xffff) || !status)
-		goto out;
-
-	handled = 1;
-
-	if (unlikely(!netif_running(dev))) {
-		rtl8169_asic_down(ioaddr);
-		goto out;
-	}
-
-	status &= tp->intr_mask;
-	RTL_W16(IntrStatus,
-		(status & RxFIFOOver) ? (status | RxOverflow) : status);
-
-	if (!(status & tp->intr_event))
-		goto out;
-
-	/* Work around for rx fifo overflow */
-	if (unlikely(status & RxFIFOOver) &&
-	    (tp->mac_version == RTL_GIGA_MAC_VER_11)) {
-		netif_stop_queue(dev);
-		rtl8169_tx_timeout(dev);
-		goto out;
-	}
-
-	if (unlikely(status & SYSErr)) {
-		rtl8169_pcierr_interrupt(dev);
-		goto out;
-	}
-
-	if (status & LinkChg)
-		rtl8169_check_link_status(dev, tp, ioaddr);
-
-	if (status & tp->napi_event) {
-		RTL_W16(IntrMask, tp->intr_event & ~tp->napi_event);
-		tp->intr_mask = ~tp->napi_event;
-
-		if (likely(netif_rx_schedule_prep(dev, &tp->napi)))
-			__netif_rx_schedule(dev, &tp->napi);
-		else if (netif_msg_intr(tp)) {
-			printk(KERN_INFO "%s: interrupt %04x in poll\n",
-			       dev->name, status);
+		/* Handle all of the error cases first. These will reset
+		 * the chip, so just exit the loop.
+		 */
+		if (unlikely(!netif_running(dev))) {
+			rtl8169_asic_down(ioaddr);
+			break;
 		}
+
+		/* Work around for rx fifo overflow */
+		if (unlikely(status & RxFIFOOver) &&
+		(tp->mac_version == RTL_GIGA_MAC_VER_11)) {
+			netif_stop_queue(dev);
+			rtl8169_tx_timeout(dev);
+			break;
+		}
+
+		if (unlikely(status & SYSErr)) {
+			rtl8169_pcierr_interrupt(dev);
+			break;
+		}
+
+		if (status & LinkChg)
+			rtl8169_check_link_status(dev, tp, ioaddr);
+
+		/* We need to see the lastest version of tp->intr_mask to
+		 * avoid ignoring an MSI interrupt and having to wait for
+		 * another event which may never come.
+		 */
+		smp_rmb();
+		if (status & tp->intr_mask & tp->napi_event) {
+			RTL_W16(IntrMask, tp->intr_event & ~tp->napi_event);
+			tp->intr_mask = ~tp->napi_event;
+
+			if (likely(napi_schedule_prep(&tp->napi)))
+				__napi_schedule(&tp->napi);
+			else if (netif_msg_intr(tp)) {
+				printk(KERN_INFO "%s: interrupt %04x in poll\n",
+				dev->name, status);
+			}
+		}
+
+		/* We only get a new MSI interrupt when all active irq
+		 * sources on the chip have been acknowledged. So, ack
+		 * everything we've seen and check if new sources have become
+		 * active to avoid blocking all interrupts from the chip.
+		 */
+		RTL_W16(IntrStatus,
+			(status & RxFIFOOver) ? (status | RxOverflow) : status);
+		status = RTL_R16(IntrStatus);
 	}
-out:
+
 	return IRQ_RETVAL(handled);
 }
 
@@ -3589,14 +3629,16 @@ static int rtl8169_poll(struct napi_struct *napi, int budget)
 	rtl8169_tx_interrupt(dev, tp, ioaddr);
 
 	if (work_done < budget) {
-		netif_rx_complete(dev, napi);
-		tp->intr_mask = 0xffff;
-		/*
-		 * 20040426: the barrier is not strictly required but the
-		 * behavior of the irq handler could be less predictable
-		 * without it. Btw, the lack of flush for the posted pci
-		 * write is safe - FR
+		napi_complete(napi);
+
+		/* We need for force the visibility of tp->intr_mask
+		 * for other CPUs, as we can loose an MSI interrupt
+		 * and potentially wait for a retransmit timeout if we don't.
+		 * The posted write to IntrMask is safe, as it will
+		 * eventually make it to the chip and we won't loose anything
+		 * until it does.
 		 */
+		tp->intr_mask = 0xffff;
 		smp_wmb();
 		RTL_W16(IntrMask, tp->intr_event);
 	}
@@ -3668,6 +3710,9 @@ static int rtl8169_close(struct net_device *dev)
 	struct rtl8169_private *tp = netdev_priv(dev);
 	struct pci_dev *pdev = tp->pci_dev;
 
+	/* update counters before going down */
+	rtl8169_update_counters(dev);
+
 	rtl8169_down(dev);
 
 	free_irq(dev->irq, dev);
@@ -3761,45 +3806,32 @@ static struct net_device_stats *rtl8169_get_stats(struct net_device *dev)
 	return &dev->stats;
 }
 
-#ifdef CONFIG_PM
-
-static int rtl8169_suspend(struct pci_dev *pdev, pm_message_t state)
+static void rtl8169_net_suspend(struct net_device *dev)
 {
-	struct net_device *dev = pci_get_drvdata(pdev);
-	struct rtl8169_private *tp = netdev_priv(dev);
-	void __iomem *ioaddr = tp->mmio_addr;
-
 	if (!netif_running(dev))
-		goto out_pci_suspend;
+		return;
 
 	netif_device_detach(dev);
 	netif_stop_queue(dev);
+}
 
-	spin_lock_irq(&tp->lock);
+#ifdef CONFIG_PM
 
-	rtl8169_asic_down(ioaddr);
+static int rtl8169_suspend(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct net_device *dev = pci_get_drvdata(pdev);
 
-	rtl8169_rx_missed(dev, ioaddr);
-
-	spin_unlock_irq(&tp->lock);
-
-out_pci_suspend:
-	pci_save_state(pdev);
-	pci_enable_wake(pdev, pci_choose_state(pdev, state),
-		(tp->features & RTL_FEATURE_WOL) ? 1 : 0);
-	pci_set_power_state(pdev, pci_choose_state(pdev, state));
+	rtl8169_net_suspend(dev);
 
 	return 0;
 }
 
-static int rtl8169_resume(struct pci_dev *pdev)
+static int rtl8169_resume(struct device *device)
 {
+	struct pci_dev *pdev = to_pci_dev(device);
 	struct net_device *dev = pci_get_drvdata(pdev);
 
-	pci_set_power_state(pdev, PCI_D0);
-	pci_restore_state(pdev);
-	pci_enable_wake(pdev, PCI_D0, 0);
-
 	if (!netif_running(dev))
 		goto out;
 
@@ -3810,23 +3842,59 @@ out:
 	return 0;
 }
 
+static struct dev_pm_ops rtl8169_pm_ops = {
+	.suspend = rtl8169_suspend,
+	.resume = rtl8169_resume,
+	.freeze = rtl8169_suspend,
+	.thaw = rtl8169_resume,
+	.poweroff = rtl8169_suspend,
+	.restore = rtl8169_resume,
+};
+
+#define RTL8169_PM_OPS	(&rtl8169_pm_ops)
+
+#else /* !CONFIG_PM */
+
+#define RTL8169_PM_OPS	NULL
+
+#endif /* !CONFIG_PM */
+
 static void rtl_shutdown(struct pci_dev *pdev)
 {
-	rtl8169_suspend(pdev, PMSG_SUSPEND);
-}
+	struct net_device *dev = pci_get_drvdata(pdev);
+	struct rtl8169_private *tp = netdev_priv(dev);
+	void __iomem *ioaddr = tp->mmio_addr;
 
-#endif /* CONFIG_PM */
+	rtl8169_net_suspend(dev);
+
+	spin_lock_irq(&tp->lock);
+
+	rtl8169_asic_down(ioaddr);
+
+	spin_unlock_irq(&tp->lock);
+
+	if (system_state == SYSTEM_POWER_OFF) {
+		/* WoL fails with some 8168 when the receiver is disabled. */
+		if (tp->features & RTL_FEATURE_WOL) {
+			pci_clear_master(pdev);
+
+			RTL_W8(ChipCmd, CmdRxEnb);
+			/* PCI commit */
+			RTL_R8(ChipCmd);
+		}
+
+		pci_wake_from_d3(pdev, true);
+		pci_set_power_state(pdev, PCI_D3hot);
+	}
+}
 
 static struct pci_driver rtl8169_pci_driver = {
 	.name		= MODULENAME,
 	.id_table	= rtl8169_pci_tbl,
 	.probe		= rtl8169_init_one,
 	.remove		= __devexit_p(rtl8169_remove_one),
-#ifdef CONFIG_PM
-	.suspend	= rtl8169_suspend,
-	.resume		= rtl8169_resume,
 	.shutdown	= rtl_shutdown,
-#endif
+	.driver.pm	= RTL8169_PM_OPS,
 };
 
 static int __init rtl8169_init_module(void)

From d9f27a25970f8e14cc7879389a2f50e81a78cd11 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Thu, 20 May 2010 12:35:36 +0200
Subject: [PATCH 03/34] Made send_interval variable an unsigned int; fixed
 format specifiers for size_t variables.

---
 master/fsm_slave.c |  2 +-
 master/master.c    | 18 +++++++++---------
 master/master.h    |  4 ++--
 master/module.c    |  7 ++++---
 4 files changed, 16 insertions(+), 15 deletions(-)

diff --git a/master/fsm_slave.c b/master/fsm_slave.c
index 1d2e67d8..ddfd9b01 100644
--- a/master/fsm_slave.c
+++ b/master/fsm_slave.c
@@ -311,7 +311,7 @@ void ec_fsm_slave_state_foe_request(
     }
 
     // finished transferring FoE
-    EC_SLAVE_DBG(slave, 1, "Successfully transferred %u bytes of FoE"
+    EC_SLAVE_DBG(slave, 1, "Successfully transferred %zu bytes of FoE"
             " data.\n", request->data_size);
 
     request->state = EC_INT_REQUEST_SUCCESS;
diff --git a/master/master.c b/master/master.c
index 56f54aa3..96c26260 100644
--- a/master/master.c
+++ b/master/master.c
@@ -698,7 +698,7 @@ void ec_master_inject_external_datagrams(
                 list_del_init(&datagram->queue);
                 datagram->state = EC_DATAGRAM_ERROR;
                 EC_MASTER_ERR(master, "External datagram %p is too large,"
-                        " size=%u, max_queue_size=%u\n",
+                        " size=%zu, max_queue_size=%zu\n",
                         datagram, datagram->data_size,
                         master->max_queue_size);
             } else {
@@ -725,8 +725,8 @@ void ec_master_inject_external_datagrams(
                         ((jiffies - datagram->jiffies_sent) * 1000000 / HZ);
 #endif
                     EC_MASTER_ERR(master, "Timeout %u us: Injecting"
-                            " external datagram %p size=%u,"
-                            " max_queue_size=%u\n", time_us, datagram,
+                            " external datagram %p size=%zu,"
+                            " max_queue_size=%zu\n", time_us, datagram,
                             datagram->data_size, master->max_queue_size);
                 }
 #if DEBUG_INJECT
@@ -749,7 +749,7 @@ void ec_master_inject_external_datagrams(
  */
 void ec_master_set_send_interval(
         ec_master_t *master, /**< EtherCAT master */
-        size_t send_interval /**< Send interval */
+        unsigned int send_interval /**< Send interval */
         )
 {
     master->send_interval = send_interval;
@@ -1198,8 +1198,8 @@ static int ec_master_idle_thread(void *priv_data)
     // send interval in IDLE phase
     ec_master_set_send_interval(master, 1000000 / HZ); 
 
-    EC_MASTER_DBG(master, 1, "Idle thread running with send interval = %d us,"
-            " max data size=%d\n", master->send_interval,
+    EC_MASTER_DBG(master, 1, "Idle thread running with send interval = %u us,"
+            " max data size=%zu\n", master->send_interval,
             master->max_queue_size);
 
     while (!kthread_should_stop()) {
@@ -1265,7 +1265,7 @@ static int ec_master_operation_thread(void *priv_data)
     int fsm_exec;
 
     EC_MASTER_DBG(master, 1, "Operation thread running"
-            " with fsm interval = %d us, max data size=%d\n",
+            " with fsm interval = %u us, max data size=%zu\n",
             master->send_interval, master->max_queue_size);
 
     while (!kthread_should_stop()) {
@@ -1989,8 +1989,8 @@ void ecrt_master_deactivate(ec_master_t *master)
     int eoe_was_running;
 #endif
 
-    EC_MASTER_DBG(master, 1, "ecrt_master_deactivate(master = 0x%x)\n",
-            (u32) master);
+    EC_MASTER_DBG(master, 1, "ecrt_master_deactivate(master = 0x%p)\n",
+            master);
 
     if (!master->active) {
         EC_MASTER_WARN(master, "%s: Master not active.\n", __func__);
diff --git a/master/master.h b/master/master.h
index 1a04379f..c4ea963b 100644
--- a/master/master.h
+++ b/master/master.h
@@ -211,7 +211,7 @@ struct ec_master {
                                       ext_datagram_queue. */
 
     struct list_head external_datagram_queue; /**< External Datagram queue. */
-    size_t send_interval; /**< Interval between calls to ecrt_master_send */
+    unsigned int send_interval; /**< Interval between calls to ecrt_master_send */
     size_t max_queue_size; /**< Maximum size of datagram queue */
     struct list_head domains; /**< List of domains. */
 
@@ -275,7 +275,7 @@ void ec_master_queue_external_datagram(ec_master_t *, ec_datagram_t *);
 void ec_master_inject_external_datagrams(ec_master_t *);
 
 // misc.
-void ec_master_set_send_interval(ec_master_t *,size_t);
+void ec_master_set_send_interval(ec_master_t *, unsigned int);
 void ec_master_attach_slave_configs(ec_master_t *);
 ec_slave_t *ec_master_find_slave(ec_master_t *, uint16_t, uint16_t);
 const ec_slave_t *ec_master_find_slave_const(const ec_master_t *, uint16_t,
diff --git a/master/module.c b/master/module.c
index 8ad81544..e32a74b3 100644
--- a/master/module.c
+++ b/master/module.c
@@ -341,13 +341,14 @@ void ec_print_data(const uint8_t *data, /**< pointer to data */
     EC_DBG("");
     for (i = 0; i < size; i++) {
         printk("%02X ", data[i]);
+
         if ((i + 1) % 16 == 0 && i < size - 1) {
             printk("\n");
             EC_DBG("");
         }
-        if (i+1 == 128 && size > 256)
-        {
-            printk("dropped %d bytes\n",size-128-i);
+
+        if (i + 1 == 128 && size > 256) {
+            printk("dropped %zu bytes\n", size - 128 - i);
             i = size - 128;
             EC_DBG("");
         }

From 6d354de9c0da8aaf8958918a0c706a8ba7576b2d Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Thu, 20 May 2010 12:46:09 +0200
Subject: [PATCH 04/34] Fixed format specifiers.

---
 master/cdev.c             |  9 ++++-----
 master/fsm_coe.c          | 34 +++++++++++++++++-----------------
 master/fsm_master.c       |  8 ++++----
 master/fsm_slave_config.c | 12 ++++++------
 master/fsm_soe.c          | 12 ++++++------
 master/slave_config.c     |  4 ++--
 master/voe_handler.c      |  4 ++--
 7 files changed, 41 insertions(+), 42 deletions(-)

diff --git a/master/cdev.c b/master/cdev.c
index 2b3532af..e2eca4db 100644
--- a/master/cdev.c
+++ b/master/cdev.c
@@ -3514,7 +3514,7 @@ int eccdev_open(struct inode *inode, struct file *filp)
     filp->private_data = priv;
 
 #if DEBUG_IOCTL
-    EC_MASTER_DBG(cdev->master, "File opened.\n");
+    EC_MASTER_DBG(cdev->master, 0, "File opened.\n");
 #endif
     return 0;
 }
@@ -3535,7 +3535,7 @@ int eccdev_release(struct inode *inode, struct file *filp)
         vfree(priv->process_data);
 
 #if DEBUG_IOCTL
-    EC_MASTER_DBG(master, "File closed.\n");
+    EC_MASTER_DBG(master, 0, "File closed.\n");
 #endif
 
     kfree(priv);
@@ -3552,9 +3552,8 @@ long eccdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
     ec_master_t *master = priv->cdev->master;
 
 #if DEBUG_IOCTL
-    EC_MASTER_DBG(master, "ioctl(filp = 0x%x, cmd = 0x%08x (0x%02x),"
-            " arg = 0x%x)\n", (u32) filp, (u32) cmd, (u32) _IOC_NR(cmd),
-            (u32) arg);
+    EC_MASTER_DBG(master, 0, "ioctl(filp = 0x%p, cmd = 0x%08x (0x%02x),"
+            " arg = 0x%lx)\n", filp, cmd, _IOC_NR(cmd), arg);
 #endif
 
     switch (cmd) {
diff --git a/master/fsm_coe.c b/master/fsm_coe.c
index 08987d5e..d1dc41e9 100644
--- a/master/fsm_coe.c
+++ b/master/fsm_coe.c
@@ -1225,9 +1225,9 @@ void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */)
         if (!datagram->working_counter) {
             if (diff_ms < fsm->request->response_timeout) {
 #if DEBUG_RETRIES
-                EC_SLAVE_DBG(slave, 1, "Slave did not respond to"
-                        " SDO download request. Retrying after %u ms...\n",
-                        (u32) diff_ms);
+                EC_SLAVE_DBG(slave, 1, "Slave did not respond to SDO"
+                        " download request. Retrying after %lu ms...\n",
+                        diff_ms);
 #endif
                 // no response; send request datagram again
                 return;
@@ -1235,16 +1235,16 @@ void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */)
         }
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE download request"
-                " for SDO 0x%04x:%x failed with timeout after %u ms: ",
-                fsm->request->index, fsm->request->subindex, (u32) diff_ms);
+                " for SDO 0x%04x:%x failed with timeout after %lu ms: ",
+                fsm->request->index, fsm->request->subindex, diff_ms);
         ec_datagram_print_wc_error(datagram);
         return;
     }
 
 #if DEBUG_LONG
     if (diff_ms > 200) {
-        EC_SLAVE_WARN(slave, "SDO 0x%04x:%x download took %u ms.\n",
-                fsm->request->index, fsm->request->subindex, (u32) diff_ms);
+        EC_SLAVE_WARN(slave, "SDO 0x%04x:%x download took %lu ms.\n",
+                fsm->request->index, fsm->request->subindex, diff_ms);
     }
 #endif
 
@@ -1288,8 +1288,8 @@ void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */)
             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
         if (diff_ms >= fsm->request->response_timeout) {
             fsm->state = ec_fsm_coe_error;
-            EC_SLAVE_ERR(slave, "Timeout after %u ms while waiting"
-                    " for SDO 0x%04x:%x download response.\n", (u32) diff_ms,
+            EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
+                    " for SDO 0x%04x:%x download response.\n", diff_ms,
                     fsm->request->index, fsm->request->subindex);
             return;
         }
@@ -1727,8 +1727,8 @@ void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */)
             if (diff_ms < fsm->request->response_timeout) {
 #if DEBUG_RETRIES
                 EC_SLAVE_DBG(slave, 1, "Slave did not respond to"
-                        " SDO upload request. Retrying after %u ms...\n",
-                        (u32) diff_ms);
+                        " SDO upload request. Retrying after %lu ms...\n",
+                        diff_ms);
 #endif
                 // no response; send request datagram again
                 return;
@@ -1736,16 +1736,16 @@ void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */)
         }
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE upload request for"
-                " SDO 0x%04x:%x failed with timeout after %u ms: ",
-                fsm->request->index, fsm->request->subindex, (u32) diff_ms);
+                " SDO 0x%04x:%x failed with timeout after %lu ms: ",
+                fsm->request->index, fsm->request->subindex, diff_ms);
         ec_datagram_print_wc_error(datagram);
         return;
     }
 
 #if DEBUG_LONG
     if (diff_ms > 200) {
-        EC_SLAVE_WARN(slave, "SDO 0x%04x:%x upload took %u ms.\n",
-                fsm->request->index, fsm->request->subindex, (u32) diff_ms);
+        EC_SLAVE_WARN(slave, "SDO 0x%04x:%x upload took %lu ms.\n",
+                fsm->request->index, fsm->request->subindex, diff_ms);
     }
 #endif
 
@@ -1790,8 +1790,8 @@ void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */)
             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
         if (diff_ms >= fsm->request->response_timeout) {
             fsm->state = ec_fsm_coe_error;
-            EC_SLAVE_ERR(slave, "Timeout after %u ms while waiting for"
-                    " SDO 0x%04x:%x upload response.\n", (u32) diff_ms,
+            EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
+                    " SDO 0x%04x:%x upload response.\n", diff_ms,
                     fsm->request->index, fsm->request->subindex);
             return;
         }
diff --git a/master/fsm_master.c b/master/fsm_master.c
index a96a7a4c..cd0f1709 100644
--- a/master/fsm_master.c
+++ b/master/fsm_master.c
@@ -792,8 +792,8 @@ void ec_fsm_master_state_scan_slave(
         return;
     }
 
-    EC_MASTER_INFO(master, "Bus scanning completed in %u ms.\n",
-            (u32) (jiffies - fsm->scan_jiffies) * 1000 / HZ);
+    EC_MASTER_INFO(master, "Bus scanning completed in %lu ms.\n",
+            (jiffies - fsm->scan_jiffies) * 1000 / HZ);
 
     master->scan_busy = 0;
     wake_up_interruptible(&master->scan_queue);
@@ -907,9 +907,9 @@ u64 ec_fsm_master_dc_offset32(
 
     EC_SLAVE_DBG(slave, 1, "DC system time offset calculation:"
             " system_time=%u (corrected with %u),"
-            " app_time=%u, diff=%i\n",
+            " app_time=%llu, diff=%i\n",
             system_time32, correction,
-            (u32) slave->master->app_time, time_diff);
+            slave->master->app_time, time_diff);
 
     if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) {
         new_offset = time_diff + old_offset32;
diff --git a/master/fsm_slave_config.c b/master/fsm_slave_config.c
index d7f156cc..50f7e934 100644
--- a/master/fsm_slave_config.c
+++ b/master/fsm_slave_config.c
@@ -1242,11 +1242,11 @@ void ec_fsm_slave_config_state_dc_sync_check(
     if (abs_sync_diff > EC_DC_MAX_SYNC_DIFF_NS) {
 
         if (diff_ms >= EC_DC_SYNC_WAIT_MS) {
-            EC_SLAVE_WARN(slave, "Slave did not sync after %u ms.\n",
-                    (u32) diff_ms);
+            EC_SLAVE_WARN(slave, "Slave did not sync after %lu ms.\n",
+                    diff_ms);
         } else {
-            EC_SLAVE_DBG(slave, 1, "Sync after %4u ms: %10u ns\n",
-                    (u32) diff_ms, abs_sync_diff);
+            EC_SLAVE_DBG(slave, 1, "Sync after %4lu ms: %10u ns\n",
+                    diff_ms, abs_sync_diff);
 
             // check synchrony again
             ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4);
@@ -1254,8 +1254,8 @@ void ec_fsm_slave_config_state_dc_sync_check(
             return;
         }
     } else {
-        EC_SLAVE_DBG(slave, 1, "%u ns difference after %u ms.\n",
-                abs_sync_diff, (u32) diff_ms);
+        EC_SLAVE_DBG(slave, 1, "%u ns difference after %lu ms.\n",
+                abs_sync_diff, diff_ms);
     }
 
     // set DC start time
diff --git a/master/fsm_soe.c b/master/fsm_soe.c
index 1f400fe8..a43ba117 100644
--- a/master/fsm_soe.c
+++ b/master/fsm_soe.c
@@ -265,7 +265,7 @@ void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */)
         }
         fsm->state = ec_fsm_soe_error;
         EC_SLAVE_ERR(slave, "Reception of SoE read request"
-                " failed after %u ms: ", (u32) diff_ms);
+                " failed after %lu ms: ", diff_ms);
         ec_datagram_print_wc_error(datagram);
         ec_fsm_soe_print_error(fsm);
         return;
@@ -311,8 +311,8 @@ void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */)
             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
         if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
             fsm->state = ec_fsm_soe_error;
-            EC_SLAVE_ERR(slave, "Timeout after %u ms while waiting for"
-                    " read response.\n", (u32) diff_ms);
+            EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
+                    " read response.\n", diff_ms);
             ec_fsm_soe_print_error(fsm);
             return;
         }
@@ -563,7 +563,7 @@ void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */)
         }
         fsm->state = ec_fsm_soe_error;
         EC_SLAVE_ERR(slave, "Reception of SoE write request"
-                " failed after %u ms: ", (u32) diff_ms);
+                " failed after %lu ms: ", diff_ms);
         ec_datagram_print_wc_error(datagram);
         ec_fsm_soe_print_error(fsm);
         return;
@@ -613,8 +613,8 @@ void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */)
                 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
             if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
                 fsm->state = ec_fsm_soe_error;
-                EC_SLAVE_ERR(slave, "Timeout after %u ms while waiting"
-                        " for write response.\n", (u32) diff_ms);
+                EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
+                        " for write response.\n", diff_ms);
                 ec_fsm_soe_print_error(fsm);
                 return;
             }
diff --git a/master/slave_config.c b/master/slave_config.c
index 84bed3bc..63eeb255 100644
--- a/master/slave_config.c
+++ b/master/slave_config.c
@@ -428,7 +428,7 @@ int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index,
     }
 
     if (dir != EC_DIR_OUTPUT && dir != EC_DIR_INPUT) {
-        EC_CONFIG_ERR(sc, "Invalid direction %u!\n", (u32) dir);
+        EC_CONFIG_ERR(sc, "Invalid direction %u!\n", (unsigned int) dir);
         return -EINVAL;
     }
 
@@ -743,7 +743,7 @@ int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index,
 
     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
             "subindex = 0x%02X, value = %u)\n",
-            __func__, sc, index, subindex, (u32) value);
+            __func__, sc, index, subindex, (unsigned int) value);
 
     EC_WRITE_U8(data, value);
     return ecrt_slave_config_sdo(sc, index, subindex, data, 1);
diff --git a/master/voe_handler.c b/master/voe_handler.c
index 47eccd02..f0374b92 100644
--- a/master/voe_handler.c
+++ b/master/voe_handler.c
@@ -267,8 +267,8 @@ void ec_voe_handler_state_write_response(ec_voe_handler_t *voe)
                 (jiffies - voe->jiffies_start) * 1000 / HZ;
             if (diff_ms < EC_VOE_RESPONSE_TIMEOUT) {
                 EC_SLAVE_DBG(slave, 1, "Slave did not respond to"
-                        " VoE write request. Retrying after %u ms...\n",
-                        (u32) diff_ms);
+                        " VoE write request. Retrying after %lu ms...\n",
+                        diff_ms);
                 // no response; send request datagram again
                 return;
             }

From 9618b9dfe625a90ce143a6c56c729716ba1e6a89 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Tue, 25 May 2010 13:32:43 +0200
Subject: [PATCH 05/34] Added debug output for ecrt_slave_config_dc().

---
 master/slave_config.c | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/master/slave_config.c b/master/slave_config.c
index 63eeb255..cdd45ceb 100644
--- a/master/slave_config.c
+++ b/master/slave_config.c
@@ -687,6 +687,12 @@ void ecrt_slave_config_dc(ec_slave_config_t *sc, uint16_t assign_activate,
         uint32_t sync0_cycle_time, uint32_t sync0_shift_time,
         uint32_t sync1_cycle_time, uint32_t sync1_shift_time)
 {
+    EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, assign_activate = 0x%04X,"
+            " sync0_cycle = %u, sync0_shift = %u,"
+            " sync1_cycle = %u, sync1_shift = %u\n",
+            __func__, sc, assign_activate, sync0_cycle_time, sync0_shift_time,
+            sync1_cycle_time, sync1_shift_time);
+
     sc->dc_assign_activate = assign_activate;
     sc->dc_sync[0].cycle_time = sync0_cycle_time;
     sc->dc_sync[0].shift_time = sync0_shift_time;

From ca921529a2ad719a43091e9892ec50054159f9e7 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Tue, 25 May 2010 14:12:02 +0200
Subject: [PATCH 06/34] Introduced configure switch --enable-regalias for
 enabling reading alias addresses from registers. Disabled by default.

---
 configure.ac            | 24 ++++++++++++++++++++++++
 master/fsm_slave_scan.c | 27 ++++++++++++++++++++-------
 2 files changed, 44 insertions(+), 7 deletions(-)

diff --git a/configure.ac b/configure.ac
index 68d2ac70..3d8d93cf 100644
--- a/configure.ac
+++ b/configure.ac
@@ -512,6 +512,30 @@ if test "x${hrtimer}" = "x1"; then
     AC_DEFINE([EC_USE_HRTIMER], [1], [Use hrtimer for scheduling])
 fi
 
+#------------------------------------------------------------------------------
+# Read alias address from register
+#------------------------------------------------------------------------------
+
+AC_ARG_ENABLE([regalias],
+    AS_HELP_STRING([--enable-regalias],
+                   [Read alias adresses from register (default: no)]),
+    [
+        case "${enableval}" in
+            yes) regalias=1
+                ;;
+            no) regalias=0
+                ;;
+            *) AC_MSG_ERROR([Invalid value for --enable-regalias])
+                ;;
+        esac
+    ],
+    [regalias=0]
+)
+
+if test "x${regalias}" = "x1"; then
+    AC_DEFINE([EC_REGALIAS], [1], [Read alias adresses from register])
+fi
+
 #------------------------------------------------------------------------------
 # Command-line tool
 #-----------------------------------------------------------------------------
diff --git a/master/fsm_slave_scan.c b/master/fsm_slave_scan.c
index 7bc3a393..460a1336 100644
--- a/master/fsm_slave_scan.c
+++ b/master/fsm_slave_scan.c
@@ -52,7 +52,9 @@ void ec_fsm_slave_scan_state_dc_times(ec_fsm_slave_scan_t *);
 void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_scan_t *);
 void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *);
 void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *);
+#ifdef EC_REGALIAS
 void ec_fsm_slave_scan_state_regalias(ec_fsm_slave_scan_t *);
+#endif
 void ec_fsm_slave_scan_state_preop(ec_fsm_slave_scan_t *);
 void ec_fsm_slave_scan_state_sync(ec_fsm_slave_scan_t *);
 void ec_fsm_slave_scan_state_pdos(ec_fsm_slave_scan_t *);
@@ -61,7 +63,9 @@ void ec_fsm_slave_scan_state_end(ec_fsm_slave_scan_t *);
 void ec_fsm_slave_scan_state_error(ec_fsm_slave_scan_t *);
 
 void ec_fsm_slave_scan_enter_datalink(ec_fsm_slave_scan_t *);
+#ifdef EC_REGALIAS
 void ec_fsm_slave_scan_enter_regalias(ec_fsm_slave_scan_t *);
+#endif
 void ec_fsm_slave_scan_enter_preop(ec_fsm_slave_scan_t *);
 void ec_fsm_slave_scan_enter_pdos(ec_fsm_slave_scan_t *);
 
@@ -694,7 +698,15 @@ void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *fsm /**< slave state
         }
     }
 
+#ifdef EC_REGALIAS
     ec_fsm_slave_scan_enter_regalias(fsm);
+#else
+    if (slave->sii.mailbox_protocols & EC_MBOX_COE) {
+        ec_fsm_slave_scan_enter_preop(fsm);
+    } else {
+        fsm->state = ec_fsm_slave_scan_state_end;
+    }
+#endif
     return;
 
 end:
@@ -703,13 +715,12 @@ end:
     fsm->state = ec_fsm_slave_scan_state_error;
 }
 
-
 /*****************************************************************************/
 
-/**
-   Slave scan entry function: REGALIAS.
-*/
+#ifdef EC_REGALIAS
 
+/** Slave scan entry function: REGALIAS.
+ */
 void ec_fsm_slave_scan_enter_regalias(
         ec_fsm_slave_scan_t *fsm /**< slave state machine */
         )
@@ -727,9 +738,8 @@ void ec_fsm_slave_scan_enter_regalias(
 
 /*****************************************************************************/
 
-/**
-   Slave scan state: REGALIAS.
-*/
+/** Slave scan state: REGALIAS.
+ */
 void ec_fsm_slave_scan_state_regalias(
         ec_fsm_slave_scan_t *fsm /**< slave state machine */
         )
@@ -754,6 +764,7 @@ void ec_fsm_slave_scan_state_regalias(
         EC_SLAVE_DBG(slave, 1, "Read alias %u from register.\n",
                 slave->effective_alias);
     }
+
     if (slave->sii.mailbox_protocols & EC_MBOX_COE) {
         ec_fsm_slave_scan_enter_preop(fsm);
     } else {
@@ -761,6 +772,8 @@ void ec_fsm_slave_scan_state_regalias(
     }
 }
 
+#endif // defined EC_REGALIAS
+
 /*****************************************************************************/
 
 /** Enter slave scan state PREOP.

From db3b881e6b994288e68a440d8fee8528d65ecf02 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Tue, 1 Jun 2010 22:51:51 +0200
Subject: [PATCH 07/34] Fixed NULL pointer dereference on request error in RTAI
 example.

---
 examples/rtai/rtai_sample.c | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/examples/rtai/rtai_sample.c b/examples/rtai/rtai_sample.c
index eb31b78c..9748907f 100644
--- a/examples/rtai/rtai_sample.c
+++ b/examples/rtai/rtai_sample.c
@@ -76,8 +76,8 @@ static cycles_t t_last_cycle = 0, t_critical;
 // process data
 static uint8_t *domain1_pd; // process data memory
 
-#define AnaInSlavePos  0, 1
-#define DigOutSlavePos 0, 3
+#define AnaInSlavePos  0, 3
+#define DigOutSlavePos 0, 2
 
 #define Beckhoff_EL2004 0x00000002, 0x07D43052
 #define Beckhoff_EL3162 0x00000002, 0x0C5A3052
@@ -286,8 +286,8 @@ int __init init_mod(void)
     t_critical = cpu_khz * 1000 / FREQUENCY - cpu_khz * INHIBIT_TIME / 1000;
 
     master = ecrt_request_master(0);
-    if (IS_ERR(master)) {
-        ret = PTR_ERR(master); 
+    if (!master) {
+        ret = -EBUSY; 
         printk(KERN_ERR PFX "Requesting master 0 failed!\n");
         goto out_return;
     }

From 791d96288afde61126a2a7be04346f75832819c9 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Tue, 1 Jun 2010 23:05:36 +0200
Subject: [PATCH 08/34] TODO.

---
 TODO | 1 +
 1 file changed, 1 insertion(+)

diff --git a/TODO b/TODO
index f11971d2..75975722 100644
--- a/TODO
+++ b/TODO
@@ -42,6 +42,7 @@ Version 1.5.0:
 * Output SoE IDN configurations in 'ethercat config'.
 * Fix casting away constness during expected WC calculation.
 * Read AL status code on spontaneous state change.
+* Only output watchdog config if not default.
 
 Future issues:
 

From eb8abe9f9ad01b28bfe25b2adf1afe127e29bbe5 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Wed, 2 Jun 2010 14:27:04 +0200
Subject: [PATCH 09/34] TODO.

---
 TODO | 1 +
 1 file changed, 1 insertion(+)

diff --git a/TODO b/TODO
index f11971d2..4fa93945 100644
--- a/TODO
+++ b/TODO
@@ -42,6 +42,7 @@ Version 1.5.0:
 * Output SoE IDN configurations in 'ethercat config'.
 * Fix casting away constness during expected WC calculation.
 * Read AL status code on spontaneous state change.
+* Fix clearing domains etc. when not activated and releasing.
 
 Future issues:
 

From c442beaa024823f3482118c50bec5acf94949489 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Wed, 2 Jun 2010 22:46:48 +0200
Subject: [PATCH 10/34] Minor change.

---
 master/cdev.c | 1 -
 1 file changed, 1 deletion(-)

diff --git a/master/cdev.c b/master/cdev.c
index e2eca4db..2440c287 100644
--- a/master/cdev.c
+++ b/master/cdev.c
@@ -1724,7 +1724,6 @@ int ec_cdev_ioctl_deactivate(
     return 0;
 }
 
-
 /*****************************************************************************/
 
 /** Set max. number of databytes in a cycle

From 8b6d5f8af3754c1f4e6a4003332d6855e1126356 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Wed, 2 Jun 2010 22:54:08 +0200
Subject: [PATCH 11/34] Fixed bug that caused the configuration not to be
 cleared when reserved but not activated and then releasing.

---
 TODO            |  1 -
 master/master.c | 30 +++++++++++++++++++++---------
 master/master.h |  5 ++---
 3 files changed, 23 insertions(+), 13 deletions(-)

diff --git a/TODO b/TODO
index a4dbf68c..75975722 100644
--- a/TODO
+++ b/TODO
@@ -42,7 +42,6 @@ Version 1.5.0:
 * Output SoE IDN configurations in 'ethercat config'.
 * Fix casting away constness during expected WC calculation.
 * Read AL status code on spontaneous state change.
-* Fix clearing domains etc. when not activated and releasing.
 * Only output watchdog config if not default.
 
 Future issues:
diff --git a/master/master.c b/master/master.c
index 96c26260..4bc2f849 100644
--- a/master/master.c
+++ b/master/master.c
@@ -439,6 +439,20 @@ void ec_master_clear_domains(ec_master_t *master)
 
 /*****************************************************************************/
 
+/** Clear the configuration applied by the application.
+ */
+void ec_master_clear_config(
+        ec_master_t *master /**< EtherCAT master. */
+        )
+{
+    down(&master->master_sem);
+    ec_master_clear_domains(master);
+    ec_master_clear_slave_configs(master);
+    up(&master->master_sem);
+}
+
+/*****************************************************************************/
+
 /** Internal sending callback.
  */
 void ec_master_internal_send_cb(
@@ -653,15 +667,17 @@ void ec_master_leave_operation_phase(
         ec_master_t *master /**< EtherCAT master */
         )
 {
-    if (master->active)
-        ecrt_master_deactivate(master);
+    if (master->active) {
+        ecrt_master_deactivate(master); // also clears config
+    } else {
+        ec_master_clear_config(master);
+    }
 
     EC_MASTER_DBG(master, 1, "OPERATION -> IDLE.\n");
 
     master->phase = EC_IDLE;
 }
 
-
 /*****************************************************************************/
 
 /** Injects external datagrams that fit into the datagram queue.
@@ -1989,8 +2005,7 @@ void ecrt_master_deactivate(ec_master_t *master)
     int eoe_was_running;
 #endif
 
-    EC_MASTER_DBG(master, 1, "ecrt_master_deactivate(master = 0x%p)\n",
-            master);
+    EC_MASTER_DBG(master, 1, "%s(master = 0x%p)\n", __func__, master);
 
     if (!master->active) {
         EC_MASTER_WARN(master, "%s: Master not active.\n", __func__);
@@ -2007,10 +2022,7 @@ void ecrt_master_deactivate(ec_master_t *master)
     master->receive_cb = ec_master_internal_receive_cb;
     master->cb_data = master;
     
-    down(&master->master_sem);
-    ec_master_clear_domains(master);
-    ec_master_clear_slave_configs(master);
-    up(&master->master_sem);
+    ec_master_clear_config(master);
 
     for (slave = master->slaves;
             slave < master->slaves + master->slave_count;
diff --git a/master/master.h b/master/master.h
index c4ea963b..d0cd7b8a 100644
--- a/master/master.h
+++ b/master/master.h
@@ -170,11 +170,12 @@ struct ec_master {
     unsigned int injection_seq_rt; /**< Datagram injection sequence number
                                      for the realtime side. */
 
-
     ec_slave_t *slaves; /**< Array of slaves on the bus. */
     unsigned int slave_count; /**< Number of slaves on the bus. */
 
+    /* Configuration applied by the application. */
     struct list_head configs; /**< List of slave configurations. */
+    struct list_head domains; /**< List of domains. */
     
     u64 app_time; /**< Time of the last ecrt_master_sync() call. */
     u64 app_start_time; /**< Application start time. */
@@ -213,7 +214,6 @@ struct ec_master {
     struct list_head external_datagram_queue; /**< External Datagram queue. */
     unsigned int send_interval; /**< Interval between calls to ecrt_master_send */
     size_t max_queue_size; /**< Maximum size of datagram queue */
-    struct list_head domains; /**< List of domains. */
 
     unsigned int debug_level; /**< Master debug level. */
     ec_stats_t stats; /**< Cyclic statistics. */
@@ -242,7 +242,6 @@ struct ec_master {
 
     struct list_head reg_requests; /**< Register requests. */
     wait_queue_head_t reg_queue; /**< Wait queue for register requests. */
-
 };
 
 /*****************************************************************************/

From 063805d54ecfdaeada41f14df8e1aef3d75672b1 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Thu, 3 Jun 2010 14:36:41 +0200
Subject: [PATCH 12/34] TODO.

---
 TODO | 1 +
 1 file changed, 1 insertion(+)

diff --git a/TODO b/TODO
index 4fa93945..f6de929c 100644
--- a/TODO
+++ b/TODO
@@ -43,6 +43,7 @@ Version 1.5.0:
 * Fix casting away constness during expected WC calculation.
 * Read AL status code on spontaneous state change.
 * Fix clearing domains etc. when not activated and releasing.
+* Output -EPROTO if mailbox protocol is not supported.
 
 Future issues:
 

From d975ec2586f635c638cdbb29c6cf4e9836c89c59 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Sun, 6 Jun 2010 21:07:36 +0200
Subject: [PATCH 13/34] Minor change.

---
 examples/dc_user/main.c | 1 -
 1 file changed, 1 deletion(-)

diff --git a/examples/dc_user/main.c b/examples/dc_user/main.c
index 1eb1f714..4685b87e 100644
--- a/examples/dc_user/main.c
+++ b/examples/dc_user/main.c
@@ -143,7 +143,6 @@ void cyclic_task()
 
         // check for master state (optional)
         check_master_state();
-
     }
 
     if (blink_counter) {

From 74230aaf04b7e2bcd0ec694afd07cffa19d1f237 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Sun, 6 Jun 2010 21:32:50 +0200
Subject: [PATCH 14/34] TODO.

---
 TODO | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/TODO b/TODO
index 514dbac8..5a035353 100644
--- a/TODO
+++ b/TODO
@@ -45,6 +45,9 @@ Version 1.5.0:
 * Fix clearing domains etc. when not activated and releasing.
 * Output -EPROTO if mailbox protocol is not supported.
 * Only output watchdog config if not default.
+* Do not output 'SDO does not exist' when querying data type.
+* Always write down PDO assignment/mapping in doubt? If not, add an interface
+  for enforcing this.
 
 Future issues:
 

From 26d4687d0e571aa79e1c0392df0cfa60c11328f2 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Sun, 6 Jun 2010 22:40:22 +0200
Subject: [PATCH 15/34] Added state parameter to ecrt_slave_config_idn().

---
 include/ecrt.h            |  13 +++
 lib/slave_config.c        |   3 +-
 master/cdev.c             |   2 +-
 master/fsm_slave_config.c | 161 +++++++++++++++++++++++++++++++-------
 master/ioctl.h            |   1 +
 master/slave_config.c     |  14 +++-
 master/soe_request.c      |   2 +
 master/soe_request.h      |   1 +
 8 files changed, 163 insertions(+), 34 deletions(-)

diff --git a/include/ecrt.h b/include/ecrt.h
index d27dded2..73607917 100644
--- a/include/ecrt.h
+++ b/include/ecrt.h
@@ -382,6 +382,17 @@ typedef enum {
     EC_REQUEST_ERROR, /**< Request processing failed. */
 } ec_request_state_t;
 
+/*****************************************************************************/
+
+/** Application-layer state.
+ */
+typedef enum {
+    EC_AL_STATE_INIT = 1, /**< Init. */
+    EC_AL_STATE_PREOP = 2, /**< Pre-operational. */
+    EC_AL_STATE_SAFEOP = 4, /**< Safe-operational. */
+    EC_AL_STATE_OP = 8, /**< Operational. */
+} ec_al_state_t;
+
 /******************************************************************************
  * Global functions
  *****************************************************************************/
@@ -1202,6 +1213,8 @@ void ecrt_slave_config_state(
 int ecrt_slave_config_idn(
         ec_slave_config_t *sc, /**< Slave configuration. */
         uint16_t idn, /**< SoE IDN. */
+        ec_al_state_t state, /**< AL state in which to write the IDN (PREOP or
+                               SAFEOP). */
         const uint8_t *data, /**< Pointer to the data. */
         size_t size /**< Size of the \a data. */
         );
diff --git a/lib/slave_config.c b/lib/slave_config.c
index d73e238e..5eaf416c 100644
--- a/lib/slave_config.c
+++ b/lib/slave_config.c
@@ -473,12 +473,13 @@ void ecrt_slave_config_state(const ec_slave_config_t *sc,
 /*****************************************************************************/
 
 int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn,
-        const uint8_t *data, size_t size)
+        ec_al_state_t state, const uint8_t *data, size_t size)
 {
     ec_ioctl_sc_idn_t io;
 
     io.config_index = sc->index;
     io.idn = idn;
+    io.state = state;
     io.data = data;
     io.size = size;
 
diff --git a/master/cdev.c b/master/cdev.c
index 2440c287..49deddd7 100644
--- a/master/cdev.c
+++ b/master/cdev.c
@@ -2463,7 +2463,7 @@ int ec_cdev_ioctl_sc_idn(
 
     up(&master->master_sem); // FIXME
 
-    ret = ecrt_slave_config_idn(sc, ioctl.idn, data, ioctl.size);
+    ret = ecrt_slave_config_idn(sc, ioctl.idn, ioctl.state, data, ioctl.size);
     kfree(data);
     return ret;
 }
diff --git a/master/fsm_slave_config.c b/master/fsm_slave_config.c
index 50f7e934..40454ec7 100644
--- a/master/fsm_slave_config.c
+++ b/master/fsm_slave_config.c
@@ -69,7 +69,7 @@ void ec_fsm_slave_config_state_dc_clear_assign(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *);
-void ec_fsm_slave_config_state_soe_conf(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_soe_conf_preop(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_watchdog_divider(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_watchdog(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *);
@@ -80,6 +80,7 @@ void ec_fsm_slave_config_state_dc_sync_check(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_soe_conf_safeop(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);
 
 void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *);
@@ -88,7 +89,7 @@ void ec_fsm_slave_config_enter_dc_clear_assign(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);
-void ec_fsm_slave_config_enter_soe_conf(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_soe_conf_preop(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_watchdog_divider(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_watchdog(ec_fsm_slave_config_t *);
@@ -96,6 +97,8 @@ void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_dc_cycle(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_soe_conf_safeop(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_op(ec_fsm_slave_config_t *);
 
 void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);
@@ -658,7 +661,7 @@ void ec_fsm_slave_config_enter_sdo_conf(
 
     // No CoE configuration to be applied?
     if (list_empty(&slave->config->sdo_configs)) { // skip SDO configuration
-        ec_fsm_slave_config_enter_soe_conf(fsm);
+        ec_fsm_slave_config_enter_soe_conf_preop(fsm);
         return;
     }
 
@@ -706,47 +709,50 @@ void ec_fsm_slave_config_state_sdo_conf(
     }
 
     // All SDOs are now configured.
-    ec_fsm_slave_config_enter_soe_conf(fsm);
+    ec_fsm_slave_config_enter_soe_conf_preop(fsm);
 }
 
 /*****************************************************************************/
 
 /** Check for SoE configurations to be applied.
  */
-void ec_fsm_slave_config_enter_soe_conf(
+void ec_fsm_slave_config_enter_soe_conf_preop(
         ec_fsm_slave_config_t *fsm /**< slave state machine */
         )
 {
     ec_slave_t *slave = fsm->slave;
     ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe;
+    ec_soe_request_t *req;
 
     if (!slave->config) {
         ec_fsm_slave_config_enter_pdo_sync(fsm);
         return;
     }
 
-    // No SoE configuration to be applied?
-    if (list_empty(&slave->config->soe_configs)) { // skip configuration
-        ec_fsm_slave_config_enter_pdo_conf(fsm);
-        return;
+    list_for_each_entry(req, &slave->config->soe_configs, list) {
+        if (req->al_state == EC_AL_STATE_PREOP) {
+            // start SoE configuration
+            fsm->state = ec_fsm_slave_config_state_soe_conf_preop;
+            fsm->soe_request = req;
+            ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request);
+            ec_soe_request_write(&fsm->soe_request_copy);
+            ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
+            ec_fsm_soe_exec(fsm_soe); // execute immediately
+            ec_master_queue_external_datagram(slave->master,
+                    fsm_soe->datagram);
+            return;
+        }
     }
 
-    // start SoE configuration
-    fsm->state = ec_fsm_slave_config_state_soe_conf;
-    fsm->soe_request = list_entry(fsm->slave->config->soe_configs.next,
-            ec_soe_request_t, list);
-    ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request);
-    ec_soe_request_write(&fsm->soe_request_copy);
-    ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
-    ec_fsm_soe_exec(fsm_soe); // execute immediately
-    ec_master_queue_external_datagram(slave->master, fsm_soe->datagram);
+    // No SoE configuration to be applied in PREOP
+    ec_fsm_slave_config_enter_pdo_conf(fsm);
 }
 
 /*****************************************************************************/
 
 /** Slave configuration state: SOE_CONF.
  */
-void ec_fsm_slave_config_state_soe_conf(
+void ec_fsm_slave_config_state_soe_conf_preop(
         ec_fsm_slave_config_t *fsm /**< slave state machine */
         )
 {
@@ -770,19 +776,22 @@ void ec_fsm_slave_config_state_soe_conf(
         return;
     }
 
-    // Another IDN to configure?
-    if (fsm->soe_request->list.next != &fsm->slave->config->soe_configs) {
+    // Another IDN to configure in PREOP?
+    while (fsm->soe_request->list.next != &fsm->slave->config->soe_configs) {
         fsm->soe_request = list_entry(fsm->soe_request->list.next,
                 ec_soe_request_t, list);
-        ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request);
-        ec_soe_request_write(&fsm->soe_request_copy);
-        ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
-        ec_fsm_soe_exec(fsm_soe); // execute immediately
-        ec_master_queue_external_datagram(slave->master, fsm_soe->datagram);
-        return;
+        if (fsm->soe_request->al_state == EC_AL_STATE_PREOP) {
+            ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request);
+            ec_soe_request_write(&fsm->soe_request_copy);
+            ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
+            ec_fsm_soe_exec(fsm_soe); // execute immediately
+            ec_master_queue_external_datagram(slave->master,
+                    fsm_soe->datagram);
+            return;
+        }
     }
 
-    // All SDOs are now configured.
+    // All PREOP IDNs are now configured.
     ec_fsm_slave_config_enter_pdo_conf(fsm);
 }
 
@@ -1416,9 +1425,103 @@ void ec_fsm_slave_config_state_safeop(
         return;
     }
 
+    ec_fsm_slave_config_enter_soe_conf_safeop(fsm);
+}
+
+/*****************************************************************************/
+
+/** Check for SoE configurations to be applied in SAFEOP.
+ */
+void ec_fsm_slave_config_enter_soe_conf_safeop(
+        ec_fsm_slave_config_t *fsm /**< slave state machine */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe;
+    ec_soe_request_t *req;
+
+    if (!slave->config) {
+        ec_fsm_slave_config_enter_op(fsm);
+        return;
+    }
+
+    list_for_each_entry(req, &slave->config->soe_configs, list) {
+        if (req->al_state == EC_AL_STATE_SAFEOP) {
+            // start SoE configuration
+            fsm->state = ec_fsm_slave_config_state_soe_conf_safeop;
+            fsm->soe_request = req;
+            ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request);
+            ec_soe_request_write(&fsm->soe_request_copy);
+            ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
+            ec_fsm_soe_exec(fsm_soe); // execute immediately
+            ec_master_queue_external_datagram(slave->master,
+                    fsm_soe->datagram);
+            return;
+        }
+    }
+
+    // No SoE configuration to be applied in SAFEOP
+    ec_fsm_slave_config_enter_op(fsm);
+}
+
+/*****************************************************************************/
+
+/** Slave configuration state: SOE_CONF.
+ */
+void ec_fsm_slave_config_state_soe_conf_safeop(
+        ec_fsm_slave_config_t *fsm /**< slave state machine */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe;
+
+    if (ec_fsm_soe_exec(fsm_soe)) {
+        ec_master_queue_external_datagram(slave->master, fsm_soe->datagram);
+        return;
+    }
+
+    if (!ec_fsm_soe_success(fsm_soe)) {
+        EC_SLAVE_ERR(slave, "SoE configuration failed.\n");
+        fsm->slave->error_flag = 1;
+        fsm->state = ec_fsm_slave_config_state_error;
+        return;
+    }
+
+    if (!fsm->slave->config) { // config removed in the meantime
+        ec_fsm_slave_config_reconfigure(fsm);
+        return;
+    }
+
+    // Another IDN to configure in SAFEOP?
+    while (fsm->soe_request->list.next != &fsm->slave->config->soe_configs) {
+        fsm->soe_request = list_entry(fsm->soe_request->list.next,
+                ec_soe_request_t, list);
+        if (fsm->soe_request->al_state == EC_AL_STATE_SAFEOP) {
+            ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request);
+            ec_soe_request_write(&fsm->soe_request_copy);
+            ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
+            ec_fsm_soe_exec(fsm_soe); // execute immediately
+            ec_master_queue_external_datagram(slave->master,
+                    fsm_soe->datagram);
+            return;
+        }
+    }
+
+    // All SAFEOP IDNs are now configured.
+    ec_fsm_slave_config_enter_op(fsm);
+}
+
+/*****************************************************************************/
+
+/** Bring slave to OP.
+ */
+void ec_fsm_slave_config_enter_op(
+        ec_fsm_slave_config_t *fsm /**< slave state machine */
+        )
+{
     // set state to OP
     fsm->state = ec_fsm_slave_config_state_op;
-    ec_fsm_change_start(fsm->fsm_change, slave, EC_SLAVE_STATE_OP);
+    ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_OP);
     ec_fsm_change_exec(fsm->fsm_change); // execute immediately
 }
 
diff --git a/master/ioctl.h b/master/ioctl.h
index e0b621bb..d21a6785 100644
--- a/master/ioctl.h
+++ b/master/ioctl.h
@@ -583,6 +583,7 @@ typedef struct {
     // inputs
     uint32_t config_index;
     uint16_t idn;
+    ec_al_state_t state;
     const uint8_t *data;
     size_t size;
 } ec_ioctl_sc_idn_t;
diff --git a/master/slave_config.c b/master/slave_config.c
index cdd45ceb..f99667d8 100644
--- a/master/slave_config.c
+++ b/master/slave_config.c
@@ -939,14 +939,21 @@ void ecrt_slave_config_state(const ec_slave_config_t *sc,
 /*****************************************************************************/
 
 int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn,
-        const uint8_t *data, size_t size)
+        ec_al_state_t state, const uint8_t *data, size_t size)
 {
     ec_slave_t *slave = sc->slave;
     ec_soe_request_t *req;
     int ret;
 
-    EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, idn = 0x%04X, "
-            "data = 0x%p, size = %zu)\n", __func__, sc, idn, data, size);
+    EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, idn = 0x%04X, state = %u, "
+            "data = 0x%p, size = %zu)\n",
+            __func__, sc, idn, state, data, size);
+
+    if (state != EC_AL_STATE_PREOP && state != EC_AL_STATE_SAFEOP) {
+        EC_CONFIG_ERR(sc, "AL state for IDN config"
+                " must be PREOP or SAFEOP!\n");
+        return -EINVAL;
+    }
 
     if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
         EC_CONFIG_WARN(sc, "Attached slave does not support SoE!\n");
@@ -961,6 +968,7 @@ int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn,
 
     ec_soe_request_init(req);
     ec_soe_request_set_idn(req, idn);
+    req->al_state = state;
 
     ret = ec_soe_request_copy_data(req, data, size);
     if (ret < 0) {
diff --git a/master/soe_request.c b/master/soe_request.c
index d77035a2..a3a058df 100644
--- a/master/soe_request.c
+++ b/master/soe_request.c
@@ -56,6 +56,7 @@ void ec_soe_request_init(
         ec_soe_request_t *req /**< SoE request. */
         )
 {
+    req->al_state = EC_AL_STATE_INIT;
     req->data = NULL;
     req->mem_size = 0;
     req->data_size = 0;
@@ -86,6 +87,7 @@ int ec_soe_request_copy(
         )
 {
     req->idn = other->idn;
+    req->al_state = other->al_state;
     return ec_soe_request_copy_data(req, other->data, other->data_size);
 }
 
diff --git a/master/soe_request.h b/master/soe_request.h
index 4d285e06..40d0fe7f 100644
--- a/master/soe_request.h
+++ b/master/soe_request.h
@@ -48,6 +48,7 @@
 typedef struct {
     struct list_head list; /**< List item. */
     uint16_t idn; /**< Sercos ID-Number. */
+    ec_al_state_t al_state; /**< AL state (only valid for IDN config). */
     uint8_t *data; /**< Pointer to SDO data. */
     size_t mem_size; /**< Size of SDO data memory. */
     size_t data_size; /**< Size of SDO data. */

From 5c131113ddc3afa7b16403a7ca4305ae6652b999 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Sun, 6 Jun 2010 23:16:30 +0200
Subject: [PATCH 16/34] Incremented ioctl interface version; Minor changes.

---
 lib/slave_config.c | 4 ++--
 master/cdev.c      | 3 ++-
 master/ioctl.h     | 4 ++--
 3 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/lib/slave_config.c b/lib/slave_config.c
index 5eaf416c..49ce635b 100644
--- a/lib/slave_config.c
+++ b/lib/slave_config.c
@@ -473,13 +473,13 @@ void ecrt_slave_config_state(const ec_slave_config_t *sc,
 /*****************************************************************************/
 
 int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn,
-        ec_al_state_t state, const uint8_t *data, size_t size)
+        ec_al_state_t al_state, const uint8_t *data, size_t size)
 {
     ec_ioctl_sc_idn_t io;
 
     io.config_index = sc->index;
     io.idn = idn;
-    io.state = state;
+    io.al_state = al_state;
     io.data = data;
     io.size = size;
 
diff --git a/master/cdev.c b/master/cdev.c
index 49deddd7..167a83de 100644
--- a/master/cdev.c
+++ b/master/cdev.c
@@ -2463,7 +2463,8 @@ int ec_cdev_ioctl_sc_idn(
 
     up(&master->master_sem); // FIXME
 
-    ret = ecrt_slave_config_idn(sc, ioctl.idn, ioctl.state, data, ioctl.size);
+    ret = ecrt_slave_config_idn(
+            sc, ioctl.idn, ioctl.al_state, data, ioctl.size);
     kfree(data);
     return ret;
 }
diff --git a/master/ioctl.h b/master/ioctl.h
index d21a6785..2606822a 100644
--- a/master/ioctl.h
+++ b/master/ioctl.h
@@ -56,7 +56,7 @@
  *
  * Increment this when changing the ioctl interface!
  */
-#define EC_IOCTL_VERSION_MAGIC 5
+#define EC_IOCTL_VERSION_MAGIC 6
 
 // Command-line tool
 #define EC_IOCTL_MODULE                EC_IOR(0x00, ec_ioctl_module_t)
@@ -583,7 +583,7 @@ typedef struct {
     // inputs
     uint32_t config_index;
     uint16_t idn;
-    ec_al_state_t state;
+    ec_al_state_t al_state;
     const uint8_t *data;
     size_t size;
 } ec_ioctl_sc_idn_t;

From 3b3221bc89b2185d35d20cc9dcd2dd419047769a Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Sun, 6 Jun 2010 23:21:09 +0200
Subject: [PATCH 17/34] Ignore files ending with ~.

---
 .hgignore | 1 +
 1 file changed, 1 insertion(+)

diff --git a/.hgignore b/.hgignore
index 41dd18cf..342b22ec 100644
--- a/.hgignore
+++ b/.hgignore
@@ -7,6 +7,7 @@ syntax: glob
 **.o
 **.o.cmd
 **.swp
+**~
 .tmp_versions
 ChangeLog
 Doxyfile

From 324b9758a26091c8e67cd254544befe97f51da94 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Tue, 6 Jul 2010 15:09:57 +0200
Subject: [PATCH 18/34] Implemented ecrt_master_read_idn() and
 ecrt_master_write_idn() in kernel space.

---
 include/ecrt.h  |   8 +--
 lib/master.c    |   4 +-
 master/cdev.c   | 151 ++++++++++--------------------------------------
 master/master.c | 146 ++++++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 183 insertions(+), 126 deletions(-)

diff --git a/include/ecrt.h b/include/ecrt.h
index 73607917..fa1b3498 100644
--- a/include/ecrt.h
+++ b/include/ecrt.h
@@ -674,6 +674,8 @@ int ecrt_master_sdo_upload(
         uint32_t *abort_code /**< Abort code of the SDO upload. */
         );
 
+#endif /* #ifndef __KERNEL__ */
+
 /** Executes an SoE write request.
  *
  * Starts writing an IDN and blocks until the request was processed, or an
@@ -688,7 +690,7 @@ int ecrt_master_write_idn(
         uint16_t idn, /**< SoE IDN (see ecrt_slave_config_idn()). */
         uint8_t *data, /**< Pointer to data to write. */
         size_t data_size, /**< Size of data to write. */
-        uint32_t *error_code /**< Pointer to variable, where an SoE error code
+        uint16_t *error_code /**< Pointer to variable, where an SoE error code
                                can be stored. */
         );
 
@@ -708,12 +710,10 @@ int ecrt_master_read_idn(
                            stored. */
         size_t target_size, /**< Size of the memory \a target points to. */
         size_t *result_size, /**< Actual size of the received data. */
-        uint32_t *error_code /**< Pointer to variable, where an SoE error code
+        uint16_t *error_code /**< Pointer to variable, where an SoE error code
                                can be stored. */
         );
 
-#endif /* #ifndef __KERNEL__ */
-
 /** Finishes the configuration phase and prepares for cyclic operation.
  *
  * This function tells the master that the configuration phase is finished and
diff --git a/lib/master.c b/lib/master.c
index 5b4f0580..0c9d7dbb 100644
--- a/lib/master.c
+++ b/lib/master.c
@@ -305,7 +305,7 @@ int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position,
 /*****************************************************************************/
 
 int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
-        uint16_t idn, uint8_t *data, size_t data_size, uint32_t *error_code)
+        uint16_t idn, uint8_t *data, size_t data_size, uint16_t *error_code)
 {
     ec_ioctl_slave_soe_write_t io;
 
@@ -329,7 +329,7 @@ int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
 
 int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position,
         uint16_t idn, uint8_t *target, size_t target_size,
-        size_t *result_size, uint32_t *error_code)
+        size_t *result_size, uint16_t *error_code)
 {
     ec_ioctl_slave_soe_read_t io;
 
diff --git a/master/cdev.c b/master/cdev.c
index 167a83de..0692d6f3 100644
--- a/master/cdev.c
+++ b/master/cdev.c
@@ -3319,86 +3319,40 @@ int ec_cdev_ioctl_slave_soe_read(
         )
 {
     ec_ioctl_slave_soe_read_t ioctl;
-    ec_master_soe_request_t request;
+    u8 *data;
     int retval;
 
     if (copy_from_user(&ioctl, (void __user *) arg, sizeof(ioctl))) {
         return -EFAULT;
     }
 
-    ec_soe_request_init(&request.req);
-    ec_soe_request_set_idn(&request.req, ioctl.idn);
-    ec_soe_request_read(&request.req);
-
-    if (down_interruptible(&master->master_sem))
-        return -EINTR;
-
-    if (!(request.slave = ec_master_find_slave(
-                    master, 0, ioctl.slave_position))) {
-        up(&master->master_sem);
-        ec_soe_request_clear(&request.req);
-        EC_MASTER_ERR(master, "Slave %u does not exist!\n",
-                ioctl.slave_position);
-        return -EINVAL;
+    data = kmalloc(ioctl.mem_size, GFP_KERNEL);
+    if (!data) {
+        EC_MASTER_ERR(master, "Failed to allocate %u bytes of IDN data.\n",
+                ioctl.mem_size);
+        return -ENOMEM;
     }
 
-    // schedule request.
-    list_add_tail(&request.list, &request.slave->soe_requests);
-
-    up(&master->master_sem);
-
-    EC_SLAVE_DBG(request.slave, 1, "Scheduled SoE read request.\n");
-
-    // wait for processing through FSM
-    if (wait_event_interruptible(request.slave->soe_queue,
-                request.req.state != EC_INT_REQUEST_QUEUED)) {
-        // interrupted by signal
-        down(&master->master_sem);
-        if (request.req.state == EC_INT_REQUEST_QUEUED) {
-            list_del(&request.list);
-            up(&master->master_sem);
-            ec_soe_request_clear(&request.req);
-            return -EINTR;
-        }
-        // request already processing: interrupt not possible.
-        up(&master->master_sem);
+    retval = ecrt_master_read_idn(master, ioctl.slave_position,
+            ioctl.idn, data, ioctl.mem_size, &ioctl.data_size,
+            &ioctl.error_code);
+    if (retval) {
+        kfree(data);
+        return retval;
     }
 
-    // wait until master FSM has finished processing
-    wait_event(request.slave->soe_queue,
-            request.req.state != EC_INT_REQUEST_BUSY);
-
-    ioctl.error_code = request.req.error_code;
-
-    EC_SLAVE_DBG(request.slave, 1, "Read %zd bytes via SoE.\n",
-            request.req.data_size);
-
-    if (request.req.state != EC_INT_REQUEST_SUCCESS) {
-        ioctl.data_size = 0;
-        retval = -EIO;
-    } else {
-        if (request.req.data_size > ioctl.mem_size) {
-            EC_MASTER_ERR(master, "Buffer too small.\n");
-            ec_soe_request_clear(&request.req);
-            return -EOVERFLOW;
-        }
-        ioctl.data_size = request.req.data_size;
-        if (copy_to_user((void __user *) ioctl.data,
-                    request.req.data, ioctl.data_size)) {
-            ec_soe_request_clear(&request.req);
-            return -EFAULT;
-        }
-        retval = 0;
+    if (copy_to_user((void __user *) ioctl.data,
+                data, ioctl.data_size)) {
+        kfree(data);
+        return -EFAULT;
     }
+    kfree(data);
 
     if (__copy_to_user((void __user *) arg, &ioctl, sizeof(ioctl))) {
         retval = -EFAULT;
     }
 
-    EC_SLAVE_DBG(request.slave, 1, "Finished SoE read request.\n");
-
-    ec_soe_request_clear(&request.req);
-
+    EC_MASTER_DBG(master, 1, "Finished SoE read request.\n");
     return retval;
 }
 
@@ -3412,79 +3366,36 @@ int ec_cdev_ioctl_slave_soe_write(
         )
 {
     ec_ioctl_slave_soe_write_t ioctl;
-    ec_master_soe_request_t request;
+    u8 *data;
     int retval;
 
     if (copy_from_user(&ioctl, (void __user *) arg, sizeof(ioctl))) {
         return -EFAULT;
     }
 
-    INIT_LIST_HEAD(&request.list);
-
-    ec_soe_request_init(&request.req);
-    ec_soe_request_set_idn(&request.req, ioctl.idn);
-
-    if (ec_soe_request_alloc(&request.req, ioctl.data_size)) {
-        ec_soe_request_clear(&request.req);
+    data = kmalloc(ioctl.data_size, GFP_KERNEL);
+    if (!data) {
+        EC_MASTER_ERR(master, "Failed to allocate %u bytes of IDN data.\n",
+                ioctl.data_size);
         return -ENOMEM;
     }
-    if (copy_from_user(request.req.data,
-                (void __user *) ioctl.data, ioctl.data_size)) {
-        ec_soe_request_clear(&request.req);
+    if (copy_from_user(data, (void __user *) ioctl.data, ioctl.data_size)) {
+        kfree(data);
         return -EFAULT;
     }
-    request.req.data_size = ioctl.data_size;
-    ec_soe_request_write(&request.req);
 
-    if (down_interruptible(&master->master_sem))
-        return -EINTR;
-
-    if (!(request.slave = ec_master_find_slave(
-                    master, 0, ioctl.slave_position))) {
-        up(&master->master_sem);
-        EC_MASTER_ERR(master, "Slave %u does not exist!\n",
-                ioctl.slave_position);
-        ec_soe_request_clear(&request.req);
-        return -EINVAL;
+    retval = ecrt_master_write_idn(master, ioctl.slave_position,
+            ioctl.idn, data, ioctl.data_size, &ioctl.error_code);
+    kfree(data);
+    if (retval) {
+        return retval;
     }
 
-    EC_SLAVE_DBG(request.slave, 1, "Scheduling SoE write request.\n");
-
-    // schedule SoE write request.
-    list_add_tail(&request.list, &request.slave->soe_requests);
-
-    up(&master->master_sem);
-
-    // wait for processing through FSM
-    if (wait_event_interruptible(request.slave->soe_queue,
-                request.req.state != EC_INT_REQUEST_QUEUED)) {
-        // interrupted by signal
-        down(&master->master_sem);
-        if (request.req.state == EC_INT_REQUEST_QUEUED) {
-            // abort request
-            list_del(&request.list);
-            up(&master->master_sem);
-            ec_soe_request_clear(&request.req);
-            return -EINTR;
-        }
-        up(&master->master_sem);
-    }
-
-    // wait until master FSM has finished processing
-    wait_event(request.slave->soe_queue,
-            request.req.state != EC_INT_REQUEST_BUSY);
-
-    ioctl.error_code = request.req.error_code;
-    retval = request.req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
-
     if (__copy_to_user((void __user *) arg, &ioctl, sizeof(ioctl))) {
         retval = -EFAULT;
     }
 
-    ec_soe_request_clear(&request.req);
-
-    EC_SLAVE_DBG(request.slave, 1, "Finished SoE write request.\n");
-
+    EC_MASTER_DBG(master, 1, "Finished SoE write request.\n");
     return retval;
 }
 
diff --git a/master/master.c b/master/master.c
index 4bc2f849..54e600d7 100644
--- a/master/master.c
+++ b/master/master.c
@@ -2344,6 +2344,150 @@ uint32_t ecrt_master_sync_monitor_process(ec_master_t *master)
 
 /*****************************************************************************/
 
+int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
+        uint16_t idn, uint8_t *data, size_t data_size, uint16_t *error_code)
+{
+    ec_master_soe_request_t request;
+    int retval;
+
+    INIT_LIST_HEAD(&request.list);
+    ec_soe_request_init(&request.req);
+    ec_soe_request_set_idn(&request.req, idn);
+
+    if (ec_soe_request_alloc(&request.req, data_size)) {
+        ec_soe_request_clear(&request.req);
+        return -ENOMEM;
+    }
+
+    memcpy(request.req.data, data, data_size);
+    request.req.data_size = data_size;
+    ec_soe_request_write(&request.req);
+
+    if (down_interruptible(&master->master_sem))
+        return -EINTR;
+
+    if (!(request.slave = ec_master_find_slave(
+                    master, 0, slave_position))) {
+        up(&master->master_sem);
+        EC_MASTER_ERR(master, "Slave %u does not exist!\n",
+                slave_position);
+        ec_soe_request_clear(&request.req);
+        return -EINVAL;
+    }
+
+    EC_SLAVE_DBG(request.slave, 1, "Scheduling SoE write request.\n");
+
+    // schedule SoE write request.
+    list_add_tail(&request.list, &request.slave->soe_requests);
+
+    up(&master->master_sem);
+
+    // wait for processing through FSM
+    if (wait_event_interruptible(request.slave->soe_queue,
+                request.req.state != EC_INT_REQUEST_QUEUED)) {
+        // interrupted by signal
+        down(&master->master_sem);
+        if (request.req.state == EC_INT_REQUEST_QUEUED) {
+            // abort request
+            list_del(&request.list);
+            up(&master->master_sem);
+            ec_soe_request_clear(&request.req);
+            return -EINTR;
+        }
+        up(&master->master_sem);
+    }
+
+    // wait until master FSM has finished processing
+    wait_event(request.slave->soe_queue,
+            request.req.state != EC_INT_REQUEST_BUSY);
+
+    if (error_code) {
+        *error_code = request.req.error_code;
+    }
+    retval = request.req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
+    ec_soe_request_clear(&request.req);
+
+    return retval;
+}
+
+/*****************************************************************************/
+
+int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position,
+        uint16_t idn, uint8_t *target, size_t target_size,
+        size_t *result_size, uint16_t *error_code)
+{
+    ec_master_soe_request_t request;
+
+    INIT_LIST_HEAD(&request.list);
+    ec_soe_request_init(&request.req);
+    ec_soe_request_set_idn(&request.req, idn);
+    ec_soe_request_read(&request.req);
+
+    if (down_interruptible(&master->master_sem))
+        return -EINTR;
+
+    if (!(request.slave = ec_master_find_slave(master, 0, slave_position))) {
+        up(&master->master_sem);
+        ec_soe_request_clear(&request.req);
+        EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position);
+        return -EINVAL;
+    }
+
+    // schedule request.
+    list_add_tail(&request.list, &request.slave->soe_requests);
+
+    up(&master->master_sem);
+
+    EC_SLAVE_DBG(request.slave, 1, "Scheduled SoE read request.\n");
+
+    // wait for processing through FSM
+    if (wait_event_interruptible(request.slave->soe_queue,
+                request.req.state != EC_INT_REQUEST_QUEUED)) {
+        // interrupted by signal
+        down(&master->master_sem);
+        if (request.req.state == EC_INT_REQUEST_QUEUED) {
+            list_del(&request.list);
+            up(&master->master_sem);
+            ec_soe_request_clear(&request.req);
+            return -EINTR;
+        }
+        // request already processing: interrupt not possible.
+        up(&master->master_sem);
+    }
+
+    // wait until master FSM has finished processing
+    wait_event(request.slave->soe_queue,
+            request.req.state != EC_INT_REQUEST_BUSY);
+
+    if (error_code) {
+        *error_code = request.req.error_code;
+    }
+
+    EC_SLAVE_DBG(request.slave, 1, "Read %zd bytes via SoE.\n",
+            request.req.data_size);
+
+    if (request.req.state != EC_INT_REQUEST_SUCCESS) {
+        if (result_size) {
+            *result_size = 0;
+        }
+        ec_soe_request_clear(&request.req);
+        return -EIO;
+    } else {
+        if (request.req.data_size > target_size) {
+            EC_MASTER_ERR(master, "Buffer too small.\n");
+            ec_soe_request_clear(&request.req);
+            return -EOVERFLOW;
+        }
+        if (result_size) {
+            *result_size = request.req.data_size;
+        }
+        memcpy(target, request.req.data, request.req.data_size);
+        return 0;
+    }
+}
+
+/*****************************************************************************/
+
 /** \cond */
 
 EXPORT_SYMBOL(ecrt_master_create_domain);
@@ -2362,6 +2506,8 @@ EXPORT_SYMBOL(ecrt_master_sync_reference_clock);
 EXPORT_SYMBOL(ecrt_master_sync_slave_clocks);
 EXPORT_SYMBOL(ecrt_master_sync_monitor_queue);
 EXPORT_SYMBOL(ecrt_master_sync_monitor_process);
+EXPORT_SYMBOL(ecrt_master_write_idn);
+EXPORT_SYMBOL(ecrt_master_read_idn);
 
 /** \endcond */
 

From 5531b692ee352fbe74bd63a75d136a6c9d4c8360 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Tue, 6 Jul 2010 17:40:20 +0200
Subject: [PATCH 19/34] Output correct errno when transferring SDOs via
 command-line interface.

---
 TODO                 |  1 -
 master/cdev.c        | 14 +++++++--
 master/fsm_coe.c     | 69 ++++++++++++++++++++++++++++++++++++++++++--
 master/sdo_request.c |  3 ++
 master/sdo_request.h |  1 +
 5 files changed, 82 insertions(+), 6 deletions(-)

diff --git a/TODO b/TODO
index 5a035353..8d63e8a6 100644
--- a/TODO
+++ b/TODO
@@ -43,7 +43,6 @@ Version 1.5.0:
 * Fix casting away constness during expected WC calculation.
 * Read AL status code on spontaneous state change.
 * Fix clearing domains etc. when not activated and releasing.
-* Output -EPROTO if mailbox protocol is not supported.
 * Only output watchdog config if not default.
 * Do not output 'SDO does not exist' when querying data type.
 * Always write down PDO assignment/mapping in doubt? If not, add an interface
diff --git a/master/cdev.c b/master/cdev.c
index 0692d6f3..1ae289cf 100644
--- a/master/cdev.c
+++ b/master/cdev.c
@@ -881,7 +881,11 @@ int ec_cdev_ioctl_slave_sdo_upload(
 
     if (request.req.state != EC_INT_REQUEST_SUCCESS) {
         data.data_size = 0;
-        retval = -EIO;
+        if (request.req.errno) {
+            retval = -request.req.errno;
+        } else {
+            retval = -EIO;
+        }
     } else {
         if (request.req.data_size > data.target_size) {
             EC_MASTER_ERR(master, "Buffer too small.\n");
@@ -986,7 +990,13 @@ int ec_cdev_ioctl_slave_sdo_download(
 
     data.abort_code = request.req.abort_code;
 
-    retval = request.req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
+    if (request.req.state == EC_INT_REQUEST_SUCCESS) {
+        retval = 0;
+    } else if (request.req.errno) {
+        retval = -request.req.errno;
+    } else {
+        retval = -EIO;
+    }
 
     if (__copy_to_user((void __user *) arg, &data, sizeof(data))) {
         retval = -EFAULT;
diff --git a/master/fsm_coe.c b/master/fsm_coe.c
index d1dc41e9..c0cff97d 100644
--- a/master/fsm_coe.c
+++ b/master/fsm_coe.c
@@ -1103,6 +1103,7 @@ void ec_fsm_coe_down_start(
 
     if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
         EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");
+        request->errno = EPROTONOSUPPORT;
         fsm->state = ec_fsm_coe_error;
         return;
     }
@@ -1110,6 +1111,7 @@ void ec_fsm_coe_down_start(
     if (slave->configured_rx_mailbox_size < 
             EC_MBOX_HEADER_SIZE + EC_COE_DOWN_REQ_HEADER_SIZE) {
         EC_SLAVE_ERR(slave, "Mailbox too small!\n");
+        request->errno = EOVERFLOW;
         fsm->state = ec_fsm_coe_error;
         return;
     }
@@ -1118,6 +1120,7 @@ void ec_fsm_coe_down_start(
         data = ec_slave_mbox_prepare_send(slave, datagram, 0x03,
                 EC_COE_DOWN_REQ_HEADER_SIZE);
         if (IS_ERR(data)) {
+            request->errno = PTR_ERR(data);
             fsm->state = ec_fsm_coe_error;
             return;
         }
@@ -1159,6 +1162,7 @@ void ec_fsm_coe_down_start(
         data = ec_slave_mbox_prepare_send(slave, datagram, 0x03,
                 data_size);
         if (IS_ERR(data)) {
+            request->errno = PTR_ERR(data);
             fsm->state = ec_fsm_coe_error;
             return;
         }
@@ -1212,6 +1216,7 @@ void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */)
         return; // FIXME: check for response first?
 
     if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE download"
                 " request datagram: ");
@@ -1233,6 +1238,7 @@ void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */)
                 return;
             }
         }
+        fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE download request"
                 " for SDO 0x%04x:%x failed with timeout after %lu ms: ",
@@ -1268,6 +1274,7 @@ void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */)
         return;
 
     if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check"
                 " datagram: ");
@@ -1276,6 +1283,7 @@ void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */)
     }
 
     if (datagram->working_counter != 1) {
+        fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
                 " datagram failed: ");
@@ -1287,6 +1295,7 @@ void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */)
         unsigned long diff_ms =
             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
         if (diff_ms >= fsm->request->response_timeout) {
+            fsm->request->errno = EIO;
             fsm->state = ec_fsm_coe_error;
             EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
                     " for SDO 0x%04x:%x download response.\n", diff_ms,
@@ -1343,6 +1352,7 @@ void ec_fsm_coe_down_prepare_segment_request(
     data = ec_slave_mbox_prepare_send(slave, datagram, 0x03,
             data_size);
     if (IS_ERR(data)) {
+        request->errno = PTR_ERR(data);
         fsm->state = ec_fsm_coe_error;
         return;
     }
@@ -1389,6 +1399,7 @@ void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */)
         return; // FIXME: request again?
 
     if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE download"
                 " response datagram: ");
@@ -1397,6 +1408,7 @@ void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */)
     }
 
     if (datagram->working_counter != 1) {
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE download response failed: ");
         ec_datagram_print_wc_error(datagram);
@@ -1405,11 +1417,13 @@ void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */)
 
     data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
     if (IS_ERR(data)) {
+        request->errno = PTR_ERR(data);
         fsm->state = ec_fsm_coe_error;
         return;
     }
 
     if (mbox_prot != 0x03) { // CoE
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
                 mbox_prot);
@@ -1430,6 +1444,7 @@ void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */)
     }
 
     if (rec_size < 6) {
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Received data are too small (%zu bytes):\n",
                 rec_size);
@@ -1440,6 +1455,7 @@ void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */)
     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
         char subidxstr[10];
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         if (request->complete_access) {
             subidxstr[0] = 0x00;
@@ -1497,6 +1513,7 @@ void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)
         return;
 
     if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
         ec_datagram_print_state(datagram);
@@ -1504,6 +1521,7 @@ void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)
     }
 
     if (datagram->working_counter != 1) {
+        fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE mailbox segment check"
                 " datagram failed: ");
@@ -1515,6 +1533,7 @@ void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)
         unsigned long diff_ms =
             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
         if (diff_ms >= fsm->request->response_timeout) {
+            fsm->request->errno = EIO;
             fsm->state = ec_fsm_coe_error;
             EC_SLAVE_ERR(slave, "Timeout while waiting for SDO download"
                     " segment response.\n");
@@ -1553,6 +1572,7 @@ void ec_fsm_coe_down_seg_response(
         return; // FIXME: request again?
 
     if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE download response"
                 " datagram: ");
@@ -1561,6 +1581,7 @@ void ec_fsm_coe_down_seg_response(
     }
 
     if (datagram->working_counter != 1) {
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE download response failed: ");
         ec_datagram_print_wc_error(datagram);
@@ -1569,11 +1590,13 @@ void ec_fsm_coe_down_seg_response(
 
     data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
     if (IS_ERR(data)) {
+        request->errno = PTR_ERR(data);
         fsm->state = ec_fsm_coe_error;
         return;
     }
 
     if (mbox_prot != 0x03) { // CoE
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
                 mbox_prot);
@@ -1594,6 +1617,7 @@ void ec_fsm_coe_down_seg_response(
     }
 
     if (rec_size < 6) {
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Received data are too small (%zu bytes):\n",
                 rec_size);
@@ -1604,6 +1628,7 @@ void ec_fsm_coe_down_seg_response(
     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
         char subidxstr[10];
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         if (request->complete_access) {
             subidxstr[0] = 0x00;
@@ -1640,6 +1665,7 @@ void ec_fsm_coe_down_seg_response(
         EC_SLAVE_ERR(slave, "Invalid toggle received during"
                 " segmented download:\n");
         ec_print_data(data, rec_size);
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         return;
     }
@@ -1671,12 +1697,14 @@ void ec_fsm_coe_up_start(ec_fsm_coe_t *fsm /**< finite state machine */)
 
     if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
         EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");
+        request->errno = EPROTONOSUPPORT;
         fsm->state = ec_fsm_coe_error;
         return;
     }
 
     data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10);
     if (IS_ERR(data)) {
+        request->errno = PTR_ERR(data);
         fsm->state = ec_fsm_coe_error;
         return;
     }
@@ -1714,6 +1742,7 @@ void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */)
         return; // FIXME: check for response first?
 
     if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE upload request: ");
         ec_datagram_print_state(datagram);
@@ -1734,6 +1763,7 @@ void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */)
                 return;
             }
         }
+        fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE upload request for"
                 " SDO 0x%04x:%x failed with timeout after %lu ms: ",
@@ -1771,6 +1801,7 @@ void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */)
         return;
 
     if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
         ec_datagram_print_state(datagram);
@@ -1778,6 +1809,7 @@ void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */)
     }
 
     if (datagram->working_counter != 1) {
+        fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
                 " datagram failed: ");
@@ -1789,6 +1821,7 @@ void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */)
         unsigned long diff_ms =
             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
         if (diff_ms >= fsm->request->response_timeout) {
+            fsm->request->errno = EIO;
             fsm->state = ec_fsm_coe_error;
             EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
                     " SDO 0x%04x:%x upload response.\n", diff_ms,
@@ -1818,6 +1851,7 @@ void ec_fsm_coe_up_prepare_segment_request(
     uint8_t *data =
         ec_slave_mbox_prepare_send(fsm->slave, fsm->datagram, 0x03, 10);
     if (IS_ERR(data)) {
+        fsm->request->errno = PTR_ERR(data);
         fsm->state = ec_fsm_coe_error;
         return;
     }
@@ -1850,11 +1884,13 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
     size_t rec_size, data_size;
     ec_sdo_request_t *request = fsm->request;
     unsigned int expedited, size_specified;
+    int ret;
 
     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
         return; // FIXME: request again?
 
     if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE upload response"
                 " datagram: ");
@@ -1863,6 +1899,7 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
     }
 
     if (datagram->working_counter != 1) {
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE upload response failed: ");
         ec_datagram_print_wc_error(datagram);
@@ -1871,6 +1908,7 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
 
     data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
     if (IS_ERR(data)) {
+        request->errno = PTR_ERR(data);
         fsm->state = ec_fsm_coe_error;
         return;
     }
@@ -1881,6 +1919,7 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
     }
 
     if (mbox_prot != 0x03) { // CoE
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_WARN(slave, "Received mailbox protocol 0x%02X"
                 " as response.\n", mbox_prot);
@@ -1896,6 +1935,7 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
     }
 
     if (rec_size < 6) {
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Received currupted SDO upload response"
                 " (%zu bytes)!\n", rec_size);
@@ -1913,6 +1953,7 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
         } else {
             EC_SLAVE_ERR(slave, "No abort message.\n");
         }
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         return;
     }
@@ -1923,6 +1964,7 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
                 " uploading SDO 0x%04X:%02X.\n",
                 request->index, request->subindex);
         ec_print_data(data, rec_size);
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         return;
     }
@@ -1955,6 +1997,7 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
         }
 
         if (rec_size < 6 + fsm->complete_size) {
+            request->errno = EIO;
             fsm->state = ec_fsm_coe_error;
             EC_SLAVE_ERR(slave, "Received corrupted SDO expedited upload"
                     " response (only %zu bytes)!\n", rec_size);
@@ -1962,12 +2005,15 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
             return;
         }
 
-        if (ec_sdo_request_copy_data(request, data + 6, fsm->complete_size)) {
+        ret = ec_sdo_request_copy_data(request, data + 6, fsm->complete_size);
+        if (ret) {
+            request->errno = -ret;
             fsm->state = ec_fsm_coe_error;
             return;
         }
     } else { // normal
         if (rec_size < 10) {
+            request->errno = EIO;
             fsm->state = ec_fsm_coe_error;
             EC_SLAVE_ERR(slave, "Received currupted SDO normal upload"
                     " response (only %zu bytes)!\n", rec_size);
@@ -1979,18 +2025,23 @@ void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)
         fsm->complete_size = EC_READ_U32(data + 6);
 
         if (!fsm->complete_size) {
+            request->errno = EIO;
             fsm->state = ec_fsm_coe_error;
             EC_SLAVE_ERR(slave, "No complete size supplied!\n");
             ec_print_data(data, rec_size);
             return;
         }
 
-        if (ec_sdo_request_alloc(request, fsm->complete_size)) {
+        ret = ec_sdo_request_alloc(request, fsm->complete_size);
+        if (ret) {
+            request->errno = -ret;
             fsm->state = ec_fsm_coe_error;
             return;
         }
 
-        if (ec_sdo_request_copy_data(request, data + 10, data_size)) {
+        ret = ec_sdo_request_copy_data(request, data + 10, data_size);
+        if (ret) {
+            request->errno = -ret;
             fsm->state = ec_fsm_coe_error;
             return;
         }
@@ -2031,6 +2082,7 @@ void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */)
         return; // FIXME: check for response first?
 
     if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment"
                 " request datagram: ");
@@ -2039,6 +2091,7 @@ void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */)
     }
 
     if (datagram->working_counter != 1) {
+        fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE upload segment"
                 " request failed: ");
@@ -2068,6 +2121,7 @@ void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)
         return;
 
     if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check"
                 " datagram: ");
@@ -2076,6 +2130,7 @@ void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)
     }
 
     if (datagram->working_counter != 1) {
+        fsm->request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE mailbox check datagram"
                 " failed: ");
@@ -2087,6 +2142,7 @@ void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)
         unsigned long diff_ms =
             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
         if (diff_ms >= fsm->request->response_timeout) {
+            fsm->request->errno = EIO;
             fsm->state = ec_fsm_coe_error;
             EC_SLAVE_ERR(slave, "Timeout while waiting for SDO upload"
                     " segment response.\n");
@@ -2125,6 +2181,7 @@ void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */)
         return; // FIXME: request again?
 
     if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment"
                 " response datagram: ");
@@ -2133,6 +2190,7 @@ void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */)
     }
 
     if (datagram->working_counter != 1) {
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         EC_SLAVE_ERR(slave, "Reception of CoE upload segment"
                 " response failed: ");
@@ -2142,6 +2200,7 @@ void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */)
 
     data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
     if (IS_ERR(data)) {
+        request->errno = PTR_ERR(data);
         fsm->state = ec_fsm_coe_error;
         return;
     }
@@ -2154,6 +2213,7 @@ void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */)
     if (mbox_prot != 0x03) { // CoE
         EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
                 mbox_prot);
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         return;
     }
@@ -2170,6 +2230,7 @@ void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */)
         EC_SLAVE_ERR(slave, "Received currupted SDO upload"
                 " segment response!\n");
         ec_print_data(data, rec_size);
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         return;
     }
@@ -2180,6 +2241,7 @@ void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */)
                request->index, request->subindex);
         request->abort_code = EC_READ_U32(data + 6);
         ec_canopen_abort_msg(slave, request->abort_code);
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         return;
     }
@@ -2208,6 +2270,7 @@ void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */)
         EC_SLAVE_ERR(slave, "SDO upload 0x%04X:%02X failed: Fragment"
                 " exceeding complete size!\n",
                 request->index, request->subindex);
+        request->errno = EOVERFLOW;
         fsm->state = ec_fsm_coe_error;
         return;
     }
diff --git a/master/sdo_request.c b/master/sdo_request.c
index 1700bf97..ae981945 100644
--- a/master/sdo_request.c
+++ b/master/sdo_request.c
@@ -64,6 +64,7 @@ void ec_sdo_request_init(
     req->issue_timeout = 0; // no timeout
     req->response_timeout = EC_SDO_REQUEST_RESPONSE_TIMEOUT;
     req->state = EC_INT_REQUEST_INIT;
+    req->errno = 0;
     req->abort_code = 0x00000000;
 }
 
@@ -226,6 +227,7 @@ void ecrt_sdo_request_read(ec_sdo_request_t *req)
 {
     req->dir = EC_DIR_INPUT;
     req->state = EC_INT_REQUEST_QUEUED;
+    req->errno = 0;
     req->abort_code = 0x00000000;
     req->jiffies_start = jiffies;
 }
@@ -236,6 +238,7 @@ void ecrt_sdo_request_write(ec_sdo_request_t *req)
 {
     req->dir = EC_DIR_OUTPUT;
     req->state = EC_INT_REQUEST_QUEUED;
+    req->errno = 0;
     req->abort_code = 0x00000000;
     req->jiffies_start = jiffies;
 }
diff --git a/master/sdo_request.h b/master/sdo_request.h
index 81975e51..3da4ef23 100644
--- a/master/sdo_request.h
+++ b/master/sdo_request.h
@@ -64,6 +64,7 @@ struct ec_sdo_request {
     unsigned long jiffies_start; /**< Jiffies, when the request was issued. */
     unsigned long jiffies_sent; /**< Jiffies, when the upload/download
                                      request was sent. */
+    int errno; /**< Error number. */
     uint32_t abort_code; /**< SDO request abort code. Zero on success. */
 };
 

From 2266cdc4aa3a55c662e8be2560a1527e3499d13c Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Tue, 6 Jul 2010 20:23:08 +0200
Subject: [PATCH 20/34] Implemented delimiter for cstruct command.

---
 TODO                    | 2 --
 tool/CommandCStruct.cpp | 7 ++++---
 2 files changed, 4 insertions(+), 5 deletions(-)

diff --git a/TODO b/TODO
index 8d63e8a6..8850be8a 100644
--- a/TODO
+++ b/TODO
@@ -31,8 +31,6 @@ Version 1.5.0:
     - Implement --output-file argument in foe_read.
     - Fix arguments of reg_read.
     - Number layout for reg_read.
-    - Implement identifier parameter for cstruct command.
-    - Implement sync delimiter for cstruct command.
     - Implement indent in 'ethercat ma'
     - Implement 0xXXXX:YY format for specifying SDOs.
     - Implement reading from stream for soe_write.
diff --git a/tool/CommandCStruct.cpp b/tool/CommandCStruct.cpp
index 74ed2e3c..bde1a765 100644
--- a/tool/CommandCStruct.cpp
+++ b/tool/CommandCStruct.cpp
@@ -117,7 +117,7 @@ void CommandCStruct::generateSlaveCStruct(
     for (i = 0; i < slave.sync_count; i++) {
         m.getSync(&sync, slave.position, i);
 
-        syncs << "   {" << dec << sync.sync_index
+        syncs << "    {" << dec << sync.sync_index
             << ", " << (EC_READ_BIT(&sync.control_register, 2) ?
                     "EC_DIR_OUTPUT" : "EC_DIR_INPUT")
             << ", " << dec << (unsigned int) sync.pdo_count
@@ -136,7 +136,7 @@ void CommandCStruct::generateSlaveCStruct(
         for (j = 0; j < sync.pdo_count; j++) {
             m.getPdo(&pdo, slave.position, i, j);
 
-            pdos << "   {0x" << hex << setfill('0')
+            pdos << "    {0x" << hex << setfill('0')
                 << setw(4) << pdo.index
                 << ", " << dec << (unsigned int) pdo.entry_count
                 << ", ";
@@ -155,7 +155,7 @@ void CommandCStruct::generateSlaveCStruct(
             for (k = 0; k < pdo.entry_count; k++) {
                 m.getPdoEntry(&entry, slave.position, i, j, k);
 
-                entries << "   {0x" << hex << setfill('0')
+                entries << "    {0x" << hex << setfill('0')
                     << setw(4) << entry.index
                     << ", 0x" << setw(2) << (unsigned int) entry.subindex
                     << ", " << dec << (unsigned int) entry.bit_length
@@ -201,6 +201,7 @@ void CommandCStruct::generateSlaveCStruct(
 
     cout << "ec_sync_info_t " << id.str() << "syncs[] = {" << endl
         << syncs.str()
+        << "    {0xff}" << endl
         << "};" << endl
         << endl;
 }

From e1478429911c631f520b0d16dcbba1457a0e6ebd Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Tue, 6 Jul 2010 18:30:35 +0200
Subject: [PATCH 21/34] TODO for 1.5.

---
 TODO | 46 +++++++++++++++++++++-------------------------
 1 file changed, 21 insertions(+), 25 deletions(-)

diff --git a/TODO b/TODO
index 8850be8a..b9a1ea62 100644
--- a/TODO
+++ b/TODO
@@ -16,38 +16,27 @@ Version 1.5.0:
 * Finish library implementation.
   - Remove stdio uses?
 * Rescan command.
-* Change SDO index at runtime for SDO request.
-* Output skipped datagrams again.
-* Output warning when send_ext() is called in illegal context.
-* Implement CompleteAccess for SDO uploads.
-* Check for Enable SDO Complete Access flag.
-* Remove allow_scanning flag.
 * ethercat tool:
     - Output error after usage.
-    - Data type abbreviations.
     - Implement ranges for slaves and domains.
-    - Add -x switch for hex display.
-    - Implement CompleteAccess
-    - Implement --output-file argument in foe_read.
-    - Fix arguments of reg_read.
-    - Number layout for reg_read.
-    - Implement indent in 'ethercat ma'
-    - Implement 0xXXXX:YY format for specifying SDOs.
-    - Implement reading from stream for soe_write.
-    - Fix AL state display if error bit set.
-* recompile tool/CommandVersion.cpp if revision changes.
-* Log SoE IDNs with real name ([SP]-x-yyyy).
-* Output SoE IDN configurations in 'ethercat config'.
 * Fix casting away constness during expected WC calculation.
-* Read AL status code on spontaneous state change.
-* Fix clearing domains etc. when not activated and releasing.
-* Only output watchdog config if not default.
-* Do not output 'SDO does not exist' when querying data type.
-* Always write down PDO assignment/mapping in doubt? If not, add an interface
-  for enforcing this.
 
 Future issues:
 
+* Remove allow_scanning flag.
+* Output skipped datagrams again.
+* Check for Enable SDO Complete Access flag.
+* Do not output 'SDO does not exist' when querying data type.
+* Always write down PDO assignment/mapping in doubt? If not, add an interface
+  for enforcing this.
+* Read AL status code on spontaneous state change.
+* recompile tool/CommandVersion.cpp if revision changes.
+* Log SoE IDNs with real name ([SP]-x-yyyy).
+* Output SoE IDN configurations in 'ethercat config'.
+* Only output watchdog config if not default.
+* Implement CompleteAccess for SDO uploads.
+* Output warning when send_ext() is called in illegal context.
+* Change SDO index at runtime for SDO request.
 * Implement ecrt_slave_config_request_state().
 * Remove default buffer size in SDO upload.
 * Override sync manager size?
@@ -80,6 +69,13 @@ Future issues:
     - Add a -n (numeric) switch.
 	- Check for unwanted options.
     - Fix number of digits in negative integer hex output.
+    - Data type abbreviations.
+    - Add -x switch for hex display.
+    - Implement CompleteAccess
+    - Implement --output-file argument in foe_read.
+    - Implement indent in 'ethercat ma'
+    - Implement 0xXXXX:YY format for specifying SDOs.
+    - Implement reading from stream for soe_write.
 * Simplify master fsm by introducing a common request state to handle external
   requests (replace write_sii, sdo_request, etc).
 * Write PDO mapping/assignment by default?

From 0e0033f57251230705cf25bf154fa6f6e717f69f Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Tue, 13 Jul 2010 13:52:59 +0200
Subject: [PATCH 22/34] Improved SII debugging.

---
 master/fsm_sii.c        | 3 ++-
 master/fsm_slave_scan.c | 6 ++++--
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/master/fsm_sii.c b/master/fsm_sii.c
index 7843a245..f1f9d923 100644
--- a/master/fsm_sii.c
+++ b/master/fsm_sii.c
@@ -277,7 +277,8 @@ void ec_fsm_sii_state_read_fetch(
 #endif
 
     if (EC_READ_U8(datagram->data + 1) & 0x20) {
-        EC_SLAVE_ERR(fsm->slave, "SII: Error on last SII command!\n");
+        EC_SLAVE_ERR(fsm->slave, "Error on last command while"
+                " reading from SII word 0x%04x.\n", fsm->word_offset);
         fsm->state = ec_fsm_sii_state_error;
         return;
     }
diff --git a/master/fsm_slave_scan.c b/master/fsm_slave_scan.c
index 460a1336..c5aac8a3 100644
--- a/master/fsm_slave_scan.c
+++ b/master/fsm_slave_scan.c
@@ -498,12 +498,14 @@ void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *fsm /**< slave state
     ec_slave_t *slave = fsm->slave;
     uint16_t cat_type, cat_size;
 
-    if (ec_fsm_sii_exec(&fsm->fsm_sii)) return;
+    if (ec_fsm_sii_exec(&fsm->fsm_sii))
+        return;
 
     if (!ec_fsm_sii_success(&fsm->fsm_sii)) {
         fsm->slave->error_flag = 1;
         fsm->state = ec_fsm_slave_scan_state_error;
-        EC_SLAVE_ERR(slave, "Failed to read SII size.\n");
+        EC_SLAVE_ERR(slave, "Failed to determine SII content size:"
+                " Reading word offset 0x%04x failed.\n", fsm->sii_offset);
         return;
     }
 

From 190cdce964a842eb9d5d37cbd0fff87ad64119b4 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Tue, 20 Jul 2010 10:52:31 +0200
Subject: [PATCH 23/34] Added drive_no parameter to SoE layer.

---
 include/ecrt.h        |  3 +++
 lib/master.c          |  7 +++++--
 lib/slave_config.c    |  5 +++--
 master/cdev.c         |  7 ++++---
 master/fsm_soe.c      | 12 +++++++-----
 master/ioctl.h        |  5 ++++-
 master/master.c       | 17 +++++++++++++++--
 master/slave_config.c | 17 ++++++++++++-----
 master/soe_request.c  | 15 +++++++++++++++
 master/soe_request.h  |  2 ++
 10 files changed, 70 insertions(+), 20 deletions(-)

diff --git a/include/ecrt.h b/include/ecrt.h
index fa1b3498..d3b82731 100644
--- a/include/ecrt.h
+++ b/include/ecrt.h
@@ -687,6 +687,7 @@ int ecrt_master_sdo_upload(
 int ecrt_master_write_idn(
         ec_master_t *master, /**< EtherCAT master. */
         uint16_t slave_position, /**< Slave position. */
+        uint8_t drive_no, /**< Drive number. */
         uint16_t idn, /**< SoE IDN (see ecrt_slave_config_idn()). */
         uint8_t *data, /**< Pointer to data to write. */
         size_t data_size, /**< Size of data to write. */
@@ -705,6 +706,7 @@ int ecrt_master_write_idn(
 int ecrt_master_read_idn(
         ec_master_t *master, /**< EtherCAT master. */
         uint16_t slave_position, /**< Slave position. */
+        uint8_t drive_no, /**< Drive number. */
         uint16_t idn, /**< SoE IDN (see ecrt_slave_config_idn()). */
         uint8_t *target, /**< Pointer to memory where the read data can be
                            stored. */
@@ -1212,6 +1214,7 @@ void ecrt_slave_config_state(
  */
 int ecrt_slave_config_idn(
         ec_slave_config_t *sc, /**< Slave configuration. */
+        uint8_t drive_no, /**< Drive number. */
         uint16_t idn, /**< SoE IDN. */
         ec_al_state_t state, /**< AL state in which to write the IDN (PREOP or
                                SAFEOP). */
diff --git a/lib/master.c b/lib/master.c
index 0c9d7dbb..382cd100 100644
--- a/lib/master.c
+++ b/lib/master.c
@@ -305,11 +305,13 @@ int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position,
 /*****************************************************************************/
 
 int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
-        uint16_t idn, uint8_t *data, size_t data_size, uint16_t *error_code)
+        uint8_t drive_no, uint16_t idn, uint8_t *data, size_t data_size,
+        uint16_t *error_code)
 {
     ec_ioctl_slave_soe_write_t io;
 
     io.slave_position = slave_position;
+    io.drive_no = drive_no;
     io.idn = idn;
     io.data_size = data_size;
     io.data = data;
@@ -328,12 +330,13 @@ int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
 /*****************************************************************************/
 
 int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position,
-        uint16_t idn, uint8_t *target, size_t target_size,
+        uint8_t drive_no, uint16_t idn, uint8_t *target, size_t target_size,
         size_t *result_size, uint16_t *error_code)
 {
     ec_ioctl_slave_soe_read_t io;
 
     io.slave_position = slave_position;
+    io.drive_no = drive_no;
     io.idn = idn;
     io.mem_size = target_size;
     io.data = target;
diff --git a/lib/slave_config.c b/lib/slave_config.c
index 49ce635b..97b3526e 100644
--- a/lib/slave_config.c
+++ b/lib/slave_config.c
@@ -472,12 +472,13 @@ void ecrt_slave_config_state(const ec_slave_config_t *sc,
 
 /*****************************************************************************/
 
-int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn,
-        ec_al_state_t al_state, const uint8_t *data, size_t size)
+int ecrt_slave_config_idn(ec_slave_config_t *sc, uint8_t drive_no,
+        uint16_t idn, ec_al_state_t al_state, const uint8_t *data, size_t size)
 {
     ec_ioctl_sc_idn_t io;
 
     io.config_index = sc->index;
+    io.drive_no = drive_no;
     io.idn = idn;
     io.al_state = al_state;
     io.data = data;
diff --git a/master/cdev.c b/master/cdev.c
index 1ae289cf..33c4b572 100644
--- a/master/cdev.c
+++ b/master/cdev.c
@@ -2474,7 +2474,7 @@ int ec_cdev_ioctl_sc_idn(
     up(&master->master_sem); // FIXME
 
     ret = ecrt_slave_config_idn(
-            sc, ioctl.idn, ioctl.al_state, data, ioctl.size);
+            sc, ioctl.drive_no, ioctl.idn, ioctl.al_state, data, ioctl.size);
     kfree(data);
     return ret;
 }
@@ -3344,7 +3344,7 @@ int ec_cdev_ioctl_slave_soe_read(
     }
 
     retval = ecrt_master_read_idn(master, ioctl.slave_position,
-            ioctl.idn, data, ioctl.mem_size, &ioctl.data_size,
+            ioctl.drive_no, ioctl.idn, data, ioctl.mem_size, &ioctl.data_size,
             &ioctl.error_code);
     if (retval) {
         kfree(data);
@@ -3395,7 +3395,8 @@ int ec_cdev_ioctl_slave_soe_write(
     }
 
     retval = ecrt_master_write_idn(master, ioctl.slave_position,
-            ioctl.idn, data, ioctl.data_size, &ioctl.error_code);
+            ioctl.drive_no, ioctl.idn, data, ioctl.data_size,
+            &ioctl.error_code);
     kfree(data);
     if (retval) {
         return retval;
diff --git a/master/fsm_soe.c b/master/fsm_soe.c
index a43ba117..7535c10e 100644
--- a/master/fsm_soe.c
+++ b/master/fsm_soe.c
@@ -201,7 +201,8 @@ void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)
     ec_soe_request_t *request = fsm->request;
     uint8_t *data;
 
-    EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X.\n", request->idn);
+    EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn,
+            request->drive_no);
 
     if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
         EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
@@ -218,7 +219,7 @@ void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)
         return;
     }
 
-    EC_WRITE_U8(data, OPCODE_READ_REQUEST);
+    EC_WRITE_U8(data, OPCODE_READ_REQUEST | (request->drive_no & 0x07) << 5);
     EC_WRITE_U8(data + 1, 1 << 6); // request value
     EC_WRITE_U16(data + 2, request->idn);
 
@@ -492,7 +493,8 @@ void ec_fsm_soe_write_next_fragment(
         return;
     }
 
-    EC_WRITE_U8(data, OPCODE_WRITE_REQUEST | incomplete << 3);
+    EC_WRITE_U8(data, OPCODE_WRITE_REQUEST | incomplete << 3 |
+            (req->drive_no & 0x07) << 5);
     EC_WRITE_U8(data + 1, 1 << 6); // only value included
     EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn);
     memcpy(data + 4, req->data + fsm->offset, fragment_size);
@@ -517,8 +519,8 @@ void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */)
     ec_slave_t *slave = fsm->slave;
     ec_soe_request_t *req = fsm->request;
 
-    EC_SLAVE_DBG(slave, 1, "Writing IDN 0x%04X (%zu byte).\n",
-            req->idn, req->data_size);
+    EC_SLAVE_DBG(slave, 1, "Writing IDN 0x%04X of drive %u (%zu byte).\n",
+            req->idn, req->drive_no, req->data_size);
 
     if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
         EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
diff --git a/master/ioctl.h b/master/ioctl.h
index 2606822a..e9337d5c 100644
--- a/master/ioctl.h
+++ b/master/ioctl.h
@@ -56,7 +56,7 @@
  *
  * Increment this when changing the ioctl interface!
  */
-#define EC_IOCTL_VERSION_MAGIC 6
+#define EC_IOCTL_VERSION_MAGIC 7
 
 // Command-line tool
 #define EC_IOCTL_MODULE                EC_IOR(0x00, ec_ioctl_module_t)
@@ -413,6 +413,7 @@ typedef struct {
 typedef struct {
     // inputs
     uint16_t slave_position;
+    uint8_t drive_no;
     uint16_t idn;
     uint32_t mem_size;
     uint8_t *data;
@@ -427,6 +428,7 @@ typedef struct {
 typedef struct {
     // inputs
     uint16_t slave_position;
+    uint8_t drive_no;
     uint16_t idn;
     uint32_t data_size;
     uint8_t *data;
@@ -582,6 +584,7 @@ typedef struct {
 typedef struct {
     // inputs
     uint32_t config_index;
+    uint8_t drive_no;
     uint16_t idn;
     ec_al_state_t al_state;
     const uint8_t *data;
diff --git a/master/master.c b/master/master.c
index 54e600d7..0bbe5b76 100644
--- a/master/master.c
+++ b/master/master.c
@@ -2345,13 +2345,20 @@ uint32_t ecrt_master_sync_monitor_process(ec_master_t *master)
 /*****************************************************************************/
 
 int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
-        uint16_t idn, uint8_t *data, size_t data_size, uint16_t *error_code)
+        uint8_t drive_no, uint16_t idn, uint8_t *data, size_t data_size,
+        uint16_t *error_code)
 {
     ec_master_soe_request_t request;
     int retval;
 
+    if (drive_no > 7) {
+        EC_MASTER_ERR(master, "Invalid drive number!\n");
+        return -EINVAL;
+    }
+
     INIT_LIST_HEAD(&request.list);
     ec_soe_request_init(&request.req);
+    ec_soe_request_set_drive_no(&request.req, drive_no);
     ec_soe_request_set_idn(&request.req, idn);
 
     if (ec_soe_request_alloc(&request.req, data_size)) {
@@ -2413,13 +2420,19 @@ int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
 /*****************************************************************************/
 
 int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position,
-        uint16_t idn, uint8_t *target, size_t target_size,
+        uint8_t drive_no, uint16_t idn, uint8_t *target, size_t target_size,
         size_t *result_size, uint16_t *error_code)
 {
     ec_master_soe_request_t request;
 
+    if (drive_no > 7) {
+        EC_MASTER_ERR(master, "Invalid drive number!\n");
+        return -EINVAL;
+    }
+
     INIT_LIST_HEAD(&request.list);
     ec_soe_request_init(&request.req);
+    ec_soe_request_set_drive_no(&request.req, drive_no);
     ec_soe_request_set_idn(&request.req, idn);
     ec_soe_request_read(&request.req);
 
diff --git a/master/slave_config.c b/master/slave_config.c
index f99667d8..1ff03122 100644
--- a/master/slave_config.c
+++ b/master/slave_config.c
@@ -938,16 +938,22 @@ void ecrt_slave_config_state(const ec_slave_config_t *sc,
 
 /*****************************************************************************/
 
-int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn,
-        ec_al_state_t state, const uint8_t *data, size_t size)
+int ecrt_slave_config_idn(ec_slave_config_t *sc, uint8_t drive_no, 
+        uint16_t idn, ec_al_state_t state, const uint8_t *data,
+        size_t size)
 {
     ec_slave_t *slave = sc->slave;
     ec_soe_request_t *req;
     int ret;
 
-    EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, idn = 0x%04X, state = %u, "
-            "data = 0x%p, size = %zu)\n",
-            __func__, sc, idn, state, data, size);
+    EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, drive_no = %u, idn = 0x%04X, "
+            "state = %u, data = 0x%p, size = %zu)\n",
+            __func__, sc, drive_no, idn, state, data, size);
+
+    if (drive_no > 7) {
+        EC_CONFIG_ERR(sc, "Invalid drive number!\n");
+        return -EINVAL;
+    }
 
     if (state != EC_AL_STATE_PREOP && state != EC_AL_STATE_SAFEOP) {
         EC_CONFIG_ERR(sc, "AL state for IDN config"
@@ -967,6 +973,7 @@ int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn,
     }
 
     ec_soe_request_init(req);
+    ec_soe_request_set_drive_no(req, drive_no);
     ec_soe_request_set_idn(req, idn);
     req->al_state = state;
 
diff --git a/master/soe_request.c b/master/soe_request.c
index a3a058df..b2fe0716 100644
--- a/master/soe_request.c
+++ b/master/soe_request.c
@@ -56,6 +56,8 @@ void ec_soe_request_init(
         ec_soe_request_t *req /**< SoE request. */
         )
 {
+    req->drive_no = 0x00;
+    req->idn = 0x0000;
     req->al_state = EC_AL_STATE_INIT;
     req->data = NULL;
     req->mem_size = 0;
@@ -86,6 +88,7 @@ int ec_soe_request_copy(
         const ec_soe_request_t *other /**< Other SoE request to copy from. */
         )
 {
+    req->drive_no = other->drive_no;
     req->idn = other->idn;
     req->al_state = other->al_state;
     return ec_soe_request_copy_data(req, other->data, other->data_size);
@@ -93,6 +96,18 @@ int ec_soe_request_copy(
 
 /*****************************************************************************/
 
+/** Set drive number.
+ */
+void ec_soe_request_set_drive_no(
+        ec_soe_request_t *req, /**< SoE request. */
+        uint8_t drive_no /** Drive Number. */
+        )
+{
+    req->drive_no = drive_no;
+}
+
+/*****************************************************************************/
+
 /** Set IDN.
  */
 void ec_soe_request_set_idn(
diff --git a/master/soe_request.h b/master/soe_request.h
index 40d0fe7f..40f61aa6 100644
--- a/master/soe_request.h
+++ b/master/soe_request.h
@@ -47,6 +47,7 @@
  */
 typedef struct {
     struct list_head list; /**< List item. */
+    uint8_t drive_no; /**< Drive number. */
     uint16_t idn; /**< Sercos ID-Number. */
     ec_al_state_t al_state; /**< AL state (only valid for IDN config). */
     uint8_t *data; /**< Pointer to SDO data. */
@@ -66,6 +67,7 @@ void ec_soe_request_init(ec_soe_request_t *);
 void ec_soe_request_clear(ec_soe_request_t *);
 
 int ec_soe_request_copy(ec_soe_request_t *, const ec_soe_request_t *);
+void ec_soe_request_set_drive_no(ec_soe_request_t *, uint8_t);
 void ec_soe_request_set_idn(ec_soe_request_t *, uint16_t);
 int ec_soe_request_alloc(ec_soe_request_t *, size_t);
 int ec_soe_request_copy_data(ec_soe_request_t *, const uint8_t *, size_t);

From 03f5ed2b289f8caa722902ecbd670c8c72a7f3d2 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Fri, 23 Jul 2010 09:30:42 +0200
Subject: [PATCH 24/34] Always set drive_no to zero in ethercat tool (TBC).

---
 TODO                     | 1 +
 tool/CommandSoeRead.cpp  | 2 ++
 tool/CommandSoeWrite.cpp | 2 ++
 3 files changed, 5 insertions(+)

diff --git a/TODO b/TODO
index b9a1ea62..81fc8bdb 100644
--- a/TODO
+++ b/TODO
@@ -20,6 +20,7 @@ Version 1.5.0:
     - Output error after usage.
     - Implement ranges for slaves and domains.
 * Fix casting away constness during expected WC calculation.
+* Include SoE drive_no in ethercat tool.
 
 Future issues:
 
diff --git a/tool/CommandSoeRead.cpp b/tool/CommandSoeRead.cpp
index 74367e79..1653ce64 100644
--- a/tool/CommandSoeRead.cpp
+++ b/tool/CommandSoeRead.cpp
@@ -90,6 +90,8 @@ void CommandSoeRead::execute(const StringVector &args)
         throwInvalidUsageException(err);
     }
 
+    ioctl.drive_no = 0; // FIXME
+
     try {
         ioctl.idn = parseIdn(args[0]);
     } catch (runtime_error &e) {
diff --git a/tool/CommandSoeWrite.cpp b/tool/CommandSoeWrite.cpp
index ee98b11a..4c542426 100644
--- a/tool/CommandSoeWrite.cpp
+++ b/tool/CommandSoeWrite.cpp
@@ -92,6 +92,8 @@ void CommandSoeWrite::execute(const StringVector &args)
         throwInvalidUsageException(err);
     }
 
+    ioctl.drive_no = 0; // FIXME
+
     try {
         ioctl.idn = parseIdn(args[0]);
     } catch (runtime_error &e) {

From 0b0f43ee1092e8a0f6e8bd0b4823bc8ca8843041 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Sat, 31 Jul 2010 16:13:24 +0200
Subject: [PATCH 25/34] TODO.

---
 TODO | 1 +
 1 file changed, 1 insertion(+)

diff --git a/TODO b/TODO
index 81fc8bdb..aa5edefd 100644
--- a/TODO
+++ b/TODO
@@ -21,6 +21,7 @@ Version 1.5.0:
     - Implement ranges for slaves and domains.
 * Fix casting away constness during expected WC calculation.
 * Include SoE drive_no in ethercat tool.
+* Fix frame statistics overflow.
 
 Future issues:
 

From b9c119817d7ca00bc42e5102b170fe4bf76fd753 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Sat, 31 Jul 2010 16:14:49 +0200
Subject: [PATCH 26/34] Dropped '0x' before process data bytes to fit on an 80
 col console.

---
 tool/CommandDomains.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/tool/CommandDomains.cpp b/tool/CommandDomains.cpp
index 738868a1..04a26f2d 100644
--- a/tool/CommandDomains.cpp
+++ b/tool/CommandDomains.cpp
@@ -71,7 +71,7 @@ string CommandDomains::helpString() const
         << endl
         << "  SlaveConfig 1001:0, SM3 ( Input), LogAddr 0x00000006, Size 6"
         << endl
-        << "    0x00 0x00 0x00 0x00 0x00 0x00" << endl
+        << "    00 00 00 00 00 00" << endl
         << endl
         << "The process data are displayed as hexadecimal bytes." << endl
         << endl
@@ -185,7 +185,7 @@ void CommandDomains::showDomain(
         for (j = 0; j < fmmu.data_size; j++) {
             if (j && !(j % BreakAfterBytes))
                 cout << endl << indent << "    ";
-            cout << "0x" << setw(2)
+            cout << setw(2)
                 << (unsigned int) *(processData + dataOffset + j) << " ";
         }
         cout << endl;

From ba64ed28fcd36538ef7b255825ebd0f7a5f53a71 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Sat, 31 Jul 2010 16:45:10 +0200
Subject: [PATCH 27/34] Fixed frame statistics overflow (to be verified).

---
 TODO                   | 1 -
 master/device.c        | 2 +-
 tool/CommandMaster.cpp | 2 +-
 3 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/TODO b/TODO
index aa5edefd..81fc8bdb 100644
--- a/TODO
+++ b/TODO
@@ -21,7 +21,6 @@ Version 1.5.0:
     - Implement ranges for slaves and domains.
 * Fix casting away constness during expected WC calculation.
 * Include SoE drive_no in ethercat tool.
-* Fix frame statistics overflow.
 
 Future issues:
 
diff --git a/master/device.c b/master/device.c
index d0b972e5..5963d34f 100644
--- a/master/device.c
+++ b/master/device.c
@@ -315,7 +315,7 @@ void ec_device_send(
         u32 tx_frame_rate =
             (u32) (device->tx_count - device->last_tx_count) * 1000;
         u32 tx_byte_rate =
-            (device->tx_bytes - device->last_tx_bytes) * 1000;
+            (device->tx_bytes - device->last_tx_bytes);
         u64 loss = device->tx_count - device->rx_count;
         s32 loss_rate = (s32) (loss - device->last_loss) * 1000;
         for (i = 0; i < EC_RATE_COUNT; i++) {
diff --git a/tool/CommandMaster.cpp b/tool/CommandMaster.cpp
index fb545437..390ca992 100644
--- a/tool/CommandMaster.cpp
+++ b/tool/CommandMaster.cpp
@@ -160,7 +160,7 @@ void CommandMaster::execute(const StringVector &args)
                     << setprecision(1) << fixed;
                 for (j = 0; j < EC_RATE_COUNT; j++) {
                     cout << setw(5)
-                        << data.devices[i].tx_byte_rates[j] / 1024000.0;
+                        << data.devices[i].tx_byte_rates[j] / 1024.0;
                     if (j < EC_RATE_COUNT - 1) {
                         cout << " ";
                     }

From a27422eb7398d7a6f58fd9a45c3ae891716cfe9c Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Thu, 5 Aug 2010 09:54:06 +0200
Subject: [PATCH 28/34] Set master stats colum with to 6.

---
 tool/CommandMaster.cpp | 11 ++++++-----
 tool/CommandMaster.h   |  3 +++
 2 files changed, 9 insertions(+), 5 deletions(-)

diff --git a/tool/CommandMaster.cpp b/tool/CommandMaster.cpp
index 390ca992..221cb9bf 100644
--- a/tool/CommandMaster.cpp
+++ b/tool/CommandMaster.cpp
@@ -149,8 +149,8 @@ void CommandMaster::execute(const StringVector &args)
                     << "      Tx frame rate [1/s]: "
                     << setfill(' ') << setprecision(0) << fixed;
                 for (j = 0; j < EC_RATE_COUNT; j++) {
-                    cout <<
-                        setw(5) << data.devices[i].tx_frame_rates[j] / 1000.0;
+                    cout << setw(ColWidth)
+                        << data.devices[i].tx_frame_rates[j] / 1000.0;
                     if (j < EC_RATE_COUNT - 1) {
                         cout << " ";
                     }
@@ -159,7 +159,7 @@ void CommandMaster::execute(const StringVector &args)
                     << "      Tx rate [KByte/s]:   "
                     << setprecision(1) << fixed;
                 for (j = 0; j < EC_RATE_COUNT; j++) {
-                    cout << setw(5)
+                    cout << setw(ColWidth)
                         << data.devices[i].tx_byte_rates[j] / 1024.0;
                     if (j < EC_RATE_COUNT - 1) {
                         cout << " ";
@@ -169,7 +169,8 @@ void CommandMaster::execute(const StringVector &args)
                     << "      Loss rate [1/s]:     "
                     << setprecision(0) << fixed;
                 for (j = 0; j < EC_RATE_COUNT; j++) {
-                    cout << setw(5) << data.devices[i].loss_rates[j] / 1000.0;
+                    cout << setw(ColWidth)
+                        << data.devices[i].loss_rates[j] / 1000.0;
                     if (j < EC_RATE_COUNT - 1) {
                         cout << " ";
                     }
@@ -183,7 +184,7 @@ void CommandMaster::execute(const StringVector &args)
                         perc = 100.0 * data.devices[i].loss_rates[j] /
                             data.devices[i].tx_frame_rates[j];
                     }
-                    cout << setw(5) << perc;
+                    cout << setw(ColWidth) << perc;
                     if (j < EC_RATE_COUNT - 1) {
                         cout << " ";
                     }
diff --git a/tool/CommandMaster.h b/tool/CommandMaster.h
index 45add947..de7976d4 100644
--- a/tool/CommandMaster.h
+++ b/tool/CommandMaster.h
@@ -42,6 +42,9 @@ class CommandMaster:
 
         string helpString() const;
         void execute(const StringVector &);
+
+    private:
+        enum {ColWidth = 6};
 };
 
 /****************************************************************************/

From f1567fbece63b836634cf279ae496111ff7b02ef Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Thu, 16 Sep 2010 08:35:21 +0200
Subject: [PATCH 29/34] Free all userspace library objects on releasing master.

---
 TODO               |  3 +-
 lib/common.c       | 23 +++++++--------
 lib/domain.c       |  7 +++++
 lib/domain.h       |  5 ++++
 lib/master.c       | 70 ++++++++++++++++++++++++++++++++++++++++++++++
 lib/master.h       |  7 +++++
 lib/sdo_request.c  | 11 +++++++-
 lib/sdo_request.h  |  5 ++++
 lib/slave_config.c | 69 ++++++++++++++++++++++++++++++++++++++++++---
 lib/slave_config.h |  7 +++++
 lib/voe_handler.c  |  8 ++++++
 lib/voe_handler.h  |  5 ++++
 12 files changed, 200 insertions(+), 20 deletions(-)

diff --git a/TODO b/TODO
index 5a035353..8f5061df 100644
--- a/TODO
+++ b/TODO
@@ -13,8 +13,6 @@ Version 1.5.0:
 * Ethernet drivers:
     - Fix link detection in generic driver.
     - Add native drivers from 2.6.24 up to 2.6.31.
-* Finish library implementation.
-  - Remove stdio uses?
 * Rescan command.
 * Change SDO index at runtime for SDO request.
 * Output skipped datagrams again.
@@ -86,6 +84,7 @@ Future issues:
 * Simplify master fsm by introducing a common request state to handle external
   requests (replace write_sii, sdo_request, etc).
 * Write PDO mapping/assignment by default?
+* Remove stdio uses in userspace library?
 
 Smaller issues:
 
diff --git a/lib/common.c b/lib/common.c
index f9f6a771..35702376 100644
--- a/lib/common.c
+++ b/lib/common.c
@@ -55,9 +55,9 @@ ec_master_t *ecrt_request_master(unsigned int master_index)
     ec_master_t *master = ecrt_open_master(master_index);
     if (master) {
         if (ecrt_master_reserve(master) < 0) {
-            close(master->fd);
+            ec_master_clear(master);
             free(master);
-            master = 0;
+            master = NULL;
         }
     }
 
@@ -82,19 +82,21 @@ ec_master_t *ecrt_open_master(unsigned int master_index)
 
     master->process_data = NULL;
     master->process_data_size = 0;
+    master->first_domain = NULL;
+    master->first_config = NULL;
 
     snprintf(path, MAX_PATH_LEN - 1, "/dev/EtherCAT%u", master_index);
 
     master->fd = open(path, O_RDWR);
     if (master->fd == -1) {
         fprintf(stderr, "Failed to open %s: %s\n", path, strerror(errno));
-        goto out_free;
+        goto out_clear;
     }
 
     if (ioctl(master->fd, EC_IOCTL_MODULE, &module_data) < 0) {
         fprintf(stderr, "Failed to get module information from %s: %s\n",
                 path, strerror(errno));
-        goto out_close;
+        goto out_clear;
     }
 
     if (module_data.ioctl_version_magic != EC_IOCTL_VERSION_MAGIC) {
@@ -102,14 +104,13 @@ ec_master_t *ecrt_open_master(unsigned int master_index)
                 " %s: %u, libethercat: %u.\n",
                 path, module_data.ioctl_version_magic,
                 EC_IOCTL_VERSION_MAGIC);
-        goto out_close;
+        goto out_clear;
     }
 
     return master;
 
-out_close:
-    close(master->fd);
-out_free:
+out_clear:
+    ec_master_clear(master);
     free(master);
     return 0;
 }
@@ -118,11 +119,7 @@ out_free:
 
 void ecrt_release_master(ec_master_t *master)
 {
-    if (master->process_data)  {
-        munmap(master->process_data, master->process_data_size);
-    }
-
-    close(master->fd);
+    ec_master_clear(master);
     free(master);
 }
 
diff --git a/lib/domain.c b/lib/domain.c
index 8b36cdc3..98ae332c 100644
--- a/lib/domain.c
+++ b/lib/domain.c
@@ -47,6 +47,13 @@
 
 /*****************************************************************************/
 
+void ec_domain_clear(ec_domain_t *domain)
+{
+    // nothing to do
+}
+
+/*****************************************************************************/
+
 int ecrt_domain_reg_pdo_entry_list(ec_domain_t *domain,
         const ec_pdo_entry_reg_t *regs)
 {
diff --git a/lib/domain.h b/lib/domain.h
index 10ca2947..3d9690fa 100644
--- a/lib/domain.h
+++ b/lib/domain.h
@@ -33,9 +33,14 @@
 /*****************************************************************************/
 
 struct ec_domain {
+    ec_domain_t *next;
     unsigned int index;
     ec_master_t *master;
     uint8_t *process_data;
 };
 
 /*****************************************************************************/
+
+void ec_domain_clear(ec_domain_t *);
+
+/*****************************************************************************/
diff --git a/lib/master.c b/lib/master.c
index 5b4f0580..193d7e57 100644
--- a/lib/master.c
+++ b/lib/master.c
@@ -53,6 +53,51 @@ int ecrt_master_reserve(ec_master_t *master)
 
 /*****************************************************************************/
 
+void ec_master_clear(ec_master_t *master)
+{
+    ec_domain_t *d, *next_d;
+    ec_slave_config_t *c, *next_c;
+
+    if (master->process_data)  {
+        munmap(master->process_data, master->process_data_size);
+    }
+
+    d = master->first_domain;
+    while (d) {
+        next_d = d->next;
+        ec_domain_clear(d);
+        d = next_d;
+    }
+
+    c = master->first_config;
+    while (c) {
+        next_c = c->next;
+        ec_slave_config_clear(c);
+        c = next_c;
+    }
+
+    if (master->fd != -1) {
+        close(master->fd);
+    }
+}
+
+/*****************************************************************************/
+
+void ec_master_add_domain(ec_master_t *master, ec_domain_t *domain)
+{
+    if (master->first_domain) {
+        ec_domain_t *d = master->first_domain;
+        while (d->next) {
+            d = d->next;
+        }
+        d->next = domain;
+    } else {
+        master->first_domain = domain;
+    }
+}
+
+/*****************************************************************************/
+
 ec_domain_t *ecrt_master_create_domain(ec_master_t *master)
 {
     ec_domain_t *domain;
@@ -71,14 +116,33 @@ ec_domain_t *ecrt_master_create_domain(ec_master_t *master)
         return 0; 
     }
 
+    domain->next = NULL;
     domain->index = (unsigned int) index;
     domain->master = master;
     domain->process_data = NULL;
+
+    ec_master_add_domain(master, domain);
+
     return domain;
 }
 
 /*****************************************************************************/
 
+void ec_master_add_slave_config(ec_master_t *master, ec_slave_config_t *sc)
+{
+    if (master->first_config) {
+        ec_slave_config_t *c = master->first_config;
+        while (c->next) {
+            c = c->next;
+        }
+        c->next = sc;
+    } else {
+        master->first_config = sc;
+    }
+}
+
+/*****************************************************************************/
+
 ec_slave_config_t *ecrt_master_slave_config(ec_master_t *master,
         uint16_t alias, uint16_t position, uint32_t vendor_id,
         uint32_t product_code)
@@ -105,10 +169,16 @@ ec_slave_config_t *ecrt_master_slave_config(ec_master_t *master,
         return 0; 
     }
 
+    sc->next = NULL;
     sc->master = master;
     sc->index = data.config_index;
     sc->alias = alias;
     sc->position = position;
+    sc->first_sdo_request = NULL;
+    sc->first_voe_handler = NULL;
+
+    ec_master_add_slave_config(master, sc);
+
     return sc;
 }
 
diff --git a/lib/master.h b/lib/master.h
index 46dfbe2d..d092c39d 100644
--- a/lib/master.h
+++ b/lib/master.h
@@ -36,6 +36,13 @@ struct ec_master {
     int fd;
     uint8_t *process_data;
     size_t process_data_size;
+
+    ec_domain_t *first_domain;
+    ec_slave_config_t *first_config;
 };
 
 /*****************************************************************************/
+
+void ec_master_clear(ec_master_t *);
+
+/*****************************************************************************/
diff --git a/lib/sdo_request.c b/lib/sdo_request.c
index 0b53d80d..8999801c 100644
--- a/lib/sdo_request.c
+++ b/lib/sdo_request.c
@@ -43,8 +43,17 @@
 #include "slave_config.h"
 #include "master.h"
 
+/*****************************************************************************/
+
+void ec_sdo_request_clear(ec_sdo_request_t *req)
+{
+    if (req->data) {
+        free(req->data);
+    }
+}
+
 /*****************************************************************************
- * Realtime interface.
+ * Application interface.
  ****************************************************************************/
 
 void ecrt_sdo_request_timeout(ec_sdo_request_t *req, uint32_t timeout)
diff --git a/lib/sdo_request.h b/lib/sdo_request.h
index 3719a44a..1e55fee2 100644
--- a/lib/sdo_request.h
+++ b/lib/sdo_request.h
@@ -33,6 +33,7 @@
 /*****************************************************************************/
 
 struct ec_sdo_request {
+    ec_sdo_request_t *next; /**< List header. */
     ec_slave_config_t *config; /**< Parent slave configuration. */
     unsigned int index; /**< Request index (identifier). */
     uint16_t sdo_index; /**< SDO index. */
@@ -43,3 +44,7 @@ struct ec_sdo_request {
 };
 
 /*****************************************************************************/
+
+void ec_sdo_request_clear(ec_sdo_request_t *);
+
+/*****************************************************************************/
diff --git a/lib/slave_config.c b/lib/slave_config.c
index 49ce635b..46845210 100644
--- a/lib/slave_config.c
+++ b/lib/slave_config.c
@@ -43,6 +43,29 @@
 
 /*****************************************************************************/
 
+void ec_slave_config_clear(ec_slave_config_t *sc)
+{
+    ec_sdo_request_t *r, *next_r;
+    ec_voe_handler_t *v, *next_v;
+
+    r = sc->first_sdo_request;
+    while (r) {
+        next_r = r->next;
+        ec_sdo_request_clear(r);
+        r = next_r;
+    }
+
+
+    v = sc->first_voe_handler;
+    while (v) {
+        next_v = v->next;
+        ec_voe_handler_clear(v);
+        v = next_v;
+    }
+}
+
+/*****************************************************************************/
+
 int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index,
         ec_direction_t dir, ec_watchdog_mode_t watchdog_mode)
 {
@@ -361,6 +384,22 @@ int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index,
 
 /*****************************************************************************/
 
+void ec_slave_config_add_sdo_request(ec_slave_config_t *sc,
+        ec_sdo_request_t *req)
+{
+    if (sc->first_sdo_request) {
+        ec_sdo_request_t *r = sc->first_sdo_request;
+        while (r->next) {
+            r = r->next;
+        }
+        r->next = req;
+    } else {
+        sc->first_sdo_request = req;
+    }
+}
+
+/*****************************************************************************/
+
 ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc,
         uint16_t index, uint8_t subindex, size_t size)
 {
@@ -393,23 +432,42 @@ ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc,
     if (ioctl(sc->master->fd, EC_IOCTL_SC_SDO_REQUEST, &data) == -1) {
         fprintf(stderr, "Failed to create SDO request: %s\n",
                 strerror(errno));
-        if (req->data)
-            free(req->data);
+        ec_sdo_request_clear(req);
         free(req);
         return NULL; 
     }
 
+    req->next = NULL;
     req->config = sc;
     req->index = data.request_index;
     req->sdo_index = data.sdo_index;
     req->sdo_subindex = data.sdo_subindex;
     req->data_size = size;
     req->mem_size = size;
+
+    ec_slave_config_add_sdo_request(sc, req);
+
     return req;
 }
 
 /*****************************************************************************/
 
+void ec_slave_config_add_voe_handler(ec_slave_config_t *sc,
+        ec_voe_handler_t *voe)
+{
+    if (sc->first_voe_handler) {
+        ec_voe_handler_t *v = sc->first_voe_handler;
+        while (v->next) {
+            v = v->next;
+        }
+        v->next = voe;
+    } else {
+        sc->first_voe_handler = voe;
+    }
+}
+
+/*****************************************************************************/
+
 ec_voe_handler_t *ecrt_slave_config_create_voe_handler(ec_slave_config_t *sc,
         size_t size)
 {
@@ -441,16 +499,19 @@ ec_voe_handler_t *ecrt_slave_config_create_voe_handler(ec_slave_config_t *sc,
     if (ioctl(sc->master->fd, EC_IOCTL_SC_VOE, &data) == -1) {
         fprintf(stderr, "Failed to create VoE handler: %s\n",
                 strerror(errno));
-        if (voe->data)
-            free(voe->data);
+        ec_voe_handler_clear(voe);
         free(voe);
         return NULL; 
     }
 
+    voe->next = NULL;
     voe->config = sc;
     voe->index = data.voe_index;
     voe->data_size = size;
     voe->mem_size = size;
+
+    ec_slave_config_add_voe_handler(sc, voe);
+
     return voe;
 }
 
diff --git a/lib/slave_config.h b/lib/slave_config.h
index 53729457..a3e26955 100644
--- a/lib/slave_config.h
+++ b/lib/slave_config.h
@@ -33,10 +33,17 @@
 /*****************************************************************************/
 
 struct ec_slave_config {
+    ec_slave_config_t *next;
     ec_master_t *master;
     unsigned int index;
     uint16_t alias;
     uint16_t position;
+    ec_sdo_request_t *first_sdo_request;
+    ec_voe_handler_t *first_voe_handler;
 };
 
 /*****************************************************************************/
+
+void ec_slave_config_clear(ec_slave_config_t *);
+
+/*****************************************************************************/
diff --git a/lib/voe_handler.c b/lib/voe_handler.c
index e1645a4f..ff87109a 100644
--- a/lib/voe_handler.c
+++ b/lib/voe_handler.c
@@ -47,6 +47,14 @@
 
 /*****************************************************************************/
 
+void ec_voe_handler_clear(ec_voe_handler_t *voe)
+{
+    if (voe->data)
+        free(voe->data);
+}
+
+/*****************************************************************************/
+
 void ecrt_voe_handler_send_header(ec_voe_handler_t *voe, uint32_t vendor_id,
         uint16_t vendor_type)
 {
diff --git a/lib/voe_handler.h b/lib/voe_handler.h
index df9542df..523c1380 100644
--- a/lib/voe_handler.h
+++ b/lib/voe_handler.h
@@ -33,6 +33,7 @@
 /*****************************************************************************/
 
 struct ec_voe_handler {
+    ec_voe_handler_t *next;
     ec_slave_config_t *config;
     unsigned int index;
     size_t data_size;
@@ -41,3 +42,7 @@ struct ec_voe_handler {
 };
 
 /*****************************************************************************/
+
+void ec_voe_handler_clear(ec_voe_handler_t *);
+
+/*****************************************************************************/

From ab10c647644fbfd76a0b1123c89d4eb35ae8be0d Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Thu, 16 Sep 2010 08:41:19 +0200
Subject: [PATCH 30/34] Clear configuration on deactivation; also in userspace.

---
 lib/master.c | 23 ++++++++++++++++-------
 1 file changed, 16 insertions(+), 7 deletions(-)

diff --git a/lib/master.c b/lib/master.c
index 193d7e57..9b208bb9 100644
--- a/lib/master.c
+++ b/lib/master.c
@@ -53,21 +53,18 @@ int ecrt_master_reserve(ec_master_t *master)
 
 /*****************************************************************************/
 
-void ec_master_clear(ec_master_t *master)
+void ec_master_clear_config(ec_master_t *master)
 {
     ec_domain_t *d, *next_d;
     ec_slave_config_t *c, *next_c;
 
-    if (master->process_data)  {
-        munmap(master->process_data, master->process_data_size);
-    }
-
     d = master->first_domain;
     while (d) {
         next_d = d->next;
         ec_domain_clear(d);
         d = next_d;
     }
+    master->first_domain = NULL;
 
     c = master->first_config;
     while (c) {
@@ -75,6 +72,18 @@ void ec_master_clear(ec_master_t *master)
         ec_slave_config_clear(c);
         c = next_c;
     }
+    master->first_config = NULL;
+}
+
+/*****************************************************************************/
+
+void ec_master_clear(ec_master_t *master)
+{
+    if (master->process_data)  {
+        munmap(master->process_data, master->process_data_size);
+    }
+
+    ec_master_clear_config(master);
 
     if (master->fd != -1) {
         close(master->fd);
@@ -456,8 +465,9 @@ void ecrt_master_deactivate(ec_master_t *master)
         fprintf(stderr, "Failed to deactivate master: %s\n", strerror(errno));
         return;
     }
-}
 
+    ec_master_clear_config(master);
+}
 
 /*****************************************************************************/
 
@@ -472,7 +482,6 @@ int ecrt_master_set_send_interval(ec_master_t *master,size_t send_interval_us)
     return 0;
 }
 
-
 /*****************************************************************************/
 
 void ecrt_master_send(ec_master_t *master)

From 6f97d7e27f13a61f287b2fb96b58f6df5ce42d4d Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Thu, 16 Sep 2010 09:08:11 +0200
Subject: [PATCH 31/34] Rescan command.

---
 TODO                   |   1 -
 master/cdev.c          |  17 ++++++
 master/fsm_master.c    |  11 ++--
 master/fsm_master.h    |   2 +-
 master/ioctl.h         | 125 +++++++++++++++++++++--------------------
 tool/CommandRescan.cpp |  85 ++++++++++++++++++++++++++++
 tool/CommandRescan.h   |  49 ++++++++++++++++
 tool/Makefile.am       |   2 +
 tool/MasterDevice.cpp  |  11 ++++
 tool/MasterDevice.h    |   1 +
 tool/main.cpp          |   2 +
 11 files changed, 236 insertions(+), 70 deletions(-)
 create mode 100644 tool/CommandRescan.cpp
 create mode 100644 tool/CommandRescan.h

diff --git a/TODO b/TODO
index 8f5061df..adeea98c 100644
--- a/TODO
+++ b/TODO
@@ -13,7 +13,6 @@ Version 1.5.0:
 * Ethernet drivers:
     - Fix link detection in generic driver.
     - Add native drivers from 2.6.24 up to 2.6.31.
-* Rescan command.
 * Change SDO index at runtime for SDO request.
 * Output skipped datagrams again.
 * Output warning when send_ext() is called in illegal context.
diff --git a/master/cdev.c b/master/cdev.c
index 167a83de..21560d6e 100644
--- a/master/cdev.c
+++ b/master/cdev.c
@@ -659,6 +659,19 @@ int ec_cdev_ioctl_master_debug(
 
 /*****************************************************************************/
 
+/** Issue a bus scan.
+ */
+int ec_cdev_ioctl_master_rescan(
+        ec_master_t *master, /**< EtherCAT master. */
+        unsigned long arg /**< ioctl() argument. */
+        )
+{
+    master->fsm.rescan_required = 1;
+    return 0;
+}
+
+/*****************************************************************************/
+
 /** Set slave state.
  */
 int ec_cdev_ioctl_slave_state(
@@ -3579,6 +3592,10 @@ long eccdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
             if (!(filp->f_mode & FMODE_WRITE))
                 return -EPERM;
             return ec_cdev_ioctl_master_debug(master, arg);
+        case EC_IOCTL_MASTER_RESCAN:
+            if (!(filp->f_mode & FMODE_WRITE))
+                return -EPERM;
+            return ec_cdev_ioctl_master_rescan(master, arg);
         case EC_IOCTL_SLAVE_STATE:
             if (!(filp->f_mode & FMODE_WRITE))
                 return -EPERM;
diff --git a/master/fsm_master.c b/master/fsm_master.c
index cd0f1709..9dd461c3 100644
--- a/master/fsm_master.c
+++ b/master/fsm_master.c
@@ -85,7 +85,7 @@ void ec_fsm_master_init(
     fsm->idle = 0;
     fsm->link_state = 0;
     fsm->slaves_responding = 0;
-    fsm->topology_change_pending = 0;
+    fsm->rescan_required = 0;
     fsm->slave_states = EC_SLAVE_STATE_UNKNOWN;
 
     // init sub-state-machines
@@ -201,7 +201,7 @@ void ec_fsm_master_state_broadcast(
 
     // bus topology change?
     if (datagram->working_counter != fsm->slaves_responding) {
-        fsm->topology_change_pending = 1;
+        fsm->rescan_required = 1;
         fsm->slaves_responding = datagram->working_counter;
         EC_MASTER_INFO(master, "%u slave(s) responding.\n",
                 fsm->slaves_responding);
@@ -237,7 +237,7 @@ void ec_fsm_master_state_broadcast(
         fsm->slave_states = 0x00;
     }
 
-    if (fsm->topology_change_pending) {
+    if (fsm->rescan_required) {
         down(&master->scan_sem);
         if (!master->allow_scan) {
             up(&master->scan_sem);
@@ -245,9 +245,8 @@ void ec_fsm_master_state_broadcast(
             master->scan_busy = 1;
             up(&master->scan_sem);
 
-            // topology change when scan is allowed:
             // clear all slaves and scan the bus
-            fsm->topology_change_pending = 0;
+            fsm->rescan_required = 0;
             fsm->idle = 0;
             fsm->scan_jiffies = jiffies;
 
@@ -630,7 +629,7 @@ void ec_fsm_master_state_read_state(
             slave->error_flag = 1;
             EC_SLAVE_DBG(slave, 1, "Slave did not respond to state query.\n");
         }
-        fsm->topology_change_pending = 1;
+        fsm->rescan_required = 1;
         ec_fsm_master_restart(fsm);
         return;
     }
diff --git a/master/fsm_master.h b/master/fsm_master.h
index 647f779a..b39074a3 100644
--- a/master/fsm_master.h
+++ b/master/fsm_master.h
@@ -119,7 +119,7 @@ struct ec_fsm_master {
     unsigned long scan_jiffies; /**< beginning of slave scanning */
     uint8_t link_state; /**< Last main device link state. */
     unsigned int slaves_responding; /**< number of responding slaves */
-    unsigned int topology_change_pending; /**< bus topology changed */
+    unsigned int rescan_required; /**< A bus rescan is required. */
     ec_slave_state_t slave_states; /**< states of responding slaves */
     ec_slave_t *slave; /**< current slave */
     ec_sii_write_request_t *sii_request; /**< SII write request */
diff --git a/master/ioctl.h b/master/ioctl.h
index 2606822a..4856c0f2 100644
--- a/master/ioctl.h
+++ b/master/ioctl.h
@@ -56,7 +56,7 @@
  *
  * Increment this when changing the ioctl interface!
  */
-#define EC_IOCTL_VERSION_MAGIC 6
+#define EC_IOCTL_VERSION_MAGIC 7
 
 // Command-line tool
 #define EC_IOCTL_MODULE                EC_IOR(0x00, ec_ioctl_module_t)
@@ -69,71 +69,72 @@
 #define EC_IOCTL_DOMAIN_FMMU          EC_IOWR(0x07, ec_ioctl_domain_fmmu_t)
 #define EC_IOCTL_DOMAIN_DATA          EC_IOWR(0x08, ec_ioctl_domain_data_t)
 #define EC_IOCTL_MASTER_DEBUG           EC_IO(0x09)
-#define EC_IOCTL_SLAVE_STATE           EC_IOW(0x0a, ec_ioctl_slave_state_t)
-#define EC_IOCTL_SLAVE_SDO            EC_IOWR(0x0b, ec_ioctl_slave_sdo_t)
-#define EC_IOCTL_SLAVE_SDO_ENTRY      EC_IOWR(0x0c, ec_ioctl_slave_sdo_entry_t)
-#define EC_IOCTL_SLAVE_SDO_UPLOAD     EC_IOWR(0x0d, ec_ioctl_slave_sdo_upload_t)
-#define EC_IOCTL_SLAVE_SDO_DOWNLOAD   EC_IOWR(0x0e, ec_ioctl_slave_sdo_download_t)
-#define EC_IOCTL_SLAVE_SII_READ       EC_IOWR(0x0f, ec_ioctl_slave_sii_t)
-#define EC_IOCTL_SLAVE_SII_WRITE       EC_IOW(0x10, ec_ioctl_slave_sii_t)
-#define EC_IOCTL_SLAVE_REG_READ       EC_IOWR(0x11, ec_ioctl_slave_reg_t)
-#define EC_IOCTL_SLAVE_REG_WRITE       EC_IOW(0x12, ec_ioctl_slave_reg_t)
-#define EC_IOCTL_SLAVE_FOE_READ       EC_IOWR(0x13, ec_ioctl_slave_foe_t)
-#define EC_IOCTL_SLAVE_FOE_WRITE       EC_IOW(0x14, ec_ioctl_slave_foe_t)
-#define EC_IOCTL_SLAVE_SOE_READ       EC_IOWR(0x15, ec_ioctl_slave_soe_read_t)
-#define EC_IOCTL_SLAVE_SOE_WRITE      EC_IOWR(0x16, ec_ioctl_slave_soe_write_t)
-#define EC_IOCTL_CONFIG               EC_IOWR(0x17, ec_ioctl_config_t)
-#define EC_IOCTL_CONFIG_PDO           EC_IOWR(0x18, ec_ioctl_config_pdo_t)
-#define EC_IOCTL_CONFIG_PDO_ENTRY     EC_IOWR(0x19, ec_ioctl_config_pdo_entry_t)
-#define EC_IOCTL_CONFIG_SDO           EC_IOWR(0x1a, ec_ioctl_config_sdo_t)
+#define EC_IOCTL_MASTER_RESCAN          EC_IO(0x0a)
+#define EC_IOCTL_SLAVE_STATE           EC_IOW(0x0b, ec_ioctl_slave_state_t)
+#define EC_IOCTL_SLAVE_SDO            EC_IOWR(0x0c, ec_ioctl_slave_sdo_t)
+#define EC_IOCTL_SLAVE_SDO_ENTRY      EC_IOWR(0x0d, ec_ioctl_slave_sdo_entry_t)
+#define EC_IOCTL_SLAVE_SDO_UPLOAD     EC_IOWR(0x0e, ec_ioctl_slave_sdo_upload_t)
+#define EC_IOCTL_SLAVE_SDO_DOWNLOAD   EC_IOWR(0x0f, ec_ioctl_slave_sdo_download_t)
+#define EC_IOCTL_SLAVE_SII_READ       EC_IOWR(0x10, ec_ioctl_slave_sii_t)
+#define EC_IOCTL_SLAVE_SII_WRITE       EC_IOW(0x11, ec_ioctl_slave_sii_t)
+#define EC_IOCTL_SLAVE_REG_READ       EC_IOWR(0x12, ec_ioctl_slave_reg_t)
+#define EC_IOCTL_SLAVE_REG_WRITE       EC_IOW(0x13, ec_ioctl_slave_reg_t)
+#define EC_IOCTL_SLAVE_FOE_READ       EC_IOWR(0x14, ec_ioctl_slave_foe_t)
+#define EC_IOCTL_SLAVE_FOE_WRITE       EC_IOW(0x15, ec_ioctl_slave_foe_t)
+#define EC_IOCTL_SLAVE_SOE_READ       EC_IOWR(0x16, ec_ioctl_slave_soe_read_t)
+#define EC_IOCTL_SLAVE_SOE_WRITE      EC_IOWR(0x17, ec_ioctl_slave_soe_write_t)
+#define EC_IOCTL_CONFIG               EC_IOWR(0x18, ec_ioctl_config_t)
+#define EC_IOCTL_CONFIG_PDO           EC_IOWR(0x19, ec_ioctl_config_pdo_t)
+#define EC_IOCTL_CONFIG_PDO_ENTRY     EC_IOWR(0x1a, ec_ioctl_config_pdo_entry_t)
+#define EC_IOCTL_CONFIG_SDO           EC_IOWR(0x1b, ec_ioctl_config_sdo_t)
 #ifdef EC_EOE
-#define EC_IOCTL_EOE_HANDLER          EC_IOWR(0x1b, ec_ioctl_eoe_handler_t)
+#define EC_IOCTL_EOE_HANDLER          EC_IOWR(0x1c, ec_ioctl_eoe_handler_t)
 #endif
 
 // Application interface
-#define EC_IOCTL_REQUEST                EC_IO(0x1c)
-#define EC_IOCTL_CREATE_DOMAIN          EC_IO(0x1d)
-#define EC_IOCTL_CREATE_SLAVE_CONFIG  EC_IOWR(0x1e, ec_ioctl_config_t)
-#define EC_IOCTL_ACTIVATE              EC_IOR(0x1f, size_t)
-#define EC_IOCTL_DEACTIVATE             EC_IO(0x20)
-#define EC_IOCTL_SEND                   EC_IO(0x21)
-#define EC_IOCTL_RECEIVE                EC_IO(0x22)
-#define EC_IOCTL_MASTER_STATE          EC_IOR(0x23, ec_master_state_t)
-#define EC_IOCTL_APP_TIME              EC_IOW(0x24, ec_ioctl_app_time_t)
-#define EC_IOCTL_SYNC_REF               EC_IO(0x25)
-#define EC_IOCTL_SYNC_SLAVES            EC_IO(0x26)
-#define EC_IOCTL_SYNC_MON_QUEUE         EC_IO(0x27)
-#define EC_IOCTL_SYNC_MON_PROCESS      EC_IOR(0x28, uint32_t)
-#define EC_IOCTL_SC_SYNC               EC_IOW(0x29, ec_ioctl_config_t)
-#define EC_IOCTL_SC_WATCHDOG           EC_IOW(0x2a, ec_ioctl_config_t)
-#define EC_IOCTL_SC_ADD_PDO            EC_IOW(0x2b, ec_ioctl_config_pdo_t)
-#define EC_IOCTL_SC_CLEAR_PDOS         EC_IOW(0x2c, ec_ioctl_config_pdo_t)
-#define EC_IOCTL_SC_ADD_ENTRY          EC_IOW(0x2d, ec_ioctl_add_pdo_entry_t)
-#define EC_IOCTL_SC_CLEAR_ENTRIES      EC_IOW(0x2e, ec_ioctl_config_pdo_t)
-#define EC_IOCTL_SC_REG_PDO_ENTRY     EC_IOWR(0x2f, ec_ioctl_reg_pdo_entry_t)
-#define EC_IOCTL_SC_DC                 EC_IOW(0x20, ec_ioctl_config_t)
-#define EC_IOCTL_SC_SDO                EC_IOW(0x31, ec_ioctl_sc_sdo_t)
-#define EC_IOCTL_SC_SDO_REQUEST       EC_IOWR(0x32, ec_ioctl_sdo_request_t)
-#define EC_IOCTL_SC_VOE               EC_IOWR(0x33, ec_ioctl_voe_t)
-#define EC_IOCTL_SC_STATE             EC_IOWR(0x34, ec_ioctl_sc_state_t)
-#define EC_IOCTL_SC_IDN                EC_IOW(0x35, ec_ioctl_sc_idn_t)
-#define EC_IOCTL_DOMAIN_OFFSET          EC_IO(0x36)
-#define EC_IOCTL_DOMAIN_PROCESS         EC_IO(0x37)
-#define EC_IOCTL_DOMAIN_QUEUE           EC_IO(0x38)
-#define EC_IOCTL_DOMAIN_STATE         EC_IOWR(0x39, ec_ioctl_domain_state_t)
-#define EC_IOCTL_SDO_REQUEST_TIMEOUT  EC_IOWR(0x3a, ec_ioctl_sdo_request_t)
-#define EC_IOCTL_SDO_REQUEST_STATE    EC_IOWR(0x3b, ec_ioctl_sdo_request_t)
-#define EC_IOCTL_SDO_REQUEST_READ     EC_IOWR(0x3c, ec_ioctl_sdo_request_t)
-#define EC_IOCTL_SDO_REQUEST_WRITE    EC_IOWR(0x3d, ec_ioctl_sdo_request_t)
-#define EC_IOCTL_SDO_REQUEST_DATA     EC_IOWR(0x3e, ec_ioctl_sdo_request_t)
-#define EC_IOCTL_VOE_SEND_HEADER       EC_IOW(0x3f, ec_ioctl_voe_t)
-#define EC_IOCTL_VOE_REC_HEADER       EC_IOWR(0x40, ec_ioctl_voe_t)
-#define EC_IOCTL_VOE_READ              EC_IOW(0x41, ec_ioctl_voe_t)
-#define EC_IOCTL_VOE_READ_NOSYNC       EC_IOW(0x42, ec_ioctl_voe_t)
-#define EC_IOCTL_VOE_WRITE            EC_IOWR(0x43, ec_ioctl_voe_t)
-#define EC_IOCTL_VOE_EXEC             EC_IOWR(0x44, ec_ioctl_voe_t)
-#define EC_IOCTL_VOE_DATA             EC_IOWR(0x45, ec_ioctl_voe_t)
-#define EC_IOCTL_SET_SEND_INTERVAL     EC_IOW(0x46, size_t)
+#define EC_IOCTL_REQUEST                EC_IO(0x1d)
+#define EC_IOCTL_CREATE_DOMAIN          EC_IO(0x1e)
+#define EC_IOCTL_CREATE_SLAVE_CONFIG  EC_IOWR(0x1f, ec_ioctl_config_t)
+#define EC_IOCTL_ACTIVATE              EC_IOR(0x20, size_t)
+#define EC_IOCTL_DEACTIVATE             EC_IO(0x21)
+#define EC_IOCTL_SEND                   EC_IO(0x22)
+#define EC_IOCTL_RECEIVE                EC_IO(0x23)
+#define EC_IOCTL_MASTER_STATE          EC_IOR(0x24, ec_master_state_t)
+#define EC_IOCTL_APP_TIME              EC_IOW(0x25, ec_ioctl_app_time_t)
+#define EC_IOCTL_SYNC_REF               EC_IO(0x26)
+#define EC_IOCTL_SYNC_SLAVES            EC_IO(0x27)
+#define EC_IOCTL_SYNC_MON_QUEUE         EC_IO(0x28)
+#define EC_IOCTL_SYNC_MON_PROCESS      EC_IOR(0x29, uint32_t)
+#define EC_IOCTL_SC_SYNC               EC_IOW(0x2a, ec_ioctl_config_t)
+#define EC_IOCTL_SC_WATCHDOG           EC_IOW(0x2b, ec_ioctl_config_t)
+#define EC_IOCTL_SC_ADD_PDO            EC_IOW(0x2c, ec_ioctl_config_pdo_t)
+#define EC_IOCTL_SC_CLEAR_PDOS         EC_IOW(0x2d, ec_ioctl_config_pdo_t)
+#define EC_IOCTL_SC_ADD_ENTRY          EC_IOW(0x2e, ec_ioctl_add_pdo_entry_t)
+#define EC_IOCTL_SC_CLEAR_ENTRIES      EC_IOW(0x2f, ec_ioctl_config_pdo_t)
+#define EC_IOCTL_SC_REG_PDO_ENTRY     EC_IOWR(0x20, ec_ioctl_reg_pdo_entry_t)
+#define EC_IOCTL_SC_DC                 EC_IOW(0x31, ec_ioctl_config_t)
+#define EC_IOCTL_SC_SDO                EC_IOW(0x32, ec_ioctl_sc_sdo_t)
+#define EC_IOCTL_SC_SDO_REQUEST       EC_IOWR(0x33, ec_ioctl_sdo_request_t)
+#define EC_IOCTL_SC_VOE               EC_IOWR(0x34, ec_ioctl_voe_t)
+#define EC_IOCTL_SC_STATE             EC_IOWR(0x35, ec_ioctl_sc_state_t)
+#define EC_IOCTL_SC_IDN                EC_IOW(0x36, ec_ioctl_sc_idn_t)
+#define EC_IOCTL_DOMAIN_OFFSET          EC_IO(0x37)
+#define EC_IOCTL_DOMAIN_PROCESS         EC_IO(0x38)
+#define EC_IOCTL_DOMAIN_QUEUE           EC_IO(0x39)
+#define EC_IOCTL_DOMAIN_STATE         EC_IOWR(0x3a, ec_ioctl_domain_state_t)
+#define EC_IOCTL_SDO_REQUEST_TIMEOUT  EC_IOWR(0x3b, ec_ioctl_sdo_request_t)
+#define EC_IOCTL_SDO_REQUEST_STATE    EC_IOWR(0x3c, ec_ioctl_sdo_request_t)
+#define EC_IOCTL_SDO_REQUEST_READ     EC_IOWR(0x3d, ec_ioctl_sdo_request_t)
+#define EC_IOCTL_SDO_REQUEST_WRITE    EC_IOWR(0x3e, ec_ioctl_sdo_request_t)
+#define EC_IOCTL_SDO_REQUEST_DATA     EC_IOWR(0x3f, ec_ioctl_sdo_request_t)
+#define EC_IOCTL_VOE_SEND_HEADER       EC_IOW(0x40, ec_ioctl_voe_t)
+#define EC_IOCTL_VOE_REC_HEADER       EC_IOWR(0x41, ec_ioctl_voe_t)
+#define EC_IOCTL_VOE_READ              EC_IOW(0x42, ec_ioctl_voe_t)
+#define EC_IOCTL_VOE_READ_NOSYNC       EC_IOW(0x43, ec_ioctl_voe_t)
+#define EC_IOCTL_VOE_WRITE            EC_IOWR(0x44, ec_ioctl_voe_t)
+#define EC_IOCTL_VOE_EXEC             EC_IOWR(0x45, ec_ioctl_voe_t)
+#define EC_IOCTL_VOE_DATA             EC_IOWR(0x46, ec_ioctl_voe_t)
+#define EC_IOCTL_SET_SEND_INTERVAL     EC_IOW(0x47, size_t)
 
 /*****************************************************************************/
 
diff --git a/tool/CommandRescan.cpp b/tool/CommandRescan.cpp
new file mode 100644
index 00000000..c3d1cd3b
--- /dev/null
+++ b/tool/CommandRescan.cpp
@@ -0,0 +1,85 @@
+/*****************************************************************************
+ *
+ *  $Id$
+ *
+ *  Copyright (C) 2006-2009  Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ *  This file is part of the IgH EtherCAT Master.
+ *
+ *  The IgH EtherCAT Master is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU General Public License version 2, as
+ *  published by the Free Software Foundation.
+ *
+ *  The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
+ *  Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with the IgH EtherCAT Master; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ *  ---
+ *
+ *  The license mentioned above concerns the source code only. Using the
+ *  EtherCAT technology and brand is only permitted in compliance with the
+ *  industrial property and similar rights of Beckhoff Automation GmbH.
+ *
+ *  vim: expandtab
+ *
+ ****************************************************************************/
+
+#include <sstream>
+#include <iomanip>
+using namespace std;
+
+#include "CommandRescan.h"
+#include "MasterDevice.h"
+
+/*****************************************************************************/
+
+CommandRescan::CommandRescan():
+    Command("rescan", "Rescan the bus.")
+{
+}
+
+/*****************************************************************************/
+
+string CommandRescan::helpString() const
+{
+    stringstream str;
+
+    str << getName() << endl
+        << endl
+        << getBriefDescription() << endl
+        << endl
+        << "Command a bus rescan. Gathered slave information will be" << endl
+        << "forgotten and slaves will be read in again." << endl
+        << endl;
+
+    return str.str();
+}
+
+/****************************************************************************/
+
+void CommandRescan::execute(const StringVector &args)
+{
+	MasterIndexList masterIndices;
+    
+    if (args.size() != 0) {
+        stringstream err;
+        err << "'" << getName() << "' takes no arguments!";
+        throwInvalidUsageException(err);
+    }
+
+	masterIndices = getMasterIndices();
+    MasterIndexList::const_iterator mi;
+    for (mi = masterIndices.begin();
+            mi != masterIndices.end(); mi++) {
+        MasterDevice m(*mi);
+        m.open(MasterDevice::ReadWrite);
+        m.rescan();
+    }
+}
+
+/*****************************************************************************/
diff --git a/tool/CommandRescan.h b/tool/CommandRescan.h
new file mode 100644
index 00000000..80c386cf
--- /dev/null
+++ b/tool/CommandRescan.h
@@ -0,0 +1,49 @@
+/*****************************************************************************
+ *
+ *  $Id$
+ *
+ *  Copyright (C) 2006-2009  Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ *  This file is part of the IgH EtherCAT Master.
+ *
+ *  The IgH EtherCAT Master is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU General Public License version 2, as
+ *  published by the Free Software Foundation.
+ *
+ *  The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
+ *  Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with the IgH EtherCAT Master; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ *  ---
+ *
+ *  The license mentioned above concerns the source code only. Using the
+ *  EtherCAT technology and brand is only permitted in compliance with the
+ *  industrial property and similar rights of Beckhoff Automation GmbH.
+ *
+ ****************************************************************************/
+
+#ifndef __COMMANDRESCAN_H__
+#define __COMMANDRESCAN_H__
+
+#include "Command.h"
+
+/****************************************************************************/
+
+class CommandRescan:
+    public Command
+{
+    public:
+        CommandRescan();
+
+        string helpString() const;
+        void execute(const StringVector &);
+};
+
+/****************************************************************************/
+
+#endif
diff --git a/tool/Makefile.am b/tool/Makefile.am
index 1a16df76..cdbce03a 100644
--- a/tool/Makefile.am
+++ b/tool/Makefile.am
@@ -52,6 +52,7 @@ ethercat_SOURCES = \
 	CommandPdos.cpp \
 	CommandRegRead.cpp \
 	CommandRegWrite.cpp \
+	CommandRescan.cpp \
 	CommandSdos.cpp \
 	CommandSiiRead.cpp \
 	CommandSiiWrite.cpp \
@@ -93,6 +94,7 @@ noinst_HEADERS = \
 	CommandPdos.h \
 	CommandRegRead.h \
 	CommandRegWrite.h \
+	CommandRescan.h \
 	CommandSdos.h \
 	CommandSiiRead.h \
 	CommandSiiWrite.h \
diff --git a/tool/MasterDevice.cpp b/tool/MasterDevice.cpp
index e6508342..a6520920 100644
--- a/tool/MasterDevice.cpp
+++ b/tool/MasterDevice.cpp
@@ -455,6 +455,17 @@ void MasterDevice::setDebug(unsigned int debugLevel)
 
 /****************************************************************************/
 
+void MasterDevice::rescan()
+{
+    if (ioctl(fd, EC_IOCTL_MASTER_RESCAN, 0) < 0) {
+        stringstream err;
+        err << "Failed to command rescan: " << strerror(errno);
+        throw MasterDeviceException(err);
+    }
+}
+
+/****************************************************************************/
+
 void MasterDevice::sdoDownload(ec_ioctl_slave_sdo_download_t *data)
 {
     if (ioctl(fd, EC_IOCTL_SLAVE_SDO_DOWNLOAD, data) < 0) {
diff --git a/tool/MasterDevice.h b/tool/MasterDevice.h
index 92301c4f..c5d50f81 100644
--- a/tool/MasterDevice.h
+++ b/tool/MasterDevice.h
@@ -132,6 +132,7 @@ class MasterDevice
         void readReg(ec_ioctl_slave_reg_t *);
         void writeReg(ec_ioctl_slave_reg_t *);
         void setDebug(unsigned int);
+        void rescan();
         void sdoDownload(ec_ioctl_slave_sdo_download_t *);
         void sdoUpload(ec_ioctl_slave_sdo_upload_t *);
         void requestState(uint16_t, uint8_t);
diff --git a/tool/main.cpp b/tool/main.cpp
index c823fcad..0afa35c9 100644
--- a/tool/main.cpp
+++ b/tool/main.cpp
@@ -52,6 +52,7 @@ using namespace std;
 #include "CommandPdos.h"
 #include "CommandRegRead.h"
 #include "CommandRegWrite.h"
+#include "CommandRescan.h"
 #include "CommandSdos.h"
 #include "CommandSiiRead.h"
 #include "CommandSiiWrite.h"
@@ -307,6 +308,7 @@ int main(int argc, char **argv)
     commandList.push_back(new CommandPdos());
     commandList.push_back(new CommandRegRead());
     commandList.push_back(new CommandRegWrite());
+    commandList.push_back(new CommandRescan());
     commandList.push_back(new CommandSdos());
     commandList.push_back(new CommandSiiRead());
     commandList.push_back(new CommandSiiWrite());

From d3a9a36d63041a42048eb465392bd4b87a1fdb17 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Tue, 21 Sep 2010 14:20:23 +0200
Subject: [PATCH 32/34] Always write PDO mapping/assignment.

---
 TODO                   |   1 -
 master/fsm_pdo.c       | 101 ++++++++++++++++++++++-------------------
 master/fsm_pdo_entry.c |  11 -----
 3 files changed, 55 insertions(+), 58 deletions(-)

diff --git a/TODO b/TODO
index 3e981ab9..66341b7a 100644
--- a/TODO
+++ b/TODO
@@ -76,7 +76,6 @@ Future issues:
     - Implement reading from stream for soe_write.
 * Simplify master fsm by introducing a common request state to handle external
   requests (replace write_sii, sdo_request, etc).
-* Write PDO mapping/assignment by default?
 * Remove stdio uses in userspace library?
 
 Smaller issues:
diff --git a/master/fsm_pdo.c b/master/fsm_pdo.c
index f80d1005..7341eed8 100644
--- a/master/fsm_pdo.c
+++ b/master/fsm_pdo.c
@@ -376,7 +376,7 @@ void ec_fsm_pdo_conf_state_start(
         return;
     }
 
-    fsm->sync_index = 0xff; // next is zero
+    fsm->sync_index = 1; // next is 2
     ec_fsm_pdo_conf_action_next_sync(fsm);
 }
 
@@ -429,7 +429,8 @@ void ec_fsm_pdo_conf_action_next_sync(
         }
 
         // get first configured PDO
-        if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {
+        if (!(fsm->pdo =
+                    ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {
             // no pdos configured
             ec_fsm_pdo_conf_action_check_assignment(fsm);
             return;
@@ -506,17 +507,30 @@ void ec_fsm_pdo_conf_action_check_mapping(
         ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
         )
 {
-    if (ec_pdo_equal_entries(fsm->pdo, &fsm->slave_pdo)) {
-        EC_SLAVE_DBG(fsm->slave, 1, "Mapping of PDO 0x%04X"
-                " is already configured correctly.\n", fsm->pdo->index);
-        ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);
+    // check, if slave supports PDO configuration
+    if ((fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)
+            && fsm->slave->sii.has_general
+            && fsm->slave->sii.coe_details.enable_pdo_configuration) {
+
+        // always write PDO mapping
+        ec_fsm_pdo_entry_start_configuration(&fsm->fsm_pdo_entry, fsm->slave,
+                fsm->pdo, &fsm->slave_pdo);
+        fsm->state = ec_fsm_pdo_conf_state_mapping;
+        fsm->state(fsm); // execure immediately
         return;
     }
+    else if (!ec_pdo_equal_entries(fsm->pdo, &fsm->slave_pdo)) {
+        EC_SLAVE_WARN(fsm->slave, "Slave does not support"
+                " changing the PDO mapping!\n");
+        EC_SLAVE_WARN(fsm->slave, "");
+        printk("Currently mapped PDO entries: ");
+        ec_pdo_print_entries(&fsm->slave_pdo);
+        printk(". Entries to map: ");
+        ec_pdo_print_entries(fsm->pdo);
+        printk("\n");
+    }
 
-    ec_fsm_pdo_entry_start_configuration(&fsm->fsm_pdo_entry, fsm->slave,
-            fsm->pdo, &fsm->slave_pdo);
-    fsm->state = ec_fsm_pdo_conf_state_mapping;
-    fsm->state(fsm); // execure immediately
+    ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);
 }
 
 /*****************************************************************************/
@@ -564,46 +578,42 @@ void ec_fsm_pdo_conf_action_check_assignment(
         ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
         )
 {
-    // check if assignment has to be re-configured
-    if (ec_pdo_list_equal(&fsm->sync->pdos, &fsm->pdos)) {
-        EC_SLAVE_DBG(fsm->slave, 1, "PDO assignment for SM%u"
-                " is already configured correctly.\n", fsm->sync_index);
-        ec_fsm_pdo_conf_action_next_sync(fsm);
+    if ((fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)
+            && fsm->slave->sii.has_general
+            && fsm->slave->sii.coe_details.enable_pdo_assign) {
+
+        // always write PDO assignment
+        if (fsm->slave->master->debug_level) {
+            EC_SLAVE_DBG(fsm->slave, 1, "Setting PDO assignment of SM%u:\n",
+                    fsm->sync_index);
+            EC_SLAVE_DBG(fsm->slave, 1, ""); ec_fsm_pdo_print(fsm);
+        }
+
+        if (ec_sdo_request_alloc(&fsm->request, 2)) {
+            fsm->state = ec_fsm_pdo_state_error;
+            return;
+        }
+
+        // set mapped PDO count to zero
+        EC_WRITE_U8(fsm->request.data, 0); // zero PDOs mapped
+        fsm->request.data_size = 1;
+        ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0);
+        ecrt_sdo_request_write(&fsm->request);
+
+        EC_SLAVE_DBG(fsm->slave, 1, "Setting number of assigned"
+                " PDOs to zero.\n");
+
+        fsm->state = ec_fsm_pdo_conf_state_zero_pdo_count;
+        ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
+        ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
         return;
     }
-
-    if (fsm->slave->master->debug_level) {
-        EC_SLAVE_DBG(fsm->slave, 1, "PDO assignment of SM%u differs:\n",
-                fsm->sync_index);
-        EC_SLAVE_DBG(fsm->slave, 1, ""); ec_fsm_pdo_print(fsm);
-    }
-
-    // PDO assignment has to be changed. Does the slave support this?
-    if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)
-            || (fsm->slave->sii.has_general
-                && !fsm->slave->sii.coe_details.enable_pdo_assign)) {
+    else if (!ec_pdo_list_equal(&fsm->sync->pdos, &fsm->pdos)) {
         EC_SLAVE_WARN(fsm->slave, "Slave does not support assigning PDOs!\n");
         EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm);
-        ec_fsm_pdo_conf_action_next_sync(fsm);
-        return;
     }
 
-    if (ec_sdo_request_alloc(&fsm->request, 2)) {
-        fsm->state = ec_fsm_pdo_state_error;
-        return;
-    }
-
-    // set mapped PDO count to zero
-    EC_WRITE_U8(fsm->request.data, 0); // zero PDOs mapped
-    fsm->request.data_size = 1;
-    ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0);
-    ecrt_sdo_request_write(&fsm->request);
-
-    EC_SLAVE_DBG(fsm->slave, 1, "Setting number of assigned PDOs to zero.\n");
-
-    fsm->state = ec_fsm_pdo_conf_state_zero_pdo_count;
-    ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
-    ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+    ec_fsm_pdo_conf_action_next_sync(fsm);
 }
 
 /*****************************************************************************/
@@ -621,7 +631,7 @@ void ec_fsm_pdo_conf_state_zero_pdo_count(
         EC_SLAVE_WARN(fsm->slave, "Failed to clear PDO assignment of SM%u.\n",
                 fsm->sync_index);
         EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm);
-        fsm->state = ec_fsm_pdo_state_error;
+        ec_fsm_pdo_conf_action_next_sync(fsm);
         return;
     }
 
@@ -632,7 +642,6 @@ void ec_fsm_pdo_conf_state_zero_pdo_count(
     
     // find first PDO
     if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {
-        EC_SLAVE_DBG(fsm->slave, 1, "No PDOs to assign.\n");
 
         // check for mapping to be altered
         ec_fsm_pdo_conf_action_next_sync(fsm);
diff --git a/master/fsm_pdo_entry.c b/master/fsm_pdo_entry.c
index be2043aa..b284355c 100644
--- a/master/fsm_pdo_entry.c
+++ b/master/fsm_pdo_entry.c
@@ -328,17 +328,6 @@ void ec_fsm_pdo_entry_conf_state_start(
         ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
         )
 {
-    // PDO mapping has to be changed. Does the slave support this?
-    if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)
-            || (fsm->slave->sii.has_general
-                && !fsm->slave->sii.coe_details.enable_pdo_configuration)) {
-            EC_SLAVE_WARN(fsm->slave, "Slave does not support"
-                    " changing the PDO mapping!\n");
-        EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm);
-        fsm->state = ec_fsm_pdo_entry_state_error;
-        return;
-    }
-
     if (ec_sdo_request_alloc(&fsm->request, 4)) {
         fsm->state = ec_fsm_pdo_entry_state_error;
         return;

From 7edceb20281041fb31365d509b7a93dd8e48b46c Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Wed, 22 Sep 2010 12:26:32 +0200
Subject: [PATCH 33/34] Minor: Improved SII debugging.

---
 master/fsm_sii.c | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/master/fsm_sii.c b/master/fsm_sii.c
index f1f9d923..fb911b38 100644
--- a/master/fsm_sii.c
+++ b/master/fsm_sii.c
@@ -184,7 +184,8 @@ void ec_fsm_sii_state_start_reading(
     EC_WRITE_U16(datagram->data + 2, fsm->word_offset);
 
 #ifdef SII_DEBUG
-    EC_SLAVE_DBG(slave, 0, "reading SII data:\n");
+    EC_SLAVE_DBG(fsm->slave, 0, "reading SII data, word %u:\n",
+            fsm->word_offset);
     ec_print_data(datagram->data, 4);
 #endif
 

From 65cffd14d9e007698937480629bb53633598004d Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh.de>
Date: Wed, 22 Sep 2010 12:28:36 +0200
Subject: [PATCH 34/34] Read at least 64 words, when SII size determination
 failed.

---
 master/fsm_slave_scan.c | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/master/fsm_slave_scan.c b/master/fsm_slave_scan.c
index c5aac8a3..18b4e371 100644
--- a/master/fsm_slave_scan.c
+++ b/master/fsm_slave_scan.c
@@ -505,8 +505,10 @@ void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *fsm /**< slave state
         fsm->slave->error_flag = 1;
         fsm->state = ec_fsm_slave_scan_state_error;
         EC_SLAVE_ERR(slave, "Failed to determine SII content size:"
-                " Reading word offset 0x%04x failed.\n", fsm->sii_offset);
-        return;
+                " Reading word offset 0x%04x failed. Assuming %u words.\n",
+                fsm->sii_offset, EC_FIRST_SII_CATEGORY_OFFSET);
+        slave->sii_nwords = EC_FIRST_SII_CATEGORY_OFFSET;
+        goto alloc_sii;
     }
 
     cat_type = EC_READ_U16(fsm->fsm_sii.value);