SMP – Sasapea's Lab

前回投稿のライブラリを改良して Raspberry Pi Pico 用のSMPタスクスイッチングライブラリを作成してみた。

世界最速のタスク・スイッチ・ライブラリ (Task Switch Library for ARM-G++ [M0/M0+/M1])

コアの排他制御処理の分だけスイッチング速度は少し遅くなってしまったが、それでも1.0usは楽に切っている。

旧: RP2040(125MHz): 82 cycles (0.656us)

※2022-02-08に追加したタスク情報の保存／復旧処理のためにサイクル数が若干増えたがなんとか1.0us未満に収まった。

新: RP2040(125MHz): 104 cycles (0.832us)

[SMPで動作しているところ]
※PicoのSMPでは2タスクが同時に実行状態になる。

前回投稿のライブラリを改良しただけなので実装は比較的楽だったのだがPico特有の落とし穴もあった。arm系にはSysTickと呼ばれるシステムクロックで動作するタイマーがありマイクロ秒以下の時間測定には必要なものなのだがなぜかSDKでは未サポートらしい。とりあえずSysTickを使えるようにしてみたところSMPでは誤動作してしまうという現象が．．．

SysTickはコア毎に存在しており、実行コア側のSysTickにしかアクセスできない。SMPではタスクが実行されるコアが不定＝どのコアのSysTickにアクセスするかわからないということが原因だった。

例えば、次のコードではyield()実行前後で別のコアに切り替わることがある。micros()がSysTickで実装されていて各SysTickの同期がとれていないと仮定した場合、どのコアからアクセスされるかによりmicros()が返す時間情報が前後してしまうことになる。

void Task::delayMicroseconds(uint32_t us)
{
  uint32_t t = micros();    // <-- Core0
  do
    yield();
  while(micros() - t < us); // <-- Core1
}

void Task::delayMicroseconds(uint32_t us)

{

uint32_t t = micros(); // <-- Core0

yield();

while(micros() - t < us); // <-- Core1

}

この問題は、各SysTickを同時スタートすることで解決できる。

具体的な実装は次のメソッドのコードを見てほしい。ちなみに対策後に上の例のmicros()が返す時間情報が前後することがないか1日中ぶん回してみたが問題はなさそうだ。

void Task::begin([[maybe_unused]] bool multicore)
void Task::core1()

また、SysTickと同じくNVIC(割り込みコントローラ)もコア毎なので割り込み処理には特別な注意が必要となる。Picoだと割り込み処理を含むＨＷドライバー等をSMP対応するのは大変そうというか可能なら割り込みは使わない方が無難な気も．．．

ちなみに、優先度ベースのRTOSをSMP対応すると優先度が高いタスクの実行中に優先度が低いタスクが同時実行されることがありコア数が多くなるほど優先度ベースである意味が薄れてくることになる。力は技に勝るというべきか．．．(-_-;)

プログラムの書き込みは以前に投稿したUSBスイッチを使うとUSBの抜き差しなしで出来るので凄く便利だ。参考まで。

USBデバイスの電源を切るスイッチ

[pico_smp.cpp]

/**
 * SMP Task Switching Sample
 */
#include <stdio.h>
#include <pico/stdlib.h>
#include "task.h"

void task1(void)
{
  while (1)
  {
    printf("cpu%u: task1(%08X) %u\n", Task::cpuid(), Task::taskid(), time_us_32());
    Task::delay(1000);
  }
}

void task2(void)
{
  while (1)
  {
    printf("cpu%u: task2(%08X) %u\n", Task::cpuid(), Task::taskid(), time_us_32());
    Task::delay(1000);
  }
}

void setup(void)
{
  printf("MultiCore Task Switching\n");
  Task::begin();
  Task::start(task1);
  Task::start(task2);
}

void loop(void)
{
  Task::yield();
}

int main()
{
  stdio_init_all();
  setup();
  while (1)
    loop();
  return 0;
}

/**

* SMP Task Switching Sample

#include <stdio.h>

#include <pico/stdlib.h>

#include "task.h"

void task1(void)

{

while (1)

{

printf("cpu%u: task1(%08X) %u\n", Task::cpuid(), Task::taskid(), time_us_32());

Task::delay(1000);

}

void task2(void)

{

while (1)

{

printf("cpu%u: task2(%08X) %u\n", Task::cpuid(), Task::taskid(), time_us_32());

Task::delay(1000);

}

void setup(void)

{

printf("MultiCore Task Switching\n");

Task::begin();

Task::start(task1);

Task::start(task2);

}

void loop(void)

{

Task::yield();

}

int main()

{

stdio_init_all();

setup();

while (1)

loop();

return 0;

}

[timer_sample.cpp]

/**
 * Timer Sample
 */
#include <stdio.h>
#include <pico/stdlib.h>
#include "timer.h" or "task.h"

#define LED_PIN 25

Alarm alarm;

bool timeup(Alarm& alarm)
{
  gpio_xor_mask(1 << LED_PIN);
  return true;
}

void setup(void)
{
  gpio_init(LED_PIN);
  gpio_set_dir(LED_PIN, GPIO_OUT);

  Timer::begin(); or Task::begin();

  Timer.start(alarm.interval(1000000).handler(timeup));
}

void loop(void)
{
}

int main()
{
  stdio_init_all();
  setup();
  while (1)
    loop();
  return 0;
}

