rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-19 09:54:15 +01:00
Code Issues Releases Activity

Merge branch 'next' into MikeL

Browse Source 
Conflicts:
	flight/Bootloaders/Revolution/inc/pios_config.h
	flight/Modules/OveroSync/inc/overosync.h
	flight/Modules/Sensors/inc/sensors.h
	flight/PiOS/Boards/STM32F4xx_Revolution.h
	flight/PiOS/STM32F4xx/pios_iap.c
	flight/Revolution/System/inc/pios_config.h
	ground/openpilotgcs/src/plugins/config/config.pro
	ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp
	ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp
This commit is contained in:
Mike LaBranche 2012-06-04 16:59:31 -07:00
commit 0ba8345c51
208 changed files with 15062 additions and 31180 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
Makefile
View File

@ -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_win32 - Build OpenPilot simulation firmware for"
@echo " Windows using mingw and msys"
@echo " sim_win32_clean - Delete all build output for the win32 simulation"
@ -230,7 +227,7 @@ openocd_install: openocd_clean
$(V1) mkdir -p "$(OPENOCD_DIR)"
$(V1) ( \
cd $(OPENOCD_BUILD_DIR)/openocd-0.5.0 ; \
./configure --prefix="$(OPENOCD_DIR)" --enable-ft2232_libftdi --enable-buspirate; \
./configure --prefix="$(OPENOCD_DIR)" --enable-ft2232_libftdi ; \
$(MAKE) --silent ; \
$(MAKE) --silent install ; \
)
@ -633,12 +630,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)
@ -692,14 +690,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

50
flight/Bootloaders/BootloaderUpdater/main.c
View File

@ -28,12 +28,13 @@
/* Bootloader Includes */
#include <pios.h>
#include <stdbool.h>
#include "pios_board_info.h"
#define MAX_WRI_RETRYS 3
/* Prototype of PIOS_Board_Init() function */
extern void PIOS_Board_Init(void);
extern void FLASH_Download();
void error(void);
void error(int);
/* The ADDRESSES of the _binary_* symbols are the important
* data. This is non-intuitive for _binary_size where you
@ -50,12 +51,40 @@ int main() {
PIOS_SYS_Init();
PIOS_Board_Init();
PIOS_LED_On(PIOS_LED_HEARTBEAT);
PIOS_DELAY_WaitmS(3000);
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
/// Self overwrite check
uint32_t base_address = SCB->VTOR;
if ((0x08000000 + embedded_image_size) > base_address)
error();
error(PIOS_LED_HEARTBEAT);
///
/*
* Make sure the bootloader we're carrying is for the same
* board type and board revision as the one we're running on.
*
* Assume the bootloader in flash and the bootloader contained in
* the updater both carry a board_info_blob at the end of the image.
*/
/* Calculate how far the board_info_blob is from the beginning of the bootloader */
uint32_t board_info_blob_offset = (uint32_t)&pios_board_info_blob - (uint32_t)0x08000000;
/* Use the same offset into our embedded bootloader image */
struct pios_board_info * new_board_info_blob = (struct pios_board_info *)
((uint32_t)embedded_image_start + board_info_blob_offset);
/* Compare the two board info blobs to make sure they're for the same HW revision */
if ((pios_board_info_blob.magic != new_board_info_blob->magic) ||
(pios_board_info_blob.board_type != new_board_info_blob->board_type) ||
(pios_board_info_blob.board_rev != new_board_info_blob->board_rev)) {
error(PIOS_LED_HEARTBEAT);
}
/* Embedded bootloader looks like it's the right one for this HW, proceed... */
FLASH_Unlock();
/// Bootloader memory space erase
@ -79,7 +108,7 @@ int main() {
}
if (fail == true)
error();
error(PIOS_LED_HEARTBEAT);
///
@ -87,6 +116,7 @@ int main() {
/// Bootloader programing
for (uint32_t offset = 0; offset < embedded_image_size/sizeof(uint32_t); ++offset) {
bool result = false;
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
for (uint8_t retry = 0; retry < MAX_WRI_RETRYS; ++retry) {
if (result == false) {
result = (FLASH_ProgramWord(0x08000000 + (offset * 4), embedded_image_start[offset])
@ -94,9 +124,15 @@ int main() {
}
}
if (result == false)
error();
error(PIOS_LED_HEARTBEAT);
}
///
for (uint8_t x = 0; x < 3; ++x) {
PIOS_LED_On(PIOS_LED_HEARTBEAT);
PIOS_DELAY_WaitmS(1000);
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
PIOS_DELAY_WaitmS(1000);
}
/// Invalidate the bootloader updater so we won't run
/// the update again on the next power cycle.
@ -109,7 +145,11 @@ int main() {
}
void error(void) {
void error(int led) {
for (;;) {
PIOS_LED_On(led);
PIOS_DELAY_WaitmS(500);
PIOS_LED_Off(led);
PIOS_DELAY_WaitmS(500);
}
}

13
flight/Bootloaders/BootloaderUpdater/pios_board.c
View File

@ -35,6 +35,8 @@
#include <pios.h>
void PIOS_Board_Init(void) {
const struct pios_board_info * bdinfo = &pios_board_info_blob;
/* Enable Prefetch Buffer */
FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
@ -42,8 +44,15 @@ void PIOS_Board_Init(void) {
FLASH_SetLatency(FLASH_Latency_2);
/* Delay system */
PIOS_DELAY_Init();
PIOS_DELAY_Init();
/* LEDs */
#if defined(PIOS_INCLUDE_LED)
const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
PIOS_Assert(led_cfg);
PIOS_LED_Init(led_cfg);
#endif /* PIOS_INCLUDE_LED */
/* Initialize the PiOS library */
PIOS_GPIO_Init();
}

1
flight/Bootloaders/CopterControl/Makefile
View File

@ -115,6 +115,7 @@ SRC += $(PIOSSTM32F10X)/pios_usb_hid_istr.c
SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c
SRC += $(OPSYSTEM)/pios_usb_board_data.c
SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
SRC += $(PIOSCOMMON)/pios_usb_util.c
## PIOS Hardware (Common)
SRC += $(PIOSCOMMON)/pios_board_info.c

1
flight/Bootloaders/CopterControl/inc/pios_usb_board_data.h
View File

@ -39,6 +39,7 @@
#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_COPTERCONTROL
#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_BL)
#define PIOS_USB_BOARD_SN_SUFFIX "+BL"
/*
* The bootloader uses a simplified report structure

2
flight/Bootloaders/CopterControl/main.c
View File

@ -74,7 +74,7 @@ int main() {
PIOS_Board_Init();
PIOS_IAP_Init();
USB_connected = PIOS_USB_CheckAvailable(0);
USB_connected = PIOS_USB_CableConnected(0);
if (PIOS_IAP_CheckRequest() == TRUE) {
PIOS_DELAY_WaitmS(1000);

18
flight/Bootloaders/CopterControl/pios_board.c
View File

@ -62,17 +62,9 @@ void PIOS_Board_Init(void) {
const struct pios_board_info * bdinfo = &pios_board_info_blob;
#if defined(PIOS_INCLUDE_LED)
switch(bdinfo->board_rev) {
case 0x01: // Revision 1
PIOS_LED_Init(&pios_led_cfg_cc);
break;
case 0x02: // Revision 2
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
PIOS_LED_Init(&pios_led_cfg_cc3d);
break;
default:
PIOS_Assert(0);
}
const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
PIOS_Assert(led_cfg);
PIOS_LED_Init(led_cfg);
#endif /* PIOS_INCLUDE_LED */
#if defined(PIOS_INCLUDE_USB)
@ -84,10 +76,10 @@ void PIOS_Board_Init(void) {
uint32_t pios_usb_id;
switch(bdinfo->board_rev) {
case 0x01: // Revision 1
case BOARD_REVISION_CC:
PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc);
break;
case 0x02: // Revision 2
case BOARD_REVISION_CC3D:
PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d);
break;
default:

40
flight/Bootloaders/CopterControl/pios_usb_board_data.c
View File

@ -31,6 +31,7 @@
#include "pios_usb_board_data.h" /* struct usb_*, USB_* */
#include "pios_sys.h" /* PIOS_SYS_SerialNumberGet */
#include "pios_usbhook.h" /* PIOS_USBHOOK_* */
#include "pios_usb_util.h" /* PIOS_USB_UTIL_AsciiToUtf8 */
static const uint8_t usb_product_id[28] = {
sizeof(usb_product_id),
@ -50,40 +51,15 @@ static const uint8_t usb_product_id[28] = {
'l', 0,
};
static uint8_t usb_serial_number[52] = {
static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN*2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1)*2] = {
sizeof(usb_serial_number),
USB_DESC_TYPE_STRING,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0
};
static const struct usb_string_langid usb_lang_id = {
.bLength = sizeof(usb_lang_id),
.bDescriptorType = USB_DESC_TYPE_STRING,
.bLangID = htousbs(USB_LANGID_ENGLISH_UK),
.bLangID = htousbs(USB_LANGID_ENGLISH_US),
};
static const uint8_t usb_vendor_id[28] = {
@ -107,11 +83,13 @@ static const uint8_t usb_vendor_id[28] = {
int32_t PIOS_USB_BOARD_DATA_Init(void)
{
/* Load device serial number into serial number string */
uint8_t sn[25];
uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1];
PIOS_SYS_SerialNumberGet((char *)sn);
for (uint8_t i = 0; sn[i] != '\0' && (2 * i) < usb_serial_number[0]; i++) {
usb_serial_number[2 + 2 * i] = sn[i];
}
/* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */
uint8_t * utf8 = &(usb_serial_number[2]);
utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN);
utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1);
PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id));
PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number));

7
flight/Bootloaders/PipXtreme/Makefile
View File

@ -86,6 +86,7 @@ RTOSDIR = $(APPLIBDIR)/FreeRTOS
RTOSSRCDIR = $(RTOSDIR)/Source
RTOSINCDIR = $(RTOSSRCDIR)/include
DOXYGENDIR = ../Doc/Doxygen
HWDEFSINC = ../../board_hw_defs/$(BOARD_NAME)
# List C source files here. (C dependencies are automatically generated.)
# use file-extension c for "c-only"-files
@ -114,12 +115,11 @@ SRC += $(PIOSSTM32F10X)/pios_usb_hid_istr.c
SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c
SRC += $(OPSYSTEM)/pios_usb_board_data.c
SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
SRC += $(PIOSCOMMON)/pios_usb_util.c
## PIOS Hardware (Common)
SRC += $(PIOSCOMMON)/pios_board_info.c
SRC += $(PIOSCOMMON)/pios_com_msg.c
SRC += $(PIOSCOMMON)/pios_bl_helper.c
SRC += $(PIOSCOMMON)/pios_iap.c
SRC += $(PIOSCOMMON)/printf-stdarg.c
## Libraries for flight calculations
@ -200,8 +200,7 @@ EXTRAINCDIRS += $(MSDDIR)
EXTRAINCDIRS += $(RTOSINCDIR)
EXTRAINCDIRS += $(APPLIBDIR)
EXTRAINCDIRS += $(RTOSSRCDIR)/portable/GCC/ARM_CM3
EXTRAINCDIRS += $(HWDEFSINC)
# List any extra directories to look for library files here.
# Also add directories where the linker should search for

1
flight/Bootloaders/PipXtreme/inc/pios_usb_board_data.h
View File

@ -39,6 +39,7 @@
#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_PIPXTREME
#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_PIPXTREME, USB_OP_BOARD_MODE_BL)
#define PIOS_USB_BOARD_SN_SUFFIX "+BL"
/*
* The bootloader uses a simplified report structure

94
flight/Bootloaders/PipXtreme/pios_board.c
View File

@ -23,97 +23,9 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "board_hw_defs.c"
#include <pios.h>
#if defined(PIOS_INCLUDE_LED)
#include <pios_led_priv.h>
static const struct pios_led pios_leds[] = {
[PIOS_LED_USB] = {
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_3,
.GPIO_Mode = GPIO_Mode_Out_PP,
.GPIO_Speed = GPIO_Speed_50MHz,
},
},
},
[PIOS_LED_LINK] = {
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_5,
.GPIO_Mode = GPIO_Mode_Out_PP,
.GPIO_Speed = GPIO_Speed_50MHz,
},
},
},
[PIOS_LED_RX] = {
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Mode = GPIO_Mode_Out_PP,
.GPIO_Speed = GPIO_Speed_50MHz,
},
},
},
[PIOS_LED_TX] = {
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Mode = GPIO_Mode_Out_PP,
.GPIO_Speed = GPIO_Speed_50MHz,
},
},
},
};
static const struct pios_led_cfg pios_led_cfg = {
.leds = pios_leds,
.num_leds = NELEMENTS(pios_leds),
};
#endif /* PIOS_INCLUDE_LED */
#if defined(PIOS_INCLUDE_COM_MSG)
#include <pios_com_msg_priv.h>
#endif /* PIOS_INCLUDE_COM_MSG */
#if defined(PIOS_INCLUDE_USB)
#include "pios_usb_priv.h"
static const struct pios_usb_cfg pios_usb_main_cfg = {
.irq = {
.init = {
.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
#include "pios_usb_board_data_priv.h"
#include "pios_usb_desc_hid_only_priv.h"
#endif /* PIOS_INCLUDE_USB */
#if defined(PIOS_INCLUDE_USB_HID)
#include <pios_usb_hid_priv.h>
const struct pios_usb_hid_cfg pios_usb_hid_cfg = {
.data_if = 0,
.data_rx_ep = 1,
.data_tx_ep = 1,
};
#endif /* PIOS_INCLUDE_USB_HID */
uint32_t pios_com_telem_usb_id;
/**
@ -139,6 +51,10 @@ void PIOS_Board_Init(void) {
/* Initialize the PiOS library */
PIOS_GPIO_Init();
#if defined(PIOS_INCLUDE_LED)
PIOS_LED_Init(&pios_led_cfg);
#endif /* PIOS_INCLUDE_LED */
#if defined(PIOS_INCLUDE_USB)
/* Initialize board specific USB data */
PIOS_USB_BOARD_DATA_Init();

40
flight/Bootloaders/PipXtreme/pios_usb_board_data.c
View File

@ -31,6 +31,7 @@
#include "pios_usb_board_data.h" /* struct usb_*, USB_* */
#include "pios_sys.h" /* PIOS_SYS_SerialNumberGet */
#include "pios_usbhook.h" /* PIOS_USBHOOK_* */
#include "pios_usb_util.h" /* PIOS_USB_UTIL_AsciiToUtf8 */
static const uint8_t usb_product_id[20] = {
sizeof(usb_product_id),
@ -46,40 +47,15 @@ static const uint8_t usb_product_id[20] = {
'e', 0,
};
static uint8_t usb_serial_number[52] = {
static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN*2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1)*2] = {
sizeof(usb_serial_number),
USB_DESC_TYPE_STRING,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0
};
static const struct usb_string_langid usb_lang_id = {
.bLength = sizeof(usb_lang_id),
.bDescriptorType = USB_DESC_TYPE_STRING,
.bLangID = htousbs(USB_LANGID_ENGLISH_UK),
.bLangID = htousbs(USB_LANGID_ENGLISH_US),
};
static const uint8_t usb_vendor_id[28] = {
@ -103,11 +79,13 @@ static const uint8_t usb_vendor_id[28] = {
int32_t PIOS_USB_BOARD_DATA_Init(void)
{
/* Load device serial number into serial number string */
uint8_t sn[25];
uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1];
PIOS_SYS_SerialNumberGet((char *)sn);
for (uint8_t i = 0; sn[i] != '\0' && (2 * i) < usb_serial_number[0]; i++) {
usb_serial_number[2 + 2 * i] = sn[i];
}
/* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */
uint8_t * utf8 = &(usb_serial_number[2]);
utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN);
utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1);
PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id));
PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number));

1
flight/CopterControl/Makefile
View File

@ -243,6 +243,7 @@ SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c
SRC += $(OPSYSTEM)/pios_usb_board_data.c
SRC += $(PIOSCOMMON)/pios_usb_desc_hid_cdc.c
SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
SRC += $(PIOSCOMMON)/pios_usb_util.c
## PIOS Hardware (Common)
SRC += $(PIOSCOMMON)/pios_crc.c

1
flight/CopterControl/System/inc/pios_usb_board_data.h
View File

@ -41,5 +41,6 @@
#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_COPTERCONTROL
#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_FW)
#define PIOS_USB_BOARD_SN_SUFFIX "+FW"
#endif /* PIOS_USB_BOARD_DATA_H */

61
flight/CopterControl/System/pios_board.c
View File

@ -156,29 +156,21 @@ void PIOS_Board_Init(void) {
const struct pios_board_info * bdinfo = &pios_board_info_blob;
#if defined(PIOS_INCLUDE_LED)
switch(bdinfo->board_rev) {
case 0x01: // Revision 1
PIOS_LED_Init(&pios_led_cfg_cc);
break;
case 0x02: // Revision 2
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
PIOS_LED_Init(&pios_led_cfg_cc3d);
break;
default:
PIOS_Assert(0);
}
const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
PIOS_Assert(led_cfg);
PIOS_LED_Init(led_cfg);
#endif /* PIOS_INCLUDE_LED */
#if defined(PIOS_INCLUDE_SPI)
/* Set up the SPI interface to the serial flash */
switch(bdinfo->board_rev) {
case 0x01: // Revision 1
case BOARD_REVISION_CC:
if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc)) {
PIOS_Assert(0);
}
break;
case 0x02: // Revision 2
case BOARD_REVISION_CC3D:
if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc3d)) {
PIOS_Assert(0);
}
@ -190,11 +182,11 @@ void PIOS_Board_Init(void) {
#endif
switch(bdinfo->board_rev) {
case 0x01: // Revision 1
case BOARD_REVISION_CC:
PIOS_Flash_Jedec_Init(pios_spi_flash_accel_id, 1, &flash_w25x_cfg);
PIOS_FLASHFS_Init(&flashfs_w25x_cfg);
break;
case 0x02: // Revision 2
case BOARD_REVISION_CC3D:
PIOS_Flash_Jedec_Init(pios_spi_flash_accel_id, 0, &flash_m25p_cfg);
PIOS_FLASHFS_Init(&flashfs_m25p_cfg);
break;
@ -246,40 +238,31 @@ void PIOS_Board_Init(void) {
/* Initialize board specific USB data */
PIOS_USB_BOARD_DATA_Init();
/* Flags to determine if various USB interfaces are advertised */
bool usb_hid_present = false;
bool usb_cdc_present = false;
uint8_t hwsettings_usb_devicetype;
HwSettingsUSB_DeviceTypeGet(&hwsettings_usb_devicetype);
switch (hwsettings_usb_devicetype) {
case HWSETTINGS_USB_DEVICETYPE_HIDONLY:
if (PIOS_USB_DESC_HID_ONLY_Init()) {
PIOS_Assert(0);
}
usb_hid_present = true;
break;
case HWSETTINGS_USB_DEVICETYPE_HIDVCP:
if (PIOS_USB_DESC_HID_CDC_Init()) {
PIOS_Assert(0);
}
usb_hid_present = true;
usb_cdc_present = true;
break;
case HWSETTINGS_USB_DEVICETYPE_VCPONLY:
break;
default:
#if defined(PIOS_INCLUDE_USB_CDC)
if (PIOS_USB_DESC_HID_CDC_Init()) {
PIOS_Assert(0);
}
usb_hid_present = true;
usb_cdc_present = true;
#else
if (PIOS_USB_DESC_HID_ONLY_Init()) {
PIOS_Assert(0);
}
usb_hid_present = true;
#endif
uint32_t pios_usb_id;
switch(bdinfo->board_rev) {
case 0x01: // Revision 1
case BOARD_REVISION_CC:
PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc);
break;
case 0x02: // Revision 2
case BOARD_REVISION_CC3D:
PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d);
break;
default:
@ -699,7 +682,7 @@ void PIOS_Board_Init(void) {
#endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */
switch(bdinfo->board_rev) {
case 0x01:
case BOARD_REVISION_CC:
// Revision 1 with invensense gyros, start the ADC
#if defined(PIOS_INCLUDE_ADC)
PIOS_ADC_Init(&pios_adc_cfg);
@ -708,7 +691,7 @@ void PIOS_Board_Init(void) {
PIOS_ADXL345_Init(pios_spi_flash_accel_id, 0);
#endif
break;
case 0x02:
case BOARD_REVISION_CC3D:
// Revision 2 with L3GD20 gyros, start a SPI interface and connect to it
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);

40
flight/CopterControl/System/pios_usb_board_data.c
View File

@ -31,6 +31,7 @@
#include "pios_usb_board_data.h" /* struct usb_*, USB_* */
#include "pios_sys.h" /* PIOS_SYS_SerialNumberGet */
#include "pios_usbhook.h" /* PIOS_USBHOOK_* */
#include "pios_usb_util.h" /* PIOS_USB_UTIL_AsciiToUtf8 */
static const uint8_t usb_product_id[28] = {
sizeof(usb_product_id),
@ -50,40 +51,15 @@ static const uint8_t usb_product_id[28] = {
'l', 0,
};
static uint8_t usb_serial_number[52] = {
static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN*2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1)*2] = {
sizeof(usb_serial_number),
USB_DESC_TYPE_STRING,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0
};
static const struct usb_string_langid usb_lang_id = {
.bLength = sizeof(usb_lang_id),
.bDescriptorType = USB_DESC_TYPE_STRING,
.bLangID = htousbs(USB_LANGID_ENGLISH_UK),
.bLangID = htousbs(USB_LANGID_ENGLISH_US),
};
static const uint8_t usb_vendor_id[28] = {
@ -107,11 +83,13 @@ static const uint8_t usb_vendor_id[28] = {
int32_t PIOS_USB_BOARD_DATA_Init(void)
{
/* Load device serial number into serial number string */
uint8_t sn[25];
uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1];
PIOS_SYS_SerialNumberGet((char *)sn);
for (uint8_t i = 0; sn[i] != '\0' && (2 * i) < usb_serial_number[0]; i++) {
usb_serial_number[2 + 2 * i] = sn[i];
}
/* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */
uint8_t * utf8 = &(usb_serial_number[2]);
utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN);
utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1);
PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id));
PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number));

0
flight/PipXtreme/aes.c → flight/Libraries/aes.c
View File

0
flight/PipXtreme/inc/aes.h → flight/Libraries/inc/aes.h
View File

122
flight/Libraries/inc/packet_handler.h Normal file
View File

@ -0,0 +1,122 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @{
*
* @file packet_handler.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief A packet handler for handeling radio packet transmission.
* @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 __PACKET_HANDLER_H__
#define __PACKET_HANDLER_H__
// Public defines / macros
#define PHPacketSize(p) ((uint8_t*)(p->data) + p->header.data_size - (uint8_t*)p)
#define PHPacketSizeECC(p) ((uint8_t*)(p->data) + p->header.data_size + RS_ECC_NPARITY - (uint8_t*)p)
// Public types
typedef enum {
PACKET_TYPE_NONE = 0,
PACKET_TYPE_CONNECT, // for requesting a connection
PACKET_TYPE_DISCONNECT, // to tell the other modem they cannot connect to us
PACKET_TYPE_READY, // tells the other modem we are ready to accept more data
PACKET_TYPE_NOTREADY, // tells the other modem we're not ready to accept more data - we can also send user data in this packet type
PACKET_TYPE_STATUS, // broadcasts status of this modem
PACKET_TYPE_DATARATE, // for changing the RF data rate
PACKET_TYPE_PING, // used to check link is still up
PACKET_TYPE_ADJUST_TX_PWR, // used to ask the other modem to adjust it's tx power
PACKET_TYPE_DATA, // data packet (packet contains user data)
PACKET_TYPE_ACKED_DATA, // data packet that requies an ACK
PACKET_TYPE_PPM, // PPM relay values
PACKET_TYPE_ACK,
PACKET_TYPE_NACK
} PHPacketType;
typedef struct {
uint32_t destination_id;
uint32_t source_id;
uint8_t type;
uint8_t data_size;
uint8_t tx_seq;
uint8_t rx_seq;
int8_t rssi;
int8_t afc;
} PHPacketHeader;
#define PH_MAX_DATA (PIOS_PH_MAX_PACKET - sizeof(PHPacketHeader) - RS_ECC_NPARITY)
#define PH_PACKET_SIZE(p) (p->data + p->header.data_size - (uint8_t*)p + RS_ECC_NPARITY)
typedef struct {
PHPacketHeader header;
uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY];
} PHPacket, *PHPacketHandle;
#define PH_PPM_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
typedef struct {
PHPacketHeader header;
uint16_t channels[PIOS_RCVR_MAX_CHANNELS];
uint8_t ecc[RS_ECC_NPARITY];
} PHPpmPacket, *PHPpmPacketHandle;
#define PH_STATUS_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
typedef struct {
PHPacketHeader header;
uint16_t retries;
uint16_t errors;
uint16_t uavtalk_errors;
uint16_t resets;
uint8_t ecc[RS_ECC_NPARITY];
} PHStatusPacket, *PHStatusPacketHandle;
typedef struct {
uint8_t winSize;
uint16_t maxConnections;
} PacketHandlerConfig;
typedef int32_t (*PHOutputStream)(PHPacketHandle packet);
typedef void (*PHDataHandler)(uint8_t *data, uint8_t len);
typedef void (*PHStatusHandler)(PHStatusPacketHandle s);
typedef void (*PHPPMHandler)(uint16_t *channels);
typedef uint32_t PHInstHandle;
// Public functions
PHInstHandle PHInitialize(PacketHandlerConfig *cfg);
void PHRegisterOutputStream(PHInstHandle h, PHOutputStream f);
void PHRegisterDataHandler(PHInstHandle h, PHDataHandler f);
void PHRegisterStatusHandler(PHInstHandle h, PHStatusHandler f);
void PHRegisterPPMHandler(PHInstHandle h, PHPPMHandler f);
uint32_t PHConnect(PHInstHandle h, uint32_t dest_id);
PHPacketHandle PHGetRXPacket(PHInstHandle h);
void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p);
PHPacketHandle PHGetTXPacket(PHInstHandle h);
void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p);
uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p);
int32_t PHVerifyPacket(PHInstHandle h, PHPacketHandle p, uint16_t received_len);
uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, bool rx_error);
#endif // __PACKET_HANDLER_H__
/**
* @}
* @}
*/

507
flight/Libraries/packet_handler.c Normal file
View File

@ -0,0 +1,507 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @{
*
* @file packet_handler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief A packet handler for handeling radio packet transmission.
* @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 "packet_handler.h"
#include "aes.h"
#include "ecc.h"
extern char *debug_msg;
// Private types and constants
typedef struct {
PacketHandlerConfig cfg;
PHPacket *tx_packets;
uint8_t tx_win_start;
uint8_t tx_win_end;
PHPacket *rx_packets;
uint8_t rx_win_start;
uint8_t rx_win_end;
uint16_t tx_seq_id;
PHOutputStream stream;
xSemaphoreHandle lock;
PHOutputStream output_stream;
PHDataHandler data_handler;
PHStatusHandler status_handler;
PHPPMHandler ppm_handler;
} PHPacketData, *PHPacketDataHandle;
// Private functions
static uint8_t PHLSendAck(PHPacketDataHandle data, PHPacketHandle p);
static uint8_t PHLSendNAck(PHPacketDataHandle data, PHPacketHandle p);
static uint8_t PHLTransmitPacket(PHPacketDataHandle data, PHPacketHandle p);
/**
* Initialize the Packet Handler library
* \param[in] txWinSize The transmission window size (number of tx packet buffers).
* \param[in] streme A callback function for transmitting the packet.
* \param[in] id The source ID of transmitter.
* \return PHInstHandle The Pachet Handler instance data.
* \return 0 Failure
*/
PHInstHandle PHInitialize(PacketHandlerConfig *cfg)
{
// Allocate the primary structure
PHPacketDataHandle data = pvPortMalloc(sizeof(PHPacketData));
if (!data)
return 0;
data->cfg = *cfg;
data->tx_seq_id = 0;
// Allocate the packet windows
data->tx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.winSize);
data->rx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.winSize);
// Initialize the windows
data->tx_win_start = data->tx_win_end = 0;
data->rx_win_start = data->rx_win_end = 0;
for (uint8_t i = 0; i < data->cfg.winSize; ++i)
{
data->tx_packets[i].header.type = PACKET_TYPE_NONE;
data->rx_packets[i].header.type = PACKET_TYPE_NONE;
}
// Create the lock
data->lock = xSemaphoreCreateRecursiveMutex();
// Initialize the ECC library.
initialize_ecc();
// Return the structure.
return (PHInstHandle)data;
}
/**
* Register an output stream handler
* \param[in] h The packet handler instance data pointer.
* \param[in] f The output stream handler function
*/
void PHRegisterOutputStream(PHInstHandle h, PHOutputStream f)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
data->output_stream = f;
}
/**
* Register a data handler
* \param[in] h The packet handler instance data pointer.
* \param[in] f The data handler function
*/
void PHRegisterDataHandler(PHInstHandle h, PHDataHandler f)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
data->data_handler = f;
}
/**
* Register a PPM packet handler
* \param[in] h The packet handler instance data pointer.
* \param[in] f The PPM handler function
*/
void PHRegisterStatusHandler(PHInstHandle h, PHStatusHandler f)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
data->status_handler = f;
}
/**
* Register a PPM packet handler
* \param[in] h The packet handler instance data pointer.
* \param[in] f The PPM handler function
*/
void PHRegisterPPMHandler(PHInstHandle h, PHPPMHandler f)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
data->ppm_handler = f;
}
/**
* Get a packet out of the transmit buffer.
* \param[in] h The packet handler instance data pointer.
* \param[in] dest_id The destination ID of this connection
* \return PHPacketHandle A pointer to the packet buffer.
* \return 0 No packets buffers avaiable in the transmit window.
*/
uint32_t PHConnect(PHInstHandle h, uint32_t dest_id)
{
return 1;
}
/**
* Get a packet out of the transmit buffer.
* \param[in] h The packet handler instance data pointer.
* \return PHPacketHandle A pointer to the packet buffer.
* \return 0 No packets buffers avaiable in the transmit window.
*/
PHPacketHandle PHGetTXPacket(PHInstHandle h)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
// Lock
xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
PHPacketHandle p = data->tx_packets + data->tx_win_end;
// Is the window full?
uint8_t next_end = (data->tx_win_end + 1) % data->cfg.winSize;
if(next_end == data->tx_win_start)
{
xSemaphoreGiveRecursive(data->lock);
return NULL;
}
data->tx_win_end = next_end;
// Release lock
xSemaphoreGiveRecursive(data->lock);
// Return a pointer to the packet at the end of the TX window.
return p;
}
/**
* Release a packet from the transmit packet buffer window.
* \param[in] h The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer.
* \return Nothing
*/
void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
// Lock
xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
// Change the packet type so we know this packet is unused.
p->header.type = PACKET_TYPE_NONE;
// If this packet is at the start of the window, increment the start index.
while ((data->tx_win_start != data->tx_win_end) &&
(data->tx_packets[data->tx_win_start].header.type == PACKET_TYPE_NONE))
data->tx_win_start = (data->tx_win_start + 1) % data->cfg.winSize;
// Release lock
xSemaphoreGiveRecursive(data->lock);
}
/**
* Get a packet out of the receive buffer.
* \param[in] h The packet handler instance data pointer.
* \return PHPacketHandle A pointer to the packet buffer.
* \return 0 No packets buffers avaiable in the transmit window.
*/
PHPacketHandle PHGetRXPacket(PHInstHandle h)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
// Lock
xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
PHPacketHandle p = data->rx_packets + data->rx_win_end;
// Is the window full?
uint8_t next_end = (data->rx_win_end + 1) % data->cfg.winSize;
if(next_end == data->rx_win_start)
{
// Release lock
xSemaphoreGiveRecursive(data->lock);
return NULL;
}
data->rx_win_end = next_end;
// Release lock
xSemaphoreGiveRecursive(data->lock);
// Return a pointer to the packet at the end of the TX window.
return p;
}
/**
* Release a packet from the receive packet buffer window.
* \param[in] h The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer.
* \return Nothing
*/
void PHReleaseRXPacket(PHInstHandle h, PHPacketHandle p)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
// Lock
xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
// Change the packet type so we know this packet is unused.
p->header.type = PACKET_TYPE_NONE;
// If this packet is at the start of the window, increment the start index.
while ((data->rx_win_start != data->rx_win_end) &&
(data->rx_packets[data->rx_win_start].header.type == PACKET_TYPE_NONE))
data->rx_win_start = (data->rx_win_start + 1) % data->cfg.winSize;
// Release lock
xSemaphoreGiveRecursive(data->lock);
}
/**
* Transmit a packet from the transmit packet buffer window.
* \param[in] h The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer.
* \return 1 Success
* \return 0 Failure
*/
uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
// Try to transmit the packet.
if (!PHLTransmitPacket(data, p))
return 0;
// If this packet doesn't require an ACK, remove it from the TX window.
switch (p->header.type) {
case PACKET_TYPE_READY:
case PACKET_TYPE_NOTREADY:
case PACKET_TYPE_DATA:
case PACKET_TYPE_PPM:
PHReleaseTXPacket(h, p);
break;
}
return 1;
}
/**
* Verify that a buffer contains a valid packet.
* \param[in] h The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer.
* \param[in] received_len The length of data received.
* \return < 0 Failure
* \return > 0 Number of bytes consumed.
*/
int32_t PHVerifyPacket(PHInstHandle h, PHPacketHandle p, uint16_t received_len)
{
// Verify the packet length.
// Note: The last two bytes should be the RSSI and AFC.
uint16_t len = PHPacketSizeECC(p);
if (received_len < (len + 2))
{
DEBUG_PRINTF(1, "Packet length error %d %d\n\r", received_len, len + 2);
return -1;
}
// Attempt to correct any errors in the packet.
decode_data((unsigned char*)p, len);
// Check that there were no unfixed errors.
bool rx_error = check_syndrome() != 0;
if(rx_error)
{
DEBUG_PRINTF(1, "Error in packet\n\r");
return -2;
}
return len + 2;
}
/**
* Process a packet that has been received.
* \param[in] h The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer.
* \param[in] received_len The length of data received.
* \return 0 Failure
* \return 1 Success
*/
uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, bool rx_error)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
uint16_t len = PHPacketSizeECC(p);
// Add the RSSI and AFC to the packet.
p->header.rssi = *(((int8_t*)p) + len);
p->header.afc = *(((int8_t*)p) + len + 1);
switch (p->header.type) {
case PACKET_TYPE_STATUS:
if (!rx_error)
// Pass on the channels to the status handler.
if(data->status_handler)
data->status_handler((PHStatusPacketHandle)p);
break;
case PACKET_TYPE_ACKED_DATA:
// Send the ACK / NACK
if (rx_error)
{
DEBUG_PRINTF(1, "Sending NACK\n\r");
PHLSendNAck(data, p);
}
else
{
PHLSendAck(data, p);
// Pass on the data.
if(data->data_handler)
data->data_handler(p->data, p->header.data_size);
}
break;
case PACKET_TYPE_ACK:
{
// Find the packet ID in the TX buffer, and free it.
unsigned int i = 0;
for (unsigned int i = 0; i < data->cfg.winSize; ++i)
if (data->tx_packets[i].header.tx_seq == p->header.rx_seq)
PHReleaseTXPacket(h, data->tx_packets + i);
#ifdef DEBUG_LEVEL
if (i == data->cfg.winSize)
DEBUG_PRINTF(1, "Error finding acked packet to release\n\r");
#endif
}
break;
case PACKET_TYPE_NACK:
{
// Resend the packet.
unsigned int i = 0;
for ( ; i < data->cfg.winSize; ++i)
if (data->tx_packets[i].header.tx_seq == p->header.rx_seq)
PHLTransmitPacket(data, data->tx_packets + i);
#ifdef DEBUG_LEVEL
if (i == data->cfg.winSize)
DEBUG_PRINTF(1, "Error finding acked packet to NACK\n\r");
DEBUG_PRINTF(1, "Resending after NACK\n\r");
#endif
}
break;
case PACKET_TYPE_PPM:
if (!rx_error)
// Pass on the channels to the PPM handler.
if(data->ppm_handler)
data->ppm_handler(((PHPpmPacketHandle)p)->channels);
break;
case PACKET_TYPE_DATA:
if (!rx_error)
// Pass on the data to the data handler.
if(data->data_handler)
data->data_handler(p->data, p->header.data_size);
break;
default:
break;
}
// Release the packet.
PHReleaseRXPacket(h, p);
return 1;
}
/**
* Transmit a packet from the transmit packet buffer window.
* \param[in] data The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer.
* \return 1 Success
* \return 0 Failure
*/
static uint8_t PHLTransmitPacket(PHPacketDataHandle data, PHPacketHandle p)
{
if(!data->output_stream)
return 0;
// Set the sequence ID to the current ID.
p->header.tx_seq = data->tx_seq_id++;
// Add the error correcting code.
encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p);
// Transmit the packet using the output stream.
if(data->output_stream(p) == -1)
return 0;
return 1;
}
/**
* Send an ACK packet.
* \param[in] data The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer of the packet to be ACKed.
* \return 1 Success
* \return 0 Failure
*/
static uint8_t PHLSendAck(PHPacketDataHandle data, PHPacketHandle p)
{
// Create the ACK message
PHPacketHeader ack;
ack.destination_id = p->header.source_id;
ack.type = PACKET_TYPE_ACK;
ack.rx_seq = p->header.tx_seq;
ack.data_size = 0;
// Send the packet.
return PHLTransmitPacket(data, (PHPacketHandle)&ack);
}
/**
* Send an NAck packet.
* \param[in] data The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer of the packet to be ACKed.
* \return 1 Success
* \return 0 Failure
*/
static uint8_t PHLSendNAck(PHPacketDataHandle data, PHPacketHandle p)
{
// Create the NAck message
PHPacketHeader ack;
ack.destination_id = p->header.source_id;
ack.type = PACKET_TYPE_NACK;
ack.rx_seq = p->header.tx_seq;
ack.data_size = 0;
// Set the packet.
return PHLTransmitPacket(data, (PHPacketHandle)&ack);
}

10
flight/Libraries/rscode/LICENSE Normal file
View File

@ -0,0 +1,10 @@
* (C) Henry Minsky (hqm@alum.mit.edu) 1991-2009
*
* This software library is licensed under terms of the GNU GENERAL
* PUBLIC LICENSE. [See file gpl.txt]
*
* Commercial licensing is available under a separate license, please
* contact author for details.
*
* Latest source code and other info at http://rscode.sourceforge.net

50
flight/Libraries/rscode/Makefile Normal file
View File

@ -0,0 +1,50 @@
# Makefile for Cross Interleaved Reed Solomon encoder/decoder
#
# (c) Henry Minsky, Universal Access 1991-1996
#
RANLIB = ranlib
AR = ar
VERSION = 1.0
DIRNAME= rscode-$(VERSION)
CC = gcc
# OPTIMIZE_FLAGS = -O69
DEBUG_FLAGS = -g
CFLAGS = -Wall -Wstrict-prototypes $(OPTIMIZE_FLAGS) $(DEBUG_FLAGS) -I..
LDFLAGS = $(OPTIMIZE_FLAGS) $(DEBUG_FLAGS)
LIB_CSRC = rs.c galois.c berlekamp.c crcgen.c
LIB_HSRC = ecc.h
LIB_OBJS = rs.o galois.o berlekamp.o crcgen.o
TARGET_LIB = libecc.a
TEST_PROGS = example
TARGETS = $(TARGET_LIB) $(TEST_PROGS)
all: $(TARGETS)
$(TARGET_LIB): $(LIB_OBJS)
$(RM) $@
$(AR) cq $@ $(LIB_OBJS)
if [ "$(RANLIB)" ]; then $(RANLIB) $@; fi
example: example.o galois.o berlekamp.o crcgen.o rs.o
gcc -o example example.o -L. -lecc
clean:
rm -f *.o example libecc.a
rm -f *~
dist:
(cd ..; tar -cvf rscode-$(VERSION).tar $(DIRNAME))
depend:
makedepend $(SRCS)
# DO NOT DELETE THIS LINE -- make depend depends on it.

27
flight/Libraries/rscode/README Normal file
View File

@ -0,0 +1,27 @@
RSCODE version 1.3
See the files
config.doc documentation of some compile time parameters
rs.doc overview of the Reed-Solomon coding program
rs.man a man page, slightly outdated at this point
example.c a simple example of encoding,decoding, and error correction
Makefile should work on a Sun system, may require GNU make.
Henry Minsky
hqm@alum.mit.edu
* (c) Henry Minsky (hqm@alum.mit.edu) 1991-2009
*
* This software library is licensed under terms of the GNU GENERAL
* PUBLIC LICENSE. (See gpl.txt)
*
* Commercial licensing is available under a separate license, please
* contact author for details.
*
* Source code is available at http://rscode.sourceforge.net

324
flight/Libraries/rscode/berlekamp.c Normal file
View File

@ -0,0 +1,324 @@
/***********************************************************************
* Copyright Henry Minsky (hqm@alum.mit.edu) 1991-2009
*
* This software library is licensed under terms of the GNU GENERAL
* PUBLIC LICENSE
*
*
* RSCODE 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.
*
* RSCODE 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 Rscode. If not, see <http://www.gnu.org/licenses/>.
*
* Commercial licensing is available under a separate license, please
* contact author for details.
*
* Source code is available at http://rscode.sourceforge.net
* Berlekamp-Peterson and Berlekamp-Massey Algorithms for error-location
*
* From Cain, Clark, "Error-Correction Coding For Digital Communications", pp. 205.
*
* This finds the coefficients of the error locator polynomial.
*
* The roots are then found by looking for the values of a^n
* where evaluating the polynomial yields zero.
*
* Error correction is done using the error-evaluator equation on pp 207.
*
*/
#include <stdio.h>
#include "ecc.h"
/* The Error Locator Polynomial, also known as Lambda or Sigma. Lambda[0] == 1 */
static int Lambda[MAXDEG];
/* The Error Evaluator Polynomial */
static int Omega[MAXDEG];
/* local ANSI declarations */
static int compute_discrepancy(int lambda[], int S[], int L, int n);
static void init_gamma(int gamma[]);
static void compute_modified_omega (void);
static void mul_z_poly (int src[]);
/* error locations found using Chien's search*/
static int ErrorLocs[256];
static int NErrors;
/* erasure flags */
static int ErasureLocs[256];
static int NErasures;
/* From Cain, Clark, "Error-Correction Coding For Digital Communications", pp. 216. */
void
Modified_Berlekamp_Massey (void)
{
int n, L, L2, k, d, i;
int psi[MAXDEG], psi2[MAXDEG], D[MAXDEG];
int gamma[MAXDEG];
/* initialize Gamma, the erasure locator polynomial */
init_gamma(gamma);
/* initialize to z */
copy_poly(D, gamma);
mul_z_poly(D);
copy_poly(psi, gamma);
k = -1; L = NErasures;
for (n = NErasures; n < RS_ECC_NPARITY; n++) {
d = compute_discrepancy(psi, synBytes, L, n);
if (d != 0) {
/* psi2 = psi - d*D */
for (i = 0; i < MAXDEG; i++) psi2[i] = psi[i] ^ gmult(d, D[i]);
if (L < (n-k)) {
L2 = n-k;
k = n-L;
/* D = scale_poly(ginv(d), psi); */
for (i = 0; i < MAXDEG; i++) D[i] = gmult(psi[i], ginv(d));
L = L2;
}
/* psi = psi2 */
for (i = 0; i < MAXDEG; i++) psi[i] = psi2[i];
}
mul_z_poly(D);
}
for(i = 0; i < MAXDEG; i++) Lambda[i] = psi[i];
compute_modified_omega();
}
/* given Psi (called Lambda in Modified_Berlekamp_Massey) and synBytes,
compute the combined erasure/error evaluator polynomial as
Psi*S mod z^4
*/
void
compute_modified_omega ()
{
int i;
int product[MAXDEG*2];
mult_polys(product, Lambda, synBytes);
zero_poly(Omega);
for(i = 0; i < RS_ECC_NPARITY; i++) Omega[i] = product[i];
}
/* polynomial multiplication */
void
mult_polys (int dst[], int p1[], int p2[])
{
int i, j;
int tmp1[MAXDEG*2];
for (i=0; i < (MAXDEG*2); i++) dst[i] = 0;
for (i = 0; i < MAXDEG; i++) {
for(j=MAXDEG; j<(MAXDEG*2); j++) tmp1[j]=0;
/* scale tmp1 by p1[i] */
for(j=0; j<MAXDEG; j++) tmp1[j]=gmult(p2[j], p1[i]);
/* and mult (shift) tmp1 right by i */
for (j = (MAXDEG*2)-1; j >= i; j--) tmp1[j] = tmp1[j-i];
for (j = 0; j < i; j++) tmp1[j] = 0;
/* add into partial product */
for(j=0; j < (MAXDEG*2); j++) dst[j] ^= tmp1[j];
}
}
/* gamma = product (1-z*a^Ij) for erasure locs Ij */
void
init_gamma (int gamma[])
{
int e, tmp[MAXDEG];
zero_poly(gamma);
zero_poly(tmp);
gamma[0] = 1;
for (e = 0; e < NErasures; e++) {
copy_poly(tmp, gamma);
scale_poly(gexp[ErasureLocs[e]], tmp);
mul_z_poly(tmp);
add_polys(gamma, tmp);
}
}
void
compute_next_omega (int d, int A[], int dst[], int src[])
{
int i;
for ( i = 0; i < MAXDEG; i++) {
dst[i] = src[i] ^ gmult(d, A[i]);
}
}
int
compute_discrepancy (int lambda[], int S[], int L, int n)
{
int i, sum=0;
for (i = 0; i <= L; i++)
sum ^= gmult(lambda[i], S[n-i]);
return (sum);
}
/********** polynomial arithmetic *******************/
void add_polys (int dst[], int src[])
{
int i;
for (i = 0; i < MAXDEG; i++) dst[i] ^= src[i];
}
void copy_poly (int dst[], int src[])
{
int i;
for (i = 0; i < MAXDEG; i++) dst[i] = src[i];
}
void scale_poly (int k, int poly[])
{
int i;
for (i = 0; i < MAXDEG; i++) poly[i] = gmult(k, poly[i]);
}
void zero_poly (int poly[])
{
int i;
for (i = 0; i < MAXDEG; i++) poly[i] = 0;
}
/* multiply by z, i.e., shift right by 1 */
static void mul_z_poly (int src[])
{
int i;
for (i = MAXDEG-1; i > 0; i--) src[i] = src[i-1];
src[0] = 0;
}
/* Finds all the roots of an error-locator polynomial with coefficients
* Lambda[j] by evaluating Lambda at successive values of alpha.
*
* This can be tested with the decoder's equations case.
*/
void
Find_Roots (void)
{
int sum, r, k;
NErrors = 0;
for (r = 1; r < 256; r++) {
sum = 0;
/* evaluate lambda at r */
for (k = 0; k < RS_ECC_NPARITY+1; k++) {
sum ^= gmult(gexp[(k*r)%255], Lambda[k]);
}
if (sum == 0)
{
ErrorLocs[NErrors] = (255-r); NErrors++;
//if (DEBUG) fprintf(stderr, "Root found at r = %d, (255-r) = %d\n", r, (255-r));
}
}
}
/* Combined Erasure And Error Magnitude Computation
*
* Pass in the codeword, its size in bytes, as well as
* an array of any known erasure locations, along the number
* of these erasures.
*
* Evaluate Omega(actually Psi)/Lambda' at the roots
* alpha^(-i) for error locs i.
*
* Returns 1 if everything ok, or 0 if an out-of-bounds error is found
*
*/
int
correct_errors_erasures (unsigned char codeword[],
int csize,
int nerasures,
int erasures[])
{
int r, i, j, err;
/* If you want to take advantage of erasure correction, be sure to
set NErasures and ErasureLocs[] with the locations of erasures.
*/
NErasures = nerasures;
for (i = 0; i < NErasures; i++) ErasureLocs[i] = erasures[i];
Modified_Berlekamp_Massey();
Find_Roots();
if ((NErrors <= RS_ECC_NPARITY) && NErrors > 0) {
/* first check for illegal error locs */
for (r = 0; r < NErrors; r++) {
if (ErrorLocs[r] >= csize) {
//if (DEBUG) fprintf(stderr, "Error loc i=%d outside of codeword length %d\n", i, csize);
return(0);
}
}
for (r = 0; r < NErrors; r++) {
int num, denom;
i = ErrorLocs[r];
/* evaluate Omega at alpha^(-i) */
num = 0;
for (j = 0; j < MAXDEG; j++)
num ^= gmult(Omega[j], gexp[((255-i)*j)%255]);
/* evaluate Lambda' (derivative) at alpha^(-i) ; all odd powers disappear */
denom = 0;
for (j = 1; j < MAXDEG; j += 2) {
denom ^= gmult(Lambda[j], gexp[((255-i)*(j-1)) % 255]);
}
err = gmult(num, ginv(denom));
//if (DEBUG) fprintf(stderr, "Error magnitude %#x at loc %d\n", err, csize-i);
codeword[csize-i-1] ^= err;
}
return(1);
}
else {
//if (DEBUG && NErrors) fprintf(stderr, "Uncorrectable codeword\n");
return(0);
}
}

18
flight/Libraries/rscode/config.doc Normal file
View File

@ -0,0 +1,18 @@
The basic coding parameters are defined using
macros, and an executable can be made by compiling using macro
definitions defining the values of the following names in the file
"ecc.h":
The important compile time parameter is the number of parity bytes,
specified by the #define NPAR.
The library is shipped with
#define NPAR 4
The error-correction routines are polynomial in the number of
parity bytes, so try to keep NPAR small for high performance.
Remember, the sum of the message length (in bytes) plus parity bytes
must be less than or equal to 255.

66
flight/Libraries/rscode/crcgen.c Normal file
View File

@ -0,0 +1,66 @@
/*****************************
* Copyright Henry Minsky (hqm@alum.mit.edu) 1991-2009
*
* This software library is licensed under terms of the GNU GENERAL
* PUBLIC LICENSE
*
* RSCODE 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.
*
* RSCODE 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 Rscode. If not, see <http://www.gnu.org/licenses/>.
* Commercial licensing is available under a separate license, please
* contact author for details.
*
* Source code is available at http://rscode.sourceforge.net
*
* CRC-CCITT generator simulator for byte wide data.
*
*
* CRC-CCITT = x^16 + x^12 + x^5 + 1
*
*
******************************/
#include "ecc.h"
BIT16 crchware(BIT16 data, BIT16 genpoly, BIT16 accum);
/* Computes the CRC-CCITT checksum on array of byte data, length len
*/
BIT16 crc_ccitt(unsigned char *msg, int len)
{
int i;
BIT16 acc = 0;
for (i = 0; i < len; i++) {
acc = crchware((BIT16) msg[i], (BIT16) 0x1021, acc);
}
return(acc);
}
/* models crc hardware (minor variation on polynomial division algorithm) */
BIT16 crchware(BIT16 data, BIT16 genpoly, BIT16 accum)
{
static BIT16 i;
data <<= 8;
for (i = 8; i > 0; i--) {
if ((data ^ accum) & 0x8000)
accum = ((accum << 1) ^ genpoly) & 0xFFFF;
else
accum = (accum<<1) & 0xFFFF;
data = (data<<1) & 0xFFFF;
}
return (accum);
}

98
flight/Libraries/rscode/ecc.h Normal file
View File

@ -0,0 +1,98 @@
/* Reed Solomon Coding for glyphs
* Copyright Henry Minsky (hqm@alum.mit.edu) 1991-2009
*
* This software library is licensed under terms of the GNU GENERAL
* PUBLIC LICENSE
*
* RSCODE 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.
*
* RSCODE 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 Rscode. If not, see <http://www.gnu.org/licenses/>.
*
* Source code is available at http://rscode.sourceforge.net
*
* Commercial licensing is available under a separate license, please
* contact author for details.
*
*/
/****************************************************************
Below is NPAR, the only compile-time parameter you should have to
modify.
It is the number of parity bytes which will be appended to
your data to create a codeword.
Note that the maximum codeword size is 255, so the
sum of your message length plus parity should be less than
or equal to this maximum limit.
In practice, you will get slooow error correction and decoding
if you use more than a reasonably small number of parity bytes.
(say, 10 or 20)
****************************************************************/
/****************************************************************/
#include <openpilot.h>
#define TRUE 1
#define FALSE 0
typedef unsigned long BIT32;
typedef unsigned short BIT16;
/* **************************************************************** */
/* Maximum degree of various polynomials. */
#define MAXDEG (RS_ECC_NPARITY*2)
/*************************************/
/* Encoder parity bytes */
extern int pBytes[MAXDEG];
/* Decoder syndrome bytes */
extern int synBytes[MAXDEG];
/* print debugging info */
extern int DEBUG;
/* Reed Solomon encode/decode routines */
void initialize_ecc (void);
int check_syndrome (void);
void decode_data (unsigned char data[], int nbytes);
void encode_data (unsigned char msg[], int nbytes, unsigned char dst[]);
/* CRC-CCITT checksum generator */
BIT16 crc_ccitt(unsigned char *msg, int len);
/* galois arithmetic tables */
extern const int gexp[];
extern const int glog[];
void init_galois_tables (void);
int ginv(int elt);
int gmult(int a, int b);
/* Error location routines */
int correct_errors_erasures (unsigned char codeword[], int csize,int nerasures, int erasures[]);
/* polynomial arithmetic */
void add_polys(int dst[], int src[]) ;
void scale_poly(int k, int poly[]);
void mult_polys(int dst[], int p1[], int p2[]);
void copy_poly(int dst[], int src[]);
void zero_poly(int poly[]);

128
flight/Libraries/rscode/example.c Normal file
View File

@ -0,0 +1,128 @@
/* Example use of Reed-Solomon library
*
* Copyright Henry Minsky (hqm@alum.mit.edu) 1991-2009
*
* This software library is licensed under terms of the GNU GENERAL
* PUBLIC LICENSE
*
* RSCODE 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.
*
* RSCODE 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 Rscode. If not, see <http://www.gnu.org/licenses/>.
* Commercial licensing is available under a separate license, please
* contact author for details.
*
* This same code demonstrates the use of the encodier and
* decoder/error-correction routines.
*
* We are assuming we have at least four bytes of parity (NPAR >= 4).
*
* This gives us the ability to correct up to two errors, or
* four erasures.
*
* In general, with E errors, and K erasures, you will need
* 2E + K bytes of parity to be able to correct the codeword
* back to recover the original message data.
*
* You could say that each error 'consumes' two bytes of the parity,
* whereas each erasure 'consumes' one byte.
*
* Thus, as demonstrated below, we can inject one error (location unknown)
* and two erasures (with their locations specified) and the
* error-correction routine will be able to correct the codeword
* back to the original message.
* */
#include <stdio.h>
#include <stdlib.h>
#include "ecc.h"
unsigned char msg[] = "Nervously I loaded the twin ducks aboard the revolving pl\
atform.";
unsigned char codeword[256];
/* Some debugging routines to introduce errors or erasures
into a codeword.
*/
/* Introduce a byte error at LOC */
void
byte_err (int err, int loc, unsigned char *dst)
{
printf("Adding Error at loc %d, data %#x\n", loc, dst[loc-1]);
dst[loc-1] ^= err;
}
/* Pass in location of error (first byte position is
labeled starting at 1, not 0), and the codeword.
*/
void
byte_erasure (int loc, unsigned char dst[], int cwsize, int erasures[])
{
printf("Erasure at loc %d, data %#x\n", loc, dst[loc-1]);
dst[loc-1] = 0;
}
int
main (int argc, char *argv[])
{
int erasures[16];
int nerasures = 0;
/* Initialization the ECC library */
initialize_ecc ();
/* ************** */
/* Encode data into codeword, adding NPAR parity bytes */
encode_data(msg, sizeof(msg), codeword);
printf("Encoded data is: \"%s\"\n", codeword);
#define ML (sizeof (msg) + NPAR)
/* Add one error and two erasures */
byte_err(0x35, 3, codeword);
byte_err(0x23, 17, codeword);
byte_err(0x34, 19, codeword);
printf("with some errors: \"%s\"\n", codeword);
/* We need to indicate the position of the erasures. Eraseure
positions are indexed (1 based) from the end of the message... */
erasures[nerasures++] = ML-17;
erasures[nerasures++] = ML-19;
/* Now decode -- encoded codeword size must be passed */
decode_data(codeword, ML);
/* check if syndrome is all zeros */
if (check_syndrome () != 0) {
correct_errors_erasures (codeword,
ML,
nerasures,
erasures);
printf("Corrected codeword: \"%s\"\n", codeword);
}
exit(0);
}

164
flight/Libraries/rscode/galois.c Normal file
View File

@ -0,0 +1,164 @@
/*****************************
* Copyright Henry Minsky (hqm@alum.mit.edu) 1991-2009
*
* This software library is licensed under terms of the GNU GENERAL
* PUBLIC LICENSE
*
* RSCODE 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.
*
* RSCODE 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 Rscode. If not, see <http://www.gnu.org/licenses/>.
* Commercial licensing is available under a separate license, please
* contact author for details.
*
* Source code is available at http://rscode.sourceforge.net
*
*
* Multiplication and Arithmetic on Galois Field GF(256)
*
* From Mee, Daniel, "Magnetic Recording, Volume III", Ch. 5 by Patel.
*
*
******************************/
#include <stdio.h>
#include <stdlib.h>
#include "ecc.h"
/* This is one of 14 irreducible polynomials
* of degree 8 and cycle length 255. (Ch 5, pp. 275, Magnetic Recording)
* The high order 1 bit is implicit */
/* x^8 + x^4 + x^3 + x^2 + 1 */
#define PPOLY 0x1D
const int gexp[512] = {
1, 2, 4, 8, 16, 32, 64, 128, 29, 58, 116, 232, 205, 135, 19, 38,
76, 152, 45, 90, 180, 117, 234, 201, 143, 3, 6, 12, 24, 48, 96, 192,
157, 39, 78, 156, 37, 74, 148, 53, 106, 212, 181, 119, 238, 193, 159, 35,
70, 140, 5, 10, 20, 40, 80, 160, 93, 186, 105, 210, 185, 111, 222, 161,
95, 190, 97, 194, 153, 47, 94, 188, 101, 202, 137, 15, 30, 60, 120, 240,
253, 231, 211, 187, 107, 214, 177, 127, 254, 225, 223, 163, 91, 182, 113, 226,
217, 175, 67, 134, 17, 34, 68, 136, 13, 26, 52, 104, 208, 189, 103, 206,
129, 31, 62, 124, 248, 237, 199, 147, 59, 118, 236, 197, 151, 51, 102, 204,
133, 23, 46, 92, 184, 109, 218, 169, 79, 158, 33, 66, 132, 21, 42, 84,
168, 77, 154, 41, 82, 164, 85, 170, 73, 146, 57, 114, 228, 213, 183, 115,
230, 209, 191, 99, 198, 145, 63, 126, 252, 229, 215, 179, 123, 246, 241, 255,
227, 219, 171, 75, 150, 49, 98, 196, 149, 55, 110, 220, 165, 87, 174, 65,
130, 25, 50, 100, 200, 141, 7, 14, 28, 56, 112, 224, 221, 167, 83, 166,
81, 162, 89, 178, 121, 242, 249, 239, 195, 155, 43, 86, 172, 69, 138, 9,
18, 36, 72, 144, 61, 122, 244, 245, 247, 243, 251, 235, 203, 139, 11, 22,
44, 88, 176, 125, 250, 233, 207, 131, 27, 54, 108, 216, 173, 71, 142, 1,
2, 4, 8, 16, 32, 64, 128, 29, 58, 116, 232, 205, 135, 19, 38, 76,
152, 45, 90, 180, 117, 234, 201, 143, 3, 6, 12, 24, 48, 96, 192, 157,
39, 78, 156, 37, 74, 148, 53, 106, 212, 181, 119, 238, 193, 159, 35, 70,
140, 5, 10, 20, 40, 80, 160, 93, 186, 105, 210, 185, 111, 222, 161, 95,
190, 97, 194, 153, 47, 94, 188, 101, 202, 137, 15, 30, 60, 120, 240, 253,
231, 211, 187, 107, 214, 177, 127, 254, 225, 223, 163, 91, 182, 113, 226, 217,
175, 67, 134, 17, 34, 68, 136, 13, 26, 52, 104, 208, 189, 103, 206, 129,
31, 62, 124, 248, 237, 199, 147, 59, 118, 236, 197, 151, 51, 102, 204, 133,
23, 46, 92, 184, 109, 218, 169, 79, 158, 33, 66, 132, 21, 42, 84, 168,
77, 154, 41, 82, 164, 85, 170, 73, 146, 57, 114, 228, 213, 183, 115, 230,
209, 191, 99, 198, 145, 63, 126, 252, 229, 215, 179, 123, 246, 241, 255, 227,
219, 171, 75, 150, 49, 98, 196, 149, 55, 110, 220, 165, 87, 174, 65, 130,
25, 50, 100, 200, 141, 7, 14, 28, 56, 112, 224, 221, 167, 83, 166, 81,
162, 89, 178, 121, 242, 249, 239, 195, 155, 43, 86, 172, 69, 138, 9, 18,
36, 72, 144, 61, 122, 244, 245, 247, 243, 251, 235, 203, 139, 11, 22, 44,
88, 176, 125, 250, 233, 207, 131, 27, 54, 108, 216, 173, 71, 142, 1, 0,
};
const int glog[256] = {
0, 0, 1, 25, 2, 50, 26, 198, 3, 223, 51, 238, 27, 104, 199, 75,
4, 100, 224, 14, 52, 141, 239, 129, 28, 193, 105, 248, 200, 8, 76, 113,
5, 138, 101, 47, 225, 36, 15, 33, 53, 147, 142, 218, 240, 18, 130, 69,
29, 181, 194, 125, 106, 39, 249, 185, 201, 154, 9, 120, 77, 228, 114, 166,
6, 191, 139, 98, 102, 221, 48, 253, 226, 152, 37, 179, 16, 145, 34, 136,
54, 208, 148, 206, 143, 150, 219, 189, 241, 210, 19, 92, 131, 56, 70, 64,
30, 66, 182, 163, 195, 72, 126, 110, 107, 58, 40, 84, 250, 133, 186, 61,
202, 94, 155, 159, 10, 21, 121, 43, 78, 212, 229, 172, 115, 243, 167, 87,
7, 112, 192, 247, 140, 128, 99, 13, 103, 74, 222, 237, 49, 197, 254, 24,
227, 165, 153, 119, 38, 184, 180, 124, 17, 68, 146, 217, 35, 32, 137, 46,
55, 63, 209, 91, 149, 188, 207, 205, 144, 135, 151, 178, 220, 252, 190, 97,
242, 86, 211, 171, 20, 42, 93, 158, 132, 60, 57, 83, 71, 109, 65, 162,
31, 45, 67, 216, 183, 123, 164, 118, 196, 23, 73, 236, 127, 12, 111, 246,
108, 161, 59, 82, 41, 157, 85, 170, 251, 96, 134, 177, 187, 204, 62, 90,
203, 89, 95, 176, 156, 169, 160, 81, 11, 245, 22, 235, 122, 117, 44, 215,
79, 174, 213, 233, 230, 231, 173, 232, 116, 214, 244, 234, 168, 80, 88, 175,
};
//static void init_exp_table (void);
void
init_galois_tables (void)
{
/* initialize the table of powers of alpha */
//init_exp_table();
}
#ifdef NEVER
static void
init_exp_table (void)
{
int i, z;
int pinit,p1,p2,p3,p4,p5,p6,p7,p8;
pinit = p2 = p3 = p4 = p5 = p6 = p7 = p8 = 0;
p1 = 1;
gexp[0] = 1;
gexp[255] = gexp[0];
glog[0] = 0; /* shouldn't log[0] be an error? */
for (i = 1; i < 256; i++) {
pinit = p8;
p8 = p7;
p7 = p6;
p6 = p5;
p5 = p4 ^ pinit;
p4 = p3 ^ pinit;
p3 = p2 ^ pinit;
p2 = p1;
p1 = pinit;
gexp[i] = p1 + p2*2 + p3*4 + p4*8 + p5*16 + p6*32 + p7*64 + p8*128;
gexp[i+255] = gexp[i];
}
for (i = 1; i < 256; i++) {
for (z = 0; z < 256; z++) {
if (gexp[z] == i) {
glog[i] = z;
break;
}
}
}
}
#endif
/* multiplication using logarithms */
int gmult(int a, int b)
{
int i,j;
if (a==0 || b == 0) return (0);
i = glog[a];
j = glog[b];
return (gexp[i+j]);
}
int ginv (int elt)
{
return (gexp[255-glog[elt]]);
}

674
flight/Libraries/rscode/gpl.txt Normal file
View File

@ -0,0 +1,674 @@
GNU GENERAL PUBLIC LICENSE
Version 3, 29 June 2007
Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
Preamble
The GNU General Public License is a free, copyleft license for
software and other kinds of works.
The licenses for most software and other practical works are designed
to take away your freedom to share and change the works. By contrast,
the GNU General Public License is intended to guarantee your freedom to
share and change all versions of a program--to make sure it remains free
software for all its users. We, the Free Software Foundation, use the
GNU General Public License for most of our software; it applies also to
any other work released this way by its authors. You can apply it to
your programs, too.
When we speak of free software, we are referring to freedom, not
price. Our General Public Licenses are designed to make sure that you
have the freedom to distribute copies of free software (and charge for
them if you wish), that you receive source code or can get it if you
want it, that you can change the software or use pieces of it in new
free programs, and that you know you can do these things.
To protect your rights, we need to prevent others from denying you
these rights or asking you to surrender the rights. Therefore, you have
certain responsibilities if you distribute copies of the software, or if
you modify it: responsibilities to respect the freedom of others.
For example, if you distribute copies of such a program, whether
gratis or for a fee, you must pass on to the recipients the same
freedoms that you received. You must make sure that they, too, receive
or can get the source code. And you must show them these terms so they
know their rights.
Developers that use the GNU GPL protect your rights with two steps:
(1) assert copyright on the software, and (2) offer you this License
giving you legal permission to copy, distribute and/or modify it.
For the developers' and authors' protection, the GPL clearly explains
that there is no warranty for this free software. For both users' and
authors' sake, the GPL requires that modified versions be marked as
changed, so that their problems will not be attributed erroneously to
authors of previous versions.
Some devices are designed to deny users access to install or run
modified versions of the software inside them, although the manufacturer
can do so. This is fundamentally incompatible with the aim of
protecting users' freedom to change the software. The systematic
pattern of such abuse occurs in the area of products for individuals to
use, which is precisely where it is most unacceptable. Therefore, we
have designed this version of the GPL to prohibit the practice for those
products. If such problems arise substantially in other domains, we
stand ready to extend this provision to those domains in future versions
of the GPL, as needed to protect the freedom of users.
Finally, every program is threatened constantly by software patents.
States should not allow patents to restrict development and use of
software on general-purpose computers, but in those that do, we wish to
avoid the special danger that patents applied to a free program could
make it effectively proprietary. To prevent this, the GPL assures that
patents cannot be used to render the program non-free.
The precise terms and conditions for copying, distribution and
modification follow.
TERMS AND CONDITIONS
0. Definitions.
"This License" refers to version 3 of the GNU General Public License.
"Copyright" also means copyright-like laws that apply to other kinds of
works, such as semiconductor masks.
"The Program" refers to any copyrightable work licensed under this
License. Each licensee is addressed as "you". "Licensees" and
"recipients" may be individuals or organizations.
To "modify" a work means to copy from or adapt all or part of the work
in a fashion requiring copyright permission, other than the making of an
exact copy. The resulting work is called a "modified version" of the
earlier work or a work "based on" the earlier work.
A "covered work" means either the unmodified Program or a work based
on the Program.
To "propagate" a work means to do anything with it that, without
permission, would make you directly or secondarily liable for
infringement under applicable copyright law, except executing it on a
computer or modifying a private copy. Propagation includes copying,
distribution (with or without modification), making available to the
public, and in some countries other activities as well.
To "convey" a work means any kind of propagation that enables other
parties to make or receive copies. Mere interaction with a user through
a computer network, with no transfer of a copy, is not conveying.
An interactive user interface displays "Appropriate Legal Notices"
to the extent that it includes a convenient and prominently visible
feature that (1) displays an appropriate copyright notice, and (2)
tells the user that there is no warranty for the work (except to the
extent that warranties are provided), that licensees may convey the
work under this License, and how to view a copy of this License. If
the interface presents a list of user commands or options, such as a
menu, a prominent item in the list meets this criterion.
1. Source Code.
The "source code" for a work means the preferred form of the work
for making modifications to it. "Object code" means any non-source
form of a work.
A "Standard Interface" means an interface that either is an official
standard defined by a recognized standards body, or, in the case of
interfaces specified for a particular programming language, one that
is widely used among developers working in that language.
The "System Libraries" of an executable work include anything, other
than the work as a whole, that (a) is included in the normal form of
packaging a Major Component, but which is not part of that Major
Component, and (b) serves only to enable use of the work with that
Major Component, or to implement a Standard Interface for which an
implementation is available to the public in source code form. A
"Major Component", in this context, means a major essential component
(kernel, window system, and so on) of the specific operating system
(if any) on which the executable work runs, or a compiler used to
produce the work, or an object code interpreter used to run it.
The "Corresponding Source" for a work in object code form means all
the source code needed to generate, install, and (for an executable
work) run the object code and to modify the work, including scripts to
control those activities. However, it does not include the work's
System Libraries, or general-purpose tools or generally available free
programs which are used unmodified in performing those activities but
which are not part of the work. For example, Corresponding Source
includes interface definition files associated with source files for
the work, and the source code for shared libraries and dynamically
linked subprograms that the work is specifically designed to require,
such as by intimate data communication or control flow between those
subprograms and other parts of the work.
The Corresponding Source need not include anything that users
can regenerate automatically from other parts of the Corresponding
Source.
The Corresponding Source for a work in source code form is that
same work.
2. Basic Permissions.
All rights granted under this License are granted for the term of
copyright on the Program, and are irrevocable provided the stated
conditions are met. This License explicitly affirms your unlimited
permission to run the unmodified Program. The output from running a
covered work is covered by this License only if the output, given its
content, constitutes a covered work. This License acknowledges your
rights of fair use or other equivalent, as provided by copyright law.
You may make, run and propagate covered works that you do not
convey, without conditions so long as your license otherwise remains
in force. You may convey covered works to others for the sole purpose
of having them make modifications exclusively for you, or provide you
with facilities for running those works, provided that you comply with
the terms of this License in conveying all material for which you do
not control copyright. Those thus making or running the covered works
for you must do so exclusively on your behalf, under your direction
and control, on terms that prohibit them from making any copies of
your copyrighted material outside their relationship with you.
Conveying under any other circumstances is permitted solely under
the conditions stated below. Sublicensing is not allowed; section 10
makes it unnecessary.
3. Protecting Users' Legal Rights From Anti-Circumvention Law.
No covered work shall be deemed part of an effective technological
measure under any applicable law fulfilling obligations under article
11 of the WIPO copyright treaty adopted on 20 December 1996, or
similar laws prohibiting or restricting circumvention of such
measures.
When you convey a covered work, you waive any legal power to forbid
circumvention of technological measures to the extent such circumvention
is effected by exercising rights under this License with respect to
the covered work, and you disclaim any intention to limit operation or
modification of the work as a means of enforcing, against the work's
users, your or third parties' legal rights to forbid circumvention of
technological measures.
4. Conveying Verbatim Copies.
You may convey verbatim copies of the Program's source code as you
receive it, in any medium, provided that you conspicuously and
appropriately publish on each copy an appropriate copyright notice;
keep intact all notices stating that this License and any
non-permissive terms added in accord with section 7 apply to the code;
keep intact all notices of the absence of any warranty; and give all
recipients a copy of this License along with the Program.
You may charge any price or no price for each copy that you convey,
and you may offer support or warranty protection for a fee.
5. Conveying Modified Source Versions.
You may convey a work based on the Program, or the modifications to
produce it from the Program, in the form of source code under the
terms of section 4, provided that you also meet all of these conditions:
a) The work must carry prominent notices stating that you modified
it, and giving a relevant date.
b) The work must carry prominent notices stating that it is
released under this License and any conditions added under section
7. This requirement modifies the requirement in section 4 to
"keep intact all notices".
c) You must license the entire work, as a whole, under this
License to anyone who comes into possession of a copy. This
License will therefore apply, along with any applicable section 7
additional terms, to the whole of the work, and all its parts,
regardless of how they are packaged. This License gives no
permission to license the work in any other way, but it does not
invalidate such permission if you have separately received it.
d) If the work has interactive user interfaces, each must display
Appropriate Legal Notices; however, if the Program has interactive
interfaces that do not display Appropriate Legal Notices, your
work need not make them do so.
A compilation of a covered work with other separate and independent
works, which are not by their nature extensions of the covered work,
and which are not combined with it such as to form a larger program,
in or on a volume of a storage or distribution medium, is called an
"aggregate" if the compilation and its resulting copyright are not
used to limit the access or legal rights of the compilation's users
beyond what the individual works permit. Inclusion of a covered work
in an aggregate does not cause this License to apply to the other
parts of the aggregate.
6. Conveying Non-Source Forms.
You may convey a covered work in object code form under the terms
of sections 4 and 5, provided that you also convey the
machine-readable Corresponding Source under the terms of this License,
in one of these ways:
a) Convey the object code in, or embodied in, a physical product
(including a physical distribution medium), accompanied by the
Corresponding Source fixed on a durable physical medium
customarily used for software interchange.
b) Convey the object code in, or embodied in, a physical product
(including a physical distribution medium), accompanied by a
written offer, valid for at least three years and valid for as
long as you offer spare parts or customer support for that product
model, to give anyone who possesses the object code either (1) a
copy of the Corresponding Source for all the software in the
product that is covered by this License, on a durable physical
medium customarily used for software interchange, for a price no
more than your reasonable cost of physically performing this
conveying of source, or (2) access to copy the
Corresponding Source from a network server at no charge.
c) Convey individual copies of the object code with a copy of the
written offer to provide the Corresponding Source. This
alternative is allowed only occasionally and noncommercially, and
only if you received the object code with such an offer, in accord
with subsection 6b.
d) Convey the object code by offering access from a designated
place (gratis or for a charge), and offer equivalent access to the
Corresponding Source in the same way through the same place at no
further charge. You need not require recipients to copy the
Corresponding Source along with the object code. If the place to
copy the object code is a network server, the Corresponding Source
may be on a different server (operated by you or a third party)
that supports equivalent copying facilities, provided you maintain
clear directions next to the object code saying where to find the
Corresponding Source. Regardless of what server hosts the
Corresponding Source, you remain obligated to ensure that it is
available for as long as needed to satisfy these requirements.
e) Convey the object code using peer-to-peer transmission, provided
you inform other peers where the object code and Corresponding
Source of the work are being offered to the general public at no
charge under subsection 6d.
A separable portion of the object code, whose source code is excluded
from the Corresponding Source as a System Library, need not be
included in conveying the object code work.
A "User Product" is either (1) a "consumer product", which means any
tangible personal property which is normally used for personal, family,
or household purposes, or (2) anything designed or sold for incorporation
into a dwelling. In determining whether a product is a consumer product,
doubtful cases shall be resolved in favor of coverage. For a particular
product received by a particular user, "normally used" refers to a
typical or common use of that class of product, regardless of the status
of the particular user or of the way in which the particular user
actually uses, or expects or is expected to use, the product. A product
is a consumer product regardless of whether the product has substantial
commercial, industrial or non-consumer uses, unless such uses represent
the only significant mode of use of the product.
"Installation Information" for a User Product means any methods,
procedures, authorization keys, or other information required to install
and execute modified versions of a covered work in that User Product from
a modified version of its Corresponding Source. The information must
suffice to ensure that the continued functioning of the modified object
code is in no case prevented or interfered with solely because
modification has been made.
If you convey an object code work under this section in, or with, or
specifically for use in, a User Product, and the conveying occurs as
part of a transaction in which the right of possession and use of the
User Product is transferred to the recipient in perpetuity or for a
fixed term (regardless of how the transaction is characterized), the
Corresponding Source conveyed under this section must be accompanied
by the Installation Information. But this requirement does not apply
if neither you nor any third party retains the ability to install
modified object code on the User Product (for example, the work has
been installed in ROM).
The requirement to provide Installation Information does not include a
requirement to continue to provide support service, warranty, or updates
for a work that has been modified or installed by the recipient, or for
the User Product in which it has been modified or installed. Access to a
network may be denied when the modification itself materially and
adversely affects the operation of the network or violates the rules and
protocols for communication across the network.
Corresponding Source conveyed, and Installation Information provided,
in accord with this section must be in a format that is publicly
documented (and with an implementation available to the public in
source code form), and must require no special password or key for
unpacking, reading or copying.
7. Additional Terms.
"Additional permissions" are terms that supplement the terms of this
License by making exceptions from one or more of its conditions.
Additional permissions that are applicable to the entire Program shall
be treated as though they were included in this License, to the extent
that they are valid under applicable law. If additional permissions
apply only to part of the Program, that part may be used separately
under those permissions, but the entire Program remains governed by
this License without regard to the additional permissions.
When you convey a copy of a covered work, you may at your option
remove any additional permissions from that copy, or from any part of
it. (Additional permissions may be written to require their own
removal in certain cases when you modify the work.) You may place
additional permissions on material, added by you to a covered work,
for which you have or can give appropriate copyright permission.
Notwithstanding any other provision of this License, for material you
add to a covered work, you may (if authorized by the copyright holders of
that material) supplement the terms of this License with terms:
a) Disclaiming warranty or limiting liability differently from the
terms of sections 15 and 16 of this License; or
b) Requiring preservation of specified reasonable legal notices or
author attributions in that material or in the Appropriate Legal
Notices displayed by works containing it; or
c) Prohibiting misrepresentation of the origin of that material, or
requiring that modified versions of such material be marked in
reasonable ways as different from the original version; or
d) Limiting the use for publicity purposes of names of licensors or
authors of the material; or
e) Declining to grant rights under trademark law for use of some
trade names, trademarks, or service marks; or
f) Requiring indemnification of licensors and authors of that
material by anyone who conveys the material (or modified versions of
it) with contractual assumptions of liability to the recipient, for
any liability that these contractual assumptions directly impose on
those licensors and authors.
All other non-permissive additional terms are considered "further
restrictions" within the meaning of section 10. If the Program as you
received it, or any part of it, contains a notice stating that it is
governed by this License along with a term that is a further
restriction, you may remove that term. If a license document contains
a further restriction but permits relicensing or conveying under this
License, you may add to a covered work material governed by the terms
of that license document, provided that the further restriction does
not survive such relicensing or conveying.
If you add terms to a covered work in accord with this section, you
must place, in the relevant source files, a statement of the
additional terms that apply to those files, or a notice indicating
where to find the applicable terms.
Additional terms, permissive or non-permissive, may be stated in the
form of a separately written license, or stated as exceptions;
the above requirements apply either way.
8. Termination.
You may not propagate or modify a covered work except as expressly
provided under this License. Any attempt otherwise to propagate or
modify it is void, and will automatically terminate your rights under
this License (including any patent licenses granted under the third
paragraph of section 11).
However, if you cease all violation of this License, then your
license from a particular copyright holder is reinstated (a)
provisionally, unless and until the copyright holder explicitly and
finally terminates your license, and (b) permanently, if the copyright
holder fails to notify you of the violation by some reasonable means
prior to 60 days after the cessation.
Moreover, your license from a particular copyright holder is
reinstated permanently if the copyright holder notifies you of the
violation by some reasonable means, this is the first time you have
received notice of violation of this License (for any work) from that
copyright holder, and you cure the violation prior to 30 days after
your receipt of the notice.
Termination of your rights under this section does not terminate the
licenses of parties who have received copies or rights from you under
this License. If your rights have been terminated and not permanently
reinstated, you do not qualify to receive new licenses for the same
material under section 10.
9. Acceptance Not Required for Having Copies.
You are not required to accept this License in order to receive or
run a copy of the Program. Ancillary propagation of a covered work
occurring solely as a consequence of using peer-to-peer transmission
to receive a copy likewise does not require acceptance. However,
nothing other than this License grants you permission to propagate or
modify any covered work. These actions infringe copyright if you do
not accept this License. Therefore, by modifying or propagating a
covered work, you indicate your acceptance of this License to do so.
10. Automatic Licensing of Downstream Recipients.
Each time you convey a covered work, the recipient automatically
receives a license from the original licensors, to run, modify and
propagate that work, subject to this License. You are not responsible
for enforcing compliance by third parties with this License.
An "entity transaction" is a transaction transferring control of an
organization, or substantially all assets of one, or subdividing an
organization, or merging organizations. If propagation of a covered
work results from an entity transaction, each party to that
transaction who receives a copy of the work also receives whatever
licenses to the work the party's predecessor in interest had or could
give under the previous paragraph, plus a right to possession of the
Corresponding Source of the work from the predecessor in interest, if
the predecessor has it or can get it with reasonable efforts.
You may not impose any further restrictions on the exercise of the
rights granted or affirmed under this License. For example, you may
not impose a license fee, royalty, or other charge for exercise of
rights granted under this License, and you may not initiate litigation
(including a cross-claim or counterclaim in a lawsuit) alleging that
any patent claim is infringed by making, using, selling, offering for
sale, or importing the Program or any portion of it.
11. Patents.
A "contributor" is a copyright holder who authorizes use under this
License of the Program or a work on which the Program is based. The
work thus licensed is called the contributor's "contributor version".
A contributor's "essential patent claims" are all patent claims
owned or controlled by the contributor, whether already acquired or
hereafter acquired, that would be infringed by some manner, permitted
by this License, of making, using, or selling its contributor version,
but do not include claims that would be infringed only as a
consequence of further modification of the contributor version. For
purposes of this definition, "control" includes the right to grant
patent sublicenses in a manner consistent with the requirements of
this License.
Each contributor grants you a non-exclusive, worldwide, royalty-free
patent license under the contributor's essential patent claims, to
make, use, sell, offer for sale, import and otherwise run, modify and
propagate the contents of its contributor version.
In the following three paragraphs, a "patent license" is any express
agreement or commitment, however denominated, not to enforce a patent
(such as an express permission to practice a patent or covenant not to
sue for patent infringement). To "grant" such a patent license to a
party means to make such an agreement or commitment not to enforce a
patent against the party.
If you convey a covered work, knowingly relying on a patent license,
and the Corresponding Source of the work is not available for anyone
to copy, free of charge and under the terms of this License, through a
publicly available network server or other readily accessible means,
then you must either (1) cause the Corresponding Source to be so
available, or (2) arrange to deprive yourself of the benefit of the
patent license for this particular work, or (3) arrange, in a manner
consistent with the requirements of this License, to extend the patent
license to downstream recipients. "Knowingly relying" means you have
actual knowledge that, but for the patent license, your conveying the
covered work in a country, or your recipient's use of the covered work
in a country, would infringe one or more identifiable patents in that
country that you have reason to believe are valid.
If, pursuant to or in connection with a single transaction or
arrangement, you convey, or propagate by procuring conveyance of, a
covered work, and grant a patent license to some of the parties
receiving the covered work authorizing them to use, propagate, modify
or convey a specific copy of the covered work, then the patent license
you grant is automatically extended to all recipients of the covered
work and works based on it.
A patent license is "discriminatory" if it does not include within
the scope of its coverage, prohibits the exercise of, or is
conditioned on the non-exercise of one or more of the rights that are
specifically granted under this License. You may not convey a covered
work if you are a party to an arrangement with a third party that is
in the business of distributing software, under which you make payment
to the third party based on the extent of your activity of conveying
the work, and under which the third party grants, to any of the
parties who would receive the covered work from you, a discriminatory
patent license (a) in connection with copies of the covered work
conveyed by you (or copies made from those copies), or (b) primarily
for and in connection with specific products or compilations that
contain the covered work, unless you entered into that arrangement,
or that patent license was granted, prior to 28 March 2007.
Nothing in this License shall be construed as excluding or limiting
any implied license or other defenses to infringement that may
otherwise be available to you under applicable patent law.
12. No Surrender of Others' Freedom.
If conditions are imposed on you (whether by court order, agreement or
otherwise) that contradict the conditions of this License, they do not
excuse you from the conditions of this License. If you cannot convey a
covered work so as to satisfy simultaneously your obligations under this
License and any other pertinent obligations, then as a consequence you may
not convey it at all. For example, if you agree to terms that obligate you
to collect a royalty for further conveying from those to whom you convey
the Program, the only way you could satisfy both those terms and this
License would be to refrain entirely from conveying the Program.
13. Use with the GNU Affero General Public License.
Notwithstanding any other provision of this License, you have
permission to link or combine any covered work with a work licensed
under version 3 of the GNU Affero General Public License into a single
combined work, and to convey the resulting work. The terms of this
License will continue to apply to the part which is the covered work,
but the special requirements of the GNU Affero General Public License,
section 13, concerning interaction through a network will apply to the
combination as such.
14. Revised Versions of this License.
The Free Software Foundation may publish revised and/or new versions of
the GNU General Public License from time to time. Such new versions will
be similar in spirit to the present version, but may differ in detail to
address new problems or concerns.
Each version is given a distinguishing version number. If the
Program specifies that a certain numbered version of the GNU General
Public License "or any later version" applies to it, you have the
option of following the terms and conditions either of that numbered
version or of any later version published by the Free Software
Foundation. If the Program does not specify a version number of the
GNU General Public License, you may choose any version ever published
by the Free Software Foundation.
If the Program specifies that a proxy can decide which future
versions of the GNU General Public License can be used, that proxy's
public statement of acceptance of a version permanently authorizes you
to choose that version for the Program.
Later license versions may give you additional or different
permissions. However, no additional obligations are imposed on any
author or copyright holder as a result of your choosing to follow a
later version.
15. Disclaimer of Warranty.
THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
16. Limitation of Liability.
IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
SUCH DAMAGES.
17. Interpretation of Sections 15 and 16.
If the disclaimer of warranty and limitation of liability provided
above cannot be given local legal effect according to their terms,
reviewing courts shall apply local law that most closely approximates
an absolute waiver of all civil liability in connection with the
Program, unless a warranty or assumption of liability accompanies a
copy of the Program in return for a fee.
END OF TERMS AND CONDITIONS
How to Apply These Terms to Your New Programs
If you develop a new program, and you want it to be of the greatest
possible use to the public, the best way to achieve this is to make it
free software which everyone can redistribute and change under these terms.
To do so, attach the following notices to the program. It is safest
to attach them to the start of each source file to most effectively
state the exclusion of warranty; and each file should have at least
the "copyright" line and a pointer to where the full notice is found.
<one line to give the program's name and a brief idea of what it does.>
Copyright (C) <year> <name of author>
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, see <http://www.gnu.org/licenses/>.
Also add information on how to contact you by electronic and paper mail.
If the program does terminal interaction, make it output a short
notice like this when it starts in an interactive mode:
<program> Copyright (C) <year> <name of author>
This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
This is free software, and you are welcome to redistribute it
under certain conditions; type `show c' for details.
The hypothetical commands `show w' and `show c' should show the appropriate
parts of the General Public License. Of course, your program's commands
might be different; for a GUI interface, you would use an "about box".
You should also get your employer (if you work as a programmer) or school,
if any, to sign a "copyright disclaimer" for the program, if necessary.
For more information on this, and how to apply and follow the GNU GPL, see
<http://www.gnu.org/licenses/>.
The GNU General Public License does not permit incorporating your program
into proprietary programs. If your program is a subroutine library, you
may consider it more useful to permit linking proprietary applications with
the library. If this is what you want to do, use the GNU Lesser General
Public License instead of this License. But first, please read
<http://www.gnu.org/philosophy/why-not-lgpl.html>.

207
flight/Libraries/rscode/rs.c Normal file
View File

@ -0,0 +1,207 @@
/*
* Reed Solomon Encoder/Decoder
*
* Copyright Henry Minsky (hqm@alum.mit.edu) 1991-2009
*
* This software library is licensed under terms of the GNU GENERAL
* PUBLIC LICENSE
*
* RSCODE 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.
*
* RSCODE 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 Rscode. If not, see <http://www.gnu.org/licenses/>.
* Commercial licensing is available under a separate license, please
* contact author for details.
*
* Source code is available at http://rscode.sourceforge.net
*/
#include <stdio.h>
#include <ctype.h>
#include "ecc.h"
/* Encoder parity bytes */
int pBytes[MAXDEG];
/* Decoder syndrome bytes */
int synBytes[MAXDEG];
/* generator polynomial */
int genPoly[MAXDEG*2];
int DEBUG = FALSE;
static void
compute_genpoly (int nbytes, int genpoly[]);
/* Initialize lookup tables, polynomials, etc. */
void
initialize_ecc ()
{
/* Initialize the galois field arithmetic tables */
init_galois_tables();
/* Compute the encoder generator polynomial */
compute_genpoly(RS_ECC_NPARITY, genPoly);
}
void
zero_fill_from (unsigned char buf[], int from, int to)
{
int i;
for (i = from; i < to; i++) buf[i] = 0;
}
/* debugging routines */
void
print_parity (void)
{
#ifdef NEVER
int i;
printf("Parity Bytes: ");
for (i = 0; i < RS_ECC_NPARITY; i++)
printf("[%d]:%x, ",i,pBytes[i]);
printf("\n");
#endif
}
void
print_syndrome (void)
{
#ifdef NEVER
int i;
printf("Syndrome Bytes: ");
for (i = 0; i < RS_ECC_NPARITY; i++)
printf("[%d]:%x, ",i,synBytes[i]);
printf("\n");
#endif
}
/* Append the parity bytes onto the end of the message */
void
build_codeword (unsigned char msg[], int nbytes, unsigned char dst[])
{
int i;
for (i = 0; i < nbytes; i++) dst[i] = msg[i];
for (i = 0; i < RS_ECC_NPARITY; i++) {
dst[i+nbytes] = pBytes[RS_ECC_NPARITY-1-i];
}
}
/**********************************************************
* Reed Solomon Decoder
*
* Computes the syndrome of a codeword. Puts the results
* into the synBytes[] array.
*/
void
decode_data(unsigned char data[], int nbytes)
{
int i, j, sum;
for (j = 0; j < RS_ECC_NPARITY; j++) {
sum = 0;
for (i = 0; i < nbytes; i++) {
sum = data[i] ^ gmult(gexp[j+1], sum);
}
synBytes[j] = sum;
}
}
/* Check if the syndrome is zero */
int
check_syndrome (void)
{
int i, nz = 0;
for (i =0 ; i < RS_ECC_NPARITY; i++) {
if (synBytes[i] != 0) {
nz = 1;
break;
}
}
return nz;
}
void
debug_check_syndrome (void)
{
#ifdef NEVER
int i;
for (i = 0; i < 3; i++) {
printf(" inv log S[%d]/S[%d] = %d\n", i, i+1,
glog[gmult(synBytes[i], ginv(synBytes[i+1]))]);
}
#endif
}
/* Create a generator polynomial for an n byte RS code.
* The coefficients are returned in the genPoly arg.
* Make sure that the genPoly array which is passed in is
* at least n+1 bytes long.
*/
static void
compute_genpoly (int nbytes, int genpoly[])
{
int i, tp[256], tp1[256];
/* multiply (x + a^n) for n = 1 to nbytes */
zero_poly(tp1);
tp1[0] = 1;
for (i = 1; i <= nbytes; i++) {
zero_poly(tp);
tp[0] = gexp[i]; /* set up x+a^n */
tp[1] = 1;
mult_polys(genpoly, tp, tp1);
copy_poly(tp1, genpoly);
}
}
/* Simulate a LFSR with generator polynomial for n byte RS code.
* Pass in a pointer to the data array, and amount of data.
*
* The parity bytes are deposited into pBytes[], and the whole message
* and parity are copied to dest to make a codeword.
*
*/
void
encode_data (unsigned char msg[], int nbytes, unsigned char dst[])
{
int i, LFSR[RS_ECC_NPARITY+1],dbyte, j;
for(i=0; i < RS_ECC_NPARITY+1; i++) LFSR[i]=0;
for (i = 0; i < nbytes; i++) {
dbyte = msg[i] ^ LFSR[RS_ECC_NPARITY-1];
for (j = RS_ECC_NPARITY-1; j > 0; j--) {
LFSR[j] = LFSR[j-1] ^ gmult(genPoly[j], dbyte);
}
LFSR[0] = gmult(genPoly[0], dbyte);
}
for (i = 0; i < RS_ECC_NPARITY; i++)
pBytes[i] = LFSR[i];
build_codeword(msg, nbytes, dst);
}

86
flight/Libraries/rscode/rs.doc Normal file
View File

@ -0,0 +1,86 @@
Introduction to Reed Solomon Codes:
Henry Minsky, Universal Access Inc.
hqm@alum.mit.edu
[For details see Cain, Clark, "Error-Correction Coding For Digital
Communications", pp. 205.] The Reed-Solomon Code is an algebraic code
belonging to the class of BCH (Bose-Chaudry-Hocquehen) multiple burst
correcting cyclic codes. The Reed Solomon code operates on bytes of
fixed length.
Given m parity bytes, a Reed-Solomon code can correct up to m byte
errors in known positions (erasures), or detect and correct up to m/2
byte errors in unknown positions.
This is an implementation of a Reed-Solomon code with 8 bit bytes, and
a configurable number of parity bytes. The maximum sequence length
(codeword) that can be generated is 255 bytes, including parity bytes.
In practice, shorter sequences are used.
ENCODING: The basic principle of encoding is to find the remainder of
the message divided by a generator polynomial G(x). The encoder works
by simulating a Linear Feedback Shift Register with degree equal to
G(x), and feedback taps with the coefficents of the generating
polynomial of the code.
The rs.c file contains an algorithm to generate the encoder polynomial
for any number of bytes of parity, configurable as the NPAR constant
in the file ecc.h.
For this RS code, G(x) = (x-a^1)(x-a^2)(x-a^3)(x-a^4)...(x-a^NPAR)
where 'a' is a primitive element of the Galois Field GF(256) (== 2).
DECODING
The decoder generates four syndrome bytes, which will be all zero if
the message has no errors. If there are errors, the location and value
of the errors can be determined in a number of ways.
Computing the syndromes is easily done as a sum of products, see pp.
179 [Rhee 89].
Fundamentally, the syndome bytes form four simultaneous equations
which can be solved to find the error locations. Once error locations
are known, the syndrome bytes can be used to find the value of the
errors, and they can thus be corrected.
A simplified solution for locating and correcting single errors is
given in Cain and Clark, Ch. 5.
The more general error-location algorithm is the Berlekamp-Massey
algorithm, which will locate up to four errors, by iteratively solving
for the error-locator polynomial. The Modified Berlekamp Massey
algorithm takes as initial conditions any known suspicious bytes
(erasure flags) which you may have (such as might be flagged by
a laser demodulator, or deduced from a failure in a cross-interleaved
block code row or column).
Once the location of errors is known, error correction is done using
the error-evaluator polynomial.
APPLICATION IDEAS
As an example application, this library could be used to implement the
Compact Disc standard of 24 data bytes and 4 parity bytes. A RS code
with 24 data bytes and 4 parity bytes is referred to as a (28,24) RS
code. A (n, k) RS code is said to have efficiency k/n. This first
(28,24) coding is called the C2 or level 2 encoding, because in a
doubly encoded scheme, the codewords are decoded at the second
decoding step.
In following the approach used by Compact Disc digital audio, the 28
byte C2 codewords are four way interleaved and then the interleaved
data is encoded again with a (32,28) RS code. The is the C1 encoding
stage. This produces what is known as a "product code", and has
excellent error correction capability due to the imposition of
two-dimensional structure on the parity checks. The interleave helps
to insure against the case that a multibyte burst error wipes out more
than two bytes in each codeword. The cross-correction capability of
the product code can provide backup if in fact there are more than 2
uncorrectable errors in a block.

21
flight/PipXtreme/inc/watchdog.h → flight/Modules/PipXtreme/inc/pipxtrememod.h
View File

@ -1,9 +1,14 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PipXtremeModule PipXtreme Module
* @{
*
* @file pipxtrememod.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief The PipXtreme system module
*
* @file watchdog.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief RF Module hardware layer
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
@ -22,11 +27,9 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIPXTREMEMOD_H
#define PIPXTREMEMOD_H
#ifndef _WATCHDOG_H_
#define _WATCHDOG_H_
int32_t PipXtremeModInitialize(void);
uint16_t watchdog_Init(uint16_t delayMs);
void watchdog_Clear(void);
#endif
#endif // PIPXTREMEMOD_H

201
flight/Modules/PipXtreme/pipxtrememod.c Normal file
View File

@ -0,0 +1,201 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @brief The OpenPilot Modules do the majority of the control in OpenPilot. The
* @ref PipXtremeModule The PipXtreme Module is the equivalanet of the System
* Module for the PipXtreme modem. it starts all the other modules.
# This is done through the @ref PIOS "PIOS Hardware abstraction layer",
# which then contains hardware specific implementations
* (currently only STM32 supported)
*
* @{
* @addtogroup PipXtremeModule PipXtreme Module
* @brief Initializes PIOS and other modules runs monitoring
* After initializing all the modules runs basic monitoring and
* alarms.
* @{
*
* @file pipxtrememod.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief System module
*
* @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 <pipxstatus.h>
#include <pios_board_info.h>
#include "systemmod.h"
// Private constants
#define SYSTEM_UPDATE_PERIOD_MS 1000
#define LED_BLINK_RATE_HZ 5
#if defined(PIOS_SYSTEM_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE
#else
#define STACK_SIZE_BYTES 924
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY+2)
// Private types
// Private variables
static uint32_t idleCounter;
static uint32_t idleCounterClear;
static xTaskHandle systemTaskHandle;
static bool stackOverflow;
static bool mallocFailed;
// Private functions
static void systemTask(void *parameters);
/**
* Create the module task.
* \returns 0 on success or -1 if initialization failed
*/
int32_t PipXtremeModStart(void)
{
// Initialize vars
stackOverflow = false;
mallocFailed = false;
// Create pipxtreme system task
xTaskCreate(systemTask, (signed char *)"PipXtreme", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &systemTaskHandle);
// Register task
TaskMonitorAdd(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
return 0;
}
/**
* Initialize the module, called on startup.
* \returns 0 on success or -1 if initialization failed
*/
int32_t PipXtremeModInitialize(void)
{
// Must registers objects here for system thread because ObjectManager started in OpenPilotInit
// Initialize out status object.
PipXStatusInitialize();
PipXStatusData pipxStatus;
PipXStatusGet(&pipxStatus);
// Get our hardware information.
const struct pios_board_info * bdinfo = &pios_board_info_blob;
pipxStatus.BoardType= bdinfo->board_type;
PIOS_BL_HELPER_FLASH_Read_Description(pipxStatus.Description, PIPXSTATUS_DESCRIPTION_NUMELEM);
PIOS_SYS_SerialNumberGetBinary(pipxStatus.CPUSerial);
pipxStatus.BoardRevision= bdinfo->board_rev;
// Update the object
PipXStatusSet(&pipxStatus);
// Call the module start function.
PipXtremeModStart();
return 0;
}
MODULE_INITCALL(PipXtremeModInitialize, 0)
/**
* System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
*/
static void systemTask(void *parameters)
{
portTickType lastSysTime;
/* create all modules thread */
MODULE_TASKCREATE_ALL;
if (mallocFailed) {
/* We failed to malloc during task creation,
* system behaviour is undefined. Reset and let
* the BootFault code recover for us.
*/
PIOS_SYS_Reset();
}
// Initialize vars
idleCounter = 0;
idleCounterClear = 0;
lastSysTime = xTaskGetTickCount();
// Main system loop
while (1) {
// Flash the heartbeat LED
#if defined(PIOS_LED_HEARTBEAT)
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
#endif /* PIOS_LED_HEARTBEAT */
// Wait until next period
vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS);
}
}
/**
* Called by the RTOS when the CPU is idle, used to measure the CPU idle time.
*/
void vApplicationIdleHook(void)
{
// Called when the scheduler has no tasks to run
if (idleCounterClear == 0) {
++idleCounter;
} else {
idleCounter = 0;
idleCounterClear = 0;
}
}
/**
* Called by the RTOS when a stack overflow is detected.
*/
#define DEBUG_STACK_OVERFLOW 0
void vApplicationStackOverflowHook(xTaskHandle * pxTask, signed portCHAR * pcTaskName)
{
stackOverflow = true;
#if DEBUG_STACK_OVERFLOW
static volatile bool wait_here = true;
while(wait_here);
wait_here = true;
#endif
}
/**
* Called by the RTOS when a malloc call fails.
*/
#define DEBUG_MALLOC_FAILURES 0
void vApplicationMallocFailedHook(void)
{
mallocFailed = true;
#if DEBUG_MALLOC_FAILURES
static volatile bool wait_here = true;
while(wait_here);
wait_here = true;
#endif
}
/**
* @}
* @}
*/

1069
flight/Modules/RadioComBridge/RadioComBridge.c Normal file
View File

@ -0,0 +1,1069 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module
* @brief Bridge Com and Radio ports
* @{
*
* @file RadioComBridge.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Bridges selected Com Port to the COM VCP emulated serial port
* @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 <radiocombridge.h>
#include <packet_handler.h>
#include <gcsreceiver.h>
#include <pipxstatus.h>
#include <objectpersistence.h>
#include <pipxsettings.h>
#include <uavtalk_priv.h>
#include <pios_rfm22b.h>
#include <ecc.h>
#if defined(PIOS_INCLUDE_FLASH_EEPROM)
#include <pios_eeprom.h>
#endif
#include <stdbool.h>
// ****************
// Private constants
#define TEMP_BUFFER_SIZE 25
#define STACK_SIZE_BYTES 150
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define BRIDGE_BUF_LEN 512
#define MAX_RETRIES 2
#define RETRY_TIMEOUT_MS 20
#define STATS_UPDATE_PERIOD_MS 2000
#define RADIOSTATS_UPDATE_PERIOD_MS 1000
#define MAX_LOST_CONTACT_TIME 4
#define PACKET_QUEUE_SIZE 10
#define MAX_PORT_DELAY 200
#define EV_PACKET_RECEIVED 0x20
#define EV_SEND_ACK 0x40
#define EV_SEND_NACK 0x80
// ****************
// Private types
typedef struct {
uint32_t pairID;
uint16_t retries;
uint16_t errors;
uint16_t uavtalk_errors;
uint16_t resets;
int8_t rssi;
uint8_t lastContact;
} PairStats;
typedef struct {
// The task handles.
xTaskHandle comUAVTalkTaskHandle;
xTaskHandle radioReceiveTaskHandle;
xTaskHandle sendPacketTaskHandle;
xTaskHandle sendDataTaskHandle;
xTaskHandle radioStatusTaskHandle;
xTaskHandle transparentCommTaskHandle;
xTaskHandle ppmInputTaskHandle;
// The UAVTalk connection on the com side.
UAVTalkConnection inUAVTalkCon;
UAVTalkConnection outUAVTalkCon;
// Queue handles.
xQueueHandle sendPacketQueue;
xQueueHandle objEventQueue;
// Error statistics.
uint32_t comTxErrors;
uint32_t comTxRetries;
uint32_t comRxErrors;
uint32_t radioTxErrors;
uint32_t radioTxRetries;
uint32_t radioRxErrors;
uint32_t UAVTalkErrors;
uint32_t packetErrors;
uint16_t txBytes;
uint16_t rxBytes;
// The destination ID
uint32_t destination_id;
// The packet timeout.
portTickType send_timeout;
uint16_t min_packet_size;
// Track other radios that are in range.
PairStats pairStats[PIPXSTATUS_PAIRIDS_NUMELEM];
} RadioComBridgeData;
typedef struct {
uint32_t com_port;
uint8_t *buffer;
uint16_t length;
uint16_t index;
uint16_t data_length;
} ReadBuffer, *BufferedReadHandle;
// ****************
// Private functions
static void comUAVTalkTask(void *parameters);
static void radioReceiveTask(void *parameters);
static void sendPacketTask(void *parameters);
static void sendDataTask(void *parameters);
static void transparentCommTask(void * parameters);
static void radioStatusTask(void *parameters);
static void ppmInputTask(void *parameters);
static int32_t transmitData(uint8_t * data, int32_t length);
static int32_t transmitPacket(PHPacketHandle packet);
static void receiveData(uint8_t *buf, uint8_t len);
static void StatusHandler(PHStatusPacketHandle p);
static void PPMHandler(uint16_t *channels);
static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length);
static bool BufferedRead(BufferedReadHandle h, uint8_t *value, uint32_t timeout_ms);
static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port);
static void updateSettings();
// ****************
// Private variables
static RadioComBridgeData *data;
/**
* Start the module
* \return -1 if initialisation failed
* \return 0 on success
*/
static int32_t RadioComBridgeStart(void)
{
if(data) {
// Start the tasks
xTaskCreate(comUAVTalkTask, (signed char *)"ComUAVTalk", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->comUAVTalkTaskHandle));
if(PIOS_COM_TRANS_COM)
xTaskCreate(transparentCommTask, (signed char *)"transparentComm", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->transparentCommTaskHandle));
xTaskCreate(radioReceiveTask, (signed char *)"RadioReceive", STACK_SIZE_BYTES, NULL, TASK_PRIORITY+ 2, &(data->radioReceiveTaskHandle));
xTaskCreate(sendPacketTask, (signed char *)"SendPacketTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->sendPacketTaskHandle));
xTaskCreate(sendDataTask, (signed char *)"SendDataTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY+ 2, &(data->sendDataTaskHandle));
xTaskCreate(radioStatusTask, (signed char *)"RadioStatus", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioStatusTaskHandle));
if(PIOS_PPM_RECEIVER)
xTaskCreate(ppmInputTask, (signed char *)"PPMInputTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->ppmInputTaskHandle));
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_COMUAVTALK);
if(PIOS_COM_TRANS_COM)
PIOS_WDG_RegisterFlag(PIOS_WDG_TRANSCOMM);
PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORECEIVE);
PIOS_WDG_RegisterFlag(PIOS_WDG_SENDPACKET);
//PIOS_WDG_RegisterFlag(PIOS_WDG_SENDDATA);
if(PIOS_PPM_RECEIVER)
PIOS_WDG_RegisterFlag(PIOS_WDG_PPMINPUT);
#endif
return 0;
}
return -1;
}
/**
* Initialise the module
* \return -1 if initialisation failed
* \return 0 on success
*/
static int32_t RadioComBridgeInitialize(void)
{
// allocate and initialize the static data storage only if module is enabled
data = (RadioComBridgeData *)pvPortMalloc(sizeof(RadioComBridgeData));
if (!data)
return -1;
// Initialize the UAVObjects that we use
GCSReceiverInitialize();
PipXStatusInitialize();
ObjectPersistenceInitialize();
updateSettings();
// Initialise UAVTalk
data->inUAVTalkCon = UAVTalkInitialize(0);
data->outUAVTalkCon = UAVTalkInitialize(&transmitData);
// Initialize the queues.
data->sendPacketQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(PHPacketHandle));
data->objEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent));
// Initialize the statistics.
data->radioTxErrors = 0;
data->radioTxRetries = 0;
data->radioRxErrors = 0;
data->comTxErrors = 0;
data->comTxRetries = 0;
data->comRxErrors = 0;
data->UAVTalkErrors = 0;
data->packetErrors = 0;
// Register the callbacks with the packet handler
PHRegisterOutputStream(pios_packet_handler, transmitPacket);
PHRegisterDataHandler(pios_packet_handler, receiveData);
PHRegisterStatusHandler(pios_packet_handler, StatusHandler);
PHRegisterPPMHandler(pios_packet_handler, PPMHandler);
// Initialize the packet send timeout
data->send_timeout = 25; // ms
data->min_packet_size = 50;
// Initialize the detected device statistics.
for (uint8_t i = 0; i < PIPXSTATUS_PAIRIDS_NUMELEM; ++i)
{
data->pairStats[i].pairID = 0;
data->pairStats[i].rssi = -127;
data->pairStats[i].retries = 0;
data->pairStats[i].errors = 0;
data->pairStats[i].uavtalk_errors = 0;
data->pairStats[i].resets = 0;
data->pairStats[i].lastContact = 0;
}
// The first slot is reserved for our current pairID
PipXSettingsPairIDGet(&(data->pairStats[0].pairID));
// Configure our UAVObjects for updates.
UAVObjConnectQueue(UAVObjGetByName("PipXStatus"), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
UAVObjConnectQueue(UAVObjGetByName("GCSReceiver"), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
UAVObjConnectQueue(UAVObjGetByName("ObjectPersistence"), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL);
return 0;
}
MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart)
/**
* Reads UAVTalk messages froma com port and creates packets out of them.
*/
static void comUAVTalkTask(void *parameters)
{
PHPacketHandle p = NULL;
// Create the buffered reader.
BufferedReadHandle f = BufferedReadInit(PIOS_COM_UAVTALK, TEMP_BUFFER_SIZE);
while (1) {
#ifdef PIOS_INCLUDE_WDG
// Update the watchdog timer.
PIOS_WDG_UpdateFlag(PIOS_WDG_COMUAVTALK);
#endif /* PIOS_INCLUDE_WDG */
// Receive from USB HID if available, otherwise UAVTalk com if it's available.
#if defined(PIOS_INCLUDE_USB)
// Determine input port (USB takes priority over telemetry port)
if (PIOS_USB_CheckAvailable(0))
BufferedReadSetCom(f, PIOS_COM_USB_HID);
else
#endif /* PIOS_INCLUDE_USB */
{
if (PIOS_COM_UAVTALK)
BufferedReadSetCom(f, PIOS_COM_UAVTALK);
else
{
vTaskDelay(5);
continue;
}
}
// Read the next byte
uint8_t rx_byte;
if(!BufferedRead(f, &rx_byte, MAX_PORT_DELAY))
continue;
data->txBytes++;
// Get a TX packet from the packet handler if required.
if (p == NULL)
{
// Wait until we receive a sync.
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(data->inUAVTalkCon, rx_byte);
if (state != UAVTALK_STATE_TYPE)
continue;
// Get a packet when we see the sync
p = PHGetTXPacket(pios_packet_handler);
// No packets available?
if (p == NULL)
{
DEBUG_PRINTF(2, "Packet dropped!\n\r");
continue;
}
// Initialize the packet.
p->header.destination_id = data->destination_id;
p->header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
//p->header.type = PACKET_TYPE_ACKED_DATA;
p->header.type = PACKET_TYPE_DATA;
p->data[0] = rx_byte;
p->header.data_size = 1;
continue;
}
// Insert this byte.
p->data[p->header.data_size++] = rx_byte;
// Keep reading until we receive a completed packet.
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(data->inUAVTalkCon, rx_byte);
UAVTalkConnectionData *connection = (UAVTalkConnectionData*)(data->inUAVTalkCon);
UAVTalkInputProcessor *iproc = &(connection->iproc);
if (state == UAVTALK_STATE_COMPLETE)
{
// Is this a local UAVObject?
// We only generate GcsReceiver ojects, we don't consume them.
if ((iproc->obj != NULL) && (iproc->objId != GCSRECEIVER_OBJID))
{
// We treat the ObjectPersistence object differently
if(iproc->objId == OBJECTPERSISTENCE_OBJID)
{
// Unpack object, if the instance does not exist it will be created!
UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer);
// Get the ObjectPersistence object.
ObjectPersistenceData obj_per;
ObjectPersistenceGet(&obj_per);
// Is this concerning or setting object?
if (obj_per.ObjectID == PIPXSETTINGS_OBJID)
{
// Queue up the ACK.
UAVObjEvent ev;
ev.obj = iproc->obj;
ev.instId = iproc->instId;
ev.event = EV_SEND_ACK;
xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY);
// Is this a save, load, or delete?
bool success = true;
switch (obj_per.Operation)
{
case OBJECTPERSISTENCE_OPERATION_LOAD:
{
#if defined(PIOS_INCLUDE_FLASH_EEPROM)
// Load the settings.
PipXSettingsData pipxSettings;
if (PIOS_EEPROM_Load((uint8_t*)&pipxSettings, sizeof(PipXSettingsData)) == 0)
PipXSettingsSet(&pipxSettings);
else
success = false;
#endif
break;
}
case OBJECTPERSISTENCE_OPERATION_SAVE:
{
#if defined(PIOS_INCLUDE_FLASH_EEPROM)
// Save the settings.
PipXSettingsData pipxSettings;
PipXSettingsGet(&pipxSettings);
int32_t ret = PIOS_EEPROM_Save((uint8_t*)&pipxSettings, sizeof(PipXSettingsData));
if (ret != 0)
success = false;
#endif
break;
}
default:
break;
}
if (success == true)
{
obj_per.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED;
ObjectPersistenceSet(&obj_per);
}
// Release the packet, since we don't need it.
PHReleaseTXPacket(pios_packet_handler, p);
}
else
{
// Otherwise, queue the packet for transmission.
xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY);
}
}
else
{
UAVObjEvent ev;
ev.obj = iproc->obj;
ev.instId = 0;
switch (iproc->type)
{
case UAVTALK_TYPE_OBJ:
// Unpack object, if the instance does not exist it will be created!
UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer);
break;
case UAVTALK_TYPE_OBJ_REQ:
// Queue up an object send request.
ev.event = EV_UPDATE_REQ;
xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY);
break;
case UAVTALK_TYPE_OBJ_ACK:
if (UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer) == 0)
{
// Queue up an ACK
ev.event = EV_SEND_ACK;
xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY);
}
break;
}
// Release the packet, since we don't need it.
PHReleaseTXPacket(pios_packet_handler, p);
}
}
else
{
// Queue the packet for transmission.
xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY);
}
p = NULL;
} else if(state == UAVTALK_STATE_ERROR) {
DEBUG_PRINTF(1, "UAVTalk FAILED!\n\r");
data->UAVTalkErrors++;
// Send a NACK if required.
if((iproc->obj) && (iproc->type == UAVTALK_TYPE_OBJ_ACK))
{
// Queue up a NACK
UAVObjEvent ev;
ev.obj = iproc->obj;
ev.event = EV_SEND_NACK;
xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY);
// Release the packet and start over again.
PHReleaseTXPacket(pios_packet_handler, p);
}
else
{
// Transmit the packet anyway...
xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY);
}
p = NULL;
}
}
}
/**
* The radio to com bridge task.
*/
static void radioReceiveTask(void *parameters)
{
PHPacketHandle p = NULL;
/* Handle radio -> usart/usb direction */
while (1) {
uint32_t rx_bytes;
#ifdef PIOS_INCLUDE_WDG
// Update the watchdog timer.
PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORECEIVE);
#endif /* PIOS_INCLUDE_WDG */
// Get a RX packet from the packet handler if required.
if (p == NULL)
p = PHGetRXPacket(pios_packet_handler);
if(p == NULL) {
DEBUG_PRINTF(2, "RX Packet Unavailable.!\n\r");
// Wait a bit for a packet to come available.
vTaskDelay(5);
continue;
}
// Receive data from the radio port
rx_bytes = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, (uint8_t*)p, PIOS_PH_MAX_PACKET, MAX_PORT_DELAY);
if(rx_bytes == 0)
continue;
data->rxBytes += rx_bytes;
// Verify that the packet is valid and pass it on.
if(PHVerifyPacket(pios_packet_handler, p, rx_bytes) > 0) {
UAVObjEvent ev;
ev.obj = (UAVObjHandle)p;
ev.event = EV_PACKET_RECEIVED;
xQueueSend(data->objEventQueue, &ev, portMAX_DELAY);
} else
{
data->packetErrors++;
PHReceivePacket(pios_packet_handler, p, false);
}
p = NULL;
}
}
/**
* Send packets to the radio.
*/
static void sendPacketTask(void *parameters)
{
PHPacketHandle p;
// Loop forever
while (1) {
#ifdef PIOS_INCLUDE_WDG
// Update the watchdog timer.
PIOS_WDG_UpdateFlag(PIOS_WDG_SENDPACKET);
#endif /* PIOS_INCLUDE_WDG */
// Wait for a packet on the queue.
if (xQueueReceive(data->sendPacketQueue, &p, MAX_PORT_DELAY) == pdTRUE) {
// Send the packet.
if(!PHTransmitPacket(pios_packet_handler, p))
PHReleaseTXPacket(pios_packet_handler, p);
}
}
}
/**
* Send packets to the radio.
*/
static void sendDataTask(void *parameters)
{
UAVObjEvent ev;
// Loop forever
while (1) {
#ifdef PIOS_INCLUDE_WDG
// Update the watchdog timer.
// NOTE: this is temporarily turned off becase PIOS_Com_SendBuffer appears to block for an uncontrollable time,
// and SendBufferNonBlocking doesn't seem to be working in this case.
//PIOS_WDG_UpdateFlag(PIOS_WDG_SENDDATA);
#endif /* PIOS_INCLUDE_WDG */
// Wait for a packet on the queue.
if (xQueueReceive(data->objEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ))
{
// Send update (with retries)
uint32_t retries = 0;
int32_t success = -1;
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObject(data->outUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS);
++retries;
}
data->comTxRetries += retries;
}
else if(ev.event == EV_SEND_ACK)
{
// Send the ACK
uint32_t retries = 0;
int32_t success = -1;
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendAck(data->outUAVTalkCon, ev.obj, ev.instId);
++retries;
}
data->comTxRetries += retries;
}
else if(ev.event == EV_SEND_NACK)
{
// Send the NACK
uint32_t retries = 0;
int32_t success = -1;
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendNack(data->outUAVTalkCon, UAVObjGetID(ev.obj));
++retries;
}
data->comTxRetries += retries;
}
else if(ev.event == EV_PACKET_RECEIVED)
{
// Receive the packet.
PHReceivePacket(pios_packet_handler, (PHPacketHandle)ev.obj, false);
}
}
}
}
/**
* The com to radio bridge task.
*/
static void transparentCommTask(void * parameters)
{
portTickType packet_start_time = 0;
uint32_t timeout = MAX_PORT_DELAY;
PHPacketHandle p = NULL;
/* Handle usart/usb -> radio direction */
while (1) {
#ifdef PIOS_INCLUDE_WDG
// Update the watchdog timer.
PIOS_WDG_UpdateFlag(PIOS_WDG_TRANSCOMM);
#endif /* PIOS_INCLUDE_WDG */
// Get a TX packet from the packet handler if required.
if (p == NULL)
{
p = PHGetTXPacket(pios_packet_handler);
// No packets available?
if (p == NULL)
{
// Wait a bit for a packet to come available.
vTaskDelay(5);
continue;
}
// Initialize the packet.
p->header.destination_id = data->destination_id;
p->header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
//p->header.type = PACKET_TYPE_ACKED_DATA;
p->header.type = PACKET_TYPE_DATA;
p->header.data_size = 0;
}
// Receive data from the com port
uint32_t cur_rx_bytes = PIOS_COM_ReceiveBuffer(PIOS_COM_TRANS_COM, p->data + p->header.data_size,
PH_MAX_DATA - p->header.data_size, timeout);
// Do we have an data to send?
p->header.data_size += cur_rx_bytes;
if (p->header.data_size > 0) {
// Check how long since last update
portTickType cur_sys_time = xTaskGetTickCount();
// Is this the start of a packet?
if(packet_start_time == 0)
packet_start_time = cur_sys_time;
// Just send the packet on wraparound
bool send_packet = (cur_sys_time < packet_start_time);
if (!send_packet)
{
portTickType dT = (cur_sys_time - packet_start_time) / portTICK_RATE_MS;
if (dT > data->send_timeout)
send_packet = true;
else
timeout = data->send_timeout - dT;
}
// Also send the packet if the size is over the minimum.
send_packet |= (p->header.data_size > data->min_packet_size);
// Should we send this packet?
if (send_packet)
{
// Queue the packet for transmission.
xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY);
// Reset the timeout
timeout = MAX_PORT_DELAY;
p = NULL;
packet_start_time = 0;
}
}
}
}
/**
* The stats update task.
*/
static void radioStatusTask(void *parameters)
{
PHStatusPacket status_packet;
while (1) {
PipXStatusData pipxStatus;
uint32_t pairID;
// Get object data
PipXStatusGet(&pipxStatus);
PipXSettingsPairIDGet(&pairID);
// Update the status
pipxStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
pipxStatus.RSSI = PIOS_RFM22B_RSSI(pios_rfm22b_id);
pipxStatus.Retries = data->comTxRetries;
pipxStatus.Errors = data->packetErrors;
pipxStatus.UAVTalkErrors = data->UAVTalkErrors;
pipxStatus.Resets = PIOS_RFM22B_Resets(pios_rfm22b_id);
pipxStatus.TXRate = (uint16_t)((float)(data->txBytes * 1000) / STATS_UPDATE_PERIOD_MS);
data->txBytes = 0;
pipxStatus.RXRate = (uint16_t)((float)(data->rxBytes * 1000) / STATS_UPDATE_PERIOD_MS);
data->rxBytes = 0;
pipxStatus.LinkState = PIPXSTATUS_LINKSTATE_DISCONNECTED;
// Update the potential pairing contacts
for (uint8_t i = 0; i < PIPXSTATUS_PAIRIDS_NUMELEM; ++i)
{
pipxStatus.PairIDs[i] = data->pairStats[i].pairID;
pipxStatus.PairSignalStrengths[i] = data->pairStats[i].rssi;
data->pairStats[i].lastContact++;
// Add the paired devices stats to ours.
if(data->pairStats[i].pairID == pairID)
{
pipxStatus.Retries += data->pairStats[i].retries;
pipxStatus.Errors += data->pairStats[i].errors;
pipxStatus.UAVTalkErrors += data->pairStats[i].uavtalk_errors;
pipxStatus.Resets += data->pairStats[i].resets;
pipxStatus.LinkState = PIPXSTATUS_LINKSTATE_CONNECTED;
}
}
// Update the object
PipXStatusSet(&pipxStatus);
// Broadcast the status.
{
static uint16_t cntr = 0;
if(cntr++ == RADIOSTATS_UPDATE_PERIOD_MS / STATS_UPDATE_PERIOD_MS)
{
// Queue the status message
status_packet.header.destination_id = 0xffffffff;
status_packet.header.type = PACKET_TYPE_STATUS;
status_packet.header.data_size = PH_STATUS_DATA_SIZE(&status_packet);
status_packet.header.source_id = pipxStatus.DeviceID;
status_packet.header.rssi = pipxStatus.RSSI;
status_packet.retries = data->comTxRetries;
status_packet.errors = data->packetErrors;
status_packet.uavtalk_errors = data->UAVTalkErrors;
status_packet.resets = PIOS_RFM22B_Resets(pios_rfm22b_id);
PHPacketHandle sph = (PHPacketHandle)&status_packet;
xQueueSend(data->sendPacketQueue, &sph, MAX_PORT_DELAY);
cntr = 0;
}
}
// Delay until the next update period.
vTaskDelay(STATS_UPDATE_PERIOD_MS / portTICK_RATE_MS);
}
}
/**
* The PPM input task.
*/
static void ppmInputTask(void *parameters)
{
PHPpmPacket ppm_packet;
PHPacketHandle pph = (PHPacketHandle)&ppm_packet;
while (1) {
#ifdef PIOS_INCLUDE_WDG
// Update the watchdog timer.
PIOS_WDG_UpdateFlag(PIOS_WDG_PPMINPUT);
#endif /* PIOS_INCLUDE_WDG */
// Send the PPM packet
for (uint8_t i = 1; i <= PIOS_PPM_NUM_INPUTS; ++i)
ppm_packet.channels[i - 1] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i);
// Send the packet.
ppm_packet.header.destination_id = data->destination_id;
ppm_packet.header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
ppm_packet.header.type = PACKET_TYPE_PPM;
ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&ppm_packet);
xQueueSend(data->sendPacketQueue, &pph, MAX_PORT_DELAY);
// Delay until the next update period.
vTaskDelay(PIOS_PPM_PACKET_UPDATE_PERIOD_MS / portTICK_RATE_MS);
}
}
/**
* Transmit data buffer to the com port.
* \param[in] buf Data buffer to send
* \param[in] length Length of buffer
* \return -1 on failure
* \return number of bytes transmitted on success
*/
static int32_t transmitData(uint8_t *buf, int32_t length)
{
uint32_t outputPort = PIOS_COM_UAVTALK;
#if defined(PIOS_INCLUDE_USB)
// Determine output port (USB takes priority over telemetry port)
if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID)
outputPort = PIOS_COM_USB_HID;
#endif /* PIOS_INCLUDE_USB */
if(outputPort)
return PIOS_COM_SendBuffer(outputPort, buf, length);
else
return -1;
}
/**
* Transmit a packet to the radio port.
* \param[in] buf Data buffer to send
* \param[in] length Length of buffer
* \return -1 on failure
* \return number of bytes transmitted on success
*/
static int32_t transmitPacket(PHPacketHandle p)
{
return PIOS_COM_SendBuffer(PIOS_COM_RADIO, (uint8_t*)p, PH_PACKET_SIZE(p));
}
/**
* Receive a packet
* \param[in] buf The received data buffer
* \param[in] length Length of buffer
*/
static void receiveData(uint8_t *buf, uint8_t len)
{
// Packet data should go to transparent com if it's configured,
// USB HID if it's connected, otherwise, UAVTalk com if it's configured.
uint32_t outputPort = PIOS_COM_TRANS_COM;
if (!outputPort)
{
outputPort = PIOS_COM_UAVTALK;
#if defined(PIOS_INCLUDE_USB)
// Determine output port (USB takes priority over telemetry port)
if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID)
outputPort = PIOS_COM_USB_HID;
#endif /* PIOS_INCLUDE_USB */
}
if (!outputPort)
return;
// Send the received data to the com port
if (PIOS_COM_SendBuffer(outputPort, buf, len) != len)
// Error on transmit
data->comTxErrors++;
}
/**
* Receive a status packet
* \param[in] status The status structure
*/
static void StatusHandler(PHStatusPacketHandle status)
{
uint32_t id = status->header.source_id;
bool found = false;
// Have we seen this device recently?
uint8_t id_idx = 0;
for ( ; id_idx < PIPXSTATUS_PAIRIDS_NUMELEM; ++id_idx)
if(data->pairStats[id_idx].pairID == id)
{
found = true;
break;
}
// If we have seen it, update the RSSI and reset the last contact couter
if(found)
{
data->pairStats[id_idx].rssi = status->header.rssi;
data->pairStats[id_idx].retries = status->retries;
data->pairStats[id_idx].errors = status->errors;
data->pairStats[id_idx].uavtalk_errors = status->uavtalk_errors;
data->pairStats[id_idx].resets = status->resets;
data->pairStats[id_idx].lastContact = 0;
}
// Remove any contacts that we haven't seen for a while.
for (id_idx = 0; id_idx < PIPXSTATUS_PAIRIDS_NUMELEM; ++id_idx)
{
if(data->pairStats[id_idx].lastContact > MAX_LOST_CONTACT_TIME)
{
data->pairStats[id_idx].pairID = 0;
data->pairStats[id_idx].rssi = -127;
data->pairStats[id_idx].retries = 0;
data->pairStats[id_idx].errors = 0;
data->pairStats[id_idx].uavtalk_errors = 0;
data->pairStats[id_idx].resets = 0;
data->pairStats[id_idx].lastContact = 0;
}
}
// If we haven't seen it, find a slot to put it in.
if (!found)
{
uint32_t pairID;
PipXSettingsPairIDGet(&pairID);
uint8_t min_idx = 0;
if(id != pairID)
{
int8_t min_rssi = data->pairStats[0].rssi;
for (id_idx = 1; id_idx < PIPXSTATUS_PAIRIDS_NUMELEM; ++id_idx)
{
if(data->pairStats[id_idx].rssi < min_rssi)
{
min_rssi = data->pairStats[id_idx].rssi;
min_idx = id_idx;
}
}
}
data->pairStats[min_idx].pairID = id;
data->pairStats[min_idx].rssi = status->header.rssi;
data->pairStats[min_idx].retries = status->retries;
data->pairStats[min_idx].errors = status->errors;
data->pairStats[min_idx].uavtalk_errors = status->uavtalk_errors;
data->pairStats[min_idx].resets = status->resets;
data->pairStats[min_idx].lastContact = 0;
}
}
/**
* Receive a ppm packet
* \param[in] channels The ppm channels
*/
static void PPMHandler(uint16_t *channels)
{
GCSReceiverData rcvr;
// Copy the receiver channels into the GCSReceiver object.
for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; ++i)
rcvr.Channel[i] = channels[i];
// Set the GCSReceiverData object.
GCSReceiverSet(&rcvr);
}
static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length)
{
BufferedReadHandle h = (BufferedReadHandle)pvPortMalloc(sizeof(ReadBuffer));
if (!h)
return NULL;
h->com_port = com_port;
h->buffer = (uint8_t*)pvPortMalloc(buffer_length);
h->length = buffer_length;
h->index = 0;
h->data_length = 0;
if (h->buffer == NULL)
return NULL;
return h;
}
static bool BufferedRead(BufferedReadHandle h, uint8_t *value, uint32_t timeout_ms)
{
// Read some data if required.
if(h->index == h->data_length)
{
uint32_t rx_bytes = PIOS_COM_ReceiveBuffer(h->com_port, h->buffer, h->length, timeout_ms);
if (rx_bytes == 0)
return false;
h->index = 0;
h->data_length = rx_bytes;
}
// Return the next byte.
*value = h->buffer[h->index++];
return true;
}
static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port)
{
h->com_port = com_port;
}
/**
* Update the telemetry settings, called on startup.
* FIXME: This should be in the TelemetrySettings object. But objects
* have too much overhead yet. Also the telemetry has no any specific
* settings, etc. Thus the HwSettings object which contains the
* telemetry port speed is used for now.
*/
static void updateSettings()
{
// Get the settings.
PipXSettingsData pipxSettings;
PipXSettingsGet(&pipxSettings);
// Initialize the destination ID
data->destination_id = pipxSettings.PairID ? pipxSettings.PairID : 0xffffffff;
DEBUG_PRINTF(2, "PairID: %x\n\r", data->destination_id);
if (PIOS_COM_TELEMETRY) {
switch (pipxSettings.TelemetrySpeed) {
case PIPXSETTINGS_TELEMETRYSPEED_2400:
PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 2400);
break;
case PIPXSETTINGS_TELEMETRYSPEED_4800:
PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 4800);
break;
case PIPXSETTINGS_TELEMETRYSPEED_9600:
PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 9600);
break;
case PIPXSETTINGS_TELEMETRYSPEED_19200:
PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 19200);
break;
case PIPXSETTINGS_TELEMETRYSPEED_38400:
PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 38400);
break;
case PIPXSETTINGS_TELEMETRYSPEED_57600:
PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 57600);
break;
case PIPXSETTINGS_TELEMETRYSPEED_115200:
PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 115200);
break;
}
}
if (PIOS_COM_FLEXI) {
switch (pipxSettings.FlexiSpeed) {
case PIPXSETTINGS_TELEMETRYSPEED_2400:
PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 2400);
break;
case PIPXSETTINGS_TELEMETRYSPEED_4800:
PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 4800);
break;
case PIPXSETTINGS_TELEMETRYSPEED_9600:
PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 9600);
break;
case PIPXSETTINGS_TELEMETRYSPEED_19200:
PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 19200);
break;
case PIPXSETTINGS_TELEMETRYSPEED_38400:
PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 38400);
break;
case PIPXSETTINGS_TELEMETRYSPEED_57600:
PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 57600);
break;
case PIPXSETTINGS_TELEMETRYSPEED_115200:
PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 115200);
break;
}
}
if (PIOS_COM_VCP) {
switch (pipxSettings.VCPSpeed) {
case PIPXSETTINGS_TELEMETRYSPEED_2400:
PIOS_COM_ChangeBaud(PIOS_COM_VCP, 2400);
break;
case PIPXSETTINGS_TELEMETRYSPEED_4800:
PIOS_COM_ChangeBaud(PIOS_COM_VCP, 4800);
break;
case PIPXSETTINGS_TELEMETRYSPEED_9600:
PIOS_COM_ChangeBaud(PIOS_COM_VCP, 9600);
break;
case PIPXSETTINGS_TELEMETRYSPEED_19200:
PIOS_COM_ChangeBaud(PIOS_COM_VCP, 19200);
break;
case PIPXSETTINGS_TELEMETRYSPEED_38400:
PIOS_COM_ChangeBaud(PIOS_COM_VCP, 38400);
break;
case PIPXSETTINGS_TELEMETRYSPEED_57600:
PIOS_COM_ChangeBaud(PIOS_COM_VCP, 57600);
break;
case PIPXSETTINGS_TELEMETRYSPEED_115200:
PIOS_COM_ChangeBaud(PIOS_COM_VCP, 115200);
break;
}
}
}

80
flight/PipXtreme/inc/gpio_in.h → flight/Modules/RadioComBridge/inc/radiocombridge.h
View File

@ -1,41 +1,39 @@
/**
******************************************************************************
*
* @file gpio_in.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief GPIO 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 __GPIO_IN_H__
#define __GPIO_IN_H__
// *****************************************************************
void GPIO_IN_Init(void);
bool GPIO_IN(uint8_t num);
// *****************************************************************
#endif
/**
* @}
* @}
*/
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module
* @brief Bridge Com and Radio ports
* @{
*
* @file radiocombridge.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Include file of the telemetry module.
* @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 RADIOCOMBRIDGE_H
#define RADIOCOMBRIDGE_H
#endif // RADIOCOMBRIDGE_H
/**
* @}
* @}
*/

20
flight/Modules/Stabilization/stabilization.c
View File

@ -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;
}

10
flight/PiOS.posix/Boards/pios_board.h Normal file
View File

@ -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_ */

263
flight/PiOS.posix/Boards/sim_posix.h Normal file
View File

@ -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_ */
/**
* @}
* @}
*/

85
flight/PiOS.posix/inc/pios_delay.h
View File

@ -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 */
/**
* @}
* @}
*/

82
flight/PiOS.posix/inc/pios_led.h
View File

@ -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 */

6
flight/PiOS.posix/inc/pios_posix.h
View File

@ -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

6
flight/PiOS.posix/inc/pios_udp_priv.h
View File

@ -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 */

4
flight/PiOS.posix/pios.h
View File

@ -29,7 +29,7 @@
#define PIOS_H
/* PIOS Feature Selection */
#include "pios_config_posix.h"
#include "pios_config.h"
#include <pios_posix.h>
#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 <pios_sys.h>

1024
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/port_linux.c
View File

@ -1,1024 +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 <pthread.h>
#include <sched.h>
#include <signal.h>
#include <errno.h>
#include <sys/time.h>
#include <time.h>
#include <sys/times.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <limits.h>
/* 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_mutex_t xRunningThreadMutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_t hMainThread = ( pthread_t )NULL;
static pthread_t hActiveThread = ( pthread_t )NULL;
static pthread_t hRequestedThread = ( pthread_t )NULL;
/*-----------------------------------------------------------*/
static volatile portBASE_TYPE xSentinel = 0;
static volatile portBASE_TYPE xGeneralFuckedUpIndicator = 0;
static volatile portBASE_TYPE xVeryFirstTask = 1;
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 );
/*-----------------------------------------------------------*/
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;
tskTCB* prvGetTaskHandle( pthread_t hThread )
{
portLONG lIndex;
if (pxThreads==NULL) return NULL;
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
{
if ( pxThreads[ lIndex ].hThread == hThread )
{
return pxThreads[ lIndex ].hTask;
}
}
return NULL;
}
#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, "%s(%li)\t%s\t%i:",debug_task_handle->pcTaskName,(long)pthread_self(),__func__,__LINE__)): \
(fprintf( stderr, "__unknown__(%li)\t%s\t%i:",(long)pthread_self(),__func__,__LINE__)) \
|1)?( \
((fprintf( stderr, __VA_ARGS__ )|1)?real_pthread_mutex_unlock( &xPrintfMutex ):0) \
):0 ):0 )
//int xbla;
//#define debug_printf(...) xbla=0
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_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 vTaskSwitchContext() ( (debug_printf("SWITCHCONTEXT!\n")|1)?vTaskSwitchContext():vTaskSwitchContext() )
#else
#define debug_printf(...)
#endif
/*-----------------------------------------------------------*/
void prvSuspendThread( pthread_t xThreadId )
{
portBASE_TYPE xResult;
// xResult = pthread_mutex_lock( &xSuspendResumeThreadMutex );
// if ( 0 == xResult )
// {
/* Set-up for the Suspend Signal handler? */
//xSentinel = 0;
//portBASE_TYPE aSentinel=xSentinel;
xResult = pthread_kill( xThreadId, SIG_SUSPEND );
// xResult = pthread_mutex_unlock( &xSuspendResumeThreadMutex );
// while ( ( aSentinel == xSentinel ) && ( pdTRUE != xServicingTick ) )
// {
// sched_yield();
// }
// }
}
/*-----------------------------------------------------------*/
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 );
// }
}
#define prvSuspendThread(thread) debug_printf("calling SuspendThread(%li)\n",(long)thread); prvSuspendThread(thread)
#define prvResumeThread(thread) debug_printf("calling ResumeThread(%li)\n",(long)thread); prvResumeThread(thread)
/*
* 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();
if ( 0 == pthread_mutex_lock( &xSuspendResumeThreadMutex ) ) {
while (hActiveThread!=hRequestedThread && !xVeryFirstTask) {
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
sched_yield();
(void)pthread_mutex_lock( &xSuspendResumeThreadMutex );
}
lIndexOfLastAddedTask = prvGetFreeThreadState();
/* Create the new pThread. */
/* On creation of the very first thread, RunningThreadMutex is not claimed yet
* by the master thread - do that! */
if (xVeryFirstTask==1) {
debug_printf("Seting up very first task (main) - MAIN is ACTIVE TASK\n");
if (0 == pthread_mutex_lock( &xRunningThreadMutex)) {
xVeryFirstTask=0;
hActiveThread=pthread_self();
hRequestedThread=hActiveThread;
} else {
printf("Failed to acquire lock for first task");
exit(1);
}
}
if ( 0 != pthread_create( &( pxThreads[ lIndexOfLastAddedTask ].hThread ), &xThreadAttributes, prvWaitForStart, (void *)pxThisThreadParams ) )
{
/* Thread create failed, signal the failure */
pxTopOfStack = 0;
}
/* Wait until the task suspends. */
xSentinel=0;
(void)pthread_mutex_unlock( &xRunningThreadMutex );
while ( xSentinel == 0 ) {
sched_yield();
}
(void)pthread_mutex_lock( &xRunningThreadMutex );
hActiveThread=pthread_self();
debug_printf("ACTIVE THREAD RECLAIMED!\n");
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
} else {
debug_printf("mutex locking failed\n");
exit(1);
}
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. */
hRequestedThread=prvGetThreadHandle( xTaskGetCurrentTaskHandle());
prvResumeThread( hRequestedThread );
sched_yield();
}
/*-----------------------------------------------------------*/
/*
* 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 );
sigaddset( &xSignals, SIG_TICK );
/* Allow other threads to run */
(void)pthread_mutex_unlock( &xRunningThreadMutex );
debug_printf( "MAIN thread is entering main signal wait loop!\n");
while ( pdTRUE != xSchedulerEnd )
{
if ( 0 != sigwait( &xSignals, &iSignal ) )
{
printf( "Main thread spurious signal: %d\n", iSignal );
}
/**
* Tick handler is called from here - AND ONLY FROM HERE
* (needed for cygwin - but should work on all)
*/
if (iSignal==SIG_TICK) {
vPortSystemTickHandler(iSignal);
}
if (iSignal==SIG_RESUME && pdTRUE != xSchedulerEnd) {
debug_printf("main: forwarding signal to %li\n",(long)hRequestedThread);
(void)pthread_kill( hRequestedThread, SIG_RESUME );
}
}
printf( "Cleaning Up, Exiting.\n" );
/* Cleanup the mutexes */
//xResult = pthread_mutex_destroy( &xSuspendResumeThreadMutex );
//xResult = pthread_mutex_destroy( &xSingleThreadMutex );
xResult = pthread_mutex_destroy( &xRunningThreadMutex );
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;
/**
* Sentinel - do not change context while the running task is not equal the task supposed to run
*/
if ( 0 == pthread_mutex_lock( &xSuspendResumeThreadMutex ) )
{
/**
* Make sure we don't create outdated resume signals
*/
while (hActiveThread!=hRequestedThread) {
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
sched_yield();
(void)pthread_mutex_lock( &xSuspendResumeThreadMutex );
}
xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
vTaskSwitchContext();
xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
if ( xTaskToSuspend != xTaskToResume )
{
/* Remember and switch the critical nesting. */
prvSetTaskCriticalNesting( xTaskToSuspend, uxCriticalNesting );
uxCriticalNesting = prvGetTaskCriticalNesting( xTaskToResume );
/* Switch tasks. */
hRequestedThread = xTaskToResume;
prvResumeThread( xTaskToResume );
//prvSuspendThread( xTaskToSuspend );
if (prvGetThreadHandle(xTaskGetCurrentTaskHandle())!=xTaskToResume) {
debug_printf("\n what the fuck???? someone else did a switchcontext?!?\n");
}
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
prvSuspendSignalHandler(SIG_SUSPEND);
return;
}
else
{
/* Yielding to self */
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
}
}
}
/*-----------------------------------------------------------*/
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;
debug_printf("init %li microseconds\n",(long)xMicroSeconds);
/* 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;
struct timespec timeout;
//debug_printf("received %i\n",sig);
/**
* do not call tick handler if
* - interrupts are disabled
* - tick handler is still running
* - old task switch not yet completed (wrong task running)
*/
if ( ( pdTRUE == xInterruptsEnabled ) && ( pdTRUE != xServicingTick ) && ( hRequestedThread == hActiveThread ) )
{
xServicingTick = pdTRUE;
if ( 0 == pthread_mutex_trylock( &xSuspendResumeThreadMutex ) )
{
debug_printf("does handle tick\n");
/**
* this shouldn't ever happen - but WELL...
* Make sure we don't create outdated resume signals
*/
if (hActiveThread!=hRequestedThread) {
xServicingTick = pdFALSE;
xPendYield = pdTRUE;
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
return;
}
xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
/* Tick Increment. */
vTaskIncrementTick();
/* Select Next Task. */
#if ( configUSE_PREEMPTION == 1 )
vTaskSwitchContext();
#endif
xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
//hRequestedThread = xTaskToResume;
/* The only thread that can process this tick is the running thread. */
if ( xTaskToSuspend != xTaskToResume )
{
/* Suspend the current task. */
hRequestedThread = 0;
prvSuspendThread( xTaskToSuspend );
timeout.tv_sec=0;
timeout.tv_nsec=10000;
while ( 0 != pthread_mutex_timedlock( &xRunningThreadMutex,&timeout ) ) {
prvSuspendThread( xTaskToSuspend );
timeout.tv_sec=0;
timeout.tv_nsec=10000;
}
/*
if ( 0 != pthread_mutex_lock( &xRunningThreadMutex)) {
debug_printf("mutex lock failed!\n");
exit(1);
}
*/
//if ( 0 == pthread_mutex_lock( &xRunningThreadMutex) ) {
/* Remember and switch the critical nesting. */
prvSetTaskCriticalNesting( xTaskToSuspend, uxCriticalNesting );
uxCriticalNesting = prvGetTaskCriticalNesting( xTaskToResume );
/* Resume next task. */
hRequestedThread = xTaskToResume;
prvResumeThread( xTaskToResume );
if (prvGetThreadHandle(xTaskGetCurrentTaskHandle())!=xTaskToResume) {
debug_printf("\n what the fuck???? someone else did a switchcontext?!?\n");
}
(void)pthread_mutex_unlock( &xRunningThreadMutex );
}
else
{
/* Release the lock as we are Resuming. */
// (void)pthread_mutex_unlock( &xSingleThreadMutex );
}
}
else
{
xPendYield = pdTRUE;
}
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
xServicingTick = pdFALSE;
}
else
{
debug_printf("will NOT handle tick\n");
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 )
{
if ( 0 == pthread_mutex_lock( &xSuspendResumeThreadMutex ) ) {
/**
* Make sure we don't create outdated resume signals
*/
while (hActiveThread!=hRequestedThread) {
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
sched_yield();
(void)pthread_mutex_lock( &xSuspendResumeThreadMutex );
}
/* This is a suicidal thread, need to select a different task to run. */
vTaskSwitchContext();
xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
hRequestedThread = xTaskToResume;
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
} else {
debug_printf("mutex lock failed!\n");
exit(1);
}
}
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 );
(void)pthread_mutex_unlock( &xRunningThreadMutex );
/* Commit suicide */
pthread_exit( (void *)1 );
}
// }
}
/*-----------------------------------------------------------*/
void *prvWaitForStart( void * pvParams )
{
xParams * pxParams = ( xParams * )pvParams;
pdTASK_CODE pvCode = pxParams->pxCode;
void * pParams = pxParams->pvParams;
sigset_t xSignals;
vPortFree( pvParams );
pthread_cleanup_push( prvDeleteThread, (void *)pthread_self() );
if ( 0 == pthread_mutex_lock( &xRunningThreadMutex ) )
{
xSentinel=1;
hActiveThread=pthread_self();
debug_printf("temporarily made ACTIVE THREAD!\n");
sigemptyset( &xSignals );
sigaddset( &xSignals, SIG_RESUME );
/* set up proc mask */
pthread_sigmask(SIG_SETMASK,&xSignals,NULL);
prvSuspendSignalHandler(SIG_SUSPEND);
//prvSuspendThread( pthread_self() );
} else {
debug_printf("now this is just WRONG!\n");
exit(1);
}
pvCode( pParams );
pthread_cleanup_pop( 1 );
return (void *)NULL;
}
/*-----------------------------------------------------------*/
void prvSuspendSignalHandler(int sig)
{
sigset_t xSignals;
//sigset_t xPendingSignals;
/* Only interested in the resume signal. */
sigemptyset( &xSignals );
sigaddset( &xSignals, SIG_RESUME );
sigaddset( &xSignals, SIG_SUSPEND );
/* Unlock the Running thread mutex to allow the resumed task to continue. */
if ( 0 != pthread_mutex_unlock( &xRunningThreadMutex ) )
{
printf( "Releasing someone else's lock.\n" );
}
debug_printf("SUSPENDING until SIG_RESUME received\n");
/* Wait on the resume signal. */
while (hRequestedThread != pthread_self()) {
if ( 0 != sigwait( &xSignals, &sig ) )
{
printf( "SSH: Sw %d\n", sig );
/* tricky one - shouldn't ever happen - trying to resolve situation as graceful as possible */
debug_printf("ALERT AAAAH PANIC! - sigwait failed.....\n\n\n");
/* Signal main thread something just went HORRIBLY wrong */
xGeneralFuckedUpIndicator = 2;
//(void)pthread_mutex_lock( &xRunningThreadMutex );
//(void)pthread_kill( pthread_self(), SIG_SUSPEND );
//return;
}
}
/* Yield the Scheduler to ensure that the yielding thread completes. */
if ( 0 != pthread_mutex_lock( &xRunningThreadMutex ) )
{
//(void)pthread_mutex_unlock( &xSingleThreadMutex );
debug_printf("critical - mutex acquiring of active thread failed!\n");
exit(1);
}
hActiveThread = pthread_self();
/* 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();
}
if (hRequestedThread==pthread_self()) {
/* doesn't look too bad, does it? */
xGeneralFuckedUpIndicator = 0;
}
debug_printf("ACTIVE THREAD!\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, 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;
sigresume.sa_handler = SIG_IGN;
sigfillset( &sigresume.sa_mask );
sigtick.sa_flags = 0;
//sigtick.sa_handler = vPortSystemTickHandler;
sigtick.sa_handler = SIG_IGN;
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;
}
/*-----------------------------------------------------------*/

1042
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/port_new-thread-implementation_works-on-linux.c
View File

@ -1,1042 +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 <pthread.h>
#include <sched.h>
#include <signal.h>
#include <errno.h>
#include <sys/time.h>
#include <time.h>
#include <sys/times.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <limits.h>
/* 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_mutex_t xRunningThreadMutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t xPrintfMutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_t hMainThread = ( pthread_t )NULL;
static pthread_t hActiveThread = ( pthread_t )NULL;
static pthread_t hRequestedThread = ( pthread_t )NULL;
/*-----------------------------------------------------------*/
static volatile portBASE_TYPE xSentinel = 0;
static volatile portBASE_TYPE xGeneralFuckedUpIndicator = 0;
static volatile portBASE_TYPE xVeryFirstTask = 1;
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 );
static void prvResolveFuckup( 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;
tskTCB* prvGetTaskHandle( pthread_t hThread )
{
portLONG lIndex;
if (pxThreads==NULL) return NULL;
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
{
if ( pxThreads[ lIndex ].hThread == hThread )
{
return pxThreads[ lIndex ].hTask;
}
}
return NULL;
}
#define debug_printf(...) ( (real_pthread_mutex_lock( &xPrintfMutex )|1)?( \
( \
(NULL != (debug_task_handle = prvGetTaskHandle(pthread_self())) )? \
(fprintf( stderr, "%s(%li)\t%s\t%i:",debug_task_handle->pcTaskName,(long)pthread_self(),__func__,__LINE__)): \
(fprintf( stderr, "__unknown__(%li)\t%s\t%i:",(long)pthread_self(),__func__,__LINE__)) \
|1)?( \
((fprintf( stderr, __VA_ARGS__ )|1)?real_pthread_mutex_unlock( &xPrintfMutex ):0) \
):0 ):0 )
//int xbla;
//#define debug_printf(...) xbla=0
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_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 vTaskSwitchContext() ( (debug_printf("SWITCHCONTEXT!\n")|1)?vTaskSwitchContext():vTaskSwitchContext() )
*/
/*-----------------------------------------------------------*/
void prvSuspendThread( pthread_t xThreadId )
{
portBASE_TYPE xResult;
// xResult = pthread_mutex_lock( &xSuspendResumeThreadMutex );
// if ( 0 == xResult )
// {
/* Set-up for the Suspend Signal handler? */
//xSentinel = 0;
//portBASE_TYPE aSentinel=xSentinel;
xResult = pthread_kill( xThreadId, SIG_SUSPEND );
// xResult = pthread_mutex_unlock( &xSuspendResumeThreadMutex );
// while ( ( aSentinel == xSentinel ) && ( pdTRUE != xServicingTick ) )
// {
// sched_yield();
// }
// }
}
/*-----------------------------------------------------------*/
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 );
// }
}
#define prvSuspendThread(thread) debug_printf("calling SuspendThread(%li)\n",(long)thread); prvSuspendThread(thread)
#define prvResumeThread(thread) debug_printf("calling ResumeThread(%li)\n",(long)thread); prvResumeThread(thread)
/*
* 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. */
/* On creation of the very first thread, RunningThreadMutex is not claimed yet
* by the master thread - do that! */
if (xVeryFirstTask==1) {
if (0 == pthread_mutex_lock( &xRunningThreadMutex)) {
xVeryFirstTask=0;
} else {
printf("Failed to acquire lock for first task");
exit(1);
}
}
if ( 0 != pthread_create( &( pxThreads[ lIndexOfLastAddedTask ].hThread ), &xThreadAttributes, prvWaitForStart, (void *)pxThisThreadParams ) )
{
/* Thread create failed, signal the failure */
pxTopOfStack = 0;
}
/* Wait until the task suspends. */
xSentinel=0;
(void)pthread_mutex_unlock( &xRunningThreadMutex );
while ( xSentinel == 0 ) {
sched_yield();
}
(void)pthread_mutex_lock( &xRunningThreadMutex );
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. */
hRequestedThread=prvGetThreadHandle( xTaskGetCurrentTaskHandle());
prvResumeThread( hRequestedThread );
sched_yield();
}
/*-----------------------------------------------------------*/
/*
* See header file for description.
*/
portBASE_TYPE xPortStartScheduler( void )
{
portBASE_TYPE xResult;
int iSignal;
int fuckedUpCount=0;
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 );
sigaddset( &xSignals, SIG_TICK );
/* Allow other threads to run */
(void)pthread_mutex_unlock( &xRunningThreadMutex );
debug_printf( "MAIN thread is entering main signal wait loop!\n");
while ( pdTRUE != xSchedulerEnd )
{
if ( 0 != sigwait( &xSignals, &iSignal ) )
{
printf( "Main thread spurious signal: %d\n", iSignal );
}
/**
* Tick handler is called from here - AND ONLY FROM HERE
* (needed for cygwin - but should work on all)
*/
if (iSignal==SIG_TICK) {
if (xGeneralFuckedUpIndicator!=0) {
fuckedUpCount++;
if (fuckedUpCount>10) {
fuckedUpCount=0;
prvResolveFuckup();
}
} else {
fuckedUpCount=0;
}
vPortSystemTickHandler(iSignal);
}
if (iSignal==SIG_RESUME && pdTRUE != xSchedulerEnd) {
debug_printf( "ALERT! Main received SIG_RESUME that was supposed to go elsewhere!");
}
}
printf( "Cleaning Up, Exiting.\n" );
/* Cleanup the mutexes */
//xResult = pthread_mutex_destroy( &xSuspendResumeThreadMutex );
//xResult = pthread_mutex_destroy( &xSingleThreadMutex );
xResult = pthread_mutex_destroy( &xRunningThreadMutex );
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;
/**
* Sentinel - do not change context while the running task is not equal the task supposed to run
*/
if ( 0 == pthread_mutex_lock( &xSuspendResumeThreadMutex ) )
{
/**
* Make sure we don't create outdated resume signals
*/
while (hActiveThread!=hRequestedThread) {
sched_yield();
}
xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
vTaskSwitchContext();
xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
hRequestedThread = xTaskToResume;
if ( xTaskToSuspend != xTaskToResume )
{
/* Remember and switch the critical nesting. */
prvSetTaskCriticalNesting( xTaskToSuspend, uxCriticalNesting );
uxCriticalNesting = prvGetTaskCriticalNesting( xTaskToResume );
/* Switch tasks. */
prvResumeThread( xTaskToResume );
//prvSuspendThread( xTaskToSuspend );
if (prvGetThreadHandle(xTaskGetCurrentTaskHandle())!=xTaskToResume) {
debug_printf("\n what the fuck???? someone else did a switchcontext?!?\n");
}
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
prvSuspendSignalHandler(SIG_SUSPEND);
return;
}
else
{
/* Yielding to self */
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
}
}
}
/*-----------------------------------------------------------*/
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;
debug_printf("init %li microseconds\n",(long)xMicroSeconds);
/* 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;
struct timespec timeout;
//debug_printf("received %i\n",sig);
/**
* do not call tick handler if
* - interrupts are disabled
* - tick handler is still running
* - old task switch not yet completed (wrong task running)
*/
if ( ( pdTRUE == xInterruptsEnabled ) && ( pdTRUE != xServicingTick ) && ( hRequestedThread == hActiveThread ) )
{
if ( 0 == pthread_mutex_trylock( &xSuspendResumeThreadMutex ) )
{
//debug_printf("will handle tick\n");
xServicingTick = pdTRUE;
/**
* this shouldn't ever happen - but WELL...
* Make sure we don't create outdated resume signals
*/
while (hActiveThread!=hRequestedThread) {
sched_yield();
}
xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
/* Tick Increment. */
vTaskIncrementTick();
/* Select Next Task. */
#if ( configUSE_PREEMPTION == 1 )
vTaskSwitchContext();
#endif
xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
hRequestedThread = xTaskToResume;
/* The only thread that can process this tick is the running thread. */
if ( xTaskToSuspend != xTaskToResume )
{
/* Suspend the current task. */
prvSuspendThread( xTaskToSuspend );
timeout.tv_sec=0;
timeout.tv_nsec=10000;
//if ( 0 == pthread_mutex_timedlock( &xRunningThreadMutex,&timeout ) ) {
if ( 0 == pthread_mutex_lock( &xRunningThreadMutex) ) {
/* Remember and switch the critical nesting. */
prvSetTaskCriticalNesting( xTaskToSuspend, uxCriticalNesting );
uxCriticalNesting = prvGetTaskCriticalNesting( xTaskToResume );
/* Resume next task. */
prvResumeThread( xTaskToResume );
if (prvGetThreadHandle(xTaskGetCurrentTaskHandle())!=xTaskToResume) {
debug_printf("\n what the fuck???? someone else did a switchcontext?!?\n");
}
(void)pthread_mutex_unlock( &xRunningThreadMutex );
} else {
debug_printf("Oh dear - tick handler could not acquire lock!\n\n");
//prvResumeThread( xTaskToSuspend );
xGeneralFuckedUpIndicator=3;
}
}
else
{
/* Release the lock as we are Resuming. */
// (void)pthread_mutex_unlock( &xSingleThreadMutex );
}
xServicingTick = pdFALSE;
}
else
{
xPendYield = pdTRUE;
}
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
}
else
{
debug_printf("will NOT handle tick\n");
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 )
{
if ( 0 == pthread_mutex_lock( &xSuspendResumeThreadMutex ) ) {
/**
* Make sure we don't create outdated resume signals
*/
while (hActiveThread!=hRequestedThread) {
sched_yield();
}
/* This is a suicidal thread, need to select a different task to run. */
vTaskSwitchContext();
xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
hRequestedThread = xTaskToResume;
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
} else {
debug_printf("THIS was never meant to happen\n");
exit(1);
}
}
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 );
(void)pthread_mutex_unlock( &xRunningThreadMutex );
/* Commit suicide */
pthread_exit( (void *)1 );
}
// }
}
/*-----------------------------------------------------------*/
void *prvWaitForStart( void * pvParams )
{
xParams * pxParams = ( xParams * )pvParams;
pdTASK_CODE pvCode = pxParams->pxCode;
void * pParams = pxParams->pvParams;
sigset_t xSignals;
vPortFree( pvParams );
pthread_cleanup_push( prvDeleteThread, (void *)pthread_self() );
if ( 0 == pthread_mutex_lock( &xRunningThreadMutex ) )
{
xSentinel=1;
sigemptyset( &xSignals );
sigaddset( &xSignals, SIG_RESUME );
/* set up proc mask */
pthread_sigmask(SIG_SETMASK,&xSignals,NULL);
prvSuspendSignalHandler(SIG_SUSPEND);
//prvSuspendThread( pthread_self() );
} else {
debug_printf("now this is just WRONG!\n");
exit(1);
}
pvCode( pParams );
pthread_cleanup_pop( 1 );
return (void *)NULL;
}
/*-----------------------------------------------------------*/
void prvSuspendSignalHandler(int sig)
{
sigset_t xSignals;
//sigset_t xPendingSignals;
/* Only interested in the resume signal. */
sigemptyset( &xSignals );
sigaddset( &xSignals, SIG_RESUME );
sigaddset( &xSignals, SIG_SUSPEND );
/* Unlock the Running thread mutex to allow the resumed task to continue. */
if ( 0 != pthread_mutex_unlock( &xRunningThreadMutex ) )
{
printf( "Releasing someone else's lock.\n" );
}
//debug_printf("SUSPENDING until SIG_RESUME received\n");
/* Wait on the resume signal. */
while (hRequestedThread != pthread_self()) {
if ( 0 != sigwait( &xSignals, &sig ) )
{
printf( "SSH: Sw %d\n", sig );
/* tricky one - shouldn't ever happen - trying to resolve situation as graceful as possible */
debug_printf("ALERT AAAAH PANIC! - sigwait failed.....\n\n\n");
/* Signal main thread something just went HORRIBLY wrong */
xGeneralFuckedUpIndicator = 2;
//(void)pthread_mutex_lock( &xRunningThreadMutex );
//(void)pthread_kill( pthread_self(), SIG_SUSPEND );
//return;
} else if (sig == SIG_RESUME) {
//debug_printf("received signal %i\n",sig);
/* Make sure the right thread received the signal */
if (hRequestedThread != pthread_self() ) {
debug_printf( "ALERT! Received SIG_RESUME which is already outdated!\n active thread is %li\n",(long)hRequestedThread);
/* Signal main thread something just went wrong */
xGeneralFuckedUpIndicator = 1;
/*
if (0 == sigpending(&xPendingSignals)) {
if (sigismember(&xPendingSignals,SIG_SUSPEND)) {
debug_printf( "reason: we slept too long...\n");
//(void)sigwait(&xPendingSignals,&sig);
// we can safely return - signal is already pending
return;
}
}
debug_printf( "reason: unknown! - whatever ...\n\n");
*/
//exit(1);
//(void)pthread_kill( xTaskToResume, SIG_RESUME );
//(void)pthread_mutex_lock( &xRunningThreadMutex );
//(void)pthread_kill( pthread_self(), SIG_SUSPEND );
//return;
}
}
}
/* Yield the Scheduler to ensure that the yielding thread completes. */
if ( 0 != pthread_mutex_lock( &xRunningThreadMutex ) )
{
//(void)pthread_mutex_unlock( &xSingleThreadMutex );
debug_printf("critical - mutex acquiring of active thread failed!\n");
exit(1);
}
hActiveThread = pthread_self();
/* 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();
}
if (hRequestedThread==pthread_self()) {
/* doesn't look too bad, does it? */
xGeneralFuckedUpIndicator = 0;
}
//debug_printf("ACTIVE THREAD!\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, 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;
sigresume.sa_handler = SIG_IGN;
sigfillset( &sigresume.sa_mask );
sigtick.sa_flags = 0;
//sigtick.sa_handler = vPortSystemTickHandler;
sigtick.sa_handler = SIG_IGN;
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;
}
/*-----------------------------------------------------------*/
/**
* Scheduler f***d up - we got to fix that
*/
void prvResolveFuckup( void )
{
printf("Scheduler fucked up again - lets try to fix it...\n");
printf("sending sig_suspend to thread that is supposed to be dead...\n");
prvSuspendThread(hActiveThread);
printf("acquire running lock...");
if ( 0 == pthread_mutex_lock( &xRunningThreadMutex) ) {
printf("sending sig_resume to thread that is supposed to be running...\n");
prvResumeThread(hRequestedThread);
printf("giving up mutex...\n");
(void)pthread_mutex_unlock(&xRunningThreadMutex);
}
}
/*-----------------------------------------------------------*/

1026
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/port_test-for-macos.c
View File

@ -1,1026 +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 <pthread.h>
#include <sched.h>
#include <signal.h>
#include <errno.h>
#include <sys/time.h>
#include <time.h>
#include <sys/times.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <limits.h>
/* 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_mutex_t xRunningThreadMutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t xPrintfMutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_t hMainThread = ( pthread_t )NULL;
static pthread_t hActiveThread = ( pthread_t )NULL;
static pthread_t hRequestedThread = ( pthread_t )NULL;
/*-----------------------------------------------------------*/
static volatile portBASE_TYPE xSentinel = 0;
static volatile portBASE_TYPE xGeneralFuckedUpIndicator = 0;
static volatile portBASE_TYPE xVeryFirstTask = 1;
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 );
/*-----------------------------------------------------------*/
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;
tskTCB* prvGetTaskHandle( pthread_t hThread )
{
portLONG lIndex;
if (pxThreads==NULL) return NULL;
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
{
if ( pxThreads[ lIndex ].hThread == hThread )
{
return pxThreads[ lIndex ].hTask;
}
}
return NULL;
}
#define debug_printf(...) ( (real_pthread_mutex_lock( &xPrintfMutex )|1)?( \
( \
(NULL != (debug_task_handle = prvGetTaskHandle(pthread_self())) )? \
(fprintf( stderr, "%s(%li)\t%s\t%i:",debug_task_handle->pcTaskName,(long)pthread_self(),__func__,__LINE__)): \
(fprintf( stderr, "__unknown__(%li)\t%s\t%i:",(long)pthread_self(),__func__,__LINE__)) \
|1)?( \
((fprintf( stderr, __VA_ARGS__ )|1)?real_pthread_mutex_unlock( &xPrintfMutex ):0) \
):0 ):0 )
//int xbla;
//#define debug_printf(...) xbla=0
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_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 vTaskSwitchContext() ( (debug_printf("SWITCHCONTEXT!\n")|1)?vTaskSwitchContext():vTaskSwitchContext() )
/*-----------------------------------------------------------*/
void prvSuspendThread( pthread_t xThreadId )
{
portBASE_TYPE xResult;
// xResult = pthread_mutex_lock( &xSuspendResumeThreadMutex );
// if ( 0 == xResult )
// {
/* Set-up for the Suspend Signal handler? */
//xSentinel = 0;
//portBASE_TYPE aSentinel=xSentinel;
xResult = pthread_kill( xThreadId, SIG_SUSPEND );
// xResult = pthread_mutex_unlock( &xSuspendResumeThreadMutex );
// while ( ( aSentinel == xSentinel ) && ( pdTRUE != xServicingTick ) )
// {
// sched_yield();
// }
// }
}
/*-----------------------------------------------------------*/
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 );
// }
}
#define prvSuspendThread(thread) debug_printf("calling SuspendThread(%li)\n",(long)thread); prvSuspendThread(thread)
#define prvResumeThread(thread) debug_printf("calling ResumeThread(%li)\n",(long)thread); prvResumeThread(thread)
/*
* 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();
if ( 0 == pthread_mutex_lock( &xSuspendResumeThreadMutex ) ) {
while (hActiveThread!=hRequestedThread && !xVeryFirstTask) {
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
sched_yield();
(void)pthread_mutex_lock( &xSuspendResumeThreadMutex );
}
lIndexOfLastAddedTask = prvGetFreeThreadState();
/* Create the new pThread. */
/* On creation of the very first thread, RunningThreadMutex is not claimed yet
* by the master thread - do that! */
if (xVeryFirstTask==1) {
debug_printf("Seting up very first task (main) - MAIN is ACTIVE TASK\n");
if (0 == pthread_mutex_lock( &xRunningThreadMutex)) {
xVeryFirstTask=0;
hActiveThread=pthread_self();
hRequestedThread=hActiveThread;
} else {
printf("Failed to acquire lock for first task");
exit(1);
}
}
if ( 0 != pthread_create( &( pxThreads[ lIndexOfLastAddedTask ].hThread ), &xThreadAttributes, prvWaitForStart, (void *)pxThisThreadParams ) )
{
/* Thread create failed, signal the failure */
pxTopOfStack = 0;
}
/* Wait until the task suspends. */
xSentinel=0;
(void)pthread_mutex_unlock( &xRunningThreadMutex );
while ( xSentinel == 0 ) {
sched_yield();
}
(void)pthread_mutex_lock( &xRunningThreadMutex );
hActiveThread=pthread_self();
debug_printf("ACTIVE THREAD RECLAIMED!\n");
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
} else {
debug_printf("mutex locking failed\n");
exit(1);
}
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. */
hRequestedThread=prvGetThreadHandle( xTaskGetCurrentTaskHandle());
prvResumeThread( hRequestedThread );
sched_yield();
}
/*-----------------------------------------------------------*/
/*
* 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 );
sigaddset( &xSignals, SIG_TICK );
/* Allow other threads to run */
(void)pthread_mutex_unlock( &xRunningThreadMutex );
debug_printf( "MAIN thread is entering main signal wait loop!\n");
while ( pdTRUE != xSchedulerEnd )
{
if ( 0 != sigwait( &xSignals, &iSignal ) )
{
printf( "Main thread spurious signal: %d\n", iSignal );
}
/**
* Tick handler is called from here - AND ONLY FROM HERE
* (needed for cygwin - but should work on all)
*/
if (iSignal==SIG_TICK) {
vPortSystemTickHandler(iSignal);
}
if (iSignal==SIG_RESUME && pdTRUE != xSchedulerEnd) {
debug_printf("main: forwarding signal to %li\n",(long)hRequestedThread);
(void)pthread_kill( hRequestedThread, SIG_RESUME );
}
}
printf( "Cleaning Up, Exiting.\n" );
/* Cleanup the mutexes */
//xResult = pthread_mutex_destroy( &xSuspendResumeThreadMutex );
//xResult = pthread_mutex_destroy( &xSingleThreadMutex );
xResult = pthread_mutex_destroy( &xRunningThreadMutex );
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;
/**
* Sentinel - do not change context while the running task is not equal the task supposed to run
*/
if ( 0 == pthread_mutex_lock( &xSuspendResumeThreadMutex ) )
{
/**
* Make sure we don't create outdated resume signals
*/
while (hActiveThread!=hRequestedThread) {
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
sched_yield();
(void)pthread_mutex_lock( &xSuspendResumeThreadMutex );
}
xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
vTaskSwitchContext();
xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
if ( xTaskToSuspend != xTaskToResume )
{
/* Remember and switch the critical nesting. */
prvSetTaskCriticalNesting( xTaskToSuspend, uxCriticalNesting );
uxCriticalNesting = prvGetTaskCriticalNesting( xTaskToResume );
/* Switch tasks. */
hRequestedThread = xTaskToResume;
prvResumeThread( xTaskToResume );
//prvSuspendThread( xTaskToSuspend );
if (prvGetThreadHandle(xTaskGetCurrentTaskHandle())!=xTaskToResume) {
debug_printf("\n what the fuck???? someone else did a switchcontext?!?\n");
}
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
prvSuspendSignalHandler(SIG_SUSPEND);
return;
}
else
{
/* Yielding to self */
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
}
}
}
/*-----------------------------------------------------------*/
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;
debug_printf("init %li microseconds\n",(long)xMicroSeconds);
/* 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;
struct timespec timeout;
//debug_printf("received %i\n",sig);
/**
* do not call tick handler if
* - interrupts are disabled
* - tick handler is still running
* - old task switch not yet completed (wrong task running)
*/
if ( ( pdTRUE == xInterruptsEnabled ) && ( pdTRUE != xServicingTick ) && ( hRequestedThread == hActiveThread ) )
{
xServicingTick = pdTRUE;
if ( 0 == pthread_mutex_trylock( &xSuspendResumeThreadMutex ) )
{
debug_printf("does handle tick\n");
/**
* this shouldn't ever happen - but WELL...
* Make sure we don't create outdated resume signals
*/
if (hActiveThread!=hRequestedThread) {
xServicingTick = pdFALSE;
xPendYield = pdTRUE;
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
return;
}
xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
/* Tick Increment. */
vTaskIncrementTick();
/* Select Next Task. */
#if ( configUSE_PREEMPTION == 1 )
vTaskSwitchContext();
#endif
xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
//hRequestedThread = xTaskToResume;
/* The only thread that can process this tick is the running thread. */
if ( xTaskToSuspend != xTaskToResume )
{
/* Suspend the current task. */
hRequestedThread = 0;
prvSuspendThread( xTaskToSuspend );
timeout.tv_sec=0;
timeout.tv_nsec=10000;
/*while ( 0 != pthread_mutex_timedlock( &xRunningThreadMutex,&timeout ) ) {
prvSuspendThread( xTaskToSuspend );
timeout.tv_sec=0;
timeout.tv_nsec=10000;
}
*/
if ( 0 != pthread_mutex_lock( &xRunningThreadMutex)) {
debug_printf("mutex lock failed!\n");
exit(1);
}
//if ( 0 == pthread_mutex_lock( &xRunningThreadMutex) ) {
/* Remember and switch the critical nesting. */
prvSetTaskCriticalNesting( xTaskToSuspend, uxCriticalNesting );
uxCriticalNesting = prvGetTaskCriticalNesting( xTaskToResume );
/* Resume next task. */
hRequestedThread = xTaskToResume;
prvResumeThread( xTaskToResume );
if (prvGetThreadHandle(xTaskGetCurrentTaskHandle())!=xTaskToResume) {
debug_printf("\n what the fuck???? someone else did a switchcontext?!?\n");
}
(void)pthread_mutex_unlock( &xRunningThreadMutex );
}
else
{
/* Release the lock as we are Resuming. */
// (void)pthread_mutex_unlock( &xSingleThreadMutex );
}
}
else
{
xPendYield = pdTRUE;
}
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
xServicingTick = pdFALSE;
}
else
{
debug_printf("will NOT handle tick\n");
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 )
{
if ( 0 == pthread_mutex_lock( &xSuspendResumeThreadMutex ) ) {
/**
* Make sure we don't create outdated resume signals
*/
while (hActiveThread!=hRequestedThread) {
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
sched_yield();
(void)pthread_mutex_lock( &xSuspendResumeThreadMutex );
}
/* This is a suicidal thread, need to select a different task to run. */
vTaskSwitchContext();
xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
hRequestedThread = xTaskToResume;
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
} else {
debug_printf("mutex lock failed!\n");
exit(1);
}
}
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 );
(void)pthread_mutex_unlock( &xRunningThreadMutex );
/* Commit suicide */
pthread_exit( (void *)1 );
}
// }
}
/*-----------------------------------------------------------*/
void *prvWaitForStart( void * pvParams )
{
xParams * pxParams = ( xParams * )pvParams;
pdTASK_CODE pvCode = pxParams->pxCode;
void * pParams = pxParams->pvParams;
sigset_t xSignals;
vPortFree( pvParams );
pthread_cleanup_push( prvDeleteThread, (void *)pthread_self() );
if ( 0 == pthread_mutex_lock( &xRunningThreadMutex ) )
{
xSentinel=1;
hActiveThread=pthread_self();
debug_printf("temporarily made ACTIVE THREAD!\n");
sigemptyset( &xSignals );
sigaddset( &xSignals, SIG_RESUME );
/* set up proc mask */
pthread_sigmask(SIG_SETMASK,&xSignals,NULL);
prvSuspendSignalHandler(SIG_SUSPEND);
//prvSuspendThread( pthread_self() );
} else {
debug_printf("now this is just WRONG!\n");
exit(1);
}
pvCode( pParams );
pthread_cleanup_pop( 1 );
return (void *)NULL;
}
/*-----------------------------------------------------------*/
void prvSuspendSignalHandler(int sig)
{
sigset_t xSignals;
//sigset_t xPendingSignals;
/* Only interested in the resume signal. */
sigemptyset( &xSignals );
sigaddset( &xSignals, SIG_RESUME );
sigaddset( &xSignals, SIG_SUSPEND );
/* Unlock the Running thread mutex to allow the resumed task to continue. */
if ( 0 != pthread_mutex_unlock( &xRunningThreadMutex ) )
{
printf( "Releasing someone else's lock.\n" );
}
debug_printf("SUSPENDING until SIG_RESUME received\n");
/* Wait on the resume signal. */
while (hRequestedThread != pthread_self()) {
if ( 0 != sigwait( &xSignals, &sig ) )
{
printf( "SSH: Sw %d\n", sig );
/* tricky one - shouldn't ever happen - trying to resolve situation as graceful as possible */
debug_printf("ALERT AAAAH PANIC! - sigwait failed.....\n\n\n");
/* Signal main thread something just went HORRIBLY wrong */
xGeneralFuckedUpIndicator = 2;
//(void)pthread_mutex_lock( &xRunningThreadMutex );
//(void)pthread_kill( pthread_self(), SIG_SUSPEND );
//return;
} else if (sig == SIG_RESUME) {
//debug_printf("received signal %i\n",sig);
/* Make sure the right thread received the signal */
if (hRequestedThread != pthread_self() ) {
debug_printf( "resume_signalhandler: forwarding to %li\n",(long)hRequestedThread);
(void)pthread_kill( hRequestedThread, SIG_RESUME );
}
}
}
/* Yield the Scheduler to ensure that the yielding thread completes. */
if ( 0 != pthread_mutex_lock( &xRunningThreadMutex ) )
{
//(void)pthread_mutex_unlock( &xSingleThreadMutex );
debug_printf("critical - mutex acquiring of active thread failed!\n");
exit(1);
}
hActiveThread = pthread_self();
/* 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();
}
if (hRequestedThread==pthread_self()) {
/* doesn't look too bad, does it? */
xGeneralFuckedUpIndicator = 0;
}
debug_printf("ACTIVE THREAD!\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, 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;
sigresume.sa_handler = SIG_IGN;
sigfillset( &sigresume.sa_mask );
sigtick.sa_flags = 0;
//sigtick.sa_handler = vPortSystemTickHandler;
sigtick.sa_handler = SIG_IGN;
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;
}
/*-----------------------------------------------------------*/

1097
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/port_with-debugging-output.c
View File

@ -1,1097 +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 <pthread.h>
#include <sched.h>
#include <signal.h>
#include <errno.h>
#include <sys/time.h>
#include <time.h>
#include <sys/times.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <limits.h>
/* 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_mutex_t xRunningThreadMutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t xPrintfMutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_t hMainThread = ( pthread_t )NULL;
static pthread_t hActiveThread = ( pthread_t )NULL;
static pthread_t hRequestedThread = ( pthread_t )NULL;
/*-----------------------------------------------------------*/
static volatile portBASE_TYPE xSentinel = 0;
static volatile portBASE_TYPE xGeneralFuckedUpIndicator = 0;
static volatile portBASE_TYPE xVeryFirstTask = 1;
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 );
static void prvResolveFuckup( 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;
tskTCB* prvGetTaskHandle( pthread_t hThread )
{
portLONG lIndex;
if (pxThreads==NULL) return NULL;
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
{
if ( pxThreads[ lIndex ].hThread == hThread )
{
return pxThreads[ lIndex ].hTask;
}
}
return NULL;
}
#define debug_printf(...) ( (real_pthread_mutex_lock( &xPrintfMutex )|1)?( \
( \
(NULL != (debug_task_handle = prvGetTaskHandle(pthread_self())) )? \
(fprintf( stderr, "%s(%li)\t%s\t%i:",debug_task_handle->pcTaskName,(long)pthread_self(),__func__,__LINE__)): \
(fprintf( stderr, "__unknown__(%li)\t%s\t%i:",(long)pthread_self(),__func__,__LINE__)) \
|1)?( \
((fprintf( stderr, __VA_ARGS__ )|1)?real_pthread_mutex_unlock( &xPrintfMutex ):0) \
):0 ):0 )
//int xbla;
//#define debug_printf(...) xbla=0
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_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_mutex_trylock(...) ( pthread_mutex_trylock(__VA_ARGS__)==0?((debug_printf(" -:)- pthread_mutex_trylock(%s) success\n",#__VA_ARGS__)|1)?0:0):-1)
#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 vTaskSwitchContext() ( (debug_printf("SWITCHCONTEXT!\n")|1)?vTaskSwitchContext():vTaskSwitchContext() )
/*-----------------------------------------------------------*/
void prvSuspendThread( pthread_t xThreadId )
{
portBASE_TYPE xResult;
// xResult = pthread_mutex_lock( &xSuspendResumeThreadMutex );
// if ( 0 == xResult )
// {
/* Set-up for the Suspend Signal handler? */
//xSentinel = 0;
//portBASE_TYPE aSentinel=xSentinel;
xResult = pthread_kill( xThreadId, SIG_SUSPEND );
// xResult = pthread_mutex_unlock( &xSuspendResumeThreadMutex );
// while ( ( aSentinel == xSentinel ) && ( pdTRUE != xServicingTick ) )
// {
// sched_yield();
// }
// }
}
/*-----------------------------------------------------------*/
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 );
// }
}
#define prvSuspendThread(thread) debug_printf("calling SuspendThread(%li)\n",(long)thread); prvSuspendThread(thread)
#define prvResumeThread(thread) debug_printf("calling ResumeThread(%li)\n",(long)thread); prvResumeThread(thread)
/*
* 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();
if ( 0 == pthread_mutex_lock( &xSuspendResumeThreadMutex ) ) {
while (hActiveThread!=hRequestedThread && !xVeryFirstTask) {
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
sched_yield();
(void)pthread_mutex_lock( &xSuspendResumeThreadMutex );
}
lIndexOfLastAddedTask = prvGetFreeThreadState();
/* Create the new pThread. */
/* On creation of the very first thread, RunningThreadMutex is not claimed yet
* by the master thread - do that! */
if (xVeryFirstTask==1) {
debug_printf("Seting up very first task (main) - MAIN is ACTIVE TASK\n");
if (0 == pthread_mutex_lock( &xRunningThreadMutex)) {
xVeryFirstTask=0;
hActiveThread=pthread_self();
hRequestedThread=hActiveThread;
} else {
printf("Failed to acquire lock for first task");
exit(1);
}
}
if ( 0 != pthread_create( &( pxThreads[ lIndexOfLastAddedTask ].hThread ), &xThreadAttributes, prvWaitForStart, (void *)pxThisThreadParams ) )
{
/* Thread create failed, signal the failure */
pxTopOfStack = 0;
}
/* Wait until the task suspends. */
xSentinel=0;
(void)pthread_mutex_unlock( &xRunningThreadMutex );
while ( xSentinel == 0 ) {
sched_yield();
}
(void)pthread_mutex_lock( &xRunningThreadMutex );
hActiveThread=pthread_self();
debug_printf("ACTIVE THREAD RECLAIMED!\n");
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
} else {
debug_printf("mutex locking failed\n");
exit(1);
}
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. */
hRequestedThread=prvGetThreadHandle( xTaskGetCurrentTaskHandle());
prvResumeThread( hRequestedThread );
sched_yield();
}
/*-----------------------------------------------------------*/
/*
* See header file for description.
*/
portBASE_TYPE xPortStartScheduler( void )
{
portBASE_TYPE xResult;
int iSignal;
int fuckedUpCount=0;
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 );
sigaddset( &xSignals, SIG_TICK );
/* Allow other threads to run */
(void)pthread_mutex_unlock( &xRunningThreadMutex );
debug_printf( "MAIN thread is entering main signal wait loop!\n");
while ( pdTRUE != xSchedulerEnd )
{
if ( 0 != sigwait( &xSignals, &iSignal ) )
{
printf( "Main thread spurious signal: %d\n", iSignal );
}
/**
* Tick handler is called from here - AND ONLY FROM HERE
* (needed for cygwin - but should work on all)
*/
if (iSignal==SIG_TICK) {
if (xGeneralFuckedUpIndicator!=0 && hActiveThread!=hRequestedThread) {
fuckedUpCount++;
if (fuckedUpCount>10) {
fuckedUpCount=0;
prvResolveFuckup();
}
} else {
fuckedUpCount=0;
}
vPortSystemTickHandler(iSignal);
}
if (iSignal==SIG_RESUME && pdTRUE != xSchedulerEnd) {
debug_printf( "ALERT! Main received SIG_RESUME that was supposed to go elsewhere!");
}
}
printf( "Cleaning Up, Exiting.\n" );
/* Cleanup the mutexes */
//xResult = pthread_mutex_destroy( &xSuspendResumeThreadMutex );
//xResult = pthread_mutex_destroy( &xSingleThreadMutex );
xResult = pthread_mutex_destroy( &xRunningThreadMutex );
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;
/**
* Sentinel - do not change context while the running task is not equal the task supposed to run
*/
// if ( 0 == pthread_mutex_lock( &xSuspendResumeThreadMutex ) )
while ( 0 != pthread_mutex_trylock ( &xSuspendResumeThreadMutex ) ) {
sched_yield();
}
/**
* Make sure we don't create outdated resume signals
*/
while (hActiveThread!=hRequestedThread) {
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
sched_yield();
while (0 != pthread_mutex_trylock( &xSuspendResumeThreadMutex )) {
sched_yield();
}
}
xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
vTaskSwitchContext();
xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
if ( xTaskToSuspend != xTaskToResume )
{
/* Remember and switch the critical nesting. */
prvSetTaskCriticalNesting( xTaskToSuspend, uxCriticalNesting );
uxCriticalNesting = prvGetTaskCriticalNesting( xTaskToResume );
/* Switch tasks. */
hRequestedThread = xTaskToResume;
prvResumeThread( xTaskToResume );
//prvSuspendThread( xTaskToSuspend );
if (prvGetThreadHandle(xTaskGetCurrentTaskHandle())!=xTaskToResume) {
debug_printf("\n what the fuck???? someone else did a switchcontext?!?\n");
}
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
prvSuspendSignalHandler(SIG_SUSPEND);
return;
}
else
{
/* Yielding to self */
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
}
}
/*-----------------------------------------------------------*/
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;
debug_printf("init %li microseconds\n",(long)xMicroSeconds);
/* 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;
struct timespec timeout;
//debug_printf("received %i\n",sig);
/**
* do not call tick handler if
* - interrupts are disabled
* - tick handler is still running
* - old task switch not yet completed (wrong task running)
*/
if ( ( pdTRUE == xInterruptsEnabled ) && ( pdTRUE != xServicingTick ) && ( hRequestedThread == hActiveThread ) )
{
xServicingTick = pdTRUE;
if ( 0 == pthread_mutex_trylock( &xSuspendResumeThreadMutex ) )
{
debug_printf("does handle tick\n");
/**
* this shouldn't ever happen - but WELL...
* Make sure we don't create outdated resume signals
*/
if (hActiveThread!=hRequestedThread) {
xServicingTick = pdFALSE;
xPendYield = pdTRUE;
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
return;
}
xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
/* Tick Increment. */
vTaskIncrementTick();
/* Select Next Task. */
#if ( configUSE_PREEMPTION == 1 )
vTaskSwitchContext();
#endif
xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
//hRequestedThread = xTaskToResume;
/* The only thread that can process this tick is the running thread. */
if ( xTaskToSuspend != xTaskToResume )
{
/* Suspend the current task. */
hRequestedThread = 0;
prvSuspendThread( xTaskToSuspend );
//timeout.tv_sec=0;
//timeout.tv_nsec=10000;
//sched_yield();
while ( 0 != pthread_mutex_lock( &xRunningThreadMutex) ) {
prvSuspendThread( xTaskToSuspend );
timeout.tv_sec=0;
timeout.tv_nsec=1;
nanosleep(&timeout,0);
}
//if ( 0 == pthread_mutex_lock( &xRunningThreadMutex) ) {
/* Remember and switch the critical nesting. */
prvSetTaskCriticalNesting( xTaskToSuspend, uxCriticalNesting );
uxCriticalNesting = prvGetTaskCriticalNesting( xTaskToResume );
/* Resume next task. */
hRequestedThread = xTaskToResume;
prvResumeThread( xTaskToResume );
if (prvGetThreadHandle(xTaskGetCurrentTaskHandle())!=xTaskToResume) {
debug_printf("\n what the fuck???? someone else did a switchcontext?!?\n");
}
(void)pthread_mutex_unlock( &xRunningThreadMutex );
}
else
{
/* Release the lock as we are Resuming. */
// (void)pthread_mutex_unlock( &xSingleThreadMutex );
}
}
else
{
xPendYield = pdTRUE;
}
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
xServicingTick = pdFALSE;
}
else
{
debug_printf("will NOT handle tick\n");
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 )
{
if ( 0 == pthread_mutex_lock( &xSuspendResumeThreadMutex ) ) {
/**
* Make sure we don't create outdated resume signals
*/
while (hActiveThread!=hRequestedThread) {
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
sched_yield();
(void)pthread_mutex_lock( &xSuspendResumeThreadMutex );
}
/* This is a suicidal thread, need to select a different task to run. */
vTaskSwitchContext();
xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
hRequestedThread = xTaskToResume;
(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
} else {
debug_printf("mutex lock failed!\n");
exit(1);
}
}
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 );
(void)pthread_mutex_unlock( &xRunningThreadMutex );
/* Commit suicide */
pthread_exit( (void *)1 );
}
// }
}
/*-----------------------------------------------------------*/
void *prvWaitForStart( void * pvParams )
{
xParams * pxParams = ( xParams * )pvParams;
pdTASK_CODE pvCode = pxParams->pxCode;
void * pParams = pxParams->pvParams;
sigset_t xSignals;
vPortFree( pvParams );
pthread_cleanup_push( prvDeleteThread, (void *)pthread_self() );
if ( 0 == pthread_mutex_lock( &xRunningThreadMutex ) )
{
xSentinel=1;
hActiveThread=pthread_self();
debug_printf("temporarily made ACTIVE THREAD!\n");
sigemptyset( &xSignals );
sigaddset( &xSignals, SIG_RESUME );
/* set up proc mask */
pthread_sigmask(SIG_SETMASK,&xSignals,NULL);
prvSuspendSignalHandler(SIG_SUSPEND);
//prvSuspendThread( pthread_self() );
} else {
debug_printf("now this is just WRONG!\n");
exit(1);
}
pvCode( pParams );
pthread_cleanup_pop( 1 );
return (void *)NULL;
}
/*-----------------------------------------------------------*/
void prvSuspendSignalHandler(int sig)
{
sigset_t xSignals;
//sigset_t xPendingSignals;
/* Only interested in the resume signal. */
sigemptyset( &xSignals );
sigaddset( &xSignals, SIG_RESUME );
sigaddset( &xSignals, SIG_SUSPEND );
/* Unlock the Running thread mutex to allow the resumed task to continue. */
if ( 0 != pthread_mutex_unlock( &xRunningThreadMutex ) )
{
printf( "Releasing someone else's lock.\n" );
}
debug_printf("SUSPENDING until SIG_RESUME received\n");
/* Wait on the resume signal. */
while (hRequestedThread != pthread_self()) {
if ( 0 != sigwait( &xSignals, &sig ) )
{
//printf( "SSH: Sw %d\n", sig );
/* tricky one - shouldn't ever happen - trying to resolve situation as graceful as possible */
debug_printf("ALERT AAAAH PANIC! - sigwait failed.....\n\n\n");
/* Signal main thread something just went HORRIBLY wrong */
xGeneralFuckedUpIndicator = 2;
sched_yield();
//(void)pthread_mutex_lock( &xRunningThreadMutex );
//(void)pthread_kill( pthread_self(), SIG_SUSPEND );
//return;
} else if (sig == SIG_RESUME) {
//debug_printf("received signal %i\n",sig);
/* Make sure the right thread received the signal */
if (hRequestedThread != pthread_self() ) {
debug_printf( "ALERT! Received SIG_RESUME which is already outdated!\n active thread is %li\n",(long)hRequestedThread);
/* Signal main thread something just went wrong */
xGeneralFuckedUpIndicator = 1;
/*
if (0 == sigpending(&xPendingSignals)) {
if (sigismember(&xPendingSignals,SIG_SUSPEND)) {
debug_printf( "reason: we slept too long...\n");
//(void)sigwait(&xPendingSignals,&sig);
// we can safely return - signal is already pending
return;
}
}
debug_printf( "reason: unknown! - whatever ...\n\n");
*/
//exit(1);
//(void)pthread_kill( xTaskToResume, SIG_RESUME );
//(void)pthread_mutex_lock( &xRunningThreadMutex );
//(void)pthread_kill( pthread_self(), SIG_SUSPEND );
//return;
}
}
}
/* Yield the Scheduler to ensure that the yielding thread completes. */
if ( 0 != pthread_mutex_lock( &xRunningThreadMutex ) )
{
//(void)pthread_mutex_unlock( &xSingleThreadMutex );
debug_printf("critical - mutex acquiring of active thread failed!\n");
exit(1);
}
hActiveThread = pthread_self();
/* 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();
}
if (hRequestedThread==pthread_self()) {
/* doesn't look too bad, does it? */
xGeneralFuckedUpIndicator = 0;
}
debug_printf("ACTIVE THREAD!\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, 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;
sigresume.sa_handler = SIG_IGN;
sigfillset( &sigresume.sa_mask );
sigtick.sa_flags = 0;
//sigtick.sa_handler = vPortSystemTickHandler;
sigtick.sa_handler = SIG_IGN;
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;
}
/*-----------------------------------------------------------*/
/**
* Scheduler f***d up - we got to fix that
*/
void prvResolveFuckup( void )
{
struct timespec timeout;
pthread_t xTaskToResume;
(void)pthread_mutex_lock ( &xSuspendResumeThreadMutex);
if (hActiveThread == hRequestedThread) {
debug_printf("emergency handler started - but not needed - returning\n");
return;
}
printf("\nScheduler fucked up again - lets try to fix it...\n");
if (hRequestedThread==0) {
printf("\nno supposedly active thread - fixing\n");
xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
} else {
xTaskToResume = hRequestedThread;
}
printf("\nsending sig_suspend to thread that is supposed to be dead...\n");
prvSuspendThread(hActiveThread);
printf("\nacquire running lock...\n");
//timeout.tv_sec=0;
//timeout.tv_nsec=100;
sched_yield();
while ( 0 != pthread_mutex_trylock( &xRunningThreadMutex) ) {
prvSuspendThread(hActiveThread);
timeout.tv_sec=0;
timeout.tv_nsec=1;
nanosleep(&timeout,NULL);
}
printf("\nsending sig_resume to thread that is supposed to be running...\n");
prvResumeThread(xTaskToResume);
printf("\ngiving up mutex...\n");
(void)pthread_mutex_unlock(&xRunningThreadMutex);
(void)pthread_mutex_unlock ( &xSuspendResumeThreadMutex);
}
/*-----------------------------------------------------------*/

42
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/scheduler.txt
View File

@ -1,42 +0,0 @@
thread initial START:
master gives up running mutex
master waits for sentinel set
master akquires running mutex
thread akquires RUNNING_MUTEX
threads sets sentinel
thread suspends itself
thread SUSPEND:
thread gives up RUNNING_MUTEX
thread waits for sig_resume
thread akquires RUNNING_MUTEX
thread runs
thread END:
if other:
delete the other thread
if self:
thread gives up RUNNING_MUTEX
thread sends sig_resume to new active thread
thread self kills
thread YIELD
thread sends sig_resume to new active thread
thread suspends itself
time tick yield
tick handler sends SUSPEND to running thread
tick handler akquires RUNNING_MUTEX
tick handler sends sig_resume to new active thread
tock handler gives up RUNNING_MUTEX
tick handler ends

35
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/test_case_1_sleep.c
View File

@ -1,35 +0,0 @@
/**
* small test program whether signals between threads work as they should
*/
#include <unistd.h>
#include <pthread.h>
#include <signal.h>
void sighandler(int sig) {
write(2,".",1);
return;
}
void* threadstart(void* arg) {
while (1) {
sleep(1);
}
}
int main(char** argc, int argv) {
pthread_t testthread1;
struct sigaction action;
action.sa_handler=sighandler;
action.sa_flags=0;
sigfillset( &action.sa_mask );
sigaction(SIGUSR1,&action,NULL);
pthread_create(&testthread1,NULL,threadstart,NULL);
while (1) {
pthread_kill(testthread1,SIGUSR1);
}
}

38
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/test_case_2_nanosleep.c
View File

@ -1,38 +0,0 @@
/**
* small test program whether signals between threads work as they should
*/
#include <time.h>
#include <pthread.h>
#include <signal.h>
void sighandler(int sig) {
write(2,".",1);
return;
}
void* threadstart(void* arg) {
struct timespec sleeptime;
while (1) {
sleeptime.tv_sec=1;
sleeptime.tv_nsec=0;
nanosleep(&sleeptime,NULL);
}
}
int main(char** argc, int argv) {
pthread_t testthread1;
struct sigaction action;
action.sa_handler=sighandler;
action.sa_flags=0;
sigfillset( &action.sa_mask );
sigaction(SIGUSR1,&action,NULL);
pthread_create(&testthread1,NULL,threadstart,NULL);
while (1) {
pthread_kill(testthread1,SIGUSR1);
}
}

40
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/test_case_3_select.c
View File

@ -1,40 +0,0 @@
/**
* small test program whether signals between threads work as they should
*/
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <pthread.h>
#include <signal.h>
void sighandler(int sig) {
write(2,".",1);
return;
}
void* threadstart(void* arg) {
struct timeval sleeptime;
while (1) {
sleeptime.tv_sec=1;
sleeptime.tv_usec=0;
select(0,NULL,NULL,NULL,&sleeptime);
}
}
int main(char** argc, int argv) {
pthread_t testthread1;
struct sigaction action;
action.sa_handler=sighandler;
action.sa_flags=0;
sigfillset( &action.sa_mask );
sigaction(SIGUSR1,&action,NULL);
pthread_create(&testthread1,NULL,threadstart,NULL);
while (1) {
pthread_kill(testthread1,SIGUSR1);
}
}

41
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/test_case_4_mutex_lock.c
View File

@ -1,41 +0,0 @@
/**
* small etst program whether signals between threads work as they should
*/
#include <pthread.h>
#include <signal.h>
static pthread_mutex_t Mutex = PTHREAD_MUTEX_INITIALIZER;
/**
* actual test program
*/
void sighandler(int sig) {
write(2,".",1);
return;
}
void* threadstart(void* arg) {
while (1) {
pthread_mutex_lock(&Mutex);
}
}
int main(char** argc, int argv) {
pthread_t testthread1;
struct sigaction action;
action.sa_handler=sighandler;
action.sa_flags=0;
sigfillset( &action.sa_mask );
sigaction(SIGUSR1,&action,NULL);
pthread_mutex_lock(&Mutex);
pthread_create(&testthread1,NULL,threadstart,NULL);
while (1) {
pthread_kill(testthread1,SIGUSR1);
}
}

42
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/test_case_5_pselect.c
View File

@ -1,42 +0,0 @@
/**
* small test program whether signals between threads work as they should
*/
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <pthread.h>
#include <signal.h>
void sighandler(int sig) {
write(2,".",1);
return;
}
void* threadstart(void* arg) {
sigset_t sigset;
struct timespec sleeptime;
while (1) {
sleeptime.tv_sec=1;
sleeptime.tv_nsec=0;
sigemptyset(&sigset);
pselect(0,NULL,NULL,NULL,&sleeptime,&sigset);
}
}
int main(char** argc, int argv) {
pthread_t testthread1;
struct sigaction action;
action.sa_handler=sighandler;
action.sa_flags=0;
sigfillset( &action.sa_mask );
sigaction(SIGUSR1,&action,NULL);
pthread_create(&testthread1,NULL,threadstart,NULL);
while (1) {
pthread_kill(testthread1,SIGUSR1);
}
}

35
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/test_case_6_sched_yield.c
View File

@ -1,35 +0,0 @@
/**
* small test program whether signals between threads work as they should
*/
#include <unistd.h>
#include <pthread.h>
#include <signal.h>
void sighandler(int sig) {
write(2,".",1);
return;
}
void* threadstart(void* arg) {
while (1) {
sched_yield();
}
}
int main(char** argc, int argv) {
pthread_t testthread1;
struct sigaction action;
action.sa_handler=sighandler;
action.sa_flags=0;
sigfillset( &action.sa_mask );
sigaction(SIGUSR1,&action,NULL);
pthread_create(&testthread1,NULL,threadstart,NULL);
while (1) {
pthread_kill(testthread1,SIGUSR1);
}
}

34
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/test_case_7_busy_waiting.c
View File

@ -1,34 +0,0 @@
/**
* small test program whether signals between threads work as they should
*/
#include <unistd.h>
#include <pthread.h>
#include <signal.h>
void sighandler(int sig) {
write(2,".",1);
return;
}
void* threadstart(void* arg) {
while (1) {
}
}
int main(char** argc, int argv) {
pthread_t testthread1;
struct sigaction action;
action.sa_handler=sighandler;
action.sa_flags=0;
sigfillset( &action.sa_mask );
sigaction(SIGUSR1,&action,NULL);
pthread_create(&testthread1,NULL,threadstart,NULL);
while (1) {
pthread_kill(testthread1,SIGUSR1);
}
}

36
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/test_case_8_io.c
View File

@ -1,36 +0,0 @@
/**
* small test program whether signals between threads work as they should
*/
#include <unistd.h>
#include <pthread.h>
#include <signal.h>
void sighandler(int sig) {
write(2,".",1);
return;
}
void* threadstart(void* arg) {
char buf[1024];
while (1) {
read(1,buf,512);
}
}
int main(char** argc, int argv) {
pthread_t testthread1;
struct sigaction action;
action.sa_handler=sighandler;
action.sa_flags=0;
sigfillset( &action.sa_mask );
sigaction(SIGUSR1,&action,NULL);
pthread_create(&testthread1,NULL,threadstart,NULL);
while (1) {
pthread_kill(testthread1,SIGUSR1);
}
}

62
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/thread_signal_test.c
View File

@ -1,62 +0,0 @@
/**
* small etst program whether signals between threads work as they should
*/
#include <stdio.h>
#include <time.h>
#include <pthread.h>
#include <signal.h>
static pthread_mutex_t Mutex = PTHREAD_MUTEX_INITIALIZER;
void sighandler(int sig) {
printf("signal handler called in thread %li - signal %i\n",(long)pthread_self(),sig);
}
void* threadstart(void* arg) {
struct timespec timeout;
struct sigaction action;
printf("thread %li started\n",(long)pthread_self());
printf("installing signal handler\n");
action.sa_handler=sighandler;
action.sa_flags=0;
sigfillset( &action.sa_mask );
sigaction(SIGUSR1,&action,NULL);
printf("getting mutex\n");
pthread_mutex_lock(&Mutex);
printf("got mutex\n");
while (1) {
timeout.tv_sec=1;
timeout.tv_nsec=0;
nanosleep(&timeout,0);
printf("thread %li still running...\n",(long)pthread_self());
}
}
int main(char** argc, int argv) {
pthread_t testthread1;
pthread_t testthread2;
printf("thread test program\n");
sleep(5);
printf("starting thread 1\n");
pthread_create(&testthread1,NULL,threadstart,NULL);
sleep(5);
printf("starting thread 2\n");
pthread_create(&testthread2,NULL,threadstart,NULL);
while (1) {
sleep(5);
printf("sending SIG_USR1 to thread 1\n");
pthread_kill(testthread1,SIGUSR1);
sleep(5);
printf("sending SIG_USR1 to thread 2\n");
pthread_kill(testthread2,SIGUSR1);
}
}

128
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/thread_signal_test2.c
View File

@ -1,128 +0,0 @@
/**
* small etst program whether signals between threads work as they should
*/
#include <stdio.h>
#include <unistd.h>
#include <time.h>
#include <pthread.h>
#include <signal.h>
static pthread_mutex_t Mutex = PTHREAD_MUTEX_INITIALIZER;
/**
* printf is not thread safe - this little helper function allow debug output in thread safe context
*/
/* write a string to a filehandle */
void print_char(int fh, char *c) {
int t=0;
while (c[t]!=0) t++;
write(fh,c,t);
}
/* create a dezimal string from an integer */
int int2char(char* x,unsigned long i) {
if (i==0) {
return 0;
}
int k=int2char(&x[0],i/10);
x[k]='0'+(i%10);
x[k+1]=0;
return k+1;
}
/* print a number*/
void print_number(int fh, long i) {
char buffer[39]; // 39 characters are enough to store a 128 bit integer in dezimal notation (~~3.4* 10^38)
if (i==0) {
print_char(fh,"0");
} else {
if (i<0) {
buffer[0]='-';
int2char(&buffer[1],-i);
} else {
int2char(buffer,i);
}
print_char(fh,buffer);
}
}
/**
* actual test program
*/
void sighandler(int sig) {
print_char(2,"signal handler called in thread ");
print_number(2,(long)pthread_self());
print_char(2," - signal ");
print_number(2,sig);
print_char(2,"\n");
}
void* threadstart(void* arg) {
struct timespec timeout;
int t;
print_char(2,"thread ");
print_number(2,(long)pthread_self());
print_char(2," started \n");
while (1) {
print_char(2,"getting mutex\n");
pthread_mutex_lock(&Mutex);
print_char(2,"got mutex\n");
for (t=0;t<20;t++) {
timeout.tv_sec=1;
timeout.tv_nsec=0;
nanosleep(&timeout,0);
print_char(2,"thread ");
print_number(2,(long)pthread_self());
print_char(2," still running...\n");
}
pthread_mutex_unlock(&Mutex);
sleep(1);
}
}
int main(char** argc, int argv) {
pthread_t testthread1;
pthread_t testthread2;
struct sigaction action;
print_char(2,"thread test program\n\n");
print_char(2,"demonstrate print function\n");
long t=1;
while (t!=0) {
print_number(2,t);
print_char(2," >> ");
t=(t>0)?t*2:t/2;
}
print_number(2,t);
print_char(2,"\ndone\n\n");
print_char(2,"installing signal handler\n");
action.sa_handler=sighandler;
action.sa_flags=0;
sigfillset( &action.sa_mask );
sigaction(SIGUSR1,&action,NULL);
sleep(5);
print_char(2,"starting thread 1\n");
pthread_create(&testthread1,NULL,threadstart,NULL);
sleep(5);
print_char(2,"starting thread 2\n");
pthread_create(&testthread2,NULL,threadstart,NULL);
while (1) {
sleep(5);
print_char(2,"sending SIG_USR1 to thread 1\n");
pthread_kill(testthread1,SIGUSR1);
sleep(5);
print_char(2,"sending SIG_USR1 to thread 2\n");
pthread_kill(testthread2,SIGUSR1);
}
}

46
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/thread_signal_test3.c
View File

@ -1,46 +0,0 @@
/**
* small etst program whether signals between threads work as they should
*/
#include <time.h>
#include <pthread.h>
#include <signal.h>
/**
* actual test program
*/
void sighandler(int sig) {
return;
}
void* threadstart(void* arg) {
struct timespec timeout;
int t;
while (1) {
timeout.tv_sec=0;
timeout.tv_nsec=1;
nanosleep(&timeout,0);
}
}
int main(char** argc, int argv) {
pthread_t testthread1;
struct sigaction action;
action.sa_handler=sighandler;
action.sa_flags=0;
sigfillset( &action.sa_mask );
sigaction(SIGUSR1,&action,NULL);
pthread_create(&testthread1,NULL,threadstart,NULL);
while (1) {
pthread_kill(testthread1,SIGUSR1);
}
}

1179
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/peabody124/port-pthread_cond.c
View File

@ -1,1179 +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 <pthread.h>
#include <sched.h>
#include <signal.h>
#include <errno.h>
#include <sys/time.h>
#include <time.h>
#include <sys/times.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <limits.h>
#include <assert.h>
/* Scheduler includes. */
#include "FreeRTOS.h"
#include "task.h"
/*-----------------------------------------------------------*/
#define MAX_NUMBER_OF_TASKS ( _POSIX_THREAD_THREADS_MAX )
/*-----------------------------------------------------------*/
#ifdef __APPLE__
#define COND_SIGNALING
#define CHECK_TASK_RESUMES
#define RUNNING_THREAD_MUTEX
#define TICK_SIGNAL
#endif
#ifdef __CYGWIN__
#define COND_SIGNALING
#define CHECK_TASK_RESUMES
#endif
#ifdef __linux__
#define COND_SIGNALING
// #define CHECK_TASK_RESUMES
#define RUNNING_THREAD_MUTEX
#define TICK_SIGNAL
#endif
/* 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 = 0;
static volatile portBASE_TYPE xRunning = pdFALSE;
static volatile portBASE_TYPE xSuspended = pdFALSE;
static volatile portBASE_TYPE xStarted = pdFALSE;
static volatile portBASE_TYPE xHandover = 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 *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
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 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() );
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 );
(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);
#ifdef TICK_SIGNAL
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;
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();
}
#else
struct timespec x;
while( pdTRUE != xSchedulerEnd ) {
x.tv_sec=0;
x.tv_nsec=portTICK_RATE_MICROSECONDS * 1000;
nanosleep(&x,NULL);
// 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 )
{
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;
sigset_t xSignals;
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" );
sigemptyset( &xSignals );
sigaddset( &xSignals, SIG_SUSPEND );
sigaddset( &xSignals, SIG_TICK );
pthread_sigmask( SIG_SETMASK, &xSignals, NULL );
oldTask = xTaskGetCurrentTaskHandle();
xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
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" );
xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
if( prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) != pthread_self() ) {
debug_printf( "The current task isn't even us. Pausing now, deal with possible interrupt later.\r\n" );
pauseThread( THREAD_PAUSE_YIELD );
/* Now we are resuming, want to be able to catch this interrupt again */
sigemptyset( &xSignals );
sigaddset( &xSignals, SIG_TICK );
pthread_sigmask( SIG_SETMASK, &xSignals, NULL);
return;
}
sched_yield();
retVal = pthread_mutex_trylock( &xSwappingThreadMutex );
}
/* At this point we have the lock, active task shouldn't change */
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);
pauseThread( THREAD_PAUSE_YIELD );
/* Now we are resuming, want to be able to catch this interrupt again */
sigemptyset( &xSignals );
sigaddset( &xSignals, SIG_TICK );
pthread_sigmask( SIG_SETMASK, &xSignals, NULL);
return;
}
assert( xTaskToSuspend == pthread_self() ); // race condition I didn't account for
xStarted = pdFALSE;
/* Get new task then release the task switching mutex */
vTaskSwitchContext();
newTask = xTaskGetCurrentTaskHandle();
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( xStarted == pdFALSE )
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 */
sigemptyset( &xSignals );
sigaddset( &xSignals, SIG_TICK );
pthread_sigmask( SIG_SETMASK, &xSignals, NULL);
}
/*-----------------------------------------------------------*/
void vPortDisableInterrupts( void )
{
//debug_printf("\r\n");
xInterruptsEnabled = pdFALSE;
}
/*-----------------------------------------------------------*/
void vPortEnableInterrupts( void )
{
//debug_printf("\r\n");
xInterruptsEnabled = pdTRUE;
}
/*-----------------------------------------------------------*/
portBASE_TYPE xPortSetInterruptMask( void )
{
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;
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();
xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
/* Need to set this before updating the task in case it notices before the */
/* interrupt is set */
xSuspended = pdFALSE;
xStarted = pdFALSE;
/* Tick Increment. */
vTaskIncrementTick();
/* Select Next Task. */
#if ( configUSE_PREEMPTION == 1 )
vTaskSwitchContext();
#endif
newTask = xTaskGetCurrentTaskHandle();
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( xSuspended == pdFALSE )
#endif
{
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( xStarted == pdFALSE)
#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 ) )
{
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 );
/* 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 */
debug_printf("Thread started, waiting till handle is added\r\n");
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");
sigemptyset( &xSignals );
sigaddset( &xSignals, SIG_TICK );
assert( pthread_sigmask( SIG_SETMASK, &xSignals, NULL ) == 0);
pvCode( pParams );
pthread_cleanup_pop( 1 );
return (void *)NULL;
}
/*-----------------------------------------------------------*/
void pauseThread( portBASE_TYPE pauseMode )
{
debug_printf( "Pausing thread %li. Set xSuspended true\r\n", (long int) pthread_self() );
xSuspended = pdTRUE;
#ifdef RUNNING_THREAD_MUTEX
if( pauseMode != THREAD_PAUSE_CREATED )
assert( 0 == pthread_mutex_unlock( &xRunningThread ) );
#endif
#ifdef COND_SIGNALING
int xResult;
xTaskHandle hTask = prvGetTaskHandle( pthread_self() );
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) {
if( pthread_self() == prvGetThreadHandle(xTaskGetCurrentTaskHandle() ) && xRunning )
{
xStarted = pdTRUE;
#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 */
if( pthread_self() == prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) ) {
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 );
sigaddset( &xBlockSignals, SIG_TICK );
assert( pthread_sigmask( SIG_SETMASK, &xBlockSignals, NULL ) == 0);
while( pthread_self() != prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) )
{
debug_printf( "Incorrectly woke up. Repausing\r\n" );
pauseThread( THREAD_PAUSE_INTERRUPT );
}
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_TICK );
assert( 0 == pthread_sigmask( SIG_SETMASK, &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
{
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;
}
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 )
{
xTaskHandle hTask = NULL;
portLONG lIndex;
/* If not initialized yet */
if( pxThreads == NULL ) return NULL;
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
{
if ( pxThreads[ lIndex ].hThread == hThread )
{
hTask = pxThreads[ lIndex ].hTask;
break;
}
}
return hTask;
}
/*-----------------------------------------------------------*/
pthread_cond_t * prvGetConditionHandle( xTaskHandle hTask )
{
pthread_cond_t * hCond;
portLONG lIndex;
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
{
if ( pxThreads[ lIndex ].hTask == hTask )
{
debug_printf( "Found condition on %i task\r\n", lIndex );
return pxThreads[ lIndex ].hCond;
break;
}
}
printf( "Failed to get handle, pausing then recursing\r\n" );
usleep(1000);
return prvGetConditionHandle( hTask );
assert(0);
return hCond;
}
/*-----------------------------------------------------------*/
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;
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;
}
/*-----------------------------------------------------------*/

1109
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/peabody124/port-stupidlysimplerversion.c
View File

@ -1,1109 +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 <pthread.h>
#include <sched.h>
#include <signal.h>
#include <errno.h>
#include <sys/time.h>
#include <time.h>
#include <sys/times.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <limits.h>
#include <assert.h>
/* Scheduler includes. */
#include "FreeRTOS.h"
#include "task.h"
/*-----------------------------------------------------------*/
#define MAX_NUMBER_OF_TASKS ( _POSIX_THREAD_THREADS_MAX )
/*-----------------------------------------------------------*/
#define DB_P(x) // x
/* 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;
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 xRunningThread = PTHREAD_MUTEX_INITIALIZER;
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 xHandover = 0;
static volatile portBASE_TYPE xSchedulerEnd = pdFALSE;
static volatile portBASE_TYPE xInterruptsEnabled = pdTRUE;
static volatile portBASE_TYPE xInterruptsCurrent = 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 prvResumeSignalHandler(int sig);
static void prvSetupSignalsAndSchedulerPolicy( void );
static void prvSuspendThread( pthread_t xThreadId );
static void pauseThread();
//static void prvResumeThread( pthread_t xThreadId );
static pthread_t prvGetThreadHandle( xTaskHandle hTask );
static pthread_cond_t prvGetThreadCondition( xTaskHandle hTask );
static pthread_mutex_t prvGetThreadMutex( xTaskHandle hTask );
static xTaskHandle prvGetThreadTask( 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 DEBUG_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_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 )
#else
#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(...)
#endif
//#define debug_printf(...) fprintf( stderr, __VA_ARGS__ );
/*
* 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;
tskTCB * prvGetTaskHandle( pthread_t hThread )
{
portLONG lIndex;
if (pxThreads==NULL) return NULL;
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
{
if ( pxThreads[ lIndex ].hThread == hThread )
{
return pxThreads[ lIndex ].hTask;
}
}
return NULL;
}
/*
* 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();
} /*else {
sigset_t xSignals;
sigemptyset( &xSignals );
sigaddset( &xSignals, SIG_TICK );
pthread_sigmask( SIG_BLOCK, &xSignals, NULL );
} */
/* 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 a condition signal for this thread */
/* pthread_condattr_t condAttr;
assert( 0 == pthread_condattr_init(&condAttr) );
assert( 0 == pthread_cond_init( &( pxThreads[ lIndexOfLastAddedTask ].hCond ), &condAttr ) ); */
/* 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 ) );
assert( 0 == pthread_mutex_init( &( pxThreads[ lIndexOfLastAddedTask ].hMutex ), &mutexAttr ) );*/
/* 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();
/* Start the first task. */
//prvResumeThread( prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) );
debug_printf( "Sending resume signal to %li\r\n", (long int) prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) );
// pthread_kill( prvGetThreadHandle( xTaskGetCurrentTaskHandle() ), SIG_RESUME );
// Set resume condition for specific thread
//pthread_cond_t hCond = prvGetThreadCondition( xTaskGetCurrentTaskHandle() );
//assert( pthread_cond_signal( &hCond ) == 0 );
}
/*-----------------------------------------------------------*/
/*
* See header file for description.
*/
portBASE_TYPE xPortStartScheduler( void )
{
portBASE_TYPE xResult;
sigset_t xSignals;
sigset_t xSignalToBlock;
sigset_t xSignalsBlocked;
portLONG lIndex;
debug_printf( "xPortStartScheduler\r\n" );
/* Establish the signals to block before they are needed. */
sigfillset( &xSignalToBlock );
sigaddset( &xSignalToBlock, SIG_SUSPEND );
/* sigaddset( &xSignalToBlock, SIG_RESUME );
sigaddset( &xSignalToBlock, SIG_TICK ); */
/* 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 ) {
struct timespec x;
x.tv_sec=0;
x.tv_nsec=1000000;
nanosleep(&x,NULL);
printf("."); fflush(stdout);
vPortSystemTickHandler(SIG_TICK);
}
debug_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;
DB_P("vPortEndScheduler\r\n");
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.
*/
DB_P("vPortYieldFromISR\r\n");
xPendYield = pdTRUE;
}
/*-----------------------------------------------------------*/
void vPortEnterCritical( void )
{
DB_P("vPortEnterCritical\r\n");
vPortDisableInterrupts();
uxCriticalNesting++;
}
/*-----------------------------------------------------------*/
void vPortExitCritical( void )
{
DB_P("vPortExitCritical\r\n");
/* 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;
sigset_t xSignals;
/* 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 */
sigemptyset( &xSignals );
sigaddset( &xSignals, SIG_SUSPEND );
pthread_sigmask( SIG_SETMASK, &xSignals, NULL );
assert( pthread_mutex_lock( &xSingleThreadMutex ) == 0);
xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
if(xTaskToSuspend != pthread_self() ) {
/* This means between masking the interrupt and getting the lock, there was an interrupt */
/* and this task should suspend. Release the lock, then unmask interrupts to go ahead and */
/* service the signal */
assert( 0 == pthread_mutex_unlock( &xSingleThreadMutex ) );
debug_printf( "The current task isn't even us, letting interrupt happen\r\n" );
/* Now we are resuming, want to be able to catch this interrupt again */
sigemptyset( &xSignals );
pthread_sigmask( SIG_SETMASK, &xSignals, NULL);
return;
}
/* Get new task then release the task switching mutex */
vTaskSwitchContext();
xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
assert( pthread_mutex_unlock( &xSingleThreadMutex ) == 0);
if ( pthread_self() != xTaskToResume )
{
/* Remember and switch the critical nesting. */
prvSetTaskCriticalNesting( xTaskToSuspend, uxCriticalNesting );
uxCriticalNesting = prvGetTaskCriticalNesting( xTaskToResume );
/* pausing self if switching tasks, can now rely on other thread waking itself up */
pauseThread();
}
/* Now we are resuming, want to be able to catch this interrupt again */
sigemptyset( &xSignals );
pthread_sigmask( SIG_SETMASK, &xSignals, NULL);
}
/*-----------------------------------------------------------*/
void vPortDisableInterrupts( void )
{
DB_P("vPortDisableInterrupts\r\n");
xInterruptsEnabled = pdFALSE;
}
/*-----------------------------------------------------------*/
void vPortEnableInterrupts( void )
{
DB_P("vPortEnableInterrupts\r\n");
xInterruptsEnabled = pdTRUE;
}
/*-----------------------------------------------------------*/
portBASE_TYPE xPortSetInterruptMask( void )
{
portBASE_TYPE xReturn = xInterruptsEnabled;
DB_P("vPortsetInterruptMask\r\n");
xInterruptsEnabled = pdFALSE;
return xReturn;
}
/*-----------------------------------------------------------*/
void vPortClearInterruptMask( portBASE_TYPE xMask )
{
DB_P("vPortClearInterruptMask\r\n");
xInterruptsEnabled = xMask;
}
/*-----------------------------------------------------------*/
void vPortSystemTickHandler( int sig )
{
pthread_t xTaskToSuspend;
pthread_t xTaskToResume;
portBASE_TYPE xInterruptValue;
tskTCB * oldTask, * newTask;
int lockResult;
DB_P("vPortSystemTickHandler");
xInterruptValue = xInterruptsEnabled;
debug_printf( "\r\n\r\n" );
debug_printf( "(xInterruptsEnabled = %i, xServicingTick = %i)\r\n", (int) xInterruptValue != 0, (int) xServicingTick != 0);
if ( ( pdTRUE == xInterruptValue ) && ( pdTRUE != xServicingTick ) )
{
debug_printf( "Checking for lock ...\r\n" );
lockResult = pthread_mutex_trylock( &xSingleThreadMutex );
// lockResult = 0;
debug_printf( "Done\r\n" );
if ( 0 == lockResult)
{
debug_printf( "Handling\r\n");
xServicingTick = pdTRUE;
oldTask = xTaskGetCurrentTaskHandle();
xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
/* Tick Increment. */
vTaskIncrementTick();
/* Select Next Task. */
#if ( configUSE_PREEMPTION == 1 )
vTaskSwitchContext();
#endif
newTask = xTaskGetCurrentTaskHandle();
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 );
debug_printf( "Swapping From %li(%s) to %li(%s)\r\n", (long int) xTaskToSuspend, oldTask->pcTaskName, (long int) xTaskToResume, newTask->pcTaskName);
prvSuspendThread( xTaskToSuspend ); /* Suspend the current task. */
debug_printf( "Swapped From %li(%s) to %li(%s)\r\n", (long int) xTaskToSuspend, oldTask->pcTaskName, (long int) xTaskToResume, newTask->pcTaskName);
}
xServicingTick = pdFALSE;
(void)pthread_mutex_unlock( &xSingleThreadMutex );
}
else
{
debug_printf( "Pending yield here (portYield has lock - hopefully)\r\n" );
xPendYield = pdTRUE;
}
}
else
{
debug_printf( "Pending yield or here\r\n");
xPendYield = pdTRUE;
}
}
/*-----------------------------------------------------------*/
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( &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 );
assert( 0 );
/* 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() );
xSentinel = 1; // tell creating block to resume
/* want to block suspend when not the active thread */
sigset_t xSignals;
sigemptyset( &xSignals );
sigaddset( &xSignals, SIG_SUSPEND );
assert( pthread_sigmask( SIG_BLOCK, &xSignals, NULL ) == 0);
debug_printf( "Delaying before the pause to make sure system knows about task\r\n" );
struct timespec x;
x.tv_sec=0;
x.tv_nsec=10000000;
nanosleep(&x,NULL);
// wait for resume signal
debug_printf("Pausing newly created thread\r\n");
// pthread_mutex_lock( &xRunningThread );
// pauseThread();
while (1) {
if( pthread_self() == prvGetThreadHandle(xTaskGetCurrentTaskHandle() ) )
{
// assert( 0 == pthread_mutex_lock( &xRunningThread ) );
break;
}
else {
struct timespec x;
x.tv_sec=0;
x.tv_nsec=100000;
nanosleep(&x,NULL);
sched_yield();
}
}
// no longer want to block any signals
sigemptyset( &xSignals );
assert( pthread_sigmask( SIG_SETMASK, &xSignals, NULL ) == 0);
debug_printf("Starting first run\r\n");
//xHandover = 1;
//assert ( 0 == pthread_mutex_lock( &xSingleThreadMutex ) )
pvCode( pParams );
pthread_cleanup_pop( 1 );
return (void *)NULL;
}
/*-----------------------------------------------------------*/
void pauseThread()
{
debug_printf("\r\n");
// assert( 0 == pthread_mutex_unlock( &xRunningThread ) );
while (1) {
if( pthread_self() == prvGetThreadHandle(xTaskGetCurrentTaskHandle() ) )
{
// assert( 0 == pthread_mutex_lock( &xRunningThread ) );
return;
}
else {
struct timespec x;
x.tv_sec=0;
x.tv_nsec=100000;
nanosleep(&x,NULL);
sched_yield();
}
}
xTaskHandle hTask = prvGetThreadTask( pthread_self() );
pthread_cond_t hCond = prvGetThreadCondition( hTask );
pthread_mutex_t hMutex = prvGetThreadMutex( hTask );
// while( !shouldResume ) {
pthread_cond_wait( &hCond, &hMutex );
assert( 0 == pthread_mutex_lock( &xRunningThread ) );
// }
}
void prvSuspendSignalHandler(int sig)
{
sigset_t xBlockSignals;
debug_printf( "Caught signal %i\r\n", sig );
/* Check that we aren't suspending when we should be running. This bug would need tracking down */
//assert( pthread_self() != prvGetThreadHandle(xTaskGetCurrentTaskHandle() ) );
if( pthread_self() == prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) ) return;
/* 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);
/* Unlock the Single thread mutex to allow the resumed task to continue. */
//assert ( 0 == pthread_mutex_unlock( &xSingleThreadMutex ) );
// Set resume condition for specific thread
/* pthread_cond_t hCond = prvGetThreadCondition( xTaskGetCurrentTaskHandle() );
assert( pthread_cond_signal( &hCond ) == 0 ); */
pauseThread();
while( pthread_self() != prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) )
{
debug_printf( "Incorrectly woke up. Repausing\r\n" );
pauseThread();
}
/* Make sure the right thread is resuming */
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 );
pthread_sigmask( SIG_SETMASK, &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
{
vPortDisableInterrupts();
}
debug_printf("Exit\r\n");
}
/*-----------------------------------------------------------*/
void prvSuspendThread( pthread_t xThreadId )
{
debug_printf( "Suspending %li\r\n", (long int) xThreadId);
/* Set-up for the Suspend Signal handler? */
xSentinel = 0;
if( pthread_self() == xThreadId ) {
debug_printf( "Pausing self, no reason to call signal handler\r\n" );
pauseThread();
}
else {
debug_printf( "About to kill %li\r\n", (long int) xThreadId );
assert( pthread_kill( xThreadId, SIG_SUSPEND ) == 0);
debug_printf( "Killed %li\r\n", (long int) xThreadId );
}
// while ( ( xSentinel == 0 ) && ( pdTRUE != xServicingTick ) )
{
// debug_printf( "sched_yield()\r\n" );
sched_yield();
}
}
/*-----------------------------------------------------------*/
/*void prvResumeSignalHandler(int sig)
{
DB_P("prvResumeSignalHandler\r\n");
debug_printf( "prvResumeSignalHandler getLock");
while(1);
// Yield the Scheduler to ensure that the yielding thread completes.
if ( 0 == pthread_mutex_lock( &xSingleThreadMutex ) )
{
debug_printf( "prvResumeSignalHandler: unlocking xSingleThreadMutex (%li)\r\n", (long int) pthread_self());
(void)pthread_mutex_unlock( &xSingleThreadMutex );
}
}*/
/*-----------------------------------------------------------*/
/*void prvResumeThread( pthread_t xThreadId )
{
portBASE_TYPE xResult;
DB_P("prvResumeThread\r\n");
debug_printf( "getLock\r\n" );
if ( 0 == pthread_mutex_lock( &xSuspendResumeThreadMutex ) )
{
debug_printf( "Resuming %li\r\n", (long int) xThreadId );
if ( pthread_self() != xThreadId )
{
//xResult = pthread_kill( xThreadId, SIG_RESUME );
debug_printf( "No longer doing anything. Suspend handler for previous thread should start %li\r\n", (long int) xThreadId );
}
else {
debug_printf( "Thread attempting to resume itself. This is not expected behavior\r\n" );
kill( getpid(), SIGKILL );
}
xResult = pthread_mutex_unlock( &xSuspendResumeThreadMutex );
}
else {
debug_printf("Error getting lock to resume thread\r\n");
kill( getpid(), SIGKILL );
}
} */
/*-----------------------------------------------------------*/
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;
}
sigsuspendself.sa_flags = 0;
sigsuspendself.sa_handler = prvSuspendSignalHandler;
sigfillset( &sigsuspendself.sa_mask );
assert ( 0 == sigaction( SIG_SUSPEND, &sigsuspendself, NULL ) );
}
/*-----------------------------------------------------------*/
pthread_mutex_t prvGetThreadMutex( 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 prvGetThreadTask( pthread_t hThread )
{
xTaskHandle hTask;
portLONG lIndex;
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
{
if ( pxThreads[ lIndex ].hThread == hThread )
{
hTask = pxThreads[ lIndex ].hTask;
break;
}
}
return hTask;
}
/*-----------------------------------------------------------*/
pthread_cond_t prvGetThreadCondition( xTaskHandle hTask )
{
pthread_cond_t hCond;
portLONG lIndex;
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
{
if ( pxThreads[ lIndex ].hTask == hTask )
{
hCond = pxThreads[ lIndex ].hCond;
break;
}
}
return hCond;
}
/*-----------------------------------------------------------*/
pthread_t prvGetThreadHandle( xTaskHandle hTask )
{
pthread_t hThread = ( pthread_t )NULL;
portLONG lIndex;
DB_P("prvGetThreadHandle\r\n");
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;
DB_P("prvGetFreeThreadState\r\n");
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;
DB_P("prvSetTaskCriticalNesting\r\n");
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;
DB_P("prvGetTaskCriticalNesting\r\n");
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;
DB_P("prvDeleteThread\r\n");
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;
}
}
}
}
/*-----------------------------------------------------------*/
void vPortFindTicksPerSecond( void )
{
DB_P("vPortFindTicksPerSecond\r\n");
/* 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;
DB_P("ulPortGetTimerValue\r\n");
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;
}
/*-----------------------------------------------------------*/

1085
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/peabody124/port.c
View File

@ -1,1085 +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 <pthread.h>
#include <sched.h>
#include <signal.h>
#include <errno.h>
#include <sys/time.h>
#include <time.h>
#include <sys/times.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <limits.h>
#include <assert.h>
/* Scheduler includes. */
#include "FreeRTOS.h"
#include "task.h"
/*-----------------------------------------------------------*/
#define MAX_NUMBER_OF_TASKS ( _POSIX_THREAD_THREADS_MAX )
/*-----------------------------------------------------------*/
#ifndef __CYGWIN__
#define COND_SIGNALING
#define CHECK_TASK_RESUMES
#define RUNNING_THREAD_MUTEX
#endif
/* 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 = 0;
static volatile portBASE_TYPE xRunning = pdFALSE;
static volatile portBASE_TYPE xSuspended = pdFALSE;
static volatile portBASE_TYPE xStarted = pdFALSE;
static volatile portBASE_TYPE xHandover = 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 *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
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 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_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() );
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. */
sigfillset( &xSignalToBlock );
sigaddset( &xSignalToBlock, SIG_SUSPEND );
/* Block until the end */
(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();
struct timespec x;
usleep(1000000);
while( pdTRUE != xSchedulerEnd ) {
x.tv_sec=0;
x.tv_nsec=portTICK_RATE_MICROSECONDS * 1000;
nanosleep(&x,NULL);
//printf("."); fflush(stdout);
vPortSystemTickHandler(SIG_TICK);
}
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 )
{
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;
sigset_t xSignals;
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" );
sigemptyset( &xSignals );
sigaddset( &xSignals, SIG_SUSPEND );
pthread_sigmask( SIG_SETMASK, &xSignals, NULL );
assert( pthread_mutex_lock( &xSwappingThreadMutex ) == 0);
oldTask = xTaskGetCurrentTaskHandle();
xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
if(xTaskToSuspend != pthread_self() ) {
/* This means between masking the interrupt and getting the lock, there was an interrupt */
/* and this task should suspend. Release the lock, then unmask interrupts to go ahead and */
/* service the signal */
assert( 0 == pthread_mutex_unlock( &xSwappingThreadMutex ) );
debug_printf( "The current task isn't even us, letting interrupt happen. Watch for swap.\r\n" );
/* Now we are resuming, want to be able to catch this interrupt again */
sigemptyset( &xSignals );
pthread_sigmask( SIG_SETMASK, &xSignals, NULL);
return;
}
xStarted = pdFALSE;
/* Get new task then release the task switching mutex */
vTaskSwitchContext();
newTask = xTaskGetCurrentTaskHandle();
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( xStarted == pdFALSE )
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 */
sigemptyset( &xSignals );
pthread_sigmask( SIG_SETMASK, &xSignals, NULL);
}
/*-----------------------------------------------------------*/
void vPortDisableInterrupts( void )
{
//debug_printf("\r\n");
xInterruptsEnabled = pdFALSE;
}
/*-----------------------------------------------------------*/
void vPortEnableInterrupts( void )
{
//debug_printf("\r\n");
xInterruptsEnabled = pdTRUE;
}
/*-----------------------------------------------------------*/
portBASE_TYPE xPortSetInterruptMask( void )
{
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;
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();
xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
/* Tick Increment. */
vTaskIncrementTick();
/* Select Next Task. */
#if ( configUSE_PREEMPTION == 1 )
vTaskSwitchContext();
#endif
newTask = xTaskGetCurrentTaskHandle();
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 )
{
xSuspended = pdFALSE;
xStarted = pdFALSE;
/* 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);
#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( xSuspended == pdFALSE )
#endif
{
assert( pthread_kill( xTaskToSuspend, SIG_SUSPEND ) == 0);
sched_yield();
}
#ifdef CHECK_TASK_RESUMES
while( xStarted == pdFALSE)
#endif
{
#ifdef COND_SIGNALING
// Set resume condition for specific thread
pthread_cond_t * hCond = prvGetConditionHandle( xTaskGetCurrentTaskHandle() );
assert( pthread_cond_signal( hCond ) == 0 );
#endif
assert( pthread_kill( xTaskToSuspend, SIG_SUSPEND ) == 0);
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;
}
}
/*-----------------------------------------------------------*/
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 ) )
{
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 );
/* 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 );
assert( pthread_sigmask( SIG_BLOCK, &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 */
debug_printf("Thread started, waiting till handle is added\r\n");
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");
sigemptyset( &xSignals );
assert( pthread_sigmask( SIG_SETMASK, &xSignals, NULL ) == 0);
pvCode( pParams );
pthread_cleanup_pop( 1 );
return (void *)NULL;
}
/*-----------------------------------------------------------*/
void pauseThread( portBASE_TYPE pauseMode )
{
debug_printf( "Pausing thread %li. Set xSuspended false\r\n", (long int) pthread_self() );
#ifdef RUNNING_THREAD_MUTEX
if( pauseMode != THREAD_PAUSE_CREATED )
assert( 0 == pthread_mutex_unlock( &xRunningThread ) );
#endif
#ifdef COND_SIGNALING
int xResult;
xTaskHandle hTask = prvGetTaskHandle( pthread_self() );
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
xSuspended = pdTRUE;
while (1) {
if( pthread_self() == prvGetThreadHandle(xTaskGetCurrentTaskHandle() ) && xRunning )
{
xStarted = pdTRUE;
#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(100);
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 );
/* Check that we aren't suspending when we should be running. This bug would need tracking down */
//assert( pthread_self() != prvGetThreadHandle(xTaskGetCurrentTaskHandle() ) );
if( pthread_self() == prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) )
{
debug_printf( "Suspend ISR called while this thread still marked active. Reflects buggy behavior in scheduler\r\n" );
return;
}
/* 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);
pauseThread( THREAD_PAUSE_INTERRUPT );
// assert( pthread_self() == prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) );
while( pthread_self() != prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) )
{
debug_printf( "Incorrectly woke up. Repausing\r\n" );
pauseThread( THREAD_PAUSE_INTERRUPT );
}
/* Make sure the right thread is resuming */
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 );
pthread_sigmask( SIG_SETMASK, &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
{
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;
}
sigsuspendself.sa_flags = 0;
sigsuspendself.sa_handler = prvSuspendSignalHandler;
sigfillset( &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 )
{
xTaskHandle hTask = NULL;
portLONG lIndex;
/* If not initialized yet */
if( pxThreads == NULL ) return NULL;
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
{
if ( pxThreads[ lIndex ].hThread == hThread )
{
hTask = pxThreads[ lIndex ].hTask;
break;
}
}
return hTask;
}
/*-----------------------------------------------------------*/
pthread_cond_t * prvGetConditionHandle( xTaskHandle hTask )
{
pthread_cond_t * hCond;
portLONG lIndex;
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
{
if ( pxThreads[ lIndex ].hTask == hTask )
{
debug_printf( "Found condition on %i task\r\n", lIndex );
return pxThreads[ lIndex ].hCond;
break;
}
}
printf( "Failed to get handle, pausing then recursing\r\n" );
usleep(1000);
return prvGetConditionHandle( hTask );
assert(0);
return hCond;
}
/*-----------------------------------------------------------*/
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;
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;
}
/*-----------------------------------------------------------*/

2332
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/peabody124/tasks.c
View File

@ -1,2332 +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 <stdio.h>
#include <time.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <pthread.h>
/* 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
#ifndef __CYGWIN__
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 );
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
/*-----------------------------------------------------------*/

1092
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/port.c Normal file
View File

@ -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 <pthread.h>
#include <sched.h>
#include <signal.h>
#include <errno.h>
#include <sys/time.h>
#include <time.h>
#include <sys/times.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <limits.h>
/* 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( &currentTime, 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( &currentTime, 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;
}
/*-----------------------------------------------------------*/

774
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/port_linux.c
View File

@ -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 <pthread.h>
#include <sched.h>
#include <signal.h>
#include <errno.h>
#include <sys/time.h>
#include <time.h>
#include <sys/times.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <limits.h>
/* 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;
}
/*-----------------------------------------------------------*/

1258
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/port_posix.c
View File

@ -1,1258 +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 <pthread.h>
#include <sched.h>
#include <signal.h>
#include <errno.h>
#include <sys/time.h>
#include <time.h>
#include <sys/times.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <limits.h>
#include <assert.h>
/* 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 )
{
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;
}
/*-----------------------------------------------------------*/

11
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/portmacro.h
View File

@ -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 );

0
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/tasks.c → flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/tasks.c
View File

2323
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/tasks_linux.c
View File

@ -1,2323 +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 <stdio.h>
#include <time.h>
#include <stdlib.h>
#include <string.h>
/* 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
/*-----------------------------------------------------------*/

2333
flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/tasks_posix.c
View File

@ -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 <stdio.h>
#include <time.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <pthread.h>
/* 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
/*-----------------------------------------------------------*/

11
flight/PiOS.posix/posix/Libraries/FreeRTOS/library.mk Normal file
View File

@ -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

76
flight/PiOS.posix/posix/library.mk Normal file
View File

@ -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

7
flight/PiOS.posix/posix/pios_debug.c
View File

@ -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

44
flight/PiOS.posix/posix/pios_delay.c
View File

@ -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, &current);
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

8
flight/PiOS.posix/posix/pios_led.c
View File

@ -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);

11
flight/PiOS.posix/posix/pios_udp.c
View File

@ -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);

1
flight/PiOS/Boards/STM32103CB_CC_Rev1.h
View File

@ -215,6 +215,7 @@ extern uint32_t pios_com_telem_usb_id;
//------------------------
#define PIOS_RCVR_MAX_DEVS 3
#define PIOS_RCVR_MAX_CHANNELS 12
#define PIOS_GCSRCVR_TIMEOUT_MS 100
//-------------------------
// Receiver PPM input

468
flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h Normal file → Executable file
View File

@ -23,20 +23,22 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_BOARD_H
#define PIOS_BOARD_H
#ifndef STM32103CB_PIPXTREME_H_
#define STM32103CB_PIPXTREME_H_
// *****************************************************************
#define ADD_ONE_ADC
//------------------------
// Timers and Channels Used
//------------------------
/*
Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
------+------------+------------+------------+------------
TIM1 | DELAY |
TIM2 | | PPM Output | PPM Input |
TIM3 | TIMER INTERRUPT |
TIM4 | STOPWATCH |
------+------------+------------+------------+------------
Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
------+-----------+-----------+-----------+----------
TIM1 | Servo 4 | | |
TIM2 | RC In 5 | RC In 6 | Servo 6 |
TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4
TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1
------+-----------+-----------+-----------+----------
*/
//------------------------
@ -55,6 +57,7 @@ TIM4 | STOPWATCH |
/* Channel 11 - */
/* Channel 12 - */
//------------------------
// BOOTLOADER_SETTINGS
//------------------------
@ -63,23 +66,26 @@ TIM4 | STOPWATCH |
#define MAX_DEL_RETRYS 3
// *****************************************************************
// System Settings
//------------------------
// WATCHDOG_SETTINGS
//------------------------
#define PIOS_WATCHDOG_TIMEOUT 500
#define PIOS_WDG_REGISTER BKP_DR4
#define PIOS_WDG_COMUAVTALK 0x0001
#define PIOS_WDG_RADIORECEIVE 0x0002
#define PIOS_WDG_SENDPACKET 0x0004
#define PIOS_WDG_SENDDATA 0x0008
#define PIOS_WDG_TRANSCOMM 0x0010
#define PIOS_WDG_PPMINPUT 0x0020
#define PIOS_MASTER_CLOCK 72000000ul
#define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2)
//------------------------
// TELEMETRY
//------------------------
#define TELEM_QUEUE_SIZE 20
// *****************************************************************
// 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_LED
//------------------------
#define PIOS_LED_USB 0
#define PIOS_LED_LINK 1
#define PIOS_LED_RX 2
@ -104,17 +110,27 @@ TIM4 | STOPWATCH |
#define TX_LED_OFF PIOS_LED_Off(PIOS_LED_TX)
#define TX_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_TX)
// *****************************************************************
// Timer interrupt
//-------------------------
// System Settings
//-------------------------
#define PIOS_MASTER_CLOCK 72000000
#define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2)
#define TIMER_INT_TIMER TIM3
#define TIMER_INT_FUNC TIM3_IRQHandler
#define TIMER_INT_PRIORITY 2
//-------------------------
// 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...
// *****************************************************************
// Stop watch timer
#define STOPWATCH_TIMER TIM4
//------------------------
// PIOS_I2C
// See also pios_board.c
//------------------------
#define PIOS_I2C_MAX_DEVS 1
extern uint32_t pios_i2c_flexi_adapter_id;
#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_flexi_adapter_id)
//------------------------
// PIOS_SPI
@ -126,286 +142,170 @@ extern uint32_t pios_spi_port_id;
//-------------------------
// PIOS_USART
//
// See also pios_board.c
//-------------------------
#define PIOS_USART_MAX_DEVS 1
#define PIOS_USART_MAX_DEVS 3
//-------------------------
// PIOS_COM
//
// See also pios_board.c
//-------------------------
#define PIOS_COM_MAX_DEVS 2
#define PIOS_COM_MAX_DEVS 5
extern uint32_t pios_com_serial_id;
#define PIOS_COM_SERIAL (pios_com_serial_id)
//#define PIOS_COM_DEBUG PIOS_COM_SERIAL // uncomment this to send debug info out the serial port
#if defined(PIOS_INCLUDE_USB_HID)
extern uint32_t pios_com_telem_usb_id;
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
#endif
extern uint32_t pios_com_telemetry_id;
extern uint32_t pios_com_flexi_id;
extern uint32_t pios_com_vcp_id;
extern uint32_t pios_com_uavtalk_com_id;
extern uint32_t pios_com_trans_com_id;
extern uint32_t pios_com_debug_id;
extern uint32_t pios_com_rfm22b_id;
extern uint32_t pios_ppm_rcvr_id;
#define PIOS_COM_USB_HID (pios_com_telem_usb_id)
#define PIOS_COM_TELEMETRY (pios_com_telemetry_id)
#define PIOS_COM_FLEXI (pios_com_flexi_id)
#define PIOS_COM_VCP (pios_com_vcp_id)
#define PIOS_COM_UAVTALK (pios_com_uavtalk_com_id)
#define PIOS_COM_TRANS_COM (pios_com_trans_com_id)
#define PIOS_COM_DEBUG (pios_com_debug_id)
#define PIOS_COM_RADIO (pios_com_rfm22b_id)
#define PIOS_COM_TELEM_USB PIOS_COM_USB_HID
#define PIOS_PPM_RECEIVER (pios_ppm_rcvr_id)
#if defined(PIOS_COM_DEBUG)
// #define DEBUG_PRINTF(...) PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, __VA_ARGS__)
#define DEBUG_PRINTF(...) PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, __VA_ARGS__)
#define DEBUG_LEVEL 2
#if DEBUG_LEVEL > 0
#define DEBUG_PRINTF(level, ...) {if(level <= DEBUG_LEVEL && PIOS_COM_DEBUG > 0) { PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, __VA_ARGS__); }}
#else
#define DEBUG_PRINTF(...)
#define DEBUG_PRINTF(...)
#endif
#define RFM22_DEBUG 1
//-------------------------
// PPM input/output
//-------------------------
#define PIOS_PPM_IN_GPIO_PORT GPIOB
#define PIOS_PPM_IN_GPIO_PIN GPIO_Pin_11
#define PIOS_PPM_IN_TIM_CHANNEL TIM_Channel_4
#define PIOS_PPM_IN_TIM_CCR TIM_IT_CC4
#define PIOS_PPM_IN_TIM_GETCAP_FUNC TIM_GetCapture4
#define PIOS_PPM_OUT_GPIO_PORT GPIOB
#define PIOS_PPM_OUT_GPIO_PIN GPIO_Pin_10
#define PIOS_PPM_OUT_TIM_CHANNEL TIM_Channel_3
#define PIOS_PPM_OUT_TIM_CCR TIM_IT_CC3
#define PIOS_PPM_MAX_CHANNELS 7
#define PIOS_PPM_TIM_PORT TIM2
#define PIOS_PPM_TIM TIM2
#define PIOS_PPM_TIM_IRQ TIM2_IRQn
#define PIOS_PPM_CC_IRQ_FUNC TIM2_IRQHandler
#define PIOS_PPM_TIMER_EN_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE)
#define PIOS_PPM_TIMER_DIS_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, DISABLE)
// *****************************************************************
// ADC
// None
//-------------------------
//#define PIOS_ADC_OVERSAMPLING_RATE 1
#define PIOS_ADC_USE_TEMP_SENSOR 0
#define PIOS_ADC_TEMP_SENSOR_ADC ADC1
#define PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL 1
// PIOS_ADC_PinGet(0) = Temperature Sensor (On-board)
// PIOS_ADC_PinGet(1) = PSU Voltage
#define PIOS_ADC_NUM_PINS 0
#define PIOS_ADC_OVERSAMPLING_RATE 2
#define PIOS_ADC_PORTS { }
#define PIOS_ADC_PINS { }
#define PIOS_ADC_CHANNELS { }
#define PIOS_ADC_MAPPING { }
#define PIOS_ADC_CHANNEL_MAPPING { }
#define PIOS_ADC_NUM_CHANNELS (PIOS_ADC_NUM_PINS + PIOS_ADC_USE_TEMP_SENSOR)
#define PIOS_ADC_NUM_ADC_CHANNELS 0
#define PIOS_ADC_USE_ADC2 0
#define PIOS_ADC_CLOCK_FUNCTION RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2, ENABLE)
#define PIOS_ADC_ADCCLK RCC_PCLK2_Div8
/* RCC_PCLK2_Div2: ADC clock = PCLK2/2 */
/* RCC_PCLK2_Div4: ADC clock = PCLK2/4 */
/* RCC_PCLK2_Div6: ADC clock = PCLK2/6 */
/* RCC_PCLK2_Div8: ADC clock = PCLK2/8 */
#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_239Cycles5
/* Sample time: */
/* With an ADCCLK = 14 MHz and a sampling time of 239.5 cycles: */
/* Tconv = 239.5 + 12.5 = 252 cycles = 18<31>s */
/* (1 / (ADCCLK / CYCLES)) = Sample Time (<28>S) */
#define PIOS_ADC_IRQ_PRIO PIOS_IRQ_PRIO_LOW
#define PIOS_ADC_USE_TEMP_SENSOR 1
#define PIOS_ADC_TEMP_SENSOR_ADC ADC1
#define PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL 16 // Temperature sensor channel
//#define PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL 17 // VREF channel
// Currently analog acquistion hard coded at 480 Hz
// PCKL2 = HCLK / 16
// ADCCLK = PCLK2 / 2
#define PIOS_ADC_RATE (72.0e6 / 1.0 / 8.0 / 252.0 / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2))
#define PIOS_ADC_MAX_OVERSAMPLING 36
#define PIOS_ADC_PIN1_GPIO_PORT GPIOB // Port B (PSU Voltage)
#define PIOS_ADC_PIN1_GPIO_PIN GPIO_Pin_1 // PB1 .. ADC12_IN9
#define PIOS_ADC_PIN1_GPIO_CHANNEL ADC_Channel_9
#define PIOS_ADC_PIN1_ADC ADC2
#define PIOS_ADC_PIN1_ADC_NUMBER 1
//------------------------
// PIOS_RCVR
// See also pios_board.c
//------------------------
#define PIOS_RCVR_MAX_DEVS 3
#define PIOS_RCVR_MAX_CHANNELS 12
#define PIOS_GCSRCVR_TIMEOUT_MS 100
#define PIOS_ADC_NUM_PINS 1
//-------------------------
// Receiver PPM input
//-------------------------
#define PIOS_PPM_MAX_DEVS 1
#define PIOS_PPM_NUM_INPUTS 12
#define PIOS_PPM_PACKET_UPDATE_PERIOD_MS 25
#define PIOS_ADC_PORTS { PIOS_ADC_PIN1_GPIO_PORT }
#define PIOS_ADC_PINS { PIOS_ADC_PIN1_GPIO_PIN }
#define PIOS_ADC_CHANNELS { PIOS_ADC_PIN1_GPIO_CHANNEL }
#define PIOS_ADC_MAPPING { PIOS_ADC_PIN1_ADC }
#define PIOS_ADC_CHANNEL_MAPPING { PIOS_ADC_PIN1_ADC_NUMBER }
//-------------------------
// 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_ADC_NUM_CHANNELS (PIOS_ADC_NUM_PINS + PIOS_ADC_USE_TEMP_SENSOR)
#define PIOS_ADC_NUM_ADC_CHANNELS 2
#define PIOS_ADC_USE_ADC2 1
#define PIOS_ADC_CLOCK_FUNCTION RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2, ENABLE)
//#define PIOS_ADC_ADCCLK RCC_PCLK2_Div2 // ADC clock = PCLK2/2
//#define PIOS_ADC_ADCCLK RCC_PCLK2_Div4 // ADC clock = PCLK2/4
//#define PIOS_ADC_ADCCLK RCC_PCLK2_Div6 // ADC clock = PCLK2/6
#define PIOS_ADC_ADCCLK RCC_PCLK2_Div8 // ADC clock = PCLK2/8
//#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_1Cycles5
//#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_7Cycles5
//#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_13Cycles5
//#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_28Cycles5
//#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_41Cycles5
//#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_55Cycles5
//#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_71Cycles5
#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_239Cycles5
/* Sample time: */
/* With an ADCCLK = 14 MHz and a sampling time of 293.5 cycles: */
/* Tconv = 239.5 + 12.5 = 252 cycles = 18<31>s */
/* (1 / (ADCCLK / CYCLES)) = Sample Time (<28>S) */
#define PIOS_ADC_IRQ_PRIO 3
#define PIOS_ADC_MAX_OVERSAMPLING 1
#define PIOS_ADC_RATE (72.0e6 / 1 / 8 / 252 / (PIOS_ADC_NUM_ADC_CHANNELS >> PIOS_ADC_USE_ADC2))
//--------------------------
// Timer controller settings
//--------------------------
#define PIOS_TIM_MAX_DEVS 3
// *****************************************************************
// GPIO output pins
//-------------------------
// GPIO
//-------------------------
#define PIOS_GPIO_PORTS { }
#define PIOS_GPIO_PINS { }
#define PIOS_GPIO_CLKS { }
#define PIOS_GPIO_NUM 0
// GPIO_Mode_Out_OD Open Drain Output
// GPIO_Mode_Out_PP Push-Pull Output
// GPIO_Mode_AF_OD Open Drain Output Alternate-Function
// GPIO_Mode_AF_PP Push-Pull Output Alternate-Function
// Serial port RTS line
#define PIOS_GPIO_OUT_0_PORT GPIOB
#define PIOS_GPIO_OUT_0_PIN GPIO_Pin_15
#define PIOS_GPIO_OUT_0_GPIO_CLK RCC_APB2Periph_GPIOB
// RF module chip-select line
#define PIOS_GPIO_OUT_1_PORT GPIOA
#define PIOS_GPIO_OUT_1_PIN GPIO_Pin_4
#define PIOS_GPIO_OUT_1_GPIO_CLK RCC_APB2Periph_GPIOA
// PPM OUT line
#define PIOS_GPIO_OUT_2_PORT GPIOB
#define PIOS_GPIO_OUT_2_PIN GPIO_Pin_10
#define PIOS_GPIO_OUT_2_GPIO_CLK RCC_APB2Periph_GPIOB
// spare pin
#define PIOS_GPIO_OUT_3_PORT GPIOA
#define PIOS_GPIO_OUT_3_PIN GPIO_Pin_0
#define PIOS_GPIO_OUT_3_GPIO_CLK RCC_APB2Periph_GPIOA
// spare pin
#define PIOS_GPIO_OUT_4_PORT GPIOA
#define PIOS_GPIO_OUT_4_PIN GPIO_Pin_1
#define PIOS_GPIO_OUT_4_GPIO_CLK RCC_APB2Periph_GPIOA
// spare pin
#define PIOS_GPIO_OUT_5_PORT GPIOC
#define PIOS_GPIO_OUT_5_PIN GPIO_Pin_13
#define PIOS_GPIO_OUT_5_GPIO_CLK RCC_APB2Periph_GPIOC
// spare pin
#define PIOS_GPIO_OUT_6_PORT GPIOC
#define PIOS_GPIO_OUT_6_PIN GPIO_Pin_14
#define PIOS_GPIO_OUT_6_GPIO_CLK RCC_APB2Periph_GPIOC
// spare pin
#define PIOS_GPIO_OUT_7_PORT GPIOC
#define PIOS_GPIO_OUT_7_PIN GPIO_Pin_15
#define PIOS_GPIO_OUT_7_GPIO_CLK RCC_APB2Periph_GPIOC
#define PIOS_GPIO_NUM 8
#define PIOS_GPIO_PORTS {PIOS_GPIO_OUT_0_PORT, PIOS_GPIO_OUT_1_PORT, PIOS_GPIO_OUT_2_PORT, PIOS_GPIO_OUT_3_PORT, PIOS_GPIO_OUT_4_PORT, PIOS_GPIO_OUT_5_PORT, PIOS_GPIO_OUT_6_PORT, PIOS_GPIO_OUT_7_PORT}
#define PIOS_GPIO_PINS {PIOS_GPIO_OUT_0_PIN, PIOS_GPIO_OUT_1_PIN, PIOS_GPIO_OUT_2_PIN, PIOS_GPIO_OUT_3_PIN, PIOS_GPIO_OUT_4_PIN, PIOS_GPIO_OUT_5_PIN, PIOS_GPIO_OUT_6_PIN, PIOS_GPIO_OUT_7_PIN}
#define PIOS_GPIO_CLKS {PIOS_GPIO_OUT_0_GPIO_CLK, PIOS_GPIO_OUT_1_GPIO_CLK, PIOS_GPIO_OUT_2_GPIO_CLK, PIOS_GPIO_OUT_3_GPIO_CLK, PIOS_GPIO_OUT_4_GPIO_CLK, PIOS_GPIO_OUT_5_GPIO_CLK, PIOS_GPIO_OUT_6_GPIO_CLK, PIOS_GPIO_OUT_7_GPIO_CLK}
#define SERIAL_RTS_ENABLE PIOS_GPIO_Enable(0)
#define SERIAL_RTS_SET PIOS_GPIO_Off(0)
#define SERIAL_RTS_CLEAR PIOS_GPIO_On(0)
#define RF_CS_ENABLE PIOS_GPIO_Enable(1)
#define RF_CS_HIGH PIOS_GPIO_Off(1)
#define RF_CS_LOW PIOS_GPIO_On(1)
#define PPM_OUT_PIN PIOS_GPIO_OUT_2_PIN
#define PPM_OUT_PORT PIOS_GPIO_OUT_2_PORT
#define PPM_OUT_ENABLE PIOS_GPIO_Enable(2)
#define PPM_OUT_HIGH PIOS_GPIO_Off(2)
#define PPM_OUT_LOW PIOS_GPIO_On(2)
#define SPARE1_ENABLE PIOS_GPIO_Enable(3)
#define SPARE1_HIGH PIOS_GPIO_Off(3)
#define SPARE1_LOW PIOS_GPIO_On(3)
#define SPARE2_ENABLE PIOS_GPIO_Enable(4)
#define SPARE2_HIGH PIOS_GPIO_Off(4)
#define SPARE2_LOW PIOS_GPIO_On(4)
#define SPARE3_ENABLE PIOS_GPIO_Enable(5)
#define SPARE3_HIGH PIOS_GPIO_Off(5)
#define SPARE3_LOW PIOS_GPIO_On(5)
#define SPARE4_ENABLE PIOS_GPIO_Enable(6)
#define SPARE4_HIGH PIOS_GPIO_Off(6)
#define SPARE4_LOW PIOS_GPIO_On(6)
#define SPARE5_ENABLE PIOS_GPIO_Enable(7)
#define SPARE5_HIGH PIOS_GPIO_Off(7)
#define SPARE5_LOW PIOS_GPIO_On(7)
// *****************************************************************
// GPIO input pins
// GPIO_Mode_AIN Analog Input
// GPIO_Mode_IN_FLOATING Input Floating
// GPIO_Mode_IPD Input Pull-Down
// GPIO_Mode_IPU Input Pull-up
// API mode line
#define GPIO_IN_0_PORT GPIOB
#define GPIO_IN_0_PIN GPIO_Pin_13
#define GPIO_IN_0_MODE GPIO_Mode_IPU
// Serial port CTS line
#define GPIO_IN_1_PORT GPIOB
#define GPIO_IN_1_PIN GPIO_Pin_14
#define GPIO_IN_1_MODE GPIO_Mode_IPU
// VBUS sense line
#define GPIO_IN_2_PORT GPIOA
#define GPIO_IN_2_PIN GPIO_Pin_8
#define GPIO_IN_2_MODE GPIO_Mode_IN_FLOATING
// 868MHz jumper option
#define GPIO_IN_3_PORT GPIOB
#define GPIO_IN_3_PIN GPIO_Pin_8
#define GPIO_IN_3_MODE GPIO_Mode_IPU
// 915MHz jumper option
#define GPIO_IN_4_PORT GPIOB
#define GPIO_IN_4_PIN GPIO_Pin_9
#define GPIO_IN_4_MODE GPIO_Mode_IPU
// RF INT line
#define GPIO_IN_5_PORT GPIOA
#define GPIO_IN_5_PIN GPIO_Pin_2
#define GPIO_IN_5_MODE GPIO_Mode_IN_FLOATING
// RF misc line
#define GPIO_IN_6_PORT GPIOB
#define GPIO_IN_6_PIN GPIO_Pin_0
#define GPIO_IN_6_MODE GPIO_Mode_IN_FLOATING
// PPM IN line
#define PPM_IN_PORT GPIOB
#define PPM_IN_PIN GPIO_Pin_11
#define PPM_IN_MODE GPIO_Mode_IPD
#define GPIO_IN_NUM 8
#define GPIO_IN_PORTS { GPIO_IN_0_PORT, GPIO_IN_1_PORT, GPIO_IN_2_PORT, GPIO_IN_3_PORT, GPIO_IN_4_PORT, GPIO_IN_5_PORT, GPIO_IN_6_PORT, PPM_IN_PORT }
#define GPIO_IN_PINS { GPIO_IN_0_PIN, GPIO_IN_1_PIN, GPIO_IN_2_PIN, GPIO_IN_3_PIN, GPIO_IN_4_PIN, GPIO_IN_5_PIN, GPIO_IN_6_PIN, PPM_IN_PIN }
#define GPIO_IN_MODES { GPIO_IN_0_MODE, GPIO_IN_1_MODE, GPIO_IN_2_MODE, GPIO_IN_3_MODE, GPIO_IN_4_MODE, GPIO_IN_5_MODE, GPIO_IN_6_MODE, PPM_IN_MODE }
#define API_MODE_PIN 0
#define SERIAL_CTS_PIN 1
#define VBUS_SENSE_PIN 2
#define _868MHz_PIN 3
#define _915MHz_PIN 4
#define RF_INT_PIN 5
#define RF_MISC_PIN 6
// *****************************************************************
//-------------------------
// USB
//-------------------------
#define PIOS_USB_HID_MAX_DEVS 1
#define PIOS_USB_ENABLED 1
#define PIOS_USB_HID_MAX_DEVS 1
#define PIOS_USB_MAX_DEVS 1
#define PIOS_USB_DETECT_GPIO_PORT GPIOC
#define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_15
#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line15
#if defined(PIOS_INCLUDE_USB_HID)
#define PIOS_USB_ENABLED 1
#define PIOS_USB_MAX_DEVS 1
#define PIOS_USB_DETECT_GPIO_PORT GPIO_IN_2_PORT
#define PIOS_USB_DETECT_GPIO_PIN GPIO_IN_2_PIN
#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line4
#define PIOS_IRQ_USB_PRIORITY 8
#endif
#define PIOS_USB_HID_MAX_DEVS 1
// *****************************************************************
//-------------------------
// RFM22
//-------------------------
//#define RFM22_EXT_INT_USE
#define RFM22_PIOS_SPI PIOS_SPI_PORT // SPIx
extern uint32_t pios_rfm22b_id;
#define RFM22_EXT_INT_USE
#define RFM22_PIOS_SPI PIOS_SPI_PORT // SPIx
#if defined(RFM22_EXT_INT_USE)
#define RFM22_EXT_INT_PORT_SOURCE GPIO_PortSourceGPIOA
#define RFM22_EXT_INT_PIN_SOURCE GPIO_PinSource2
#define RFM22_EXT_INT_LINE EXTI_Line2
#define RFM22_EXT_INT_IRQn EXTI2_IRQn
#define RFM22_EXT_INT_FUNC EXTI2_IRQHandler
#define RFM22_EXT_INT_PRIORITY 1
#define PIOS_RFM22_EXTI_GPIO_PORT GPIOA
#define PIOS_RFM22_EXTI_GPIO_PIN GPIO_Pin_2
#define PIOS_RFM22_EXTI_PORT_SOURCE GPIO_PortSourceGPIOA
#define PIOS_RFM22_EXTI_PIN_SOURCE GPIO_PinSource2
#define PIOS_RFM22_EXTI_CLK RCC_APB2Periph_GPIOA
#define PIOS_RFM22_EXTI_LINE EXTI_Line2
#define PIOS_RFM22_EXTI_IRQn EXTI2_IRQn
#define PIOS_RFM22_EXTI_PRIO PIOS_IRQ_PRIO_LOW
#endif
// *****************************************************************
//-------------------------
// Packet Handler
//-------------------------
#endif /* PIOS_BOARD_H */
uint32_t pios_packet_handler;
#define PIOS_INCLUDE_PACKET_HANDLER
#define PIOS_PH_MAX_PACKET 255
#define PIOS_PH_WIN_SIZE 3
#define PIOS_PH_MAX_CONNECTIONS 1
//-------------------------
// Reed-Solomon ECC
//-------------------------
#define RS_ECC_NPARITY 4
//-------------------------
// Flash EEPROM Emulation
//-------------------------
#define PIOS_FLASH_SIZE 0x20000
#define PIOS_FLASH_EEPROM_START_ADDR 0x08000000
#define PIOS_FLASH_PAGE_SIZE 1024
#define PIOS_FLASH_EEPROM_ADDR (PIOS_FLASH_EEPROM_START_ADDR + PIOS_FLASH_SIZE - PIOS_FLASH_PAGE_SIZE)
#define PIOS_FLASH_EEPROM_LEN PIOS_FLASH_PAGE_SIZE
#endif /* STM32103CB_PIPXTREME_H_ */

127
flight/PiOS/Common/pios_crc.c
View File

@ -29,6 +29,76 @@ static const uint8_t crc_table[256] = {
0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3
};
static const uint16_t CRC_Table16[] = { // HDLC polynomial
0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf,
0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7,
0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e,
0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876,
0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd,
0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c,
0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974,
0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3,
0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a,
0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9,
0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1,
0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70,
0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7,
0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036,
0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e,
0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd,
0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134,
0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3,
0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb,
0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a,
0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1,
0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330,
0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78
};
static const uint32_t CRC_Table32[] = {
0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9, 0x130476dc, 0x17c56b6b, 0x1a864db2, 0x1e475005,
0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61, 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd,
0x4c11db70, 0x48d0c6c7, 0x4593e01e, 0x4152fda9, 0x5f15adac, 0x5bd4b01b, 0x569796c2, 0x52568b75,
0x6a1936c8, 0x6ed82b7f, 0x639b0da6, 0x675a1011, 0x791d4014, 0x7ddc5da3, 0x709f7b7a, 0x745e66cd,
0x9823b6e0, 0x9ce2ab57, 0x91a18d8e, 0x95609039, 0x8b27c03c, 0x8fe6dd8b, 0x82a5fb52, 0x8664e6e5,
0xbe2b5b58, 0xbaea46ef, 0xb7a96036, 0xb3687d81, 0xad2f2d84, 0xa9ee3033, 0xa4ad16ea, 0xa06c0b5d,
0xd4326d90, 0xd0f37027, 0xddb056fe, 0xd9714b49, 0xc7361b4c, 0xc3f706fb, 0xceb42022, 0xca753d95,
0xf23a8028, 0xf6fb9d9f, 0xfbb8bb46, 0xff79a6f1, 0xe13ef6f4, 0xe5ffeb43, 0xe8bccd9a, 0xec7dd02d,
0x34867077, 0x30476dc0, 0x3d044b19, 0x39c556ae, 0x278206ab, 0x23431b1c, 0x2e003dc5, 0x2ac12072,
0x128e9dcf, 0x164f8078, 0x1b0ca6a1, 0x1fcdbb16, 0x018aeb13, 0x054bf6a4, 0x0808d07d, 0x0cc9cdca,
0x7897ab07, 0x7c56b6b0, 0x71159069, 0x75d48dde, 0x6b93dddb, 0x6f52c06c, 0x6211e6b5, 0x66d0fb02,
0x5e9f46bf, 0x5a5e5b08, 0x571d7dd1, 0x53dc6066, 0x4d9b3063, 0x495a2dd4, 0x44190b0d, 0x40d816ba,
0xaca5c697, 0xa864db20, 0xa527fdf9, 0xa1e6e04e, 0xbfa1b04b, 0xbb60adfc, 0xb6238b25, 0xb2e29692,
0x8aad2b2f, 0x8e6c3698, 0x832f1041, 0x87ee0df6, 0x99a95df3, 0x9d684044, 0x902b669d, 0x94ea7b2a,
0xe0b41de7, 0xe4750050, 0xe9362689, 0xedf73b3e, 0xf3b06b3b, 0xf771768c, 0xfa325055, 0xfef34de2,
0xc6bcf05f, 0xc27dede8, 0xcf3ecb31, 0xcbffd686, 0xd5b88683, 0xd1799b34, 0xdc3abded, 0xd8fba05a,
0x690ce0ee, 0x6dcdfd59, 0x608edb80, 0x644fc637, 0x7a089632, 0x7ec98b85, 0x738aad5c, 0x774bb0eb,
0x4f040d56, 0x4bc510e1, 0x46863638, 0x42472b8f, 0x5c007b8a, 0x58c1663d, 0x558240e4, 0x51435d53,
0x251d3b9e, 0x21dc2629, 0x2c9f00f0, 0x285e1d47, 0x36194d42, 0x32d850f5, 0x3f9b762c, 0x3b5a6b9b,
0x0315d626, 0x07d4cb91, 0x0a97ed48, 0x0e56f0ff, 0x1011a0fa, 0x14d0bd4d, 0x19939b94, 0x1d528623,
0xf12f560e, 0xf5ee4bb9, 0xf8ad6d60, 0xfc6c70d7, 0xe22b20d2, 0xe6ea3d65, 0xeba91bbc, 0xef68060b,
0xd727bbb6, 0xd3e6a601, 0xdea580d8, 0xda649d6f, 0xc423cd6a, 0xc0e2d0dd, 0xcda1f604, 0xc960ebb3,
0xbd3e8d7e, 0xb9ff90c9, 0xb4bcb610, 0xb07daba7, 0xae3afba2, 0xaafbe615, 0xa7b8c0cc, 0xa379dd7b,
0x9b3660c6, 0x9ff77d71, 0x92b45ba8, 0x9675461f, 0x8832161a, 0x8cf30bad, 0x81b02d74, 0x857130c3,
0x5d8a9099, 0x594b8d2e, 0x5408abf7, 0x50c9b640, 0x4e8ee645, 0x4a4ffbf2, 0x470cdd2b, 0x43cdc09c,
0x7b827d21, 0x7f436096, 0x7200464f, 0x76c15bf8, 0x68860bfd, 0x6c47164a, 0x61043093, 0x65c52d24,
0x119b4be9, 0x155a565e, 0x18197087, 0x1cd86d30, 0x029f3d35, 0x065e2082, 0x0b1d065b, 0x0fdc1bec,
0x3793a651, 0x3352bbe6, 0x3e119d3f, 0x3ad08088, 0x2497d08d, 0x2056cd3a, 0x2d15ebe3, 0x29d4f654,
0xc5a92679, 0xc1683bce, 0xcc2b1d17, 0xc8ea00a0, 0xd6ad50a5, 0xd26c4d12, 0xdf2f6bcb, 0xdbee767c,
0xe3a1cbc1, 0xe760d676, 0xea23f0af, 0xeee2ed18, 0xf0a5bd1d, 0xf464a0aa, 0xf9278673, 0xfde69bc4,
0x89b8fd09, 0x8d79e0be, 0x803ac667, 0x84fbdbd0, 0x9abc8bd5, 0x9e7d9662, 0x933eb0bb, 0x97ffad0c,
0xafb010b1, 0xab710d06, 0xa6322bdf, 0xa2f33668, 0xbcb4666d, 0xb8757bda, 0xb5365d03, 0xb1f740b4
};
/**
* Update the crc value with new data.
*
@ -52,7 +122,7 @@ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data)
return crc_table[crc ^ data];
}
/*
/**
* @brief Update a CRC with a data buffer
* @param[in] crc Starting CRC value
* @param[in] data Data buffer
@ -72,3 +142,58 @@ uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length)
return crc8;
}
/**
* Update the crc value with new data.
* \param crc The current crc value.
* \param data Pointer to a buffer of \a data_len bytes.
* \param length Number of bytes in the \a data buffer.
* \return The updated crc value.
*/
uint16_t PIOS_CRC16_updateByte(uint16_t crc, const uint8_t data)
{
return ((crc >> 8) ^ CRC_Table16[(crc & 0xff) ^ data]);
}
/**
* @brief Update a CRC with a data buffer
* @param[in] crc Starting CRC value
* @param[in] data Data buffer
* @param[in] length Number of bytes to process
* @returns Updated CRC
*/
uint16_t PIOS_CRC16_updateCRC(uint16_t crc, const uint8_t* data, int32_t length)
{
register uint8_t *p = (uint8_t *)data;
register uint16_t _crc = crc;
for (register uint32_t i = length; i > 0; i--)
_crc = (_crc >> 8) ^ CRC_Table16[(_crc ^ *p++) & 0xff];
return _crc;
}
/**
* Update the crc value with new data.
* \param crc The current crc value.
* \param data Pointer to a buffer of \a data_len bytes.
* \param length Number of bytes in the \a data buffer.
* \return The updated crc value.
*/
uint32_t PIOS_CRC32_updateByte(uint32_t crc, const uint8_t data)
{
return ((crc << 8) ^ CRC_Table32[(crc >> 24) ^ data]);
}
/**
* @brief Update a CRC with a data buffer
* @param[in] crc Starting CRC value
* @param[in] data Data buffer
* @param[in] length Number of bytes to process
* @returns Updated CRC
*/
uint32_t PIOS_CRC32_updateCRC(uint32_t crc, const uint8_t* data, int32_t length)
{
register uint8_t *p = (uint8_t *)data;
register uint32_t _crc = crc;
for (register uint32_t i = length; i > 0; i--)
_crc = (_crc << 8) ^ CRC_Table32[(_crc >> 24) ^ *p++];
return _crc;
}

5
flight/PiOS/Common/pios_gcsrcvr.c
View File

@ -152,10 +152,9 @@ static void PIOS_gcsrcvr_Supervisor(uint32_t gcsrcvr_id) {
}
/*
* RTC runs at 625Hz so divide down the base rate so
* that this loop runs at 25Hz.
* RTC runs at 625Hz.
*/
if(++(gcsrcvr_dev->supv_timer) < 25) {
if(++(gcsrcvr_dev->supv_timer) < (PIOS_GCSRCVR_TIMEOUT_MS * 1000 / 625)) {
return;
}
gcsrcvr_dev->supv_timer = 0;

2407
flight/PiOS/Common/pios_rfm22b.c Normal file
View File

@ -0,0 +1,2407 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_RFM22B Radio Functions
* @brief PIOS interface for for the RFM22B radio
* @{
*
* @file pios_rfm22b.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Implements a driver the the RFM22B driver
* @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
*/
// *****************************************************************
// RFM22B hardware layer
//
// This module uses the RFM22B's internal packet handling hardware to
// encapsulate our own packet data.
//
// The RFM22B internal hardware packet handler configuration is as follows ..
//
// 4-byte (32-bit) preamble .. alternating 0's & 1's
// 4-byte (32-bit) sync
// 1-byte packet length (number of data bytes to follow)
// 0 to 255 user data bytes
//
// Our own packet data will also contain it's own header and 32-bit CRC
// as a single 16-bit CRC is not sufficient for wireless comms.
//
// *****************************************************************
/* Project Includes */
#include "pios.h"
#if defined(PIOS_INCLUDE_RFM22B)
#include <string.h> // memmove
#include <stm32f10x.h>
#include <stopwatch.h>
#include <packet_handler.h>
#include <pios_rfm22b_priv.h>
/* Local Defines */
#define STACK_SIZE_BYTES 200
// RTC timer is running at 625Hz (1.6ms or 5 ticks == 8ms).
// A 256 byte message at 56kbps should take less than 40ms
// Note: This timeout should be rate dependent.
#define PIOS_RFM22B_SUPERVISOR_TIMEOUT 65 // ~100ms
// this is too adjust the RF module so that it is on frequency
#define OSC_LOAD_CAP 0x7F // cap = 12.5pf .. default
#define OSC_LOAD_CAP_1 0x7D // board 1
#define OSC_LOAD_CAP_2 0x7B // board 2
#define OSC_LOAD_CAP_3 0x7E // board 3
#define OSC_LOAD_CAP_4 0x7F // board 4
// ************************************
#define TX_TEST_MODE_TIMELIMIT_MS 30000 // TX test modes time limit (in ms)
//#define TX_PREAMBLE_NIBBLES 8 // 7 to 511 (number of nibbles)
//#define RX_PREAMBLE_NIBBLES 5 // 5 to 31 (number of nibbles)
#define TX_PREAMBLE_NIBBLES 12 // 7 to 511 (number of nibbles)
#define RX_PREAMBLE_NIBBLES 6 // 5 to 31 (number of nibbles)
// the size of the rf modules internal transmit buffers.
#define TX_BUFFER_SIZE 256
// the size of the rf modules internal FIFO buffers
#define FIFO_SIZE 64
#define TX_FIFO_HI_WATERMARK 62 // 0-63
#define TX_FIFO_LO_WATERMARK 32 // 0-63
#define RX_FIFO_HI_WATERMARK 32 // 0-63
#define PREAMBLE_BYTE 0x55 // preamble byte (preceeds SYNC_BYTE's)
#define SYNC_BYTE_1 0x2D // RF sync bytes (32-bit in all)
#define SYNC_BYTE_2 0xD4 //
#define SYNC_BYTE_3 0x4B //
#define SYNC_BYTE_4 0x59 //
// ************************************
// the default TX power level
#define RFM22_DEFAULT_RF_POWER RFM22_tx_pwr_txpow_0 // +1dBm ... 1.25mW
//#define RFM22_DEFAULT_RF_POWER RFM22_tx_pwr_txpow_1 // +2dBm ... 1.6mW
//#define RFM22_DEFAULT_RF_POWER RFM22_tx_pwr_txpow_2 // +5dBm ... 3.16mW
//#define RFM22_DEFAULT_RF_POWER RFM22_tx_pwr_txpow_3 // +8dBm ... 6.3mW
//#define RFM22_DEFAULT_RF_POWER RFM22_tx_pwr_txpow_4 // +11dBm .. 12.6mW
//#define RFM22_DEFAULT_RF_POWER RFM22_tx_pwr_txpow_5 // +14dBm .. 25mW
//#define RFM22_DEFAULT_RF_POWER RFM22_tx_pwr_txpow_6 // +17dBm .. 50mW
//#define RFM22_DEFAULT_RF_POWER RFM22_tx_pwr_txpow_7 // +20dBm .. 100mW
// ************************************
// the default RF datarate
//#define RFM22_DEFAULT_RF_DATARATE 500 // 500 bits per sec
//#define RFM22_DEFAULT_RF_DATARATE 1000 // 1k bits per sec
//#define RFM22_DEFAULT_RF_DATARATE 2000 // 2k bits per sec
//#define RFM22_DEFAULT_RF_DATARATE 4000 // 4k bits per sec
//#define RFM22_DEFAULT_RF_DATARATE 8000 // 8k bits per sec
//#define RFM22_DEFAULT_RF_DATARATE 9600 // 9.6k bits per sec
//#define RFM22_DEFAULT_RF_DATARATE 16000 // 16k bits per sec
//#define RFM22_DEFAULT_RF_DATARATE 19200 // 19k2 bits per sec
//#define RFM22_DEFAULT_RF_DATARATE 24000 // 24k bits per sec
//#define RFM22_DEFAULT_RF_DATARATE 32000 // 32k bits per sec
//#define RFM22_DEFAULT_RF_DATARATE 64000 // 64k bits per sec
#define RFM22_DEFAULT_RF_DATARATE 128000 // 128k bits per sec
//#define RFM22_DEFAULT_RF_DATARATE 192000 // 192k bits per sec
//#define RFM22_DEFAULT_RF_DATARATE 256000 // 256k bits per sec .. NOT YET WORKING
// ************************************
#define RFM22_DEFAULT_SS_RF_DATARATE 125 // 128bps
// ************************************
// Normal data streaming
// GFSK modulation
// no manchester encoding
// data whitening
// FIFO mode
// 5-nibble rx preamble length detection
// 10-nibble tx preamble length
// AFC enabled
/* Local type definitions */
enum pios_rfm22b_dev_magic {
PIOS_RFM22B_DEV_MAGIC = 0x68e971b6,
};
struct pios_rfm22b_dev {
enum pios_rfm22b_dev_magic magic;
struct pios_rfm22b_cfg cfg;
uint32_t deviceID;
// The COM callback functions.
pios_com_callback rx_in_cb;
uint32_t rx_in_context;
pios_com_callback tx_out_cb;
uint32_t tx_out_context;
// The supervisor countdown timer.
uint16_t supv_timer;
uint16_t resets;
};
uint32_t random32 = 0x459ab8d8;
/* Local function forwared declarations */
static void PIOS_RFM22B_Supervisor(uint32_t ppm_id);
static void rfm22_processInt(void);
static void PIOS_RFM22_EXT_Int(void);
static void rfm22_setTxMode(uint8_t mode);
// SPI read/write functions
void rfm22_startBurstWrite(uint8_t addr);
inline void rfm22_burstWrite(uint8_t data)
{
PIOS_SPI_TransferByte(RFM22_PIOS_SPI, data);
}
void rfm22_endBurstWrite(void);
void rfm22_write(uint8_t addr, uint8_t data);
void rfm22_startBurstRead(uint8_t addr);
inline uint8_t rfm22_burstRead(void)
{
return PIOS_SPI_TransferByte(RFM22_PIOS_SPI, 0xff);
}
void rfm22_endBurstRead(void);
uint8_t rfm22_read(uint8_t addr);
uint8_t rfm22_txStart();
/* Provide a COM driver */
static void PIOS_RFM22B_ChangeBaud(uint32_t rfm22b_id, uint32_t baud);
static void PIOS_RFM22B_RegisterRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context);
static void PIOS_RFM22B_RegisterTxCallback(uint32_t rfm22b_id, pios_com_callback tx_out_cb, uint32_t context);
static void PIOS_RFM22B_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail);
static void PIOS_RFM22B_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail);
/* Local variables */
const struct pios_com_driver pios_rfm22b_com_driver = {
.set_baud = PIOS_RFM22B_ChangeBaud,
.tx_start = PIOS_RFM22B_TxStart,
.rx_start = PIOS_RFM22B_RxStart,
.bind_tx_cb = PIOS_RFM22B_RegisterTxCallback,
.bind_rx_cb = PIOS_RFM22B_RegisterRxCallback,
};
// External interrupt configuration
static const struct pios_exti_cfg pios_exti_rfm22b_cfg __exti_config = {
.vector = PIOS_RFM22_EXT_Int,
.line = PIOS_RFM22_EXTI_LINE,
.pin = {
.gpio = PIOS_RFM22_EXTI_GPIO_PORT,
.init = {
.GPIO_Pin = PIOS_RFM22_EXTI_GPIO_PIN,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
.irq = {
.init = {
.NVIC_IRQChannel = PIOS_RFM22_EXTI_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_RFM22_EXTI_PRIO,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.exti = {
.init = {
.EXTI_Line = PIOS_RFM22_EXTI_LINE,
.EXTI_Mode = EXTI_Mode_Interrupt,
.EXTI_Trigger = EXTI_Trigger_Falling,
.EXTI_LineCmd = ENABLE,
},
},
};
// xtal 10 ppm, 434MHz
#define LOOKUP_SIZE 14
const uint32_t data_rate[] = { 500, 1000, 2000, 4000, 8000, 9600, 16000, 19200, 24000, 32000, 64000, 128000, 192000, 256000};
const uint8_t modulation_index[] = { 16, 8, 4, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
const uint32_t freq_deviation[] = { 4000, 4000, 4000, 4000, 4000, 4800, 8000, 9600, 12000, 16000, 32000, 64000, 96000, 128000};
const uint32_t rx_bandwidth[] = { 17500, 17500, 17500, 17500, 17500, 19400, 32200, 38600, 51200, 64100, 137900, 269300, 420200, 518800};
const int8_t est_rx_sens_dBm[] = { -118, -118, -117, -116, -115, -115, -112, -112, -110, -109, -106, -103, -101, -100}; // estimated receiver sensitivity for BER = 1E-3
const uint8_t reg_1C[] = { 0x37, 0x37, 0x37, 0x37, 0x3A, 0x3B, 0x26, 0x28, 0x2E, 0x16, 0x07, 0x83, 0x8A, 0x8C}; // rfm22_if_filter_bandwidth
const uint8_t reg_1D[] = { 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44}; // rfm22_afc_loop_gearshift_override
const uint8_t reg_1E[] = { 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x02}; // rfm22_afc_timing_control
const uint8_t reg_1F[] = { 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03}; // rfm22_clk_recovery_gearshift_override
const uint8_t reg_20[] = { 0xE8, 0xF4, 0xFA, 0x70, 0x3F, 0x34, 0x3F, 0x34, 0x2A, 0x3F, 0x3F, 0x5E, 0x3F, 0x2F}; // rfm22_clk_recovery_oversampling_ratio
const uint8_t reg_21[] = { 0x60, 0x20, 0x00, 0x01, 0x02, 0x02, 0x02, 0x02, 0x03, 0x02, 0x02, 0x01, 0x02, 0x02}; // rfm22_clk_recovery_offset2
const uint8_t reg_22[] = { 0x20, 0x41, 0x83, 0x06, 0x0C, 0x75, 0x0C, 0x75, 0x12, 0x0C, 0x0C, 0x5D, 0x0C, 0xBB}; // rfm22_clk_recovery_offset1
const uint8_t reg_23[] = { 0xC5, 0x89, 0x12, 0x25, 0x4A, 0x25, 0x4A, 0x25, 0x6F, 0x4A, 0x4A, 0x86, 0x4A, 0x0D}; // rfm22_clk_recovery_offset0
const uint8_t reg_24[] = { 0x00, 0x00, 0x00, 0x02, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x05, 0x07, 0x07}; // rfm22_clk_recovery_timing_loop_gain1
const uint8_t reg_25[] = { 0x0A, 0x23, 0x85, 0x0E, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x74, 0xFF, 0xFF}; // rfm22_clk_recovery_timing_loop_gain0
const uint8_t reg_2A[] = { 0x0E, 0x0E, 0x0E, 0x0E, 0x0E, 0x0D, 0x0D, 0x0E, 0x12, 0x17, 0x31, 0x50, 0x50, 0x50}; // rfm22_afc_limiter .. AFC_pull_in_range = ±AFCLimiter[7:0] x (hbsel+1) x 625 Hz
const uint8_t reg_6E[] = { 0x04, 0x08, 0x10, 0x20, 0x41, 0x4E, 0x83, 0x9D, 0xC4, 0x08, 0x10, 0x20, 0x31, 0x41}; // rfm22_tx_data_rate1
const uint8_t reg_6F[] = { 0x19, 0x31, 0x62, 0xC5, 0x89, 0xA5, 0x12, 0x49, 0x9C, 0x31, 0x62, 0xC5, 0x27, 0x89}; // rfm22_tx_data_rate0
const uint8_t reg_70[] = { 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x0D, 0x0D, 0x0D, 0x0D, 0x0D}; // rfm22_modulation_mode_control1
const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23}; // rfm22_modulation_mode_control2
const uint8_t reg_72[] = { 0x06, 0x06, 0x06, 0x06, 0x06, 0x08, 0x0D, 0x0F, 0x13, 0x1A, 0x33, 0x66, 0x9A, 0xCD}; // rfm22_frequency_deviation
// ************************************
// Scan Spectrum settings
// GFSK modulation
// no manchester encoding
// data whitening
// FIFO mode
// 5-nibble rx preamble length detection
// 10-nibble tx preamble length
#define SS_LOOKUP_SIZE 2
// xtal 1 ppm, 434MHz
const uint32_t ss_rx_bandwidth[] = { 2600, 10600};
const uint8_t ss_reg_1C[] = { 0x51, 0x32}; // rfm22_if_filter_bandwidth
const uint8_t ss_reg_1D[] = { 0x00, 0x00}; // rfm22_afc_loop_gearshift_override
const uint8_t ss_reg_20[] = { 0xE8, 0x38}; // rfm22_clk_recovery_oversampling_ratio
const uint8_t ss_reg_21[] = { 0x60, 0x02}; // rfm22_clk_recovery_offset2
const uint8_t ss_reg_22[] = { 0x20, 0x4D}; // rfm22_clk_recovery_offset1
const uint8_t ss_reg_23[] = { 0xC5, 0xD3}; // rfm22_clk_recovery_offset0
const uint8_t ss_reg_24[] = { 0x00, 0x07}; // rfm22_clk_recovery_timing_loop_gain1
const uint8_t ss_reg_25[] = { 0x0F, 0xFF}; // rfm22_clk_recovery_timing_loop_gain0
const uint8_t ss_reg_2A[] = { 0xFF, 0xFF}; // rfm22_afc_limiter .. AFC_pull_in_range = ±AFCLimiter[7:0] x (hbsel+1) x 625 Hz
const uint8_t ss_reg_70[] = { 0x24, 0x2D}; // rfm22_modulation_mode_control1
const uint8_t ss_reg_71[] = { 0x2B, 0x23}; // rfm22_modulation_mode_control2
// ************************************
volatile bool initialized = false;
struct pios_rfm22b_dev * rfm22b_dev;
#if defined(RFM22_EXT_INT_USE)
volatile bool exec_using_spi; // set this if you want to access the SPI bus outside of the interrupt
#endif
uint8_t device_type; // the RF chips device ID number
uint8_t device_version; // the RF chips revision number
volatile uint8_t rf_mode; // holds our current RF mode
uint32_t lower_carrier_frequency_limit_Hz; // the minimum RF frequency we can use
uint32_t upper_carrier_frequency_limit_Hz; // the maximum RF frequency we can use
uint32_t carrier_frequency_hz; // the current RF frequency we are on
uint32_t carrier_datarate_bps; // the RF data rate we are using
uint32_t rf_bandwidth_used; // the RF bandwidth currently used
uint32_t ss_rf_bandwidth_used; // the RF bandwidth currently used
uint8_t hbsel; // holds the hbsel (1 or 2)
float frequency_step_size; // holds the minimum frequency step size
uint8_t frequency_hop_channel; // current frequency hop channel
uint8_t frequency_hop_step_size_reg; //
uint8_t adc_config; // holds the adc config reg value
volatile uint8_t device_status; // device status register
volatile uint8_t int_status1; // interrupt status register 1
volatile uint8_t int_status2; // interrupt status register 2
volatile uint8_t ezmac_status; // ezmac status register
volatile int16_t afc_correction; // afc correction reading
volatile int32_t afc_correction_Hz; // afc correction reading (in Hz)
volatile int16_t temperature_reg; // the temperature sensor reading
#if defined(RFM22_DEBUG)
volatile uint8_t prev_device_status; // just for debugging
volatile uint8_t prev_int_status1; // " "
volatile uint8_t prev_int_status2; // " "
volatile uint8_t prev_ezmac_status; // " "
const char *debug_msg = NULL;
const char *error_msg = NULL;
static uint32_t debug_val = 0;
#endif
volatile uint8_t osc_load_cap; // xtal frequency calibration value
volatile uint8_t rssi; // the current RSSI (register value)
volatile int8_t rssi_dBm; // dBm value
// the transmit power to use for data transmissions
uint8_t tx_power;
// the tx power register read back
volatile uint8_t tx_pwr;
// The transmit buffer. Holds data that is being transmitted.
uint8_t tx_buffer[TX_BUFFER_SIZE] __attribute__ ((aligned(4)));
// The transmit buffer. Hosts data that is wating to be transmitted.
uint8_t tx_pre_buffer[TX_BUFFER_SIZE] __attribute__ ((aligned(4)));
// The tx pre-buffer write index.
uint16_t tx_pre_buffer_size;
// the tx data read index
volatile uint16_t tx_data_rd;
// the tx data write index
volatile uint16_t tx_data_wr;
// the current receive buffer in use (double buffer)
volatile uint8_t rx_buffer_current;
// the receive buffer .. received packet data is saved here
volatile uint8_t rx_buffer[258] __attribute__ ((aligned(4)));
// the receive buffer write index
volatile uint16_t rx_buffer_wr;
// the received packet
volatile int8_t rx_packet_start_rssi_dBm; //
volatile int8_t rx_packet_start_afc_Hz; //
volatile int8_t rx_packet_rssi_dBm; // the received packet signal strength
volatile int8_t rx_packet_afc_Hz; // the receive packet frequency offset
int lookup_index;
int ss_lookup_index;
volatile bool power_on_reset; // set if the RF module has reset itself
volatile uint16_t rfm22_int_timer; // used to detect if the RF module stops responding. thus act accordingly if it does stop responding.
volatile uint16_t rfm22_int_time_outs; // counter
volatile uint16_t prev_rfm22_int_time_outs; //
uint16_t timeout_ms = 20000; //
uint16_t timeout_sync_ms = 3; //
uint16_t timeout_data_ms = 20; //
struct pios_rfm22b_dev * rfm22b_dev_g;
static bool PIOS_RFM22B_validate(struct pios_rfm22b_dev * rfm22b_dev)
{
return (rfm22b_dev->magic == PIOS_RFM22B_DEV_MAGIC);
}
#if defined(PIOS_INCLUDE_FREERTOS)
static struct pios_rfm22b_dev * PIOS_RFM22B_alloc(void)
{
struct pios_rfm22b_dev * rfm22b_dev;
rfm22b_dev = (struct pios_rfm22b_dev *)pvPortMalloc(sizeof(*rfm22b_dev));
if (!rfm22b_dev) return(NULL);
rfm22b_dev_g = rfm22b_dev;
rfm22b_dev->magic = PIOS_RFM22B_DEV_MAGIC;
return(rfm22b_dev);
}
#else
static struct pios_rfm22b_dev pios_rfm22b_devs[PIOS_RFM22B_MAX_DEVS];
static uint8_t pios_rfm22b_num_devs;
static struct pios_rfm22b_dev * PIOS_RFM22B_alloc(void)
{
struct pios_rfm22b_dev * rfm22b_dev;
if (pios_rfm22b_num_devs >= PIOS_RFM22B_MAX_DEVS)
return NULL;
rfm22b_dev = &pios_rfm22b_devs[pios_rfm22b_num_devs++];
rfm22b_dev->magic = PIOS_RFM22B_DEV_MAGIC;
return (rfm22b_dev);
}
#endif
/**
* Initialise an RFM22B device
*/
int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, const struct pios_rfm22b_cfg *cfg)
{
PIOS_DEBUG_Assert(rfm22b_id);
PIOS_DEBUG_Assert(cfg);
// Allocate the device structure.
rfm22b_dev = (struct pios_rfm22b_dev *) PIOS_RFM22B_alloc();
if (!rfm22b_dev)
return(-1);
// Bind the configuration to the device instance
rfm22b_dev->cfg = *cfg;
*rfm22b_id = (uint32_t)rfm22b_dev;
// Initialize the TX pre-buffer pointer.
tx_pre_buffer_size = 0;
// Create our (hopefully) unique 32 bit id from the processor serial number.
uint8_t crcs[] = { 0, 0, 0, 0 };
{
char serial_no_str[33];
PIOS_SYS_SerialNumberGet(serial_no_str);
// Create a 32 bit value using 4 8 bit CRC values.
for (uint8_t i = 0; serial_no_str[i] != 0; ++i)
crcs[i % 4] = PIOS_CRC_updateByte(crcs[i % 4], serial_no_str[i]);
}
rfm22b_dev->deviceID = crcs[0] | crcs[1] << 8 | crcs[2] << 16 | crcs[3] << 24;
DEBUG_PRINTF(2, "RF device ID: %x\n\r", rfm22b_dev->deviceID);
// Initialize the supervisor timer.
rfm22b_dev->supv_timer = PIOS_RFM22B_SUPERVISOR_TIMEOUT;
rfm22b_dev->resets = 0;
// Initialize the radio device.
int initval = rfm22_init_normal(rfm22b_dev->deviceID, cfg->minFrequencyHz, cfg->maxFrequencyHz, 50000);
if (initval < 0)
{
// RF module error .. flash the LED's
#if defined(PIOS_COM_DEBUG)
DEBUG_PRINTF(2, "RF ERROR res: %d\n\r\n\r", initval);
#endif
for(unsigned int j = 0; j < 16; j++)
{
USB_LED_ON;
LINK_LED_ON;
RX_LED_OFF;
TX_LED_OFF;
PIOS_DELAY_WaitmS(200);
USB_LED_OFF;
LINK_LED_OFF;
RX_LED_ON;
TX_LED_ON;
PIOS_DELAY_WaitmS(200);
}
PIOS_DELAY_WaitmS(1000);
return initval;
}
rfm22_setFreqCalibration(cfg->RFXtalCap);
rfm22_setNominalCarrierFrequency(cfg->frequencyHz);
rfm22_setDatarate(cfg->maxRFBandwidth, TRUE);
rfm22_setTxPower(cfg->maxTxPower);
DEBUG_PRINTF(2, "\n\r");
DEBUG_PRINTF(2, "RF device ID: %x\n\r", rfm22b_dev->deviceID);
DEBUG_PRINTF(2, "RF datarate: %dbps\n\r", rfm22_getDatarate());
DEBUG_PRINTF(2, "RF frequency: %dHz\n\r", rfm22_getNominalCarrierFrequency());
DEBUG_PRINTF(2, "RF TX power: %d\n\r", rfm22_getTxPower());
// Setup a real-time clock callback to kickstart the radio if a transfer lock sup.
if (!PIOS_RTC_RegisterTickCallback(PIOS_RFM22B_Supervisor, *rfm22b_id)) {
PIOS_DEBUG_Assert(0);
}
return(0);
}
uint32_t PIOS_RFM22B_DeviceID(uint32_t rfm22b_id)
{
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
return rfm22b_dev->deviceID;
}
int8_t PIOS_RFM22B_RSSI(uint32_t rfm22b_id)
{
return rfm22_receivedRSSI();
}
int16_t PIOS_RFM22B_Resets(uint32_t rfm22b_id)
{
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
return rfm22b_dev->resets;
}
static void PIOS_RFM22B_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail)
{
struct pios_rfm22b_dev * rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
bool valid = PIOS_RFM22B_validate(rfm22b_dev);
PIOS_Assert(valid);
}
static void PIOS_RFM22B_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail)
{
struct pios_rfm22b_dev * rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
bool valid = PIOS_RFM22B_validate(rfm22b_dev);
PIOS_Assert(valid);
// Get some data to send
bool need_yield = false;
if(tx_pre_buffer_size== 0)
tx_pre_buffer_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, tx_pre_buffer,
TX_BUFFER_SIZE, NULL, &need_yield);
if(tx_pre_buffer_size > 0)
{
// already have data to be sent
if (tx_data_wr > 0)
return;
// we are currently transmitting or scanning the spectrum
if (rf_mode == TX_DATA_MODE || rf_mode == TX_STREAM_MODE || rf_mode == TX_CARRIER_MODE ||
rf_mode == TX_PN_MODE || rf_mode == RX_SCAN_SPECTRUM)
return;
// is the channel clear to transmit on?
if (!rfm22_channelIsClear())
return;
// Start the transmit
rfm22_txStart();
}
}
/**
* Changes the baud rate of the RFM22B peripheral without re-initialising.
* \param[in] rfm22b_id RFM22B name (GPS, TELEM, AUX)
* \param[in] baud Requested baud rate
*/
static void PIOS_RFM22B_ChangeBaud(uint32_t rfm22b_id, uint32_t baud)
{
struct pios_rfm22b_dev * rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
bool valid = PIOS_RFM22B_validate(rfm22b_dev);
PIOS_Assert(valid);
}
static void PIOS_RFM22B_RegisterRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context)
{
struct pios_rfm22b_dev * rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
bool valid = PIOS_RFM22B_validate(rfm22b_dev);
PIOS_Assert(valid);
/*
* Order is important in these assignments since ISR uses _cb
* field to determine if it's ok to dereference _cb and _context
*/
rfm22b_dev->rx_in_context = context;
rfm22b_dev->rx_in_cb = rx_in_cb;
}
static void PIOS_RFM22B_RegisterTxCallback(uint32_t rfm22b_id, pios_com_callback tx_out_cb, uint32_t context)
{
struct pios_rfm22b_dev * rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
bool valid = PIOS_RFM22B_validate(rfm22b_dev);
PIOS_Assert(valid);
/*
* Order is important in these assignments since ISR uses _cb
* field to determine if it's ok to dereference _cb and _context
*/
rfm22b_dev->tx_out_context = context;
rfm22b_dev->tx_out_cb = tx_out_cb;
}
static void PIOS_RFM22B_Supervisor(uint32_t rfm22b_id)
{
/* Recover our device context */
struct pios_rfm22b_dev * rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
if (!PIOS_RFM22B_validate(rfm22b_dev)) {
/* Invalid device specified */
return;
}
/* Not a problem if we're waiting for a packet. */
if(rf_mode == RX_WAIT_SYNC_MODE)
return;
/* The radio must be locked up if the timer reaches 0 */
if(--(rfm22b_dev->supv_timer) != 0)
return;
++(rfm22b_dev->resets);
/* Start a packet transfer if one is available. */
if(!rfm22_txStart())
{
/* Otherwise, switch to RX mode */
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
}
}
static void rfm22_setDebug(const char* msg)
{
debug_msg = msg;
}
static void rfm22_setError(const char* msg)
{
error_msg = msg;
}
// ************************************
// SPI read/write
void rfm22_startBurstWrite(uint8_t addr)
{
// wait 1us .. so we don't toggle the CS line to quickly
PIOS_DELAY_WaituS(1);
// chip select line LOW
PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 0);
PIOS_SPI_TransferByte(RFM22_PIOS_SPI, 0x80 | addr);
}
void rfm22_endBurstWrite(void)
{
// chip select line HIGH
PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 1);
}
void rfm22_write(uint8_t addr, uint8_t data)
{
// wait 1us .. so we don't toggle the CS line to quickly
PIOS_DELAY_WaituS(1);
// chip select line LOW
PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 0);
PIOS_SPI_TransferByte(RFM22_PIOS_SPI, 0x80 | addr);
PIOS_SPI_TransferByte(RFM22_PIOS_SPI, data);
// chip select line HIGH
PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 1);
}
void rfm22_startBurstRead(uint8_t addr)
{
// wait 1us .. so we don't toggle the CS line to quickly
PIOS_DELAY_WaituS(1);
// chip select line LOW
PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 0);
PIOS_SPI_TransferByte(RFM22_PIOS_SPI, addr & 0x7f);
}
void rfm22_endBurstRead(void)
{
// chip select line HIGH
PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 1);
}
uint8_t rfm22_read(uint8_t addr)
{
uint8_t rdata;
// wait 1us .. so we don't toggle the CS line to quickly
PIOS_DELAY_WaituS(1);
// chip select line LOW
PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 0);
PIOS_SPI_TransferByte(RFM22_PIOS_SPI, addr & 0x7f);
rdata = PIOS_SPI_TransferByte(RFM22_PIOS_SPI, 0xff);
// chip select line HIGH
PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 1);
return rdata;
}
// ************************************
// external interrupt
static void PIOS_RFM22_EXT_Int(void)
{
rfm22_setDebug("Ext Int");
if (!exec_using_spi)
rfm22_processInt();
rfm22_setDebug("Ext Done");
}
void rfm22_disableExtInt(void)
{
#if defined(RFM22_EXT_INT_USE)
rfm22_setDebug("Disable Int");
// Configure the external interrupt
GPIO_EXTILineConfig(PIOS_RFM22_EXTI_PORT_SOURCE, PIOS_RFM22_EXTI_PIN_SOURCE);
EXTI_InitTypeDef EXTI_InitStructure = pios_exti_rfm22b_cfg.exti.init;
EXTI_InitStructure.EXTI_LineCmd = DISABLE;
EXTI_Init(&EXTI_InitStructure);
EXTI_ClearFlag(PIOS_RFM22_EXTI_LINE);
rfm22_setDebug("Disable Int done");
#endif
}
void rfm22_enableExtInt(void)
{
#if defined(RFM22_EXT_INT_USE)
rfm22_setDebug("Ensable Int");
if (PIOS_EXTI_Init(&pios_exti_rfm22b_cfg))
PIOS_Assert(0);
rfm22_setDebug("Ensable Int done");
#endif
}
// ************************************
// set/get the current tx power setting
void rfm22_setTxPower(uint8_t tx_pwr)
{
switch (tx_pwr)
{
case 0: tx_power = RFM22_tx_pwr_txpow_0; break; // +1dBm ... 1.25mW
case 1: tx_power = RFM22_tx_pwr_txpow_1; break; // +2dBm ... 1.6mW
case 2: tx_power = RFM22_tx_pwr_txpow_2; break; // +5dBm ... 3.16mW
case 3: tx_power = RFM22_tx_pwr_txpow_3; break; // +8dBm ... 6.3mW
case 4: tx_power = RFM22_tx_pwr_txpow_4; break; // +11dBm .. 12.6mW
case 5: tx_power = RFM22_tx_pwr_txpow_5; break; // +14dBm .. 25mW
case 6: tx_power = RFM22_tx_pwr_txpow_6; break; // +17dBm .. 50mW
case 7: tx_power = RFM22_tx_pwr_txpow_7; break; // +20dBm .. 100mW
default: break;
}
}
uint8_t rfm22_getTxPower(void)
{
return tx_power;
}
// ************************************
uint32_t rfm22_minFrequency(void)
{
return lower_carrier_frequency_limit_Hz;
}
uint32_t rfm22_maxFrequency(void)
{
return upper_carrier_frequency_limit_Hz;
}
void rfm22_setNominalCarrierFrequency(uint32_t frequency_hz)
{
exec_using_spi = TRUE;
// *******
if (frequency_hz < lower_carrier_frequency_limit_Hz) frequency_hz = lower_carrier_frequency_limit_Hz;
else
if (frequency_hz > upper_carrier_frequency_limit_Hz) frequency_hz = upper_carrier_frequency_limit_Hz;
if (frequency_hz < 480000000)
hbsel = 1;
else
hbsel = 2;
uint8_t fb = (uint8_t)(frequency_hz / (10000000 * hbsel));
uint32_t fc = (uint32_t)(frequency_hz - (10000000 * hbsel * fb));
fc = (fc * 64u) / (10000ul * hbsel);
fb -= 24;
// carrier_frequency_hz = frequency_hz;
carrier_frequency_hz = ((uint32_t)fb + 24 + ((float)fc / 64000)) * 10000000 * hbsel;
if (hbsel > 1)
fb |= RFM22_fbs_hbsel;
fb |= RFM22_fbs_sbse; // is this the RX LO polarity?
frequency_step_size = 156.25f * hbsel;
rfm22_write(RFM22_frequency_hopping_channel_select, frequency_hop_channel); // frequency hopping channel (0-255)
rfm22_write(RFM22_frequency_offset1, 0); // no frequency offset
rfm22_write(RFM22_frequency_offset2, 0); // no frequency offset
rfm22_write(RFM22_frequency_band_select, fb); // set the carrier frequency
rfm22_write(RFM22_nominal_carrier_frequency1, fc >> 8); // " "
rfm22_write(RFM22_nominal_carrier_frequency0, fc & 0xff); // " "
// *******
#if defined(RFM22_DEBUG)
//DEBUG_PRINTF(2, "rf setFreq: %u\n\r", carrier_frequency_hz);
// DEBUG_PRINTF(2, "rf setFreq frequency_step_size: %0.2f\n\r", frequency_step_size);
#endif
exec_using_spi = FALSE;
}
uint32_t rfm22_getNominalCarrierFrequency(void)
{
return carrier_frequency_hz;
}
float rfm22_getFrequencyStepSize(void)
{
return frequency_step_size;
}
void rfm22_setFreqHopChannel(uint8_t channel)
{ // set the frequency hopping channel
frequency_hop_channel = channel;
rfm22_write(RFM22_frequency_hopping_channel_select, frequency_hop_channel);
}
uint8_t rfm22_freqHopChannel(void)
{ // return the current frequency hopping channel
return frequency_hop_channel;
}
uint32_t rfm22_freqHopSize(void)
{ // return the frequency hopping step size
return ((uint32_t)frequency_hop_step_size_reg * 10000);
}
// ************************************
// radio datarate about 19200 Baud
// radio frequency deviation 45kHz
// radio receiver bandwidth 67kHz.
//
// Carson's rule:
// The signal bandwidth is about 2(Delta-f + fm) ..
//
// Delta-f = frequency deviation
// fm = maximum frequency of the signal
//
// This gives 2(45 + 9.6) = 109.2kHz.
void rfm22_setDatarate(uint32_t datarate_bps, bool data_whitening)
{
exec_using_spi = TRUE;
lookup_index = 0;
while (lookup_index < (LOOKUP_SIZE - 1) && data_rate[lookup_index] < datarate_bps)
lookup_index++;
carrier_datarate_bps = datarate_bps = data_rate[lookup_index];
rf_bandwidth_used = rx_bandwidth[lookup_index];
// rfm22_if_filter_bandwidth
rfm22_write(0x1C, reg_1C[lookup_index]);
// rfm22_afc_loop_gearshift_override
rfm22_write(0x1D, reg_1D[lookup_index]);
// RFM22_afc_timing_control
rfm22_write(0x1E, reg_1E[lookup_index]);
// RFM22_clk_recovery_gearshift_override
rfm22_write(0x1F, reg_1F[lookup_index]);
// rfm22_clk_recovery_oversampling_ratio
rfm22_write(0x20, reg_20[lookup_index]);
// rfm22_clk_recovery_offset2
rfm22_write(0x21, reg_21[lookup_index]);
// rfm22_clk_recovery_offset1
rfm22_write(0x22, reg_22[lookup_index]);
// rfm22_clk_recovery_offset0
rfm22_write(0x23, reg_23[lookup_index]);
// rfm22_clk_recovery_timing_loop_gain1
rfm22_write(0x24, reg_24[lookup_index]);
// rfm22_clk_recovery_timing_loop_gain0
rfm22_write(0x25, reg_25[lookup_index]);
// rfm22_afc_limiter
rfm22_write(0x2A, reg_2A[lookup_index]);
if (carrier_datarate_bps < 100000)
// rfm22_chargepump_current_trimming_override
rfm22_write(0x58, 0x80);
else
// rfm22_chargepump_current_trimming_override
rfm22_write(0x58, 0xC0);
// rfm22_tx_data_rate1
rfm22_write(0x6E, reg_6E[lookup_index]);
// rfm22_tx_data_rate0
rfm22_write(0x6F, reg_6F[lookup_index]);
// Enable data whitening
// uint8_t txdtrtscale_bit = rfm22_read(RFM22_modulation_mode_control1) & RFM22_mmc1_txdtrtscale;
// rfm22_write(RFM22_modulation_mode_control1, txdtrtscale_bit | RFM22_mmc1_enwhite);
if (!data_whitening)
// rfm22_modulation_mode_control1
rfm22_write(0x70, reg_70[lookup_index] & ~RFM22_mmc1_enwhite);
else
// rfm22_modulation_mode_control1
rfm22_write(0x70, reg_70[lookup_index] | RFM22_mmc1_enwhite);
// rfm22_modulation_mode_control2
rfm22_write(0x71, reg_71[lookup_index]);
// rfm22_frequency_deviation
rfm22_write(0x72, reg_72[lookup_index]);
rfm22_write(RFM22_ook_counter_value1, 0x00);
rfm22_write(RFM22_ook_counter_value2, 0x00);
// ********************************
// calculate the TX register values
/*
uint16_t fd = frequency_deviation / 625;
uint8_t mmc1 = RFM22_mmc1_enphpwdn | RFM22_mmc1_manppol;
uint16_t txdr;
if (datarate_bps < 30000)
{
txdr = (datarate_bps * 20972) / 10000;
mmc1 |= RFM22_mmc1_txdtrtscale;
}
else
txdr = (datarate_bps * 6553) / 100000;
uint8_t mmc2 = RFM22_mmc2_dtmod_fifo | RFM22_mmc2_modtyp_gfsk; // FIFO mode, GFSK
// uint8_t mmc2 = RFM22_mmc2_dtmod_pn9 | RFM22_mmc2_modtyp_gfsk; // PN9 mode, GFSK .. TX TEST MODE
if (fd & 0x100) mmc2 |= RFM22_mmc2_fd;
rfm22_write(RFM22_frequency_deviation, fd); // set the TX peak frequency deviation
rfm22_write(RFM22_modulation_mode_control1, mmc1);
rfm22_write(RFM22_modulation_mode_control2, mmc2);
rfm22_write(RFM22_tx_data_rate1, txdr >> 8); // set the TX data rate
rfm22_write(RFM22_tx_data_rate0, txdr); // " "
*/
// ********************************
// determine a clear channel time
// initialise the stopwatch with a suitable resolution for the datarate
//STOPWATCH_init(4000000ul / carrier_datarate_bps); // set resolution to the time for 1 nibble (4-bits) at rf datarate
// ********************************
// determine suitable time-out periods
// milliseconds
timeout_sync_ms = (8000ul * 16) / carrier_datarate_bps;
if (timeout_sync_ms < 3)
// because out timer resolution is only 1ms
timeout_sync_ms = 3;
// milliseconds
timeout_data_ms = (8000ul * 100) / carrier_datarate_bps;
if (timeout_data_ms < 3)
// because out timer resolution is only 1ms
timeout_data_ms = 3;
// ********************************
#if defined(RFM22_DEBUG)
/*
DEBUG_PRINTF(2, "rf datarate_bps: %d\n\r", datarate_bps);
DEBUG_PRINTF(2, "rf frequency_deviation: %d\n\r", frequency_deviation);
uint32_t frequency_deviation = freq_deviation[lookup_index]; // Hz
uint32_t modulation_bandwidth = datarate_bps + (2 * frequency_deviation);
DEBUG_PRINTF(2, "rf modulation_bandwidth: %u\n\r", modulation_bandwidth);
DEBUG_PRINTF(2, "rf_rx_bandwidth[%u]: %u\n\r", lookup_index, rx_bandwidth[lookup_index]);
DEBUG_PRINTF(2, "rf est rx sensitivity[%u]: %ddBm\n\r", lookup_index, est_rx_sens_dBm[lookup_index]);
*/
#endif
// *******
exec_using_spi = FALSE;
}
uint32_t rfm22_getDatarate(void)
{
return carrier_datarate_bps;
}
// ************************************
void rfm22_setSSBandwidth(uint32_t bandwidth_index)
{
exec_using_spi = TRUE;
ss_lookup_index = bandwidth_index;
ss_rf_bandwidth_used = ss_rx_bandwidth[lookup_index];
// ********************************
rfm22_write(0x1C, ss_reg_1C[ss_lookup_index]); // rfm22_if_filter_bandwidth
rfm22_write(0x1D, ss_reg_1D[ss_lookup_index]); // rfm22_afc_loop_gearshift_override
rfm22_write(0x20, ss_reg_20[ss_lookup_index]); // rfm22_clk_recovery_oversampling_ratio
rfm22_write(0x21, ss_reg_21[ss_lookup_index]); // rfm22_clk_recovery_offset2
rfm22_write(0x22, ss_reg_22[ss_lookup_index]); // rfm22_clk_recovery_offset1
rfm22_write(0x23, ss_reg_23[ss_lookup_index]); // rfm22_clk_recovery_offset0
rfm22_write(0x24, ss_reg_24[ss_lookup_index]); // rfm22_clk_recovery_timing_loop_gain1
rfm22_write(0x25, ss_reg_25[ss_lookup_index]); // rfm22_clk_recovery_timing_loop_gain0
rfm22_write(0x2A, ss_reg_2A[ss_lookup_index]); // rfm22_afc_limiter
rfm22_write(0x58, 0x80); // rfm22_chargepump_current_trimming_override
rfm22_write(0x70, ss_reg_70[ss_lookup_index]); // rfm22_modulation_mode_control1
rfm22_write(0x71, ss_reg_71[ss_lookup_index]); // rfm22_modulation_mode_control2
rfm22_write(RFM22_ook_counter_value1, 0x00);
rfm22_write(RFM22_ook_counter_value2, 0x00);
// ********************************
#if defined(RFM22_DEBUG)
DEBUG_PRINTF(2, "ss_rf_rx_bandwidth[%u]: %u\n\r", ss_lookup_index, ss_rx_bandwidth[ss_lookup_index]);
#endif
// *******
exec_using_spi = FALSE;
}
// ************************************
void rfm22_setRxMode(uint8_t mode, bool multi_packet_mode)
{
exec_using_spi = TRUE;
// disable interrupts
rfm22_write(RFM22_interrupt_enable1, 0x00);
rfm22_write(RFM22_interrupt_enable2, 0x00);
// Switch to TUNE mode
rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon);
RX_LED_OFF;
TX_LED_OFF;
if (rf_mode == TX_CARRIER_MODE || rf_mode == TX_PN_MODE)
{
// FIFO mode, GFSK modulation
uint8_t fd_bit = rfm22_read(RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
rfm22_write(RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_fifo |
RFM22_mmc2_modtyp_gfsk);
}
// empty the rx buffer
rx_buffer_wr = 0;
// reset the timer
rfm22_int_timer = 0;
rf_mode = mode;
// Clear the TX buffer.
tx_data_rd = tx_data_wr = 0;
// clear FIFOs
if (!multi_packet_mode)
{
rfm22_write(RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
rfm22_write(RFM22_op_and_func_ctrl2, 0x00);
} else {
rfm22_write(RFM22_op_and_func_ctrl2, RFM22_opfc2_rxmpk | RFM22_opfc2_ffclrrx |
RFM22_opfc2_ffclrtx);
rfm22_write(RFM22_op_and_func_ctrl2, RFM22_opfc2_rxmpk);
}
// enable RX interrupts
rfm22_write(RFM22_interrupt_enable1, RFM22_ie1_encrcerror | RFM22_ie1_enpkvalid |
RFM22_ie1_enrxffafull | RFM22_ie1_enfferr);
rfm22_write(RFM22_interrupt_enable2, RFM22_ie2_enpreainval | RFM22_ie2_enpreaval |
RFM22_ie2_enswdet);
// enable the receiver
// rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_xton | RFM22_opfc1_rxon);
rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_rxon);
exec_using_spi = FALSE;
}
// ************************************
uint16_t rfm22_addHeader()
{
uint16_t i = 0;
for (uint16_t j = (TX_PREAMBLE_NIBBLES + 1) / 2; j > 0; j--)
{
rfm22_burstWrite(PREAMBLE_BYTE);
i++;
}
rfm22_burstWrite(SYNC_BYTE_1); i++;
rfm22_burstWrite(SYNC_BYTE_2); i++;
return i;
}
// ************************************
uint8_t rfm22_txStart()
{
if((tx_pre_buffer_size == 0) || (exec_using_spi == TRUE))
{
// Clear the TX buffer.
tx_data_rd = tx_data_wr = 0;
return 0;
}
exec_using_spi = TRUE;
// Disable interrrupts.
PIOS_IRQ_Disable();
// Initialize the supervisor timer.
rfm22b_dev->supv_timer = PIOS_RFM22B_SUPERVISOR_TIMEOUT;
// disable interrupts
rfm22_write(RFM22_interrupt_enable1, 0x00);
rfm22_write(RFM22_interrupt_enable2, 0x00);
// TUNE mode
rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon);
// Queue the data up for sending
memcpy(tx_buffer, tx_pre_buffer, tx_pre_buffer_size);
tx_data_rd = 0;
tx_data_wr = tx_pre_buffer_size;
tx_pre_buffer_size = 0;
RX_LED_OFF;
// Set the destination address in the transmit header.
// The destination address is the first 4 bytes of the message.
rfm22_write(RFM22_transmit_header0, tx_buffer[0]);
rfm22_write(RFM22_transmit_header1, tx_buffer[1]);
rfm22_write(RFM22_transmit_header2, tx_buffer[2]);
rfm22_write(RFM22_transmit_header3, tx_buffer[3]);
// FIFO mode, GFSK modulation
uint8_t fd_bit = rfm22_read(RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
rfm22_write(RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_fifo |
RFM22_mmc2_modtyp_gfsk);
// set the tx power
rfm22_write(RFM22_tx_power, RFM22_tx_pwr_papeaken | RFM22_tx_pwr_papeaklvl_1 |
RFM22_tx_pwr_papeaklvl_0 | RFM22_tx_pwr_lna_sw | tx_power);
// clear FIFOs
rfm22_write(RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
rfm22_write(RFM22_op_and_func_ctrl2, 0x00);
// *******************
// add some data to the chips TX FIFO before enabling the transmitter
// set the total number of data bytes we are going to transmit
rfm22_write(RFM22_transmit_packet_length, tx_data_wr);
// add some data
rfm22_startBurstWrite(RFM22_fifo_access);
for (uint16_t i = 0; (tx_data_rd < tx_data_wr) && (i < FIFO_SIZE); ++tx_data_rd, ++i)
rfm22_burstWrite(tx_buffer[tx_data_rd]);
rfm22_endBurstWrite();
// *******************
// reset the timer
rfm22_int_timer = 0;
rf_mode = TX_DATA_MODE;
// enable TX interrupts
// rfm22_write(RFM22_interrupt_enable1, RFM22_ie1_enpksent | RFM22_ie1_entxffaem | RFM22_ie1_enfferr);
rfm22_write(RFM22_interrupt_enable1, RFM22_ie1_enpksent | RFM22_ie1_entxffaem);
// read interrupt status - clear interrupts
//rfm22_read(RFM22_interrupt_status1);
//rfm22_read(RFM22_interrupt_status2);
// enable the transmitter
// rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_xton | RFM22_opfc1_txon);
rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_txon);
// Re-ensable interrrupts.
PIOS_IRQ_Enable();
TX_LED_ON;
exec_using_spi = FALSE;
return 1;
}
static void rfm22_setTxMode(uint8_t mode)
{
rfm22_setDebug("setTxMode");
if (mode != TX_DATA_MODE && mode != TX_STREAM_MODE && mode != TX_CARRIER_MODE && mode != TX_PN_MODE)
return; // invalid mode
exec_using_spi = TRUE;
// *******************
// disable interrupts
rfm22_write(RFM22_interrupt_enable1, 0x00);
rfm22_write(RFM22_interrupt_enable2, 0x00);
// rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_xton); // READY mode
rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon); // TUNE mode
RX_LED_OFF;
// set the tx power
rfm22_write(RFM22_tx_power, RFM22_tx_pwr_papeaken | RFM22_tx_pwr_papeaklvl_1 |
RFM22_tx_pwr_papeaklvl_0 | RFM22_tx_pwr_lna_sw | tx_power);
uint8_t fd_bit = rfm22_read(RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
if (mode == TX_CARRIER_MODE)
// blank carrier mode - for testing
rfm22_write(RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_pn9 |
RFM22_mmc2_modtyp_none); // FIFO mode, Blank carrier
else if (mode == TX_PN_MODE)
// psuedo random data carrier mode - for testing
rfm22_write(RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_pn9 |
RFM22_mmc2_modtyp_gfsk); // FIFO mode, PN9 carrier
else
// data transmission
// FIFO mode, GFSK modulation
rfm22_write(RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_fifo |
RFM22_mmc2_modtyp_gfsk);
// rfm22_write(0x72, reg_72[lookup_index]); // rfm22_frequency_deviation
// clear FIFOs
rfm22_write(RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
rfm22_write(RFM22_op_and_func_ctrl2, 0x00);
// *******************
// add some data to the chips TX FIFO before enabling the transmitter
{
uint16_t rd = 0;
uint16_t wr = tx_data_wr;
if (mode == TX_DATA_MODE)
// set the total number of data bytes we are going to transmit
rfm22_write(RFM22_transmit_packet_length, wr);
uint16_t max_bytes = FIFO_SIZE - 1;
uint16_t i = 0;
rfm22_startBurstWrite(RFM22_fifo_access);
if (mode == TX_STREAM_MODE) {
if (rd >= wr) {
// no data to send - yet .. just send preamble pattern
while (true) {
rfm22_burstWrite(PREAMBLE_BYTE);
if (++i >= max_bytes) break;
}
} else // add the RF heaader
i += rfm22_addHeader();
}
// add some data
for (uint16_t j = wr - rd; j > 0; j--) {
rfm22_burstWrite(tx_buffer[rd++]);
if (++i >= max_bytes)
break;
}
rfm22_endBurstWrite();
tx_data_rd = rd;
}
// *******************
// reset the timer
rfm22_int_timer = 0;
rf_mode = mode;
// enable TX interrupts
// rfm22_write(RFM22_interrupt_enable1, RFM22_ie1_enpksent | RFM22_ie1_entxffaem | RFM22_ie1_enfferr);
rfm22_write(RFM22_interrupt_enable1, RFM22_ie1_enpksent | RFM22_ie1_entxffaem);
// read interrupt status - clear interrupts
rfm22_read(RFM22_interrupt_status1);
rfm22_read(RFM22_interrupt_status2);
// enable the transmitter
// rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_xton | RFM22_opfc1_txon);
rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_txon);
TX_LED_ON;
// *******************
exec_using_spi = FALSE;
rfm22_setDebug("setTxMode end");
}
// ************************************
// external interrupt line triggered (or polled) from the rf chip
void rfm22_processRxInt(void)
{
register uint8_t int_stat1 = int_status1;
register uint8_t int_stat2 = int_status2;
rfm22_setDebug("processRxInt");
// FIFO under/over flow error. Restart RX mode.
if (device_status & (RFM22_ds_ffunfl | RFM22_ds_ffovfl))
{
rfm22_setError("R_UNDER/OVERRUN");
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
return;
}
// Sync timeout. Restart RX mode.
if (rf_mode == RX_WAIT_SYNC_MODE && rfm22_int_timer >= timeout_sync_ms)
{
rfm22_int_time_outs++;
rfm22_setError("R_SYNC_TIMEOUT");
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
return;
}
// RX timeout. Restart RX mode.
if (rf_mode == RX_DATA_MODE && rfm22_int_timer >= timeout_data_ms)
{
rfm22_setError("MISSING_INTERRUPTS");
rfm22_int_time_outs++;
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
return;
}
// The module is not in RX mode. Restart RX mode.
if ((device_status & RFM22_ds_cps_mask) != RFM22_ds_cps_rx)
{
// the rf module is not in rx mode
if (rfm22_int_timer >= 100)
{
rfm22_int_time_outs++;
// reset the receiver
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
return;
}
}
// Valid preamble detected
if (int_stat2 & RFM22_is2_ipreaval)
{
if (rf_mode == RX_WAIT_PREAMBLE_MODE)
{
rfm22_int_timer = 0; // reset the timer
rf_mode = RX_WAIT_SYNC_MODE;
RX_LED_ON;
rfm22_setDebug("pream_det");
}
}
// Sync word detected
if (int_stat2 & RFM22_is2_iswdet)
{
if (rf_mode == RX_WAIT_PREAMBLE_MODE || rf_mode == RX_WAIT_SYNC_MODE)
{
rfm22_int_timer = 0; // reset the timer
rf_mode = RX_DATA_MODE;
RX_LED_ON;
rfm22_setDebug("sync_det");
// read the 10-bit signed afc correction value
// bits 9 to 2
afc_correction = (uint16_t)rfm22_read(RFM22_afc_correction_read) << 8;
// bits 1 & 0
afc_correction |= (uint16_t)rfm22_read(RFM22_ook_counter_value1) & 0x00c0;
afc_correction >>= 6;
// convert the afc value to Hz
afc_correction_Hz = (int32_t)(frequency_step_size * afc_correction + 0.5f);
// remember the rssi for this packet
rx_packet_start_rssi_dBm = rssi_dBm;
// remember the afc value for this packet
rx_packet_start_afc_Hz = afc_correction_Hz;
}
}
// RX FIFO almost full, it needs emptying
if (int_stat1 & RFM22_is1_irxffafull)
{
if (rf_mode == RX_DATA_MODE)
{
// read data from the rf chips FIFO buffer
rfm22_int_timer = 0; // reset the timer
// read the total length of the packet data
uint16_t len = rfm22_read(RFM22_received_packet_length);
// The received packet is going to be larger than the specified length
if ((rx_buffer_wr + RX_FIFO_HI_WATERMARK) > len)
{
rfm22_setError("r_size_error1");
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
return;
}
// Another packet length error.
if (((rx_buffer_wr + RX_FIFO_HI_WATERMARK) >= len) && !(int_stat1 & RFM22_is1_ipkvalid))
{
rfm22_setError("r_size_error2");
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
return;
}
// Fetch the data from the RX FIFO
rfm22_startBurstRead(RFM22_fifo_access);
for (uint8_t i = 0; i < RX_FIFO_HI_WATERMARK; ++i)
rx_buffer[rx_buffer_wr++] = rfm22_burstRead();
rfm22_endBurstRead();
}
else
{ // just clear the RX FIFO
rfm22_startBurstRead(RFM22_fifo_access);
for (register uint16_t i = RX_FIFO_HI_WATERMARK; i > 0; i--)
rfm22_burstRead(); // read a byte from the rf modules RX FIFO buffer
rfm22_endBurstRead();
}
}
// CRC error .. discard the received data
if (int_stat1 & RFM22_is1_icrerror)
{
rfm22_int_timer = 0; // reset the timer
rfm22_setError("CRC_ERR");
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
return;
}
// if (int_stat2 & RFM22_is2_irssi)
// { // RSSI level is >= the set threshold
// }
// if (device_status & RFM22_ds_rxffem)
// { // RX FIFO empty
// }
// if (device_status & RFM22_ds_headerr)
// { // Header check error
// }
// Valid packet received
if (int_stat1 & RFM22_is1_ipkvalid)
{
// reset the timer
rfm22_int_timer = 0;
// read the total length of the packet data
register uint16_t len = rfm22_read(RFM22_received_packet_length);
// their must still be data in the RX FIFO we need to get
if (rx_buffer_wr < len)
{
// Fetch the data from the RX FIFO
rfm22_startBurstRead(RFM22_fifo_access);
while (rx_buffer_wr < len)
rx_buffer[rx_buffer_wr++] = rfm22_burstRead();
rfm22_endBurstRead();
}
if (rx_buffer_wr != len)
{
// we have a packet length error .. discard the packet
rfm22_setError("r_pack_len_error");
debug_val = len;
return;
}
// we have a valid received packet
rfm22_setDebug("VALID_R_PACKET");
if (rx_buffer_wr > 0)
{
// remember the rssi for this packet
rx_packet_rssi_dBm = rx_packet_start_rssi_dBm;
// remember the afc offset for this packet
rx_packet_afc_Hz = rx_packet_start_afc_Hz;
// Add the rssi and afc to the end of the packet.
rx_buffer[rx_buffer_wr++] = rx_packet_start_rssi_dBm;
rx_buffer[rx_buffer_wr++] = rx_packet_start_afc_Hz;
// Pass this packet on
bool need_yield = false;
if (rfm22b_dev_g->rx_in_cb)
(rfm22b_dev_g->rx_in_cb)(rfm22b_dev_g->rx_in_context, (uint8_t*)rx_buffer,
rx_buffer_wr, NULL, &need_yield);
rx_buffer_wr = 0;
}
// Send a packet if it's available.
if(!rfm22_txStart())
{
// Switch to RX mode
rfm22_setDebug(" Set RX");
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
}
}
rfm22_setDebug("processRxInt end");
}
void rfm22_processTxInt(void)
{
register uint8_t int_stat1 = int_status1;
// reset the timer
rfm22_int_timer = 0;
// FIFO under/over flow error. Back to RX mode.
if (device_status & (RFM22_ds_ffunfl | RFM22_ds_ffovfl))
{
rfm22_setError("T_UNDER/OVERRUN");
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
return;
}
// Transmit timeout. Abort the transmit.
if (rfm22_int_timer >= timeout_data_ms)
{
rfm22_int_time_outs++;
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
return;
}
// the rf module is not in tx mode
if ((device_status & RFM22_ds_cps_mask) != RFM22_ds_cps_tx)
{
if (rfm22_int_timer >= 100)
{
rfm22_int_time_outs++;
rfm22_setError("T_TIMEOUT");
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false); // back to rx mode
return;
}
}
// TX FIFO almost empty, it needs filling up
if (int_stat1 & RFM22_is1_ixtffaem)
{
// top-up the rf chips TX FIFO buffer
uint16_t max_bytes = FIFO_SIZE - TX_FIFO_LO_WATERMARK - 1;
rfm22_startBurstWrite(RFM22_fifo_access);
for (uint16_t i = 0; (tx_data_rd < tx_data_wr) && (i < max_bytes); ++i, ++tx_data_rd)
rfm22_burstWrite(tx_buffer[tx_data_rd]);
rfm22_endBurstWrite();
}
// Packet has been sent
if (int_stat1 & RFM22_is1_ipksent)
{
rfm22_setDebug(" T_Sent");
// Send another packet if it's available.
if(!rfm22_txStart())
{
// Switch to RX mode
rfm22_setDebug(" Set RX");
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
return;
}
}
rfm22_setDebug("ProcessTxInt done");
}
static void rfm22_processInt(void)
{
rfm22_setDebug("ProcessInt");
// this is called from the external interrupt handler
if (!initialized || power_on_reset)
// we haven't yet been initialized
return;
exec_using_spi = TRUE;
// Reset the supervisor timer.
rfm22b_dev->supv_timer = PIOS_RFM22B_SUPERVISOR_TIMEOUT;
// ********************************
// read the RF modules current status registers
// read interrupt status registers - clears the interrupt line
int_status1 = rfm22_read(RFM22_interrupt_status1);
int_status2 = rfm22_read(RFM22_interrupt_status2);
// read device status register
device_status = rfm22_read(RFM22_device_status);
// read ezmac status register
ezmac_status = rfm22_read(RFM22_ezmac_status);
// Read the RSSI if we're in RX mode
if (rf_mode != TX_DATA_MODE && rf_mode != TX_STREAM_MODE &&
rf_mode != TX_CARRIER_MODE && rf_mode != TX_PN_MODE)
{
// read rx signal strength .. 45 = -100dBm, 205 = -20dBm
rssi = rfm22_read(RFM22_rssi);
// convert to dBm
rssi_dBm = (int8_t)(rssi >> 1) - 122;
// calibrate the RSSI value (rf bandwidth appears to affect it)
// if (rf_bandwidth_used > 0)
// rssi_dBm -= 10000 / rf_bandwidth_used;
}
else
{
// read the tx power register
tx_pwr = rfm22_read(RFM22_tx_power);
}
// the RF module has gone and done a reset - we need to re-initialize the rf module
if (int_status2 & RFM22_is2_ipor)
{
initialized = FALSE;
power_on_reset = TRUE;
rfm22_setError("Reset");
// Need to do something here!
return;
}
switch (rf_mode)
{
case RX_SCAN_SPECTRUM:
break;
case RX_WAIT_PREAMBLE_MODE:
case RX_WAIT_SYNC_MODE:
case RX_DATA_MODE:
rfm22_processRxInt();
break;
case TX_DATA_MODE:
case TX_STREAM_MODE:
rfm22_processTxInt();
break;
case TX_CARRIER_MODE:
case TX_PN_MODE:
// if (rfm22_int_timer >= TX_TEST_MODE_TIMELIMIT_MS) // 'nn'ms limit
// {
// rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false); // back to rx mode
// tx_data_rd = tx_data_wr = 0; // wipe TX buffer
// break;
// }
break;
default: // unknown mode - this should NEVER happen, maybe we should do a complete CPU reset here
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false); // to rx mode
break;
}
exec_using_spi = FALSE;
rfm22_setDebug("ProcessInt done");
}
// ************************************
int8_t rfm22_getRSSI(void)
{
exec_using_spi = TRUE;
rssi = rfm22_read(RFM22_rssi); // read rx signal strength .. 45 = -100dBm, 205 = -20dBm
rssi_dBm = (int8_t)(rssi >> 1) - 122; // convert to dBm
exec_using_spi = FALSE;
return rssi_dBm;
}
int8_t rfm22_receivedRSSI(void)
{ // return the packets signal strength
if (!initialized)
return -127;
else
return rx_packet_rssi_dBm;
}
int32_t rfm22_receivedAFCHz(void)
{ // return the packets offset frequency
if (!initialized)
return 0;
else
return rx_packet_afc_Hz;
}
// ************************************
// ************************************
void rfm22_setTxStream(void) // TEST ONLY
{
if (!initialized)
return;
tx_data_rd = tx_data_wr = 0;
rfm22_setTxMode(TX_STREAM_MODE);
}
// ************************************
void rfm22_setTxNormal(void)
{
if (!initialized)
return;
// if (rf_mode == TX_CARRIER_MODE || rf_mode == TX_PN_MODE)
if (rf_mode != RX_SCAN_SPECTRUM)
{
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
tx_data_rd = tx_data_wr = 0;
rx_packet_start_rssi_dBm = 0;
rx_packet_start_afc_Hz = 0;
rx_packet_rssi_dBm = 0;
rx_packet_afc_Hz = 0;
}
}
// enable a blank tx carrier (for frequency alignment)
void rfm22_setTxCarrierMode(void)
{
if (!initialized)
return;
if (rf_mode != TX_CARRIER_MODE && rf_mode != RX_SCAN_SPECTRUM)
rfm22_setTxMode(TX_CARRIER_MODE);
}
// enable a psuedo random data tx carrier (for spectrum inspection)
void rfm22_setTxPNMode(void)
{
if (!initialized)
return;
if (rf_mode != TX_PN_MODE && rf_mode != RX_SCAN_SPECTRUM)
rfm22_setTxMode(TX_PN_MODE);
}
// ************************************
// return the current mode
int8_t rfm22_currentMode(void)
{
return rf_mode;
}
// return TRUE if we are transmitting
bool rfm22_transmitting(void)
{
return (rf_mode == TX_DATA_MODE || rf_mode == TX_STREAM_MODE || rf_mode == TX_CARRIER_MODE || rf_mode == TX_PN_MODE);
}
// return TRUE if the channel is clear to transmit on
bool rfm22_channelIsClear(void)
{
if (!initialized)
// we haven't yet been initialized
return FALSE;
if (rf_mode != RX_WAIT_PREAMBLE_MODE && rf_mode != RX_WAIT_SYNC_MODE)
// we are receiving something or we are transmitting or we are scanning the spectrum
return FALSE;
return TRUE;
}
// return TRUE if the transmiter is ready for use
bool rfm22_txReady(void)
{
if (!initialized)
// we haven't yet been initialized
return FALSE;
return (tx_data_rd == 0 && tx_data_wr == 0 && rf_mode != TX_DATA_MODE &&
rf_mode != TX_STREAM_MODE && rf_mode != TX_CARRIER_MODE && rf_mode != TX_PN_MODE &&
rf_mode != RX_SCAN_SPECTRUM);
}
// ************************************
// set/get the frequency calibration value
void rfm22_setFreqCalibration(uint8_t value)
{
osc_load_cap = value;
if (!initialized || power_on_reset)
return; // we haven't yet been initialized
uint8_t prev_rf_mode = rf_mode;
if (rf_mode == TX_CARRIER_MODE || rf_mode == TX_PN_MODE)
{
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
tx_data_rd = tx_data_wr = 0;
}
exec_using_spi = TRUE;
rfm22_write(RFM22_xtal_osc_load_cap, osc_load_cap);
exec_using_spi = FALSE;
if (prev_rf_mode == TX_CARRIER_MODE || prev_rf_mode == TX_PN_MODE)
rfm22_setTxMode(prev_rf_mode);
}
uint8_t rfm22_getFreqCalibration(void)
{
return osc_load_cap;
}
// ************************************
// can be called from an interrupt if you wish
void rfm22_1ms_tick(void)
{ // call this once every ms
if (!initialized) return; // we haven't yet been initialized
if (rf_mode != RX_SCAN_SPECTRUM)
{
if (rfm22_int_timer < 0xffff) rfm22_int_timer++;
}
}
// ************************************
// reset the RF module
int rfm22_resetModule(uint8_t mode, uint32_t min_frequency_hz, uint32_t max_frequency_hz)
{
initialized = false;
#if defined(RFM22_EXT_INT_USE)
rfm22_disableExtInt();
#endif
power_on_reset = false;
// ****************
exec_using_spi = TRUE;
// ****************
// setup the SPI port
// chip select line HIGH
PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 1);
// set SPI port SCLK frequency .. 4.5MHz
PIOS_SPI_SetClockSpeed(RFM22_PIOS_SPI, PIOS_SPI_PRESCALER_16);
// set SPI port SCLK frequency .. 2.25MHz
// PIOS_SPI_SetClockSpeed(RFM22_PIOS_SPI, PIOS_SPI_PRESCALER_32);
// set SPI port SCLK frequency .. 285kHz .. purely for hardware fault finding
// PIOS_SPI_SetClockSpeed(RFM22_PIOS_SPI, PIOS_SPI_PRESCALER_256);
// ****************
// software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B)
rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); // software reset the radio
PIOS_DELAY_WaitmS(26); // wait 26ms
for (int i = 50; i > 0; i--)
{
PIOS_DELAY_WaitmS(1); // wait 1ms
// read the status registers
int_status1 = rfm22_read(RFM22_interrupt_status1);
int_status2 = rfm22_read(RFM22_interrupt_status2);
if (int_status2 & RFM22_is2_ichiprdy) break;
}
// ****************
// read status - clears interrupt
device_status = rfm22_read(RFM22_device_status);
int_status1 = rfm22_read(RFM22_interrupt_status1);
int_status2 = rfm22_read(RFM22_interrupt_status2);
ezmac_status = rfm22_read(RFM22_ezmac_status);
// disable all interrupts
rfm22_write(RFM22_interrupt_enable1, 0x00);
rfm22_write(RFM22_interrupt_enable2, 0x00);
// ****************
exec_using_spi = FALSE;
// ****************
rf_mode = mode;
device_status = int_status1 = int_status2 = ezmac_status = 0;
rssi = 0;
rssi_dBm = -127;
rx_buffer_current = 0;
rx_buffer_wr = 0;
rx_packet_rssi_dBm = -127;
rx_packet_afc_Hz = 0;
tx_data_rd = tx_data_wr = 0;
lookup_index = 0;
ss_lookup_index = 0;
rf_bandwidth_used = 0;
ss_rf_bandwidth_used = 0;
rfm22_int_timer = 0;
rfm22_int_time_outs = 0;
prev_rfm22_int_time_outs = 0;
hbsel = 0;
frequency_step_size = 0.0f;
frequency_hop_channel = 0;
afc_correction = 0;
afc_correction_Hz = 0;
temperature_reg = 0;
// set the TX power
tx_power = RFM22_DEFAULT_RF_POWER;
tx_pwr = 0;
// ****************
// read the RF chip ID bytes
device_type = rfm22_read(RFM22_DEVICE_TYPE) & RFM22_DT_MASK; // read the device type
device_version = rfm22_read(RFM22_DEVICE_VERSION) & RFM22_DV_MASK; // read the device version
#if defined(RFM22_DEBUG)
DEBUG_PRINTF(2, "rf device type: %d\n\r", device_type);
DEBUG_PRINTF(2, "rf device version: %d\n\r", device_version);
#endif
if (device_type != 0x08)
{
#if defined(RFM22_DEBUG)
DEBUG_PRINTF(2, "rf device type: INCORRECT - should be 0x08\n\r");
#endif
return -1; // incorrect RF module type
}
// if (device_version != RFM22_DEVICE_VERSION_V2) // V2
// return -2; // incorrect RF module version
// if (device_version != RFM22_DEVICE_VERSION_A0) // A0
// return -2; // incorrect RF module version
if (device_version != RFM22_DEVICE_VERSION_B1) // B1
{
#if defined(RFM22_DEBUG)
DEBUG_PRINTF(2, "rf device version: INCORRECT\n\r");
#endif
return -2; // incorrect RF module version
}
// ****************
// set the minimum and maximum carrier frequency allowed
if (min_frequency_hz < RFM22_MIN_CARRIER_FREQUENCY_HZ) min_frequency_hz = RFM22_MIN_CARRIER_FREQUENCY_HZ;
else
if (min_frequency_hz > RFM22_MAX_CARRIER_FREQUENCY_HZ) min_frequency_hz = RFM22_MAX_CARRIER_FREQUENCY_HZ;
if (max_frequency_hz < RFM22_MIN_CARRIER_FREQUENCY_HZ) max_frequency_hz = RFM22_MIN_CARRIER_FREQUENCY_HZ;
else
if (max_frequency_hz > RFM22_MAX_CARRIER_FREQUENCY_HZ) max_frequency_hz = RFM22_MAX_CARRIER_FREQUENCY_HZ;
if (min_frequency_hz > max_frequency_hz)
{ // swap them over
uint32_t tmp = min_frequency_hz;
min_frequency_hz = max_frequency_hz;
max_frequency_hz = tmp;
}
lower_carrier_frequency_limit_Hz = min_frequency_hz;
upper_carrier_frequency_limit_Hz = max_frequency_hz;
// ****************
// calibrate our RF module to be exactly on frequency .. different for every module
osc_load_cap = OSC_LOAD_CAP; // default
rfm22_write(RFM22_xtal_osc_load_cap, osc_load_cap);
// ****************
// disable Low Duty Cycle Mode
rfm22_write(RFM22_op_and_func_ctrl2, 0x00);
rfm22_write(RFM22_cpu_output_clk, RFM22_coc_1MHz); // 1MHz clock output
rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_xton); // READY mode
// rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon); // TUNE mode
// choose the 3 GPIO pin functions
rfm22_write(RFM22_io_port_config, RFM22_io_port_default); // GPIO port use default value
rfm22_write(RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_txstate); // GPIO0 = TX State (to control RF Switch)
rfm22_write(RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_rxstate); // GPIO1 = RX State (to control RF Switch)
rfm22_write(RFM22_gpio2_config, RFM22_gpio2_config_drv3 | RFM22_gpio2_config_cca); // GPIO2 = Clear Channel Assessment
// ****************
return 0; // OK
}
// ************************************
int rfm22_init_scan_spectrum(uint32_t min_frequency_hz, uint32_t max_frequency_hz)
{
#if defined(RFM22_DEBUG)
DEBUG_PRINTF(2, "\n\rRF init scan spectrum\n\r");
#endif
int res = rfm22_resetModule(RX_SCAN_SPECTRUM, min_frequency_hz, max_frequency_hz);
if (res < 0)
return res;
// rfm22_setSSBandwidth(0);
rfm22_setSSBandwidth(1);
// FIFO mode, GFSK modulation
uint8_t fd_bit = rfm22_read(RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
rfm22_write(RFM22_modulation_mode_control2, RFM22_mmc2_trclk_clk_none | RFM22_mmc2_dtmod_fifo | fd_bit | RFM22_mmc2_modtyp_gfsk);
rfm22_write(RFM22_cpu_output_clk, RFM22_coc_1MHz); // 1MHz clock output
rfm22_write(RFM22_rssi_threshold_clear_chan_indicator, 0);
rfm22_write(RFM22_preamble_detection_ctrl1, 31 << 3); // 31-nibbles rx preamble detection
// avoid packet detection
rfm22_write(RFM22_data_access_control, RFM22_dac_enpacrx | RFM22_dac_encrc);
rfm22_write(RFM22_header_control1, 0x0f);
rfm22_write(RFM22_header_control2, 0x77);
rfm22_write(RFM22_sync_word3, SYNC_BYTE_1);
rfm22_write(RFM22_sync_word2, SYNC_BYTE_2);
rfm22_write(RFM22_sync_word1, SYNC_BYTE_3 ^ 0xff);
rfm22_write(RFM22_sync_word0, SYNC_BYTE_4 ^ 0xff);
// all the bits to be checked
rfm22_write(RFM22_header_enable3, 0xff);
rfm22_write(RFM22_header_enable2, 0xff);
rfm22_write(RFM22_header_enable1, 0xff);
rfm22_write(RFM22_header_enable0, 0xff);
// set frequency hopping channel step size (multiples of 10kHz)
// rfm22_write(RFM22_frequency_hopping_step_size, 0);
// set our nominal carrier frequency
rfm22_setNominalCarrierFrequency(min_frequency_hz);
// set minimum tx power
rfm22_write(RFM22_tx_power, RFM22_tx_pwr_lna_sw | 0);
rfm22_write(RFM22_agc_override1, RFM22_agc_ovr1_sgi | RFM22_agc_ovr1_agcen);
// rfm22_write(RFM22_vco_current_trimming, 0x7f);
// rfm22_write(RFM22_vco_calibration_override, 0x40);
// rfm22_write(RFM22_chargepump_current_trimming_override, 0x80);
// Enable RF module external interrupt
rfm22_enableExtInt();
rfm22_setRxMode(RX_SCAN_SPECTRUM, true);
initialized = true;
return 0; // OK
}
// ************************************
int rfm22_init_tx_stream(uint32_t min_frequency_hz, uint32_t max_frequency_hz)
{
#if defined(RFM22_DEBUG)
DEBUG_PRINTF(2, "\n\rRF init TX stream\n\r");
#endif
int res = rfm22_resetModule(TX_STREAM_MODE, min_frequency_hz, max_frequency_hz);
if (res < 0)
return res;
frequency_hop_step_size_reg = 0;
// set the RF datarate
rfm22_setDatarate(RFM22_DEFAULT_RF_DATARATE, FALSE);
// FIFO mode, GFSK modulation
uint8_t fd_bit = rfm22_read(RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
rfm22_write(RFM22_modulation_mode_control2, RFM22_mmc2_trclk_clk_none | RFM22_mmc2_dtmod_fifo | fd_bit | RFM22_mmc2_modtyp_gfsk);
// disable the internal Tx & Rx packet handlers (without CRC)
rfm22_write(RFM22_data_access_control, 0);
rfm22_write(RFM22_preamble_length, TX_PREAMBLE_NIBBLES); // x-nibbles tx preamble
rfm22_write(RFM22_preamble_detection_ctrl1, RX_PREAMBLE_NIBBLES << 3); // x-nibbles rx preamble detection
rfm22_write(RFM22_header_control1, RFM22_header_cntl1_bcen_none | RFM22_header_cntl1_hdch_none); // header control - we are not using the header
rfm22_write(RFM22_header_control2, RFM22_header_cntl2_fixpklen | RFM22_header_cntl2_hdlen_none | RFM22_header_cntl2_synclen_32 | ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); // no header bytes, synchronization word length 3, 2 used, packet length not included in header (fixed packet length).
rfm22_write(RFM22_sync_word3, SYNC_BYTE_1); // sync word
rfm22_write(RFM22_sync_word2, SYNC_BYTE_2); //
// rfm22_write(RFM22_modem_test, 0x01);
rfm22_write(RFM22_agc_override1, RFM22_agc_ovr1_agcen);
// rfm22_write(RFM22_agc_override1, RFM22_agc_ovr1_sgi | RFM22_agc_ovr1_agcen);
rfm22_write(RFM22_frequency_hopping_step_size, frequency_hop_step_size_reg); // set frequency hopping channel step size (multiples of 10kHz)
rfm22_setNominalCarrierFrequency((min_frequency_hz + max_frequency_hz) / 2); // set our nominal carrier frequency
rfm22_write(RFM22_tx_power, RFM22_tx_pwr_papeaken | RFM22_tx_pwr_papeaklvl_0 | RFM22_tx_pwr_lna_sw | tx_power); // set the tx power
// rfm22_write(RFM22_tx_power, RFM22_tx_pwr_lna_sw | tx_power); // set the tx power
// rfm22_write(RFM22_vco_current_trimming, 0x7f);
// rfm22_write(RFM22_vco_calibration_override, 0x40);
// rfm22_write(RFM22_chargepump_current_trimming_override, 0x80);
rfm22_write(RFM22_tx_fifo_control1, TX_FIFO_HI_WATERMARK); // TX FIFO Almost Full Threshold (0 - 63)
rfm22_write(RFM22_tx_fifo_control2, TX_FIFO_LO_WATERMARK); // TX FIFO Almost Empty Threshold (0 - 63)
// Enable RF module external interrupt
rfm22_enableExtInt();
initialized = true;
return 0; // OK
}
// ************************************
int rfm22_init_rx_stream(uint32_t min_frequency_hz, uint32_t max_frequency_hz)
{
#if defined(RFM22_DEBUG)
DEBUG_PRINTF(2, "\n\rRF init RX stream\n\r");
#endif
int res = rfm22_resetModule(RX_WAIT_PREAMBLE_MODE, min_frequency_hz, max_frequency_hz);
if (res < 0)
return res;
frequency_hop_step_size_reg = 0;
// set the RF datarate
rfm22_setDatarate(RFM22_DEFAULT_RF_DATARATE, FALSE);
// FIFO mode, GFSK modulation
uint8_t fd_bit = rfm22_read(RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
rfm22_write(RFM22_modulation_mode_control2, RFM22_mmc2_trclk_clk_none | RFM22_mmc2_dtmod_fifo | fd_bit | RFM22_mmc2_modtyp_gfsk);
// disable the internal Tx & Rx packet handlers (without CRC)
rfm22_write(RFM22_data_access_control, 0);
rfm22_write(RFM22_preamble_length, TX_PREAMBLE_NIBBLES); // x-nibbles tx preamble
rfm22_write(RFM22_preamble_detection_ctrl1, RX_PREAMBLE_NIBBLES << 3); // x-nibbles rx preamble detection
rfm22_write(RFM22_header_control1, RFM22_header_cntl1_bcen_none | RFM22_header_cntl1_hdch_none); // header control - we are not using the header
rfm22_write(RFM22_header_control2, RFM22_header_cntl2_fixpklen | RFM22_header_cntl2_hdlen_none | RFM22_header_cntl2_synclen_32 | ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); // no header bytes, synchronization word length 3, 2 used, packet length not included in header (fixed packet length).
rfm22_write(RFM22_sync_word3, SYNC_BYTE_1); // sync word
rfm22_write(RFM22_sync_word2, SYNC_BYTE_2); //
// no header bits to be checked
rfm22_write(RFM22_header_enable3, 0x00);
rfm22_write(RFM22_header_enable2, 0x00);
rfm22_write(RFM22_header_enable1, 0x00);
rfm22_write(RFM22_header_enable0, 0x00);
// rfm22_write(RFM22_modem_test, 0x01);
rfm22_write(RFM22_agc_override1, RFM22_agc_ovr1_agcen);
// rfm22_write(RFM22_agc_override1, RFM22_agc_ovr1_sgi | RFM22_agc_ovr1_agcen);
rfm22_write(RFM22_frequency_hopping_step_size, frequency_hop_step_size_reg); // set frequency hopping channel step size (multiples of 10kHz)
rfm22_setNominalCarrierFrequency((min_frequency_hz + max_frequency_hz) / 2); // set our nominal carrier frequency
// rfm22_write(RFM22_vco_current_trimming, 0x7f);
// rfm22_write(RFM22_vco_calibration_override, 0x40);
// rfm22_write(RFM22_chargepump_current_trimming_override, 0x80);
// RX FIFO Almost Full Threshold (0 - 63)
rfm22_write(RFM22_rx_fifo_control, RX_FIFO_HI_WATERMARK);
// Enable RF module external interrupt
rfm22_enableExtInt();
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
initialized = true;
return 0; // OK
}
// ************************************
// Initialise this hardware layer module and the rf module
int rfm22_init_normal(uint32_t id, uint32_t min_frequency_hz, uint32_t max_frequency_hz, uint32_t freq_hop_step_size)
{
int res = rfm22_resetModule(RX_WAIT_PREAMBLE_MODE, min_frequency_hz, max_frequency_hz);
if (res < 0)
return res;
// initialize the frequency hopping step size
freq_hop_step_size /= 10000; // in 10kHz increments
if (freq_hop_step_size > 255) freq_hop_step_size = 255;
frequency_hop_step_size_reg = freq_hop_step_size;
// set the RF datarate
rfm22_setDatarate(RFM22_DEFAULT_RF_DATARATE, TRUE);
// FIFO mode, GFSK modulation
uint8_t fd_bit = rfm22_read(RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
rfm22_write(RFM22_modulation_mode_control2, RFM22_mmc2_trclk_clk_none | RFM22_mmc2_dtmod_fifo | fd_bit | RFM22_mmc2_modtyp_gfsk);
// setup to read the internal temperature sensor
// ADC used to sample the temperature sensor
adc_config = RFM22_ac_adcsel_temp_sensor | RFM22_ac_adcref_bg;
rfm22_write(RFM22_adc_config, adc_config);
// adc offset
rfm22_write(RFM22_adc_sensor_amp_offset, 0);
// temp sensor calibration .. –40C to +64C 0.5C resolution
rfm22_write(RFM22_temp_sensor_calib, RFM22_tsc_tsrange0 | RFM22_tsc_entsoffs);
// temp sensor offset
rfm22_write(RFM22_temp_value_offset, 0);
// start an ADC conversion
rfm22_write(RFM22_adc_config, adc_config | RFM22_ac_adcstartbusy);
// set the RSSI threshold interrupt to about -90dBm
rfm22_write(RFM22_rssi_threshold_clear_chan_indicator, (-90 + 122) * 2);
// enable the internal Tx & Rx packet handlers (without CRC)
rfm22_write(RFM22_data_access_control, RFM22_dac_enpacrx | RFM22_dac_enpactx);
// x-nibbles tx preamble
rfm22_write(RFM22_preamble_length, TX_PREAMBLE_NIBBLES);
// x-nibbles rx preamble detection
rfm22_write(RFM22_preamble_detection_ctrl1, RX_PREAMBLE_NIBBLES << 3);
#ifdef PIOS_RFM22_NO_HEADER
// header control - we are not using the header
rfm22_write(RFM22_header_control1, RFM22_header_cntl1_bcen_none | RFM22_header_cntl1_hdch_none);
// no header bytes, synchronization word length 3, 2, 1 & 0 used, packet length included in header.
rfm22_write(RFM22_header_control2, RFM22_header_cntl2_hdlen_none |
RFM22_header_cntl2_synclen_3210 | ((TX_PREAMBLE_NIBBLES >> 8) & 0x01));
#else
// header control - using a 4 by header with broadcast of 0xffffffff
rfm22_write(RFM22_header_control1,
RFM22_header_cntl1_bcen_0 |
RFM22_header_cntl1_bcen_1 |
RFM22_header_cntl1_bcen_2 |
RFM22_header_cntl1_bcen_3 |
RFM22_header_cntl1_hdch_0 |
RFM22_header_cntl1_hdch_1 |
RFM22_header_cntl1_hdch_2 |
RFM22_header_cntl1_hdch_3);
// Check all bit of all bytes of the header
rfm22_write(RFM22_header_enable0, 0xff);
rfm22_write(RFM22_header_enable1, 0xff);
rfm22_write(RFM22_header_enable2, 0xff);
rfm22_write(RFM22_header_enable3, 0xff);
// Set the ID to be checked
rfm22_write(RFM22_check_header0, id & 0xff);
rfm22_write(RFM22_check_header1, (id >> 8) & 0xff);
rfm22_write(RFM22_check_header2, (id >> 16) & 0xff);
rfm22_write(RFM22_check_header3, (id >> 24) & 0xff);
// 4 header bytes, synchronization word length 3, 2, 1 & 0 used, packet length included in header.
rfm22_write(RFM22_header_control2,
RFM22_header_cntl2_hdlen_3210 |
RFM22_header_cntl2_synclen_3210 |
((TX_PREAMBLE_NIBBLES >> 8) & 0x01));
#endif
// sync word
rfm22_write(RFM22_sync_word3, SYNC_BYTE_1);
rfm22_write(RFM22_sync_word2, SYNC_BYTE_2);
rfm22_write(RFM22_sync_word1, SYNC_BYTE_3);
rfm22_write(RFM22_sync_word0, SYNC_BYTE_4);
rfm22_write(RFM22_agc_override1, RFM22_agc_ovr1_agcen);
// set frequency hopping channel step size (multiples of 10kHz)
rfm22_write(RFM22_frequency_hopping_step_size, frequency_hop_step_size_reg);
// set our nominal carrier frequency
rfm22_setNominalCarrierFrequency((min_frequency_hz + max_frequency_hz) / 2);
// set the tx power
rfm22_write(RFM22_tx_power, RFM22_tx_pwr_papeaken | RFM22_tx_pwr_papeaklvl_0 |
RFM22_tx_pwr_lna_sw | tx_power);
// TX FIFO Almost Full Threshold (0 - 63)
rfm22_write(RFM22_tx_fifo_control1, TX_FIFO_HI_WATERMARK);
// TX FIFO Almost Empty Threshold (0 - 63)
rfm22_write(RFM22_tx_fifo_control2, TX_FIFO_LO_WATERMARK);
// RX FIFO Almost Full Threshold (0 - 63)
rfm22_write(RFM22_rx_fifo_control, RX_FIFO_HI_WATERMARK);
// Enable RF module external interrupt
rfm22_enableExtInt();
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
initialized = true;
return 0; // ok
}
// ************************************
#endif
/**
* @}
* @}
*/

166
flight/PiOS/Common/pios_usb_desc_hid_cdc.c
View File

@ -38,9 +38,9 @@ static const struct usb_device_desc device_desc = {
.bLength = sizeof(struct usb_device_desc),
.bDescriptorType = USB_DESC_TYPE_DEVICE,
.bcdUSB = htousbs(0x0200),
.bDeviceClass = 0x02,
.bDeviceSubClass = 0x00,
.bDeviceProtocol = 0x00,
.bDeviceClass = 0xef,
.bDeviceSubClass = 0x02,
.bDeviceProtocol = 0x01,
.bMaxPacketSize0 = 64, /* Must be 64 for high-speed devices */
.idVendor = htousbs(USB_VENDOR_ID_OPENPILOT),
.idProduct = htousbs(PIOS_USB_BOARD_PRODUCT_ID),
@ -98,10 +98,6 @@ static const uint8_t hid_report_desc[36] = {
struct usb_config_hid_cdc {
struct usb_configuration_desc config;
struct usb_interface_association_desc iad;
struct usb_interface_desc hid_if;
struct usb_hid_desc hid;
struct usb_endpoint_desc hid_in;
struct usb_endpoint_desc hid_out;
struct usb_interface_desc cdc_control_if;
struct usb_cdc_header_func_desc cdc_header;
struct usb_cdc_callmgmt_func_desc cdc_callmgmt;
@ -111,6 +107,10 @@ struct usb_config_hid_cdc {
struct usb_interface_desc cdc_data_if;
struct usb_endpoint_desc cdc_in;
struct usb_endpoint_desc cdc_out;
struct usb_interface_desc hid_if;
struct usb_hid_desc hid;
struct usb_endpoint_desc hid_in;
struct usb_endpoint_desc hid_out;
} __attribute__((packed));
static const struct usb_config_hid_cdc config_hid_cdc = {
@ -129,15 +129,87 @@ static const struct usb_config_hid_cdc config_hid_cdc = {
.bDescriptorType = USB_DESC_TYPE_IAD,
.bFirstInterface = 0,
.bInterfaceCount = 2,
.bFunctionClass = 2, /* Communication */
.bFunctionSubClass = 2, /* Abstract Control Model */
.bFunctionProtocol = 0, /* V.25ter, Common AT commands */
.bFunctionClass = USB_INTERFACE_CLASS_CDC, /* Communication */
.bFunctionSubClass = USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL, /* Abstract Control Model */
.bFunctionProtocol = 1, /* V.25ter, Common AT commands */
.iInterface = 0,
},
.cdc_control_if = {
.bLength = sizeof(struct usb_interface_desc),
.bDescriptorType = USB_DESC_TYPE_INTERFACE,
.bInterfaceNumber = 0,
.bAlternateSetting = 0,
.bNumEndpoints = 1,
.bInterfaceClass = USB_INTERFACE_CLASS_CDC,
.bInterfaceSubClass = USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL, /* Abstract Control Model */
.nInterfaceProtocol = 1, /* V.25ter, Common AT commands */
.iInterface = 0,
},
.cdc_header = {
.bLength = sizeof(struct usb_cdc_header_func_desc),
.bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC,
.bDescriptorSubType = USB_CDC_DESC_SUBTYPE_HEADER,
.bcdCDC = htousbs(0x0110),
},
.cdc_callmgmt = {
.bLength = sizeof(struct usb_cdc_callmgmt_func_desc),
.bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC,
.bDescriptorSubType = USB_CDC_DESC_SUBTYPE_CALLMGMT,
.bmCapabilities = 0x00, /* No call handling */
.bDataInterface = 1,
},
.cdc_acm = {
.bLength = sizeof(struct usb_cdc_acm_func_desc),
.bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC,
.bDescriptorSubType = USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL,
.bmCapabilities = 0x00,
},
.cdc_union = {
.bLength = sizeof(struct usb_cdc_union_func_desc),
.bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC,
.bDescriptorSubType = USB_CDC_DESC_SUBTYPE_UNION,
.bMasterInterface = 0,
.bSlaveInterface = 1,
},
.cdc_mgmt_in = {
.bLength = sizeof(struct usb_endpoint_desc),
.bDescriptorType = USB_DESC_TYPE_ENDPOINT,
.bEndpointAddress = USB_EP_IN(2),
.bmAttributes = USB_EP_ATTR_TT_INTERRUPT,
.wMaxPacketSize = htousbs(PIOS_USB_BOARD_CDC_MGMT_LENGTH),
.bInterval = 4, /* ms */
},
.cdc_data_if = {
.bLength = sizeof(struct usb_interface_desc),
.bDescriptorType = USB_DESC_TYPE_INTERFACE,
.bInterfaceNumber = 1,
.bAlternateSetting = 0,
.bNumEndpoints = 2,
.bInterfaceClass = USB_INTERFACE_CLASS_DATA,
.bInterfaceSubClass = 0,
.nInterfaceProtocol = 0, /* No class specific protocol */
.iInterface = 0,
},
.cdc_in = {
.bLength = sizeof(struct usb_endpoint_desc),
.bDescriptorType = USB_DESC_TYPE_ENDPOINT,
.bEndpointAddress = USB_EP_IN(3),
.bmAttributes = USB_EP_ATTR_TT_BULK,
.wMaxPacketSize = htousbs(PIOS_USB_BOARD_CDC_DATA_LENGTH),
.bInterval = 0, /* ms */
},
.cdc_out = {
.bLength = sizeof(struct usb_endpoint_desc),
.bDescriptorType = USB_DESC_TYPE_ENDPOINT,
.bEndpointAddress = USB_EP_OUT(3),
.bmAttributes = USB_EP_ATTR_TT_BULK, /* Bulk */
.wMaxPacketSize = htousbs(PIOS_USB_BOARD_CDC_DATA_LENGTH),
.bInterval = 0, /* ms */
},
.hid_if = {
.bLength = sizeof(struct usb_interface_desc),
.bDescriptorType = USB_DESC_TYPE_INTERFACE,
.bInterfaceNumber = 0,
.bInterfaceNumber = 2,
.bAlternateSetting = 0,
.bNumEndpoints = 2,
.bInterfaceClass = USB_INTERFACE_CLASS_HID,
@ -170,78 +242,6 @@ static const struct usb_config_hid_cdc config_hid_cdc = {
.wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH),
.bInterval = 4, /* ms */
},
.cdc_control_if = {
.bLength = sizeof(struct usb_interface_desc),
.bDescriptorType = USB_DESC_TYPE_INTERFACE,
.bInterfaceNumber = 1,
.bAlternateSetting = 0,
.bNumEndpoints = 1,
.bInterfaceClass = USB_INTERFACE_CLASS_CDC,
.bInterfaceSubClass = 2, /* Abstract Control Model */
.nInterfaceProtocol = 1, /* V.25ter, Common AT commands */
.iInterface = 0,
},
.cdc_header = {
.bLength = sizeof(struct usb_cdc_header_func_desc),
.bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC,
.bDescriptorSubType = USB_CDC_DESC_SUBTYPE_HEADER,
.bcdCDC = htousbs(0x0110),
},
.cdc_callmgmt = {
.bLength = sizeof(struct usb_cdc_callmgmt_func_desc),
.bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC,
.bDescriptorSubType = USB_CDC_DESC_SUBTYPE_CALLMGMT,
.bmCapabilities = 0x00, /* No call handling */
.bDataInterface = 2,
},
.cdc_acm = {
.bLength = sizeof(struct usb_cdc_acm_func_desc),
.bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC,
.bDescriptorSubType = USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL,
.bmCapabilities = 0,
},
.cdc_union = {
.bLength = sizeof(struct usb_cdc_union_func_desc),
.bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC,
.bDescriptorSubType = USB_CDC_DESC_SUBTYPE_UNION,
.bMasterInterface = 1,
.bSlaveInterface = 2,
},
.cdc_mgmt_in = {
.bLength = sizeof(struct usb_endpoint_desc),
.bDescriptorType = USB_DESC_TYPE_ENDPOINT,
.bEndpointAddress = USB_EP_IN(2),
.bmAttributes = USB_EP_ATTR_TT_INTERRUPT,
.wMaxPacketSize = htousbs(PIOS_USB_BOARD_CDC_MGMT_LENGTH),
.bInterval = 4, /* ms */
},
.cdc_data_if = {
.bLength = sizeof(struct usb_interface_desc),
.bDescriptorType = USB_DESC_TYPE_INTERFACE,
.bInterfaceNumber = 2,
.bAlternateSetting = 0,
.bNumEndpoints = 2,
.bInterfaceClass = USB_INTERFACE_CLASS_DATA,
.bInterfaceSubClass = 0,
.nInterfaceProtocol = 0, /* No class specific protocol */
.iInterface = 0,
},
.cdc_in = {
.bLength = sizeof(struct usb_endpoint_desc),
.bDescriptorType = USB_DESC_TYPE_ENDPOINT,
.bEndpointAddress = USB_EP_IN(3),
.bmAttributes = USB_EP_ATTR_TT_BULK,
.wMaxPacketSize = htousbs(PIOS_USB_BOARD_CDC_DATA_LENGTH),
.bInterval = 0, /* ms */
},
.cdc_out = {
.bLength = sizeof(struct usb_endpoint_desc),
.bDescriptorType = USB_DESC_TYPE_ENDPOINT,
.bEndpointAddress = USB_EP_OUT(3),
.bmAttributes = USB_EP_ATTR_TT_BULK, /* Bulk */
.wMaxPacketSize = htousbs(PIOS_USB_BOARD_CDC_DATA_LENGTH),
.bInterval = 0, /* ms */
},
};
int32_t PIOS_USB_DESC_HID_CDC_Init(void)

34
flight/PipXtreme/inc/ppm.h → flight/PiOS/Common/pios_usb_util.c
View File

@ -1,9 +1,14 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB_UTIL USB utility functions
* @brief USB utility functions
* @{
*
* @file ppm.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Sends or Receives the ppm values to/from the remote unit
* @file pios_usb_util.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief USB utility functions
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
@ -23,18 +28,15 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PPM_H_
#define PPM_H_
#include "pios_usb_util.h"
#include "stdint.h"
uint8_t * PIOS_USB_UTIL_AsciiToUtf8(uint8_t * dst, uint8_t * src, uint16_t srclen)
{
for (uint8_t i = 0; i < srclen; i++) {
*dst = *src;
dst += 2;
src += 1;
}
// *************************************************************
void ppm_1ms_tick(void);
void ppm_process(void);
void ppm_deinit(void);
void ppm_init(uint32_t our_sn);
// *************************************************************
#endif
return dst;
}

56
flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_BL_sections.ld
View File

@ -3,7 +3,8 @@ PROVIDE ( vPortSVCHandler = 0 ) ;
PROVIDE ( xPortPendSVHandler = 0 ) ;
PROVIDE ( xPortSysTickHandler = 0 ) ;
_estack = 0x20004FF0;
/* This is the size of the stack for early init and for all FreeRTOS IRQs */
_irq_stack_size = 0x400;
/* Section Definitions */
SECTIONS
@ -49,6 +50,24 @@ SECTIONS
_ebss = . ;
} > SRAM
/*
* This stack is used both as the initial sp during early init as well as ultimately
* being used as the STM32's MSP (Main Stack Pointer) which is the same stack that
* is used for _all_ interrupt handlers. The end of this stack should be placed
* against the lowest address in RAM so that a stack overrun results in a hard fault
* at the first access beyond the end of the stack.
*/
.irq_stack :
{
. = ALIGN(4);
_irq_stack_end = . ;
. = . + _irq_stack_size ;
. = ALIGN(4);
_irq_stack_top = . - 4 ;
_init_stack_top = _irq_stack_top;
. = ALIGN(4);
} > SRAM
. = ALIGN(4);
_end = . ;
@ -56,6 +75,39 @@ SECTIONS
{
. = ALIGN(4);
KEEP(*(.boardinfo))
. = ALIGN(4);
} > BD_INFO
/* Stabs debugging sections. */
.stab 0 : { *(.stab) }
.stabstr 0 : { *(.stabstr) }
.stab.excl 0 : { *(.stab.excl) }
.stab.exclstr 0 : { *(.stab.exclstr) }
.stab.index 0 : { *(.stab.index) }
.stab.indexstr 0 : { *(.stab.indexstr) }
.comment 0 : { *(.comment) }
/* DWARF debug sections.
Symbols in the DWARF debugging sections are relative to the beginning
of the section so we begin them at 0. */
/* DWARF 1 */
.debug 0 : { *(.debug) }
.line 0 : { *(.line) }
/* GNU DWARF 1 extensions */
.debug_srcinfo 0 : { *(.debug_srcinfo) }
.debug_sfnames 0 : { *(.debug_sfnames) }
/* DWARF 1.1 and DWARF 2 */
.debug_aranges 0 : { *(.debug_aranges) }
.debug_pubnames 0 : { *(.debug_pubnames) }
/* DWARF 2 */
.debug_info 0 : { *(.debug_info .gnu.linkonce.wi.*) }
.debug_abbrev 0 : { *(.debug_abbrev) }
.debug_line 0 : { *(.debug_line) }
.debug_frame 0 : { *(.debug_frame) }
.debug_str 0 : { *(.debug_str) }
.debug_loc 0 : { *(.debug_loc) }
.debug_macinfo 0 : { *(.debug_macinfo) }
/* SGI/MIPS DWARF 2 extensions */
.debug_weaknames 0 : { *(.debug_weaknames) }
.debug_funcnames 0 : { *(.debug_funcnames) }
.debug_typenames 0 : { *(.debug_typenames) }
.debug_varnames 0 : { *(.debug_varnames) }
}

107
flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_sections.ld
View File

@ -1,3 +1,8 @@
/* This is the size of the stack for all FreeRTOS IRQs */
_irq_stack_size = 0x1A0;
/* This is the size of the stack for early init: life span is until scheduler starts */
_init_stack_size = 0x100;
/* Stub out these functions since we don't use them anyway */
PROVIDE ( vPortSVCHandler = 0 ) ;
PROVIDE ( xPortPendSVHandler = 0 ) ;
@ -5,8 +10,6 @@ PROVIDE ( xPortSysTickHandler = 0 ) ;
PROVIDE(pios_board_info_blob = ORIGIN(BD_INFO));
_estack = 0x20004FF0;
/* Section Definitions */
SECTIONS
{
@ -19,6 +22,16 @@ SECTIONS
*(.rodata .rodata* .gnu.linkonce.r.*)
} > FLASH
/* module sections */
.initcallmodule.init :
{
. = ALIGN(4);
__module_initcall_start = .;
KEEP(*(.initcallmodule.init))
. = ALIGN(4);
__module_initcall_end = .;
} >FLASH
.ARM.extab :
{
*(.ARM.extab* .gnu.linkonce.armextab.*)
@ -33,6 +46,23 @@ SECTIONS
_etext = .;
_sidata = .;
/*
* This stack is used both as the initial sp during early init as well as ultimately
* being used as the STM32's MSP (Main Stack Pointer) which is the same stack that
* is used for _all_ interrupt handlers. The end of this stack should be placed
* against the lowest address in RAM so that a stack overrun results in a hard fault
* at the first access beyond the end of the stack.
*/
.irq_stack :
{
. = ALIGN(4);
_irq_stack_end = . ;
. = . + _irq_stack_size ;
. = ALIGN(4);
_irq_stack_top = . - 4 ;
. = ALIGN(4);
} > SRAM
.data : AT (_etext)
{
_sdata = .;
@ -41,16 +71,81 @@ SECTIONS
_edata = . ;
} > SRAM
/* .bss section which is used for uninitialized data */
.bss (NOLOAD) :
{
_sbss = . ;
*(.bss .bss.*)
*(COMMON)
. = ALIGN(4);
_ebss = . ;
} > SRAM
. = ALIGN(4);
_end = . ;
.heap (NOLOAD) :
{
. = ALIGN(4);
_sheap = . ;
_sheap_pre_rtos = . ;
*(.heap)
. = ALIGN(4);
_eheap = . ;
_eheap_pre_rtos = . ;
_init_stack_end = . ;
_sheap_post_rtos = . ;
. = . + _init_stack_size ;
. = ALIGN(4);
_eheap_post_rtos = . ;
_init_stack_top = . - 4 ;
} > SRAM
_free_ram = . ;
.free_ram (NOLOAD) :
{
. = ORIGIN(SRAM) + LENGTH(SRAM) - _free_ram ;
/* This is used by the startup in order to initialize the .bss section */
_ebss = . ;
_eram = . ;
} > SRAM
/* keep the heap section at the end of the SRAM
* this will allow to claim the remaining bytes not used
* at run time! (done by the reset vector).
*/
PROVIDE ( _end = _ebss ) ;
/* Stabs debugging sections. */
.stab 0 : { *(.stab) }
.stabstr 0 : { *(.stabstr) }
.stab.excl 0 : { *(.stab.excl) }
.stab.exclstr 0 : { *(.stab.exclstr) }
.stab.index 0 : { *(.stab.index) }
.stab.indexstr 0 : { *(.stab.indexstr) }
.comment 0 : { *(.comment) }
/* DWARF debug sections.
Symbols in the DWARF debugging sections are relative to the beginning
of the section so we begin them at 0. */
/* DWARF 1 */
.debug 0 : { *(.debug) }
.line 0 : { *(.line) }
/* GNU DWARF 1 extensions */
.debug_srcinfo 0 : { *(.debug_srcinfo) }
.debug_sfnames 0 : { *(.debug_sfnames) }
/* DWARF 1.1 and DWARF 2 */
.debug_aranges 0 : { *(.debug_aranges) }
.debug_pubnames 0 : { *(.debug_pubnames) }
/* DWARF 2 */
.debug_info 0 : { *(.debug_info .gnu.linkonce.wi.*) }
.debug_abbrev 0 : { *(.debug_abbrev) }
.debug_line 0 : { *(.debug_line) }
.debug_frame 0 : { *(.debug_frame) }
.debug_str 0 : { *(.debug_str) }
.debug_loc 0 : { *(.debug_loc) }
.debug_macinfo 0 : { *(.debug_macinfo) }
/* SGI/MIPS DWARF 2 extensions */
.debug_weaknames 0 : { *(.debug_weaknames) }
.debug_funcnames 0 : { *(.debug_funcnames) }
.debug_typenames 0 : { *(.debug_typenames) }
.debug_varnames 0 : { *(.debug_varnames) }
}

155
flight/PiOS/STM32F10x/pios_eeprom.c Normal file
View File

@ -0,0 +1,155 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_EEPROM EEPROM reading/writing functions
* @brief PIOS EEPROM reading/writing functions
* @{
*
* @file pios_eeprom.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief COM layer functions
* @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
*/
/* Project Includes */
#include <pios.h>
#include <pios_crc.h>
#include <stm32f10x_flash.h>
#include <pios_board_info.h>
#include <pios_eeprom.h>
static struct pios_eeprom_cfg config;
/**
* Initialize the flash eeprom device
* \param cfg The configuration structure.
*/
void PIOS_EEPROM_Init(const struct pios_eeprom_cfg *cfg)
{
config = *cfg;
}
/**
* Save a block of data to the flash eeprom device.
* \param data A pointer to the data to write.
* \param len The length of data to write.
* \return 0 on sucess
*/
int32_t PIOS_EEPROM_Save(uint8_t *data, uint32_t len)
{
// We need to write 32 bit words, so extend the length to be an even multiple of 4 bytes,
// and include 4 bytes for the 32 bit CRC.
uint32_t nwords = (len / 4) + 1 + (len % 4 ? 1 : 0);
uint32_t size = nwords * 4;
// Ensure that the length is not longer than the max size.
if (size > config.max_size)
return -1;
// Calculate a 32 bit CRC of the data.
uint32_t crc = PIOS_CRC32_updateCRC(0xffffffff, data, len);
// Unlock the Flash Program Erase controller
FLASH_Unlock();
// See if we have to write the data.
if ((memcmp(data, (uint8_t*)config.base_address, len) == 0) &&
(memcmp((uint8_t*)&crc, (uint8_t*)config.base_address + size - 4, 4) == 0))
return 0;
// TODO: Check that the area isn't already erased
// Erase page
FLASH_Status fs = FLASH_ErasePage(config.base_address);
if (fs != FLASH_COMPLETE)
{ // error
FLASH_Lock();
return -2;
}
// write 4 bytes at a time into program flash area (emulated EEPROM area)
uint8_t *p1 = data;
uint32_t *p3 = (uint32_t *)config.base_address;
for (int32_t i = 0; i < size; p3++)
{
uint32_t value = 0;
if (i == (size - 4))
{
// write the CRC.
value = crc;
i += 4;
}
else
{
if (i < len) value |= (uint32_t)*p1++ << 0; else value |= 0x000000ff; i++;
if (i < len) value |= (uint32_t)*p1++ << 8; else value |= 0x0000ff00; i++;
if (i < len) value |= (uint32_t)*p1++ << 16; else value |= 0x00ff0000; i++;
if (i < len) value |= (uint32_t)*p1++ << 24; else value |= 0xff000000; i++;
}
// write a 32-bit value
fs = FLASH_ProgramWord((uint32_t)p3, value);
if (fs != FLASH_COMPLETE)
{
FLASH_Lock();
return -3;
}
}
// Lock the Flash Program Erase controller
FLASH_Lock();
return 0;
}
/**
* Reads a block of data from the flash eeprom device.
* \param data A pointer to the output data buffer.
* \param len The length of data to read.
* \return 0 on sucess
*/
int32_t PIOS_EEPROM_Load(uint8_t *data, uint32_t len)
{
// We need to write 32 bit words, so the length should have been extended
// to an even multiple of 4 bytes, and should include 4 bytes for the 32 bit CRC.
uint32_t nwords = (len / 4) + 1 + (len % 4 ? 1 : 0);
uint32_t size = nwords * 4;
// Ensure that the length is not longer than the max size.
if (size > config.max_size)
return -1;
// Read the data from flash.
memcpy(data, (uint8_t*)config.base_address, len);
// Read the CRC.
uint32_t crc_flash = *((uint32_t*)(config.base_address + size - 4));
// Calculate a 32 bit CRC of the data.
uint32_t crc = PIOS_CRC32_updateCRC(0xffffffff, data, len);
if(crc != crc_flash)
return -2;
return 0;
}

8
flight/PiOS/STM32F10x/pios_sys.c
View File

@ -118,8 +118,8 @@ uint32_t PIOS_SYS_getCPUFlashSize(void)
/**
* Returns the serial number as a string
* param[out] str pointer to a string which can store at least 32 digits + zero terminator!
* (24 digits returned for STM32)
* param[out] uint8_t pointer to a string which can store at least 12 bytes
* (12 bytes returned for STM32)
* return < 0 if feature not supported
*/
int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t *array)
@ -127,7 +127,7 @@ int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t *array)
int i;
/* Stored in the so called "electronic signature" */
for (i = 0; i < 12; ++i) {
for (i = 0; i < PIOS_SYS_SERIAL_NUM_BINARY_LEN; ++i) {
uint8_t b = MEM8(0x1ffff7e8 + i);
array[i] = b;
@ -148,7 +148,7 @@ int32_t PIOS_SYS_SerialNumberGet(char *str)
int i;
/* Stored in the so called "electronic signature" */
for (i = 0; i < 24; ++i) {
for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) {
uint8_t b = MEM8(0x1ffff7e8 + (i / 2));
if (!(i & 1))
b >>= 4;

25
flight/PiOS/STM32F10x/pios_usb.c
View File

@ -40,7 +40,7 @@
#if defined(PIOS_INCLUDE_USB_HID)
/* Rx/Tx status */
static uint8_t transfer_possible = 0;
static bool transfer_possible = false;
enum pios_usb_dev_magic {
PIOS_USB_DEV_MAGIC = 0x17365904,
@ -152,7 +152,7 @@ int32_t PIOS_USB_ChangeConnectionState(bool Connected)
{
// In all cases: re-initialise USB HID driver
if (Connected) {
transfer_possible = 1;
transfer_possible = true;
//TODO: Check SetEPRxValid(ENDP1);
@ -161,7 +161,7 @@ int32_t PIOS_USB_ChangeConnectionState(bool Connected)
#endif
} else {
// Cable disconnected: disable transfers
transfer_possible = 0;
transfer_possible = false;
#if defined(USB_LED_OFF)
USB_LED_OFF; // turn the USB led off
@ -207,23 +207,30 @@ int32_t PIOS_USB_Reenumerate()
return 0;
}
bool PIOS_USB_CableConnected(uint8_t id)
{
struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_com_id;
if (PIOS_USB_validate(usb_dev) != 0)
return false;
return usb_dev->cfg->vsense.gpio->IDR & usb_dev->cfg->vsense.init.GPIO_Pin;
}
/**
* This function returns the connection status of the USB HID interface
* \return 1: interface available
* \return 0: interface not available
* \note Applications shouldn't call this function directly, instead please use \ref PIOS_COM layer functions
*/
uint32_t usb_found;
bool PIOS_USB_CheckAvailable(uint8_t id)
{
struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_com_id;
if(PIOS_USB_validate(usb_dev) != 0)
return 0;
if (PIOS_USB_validate(usb_dev) != 0)
return false;
usb_found = (usb_dev->cfg->vsense.gpio->IDR & usb_dev->cfg->vsense.init.GPIO_Pin);
return usb_found;
return usb_found != 0 && transfer_possible ? 1 : 0;
return PIOS_USB_CableConnected(id) && transfer_possible;
}
#endif

20
flight/PiOS/STM32F10x/pios_usbhook.c
View File

@ -304,7 +304,11 @@ static RESULT PIOS_USBHOOK_Data_Setup(uint8_t RequestNo)
switch (Type_Recipient) {
case (STANDARD_REQUEST | INTERFACE_RECIPIENT):
switch (pInformation->USBwIndex0) {
#if defined(PIOS_INCLUDE_USB_CDC)
case 2: /* HID Interface */
#else
case 0: /* HID Interface */
#endif
switch (RequestNo) {
case GET_DESCRIPTOR:
switch (pInformation->USBwValue1) {
@ -321,7 +325,11 @@ static RESULT PIOS_USBHOOK_Data_Setup(uint8_t RequestNo)
case (CLASS_REQUEST | INTERFACE_RECIPIENT):
switch (pInformation->USBwIndex0) {
#if defined(PIOS_INCLUDE_USB_CDC)
case 2: /* HID Interface */
#else
case 0: /* HID Interface */
#endif
switch (RequestNo) {
case USB_HID_REQ_GET_PROTOCOL:
CopyRoutine = PIOS_USBHOOK_GetProtocolValue;
@ -330,16 +338,16 @@ static RESULT PIOS_USBHOOK_Data_Setup(uint8_t RequestNo)
break;
#if defined(PIOS_INCLUDE_USB_CDC)
case 1: /* CDC Call Control Interface */
case 0: /* CDC Call Control Interface */
switch (RequestNo) {
case USB_CDC_REQ_GET_LINE_CODING:
CopyRoutine = PIOS_USB_CDC_GetLineCoding;
//CopyRoutine = PIOS_USB_CDC_GetLineCoding;
break;
}
break;
case 2: /* CDC Data Interface */
case 1: /* CDC Data Interface */
switch (RequestNo) {
case 0:
break;
@ -376,7 +384,11 @@ static RESULT PIOS_USBHOOK_NoData_Setup(uint8_t RequestNo)
switch (Type_Recipient) {
case (CLASS_REQUEST | INTERFACE_RECIPIENT):
switch (pInformation->USBwIndex0) {
#if defined(PIOS_INCLUDE_USB_CDC)
case 2: /* HID */
#else
case 0: /* HID */
#endif
switch (RequestNo) {
case USB_HID_REQ_SET_PROTOCOL:
return PIOS_USBHOOK_SetProtocol();
@ -386,7 +398,7 @@ static RESULT PIOS_USBHOOK_NoData_Setup(uint8_t RequestNo)
break;
#if defined(PIOS_INCLUDE_USB_CDC)
case 1: /* CDC Call Control Interface */
case 0: /* CDC Call Control Interface */
switch (RequestNo) {
case USB_CDC_REQ_SET_LINE_CODING:
return PIOS_USB_CDC_SetLineCoding();

427
flight/PiOS/STM32F10x/startup_stm32f10x_MD_PX.S Normal file
View File

@ -0,0 +1,427 @@
/**
******************************************************************************
* @file startup_stm32f10x_md.s
* @author MCD Application Team / Angus Peart
* @version V3.1.2
* @date 09/28/2009
* @brief STM32F10x Medium Density Devices vector table for RIDE7 toolchain.
* This module performs:
* - Set the initial SP
* - Set the initial PC == Reset_Handler,
* - Set the vector table entries with the exceptions ISR address
* - Branches to main in the C library (which eventually
* calls main()).
* After Reset the Cortex-M3 processor is in Thread mode,
* priority is Privileged, and the Stack is set to Main.
*******************************************************************************
* @copy
*
* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
* TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
* DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
* FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
* CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*
* <h2><center>&copy; COPYRIGHT 2009 STMicroelectronics</center></h2>
*/
.syntax unified
.cpu cortex-m3
.fpu softvfp
.thumb
.global g_pfnVectors
.global Default_Handler
.global xPortIncreaseHeapSize
.global Stack_Change
/* start address for the initialization values of the .data section.
defined in linker script */
.word _sidata
/* start address for the .data section. defined in linker script */
.word _sdata
/* end address for the .data section. defined in linker script */
.word _edata
/* start address for the .bss section. defined in linker script */
.word _sbss
/* end address for the .bss section. defined in linker script */
.word _ebss
.equ BootRAM, 0xF108F85F
/**
* @brief This is the code that gets called when the processor first
* starts execution following a reset event. Only the absolutely
* necessary set is performed, after which the application
* supplied main() routine is called.
* @param None
* @retval : None
*/
.section .text.Reset_Handler
.weak Reset_Handler
.type Reset_Handler, %function
Reset_Handler:
/*
* From COrtex-M3 reference manual:
* - Handler IRQ always use SP_main
* - Process use SP_main or SP_process
* Here, we will use beginning of SRAM for IRQ (SP_main)
* and end of heap for initialization (SP_process).
* Once the schedule starts, all threads will use their own stack
* from heap and NOBOBY should use SP_process again.
*/
/* Do set/reset the stack pointers */
LDR r0, =_irq_stack_top
MSR msp, r0
LDR r2, =_init_stack_top
MSR psp, r2
/* check if irq and init stack are the same */
/* if they are, we don't do stack swap */
/* and lets bypass the monitoring as well for now */
cmp r0, r2
beq SectionBssInit
/* DO
* - stay in thread process mode
* - stay in privilege level
* - use process stack
*/
movs r0, #2
MSR control, r0
ISB
/* Fill IRQ stack for watermark monitoring */
ldr r2, =_irq_stack_end
b LoopFillIRQStack
FillIRQStack:
movw r3, #0xA5A5
str r3, [r2], #4
LoopFillIRQStack:
ldr r3, = _irq_stack_top
cmp r2, r3
bcc FillIRQStack
SectionBssInit:
/* Copy the data segment initializers from flash to SRAM */
movs r1, #0
b LoopCopyDataInit
CopyDataInit:
ldr r3, =_sidata
ldr r3, [r3, r1]
str r3, [r0, r1]
adds r1, r1, #4
LoopCopyDataInit:
ldr r0, =_sdata
ldr r3, =_edata
adds r2, r0, r1
cmp r2, r3
bcc CopyDataInit
ldr r2, =_sbss
b LoopFillZerobss
/* Zero fill the bss segment. */
FillZerobss:
movs r3, #0
str r3, [r2], #4
LoopFillZerobss:
ldr r3, = _ebss
cmp r2, r3
bcc FillZerobss
/* Call the application's entry point.*/
bl main
/* will never return here */
bx lr
.size Reset_Handler, .-Reset_Handler
/**
* @brief This is the code that swaps stack (from end of heap to irq_stack).
* Also reclaim the heap that was used as a stack.
* @param None
* @retval : None
*/
.section .text.Stack_Change
.weak Stack_Change
.type Stack_Change, %function
Stack_Change:
mov r4, lr
/* Switches stack back momentarily to MSP */
movs r0, #0
msr control, r0
Heap_Reclaim:
/* add heap_post_rtos to the heap (if the capability/function exist) */
/* Also claim the unused memory (between end of heap to end of memory */
/* CAREFULL: the heap section must be the last section in RAM in order this to work */
ldr r0, = _init_stack_size
ldr r1, = _eheap_post_rtos
ldr r2, = _eram
subs r2, r2, r1
adds r0, r0, r2
bl xPortIncreaseHeapSize
bx r4
.size Stack_Change, .-Stack_Change
/**
* @brief This is the code that gets called when the processor receives an
* unexpected interrupt. This simply enters an infinite loop, preserving
* the system state for examination by a debugger.
*
* @param None
* @retval : None
*/
.section .text.Default_Handler,"ax",%progbits
Default_Handler:
Infinite_Loop:
b Infinite_Loop
.size Default_Handler, .-Default_Handler
/******************************************************************************
*
* The minimal vector table for a Cortex M3. Note that the proper constructs
* must be placed on this to ensure that it ends up at physical address
* 0x0000.0000.
*
******************************************************************************/
.section .isr_vector,"a",%progbits
.type g_pfnVectors, %object
.size g_pfnVectors, .-g_pfnVectors
g_pfnVectors:
.word _irq_stack_top
.word Reset_Handler
.word NMI_Handler
.word HardFault_Handler
.word MemManage_Handler
.word BusFault_Handler
.word UsageFault_Handler
.word 0
.word 0
.word 0
.word 0
.word vPortSVCHandler
.word DebugMon_Handler
.word 0
.word xPortPendSVHandler
.word xPortSysTickHandler
.word WWDG_IRQHandler
.word PVD_IRQHandler
.word TAMPER_IRQHandler
.word RTC_IRQHandler
.word FLASH_IRQHandler
.word RCC_IRQHandler
.word EXTI0_IRQHandler
.word EXTI1_IRQHandler
.word EXTI2_IRQHandler
.word EXTI3_IRQHandler
.word EXTI4_IRQHandler
.word DMA1_Channel1_IRQHandler
.word DMA1_Channel2_IRQHandler
.word DMA1_Channel3_IRQHandler
.word DMA1_Channel4_IRQHandler
.word DMA1_Channel5_IRQHandler
.word DMA1_Channel6_IRQHandler
.word DMA1_Channel7_IRQHandler
.word ADC1_2_IRQHandler
.word USB_HP_CAN1_TX_IRQHandler
.word USB_LP_CAN1_RX0_IRQHandler
.word CAN1_RX1_IRQHandler
.word CAN1_SCE_IRQHandler
.word EXTI9_5_IRQHandler
.word TIM1_BRK_IRQHandler
.word TIM1_UP_IRQHandler
.word TIM1_TRG_COM_IRQHandler
.word TIM1_CC_IRQHandler
.word TIM2_IRQHandler
.word TIM3_IRQHandler
.word TIM4_IRQHandler
.word I2C1_EV_IRQHandler
.word I2C1_ER_IRQHandler
.word I2C2_EV_IRQHandler
.word I2C2_ER_IRQHandler
.word SPI1_IRQHandler
.word SPI2_IRQHandler
.word USART1_IRQHandler
.word USART2_IRQHandler
.word USART3_IRQHandler
.word EXTI15_10_IRQHandler
.word RTCAlarm_IRQHandler
.word USBWakeUp_IRQHandler
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word BootRAM /* @0x108. This is for boot in RAM mode for
STM32F10x Medium Density devices. */
/*******************************************************************************
*
* Provide weak aliases for each Exception handler to the Default_Handler.
* As they are weak aliases, any function with the same name will override
* this definition.
*
*******************************************************************************/
.weak NMI_Handler
.thumb_set NMI_Handler,Default_Handler
.weak HardFault_Handler
.thumb_set HardFault_Handler,Default_Handler
.weak MemManage_Handler
.thumb_set MemManage_Handler,Default_Handler
.weak BusFault_Handler
.thumb_set BusFault_Handler,Default_Handler
.weak UsageFault_Handler
.thumb_set UsageFault_Handler,Default_Handler
.weak SVC_Handler
.thumb_set SVC_Handler,Default_Handler
.weak DebugMon_Handler
.thumb_set DebugMon_Handler,Default_Handler
.weak PendSV_Handler
.thumb_set PendSV_Handler,Default_Handler
.weak SysTick_Handler
.thumb_set SysTick_Handler,Default_Handler
.weak WWDG_IRQHandler
.thumb_set WWDG_IRQHandler,Default_Handler
.weak PVD_IRQHandler
.thumb_set PVD_IRQHandler,Default_Handler
.weak TAMPER_IRQHandler
.thumb_set TAMPER_IRQHandler,Default_Handler
.weak RTC_IRQHandler
.thumb_set RTC_IRQHandler,Default_Handler
.weak FLASH_IRQHandler
.thumb_set FLASH_IRQHandler,Default_Handler
.weak RCC_IRQHandler
.thumb_set RCC_IRQHandler,Default_Handler
.weak EXTI0_IRQHandler
.thumb_set EXTI0_IRQHandler,Default_Handler
.weak EXTI1_IRQHandler
.thumb_set EXTI1_IRQHandler,Default_Handler
.weak EXTI2_IRQHandler
.thumb_set EXTI2_IRQHandler,Default_Handler
.weak EXTI3_IRQHandler
.thumb_set EXTI3_IRQHandler,Default_Handler
.weak EXTI4_IRQHandler
.thumb_set EXTI4_IRQHandler,Default_Handler
.weak DMA1_Channel1_IRQHandler
.thumb_set DMA1_Channel1_IRQHandler,Default_Handler
.weak DMA1_Channel2_IRQHandler
.thumb_set DMA1_Channel2_IRQHandler,Default_Handler
.weak DMA1_Channel3_IRQHandler
.thumb_set DMA1_Channel3_IRQHandler,Default_Handler
.weak DMA1_Channel4_IRQHandler
.thumb_set DMA1_Channel4_IRQHandler,Default_Handler
.weak DMA1_Channel5_IRQHandler
.thumb_set DMA1_Channel5_IRQHandler,Default_Handler
.weak DMA1_Channel6_IRQHandler
.thumb_set DMA1_Channel6_IRQHandler,Default_Handler
.weak DMA1_Channel7_IRQHandler
.thumb_set DMA1_Channel7_IRQHandler,Default_Handler
.weak ADC1_2_IRQHandler
.thumb_set ADC1_2_IRQHandler,Default_Handler
.weak USB_HP_CAN1_TX_IRQHandler
.thumb_set USB_HP_CAN1_TX_IRQHandler,Default_Handler
.weak USB_LP_CAN1_RX0_IRQHandler
.thumb_set USB_LP_CAN1_RX0_IRQHandler,Default_Handler
.weak CAN1_RX1_IRQHandler
.thumb_set CAN1_RX1_IRQHandler,Default_Handler
.weak CAN1_SCE_IRQHandler
.thumb_set CAN1_SCE_IRQHandler,Default_Handler
.weak EXTI9_5_IRQHandler
.thumb_set EXTI9_5_IRQHandler,Default_Handler
.weak TIM1_BRK_IRQHandler
.thumb_set TIM1_BRK_IRQHandler,Default_Handler
.weak TIM1_UP_IRQHandler
.thumb_set TIM1_UP_IRQHandler,Default_Handler
.weak TIM1_TRG_COM_IRQHandler
.thumb_set TIM1_TRG_COM_IRQHandler,Default_Handler
.weak TIM1_CC_IRQHandler
.thumb_set TIM1_CC_IRQHandler,Default_Handler
.weak TIM2_IRQHandler
.thumb_set TIM2_IRQHandler,Default_Handler
.weak TIM3_IRQHandler
.thumb_set TIM3_IRQHandler,Default_Handler
.weak TIM4_IRQHandler
.thumb_set TIM4_IRQHandler,Default_Handler
.weak I2C1_EV_IRQHandler
.thumb_set I2C1_EV_IRQHandler,Default_Handler
.weak I2C1_ER_IRQHandler
.thumb_set I2C1_ER_IRQHandler,Default_Handler
.weak I2C2_EV_IRQHandler
.thumb_set I2C2_EV_IRQHandler,Default_Handler
.weak I2C2_ER_IRQHandler
.thumb_set I2C2_ER_IRQHandler,Default_Handler
.weak SPI1_IRQHandler
.thumb_set SPI1_IRQHandler,Default_Handler
.weak SPI2_IRQHandler
.thumb_set SPI2_IRQHandler,Default_Handler
.weak USART1_IRQHandler
.thumb_set USART1_IRQHandler,Default_Handler
.weak USART2_IRQHandler
.thumb_set USART2_IRQHandler,Default_Handler
.weak USART3_IRQHandler
.thumb_set USART3_IRQHandler,Default_Handler
.weak EXTI15_10_IRQHandler
.thumb_set EXTI15_10_IRQHandler,Default_Handler
.weak RTCAlarm_IRQHandler
.thumb_set RTCAlarm_IRQHandler,Default_Handler
.weak USBWakeUp_IRQHandler
.thumb_set USBWakeUp_IRQHandler,Default_Handler

7
flight/PiOS/inc/pios_board_info.h
View File

@ -1,3 +1,8 @@
#ifndef PIOS_BOARD_INFO_H
#define PIOS_BOARD_INFO_H
#include <stdint.h> /* uint* */
#define PIOS_BOARD_INFO_BLOB_MAGIC 0xBDBDBDBD
struct pios_board_info {
@ -15,3 +20,5 @@ struct pios_board_info {
} __attribute__((packed));
extern const struct pios_board_info pios_board_info_blob;
#endif /* PIOS_BOARD_INFO_H */

6
flight/PiOS/inc/pios_crc.h
View File

@ -29,3 +29,9 @@
uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data);
uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length);
uint16_t PIOS_CRC16_updateByte(uint16_t crc, const uint8_t data);
uint16_t PIOS_CRC16_updateCRC(uint16_t crc, const uint8_t* data, int32_t length);
uint32_t PIOS_CRC32_updateByte(uint32_t crc, const uint8_t data);
uint32_t PIOS_CRC32_updateCRC(uint32_t crc, const uint8_t* data, int32_t length);

50
flight/PiOS/inc/pios_eeprom.h Normal file
View File

@ -0,0 +1,50 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_EEPROM EEPROM reading/writing functions
* @brief PIOS EEPROM reading/writing functions
* @{
*
* @file pios_eeprom.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief COM layer functions
* @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_EEPROM_H
#define PIOS_EEPROM_H
/* Public Structures */
struct pios_eeprom_cfg {
uint32_t base_address;
uint32_t max_size;
};
/* Public Functions */
extern void PIOS_EEPROM_Init(const struct pios_eeprom_cfg *cfg);
extern int32_t PIOS_EEPROM_Save(uint8_t *data, uint32_t len);
extern int32_t PIOS_EEPROM_Load(uint8_t *data, uint32_t len);
#endif /* PIOS_EEPROM_H */
/**
* @}
* @}
*/

55
flight/PiOS/inc/pios_rfm22b.h Normal file
View File

@ -0,0 +1,55 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_RFM22B Radio Functions
* @brief PIOS interface for RFM22B Radio
* @{
*
* @file pios_rfm22b.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief RFM22B 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_RFM22B_H
#define PIOS_RFM22B_H
/* Global Types */
struct pios_rfm22b_cfg {
uint32_t frequencyHz;
uint32_t minFrequencyHz;
uint32_t maxFrequencyHz;
uint8_t RFXtalCap;
uint32_t maxRFBandwidth;
uint8_t maxTxPower;
};
/* Public Functions */
extern int32_t PIOS_RFM22B_Init(uint32_t *rfb22b_id, const struct pios_rfm22b_cfg *cfg);
extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id);
extern int8_t PIOS_RFM22B_RSSI(uint32_t rfm22b_id);
extern int16_t PIOS_RFM22B_Resets(uint32_t rfm22b_id);
#endif /* PIOS_RFM22B_H */
/**
* @}
* @}
*/

41
flight/PipXtreme/inc/rfm22b.h → flight/PiOS/inc/pios_rfm22b_priv.h
View File

@ -1,9 +1,14 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_RFM22B Radio Functions
* @brief PIOS interface for RFM22B Radio
* @{
*
* @file rfm22b.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief RF Module hardware layer
* @file pios_rfm22b_priv.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief RFM22B private definitions.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
@ -23,10 +28,14 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef __RFM22B_H__
#define __RFM22B_H__
#ifndef PIOS_RFM22B_PRIV_H
#define PIOS_RFM22B_PRIV_H
#include "stdint.h"
#include <pios.h>
#include "fifo_buffer.h"
#include "pios_rfm22b.h"
extern const struct pios_com_driver pios_rfm22b_com_driver;
// ************************************
@ -386,11 +395,13 @@ enum { RX_SCAN_SPECTRUM = 0,
#define RFM22_header_cntl1_bcen_none 0x00 // No broadcast address enable.
#define RFM22_header_cntl1_bcen_0 0x10 // Broadcast address enable for header byte 0.
#define RFM22_header_cntl1_bcen_1 0x20 // Broadcast address enable for header byte 1.
#define RFM22_header_cntl1_bcen_01 0x30 // Broadcast address enable for header bytes 0 & 1.
#define RFM22_header_cntl1_bcen_2 0x40 // Broadcast address enable for header byte 2.
#define RFM22_header_cntl1_bcen_3 0x80 // Broadcast address enable for header byte 3.
#define RFM22_header_cntl1_hdch_none 0x00 // No Received Header check
#define RFM22_header_cntl1_hdch_0 0x01 // Received Header check for byte 0.
#define RFM22_header_cntl1_hdch_1 0x02 // Received Header check for byte 1.
#define RFM22_header_cntl1_hdch_01 0x03 // Received Header check for bytes 0 & 1.
#define RFM22_header_cntl1_hdch_2 0x04 // Received Header check for byte 2.
#define RFM22_header_cntl1_hdch_3 0x08 // Received Header check for byte 3.
#define RFM22_header_control2 0x33 // R/W
#define RFM22_header_cntl2_prealen 0x01 // MSB of Preamble Length. See register Preamble Length.
@ -598,9 +609,9 @@ uint32_t rfm22_getDatarate(void);
void rfm22_setRxMode(uint8_t mode, bool multi_packet_mode);
int16_t rfm22_getRSSI(void);
int8_t rfm22_getRSSI(void);
int16_t rfm22_receivedRSSI(void);
int8_t rfm22_receivedRSSI(void);
int32_t rfm22_receivedAFCHz(void);
uint16_t rfm22_receivedLength(void);
uint8_t * rfm22_receivedPointer(void);
@ -628,7 +639,6 @@ bool rfm22_channelIsClear(void);
bool rfm22_txReady(void);
void rfm22_1ms_tick(void);
void rfm22_process(void);
void rfm22_TxDataByte_SetCallback(t_rfm22_TxDataByteCallback new_function);
void rfm22_RxData_SetCallback(t_rfm22_RxDataCallback new_function);
@ -636,8 +646,11 @@ void rfm22_RxData_SetCallback(t_rfm22_RxDataCallback new_function);
int rfm22_init_scan_spectrum(uint32_t min_frequency_hz, uint32_t max_frequency_hz);
int rfm22_init_tx_stream(uint32_t min_frequency_hz, uint32_t max_frequency_hz);
int rfm22_init_rx_stream(uint32_t min_frequency_hz, uint32_t max_frequency_hz);
int rfm22_init_normal(uint32_t min_frequency_hz, uint32_t max_frequency_hz, uint32_t freq_hop_step_size);
int rfm22_init_normal(uint32_t id, uint32_t min_frequency_hz, uint32_t max_frequency_hz, uint32_t freq_hop_step_size);
// ************************************
#endif /* PIOS_RFM22B_PRIV_H */
#endif
/**
* @}
* @}
*/

7
flight/PiOS/inc/pios_sys.h
View File

@ -32,12 +32,15 @@
#ifndef PIOS_SYS_H
#define PIOS_SYS_H
#define PIOS_SYS_SERIAL_NUM_BINARY_LEN 12
#define PIOS_SYS_SERIAL_NUM_ASCII_LEN (PIOS_SYS_SERIAL_NUM_BINARY_LEN * 2)
/* Public Functions */
extern void PIOS_SYS_Init(void);
extern int32_t PIOS_SYS_Reset(void);
extern uint32_t PIOS_SYS_getCPUFlashSize(void);
extern int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t *array);
extern int32_t PIOS_SYS_SerialNumberGet(char *str);
extern int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t array[PIOS_SYS_SERIAL_NUM_BINARY_LEN]);
extern int32_t PIOS_SYS_SerialNumberGet(char str[PIOS_SYS_SERIAL_NUM_ASCII_LEN+1]);
#endif /* PIOS_SYS_H */

1
flight/PiOS/inc/pios_usb.h
View File

@ -35,6 +35,7 @@
/* Global functions */
extern int32_t PIOS_USB_Reenumerate();
extern int32_t PIOS_USB_ChangeConnectionState(bool connected);
extern bool PIOS_USB_CableConnected(uint8_t id);
extern bool PIOS_USB_CheckAvailable(uint8_t id);
#endif /* PIOS_USB_H */

2
flight/PiOS/inc/pios_usb_defs.h
View File

@ -300,7 +300,7 @@ struct usb_cdc_union_func_desc {
uint8_t bSlaveInterface;
} __attribute__((packed));
#define USB_LANGID_ENGLISH_UK 0x0809
#define USB_LANGID_ENGLISH_US 0x0409
struct usb_string_langid {
uint8_t bLength;

28
flight/PipXtreme/inc/api_config.h → flight/PiOS/inc/pios_usb_util.h
View File

@ -1,9 +1,14 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB_UTIL USB utility functions
* @brief USB utility functions
* @{
*
* @file api_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief RF Module hardware layer
* @file pios_usb_util.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief USB utility functions
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
@ -23,18 +28,11 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef __API_CONFIG_H__
#define __API_CONFIG_H__
#ifndef PIOS_USB_UTIL_H
#define PIOS_USB_UTIL_H
#include "stdint.h"
#include <stdint.h> /* uint8_t */
// *****************************************************************************
uint8_t * PIOS_USB_UTIL_AsciiToUtf8(uint8_t * dst, uint8_t * src, uint16_t srclen);
void apiconfig_1ms_tick(void);
void apiconfig_process(void);
void apiconfig_init(void);
// *****************************************************************************
#endif
#endif /* PIOS_USB_UTIL_H */

Some files were not shown because too many files have changed in this diff Show More