NXP JN516X (TWELITE) をプログラミングする（Multi Task)

JN516xの詳細を知りたくて調べてみたらNXP社の前はJENNIC社がASICで製造していて組み込んだCPUがBeyond Semiconductor社が開発していたOpenRISCの派生物らしい。NXP社が提供している開発ツールがBeyond Studioであることからもほぼ間違いないとは思う。

CPUはOpenRISCと似通ってはいるが異なる点も多くOpenRISCの命令セットの一部に独自命令を追加した命令セットになっているようだ。レジスタはR0 – R31まであって、R0が常にゼロを返すレジスタ、R1がスタックポインタ(SP)、R2は不明(OpenRISCではFP)、R3-R8が引数レジスタ、R9がリンクレジスタ(LR)となっている。またR9以上のレジスタについてはCallee-saveとなっていてSDKがR16-R31を予約使用しているようなのでアプリはR16以上を使ってはいけないようだ。特にR16はペリフェラルへアクセスするためにSDKの初期化で設定された後は変更されないことを前提に各ＡＰＩで使用しているようなので絶対変更してはいけない。

JN516xでマルチタスクというとContikiというswich構文ベースの疑似マルチタスクぐらいしかないようだ。SDKにはsetjmp/longjmpが実装されていたのでそれを使ってマルチタスクできるだろうと試してみたらまたまたドツボにハマッてしまった。このCPUは本当に落とし穴が多すぎる。(-_-;)

実装は問題なさそうなのだがsetjmp.hを見ると”stubs for future use.”と書かれていたので何か問題あるのかもとは思っていたがsetjmpは正しく動作するもののlongjmpを実行するとCPUが停止してしまう。R1からR31までのレジスタを単純に設定するだけで停止？．．．試しにと設定するレジスタを一つずつ減らしてみたら特定のレジスタ(R15,R17,R19,R25,R26)の設定でCPUが停止(例外？)するという普通にありえない現象だった。orz

しかも、それらのレジスタはコードで現実に使われているレジスタなので不安は尽きない．．．

とりあえず一部のレジスタのみを保存復旧するsetjmp/longjmpを試してみたら見事に動作するようになった。(/・ω・)/

タスク・ライブラリは、以前に投稿した”シンプルすぎる汎用C++タスクスケジューラー”を改良しJN516xに対応してみた。CPU非依存にするためスタックポインタの調整はalloca()を利用している。タイムスライスなしのノンプリエンプティブなタスクライブラリである。ちなみに前回のクラス名が長すぎてシンプルでなかったので今回は短くシンプルな名前にしてみた。(笑)

シンプルすぎる汎用C++タスクスケジューラー(CScheduler)を作ってみた。

以前と同じくWindows/Linuxでも使うことが可能なカレントスタックを切り刻んで使う方法に加え、組み込み用途でよくある静的配列変数領域をスタック領域としたりmalloc()で割り当てた領域をスタック領域とすることもできるようにしてみた。但し、スタックとヒープ領域が同じアドレス空間にあることやメモリレイアウトがスタック(下方伸長)＞ヒープ(上方伸長)であるという前提である。それ以外のCPUでは動作はしない。

※JN516xのSDKではmalloc()が実装されていないことに注意。

タスクはダブルエンデット・リストで管理されるがその操作はこれ以上無理なレベルまで最適化している。複数行のコードを１行にまとめただけとも言えるがコンパイラの最適化により全く無駄のないコードが生成される。

/* rolling (yield) */

_head = (_tail = (_tail->next = tcb = _head))->next;

/* enqueue */

(_tail = *(_tail ? &_tail->next : &_head) = tcb)->next = 0;

/* dequeue */

_head = (_head != _tail ? _head->next : _tail = 0);

/* rolling (yield) */

_head = (_tail = (_tail->next = tcb = _head))->next;

/* enqueue */

(_tail = *(_tail ? &_tail->next : &_head) = tcb)->next = 0;

/* dequeue */

_head = (_head != _tail ? _head->next : _tail = 0);

タスク同期機能はsleep/wakeup, mutex, semaphore, eventに対応している。ないよりはましという程度ではあるが。(-_-;)

JN516xのSDKではクラスのコンストラクターが実行されないのでそのかわりにinit()を定義している。またmalloc()が使えないためタスク同期クラスはstatic変数として定義し最初にinit()を呼び出す必要があることに注意しよう。

【修正履歴】
2022-06-29
昨日のSemクラスの修正の修正。

2022-06-28
Semクラスのタイミングバグ修正とTask::start()に負数のスタックサイズ指定をしたとき登録タスクではなく直前の登録タスクのスタックサイズ指定になっていて混乱しやすいので修正。

2022-06-23
Semクラスのバグ修正とTaskクラスのチューニング。それと２レベルではあるがタスク優先度を追加し起床タスクの優先度を一時的にあげる仕組みを追加してみた。リアルタイムにはスイッチングしないが休止状態から起床されたタスクが最優先で実行されるようになる。可能ならリアルタイム性を追加したいところではあるがリアルタイムやタイムスライスに対応すると標準ライブラリを含む全てのライブラリもマルチスレッド対応にする必要があったりしてとても大変なことになるで対応はしない。

2022-06-21
Task::start()の最適化を行った。

