static void prvTestMasterTask( void *pvParameters )
{
    BaseType_t xError;

    /* The handle to the slave task is passed in as the task parameter. */
    TaskHandle_t xTestSlaveTaskHandle = ( TaskHandle_t ) pvParameters;

    /* Avoid compiler warnings. */
    ( void ) pvParameters;

    /* Create the event group used by the tasks ready for the initial tests. */
    xEventGroup = xEventGroupCreate();
    configASSERT( xEventGroup );

    /* Perform the tests that block two tasks on different combinations of bits,
    then set each bit in turn and check the correct tasks unblock at the correct
    times. */
    xError = prvSelectiveBitsTestMasterFunction();

    for( ;; ) {
        /* Recreate the event group ready for the next cycle. */
        xEventGroup = xEventGroupCreate();
        configASSERT( xEventGroup );

        /* Perform the tests that check the behaviour when a single task is
        blocked on various combinations of event bits. */
        xError = prvBitCombinationTestMasterFunction( xError, xTestSlaveTaskHandle );

        /* Perform the task synchronisation tests. */
        xError = prvPerformTaskSyncTests( xError, xTestSlaveTaskHandle );

        /* Delete the event group. */
        vEventGroupDelete( xEventGroup );

        /* Now all the other tasks should have completed and suspended
        themselves ready for the next go around the loop. */
        if( eTaskGetState( xTestSlaveTaskHandle ) != eSuspended ) {
            xError = pdTRUE;
        }

        if( eTaskGetState( xSyncTask1 ) != eSuspended ) {
            xError = pdTRUE;
        }

        if( eTaskGetState( xSyncTask2 ) != eSuspended ) {
            xError = pdTRUE;
        }

        /* Only increment the cycle variable if no errors have been detected. */
        if( xError == pdFALSE ) {
            ulTestMasterCycles++;
        }

        configASSERT( xError == pdFALSE );
    }
}

void test_task_get_state(void* arg)
{
    //Current task should return eRunning
    TEST_ASSERT(eTaskGetState(xTaskGetCurrentTaskHandle()) == eRunning);
    //Idle task of current core should return eReady
    TEST_ASSERT(eTaskGetState(xTaskGetIdleTaskHandle()) == eReady);
    //Blocked Task should return eBlocked
    TEST_ASSERT(eTaskGetState(blocked_task_handle) == eBlocked);
    //Suspended Task should return eSuspended
    TEST_ASSERT(eTaskGetState(suspended_task_handle) == eSuspended);

    xSemaphoreGive(done_sem);
    vTaskDelete(NULL);
}

// returns whether the given task state is consistent with its current RTOS state (given by its task handle state)
static bool task_state_consistent(bool expected_state, task_type_t task_id) {
	configASSERT(task_handles[task_id] != NULL && *task_handles[task_id] != NULL);
	eTaskState task_state_rtos = eTaskGetState(*task_handles[task_id]);
	// a task is "running" (expected_state should be true) if it's NOT suspended
	// see https://www.freertos.org/RTOS-task-states.html
	return expected_state == (task_state_rtos != eSuspended);
}

void Task::suspend() {
    if (mTaskHandle && eTaskGetState(mTaskHandle) != eSuspended) {
        debug::log("Suspend task: " + getName());
        vTaskSuspend(mTaskHandle);
        afterSuspension();
    } 
}

/**
* @brief Check if a thread is already suspended or not.
* @param thread_id thread ID obtained by \ref osThreadCreate or \ref osThreadGetId.
* @retval status code that indicates the execution status of the function.
*/
osStatus osThreadIsSuspended(osThreadId thread_id)
{
  if (eTaskGetState(thread_id) == eSuspended)
    return osOK;
  else
    return osErrorOS;
}

