diff --git a/devices/cadence/macb-6.6-ethercat.h b/devices/cadence/macb-6.6-ethercat.h
index be5fe4671cd75f450d63eb11f6b16672ebb106bd..dfe61027bfe8e10d34acc2e8efa6405246c0b287 100644
--- a/devices/cadence/macb-6.6-ethercat.h
+++ b/devices/cadence/macb-6.6-ethercat.h
@@ -1339,11 +1339,12 @@ struct macb {
 	struct phylink_config	phylink_config;
 	struct phylink_pcs	phylink_usx_pcs;
 	struct phylink_pcs	phylink_sgmii_pcs;
-	int			link;
 	int			speed;
 	int			duplex;
 	int			use_ncsi;
 
+	int             	force_phy_mode;
+
 	u32			caps;
 	unsigned int		dma_burst_length;
 
diff --git a/devices/cadence/macb-6.6-orig.h b/devices/cadence/macb-6.6-orig.h
index 3a5441dcc7337744e9de84094c84b6e4fbbc4e0e..d8a84e28d63c2c493f43e29712e293e4ccab2c5b 100644
--- a/devices/cadence/macb-6.6-orig.h
+++ b/devices/cadence/macb-6.6-orig.h
@@ -1337,11 +1337,12 @@ struct macb {
 	struct phylink_config	phylink_config;
 	struct phylink_pcs	phylink_usx_pcs;
 	struct phylink_pcs	phylink_sgmii_pcs;
-	int			link;
 	int			speed;
 	int			duplex;
 	int			use_ncsi;
 
+	int             	force_phy_mode;
+
 	u32			caps;
 	unsigned int		dma_burst_length;
 
diff --git a/devices/cadence/macb_main-4.19-ethercat.c b/devices/cadence/macb_main-4.19-ethercat.c
index 9bcf1d889addef2bda87c5e324b442d40c69363f..14e2a75d8e63e7c3faf4907ac308d29bc297116e 100644
--- a/devices/cadence/macb_main-4.19-ethercat.c
+++ b/devices/cadence/macb_main-4.19-ethercat.c
@@ -1437,12 +1437,11 @@ static int gem_rx(struct macb_queue *queue, int budget)
 			  !(bp->dev->flags & IFF_PROMISC) &&
 			  GEM_BFEXT(RX_CSUM, ctrl) & GEM_RX_CSUM_CHECKED_MASK)
 				skb->ip_summed = CHECKSUM_UNNECESSARY;
-
-			bp->dev->stats.rx_packets++;
-			queue->stats.rx_packets++;
-			bp->dev->stats.rx_bytes += skb->len;
-			queue->stats.rx_bytes += skb->len;
 		}
+		bp->dev->stats.rx_packets++;
+		queue->stats.rx_packets++;
+		bp->dev->stats.rx_bytes += skb->len;
+		queue->stats.rx_bytes += skb->len;
 
 		gem_ptp_do_rxstamp(bp, skb, desc);
 
@@ -3088,6 +3087,7 @@ static int macb_open(struct net_device *dev)
 	return 0;
 }
 
+static void gem_update_stats(struct macb *bp);
 static int macb_close(struct net_device *dev)
 {
 	struct macb *bp = netdev_priv(dev);
@@ -3099,6 +3099,15 @@ static int macb_close(struct net_device *dev)
 		netif_tx_stop_all_queues(dev);
 		for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue)
 			napi_disable(&queue->napi);
+	} else {
+		int i;
+		printk("%s tx packets %lu\n", __func__, bp->dev->stats.tx_packets);
+		printk("%s tx dropped packets %lu\n", __func__, bp->dev->stats.tx_dropped);
+		printk("%s rx packets %lu\n", __func__, bp->dev->stats.rx_packets);
+		printk("%s rx dropped packets %lu\n", __func__, bp->dev->stats.rx_dropped);
+		gem_update_stats(bp);
+		for (i = 0; i < GEM_STATS_LEN; i++)
+			printk("%s %llu\n", gem_statistics[i].stat_string,  (unsigned long long)bp->ethtool_stats[i]);
 	}
 	if (dev->phydev)
 		phy_stop(dev->phydev);
