# HG changeset patch
# User Martin Troxler <ch1010277@ch10pc446>
# Date 1287391507 -7200
# Node ID 7d748d9cf9e87781a7f08511941f6ceb11ca11ca
# Parent  b369f3f92eb8b7a41a3d77fd5594d217a7cca299# Parent  c6e6ec6ba5d85338e5e3c3585601dcf38f24ed1e
merged

diff -r b369f3f92eb8 -r 7d748d9cf9e8 .hgignore
--- a/.hgignore	Mon Oct 18 10:30:57 2010 +0200
+++ b/.hgignore	Mon Oct 18 10:45:07 2010 +0200
@@ -7,6 +7,7 @@
 **.o
 **.o.cmd
 **.swp
+**~
 .tmp_versions
 ChangeLog
 Doxyfile
diff -r b369f3f92eb8 -r 7d748d9cf9e8 TODO
--- a/TODO	Mon Oct 18 10:30:57 2010 +0200
+++ b/TODO	Mon Oct 18 10:45:07 2010 +0200
@@ -13,38 +13,28 @@
 * 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.
-* 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 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.
-    - Fix AL state display if error bit set.
+* Fix casting away constness during expected WC calculation.
+* Include SoE drive_no in ethercat tool.
+
+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'.
-* Fix casting away constness during expected WC calculation.
-* Read AL status code on spontaneous state change.
-
-Future issues:
-
+* 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?
@@ -77,9 +67,16 @@
     - 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?
+* Remove stdio uses in userspace library?
 
 Smaller issues:
 
diff -r b369f3f92eb8 -r 7d748d9cf9e8 configure.ac
--- a/configure.ac	Mon Oct 18 10:30:57 2010 +0200
+++ b/configure.ac	Mon Oct 18 10:45:07 2010 +0200
@@ -513,6 +513,30 @@
 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 -r b369f3f92eb8 -r 7d748d9cf9e8 devices/e100-2.6.31-ethercat.c
--- a/devices/e100-2.6.31-ethercat.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/devices/e100-2.6.31-ethercat.c	Mon Oct 18 10:45:07 2010 +0200
@@ -704,7 +704,7 @@
 
 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);
diff -r b369f3f92eb8 -r 7d748d9cf9e8 devices/r8169-2.6.31-ethercat.c
--- a/devices/r8169-2.6.31-ethercat.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/devices/r8169-2.6.31-ethercat.c	Mon Oct 18 10:45:07 2010 +0200
@@ -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 @@
 #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 @@
 #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 @@
 	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 @@
 	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 @@
 	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 @@
 {
 	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;
-
-	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.
-		 */
+
+		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;
+
 		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 @@
 	"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 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;
-
-	ASSERT_RTNL();
+	int wait = 1000;
+
+	/*
+	 * 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 @@
 		{ 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 @@
 	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 @@
 	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 @@
 	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 @@
 	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 @@
 		.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 @@
 		.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 @@
 		.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 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 @@
 	}
 
 	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 @@
 	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 @@
 		}
 	}
 
-	pci_set_master(pdev);
-
 	/* ioremap MMIO region */
 	ioaddr = ioremap(pci_resource_start(pdev, region), R8169_REGS_SIZE);
 	if (!ioaddr) {
@@ -2057,9 +2104,20 @@
 
 	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 @@
 			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 @@
 		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 @@
 		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 @@
 		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_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 @@
 			goto err_release_ring_2;
 
 		napi_enable(&tp->napi);
-	}
-
+
+	}
 	rtl_hw_start(dev);
 
 	rtl8169_request_timer(dev);
@@ -2342,9 +2383,9 @@
 	 * 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 @@
 	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 @@
 
 	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 @@
 
 	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 @@
 
 	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 @@
 		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 @@
 	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 @@
 	if (!tp->ecdev)
 		netif_stop_queue(dev);
 	ret = NETDEV_TX_BUSY;
-err_update_stats:
 	dev->stats.tx_dropped++;
 	goto out;
 }
@@ -3390,7 +3421,7 @@
 
 		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 @@
 					netif_receive_skb(skb);
 			}
 
-			dev->last_rx = jiffies;
 			dev->stats.rx_bytes += pkt_size;
 			dev->stats.rx_packets++;
 		}
@@ -3594,54 +3624,64 @@
 	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);