/**

* Timer Sample

#include <stdio.h>

#include <pico/stdlib.h>

#include "timer.h" or "task.h"

#define LED_PIN 25

Alarm alarm;

bool timeup(Alarm& alarm)

{

gpio_xor_mask(1 << LED_PIN);

return true;

}

void setup(void)

{

gpio_init(LED_PIN);

gpio_set_dir(LED_PIN, GPIO_OUT);

Timer::begin(); or Task::begin();

Timer.start(alarm.interval(1000000).handler(timeup));

}

void loop(void)

{

}

int main()

{

stdio_init_all();

setup();

while (1)

loop();

return 0;

}

[update]
2022-02-18
PCEventクラスの仕様変更とチューニング、及び、Task::taskid()の型をvoid *からsize_tに変更。

2022-02-17
GPIOを監視するためのライブラリ(pcevent.h/pcevent.cpp)を追加。

PCEventクラスはGPIO割込を管理し割込ハンドラの呼び出しや指定のピン状態になるまでタスクを休止することができる。またキャプチャー処理のためのエッジ割込の時間(time_us_32)も取得可能だ。ピンの割込のみ管理しているので利用する前にピンを適切に初期化しておく必要がある。PCEventクラスを利用することでマルチタスク環境にてピンの監視処理を効率よく行うことが可能となる。

※Pico-SDKのGPIO割り込み機能とは排他的に利用可能であることに注意！。

PCEvent.wait(pin, PCEVENT_LEVEL_LOW, timeout);

1	PCEvent.wait(pin, PCEVENT_LEVEL_LOW, timeout);

タイムアウト或いはピンがLOWになるまで休止する。既にLOWの場合は即座に戻る。

PCEvent.wait(pin, PCEVENT_LEVEL_HIGH, timeout);

1	PCEvent.wait(pin, PCEVENT_LEVEL_HIGH, timeout);

タイムアウト或いはピンがHIGHになるまで休止する。既にHIGHの場合は即座に戻る。

PCEvent.wait(pin, PCEVENT_EDGE_FALL, timeout);

1	PCEvent.wait(pin, PCEVENT_EDGE_FALL, timeout);

タイムアウト或いは次回のピンの立下りまで休止する。

PCEvent.wait(pin, PCEVENT_EDGE_RISE, timeout);

1	PCEvent.wait(pin, PCEVENT_EDGE_RISE, timeout);

タイムアウト或いは次回のピンの立上がりまで休止する。

PCEvent.wait(pin, PCEVENT_EDGE_BOTH, timeout);

1	PCEvent.wait(pin, PCEVENT_EDGE_BOTH, timeout);

タイムアウト或いは次回のピンの立下り或いは立上がりまで休止する。

※タイムアウトの指定値がゼロの場合は状態を見て即座に戻り、マイナス値の場合は無限に、ゼロより大きい場合は指定時間まで待つ。
※戻り値がSYNC_TIMEOUTEDならタイムアウト発生、それ以外はイベント待ち完了。

2022-02-08
全タスクが休止するときに誤動作するという致命的なバグがあったので修正。それとタスク固有の識別子やデータを管理できるようにしてみた。

size_t Task::taskid(void);

1	size_t Task::taskid(void);

タスク識別子の取得。

void *Task::data(void);

1	void *Task::data(void);

タスク固有データの取得。Task::start(void *data)で指定したデータが取得できる。

void stop(void);

1	void stop(void);

割当てされたメモリを解放しタスクを終了する。タスク関数からのリターン、或いは、この関数の呼び出しにより何時でもタスク終了することができる。

bool start(void (&func)(void), void *data = nullptr, size_t size = DEFAULT_STACK_SIZE, void *stack = nullptr);

1	bool start(void (&func)(void), void data = nullptr, size_t size = DEFAULT_STACK_SIZE, void stack = nullptr);

Task::start() … タスク固有データの指定に加えタスクのスタック領域のための静的メモリの指定もできるようにしてみた。

2022-02-07
Event/Sem/Mutexクラスのタイミング・バグがまだ残っていたので再改修。これで完璧．．．かも。(-_-;)

2022-02-06
Event/Sem/Mutexクラスのタイミング・バグがあまりに酷すぎたので全面的に改修。

2022-02-05
Eventクラスのバグを修正。おまけとしてSemaphoreクラスとMutexクラスを追加。但し、デバッグはしてないので注意すべし。

2022-02-04
シンプルかつ単機能なEventクラスを追加してみた。

template<size_t MASK = SIZE_MAX, bool WAITALL = false> class Event

1	template<size_t MASK = SIZE_MAX, bool WAITALL = false> class Event

Eventテンプレートクラス。マスク値(MASK)と待ち方(WAITALL)が指定可能。

int wait(int64_t timeout = -1)

1	int wait(int64_t timeout = -1)

イベント発生まで休止する。タイムアウト指定が可能。

void post(uint8_t event = 0)

1	void post(uint8_t event = 0)

イベント通知する。複数タスクが待っている場合は休止順に起床する。event引数には0-31までのビット番号を指定する。

2022-01-13
Alarmクラスの仕様変更とそれに伴う修正を行った。

2022-01-12
タイマークラスの仕様変更と、Sync::sleep()の戻り値でタイムアウト判定が出来るようにしてみた。

2022-01-11
全体的な最適化とsleep()のタイムアウト機能がタスクとの関連性がないというヘンな仕様だったので修正。

2022-01-10
Pico-SDKのタイマーライブラリはコア依存があり扱いが面倒であったため、コアに依然せずに使えるマルチコア対応のタイマーライブラリを作成してみた。これに伴い、Pico-SDKのタイマーライブラリを利用していたコードを削除するとともに時間指定する関数は全てマイクロ秒で統一してみた。
新たなタイマーライブラリはTimer::begin()を呼び出したコア側でのみ割り込み処理を行うが、実行コアを気にせずに使えることやアラーム数が無制限であること、及び、正確なリピート時間を特徴としている。

2021-12-14
SysTick::micros()を割り込み禁止状態からの呼び出しにも対応。

2021-12-10
タスク休止／起床方式のdelay()/delayMicroseconds()のオーバーヘッドが大きく時間が少し不正確な感じなので名称を変更しsleep()/sleepMicroseconds()として新たに関数を追加。今まで通りポーリング方式のdelay()/delayMicroseconds()も利用可能。待つ時間が長い場合はsleep、短い時間であればdelayのように使い分けするのが良さそうだ。

2021-12-09
Task::delay()/Task::dealyMicroseconds()をポーリングによる実装からPico-SDKのalarm機能を利用してタスク休止／起床する実装に改良してみた。おまけとしてget_alarm_pool()を追加。get_alarm_pool()によりSMP環境で実行コアに対して適切なalarm_poolを取得することができるようになる。それと、Sync::sleep()がコンパイラ最適化によりインライン展開されると誤動作するためインライン展開させないように修正。

[Library]

/*
  task.h - SMP Task Switching Library for Paspberry Pi Pico (RP2040)

  Copyright (c) 2021 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(PICO_ON_DEVICE)
  #define RP2040 1
  #define NUMBER_OF_CORES 2
  #include "systick.h"
  #include "timer.h"
#else
  #define NUMBER_OF_CORES 1
#endif

#define DEFAULT_STACK_SIZE 2048

class Task
{
  public:

    static void begin(bool multicore = true);
    static bool start(void (&func)(void), void *data = nullptr, size_t size = DEFAULT_STACK_SIZE, void *stack = nullptr);
    static void stop(void) __attribute__ ((noinline));
    static void yield(void) __attribute__ ((naked));
    static uint32_t cpuid(void);
    static size_t taskid(void);
    static void *data(void);

#if RP2040
    static void sleep(int64_t us);
    static void delay(uint32_t us);
#endif

  protected:

    typedef struct
    {
      void *data;
      void *free;
    } task_t;

    typedef size_t reg_t;

    typedef struct context
    {
      reg_t _r8;
      reg_t _r9;
      reg_t _sl;
      reg_t _fp;
      reg_t _rsv;            // r0
      reg_t _task;           // r1
      struct context *_next; // r2
      reg_t _primask;        // r3
      reg_t _r4;
      reg_t _r5;
      reg_t _r6;
      reg_t _r7;
      reg_t _lr;
    } context_t;

    typedef struct
    {
      context_t *head;
      context_t *tail;
    } queue_t;

    static void invoke(void) __attribute__ ((naked));
    static void dispatch(void) __attribute__ ((naked));
    static void append(context_t *head, context_t *tail);
    static void prepend(context_t *head, context_t *tail);

    static task_t *_task[NUMBER_OF_CORES];

  private:

    static void begin1(void) __attribute__ ((naked));

    static queue_t _queue;
};

#define SYNC_WAKEUPED      0
#define SYNC_TIMEOUTED     -1
#define SYNC_BLOCKED 0x80000000

class Sync : protected Task
{
  public:

    Sync(void) : _queue({}), _wakeup(0) {}
    virtual ~Sync(void) { wakeup(true); }
#if RP2040
    int sleep(int64_t timeout = -1, uint32_t save = 0);
#else
    int sleep(uint32_t save = 0) __attribute__((noinline));
#endif
    void wakeup(bool all = false, bool prepend = false);

  protected:

#if RP2040
    static bool alarm_handler(Alarm& alarm);
    void timeout(Alarm& alarm);
    int sleep0(Alarm& alarm, uint32_t save) __attribute__((noinline));
#endif
    bool wakeup0(bool all = false, bool prepend = false);
    bool wakeup0(size_t taskid);
    size_t wakeuped(bool confirmed = false);
    static void lock(void);
    static void unlock(void);
    static uint32_t blocking(void);
    static void unblocking(uint32_t save);

  private:

    queue_t _queue;
    size_t  _wakeup;
};

template<size_t MASK = SIZE_MAX, bool WAITALL = false>
class Event : protected Sync
{
  public:

    Event(void)
    : _events(0)
    {
    }

    virtual ~Event(void) {}

    int wait(int64_t timeout = -1)
    {
      int rv = SYNC_TIMEOUTED;
      uint32_t save = blocking();
      if (!wakeuped() && match())
      {
        _events = 0;
        rv = SYNC_WAKEUPED;
      }
      else if (timeout)
      {
#if RP2040
        rv = sleep(timeout, SYNC_BLOCKED | save);
#else
        rv = sleep(SYNC_BLOCKED | save);
#endif
        blocking();
        if (rv == SYNC_WAKEUPED)
        {
          wakeuped(true);
          if (match())
          {
            if (wakeup0())
              _events = 0;
          }
        }
      }
      unblocking(save);
      return rv;
    }

    void post(uint8_t event = 0)
    {
      uint32_t save = blocking();
      _events |= (1 << event) & MASK;
      if (match())
      {
        if (wakeup0())
          _events = 0;
      }
      unblocking(save);
    }

  private:

    bool match(void)
    {
      return WAITALL ? _events == MASK : _events != 0;
    }

    size_t _events;
};

class Sem : protected Sync
{
  public:

    Sem(size_t init = 1, size_t limit = 1)
    : _count(init)
    , _limit(SIZE_MAX)
    {
      if (limit)
        _limit = limit;
    }

    virtual ~Sem(void) {}

    int acquire(int64_t timeout = -1)
    {
      int rv = SYNC_TIMEOUTED;
      uint32_t save = blocking();
      if (!wakeuped() && _count)
      {
        --_count;
        rv = SYNC_WAKEUPED;
      }
      else if (timeout)
      {
#if RP2040
        rv = sleep(timeout, SYNC_BLOCKED | save);
#else
        rv = sleep(SYNC_BLOCKED | save);
#endif
        blocking();
        if (rv == SYNC_WAKEUPED)
        {
          wakeuped(true);
          if (_count)
          {
            if (wakeup0())
              --_count;
          }
        }
      }
      unblocking(save);
      return rv;
    }

    bool release(void)
    {
      uint32_t save = blocking();
      bool rv = _count < _limit;
      if (rv)
      {
        if (!wakeup0())
          ++_count;
      }
      unblocking(save);
      return rv;
    }
    
  private:

    size_t _limit;
    size_t _count;
};

class Mutex : protected Sync
{
  public:

    Mutex(void)
    : _count(0)
    , _owner(0)
    {
    }

    virtual ~Mutex(void) {}

    int lock(int64_t timeout = -1)
    {
      int rv;
      uint32_t save = blocking();
      if (_count >= SIZE_MAX)
        rv = SYNC_TIMEOUTED;
      else if (!wakeuped() && ((_owner == taskid()) || (_count == 0)))
      {
        ++_count;
        _owner = taskid();
        rv = SYNC_WAKEUPED;
      }
      else if (timeout)
      {
#if RP2040
        rv = sleep(timeout, SYNC_BLOCKED | save);
#else
        rv = sleep(SYNC_BLOCKED | save);
#endif
        blocking();
        if (rv == SYNC_WAKEUPED)
        {
          wakeuped(true);
          ++_count;
          _owner = taskid();
        }
      }
      unblocking(save);
      return rv; 
    }

    size_t unlock(void)
    {
      size_t rv = SIZE_MAX;
      uint32_t save = blocking();
      if ((_owner == taskid()) && _count)
      {
        rv = --_count;
        if (rv == 0)
          wakeup0();
      }
      unblocking(save);
      return rv;
    }

  private:

    size_t _count;
    size_t _owner;
};

#if RP2040
  #include "pcevent.h"
#endif

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

task.h - SMP Task Switching Library for Paspberry Pi Pico (RP2040)

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(PICO_ON_DEVICE)

#define RP2040 1

#define NUMBER_OF_CORES 2

#include "systick.h"

#include "timer.h"

#else

#define NUMBER_OF_CORES 1

#endif

#define DEFAULT_STACK_SIZE 2048

class Task

{

public:

static void begin(bool multicore = true);

static bool start(void (&func)(void), void *data = nullptr, size_t size = DEFAULT_STACK_SIZE, void *stack = nullptr);

static void stop(void) __attribute__ ((noinline));

static void yield(void) __attribute__ ((naked));

static uint32_t cpuid(void);

static size_t taskid(void);

static void *data(void);

#if RP2040

static void sleep(int64_t us);

static void delay(uint32_t us);

#endif

protected:

typedef struct

{

void *data;

void *free;

} task_t;

typedef size_t reg_t;

typedef struct context

{

reg_t _r8;

reg_t _r9;

reg_t _sl;

reg_t _fp;

reg_t _rsv; // r0

reg_t _task; // r1

struct context *_next; // r2

reg_t _primask; // r3

reg_t _r4;

reg_t _r5;

reg_t _r6;

reg_t _r7;

reg_t _lr;

} context_t;

typedef struct

{

context_t *head;

context_t *tail;

} queue_t;

static void invoke(void) __attribute__ ((naked));

static void dispatch(void) __attribute__ ((naked));

static void append(context_t *head, context_t *tail);

static void prepend(context_t *head, context_t *tail);

static task_t *_task[NUMBER_OF_CORES];

private:

static void begin1(void) __attribute__ ((naked));

static queue_t _queue;

};

#define SYNC_WAKEUPED 0

#define SYNC_TIMEOUTED -1

#define SYNC_BLOCKED 0x80000000

class Sync : protected Task

{

public:

Sync(void) : _queue({}), _wakeup(0) {}

virtual ~Sync(void) { wakeup(true); }

#if RP2040

int sleep(int64_t timeout = -1, uint32_t save = 0);

#else

int sleep(uint32_t save = 0) __attribute__((noinline));

#endif

void wakeup(bool all = false, bool prepend = false);

protected:

#if RP2040

static bool alarm_handler(Alarm& alarm);

void timeout(Alarm& alarm);

int sleep0(Alarm& alarm, uint32_t save) __attribute__((noinline));

#endif

bool wakeup0(bool all = false, bool prepend = false);

bool wakeup0(size_t taskid);

size_t wakeuped(bool confirmed = false);

static void lock(void);

static void unlock(void);

static uint32_t blocking(void);

static void unblocking(uint32_t save);

private:

queue_t _queue;

size_t _wakeup;

};

template<size_t MASK = SIZE_MAX, bool WAITALL = false>

class Event : protected Sync

{

public:

Event(void)

: _events(0)

{

}

virtual ~Event(void) {}

int wait(int64_t timeout = -1)

{

int rv = SYNC_TIMEOUTED;

uint32_t save = blocking();

if (!wakeuped() && match())

{

_events = 0;

rv = SYNC_WAKEUPED;

}

else if (timeout)

{

#if RP2040

rv = sleep(timeout, SYNC_BLOCKED | save);

#else

rv = sleep(SYNC_BLOCKED | save);

#endif

blocking();

if (rv == SYNC_WAKEUPED)

{

wakeuped(true);

if (match())

{

if (wakeup0())

_events = 0;

}

unblocking(save);

return rv;

}

void post(uint8_t event = 0)

{

uint32_t save = blocking();

_events |= (1 << event) & MASK;

if (match())

{

if (wakeup0())

_events = 0;

}

unblocking(save);

}

private:

bool match(void)

{

return WAITALL ? _events == MASK : _events != 0;

}

size_t _events;

};

class Sem : protected Sync

{

public:

Sem(size_t init = 1, size_t limit = 1)

: _count(init)

, _limit(SIZE_MAX)

{

if (limit)

_limit = limit;

}

virtual ~Sem(void) {}

int acquire(int64_t timeout = -1)

{

int rv = SYNC_TIMEOUTED;

uint32_t save = blocking();

if (!wakeuped() && _count)

{

--_count;

rv = SYNC_WAKEUPED;

}

else if (timeout)

{

#if RP2040

rv = sleep(timeout, SYNC_BLOCKED | save);

#else

rv = sleep(SYNC_BLOCKED | save);

#endif

blocking();

if (rv == SYNC_WAKEUPED)

{

wakeuped(true);

if (_count)

{

if (wakeup0())

--_count;

}

unblocking(save);

return rv;

}

bool release(void)

{

uint32_t save = blocking();

bool rv = _count < _limit;

if (rv)

{

if (!wakeup0())

++_count;

}

unblocking(save);

return rv;

}

private:

size_t _limit;

size_t _count;

};

class Mutex : protected Sync

{

public:

Mutex(void)

: _count(0)

, _owner(0)

{

}

virtual ~Mutex(void) {}

int lock(int64_t timeout = -1)

{

int rv;

uint32_t save = blocking();

if (_count >= SIZE_MAX)

rv = SYNC_TIMEOUTED;

else if (!wakeuped() && ((_owner == taskid()) || (_count == 0)))

{

++_count;

_owner = taskid();

rv = SYNC_WAKEUPED;

}

else if (timeout)

{

#if RP2040

rv = sleep(timeout, SYNC_BLOCKED | save);

#else

rv = sleep(SYNC_BLOCKED | save);

#endif

blocking();

if (rv == SYNC_WAKEUPED)

{

wakeuped(true);

++_count;

_owner = taskid();

}

unblocking(save);

return rv;

}

size_t unlock(void)

{

size_t rv = SIZE_MAX;

uint32_t save = blocking();

if ((_owner == taskid()) && _count)

{

rv = --_count;

if (rv == 0)

wakeup0();

}

unblocking(save);

return rv;

}

private:

size_t _count;

size_t _owner;

};

#if RP2040

#include "pcevent.h"

#endif

/*
  task.cpp - SMP Task Switching Library for Paspberry Pi Pico (RP2040)
  
  Copyright (c) 2021 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 <stdlib.h>
#include <string.h>
#include "task.h"

#define DECLARE_CRITICAL uint32_t __primask__

#if RP2040
  #include <pico/stdlib.h>
  #include <pico/multicore.h>
  #if !defined(THREAD_LOCK_ID_TASK)
    #if defined(PICO_SPINLOCK_ID_OS1)  // Pico SDK 1.2
      #define THREAD_LOCK_ID_TASK PICO_SPINLOCK_ID_OS1
    #else
      #define THREAD_LOCK_ID_TASK 14
    #endif
  #endif
  #if !defined(THREAD_LOCK_ID_SYNC)
    #if defined(PICO_SPINLOCK_ID_OS2)  // Pico SDK 1.2
      #define THREAD_LOCK_ID_SYNC PICO_SPINLOCK_ID_OS2
    #else
      #define THREAD_LOCK_ID_SYNC 15
    #endif
  #endif
  #define THREAD_LOCK(lock_num)    spin_lock_unsafe_blocking(spin_lock_instance(lock_num))
  #define THREAD_UNLOCK(lock_num)  spin_unlock_unsafe(spin_lock_instance(lock_num))
  #define ENTER_CRITICAL(lock_num) __primask__ = spin_lock_blocking(spin_lock_instance(lock_num))
  #define LEAVE_CRITICAL(lock_num) spin_unlock(spin_lock_instance(lock_num), __primask__)
#else
  #define THREAD_LOCK_ID_TASK 0
  #define THREAD_LOCK_ID_SYNC 0
  #define THREAD_LOCK(lock_num)
  #define THREAD_UNLOCK(lock_num)
  #define ENTER_CRITICAL(lock_num) \
    ASM("mrs %0, PRIMASK" : "=r" (__primask__)); \
    ASM("cpsid i")
  #define LEAVE_CRITICAL(lock_num) \
    ASM("msr PRIMASK, %0" :: "r" (__primask__))
#endif

#define ASM __asm__ volatile

#if RP2040

#define SAVE_CONTEXT_RETURN_LR() \
/* 3*/  ASM("mov   r0, %0"       ::  "r" (_task) : "r0"); \
/* 1*/  ASM("mov   r1, %0"       ::  "I" ((SIO_BASE + SIO_CPUID_OFFSET) >> 24) : "r1"); \
/* 1*/  ASM("lsl   r1, #24"      ::: "r1"); \
/* 2*/  ASM("ldr   r1, [r1]"     ::: "r1"); \
/* 1*/  ASM("lsl   r1, #2"       ::: "r1"); \
/* 2*/  ASM("ldr   r1, [r0, r1]" ::: "r1"); \
/* 1*/  ASM("mov   r2, #0"       ::: "r2"); \
/* 3*/  ASM("mrs   r3, PRIMASK"  ::: "r3"); \
/*10*/  ASM("push  {r0-r7, lr}"  :::     ); \
/* 1*/  ASM("mov   r0, r8"       ::: "r0"); \
/* 1*/  ASM("mov   r1, r9"       ::: "r1"); \
/* 1*/  ASM("mov   r2, sl"       ::: "r2"); \
/* 1*/  ASM("mov   r3, fp"       ::: "r3"); \
/* 5*/  ASM("push  {r0-r3}")
/*33*/