/**
* @brief  Obtain the state of any thread.
* @param   thread_id   thread ID obtained by \ref osThreadCreate or \ref osThreadGetId.
* @retval  the stae of the thread, states are encoded by the osThreadState enumerated type.
*/
osThreadState osThreadGetState(osThreadId thread_id)
{
  eTaskState ThreadState;
  osThreadState result;
  
  ThreadState = eTaskGetState(thread_id);
  
  switch (ThreadState)
  {
  case eRunning :
    result = osThreadRunning;
    break;
  case eReady :
    result = osThreadReady;
    break;
  case eBlocked :
    result = osThreadBlocked;
    break;
  case eSuspended :
    result = osThreadSuspended;
    break;
  case eDeleted :
    result = osThreadDeleted;
    break;
  default:
    result = osThreadError;
  } 
  
  return result;
}

void print_task_info(void) {
	print("\n\n===========Task Information===========\n");
	print("state consistency: %s\n", 
		check_task_state_consistency() ? "consistent": "!! INCONSISTENT TASK STATES !!");
	for (int task = 0; task < NUM_TASKS; task++) {
		eTaskState task_state = eTaskGetState(*(task_handles[task]));
		uint16_t stack_space_left = uxTaskGetStackHighWaterMark(*task_handles[task]) * sizeof(portSTACK_TYPE);
		uint16_t stack_size = get_task_stack_size(task);
		uint16_t stack_space_available = stack_size * sizeof(portSTACK_TYPE);
		// note watchdog task as checked out in sys test so it doesn't look like something's up
		bool checked_in = (task == WATCHDOG_TASK) ? false : _get_task_checked_in(task);
		uint32_t last_check_in = _get_task_checked_in_time(task);
		uint32_t task_freq = get_task_freq(task);
		
		print("%s: %s (%s) LRT: %5d (%3d%%) stack: %4d / %4d (%3d%%)\n", 
			get_task_str(task), 
			get_task_state_str(task_state),
			checked_in ? "checked in " : "checked out",
			checked_in ? (xTaskGetTickCount() - last_check_in) : 0,
			checked_in ? (100 * (xTaskGetTickCount() - last_check_in) / task_freq) : 0,
			stack_space_available - stack_space_left,
			stack_space_available,
			(100 * (stack_space_available - stack_space_left)) / stack_space_available);
	}
}

dispatch_queue::~dispatch_queue()
{
	BaseType_t status;

	// Signal to dispatch threads that it's time to wrap up
	quit_ = true;

	// We will join each thread to confirm exiting
	for (size_t i = 0; i < threads_.size(); ++i) {
		eTaskState state;

		do {
			// Signal wake - check exit flag
			xEventGroupSetBits(notify_flags_, DISPATCH_WAKE_EVT);

			// Wait until a thread signals exit. Timeout is acceptable.
			xEventGroupWaitBits(notify_flags_, DISPATCH_EXIT_EVT, pdTRUE, pdFALSE, 10);

			// If it was not thread_[i], that is ok, but we will loop around
			// until threads_[i] has exited
			state = eTaskGetState(threads_[i].thread);
		} while (state != eDeleted);

		threads_[i].name.clear();
	}

	// Cleanup event flags and mutex
	vEventGroupDelete(notify_flags_);

	vSemaphoreDelete(mutex_);
}

void vButtonTask(void* pvParameters){

	xSemaphoreTake(xButtonSemaphore,buttonNO_BLOCK);
	for(;;){
		xSemaphoreTake(xButtonSemaphore, portMAX_DELAY);
		ulButtonPressCount++;
		xSemaphoreGive(xStatusSemaphore);
		if(eTaskGetState(xLedTaskHandle)== eBlocked || eTaskGetState(xLedTaskHandle)== eRunning ){
			vTaskSuspend(xLedTaskHandle);
		}
		else if(eTaskGetState(xLedTaskHandle)==eSuspended){
			vTaskResume(xLedTaskHandle);
		}

		vTaskDelay(500);
	}
}

eTaskState MPU_eTaskGetState( TaskHandle_t pxTask )
{
    BaseType_t xRunningPrivileged = prvRaisePrivilege();
    eTaskState eReturn;

    eReturn = eTaskGetState( pxTask );
    portRESET_PRIVILEGE( xRunningPrivileged );
    return eReturn;
}

	eTaskState MPU_eTaskGetState( xTaskHandle pxTask )
	{
    portBASE_TYPE xRunningPrivileged = prvRaisePrivilege();
	eTaskState eReturn;

		eReturn = eTaskGetState( pxTask );
        portRESET_PRIVILEGE( xRunningPrivileged );
		return eReturn;
	}

	eTaskState MPU_eTaskGetState( TaskHandle_t pxTask )
	{
	BaseType_t xRunningPrivileged = xPortRaisePrivilege();
	eTaskState eReturn;

		eReturn = eTaskGetState( pxTask );
		vPortResetPrivilege( xRunningPrivileged );
		return eReturn;
	}

