diff --git a/Makefile b/Makefile index 20a56ffc9..b6e12bad9 100644 --- a/Makefile +++ b/Makefile @@ -114,9 +114,6 @@ help: @echo " supported boards are ($(BL_BOARDS))" @echo @echo " [Simulation]" - @echo " sim_posix - Build OpenPilot simulation firmware for" - @echo " a POSIX compatible system (Linux, Mac OS X, ...)" - @echo " sim_posix_clean - Delete all build output for the POSIX simulation" @echo " sim_osx - Build OpenPilot simulation firmware for OSX" @echo " sim_osx_clean - Delete all build output for the osx simulation" @echo " sim_win32 - Build OpenPilot simulation firmware for" @@ -635,12 +632,13 @@ all_$(1)_clean: $$(addsuffix _clean, $$(filter bu_$(1), $$(BU_TARGETS))) all_$(1)_clean: $$(addsuffix _clean, $$(filter ef_$(1), $$(EF_TARGETS))) endef -ALL_BOARDS := coptercontrol pipxtreme revolution +ALL_BOARDS := coptercontrol pipxtreme revolution simposix # Friendly names of each board (used to find source tree) coptercontrol_friendly := CopterControl pipxtreme_friendly := PipXtreme revolution_friendly := Revolution +simposix_friendly := SimPosix # Start out assuming that we'll build fw, bl and bu for all boards FW_BOARDS := $(ALL_BOARDS) @@ -694,14 +692,6 @@ $(foreach board, $(ALL_BOARDS), $(eval $(call BL_TEMPLATE,$(board),$($(board)_fr # Expand the entire-flash rules $(foreach board, $(ALL_BOARDS), $(eval $(call EF_TEMPLATE,$(board),$($(board)_friendly)))) -.PHONY: sim_posix -sim_posix: sim_posix_elf - -sim_posix_%: uavobjects_flight - $(V1) mkdir -p $(BUILD_DIR)/sitl_posix - $(V1) $(MAKE) --no-print-directory \ - -C $(ROOT_DIR)/flight/OpenPilot --file=$(ROOT_DIR)/flight/OpenPilot/Makefile.posix $* - .PHONY: sim_win32 sim_win32: sim_win32_exe diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 34a89870f..571e6ac7a 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -99,6 +99,15 @@ static void SettingsUpdatedCb(UAVObjEvent * ev); int32_t StabilizationStart() { // Initialize variables + // Create object queue + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + + // Listen for updates. + // AttitudeActualConnectQueue(queue); + GyrosConnectQueue(queue); + + StabilizationSettingsConnectCallback(SettingsUpdatedCb); + SettingsUpdatedCb(StabilizationSettingsHandle()); // Start main task xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); @@ -119,17 +128,6 @@ int32_t StabilizationInitialize() RateDesiredInitialize(); #endif - // Create object queue - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - - // Listen for updates. - // AttitudeActualConnectQueue(queue); - GyrosConnectQueue(queue); - - StabilizationSettingsConnectCallback(SettingsUpdatedCb); - SettingsUpdatedCb(StabilizationSettingsHandle()); - // Start main task - return 0; } diff --git a/flight/PiOS.posix/Boards/pios_board.h b/flight/PiOS.posix/Boards/pios_board.h new file mode 100644 index 000000000..9917a1d23 --- /dev/null +++ b/flight/PiOS.posix/Boards/pios_board.h @@ -0,0 +1,10 @@ +#ifndef PIOS_BOARD_H_ +#define PIOS_BOARD_H_ + +#ifdef USE_SIM_POSIX +#include "sim_posix.h" +#else +#error Board definition has not been provided. +#endif + +#endif /* PIOS_BOARD_H_ */ diff --git a/flight/PiOS.posix/Boards/sim_posix.h b/flight/PiOS.posix/Boards/sim_posix.h new file mode 100644 index 000000000..6fbe14551 --- /dev/null +++ b/flight/PiOS.posix/Boards/sim_posix.h @@ -0,0 +1,263 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @file pios_board.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + + +#ifndef SIM_POSIX_H_ +#define SIM_POSIX_H_ + + +/** + * glue macros for file IO + **/ +#define FILEINFO FILE* +#define PIOS_FOPEN_READ(filename,file) (file=fopen((char*)filename,"r"))==NULL +#define PIOS_FOPEN_WRITE(filename,file) (file=fopen((char*)filename,"w"))==NULL +#define PIOS_FREAD(file,bufferadr,length,resultadr) (*resultadr=fread((uint8_t*)bufferadr,1,length,*file)) != length +#define PIOS_FWRITE(file,bufferadr,length,resultadr) *resultadr=fwrite((uint8_t*)bufferadr,1,length,*file) +#define PIOS_FCLOSE(file) fclose(file) +#define PIOS_FUNLINK(file) unlink((char*)filename) + +//------------------------ +// Timers and Channels Used +//------------------------ +/* +Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 +------+-----------+-----------+-----------+---------- +TIM1 | | | | +TIM2 | --------------- PIOS_DELAY ----------------- +TIM3 | | | | +TIM4 | | | | +TIM5 | | | | +TIM6 | | | | +TIM7 | | | | +TIM8 | | | | +------+-----------+-----------+-----------+---------- +*/ + +//------------------------ +// DMA Channels Used +//------------------------ +/* Channel 1 - */ +/* Channel 2 - SPI1 RX */ +/* Channel 3 - SPI1 TX */ +/* Channel 4 - SPI2 RX */ +/* Channel 5 - SPI2 TX */ +/* Channel 6 - */ +/* Channel 7 - */ +/* Channel 8 - */ +/* Channel 9 - */ +/* Channel 10 - */ +/* Channel 11 - */ +/* Channel 12 - */ + +//------------------------ +// BOOTLOADER_SETTINGS +//------------------------ +//#define BOARD_READABLE true +//#define BOARD_WRITABLE true +//#define MAX_DEL_RETRYS 3 + + +//------------------------ +// PIOS_LED +//------------------------ +#define PIOS_LED_NUM 3 +#define PIOS_LED_HEARTBEAT 0 +#define PIOS_LED_ALARM 1 + +//------------------------ +// PIOS_SPI +// See also pios_board.c +//------------------------ +//#define PIOS_SPI_MAX_DEVS 3 + +//------------------------ +// PIOS_WDG +//------------------------ +#define PIOS_WATCHDOG_TIMEOUT 250 +//#define PIOS_WDG_REGISTER RTC_BKP_DR4 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_ATTITUDE 0x0004 +#define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_SENSORS 0x0010 + +//------------------------ +// PIOS_I2C +// See also pios_board.c +//------------------------ +//#define PIOS_I2C_MAX_DEVS 3 +//extern uint32_t pios_i2c_mag_adapter_id; +//#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_adapter_id) + +//------------------------- +// PIOS_USART +// +// See also pios_board.c +//------------------------- +//#define PIOS_USART_MAX_DEVS 5 + +//------------------------- +// PIOS_COM +// +// See also pios_board.c +//------------------------- +#define PIOS_COM_MAX_DEVS 25 +extern uint32_t pios_com_telem_rf_id; +extern uint32_t pios_com_gps_id; +extern uint32_t pios_com_aux_id; +extern uint32_t pios_com_telem_usb_id; +extern uint32_t pios_com_bridge_id; +extern uint32_t pios_com_vcp_id; +#define PIOS_COM_AUX (pios_com_aux_id) +#define PIOS_COM_GPS (pios_com_gps_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_BRIDGE (pios_com_bridge_id) +#define PIOS_COM_VCP (pios_com_vcp_id) +#define PIOS_COM_DEBUG PIOS_COM_AUX + +//------------------------ +// TELEMETRY +//------------------------ +#define TELEM_QUEUE_SIZE 20 +#define PIOS_TELEM_STACK_SIZE 624 + +#define PIOS_COM_BUFFER_SIZE 1024 +#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE +#define PIOS_UDP_TX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE + +//------------------------- +// System Settings +// +// See also System_stm32f4xx.c +//------------------------- +//These macros are deprecated +//please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below +//#define PIOS_MASTER_CLOCK +//#define PIOS_PERIPHERAL_CLOCK +//#define PIOS_PERIPHERAL_CLOCK + +#define PIOS_SYSCLK 168000000 +// Peripherals that belongs to APB1 are: +// DAC |PWR |CAN1,2 +// I2C1,2,3 |UART4,5 |USART3,2 +// I2S3Ext |SPI3/I2S3 |SPI2/I2S2 +// I2S2Ext |IWDG |WWDG +// RTC/BKP reg +// TIM2,3,4,5,6,7,12,13,14 + +// Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2) +// Default APB1 Prescaler = 4 +//#define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2) + +// Peripherals belonging to APB2 +// SDIO |EXTI |SYSCFG |SPI1 +// ADC1,2,3 +// USART1,6 +// TIM1,8,9,10,11 +// +// Default APB2 Prescaler = 2 +// +//#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK + + +//------------------------- +// Interrupt Priorities +//------------------------- +//#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +//#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +//#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +//#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... + +//------------------------ +// PIOS_RCVR +// See also pios_board.c +//------------------------ +#define PIOS_RCVR_MAX_DEVS 3 +#define PIOS_RCVR_MAX_CHANNELS 12 + +//------------------------- +// Receiver PPM input +//------------------------- +//#define PIOS_PPM_MAX_DEVS 1 +//#define PIOS_PPM_NUM_INPUTS 12 + +//------------------------- +// Receiver PWM input +//------------------------- +//#define PIOS_PWM_MAX_DEVS 1 +//#define PIOS_PWM_NUM_INPUTS 8 + +//------------------------- +// Receiver SPEKTRUM input +//------------------------- +//#define PIOS_SPEKTRUM_MAX_DEVS 2 +//#define PIOS_SPEKTRUM_NUM_INPUTS 12 + +//------------------------- +// Receiver S.Bus input +//------------------------- +//#define PIOS_SBUS_MAX_DEVS 1 +//#define PIOS_SBUS_NUM_INPUTS (16+2) + +//------------------------- +// Receiver DSM input +//------------------------- +//#define PIOS_DSM_MAX_DEVS 2 +//#define PIOS_DSM_NUM_INPUTS 12 + +//------------------------- +// Servo outputs +//------------------------- +//#define PIOS_SERVO_UPDATE_HZ 50 +//#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +#define PIOS_SERVO_NUM_OUTPUTS 8 +#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS +//-------------------------- +// Timer controller settings +//-------------------------- +//#define PIOS_TIM_MAX_DEVS 6 + +//------------------------- +// ADC +// None. +//------------------------- + +//------------------------- +// USB +//------------------------- +//#define PIOS_USB_MAX_DEVS 1 +//#define PIOS_USB_ENABLED 1 /* Should remove all references to this */ +//#define PIOS_USB_HID_MAX_DEVS 1 + +#endif /* SIM_POSIX_H_ */ +/** + * @} + * @} + */ diff --git a/flight/PiOS.posix/inc/pios_delay.h b/flight/PiOS.posix/inc/pios_delay.h index dcd9bb68d..4b79e3ef1 100644 --- a/flight/PiOS.posix/inc/pios_delay.h +++ b/flight/PiOS.posix/inc/pios_delay.h @@ -1,37 +1,48 @@ -/** - ****************************************************************************** - * - * @file pios_settings.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) - * @brief Settings functions header - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef PIOS_DELAY_H -#define PIOS_DELAY_H - - -/* Public Functions */ -extern int32_t PIOS_DELAY_Init(void); -extern int32_t PIOS_DELAY_WaituS(uint16_t uS); -extern int32_t PIOS_DELAY_WaitmS(uint16_t mS); - - -#endif /* PIOS_DELAY_H */ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_DELAY Delay Functions + * @brief PiOS Delay functionality + * @{ + * + * @file pios_settings.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Settings functions header + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef PIOS_DELAY_H +#define PIOS_DELAY_H + +/* Public Functions */ +extern int32_t PIOS_DELAY_Init(void); +extern int32_t PIOS_DELAY_WaituS(uint32_t uS); +extern int32_t PIOS_DELAY_WaitmS(uint32_t mS); +extern uint32_t PIOS_DELAY_GetuS(); +extern uint32_t PIOS_DELAY_GetuSSince(uint32_t t); +extern uint32_t PIOS_DELAY_GetRaw(); +extern uint32_t PIOS_DELAY_DiffuS(uint32_t raw); + +#endif /* PIOS_DELAY_H */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS.posix/inc/pios_led.h b/flight/PiOS.posix/inc/pios_led.h index 42b0d2443..6ce08b0a0 100644 --- a/flight/PiOS.posix/inc/pios_led.h +++ b/flight/PiOS.posix/inc/pios_led.h @@ -1,43 +1,39 @@ -/** - ****************************************************************************** - * - * @file pios_led.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief LED functions header. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef PIOS_LED_H -#define PIOS_LED_H - -/* Type Definitions */ - -#if (PIOS_LED_NUM == 1) -typedef enum {LED1 = 0} LedTypeDef; -#elif (PIOS_LED_NUM == 2) -typedef enum {LED1 = 0, LED2 = 1} LedTypeDef; -#endif - -/* Public Functions */ -extern void PIOS_LED_Init(void); -extern void PIOS_LED_On(LedTypeDef LED); -extern void PIOS_LED_Off(LedTypeDef LED); -extern void PIOS_LED_Toggle(LedTypeDef LED); - -#endif /* PIOS_LED_H */ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_LED LED Functions + * @{ + * + * @file pios_led.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief LED functions header. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef PIOS_LED_H +#define PIOS_LED_H + +/* Public Functions */ +extern void PIOS_LED_On(uint32_t led_id); +extern void PIOS_LED_Off(uint32_t led_id); +extern void PIOS_LED_Toggle(uint32_t led_id); +extern void PIOS_LED_Init(); + +#endif /* PIOS_LED_H */ diff --git a/flight/PiOS.posix/inc/pios_posix.h b/flight/PiOS.posix/inc/pios_posix.h index b458f77b6..2b411d270 100644 --- a/flight/PiOS.posix/inc/pios_posix.h +++ b/flight/PiOS.posix/inc/pios_posix.h @@ -35,10 +35,10 @@ typedef enum {FALSE = 0, TRUE = !FALSE} bool; #define true TRUE #endif -#define FILEINFO FILE* +//#define FILEINFO FILE* -#define PIOS_SERVO_NUM_OUTPUTS 8 -#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS +//#define PIOS_SERVO_NUM_OUTPUTS 8 +//#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS #endif diff --git a/flight/PiOS.posix/inc/pios_udp_priv.h b/flight/PiOS.posix/inc/pios_udp_priv.h index 999e192e9..5ce32a01a 100644 --- a/flight/PiOS.posix/inc/pios_udp_priv.h +++ b/flight/PiOS.posix/inc/pios_udp_priv.h @@ -45,7 +45,11 @@ struct pios_udp_cfg { typedef struct { const struct pios_udp_cfg * cfg; +#if defined(PIOS_INCLUDE_FREERTOS) + xTaskHandle rxThread; +#else pthread_t rxThread; +#endif int socket; struct sockaddr_in server; @@ -66,6 +70,6 @@ typedef struct { extern int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg); - +extern const struct pios_com_driver pios_udp_com_driver; #endif /* PIOS_UDP_PRIV_H */ diff --git a/flight/PiOS.posix/pios.h b/flight/PiOS.posix/pios.h index 9256891b1..8e96fa092 100644 --- a/flight/PiOS.posix/pios.h +++ b/flight/PiOS.posix/pios.h @@ -29,7 +29,7 @@ #define PIOS_H /* PIOS Feature Selection */ -#include "pios_config_posix.h" +#include "pios_config.h" #include #if defined(PIOS_INCLUDE_FREERTOS) @@ -52,7 +52,7 @@ #include "pios_initcall.h" /* PIOS Board Specific Device Configuration */ -#include "pios_board_posix.h" +#include "pios_board.h" /* PIOS Hardware Includes (posix) */ #include diff --git a/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/port.c b/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/port.c new file mode 100644 index 000000000..1deb36cc7 --- /dev/null +++ b/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/port.c @@ -0,0 +1,1092 @@ +/* + Copyright (C) 2011 Corvus Corax from OpenPilot.org + based on linux port from William Davy + + This file is part of the FreeRTOS.org distribution. + + FreeRTOS.org is free software; you can redistribute it and/or modify it + under the terms of the GNU General Public License (version 2) as published + by the Free Software Foundation and modified by the FreeRTOS exception. + + FreeRTOS.org is distributed in the hope that it will be useful, but WITHOUT + ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + + You should have received a copy of the GNU General Public License along + with FreeRTOS.org; if not, write to the Free Software Foundation, Inc., 59 + Temple Place, Suite 330, Boston, MA 02111-1307 USA. + + A special exception to the GPL is included to allow you to distribute a + combined work that includes FreeRTOS.org without being obliged to provide + the source code for any proprietary components. See the licensing section + of http://www.FreeRTOS.org for full details. + + + *************************************************************************** + * * + * Get the FreeRTOS eBook! See http://www.FreeRTOS.org/Documentation * + * * + * This is a concise, step by step, 'hands on' guide that describes both * + * general multitasking concepts and FreeRTOS specifics. It presents and * + * explains numerous examples that are written using the FreeRTOS API. * + * Full source code for all the examples is provided in an accompanying * + * .zip file. * + * * + *************************************************************************** + + 1 tab == 4 spaces! + + Please ensure to read the configuration and relevant port sections of the + online documentation. + + http://www.FreeRTOS.org - Documentation, latest information, license and + contact details. + + http://www.SafeRTOS.com - A version that is certified for use in safety + critical systems. + + http://www.OpenRTOS.com - Commercial support, development, porting, + licensing and training services. +*/ + +/*----------------------------------------------------------- + * Implementation of functions defined in portable.h for the Posix port. + *----------------------------------------------------------*/ + + +/** Description of scheduler: + +This scheduler is based on posix signals to halt or preempt tasks, and on +pthread conditions to resume them. + +Each FreeRTOS thread is created as a posix thread, with a signal handler to +SIGUSR1 (SIG_SUSPEND) signals. + +Suspension of a thread is done by setting the threads state to +"YIELDING/PREEMPTING", then signaling the thread until the signal handler +changes that state to "SLEEPING", thus acknowledging the suspend. + +The thread will then wait within the signal handler for a thread specific +condition to be set, which allows it to resume operation, setting its state to +"RUNNING" + +The running thread also always holds a mutex (xRunningThreadMutex) which is +given up only when the thread suspends. + +On thread creation the new thread will acquire this mutex, then yield. + +Both preemption and yielding is done using the same mechanism, sending a +SIG_SUSPEND to the preempted thread respectively to itself, however different +synchronization safeguards apply depending if a thread suspends itself or is +suspended remotely + +Preemption is done by the main scheduler thread which attempts to run a tick +handler at accurate intervals using nanosleep and gettimeofday, which allows +more accurate high frequency ticks than a timer signal handler. + +All public functions in this port are protected by a safeguard mutex which +assures priority access on all data objects + +This approach is tested and works both on Linux and BSD style Unix (MAC OS X) + +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* Scheduler includes. */ +#include "FreeRTOS.h" +#include "task.h" +/*-----------------------------------------------------------*/ + +#define MAX_NUMBER_OF_TASKS ( _POSIX_THREAD_THREADS_MAX ) +/*-----------------------------------------------------------*/ + +#define PORT_PRINT(...) fprintf(stderr,__VA_ARGS__) +#define PORT_ASSERT(assertion) if ( !(assertion) ) { PORT_PRINT("Assertion failed in %s:%i " #assertion "\n",__FILE__,__LINE__); int volatile assfail=0; assfail=assfail/assfail; } + + +#define PORT_LOCK(mutex) PORT_ASSERT( 0 == pthread_mutex_lock(&(mutex)) ) +#define PORT_TRYLOCK(mutex) pthread_mutex_trylock(&(mutex)) +#define PORT_UNLOCK(mutex) PORT_ASSERT( 0 == pthread_mutex_unlock(&(mutex)) ) + + +/* Parameters to pass to the newly created pthread. */ +typedef struct XPARAMS +{ + pdTASK_CODE pxCode; + void *pvParams; +} xParams; + +/* Each task maintains its own interrupt status in the critical nesting variable. */ +typedef struct THREAD_SUSPENSIONS +{ + pthread_t hThread; + xTaskHandle hTask; + unsigned portBASE_TYPE uxCriticalNesting; + pthread_mutex_t threadSleepMutex; + pthread_cond_t threadSleepCond; + volatile enum {THREAD_SLEEPING,THREAD_RUNNING,THREAD_STARTING,THREAD_YIELDING,THREAD_PREEMPTING,THREAD_WAKING} threadStatus; +} xThreadState; +/*-----------------------------------------------------------*/ + +/* Needed to keep track of critical section depth before scheduler got started */ +static xThreadState xDummyThread = { .uxCriticalNesting=0, .threadStatus=THREAD_RUNNING }; + +static xThreadState *pxThreads = NULL; +static pthread_once_t hSigSetupThread = PTHREAD_ONCE_INIT; +static pthread_attr_t xThreadAttributes; +static pthread_mutex_t xRunningThreadMutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_mutex_t xYieldingThreadMutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_mutex_t xResumingThreadMutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_mutex_t xGuardMutex = PTHREAD_MUTEX_INITIALIZER; +/*-----------------------------------------------------------*/ + +static volatile portBASE_TYPE xSchedulerEnd = pdFALSE; +static volatile portBASE_TYPE xSchedulerStarted = pdFALSE; +static volatile portBASE_TYPE xInterruptsEnabled = pdFALSE; +static volatile portBASE_TYPE xSchedulerNesting = 0; +static volatile portBASE_TYPE xPendYield = pdFALSE; +static volatile portLONG lIndexOfLastAddedTask = 0; +/*-----------------------------------------------------------*/ + +/* + * Setup the timer to generate the tick interrupts. + */ +static void *prvWaitForStart( void * pvParams ); +static void prvSuspendSignalHandler(int sig); +static void prvSetupSignalsAndSchedulerPolicy( void ); +static void prvResumeThread( xThreadState* xThreadId ); +static xThreadState* prvGetThreadHandle( xTaskHandle hTask ); +static xThreadState* prvGetThreadHandleByThread( pthread_t hThread ); +static portLONG prvGetFreeThreadState( void ); +static void prvDeleteThread( void *xThreadId ); +static void prvPortYield(); +/*-----------------------------------------------------------*/ + +/* + * Exception handlers. + */ +void vPortYield( void ); +void vPortSystemTickHandler( void ); + +/* + * Start first task is a separate function so it can be tested in isolation. + */ +void vPortStartFirstTask( void ); +/*-----------------------------------------------------------*/ + +/** + * inline macro functions + * (easierto debug than macros) + */ +static inline void PORT_ENTER() { + while( prvGetThreadHandleByThread(pthread_self())->threadStatus!=THREAD_RUNNING) sched_yield(); + PORT_LOCK( xGuardMutex ); + PORT_ASSERT( xSchedulerStarted?( prvGetThreadHandleByThread(pthread_self())==prvGetThreadHandle(xTaskGetCurrentTaskHandle()) ):pdTRUE ) +} + +static inline void PORT_LEAVE() { + PORT_ASSERT( xSchedulerStarted?( prvGetThreadHandleByThread(pthread_self())==prvGetThreadHandle(xTaskGetCurrentTaskHandle()) ):pdTRUE ); + PORT_ASSERT( prvGetThreadHandleByThread(pthread_self())->threadStatus==THREAD_RUNNING ); + PORT_UNLOCK( xGuardMutex ); +} + +/*-----------------------------------------------------------*/ + +/** + * Creates a new thread. + */ +portSTACK_TYPE *pxPortInitialiseStack( portSTACK_TYPE *pxTopOfStack, pdTASK_CODE pxCode, void *pvParameters ) +{ + /* Should actually keep this struct on the stack. */ + xParams *pxThisThreadParams = pvPortMalloc( sizeof( xParams ) ); + + /* Initialize scheduler during first call */ + (void)pthread_once( &hSigSetupThread, prvSetupSignalsAndSchedulerPolicy ); + + /** + * port enter needs to be delayed since pvPortMalloc() and + * SetupSignalsAndSchedulerPolicy() both call critical section code + */ + PORT_ENTER(); + + /* No need to join the threads. */ + pthread_attr_init( &xThreadAttributes ); + pthread_attr_setdetachstate( &xThreadAttributes, PTHREAD_CREATE_DETACHED ); + + /* Add the task parameters. */ + pxThisThreadParams->pxCode = pxCode; + pxThisThreadParams->pvParams = pvParameters; + + lIndexOfLastAddedTask = prvGetFreeThreadState(); + + PORT_LOCK( xYieldingThreadMutex ); + + pxThreads[ lIndexOfLastAddedTask ].threadStatus = THREAD_STARTING; + pxThreads[ lIndexOfLastAddedTask ].uxCriticalNesting = 0; + + /* create the thead */ + PORT_ASSERT( 0 == pthread_create( &( pxThreads[ lIndexOfLastAddedTask ].hThread ), &xThreadAttributes, prvWaitForStart, (void *)pxThisThreadParams ) ); + + /* Let the task run a bit and wait until it suspends. */ + while ( pxThreads[ lIndexOfLastAddedTask ].threadStatus == THREAD_STARTING ) sched_yield(); + + /* this ensures the sleeping thread reached deep sleep (and not more) */ + PORT_UNLOCK( xYieldingThreadMutex ); + + PORT_LEAVE(); + + return pxTopOfStack; +} +/*-----------------------------------------------------------*/ + +/** + * Initially the schedulers main thread holds the running thread mutex. + * it needs to be given up, to allow the first running task to execute + */ +void vPortStartFirstTask( void ) +{ + /* Mark scheduler as started */ + xSchedulerStarted = pdTRUE; + + /* Start the first task. */ + prvResumeThread( prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) ); + + /* Give up running thread handle */ + PORT_UNLOCK(xRunningThreadMutex); +} +/*-----------------------------------------------------------*/ + +/** + * After tasks have been set up the main thread goes into a sleeping loop, but + * allows to be interrupted by timer ticks. + */ +portBASE_TYPE xPortStartScheduler( void ) +{ + portBASE_TYPE xResult; + sigset_t xSignalToBlock; + + /** + * note: NO PORT_ENTER ! - this is the supervisor thread which runs outside + * of the schedulers context + */ + + /* do not respond to SUSPEND signal (but all others) */ + sigemptyset( &xSignalToBlock ); + (void)pthread_sigmask( SIG_SETMASK, &xSignalToBlock, NULL ); + sigemptyset(&xSignalToBlock); + sigaddset(&xSignalToBlock,SIG_SUSPEND); + (void)pthread_sigmask(SIG_BLOCK, &xSignalToBlock, NULL); + + /* Start the first task. This gives up the RunningThreadMutex*/ + vPortStartFirstTask(); + + /** + * Main scheduling loop. Call the tick handler every + * portTICK_RATE_MICROSECONDS + */ + portLONG sleepTimeUS = portTICK_RATE_MICROSECONDS; + portLONG actualSleepTime; + struct timeval lastTime,currentTime; + gettimeofday( &lastTime, NULL ); + struct timespec wait; + + while ( pdTRUE != xSchedulerEnd ) + { + /* wait for the specified wait time */ + wait.tv_sec = sleepTimeUS / 1000000; + wait.tv_nsec = 1000 * ( sleepTimeUS % 1000000 ); + nanosleep( &wait, NULL ); + + /* check the time */ + gettimeofday( ¤tTime, NULL); + actualSleepTime = 1000000 * ( currentTime.tv_sec - lastTime.tv_sec ) + ( currentTime.tv_usec - lastTime.tv_usec ); + + /* only hit the tick if we slept at least half the period */ + if ( actualSleepTime >= sleepTimeUS/2 ) { + + vPortSystemTickHandler(); + + /* check the time again */ + gettimeofday( ¤tTime, NULL); + actualSleepTime = 1000000 * ( currentTime.tv_sec - lastTime.tv_sec ) + ( currentTime.tv_usec - lastTime.tv_usec ); + + /* sleep until the next tick is due */ + sleepTimeUS += portTICK_RATE_MICROSECONDS; + } + + /* reduce remaining sleep time by the slept time */ + sleepTimeUS -= actualSleepTime; + lastTime = currentTime; + + /* safety checks */ + if (sleepTimeUS <=0 || sleepTimeUS >= 3 * portTICK_RATE_MICROSECONDS) sleepTimeUS = portTICK_RATE_MICROSECONDS; + + } + + PORT_PRINT( "Cleaning Up, Exiting.\n" ); + /* Cleanup the mutexes */ + xResult = pthread_mutex_destroy( &xRunningThreadMutex ); + xResult = pthread_mutex_destroy( &xYieldingThreadMutex ); + xResult = pthread_mutex_destroy( &xGuardMutex ); + vPortFree( (void *)pxThreads ); + + /* Should not get here! */ + return 0; +} +/*-----------------------------------------------------------*/ + +/** + * quickly clean up all running threads, without asking them first + */ +void vPortEndScheduler( void ) +{ +portBASE_TYPE xNumberOfThreads; +portBASE_TYPE xResult; + for ( xNumberOfThreads = 0; xNumberOfThreads < MAX_NUMBER_OF_TASKS; xNumberOfThreads++ ) + { + if ( ( pthread_t )NULL != pxThreads[ xNumberOfThreads ].hThread ) + { + /* Kill all of the threads, they are in the detached state. */ + xResult = pthread_cancel( pxThreads[ xNumberOfThreads ].hThread ); + } + } + + /* Signal the scheduler to exit its loop. */ + xSchedulerEnd = pdTRUE; +} +/*-----------------------------------------------------------*/ + +/** + * we must assume this one is called from outside the schedulers context + * (ISR's, signal handlers, or non-freertos threads) + * we cannot safely assume mutual exclusion + */ +void vPortYieldFromISR( void ) +{ + xPendYield = pdTRUE; +} +/*-----------------------------------------------------------*/ + +/** + * enter a critical section (public) + */ +void vPortEnterCritical( void ) +{ + PORT_ENTER(); + xInterruptsEnabled = pdFALSE; + prvGetThreadHandleByThread(pthread_self())->uxCriticalNesting++; + PORT_LEAVE(); +} +/*-----------------------------------------------------------*/ + +/** + * leave a critical section (public) + */ +void vPortExitCritical( void ) +{ + PORT_ENTER(); + + /* Check for unmatched exits. */ + PORT_ASSERT( prvGetThreadHandleByThread(pthread_self())->uxCriticalNesting > 0 ); + if ( prvGetThreadHandleByThread(pthread_self())->uxCriticalNesting > 0 ) + { + prvGetThreadHandleByThread(pthread_self())->uxCriticalNesting--; + } + + /* If we have reached 0 then re-enable the interrupts. */ + if( prvGetThreadHandleByThread(pthread_self())->uxCriticalNesting == 0 ) + { + /* Have we missed ticks? This is the equivalent of pending an interrupt. */ + if ( pdTRUE == xPendYield ) + { + xPendYield = pdFALSE; + prvPortYield(); + } + + xInterruptsEnabled = pdTRUE; + + } + + PORT_LEAVE(); +} +/*-----------------------------------------------------------*/ + +/** + * code to self-yield a task + * (without the mutex encapsulation) + * for internal use + */ +void prvPortYield() +{ + xThreadState *xTaskToSuspend, *xTaskToResume; + + /* timer handler should NOT get in our way (just in case) */ + xInterruptsEnabled = pdFALSE; + + /* suspend the current task */ + xTaskToSuspend = prvGetThreadHandleByThread( pthread_self() ); + + /** + * make sure not to suspend threads that are already trying to do so + */ + PORT_ASSERT( xTaskToSuspend->threadStatus == THREAD_RUNNING ); + + /** + * FreeRTOS switch context + */ + vTaskSwitchContext(); + + /** + * find out which task to resume + */ + xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() ); + if ( xTaskToSuspend != xTaskToResume ) + { + /* Resume the other thread first */ + prvResumeThread( xTaskToResume ); + + /* prepare the current task for yielding */ + xTaskToSuspend->threadStatus = THREAD_YIELDING; + + /** + * Send signals until the signal handler acknowledges. How long that takes + * depends on the systems signal implementation. During a preemption we + * will see the actual THREAD_SLEEPING STATE - but when yielding we + * would only see a future THREAD_RUNNING after having woken up - both is + * OK + */ + while ( xTaskToSuspend->threadStatus == THREAD_YIELDING ) { + pthread_kill( xTaskToSuspend->hThread, SIG_SUSPEND ); + sched_yield(); + } + + /** + * mark: once we reach this point, the task has already slept and awaken anew + */ + + } else { + /** + * no context switch - keep running + */ + if (xTaskToResume->uxCriticalNesting==0) { + xInterruptsEnabled = pdTRUE; + } + } + +} +/*-----------------------------------------------------------*/ + +/** + * public yield function - secure + */ +void vPortYield( void ) +{ + PORT_ENTER(); + + prvPortYield(); + + PORT_LEAVE(); +} +/*-----------------------------------------------------------*/ + +/** + * public function to disable interrupts + */ +void vPortDisableInterrupts( void ) +{ + PORT_ENTER(); + + xInterruptsEnabled = pdFALSE; + + PORT_LEAVE(); +} +/*-----------------------------------------------------------*/ + +/** + * public function to enable interrupts + */ +void vPortEnableInterrupts( void ) +{ + PORT_ENTER(); + + /** + * It is bad practice to enable interrupts explicitly while in a critical section + * most likely this is a bug - better prevent the userspace from being stupid + */ + PORT_ASSERT( prvGetThreadHandleByThread(pthread_self())->uxCriticalNesting == 0 ); + + xInterruptsEnabled = pdTRUE; + + PORT_LEAVE(); +} +/*-----------------------------------------------------------*/ + +/** + * set and clear interrupt masks are used by FreeRTOS to enter and leave critical sections + * with unknown nexting level - but we DO know the nesting level + */ +portBASE_TYPE xPortSetInterruptMask( void ) +{ + portBASE_TYPE xReturn; + + PORT_ENTER(); + + xReturn = xInterruptsEnabled; + + xInterruptsEnabled = pdFALSE; + + prvGetThreadHandleByThread(pthread_self())->uxCriticalNesting++; + + PORT_LEAVE(); + + return xReturn; +} +/*-----------------------------------------------------------*/ + +/** + * sets the "interrupt mask back to a stored setting + */ +void vPortClearInterruptMask( portBASE_TYPE xMask ) +{ + PORT_ENTER(); + + /** + * we better make sure the calling code behaves + * if it doesn't it might indicate something went seriously wrong + */ + PORT_ASSERT( xMask == pdTRUE || xMask == pdFALSE ); + PORT_ASSERT( + ( prvGetThreadHandleByThread(pthread_self())->uxCriticalNesting == 1 && xMask==pdTRUE ) + || + ( prvGetThreadHandleByThread(pthread_self())->uxCriticalNesting > 1 && xMask==pdFALSE ) + ); + + xInterruptsEnabled = xMask; + if (prvGetThreadHandleByThread(pthread_self())->uxCriticalNesting>0) { + prvGetThreadHandleByThread(pthread_self())->uxCriticalNesting--; + } + + PORT_LEAVE(); +} + +/*-----------------------------------------------------------*/ + +/** + * the tick handler is just an ordinary function, called by the supervisor thread periodically + */ +void vPortSystemTickHandler() +{ + /** + * the problem with the tick handler is, that it runs outside of the schedulers domain - worse, + * on a multi core machine there might be a task running *right now* + * - we need to stop it in order to do anything. However we need to make sure we are able to first + */ + PORT_LOCK( xGuardMutex ); + + /* thread MUST be running */ + if ( prvGetThreadHandle(xTaskGetCurrentTaskHandle())->threadStatus!=THREAD_RUNNING ) { + xPendYield = pdTRUE; + PORT_UNLOCK( xGuardMutex ); + return; + } + + /* interrupts MUST be enabled */ + if ( xInterruptsEnabled != pdTRUE ) { + xPendYield = pdTRUE; + PORT_UNLOCK( xGuardMutex ); + return; + } + + /* this should always be true, but it can't harm to check */ + PORT_ASSERT( prvGetThreadHandle(xTaskGetCurrentTaskHandle())->uxCriticalNesting==0 ); + + /* acquire switching mutex for synchronization */ + PORT_LOCK(xYieldingThreadMutex); + + xThreadState *xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() ); + + + /** + * halt current task - this means NO task is running! + * Send signals until the signal handler acknowledges. how long that takes + * depends on the systems signal implementation. During a preemption we + * will see the actual THREAD_SLEEPING STATE when yielding we would only + * see a future THREAD_RUNNING after having woken up both is OK + * note: we do NOT give up switchingThreadMutex! + */ + xTaskToSuspend->threadStatus = THREAD_PREEMPTING; + while ( xTaskToSuspend->threadStatus != THREAD_SLEEPING ) { + pthread_kill( xTaskToSuspend->hThread, SIG_SUSPEND ); + sched_yield(); + } + + /** + * synchronize and acquire the running thread mutex + */ + PORT_UNLOCK( xYieldingThreadMutex ); + PORT_LOCK( xRunningThreadMutex ); + + /** + * now the tick handler runs INSTEAD of the currently active thread + * - even on a multicore system + * failure to do so can lead to unexpected results during + * vTaskIncrementTick()... + */ + + /** + * call tick handler + */ + vTaskIncrementTick(); + + +#if ( configUSE_PREEMPTION == 1 ) + /** + * while we are here we can as well switch the running thread + */ + vTaskSwitchContext(); + + xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() ); +#endif + + /** + * wake up the task (again) + */ + prvResumeThread( xTaskToSuspend ); + + /** + * give control to the userspace task + */ + PORT_UNLOCK( xRunningThreadMutex ); + + /* finish up */ + PORT_UNLOCK( xGuardMutex ); +} +/*-----------------------------------------------------------*/ + +/** + * thread kill implementation + */ +void vPortForciblyEndThread( void *pxTaskToDelete ) +{ +xTaskHandle hTaskToDelete = ( xTaskHandle )pxTaskToDelete; +xThreadState* xTaskToDelete; +xThreadState* xTaskToResume; +portBASE_TYPE xResult; + + PORT_ENTER(); + + xTaskToDelete = prvGetThreadHandle( hTaskToDelete ); + xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() ); + + PORT_ASSERT( xTaskToDelete ); + PORT_ASSERT( xTaskToResume ); + + if ( xTaskToResume == xTaskToDelete ) + { + /* This is a suicidal thread, need to select a different task to run. */ + vTaskSwitchContext(); + xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() ); + } + + if ( pthread_self() != xTaskToDelete->hThread ) + { + /* Cancelling a thread that is not me. */ + + /* Send a signal to wake the task so that it definitely cancels. */ + pthread_testcancel(); + xResult = pthread_cancel( xTaskToDelete->hThread ); + + } + else + { + /* Resume the other thread. */ + prvResumeThread( xTaskToResume ); + /* Pthread Clean-up function will note the cancellation. */ + /* Release the execution. */ + + PORT_UNLOCK( xRunningThreadMutex ); + + //PORT_LEAVE(); + PORT_UNLOCK( xGuardMutex ); + /* Commit suicide */ + pthread_exit( (void *)1 ); + } + PORT_LEAVE(); +} +/*-----------------------------------------------------------*/ + +/** + * any new thread first acquires the runningThreadMutex, but then suspends + * immediately, giving control back to the thread starting the new one + */ +void *prvWaitForStart( void * pvParams ) +{ + xParams * pxParams = ( xParams * )pvParams; + pdTASK_CODE pvCode = pxParams->pxCode; + void * pParams = pxParams->pvParams; + sigset_t xSignalToBlock; + xThreadState * myself = prvGetThreadHandleByThread( pthread_self() ); + + pthread_cleanup_push( prvDeleteThread, (void *)pthread_self() ); + + /* do respond to signals */ + sigemptyset( &xSignalToBlock ); + (void)pthread_sigmask( SIG_SETMASK, &xSignalToBlock, NULL ); + + /** + * Suspend ourselves. It's important to do that first + * because until we come back from this we run outside the schedulers scope + * and can't call functions like vPortFree() safely + */ + while ( myself->threadStatus == THREAD_STARTING ) { + pthread_kill( myself->hThread, SIG_SUSPEND ); + sched_yield(); + } + + /** + * now we have returned from the dead - reborn as a real thread inside the + * schedulers scope. + */ + vPortFree( pvParams ); + + /* run the actual payload */ + pvCode( pParams ); + + pthread_cleanup_pop( 1 ); + return (void *)NULL; +} +/*-----------------------------------------------------------*/ + +/** + * The suspend signal handler is called when a thread gets a SIGSUSPEND + * signal, which is supposed to send it to sleep + */ +void prvSuspendSignalHandler(int sig) +{ + + portBASE_TYPE hangover; + + /* make sure who we are */ + xThreadState* myself = prvGetThreadHandleByThread(pthread_self()); + PORT_ASSERT( myself ); + + /* make sure we are actually supposed to sleep */ + if (myself->threadStatus != THREAD_YIELDING && myself->threadStatus != THREAD_STARTING && myself->threadStatus != THREAD_PREEMPTING ) { + /* Spurious signal has arrived, we are not really supposed to halt. + * Not a real problem, we can safely ignore that. */ + return; + } + + /* we need that to wake up later (cond_wait needs a mutex locked) */ + PORT_LOCK(myself->threadSleepMutex); + + /* even waking up is a bit different depending on how we went to sleep */ + hangover = myself->threadStatus; + + myself->threadStatus = THREAD_SLEEPING; + + if ( hangover == THREAD_STARTING ) { + /** + * Synchronization with spawning thread through YieldingMutex + * This thread does NOT have the running thread mutex + * because it never officially ran before. + * It will get that mutex on wakeup though. + */ + PORT_LOCK(xYieldingThreadMutex); + PORT_UNLOCK(xYieldingThreadMutex); + } else if ( hangover == THREAD_YIELDING) { + /** + * The caller is the same thread as the signal handler. + * No synchronization possible or needed. + * But we need to unlock the mutexes it holds, so + * other threads can run. + */ + PORT_UNLOCK(xRunningThreadMutex ); + PORT_UNLOCK(xGuardMutex ); + } else if ( hangover == THREAD_PREEMPTING) { + /** + * The caller is the tick handler. + * Use YieldingMutex for synchronization + * Give up RunningThreadMutex, so the tick handler + * can take it and start another thread. + */ + PORT_LOCK(xYieldingThreadMutex); + PORT_UNLOCK(xRunningThreadMutex ); + PORT_UNLOCK(xYieldingThreadMutex); + } + + /* deep sleep until wake condition is met*/ + pthread_cond_wait( &myself->threadSleepCond, &myself->threadSleepMutex ); + + /* waking */ + myself->threadStatus = THREAD_WAKING; + + /* synchronize with waker - quick assertion if the right thread got the condition sent to*/ + PORT_LOCK(xResumingThreadMutex); + PORT_ASSERT(prvGetThreadHandle( xTaskGetCurrentTaskHandle())==myself); + PORT_UNLOCK(xResumingThreadMutex); + + /* we don't need that condition mutex anymore */ + PORT_UNLOCK(myself->threadSleepMutex); + + /* we ARE the running thread now (the one and only) */ + PORT_LOCK(xRunningThreadMutex); + + /** + * and we have important stuff to do, nobody should interfere with + * ( GuardMutex is usually set by PORT_ENTER() ) + * */ + PORT_LOCK( xGuardMutex ); + if ( myself->uxCriticalNesting == 0 ) + { + xInterruptsEnabled = pdTRUE; + } + else + { + xInterruptsEnabled = pdFALSE; + } + + myself->threadStatus = THREAD_RUNNING; + + /** + * if we jump back to user code, we are done with important stuff, + * but if we had yielded we are still in protected code after returning. + **/ + if (hangover!=THREAD_YIELDING) { + PORT_UNLOCK( xGuardMutex ); + } + +} +/*-----------------------------------------------------------*/ + +/** + * Signal the condition. + * Unlike pthread_kill this actually is supposed to be reliable, so we need no + * checks on the outcome. + */ +void prvResumeThread( xThreadState* xThreadId ) +{ + PORT_ASSERT( xThreadId ); + + PORT_LOCK( xResumingThreadMutex ); + + PORT_ASSERT(xThreadId->threadStatus == THREAD_SLEEPING); + + /** + * Unfortunately "is supposed to" does not hold on all Posix-ish systems + * but sending the cond_signal again doesn't hurt anyone. + */ + while ( xThreadId->threadStatus != THREAD_WAKING ) { + pthread_cond_signal(& xThreadId->threadSleepCond); + sched_yield(); + } + + PORT_UNLOCK( xResumingThreadMutex ); + +} +/*-----------------------------------------------------------*/ + +/** + * this is init code executed the first time a thread is created + */ +void prvSetupSignalsAndSchedulerPolicy( void ) +{ +/* The following code would allow for configuring the scheduling of this task as a Real-time task. + * The process would then need to be run with higher privileges for it to take affect. +int iPolicy; +int iResult; +int iSchedulerPriority; + iResult = pthread_getschedparam( pthread_self(), &iPolicy, &iSchedulerPriority ); + iResult = pthread_attr_setschedpolicy( &xThreadAttributes, SCHED_FIFO ); + iPolicy = SCHED_FIFO; + iResult = pthread_setschedparam( pthread_self(), iPolicy, &iSchedulerPriority ); */ + +struct sigaction sigsuspendself; +portLONG lIndex; + + pxThreads = ( xThreadState *)pvPortMalloc( sizeof( xThreadState ) * MAX_NUMBER_OF_TASKS ); + + const pthread_cond_t cinit = PTHREAD_COND_INITIALIZER; + const pthread_mutex_t minit = PTHREAD_MUTEX_INITIALIZER; + for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) + { + pxThreads[ lIndex ].hThread = ( pthread_t )NULL; + pxThreads[ lIndex ].hTask = ( xTaskHandle )NULL; + pxThreads[ lIndex ].uxCriticalNesting = 0; + pxThreads[ lIndex ].threadSleepMutex = minit; + pxThreads[ lIndex ].threadSleepCond = cinit; + } + + sigsuspendself.sa_flags = 0; + sigsuspendself.sa_handler = prvSuspendSignalHandler; + sigfillset( &sigsuspendself.sa_mask ); + + if ( 0 != sigaction( SIG_SUSPEND, &sigsuspendself, NULL ) ) + { + PORT_PRINT( "Problem installing SIG_SUSPEND_SELF\n" ); + } + PORT_PRINT( "Running as PID: %d\n", getpid() ); + + /* When scheduler is set up main thread first claims the running thread mutex */ + PORT_LOCK( xRunningThreadMutex ); + +} +/*-----------------------------------------------------------*/ + +/** + * get a thread handle based on a task handle + */ +xThreadState* prvGetThreadHandle( xTaskHandle hTask ) +{ +portLONG lIndex; +if (!pxThreads) return NULL; + for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) + { + if ( pxThreads[ lIndex ].hTask == hTask ) + { + return &pxThreads[ lIndex ]; + break; + } + } + return NULL; +} +/*-----------------------------------------------------------*/ + +/** + * get a thread handle based on a posix thread handle + */ +xThreadState* prvGetThreadHandleByThread( pthread_t hThread ) +{ +portLONG lIndex; + /** + * if the scheduler is NOT yet started, we can give back a dummy thread handle + * to allow keeping track of interrupt nesting. + * However once the scheduler is started we return a NULL, + * so any misbehaving code can nicely segfault. + */ + if (!xSchedulerStarted && !pxThreads) return &xDummyThread; + if (!pxThreads) return NULL; + for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) + { + if ( pxThreads[ lIndex ].hThread == hThread ) + { + return &pxThreads[ lIndex ]; + } + } + if (!xSchedulerStarted) return &xDummyThread; + return NULL; +} +/*-----------------------------------------------------------*/ + +/* next free task handle */ +portLONG prvGetFreeThreadState( void ) +{ +portLONG lIndex; + for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) + { + if ( pxThreads[ lIndex ].hThread == ( pthread_t )NULL ) + { + break; + } + } + + if ( MAX_NUMBER_OF_TASKS == lIndex ) + { + PORT_PRINT( "No more free threads, please increase the maximum.\n" ); + lIndex = 0; + vPortEndScheduler(); + } + + return lIndex; +} + +/*-----------------------------------------------------------*/ + +/** + * delete a thread from the list + */ +void prvDeleteThread( void *xThreadId ) +{ +portLONG lIndex; + for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) + { + if ( pxThreads[ lIndex ].hThread == ( pthread_t )xThreadId ) + { + pxThreads[ lIndex ].hThread = (pthread_t)NULL; + pxThreads[ lIndex ].hTask = (xTaskHandle)NULL; + if ( pxThreads[ lIndex ].uxCriticalNesting > 0 ) + { + //vPortEnableInterrupts(); + xInterruptsEnabled = pdTRUE; + } + pxThreads[ lIndex ].uxCriticalNesting = 0; + break; + } + } +} +/*-----------------------------------------------------------*/ + +/** + * add a thread to the list + */ +void vPortAddTaskHandle( void *pxTaskHandle ) +{ +portLONG lIndex; + + pxThreads[ lIndexOfLastAddedTask ].hTask = ( xTaskHandle )pxTaskHandle; + for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) + { + if ( pxThreads[ lIndex ].hThread == pxThreads[ lIndexOfLastAddedTask ].hThread ) + { + if ( pxThreads[ lIndex ].hTask != pxThreads[ lIndexOfLastAddedTask ].hTask ) + { + pxThreads[ lIndex ].hThread = ( pthread_t )NULL; + pxThreads[ lIndex ].hTask = NULL; + pxThreads[ lIndex ].uxCriticalNesting = 0; + } + } + } +} +/*-----------------------------------------------------------*/ + +/** + * find out system speed + */ +void vPortFindTicksPerSecond( void ) +{ + /* Needs to be reasonably high for accuracy. */ + unsigned long ulTicksPerSecond = sysconf(_SC_CLK_TCK); + PORT_PRINT( "Timer Resolution for Run TimeStats is %ld ticks per second.\n", ulTicksPerSecond ); +} +/*-----------------------------------------------------------*/ + +/** + * timer stuff + */ +unsigned long ulPortGetTimerValue( void ) +{ +struct tms xTimes; + unsigned long ulTotalTime = times( &xTimes ); + /* Return the application code times. + * The timer only increases when the application code is actually running + * which means that the total execution times should add up to 100%. + */ + return ( unsigned long ) xTimes.tms_utime; + + /* Should check ulTotalTime for being clock_t max minus 1. */ + (void)ulTotalTime; +} +/*-----------------------------------------------------------*/ + diff --git a/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/port_linux.c b/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/port_linux.c deleted file mode 100644 index 639dd1d33..000000000 --- a/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/port_linux.c +++ /dev/null @@ -1,774 +0,0 @@ -/* - Copyright (C) 2009 William Davy - william.davy@wittenstein.co.uk - Contributed to FreeRTOS.org V5.3.0. - - This file is part of the FreeRTOS.org distribution. - - FreeRTOS.org is free software; you can redistribute it and/or modify it - under the terms of the GNU General Public License (version 2) as published - by the Free Software Foundation and modified by the FreeRTOS exception. - - FreeRTOS.org is distributed in the hope that it will be useful, but WITHOUT - ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or - FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for - more details. - - You should have received a copy of the GNU General Public License along - with FreeRTOS.org; if not, write to the Free Software Foundation, Inc., 59 - Temple Place, Suite 330, Boston, MA 02111-1307 USA. - - A special exception to the GPL is included to allow you to distribute a - combined work that includes FreeRTOS.org without being obliged to provide - the source code for any proprietary components. See the licensing section - of http://www.FreeRTOS.org for full details. - - - *************************************************************************** - * * - * Get the FreeRTOS eBook! See http://www.FreeRTOS.org/Documentation * - * * - * This is a concise, step by step, 'hands on' guide that describes both * - * general multitasking concepts and FreeRTOS specifics. It presents and * - * explains numerous examples that are written using the FreeRTOS API. * - * Full source code for all the examples is provided in an accompanying * - * .zip file. * - * * - *************************************************************************** - - 1 tab == 4 spaces! - - Please ensure to read the configuration and relevant port sections of the - online documentation. - - http://www.FreeRTOS.org - Documentation, latest information, license and - contact details. - - http://www.SafeRTOS.com - A version that is certified for use in safety - critical systems. - - http://www.OpenRTOS.com - Commercial support, development, porting, - licensing and training services. -*/ - -/*----------------------------------------------------------- - * Implementation of functions defined in portable.h for the Posix port. - *----------------------------------------------------------*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* Scheduler includes. */ -#include "FreeRTOS.h" -#include "task.h" -/*-----------------------------------------------------------*/ - -#define MAX_NUMBER_OF_TASKS ( _POSIX_THREAD_THREADS_MAX ) -/*-----------------------------------------------------------*/ - -/* Parameters to pass to the newly created pthread. */ -typedef struct XPARAMS -{ - pdTASK_CODE pxCode; - void *pvParams; -} xParams; - -/* Each task maintains its own interrupt status in the critical nesting variable. */ -typedef struct THREAD_SUSPENSIONS -{ - pthread_t hThread; - xTaskHandle hTask; - unsigned portBASE_TYPE uxCriticalNesting; -} xThreadState; -/*-----------------------------------------------------------*/ - -static xThreadState *pxThreads; -static pthread_once_t hSigSetupThread = PTHREAD_ONCE_INIT; -static pthread_attr_t xThreadAttributes; -static pthread_mutex_t xSuspendResumeThreadMutex = PTHREAD_MUTEX_INITIALIZER; -static pthread_mutex_t xSingleThreadMutex = PTHREAD_MUTEX_INITIALIZER; -static pthread_t hMainThread = ( pthread_t )NULL; -/*-----------------------------------------------------------*/ - -static volatile portBASE_TYPE xSentinel = 0; -static volatile portBASE_TYPE xSchedulerEnd = pdFALSE; -static volatile portBASE_TYPE xInterruptsEnabled = pdTRUE; -static volatile portBASE_TYPE xServicingTick = pdFALSE; -static volatile portBASE_TYPE xPendYield = pdFALSE; -static volatile portLONG lIndexOfLastAddedTask = 0; -static volatile unsigned portBASE_TYPE uxCriticalNesting; -/*-----------------------------------------------------------*/ - -/* - * Setup the timer to generate the tick interrupts. - */ -static void prvSetupTimerInterrupt( void ); -static void *prvWaitForStart( void * pvParams ); -static void prvSuspendSignalHandler(int sig); -static void prvResumeSignalHandler(int sig); -static void prvSetupSignalsAndSchedulerPolicy( void ); -static void prvSuspendThread( pthread_t xThreadId ); -static void prvResumeThread( pthread_t xThreadId ); -static pthread_t prvGetThreadHandle( xTaskHandle hTask ); -static portLONG prvGetFreeThreadState( void ); -static void prvSetTaskCriticalNesting( pthread_t xThreadId, unsigned portBASE_TYPE uxNesting ); -static unsigned portBASE_TYPE prvGetTaskCriticalNesting( pthread_t xThreadId ); -static void prvDeleteThread( void *xThreadId ); -/*-----------------------------------------------------------*/ - -/* - * Exception handlers. - */ -void vPortYield( void ); -void vPortSystemTickHandler( int sig ); - -/* - * Start first task is a separate function so it can be tested in isolation. - */ -void vPortStartFirstTask( void ); -/*-----------------------------------------------------------*/ - -/* - * See header file for description. - */ -portSTACK_TYPE *pxPortInitialiseStack( portSTACK_TYPE *pxTopOfStack, pdTASK_CODE pxCode, void *pvParameters ) -{ -/* Should actually keep this struct on the stack. */ -xParams *pxThisThreadParams = pvPortMalloc( sizeof( xParams ) ); - - (void)pthread_once( &hSigSetupThread, prvSetupSignalsAndSchedulerPolicy ); - - if ( (pthread_t)NULL == hMainThread ) - { - hMainThread = pthread_self(); - } - - /* No need to join the threads. */ - pthread_attr_init( &xThreadAttributes ); - pthread_attr_setdetachstate( &xThreadAttributes, PTHREAD_CREATE_DETACHED ); - - /* Add the task parameters. */ - pxThisThreadParams->pxCode = pxCode; - pxThisThreadParams->pvParams = pvParameters; - - vPortEnterCritical(); - - lIndexOfLastAddedTask = prvGetFreeThreadState(); - - /* Create the new pThread. */ - if ( 0 == pthread_mutex_lock( &xSingleThreadMutex ) ) - { - xSentinel = 0; - if ( 0 != pthread_create( &( pxThreads[ lIndexOfLastAddedTask ].hThread ), &xThreadAttributes, prvWaitForStart, (void *)pxThisThreadParams ) ) - { - /* Thread create failed, signal the failure */ - pxTopOfStack = 0; - } - - /* Wait until the task suspends. */ - (void)pthread_mutex_unlock( &xSingleThreadMutex ); - while ( xSentinel == 0 ); - vPortExitCritical(); - } - - return pxTopOfStack; -} -/*-----------------------------------------------------------*/ - -void vPortStartFirstTask( void ) -{ - /* Initialise the critical nesting count ready for the first task. */ - uxCriticalNesting = 0; - - /* Start the first task. */ - vPortEnableInterrupts(); - - /* Start the first task. */ - prvResumeThread( prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) ); -} -/*-----------------------------------------------------------*/ - -/* - * See header file for description. - */ -portBASE_TYPE xPortStartScheduler( void ) -{ -portBASE_TYPE xResult; -int iSignal; -sigset_t xSignals; -sigset_t xSignalToBlock; -sigset_t xSignalsBlocked; -portLONG lIndex; - - /* Establish the signals to block before they are needed. */ - sigfillset( &xSignalToBlock ); - - /* Block until the end */ - (void)pthread_sigmask( SIG_SETMASK, &xSignalToBlock, &xSignalsBlocked ); - - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - pxThreads[ lIndex ].uxCriticalNesting = 0; - } - - /* Start the timer that generates the tick ISR. Interrupts are disabled - here already. */ - prvSetupTimerInterrupt(); - - /* Start the first task. Will not return unless all threads are killed. */ - vPortStartFirstTask(); - - /* This is the end signal we are looking for. */ - sigemptyset( &xSignals ); - sigaddset( &xSignals, SIG_RESUME ); - - while ( pdTRUE != xSchedulerEnd ) - { - if ( 0 != sigwait( &xSignals, &iSignal ) ) - { - printf( "Main thread spurious signal: %d\n", iSignal ); - } - } - - printf( "Cleaning Up, Exiting.\n" ); - /* Cleanup the mutexes */ - xResult = pthread_mutex_destroy( &xSuspendResumeThreadMutex ); - xResult = pthread_mutex_destroy( &xSingleThreadMutex ); - vPortFree( (void *)pxThreads ); - - /* Should not get here! */ - return 0; -} -/*-----------------------------------------------------------*/ - -void vPortEndScheduler( void ) -{ -portBASE_TYPE xNumberOfThreads; -portBASE_TYPE xResult; - for ( xNumberOfThreads = 0; xNumberOfThreads < MAX_NUMBER_OF_TASKS; xNumberOfThreads++ ) - { - if ( ( pthread_t )NULL != pxThreads[ xNumberOfThreads ].hThread ) - { - /* Kill all of the threads, they are in the detached state. */ - xResult = pthread_cancel( pxThreads[ xNumberOfThreads ].hThread ); - } - } - - /* Signal the scheduler to exit its loop. */ - xSchedulerEnd = pdTRUE; - (void)pthread_kill( hMainThread, SIG_RESUME ); -} -/*-----------------------------------------------------------*/ - -void vPortYieldFromISR( void ) -{ - /* Calling Yield from a Interrupt/Signal handler often doesn't work because the - * xSingleThreadMutex is already owned by an original call to Yield. Therefore, - * simply indicate that a yield is required soon. - */ - xPendYield = pdTRUE; -} -/*-----------------------------------------------------------*/ - -void vPortEnterCritical( void ) -{ - vPortDisableInterrupts(); - uxCriticalNesting++; -} -/*-----------------------------------------------------------*/ - -void vPortExitCritical( void ) -{ - /* Check for unmatched exits. */ - if ( uxCriticalNesting > 0 ) - { - uxCriticalNesting--; - } - - /* If we have reached 0 then re-enable the interrupts. */ - if( uxCriticalNesting == 0 ) - { - /* Have we missed ticks? This is the equivalent of pending an interrupt. */ - if ( pdTRUE == xPendYield ) - { - xPendYield = pdFALSE; - vPortYield(); - } - vPortEnableInterrupts(); - } -} -/*-----------------------------------------------------------*/ - -void vPortYield( void ) -{ -pthread_t xTaskToSuspend; -pthread_t xTaskToResume; - - if ( 0 == pthread_mutex_lock( &xSingleThreadMutex ) ) - { - xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() ); - - vTaskSwitchContext(); - - xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() ); - if ( xTaskToSuspend != xTaskToResume ) - { - /* Remember and switch the critical nesting. */ - prvSetTaskCriticalNesting( xTaskToSuspend, uxCriticalNesting ); - uxCriticalNesting = prvGetTaskCriticalNesting( xTaskToResume ); - /* Switch tasks. */ - prvResumeThread( xTaskToResume ); - prvSuspendThread( xTaskToSuspend ); - } - else - { - /* Yielding to self */ - (void)pthread_mutex_unlock( &xSingleThreadMutex ); - } - } -} -/*-----------------------------------------------------------*/ - -void vPortDisableInterrupts( void ) -{ - xInterruptsEnabled = pdFALSE; -} -/*-----------------------------------------------------------*/ - -void vPortEnableInterrupts( void ) -{ - xInterruptsEnabled = pdTRUE; -} -/*-----------------------------------------------------------*/ - -portBASE_TYPE xPortSetInterruptMask( void ) -{ -portBASE_TYPE xReturn = xInterruptsEnabled; - xInterruptsEnabled = pdFALSE; - return xReturn; -} -/*-----------------------------------------------------------*/ - -void vPortClearInterruptMask( portBASE_TYPE xMask ) -{ - xInterruptsEnabled = xMask; -} -/*-----------------------------------------------------------*/ - -/* - * Setup the systick timer to generate the tick interrupts at the required - * frequency. - */ -void prvSetupTimerInterrupt( void ) -{ -struct itimerval itimer, oitimer; -portTickType xMicroSeconds = portTICK_RATE_MICROSECONDS; - - /* Initialise the structure with the current timer information. */ - if ( 0 == getitimer( TIMER_TYPE, &itimer ) ) - { - /* Set the interval between timer events. */ - itimer.it_interval.tv_sec = 0; - itimer.it_interval.tv_usec = xMicroSeconds; - - /* Set the current count-down. */ - itimer.it_value.tv_sec = 0; - itimer.it_value.tv_usec = xMicroSeconds; - - /* Set-up the timer interrupt. */ - if ( 0 != setitimer( TIMER_TYPE, &itimer, &oitimer ) ) - { - printf( "Set Timer problem.\n" ); - } - } - else - { - printf( "Get Timer problem.\n" ); - } -} -/*-----------------------------------------------------------*/ - -void vPortSystemTickHandler( int sig ) -{ -pthread_t xTaskToSuspend; -pthread_t xTaskToResume; - - if ( ( pdTRUE == xInterruptsEnabled ) && ( pdTRUE != xServicingTick ) ) - { - if ( 0 == pthread_mutex_trylock( &xSingleThreadMutex ) ) - { - xServicingTick = pdTRUE; - - xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() ); - /* Tick Increment. */ - vTaskIncrementTick(); - - /* Select Next Task. */ -#if ( configUSE_PREEMPTION == 1 ) - vTaskSwitchContext(); -#endif - xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() ); - - /* The only thread that can process this tick is the running thread. */ - if ( xTaskToSuspend != xTaskToResume ) - { - /* Remember and switch the critical nesting. */ - prvSetTaskCriticalNesting( xTaskToSuspend, uxCriticalNesting ); - uxCriticalNesting = prvGetTaskCriticalNesting( xTaskToResume ); - /* Resume next task. */ - prvResumeThread( xTaskToResume ); - /* Suspend the current task. */ - prvSuspendThread( xTaskToSuspend ); - } - else - { - /* Release the lock as we are Resuming. */ - (void)pthread_mutex_unlock( &xSingleThreadMutex ); - } - xServicingTick = pdFALSE; - } - else - { - xPendYield = pdTRUE; - } - } - else - { - xPendYield = pdTRUE; - } -} -/*-----------------------------------------------------------*/ - -void vPortForciblyEndThread( void *pxTaskToDelete ) -{ -xTaskHandle hTaskToDelete = ( xTaskHandle )pxTaskToDelete; -pthread_t xTaskToDelete; -pthread_t xTaskToResume; -portBASE_TYPE xResult; - - if ( 0 == pthread_mutex_lock( &xSingleThreadMutex ) ) - { - xTaskToDelete = prvGetThreadHandle( hTaskToDelete ); - xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() ); - - if ( xTaskToResume == xTaskToDelete ) - { - /* This is a suicidal thread, need to select a different task to run. */ - vTaskSwitchContext(); - xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() ); - } - - if ( pthread_self() != xTaskToDelete ) - { - /* Cancelling a thread that is not me. */ - if ( xTaskToDelete != ( pthread_t )NULL ) - { - /* Send a signal to wake the task so that it definitely cancels. */ - pthread_testcancel(); - xResult = pthread_cancel( xTaskToDelete ); - /* Pthread Clean-up function will note the cancellation. */ - } - (void)pthread_mutex_unlock( &xSingleThreadMutex ); - } - else - { - /* Resume the other thread. */ - prvResumeThread( xTaskToResume ); - /* Pthread Clean-up function will note the cancellation. */ - /* Release the execution. */ - uxCriticalNesting = 0; - vPortEnableInterrupts(); - (void)pthread_mutex_unlock( &xSingleThreadMutex ); - /* Commit suicide */ - pthread_exit( (void *)1 ); - } - } -} -/*-----------------------------------------------------------*/ - -void *prvWaitForStart( void * pvParams ) -{ -xParams * pxParams = ( xParams * )pvParams; -pdTASK_CODE pvCode = pxParams->pxCode; -void * pParams = pxParams->pvParams; - vPortFree( pvParams ); - - pthread_cleanup_push( prvDeleteThread, (void *)pthread_self() ); - - if ( 0 == pthread_mutex_lock( &xSingleThreadMutex ) ) - { - prvSuspendThread( pthread_self() ); - } - - pvCode( pParams ); - - pthread_cleanup_pop( 1 ); - return (void *)NULL; -} -/*-----------------------------------------------------------*/ - -void prvSuspendSignalHandler(int sig) -{ -sigset_t xSignals; - - /* Only interested in the resume signal. */ - sigemptyset( &xSignals ); - sigaddset( &xSignals, SIG_RESUME ); - xSentinel = 1; - - /* Unlock the Single thread mutex to allow the resumed task to continue. */ - if ( 0 != pthread_mutex_unlock( &xSingleThreadMutex ) ) - { - printf( "Releasing someone else's lock.\n" ); - } - - /* Wait on the resume signal. */ - if ( 0 != sigwait( &xSignals, &sig ) ) - { - printf( "SSH: Sw %d\n", sig ); - } - - /* Will resume here when the SIG_RESUME signal is received. */ - /* Need to set the interrupts based on the task's critical nesting. */ - if ( uxCriticalNesting == 0 ) - { - vPortEnableInterrupts(); - } - else - { - vPortDisableInterrupts(); - } -} -/*-----------------------------------------------------------*/ - -void prvSuspendThread( pthread_t xThreadId ) -{ -portBASE_TYPE xResult = pthread_mutex_lock( &xSuspendResumeThreadMutex ); - if ( 0 == xResult ) - { - /* Set-up for the Suspend Signal handler? */ - xSentinel = 0; - xResult = pthread_mutex_unlock( &xSuspendResumeThreadMutex ); - xResult = pthread_kill( xThreadId, SIG_SUSPEND ); - while ( ( xSentinel == 0 ) && ( pdTRUE != xServicingTick ) ) - { - sched_yield(); - } - } -} -/*-----------------------------------------------------------*/ - -void prvResumeSignalHandler(int sig) -{ - /* Yield the Scheduler to ensure that the yielding thread completes. */ - if ( 0 == pthread_mutex_lock( &xSingleThreadMutex ) ) - { - (void)pthread_mutex_unlock( &xSingleThreadMutex ); - } -} -/*-----------------------------------------------------------*/ - -void prvResumeThread( pthread_t xThreadId ) -{ -portBASE_TYPE xResult; - if ( 0 == pthread_mutex_lock( &xSuspendResumeThreadMutex ) ) - { - if ( pthread_self() != xThreadId ) - { - xResult = pthread_kill( xThreadId, SIG_RESUME ); - } - xResult = pthread_mutex_unlock( &xSuspendResumeThreadMutex ); - } -} -/*-----------------------------------------------------------*/ - -void prvSetupSignalsAndSchedulerPolicy( void ) -{ -/* The following code would allow for configuring the scheduling of this task as a Real-time task. - * The process would then need to be run with higher privileges for it to take affect. -int iPolicy; -int iResult; -int iSchedulerPriority; - iResult = pthread_getschedparam( pthread_self(), &iPolicy, &iSchedulerPriority ); - iResult = pthread_attr_setschedpolicy( &xThreadAttributes, SCHED_FIFO ); - iPolicy = SCHED_FIFO; - iResult = pthread_setschedparam( pthread_self(), iPolicy, &iSchedulerPriority ); */ - -struct sigaction sigsuspendself, sigresume, sigtick; -portLONG lIndex; - - pxThreads = ( xThreadState *)pvPortMalloc( sizeof( xThreadState ) * MAX_NUMBER_OF_TASKS ); - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - pxThreads[ lIndex ].hThread = ( pthread_t )NULL; - pxThreads[ lIndex ].hTask = ( xTaskHandle )NULL; - pxThreads[ lIndex ].uxCriticalNesting = 0; - } - - sigsuspendself.sa_flags = 0; - sigsuspendself.sa_handler = prvSuspendSignalHandler; - sigfillset( &sigsuspendself.sa_mask ); - - sigresume.sa_flags = 0; - sigresume.sa_handler = prvResumeSignalHandler; - sigfillset( &sigresume.sa_mask ); - - sigtick.sa_flags = 0; - sigtick.sa_handler = vPortSystemTickHandler; - sigfillset( &sigtick.sa_mask ); - - if ( 0 != sigaction( SIG_SUSPEND, &sigsuspendself, NULL ) ) - { - printf( "Problem installing SIG_SUSPEND_SELF\n" ); - } - if ( 0 != sigaction( SIG_RESUME, &sigresume, NULL ) ) - { - printf( "Problem installing SIG_RESUME\n" ); - } - if ( 0 != sigaction( SIG_TICK, &sigtick, NULL ) ) - { - printf( "Problem installing SIG_TICK\n" ); - } - printf( "Running as PID: %d\n", getpid() ); -} -/*-----------------------------------------------------------*/ - -pthread_t prvGetThreadHandle( xTaskHandle hTask ) -{ -pthread_t hThread = ( pthread_t )NULL; -portLONG lIndex; - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - if ( pxThreads[ lIndex ].hTask == hTask ) - { - hThread = pxThreads[ lIndex ].hThread; - break; - } - } - return hThread; -} -/*-----------------------------------------------------------*/ - -portLONG prvGetFreeThreadState( void ) -{ -portLONG lIndex; - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - if ( pxThreads[ lIndex ].hThread == ( pthread_t )NULL ) - { - break; - } - } - - if ( MAX_NUMBER_OF_TASKS == lIndex ) - { - printf( "No more free threads, please increase the maximum.\n" ); - lIndex = 0; - vPortEndScheduler(); - } - - return lIndex; -} -/*-----------------------------------------------------------*/ - -void prvSetTaskCriticalNesting( pthread_t xThreadId, unsigned portBASE_TYPE uxNesting ) -{ -portLONG lIndex; - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - if ( pxThreads[ lIndex ].hThread == xThreadId ) - { - pxThreads[ lIndex ].uxCriticalNesting = uxNesting; - break; - } - } -} -/*-----------------------------------------------------------*/ - -unsigned portBASE_TYPE prvGetTaskCriticalNesting( pthread_t xThreadId ) -{ -unsigned portBASE_TYPE uxNesting = 0; -portLONG lIndex; - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - if ( pxThreads[ lIndex ].hThread == xThreadId ) - { - uxNesting = pxThreads[ lIndex ].uxCriticalNesting; - break; - } - } - return uxNesting; -} -/*-----------------------------------------------------------*/ - -void prvDeleteThread( void *xThreadId ) -{ -portLONG lIndex; - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - if ( pxThreads[ lIndex ].hThread == ( pthread_t )xThreadId ) - { - pxThreads[ lIndex ].hThread = (pthread_t)NULL; - pxThreads[ lIndex ].hTask = (xTaskHandle)NULL; - if ( pxThreads[ lIndex ].uxCriticalNesting > 0 ) - { - uxCriticalNesting = 0; - vPortEnableInterrupts(); - } - pxThreads[ lIndex ].uxCriticalNesting = 0; - break; - } - } -} -/*-----------------------------------------------------------*/ - -void vPortAddTaskHandle( void *pxTaskHandle ) -{ -portLONG lIndex; - - pxThreads[ lIndexOfLastAddedTask ].hTask = ( xTaskHandle )pxTaskHandle; - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - if ( pxThreads[ lIndex ].hThread == pxThreads[ lIndexOfLastAddedTask ].hThread ) - { - if ( pxThreads[ lIndex ].hTask != pxThreads[ lIndexOfLastAddedTask ].hTask ) - { - pxThreads[ lIndex ].hThread = ( pthread_t )NULL; - pxThreads[ lIndex ].hTask = NULL; - pxThreads[ lIndex ].uxCriticalNesting = 0; - } - } - } -} -/*-----------------------------------------------------------*/ - -void vPortFindTicksPerSecond( void ) -{ - /* Needs to be reasonably high for accuracy. */ - unsigned long ulTicksPerSecond = sysconf(_SC_CLK_TCK); - printf( "Timer Resolution for Run TimeStats is %ld ticks per second.\n", ulTicksPerSecond ); -} -/*-----------------------------------------------------------*/ - -unsigned long ulPortGetTimerValue( void ) -{ -struct tms xTimes; - unsigned long ulTotalTime = times( &xTimes ); - /* Return the application code times. - * The timer only increases when the application code is actually running - * which means that the total execution times should add up to 100%. - */ - return ( unsigned long ) xTimes.tms_utime; - - /* Should check ulTotalTime for being clock_t max minus 1. */ - (void)ulTotalTime; -} -/*-----------------------------------------------------------*/ diff --git a/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/port_posix.c b/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/port_posix.c deleted file mode 100644 index 5dbbc3a40..000000000 --- a/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/port_posix.c +++ /dev/null @@ -1,1259 +0,0 @@ -/* - Copyright (C) 2009 William Davy - william.davy@wittenstein.co.uk - Contributed to FreeRTOS.org V5.3.0. - - This file is part of the FreeRTOS.org distribution. - - FreeRTOS.org is free software; you can redistribute it and/or modify it - under the terms of the GNU General Public License (version 2) as published - by the Free Software Foundation and modified by the FreeRTOS exception. - - FreeRTOS.org is distributed in the hope that it will be useful, but WITHOUT - ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or - FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for - more details. - - You should have received a copy of the GNU General Public License along - with FreeRTOS.org; if not, write to the Free Software Foundation, Inc., 59 - Temple Place, Suite 330, Boston, MA 02111-1307 USA. - - A special exception to the GPL is included to allow you to distribute a - combined work that includes FreeRTOS.org without being obliged to provide - the source code for any proprietary components. See the licensing section - of http://www.FreeRTOS.org for full details. - - - *************************************************************************** - * * - * Get the FreeRTOS eBook! See http://www.FreeRTOS.org/Documentation * - * * - * This is a concise, step by step, 'hands on' guide that describes both * - * general multitasking concepts and FreeRTOS specifics. It presents and * - * explains numerous examples that are written using the FreeRTOS API. * - * Full source code for all the examples is provided in an accompanying * - * .zip file. * - * * - *************************************************************************** - - 1 tab == 4 spaces! - - Please ensure to read the configuration and relevant port sections of the - online documentation. - - http://www.FreeRTOS.org - Documentation, latest information, license and - contact details. - - http://www.SafeRTOS.com - A version that is certified for use in safety - critical systems. - - http://www.OpenRTOS.com - Commercial support, development, porting, - licensing and training services. -*/ - -/*----------------------------------------------------------- - * Implementation of functions defined in portable.h for the Posix port. - *----------------------------------------------------------*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -/* Scheduler includes. */ -#include "FreeRTOS.h" -#include "task.h" -/*-----------------------------------------------------------*/ - -#define MAX_NUMBER_OF_TASKS ( _POSIX_THREAD_THREADS_MAX ) -/*-----------------------------------------------------------*/ - -/* Parameters to pass to the newly created pthread. */ -typedef struct XPARAMS -{ - pdTASK_CODE pxCode; - void *pvParams; -} xParams; - -/* Each task maintains its own interrupt status in the critical nesting variable. */ -typedef struct THREAD_SUSPENSIONS -{ - pthread_t hThread; - pthread_cond_t * hCond; - pthread_mutex_t * hMutex; - xTaskHandle hTask; - portBASE_TYPE xThreadState; - unsigned portBASE_TYPE uxCriticalNesting; -} xThreadState; -/*-----------------------------------------------------------*/ - -static xThreadState *pxThreads; -static pthread_once_t hSigSetupThread = PTHREAD_ONCE_INIT; -static pthread_attr_t xThreadAttributes; -#ifdef RUNNING_THREAD_MUTEX -static pthread_mutex_t xRunningThread = PTHREAD_MUTEX_INITIALIZER; -#endif -static pthread_mutex_t xSuspendResumeThreadMutex = PTHREAD_MUTEX_INITIALIZER; -static pthread_mutex_t xSwappingThreadMutex = PTHREAD_MUTEX_INITIALIZER; -static pthread_t hMainThread = ( pthread_t )NULL; -/*-----------------------------------------------------------*/ - -static volatile portBASE_TYPE xSentinel = pdFALSE; -static volatile portBASE_TYPE xRunning = pdFALSE; -static volatile portBASE_TYPE xSchedulerEnd = pdFALSE; -static volatile portBASE_TYPE xInterruptsEnabled = pdTRUE; -static volatile portBASE_TYPE xServicingTick = pdFALSE; -static volatile portBASE_TYPE xPendYield = pdFALSE; -static volatile portLONG lIndexOfLastAddedTask = 0; -static volatile unsigned portBASE_TYPE uxCriticalNesting; -/*-----------------------------------------------------------*/ - -/* - * Setup the timer to generate the tick interrupts. - */ -static void *prvWaitForStart( void * pvParams ); -static void prvSuspendSignalHandler(int sig); -static void prvSetupSignalsAndSchedulerPolicy( void ); -static void pauseThread( portBASE_TYPE pauseMode ); -static pthread_t prvGetThreadHandle( xTaskHandle hTask ); -#ifdef COND_SIGNALING -static pthread_cond_t * prvGetConditionHandle( xTaskHandle hTask ); -static pthread_mutex_t * prvGetMutexHandle( xTaskHandle hTask ); -#endif -#ifdef CHECK_TASK_RESUMES -static portBASE_TYPE prvGetTaskState( xTaskHandle hTask ); -#endif -static void prvSetTaskState( xTaskHandle hTask, portBASE_TYPE state ); -static xTaskHandle prvGetTaskHandle( pthread_t hThread ); -static portLONG prvGetFreeThreadState( void ); -static void prvSetTaskCriticalNesting( pthread_t xThreadId, unsigned portBASE_TYPE uxNesting ); -static unsigned portBASE_TYPE prvGetTaskCriticalNesting( pthread_t xThreadId ); -static void prvDeleteThread( void *xThreadId ); -/*-----------------------------------------------------------*/ - -/* - * Exception handlers. - */ -void vPortYield( void ); -void vPortSystemTickHandler( int sig ); - -#define THREAD_PAUSE_CREATED 0 -#define THREAD_PAUSE_YIELD 1 -#define THREAD_PAUSE_INTERRUPT 2 - -#define THREAD_STATE_PAUSE 1 -#define THREAD_STATE_RUNNING 2 - -//#define DEBUG_OUTPUT -//#define ERROR_OUTPUT -#ifdef DEBUG_OUTPUT - - static pthread_mutex_t xPrintfMutex = PTHREAD_MUTEX_INITIALIZER; - - #define debug_printf(...) ( (real_pthread_mutex_lock( &xPrintfMutex )|1)?( \ - ( \ - (NULL != (debug_task_handle = prvGetTaskHandle(pthread_self())) )? \ - (fprintf( stderr, "%20s(%li)\t%20s\t%i: ",debug_task_handle->pcTaskName,(long)pthread_self(),__func__,__LINE__)): \ - (fprintf( stderr, "%20s(%li)\t%20s\t%i: ","__unknown__",(long)pthread_self(),__func__,__LINE__)) \ - |1)?( \ - ((fprintf( stderr, __VA_ARGS__ )|1)?real_pthread_mutex_unlock( &xPrintfMutex ):0) \ - ):0 ):0 ) - - #define debug_error debug_printf - - int real_pthread_mutex_lock(pthread_mutex_t* mutex) { - return pthread_mutex_lock(mutex); - } - int real_pthread_mutex_unlock(pthread_mutex_t* mutex) { - return pthread_mutex_unlock(mutex); - } - #define pthread_mutex_trylock(...) ( (debug_printf(" -!- pthread_mutex_trylock(%s)\n",#__VA_ARGS__)|1)?pthread_mutex_trylock(__VA_ARGS__):0 ) - #define pthread_mutex_lock(...) ( (debug_printf(" -!- pthread_mutex_lock(%s)\n",#__VA_ARGS__)|1)?pthread_mutex_lock(__VA_ARGS__):0 ) - #define pthread_mutex_unlock(...) ( (debug_printf(" -=- pthread_mutex_unlock(%s)\n",#__VA_ARGS__)|1)?pthread_mutex_unlock(__VA_ARGS__):0 ) - #define pthread_kill(thread,signal) ( (debug_printf("Sending signal %i to thread %li!\n",(int)signal,(long)thread)|1)?pthread_kill(thread,signal):0 ) - #define pthread_cond_signal( hCond ) (debug_printf( "pthread_cond_signals(%li)\r\n", *((long int *) hCond) ) ? 1 : pthread_cond_signal( hCond ) ) - #define pthread_cond_timedwait( hCond, hMutex, it ) (debug_printf( "pthread_cond_timedwait(%li,%li)\r\n", *((long int *) hCond), *((long int *) hMutex )) ? 1 : pthread_cond_timedwait( hCond, hMutex, it ) ) - #define pthread_sigmask( how, set, out ) (debug_printf( "pthread_sigmask( %i, %li )\r\n", how, *((long int*) set) ) ? 1 : pthread_sigmask( how, set, out ) ) - -#else - #ifdef ERROR_OUTPUT - static pthread_mutex_t xPrintfMutex = PTHREAD_MUTEX_INITIALIZER; - #define debug_error(...) ( (pthread_mutex_lock( &xPrintfMutex )|1)?( \ - ( \ - (NULL != (debug_task_handle = prvGetTaskHandle(pthread_self())) )? \ - (fprintf( stderr, "%20s(%li)\t%20s\t%i: ",debug_task_handle->pcTaskName,(long)pthread_self(),__func__,__LINE__)): \ - (fprintf( stderr, "%20s(%li)\t%20s\t%i: ","__unknown__",(long)pthread_self(),__func__,__LINE__)) \ - |1)?( \ - ((fprintf( stderr, __VA_ARGS__ )|1)?pthread_mutex_unlock( &xPrintfMutex ):0) \ - ):0 ):0 ) - - #define debug_printf(...) - #else - #define debug_printf(...) - #define debug_error(...) - #endif -#endif - - -/* - * Start first task is a separate function so it can be tested in isolation. - */ -void vPortStartFirstTask( void ); -/*-----------------------------------------------------------*/ - - -typedef struct tskTaskControlBlock -{ - volatile portSTACK_TYPE *pxTopOfStack; /*< Points to the location of the last item placed on the tasks stack. THIS MUST BE THE FIRST MEMBER OF THE STRUCT. */ - -#if ( portUSING_MPU_WRAPPERS == 1 ) - xMPU_SETTINGS xMPUSettings; /*< The MPU settings are defined as part of the port layer. THIS MUST BE THE SECOND MEMBER OF THE STRUCT. */ -#endif - - xListItem xGenericListItem; /*< List item used to place the TCB in ready and blocked queues. */ - xListItem xEventListItem; /*< List item used to place the TCB in event lists. */ - unsigned portBASE_TYPE uxPriority; /*< The priority of the task where 0 is the lowest priority. */ - portSTACK_TYPE *pxStack; /*< Points to the start of the stack. */ - signed char pcTaskName[ configMAX_TASK_NAME_LEN ];/*< Descriptive name given to the task when created. Facilitates debugging only. */ - -#if ( portSTACK_GROWTH > 0 ) - portSTACK_TYPE *pxEndOfStack; /*< Used for stack overflow checking on architectures where the stack grows up from low memory. */ -#endif - -#if ( portCRITICAL_NESTING_IN_TCB == 1 ) - unsigned portBASE_TYPE uxCriticalNesting; -#endif - -#if ( configUSE_TRACE_FACILITY == 1 ) - unsigned portBASE_TYPE uxTCBNumber; /*< This is used for tracing the scheduler and making debugging easier only. */ -#endif - -#if ( configUSE_MUTEXES == 1 ) - unsigned portBASE_TYPE uxBasePriority; /*< The priority last assigned to the task - used by the priority inheritance mechanism. */ -#endif - -#if ( configUSE_APPLICATION_TASK_TAG == 1 ) - pdTASK_HOOK_CODE pxTaskTag; -#endif - -#if ( configGENERATE_RUN_TIME_STATS == 1 ) - unsigned long ulRunTimeCounter; /*< Used for calculating how much CPU time each task is utilising. */ -#endif - -} tskTCB; - -tskTCB *debug_task_handle; - -/* - * See header file for description. - */ -portSTACK_TYPE *pxPortInitialiseStack( portSTACK_TYPE *pxTopOfStack, pdTASK_CODE pxCode, void *pvParameters ) -{ -/* Should actually keep this struct on the stack. */ -xParams *pxThisThreadParams = pvPortMalloc( sizeof( xParams ) ); - - debug_printf("pxPortInitialiseStack\r\n"); - - (void)pthread_once( &hSigSetupThread, prvSetupSignalsAndSchedulerPolicy ); - - if ( (pthread_t)NULL == hMainThread ) - { - hMainThread = pthread_self(); - } - - /* No need to join the threads. */ - pthread_attr_init( &xThreadAttributes ); - pthread_attr_setdetachstate( &xThreadAttributes, PTHREAD_CREATE_DETACHED ); - - /* Add the task parameters. */ - pxThisThreadParams->pxCode = pxCode; - pxThisThreadParams->pvParams = pvParameters; - - vPortEnterCritical(); - - lIndexOfLastAddedTask = prvGetFreeThreadState(); - - debug_printf( "Got index for new task %i\r\n", lIndexOfLastAddedTask ); - -#ifdef COND_SIGNALING - /* Create a condition signal for this thread */ -// pthread_condattr_t condAttr; -// assert( 0 == pthread_condattr_init( &condAttr ) ); - pxThreads[ lIndexOfLastAddedTask ].hCond = ( pthread_cond_t *) malloc( sizeof( pthread_cond_t ) ); - assert( 0 == pthread_cond_init( pxThreads[ lIndexOfLastAddedTask ].hCond , NULL ) ); //&condAttr ) ); - debug_printf("Cond: %li\r\n", *( (long int *) &pxThreads[ lIndexOfLastAddedTask ].hCond) ); - - /* Create a condition mutex for this thread */ -// pthread_mutexattr_t mutexAttr; -// assert( 0 == pthread_mutexattr_init( &mutexAttr ) ); -// assert( 0 == pthread_mutexattr_settype( &mutexAttr, PTHREAD_MUTEX_ERRORCHECK ) ); - pxThreads[ lIndexOfLastAddedTask ].hMutex = ( pthread_mutex_t *) malloc( sizeof( pthread_mutex_t ) ); - assert( 0 == pthread_mutex_init( pxThreads[ lIndexOfLastAddedTask ].hMutex, NULL ) ); //&mutexAttr ) ); - debug_printf("Mutex: %li\r\n", *( (long int *) &pxThreads[ lIndexOfLastAddedTask ].hMutex) ); -#endif - - /* Create a thread and store it's handle number */ - xSentinel = 0; - assert( 0 == pthread_create( &( pxThreads[ lIndexOfLastAddedTask ].hThread ), &xThreadAttributes, prvWaitForStart, (void *)pxThisThreadParams ) ); - - /* Wait until the task suspends. */ - while ( xSentinel == 0 ); - vPortExitCritical(); - - return pxTopOfStack; -} -/*-----------------------------------------------------------*/ - -void vPortStartFirstTask( void ) -{ - /* Initialise the critical nesting count ready for the first task. */ - uxCriticalNesting = 0; - - debug_printf("vPortStartFirstTask\r\n"); - - /* Start the first task. */ - vPortEnableInterrupts(); - xRunning = 1; - - /* Start the first task. */ -#ifdef COND_SIGNALING - pthread_cond_t * hCond = prvGetConditionHandle( xTaskGetCurrentTaskHandle() ); -// careful! race condition? if u mutex lock here, could u start the tick handler more early? - assert( pthread_cond_signal( hCond ) == 0 ); -#endif -} -/*-----------------------------------------------------------*/ - -/* - * See header file for description. - */ -portBASE_TYPE xPortStartScheduler( void ) -{ -portBASE_TYPE xResult; -sigset_t xSignalToBlock; -portLONG lIndex; - - debug_printf( "xPortStartScheduler\r\n" ); - - /* Establish the signals to block before they are needed. */ - sigemptyset( &xSignalToBlock ); - sigaddset( &xSignalToBlock, SIG_SUSPEND ); - sigaddset( &xSignalToBlock, SIG_TICK ); - (void)pthread_sigmask( SIG_SETMASK, &xSignalToBlock, NULL ); - - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - pxThreads[ lIndex ].uxCriticalNesting = 0; - } - - /* Start the first task. Will not return unless all threads are killed. */ - vPortStartFirstTask(); - - /* Unfortunately things are stable if we start ticking during setup. This need to be */ - /* checked careful in startup on hardware */ - usleep(1000000); - -#if defined(TICK_SIGNAL) || defined(TICK_SIGWAIT) - - struct itimerval itimer; - portTickType xMicroSeconds = portTICK_RATE_MICROSECONDS; - - debug_printf("init %li microseconds\n",(long)xMicroSeconds); - /* Initialise the structure with the current timer information. */ - assert ( 0 == getitimer( TIMER_TYPE, &itimer ) ); - - /* Set the interval between timer events. */ - itimer.it_interval.tv_sec = 0; - itimer.it_interval.tv_usec = xMicroSeconds; - - /* Set the current count-down. */ - itimer.it_value.tv_sec = 0; - itimer.it_value.tv_usec = xMicroSeconds; - -#endif - -#ifdef TICK_SIGNAL - struct sigaction sigtick; - sigtick.sa_flags = 0; - sigtick.sa_handler = vPortSystemTickHandler; - sigfillset( &sigtick.sa_mask ); - assert ( 0 == sigaction( SIG_TICK, &sigtick, NULL ) ); - - /* Set-up the timer interrupt. */ - assert ( 0 == setitimer( TIMER_TYPE, &itimer, NULL ) ); - - sigemptyset( &xSignalToBlock ); - sigaddset( &xSignalToBlock, SIG_SUSPEND ); - (void)pthread_sigmask( SIG_SETMASK, &xSignalToBlock, NULL ); - - while(1) - { - usleep(1000); - sched_yield(); - } -#endif - -#ifdef TICK_SIGWAIT - /* Tick signal already blocked */ - sigset_t xSignalsToWait; - sigemptyset( &xSignalsToWait ); - sigaddset( &xSignalsToWait, SIG_TICK ); - - /* Set-up the timer interrupt. */ - assert ( 0 == setitimer( TIMER_TYPE, &itimer, NULL ) ); - - while( pdTRUE != xSchedulerEnd ) { - int xResult; - assert( 0 == sigwait( &xSignalsToWait, &xResult ) ); -// assert( xResult == SIG_TICK ); - vPortSystemTickHandler(SIG_TICK); - } -#endif - -#if !defined(TICK_SIGNAL) && !defined(TICK_SIGWAIT) - - struct timespec x; - while( pdTRUE != xSchedulerEnd ) { - x.tv_sec=0; - x.tv_nsec=portTICK_RATE_MICROSECONDS * 1000; - nanosleep(&x,NULL); -// careful - on some systems a signal to ANY thread in the process will -// end nanosleeps immediately - better sleep with pselect() and set the -// wakeup sigmask to all blocked (see test_case_x_pselect.c) -// printf("."); fflush(stdout); - vPortSystemTickHandler(SIG_TICK); -// printf("*"); fflush(stdout); - } - -#endif - debug_printf( "Cleaning Up, Exiting.\n" ); - /* Cleanup the mutexes */ - xResult = pthread_mutex_destroy( &xSuspendResumeThreadMutex ); - xResult = pthread_mutex_destroy( &xSwappingThreadMutex ); - vPortFree( (void *)pxThreads ); - - /* Should not get here! */ - return 0; -} -/*-----------------------------------------------------------*/ - -void vPortEndScheduler( void ) -{ -portBASE_TYPE xNumberOfThreads; -portBASE_TYPE xResult; - - - for ( xNumberOfThreads = 0; xNumberOfThreads < MAX_NUMBER_OF_TASKS; xNumberOfThreads++ ) - { - if ( ( pthread_t )NULL != pxThreads[ xNumberOfThreads ].hThread ) - { - /* Kill all of the threads, they are in the detached state. */ - xResult = pthread_cancel( pxThreads[ xNumberOfThreads ].hThread ); - } - } - - /* Signal the scheduler to exit its loop. */ - xSchedulerEnd = pdTRUE; - (void)pthread_kill( hMainThread, SIG_RESUME ); -} -/*-----------------------------------------------------------*/ - -void vPortYieldFromISR( void ) -{ - /* Calling Yield from a Interrupt/Signal handler often doesn't work because the - * xSwappingThreadMutex is already owned by an original call to Yield. Therefore, - * simply indicate that a yield is required soon. - */ - - xPendYield = pdTRUE; -} -/*-----------------------------------------------------------*/ - -void vPortEnterCritical( void ) -{ - vPortDisableInterrupts(); - uxCriticalNesting++; -} -/*-----------------------------------------------------------*/ - -void vPortExitCritical( void ) -{ - /* Check for unmatched exits. */ - if ( uxCriticalNesting > 0 ) - { -// careful - race condition possible? - uxCriticalNesting--; - } - - /* If we have reached 0 then re-enable the interrupts. */ - if( uxCriticalNesting == 0 ) - { - /* Have we missed ticks? This is the equivalent of pending an interrupt. */ - if ( pdTRUE == xPendYield ) - { - xPendYield = pdFALSE; - vPortYield(); - } - vPortEnableInterrupts(); - } -} -/*-----------------------------------------------------------*/ - -void vPortYield( void ) -{ -pthread_t xTaskToSuspend; -pthread_t xTaskToResume; -int retVal; -tskTCB * oldTask, * newTask; - - /* We must mask the suspend signal here, because otherwise there can be an */ - /* interrupt while in pthread_mutex_lock and that will cause the next thread */ - /* to deadlock when it tries to get this mutex */ - - debug_printf( "Entering\r\n" ); - - vPortEnterCritical(); - - retVal = pthread_mutex_trylock( &xSwappingThreadMutex ); - while( retVal != 0 ) { - - assert( retVal == EBUSY ); - - /* If we can't get the mutex, that means an interrupt is running and we */ - /* should keep an eye out if this task should suspend so the interrupt */ - /* routine doesn't stall waiting for this task to pause */ - debug_printf( "Waiting to get swapping mutex from ISR\r\n" ); - - assert( xTaskGetCurrentTaskHandle() != NULL ); - xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() ); -// careful! race condition!!!! unprotected by mutex - - if( prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) != pthread_self() ) { -// careful! race condition!!!! unprotected by mutex - debug_printf( "The current task isn't even us. Pausing now, deal with possible interrupt later.\r\n" ); - vPortExitCritical(); - pauseThread( THREAD_PAUSE_YIELD ); - - return; - } - sched_yield(); - retVal = pthread_mutex_trylock( &xSwappingThreadMutex ); - } - - /* At this point we have the lock, active task shouldn't change */ - oldTask = xTaskGetCurrentTaskHandle(); - assert( oldTask != NULL ); - xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() ); - - if(xTaskToSuspend != pthread_self() ) { - debug_printf( "The current task isn't even us, letting interrupt happen. Watch for swap.\r\n" ); - - assert( pthread_mutex_unlock( &xSwappingThreadMutex ) == 0); - vPortExitCritical(); - pauseThread( THREAD_PAUSE_YIELD ); - - return; - } - - assert( xTaskToSuspend == pthread_self() ); // race condition I didn't account for - - /* Get new task then release the task switching mutex */ - vTaskSwitchContext(); - newTask = xTaskGetCurrentTaskHandle(); - assert( newTask != NULL ); - xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() ); - - if ( pthread_self() != xTaskToResume ) - { - /* Remember and switch the critical nesting. */ - prvSetTaskCriticalNesting( xTaskToSuspend, uxCriticalNesting ); - uxCriticalNesting = prvGetTaskCriticalNesting( xTaskToResume ); - - debug_error( "Swapping From %li(%s) to %li(%s)\r\n", (long int) xTaskToSuspend, oldTask->pcTaskName, (long int) xTaskToResume, newTask->pcTaskName); - -#ifdef COND_SIGNALING - /* Set resume condition for specific thread */ - pthread_cond_t * hCond = prvGetConditionHandle( xTaskGetCurrentTaskHandle() ); - assert( pthread_cond_signal( hCond ) == 0 ); -#endif -#ifdef CHECK_TASK_RESUMES - while( prvGetTaskState( oldTask ) != THREAD_STATE_RUNNING ) - { - usleep(100); - sched_yield(); - debug_printf( "Waiting for task to resume\r\n" ); - } -#endif - - debug_printf( "Detected task resuming. Pausing this task\r\n" ); - - /* Release swapping thread mutex and pause self */ - assert( pthread_mutex_unlock( &xSwappingThreadMutex ) == 0); - pauseThread( THREAD_PAUSE_YIELD ); - } - else { - assert( pthread_mutex_unlock( &xSwappingThreadMutex ) == 0); - } - - /* Now we are resuming, want to be able to catch this interrupt again */ - vPortExitCritical(); - -} -/*-----------------------------------------------------------*/ - -void vPortDisableInterrupts( void ) -{ - //debug_printf("\r\n"); - sigset_t xSignals; - sigemptyset( &xSignals ); - sigaddset( &xSignals, SIG_SUSPEND ); - pthread_sigmask( SIG_BLOCK, &xSignals, NULL ); - - xInterruptsEnabled = pdFALSE; -} -/*-----------------------------------------------------------*/ - -void vPortEnableInterrupts( void ) -{ - - xInterruptsEnabled = pdTRUE; - //debug_printf("\r\n"); - sigset_t xSignals; - sigemptyset( &xSignals ); - sigaddset( &xSignals, SIG_SUSPEND ); - pthread_sigmask( SIG_UNBLOCK, &xSignals, NULL ); -} -/*-----------------------------------------------------------*/ - -portBASE_TYPE xPortSetInterruptMask( void ) -{ - fprintf(stderr, "This is called!\r\n"); -portBASE_TYPE xReturn = xInterruptsEnabled; - debug_printf("\r\n"); - xInterruptsEnabled = pdFALSE; - return xReturn; -} -/*-----------------------------------------------------------*/ - -void vPortClearInterruptMask( portBASE_TYPE xMask ) -{ - debug_printf("\r\n"); - xInterruptsEnabled = xMask; -} -/*-----------------------------------------------------------*/ - - -void vPortSystemTickHandler( int sig ) -{ -pthread_t xTaskToSuspend; -pthread_t xTaskToResume; -tskTCB * oldTask, * newTask; - -/* assert( SIG_TICK == sig ); - assert( prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) != NULL ); - assert( pthread_self() != prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) ); */ - - debug_printf( "\r\n\r\n" ); - debug_printf( "(xInterruptsEnabled = %i, xServicingTick = %i)\r\n", (int) xInterruptsEnabled != 0, (int) xServicingTick != 0); - if ( ( pdTRUE == xInterruptsEnabled ) && ( pdTRUE != xServicingTick ) ) - { -// debug_printf( "Checking for lock ...\r\n" ); - if ( 0 == pthread_mutex_trylock( &xSwappingThreadMutex ) ) - { - debug_printf( "Handling\r\n"); - xServicingTick = pdTRUE; - - oldTask = xTaskGetCurrentTaskHandle(); - assert( oldTask != NULL ); - xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() ); - - /* Tick Increment. */ - vTaskIncrementTick(); - - /* Select Next Task. */ -#if ( configUSE_PREEMPTION == 1 ) - vTaskSwitchContext(); -#endif - - newTask = xTaskGetCurrentTaskHandle(); - assert( newTask != NULL ); - xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() ); - - debug_printf( "Want %s running\r\n", newTask->pcTaskName ); - /* The only thread that can process this tick is the running thread. */ - if ( xTaskToSuspend != xTaskToResume ) - { - /* Remember and switch the critical nesting. */ - prvSetTaskCriticalNesting( xTaskToSuspend, uxCriticalNesting ); - uxCriticalNesting = prvGetTaskCriticalNesting( xTaskToResume ); - - debug_printf( "Swapping From %li(%s) to %li(%s)\r\n", (long int) xTaskToSuspend, oldTask->pcTaskName, (long int) xTaskToResume, newTask->pcTaskName); - - assert( pthread_kill( xTaskToSuspend, SIG_SUSPEND ) == 0); - -#ifdef CHECK_TASK_RESUMES - /* It shouldn't be possible for a second task swap to happen while waiting for this because */ - /* they can't get the xSwappingThreadMutex */ - while( prvGetTaskState( oldTask ) != THREAD_STATE_PAUSE ) -#endif - { - usleep(100); - debug_printf( "Waiting for old task to suspend\r\n" ); - debug_printf( "Sent signal\r\n" ); - sched_yield(); - } - debug_printf( "Suspended\r\n" ); - -#ifdef CHECK_TASK_RESUMES - while( prvGetTaskState( newTask ) != THREAD_STATE_RUNNING ) -#endif - { - debug_printf( "Waiting for new task to resume\r\n" ); -#ifdef COND_SIGNALING - // Set resume condition for specific thread - pthread_cond_t * hCond = prvGetConditionHandle( xTaskGetCurrentTaskHandle() ); - assert( pthread_cond_signal( hCond ) == 0 ); -#endif - - sched_yield(); - } - - debug_printf( "Swapped From %li(%s) to %li(%s)\r\n", (long int) xTaskToSuspend, oldTask->pcTaskName, (long int) xTaskToResume, newTask->pcTaskName); } - else - { - // debug_error ("Want %s running \r\n", newTask->pcTaskName ); - } - xServicingTick = pdFALSE; - (void)pthread_mutex_unlock( &xSwappingThreadMutex ); - } - else - { - debug_error( "Pending yield here (portYield has lock - hopefully)\r\n" ); - xPendYield = pdTRUE; - } - - } - else - { - debug_printf( "Pending yield or here\r\n"); - xPendYield = pdTRUE; - } - debug_printf("Exiting\r\n"); -} -/*-----------------------------------------------------------*/ - -void vPortForciblyEndThread( void *pxTaskToDelete ) -{ -xTaskHandle hTaskToDelete = ( xTaskHandle )pxTaskToDelete; -pthread_t xTaskToDelete; -pthread_t xTaskToResume; -portBASE_TYPE xResult; - - printf("vPortForciblyEndThread\r\n"); - - if ( 0 == pthread_mutex_lock( &xSwappingThreadMutex ) ) -// careful! windows bug - this thread won't be suspendable while waiting for mutex! -// so tick handler will wait forever for this thread to go to sleep -// might want to put a try_lock() - sched_yield() loop when on cygwin! - { - xTaskToDelete = prvGetThreadHandle( hTaskToDelete ); - xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() ); - - if ( xTaskToResume == xTaskToDelete ) - { - /* This is a suicidal thread, need to select a different task to run. */ - vTaskSwitchContext(); - xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() ); - } - - if ( pthread_self() != xTaskToDelete ) - { - /* Cancelling a thread that is not me. */ - if ( xTaskToDelete != ( pthread_t )NULL ) - { - /* Send a signal to wake the task so that it definitely cancels. */ - pthread_testcancel(); - xResult = pthread_cancel( xTaskToDelete ); - /* Pthread Clean-up function will note the cancellation. */ - } - (void)pthread_mutex_unlock( &xSwappingThreadMutex ); - } - else - { - /* Resume the other thread. */ - /* Assert zero - I never fixed this functionality */ - assert( 0 ); -// careful! will be hit every time a thread exits itself gracefully - better fix this, we might need -// it - - /* Pthread Clean-up function will note the cancellation. */ - /* Release the execution. */ - uxCriticalNesting = 0; - vPortEnableInterrupts(); - (void)pthread_mutex_unlock( &xSwappingThreadMutex ); - /* Commit suicide */ - pthread_exit( (void *)1 ); - } - } -} -/*-----------------------------------------------------------*/ - -void *prvWaitForStart( void * pvParams ) -{ -xParams * pxParams = ( xParams * )pvParams; -pdTASK_CODE pvCode = pxParams->pxCode; -void * pParams = pxParams->pvParams; - vPortFree( pvParams ); - - pthread_cleanup_push( prvDeleteThread, (void *)pthread_self() ); - - /* want to block suspend when not the active thread */ - sigset_t xSignals; - sigemptyset( &xSignals ); - sigaddset( &xSignals, SIG_SUSPEND ); - sigaddset( &xSignals, SIG_TICK ); - assert( pthread_sigmask( SIG_SETMASK, &xSignals, NULL ) == 0); - - /* Because the FreeRTOS creates the TCB stack, which in this implementation */ - /* creates a thread, we need to wait until the task handle is added before */ - /* trying to pause. Must set xSentinel high so the creating task knows we're */ - /* here. Order is strange but because of how this is hacked onto the trace */ - /*handling code in tasks.c */ - xSentinel = 1; - - while( prvGetTaskHandle( pthread_self() ) == NULL ){ - sched_yield(); - } - - debug_printf("Handle added, pausing\r\n"); - - /* Want to delay briefly until we have explicit resume signal as otherwise the */ - /* current task variable might be in the wrong state */ - pauseThread( THREAD_PAUSE_CREATED ); - debug_printf("Starting first run\r\n"); - - /* Since all starting tasks have the critical nesting at zero, just enable interrupts */ - vPortEnableInterrupts(); - - pvCode( pParams ); - - pthread_cleanup_pop( 1 ); - return (void *)NULL; -} -/*-----------------------------------------------------------*/ - -void pauseThread( portBASE_TYPE pauseMode ) -{ - xTaskHandle hTask = prvGetTaskHandle( pthread_self() ); - - debug_printf( "Pausing thread %li. Set state to suspended\r\n", (long int) pthread_self() ); - prvSetTaskState( hTask, THREAD_STATE_PAUSE ); - -#ifdef RUNNING_THREAD_MUTEX - if( pauseMode != THREAD_PAUSE_CREATED ) - assert( 0 == pthread_mutex_unlock( &xRunningThread ) ); -#endif - -#ifdef COND_SIGNALING - int xResult; - pthread_cond_t * hCond = prvGetConditionHandle( hTask ); - pthread_mutex_t * hMutex = prvGetMutexHandle( hTask ); - debug_printf("Cond: %li\r\n", *( (long int *) hCond) ); - debug_printf("Mutex: %li\r\n", *( (long int *) hMutex) ); - - struct timeval tv; - struct timespec ts; - gettimeofday( &tv, NULL ); - ts.tv_sec = tv.tv_sec + 0; -#endif - - while (1) { - assert( xTaskGetCurrentTaskHandle() != NULL ); - if( pthread_self() == prvGetThreadHandle(xTaskGetCurrentTaskHandle() ) && xRunning ) -// careful! race condition!!!! possibly unprotected by mutex when CHECK_TASK_RESUMES is not set? - { - - /* Must do this before trying to lock the mutex, because if CHECK_TASK_RESUMES */ - /* is defined then the mutex not unlocked until this is changed */ - debug_printf( "Resuming. Marking task as running\r\n" ); - prvSetTaskState( hTask, THREAD_STATE_RUNNING ); - -#ifdef RUNNING_THREAD_MUTEX - assert( 0 == pthread_mutex_lock( &xRunningThread ) ); -#endif - debug_error("Resuming\r\n"); - return; - } - else { -#ifdef COND_SIGNALING - gettimeofday( &tv, NULL ); - ts.tv_sec = ts.tv_sec + 1; - ts.tv_nsec = 0; - xResult = pthread_cond_timedwait( hCond, hMutex, &ts ); - assert( xResult != EINVAL ); -#else - /* For windows where conditional signaling is buggy */ - /* It would be wonderful to put a nanosleep here, but since its not reentrant safe */ - /* and there may be a sleep in the main code (this can be called from an ISR) we must */ - /* check this */ - if( pauseMode != THREAD_PAUSE_INTERRUPT ) - usleep(1000); - sched_yield(); - -#endif -// debug_error( "Checked my status\r\n" ); - } - - } -} - -void prvSuspendSignalHandler(int sig) -{ -//sigset_t xBlockSignals; - - /* This signal is set here instead of pauseThread because it is checked by the tick handler */ - /* which means if there were a swap it should result in a suspend interrupt */ - - debug_error( "Caught signal %i\r\n", sig ); - -#ifdef CHECK_TASK_RESUMES - /* This would seem like a major bug, but can happen because now we send extra suspend signals */ - /* if they aren't caught */ - assert( xTaskGetCurrentTaskHandle() != NULL ); - if( pthread_self() == prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) ) { -// careful! race condition? Or does the tick handler wait for us to sleep before unlocking? - debug_printf( "Marked as current task, resuming\r\n" ); - return; - } -#endif - - /* Check that we aren't suspending when we should be running. This bug would need tracking down */ -// assert( pthread_self() != prvGetThreadHandle(xTaskGetCurrentTaskHandle() ) ); - - /* Block further suspend signals. They need to go to their thread */ -/* sigemptyset( &xBlockSignals ); - sigaddset( &xBlockSignals, SIG_SUSPEND ); - assert( pthread_sigmask( SIG_BLOCK, &xBlockSignals, NULL ) == 0); - - assert( xTaskGetCurrentTaskHandle() != NULL ); - while( pthread_self() != prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) ) -// careful! race condition? could a port_yield mess with this? - { - debug_printf( "Incorrectly woke up. Repausing\r\n" ); */ - pauseThread( THREAD_PAUSE_INTERRUPT ); -/* } - - assert( xTaskGetCurrentTaskHandle() != NULL ); - assert( pthread_self() == prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) ); */ - - /* Old synchronization code, may still be required - while( !xHandover ); - assert( 0 == pthread_mutex_lock( &xSingleThreadMutex ) ); */ - - /* Respond to signals again */ -/* sigemptyset( &xBlockSignals ); - sigaddset( &xBlockSignals, SIG_SUSPEND ); - assert( 0 == pthread_sigmask( SIG_UNBLOCK, &xBlockSignals, NULL ) ); - - debug_printf( "Resuming %li from signal %i\r\n", (long int) pthread_self(), sig ); */ - - /* Will resume here when the SIG_RESUME signal is received. */ - /* Need to set the interrupts based on the task's critical nesting. */ - if ( uxCriticalNesting == 0 ) - { - vPortEnableInterrupts(); - } - else - { - debug_printf( "Not reenabling interrupts\r\n" ); - vPortDisableInterrupts(); - } - debug_printf("Exit\r\n"); -} - -/*-----------------------------------------------------------*/ - -void prvSetupSignalsAndSchedulerPolicy( void ) -{ -/* The following code would allow for configuring the scheduling of this task as a Real-time task. - * The process would then need to be run with higher privileges for it to take affect. -int iPolicy; -int iResult; -int iSchedulerPriority; - iResult = pthread_getschedparam( pthread_self(), &iPolicy, &iSchedulerPriority ); - iResult = pthread_attr_setschedpolicy( &xThreadAttributes, SCHED_FIFO ); - iPolicy = SCHED_FIFO; - iResult = pthread_setschedparam( pthread_self(), iPolicy, &iSchedulerPriority ); */ - -struct sigaction sigsuspendself; -portLONG lIndex; - - debug_printf("prvSetupSignalAndSchedulerPolicy\r\n"); - - pxThreads = ( xThreadState *)pvPortMalloc( sizeof( xThreadState ) * MAX_NUMBER_OF_TASKS ); - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - pxThreads[ lIndex ].hThread = ( pthread_t )NULL; - pxThreads[ lIndex ].hTask = ( xTaskHandle )NULL; - pxThreads[ lIndex ].uxCriticalNesting = 0; - pxThreads[ lIndex ].xThreadState = 0; - } - - sigsuspendself.sa_flags = 0; - sigsuspendself.sa_handler = prvSuspendSignalHandler; - sigemptyset( &sigsuspendself.sa_mask ); - - assert ( 0 == sigaction( SIG_SUSPEND, &sigsuspendself, NULL ) ); -} - -/*-----------------------------------------------------------*/ -pthread_mutex_t * prvGetMutexHandle( xTaskHandle hTask ) -{ -pthread_mutex_t * hMutex; -portLONG lIndex; - - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - if ( pxThreads[ lIndex ].hTask == hTask ) - { - hMutex = pxThreads[ lIndex ].hMutex; - break; - } - } - return hMutex; -} - -/*-----------------------------------------------------------*/ -xTaskHandle prvGetTaskHandle( pthread_t hThread ) -{ -portLONG lIndex; - - /* If not initialized yet */ - if( pxThreads == NULL ) return NULL; - assert( hThread != (pthread_t) NULL ); - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - if ( pxThreads[ lIndex ].hThread == hThread ) - { - return pxThreads[ lIndex ].hTask; - } - } -// assert( 0 ); - return NULL; -} - -/*-----------------------------------------------------------*/ -pthread_cond_t * prvGetConditionHandle( xTaskHandle hTask ) -{ -pthread_cond_t * hCond = NULL; -portLONG lIndex; - - assert( hTask != NULL ); - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - if ( pxThreads[ lIndex ].hTask == hTask ) - { - debug_printf( "Found condition on %i task\r\n", lIndex ); - hCond = pxThreads[ lIndex ].hCond; - break; - } - } - assert( hCond != NULL ); - return hCond; - printf( "Failed to get handle, pausing then recursing\r\n" ); - usleep(1000); - return prvGetConditionHandle( hTask ); - assert(0); - return hCond; -} - -/*-----------------------------------------------------------*/ -#ifdef CHECK_TASK_RESUMES -static portBASE_TYPE prvGetTaskState( xTaskHandle hTask ) -{ -portLONG lIndex; - assert( hTask != NULL ); - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - if ( pxThreads[ lIndex ].hTask == hTask ) - { - debug_printf( "Found state (%li) on %i task\r\n",pxThreads[ lIndex ].xThreadState, lIndex ); - return pxThreads[ lIndex ].xThreadState; - } - } - assert(0); - return 0; -} -#endif - -void prvSetTaskState( xTaskHandle hTask, portBASE_TYPE state ) -{ -portLONG lIndex; - assert( hTask != NULL ); - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - if ( pxThreads[ lIndex ].hTask == hTask ) - { - pxThreads[ lIndex ].xThreadState = state; - return; - } - } - assert(0); - return; -} - - -/*-----------------------------------------------------------*/ -pthread_t prvGetThreadHandle( xTaskHandle hTask ) -{ - pthread_t hThread = ( pthread_t )NULL; - portLONG lIndex; - - assert( hTask != NULL ); - - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - if ( pxThreads[ lIndex ].hTask == hTask ) - { - hThread = pxThreads[ lIndex ].hThread; - break; - } - } - - assert( hThread != (pthread_t) NULL ); - return hThread; -} -/*-----------------------------------------------------------*/ - -portLONG prvGetFreeThreadState( void ) -{ -portLONG lIndex; - - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - if ( pxThreads[ lIndex ].hThread == ( pthread_t )NULL ) - { - break; - } - } - - if ( MAX_NUMBER_OF_TASKS == lIndex ) - { - printf( "No more free threads, please increase the maximum.\n" ); - lIndex = 0; - vPortEndScheduler(); - } - - return lIndex; -} -/*-----------------------------------------------------------*/ - -void prvSetTaskCriticalNesting( pthread_t xThreadId, unsigned portBASE_TYPE uxNesting ) -{ -portLONG lIndex; - - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - if ( pxThreads[ lIndex ].hThread == xThreadId ) - { - pxThreads[ lIndex ].uxCriticalNesting = uxNesting; - break; - } - } -} -/*-----------------------------------------------------------*/ - -unsigned portBASE_TYPE prvGetTaskCriticalNesting( pthread_t xThreadId ) -{ -unsigned portBASE_TYPE uxNesting = 0; -portLONG lIndex; - - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - if ( pxThreads[ lIndex ].hThread == xThreadId ) - { - uxNesting = pxThreads[ lIndex ].uxCriticalNesting; - break; - } - } - return uxNesting; -} -/*-----------------------------------------------------------*/ - -void prvDeleteThread( void *xThreadId ) -{ -portLONG lIndex; - - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - if ( pxThreads[ lIndex ].hThread == ( pthread_t )xThreadId ) - { - pxThreads[ lIndex ].hThread = (pthread_t)NULL; - pxThreads[ lIndex ].hTask = (xTaskHandle)NULL; - if ( pxThreads[ lIndex ].uxCriticalNesting > 0 ) - { - uxCriticalNesting = 0; - vPortEnableInterrupts(); - } - pxThreads[ lIndex ].uxCriticalNesting = 0; - break; - } - } -} -/*-----------------------------------------------------------*/ - -void vPortAddTaskHandle( void *pxTaskHandle ) -{ -portLONG lIndex; - - debug_printf("vPortAddTaskHandle\r\n"); - - pxThreads[ lIndexOfLastAddedTask ].hTask = ( xTaskHandle )pxTaskHandle; - for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) - { - if ( pxThreads[ lIndex ].hThread == pxThreads[ lIndexOfLastAddedTask ].hThread ) - { - if ( pxThreads[ lIndex ].hTask != pxThreads[ lIndexOfLastAddedTask ].hTask ) - { - pxThreads[ lIndex ].hThread = ( pthread_t )NULL; - pxThreads[ lIndex ].hTask = NULL; - pxThreads[ lIndex ].uxCriticalNesting = 0; - } - } - } - usleep(10000); -} -/*-----------------------------------------------------------*/ - -void vPortFindTicksPerSecond( void ) -{ - - /* Needs to be reasonably high for accuracy. */ - unsigned long ulTicksPerSecond = sysconf(_SC_CLK_TCK); - printf( "Timer Resolution for Run TimeStats is %ld ticks per second.\n", ulTicksPerSecond ); -} -/*-----------------------------------------------------------*/ - -unsigned long ulPortGetTimerValue( void ) -{ -struct tms xTimes; - - unsigned long ulTotalTime = times( &xTimes ); - /* Return the application code times. - * The timer only increases when the application code is actually running - * which means that the total execution times should add up to 100%. - */ - return ( unsigned long ) xTimes.tms_utime; - - /* Should check ulTotalTime for being clock_t max minus 1. */ - (void)ulTotalTime; -} -/*-----------------------------------------------------------*/ diff --git a/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/portmacro.h b/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/portmacro.h index 4da6b124b..775c2bac9 100644 --- a/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/portmacro.h +++ b/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/portmacro.h @@ -142,17 +142,6 @@ extern void vPortAddTaskHandle( void *pxTaskHandle ); /* Posix Signal definitions that can be changed or read as appropriate. */ #define SIG_SUSPEND SIGUSR1 -#define SIG_RESUME SIGUSR2 - -/* Enable the following hash defines to make use of the real-time tick where time progresses at real-time. */ -#define SIG_TICK SIGALRM -#define TIMER_TYPE ITIMER_REAL -/* Enable the following hash defines to make use of the process tick where time progresses only when the process is executing. -#define SIG_TICK SIGVTALRM -#define TIMER_TYPE ITIMER_VIRTUAL */ -/* Enable the following hash defines to make use of the profile tick where time progresses when the process or system calls are executing. -#define SIG_TICK SIGPROF -#define TIMER_TYPE ITIMER_PROF */ /* Make use of times(man 2) to gather run-time statistics on the tasks. */ extern void vPortFindTicksPerSecond( void ); diff --git a/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/tasks_linux.c b/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/tasks.c similarity index 96% rename from flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/tasks_linux.c rename to flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/tasks.c index 9b06c3f89..1d6e7a17a 100644 --- a/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/tasks_linux.c +++ b/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/tasks.c @@ -1,2323 +1,2323 @@ -/* - FreeRTOS V6.0.4 - Copyright (C) 2010 Real Time Engineers Ltd. - - *************************************************************************** - * * - * If you are: * - * * - * + New to FreeRTOS, * - * + Wanting to learn FreeRTOS or multitasking in general quickly * - * + Looking for basic training, * - * + Wanting to improve your FreeRTOS skills and productivity * - * * - * then take a look at the FreeRTOS eBook * - * * - * "Using the FreeRTOS Real Time Kernel - a Practical Guide" * - * http://www.FreeRTOS.org/Documentation * - * * - * A pdf reference manual is also available. Both are usually delivered * - * to your inbox within 20 minutes to two hours when purchased between 8am * - * and 8pm GMT (although please allow up to 24 hours in case of * - * exceptional circumstances). Thank you for your support! * - * * - *************************************************************************** - - This file is part of the FreeRTOS distribution. - - FreeRTOS is free software; you can redistribute it and/or modify it under - the terms of the GNU General Public License (version 2) as published by the - Free Software Foundation AND MODIFIED BY the FreeRTOS exception. - ***NOTE*** The exception to the GPL is included to allow you to distribute - a combined work that includes FreeRTOS without being obliged to provide the - source code for proprietary components outside of the FreeRTOS kernel. - FreeRTOS is distributed in the hope that it will be useful, but WITHOUT - ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or - FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for - more details. You should have received a copy of the GNU General Public - License and the FreeRTOS license exception along with FreeRTOS; if not it - can be viewed here: http://www.freertos.org/a00114.html and also obtained - by writing to Richard Barry, contact details for whom are available on the - FreeRTOS WEB site. - - 1 tab == 4 spaces! - - http://www.FreeRTOS.org - Documentation, latest information, license and - contact details. - - http://www.SafeRTOS.com - A version that is certified for use in safety - critical systems. - - http://www.OpenRTOS.com - Commercial support, development, porting, - licensing and training services. -*/ - - -#include -#include -#include -#include - -/* Defining MPU_WRAPPERS_INCLUDED_FROM_API_FILE prevents task.h from redefining -all the API functions to use the MPU wrappers. That should only be done when -task.h is included from an application file. */ -#define MPU_WRAPPERS_INCLUDED_FROM_API_FILE - -#include "FreeRTOS.h" -#include "task.h" -#include "StackMacros.h" - -#undef MPU_WRAPPERS_INCLUDED_FROM_API_FILE - -/* - * Macro to define the amount of stack available to the idle task. - */ -#define tskIDLE_STACK_SIZE configMINIMAL_STACK_SIZE - -/* - * Task control block. A task control block (TCB) is allocated to each task, - * and stores the context of the task. - */ -typedef struct tskTaskControlBlock -{ - volatile portSTACK_TYPE *pxTopOfStack; /*< Points to the location of the last item placed on the tasks stack. THIS MUST BE THE FIRST MEMBER OF THE STRUCT. */ - - #if ( portUSING_MPU_WRAPPERS == 1 ) - xMPU_SETTINGS xMPUSettings; /*< The MPU settings are defined as part of the port layer. THIS MUST BE THE SECOND MEMBER OF THE STRUCT. */ - #endif - - xListItem xGenericListItem; /*< List item used to place the TCB in ready and blocked queues. */ - xListItem xEventListItem; /*< List item used to place the TCB in event lists. */ - unsigned portBASE_TYPE uxPriority; /*< The priority of the task where 0 is the lowest priority. */ - portSTACK_TYPE *pxStack; /*< Points to the start of the stack. */ - signed char pcTaskName[ configMAX_TASK_NAME_LEN ];/*< Descriptive name given to the task when created. Facilitates debugging only. */ - - #if ( portSTACK_GROWTH > 0 ) - portSTACK_TYPE *pxEndOfStack; /*< Used for stack overflow checking on architectures where the stack grows up from low memory. */ - #endif - - #if ( portCRITICAL_NESTING_IN_TCB == 1 ) - unsigned portBASE_TYPE uxCriticalNesting; - #endif - - #if ( configUSE_TRACE_FACILITY == 1 ) - unsigned portBASE_TYPE uxTCBNumber; /*< This is used for tracing the scheduler and making debugging easier only. */ - #endif - - #if ( configUSE_MUTEXES == 1 ) - unsigned portBASE_TYPE uxBasePriority; /*< The priority last assigned to the task - used by the priority inheritance mechanism. */ - #endif - - #if ( configUSE_APPLICATION_TASK_TAG == 1 ) - pdTASK_HOOK_CODE pxTaskTag; - #endif - - #if ( configGENERATE_RUN_TIME_STATS == 1 ) - unsigned long ulRunTimeCounter; /*< Used for calculating how much CPU time each task is utilising. */ - #endif - -} tskTCB; - - -/* - * Some kernel aware debuggers require data to be viewed to be global, rather - * than file scope. - */ -#ifdef portREMOVE_STATIC_QUALIFIER - #define static -#endif - -/*lint -e956 */ -PRIVILEGED_DATA tskTCB * volatile pxCurrentTCB = NULL; - -/* Lists for ready and blocked tasks. --------------------*/ - -PRIVILEGED_DATA static xList pxReadyTasksLists[ configMAX_PRIORITIES ]; /*< Prioritised ready tasks. */ -PRIVILEGED_DATA static xList xDelayedTaskList1; /*< Delayed tasks. */ -PRIVILEGED_DATA static xList xDelayedTaskList2; /*< Delayed tasks (two lists are used - one for delays that have overflowed the current tick count. */ -PRIVILEGED_DATA static xList * volatile pxDelayedTaskList ; /*< Points to the delayed task list currently being used. */ -PRIVILEGED_DATA static xList * volatile pxOverflowDelayedTaskList; /*< Points to the delayed task list currently being used to hold tasks that have overflowed the current tick count. */ -PRIVILEGED_DATA static xList xPendingReadyList; /*< Tasks that have been readied while the scheduler was suspended. They will be moved to the ready queue when the scheduler is resumed. */ - -#if ( INCLUDE_vTaskDelete == 1 ) - - PRIVILEGED_DATA static volatile xList xTasksWaitingTermination; /*< Tasks that have been deleted - but the their memory not yet freed. */ - PRIVILEGED_DATA static volatile unsigned portBASE_TYPE uxTasksDeleted = ( unsigned portBASE_TYPE ) 0; - -#endif - -#if ( INCLUDE_vTaskSuspend == 1 ) - - PRIVILEGED_DATA static xList xSuspendedTaskList; /*< Tasks that are currently suspended. */ - -#endif - -/* File private variables. --------------------------------*/ -PRIVILEGED_DATA static volatile unsigned portBASE_TYPE uxCurrentNumberOfTasks = ( unsigned portBASE_TYPE ) 0; -PRIVILEGED_DATA static volatile portTickType xTickCount = ( portTickType ) 0; -PRIVILEGED_DATA static unsigned portBASE_TYPE uxTopUsedPriority = tskIDLE_PRIORITY; -PRIVILEGED_DATA static volatile unsigned portBASE_TYPE uxTopReadyPriority = tskIDLE_PRIORITY; -PRIVILEGED_DATA static volatile signed portBASE_TYPE xSchedulerRunning = pdFALSE; -PRIVILEGED_DATA static volatile unsigned portBASE_TYPE uxSchedulerSuspended = ( unsigned portBASE_TYPE ) pdFALSE; -PRIVILEGED_DATA static volatile unsigned portBASE_TYPE uxMissedTicks = ( unsigned portBASE_TYPE ) 0; -PRIVILEGED_DATA static volatile portBASE_TYPE xMissedYield = ( portBASE_TYPE ) pdFALSE; -PRIVILEGED_DATA static volatile portBASE_TYPE xNumOfOverflows = ( portBASE_TYPE ) 0; -PRIVILEGED_DATA static unsigned portBASE_TYPE uxTaskNumber = ( unsigned portBASE_TYPE ) 0; - -#if ( configGENERATE_RUN_TIME_STATS == 1 ) - - PRIVILEGED_DATA static char pcStatsString[ 50 ] ; - PRIVILEGED_DATA static unsigned long ulTaskSwitchedInTime = 0UL; /*< Holds the value of a timer/counter the last time a task was switched in. */ - static void prvGenerateRunTimeStatsForTasksInList( const signed char *pcWriteBuffer, xList *pxList, unsigned long ulTotalRunTime ) PRIVILEGED_FUNCTION; - -#endif - -/* Debugging and trace facilities private variables and macros. ------------*/ - -/* - * The value used to fill the stack of a task when the task is created. This - * is used purely for checking the high water mark for tasks. - */ -#define tskSTACK_FILL_BYTE ( 0xa5 ) - -/* - * Macros used by vListTask to indicate which state a task is in. - */ -#define tskBLOCKED_CHAR ( ( signed char ) 'B' ) -#define tskREADY_CHAR ( ( signed char ) 'R' ) -#define tskDELETED_CHAR ( ( signed char ) 'D' ) -#define tskSUSPENDED_CHAR ( ( signed char ) 'S' ) - -/* - * Macros and private variables used by the trace facility. - */ -#if ( configUSE_TRACE_FACILITY == 1 ) - - #define tskSIZE_OF_EACH_TRACE_LINE ( ( unsigned long ) ( sizeof( unsigned long ) + sizeof( unsigned long ) ) ) - PRIVILEGED_DATA static volatile signed char * volatile pcTraceBuffer; - PRIVILEGED_DATA static signed char *pcTraceBufferStart; - PRIVILEGED_DATA static signed char *pcTraceBufferEnd; - PRIVILEGED_DATA static signed portBASE_TYPE xTracing = pdFALSE; - static unsigned portBASE_TYPE uxPreviousTask = 255; - PRIVILEGED_DATA static char pcStatusString[ 50 ]; - -#endif - -/*-----------------------------------------------------------*/ - -/* - * Macro that writes a trace of scheduler activity to a buffer. This trace - * shows which task is running when and is very useful as a debugging tool. - * As this macro is called each context switch it is a good idea to undefine - * it if not using the facility. - */ -#if ( configUSE_TRACE_FACILITY == 1 ) - - #define vWriteTraceToBuffer() \ - { \ - if( xTracing ) \ - { \ - if( uxPreviousTask != pxCurrentTCB->uxTCBNumber ) \ - { \ - if( ( pcTraceBuffer + tskSIZE_OF_EACH_TRACE_LINE ) < pcTraceBufferEnd ) \ - { \ - uxPreviousTask = pxCurrentTCB->uxTCBNumber; \ - *( unsigned long * ) pcTraceBuffer = ( unsigned long ) xTickCount; \ - pcTraceBuffer += sizeof( unsigned long ); \ - *( unsigned long * ) pcTraceBuffer = ( unsigned long ) uxPreviousTask; \ - pcTraceBuffer += sizeof( unsigned long ); \ - } \ - else \ - { \ - xTracing = pdFALSE; \ - } \ - } \ - } \ - } - -#else - - #define vWriteTraceToBuffer() - -#endif -/*-----------------------------------------------------------*/ - -/* - * Place the task represented by pxTCB into the appropriate ready queue for - * the task. It is inserted at the end of the list. One quirk of this is - * that if the task being inserted is at the same priority as the currently - * executing task, then it will only be rescheduled after the currently - * executing task has been rescheduled. - */ -#define prvAddTaskToReadyQueue( pxTCB ) \ -{ \ - if( pxTCB->uxPriority > uxTopReadyPriority ) \ - { \ - uxTopReadyPriority = pxTCB->uxPriority; \ - } \ - vListInsertEnd( ( xList * ) &( pxReadyTasksLists[ pxTCB->uxPriority ] ), &( pxTCB->xGenericListItem ) ); \ -} -/*-----------------------------------------------------------*/ - -/* - * Macro that looks at the list of tasks that are currently delayed to see if - * any require waking. - * - * Tasks are stored in the queue in the order of their wake time - meaning - * once one tasks has been found whose timer has not expired we need not look - * any further down the list. - */ -#define prvCheckDelayedTasks() \ -{ \ -register tskTCB *pxTCB; \ - \ - while( ( pxTCB = ( tskTCB * ) listGET_OWNER_OF_HEAD_ENTRY( pxDelayedTaskList ) ) != NULL ) \ - { \ - if( xTickCount < listGET_LIST_ITEM_VALUE( &( pxTCB->xGenericListItem ) ) ) \ - { \ - break; \ - } \ - vListRemove( &( pxTCB->xGenericListItem ) ); \ - /* Is the task waiting on an event also? */ \ - if( pxTCB->xEventListItem.pvContainer ) \ - { \ - vListRemove( &( pxTCB->xEventListItem ) ); \ - } \ - prvAddTaskToReadyQueue( pxTCB ); \ - } \ -} -/*-----------------------------------------------------------*/ - -/* - * Several functions take an xTaskHandle parameter that can optionally be NULL, - * where NULL is used to indicate that the handle of the currently executing - * task should be used in place of the parameter. This macro simply checks to - * see if the parameter is NULL and returns a pointer to the appropriate TCB. - */ -#define prvGetTCBFromHandle( pxHandle ) ( ( pxHandle == NULL ) ? ( tskTCB * ) pxCurrentTCB : ( tskTCB * ) pxHandle ) - - -/* File private functions. --------------------------------*/ - -/* - * Utility to ready a TCB for a given task. Mainly just copies the parameters - * into the TCB structure. - */ -static void prvInitialiseTCBVariables( tskTCB *pxTCB, const signed char * const pcName, unsigned portBASE_TYPE uxPriority, const xMemoryRegion * const xRegions, unsigned short usStackDepth ) PRIVILEGED_FUNCTION; - -/* - * Utility to ready all the lists used by the scheduler. This is called - * automatically upon the creation of the first task. - */ -static void prvInitialiseTaskLists( void ) PRIVILEGED_FUNCTION; - -/* - * The idle task, which as all tasks is implemented as a never ending loop. - * The idle task is automatically created and added to the ready lists upon - * creation of the first user task. - * - * The portTASK_FUNCTION_PROTO() macro is used to allow port/compiler specific - * language extensions. The equivalent prototype for this function is: - * - * void prvIdleTask( void *pvParameters ); - * - */ -static portTASK_FUNCTION_PROTO( prvIdleTask, pvParameters ); - -/* - * Utility to free all memory allocated by the scheduler to hold a TCB, - * including the stack pointed to by the TCB. - * - * This does not free memory allocated by the task itself (i.e. memory - * allocated by calls to pvPortMalloc from within the tasks application code). - */ -#if ( ( INCLUDE_vTaskDelete == 1 ) || ( INCLUDE_vTaskCleanUpResources == 1 ) ) - - static void prvDeleteTCB( tskTCB *pxTCB ) PRIVILEGED_FUNCTION; - -#endif - -/* - * Used only by the idle task. This checks to see if anything has been placed - * in the list of tasks waiting to be deleted. If so the task is cleaned up - * and its TCB deleted. - */ -static void prvCheckTasksWaitingTermination( void ) PRIVILEGED_FUNCTION; - -/* - * Allocates memory from the heap for a TCB and associated stack. Checks the - * allocation was successful. - */ -static tskTCB *prvAllocateTCBAndStack( unsigned short usStackDepth, portSTACK_TYPE *puxStackBuffer ) PRIVILEGED_FUNCTION; - -/* - * Called from vTaskList. vListTasks details all the tasks currently under - * control of the scheduler. The tasks may be in one of a number of lists. - * prvListTaskWithinSingleList accepts a list and details the tasks from - * within just that list. - * - * THIS FUNCTION IS INTENDED FOR DEBUGGING ONLY, AND SHOULD NOT BE CALLED FROM - * NORMAL APPLICATION CODE. - */ -#if ( configUSE_TRACE_FACILITY == 1 ) - - static void prvListTaskWithinSingleList( const signed char *pcWriteBuffer, xList *pxList, signed char cStatus ) PRIVILEGED_FUNCTION; - -#endif - -/* - * When a task is created, the stack of the task is filled with a known value. - * This function determines the 'high water mark' of the task stack by - * determining how much of the stack remains at the original preset value. - */ -#if ( ( configUSE_TRACE_FACILITY == 1 ) || ( INCLUDE_uxTaskGetStackHighWaterMark == 1 ) ) - - static unsigned short usTaskCheckFreeStackSpace( const unsigned char * pucStackByte ) PRIVILEGED_FUNCTION; - -#endif - - -/*lint +e956 */ - - - -/*----------------------------------------------------------- - * TASK CREATION API documented in task.h - *----------------------------------------------------------*/ - -signed portBASE_TYPE xTaskGenericCreate( pdTASK_CODE pxTaskCode, const signed char * const pcName, unsigned short usStackDepth, void *pvParameters, unsigned portBASE_TYPE uxPriority, xTaskHandle *pxCreatedTask, portSTACK_TYPE *puxStackBuffer, const xMemoryRegion * const xRegions ) -{ -signed portBASE_TYPE xReturn; -tskTCB * pxNewTCB; - - /* Allocate the memory required by the TCB and stack for the new task, - checking that the allocation was successful. */ - pxNewTCB = prvAllocateTCBAndStack( usStackDepth, puxStackBuffer ); - - if( pxNewTCB != NULL ) - { - portSTACK_TYPE *pxTopOfStack; - - #if( portUSING_MPU_WRAPPERS == 1 ) - /* Should the task be created in privileged mode? */ - portBASE_TYPE xRunPrivileged; - if( ( uxPriority & portPRIVILEGE_BIT ) != 0x00 ) - { - xRunPrivileged = pdTRUE; - } - else - { - xRunPrivileged = pdFALSE; - } - uxPriority &= ~portPRIVILEGE_BIT; - #endif /* portUSING_MPU_WRAPPERS == 1 */ - - /* Calculate the top of stack address. This depends on whether the - stack grows from high memory to low (as per the 80x86) or visa versa. - portSTACK_GROWTH is used to make the result positive or negative as - required by the port. */ - #if( portSTACK_GROWTH < 0 ) - { - pxTopOfStack = pxNewTCB->pxStack + ( usStackDepth - 1 ); - pxTopOfStack = ( portSTACK_TYPE * ) ( ( ( unsigned long ) pxTopOfStack ) & ( ( unsigned long ) ~portBYTE_ALIGNMENT_MASK ) ); - } - #else - { - pxTopOfStack = pxNewTCB->pxStack; - - /* If we want to use stack checking on architectures that use - a positive stack growth direction then we also need to store the - other extreme of the stack space. */ - pxNewTCB->pxEndOfStack = pxNewTCB->pxStack + ( usStackDepth - 1 ); - } - #endif - - /* Setup the newly allocated TCB with the initial state of the task. */ - prvInitialiseTCBVariables( pxNewTCB, pcName, uxPriority, xRegions, usStackDepth ); - - /* Initialize the TCB stack to look as if the task was already running, - but had been interrupted by the scheduler. The return address is set - to the start of the task function. Once the stack has been initialised - the top of stack variable is updated. */ - #if( portUSING_MPU_WRAPPERS == 1 ) - { - pxNewTCB->pxTopOfStack = pxPortInitialiseStack( pxTopOfStack, pxTaskCode, pvParameters, xRunPrivileged ); - } - #else - { - pxNewTCB->pxTopOfStack = pxPortInitialiseStack( pxTopOfStack, pxTaskCode, pvParameters ); - } - #endif - - /* We are going to manipulate the task queues to add this task to a - ready list, so must make sure no interrupts occur. */ - portENTER_CRITICAL(); - { - uxCurrentNumberOfTasks++; - if( uxCurrentNumberOfTasks == ( unsigned portBASE_TYPE ) 1 ) - { - /* As this is the first task it must also be the current task. */ - pxCurrentTCB = pxNewTCB; - - /* This is the first task to be created so do the preliminary - initialisation required. We will not recover if this call - fails, but we will report the failure. */ - prvInitialiseTaskLists(); - } - else - { - /* If the scheduler is not already running, make this task the - current task if it is the highest priority task to be created - so far. */ - if( xSchedulerRunning == pdFALSE ) - { - if( pxCurrentTCB->uxPriority <= uxPriority ) - { - pxCurrentTCB = pxNewTCB; - } - } - } - - /* Remember the top priority to make context switching faster. Use - the priority in pxNewTCB as this has been capped to a valid value. */ - if( pxNewTCB->uxPriority > uxTopUsedPriority ) - { - uxTopUsedPriority = pxNewTCB->uxPriority; - } - - #if ( configUSE_TRACE_FACILITY == 1 ) - { - /* Add a counter into the TCB for tracing only. */ - pxNewTCB->uxTCBNumber = uxTaskNumber; - } - #endif - uxTaskNumber++; - - prvAddTaskToReadyQueue( pxNewTCB ); - - xReturn = pdPASS; - traceTASK_CREATE( pxNewTCB ); - } - portEXIT_CRITICAL(); - } - else - { - xReturn = errCOULD_NOT_ALLOCATE_REQUIRED_MEMORY; - traceTASK_CREATE_FAILED( pxNewTCB ); - } - - if( xReturn == pdPASS ) - { - if( ( void * ) pxCreatedTask != NULL ) - { - /* Pass the TCB out - in an anonymous way. The calling function/ - task can use this as a handle to delete the task later if - required.*/ - *pxCreatedTask = ( xTaskHandle ) pxNewTCB; - } - - if( xSchedulerRunning != pdFALSE ) - { - /* If the created task is of a higher priority than the current task - then it should run now. */ - if( pxCurrentTCB->uxPriority < uxPriority ) - { - portYIELD_WITHIN_API(); - } - } - } - - return xReturn; -} -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_vTaskDelete == 1 ) - - void vTaskDelete( xTaskHandle pxTaskToDelete ) - { - tskTCB *pxTCB; - - portENTER_CRITICAL(); - { - /* Ensure a yield is performed if the current task is being - deleted. */ - if( pxTaskToDelete == pxCurrentTCB ) - { - pxTaskToDelete = NULL; - } - - /* If null is passed in here then we are deleting ourselves. */ - pxTCB = prvGetTCBFromHandle( pxTaskToDelete ); - - /* Remove task from the ready list and place in the termination list. - This will stop the task from be scheduled. The idle task will check - the termination list and free up any memory allocated by the - scheduler for the TCB and stack. */ - vListRemove( &( pxTCB->xGenericListItem ) ); - - /* Is the task waiting on an event also? */ - if( pxTCB->xEventListItem.pvContainer ) - { - vListRemove( &( pxTCB->xEventListItem ) ); - } - - vListInsertEnd( ( xList * ) &xTasksWaitingTermination, &( pxTCB->xGenericListItem ) ); - - /* Increment the ucTasksDeleted variable so the idle task knows - there is a task that has been deleted and that it should therefore - check the xTasksWaitingTermination list. */ - ++uxTasksDeleted; - - /* Increment the uxTaskNumberVariable also so kernel aware debuggers - can detect that the task lists need re-generating. */ - uxTaskNumber++; - - traceTASK_DELETE( pxTCB ); - } - portEXIT_CRITICAL(); - - /* Force a reschedule if we have just deleted the current task. */ - if( xSchedulerRunning != pdFALSE ) - { - if( ( void * ) pxTaskToDelete == NULL ) - { - portYIELD_WITHIN_API(); - } - } - } - -#endif - - - - - - -/*----------------------------------------------------------- - * TASK CONTROL API documented in task.h - *----------------------------------------------------------*/ - -#if ( INCLUDE_vTaskDelayUntil == 1 ) - - void vTaskDelayUntil( portTickType * const pxPreviousWakeTime, portTickType xTimeIncrement ) - { - portTickType xTimeToWake; - portBASE_TYPE xAlreadyYielded, xShouldDelay = pdFALSE; - - vTaskSuspendAll(); - { - /* Generate the tick time at which the task wants to wake. */ - xTimeToWake = *pxPreviousWakeTime + xTimeIncrement; - - if( xTickCount < *pxPreviousWakeTime ) - { - /* The tick count has overflowed since this function was - lasted called. In this case the only time we should ever - actually delay is if the wake time has also overflowed, - and the wake time is greater than the tick time. When this - is the case it is as if neither time had overflowed. */ - if( ( xTimeToWake < *pxPreviousWakeTime ) && ( xTimeToWake > xTickCount ) ) - { - xShouldDelay = pdTRUE; - } - } - else - { - /* The tick time has not overflowed. In this case we will - delay if either the wake time has overflowed, and/or the - tick time is less than the wake time. */ - if( ( xTimeToWake < *pxPreviousWakeTime ) || ( xTimeToWake > xTickCount ) ) - { - xShouldDelay = pdTRUE; - } - } - - /* Update the wake time ready for the next call. */ - *pxPreviousWakeTime = xTimeToWake; - - if( xShouldDelay ) - { - traceTASK_DELAY_UNTIL(); - - /* We must remove ourselves from the ready list before adding - ourselves to the blocked list as the same list item is used for - both lists. */ - vListRemove( ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - - /* The list item will be inserted in wake time order. */ - listSET_LIST_ITEM_VALUE( &( pxCurrentTCB->xGenericListItem ), xTimeToWake ); - - if( xTimeToWake < xTickCount ) - { - /* Wake time has overflowed. Place this item in the - overflow list. */ - vListInsert( ( xList * ) pxOverflowDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - } - else - { - /* The wake time has not overflowed, so we can use the - current block list. */ - vListInsert( ( xList * ) pxDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - } - } - } - xAlreadyYielded = xTaskResumeAll(); - - /* Force a reschedule if xTaskResumeAll has not already done so, we may - have put ourselves to sleep. */ - if( !xAlreadyYielded ) - { - portYIELD_WITHIN_API(); - } - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_vTaskDelay == 1 ) - - void vTaskDelay( portTickType xTicksToDelay ) - { - portTickType xTimeToWake; - signed portBASE_TYPE xAlreadyYielded = pdFALSE; - - /* A delay time of zero just forces a reschedule. */ - if( xTicksToDelay > ( portTickType ) 0 ) - { - vTaskSuspendAll(); - { - traceTASK_DELAY(); - - /* A task that is removed from the event list while the - scheduler is suspended will not get placed in the ready - list or removed from the blocked list until the scheduler - is resumed. - - This task cannot be in an event list as it is the currently - executing task. */ - - /* Calculate the time to wake - this may overflow but this is - not a problem. */ - xTimeToWake = xTickCount + xTicksToDelay; - - /* We must remove ourselves from the ready list before adding - ourselves to the blocked list as the same list item is used for - both lists. */ - vListRemove( ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - - /* The list item will be inserted in wake time order. */ - listSET_LIST_ITEM_VALUE( &( pxCurrentTCB->xGenericListItem ), xTimeToWake ); - - if( xTimeToWake < xTickCount ) - { - /* Wake time has overflowed. Place this item in the - overflow list. */ - vListInsert( ( xList * ) pxOverflowDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - } - else - { - /* The wake time has not overflowed, so we can use the - current block list. */ - vListInsert( ( xList * ) pxDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - } - } - xAlreadyYielded = xTaskResumeAll(); - } - - /* Force a reschedule if xTaskResumeAll has not already done so, we may - have put ourselves to sleep. */ - if( !xAlreadyYielded ) - { - portYIELD_WITHIN_API(); - } - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_uxTaskPriorityGet == 1 ) - - unsigned portBASE_TYPE uxTaskPriorityGet( xTaskHandle pxTask ) - { - tskTCB *pxTCB; - unsigned portBASE_TYPE uxReturn; - - portENTER_CRITICAL(); - { - /* If null is passed in here then we are changing the - priority of the calling function. */ - pxTCB = prvGetTCBFromHandle( pxTask ); - uxReturn = pxTCB->uxPriority; - } - portEXIT_CRITICAL(); - - return uxReturn; - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_vTaskPrioritySet == 1 ) - - void vTaskPrioritySet( xTaskHandle pxTask, unsigned portBASE_TYPE uxNewPriority ) - { - tskTCB *pxTCB; - unsigned portBASE_TYPE uxCurrentPriority, xYieldRequired = pdFALSE; - - /* Ensure the new priority is valid. */ - if( uxNewPriority >= configMAX_PRIORITIES ) - { - uxNewPriority = configMAX_PRIORITIES - 1; - } - - portENTER_CRITICAL(); - { - if( pxTask == pxCurrentTCB ) - { - pxTask = NULL; - } - - /* If null is passed in here then we are changing the - priority of the calling function. */ - pxTCB = prvGetTCBFromHandle( pxTask ); - - traceTASK_PRIORITY_SET( pxTask, uxNewPriority ); - - #if ( configUSE_MUTEXES == 1 ) - { - uxCurrentPriority = pxTCB->uxBasePriority; - } - #else - { - uxCurrentPriority = pxTCB->uxPriority; - } - #endif - - if( uxCurrentPriority != uxNewPriority ) - { - /* The priority change may have readied a task of higher - priority than the calling task. */ - if( uxNewPriority > uxCurrentPriority ) - { - if( pxTask != NULL ) - { - /* The priority of another task is being raised. If we - were raising the priority of the currently running task - there would be no need to switch as it must have already - been the highest priority task. */ - xYieldRequired = pdTRUE; - } - } - else if( pxTask == NULL ) - { - /* Setting our own priority down means there may now be another - task of higher priority that is ready to execute. */ - xYieldRequired = pdTRUE; - } - - - - #if ( configUSE_MUTEXES == 1 ) - { - /* Only change the priority being used if the task is not - currently using an inherited priority. */ - if( pxTCB->uxBasePriority == pxTCB->uxPriority ) - { - pxTCB->uxPriority = uxNewPriority; - } - - /* The base priority gets set whatever. */ - pxTCB->uxBasePriority = uxNewPriority; - } - #else - { - pxTCB->uxPriority = uxNewPriority; - } - #endif - - listSET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ), ( configMAX_PRIORITIES - ( portTickType ) uxNewPriority ) ); - - /* If the task is in the blocked or suspended list we need do - nothing more than change it's priority variable. However, if - the task is in a ready list it needs to be removed and placed - in the queue appropriate to its new priority. */ - if( listIS_CONTAINED_WITHIN( &( pxReadyTasksLists[ uxCurrentPriority ] ), &( pxTCB->xGenericListItem ) ) ) - { - /* The task is currently in its ready list - remove before adding - it to it's new ready list. As we are in a critical section we - can do this even if the scheduler is suspended. */ - vListRemove( &( pxTCB->xGenericListItem ) ); - prvAddTaskToReadyQueue( pxTCB ); - } - - if( xYieldRequired == pdTRUE ) - { - portYIELD_WITHIN_API(); - } - } - } - portEXIT_CRITICAL(); - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_vTaskSuspend == 1 ) - - void vTaskSuspend( xTaskHandle pxTaskToSuspend ) - { - tskTCB *pxTCB; - - portENTER_CRITICAL(); - { - /* Ensure a yield is performed if the current task is being - suspended. */ - if( pxTaskToSuspend == pxCurrentTCB ) - { - pxTaskToSuspend = NULL; - } - - /* If null is passed in here then we are suspending ourselves. */ - pxTCB = prvGetTCBFromHandle( pxTaskToSuspend ); - - traceTASK_SUSPEND( pxTCB ); - - /* Remove task from the ready/delayed list and place in the suspended list. */ - vListRemove( &( pxTCB->xGenericListItem ) ); - - /* Is the task waiting on an event also? */ - if( pxTCB->xEventListItem.pvContainer ) - { - vListRemove( &( pxTCB->xEventListItem ) ); - } - - vListInsertEnd( ( xList * ) &xSuspendedTaskList, &( pxTCB->xGenericListItem ) ); - } - portEXIT_CRITICAL(); - - /* We may have just suspended the current task. */ - if( ( void * ) pxTaskToSuspend == NULL ) - { - portYIELD_WITHIN_API(); - } - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_vTaskSuspend == 1 ) - - signed portBASE_TYPE xTaskIsTaskSuspended( xTaskHandle xTask ) - { - portBASE_TYPE xReturn = pdFALSE; - const tskTCB * const pxTCB = ( tskTCB * ) xTask; - - /* Is the task we are attempting to resume actually in the - suspended list? */ - if( listIS_CONTAINED_WITHIN( &xSuspendedTaskList, &( pxTCB->xGenericListItem ) ) != pdFALSE ) - { - /* Has the task already been resumed from within an ISR? */ - if( listIS_CONTAINED_WITHIN( &xPendingReadyList, &( pxTCB->xEventListItem ) ) != pdTRUE ) - { - /* Is it in the suspended list because it is in the - Suspended state? It is possible to be in the suspended - list because it is blocked on a task with no timeout - specified. */ - if( listIS_CONTAINED_WITHIN( NULL, &( pxTCB->xEventListItem ) ) == pdTRUE ) - { - xReturn = pdTRUE; - } - } - } - - return xReturn; - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_vTaskSuspend == 1 ) - - void vTaskResume( xTaskHandle pxTaskToResume ) - { - tskTCB *pxTCB; - - /* Remove the task from whichever list it is currently in, and place - it in the ready list. */ - pxTCB = ( tskTCB * ) pxTaskToResume; - - /* The parameter cannot be NULL as it is impossible to resume the - currently executing task. */ - if( ( pxTCB != NULL ) && ( pxTCB != pxCurrentTCB ) ) - { - portENTER_CRITICAL(); - { - if( xTaskIsTaskSuspended( pxTCB ) == pdTRUE ) - { - traceTASK_RESUME( pxTCB ); - - /* As we are in a critical section we can access the ready - lists even if the scheduler is suspended. */ - vListRemove( &( pxTCB->xGenericListItem ) ); - prvAddTaskToReadyQueue( pxTCB ); - - /* We may have just resumed a higher priority task. */ - if( pxTCB->uxPriority >= pxCurrentTCB->uxPriority ) - { - /* This yield may not cause the task just resumed to run, but - will leave the lists in the correct state for the next yield. */ - portYIELD_WITHIN_API(); - } - } - } - portEXIT_CRITICAL(); - } - } - -#endif - -/*-----------------------------------------------------------*/ - -#if ( ( INCLUDE_xTaskResumeFromISR == 1 ) && ( INCLUDE_vTaskSuspend == 1 ) ) - - portBASE_TYPE xTaskResumeFromISR( xTaskHandle pxTaskToResume ) - { - portBASE_TYPE xYieldRequired = pdFALSE; - tskTCB *pxTCB; - - pxTCB = ( tskTCB * ) pxTaskToResume; - - if( xTaskIsTaskSuspended( pxTCB ) == pdTRUE ) - { - traceTASK_RESUME_FROM_ISR( pxTCB ); - - if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) - { - xYieldRequired = ( pxTCB->uxPriority >= pxCurrentTCB->uxPriority ); - vListRemove( &( pxTCB->xGenericListItem ) ); - prvAddTaskToReadyQueue( pxTCB ); - } - else - { - /* We cannot access the delayed or ready lists, so will hold this - task pending until the scheduler is resumed, at which point a - yield will be performed if necessary. */ - vListInsertEnd( ( xList * ) &( xPendingReadyList ), &( pxTCB->xEventListItem ) ); - } - } - - return xYieldRequired; - } - -#endif - - - - -/*----------------------------------------------------------- - * PUBLIC SCHEDULER CONTROL documented in task.h - *----------------------------------------------------------*/ - - -void vTaskStartScheduler( void ) -{ -portBASE_TYPE xReturn; - - /* Add the idle task at the lowest priority. */ - xReturn = xTaskCreate( prvIdleTask, ( signed char * ) "IDLE", tskIDLE_STACK_SIZE, ( void * ) NULL, ( tskIDLE_PRIORITY | portPRIVILEGE_BIT ), ( xTaskHandle * ) NULL ); - - if( xReturn == pdPASS ) - { - /* Interrupts are turned off here, to ensure a tick does not occur - before or during the call to xPortStartScheduler(). The stacks of - the created tasks contain a status word with interrupts switched on - so interrupts will automatically get re-enabled when the first task - starts to run. - - STEPPING THROUGH HERE USING A DEBUGGER CAN CAUSE BIG PROBLEMS IF THE - DEBUGGER ALLOWS INTERRUPTS TO BE PROCESSED. */ - portDISABLE_INTERRUPTS(); - - xSchedulerRunning = pdTRUE; - xTickCount = ( portTickType ) 0; - - /* If configGENERATE_RUN_TIME_STATS is defined then the following - macro must be defined to configure the timer/counter used to generate - the run time counter time base. */ - portCONFIGURE_TIMER_FOR_RUN_TIME_STATS(); - - /* Setting up the timer tick is hardware specific and thus in the - portable interface. */ - if( xPortStartScheduler() ) - { - /* Should not reach here as if the scheduler is running the - function will not return. */ - } - else - { - /* Should only reach here if a task calls xTaskEndScheduler(). */ - } - } -} -/*-----------------------------------------------------------*/ - -void vTaskEndScheduler( void ) -{ - /* Stop the scheduler interrupts and call the portable scheduler end - routine so the original ISRs can be restored if necessary. The port - layer must ensure interrupts enable bit is left in the correct state. */ - portDISABLE_INTERRUPTS(); - xSchedulerRunning = pdFALSE; - vPortEndScheduler(); -} -/*----------------------------------------------------------*/ - -void vTaskSuspendAll( void ) -{ - /* A critical section is not required as the variable is of type - portBASE_TYPE. */ - ++uxSchedulerSuspended; -} -/*----------------------------------------------------------*/ - -signed portBASE_TYPE xTaskResumeAll( void ) -{ -register tskTCB *pxTCB; -signed portBASE_TYPE xAlreadyYielded = pdFALSE; - - /* It is possible that an ISR caused a task to be removed from an event - list while the scheduler was suspended. If this was the case then the - removed task will have been added to the xPendingReadyList. Once the - scheduler has been resumed it is safe to move all the pending ready - tasks from this list into their appropriate ready list. */ - portENTER_CRITICAL(); - { - --uxSchedulerSuspended; - - if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) - { - if( uxCurrentNumberOfTasks > ( unsigned portBASE_TYPE ) 0 ) - { - portBASE_TYPE xYieldRequired = pdFALSE; - - /* Move any readied tasks from the pending list into the - appropriate ready list. */ - while( ( pxTCB = ( tskTCB * ) listGET_OWNER_OF_HEAD_ENTRY( ( ( xList * ) &xPendingReadyList ) ) ) != NULL ) - { - vListRemove( &( pxTCB->xEventListItem ) ); - vListRemove( &( pxTCB->xGenericListItem ) ); - prvAddTaskToReadyQueue( pxTCB ); - - /* If we have moved a task that has a priority higher than - the current task then we should yield. */ - if( pxTCB->uxPriority >= pxCurrentTCB->uxPriority ) - { - xYieldRequired = pdTRUE; - } - } - - /* If any ticks occurred while the scheduler was suspended then - they should be processed now. This ensures the tick count does not - slip, and that any delayed tasks are resumed at the correct time. */ - if( uxMissedTicks > ( unsigned portBASE_TYPE ) 0 ) - { - while( uxMissedTicks > ( unsigned portBASE_TYPE ) 0 ) - { - vTaskIncrementTick(); - --uxMissedTicks; - } - - /* As we have processed some ticks it is appropriate to yield - to ensure the highest priority task that is ready to run is - the task actually running. */ - #if configUSE_PREEMPTION == 1 - { - xYieldRequired = pdTRUE; - } - #endif - } - - if( ( xYieldRequired == pdTRUE ) || ( xMissedYield == pdTRUE ) ) - { - xAlreadyYielded = pdTRUE; - xMissedYield = pdFALSE; - portYIELD_WITHIN_API(); - } - } - } - } - portEXIT_CRITICAL(); - - return xAlreadyYielded; -} - - - - - - -/*----------------------------------------------------------- - * PUBLIC TASK UTILITIES documented in task.h - *----------------------------------------------------------*/ - - - -portTickType xTaskGetTickCount( void ) -{ -portTickType xTicks; - - /* Critical section required if running on a 16 bit processor. */ - portENTER_CRITICAL(); - { - xTicks = xTickCount; - } - portEXIT_CRITICAL(); - - return xTicks; -} -/*-----------------------------------------------------------*/ - -unsigned portBASE_TYPE uxTaskGetNumberOfTasks( void ) -{ - /* A critical section is not required because the variables are of type - portBASE_TYPE. */ - return uxCurrentNumberOfTasks; -} -/*-----------------------------------------------------------*/ - -#if ( configUSE_TRACE_FACILITY == 1 ) - - void vTaskList( signed char *pcWriteBuffer ) - { - unsigned portBASE_TYPE uxQueue; - - /* This is a VERY costly function that should be used for debug only. - It leaves interrupts disabled for a LONG time. */ - - vTaskSuspendAll(); - { - /* Run through all the lists that could potentially contain a TCB and - report the task name, state and stack high water mark. */ - - pcWriteBuffer[ 0 ] = ( signed char ) 0x00; - strcat( ( char * ) pcWriteBuffer, ( const char * ) "\r\n" ); - - uxQueue = uxTopUsedPriority + 1; - - do - { - uxQueue--; - - if( !listLIST_IS_EMPTY( &( pxReadyTasksLists[ uxQueue ] ) ) ) - { - prvListTaskWithinSingleList( pcWriteBuffer, ( xList * ) &( pxReadyTasksLists[ uxQueue ] ), tskREADY_CHAR ); - } - }while( uxQueue > ( unsigned short ) tskIDLE_PRIORITY ); - - if( !listLIST_IS_EMPTY( pxDelayedTaskList ) ) - { - prvListTaskWithinSingleList( pcWriteBuffer, ( xList * ) pxDelayedTaskList, tskBLOCKED_CHAR ); - } - - if( !listLIST_IS_EMPTY( pxOverflowDelayedTaskList ) ) - { - prvListTaskWithinSingleList( pcWriteBuffer, ( xList * ) pxOverflowDelayedTaskList, tskBLOCKED_CHAR ); - } - - #if( INCLUDE_vTaskDelete == 1 ) - { - if( !listLIST_IS_EMPTY( &xTasksWaitingTermination ) ) - { - prvListTaskWithinSingleList( pcWriteBuffer, ( xList * ) &xTasksWaitingTermination, tskDELETED_CHAR ); - } - } - #endif - - #if ( INCLUDE_vTaskSuspend == 1 ) - { - if( !listLIST_IS_EMPTY( &xSuspendedTaskList ) ) - { - prvListTaskWithinSingleList( pcWriteBuffer, ( xList * ) &xSuspendedTaskList, tskSUSPENDED_CHAR ); - } - } - #endif - } - xTaskResumeAll(); - } - -#endif -/*----------------------------------------------------------*/ - -#if ( configGENERATE_RUN_TIME_STATS == 1 ) - - void vTaskGetRunTimeStats( signed char *pcWriteBuffer ) - { - unsigned portBASE_TYPE uxQueue; - unsigned long ulTotalRunTime = portGET_RUN_TIME_COUNTER_VALUE(); - - /* This is a VERY costly function that should be used for debug only. - It leaves interrupts disabled for a LONG time. */ - - vTaskSuspendAll(); - { - /* Run through all the lists that could potentially contain a TCB, - generating a table of run timer percentages in the provided - buffer. */ - - pcWriteBuffer[ 0 ] = ( signed char ) 0x00; - strcat( ( char * ) pcWriteBuffer, ( const char * ) "\r\n" ); - - uxQueue = uxTopUsedPriority + 1; - - do - { - uxQueue--; - - if( !listLIST_IS_EMPTY( &( pxReadyTasksLists[ uxQueue ] ) ) ) - { - prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, ( xList * ) &( pxReadyTasksLists[ uxQueue ] ), ulTotalRunTime ); - } - }while( uxQueue > ( unsigned short ) tskIDLE_PRIORITY ); - - if( !listLIST_IS_EMPTY( pxDelayedTaskList ) ) - { - prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, ( xList * ) pxDelayedTaskList, ulTotalRunTime ); - } - - if( !listLIST_IS_EMPTY( pxOverflowDelayedTaskList ) ) - { - prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, ( xList * ) pxOverflowDelayedTaskList, ulTotalRunTime ); - } - - #if ( INCLUDE_vTaskDelete == 1 ) - { - if( !listLIST_IS_EMPTY( &xTasksWaitingTermination ) ) - { - prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, ( xList * ) &xTasksWaitingTermination, ulTotalRunTime ); - } - } - #endif - - #if ( INCLUDE_vTaskSuspend == 1 ) - { - if( !listLIST_IS_EMPTY( &xSuspendedTaskList ) ) - { - prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, ( xList * ) &xSuspendedTaskList, ulTotalRunTime ); - } - } - #endif - } - xTaskResumeAll(); - } - -#endif -/*----------------------------------------------------------*/ - -#if ( configUSE_TRACE_FACILITY == 1 ) - - void vTaskStartTrace( signed char * pcBuffer, unsigned long ulBufferSize ) - { - portENTER_CRITICAL(); - { - pcTraceBuffer = ( signed char * )pcBuffer; - pcTraceBufferStart = pcBuffer; - pcTraceBufferEnd = pcBuffer + ( ulBufferSize - tskSIZE_OF_EACH_TRACE_LINE ); - xTracing = pdTRUE; - } - portEXIT_CRITICAL(); - } - -#endif -/*----------------------------------------------------------*/ - -#if ( configUSE_TRACE_FACILITY == 1 ) - - unsigned long ulTaskEndTrace( void ) - { - unsigned long ulBufferLength; - - portENTER_CRITICAL(); - xTracing = pdFALSE; - portEXIT_CRITICAL(); - - ulBufferLength = ( unsigned long ) ( pcTraceBuffer - pcTraceBufferStart ); - - return ulBufferLength; - } - -#endif - - - -/*----------------------------------------------------------- - * SCHEDULER INTERNALS AVAILABLE FOR PORTING PURPOSES - * documented in task.h - *----------------------------------------------------------*/ - - -void vTaskIncrementTick( void ) -{ - /* Called by the portable layer each time a tick interrupt occurs. - Increments the tick then checks to see if the new tick value will cause any - tasks to be unblocked. */ - if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) - { - ++xTickCount; - if( xTickCount == ( portTickType ) 0 ) - { - xList *pxTemp; - - /* Tick count has overflowed so we need to swap the delay lists. - If there are any items in pxDelayedTaskList here then there is - an error! */ - pxTemp = pxDelayedTaskList; - pxDelayedTaskList = pxOverflowDelayedTaskList; - pxOverflowDelayedTaskList = pxTemp; - xNumOfOverflows++; - } - - /* See if this tick has made a timeout expire. */ - prvCheckDelayedTasks(); - } - else - { - ++uxMissedTicks; - - /* The tick hook gets called at regular intervals, even if the - scheduler is locked. */ - #if ( configUSE_TICK_HOOK == 1 ) - { - extern void vApplicationTickHook( void ); - - vApplicationTickHook(); - } - #endif - } - - #if ( configUSE_TICK_HOOK == 1 ) - { - extern void vApplicationTickHook( void ); - - /* Guard against the tick hook being called when the missed tick - count is being unwound (when the scheduler is being unlocked. */ - if( uxMissedTicks == 0 ) - { - vApplicationTickHook(); - } - } - #endif - - traceTASK_INCREMENT_TICK( xTickCount ); -} -/*-----------------------------------------------------------*/ - -#if ( ( INCLUDE_vTaskCleanUpResources == 1 ) && ( INCLUDE_vTaskSuspend == 1 ) ) - - void vTaskCleanUpResources( void ) - { - unsigned short usQueue; - volatile tskTCB *pxTCB; - - usQueue = ( unsigned short ) uxTopUsedPriority + ( unsigned short ) 1; - - /* Remove any TCB's from the ready queues. */ - do - { - usQueue--; - - while( !listLIST_IS_EMPTY( &( pxReadyTasksLists[ usQueue ] ) ) ) - { - listGET_OWNER_OF_NEXT_ENTRY( pxTCB, &( pxReadyTasksLists[ usQueue ] ) ); - vListRemove( ( xListItem * ) &( pxTCB->xGenericListItem ) ); - - prvDeleteTCB( ( tskTCB * ) pxTCB ); - } - }while( usQueue > ( unsigned short ) tskIDLE_PRIORITY ); - - /* Remove any TCB's from the delayed queue. */ - while( !listLIST_IS_EMPTY( &xDelayedTaskList1 ) ) - { - listGET_OWNER_OF_NEXT_ENTRY( pxTCB, &xDelayedTaskList1 ); - vListRemove( ( xListItem * ) &( pxTCB->xGenericListItem ) ); - - prvDeleteTCB( ( tskTCB * ) pxTCB ); - } - - /* Remove any TCB's from the overflow delayed queue. */ - while( !listLIST_IS_EMPTY( &xDelayedTaskList2 ) ) - { - listGET_OWNER_OF_NEXT_ENTRY( pxTCB, &xDelayedTaskList2 ); - vListRemove( ( xListItem * ) &( pxTCB->xGenericListItem ) ); - - prvDeleteTCB( ( tskTCB * ) pxTCB ); - } - - while( !listLIST_IS_EMPTY( &xSuspendedTaskList ) ) - { - listGET_OWNER_OF_NEXT_ENTRY( pxTCB, &xSuspendedTaskList ); - vListRemove( ( xListItem * ) &( pxTCB->xGenericListItem ) ); - - prvDeleteTCB( ( tskTCB * ) pxTCB ); - } - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( configUSE_APPLICATION_TASK_TAG == 1 ) - - void vTaskSetApplicationTaskTag( xTaskHandle xTask, pdTASK_HOOK_CODE pxTagValue ) - { - tskTCB *xTCB; - - /* If xTask is NULL then we are setting our own task hook. */ - if( xTask == NULL ) - { - xTCB = ( tskTCB * ) pxCurrentTCB; - } - else - { - xTCB = ( tskTCB * ) xTask; - } - - /* Save the hook function in the TCB. A critical section is required as - the value can be accessed from an interrupt. */ - portENTER_CRITICAL(); - xTCB->pxTaskTag = pxTagValue; - portEXIT_CRITICAL(); - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( configUSE_APPLICATION_TASK_TAG == 1 ) - - pdTASK_HOOK_CODE xTaskGetApplicationTaskTag( xTaskHandle xTask ) - { - tskTCB *xTCB; - pdTASK_HOOK_CODE xReturn; - - /* If xTask is NULL then we are setting our own task hook. */ - if( xTask == NULL ) - { - xTCB = ( tskTCB * ) pxCurrentTCB; - } - else - { - xTCB = ( tskTCB * ) xTask; - } - - /* Save the hook function in the TCB. A critical section is required as - the value can be accessed from an interrupt. */ - portENTER_CRITICAL(); - xReturn = xTCB->pxTaskTag; - portEXIT_CRITICAL(); - - return xReturn; - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( configUSE_APPLICATION_TASK_TAG == 1 ) - - portBASE_TYPE xTaskCallApplicationTaskHook( xTaskHandle xTask, void *pvParameter ) - { - tskTCB *xTCB; - portBASE_TYPE xReturn; - - /* If xTask is NULL then we are calling our own task hook. */ - if( xTask == NULL ) - { - xTCB = ( tskTCB * ) pxCurrentTCB; - } - else - { - xTCB = ( tskTCB * ) xTask; - } - - if( xTCB->pxTaskTag != NULL ) - { - xReturn = xTCB->pxTaskTag( pvParameter ); - } - else - { - xReturn = pdFAIL; - } - - return xReturn; - } - -#endif -/*-----------------------------------------------------------*/ - -void vTaskSwitchContext( void ) -{ - if( uxSchedulerSuspended != ( unsigned portBASE_TYPE ) pdFALSE ) - { - /* The scheduler is currently suspended - do not allow a context - switch. */ - xMissedYield = pdTRUE; - return; - } - - traceTASK_SWITCHED_OUT(); - - #if ( configGENERATE_RUN_TIME_STATS == 1 ) - { - unsigned long ulTempCounter = portGET_RUN_TIME_COUNTER_VALUE(); - - /* Add the amount of time the task has been running to the accumulated - time so far. The time the task started running was stored in - ulTaskSwitchedInTime. Note that there is no overflow protection here - so count values are only valid until the timer overflows. Generally - this will be about 1 hour assuming a 1uS timer increment. */ - pxCurrentTCB->ulRunTimeCounter += ( ulTempCounter - ulTaskSwitchedInTime ); - ulTaskSwitchedInTime = ulTempCounter; - } - #endif - - taskFIRST_CHECK_FOR_STACK_OVERFLOW(); - taskSECOND_CHECK_FOR_STACK_OVERFLOW(); - - /* Find the highest priority queue that contains ready tasks. */ - while( listLIST_IS_EMPTY( &( pxReadyTasksLists[ uxTopReadyPriority ] ) ) ) - { - --uxTopReadyPriority; - } - - /* listGET_OWNER_OF_NEXT_ENTRY walks through the list, so the tasks of the - same priority get an equal share of the processor time. */ - listGET_OWNER_OF_NEXT_ENTRY( pxCurrentTCB, &( pxReadyTasksLists[ uxTopReadyPriority ] ) ); - - traceTASK_SWITCHED_IN(); - vWriteTraceToBuffer(); -} -/*-----------------------------------------------------------*/ - -void vTaskPlaceOnEventList( const xList * const pxEventList, portTickType xTicksToWait ) -{ -portTickType xTimeToWake; - - /* THIS FUNCTION MUST BE CALLED WITH INTERRUPTS DISABLED OR THE - SCHEDULER SUSPENDED. */ - - /* Place the event list item of the TCB in the appropriate event list. - This is placed in the list in priority order so the highest priority task - is the first to be woken by the event. */ - vListInsert( ( xList * ) pxEventList, ( xListItem * ) &( pxCurrentTCB->xEventListItem ) ); - - /* We must remove ourselves from the ready list before adding ourselves - to the blocked list as the same list item is used for both lists. We have - exclusive access to the ready lists as the scheduler is locked. */ - vListRemove( ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - - - #if ( INCLUDE_vTaskSuspend == 1 ) - { - if( xTicksToWait == portMAX_DELAY ) - { - /* Add ourselves to the suspended task list instead of a delayed task - list to ensure we are not woken by a timing event. We will block - indefinitely. */ - vListInsertEnd( ( xList * ) &xSuspendedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - } - else - { - /* Calculate the time at which the task should be woken if the event does - not occur. This may overflow but this doesn't matter. */ - xTimeToWake = xTickCount + xTicksToWait; - - listSET_LIST_ITEM_VALUE( &( pxCurrentTCB->xGenericListItem ), xTimeToWake ); - - if( xTimeToWake < xTickCount ) - { - /* Wake time has overflowed. Place this item in the overflow list. */ - vListInsert( ( xList * ) pxOverflowDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - } - else - { - /* The wake time has not overflowed, so we can use the current block list. */ - vListInsert( ( xList * ) pxDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - } - } - } - #else - { - /* Calculate the time at which the task should be woken if the event does - not occur. This may overflow but this doesn't matter. */ - xTimeToWake = xTickCount + xTicksToWait; - - listSET_LIST_ITEM_VALUE( &( pxCurrentTCB->xGenericListItem ), xTimeToWake ); - - if( xTimeToWake < xTickCount ) - { - /* Wake time has overflowed. Place this item in the overflow list. */ - vListInsert( ( xList * ) pxOverflowDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - } - else - { - /* The wake time has not overflowed, so we can use the current block list. */ - vListInsert( ( xList * ) pxDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - } - } - #endif -} -/*-----------------------------------------------------------*/ - -signed portBASE_TYPE xTaskRemoveFromEventList( const xList * const pxEventList ) -{ -tskTCB *pxUnblockedTCB; -portBASE_TYPE xReturn; - - /* THIS FUNCTION MUST BE CALLED WITH INTERRUPTS DISABLED OR THE - SCHEDULER SUSPENDED. It can also be called from within an ISR. */ - - /* The event list is sorted in priority order, so we can remove the - first in the list, remove the TCB from the delayed list, and add - it to the ready list. - - If an event is for a queue that is locked then this function will never - get called - the lock count on the queue will get modified instead. This - means we can always expect exclusive access to the event list here. */ - pxUnblockedTCB = ( tskTCB * ) listGET_OWNER_OF_HEAD_ENTRY( pxEventList ); - vListRemove( &( pxUnblockedTCB->xEventListItem ) ); - - if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) - { - vListRemove( &( pxUnblockedTCB->xGenericListItem ) ); - prvAddTaskToReadyQueue( pxUnblockedTCB ); - } - else - { - /* We cannot access the delayed or ready lists, so will hold this - task pending until the scheduler is resumed. */ - vListInsertEnd( ( xList * ) &( xPendingReadyList ), &( pxUnblockedTCB->xEventListItem ) ); - } - - if( pxUnblockedTCB->uxPriority >= pxCurrentTCB->uxPriority ) - { - /* Return true if the task removed from the event list has - a higher priority than the calling task. This allows - the calling task to know if it should force a context - switch now. */ - xReturn = pdTRUE; - } - else - { - xReturn = pdFALSE; - } - - return xReturn; -} -/*-----------------------------------------------------------*/ - -void vTaskSetTimeOutState( xTimeOutType * const pxTimeOut ) -{ - pxTimeOut->xOverflowCount = xNumOfOverflows; - pxTimeOut->xTimeOnEntering = xTickCount; -} -/*-----------------------------------------------------------*/ - -portBASE_TYPE xTaskCheckForTimeOut( xTimeOutType * const pxTimeOut, portTickType * const pxTicksToWait ) -{ -portBASE_TYPE xReturn; - - portENTER_CRITICAL(); - { - #if ( INCLUDE_vTaskSuspend == 1 ) - /* If INCLUDE_vTaskSuspend is set to 1 and the block time specified is - the maximum block time then the task should block indefinitely, and - therefore never time out. */ - if( *pxTicksToWait == portMAX_DELAY ) - { - xReturn = pdFALSE; - } - else /* We are not blocking indefinitely, perform the checks below. */ - #endif - - if( ( xNumOfOverflows != pxTimeOut->xOverflowCount ) && ( ( portTickType ) xTickCount >= ( portTickType ) pxTimeOut->xTimeOnEntering ) ) - { - /* The tick count is greater than the time at which vTaskSetTimeout() - was called, but has also overflowed since vTaskSetTimeOut() was called. - It must have wrapped all the way around and gone past us again. This - passed since vTaskSetTimeout() was called. */ - xReturn = pdTRUE; - } - else if( ( ( portTickType ) ( ( portTickType ) xTickCount - ( portTickType ) pxTimeOut->xTimeOnEntering ) ) < ( portTickType ) *pxTicksToWait ) - { - /* Not a genuine timeout. Adjust parameters for time remaining. */ - *pxTicksToWait -= ( ( portTickType ) xTickCount - ( portTickType ) pxTimeOut->xTimeOnEntering ); - vTaskSetTimeOutState( pxTimeOut ); - xReturn = pdFALSE; - } - else - { - xReturn = pdTRUE; - } - } - portEXIT_CRITICAL(); - - return xReturn; -} -/*-----------------------------------------------------------*/ - -void vTaskMissedYield( void ) -{ - xMissedYield = pdTRUE; -} - -/* - * ----------------------------------------------------------- - * The Idle task. - * ---------------------------------------------------------- - * - * The portTASK_FUNCTION() macro is used to allow port/compiler specific - * language extensions. The equivalent prototype for this function is: - * - * void prvIdleTask( void *pvParameters ); - * - */ -static portTASK_FUNCTION( prvIdleTask, pvParameters ) -{ - /* Stop warnings. */ - ( void ) pvParameters; - - for( ;; ) - { - /* See if any tasks have been deleted. */ - prvCheckTasksWaitingTermination(); - - #if ( configUSE_PREEMPTION == 0 ) - { - /* If we are not using preemption we keep forcing a task switch to - see if any other task has become available. If we are using - preemption we don't need to do this as any task becoming available - will automatically get the processor anyway. */ - taskYIELD(); - } - #endif - - #if ( ( configUSE_PREEMPTION == 1 ) && ( configIDLE_SHOULD_YIELD == 1 ) ) - { - /* When using preemption tasks of equal priority will be - timesliced. If a task that is sharing the idle priority is ready - to run then the idle task should yield before the end of the - timeslice. - - A critical region is not required here as we are just reading from - the list, and an occasional incorrect value will not matter. If - the ready list at the idle priority contains more than one task - then a task other than the idle task is ready to execute. */ - if( listCURRENT_LIST_LENGTH( &( pxReadyTasksLists[ tskIDLE_PRIORITY ] ) ) > ( unsigned portBASE_TYPE ) 1 ) - { - taskYIELD(); - } - } - #endif - - #if ( configUSE_IDLE_HOOK == 1 ) - { - extern void vApplicationIdleHook( void ); - - /* Call the user defined function from within the idle task. This - allows the application designer to add background functionality - without the overhead of a separate task. - NOTE: vApplicationIdleHook() MUST NOT, UNDER ANY CIRCUMSTANCES, - CALL A FUNCTION THAT MIGHT BLOCK. */ - vApplicationIdleHook(); - } - #endif - // call nanosleep for smalles sleep time possible - // (depending on kernel settings - around 100 microseconds) - // decreases idle thread CPU load from 100 to practically 0 - struct timespec x; - x.tv_sec=1; - x.tv_nsec=0; - nanosleep(&x,NULL); - } -} /*lint !e715 pvParameters is not accessed but all task functions require the same prototype. */ - - - - - - - -/*----------------------------------------------------------- - * File private functions documented at the top of the file. - *----------------------------------------------------------*/ - - - -static void prvInitialiseTCBVariables( tskTCB *pxTCB, const signed char * const pcName, unsigned portBASE_TYPE uxPriority, const xMemoryRegion * const xRegions, unsigned short usStackDepth ) -{ - /* Store the function name in the TCB. */ - #if configMAX_TASK_NAME_LEN > 1 - { - /* Don't bring strncpy into the build unnecessarily. */ - strncpy( ( char * ) pxTCB->pcTaskName, ( const char * ) pcName, ( unsigned short ) configMAX_TASK_NAME_LEN ); - } - #endif - pxTCB->pcTaskName[ ( unsigned short ) configMAX_TASK_NAME_LEN - ( unsigned short ) 1 ] = '\0'; - - /* This is used as an array index so must ensure it's not too large. First - remove the privilege bit if one is present. */ - if( uxPriority >= configMAX_PRIORITIES ) - { - uxPriority = configMAX_PRIORITIES - 1; - } - - pxTCB->uxPriority = uxPriority; - #if ( configUSE_MUTEXES == 1 ) - { - pxTCB->uxBasePriority = uxPriority; - } - #endif - - vListInitialiseItem( &( pxTCB->xGenericListItem ) ); - vListInitialiseItem( &( pxTCB->xEventListItem ) ); - - /* Set the pxTCB as a link back from the xListItem. This is so we can get - back to the containing TCB from a generic item in a list. */ - listSET_LIST_ITEM_OWNER( &( pxTCB->xGenericListItem ), pxTCB ); - - /* Event lists are always in priority order. */ - listSET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ), configMAX_PRIORITIES - ( portTickType ) uxPriority ); - listSET_LIST_ITEM_OWNER( &( pxTCB->xEventListItem ), pxTCB ); - - #if ( portCRITICAL_NESTING_IN_TCB == 1 ) - { - pxTCB->uxCriticalNesting = ( unsigned portBASE_TYPE ) 0; - } - #endif - - #if ( configUSE_APPLICATION_TASK_TAG == 1 ) - { - pxTCB->pxTaskTag = NULL; - } - #endif - - #if ( configGENERATE_RUN_TIME_STATS == 1 ) - { - pxTCB->ulRunTimeCounter = 0UL; - } - #endif - - #if ( portUSING_MPU_WRAPPERS == 1 ) - { - vPortStoreTaskMPUSettings( &( pxTCB->xMPUSettings ), xRegions, pxTCB->pxStack, usStackDepth ); - } - #else - { - ( void ) xRegions; - ( void ) usStackDepth; - } - #endif -} -/*-----------------------------------------------------------*/ - -#if ( portUSING_MPU_WRAPPERS == 1 ) - - void vTaskAllocateMPURegions( xTaskHandle xTaskToModify, const xMemoryRegion * const xRegions ) - { - tskTCB *pxTCB; - - if( xTaskToModify == pxCurrentTCB ) - { - xTaskToModify = NULL; - } - - /* If null is passed in here then we are deleting ourselves. */ - pxTCB = prvGetTCBFromHandle( xTaskToModify ); - - vPortStoreTaskMPUSettings( &( pxTCB->xMPUSettings ), xRegions, NULL, 0 ); - } - /*-----------------------------------------------------------*/ -#endif - -static void prvInitialiseTaskLists( void ) -{ -unsigned portBASE_TYPE uxPriority; - - for( uxPriority = 0; uxPriority < configMAX_PRIORITIES; uxPriority++ ) - { - vListInitialise( ( xList * ) &( pxReadyTasksLists[ uxPriority ] ) ); - } - - vListInitialise( ( xList * ) &xDelayedTaskList1 ); - vListInitialise( ( xList * ) &xDelayedTaskList2 ); - vListInitialise( ( xList * ) &xPendingReadyList ); - - #if ( INCLUDE_vTaskDelete == 1 ) - { - vListInitialise( ( xList * ) &xTasksWaitingTermination ); - } - #endif - - #if ( INCLUDE_vTaskSuspend == 1 ) - { - vListInitialise( ( xList * ) &xSuspendedTaskList ); - } - #endif - - /* Start with pxDelayedTaskList using list1 and the pxOverflowDelayedTaskList - using list2. */ - pxDelayedTaskList = &xDelayedTaskList1; - pxOverflowDelayedTaskList = &xDelayedTaskList2; -} -/*-----------------------------------------------------------*/ - -static void prvCheckTasksWaitingTermination( void ) -{ - #if ( INCLUDE_vTaskDelete == 1 ) - { - portBASE_TYPE xListIsEmpty; - - /* ucTasksDeleted is used to prevent vTaskSuspendAll() being called - too often in the idle task. */ - if( uxTasksDeleted > ( unsigned portBASE_TYPE ) 0 ) - { - vTaskSuspendAll(); - xListIsEmpty = listLIST_IS_EMPTY( &xTasksWaitingTermination ); - xTaskResumeAll(); - - if( !xListIsEmpty ) - { - tskTCB *pxTCB; - - portENTER_CRITICAL(); - { - pxTCB = ( tskTCB * ) listGET_OWNER_OF_HEAD_ENTRY( ( ( xList * ) &xTasksWaitingTermination ) ); - vListRemove( &( pxTCB->xGenericListItem ) ); - --uxCurrentNumberOfTasks; - --uxTasksDeleted; - } - portEXIT_CRITICAL(); - - prvDeleteTCB( pxTCB ); - } - } - } - #endif -} -/*-----------------------------------------------------------*/ - -static tskTCB *prvAllocateTCBAndStack( unsigned short usStackDepth, portSTACK_TYPE *puxStackBuffer ) -{ -tskTCB *pxNewTCB; - - /* Allocate space for the TCB. Where the memory comes from depends on - the implementation of the port malloc function. */ - pxNewTCB = ( tskTCB * ) pvPortMalloc( sizeof( tskTCB ) ); - - if( pxNewTCB != NULL ) - { - /* Allocate space for the stack used by the task being created. - The base of the stack memory stored in the TCB so the task can - be deleted later if required. */ - pxNewTCB->pxStack = ( portSTACK_TYPE * ) pvPortMallocAligned( ( ( ( size_t )usStackDepth ) * sizeof( portSTACK_TYPE ) ), puxStackBuffer ); - - if( pxNewTCB->pxStack == NULL ) - { - /* Could not allocate the stack. Delete the allocated TCB. */ - vPortFree( pxNewTCB ); - pxNewTCB = NULL; - } - else - { - /* Just to help debugging. */ - memset( pxNewTCB->pxStack, tskSTACK_FILL_BYTE, usStackDepth * sizeof( portSTACK_TYPE ) ); - } - } - - return pxNewTCB; -} -/*-----------------------------------------------------------*/ - -#if ( configUSE_TRACE_FACILITY == 1 ) - - static void prvListTaskWithinSingleList( const signed char *pcWriteBuffer, xList *pxList, signed char cStatus ) - { - volatile tskTCB *pxNextTCB, *pxFirstTCB; - unsigned short usStackRemaining; - - /* Write the details of all the TCB's in pxList into the buffer. */ - listGET_OWNER_OF_NEXT_ENTRY( pxFirstTCB, pxList ); - do - { - listGET_OWNER_OF_NEXT_ENTRY( pxNextTCB, pxList ); - #if ( portSTACK_GROWTH > 0 ) - { - usStackRemaining = usTaskCheckFreeStackSpace( ( unsigned char * ) pxNextTCB->pxEndOfStack ); - } - #else - { - usStackRemaining = usTaskCheckFreeStackSpace( ( unsigned char * ) pxNextTCB->pxStack ); - } - #endif - - sprintf( pcStatusString, ( char * ) "%s\t\t%c\t%u\t%u\t%u\r\n", pxNextTCB->pcTaskName, cStatus, ( unsigned int ) pxNextTCB->uxPriority, usStackRemaining, ( unsigned int ) pxNextTCB->uxTCBNumber ); - strcat( ( char * ) pcWriteBuffer, ( char * ) pcStatusString ); - - } while( pxNextTCB != pxFirstTCB ); - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( configGENERATE_RUN_TIME_STATS == 1 ) - - static void prvGenerateRunTimeStatsForTasksInList( const signed char *pcWriteBuffer, xList *pxList, unsigned long ulTotalRunTime ) - { - volatile tskTCB *pxNextTCB, *pxFirstTCB; - unsigned long ulStatsAsPercentage; - - /* Write the run time stats of all the TCB's in pxList into the buffer. */ - listGET_OWNER_OF_NEXT_ENTRY( pxFirstTCB, pxList ); - do - { - /* Get next TCB in from the list. */ - listGET_OWNER_OF_NEXT_ENTRY( pxNextTCB, pxList ); - - /* Divide by zero check. */ - if( ulTotalRunTime > 0UL ) - { - /* Has the task run at all? */ - if( pxNextTCB->ulRunTimeCounter == 0 ) - { - /* The task has used no CPU time at all. */ - sprintf( pcStatsString, ( char * ) "%s\t\t0\t\t0%%\r\n", pxNextTCB->pcTaskName ); - } - else - { - /* What percentage of the total run time as the task used? - This will always be rounded down to the nearest integer. */ - ulStatsAsPercentage = ( 100UL * pxNextTCB->ulRunTimeCounter ) / ulTotalRunTime; - - if( ulStatsAsPercentage > 0UL ) - { - sprintf( pcStatsString, ( char * ) "%s\t\t%u\t\t%u%%\r\n", pxNextTCB->pcTaskName, ( unsigned int ) pxNextTCB->ulRunTimeCounter, ( unsigned int ) ulStatsAsPercentage ); - } - else - { - /* If the percentage is zero here then the task has - consumed less than 1% of the total run time. */ - sprintf( pcStatsString, ( char * ) "%s\t\t%u\t\t<1%%\r\n", pxNextTCB->pcTaskName, ( unsigned int ) pxNextTCB->ulRunTimeCounter ); - } - } - - strcat( ( char * ) pcWriteBuffer, ( char * ) pcStatsString ); - } - - } while( pxNextTCB != pxFirstTCB ); - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( ( configUSE_TRACE_FACILITY == 1 ) || ( INCLUDE_uxTaskGetStackHighWaterMark == 1 ) ) - - static unsigned short usTaskCheckFreeStackSpace( const unsigned char * pucStackByte ) - { - register unsigned short usCount = 0; - - while( *pucStackByte == tskSTACK_FILL_BYTE ) - { - pucStackByte -= portSTACK_GROWTH; - usCount++; - } - - usCount /= sizeof( portSTACK_TYPE ); - - return usCount; - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_uxTaskGetStackHighWaterMark == 1 ) - - unsigned portBASE_TYPE uxTaskGetStackHighWaterMark( xTaskHandle xTask ) - { - tskTCB *pxTCB; - unsigned char *pcEndOfStack; - unsigned portBASE_TYPE uxReturn; - - pxTCB = prvGetTCBFromHandle( xTask ); - - #if portSTACK_GROWTH < 0 - { - pcEndOfStack = ( unsigned char * ) pxTCB->pxStack; - } - #else - { - pcEndOfStack = ( unsigned char * ) pxTCB->pxEndOfStack; - } - #endif - - uxReturn = ( unsigned portBASE_TYPE ) usTaskCheckFreeStackSpace( pcEndOfStack ); - - return uxReturn; - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( ( INCLUDE_vTaskDelete == 1 ) || ( INCLUDE_vTaskCleanUpResources == 1 ) ) - - static void prvDeleteTCB( tskTCB *pxTCB ) - { - /* Free up the memory allocated by the scheduler for the task. It is up to - the task to free any memory allocated at the application level. */ - vPortFreeAligned( pxTCB->pxStack ); - vPortFree( pxTCB ); - } - -#endif - - -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_xTaskGetCurrentTaskHandle == 1 ) - - xTaskHandle xTaskGetCurrentTaskHandle( void ) - { - xTaskHandle xReturn; - - /* A critical section is not required as this is not called from - an interrupt and the current TCB will always be the same for any - individual execution thread. */ - xReturn = pxCurrentTCB; - - return xReturn; - } - -#endif - -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_xTaskGetSchedulerState == 1 ) - - portBASE_TYPE xTaskGetSchedulerState( void ) - { - portBASE_TYPE xReturn; - - if( xSchedulerRunning == pdFALSE ) - { - xReturn = taskSCHEDULER_NOT_STARTED; - } - else - { - if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) - { - xReturn = taskSCHEDULER_RUNNING; - } - else - { - xReturn = taskSCHEDULER_SUSPENDED; - } - } - - return xReturn; - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( configUSE_MUTEXES == 1 ) - - void vTaskPriorityInherit( xTaskHandle * const pxMutexHolder ) - { - tskTCB * const pxTCB = ( tskTCB * ) pxMutexHolder; - - if( pxTCB->uxPriority < pxCurrentTCB->uxPriority ) - { - /* Adjust the mutex holder state to account for its new priority. */ - listSET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ), configMAX_PRIORITIES - ( portTickType ) pxCurrentTCB->uxPriority ); - - /* If the task being modified is in the ready state it will need to - be moved in to a new list. */ - if( listIS_CONTAINED_WITHIN( &( pxReadyTasksLists[ pxTCB->uxPriority ] ), &( pxTCB->xGenericListItem ) ) ) - { - vListRemove( &( pxTCB->xGenericListItem ) ); - - /* Inherit the priority before being moved into the new list. */ - pxTCB->uxPriority = pxCurrentTCB->uxPriority; - prvAddTaskToReadyQueue( pxTCB ); - } - else - { - /* Just inherit the priority. */ - pxTCB->uxPriority = pxCurrentTCB->uxPriority; - } - } - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( configUSE_MUTEXES == 1 ) - - void vTaskPriorityDisinherit( xTaskHandle * const pxMutexHolder ) - { - tskTCB * const pxTCB = ( tskTCB * ) pxMutexHolder; - - if( pxMutexHolder != NULL ) - { - if( pxTCB->uxPriority != pxTCB->uxBasePriority ) - { - /* We must be the running task to be able to give the mutex back. - Remove ourselves from the ready list we currently appear in. */ - vListRemove( &( pxTCB->xGenericListItem ) ); - - /* Disinherit the priority before adding ourselves into the new - ready list. */ - pxTCB->uxPriority = pxTCB->uxBasePriority; - listSET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ), configMAX_PRIORITIES - ( portTickType ) pxTCB->uxPriority ); - prvAddTaskToReadyQueue( pxTCB ); - } - } - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( portCRITICAL_NESTING_IN_TCB == 1 ) - - void vTaskEnterCritical( void ) - { - portDISABLE_INTERRUPTS(); - - if( xSchedulerRunning != pdFALSE ) - { - pxCurrentTCB->uxCriticalNesting++; - } - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( portCRITICAL_NESTING_IN_TCB == 1 ) - -void vTaskExitCritical( void ) -{ - if( xSchedulerRunning != pdFALSE ) - { - if( pxCurrentTCB->uxCriticalNesting > 0 ) - { - pxCurrentTCB->uxCriticalNesting--; - - if( pxCurrentTCB->uxCriticalNesting == 0 ) - { - portENABLE_INTERRUPTS(); - } - } - } -} - -#endif -/*-----------------------------------------------------------*/ - - - - +/* + FreeRTOS V6.0.4 - Copyright (C) 2010 Real Time Engineers Ltd. + + *************************************************************************** + * * + * If you are: * + * * + * + New to FreeRTOS, * + * + Wanting to learn FreeRTOS or multitasking in general quickly * + * + Looking for basic training, * + * + Wanting to improve your FreeRTOS skills and productivity * + * * + * then take a look at the FreeRTOS eBook * + * * + * "Using the FreeRTOS Real Time Kernel - a Practical Guide" * + * http://www.FreeRTOS.org/Documentation * + * * + * A pdf reference manual is also available. Both are usually delivered * + * to your inbox within 20 minutes to two hours when purchased between 8am * + * and 8pm GMT (although please allow up to 24 hours in case of * + * exceptional circumstances). Thank you for your support! * + * * + *************************************************************************** + + This file is part of the FreeRTOS distribution. + + FreeRTOS is free software; you can redistribute it and/or modify it under + the terms of the GNU General Public License (version 2) as published by the + Free Software Foundation AND MODIFIED BY the FreeRTOS exception. + ***NOTE*** The exception to the GPL is included to allow you to distribute + a combined work that includes FreeRTOS without being obliged to provide the + source code for proprietary components outside of the FreeRTOS kernel. + FreeRTOS is distributed in the hope that it will be useful, but WITHOUT + ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. You should have received a copy of the GNU General Public + License and the FreeRTOS license exception along with FreeRTOS; if not it + can be viewed here: http://www.freertos.org/a00114.html and also obtained + by writing to Richard Barry, contact details for whom are available on the + FreeRTOS WEB site. + + 1 tab == 4 spaces! + + http://www.FreeRTOS.org - Documentation, latest information, license and + contact details. + + http://www.SafeRTOS.com - A version that is certified for use in safety + critical systems. + + http://www.OpenRTOS.com - Commercial support, development, porting, + licensing and training services. +*/ + + +#include +#include +#include +#include + +/* Defining MPU_WRAPPERS_INCLUDED_FROM_API_FILE prevents task.h from redefining +all the API functions to use the MPU wrappers. That should only be done when +task.h is included from an application file. */ +#define MPU_WRAPPERS_INCLUDED_FROM_API_FILE + +#include "FreeRTOS.h" +#include "task.h" +#include "StackMacros.h" + +#undef MPU_WRAPPERS_INCLUDED_FROM_API_FILE + +/* + * Macro to define the amount of stack available to the idle task. + */ +#define tskIDLE_STACK_SIZE configMINIMAL_STACK_SIZE + +/* + * Task control block. A task control block (TCB) is allocated to each task, + * and stores the context of the task. + */ +typedef struct tskTaskControlBlock +{ + volatile portSTACK_TYPE *pxTopOfStack; /*< Points to the location of the last item placed on the tasks stack. THIS MUST BE THE FIRST MEMBER OF THE STRUCT. */ + + #if ( portUSING_MPU_WRAPPERS == 1 ) + xMPU_SETTINGS xMPUSettings; /*< The MPU settings are defined as part of the port layer. THIS MUST BE THE SECOND MEMBER OF THE STRUCT. */ + #endif + + xListItem xGenericListItem; /*< List item used to place the TCB in ready and blocked queues. */ + xListItem xEventListItem; /*< List item used to place the TCB in event lists. */ + unsigned portBASE_TYPE uxPriority; /*< The priority of the task where 0 is the lowest priority. */ + portSTACK_TYPE *pxStack; /*< Points to the start of the stack. */ + signed char pcTaskName[ configMAX_TASK_NAME_LEN ];/*< Descriptive name given to the task when created. Facilitates debugging only. */ + + #if ( portSTACK_GROWTH > 0 ) + portSTACK_TYPE *pxEndOfStack; /*< Used for stack overflow checking on architectures where the stack grows up from low memory. */ + #endif + + #if ( portCRITICAL_NESTING_IN_TCB == 1 ) + unsigned portBASE_TYPE uxCriticalNesting; + #endif + + #if ( configUSE_TRACE_FACILITY == 1 ) + unsigned portBASE_TYPE uxTCBNumber; /*< This is used for tracing the scheduler and making debugging easier only. */ + #endif + + #if ( configUSE_MUTEXES == 1 ) + unsigned portBASE_TYPE uxBasePriority; /*< The priority last assigned to the task - used by the priority inheritance mechanism. */ + #endif + + #if ( configUSE_APPLICATION_TASK_TAG == 1 ) + pdTASK_HOOK_CODE pxTaskTag; + #endif + + #if ( configGENERATE_RUN_TIME_STATS == 1 ) + unsigned long ulRunTimeCounter; /*< Used for calculating how much CPU time each task is utilising. */ + #endif + +} tskTCB; + + +/* + * Some kernel aware debuggers require data to be viewed to be global, rather + * than file scope. + */ +#ifdef portREMOVE_STATIC_QUALIFIER + #define static +#endif + +/*lint -e956 */ +PRIVILEGED_DATA tskTCB * volatile pxCurrentTCB = NULL; + +/* Lists for ready and blocked tasks. --------------------*/ + +PRIVILEGED_DATA static xList pxReadyTasksLists[ configMAX_PRIORITIES ]; /*< Prioritised ready tasks. */ +PRIVILEGED_DATA static xList xDelayedTaskList1; /*< Delayed tasks. */ +PRIVILEGED_DATA static xList xDelayedTaskList2; /*< Delayed tasks (two lists are used - one for delays that have overflowed the current tick count. */ +PRIVILEGED_DATA static xList * volatile pxDelayedTaskList ; /*< Points to the delayed task list currently being used. */ +PRIVILEGED_DATA static xList * volatile pxOverflowDelayedTaskList; /*< Points to the delayed task list currently being used to hold tasks that have overflowed the current tick count. */ +PRIVILEGED_DATA static xList xPendingReadyList; /*< Tasks that have been readied while the scheduler was suspended. They will be moved to the ready queue when the scheduler is resumed. */ + +#if ( INCLUDE_vTaskDelete == 1 ) + + PRIVILEGED_DATA static volatile xList xTasksWaitingTermination; /*< Tasks that have been deleted - but the their memory not yet freed. */ + PRIVILEGED_DATA static volatile unsigned portBASE_TYPE uxTasksDeleted = ( unsigned portBASE_TYPE ) 0; + +#endif + +#if ( INCLUDE_vTaskSuspend == 1 ) + + PRIVILEGED_DATA static xList xSuspendedTaskList; /*< Tasks that are currently suspended. */ + +#endif + +/* File private variables. --------------------------------*/ +PRIVILEGED_DATA static volatile unsigned portBASE_TYPE uxCurrentNumberOfTasks = ( unsigned portBASE_TYPE ) 0; +PRIVILEGED_DATA static volatile portTickType xTickCount = ( portTickType ) 0; +PRIVILEGED_DATA static unsigned portBASE_TYPE uxTopUsedPriority = tskIDLE_PRIORITY; +PRIVILEGED_DATA static volatile unsigned portBASE_TYPE uxTopReadyPriority = tskIDLE_PRIORITY; +PRIVILEGED_DATA static volatile signed portBASE_TYPE xSchedulerRunning = pdFALSE; +PRIVILEGED_DATA static volatile unsigned portBASE_TYPE uxSchedulerSuspended = ( unsigned portBASE_TYPE ) pdFALSE; +PRIVILEGED_DATA static volatile unsigned portBASE_TYPE uxMissedTicks = ( unsigned portBASE_TYPE ) 0; +PRIVILEGED_DATA static volatile portBASE_TYPE xMissedYield = ( portBASE_TYPE ) pdFALSE; +PRIVILEGED_DATA static volatile portBASE_TYPE xNumOfOverflows = ( portBASE_TYPE ) 0; +PRIVILEGED_DATA static unsigned portBASE_TYPE uxTaskNumber = ( unsigned portBASE_TYPE ) 0; + +#if ( configGENERATE_RUN_TIME_STATS == 1 ) + + PRIVILEGED_DATA static char pcStatsString[ 50 ] ; + PRIVILEGED_DATA static unsigned long ulTaskSwitchedInTime = 0UL; /*< Holds the value of a timer/counter the last time a task was switched in. */ + static void prvGenerateRunTimeStatsForTasksInList( const signed char *pcWriteBuffer, xList *pxList, unsigned long ulTotalRunTime ) PRIVILEGED_FUNCTION; + +#endif + +/* Debugging and trace facilities private variables and macros. ------------*/ + +/* + * The value used to fill the stack of a task when the task is created. This + * is used purely for checking the high water mark for tasks. + */ +#define tskSTACK_FILL_BYTE ( 0xa5 ) + +/* + * Macros used by vListTask to indicate which state a task is in. + */ +#define tskBLOCKED_CHAR ( ( signed char ) 'B' ) +#define tskREADY_CHAR ( ( signed char ) 'R' ) +#define tskDELETED_CHAR ( ( signed char ) 'D' ) +#define tskSUSPENDED_CHAR ( ( signed char ) 'S' ) + +/* + * Macros and private variables used by the trace facility. + */ +#if ( configUSE_TRACE_FACILITY == 1 ) + + #define tskSIZE_OF_EACH_TRACE_LINE ( ( unsigned long ) ( sizeof( unsigned long ) + sizeof( unsigned long ) ) ) + PRIVILEGED_DATA static volatile signed char * volatile pcTraceBuffer; + PRIVILEGED_DATA static signed char *pcTraceBufferStart; + PRIVILEGED_DATA static signed char *pcTraceBufferEnd; + PRIVILEGED_DATA static signed portBASE_TYPE xTracing = pdFALSE; + static unsigned portBASE_TYPE uxPreviousTask = 255; + PRIVILEGED_DATA static char pcStatusString[ 50 ]; + +#endif + +/*-----------------------------------------------------------*/ + +/* + * Macro that writes a trace of scheduler activity to a buffer. This trace + * shows which task is running when and is very useful as a debugging tool. + * As this macro is called each context switch it is a good idea to undefine + * it if not using the facility. + */ +#if ( configUSE_TRACE_FACILITY == 1 ) + + #define vWriteTraceToBuffer() \ + { \ + if( xTracing ) \ + { \ + if( uxPreviousTask != pxCurrentTCB->uxTCBNumber ) \ + { \ + if( ( pcTraceBuffer + tskSIZE_OF_EACH_TRACE_LINE ) < pcTraceBufferEnd ) \ + { \ + uxPreviousTask = pxCurrentTCB->uxTCBNumber; \ + *( unsigned long * ) pcTraceBuffer = ( unsigned long ) xTickCount; \ + pcTraceBuffer += sizeof( unsigned long ); \ + *( unsigned long * ) pcTraceBuffer = ( unsigned long ) uxPreviousTask; \ + pcTraceBuffer += sizeof( unsigned long ); \ + } \ + else \ + { \ + xTracing = pdFALSE; \ + } \ + } \ + } \ + } + +#else + + #define vWriteTraceToBuffer() + +#endif +/*-----------------------------------------------------------*/ + +/* + * Place the task represented by pxTCB into the appropriate ready queue for + * the task. It is inserted at the end of the list. One quirk of this is + * that if the task being inserted is at the same priority as the currently + * executing task, then it will only be rescheduled after the currently + * executing task has been rescheduled. + */ +#define prvAddTaskToReadyQueue( pxTCB ) \ +{ \ + if( pxTCB->uxPriority > uxTopReadyPriority ) \ + { \ + uxTopReadyPriority = pxTCB->uxPriority; \ + } \ + vListInsertEnd( ( xList * ) &( pxReadyTasksLists[ pxTCB->uxPriority ] ), &( pxTCB->xGenericListItem ) ); \ +} +/*-----------------------------------------------------------*/ + +/* + * Macro that looks at the list of tasks that are currently delayed to see if + * any require waking. + * + * Tasks are stored in the queue in the order of their wake time - meaning + * once one tasks has been found whose timer has not expired we need not look + * any further down the list. + */ +#define prvCheckDelayedTasks() \ +{ \ +register tskTCB *pxTCB; \ + \ + while( ( pxTCB = ( tskTCB * ) listGET_OWNER_OF_HEAD_ENTRY( pxDelayedTaskList ) ) != NULL ) \ + { \ + if( xTickCount < listGET_LIST_ITEM_VALUE( &( pxTCB->xGenericListItem ) ) ) \ + { \ + break; \ + } \ + vListRemove( &( pxTCB->xGenericListItem ) ); \ + /* Is the task waiting on an event also? */ \ + if( pxTCB->xEventListItem.pvContainer ) \ + { \ + vListRemove( &( pxTCB->xEventListItem ) ); \ + } \ + prvAddTaskToReadyQueue( pxTCB ); \ + } \ +} +/*-----------------------------------------------------------*/ + +/* + * Several functions take an xTaskHandle parameter that can optionally be NULL, + * where NULL is used to indicate that the handle of the currently executing + * task should be used in place of the parameter. This macro simply checks to + * see if the parameter is NULL and returns a pointer to the appropriate TCB. + */ +#define prvGetTCBFromHandle( pxHandle ) ( ( pxHandle == NULL ) ? ( tskTCB * ) pxCurrentTCB : ( tskTCB * ) pxHandle ) + + +/* File private functions. --------------------------------*/ + +/* + * Utility to ready a TCB for a given task. Mainly just copies the parameters + * into the TCB structure. + */ +static void prvInitialiseTCBVariables( tskTCB *pxTCB, const signed char * const pcName, unsigned portBASE_TYPE uxPriority, const xMemoryRegion * const xRegions, unsigned short usStackDepth ) PRIVILEGED_FUNCTION; + +/* + * Utility to ready all the lists used by the scheduler. This is called + * automatically upon the creation of the first task. + */ +static void prvInitialiseTaskLists( void ) PRIVILEGED_FUNCTION; + +/* + * The idle task, which as all tasks is implemented as a never ending loop. + * The idle task is automatically created and added to the ready lists upon + * creation of the first user task. + * + * The portTASK_FUNCTION_PROTO() macro is used to allow port/compiler specific + * language extensions. The equivalent prototype for this function is: + * + * void prvIdleTask( void *pvParameters ); + * + */ +static portTASK_FUNCTION_PROTO( prvIdleTask, pvParameters ); + +/* + * Utility to free all memory allocated by the scheduler to hold a TCB, + * including the stack pointed to by the TCB. + * + * This does not free memory allocated by the task itself (i.e. memory + * allocated by calls to pvPortMalloc from within the tasks application code). + */ +#if ( ( INCLUDE_vTaskDelete == 1 ) || ( INCLUDE_vTaskCleanUpResources == 1 ) ) + + static void prvDeleteTCB( tskTCB *pxTCB ) PRIVILEGED_FUNCTION; + +#endif + +/* + * Used only by the idle task. This checks to see if anything has been placed + * in the list of tasks waiting to be deleted. If so the task is cleaned up + * and its TCB deleted. + */ +static void prvCheckTasksWaitingTermination( void ) PRIVILEGED_FUNCTION; + +/* + * Allocates memory from the heap for a TCB and associated stack. Checks the + * allocation was successful. + */ +static tskTCB *prvAllocateTCBAndStack( unsigned short usStackDepth, portSTACK_TYPE *puxStackBuffer ) PRIVILEGED_FUNCTION; + +/* + * Called from vTaskList. vListTasks details all the tasks currently under + * control of the scheduler. The tasks may be in one of a number of lists. + * prvListTaskWithinSingleList accepts a list and details the tasks from + * within just that list. + * + * THIS FUNCTION IS INTENDED FOR DEBUGGING ONLY, AND SHOULD NOT BE CALLED FROM + * NORMAL APPLICATION CODE. + */ +#if ( configUSE_TRACE_FACILITY == 1 ) + + static void prvListTaskWithinSingleList( const signed char *pcWriteBuffer, xList *pxList, signed char cStatus ) PRIVILEGED_FUNCTION; + +#endif + +/* + * When a task is created, the stack of the task is filled with a known value. + * This function determines the 'high water mark' of the task stack by + * determining how much of the stack remains at the original preset value. + */ +#if ( ( configUSE_TRACE_FACILITY == 1 ) || ( INCLUDE_uxTaskGetStackHighWaterMark == 1 ) ) + + static unsigned short usTaskCheckFreeStackSpace( const unsigned char * pucStackByte ) PRIVILEGED_FUNCTION; + +#endif + + +/*lint +e956 */ + + + +/*----------------------------------------------------------- + * TASK CREATION API documented in task.h + *----------------------------------------------------------*/ + +signed portBASE_TYPE xTaskGenericCreate( pdTASK_CODE pxTaskCode, const signed char * const pcName, unsigned short usStackDepth, void *pvParameters, unsigned portBASE_TYPE uxPriority, xTaskHandle *pxCreatedTask, portSTACK_TYPE *puxStackBuffer, const xMemoryRegion * const xRegions ) +{ +signed portBASE_TYPE xReturn; +tskTCB * pxNewTCB; + + /* Allocate the memory required by the TCB and stack for the new task, + checking that the allocation was successful. */ + pxNewTCB = prvAllocateTCBAndStack( usStackDepth, puxStackBuffer ); + + if( pxNewTCB != NULL ) + { + portSTACK_TYPE *pxTopOfStack; + + #if( portUSING_MPU_WRAPPERS == 1 ) + /* Should the task be created in privileged mode? */ + portBASE_TYPE xRunPrivileged; + if( ( uxPriority & portPRIVILEGE_BIT ) != 0x00 ) + { + xRunPrivileged = pdTRUE; + } + else + { + xRunPrivileged = pdFALSE; + } + uxPriority &= ~portPRIVILEGE_BIT; + #endif /* portUSING_MPU_WRAPPERS == 1 */ + + /* Calculate the top of stack address. This depends on whether the + stack grows from high memory to low (as per the 80x86) or visa versa. + portSTACK_GROWTH is used to make the result positive or negative as + required by the port. */ + #if( portSTACK_GROWTH < 0 ) + { + pxTopOfStack = pxNewTCB->pxStack + ( usStackDepth - 1 ); + pxTopOfStack = ( portSTACK_TYPE * ) ( ( ( unsigned long ) pxTopOfStack ) & ( ( unsigned long ) ~portBYTE_ALIGNMENT_MASK ) ); + } + #else + { + pxTopOfStack = pxNewTCB->pxStack; + + /* If we want to use stack checking on architectures that use + a positive stack growth direction then we also need to store the + other extreme of the stack space. */ + pxNewTCB->pxEndOfStack = pxNewTCB->pxStack + ( usStackDepth - 1 ); + } + #endif + + /* Setup the newly allocated TCB with the initial state of the task. */ + prvInitialiseTCBVariables( pxNewTCB, pcName, uxPriority, xRegions, usStackDepth ); + + /* Initialize the TCB stack to look as if the task was already running, + but had been interrupted by the scheduler. The return address is set + to the start of the task function. Once the stack has been initialised + the top of stack variable is updated. */ + #if( portUSING_MPU_WRAPPERS == 1 ) + { + pxNewTCB->pxTopOfStack = pxPortInitialiseStack( pxTopOfStack, pxTaskCode, pvParameters, xRunPrivileged ); + } + #else + { + pxNewTCB->pxTopOfStack = pxPortInitialiseStack( pxTopOfStack, pxTaskCode, pvParameters ); + } + #endif + + /* We are going to manipulate the task queues to add this task to a + ready list, so must make sure no interrupts occur. */ + portENTER_CRITICAL(); + { + uxCurrentNumberOfTasks++; + if( uxCurrentNumberOfTasks == ( unsigned portBASE_TYPE ) 1 ) + { + /* As this is the first task it must also be the current task. */ + pxCurrentTCB = pxNewTCB; + + /* This is the first task to be created so do the preliminary + initialisation required. We will not recover if this call + fails, but we will report the failure. */ + prvInitialiseTaskLists(); + } + else + { + /* If the scheduler is not already running, make this task the + current task if it is the highest priority task to be created + so far. */ + if( xSchedulerRunning == pdFALSE ) + { + if( pxCurrentTCB->uxPriority <= uxPriority ) + { + pxCurrentTCB = pxNewTCB; + } + } + } + + /* Remember the top priority to make context switching faster. Use + the priority in pxNewTCB as this has been capped to a valid value. */ + if( pxNewTCB->uxPriority > uxTopUsedPriority ) + { + uxTopUsedPriority = pxNewTCB->uxPriority; + } + + #if ( configUSE_TRACE_FACILITY == 1 ) + { + /* Add a counter into the TCB for tracing only. */ + pxNewTCB->uxTCBNumber = uxTaskNumber; + } + #endif + uxTaskNumber++; + + prvAddTaskToReadyQueue( pxNewTCB ); + + xReturn = pdPASS; + traceTASK_CREATE( pxNewTCB ); + } + portEXIT_CRITICAL(); + } + else + { + xReturn = errCOULD_NOT_ALLOCATE_REQUIRED_MEMORY; + traceTASK_CREATE_FAILED( pxNewTCB ); + } + + if( xReturn == pdPASS ) + { + if( ( void * ) pxCreatedTask != NULL ) + { + /* Pass the TCB out - in an anonymous way. The calling function/ + task can use this as a handle to delete the task later if + required.*/ + *pxCreatedTask = ( xTaskHandle ) pxNewTCB; + } + + if( xSchedulerRunning != pdFALSE ) + { + /* If the created task is of a higher priority than the current task + then it should run now. */ + if( pxCurrentTCB->uxPriority < uxPriority ) + { + portYIELD_WITHIN_API(); + } + } + } + + return xReturn; +} +/*-----------------------------------------------------------*/ + +#if ( INCLUDE_vTaskDelete == 1 ) + + void vTaskDelete( xTaskHandle pxTaskToDelete ) + { + tskTCB *pxTCB; + + portENTER_CRITICAL(); + { + /* Ensure a yield is performed if the current task is being + deleted. */ + if( pxTaskToDelete == pxCurrentTCB ) + { + pxTaskToDelete = NULL; + } + + /* If null is passed in here then we are deleting ourselves. */ + pxTCB = prvGetTCBFromHandle( pxTaskToDelete ); + + /* Remove task from the ready list and place in the termination list. + This will stop the task from be scheduled. The idle task will check + the termination list and free up any memory allocated by the + scheduler for the TCB and stack. */ + vListRemove( &( pxTCB->xGenericListItem ) ); + + /* Is the task waiting on an event also? */ + if( pxTCB->xEventListItem.pvContainer ) + { + vListRemove( &( pxTCB->xEventListItem ) ); + } + + vListInsertEnd( ( xList * ) &xTasksWaitingTermination, &( pxTCB->xGenericListItem ) ); + + /* Increment the ucTasksDeleted variable so the idle task knows + there is a task that has been deleted and that it should therefore + check the xTasksWaitingTermination list. */ + ++uxTasksDeleted; + + /* Increment the uxTaskNumberVariable also so kernel aware debuggers + can detect that the task lists need re-generating. */ + uxTaskNumber++; + + traceTASK_DELETE( pxTCB ); + } + portEXIT_CRITICAL(); + + /* Force a reschedule if we have just deleted the current task. */ + if( xSchedulerRunning != pdFALSE ) + { + if( ( void * ) pxTaskToDelete == NULL ) + { + portYIELD_WITHIN_API(); + } + } + } + +#endif + + + + + + +/*----------------------------------------------------------- + * TASK CONTROL API documented in task.h + *----------------------------------------------------------*/ + +#if ( INCLUDE_vTaskDelayUntil == 1 ) + + void vTaskDelayUntil( portTickType * const pxPreviousWakeTime, portTickType xTimeIncrement ) + { + portTickType xTimeToWake; + portBASE_TYPE xAlreadyYielded, xShouldDelay = pdFALSE; + + vTaskSuspendAll(); + { + /* Generate the tick time at which the task wants to wake. */ + xTimeToWake = *pxPreviousWakeTime + xTimeIncrement; + + if( xTickCount < *pxPreviousWakeTime ) + { + /* The tick count has overflowed since this function was + lasted called. In this case the only time we should ever + actually delay is if the wake time has also overflowed, + and the wake time is greater than the tick time. When this + is the case it is as if neither time had overflowed. */ + if( ( xTimeToWake < *pxPreviousWakeTime ) && ( xTimeToWake > xTickCount ) ) + { + xShouldDelay = pdTRUE; + } + } + else + { + /* The tick time has not overflowed. In this case we will + delay if either the wake time has overflowed, and/or the + tick time is less than the wake time. */ + if( ( xTimeToWake < *pxPreviousWakeTime ) || ( xTimeToWake > xTickCount ) ) + { + xShouldDelay = pdTRUE; + } + } + + /* Update the wake time ready for the next call. */ + *pxPreviousWakeTime = xTimeToWake; + + if( xShouldDelay ) + { + traceTASK_DELAY_UNTIL(); + + /* We must remove ourselves from the ready list before adding + ourselves to the blocked list as the same list item is used for + both lists. */ + vListRemove( ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); + + /* The list item will be inserted in wake time order. */ + listSET_LIST_ITEM_VALUE( &( pxCurrentTCB->xGenericListItem ), xTimeToWake ); + + if( xTimeToWake < xTickCount ) + { + /* Wake time has overflowed. Place this item in the + overflow list. */ + vListInsert( ( xList * ) pxOverflowDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); + } + else + { + /* The wake time has not overflowed, so we can use the + current block list. */ + vListInsert( ( xList * ) pxDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); + } + } + } + xAlreadyYielded = xTaskResumeAll(); + + /* Force a reschedule if xTaskResumeAll has not already done so, we may + have put ourselves to sleep. */ + if( !xAlreadyYielded ) + { + portYIELD_WITHIN_API(); + } + } + +#endif +/*-----------------------------------------------------------*/ + +#if ( INCLUDE_vTaskDelay == 1 ) + + void vTaskDelay( portTickType xTicksToDelay ) + { + portTickType xTimeToWake; + signed portBASE_TYPE xAlreadyYielded = pdFALSE; + + /* A delay time of zero just forces a reschedule. */ + if( xTicksToDelay > ( portTickType ) 0 ) + { + vTaskSuspendAll(); + { + traceTASK_DELAY(); + + /* A task that is removed from the event list while the + scheduler is suspended will not get placed in the ready + list or removed from the blocked list until the scheduler + is resumed. + + This task cannot be in an event list as it is the currently + executing task. */ + + /* Calculate the time to wake - this may overflow but this is + not a problem. */ + xTimeToWake = xTickCount + xTicksToDelay; + + /* We must remove ourselves from the ready list before adding + ourselves to the blocked list as the same list item is used for + both lists. */ + vListRemove( ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); + + /* The list item will be inserted in wake time order. */ + listSET_LIST_ITEM_VALUE( &( pxCurrentTCB->xGenericListItem ), xTimeToWake ); + + if( xTimeToWake < xTickCount ) + { + /* Wake time has overflowed. Place this item in the + overflow list. */ + vListInsert( ( xList * ) pxOverflowDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); + } + else + { + /* The wake time has not overflowed, so we can use the + current block list. */ + vListInsert( ( xList * ) pxDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); + } + } + xAlreadyYielded = xTaskResumeAll(); + } + + /* Force a reschedule if xTaskResumeAll has not already done so, we may + have put ourselves to sleep. */ + if( !xAlreadyYielded ) + { + portYIELD_WITHIN_API(); + } + } + +#endif +/*-----------------------------------------------------------*/ + +#if ( INCLUDE_uxTaskPriorityGet == 1 ) + + unsigned portBASE_TYPE uxTaskPriorityGet( xTaskHandle pxTask ) + { + tskTCB *pxTCB; + unsigned portBASE_TYPE uxReturn; + + portENTER_CRITICAL(); + { + /* If null is passed in here then we are changing the + priority of the calling function. */ + pxTCB = prvGetTCBFromHandle( pxTask ); + uxReturn = pxTCB->uxPriority; + } + portEXIT_CRITICAL(); + + return uxReturn; + } + +#endif +/*-----------------------------------------------------------*/ + +#if ( INCLUDE_vTaskPrioritySet == 1 ) + + void vTaskPrioritySet( xTaskHandle pxTask, unsigned portBASE_TYPE uxNewPriority ) + { + tskTCB *pxTCB; + unsigned portBASE_TYPE uxCurrentPriority, xYieldRequired = pdFALSE; + + /* Ensure the new priority is valid. */ + if( uxNewPriority >= configMAX_PRIORITIES ) + { + uxNewPriority = configMAX_PRIORITIES - 1; + } + + portENTER_CRITICAL(); + { + if( pxTask == pxCurrentTCB ) + { + pxTask = NULL; + } + + /* If null is passed in here then we are changing the + priority of the calling function. */ + pxTCB = prvGetTCBFromHandle( pxTask ); + + traceTASK_PRIORITY_SET( pxTask, uxNewPriority ); + + #if ( configUSE_MUTEXES == 1 ) + { + uxCurrentPriority = pxTCB->uxBasePriority; + } + #else + { + uxCurrentPriority = pxTCB->uxPriority; + } + #endif + + if( uxCurrentPriority != uxNewPriority ) + { + /* The priority change may have readied a task of higher + priority than the calling task. */ + if( uxNewPriority > uxCurrentPriority ) + { + if( pxTask != NULL ) + { + /* The priority of another task is being raised. If we + were raising the priority of the currently running task + there would be no need to switch as it must have already + been the highest priority task. */ + xYieldRequired = pdTRUE; + } + } + else if( pxTask == NULL ) + { + /* Setting our own priority down means there may now be another + task of higher priority that is ready to execute. */ + xYieldRequired = pdTRUE; + } + + + + #if ( configUSE_MUTEXES == 1 ) + { + /* Only change the priority being used if the task is not + currently using an inherited priority. */ + if( pxTCB->uxBasePriority == pxTCB->uxPriority ) + { + pxTCB->uxPriority = uxNewPriority; + } + + /* The base priority gets set whatever. */ + pxTCB->uxBasePriority = uxNewPriority; + } + #else + { + pxTCB->uxPriority = uxNewPriority; + } + #endif + + listSET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ), ( configMAX_PRIORITIES - ( portTickType ) uxNewPriority ) ); + + /* If the task is in the blocked or suspended list we need do + nothing more than change it's priority variable. However, if + the task is in a ready list it needs to be removed and placed + in the queue appropriate to its new priority. */ + if( listIS_CONTAINED_WITHIN( &( pxReadyTasksLists[ uxCurrentPriority ] ), &( pxTCB->xGenericListItem ) ) ) + { + /* The task is currently in its ready list - remove before adding + it to it's new ready list. As we are in a critical section we + can do this even if the scheduler is suspended. */ + vListRemove( &( pxTCB->xGenericListItem ) ); + prvAddTaskToReadyQueue( pxTCB ); + } + + if( xYieldRequired == pdTRUE ) + { + portYIELD_WITHIN_API(); + } + } + } + portEXIT_CRITICAL(); + } + +#endif +/*-----------------------------------------------------------*/ + +#if ( INCLUDE_vTaskSuspend == 1 ) + + void vTaskSuspend( xTaskHandle pxTaskToSuspend ) + { + tskTCB *pxTCB; + + portENTER_CRITICAL(); + { + /* Ensure a yield is performed if the current task is being + suspended. */ + if( pxTaskToSuspend == pxCurrentTCB ) + { + pxTaskToSuspend = NULL; + } + + /* If null is passed in here then we are suspending ourselves. */ + pxTCB = prvGetTCBFromHandle( pxTaskToSuspend ); + + traceTASK_SUSPEND( pxTCB ); + + /* Remove task from the ready/delayed list and place in the suspended list. */ + vListRemove( &( pxTCB->xGenericListItem ) ); + + /* Is the task waiting on an event also? */ + if( pxTCB->xEventListItem.pvContainer ) + { + vListRemove( &( pxTCB->xEventListItem ) ); + } + + vListInsertEnd( ( xList * ) &xSuspendedTaskList, &( pxTCB->xGenericListItem ) ); + } + portEXIT_CRITICAL(); + + /* We may have just suspended the current task. */ + if( ( void * ) pxTaskToSuspend == NULL ) + { + portYIELD_WITHIN_API(); + } + } + +#endif +/*-----------------------------------------------------------*/ + +#if ( INCLUDE_vTaskSuspend == 1 ) + + signed portBASE_TYPE xTaskIsTaskSuspended( xTaskHandle xTask ) + { + portBASE_TYPE xReturn = pdFALSE; + const tskTCB * const pxTCB = ( tskTCB * ) xTask; + + /* Is the task we are attempting to resume actually in the + suspended list? */ + if( listIS_CONTAINED_WITHIN( &xSuspendedTaskList, &( pxTCB->xGenericListItem ) ) != pdFALSE ) + { + /* Has the task already been resumed from within an ISR? */ + if( listIS_CONTAINED_WITHIN( &xPendingReadyList, &( pxTCB->xEventListItem ) ) != pdTRUE ) + { + /* Is it in the suspended list because it is in the + Suspended state? It is possible to be in the suspended + list because it is blocked on a task with no timeout + specified. */ + if( listIS_CONTAINED_WITHIN( NULL, &( pxTCB->xEventListItem ) ) == pdTRUE ) + { + xReturn = pdTRUE; + } + } + } + + return xReturn; + } + +#endif +/*-----------------------------------------------------------*/ + +#if ( INCLUDE_vTaskSuspend == 1 ) + + void vTaskResume( xTaskHandle pxTaskToResume ) + { + tskTCB *pxTCB; + + /* Remove the task from whichever list it is currently in, and place + it in the ready list. */ + pxTCB = ( tskTCB * ) pxTaskToResume; + + /* The parameter cannot be NULL as it is impossible to resume the + currently executing task. */ + if( ( pxTCB != NULL ) && ( pxTCB != pxCurrentTCB ) ) + { + portENTER_CRITICAL(); + { + if( xTaskIsTaskSuspended( pxTCB ) == pdTRUE ) + { + traceTASK_RESUME( pxTCB ); + + /* As we are in a critical section we can access the ready + lists even if the scheduler is suspended. */ + vListRemove( &( pxTCB->xGenericListItem ) ); + prvAddTaskToReadyQueue( pxTCB ); + + /* We may have just resumed a higher priority task. */ + if( pxTCB->uxPriority >= pxCurrentTCB->uxPriority ) + { + /* This yield may not cause the task just resumed to run, but + will leave the lists in the correct state for the next yield. */ + portYIELD_WITHIN_API(); + } + } + } + portEXIT_CRITICAL(); + } + } + +#endif + +/*-----------------------------------------------------------*/ + +#if ( ( INCLUDE_xTaskResumeFromISR == 1 ) && ( INCLUDE_vTaskSuspend == 1 ) ) + + portBASE_TYPE xTaskResumeFromISR( xTaskHandle pxTaskToResume ) + { + portBASE_TYPE xYieldRequired = pdFALSE; + tskTCB *pxTCB; + + pxTCB = ( tskTCB * ) pxTaskToResume; + + if( xTaskIsTaskSuspended( pxTCB ) == pdTRUE ) + { + traceTASK_RESUME_FROM_ISR( pxTCB ); + + if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) + { + xYieldRequired = ( pxTCB->uxPriority >= pxCurrentTCB->uxPriority ); + vListRemove( &( pxTCB->xGenericListItem ) ); + prvAddTaskToReadyQueue( pxTCB ); + } + else + { + /* We cannot access the delayed or ready lists, so will hold this + task pending until the scheduler is resumed, at which point a + yield will be performed if necessary. */ + vListInsertEnd( ( xList * ) &( xPendingReadyList ), &( pxTCB->xEventListItem ) ); + } + } + + return xYieldRequired; + } + +#endif + + + + +/*----------------------------------------------------------- + * PUBLIC SCHEDULER CONTROL documented in task.h + *----------------------------------------------------------*/ + + +void vTaskStartScheduler( void ) +{ +portBASE_TYPE xReturn; + + /* Add the idle task at the lowest priority. */ + xReturn = xTaskCreate( prvIdleTask, ( signed char * ) "IDLE", tskIDLE_STACK_SIZE, ( void * ) NULL, ( tskIDLE_PRIORITY | portPRIVILEGE_BIT ), ( xTaskHandle * ) NULL ); + + if( xReturn == pdPASS ) + { + /* Interrupts are turned off here, to ensure a tick does not occur + before or during the call to xPortStartScheduler(). The stacks of + the created tasks contain a status word with interrupts switched on + so interrupts will automatically get re-enabled when the first task + starts to run. + + STEPPING THROUGH HERE USING A DEBUGGER CAN CAUSE BIG PROBLEMS IF THE + DEBUGGER ALLOWS INTERRUPTS TO BE PROCESSED. */ + portDISABLE_INTERRUPTS(); + + xSchedulerRunning = pdTRUE; + xTickCount = ( portTickType ) 0; + + /* If configGENERATE_RUN_TIME_STATS is defined then the following + macro must be defined to configure the timer/counter used to generate + the run time counter time base. */ + portCONFIGURE_TIMER_FOR_RUN_TIME_STATS(); + + /* Setting up the timer tick is hardware specific and thus in the + portable interface. */ + if( xPortStartScheduler() ) + { + /* Should not reach here as if the scheduler is running the + function will not return. */ + } + else + { + /* Should only reach here if a task calls xTaskEndScheduler(). */ + } + } +} +/*-----------------------------------------------------------*/ + +void vTaskEndScheduler( void ) +{ + /* Stop the scheduler interrupts and call the portable scheduler end + routine so the original ISRs can be restored if necessary. The port + layer must ensure interrupts enable bit is left in the correct state. */ + portDISABLE_INTERRUPTS(); + xSchedulerRunning = pdFALSE; + vPortEndScheduler(); +} +/*----------------------------------------------------------*/ + +void vTaskSuspendAll( void ) +{ + /* A critical section is not required as the variable is of type + portBASE_TYPE. */ + ++uxSchedulerSuspended; +} +/*----------------------------------------------------------*/ + +signed portBASE_TYPE xTaskResumeAll( void ) +{ +register tskTCB *pxTCB; +signed portBASE_TYPE xAlreadyYielded = pdFALSE; + + /* It is possible that an ISR caused a task to be removed from an event + list while the scheduler was suspended. If this was the case then the + removed task will have been added to the xPendingReadyList. Once the + scheduler has been resumed it is safe to move all the pending ready + tasks from this list into their appropriate ready list. */ + portENTER_CRITICAL(); + { + --uxSchedulerSuspended; + + if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) + { + if( uxCurrentNumberOfTasks > ( unsigned portBASE_TYPE ) 0 ) + { + portBASE_TYPE xYieldRequired = pdFALSE; + + /* Move any readied tasks from the pending list into the + appropriate ready list. */ + while( ( pxTCB = ( tskTCB * ) listGET_OWNER_OF_HEAD_ENTRY( ( ( xList * ) &xPendingReadyList ) ) ) != NULL ) + { + vListRemove( &( pxTCB->xEventListItem ) ); + vListRemove( &( pxTCB->xGenericListItem ) ); + prvAddTaskToReadyQueue( pxTCB ); + + /* If we have moved a task that has a priority higher than + the current task then we should yield. */ + if( pxTCB->uxPriority >= pxCurrentTCB->uxPriority ) + { + xYieldRequired = pdTRUE; + } + } + + /* If any ticks occurred while the scheduler was suspended then + they should be processed now. This ensures the tick count does not + slip, and that any delayed tasks are resumed at the correct time. */ + if( uxMissedTicks > ( unsigned portBASE_TYPE ) 0 ) + { + while( uxMissedTicks > ( unsigned portBASE_TYPE ) 0 ) + { + vTaskIncrementTick(); + --uxMissedTicks; + } + + /* As we have processed some ticks it is appropriate to yield + to ensure the highest priority task that is ready to run is + the task actually running. */ + #if configUSE_PREEMPTION == 1 + { + xYieldRequired = pdTRUE; + } + #endif + } + + if( ( xYieldRequired == pdTRUE ) || ( xMissedYield == pdTRUE ) ) + { + xAlreadyYielded = pdTRUE; + xMissedYield = pdFALSE; + portYIELD_WITHIN_API(); + } + } + } + } + portEXIT_CRITICAL(); + + return xAlreadyYielded; +} + + + + + + +/*----------------------------------------------------------- + * PUBLIC TASK UTILITIES documented in task.h + *----------------------------------------------------------*/ + + + +portTickType xTaskGetTickCount( void ) +{ +portTickType xTicks; + + /* Critical section required if running on a 16 bit processor. */ + portENTER_CRITICAL(); + { + xTicks = xTickCount; + } + portEXIT_CRITICAL(); + + return xTicks; +} +/*-----------------------------------------------------------*/ + +unsigned portBASE_TYPE uxTaskGetNumberOfTasks( void ) +{ + /* A critical section is not required because the variables are of type + portBASE_TYPE. */ + return uxCurrentNumberOfTasks; +} +/*-----------------------------------------------------------*/ + +#if ( configUSE_TRACE_FACILITY == 1 ) + + void vTaskList( signed char *pcWriteBuffer ) + { + unsigned portBASE_TYPE uxQueue; + + /* This is a VERY costly function that should be used for debug only. + It leaves interrupts disabled for a LONG time. */ + + vTaskSuspendAll(); + { + /* Run through all the lists that could potentially contain a TCB and + report the task name, state and stack high water mark. */ + + pcWriteBuffer[ 0 ] = ( signed char ) 0x00; + strcat( ( char * ) pcWriteBuffer, ( const char * ) "\r\n" ); + + uxQueue = uxTopUsedPriority + 1; + + do + { + uxQueue--; + + if( !listLIST_IS_EMPTY( &( pxReadyTasksLists[ uxQueue ] ) ) ) + { + prvListTaskWithinSingleList( pcWriteBuffer, ( xList * ) &( pxReadyTasksLists[ uxQueue ] ), tskREADY_CHAR ); + } + }while( uxQueue > ( unsigned short ) tskIDLE_PRIORITY ); + + if( !listLIST_IS_EMPTY( pxDelayedTaskList ) ) + { + prvListTaskWithinSingleList( pcWriteBuffer, ( xList * ) pxDelayedTaskList, tskBLOCKED_CHAR ); + } + + if( !listLIST_IS_EMPTY( pxOverflowDelayedTaskList ) ) + { + prvListTaskWithinSingleList( pcWriteBuffer, ( xList * ) pxOverflowDelayedTaskList, tskBLOCKED_CHAR ); + } + + #if( INCLUDE_vTaskDelete == 1 ) + { + if( !listLIST_IS_EMPTY( &xTasksWaitingTermination ) ) + { + prvListTaskWithinSingleList( pcWriteBuffer, ( xList * ) &xTasksWaitingTermination, tskDELETED_CHAR ); + } + } + #endif + + #if ( INCLUDE_vTaskSuspend == 1 ) + { + if( !listLIST_IS_EMPTY( &xSuspendedTaskList ) ) + { + prvListTaskWithinSingleList( pcWriteBuffer, ( xList * ) &xSuspendedTaskList, tskSUSPENDED_CHAR ); + } + } + #endif + } + xTaskResumeAll(); + } + +#endif +/*----------------------------------------------------------*/ + +#if ( configGENERATE_RUN_TIME_STATS == 1 ) + + void vTaskGetRunTimeStats( signed char *pcWriteBuffer ) + { + unsigned portBASE_TYPE uxQueue; + unsigned long ulTotalRunTime = portGET_RUN_TIME_COUNTER_VALUE(); + + /* This is a VERY costly function that should be used for debug only. + It leaves interrupts disabled for a LONG time. */ + + vTaskSuspendAll(); + { + /* Run through all the lists that could potentially contain a TCB, + generating a table of run timer percentages in the provided + buffer. */ + + pcWriteBuffer[ 0 ] = ( signed char ) 0x00; + strcat( ( char * ) pcWriteBuffer, ( const char * ) "\r\n" ); + + uxQueue = uxTopUsedPriority + 1; + + do + { + uxQueue--; + + if( !listLIST_IS_EMPTY( &( pxReadyTasksLists[ uxQueue ] ) ) ) + { + prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, ( xList * ) &( pxReadyTasksLists[ uxQueue ] ), ulTotalRunTime ); + } + }while( uxQueue > ( unsigned short ) tskIDLE_PRIORITY ); + + if( !listLIST_IS_EMPTY( pxDelayedTaskList ) ) + { + prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, ( xList * ) pxDelayedTaskList, ulTotalRunTime ); + } + + if( !listLIST_IS_EMPTY( pxOverflowDelayedTaskList ) ) + { + prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, ( xList * ) pxOverflowDelayedTaskList, ulTotalRunTime ); + } + + #if ( INCLUDE_vTaskDelete == 1 ) + { + if( !listLIST_IS_EMPTY( &xTasksWaitingTermination ) ) + { + prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, ( xList * ) &xTasksWaitingTermination, ulTotalRunTime ); + } + } + #endif + + #if ( INCLUDE_vTaskSuspend == 1 ) + { + if( !listLIST_IS_EMPTY( &xSuspendedTaskList ) ) + { + prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, ( xList * ) &xSuspendedTaskList, ulTotalRunTime ); + } + } + #endif + } + xTaskResumeAll(); + } + +#endif +/*----------------------------------------------------------*/ + +#if ( configUSE_TRACE_FACILITY == 1 ) + + void vTaskStartTrace( signed char * pcBuffer, unsigned long ulBufferSize ) + { + portENTER_CRITICAL(); + { + pcTraceBuffer = ( signed char * )pcBuffer; + pcTraceBufferStart = pcBuffer; + pcTraceBufferEnd = pcBuffer + ( ulBufferSize - tskSIZE_OF_EACH_TRACE_LINE ); + xTracing = pdTRUE; + } + portEXIT_CRITICAL(); + } + +#endif +/*----------------------------------------------------------*/ + +#if ( configUSE_TRACE_FACILITY == 1 ) + + unsigned long ulTaskEndTrace( void ) + { + unsigned long ulBufferLength; + + portENTER_CRITICAL(); + xTracing = pdFALSE; + portEXIT_CRITICAL(); + + ulBufferLength = ( unsigned long ) ( pcTraceBuffer - pcTraceBufferStart ); + + return ulBufferLength; + } + +#endif + + + +/*----------------------------------------------------------- + * SCHEDULER INTERNALS AVAILABLE FOR PORTING PURPOSES + * documented in task.h + *----------------------------------------------------------*/ + + +void vTaskIncrementTick( void ) +{ + /* Called by the portable layer each time a tick interrupt occurs. + Increments the tick then checks to see if the new tick value will cause any + tasks to be unblocked. */ + if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) + { + ++xTickCount; + if( xTickCount == ( portTickType ) 0 ) + { + xList *pxTemp; + + /* Tick count has overflowed so we need to swap the delay lists. + If there are any items in pxDelayedTaskList here then there is + an error! */ + pxTemp = pxDelayedTaskList; + pxDelayedTaskList = pxOverflowDelayedTaskList; + pxOverflowDelayedTaskList = pxTemp; + xNumOfOverflows++; + } + + /* See if this tick has made a timeout expire. */ + prvCheckDelayedTasks(); + } + else + { + ++uxMissedTicks; + + /* The tick hook gets called at regular intervals, even if the + scheduler is locked. */ + #if ( configUSE_TICK_HOOK == 1 ) + { + extern void vApplicationTickHook( void ); + + vApplicationTickHook(); + } + #endif + } + + #if ( configUSE_TICK_HOOK == 1 ) + { + extern void vApplicationTickHook( void ); + + /* Guard against the tick hook being called when the missed tick + count is being unwound (when the scheduler is being unlocked. */ + if( uxMissedTicks == 0 ) + { + vApplicationTickHook(); + } + } + #endif + + traceTASK_INCREMENT_TICK( xTickCount ); +} +/*-----------------------------------------------------------*/ + +#if ( ( INCLUDE_vTaskCleanUpResources == 1 ) && ( INCLUDE_vTaskSuspend == 1 ) ) + + void vTaskCleanUpResources( void ) + { + unsigned short usQueue; + volatile tskTCB *pxTCB; + + usQueue = ( unsigned short ) uxTopUsedPriority + ( unsigned short ) 1; + + /* Remove any TCB's from the ready queues. */ + do + { + usQueue--; + + while( !listLIST_IS_EMPTY( &( pxReadyTasksLists[ usQueue ] ) ) ) + { + listGET_OWNER_OF_NEXT_ENTRY( pxTCB, &( pxReadyTasksLists[ usQueue ] ) ); + vListRemove( ( xListItem * ) &( pxTCB->xGenericListItem ) ); + + prvDeleteTCB( ( tskTCB * ) pxTCB ); + } + }while( usQueue > ( unsigned short ) tskIDLE_PRIORITY ); + + /* Remove any TCB's from the delayed queue. */ + while( !listLIST_IS_EMPTY( &xDelayedTaskList1 ) ) + { + listGET_OWNER_OF_NEXT_ENTRY( pxTCB, &xDelayedTaskList1 ); + vListRemove( ( xListItem * ) &( pxTCB->xGenericListItem ) ); + + prvDeleteTCB( ( tskTCB * ) pxTCB ); + } + + /* Remove any TCB's from the overflow delayed queue. */ + while( !listLIST_IS_EMPTY( &xDelayedTaskList2 ) ) + { + listGET_OWNER_OF_NEXT_ENTRY( pxTCB, &xDelayedTaskList2 ); + vListRemove( ( xListItem * ) &( pxTCB->xGenericListItem ) ); + + prvDeleteTCB( ( tskTCB * ) pxTCB ); + } + + while( !listLIST_IS_EMPTY( &xSuspendedTaskList ) ) + { + listGET_OWNER_OF_NEXT_ENTRY( pxTCB, &xSuspendedTaskList ); + vListRemove( ( xListItem * ) &( pxTCB->xGenericListItem ) ); + + prvDeleteTCB( ( tskTCB * ) pxTCB ); + } + } + +#endif +/*-----------------------------------------------------------*/ + +#if ( configUSE_APPLICATION_TASK_TAG == 1 ) + + void vTaskSetApplicationTaskTag( xTaskHandle xTask, pdTASK_HOOK_CODE pxTagValue ) + { + tskTCB *xTCB; + + /* If xTask is NULL then we are setting our own task hook. */ + if( xTask == NULL ) + { + xTCB = ( tskTCB * ) pxCurrentTCB; + } + else + { + xTCB = ( tskTCB * ) xTask; + } + + /* Save the hook function in the TCB. A critical section is required as + the value can be accessed from an interrupt. */ + portENTER_CRITICAL(); + xTCB->pxTaskTag = pxTagValue; + portEXIT_CRITICAL(); + } + +#endif +/*-----------------------------------------------------------*/ + +#if ( configUSE_APPLICATION_TASK_TAG == 1 ) + + pdTASK_HOOK_CODE xTaskGetApplicationTaskTag( xTaskHandle xTask ) + { + tskTCB *xTCB; + pdTASK_HOOK_CODE xReturn; + + /* If xTask is NULL then we are setting our own task hook. */ + if( xTask == NULL ) + { + xTCB = ( tskTCB * ) pxCurrentTCB; + } + else + { + xTCB = ( tskTCB * ) xTask; + } + + /* Save the hook function in the TCB. A critical section is required as + the value can be accessed from an interrupt. */ + portENTER_CRITICAL(); + xReturn = xTCB->pxTaskTag; + portEXIT_CRITICAL(); + + return xReturn; + } + +#endif +/*-----------------------------------------------------------*/ + +#if ( configUSE_APPLICATION_TASK_TAG == 1 ) + + portBASE_TYPE xTaskCallApplicationTaskHook( xTaskHandle xTask, void *pvParameter ) + { + tskTCB *xTCB; + portBASE_TYPE xReturn; + + /* If xTask is NULL then we are calling our own task hook. */ + if( xTask == NULL ) + { + xTCB = ( tskTCB * ) pxCurrentTCB; + } + else + { + xTCB = ( tskTCB * ) xTask; + } + + if( xTCB->pxTaskTag != NULL ) + { + xReturn = xTCB->pxTaskTag( pvParameter ); + } + else + { + xReturn = pdFAIL; + } + + return xReturn; + } + +#endif +/*-----------------------------------------------------------*/ + +void vTaskSwitchContext( void ) +{ + if( uxSchedulerSuspended != ( unsigned portBASE_TYPE ) pdFALSE ) + { + /* The scheduler is currently suspended - do not allow a context + switch. */ + xMissedYield = pdTRUE; + return; + } + + traceTASK_SWITCHED_OUT(); + + #if ( configGENERATE_RUN_TIME_STATS == 1 ) + { + unsigned long ulTempCounter = portGET_RUN_TIME_COUNTER_VALUE(); + + /* Add the amount of time the task has been running to the accumulated + time so far. The time the task started running was stored in + ulTaskSwitchedInTime. Note that there is no overflow protection here + so count values are only valid until the timer overflows. Generally + this will be about 1 hour assuming a 1uS timer increment. */ + pxCurrentTCB->ulRunTimeCounter += ( ulTempCounter - ulTaskSwitchedInTime ); + ulTaskSwitchedInTime = ulTempCounter; + } + #endif + + taskFIRST_CHECK_FOR_STACK_OVERFLOW(); + taskSECOND_CHECK_FOR_STACK_OVERFLOW(); + + /* Find the highest priority queue that contains ready tasks. */ + while( listLIST_IS_EMPTY( &( pxReadyTasksLists[ uxTopReadyPriority ] ) ) ) + { + --uxTopReadyPriority; + } + + /* listGET_OWNER_OF_NEXT_ENTRY walks through the list, so the tasks of the + same priority get an equal share of the processor time. */ + listGET_OWNER_OF_NEXT_ENTRY( pxCurrentTCB, &( pxReadyTasksLists[ uxTopReadyPriority ] ) ); + + traceTASK_SWITCHED_IN(); + vWriteTraceToBuffer(); +} +/*-----------------------------------------------------------*/ + +void vTaskPlaceOnEventList( const xList * const pxEventList, portTickType xTicksToWait ) +{ +portTickType xTimeToWake; + + /* THIS FUNCTION MUST BE CALLED WITH INTERRUPTS DISABLED OR THE + SCHEDULER SUSPENDED. */ + + /* Place the event list item of the TCB in the appropriate event list. + This is placed in the list in priority order so the highest priority task + is the first to be woken by the event. */ + vListInsert( ( xList * ) pxEventList, ( xListItem * ) &( pxCurrentTCB->xEventListItem ) ); + + /* We must remove ourselves from the ready list before adding ourselves + to the blocked list as the same list item is used for both lists. We have + exclusive access to the ready lists as the scheduler is locked. */ + vListRemove( ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); + + + #if ( INCLUDE_vTaskSuspend == 1 ) + { + if( xTicksToWait == portMAX_DELAY ) + { + /* Add ourselves to the suspended task list instead of a delayed task + list to ensure we are not woken by a timing event. We will block + indefinitely. */ + vListInsertEnd( ( xList * ) &xSuspendedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); + } + else + { + /* Calculate the time at which the task should be woken if the event does + not occur. This may overflow but this doesn't matter. */ + xTimeToWake = xTickCount + xTicksToWait; + + listSET_LIST_ITEM_VALUE( &( pxCurrentTCB->xGenericListItem ), xTimeToWake ); + + if( xTimeToWake < xTickCount ) + { + /* Wake time has overflowed. Place this item in the overflow list. */ + vListInsert( ( xList * ) pxOverflowDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); + } + else + { + /* The wake time has not overflowed, so we can use the current block list. */ + vListInsert( ( xList * ) pxDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); + } + } + } + #else + { + /* Calculate the time at which the task should be woken if the event does + not occur. This may overflow but this doesn't matter. */ + xTimeToWake = xTickCount + xTicksToWait; + + listSET_LIST_ITEM_VALUE( &( pxCurrentTCB->xGenericListItem ), xTimeToWake ); + + if( xTimeToWake < xTickCount ) + { + /* Wake time has overflowed. Place this item in the overflow list. */ + vListInsert( ( xList * ) pxOverflowDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); + } + else + { + /* The wake time has not overflowed, so we can use the current block list. */ + vListInsert( ( xList * ) pxDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); + } + } + #endif +} +/*-----------------------------------------------------------*/ + +signed portBASE_TYPE xTaskRemoveFromEventList( const xList * const pxEventList ) +{ +tskTCB *pxUnblockedTCB; +portBASE_TYPE xReturn; + + /* THIS FUNCTION MUST BE CALLED WITH INTERRUPTS DISABLED OR THE + SCHEDULER SUSPENDED. It can also be called from within an ISR. */ + + /* The event list is sorted in priority order, so we can remove the + first in the list, remove the TCB from the delayed list, and add + it to the ready list. + + If an event is for a queue that is locked then this function will never + get called - the lock count on the queue will get modified instead. This + means we can always expect exclusive access to the event list here. */ + pxUnblockedTCB = ( tskTCB * ) listGET_OWNER_OF_HEAD_ENTRY( pxEventList ); + vListRemove( &( pxUnblockedTCB->xEventListItem ) ); + + if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) + { + vListRemove( &( pxUnblockedTCB->xGenericListItem ) ); + prvAddTaskToReadyQueue( pxUnblockedTCB ); + } + else + { + /* We cannot access the delayed or ready lists, so will hold this + task pending until the scheduler is resumed. */ + vListInsertEnd( ( xList * ) &( xPendingReadyList ), &( pxUnblockedTCB->xEventListItem ) ); + } + + if( pxUnblockedTCB->uxPriority >= pxCurrentTCB->uxPriority ) + { + /* Return true if the task removed from the event list has + a higher priority than the calling task. This allows + the calling task to know if it should force a context + switch now. */ + xReturn = pdTRUE; + } + else + { + xReturn = pdFALSE; + } + + return xReturn; +} +/*-----------------------------------------------------------*/ + +void vTaskSetTimeOutState( xTimeOutType * const pxTimeOut ) +{ + pxTimeOut->xOverflowCount = xNumOfOverflows; + pxTimeOut->xTimeOnEntering = xTickCount; +} +/*-----------------------------------------------------------*/ + +portBASE_TYPE xTaskCheckForTimeOut( xTimeOutType * const pxTimeOut, portTickType * const pxTicksToWait ) +{ +portBASE_TYPE xReturn; + + portENTER_CRITICAL(); + { + #if ( INCLUDE_vTaskSuspend == 1 ) + /* If INCLUDE_vTaskSuspend is set to 1 and the block time specified is + the maximum block time then the task should block indefinitely, and + therefore never time out. */ + if( *pxTicksToWait == portMAX_DELAY ) + { + xReturn = pdFALSE; + } + else /* We are not blocking indefinitely, perform the checks below. */ + #endif + + if( ( xNumOfOverflows != pxTimeOut->xOverflowCount ) && ( ( portTickType ) xTickCount >= ( portTickType ) pxTimeOut->xTimeOnEntering ) ) + { + /* The tick count is greater than the time at which vTaskSetTimeout() + was called, but has also overflowed since vTaskSetTimeOut() was called. + It must have wrapped all the way around and gone past us again. This + passed since vTaskSetTimeout() was called. */ + xReturn = pdTRUE; + } + else if( ( ( portTickType ) ( ( portTickType ) xTickCount - ( portTickType ) pxTimeOut->xTimeOnEntering ) ) < ( portTickType ) *pxTicksToWait ) + { + /* Not a genuine timeout. Adjust parameters for time remaining. */ + *pxTicksToWait -= ( ( portTickType ) xTickCount - ( portTickType ) pxTimeOut->xTimeOnEntering ); + vTaskSetTimeOutState( pxTimeOut ); + xReturn = pdFALSE; + } + else + { + xReturn = pdTRUE; + } + } + portEXIT_CRITICAL(); + + return xReturn; +} +/*-----------------------------------------------------------*/ + +void vTaskMissedYield( void ) +{ + xMissedYield = pdTRUE; +} + +/* + * ----------------------------------------------------------- + * The Idle task. + * ---------------------------------------------------------- + * + * The portTASK_FUNCTION() macro is used to allow port/compiler specific + * language extensions. The equivalent prototype for this function is: + * + * void prvIdleTask( void *pvParameters ); + * + */ +static portTASK_FUNCTION( prvIdleTask, pvParameters ) +{ + /* Stop warnings. */ + ( void ) pvParameters; + + for( ;; ) + { + /* See if any tasks have been deleted. */ + prvCheckTasksWaitingTermination(); + + #if ( configUSE_PREEMPTION == 0 ) + { + /* If we are not using preemption we keep forcing a task switch to + see if any other task has become available. If we are using + preemption we don't need to do this as any task becoming available + will automatically get the processor anyway. */ + taskYIELD(); + } + #endif + + #if ( ( configUSE_PREEMPTION == 1 ) && ( configIDLE_SHOULD_YIELD == 1 ) ) + { + /* When using preemption tasks of equal priority will be + timesliced. If a task that is sharing the idle priority is ready + to run then the idle task should yield before the end of the + timeslice. + + A critical region is not required here as we are just reading from + the list, and an occasional incorrect value will not matter. If + the ready list at the idle priority contains more than one task + then a task other than the idle task is ready to execute. */ + if( listCURRENT_LIST_LENGTH( &( pxReadyTasksLists[ tskIDLE_PRIORITY ] ) ) > ( unsigned portBASE_TYPE ) 1 ) + { + taskYIELD(); + } + } + #endif + + #if ( configUSE_IDLE_HOOK == 1 ) + { + extern void vApplicationIdleHook( void ); + + /* Call the user defined function from within the idle task. This + allows the application designer to add background functionality + without the overhead of a separate task. + NOTE: vApplicationIdleHook() MUST NOT, UNDER ANY CIRCUMSTANCES, + CALL A FUNCTION THAT MIGHT BLOCK. */ + vApplicationIdleHook(); + } + #endif + // call nanosleep for smalles sleep time possible + // (depending on kernel settings - around 100 microseconds) + // decreases idle thread CPU load from 100 to practically 0 + struct timespec x; + x.tv_sec=1; + x.tv_nsec=0; + nanosleep(&x,NULL); + } +} /*lint !e715 pvParameters is not accessed but all task functions require the same prototype. */ + + + + + + + +/*----------------------------------------------------------- + * File private functions documented at the top of the file. + *----------------------------------------------------------*/ + + + +static void prvInitialiseTCBVariables( tskTCB *pxTCB, const signed char * const pcName, unsigned portBASE_TYPE uxPriority, const xMemoryRegion * const xRegions, unsigned short usStackDepth ) +{ + /* Store the function name in the TCB. */ + #if configMAX_TASK_NAME_LEN > 1 + { + /* Don't bring strncpy into the build unnecessarily. */ + strncpy( ( char * ) pxTCB->pcTaskName, ( const char * ) pcName, ( unsigned short ) configMAX_TASK_NAME_LEN ); + } + #endif + pxTCB->pcTaskName[ ( unsigned short ) configMAX_TASK_NAME_LEN - ( unsigned short ) 1 ] = '\0'; + + /* This is used as an array index so must ensure it's not too large. First + remove the privilege bit if one is present. */ + if( uxPriority >= configMAX_PRIORITIES ) + { + uxPriority = configMAX_PRIORITIES - 1; + } + + pxTCB->uxPriority = uxPriority; + #if ( configUSE_MUTEXES == 1 ) + { + pxTCB->uxBasePriority = uxPriority; + } + #endif + + vListInitialiseItem( &( pxTCB->xGenericListItem ) ); + vListInitialiseItem( &( pxTCB->xEventListItem ) ); + + /* Set the pxTCB as a link back from the xListItem. This is so we can get + back to the containing TCB from a generic item in a list. */ + listSET_LIST_ITEM_OWNER( &( pxTCB->xGenericListItem ), pxTCB ); + + /* Event lists are always in priority order. */ + listSET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ), configMAX_PRIORITIES - ( portTickType ) uxPriority ); + listSET_LIST_ITEM_OWNER( &( pxTCB->xEventListItem ), pxTCB ); + + #if ( portCRITICAL_NESTING_IN_TCB == 1 ) + { + pxTCB->uxCriticalNesting = ( unsigned portBASE_TYPE ) 0; + } + #endif + + #if ( configUSE_APPLICATION_TASK_TAG == 1 ) + { + pxTCB->pxTaskTag = NULL; + } + #endif + + #if ( configGENERATE_RUN_TIME_STATS == 1 ) + { + pxTCB->ulRunTimeCounter = 0UL; + } + #endif + + #if ( portUSING_MPU_WRAPPERS == 1 ) + { + vPortStoreTaskMPUSettings( &( pxTCB->xMPUSettings ), xRegions, pxTCB->pxStack, usStackDepth ); + } + #else + { + ( void ) xRegions; + ( void ) usStackDepth; + } + #endif +} +/*-----------------------------------------------------------*/ + +#if ( portUSING_MPU_WRAPPERS == 1 ) + + void vTaskAllocateMPURegions( xTaskHandle xTaskToModify, const xMemoryRegion * const xRegions ) + { + tskTCB *pxTCB; + + if( xTaskToModify == pxCurrentTCB ) + { + xTaskToModify = NULL; + } + + /* If null is passed in here then we are deleting ourselves. */ + pxTCB = prvGetTCBFromHandle( xTaskToModify ); + + vPortStoreTaskMPUSettings( &( pxTCB->xMPUSettings ), xRegions, NULL, 0 ); + } + /*-----------------------------------------------------------*/ +#endif + +static void prvInitialiseTaskLists( void ) +{ +unsigned portBASE_TYPE uxPriority; + + for( uxPriority = 0; uxPriority < configMAX_PRIORITIES; uxPriority++ ) + { + vListInitialise( ( xList * ) &( pxReadyTasksLists[ uxPriority ] ) ); + } + + vListInitialise( ( xList * ) &xDelayedTaskList1 ); + vListInitialise( ( xList * ) &xDelayedTaskList2 ); + vListInitialise( ( xList * ) &xPendingReadyList ); + + #if ( INCLUDE_vTaskDelete == 1 ) + { + vListInitialise( ( xList * ) &xTasksWaitingTermination ); + } + #endif + + #if ( INCLUDE_vTaskSuspend == 1 ) + { + vListInitialise( ( xList * ) &xSuspendedTaskList ); + } + #endif + + /* Start with pxDelayedTaskList using list1 and the pxOverflowDelayedTaskList + using list2. */ + pxDelayedTaskList = &xDelayedTaskList1; + pxOverflowDelayedTaskList = &xDelayedTaskList2; +} +/*-----------------------------------------------------------*/ + +static void prvCheckTasksWaitingTermination( void ) +{ + #if ( INCLUDE_vTaskDelete == 1 ) + { + portBASE_TYPE xListIsEmpty; + + /* ucTasksDeleted is used to prevent vTaskSuspendAll() being called + too often in the idle task. */ + if( uxTasksDeleted > ( unsigned portBASE_TYPE ) 0 ) + { + vTaskSuspendAll(); + xListIsEmpty = listLIST_IS_EMPTY( &xTasksWaitingTermination ); + xTaskResumeAll(); + + if( !xListIsEmpty ) + { + tskTCB *pxTCB; + + portENTER_CRITICAL(); + { + pxTCB = ( tskTCB * ) listGET_OWNER_OF_HEAD_ENTRY( ( ( xList * ) &xTasksWaitingTermination ) ); + vListRemove( &( pxTCB->xGenericListItem ) ); + --uxCurrentNumberOfTasks; + --uxTasksDeleted; + } + portEXIT_CRITICAL(); + + prvDeleteTCB( pxTCB ); + } + } + } + #endif +} +/*-----------------------------------------------------------*/ + +static tskTCB *prvAllocateTCBAndStack( unsigned short usStackDepth, portSTACK_TYPE *puxStackBuffer ) +{ +tskTCB *pxNewTCB; + + /* Allocate space for the TCB. Where the memory comes from depends on + the implementation of the port malloc function. */ + pxNewTCB = ( tskTCB * ) pvPortMalloc( sizeof( tskTCB ) ); + + if( pxNewTCB != NULL ) + { + /* Allocate space for the stack used by the task being created. + The base of the stack memory stored in the TCB so the task can + be deleted later if required. */ + pxNewTCB->pxStack = ( portSTACK_TYPE * ) pvPortMallocAligned( ( ( ( size_t )usStackDepth ) * sizeof( portSTACK_TYPE ) ), puxStackBuffer ); + + if( pxNewTCB->pxStack == NULL ) + { + /* Could not allocate the stack. Delete the allocated TCB. */ + vPortFree( pxNewTCB ); + pxNewTCB = NULL; + } + else + { + /* Just to help debugging. */ + memset( pxNewTCB->pxStack, tskSTACK_FILL_BYTE, usStackDepth * sizeof( portSTACK_TYPE ) ); + } + } + + return pxNewTCB; +} +/*-----------------------------------------------------------*/ + +#if ( configUSE_TRACE_FACILITY == 1 ) + + static void prvListTaskWithinSingleList( const signed char *pcWriteBuffer, xList *pxList, signed char cStatus ) + { + volatile tskTCB *pxNextTCB, *pxFirstTCB; + unsigned short usStackRemaining; + + /* Write the details of all the TCB's in pxList into the buffer. */ + listGET_OWNER_OF_NEXT_ENTRY( pxFirstTCB, pxList ); + do + { + listGET_OWNER_OF_NEXT_ENTRY( pxNextTCB, pxList ); + #if ( portSTACK_GROWTH > 0 ) + { + usStackRemaining = usTaskCheckFreeStackSpace( ( unsigned char * ) pxNextTCB->pxEndOfStack ); + } + #else + { + usStackRemaining = usTaskCheckFreeStackSpace( ( unsigned char * ) pxNextTCB->pxStack ); + } + #endif + + sprintf( pcStatusString, ( char * ) "%s\t\t%c\t%u\t%u\t%u\r\n", pxNextTCB->pcTaskName, cStatus, ( unsigned int ) pxNextTCB->uxPriority, usStackRemaining, ( unsigned int ) pxNextTCB->uxTCBNumber ); + strcat( ( char * ) pcWriteBuffer, ( char * ) pcStatusString ); + + } while( pxNextTCB != pxFirstTCB ); + } + +#endif +/*-----------------------------------------------------------*/ + +#if ( configGENERATE_RUN_TIME_STATS == 1 ) + + static void prvGenerateRunTimeStatsForTasksInList( const signed char *pcWriteBuffer, xList *pxList, unsigned long ulTotalRunTime ) + { + volatile tskTCB *pxNextTCB, *pxFirstTCB; + unsigned long ulStatsAsPercentage; + + /* Write the run time stats of all the TCB's in pxList into the buffer. */ + listGET_OWNER_OF_NEXT_ENTRY( pxFirstTCB, pxList ); + do + { + /* Get next TCB in from the list. */ + listGET_OWNER_OF_NEXT_ENTRY( pxNextTCB, pxList ); + + /* Divide by zero check. */ + if( ulTotalRunTime > 0UL ) + { + /* Has the task run at all? */ + if( pxNextTCB->ulRunTimeCounter == 0 ) + { + /* The task has used no CPU time at all. */ + sprintf( pcStatsString, ( char * ) "%s\t\t0\t\t0%%\r\n", pxNextTCB->pcTaskName ); + } + else + { + /* What percentage of the total run time as the task used? + This will always be rounded down to the nearest integer. */ + ulStatsAsPercentage = ( 100UL * pxNextTCB->ulRunTimeCounter ) / ulTotalRunTime; + + if( ulStatsAsPercentage > 0UL ) + { + sprintf( pcStatsString, ( char * ) "%s\t\t%u\t\t%u%%\r\n", pxNextTCB->pcTaskName, ( unsigned int ) pxNextTCB->ulRunTimeCounter, ( unsigned int ) ulStatsAsPercentage ); + } + else + { + /* If the percentage is zero here then the task has + consumed less than 1% of the total run time. */ + sprintf( pcStatsString, ( char * ) "%s\t\t%u\t\t<1%%\r\n", pxNextTCB->pcTaskName, ( unsigned int ) pxNextTCB->ulRunTimeCounter ); + } + } + + strcat( ( char * ) pcWriteBuffer, ( char * ) pcStatsString ); + } + + } while( pxNextTCB != pxFirstTCB ); + } + +#endif +/*-----------------------------------------------------------*/ + +#if ( ( configUSE_TRACE_FACILITY == 1 ) || ( INCLUDE_uxTaskGetStackHighWaterMark == 1 ) ) + + static unsigned short usTaskCheckFreeStackSpace( const unsigned char * pucStackByte ) + { + register unsigned short usCount = 0; + + while( *pucStackByte == tskSTACK_FILL_BYTE ) + { + pucStackByte -= portSTACK_GROWTH; + usCount++; + } + + usCount /= sizeof( portSTACK_TYPE ); + + return usCount; + } + +#endif +/*-----------------------------------------------------------*/ + +#if ( INCLUDE_uxTaskGetStackHighWaterMark == 1 ) + + unsigned portBASE_TYPE uxTaskGetStackHighWaterMark( xTaskHandle xTask ) + { + tskTCB *pxTCB; + unsigned char *pcEndOfStack; + unsigned portBASE_TYPE uxReturn; + + pxTCB = prvGetTCBFromHandle( xTask ); + + #if portSTACK_GROWTH < 0 + { + pcEndOfStack = ( unsigned char * ) pxTCB->pxStack; + } + #else + { + pcEndOfStack = ( unsigned char * ) pxTCB->pxEndOfStack; + } + #endif + + uxReturn = ( unsigned portBASE_TYPE ) usTaskCheckFreeStackSpace( pcEndOfStack ); + + return uxReturn; + } + +#endif +/*-----------------------------------------------------------*/ + +#if ( ( INCLUDE_vTaskDelete == 1 ) || ( INCLUDE_vTaskCleanUpResources == 1 ) ) + + static void prvDeleteTCB( tskTCB *pxTCB ) + { + /* Free up the memory allocated by the scheduler for the task. It is up to + the task to free any memory allocated at the application level. */ + vPortFreeAligned( pxTCB->pxStack ); + vPortFree( pxTCB ); + } + +#endif + + +/*-----------------------------------------------------------*/ + +#if ( INCLUDE_xTaskGetCurrentTaskHandle == 1 ) + + xTaskHandle xTaskGetCurrentTaskHandle( void ) + { + xTaskHandle xReturn; + + /* A critical section is not required as this is not called from + an interrupt and the current TCB will always be the same for any + individual execution thread. */ + xReturn = pxCurrentTCB; + + return xReturn; + } + +#endif + +/*-----------------------------------------------------------*/ + +#if ( INCLUDE_xTaskGetSchedulerState == 1 ) + + portBASE_TYPE xTaskGetSchedulerState( void ) + { + portBASE_TYPE xReturn; + + if( xSchedulerRunning == pdFALSE ) + { + xReturn = taskSCHEDULER_NOT_STARTED; + } + else + { + if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) + { + xReturn = taskSCHEDULER_RUNNING; + } + else + { + xReturn = taskSCHEDULER_SUSPENDED; + } + } + + return xReturn; + } + +#endif +/*-----------------------------------------------------------*/ + +#if ( configUSE_MUTEXES == 1 ) + + void vTaskPriorityInherit( xTaskHandle * const pxMutexHolder ) + { + tskTCB * const pxTCB = ( tskTCB * ) pxMutexHolder; + + if( pxTCB->uxPriority < pxCurrentTCB->uxPriority ) + { + /* Adjust the mutex holder state to account for its new priority. */ + listSET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ), configMAX_PRIORITIES - ( portTickType ) pxCurrentTCB->uxPriority ); + + /* If the task being modified is in the ready state it will need to + be moved in to a new list. */ + if( listIS_CONTAINED_WITHIN( &( pxReadyTasksLists[ pxTCB->uxPriority ] ), &( pxTCB->xGenericListItem ) ) ) + { + vListRemove( &( pxTCB->xGenericListItem ) ); + + /* Inherit the priority before being moved into the new list. */ + pxTCB->uxPriority = pxCurrentTCB->uxPriority; + prvAddTaskToReadyQueue( pxTCB ); + } + else + { + /* Just inherit the priority. */ + pxTCB->uxPriority = pxCurrentTCB->uxPriority; + } + } + } + +#endif +/*-----------------------------------------------------------*/ + +#if ( configUSE_MUTEXES == 1 ) + + void vTaskPriorityDisinherit( xTaskHandle * const pxMutexHolder ) + { + tskTCB * const pxTCB = ( tskTCB * ) pxMutexHolder; + + if( pxMutexHolder != NULL ) + { + if( pxTCB->uxPriority != pxTCB->uxBasePriority ) + { + /* We must be the running task to be able to give the mutex back. + Remove ourselves from the ready list we currently appear in. */ + vListRemove( &( pxTCB->xGenericListItem ) ); + + /* Disinherit the priority before adding ourselves into the new + ready list. */ + pxTCB->uxPriority = pxTCB->uxBasePriority; + listSET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ), configMAX_PRIORITIES - ( portTickType ) pxTCB->uxPriority ); + prvAddTaskToReadyQueue( pxTCB ); + } + } + } + +#endif +/*-----------------------------------------------------------*/ + +#if ( portCRITICAL_NESTING_IN_TCB == 1 ) + + void vTaskEnterCritical( void ) + { + portDISABLE_INTERRUPTS(); + + if( xSchedulerRunning != pdFALSE ) + { + pxCurrentTCB->uxCriticalNesting++; + } + } + +#endif +/*-----------------------------------------------------------*/ + +#if ( portCRITICAL_NESTING_IN_TCB == 1 ) + +void vTaskExitCritical( void ) +{ + if( xSchedulerRunning != pdFALSE ) + { + if( pxCurrentTCB->uxCriticalNesting > 0 ) + { + pxCurrentTCB->uxCriticalNesting--; + + if( pxCurrentTCB->uxCriticalNesting == 0 ) + { + portENABLE_INTERRUPTS(); + } + } + } +} + +#endif +/*-----------------------------------------------------------*/ + + + + diff --git a/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/tasks_posix.c b/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/tasks_posix.c deleted file mode 100644 index f9fb0dcc0..000000000 --- a/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/tasks_posix.c +++ /dev/null @@ -1,2333 +0,0 @@ -/* - FreeRTOS V6.0.4 - Copyright (C) 2010 Real Time Engineers Ltd. - - *************************************************************************** - * * - * If you are: * - * * - * + New to FreeRTOS, * - * + Wanting to learn FreeRTOS or multitasking in general quickly * - * + Looking for basic training, * - * + Wanting to improve your FreeRTOS skills and productivity * - * * - * then take a look at the FreeRTOS eBook * - * * - * "Using the FreeRTOS Real Time Kernel - a Practical Guide" * - * http://www.FreeRTOS.org/Documentation * - * * - * A pdf reference manual is also available. Both are usually delivered * - * to your inbox within 20 minutes to two hours when purchased between 8am * - * and 8pm GMT (although please allow up to 24 hours in case of * - * exceptional circumstances). Thank you for your support! * - * * - *************************************************************************** - - This file is part of the FreeRTOS distribution. - - FreeRTOS is free software; you can redistribute it and/or modify it under - the terms of the GNU General Public License (version 2) as published by the - Free Software Foundation AND MODIFIED BY the FreeRTOS exception. - ***NOTE*** The exception to the GPL is included to allow you to distribute - a combined work that includes FreeRTOS without being obliged to provide the - source code for proprietary components outside of the FreeRTOS kernel. - FreeRTOS is distributed in the hope that it will be useful, but WITHOUT - ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or - FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for - more details. You should have received a copy of the GNU General Public - License and the FreeRTOS license exception along with FreeRTOS; if not it - can be viewed here: http://www.freertos.org/a00114.html and also obtained - by writing to Richard Barry, contact details for whom are available on the - FreeRTOS WEB site. - - 1 tab == 4 spaces! - - http://www.FreeRTOS.org - Documentation, latest information, license and - contact details. - - http://www.SafeRTOS.com - A version that is certified for use in safety - critical systems. - - http://www.OpenRTOS.com - Commercial support, development, porting, - licensing and training services. -*/ - - -#include -#include -#include -#include -#include -#include - -/* Defining MPU_WRAPPERS_INCLUDED_FROM_API_FILE prevents task.h from redefining -all the API functions to use the MPU wrappers. That should only be done when -task.h is included from an application file. */ -#define MPU_WRAPPERS_INCLUDED_FROM_API_FILE - -#include "FreeRTOS.h" -#include "task.h" -#include "StackMacros.h" - -#undef MPU_WRAPPERS_INCLUDED_FROM_API_FILE - -/* - * Macro to define the amount of stack available to the idle task. - */ -#define tskIDLE_STACK_SIZE configMINIMAL_STACK_SIZE - -/* - * Task control block. A task control block (TCB) is allocated to each task, - * and stores the context of the task. - */ -typedef struct tskTaskControlBlock -{ - volatile portSTACK_TYPE *pxTopOfStack; /*< Points to the location of the last item placed on the tasks stack. THIS MUST BE THE FIRST MEMBER OF THE STRUCT. */ - - #if ( portUSING_MPU_WRAPPERS == 1 ) - xMPU_SETTINGS xMPUSettings; /*< The MPU settings are defined as part of the port layer. THIS MUST BE THE SECOND MEMBER OF THE STRUCT. */ - #endif - - xListItem xGenericListItem; /*< List item used to place the TCB in ready and blocked queues. */ - xListItem xEventListItem; /*< List item used to place the TCB in event lists. */ - unsigned portBASE_TYPE uxPriority; /*< The priority of the task where 0 is the lowest priority. */ - portSTACK_TYPE *pxStack; /*< Points to the start of the stack. */ - signed char pcTaskName[ configMAX_TASK_NAME_LEN ];/*< Descriptive name given to the task when created. Facilitates debugging only. */ - - #if ( portSTACK_GROWTH > 0 ) - portSTACK_TYPE *pxEndOfStack; /*< Used for stack overflow checking on architectures where the stack grows up from low memory. */ - #endif - - #if ( portCRITICAL_NESTING_IN_TCB == 1 ) - unsigned portBASE_TYPE uxCriticalNesting; - #endif - - #if ( configUSE_TRACE_FACILITY == 1 ) - unsigned portBASE_TYPE uxTCBNumber; /*< This is used for tracing the scheduler and making debugging easier only. */ - #endif - - #if ( configUSE_MUTEXES == 1 ) - unsigned portBASE_TYPE uxBasePriority; /*< The priority last assigned to the task - used by the priority inheritance mechanism. */ - #endif - - #if ( configUSE_APPLICATION_TASK_TAG == 1 ) - pdTASK_HOOK_CODE pxTaskTag; - #endif - - #if ( configGENERATE_RUN_TIME_STATS == 1 ) - unsigned long ulRunTimeCounter; /*< Used for calculating how much CPU time each task is utilising. */ - #endif - -} tskTCB; - - -/* - * Some kernel aware debuggers require data to be viewed to be global, rather - * than file scope. - */ -#ifdef portREMOVE_STATIC_QUALIFIER - #define static -#endif - -/*lint -e956 */ -PRIVILEGED_DATA tskTCB * volatile pxCurrentTCB = NULL; - -/* Lists for ready and blocked tasks. --------------------*/ - -PRIVILEGED_DATA static xList pxReadyTasksLists[ configMAX_PRIORITIES ]; /*< Prioritised ready tasks. */ -PRIVILEGED_DATA static xList xDelayedTaskList1; /*< Delayed tasks. */ -PRIVILEGED_DATA static xList xDelayedTaskList2; /*< Delayed tasks (two lists are used - one for delays that have overflowed the current tick count. */ -PRIVILEGED_DATA static xList * volatile pxDelayedTaskList ; /*< Points to the delayed task list currently being used. */ -PRIVILEGED_DATA static xList * volatile pxOverflowDelayedTaskList; /*< Points to the delayed task list currently being used to hold tasks that have overflowed the current tick count. */ -PRIVILEGED_DATA static xList xPendingReadyList; /*< Tasks that have been readied while the scheduler was suspended. They will be moved to the ready queue when the scheduler is resumed. */ - -#if ( INCLUDE_vTaskDelete == 1 ) - - PRIVILEGED_DATA static volatile xList xTasksWaitingTermination; /*< Tasks that have been deleted - but the their memory not yet freed. */ - PRIVILEGED_DATA static volatile unsigned portBASE_TYPE uxTasksDeleted = ( unsigned portBASE_TYPE ) 0; - -#endif - -#if ( INCLUDE_vTaskSuspend == 1 ) - - PRIVILEGED_DATA static xList xSuspendedTaskList; /*< Tasks that are currently suspended. */ - -#endif - -/* File private variables. --------------------------------*/ -PRIVILEGED_DATA static volatile unsigned portBASE_TYPE uxCurrentNumberOfTasks = ( unsigned portBASE_TYPE ) 0; -PRIVILEGED_DATA static volatile portTickType xTickCount = ( portTickType ) 0; -PRIVILEGED_DATA static unsigned portBASE_TYPE uxTopUsedPriority = tskIDLE_PRIORITY; -PRIVILEGED_DATA static volatile unsigned portBASE_TYPE uxTopReadyPriority = tskIDLE_PRIORITY; -PRIVILEGED_DATA static volatile signed portBASE_TYPE xSchedulerRunning = pdFALSE; -PRIVILEGED_DATA static volatile unsigned portBASE_TYPE uxSchedulerSuspended = ( unsigned portBASE_TYPE ) pdFALSE; -PRIVILEGED_DATA static volatile unsigned portBASE_TYPE uxMissedTicks = ( unsigned portBASE_TYPE ) 0; -PRIVILEGED_DATA static volatile portBASE_TYPE xMissedYield = ( portBASE_TYPE ) pdFALSE; -PRIVILEGED_DATA static volatile portBASE_TYPE xNumOfOverflows = ( portBASE_TYPE ) 0; -PRIVILEGED_DATA static unsigned portBASE_TYPE uxTaskNumber = ( unsigned portBASE_TYPE ) 0; - -#if ( configGENERATE_RUN_TIME_STATS == 1 ) - - PRIVILEGED_DATA static char pcStatsString[ 50 ] ; - PRIVILEGED_DATA static unsigned long ulTaskSwitchedInTime = 0UL; /*< Holds the value of a timer/counter the last time a task was switched in. */ - static void prvGenerateRunTimeStatsForTasksInList( const signed char *pcWriteBuffer, xList *pxList, unsigned long ulTotalRunTime ) PRIVILEGED_FUNCTION; - -#endif - -/* Debugging and trace facilities private variables and macros. ------------*/ - -/* - * The value used to fill the stack of a task when the task is created. This - * is used purely for checking the high water mark for tasks. - */ -#define tskSTACK_FILL_BYTE ( 0xa5 ) - -/* - * Macros used by vListTask to indicate which state a task is in. - */ -#define tskBLOCKED_CHAR ( ( signed char ) 'B' ) -#define tskREADY_CHAR ( ( signed char ) 'R' ) -#define tskDELETED_CHAR ( ( signed char ) 'D' ) -#define tskSUSPENDED_CHAR ( ( signed char ) 'S' ) - -/* - * Macros and private variables used by the trace facility. - */ -#if ( configUSE_TRACE_FACILITY == 1 ) - - #define tskSIZE_OF_EACH_TRACE_LINE ( ( unsigned long ) ( sizeof( unsigned long ) + sizeof( unsigned long ) ) ) - PRIVILEGED_DATA static volatile signed char * volatile pcTraceBuffer; - PRIVILEGED_DATA static signed char *pcTraceBufferStart; - PRIVILEGED_DATA static signed char *pcTraceBufferEnd; - PRIVILEGED_DATA static signed portBASE_TYPE xTracing = pdFALSE; - static unsigned portBASE_TYPE uxPreviousTask = 255; - PRIVILEGED_DATA static char pcStatusString[ 50 ]; - -#endif - -/*-----------------------------------------------------------*/ - -/* - * Macro that writes a trace of scheduler activity to a buffer. This trace - * shows which task is running when and is very useful as a debugging tool. - * As this macro is called each context switch it is a good idea to undefine - * it if not using the facility. - */ -#if ( configUSE_TRACE_FACILITY == 1 ) - - #define vWriteTraceToBuffer() \ - { \ - if( xTracing ) \ - { \ - if( uxPreviousTask != pxCurrentTCB->uxTCBNumber ) \ - { \ - if( ( pcTraceBuffer + tskSIZE_OF_EACH_TRACE_LINE ) < pcTraceBufferEnd ) \ - { \ - uxPreviousTask = pxCurrentTCB->uxTCBNumber; \ - *( unsigned long * ) pcTraceBuffer = ( unsigned long ) xTickCount; \ - pcTraceBuffer += sizeof( unsigned long ); \ - *( unsigned long * ) pcTraceBuffer = ( unsigned long ) uxPreviousTask; \ - pcTraceBuffer += sizeof( unsigned long ); \ - } \ - else \ - { \ - xTracing = pdFALSE; \ - } \ - } \ - } \ - } - -#else - - #define vWriteTraceToBuffer() - -#endif -/*-----------------------------------------------------------*/ - -/* - * Place the task represented by pxTCB into the appropriate ready queue for - * the task. It is inserted at the end of the list. One quirk of this is - * that if the task being inserted is at the same priority as the currently - * executing task, then it will only be rescheduled after the currently - * executing task has been rescheduled. - */ -#define prvAddTaskToReadyQueue( pxTCB ) \ -{ \ - if( pxTCB->uxPriority > uxTopReadyPriority ) \ - { \ - uxTopReadyPriority = pxTCB->uxPriority; \ - } \ - vListInsertEnd( ( xList * ) &( pxReadyTasksLists[ pxTCB->uxPriority ] ), &( pxTCB->xGenericListItem ) ); \ -} -/*-----------------------------------------------------------*/ - -/* - * Macro that looks at the list of tasks that are currently delayed to see if - * any require waking. - * - * Tasks are stored in the queue in the order of their wake time - meaning - * once one tasks has been found whose timer has not expired we need not look - * any further down the list. - */ -#define prvCheckDelayedTasks() \ -{ \ -register tskTCB *pxTCB; \ - \ - while( ( pxTCB = ( tskTCB * ) listGET_OWNER_OF_HEAD_ENTRY( pxDelayedTaskList ) ) != NULL ) \ - { \ - if( xTickCount < listGET_LIST_ITEM_VALUE( &( pxTCB->xGenericListItem ) ) ) \ - { \ - break; \ - } \ - vListRemove( &( pxTCB->xGenericListItem ) ); \ - /* Is the task waiting on an event also? */ \ - if( pxTCB->xEventListItem.pvContainer ) \ - { \ - vListRemove( &( pxTCB->xEventListItem ) ); \ - } \ - prvAddTaskToReadyQueue( pxTCB ); \ - } \ -} -/*-----------------------------------------------------------*/ - -/* - * Several functions take an xTaskHandle parameter that can optionally be NULL, - * where NULL is used to indicate that the handle of the currently executing - * task should be used in place of the parameter. This macro simply checks to - * see if the parameter is NULL and returns a pointer to the appropriate TCB. - */ -#define prvGetTCBFromHandle( pxHandle ) ( ( pxHandle == NULL ) ? ( tskTCB * ) pxCurrentTCB : ( tskTCB * ) pxHandle ) - - -/* File private functions. --------------------------------*/ - -/* - * Utility to ready a TCB for a given task. Mainly just copies the parameters - * into the TCB structure. - */ -static void prvInitialiseTCBVariables( tskTCB *pxTCB, const signed char * const pcName, unsigned portBASE_TYPE uxPriority, const xMemoryRegion * const xRegions, unsigned short usStackDepth ) PRIVILEGED_FUNCTION; - -/* - * Utility to ready all the lists used by the scheduler. This is called - * automatically upon the creation of the first task. - */ -static void prvInitialiseTaskLists( void ) PRIVILEGED_FUNCTION; - -/* - * The idle task, which as all tasks is implemented as a never ending loop. - * The idle task is automatically created and added to the ready lists upon - * creation of the first user task. - * - * The portTASK_FUNCTION_PROTO() macro is used to allow port/compiler specific - * language extensions. The equivalent prototype for this function is: - * - * void prvIdleTask( void *pvParameters ); - * - */ -static portTASK_FUNCTION_PROTO( prvIdleTask, pvParameters ); - -/* - * Utility to free all memory allocated by the scheduler to hold a TCB, - * including the stack pointed to by the TCB. - * - * This does not free memory allocated by the task itself (i.e. memory - * allocated by calls to pvPortMalloc from within the tasks application code). - */ -#if ( ( INCLUDE_vTaskDelete == 1 ) || ( INCLUDE_vTaskCleanUpResources == 1 ) ) - - static void prvDeleteTCB( tskTCB *pxTCB ) PRIVILEGED_FUNCTION; - -#endif - -/* - * Used only by the idle task. This checks to see if anything has been placed - * in the list of tasks waiting to be deleted. If so the task is cleaned up - * and its TCB deleted. - */ -static void prvCheckTasksWaitingTermination( void ) PRIVILEGED_FUNCTION; - -/* - * Allocates memory from the heap for a TCB and associated stack. Checks the - * allocation was successful. - */ -static tskTCB *prvAllocateTCBAndStack( unsigned short usStackDepth, portSTACK_TYPE *puxStackBuffer ) PRIVILEGED_FUNCTION; - -/* - * Called from vTaskList. vListTasks details all the tasks currently under - * control of the scheduler. The tasks may be in one of a number of lists. - * prvListTaskWithinSingleList accepts a list and details the tasks from - * within just that list. - * - * THIS FUNCTION IS INTENDED FOR DEBUGGING ONLY, AND SHOULD NOT BE CALLED FROM - * NORMAL APPLICATION CODE. - */ -#if ( configUSE_TRACE_FACILITY == 1 ) - - static void prvListTaskWithinSingleList( const signed char *pcWriteBuffer, xList *pxList, signed char cStatus ) PRIVILEGED_FUNCTION; - -#endif - -/* - * When a task is created, the stack of the task is filled with a known value. - * This function determines the 'high water mark' of the task stack by - * determining how much of the stack remains at the original preset value. - */ -#if ( ( configUSE_TRACE_FACILITY == 1 ) || ( INCLUDE_uxTaskGetStackHighWaterMark == 1 ) ) - - static unsigned short usTaskCheckFreeStackSpace( const unsigned char * pucStackByte ) PRIVILEGED_FUNCTION; - -#endif - - -/*lint +e956 */ - - - -/*----------------------------------------------------------- - * TASK CREATION API documented in task.h - *----------------------------------------------------------*/ - -signed portBASE_TYPE xTaskGenericCreate( pdTASK_CODE pxTaskCode, const signed char * const pcName, unsigned short usStackDepth, void *pvParameters, unsigned portBASE_TYPE uxPriority, xTaskHandle *pxCreatedTask, portSTACK_TYPE *puxStackBuffer, const xMemoryRegion * const xRegions ) -{ -signed portBASE_TYPE xReturn; -tskTCB * pxNewTCB; - - /* Allocate the memory required by the TCB and stack for the new task, - checking that the allocation was successful. */ - pxNewTCB = prvAllocateTCBAndStack( usStackDepth, puxStackBuffer ); - - if( pxNewTCB != NULL ) - { - portSTACK_TYPE *pxTopOfStack; - - #if( portUSING_MPU_WRAPPERS == 1 ) - /* Should the task be created in privileged mode? */ - portBASE_TYPE xRunPrivileged; - if( ( uxPriority & portPRIVILEGE_BIT ) != 0x00 ) - { - xRunPrivileged = pdTRUE; - } - else - { - xRunPrivileged = pdFALSE; - } - uxPriority &= ~portPRIVILEGE_BIT; - #endif /* portUSING_MPU_WRAPPERS == 1 */ - - /* Calculate the top of stack address. This depends on whether the - stack grows from high memory to low (as per the 80x86) or visa versa. - portSTACK_GROWTH is used to make the result positive or negative as - required by the port. */ - #if( portSTACK_GROWTH < 0 ) - { - pxTopOfStack = pxNewTCB->pxStack + ( usStackDepth - 1 ); - pxTopOfStack = ( portSTACK_TYPE * ) ( ( ( unsigned long ) pxTopOfStack ) & ( ( unsigned long ) ~portBYTE_ALIGNMENT_MASK ) ); - } - #else - { - pxTopOfStack = pxNewTCB->pxStack; - - /* If we want to use stack checking on architectures that use - a positive stack growth direction then we also need to store the - other extreme of the stack space. */ - pxNewTCB->pxEndOfStack = pxNewTCB->pxStack + ( usStackDepth - 1 ); - } - #endif - - /* Setup the newly allocated TCB with the initial state of the task. */ - prvInitialiseTCBVariables( pxNewTCB, pcName, uxPriority, xRegions, usStackDepth ); - - /* Initialize the TCB stack to look as if the task was already running, - but had been interrupted by the scheduler. The return address is set - to the start of the task function. Once the stack has been initialised - the top of stack variable is updated. */ - #if( portUSING_MPU_WRAPPERS == 1 ) - { - pxNewTCB->pxTopOfStack = pxPortInitialiseStack( pxTopOfStack, pxTaskCode, pvParameters, xRunPrivileged ); - } - #else - { - pxNewTCB->pxTopOfStack = pxPortInitialiseStack( pxTopOfStack, pxTaskCode, pvParameters ); - } - #endif - - /* We are going to manipulate the task queues to add this task to a - ready list, so must make sure no interrupts occur. */ - portENTER_CRITICAL(); - { - uxCurrentNumberOfTasks++; - if( uxCurrentNumberOfTasks == ( unsigned portBASE_TYPE ) 1 ) - { - /* As this is the first task it must also be the current task. */ - pxCurrentTCB = pxNewTCB; - - /* This is the first task to be created so do the preliminary - initialisation required. We will not recover if this call - fails, but we will report the failure. */ - prvInitialiseTaskLists(); - } - else - { - /* If the scheduler is not already running, make this task the - current task if it is the highest priority task to be created - so far. */ - if( xSchedulerRunning == pdFALSE ) - { - if( pxCurrentTCB->uxPriority <= uxPriority ) - { - pxCurrentTCB = pxNewTCB; - } - } - } - - /* Remember the top priority to make context switching faster. Use - the priority in pxNewTCB as this has been capped to a valid value. */ - if( pxNewTCB->uxPriority > uxTopUsedPriority ) - { - uxTopUsedPriority = pxNewTCB->uxPriority; - } - - #if ( configUSE_TRACE_FACILITY == 1 ) - { - /* Add a counter into the TCB for tracing only. */ - pxNewTCB->uxTCBNumber = uxTaskNumber; - } - #endif - uxTaskNumber++; - - prvAddTaskToReadyQueue( pxNewTCB ); - - xReturn = pdPASS; - traceTASK_CREATE( pxNewTCB ); - } - portEXIT_CRITICAL(); - } - else - { - xReturn = errCOULD_NOT_ALLOCATE_REQUIRED_MEMORY; - traceTASK_CREATE_FAILED( pxNewTCB ); - } - - if( xReturn == pdPASS ) - { - if( ( void * ) pxCreatedTask != NULL ) - { - /* Pass the TCB out - in an anonymous way. The calling function/ - task can use this as a handle to delete the task later if - required.*/ - *pxCreatedTask = ( xTaskHandle ) pxNewTCB; - } - - if( xSchedulerRunning != pdFALSE ) - { - /* If the created task is of a higher priority than the current task - then it should run now. */ - if( pxCurrentTCB->uxPriority < uxPriority ) - { - portYIELD_WITHIN_API(); - } - } - } - - return xReturn; -} -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_vTaskDelete == 1 ) - - void vTaskDelete( xTaskHandle pxTaskToDelete ) - { - tskTCB *pxTCB; - - portENTER_CRITICAL(); - { - /* Ensure a yield is performed if the current task is being - deleted. */ - if( pxTaskToDelete == pxCurrentTCB ) - { - pxTaskToDelete = NULL; - } - - /* If null is passed in here then we are deleting ourselves. */ - pxTCB = prvGetTCBFromHandle( pxTaskToDelete ); - - /* Remove task from the ready list and place in the termination list. - This will stop the task from be scheduled. The idle task will check - the termination list and free up any memory allocated by the - scheduler for the TCB and stack. */ - vListRemove( &( pxTCB->xGenericListItem ) ); - - /* Is the task waiting on an event also? */ - if( pxTCB->xEventListItem.pvContainer ) - { - vListRemove( &( pxTCB->xEventListItem ) ); - } - - vListInsertEnd( ( xList * ) &xTasksWaitingTermination, &( pxTCB->xGenericListItem ) ); - - /* Increment the ucTasksDeleted variable so the idle task knows - there is a task that has been deleted and that it should therefore - check the xTasksWaitingTermination list. */ - ++uxTasksDeleted; - - /* Increment the uxTaskNumberVariable also so kernel aware debuggers - can detect that the task lists need re-generating. */ - uxTaskNumber++; - - traceTASK_DELETE( pxTCB ); - } - portEXIT_CRITICAL(); - - /* Force a reschedule if we have just deleted the current task. */ - if( xSchedulerRunning != pdFALSE ) - { - if( ( void * ) pxTaskToDelete == NULL ) - { - portYIELD_WITHIN_API(); - } - } - } - -#endif - - - - - - -/*----------------------------------------------------------- - * TASK CONTROL API documented in task.h - *----------------------------------------------------------*/ - -#if ( INCLUDE_vTaskDelayUntil == 1 ) - - void vTaskDelayUntil( portTickType * const pxPreviousWakeTime, portTickType xTimeIncrement ) - { - portTickType xTimeToWake; - portBASE_TYPE xAlreadyYielded, xShouldDelay = pdFALSE; - - vTaskSuspendAll(); - { - /* Generate the tick time at which the task wants to wake. */ - xTimeToWake = *pxPreviousWakeTime + xTimeIncrement; - - if( xTickCount < *pxPreviousWakeTime ) - { - /* The tick count has overflowed since this function was - lasted called. In this case the only time we should ever - actually delay is if the wake time has also overflowed, - and the wake time is greater than the tick time. When this - is the case it is as if neither time had overflowed. */ - if( ( xTimeToWake < *pxPreviousWakeTime ) && ( xTimeToWake > xTickCount ) ) - { - xShouldDelay = pdTRUE; - } - } - else - { - /* The tick time has not overflowed. In this case we will - delay if either the wake time has overflowed, and/or the - tick time is less than the wake time. */ - if( ( xTimeToWake < *pxPreviousWakeTime ) || ( xTimeToWake > xTickCount ) ) - { - xShouldDelay = pdTRUE; - } - } - - /* Update the wake time ready for the next call. */ - *pxPreviousWakeTime = xTimeToWake; - - if( xShouldDelay ) - { - traceTASK_DELAY_UNTIL(); - - /* We must remove ourselves from the ready list before adding - ourselves to the blocked list as the same list item is used for - both lists. */ - vListRemove( ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - - /* The list item will be inserted in wake time order. */ - listSET_LIST_ITEM_VALUE( &( pxCurrentTCB->xGenericListItem ), xTimeToWake ); - - if( xTimeToWake < xTickCount ) - { - /* Wake time has overflowed. Place this item in the - overflow list. */ - vListInsert( ( xList * ) pxOverflowDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - } - else - { - /* The wake time has not overflowed, so we can use the - current block list. */ - vListInsert( ( xList * ) pxDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - } - } - } - xAlreadyYielded = xTaskResumeAll(); - - /* Force a reschedule if xTaskResumeAll has not already done so, we may - have put ourselves to sleep. */ - if( !xAlreadyYielded ) - { - portYIELD_WITHIN_API(); - } - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_vTaskDelay == 1 ) - - void vTaskDelay( portTickType xTicksToDelay ) - { - portTickType xTimeToWake; - signed portBASE_TYPE xAlreadyYielded = pdFALSE; - - /* A delay time of zero just forces a reschedule. */ - if( xTicksToDelay > ( portTickType ) 0 ) - { - vTaskSuspendAll(); - { - traceTASK_DELAY(); - - /* A task that is removed from the event list while the - scheduler is suspended will not get placed in the ready - list or removed from the blocked list until the scheduler - is resumed. - - This task cannot be in an event list as it is the currently - executing task. */ - - /* Calculate the time to wake - this may overflow but this is - not a problem. */ - xTimeToWake = xTickCount + xTicksToDelay; - - /* We must remove ourselves from the ready list before adding - ourselves to the blocked list as the same list item is used for - both lists. */ - vListRemove( ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - - /* The list item will be inserted in wake time order. */ - listSET_LIST_ITEM_VALUE( &( pxCurrentTCB->xGenericListItem ), xTimeToWake ); - - if( xTimeToWake < xTickCount ) - { - /* Wake time has overflowed. Place this item in the - overflow list. */ - vListInsert( ( xList * ) pxOverflowDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - } - else - { - /* The wake time has not overflowed, so we can use the - current block list. */ - vListInsert( ( xList * ) pxDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - } - } - xAlreadyYielded = xTaskResumeAll(); - } - - /* Force a reschedule if xTaskResumeAll has not already done so, we may - have put ourselves to sleep. */ - if( !xAlreadyYielded ) - { - portYIELD_WITHIN_API(); - } - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_uxTaskPriorityGet == 1 ) - - unsigned portBASE_TYPE uxTaskPriorityGet( xTaskHandle pxTask ) - { - tskTCB *pxTCB; - unsigned portBASE_TYPE uxReturn; - - portENTER_CRITICAL(); - { - /* If null is passed in here then we are changing the - priority of the calling function. */ - pxTCB = prvGetTCBFromHandle( pxTask ); - uxReturn = pxTCB->uxPriority; - } - portEXIT_CRITICAL(); - - return uxReturn; - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_vTaskPrioritySet == 1 ) - - void vTaskPrioritySet( xTaskHandle pxTask, unsigned portBASE_TYPE uxNewPriority ) - { - tskTCB *pxTCB; - unsigned portBASE_TYPE uxCurrentPriority, xYieldRequired = pdFALSE; - - /* Ensure the new priority is valid. */ - if( uxNewPriority >= configMAX_PRIORITIES ) - { - uxNewPriority = configMAX_PRIORITIES - 1; - } - - portENTER_CRITICAL(); - { - if( pxTask == pxCurrentTCB ) - { - pxTask = NULL; - } - - /* If null is passed in here then we are changing the - priority of the calling function. */ - pxTCB = prvGetTCBFromHandle( pxTask ); - - traceTASK_PRIORITY_SET( pxTask, uxNewPriority ); - - #if ( configUSE_MUTEXES == 1 ) - { - uxCurrentPriority = pxTCB->uxBasePriority; - } - #else - { - uxCurrentPriority = pxTCB->uxPriority; - } - #endif - - if( uxCurrentPriority != uxNewPriority ) - { - /* The priority change may have readied a task of higher - priority than the calling task. */ - if( uxNewPriority > uxCurrentPriority ) - { - if( pxTask != NULL ) - { - /* The priority of another task is being raised. If we - were raising the priority of the currently running task - there would be no need to switch as it must have already - been the highest priority task. */ - xYieldRequired = pdTRUE; - } - } - else if( pxTask == NULL ) - { - /* Setting our own priority down means there may now be another - task of higher priority that is ready to execute. */ - xYieldRequired = pdTRUE; - } - - - - #if ( configUSE_MUTEXES == 1 ) - { - /* Only change the priority being used if the task is not - currently using an inherited priority. */ - if( pxTCB->uxBasePriority == pxTCB->uxPriority ) - { - pxTCB->uxPriority = uxNewPriority; - } - - /* The base priority gets set whatever. */ - pxTCB->uxBasePriority = uxNewPriority; - } - #else - { - pxTCB->uxPriority = uxNewPriority; - } - #endif - - listSET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ), ( configMAX_PRIORITIES - ( portTickType ) uxNewPriority ) ); - - /* If the task is in the blocked or suspended list we need do - nothing more than change it's priority variable. However, if - the task is in a ready list it needs to be removed and placed - in the queue appropriate to its new priority. */ - if( listIS_CONTAINED_WITHIN( &( pxReadyTasksLists[ uxCurrentPriority ] ), &( pxTCB->xGenericListItem ) ) ) - { - /* The task is currently in its ready list - remove before adding - it to it's new ready list. As we are in a critical section we - can do this even if the scheduler is suspended. */ - vListRemove( &( pxTCB->xGenericListItem ) ); - prvAddTaskToReadyQueue( pxTCB ); - } - - if( xYieldRequired == pdTRUE ) - { - portYIELD_WITHIN_API(); - } - } - } - portEXIT_CRITICAL(); - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_vTaskSuspend == 1 ) - - void vTaskSuspend( xTaskHandle pxTaskToSuspend ) - { - tskTCB *pxTCB; - - portENTER_CRITICAL(); - { - /* Ensure a yield is performed if the current task is being - suspended. */ - if( pxTaskToSuspend == pxCurrentTCB ) - { - pxTaskToSuspend = NULL; - } - - /* If null is passed in here then we are suspending ourselves. */ - pxTCB = prvGetTCBFromHandle( pxTaskToSuspend ); - - traceTASK_SUSPEND( pxTCB ); - - /* Remove task from the ready/delayed list and place in the suspended list. */ - vListRemove( &( pxTCB->xGenericListItem ) ); - - /* Is the task waiting on an event also? */ - if( pxTCB->xEventListItem.pvContainer ) - { - vListRemove( &( pxTCB->xEventListItem ) ); - } - - vListInsertEnd( ( xList * ) &xSuspendedTaskList, &( pxTCB->xGenericListItem ) ); - } - portEXIT_CRITICAL(); - - /* We may have just suspended the current task. */ - if( ( void * ) pxTaskToSuspend == NULL ) - { - portYIELD_WITHIN_API(); - } - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_vTaskSuspend == 1 ) - - signed portBASE_TYPE xTaskIsTaskSuspended( xTaskHandle xTask ) - { - portBASE_TYPE xReturn = pdFALSE; - const tskTCB * const pxTCB = ( tskTCB * ) xTask; - - /* Is the task we are attempting to resume actually in the - suspended list? */ - if( listIS_CONTAINED_WITHIN( &xSuspendedTaskList, &( pxTCB->xGenericListItem ) ) != pdFALSE ) - { - /* Has the task already been resumed from within an ISR? */ - if( listIS_CONTAINED_WITHIN( &xPendingReadyList, &( pxTCB->xEventListItem ) ) != pdTRUE ) - { - /* Is it in the suspended list because it is in the - Suspended state? It is possible to be in the suspended - list because it is blocked on a task with no timeout - specified. */ - if( listIS_CONTAINED_WITHIN( NULL, &( pxTCB->xEventListItem ) ) == pdTRUE ) - { - xReturn = pdTRUE; - } - } - } - - return xReturn; - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_vTaskSuspend == 1 ) - - void vTaskResume( xTaskHandle pxTaskToResume ) - { - tskTCB *pxTCB; - - /* Remove the task from whichever list it is currently in, and place - it in the ready list. */ - pxTCB = ( tskTCB * ) pxTaskToResume; - - /* The parameter cannot be NULL as it is impossible to resume the - currently executing task. */ - if( ( pxTCB != NULL ) && ( pxTCB != pxCurrentTCB ) ) - { - portENTER_CRITICAL(); - { - if( xTaskIsTaskSuspended( pxTCB ) == pdTRUE ) - { - traceTASK_RESUME( pxTCB ); - - /* As we are in a critical section we can access the ready - lists even if the scheduler is suspended. */ - vListRemove( &( pxTCB->xGenericListItem ) ); - prvAddTaskToReadyQueue( pxTCB ); - - /* We may have just resumed a higher priority task. */ - if( pxTCB->uxPriority >= pxCurrentTCB->uxPriority ) - { - /* This yield may not cause the task just resumed to run, but - will leave the lists in the correct state for the next yield. */ - portYIELD_WITHIN_API(); - } - } - } - portEXIT_CRITICAL(); - } - } - -#endif - -/*-----------------------------------------------------------*/ - -#if ( ( INCLUDE_xTaskResumeFromISR == 1 ) && ( INCLUDE_vTaskSuspend == 1 ) ) - - portBASE_TYPE xTaskResumeFromISR( xTaskHandle pxTaskToResume ) - { - portBASE_TYPE xYieldRequired = pdFALSE; - tskTCB *pxTCB; - - pxTCB = ( tskTCB * ) pxTaskToResume; - - if( xTaskIsTaskSuspended( pxTCB ) == pdTRUE ) - { - traceTASK_RESUME_FROM_ISR( pxTCB ); - - if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) - { - xYieldRequired = ( pxTCB->uxPriority >= pxCurrentTCB->uxPriority ); - vListRemove( &( pxTCB->xGenericListItem ) ); - prvAddTaskToReadyQueue( pxTCB ); - } - else - { - /* We cannot access the delayed or ready lists, so will hold this - task pending until the scheduler is resumed, at which point a - yield will be performed if necessary. */ - vListInsertEnd( ( xList * ) &( xPendingReadyList ), &( pxTCB->xEventListItem ) ); - } - } - - return xYieldRequired; - } - -#endif - - - - -/*----------------------------------------------------------- - * PUBLIC SCHEDULER CONTROL documented in task.h - *----------------------------------------------------------*/ - - -void vTaskStartScheduler( void ) -{ -portBASE_TYPE xReturn; - - /* Add the idle task at the lowest priority. */ - xReturn = xTaskCreate( prvIdleTask, ( signed char * ) "IDLE", tskIDLE_STACK_SIZE, ( void * ) NULL, ( tskIDLE_PRIORITY | portPRIVILEGE_BIT ), ( xTaskHandle * ) NULL ); - - if( xReturn == pdPASS ) - { - /* Interrupts are turned off here, to ensure a tick does not occur - before or during the call to xPortStartScheduler(). The stacks of - the created tasks contain a status word with interrupts switched on - so interrupts will automatically get re-enabled when the first task - starts to run. - - STEPPING THROUGH HERE USING A DEBUGGER CAN CAUSE BIG PROBLEMS IF THE - DEBUGGER ALLOWS INTERRUPTS TO BE PROCESSED. */ - portDISABLE_INTERRUPTS(); - - xSchedulerRunning = pdTRUE; - xTickCount = ( portTickType ) 0; - - /* If configGENERATE_RUN_TIME_STATS is defined then the following - macro must be defined to configure the timer/counter used to generate - the run time counter time base. */ - portCONFIGURE_TIMER_FOR_RUN_TIME_STATS(); - - /* Setting up the timer tick is hardware specific and thus in the - portable interface. */ - if( xPortStartScheduler() ) - { - /* Should not reach here as if the scheduler is running the - function will not return. */ - } - else - { - /* Should only reach here if a task calls xTaskEndScheduler(). */ - } - } -} -/*-----------------------------------------------------------*/ - -void vTaskEndScheduler( void ) -{ - /* Stop the scheduler interrupts and call the portable scheduler end - routine so the original ISRs can be restored if necessary. The port - layer must ensure interrupts enable bit is left in the correct state. */ - portDISABLE_INTERRUPTS(); - xSchedulerRunning = pdFALSE; - vPortEndScheduler(); -} -/*----------------------------------------------------------*/ - -void vTaskSuspendAll( void ) -{ - /* A critical section is not required as the variable is of type - portBASE_TYPE. */ - ++uxSchedulerSuspended; -} -/*----------------------------------------------------------*/ - -signed portBASE_TYPE xTaskResumeAll( void ) -{ -register tskTCB *pxTCB; -signed portBASE_TYPE xAlreadyYielded = pdFALSE; - - /* It is possible that an ISR caused a task to be removed from an event - list while the scheduler was suspended. If this was the case then the - removed task will have been added to the xPendingReadyList. Once the - scheduler has been resumed it is safe to move all the pending ready - tasks from this list into their appropriate ready list. */ - portENTER_CRITICAL(); - { - --uxSchedulerSuspended; - - if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) - { - if( uxCurrentNumberOfTasks > ( unsigned portBASE_TYPE ) 0 ) - { - portBASE_TYPE xYieldRequired = pdFALSE; - - /* Move any readied tasks from the pending list into the - appropriate ready list. */ - while( ( pxTCB = ( tskTCB * ) listGET_OWNER_OF_HEAD_ENTRY( ( ( xList * ) &xPendingReadyList ) ) ) != NULL ) - { - vListRemove( &( pxTCB->xEventListItem ) ); - vListRemove( &( pxTCB->xGenericListItem ) ); - prvAddTaskToReadyQueue( pxTCB ); - - /* If we have moved a task that has a priority higher than - the current task then we should yield. */ - if( pxTCB->uxPriority >= pxCurrentTCB->uxPriority ) - { - xYieldRequired = pdTRUE; - } - } - - /* If any ticks occurred while the scheduler was suspended then - they should be processed now. This ensures the tick count does not - slip, and that any delayed tasks are resumed at the correct time. */ - if( uxMissedTicks > ( unsigned portBASE_TYPE ) 0 ) - { - while( uxMissedTicks > ( unsigned portBASE_TYPE ) 0 ) - { - vTaskIncrementTick(); - --uxMissedTicks; - } - - /* As we have processed some ticks it is appropriate to yield - to ensure the highest priority task that is ready to run is - the task actually running. */ - #if configUSE_PREEMPTION == 1 - { - xYieldRequired = pdTRUE; - } - #endif - } - - if( ( xYieldRequired == pdTRUE ) || ( xMissedYield == pdTRUE ) ) - { - xAlreadyYielded = pdTRUE; - xMissedYield = pdFALSE; - portYIELD_WITHIN_API(); - } - } - } - } - portEXIT_CRITICAL(); - - return xAlreadyYielded; -} - - - - - - -/*----------------------------------------------------------- - * PUBLIC TASK UTILITIES documented in task.h - *----------------------------------------------------------*/ - - - -portTickType xTaskGetTickCount( void ) -{ -portTickType xTicks; - - /* Critical section required if running on a 16 bit processor. */ - portENTER_CRITICAL(); - { - xTicks = xTickCount; - } - portEXIT_CRITICAL(); - - return xTicks; -} -/*-----------------------------------------------------------*/ - -unsigned portBASE_TYPE uxTaskGetNumberOfTasks( void ) -{ - /* A critical section is not required because the variables are of type - portBASE_TYPE. */ - return uxCurrentNumberOfTasks; -} -/*-----------------------------------------------------------*/ - -#if ( configUSE_TRACE_FACILITY == 1 ) - - void vTaskList( signed char *pcWriteBuffer ) - { - unsigned portBASE_TYPE uxQueue; - - /* This is a VERY costly function that should be used for debug only. - It leaves interrupts disabled for a LONG time. */ - - vTaskSuspendAll(); - { - /* Run through all the lists that could potentially contain a TCB and - report the task name, state and stack high water mark. */ - - pcWriteBuffer[ 0 ] = ( signed char ) 0x00; - strcat( ( char * ) pcWriteBuffer, ( const char * ) "\r\n" ); - - uxQueue = uxTopUsedPriority + 1; - - do - { - uxQueue--; - - if( !listLIST_IS_EMPTY( &( pxReadyTasksLists[ uxQueue ] ) ) ) - { - prvListTaskWithinSingleList( pcWriteBuffer, ( xList * ) &( pxReadyTasksLists[ uxQueue ] ), tskREADY_CHAR ); - } - }while( uxQueue > ( unsigned short ) tskIDLE_PRIORITY ); - - if( !listLIST_IS_EMPTY( pxDelayedTaskList ) ) - { - prvListTaskWithinSingleList( pcWriteBuffer, ( xList * ) pxDelayedTaskList, tskBLOCKED_CHAR ); - } - - if( !listLIST_IS_EMPTY( pxOverflowDelayedTaskList ) ) - { - prvListTaskWithinSingleList( pcWriteBuffer, ( xList * ) pxOverflowDelayedTaskList, tskBLOCKED_CHAR ); - } - - #if( INCLUDE_vTaskDelete == 1 ) - { - if( !listLIST_IS_EMPTY( &xTasksWaitingTermination ) ) - { - prvListTaskWithinSingleList( pcWriteBuffer, ( xList * ) &xTasksWaitingTermination, tskDELETED_CHAR ); - } - } - #endif - - #if ( INCLUDE_vTaskSuspend == 1 ) - { - if( !listLIST_IS_EMPTY( &xSuspendedTaskList ) ) - { - prvListTaskWithinSingleList( pcWriteBuffer, ( xList * ) &xSuspendedTaskList, tskSUSPENDED_CHAR ); - } - } - #endif - } - xTaskResumeAll(); - } - -#endif -/*----------------------------------------------------------*/ - -#if ( configGENERATE_RUN_TIME_STATS == 1 ) - - void vTaskGetRunTimeStats( signed char *pcWriteBuffer ) - { - unsigned portBASE_TYPE uxQueue; - unsigned long ulTotalRunTime = portGET_RUN_TIME_COUNTER_VALUE(); - - /* This is a VERY costly function that should be used for debug only. - It leaves interrupts disabled for a LONG time. */ - - vTaskSuspendAll(); - { - /* Run through all the lists that could potentially contain a TCB, - generating a table of run timer percentages in the provided - buffer. */ - - pcWriteBuffer[ 0 ] = ( signed char ) 0x00; - strcat( ( char * ) pcWriteBuffer, ( const char * ) "\r\n" ); - - uxQueue = uxTopUsedPriority + 1; - - do - { - uxQueue--; - - if( !listLIST_IS_EMPTY( &( pxReadyTasksLists[ uxQueue ] ) ) ) - { - prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, ( xList * ) &( pxReadyTasksLists[ uxQueue ] ), ulTotalRunTime ); - } - }while( uxQueue > ( unsigned short ) tskIDLE_PRIORITY ); - - if( !listLIST_IS_EMPTY( pxDelayedTaskList ) ) - { - prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, ( xList * ) pxDelayedTaskList, ulTotalRunTime ); - } - - if( !listLIST_IS_EMPTY( pxOverflowDelayedTaskList ) ) - { - prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, ( xList * ) pxOverflowDelayedTaskList, ulTotalRunTime ); - } - - #if ( INCLUDE_vTaskDelete == 1 ) - { - if( !listLIST_IS_EMPTY( &xTasksWaitingTermination ) ) - { - prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, ( xList * ) &xTasksWaitingTermination, ulTotalRunTime ); - } - } - #endif - - #if ( INCLUDE_vTaskSuspend == 1 ) - { - if( !listLIST_IS_EMPTY( &xSuspendedTaskList ) ) - { - prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, ( xList * ) &xSuspendedTaskList, ulTotalRunTime ); - } - } - #endif - } - xTaskResumeAll(); - } - -#endif -/*----------------------------------------------------------*/ - -#if ( configUSE_TRACE_FACILITY == 1 ) - - void vTaskStartTrace( signed char * pcBuffer, unsigned long ulBufferSize ) - { - portENTER_CRITICAL(); - { - pcTraceBuffer = ( signed char * )pcBuffer; - pcTraceBufferStart = pcBuffer; - pcTraceBufferEnd = pcBuffer + ( ulBufferSize - tskSIZE_OF_EACH_TRACE_LINE ); - xTracing = pdTRUE; - } - portEXIT_CRITICAL(); - } - -#endif -/*----------------------------------------------------------*/ - -#if ( configUSE_TRACE_FACILITY == 1 ) - - unsigned long ulTaskEndTrace( void ) - { - unsigned long ulBufferLength; - - portENTER_CRITICAL(); - xTracing = pdFALSE; - portEXIT_CRITICAL(); - - ulBufferLength = ( unsigned long ) ( pcTraceBuffer - pcTraceBufferStart ); - - return ulBufferLength; - } - -#endif - - - -/*----------------------------------------------------------- - * SCHEDULER INTERNALS AVAILABLE FOR PORTING PURPOSES - * documented in task.h - *----------------------------------------------------------*/ - - -void vTaskIncrementTick( void ) -{ - /* Called by the portable layer each time a tick interrupt occurs. - Increments the tick then checks to see if the new tick value will cause any - tasks to be unblocked. */ - if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) - { - ++xTickCount; - if( xTickCount == ( portTickType ) 0 ) - { - xList *pxTemp; - - /* Tick count has overflowed so we need to swap the delay lists. - If there are any items in pxDelayedTaskList here then there is - an error! */ - pxTemp = pxDelayedTaskList; - pxDelayedTaskList = pxOverflowDelayedTaskList; - pxOverflowDelayedTaskList = pxTemp; - xNumOfOverflows++; - } - - /* See if this tick has made a timeout expire. */ - prvCheckDelayedTasks(); - } - else - { - ++uxMissedTicks; - - /* The tick hook gets called at regular intervals, even if the - scheduler is locked. */ - #if ( configUSE_TICK_HOOK == 1 ) - { - extern void vApplicationTickHook( void ); - - vApplicationTickHook(); - } - #endif - } - - #if ( configUSE_TICK_HOOK == 1 ) - { - extern void vApplicationTickHook( void ); - - /* Guard against the tick hook being called when the missed tick - count is being unwound (when the scheduler is being unlocked. */ - if( uxMissedTicks == 0 ) - { - vApplicationTickHook(); - } - } - #endif - - traceTASK_INCREMENT_TICK( xTickCount ); -} -/*-----------------------------------------------------------*/ - -#if ( ( INCLUDE_vTaskCleanUpResources == 1 ) && ( INCLUDE_vTaskSuspend == 1 ) ) - - void vTaskCleanUpResources( void ) - { - unsigned short usQueue; - volatile tskTCB *pxTCB; - - usQueue = ( unsigned short ) uxTopUsedPriority + ( unsigned short ) 1; - - /* Remove any TCB's from the ready queues. */ - do - { - usQueue--; - - while( !listLIST_IS_EMPTY( &( pxReadyTasksLists[ usQueue ] ) ) ) - { - listGET_OWNER_OF_NEXT_ENTRY( pxTCB, &( pxReadyTasksLists[ usQueue ] ) ); - vListRemove( ( xListItem * ) &( pxTCB->xGenericListItem ) ); - - prvDeleteTCB( ( tskTCB * ) pxTCB ); - } - }while( usQueue > ( unsigned short ) tskIDLE_PRIORITY ); - - /* Remove any TCB's from the delayed queue. */ - while( !listLIST_IS_EMPTY( &xDelayedTaskList1 ) ) - { - listGET_OWNER_OF_NEXT_ENTRY( pxTCB, &xDelayedTaskList1 ); - vListRemove( ( xListItem * ) &( pxTCB->xGenericListItem ) ); - - prvDeleteTCB( ( tskTCB * ) pxTCB ); - } - - /* Remove any TCB's from the overflow delayed queue. */ - while( !listLIST_IS_EMPTY( &xDelayedTaskList2 ) ) - { - listGET_OWNER_OF_NEXT_ENTRY( pxTCB, &xDelayedTaskList2 ); - vListRemove( ( xListItem * ) &( pxTCB->xGenericListItem ) ); - - prvDeleteTCB( ( tskTCB * ) pxTCB ); - } - - while( !listLIST_IS_EMPTY( &xSuspendedTaskList ) ) - { - listGET_OWNER_OF_NEXT_ENTRY( pxTCB, &xSuspendedTaskList ); - vListRemove( ( xListItem * ) &( pxTCB->xGenericListItem ) ); - - prvDeleteTCB( ( tskTCB * ) pxTCB ); - } - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( configUSE_APPLICATION_TASK_TAG == 1 ) - - void vTaskSetApplicationTaskTag( xTaskHandle xTask, pdTASK_HOOK_CODE pxTagValue ) - { - tskTCB *xTCB; - - /* If xTask is NULL then we are setting our own task hook. */ - if( xTask == NULL ) - { - xTCB = ( tskTCB * ) pxCurrentTCB; - } - else - { - xTCB = ( tskTCB * ) xTask; - } - - /* Save the hook function in the TCB. A critical section is required as - the value can be accessed from an interrupt. */ - portENTER_CRITICAL(); - xTCB->pxTaskTag = pxTagValue; - portEXIT_CRITICAL(); - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( configUSE_APPLICATION_TASK_TAG == 1 ) - - pdTASK_HOOK_CODE xTaskGetApplicationTaskTag( xTaskHandle xTask ) - { - tskTCB *xTCB; - pdTASK_HOOK_CODE xReturn; - - /* If xTask is NULL then we are setting our own task hook. */ - if( xTask == NULL ) - { - xTCB = ( tskTCB * ) pxCurrentTCB; - } - else - { - xTCB = ( tskTCB * ) xTask; - } - - /* Save the hook function in the TCB. A critical section is required as - the value can be accessed from an interrupt. */ - portENTER_CRITICAL(); - xReturn = xTCB->pxTaskTag; - portEXIT_CRITICAL(); - - return xReturn; - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( configUSE_APPLICATION_TASK_TAG == 1 ) - - portBASE_TYPE xTaskCallApplicationTaskHook( xTaskHandle xTask, void *pvParameter ) - { - tskTCB *xTCB; - portBASE_TYPE xReturn; - - /* If xTask is NULL then we are calling our own task hook. */ - if( xTask == NULL ) - { - xTCB = ( tskTCB * ) pxCurrentTCB; - } - else - { - xTCB = ( tskTCB * ) xTask; - } - - if( xTCB->pxTaskTag != NULL ) - { - xReturn = xTCB->pxTaskTag( pvParameter ); - } - else - { - xReturn = pdFAIL; - } - - return xReturn; - } - -#endif -/*-----------------------------------------------------------*/ - -void vTaskSwitchContext( void ) -{ - if( uxSchedulerSuspended != ( unsigned portBASE_TYPE ) pdFALSE ) - { - /* The scheduler is currently suspended - do not allow a context - switch. */ - xMissedYield = pdTRUE; - return; - } - - traceTASK_SWITCHED_OUT(); - - #if ( configGENERATE_RUN_TIME_STATS == 1 ) - { - unsigned long ulTempCounter = portGET_RUN_TIME_COUNTER_VALUE(); - - /* Add the amount of time the task has been running to the accumulated - time so far. The time the task started running was stored in - ulTaskSwitchedInTime. Note that there is no overflow protection here - so count values are only valid until the timer overflows. Generally - this will be about 1 hour assuming a 1uS timer increment. */ - pxCurrentTCB->ulRunTimeCounter += ( ulTempCounter - ulTaskSwitchedInTime ); - ulTaskSwitchedInTime = ulTempCounter; - } - #endif - - taskFIRST_CHECK_FOR_STACK_OVERFLOW(); - taskSECOND_CHECK_FOR_STACK_OVERFLOW(); - - /* Find the highest priority queue that contains ready tasks. */ - while( listLIST_IS_EMPTY( &( pxReadyTasksLists[ uxTopReadyPriority ] ) ) ) - { - --uxTopReadyPriority; - } - - /* listGET_OWNER_OF_NEXT_ENTRY walks through the list, so the tasks of the - same priority get an equal share of the processor time. */ - listGET_OWNER_OF_NEXT_ENTRY( pxCurrentTCB, &( pxReadyTasksLists[ uxTopReadyPriority ] ) ); - - traceTASK_SWITCHED_IN(); - vWriteTraceToBuffer(); -} -/*-----------------------------------------------------------*/ - -void vTaskPlaceOnEventList( const xList * const pxEventList, portTickType xTicksToWait ) -{ -portTickType xTimeToWake; - - /* THIS FUNCTION MUST BE CALLED WITH INTERRUPTS DISABLED OR THE - SCHEDULER SUSPENDED. */ - - /* Place the event list item of the TCB in the appropriate event list. - This is placed in the list in priority order so the highest priority task - is the first to be woken by the event. */ - vListInsert( ( xList * ) pxEventList, ( xListItem * ) &( pxCurrentTCB->xEventListItem ) ); - - /* We must remove ourselves from the ready list before adding ourselves - to the blocked list as the same list item is used for both lists. We have - exclusive access to the ready lists as the scheduler is locked. */ - vListRemove( ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - - - #if ( INCLUDE_vTaskSuspend == 1 ) - { - if( xTicksToWait == portMAX_DELAY ) - { - /* Add ourselves to the suspended task list instead of a delayed task - list to ensure we are not woken by a timing event. We will block - indefinitely. */ - vListInsertEnd( ( xList * ) &xSuspendedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - } - else - { - /* Calculate the time at which the task should be woken if the event does - not occur. This may overflow but this doesn't matter. */ - xTimeToWake = xTickCount + xTicksToWait; - - listSET_LIST_ITEM_VALUE( &( pxCurrentTCB->xGenericListItem ), xTimeToWake ); - - if( xTimeToWake < xTickCount ) - { - /* Wake time has overflowed. Place this item in the overflow list. */ - vListInsert( ( xList * ) pxOverflowDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - } - else - { - /* The wake time has not overflowed, so we can use the current block list. */ - vListInsert( ( xList * ) pxDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - } - } - } - #else - { - /* Calculate the time at which the task should be woken if the event does - not occur. This may overflow but this doesn't matter. */ - xTimeToWake = xTickCount + xTicksToWait; - - listSET_LIST_ITEM_VALUE( &( pxCurrentTCB->xGenericListItem ), xTimeToWake ); - - if( xTimeToWake < xTickCount ) - { - /* Wake time has overflowed. Place this item in the overflow list. */ - vListInsert( ( xList * ) pxOverflowDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - } - else - { - /* The wake time has not overflowed, so we can use the current block list. */ - vListInsert( ( xList * ) pxDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); - } - } - #endif -} -/*-----------------------------------------------------------*/ - -signed portBASE_TYPE xTaskRemoveFromEventList( const xList * const pxEventList ) -{ -tskTCB *pxUnblockedTCB; -portBASE_TYPE xReturn; - - /* THIS FUNCTION MUST BE CALLED WITH INTERRUPTS DISABLED OR THE - SCHEDULER SUSPENDED. It can also be called from within an ISR. */ - - /* The event list is sorted in priority order, so we can remove the - first in the list, remove the TCB from the delayed list, and add - it to the ready list. - - If an event is for a queue that is locked then this function will never - get called - the lock count on the queue will get modified instead. This - means we can always expect exclusive access to the event list here. */ - pxUnblockedTCB = ( tskTCB * ) listGET_OWNER_OF_HEAD_ENTRY( pxEventList ); - vListRemove( &( pxUnblockedTCB->xEventListItem ) ); - - if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) - { - vListRemove( &( pxUnblockedTCB->xGenericListItem ) ); - prvAddTaskToReadyQueue( pxUnblockedTCB ); - } - else - { - /* We cannot access the delayed or ready lists, so will hold this - task pending until the scheduler is resumed. */ - vListInsertEnd( ( xList * ) &( xPendingReadyList ), &( pxUnblockedTCB->xEventListItem ) ); - } - - if( pxUnblockedTCB->uxPriority >= pxCurrentTCB->uxPriority ) - { - /* Return true if the task removed from the event list has - a higher priority than the calling task. This allows - the calling task to know if it should force a context - switch now. */ - xReturn = pdTRUE; - } - else - { - xReturn = pdFALSE; - } - - return xReturn; -} -/*-----------------------------------------------------------*/ - -void vTaskSetTimeOutState( xTimeOutType * const pxTimeOut ) -{ - pxTimeOut->xOverflowCount = xNumOfOverflows; - pxTimeOut->xTimeOnEntering = xTickCount; -} -/*-----------------------------------------------------------*/ - -portBASE_TYPE xTaskCheckForTimeOut( xTimeOutType * const pxTimeOut, portTickType * const pxTicksToWait ) -{ -portBASE_TYPE xReturn; - - portENTER_CRITICAL(); - { - #if ( INCLUDE_vTaskSuspend == 1 ) - /* If INCLUDE_vTaskSuspend is set to 1 and the block time specified is - the maximum block time then the task should block indefinitely, and - therefore never time out. */ - if( *pxTicksToWait == portMAX_DELAY ) - { - xReturn = pdFALSE; - } - else /* We are not blocking indefinitely, perform the checks below. */ - #endif - - if( ( xNumOfOverflows != pxTimeOut->xOverflowCount ) && ( ( portTickType ) xTickCount >= ( portTickType ) pxTimeOut->xTimeOnEntering ) ) - { - /* The tick count is greater than the time at which vTaskSetTimeout() - was called, but has also overflowed since vTaskSetTimeOut() was called. - It must have wrapped all the way around and gone past us again. This - passed since vTaskSetTimeout() was called. */ - xReturn = pdTRUE; - } - else if( ( ( portTickType ) ( ( portTickType ) xTickCount - ( portTickType ) pxTimeOut->xTimeOnEntering ) ) < ( portTickType ) *pxTicksToWait ) - { - /* Not a genuine timeout. Adjust parameters for time remaining. */ - *pxTicksToWait -= ( ( portTickType ) xTickCount - ( portTickType ) pxTimeOut->xTimeOnEntering ); - vTaskSetTimeOutState( pxTimeOut ); - xReturn = pdFALSE; - } - else - { - xReturn = pdTRUE; - } - } - portEXIT_CRITICAL(); - - return xReturn; -} -/*-----------------------------------------------------------*/ - -void vTaskMissedYield( void ) -{ - xMissedYield = pdTRUE; -} - -/* - * ----------------------------------------------------------- - * The Idle task. - * ---------------------------------------------------------- - * - * The portTASK_FUNCTION() macro is used to allow port/compiler specific - * language extensions. The equivalent prototype for this function is: - * - * void prvIdleTask( void *pvParameters ); - * - */ -static portTASK_FUNCTION( prvIdleTask, pvParameters ) -{ - /* Stop warnings. */ - ( void ) pvParameters; - - for( ;; ) - { - /* See if any tasks have been deleted. */ - prvCheckTasksWaitingTermination(); - - #if ( configUSE_PREEMPTION == 0 ) - { - /* If we are not using preemption we keep forcing a task switch to - see if any other task has become available. If we are using - preemption we don't need to do this as any task becoming available - will automatically get the processor anyway. */ - taskYIELD(); - } - #endif - - #if ( ( configUSE_PREEMPTION == 1 ) && ( configIDLE_SHOULD_YIELD == 1 ) ) - { - /* When using preemption tasks of equal priority will be - timesliced. If a task that is sharing the idle priority is ready - to run then the idle task should yield before the end of the - timeslice. - - A critical region is not required here as we are just reading from - the list, and an occasional incorrect value will not matter. If - the ready list at the idle priority contains more than one task - then a task other than the idle task is ready to execute. */ - if( listCURRENT_LIST_LENGTH( &( pxReadyTasksLists[ tskIDLE_PRIORITY ] ) ) > ( unsigned portBASE_TYPE ) 1 ) - { - taskYIELD(); - } - } - #endif - - #if ( configUSE_IDLE_HOOK == 1 ) - { - extern void vApplicationIdleHook( void ); - - /* Call the user defined function from within the idle task. This - allows the application designer to add background functionality - without the overhead of a separate task. - NOTE: vApplicationIdleHook() MUST NOT, UNDER ANY CIRCUMSTANCES, - CALL A FUNCTION THAT MIGHT BLOCK. */ - vApplicationIdleHook(); - } - #endif - // call nanosleep for smalles sleep time possible - // (depending on kernel settings - around 100 microseconds) - // decreases idle thread CPU load from 100 to practically 0 -#ifdef IDLE_SLEEPS - sigset_t xSignals; - sigfillset( &xSignals ); - pthread_sigmask( SIG_SETMASK, &xSignals, NULL ); - struct timespec x; - x.tv_sec=0; - x.tv_nsec=10000; - nanosleep(&x,NULL); - sigemptyset( &xSignals ); - sigaddset( &xSignals, SIG_TICK ); - pthread_sigmask( SIG_SETMASK, &xSignals, NULL ); -#endif - } -} /*lint !e715 pvParameters is not accessed but all task functions require the same prototype. */ - - - - - - - -/*----------------------------------------------------------- - * File private functions documented at the top of the file. - *----------------------------------------------------------*/ - - - -static void prvInitialiseTCBVariables( tskTCB *pxTCB, const signed char * const pcName, unsigned portBASE_TYPE uxPriority, const xMemoryRegion * const xRegions, unsigned short usStackDepth ) -{ - /* Store the function name in the TCB. */ - #if configMAX_TASK_NAME_LEN > 1 - { - /* Don't bring strncpy into the build unnecessarily. */ - strncpy( ( char * ) pxTCB->pcTaskName, ( const char * ) pcName, ( unsigned short ) configMAX_TASK_NAME_LEN ); - } - #endif - pxTCB->pcTaskName[ ( unsigned short ) configMAX_TASK_NAME_LEN - ( unsigned short ) 1 ] = '\0'; - - /* This is used as an array index so must ensure it's not too large. First - remove the privilege bit if one is present. */ - if( uxPriority >= configMAX_PRIORITIES ) - { - uxPriority = configMAX_PRIORITIES - 1; - } - - pxTCB->uxPriority = uxPriority; - #if ( configUSE_MUTEXES == 1 ) - { - pxTCB->uxBasePriority = uxPriority; - } - #endif - - vListInitialiseItem( &( pxTCB->xGenericListItem ) ); - vListInitialiseItem( &( pxTCB->xEventListItem ) ); - - /* Set the pxTCB as a link back from the xListItem. This is so we can get - back to the containing TCB from a generic item in a list. */ - listSET_LIST_ITEM_OWNER( &( pxTCB->xGenericListItem ), pxTCB ); - - /* Event lists are always in priority order. */ - listSET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ), configMAX_PRIORITIES - ( portTickType ) uxPriority ); - listSET_LIST_ITEM_OWNER( &( pxTCB->xEventListItem ), pxTCB ); - - #if ( portCRITICAL_NESTING_IN_TCB == 1 ) - { - pxTCB->uxCriticalNesting = ( unsigned portBASE_TYPE ) 0; - } - #endif - - #if ( configUSE_APPLICATION_TASK_TAG == 1 ) - { - pxTCB->pxTaskTag = NULL; - } - #endif - - #if ( configGENERATE_RUN_TIME_STATS == 1 ) - { - pxTCB->ulRunTimeCounter = 0UL; - } - #endif - - #if ( portUSING_MPU_WRAPPERS == 1 ) - { - vPortStoreTaskMPUSettings( &( pxTCB->xMPUSettings ), xRegions, pxTCB->pxStack, usStackDepth ); - } - #else - { - ( void ) xRegions; - ( void ) usStackDepth; - } - #endif -} -/*-----------------------------------------------------------*/ - -#if ( portUSING_MPU_WRAPPERS == 1 ) - - void vTaskAllocateMPURegions( xTaskHandle xTaskToModify, const xMemoryRegion * const xRegions ) - { - tskTCB *pxTCB; - - if( xTaskToModify == pxCurrentTCB ) - { - xTaskToModify = NULL; - } - - /* If null is passed in here then we are deleting ourselves. */ - pxTCB = prvGetTCBFromHandle( xTaskToModify ); - - vPortStoreTaskMPUSettings( &( pxTCB->xMPUSettings ), xRegions, NULL, 0 ); - } - /*-----------------------------------------------------------*/ -#endif - -static void prvInitialiseTaskLists( void ) -{ -unsigned portBASE_TYPE uxPriority; - - for( uxPriority = 0; uxPriority < configMAX_PRIORITIES; uxPriority++ ) - { - vListInitialise( ( xList * ) &( pxReadyTasksLists[ uxPriority ] ) ); - } - - vListInitialise( ( xList * ) &xDelayedTaskList1 ); - vListInitialise( ( xList * ) &xDelayedTaskList2 ); - vListInitialise( ( xList * ) &xPendingReadyList ); - - #if ( INCLUDE_vTaskDelete == 1 ) - { - vListInitialise( ( xList * ) &xTasksWaitingTermination ); - } - #endif - - #if ( INCLUDE_vTaskSuspend == 1 ) - { - vListInitialise( ( xList * ) &xSuspendedTaskList ); - } - #endif - - /* Start with pxDelayedTaskList using list1 and the pxOverflowDelayedTaskList - using list2. */ - pxDelayedTaskList = &xDelayedTaskList1; - pxOverflowDelayedTaskList = &xDelayedTaskList2; -} -/*-----------------------------------------------------------*/ - -static void prvCheckTasksWaitingTermination( void ) -{ - #if ( INCLUDE_vTaskDelete == 1 ) - { - portBASE_TYPE xListIsEmpty; - - /* ucTasksDeleted is used to prevent vTaskSuspendAll() being called - too often in the idle task. */ - if( uxTasksDeleted > ( unsigned portBASE_TYPE ) 0 ) - { - vTaskSuspendAll(); - xListIsEmpty = listLIST_IS_EMPTY( &xTasksWaitingTermination ); - xTaskResumeAll(); - - if( !xListIsEmpty ) - { - tskTCB *pxTCB; - - portENTER_CRITICAL(); - { - pxTCB = ( tskTCB * ) listGET_OWNER_OF_HEAD_ENTRY( ( ( xList * ) &xTasksWaitingTermination ) ); - vListRemove( &( pxTCB->xGenericListItem ) ); - --uxCurrentNumberOfTasks; - --uxTasksDeleted; - } - portEXIT_CRITICAL(); - - prvDeleteTCB( pxTCB ); - } - } - } - #endif -} -/*-----------------------------------------------------------*/ - -static tskTCB *prvAllocateTCBAndStack( unsigned short usStackDepth, portSTACK_TYPE *puxStackBuffer ) -{ -tskTCB *pxNewTCB; - - /* Allocate space for the TCB. Where the memory comes from depends on - the implementation of the port malloc function. */ - pxNewTCB = ( tskTCB * ) pvPortMalloc( sizeof( tskTCB ) ); - - if( pxNewTCB != NULL ) - { - /* Allocate space for the stack used by the task being created. - The base of the stack memory stored in the TCB so the task can - be deleted later if required. */ - pxNewTCB->pxStack = ( portSTACK_TYPE * ) pvPortMallocAligned( ( ( ( size_t )usStackDepth ) * sizeof( portSTACK_TYPE ) ), puxStackBuffer ); - - if( pxNewTCB->pxStack == NULL ) - { - /* Could not allocate the stack. Delete the allocated TCB. */ - vPortFree( pxNewTCB ); - pxNewTCB = NULL; - } - else - { - /* Just to help debugging. */ - memset( pxNewTCB->pxStack, tskSTACK_FILL_BYTE, usStackDepth * sizeof( portSTACK_TYPE ) ); - } - } - - return pxNewTCB; -} -/*-----------------------------------------------------------*/ - -#if ( configUSE_TRACE_FACILITY == 1 ) - - static void prvListTaskWithinSingleList( const signed char *pcWriteBuffer, xList *pxList, signed char cStatus ) - { - volatile tskTCB *pxNextTCB, *pxFirstTCB; - unsigned short usStackRemaining; - - /* Write the details of all the TCB's in pxList into the buffer. */ - listGET_OWNER_OF_NEXT_ENTRY( pxFirstTCB, pxList ); - do - { - listGET_OWNER_OF_NEXT_ENTRY( pxNextTCB, pxList ); - #if ( portSTACK_GROWTH > 0 ) - { - usStackRemaining = usTaskCheckFreeStackSpace( ( unsigned char * ) pxNextTCB->pxEndOfStack ); - } - #else - { - usStackRemaining = usTaskCheckFreeStackSpace( ( unsigned char * ) pxNextTCB->pxStack ); - } - #endif - - sprintf( pcStatusString, ( char * ) "%s\t\t%c\t%u\t%u\t%u\r\n", pxNextTCB->pcTaskName, cStatus, ( unsigned int ) pxNextTCB->uxPriority, usStackRemaining, ( unsigned int ) pxNextTCB->uxTCBNumber ); - strcat( ( char * ) pcWriteBuffer, ( char * ) pcStatusString ); - - } while( pxNextTCB != pxFirstTCB ); - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( configGENERATE_RUN_TIME_STATS == 1 ) - - static void prvGenerateRunTimeStatsForTasksInList( const signed char *pcWriteBuffer, xList *pxList, unsigned long ulTotalRunTime ) - { - volatile tskTCB *pxNextTCB, *pxFirstTCB; - unsigned long ulStatsAsPercentage; - - /* Write the run time stats of all the TCB's in pxList into the buffer. */ - listGET_OWNER_OF_NEXT_ENTRY( pxFirstTCB, pxList ); - do - { - /* Get next TCB in from the list. */ - listGET_OWNER_OF_NEXT_ENTRY( pxNextTCB, pxList ); - - /* Divide by zero check. */ - if( ulTotalRunTime > 0UL ) - { - /* Has the task run at all? */ - if( pxNextTCB->ulRunTimeCounter == 0 ) - { - /* The task has used no CPU time at all. */ - sprintf( pcStatsString, ( char * ) "%s\t\t0\t\t0%%\r\n", pxNextTCB->pcTaskName ); - } - else - { - /* What percentage of the total run time as the task used? - This will always be rounded down to the nearest integer. */ - ulStatsAsPercentage = ( 100UL * pxNextTCB->ulRunTimeCounter ) / ulTotalRunTime; - - if( ulStatsAsPercentage > 0UL ) - { - sprintf( pcStatsString, ( char * ) "%s\t\t%u\t\t%u%%\r\n", pxNextTCB->pcTaskName, ( unsigned int ) pxNextTCB->ulRunTimeCounter, ( unsigned int ) ulStatsAsPercentage ); - } - else - { - /* If the percentage is zero here then the task has - consumed less than 1% of the total run time. */ - sprintf( pcStatsString, ( char * ) "%s\t\t%u\t\t<1%%\r\n", pxNextTCB->pcTaskName, ( unsigned int ) pxNextTCB->ulRunTimeCounter ); - } - } - - strcat( ( char * ) pcWriteBuffer, ( char * ) pcStatsString ); - } - - } while( pxNextTCB != pxFirstTCB ); - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( ( configUSE_TRACE_FACILITY == 1 ) || ( INCLUDE_uxTaskGetStackHighWaterMark == 1 ) ) - - static unsigned short usTaskCheckFreeStackSpace( const unsigned char * pucStackByte ) - { - register unsigned short usCount = 0; - - while( *pucStackByte == tskSTACK_FILL_BYTE ) - { - pucStackByte -= portSTACK_GROWTH; - usCount++; - } - - usCount /= sizeof( portSTACK_TYPE ); - - return usCount; - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_uxTaskGetStackHighWaterMark == 1 ) - - unsigned portBASE_TYPE uxTaskGetStackHighWaterMark( xTaskHandle xTask ) - { - tskTCB *pxTCB; - unsigned char *pcEndOfStack; - unsigned portBASE_TYPE uxReturn; - - pxTCB = prvGetTCBFromHandle( xTask ); - - #if portSTACK_GROWTH < 0 - { - pcEndOfStack = ( unsigned char * ) pxTCB->pxStack; - } - #else - { - pcEndOfStack = ( unsigned char * ) pxTCB->pxEndOfStack; - } - #endif - - uxReturn = ( unsigned portBASE_TYPE ) usTaskCheckFreeStackSpace( pcEndOfStack ); - - return uxReturn; - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( ( INCLUDE_vTaskDelete == 1 ) || ( INCLUDE_vTaskCleanUpResources == 1 ) ) - - static void prvDeleteTCB( tskTCB *pxTCB ) - { - /* Free up the memory allocated by the scheduler for the task. It is up to - the task to free any memory allocated at the application level. */ - vPortFreeAligned( pxTCB->pxStack ); - vPortFree( pxTCB ); - } - -#endif - - -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_xTaskGetCurrentTaskHandle == 1 ) - - xTaskHandle xTaskGetCurrentTaskHandle( void ) - { - xTaskHandle xReturn; - - /* A critical section is not required as this is not called from - an interrupt and the current TCB will always be the same for any - individual execution thread. */ - xReturn = pxCurrentTCB; - - return xReturn; - } - -#endif - -/*-----------------------------------------------------------*/ - -#if ( INCLUDE_xTaskGetSchedulerState == 1 ) - - portBASE_TYPE xTaskGetSchedulerState( void ) - { - portBASE_TYPE xReturn; - - if( xSchedulerRunning == pdFALSE ) - { - xReturn = taskSCHEDULER_NOT_STARTED; - } - else - { - if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) - { - xReturn = taskSCHEDULER_RUNNING; - } - else - { - xReturn = taskSCHEDULER_SUSPENDED; - } - } - - return xReturn; - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( configUSE_MUTEXES == 1 ) - - void vTaskPriorityInherit( xTaskHandle * const pxMutexHolder ) - { - tskTCB * const pxTCB = ( tskTCB * ) pxMutexHolder; - - if( pxTCB->uxPriority < pxCurrentTCB->uxPriority ) - { - /* Adjust the mutex holder state to account for its new priority. */ - listSET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ), configMAX_PRIORITIES - ( portTickType ) pxCurrentTCB->uxPriority ); - - /* If the task being modified is in the ready state it will need to - be moved in to a new list. */ - if( listIS_CONTAINED_WITHIN( &( pxReadyTasksLists[ pxTCB->uxPriority ] ), &( pxTCB->xGenericListItem ) ) ) - { - vListRemove( &( pxTCB->xGenericListItem ) ); - - /* Inherit the priority before being moved into the new list. */ - pxTCB->uxPriority = pxCurrentTCB->uxPriority; - prvAddTaskToReadyQueue( pxTCB ); - } - else - { - /* Just inherit the priority. */ - pxTCB->uxPriority = pxCurrentTCB->uxPriority; - } - } - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( configUSE_MUTEXES == 1 ) - - void vTaskPriorityDisinherit( xTaskHandle * const pxMutexHolder ) - { - tskTCB * const pxTCB = ( tskTCB * ) pxMutexHolder; - - if( pxMutexHolder != NULL ) - { - if( pxTCB->uxPriority != pxTCB->uxBasePriority ) - { - /* We must be the running task to be able to give the mutex back. - Remove ourselves from the ready list we currently appear in. */ - vListRemove( &( pxTCB->xGenericListItem ) ); - - /* Disinherit the priority before adding ourselves into the new - ready list. */ - pxTCB->uxPriority = pxTCB->uxBasePriority; - listSET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ), configMAX_PRIORITIES - ( portTickType ) pxTCB->uxPriority ); - prvAddTaskToReadyQueue( pxTCB ); - } - } - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( portCRITICAL_NESTING_IN_TCB == 1 ) - - void vTaskEnterCritical( void ) - { - portDISABLE_INTERRUPTS(); - - if( xSchedulerRunning != pdFALSE ) - { - pxCurrentTCB->uxCriticalNesting++; - } - } - -#endif -/*-----------------------------------------------------------*/ - -#if ( portCRITICAL_NESTING_IN_TCB == 1 ) - -void vTaskExitCritical( void ) -{ - if( xSchedulerRunning != pdFALSE ) - { - if( pxCurrentTCB->uxCriticalNesting > 0 ) - { - pxCurrentTCB->uxCriticalNesting--; - - if( pxCurrentTCB->uxCriticalNesting == 0 ) - { - portENABLE_INTERRUPTS(); - } - } - } -} - -#endif -/*-----------------------------------------------------------*/ - - - - diff --git a/flight/PiOS.posix/posix/Libraries/FreeRTOS/library.mk b/flight/PiOS.posix/posix/Libraries/FreeRTOS/library.mk new file mode 100644 index 000000000..ae65aff82 --- /dev/null +++ b/flight/PiOS.posix/posix/Libraries/FreeRTOS/library.mk @@ -0,0 +1,11 @@ +# +# Rules to add FreeRTOS to a PiOS target +# +# Note that the PIOS target-specific makefile will detect that FREERTOS_DIR +# has been defined and add in the target-specific pieces separately. +# + +FREERTOS_DIR := $(dir $(lastword $(MAKEFILE_LIST)))/Source +SRC += $(wildcard $(FREERTOS_DIR)/*.c) +EXTRAINCDIRS += $(FREERTOS_DIR)/include + diff --git a/flight/PiOS.posix/posix/library.mk b/flight/PiOS.posix/posix/library.mk new file mode 100644 index 000000000..530431210 --- /dev/null +++ b/flight/PiOS.posix/posix/library.mk @@ -0,0 +1,76 @@ +# +# Rules to (help) build the F4xx device support. +# + +# +# Directory containing this makefile +# +PIOS_DEVLIB := $(dir $(lastword $(MAKEFILE_LIST))) + +# +# Hardcoded linker script names for now +# +#LINKER_SCRIPTS_APP = $(PIOS_DEVLIB)/link_STM32F4xx_OP_memory.ld \ + $(PIOS_DEVLIB)/link_STM32F4xx_sections.ld + +#LINKER_SCRIPTS_BL = $(PIOS_DEVLIB)/link_STM32F4xx_BL_memory.ld \ + $(PIOS_DEVLIB)/link_STM32F4xx_sections.ld + +# +# Compiler options implied by the F4xx +# +#CDEFS += -DSTM32F4XX +#CDEFS += -DHSE_VALUE=$(OSCILLATOR_FREQ) +#CDEFS += -DUSE_STDPERIPH_DRIVER +ARCHFLAGS += -DARCH_POSIX + +# +# PIOS device library source and includes +# +SRC += $(wildcard $(PIOS_DEVLIB)*.c) +EXTRAINCDIRS += $(PIOS_DEVLIB)/inc + +# +# CMSIS for the F4 +# +#include $(PIOSCOMMONLIB)/CMSIS2/library.mk +#CMSIS2_DEVICEDIR := $(PIOS_DEVLIB)/Libraries/CMSIS2/Device/ST/STM32F4xx +#SRC += $(wildcard $(CMSIS2_DEVICEDIR)/Source/*.c) +#EXTRAINCDIRS += $(CMSIS2_DEVICEDIR)/Include + +# +# ST Peripheral library +# +#PERIPHLIB = $(PIOS_DEVLIB)/Libraries/STM32F4xx_StdPeriph_Driver +#SRC += $(wildcard $(PERIPHLIB)/src/*.c) +#EXTRAINCDIRS += $(PERIPHLIB)/inc + +# +# ST USB OTG library +# +#USBOTGLIB = $(PIOS_DEVLIB)/Libraries/STM32_USB_OTG_Driver +#USBOTGLIB_SRC = usb_core.c usb_dcd.c usb_dcd_int.c +#SRC += $(addprefix $(USBOTGLIB)/src/,$(USBOTGLIB_SRC)) +#EXTRAINCDIRS += $(USBOTGLIB)/inc + +# +# ST USB Device library +# +#USBDEVLIB = $(PIOS_DEVLIB)/Libraries/STM32_USB_Device_Library +#SRC += $(wildcard $(USBDEVLIB)/Core/src/*.c) +#EXTRAINCDIRS += $(USBDEVLIB)/Core/inc + +# +# FreeRTOS +# +# If the application has included the generic FreeRTOS support, then add in +# the device-specific pieces of the code. +# +ifneq ($(FREERTOS_DIR),) +FREERTOS_PORTDIR := $(PIOS_DEVLIB)/Libraries/FreeRTOS/Source +SRC += $(wildcard $(FREERTOS_PORTDIR)/portable/GCC/Posix/*.c) +SRC += $(wildcard $(FREERTOS_PORTDIR)/portable/MemMang/*.c) + +EXTRAINCDIRS += $(FREERTOS_PORTDIR)/portable/GCC/Posix +endif + diff --git a/flight/PiOS.posix/posix/pios_debug.c b/flight/PiOS.posix/posix/pios_debug.c index 7f3dc99da..02d41ee64 100644 --- a/flight/PiOS.posix/posix/pios_debug.c +++ b/flight/PiOS.posix/posix/pios_debug.c @@ -72,11 +72,10 @@ void PIOS_DEBUG_PinValue4BitL(uint8_t value) /** * Report a serious error and halt */ -void PIOS_DEBUG_Panic(const char *msg) __attribute__ ((noreturn)) +void PIOS_DEBUG_Panic(const char *msg) { -#ifdef PIOS_COM_DEBUG - register int *lr asm("lr"); // Link-register holds the PC of the caller - PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, "\r%s @0x%x\r", msg, lr); +#ifdef PIOS_COM_AUX + PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, "\r%s\r", msg); #endif // tell the user whats going on on commandline too diff --git a/flight/PiOS.posix/posix/pios_delay.c b/flight/PiOS.posix/posix/pios_delay.c index cec10467c..386e2bb2f 100644 --- a/flight/PiOS.posix/posix/pios_delay.c +++ b/flight/PiOS.posix/posix/pios_delay.c @@ -59,7 +59,7 @@ int32_t PIOS_DELAY_Init(void) * \param[in] uS delay (1..65535 microseconds) * \return < 0 on errors */ -int32_t PIOS_DELAY_WaituS(uint16_t uS) +int32_t PIOS_DELAY_WaituS(uint32_t uS) { static struct timespec wait,rest; wait.tv_sec=0; @@ -83,7 +83,7 @@ int32_t PIOS_DELAY_WaituS(uint16_t uS) * \param[in] mS delay (1..65535 milliseconds) * \return < 0 on errors */ -int32_t PIOS_DELAY_WaitmS(uint16_t mS) +int32_t PIOS_DELAY_WaitmS(uint32_t mS) { //for(int i = 0; i < mS; i++) { // PIOS_DELAY_WaituS(1000); @@ -99,4 +99,44 @@ int32_t PIOS_DELAY_WaitmS(uint16_t mS) return 0; } +/** + * @brief Query the Delay timer for the current uS + * @return A microsecond value + */ +uint32_t PIOS_DELAY_GetuS() +{ + static struct timespec current; + clock_gettime(CLOCK_REALTIME, ¤t); + return ((current.tv_sec * 1000000) + (current.tv_nsec / 1000)); +} + +/** + * @brief Calculate time in microseconds since a previous time + * @param[in] t previous time + * @return time in us since previous time t. + */ +uint32_t PIOS_DELAY_GetuSSince(uint32_t t) +{ + return (PIOS_DELAY_GetuS() - t); +} + +/** + * @brief Get the raw delay timer, useful for timing + * @return Unitless value (uint32 wrap around) + */ +uint32_t PIOS_DELAY_GetRaw() +{ + return (PIOS_DELAY_GetuS()); +} + +/** + * @brief Compare to raw times to and convert to us + * @return A microsecond value + */ +uint32_t PIOS_DELAY_DiffuS(uint32_t raw) +{ + return ( PIOS_DELAY_GetuS() - raw ); +} + + #endif diff --git a/flight/PiOS.posix/posix/pios_led.c b/flight/PiOS.posix/posix/pios_led.c index b35477d45..8e0776883 100644 --- a/flight/PiOS.posix/posix/pios_led.c +++ b/flight/PiOS.posix/posix/pios_led.c @@ -41,7 +41,7 @@ static uint8_t LED_GPIO[PIOS_LED_NUM]; -static inline void PIOS_SetLED(LedTypeDef LED,uint8_t stat) { +static inline void PIOS_SetLED(uint32_t LED,uint8_t stat) { printf("PIOS: LED %i status %i\n",LED,stat); LED_GPIO[LED]=stat; } @@ -71,7 +71,7 @@ void PIOS_LED_Init(void) * Turn on LED * \param[in] LED LED Name (LED1, LED2) */ -void PIOS_LED_On(LedTypeDef LED) +void PIOS_LED_On(uint32_t LED) { //LED_GPIO_PORT[LED]->BRR = LED_GPIO_PIN[LED]; PIOS_SetLED(LED,1); @@ -82,7 +82,7 @@ void PIOS_LED_On(LedTypeDef LED) * Turn off LED * \param[in] LED LED Name (LED1, LED2) */ -void PIOS_LED_Off(LedTypeDef LED) +void PIOS_LED_Off(uint32_t LED) { //LED_GPIO_PORT[LED]->BSRR = LED_GPIO_PIN[LED]; PIOS_SetLED(LED,0); @@ -93,7 +93,7 @@ void PIOS_LED_Off(LedTypeDef LED) * Toggle LED on/off * \param[in] LED LED Name (LED1, LED2) */ -void PIOS_LED_Toggle(LedTypeDef LED) +void PIOS_LED_Toggle(uint32_t LED) { //LED_GPIO_PORT[LED]->ODR ^= LED_GPIO_PIN[LED]; PIOS_SetLED(LED,LED_GPIO[LED]?0:1); diff --git a/flight/PiOS.posix/posix/pios_udp.c b/flight/PiOS.posix/posix/pios_udp.c index 5ccbae767..2c98c42ee 100644 --- a/flight/PiOS.posix/posix/pios_udp.c +++ b/flight/PiOS.posix/posix/pios_udp.c @@ -78,11 +78,6 @@ static pios_udp_dev * find_udp_dev_by_id (uint8_t udp) void * PIOS_UDP_RxThread(void * udp_dev_n) { - /* needed because of FreeRTOS.posix scheduling */ - sigset_t set; - sigfillset(&set); - sigprocmask(SIG_BLOCK, &set, NULL); - pios_udp_dev * udp_dev = (pios_udp_dev*) udp_dev_n; /** @@ -151,7 +146,13 @@ int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg) int res= bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server,sizeof(udp_dev->server)); /* Create transmit thread for this connection */ +#if defined(PIOS_INCLUDE_FREERTOS) +//( pdTASK_CODE pvTaskCode, const portCHAR * const pcName, unsigned portSHORT usStackDepth, void *pvParameters, unsigned portBASE_TYPE uxPriority, xTaskHandle *pvCreatedTask ); + xTaskCreate((pdTASK_CODE)PIOS_UDP_RxThread, (const signed char *)"UDP_Rx_Thread",1024,(void*)udp_dev,(tskIDLE_PRIORITY + 1),&udp_dev->rxThread); +#else pthread_create(&udp_dev->rxThread, NULL, PIOS_UDP_RxThread, (void*)udp_dev); +#endif + printf("udp dev %i - socket %i opened - result %i\n",pios_udp_num_devices-1,udp_dev->socket,res); diff --git a/flight/SimPosix/Makefile b/flight/SimPosix/Makefile new file mode 100644 index 000000000..9c3a8cdcf --- /dev/null +++ b/flight/SimPosix/Makefile @@ -0,0 +1,511 @@ +##### +# Project: OpenPilot INS +# +# +# Makefile for OpenPilot INS project +# +# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009. +# +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY +# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License +# for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program; if not, write to the Free Software Foundation, Inc., +# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +##### + +WHEREAMI := $(dir $(lastword $(MAKEFILE_LIST))) +TOP := $(realpath $(WHEREAMI)/../../) + +override TCHAIN_PREFIX := +override THUMB := +include $(TOP)/make/firmware-defs.mk +include $(TOP)/make/boards/$(BOARD_NAME)/board-info.mk + +# Target file name (without extension). +TARGET := fw_$(BOARD_NAME) + +# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.) +OUTDIR := $(TOP)/build/$(TARGET) + +# Set developer code and compile options +# Set to YES for debugging +DEBUG ?= NO + +# Set to YES when using Code Sourcery toolchain +CODE_SOURCERY ?= NO + +ifeq ($(CODE_SOURCERY), YES) +REMOVE_CMD = cs-rm +else +REMOVE_CMD = rm +endif + +FLASH_TOOL = OPENOCD + +# List of modules to include +MODULES = ManualControl Stabilization GPS +MODULES += CameraStab +MODULES += Telemetry +#MODULES += OveroSync +PYMODULES = +#FlightPlan + +# Paths +OPSYSTEM = ./System +OPSYSTEMINC = $(OPSYSTEM)/inc +OPUAVTALK = ../UAVTalk +OPUAVTALKINC = $(OPUAVTALK)/inc +OPUAVOBJ = ../UAVObjects +OPUAVOBJINC = $(OPUAVOBJ)/inc +PIOS = ../PiOS.posix +PIOSINC = $(PIOS)/inc +OPMODULEDIR = ../Modules +FLIGHTLIB = ../Libraries +FLIGHTLIBINC = ../Libraries/inc +PIOSPOSIX = $(PIOS)/posix +PIOSCOMMON = $(PIOS)/posix +PIOSBOARDS = $(PIOS)/Boards +PIOSCOMMONLIB = $(PIOSCOMMON)/Libraries +#APPLIBDIR = $(PIOSSTM32F4XX)/Libraries +OPUAVOBJ = ../UAVObjects +OPUAVOBJINC = $(OPUAVOBJ)/inc +BOOT = +BOOTINC = +PYMITE = $(FLIGHTLIB)/PyMite +PYMITELIB = $(PYMITE)/lib +PYMITEPLAT = $(PYMITE)/platform/openpilot +PYMITETOOLS = $(PYMITE)/tools +PYMITEVM = $(PYMITE)/vm +PYMITEINC = $(PYMITEVM) +PYMITEINC += $(PYMITEPLAT) +PYMITEINC += $(OUTDIR) +FLIGHTPLANLIB = $(OPMODULEDIR)/FlightPlan/lib +FLIGHTPLANS = $(OPMODULEDIR)/FlightPlan/flightplans +HWDEFSINC = ../board_hw_defs/$(BOARD_NAME) +UAVOBJSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight + +SRC = +# optional component libraries +include $(PIOSCOMMONLIB)/FreeRTOS/library.mk +#include $(PIOSCOMMONLIB)/dosfs/library.mk +#include $(PIOSCOMMONLIB)/msheap/library.mk + + +# List C source files here. (C dependencies are automatically generated.) +# use file-extension c for "c-only"-files + +## PyMite files and modules +SRC += $(OUTDIR)/pmlib_img.c +SRC += $(OUTDIR)/pmlib_nat.c +SRC += $(OUTDIR)/pmlibusr_img.c +SRC += $(OUTDIR)/pmlibusr_nat.c +PYSRC += $(wildcard ${PYMITEVM}/*.c) +PYSRC += $(wildcard ${PYMITEPLAT}/*.c) +PYSRC += ${foreach MOD, ${PYMODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}} +SRC += $(PYSRC) + +## MODULES +SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}} +SRC += ${OUTDIR}/InitMods.c +## OPENPILOT CORE: +SRC += ${OPMODULEDIR}/System/systemmod.c +SRC += $(OPSYSTEM)/simposix.c +SRC += $(OPSYSTEM)/pios_board.c +SRC += $(OPSYSTEM)/alarms.c +SRC += $(OPUAVTALK)/uavtalk.c +SRC += $(OPUAVOBJ)/uavobjectmanager.c +SRC += $(OPUAVOBJ)/eventdispatcher.c +SRC += $(UAVOBJSYNTHDIR)/uavobjectsinit.c + +SRC += $(FLIGHTLIB)/CoordinateConversions.c +SRC += $(FLIGHTLIB)/fifo_buffer.c +SRC += $(FLIGHTLIB)/WorldMagModel.c +SRC += $(FLIGHTLIB)/insgps13state.c +SRC += $(FLIGHTLIB)/taskmonitor.c + +## PIOS Hardware (STM32F4xx) +include $(PIOS)/posix/library.mk + +## PIOS Hardware (Common) +#SRC += $(PIOSCOMMON)/pios_mpu6000.c +#SRC += $(PIOSCOMMON)/pios_bma180.c +#SRC += $(PIOSCOMMON)/pios_l3gd20.c +#SRC += $(PIOSCOMMON)/pios_hmc5883.c +#SRC += $(PIOSCOMMON)/pios_ms5611.c +#SRC += $(PIOSCOMMON)/pios_crc.c +#SRC += $(PIOSCOMMON)/pios_com.c +#SRC += $(PIOSCOMMON)/pios_rcvr.c +#SRC += $(PIOSCOMMON)/pios_flash_jedec.c +#SRC += $(PIOSCOMMON)/pios_flashfs_objlist.c +#SRC += $(PIOSCOMMON)/printf-stdarg.c +#SRC += $(PIOSCOMMON)/pios_usb_desc_hid_cdc.c +#SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c + +include ./UAVObjects.inc +SRC += $(UAVOBJSRC) + +# List C source files here which must be compiled in ARM-Mode (no -mthumb). +# use file-extension c for "c-only"-files +## just for testing, timer.c could be compiled in thumb-mode too +SRCARM = + +# List C++ source files here. +# use file-extension .cpp for C++-files (not .C) +CPPSRC = + +# List C++ source files here which must be compiled in ARM-Mode. +# use file-extension .cpp for C++-files (not .C) +#CPPSRCARM = $(TARGET).cpp +CPPSRCARM = + +# List Assembler source files here. +# Make them always end in a capital .S. Files ending in a lowercase .s +# will not be considered source files but generated files (assembler +# output from the compiler), and will be deleted upon "make clean"! +# Even though the DOS/Win* filesystem matches both .s and .S the same, +# it will preserve the spelling of the filenames, and gcc itself does +# care about how the name is spelled on its command-line. + + +# List Assembler source files here which must be assembled in ARM-Mode.. +ASRCARM = + +# List any extra directories to look for include files here. +# Each directory must be seperated by a space. +EXTRAINCDIRS += $(PIOS) +EXTRAINCDIRS += $(PIOSINC) +EXTRAINCDIRS += $(OPSYSTEMINC) +EXTRAINCDIRS += $(OPUAVTALK) +EXTRAINCDIRS += $(OPUAVTALKINC) +EXTRAINCDIRS += $(OPUAVOBJ) +EXTRAINCDIRS += $(OPUAVOBJINC) +EXTRAINCDIRS += $(UAVOBJSYNTHDIR) +EXTRAINCDIRS += $(FLIGHTLIBINC) +#EXTRAINCDIRS += $(PIOSSTM32F4XX) +EXTRAINCDIRS += $(PIOSCOMMON) +EXTRAINCDIRS += $(PIOSBOARDS) +#EXTRAINCDIRS += $(STMSPDINCDIR) +EXTRAINCDIRS += $(CMSISDIR) +EXTRAINCDIRS += $(OPUAVSYNTHDIR) +EXTRAINCDIRS += $(BOOTINC) +EXTRAINCDIRS += $(PYMITEINC) +EXTRAINCDIRS += $(HWDEFSINC) + +# Generate intermediate code +gencode: ${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h + +$(PYSRC): gencode + +PYTHON = python + +# Generate code for PyMite +${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h: $(wildcard ${PYMITELIB}/*.py) $(wildcard ${PYMITEPLAT}/*.py) $(wildcard ${FLIGHTPLANLIB}/*.py) $(wildcard ${FLIGHTPLANS}/*.py) + @echo $(MSG_PYMITEINIT) $(call toprel, $@) + @$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py) + @$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h + @$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -u -o $(OUTDIR)/pmlibusr_img.c --native-file=$(OUTDIR)/pmlibusr_nat.c $(FLIGHTPLANS)/test.py +EXTRAINCDIRS += ${foreach MOD, ${MODULES} ${PYMODULES}, $(OPMODULEDIR)/${MOD}/inc} ${OPMODULEDIR}/System/inc + +# List any extra directories to look for library files here. +# Also add directories where the linker should search for +# includes from linker-script to the list +# Each directory must be seperated by a space. +EXTRA_LIBDIRS = + +# Extra Libraries +# Each library-name must be seperated by a space. +# i.e. to link with libxyz.a, libabc.a and libefsl.a: +# EXTRA_LIBS = xyz abc efsl +# for newlib-lpc (file: libnewlibc-lpc.a): +# EXTRA_LIBS = newlib-lpc +EXTRA_LIBS = + +# Path to Linker-Scripts +#LINKERSCRIPTPATH = $(PIOSSTM32F4XX) + +# Optimization level, can be [0, 1, 2, 3, s]. +# 0 = turn off optimization. s = optimize for size. +# (Note: 3 is not always the best optimization level. See avr-libc FAQ.) + +ifeq ($(DEBUG),YES) +CFLAGS += -O0 +CFLAGS += -DGENERAL_COV +CFLAGS += -finstrument-functions -ffixed-r10 +else +CFLAGS += -Os +endif + + + +# common architecture-specific flags from the device-specific library makefile +CFLAGS += $(ARCHFLAGS) + +CFLAGS += $(UAVOBJDEFINE) +CFLAGS += -DDIAGNOSTICS +CFLAGS += -DDIAG_TASKS + +# This is not the best place for these. Really should abstract out +# to the board file or something +#CFLAGS += -DSTM32F4XX +CFLAGS += -DMEM_SIZE=1024000000 + +# Output format. (can be ihex or binary or both) +# binary to create a load-image in raw-binary format i.e. for SAM-BA, +# ihex to create a load-image in Intel hex format +#LOADFORMAT = ihex +#LOADFORMAT = binary +LOADFORMAT = both + +# Debugging format. +DEBUGF = dwarf-2 + +# Place project-specific -D (define) and/or +# -U options for C here. +CDEFS += -DHSE_VALUE=$(OSCILLATOR_FREQ) +CDEFS += -DSYSCLK_FREQ=$(SYSCLK_FREQ) +CDEFS += -DUSE_STDPERIPH_DRIVER +CDEFS += -DUSE_$(BOARD) + +# Place project-specific -D and/or -U options for +# Assembler with preprocessor here. +#ADEFS = -DUSE_IRQ_ASM_WRAPPER +ADEFS = -D__ASSEMBLY__ + +# Compiler flag to set the C Standard level. +# c89 - "ANSI" C +# gnu89 - c89 plus GCC extensions +# c99 - ISO C99 standard (not yet fully implemented) +# gnu99 - c99 plus GCC extensions +CSTANDARD = -std=gnu99 + +#----- + +# Compiler flags. + +# -g*: generate debugging information +# -O*: optimization level +# -f...: tuning, see GCC manual and avr-libc documentation +# -Wall...: warning level +# -Wa,...: tell GCC to pass this to the assembler. +# -adhlns...: create assembler listing +# +# Flags for C and C++ (arm-elf-gcc/arm-elf-g++) + +CFLAGS += -g$(DEBUGF) + +CFLAGS += -ffast-math + +#CFLAGS += -mcpu=$(MCU) +CFLAGS += $(CDEFS) +CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I. + +#CFLAGS += -mapcs-frame +CFLAGS += -fomit-frame-pointer +ifeq ($(CODE_SOURCERY), YES) +CFLAGS += -fpromote-loop-indices +endif + +CFLAGS += -Wall +#CFLAGS += -Werror +CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<)))) +# Compiler flags to generate dependency files: +CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d + +# flags only for C +#CONLYFLAGS += -Wnested-externs +CONLYFLAGS += $(CSTANDARD) + +# Assembler flags. +# -Wa,...: tell GCC to pass this to the assembler. +# -ahlns: create listing +ASFLAGS = $(ARCHFLAGS) -I. -x assembler-with-cpp +ASFLAGS += $(ADEFS) +ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<)))) +ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) + +MATH_LIB = -lm + +# Linker flags. +# -Wl,...: tell GCC to pass this to linker. +# -Map: create map file +# --cref: add cross reference to map file +#LDFLAGS = -nostartfiles -Wl,-Map=$(OUTDIR)/$(TARGET).map,--cref,--gc-sections +LDFLAGS = -Wl,-Map=$(OUTDIR)/$(TARGET).map,--cref,--gc-sections +LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS)) +LDFLAGS += -lc +LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS)) +LDFLAGS += $(MATH_LIB) +LDFLAGS += -lc -lgcc -lpthread -lrt + +#Linker scripts +LDFLAGS += $(addprefix -T,$(LINKER_SCRIPTS_APP)) + + +# Define programs and commands. +REMOVE = $(REMOVE_CMD) -f +PYHON = python + +# List of all source files. +ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC) +# List of all source files without directory and file-extension. +ALLSRCBASE = $(notdir $(basename $(ALLSRC))) + +# Define all object files. +ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE))) + +# Define all listing files (used for make clean). +LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE))) +# Define all depedency-files (used for make clean). +DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE))) + +# Default target. +all: gccversion build + +ifeq ($(LOADFORMAT),ihex) +build: elf hex lss sym +else +ifeq ($(LOADFORMAT),binary) +build: elf bin lss sym +else +ifeq ($(LOADFORMAT),both) +build: elf hex bin lss sym +else +$(error "$(MSG_FORMATERROR) $(FORMAT)") +endif +endif +endif + + +# Link: create ELF output file from object files. +$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ))) + +# Assemble: create object files from assembler source files. +$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src)))) + +# Assemble: create object files from assembler source files. ARM-only +$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src)))) + +# Compile: create object files from C source files. +$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src)))) + +# Compile: create object files from C source files. ARM-only +$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src)))) + +# Compile: create object files from C++ source files. +$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src)))) + +# Compile: create object files from C++ source files. ARM-only +$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src)))) + +# Compile: create assembler files from C source files. ARM/Thumb +$(eval $(call PARTIAL_COMPILE_TEMPLATE, SRC)) + +# Compile: create assembler files from C source files. ARM only +$(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM)) + +$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin + +$(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION))) + +# Add jtag targets (program and wipe) +$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_JTAG_CONFIG),$(OPENOCD_CONFIG))) + +.PHONY: elf lss sym hex bin bino opfw +elf: $(OUTDIR)/$(TARGET).elf +lss: $(OUTDIR)/$(TARGET).lss +sym: $(OUTDIR)/$(TARGET).sym +hex: $(OUTDIR)/$(TARGET).hex +bin: $(OUTDIR)/$(TARGET).bin +bino: $(OUTDIR)/$(TARGET).bin.o +opfw: $(OUTDIR)/$(TARGET).opfw + +# Display sizes of sections. +$(eval $(call SIZE_TEMPLATE, $(OUTDIR)/$(TARGET).elf)) + +# Generate Doxygen documents +docs: + doxygen $(DOXYGENDIR)/doxygen.cfg + +# Install: install binary file with prefix/suffix into install directory +install: $(OUTDIR)/$(TARGET).opfw +ifneq ($(INSTALL_DIR),) + @echo $(MSG_INSTALLING) $(call toprel, $<) + $(V1) mkdir -p $(INSTALL_DIR) + $(V1) $(INSTALL) $< $(INSTALL_DIR)/$(INSTALL_PFX)$(TARGET)$(INSTALL_SFX).opfw +else + $(error INSTALL_DIR must be specified for $@) +endif + +# Target: clean project. +clean: clean_list + +clean_list : + @echo $(MSG_CLEANING) + $(V1) $(REMOVE) $(OUTDIR)/$(TARGET).map + $(V1) $(REMOVE) $(OUTDIR)/$(TARGET).elf + $(V1) $(REMOVE) $(OUTDIR)/$(TARGET).hex + $(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin + $(V1) $(REMOVE) $(OUTDIR)/$(TARGET).sym + $(V1) $(REMOVE) $(OUTDIR)/$(TARGET).lss + $(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin.o + $(V1) $(REMOVE) $(ALLOBJ) + $(V1) $(REMOVE) $(LSTFILES) + $(V1) $(REMOVE) $(DEPFILES) + $(V1) $(REMOVE) $(SRC:.c=.s) + $(V1) $(REMOVE) $(SRCARM:.c=.s) + $(V1) $(REMOVE) $(CPPSRC:.cpp=.s) + $(V1) $(REMOVE) $(CPPSRCARM:.cpp=.s) + +# Create output files directory +# all known MS Windows OS define the ComSpec environment variable +ifdef ComSpec +$(shell md $(subst /,\\,$(OUTDIR)) 2>NUL) +else +$(shell mkdir -p $(OUTDIR) 2>/dev/null) +endif + +# Include the dependency files. +ifdef ComSpec +-include $(shell md $(subst /,\\,$(OUTDIR))\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*) +else +-include $(shell mkdir $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*) +endif + +#create compile-time module auto-initialisation +MODNAMES = ${notdir $(subst /revolution,,$(MODULES))} + +# Test if quotes are needed for the echo-command +result = ${shell echo "test"} +ifeq (${result}, test) + quote = ' +else + quote = +endif + +# Generate intermediate code +gencode: ${OUTDIR}/InitMods.c ${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h + +# Generate code for module initialization +${OUTDIR}/InitMods.c: Makefile + @echo ${MSG_MODINIT} + @echo ${quote}// Autogenerated file${quote} > ${OUTDIR}/InitMods.c + @echo ${quote}${foreach MOD, ${MODNAMES}, extern unsigned int ${MOD}Initialize(void);}${quote} >> ${OUTDIR}/InitMods.c + @echo ${quote}${foreach MOD, ${MODNAMES}, extern unsigned int ${MOD}Start(void);}${quote} >> ${OUTDIR}/InitMods.c + @echo ${quote}void InitModules() {${quote} >> ${OUTDIR}/InitMods.c + @echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Initialize();}${quote} >> ${OUTDIR}/InitMods.c + @echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c + @echo ${quote}void StartModules() {${quote} >> ${OUTDIR}/InitMods.c + @echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Start();}${quote} >> ${OUTDIR}/InitMods.c + @echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c + +# Listing of phony targets. +.PHONY : all build clean clean_list install diff --git a/flight/SimPosix/System/alarms.c b/flight/SimPosix/System/alarms.c new file mode 100644 index 000000000..6ccd5fb62 --- /dev/null +++ b/flight/SimPosix/System/alarms.c @@ -0,0 +1,210 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotLibraries OpenPilot System Libraries + * @brief OpenPilot System libraries are available to all OP modules. + * @{ + * @file alarms.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Library for setting and clearing system alarms + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "openpilot.h" +#include "alarms.h" + +// Private constants + +// Private types + +// Private variables +static xSemaphoreHandle lock; + +// Private functions +static int32_t hasSeverity(SystemAlarmsAlarmOptions severity); + +/** + * Initialize the alarms library + */ +int32_t AlarmsInitialize(void) +{ + SystemAlarmsInitialize(); + lock = xSemaphoreCreateRecursiveMutex(); + return 0; +} + +/** + * Set an alarm + * @param alarm The system alarm to be modified + * @param severity The alarm severity + * @return 0 if success, -1 if an error + */ +int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity) +{ + SystemAlarmsData alarms; + + // Check that this is a valid alarm + if (alarm >= SYSTEMALARMS_ALARM_NUMELEM) + { + return -1; + } + + // Lock + xSemaphoreTakeRecursive(lock, portMAX_DELAY); + + // Read alarm and update its severity only if it was changed + SystemAlarmsGet(&alarms); + if ( alarms.Alarm[alarm] != severity ) + { + alarms.Alarm[alarm] = severity; + SystemAlarmsSet(&alarms); + } + + // Release lock + xSemaphoreGiveRecursive(lock); + return 0; + +} + +/** + * Get an alarm + * @param alarm The system alarm to be read + * @return Alarm severity + */ +SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm) +{ + SystemAlarmsData alarms; + + // Check that this is a valid alarm + if (alarm >= SYSTEMALARMS_ALARM_NUMELEM) + { + return 0; + } + + // Read alarm + SystemAlarmsGet(&alarms); + return alarms.Alarm[alarm]; +} + +/** + * Set an alarm to it's default value + * @param alarm The system alarm to be modified + * @return 0 if success, -1 if an error + */ +int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm) +{ + return AlarmsSet(alarm, SYSTEMALARMS_ALARM_DEFAULT); +} + +/** + * Default all alarms + */ +void AlarmsDefaultAll() +{ + uint32_t n; + for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) + { + AlarmsDefault(n); + } +} + +/** + * Clear an alarm + * @param alarm The system alarm to be modified + * @return 0 if success, -1 if an error + */ +int32_t AlarmsClear(SystemAlarmsAlarmElem alarm) +{ + return AlarmsSet(alarm, SYSTEMALARMS_ALARM_OK); +} + +/** + * Clear all alarms + */ +void AlarmsClearAll() +{ + uint32_t n; + for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) + { + AlarmsClear(n); + } +} + +/** + * Check if there are any alarms with the given or higher severity + * @return 0 if no alarms are found, 1 if at least one alarm is found + */ +int32_t AlarmsHasWarnings() +{ + return hasSeverity(SYSTEMALARMS_ALARM_WARNING); +} + +/** + * Check if there are any alarms with error or higher severity + * @return 0 if no alarms are found, 1 if at least one alarm is found + */ +int32_t AlarmsHasErrors() +{ + return hasSeverity(SYSTEMALARMS_ALARM_ERROR); +}; + +/** + * Check if there are any alarms with critical or higher severity + * @return 0 if no alarms are found, 1 if at least one alarm is found + */ +int32_t AlarmsHasCritical() +{ + return hasSeverity(SYSTEMALARMS_ALARM_CRITICAL); +}; + +/** + * Check if there are any alarms with the given or higher severity + * @return 0 if no alarms are found, 1 if at least one alarm is found + */ +static int32_t hasSeverity(SystemAlarmsAlarmOptions severity) +{ + SystemAlarmsData alarms; + uint32_t n; + + // Lock + xSemaphoreTakeRecursive(lock, portMAX_DELAY); + + // Read alarms + SystemAlarmsGet(&alarms); + + // Go through alarms and check if any are of the given severity or higher + for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) + { + if ( alarms.Alarm[n] >= severity) + { + xSemaphoreGiveRecursive(lock); + return 1; + } + } + + // If this point is reached then no alarms found + xSemaphoreGiveRecursive(lock); + return 0; +} +/** + * @} + * @} + */ + diff --git a/flight/SimPosix/System/inc/FreeRTOSConfig.h b/flight/SimPosix/System/inc/FreeRTOSConfig.h new file mode 100644 index 000000000..72d6e288a --- /dev/null +++ b/flight/SimPosix/System/inc/FreeRTOSConfig.h @@ -0,0 +1,103 @@ + +#ifndef FREERTOS_CONFIG_H +#define FREERTOS_CONFIG_H + +/*----------------------------------------------------------- + * Application specific definitions. + * + * These definitions should be adjusted for your particular hardware and + * application requirements. + * + * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE + * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. + * + * See http://www.freertos.org/a00110.html. + *----------------------------------------------------------*/ + +/** + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ + +/* Notes: We use 5 task priorities */ + +#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ... +#define configTICK_RATE_HZ ((portTickType )1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)512) +#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total +#define configMAX_TASK_NAME_LEN (16) + +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configQUEUE_REGISTRY_SIZE 10 + +#define configUSE_TIMERS 1 +#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */ +#define configTIMER_QUEUE_LENGTH 10 +#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE + +/* Co-routine definitions. */ +#define configUSE_CO_ROUTINES 0 +//#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) + +/* Set the following definitions to 1 to include the API function, or zero +to exclude the API function. */ + +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 1 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 + +/* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 +(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ +#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + +/* This is the value being used as per the ST library which permits 16 +priority values, 0 to 15. This must correspond to the +configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest +NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + +/* Enable run time stats collection */ +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 + +/* + * Once we move to CMSIS2 we can at least use: + * + * CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + * + * (still nothing for the DWT registers, surprisingly) + */ +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1<<24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } while(0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ + + +/** + * @} + */ + +#endif /* FREERTOS_CONFIG_H */ diff --git a/flight/SimPosix/System/inc/alarms.h b/flight/SimPosix/System/inc/alarms.h new file mode 100644 index 000000000..0f0faeb8f --- /dev/null +++ b/flight/SimPosix/System/inc/alarms.h @@ -0,0 +1,50 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotLibraries OpenPilot System Libraries + * @{ + * @file alarms.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Include file of the alarm library + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef ALARMS_H +#define ALARMS_H + +#include "systemalarms.h" +#define SYSTEMALARMS_ALARM_DEFAULT SYSTEMALARMS_ALARM_UNINITIALISED + +int32_t AlarmsInitialize(void); +int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity); +SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm); +int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm); +void AlarmsDefaultAll(); +int32_t AlarmsClear(SystemAlarmsAlarmElem alarm); +void AlarmsClearAll(); +int32_t AlarmsHasWarnings(); +int32_t AlarmsHasErrors(); +int32_t AlarmsHasCritical(); + +#endif // ALARMS_H + +/** + * @} + * @} + */ diff --git a/flight/SimPosix/System/inc/dcc_stdio.h b/flight/SimPosix/System/inc/dcc_stdio.h new file mode 100644 index 000000000..dbfd39342 --- /dev/null +++ b/flight/SimPosix/System/inc/dcc_stdio.h @@ -0,0 +1,36 @@ +/*************************************************************************** + * Copyright (C) 2008 by Dominic Rath * + * Dominic.Rath@gmx.de * + * Copyright (C) 2008 by Spencer Oliver * + * spen@spen-soft.co.uk * + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 2 of the License, or * + * (at your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program; if not, write to the * + * Free Software Foundation, Inc., * + * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * + ***************************************************************************/ + +#ifndef DCC_STDIO_H +#define DCC_STDIO_H + +void dbg_trace_point(unsigned long number); + +void dbg_write_u32(const unsigned long *val, long len); +void dbg_write_u16(const unsigned short *val, long len); +void dbg_write_u8(const unsigned char *val, long len); + +void dbg_write_str(const char *msg); +void dbg_write_char(char msg); +void dbg_write_hex32(const unsigned long val); + +#endif /* DCC_STDIO_H */ diff --git a/flight/SimPosix/System/inc/op_config.h b/flight/SimPosix/System/inc/op_config.h new file mode 100644 index 000000000..0fe9e2bdc --- /dev/null +++ b/flight/SimPosix/System/inc/op_config.h @@ -0,0 +1,39 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * + * @file op_config.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief OpenPilot configuration header. + * Compile time config for OpenPilot Application + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + + +#ifndef OP_CONFIG_H +#define OP_CONFIG_H + +#endif /* OP_CONFIG_H */ +/** + * @} + * @} + */ diff --git a/flight/SimPosix/System/inc/openpilot.h b/flight/SimPosix/System/inc/openpilot.h new file mode 100644 index 000000000..10f40d68a --- /dev/null +++ b/flight/SimPosix/System/inc/openpilot.h @@ -0,0 +1,53 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @file openpilot.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Main OpenPilot header. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + + +#ifndef OPENPILOT_H +#define OPENPILOT_H + + +/* PIOS Includes */ +#include + +/* OpenPilot Libraries */ +#include "op_config.h" +#include "utlist.h" +#include "uavobjectmanager.h" +#include "eventdispatcher.h" +#include "alarms.h" +#include "taskmonitor.h" +#include "uavtalk.h" + +/* Global Functions */ +void OpenPilotInit(void); + +#endif /* OPENPILOT_H */ +/** + * @} + * @} + */ diff --git a/flight/SimPosix/System/inc/pios_config.h b/flight/SimPosix/System/inc/pios_config.h new file mode 100644 index 000000000..d4add3666 --- /dev/null +++ b/flight/SimPosix/System/inc/pios_config.h @@ -0,0 +1,120 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * + * @file pios_config.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief PiOS configuration header. + * Central compile time config for the project. + * In particular, pios_config.h is where you define which PiOS libraries + * and features are included in the firmware. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + + +#ifndef PIOS_CONFIG_H +#define PIOS_CONFIG_H + +/* Major features */ +#define PIOS_INCLUDE_FREERTOS +#define PIOS_INCLUDE_BL_HELPER + +/* Enable/Disable PiOS Modules */ +//#define PIOS_INCLUDE_ADC +#define PIOS_INCLUDE_DELAY +//#define PIOS_INCLUDE_I2C +#define PIOS_INCLUDE_IRQ +#define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_SDCARD +//#define PIOS_INCLUDE_IAP +#define PIOS_INCLUDE_SERVO +#define PIOS_INCLUDE_SPI +#define PIOS_INCLUDE_SYS +#define PIOS_INCLUDE_USART +//#define PIOS_INCLUDE_USB +#define PIOS_INCLUDE_USB_HID +//#define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_EXTI +#define PIOS_INCLUDE_RTC +#define PIOS_INCLUDE_WDG +#define PIOS_INCLUDE_UDP + +/* Select the sensors to include */ +//#define PIOS_INCLUDE_BMA180 +//#define PIOS_INCLUDE_HMC5883 +//#define PIOS_INCLUDE_MPU6000 +//#define PIOS_MPU6000_ACCEL +//#define PIOS_INCLUDE_L3GD20 +//#define PIOS_INCLUDE_MS5611 +//#define PIOS_INCLUDE_HCSR04 +#define PIOS_FLASH_ON_ACCEL /* true for second revo */ +#define FLASH_FREERTOS +/* Com systems to include */ +#define PIOS_INCLUDE_COM +#define PIOS_INCLUDE_COM_TELEM +#define PIOS_INCLUDE_COM_AUX +#define PIOS_INCLUDE_COM_AUXSBUS +#define PIOS_INCLUDE_COM_FLEXI + +#define PIOS_INCLUDE_GPS +#define PIOS_OVERO_SPI +/* Supported receiver interfaces */ +#define PIOS_INCLUDE_RCVR +#define PIOS_INCLUDE_DSM +//#define PIOS_INCLUDE_SBUS +#define PIOS_INCLUDE_PPM +#define PIOS_INCLUDE_PWM +//#define PIOS_INCLUDE_GCSRCVR + +#define PIOS_INCLUDE_SETTINGS +#define PIOS_INCLUDE_FLASH +/* A really shitty setting saving implementation */ +//#define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS + +/* Other Interfaces */ +//#define PIOS_INCLUDE_I2C_ESC + +/* Flags that alter behaviors - mostly to lower resources for CC */ +#define PIOS_INCLUDE_INITCALL /* Include init call structures */ +#define PIOS_TELEM_PRIORITY_QUEUE /* Enable a priority queue in telemetry */ +#define PIOS_QUATERNION_STABILIZATION /* Stabilization options */ +//#define PIOS_GPS_SETS_HOMELOCATION /* GPS options */ + +/* Alarm Thresholds */ +#define HEAP_LIMIT_WARNING 4000 +#define HEAP_LIMIT_CRITICAL 1000 +#define IRQSTACK_LIMIT_WARNING 150 +#define IRQSTACK_LIMIT_CRITICAL 80 +#define CPULOAD_LIMIT_WARNING 80 +#define CPULOAD_LIMIT_CRITICAL 95 + +// This actually needs calibrating +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD (8379692) + +#define REVOLUTION +#define SIMPOSIX + +#endif /* PIOS_CONFIG_H */ +/** + * @} + * @} + */ diff --git a/flight/SimPosix/System/pios_board.c b/flight/SimPosix/System/pios_board.c new file mode 100644 index 000000000..45b078120 --- /dev/null +++ b/flight/SimPosix/System/pios_board.c @@ -0,0 +1,187 @@ +/** + ****************************************************************************** + * @addtogroup Revolution Revolution configuration files + * @{ + * @brief Configures the revolution board + * @{ + * + * @file pios_board.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. + * @brief Defines board specific static initializers for hardware for the Revolution board. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include + +#include +#include +#include "hwsettings.h" +#include "manualcontrolsettings.h" + +#include "board_hw_defs.c" + +/** + * Sensor configurations + */ + +/* One slot per selectable receiver group. + * eg. PWM, PPM, GCS, SPEKTRUM1, SPEKTRUM2, SBUS + * NOTE: No slot in this map for NONE. + */ +uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; + +#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 + +#define PIOS_COM_GPS_RX_BUF_LEN 32 + +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 + +#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 +#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 + +#define PIOS_COM_AUX_RX_BUF_LEN 512 +#define PIOS_COM_AUX_TX_BUF_LEN 512 + +uint32_t pios_com_aux_id = 0; +uint32_t pios_com_gps_id = 0; +uint32_t pios_com_telem_usb_id = 0; +uint32_t pios_com_telem_rf_id = 0; +uint32_t pios_com_bridge_id = 0; + +/* + * Setup a com port based on the passed cfg, driver and buffer sizes. tx size of -1 make the port rx only + */ +static void PIOS_Board_configure_com(const struct pios_udp_cfg *usart_port_cfg, size_t rx_buf_len, size_t tx_buf_len, + const struct pios_com_driver *com_driver, uint32_t *pios_com_id) +{ + uint32_t pios_usart_id; + if (PIOS_UDP_Init(&pios_usart_id, usart_port_cfg)) { + PIOS_Assert(0); + } + + uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(rx_buf_len); + PIOS_Assert(rx_buffer); + if(tx_buf_len!= -1){ // this is the case for rx/tx ports + uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(tx_buf_len); + PIOS_Assert(tx_buffer); + + if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, + rx_buffer, rx_buf_len, + tx_buffer, tx_buf_len)) { + PIOS_Assert(0); + } + } + else{ //rx only port + if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, + rx_buffer, rx_buf_len, + NULL, 0)) { + PIOS_Assert(0); + } + } +} + +/** + * PIOS_Board_Init() + * initializes all the core subsystems on this specific hardware + * called from System/openpilot.c + */ +void PIOS_Board_Init(void) { + + /* Delay system */ + PIOS_DELAY_Init(); + + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); + + HwSettingsInitialize(); + + UAVObjectsInitializeAll(); + + /* Initialize the alarms library */ + AlarmsInitialize(); + + /* Initialize the task monitor library */ + TaskMonitorInitialize(); + + /* Configure IO ports */ + + /* Configure Telemetry port */ + uint8_t hwsettings_rv_telemetryport; + HwSettingsRV_TelemetryPortGet(&hwsettings_rv_telemetryport); + + switch (hwsettings_rv_telemetryport){ + case HWSETTINGS_RV_TELEMETRYPORT_DISABLED: + break; + case HWSETTINGS_RV_TELEMETRYPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_udp_telem_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_telem_rf_id); + break; + case HWSETTINGS_RV_TELEMETRYPORT_COMAUX: + PIOS_Board_configure_com(&pios_udp_telem_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id); + break; + + } /* hwsettings_rv_telemetryport */ + + /* Configure GPS port */ + uint8_t hwsettings_rv_gpsport; + HwSettingsRV_GPSPortGet(&hwsettings_rv_gpsport); + switch (hwsettings_rv_gpsport){ + case HWSETTINGS_RV_GPSPORT_DISABLED: + break; + + case HWSETTINGS_RV_GPSPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_udp_gps_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_telem_rf_id); + break; + + case HWSETTINGS_RV_GPSPORT_GPS: + PIOS_Board_configure_com(&pios_udp_gps_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_udp_com_driver, &pios_com_gps_id); + break; + + case HWSETTINGS_RV_GPSPORT_COMAUX: + PIOS_Board_configure_com(&pios_udp_gps_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id); + break; + + }/* hwsettings_rv_gpsport */ + + /* Configure AUXPort */ + uint8_t hwsettings_rv_auxport; + HwSettingsRV_AuxPortGet(&hwsettings_rv_auxport); + + switch (hwsettings_rv_auxport) { + case HWSETTINGS_RV_AUXPORT_DISABLED: + break; + + case HWSETTINGS_RV_AUXPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_udp_aux_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_telem_rf_id); + break; + + case HWSETTINGS_RV_AUXPORT_COMAUX: + PIOS_Board_configure_com(&pios_udp_aux_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id); + break; + break; + } /* hwsettings_rv_auxport */ +} + +/** + * @} + * @} + */ + diff --git a/flight/SimPosix/System/simposix.c b/flight/SimPosix/System/simposix.c new file mode 100644 index 000000000..1863b81c7 --- /dev/null +++ b/flight/SimPosix/System/simposix.c @@ -0,0 +1,142 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @brief These files are the core system files of OpenPilot. + * They are the ground layer just above PiOS. In practice, OpenPilot actually starts + * in the main() function of openpilot.c + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @brief This is where the OP firmware starts. Those files also define the compile-time + * options of the firmware. + * @{ + * @file openpilot.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Sets up and runs main OpenPilot tasks. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + + +/* OpenPilot Includes */ +#include "openpilot.h" +#include "uavobjectsinit.h" +#include "systemmod.h" + +/* Task Priorities */ +#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) + +/* Global Variables */ + +/* Local Variables */ +#define INCLUDE_TEST_TASKS 0 +#if INCLUDE_TEST_TASKS +static uint8_t sdcard_available; +#endif +FILEINFO File; +char Buffer[1024]; +uint32_t Cache; + +/* Function Prototypes */ +#if INCLUDE_TEST_TASKS +static void TaskTick(void *pvParameters); +static void TaskTesting(void *pvParameters); +static void TaskHIDTest(void *pvParameters); +static void TaskServos(void *pvParameters); +static void TaskSDCard(void *pvParameters); +#endif +int32_t CONSOLE_Parse(uint8_t port, char c); +void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value); + +/* Prototype of PIOS_Board_Init() function */ +extern void PIOS_Board_Init(void); +extern void Stack_Change(void); +static void Stack_Change_Weak () __attribute__ ((weakref ("Stack_Change"))); + +/* Local Variables */ +#define INIT_TASK_PRIORITY (tskIDLE_PRIORITY + configMAX_PRIORITIES - 1) // max priority +#define INIT_TASK_STACK (1024 / 4) // XXX this seems excessive +static xTaskHandle initTaskHandle; + +/* Function Prototypes */ +static void initTask(void *parameters); + +/* Prototype of generated InitModules() function */ +extern void InitModules(void); + +/** +* OpenPilot Main function: +* +* Initialize PiOS
+* Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+* Start FreeRTOS Scheduler (vTaskStartScheduler)
+* If something goes wrong, blink LED1 and LED2 every 100ms +* +*/ +int main() +{ + int result; + + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ + + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); + + /* For Revolution we use a FreeRTOS task to bring up the system so we can */ + /* always rely on FreeRTOS primitive */ + result = xTaskCreate(initTask, (const signed char *)"init", + INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY, + &initTaskHandle); + PIOS_Assert(result == pdPASS); + + /* Start the FreeRTOS scheduler */ + vTaskStartScheduler(); + + /* If all is well we will never reach here as the scheduler will now be running. */ + /* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */ + PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ + for(;;) { \ + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ + PIOS_DELAY_WaitmS(100); \ + }; + + return 0; +} +/** + * Initialisation task. + * + * Runs board and module initialisation, then terminates. + */ +void +initTask(void *parameters) +{ + /* board driver init */ + PIOS_Board_Init(); + + /* Initialize modules */ + MODULE_INITIALISE_ALL; + + /* terminate this task */ + vTaskDelete(NULL); +} + +/** + * @} + * @} + */ + diff --git a/flight/SimPosix/UAVObjects.inc b/flight/SimPosix/UAVObjects.inc new file mode 100644 index 000000000..d91eb9c17 --- /dev/null +++ b/flight/SimPosix/UAVObjects.inc @@ -0,0 +1,83 @@ +##### +# Project: OpenPilot +# +# Makefile for OpenPilot UAVObject code +# +# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2011. +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY +# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License +# for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program; if not, write to the Free Software Foundation, Inc., +# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +##### + +# These are the UAVObjects supposed to be build as part of the OpenPilot target +# (all architectures) + +UAVOBJSRCFILENAMES = +UAVOBJSRCFILENAMES += accessorydesired +UAVOBJSRCFILENAMES += actuatorcommand +UAVOBJSRCFILENAMES += actuatordesired +UAVOBJSRCFILENAMES += actuatorsettings +UAVOBJSRCFILENAMES += altholdsmoothed +UAVOBJSRCFILENAMES += attitudesettings +UAVOBJSRCFILENAMES += attitudeactual +UAVOBJSRCFILENAMES += gyros +UAVOBJSRCFILENAMES += gyrosbias +UAVOBJSRCFILENAMES += accels +UAVOBJSRCFILENAMES += magnetometer +UAVOBJSRCFILENAMES += baroaltitude +UAVOBJSRCFILENAMES += flightbatterysettings +UAVOBJSRCFILENAMES += firmwareiapobj +UAVOBJSRCFILENAMES += flightbatterystate +UAVOBJSRCFILENAMES += flightplancontrol +UAVOBJSRCFILENAMES += flightplansettings +UAVOBJSRCFILENAMES += flightplanstatus +UAVOBJSRCFILENAMES += flighttelemetrystats +UAVOBJSRCFILENAMES += gcstelemetrystats +UAVOBJSRCFILENAMES += gpsposition +UAVOBJSRCFILENAMES += gpssatellites +UAVOBJSRCFILENAMES += gpstime +UAVOBJSRCFILENAMES += guidancesettings +UAVOBJSRCFILENAMES += homelocation +UAVOBJSRCFILENAMES += i2cstats +UAVOBJSRCFILENAMES += manualcontrolcommand +UAVOBJSRCFILENAMES += manualcontrolsettings +UAVOBJSRCFILENAMES += mixersettings +UAVOBJSRCFILENAMES += mixerstatus +UAVOBJSRCFILENAMES += nedaccel +UAVOBJSRCFILENAMES += objectpersistence +UAVOBJSRCFILENAMES += overosyncstats +UAVOBJSRCFILENAMES += positionactual +UAVOBJSRCFILENAMES += positiondesired +UAVOBJSRCFILENAMES += ratedesired +UAVOBJSRCFILENAMES += revocalibration +UAVOBJSRCFILENAMES += sonaraltitude +UAVOBJSRCFILENAMES += stabilizationdesired +UAVOBJSRCFILENAMES += stabilizationsettings +UAVOBJSRCFILENAMES += systemalarms +UAVOBJSRCFILENAMES += systemsettings +UAVOBJSRCFILENAMES += systemstats +UAVOBJSRCFILENAMES += taskinfo +UAVOBJSRCFILENAMES += velocityactual +UAVOBJSRCFILENAMES += velocitydesired +UAVOBJSRCFILENAMES += watchdogstatus +UAVOBJSRCFILENAMES += flightstatus +UAVOBJSRCFILENAMES += hwsettings +UAVOBJSRCFILENAMES += receiveractivity +UAVOBJSRCFILENAMES += cameradesired +UAVOBJSRCFILENAMES += camerastabsettings +UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += altitudeholddesired + +UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c ) +UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/UAVObjects/uavobjectsinittemplate.c b/flight/UAVObjects/uavobjectsinittemplate.c index 97a1525a4..75a104d18 100644 --- a/flight/UAVObjects/uavobjectsinittemplate.c +++ b/flight/UAVObjects/uavobjectsinittemplate.c @@ -36,7 +36,5 @@ $(OBJINC) */ void UAVObjectsInitializeAll() { -return; -// This function is no longer used anyway $(OBJINIT) } diff --git a/flight/board_hw_defs/simposix/board_hw_defs.c b/flight/board_hw_defs/simposix/board_hw_defs.c new file mode 100644 index 000000000..28740cdb9 --- /dev/null +++ b/flight/board_hw_defs/simposix/board_hw_defs.c @@ -0,0 +1,73 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * + * @file board_hw_defs.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Defines board specific static initializers for hardware for the OpenPilot board. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#include + +#ifdef PIOS_INCLUDE_UDP + +#include + +#ifdef PIOS_INCLUDE_COM_TELEM +/* + * Telemetry on main USART + */ +const struct pios_udp_cfg pios_udp_telem_cfg = { + .ip = "0.0.0.0", + .port = 9000, +}; +#endif /* PIOS_COM_TELEM */ + +#ifdef PIOS_INCLUDE_GPS +/* + * GPS USART + */ +const struct pios_udp_cfg pios_udp_gps_cfg = { + .ip = "0.0.0.0", + .port = 9001, +}; + +#endif /* PIOS_INCLUDE_GPS */ + +#ifdef PIOS_INCLUDE_COM_AUX +/* + * AUX USART (UART label on rev2) + */ +const struct pios_udp_cfg pios_udp_aux_cfg = { + .ip = "0.0.0.0", + .port = 9002, +}; +#endif /* PIOS_COM_AUX */ + +#endif /* PIOS_UDP */ + +#if defined(PIOS_INCLUDE_COM) + +#include + +#endif /* PIOS_INCLUDE_COM */ + diff --git a/make/boards/simposix/board-info.mk b/make/boards/simposix/board-info.mk new file mode 100644 index 000000000..4a637a53d --- /dev/null +++ b/make/boards/simposix/board-info.mk @@ -0,0 +1,24 @@ +BOARD_TYPE := 0x10 +BOARD_REVISION := 0x01 +BOOTLOADER_VERSION := 0x01 +HW_TYPE := 0x00 + +MCU := +CHIP := +BOARD := SIM_POSIX +MODEL := +MODEL_SUFFIX := + +OPENOCD_JTAG_CONFIG := +OPENOCD_CONFIG := + +# Note: These must match the values in link_$(BOARD)_memory.ld +#BL_BANK_BASE := 0x08000000 # Start of bootloader flash +#BL_BANK_SIZE := 0x00008000 # Should include BD_INFO region +#FW_BANK_BASE := 0x08008000 # Start of firmware flash +#FW_BANK_SIZE := 0x00038000 # Should include FW_DESC_SIZE + +#FW_DESC_SIZE := 0x00000064 + +OSCILLATOR_FREQ := 8000000 +SYSCLK_FREQ := 168000000 diff --git a/make/firmware-defs.mk b/make/firmware-defs.mk index 087792b45..40e0fec79 100644 --- a/make/firmware-defs.mk +++ b/make/firmware-defs.mk @@ -137,7 +137,7 @@ endef define ASSEMBLE_TEMPLATE $(OUTDIR)/$(notdir $(basename $(1))).o : $(1) @echo $(MSG_ASSEMBLING) $$(call toprel, $$<) - $(V1) $(CC) -c -mthumb $$(ASFLAGS) $$< -o $$@ + $(V1) $(CC) -c $(THUMB) $$(ASFLAGS) $$< -o $$@ endef # Assemble: create object files from assembler source files. ARM-only @@ -151,7 +151,7 @@ endef define COMPILE_C_TEMPLATE $(OUTDIR)/$(notdir $(basename $(1))).o : $(1) @echo $(MSG_COMPILING) $$(call toprel, $$<) - $(V1) $(CC) -c -mthumb $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@ + $(V1) $(CC) -c $(THUMB) $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@ endef # Compile: create object files from C source files. ARM-only @@ -165,7 +165,7 @@ endef define COMPILE_CPP_TEMPLATE $(OUTDIR)/$(notdir $(basename $(1))).o : $(1) @echo $(MSG_COMPILINGCPP) $$(call toprel, $$<) - $(V1) $(CC) -c -mthumb $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@ + $(V1) $(CC) -c $(THUMB) $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@ endef # Compile: create object files from C++ source files. ARM-only @@ -183,14 +183,14 @@ define LINK_TEMPLATE .PRECIOUS : $(2) $(1): $(2) @echo $(MSG_LINKING) $$(call toprel, $$@) - $(V1) $(CC) -mthumb $$(CFLAGS) $(2) --output $$@ $$(LDFLAGS) + $(V1) $(CC) $(THUMB) $$(CFLAGS) $(2) --output $$@ $$(LDFLAGS) endef # Compile: create assembler files from C source files. ARM/Thumb define PARTIAL_COMPILE_TEMPLATE $($(1):.c=.s) : %.s : %.c @echo $(MSG_ASMFROMC) $$(call toprel, $$<) - $(V1) $(CC) -mthumb -S $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@ + $(V1) $(CC) $(THUMB) -S $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@ endef # Compile: create assembler files from C source files. ARM only