#define SAVE_CONTEXT_RETURN_R3() \
/* 2*/  ASM("push  {r3}"); \
/* 3*/  ASM("mov   r0, %0"       ::  "r" (_task) : "r0"); \
/* 1*/  ASM("mov   r1, %0"       ::  "I" ((SIO_BASE + SIO_CPUID_OFFSET) >> 24) : "r1"); \
/* 1*/  ASM("lsl   r1, #24"      ::: "r1"); \
/* 2*/  ASM("ldr   r1, [r1]"     ::: "r1"); \
/* 1*/  ASM("lsl   r1, #2"       ::: "r1"); \
/* 2*/  ASM("ldr   r1, [r0, r1]" ::: "r1"); \
/* 1*/  ASM("mov   r2, #0"       ::: "r2"); \
/* 3*/  ASM("mrs   r3, PRIMASK"  ::: "r3"); \
/* 9*/  ASM("push  {r0-r7}"      :::     ); \
/* 1*/  ASM("mov   r0, r8"       ::: "r0"); \
/* 1*/  ASM("mov   r1, r9"       ::: "r1"); \
/* 1*/  ASM("mov   r2, sl"       ::: "r2"); \
/* 1*/  ASM("mov   r3, fp"       ::: "r3"); \
/* 5*/  ASM("push  {r0-r3}")
/*34*/

#define LOAD_CONTEXT() \
/* 2*/  ASM("ldr   r0, [sp, %0]" :: "I" (offsetof(context_t, _primask)) : "r0"); \
/* 3*/  ASM("msr   PRIMASK, r0"); \
/* 5*/  ASM("pop   {r0-r3}"      ::: "r0", "r1", "r2", "r3"); \
/* 1*/  ASM("mov   r8, r0"       ::: "r8"); \
/* 1*/  ASM("mov   r9, r1"       ::: "r9"); \
/* 1*/  ASM("mov   sl, r2"       ::: "sl"); \
/* 1*/  ASM("mov   fp, r3"       ::: "fp"); \
/* 9*/  ASM("pop   {r0-r7}"      ::: "r0", "r1", "r2", "r3", "r4", "r5", "r6", "r7"); \
/* 1*/  ASM("mov   r2, %0"       ::  "I" ((SIO_BASE + SIO_CPUID_OFFSET) >> 24) : "r2"); \
/* 1*/  ASM("lsl   r2, #24"      ::: "r2"); \
/* 2*/  ASM("ldr   r2, [r2]"     ::: "r2"); \
/* 1*/  ASM("lsl   r2, #2"       ::: "r2"); \
/* 2*/  ASM("str   r1, [r0, r2]" :::     ); \
/* 2*/  ASM("pop   {pc}")
/*32*/

#else

#define SAVE_CONTEXT_RETURN_LR() \
/* 3*/  ASM("mov   r0, %0"       ::  "r" (_task) : "r0"); \
/* 2*/  ASM("ldr   r1, [r0]"     ::: "r1"); \
/* 1*/  ASM("mov   r2, #0"       ::: "r2"); \
/* 3*/  ASM("mrs   r3, PRIMASK"  ::: "r3"); \
/*10*/  ASM("push  {r0-r7, lr}"); \
/* 1*/  ASM("mov   r0, r8"       ::: "r0"); \
/* 1*/  ASM("mov   r1, r9"       ::: "r1"); \
/* 1*/  ASM("mov   r2, sl"       ::: "r2"); \
/* 1*/  ASM("mov   r3, fp"       ::: "r3"); \
/* 5*/  ASM("push  {r0-r3}")
/*28*/

#define SAVE_CONTEXT_RETURN_R3() \
/* 2*/  ASM("push  {r3}"); \
/* 3*/  ASM("mov   r0, %0"       ::  "r" (_task) : "r0"); \
/* 2*/  ASM("ldr   r1, [r0]"     ::: "r1"); \
/* 1*/  ASM("mov   r2, #0"       ::: "r2"); \
/* 3*/  ASM("mrs   r3, PRIMASK"  ::: "r3"); \
/* 9*/  ASM("push  {r0-r7}"); \
/* 1*/  ASM("mov   r0, r8"       ::: "r0"); \
/* 1*/  ASM("mov   r1, r9"       ::: "r1"); \
/* 1*/  ASM("mov   r2, sl"       ::: "r2"); \
/* 1*/  ASM("mov   r3, fp"       ::: "r3"); \
/* 5*/  ASM("push  {r0-r3}")
/*29*/

#define LOAD_CONTEXT() \
/* 2*/  ASM("ldr   r0, [sp, %0]" :: "I" (offsetof(context_t, _primask)) : "r0"); \
/* 3*/  ASM("msr   PRIMASK, r0"); \
/* 5*/  ASM("pop   {r0-r3}"      ::: "r0", "r1", "r2", "r3"); \
/* 1*/  ASM("mov   r8, r0"       ::: "r8"); \
/* 1*/  ASM("mov   r9, r1"       ::: "r9"); \
/* 1*/  ASM("mov   sl, r2"       ::: "sl"); \
/* 1*/  ASM("mov   fp, r3"       ::: "fp"); \
/* 9*/  ASM("pop   {r0-r7}"      ::: "r0", "r1", "r2", "r3", "r4", "r5", "r6", "r7"); \
/* 2*/  ASM("str   r1, [r0]"); \
/* 2*/  ASM("pop   {pc}")
/*27*/

#endif

Task::queue_t Task::_queue = {};
Task::task_t *Task::_task[NUMBER_OF_CORES] = {};

static volatile bool sync_wait = false;
static uint8_t def_stack[NUMBER_OF_CORES][DEFAULT_STACK_SIZE];// __attribute__((__aligned__(256)));

uint32_t Task::cpuid(void)
{
#if RP2040
  return get_core_num();
#else
  return 0;
#endif
}

size_t Task::taskid(void)
{
  return (size_t)_task[cpuid()];
}

void *Task::data(void)
{
  task_t *task = _task[cpuid()];
  return task ? task->data : nullptr;
}

void Task::begin([[maybe_unused]] bool multicore)
{
#if RP2040
  THREAD_UNLOCK(THREAD_LOCK_ID_SYNC);
  THREAD_UNLOCK(THREAD_LOCK_ID_TASK);
  Timer.begin();
  PCEvent.begin();
  if (multicore)
  {
    //
    // disable interrupt
    //
    ASM("cpsid i");
    //
    // start core1
    //
    sync_wait = false;
    multicore_launch_core1(begin1);
    //
    // wait for synchronizing
    //
    while (!sync_wait)
      continue;
    //
    // set event (wakeup core1)
    //
    ASM("sev");
    //
    // start systick
    //
    SysTick::begin();
    //
    // enable interrupt
    //
    ASM("cpsie i");
  }
  else
    SysTick::begin();
#endif
}

void Task::begin1(void)
{
  //
  // disable interrupt
  //
  ASM("cpsid i");
  //
  // clear event
  //
  ASM("sev");
  ASM("wfe");
  //
  // wait event
  //
  sync_wait = true;
  ASM("wfe");
  //
  // start systick
  //
  SysTick::begin();
  //
  // enable interrupt
  //
  ASM("cpsie i");
  //
  // dispatch next task
  //
  dispatch();
}

//
// task switching
//
//   Number of cycles from function call to return
//
//   [When compiled for release mode]
//
//     ATSAMD21(48MHz)
//       77 cycles (1.604us)
//     RP2040(125MHz)
//      104 cycles (0.832us)
//
void Task::yield(void) // (3)
{
  //
  // save context (28/33)
  //
  SAVE_CONTEXT_RETURN_LR();
  //
  // thread lock (1/10)
  //
  ASM("cpsid i");
  THREAD_LOCK(THREAD_LOCK_ID_TASK);
  //
  // check head link (6)
  //
  context_t *tail, *head = _queue.head;
  if (head)
  {
    //
    // update task queue (12)
    //
    ASM("mov  %0, sp" : "=r" (tail));
    _queue.tail = _queue.tail->_next = tail;
    _queue.head = _queue.head->_next;
    ASM("mov  sp, %0" :: "r" (head));
  }
  //
  // thread unlock (0/8)
  //
  THREAD_UNLOCK(THREAD_LOCK_ID_TASK);
  //
  // load context (27/32)
  //
  LOAD_CONTEXT();
}

bool Task::start(void (&func)(void), void *data, size_t size, void *stack)
{
  //
  // allocate task stack area
  //
  task_t *task = (task_t *)stack;
  if (task == nullptr)
  {
    task = (task_t *)malloc(size += sizeof(task_t) + 3);
    if (task == nullptr)
      return false;
  }
  task->data = data;
  task->free = stack ? nullptr : task;
  //
  // save context
  //
  void *sp;
  context_t *ctx = (context_t *)(((size_t)task + size) & ~3);
  ASM("mov  %0, sp" :  "=r" (sp));
  ASM("mov  sp, %0" :: "r" (ctx));
  ASM("push {%0}"   :: "r" (func));
  ASM("mov  r3, %0" :: "r" (invoke) : "r3");
  SAVE_CONTEXT_RETURN_R3();
  ASM("mov  %0, sp" :  "=r" (ctx));
  ASM("mov  sp, %0" :: "r" (sp));
  //
  // set task info
  //
  ctx->_task = (reg_t)task;
  //
  // append tail link
  //
  append(ctx, ctx);
  return true;
}