void Task::start() {
    if (mTaskHandle == NULL) {
        debug::log("Creating task: " + getName());
        
        // get task name
        String taskName = getName();

        // start the task
        if (xTaskCreate(_task, taskName.c_str(), ( uint8_t ) 255, static_cast<void*>(this), 2, &mTaskHandle) != pdPASS) {
            debug::stopWithMessage("Failed to create " + getName() + " task");
        }
    } else if (eTaskGetState(mTaskHandle) == eSuspended) {
        debug::log("Resume task: " + getName());
        beforeResume();
        vTaskResume(mTaskHandle);
    }
}

void metal_finish_threads(int threads, void *tids)
{
	int i;
	TaskHandle_t *tid_p = (TaskHandle_t *)tids;

	if (!tids) {
		metal_log(METAL_LOG_ERROR, "invalid argument, tids is NULL.\n");
		return;
	}

	for (i = 0; i < threads; i++) {
		eTaskState ts;
		do {
			taskYIELD();
			ts=eTaskGetState(tid_p[i]);
		} while (ts != eDeleted);
	}
}

// resumes the given task if it was suspended, and (always) checks it in to the watchdog
void task_resume(task_type_t task_id)
{
	TaskHandle_t* task_handle = task_handles[task_id];
	configASSERT(task_handle != NULL && *task_handle != NULL);

	// always check in for watchdog when called
	// (in case it wasn't, for example on BOOT)
	// this is only called here so doesn't need to be safe
	check_in_task_unsafe(task_id);

	if (task_handle != NULL && *task_handle != NULL
		&& eTaskGetState(*task_handle) == eSuspended)
	{
		// actually resume task (will be graceful if task_handle task is not actually suspended)
		vTaskResume(*task_handle);
	}
	// if task_handle or it's value was NULL, we're in for a watchdog reset
	// which is what we want because somethings very wrong
}

// suspends the given task if it was not already suspended, and (always) checks it out of the watchdog
void task_suspend(task_type_t task_id) {
	TaskHandle_t* task_handle = task_handles[task_id];
	configASSERT(task_handle != NULL && *task_handle != NULL); // the latter would suspend THIS task

	configASSERT(task_id != BATTERY_CHARGING_TASK
				&& task_id != STATE_HANDLING_TASK 
				&& task_id != WATCHDOG_TASK 
				&& task_id != PERSISTENT_DATA_BACKUP_TASK);

	// always check out of watchdog when called (to be double-sure)
	// this is only called here so doesn't need to be safe
	check_out_task_unsafe(task_id);
	if (task_handle != NULL && *task_handle != NULL
		&& eTaskGetState(*task_handle) != eSuspended) 
	{
		// actually suspend using handle
		vTaskSuspend(*task_handle);
	}
	// if task_handle or it's value was NULL, we're in for a watchdog reset
	// which is what we want because somethings very wrong
}

