TeensyThreads.cpp

/*
 * Threads.cpp - Library for threading on the Teensy.
 *
 *******************
 * 
 * Copyright 2017 by Fernando Trias.
 * 
 * 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 "TeensyThreads.h"
#include <Arduino.h>
#include <string.h>

#ifndef __IMXRT1062__

#include <IntervalTimer.h>
IntervalTimer context_timer;

#endif

Threads threads;

unsigned int time_start;
unsigned int time_end;

#define __flush_cpu() __asm__ volatile("DMB");

// These variables are used by the assembly context_switch() function.
// They are copies or pointers to data in Threads and ThreadInfo
// and put here seperately in order to simplify the code.
extern "C" {
  int currentUseSystick;      // using Systick vs PIT/GPT
  int currentActive;          // state of the system (first, start, stop)
  int currentCount;
  ThreadInfo *currentThread;  // the thread currently running
  void *currentSave;
  int currentMSP;             // Stack pointers to save
  void *currentSP;
  void loadNextThread() {
    threads.getNextThread();
  }
}

const int overflow_stack_size = 8;

extern "C" void stack_overflow_default_isr() { 
  currentThread->flags = Threads::ENDED;
}
extern "C" void stack_overflow_isr(void)       __attribute__ ((weak, alias("stack_overflow_default_isr")));

extern unsigned long _estack;   // the main thread 0 stack

// static void threads_svcall_isr(void);
// static void threads_systick_isr(void);

IsrFunction Threads::save_systick_isr;
IsrFunction Threads::save_svcall_isr;

/*
 * Teensy 3:
 * Replace the SysTick interrupt for our context switching. Note that
 * this function is "naked" meaning it does not save it's registers
 * on the stack. This is so we can preserve the stack of the caller.
 *
 * Interrupts will save r0-r4 in the stack and since this function
 * is short and simple, it should only use those registers. In the
 * future, this should be coded in assembly to make sure.
 */
extern volatile uint32_t systick_millis_count;
extern "C" void systick_isr();
void __attribute((naked, noinline)) threads_systick_isr(void)
{
  if (Threads::save_systick_isr) {
    asm volatile("push {r0-r4,lr}");
    (*Threads::save_systick_isr)();
    asm volatile("pop {r0-r4,lr}");
  }

  // TODO: Teensyduino 1.38 calls MillisTimer::runFromTimer() from SysTick
  if (currentUseSystick) {
    // we branch in order to preserve LR and the stack
    __asm volatile("b context_switch");
  }
  __asm volatile("bx lr");
}

void __attribute((naked, noinline)) threads_svcall_isr(void)
{
  if (Threads::save_svcall_isr) {
    asm volatile("push {r0-r4,lr}");
    (*Threads::save_svcall_isr)();
    asm volatile("pop {r0-r4,lr}");
  }

  // Get the right stack so we can extract the PC (next instruction)
  // and then see the SVC calling instruction number
  __asm volatile("TST lr, #4 \n"
                 "ITE EQ \n"
                 "MRSEQ r0, msp \n"
                 "MRSNE r0, psp \n");
  register unsigned int *rsp __asm("r0");
  unsigned int svc = ((uint8_t*)rsp[6])[-2];
  if (svc == Threads::SVC_NUMBER) {
    __asm volatile("b context_switch_direct");
  }
  else if (svc == Threads::SVC_NUMBER_ACTIVE) {
    currentActive = Threads::STARTED;
    __asm volatile("b context_switch_direct_active");
  }
  __asm volatile("bx lr");
}

#ifdef __IMXRT1062__

/*
 * 
 * Teensy 4:
 * Use unused GPT timers for context switching
 */

extern "C" void unused_interrupt_vector(void);

static void __attribute((naked, noinline)) gpt1_isr() {
  GPT1_SR |= GPT_SR_OF1;  // clear set bit
  __asm volatile ("dsb"); // see github bug #20 by manitou48
  __asm volatile("b context_switch");
}

static void __attribute((naked, noinline)) gpt2_isr() {
  GPT2_SR |= GPT_SR_OF1;  // clear set bit
  __asm volatile ("dsb"); // see github bug #20 by manitou48
  __asm volatile("b context_switch");
}