-
-	/* 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);
+	while (status && status != 0xffff) {
+		handled = 1;
+
+		/* 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;
 		}
-	}
-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);
+			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);
+	}
+
 	return IRQ_RETVAL(handled);
 }
 
@@ -3671,14 +3711,16 @@
 	rtl8169_tx_interrupt(dev, tp, ioaddr);
 
 	if (work_done < budget) {
-		netif_rx_complete(dev, napi);
+		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;
-		/*
-		 * 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
-		 */
 		smp_wmb();
 		RTL_W16(IntrMask, tp->intr_event);
 	}
@@ -3709,8 +3751,8 @@
 		netif_stop_queue(dev);
 
 		napi_disable(&tp->napi);
-	}
-
+
+	}
 core_down:
 	if (!tp->ecdev)
 		spin_lock_irq(&tp->lock);
@@ -3755,6 +3797,9 @@
 	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,53 +3894,41 @@
 	return &dev->stats;
 }
 
+static void rtl8169_net_suspend(struct net_device *dev)
+{
+	if (!netif_running(dev))
+		return;
+
+	netif_device_detach(dev);
+	netif_stop_queue(dev);
+}
+
 #ifdef CONFIG_PM
 
-static int rtl8169_suspend(struct pci_dev *pdev, pm_message_t state)
-{
+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);
-	void __iomem *ioaddr = tp->mmio_addr;
-
+	
+	if (tp->ecdev)
+ 		return -EBUSY;
+
+	rtl8169_net_suspend(dev);
+
+	return 0;
+}
+
+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;
 
 	if (!netif_running(dev))
-		goto out_pci_suspend;
-
-	netif_device_detach(dev);
-	netif_stop_queue(dev);
-
-	spin_lock_irq(&tp->lock);
-
-	rtl8169_asic_down(ioaddr);
-
-	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));
-
-	return 0;
-}
-
-static int rtl8169_resume(struct pci_dev *pdev)
-{
-	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;
 
 	netif_device_attach(dev);
@@ -3905,23 +3938,59 @@
 	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);
-}
-
-#endif /* CONFIG_PM */
+	struct net_device *dev = pci_get_drvdata(pdev);
+	struct rtl8169_private *tp = netdev_priv(dev);
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	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 -r b369f3f92eb8 -r 7d748d9cf9e8 devices/r8169-2.6.31-orig.c
--- a/devices/r8169-2.6.31-orig.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/devices/r8169-2.6.31-orig.c	Mon Oct 18 10:45:07 2010 +0200
@@ -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 @@
 #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 @@
 #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 @@
 	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 @@
 	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 @@
 	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 @@
 {
 	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;
-
-	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.
-		 */
+
+		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;
+
 		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 @@
 	"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 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;
-
-	ASSERT_RTNL();
+	int wait = 1000;
+
+	/*
+	 * 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 @@
 		{ 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 @@
 	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 @@
 	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 @@
 	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 @@
 	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 @@
 		.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 @@
 		.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 @@
 		.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 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 @@
 	}
 
 	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 @@
 	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 @@
 		}
 	}
 
-	pci_set_master(pdev);
-
 	/* ioremap MMIO region */
 	ioaddr = ioremap(pci_resource_start(pdev, region), R8169_REGS_SIZE);
 	if (!ioaddr) {
@@ -2040,9 +2087,20 @@
 
 	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 @@
 			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 @@
 		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 @@
 		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 @@
 		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 @@
 	 * 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 @@
 	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 @@
 
 	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 @@
 
 	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 @@
 
 	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 @@
 		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 @@
 	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 @@
 err_stop:
 	netif_stop_queue(dev);
 	ret = NETDEV_TX_BUSY;
-err_update_stats:
 	dev->stats.tx_dropped++;
 	goto out;
 }
@@ -3344,7 +3375,7 @@
 		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 @@
 			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 @@
 	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);
-
-	/* 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);
+	while (status && status != 0xffff) {
+		handled = 1;
+
+		/* 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;
 		}
-	}
-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);
+			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);
+	}
+
 	return IRQ_RETVAL(handled);
 }
 
@@ -3589,14 +3629,16 @@
 	rtl8169_tx_interrupt(dev, tp, ioaddr);
 
 	if (work_done < budget) {
-		netif_rx_complete(dev, napi);
+		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;
-		/*
-		 * 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
-		 */
 		smp_wmb();
 		RTL_W16(IntrMask, tp->intr_event);
 	}
@@ -3668,6 +3710,9 @@
 	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 @@
 	return &dev->stats;
 }
 
