diff --git a/src/EthernetClient.cpp b/src/EthernetClient.cpp
index f6aef07..f02c486 100644
--- a/src/EthernetClient.cpp
+++ b/src/EthernetClient.cpp
@@ -158,15 +158,10 @@ void EthernetClient::stop() {
     return;
   }
 
-  // Close tcp connection. If failed, force to close connection.
-  if(ERR_OK != tcp_close(_tcp_client->pcb)) {
-    tcp_abort(_tcp_client->pcb);
+  // close tcp connection if not closed yet
+  if(status() != TCP_CLOSING) {
+    tcp_connection_close(_tcp_client->pcb, _tcp_client);
   }
-
-  _tcp_client->pcb = NULL;
-
-   mem_free(_tcp_client);
-  _tcp_client = NULL;
 }
 
 uint8_t EthernetClient::connected() {
diff --git a/src/utility/stm32_eth.c b/src/utility/stm32_eth.c
index 3de5aeb..edd28fe 100644
--- a/src/utility/stm32_eth.c
+++ b/src/utility/stm32_eth.c
@@ -36,6 +36,7 @@
   ******************************************************************************
   */
 
+#include "Arduino.h"
 #include "stm32_eth.h"
 #include "lwip/init.h"
 #include "lwip/netif.h"
@@ -46,16 +47,6 @@
 #include "lwip/prot/dhcp.h"
 #include "lwip/dns.h"
 
-//Keeps compatibilty with older version of the STM32 core
-#if __has_include("core_callback.h")
-#include "core_callback.h"
-#else
-void registerCoreCallback(void (*func)(void)) {
-  UNUSED(func);
-}
-#endif
-
-
 #ifdef __cplusplus
  extern "C" {
 #endif
@@ -69,6 +60,11 @@ void registerCoreCallback(void (*func)(void)) {
 /* Maximum number of retries for DHCP request */
 #define MAX_DHCP_TRIES  4
 
+/* Timer used to call the scheduler */
+#ifndef DEFAULT_ETHERNET_TIMER
+#define DEFAULT_ETHERNET_TIMER  TIM14
+#endif
+
 /* Ethernet configuration: user parameters */
 struct stm32_eth_config {
   ip_addr_t ipaddr;
@@ -94,13 +90,16 @@ static uint8_t DHCP_Started_by_user = 0;
 /* Ethernet link status periodic timer */
 static uint32_t gEhtLinkTickStart = 0;
 
+/* Handler for stimer */
+static stimer_t TimHandle;
+
 /*************************** Function prototype *******************************/
 static void Netif_Config(void);
-static void tcp_connection_close(struct tcp_pcb *tpcb, struct tcp_struct *tcp);
 static err_t tcp_recv_callback(void *arg, struct tcp_pcb *tpcb, struct pbuf *p, err_t err);
 static err_t tcp_sent_callback(void *arg, struct tcp_pcb *tpcb, u16_t len);
 static void tcp_err_callback(void *arg, err_t err);
-
+static void scheduler_callback(stimer_t *htim);
+static void TIM_scheduler_Config(void);
 
 /**
 * @brief  Configurates the network interface
@@ -132,54 +131,90 @@ static void Netif_Config(void)
 #endif /* LWIP_NETIF_LINK_CALLBACK */
 }
 
+/**
+* @brief  Scheduler callback. Call by a timer interrupt.
+* @param  htim: pointer to stimer_t
+* @retval None
+*/
+static void scheduler_callback(stimer_t *htim)
+{
+  UNUSED(htim);
+  stm32_eth_scheduler();
+}
+
+/**
+* @brief  Enable the timer used to call ethernet scheduler function at regular
+*         interval.
+* @param  None
+* @retval None
+*/
+static void TIM_scheduler_Config(void)
+{
+  /* Set TIMx instance. */
+  TimHandle.timer = DEFAULT_ETHERNET_TIMER;
+
+  /* Timer set to 1ms */
+  TimerHandleInit(&TimHandle, (uint16_t)(1000 - 1), ((uint32_t)(getTimerClkFreq(DEFAULT_ETHERNET_TIMER) / (1000000)) - 1));
+
+  attachIntHandle(&TimHandle, scheduler_callback);
+}
+
 void stm32_eth_init(const uint8_t *mac, const uint8_t *ip, const uint8_t *gw, const uint8_t *netmask)
 {
-  /* Initialize the LwIP stack */
-  lwip_init();
+  static uint8_t initDone = 0;
+
+  if(!initDone) {
+    /* Initialize the LwIP stack */
+    lwip_init();
+
+    if(mac != NULL) {
+      ethernetif_set_mac_addr(mac);
+    } // else default value is used: MAC_ADDR0 ... MAC_ADDR5
+
+    if(ip != NULL) {
+      IP_ADDR4(&(gconfig.ipaddr),ip[0],ip[1],ip[2],ip[3]);
+    } else {
+      #if LWIP_DHCP
+        ip_addr_set_zero_ip4(&(gconfig.ipaddr));
+      #else
+        IP_ADDR4(&(gconfig.ipaddr),IP_ADDR0,IP_ADDR1,IP_ADDR2,IP_ADDR3);
+      #endif /* LWIP_DHCP */
+    }
 
-  if(mac != NULL) {
-    ethernetif_set_mac_addr(mac);
-  } // else default value is used: MAC_ADDR0 ... MAC_ADDR5
+    if(gw != NULL) {
+      IP_ADDR4(&(gconfig.gw),gw[0],gw[1],gw[2],gw[3]);
+    } else {
+      #if LWIP_DHCP
+        ip_addr_set_zero_ip4(&(gconfig.gw));
+      #else
+        IP_ADDR4(&(gconfig.gw),GW_ADDR0,GW_ADDR1,GW_ADDR2,GW_ADDR3);
+      #endif /* LWIP_DHCP */
+    }
 
-  if(ip != NULL) {
-    IP_ADDR4(&(gconfig.ipaddr),ip[0],ip[1],ip[2],ip[3]);
-  } else {
-    #if LWIP_DHCP
-      ip_addr_set_zero_ip4(&(gconfig.ipaddr));
-    #else
-      IP_ADDR4(&(gconfig.ipaddr),IP_ADDR0,IP_ADDR1,IP_ADDR2,IP_ADDR3);
-    #endif /* LWIP_DHCP */
-  }
+    if(netmask != NULL) {
+      IP_ADDR4(&(gconfig.netmask),netmask[0],netmask[1],netmask[2],netmask[3]);
+    } else {
+      #if LWIP_DHCP
+        ip_addr_set_zero_ip4(&(gconfig.netmask));
+      #else
+        IP_ADDR4(&(gconfig.netmask),NETMASK_ADDR0,NETMASK_ADDR1,NETMASK_ADDR2,NETMASK_ADDR3);
+      #endif /* LWIP_DHCP */
+    }
 
-  if(gw != NULL) {
-    IP_ADDR4(&(gconfig.gw),gw[0],gw[1],gw[2],gw[3]);
-  } else {
-    #if LWIP_DHCP
-      ip_addr_set_zero_ip4(&(gconfig.gw));
-    #else
-      IP_ADDR4(&(gconfig.gw),GW_ADDR0,GW_ADDR1,GW_ADDR2,GW_ADDR3);
-    #endif /* LWIP_DHCP */
-  }
+    /* Configure the Network interface */
+    Netif_Config();
 
-  if(netmask != NULL) {
-    IP_ADDR4(&(gconfig.netmask),netmask[0],netmask[1],netmask[2],netmask[3]);
-  } else {
-    #if LWIP_DHCP
-      ip_addr_set_zero_ip4(&(gconfig.netmask));
-    #else
-      IP_ADDR4(&(gconfig.netmask),NETMASK_ADDR0,NETMASK_ADDR1,NETMASK_ADDR2,NETMASK_ADDR3);
-    #endif /* LWIP_DHCP */
-  }
+    // stm32_eth_scheduler() will be called every 1ms.
+    TIM_scheduler_Config();
 
-  /* Configure the Network interface */
-  Netif_Config();
+    initDone = 1;
+  }
 
+  /* Reset DHCP if used */
   User_notification(&gnetif);
 
+  /* Update LwIP stack */
   stm32_eth_scheduler();
-
-  // stm32_eth_scheduler() will be called directly inside the loop of the main() function.
-  registerCoreCallback(stm32_eth_scheduler);
 }
 
 /**
@@ -954,7 +989,7 @@ static void tcp_err_callback(void *arg, err_t err)
   * @param es: pointer on echoclient structure
   * @retval None
   */
-static void tcp_connection_close(struct tcp_pcb *tpcb, struct tcp_struct *tcp)
+void tcp_connection_close(struct tcp_pcb *tpcb, struct tcp_struct *tcp)
 {
   /* remove callbacks */
   tcp_recv(tpcb, NULL);
diff --git a/src/utility/stm32_eth.h b/src/utility/stm32_eth.h
index 2dd7257..bf1bb28 100644
--- a/src/utility/stm32_eth.h
+++ b/src/utility/stm32_eth.h
@@ -167,6 +167,7 @@ uint32_t ip_addr_to_u32(ip_addr_t *ipaddr);
 #if LWIP_TCP
 err_t tcp_connected_callback(void *arg, struct tcp_pcb *tpcb, err_t err);
 err_t tcp_accept_callback(void *arg, struct tcp_pcb *newpcb, err_t err);
+void tcp_connection_close(struct tcp_pcb *tpcb, struct tcp_struct *tcp);
 #else
 #error "LWIP_TCP must be enabled in lwipopts.h"
 #endif