bool gtp1_init(unsigned int microseconds)
{
  // Initialization code derived from @manitou48.
  // See https://github.com/manitou48/teensy4/blob/master/gpt_isr.ino
  // See https://forum.pjrc.com/threads/54265-Teensy-4-testing-mbed-NXP-MXRT1050-EVKB-(600-Mhz-M7)?p=193217&viewfull=1#post193217

  // keep track of which GPT timer we are using
  static int gpt_number = 0;

  // not configured yet, so find an inactive GPT timer
  if (gpt_number == 0) {
    if (! NVIC_IS_ENABLED(IRQ_GPT1)) {
      attachInterruptVector(IRQ_GPT1, &gpt1_isr);
      NVIC_SET_PRIORITY(IRQ_GPT1, 255);
      NVIC_ENABLE_IRQ(IRQ_GPT1);
      gpt_number = 1;
    }
    else if (! NVIC_IS_ENABLED(IRQ_GPT2)) {
      attachInterruptVector(IRQ_GPT2, &gpt2_isr);
      NVIC_SET_PRIORITY(IRQ_GPT2, 255);
      NVIC_ENABLE_IRQ(IRQ_GPT2);
      gpt_number = 2;
    }
    else {
      // if neither timer is free, we fail
      return false;
    }
  }

  switch (gpt_number) {
    case 1:
      CCM_CCGR1 |= CCM_CCGR1_GPT1_BUS(CCM_CCGR_ON) ;  // enable GPT1 module
      GPT1_CR = 0;                   // disable timer
      GPT1_PR = 23;                  // prescale: divide by 24 so 1 tick = 1 microsecond at 24MHz
      GPT1_OCR1 = microseconds - 1;  // compare value
      GPT1_SR = 0x3F;                // clear all prior status
      GPT1_IR = GPT_IR_OF1IE;        // use first timer
      GPT1_CR = GPT_CR_EN | GPT_CR_CLKSRC(1) ; // set to peripheral clock (24MHz)
      break;
    case 2:
      CCM_CCGR1 |= CCM_CCGR1_GPT1_BUS(CCM_CCGR_ON) ;  // enable GPT1 module
      GPT2_CR = 0;                   // disable timer
      GPT2_PR = 23;                  // prescale: divide by 24 so 1 tick = 1 microsecond at 24MHz
      GPT2_OCR1 = microseconds - 1;  // compare value
      GPT2_SR = 0x3F;                // clear all prior status
      GPT2_IR = GPT_IR_OF1IE;        // use first timer
      GPT2_CR = GPT_CR_EN | GPT_CR_CLKSRC(1) ; // set to peripheral clock (24MHz)
      break;
    default:
      return false;
  }

  return true;
}

#endif

/*************************************************/
/**\name UTILITIES FUNCTIONS                     */
/*************************************************/
/**
 * \brief Convert thead state to printable string
 */
char * _util_state_2_string(int state){
    static char _state[Threads::UTIL_STATE_NAME_DESCRIPTION_LENGTH];
    memset(_state, 0, sizeof(_state));

    switch (state)
    {
    case 0:
        sprintf(_state, "EMPTY");
        break;
    case 1:
        sprintf(_state, "RUNNING");
        break;
    case 2:
        sprintf(_state, "ENDED");
        break;
    case 3:
        sprintf(_state, "ENDING");
        break;
    case 4:
        sprintf(_state, "SUSPENDED");
        break;
    default:
        sprintf(_state, "%d", state);
        break;
    }
    
    return _state;
}

/*************************************************/
/**\name CLASS THREAD                            */
/*************************************************/
Threads::Threads() : current_thread(0), thread_count(0), thread_error(0) {
  // initilize thread slots to empty
  for(int i=1; i<MAX_THREADS; i++) {
    threadp[i] = NULL;
  }
  // fill thread 0, which is always running
  threadp[0] = new ThreadInfo();

  // initialize context_switch() globals from thread 0, which is MSP and always running
  currentThread = threadp[0];        // thread 0 is active
  currentSave = &threadp[0]->save;
  currentMSP = 1;
  currentSP = 0;
  currentCount = Threads::DEFAULT_TICKS;
  currentActive = FIRST_RUN;
  threadp[0]->flags = RUNNING;
  threadp[0]->ticks = DEFAULT_TICKS;
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Warray-bounds"
  threadp[0]->stack = (uint8_t*)&_estack - DEFAULT_STACK0_SIZE;
#pragma GCC diagnostic pop
  threadp[0]->stack_size = DEFAULT_STACK0_SIZE;
  setStackMarker(threadp[0]->stack);

#ifdef __IMXRT1062__

  // commandeer SVCall & use GTP1 Interrupt
  save_svcall_isr = _VectorsRam[11];
  if (save_svcall_isr == unused_interrupt_vector) save_svcall_isr = 0;
  _VectorsRam[11] = threads_svcall_isr;

  currentUseSystick = 0; // disable Systick calls
  gtp1_init(1000);       // tick every millisecond

#else

  currentUseSystick = 1;

  // commandeer the SVCall & SysTick Exceptions
  save_svcall_isr = _VectorsRam[11];
  if (save_svcall_isr == unused_isr) save_svcall_isr = 0;
  _VectorsRam[11] = threads_svcall_isr;
  
  save_systick_isr = _VectorsRam[15];
  if (save_systick_isr == unused_isr) save_systick_isr = 0;
  _VectorsRam[15] = threads_systick_isr;

#ifdef DEBUG
#if defined(__MK20DX256__) || defined(__MK20DX128__)
  ARM_DEMCR |= ARM_DEMCR_TRCENA; // Make ssure Cycle Counter active
  ARM_DWT_CTRL |= ARM_DWT_CTRL_CYCCNTENA;
#endif
#endif

#endif
}