void Task::invoke(void)
{
  //
  // load task parameter
  //
  ASM("pop  {r0}");
  //
  // call task method
  //
  ASM("blx  r0");
  //
  // stop
  //
  stop();
}

void Task::stop(void)
{
  task_t *task = (task_t *)taskid();
  if (task)
  {
    //
    // change internal stack
    //
    ASM("mov sp, %0" :: "r" (def_stack[cpuid() + 1]));
    //
    // free stack area
    //
    if (task->free)
      free(task->free);
    //
    // dispatch next task
    //
    dispatch();
  }
}

void Task::dispatch(void)
{
  while (1)
  {
    //
    // thread lock
    //
    ASM("cpsid i");
    THREAD_LOCK(THREAD_LOCK_ID_TASK);
    //
    // check task queue
    //
    context_t *ctx = _queue.head;
    if (ctx)
    {
      //
      // dequeue
      //
      if ((_queue.head = ctx->_next) == nullptr)
        _queue.tail = nullptr;
      //
      // thread unlock
      //
      THREAD_UNLOCK(THREAD_LOCK_ID_TASK);
      //
      // load context
      //
      ASM("mov  sp, %0" :: "r" (ctx));
      LOAD_CONTEXT();
      //
      // Dummy code to avoid compilation errors
      //
      return;
    }
    //
    // thread unlock
    //
    THREAD_UNLOCK(THREAD_LOCK_ID_TASK);
    ASM("cpsie i");
    //
    // wait event
    //
    ASM("wfe");
    ASM("nop");
  }
}

void Task::append(context_t *head, context_t *tail)
{
  DECLARE_CRITICAL;
  ENTER_CRITICAL(THREAD_LOCK_ID_TASK);
  *(_queue.tail ? &_queue.tail->_next : &_queue.head) = head;
  _queue.tail = tail;
  tail->_next = nullptr;
  LEAVE_CRITICAL(THREAD_LOCK_ID_TASK);
  ASM("sev");
}

void Task::prepend(context_t *head, context_t *tail)
{
  DECLARE_CRITICAL;
  ENTER_CRITICAL(THREAD_LOCK_ID_TASK);
  if ((tail->_next = _queue.head) == nullptr)
    _queue.tail = tail;
  _queue.head = head;
  LEAVE_CRITICAL(THREAD_LOCK_ID_TASK);
  ASM("sev");
}

#if RP2040

void Task::delay(uint32_t us)
{
  uint32_t t = time_us_32();
  do
    yield();
  while(time_us_32() - t < us);
}

void Task::sleep(int64_t us)
{
  Sync sync;
  sync.sleep(us);
}

bool Sync::alarm_handler(Alarm& alarm)
{
  ((Sync *)alarm.params(0))->timeout(alarm);
  return false;
}

void Sync::timeout(Alarm& alarm)
{
  context_t *ctx = (context_t *)alarm.params(1);
  uint32_t save = blocking();
  for (context_t **p = &_queue.head; *p; p = &(*p)->_next)
  {
    if (*p == ctx)
    {
      *p = (*p)->_next;
      if (_queue.head == nullptr)
        _queue.tail = nullptr;
      Task::append(ctx, ctx);
      alarm.params(1, nullptr);
      break;
    }
  }
  unblocking(save);
}

int Sync::sleep(int64_t timeout, uint32_t save)
{
  Alarm alarm;
  return sleep0(alarm.interval(timeout), save);
}

int Sync::sleep0(Alarm& alarm, uint32_t save)
#else
int Sync::sleep(uint32_t save)
#endif
{
  context *ctx;
  //
  // check timeout
  //
#if RP2040
  if (alarm.interval() == 0)
    return SYNC_TIMEOUTED;
#endif
  //
  // save context
  //
  ASM("ldr r3, =9f + 1" ::: "r3");
  SAVE_CONTEXT_RETURN_R3();
  ASM("mov %0, sp" : "=r" (ctx));
  //
  // blocking
  //
  if (save & SYNC_BLOCKED)
  {
    save &= ~SYNC_BLOCKED;
    ctx->_primask = save;
  }
  else
    save = blocking();
  //
  // append tail link
  //
  _queue.tail = *(_queue.tail ? &_queue.tail->_next : &_queue.head) = ctx;
  //
  // start timer
  //
#if RP2040
  alarm.handler(alarm_handler).params(0, this).params(1, ctx);
  if (alarm.interval() > 0)
    Timer.start(alarm);
#endif
  //
  // change internal stack
  //
  ASM("mov sp, %0" :: "r" (def_stack[cpuid() + 1]));
  //
  // unblocking
  //
  unblocking(save);
  //
  // dispatch next task
  //
  Task::dispatch();
  //
  // execute prologue code
  //
  ASM("9:");
#if RP2040
  return alarm.params(1) ? SYNC_WAKEUPED : SYNC_TIMEOUTED;
#else
  return SYNC_WAKEUPED;
#endif
}

void Sync::wakeup(bool all, bool prepend)
{
  uint32_t save = blocking();
  wakeup0(all, prepend);
  unblocking(save);
}

bool Sync::wakeup0(bool all, bool prepend)
{
  context_t *head = _queue.head;
  if (head)
  {
    ++_wakeup;
    context_t *tail;
    if (all)
    {
      tail = _queue.tail;
      _queue.head = _queue.tail = nullptr;
    }
    else
    {
      tail = head;
      if ((_queue.head = head->_next) == nullptr)
        _queue.tail = nullptr;
    }
#if RP2040
    for (context_t *p = head;; p = p->_next)
    {
      Timer.cancel(1, p);
      if (p == tail)
        break;
    }
#endif
    if (prepend)
      Task::prepend(head, tail);
    else
      Task::append(head, tail);
    return true;
  }
  return false;
}

bool Sync::wakeup0(size_t taskid)
{
  for (context_t **p = &_queue.head; *p; p = &(*p)->_next)
  {
    context_t *q = *p;
    if (q->_task == taskid)
    {
      if (((*p = q->_next) == nullptr) && (p == &_queue.head))
        _queue.tail = nullptr;
      Timer.cancel(1, q);
      Task::append(q, q);
      return true;
    }
  }
  return false;
}

size_t Sync::wakeuped(bool confirmed)
{
  size_t rv = _wakeup;
  if (rv && confirmed)
    --_wakeup;
  return rv;
}

void Sync::lock(void)
{
  THREAD_LOCK(THREAD_LOCK_ID_SYNC);
}

void Sync::unlock(void)
{
  THREAD_UNLOCK(THREAD_LOCK_ID_SYNC);
}

uint32_t Sync::blocking(void)
{
  DECLARE_CRITICAL;
  ENTER_CRITICAL(THREAD_LOCK_ID_SYNC);
  return __primask__;
}

void Sync::unblocking(DECLARE_CRITICAL)
{
  LEAVE_CRITICAL(THREAD_LOCK_ID_SYNC);
}

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

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

575

576

577

578

579

580

581

582

583

584

585

586

587

588

589

590

591

592

593

594

595

596

597

598

599

600

601

602

603

604

605

606

607

608

609

610

611

612

613

614

615

616

617

618

619

620

621

622

623

624

625

626

627

628

629

630

631

632

633

634

635

636

637

638

639

640

641

642

643

644

645

task.cpp - SMP Task Switching Library for Paspberry Pi Pico (RP2040)

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 <stdlib.h>

#include <string.h>

#include "task.h"

#define DECLARE_CRITICAL uint32_t __primask__

#if RP2040

#include <pico/stdlib.h>

#include <pico/multicore.h>

#if !defined(THREAD_LOCK_ID_TASK)

#if defined(PICO_SPINLOCK_ID_OS1) // Pico SDK 1.2

#define THREAD_LOCK_ID_TASK PICO_SPINLOCK_ID_OS1

#else

#define THREAD_LOCK_ID_TASK 14

#endif

#if !defined(THREAD_LOCK_ID_SYNC)

#if defined(PICO_SPINLOCK_ID_OS2) // Pico SDK 1.2

#define THREAD_LOCK_ID_SYNC PICO_SPINLOCK_ID_OS2

#else

#define THREAD_LOCK_ID_SYNC 15

#endif

#define THREAD_LOCK(lock_num) spin_lock_unsafe_blocking(spin_lock_instance(lock_num))

#define THREAD_UNLOCK(lock_num) spin_unlock_unsafe(spin_lock_instance(lock_num))

#define ENTER_CRITICAL(lock_num) __primask__ = spin_lock_blocking(spin_lock_instance(lock_num))

#define LEAVE_CRITICAL(lock_num) spin_unlock(spin_lock_instance(lock_num), __primask__)

#else

#define THREAD_LOCK_ID_TASK 0

#define THREAD_LOCK_ID_SYNC 0

#define THREAD_LOCK(lock_num)

#define THREAD_UNLOCK(lock_num)

#define ENTER_CRITICAL(lock_num) \

ASM("mrs %0, PRIMASK" : "=r" (__primask__)); \

ASM("cpsid i")

#define LEAVE_CRITICAL(lock_num) \

ASM("msr PRIMASK, %0" :: "r" (__primask__))

#endif

#define ASM __asm__ volatile

#if RP2040

#define SAVE_CONTEXT_RETURN_LR() \

/* 3*/ ASM("mov r0, %0" :: "r" (_task) : "r0"); \

/* 1*/ ASM("mov r1, %0" :: "I" ((SIO_BASE + SIO_CPUID_OFFSET) >> 24) : "r1"); \

/* 1*/ ASM("lsl r1, #24" ::: "r1"); \

/* 2*/ ASM("ldr r1, [r1]" ::: "r1"); \

/* 1*/ ASM("lsl r1, #2" ::: "r1"); \

/* 2*/ ASM("ldr r1, [r0, r1]" ::: "r1"); \

/* 1*/ ASM("mov r2, #0" ::: "r2"); \

/* 3*/ ASM("mrs r3, PRIMASK" ::: "r3"); \

/*10*/ ASM("push {r0-r7, lr}" ::: ); \

/* 1*/ ASM("mov r0, r8" ::: "r0"); \

/* 1*/ ASM("mov r1, r9" ::: "r1"); \

/* 1*/ ASM("mov r2, sl" ::: "r2"); \

/* 1*/ ASM("mov r3, fp" ::: "r3"); \

/* 5*/ ASM("push {r0-r3}")

/*33*/

#define SAVE_CONTEXT_RETURN_R3() \

/* 2*/ ASM("push {r3}"); \

/* 3*/ ASM("mov r0, %0" :: "r" (_task) : "r0"); \

/* 1*/ ASM("mov r1, %0" :: "I" ((SIO_BASE + SIO_CPUID_OFFSET) >> 24) : "r1"); \

/* 1*/ ASM("lsl r1, #24" ::: "r1"); \

/* 2*/ ASM("ldr r1, [r1]" ::: "r1"); \

/* 1*/ ASM("lsl r1, #2" ::: "r1"); \

/* 2*/ ASM("ldr r1, [r0, r1]" ::: "r1"); \

/* 1*/ ASM("mov r2, #0" ::: "r2"); \

/* 3*/ ASM("mrs r3, PRIMASK" ::: "r3"); \

/* 9*/ ASM("push {r0-r7}" ::: ); \

/* 1*/ ASM("mov r0, r8" ::: "r0"); \

/* 1*/ ASM("mov r1, r9" ::: "r1"); \

/* 1*/ ASM("mov r2, sl" ::: "r2"); \

/* 1*/ ASM("mov r3, fp" ::: "r3"); \

/* 5*/ ASM("push {r0-r3}")

/*34*/

#define LOAD_CONTEXT() \

/* 2*/ ASM("ldr r0, [sp, %0]" :: "I" (offsetof(context_t, _primask)) : "r0"); \

/* 3*/ ASM("msr PRIMASK, r0"); \

/* 5*/ ASM("pop {r0-r3}" ::: "r0", "r1", "r2", "r3"); \

/* 1*/ ASM("mov r8, r0" ::: "r8"); \

/* 1*/ ASM("mov r9, r1" ::: "r9"); \

