Пример #1
0
OS_RESULT rt_mut_delete (OS_ID mutex) {
  /* Delete a mutex object */
  P_MUCB p_MCB = mutex;
  P_TCB  p_TCB;

  __DMB();
  /* Restore owner task's priority. */
  if (p_MCB->level != 0) {
    p_MCB->owner->prio = p_MCB->prio;
    if (p_MCB->owner != os_tsk.run) {
      rt_resort_prio (p_MCB->owner);
    }
  }

  while (p_MCB->p_lnk != NULL) {
    /* A task is waiting for mutex. */
    p_TCB = rt_get_first ((P_XCB)p_MCB);
    rt_ret_val(p_TCB, 0/*osOK*/);
    rt_rmv_dly(p_TCB);
    p_TCB->state = READY;
    rt_put_prio (&os_rdy, p_TCB);
  }

  if (os_rdy.p_lnk && (os_rdy.p_lnk->prio > os_tsk.run->prio)) {
    /* preempt running task */
    rt_put_prio (&os_rdy, os_tsk.run);
    os_tsk.run->state = READY;
    rt_dispatch (NULL);
  }

  p_MCB->cb_type = 0;

  return (OS_R_OK);
}
Пример #2
0
OS_RESULT rt_sem_delete (OS_ID semaphore) {
  /* Delete semaphore */
  P_SCB p_SCB = semaphore;
  P_TCB p_TCB;

  while (p_SCB->p_lnk != NULL) {
    /* A task is waiting for token */
    p_TCB = rt_get_first ((P_XCB)p_SCB);
    rt_ret_val(p_TCB, 0);
    rt_rmv_dly(p_TCB);
    p_TCB->state = READY;
    rt_put_prio (&os_rdy, p_TCB);
  }

  if (os_rdy.p_lnk && (os_rdy.p_lnk->prio > os_tsk.run->prio)) {
    /* preempt running task */
    rt_put_prio (&os_rdy, os_tsk.run);
    os_tsk.run->state = READY;
    rt_dispatch (NULL);
  }

  p_SCB->cb_type = 0;

  return (OS_R_OK);
}
Пример #3
0
OS_RESULT rt_mut_release (OS_ID mutex) {
  /* Release a mutex object */
  P_MUCB p_MCB = mutex;
  P_TCB  p_TCB;

  if (p_MCB->level == 0 || p_MCB->owner != os_tsk.run) {
    /* Unbalanced mutex release or task is not the owner */
    return (OS_R_NOK);
  }
  __DMB();
  if (--p_MCB->level != 0) {
    return (OS_R_OK);
  }
  /* Restore owner task's priority. */
  os_tsk.run->prio = p_MCB->prio;
  if (p_MCB->p_lnk != NULL) {
    /* A task is waiting for mutex. */
    p_TCB = rt_get_first ((P_XCB)p_MCB);
#ifdef __CMSIS_RTOS
    rt_ret_val(p_TCB, 0/*osOK*/);
#else
    rt_ret_val(p_TCB, OS_R_MUT); 
#endif
    rt_rmv_dly (p_TCB);
    /* A waiting task becomes the owner of this mutex. */
    p_MCB->level     = 1;
    p_MCB->owner     = p_TCB;
    p_MCB->prio      = p_TCB->prio;
    /* Priority inversion, check which task continues. */
    if (os_tsk.run->prio >= rt_rdy_prio()) {
      rt_dispatch (p_TCB);
    }
    else {
      /* Ready task has higher priority than running task. */
      rt_put_prio (&os_rdy, os_tsk.run);
      rt_put_prio (&os_rdy, p_TCB);
      os_tsk.run->state = READY;
      p_TCB->state      = READY;
      rt_dispatch (NULL);
    }
  }
  else {
    /* Check if own priority raised by priority inversion. */
    if (rt_rdy_prio() > os_tsk.run->prio) {
      rt_put_prio (&os_rdy, os_tsk.run);
      os_tsk.run->state = READY;
      rt_dispatch (NULL);
    }
  }
  return (OS_R_OK);
}
Пример #4
0
OS_RESULT rt_tsk_prio (OS_TID task_id, U8 new_prio) {
  /* Change execution priority of a task to "new_prio". */
  P_TCB p_task;

  if (task_id == 0) {
    /* Change execution priority of calling task. */
    os_tsk.run->prio = new_prio;
run:if (rt_rdy_prio() > new_prio) {
      rt_put_prio (&os_rdy, os_tsk.run);
      os_tsk.run->state   = READY;
      rt_dispatch (NULL);
    }
    return (OS_R_OK);
  }

  /* Find the task in the "os_active_TCB" array. */
  if (task_id > os_maxtaskrun || os_active_TCB[task_id-1] == NULL) {
    /* Task with "task_id" not found or not started. */
    return (OS_R_NOK);
  }
  p_task = os_active_TCB[task_id-1];
  p_task->prio = new_prio;
  if (p_task == os_tsk.run) {
    goto run;
  }
  rt_resort_prio (p_task);
  if (p_task->state == READY) {
    /* Task enqueued in a ready list. */
    p_task = rt_get_first (&os_rdy);
    rt_dispatch (p_task);
  }
  return (OS_R_OK);
}
Пример #5
0
void rt_evt_psh (P_TCB p_CB, U16 set_flags) {
  /* Check if task has to be waken up */
  U16 event_flags;

  p_CB->events |= set_flags;
  event_flags = p_CB->waits;
  if (p_CB->state == WAIT_AND) {
    /* Check for AND-connected events */
    if ((p_CB->events & event_flags) == event_flags) {
      goto rdy;
    }
  }
  if (p_CB->state == WAIT_OR) {
    /* Check for OR-connected events */
    if (p_CB->events & event_flags) {
      p_CB->waits  &= p_CB->events;
rdy:  p_CB->events &= ~event_flags;
      rt_rmv_dly (p_CB);
      p_CB->state   = READY;
#ifdef __CMSIS_RTOS
      rt_ret_val2(p_CB, 0x08/*osEventSignal*/, p_CB->waits); 
#else
      rt_ret_val (p_CB, OS_R_EVT);
#endif
      rt_put_prio (&os_rdy, p_CB);
    }
  }
}
Пример #6
0
OS_RESULT rt_mut_wait (OS_ID mutex, U16 timeout) {
  /* Wait for a mutex, continue when mutex is free. */
  P_MUCB p_MCB = mutex;

  if (p_MCB->level == 0) {
    p_MCB->owner = os_tsk.run;
    p_MCB->prio  = os_tsk.run->prio;
    goto inc;
  }
  if (p_MCB->owner == os_tsk.run) {
    /* OK, running task is the owner of this mutex. */
inc:p_MCB->level++;
    return (OS_R_OK);
  }
  /* Mutex owned by another task, wait until released. */
  if (timeout == 0) {
    return (OS_R_TMO);
  }
  /* Raise the owner task priority if lower than current priority. */
  /* This priority inversion is called priority inheritance.       */
  if (p_MCB->prio < os_tsk.run->prio) {
    p_MCB->owner->prio = os_tsk.run->prio;
    rt_resort_prio (p_MCB->owner);
  }
  if (p_MCB->p_lnk != NULL) {
    rt_put_prio ((P_XCB)p_MCB, os_tsk.run);
  }
  else {
    p_MCB->p_lnk = os_tsk.run;
    os_tsk.run->p_lnk  = NULL;
    os_tsk.run->p_rlnk = (P_TCB)p_MCB;
  }
  rt_block(timeout, WAIT_MUT);
  return (OS_R_TMO);
}
Пример #7
0
void rt_dec_dly (void) {
  /* Decrement delta time of list head: remove tasks having a value of zero.*/
  P_TCB p_rdy;

  if (os_dly.p_dlnk == NULL) {
    return;
  }
  os_dly.delta_time--;
  while ((os_dly.delta_time == 0) && (os_dly.p_dlnk != NULL)) {
    p_rdy = os_dly.p_dlnk;
    if (p_rdy->p_rlnk != NULL) {
      /* Task is really enqueued, remove task from semaphore/mailbox */
      /* timeout waiting list. */
      p_rdy->p_rlnk->p_lnk = p_rdy->p_lnk;
      if (p_rdy->p_lnk != NULL) {
        p_rdy->p_lnk->p_rlnk = p_rdy->p_rlnk;
        p_rdy->p_lnk = NULL;
      }
      p_rdy->p_rlnk = NULL;
    }
    rt_put_prio (&os_rdy, p_rdy);
    os_dly.delta_time = p_rdy->delta_time;
    if (p_rdy->state == WAIT_ITV) {
      /* Calculate the next time for interval wait. */
      p_rdy->delta_time = p_rdy->interval_time + (U16)os_time;
    }
    p_rdy->state   = READY;
    os_dly.p_dlnk = p_rdy->p_dlnk;
    if (p_rdy->p_dlnk != NULL) {
      p_rdy->p_dlnk->p_blnk =  (P_TCB)&os_dly;
      p_rdy->p_dlnk = NULL;
    }
    p_rdy->p_blnk = NULL;
  }
}
Пример #8
0
OS_RESULT rt_mbx_wait (OS_ID mailbox, void **message, U16 timeout) {
  /* Receive a message; possibly wait for it */
  P_MCB p_MCB = mailbox;
  P_TCB p_TCB;

  /* If a message is available in the fifo buffer */
  /* remove it from the fifo buffer and return. */
  if (p_MCB->count) {
    *message = p_MCB->msg[p_MCB->last];
    if (++p_MCB->last == p_MCB->size) {
      p_MCB->last = 0;
    }
    if (p_MCB->state == 2) {
      /* A task is waiting to send message */
      p_TCB = rt_get_first ((P_XCB)p_MCB);
      if (p_MCB->p_lnk == NULL) {
        p_MCB->state = 0;
      }
#ifdef __CMSIS_RTOS
      rt_ret_val(p_TCB, 0/*osOK*/);
#else
      rt_ret_val(p_TCB, OS_R_OK);
#endif
      p_MCB->msg[p_MCB->first] = p_TCB->msg;
      if (++p_MCB->first == p_MCB->size) {
        p_MCB->first = 0;
      }
      rt_rmv_dly (p_TCB);
      rt_dispatch (p_TCB);
    }
    else {
      rt_dec (&p_MCB->count);
    }
    return (OS_R_OK);
  }
  /* No message available: wait for one */
  if (timeout == 0) {
    return (OS_R_TMO);
  }
  if (p_MCB->p_lnk != NULL) {
    rt_put_prio ((P_XCB)p_MCB, os_tsk.run);
  }
  else {
    p_MCB->p_lnk = os_tsk.run;
    os_tsk.run->p_lnk = NULL;
    os_tsk.run->p_rlnk = (P_TCB)p_MCB;
    /* Task is waiting to receive a message */      
    p_MCB->state = 1;
  }
  rt_block(timeout, WAIT_MBX);
#ifndef __CMSIS_RTOS
  os_tsk.run->msg = message;
#endif
  return (OS_R_TMO);
}
Пример #9
0
void rt_tsk_pass (void) {
  /* Allow tasks of same priority level to run cooperatively.*/
  P_TCB p_new;

  p_new = rt_get_same_rdy_prio();
  if (p_new != NULL) {
    rt_put_prio ((P_XCB)&os_rdy, os_tsk.run);
    os_tsk.run->state = READY;
    rt_switch_req (p_new);
  }
}
Пример #10
0
OS_RESULT rt_mbx_send (OS_ID mailbox, void *p_msg, U16 timeout) {
  /* Send message to a mailbox */
  P_MCB p_MCB = mailbox;
  P_TCB p_TCB;

  if (p_MCB->state == 1) {
    /* A task is waiting for message */
    p_TCB = rt_get_first ((P_XCB)p_MCB);
    if (p_MCB->p_lnk == NULL) {
      p_MCB->state = 0;
    }
#ifdef __CMSIS_RTOS
    rt_ret_val2(p_TCB, 0x10/*osEventMessage*/, (U32)p_msg);
#else
    *p_TCB->msg = p_msg;
    rt_ret_val (p_TCB, OS_R_MBX);
#endif
    rt_rmv_dly (p_TCB);
    rt_dispatch (p_TCB);
  }
  else {
    /* Store message in mailbox queue */
    if (p_MCB->count == p_MCB->size) {
      /* No free message entry, wait for one. If message queue is full, */
      /* then no task is waiting for message. The 'p_MCB->p_lnk' list   */
      /* pointer can now be reused for send message waits task list.    */
      if (timeout == 0) {
        return (OS_R_TMO);
      }
      if (p_MCB->p_lnk != NULL) {
        rt_put_prio ((P_XCB)p_MCB, os_tsk.run);
      }
      else {
        p_MCB->p_lnk = os_tsk.run;
        os_tsk.run->p_lnk  = NULL;
        os_tsk.run->p_rlnk = (P_TCB)p_MCB;
        /* Task is waiting to send a message */      
        p_MCB->state = 2;
      }
      os_tsk.run->msg = p_msg;
      rt_block (timeout, WAIT_MBX);
      return (OS_R_TMO);
    }
    /* Yes, there is a free entry in a mailbox. */
    p_MCB->msg[p_MCB->first] = p_msg;
    rt_inc (&p_MCB->count);
    if (++p_MCB->first == p_MCB->size) {
      p_MCB->first = 0;
    }
  }
  return (OS_R_OK);
}
Пример #11
0
__weak void rt_chk_robin (void) {
  /* Check if Round Robin timeout expired and switch to the next ready task.*/
  P_TCB p_new;

  if (os_robin.task != os_rdy.p_lnk) {
    /* New task was suspended, reset Round Robin timeout. */
    os_robin.task = os_rdy.p_lnk;
    os_robin.time = (U16)os_time + os_robin.tout - 1;
  }
  if (os_robin.time == (U16)os_time) {
    /* Round Robin timeout has expired, swap Robin tasks. */
    os_robin.task = NULL;
    p_new = rt_get_first (&os_rdy);
    rt_put_prio ((P_XCB)&os_rdy, p_new);
  }
}
Пример #12
0
OS_RESULT rt_mbx_send (OS_ID mailbox, void *p_msg, U16 timeout) {
  /* Send message to a mailbox */
  P_MCB p_MCB = mailbox;
  P_TCB p_TCB;

  if (p_MCB->p_lnk != NULL && p_MCB->count == 0) {
    /* A task is waiting for message */
    p_TCB = rt_get_first ((P_XCB)p_MCB);
    *p_TCB->msg    = p_msg;
    p_TCB->ret_val = OS_R_MBX;
    rt_rmv_dly (p_TCB);
    rt_dispatch (p_TCB);
    os_tsk.run->ret_val = OS_R_OK;
  }
  else {
    /* Store message in mailbox queue */
    if (p_MCB->count == p_MCB->size) {
      /* No free message entry, wait for one. If message queue is full, */
      /* then no task is waiting for message. The 'p_MCB->p_lnk' list   */
      /* pointer can now be reused for send message waits task list.    */
      if (timeout == 0) {
        return (OS_R_TMO);
      }
      if (p_MCB->p_lnk != NULL) {
        rt_put_prio ((P_XCB)p_MCB, os_tsk.run);
      }
      else {
        p_MCB->p_lnk = os_tsk.run;
        os_tsk.run->p_lnk  = NULL;
        os_tsk.run->p_rlnk = (P_TCB)p_MCB;
        /* Signal the 'isr_mbx_receive ()' that the task is waiting */
        /* to send a message */
        p_MCB->isr_st = 1;
      }
      os_tsk.run->msg = p_msg;
      rt_block (timeout, WAIT_MBX);
      return (OS_R_TMO);
    }
    /* Yes, there is a free entry in a mailbox. */
    p_MCB->msg[p_MCB->first] = p_msg;
    rt_inc (&p_MCB->count);
    if (++p_MCB->first == p_MCB->size) {
      p_MCB->first = 0;
    }
  }
  return (OS_R_OK);
}
Пример #13
0
void *rt_mem_alloc (void *box_mem)
{
	void *ptr;
	//printf("Entered malloc\n");
	ptr = rt_alloc_box(box_mem);
	if(ptr != NULL)	
		return ptr;
	else	{
		// add task to waiting list.
		rt_put_prio(&wait_list, os_tsk.run);
		rt_block (0xffff, (U8)os_tsk.run);
		return NULL;
	}
	
	/*
	void *ptr;
	ptr = rt_alloc_box(box_mem);
	
	printf("wait_count: %d\n", wait_count);
	
	if (ptr != NULL)
	{
		if (wait_count > 0)
		{
			if (os_tsk.run->prio <= highest_prio)
			{
				highest_prio = os_tsk.run->prio;
				printf("highest_prio: %d\n", highest_prio);  
				return ptr;
			}
		}
		else if (wait_count == 0)	//Empty list, with mem available, just add task
		{
			return ptr;
		}
	}
	else
	{
		rt_put_prio(&wait_list, os_tsk.run);
		//increment counter when a task is added to wait list
		wait_count++;
		printf("wait_count, if mem no available: %d\n", wait_count);
		rt_block (0xffff, (U8)os_tsk.run);
	}	
*/
}
Пример #14
0
void rt_mbx_psh (P_MCB p_CB, void *p_msg) {
  /* Store the message to the mailbox queue or pass it to task directly. */
  P_TCB p_TCB;

  /* Check if this was an 'isr_mbx_receive ()' post service request.   */
  if (p_CB->p_lnk != NULL && p_CB->isr_st == 2) {
    /* A task is waiting to send message, remove it from the waiting list. */
    p_CB->isr_st = 0;
    p_TCB = rt_get_first ((P_XCB)p_CB);
    p_TCB->ret_val = OS_R_OK;
    /* Store the message to the mailbox queue. */
    p_CB->msg[p_CB->first] = p_TCB->msg;
    rt_inc (&p_CB->count);
    if (++p_CB->first == p_CB->size) {
      p_CB->first = 0;
    }
    goto rdy;
  }
  /* A task is waiting for message, pass the message to task directly. */
  if (p_CB->p_lnk != NULL && p_CB->count == 0) {
    p_TCB = rt_get_first ((P_XCB)p_CB);
    *p_TCB->msg = p_msg;
    p_TCB->ret_val = OS_R_MBX;
rdy:p_TCB->state = READY;
    rt_rmv_dly (p_TCB);
    rt_put_prio (&os_rdy, p_TCB);
  }
  else {
    /* No task is waiting for message, store the message to the mailbox queue.*/
    if (p_CB->count < p_CB->size) {
      p_CB->msg[p_CB->first] = p_msg;
      rt_inc (&p_CB->count);
      if (++p_CB->first == p_CB->size) {
        p_CB->first = 0;
      }
    }
    else {
      os_error (OS_ERR_MBX_OVF);
    }
  }
}
Пример #15
0
void rt_resort_prio (P_TCB p_task) {
  /* Re-sort ordered lists after the priority of 'p_task' has changed.      */
  P_TCB p_CB;

  if (p_task->p_rlnk == NULL) {
    if (p_task->state == READY) {
      /* Task is chained into READY list. */
      p_CB = (P_TCB)&os_rdy;
      goto res;
    }
  }
  else {
    p_CB = p_task->p_rlnk;
    while (p_CB->cb_type == TCB) {
      /* Find a header of this task chain list. */
      p_CB = p_CB->p_rlnk;
    }
res:rt_rmv_list (p_task);
    rt_put_prio ((P_XCB)p_CB, p_task);
  }
}
Пример #16
0
void rt_sem_psh (P_SCB p_CB) {
  /* Check if task has to be waken up */
  P_TCB p_TCB;

  if (p_CB->p_lnk != NULL) {
    /* A task is waiting for token */
    p_TCB = rt_get_first ((P_XCB)p_CB);
    rt_rmv_dly (p_TCB);
    p_TCB->state   = READY;
#ifdef __CMSIS_RTOS
    rt_ret_val(p_TCB, 1);
#else
    rt_ret_val(p_TCB, OS_R_SEM);
#endif
    rt_put_prio (&os_rdy, p_TCB);
  }
  else {
    /* Store token */
    p_CB->tokens++;
  }
}
Пример #17
0
void rt_dispatch (P_TCB next_TCB) {
  /* Dispatch next task if any identified or dispatch highest ready task    */
  /* "next_TCB" identifies a task to run or has value NULL (=no next task)  */
  if (next_TCB == NULL) {
    /* Running task was blocked: continue with highest ready task */
    next_TCB = rt_get_first (&os_rdy);
    rt_switch_req (next_TCB);
  }
  else {
    /* Check which task continues */
    if (next_TCB->prio > os_tsk.run->prio) {
      /* preempt running task */
      rt_put_rdy_first (os_tsk.run);
      os_tsk.run->state = READY;
      rt_switch_req (next_TCB);
    }
    else {
      /* put next task into ready list, no task switch takes place */
      next_TCB->state = READY;
      rt_put_prio (&os_rdy, next_TCB);
    }
  }
}
Пример #18
0
OS_RESULT rt_sem_wait (OS_ID semaphore, U16 timeout) {
  /* Obtain a token; possibly wait for it */
  P_SCB p_SCB = semaphore;

  if (p_SCB->tokens) {
    p_SCB->tokens--;
    return (OS_R_OK);
  }
  /* No token available: wait for one */
  if (timeout == 0) {
    return (OS_R_TMO);
  }
  if (p_SCB->p_lnk != NULL) {
    rt_put_prio ((P_XCB)p_SCB, os_tsk.run);
  }
  else {
    p_SCB->p_lnk = os_tsk.run;
    os_tsk.run->p_lnk = NULL;
    os_tsk.run->p_rlnk = (P_TCB)p_SCB;
  }
  rt_block(timeout, WAIT_SEM);
  return (OS_R_TMO);
}
Пример #19
0
void rt_evt_psh (P_TCB p_CB, U16 set_flags) {
  /* Check if task has to be waken up */
  U16 event_flags;

  p_CB->events |= set_flags;
  event_flags = p_CB->waits;
  if (p_CB->state == WAIT_AND) {
    /* Check for AND-connected events */
    if ((p_CB->events & event_flags) == event_flags) {
      goto rdy;
    }
  }
  if (p_CB->state == WAIT_OR) {
    /* Check for OR-connected events */
    if (p_CB->events & event_flags) {
      p_CB->waits  &= p_CB->events;
rdy:  p_CB->events &= ~event_flags;
      rt_rmv_dly (p_CB);
      p_CB->state   = READY;
      p_CB->ret_val = OS_R_EVT;
      rt_put_prio (&os_rdy, p_CB);
    }
  }
}
Пример #20
0
OS_RESULT rt_tsk_delete (OS_TID task_id) {
    /* Terminate the task identified with "task_id". */
    P_TCB  task_context;
    P_TCB  p_TCB;
    P_MUCB p_MCB, p_MCB0;

    if ((task_id == 0U) || (task_id == os_tsk.run->task_id)) {
        /* Terminate itself. */
        os_tsk.run->state     = INACTIVE;
        os_tsk.run->tsk_stack = rt_get_PSP ();
        rt_stk_check ();
        p_MCB = os_tsk.run->p_mlnk;
        while (p_MCB) {
            /* Release mutexes owned by this task */
            if (p_MCB->p_lnk) {
                /* A task is waiting for mutex. */
                p_TCB = rt_get_first ((P_XCB)p_MCB);
#ifdef __CMSIS_RTOS
                rt_ret_val (p_TCB, 0U/*osOK*/);
#else
                rt_ret_val (p_TCB, OS_R_MUT);
#endif
                rt_rmv_dly (p_TCB);
                p_TCB->state = READY;
                rt_put_prio (&os_rdy, p_TCB);
                /* A waiting task becomes the owner of this mutex. */
                p_MCB0 = p_MCB->p_mlnk;
                p_MCB->level  = 1U;
                p_MCB->owner  = p_TCB;
                p_MCB->p_mlnk = p_TCB->p_mlnk;
                p_TCB->p_mlnk = p_MCB;
                p_MCB = p_MCB0;
            }
            else {
                p_MCB0 = p_MCB->p_mlnk;
                p_MCB->level  = 0U;
                p_MCB->owner  = NULL;
                p_MCB->p_mlnk = NULL;
                p_MCB = p_MCB0;
            }
        }
        os_active_TCB[os_tsk.run->task_id-1U] = NULL;
        rt_free_box (mp_stk, os_tsk.run->stack);
        os_tsk.run->stack = NULL;
        DBG_TASK_NOTIFY(os_tsk.run, __FALSE);
        rt_free_box (mp_tcb, os_tsk.run);
        os_tsk.run = NULL;
        rt_dispatch (NULL);
        /* The program should never come to this point. */
    }
    else {
        /* Find the task in the "os_active_TCB" array. */
        if ((task_id > os_maxtaskrun) || (os_active_TCB[task_id-1U] == NULL)) {
            /* Task with "task_id" not found or not started. */
            return (OS_R_NOK);
        }
        task_context = os_active_TCB[task_id-1U];
        rt_rmv_list (task_context);
        rt_rmv_dly (task_context);
        p_MCB = task_context->p_mlnk;
        while (p_MCB) {
            /* Release mutexes owned by this task */
            if (p_MCB->p_lnk) {
                /* A task is waiting for mutex. */
                p_TCB = rt_get_first ((P_XCB)p_MCB);
#ifdef __CMSIS_RTOS
                rt_ret_val (p_TCB, 0U/*osOK*/);
#else
                rt_ret_val (p_TCB, OS_R_MUT);
#endif
                rt_rmv_dly (p_TCB);
                p_TCB->state = READY;
                rt_put_prio (&os_rdy, p_TCB);
                /* A waiting task becomes the owner of this mutex. */
                p_MCB0 = p_MCB->p_mlnk;
                p_MCB->level  = 1U;
                p_MCB->owner  = p_TCB;
                p_MCB->p_mlnk = p_TCB->p_mlnk;
                p_TCB->p_mlnk = p_MCB;
                p_MCB = p_MCB0;
            }
            else {
                p_MCB0 = p_MCB->p_mlnk;
                p_MCB->level  = 0U;
                p_MCB->owner  = NULL;
                p_MCB->p_mlnk = NULL;
                p_MCB = p_MCB0;
            }
        }
        os_active_TCB[task_id-1U] = NULL;
        rt_free_box (mp_stk, task_context->stack);
        task_context->stack = NULL;
        DBG_TASK_NOTIFY(task_context, __FALSE);
        rt_free_box (mp_tcb, task_context);
        if (rt_rdy_prio() > os_tsk.run->prio) {
            /* Ready task has higher priority than running task. */
            os_tsk.run->state = READY;
            rt_put_prio (&os_rdy, os_tsk.run);
            rt_dispatch (NULL);
        }
    }
    return (OS_R_OK);
}
Пример #21
0
OS_RESULT rt_mut_delete (OS_ID mutex) {
  /* Delete a mutex object */
  P_MUCB p_MCB = mutex;
  P_TCB  p_TCB;
  P_MUCB p_mlnk;
  U8     prio;

  if (p_MCB->level != 0U) {

    p_TCB = p_MCB->owner;

    /* Remove mutex from task mutex owner list. */
    p_mlnk = p_TCB->p_mlnk;
    if (p_mlnk == p_MCB) {
      p_TCB->p_mlnk = p_MCB->p_mlnk;
    }
    else {
      while (p_mlnk) {
        if (p_mlnk->p_mlnk == p_MCB) {
          p_mlnk->p_mlnk = p_MCB->p_mlnk;
          break;
        }
        p_mlnk = p_mlnk->p_mlnk;
      }
    }

    /* Restore owner task's priority. */
    prio = p_TCB->prio_base;
    p_mlnk = p_TCB->p_mlnk;
    while (p_mlnk) {
      if ((p_mlnk->p_lnk != NULL) && (p_mlnk->p_lnk->prio > prio)) {
        /* A task with higher priority is waiting for mutex. */
        prio = p_mlnk->p_lnk->prio;
      }
      p_mlnk = p_mlnk->p_mlnk;
    }
    if (p_TCB->prio != prio) {
      p_TCB->prio = prio;
      if (p_TCB != os_tsk.run) {
        rt_resort_prio (p_TCB);
      }
    }

  }

  while (p_MCB->p_lnk != NULL) {
    /* A task is waiting for mutex. */
    p_TCB = rt_get_first ((P_XCB)p_MCB);
    rt_ret_val(p_TCB, 0U/*osOK*/);
    rt_rmv_dly(p_TCB);
    p_TCB->state = READY;
    rt_put_prio (&os_rdy, p_TCB);
  }

  if ((os_rdy.p_lnk != NULL) && (os_rdy.p_lnk->prio > os_tsk.run->prio)) {
    /* preempt running task */
    rt_put_prio (&os_rdy, os_tsk.run);
    os_tsk.run->state = READY;
    rt_dispatch (NULL);
  }

  p_MCB->cb_type = 0U;

  return (OS_R_OK);
}
Пример #22
0
OS_RESULT rt_mut_release (OS_ID mutex) {
  /* Release a mutex object */
  P_MUCB p_MCB = mutex;
  P_TCB  p_TCB;
  P_MUCB p_mlnk;
  U8     prio;

  if ((p_MCB->level == 0U) || (p_MCB->owner != os_tsk.run)) {
    /* Unbalanced mutex release or task is not the owner */
    return (OS_R_NOK);
  }
  if (--p_MCB->level != 0U) {
    return (OS_R_OK);
  }

  /* Remove mutex from task mutex owner list. */
  p_mlnk = os_tsk.run->p_mlnk;
  if (p_mlnk == p_MCB) {
    os_tsk.run->p_mlnk = p_MCB->p_mlnk;
  }
  else {
    while (p_mlnk) {
      if (p_mlnk->p_mlnk == p_MCB) {
        p_mlnk->p_mlnk = p_MCB->p_mlnk;
        break;
      }
      p_mlnk = p_mlnk->p_mlnk;
    }
  }

  /* Restore owner task's priority. */
  prio = os_tsk.run->prio_base;
  p_mlnk = os_tsk.run->p_mlnk;
  while (p_mlnk) {
    if ((p_mlnk->p_lnk != NULL) && (p_mlnk->p_lnk->prio > prio)) {
      /* A task with higher priority is waiting for mutex. */
      prio = p_mlnk->p_lnk->prio;
    }
    p_mlnk = p_mlnk->p_mlnk;
  }
  os_tsk.run->prio = prio;

  if (p_MCB->p_lnk != NULL) {
    /* A task is waiting for mutex. */
    p_TCB = rt_get_first ((P_XCB)p_MCB);
#ifdef __CMSIS_RTOS
    rt_ret_val(p_TCB, 0U/*osOK*/);
#else
    rt_ret_val(p_TCB, OS_R_MUT); 
#endif
    rt_rmv_dly (p_TCB);
    /* A waiting task becomes the owner of this mutex. */
    p_MCB->level  = 1U;
    p_MCB->owner  = p_TCB;
    p_MCB->p_mlnk = p_TCB->p_mlnk;
    p_TCB->p_mlnk = p_MCB; 
    /* Priority inversion, check which task continues. */
    if (os_tsk.run->prio >= rt_rdy_prio()) {
      rt_dispatch (p_TCB);
    }
    else {
      /* Ready task has higher priority than running task. */
      rt_put_prio (&os_rdy, os_tsk.run);
      rt_put_prio (&os_rdy, p_TCB);
      os_tsk.run->state = READY;
      p_TCB->state      = READY;
      rt_dispatch (NULL);
    }
  }
  else {
    /* Check if own priority lowered by priority inversion. */
    if (rt_rdy_prio() > os_tsk.run->prio) {
      rt_put_prio (&os_rdy, os_tsk.run);
      os_tsk.run->state = READY;
      rt_dispatch (NULL);
    }
  }
  return (OS_R_OK);
}
Пример #23
0
void rt_mbx_psh (P_MCB p_CB, void *p_msg) {
  /* Store the message to the mailbox queue or pass it to task directly. */
  P_TCB p_TCB;
  void *mem;

  if (p_CB->p_lnk != NULL) switch (p_CB->state) {
#ifdef __CMSIS_RTOS
    case 3:
      /* Task is waiting to allocate memory, remove it from the waiting list */
      mem = rt_alloc_box(p_msg);
      if (mem == NULL) break;
      p_TCB = rt_get_first ((P_XCB)p_CB);
      rt_ret_val(p_TCB, (U32)mem);
      p_TCB->state = READY;
      rt_rmv_dly (p_TCB);
      rt_put_prio (&os_rdy, p_TCB);
      break;
#endif
    case 2:
      /* Task is waiting to send a message, remove it from the waiting list */
      p_TCB = rt_get_first ((P_XCB)p_CB);
#ifdef __CMSIS_RTOS
      rt_ret_val(p_TCB, 0/*osOK*/);
#else
      rt_ret_val(p_TCB, OS_R_OK);
#endif
      p_CB->msg[p_CB->first] = p_TCB->msg;
      rt_inc (&p_CB->count);
      if (++p_CB->first == p_CB->size) {
        p_CB->first = 0;
      }
      p_TCB->state = READY;
      rt_rmv_dly (p_TCB);
      rt_put_prio (&os_rdy, p_TCB);
      break;
    case 1:
      /* Task is waiting for a message, pass the message to the task directly */
      p_TCB = rt_get_first ((P_XCB)p_CB);
#ifdef __CMSIS_RTOS
      rt_ret_val2(p_TCB, 0x10/*osEventMessage*/, (U32)p_msg);
#else
      *p_TCB->msg = p_msg;
      rt_ret_val (p_TCB, OS_R_MBX);
#endif
      p_TCB->state = READY;
      rt_rmv_dly (p_TCB);
      rt_put_prio (&os_rdy, p_TCB);
      break;
  } else {
      /* No task is waiting for a message, store it to the mailbox queue */
      if (p_CB->count < p_CB->size) {
        p_CB->msg[p_CB->first] = p_msg;
        rt_inc (&p_CB->count);
        if (++p_CB->first == p_CB->size) {
          p_CB->first = 0;
        }
      }
      else {
        os_error (OS_ERR_MBX_OVF);
      }
  }
}