/*
 * start() - Begin threading
 */
int Threads::start(int prev_state) {
  __disable_irq();
  int old_state = currentActive;
  if (prev_state == -1) prev_state = STARTED;
  currentActive = prev_state;
  __enable_irq();
  return old_state;
}

/*
 * stop() - Stop threading, even if active.
 *
 * If threads have already started, this should be called sparingly
 * because it could destabalize the system if thread 0 is stopped.
 */
int Threads::stop() {
  __disable_irq();
  int old_state = currentActive;
  currentActive = STOPPED;
  __enable_irq();
  return old_state;
}

/*
 * getNextThread() - Find next running thread
 *
 * This will also set the context_switcher() state variables
 */
void Threads::getNextThread() {

#ifdef DEBUG
  // Keep track of the number of cycles expended by each thread.
  // See @dfragster: https://forum.pjrc.com/threads/41504-Teensy-3-x-multithreading-library-first-release?p=213086#post213086
  currentThread->cyclesAccum += ARM_DWT_CYCCNT - currentThread->cyclesStart;
#endif

  // First, save the currentSP set by context_switch
  currentThread->sp = currentSP;

  // did we overflow the stack (don't check thread 0)?
  // allow an extra 8 bytes for a call to the ISR and one additional call or variable
  if (current_thread && ((uint8_t*)currentThread->sp - currentThread->stack <= overflow_stack_size)) {
    stack_overflow_isr();
  }

  // Find the next running thread
  while(1) {
    current_thread++;
    if (current_thread >= MAX_THREADS) {
      current_thread = 0; // thread 0 is MSP; always active so return
      break;
    }
    if (threadp[current_thread] && threadp[current_thread]->flags == RUNNING) break;
  }
  currentCount = threadp[current_thread]->ticks;

  currentThread = threadp[current_thread];
  currentSave = &threadp[current_thread]->save;
  currentMSP = (current_thread==0?1:0);
  currentSP = threadp[current_thread]->sp;

#ifdef DEBUG
  currentThread->cyclesStart = ARM_DWT_CYCCNT;
#endif
}

/*
 * Empty placeholder for IntervalTimer class
 */
#ifndef __IMXRT1062__
static void context_pit_empty() {}
#endif
/*
 * Store the PIT timer flag register for use in assembly
 */
volatile uint32_t *context_timer_flag;

/*
 * Defined in assembly code
 */
extern "C" void context_switch_pit_isr();

/*
 * Stop using the SysTick interrupt and start using
 * the IntervalTimer timer. The parameter is the number of microseconds
 * for each tick.
 */
int Threads::setMicroTimer(int tick_microseconds)
{
#ifdef __IMXRT1062__

  gtp1_init(tick_microseconds);

#else

/*
 * Implementation strategy suggested by @tni in Teensy Forums; see
 * https://forum.pjrc.com/threads/41504-Teensy-3-x-multithreading-library-first-release
 */

  // lowest priority so we don't interrupt other interrupts
  context_timer.priority(255);
  // start timer with dummy fuction
  if (context_timer.begin(context_pit_empty, tick_microseconds) == 0) {
    // failed to set the timer!
    return 0;
  }
  currentUseSystick = 0; // disable Systick calls

  // get the PIT number [0-3] (IntervalTimer overrides IRQ_NUMBER_t op)
  int number = (IRQ_NUMBER_t)context_timer - IRQ_PIT_CH0;
  // calculate number of uint32_t per PIT; should be 4.
  // Not hard-coded in case this changes in future CPUs.
  const int width = (PIT_TFLG1 - PIT_TFLG0) / sizeof(uint32_t);
  // get the right flag to ackowledge PIT interrupt
  context_timer_flag = &PIT_TFLG0 + (width * number);
  attachInterruptVector(context_timer, context_switch_pit_isr);

#endif

  return 1;
}