-#ifdef CONFIG_PM
-
-static int rtl8169_suspend(struct pci_dev *pdev, pm_message_t state)
-{
-	struct net_device *dev = pci_get_drvdata(pdev);
-	struct rtl8169_private *tp = netdev_priv(dev);
-	void __iomem *ioaddr = tp->mmio_addr;
-
+static void rtl8169_net_suspend(struct net_device *dev)
+{
 	if (!netif_running(dev))
-		goto out_pci_suspend;
+		return;
 
 	netif_device_detach(dev);
 	netif_stop_queue(dev);
-
-	spin_lock_irq(&tp->lock);
-
-	rtl8169_asic_down(ioaddr);
-
-	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));
+}
+
+#ifdef CONFIG_PM
+
+static int rtl8169_suspend(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct net_device *dev = pci_get_drvdata(pdev);
+
+	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 @@
 	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);
-}
-
-#endif /* CONFIG_PM */
+	struct net_device *dev = pci_get_drvdata(pdev);
+	struct rtl8169_private *tp = netdev_priv(dev);
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	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 -r b369f3f92eb8 -r 7d748d9cf9e8 examples/dc_user/main.c
--- a/examples/dc_user/main.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/examples/dc_user/main.c	Mon Oct 18 10:45:07 2010 +0200
@@ -143,7 +143,6 @@
 
         // check for master state (optional)
         check_master_state();
-
     }
 
     if (blink_counter) {
diff -r b369f3f92eb8 -r 7d748d9cf9e8 examples/rtai/rtai_sample.c
--- a/examples/rtai/rtai_sample.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/examples/rtai/rtai_sample.c	Mon Oct 18 10:45:07 2010 +0200
@@ -76,8 +76,8 @@
 // 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 @@
     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;
     }
diff -r b369f3f92eb8 -r 7d748d9cf9e8 include/ecrt.h
--- a/include/ecrt.h	Mon Oct 18 10:30:57 2010 +0200
+++ b/include/ecrt.h	Mon Oct 18 10:45:07 2010 +0200
@@ -416,6 +416,17 @@
     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
  *****************************************************************************/
@@ -697,6 +708,8 @@
         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
@@ -708,10 +721,11 @@
 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. */
-        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. */
         );
 
@@ -726,17 +740,16 @@
 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. */
         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
@@ -1258,7 +1271,10 @@
  */
 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). */
         const uint8_t *data, /**< Pointer to the data. */
         size_t size /**< Size of the \a data. */
         );
diff -r b369f3f92eb8 -r 7d748d9cf9e8 lib/common.c
--- a/lib/common.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/lib/common.c	Mon Oct 18 10:45:07 2010 +0200
@@ -55,9 +55,9 @@
     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 @@
 
     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 @@
                 " %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 @@
 
 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 -r b369f3f92eb8 -r 7d748d9cf9e8 lib/domain.c
--- a/lib/domain.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/lib/domain.c	Mon Oct 18 10:45:07 2010 +0200
@@ -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 -r b369f3f92eb8 -r 7d748d9cf9e8 lib/domain.h
--- a/lib/domain.h	Mon Oct 18 10:30:57 2010 +0200
+++ b/lib/domain.h	Mon Oct 18 10:45:07 2010 +0200
@@ -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 -r b369f3f92eb8 -r 7d748d9cf9e8 lib/master.c
--- a/lib/master.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/lib/master.c	Mon Oct 18 10:45:07 2010 +0200
@@ -53,6 +53,60 @@
 
 /*****************************************************************************/
 
+void ec_master_clear_config(ec_master_t *master)
+{
+    ec_domain_t *d, *next_d;
+    ec_slave_config_t *c, *next_c;
+
+    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) {
+        next_c = c->next;
+        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);
+    }
+}
+
+/*****************************************************************************/
+
+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 +125,33 @@
         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 +178,16 @@
         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;
 }
 
@@ -314,11 +393,13 @@
 /*****************************************************************************/
 
 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)
+        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;
@@ -337,12 +418,13 @@
 /*****************************************************************************/
 
 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)
+        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;
@@ -395,8 +477,9 @@
         fprintf(stderr, "Failed to deactivate master: %s\n", strerror(errno));
         return;
     }
-}
-
+
+    ec_master_clear_config(master);
+}
 
 /*****************************************************************************/
 
@@ -411,7 +494,6 @@
     return 0;
 }
 
-
 /*****************************************************************************/
 
 void ecrt_master_send(ec_master_t *master)
diff -r b369f3f92eb8 -r 7d748d9cf9e8 lib/master.h
--- a/lib/master.h	Mon Oct 18 10:30:57 2010 +0200
+++ b/lib/master.h	Mon Oct 18 10:45:07 2010 +0200
@@ -36,6 +36,13 @@
     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 -r b369f3f92eb8 -r 7d748d9cf9e8 lib/sdo_request.c