2022-06-15
Sync/Mutex/Sem/Eventクラスのコード最適化の続きを行った。

2022-06-14
Sync/Mutex/Sem/Eventクラスのコード最適化を行った。

2022-06-13
Eventクラスがバグッてたので修正。

2022-06-12
Task::start()で指定するタスクのスタックサイズが割り当てモード毎に違っていて混乱するのでスタックサイズにはTask::TCB_SIZEを加算したサイズが指定されているものと仮定した仕様変更を行った。

例えば、スタックサイズとして2048バイトを指定する場合は
2048 + Task::TCB_SIZE
を指定しなければならない。

2022-06-11
いつの間にかタスクが動作しなくなっていたのとSem/Mutex/Eventのウェイト処理のタイミングバグを修正。あとメッセージバッファのような機能を作りやすくするためSemクラスのwait()/post()にカウント値引数を追加。

2022-06-10
Sem::init()にカウンター初期値を設定するための引数を追加

2022-05-01
詳細不明であることが気になっていたためsetjmp/longjmpにて可能な限りのレジスタを保存復旧してみた。しかし、どうやってもCPUが不安定になってしまうためR17/R19の対応は無理なようだ。命令の順番を入れ替えたりすると挙動が変わるため投機実行処理(パイプライン)のバグなのかもしれない。R16以降のレジスタがSDKの予約レジスタであるなら問題にはならないはず。
ちなみにアセンブラのニーモニックにJRとかJALがあって日本に詳しい設計者がいたのかな？みたいに感じるところが少し面白い。(笑)　でも命令セットに起因して巨大なコードになってしまいがちだし実行速度が特別速いという感じでもないしコンパイラが64bit型の計算時にcarryを使わない非効率的なコードを生成するのも気になる点だ。OpenRISCの初期開発チップにcarryバグがあったらしいのでその回避策なのかもしれないが．．．
ちなみに最近存在感が目立ってきたRISC-V(OpenRISCの後継？)がどこまで進歩しているのか少し気になってきたかも。

【サンプル】

#include "device.h"

void task1(void *arg)
{
  (void)arg;
  while (1)
  {
    System::delay(1000);
    Debug::printf("Task1\n");
  }
}

void task2(void *arg)
{
  (void)arg;
  while (1)
  {
    System::delay(1000);
    Debug::printf("Task2\n");
  }
}

void setup(void)
{
#if 1
  /* 現在のスタック領域を刻んで利用 */
  Task::start(task1, 0, -TASK_DEFAULT_STACK_SIZE);
  Task::start(task2, 0, -TASK_DEFAULT_STACK_SIZE);
#else
  /* 変数領域をスタック領域とする */
  static uint32 stacks[2][TASK_DEFAULT_STACK_SIZE / sizeof(uint32)];
  Task::start(task1, 0, sizeof(stacks[0]), stacks[0]);
  Task::start(task2, 0, sizeof(stacks[1]), stacks[1]);
#endif
}

void loop(void)
{
  /* バックグランド処理にてTask::yield()を実行 */
}

#include "device.h"

void task1(void *arg)