/*
 * Set each time slice to be 'microseconds' long
 */
int Threads::setSliceMicros(int microseconds)
{
  setMicroTimer(microseconds);
  setDefaultTimeSlice(1);
  return 1;
}

/*
 * Set each time slice to be 'milliseconds' long
 */
int Threads::setSliceMillis(int milliseconds)
{
  if (currentUseSystick) {
    setDefaultTimeSlice(milliseconds);
  }
  else {
    // if we're using the PIT, we should probably really disable it and
    // re-establish the systick timer; but this is easier for now
    setSliceMicros(milliseconds * 1000);
  }
  return 1;
}

/*
 * del_process() - This is called when the task returns
 *
 * Turns thread off. Thread continues running until next call to
 * context_switch() at which point it all stops. The while(1) statement
 * just stalls until such time.
 */
void Threads::del_process(void)
{
  int old_state = threads.stop();
  ThreadInfo *me = threads.threadp[threads.current_thread];
  // Would love to delete stack here but the thread doesn't
  // end now. It continues until the next tick.
  // if (me->my_stack) {
  //   delete[] me->stack;
  //   me->stack = 0;
  // }
  threads.thread_count--;
  me->flags = ENDED; //clear the flags so thread can stop and be reused
  threads.start(old_state);
  while(1); // just in case, keep working until context change when execution will not return to this thread
}

/*
 * Set a marker at memory so we can detect memory overruns
 */

const uint32_t thread_marker = 0xDEADDEAD;

void Threads::setStackMarker(void *stack)
{
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Warray-bounds"
  uint32_t *m = (uint32_t*)stack;
  *m = thread_marker;
#pragma GCC diagnostic pop
}

/*
 * Users call this function to see if stack has been corrupted
 */
int Threads::testStackMarkers(int *threadid)
{
  for (int i=0; i < MAX_THREADS; i++) {
    if (threadp[i] == NULL) continue;
    if (threadp[i]->flags == RUNNING) {
      uint32_t *m = (uint32_t*)threadp[i]->stack;
      if (*m != thread_marker) {
        if (threadid) *threadid = i;
        return -1;
      }
    }
  }
  return 0;
}

/*
 * Initializes a thread's stack. Called when thread is created
 */
void *Threads::loadstack(ThreadFunction p, void * arg, void *stackaddr, int stack_size)
{
  interrupt_stack_t * process_frame = (interrupt_stack_t *)((uint8_t*)stackaddr + stack_size - sizeof(interrupt_stack_t) - overflow_stack_size);
  process_frame->r0 = (uint32_t)arg;
  process_frame->r1 = 0;
  process_frame->r2 = 0;
  process_frame->r3 = 0;
  process_frame->r12 = 0;
  process_frame->lr = (uint32_t)Threads::del_process;
  process_frame->pc = ((uint32_t)p);
  process_frame->xpsr = 0x1000000;
  uint8_t *ret = (uint8_t*)process_frame;
  // ret -= sizeof(software_stack_t); // uncomment this if we are saving R4-R11 to the stack
  return (void*)ret;
}

/*
 * Add a new thread to the queue.
 *    add_thread(fund, arg)
 *
 *    fund : is a function pointer. The function prototype is:
 *           void *func(void *param)
 *    arg  : is a void pointer that is passed as the first parameter
 *           of the function. In the example above, arg is passed
 *           as param.
 *    stack_size : the size of the buffer pointed to by stack. If
 *           it is 0, then "stack" must also be 0. If so, the function
 *           will allocate the default stack size of the heap using new().
 *    stack : pointer to new data stack of size stack_size. If this is 0,
 *           then it will allocate a stack on the heap using new() of size
 *           stack_size. If stack_size is 0, a default size will be used.
 *    return: an integer ID to be used for other calls
 */