/* 1*/ ASM("mov sl, r2" ::: "sl"); \

/* 1*/ ASM("mov fp, r3" ::: "fp"); \

/* 9*/ ASM("pop {r0-r7}" ::: "r0", "r1", "r2", "r3", "r4", "r5", "r6", "r7"); \

/* 1*/ ASM("mov r2, %0" :: "I" ((SIO_BASE + SIO_CPUID_OFFSET) >> 24) : "r2"); \

/* 1*/ ASM("lsl r2, #24" ::: "r2"); \

/* 2*/ ASM("ldr r2, [r2]" ::: "r2"); \

/* 1*/ ASM("lsl r2, #2" ::: "r2"); \

/* 2*/ ASM("str r1, [r0, r2]" ::: ); \

/* 2*/ ASM("pop {pc}")

/*32*/

#else

#define SAVE_CONTEXT_RETURN_LR() \

/* 3*/ ASM("mov r0, %0" :: "r" (_task) : "r0"); \

/* 2*/ ASM("ldr r1, [r0]" ::: "r1"); \

/* 1*/ ASM("mov r2, #0" ::: "r2"); \

/* 3*/ ASM("mrs r3, PRIMASK" ::: "r3"); \

/*10*/ ASM("push {r0-r7, lr}"); \

/* 1*/ ASM("mov r0, r8" ::: "r0"); \

/* 1*/ ASM("mov r1, r9" ::: "r1"); \

/* 1*/ ASM("mov r2, sl" ::: "r2"); \

/* 1*/ ASM("mov r3, fp" ::: "r3"); \

/* 5*/ ASM("push {r0-r3}")

/*28*/

#define SAVE_CONTEXT_RETURN_R3() \

/* 2*/ ASM("push {r3}"); \

/* 3*/ ASM("mov r0, %0" :: "r" (_task) : "r0"); \

/* 2*/ ASM("ldr r1, [r0]" ::: "r1"); \

/* 1*/ ASM("mov r2, #0" ::: "r2"); \

/* 3*/ ASM("mrs r3, PRIMASK" ::: "r3"); \

/* 9*/ ASM("push {r0-r7}"); \

/* 1*/ ASM("mov r0, r8" ::: "r0"); \

/* 1*/ ASM("mov r1, r9" ::: "r1"); \

/* 1*/ ASM("mov r2, sl" ::: "r2"); \

/* 1*/ ASM("mov r3, fp" ::: "r3"); \

/* 5*/ ASM("push {r0-r3}")

/*29*/

#define LOAD_CONTEXT() \

/* 2*/ ASM("ldr r0, [sp, %0]" :: "I" (offsetof(context_t, _primask)) : "r0"); \

/* 3*/ ASM("msr PRIMASK, r0"); \

/* 5*/ ASM("pop {r0-r3}" ::: "r0", "r1", "r2", "r3"); \

/* 1*/ ASM("mov r8, r0" ::: "r8"); \

/* 1*/ ASM("mov r9, r1" ::: "r9"); \

/* 1*/ ASM("mov sl, r2" ::: "sl"); \

/* 1*/ ASM("mov fp, r3" ::: "fp"); \

/* 9*/ ASM("pop {r0-r7}" ::: "r0", "r1", "r2", "r3", "r4", "r5", "r6", "r7"); \

/* 2*/ ASM("str r1, [r0]"); \

/* 2*/ ASM("pop {pc}")

/*27*/

#endif

Task::queue_t Task::_queue = {};

Task::task_t *Task::_task[NUMBER_OF_CORES] = {};

static volatile bool sync_wait = false;

static uint8_t def_stack[NUMBER_OF_CORES][DEFAULT_STACK_SIZE];// __attribute__((__aligned__(256)));

uint32_t Task::cpuid(void)

{

#if RP2040

return get_core_num();

#else

return 0;

#endif

}

size_t Task::taskid(void)

{

return (size_t)_task[cpuid()];

}

void *Task::data(void)

{

task_t *task = _task[cpuid()];

return task ? task->data : nullptr;

}

void Task::begin([[maybe_unused]] bool multicore)

{

#if RP2040

THREAD_UNLOCK(THREAD_LOCK_ID_SYNC);

THREAD_UNLOCK(THREAD_LOCK_ID_TASK);

Timer.begin();

PCEvent.begin();

if (multicore)

{

// disable interrupt

ASM("cpsid i");

// start core1

sync_wait = false;

multicore_launch_core1(begin1);

// wait for synchronizing

while (!sync_wait)

continue;

// set event (wakeup core1)

ASM("sev");

// start systick

SysTick::begin();

// enable interrupt

ASM("cpsie i");

}

else

SysTick::begin();

#endif

}

void Task::begin1(void)

{

// disable interrupt

ASM("cpsid i");

// clear event

ASM("sev");

ASM("wfe");

// wait event

sync_wait = true;

ASM("wfe");

// start systick

SysTick::begin();

// enable interrupt

ASM("cpsie i");

// dispatch next task

dispatch();

}

// task switching

// Number of cycles from function call to return

// [When compiled for release mode]

// ATSAMD21(48MHz)

// 77 cycles (1.604us)

// RP2040(125MHz)

// 104 cycles (0.832us)

void Task::yield(void) // (3)

{

// save context (28/33)

SAVE_CONTEXT_RETURN_LR();

// thread lock (1/10)

ASM("cpsid i");

THREAD_LOCK(THREAD_LOCK_ID_TASK);

// check head link (6)

context_t *tail, *head = _queue.head;

if (head)

{

// update task queue (12)

ASM("mov %0, sp" : "=r" (tail));

_queue.tail = _queue.tail->_next = tail;

_queue.head = _queue.head->_next;

ASM("mov sp, %0" :: "r" (head));

}

// thread unlock (0/8)

THREAD_UNLOCK(THREAD_LOCK_ID_TASK);

// load context (27/32)

LOAD_CONTEXT();

}

bool Task::start(void (&func)(void), void *data, size_t size, void *stack)

{

// allocate task stack area

task_t *task = (task_t *)stack;

if (task == nullptr)

{

task = (task_t *)malloc(size += sizeof(task_t) + 3);

if (task == nullptr)

return false;

}

task->data = data;

task->free = stack ? nullptr : task;

// save context

void *sp;

context_t *ctx = (context_t *)(((size_t)task + size) & ~3);

ASM("mov %0, sp" : "=r" (sp));

ASM("mov sp, %0" :: "r" (ctx));

ASM("push {%0}" :: "r" (func));

ASM("mov r3, %0" :: "r" (invoke) : "r3");

SAVE_CONTEXT_RETURN_R3();

ASM("mov %0, sp" : "=r" (ctx));

ASM("mov sp, %0" :: "r" (sp));

// set task info

ctx->_task = (reg_t)task;

// append tail link

append(ctx, ctx);

return true;

}

void Task::invoke(void)

{

// load task parameter

ASM("pop {r0}");

// call task method

ASM("blx r0");

// stop

stop();

}

void Task::stop(void)

