自学教程：C++ up_fullcontextrestore函数代码示例

void _exit(int status){  _TCB* tcb;  /* Disable interrupts.  They will be restored when the next   * task is started.   */  (void)irqsave();  slldbg("TCB=%p exitting/n", g_readytorun.head);#if defined(CONFIG_DUMP_ON_EXIT) && defined(CONFIG_DEBUG)  slldbg("Other tasks:/n");  sched_foreach(_up_dumponexit, NULL);#endif  /* Destroy the task at the head of the ready to run list. */  (void)task_deletecurrent();  /* Now, perform the context switch to the new ready-to-run task at the   * head of the list.   */  tcb = (_TCB*)g_readytorun.head;  /* Then switch contexts */  up_fullcontextrestore(tcb->xcp.regs);}

void up_sigdeliver(void){#ifndef CONFIG_DISABLE_SIGNALS  struct tcb_s  *rtcb = this_task();  uint32_t regs[XCPTCONTEXT_REGS];  sig_deliver_t sigdeliver;  /* Save the errno.  This must be preserved throughout the signal handling   * so that the user code final gets the correct errno value (probably   * EINTR).   */  int saved_errno = rtcb->pterrno;  board_autoled_on(LED_SIGNAL);  sinfo("rtcb=%p sigdeliver=%p sigpendactionq.head=%p/n",        rtcb, rtcb->xcp.sigdeliver, rtcb->sigpendactionq.head);  ASSERT(rtcb->xcp.sigdeliver != NULL);  /* Save the real return state on the stack. */  up_copystate(regs, rtcb->xcp.regs);  regs[REG_PC] = rtcb->xcp.saved_pc;  regs[REG_SR] = rtcb->xcp.saved_sr;  /* Get a local copy of the sigdeliver function pointer.  We do this so   * that we can nullify the sigdeliver function pointer in the TCB and   * accept more signal deliveries while processing the current pending   * signals.   */  sigdeliver           = rtcb->xcp.sigdeliver;  rtcb->xcp.sigdeliver = NULL;  /* Then restore the task interrupt state. */  up_irq_restore(regs[REG_SR] & 0x000000f0);  /* Deliver the signals */  sigdeliver(rtcb);  /* Output any debug messages BEFORE restoring errno (because they may   * alter errno), then disable interrupts again and restore the original   * errno that is needed by the user logic (it is probably EINTR).   */  sinfo("Resuming/n");  (void)up_irq_save();  rtcb->pterrno = saved_errno;  /* Then restore the correct state for this thread of execution. */  board_autoled_off(LED_SIGNAL);  up_fullcontextrestore(regs);#endif}

void up_sigdeliver(void){  _TCB  *rtcb = (_TCB*)g_readytorun.head;  uint32_t regs[XCPTCONTEXT_REGS];  sig_deliver_t sigdeliver;  /* Save the errno.  This must be preserved throughout the signal handling   * so that the user code final gets the correct errno value (probably   * EINTR).   */  int saved_errno = rtcb->pterrno;  up_ledon(LED_SIGNAL);  sdbg("rtcb=%p sigdeliver=%p sigpendactionq.head=%p/n",        rtcb, rtcb->xcp.sigdeliver, rtcb->sigpendactionq.head);  ASSERT(rtcb->xcp.sigdeliver != NULL);  /* Save the real return state on the stack. */  up_copystate(regs, rtcb->xcp.regs);  regs[REG_PC]         = rtcb->xcp.saved_pc;  regs[REG_PRIMASK]    = rtcb->xcp.saved_primask;  regs[REG_XPSR]       = rtcb->xcp.saved_xpsr;  /* Get a local copy of the sigdeliver function pointer. We do this so that   * we can nullify the sigdeliver function pointer in the TCB and accept   * more signal deliveries while processing the current pending signals.   */  sigdeliver           = rtcb->xcp.sigdeliver;  rtcb->xcp.sigdeliver = NULL;  /* Then restore the task interrupt state */  irqrestore((uint16_t)regs[REG_PRIMASK]);  /* Deliver the signals */  sigdeliver(rtcb);  /* Output any debug messages BEFORE restoring errno (because they may   * alter errno), then disable interrupts again and restore the original   * errno that is needed by the user logic (it is probably EINTR).   */  sdbg("Resuming/n");  (void)irqsave();  rtcb->pterrno = saved_errno;  /* Then restore the correct state for this thread of   * execution.   */  up_ledoff(LED_SIGNAL);  up_fullcontextrestore(regs);}

void up_sigdeliver(void){	struct tcb_s *rtcb = this_task();	uint32_t regs[XCPTCONTEXT_REGS];	sig_deliver_t sigdeliver;	/* Save the errno.  This must be preserved throughout the signal handling	 * so that the user code final gets the correct errno value (probably	 * EINTR).	 */	int saved_errno = rtcb->pterrno;	board_autoled_on(LED_SIGNAL);	svdbg("rtcb=%p sigdeliver=%p sigpendactionq.head=%p/n", rtcb, rtcb->xcp.sigdeliver, rtcb->sigpendactionq.head);	ASSERT(rtcb->xcp.sigdeliver != NULL);	/* Save the real return state on the stack. */	up_copyfullstate(regs, rtcb->xcp.regs);	regs[REG_PC] = rtcb->xcp.saved_pc;	regs[REG_CPSR] = rtcb->xcp.saved_cpsr;	/* Get a local copy of the sigdeliver function pointer. we do this so that	 * we can nullify the sigdeliver function pointer in the TCB and accept	 * more signal deliveries while processing the current pending signals.	 */	sigdeliver = rtcb->xcp.sigdeliver;	rtcb->xcp.sigdeliver = NULL;	/* Then restore the task interrupt state */	irqrestore(regs[REG_CPSR]);	/* Deliver the signals */	sigdeliver(rtcb);	/* Output any debug messages BEFORE restoring errno (because they may	 * alter errno), then disable interrupts again and restore the original	 * errno that is needed by the user logic (it is probably EINTR).	 */	svdbg("Resuming/n");	(void)irqsave();	rtcb->pterrno = saved_errno;	/* Then restore the correct state for this thread of execution. */	board_autoled_off(LED_SIGNAL);#ifdef CONFIG_TASK_SCHED_HISTORY	/* Save the task name which will be scheduled */	save_task_scheduling_status(rtcb);#endif	up_fullcontextrestore(regs);}

void up_release_pending(void){  struct tcb_s *rtcb = (struct tcb_s*)g_readytorun.head;  slldbg("From TCB=%p/n", rtcb);  /* Merge the g_pendingtasks list into the g_readytorun task list */  /* sched_lock(); */  if (sched_mergepending())    {      /* The currently active task has changed!  We will need to       * switch contexts.  First check if we are operating in       * interrupt context:       */      if (current_regs)        {          /* Yes, then we have to do things differently.           * Just copy the current_regs into the OLD rtcb.           */           up_copystate(rtcb->xcp.regs, current_regs);          /* Restore the exception context of the rtcb at the (new) head           * of the g_readytorun task list.           */          rtcb = (struct tcb_s*)g_readytorun.head;          slldbg("New Active Task TCB=%p/n", rtcb);          /* Then switch contexts */          current_regs = rtcb->xcp.regs;        }      /* Copy the exception context into the TCB of the task that       * was currently active. if up_saveusercontext returns a non-zero       * value, then this is really the previously running task       * restarting!       */      else if (!up_saveusercontext(rtcb->xcp.regs))        {          /* Restore the exception context of the rtcb at the (new) head           * of the g_readytorun task list.           */          rtcb = (struct tcb_s*)g_readytorun.head;          slldbg("New Active Task TCB=%p/n", rtcb);           /* Then switch contexts */          up_fullcontextrestore(rtcb->xcp.regs);        }    }}

void _exit(int status){  struct tcb_s *tcb;  /* Disable interrupts.  They will be restored when the next   * task is started.   */  (void)irqsave();  slldbg("TCB=%p exiting/n", this_task());#if defined(CONFIG_DUMP_ON_EXIT) && defined(CONFIG_DEBUG)  slldbg("Other tasks:/n");  sched_foreach(_up_dumponexit, NULL);#endif  /* Destroy the task at the head of the ready to run list. */  (void)task_exit();  /* Now, perform the context switch to the new ready-to-run task at the   * head of the list.   */  tcb = this_task();#ifdef CONFIG_ARCH_ADDRENV  /* Make sure that the address environment for the previously running   * task is closed down gracefully (data caches dump, MMU flushed) and   * set up the address environment for the new thread at the head of   * the ready-to-run list.   */  (void)group_addrenv(tcb);#endif  /* Then switch contexts */  up_fullcontextrestore(tcb->xcp.regs);  /* up_fullcontextrestore() should not return but could if the software   * interrupts are disabled.   */  PANIC();}

void _exit(int status){  struct tcb_s *tcb;  /* Make sure that we are in a critical section with local interrupts.   * The IRQ state will be restored when the next task is started.   */  (void)enter_critical_section();  sinfo("TCB=%p exiting/n", this_task());#ifdef CONFIG_DUMP_ON_EXIT  sinfo("Other tasks:/n");  sched_foreach(_up_dumponexit, NULL);#endif  /* Destroy the task at the head of the ready to run list. */  (void)task_exit();  /* Now, perform the context switch to the new ready-to-run task at the   * head of the list.   */  tcb = this_task();#ifdef CONFIG_ARCH_ADDRENV  /* Make sure that the address environment for the previously running   * task is closed down gracefully (data caches dump, MMU flushed) and   * set up the address environment for the new thread at the head of   * the ready-to-run list.   */  (void)group_addrenv(tcb);#endif  /* Then switch contexts */  up_fullcontextrestore(tcb->xcp.regs);}

void up_sigdeliver(void){  _TCB  *rtcb = (_TCB*)g_readytorun.head;#if 0  uint32_t regs[XCPTCONTEXT_REGS+3];  /* Why +3? See below */#else  uint32_t regs[XCPTCONTEXT_REGS];#endif  sig_deliver_t sigdeliver;  /* Save the errno.  This must be preserved throughout the signal handling   * so that the user code final gets the correct errno value (probably EINTR).   */  int saved_errno = rtcb->pterrno;  up_ledon(LED_SIGNAL);  sdbg("rtcb=%p sigdeliver=%p sigpendactionq.head=%p/n",        rtcb, rtcb->xcp.sigdeliver, rtcb->sigpendactionq.head);  ASSERT(rtcb->xcp.sigdeliver != NULL);  /* Save the real return state on the stack. */  up_copystate(regs, rtcb->xcp.regs);  regs[REG_PC]         = rtcb->xcp.saved_pc;  regs[REG_SR]         = rtcb->xcp.saved_sr;  /* Get a local copy of the sigdeliver function pointer. We do this so that   * we can nullify the sigdeliver function pointer in the TCB and accept   * more signal deliveries while processing the current pending signals.   */  sigdeliver           = rtcb->xcp.sigdeliver;  rtcb->xcp.sigdeliver = NULL;  /* Then restore the task interrupt state */  irqrestore(regs[REG_SR]);  /* Deliver the signals */  sigdeliver(rtcb);  /* Output any debug messages BEFORE restoring errno (because they may   * alter errno), then disable interrupts again and restore the original   * errno that is needed by the user logic (it is probably EINTR).   */  sdbg("Resuming/n");  (void)irqsave();  rtcb->pterrno = saved_errno;  /* Then restore the correct state for this thread of execution. This is an   * unusual case that must be handled by up_fullcontextresore. This case is   * unusal in two ways:   *   *   1. It is not a context switch between threads.  Rather, up_fullcontextrestore   *      must behave more it more like a longjmp within the same task, using   *      he same stack.   *   2. In this case, up_fullcontextrestore is called with r12 pointing to   *      a register save area on the stack to be destroyed.  This is   *      dangerous because there is the very real possibility that the new   *      stack pointer might overlap with the register save area and hat stack   *      usage in up_fullcontextrestore might corrupt the register save data   *      before the state is restored.  At present, there does not appear to   *      be any stack overlap problems.  If there were, then adding 3 words   *      to the size of register save structure size will protect its contents.   */  up_ledoff(LED_SIGNAL);  up_fullcontextrestore(regs);}

void up_unblock_task(struct tcb_s *tcb){  struct tcb_s *rtcb = this_task();  /* Verify that the context switch can be performed */  ASSERT((tcb->task_state >= FIRST_BLOCKED_STATE) &&         (tcb->task_state <= LAST_BLOCKED_STATE));  /* Remove the task from the blocked task list */  sched_removeblocked(tcb);  /* Add the task in the correct location in the prioritized   * ready-to-run task list   */  if (sched_addreadytorun(tcb))    {      /* The currently active task has changed! We need to do       * a context switch to the new task.       */      /* Update scheduler parameters */      sched_suspend_scheduler(rtcb);      /* Are we in an interrupt handler? */      if (current_regs)        {          /* Yes, then we have to do things differently.           * Just copy the current_regs into the OLD rtcb.           */          up_copystate(rtcb->xcp.regs, current_regs);          /* Restore the exception context of the rtcb at the (new) head           * of the ready-to-run task list.           */          rtcb = this_task();          /* Update scheduler parameters */          sched_resume_scheduler(rtcb);          /* Then switch contexts.  Any necessary address environment           * changes will be made when the interrupt returns.           */          current_regs = rtcb->xcp.regs;        }      /* We are not in an interrupt handler.  Copy the user C context       * into the TCB of the task that was previously active.  if       * up_saveusercontext returns a non-zero value, then this is really the       * previously running task restarting!       */      else if (!up_saveusercontext(rtcb->xcp.regs))        {          /* Restore the exception context of the new task that is ready to           * run (probably tcb).  This is the new rtcb at the head of the           * ready-to-run task list.           */          rtcb = this_task();#ifdef CONFIG_ARCH_ADDRENV         /* Make sure that the address environment for the previously          * running task is closed down gracefully (data caches dump,          * MMU flushed) and set up the address environment for the new          * thread at the head of the ready-to-run list.          */         (void)group_addrenv(rtcb);#endif          /* Update scheduler parameters */          sched_resume_scheduler(rtcb);          /* Then switch contexts */          up_fullcontextrestore(rtcb->xcp.regs);        }    }}

void up_reprioritize_rtr(struct tcb_s *tcb, uint8_t priority){  /* Verify that the caller is sane */  if (tcb->task_state < FIRST_READY_TO_RUN_STATE ||      tcb->task_state > LAST_READY_TO_RUN_STATE#if SCHED_PRIORITY_MIN > 0      || priority < SCHED_PRIORITY_MIN#endif#if SCHED_PRIORITY_MAX < UINT8_MAX      || priority > SCHED_PRIORITY_MAX#endif    )    {       PANIC(OSERR_BADREPRIORITIZESTATE);    }  else    {      struct tcb_s *rtcb = (struct tcb_s*)g_readytorun.head;      bool switch_needed;      slldbg("TCB=%p PRI=%d/n", tcb, priority);      /* Remove the tcb task from the ready-to-run list.       * sched_removereadytorun will return true if we just       * remove the head of the ready to run list.       */      switch_needed = sched_removereadytorun(tcb);      /* Setup up the new task priority */      tcb->sched_priority = (uint8_t)priority;      /* Return the task to the specified blocked task list.       * sched_addreadytorun will return true if the task was       * added to the new list.  We will need to perform a context       * switch only if the EXCLUSIVE or of the two calls is non-zero       * (i.e., one and only one the calls changes the head of the       * ready-to-run list).       */      switch_needed ^= sched_addreadytorun(tcb);      /* Now, perform the context switch if one is needed */      if (switch_needed)        {          /* If we are going to do a context switch, then now is the right           * time to add any pending tasks back into the ready-to-run list.           * task list now           */          if (g_pendingtasks.head)            {              sched_mergepending();            }         /* Are we in an interrupt handler? */          if (current_regs)            {              /* Yes, then we have to do things differently.               * Just copy the current_regs into the OLD rtcb.               */               up_savestate(rtcb->xcp.regs);              /* Restore the exception context of the rtcb at the (new) head                * of the g_readytorun task list.               */              rtcb = (struct tcb_s*)g_readytorun.head;              slldbg("New Active Task TCB=%p/n", rtcb);              /* Then switch contexts */              up_restorestate(rtcb->xcp.regs);            }          /* Copy the exception context into the TCB at the (old) head of the           * g_readytorun Task list. if up_saveusercontext returns a non-zero           * value, then this is really the previously running task restarting!           */          else if (!up_saveusercontext(rtcb->xcp.regs))            {              /* Restore the exception context of the rtcb at the (new) head                * of the g_readytorun task list.               */              rtcb = (struct tcb_s*)g_readytorun.head;              slldbg("New Active Task TCB=%p/n", rtcb);              /* Then switch contexts */              up_fullcontextrestore(rtcb->xcp.regs);            }        }    }//.........这里部分代码省略.........

void up_release_pending(void){  struct tcb_s *rtcb = this_task();  sinfo("From TCB=%p/n", rtcb);  /* Merge the g_pendingtasks list into the ready-to-run task list */  /* sched_lock(); */  if (sched_mergepending())    {      /* The currently active task has changed!  We will need to       * switch contexts.       */      /* Update scheduler parameters */      sched_suspend_scheduler(rtcb);      /* Are we operating in interrupt context? */      if (CURRENT_REGS)        {          /* Yes, then we have to do things differently.           * Just copy the CURRENT_REGS into the OLD rtcb.           */           up_savestate(rtcb->xcp.regs);          /* Restore the exception context of the rtcb at the (new) head           * of the ready-to-run task list.           */          rtcb = this_task();          /* Update scheduler parameters */          sched_resume_scheduler(rtcb);          /* Then switch contexts.  Any necessary address environment           * changes will be made when the interrupt returns.           */          up_restorestate(rtcb->xcp.regs);        }      /* Copy the exception context into the TCB of the task that       * was currently active. if up_saveusercontext returns a non-zero       * value, then this is really the previously running task       * restarting!       */      else if (!up_saveusercontext(rtcb->xcp.regs))        {          /* Restore the exception context of the rtcb at the (new) head           * of the ready-to-run task list.           */          rtcb = this_task();          /* Update scheduler parameters */          sched_resume_scheduler(rtcb);          /* Then switch contexts */          up_fullcontextrestore(rtcb->xcp.regs);        }    }}

void up_unblock_task(struct tcb_s *tcb){  struct tcb_s *rtcb = (struct tcb_s*)g_readytorun.head;  /* Verify that the context switch can be performed */  ASSERT((tcb->task_state >= FIRST_BLOCKED_STATE) &&         (tcb->task_state <= LAST_BLOCKED_STATE));  /* Remove the task from the blocked task list */  sched_removeblocked(tcb);  /* Reset its timeslice.  This is only meaningful for round   * robin tasks but it doesn't here to do it for everything   */#if CONFIG_RR_INTERVAL > 0  tcb->timeslice = MSEC2TICK(CONFIG_RR_INTERVAL);#endif  /* Add the task in the correct location in the prioritized   * g_readytorun task list   */  if (sched_addreadytorun(tcb))    {      /* The currently active task has changed! We need to do       * a context switch to the new task.       *       * Are we in an interrupt handler?       */      if (current_regs)        {          /* Yes, then we have to do things differently.           * Just copy the current_regs into the OLD rtcb.           */          up_savestate(rtcb->xcp.regs);          /* Restore the exception context of the rtcb at the (new) head           * of the g_readytorun task list.           */          rtcb = (struct tcb_s*)g_readytorun.head;          /* Then switch contexts */          up_restorestate(rtcb->xcp.regs);#ifdef CONFIG_ARCH_ADDRENV         /* Make sure that the address environment for the previously          * running task is closed down gracefully (data caches dump,          * MMU flushed) and set up the address environment for the new          * thread at the head of the ready-to-run list.          */         (void)group_addrenv(rtcb);#endif        }      /* We are not in an interrupt handler.  Copy the user C context       * into the TCB of the task that was previously active.  if       * up_saveusercontext returns a non-zero value, then this is really the       * previously running task restarting!       */      else if (!up_saveusercontext(rtcb->xcp.regs))        {          /* Restore the exception context of the new task that is ready to           * run (probably tcb).  This is the new rtcb at the head of the           * g_readytorun task list.           */          rtcb = (struct tcb_s*)g_readytorun.head;#ifdef CONFIG_ARCH_ADDRENV         /* Make sure that the address environment for the previously          * running task is closed down gracefully (data caches dump,          * MMU flushed) and set up the address environment for the new          * thread at the head of the ready-to-run list.          */         (void)group_addrenv(rtcb);#endif          /* Then switch contexts */          up_fullcontextrestore(rtcb->xcp.regs);        }    }}

void up_sigdeliver(void){  struct tcb_s *rtcb = this_task();  uint32_t regs[XCPTCONTEXT_REGS];  sig_deliver_t sigdeliver;  /* Save the errno.  This must be preserved throughout the signal handling   * so that the user code final gets the correct errno value (probably   * EINTR).   */  int saved_errno = rtcb->pterrno;  board_autoled_on(LED_SIGNAL);  sinfo("rtcb=%p sigdeliver=%p sigpendactionq.head=%p/n",        rtcb, rtcb->xcp.sigdeliver, rtcb->sigpendactionq.head);  ASSERT(rtcb->xcp.sigdeliver != NULL);  /* Save the real return state on the stack. */  up_copystate(regs, rtcb->xcp.regs);  regs[REG_EPC]        = rtcb->xcp.saved_epc;  regs[REG_STATUS]     = rtcb->xcp.saved_status;  /* Get a local copy of the sigdeliver function pointer. We do this so that   * we can nullify the sigdeliver function pointer in the TCB and accept   * more signal deliveries while processing the current pending signals.   */  sigdeliver           = rtcb->xcp.sigdeliver;  rtcb->xcp.sigdeliver = NULL;  /* Then restore the task interrupt state */  up_irq_restore((irqstate_t)regs[REG_STATUS]);  /* Deliver the signals */  sigdeliver(rtcb);  /* Output any debug messages BEFORE restoring errno (because they may   * alter errno), then disable interrupts again and restore the original   * errno that is needed by the user logic (it is probably EINTR).   */  sinfo("Resuming EPC: %08x STATUS: %08x/n", regs[REG_EPC], regs[REG_STATUS]);  (void)up_irq_save();  rtcb->pterrno = saved_errno;  /* Then restore the correct state for this thread of   * execution.   */  board_autoled_off(LED_SIGNAL);  up_fullcontextrestore(regs);  /* up_fullcontextrestore() should not return but could if the software   * interrupts are disabled.   */  PANIC();}

void up_sigdeliver(void){    /* NOTE the "magic" guard space added to regs.  This is a little kludge     * because up_fullcontextrestore (called below) will do a stack-to-stack     * copy an may overwrite the regs[] array contents.  Sorry.     */    struct tcb_s  *rtcb = (struct tcb_s*)g_readytorun.head;    uint32_t regs[XCPTCONTEXT_REGS + 4];    sig_deliver_t sigdeliver;    /* Save the errno.  This must be preserved throughout the signal handling     * so that the user code final gets the correct errno value (probably     * EINTR).     */    int saved_errno = rtcb->pterrno;    board_led_on(LED_SIGNAL);    sdbg("rtcb=%p sigdeliver=%p sigpendactionq.head=%p/n",         rtcb, rtcb->xcp.sigdeliver, rtcb->sigpendactionq.head);    ASSERT(rtcb->xcp.sigdeliver != NULL);    /* Save the real return state on the stack. */    up_copyfullstate(regs, rtcb->xcp.regs);    regs[REG_PC]         = rtcb->xcp.saved_pc;    regs[REG_PRIMASK]    = rtcb->xcp.saved_primask;    regs[REG_XPSR]       = rtcb->xcp.saved_xpsr;#ifdef CONFIG_BUILD_PROTECTED    regs[REG_LR]         = rtcb->xcp.saved_lr;#endif    /* Get a local copy of the sigdeliver function pointer. We do this so that     * we can nullify the sigdeliver function pointer in the TCB and accept     * more signal deliveries while processing the current pending signals.     */    sigdeliver           = rtcb->xcp.sigdeliver;    rtcb->xcp.sigdeliver = NULL;    /* Then restore the task interrupt state */    irqrestore((uint8_t)regs[REG_PRIMASK]);    /* Deliver the signal */    sigdeliver(rtcb);    /* Output any debug messages BEFORE restoring errno (because they may     * alter errno), then disable interrupts again and restore the original     * errno that is needed by the user logic (it is probably EINTR).     */    sdbg("Resuming/n");    (void)irqsave();    rtcb->pterrno = saved_errno;    /* Then restore the correct state for this thread of     * execution.     */    board_led_off(LED_SIGNAL);    up_fullcontextrestore(regs);}

void up_block_task(_TCB *tcb, tstate_t task_state){  /* Verify that the context switch can be performed */  if ((tcb->task_state < FIRST_READY_TO_RUN_STATE) ||      (tcb->task_state > LAST_READY_TO_RUN_STATE))    {      PANIC(OSERR_BADBLOCKSTATE);    }  else    {      _TCB *rtcb = (_TCB*)g_readytorun.head;      bool switch_needed;      /* Remove the tcb task from the ready-to-run list.  If we       * are blocking the task at the head of the task list (the       * most likely case), then a context switch to the next       * ready-to-run task is needed. In this case, it should       * also be true that rtcb == tcb.       */      switch_needed = sched_removereadytorun(tcb);      /* Add the task to the specified blocked task list */      sched_addblocked(tcb, (tstate_t)task_state);      /* If there are any pending tasks, then add them to the g_readytorun       * task list now       */      if (g_pendingtasks.head)        {          switch_needed |= sched_mergepending();        }      /* Now, perform the context switch if one is needed */      if (switch_needed)        {          /* Are we in an interrupt handler? */          if (current_regs)            {              /* Yes, then we have to do things differently.               * Just copy the current_regs into the OLD rtcb.               */               up_savestate(rtcb->xcp.regs);              /* Restore the exception context of the rtcb at the (new) head                * of the g_readytorun task list.               */              rtcb = (_TCB*)g_readytorun.head;              /* Then switch contexts */              up_restorestate(rtcb->xcp.regs);            }          /* Copy the user C context into the TCB at the (old) head of the           * g_readytorun Task list. if up_saveusercontext returns a non-zero           * value, then this is really the previously running task restarting!           */          else if (!up_saveusercontext(rtcb->xcp.regs))            {              /* Restore the exception context of the rtcb at the (new) head                * of the g_readytorun task list.               */              rtcb = (_TCB*)g_readytorun.head;              /* Then switch contexts */              up_fullcontextrestore(rtcb->xcp.regs);            }        }    }}

void up_block_task(struct tcb_s *tcb, tstate_t task_state){  struct tcb_s *rtcb = this_task();  bool switch_needed;  /* Verify that the context switch can be performed */  DEBUGASSERT((tcb->task_state >= FIRST_READY_TO_RUN_STATE) &&              (tcb->task_state <= LAST_READY_TO_RUN_STATE));  /* Remove the tcb task from the ready-to-run list.  If we   * are blocking the task at the head of the task list (the   * most likely case), then a context switch to the next   * ready-to-run task is needed. In this case, it should   * also be true that rtcb == tcb.   */  switch_needed = sched_removereadytorun(tcb);  /* Add the task to the specified blocked task list */  sched_addblocked(tcb, (tstate_t)task_state);  /* If there are any pending tasks, then add them to the ready-to-run   * task list now   */  if (g_pendingtasks.head)    {      switch_needed |= sched_mergepending();    }  /* Now, perform the context switch if one is needed */  if (switch_needed)    {      /* Update scheduler parameters */      sched_suspend_scheduler(rtcb);      /* Are we in an interrupt handler? */      if (g_current_regs)        {          /* Yes, then we have to do things differently.           * Just copy the g_current_regs into the OLD rtcb.           */          up_copystate(rtcb->xcp.regs, g_current_regs);          /* Restore the exception context of the rtcb at the (new) head           * of the ready-to-run task list.           */          rtcb = this_task();          /* Reset scheduler parameters */          sched_resume_scheduler(rtcb);          /* Then switch contexts.  Any necessary address environment           * changes will be made when the interrupt returns.           */          g_current_regs = rtcb->xcp.regs;        }      /* Copy the user C context into the TCB at the (old) head of the       * ready-to-run Task list. if up_saveusercontext returns a non-zero       * value, then this is really the previously running task restarting!       */      else if (!up_saveusercontext(rtcb->xcp.regs))        {          /* Restore the exception context of the rtcb at the (new) head           * of the ready-to-run task list.           */          rtcb = this_task();#ifdef CONFIG_ARCH_ADDRENV         /* Make sure that the address environment for the previously          * running task is closed down gracefully (data caches dump,          * MMU flushed) and set up the address environment for the new          * thread at the head of the ready-to-run list.          */         (void)group_addrenv(rtcb);#endif          /* Reset scheduler parameters */          sched_resume_scheduler(rtcb);          /* Then switch contexts */          up_fullcontextrestore(rtcb->xcp.regs);        }    }}

void up_release_pending(void){  struct tcb_s *rtcb = (struct tcb_s*)g_readytorun.head;  slldbg("From TCB=%p/n", rtcb);  /* Merge the g_pendingtasks list into the g_readytorun task list */  /* sched_lock(); */  if (sched_mergepending())    {      /* The currently active task has changed!  We will need to switch       * contexts.       *       * Update scheduler parameters.       */      sched_suspend_scheduler(rtcb);      /* Are we operating in interrupt context? */      if (current_regs)        {          /* Yes, then we have to do things differently.           * Just copy the current_regs into the OLD rtcb.           */           up_savestate(rtcb->xcp.regs);          /* Restore the exception context of the rtcb at the (new) head           * of the g_readytorun task list.           */          rtcb = (struct tcb_s*)g_readytorun.head;          /* Update scheduler parameters */          sched_resume_scheduler(rtcb);          /* Then switch contexts.  Any necessary address environment           * changes will be made when the interrupt returns.           */          up_restorestate(rtcb->xcp.regs);        }      /* Copy the exception context into the TCB of the task that       * was currently active. if up_saveusercontext returns a non-zero       * value, then this is really the previously running task       * restarting!       */      else if (!up_saveusercontext(rtcb->xcp.regs))        {          /* Restore the exception context of the rtcb at the (new) head           * of the g_readytorun task list.           */          rtcb = (struct tcb_s*)g_readytorun.head;#ifdef CONFIG_ARCH_ADDRENV          /* Make sure that the address environment for the previously           * running task is closed down gracefully (data caches dump,           * MMU flushed) and set up the address environment for the new           * thread at the head of the ready-to-run list.           */          (void)group_addrenv(rtcb);#endif          /* Update scheduler parameters */          sched_resume_scheduler(rtcb);          /* Then switch contexts */          up_fullcontextrestore(rtcb->xcp.regs);        }    }}

void up_unblock_task(struct tcb_s *tcb){  /* Verify that the context switch can be performed */  if ((tcb->task_state < FIRST_BLOCKED_STATE) ||      (tcb->task_state > LAST_BLOCKED_STATE))    {      PANIC(OSERR_BADUNBLOCKSTATE);    }  else    {      struct tcb_s *rtcb = (struct tcb_s*)g_readytorun.head;      /* Remove the task from the blocked task list */      sched_removeblocked(tcb);      /* Reset its timeslice.  This is only meaningful for round       * robin tasks but it doesn't here to do it for everything       */#if CONFIG_RR_INTERVAL > 0      tcb->timeslice = CONFIG_RR_INTERVAL / MSEC_PER_TICK;#endif      /* Add the task in the correct location in the prioritized       * g_readytorun task list       */      if (sched_addreadytorun(tcb))        {          /* The currently active task has changed! We need to do           * a context switch to the new task.           *           * Are we in an interrupt handler?            */          if (current_regs)            {              /* Yes, then we have to do things differently.               * Just copy the current_regs into the OLD rtcb.               */               up_copystate(rtcb->xcp.regs, current_regs);              /* Restore the exception context of the rtcb at the (new) head                * of the g_readytorun task list.               */              rtcb = (struct tcb_s*)g_readytorun.head;              /* Then switch contexts */              current_regs = rtcb->xcp.regs;            }          /* We are not in an interrupt handler.  Copy the user C context           * into the TCB of the task that was previously active.  if            * up_saveusercontext returns a non-zero value, then this is really the           * previously running task restarting!           */          else if (!up_saveusercontext(rtcb->xcp.regs))            {              /* Restore the exception context of the new task that is ready to               * run (probably tcb).  This is the new rtcb at the head of the               * g_readytorun task list.               */              rtcb = (struct tcb_s*)g_readytorun.head;              /* Then switch contexts */              up_fullcontextrestore(rtcb->xcp.regs);            }        }    }}

//.........这里部分代码省略.........      /* Remove the tcb task from the ready-to-run list.       * sched_removereadytorun will return true if we just       * remove the head of the ready to run list.       */      switch_needed = sched_removereadytorun(tcb);      /* Setup up the new task priority */      tcb->sched_priority = (uint8_t)priority;      /* Return the task to the specified blocked task list.       * sched_addreadytorun will return true if the task was       * added to the new list.  We will need to perform a context       * switch only if the EXCLUSIVE or of the two calls is non-zero       * (i.e., one and only one the calls changes the head of the       * ready-to-run list).       */      switch_needed ^= sched_addreadytorun(tcb);      /* Now, perform the context switch if one is needed */      if (switch_needed)        {          /* If we are going to do a context switch, then now is the right           * time to add any pending tasks back into the ready-to-run list.           * task list now           */          if (g_pendingtasks.head)            {              sched_mergepending();            }          /* Update scheduler parameters */          sched_suspend_scheduler(rtcb);          /* Are we in an interrupt handler? */          if (current_regs)            {              /* Yes, then we have to do things differently.               * Just copy the current_regs into the OLD rtcb.               */               up_savestate(rtcb->xcp.regs);              /* Restore the exception context of the rtcb at the (new) head               * of the g_readytorun task list.               */              rtcb = (struct tcb_s*)g_readytorun.head;              /* Update scheduler parameters */              sched_resume_scheduler(rtcb);              /* Then switch contexts.  Any necessary address environment               * changes will be made when the interrupt returns.               */              up_restorestate(rtcb->xcp.regs);            }          /* Copy the exception context into the TCB at the (old) head of the           * g_readytorun Task list. if up_saveusercontext returns a non-zero           * value, then this is really the previously running task restarting!           */          else if (!up_saveusercontext(rtcb->xcp.regs))            {              /* Restore the exception context of the rtcb at the (new) head               * of the g_readytorun task list.               */              rtcb = (struct tcb_s*)g_readytorun.head;#ifdef CONFIG_ARCH_ADDRENV              /* Make sure that the address environment for the previously               * running task is closed down gracefully (data caches dump,               * MMU flushed) and set up the address environment for the new               * thread at the head of the ready-to-run list.               */              (void)group_addrenv(rtcb);#endif              /* Update scheduler parameters */              sched_resume_scheduler(rtcb);              /* Then switch contexts */              up_fullcontextrestore(rtcb->xcp.regs);            }        }    }}