int Threads::addThread(ThreadFunction p, void * arg, int stack_size, void *stack)
{
  int old_state = stop();
  if (stack_size == -1) stack_size = DEFAULT_STACK_SIZE;
  for (int i=1; i < MAX_THREADS; i++) {
    if (threadp[i] == NULL) { // empty thread, so fill it
      threadp[i] = new ThreadInfo();
    }
    if (threadp[i]->flags == ENDED || threadp[i]->flags == EMPTY) { // free thread
      ThreadInfo *tp = threadp[i]; // working on this thread
      if (tp->stack && tp->my_stack) {
        delete[] tp->stack;
      }
      if (stack==0) {
        stack = new uint8_t[stack_size];
        tp->my_stack = 1;
      }
      else {
        tp->my_stack = 0;
      }
      setStackMarker(stack);
      tp->stack = (uint8_t*)stack;
      tp->stack_size = stack_size;
      void *psp = loadstack(p, arg, tp->stack, tp->stack_size);
      tp->sp = psp;
      tp->ticks = DEFAULT_TICKS;
      tp->flags = RUNNING;
      tp->save.lr = 0xFFFFFFF9;

#ifdef DEBUG
      tp->cyclesStart = ARM_DWT_CYCCNT;
      tp->cyclesAccum = 0;
#endif

      currentActive = old_state;
      thread_count++;
      if (old_state == STARTED || old_state == FIRST_RUN) start();
      return i;
    }
  }
  if (old_state == STARTED) start();
  return -1;
}

int Threads::getState(int id)
{
  return threadp[id]->flags;
}

int Threads::setState(int id, int state)
{
  threadp[id]->flags = state;
  return state;
}

int Threads::wait(int id, unsigned int timeout_ms)
{
  unsigned int start = millis();
  // need to store state in temp volatile memory for optimizer.
  // "while (thread[id].flags != RUNNING)" will be optimized away
  volatile int state;
  while (1) {
    if (timeout_ms != 0 && millis() - start > timeout_ms) return -1;
    state = threadp[id]->flags;
    if (state != RUNNING) break;
    yield();
  }
  return id;
}

int Threads::kill(int id)
{
  threadp[id]->flags = ENDED;
  return id;
}

int Threads::suspend(int id)
{
  threadp[id]->flags = SUSPENDED;
  return id;
}

int Threads::restart(int id)
{
  threadp[id]->flags = RUNNING;
  return id;
}

void Threads::setTimeSlice(int id, unsigned int ticks)
{
  threadp[id]->ticks = ticks - 1;
}

void Threads::setDefaultTimeSlice(unsigned int ticks)
{
  DEFAULT_TICKS = ticks - 1;
}

void Threads::setDefaultStackSize(unsigned int bytes_size)
{
  DEFAULT_STACK_SIZE = bytes_size;
}

void Threads::yield() {
  __asm volatile("svc %0" : : "i"(Threads::SVC_NUMBER));
}

void Threads::yield_and_start() {
  __asm volatile("svc %0" : : "i"(Threads::SVC_NUMBER_ACTIVE));
}

void Threads::delay(int millisecond) {
  int mx = millis();
  while((int)millis() - mx < millisecond) yield();
}

/*
 * Experimental code for putting CPU into sleep mode during delays
 */

void Threads::setSleepCallback(ThreadFunctionSleep callback) 
{
  enter_sleep_callback = callback;
}

void Threads::delay_us(int microsecond){
  int mx = micros();
  while ((int)micros() - mx < microsecond) yield();
}

void Threads::idle() {
	volatile bool needs_run[thread_count];
	volatile int i, j;
	volatile int task_id_ends;

  if (enter_sleep_callback==NULL) return;

	__disable_irq();
	task_id_ends = -1;
	//get lowest sleep interval from sleeping tasks into task_id_ends
	for (i = 0; i < thread_count; i++) {
		//sort by ending time first
		for (j = i + 1; j < thread_count; ++j) {
			if (! threadp[i]) { continue; }
			if (threadp[i]->sleep_time_till_end_tick > threadp[j]->sleep_time_till_end_tick) {
				//if end time soonest
				if (getState(i+1) == SUSPENDED) {
					task_id_ends = j; //store next task
				}
			}
		}
	}
  if (task_id_ends==-1) return;

	//set the sleeping time to substractor
	int subtractor = threadp[task_id_ends]->sleep_time_till_end_tick;
	
	if (subtractor > 0) {
		//if sleep is needed
		volatile int time_spent_asleep = enter_sleep_callback(subtractor);
		//store new data based on time spent asleep
		for (i = 0; i < thread_count; i++) {
      if (! threadp[i]) continue;
      needs_run[i] = 0;
      if (getState(i+1) == SUSPENDED) {
				threadp[i]->sleep_time_till_end_tick -= time_spent_asleep; //substract sleep time
				//time to run?
				if (threadp[i]->sleep_time_till_end_tick <= 0) {
					needs_run[i] = 1;
				} else {
					needs_run[i] = 0;
				}
			}
		}
		//for each thread when slept, resume if needed
		for (i = 0; i < thread_count; i++) {
			if (! threadp[i]) { continue; }
			if (needs_run[i]) {
				setState(i+1, RUNNING);
				threadp[i]->sleep_time_till_end_tick = 60000;
			}
		}
	}
	__enable_irq();
	yield();
}

