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

全部博文(129)

文章存档

2015年(3)

2014年(20)

2013年(65)

2012年(41)

分类: C/C++

2013-03-08 09:07:11

修改如下:

点击(此处)折叠或打开

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

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

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

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

  11. Code (inc. data) RO Data RW Data ZI Data Debug Object Name
  12. 4808 256 0 37 96 0 rtx_cm3.o
  13. 4816 254 0 37 96 0 rtx_cm3.o

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

  25. #define OS_MBOX_EN 1 /* Enable (1) or Disable (0) code generation for MAILBOXES */
  26. #define OS_SEM_EN 1 /* Enable (1) or Disable (0) code generation for SEMAPHORES */
  27. #define OS_MUTEX_EN 1 /* Enable (1) or Disable (0) code generation for MUTEX */
  28. #define OS_FLAG_EN 0 /* Enable (1) or Disable (0) code generation for EVENT FLAGS */
  29. #define OS_TMR_EN 0 /* Enable (1) or Disable (0) code generation for TIMERS */

  30. //---------------------------------------------------------------------------

  31. /* Types */
  32. typedef char S8;
  33. typedef unsigned char U8;
  34. typedef short S16;
  35. typedef unsigned short U16;
  36. typedef int S32;
  37. typedef unsigned int U32;
  38. typedef long long S64;
  39. typedef unsigned long long U64;
  40. typedef unsigned char BIT;
  41. typedef unsigned int BOOL;

  42. /* Definitions */
  43. #define __TRUE 1
  44. #define __FALSE 0
  45. #define NULL ((void *) 0)

  46. /* Values for 'state' */
  47. #define INACTIVE 0
  48. #define READY 1
  49. #define RUNNING 2
  50. #define WAIT_DLY 3
  51. #define WAIT_ITV 4
  52. #define WAIT_OR 5
  53. #define WAIT_AND 6
  54. #define WAIT_SEM 7
  55. #define WAIT_MBX 8
  56. #define WAIT_MUT 9

  57. /* Return codes */
  58. #define OS_R_TMO 0x01
  59. #define OS_R_EVT 0x02
  60. #define OS_R_SEM 0x03
  61. #define OS_R_MBX 0x04
  62. #define OS_R_MUT 0x05
  63. #define OS_R_OK 0x00
  64. #define OS_R_NOK 0xff

  65. /* Error Codes */
  66. #define OS_ERR_STK_OVF 1
  67. #define OS_ERR_FIFO_OVF 2
  68. #define OS_ERR_MBX_OVF 3

  69. typedef void (*FUNCP)(void);
  70. typedef U32 OS_TID;
  71. typedef void *OS_ID;
  72. typedef U32 OS_RESULT;

  73. typedef struct OS_TCB {
  74.   /* General part: identical for all implementations. */
  75.   U8 cb_type; /* Control Block Type */
  76.   U8 state; /* Task state */
  77.   U8 prio; /* Execution priority */
  78.   U8 task_id; /* Task ID value for optimized TCB access */
  79.   struct OS_TCB *p_lnk; /* Link pointer for ready/sem. wait list */
  80.   struct OS_TCB *p_rlnk; /* Link pointer for sem./mbx lst backwards */
  81.   struct OS_TCB *p_dlnk; /* Link pointer for delay list */
  82.   struct OS_TCB *p_blnk; /* Link pointer for delay list backwards */
  83.   U16 delta_time; /* Time until time out */
  84.   U16 interval_time; /* Time interval for periodic waits */
  85.   U16 events; /* Event flags */
  86.   U16 waits; /* Wait flags */
  87.   void **msg; /* Direct message passing when task waits */
  88.   U8 ret_val; /* Return value upon completion of a wait */

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

  94.   /* Task entry point used for uVision debugger */
  95.   FUNCP ptask; /* Task entry address */
  96. } *P_TCB;

  97. #define TCB_RETVAL 32 /* 'ret_val' offset */
  98. #define TCB_RETUPD 33 /* 'ret_upd' offset */
  99. #define TCB_TSTACK 36 /* 'tsk_stack' offset */

  100. typedef struct OS_PSFE { /* Post Service Fifo Entry */
  101.   void *id; /* Object Identification */
  102.   U32 arg; /* Object Argument */
  103. } *P_PSFE;

  104. typedef struct OS_PSQ { /* Post Service Queue */
  105.   U8 first; /* FIFO Head Index */
  106.   U8 last; /* FIFO Tail Index */
  107.   U8 count; /* Number of stored items in FIFO */
  108.   U8 size; /* FIFO Size */
  109.   struct OS_PSFE q[1]; /* FIFO Content */
  110. } *P_PSQ;

  111. typedef struct OS_TSK {
  112.   P_TCB run; /* Current running task */
  113.   P_TCB new; /* Scheduled task to run */
  114. } *P_TSK;

  115. typedef struct OS_ROBIN { /* Round Robin Control */
  116.   P_TCB task; /* Round Robin task */
  117.   U16 time; /* Round Robin switch time */
  118.   U16 tout; /* Round Robin timeout */
  119. } *P_ROBIN;

  120. typedef struct OS_XCB {
  121.   U8 cb_type; /* Control Block Type */
  122.   struct OS_TCB *p_lnk; /* Link pointer for ready/sem. wait list */
  123.   struct OS_TCB *p_rlnk; /* Link pointer for sem./mbx lst backwards */
  124.   struct OS_TCB *p_dlnk; /* Link pointer for delay list */
  125.   struct OS_TCB *p_blnk; /* Link pointer for delay list backwards */
  126.   U16 delta_time; /* Time until time out */
  127. } *P_XCB;

  128. typedef struct OS_MCB {
  129.   U8 cb_type; /* Control Block Type */
  130.   U8 isr_st; /* State flag variable for isr functions */
  131.   struct OS_TCB *p_lnk; /* Chain of tasks waiting for message */
  132.   U16 first; /* Index of the message list begin */
  133.   U16 last; /* Index of the message list end */
  134.   U16 count; /* Actual number of stored messages */
  135.   U16 size; /* Maximum number of stored messages */
  136.   void *msg[1]; /* FIFO for Message pointers 1st element */
  137. } *P_MCB;

  138. typedef struct OS_SCB {
  139.   U8 cb_type; /* Control Block Type */
  140.   U16 tokens; /* Semaphore tokens */
  141.   struct OS_TCB *p_lnk; /* Chain of tasks waiting for tokens */
  142. } *P_SCB;

  143. typedef struct OS_MUCB {
  144.   U8 cb_type; /* Control Block Type */
  145.   U8 prio; /* Owner task default priority */
  146.   U16 level; /* Call nesting level */
  147.   struct OS_TCB *p_lnk; /* Chain of tasks waiting for mutex */
  148.   struct OS_TCB *owner; /* Mutex owner task */
  149. } *P_MUCB;

  150. typedef struct OS_XTMR {
  151.   struct OS_TMR *next;
  152.   U16 tcnt;
  153. } *P_XTMR;

  154. typedef struct OS_TMR {
  155.   struct OS_TMR *next; /* Link pointer to Next timer */
  156.   U16 tcnt; /* Timer delay count */
  157.   U16 info; /* User defined call info */
  158. } *P_TMR;

  159. typedef struct OS_BM {
  160.   void *free; /* Pointer to first free memory block */
  161.   void *end; /* Pointer to memory block end */
  162.   U32 blk_size; /* Memory block size */
  163. } *P_BM;

  164. //----------------------------------------------------------------------------------------

  165. #define INITIAL_xPSR 0x01000000
  166. #define DEMCR_TRCENA 0x01000000
  167. #define ITM_ITMENA 0x00000001
  168. #define MAGIC_WORD 0xE25A2EA5

  169. /* NVIC SysTick registers */
  170. #define NVIC_ST_CTRL (*((volatile U32 *)0xE000E010))
  171. #define NVIC_ST_RELOAD (*((volatile U32 *)0xE000E014))
  172. #define NVIC_ST_CURRENT (*((volatile U32 *)0xE000E018))
  173. #define NVIC_INT_CTRL (*((volatile U32 *)0xE000ED04))
  174. #define NVIC_SYS_PRI2 (*((volatile U32 *)0xE000ED1C))
  175. #define NVIC_SYS_PRI3 (*((volatile U32 *)0xE000ED20))

  176. #define OS_PEND_IRQ() NVIC_INT_CTRL = (1<<28);
  177. #define OS_TINIT() NVIC_ST_RELOAD = os_trv;
  178.                         NVIC_ST_CTRL = 0x0007;
  179.                         NVIC_SYS_PRI3 |= 0xFFFF0000;
  180.                         NVIC_SYS_PRI2 |= (NVIC_SYS_PRI3<<1) & 0xFE000000;
  181. #define OS_LOCK() NVIC_ST_CTRL = 0x0005;
  182. #define OS_UNLOCK() NVIC_ST_CTRL = 0x0007;

  183. /* Core Debug registers */
  184. #define DEMCR (*((volatile U32 *)0xE000EDFC))

  185. /* ITM registers */
  186. #define ITM_CONTROL (*((volatile U32 *)0xE0000E80))
  187. #define ITM_ENABLE (*((volatile U32 *)0xE0000E00))
  188. #define ITM_PORT30_U32 (*((volatile U32 *)0xE0000078))
  189. #define ITM_PORT31_U32 (*((volatile U32 *)0xE000007C))
  190. #define ITM_PORT31_U16 (*((volatile U16 *)0xE000007C))
  191. #define ITM_PORT31_U8 (*((volatile U8 *)0xE000007C))

  192. /* Functions */
  193. #if defined(__TARGET_ARCH_7_M) || defined(__TARGET_ARCH_7E_M)
  194.  #define rt_inc(p) while(__strex((__ldrex(p)+1),p))
  195.  #define rt_dec(p) while(__strex((__ldrex(p)-1),p))
  196. #else
  197.  #define rt_inc(p) __disable_irq();(*p)++;__enable_irq();
  198.  #define rt_dec(p) __disable_irq();(*p)--;__enable_irq();
  199. #endif

  200. __inline U32 rt_inc_qi (U32 size, U8 *count, U8 *first) {
  201.   U32 cnt,c2;
  202. #if defined(__TARGET_ARCH_7_M) || defined(__TARGET_ARCH_7E_M)
  203.   do {
  204.     if ((cnt = __ldrex(count)) == size) {
  205.       __clrex();
  206.       return (cnt); }
  207.   } while (__strex(cnt+1, count));
  208.   do {
  209.     c2 = (cnt = __ldrex(first)) + 1;
  210.     if (c2 == size) c2 = 0;
  211.   } while (__strex(c2, first));
  212. #else
  213.   __disable_irq();
  214.   if ((cnt = *count) < size) {
  215.     *count = cnt+1;
  216.     c2 = (cnt = *first) + 1;
  217.     if (c2 == size) c2 = 0;
  218.     *first = c2;
  219.   }
  220.   __enable_irq ();
  221. #endif
  222.   return (cnt);
  223. }

  224. #define rt_tmr_init() OS_TINIT();
  225. //void rt_init_stack (P_TCB p_TCB, FUNCP task_body);
  226. //void rt_set_PSP (U32 stack);
  227. //void os_set_env (void);
  228. void *_alloc_box (void *box_mem);
  229. int _free_box (void *box_mem, void *box);

  230. void dbg_init (void);
  231. void dbg_task_notify (P_TCB p_tcb, BOOL create);
  232. void dbg_task_switch (U32 task_id);

  233. #ifdef DBG_MSG
  234. #define DBG_INIT() dbg_init()
  235. #define DBG_TASK_NOTIFY(p_tcb,create) if (dbg_msg) dbg_task_notify(p_tcb,create)
  236. #define DBG_TASK_SWITCH(task_id) if (dbg_msg && (os_tsk.new!=os_tsk.run))
  237.                                                    dbg_task_switch(task_id)
  238. #else
  239. #define DBG_INIT()
  240. #define DBG_TASK_NOTIFY(p_tcb,create)
  241. #define DBG_TASK_SWITCH(task_id)
  242. #endif

  243. //OS_RESULT rt_evt_wait (U16 wait_flags, U16 timeout, BOOL and_wait);
  244. //void rt_evt_set (U16 event_flags, OS_TID task_id);
  245. //void rt_evt_clr (U16 clear_flags, OS_TID task_id);
  246. //void isr_evt_set (U16 event_flags, OS_TID task_id);
  247. //U16 rt_evt_get (void);
  248. void rt_evt_psh (P_TCB p_CB, U16 set_flags);

  249. /* Functions */
  250. void rt_switch_req (P_TCB p_new);
  251. void rt_dispatch (P_TCB next_TCB);
  252. void rt_block (U16 timeout, U8 block_state);
  253. //void rt_tsk_pass (void);
  254. //OS_TID rt_tsk_self (void);
  255. //OS_RESULT rt_tsk_prio (OS_TID task_id, U8 new_prio);
  256. //OS_TID rt_tsk_create (FUNCP task, U32 prio_stksz, void *stk, void *argv);
  257. //OS_RESULT rt_tsk_delete (OS_TID task_id);
  258. //void rt_sys_init (FUNCP first_task, U32 prio_stksz, void *stk);

  259. //void rt_dly_wait (U16 delay_time);
  260. //void rt_itv_set (U16 interval_time);
  261. //void rt_itv_wait (void);

  262. void rt_tmr_tick (void);
  263. //OS_ID rt_tmr_create (U16 tcnt, U16 info);
  264. //OS_ID rt_tmr_kill (OS_ID timer);


  265. #define rt_init_box _init_box
  266. #define rt_calloc_box _calloc_box
  267. int _init_box (void *box_mem, U32 box_size, U32 blk_size);
  268. void *rt_alloc_box (void *box_mem);
  269. void * _calloc_box (void *box_mem);
  270. int rt_free_box (void *box_mem, void *box);

  271. //void rt_mbx_init (OS_ID mailbox, U16 mbx_size);
  272. //OS_RESULT rt_mbx_send (OS_ID mailbox, void *p_msg, U16 timeout);
  273. //OS_RESULT rt_mbx_wait (OS_ID mailbox, void **message, U16 timeout);
  274. //OS_RESULT rt_mbx_check (OS_ID mailbox);
  275. //void isr_mbx_send (OS_ID mailbox, void *p_msg);
  276. //OS_RESULT isr_mbx_receive (OS_ID mailbox, void **message);
  277. void rt_mbx_psh (P_MCB p_CB, void *p_msg);

  278. //void rt_sem_init (OS_ID semaphore, U16 token_count);
  279. //OS_RESULT rt_sem_send (OS_ID semaphore);
  280. //OS_RESULT rt_sem_wait (OS_ID semaphore, U16 timeout);
  281. //void isr_sem_send (OS_ID semaphore);
  282. void rt_sem_psh (P_SCB p_CB);

  283. //void rt_mut_init (OS_ID mutex);
  284. //OS_RESULT rt_mut_release (OS_ID mutex);
  285. //OS_RESULT rt_mut_wait (OS_ID mutex, U16 timeout);

  286. #define os_psq ((P_PSQ)&os_fifo)
  287. //void rt_tsk_lock (void);
  288. //void rt_tsk_unlock (void);
  289. void rt_psh_req (void);
  290. void rt_pop_req (void);
  291. void rt_systick (void);
  292. void rt_stk_check (void);

  293. //void rt_init_robin (void);
  294. //void rt_chk_robin (void);

  295. #define TCB 0
  296. #define MCB 1
  297. #define SCB 2
  298. #define MUCB 3
  299. #define HCB 4
  300. /* Functions */
  301. void rt_put_prio (P_XCB p_CB, P_TCB p_task);
  302. P_TCB rt_get_first (P_XCB p_CB);
  303. void rt_put_rdy_first (P_TCB p_task);
  304. P_TCB rt_get_same_rdy_prio (void);
  305. void rt_resort_prio (P_TCB p_task);
  306. void rt_put_dly (P_TCB p_task, U16 delay);
  307. void rt_dec_dly (void);
  308. void rt_rmv_list (P_TCB p_task);
  309. void rt_rmv_dly (P_TCB p_task);
  310. void rt_psq_enq (OS_ID entry, U32 arg);

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

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

  319. /* Variables */
  320. extern U32 mp_tcb[];            //_declare_box (mp_tcb, OS_TCB_SIZE, OS_TASKCNT);
  321. extern U64 mp_stk[];
  322. extern U32 os_fifo[];
  323. extern void *os_active_TCB[];

  324. /* Constants */
  325. extern U16 const os_maxtaskrun;    //U16 const os_maxtaskrun = OS_TASKCNT;
  326. extern U32 const os_trv;
  327. extern U8 const os_flags;        //U8 const os_flags = OS_RUNPRIV;
  328. extern U32 const os_stackinfo;    //U32 const os_stackinfo = (OS_STKCHECK<<24)| (OS_PRIVCNT<<16) | (OS_STKSIZE*4);
  329. extern U32 const os_rrobin;
  330. //extern U32 const os_clockrate;    //U32 const os_clockrate = OS_TICK;
  331. extern U32 const os_timernum;    //U32 const os_timernum = (OS_TIMER << 16) | OS_TIMERCNT;
  332. extern U16 const mp_tcb_size;    //U16 const mp_tcb_size = sizeof(mp_tcb);
  333. extern U32 const mp_stk_size;
  334. extern U32 const *m_tmr;        //U32 const *m_tmr = &mp_tmr[0];
  335. extern U16 const mp_tmr_size;    //U16 const mp_tmr_size = sizeof(mp_tmr);
  336. extern U8 const os_fifo_size;

  337. /* Functions */
  338. extern void os_idle_demon (void);
  339. extern void os_tmr_call (U16 info);
  340. extern void os_error (U32 err_code);

  341. /*----------------------------------------------------------------------------
  342.  * Global Variables
  343.  *---------------------------------------------------------------------------*/
  344. U16 os_time;                        // Free running system tick counter
  345. struct OS_XTMR os_tmr;                // User Timer list pointer
  346. struct OS_TSK os_tsk;                // Running and next task info.
  347. struct OS_TCB os_idle_TCB;        // Task Control Blocks of idle demon
  348. static BIT os_lock;
  349. static BIT os_psh_flag;
  350. struct OS_XCB os_rdy;                // List head of chained ready tasks
  351. struct OS_XCB os_dly;                // List head of chained delay tasks
  352. struct OS_ROBIN os_robin;

  353. //======================== HAL_CM3.c Start ==========================================================
  354. BIT dbg_msg;

  355. __asm void rt_set_PSP (U32 stack) {
  356.         MSR PSP,R0
  357.         BX LR
  358. }

  359. __asm void os_set_env (void) {
  360.    /* Switch to Unpriviliged/Priviliged Thread mode, use PSP. */
  361.         MOV R0,SP
  362.         MSR PSP,R0                    ; PSP = MSP
  363.         LDR R0,=__cpp(&os_flags)
  364.         LDRB R0,[R0]
  365.         LSLS R0,#31
  366.         MOVNE R0,#0x02 ; Priviliged Thread mode, use PSP
  367.         MOVEQ R0,#0x03 ; Unpriviliged Thread mode, use PSP
  368.         MSR CONTROL,R0
  369.         BX LR

  370.         ALIGN
  371. }

  372. __asm void *_alloc_box (void *box_mem) {
  373.    /* Function wrapper for Unpriviliged/Priviliged mode. */
  374.         LDR R12,=__cpp(rt_alloc_box)
  375.         MRS R3,IPSR
  376.         LSLS R3,#24
  377.         BXNE R12
  378.         MRS R3,CONTROL
  379.         LSLS R3,#31
  380.         BXEQ R12
  381.         SVC 0
  382.         BX LR

  383.         ALIGN
  384. }

  385. __asm int _free_box (void *box_mem, void *box) {
  386.    /* Function wrapper for Unpriviliged/Priviliged mode. */
  387.         LDR R12,=__cpp(rt_free_box)
  388.         MRS R3,IPSR
  389.         LSLS R3,#24
  390.         BXNE R12
  391.         MRS R3,CONTROL
  392.         LSLS R3,#31
  393.         BXEQ R12
  394.         SVC 0
  395.         BX LR

  396.         ALIGN
  397. }

  398. __asm void SVC_Handler (void) {
  399.         PRESERVE8

  400. #ifdef SVCUSER
  401.         IMPORT SVC_Count
  402.         IMPORT SVC_Table
  403. #endif
  404.         IMPORT rt_stk_check

  405.         MRS R0,PSP ; Read PSP
  406.         LDR R1,[R0,#24] ; Read Saved PC from Stack
  407.         LDRH R1,[R1,#-2] ; Load Halfword
  408. #ifdef SVCUSER
  409.         BICS R1,R1,#0xFF00 ; Extract SVC Number
  410.         BNE SVC_User
  411. #endif

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

  414.         MRS R12,PSP ; Read PSP
  415.         LDR R3,=__cpp(&os_tsk)
  416.         LDM R3,{R1,R2} ; os_tsk.run, os_tsk.new
  417.         CMP R1,R2
  418.         BEQ SVC_Exit ; no task switch

  419.         PUSH {R2,R3}
  420.         MOV R3,#1
  421.         CMP R1,#0 ; Runtask deleted?
  422.         STRBNE R3,[R1,#TCB_RETUPD] ; os_tsk.run->ret_upd = 1
  423.         STMDBNE R12!,{R4-R11} ; Save Old context
  424.         STRNE R12,[R1,#TCB_TSTACK] ; Update os_tsk.run->tsk_stack
  425.         BLNE rt_stk_check ; Check for Stack overflow

  426.         POP {R2,R3}
  427.         STR R2,[R3] ; os_tsk.run = os_tsk.new

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

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

  434. SVC_Exit
  435.         STR R0,[R12] ; Function return value

  436. SVC_Return
  437.         MVN LR,#:NOT:0xFFFFFFFD ; set EXC_RETURN value
  438.         BX LR

  439.         /*------------------- User SVC ------------------------------*/
  440. #ifdef SVCUSER
  441. SVC_User
  442.         PUSH {R4,LR} ; Save Registers
  443.         LDR R2,=SVC_Count
  444.         LDR R2,[R2]
  445.         CMP R1,R2
  446.         BHI SVC_Done ; Overflow

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

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

  451.         MRS R12,PSP
  452.         STM R12,{R0-R3} ; Function return values
  453. SVC_Done
  454.         POP {R4,PC} ; RETI

  455.         ALIGN
  456. #endif
  457. }

  458. __asm void Sys_Handler (void) {
  459.         PRESERVE8

  460.         EXPORT SysTick_Handler
  461.         EXPORT PendSV_Handler

  462. PendSV_Handler
  463.         BL __cpp(rt_pop_req)

  464. Sys_Switch
  465.         LDR R3,=__cpp(&os_tsk)
  466.         LDM R3,{R1,R2} ; os_tsk.run, os_tsk.new
  467.         CMP R1,R2
  468.         BEQ SysExit

  469.         PUSH {R2,R3}
  470.         MOV R3,#0
  471.         STRB R3,[R1,#TCB_RETUPD] ; os_tsk.run->ret_upd = 0
  472.         MRS R12,PSP ; Read PSP
  473.         STMDB R12!,{R4-R11} ; Save Old context
  474.         STR R12,[R1,#TCB_TSTACK] ; Update os_tsk.run->tsk_stack
  475.         BL rt_stk_check ; Check for Stack overflow

  476.         POP {R2,R3}
  477.         STR R2,[R3] ; os_tsk.run = os_tsk.new

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

  481.         LDRB R3,[R2,#TCB_RETUPD] ; Update ret_val?
  482.         CBZ R3,SysExit
  483.         LDRB R3,[R2,#TCB_RETVAL] ; Write os_tsk.new->ret_val
  484.         STR R3,[R12]
  485. SysExit
  486.         MVN LR,#:NOT:0xFFFFFFFD ; set EXC_RETURN value
  487.         BX LR ; Return to Thread Mode

  488. SysTick_Handler
  489.         BL __cpp(rt_systick)
  490.         B Sys_Switch

  491.         ALIGN
  492. }

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

  496.   /* Prepare a complete interrupt frame for first task start */
  497.   size = p_TCB->priv_stack >> 2;
  498.   if (size == 0) {
  499.     size = (U16)os_stackinfo >> 2;
  500.   }

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

  503.   /* Auto correct to 8-byte ARM stack alignment. */
  504.   if ((U32)stk & 0x04) {
  505.     stk--;
  506.   }

  507.   stk -= 16;

  508.   /* Default xPSR and initial PC */
  509.   stk[15] = INITIAL_xPSR;
  510.   stk[14] = (U32)task_body;

  511.   /* Clear R1-R12,LR registers. */
  512.   for (i = 0; i < 14; i++) {
  513.     stk[i] = 0;
  514.   }

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

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

  519.   /* Task entry point. */
  520.   p_TCB->ptask = task_body;

  521.   /* Set a magic word for checking of stack overflow. */
  522.   p_TCB->stack[0] = MAGIC_WORD;
  523. }

  524. void dbg_init (void) {
  525.   if ((DEMCR & DEMCR_TRCENA) &&
  526.       (ITM_CONTROL & ITM_ITMENA) &&
  527.       (ITM_ENABLE & (1UL << 31))) {
  528.     dbg_msg = __TRUE;
  529.   }
  530. }

  531. void dbg_task_notify (P_TCB p_tcb, BOOL create) {
  532.   while (ITM_PORT31_U32 == 0);
  533.   ITM_PORT31_U32 = (U32)p_tcb->ptask;
  534.   while (ITM_PORT31_U32 == 0);
  535.   ITM_PORT31_U16 = (create << 8) | p_tcb->task_id;
  536. }

  537. void dbg_task_switch (U32 task_id) {
  538.   while (ITM_PORT31_U32 == 0);
  539.   ITM_PORT31_U8 = task_id;
  540. }
  541. //======================== HAL_CM3.c End ==========================================================

  542. //==================== rt_Robin.c ===============================================================
  543. __weak void rt_init_robin (void) {
  544.   /* Initialize Round Robin variables. */
  545.   os_robin.task = NULL;
  546.   os_robin.tout = (U16)os_rrobin;
  547. }

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

  551.   if (os_robin.task != os_rdy.p_lnk) {
  552.     /* New task was suspended, reset Round Robin timeout. */
  553.     os_robin.task = os_rdy.p_lnk;
  554.     os_robin.time = os_time + os_robin.tout - 1;
  555.   }
  556.   if (os_robin.time == os_time) {
  557.     /* Round Robin timeout has expired, swap Robin tasks. */
  558.     os_robin.task = NULL;
  559.     p_new = rt_get_first (&os_rdy);
  560.     rt_put_prio ((P_XCB)&os_rdy, p_new);
  561.   }
  562. }
  563. //=========================================================================================

  564. //--- rt_system.c
  565. #if 0
  566. __asm void $$RTX$$version (void) {
  567.    /* Export a version number symbol for a version control. */
  568.                 EXPORT __RTX_ARM_VER
  569. __RTX_ARM_VER EQU 0x412
  570. }
  571. #endif

  572. void rt_tsk_lock (void) {
  573.   /* Lock out tasks: prevents task switching by locking out scheduler */
  574.   OS_LOCK();
  575.   os_lock = __TRUE;
  576. }

  577. void rt_tsk_unlock (void) {
  578.   /* Enable System Tick Timer Interrupts. */
  579.   OS_UNLOCK();
  580.   os_lock = __FALSE;
  581.   if (os_psh_flag) {
  582.     OS_PEND_IRQ ();
  583.   }
  584. }

  585. void rt_psh_req (void) {
  586.   /* Initiate a post service handling request if required. */
  587.   if (os_lock == __FALSE) {
  588.     OS_PEND_IRQ ();
  589.   }
  590.   else {
  591.     os_psh_flag = __TRUE;
  592.   }
  593. }

  594. void rt_pop_req (void) {
  595.   /* Process an ISR post service requests. */
  596.   struct OS_XCB *p_CB;
  597.   P_TCB next;
  598.   U32 idx;

  599.   os_tsk.run->state = READY;
  600.   rt_put_rdy_first (os_tsk.run);

  601.   os_psh_flag = __FALSE;
  602.   idx = os_psq->last;
  603.   while (os_psq->count) {
  604.     p_CB = os_psq->q[idx].id;
  605.     if (p_CB->cb_type == TCB) {
  606.       /* Is of TCB type */
  607.       rt_evt_psh ((P_TCB)p_CB, (U16)os_psq->q[idx].arg);
  608.     }
  609.     else if (p_CB->cb_type == MCB) {
  610.       /* Is of MCB type */
  611.       rt_mbx_psh ((P_MCB)p_CB, (void *)os_psq->q[idx].arg);
  612.     }
  613.     else {
  614.       /* Must be of SCB type */
  615.       rt_sem_psh ((P_SCB)p_CB);
  616.     }
  617.     if (++idx == os_psq->size) idx = 0;
  618.     rt_dec (&os_psq->count);
  619.   }
  620.   os_psq->last = idx;

  621.   next = rt_get_first (&os_rdy);
  622.   rt_switch_req (next);
  623. }

  624. void rt_systick (void) {
  625.   /* Check for system clock update, suspend running task. */
  626.   P_TCB next;

  627.   os_tsk.run->state = READY;
  628.   rt_put_rdy_first (os_tsk.run);

  629.   /* Check Round Robin timeout. */
  630.   rt_chk_robin ();

  631.   /* Update delays. */
  632.   os_time++;
  633.   rt_dec_dly ();

  634.   /* Check the user timers. */
  635. #if OS_TMR_EN > 0
  636.       rt_tmr_tick ();
  637. #endif

  638.   /* Switch back to highest ready task */
  639.   next = rt_get_first (&os_rdy);
  640.   rt_switch_req (next);
  641. }

  642. __weak void rt_stk_check (void) {
  643.   /* Check for stack overflow. */
  644.   if ((os_tsk.run->tsk_stack < (U32)os_tsk.run->stack) ||
  645.       (os_tsk.run->stack[0] != MAGIC_WORD)) {
  646.     os_error (OS_ERR_STK_OVF);
  647.   }
  648. }
  649. //=========================================================================================

  650. //--- rt_task.c
  651. static OS_TID rt_get_TID (void) {
  652.   U32 tid;

  653.   for (tid = 1; tid <= os_maxtaskrun; tid++) {
  654.     if (os_active_TCB[tid-1] == NULL) {
  655.       return ((OS_TID)tid);
  656.     }
  657.   }
  658.   return (0);
  659. }

  660. static void rt_init_context (P_TCB p_TCB, U8 priority, FUNCP task_body) {
  661.   /* Initialize general part of the Task Control Block. */
  662.   p_TCB->cb_type = TCB;
  663.   p_TCB->state = READY;
  664.   p_TCB->prio = priority;
  665.   p_TCB->p_lnk = NULL;
  666.   p_TCB->p_rlnk = NULL;
  667.   p_TCB->p_dlnk = NULL;
  668.   p_TCB->p_blnk = NULL;
  669.   p_TCB->delta_time = 0;
  670.   p_TCB->interval_time = 0;
  671.   p_TCB->events = 0;
  672.   p_TCB->waits = 0;
  673.   p_TCB->ret_val = OS_R_OK;
  674.   p_TCB->ret_upd = 0;

  675.   if (p_TCB->priv_stack == 0) {
  676.     /* Allocate the memory space for the stack. */
  677.     p_TCB->stack = rt_alloc_box (mp_stk);
  678.   }
  679.   rt_init_stack (p_TCB, task_body);
  680. }

  681. void rt_switch_req (P_TCB p_new) {
  682.   /* Switch to next task (identified by "p_new"). */
  683.   os_tsk.new = p_new;
  684.   p_new->state = RUNNING;
  685.   DBG_TASK_SWITCH(p_new->task_id);
  686. }

  687. void rt_dispatch (P_TCB next_TCB) {
  688.   /* Dispatch next task if any identified or dispatch highest ready task */
  689.   /* "next_TCB" identifies a task to run or has value NULL (=no next task) */
  690.   if (next_TCB == NULL) {
  691.     /* Running task was blocked: continue with highest ready task */
  692.     next_TCB = rt_get_first (&os_rdy);
  693.     rt_switch_req (next_TCB);
  694.   }
  695.   else {
  696.     /* Check which task continues */
  697.     if (next_TCB->prio > os_tsk.run->prio) {
  698.       /* preempt running task */
  699.       rt_put_rdy_first (os_tsk.run);
  700.       os_tsk.run->state = READY;
  701.       rt_switch_req (next_TCB);
  702.     }
  703.     else {
  704.       /* put next task into ready list, no task switch takes place */
  705.       next_TCB->state = READY;
  706.       rt_put_prio (&os_rdy, next_TCB);
  707.     }
  708.   }
  709. }

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

  715.   if (timeout) {
  716.     if (timeout < 0xffff) {
  717.       rt_put_dly (os_tsk.run, timeout);
  718.     }
  719.     os_tsk.run->state = block_state;
  720.     next_TCB = rt_get_first (&os_rdy);
  721.     rt_switch_req (next_TCB);
  722.   }
  723. }

  724. void rt_tsk_pass (void) {
  725.   /* Allow tasks of same priority level to run cooperatively.*/
  726.   P_TCB p_new;

  727.   p_new = rt_get_same_rdy_prio();
  728.   if (p_new != NULL) {
  729.     rt_put_prio ((P_XCB)&os_rdy, os_tsk.run);
  730.     os_tsk.run->state = READY;
  731.     rt_switch_req (p_new);
  732.   }
  733. }

  734. OS_TID rt_tsk_self (void) {
  735.   /* Return own task identifier value. */
  736.   if (os_tsk.run == NULL) {
  737.     return (0);
  738.   }
  739.   return (os_tsk.run->task_id);
  740. }

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

  744.   if (task_id == 0) {
  745.     /* Change execution priority of calling task. */
  746.     os_tsk.run->prio = new_prio;
  747. run:if (rt_rdy_prio() > new_prio) {
  748.       rt_put_prio (&os_rdy, os_tsk.run);
  749.       os_tsk.run->state = READY;
  750.       os_tsk.run->ret_val = OS_R_OK;
  751.       rt_dispatch (NULL);
  752.     }
  753.     return (OS_R_OK);
  754.   }

  755.   /* Find the task in the "os_active_TCB" array. */
  756.   if (task_id > os_maxtaskrun || os_active_TCB[task_id-1] == NULL) {
  757.     /* Task with "task_id" not found or not started. */
  758.     return (OS_R_NOK);
  759.   }
  760.   p_task = os_active_TCB[task_id-1];
  761.   p_task->prio = new_prio;
  762.   if (p_task == os_tsk.run) {
  763.     goto run;
  764.   }
  765.   rt_resort_prio (p_task);
  766.   if (p_task->state == READY) {
  767.     /* Task enqueued in a ready list. */
  768.     p_task = rt_get_first (&os_rdy);
  769.     os_tsk.run->ret_val = OS_R_OK;
  770.     rt_dispatch (p_task);
  771.   }
  772.   return (OS_R_OK);
  773. }

  774. OS_TID rt_tsk_create (FUNCP task, U32 prio_stksz, void *stk, void *argv) {
  775.   /* Start a new task declared with "task". */
  776.   P_TCB task_context;
  777.   U32 i;

  778.   /* Priority 0 is reserved for idle */
  779.   if ((prio_stksz & 0xFF) == 0) {
  780.     prio_stksz += 1;
  781.   }
  782.   task_context = rt_alloc_box (mp_tcb);
  783.   if (task_context == NULL) {
  784.     return (0);
  785.   }
  786.   /* If "size != 0" use a private user provided stack. */
  787.   task_context->stack = stk;
  788.   task_context->priv_stack = prio_stksz >> 8;
  789.   /* Pass parameter 'argv' to 'rt_init_context' */
  790.   task_context->msg = argv;
  791.   /* For 'size == 0' system allocates the user stack from the memory pool. */
  792.   rt_init_context (task_context, prio_stksz & 0xFF, task);

  793.   /* Find a free entry in 'os_active_TCB' table. */
  794.   i = rt_get_TID ();
  795.   os_active_TCB[i-1] = task_context;
  796.   task_context->task_id = i;
  797.   DBG_TASK_NOTIFY(task_context, __TRUE);
  798.   rt_dispatch (task_context);
  799.   os_tsk.run->ret_val = i;
  800.   return ((OS_TID)i);
  801. }

  802. OS_RESULT rt_tsk_delete (OS_TID task_id) {
  803.   /* Terminate the task identified with "task_id". */
  804.   P_TCB task_context;

  805.   if (task_id == 0 || task_id == os_tsk.run->task_id) {
  806.     /* Terminate itself. */
  807.     os_tsk.run->state = INACTIVE;
  808.     os_active_TCB[os_tsk.run->task_id-1] = NULL;
  809.     rt_free_box (mp_stk, os_tsk.run->stack);
  810.     os_tsk.run->stack = NULL;
  811.     DBG_TASK_NOTIFY(os_tsk.run, __FALSE);
  812.     rt_free_box (mp_tcb, os_tsk.run);
  813.     os_tsk.run = NULL;
  814.     rt_dispatch (NULL);
  815.     /* The program should never come to this point. */
  816.   }
  817.   else {
  818.     /* Find the task in the "os_active_TCB" array. */
  819.     if (task_id > os_maxtaskrun || os_active_TCB[task_id-1] == NULL) {
  820.       /* Task with "task_id" not found or not started. */
  821.       return (OS_R_NOK);
  822.     }
  823.     task_context = os_active_TCB[task_id-1];
  824.     rt_rmv_list (task_context);
  825.     rt_rmv_dly (task_context);
  826.     os_active_TCB[task_id-1] = NULL;
  827.     rt_free_box (mp_stk, task_context->stack);
  828.     task_context->stack = NULL;
  829.     DBG_TASK_NOTIFY(task_context, __FALSE);
  830.     rt_free_box (mp_tcb, task_context);
  831.   }
  832.   return (OS_R_OK);
  833. }

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

  837.   DBG_INIT();

  838.   /* Initialize dynamic memory and task TCB pointers to NULL. */
  839.   for (i = 0; i < os_maxtaskrun; i++) {
  840.     os_active_TCB[i] = NULL;
  841.   }
  842.   rt_init_box (&mp_tcb, mp_tcb_size, sizeof(struct OS_TCB));
  843.   rt_init_box (&mp_stk, mp_stk_size, BOX_ALIGN_8 | (U16)(os_stackinfo));
  844. #if (os_timernum & 0xff) > 0
  845.   rt_init_box ((U32 *)m_tmr, mp_tmr_size, sizeof(struct OS_TMR));
  846. #endif

  847.   /* Set up TCB of idle demon */
  848.   os_idle_TCB.task_id = 255;
  849.   os_idle_TCB.priv_stack = 0;
  850.   rt_init_context (&os_idle_TCB, 0, os_idle_demon);

  851.   /* Set up ready list: initially empty */
  852.   os_rdy.cb_type = HCB;
  853.   os_rdy.p_lnk = NULL;
  854.   /* Set up delay list: initially empty */
  855.   os_dly.cb_type = HCB;
  856.   os_dly.p_dlnk = NULL;
  857.   os_dly.p_blnk = NULL;
  858.   os_dly.delta_time = 0;

  859.   /* Fix SP and systemvariables to assume idle task is running */
  860.   /* Transform main program into idle task by assuming idle TCB */
  861.   rt_set_PSP (os_idle_TCB.tsk_stack+32);
  862.   os_tsk.run = &os_idle_TCB;
  863.   os_tsk.run->state = RUNNING;

  864.   /* Initialize ps queue */
  865.   os_psq->first = 0;
  866.   os_psq->last = 0;
  867.   os_psq->size = os_fifo_size;

  868.   /* Intitialize system clock timer */
  869.   rt_tmr_init ();
  870.   rt_init_robin ();

  871.   /* Start up first user task before entering the endless loop */
  872.   rt_tsk_create (first_task, prio_stksz, stk, NULL);
  873. }
  874. //=========================================================================================

  875. //--- rt_time.c ----------------------------------------------------------
  876. void rt_dly_wait (U16 delay_time) {
  877.   /* Delay task by "delay_time" */
  878.   rt_block (delay_time, WAIT_DLY);
  879. }

  880. void rt_itv_set (U16 interval_time) {
  881.   /* Set interval length and define start of first interval */
  882.   os_tsk.run->interval_time = interval_time;
  883.   os_tsk.run->delta_time = interval_time + os_time;
  884. }

  885. void rt_itv_wait (void) {
  886.   /* Wait for interval end and define start of next one */
  887.   U16 delta;

  888.   delta = os_tsk.run->delta_time - os_time;
  889.   os_tsk.run->delta_time += os_tsk.run->interval_time;
  890.   if ((delta & 0x8000) == 0) {
  891.     rt_block (delta, WAIT_ITV);
  892.   }
  893. }
  894. //=========================================================================================

  895. //--- rt_membox.c
  896. int _init_box (void *box_mem, U32 box_size, U32 blk_size) {
  897.   /* Initialize memory block system, returns 0 if OK, 1 if fails. */
  898.   void *end;
  899.   void *blk;
  900.   void *next;
  901.   U32 sizeof_bm;

  902.   /* Create memory structure. */
  903.   if (blk_size & BOX_ALIGN_8) {
  904.     /* Memory blocks 8-byte aligned. */
  905.     blk_size = ((blk_size & ~BOX_ALIGN_8) + 7) & ~7;
  906.     sizeof_bm = (sizeof (struct OS_BM) + 7) & ~7;
  907.   }
  908.   else {
  909.     /* Memory blocks 4-byte aligned. */
  910.     blk_size = (blk_size + 3) & ~3;
  911.     sizeof_bm = sizeof (struct OS_BM);
  912.   }
  913.   if (blk_size == 0) {
  914.     return (1);
  915.   }
  916.   if ((blk_size + sizeof_bm) > box_size) {
  917.     return (1);
  918.   }
  919.   /* Create a Memory structure. */
  920.   blk = ((U8 *) box_mem) + sizeof_bm;
  921.   ((P_BM) box_mem)->free = blk;
  922.   end = ((U8 *) box_mem) + box_size;
  923.   ((P_BM) box_mem)->end = end;
  924.   ((P_BM) box_mem)->blk_size = blk_size;

  925.   /* Link all free blocks using offsets. */
  926.   end = ((U8 *) end) - blk_size;
  927.   while (1) {
  928.     next = ((U8 *) blk) + blk_size;
  929.     if (next > end) break;
  930.     *((void **)blk) = next;
  931.     blk = next;
  932.   }
  933.   /* end marker */
  934.   *((void **)blk) = 0;
  935.   return (0);
  936. }

  937. void *rt_alloc_box (void *box_mem) {
  938.   /* Allocate a memory block and return start address. */
  939.   void **free;
  940.   int irq_dis;

  941.   irq_dis = __disable_irq ();
  942.   free = ((P_BM) box_mem)->free;
  943.   if (free) {
  944.     ((P_BM) box_mem)->free = *free;
  945.   }
  946.   if (!irq_dis) __enable_irq ();
  947.   return (free);
  948. }

  949. void *_calloc_box (void *box_mem) {
  950.   /* Allocate a 0-initialized memory block and return start address. */
  951.   void *free;
  952.   U32 *p;
  953.   U32 i;

  954.   free = _alloc_box (box_mem);
  955.   if (free) {
  956.     p = free;
  957.     for (i = ((P_BM) box_mem)->blk_size; i; i -= 4) {
  958.       *p = 0;
  959.       p++;
  960.     }
  961.   }
  962.   return (free);
  963. }

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

  967.   if (box < box_mem || box > ((P_BM) box_mem)->end) {
  968.     return (1);
  969.   }
  970.   irq_dis = __disable_irq ();
  971.   *((void **)box) = ((P_BM) box_mem)->free;
  972.   ((P_BM) box_mem)->free = box;
  973.   if (!irq_dis) __enable_irq ();
  974.   return (0);
  975. }
  976. //==============================================================================================

  977. //----------------------------- rt_List.c -------------------------------------------
  978. void rt_put_prio (P_XCB p_CB, P_TCB p_task) {
  979.   /* Put task identified with "p_task" into list ordered by priority. */
  980.   /* "p_CB" points to head of list; list has always an element at end with */
  981.   /* a priority less than "p_task->prio". */
  982.   P_TCB p_CB2;
  983.   U32 prio;
  984.   BOOL sem_mbx = __FALSE;

  985.   if (p_CB->cb_type == SCB || p_CB->cb_type == MCB || p_CB->cb_type == MUCB) {
  986.     sem_mbx = __TRUE;
  987.   }
  988.   prio = p_task->prio;
  989.   p_CB2 = p_CB->p_lnk;
  990.   /* Search for an entry in the list */
  991.   while (p_CB2 != NULL && prio <= p_CB2->prio) {
  992.     p_CB = (P_XCB)p_CB2;
  993.     p_CB2 = p_CB2->p_lnk;
  994.   }
  995.   /* Entry found, insert the task into the list */
  996.   p_task->p_lnk = p_CB2;
  997.   p_CB->p_lnk = p_task;
  998.   if (sem_mbx) {
  999.     if (p_CB2 != NULL) {
  1000.       p_CB2->p_rlnk = p_task;
  1001.     }
  1002.     p_task->p_rlnk = (P_TCB)p_CB;
  1003.   }
  1004.   else {
  1005.     p_task->p_rlnk = NULL;
  1006.   }
  1007. }

  1008. P_TCB rt_get_first (P_XCB p_CB) {
  1009.   /* Get task at head of list: it is the task with highest priority. */
  1010.   /* "p_CB" points to head of list. */
  1011.   P_TCB p_first;

  1012.   p_first = p_CB->p_lnk;
  1013.   p_CB->p_lnk = p_first->p_lnk;
  1014.   if (p_CB->cb_type == SCB || p_CB->cb_type == MCB || p_CB->cb_type == MUCB) {
  1015.     if (p_first->p_lnk != NULL) {
  1016.       p_first->p_lnk->p_rlnk = (P_TCB)p_CB;
  1017.       p_first->p_lnk = NULL;
  1018.     }
  1019.     p_first->p_rlnk = NULL;
  1020.   }
  1021.   else {
  1022.     p_first->p_lnk = NULL;
  1023.   }
  1024.   return (p_first);
  1025. }

  1026. void rt_put_rdy_first (P_TCB p_task) {
  1027.   /* Put task identified with "p_task" at the head of the ready list. The */
  1028.   /* task must have at least a priority equal to highest priority in list. */
  1029.   p_task->p_lnk = os_rdy.p_lnk;
  1030.   p_task->p_rlnk = NULL;
  1031.   os_rdy.p_lnk = p_task;
  1032. }

  1033. P_TCB rt_get_same_rdy_prio (void) {
  1034.   /* Remove a task of same priority from ready list if any exists. Other- */
  1035.   /* wise return NULL. */
  1036.   P_TCB p_first;

  1037.   p_first = os_rdy.p_lnk;
  1038.   if (p_first->prio == os_tsk.run->prio) {
  1039.     os_rdy.p_lnk = os_rdy.p_lnk->p_lnk;
  1040.     return (p_first);
  1041.   }
  1042.   return (NULL);
  1043. }

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

  1047.   if (p_task->p_rlnk == NULL) {
  1048.     if (p_task->state == READY) {
  1049.       /* Task is chained into READY list. */
  1050.       p_CB = (P_TCB)&os_rdy;
  1051.       goto res;
  1052.     }
  1053.   }
  1054.   else {
  1055.     p_CB = p_task->p_rlnk;
  1056.     while (p_CB->cb_type == TCB) {
  1057.       /* Find a header of this task chain list. */
  1058.       p_CB = p_CB->p_rlnk;
  1059.     }
  1060. res:rt_rmv_list (p_task);
  1061.     rt_put_prio ((P_XCB)p_CB, p_task);
  1062.   }
  1063. }

  1064. void rt_put_dly (P_TCB p_task, U16 delay) {
  1065.   /* Put a task identified with "p_task" into chained delay wait list using */
  1066.   /* a delay value of "delay". */
  1067.   P_TCB p;
  1068.   U32 delta,idelay = delay;

  1069.   p = (P_TCB)&os_dly;
  1070.   if (p->p_dlnk == NULL) {
  1071.     /* Delay list empty */
  1072.     delta = 0;
  1073.     goto last;
  1074.   }
  1075.   delta = os_dly.delta_time;
  1076.   while (delta < idelay) {
  1077.     if (p->p_dlnk == NULL) {
  1078.       /* End of list found */
  1079. last: p_task->p_dlnk = NULL;
  1080.       p->p_dlnk = p_task;
  1081.       p_task->p_blnk = p;
  1082.       p->delta_time = (U16)(idelay - delta);
  1083.       p_task->delta_time = 0;
  1084.       return;
  1085.     }
  1086.     p = p->p_dlnk;
  1087.     delta += p->delta_time;
  1088.   }
  1089.   /* Right place found */
  1090.   p_task->p_dlnk = p->p_dlnk;
  1091.   p->p_dlnk = p_task;
  1092.   p_task->p_blnk = p;
  1093.   if (p_task->p_dlnk != NULL) {
  1094.     p_task->p_dlnk->p_blnk = p_task;
  1095.   }
  1096.   p_task->delta_time = (U16)(delta - idelay);
  1097.   p->delta_time -= p_task->delta_time;
  1098. }

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

  1102.   if (os_dly.p_dlnk == NULL) {
  1103.     return;
  1104.   }
  1105.   os_dly.delta_time--;
  1106.   while ((os_dly.delta_time == 0) && (os_dly.p_dlnk != NULL)) {
  1107.     p_rdy = os_dly.p_dlnk;
  1108.     if (p_rdy->p_rlnk != NULL) {
  1109.       /* Task is really enqueued, remove task from semaphore/mailbox */
  1110.       /* timeout waiting list. */
  1111.       p_rdy->p_rlnk->p_lnk = p_rdy->p_lnk;
  1112.       if (p_rdy->p_lnk != NULL) {
  1113.         p_rdy->p_lnk->p_rlnk = p_rdy->p_rlnk;
  1114.         p_rdy->p_lnk = NULL;
  1115.       }
  1116.       p_rdy->p_rlnk = NULL;
  1117.     }
  1118.     rt_put_prio (&os_rdy, p_rdy);
  1119.     os_dly.delta_time = p_rdy->delta_time;
  1120.     if (p_rdy->state == WAIT_ITV) {
  1121.       /* Calculate the next time for interval wait. */
  1122.       p_rdy->delta_time = p_rdy->interval_time + os_time;
  1123.     }
  1124.     p_rdy->state = READY;
  1125.     p_rdy->ret_val = OS_R_TMO;
  1126.     os_dly.p_dlnk = p_rdy->p_dlnk;
  1127.     if (p_rdy->p_dlnk != NULL) {
  1128.       p_rdy->p_dlnk->p_blnk = (P_TCB)&os_dly;
  1129.       p_rdy->p_dlnk = NULL;
  1130.     }
  1131.     p_rdy->p_blnk = NULL;
  1132.   }
  1133. }

  1134. void rt_rmv_list (P_TCB p_task) {
  1135.   /* Remove task identified with "p_task" from ready, semaphore or mailbox */
  1136.   /* waiting list if enqueued. */
  1137.   P_TCB p_b;

  1138.   if (p_task->p_rlnk != NULL) {
  1139.     /* A task is enqueued in semaphore / mailbox waiting list. */
  1140.     p_task->p_rlnk->p_lnk = p_task->p_lnk;
  1141.     if (p_task->p_lnk != NULL) {
  1142.       p_task->p_lnk->p_rlnk = p_task->p_rlnk;
  1143.     }
  1144.     return;
  1145.   }

  1146.   p_b = (P_TCB)&os_rdy;
  1147.   while (p_b != NULL) {
  1148.     /* Search the ready list for task "p_task" */
  1149.     if (p_b->p_lnk == p_task) {
  1150.       p_b->p_lnk = p_task->p_lnk;
  1151.       return;
  1152.     }
  1153.     p_b = p_b->p_lnk;
  1154.   }
  1155. }

  1156. void rt_rmv_dly (P_TCB p_task) {
  1157.   /* Remove task identified with "p_task" from delay list if enqueued. */
  1158.   P_TCB p_b;

  1159.   p_b = p_task->p_blnk;
  1160.   if (p_b != NULL) {
  1161.     /* Task is really enqueued */
  1162.     p_b->p_dlnk = p_task->p_dlnk;
  1163.     if (p_task->p_dlnk != NULL) {
  1164.       /* 'p_task' is in the middle of list */
  1165.       p_b->delta_time += p_task->delta_time;
  1166.       p_task->p_dlnk->p_blnk = p_b;
  1167.       p_task->p_dlnk = NULL;
  1168.     }
  1169.     else {
  1170.       /* 'p_task' is at the end of list */
  1171.       p_b->delta_time = 0;
  1172.     }
  1173.     p_task->p_blnk = NULL;
  1174.   }
  1175. }

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

  1179.   idx = rt_inc_qi (os_psq->size, &os_psq->count, &os_psq->first);
  1180.   if (idx < os_psq->size) {
  1181.     os_psq->q[idx].id = entry;
  1182.     os_psq->q[idx].arg = arg;
  1183.   }
  1184.   else {
  1185.     os_error (OS_ERR_FIFO_OVF);
  1186.   }
  1187. }
  1188. //==================================================================================

  1189. //---------------- rt_Semaphore.c --------------------------------------------------
  1190. #if OS_SEM_EN > 0
  1191. void rt_sem_init (OS_ID semaphore, U16 token_count) {
  1192.   /* Initialize a semaphore */
  1193.   P_SCB p_SCB = semaphore;

  1194.   p_SCB->cb_type = SCB;
  1195.   p_SCB->p_lnk = NULL;
  1196.   p_SCB->tokens = token_count;
  1197. }

  1198. OS_RESULT rt_sem_send (OS_ID semaphore) {
  1199.   /* Return a token to semaphore */
  1200.   P_SCB p_SCB = semaphore;
  1201.   P_TCB p_TCB;

  1202.   if (p_SCB->p_lnk != NULL) {
  1203.     /* A task is waiting for token */
  1204.     p_TCB = rt_get_first ((P_XCB)p_SCB);
  1205.     p_TCB->ret_val = OS_R_SEM;
  1206.     rt_rmv_dly (p_TCB);
  1207.     rt_dispatch (p_TCB);
  1208.     os_tsk.run->ret_val = OS_R_OK;
  1209.   }
  1210.   else {
  1211.     /* Store token. */
  1212.     p_SCB->tokens++;
  1213.   }
  1214.   return (OS_R_OK);
  1215. }

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

  1219.   if (p_SCB->tokens) {
  1220.     p_SCB->tokens--;
  1221.     return (OS_R_OK);
  1222.   }
  1223.   /* No token available: wait for one */
  1224.   if (timeout == 0) {
  1225.     return (OS_R_TMO);
  1226.   }
  1227.   if (p_SCB->p_lnk != NULL) {
  1228.     rt_put_prio ((P_XCB)p_SCB, os_tsk.run);
  1229.   }
  1230.   else {
  1231.     p_SCB->p_lnk = os_tsk.run;
  1232.     os_tsk.run->p_lnk = NULL;
  1233.     os_tsk.run->p_rlnk = (P_TCB)p_SCB;
  1234.   }
  1235.   rt_block(timeout, WAIT_SEM);
  1236.   return (OS_R_TMO);
  1237. }

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

  1241.   rt_psq_enq (p_SCB, 0);
  1242.   rt_psh_req ();
  1243. }
  1244. #endif

  1245. void rt_sem_psh (P_SCB p_CB) {
  1246.   /* Check if task has to be waken up */
  1247.   P_TCB p_TCB;

  1248.   if (p_CB->p_lnk != NULL) {
  1249.     /* A task is waiting for token */
  1250.     p_TCB = rt_get_first ((P_XCB)p_CB);
  1251.     rt_rmv_dly (p_TCB);
  1252.     p_TCB->state = READY;
  1253.     p_TCB->ret_val = OS_R_SEM;
  1254.     rt_put_prio (&os_rdy, p_TCB);
  1255.   }
  1256.   else {
  1257.     /* Store token */
  1258.     p_CB->tokens++;
  1259.   }
  1260. }
  1261. //=========================================================================================

  1262. //----------------------- rt_Mutex.c --------------------------------------------------
  1263. #if OS_MUTEX_EN > 0
  1264. void rt_mut_init (OS_ID mutex) {
  1265.   /* Initialize a mutex object */
  1266.   P_MUCB p_MCB = mutex;

  1267.   p_MCB->cb_type = MUCB;
  1268.   p_MCB->prio = 0;
  1269.   p_MCB->level = 0;
  1270.   p_MCB->p_lnk = NULL;
  1271.   p_MCB->owner = NULL;
  1272. }

  1273. OS_RESULT rt_mut_release (OS_ID mutex) {
  1274.   /* Release a mutex object */
  1275.   P_MUCB p_MCB = mutex;
  1276.   P_TCB p_TCB;

  1277.   if (p_MCB->level == 0 || p_MCB->owner != os_tsk.run) {
  1278.     /* Unbalanced mutex release or task is not the owner */
  1279.     return (OS_R_NOK);
  1280.   }
  1281.   if (--p_MCB->level != 0) {
  1282.     return (OS_R_OK);
  1283.   }
  1284.   /* Restore owner task's priority. */
  1285.   os_tsk.run->prio = p_MCB->prio;
  1286.   if (p_MCB->p_lnk != NULL) {
  1287.     /* A task is waiting for mutex. */
  1288.     p_TCB = rt_get_first ((P_XCB)p_MCB);
  1289.     p_TCB->ret_val = OS_R_MUT;
  1290.     rt_rmv_dly (p_TCB);
  1291.     /* A waiting task becomes the owner of this mutex. */
  1292.     p_MCB->level = 1;
  1293.     p_MCB->owner = p_TCB;
  1294.     p_MCB->prio = p_TCB->prio;
  1295.     /* Priority inversion, check which task continues. */
  1296.     if (os_tsk.run->prio >= rt_rdy_prio()) {
  1297.       rt_dispatch (p_TCB);
  1298.     }
  1299.     else {
  1300.       /* Ready task has higher priority than running task. */
  1301.       rt_put_prio (&os_rdy, os_tsk.run);
  1302.       rt_put_prio (&os_rdy, p_TCB);
  1303.       os_tsk.run->state = READY;
  1304.       p_TCB->state = READY;
  1305.       rt_dispatch (NULL);
  1306.     }
  1307.     os_tsk.run->ret_val = OS_R_OK;
  1308.   }
  1309.   else {
  1310.     /* Check if own priority raised by priority inversion. */
  1311.     if (rt_rdy_prio() > os_tsk.run->prio) {
  1312.       rt_put_prio (&os_rdy, os_tsk.run);
  1313.       os_tsk.run->state = READY;
  1314.       rt_dispatch (NULL);
  1315.       os_tsk.run->ret_val = OS_R_OK;
  1316.     }
  1317.   }
  1318.   return (OS_R_OK);
  1319. }

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

  1323.   if (p_MCB->level == 0) {
  1324.     p_MCB->owner = os_tsk.run;
  1325.     p_MCB->prio = os_tsk.run->prio;
  1326.     goto inc;
  1327.   }
  1328.   if (p_MCB->owner == os_tsk.run) {
  1329.     /* OK, running task is the owner of this mutex. */
  1330. inc:p_MCB->level++;
  1331.     return (OS_R_OK);
  1332.   }
  1333.   /* Mutex owned by another task, wait until released. */
  1334.   if (timeout == 0) {
  1335.     return (OS_R_TMO);
  1336.   }
  1337.   /* Raise the owner task priority if lower than current priority. */
  1338.   /* This priority inversion is called priority inheritance. */
  1339.   if (p_MCB->prio < os_tsk.run->prio) {
  1340.     p_MCB->owner->prio = os_tsk.run->prio;
  1341.     rt_resort_prio (p_MCB->owner);
  1342.   }
  1343.   if (p_MCB->p_lnk != NULL) {
  1344.     rt_put_prio ((P_XCB)p_MCB, os_tsk.run);
  1345.   }
  1346.   else {
  1347.     p_MCB->p_lnk = os_tsk.run;
  1348.     os_tsk.run->p_lnk = NULL;
  1349.     os_tsk.run->p_rlnk = (P_TCB)p_MCB;
  1350.   }
  1351.   rt_block(timeout, WAIT_MUT);
  1352.   return (OS_R_TMO);
  1353. }
  1354. #endif
  1355. //=========================================================================================

  1356. //------------------------------ rt_Event.c ------------------------------------
  1357. #if OS_FLAG_EN > 0
  1358. OS_RESULT rt_evt_wait (U16 wait_flags, U16 timeout, BOOL and_wait) {
  1359.   /* Wait for one or more event flags with optional time-out. */
  1360.   /* "wait_flags" identifies the flags to wait for. */
  1361.   /* "timeout" is the time-out limit in system ticks (0xffff if no time-out) */
  1362.   /* "and_wait" specifies the AND-ing of "wait_flags" as condition to be met */
  1363.   /* to complete the wait. (OR-ing if set to 0). */
  1364.   U32 block_state;

  1365.   if (and_wait) {
  1366.     /* Check for AND-connected events */
  1367.     if ((os_tsk.run->events & wait_flags) == wait_flags) {
  1368.       os_tsk.run->events &= ~wait_flags;
  1369.       return (OS_R_EVT);
  1370.     }
  1371.     block_state = WAIT_AND;
  1372.   }
  1373.   else {
  1374.     /* Check for OR-connected events */
  1375.     if (os_tsk.run->events & wait_flags) {
  1376.       os_tsk.run->waits = os_tsk.run->events & wait_flags;
  1377.       os_tsk.run->events &= ~wait_flags;
  1378.       return (OS_R_EVT);
  1379.     }
  1380.     block_state = WAIT_OR;
  1381.   }
  1382.   /* Task has to wait */
  1383.   os_tsk.run->waits = wait_flags;
  1384.   rt_block (timeout, (U8)block_state);
  1385.   return (OS_R_TMO);
  1386. }

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

  1390.   p_tcb = os_active_TCB[task_id-1];
  1391.   if (p_tcb == NULL) {
  1392.     return;
  1393.   }
  1394.   p_tcb->events |= event_flags;
  1395.   event_flags = p_tcb->waits;
  1396.   /* If the task is not waiting for an event, it should not be put */
  1397.   /* to ready state. */
  1398.   if (p_tcb->state == WAIT_AND) {
  1399.     /* Check for AND-connected events */
  1400.     if ((p_tcb->events & event_flags) == event_flags) {
  1401.       goto wkup;
  1402.     }
  1403.   }
  1404.   if (p_tcb->state == WAIT_OR) {
  1405.     /* Check for OR-connected events */
  1406.     if (p_tcb->events & event_flags) {
  1407.       p_tcb->waits &= p_tcb->events;
  1408. wkup: p_tcb->events &= ~event_flags;
  1409.       rt_rmv_dly (p_tcb);
  1410.       p_tcb->events &= ~event_flags;
  1411.       p_tcb->state = READY;
  1412.       p_tcb->ret_val = OS_R_EVT;
  1413.       rt_dispatch (p_tcb);
  1414.     }
  1415.   }
  1416. }

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

  1421.   if (task == NULL) {
  1422.     return;
  1423.   }
  1424.   task->events &= ~clear_flags;
  1425. }

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

  1429.   if (p_tcb == NULL) {
  1430.     return;
  1431.   }
  1432.   rt_psq_enq (p_tcb, event_flags);
  1433.   rt_psh_req ();
  1434. }

  1435. U16 rt_evt_get (void) {
  1436.   /* Get events of a running task after waiting for OR connected events. */
  1437.   return (os_tsk.run->waits);
  1438. }
  1439. #endif

  1440. void rt_evt_psh (P_TCB p_CB, U16 set_flags) {
  1441.   /* Check if task has to be waken up */
  1442.   U16 event_flags;

  1443.   p_CB->events |= set_flags;
  1444.   event_flags = p_CB->waits;
  1445.   if (p_CB->state == WAIT_AND) {
  1446.     /* Check for AND-connected events */
  1447.     if ((p_CB->events & event_flags) == event_flags) {
  1448.       goto rdy;
  1449.     }
  1450.   }
  1451.   if (p_CB->state == WAIT_OR) {
  1452.     /* Check for OR-connected events */
  1453.     if (p_CB->events & event_flags) {
  1454.       p_CB->waits &= p_CB->events;
  1455. rdy: p_CB->events &= ~event_flags;
  1456.       rt_rmv_dly (p_CB);
  1457.       p_CB->state = READY;
  1458.       p_CB->ret_val = OS_R_EVT;
  1459.       rt_put_prio (&os_rdy, p_CB);
  1460.     }
  1461.   }
  1462. }
  1463. //=========================================================================================

  1464. //---- rt_mailbox.c ----------------------------------------------------------------------------
  1465. #if OS_MBOX_EN > 0
  1466. void rt_mbx_init (OS_ID mailbox, U16 mbx_size) {
  1467.   /* Initialize a mailbox */
  1468.   P_MCB p_MCB = mailbox;

  1469.   p_MCB->cb_type = MCB;
  1470.   p_MCB->isr_st = 0;
  1471.   p_MCB->p_lnk = NULL;
  1472.   p_MCB->first = 0;
  1473.   p_MCB->last = 0;
  1474.   p_MCB->count = 0;
  1475.   p_MCB->size = (mbx_size + sizeof(void *) - sizeof(struct OS_MCB)) /
  1476.                                                      (U32)sizeof (void *);
  1477. }

  1478. OS_RESULT rt_mbx_send (OS_ID mailbox, void *p_msg, U16 timeout) {
  1479.   /* Send message to a mailbox */
  1480.   P_MCB p_MCB = mailbox;
  1481.   P_TCB p_TCB;

  1482.   if (p_MCB->p_lnk != NULL && p_MCB->count == 0) {
  1483.     /* A task is waiting for message */
  1484.     p_TCB = rt_get_first ((P_XCB)p_MCB);
  1485.     *p_TCB->msg = p_msg;
  1486.     p_TCB->ret_val = OS_R_MBX;
  1487.     rt_rmv_dly (p_TCB);
  1488.     rt_dispatch (p_TCB);
  1489.     os_tsk.run->ret_val = OS_R_OK;
  1490.   }
  1491.   else {
  1492.     /* Store message in mailbox queue */
  1493.     if (p_MCB->count == p_MCB->size) {
  1494.       /* No free message entry, wait for one. If message queue is full, */
  1495.       /* then no task is waiting for message. The 'p_MCB->p_lnk' list */
  1496.       /* pointer can now be reused for send message waits task list. */
  1497.       if (timeout == 0) {
  1498.         return (OS_R_TMO);
  1499.       }
  1500.       if (p_MCB->p_lnk != NULL) {
  1501.         rt_put_prio ((P_XCB)p_MCB, os_tsk.run);
  1502.       }
  1503.       else {
  1504.         p_MCB->p_lnk = os_tsk.run;
  1505.         os_tsk.run->p_lnk = NULL;
  1506.         os_tsk.run->p_rlnk = (P_TCB)p_MCB;
  1507.         /* Signal the 'isr_mbx_receive ()' that the task is waiting */
  1508.         /* to send a message */
  1509.         p_MCB->isr_st = 1;
  1510.       }
  1511.       os_tsk.run->msg = p_msg;
  1512.       rt_block (timeout, WAIT_MBX);
  1513.       return (OS_R_TMO);
  1514.     }
  1515.     /* Yes, there is a free entry in a mailbox. */
  1516.     p_MCB->msg[p_MCB->first] = p_msg;
  1517.     rt_inc (&p_MCB->count);
  1518.     if (++p_MCB->first == p_MCB->size) {
  1519.       p_MCB->first = 0;
  1520.     }
  1521.   }
  1522.   return (OS_R_OK);
  1523. }

  1524. OS_RESULT rt_mbx_wait (OS_ID mailbox, void **message, U16 timeout) {
  1525.   /* Receive a message; possibly wait for it */
  1526.   P_MCB p_MCB = mailbox;
  1527.   P_TCB p_TCB;

  1528.   /* If a message is available in the fifo buffer */
  1529.   /* remove it from the fifo buffer and return. */
  1530.   if (p_MCB->count) {
  1531.     *message = p_MCB->msg[p_MCB->last];
  1532.     if (++p_MCB->last == p_MCB->size) {
  1533.       p_MCB->last = 0;
  1534.     }
  1535.     if (p_MCB->p_lnk != NULL) {
  1536.       /* A task is waiting to send message */
  1537.       p_TCB = rt_get_first ((P_XCB)p_MCB);
  1538.       p_TCB->ret_val = OS_R_OK;
  1539.       p_MCB->msg[p_MCB->first] = p_TCB->msg;
  1540.       if (++p_MCB->first == p_MCB->size) {
  1541.         p_MCB->first = 0;
  1542.       }
  1543.       rt_rmv_dly (p_TCB);
  1544.       rt_dispatch (p_TCB);
  1545.       os_tsk.run->ret_val = OS_R_OK;
  1546.     }
  1547.     else {
  1548.       rt_dec (&p_MCB->count);
  1549.     }
  1550.     return (OS_R_OK);
  1551.   }
  1552.   /* No message available: wait for one */
  1553.   if (timeout == 0) {
  1554.     return (OS_R_TMO);
  1555.   }
  1556.   if (p_MCB->p_lnk != NULL) {
  1557.     rt_put_prio ((P_XCB)p_MCB, os_tsk.run);
  1558.   }
  1559.   else {
  1560.     p_MCB->p_lnk = os_tsk.run;
  1561.     os_tsk.run->p_lnk = NULL;
  1562.     os_tsk.run->p_rlnk = (P_TCB)p_MCB;
  1563.   }
  1564.   rt_block(timeout, WAIT_MBX);
  1565.   os_tsk.run->msg = message;
  1566.   return (OS_R_TMO);
  1567. }

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

  1572.   return (p_MCB->size - p_MCB->count);
  1573. }

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

  1577.   rt_psq_enq (p_MCB, (U32)p_msg);
  1578.   rt_psh_req ();
  1579. }

  1580. //---V4.13
  1581. OS_RESULT isr_mbx_receive (OS_ID mailbox, void **message) {
  1582.   /* Receive a message in the interrupt function. The interrupt function */
  1583.   /* should not wait for a message since this would block the rtx os. */
  1584.   P_MCB p_MCB = mailbox;

  1585.   if (p_MCB->count) {
  1586.     /* A message is available in the fifo buffer. */
  1587.     *message = p_MCB->msg[p_MCB->last];
  1588.     if (p_MCB->isr_st == 1) {
  1589.       /* A task is locked waiting to send message */
  1590.       p_MCB->isr_st = 2;
  1591.       rt_psq_enq (p_MCB, 0);
  1592.       rt_psh_req ();
  1593.     }
  1594.     rt_dec (&p_MCB->count);
  1595.     if (++p_MCB->last == p_MCB->size) {
  1596.       p_MCB->last = 0;
  1597.     }
  1598.     return (OS_R_MBX);
  1599.   }
  1600.   return (OS_R_OK);
  1601. }

  1602. //---V4.12
  1603. #if 0
  1604. OS_RESULT isr_mbx_receive_old (OS_ID mailbox, void **message) {
  1605.   /* Receive a message in the interrupt function. The interrupt function */
  1606.   /* should not wait for a message since this would block the rtx os. */
  1607.   P_MCB p_MCB = mailbox;

  1608.   if (p_MCB->count) {
  1609.     /* A message is available in the fifo buffer. */
  1610.     *message = p_MCB->msg[p_MCB->last];
  1611.     if (p_MCB->isr_st == 1) {
  1612.       /* A task is locked waiting to send message */
  1613.       p_MCB->isr_st = 2;
  1614.       rt_psq_enq (p_MCB, 0);
  1615.       rt_psh_req ();
  1616.     }
  1617.     rt_dec (&p_MCB->count);
  1618.     if (++p_MCB->last == p_MCB->size) {
  1619.       p_MCB->last = 0;
  1620.     }
  1621.     return (OS_R_MBX);
  1622.   }
  1623.   return (OS_R_OK);
  1624. }
  1625. #endif
  1626. #endif

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

  1630.   /* Check if this was an 'isr_mbx_receive ()' post service request. */
  1631.   if (p_CB->p_lnk != NULL && p_CB->isr_st == 2) {
  1632.     /* A task is waiting to send message, remove it from the waiting list. */
  1633.     p_CB->isr_st = 0;
  1634.     p_TCB = rt_get_first ((P_XCB)p_CB);
  1635.     p_TCB->ret_val = OS_R_OK;
  1636.     goto rdy;
  1637.   }
  1638.   /* A task is waiting for message, pass the message to task directly. */
  1639.   if (p_CB->p_lnk != NULL && p_CB->count == 0) {
  1640.     p_TCB = rt_get_first ((P_XCB)p_CB);
  1641.     *p_TCB->msg = p_msg;
  1642.     p_TCB->ret_val = OS_R_MBX;
  1643. rdy:p_TCB->state = READY;
  1644.     rt_rmv_dly (p_TCB);
  1645.     rt_put_prio (&os_rdy, p_TCB);
  1646.   }
  1647.   else {
  1648.     /* No task is waiting for message, store the message to the mailbox queue.*/
  1649.     if (p_CB->count < p_CB->size) {
  1650.       p_CB->msg[p_CB->first] = p_msg;
  1651.       rt_inc (&p_CB->count);
  1652.       if (++p_CB->first == p_CB->size) {
  1653.         p_CB->first = 0;
  1654.       }
  1655.     }
  1656.     else {
  1657.       os_error (OS_ERR_MBX_OVF);
  1658.     }
  1659.   }
  1660. }
  1661. //=========================================================================================

  1662. //------ rt_timer.c ---------------------------------------------------------------------
  1663. #if OS_TMR_EN > 0
  1664. void rt_tmr_tick (void) {
  1665.   /* Decrement delta count of timer list head. Timers having the value of */
  1666.   /* zero are removed from the list and the callback function is called. */
  1667.   P_TMR p;

  1668.   if (os_tmr.next == NULL) {
  1669.     return;
  1670.   }
  1671.   os_tmr.tcnt--;
  1672.   while (os_tmr.tcnt == 0 && (p = os_tmr.next) != NULL) {
  1673.     /* Call a user provided function to handle an elapsed timer */
  1674.     os_tmr_call (p->info);
  1675.     os_tmr.tcnt = p->tcnt;
  1676.     os_tmr.next = p->next;
  1677.     rt_free_box ((U32 *)m_tmr, p);
  1678.   }
  1679. }

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

  1686.   if (tcnt == 0 || m_tmr == NULL) {
  1687.     return (NULL);
  1688.   }
  1689.   p_tmr = rt_alloc_box ((U32 *)m_tmr);
  1690.   if (!p_tmr) {
  1691.     return (NULL);
  1692.   }
  1693.   p_tmr->info = info;
  1694.   p = (P_TMR)&os_tmr;
  1695.   delta = p->tcnt;
  1696.   while (delta < itcnt && p->next != NULL) {
  1697.     p = p->next;
  1698.     delta += p->tcnt;
  1699.   }
  1700.   /* Right place found, insert timer into the list */
  1701.   p_tmr->next = p->next;
  1702.   p_tmr->tcnt = (U16)(delta - itcnt);
  1703.   p->next = p_tmr;
  1704.   p->tcnt -= p_tmr->tcnt;
  1705.   return (p_tmr);
  1706. }

  1707. OS_ID rt_tmr_kill (OS_ID timer) {
  1708.   /* Remove user timer from the chained timer list. */
  1709.   P_TMR p, p_tmr;

  1710.   p_tmr = (P_TMR)timer;
  1711.   p = (P_TMR)&os_tmr;
  1712.   /* Search timer list for requested timer */
  1713.   while (p->next != p_tmr) {
  1714.     if (p->next == NULL) {
  1715.       /* Failed, "timer














阅读(7088) | 评论(0) | 转发(0) |
给主人留下些什么吧!~~