--- a/lib/sdo_request.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/lib/sdo_request.c	Mon Oct 18 10:45:07 2010 +0200
@@ -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 -r b369f3f92eb8 -r 7d748d9cf9e8 lib/sdo_request.h
--- a/lib/sdo_request.h	Mon Oct 18 10:30:57 2010 +0200
+++ b/lib/sdo_request.h	Mon Oct 18 10:45:07 2010 +0200
@@ -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 @@
 };
 
 /*****************************************************************************/
+
+void ec_sdo_request_clear(ec_sdo_request_t *);
+
+/*****************************************************************************/
diff -r b369f3f92eb8 -r 7d748d9cf9e8 lib/slave_config.c
--- a/lib/slave_config.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/lib/slave_config.c	Mon Oct 18 10:45:07 2010 +0200
@@ -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)
 {
@@ -378,6 +401,22 @@
 
 /*****************************************************************************/
 
+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)
 {
@@ -410,23 +449,42 @@
     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)
 {
@@ -458,16 +516,19 @@
     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;
 }
 
@@ -489,13 +550,15 @@
 
 /*****************************************************************************/
 
-int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn,
-        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;
     io.size = size;
 
diff -r b369f3f92eb8 -r 7d748d9cf9e8 lib/slave_config.h
--- a/lib/slave_config.h	Mon Oct 18 10:30:57 2010 +0200
+++ b/lib/slave_config.h	Mon Oct 18 10:45:07 2010 +0200
@@ -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 -r b369f3f92eb8 -r 7d748d9cf9e8 lib/voe_handler.c
--- a/lib/voe_handler.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/lib/voe_handler.c	Mon Oct 18 10:45:07 2010 +0200
@@ -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 -r b369f3f92eb8 -r 7d748d9cf9e8 lib/voe_handler.h
--- a/lib/voe_handler.h	Mon Oct 18 10:30:57 2010 +0200
+++ b/lib/voe_handler.h	Mon Oct 18 10:45:07 2010 +0200
@@ -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 @@
 };
 
 /*****************************************************************************/
+
+void ec_voe_handler_clear(ec_voe_handler_t *);
+
+/*****************************************************************************/
diff -r b369f3f92eb8 -r 7d748d9cf9e8 master/cdev.c
--- a/master/cdev.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/cdev.c	Mon Oct 18 10:45:07 2010 +0200
@@ -661,6 +661,19 @@
 
 /*****************************************************************************/
 