void Threads::sleep(int ms) {
	int i = id();
	if (getState(i) == RUNNING) {
		__disable_irq();
		threadp[i-1]->sleep_time_till_end_tick = ms;
		setState(i, SUSPENDED);
		__enable_irq();
		yield();
	}
}

/* End of experimental code */

int Threads::id() {
  volatile int ret;
  __disable_irq();
  ret = current_thread;
  __enable_irq();
  return ret;
}

int Threads::getStackUsed(int id) {
  return threadp[id]->stack + threadp[id]->stack_size - (uint8_t*)threadp[id]->sp;
}

int Threads::getStackRemaining(int id) {
  return (uint8_t*)threadp[id]->sp - threadp[id]->stack;
}

char *Threads::threadsInfo(void)
{
  static char _buffer[Threads::UTIL_TRHEADS_BUFFER_LENGTH];
  unsigned int _buffer_cursor = 0;
  _buffer_cursor = sprintf(_buffer, "_____\n");
  for (int each_thread = 0; each_thread < thread_count; each_thread++)
  {
    if (threadp[each_thread] != NULL)
    {
      _buffer_cursor += sprintf(_buffer + _buffer_cursor, "%d:", each_thread);
      _buffer_cursor += sprintf(_buffer + _buffer_cursor, "Stack size:%d|",
                                threadp[each_thread]->stack_size);
      _buffer_cursor += sprintf(_buffer + _buffer_cursor, "Used:%d|Remains:%d|",
                                getStackUsed(each_thread),
                                getStackRemaining(each_thread));
      char *_thread_state = _util_state_2_string(threadp[each_thread]->flags);
      _buffer_cursor += sprintf(_buffer + _buffer_cursor, "State:%s|",
                                _thread_state);
#ifdef DEBUG
      _buffer_cursor += sprintf(_buffer + _buffer_cursor, "cycles:%lu\n",
                                threadp[each_thread]->cyclesAccum);
#else
      _buffer_cursor += sprintf(_buffer + _buffer_cursor, "\n");
#endif
    }
  }
  return _buffer;
}

#ifdef DEBUG
unsigned long Threads::getCyclesUsed(int id) {
  stop();
  unsigned long ret = threadp[id]->cyclesAccum;
  start();
  return ret;
}
#endif

/*
 * On creation, stop threading and save state
 */
Threads::Suspend::Suspend() {
  __disable_irq();
  save_state = currentActive;
  currentActive = 0;
  __enable_irq();
}

/*
 * On destruction, restore threading state
 */
Threads::Suspend::~Suspend() {
  __disable_irq();
  currentActive = save_state;
  __enable_irq();
}

int Threads::Mutex::getState() {
  int p = threads.stop();
  int ret = state;
  threads.start(p);
  return ret;
}

int __attribute__ ((noinline)) Threads::Mutex::lock(unsigned int timeout_ms) {
  if (try_lock()) return 1; // we're good, so avoid more checks

  uint32_t start = systick_millis_count;
  while (1) {
    if (try_lock()) return 1;
    if (timeout_ms && (systick_millis_count - start > timeout_ms)) return 0;
    if (waitthread==-1) { // can hold 1 thread suspend until unlock
      int p = threads.stop();
      waitthread = threads.current_thread;
      waitcount = currentCount;
      threads.suspend(waitthread);
      threads.start(p);
    }
    threads.yield();
  }
  __flush_cpu();
  return 0;
}

int Threads::Mutex::try_lock() {
  int p = threads.stop();
  if (state == 0) {
    state = 1;
    threads.start(p);
    return 1;
  }
  threads.start(p);
  return 0;
}

int __attribute__ ((noinline)) Threads::Mutex::unlock() {
  int p = threads.stop();
  if (state==1) {
    state = 0;
    if (waitthread >= 0) { // reanimate a suspended thread waiting for unlock
      threads.restart(waitthread);
      waitthread = -1;
      __flush_cpu();
      threads.yield_and_start();
      return 1;
    }
  }
  __flush_cpu();
  threads.start(p);
  return 1;
}