diff --git a/devices/cadence/macb_main-5.10-ethercat.c b/devices/cadence/macb_main-5.10-ethercat.c
index 57c993fb5eb7374001d42ac33ec57bfc76b39b02..894a34dd3ece8905e73af47e9c029f9256f40648 100644
--- a/devices/cadence/macb_main-5.10-ethercat.c
+++ b/devices/cadence/macb_main-5.10-ethercat.c
@@ -1793,12 +1793,11 @@ static int gem_rx(struct macb_queue *queue, struct napi_struct *napi,
 			  !(bp->dev->flags & IFF_PROMISC) &&
 			  GEM_BFEXT(RX_CSUM, ctrl) & GEM_RX_CSUM_CHECKED_MASK)
 				skb->ip_summed = CHECKSUM_UNNECESSARY;
-
-			bp->dev->stats.rx_packets++;
-			queue->stats.rx_packets++;
-			bp->dev->stats.rx_bytes += skb->len;
-			queue->stats.rx_bytes += skb->len;
 		}
+		bp->dev->stats.rx_packets++;
+		queue->stats.rx_packets++;
+		bp->dev->stats.rx_bytes += skb->len;
+		queue->stats.rx_bytes += skb->len;
 
 		gem_ptp_do_rxstamp(bp, skb, desc);
 
@@ -3503,6 +3502,7 @@ static int macb_open(struct net_device *dev)
 	return ret;
 }
 
+static void gem_update_stats(struct macb *bp);
 static int __macb_close(struct net_device *dev)
 {
 	struct macb *bp = netdev_priv(dev);
@@ -3514,6 +3514,15 @@ static int __macb_close(struct net_device *dev)
 		netif_tx_stop_all_queues(dev);
 		for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue)
 			napi_disable(&queue->napi);
+	} else {
+		int i;
+		printk("%s tx packets %lu\n", __func__, bp->dev->stats.tx_packets);
+		printk("%s tx dropped packets %lu\n", __func__, bp->dev->stats.tx_dropped);
+		printk("%s rx packets %lu\n", __func__, bp->dev->stats.rx_packets);
+		printk("%s rx dropped packets %lu\n", __func__, bp->dev->stats.rx_dropped);
+		gem_update_stats(bp);
+		for (i = 0; i < GEM_STATS_LEN; i++)
+			printk("%s %llu\n", gem_statistics[i].stat_string,  (unsigned long long)bp->ethtool_stats[i]);
 	}
 	phylink_stop(bp->phylink);
 	phylink_disconnect_phy(bp->phylink);
diff --git a/devices/cadence/macb_main-6.6-ethercat.c b/devices/cadence/macb_main-6.6-ethercat.c
index 54214137382db0cf95a3a4c4e48687fe87b8d661..7129ee06d2953b27eaac1b2732b57c14841ab6b4 100644
--- a/devices/cadence/macb_main-6.6-ethercat.c
+++ b/devices/cadence/macb_main-6.6-ethercat.c
@@ -88,7 +88,7 @@ struct sifive_fu540_macb_mgmt {
 #define GEM_MAX_TX_LEN		(unsigned int)(0x3FC0)
 
 #define GEM_MTU_MIN_SIZE	ETH_MIN_MTU
-#define MACB_NETIF_LSO		NETIF_F_TSO
+#define MACB_NETIF_LSO		(NETIF_F_TSO | NETIF_F_TSO6)
 
 #define MACB_WOL_HAS_MAGIC_PACKET	(0x1 << 0)
 #define MACB_WOL_ENABLED		(0x1 << 1)
@@ -640,7 +640,7 @@ static int macb_usx_pcs_config(struct phylink_pcs *pcs,
 	struct macb *bp = container_of(pcs, struct macb, phylink_usx_pcs);
 
 	gem_writel(bp, USX_CONTROL, gem_readl(bp, USX_CONTROL) |
-		   GEM_BIT(SIGNAL_OK));
+		   GEM_BIT(SIGNAL_OK) | GEM_BIT(TX_EN));
 
 	return 0;
 }