static void prvTakeAndGiveInTheOppositeOrder( void )
{
    /* Ensure the slave is suspended, and that this task is running at the
    lower priority as expected as the start conditions. */
#if( INCLUDE_eTaskGetState == 1 )
    {
        configASSERT( eTaskGetState( xSlaveHandle ) == eSuspended );
    }
#endif /* INCLUDE_eTaskGetState */

    if( uxTaskPriorityGet( NULL ) != intsemMASTER_PRIORITY ) {
        xErrorDetected = pdTRUE;
    }

    /* Take the semaphore that is shared with the slave. */
    if( xSemaphoreTake( xMasterSlaveMutex, intsemNO_BLOCK ) != pdPASS ) {
        xErrorDetected = pdTRUE;
    }

    /* This task now has the mutex.  Unsuspend the slave so it too
    attempts to take the mutex. */
    vTaskResume( xSlaveHandle );

    /* The slave has the higher priority so should now have executed and
    blocked on the semaphore. */
#if( INCLUDE_eTaskGetState == 1 )
    {
        configASSERT( eTaskGetState( xSlaveHandle ) == eBlocked );
    }
#endif /* INCLUDE_eTaskGetState */

    /* This task should now have inherited the priority of the slave
    task. */
    if( uxTaskPriorityGet( NULL ) != intsemSLAVE_PRIORITY ) {
        xErrorDetected = pdTRUE;
    }

    /* Now wait a little longer than the time between ISR gives to also
    obtain the ISR mutex. */
    xOkToGiveMutex = pdTRUE;
    if( xSemaphoreTake( xISRMutex, ( xInterruptGivePeriod * 2 ) ) != pdPASS ) {
        xErrorDetected = pdTRUE;
    }
    xOkToGiveMutex = pdFALSE;

    /* Attempting to take again immediately should fail as the mutex is
    already held. */
    if( xSemaphoreTake( xISRMutex, intsemNO_BLOCK ) != pdFAIL ) {
        xErrorDetected = pdTRUE;
    }

    /* Should still be at the priority of the slave task. */
    if( uxTaskPriorityGet( NULL ) != intsemSLAVE_PRIORITY ) {
        xErrorDetected = pdTRUE;
    }

    /* Give back the shared semaphore to ensure the priority is not disinherited
    as the ISR mutex is still held.  The higher priority slave task should run
    before this task runs again. */
    if( xSemaphoreGive( xMasterSlaveMutex ) != pdPASS ) {
        xErrorDetected = pdTRUE;
    }

    /* Should still be at the priority of the slave task as this task still
    holds one semaphore (this is a simplification in the priority inheritance
    mechanism. */
    if( uxTaskPriorityGet( NULL ) != intsemSLAVE_PRIORITY ) {
        xErrorDetected = pdTRUE;
    }

    /* Give back the ISR semaphore, which should result in the priority being
    disinherited as it was the last mutex held. */
    if( xSemaphoreGive( xISRMutex ) != pdPASS ) {
        xErrorDetected = pdTRUE;
    }

    if( uxTaskPriorityGet( NULL ) != intsemMASTER_PRIORITY ) {
        xErrorDetected = pdTRUE;
    }

    /* Reset the mutex ready for the next round. */
    xQueueReset( xISRMutex );
}

static void prvRecursiveMutexPollingTask( void *pvParameters )
{
	/* Just to remove compiler warning. */
	( void ) pvParameters;

	for( ;; )
	{
		/* Keep attempting to obtain the mutex.  We should only obtain it when
		the blocking task has suspended itself, which in turn should only
		happen when the controlling task is also suspended. */
		if( xSemaphoreTakeRecursive( xMutex, recmuNO_DELAY ) == pdPASS )
		{
			#if( INCLUDE_eTaskGetState == 1 )
			{
				configASSERT( eTaskGetState( xControllingTaskHandle ) == eSuspended );
				configASSERT( eTaskGetState( xBlockingTaskHandle ) == eSuspended );
			}
			#endif /* INCLUDE_eTaskGetState */

			/* Is the blocking task suspended? */
			if( ( xBlockingIsSuspended != pdTRUE ) || ( xControllingIsSuspended != pdTRUE ) )
			{
				xErrorOccurred = pdTRUE;
			}
			else
			{
				/* Keep count of the number of cycles this task has performed
				so a stall can be detected. */
				uxPollingCycles++;

				/* We can resume the other tasks here even though they have a
				higher priority than the polling task.  When they execute they
				will attempt to obtain the mutex but fail because the polling
				task is still the mutex holder.  The polling task (this task)
				will then inherit the higher priority.  The Blocking task will
				block indefinitely when it attempts to obtain the mutex, the
				Controlling task will only block for a fixed period and an
				error will be latched if the polling task has not returned the
				mutex by the time this fixed period has expired. */
				vTaskResume( xBlockingTaskHandle );
                vTaskResume( xControllingTaskHandle );

				/* The other two tasks should now have executed and no longer
				be suspended. */
				if( ( xBlockingIsSuspended == pdTRUE ) || ( xControllingIsSuspended == pdTRUE ) )
				{
					xErrorOccurred = pdTRUE;
				}

				#if( INCLUDE_uxTaskPriorityGet == 1 )
				{
					/* Check priority inherited. */
					configASSERT( uxTaskPriorityGet( NULL ) == recmuCONTROLLING_TASK_PRIORITY );
				}
				#endif /* INCLUDE_uxTaskPriorityGet */

				#if( INCLUDE_eTaskGetState == 1 )
				{
					configASSERT( eTaskGetState( xControllingTaskHandle ) == eBlocked );
					configASSERT( eTaskGetState( xBlockingTaskHandle ) == eBlocked );
				}
				#endif /* INCLUDE_eTaskGetState */

				/* Release the mutex, disinheriting the higher priority again. */
				if( xSemaphoreGiveRecursive( xMutex ) != pdPASS )
				{
					xErrorOccurred = pdTRUE;
				}

				#if( INCLUDE_uxTaskPriorityGet == 1 )
				{
					/* Check priority disinherited. */
					configASSERT( uxTaskPriorityGet( NULL ) == recmuPOLLING_TASK_PRIORITY );
				}
				#endif /* INCLUDE_uxTaskPriorityGet */
			}
		}

		#if configUSE_PREEMPTION == 0
		{
			taskYIELD();
		}
		#endif
	}
}