+/** 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(
@@ -883,7 +896,11 @@
 
     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");
@@ -988,7 +1005,13 @@
 
     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;
@@ -1726,7 +1749,6 @@
     return 0;
 }
 
-
 /*****************************************************************************/
 
 /** Set max. number of databytes in a cycle
@@ -2533,7 +2555,8 @@
 
     up(&master->master_sem); // FIXME
 
-    ret = ecrt_slave_config_idn(sc, ioctl.idn, data, ioctl.size);
+    ret = ecrt_slave_config_idn(
+            sc, ioctl.drive_no, ioctl.idn, ioctl.al_state, data, ioctl.size);
     kfree(data);
     return ret;
 }
@@ -3388,86 +3411,40 @@
         )
 {
     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;
-    }
-
-    // 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);
-
-    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;
-    }
+    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;
+    }
+
+    retval = ecrt_master_read_idn(master, ioctl.slave_position,
+            ioctl.drive_no, ioctl.idn, data, ioctl.mem_size, &ioctl.data_size,
+            &ioctl.error_code);
+    if (retval) {
+        kfree(data);
+        return retval;
+    }
+
+    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;
 }
 
@@ -3481,79 +3458,37 @@
         )
 {
     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);
-        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;
-    }
-
-    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_from_user(data, (void __user *) ioctl.data, ioctl.data_size)) {
+        kfree(data);
+        return -EFAULT;
+    }
+
+    retval = ecrt_master_write_idn(master, ioctl.slave_position,
+            ioctl.drive_no, ioctl.idn, data, ioctl.data_size,
+            &ioctl.error_code);
+    kfree(data);
+    if (retval) {
+        return retval;
+    }
 
     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;
 }
 
@@ -3583,7 +3518,7 @@
     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;
 }
@@ -3604,7 +3539,7 @@
         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);
@@ -3621,9 +3556,8 @@
     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) {
@@ -3649,6 +3583,10 @@
             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 -r b369f3f92eb8 -r 7d748d9cf9e8 master/device.c
--- a/master/device.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/device.c	Mon Oct 18 10:45:07 2010 +0200
@@ -315,7 +315,7 @@
         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 -r b369f3f92eb8 -r 7d748d9cf9e8 master/fsm_coe.c
--- a/master/fsm_coe.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/fsm_coe.c	Mon Oct 18 10:45:07 2010 +0200
@@ -1103,6 +1103,7 @@
 
     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 @@
     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 @@
         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 @@
         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 @@
         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: ");
@@ -1225,26 +1230,27 @@
         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;
             }
         }
+        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 %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
 
@@ -1268,6 +1274,7 @@
         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 @@
     }
 
     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,9 +1295,10 @@
         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 %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;
         }
@@ -1343,6 +1352,7 @@
     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 @@
         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 @@
     }
 
     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 @@
 
     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 @@
     }
 
     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 @@
     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 @@
         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 @@
     }
 
     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 @@
         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 @@
         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 @@
     }
 
     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 @@
 
     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 @@
     }
 
     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 @@
     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 @@
         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 @@
 
     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 @@
         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);
@@ -1727,25 +1756,26 @@
             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;
             }
         }
+        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 %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
 
@@ -1771,6 +1801,7 @@
         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 @@
     }
 
     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,9 +1821,10 @@
         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 %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;
         }
@@ -1818,6 +1851,7 @@
     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 @@
     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 @@
     }
 
     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 @@
 
     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 @@
     }
 
     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 @@
     }
 
     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 @@
         } else {
             EC_SLAVE_ERR(slave, "No abort message.\n");
         }
+        request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         return;
     }
@@ -1923,6 +1964,7 @@
                 " 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 @@
         }
 
         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 @@
             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 @@
         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 @@
         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 @@
     }
 
     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 @@
         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 @@
     }
 
     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 @@
         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 @@
         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 @@
     }
 
     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 @@
 
     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 @@
     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 @@
         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 @@
                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 @@
         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 -r b369f3f92eb8 -r 7d748d9cf9e8 master/fsm_master.c
--- a/master/fsm_master.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/fsm_master.c	Mon Oct 18 10:45:07 2010 +0200
@@ -85,7 +85,7 @@
     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 @@
 
     // 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 @@
         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 @@
             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 @@
             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;
     }
@@ -792,8 +791,8 @@
         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);
@@ -908,9 +907,9 @@
 
     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, correction32,
-            (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 -r b369f3f92eb8 -r 7d748d9cf9e8 master/fsm_master.h
--- a/master/fsm_master.h	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/fsm_master.h	Mon Oct 18 10:45:07 2010 +0200
@@ -119,7 +119,7 @@
     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 -r b369f3f92eb8 -r 7d748d9cf9e8 master/fsm_pdo.c
--- a/master/fsm_pdo.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/fsm_pdo.c	Mon Oct 18 10:45:07 2010 +0200
@@ -376,7 +376,7 @@
         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 @@
         }
 
         // 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 @@
         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);
-        return;
-    }
-
-    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
+    // 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_conf_action_next_pdo_mapping(fsm);
 }
 
 /*****************************************************************************/
@@ -564,46 +578,42 @@
         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);
-        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)) {
+    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;
+    }
+    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 @@
         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 @@
     
     // 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 -r b369f3f92eb8 -r 7d748d9cf9e8 master/fsm_pdo_entry.c
--- a/master/fsm_pdo_entry.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/fsm_pdo_entry.c	Mon Oct 18 10:45:07 2010 +0200
@@ -328,17 +328,6 @@
         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;
diff -r b369f3f92eb8 -r 7d748d9cf9e8 master/fsm_sii.c
--- a/master/fsm_sii.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/fsm_sii.c	Mon Oct 18 10:45:07 2010 +0200
@@ -184,7 +184,8 @@
     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
 
@@ -277,7 +278,8 @@
 #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 -r b369f3f92eb8 -r 7d748d9cf9e8 master/fsm_slave.c
--- a/master/fsm_slave.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/fsm_slave.c	Mon Oct 18 10:45:07 2010 +0200
@@ -311,7 +311,7 @@
     }
 
     // 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 -r b369f3f92eb8 -r 7d748d9cf9e8 master/fsm_slave_config.c
--- a/master/fsm_slave_config.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/fsm_slave_config.c	Mon Oct 18 10:45:07 2010 +0200
@@ -69,7 +69,7 @@
 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_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_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_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 @@
 
     // 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 @@
     }
 
     // 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;
-    }
-
-    // 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);
+    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;
+        }
+    }
+
+    // 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 @@
         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;