@@ -765,7 +765,7 @@ static void macb_mac_link_down(struct phylink_config *config, unsigned int mode,
 	macb_writel(bp, NCR, ctrl);
 
 	if (bp->ecdev) {
-		bp->link = 0;
+		ecdev_set_link(bp->ecdev, 0);
 		return;
 	}
 
@@ -994,6 +994,9 @@ static void macb_mac_link_up(struct phylink_config *config,
 
 		macb_set_tx_clk(bp, speed);
 
+		bp->speed = speed;
+		bp->duplex = duplex;
+
 		/* Initialize rings & buffers as clearing MACB_BIT(TE) in link down
 		 * cleared the pipeline and control registers.
 		 */
@@ -1063,7 +1066,7 @@ static void macb_mac_link_up(struct phylink_config *config,
 	if (!bp->ecdev)
 		netif_tx_wake_all_queues(ndev);
 	else
-		bp->link = 1;
+		ecdev_set_link(bp->ecdev, 1);;
 }
 
 static struct phylink_pcs *macb_mac_select_pcs(struct phylink_config *config,
@@ -1091,33 +1094,26 @@ static const struct phylink_mac_ops macb_phylink_ops = {
 	.mac_link_up = macb_mac_link_up,
 };
 
-static bool macb_phy_handle_exists(struct device_node *dn)
-{
-	dn = of_parse_phandle(dn, "phy-handle", 0);
-	of_node_put(dn);
-	return dn != NULL;
-}
-
 static int macb_phylink_connect(struct macb *bp)
 {
-	struct device_node *dn = bp->pdev->dev.of_node;
+	struct fwnode_handle *fwnode = dev_fwnode(&bp->pdev->dev);
 	struct net_device *dev = bp->dev;
 	struct phy_device *phydev;
 	int ret = 0;
 
-	if (dn)
-		ret = phylink_of_phy_connect(bp->phylink, dn, 0);
+	if (fwnode)
+		ret = phylink_fwnode_phy_connect(bp->phylink, fwnode, 0);
 
-	if (!dn || (ret && !macb_phy_handle_exists(dn))) {
+	if (!fwnode || ret) {
 		phydev = phy_find_first(bp->mii_bus);
 		if (!phydev) {
 			netdev_err(dev, "no PHY found\n");
 			return -ENXIO;
 		}
+		phydev->force_mode = bp->force_phy_mode;
 
 		/* attach the mac to the phy */
-		if (phylink_expects_phy(bp->phylink))
-			ret = phylink_connect_phy(bp->phylink, phydev);
+		ret = phylink_connect_phy(bp->phylink, phydev);
 	}
 
 	if (ret) {
@@ -1849,12 +1845,11 @@ static int gem_rx(struct macb_queue *queue, struct napi_struct *napi,
 			  !(bp->dev->flags & IFF_PROMISC) &&
 			  GEM_BFEXT(RX_CSUM, ctrl) & GEM_RX_CSUM_CHECKED_MASK)
 				skb->ip_summed = CHECKSUM_UNNECESSARY;
-
-			bp->dev->stats.rx_packets++;
-			queue->stats.rx_packets++;
-			bp->dev->stats.rx_bytes += skb->len;
-			queue->stats.rx_bytes += skb->len;
 		}
+		bp->dev->stats.rx_packets++;
+		queue->stats.rx_packets++;
+		bp->dev->stats.rx_bytes += skb->len;
+		queue->stats.rx_bytes += skb->len;
 
 		gem_ptp_do_rxstamp(bp, skb, desc);
 
@@ -2121,7 +2116,8 @@ static void macb_tx_restart(struct macb_queue *queue)
 	if (queue->tx_head == queue->tx_tail)
 		goto out_tx_ptr_unlock;
 
-	tbqp = queue_readl(queue, TBQP) / macb_dma_desc_get_size(bp);
+	tbqp = queue_readl(queue, TBQP) - lower_32_bits(queue->tx_ring_dma);
+	tbqp = tbqp / macb_dma_desc_get_size(bp);
 	tbqp = macb_adj_dma_desc_idx(bp, macb_tx_ring_wrap(bp, tbqp));
 	head_idx = macb_adj_dma_desc_idx(bp, macb_tx_ring_wrap(bp, queue->tx_head));
 
@@ -2228,10 +2224,8 @@ static void ec_poll(struct net_device *dev)
 		ec_tx_poll(queue, 64);
 	}
 
-	if (jiffies - bp->ec_watchdog_jiffies >= 2 * HZ) {
-		ecdev_set_link(bp->ecdev, bp->link ? 1 : 0);
+	if (jiffies - bp->ec_watchdog_jiffies >= 2 * HZ)
 		bp->ec_watchdog_jiffies = jiffies;
-	}
 }
 
 static void macb_hresp_error_task(struct tasklet_struct *t)
@@ -3441,9 +3435,6 @@ static void macb_init_hw(struct macb *bp)
 		if (bp->caps & MACB_CAPS_SEL_CLK)
 			bp->sel_clk_hw(bp, bp->speed);
 		phytium_mac_config(bp);
-		if (bp->link)
-			macb_usx_pcs_link_up(&bp->phylink_usx_pcs, 0,
-					     bp->phy_interface, bp->speed, bp->duplex);
 	} else {
 		bp->speed = SPEED_10;
 		bp->duplex = DUPLEX_HALF;
@@ -3650,6 +3641,7 @@ static int macb_open(struct net_device *dev)
 	return ret;
 }
 
+static void gem_update_stats(struct macb *bp);
 static int __macb_close(struct net_device *dev)
 {
 	struct macb *bp = netdev_priv(dev);
@@ -3664,6 +3656,15 @@ static int __macb_close(struct net_device *dev)
 			napi_disable(&queue->napi_rx);
 			napi_disable(&queue->napi_tx);
 		}
+	} else {
+		int i;
+		printk("%s tx packets %lu\n", __func__, bp->dev->stats.tx_packets);
+		printk("%s tx dropped packets %lu\n", __func__, bp->dev->stats.tx_dropped);
+		printk("%s rx packets %lu\n", __func__, bp->dev->stats.rx_packets);
+		printk("%s rx dropped packets %lu\n", __func__, bp->dev->stats.rx_dropped);
+		gem_update_stats(bp);
+		for (i = 0; i < GEM_STATS_LEN; i++)
+			printk("%s %llu\n", gem_statistics[i].stat_string,  (unsigned long long)bp->ethtool_stats[i]);
 	}
 
 	phylink_stop(bp->phylink);
@@ -4002,8 +4003,13 @@ static int macb_get_link_ksettings(struct net_device *netdev,
 							supported);
 		ethtool_convert_legacy_u32_to_link_mode(kset->link_modes.advertising,
 							advertising);
-		kset->base.speed = bp->speed;
-		kset->base.duplex = bp->duplex;
+		if (netif_carrier_ok(netdev)) {
+			kset->base.speed = bp->speed;
+			kset->base.duplex = bp->duplex;
+		} else {
+			kset->base.speed = SPEED_UNKNOWN;
+			kset->base.duplex = DUPLEX_UNKNOWN;
+		}
 	} else {
 		phylink_ethtool_ksettings_get(bp->phylink, kset);
 	}
