Thread and context switch

I implemented a simple context switch for ARM. First, I implemented a THREAD and THREADCTL as below.

*** rpi.h
#define MAX_THREADS 1024
void InitThread();
void CreateThread(void *thread_entry);
void ContextSwitch();

typedef enum {
  THREAD_NONE,
  THREAD_CREATED,
  THREAD_RUNNING,
  THREAD_WAITING
} THREAD_STATE;

typedef struct {
  THREAD_STATE state;
  int *stack;
  unsigned int stackSize;
} THREAD;

typedef struct {
  unsigned int currentId;
  unsigned int length;
  THREAD thread[MAX_THREADS];
} THREADCTL;

extern THREADCTL threadctl;

*** rpi-thread.c
THREADCTL threadctl;

void InitThread() {
  threadctl.length = 1;
  threadctl.thread[0].state = THREAD_RUNNING;
  threadctl.currentId = 0;

  threadctl.thread[0].stack = NULL;
  /* threadctl.thrad[0].stackSize = TBD; */
}

void CreateThread(void *thread_entry) {
  unsigned int id = threadctl.length;
  const unsigned int stackSize = 4096;  // 4096 * sizeof(int) allocated
  int *stackBase;

  threadctl.length++;
  threadctl.thread[id].state = THREAD_CREATED;
  stackBase = (int *)malloc(stackSize * sizeof(int));
  stackBase += stackSize - 1;
  threadctl.thread[id].stack = stackBase-13;
  threadctl.thread[id].stackSize = stackSize;

  // push default registers into the stack which will be poped in
  // _context_switch
  *threadctl.thread[id].stack =
      (int)thread_entry;  // r14: lr (to be stored in pc)
  threadctl.thread[id].stack--;
  *threadctl.thread[id].stack = 0;  // r12
  threadctl.thread[id].stack--;
  *threadctl.thread[id].stack = 0;  // r11
  threadctl.thread[id].stack--;
  *threadctl.thread[id].stack = 0;  // r10
  threadctl.thread[id].stack--;
  *threadctl.thread[id].stack = 0;  // r9
  threadctl.thread[id].stack--;
  *threadctl.thread[id].stack = 0;  // r8
  threadctl.thread[id].stack--;
  *threadctl.thread[id].stack = 0;  // r7
  threadctl.thread[id].stack--;
  *threadctl.thread[id].stack = 0;  // r6
  threadctl.thread[id].stack--;
  *threadctl.thread[id].stack = 0;  // r5
  threadctl.thread[id].stack--;
  *threadctl.thread[id].stack = 0;  // r4
  threadctl.thread[id].stack--;
  *threadctl.thread[id].stack = 0;  // r3
  threadctl.thread[id].stack--;
  *threadctl.thread[id].stack = 0;  // r2
  threadctl.thread[id].stack--;
  *threadctl.thread[id].stack = 0;  // r1
  threadctl.thread[id].stack--;
  *threadctl.thread[id].stack = 0;  // r0
  // don't do stack--!
}

void ContextSwitch() {
  if (threadctl.length <= 1) {
    return;
  }

  unsigned int currentId = threadctl.currentId;
  unsigned int nextId = currentId + 1;

  if (nextId >= threadctl.length) {
    nextId = 0;
  }

  char message[512];
  if (NULL != threadctl.thread[currentId].stack &&
      NULL != threadctl.thread[nextId].stack) {
    sprintf(message, "%d jumping from th:%d@%p to th:%d@%p", timerctl.counter,
            currentId, *threadctl.thread[currentId].stack, nextId,
            *threadctl.thread[nextId].stack);
  } else {
    sprintf(message, "%d jumping from th:%d to th:%d", timerctl.counter,
            currentId, nextId);
  }
  FillRect(0, 16, kWidth, 16, 0);
  PrintStr(0, 16, message, 7);

  // get current sp
  threadctl.thread[currentId].stack = (int *)_get_stack_pointer();

  // do context switch
  threadctl.thread[nextId].state = THREAD_RUNNING;
  threadctl.thread[currentId].state = THREAD_WAITING;
  threadctl.currentId = nextId;
  _context_switch(&threadctl.thread[currentId].stack,
                  &threadctl.thread[nextId].stack);
}

The _context_switch is written in assembly which pushes r0-14 in per-thread stack and save the stack pointer in the per-thread variable ‘stack’.

.global _get_stack_pointer
_get_stack_pointer:
  mov r0, r13
  bx lr
  
.global _context_switch
_context_switch:
  // same as stmfd/stmdb !r13, {...}
  push {r0-r12,r14}
  str sp, [r0]
  ldr sp, [r1]
  // same as ldmfd/ldmia !r13, {...}
  pop {r0-r12}
  pop {pc} // pc points to the previous lr

I use it in main function.

InitThread(); // add the current thread into THREADCTL
CreateThread(task_a); // create a new thread for task_a and start it
CreateThread(task_b); // create a new thread for task_b and start it
...
// switch contexts using timer interrupt
while (true) {
  _disable_IRQ();
  _wfi();
  if (StatusFifo8(&fifoTimer) == 0) {
    _enable_IRQ();
  } else {
    unsigned char data = GetFifo8(&fifoTimer);
    _enable_IRQ();
      
     switch (data) {
      case (const int)timerData1:
        counter1++;
        SetTimer(timer1, timerInterval1, timerData1);
        draw_counter(0, counter1);
        ContextSwitch();
        break;
    }
  }
}
  
void task_a() {
  unsigned int counter = 0;
  while (true) {
    draw_counter(1, counter++);
    ContextSwitch(); // for now, it's changing contexts by the function itself
  }
}

void task_b() {
  unsigned int counter = 0;
  while (true) {
    draw_counter(2, counter++);
    ContextSwitch(); // for now, it's changing contexts by the function itself
  }
}

Tested it and worked!
Today’s task_a/b have ContextSwitch() explicitly. I’ll make the context switch full automatic in the next post.

https://github.com/sokoide/rpi-baremetal -> 008_context_switch.