-    }
-
-    // All SDOs are now configured.
+        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 PREOP IDNs are now configured.
     ec_fsm_slave_config_enter_pdo_conf(fsm);
 }
 
@@ -1242,11 +1251,11 @@
     if (0 && 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 +1263,8 @@
             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
@@ -1416,9 +1425,103 @@
         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 -r b369f3f92eb8 -r 7d748d9cf9e8 master/fsm_slave_scan.c
--- a/master/fsm_slave_scan.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/fsm_slave_scan.c	Mon Oct 18 10:45:07 2010 +0200
@@ -52,7 +52,9 @@
 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_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 *);
 
@@ -494,13 +498,17 @@
     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");
-        return;
+        EC_SLAVE_ERR(slave, "Failed to determine SII content size:"
+                " 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);
@@ -694,7 +702,15 @@
         }
     }
 
+#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 +719,12 @@
     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 +742,8 @@
 
 /*****************************************************************************/
 
-/**
-   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 +768,7 @@
         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 +776,8 @@
     }
 }
 
+#endif // defined EC_REGALIAS
+
 /*****************************************************************************/
 
 /** Enter slave scan state PREOP.
diff -r b369f3f92eb8 -r 7d748d9cf9e8 master/fsm_soe.c
--- a/master/fsm_soe.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/fsm_soe.c	Mon Oct 18 10:45:07 2010 +0200
@@ -201,7 +201,8 @@
     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 @@
         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);
 
@@ -265,7 +266,7 @@
         }
         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 +312,8 @@
             (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;
         }
@@ -492,7 +493,8 @@
         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 @@
     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");
@@ -563,7 +565,7 @@
         }
         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 +615,8 @@
                 (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 -r b369f3f92eb8 -r 7d748d9cf9e8 master/ioctl.h
--- a/master/ioctl.h	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/ioctl.h	Mon Oct 18 10:45:07 2010 +0200
@@ -56,7 +56,7 @@
  *
  * Increment this when changing the ioctl interface!
  */
-#define EC_IOCTL_VERSION_MAGIC 6
+#define EC_IOCTL_VERSION_MAGIC 8
 
 // 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)
 #define EC_IOCTL_MASTER_SC_STATE        EC_IOR(0x47, ec_master_state_t)
 #define EC_IOCTL_SC_OVERLAPPING_IO      EC_IOW(0x48, ec_ioctl_config_t)
 
@@ -417,6 +418,7 @@
 typedef struct {
     // inputs
     uint16_t slave_position;
+    uint8_t drive_no;
     uint16_t idn;
     uint32_t mem_size;
     uint8_t *data;
@@ -431,6 +433,7 @@
 typedef struct {
     // inputs
     uint16_t slave_position;
+    uint8_t drive_no;
     uint16_t idn;
     uint32_t data_size;
     uint8_t *data;
@@ -587,7 +590,9 @@
 typedef struct {
     // inputs
     uint32_t config_index;
+    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;
diff -r b369f3f92eb8 -r 7d748d9cf9e8 master/master.c
--- a/master/master.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/master.c	Mon Oct 18 10:45:07 2010 +0200
@@ -443,6 +443,20 @@
 
 /*****************************************************************************/
 
+/** 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(
@@ -657,15 +671,17 @@
         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.
@@ -702,7 +718,7 @@
                 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 {
@@ -729,8 +745,8 @@
                         ((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
@@ -753,7 +769,7 @@
  */
 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;
@@ -1245,8 +1261,8 @@
     // 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()) {
@@ -1312,7 +1328,7 @@
     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()) {
@@ -2036,8 +2052,7 @@
     int eoe_was_running;
 #endif
 
-    EC_MASTER_DBG(master, 1, "ecrt_master_deactivate(master = 0x%x)\n",
-            (u32) 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__);
@@ -2054,10 +2069,7 @@
     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;
@@ -2403,6 +2415,163 @@
 
 /*****************************************************************************/
 
+int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
+        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)) {
+        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,
+        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);
+
+    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);
@@ -2421,6 +2590,8 @@
 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 */
 
diff -r b369f3f92eb8 -r 7d748d9cf9e8 master/master.h
--- a/master/master.h	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/master.h	Mon Oct 18 10:45:07 2010 +0200
@@ -170,11 +170,12 @@
     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. */
@@ -215,9 +216,8 @@
                                       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. */
 
     unsigned int debug_level; /**< Master debug level. */
     ec_stats_t stats; /**< Cyclic statistics. */
