RTOS设计与实现

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21

// 初始化系统
bool ros_init();
// 获取当前在运行的任务
ROS_TCB *ros_current_tcb();
// 创建任务
status_t ros_create_task(ROS_TCB *tcb, task_func task, uint8_t priority,stack_t *stack, int stack_size);
// 任务调度
void ros_schedule();

// 阻塞任务，以 System tick 为单位
status_t ros_delay(uint32_t ticks);

// 下面四个函数,是移植 ROS 时需要实现的
// 初始化 Timer
extern void ros_init_timer();
// 默认任务
extern void ros_idle_task();
// 初始化任务栈
extern void ros_task_context_init(ROS_TCB *tcb_ptr, task_func task_f,void *stack_top);
// 上下文切换
extern void ros_switch_context(ROS_TCB *old_tcb, ROS_TCB *new_tcb);

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14

typedef enum {
TASK_READY = 0,
TASK_BLOCKED,
TASK_TERMINATED
} Task_Status;

typedef void (*task_func)();
typedef struct ros_tcb {
void *sp;
Task_Status status;
uint8_t priority; // 0~255
task_func task_entry;
struct ros_tcb *next_tcb;
} ROS_TCB;

1
2
3
4
5
6

void task1() {
for(;;) {
// task code here
}
// should never return, once return the task will be deleted from the ready list
}

1
2
3
4
5

typedef struct ros_timer {
ROS_TCB *blocked_tcb;
uint32_t ticks;
struct ros_timer *next_timer;
} ROS_TIMER

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16

/**
 * @brief  Start the os:
 * 1. init the system timer, start ticking
 * 2. add a idle task into the list
 * @retval ture if os started
 */