{

task_t *task = (task_t *)taskid();

if (task)

{

// change internal stack

ASM("mov sp, %0" :: "r" (def_stack[cpuid() + 1]));

// free stack area

if (task->free)

free(task->free);

// dispatch next task

dispatch();

}

void Task::dispatch(void)

{

while (1)

{

// thread lock

ASM("cpsid i");

THREAD_LOCK(THREAD_LOCK_ID_TASK);

// check task queue

context_t *ctx = _queue.head;

if (ctx)

{

// dequeue

if ((_queue.head = ctx->_next) == nullptr)

_queue.tail = nullptr;

// thread unlock

THREAD_UNLOCK(THREAD_LOCK_ID_TASK);

// load context

ASM("mov sp, %0" :: "r" (ctx));

LOAD_CONTEXT();

// Dummy code to avoid compilation errors

return;

}

// thread unlock

THREAD_UNLOCK(THREAD_LOCK_ID_TASK);

ASM("cpsie i");

// wait event

ASM("wfe");

ASM("nop");

}

void Task::append(context_t *head, context_t *tail)

{

DECLARE_CRITICAL;

ENTER_CRITICAL(THREAD_LOCK_ID_TASK);

*(_queue.tail ? &_queue.tail->_next : &_queue.head) = head;

_queue.tail = tail;

tail->_next = nullptr;

LEAVE_CRITICAL(THREAD_LOCK_ID_TASK);

ASM("sev");

}

void Task::prepend(context_t *head, context_t *tail)

{

DECLARE_CRITICAL;

ENTER_CRITICAL(THREAD_LOCK_ID_TASK);

if ((tail->_next = _queue.head) == nullptr)

_queue.tail = tail;

_queue.head = head;

LEAVE_CRITICAL(THREAD_LOCK_ID_TASK);

ASM("sev");

}

#if RP2040

void Task::delay(uint32_t us)

{

uint32_t t = time_us_32();

yield();

while(time_us_32() - t < us);

}

void Task::sleep(int64_t us)

{

Sync sync;

sync.sleep(us);

}

bool Sync::alarm_handler(Alarm& alarm)

{

((Sync *)alarm.params(0))->timeout(alarm);

return false;

}

void Sync::timeout(Alarm& alarm)

{

context_t *ctx = (context_t *)alarm.params(1);

uint32_t save = blocking();

for (context_t **p = &_queue.head; *p; p = &(*p)->_next)

{

if (*p == ctx)

{

*p = (*p)->_next;

if (_queue.head == nullptr)

_queue.tail = nullptr;

Task::append(ctx, ctx);

alarm.params(1, nullptr);

break;

}

unblocking(save);

}

int Sync::sleep(int64_t timeout, uint32_t save)

{

Alarm alarm;

return sleep0(alarm.interval(timeout), save);

}

int Sync::sleep0(Alarm& alarm, uint32_t save)

#else

int Sync::sleep(uint32_t save)

#endif

{

context *ctx;

// check timeout

#if RP2040

if (alarm.interval() == 0)

return SYNC_TIMEOUTED;

#endif

// save context

ASM("ldr r3, =9f + 1" ::: "r3");

SAVE_CONTEXT_RETURN_R3();

ASM("mov %0, sp" : "=r" (ctx));

// blocking

if (save & SYNC_BLOCKED)

{

save &= ~SYNC_BLOCKED;

ctx->_primask = save;

}

else

save = blocking();

// append tail link

_queue.tail = *(_queue.tail ? &_queue.tail->_next : &_queue.head) = ctx;

// start timer

#if RP2040

alarm.handler(alarm_handler).params(0, this).params(1, ctx);

if (alarm.interval() > 0)

Timer.start(alarm);

#endif

// change internal stack

ASM("mov sp, %0" :: "r" (def_stack[cpuid() + 1]));

// unblocking

unblocking(save);

// dispatch next task

Task::dispatch();

// execute prologue code

ASM("9:");

#if RP2040

return alarm.params(1) ? SYNC_WAKEUPED : SYNC_TIMEOUTED;

#else

return SYNC_WAKEUPED;

#endif

}

void Sync::wakeup(bool all, bool prepend)

{

uint32_t save = blocking();

wakeup0(all, prepend);

unblocking(save);

}

bool Sync::wakeup0(bool all, bool prepend)

{

context_t *head = _queue.head;

if (head)

{

++_wakeup;

context_t *tail;

if (all)

{

tail = _queue.tail;

_queue.head = _queue.tail = nullptr;

}

else

{

tail = head;

if ((_queue.head = head->_next) == nullptr)

_queue.tail = nullptr;

}

#if RP2040

for (context_t *p = head;; p = p->_next)

{

Timer.cancel(1, p);

if (p == tail)

break;

}

#endif

if (prepend)

Task::prepend(head, tail);

else

Task::append(head, tail);

return true;

}

return false;

}

bool Sync::wakeup0(size_t taskid)

{

for (context_t **p = &_queue.head; *p; p = &(*p)->_next)

{

context_t *q = *p;

if (q->_task == taskid)

{

if (((*p = q->_next) == nullptr) && (p == &_queue.head))

_queue.tail = nullptr;

Timer.cancel(1, q);

Task::append(q, q);

return true;

}

return false;

}

size_t Sync::wakeuped(bool confirmed)

{

size_t rv = _wakeup;

if (rv && confirmed)

--_wakeup;

return rv;

}

void Sync::lock(void)

{

THREAD_LOCK(THREAD_LOCK_ID_SYNC);

}

void Sync::unlock(void)

{

THREAD_UNLOCK(THREAD_LOCK_ID_SYNC);

}

uint32_t Sync::blocking(void)

{

DECLARE_CRITICAL;

ENTER_CRITICAL(THREAD_LOCK_ID_SYNC);

return __primask__;

}

void Sync::unblocking(DECLARE_CRITICAL)

{

LEAVE_CRITICAL(THREAD_LOCK_ID_SYNC);

}

/*
  systick.h - SysTick Library for Raspberry Pi Pico (RP2040)

  Copyright (c) 2021 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 <stdint.h>
#include <stdbool.h>
#include "task.h"

#define SYSTICK_CLOCK    125 // 125MHz
#define SYSTICK_TIME_US 1000 // 1000us
#define SYSTICK_RELOAD (SYSTICK_CLOCK * SYSTICK_TIME_US - 1)

class SysTick
{
  private:

    typedef struct
    {
      uint32_t counts;
      uint32_t micros;
    } overflow_t;

    static volatile overflow_t _overflow[NUMBER_OF_CORES];

    friend void _isr_systick(void);

  public:

    static void begin(void);
    static uint32_t raw(void);
    static uint32_t micros(void);
    static uint32_t millis(void);
};

systick.h - SysTick Library for Raspberry Pi Pico (RP2040)

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 <stdint.h>

#include <stdbool.h>

#include "task.h"

#define SYSTICK_CLOCK 125 // 125MHz

#define SYSTICK_TIME_US 1000 // 1000us

#define SYSTICK_RELOAD (SYSTICK_CLOCK * SYSTICK_TIME_US - 1)

class SysTick

{

private:

typedef struct

{

uint32_t counts;

uint32_t micros;

} overflow_t;

static volatile overflow_t _overflow[NUMBER_OF_CORES];

friend void _isr_systick(void);

public:

static void begin(void);

static uint32_t raw(void);

static uint32_t micros(void);

static uint32_t millis(void);

};

/*
  systick.cpp - SysTick Library for Raspberry Pi Pico (RP2040)

  Copyright (c) 2021 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 <hardware/structs/systick.h>
#include <hardware/regs/m0plus.h>
#include "systick.h"

volatile SysTick::overflow_t SysTick::_overflow[NUMBER_OF_CORES] = {};

inline
void _isr_systick(void)
{
  volatile SysTick::overflow_t *ovf = &SysTick::_overflow[Task::cpuid()];
  (void)systick_hw->csr;
  ovf->counts += 1;
  ovf->micros += SYSTICK_TIME_US;
}

extern "C"
void isr_systick(void)
{
  _isr_systick();
}

void SysTick::begin(void)
{
  systick_hw->csr = 0; // disable SysTick
  *(io_rw_32 *)(&systick_hw->cvr) = M0PLUS_SYST_CVR_CURRENT_RESET; // reset counter to zero.
  systick_hw->rvr = SYSTICK_RELOAD;
  systick_hw->csr = M0PLUS_SYST_CSR_ENABLE_BITS | M0PLUS_SYST_CSR_TICKINT_BITS | M0PLUS_SYST_CSR_CLKSOURCE_BITS;
}

uint32_t SysTick::raw(void)
{
  return systick_hw->cvr;
}

uint32_t SysTick::micros(void)
{
  uint32_t cvr, cnt;
  volatile overflow_t *ovf = &_overflow[Task::cpuid()];
  while (1)
  {
    cnt = ovf->micros;
    cvr = systick_hw->cvr;
    if (systick_hw->csr & M0PLUS_SYST_CSR_COUNTFLAG_BITS)
    {
      ovf->counts += 1;
      ovf->micros += SYSTICK_TIME_US;
    }
    else if (cnt == ovf->micros)
      break;
  }
  return cnt + (SYSTICK_RELOAD - cvr) / SYSTICK_CLOCK;
}

uint32_t SysTick::millis(void)
{
#if (SYSTICK_TIME % 1000 == 0)
  return _overflow[Task::cpuid()].counts * (SYSTICK_TIME_US / 1000);
#else
  return (uint64_t)_overflow[Task::cpuid()].counts * 1000 / SYSTICK_TIME_US;
#endif
}

systick.cpp - SysTick Library for Raspberry Pi Pico (RP2040)

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 <hardware/structs/systick.h>

#include <hardware/regs/m0plus.h>

#include "systick.h"

volatile SysTick::overflow_t SysTick::_overflow[NUMBER_OF_CORES] = {};

inline

void _isr_systick(void)

{

volatile SysTick::overflow_t *ovf = &SysTick::_overflow[Task::cpuid()];

(void)systick_hw->csr;

ovf->counts += 1;

ovf->micros += SYSTICK_TIME_US;

}

extern "C"

void isr_systick(void)

{

_isr_systick();

}

void SysTick::begin(void)

{

systick_hw->csr = 0; // disable SysTick

*(io_rw_32 *)(&systick_hw->cvr) = M0PLUS_SYST_CVR_CURRENT_RESET; // reset counter to zero.

systick_hw->rvr = SYSTICK_RELOAD;

systick_hw->csr = M0PLUS_SYST_CSR_ENABLE_BITS | M0PLUS_SYST_CSR_TICKINT_BITS | M0PLUS_SYST_CSR_CLKSOURCE_BITS;

}

uint32_t SysTick::raw(void)

{

return systick_hw->cvr;

}

uint32_t SysTick::micros(void)

{

uint32_t cvr, cnt;

volatile overflow_t *ovf = &_overflow[Task::cpuid()];

while (1)

{

cnt = ovf->micros;

cvr = systick_hw->cvr;

if (systick_hw->csr & M0PLUS_SYST_CSR_COUNTFLAG_BITS)

{

ovf->counts += 1;

ovf->micros += SYSTICK_TIME_US;

}

else if (cnt == ovf->micros)

break;

}

return cnt + (SYSTICK_RELOAD - cvr) / SYSTICK_CLOCK;

}

uint32_t SysTick::millis(void)

{

#if (SYSTICK_TIME % 1000 == 0)

return _overflow[Task::cpuid()].counts * (SYSTICK_TIME_US / 1000);

#else

return (uint64_t)_overflow[Task::cpuid()].counts * 1000 / SYSTICK_TIME_US;

#endif

}

/*
  timer.h - Timer Library for Paspberry Pi Pico (RP2040)
  
  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 <stdint.h>
#include <stdbool.h>
#include <hardware/platform_defs.h>

class Alarm;

typedef bool (*alarm_handler_t)(Alarm& alarm);

class Alarm
{
  protected:

    Alarm *_next;
    uint64_t _alarm;
    int64_t _interval;
    alarm_handler_t _handler;
    void *_params[4];

    friend class TimerClass;

  public:

    Alarm(void);
    virtual ~Alarm(void);
    Alarm& interval(int64_t interval, bool fromnow = true);
    int64_t interval(void);
    Alarm& handler(alarm_handler_t handler);
    alarm_handler_t handler(void);
    Alarm& params(int num, void *val);
    void *params(int num = 0);
};

class TimerClass
{
  protected:

    int _timer_num;
    Alarm *_alarm_list;

    static void timeup0(void);
    void timeup(void);
    bool add(Alarm& alarm);
    void restart(void);
    inline static uint32_t blocking(void);
    inline static void unblocking(uint32_t save);

  public:

    TimerClass(void);
    virtual ~TimerClass(void);
    int begin(int timer_num = -1);
    void end(void);
    bool start(Alarm& alarm);
    void cancel(Alarm& alarm);
    void cancel(int num, void *arg);
};

extern TimerClass Timer;

timer.h - Timer Library for Paspberry Pi Pico (RP2040)

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 <stdint.h>

#include <stdbool.h>

#include <hardware/platform_defs.h>

class Alarm;

typedef bool (*alarm_handler_t)(Alarm& alarm);

class Alarm

{

protected:

Alarm *_next;

uint64_t _alarm;

int64_t _interval;

alarm_handler_t _handler;

void *_params[4];

friend class TimerClass;

public:

Alarm(void);

virtual ~Alarm(void);

Alarm& interval(int64_t interval, bool fromnow = true);

int64_t interval(void);

Alarm& handler(alarm_handler_t handler);

alarm_handler_t handler(void);

Alarm& params(int num, void *val);

void *params(int num = 0);

};

class TimerClass

{

protected:

int _timer_num;

Alarm *_alarm_list;

static void timeup0(void);

void timeup(void);

bool add(Alarm& alarm);

void restart(void);

inline static uint32_t blocking(void);

inline static void unblocking(uint32_t save);

public:

TimerClass(void);

virtual ~TimerClass(void);

int begin(int timer_num = -1);

void end(void);

bool start(Alarm& alarm);

void cancel(Alarm& alarm);

void cancel(int num, void *arg);

};

extern TimerClass Timer;

/*
  timer.cpp - Timer Library for Paspberry Pi Pico (RP2040)
  
  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 <string.h>
#include <hardware/irq.h>
#include <hardware/structs/timer.h>
#include <hardware/timer.h>
#include <hardware/sync.h>
#include "timer.h"

#define SPINLOCK_ID_TIMER PICO_SPINLOCK_ID_TIMER

TimerClass Timer;

static TimerClass  *_instances[NUM_TIMERS];
static irq_handler_t _handlers[NUM_TIMERS];

TimerClass::TimerClass(void)
: _timer_num(-1)
, _alarm_list(nullptr)
{
}

TimerClass::~TimerClass(void)
{
  end();
}

int TimerClass::begin(int timer_num)
{
  if (timer_num < 0)
  {
    for (int i = 0; i < NUM_TIMERS; ++i)
    {
      if (begin(i) >= 0)
        return i;
    }
  }
  else if ((timer_num < NUM_TIMERS) && !(timer_hw->inte & (1 << timer_num)))
  {
    _timer_num = timer_num;
    _instances[timer_num] = this;
    _handlers[timer_num] = irq_get_exclusive_handler(timer_num + TIMER_IRQ_0);
    irq_set_exclusive_handler(timer_num + TIMER_IRQ_0, timeup0);
    timer_hw->intr = 1 << timer_num;
    hw_set_bits(&timer_hw->inte, 1 << timer_num);
    irq_clear(timer_num + TIMER_IRQ_0);
    irq_set_enabled(timer_num + TIMER_IRQ_0, true);
    return timer_num;
  }
  return -1;
}

void TimerClass::end(void)
{
  if (_timer_num >= 0)
  {
    timer_hw->armed = 1 << _timer_num;
    hw_set_bits(&timer_hw->intf, 1 << _timer_num); 
    _alarm_list = nullptr;
    _timer_num = -1;
  }
}

uint32_t TimerClass::blocking(void)
{
  return spin_lock_blocking(spin_lock_instance(SPINLOCK_ID_TIMER));
}

void TimerClass::unblocking(uint32_t save)
{
  spin_unlock(spin_lock_instance(SPINLOCK_ID_TIMER), save);
}

bool TimerClass::start(Alarm& alarm)
{
  if ((_timer_num < 0) || (alarm._interval < 1) || !alarm._handler)
    return false;
  uint32_t save = blocking();
  if (add(alarm))
    restart();
  unblocking(save);
  return true;
}

void TimerClass::cancel(Alarm& alarm)
{
  uint32_t save = blocking();
  for (Alarm **p = &_alarm_list; *p; p = &(*p)->_next)
  {
    if (*p == &alarm)
    {
      *p = (*p)->_next;
      break;
    }
  }
  unblocking(save);
}

void TimerClass::cancel(int num, void *arg)
{
  if ((num < (sizeof(Alarm::_params) / sizeof(Alarm::_params[0]))) && arg)
  {
    uint32_t save = blocking();
    for (Alarm **p = &_alarm_list; *p; p = &(*p)->_next)
    {
      if ((*p)->_params[num] == arg)
      {
        *p = (*p)->_next;
        break;
      }
    }
    unblocking(save);
  }
}

bool TimerClass::add(Alarm& alarm)
{
  Alarm **p;
  for (p = &_alarm_list; *p && ((int64_t)((*p)->_alarm - alarm._alarm) <= 0); p = &(*p)->_next)
    continue;
  alarm._next = *p;
  *p = &alarm;
  return p == &_alarm_list;
}

void TimerClass::timeup0(void)
{
  uint32_t timer_num;
  __asm volatile ("mrs %0, ipsr" : "=r" (timer_num));
  timer_num = (timer_num & 0x3F) - 16 - TIMER_IRQ_0;
  timer_hw->intr = 1 << timer_num;
  if (timer_hw->intf & (1 << timer_num))
  {
    hw_clear_bits(&timer_hw->intf, 1 << timer_num);
    hw_clear_bits(&timer_hw->inte, 1 << timer_num);
    irq_set_enabled(timer_num + TIMER_IRQ_0, false);
    irq_set_exclusive_handler(timer_num + TIMER_IRQ_0, _handlers[timer_num]);
    _instances[timer_num] = nullptr;
  }
  else
    _instances[timer_num]->timeup();
}

void TimerClass::timeup(void)
{
  Alarm *p = nullptr;
  spin_lock_unsafe_blocking(spin_lock_instance(SPINLOCK_ID_TIMER));
  while (_alarm_list)
  {
    if ((p == _alarm_list) || ((int64_t)(_alarm_list->_alarm - time_us_64()) > 0))
    {
      restart();
      break;
    }
    _alarm_list = (p = _alarm_list)->_next;
    spin_unlock_unsafe(spin_lock_instance(SPINLOCK_ID_TIMER));
    bool repeat = p->_handler(*p);
    spin_lock_unsafe_blocking(spin_lock_instance(SPINLOCK_ID_TIMER));
    if (repeat)
    {
      p->_alarm += p->_interval;
      if (!add(*p))
        p = nullptr;
    }
    else
      p = nullptr;
  }
  spin_unlock_unsafe(spin_lock_instance(SPINLOCK_ID_TIMER));
}

void TimerClass::restart(void)
{
  uint32_t n;
  uint64_t t = time_us_64();
  uint64_t d = _alarm_list->_alarm - t;
  if ((int64_t)d <= 1)
    n = (uint32_t)t + 2;
  else if (d >> 32)
    n = (uint32_t)t + 0x80000000;
  else
    n = (uint32_t)t + (uint32_t)d;
  timer_hw->armed = 1 << _timer_num;
  timer_hw->intr  = 1 << _timer_num;
  timer_hw->alarm[_timer_num] = n;
}

Alarm::Alarm(void)
{
}

Alarm::~Alarm(void)
{
}

Alarm& Alarm::interval(int64_t interval, bool fromnow)
{
  if (fromnow)
    _alarm = time_us_64() + interval;
  _interval = interval;
  return *this;
}

int64_t Alarm::interval(void)
{
  return _interval;
}

Alarm& Alarm::handler(alarm_handler_t handler)
{
  _handler = handler;
  return *this;
}

alarm_handler_t Alarm::handler(void)
{
  return _handler;
}

Alarm& Alarm::params(int num, void *val)
{
  if (num < (sizeof(_params) / sizeof(_params[0])))
    _params[num] = val;
  return *this;
}

void *Alarm::params(int num)
{
  return num < (sizeof(_params) / sizeof(_params[0])) ? _params[num] : nullptr;
}

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

timer.cpp - Timer Library for Paspberry Pi Pico (RP2040)

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 <string.h>

#include <hardware/irq.h>

#include <hardware/structs/timer.h>

#include <hardware/timer.h>

#include <hardware/sync.h>

#include "timer.h"

#define SPINLOCK_ID_TIMER PICO_SPINLOCK_ID_TIMER

TimerClass Timer;

static TimerClass *_instances[NUM_TIMERS];

static irq_handler_t _handlers[NUM_TIMERS];

TimerClass::TimerClass(void)

: _timer_num(-1)

, _alarm_list(nullptr)

{

}

TimerClass::~TimerClass(void)

{

end();

}

int TimerClass::begin(int timer_num)

{

if (timer_num < 0)

{

for (int i = 0; i < NUM_TIMERS; ++i)

{

if (begin(i) >= 0)

return i;

}

else if ((timer_num < NUM_TIMERS) && !(timer_hw->inte & (1 << timer_num)))

{

_timer_num = timer_num;

_instances[timer_num] = this;

_handlers[timer_num] = irq_get_exclusive_handler(timer_num + TIMER_IRQ_0);

irq_set_exclusive_handler(timer_num + TIMER_IRQ_0, timeup0);

timer_hw->intr = 1 << timer_num;

hw_set_bits(&timer_hw->inte, 1 << timer_num);

irq_clear(timer_num + TIMER_IRQ_0);

irq_set_enabled(timer_num + TIMER_IRQ_0, true);

return timer_num;

}

return -1;

}

void TimerClass::end(void)

{

if (_timer_num >= 0)

{

timer_hw->armed = 1 << _timer_num;

hw_set_bits(&timer_hw->intf, 1 << _timer_num);

_alarm_list = nullptr;

_timer_num = -1;

}

uint32_t TimerClass::blocking(void)

{

return spin_lock_blocking(spin_lock_instance(SPINLOCK_ID_TIMER));

}

void TimerClass::unblocking(uint32_t save)

{

spin_unlock(spin_lock_instance(SPINLOCK_ID_TIMER), save);

}

bool TimerClass::start(Alarm& alarm)

{

if ((_timer_num < 0) || (alarm._interval < 1) || !alarm._handler)

return false;

uint32_t save = blocking();

if (add(alarm))

restart();

unblocking(save);

return true;

}

void TimerClass::cancel(Alarm& alarm)

{

uint32_t save = blocking();

for (Alarm **p = &_alarm_list; *p; p = &(*p)->_next)

{

if (*p == &alarm)

{

*p = (*p)->_next;

break;

}

unblocking(save);

}

void TimerClass::cancel(int num, void *arg)

{

if ((num < (sizeof(Alarm::_params) / sizeof(Alarm::_params[0]))) && arg)

{

uint32_t save = blocking();

for (Alarm **p = &_alarm_list; *p; p = &(*p)->_next)

{

if ((*p)->_params[num] == arg)

{

*p = (*p)->_next;

break;

}

unblocking(save);

}

bool TimerClass::add(Alarm& alarm)

{

Alarm **p;

for (p = &_alarm_list; *p && ((int64_t)((*p)->_alarm - alarm._alarm) <= 0); p = &(*p)->_next)

continue;

alarm._next = *p;

*p = &alarm;

return p == &_alarm_list;

}

void TimerClass::timeup0(void)

{

uint32_t timer_num;

__asm volatile ("mrs %0, ipsr" : "=r" (timer_num));

timer_num = (timer_num & 0x3F) - 16 - TIMER_IRQ_0;

timer_hw->intr = 1 << timer_num;

if (timer_hw->intf & (1 << timer_num))

{

hw_clear_bits(&timer_hw->intf, 1 << timer_num);

hw_clear_bits(&timer_hw->inte, 1 << timer_num);

irq_set_enabled(timer_num + TIMER_IRQ_0, false);

irq_set_exclusive_handler(timer_num + TIMER_IRQ_0, _handlers[timer_num]);

_instances[timer_num] = nullptr;

}

else

_instances[timer_num]->timeup();

}

void TimerClass::timeup(void)

{

Alarm *p = nullptr;

spin_lock_unsafe_blocking(spin_lock_instance(SPINLOCK_ID_TIMER));

while (_alarm_list)

{

if ((p == _alarm_list) || ((int64_t)(_alarm_list->_alarm - time_us_64()) > 0))

{

restart();

break;

}

_alarm_list = (p = _alarm_list)->_next;

spin_unlock_unsafe(spin_lock_instance(SPINLOCK_ID_TIMER));

bool repeat = p->_handler(*p);

spin_lock_unsafe_blocking(spin_lock_instance(SPINLOCK_ID_TIMER));

if (repeat)

{

p->_alarm += p->_interval;

if (!add(*p))

p = nullptr;

}

else

p = nullptr;

}

spin_unlock_unsafe(spin_lock_instance(SPINLOCK_ID_TIMER));

}

void TimerClass::restart(void)

{

uint32_t n;

uint64_t t = time_us_64();

uint64_t d = _alarm_list->_alarm - t;

if ((int64_t)d <= 1)

n = (uint32_t)t + 2;

else if (d >> 32)

n = (uint32_t)t + 0x80000000;

else

n = (uint32_t)t + (uint32_t)d;

timer_hw->armed = 1 << _timer_num;

timer_hw->intr = 1 << _timer_num;

timer_hw->alarm[_timer_num] = n;

}

Alarm::Alarm(void)

{

}

Alarm::~Alarm(void)

{

}

Alarm& Alarm::interval(int64_t interval, bool fromnow)

{

if (fromnow)

_alarm = time_us_64() + interval;

_interval = interval;

return *this;

}

int64_t Alarm::interval(void)

{

return _interval;

}

Alarm& Alarm::handler(alarm_handler_t handler)

{

_handler = handler;

return *this;

}

alarm_handler_t Alarm::handler(void)

{

return _handler;

}

Alarm& Alarm::params(int num, void *val)

{

if (num < (sizeof(_params) / sizeof(_params[0])))

_params[num] = val;

return *this;

}

void *Alarm::params(int num)

{

return num < (sizeof(_params) / sizeof(_params[0])) ? _params[num] : nullptr;

}

/*
  pcevent.h - Pin Change Event Library for Raspberry Pi Pico (RP2040)

  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 <stdint.h>
#include <stdbool.h>
#include "hardware/gpio.h"
#include "task.h"

typedef enum
{
  PCEVENT_DISABLE    = 0,
  PCEVENT_LEVEL_LOW  = GPIO_IRQ_LEVEL_LOW,
  PCEVENT_LEVEL_HIGH = GPIO_IRQ_LEVEL_HIGH,
  PCEVENT_EDGE_FALL  = GPIO_IRQ_EDGE_FALL,
  PCEVENT_EDGE_RISE  = GPIO_IRQ_EDGE_RISE,
  PCEVENT_EDGE_BOTH  = GPIO_IRQ_EDGE_FALL | GPIO_IRQ_EDGE_RISE,
} PCEVENT_LEVEL;

class PCEventClass : protected Sync
{
  public:

    PCEventClass(void) {}
    virtual ~PCEventClass(void) {}

    static void begin(void);
    static void end(void);
    static void interrupt(uint32_t pin, PCEVENT_LEVEL level, gpio_irq_callback_t handler = nullptr);
    int wait(uint32_t pin, PCEVENT_LEVEL level, int64_t timeout = -1);
    static uint32_t edge(uint32_t pin);

  protected:

    void wakeup(uint32_t pin, uint32_t events);
    static void enableInt(uint32_t pin);

  private:

    static void event(void);
    static void end0(void);
};

extern PCEventClass PCEvent;

pcevent.h - Pin Change Event Library for Raspberry Pi Pico (RP2040)

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 <stdint.h>

#include <stdbool.h>

#include "hardware/gpio.h"

#include "task.h"

typedef enum

{

PCEVENT_DISABLE = 0,

PCEVENT_LEVEL_LOW = GPIO_IRQ_LEVEL_LOW,

PCEVENT_LEVEL_HIGH = GPIO_IRQ_LEVEL_HIGH,

PCEVENT_EDGE_FALL = GPIO_IRQ_EDGE_FALL,

PCEVENT_EDGE_RISE = GPIO_IRQ_EDGE_RISE,

PCEVENT_EDGE_BOTH = GPIO_IRQ_EDGE_FALL | GPIO_IRQ_EDGE_RISE,

} PCEVENT_LEVEL;

class PCEventClass : protected Sync

{

public:

PCEventClass(void) {}

virtual ~PCEventClass(void) {}

static void begin(void);

static void end(void);

static void interrupt(uint32_t pin, PCEVENT_LEVEL level, gpio_irq_callback_t handler = nullptr);

int wait(uint32_t pin, PCEVENT_LEVEL level, int64_t timeout = -1);

static uint32_t edge(uint32_t pin);

protected:

void wakeup(uint32_t pin, uint32_t events);

static void enableInt(uint32_t pin);

private:

static void event(void);

static void end0(void);

};

extern PCEventClass PCEvent;

/*
  pcevent.cpp - Pin Change Event Library for Raspberry Pi Pico (RP2040)

  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 <string.h>
#include <hardware/irq.h>
#include <hardware/sync.h>
#include <hardware/timer.h>
#include <hardware/structs/iobank0.h>
#include "pcevent.h"

#define lengthof(a) (sizeof(a) / sizeof(a[0]))

typedef struct pcwait
{
  struct pcwait *next;
  size_t level;
  size_t taskid;
} pcwait_t;

typedef struct
{
  pcwait_t *head;
  pcwait_t *tail;
} pcqueue_t;

typedef struct
{
  uint32_t edge;
  size_t inte_wait;
  size_t inte_callback;
  gpio_irq_callback_t handler;
} pcevent_t;

static io_irq_ctrl_hw_t *_irqctrl;
static irq_handler_t     _default;
static pcevent_t         _pcevent[NUM_BANK0_GPIOS];
static pcqueue_t         _pcqueue;

PCEventClass PCEvent;

void PCEventClass::begin(void)
{
  memset(_pcevent, 0, sizeof(_pcevent));
  _irqctrl = get_core_num() ? &iobank0_hw->proc1_irq_ctrl : &iobank0_hw->proc0_irq_ctrl;
  for (size_t i = 0; i < lengthof(iobank0_hw->intr); ++i)
  {
    iobank0_hw->proc0_irq_ctrl.inte[i] = 0;
    iobank0_hw->proc1_irq_ctrl.inte[i] = 0;
    iobank0_hw->intr[i] = (io_rw_32)-1;
  }
  _default = irq_get_exclusive_handler(IO_IRQ_BANK0);
  irq_set_exclusive_handler(IO_IRQ_BANK0, event);
  irq_clear(IO_IRQ_BANK0);
  irq_set_enabled(IO_IRQ_BANK0, true);
}

void PCEventClass::end(void)
{
  if (_irqctrl)
    _irqctrl->intf[0] = 1;
}

void PCEventClass::end0(void)
{
  for (size_t i = 0; i < lengthof(iobank0_hw->intr); ++i)
  {
    _irqctrl->intf[i] = 0;
    _irqctrl->inte[i] = 0;
    iobank0_hw->intr[i] = (io_rw_32)-1;
  }
  irq_set_enabled(IO_IRQ_BANK0, false);
  irq_set_exclusive_handler(IO_IRQ_BANK0, _default);
  _irqctrl = nullptr;
}

void PCEventClass::enableInt(uint32_t pin)
{
  size_t bpos = (pin & 7) << 2;
  size_t inte = _pcevent[pin].inte_wait | _pcevent[pin].inte_callback;
  io_rw_32 *inte_reg = &_irqctrl->inte[pin >> 3];
  hw_clear_bits(inte_reg, (~inte & 0x0F) << bpos);
  hw_set_bits(inte_reg, inte << bpos);
}

void PCEventClass::interrupt(uint32_t pin, PCEVENT_LEVEL level, gpio_irq_callback_t handler)
{
  if (pin < lengthof(_pcevent))
  {
    pcevent_t& pcevent = _pcevent[pin];
    pcevent.inte_callback = level;
    pcevent.handler = handler;
    enableInt(pin);
  }
}

int PCEventClass::wait(uint32_t pin, PCEVENT_LEVEL level, int64_t timeout)
{
  int rv = SYNC_TIMEOUTED;
  if ((pin < lengthof(_pcevent)) && level)
  {
    if (level & PCEVENT_LEVEL_HIGH)
    {
      if (gpio_get(pin))
        return SYNC_WAKEUPED;
      level = (PCEVENT_LEVEL)(level | PCEVENT_EDGE_RISE);
    }
    if (level & PCEVENT_LEVEL_LOW)
    {
      if (!gpio_get(pin))
        return SYNC_WAKEUPED;
      level = (PCEVENT_LEVEL)(level | PCEVENT_EDGE_FALL);
    }
    if (timeout)
    {
      uint32_t save = blocking();
      pcwait_t wait = {nullptr, (size_t)(level & PCEVENT_EDGE_BOTH), Task::taskid()};
      _pcqueue.tail = *(_pcqueue.tail ? &_pcqueue.tail->next : &_pcqueue.head) = &wait;
      _pcevent[pin].inte_wait |= level;
      enableInt(pin);
      rv = sleep(timeout, SYNC_BLOCKED | save);
      if (rv == SYNC_TIMEOUTED)
      {
        blocking();
        for (pcwait_t **p = &_pcqueue.head; *p; p = &(*p)->next)
        {
          if (*p == &wait)
          {
            if (((*p = (*p)->next) == nullptr) && (p == &_pcqueue.head))
              _pcqueue.tail = nullptr;
            break;
          }
        }
        unblocking(save);
      }
    }
  }
  return rv;
}

uint32_t PCEventClass::edge(uint32_t pin)
{
  return _pcevent[pin].edge;
}

void PCEventClass::wakeup(uint32_t pin, uint32_t events)
{
  pcevent_t& pcevent = _pcevent[pin];
  lock();
  if (pcevent.inte_wait & events)
  {
    pcevent.inte_wait &= ~events;
    enableInt(pin);
    for (pcwait_t **p = &_pcqueue.head; *p; )
    {
      pcwait_t *q = *p;
      if (q->level & events)
      {
        if (((*p = q->next) == nullptr) && (p == &_pcqueue.head))
          _pcqueue.tail = nullptr;
        wakeup0(q->taskid);
      }
      else
        p = &q->next;
    }
  }
  unlock();
}

void PCEventClass::event(void)
{
  io_ro_32 *intf_reg = _irqctrl->intf;
  io_ro_32 *ints_reg = _irqctrl->ints;
  io_rw_32 *intr_reg = iobank0_hw->intr;
  for (size_t i = 0; i < lengthof(_irqctrl->ints); ++i)
  {
    if (*intf_reg++)
    {
      end0();
      break;
    }
    io_rw_32 ints = *ints_reg++;
    *intr_reg++ = ints;
    for (uint32_t pin = i << 3; ints && (pin < lengthof(_pcevent)); ++pin)
    {
      uint32_t events = ints & 0x0F;
      ints >>= 4;
      if (events)
      {
        pcevent_t& pcevent = _pcevent[pin];
        if (events & PCEVENT_EDGE_BOTH)
          pcevent.edge = time_us_32();
        PCEvent.wakeup(pin, events);
        if (pcevent.inte_callback & events)
        {
          gpio_irq_callback_t cb = pcevent.handler;
          if (cb)
            cb(pin, events);
        }
      }
    }
  }
}

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

pcevent.cpp - Pin Change Event Library for Raspberry Pi Pico (RP2040)

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 <string.h>

#include <hardware/irq.h>

#include <hardware/sync.h>

#include <hardware/timer.h>

#include <hardware/structs/iobank0.h>

#include "pcevent.h"

#define lengthof(a) (sizeof(a) / sizeof(a[0]))

typedef struct pcwait

{

struct pcwait *next;

size_t level;

size_t taskid;

} pcwait_t;

typedef struct

{

pcwait_t *head;

pcwait_t *tail;

} pcqueue_t;

typedef struct

{

uint32_t edge;

size_t inte_wait;

size_t inte_callback;

gpio_irq_callback_t handler;

} pcevent_t;

static io_irq_ctrl_hw_t *_irqctrl;

static irq_handler_t _default;

static pcevent_t _pcevent[NUM_BANK0_GPIOS];

static pcqueue_t _pcqueue;

PCEventClass PCEvent;

void PCEventClass::begin(void)

{

memset(_pcevent, 0, sizeof(_pcevent));

_irqctrl = get_core_num() ? &iobank0_hw->proc1_irq_ctrl : &iobank0_hw->proc0_irq_ctrl;

for (size_t i = 0; i < lengthof(iobank0_hw->intr); ++i)

{

iobank0_hw->proc0_irq_ctrl.inte[i] = 0;

iobank0_hw->proc1_irq_ctrl.inte[i] = 0;

iobank0_hw->intr[i] = (io_rw_32)-1;

}

_default = irq_get_exclusive_handler(IO_IRQ_BANK0);

irq_set_exclusive_handler(IO_IRQ_BANK0, event);

irq_clear(IO_IRQ_BANK0);

irq_set_enabled(IO_IRQ_BANK0, true);

}

void PCEventClass::end(void)

{

if (_irqctrl)

_irqctrl->intf[0] = 1;

}

void PCEventClass::end0(void)

{

for (size_t i = 0; i < lengthof(iobank0_hw->intr); ++i)

{

_irqctrl->intf[i] = 0;

_irqctrl->inte[i] = 0;

iobank0_hw->intr[i] = (io_rw_32)-1;

}

irq_set_enabled(IO_IRQ_BANK0, false);

irq_set_exclusive_handler(IO_IRQ_BANK0, _default);

_irqctrl = nullptr;

}

void PCEventClass::enableInt(uint32_t pin)

{

size_t bpos = (pin & 7) << 2;

size_t inte = _pcevent[pin].inte_wait | _pcevent[pin].inte_callback;

io_rw_32 *inte_reg = &_irqctrl->inte[pin >> 3];

hw_clear_bits(inte_reg, (~inte & 0x0F) << bpos);

hw_set_bits(inte_reg, inte << bpos);

}

void PCEventClass::interrupt(uint32_t pin, PCEVENT_LEVEL level, gpio_irq_callback_t handler)

{

if (pin < lengthof(_pcevent))

{

pcevent_t& pcevent = _pcevent[pin];

pcevent.inte_callback = level;

pcevent.handler = handler;

enableInt(pin);

}

int PCEventClass::wait(uint32_t pin, PCEVENT_LEVEL level, int64_t timeout)

{

int rv = SYNC_TIMEOUTED;

if ((pin < lengthof(_pcevent)) && level)

{

if (level & PCEVENT_LEVEL_HIGH)

{

if (gpio_get(pin))

return SYNC_WAKEUPED;

level = (PCEVENT_LEVEL)(level | PCEVENT_EDGE_RISE);

}

if (level & PCEVENT_LEVEL_LOW)

{

if (!gpio_get(pin))

return SYNC_WAKEUPED;

level = (PCEVENT_LEVEL)(level | PCEVENT_EDGE_FALL);

}

if (timeout)

{

uint32_t save = blocking();

pcwait_t wait = {nullptr, (size_t)(level & PCEVENT_EDGE_BOTH), Task::taskid()};

_pcqueue.tail = *(_pcqueue.tail ? &_pcqueue.tail->next : &_pcqueue.head) = &wait;

_pcevent[pin].inte_wait |= level;

enableInt(pin);

rv = sleep(timeout, SYNC_BLOCKED | save);

if (rv == SYNC_TIMEOUTED)

{

blocking();

for (pcwait_t **p = &_pcqueue.head; *p; p = &(*p)->next)

{

if (*p == &wait)

{

if (((*p = (*p)->next) == nullptr) && (p == &_pcqueue.head))

_pcqueue.tail = nullptr;

break;

}

unblocking(save);

}

return rv;

}

uint32_t PCEventClass::edge(uint32_t pin)

{

return _pcevent[pin].edge;

}

void PCEventClass::wakeup(uint32_t pin, uint32_t events)

{

pcevent_t& pcevent = _pcevent[pin];

lock();

if (pcevent.inte_wait & events)

{

pcevent.inte_wait &= ~events;

enableInt(pin);

for (pcwait_t **p = &_pcqueue.head; *p; )

{

pcwait_t *q = *p;

if (q->level & events)

{

if (((*p = q->next) == nullptr) && (p == &_pcqueue.head))

_pcqueue.tail = nullptr;

wakeup0(q->taskid);

}

else

p = &q->next;

}

unlock();

}

void PCEventClass::event(void)

{

io_ro_32 *intf_reg = _irqctrl->intf;

io_ro_32 *ints_reg = _irqctrl->ints;

io_rw_32 *intr_reg = iobank0_hw->intr;

for (size_t i = 0; i < lengthof(_irqctrl->ints); ++i)

{

if (*intf_reg++)

{

end0();

break;

}

io_rw_32 ints = *ints_reg++;

*intr_reg++ = ints;

for (uint32_t pin = i << 3; ints && (pin < lengthof(_pcevent)); ++pin)

{

uint32_t events = ints & 0x0F;

ints >>= 4;

if (events)

{

pcevent_t& pcevent = _pcevent[pin];

if (events & PCEVENT_EDGE_BOTH)

pcevent.edge = time_us_32();

PCEvent.wakeup(pin, events);

if (pcevent.inte_callback & events)

{

gpio_irq_callback_t cb = pcevent.handler;

if (cb)

cb(pin, events);

}

タグ: SMP

Symmetric multiprocessing (SMP) with Raspberry Pi Pico (RP2040)