text: /*
   * This file is part of the MicroPython ESP32 project, https://github.com/loboris/MicroPython_ESP32_psRAM_LoBo
   *
   * The MIT License (MIT)
   *
   * Copyright (c) 2018 LoBo (https://github.com/loboris)
   *
   * Permission is hereby granted, free of charge, to any person obtaining a copy
   * of this software and associated documentation files (the "Software"), to deal
   * in the Software without restriction, including without limitation the rights
   * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   * copies of the Software, and to permit persons to whom the Software is
   * furnished to do so, subject to the following conditions:
   *
   * The above copyright notice and this permission notice shall be included in
   * all copies or substantial portions of the Software.
   *
   * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
   * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
   * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
   * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
   * THE SOFTWARE.
   */
  
  #include <stdio.h>
  #include <string.h>
  #include <sys/time.h>
  #include <time.h>
  
  #include "py/mpconfig.h"
  
  #include "sdkconfig.h"
  #include "esp_attr.h"
  #include "esp_log.h"
  #include "soc/cpu.h"
  
  #include "py/mpstate.h"
  #include "py/gc.h"
  #include "py/mpthread.h"
  #include "py/mphal.h"
  #include "mpthreadport.h"
  #include "modmachine.h"
  
  #if defined(CONFIG_MICROPY_USE_TELNET) || defined(CONFIG_MICROPY_USE_FTPSERVER)
  #include "libs/libGSM.h"
  #include "modnetwork.h"
  #include "tcpip_adapter.h"
  #include "esp_wifi_types.h"
  #include "esp_wifi.h"
  #endif
  
  #ifdef CONFIG_MICROPY_USE_TELNET
  #include "libs/telnet.h"
  
  extern TaskHandle_t TelnetTaskHandle;
  #endif
  
  #ifdef CONFIG_MICROPY_USE_FTPSERVER
  #include "libs/ftp.h"
  
  extern TaskHandle_t FtpTaskHandle;
  #endif
  
  extern int MainTaskCore;
  
  TaskHandle_t MainTaskHandle = NULL;
  TaskHandle_t ReplTaskHandle = NULL;
  
  uint8_t main_accept_msg = 1;
  
  // this structure forms a linked list, one node per active thread
  //========================
  typedef struct _thread_t {
      TaskHandle_t id;					// system id of thread
      int ready;							// whether the thread is ready and running
      void *arg;							// thread Python args, a GC root pointer
      void *stack;						// pointer to the stack
      void *curr_sp;						// current stack pointer
      void *stack_top;					// stack top
      StaticTask_t *tcb;     				// pointer to the Task Control Block
      size_t stack_len;      				// number of words in the stack
      void **ptrs;						// root pointers
      size_t ptrs_len;					// length of root pointers
      char name[THREAD_NAME_MAX_SIZE];	// thread name
      QueueHandle_t threadQueue;			// queue used for inter thread communication
      int8_t allow_suspend;
      int8_t suspended;
      int8_t waiting;
      int8_t deleted;
      int16_t notifyed;
      uint16_t type;
      struct _thread_t *next;
  } thread_t;
  
  // the mutex controls access to the linked list
  STATIC mp_thread_mutex_t thread_mutex;
  STATIC thread_t thread_entry0;
  STATIC thread_t *thread = NULL; // root pointer, handled by mp_thread_gc_others
  
  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  void vPortCleanUpTCB(void *tcb)
  {
  	if ((MainTaskHandle) && (thread)) {
  		thread_t *prev = NULL;
  		mp_thread_mutex_lock(&thread_mutex, 1);
  		for (thread_t *th = thread; th != NULL; prev = th, th = th->next) {
  			// unlink the node from the list
  			if (th->tcb == tcb) {
  				if (prev != NULL) {
  					prev->next = th->next;
  				} else {
  					// move the start pointer
  					thread = th->next;
  				}
  				// explicitly release all its memory
  				if (th->tcb) free(th->tcb);
  				if (th->stack) free(th->stack);
  				//m_del(thread_t, th, 1);
  				free(th);
  				break;
  			}
  		}
  		mp_thread_mutex_unlock(&thread_mutex);
  	}
  }
  
  
  // === Initialize the main MicroPython thread ===
  //-----------------------------------------------------
  void mp_thread_preinit(void *stack, uint32_t stack_len)
  {
      // Initialize threads mutex
      mp_thread_mutex_init(&thread_mutex);
  
      mp_thread_set_state(&mp_state_ctx.thread);
      // create first entry in linked list of all threads
      thread = &thread_entry0;
      thread->id = xTaskGetCurrentTaskHandle();
      thread->ready = 1;
      thread->arg = NULL;
      thread->stack = stack;
      thread->curr_sp = stack+stack_len;
      thread->stack_top = stack+stack_len;
      thread->stack_len = stack_len;
      sprintf(thread->name, "MainThread");
      thread->threadQueue = xQueueCreate( THREAD_QUEUE_MAX_ITEMS, sizeof(thread_msg_t) );
      thread->allow_suspend = 0;
      thread->suspended = 0;
      thread->waiting = 0;
      thread->deleted = 0;
      thread->notifyed = 0;
      thread->type = THREAD_TYPE_MAIN;
      thread->next = NULL;
      MainTaskHandle = thread->id;
  
  }
  
  //----------------------------------
  void mp_thread_gc_others(int flag) {
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          if (!th->ready) {
              continue;
          }
      	if (th->type == THREAD_TYPE_SERVICE) continue;
  
  		if (th->arg) {
  			#if MICROPY_PY_GC_COLLECT_RETVAL
  			n_marked = MP_STATE_MEM(gc_marked);
  			#endif
  			// Mark the pointers on thread arguments
  			gc_collect_root(&th->arg, 1);
  			#if MICROPY_PY_GC_COLLECT_RETVAL
  			if (flag) printf("th_collect:    marked on arg: %d (%s)\n", MP_STATE_MEM(gc_marked) - n_marked, th->name);
  			#endif
  		}
  
  		if (th->id == xTaskGetCurrentTaskHandle()) continue;
  
  		#if MICROPY_PY_GC_COLLECT_RETVAL
  		n_marked = MP_STATE_MEM(gc_marked);
  		#endif
  		// Mark the thread root pointers
  	    gc_collect_root(th->ptrs, th->ptrs_len);
          //gc_collect_root((void**)&th, 1);
  		#if MICROPY_PY_GC_COLLECT_RETVAL
  		if (flag) printf("th_collect:   marked on thrd: %d (%s)\n", MP_STATE_MEM(gc_marked) - n_marked, th->name);
  		#endif
  
  		#if MICROPY_PY_GC_COLLECT_RETVAL
  		n_marked = MP_STATE_MEM(gc_marked);
  		#endif
  		// Mark the pointers on thread stack
  		gc_collect_root(th->curr_sp, (th->stack_top - th->curr_sp) / sizeof(uint32_t));
  
  		#if MICROPY_PY_GC_COLLECT_RETVAL
  		if (flag) printf("th_collect:  marked on stack: %d (%s) [%p - %p]\n",
  				MP_STATE_MEM(gc_marked) - n_marked, th->name, th->curr_sp, th->stack_top);
  		#endif
      }
      mp_thread_mutex_unlock(&thread_mutex);
  }
  
  //--------------------------------------------
  mp_state_thread_t *mp_thread_get_state(void) {
      return pvTaskGetThreadLocalStoragePointer(NULL, 1);
  }
  
  //-------------------------------------
  void mp_thread_set_state(void *state) {
      vTaskSetThreadLocalStoragePointer(NULL, 1, state);
  }
  
  //--------------------------
  void mp_thread_start(void) {
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          if (th->id == xTaskGetCurrentTaskHandle()) {
              th->ready = 1;
              break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
  }
  
  STATIC void *(*ext_thread_entry)(void*) = NULL;
  
  //-------------------------------------
  STATIC void freertos_entry(void *arg) {
      if (ext_thread_entry) {
          ext_thread_entry(arg);
      }
      vTaskDelete(NULL);
  }
  
  //---------------------------------------------------------------------------------------------------------------------------------
  TaskHandle_t mp_thread_create_ex(void *(*entry)(void*), void *arg, size_t *in_stack_size, int priority, char *name, bool same_core)
  {
  	size_t stack_size = *in_stack_size;
  	bool is_repl = (strcmp(name, "REPLthread") == 0);
  	if (is_repl) {
  		if (ReplTaskHandle) {
  	        nlr_raise(mp_obj_new_exception_msg(&mp_type_OSError, "REPL thread already started"));
  		}
  	}
  	// store thread entry function into a global variable so we can access it
      ext_thread_entry = entry;
  
      // Check thread stack size
      if (stack_size == 0) {
      	stack_size = MP_THREAD_DEFAULT_STACK_SIZE; //use default stack size
      }
      else {
          if (stack_size < MP_THREAD_MIN_STACK_SIZE) stack_size = MP_THREAD_MIN_STACK_SIZE;
          else if (stack_size > MP_THREAD_MAX_STACK_SIZE) stack_size = MP_THREAD_MAX_STACK_SIZE;
      }
      stack_size &= 0x7FFFFFF8;
  
      // === Allocate TCB, stack and linked-list node ===
      StaticTask_t *tcb = NULL;
      StackType_t *stack = NULL;
      thread_t *th = NULL;
  
  	if (mpy_use_spiram) tcb = heap_caps_malloc(sizeof(StaticTask_t), MALLOC_CAP_8BIT | MALLOC_CAP_INTERNAL);
  	else tcb = malloc(sizeof(StaticTask_t));
      if (tcb == NULL) {
          nlr_raise(mp_obj_new_exception_msg(&mp_type_OSError, "can't create thread (tcb)"));
      }
  
  	if (mpy_use_spiram) stack = heap_caps_malloc((stack_size * sizeof(StackType_t))+8, MALLOC_CAP_8BIT | MALLOC_CAP_INTERNAL);
  	else stack = malloc((stack_size * sizeof(StackType_t))+8);;
      if (stack == NULL) {
      	free(tcb);
          nlr_raise(mp_obj_new_exception_msg(&mp_type_OSError, "can't create thread (stack)"));
      }
  
  	if (mpy_use_spiram) th = heap_caps_malloc(sizeof(thread_t), MALLOC_CAP_8BIT | MALLOC_CAP_INTERNAL);
  	else th = malloc(sizeof(thread_t));
      if (th == NULL) {
      	free(stack);
      	free(tcb);
          nlr_raise(mp_obj_new_exception_msg(&mp_type_OSError, "can't create thread (th)"));
      }
      memset(th, 0, sizeof(thread_t));
  
      mp_thread_mutex_lock(&thread_mutex, 1);
  
      // adjust the stack_size to provide room to recover from hitting the limit
      //stack_size -= 1024;
  
      // add thread to linked list of all threads
      th->ready = 0;
      th->arg = arg;
      th->stack = stack;
      th->curr_sp = stack+stack_size;
      th->stack_top = stack+stack_size;
      th->tcb = tcb;
      th->stack_len = stack_size;
      th->next = thread;
      snprintf(th->name, THREAD_NAME_MAX_SIZE, name);
      th->threadQueue = xQueueCreate( THREAD_QUEUE_MAX_ITEMS, sizeof(thread_msg_t) );
      th->allow_suspend = 0;
      th->suspended = 0;
      th->waiting = 0;
      th->deleted = 0;
      th->notifyed = 0;
      if (is_repl) th->type = THREAD_TYPE_REPL;
      else th->type = THREAD_TYPE_PYTHON;
      thread = th;
  
      // === Create and start the thread task ===
  	#if CONFIG_MICROPY_USE_BOTH_CORES
      	TaskHandle_t id = xTaskCreateStatic(freertos_entry, name, stack_size, arg, priority, stack, tcb);
  	#else
  		int run_on_core = MainTaskCore;
  		#if !CONFIG_FREERTOS_UNICORE
  		if (!same_core) run_on_core = MainTaskCore ^ 1;
  		#endif
  		TaskHandle_t id = xTaskCreateStaticPinnedToCore(freertos_entry, name, stack_size, arg, priority, stack, tcb, run_on_core);
  	#endif
      if (id == NULL) {
      	// Task not started, restore previous thread and clean-up
      	thread = th->next;
  		if (th->threadQueue) vQueueDelete(th->threadQueue);
      	free(th);
      	free(stack);
      	free(tcb);
          mp_thread_mutex_unlock(&thread_mutex);
          nlr_raise(mp_obj_new_exception_msg(&mp_type_OSError, "can't create thread"));
      }
  
      th->id = id;
  	if (is_repl) ReplTaskHandle = id;
  
  	mp_thread_mutex_unlock(&thread_mutex);
      *in_stack_size = stack_size;
      return id;
  }
  
  //--------------------------------------------------------------------------------------------------------
  void *mp_thread_create(void *(*entry)(void*), void *arg, size_t *stack_size, char *name, bool same_core) {
      return mp_thread_create_ex(entry, arg, stack_size, MP_THREAD_PRIORITY, name, same_core);
  }
  
  //---------------------------------------
  STATIC void mp_clean_thread(thread_t *th)
  {
  	if (th->threadQueue) {
  		int n = 1;
  		while (n > 0) {
  			n = uxQueueMessagesWaiting(th->threadQueue);
  			if (n > 0) {
  				thread_msg_t msg;
  				xQueueReceive(th->threadQueue, &msg, 0);
  				if (msg.strdata != NULL) free(msg.strdata);
  			}
  		}
  		if (th->threadQueue) vQueueDelete(th->threadQueue);
  		th->threadQueue = NULL;
  	}
      th->ready = 0;
  	th->deleted = 1;
  }
  
  //---------------------------
  void mp_thread_finish(void) {
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          if (th->id == xTaskGetCurrentTaskHandle()) {
          	mp_clean_thread(th);
              break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
  }
  
  //---------------------------------------------------
  void mp_thread_mutex_init(mp_thread_mutex_t *mutex) {
      mutex->handle = xSemaphoreCreateMutexStatic(&mutex->buffer);
  }
  
  //------------------------------------------------------------
  int mp_thread_mutex_lock(mp_thread_mutex_t *mutex, int wait) {
      return (pdTRUE == xSemaphoreTake(mutex->handle, wait ? portMAX_DELAY : 0));
  }
  
  //-----------------------------------------------------
  void mp_thread_mutex_unlock(mp_thread_mutex_t *mutex) {
      xSemaphoreGive(mutex->handle);
  }
  
  //--------------------------------------
  void mp_thread_allowsuspend(int allow) {
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          // don't allow suspending main task task
          if ((th->id != MainTaskHandle) && (th->id == xTaskGetCurrentTaskHandle())) {
          	th->allow_suspend = allow & 1;
              break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
  }
  
  //--------------------------------------
  int mp_thread_suspend(TaskHandle_t id) {
  	int res = 0;
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          // don't suspend the current task
          if (th->id == xTaskGetCurrentTaskHandle()) {
              continue;
          }
          if (th->id == id) {
          	if ((th->allow_suspend) && (th->suspended == 0) && (th->waiting == 0)) {
          		th->suspended = 1;
          		vTaskSuspend(th->id);
          		res = 1;
          	}
              break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
      return res;
  }
  
  //-------------------------------------
  int mp_thread_resume(TaskHandle_t id) {
  	int res = 0;
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          // don't resume the current task
          if (th->id == xTaskGetCurrentTaskHandle()) {
              continue;
          }
          if (th->id == id) {
          	if ((th->allow_suspend) && (th->suspended) && (th->waiting == 0)) {
          		th->suspended = 0;
          		vTaskResume(th->id);
          		res = 1;
          	}
              break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
      return res;
  }
  
  //--------------------------
  int mp_thread_setblocked() {
  	int res = 0;
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          if (th->id == xTaskGetCurrentTaskHandle()) {
  			th->waiting = 1;
  			res = 1;
              break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
      return res;
  }
  
  //-----------------------------------------
  int mp_thread_set_sp(void *sp, void *top) {
  	int res = 0;
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          if (th->id == xTaskGetCurrentTaskHandle()) {
  			th->curr_sp = sp;
  			th->stack_top = top;
  			res = 1;
              break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
      return res;
  }
  
  //--------------------------
  int mp_thread_get_sp(void) {
  	int res = 0;
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          if (th->id == xTaskGetCurrentTaskHandle()) {
  			res = (uintptr_t)th->curr_sp;
              break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
      return res;
  }
  
  //------------------------------------------------
  int mp_thread_set_ptrs(void **ptrs, size_t size) {
  	int res = 0;
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          if (th->id == xTaskGetCurrentTaskHandle()) {
  			th->ptrs = ptrs;
  			th->ptrs_len = size;
  			res = 1;
              break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
      return res;
  }
  
  //-----------------------------
  int mp_thread_setnotblocked() {
  	int res = 0;
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          if (th->id == xTaskGetCurrentTaskHandle()) {
          	th->waiting = 0;
          	res = 1;
              break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
      return res;
  }
  
  // Send notification to thread 'id'
  // or to all threads if id=0
  //-----------------------------------------------------
  int mp_thread_notify(TaskHandle_t id, uint32_t value) {
  	int res = 0;
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          if ( (th->id != xTaskGetCurrentTaskHandle()) && ( (id == 0) || (th->id == id) ) ) {
          	res = xTaskNotify(th->id, value, eSetValueWithOverwrite); //eSetValueWithoutOverwrite
          	th->notifyed = 1;
              if (id != 0) break;
          }
      }
      if (id == 0) res = 1;
      mp_thread_mutex_unlock(&thread_mutex);
      return res;
  }
  
  //---------------------------
  int mp_thread_num_threads() {
  	int res = 0;
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          if (th->id != xTaskGetCurrentTaskHandle()) res++;
      }
      mp_thread_mutex_unlock(&thread_mutex);
      return res;
  }
  
  //---------------------------------------------
  uint32_t mp_thread_getnotify(bool check_only) {
  	uint32_t value = 0;
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          if (th->id == xTaskGetCurrentTaskHandle()) {
          	xTaskNotifyWait(0, 0, &value, 0);
        		if (!check_only) {
        			xTaskNotifyWait(ULONG_MAX, ULONG_MAX, NULL, 0);
            		th->notifyed = 0;
        		}
  			break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
      return value;
  }
  
  //--------------------------------------------
  int mp_thread_notifyPending(TaskHandle_t id) {
  	int res = -1;
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          if (th->id == id) {
          	res = th->notifyed;
              break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
      return res;
  }
  
  //-----------------------------
  void mp_thread_resetPending() {
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          if (th->id == xTaskGetCurrentTaskHandle()) {
          	th->notifyed = 0;
              break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
  }
  
  //------------------------------
  uint32_t mp_thread_getSelfID() {
  	uint32_t id = 0;
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          if (th->id == xTaskGetCurrentTaskHandle()) {
          	id = (uint32_t)th->id;
  			break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
      return id;
  }
  
  //-------------------------------------
  int mp_thread_getSelfname(char *name) {
  	int res = 0;
  	name[0] = '?';
  	name[1] = '\0';
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          if (th->id == xTaskGetCurrentTaskHandle()) {
          	sprintf(name, th->name);
          	res = 1;
  			break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
      return res;
  }
  
  //--------------------------------------------------
  int mp_thread_getname(TaskHandle_t id, char *name) {
  	int res = 0;
  	name[0] = '?';
  	name[1] = '\0';
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          if (th->id == id) {
          	sprintf(name, th->name);
          	res = 1;
  			break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
      return res;
  }
  
  //-------------------------------------------------------------------------------------------------
  int mp_thread_semdmsg(TaskHandle_t id, int type, uint32_t msg_int, uint8_t *buf, uint32_t buflen) {
  	int res = 0;
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          // don't send to the current task or service thread
          if ((th->id == xTaskGetCurrentTaskHandle()) || (th->type == THREAD_TYPE_SERVICE)) {
              continue;
          }
          if ((id == 0) || (th->id == id)) {
          	if (th->threadQueue == NULL) break;
      		thread_msg_t msg;
      	    struct timeval tv;
      	    uint64_t tmstamp;
      	    gettimeofday(&tv, NULL);
      	    tmstamp = (tv.tv_sec * 1000) + (tv.tv_usec / 1000);
      	    msg.timestamp = tmstamp;
  			msg.sender_id = xTaskGetCurrentTaskHandle();
  			if (type == THREAD_MSG_TYPE_INTEGER) {
  				msg.intdata = msg_int;
  				msg.strdata = NULL;
  				msg.type = type;
  				res = 1;
  			}
  			else if (type == THREAD_MSG_TYPE_STRING) {
  				msg.intdata = buflen;
  				msg.strdata = malloc(buflen+1);
  				if (msg.strdata != NULL) {
  					memcpy(msg.strdata, buf, buflen);
  					msg.strdata[buflen] = 0;
  					msg.type = type;
  					res = 1;
  				}
  			}
  			if (res) {
  				if (xQueueSend(th->threadQueue, &msg, 0) != pdTRUE) res = 0;
  			}
              if (id != 0) break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
      return res;
  }
  
  //------------------------------------------------------------------------------------------
  int mp_thread_getmsg(uint32_t *msg_int, uint8_t **buf, uint32_t *buflen, uint32_t *sender) {
  	int res = 0;
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          // get message for current task
          if ((th->id == xTaskGetCurrentTaskHandle()) && (th->type != THREAD_TYPE_SERVICE)) {
          	if (th->threadQueue == NULL) break;
  
          	thread_msg_t msg;
          	if (xQueueReceive(th->threadQueue, &msg, 0) == pdTRUE) {
          		*sender = (uint32_t)msg.sender_id;
          		if (msg.type == THREAD_MSG_TYPE_INTEGER) {
          			*msg_int = msg.intdata;
          			*buflen = 0;
          			res = THREAD_MSG_TYPE_INTEGER;
          		}
          		else if (msg.type == THREAD_MSG_TYPE_STRING) {
          			*msg_int = 0;
          			if ((msg.strdata != NULL) && (msg.intdata > 0)) {
              			*buflen = msg.intdata;
              			*buf = msg.strdata;
              			res = THREAD_MSG_TYPE_STRING;
          			}
          		}
          	}
          	break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
  
      return res;
  }
  
  //-------------------------------------
  int mp_thread_status(TaskHandle_t id) {
  	int res = -1;
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
          if ((th->id == xTaskGetCurrentTaskHandle()) || (th->type == THREAD_TYPE_SERVICE)) {
              continue;
          }
          if (th->id == id) {
  			if (!th->deleted) {
  				if (th->suspended) res = 1;
  				else if (th->waiting) res = 2;
  				else res = 0;
  			}
  			break;
          }
      }
      mp_thread_mutex_unlock(&thread_mutex);
      return res;
  }
  
  //---------------------------------------
  int mp_thread_list(thread_list_t *list) {
  	int num = 0;
  
  
      mp_thread_mutex_lock(&thread_mutex, 1);
  
      for (thread_t *th = thread; th != NULL; th = th->next) {
      	num++;
      }
      if ((num == 0) || (list == NULL)) {
          mp_thread_mutex_unlock(&thread_mutex);
      	return num;
      }
  
  	list->nth = num;
  	list->threads = malloc(sizeof(threadlistitem_t) * (num+1));
  	if (list->threads == NULL) num = 0;
  	else {
  		int nth = 0;
  		threadlistitem_t *thr = NULL;
  		uint32_t min_stack;
  		for (thread_t *th = thread; th != NULL; th = th->next) {
  			thr = list->threads + (sizeof(threadlistitem_t) * nth);
  	        if (th->id == xTaskGetCurrentTaskHandle()) min_stack = uxTaskGetStackHighWaterMark(NULL);
  	        else min_stack = uxTaskGetStackHighWaterMark(th->id);
  
  			thr->id = (uint32_t)th->id;
  			sprintf(thr->name, "%s", th->name);
  			thr->suspended = th->suspended;
  			thr->waiting = th->waiting;
  			thr->type = th->type;
  			thr->stack_len = th->stack_len;
  			thr->stack_max = th->stack_len - min_stack;
  			nth++;
  			if (nth > num) break;
  		}
  		if (nth != num) {
  			free(list->threads);
  			list->threads = NULL;
  			num = 0;
  		}
      }
      mp_thread_mutex_unlock(&thread_mutex);
      return num;
  }
  
  //------------------------------------------
  int mp_thread_replAcceptMsg(int8_t accept) {
  	int res = main_accept_msg;
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
      	 if (th->id == xTaskGetCurrentTaskHandle()) {
      		 if ((th->id == ReplTaskHandle) && (accept >= 0)) {
      			 main_accept_msg = accept & 1;
      		 }
  			 break;
      	 }
      }
      mp_thread_mutex_unlock(&thread_mutex);
  
      return res;
  }
  
  //------------------------------------------
  int mp_thread_mainAcceptMsg(int8_t accept) {
  	int res = main_accept_msg;
      mp_thread_mutex_lock(&thread_mutex, 1);
      for (thread_t *th = thread; th != NULL; th = th->next) {
      	 if (th->id == xTaskGetCurrentTaskHandle()) {
      		 if ((th->id == MainTaskHandle) && (accept >= 0)) {
      			 main_accept_msg = accept & 1;
      		 }
  			 break;
      	 }
      }
      mp_thread_mutex_unlock(&thread_mutex);
  
      return res;
  }
  
  
  // ===== SERVICE THREADS ==============================================
  
  #if defined(CONFIG_MICROPY_USE_TELNET) || defined(CONFIG_MICROPY_USE_FTPSERVER)
  
  // Check if any network connection is available (WiFi STA and/or AP, ethernet)
  //-----------------------
  static bool _check_network()
  {
      uint32_t ip = network_hasip();
      if (ip == 0) {
          #ifdef CONFIG_MICROPY_USE_GSM
          //ToDo: should we enable telnet/ftp over GSM ?
          //if (ppposStatus(NULL, NULL, NULL) != GSM_STATE_CONNECTED) {
              return false;
          //}
          #else
          return false;
          #endif
      }
      return true;
  }
  #endif
  
  #ifdef CONFIG_MICROPY_USE_TELNET
  //===================================
  void telnet_task (void *pvParameters)
  {
      // Initialize telnet, create rx buffer and mutex
      telnet_init();
  
      // Check if network connection is available
      while (!_check_network()) {
          vTaskDelay(1000 / portTICK_PERIOD_MS);
          if (telnet_stop_requested()) goto exit;
      }
  
      // We have network connection, enable telnet
      telnet_enable();
  
      while (1) {
          int res = telnet_run();
          if ( res < 0) {
              if (res == -1) {
                  ESP_LOGD("[Telnet]", "\nRun Error");
              }
              // -2 is returned if Telnet stop was requested by user
              break;
          }
  
          vTaskDelay(1);
  
          // ---- Check if network is still available ----
          if (!_check_network()) {
              bool was_enabled = telnet_isenabled();
              telnet_disable();
              while (!_check_network()) {
                  vTaskDelay(200 / portTICK_PERIOD_MS);
                  if (telnet_stop_requested()) goto exit;
              }
              if (was_enabled) telnet_enable();
          }
          // ------------------------------------------
      }
  exit:
      telnet_disable();
      telnet_deinit();
      ESP_LOGD("[Telnet]", "\nTask terminated!");
      TelnetTaskHandle = NULL;
      vSemaphoreDelete(telnet_mutex);
      telnet_mutex = NULL;
      vTaskDelete(NULL);
  }
  
  //-----------------------------------------------------
  uintptr_t mp_thread_createTelnetTask(size_t stack_size)
  {
      if (TelnetTaskHandle == NULL) {
          telnet_stack_size = stack_size;
          #if CONFIG_MICROPY_USE_BOTH_CORES
          xTaskCreate(&telnet_task, "Telnet", stack_size, NULL, CONFIG_MICROPY_TASK_PRIORITY, &TelnetTaskHandle);
          #else
          xTaskCreatePinnedToCore(&telnet_task, "Telnet", stack_size, NULL, CONFIG_MICROPY_TASK_PRIORITY, &TelnetTaskHandle, MainTaskCore);
          #endif
      }
      return (uintptr_t)TelnetTaskHandle;
  }
  #endif
  
  
  #ifdef CONFIG_MICROPY_USE_FTPSERVER
  
  //================================
  void ftp_task (void *pvParameters)
  {
      uint64_t elapsed, time_ms = mp_hal_ticks_ms();
      // Initialize ftp, create rx buffer and mutex
      if (!ftp_init()) {
          ESP_LOGE("[Ftp]", "Init Error");
          goto exit1;
      }
  
      while (!_check_network()) {
          vTaskDelay(1000 / portTICK_PERIOD_MS);
          if (ftp_stop_requested()) goto exit;
      }
  
      // We have network connection, enable ftp
      ftp_enable();
  
      time_ms = mp_hal_ticks_ms();
      while (1) {
          // Calculate time between two ftp_run() calls
          elapsed = mp_hal_ticks_ms() - time_ms;
          time_ms = mp_hal_ticks_ms();
  
          int res = ftp_run(elapsed);
          if (res < 0) {
              if (res == -1) {
                  ESP_LOGD("[Ftp]", "\nRun Error");
              }
              // -2 is returned if Ftp stop was requested by user
              break;
          }
  
          vTaskDelay(1);
  
          // ---- Check if network is still available ----
          if (!_check_network()) {
              bool was_enabled = ftp_isenabled();
              ftp_disable();
              while (!_check_network()) {
                  vTaskDelay(200 / portTICK_PERIOD_MS);
                  if (ftp_stop_requested()) goto exit;
              }
              if (was_enabled) ftp_enable();
          }
          // ------------------------------------------
      }
  exit:
      ftp_disable();
      ftp_deinit();
  exit1:
      ESP_LOGD("[Ftp]", "\nTask terminated!");
      FtpTaskHandle = NULL;
      vSemaphoreDelete(ftp_mutex);
      ftp_mutex = NULL;
      vTaskDelete(NULL);
  }
  
  //--------------------------------------------------
  uintptr_t mp_thread_createFtpTask(size_t stack_size)
  {
      if (FtpTaskHandle == NULL) {
          ftp_stack_size = stack_size;
          #if CONFIG_MICROPY_USE_BOTH_CORES
          xTaskCreate(&ftp_task, "FtpServer", stack_size, NULL, CONFIG_MICROPY_TASK_PRIORITY, &FtpTaskHandle);
          #else
          xTaskCreatePinnedToCore(&ftp_task, "FtpServer", stack_size, NULL, CONFIG_MICROPY_TASK_PRIORITY, &FtpTaskHandle, MainTaskCore);
          #endif
      }
      return (uintptr_t)FtpTaskHandle;
  }
  #endif
  