bool ros_init() {
  CRITICAL_STORE;
  CRITICAL_START();
  ros_init_timer();
  status_t ok =
      ros_create_task(&idle_tcb, ros_idle_task, MIN_TASK_PRIORITY, idle_task_stack, ROS_IDLE_STACK_SIZE);
  ROS_STARTED = ok == ROS_OK;
  CRITICAL_END();
  return ROS_STARTED;
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10

static void init_timer1() {
  // Set prescaler 256
  TCCR1B = _BV(CS12) | _BV(WGM12);
  // For Arduino Uno CPU 16000000 HZ, so the OCR1A should count from 0 to 624
  // x * 1/16M * 256 = 10 ms = 0.01 s
  // x = 16 M / 100 / 256 = 625
  OCR1A = (F_CPU / 256 / ROS_SYS_TICK) - 1;
  // enable compare match 1A interrupt
  TIMSK1 = _BV(OCIE1A);
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10

/**
 * @brief The idle task takes advantage of atmega328p's sleep mode, sleep when
 * there is no task to run
 */
void ros_idle_task() {
  set_sleep_mode(SLEEP_MODE_IDLE);
  while (1) {
    sleep_mode();
  }
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

void ros_task_context_init(ROS_TCB *tcb_ptr, task_func task_f, void *sp) {
  uint8_t *stack_top = (uint8_t *)sp;
  // pc
  // the function pointer is uint16_t in avr
  *stack_top-- = (uint8_t)((uint16_t)task_shell & 0xFF);         // the LSB
  *stack_top-- = (uint8_t)(((uint16_t)task_shell >> 8) & 0xFF);  // THE MSB
  // Make space for R2-R17, R28-R29
  *stack_top-- = 0x00; // R2
  *stack_top-- = 0x00; // R3
  *stack_top-- = 0x00; // R4
  *stack_top-- = 0x00; // R5
  *stack_top-- = 0x00; // R6
  *stack_top-- = 0x00; // R7
  *stack_top-- = 0x00; // R8
  *stack_top-- = 0x00; // R9
  *stack_top-- = 0x00; // R10
  *stack_top-- = 0x00; // R11
  *stack_top-- = 0x00; // R12
  *stack_top-- = 0x00; // R13
  *stack_top-- = 0x00; // R14
  *stack_top-- = 0x00; // R15
  *stack_top-- = 0x00; // R16
  *stack_top-- = 0x00; // R17
  *stack_top-- = 0x00; // R28
  *stack_top-- = 0x00; // R29
  tcb_ptr->sp = stack_top;
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18

/**
 * @brief Wrapper of task function, which can pass param to task(for furture
 * usage), and terminated it and re-schedule when a task run to compeletion
 */
static void task_shell() {
  ROS_TCB *cur_tcb = ros_current_tcb();

  // enable interrupt after context switching finished.
  sei();

  if (cur_tcb && cur_tcb->task_entry) {
    cur_tcb->task_entry();
    // when the task terminated(task return), remove it from ready list and
    cur_tcb->status = TASK_TERMINATED;
  }
  // re-schedule
  ros_schedule();
}

1
2
3

ISR(interrupt_name_in_vect) {
 //your code
}

1
2
3
4
5

void ros_int_enter() { ros_int_cnt++; }
void ros_int_exit() {
	ros_int_cnt--;
	ros_schedule();
}

1
2
3
4
5
6
7

// interrupt every SYS_TICK to re-schedule tasks
ISR(TIMER1_COMPA_vect) {
  ros_int_enter();
  ros_sys_tick();
  // exit ISR, ready to call scheduler
  ros_int_exit();
}

1
2
3
4

void ros_schedule() {
	if (ros_int_cnt != 0 || !ROS_STARTED) return
	// core scheduler code
}

1
2

void ros_tcb_enqueue(ROS_TCB *tcb);
ROS_TCB *ros_tcb_dequeue(uint8_t lowest_priority);

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

/**
 * @brief enqueue tcb to list order by priority. Do round-robin when priority is
 * same.
 * @param  *tcb: the tcb to insert
 */
void ros_tcb_enqueue(ROS_TCB *tcb) {
  if (tcb == NULL) return;
  ROS_TCB *prev_ptr, *next_ptr;
  prev_ptr = next_ptr = tcb_ready_list;
  do {
    // Insert when:
    // next == NULL
    // next tcb's priority is lower than than the one we enqueuing
    // same priority task will do round-bobin
    if ((next_ptr == NULL) || (next_ptr->priority > tcb->priority)) {
      // list is empty or insert to head
      if (next_ptr == tcb_ready_list) {
        tcb_ready_list = tcb;
        tcb->next_tcb = next_ptr;  // next_ptr maybe NULL
      } else {                     // insert between tow tcb or tail
        tcb->next_tcb = next_ptr;  // next_ptr maybe NUL
        prev_ptr->next_tcb = tcb;
      }
      break;
    } else {
      prev_ptr = next_ptr;
      next_ptr = next_ptr->next_tcb;
    }
  } while (prev_ptr != NULL);
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21

/**
 * @brief  dequeue a tcb to swap in, requeir its priority no lower than
 * lowest_priority Because the list ordered by priority, we just check the head,
 * if the head is lower than lowest_priority, return NULL. use
 * ros_tcb_dequeue(MIN_TASK_PRIORITY) to dequeue head unconditionally
 * @param lowest_priority: the lowest priority of dequeue tcb or NULL if no such
 * tcb
 */
ROS_TCB *ros_tcb_dequeue(uint8_t lowest_priority) {
  if (tcb_ready_list == NULL || tcb_ready_list->priority > lowest_priority) {
    return NULL;
  } else {
    ROS_TCB *tcb = tcb_ready_list;
    tcb_ready_list = tcb_ready_list->next_tcb;
    if (tcb_ready_list) {
      // make return tcb isolated
      tcb->next_tcb = NULL;
    }
    return tcb;
  }
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

void ros_schedule() {
  // no schedule and context switch util the very end of ISR
  if (ros_int_cnt != 0 || !ROS_STARTED) return;
  CRITICAL_STORE;
  ROS_TCB *new_tcb = NULL;
  CRITICAL_START();
  // if current task is NULL or suspend or terminated, a new task will swap in
  // unconditionally
  if (current_tcb == NULL || current_tcb->status == TASK_BLOCKED ||
      current_tcb->status == TASK_TERMINATED) {
    // task with any priority(0~255) can be swap in
    new_tcb = ros_tcb_dequeue(MIN_TASK_PRIORITY);
    if (new_tcb) {
      // Do not enqueue curren_tcb here, when the task is blocked, it is added
      // to timer_queue, it will enqueue when the ticks due.
      ros_switch_context_shell(current_tcb, new_tcb);
    } else {
      // but you can't block the idle task
      if (current_tcb == &idle_tcb) current_tcb->status = TASK_READY;
    }
  } else {
    // remove terminated task
    do {
      new_tcb = ros_tcb_dequeue(current_tcb->priority);
    } while (new_tcb && new_tcb->status == TASK_TERMINATED);
    if (new_tcb) {
      ros_tcb_enqueue(current_tcb);
      ros_switch_context_shell(current_tcb, new_tcb);
    }
  }
  CRITICAL_END();
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64

void ros_switch_context(ROS_TCB *old_tcb, ROS_TCB *new_tcb) {
  // The assembly code is in intel style, source is always on the right
  // Y-reg is R28 and R29
  __asm__ __volatile__(
      "push r2\n\t"
      "push r3\n\t"
      "push r4\n\t"
      "push r5\n\t"
      "push r6\n\t"
      "push r7\n\t"
      "push r8\n\t"
      "push r9\n\t"
      "push r10\n\t"
      "push r11\n\t"
      "push r12\n\t"
      "push r13\n\t"
      "push r14\n\t"
      "push r15\n\t"
      "push r16\n\t"
      "push r17\n\t"
      "push r28\n\t"
      "push r29\n\t"
      // r16, r17, r28 and r29 is saved, we're safe to use them
      "mov r28, %A[_old_tcb_]\n\t" // move old tcb(LSB) to Y-regs
      "mov r29, %B[_old_tcb_]\n\t" // MSB
      "sbiw r28, 0\n\t"        // subract 0 from r29:r28, we need this to set SREG-Z if result is zero
      "breq restore\n\t"           // if old_tcb is NULL, jump to restore
      "in r16, %[_SPL_]\n\t"       // get stack pointer to r17:r16
      "in r17, %[_SPH_]\n\t"
      "st Y, r16\n\t"              // set old_tcb->sp to stack pointer
      "std Y+1, r17\n\t"           // because sp is the first member of the TCB struct
      "restore:"
      "mov r28, %A[_new_tcb_]\n\t"
      "mov r29, %B[_new_tcb_]\n\t"
      "ld r16, Y\n\t"              //load new_tcb->sp to r17:r16
      "ldd r17, Y+1\n\t"
      "out %[_SPL_], r16\n\t"      //change the stack pointer to new_tcb->sp
      "out %[_SPH_], r17\n\t"
      "pop r29\n\t"                // restore new_tcb's context
      "pop r28\n\t"
      "pop r17\n\t"
      "pop r16\n\t"
      "pop r15\n\t"
      "pop r14\n\t"
      "pop r13\n\t"
      "pop r12\n\t"
      "pop r11\n\t"
      "pop r10\n\t"
      "pop r9\n\t"
      "pop r8\n\t"
      "pop r7\n\t"
      "pop r6\n\t"
      "pop r5\n\t"
      "pop r4\n\t"
      "pop r3\n\t"
      "pop r2\n\t"
      "ret\n\t"
      "" ::
      [_SPL_] "i" _SFR_IO_ADDR(SPL),
      [_SPH_] "i" _SFR_IO_ADDR(SPH),
      [_old_tcb_] "r"(old_tcb),
      [_new_tcb_] "r" (new_tcb)
  );
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

// delay current tcb
status_t ros_delay(uint32_t ticks) {
  ROS_TIMER timer;
  ROS_TCB *cur_tcb;
  uint8_t status;
  CRITICAL_STORE;
  cur_tcb = ros_current_tcb();
  if (ticks == 0) {
    status = ROS_ERR_PARAM;
  } else if (cur_tcb == NULL) {
    status = ROS_ERR_CONTEXT;
  } else {
    CRITICAL_START();
    cur_tcb->status = TASK_BLOCKED;
    timer.ticks = ticks;
    timer.blocked_tcb = cur_tcb;
    if (ros_register_timer(&timer) != ROS_OK) {
      status = ROS_ERR_TIMER;
      CRITICAL_END();
    } else {
      status = ROS_OK;
      CRITICAL_END();
      // call scheduler to swap out current task
      ros_schedule();
    }
  }
  return status;
}

1
2
3
4
5
6
7

static void wakeup_task(ROS_TCB *tcb) {
  CRITICAL_STORE;
  CRITICAL_START();
  tcb->status = TASK_READY;
  ros_tcb_enqueue(tcb);
  CRITICAL_END();
}

1
2
3
4

# comment
# (note: the <tab> in the command line is necessary for make to work)
target:  dependency1 dependency2 ...
      <tab> command

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45

CC=avr-gcc
OBJCOPY=avr-objcopy
SIZE=avr-size
DUDE=$(ARDUINO_DIR)/hardware/tools/avr/bin/avrdude
SIMAVR=simavr

MCU=atmega328p
FCPU=16000000L

ARDUINO_DIR=/home/leer/program/arduino-1.8.9
DUDE_CONF=$(ARDUINO_DIR)/hardware/tools/avr/etc/avrdude.conf
BUILD_DIR=build

CFLAGS=$(INCLUDES) -g -Wall -Werror -Os -mmcu=$(MCU) -DF_CPU=$(FCPU) -DARDUINO=10809 -DARDUINO_AVR_UNO -DARDUINO_ARCH_AVR

TARGET=ros
SOURCE=$(wildcard *.c)
# *.c -> build/*.o
OBJS=$(addprefix $(BUILD_DIR)/,$(SOURCE:.c=.o))

all: $(BUILD_DIR) $(BUILD_DIR)/$(TARGET).hex

$(BUILD_DIR)/%.o: %.c
  $(CC) $(CFLAGS) -c $< -o $@

$(BUILD_DIR):
  mkdir -p $(BUILD_DIR)

# Build elf, depends on *.o
$(BUILD_DIR)/$(TARGET).elf: $(OBJS)
  @echo Building $@...
  $(CC) $(CFLAGS) $(OBJS) -o $@
  $(SIZE) -C --mcu=$(MCU) $@

# Build hex, depends on elf
$(BUILD_DIR)/$(TARGET).hex: $(BUILD_DIR)/$(TARGET).elf
  @echo Building $@...
  $(OBJCOPY) -j .text -j .data -O ihex $< $@

upload:
  @echo Upload ROS to board...
  $(DUDE) -C $(DUDE_CONF) -v -p $(MCU) -c arduino -P /dev/ttyACM0 -b 115200 -D -U flash:w:$(BUILD_DIR)/$(TARGET).hex:i
.PHONY: clean
clean:
  rm -rf build

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64

/**
 * Blink example in ROS, DO NOT use any function in Arduino.h
 */
#include <avr/io.h>
#include "ros.h"

// include for simavr
// #include "avr_mcu_section.h"
// AVR_MCU(F_CPU, "atmega328p");
ROS_TCB task1;
ROS_TCB task2;
uint8_t task1_stack[ROS_DEFAULT_STACK_SIZE];
uint8_t task2_stack[ROS_DEFAULT_STACK_SIZE];

#define LED1 13
#define LED2 12
#define bitSet(value, bit) ((value) |= (1UL << (bit)))
#define bitClear(value, bit) ((value) &= ~(1UL << (bit)))

#define TASK1_PRIORITY 1
#define TASK2_PRIORITY 0  // max priority

void t1() {
  while (1) {
    // set LED1 high
    bitSet(PORTB, 5);
    ros_delay(200);
    bitClear(PORTB, 5);
    ros_delay(200);
  }
}

void t2() {
  while (1) {
    bitSet(PORTB, 4);
    // delay a second
    ros_delay(100);
    bitClear(PORTB, 4);
    ros_delay(100);
  }
}

void setup() {
  // set LED 13 and LED 12 as output
  bitSet(DDRB, 5);
  bitSet(DDRB, 4);
  bool os_started = ros_init();
  if (os_started) {
    ros_create_task(&task1, t1, TASK1_PRIORITY, task1_stack, ROS_DEFAULT_STACK_SIZE);
    ros_create_task(&task2, t2, TASK2_PRIORITY, task2_stack, ROS_DEFAULT_STACK_SIZE);
    ros_schedule();
  }
}

void loop() {
  // nothing
}

int main() {
  setup();
  // never call loop
  loop();
  return 0;
}