{

(void)arg;

while (1)

{

System::delay(1000);

Debug::printf("Task1\n");

}

void task2(void *arg)

{

(void)arg;

while (1)

{

System::delay(1000);

Debug::printf("Task2\n");

}

void setup(void)

{

#if 1

/* 現在のスタック領域を刻んで利用 */

Task::start(task1, 0, -TASK_DEFAULT_STACK_SIZE);

Task::start(task2, 0, -TASK_DEFAULT_STACK_SIZE);

#else

/* 変数領域をスタック領域とする */

static uint32 stacks[2][TASK_DEFAULT_STACK_SIZE / sizeof(uint32)];

Task::start(task1, 0, sizeof(stacks[0]), stacks[0]);

Task::start(task2, 0, sizeof(stacks[1]), stacks[1]);

#endif

}

void loop(void)

{

/* バックグランド処理にてTask::yield()を実行 */

}

【ライブラリ】

/*
  task.h - Smallest Non Preemptive Multi Task Library for Embedded System

  Copyright (c) 2022 Sasapea's Lab. All right reserved.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General
  Public License along with this library; if not, write to the
  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
  Boston, MA  02111-1307  USA
*/
#pragma once

#include <stddef.h>
#include <stdint.h>
#include <stdbool.h>
#if defined(JENNIC_CHIP_FAMILY_JN516x)
  #define malloc(a) 0
  #define free(a)
  typedef size_t jn516x_jmp_buf[25];
  #define jmp_buf jn516x_jmp_buf
#else
  #include <stdlib.h>
  #include <setjmp.h>
#endif
#include "alarm.h"

#define TASK_DEFAULT_STACK_SIZE (2048 + Task::TCB_SIZE)

class Task
{
  public:

    typedef void (*func_t)(void *arg);

  protected:

    typedef struct tcb
    {
      struct tcb *next;
      jmp_buf context;
      void *alloc;
      void *arg;
      func_t start;
    } tcb_t;

    typedef struct
    {
      Alarm  alarm;
      size_t save;
      bool   retval;
    } ctrl_t;

    static void enqueue0(tcb_t *tcb);
    static void enqueue1(tcb_t *tcb);
    static tcb_t *dequeue(void);
    static void switching(void);
    static void dispatch(void);

#if defined(JENNIC_CHIP_FAMILY_JN516x)

    static int setjmp(jn516x_jmp_buf __jmpb) __attribute__((noinline,optimize(1)));
    static void longjmp(jn516x_jmp_buf __jmpb, int __retval = 1) __attribute__((noinline,optimize(1)));

#endif

  public:

    static const size_t TCB_SIZE = sizeof(tcb_t);

    static void begin(size_t size = TASK_DEFAULT_STACK_SIZE);
    static bool start(func_t func, void *arg = 0, int size = TASK_DEFAULT_STACK_SIZE, void *stack = 0);
    static void stop(void);
    static bool yield(void);
    static size_t current(void);
    static void sleep(int32_t ms, uint8 busyLoop = 0);
    static void sleepMicroseconds(int32_t us, uint8 busyLoop = 0);
    static void sleepMicroseconds64(int64_t us, uint8_t busyLoop = 0);
    static void delayMicroseconds(int32_t us, uint8 busyLoop = 0);
    static void delayMicroseconds64(int64_t us, uint8_t busyLoop = 0);

  private:

    static tcb_t *_disp;
    static tcb_t *_curr;
    static tcb_t *_head;
    static tcb_t *_tail[2];
    static size_t _used;
};

class Sync : protected Task
{
  public:

    Sync(void) { init(); }

    void init(void);
    bool sleep(int64_t timeout = -1);
    size_t wakeup(size_t task = 0);

  protected:

    bool sleep(ctrl_t& ctrl);
    static bool timeup(Alarm& alarm);

    tcb_t *_head;
    tcb_t *_tail;
};

class Mutex : protected Sync
{
  public:

    Mutex(void) { init(); }

    void init(void);
    bool lock(int64_t timeout = -1);
    void unlock(void);

  private:

    size_t _owner;
    size_t _count;
};

class Sem : protected Sync
{
  public:

    Sem(size_t limit = (size_t)-1, size_t count = 0) { init(limit, count); }

    void init(size_t limit = (size_t)-1, size_t count = 0);
    bool wait(size_t count = 1, int64_t timeout = -1);
    void post(size_t count = 1);

  protected:

    size_t _limit;
    size_t _count;
    size_t _wait;
};

class Event : protected Sem
{
  public:

    Event(void) { init(); }

    void init(void);
    bool wait(int64_t timeout = -1);
    void signal(void);
};

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

task.h - Smallest Non Preemptive Multi Task Library for Embedded System

This library is free software; you can redistribute it and/or

modify it under the terms of the GNU Lesser General Public

License as published by the Free Software Foundation; either

version 2.1 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,

but WITHOUT ANY WARRANTY; without even the implied warranty of

MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU

Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General

Public License along with this library; if not, write to the

Free Software Foundation, Inc., 59 Temple Place, Suite 330,

Boston, MA 02111-1307 USA

#pragma once

#include <stddef.h>

#include <stdint.h>

#include <stdbool.h>

#if defined(JENNIC_CHIP_FAMILY_JN516x)

#define malloc(a) 0

#define free(a)

typedef size_t jn516x_jmp_buf[25];

#define jmp_buf jn516x_jmp_buf

#else

#include <stdlib.h>

#include <setjmp.h>

#endif

#include "alarm.h"

#define TASK_DEFAULT_STACK_SIZE (2048 + Task::TCB_SIZE)

class Task

{

public:

typedef void (*func_t)(void *arg);

protected:

typedef struct tcb

{

struct tcb *next;

jmp_buf context;

void *alloc;

void *arg;

func_t start;

} tcb_t;

typedef struct

{

Alarm alarm;

size_t save;

bool retval;

} ctrl_t;

static void enqueue0(tcb_t *tcb);

static void enqueue1(tcb_t *tcb);

static tcb_t *dequeue(void);

static void switching(void);

static void dispatch(void);

#if defined(JENNIC_CHIP_FAMILY_JN516x)

static int setjmp(jn516x_jmp_buf __jmpb) __attribute__((noinline,optimize(1)));

static void longjmp(jn516x_jmp_buf __jmpb, int __retval = 1) __attribute__((noinline,optimize(1)));

#endif

public:

static const size_t TCB_SIZE = sizeof(tcb_t);

static void begin(size_t size = TASK_DEFAULT_STACK_SIZE);

static bool start(func_t func, void *arg = 0, int size = TASK_DEFAULT_STACK_SIZE, void *stack = 0);

static void stop(void);

static bool yield(void);

static size_t current(void);

static void sleep(int32_t ms, uint8 busyLoop = 0);

static void sleepMicroseconds(int32_t us, uint8 busyLoop = 0);

static void sleepMicroseconds64(int64_t us, uint8_t busyLoop = 0);

static void delayMicroseconds(int32_t us, uint8 busyLoop = 0);

static void delayMicroseconds64(int64_t us, uint8_t busyLoop = 0);

private:

static tcb_t *_disp;

static tcb_t *_curr;

static tcb_t *_head;

static tcb_t *_tail[2];

static size_t _used;

};

class Sync : protected Task

{

public:

Sync(void) { init(); }

void init(void);

bool sleep(int64_t timeout = -1);

size_t wakeup(size_t task = 0);

protected:

bool sleep(ctrl_t& ctrl);

static bool timeup(Alarm& alarm);

tcb_t *_head;

tcb_t *_tail;

};

class Mutex : protected Sync

{

public:

Mutex(void) { init(); }

void init(void);

bool lock(int64_t timeout = -1);

void unlock(void);

private:

size_t _owner;

size_t _count;

};

class Sem : protected Sync

{

public:

Sem(size_t limit = (size_t)-1, size_t count = 0) { init(limit, count); }

void init(size_t limit = (size_t)-1, size_t count = 0);

bool wait(size_t count = 1, int64_t timeout = -1);

void post(size_t count = 1);

protected:

size_t _limit;

size_t _count;

size_t _wait;

};

class Event : protected Sem

{

public:

Event(void) { init(); }

void init(void);

bool wait(int64_t timeout = -1);

void signal(void);

};

/*
  task.cpp - Smallest Non Preemptive Multi Task Library for Embedded System

  Copyright (c) 2022 Sasapea's Lab. All right reserved.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General
  Public License along with this library; if not, write to the
  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
  Boston, MA  02111-1307  USA
*/
#include <alloca.h>
#include "system.h"
#include "task.h"

Task::tcb_t *Task::_disp;
Task::tcb_t *Task::_curr;
Task::tcb_t *Task::_head;
Task::tcb_t *Task::_tail[];
size_t       Task::_used;

#if defined(JENNIC_CHIP_FAMILY_JN516x)

int Task::setjmp(jn516x_jmp_buf __jmpb)
{
  asm volatile ("b.sw 0x00(r3), r1");  /* SP (Stack pointer) */
  asm volatile ("b.sw 0x04(r3), r2");
  asm volatile ("b.sw 0x08(r3), r9");  /* LR (Link address register) */
  asm volatile ("b.sw 0x0C(r3), r10"); /* FP (Frame pointer) */
  asm volatile ("b.sw 0x10(r3), r11");
  asm volatile ("b.sw 0x14(r3), r12");
  asm volatile ("b.sw 0x18(r3), r13");
  asm volatile ("b.sw 0x1C(r3), r14");
  asm volatile ("b.sw 0x20(r3), r15");
  asm volatile ("b.sw 0x24(r3), r16");
  asm volatile ("b.sw 0x28(r3), r17");
  asm volatile ("b.sw 0x2C(r3), r18");
  asm volatile ("b.sw 0x30(r3), r19");
  asm volatile ("b.sw 0x34(r3), r20");
  asm volatile ("b.sw 0x38(r3), r21");
  asm volatile ("b.sw 0x3C(r3), r22");
  asm volatile ("b.sw 0x40(r3), r23");
  asm volatile ("b.sw 0x44(r3), r24");
  asm volatile ("b.sw 0x48(r3), r25");
  asm volatile ("b.sw 0x4C(r3), r26");
  asm volatile ("b.sw 0x50(r3), r27");
  asm volatile ("b.sw 0x54(r3), r28");
  asm volatile ("b.sw 0x58(r3), r29");
  asm volatile ("b.sw 0x5C(r3), r30");
  asm volatile ("b.sw 0x60(r3), r31");
  return 0;
}

void Task::longjmp(jn516x_jmp_buf __jmpb, int __retval)
{
  asm volatile ("b.lwz  r31, 0x60(r3)");
  asm volatile ("b.lwz  r30, 0x5C(r3)");
  asm volatile ("b.lwz  r29, 0x58(r3)");
  asm volatile ("b.lwz  r28, 0x54(r3)");
  asm volatile ("b.lwz  r27, 0x50(r3)");
  asm volatile ("b.lwz  r26, 0x4C(r3)");
  asm volatile ("b.lwz  r25, 0x48(r3)");
  asm volatile ("b.lwz  r24, 0x44(r3)");
  asm volatile ("b.lwz  r23, 0x40(r3)");
  asm volatile ("b.lwz  r22, 0x3C(r3)");
  asm volatile ("b.lwz  r21, 0x38(r3)");
  asm volatile ("b.lwz  r20, 0x34(r3)");
//asm volatile ("b.lwz  r19, 0x30(r3)"); /* CPU-BUG? */
  asm volatile ("b.lwz  r18, 0x2C(r3)");
//asm volatile ("b.lwz  r17, 0x28(r3)"); /* CPU-BUG? */
  asm volatile ("b.lwz  r16, 0x24(r3)");
  asm volatile ("b.lwz  r15, 0x20(r3)");
  asm volatile ("b.lwz  r14, 0x1C(r3)");
  asm volatile ("b.lwz  r13, 0x18(r3)");
  asm volatile ("b.lwz  r12, 0x14(r3)");
  asm volatile ("b.lwz  r11, 0x10(r3)");
  asm volatile ("b.lwz  r10, 0x0C(r3)"); /* FP (Frame pointer) */
  asm volatile ("b.lwz  r9,  0x08(r3)"); /* LR (Link address register) */
  asm volatile ("b.lwz  r2,  0x04(r3)");
  asm volatile ("b.lwz  r1,  0x00(r3)"); /* SP (Stack pointer) */
  asm volatile ("b.mov  r3,  r4");
}

#endif

void Task::begin(size_t size)
{
  /* Registration of main task */
  static tcb_t tcb;
  _curr = &tcb;
  _used = size;
}

bool Task::start(func_t func, void *arg, int size, void *stack)
{
  tcb_t *tcb;
  if (size < 0)
  {
    /* Use the current stack area */
    size_t n = - size;
    size  = _used;
    stack = 0;
    _used += n;
  }
  else if (stack)
  {
    /* Set the stack area to the variable area */
    size  = (char *)&tcb - (char *)stack - size + sizeof(*tcb);
    stack = 0;
  }
  else
  {
    /* Dynamically allocate stack area */
    stack = malloc(size);
    if (!stack)
      return false;
    size = (char *)&tcb - (char *)stack - size + sizeof(*tcb);
  }
  tcb = (tcb_t *)alloca(size);
  tcb->alloc = stack;
  tcb->arg   = arg;
  tcb->start = func;
  enqueue0(tcb);
  if (setjmp(tcb->context))
  {
    _curr = _disp;
    _curr->start(_curr->arg);
    stop();
  }
  return true;
}

void Task::stop(void)
{
  if (_curr->start)
  {
    if (_curr->alloc)
      free(_curr->alloc);
    dispatch();
  }
}

void Task::enqueue0(tcb_t *tcb)
{
  size_t save = System::disableAndSaveInterrupts();
  if (_tail[0])
  {
    tcb->next = _tail[0]->next;
    _tail[0]  = (_tail[0]->next = tcb);
  }
  else
  {
    tcb->next = _head;
    _tail[0]  = (_head = tcb);
  }
  System::restoreInterrupts(save);
}

void Task::enqueue1(tcb_t *tcb)
{
  size_t save = System::disableAndSaveInterrupts();
  (_tail[1] = *(_tail[1] ? &_tail[1]->next : (_tail[0] ? &_tail[0]->next : &_head)) = tcb)->next = 0;
  System::restoreInterrupts(save);
}

Task::tcb_t *Task::dequeue(void)
{
  tcb_t *tcb;
  size_t save = System::disableAndSaveInterrupts();
  if ((tcb = _head))
  {
    if (_head == _tail[0])
      _tail[0] = 0;
    else if (_head == _tail[1])
      _tail[1] = 0;
    _head = _head->next;
  }
  System::restoreInterrupts(save);
  return tcb;
}

size_t Task::current(void)
{
  return (size_t)_curr;
}

bool Task::yield(void)
{
  enqueue1(_curr);
  switching();
  return !_curr->start;
}

void Task::switching(void)
{
  if (!setjmp(_curr->context))
    dispatch();
  _curr = _disp;
}

void Task::dispatch(void)
{
  while (1)
  {
    if ((_disp = dequeue()))
      longjmp(_disp->context, 1);
#if defined(JENNIC_CHIP_FAMILY_JN516x)
    System::enableInterrupts();
    ::polling();
    System::cpuDoze();
#endif
  }
}

void Task::sleep(int32_t ms, uint8_t busyLoop)
{
  sleepMicroseconds(ms * 1000L, busyLoop);
}

void Task::sleepMicroseconds(int32_t us, uint8_t busyLoop)
{
  if (!_curr->start)
    System::delayMicroseconds(us, busyLoop);
  else
  {
    uint32 t = System::micros();
    if (us > busyLoop)
    {
      Sync sync;
      sync.init();
      sync.sleep(us -= busyLoop);
      t += us;
    }
    if (busyLoop)
    {
      while (System::micros() - t < busyLoop)
        continue;
    }
  }
}

void Task::sleepMicroseconds64(int64_t us, uint8_t busyLoop)
{
  if (!_curr->start)
    System::delayMicroseconds64(us, busyLoop);
  else
  {
    uint64 t = System::micros64();
    if (us > busyLoop)
    {
      Sync sync;
      sync.init();
      sync.sleep(us -= busyLoop);
      t += us;
    }
    if (busyLoop)
    {
      while (System::micros64() - t < busyLoop)
        continue;
    }
  }
}

void Task::delayMicroseconds(int32_t us, uint8 busyLoop)
{
  System::delayMicroseconds(us, busyLoop);
}

void Task::delayMicroseconds64(int64_t us, uint8_t busyLoop)
{
  System::delayMicroseconds64(us, busyLoop);
}

void Sync::init(void)
{
  _head = _tail = 0;
}

bool Sync::timeup(Alarm& alarm)
{
  if (((Sync *)alarm.param(0))->wakeup((size_t)alarm.param(1)))
    *(bool *)alarm.param(2) = false;
  return false;
}

bool Sync::sleep(ctrl_t& ctrl)
{
  tcb_t *tcb = (tcb_t *)current();
  (_tail = *(_tail ? &_tail->next : &_head) = tcb)->next = 0;
  System::restoreInterrupts(ctrl.save);
  ctrl.alarm.param(0, this);
  ctrl.alarm.param(1, tcb);
  ctrl.alarm.param(2, &ctrl.retval);
  ctrl.alarm.handler(timeup);
  AlarmTimer.start(ctrl.alarm);
  switching();
  return ctrl.retval;
}

bool Sync::sleep(int64_t timeout)
{
  if (timeout == 0)
    return false;
  ctrl_t ctrl;
  ctrl.alarm.interval(timeout);
  ctrl.retval = true;
  ctrl.save   = System::disableAndSaveInterrupts();
  return sleep(ctrl);
}

size_t Sync::wakeup(size_t task)
{
  tcb_t *tcb = 0;
  size_t save = System::disableAndSaveInterrupts();
  if (task)
  {
    for (tcb_t **p = &_head; *p; p = &(*p)->next)
    {
      if (*p == (tcb_t *)task)
      {
        if (!(*p = (*p)->next))
          _tail = (p == &_head ? 0 : (tcb_t *)p);
        tcb = (tcb_t *)task;
        break;
      }
    }
  }
  else
    _head = ((tcb = _head) != _tail ? _head->next : _tail = 0);
  System::restoreInterrupts(save);
  if (tcb)
  {
    AlarmTimer.cancel(1, tcb);
    enqueue0(tcb);
  }
  return (size_t)tcb;
}

void Mutex::init(void)
{
  Sync::init();
  _owner = 0;
}

bool Mutex::lock(int64_t timeout)
{
  ctrl_t ctrl;
  ctrl.alarm.interval(timeout);
  ctrl.retval = true;
  ctrl.save   = System::disableAndSaveInterrupts();
  size_t task = current();
  if (!_owner)
  {
    _owner = task;
    _count = 1;
  }
  else if (_owner == task)
    ++_count;
  else if (timeout == 0)
    ctrl.retval = false;
  else
    return sleep(ctrl);
  System::restoreInterrupts(ctrl.save);
  return ctrl.retval;
}

void Mutex::unlock(void)
{
  size_t save = System::disableAndSaveInterrupts();
  if (_owner == current())
  {
    if (--_count == 0)
    {
      _owner = wakeup();
      _count = 1;
    }
  }
  System::restoreInterrupts(save);
}

void Sem::init(size_t limit, size_t count)
{
  Sync::init();
  _limit = limit;
  _count = count;
  _wait  = 0;
}

bool Sem::wait(size_t count, int64_t timeout)
{
  ctrl_t ctrl;
  ctrl.alarm.interval(timeout);
  ctrl.retval = true;
  ctrl.save   = System::disableAndSaveInterrupts();
  if ((count == 0) || (count > _limit))
    ctrl.retval = false;
  else if (!_wait && (_count >= count))
    _count -= count;
  else if (timeout == 0)
    ctrl.retval = false;
  else
  {
    ++_wait;
    ((tcb_t *)current())->arg = (void *)count;
    sleep(ctrl);
    System::disableAndSaveInterrupts();
    --_wait;
    while (_head)
    {
      count = (size_t)_head->arg;
      if (_count < count)
        break;
      _count -= count;
      wakeup();
    }
  }
  System::restoreInterrupts(ctrl.save);
  return ctrl.retval;
}

void Sem::post(size_t count)
{
  size_t save = System::disableAndSaveInterrupts();
  if (count && (count + _count <= _limit))
  {
    _count += count;
    if (_head)
    {
      count = (size_t)_head->arg;
      if (_count >= count)
      {
        _count -= count;
        wakeup();
      }
    }
  }
  System::restoreInterrupts(save);
}

void Event::init(void)
{
  Sem::init(1);
}

bool Event::wait(int64_t timeout)
{
  return Sem::wait(1, timeout);
}

void Event::signal(void)
{
  Sem::post();
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

task.cpp - Smallest Non Preemptive Multi Task Library for Embedded System

This library is free software; you can redistribute it and/or

modify it under the terms of the GNU Lesser General Public

License as published by the Free Software Foundation; either

version 2.1 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,

but WITHOUT ANY WARRANTY; without even the implied warranty of

MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU

Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General

Public License along with this library; if not, write to the

Free Software Foundation, Inc., 59 Temple Place, Suite 330,

Boston, MA 02111-1307 USA

#include <alloca.h>

#include "system.h"

#include "task.h"

Task::tcb_t *Task::_disp;

Task::tcb_t *Task::_curr;

Task::tcb_t *Task::_head;

Task::tcb_t *Task::_tail[];

size_t Task::_used;

#if defined(JENNIC_CHIP_FAMILY_JN516x)

int Task::setjmp(jn516x_jmp_buf __jmpb)

{

asm volatile ("b.sw 0x00(r3), r1"); /* SP (Stack pointer) */

asm volatile ("b.sw 0x04(r3), r2");

asm volatile ("b.sw 0x08(r3), r9"); /* LR (Link address register) */

asm volatile ("b.sw 0x0C(r3), r10"); /* FP (Frame pointer) */

asm volatile ("b.sw 0x10(r3), r11");

asm volatile ("b.sw 0x14(r3), r12");

asm volatile ("b.sw 0x18(r3), r13");

asm volatile ("b.sw 0x1C(r3), r14");

asm volatile ("b.sw 0x20(r3), r15");

asm volatile ("b.sw 0x24(r3), r16");

asm volatile ("b.sw 0x28(r3), r17");

asm volatile ("b.sw 0x2C(r3), r18");

asm volatile ("b.sw 0x30(r3), r19");

asm volatile ("b.sw 0x34(r3), r20");

asm volatile ("b.sw 0x38(r3), r21");

asm volatile ("b.sw 0x3C(r3), r22");

asm volatile ("b.sw 0x40(r3), r23");

asm volatile ("b.sw 0x44(r3), r24");

asm volatile ("b.sw 0x48(r3), r25");

asm volatile ("b.sw 0x4C(r3), r26");

asm volatile ("b.sw 0x50(r3), r27");

asm volatile ("b.sw 0x54(r3), r28");

asm volatile ("b.sw 0x58(r3), r29");

asm volatile ("b.sw 0x5C(r3), r30");

asm volatile ("b.sw 0x60(r3), r31");

return 0;

}

void Task::longjmp(jn516x_jmp_buf __jmpb, int __retval)

{

asm volatile ("b.lwz r31, 0x60(r3)");

asm volatile ("b.lwz r30, 0x5C(r3)");

asm volatile ("b.lwz r29, 0x58(r3)");

asm volatile ("b.lwz r28, 0x54(r3)");

asm volatile ("b.lwz r27, 0x50(r3)");

asm volatile ("b.lwz r26, 0x4C(r3)");

asm volatile ("b.lwz r25, 0x48(r3)");

asm volatile ("b.lwz r24, 0x44(r3)");

asm volatile ("b.lwz r23, 0x40(r3)");

asm volatile ("b.lwz r22, 0x3C(r3)");

asm volatile ("b.lwz r21, 0x38(r3)");

asm volatile ("b.lwz r20, 0x34(r3)");

//asm volatile ("b.lwz r19, 0x30(r3)"); /* CPU-BUG? */

asm volatile ("b.lwz r18, 0x2C(r3)");

//asm volatile ("b.lwz r17, 0x28(r3)"); /* CPU-BUG? */

asm volatile ("b.lwz r16, 0x24(r3)");

asm volatile ("b.lwz r15, 0x20(r3)");

asm volatile ("b.lwz r14, 0x1C(r3)");

asm volatile ("b.lwz r13, 0x18(r3)");

asm volatile ("b.lwz r12, 0x14(r3)");

asm volatile ("b.lwz r11, 0x10(r3)");

asm volatile ("b.lwz r10, 0x0C(r3)"); /* FP (Frame pointer) */

asm volatile ("b.lwz r9, 0x08(r3)"); /* LR (Link address register) */

asm volatile ("b.lwz r2, 0x04(r3)");

asm volatile ("b.lwz r1, 0x00(r3)"); /* SP (Stack pointer) */

asm volatile ("b.mov r3, r4");

}

#endif

void Task::begin(size_t size)

{

/* Registration of main task */

static tcb_t tcb;

_curr = &tcb;

_used = size;

}

bool Task::start(func_t func, void *arg, int size, void *stack)

{

tcb_t *tcb;

if (size < 0)

{

/* Use the current stack area */

size_t n = - size;

size = _used;

stack = 0;

_used += n;

}

else if (stack)

{

/* Set the stack area to the variable area */

size = (char *)&tcb - (char *)stack - size + sizeof(*tcb);

stack = 0;

}

else

{

/* Dynamically allocate stack area */

stack = malloc(size);

if (!stack)

return false;

size = (char *)&tcb - (char *)stack - size + sizeof(*tcb);

}

tcb = (tcb_t *)alloca(size);

tcb->alloc = stack;

tcb->arg = arg;

tcb->start = func;

enqueue0(tcb);

if (setjmp(tcb->context))

{

_curr = _disp;

_curr->start(_curr->arg);

stop();

}

return true;

}

void Task::stop(void)

{

if (_curr->start)

{

if (_curr->alloc)

free(_curr->alloc);

dispatch();

}

void Task::enqueue0(tcb_t *tcb)

{

size_t save = System::disableAndSaveInterrupts();

if (_tail[0])

{

tcb->next = _tail[0]->next;

_tail[0] = (_tail[0]->next = tcb);

}

else

{

tcb->next = _head;

_tail[0] = (_head = tcb);

}

System::restoreInterrupts(save);

}

void Task::enqueue1(tcb_t *tcb)

{

size_t save = System::disableAndSaveInterrupts();

(_tail[1] = *(_tail[1] ? &_tail[1]->next : (_tail[0] ? &_tail[0]->next : &_head)) = tcb)->next = 0;

System::restoreInterrupts(save);

}

Task::tcb_t *Task::dequeue(void)

{

tcb_t *tcb;

size_t save = System::disableAndSaveInterrupts();

if ((tcb = _head))

{

if (_head == _tail[0])

_tail[0] = 0;

else if (_head == _tail[1])

_tail[1] = 0;

_head = _head->next;

}

System::restoreInterrupts(save);

return tcb;

}

size_t Task::current(void)

{

return (size_t)_curr;

}

bool Task::yield(void)

{

enqueue1(_curr);

switching();

return !_curr->start;

}

void Task::switching(void)

{

if (!setjmp(_curr->context))

dispatch();

_curr = _disp;

}

void Task::dispatch(void)

{

while (1)

{

if ((_disp = dequeue()))

longjmp(_disp->context, 1);

#if defined(JENNIC_CHIP_FAMILY_JN516x)

System::enableInterrupts();

::polling();

System::cpuDoze();

#endif

}

void Task::sleep(int32_t ms, uint8_t busyLoop)

{

sleepMicroseconds(ms * 1000L, busyLoop);

}

void Task::sleepMicroseconds(int32_t us, uint8_t busyLoop)

{

if (!_curr->start)

System::delayMicroseconds(us, busyLoop);

else

{

uint32 t = System::micros();

if (us > busyLoop)

{

Sync sync;

sync.init();

sync.sleep(us -= busyLoop);

t += us;

}

if (busyLoop)

{

while (System::micros() - t < busyLoop)

continue;

}

void Task::sleepMicroseconds64(int64_t us, uint8_t busyLoop)

{

if (!_curr->start)

System::delayMicroseconds64(us, busyLoop);

else

{

uint64 t = System::micros64();

if (us > busyLoop)

{

Sync sync;

sync.init();

sync.sleep(us -= busyLoop);

t += us;

}

if (busyLoop)

{

while (System::micros64() - t < busyLoop)

continue;

}

void Task::delayMicroseconds(int32_t us, uint8 busyLoop)

{

System::delayMicroseconds(us, busyLoop);

}

void Task::delayMicroseconds64(int64_t us, uint8_t busyLoop)

{

System::delayMicroseconds64(us, busyLoop);

}

void Sync::init(void)

{

_head = _tail = 0;

}

bool Sync::timeup(Alarm& alarm)

{

if (((Sync *)alarm.param(0))->wakeup((size_t)alarm.param(1)))

*(bool *)alarm.param(2) = false;

return false;

}

bool Sync::sleep(ctrl_t& ctrl)

{

tcb_t *tcb = (tcb_t *)current();

(_tail = *(_tail ? &_tail->next : &_head) = tcb)->next = 0;

System::restoreInterrupts(ctrl.save);

ctrl.alarm.param(0, this);

ctrl.alarm.param(1, tcb);

ctrl.alarm.param(2, &ctrl.retval);

ctrl.alarm.handler(timeup);

AlarmTimer.start(ctrl.alarm);

switching();

return ctrl.retval;

}

bool Sync::sleep(int64_t timeout)

{

if (timeout == 0)

return false;

ctrl_t ctrl;

ctrl.alarm.interval(timeout);

ctrl.retval = true;

ctrl.save = System::disableAndSaveInterrupts();

return sleep(ctrl);

}

size_t Sync::wakeup(size_t task)

{

tcb_t *tcb = 0;

size_t save = System::disableAndSaveInterrupts();

if (task)

{

for (tcb_t **p = &_head; *p; p = &(*p)->next)

{

if (*p == (tcb_t *)task)

{

if (!(*p = (*p)->next))

_tail = (p == &_head ? 0 : (tcb_t *)p);

tcb = (tcb_t *)task;

break;

}

else

_head = ((tcb = _head) != _tail ? _head->next : _tail = 0);

System::restoreInterrupts(save);

if (tcb)

{

AlarmTimer.cancel(1, tcb);

enqueue0(tcb);

}

return (size_t)tcb;

}

void Mutex::init(void)

{

Sync::init();

_owner = 0;

}

bool Mutex::lock(int64_t timeout)

{

ctrl_t ctrl;

ctrl.alarm.interval(timeout);

ctrl.retval = true;

ctrl.save = System::disableAndSaveInterrupts();

size_t task = current();

if (!_owner)

{

_owner = task;

_count = 1;

}

else if (_owner == task)

++_count;

else if (timeout == 0)

ctrl.retval = false;

else

return sleep(ctrl);

System::restoreInterrupts(ctrl.save);

return ctrl.retval;

}

void Mutex::unlock(void)

{

size_t save = System::disableAndSaveInterrupts();

if (_owner == current())

{

if (--_count == 0)

{

_owner = wakeup();

_count = 1;

}

System::restoreInterrupts(save);

}

void Sem::init(size_t limit, size_t count)

{

Sync::init();

_limit = limit;

_count = count;

_wait = 0;

}

bool Sem::wait(size_t count, int64_t timeout)

{

ctrl_t ctrl;

ctrl.alarm.interval(timeout);

ctrl.retval = true;

ctrl.save = System::disableAndSaveInterrupts();

if ((count == 0) || (count > _limit))

ctrl.retval = false;

else if (!_wait && (_count >= count))

_count -= count;

else if (timeout == 0)

ctrl.retval = false;

else

{

++_wait;

((tcb_t *)current())->arg = (void *)count;

sleep(ctrl);

System::disableAndSaveInterrupts();

--_wait;

while (_head)

{

count = (size_t)_head->arg;

if (_count < count)

break;

_count -= count;

wakeup();

}

System::restoreInterrupts(ctrl.save);

return ctrl.retval;

}

void Sem::post(size_t count)

{

size_t save = System::disableAndSaveInterrupts();

if (count && (count + _count <= _limit))

{

_count += count;

if (_head)

{

count = (size_t)_head->arg;

if (_count >= count)

{

_count -= count;

wakeup();

}

System::restoreInterrupts(save);

}

void Event::init(void)

{

Sem::init(1);

}

bool Event::wait(int64_t timeout)

{

return Sem::wait(1, timeout);

}

void Event::signal(void)

{

Sem::post();

}

コメントを残す コメントをキャンセル

コメントを残すコメントをキャンセル