@@ -246,7 +246,6 @@
 
     struct list_head reg_requests; /**< Register requests. */
     wait_queue_head_t reg_queue; /**< Wait queue for register requests. */
-
 };
 
 /*****************************************************************************/
@@ -279,7 +278,7 @@
 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 -r b369f3f92eb8 -r 7d748d9cf9e8 master/module.c
--- a/master/module.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/module.c	Mon Oct 18 10:45:07 2010 +0200
@@ -341,13 +341,14 @@
     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("");
         }
diff -r b369f3f92eb8 -r 7d748d9cf9e8 master/sdo_request.c
--- a/master/sdo_request.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/sdo_request.c	Mon Oct 18 10:45:07 2010 +0200
@@ -64,6 +64,7 @@
     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 @@
 {
     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 @@
 {
     req->dir = EC_DIR_OUTPUT;
     req->state = EC_INT_REQUEST_QUEUED;
+    req->errno = 0;
     req->abort_code = 0x00000000;
     req->jiffies_start = jiffies;
 }
diff -r b369f3f92eb8 -r 7d748d9cf9e8 master/sdo_request.h
--- a/master/sdo_request.h	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/sdo_request.h	Mon Oct 18 10:45:07 2010 +0200
@@ -64,6 +64,7 @@
     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. */
 };
 
diff -r b369f3f92eb8 -r 7d748d9cf9e8 master/slave_config.c
--- a/master/slave_config.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/slave_config.c	Mon Oct 18 10:45:07 2010 +0200
@@ -445,7 +445,7 @@
     }
 
     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;
     }
 
@@ -716,6 +716,12 @@
         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;
@@ -772,7 +778,7 @@
 
     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);
@@ -961,15 +967,28 @@
 
 /*****************************************************************************/
 
-int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn,
-        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, "
-            "data = 0x%p, size = %zu)\n", __func__, sc, idn, 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"
+                " 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");
@@ -983,7 +1002,9 @@
     }
 
     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;
 
     ret = ec_soe_request_copy_data(req, data, size);
     if (ret < 0) {
diff -r b369f3f92eb8 -r 7d748d9cf9e8 master/soe_request.c
--- a/master/soe_request.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/soe_request.c	Mon Oct 18 10:45:07 2010 +0200
@@ -56,6 +56,9 @@
         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;
     req->data_size = 0;
@@ -85,12 +88,26 @@
         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);
 }
 
 /*****************************************************************************/
 
+/** 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 -r b369f3f92eb8 -r 7d748d9cf9e8 master/soe_request.h
--- a/master/soe_request.h	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/soe_request.h	Mon Oct 18 10:45:07 2010 +0200
@@ -47,7 +47,9 @@
  */
 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. */
     size_t mem_size; /**< Size of SDO data memory. */
     size_t data_size; /**< Size of SDO data. */
@@ -65,6 +67,7 @@
 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);
diff -r b369f3f92eb8 -r 7d748d9cf9e8 master/voe_handler.c
--- a/master/voe_handler.c	Mon Oct 18 10:30:57 2010 +0200
+++ b/master/voe_handler.c	Mon Oct 18 10:45:07 2010 +0200
@@ -267,8 +267,8 @@
                 (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;
             }
diff -r b369f3f92eb8 -r 7d748d9cf9e8 tool/CommandCStruct.cpp
--- a/tool/CommandCStruct.cpp	Mon Oct 18 10:30:57 2010 +0200
+++ b/tool/CommandCStruct.cpp	Mon Oct 18 10:45:07 2010 +0200
@@ -117,7 +117,7 @@
     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 @@
         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 @@
             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 @@
 
     cout << "ec_sync_info_t " << id.str() << "syncs[] = {" << endl
         << syncs.str()
+        << "    {0xff}" << endl
         << "};" << endl
         << endl;
 }
diff -r b369f3f92eb8 -r 7d748d9cf9e8 tool/CommandDomains.cpp
--- a/tool/CommandDomains.cpp	Mon Oct 18 10:30:57 2010 +0200
+++ b/tool/CommandDomains.cpp	Mon Oct 18 10:45:07 2010 +0200
@@ -71,7 +71,7 @@
         << 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
@@ -187,7 +187,7 @@
         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;