/*
 * Controller task as described above.
 */
static portTASK_FUNCTION( vCounterControlTask, pvParameters )
{
uint32_t ulLastCounter;
short sLoops;
short sError = pdFALSE;

	/* Just to stop warning messages. */
	( void ) pvParameters;

	for( ;; )
	{
		/* Start with the counter at zero. */
		ulCounter = ( uint32_t ) 0;

		/* First section : */

		/* Check the continuous count task is running. */
		for( sLoops = 0; sLoops < priLOOPS; sLoops++ )
		{
			/* Suspend the continuous count task so we can take a mirror of the
			shared variable without risk of corruption.  This is not really
			needed as the other task raises its priority above this task's
			priority. */
			vTaskSuspend( xContinuousIncrementHandle );
			{
				#if( INCLUDE_eTaskGetState == 1 )
				{
					configASSERT( eTaskGetState( xContinuousIncrementHandle ) == eSuspended );
				}
				#endif /* INCLUDE_eTaskGetState */

				ulLastCounter = ulCounter;
			}
			vTaskResume( xContinuousIncrementHandle );

			#if( configUSE_PREEMPTION == 0 )
				taskYIELD();
			#endif

			#if( INCLUDE_eTaskGetState == 1 )
			{
				configASSERT( eTaskGetState( xContinuousIncrementHandle ) == eReady );
			}
			#endif /* INCLUDE_eTaskGetState */

			/* Now delay to ensure the other task has processor time. */
			vTaskDelay( priSLEEP_TIME );

			/* Check the shared variable again.  This time to ensure mutual
			exclusion the whole scheduler will be locked.  This is just for
			demo purposes! */
			vTaskSuspendAll();
			{
				if( ulLastCounter == ulCounter )
				{
					/* The shared variable has not changed.  There is a problem
					with the continuous count task so flag an error. */
					sError = pdTRUE;
				}
			}
			xTaskResumeAll();
		}

		/* Second section: */

		/* Suspend the continuous counter task so it stops accessing the shared
		variable. */
		vTaskSuspend( xContinuousIncrementHandle );

		/* Reset the variable. */
		ulCounter = ( uint32_t ) 0;

		#if( INCLUDE_eTaskGetState == 1 )
		{
			configASSERT( eTaskGetState( xLimitedIncrementHandle ) == eSuspended );
		}
		#endif /* INCLUDE_eTaskGetState */

		/* Resume the limited count task which has a higher priority than us.
		We should therefore not return from this call until the limited count
		task has suspended itself with a known value in the counter variable. */
		vTaskResume( xLimitedIncrementHandle );

		#if( configUSE_PREEMPTION == 0 )
			taskYIELD();
		#endif

		/* This task should not run again until xLimitedIncrementHandle has
		suspended itself. */
		#if( INCLUDE_eTaskGetState == 1 )
		{
			configASSERT( eTaskGetState( xLimitedIncrementHandle ) == eSuspended );
		}
		#endif /* INCLUDE_eTaskGetState */

		/* Does the counter variable have the expected value? */
		if( ulCounter != priMAX_COUNT )
//.........这里部分代码省略.........

static void prvTakeTwoMutexesReturnInSameOrder( SemaphoreHandle_t xMutex, SemaphoreHandle_t xLocalMutex )
{
	/* Take the mutex.  It should be available now. */
	if( xSemaphoreTake( xMutex, intsemNO_BLOCK ) != pdPASS )
	{
		xErrorDetected = pdTRUE;
	}

	/* Set the guarded variable to a known start value. */
	ulGuardedVariable = 0;

	/* This task's priority should be as per that assigned when the task was
	created. */
	if( uxTaskPriorityGet( NULL ) != genqMUTEX_LOW_PRIORITY )
	{
		xErrorDetected = pdTRUE;
	}

	/* Now unsuspend the high priority task.  This will attempt to take the
	mutex, and block when it finds it cannot obtain it. */
	vTaskResume( xHighPriorityMutexTask );

	#if configUSE_PREEMPTION == 0
		taskYIELD();
	#endif

	/* Ensure the task is reporting its priority as blocked and not
	suspended (as it would have done in versions up to V7.5.3). */
	#if( INCLUDE_eTaskGetState == 1 )
	{
		configASSERT( eTaskGetState( xHighPriorityMutexTask ) == eBlocked );
	}
	#endif /* INCLUDE_eTaskGetState */

	/* The priority of the high priority task should now have been inherited
	as by now it will have attempted to get the mutex. */
	if( uxTaskPriorityGet( NULL ) != genqMUTEX_HIGH_PRIORITY )
	{
		xErrorDetected = pdTRUE;
	}

	/* Now unsuspend the medium priority task.  This should not run as the
	inherited priority of this task is above that of the medium priority
	task. */
	vTaskResume( xMediumPriorityMutexTask );

	/* If the medium priority task did run then it will have incremented the
	guarded variable. */
	if( ulGuardedVariable != 0 )
	{
		xErrorDetected = pdTRUE;
	}

	/* Take the local mutex too, so two mutexes are now held. */
	if( xSemaphoreTake( xLocalMutex, intsemNO_BLOCK ) != pdPASS )
	{
		xErrorDetected = pdTRUE;
	}

	/* When the local semaphore is given back the priority of this task should
	not	yet be disinherited because the shared mutex is still held.  This is a
	simplification to allow FreeRTOS to be integrated with middleware that
	attempts to hold multiple mutexes without bloating the code with complex
	algorithms.  It is possible that the high priority mutex task will
	execute as it shares a priority with this task. */
	if( xSemaphoreGive( xLocalMutex ) != pdPASS )
	{
		xErrorDetected = pdTRUE;
	}

	#if configUSE_PREEMPTION == 0
		taskYIELD();
	#endif

	/* The guarded variable is only incremented by the medium priority task,
	which still should not have executed as this task should remain at the
	higher priority, ensure this is the case. */
	if( ulGuardedVariable != 0 )
	{
		xErrorDetected = pdTRUE;
	}

	if( uxTaskPriorityGet( NULL ) != genqMUTEX_HIGH_PRIORITY )
	{
		xErrorDetected = pdTRUE;
	}

	/* Now also give back the shared mutex, taking the held count back to 0.
	This time the priority of this task should be disinherited back to the
	priority at which it was created.  This means the medium priority task
	should execute and increment the guarded variable.  When this task next runs
	both the high and medium priority tasks will have been suspended again. */
	if( xSemaphoreGive( xMutex ) != pdPASS )
	{
		xErrorDetected = pdTRUE;
	}

	#if configUSE_PREEMPTION == 0
		taskYIELD();
	#endif
//.........这里部分代码省略.........