@@ -5040,7 +5046,6 @@ static const struct macb_usrio_config macb_default_usrio = {
 	.refclk = MACB_BIT(CLKEN),
 };
 
-#if defined(CONFIG_OF)
 /* 1518 rounded up */
 #define AT91ETHER_MAX_RBUFF_SZ	0x600
 /* max number of receive buffers */
@@ -5757,6 +5762,7 @@ static const struct macb_config phytium_gem2p0_config = {
 	.usrio = &macb_default_usrio,
 };
 
+#if defined(CONFIG_OF)
 static const struct of_device_id macb_dt_ids[] = {
 	{ .compatible = "cdns,at91sam9260-macb", .data = &at91sam9260_config },
 	{ .compatible = "cdns,macb" },
@@ -6035,6 +6041,10 @@ static int macb_probe(struct platform_device *pdev)
 	if (err)
 		goto err_out_free_netdev;
 
+	if (device_property_read_bool(&pdev->dev, "force-phy-mode")) {
+		bp->force_phy_mode = 1;
+	}
+
 	err = macb_mii_init(bp);
 	if (err)
 		goto err_out_phy_exit;
@@ -6088,8 +6098,10 @@ static int macb_probe(struct platform_device *pdev)
 	return 0;
 
 err_out_unregister_mdio:
-	mdiobus_unregister(bp->mii_bus);
-	mdiobus_free(bp->mii_bus);
+	if (bp->mii_bus) {
+		mdiobus_unregister(bp->mii_bus);
+		mdiobus_free(bp->mii_bus);
+	}
 
 err_out_phy_exit:
 	phy_exit(bp->sgmii_phy);
@@ -6125,9 +6137,10 @@ static int macb_remove(struct platform_device *pdev)
 			unregister_netdev(dev);
 
 		phy_exit(bp->sgmii_phy);
-		mdiobus_unregister(bp->mii_bus);
-		mdiobus_free(bp->mii_bus);
-
+		if (bp->mii_bus) {
+			mdiobus_unregister(bp->mii_bus);
+			mdiobus_free(bp->mii_bus);
+		}
 		if (!bp->ecdev) {
 			tasklet_kill(&bp->hresp_err_tasklet);
 			pm_runtime_disable(&pdev->dev);
diff --git a/devices/cadence/macb_main-6.6-orig.c b/devices/cadence/macb_main-6.6-orig.c
index 18e28ac80f5f613df533591890e1e0905cef44c8..f656e817c515a966d3a12820c67d276436e44061 100644
--- a/devices/cadence/macb_main-6.6-orig.c
+++ b/devices/cadence/macb_main-6.6-orig.c
@@ -88,7 +88,7 @@ struct sifive_fu540_macb_mgmt {
 #define GEM_MAX_TX_LEN		(unsigned int)(0x3FC0)
 
 #define GEM_MTU_MIN_SIZE	ETH_MIN_MTU
-#define MACB_NETIF_LSO		NETIF_F_TSO
+#define MACB_NETIF_LSO		(NETIF_F_TSO | NETIF_F_TSO6)
 
 #define MACB_WOL_HAS_MAGIC_PACKET	(0x1 << 0)
 #define MACB_WOL_ENABLED		(0x1 << 1)
@@ -640,7 +640,7 @@ static int macb_usx_pcs_config(struct phylink_pcs *pcs,
 	struct macb *bp = container_of(pcs, struct macb, phylink_usx_pcs);
 
 	gem_writel(bp, USX_CONTROL, gem_readl(bp, USX_CONTROL) |
-		   GEM_BIT(SIGNAL_OK));
+		   GEM_BIT(SIGNAL_OK) | GEM_BIT(TX_EN));
 
 	return 0;
 }
@@ -989,6 +989,9 @@ static void macb_mac_link_up(struct phylink_config *config,
 
 		macb_set_tx_clk(bp, speed);
 
+		bp->speed = speed;
+		bp->duplex = duplex;
+
 		/* Initialize rings & buffers as clearing MACB_BIT(TE) in link down
 		 * cleared the pipeline and control registers.
 		 */
@@ -1080,33 +1083,26 @@ static const struct phylink_mac_ops macb_phylink_ops = {
 	.mac_link_up = macb_mac_link_up,
 };
 
-static bool macb_phy_handle_exists(struct device_node *dn)
-{
-	dn = of_parse_phandle(dn, "phy-handle", 0);
-	of_node_put(dn);
-	return dn != NULL;
-}
-
 static int macb_phylink_connect(struct macb *bp)
 {
-	struct device_node *dn = bp->pdev->dev.of_node;
+	struct fwnode_handle *fwnode = dev_fwnode(&bp->pdev->dev);
 	struct net_device *dev = bp->dev;
 	struct phy_device *phydev;
 	int ret = 0;
 
-	if (dn)
-		ret = phylink_of_phy_connect(bp->phylink, dn, 0);
+	if (fwnode)
+		ret = phylink_fwnode_phy_connect(bp->phylink, fwnode, 0);
 
-	if (!dn || (ret && !macb_phy_handle_exists(dn))) {
+	if (!fwnode || ret) {
 		phydev = phy_find_first(bp->mii_bus);
 		if (!phydev) {
 			netdev_err(dev, "no PHY found\n");
 			return -ENXIO;
 		}
+		phydev->force_mode = bp->force_phy_mode;
 
 		/* attach the mac to the phy */
-		if (phylink_expects_phy(bp->phylink))
-			ret = phylink_connect_phy(bp->phylink, phydev);
+		ret = phylink_connect_phy(bp->phylink, phydev);
 	}
 
 	if (ret) {
@@ -2018,7 +2014,8 @@ static void macb_tx_restart(struct macb_queue *queue)
 	if (queue->tx_head == queue->tx_tail)
 		goto out_tx_ptr_unlock;
 
-	tbqp = queue_readl(queue, TBQP) / macb_dma_desc_get_size(bp);
+	tbqp = queue_readl(queue, TBQP) - lower_32_bits(queue->tx_ring_dma);
+	tbqp = tbqp / macb_dma_desc_get_size(bp);
 	tbqp = macb_adj_dma_desc_idx(bp, macb_tx_ring_wrap(bp, tbqp));
 	head_idx = macb_adj_dma_desc_idx(bp, macb_tx_ring_wrap(bp, queue->tx_head));
 
@@ -3248,9 +3245,6 @@ static void macb_init_hw(struct macb *bp)
 		if (bp->caps & MACB_CAPS_SEL_CLK)
 			bp->sel_clk_hw(bp, bp->speed);
 		phytium_mac_config(bp);
-		if (bp->link)
-			macb_usx_pcs_link_up(&bp->phylink_usx_pcs, 0,
-					     bp->phy_interface, bp->speed, bp->duplex);
 	} else {
 		bp->speed = SPEED_10;
 		bp->duplex = DUPLEX_HALF;
@@ -3784,8 +3778,13 @@ static int macb_get_link_ksettings(struct net_device *netdev,
 							supported);
 		ethtool_convert_legacy_u32_to_link_mode(kset->link_modes.advertising,
 							advertising);
-		kset->base.speed = bp->speed;
-		kset->base.duplex = bp->duplex;
+		if (netif_carrier_ok(netdev)) {
+			kset->base.speed = bp->speed;
+			kset->base.duplex = bp->duplex;
+		} else {
+			kset->base.speed = SPEED_UNKNOWN;
+			kset->base.duplex = DUPLEX_UNKNOWN;
+		}
 	} else {
 		phylink_ethtool_ksettings_get(bp->phylink, kset);
 	}
@@ -4818,7 +4817,6 @@ static const struct macb_usrio_config macb_default_usrio = {
 	.refclk = MACB_BIT(CLKEN),
 };
 
-#if defined(CONFIG_OF)
 /* 1518 rounded up */
 #define AT91ETHER_MAX_RBUFF_SZ	0x600
 /* max number of receive buffers */
@@ -5535,6 +5533,7 @@ static const struct macb_config phytium_gem2p0_config = {
 	.usrio = &macb_default_usrio,
 };
 
+#if defined(CONFIG_OF)
 static const struct of_device_id macb_dt_ids[] = {
 	{ .compatible = "cdns,at91sam9260-macb", .data = &at91sam9260_config },
 	{ .compatible = "cdns,macb" },
@@ -5799,6 +5798,10 @@ static int macb_probe(struct platform_device *pdev)
 	if (err)
 		goto err_out_free_netdev;
 
+	if (device_property_read_bool(&pdev->dev, "force-phy-mode")) {
+		bp->force_phy_mode = 1;
+	}
+
 	err = macb_mii_init(bp);
 	if (err)
 		goto err_out_phy_exit;
@@ -5837,8 +5840,10 @@ static int macb_probe(struct platform_device *pdev)
 	return 0;
 
 err_out_unregister_mdio:
-	mdiobus_unregister(bp->mii_bus);
-	mdiobus_free(bp->mii_bus);
+	if (bp->mii_bus) {
+		mdiobus_unregister(bp->mii_bus);
+		mdiobus_free(bp->mii_bus);
+	}
 
 err_out_phy_exit:
 	phy_exit(bp->sgmii_phy);
@@ -5866,8 +5871,11 @@ static int macb_remove(struct platform_device *pdev)
 		bp = netdev_priv(dev);
 		unregister_netdev(dev);
 		phy_exit(bp->sgmii_phy);
-		mdiobus_unregister(bp->mii_bus);
-		mdiobus_free(bp->mii_bus);
+
+		if (bp->mii_bus) {
+			mdiobus_unregister(bp->mii_bus);
+			mdiobus_free(bp->mii_bus);
+		}
 
 		tasklet_kill(&bp->hresp_err_tasklet);
 		pm_runtime_disable(&pdev->dev);
diff --git a/lib/slave_config.c b/lib/slave_config.c
index 7bef3a9918107313132c2930bfbec29b3d38ae95..58d6c7b407b9178307412698fc142daec0c39997 100644
--- a/lib/slave_config.c
+++ b/lib/slave_config.c
@@ -40,6 +40,7 @@
 void ec_slave_config_add_sdo_request(ec_slave_config_t *, ec_sdo_request_t *);
 void ec_slave_config_add_reg_request(ec_slave_config_t *, ec_reg_request_t *);
 void ec_slave_config_add_voe_handler(ec_slave_config_t *, ec_voe_handler_t *);
+void ec_slave_config_add_soe_request(ec_slave_config_t *, ec_soe_request_t *);
 
 /****************************************************************************/
 
@@ -625,7 +626,7 @@ ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc,
 
 /****************************************************************************/
 
-inline void ec_slave_config_add_soe_request(ec_slave_config_t *sc,
+void ec_slave_config_add_soe_request(ec_slave_config_t *sc,
         ec_soe_request_t *req)
 {
     if (sc->first_soe_request) {
diff --git a/master/fsm_coe.c b/master/fsm_coe.c
index 69765cd006ebacc05eea1994b3b942218489ef58..7ba727c421ad311753bd62384b73b94632175bde 100644
--- a/master/fsm_coe.c
+++ b/master/fsm_coe.c
@@ -1614,14 +1614,15 @@ void ec_fsm_coe_down_response(
         } else {
             sprintf(subidxstr, ":%02X", request->subindex);
         }
-        EC_SLAVE_ERR(slave, "SDO download 0x%04X%s (%zu bytes) aborted.\n",
-                request->index, subidxstr, request->data_size);
+        EC_SLAVE_ERR(slave, "%s SDO download 0x%04X%s (%zu bytes) aborted.\n",
+                __func__, request->index, subidxstr, request->data_size);
         if (rec_size < 10) {
             EC_SLAVE_ERR(slave, "Incomplete abort command:\n");
             ec_print_data(data, rec_size);
         } else {
             fsm->request->abort_code = EC_READ_U32(data + 6);
             ec_canopen_abort_msg(slave, fsm->request->abort_code);
+            ec_print_data(data, rec_size);
         }
         return;
     }
@@ -1792,14 +1793,15 @@ void ec_fsm_coe_down_seg_response(
         } else {
             sprintf(subidxstr, ":%02X", request->subindex);
         }
-        EC_SLAVE_ERR(slave, "SDO download 0x%04X%s (%zu bytes) aborted.\n",
-                request->index, subidxstr, request->data_size);
+        EC_SLAVE_ERR(slave, "%s SDO download 0x%04X%s (%zu bytes) aborted.\n",
+                __func__, request->index, subidxstr, request->data_size);
         if (rec_size < 10) {
             EC_SLAVE_ERR(slave, "Incomplete abort command:\n");
             ec_print_data(data, rec_size);
         } else {
             fsm->request->abort_code = EC_READ_U32(data + 6);
             ec_canopen_abort_msg(slave, fsm->request->abort_code);
+            ec_print_data(data, rec_size);
         }
         return;
     }
@@ -2146,14 +2148,15 @@ void ec_fsm_coe_up_response(
 
     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
             EC_READ_U8(data + 2) >> 5 == 0x4) { // abort SDO transfer request
-        EC_SLAVE_ERR(slave, "SDO upload 0x%04X:%02X aborted.\n",
-               request->index, request->subindex);
+        EC_SLAVE_ERR(slave, "%s SDO upload 0x%04X:%02X aborted.\n",
+                __func__, request->index, request->subindex);
         if (rec_size >= 10) {
             request->abort_code = EC_READ_U32(data + 6);
             ec_canopen_abort_msg(slave, request->abort_code);
         } else {
             EC_SLAVE_ERR(slave, "No abort message.\n");
         }
+        ec_print_data(data, rec_size);
         request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         return;
@@ -2161,9 +2164,11 @@ void ec_fsm_coe_up_response(
 
     if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
             EC_READ_U8(data + 2) >> 5 != 0x2) { // upload response
-        EC_SLAVE_ERR(slave, "Received unknown response while"
-                " uploading SDO 0x%04X:%02X.\n",
-                request->index, request->subindex);
+        EC_SLAVE_ERR(slave, "%s Received unknown response while"
+                " uploading SDO 0x%04X:%02X, SDO response %#hx,"
+                " upload response %#hhx.\n",
+                __func__, request->index, request->subindex,
+                EC_READ_U16(data) >> 12, EC_READ_U8 (data + 2) >> 5);
         ec_print_data(data, rec_size);
         request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
@@ -2443,10 +2448,11 @@ void ec_fsm_coe_up_seg_response(
 
     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
             EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
-        EC_SLAVE_ERR(slave, "SDO upload 0x%04X:%02X aborted.\n",
-               request->index, request->subindex);
+        EC_SLAVE_ERR(slave, "%s SDO upload 0x%04X:%02X aborted.\n",
+                __func__, request->index, request->subindex);
         request->abort_code = EC_READ_U32(data + 6);
         ec_canopen_abort_msg(slave, request->abort_code);
+        ec_print_data(data, rec_size);
         request->errno = EIO;
         fsm->state = ec_fsm_coe_error;
         return;
diff --git a/master/slave.c b/master/slave.c
index 880b41d555e499475049f5e2cc4bf60e9b5e40f3..3347740292596fa05abd06320068b4cefefe222b 100644
--- a/master/slave.c
+++ b/master/slave.c
@@ -541,8 +541,11 @@ int ec_slave_fetch_sii_pdos(
 
         data_size -= 8;
         data += 8;
-        EC_SLAVE_DBG(slave, 1, "%s(j %u, entry_count %u, pdo name: %s)\n",
-            __func__, j, entry_count, pdo->name);
+        EC_SLAVE_DBG(slave, 1, 
+            "%s(j %u, pdo->index 0x%x, entry_count %u, pdo->sync_index %d, "
+            "data size %lu, pdo name: %s)\n",
+            __func__, j, pdo->index, entry_count, pdo->sync_index,
+            data_size, pdo->name);
         for (i = 0; i < entry_count; i++) {
             if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {
                 EC_SLAVE_ERR(slave, "Failed to allocate PDO entry memory.\n");
@@ -564,6 +567,11 @@ int ec_slave_fetch_sii_pdos(
 
             data_size -= 8;
             data += 8;
+            EC_SLAVE_DBG(slave, 1, 
+                "   %s(i %u, entry->index 0x%x, subindex %u, "
+                "bit_length %u, name: %s)\n",
+                __func__, i, entry->index, entry->subindex, entry->bit_length,
+                entry->name);
         }
 
         // if sync manager index is positive, the PDO is mapped by default