diff -r b369f3f92eb8 -r 7d748d9cf9e8 tool/CommandMaster.cpp
--- a/tool/CommandMaster.cpp	Mon Oct 18 10:30:57 2010 +0200
+++ b/tool/CommandMaster.cpp	Mon Oct 18 10:45:07 2010 +0200
@@ -149,8 +149,8 @@
                     << "      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,8 +159,8 @@
                     << "      Tx rate [KByte/s]:   "
                     << setprecision(1) << fixed;
                 for (j = 0; j < EC_RATE_COUNT; j++) {
-                    cout << setw(5)
-                        << data.devices[i].tx_byte_rates[j] / 1024000.0;
+                    cout << setw(ColWidth)
+                        << data.devices[i].tx_byte_rates[j] / 1024.0;
                     if (j < EC_RATE_COUNT - 1) {
                         cout << " ";
                     }
@@ -169,7 +169,8 @@
                     << "      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 @@
                         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 -r b369f3f92eb8 -r 7d748d9cf9e8 tool/CommandMaster.h
--- a/tool/CommandMaster.h	Mon Oct 18 10:30:57 2010 +0200
+++ b/tool/CommandMaster.h	Mon Oct 18 10:45:07 2010 +0200
@@ -42,6 +42,9 @@
 
         string helpString() const;
         void execute(const StringVector &);
+
+    private:
+        enum {ColWidth = 6};
 };
 
 /****************************************************************************/
diff -r b369f3f92eb8 -r 7d748d9cf9e8 tool/CommandRescan.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/tool/CommandRescan.cpp	Mon Oct 18 10:45:07 2010 +0200
@@ -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 -r b369f3f92eb8 -r 7d748d9cf9e8 tool/CommandRescan.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/tool/CommandRescan.h	Mon Oct 18 10:45:07 2010 +0200
@@ -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 -r b369f3f92eb8 -r 7d748d9cf9e8 tool/CommandSoeRead.cpp
--- a/tool/CommandSoeRead.cpp	Mon Oct 18 10:30:57 2010 +0200
+++ b/tool/CommandSoeRead.cpp	Mon Oct 18 10:45:07 2010 +0200
@@ -90,6 +90,8 @@
         throwInvalidUsageException(err);
     }
 
+    ioctl.drive_no = 0; // FIXME
+
     try {
         ioctl.idn = parseIdn(args[0]);
     } catch (runtime_error &e) {
diff -r b369f3f92eb8 -r 7d748d9cf9e8 tool/CommandSoeWrite.cpp
--- a/tool/CommandSoeWrite.cpp	Mon Oct 18 10:30:57 2010 +0200
+++ b/tool/CommandSoeWrite.cpp	Mon Oct 18 10:45:07 2010 +0200
@@ -92,6 +92,8 @@
         throwInvalidUsageException(err);
     }
 
+    ioctl.drive_no = 0; // FIXME
+
     try {
         ioctl.idn = parseIdn(args[0]);
     } catch (runtime_error &e) {
diff -r b369f3f92eb8 -r 7d748d9cf9e8 tool/Makefile.am
--- a/tool/Makefile.am	Mon Oct 18 10:30:57 2010 +0200
+++ b/tool/Makefile.am	Mon Oct 18 10:45:07 2010 +0200
@@ -52,6 +52,7 @@
 	CommandPdos.cpp \
 	CommandRegRead.cpp \
 	CommandRegWrite.cpp \
+	CommandRescan.cpp \
 	CommandSdos.cpp \
 	CommandSiiRead.cpp \
 	CommandSiiWrite.cpp \
@@ -93,6 +94,7 @@
 	CommandPdos.h \
 	CommandRegRead.h \
 	CommandRegWrite.h \
+	CommandRescan.h \
 	CommandSdos.h \
 	CommandSiiRead.h \
 	CommandSiiWrite.h \
diff -r b369f3f92eb8 -r 7d748d9cf9e8 tool/MasterDevice.cpp
--- a/tool/MasterDevice.cpp	Mon Oct 18 10:30:57 2010 +0200
+++ b/tool/MasterDevice.cpp	Mon Oct 18 10:45:07 2010 +0200
@@ -455,6 +455,17 @@
 
 /****************************************************************************/
 
+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 -r b369f3f92eb8 -r 7d748d9cf9e8 tool/MasterDevice.h
--- a/tool/MasterDevice.h	Mon Oct 18 10:30:57 2010 +0200
+++ b/tool/MasterDevice.h	Mon Oct 18 10:45:07 2010 +0200
@@ -132,6 +132,7 @@
         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 -r b369f3f92eb8 -r 7d748d9cf9e8 tool/main.cpp
--- a/tool/main.cpp	Mon Oct 18 10:30:57 2010 +0200
+++ b/tool/main.cpp	Mon Oct 18 10:45:07 2010 +0200
@@ -52,6 +52,7 @@
 #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 @@
     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());