Chinaunix首页 | 论坛 | 博客

嵌入式之囧途qiushui007.blog.chinaunix.net

首页　| 　博文目录　| 　关于我

qiushui_007

  • 博客访问： 1511203
  • 博文数量： 129
  • 博客积分： 1449
  • 博客等级： 上尉
  • 技术积分： 3048
  • 用 户 组： 普通用户
  • 注册时间： 2012-07-24 18:36
文章分类

全部博文（129）

文章存档

2015年（3）

2014年（20）

2013年（65）

2012年（41）

我的朋友
最近访客
推荐博文
相关博文
RL-RTX 源码汇总和修改, 自测成功

分类： C/C++

2013-03-08 09:07:11

修改如下:

点击(此处)折叠或打开

  1. /*------------------------------------------------------------------------------------------------
  2.        RL-ARM - RTX 源码汇总 Copyright (c) 2004-2010 KEIL - An ARM Company. All rights reserved

  4. 感谢党感谢政府,
  5. 本文件+rtx_config.c+svc_table.s(根据情况)加入工程, 工程的Target-->Operating System选择None

  7. qiushui_007 于2011-3-3在V4.12的源码上汇总并在STM32和LM3S系列上实验成功(Email:xxg6688@163.com)
  8. V4.12     Corrected: a problem in stack checking for Cortex-M library.
  9.         The stack was checked before the task context was completely stacked.

  11. V4.13     Corrected: a problem in isr_mbx_receive() function for Cortex-M library.
  12.         If sending task was suspended on a full mailbox, a message was lost. (但代码未改变????)
  13.         Added: big endian RTX libraries for Cortex-M0, Cortex-M1, Cortex-M3 and Cortex-M4

  15. Code (inc. data) RO Data RW Data ZI Data Debug Object Name
  16. 4808 256 0 37 96 0 rtx_cm3.o
  17. 4816 254 0 37 96 0 rtx_cm3.o

  19. 备注:
  20. - 启动代码中在__Vectors之前增加如下, 并且屏蔽相关的输出和定义. stm32f10x_it.c中同样屏蔽同名的函数
  21.     IMPORT SVC_Handler
  22.     IMPORT PendSV_Handler
  23.     IMPORT SysTick_Handler    ;RTX中的EXPORT    
  24. - RTX用户进程优先级1-254,(最高254; 最低1, 0和255为系统进程). 优先级可以相同.
  25. - RTX系统创建了两个系统任务 os_clock_demon 和 os_idle_demon. 这两个任务经常是RTX内核所必需的。
  26. - 对SysTick的操作: 比较相关节拍代码, uCOS可定义为1000(每个节拍为1ms), RTX经试验最大为100(每个节拍10ms)
  27.     
  28. //------------------- New/Modify -----------------------------------------*/
  29. //#define SVCUSER    //若定义此, 工程中必须加上svc_table.s

  31. #define OS_MBOX_EN 1 /* Enable (1) or Disable (0) code generation for MAILBOXES */
  32. #define OS_SEM_EN 1 /* Enable (1) or Disable (0) code generation for SEMAPHORES */
  33. #define OS_MUTEX_EN 1 /* Enable (1) or Disable (0) code generation for MUTEX */
  34. #define OS_FLAG_EN 0 /* Enable (1) or Disable (0) code generation for EVENT FLAGS */
  35. #define OS_TMR_EN 0 /* Enable (1) or Disable (0) code generation for TIMERS */

  37. //---------------------------------------------------------------------------

  39. /* Types */
  40. typedef char S8;
  41. typedef unsigned char U8;
  42. typedef short S16;
  43. typedef unsigned short U16;
  44. typedef int S32;
  45. typedef unsigned int U32;
  46. typedef long long S64;
  47. typedef unsigned long long U64;
  48. typedef unsigned char BIT;
  49. typedef unsigned int BOOL;

  51. /* Definitions */
  52. #define __TRUE 1
  53. #define __FALSE 0
  54. #define NULL ((void *) 0)

  56. /* Values for 'state' */
  57. #define INACTIVE 0
  58. #define READY 1
  59. #define RUNNING 2
  60. #define WAIT_DLY 3
  61. #define WAIT_ITV 4
  62. #define WAIT_OR 5
  63. #define WAIT_AND 6
  64. #define WAIT_SEM 7
  65. #define WAIT_MBX 8
  66. #define WAIT_MUT 9

  68. /* Return codes */
  69. #define OS_R_TMO 0x01
  70. #define OS_R_EVT 0x02
  71. #define OS_R_SEM 0x03
  72. #define OS_R_MBX 0x04
  73. #define OS_R_MUT 0x05
  74. #define OS_R_OK 0x00
  75. #define OS_R_NOK 0xff

  77. /* Error Codes */
  78. #define OS_ERR_STK_OVF 1
  79. #define OS_ERR_FIFO_OVF 2
  80. #define OS_ERR_MBX_OVF 3

  82. typedef void (*FUNCP)(void);
  83. typedef U32 OS_TID;
  84. typedef void *OS_ID;
  85. typedef U32 OS_RESULT;

  87. typedef struct OS_TCB {
  88.   /* General part: identical for all implementations. */
  89.   U8 cb_type; /* Control Block Type */
  90.   U8 state; /* Task state */
  91.   U8 prio; /* Execution priority */
  92.   U8 task_id; /* Task ID value for optimized TCB access */
  93.   struct OS_TCB *p_lnk; /* Link pointer for ready/sem. wait list */
  94.   struct OS_TCB *p_rlnk; /* Link pointer for sem./mbx lst backwards */
  95.   struct OS_TCB *p_dlnk; /* Link pointer for delay list */
  96.   struct OS_TCB *p_blnk; /* Link pointer for delay list backwards */
  97.   U16 delta_time; /* Time until time out */
  98.   U16 interval_time; /* Time interval for periodic waits */
  99.   U16 events; /* Event flags */
  100.   U16 waits; /* Wait flags */
  101.   void **msg; /* Direct message passing when task waits */
  102.   U8 ret_val; /* Return value upon completion of a wait */

  104.   /* Hardware dependant part: specific for CM processor */
  105.   U8 ret_upd; /* Updated return value */
  106.   U16 priv_stack; /* Private stack size, 0= system assigned */
  107.   U32 tsk_stack; /* Current task Stack pointer (R13) */
  108.   U32 *stack; /* Pointer to Task Stack memory block */

  110.   /* Task entry point used for uVision debugger */
  111.   FUNCP ptask; /* Task entry address */
  112. } *P_TCB;

  114. #define TCB_RETVAL 32 /* 'ret_val' offset */
  115. #define TCB_RETUPD 33 /* 'ret_upd' offset */
  116. #define TCB_TSTACK 36 /* 'tsk_stack' offset */

  118. typedef struct OS_PSFE { /* Post Service Fifo Entry */
  119.   void *id; /* Object Identification */
  120.   U32 arg; /* Object Argument */
  121. } *P_PSFE;

  123. typedef struct OS_PSQ { /* Post Service Queue */
  124.   U8 first; /* FIFO Head Index */
  125.   U8 last; /* FIFO Tail Index */
  126.   U8 count; /* Number of stored items in FIFO */
  127.   U8 size; /* FIFO Size */
  128.   struct OS_PSFE q[1]; /* FIFO Content */
  129. } *P_PSQ;

  131. typedef struct OS_TSK {
  132.   P_TCB run; /* Current running task */
  133.   P_TCB new; /* Scheduled task to run */
  134. } *P_TSK;

  136. typedef struct OS_ROBIN { /* Round Robin Control */
  137.   P_TCB task; /* Round Robin task */
  138.   U16 time; /* Round Robin switch time */
  139.   U16 tout; /* Round Robin timeout */
  140. } *P_ROBIN;

  142. typedef struct OS_XCB {
  143.   U8 cb_type; /* Control Block Type */
  144.   struct OS_TCB *p_lnk; /* Link pointer for ready/sem. wait list */
  145.   struct OS_TCB *p_rlnk; /* Link pointer for sem./mbx lst backwards */
  146.   struct OS_TCB *p_dlnk; /* Link pointer for delay list */
  147.   struct OS_TCB *p_blnk; /* Link pointer for delay list backwards */
  148.   U16 delta_time; /* Time until time out */
  149. } *P_XCB;

  151. typedef struct OS_MCB {
  152.   U8 cb_type; /* Control Block Type */
  153.   U8 isr_st; /* State flag variable for isr functions */
  154.   struct OS_TCB *p_lnk; /* Chain of tasks waiting for message */
  155.   U16 first; /* Index of the message list begin */
  156.   U16 last; /* Index of the message list end */
  157.   U16 count; /* Actual number of stored messages */
  158.   U16 size; /* Maximum number of stored messages */
  159.   void *msg[1]; /* FIFO for Message pointers 1st element */
  160. } *P_MCB;

  162. typedef struct OS_SCB {
  163.   U8 cb_type; /* Control Block Type */
  164.   U16 tokens; /* Semaphore tokens */
  165.   struct OS_TCB *p_lnk; /* Chain of tasks waiting for tokens */
  166. } *P_SCB;

  168. typedef struct OS_MUCB {
  169.   U8 cb_type; /* Control Block Type */
  170.   U8 prio; /* Owner task default priority */
  171.   U16 level; /* Call nesting level */
  172.   struct OS_TCB *p_lnk; /* Chain of tasks waiting for mutex */
  173.   struct OS_TCB *owner; /* Mutex owner task */
  174. } *P_MUCB;

  176. typedef struct OS_XTMR {
  177.   struct OS_TMR *next;
  178.   U16 tcnt;
  179. } *P_XTMR;

  181. typedef struct OS_TMR {
  182.   struct OS_TMR *next; /* Link pointer to Next timer */
  183.   U16 tcnt; /* Timer delay count */
  184.   U16 info; /* User defined call info */
  185. } *P_TMR;

  187. typedef struct OS_BM {
  188.   void *free; /* Pointer to first free memory block */
  189.   void *end; /* Pointer to memory block end */
  190.   U32 blk_size; /* Memory block size */
  191. } *P_BM;

  193. //----------------------------------------------------------------------------------------

  195. #define INITIAL_xPSR 0x01000000
  196. #define DEMCR_TRCENA 0x01000000
  197. #define ITM_ITMENA 0x00000001
  198. #define MAGIC_WORD 0xE25A2EA5

  200. /* NVIC SysTick registers */
  201. #define NVIC_ST_CTRL (*((volatile U32 *)0xE000E010))
  202. #define NVIC_ST_RELOAD (*((volatile U32 *)0xE000E014))
  203. #define NVIC_ST_CURRENT (*((volatile U32 *)0xE000E018))
  204. #define NVIC_INT_CTRL (*((volatile U32 *)0xE000ED04))
  205. #define NVIC_SYS_PRI2 (*((volatile U32 *)0xE000ED1C))
  206. #define NVIC_SYS_PRI3 (*((volatile U32 *)0xE000ED20))

  208. #define OS_PEND_IRQ() NVIC_INT_CTRL = (1<<28);
  209. #define OS_TINIT() NVIC_ST_RELOAD = os_trv;
  210.                         NVIC_ST_CTRL = 0x0007;
  211.                         NVIC_SYS_PRI3 |= 0xFFFF0000;
  212.                         NVIC_SYS_PRI2 |= (NVIC_SYS_PRI3<<1) & 0xFE000000;
  213. #define OS_LOCK() NVIC_ST_CTRL = 0x0005;
  214. #define OS_UNLOCK() NVIC_ST_CTRL = 0x0007;

  216. /* Core Debug registers */
  217. #define DEMCR (*((volatile U32 *)0xE000EDFC))

  219. /* ITM registers */
  220. #define ITM_CONTROL (*((volatile U32 *)0xE0000E80))
  221. #define ITM_ENABLE (*((volatile U32 *)0xE0000E00))
  222. #define ITM_PORT30_U32 (*((volatile U32 *)0xE0000078))
  223. #define ITM_PORT31_U32 (*((volatile U32 *)0xE000007C))
  224. #define ITM_PORT31_U16 (*((volatile U16 *)0xE000007C))
  225. #define ITM_PORT31_U8 (*((volatile U8 *)0xE000007C))

  227. /* Functions */
  228. #if defined(__TARGET_ARCH_7_M) || defined(__TARGET_ARCH_7E_M)
  229.  #define rt_inc(p) while(__strex((__ldrex(p)+1),p))
  230.  #define rt_dec(p) while(__strex((__ldrex(p)-1),p))
  231. #else
  232.  #define rt_inc(p) __disable_irq();(*p)++;__enable_irq();
  233.  #define rt_dec(p) __disable_irq();(*p)--;__enable_irq();
  234. #endif

  236. __inline U32 rt_inc_qi (U32 size, U8 *count, U8 *first) {
  237.   U32 cnt,c2;
  238. #if defined(__TARGET_ARCH_7_M) || defined(__TARGET_ARCH_7E_M)
  239.   do {
  240.     if ((cnt = __ldrex(count)) == size) {
  241.       __clrex();
  242.       return (cnt); }
  243.   } while (__strex(cnt+1, count));
  244.   do {
  245.     c2 = (cnt = __ldrex(first)) + 1;
  246.     if (c2 == size) c2 = 0;
  247.   } while (__strex(c2, first));
  248. #else
  249.   __disable_irq();
  250.   if ((cnt = *count) < size) {
  251.     *count = cnt+1;
  252.     c2 = (cnt = *first) + 1;
  253.     if (c2 == size) c2 = 0;
  254.     *first = c2;
  255.   }
  256.   __enable_irq ();
  257. #endif
  258.   return (cnt);
  259. }

  261. #define rt_tmr_init() OS_TINIT();
  262. //void rt_init_stack (P_TCB p_TCB, FUNCP task_body);
  263. //void rt_set_PSP (U32 stack);
  264. //void os_set_env (void);
  265. void *_alloc_box (void *box_mem);
  266. int _free_box (void *box_mem, void *box);

  268. void dbg_init (void);
  269. void dbg_task_notify (P_TCB p_tcb, BOOL create);
  270. void dbg_task_switch (U32 task_id);

  272. #ifdef DBG_MSG
  273. #define DBG_INIT() dbg_init()
  274. #define DBG_TASK_NOTIFY(p_tcb,create) if (dbg_msg) dbg_task_notify(p_tcb,create)
  275. #define DBG_TASK_SWITCH(task_id) if (dbg_msg && (os_tsk.new!=os_tsk.run))
  276.                                                    dbg_task_switch(task_id)
  277. #else
  278. #define DBG_INIT()
  279. #define DBG_TASK_NOTIFY(p_tcb,create)
  280. #define DBG_TASK_SWITCH(task_id)
  281. #endif

  283. //OS_RESULT rt_evt_wait (U16 wait_flags, U16 timeout, BOOL and_wait);
  284. //void rt_evt_set (U16 event_flags, OS_TID task_id);
  285. //void rt_evt_clr (U16 clear_flags, OS_TID task_id);
  286. //void isr_evt_set (U16 event_flags, OS_TID task_id);
  287. //U16 rt_evt_get (void);
  288. void rt_evt_psh (P_TCB p_CB, U16 set_flags);

  290. /* Functions */
  291. void rt_switch_req (P_TCB p_new);
  292. void rt_dispatch (P_TCB next_TCB);
  293. void rt_block (U16 timeout, U8 block_state);
  294. //void rt_tsk_pass (void);
  295. //OS_TID rt_tsk_self (void);
  296. //OS_RESULT rt_tsk_prio (OS_TID task_id, U8 new_prio);
  297. //OS_TID rt_tsk_create (FUNCP task, U32 prio_stksz, void *stk, void *argv);
  298. //OS_RESULT rt_tsk_delete (OS_TID task_id);
  299. //void rt_sys_init (FUNCP first_task, U32 prio_stksz, void *stk);

  301. //void rt_dly_wait (U16 delay_time);
  302. //void rt_itv_set (U16 interval_time);
  303. //void rt_itv_wait (void);

  305. void rt_tmr_tick (void);
  306. //OS_ID rt_tmr_create (U16 tcnt, U16 info);
  307. //OS_ID rt_tmr_kill (OS_ID timer);


  310. #define rt_init_box _init_box
  311. #define rt_calloc_box _calloc_box
  312. int _init_box (void *box_mem, U32 box_size, U32 blk_size);
  313. void *rt_alloc_box (void *box_mem);
  314. void * _calloc_box (void *box_mem);
  315. int rt_free_box (void *box_mem, void *box);

  317. //void rt_mbx_init (OS_ID mailbox, U16 mbx_size);
  318. //OS_RESULT rt_mbx_send (OS_ID mailbox, void *p_msg, U16 timeout);
  319. //OS_RESULT rt_mbx_wait (OS_ID mailbox, void **message, U16 timeout);
  320. //OS_RESULT rt_mbx_check (OS_ID mailbox);
  321. //void isr_mbx_send (OS_ID mailbox, void *p_msg);
  322. //OS_RESULT isr_mbx_receive (OS_ID mailbox, void **message);
  323. void rt_mbx_psh (P_MCB p_CB, void *p_msg);

  325. //void rt_sem_init (OS_ID semaphore, U16 token_count);
  326. //OS_RESULT rt_sem_send (OS_ID semaphore);
  327. //OS_RESULT rt_sem_wait (OS_ID semaphore, U16 timeout);
  328. //void isr_sem_send (OS_ID semaphore);
  329. void rt_sem_psh (P_SCB p_CB);

  331. //void rt_mut_init (OS_ID mutex);
  332. //OS_RESULT rt_mut_release (OS_ID mutex);
  333. //OS_RESULT rt_mut_wait (OS_ID mutex, U16 timeout);

  335. #define os_psq ((P_PSQ)&os_fifo)
  336. //void rt_tsk_lock (void);
  337. //void rt_tsk_unlock (void);
  338. void rt_psh_req (void);
  339. void rt_pop_req (void);
  340. void rt_systick (void);
  341. void rt_stk_check (void);

  343. //void rt_init_robin (void);
  344. //void rt_chk_robin (void);

  346. #define TCB 0
  347. #define MCB 1
  348. #define SCB 2
  349. #define MUCB 3
  350. #define HCB 4
  351. /* Functions */
  352. void rt_put_prio (P_XCB p_CB, P_TCB p_task);
  353. P_TCB rt_get_first (P_XCB p_CB);
  354. void rt_put_rdy_first (P_TCB p_task);
  355. P_TCB rt_get_same_rdy_prio (void);
  356. void rt_resort_prio (P_TCB p_task);
  357. void rt_put_dly (P_TCB p_task, U16 delay);
  358. void rt_dec_dly (void);
  359. void rt_rmv_list (P_TCB p_task);
  360. void rt_rmv_dly (P_TCB p_task);
  361. void rt_psq_enq (OS_ID entry, U32 arg);

  363. /* This is a fast macro generating in-line code */
  364. #define rt_rdy_prio(void) (os_rdy.p_lnk->prio)

  366. //--- rtx_config.h, Exported functions of RTX_Config.c. 相关的变量在rtx_config.c中定义
  367. /* Definitions */
  368. #define BOX_ALIGN_8 0x80000000
  369. #define _declare_box(pool,size,cnt) U32 pool[(((size)+3)/4)*(cnt) + 3]
  370. #define _declare_box8(pool,size,cnt) U64 pool[(((size)+7)/8)*(cnt) + 2]
  371. #define _init_box8(pool,size,bsize) _init_box (pool,size,(bsize) | BOX_ALIGN_8)

  373. /* Variables */
  374. extern U32 mp_tcb[];            //_declare_box (mp_tcb, OS_TCB_SIZE, OS_TASKCNT);
  375. extern U64 mp_stk[];
  376. extern U32 os_fifo[];
  377. extern void *os_active_TCB[];

  379. /* Constants */
  380. extern U16 const os_maxtaskrun;    //U16 const os_maxtaskrun = OS_TASKCNT;
  381. extern U32 const os_trv;
  382. extern U8 const os_flags;        //U8 const os_flags = OS_RUNPRIV;
  383. extern U32 const os_stackinfo;    //U32 const os_stackinfo = (OS_STKCHECK<<24)| (OS_PRIVCNT<<16) | (OS_STKSIZE*4);
  384. extern U32 const os_rrobin;
  385. //extern U32 const os_clockrate;    //U32 const os_clockrate = OS_TICK;
  386. extern U32 const os_timernum;    //U32 const os_timernum = (OS_TIMER << 16) | OS_TIMERCNT;
  387. extern U16 const mp_tcb_size;    //U16 const mp_tcb_size = sizeof(mp_tcb);
  388. extern U32 const mp_stk_size;
  389. extern U32 const *m_tmr;        //U32 const *m_tmr = &mp_tmr[0];
  390. extern U16 const mp_tmr_size;    //U16 const mp_tmr_size = sizeof(mp_tmr);
  391. extern U8 const os_fifo_size;

  393. /* Functions */
  394. extern void os_idle_demon (void);
  395. extern void os_tmr_call (U16 info);
  396. extern void os_error (U32 err_code);

  398. /*----------------------------------------------------------------------------
  399.  * Global Variables
  400.  *---------------------------------------------------------------------------*/
  401. U16 os_time;                        // Free running system tick counter
  402. struct OS_XTMR os_tmr;                // User Timer list pointer
  403. struct OS_TSK os_tsk;                // Running and next task info.
  404. struct OS_TCB os_idle_TCB;        // Task Control Blocks of idle demon
  405. static BIT os_lock;
  406. static BIT os_psh_flag;
  407. struct OS_XCB os_rdy;                // List head of chained ready tasks
  408. struct OS_XCB os_dly;                // List head of chained delay tasks
  409. struct OS_ROBIN os_robin;

  411. //======================== HAL_CM3.c Start ==========================================================
  412. BIT dbg_msg;

  414. __asm void rt_set_PSP (U32 stack) {
  415.         MSR PSP,R0
  416.         BX LR
  417. }

  419. __asm void os_set_env (void) {
  420.    /* Switch to Unpriviliged/Priviliged Thread mode, use PSP. */
  421.         MOV R0,SP
  422.         MSR PSP,R0                    ; PSP = MSP
  423.         LDR R0,=__cpp(&os_flags)
  424.         LDRB R0,[R0]
  425.         LSLS R0,#31
  426.         MOVNE R0,#0x02 ; Priviliged Thread mode, use PSP
  427.         MOVEQ R0,#0x03 ; Unpriviliged Thread mode, use PSP
  428.         MSR CONTROL,R0
  429.         BX LR

  431.         ALIGN
  432. }

  434. __asm void *_alloc_box (void *box_mem) {
  435.    /* Function wrapper for Unpriviliged/Priviliged mode. */
  436.         LDR R12,=__cpp(rt_alloc_box)
  437.         MRS R3,IPSR
  438.         LSLS R3,#24
  439.         BXNE R12
  440.         MRS R3,CONTROL
  441.         LSLS R3,#31
  442.         BXEQ R12
  443.         SVC 0
  444.         BX LR

  446.         ALIGN
  447. }

  449. __asm int _free_box (void *box_mem, void *box) {
  450.    /* Function wrapper for Unpriviliged/Priviliged mode. */
  451.         LDR R12,=__cpp(rt_free_box)
  452.         MRS R3,IPSR
  453.         LSLS R3,#24
  454.         BXNE R12
  455.         MRS R3,CONTROL
  456.         LSLS R3,#31
  457.         BXEQ R12
  458.         SVC 0
  459.         BX LR

  461.         ALIGN
  462. }

  464. __asm void SVC_Handler (void) {
  465.         PRESERVE8

  467. #ifdef SVCUSER
  468.         IMPORT SVC_Count
  469.         IMPORT SVC_Table
  470. #endif
  471.         IMPORT rt_stk_check

  473.         MRS R0,PSP ; Read PSP
  474.         LDR R1,[R0,#24] ; Read Saved PC from Stack
  475.         LDRH R1,[R1,#-2] ; Load Halfword
  476. #ifdef SVCUSER
  477.         BICS R1,R1,#0xFF00 ; Extract SVC Number
  478.         BNE SVC_User
  479. #endif

  481.         LDM R0,{R0-R3,R12} ; Read R0-R3,R12 from stack
  482.         BLX R12 ; Call SVC Function

  484.         MRS R12,PSP ; Read PSP
  485.         LDR R3,=__cpp(&os_tsk)
  486.         LDM R3,{R1,R2} ; os_tsk.run, os_tsk.new
  487.         CMP R1,R2
  488.         BEQ SVC_Exit ; no task switch

  490.         PUSH {R2,R3}
  491.         MOV R3,#1
  492.         CMP R1,#0 ; Runtask deleted?
  493.         STRBNE R3,[R1,#TCB_RETUPD] ; os_tsk.run->ret_upd = 1
  494.         STMDBNE R12!,{R4-R11} ; Save Old context
  495.         STRNE R12,[R1,#TCB_TSTACK] ; Update os_tsk.run->tsk_stack
  496.         BLNE rt_stk_check ; Check for Stack overflow

  498.         POP {R2,R3}
  499.         STR R2,[R3] ; os_tsk.run = os_tsk.new

  501.         LDR R12,[R2,#TCB_TSTACK] ; os_tsk.new->tsk_stack
  502.         LDMIA R12!,{R4-R11} ; Restore New Context
  503.         LDRB R3,[R2,#TCB_RETUPD] ; Update ret_val?
  504.         MSR PSP,R12 ; Write PSP

  506.         CBZ R3,SVC_Return
  507.         LDRB R0,[R2,#TCB_RETVAL] ; Write os_tsk.new->ret_val

  509. SVC_Exit
  510.         STR R0,[R12] ; Function return value

  512. SVC_Return
  513.         MVN LR,#:NOT:0xFFFFFFFD ; set EXC_RETURN value
  514.         BX LR

  516.         /*------------------- User SVC ------------------------------*/
  517. #ifdef SVCUSER
  518. SVC_User
  519.         PUSH {R4,LR} ; Save Registers
  520.         LDR R2,=SVC_Count
  521.         LDR R2,[R2]
  522.         CMP R1,R2
  523.         BHI SVC_Done ; Overflow

  525.         LDR R4,=SVC_Table-4
  526.         LDR R4,[R4,R1,LSL #2] ; Load SVC Function Address

  528.         LDM R0,{R0-R3,R12} ; Read R0-R3,R12 from stack
  529.         BLX R4 ; Call SVC Function

  531.         MRS R12,PSP
  532.         STM R12,{R0-R3} ; Function return values
  533. SVC_Done
  534.         POP {R4,PC} ; RETI

  536.         ALIGN
  537. #endif
  538. }

  540. __asm void Sys_Handler (void) {
  541.         PRESERVE8

  543.         EXPORT SysTick_Handler
  544.         EXPORT PendSV_Handler

  546. PendSV_Handler
  547.         BL __cpp(rt_pop_req)

  549. Sys_Switch
  550.         LDR R3,=__cpp(&os_tsk)
  551.         LDM R3,{R1,R2} ; os_tsk.run, os_tsk.new
  552.         CMP R1,R2
  553.         BEQ SysExit

  555.         PUSH {R2,R3}
  556.         MOV R3,#0
  557.         STRB R3,[R1,#TCB_RETUPD] ; os_tsk.run->ret_upd = 0
  558.         MRS R12,PSP ; Read PSP
  559.         STMDB R12!,{R4-R11} ; Save Old context
  560.         STR R12,[R1,#TCB_TSTACK] ; Update os_tsk.run->tsk_stack
  561.         BL rt_stk_check ; Check for Stack overflow

  563.         POP {R2,R3}
  564.         STR R2,[R3] ; os_tsk.run = os_tsk.new

  566.         LDR R12,[R2,#TCB_TSTACK] ; os_tsk.new->tsk_stack
  567.         LDMIA R12!,{R4-R11} ; Restore New Context
  568.         MSR PSP,R12 ; Write PSP

  570.         LDRB R3,[R2,#TCB_RETUPD] ; Update ret_val?
  571.         CBZ R3,SysExit
  572.         LDRB R3,[R2,#TCB_RETVAL] ; Write os_tsk.new->ret_val
  573.         STR R3,[R12]
  574. SysExit
  575.         MVN LR,#:NOT:0xFFFFFFFD ; set EXC_RETURN value
  576.         BX LR ; Return to Thread Mode

  578. SysTick_Handler
  579.         BL __cpp(rt_systick)
  580.         B Sys_Switch

  582.         ALIGN
  583. }

  585. void rt_init_stack (P_TCB p_TCB, FUNCP task_body) {
  586.   /* Prepare TCB and saved context for a first time start of a task. */
  587.   U32 *stk,i,size;

  589.   /* Prepare a complete interrupt frame for first task start */
  590.   size = p_TCB->priv_stack >> 2;
  591.   if (size == 0) {
  592.     size = (U16)os_stackinfo >> 2;
  593.   }

  595.   /* Write to the top of stack. */
  596.   stk = &p_TCB->stack[size];

  598.   /* Auto correct to 8-byte ARM stack alignment. */
  599.   if ((U32)stk & 0x04) {
  600.     stk--;
  601.   }

  603.   stk -= 16;

  605.   /* Default xPSR and initial PC */
  606.   stk[15] = INITIAL_xPSR;
  607.   stk[14] = (U32)task_body;

  609.   /* Clear R1-R12,LR registers. */
  610.   for (i = 0; i < 14; i++) {
  611.     stk[i] = 0;
  612.   }

  614.   /* Assign a void pointer to R0. */
  615.   stk[8] = (U32)p_TCB->msg;

  617.   /* Initial Task stack pointer. */
  618.   p_TCB->tsk_stack = (U32)stk;

  620.   /* Task entry point. */
  621.   p_TCB->ptask = task_body;

  623.   /* Set a magic word for checking of stack overflow. */
  624.   p_TCB->stack[0] = MAGIC_WORD;
  625. }

  627. void dbg_init (void) {
  628.   if ((DEMCR & DEMCR_TRCENA) &&
  629.       (ITM_CONTROL & ITM_ITMENA) &&
  630.       (ITM_ENABLE & (1UL << 31))) {
  631.     dbg_msg = __TRUE;
  632.   }
  633. }

  635. void dbg_task_notify (P_TCB p_tcb, BOOL create) {
  636.   while (ITM_PORT31_U32 == 0);
  637.   ITM_PORT31_U32 = (U32)p_tcb->ptask;
  638.   while (ITM_PORT31_U32 == 0);
  639.   ITM_PORT31_U16 = (create << 8) | p_tcb->task_id;
  640. }

  642. void dbg_task_switch (U32 task_id) {
  643.   while (ITM_PORT31_U32 == 0);
  644.   ITM_PORT31_U8 = task_id;
  645. }
  646. //======================== HAL_CM3.c End ==========================================================

  648. //==================== rt_Robin.c ===============================================================
  649. __weak void rt_init_robin (void) {
  650.   /* Initialize Round Robin variables. */
  651.   os_robin.task = NULL;
  652.   os_robin.tout = (U16)os_rrobin;
  653. }

  655. __weak void rt_chk_robin (void) {
  656.   /* Check if Round Robin timeout expired and switch to the next ready task.*/
  657.   P_TCB p_new;

  659.   if (os_robin.task != os_rdy.p_lnk) {
  660.     /* New task was suspended, reset Round Robin timeout. */
  661.     os_robin.task = os_rdy.p_lnk;
  662.     os_robin.time = os_time + os_robin.tout - 1;
  663.   }
  664.   if (os_robin.time == os_time) {
  665.     /* Round Robin timeout has expired, swap Robin tasks. */
  666.     os_robin.task = NULL;
  667.     p_new = rt_get_first (&os_rdy);
  668.     rt_put_prio ((P_XCB)&os_rdy, p_new);
  669.   }
  670. }
  671. //=========================================================================================

  673. //--- rt_system.c
  674. #if 0
  675. __asm void $$RTX$$version (void) {
  676.    /* Export a version number symbol for a version control. */
  677.                 EXPORT __RTX_ARM_VER
  678. __RTX_ARM_VER EQU 0x412
  679. }
  680. #endif

  682. void rt_tsk_lock (void) {
  683.   /* Lock out tasks: prevents task switching by locking out scheduler */
  684.   OS_LOCK();
  685.   os_lock = __TRUE;
  686. }

  688. void rt_tsk_unlock (void) {
  689.   /* Enable System Tick Timer Interrupts. */
  690.   OS_UNLOCK();
  691.   os_lock = __FALSE;
  692.   if (os_psh_flag) {
  693.     OS_PEND_IRQ ();
  694.   }
  695. }

  697. void rt_psh_req (void) {
  698.   /* Initiate a post service handling request if required. */
  699.   if (os_lock == __FALSE) {
  700.     OS_PEND_IRQ ();
  701.   }
  702.   else {
  703.     os_psh_flag = __TRUE;
  704.   }
  705. }

  707. void rt_pop_req (void) {
  708.   /* Process an ISR post service requests. */
  709.   struct OS_XCB *p_CB;
  710.   P_TCB next;
  711.   U32 idx;

  713.   os_tsk.run->state = READY;
  714.   rt_put_rdy_first (os_tsk.run);

  716.   os_psh_flag = __FALSE;
  717.   idx = os_psq->last;
  718.   while (os_psq->count) {
  719.     p_CB = os_psq->q[idx].id;
  720.     if (p_CB->cb_type == TCB) {
  721.       /* Is of TCB type */
  722.       rt_evt_psh ((P_TCB)p_CB, (U16)os_psq->q[idx].arg);
  723.     }
  724.     else if (p_CB->cb_type == MCB) {
  725.       /* Is of MCB type */
  726.       rt_mbx_psh ((P_MCB)p_CB, (void *)os_psq->q[idx].arg);
  727.     }
  728.     else {
  729.       /* Must be of SCB type */
  730.       rt_sem_psh ((P_SCB)p_CB);
  731.     }
  732.     if (++idx == os_psq->size) idx = 0;
  733.     rt_dec (&os_psq->count);
  734.   }
  735.   os_psq->last = idx;

  737.   next = rt_get_first (&os_rdy);
  738.   rt_switch_req (next);
  739. }

  741. void rt_systick (void) {
  742.   /* Check for system clock update, suspend running task. */
  743.   P_TCB next;

  745.   os_tsk.run->state = READY;
  746.   rt_put_rdy_first (os_tsk.run);

  748.   /* Check Round Robin timeout. */
  749.   rt_chk_robin ();

  751.   /* Update delays. */
  752.   os_time++;
  753.   rt_dec_dly ();

  755.   /* Check the user timers. */
  756. #if OS_TMR_EN > 0
  757.       rt_tmr_tick ();
  758. #endif

  760.   /* Switch back to highest ready task */
  761.   next = rt_get_first (&os_rdy);
  762.   rt_switch_req (next);
  763. }

  765. __weak void rt_stk_check (void) {
  766.   /* Check for stack overflow. */
  767.   if ((os_tsk.run->tsk_stack < (U32)os_tsk.run->stack) ||
  768.       (os_tsk.run->stack[0] != MAGIC_WORD)) {
  769.     os_error (OS_ERR_STK_OVF);
  770.   }
  771. }
  772. //=========================================================================================

  774. //--- rt_task.c
  775. static OS_TID rt_get_TID (void) {
  776.   U32 tid;

  778.   for (tid = 1; tid <= os_maxtaskrun; tid++) {
  779.     if (os_active_TCB[tid-1] == NULL) {
  780.       return ((OS_TID)tid);
  781.     }
  782.   }
  783.   return (0);
  784. }

  786. static void rt_init_context (P_TCB p_TCB, U8 priority, FUNCP task_body) {
  787.   /* Initialize general part of the Task Control Block. */
  788.   p_TCB->cb_type = TCB;
  789.   p_TCB->state = READY;
  790.   p_TCB->prio = priority;
  791.   p_TCB->p_lnk = NULL;
  792.   p_TCB->p_rlnk = NULL;
  793.   p_TCB->p_dlnk = NULL;
  794.   p_TCB->p_blnk = NULL;
  795.   p_TCB->delta_time = 0;
  796.   p_TCB->interval_time = 0;
  797.   p_TCB->events = 0;
  798.   p_TCB->waits = 0;
  799.   p_TCB->ret_val = OS_R_OK;
  800.   p_TCB->ret_upd = 0;

  802.   if (p_TCB->priv_stack == 0) {
  803.     /* Allocate the memory space for the stack. */
  804.     p_TCB->stack = rt_alloc_box (mp_stk);
  805.   }
  806.   rt_init_stack (p_TCB, task_body);
  807. }

  809. void rt_switch_req (P_TCB p_new) {
  810.   /* Switch to next task (identified by "p_new"). */
  811.   os_tsk.new = p_new;
  812.   p_new->state = RUNNING;
  813.   DBG_TASK_SWITCH(p_new->task_id);
  814. }

  816. void rt_dispatch (P_TCB next_TCB) {
  817.   /* Dispatch next task if any identified or dispatch highest ready task */
  818.   /* "next_TCB" identifies a task to run or has value NULL (=no next task) */
  819.   if (next_TCB == NULL) {
  820.     /* Running task was blocked: continue with highest ready task */
  821.     next_TCB = rt_get_first (&os_rdy);
  822.     rt_switch_req (next_TCB);
  823.   }
  824.   else {
  825.     /* Check which task continues */
  826.     if (next_TCB->prio > os_tsk.run->prio) {
  827.       /* preempt running task */
  828.       rt_put_rdy_first (os_tsk.run);
  829.       os_tsk.run->state = READY;
  830.       rt_switch_req (next_TCB);
  831.     }
  832.     else {
  833.       /* put next task into ready list, no task switch takes place */
  834.       next_TCB->state = READY;
  835.       rt_put_prio (&os_rdy, next_TCB);
  836.     }
  837.   }
  838. }

  840. void rt_block (U16 timeout, U8 block_state) {
  841.   /* Block running task and choose next ready task. */
  842.   /* "timeout" sets a time-out value or is 0xffff (=no time-out). */
  843.   /* "block_state" defines the appropriate task state */
  844.   P_TCB next_TCB;

  846.   if (timeout) {
  847.     if (timeout < 0xffff) {
  848.       rt_put_dly (os_tsk.run, timeout);
  849.     }
  850.     os_tsk.run->state = block_state;
  851.     next_TCB = rt_get_first (&os_rdy);
  852.     rt_switch_req (next_TCB);
  853.   }
  854. }

  856. void rt_tsk_pass (void) {
  857.   /* Allow tasks of same priority level to run cooperatively.*/
  858.   P_TCB p_new;

  860.   p_new = rt_get_same_rdy_prio();
  861.   if (p_new != NULL) {
  862.     rt_put_prio ((P_XCB)&os_rdy, os_tsk.run);
  863.     os_tsk.run->state = READY;
  864.     rt_switch_req (p_new);
  865.   }
  866. }

  868. OS_TID rt_tsk_self (void) {
  869.   /* Return own task identifier value. */
  870.   if (os_tsk.run == NULL) {
  871.     return (0);
  872.   }
  873.   return (os_tsk.run->task_id);
  874. }

  876. OS_RESULT rt_tsk_prio (OS_TID task_id, U8 new_prio) {
  877.   /* Change execution priority of a task to "new_prio". */
  878.   P_TCB p_task;

  880.   if (task_id == 0) {
  881.     /* Change execution priority of calling task. */
  882.     os_tsk.run->prio = new_prio;
  883. run:if (rt_rdy_prio() > new_prio) {
  884.       rt_put_prio (&os_rdy, os_tsk.run);
  885.       os_tsk.run->state = READY;
  886.       os_tsk.run->ret_val = OS_R_OK;
  887.       rt_dispatch (NULL);
  888.     }
  889.     return (OS_R_OK);
  890.   }

  892.   /* Find the task in the "os_active_TCB" array. */
  893.   if (task_id > os_maxtaskrun || os_active_TCB[task_id-1] == NULL) {
  894.     /* Task with "task_id" not found or not started. */
  895.     return (OS_R_NOK);
  896.   }
  897.   p_task = os_active_TCB[task_id-1];
  898.   p_task->prio = new_prio;
  899.   if (p_task == os_tsk.run) {
  900.     goto run;
  901.   }
  902.   rt_resort_prio (p_task);
  903.   if (p_task->state == READY) {
  904.     /* Task enqueued in a ready list. */
  905.     p_task = rt_get_first (&os_rdy);
  906.     os_tsk.run->ret_val = OS_R_OK;
  907.     rt_dispatch (p_task);
  908.   }
  909.   return (OS_R_OK);
  910. }

  912. OS_TID rt_tsk_create (FUNCP task, U32 prio_stksz, void *stk, void *argv) {
  913.   /* Start a new task declared with "task". */
  914.   P_TCB task_context;
  915.   U32 i;

  917.   /* Priority 0 is reserved for idle */
  918.   if ((prio_stksz & 0xFF) == 0) {
  919.     prio_stksz += 1;
  920.   }
  921.   task_context = rt_alloc_box (mp_tcb);
  922.   if (task_context == NULL) {
  923.     return (0);
  924.   }
  925.   /* If "size != 0" use a private user provided stack. */
  926.   task_context->stack = stk;
  927.   task_context->priv_stack = prio_stksz >> 8;
  928.   /* Pass parameter 'argv' to 'rt_init_context' */
  929.   task_context->msg = argv;
  930.   /* For 'size == 0' system allocates the user stack from the memory pool. */
  931.   rt_init_context (task_context, prio_stksz & 0xFF, task);

  933.   /* Find a free entry in 'os_active_TCB' table. */
  934.   i = rt_get_TID ();
  935.   os_active_TCB[i-1] = task_context;
  936.   task_context->task_id = i;
  937.   DBG_TASK_NOTIFY(task_context, __TRUE);
  938.   rt_dispatch (task_context);
  939.   os_tsk.run->ret_val = i;
  940.   return ((OS_TID)i);
  941. }

  943. OS_RESULT rt_tsk_delete (OS_TID task_id) {
  944.   /* Terminate the task identified with "task_id". */
  945.   P_TCB task_context;

  947.   if (task_id == 0 || task_id == os_tsk.run->task_id) {
  948.     /* Terminate itself. */
  949.     os_tsk.run->state = INACTIVE;
  950.     os_active_TCB[os_tsk.run->task_id-1] = NULL;
  951.     rt_free_box (mp_stk, os_tsk.run->stack);
  952.     os_tsk.run->stack = NULL;
  953.     DBG_TASK_NOTIFY(os_tsk.run, __FALSE);
  954.     rt_free_box (mp_tcb, os_tsk.run);
  955.     os_tsk.run = NULL;
  956.     rt_dispatch (NULL);
  957.     /* The program should never come to this point. */
  958.   }
  959.   else {
  960.     /* Find the task in the "os_active_TCB" array. */
  961.     if (task_id > os_maxtaskrun || os_active_TCB[task_id-1] == NULL) {
  962.       /* Task with "task_id" not found or not started. */
  963.       return (OS_R_NOK);
  964.     }
  965.     task_context = os_active_TCB[task_id-1];
  966.     rt_rmv_list (task_context);
  967.     rt_rmv_dly (task_context);
  968.     os_active_TCB[task_id-1] = NULL;
  969.     rt_free_box (mp_stk, task_context->stack);
  970.     task_context->stack = NULL;
  971.     DBG_TASK_NOTIFY(task_context, __FALSE);
  972.     rt_free_box (mp_tcb, task_context);
  973.   }
  974.   return (OS_R_OK);
  975. }

  977. void rt_sys_init (FUNCP first_task, U32 prio_stksz, void *stk) {
  978.   /* Initialize system and start up task declared with "first_task". */
  979.   U32 i;

  981.   DBG_INIT();

  983.   /* Initialize dynamic memory and task TCB pointers to NULL. */
  984.   for (i = 0; i < os_maxtaskrun; i++) {
  985.     os_active_TCB[i] = NULL;
  986.   }
  987.   rt_init_box (&mp_tcb, mp_tcb_size, sizeof(struct OS_TCB));
  988.   rt_init_box (&mp_stk, mp_stk_size, BOX_ALIGN_8 | (U16)(os_stackinfo));
  989. #if (os_timernum & 0xff) > 0
  990.   rt_init_box ((U32 *)m_tmr, mp_tmr_size, sizeof(struct OS_TMR));
  991. #endif

  993.   /* Set up TCB of idle demon */
  994.   os_idle_TCB.task_id = 255;
  995.   os_idle_TCB.priv_stack = 0;
  996.   rt_init_context (&os_idle_TCB, 0, os_idle_demon);

  998.   /* Set up ready list: initially empty */
  999.   os_rdy.cb_type = HCB;
  1000.   os_rdy.p_lnk = NULL;
  1001.   /* Set up delay list: initially empty */
  1002.   os_dly.cb_type = HCB;
  1003.   os_dly.p_dlnk = NULL;
  1004.   os_dly.p_blnk = NULL;
  1005.   os_dly.delta_time = 0;

  1007.   /* Fix SP and systemvariables to assume idle task is running */
  1008.   /* Transform main program into idle task by assuming idle TCB */
  1009.   rt_set_PSP (os_idle_TCB.tsk_stack+32);
  1010.   os_tsk.run = &os_idle_TCB;
  1011.   os_tsk.run->state = RUNNING;

  1013.   /* Initialize ps queue */
  1014.   os_psq->first = 0;
  1015.   os_psq->last = 0;
  1016.   os_psq->size = os_fifo_size;

  1018.   /* Intitialize system clock timer */
  1019.   rt_tmr_init ();
  1020.   rt_init_robin ();

  1022.   /* Start up first user task before entering the endless loop */
  1023.   rt_tsk_create (first_task, prio_stksz, stk, NULL);
  1024. }
  1025. //=========================================================================================

  1027. //--- rt_time.c ----------------------------------------------------------
  1028. void rt_dly_wait (U16 delay_time) {
  1029.   /* Delay task by "delay_time" */
  1030.   rt_block (delay_time, WAIT_DLY);
  1031. }

  1033. void rt_itv_set (U16 interval_time) {
  1034.   /* Set interval length and define start of first interval */
  1035.   os_tsk.run->interval_time = interval_time;
  1036.   os_tsk.run->delta_time = interval_time + os_time;
  1037. }

  1039. void rt_itv_wait (void) {
  1040.   /* Wait for interval end and define start of next one */
  1041.   U16 delta;

  1043.   delta = os_tsk.run->delta_time - os_time;
  1044.   os_tsk.run->delta_time += os_tsk.run->interval_time;
  1045.   if ((delta & 0x8000) == 0) {
  1046.     rt_block (delta, WAIT_ITV);
  1047.   }
  1048. }
  1049. //=========================================================================================

  1051. //--- rt_membox.c
  1052. int _init_box (void *box_mem, U32 box_size, U32 blk_size) {
  1053.   /* Initialize memory block system, returns 0 if OK, 1 if fails. */
  1054.   void *end;
  1055.   void *blk;
  1056.   void *next;
  1057.   U32 sizeof_bm;

  1059.   /* Create memory structure. */
  1060.   if (blk_size & BOX_ALIGN_8) {
  1061.     /* Memory blocks 8-byte aligned. */
  1062.     blk_size = ((blk_size & ~BOX_ALIGN_8) + 7) & ~7;
  1063.     sizeof_bm = (sizeof (struct OS_BM) + 7) & ~7;
  1064.   }
  1065.   else {
  1066.     /* Memory blocks 4-byte aligned. */
  1067.     blk_size = (blk_size + 3) & ~3;
  1068.     sizeof_bm = sizeof (struct OS_BM);
  1069.   }
  1070.   if (blk_size == 0) {
  1071.     return (1);
  1072.   }
  1073.   if ((blk_size + sizeof_bm) > box_size) {
  1074.     return (1);
  1075.   }
  1076.   /* Create a Memory structure. */
  1077.   blk = ((U8 *) box_mem) + sizeof_bm;
  1078.   ((P_BM) box_mem)->free = blk;
  1079.   end = ((U8 *) box_mem) + box_size;
  1080.   ((P_BM) box_mem)->end = end;
  1081.   ((P_BM) box_mem)->blk_size = blk_size;

  1083.   /* Link all free blocks using offsets. */
  1084.   end = ((U8 *) end) - blk_size;
  1085.   while (1) {
  1086.     next = ((U8 *) blk) + blk_size;
  1087.     if (next > end) break;
  1088.     *((void **)blk) = next;
  1089.     blk = next;
  1090.   }
  1091.   /* end marker */
  1092.   *((void **)blk) = 0;
  1093.   return (0);
  1094. }

  1096. void *rt_alloc_box (void *box_mem) {
  1097.   /* Allocate a memory block and return start address. */
  1098.   void **free;
  1099.   int irq_dis;

  1101.   irq_dis = __disable_irq ();
  1102.   free = ((P_BM) box_mem)->free;
  1103.   if (free) {
  1104.     ((P_BM) box_mem)->free = *free;
  1105.   }
  1106.   if (!irq_dis) __enable_irq ();
  1107.   return (free);
  1108. }

  1110. void *_calloc_box (void *box_mem) {
  1111.   /* Allocate a 0-initialized memory block and return start address. */
  1112.   void *free;
  1113.   U32 *p;
  1114.   U32 i;

  1116.   free = _alloc_box (box_mem);
  1117.   if (free) {
  1118.     p = free;
  1119.     for (i = ((P_BM) box_mem)->blk_size; i; i -= 4) {
  1120.       *p = 0;
  1121.       p++;
  1122.     }
  1123.   }
  1124.   return (free);
  1125. }

  1127. int rt_free_box (void *box_mem, void *box) {
  1128.   /* Free a memory block, returns 0 if OK, 1 if box does not belong to box_mem */
  1129.   int irq_dis;

  1131.   if (box < box_mem || box > ((P_BM) box_mem)->end) {
  1132.     return (1);
  1133.   }
  1134.   irq_dis = __disable_irq ();
  1135.   *((void **)box) = ((P_BM) box_mem)->free;
  1136.   ((P_BM) box_mem)->free = box;
  1137.   if (!irq_dis) __enable_irq ();
  1138.   return (0);
  1139. }
  1140. //==============================================================================================

  1142. //----------------------------- rt_List.c -------------------------------------------
  1143. void rt_put_prio (P_XCB p_CB, P_TCB p_task) {
  1144.   /* Put task identified with "p_task" into list ordered by priority. */
  1145.   /* "p_CB" points to head of list; list has always an element at end with */
  1146.   /* a priority less than "p_task->prio". */
  1147.   P_TCB p_CB2;
  1148.   U32 prio;
  1149.   BOOL sem_mbx = __FALSE;

  1151.   if (p_CB->cb_type == SCB || p_CB->cb_type == MCB || p_CB->cb_type == MUCB) {
  1152.     sem_mbx = __TRUE;
  1153.   }
  1154.   prio = p_task->prio;
  1155.   p_CB2 = p_CB->p_lnk;
  1156.   /* Search for an entry in the list */
  1157.   while (p_CB2 != NULL && prio <= p_CB2->prio) {
  1158.     p_CB = (P_XCB)p_CB2;
  1159.     p_CB2 = p_CB2->p_lnk;
  1160.   }
  1161.   /* Entry found, insert the task into the list */
  1162.   p_task->p_lnk = p_CB2;
  1163.   p_CB->p_lnk = p_task;
  1164.   if (sem_mbx) {
  1165.     if (p_CB2 != NULL) {
  1166.       p_CB2->p_rlnk = p_task;
  1167.     }
  1168.     p_task->p_rlnk = (P_TCB)p_CB;
  1169.   }
  1170.   else {
  1171.     p_task->p_rlnk = NULL;
  1172.   }
  1173. }

  1175. P_TCB rt_get_first (P_XCB p_CB) {
  1176.   /* Get task at head of list: it is the task with highest priority. */
  1177.   /* "p_CB" points to head of list. */
  1178.   P_TCB p_first;

  1180.   p_first = p_CB->p_lnk;
  1181.   p_CB->p_lnk = p_first->p_lnk;
  1182.   if (p_CB->cb_type == SCB || p_CB->cb_type == MCB || p_CB->cb_type == MUCB) {
  1183.     if (p_first->p_lnk != NULL) {
  1184.       p_first->p_lnk->p_rlnk = (P_TCB)p_CB;
  1185.       p_first->p_lnk = NULL;
  1186.     }
  1187.     p_first->p_rlnk = NULL;
  1188.   }
  1189.   else {
  1190.     p_first->p_lnk = NULL;
  1191.   }
  1192.   return (p_first);
  1193. }

  1195. void rt_put_rdy_first (P_TCB p_task) {
  1196.   /* Put task identified with "p_task" at the head of the ready list. The */
  1197.   /* task must have at least a priority equal to highest priority in list. */
  1198.   p_task->p_lnk = os_rdy.p_lnk;
  1199.   p_task->p_rlnk = NULL;
  1200.   os_rdy.p_lnk = p_task;
  1201. }

  1203. P_TCB rt_get_same_rdy_prio (void) {
  1204.   /* Remove a task of same priority from ready list if any exists. Other- */
  1205.   /* wise return NULL. */
  1206.   P_TCB p_first;

  1208.   p_first = os_rdy.p_lnk;
  1209.   if (p_first->prio == os_tsk.run->prio) {
  1210.     os_rdy.p_lnk = os_rdy.p_lnk->p_lnk;
  1211.     return (p_first);
  1212.   }
  1213.   return (NULL);
  1214. }

  1216. void rt_resort_prio (P_TCB p_task) {
  1217.   /* Re-sort ordered lists after the priority of 'p_task' has changed. */
  1218.   P_TCB p_CB;

  1220.   if (p_task->p_rlnk == NULL) {
  1221.     if (p_task->state == READY) {
  1222.       /* Task is chained into READY list. */
  1223.       p_CB = (P_TCB)&os_rdy;
  1224.       goto res;
  1225.     }
  1226.   }
  1227.   else {
  1228.     p_CB = p_task->p_rlnk;
  1229.     while (p_CB->cb_type == TCB) {
  1230.       /* Find a header of this task chain list. */
  1231.       p_CB = p_CB->p_rlnk;
  1232.     }
  1233. res:rt_rmv_list (p_task);
  1234.     rt_put_prio ((P_XCB)p_CB, p_task);
  1235.   }
  1236. }

  1238. void rt_put_dly (P_TCB p_task, U16 delay) {
  1239.   /* Put a task identified with "p_task" into chained delay wait list using */
  1240.   /* a delay value of "delay". */
  1241.   P_TCB p;
  1242.   U32 delta,idelay = delay;

  1244.   p = (P_TCB)&os_dly;
  1245.   if (p->p_dlnk == NULL) {
  1246.     /* Delay list empty */
  1247.     delta = 0;
  1248.     goto last;
  1249.   }
  1250.   delta = os_dly.delta_time;
  1251.   while (delta < idelay) {
  1252.     if (p->p_dlnk == NULL) {
  1253.       /* End of list found */
  1254. last: p_task->p_dlnk = NULL;
  1255.       p->p_dlnk = p_task;
  1256.       p_task->p_blnk = p;
  1257.       p->delta_time = (U16)(idelay - delta);
  1258.       p_task->delta_time = 0;
  1259.       return;
  1260.     }
  1261.     p = p->p_dlnk;
  1262.     delta += p->delta_time;
  1263.   }
  1264.   /* Right place found */
  1265.   p_task->p_dlnk = p->p_dlnk;
  1266.   p->p_dlnk = p_task;
  1267.   p_task->p_blnk = p;
  1268.   if (p_task->p_dlnk != NULL) {
  1269.     p_task->p_dlnk->p_blnk = p_task;
  1270.   }
  1271.   p_task->delta_time = (U16)(delta - idelay);
  1272.   p->delta_time -= p_task->delta_time;
  1273. }

  1275. void rt_dec_dly (void) {
  1276.   /* Decrement delta time of list head: remove tasks having a value of zero.*/
  1277.   P_TCB p_rdy;

  1279.   if (os_dly.p_dlnk == NULL) {
  1280.     return;
  1281.   }
  1282.   os_dly.delta_time--;
  1283.   while ((os_dly.delta_time == 0) && (os_dly.p_dlnk != NULL)) {
  1284.     p_rdy = os_dly.p_dlnk;
  1285.     if (p_rdy->p_rlnk != NULL) {
  1286.       /* Task is really enqueued, remove task from semaphore/mailbox */
  1287.       /* timeout waiting list. */
  1288.       p_rdy->p_rlnk->p_lnk = p_rdy->p_lnk;
  1289.       if (p_rdy->p_lnk != NULL) {
  1290.         p_rdy->p_lnk->p_rlnk = p_rdy->p_rlnk;
  1291.         p_rdy->p_lnk = NULL;
  1292.       }
  1293.       p_rdy->p_rlnk = NULL;
  1294.     }
  1295.     rt_put_prio (&os_rdy, p_rdy);
  1296.     os_dly.delta_time = p_rdy->delta_time;
  1297.     if (p_rdy->state == WAIT_ITV) {
  1298.       /* Calculate the next time for interval wait. */
  1299.       p_rdy->delta_time = p_rdy->interval_time + os_time;
  1300.     }
  1301.     p_rdy->state = READY;
  1302.     p_rdy->ret_val = OS_R_TMO;
  1303.     os_dly.p_dlnk = p_rdy->p_dlnk;
  1304.     if (p_rdy->p_dlnk != NULL) {
  1305.       p_rdy->p_dlnk->p_blnk = (P_TCB)&os_dly;
  1306.       p_rdy->p_dlnk = NULL;
  1307.     }
  1308.     p_rdy->p_blnk = NULL;
  1309.   }
  1310. }

  1312. void rt_rmv_list (P_TCB p_task) {
  1313.   /* Remove task identified with "p_task" from ready, semaphore or mailbox */
  1314.   /* waiting list if enqueued. */
  1315.   P_TCB p_b;

  1317.   if (p_task->p_rlnk != NULL) {
  1318.     /* A task is enqueued in semaphore / mailbox waiting list. */
  1319.     p_task->p_rlnk->p_lnk = p_task->p_lnk;
  1320.     if (p_task->p_lnk != NULL) {
  1321.       p_task->p_lnk->p_rlnk = p_task->p_rlnk;
  1322.     }
  1323.     return;
  1324.   }

  1326.   p_b = (P_TCB)&os_rdy;
  1327.   while (p_b != NULL) {
  1328.     /* Search the ready list for task "p_task" */
  1329.     if (p_b->p_lnk == p_task) {
  1330.       p_b->p_lnk = p_task->p_lnk;
  1331.       return;
  1332.     }
  1333.     p_b = p_b->p_lnk;
  1334.   }
  1335. }

  1337. void rt_rmv_dly (P_TCB p_task) {
  1338.   /* Remove task identified with "p_task" from delay list if enqueued. */
  1339.   P_TCB p_b;

  1341.   p_b = p_task->p_blnk;
  1342.   if (p_b != NULL) {
  1343.     /* Task is really enqueued */
  1344.     p_b->p_dlnk = p_task->p_dlnk;
  1345.     if (p_task->p_dlnk != NULL) {
  1346.       /* 'p_task' is in the middle of list */
  1347.       p_b->delta_time += p_task->delta_time;
  1348.       p_task->p_dlnk->p_blnk = p_b;
  1349.       p_task->p_dlnk = NULL;
  1350.     }
  1351.     else {
  1352.       /* 'p_task' is at the end of list */
  1353.       p_b->delta_time = 0;
  1354.     }
  1355.     p_task->p_blnk = NULL;
  1356.   }
  1357. }

  1359. void rt_psq_enq (OS_ID entry, U32 arg) {
  1360.   /* Insert post service request "entry" into ps-queue. */
  1361.   U32 idx;

  1363.   idx = rt_inc_qi (os_psq->size, &os_psq->count, &os_psq->first);
  1364.   if (idx < os_psq->size) {
  1365.     os_psq->q[idx].id = entry;
  1366.     os_psq->q[idx].arg = arg;
  1367.   }
  1368.   else {
  1369.     os_error (OS_ERR_FIFO_OVF);
  1370.   }
  1371. }
  1372. //==================================================================================

  1374. //---------------- rt_Semaphore.c --------------------------------------------------
  1375. #if OS_SEM_EN > 0
  1376. void rt_sem_init (OS_ID semaphore, U16 token_count) {
  1377.   /* Initialize a semaphore */
  1378.   P_SCB p_SCB = semaphore;

  1380.   p_SCB->cb_type = SCB;
  1381.   p_SCB->p_lnk = NULL;
  1382.   p_SCB->tokens = token_count;
  1383. }

  1385. OS_RESULT rt_sem_send (OS_ID semaphore) {
  1386.   /* Return a token to semaphore */
  1387.   P_SCB p_SCB = semaphore;
  1388.   P_TCB p_TCB;

  1390.   if (p_SCB->p_lnk != NULL) {
  1391.     /* A task is waiting for token */
  1392.     p_TCB = rt_get_first ((P_XCB)p_SCB);
  1393.     p_TCB->ret_val = OS_R_SEM;
  1394.     rt_rmv_dly (p_TCB);
  1395.     rt_dispatch (p_TCB);
  1396.     os_tsk.run->ret_val = OS_R_OK;
  1397.   }
  1398.   else {
  1399.     /* Store token. */
  1400.     p_SCB->tokens++;
  1401.   }
  1402.   return (OS_R_OK);
  1403. }

  1405. OS_RESULT rt_sem_wait (OS_ID semaphore, U16 timeout) {
  1406.   /* Obtain a token; possibly wait for it */
  1407.   P_SCB p_SCB = semaphore;

  1409.   if (p_SCB->tokens) {
  1410.     p_SCB->tokens--;
  1411.     return (OS_R_OK);
  1412.   }
  1413.   /* No token available: wait for one */
  1414.   if (timeout == 0) {
  1415.     return (OS_R_TMO);
  1416.   }
  1417.   if (p_SCB->p_lnk != NULL) {
  1418.     rt_put_prio ((P_XCB)p_SCB, os_tsk.run);
  1419.   }
  1420.   else {
  1421.     p_SCB->p_lnk = os_tsk.run;
  1422.     os_tsk.run->p_lnk = NULL;
  1423.     os_tsk.run->p_rlnk = (P_TCB)p_SCB;
  1424.   }
  1425.   rt_block(timeout, WAIT_SEM);
  1426.   return (OS_R_TMO);
  1427. }

  1429. void isr_sem_send (OS_ID semaphore) {
  1430.   /* Same function as "os_sem"send", but to be called by ISRs */
  1431.   P_SCB p_SCB = semaphore;

  1433.   rt_psq_enq (p_SCB, 0);
  1434.   rt_psh_req ();
  1435. }
  1436. #endif

  1438. void rt_sem_psh (P_SCB p_CB) {
  1439.   /* Check if task has to be waken up */
  1440.   P_TCB p_TCB;

  1442.   if (p_CB->p_lnk != NULL) {
  1443.     /* A task is waiting for token */
  1444.     p_TCB = rt_get_first ((P_XCB)p_CB);
  1445.     rt_rmv_dly (p_TCB);
  1446.     p_TCB->state = READY;
  1447.     p_TCB->ret_val = OS_R_SEM;
  1448.     rt_put_prio (&os_rdy, p_TCB);
  1449.   }
  1450.   else {
  1451.     /* Store token */
  1452.     p_CB->tokens++;
  1453.   }
  1454. }
  1455. //=========================================================================================

  1457. //----------------------- rt_Mutex.c --------------------------------------------------
  1458. #if OS_MUTEX_EN > 0
  1459. void rt_mut_init (OS_ID mutex) {
  1460.   /* Initialize a mutex object */
  1461.   P_MUCB p_MCB = mutex;

  1463.   p_MCB->cb_type = MUCB;
  1464.   p_MCB->prio = 0;
  1465.   p_MCB->level = 0;
  1466.   p_MCB->p_lnk = NULL;
  1467.   p_MCB->owner = NULL;
  1468. }

  1470. OS_RESULT rt_mut_release (OS_ID mutex) {
  1471.   /* Release a mutex object */
  1472.   P_MUCB p_MCB = mutex;
  1473.   P_TCB p_TCB;

  1475.   if (p_MCB->level == 0 || p_MCB->owner != os_tsk.run) {
  1476.     /* Unbalanced mutex release or task is not the owner */
  1477.     return (OS_R_NOK);
  1478.   }
  1479.   if (--p_MCB->level != 0) {
  1480.     return (OS_R_OK);
  1481.   }
  1482.   /* Restore owner task's priority. */
  1483.   os_tsk.run->prio = p_MCB->prio;
  1484.   if (p_MCB->p_lnk != NULL) {
  1485.     /* A task is waiting for mutex. */
  1486.     p_TCB = rt_get_first ((P_XCB)p_MCB);
  1487.     p_TCB->ret_val = OS_R_MUT;
  1488.     rt_rmv_dly (p_TCB);
  1489.     /* A waiting task becomes the owner of this mutex. */
  1490.     p_MCB->level = 1;
  1491.     p_MCB->owner = p_TCB;
  1492.     p_MCB->prio = p_TCB->prio;
  1493.     /* Priority inversion, check which task continues. */
  1494.     if (os_tsk.run->prio >= rt_rdy_prio()) {
  1495.       rt_dispatch (p_TCB);
  1496.     }
  1497.     else {
  1498.       /* Ready task has higher priority than running task. */
  1499.       rt_put_prio (&os_rdy, os_tsk.run);
  1500.       rt_put_prio (&os_rdy, p_TCB);
  1501.       os_tsk.run->state = READY;
  1502.       p_TCB->state = READY;
  1503.       rt_dispatch (NULL);
  1504.     }
  1505.     os_tsk.run->ret_val = OS_R_OK;
  1506.   }
  1507.   else {
  1508.     /* Check if own priority raised by priority inversion. */
  1509.     if (rt_rdy_prio() > os_tsk.run->prio) {
  1510.       rt_put_prio (&os_rdy, os_tsk.run);
  1511.       os_tsk.run->state = READY;
  1512.       rt_dispatch (NULL);
  1513.       os_tsk.run->ret_val = OS_R_OK;
  1514.     }
  1515.   }
  1516.   return (OS_R_OK);
  1517. }

  1519. OS_RESULT rt_mut_wait (OS_ID mutex, U16 timeout) {
  1520.   /* Wait for a mutex, continue when mutex is free. */
  1521.   P_MUCB p_MCB = mutex;

  1523.   if (p_MCB->level == 0) {
  1524.     p_MCB->owner = os_tsk.run;
  1525.     p_MCB->prio = os_tsk.run->prio;
  1526.     goto inc;
  1527.   }
  1528.   if (p_MCB->owner == os_tsk.run) {
  1529.     /* OK, running task is the owner of this mutex. */
  1530. inc:p_MCB->level++;
  1531.     return (OS_R_OK);
  1532.   }
  1533.   /* Mutex owned by another task, wait until released. */
  1534.   if (timeout == 0) {
  1535.     return (OS_R_TMO);
  1536.   }
  1537.   /* Raise the owner task priority if lower than current priority. */
  1538.   /* This priority inversion is called priority inheritance. */
  1539.   if (p_MCB->prio < os_tsk.run->prio) {
  1540.     p_MCB->owner->prio = os_tsk.run->prio;
  1541.     rt_resort_prio (p_MCB->owner);
  1542.   }
  1543.   if (p_MCB->p_lnk != NULL) {
  1544.     rt_put_prio ((P_XCB)p_MCB, os_tsk.run);
  1545.   }
  1546.   else {
  1547.     p_MCB->p_lnk = os_tsk.run;
  1548.     os_tsk.run->p_lnk = NULL;
  1549.     os_tsk.run->p_rlnk = (P_TCB)p_MCB;
  1550.   }
  1551.   rt_block(timeout, WAIT_MUT);
  1552.   return (OS_R_TMO);
  1553. }
  1554. #endif
  1555. //=========================================================================================

  1557. //------------------------------ rt_Event.c ------------------------------------
  1558. #if OS_FLAG_EN > 0
  1559. OS_RESULT rt_evt_wait (U16 wait_flags, U16 timeout, BOOL and_wait) {
  1560.   /* Wait for one or more event flags with optional time-out. */
  1561.   /* "wait_flags" identifies the flags to wait for. */
  1562.   /* "timeout" is the time-out limit in system ticks (0xffff if no time-out) */
  1563.   /* "and_wait" specifies the AND-ing of "wait_flags" as condition to be met */
  1564.   /* to complete the wait. (OR-ing if set to 0). */
  1565.   U32 block_state;

  1567.   if (and_wait) {
  1568.     /* Check for AND-connected events */
  1569.     if ((os_tsk.run->events & wait_flags) == wait_flags) {
  1570.       os_tsk.run->events &= ~wait_flags;
  1571.       return (OS_R_EVT);
  1572.     }
  1573.     block_state = WAIT_AND;
  1574.   }
  1575.   else {
  1576.     /* Check for OR-connected events */
  1577.     if (os_tsk.run->events & wait_flags) {
  1578.       os_tsk.run->waits = os_tsk.run->events & wait_flags;
  1579.       os_tsk.run->events &= ~wait_flags;
  1580.       return (OS_R_EVT);
  1581.     }
  1582.     block_state = WAIT_OR;
  1583.   }
  1584.   /* Task has to wait */
  1585.   os_tsk.run->waits = wait_flags;
  1586.   rt_block (timeout, (U8)block_state);
  1587.   return (OS_R_TMO);
  1588. }

  1590. void rt_evt_set (U16 event_flags, OS_TID task_id) {
  1591.   /* Set one or more event flags of a selectable task. */
  1592.   P_TCB p_tcb;

  1594.   p_tcb = os_active_TCB[task_id-1];
  1595.   if (p_tcb == NULL) {
  1596.     return;
  1597.   }
  1598.   p_tcb->events |= event_flags;
  1599.   event_flags = p_tcb->waits;
  1600.   /* If the task is not waiting for an event, it should not be put */
  1601.   /* to ready state. */
  1602.   if (p_tcb->state == WAIT_AND) {
  1603.     /* Check for AND-connected events */
  1604.     if ((p_tcb->events & event_flags) == event_flags) {
  1605.       goto wkup;
  1606.     }
  1607.   }
  1608.   if (p_tcb->state == WAIT_OR) {
  1609.     /* Check for OR-connected events */
  1610.     if (p_tcb->events & event_flags) {
  1611.       p_tcb->waits &= p_tcb->events;
  1612. wkup: p_tcb->events &= ~event_flags;
  1613.       rt_rmv_dly (p_tcb);
  1614.       p_tcb->events &= ~event_flags;
  1615.       p_tcb->state = READY;
  1616.       p_tcb->ret_val = OS_R_EVT;
  1617.       rt_dispatch (p_tcb);
  1618.     }
  1619.   }
  1620. }

  1622. void rt_evt_clr (U16 clear_flags, OS_TID task_id) {
  1623.   /* Clear one or more event flags (identified by "clear_flags") of a */
  1624.   /* selectable task (identified by "task"). */
  1625.   P_TCB task = os_active_TCB[task_id-1];

  1627.   if (task == NULL) {
  1628.     return;
  1629.   }
  1630.   task->events &= ~clear_flags;
  1631. }

  1633. void isr_evt_set (U16 event_flags, OS_TID task_id) {
  1634.   /* Same function as "os_evt_set", but to be called by ISRs. */
  1635.   P_TCB p_tcb = os_active_TCB[task_id-1];

  1637.   if (p_tcb == NULL) {
  1638.     return;
  1639.   }
  1640.   rt_psq_enq (p_tcb, event_flags);
  1641.   rt_psh_req ();
  1642. }

  1644. U16 rt_evt_get (void) {
  1645.   /* Get events of a running task after waiting for OR connected events. */
  1646.   return (os_tsk.run->waits);
  1647. }
  1648. #endif

  1650. void rt_evt_psh (P_TCB p_CB, U16 set_flags) {
  1651.   /* Check if task has to be waken up */
  1652.   U16 event_flags;

  1654.   p_CB->events |= set_flags;
  1655.   event_flags = p_CB->waits;
  1656.   if (p_CB->state == WAIT_AND) {
  1657.     /* Check for AND-connected events */
  1658.     if ((p_CB->events & event_flags) == event_flags) {
  1659.       goto rdy;
  1660.     }
  1661.   }
  1662.   if (p_CB->state == WAIT_OR) {
  1663.     /* Check for OR-connected events */
  1664.     if (p_CB->events & event_flags) {
  1665.       p_CB->waits &= p_CB->events;
  1666. rdy: p_CB->events &= ~event_flags;
  1667.       rt_rmv_dly (p_CB);
  1668.       p_CB->state = READY;
  1669.       p_CB->ret_val = OS_R_EVT;
  1670.       rt_put_prio (&os_rdy, p_CB);
  1671.     }
  1672.   }
  1673. }
  1674. //=========================================================================================

  1676. //---- rt_mailbox.c ----------------------------------------------------------------------------
  1677. #if OS_MBOX_EN > 0
  1678. void rt_mbx_init (OS_ID mailbox, U16 mbx_size) {
  1679.   /* Initialize a mailbox */
  1680.   P_MCB p_MCB = mailbox;

  1682.   p_MCB->cb_type = MCB;
  1683.   p_MCB->isr_st = 0;
  1684.   p_MCB->p_lnk = NULL;
  1685.   p_MCB->first = 0;
  1686.   p_MCB->last = 0;
  1687.   p_MCB->count = 0;
  1688.   p_MCB->size = (mbx_size + sizeof(void *) - sizeof(struct OS_MCB)) /
  1689.                                                      (U32)sizeof (void *);
  1690. }

  1692. OS_RESULT rt_mbx_send (OS_ID mailbox, void *p_msg, U16 timeout) {
  1693.   /* Send message to a mailbox */
  1694.   P_MCB p_MCB = mailbox;
  1695.   P_TCB p_TCB;

  1697.   if (p_MCB->p_lnk != NULL && p_MCB->count == 0) {
  1698.     /* A task is waiting for message */
  1699.     p_TCB = rt_get_first ((P_XCB)p_MCB);
  1700.     *p_TCB->msg = p_msg;
  1701.     p_TCB->ret_val = OS_R_MBX;
  1702.     rt_rmv_dly (p_TCB);
  1703.     rt_dispatch (p_TCB);
  1704.     os_tsk.run->ret_val = OS_R_OK;
  1705.   }
  1706.   else {
  1707.     /* Store message in mailbox queue */
  1708.     if (p_MCB->count == p_MCB->size) {
  1709.       /* No free message entry, wait for one. If message queue is full, */
  1710.       /* then no task is waiting for message. The 'p_MCB->p_lnk' list */
  1711.       /* pointer can now be reused for send message waits task list. */
  1712.       if (timeout == 0) {
  1713.         return (OS_R_TMO);
  1714.       }
  1715.       if (p_MCB->p_lnk != NULL) {
  1716.         rt_put_prio ((P_XCB)p_MCB, os_tsk.run);
  1717.       }
  1718.       else {
  1719.         p_MCB->p_lnk = os_tsk.run;
  1720.         os_tsk.run->p_lnk = NULL;
  1721.         os_tsk.run->p_rlnk = (P_TCB)p_MCB;
  1722.         /* Signal the 'isr_mbx_receive ()' that the task is waiting */
  1723.         /* to send a message */
  1724.         p_MCB->isr_st = 1;
  1725.       }
  1726.       os_tsk.run->msg = p_msg;
  1727.       rt_block (timeout, WAIT_MBX);
  1728.       return (OS_R_TMO);
  1729.     }
  1730.     /* Yes, there is a free entry in a mailbox. */
  1731.     p_MCB->msg[p_MCB->first] = p_msg;
  1732.     rt_inc (&p_MCB->count);
  1733.     if (++p_MCB->first == p_MCB->size) {
  1734.       p_MCB->first = 0;
  1735.     }
  1736.   }
  1737.   return (OS_R_OK);
  1738. }

  1740. OS_RESULT rt_mbx_wait (OS_ID mailbox, void **message, U16 timeout) {
  1741.   /* Receive a message; possibly wait for it */
  1742.   P_MCB p_MCB = mailbox;
  1743.   P_TCB p_TCB;

  1745.   /* If a message is available in the fifo buffer */
  1746.   /* remove it from the fifo buffer and return. */
  1747.   if (p_MCB->count) {
  1748.     *message = p_MCB->msg[p_MCB->last];
  1749.     if (++p_MCB->last == p_MCB->size) {
  1750.       p_MCB->last = 0;
  1751.     }
  1752.     if (p_MCB->p_lnk != NULL) {
  1753.       /* A task is waiting to send message */
  1754.       p_TCB = rt_get_first ((P_XCB)p_MCB);
  1755.       p_TCB->ret_val = OS_R_OK;
  1756.       p_MCB->msg[p_MCB->first] = p_TCB->msg;
  1757.       if (++p_MCB->first == p_MCB->size) {
  1758.         p_MCB->first = 0;
  1759.       }
  1760.       rt_rmv_dly (p_TCB);
  1761.       rt_dispatch (p_TCB);
  1762.       os_tsk.run->ret_val = OS_R_OK;
  1763.     }
  1764.     else {
  1765.       rt_dec (&p_MCB->count);
  1766.     }
  1767.     return (OS_R_OK);
  1768.   }
  1769.   /* No message available: wait for one */
  1770.   if (timeout == 0) {
  1771.     return (OS_R_TMO);
  1772.   }
  1773.   if (p_MCB->p_lnk != NULL) {
  1774.     rt_put_prio ((P_XCB)p_MCB, os_tsk.run);
  1775.   }
  1776.   else {
  1777.     p_MCB->p_lnk = os_tsk.run;
  1778.     os_tsk.run->p_lnk = NULL;
  1779.     os_tsk.run->p_rlnk = (P_TCB)p_MCB;
  1780.   }
  1781.   rt_block(timeout, WAIT_MBX);
  1782.   os_tsk.run->msg = message;
  1783.   return (OS_R_TMO);
  1784. }

  1786. OS_RESULT rt_mbx_check (OS_ID mailbox) {
  1787.   /* Check for free space in a mailbox. Returns the number of messages */
  1788.   /* that can be stored to a mailbox. It returns 0 when mailbox is full. */
  1789.   P_MCB p_MCB = mailbox;

  1791.   return (p_MCB->size - p_MCB->count);
  1792. }

  1794. void isr_mbx_send (OS_ID mailbox, void *p_msg) {
  1795.   /* Same function as "os_mbx_send", but to be called by ISRs. */
  1796.   P_MCB p_MCB = mailbox;

  1798.   rt_psq_enq (p_MCB, (U32)p_msg);
  1799.   rt_psh_req ();
  1800. }

  1802. //---V4.13
  1803. OS_RESULT isr_mbx_receive (OS_ID mailbox, void **message) {
  1804.   /* Receive a message in the interrupt function. The interrupt function */
  1805.   /* should not wait for a message since this would block the rtx os. */
  1806.   P_MCB p_MCB = mailbox;

  1808.   if (p_MCB->count) {
  1809.     /* A message is available in the fifo buffer. */
  1810.     *message = p_MCB->msg[p_MCB->last];
  1811.     if (p_MCB->isr_st == 1) {
  1812.       /* A task is locked waiting to send message */
  1813.       p_MCB->isr_st = 2;
  1814.       rt_psq_enq (p_MCB, 0);
  1815.       rt_psh_req ();
  1816.     }
  1817.     rt_dec (&p_MCB->count);
  1818.     if (++p_MCB->last == p_MCB->size) {
  1819.       p_MCB->last = 0;
  1820.     }
  1821.     return (OS_R_MBX);
  1822.   }
  1823.   return (OS_R_OK);
  1824. }

  1826. //---V4.12
  1827. #if 0
  1828. OS_RESULT isr_mbx_receive_old (OS_ID mailbox, void **message) {
  1829.   /* Receive a message in the interrupt function. The interrupt function */
  1830.   /* should not wait for a message since this would block the rtx os. */
  1831.   P_MCB p_MCB = mailbox;

  1833.   if (p_MCB->count) {
  1834.     /* A message is available in the fifo buffer. */
  1835.     *message = p_MCB->msg[p_MCB->last];
  1836.     if (p_MCB->isr_st == 1) {
  1837.       /* A task is locked waiting to send message */
  1838.       p_MCB->isr_st = 2;
  1839.       rt_psq_enq (p_MCB, 0);
  1840.       rt_psh_req ();
  1841.     }
  1842.     rt_dec (&p_MCB->count);
  1843.     if (++p_MCB->last == p_MCB->size) {
  1844.       p_MCB->last = 0;
  1845.     }
  1846.     return (OS_R_MBX);
  1847.   }
  1848.   return (OS_R_OK);
  1849. }
  1850. #endif
  1851. #endif

  1853. void rt_mbx_psh (P_MCB p_CB, void *p_msg) {
  1854.   /* Store the message to the mailbox queue or pass it to task directly. */
  1855.   P_TCB p_TCB;

  1857.   /* Check if this was an 'isr_mbx_receive ()' post service request. */
  1858.   if (p_CB->p_lnk != NULL && p_CB->isr_st == 2) {
  1859.     /* A task is waiting to send message, remove it from the waiting list. */
  1860.     p_CB->isr_st = 0;
  1861.     p_TCB = rt_get_first ((P_XCB)p_CB);
  1862.     p_TCB->ret_val = OS_R_OK;
  1863.     goto rdy;
  1864.   }
  1865.   /* A task is waiting for message, pass the message to task directly. */
  1866.   if (p_CB->p_lnk != NULL && p_CB->count == 0) {
  1867.     p_TCB = rt_get_first ((P_XCB)p_CB);
  1868.     *p_TCB->msg = p_msg;
  1869.     p_TCB->ret_val = OS_R_MBX;
  1870. rdy:p_TCB->state = READY;
  1871.     rt_rmv_dly (p_TCB);
  1872.     rt_put_prio (&os_rdy, p_TCB);
  1873.   }
  1874.   else {
  1875.     /* No task is waiting for message, store the message to the mailbox queue.*/
  1876.     if (p_CB->count < p_CB->size) {
  1877.       p_CB->msg[p_CB->first] = p_msg;
  1878.       rt_inc (&p_CB->count);
  1879.       if (++p_CB->first == p_CB->size) {
  1880.         p_CB->first = 0;
  1881.       }
  1882.     }
  1883.     else {
  1884.       os_error (OS_ERR_MBX_OVF);
  1885.     }
  1886.   }
  1887. }
  1888. //=========================================================================================

  1890. //------ rt_timer.c ---------------------------------------------------------------------
  1891. #if OS_TMR_EN > 0
  1892. void rt_tmr_tick (void) {
  1893.   /* Decrement delta count of timer list head. Timers having the value of */
  1894.   /* zero are removed from the list and the callback function is called. */
  1895.   P_TMR p;

  1897.   if (os_tmr.next == NULL) {
  1898.     return;
  1899.   }
  1900.   os_tmr.tcnt--;
  1901.   while (os_tmr.tcnt == 0 && (p = os_tmr.next) != NULL) {
  1902.     /* Call a user provided function to handle an elapsed timer */
  1903.     os_tmr_call (p->info);
  1904.     os_tmr.tcnt = p->tcnt;
  1905.     os_tmr.next = p->next;
  1906.     rt_free_box ((U32 *)m_tmr, p);
  1907.   }
  1908. }

  1910. OS_ID rt_tmr_create (U16 tcnt, U16 info) {
  1911.   /* Create an user timer and put it into the chained timer list using */
  1912.   /* a timeout count value of "tcnt". User parameter "info" is used as a */
  1913.   /* parameter for the user provided callback function "os_tmr_call ()". */
  1914.   P_TMR p_tmr, p;
  1915.   U32 delta,itcnt = tcnt;

  1917.   if (tcnt == 0 || m_tmr == NULL) {
  1918.     return (NULL);
  1919.   }
  1920.   p_tmr = rt_alloc_box ((U32 *)m_tmr);
  1921.   if (!p_tmr) {
  1922.     return (NULL);
  1923.   }
  1924.   p_tmr->info = info;
  1925.   p = (P_TMR)&os_tmr;
  1926.   delta = p->tcnt;
  1927.   while (delta < itcnt && p->next != NULL) {
  1928.     p = p->next;
  1929.     delta += p->tcnt;
  1930.   }
  1931.   /* Right place found, insert timer into the list */
  1932.   p_tmr->next = p->next;
  1933.   p_tmr->tcnt = (U16)(delta - itcnt);
  1934.   p->next = p_tmr;
  1935.   p->tcnt -= p_tmr->tcnt;
  1936.   return (p_tmr);
  1937. }

  1939. OS_ID rt_tmr_kill (OS_ID timer) {
  1940.   /* Remove user timer from the chained timer list. */
  1941.   P_TMR p, p_tmr;

  1943.   p_tmr = (P_TMR)timer;
  1944.   p = (P_TMR)&os_tmr;
  1945.   /* Search timer list for requested timer */
  1946.   while (p->next != p_tmr) {
  1947.     if (p->next == NULL) {
  1948.       /* Failed, "timer














阅读(8653) | 评论(0) | 转发(0) |
0

上一篇：CJSON 调试记录

下一篇：OPENWRT的中继或者桥接模式, Client+AP实现

给主人留下些什么吧！~~
关于我们 | 关于IT168 | 联系方式 | 广告合作 | 法律声明 | 免费注册

Copyright 2001-2010 ChinaUnix.net All Rights Reserved 北京皓辰网域网络信息技术有限公司. 版权所有

感谢所有关心和支持过ChinaUnix的朋友们

16024965号-6