From d6fe4ef41d908f6c9df201f55508aa6a1e60fb1f Mon Sep 17 00:00:00 2001 From: Marcus Eliasson Date: Mon, 16 Mar 2015 17:28:14 +0100 Subject: [PATCH 01/58] Added initial code for buzzer --- Makefile | 4 +- drivers/interface/piezo.h | 109 ++++++++++++ drivers/src/piezo.c | 330 +++++++++++++++++++++++++++++++++++++ modules/interface/buzzer.h | 9 + modules/src/buzzer.c | 305 ++++++++++++++++++++++++++++++++++ modules/src/system.c | 13 +- 6 files changed, 764 insertions(+), 6 deletions(-) create mode 100644 drivers/interface/piezo.h create mode 100644 drivers/src/piezo.c create mode 100644 modules/interface/buzzer.h create mode 100644 modules/src/buzzer.c diff --git a/Makefile b/Makefile index f4f2755390..6a8e05ce73 100644 --- a/Makefile +++ b/Makefile @@ -122,7 +122,7 @@ PROJ_OBJ_CF2 = platform_cf2.o # Drivers PROJ_OBJ += exti.o nvic.o motors.o PROJ_OBJ_CF1 += led_f103.o i2cdev_f103.o i2croutines.o adc_f103.o mpu6050.o hmc5883l.o ms5611.o nrf24l01.o eeprom.o -PROJ_OBJ_CF2 += led_f405.o mpu6500.o i2cdev_f405.o ws2812.o lps25h.o ak8963.o eeprom.o maxsonar.o +PROJ_OBJ_CF2 += led_f405.o mpu6500.o i2cdev_f405.o ws2812.o lps25h.o ak8963.o eeprom.o maxsonar.o piezo.o PROJ_OBJ_CF2 += uart_syslink.o swd.o uart1.o uart2.o # USB Files PROJ_OBJ_CF2 += usb_bsp.o usblink.o usbd_desc.o usb.o @@ -135,7 +135,7 @@ PROJ_OBJ_CF2 += imu_cf2.o pm_f405.o syslink.o radiolink.o ow_syslink.o proximity # Modules PROJ_OBJ += system.o comm.o console.o pid.o crtpservice.o param.o mem.o PROJ_OBJ += commander.o controller.o sensfusion6.o stabilizer.o -PROJ_OBJ += log.o worker.o trigger.o sitaw.o +PROJ_OBJ += log.o worker.o trigger.o sitaw.o buzzer.o PROJ_OBJ_CF2 += neopixelring.o expbrd.o platformservice.o bigquad.o # Expansion boards diff --git a/drivers/interface/piezo.h b/drivers/interface/piezo.h new file mode 100644 index 0000000000..ba9959a652 --- /dev/null +++ b/drivers/interface/piezo.h @@ -0,0 +1,109 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2011-2012 Bitcraze AB + * + * 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, in version 3. + * + * 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 . + * + * Motors.h - Motor driver header file + * + * The motors PWM ratio is a value on 16 bits (from 0 to 0xFFFF) + * the functions of the driver will make the conversions to the actual PWM + * precision (ie. if the precision is 8bits 0xFFFF and 0xFF00 are equivalents). + * + * The precision is set in number of bits by the define MOTORS_PWM_BITS + * The timer prescaler is set by MOTORS_PWM_PRESCALE + */ +#ifndef __PIEZO_H__ +#define __PIEZO_H__ + +#include +#include +#include "config.h" + +/******** Defines ********/ + +// The following defines gives a PWM of 9 bits at ~140KHz for a sysclock of 72MHz +#ifdef F10X + #define MOTORS_PWM_BITS 9 + #define MOTORS_PWM_PERIOD ((1<. + * + * motors.c - Motor driver + * + * This code mainly interfacing the PWM peripheral lib of ST. + */ + +#include + +/* ST includes */ +#include "stm32fxxx.h" + +#include "piezo.h" + +//FreeRTOS includes +#include "FreeRTOS.h" +#include "task.h" + +// HW defines +#define PIEZO_TIM_PERIF RCC_APB1Periph_TIM5 +#define PIEZO_TIM TIM5 +#define PIEZO_TIM_DBG DBGMCU_TIM2_STOP +#define PIEZO_TIM_SETCOMPARE TIM_SetCompare2 +#define PIEZO_TIM_GETCAPTURE TIM_GetCapture2 + +#define PIEZO_GPIO_POS_PERIF RCC_AHB1Periph_GPIOA +#define PIEZO_GPIO_POS_PORT GPIOA +#define PIEZO_GPIO_POS_PIN GPIO_Pin_2 // TIM5_CH3 +#define PIEZO_GPIO_AF_POS_PIN GPIO_PinSource2 +#define PIEZO_GPIO_AF_POS GPIO_AF_TIM5 + +#define PIEZO_GPIO_NEG_PERIF RCC_AHB1Periph_GPIOA +#define PIEZO_GPIO_NEG_PORT GPIOA +#define PIEZO_GPIO_NEG_PIN GPIO_Pin_3 // TIM5_CH4 +#define PIEZO_GPIO_AF_NEG_PIN GPIO_PinSource3 +#define PIEZO_GPIO_AF_NEG GPIO_AF_TIM5 + +#define PIEZO_PWM_BITS (8) +#define PIEZO_PWM_PERIOD ((1<>(16-MOTORS_PWM_BITS)&((1<CR1 = PIEZO_TIM->CR1 | TIM_CR1_CEN; + portENABLE_INTERRUPTS(); + + //Enable the timer PWM outputs + TIM_CtrlPWMOutputs(PIEZO_TIM, ENABLE); + /*TIM_CtrlPWMOutputs(MOTORS_TIM_M2, ENABLE); + TIM_CtrlPWMOutputs(MOTORS_TIM_M3, ENABLE); + TIM_CtrlPWMOutputs(MOTORS_TIM_M4, ENABLE);*/ + // Halt timer during debug halt. + //DBGMCU_APB2PeriphConfig(PIEZO_TIM_DBG, ENABLE); + TIM_SetCompare4(PIEZO_TIM, 0x00); + TIM_SetCompare3(PIEZO_TIM, 0x00); + + //TIM_SetCompare4(PIEZO_TIM, 0x40); + //TIM_SetCompare3(PIEZO_TIM, 0x40); + + +/* DBGMCU_APB2PeriphConfig(MOTORS_TIM_M2_DBG, ENABLE); + DBGMCU_APB2PeriphConfig(MOTORS_TIM_M3_DBG, ENABLE); + DBGMCU_APB2PeriphConfig(MOTORS_TIM_M4_DBG, ENABLE);*/ + + isInit = true; +} + +bool piezoTest(void) +{ + return isInit; +} + +void piezoSetRatio(uint8_t ratio) +{ + TIM_SetCompare4(PIEZO_TIM, ratio); + TIM_SetCompare3(PIEZO_TIM, ratio); +} + +void piezoSetFreq(uint16_t freq) +{ + TIM_PrescalerConfig(PIEZO_TIM, (PIEZO_BASE_FREQ/freq), TIM_PSCReloadMode_Update); +} + +/*void motorsSetRatio(int id, uint16_t ratio) +{ + switch(id) + { + case MOTOR_M1: + M1_TIM_SETCOMPARE(MOTORS_TIM_M1, C_16_TO_BITS(ratio)); + break; + case MOTOR_M2: + M2_TIM_SETCOMPARE(MOTORS_TIM_M2, C_16_TO_BITS(ratio)); + break; + case MOTOR_M3: + M3_TIM_SETCOMPARE(MOTORS_TIM_M3, C_16_TO_BITS(ratio)); + break; + case MOTOR_M4: + M4_TIM_SETCOMPARE(MOTORS_TIM_M4, C_16_TO_BITS(ratio)); + break; + } +} + +int motorsGetRatio(int id) +{ + switch(id) + { + case MOTOR_M1: + return C_BITS_TO_16(M1_TIM_GETCAPTURE(MOTORS_TIM_M1)); + case MOTOR_M2: + return C_BITS_TO_16(M2_TIM_GETCAPTURE(MOTORS_TIM_M2)); + case MOTOR_M3: + return C_BITS_TO_16(M3_TIM_GETCAPTURE(MOTORS_TIM_M3)); + case MOTOR_M4: + return C_BITS_TO_16(M4_TIM_GETCAPTURE(MOTORS_TIM_M4)); + } + + return -1; +} +*/ +/*#ifdef MOTOR_RAMPUP_TEST +// FreeRTOS Task to test the Motors driver with a rampup of each motor alone. +void motorsTestTask(void* params) +{ + int step=0; + float rampup = 0.01; + + motorsSetupMinMaxPos(); + motorsSetRatio(MOTOR_M4, 1*(1<<16) * 0.0); + motorsSetRatio(MOTOR_M3, 1*(1<<16) * 0.0); + motorsSetRatio(MOTOR_M2, 1*(1<<16) * 0.0); + motorsSetRatio(MOTOR_M1, 1*(1<<16) * 0.0); + vTaskDelay(M2T(1000)); + + while(1) + { + vTaskDelay(M2T(100)); + + motorsSetRatio(MOTOR_M4, 1*(1<<16) * rampup); + motorsSetRatio(MOTOR_M3, 1*(1<<16) * rampup); + motorsSetRatio(MOTOR_M2, 1*(1<<16) * rampup); + motorsSetRatio(MOTOR_M1, 1*(1<<16) * rampup); + + rampup += 0.001; + if (rampup >= 0.1) + { + if(++step>3) step=0; + rampup = 0.01; + } + } +} +#else +// FreeRTOS Task to test the Motors driver +void motorsTestTask(void* params) +{ + static const int sequence[] = {0.1*(1<<16), 0.15*(1<<16), 0.2*(1<<16), 0.25*(1<<16)}; + int step=0; + + //Wait 3 seconds before starting the motors + vTaskDelay(M2T(3000)); + + while(1) + { + motorsSetRatio(MOTOR_M4, sequence[step%4]); + motorsSetRatio(MOTOR_M3, sequence[(step+1)%4]); + motorsSetRatio(MOTOR_M2, sequence[(step+2)%4]); + motorsSetRatio(MOTOR_M1, sequence[(step+3)%4]); + + if(++step>3) step=0; + + vTaskDelay(M2T(1000)); + } +} +#endif*/ diff --git a/modules/interface/buzzer.h b/modules/interface/buzzer.h new file mode 100644 index 0000000000..e9d2d936e6 --- /dev/null +++ b/modules/interface/buzzer.h @@ -0,0 +1,9 @@ +#ifndef __BUZZER_H__ +#define __BUZZER_H__ + +#include +#include + +void buzzerInit(void); + +#endif //__BUZZER_H__ diff --git a/modules/src/buzzer.c b/modules/src/buzzer.c new file mode 100644 index 0000000000..fd85359ca6 --- /dev/null +++ b/modules/src/buzzer.c @@ -0,0 +1,305 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2012 BitCraze AB + * + * 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, in version 3. + * + * 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 . + * + * neopixelring.c: NeoPixel Ring 12 Leds effects/driver + */ + +#include "neopixelring.h" + +#include +#include +#include "stm32fxxx.h" + +#include "FreeRTOS.h" +#include "timers.h" + +#include "param.h" +#include "pm.h" +#include "log.h" +#include "piezo.h" + +typedef void (*BuzzerEffect)(uint32_t timer); + +/** + * Credit to http://tny.cz/e525c1b2 for supplying the tones + */ +#define C0 16 +#define Db0 17 +#define D0 18 +#define Eb0 19 +#define E0 20 +#define F0 21 +#define Gb0 23 +#define G0 24 +#define Ab0 25 +#define A0 27 +#define Bb0 29 +#define B0 30 +#define C1 32 +#define Db1 34 +#define D1 36 +#define Eb1 38 +#define E1 41 +#define F1 43 +#define Gb1 46 +#define G1 49 +#define Ab1 51 +#define A1 55 +#define Bb1 58 +#define B1 61 +#define C2 65 +#define Db2 69 +#define D2 73 +#define Eb2 77 +#define E2 82 +#define F2 87 +#define Gb2 92 +#define G2 98 +#define Ab2 103 +#define A2 110 +#define Bb2 116 +#define B2 123 +#define C3 130 +#define Db3 138 +#define D3 146 +#define Eb3 155 +#define E3 164 +#define F3 174 +#define Gb3 185 +#define G3 196 +#define Ab3 207 +#define A3 220 +#define Bb3 233 +#define B3 246 +#define C4 261 +#define Db4 277 +#define D4 293 +#define Eb4 311 +#define E4 329 +#define F4 349 +#define Gb4 369 +#define G4 392 +#define Ab4 415 +#define A4 440 +#define Bb4 466 +#define B4 493 +#define C5 523 +#define Db5 554 +#define D5 587 +#define Eb5 622 +#define E5 659 +#define F5 698 +#define Gb5 739 +#define G5 783 +#define Ab5 830 +#define A5 880 +#define Bb5 932 +#define B5 987 +#define C6 1046 +#define Db6 1108 +#define D6 1174 +#define Eb6 1244 +#define E6 1318 +#define F6 1396 +#define Gb6 1479 +#define G6 1567 +#define Ab6 1661 +#define A6 1760 +#define Bb6 1864 +#define B6 1975 +#define C7 2093 +#define Db7 2217 +#define D7 2349 +#define Eb7 2489 +#define E7 2637 +#define F7 2793 +#define Gb7 2959 +#define G7 3135 +#define Ab7 3322 +#define A7 3520 +#define Bb7 3729 +#define B7 3951 +#define C8 4186 +#define Db8 4434 +#define D8 4698 +#define Eb8 4978 +/* Duration of notes */ +#define W (60) +#define H (W * 2) +#define Q (W * 4) +#define E (W * 8) +#define S (W * 16) +#define ES (W*6) + + +typedef struct { + uint16_t tone; + uint16_t duration; +} Note; + +typedef struct { + uint32_t bpm; + uint32_t ni; + uint32_t delay; + Note notes[80]; +} Melody; + +Melody melodies[] = { + {.bpm = 120, .ni = 0, .delay = 1, .notes = {{C4, H}, {D4, H}, {E4, H}, {F4, H}, {G4, H}, {A4, H}, {B4, H}, {0xFF, 0}}}, + {.bpm = 120, .ni = 0, .delay = 1, .notes = {{C4, S}, {D4, S}, {E4, S}, {F4, S}, {G4, S}, {A4, S}, {B4, S}, {0xFF, 0}}}, + /* Imperial march from http://tny.cz/e525c1b2A */ + {.bpm = 120, .ni = 0, .delay = 1, .notes = {{A3, Q}, {A3, Q}, {A3, Q},{F3, ES}, {C4, S}, + {A3, Q}, {F3, ES}, {C4, S}, {A3, H}, + {E4, Q}, {E4, Q}, {E4, Q}, {F4, ES}, {C4, S}, + {Ab3, Q}, {F3, ES}, {C4, S}, {A3, H}, + {A4, Q}, {A3, ES}, {A3, S}, {A4, Q}, {Ab4, ES}, {G4, S}, + {Gb4, S}, {E4, S}, {F4, E}, {0, E}, {Bb3, E}, {Eb4, Q}, {D4, ES}, {Db4, S}, + {C4, S}, {B3, S}, {C4, E}, {0, E}, {F3, E}, {Ab3, Q}, {F3, ES}, {A3, S}, + {C4, Q}, {A3, ES}, {C4, S}, {E4, H}, + {A4, Q}, {A3, ES}, {A3, S}, {A4, Q}, {Ab4, ES}, {G4, S}, + {Gb4, S}, {E4, S}, {F4, E}, {0, E}, {Bb3, E}, {Eb4, Q}, {D4, ES}, {Db4, S}, + {Gb4, S}, {E4, S}, {F4, E}, {0, E}, {Bb3, E}, {Eb4, Q}, {D4, ES}, {Db4, S}, + {C4,S}, {B3, S}, {C4, E}, {0, E}, {F3, E}, {Ab3, Q}, {F3, ES}, {C4, S}, + {A3, Q}, {F3, ES}, {C4, S}, {A3, H}, {0, H}, + {0xFF, 0}}} + +}; + +uint8_t melody = 2; +uint8_t nmelody = 3; +unsigned int mcounter = 0; +static bool playing_sound = false; +static void melodyplayer(uint32_t counter) { + // Sync one melody for the loop + Melody* m = &melodies[melody]; + if (mcounter == 0) { + if (m->notes[m->ni].tone == 0xFF) + { + // Loop the melody + m->ni = 0; + } + // Play current note + piezoSetFreq(m->notes[m->ni].tone); + piezoSetRatio(127); + mcounter = (m->bpm * 100) / m->notes[m->ni].duration; + playing_sound = true; + m->ni++; + } + else if (mcounter == 1) + { + piezoSetRatio(0); + } + mcounter--; +} + +uint8_t static_ratio = 0; +uint16_t static_freq = 4000; +static void bypass(uint32_t counter) +{ + /* Just set the params from the host */ + piezoSetRatio(static_ratio); + piezoSetFreq(static_freq); +} + +uint16_t siren_start = 2000; +uint16_t siren_freq = 2000; +uint16_t siren_stop = 4000; +int16_t siren_step = 40; +static void siren(uint32_t counter) +{ + siren_freq += siren_step; + if (siren_freq > siren_stop) + { + siren_step *= -1; + siren_freq = siren_stop; + } + if (siren_freq < siren_start) + { + siren_step *= -1; + siren_freq = siren_start; + } + piezoSetRatio(127); + piezoSetFreq(siren_freq); +} + + +static int pitchid; +static int rollid; +static int pitch; +static int roll; +static int tilt_freq; +static int tilt_ratio; +static void tilt(uint32_t counter) +{ + pitchid = logGetVarId("stabilizer", "pitch"); + rollid = logGetVarId("stabilizer", "roll"); + + pitch = logGetInt(pitchid); + roll = logGetInt(rollid); + tilt_freq = 0; + tilt_ratio = 127; + + if (abs(pitch) > 5) { + tilt_freq = 3000 - 50 * pitch; + } + + piezoSetRatio(tilt_ratio); + piezoSetFreq(tilt_freq); +} + + +unsigned int neffect = 0; +unsigned int effect = 3; +BuzzerEffect effects[] = {bypass, siren, melodyplayer, tilt}; + +static xTimerHandle timer; +static uint32_t counter = 0; + +static void buzzTimer(xTimerHandle timer) +{ + counter++; + + if (effects[effect] != 0) + effects[effect](counter*10); +} + +void buzzerInit(void) +{ + piezoInit(); + + neffect = sizeof(effects)/sizeof(effects[0])-1; + nmelody = sizeof(melodies)/sizeof(melodies[0])-1; + + timer = xTimerCreate( (const signed char *)"buzztimer", M2T(10), + pdTRUE, NULL, buzzTimer ); + xTimerStart(timer, 100); +} + +PARAM_GROUP_START(buzzer) +PARAM_ADD(PARAM_UINT8, effect, &effect) +PARAM_ADD(PARAM_UINT32 | PARAM_RONLY, neffect, &neffect) +PARAM_ADD(PARAM_UINT8, melody, &melody) +PARAM_ADD(PARAM_UINT32 | PARAM_RONLY, nmelody, &melody) +PARAM_ADD(PARAM_UINT8, freq, &static_freq) +PARAM_ADD(PARAM_UINT8, ratio, &static_ratio) +PARAM_GROUP_STOP(buzzer) diff --git a/modules/src/system.c b/modules/src/system.c index e3eac18a06..4b0ca9cc7b 100644 --- a/modules/src/system.c +++ b/modules/src/system.c @@ -57,6 +57,7 @@ #include "expbrd.h" #include "mem.h" #include "proximity.h" +#include "buzzer.h" /* Private variable */ static bool selftestPassed; @@ -104,10 +105,6 @@ void systemInit(void) adcInit(); ledseqInit(); pmInit(); - -#ifdef PROXIMITY_ENABLED - proximityInit(); -#endif isInit = true; } @@ -162,6 +159,14 @@ void systemTask(void *arg) expbrdInit(); #endif memInit(); + +#ifdef PROXIMITY_ENABLED + proximityInit(); +#endif + +#ifdef BUZZER_ENABLED + buzzerInit(); +#endif //Test the modules pass &= systemTest(); From c7d4bb801783cd07c4e6ee530400e4dab778daed Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Wed, 6 May 2015 14:40:44 +0200 Subject: [PATCH 02/58] Initial imp. if virtual led mem. --- modules/interface/neopixelring.h | 4 ++ modules/src/mem.c | 17 ++++++-- modules/src/neopixelring.c | 68 +++++++++++++++++++------------- 3 files changed, 58 insertions(+), 31 deletions(-) diff --git a/modules/interface/neopixelring.h b/modules/interface/neopixelring.h index d83a5608f8..fbfd2282d4 100644 --- a/modules/interface/neopixelring.h +++ b/modules/interface/neopixelring.h @@ -4,6 +4,10 @@ #include #include +#define NBR_LEDS 12 + +extern uint8_t ledringmem[NBR_LEDS][3]; + typedef void (*NeopixelRingEffect)(uint8_t buffer[][3], bool reset); void neopixelringInit(void); diff --git a/modules/src/mem.c b/modules/src/mem.c index 4ca04cd837..d235c9ff30 100644 --- a/modules/src/mem.c +++ b/modules/src/mem.c @@ -40,6 +40,7 @@ #include "mem.h" #include "ow.h" #include "eeprom.h" +#include "neopixelring.h" #include "console.h" #include "cfassert.h" @@ -68,10 +69,13 @@ #else #define NBR_EEPROM 1 #endif -#define EEPROM_ID 0 -#define MEM_TYPE_EEPROM 0 -#define MEM_TYPE_OW 1 +#define EEPROM_ID 0x00 +#define LEDMEM_ID 0x10 + +#define MEM_TYPE_EEPROM 0x00 +#define MEM_TYPE_OW 0x01 +#define MEM_TYPE_LED12 0x10 //Private functions static void memTask(void * prm); @@ -262,6 +266,13 @@ void memWriteProcess() else status = EIO; } + else if(memId == LEDMEM_ID) + { + if ((memAddr + writeLen) < sizeof(ledringmem)) + { + memcpy(ledringmem + memAddr, &p.data[5], writeLen); + } + } else { memId = memId - NBR_EEPROM; diff --git a/modules/src/neopixelring.c b/modules/src/neopixelring.c index 183fbe756f..f6f635670b 100644 --- a/modules/src/neopixelring.c +++ b/modules/src/neopixelring.c @@ -28,6 +28,7 @@ #include #include +#include #include "stm32fxxx.h" @@ -59,7 +60,6 @@ /**************** Some useful macros ***************/ -#define NBR_LEDS 12 #define RED {0x10, 0x00, 0x00} #define GREEN {0x00, 0x10, 0x00} #define BLUE {0x00, 0x00, 0x10} @@ -76,7 +76,7 @@ #define LINSCALE(domain_low, domain_high, codomain_low, codomain_high, value) ((codomain_high - codomain_low) / (domain_high - domain_low)) * (value - domain_low) + codomain_low #define SET_WHITE(dest, intensity) dest[0] = intensity; dest[1] = intensity; dest[2] = intensity; -static uint32_t effect = 9; +static uint32_t effect = 13; static uint32_t neffect; static uint8_t headlightEnable = 0; static uint8_t black[][3] = {BLACK, BLACK, BLACK, @@ -85,6 +85,14 @@ static uint8_t black[][3] = {BLACK, BLACK, BLACK, BLACK, BLACK, BLACK, }; +static const uint8_t green[] = {0x00, 0xFF, 0x00}; +static const uint8_t red[] = {0xFF, 0x00, 0x00}; +static const uint8_t blue[] = {0x00, 0x00, 0xFF}; +static const uint8_t white[] = WHITE; +static const uint8_t part_black[] = BLACK; + +uint8_t ledringmem[NBR_LEDS][3]; + /**************** Black (LEDs OFF) ***************/ static void blackEffect(uint8_t buffer[][3], bool reset) @@ -165,11 +173,19 @@ static void solidColorEffect(uint8_t buffer[][3], bool reset) } } -static const uint8_t green[] = {0x00, 0xFF, 0x00}; -static const uint8_t red[] = {0xFF, 0x00, 0x00}; -static const uint8_t blue[] = {0x00, 0x00, 0xFF}; -static const uint8_t white[] = WHITE; -static const uint8_t part_black[] = BLACK; +static void virtualMemEffect(uint8_t buffer[][3], bool reset) +{ + int i; + + if (reset) + { + for (i=0; i Date: Thu, 7 May 2015 22:36:38 +0200 Subject: [PATCH 03/58] Fixed param size of buzzer freq --- modules/src/buzzer.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/src/buzzer.c b/modules/src/buzzer.c index fd85359ca6..0136b125ee 100644 --- a/modules/src/buzzer.c +++ b/modules/src/buzzer.c @@ -300,6 +300,6 @@ PARAM_ADD(PARAM_UINT8, effect, &effect) PARAM_ADD(PARAM_UINT32 | PARAM_RONLY, neffect, &neffect) PARAM_ADD(PARAM_UINT8, melody, &melody) PARAM_ADD(PARAM_UINT32 | PARAM_RONLY, nmelody, &melody) -PARAM_ADD(PARAM_UINT8, freq, &static_freq) +PARAM_ADD(PARAM_UINT16, freq, &static_freq) PARAM_ADD(PARAM_UINT8, ratio, &static_ratio) PARAM_GROUP_STOP(buzzer) From 70cb52ffda4233e0baf77e07468ba15cccb3e707 Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Thu, 7 May 2015 22:39:09 +0200 Subject: [PATCH 04/58] Virtual LED ring mem with RGB565 working. --- modules/interface/neopixelring.h | 2 +- modules/src/mem.c | 48 ++++++++++++++++++++++++++------ modules/src/neopixelring.c | 19 ++++++++++--- 3 files changed, 55 insertions(+), 14 deletions(-) diff --git a/modules/interface/neopixelring.h b/modules/interface/neopixelring.h index fbfd2282d4..e75ab65c9a 100644 --- a/modules/interface/neopixelring.h +++ b/modules/interface/neopixelring.h @@ -6,7 +6,7 @@ #define NBR_LEDS 12 -extern uint8_t ledringmem[NBR_LEDS][3]; +extern uint8_t ledringmem[NBR_LEDS * 2]; typedef void (*NeopixelRingEffect)(uint8_t buffer[][3], bool reset); diff --git a/modules/src/mem.c b/modules/src/mem.c index d235c9ff30..294955732c 100644 --- a/modules/src/mem.c +++ b/modules/src/mem.c @@ -44,6 +44,7 @@ #include "console.h" #include "cfassert.h" +#include "debug.h" #if 0 #define MEM_DEBUG(fmt, ...) DEBUG_PRINT("D/log " fmt, ## __VA_ARGS__) @@ -71,12 +72,16 @@ #endif #define EEPROM_ID 0x00 -#define LEDMEM_ID 0x10 +#define NBR_LEDMEM 1 +#define LEDMEM_ID 0x01 + +#define NBR_STATIC_MEM (NBR_EEPROM + NBR_LEDMEM) #define MEM_TYPE_EEPROM 0x00 #define MEM_TYPE_OW 0x01 #define MEM_TYPE_LED12 0x10 + //Private functions static void memTask(void * prm); static void memSettingsProcess(int command); @@ -150,7 +155,7 @@ void memSettingsProcess(int command) p.header = CRTP_HEADER(CRTP_PORT_MEM, SETTINGS_CH); p.size = 2; p.data[0] = CMD_GET_NBR; - p.data[1] = nbrOwMems + NBR_EEPROM; + p.data[1] = nbrOwMems + NBR_STATIC_MEM; crtpSendPacket(&p); break; @@ -173,9 +178,21 @@ void memSettingsProcess(int command) memcpy(&p.data[7], eepromSerialNum.data, 8); p.size += 8; } + else if (memId == LEDMEM_ID) + { + // Memory type (eeprom) + p.data[2] = MEM_TYPE_LED12; + p.size += 1; + // Size of the memory + memSize = sizeof(ledringmem); + memcpy(&p.data[3], &memSize, 4); + p.size += 4; + memcpy(&p.data[7], eepromSerialNum.data, 8); //TODO + p.size += 8; + } else { - if (owGetinfo(memId - NBR_EEPROM, &serialNbr)) + if (owGetinfo(memId - NBR_STATIC_MEM, &serialNbr)) { // Memory type (1-wire) p.data[2] = MEM_TYPE_OW; @@ -199,7 +216,7 @@ void memReadProcess() uint8_t memId = p.data[0]; uint8_t readLen = p.data[5]; uint32_t memAddr; - uint8_t status; + uint8_t status = 0; memcpy(&memAddr, &p.data[1], 4); @@ -215,9 +232,17 @@ void memReadProcess() else status = EIO; } + else if (memId == LEDMEM_ID) + { + if (memAddr + readLen <= sizeof(ledringmem) && + memcpy(&p.data[6], &(ledringmem[memAddr]), readLen)) + status = 0; + else + status = EIO; + } else { - memId = memId - NBR_EEPROM; + memId = memId - NBR_STATIC_MEM; if (memAddr + readLen <= OW_MAX_SIZE && owRead(memId, memAddr, readLen, &p.data[6])) status = 0; @@ -250,7 +275,7 @@ void memWriteProcess() uint8_t memId = p.data[0]; uint8_t writeLen; uint32_t memAddr; - uint8_t status; + uint8_t status = 0; memcpy(&memAddr, &p.data[1], 4); writeLen = p.size - 5; @@ -268,14 +293,19 @@ void memWriteProcess() } else if(memId == LEDMEM_ID) { - if ((memAddr + writeLen) < sizeof(ledringmem)) + if ((memAddr + writeLen) <= sizeof(ledringmem)) + { + memcpy(&(ledringmem[memAddr]), &p.data[5], writeLen); + DEBUG("LED write addr:%i, led:%i\n", memAddr, writeLen); + } + else { - memcpy(ledringmem + memAddr, &p.data[5], writeLen); + DEBUG("\LED write failed! addr:%i, led:%i\n", memAddr, writeLen); } } else { - memId = memId - NBR_EEPROM; + memId = memId - NBR_STATIC_MEM; if (memAddr + writeLen <= OW_MAX_SIZE && owWrite(memId, memAddr, writeLen, &p.data[5])) status = 0; diff --git a/modules/src/neopixelring.c b/modules/src/neopixelring.c index f6f635670b..4df7ab7aae 100644 --- a/modules/src/neopixelring.c +++ b/modules/src/neopixelring.c @@ -91,7 +91,7 @@ static const uint8_t blue[] = {0x00, 0x00, 0xFF}; static const uint8_t white[] = WHITE; static const uint8_t part_black[] = BLACK; -uint8_t ledringmem[NBR_LEDS][3]; +uint8_t ledringmem[NBR_LEDS * 2]; /**************** Black (LEDs OFF) ***************/ @@ -180,11 +180,22 @@ static void virtualMemEffect(uint8_t buffer[][3], bool reset) if (reset) { for (i=0; i> 3; + G6 = ((led[i][0] & 0x07) << 3) | (led[i][1] >> 5); + B5 = led[i][1] & 0x1F; + buffer[i][0] = ((uint16_t)R5 * 527 + 23 ) >> 6; + buffer[i][1] = ((uint16_t)G6 * 259 + 33 ) >> 6; + buffer[i][2] = ((uint16_t)B5 * 527 + 23 ) >> 6; + } } static void boatEffect(uint8_t buffer[][3], bool reset) @@ -616,7 +627,7 @@ void neopixelringInit(void) GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; GPIO_Init(GPIOB, &GPIO_InitStructure); - timer = xTimerCreate( (const signed char *)"ringTimer", M2T(55), + timer = xTimerCreate( (const signed char *)"ringTimer", M2T(50), pdTRUE, NULL, neopixelringTimer ); xTimerStart(timer, 100); } From 961d0b51f97121270017424db336b71d1fe82387 Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Thu, 13 Aug 2015 21:23:09 +0200 Subject: [PATCH 05/58] Buzzer driver added. Virtual ledring mem added. Renamed neopixelring to ledring12. Added ws2812 driver for CF1. --- Makefile | 8 +- drivers/interface/piezo.h | 55 +----- drivers/src/piezo.c | 199 +------------------- drivers/src/{ws2812.c => ws2812_cf2.c} | 2 +- modules/interface/ledring12.h | 15 ++ modules/interface/neopixelring.h | 15 -- modules/src/buzzer.c | 4 +- modules/src/expbrd.c | 4 +- modules/src/{neopixelring.c => ledring12.c} | 18 +- modules/src/mem.c | 18 +- modules/src/system.c | 1 - 11 files changed, 60 insertions(+), 279 deletions(-) rename drivers/src/{ws2812.c => ws2812_cf2.c} (99%) create mode 100644 modules/interface/ledring12.h delete mode 100644 modules/interface/neopixelring.h rename modules/src/{neopixelring.c => ledring12.c} (97%) diff --git a/Makefile b/Makefile index 6a8e05ce73..2988577f4a 100644 --- a/Makefile +++ b/Makefile @@ -121,8 +121,10 @@ PROJ_OBJ_CF2 = platform_cf2.o # Drivers PROJ_OBJ += exti.o nvic.o motors.o -PROJ_OBJ_CF1 += led_f103.o i2cdev_f103.o i2croutines.o adc_f103.o mpu6050.o hmc5883l.o ms5611.o nrf24l01.o eeprom.o -PROJ_OBJ_CF2 += led_f405.o mpu6500.o i2cdev_f405.o ws2812.o lps25h.o ak8963.o eeprom.o maxsonar.o piezo.o +PROJ_OBJ_CF1 += led_f103.o i2cdev_f103.o i2croutines.o adc_f103.o mpu6050.o +PROJ_OBJ_CF1 += hmc5883l.o ms5611.o nrf24l01.o eeprom.o ws2812_cf1.o +PROJ_OBJ_CF2 += led_f405.o mpu6500.o i2cdev_f405.o ws2812_cf2.o lps25h.o +PROJ_OBJ_CF2 += ak8963.o eeprom.o maxsonar.o piezo.o PROJ_OBJ_CF2 += uart_syslink.o swd.o uart1.o uart2.o # USB Files PROJ_OBJ_CF2 += usb_bsp.o usblink.o usbd_desc.o usb.o @@ -136,7 +138,7 @@ PROJ_OBJ_CF2 += imu_cf2.o pm_f405.o syslink.o radiolink.o ow_syslink.o proximity PROJ_OBJ += system.o comm.o console.o pid.o crtpservice.o param.o mem.o PROJ_OBJ += commander.o controller.o sensfusion6.o stabilizer.o PROJ_OBJ += log.o worker.o trigger.o sitaw.o buzzer.o -PROJ_OBJ_CF2 += neopixelring.o expbrd.o platformservice.o bigquad.o +PROJ_OBJ_CF2 += ledring12.o expbrd.o platformservice.o bigquad.o # Expansion boards PROJ_OBJ_CF2 += exptest.o diff --git a/drivers/interface/piezo.h b/drivers/interface/piezo.h index ba9959a652..fa89cfd4c9 100644 --- a/drivers/interface/piezo.h +++ b/drivers/interface/piezo.h @@ -39,46 +39,11 @@ /******** Defines ********/ -// The following defines gives a PWM of 9 bits at ~140KHz for a sysclock of 72MHz -#ifdef F10X - #define MOTORS_PWM_BITS 9 - #define MOTORS_PWM_PERIOD ((1<. * - * motors.c - Motor driver + * piezo.c - Piezo/Buzzer driver * * This code mainly interfacing the PWM peripheral lib of ST. */ @@ -63,76 +63,12 @@ /* This should be calculated.. */ #define PIEZO_BASE_FREQ (329500) -/* -#define MOTORS_TIM_M1_PERIF RCC_APB1Periph_TIM2 -#define MOTORS_TIM_M1 TIM2 -#define MOTORS_TIM_M1_DBG DBGMCU_TIM2_STOP -#define M1_TIM_SETCOMPARE TIM_SetCompare2 -#define M1_TIM_GETCAPTURE TIM_GetCapture2 - -#define MOTORS_TIM_M2_PERIF RCC_APB1Periph_TIM2 -#define MOTORS_TIM_M2 TIM2 -#define MOTORS_TIM_M2_DBG DBGMCU_TIM2_STOP -#define M2_TIM_SETCOMPARE TIM_SetCompare4 -#define M2_TIM_GETCAPTURE TIM_GetCapture4 - -#define MOTORS_TIM_M3_PERIF RCC_APB1Periph_TIM2 -#define MOTORS_TIM_M3 TIM2 -#define MOTORS_TIM_M3_DBG DBGMCU_TIM2_STOP -#define M3_TIM_SETCOMPARE TIM_SetCompare1 -#define M3_TIM_GETCAPTURE TIM_GetCapture1 - -#define MOTORS_TIM_M4_PERIF RCC_APB1Periph_TIM4 -#define MOTORS_TIM_M4 TIM4 -#define MOTORS_TIM_M4_DBG DBGMCU_TIM4_STOP -#define M4_TIM_SETCOMPARE TIM_SetCompare4 -#define M4_TIM_GETCAPTURE TIM_GetCapture4 - -#define MOTORS_GPIO_M1_PERIF RCC_AHB1Periph_GPIOB -#define MOTORS_GPIO_M1_PORT GPIOA -#define MOTORS_GPIO_M1_PIN GPIO_Pin_1 // TIM2_CH2 -#define MOTORS_GPIO_AF_M1_PIN GPIO_PinSource1 -#define MOTORS_GPIO_AF_M1 GPIO_AF_TIM2 - -#define MOTORS_GPIO_M2_PERIF RCC_AHB1Periph_GPIOB -#define MOTORS_GPIO_M2_PORT GPIOB -#define MOTORS_GPIO_M2_PIN GPIO_Pin_11 // TIM2_CH4 -#define MOTORS_GPIO_AF_M2_PIN GPIO_PinSource11 -#define MOTORS_GPIO_AF_M2 GPIO_AF_TIM2 - -#define MOTORS_GPIO_M3_PERIF RCC_AHB1Periph_GPIOA -#define MOTORS_GPIO_M3_PORT GPIOA -#define MOTORS_GPIO_M3_PIN GPIO_Pin_15 // TIM2_CH1 -#define MOTORS_GPIO_AF_M3_PIN GPIO_PinSource15 -#define MOTORS_GPIO_AF_M3 GPIO_AF_TIM2 - -#define MOTORS_GPIO_M4_PERIF RCC_AHB1Periph_GPIOB -#define MOTORS_GPIO_M4_PORT GPIOB -#define MOTORS_GPIO_M4_PIN GPIO_Pin_9 // TIM4_CH4 -#define MOTORS_GPIO_AF_M4_PIN GPIO_PinSource9 -#define MOTORS_GPIO_AF_M4 GPIO_AF_TIM4*/ - -/* Utils Conversion macro */ -/* -#ifdef BRUSHLESS_MOTORCONTROLLER -#define C_BITS_TO_16(X) (0xFFFF * (X - MOTORS_PWM_CNT_FOR_1MS) / MOTORS_PWM_CNT_FOR_1MS) -#define C_16_TO_BITS(X) (MOTORS_PWM_CNT_FOR_1MS + ((X * MOTORS_PWM_CNT_FOR_1MS) / 0xFFFF)) -#else -#define C_BITS_TO_16(X) ((X)<<(16-MOTORS_PWM_BITS)) -#define C_16_TO_BITS(X) ((X)>>(16-MOTORS_PWM_BITS)&((1<CR1 = PIEZO_TIM->CR1 | TIM_CR1_CEN; - portENABLE_INTERRUPTS(); - //Enable the timer PWM outputs TIM_CtrlPWMOutputs(PIEZO_TIM, ENABLE); - /*TIM_CtrlPWMOutputs(MOTORS_TIM_M2, ENABLE); - TIM_CtrlPWMOutputs(MOTORS_TIM_M3, ENABLE); - TIM_CtrlPWMOutputs(MOTORS_TIM_M4, ENABLE);*/ - // Halt timer during debug halt. - //DBGMCU_APB2PeriphConfig(PIEZO_TIM_DBG, ENABLE); - TIM_SetCompare4(PIEZO_TIM, 0x00); TIM_SetCompare3(PIEZO_TIM, 0x00); + TIM_SetCompare4(PIEZO_TIM, 0x00); - //TIM_SetCompare4(PIEZO_TIM, 0x40); - //TIM_SetCompare3(PIEZO_TIM, 0x40); - - -/* DBGMCU_APB2PeriphConfig(MOTORS_TIM_M2_DBG, ENABLE); - DBGMCU_APB2PeriphConfig(MOTORS_TIM_M3_DBG, ENABLE); - DBGMCU_APB2PeriphConfig(MOTORS_TIM_M4_DBG, ENABLE);*/ + //Enable the timer + TIM_Cmd(PIEZO_TIM, ENABLE); isInit = true; } @@ -229,102 +137,11 @@ bool piezoTest(void) void piezoSetRatio(uint8_t ratio) { - TIM_SetCompare4(PIEZO_TIM, ratio); TIM_SetCompare3(PIEZO_TIM, ratio); + TIM_SetCompare4(PIEZO_TIM, ratio); } void piezoSetFreq(uint16_t freq) { TIM_PrescalerConfig(PIEZO_TIM, (PIEZO_BASE_FREQ/freq), TIM_PSCReloadMode_Update); } - -/*void motorsSetRatio(int id, uint16_t ratio) -{ - switch(id) - { - case MOTOR_M1: - M1_TIM_SETCOMPARE(MOTORS_TIM_M1, C_16_TO_BITS(ratio)); - break; - case MOTOR_M2: - M2_TIM_SETCOMPARE(MOTORS_TIM_M2, C_16_TO_BITS(ratio)); - break; - case MOTOR_M3: - M3_TIM_SETCOMPARE(MOTORS_TIM_M3, C_16_TO_BITS(ratio)); - break; - case MOTOR_M4: - M4_TIM_SETCOMPARE(MOTORS_TIM_M4, C_16_TO_BITS(ratio)); - break; - } -} - -int motorsGetRatio(int id) -{ - switch(id) - { - case MOTOR_M1: - return C_BITS_TO_16(M1_TIM_GETCAPTURE(MOTORS_TIM_M1)); - case MOTOR_M2: - return C_BITS_TO_16(M2_TIM_GETCAPTURE(MOTORS_TIM_M2)); - case MOTOR_M3: - return C_BITS_TO_16(M3_TIM_GETCAPTURE(MOTORS_TIM_M3)); - case MOTOR_M4: - return C_BITS_TO_16(M4_TIM_GETCAPTURE(MOTORS_TIM_M4)); - } - - return -1; -} -*/ -/*#ifdef MOTOR_RAMPUP_TEST -// FreeRTOS Task to test the Motors driver with a rampup of each motor alone. -void motorsTestTask(void* params) -{ - int step=0; - float rampup = 0.01; - - motorsSetupMinMaxPos(); - motorsSetRatio(MOTOR_M4, 1*(1<<16) * 0.0); - motorsSetRatio(MOTOR_M3, 1*(1<<16) * 0.0); - motorsSetRatio(MOTOR_M2, 1*(1<<16) * 0.0); - motorsSetRatio(MOTOR_M1, 1*(1<<16) * 0.0); - vTaskDelay(M2T(1000)); - - while(1) - { - vTaskDelay(M2T(100)); - - motorsSetRatio(MOTOR_M4, 1*(1<<16) * rampup); - motorsSetRatio(MOTOR_M3, 1*(1<<16) * rampup); - motorsSetRatio(MOTOR_M2, 1*(1<<16) * rampup); - motorsSetRatio(MOTOR_M1, 1*(1<<16) * rampup); - - rampup += 0.001; - if (rampup >= 0.1) - { - if(++step>3) step=0; - rampup = 0.01; - } - } -} -#else -// FreeRTOS Task to test the Motors driver -void motorsTestTask(void* params) -{ - static const int sequence[] = {0.1*(1<<16), 0.15*(1<<16), 0.2*(1<<16), 0.25*(1<<16)}; - int step=0; - - //Wait 3 seconds before starting the motors - vTaskDelay(M2T(3000)); - - while(1) - { - motorsSetRatio(MOTOR_M4, sequence[step%4]); - motorsSetRatio(MOTOR_M3, sequence[(step+1)%4]); - motorsSetRatio(MOTOR_M2, sequence[(step+2)%4]); - motorsSetRatio(MOTOR_M1, sequence[(step+3)%4]); - - if(++step>3) step=0; - - vTaskDelay(M2T(1000)); - } -} -#endif*/ diff --git a/drivers/src/ws2812.c b/drivers/src/ws2812_cf2.c similarity index 99% rename from drivers/src/ws2812.c rename to drivers/src/ws2812_cf2.c index 74525c08d3..806f8c873c 100644 --- a/drivers/src/ws2812.c +++ b/drivers/src/ws2812_cf2.c @@ -86,7 +86,7 @@ void ws2812Init(void) GPIO_PinAFConfig(GPIOB, GPIO_PinSource5, GPIO_AF_TIM3); /* Compute the prescaler value */ - PrescalerValue = 0; //(uint16_t) (72000000 / 24000000) - 1; + PrescalerValue = 0; /* Time base configuration */ TIM_TimeBaseStructure.TIM_Period = (105 - 1); // 800kHz TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue; diff --git a/modules/interface/ledring12.h b/modules/interface/ledring12.h new file mode 100644 index 0000000000..d485710fa4 --- /dev/null +++ b/modules/interface/ledring12.h @@ -0,0 +1,15 @@ +#ifndef __LEDRING12_H__ +#define __LEDRING12_H__ + +#include +#include + +#define NBR_LEDS 12 + +extern uint8_t ledringmem[NBR_LEDS * 2]; + +typedef void (*Ledring12Effect)(uint8_t buffer[][3], bool reset); + +void ledring12Init(void); + +#endif //__LEDRING12_H__ diff --git a/modules/interface/neopixelring.h b/modules/interface/neopixelring.h deleted file mode 100644 index e75ab65c9a..0000000000 --- a/modules/interface/neopixelring.h +++ /dev/null @@ -1,15 +0,0 @@ -#ifndef __NEOPIXELRING_H__ -#define __NEOPIXELRING_H__ - -#include -#include - -#define NBR_LEDS 12 - -extern uint8_t ledringmem[NBR_LEDS * 2]; - -typedef void (*NeopixelRingEffect)(uint8_t buffer[][3], bool reset); - -void neopixelringInit(void); - -#endif //__NEOPIXELRING_H__ diff --git a/modules/src/buzzer.c b/modules/src/buzzer.c index 0136b125ee..324d3463c2 100644 --- a/modules/src/buzzer.c +++ b/modules/src/buzzer.c @@ -21,11 +21,9 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . * - * neopixelring.c: NeoPixel Ring 12 Leds effects/driver + * buzzer.c: Play tones or melodies */ -#include "neopixelring.h" - #include #include #include "stm32fxxx.h" diff --git a/modules/src/expbrd.c b/modules/src/expbrd.c index d6b08e2099..b347cbbbdb 100644 --- a/modules/src/expbrd.c +++ b/modules/src/expbrd.c @@ -34,7 +34,7 @@ #include "ow.h" #include "crc.h" #include "exptest.h" -#include "neopixelring.h" +#include "ledring12.h" #include "bigquad.h" #include "imu.h" #include "config.h" @@ -64,7 +64,7 @@ void expbrdInit() #ifndef BRUSHLESS_PROTO_DECK_MAPPING // Can't have LED-ring and brushless breakout at the same time // as they share TIM3 - neopixelringInit(); + ledring12Init(); #endif } diff --git a/modules/src/neopixelring.c b/modules/src/ledring12.c similarity index 97% rename from modules/src/neopixelring.c rename to modules/src/ledring12.c index 4df7ab7aae..f1f9efbffd 100644 --- a/modules/src/neopixelring.c +++ b/modules/src/ledring12.c @@ -21,10 +21,10 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . * - * neopixelring.c: NeoPixel Ring 12 Leds effects/driver + * ledring12.c: RGB Ring 12 Leds effects/driver */ -#include "neopixelring.h" +#include "ledring12.h" #include #include @@ -187,7 +187,7 @@ static void virtualMemEffect(uint8_t buffer[][3], bool reset) for (i = 0; i < NBR_LEDS; i++) { uint8_t R5, G6, B5; - uint8_t (*led)[2] = ledringmem; + uint8_t (*led)[2] = (uint8_t (*)[2])ledringmem; // Convert from RGB565 to RGB888 R5 = led[i][0] >> 3; G6 = ((led[i][0] & 0x07) << 3) | (led[i][1] >> 5); @@ -561,7 +561,7 @@ static void siren(uint8_t buffer[][3], bool reset) /**************** Effect list ***************/ -NeopixelRingEffect effectsFct[] = +Ledring12Effect effectsFct[] = { blackEffect, whiteSpinEffect, @@ -584,7 +584,7 @@ static xTimerHandle timer; -void neopixelringWorker(void * data) +void ledring12Worker(void * data) { static int current_effect = 0; static uint8_t buffer[NBR_LEDS][3]; @@ -606,14 +606,14 @@ void neopixelringWorker(void * data) ws2812Send(buffer, NBR_LEDS); } -static void neopixelringTimer(xTimerHandle timer) +static void ledring12Timer(xTimerHandle timer) { - workerSchedule(neopixelringWorker, NULL); + workerSchedule(ledring12Worker, NULL); setHeadlightsOn(headlightEnable); } -void neopixelringInit(void) +void ledring12Init(void) { GPIO_InitTypeDef GPIO_InitStructure; @@ -628,7 +628,7 @@ void neopixelringInit(void) GPIO_Init(GPIOB, &GPIO_InitStructure); timer = xTimerCreate( (const signed char *)"ringTimer", M2T(50), - pdTRUE, NULL, neopixelringTimer ); + pdTRUE, NULL, ledring12Timer ); xTimerStart(timer, 100); } diff --git a/modules/src/mem.c b/modules/src/mem.c index 294955732c..4e84266bf9 100644 --- a/modules/src/mem.c +++ b/modules/src/mem.c @@ -40,7 +40,10 @@ #include "mem.h" #include "ow.h" #include "eeprom.h" -#include "neopixelring.h" +#ifdef PLATFORM_CF2 +#include "ledring12.h" +#endif + #include "console.h" #include "cfassert.h" @@ -72,7 +75,12 @@ #endif #define EEPROM_ID 0x00 -#define NBR_LEDMEM 1 +#ifdef PLATFORM_CF1 + #define NBR_LEDMEM 0 + uint8_t ledringmem[1]; +#else + #define NBR_LEDMEM 1 +#endif #define LEDMEM_ID 0x01 #define NBR_STATIC_MEM (NBR_EEPROM + NBR_LEDMEM) @@ -180,7 +188,7 @@ void memSettingsProcess(int command) } else if (memId == LEDMEM_ID) { - // Memory type (eeprom) + // Memory type virtual ledring mem p.data[2] = MEM_TYPE_LED12; p.size += 1; // Size of the memory @@ -296,11 +304,11 @@ void memWriteProcess() if ((memAddr + writeLen) <= sizeof(ledringmem)) { memcpy(&(ledringmem[memAddr]), &p.data[5], writeLen); - DEBUG("LED write addr:%i, led:%i\n", memAddr, writeLen); + MEM_DEBUG("LED write addr:%i, led:%i\n", memAddr, writeLen); } else { - DEBUG("\LED write failed! addr:%i, led:%i\n", memAddr, writeLen); + MEM_DEBUG("\LED write failed! addr:%i, led:%i\n", memAddr, writeLen); } } else diff --git a/modules/src/system.c b/modules/src/system.c index 4b0ca9cc7b..4f9bd5e3b7 100644 --- a/modules/src/system.c +++ b/modules/src/system.c @@ -51,7 +51,6 @@ #include "comm.h" #include "stabilizer.h" #include "commander.h" -#include "neopixelring.h" #include "console.h" #include "usb.h" #include "expbrd.h" From 97048739620377a2331ec865a47ee5281f6a8d30 Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Mon, 24 Aug 2015 12:09:12 +0200 Subject: [PATCH 06/58] Removed ws2812 driver from CF1. It is conflicting (TIM1) with usec_time and rarely used. --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 2988577f4a..9ed0add369 100644 --- a/Makefile +++ b/Makefile @@ -122,7 +122,7 @@ PROJ_OBJ_CF2 = platform_cf2.o # Drivers PROJ_OBJ += exti.o nvic.o motors.o PROJ_OBJ_CF1 += led_f103.o i2cdev_f103.o i2croutines.o adc_f103.o mpu6050.o -PROJ_OBJ_CF1 += hmc5883l.o ms5611.o nrf24l01.o eeprom.o ws2812_cf1.o +PROJ_OBJ_CF1 += hmc5883l.o ms5611.o nrf24l01.o eeprom.o PROJ_OBJ_CF2 += led_f405.o mpu6500.o i2cdev_f405.o ws2812_cf2.o lps25h.o PROJ_OBJ_CF2 += ak8963.o eeprom.o maxsonar.o piezo.o PROJ_OBJ_CF2 += uart_syslink.o swd.o uart1.o uart2.o From 66cbc7a32f2980f773b260887d4b996b2c458e80 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Mon, 24 Aug 2015 19:04:15 +0200 Subject: [PATCH 07/58] Implemented modular Deck driver subsystem The deck drivers are now placed in deck/drivers/src and they are declare inside the driver file itself, for example: ``` static const DeckDriver ledring12_deck = { .vid = 0xBC, .pid = 0x01, .name = "bcLedRing", .usedPeriph = DECK_TIMER3, .usedGpio = DECK_IO_2 | DECK_IO_3, .init = ledring12Init, }; DECK_DRIVER(ledring12_deck); ``` --- Makefile | 27 +- deck/core/deck.c | 118 +++++++++ deck/core/deck_drivers.c | 105 ++++++++ deck/core/deck_info.c | 250 ++++++++++++++++++ .../drivers}/interface/ledring12.h | 4 - {modules => deck/drivers}/src/bigquad.c | 24 +- {modules => deck/drivers}/src/buzzer.c | 16 +- {modules => deck/drivers}/src/exptest.c | 24 +- {modules => deck/drivers}/src/ledring12.c | 19 +- deck/interface/deck.h | 4 + deck/interface/deck_core.h | 164 ++++++++++++ modules/interface/bigquad.h | 35 --- modules/interface/buzzer.h | 9 - modules/interface/expbrd.h | 58 ---- modules/interface/exptest.h | 34 --- modules/src/expbrd.c | 189 ------------- modules/src/system.c | 29 +- scripts/F405/linker/sections_FLASH.ld | 67 ++--- 18 files changed, 776 insertions(+), 400 deletions(-) create mode 100644 deck/core/deck.c create mode 100644 deck/core/deck_drivers.c create mode 100644 deck/core/deck_info.c rename {modules => deck/drivers}/interface/ledring12.h (65%) rename {modules => deck/drivers}/src/bigquad.c (75%) rename {modules => deck/drivers}/src/buzzer.c (96%) rename {modules => deck/drivers}/src/exptest.c (93%) rename {modules => deck/drivers}/src/ledring12.c (97%) create mode 100644 deck/interface/deck_core.h delete mode 100644 modules/interface/bigquad.h delete mode 100644 modules/interface/buzzer.h delete mode 100644 modules/interface/expbrd.h delete mode 100644 modules/interface/exptest.h delete mode 100644 modules/src/expbrd.c diff --git a/Makefile b/Makefile index 9ed0add369..dc1d112b8c 100644 --- a/Makefile +++ b/Makefile @@ -75,7 +75,7 @@ VPATH_CF2 += $(STLIB)/STM32_CPAL_Driver/src VPATH_CF2 += $(STLIB)/STM32_USB_Device_Library/Core/src VPATH_CF2 += $(STLIB)/STM32_USB_OTG_Driver/src VPATH_CF2 += $(STLIB)/STM32_CPAL_Driver/devices/stm32f4xx -VPATH_CF2 += deck/api +VPATH_CF2 += deck/api deck/core deck/drivers/src CRT0_CF2 = startup_stm32f40xx.o system_stm32f4xx.o # Should maybe be in separate file? @@ -115,15 +115,15 @@ endif ############### Source files configuration ################ # Init -PROJ_OBJ = main.o -PROJ_OBJ_CF1 = platform_cf1.o -PROJ_OBJ_CF2 = platform_cf2.o +PROJ_OBJ += main.o +PROJ_OBJ_CF1 += platform_cf1.o +PROJ_OBJ_CF2 += platform_cf2.o # Drivers PROJ_OBJ += exti.o nvic.o motors.o -PROJ_OBJ_CF1 += led_f103.o i2cdev_f103.o i2croutines.o adc_f103.o mpu6050.o +PROJ_OBJ_CF1 += led_f103.o i2cdev_f103.o i2croutines.o adc_f103.o mpu6050.o PROJ_OBJ_CF1 += hmc5883l.o ms5611.o nrf24l01.o eeprom.o -PROJ_OBJ_CF2 += led_f405.o mpu6500.o i2cdev_f405.o ws2812_cf2.o lps25h.o +PROJ_OBJ_CF2 += led_f405.o mpu6500.o i2cdev_f405.o ws2812_cf2.o lps25h.o PROJ_OBJ_CF2 += ak8963.o eeprom.o maxsonar.o piezo.o PROJ_OBJ_CF2 += uart_syslink.o swd.o uart1.o uart2.o # USB Files @@ -138,14 +138,21 @@ PROJ_OBJ_CF2 += imu_cf2.o pm_f405.o syslink.o radiolink.o ow_syslink.o proximity PROJ_OBJ += system.o comm.o console.o pid.o crtpservice.o param.o mem.o PROJ_OBJ += commander.o controller.o sensfusion6.o stabilizer.o PROJ_OBJ += log.o worker.o trigger.o sitaw.o buzzer.o -PROJ_OBJ_CF2 += ledring12.o expbrd.o platformservice.o bigquad.o +PROJ_OBJ_CF2 += platformservice.o -# Expansion boards -PROJ_OBJ_CF2 += exptest.o +# Deck Core +PROJ_OBJ_CF2 += deck.o deck_info.o deck_drivers.o + +# Deck API PROJ_OBJ_CF2 += deck_constants.o PROJ_OBJ_CF2 += deck_digital.o PROJ_OBJ_CF2 += deck_analog.o +# Decks +PROJ_OBJ_CF2 += bigquad.o +PROJ_OBJ_CF2 += exptest.o +PROJ_OBJ_CF2 += ledring12.o + # Utilities PROJ_OBJ += filter.o cpuid.o cfassert.o eprintf.o crc.o fp16.o debug.o PROJ_OBJ += version.o @@ -187,7 +194,7 @@ INCLUDES_CF2 += -I$(STLIB)/STM32_CPAL_Driver/inc INCLUDES_CF2 += -I$(STLIB)/STM32_CPAL_Driver/devices/stm32f4xx INCLUDES_CF2 += -I$(STLIB)/STM32_USB_Device_Library/Core/inc INCLUDES_CF2 += -I$(STLIB)/STM32_USB_OTG_Driver/inc -INCLUDES_CF2 += -Ideck/interface +INCLUDES_CF2 += -Ideck/interface -I deck/drivers/interface ifeq ($(USE_FPU), 1) PROCESSOR = -mcpu=cortex-m4 -mthumb -mfloat-abi=hard -mfpu=fpv4-sp-d16 diff --git a/deck/core/deck.c b/deck/core/deck.c new file mode 100644 index 0000000000..30f2f67de9 --- /dev/null +++ b/deck/core/deck.c @@ -0,0 +1,118 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2015 Bitcraze AB + * + * 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, in version 3. + * + * 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 . + * + * deck.c - Deck subsystem main entry points + */ + +#define DEBUG_MODULE "DECK_CORE" + +#include + +#include "deck.h" +#include "debug.h" + +#ifndef DECK_FORCE +#define DECK_FORCE +#endif + +#define xstr(s) str(s) +#define str(s) #s + +static char* deck_force = xstr(DECK_FORCE); + +extern void deckInfoInit(); + +void deckInit() +{ + int nDecks; + int i; + + deckDriverCount(); + deckInfoInit(); + + nDecks = deckCount(); + + DEBUG_PRINT("%d deck enumerated\n", nDecks); + + for (i=0; idriver->init) { + if (deck->driver->name) { + DEBUG_PRINT("Calling INIT from driver %s for deck %i\n", deck->driver->name, i); + } else { + DEBUG_PRINT("Calling INIT for deck %i\n", i); + } + + deck->driver->init(deck); + } + } + + // Init build-forced driver + if (strlen(deck_force)>0) { + const DeckDriver *driver = deckFindDriverByName(deck_force); + if (!driver) { + DEBUG_PRINT("WARNING: compile-time forced driver '%s' not found\n", deck_force); + } else if (driver->init) { + DEBUG_PRINT("Initializing compile-time forced driver '%s'\n", deck_force); + driver->init(NULL); // Passing NULL as deck info + } + } +} + +bool deckTest() +{ + int nDecks; + int i; + bool pass = true; + + nDecks = deckCount(); + + for (i=0; idriver->test) { + if (deck->driver->test()) { + DEBUG_PRINT("Deck %i test [OK].", i); + } else { + DEBUG_PRINT("Deck %i test [FAIL].", i); + pass = false; + } + } + } + + // Test build-forced driver + if (strlen(deck_force)>0) { + const DeckDriver *driver = deckFindDriverByName(deck_force); + if (driver && driver->test) { + if (driver->test()) { + DEBUG_PRINT("Compile-time forced driver '%s' test [OK]\n", deck_force); + } else { + DEBUG_PRINT("Compile-time forced driver '%s' test [FAIL]\n", deck_force); + pass = false; + } + } + } + + return pass; +} diff --git a/deck/core/deck_drivers.c b/deck/core/deck_drivers.c new file mode 100644 index 0000000000..dfcfb74e16 --- /dev/null +++ b/deck/core/deck_drivers.c @@ -0,0 +1,105 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2011-2012 Bitcraze AB + * + * 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, in version 3. + * + * 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 . + * + * deck_drivers.c - Deck drivers loading and handling + */ + +#define DEBUG_MODULE "DECK_DRIVERS" + +#include +#include + +#include "deck.h" + +#include "debug.h" + +/* Symbols set by the linker script */ +extern const struct deck_driver * _deckDriver_start; +extern const struct deck_driver * _deckDriver_stop; + +static const struct deck_driver ** drivers; +static int driversLen; + +// Init the toc access variables. Lazy initialisation: it is going to be done +// the first time any api function is called. +static void deckdriversInit() { + static bool init = false; + if (!init) { + int i; + + drivers = &_deckDriver_start; + driversLen = &_deckDriver_stop - &_deckDriver_start; + init = true; + + DEBUG_PRINT("Found %d drivers\n", driversLen); + for (i=0; iname) { + DEBUG_PRINT("VID:PID %02x:%02x (%s)\n", drivers[i]->vid, drivers[i]->pid, drivers[i]->name); + } else { + DEBUG_PRINT("VID:PID %02x:%02x\n", drivers[i]->vid, drivers[i]->pid); + } + + } + } +} + +int deckDriverCount() { + deckdriversInit(); + + return driversLen; +} + +const struct deck_driver* deckGetDriver(int i) { + deckdriversInit(); + + if (ivid) && (pid == drivers[i]->pid)) { + return drivers[i]; + } + } + return NULL; +} + +const DeckDriver* deckFindDriverByName(char* name) { + int i; + + deckdriversInit(); + + for (i=0; iname)) { + return drivers[i]; + } + } + return NULL; +} diff --git a/deck/core/deck_info.c b/deck/core/deck_info.c new file mode 100644 index 0000000000..b828cb01bb --- /dev/null +++ b/deck/core/deck_info.c @@ -0,0 +1,250 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2015 Bitcraze AB + * + * 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, in version 3. + * + * 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 . + * + * deck_ow.c - Functions to decode the decks oneWire memory content + */ + +#include +#include +#include + +#define DEBUG_MODULE "DECK_INFO" + +#include "deck.h" + +#include "ow.h" +#include "crc.h" +#include "debug.h" + +static int count = 0; +static DeckInfo deckInfos[DECK_MAX_COUNT]; + +static void enumerateDecks(void); + +void deckInfoInit() +{ + static bool isInit = false; + + if (isInit) return; + + enumerateDecks(); + + isInit = true; +} + +int deckCount(void) +{ + return count; +} + +DeckInfo * deckInfo(int i) +{ + if (itlv, DECK_INFO_NAME, name, 30); + + if (deck->vid) { + driver = deckFindDriverByVidPid(deck->vid, deck->pid); + } else if (strlen(name)>0) { + driver = deckFindDriverByName(name); + } + + if (driver == NULL) + driver = &dummyDriver; + + return driver; +} + +void printDeckInfo(DeckInfo *info) +{ + char name[30] = "NoName"; + char rev[10] = "NoRev"; + + if (deckTlvHasElement(&info->tlv, DECK_INFO_NAME)) { + deckTlvGetString(&info->tlv, DECK_INFO_NAME, name, 30); + } + + if (deckTlvHasElement(&info->tlv, DECK_INFO_REVISION)) { + deckTlvGetString(&info->tlv, DECK_INFO_REVISION, rev, 10); + } + + DEBUG_PRINT("Deck %02x:%02x %s (Rev. %s)\n", info->vid, info->pid, name, rev); + DEBUG_PRINT("Used pin: %08x\n", (unsigned int)info->usedPins); + + if (info->driver == &dummyDriver) { + DEBUG_PRINT("Warning! No driver found for deck.\n"); + } else { + DEBUG_PRINT("Driver implements: [ %s%s]\n", + info->driver->init?"init ":"", info->driver->test?"test ":""); + } +} + +static bool infoDecode(DeckInfo * info) +{ + uint8_t crcHeader; + uint8_t crcTlv; + + if (info->header != DECK_INFO_HEADER_ID) { + DEBUG_PRINT("Memory error: wrong header ID\n"); + return false; + } + + crcHeader = crcSlow(info->raw, DECK_INFO_HEADER_SIZE); + if(info->crc != crcHeader) { + DEBUG_PRINT("Memory error: incorrect header CRC\n"); + return false; + } + + if(info->raw[DECK_INFO_TLV_VERSION_POS] != DECK_INFO_TLV_VERSION) { + DEBUG_PRINT("Memory error: incorrect TLV version\n"); + return false; + } + + crcTlv = crcSlow(&info->raw[DECK_INFO_TLV_VERSION_POS], info->raw[DECK_INFO_TLV_LENGTH_POS]+2); + if(crcTlv != info->raw[DECK_INFO_TLV_DATA_POS + info->raw[DECK_INFO_TLV_LENGTH_POS]]) { + DEBUG_PRINT("Memory error: incorrect TLV CRC %x!=%x\n", (unsigned int)crcTlv, + info->raw[DECK_INFO_TLV_DATA_POS + info->raw[DECK_INFO_TLV_LENGTH_POS]]); + return false; + } + + info->tlv.data = &info->raw[DECK_INFO_TLV_DATA_POS]; + info->tlv.length = info->raw[DECK_INFO_TLV_LENGTH_POS]; + + return true; +} + +static void enumerateDecks(void) +{ + uint8_t nDecks = 0; + int i; + bool noError = true; + + owInit(); + + if (owScan(&nDecks)) + { + DEBUG_PRINT("Found %d deck memor%s.\n", nDecks, nDecks>1?"ies":"y"); + } else { + DEBUG_PRINT("Error scanning for deck memories, " + "no deck drivers will be initialised\n"); + nDecks = 0; + } + + for (i = 0; i < nDecks; i++) + { + DEBUG_PRINT("Enumerating deck %i\n", i); + if (owRead(i, 0, sizeof(deckInfos[0].raw), (uint8_t *)&deckInfos[i])) + { + if (infoDecode(&deckInfos[i])) + { + deckInfos[i].driver = findDriver(&deckInfos[i]); + + printDeckInfo(&deckInfos[i]); + } else { + DEBUG_PRINT("Deck %i has corrupted OW memory. " + "No driver will be initialized!\n", i); + noError = false; + } + } + else + { + DEBUG_PRINT("Reading deck nr:%d [FAILED]. " + "No driver will be initialized!\n", i); + noError = false; + } + } + + if (noError) { + count = nDecks; + } + + return; +} + +/****** Key/value area handling ********/ +static int findType(TlvArea *tlv, int type) { + int pos = 0; + + while (pos < tlv->length) { + if (tlv->data[pos] == type) { + return pos; + } else { + pos += tlv->data[pos+1]+2; + } + } + return -1; +} + +bool deckTlvHasElement(TlvArea *tlv, int type) { + return findType(tlv, type) >= 0; +} + +int deckTlvGetString(TlvArea *tlv, int type, char *string, int length) { + int pos = findType(tlv, type); + int strlength = 0; + + if (pos >= 0) { + strlength = tlv->data[pos+1]; + + if (strlength > (length-1)) { + strlength = length-1; + } + + memcpy(string, &tlv->data[pos+2], strlength); + string[strlength] = '\0'; + + return strlength; + } else { + string[0] = '\0'; + + return -1; + } +} + +char* deckTlvGetBuffer(TlvArea *tlv, int type, int *length) { + int pos = findType(tlv, type); + if (pos >= 0) { + *length = tlv->data[pos+1]; + return (char*) &tlv->data[pos+2]; + } + + return NULL; +} + +void deckTlvGetTlv(TlvArea *tlv, int type, TlvArea *output) { + output->length = 0; + output->data = (uint8_t *)deckTlvGetBuffer(tlv, type, &output->length); +} diff --git a/modules/interface/ledring12.h b/deck/drivers/interface/ledring12.h similarity index 65% rename from modules/interface/ledring12.h rename to deck/drivers/interface/ledring12.h index d485710fa4..c4bbfc2fe9 100644 --- a/modules/interface/ledring12.h +++ b/deck/drivers/interface/ledring12.h @@ -8,8 +8,4 @@ extern uint8_t ledringmem[NBR_LEDS * 2]; -typedef void (*Ledring12Effect)(uint8_t buffer[][3], bool reset); - -void ledring12Init(void); - #endif //__LEDRING12_H__ diff --git a/modules/src/bigquad.c b/deck/drivers/src/bigquad.c similarity index 75% rename from modules/src/bigquad.c rename to deck/drivers/src/bigquad.c index 82bb817b35..ce81e9babe 100644 --- a/modules/src/bigquad.c +++ b/deck/drivers/src/bigquad.c @@ -1,6 +1,6 @@ /* - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ @@ -30,14 +30,14 @@ #include "stm32fxxx.h" #include "config.h" -#include "bigquad.h" #include "motors.h" #include "debug.h" +#include "deck.h" //Hardware configuration static bool isInit; -void bigquadInit() +static void bigquadInit(DeckInfo *info) { if(isInit) return; @@ -48,7 +48,7 @@ void bigquadInit() isInit = true; } -bool bigquadTest() +static bool bigquadTest() { bool status = true; @@ -59,3 +59,17 @@ bool bigquadTest() return status; } + +static const DeckDriver bigquad_deck = { + .vid = 0xBC, + .pid = 0x03, + .name = "bcBigQuad", + + .usedPeriph = DECK_TIMER3, + .usedGpio = 0, // FIXME: Edit the used GPIOs + + .init = bigquadInit, + .test = bigquadTest, +}; + +DECK_DRIVER(bigquad_deck); diff --git a/modules/src/buzzer.c b/deck/drivers/src/buzzer.c similarity index 96% rename from modules/src/buzzer.c rename to deck/drivers/src/buzzer.c index 324d3463c2..997ff13e71 100644 --- a/modules/src/buzzer.c +++ b/deck/drivers/src/buzzer.c @@ -31,6 +31,8 @@ #include "FreeRTOS.h" #include "timers.h" +#include "deck.h" + #include "param.h" #include "pm.h" #include "log.h" @@ -281,7 +283,7 @@ static void buzzTimer(xTimerHandle timer) effects[effect](counter*10); } -void buzzerInit(void) +static void buzzerInit(DeckInfo *info) { piezoInit(); @@ -301,3 +303,15 @@ PARAM_ADD(PARAM_UINT32 | PARAM_RONLY, nmelody, &melody) PARAM_ADD(PARAM_UINT16, freq, &static_freq) PARAM_ADD(PARAM_UINT8, ratio, &static_ratio) PARAM_GROUP_STOP(buzzer) + +static const DeckDriver buzzer_deck = { + .vid = 0, + .pid = 0, + .name = "bcBuzzer", + + .usedGpio = DECK_PA2 | DECK_PA3, + + .init = buzzerInit, +}; + +DECK_DRIVER(buzzer_deck); diff --git a/modules/src/exptest.c b/deck/drivers/src/exptest.c similarity index 93% rename from modules/src/exptest.c rename to deck/drivers/src/exptest.c index d5e887b56b..ba1dad40f0 100644 --- a/modules/src/exptest.c +++ b/deck/drivers/src/exptest.c @@ -1,6 +1,6 @@ /* - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ @@ -30,8 +30,10 @@ #include "stm32fxxx.h" #include "config.h" -#include "exptest.h" #include "debug.h" +#include "deck.h" + +#include "imu.h" //Hardware configuration #define ET_GPIO_PERIF (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOC) @@ -101,13 +103,15 @@ static bool isInit; static bool exptestTestAllPins(bool test); static bool exptestTestPin(EtGpio *etPin, bool test); -bool exptestRun(void) +static bool exptestRun(void) { int i; volatile int delay; bool status = true; isInit = true; + status &= imu6ManufacturingTest(); + GPIO_InitTypeDef GPIO_InitStructure; // Enable GPIOs @@ -211,3 +215,15 @@ static bool exptestTestPin(EtGpio *etPin, bool test) return false; } } + +static const DeckDriver exptest_deck = { + .vid = 0xBC, + .pid = 0xFF, + .name = "bcExpTest", + + .usedGpio = 0, // FIXME: Edit the used GPIOs + + .test = exptestRun, +}; + +DECK_DRIVER(exptest_deck); diff --git a/modules/src/ledring12.c b/deck/drivers/src/ledring12.c similarity index 97% rename from modules/src/ledring12.c rename to deck/drivers/src/ledring12.c index f1f9efbffd..286d28bc85 100644 --- a/modules/src/ledring12.c +++ b/deck/drivers/src/ledring12.c @@ -32,9 +32,12 @@ #include "stm32fxxx.h" +#include "deck.h" + #include "FreeRTOS.h" #include "timers.h" +#include "ledring12.h" #include "ws2812.h" #include "worker.h" #include "param.h" @@ -57,6 +60,7 @@ * system. See tiltEffect for an example. */ +typedef void (*Ledring12Effect)(uint8_t buffer[][3], bool reset); /**************** Some useful macros ***************/ @@ -613,7 +617,7 @@ static void ledring12Timer(xTimerHandle timer) setHeadlightsOn(headlightEnable); } -void ledring12Init(void) +static void ledring12Init(DeckInfo *info) { GPIO_InitTypeDef GPIO_InitStructure; @@ -643,3 +647,16 @@ PARAM_ADD(PARAM_FLOAT, glowstep, &glowstep) PARAM_ADD(PARAM_FLOAT, emptyCharge, &emptyCharge) PARAM_ADD(PARAM_FLOAT, fullCharge, &fullCharge) PARAM_GROUP_STOP(ring) + +static const DeckDriver ledring12_deck = { + .vid = 0xBC, + .pid = 0x01, + .name = "bcLedRing", + + .usedPeriph = DECK_TIMER3, + .usedGpio = DECK_IO_2 | DECK_IO_3, + + .init = ledring12Init, +}; + +DECK_DRIVER(ledring12_deck); diff --git a/deck/interface/deck.h b/deck/interface/deck.h index a7d3176581..5a53af82c5 100644 --- a/deck/interface/deck.h +++ b/deck/interface/deck.h @@ -3,6 +3,10 @@ #ifndef __DECK_H__ #define __DECK_H__ +/* Core: handles initialisation, discovery and drivers */ +#include "deck_core.h" + +/* Deck APIs */ #include "deck_contants.h" #include "deck_digital.h" #include "deck_analog.h" diff --git a/deck/interface/deck_core.h b/deck/interface/deck_core.h new file mode 100644 index 0000000000..6dc3da38e7 --- /dev/null +++ b/deck/interface/deck_core.h @@ -0,0 +1,164 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2011-2012 Bitcraze AB + * + * 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, in version 3. + * + * 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 . + * + * deck_core.h - Definitions and interface to handle deck init and drivers + * Implementation is in deck.c, deck_info.c and deck_drivers.c + */ + +#ifndef __DECK_CODE_H__ +#define __DECK_CODE_H__ + + +#include +#include + +/* Maximum number of decks that can be enumerated */ +#define DECK_MAX_COUNT 4 + +/* Main functions to init and test the decks, called during system initialisation */ +void deckInit(void); +bool deckTest(void); + +/***** Driver TOC definitions ******/ + +/* Used peripherals */ +#define DECK_UART1 (1<<0) +#define DECK_UART2 (1<<1) +#define DECK_SPI (1<<2) +#define DECK_TIMER3 (1<<3) + +/* Used GPIO */ +#define DECK_PC11 (1<<0) +#define DECK_RX1 (1<<0) +#define DECK_PC10 (1<<1) +#define DECK_TX1 (1<<1) +#define DECK_PB7 (1<<2) +#define DECK_SDA (1<<2) +#define DECK_PB6 (1<<3) +#define DECK_SCL (1<<3) +#define DECK_PB8 (1<<4) +#define DECK_IO_1 (1<<4) +#define DECK_PB5 (1<<5) +#define DECK_IO_2 (1<<5) +#define DECK_PB4 (1<<6) +#define DECK_IO_3 (1<<6) +#define DECK_PC12 (1<<7) +#define DECK_IO_4 (1<<7) +#define DECK_PA2 (1<<8) +#define DECK_TX2 (1<<8) +#define DECK_PA3 (1<<9) +#define DECK_RX2 (1<<9) +#define DECK_PA5 (1<<10) +#define DECK_SCK (1<<10) +#define DECK_PA6 (1<<11) +#define DECK_MISO (1<<11) +#define DECK_PA7 (1<<12) +#define DECK_MOSI (1<<12) + +struct deckInfo_s; + +/* Structure definition and registering macro */ +typedef struct deck_driver { + /* Identification of the deck (written in the board) */ + uint8_t vid; + uint8_t pid; + char *name; + + /* Periphreal and Gpio used _dirrectly_ by the driver */ + uint32_t usedPeriph; + uint32_t usedGpio; + + /* Init and test functions */ + void (*init)(struct deckInfo_s *); + bool (*test)(void); +} DeckDriver; + +#define DECK_DRIVER(NAME) const struct deck_driver * driver_##NAME __attribute__((section(".deckDriver." #NAME), used)) = &(NAME) + +/****** Deck_info *******/ + +#define DECK_INFO_HEADER_ID 0xeb +#define DECK_INFO_HEADER_SIZE 7 +#define DECK_INFO_TLV_VERSION 0 +#define DECK_INFO_TLV_VERSION_POS 8 +#define DECK_INFO_TLV_LENGTH_POS 9 +#define DECK_INFO_TLV_DATA_POS 10 + +typedef struct { + uint8_t *data; + int length; +} TlvArea; + +typedef struct deckInfo_s { + + union { + struct { + uint8_t header; + uint32_t usedPins; + uint8_t vid; + uint8_t pid; + uint8_t crc; + + uint8_t rawTlv[104]; + } __attribute__((packed)); + + uint8_t raw[112]; + }; + + TlvArea tlv; + const DeckDriver *driver; +} DeckInfo; + +int deckCount(void); + +DeckInfo * deckInfo(int i); + +/* Key/value area handling */ +bool deckTlvHasElement(TlvArea *tlv, int type); + +int deckTlvGetString(TlvArea *tlv, int type, char *string, int maxLength); + +char* deckTlvGetBuffer(TlvArea *tlv, int type, int *length); + +void deckTlvGetTlv(TlvArea *tlv, int type, TlvArea *output); + +/* Defined Types */ +#define DECK_INFO_NAME 1 +#define DECK_INFO_REVISION 2 +#define DECK_INFO_CUSTOMDATA 3 + +/***** Drivers introspection API ******/ + +/* Returns the number of driver registered */ +int deckDriverCount(); + +/* Returns one driver definition */ +const struct deck_driver* deckGetDriver(int i); + +/* Find driver by pid/vid */ +const struct deck_driver* deckFindDriverByVidPid(uint8_t vid, uint8_t pid); + +/*find driver by name */ +const struct deck_driver* deckFindDriverByName(char* name); + +#endif //__DECK_CODE_H__ diff --git a/modules/interface/bigquad.h b/modules/interface/bigquad.h deleted file mode 100644 index 164e450628..0000000000 --- a/modules/interface/bigquad.h +++ /dev/null @@ -1,35 +0,0 @@ -/** - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ - * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ - * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ - * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ - * - * Crazyflie control firmware - * - * Copyright (C) 2011-2012 Bitcraze AB - * - * 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, in version 3. - * - * 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 . - * - * bigquad.h - Big-Quad-Deck - */ -#ifndef __BIGQUAD_H__ -#define __BIGQUAD_H__ - -#include - -void bigquadInit(); -bool bigquadTest(); - -#endif //__BIGQUAD_H__ - diff --git a/modules/interface/buzzer.h b/modules/interface/buzzer.h deleted file mode 100644 index e9d2d936e6..0000000000 --- a/modules/interface/buzzer.h +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef __BUZZER_H__ -#define __BUZZER_H__ - -#include -#include - -void buzzerInit(void); - -#endif //__BUZZER_H__ diff --git a/modules/interface/expbrd.h b/modules/interface/expbrd.h deleted file mode 100644 index 74c04e9a02..0000000000 --- a/modules/interface/expbrd.h +++ /dev/null @@ -1,58 +0,0 @@ -/** - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ - * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ - * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ - * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ - * - * Crazyflie control firmware - * - * Copyright (C) 2011-2012 Bitcraze AB - * - * 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, in version 3. - * - * 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 . - * - * expbrd.h - Expansion board handling functions - */ -#ifndef __EXPBRD_H__ -#define __EXPBRD_H__ - -#include - -#define EXPBRD_MAX 5 -#define EXPBRD_ID 0xEB -#define EXPBRD_OW_ADDR 0x00 - -// Definition of Vendor ID -#define EXPBRD_VID_BITCRAZE 0xBC - -// Definition of expansion board Product ID -#define EXPBRD_PID_LEDRING 0x01 -#define EXPBRD_PID_QI 0x02 -#define EXPBRD_PID_BIGQUAD 0x03 -#define EXPBRD_PID_ET 0xFF - - -typedef struct _ExpbrdData -{ - uint8_t header; - uint8_t usedPins[4]; - uint8_t vid; - uint8_t pid; - uint8_t crc; -} __attribute__((packed)) ExpbrdData ; - -void expbrdInit(); -bool expbrdTest(); - -#endif //__EXPBRD_H__ - diff --git a/modules/interface/exptest.h b/modules/interface/exptest.h deleted file mode 100644 index bc2ffa16db..0000000000 --- a/modules/interface/exptest.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ - * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ - * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ - * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ - * - * Crazyflie control firmware - * - * Copyright (C) 2011-2012 Bitcraze AB - * - * 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, in version 3. - * - * 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 . - * - * exptest.c - Testing of expansion port. - */ - -#ifndef EXPTEST_H_ -#define EXPTEST_H_ - -#include - -bool exptestRun(void); - -#endif diff --git a/modules/src/expbrd.c b/modules/src/expbrd.c deleted file mode 100644 index b347cbbbdb..0000000000 --- a/modules/src/expbrd.c +++ /dev/null @@ -1,189 +0,0 @@ -/** - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ - * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ - * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ - * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ - * - * Crazyflie control firmware - * - * Copyright (C) 2011-2012 Bitcraze AB - * - * 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, in version 3. - * - * 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 . - * - * expbrd.c - Expansion board handling functions - */ -#define DEBUG_MODULE "EXPBRD" - -#include "FreeRTOS.h" -#include "task.h" -#include "queue.h" - -#include "expbrd.h" -#include "debug.h" -#include "ow.h" -#include "crc.h" -#include "exptest.h" -#include "ledring12.h" -#include "bigquad.h" -#include "imu.h" -#include "config.h" - -static bool isInit; -static uint8_t nBoards = 0; - -static ExpbrdData expbrdData[EXPBRD_MAX]; - -static void expbrdPrintData(ExpbrdData *data); -static bool expbrdIsValid(ExpbrdData *data); -static bool expbrdScan(void); -static bool expbrdIsPresent(uint8_t vid, uint8_t pid); - -void expbrdInit() -{ - if(isInit) - return; - - owInit(); - expbrdScan(); - -#ifndef FORCE_EXP_DETECT - if (expbrdIsPresent(EXPBRD_VID_BITCRAZE, EXPBRD_PID_LEDRING)) -#endif - { -#ifndef BRUSHLESS_PROTO_DECK_MAPPING - // Can't have LED-ring and brushless breakout at the same time - // as they share TIM3 - ledring12Init(); -#endif - } - - if (expbrdIsPresent(EXPBRD_VID_BITCRAZE, EXPBRD_PID_BIGQUAD)) - { - bigquadInit(); - } - - isInit = true; -} - -bool expbrdTest() -{ - bool status = true; - - if(!isInit) - return false; - - // For production test - if (expbrdIsPresent(EXPBRD_VID_BITCRAZE, EXPBRD_PID_ET)) - { - status &= imu6ManufacturingTest(); - status &= exptestRun(); - if (status) - { - DEBUG_PRINT("Expansion port test [OK].\n"); - } - else - { - DEBUG_PRINT("Expansion port test [FAIL].\n"); - } - } - status &= owTest(); - - return status; -} - -static bool expbrdScan(void) -{ - uint8_t i = 0; - bool status = false; - - if (owScan(&nBoards)) - { - DEBUG_PRINT("Found %d memories.\n", nBoards); - } - else - { - DEBUG_PRINT("Scan [FAILED].\n"); - } - - for (i = 0; i < nBoards; i++) - { - if (owRead(i, EXPBRD_OW_ADDR, sizeof(ExpbrdData), (uint8_t *)&expbrdData[i])) - { - if (expbrdIsValid(&expbrdData[i])) - { - DEBUG_PRINT("Info board %i:\n", i); - expbrdPrintData(&expbrdData[i]); - status = true; - } - } - else - { - DEBUG_PRINT("Reading board nr:%d [FAILED].\n", i); - } - } - - return status; -} - -static bool expbrdIsValid(ExpbrdData *data) -{ - bool status = false; - uint8_t crc; - - if (data->header == EXPBRD_ID) - { - crc = crcSlow(data, sizeof(ExpbrdData) - 1); - if (crc == data->crc) - { - status = true; - } - else - { - DEBUG_PRINT("Wrong CRC:0x%X!=0x%X\n", crc, data->crc); - } - } - else - { - DEBUG_PRINT("Wrong header id: 0x%X\n", data->header); - } - - return status; -} - -static void expbrdPrintData(ExpbrdData *data) -{ - consolePrintf(" -Header:%X\n", data->header); - consolePrintf(" -Pins :0x%X\n", *(unsigned int*)&data->usedPins); - consolePrintf(" -Vid :%X\n", data->vid); - consolePrintf(" -Pid :%X\n", data->pid); - consolePrintf(" -crc :%X\n", data->crc); -} - -static bool expbrdIsPresent(uint8_t vid, uint8_t pid) -{ - int i; - - if (nBoards == 0) - return false; - - for (i = 0; i < EXPBRD_MAX; i++) - { - if (expbrdData[i].vid == vid && expbrdData[i].pid == pid) - { - return true; - } - } - - return false; -} diff --git a/modules/src/system.c b/modules/src/system.c index 4f9bd5e3b7..1dec423809 100644 --- a/modules/src/system.c +++ b/modules/src/system.c @@ -1,6 +1,6 @@ /* - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ @@ -53,10 +53,9 @@ #include "commander.h" #include "console.h" #include "usb.h" -#include "expbrd.h" +#include "deck.h" #include "mem.h" #include "proximity.h" -#include "buzzer.h" /* Private variable */ static bool selftestPassed; @@ -104,21 +103,21 @@ void systemInit(void) adcInit(); ledseqInit(); pmInit(); - + isInit = true; } bool systemTest() { bool pass=isInit; - + #ifdef PLATFORM_CF1 pass &= adcTest(); #endif pass &= ledseqTest(); pass &= pmTest(); pass &= workerTest(); - + return pass; } @@ -127,7 +126,7 @@ bool systemTest() void systemTask(void *arg) { bool pass = true; - + ledInit(); ledSet(CHG_LED, 1); @@ -155,7 +154,7 @@ void systemTask(void *arg) commanderInit(); stabilizerInit(); #ifdef PLATFORM_CF2 - expbrdInit(); + deckInit(); #endif memInit(); @@ -163,10 +162,6 @@ void systemTask(void *arg) proximityInit(); #endif -#ifdef BUZZER_ENABLED - buzzerInit(); -#endif - //Test the modules pass &= systemTest(); pass &= configblockTest(); @@ -174,10 +169,10 @@ void systemTask(void *arg) pass &= commanderTest(); pass &= stabilizerTest(); #ifdef PLATFORM_CF2 - pass &= expbrdTest(); + pass &= deckTest(); #endif pass &= memTest(); - + //Start the firmware if(pass) { @@ -211,9 +206,9 @@ void systemTask(void *arg) } } DEBUG_PRINT("Free heap: %d bytes\n", xPortGetFreeHeapSize()); - + workerLoop(); - + //Should never reach this point! while(1) vTaskDelay(portMAX_DELAY); diff --git a/scripts/F405/linker/sections_FLASH.ld b/scripts/F405/linker/sections_FLASH.ld index 60304236bc..3f3a43a035 100644 --- a/scripts/F405/linker/sections_FLASH.ld +++ b/scripts/F405/linker/sections_FLASH.ld @@ -18,7 +18,7 @@ SECTIONS KEEP(*(.isr_vector)) /* Startup code */ . = ALIGN(4); } >FLASH - + /* for some STRx devices, the beginning of the startup code is stored in the .flashtext section, which goes to FLASH */ .flashtext : { @@ -26,20 +26,20 @@ SECTIONS *(.flashtext) /* Startup code */ . = ALIGN(4); } >FLASH - - + + /* the program code is stored in the .text section, which goes to Flash */ .text : { . = ALIGN(4); - + *(.text) /* remaining code */ *(.text.*) /* remaining code */ *(.rodata) /* read-only data (constants) */ *(.rodata*) *(.glue_7) *(.glue_7t) - + /* Some stuff of CodeSourcery? */ *(.init) *(.fini) @@ -56,8 +56,12 @@ SECTIONS KEEP(*(.log)) KEEP(*(.log.*)) _log_stop = .; - - + _deckDriver_start = .; + KEEP(*(.deckDriver)); + KEEP(*(.deckDriver.*)); + _deckDriver_stop = .; + + . = ALIGN(4); _etext = .; /* This is used by the startup in order to initialize the .data secion */ @@ -69,11 +73,11 @@ SECTIONS . = ALIGN(4); *(.ARM.exidx*) . = ALIGN(4); - + _elibc = .; _sidata = _elibc; } >FLASH - + /* for some LPC devices, there is a FLASH patch to place at a specified address */ .flashpatch : { @@ -81,7 +85,7 @@ SECTIONS KEEP(*(.flashpatch)) /* flashpatch data */ . = ALIGN(4); } >FLASHPATCH - + /* for some LPC devices, there is a FLASH patch to place at a specified address and then there is the rest of the flash */ .endflash : @@ -100,7 +104,7 @@ SECTIONS . = ALIGN(4); /* This is used by the startup in order to initialize the .data secion */ _sdata = . ; - + *(.data) *(.data.*) *(.RAMtext) @@ -109,8 +113,8 @@ SECTIONS /* This is used by the startup in order to initialize the .data secion */ _edata = . ; } >RAM - - + + /* This is the uninitialized data section */ .bss : @@ -118,20 +122,20 @@ SECTIONS . = ALIGN(4); /* This is used by the startup in order to initialize the .bss secion */ _sbss = .; - + *(.bss) *(.bss.*) *(COMMON) - + . = ALIGN(4); /* This is used by the startup in order to initialize the .bss secion */ _ebss = . ; } >RAM - + PROVIDE ( end = _ebss ); PROVIDE ( _end = _ebss ); - - /* This is the user stack section + + /* This is the user stack section This is just to check that there is enough RAM left for the User mode stack It should generate an error if it's full. */ @@ -139,15 +143,15 @@ SECTIONS { . = ALIGN(4); _susrstack = . ; - + . = . + _Minimum_Stack_Size ; - + . = ALIGN(4); _eusrstack = . ; } >RAM - - + + /* this is the FLASH Bank1 */ /* the C or assembly source must explicitly place the code or data there using the "section" attribute */ @@ -157,11 +161,11 @@ SECTIONS *(.b1rodata) /* read-only data (constants) */ *(.b1rodata*) } >FLASHB1 - + /* this is the EXTMEM */ /* the C or assembly source must explicitly place the code or data there using the "section" attribute */ - + /* EXTMEM Bank0 */ .eb0text : { @@ -169,7 +173,7 @@ SECTIONS *(.eb0rodata) /* read-only data (constants) */ *(.eb0rodata*) } >EXTMEMB0 - + /* EXTMEM Bank1 */ .eb1text : { @@ -177,7 +181,7 @@ SECTIONS *(.eb1rodata) /* read-only data (constants) */ *(.eb1rodata*) } >EXTMEMB1 - + /* EXTMEM Bank2 */ .eb2text : { @@ -185,7 +189,7 @@ SECTIONS *(.eb2rodata) /* read-only data (constants) */ *(.eb2rodata*) } >EXTMEMB2 - + /* EXTMEM Bank0 */ .eb3text : { @@ -193,12 +197,12 @@ SECTIONS *(.eb3rodata) /* read-only data (constants) */ *(.eb3rodata*) } >EXTMEMB3 - + __exidx_start = .; __exidx_end = .; - + /* after that it's only debugging information. */ - + /* remove the debugging information from the standard libraries */ /DISCARD/ : { @@ -241,6 +245,3 @@ SECTIONS .debug_typenames 0 : { *(.debug_typenames) } .debug_varnames 0 : { *(.debug_varnames) } } - - - From 0d8058d170b61e5ca8fead934d7d6b82750a53f8 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Mon, 24 Aug 2015 19:36:41 +0200 Subject: [PATCH 08/58] Deck: Added driver-conflict detection --- Makefile | 2 +- deck/core/deck.c | 4 ++-- deck/core/deck_info.c | 20 ++++++++++++++++++++ 3 files changed, 23 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index dc1d112b8c..d9647e8a40 100644 --- a/Makefile +++ b/Makefile @@ -29,7 +29,7 @@ endif ## Flag that can be added to config.mk # CFLAGS += -DUSE_ESKYLINK # Set CRTP link to E-SKY receiver # CFLAGS += -DDEBUG_PRINT_ON_UART # Redirect the console output to the UART - +# CFLAGS += -DDECK_FORCE=bcBuzzer # Load a deck driver that has no OW memory ifeq ($(PLATFORM), CF1) REV ?= F endif diff --git a/deck/core/deck.c b/deck/core/deck.c index 30f2f67de9..3a8c9c5e67 100644 --- a/deck/core/deck.c +++ b/deck/core/deck.c @@ -93,9 +93,9 @@ bool deckTest() if (deck->driver->test) { if (deck->driver->test()) { - DEBUG_PRINT("Deck %i test [OK].", i); + DEBUG_PRINT("Deck %i test [OK].\n", i); } else { - DEBUG_PRINT("Deck %i test [FAIL].", i); + DEBUG_PRINT("Deck %i test [FAIL].\n", i); pass = false; } } diff --git a/deck/core/deck_info.c b/deck/core/deck_info.c index b828cb01bb..1a92b18f14 100644 --- a/deck/core/deck_info.c +++ b/deck/core/deck_info.c @@ -151,6 +151,8 @@ static void enumerateDecks(void) uint8_t nDecks = 0; int i; bool noError = true; + uint32_t usedPeriph = 0; + uint32_t usedGpio = 0; owInit(); @@ -173,6 +175,24 @@ static void enumerateDecks(void) deckInfos[i].driver = findDriver(&deckInfos[i]); printDeckInfo(&deckInfos[i]); + + // Check for Periph and Gpio conflict + if (usedPeriph & deckInfos[i].driver->usedPeriph) { + DEBUG_PRINT("ERROR: Driver Periph usage conflicts with a " + "previously enumerated deck driver. No decks will be " + "initialized!\n"); + noError = false; + } + + if (usedGpio & deckInfos[i].driver->usedGpio) { + DEBUG_PRINT("ERROR: Driver Gpio usage conflicts with a " + "previously enumerated deck driver. No decks will be " + "initialized!\n"); + noError = false; + } + + usedPeriph |= deckInfos[i].driver->usedPeriph; + usedGpio |= deckInfos[i].driver->usedGpio; } else { DEBUG_PRINT("Deck %i has corrupted OW memory. " "No driver will be initialized!\n", i); From 58b66332261c11118cd434fb2a2a1ae414be2182 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 25 Aug 2015 17:34:45 +0200 Subject: [PATCH 09/58] Deck: Change used pins and periph defines Used pin defines could have been confused with the digital API constants --- deck/drivers/src/bigquad.c | 2 +- deck/drivers/src/buzzer.c | 2 +- deck/drivers/src/ledring12.c | 4 +-- deck/interface/deck_core.h | 60 ++++++++++++++++++------------------ 4 files changed, 34 insertions(+), 34 deletions(-) diff --git a/deck/drivers/src/bigquad.c b/deck/drivers/src/bigquad.c index ce81e9babe..27126a3f8c 100644 --- a/deck/drivers/src/bigquad.c +++ b/deck/drivers/src/bigquad.c @@ -65,7 +65,7 @@ static const DeckDriver bigquad_deck = { .pid = 0x03, .name = "bcBigQuad", - .usedPeriph = DECK_TIMER3, + .usedPeriph = DECK_USING_TIMER3, .usedGpio = 0, // FIXME: Edit the used GPIOs .init = bigquadInit, diff --git a/deck/drivers/src/buzzer.c b/deck/drivers/src/buzzer.c index 997ff13e71..30267db6f8 100644 --- a/deck/drivers/src/buzzer.c +++ b/deck/drivers/src/buzzer.c @@ -309,7 +309,7 @@ static const DeckDriver buzzer_deck = { .pid = 0, .name = "bcBuzzer", - .usedGpio = DECK_PA2 | DECK_PA3, + .usedGpio = DECK_USING_PA2 | DECK_USING_PA3, .init = buzzerInit, }; diff --git a/deck/drivers/src/ledring12.c b/deck/drivers/src/ledring12.c index 286d28bc85..fa0a8a15f9 100644 --- a/deck/drivers/src/ledring12.c +++ b/deck/drivers/src/ledring12.c @@ -653,8 +653,8 @@ static const DeckDriver ledring12_deck = { .pid = 0x01, .name = "bcLedRing", - .usedPeriph = DECK_TIMER3, - .usedGpio = DECK_IO_2 | DECK_IO_3, + .usedPeriph = DECK_USING_TIMER3, + .usedGpio = DECK_USING_IO_2 | DECK_USING_IO_3, .init = ledring12Init, }; diff --git a/deck/interface/deck_core.h b/deck/interface/deck_core.h index 6dc3da38e7..5037d451ef 100644 --- a/deck/interface/deck_core.h +++ b/deck/interface/deck_core.h @@ -42,38 +42,38 @@ bool deckTest(void); /***** Driver TOC definitions ******/ /* Used peripherals */ -#define DECK_UART1 (1<<0) -#define DECK_UART2 (1<<1) -#define DECK_SPI (1<<2) -#define DECK_TIMER3 (1<<3) +#define DECK_USING_UART1 (1<<0) +#define DECK_USING_UART2 (1<<1) +#define DECK_USING_SPI (1<<2) +#define DECK_USING_TIMER3 (1<<3) /* Used GPIO */ -#define DECK_PC11 (1<<0) -#define DECK_RX1 (1<<0) -#define DECK_PC10 (1<<1) -#define DECK_TX1 (1<<1) -#define DECK_PB7 (1<<2) -#define DECK_SDA (1<<2) -#define DECK_PB6 (1<<3) -#define DECK_SCL (1<<3) -#define DECK_PB8 (1<<4) -#define DECK_IO_1 (1<<4) -#define DECK_PB5 (1<<5) -#define DECK_IO_2 (1<<5) -#define DECK_PB4 (1<<6) -#define DECK_IO_3 (1<<6) -#define DECK_PC12 (1<<7) -#define DECK_IO_4 (1<<7) -#define DECK_PA2 (1<<8) -#define DECK_TX2 (1<<8) -#define DECK_PA3 (1<<9) -#define DECK_RX2 (1<<9) -#define DECK_PA5 (1<<10) -#define DECK_SCK (1<<10) -#define DECK_PA6 (1<<11) -#define DECK_MISO (1<<11) -#define DECK_PA7 (1<<12) -#define DECK_MOSI (1<<12) +#define DECK_USING_PC11 (1<<0) +#define DECK_USING_RX1 (1<<0) +#define DECK_USING_PC10 (1<<1) +#define DECK_USING_TX1 (1<<1) +#define DECK_USING_PB7 (1<<2) +#define DECK_USING_SDA (1<<2) +#define DECK_USING_PB6 (1<<3) +#define DECK_USING_SCL (1<<3) +#define DECK_USING_PB8 (1<<4) +#define DECK_USING_IO_1 (1<<4) +#define DECK_USING_PB5 (1<<5) +#define DECK_USING_IO_2 (1<<5) +#define DECK_USING_PB4 (1<<6) +#define DECK_USING_IO_3 (1<<6) +#define DECK_USING_PC12 (1<<7) +#define DECK_USING_IO_4 (1<<7) +#define DECK_USING_PA2 (1<<8) +#define DECK_USING_TX2 (1<<8) +#define DECK_USING_PA3 (1<<9) +#define DECK_USING_RX2 (1<<9) +#define DECK_USING_PA5 (1<<10) +#define DECK_USING_SCK (1<<10) +#define DECK_USING_PA6 (1<<11) +#define DECK_USING_MISO (1<<11) +#define DECK_USING_PA7 (1<<12) +#define DECK_USING_MOSI (1<<12) struct deckInfo_s; From 18713ac0b4f462ae8dc57656eca0213ae71149fa Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Wed, 26 Aug 2015 11:10:19 +0200 Subject: [PATCH 10/58] Fix CF1 build --- Makefile | 3 ++- modules/src/system.c | 5 ++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index d9647e8a40..24de36295a 100644 --- a/Makefile +++ b/Makefile @@ -137,7 +137,7 @@ PROJ_OBJ_CF2 += imu_cf2.o pm_f405.o syslink.o radiolink.o ow_syslink.o proximity # Modules PROJ_OBJ += system.o comm.o console.o pid.o crtpservice.o param.o mem.o PROJ_OBJ += commander.o controller.o sensfusion6.o stabilizer.o -PROJ_OBJ += log.o worker.o trigger.o sitaw.o buzzer.o +PROJ_OBJ += log.o worker.o trigger.o sitaw.o PROJ_OBJ_CF2 += platformservice.o # Deck Core @@ -147,6 +147,7 @@ PROJ_OBJ_CF2 += deck.o deck_info.o deck_drivers.o PROJ_OBJ_CF2 += deck_constants.o PROJ_OBJ_CF2 += deck_digital.o PROJ_OBJ_CF2 += deck_analog.o +PROJ_OBJ_CF2 += buzzer.o # Decks PROJ_OBJ_CF2 += bigquad.o diff --git a/modules/src/system.c b/modules/src/system.c index 1dec423809..7f3dd0c527 100644 --- a/modules/src/system.c +++ b/modules/src/system.c @@ -53,10 +53,13 @@ #include "commander.h" #include "console.h" #include "usb.h" -#include "deck.h" #include "mem.h" #include "proximity.h" +#ifdef PLATFORM_CF2 +#include "deck.h" +#endif + /* Private variable */ static bool selftestPassed; static bool canFly; From 5247a38a20468a9e3da6ac257e05280bc9038a3b Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 26 Aug 2015 11:33:40 +0200 Subject: [PATCH 11/58] Added build support for travis and internal build tools --- .gitignore | 3 +++ .travis.yml | 10 ++++++++++ module.json | 4 ++++ tools/build/build.sh | 6 ++++++ 4 files changed, 23 insertions(+) create mode 100644 .travis.yml create mode 100644 module.json create mode 100755 tools/build/build.sh diff --git a/.gitignore b/.gitignore index 4a068513ab..8dbc93a9c0 100644 --- a/.gitignore +++ b/.gitignore @@ -8,3 +8,6 @@ bin/* config.mk cflie.* version.c + +/cf2.* +/cf1.* diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000000..4140317847 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,10 @@ +sudo: required +language: c +services: + - docker +before_install: + - docker pull bitcraze/builder +script: + - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build.sh PLATFORM=CF2 + - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build.sh PLATFORM=CF2 clean + - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build.sh PLATFORM=CF1 diff --git a/module.json b/module.json new file mode 100644 index 0000000000..ce6a47200f --- /dev/null +++ b/module.json @@ -0,0 +1,4 @@ +{ + "artifactType": "firmware", + "buildScript": "tools/build/build.sh" +} diff --git a/tools/build/build.sh b/tools/build/build.sh new file mode 100755 index 0000000000..c4e1b0fcf8 --- /dev/null +++ b/tools/build/build.sh @@ -0,0 +1,6 @@ +#!/usr/bin/env bash +set -e + +scriptDir=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) + +make --directory=${scriptDir}/../.. ${@} From 180cb7520564a1530fcfd8ecd4f10709acada374 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 26 Aug 2015 11:37:11 +0200 Subject: [PATCH 12/58] Added travis build status icon to readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index a41834a7cb..a4ae64f965 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# Crazyflie 1.0/2.0 Firmware +# Crazyflie 1.0/2.0 Firmware [![Build Status](https://api.travis-ci.org/bitcraze/crazyflie-firmware.svg)](https://travis-ci.org/bitcraze/crazyflie-firmware) This project contains the source code for the Crazyflie 1.0/2.0 firmware. From c3f66f060189a40dbd5dd65efe381045ed099256 Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Wed, 2 Sep 2015 14:25:20 +0200 Subject: [PATCH 13/58] Fixed syslink priority issue causing corrupt syslink packages. --- config/FreeRTOSConfig.h | 2 +- config/config.h | 2 +- drivers/src/uart_syslink.c | 8 +++++++- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/config/FreeRTOSConfig.h b/config/FreeRTOSConfig.h index 985fb8a960..6ef9965c10 100644 --- a/config/FreeRTOSConfig.h +++ b/config/FreeRTOSConfig.h @@ -87,7 +87,7 @@ #define configUSE_MALLOC_FAILED_HOOK 1 #define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE -#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 ) +#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 6 ) #define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) /* Set the following definitions to 1 to include the API function, or zero diff --git a/config/config.h b/config/config.h index c07bf3c138..02b3dd16a4 100644 --- a/config/config.h +++ b/config/config.h @@ -74,7 +74,7 @@ #define MEM_TASK_PRI 1 #define PARAM_TASK_PRI 1 #define STABILIZER_TASK_PRI 4 -#define SYSLINK_TASK_PRI 3 +#define SYSLINK_TASK_PRI 5 #define USBLINK_TASK_PRI 3 #define PROXIMITY_TASK_PRI 0 //CF1 diff --git a/drivers/src/uart_syslink.c b/drivers/src/uart_syslink.c index ce0d92e658..bda3a79eaf 100644 --- a/drivers/src/uart_syslink.c +++ b/drivers/src/uart_syslink.c @@ -322,13 +322,19 @@ void uartIsr(void) xHigherPriorityTaskWoken = pdFALSE; xSemaphoreGiveFromISR(waitUntilSendDone, &xHigherPriorityTaskWoken); } + USART_ClearITPendingBit(UART_TYPE, USART_IT_TXE); } - USART_ClearITPendingBit(UART_TYPE, USART_IT_TXE); if (USART_GetITStatus(UART_TYPE, USART_IT_RXNE)) { + // Note: UART interrupt pending bit cleared by reading DR rxDataInterrupt = USART_ReceiveData(UART_TYPE) & 0x00FF; xQueueSendFromISR(uartDataDelivery, &rxDataInterrupt, &xHigherPriorityTaskWoken); } + + if (xHigherPriorityTaskWoken) + { + vPortYieldFromISR(); + } } void uartTxenFlowctrlIsr() From 9a73e8a69634950a5b7d731998d7b786dae58d3a Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Wed, 2 Sep 2015 14:26:14 +0200 Subject: [PATCH 14/58] Lowered amount of deck system prints when DEBUG isn't defined. --- deck/core/deck.c | 10 ++++++++-- deck/core/deck_drivers.c | 11 ++++++++--- deck/core/deck_info.c | 16 +++++++++++----- 3 files changed, 27 insertions(+), 10 deletions(-) diff --git a/deck/core/deck.c b/deck/core/deck.c index 3a8c9c5e67..23dab2ff95 100644 --- a/deck/core/deck.c +++ b/deck/core/deck.c @@ -31,6 +31,12 @@ #include "deck.h" #include "debug.h" +#ifdef DEBUG + #define DECK_CORE_DBG_PRINT(fmt, ...) DEBUG_PRINT(fmt, ## __VA_ARGS__) +#else + #define DECK_CORE_DBG_PRINT(...) +#endif + #ifndef DECK_FORCE #define DECK_FORCE #endif @@ -59,9 +65,9 @@ void deckInit() if (deck->driver->init) { if (deck->driver->name) { - DEBUG_PRINT("Calling INIT from driver %s for deck %i\n", deck->driver->name, i); + DECK_CORE_DBG_PRINT("Calling INIT from driver %s for deck %i\n", deck->driver->name, i); } else { - DEBUG_PRINT("Calling INIT for deck %i\n", i); + DECK_CORE_DBG_PRINT("Calling INIT for deck %i\n", i); } deck->driver->init(deck); diff --git a/deck/core/deck_drivers.c b/deck/core/deck_drivers.c index dfcfb74e16..8ec756baa9 100644 --- a/deck/core/deck_drivers.c +++ b/deck/core/deck_drivers.c @@ -30,9 +30,14 @@ #include #include "deck.h" - #include "debug.h" +#ifdef DEBUG + #define DECK_DRV_DBG_PRINT(fmt, ...) DEBUG_PRINT(fmt, ## __VA_ARGS__) +#else + #define DECK_DRV_DBG_PRINT(...) +#endif + /* Symbols set by the linker script */ extern const struct deck_driver * _deckDriver_start; extern const struct deck_driver * _deckDriver_stop; @@ -54,9 +59,9 @@ static void deckdriversInit() { DEBUG_PRINT("Found %d drivers\n", driversLen); for (i=0; iname) { - DEBUG_PRINT("VID:PID %02x:%02x (%s)\n", drivers[i]->vid, drivers[i]->pid, drivers[i]->name); + DECK_DRV_DBG_PRINT("VID:PID %02x:%02x (%s)\n", drivers[i]->vid, drivers[i]->pid, drivers[i]->name); } else { - DEBUG_PRINT("VID:PID %02x:%02x\n", drivers[i]->vid, drivers[i]->pid); + DECK_DRV_DBG_PRINT("VID:PID %02x:%02x\n", drivers[i]->vid, drivers[i]->pid); } } diff --git a/deck/core/deck_info.c b/deck/core/deck_info.c index 1a92b18f14..b2ae94dbc9 100644 --- a/deck/core/deck_info.c +++ b/deck/core/deck_info.c @@ -36,6 +36,12 @@ #include "crc.h" #include "debug.h" +#ifdef DEBUG + #define DECK_INFO_DBG_PRINT(fmt, ...) DEBUG_PRINT(fmt, ## __VA_ARGS__) +#else + #define DECK_INFO_DBG_PRINT(...) +#endif + static int count = 0; static DeckInfo deckInfos[DECK_MAX_COUNT]; @@ -101,14 +107,14 @@ void printDeckInfo(DeckInfo *info) deckTlvGetString(&info->tlv, DECK_INFO_REVISION, rev, 10); } - DEBUG_PRINT("Deck %02x:%02x %s (Rev. %s)\n", info->vid, info->pid, name, rev); - DEBUG_PRINT("Used pin: %08x\n", (unsigned int)info->usedPins); + DECK_INFO_DBG_PRINT("Deck %02x:%02x %s (Rev. %s)\n", info->vid, info->pid, name, rev); + DECK_INFO_DBG_PRINT("Used pin: %08x\n", (unsigned int)info->usedPins); if (info->driver == &dummyDriver) { DEBUG_PRINT("Warning! No driver found for deck.\n"); } else { - DEBUG_PRINT("Driver implements: [ %s%s]\n", - info->driver->init?"init ":"", info->driver->test?"test ":""); + DECK_INFO_DBG_PRINT("Driver implements: [ %s%s]\n", + info->driver->init?"init ":"", info->driver->test?"test ":""); } } @@ -167,7 +173,7 @@ static void enumerateDecks(void) for (i = 0; i < nDecks; i++) { - DEBUG_PRINT("Enumerating deck %i\n", i); + DECK_INFO_DBG_PRINT("Enumerating deck %i\n", i); if (owRead(i, 0, sizeof(deckInfos[0].raw), (uint8_t *)&deckInfos[i])) { if (infoDecode(&deckInfos[i])) From 6db320a1d3d6fc930d077fd8e09b5274fe06f256 Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Wed, 2 Sep 2015 16:53:07 +0200 Subject: [PATCH 15/58] Made double spinner the default LED-ring effect. --- deck/drivers/src/ledring12.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/deck/drivers/src/ledring12.c b/deck/drivers/src/ledring12.c index fa0a8a15f9..f3271e7759 100644 --- a/deck/drivers/src/ledring12.c +++ b/deck/drivers/src/ledring12.c @@ -80,7 +80,7 @@ typedef void (*Ledring12Effect)(uint8_t buffer[][3], bool reset); #define LINSCALE(domain_low, domain_high, codomain_low, codomain_high, value) ((codomain_high - codomain_low) / (domain_high - domain_low)) * (value - domain_low) + codomain_low #define SET_WHITE(dest, intensity) dest[0] = intensity; dest[1] = intensity; dest[2] = intensity; -static uint32_t effect = 13; +static uint32_t effect = 6; static uint32_t neffect; static uint8_t headlightEnable = 0; static uint8_t black[][3] = {BLACK, BLACK, BLACK, From ee490f2b95295a9a615384443cea24e0b5ed0d25 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 9 Sep 2015 10:44:45 +0200 Subject: [PATCH 16/58] Updated build tool configuration to version 1.0, and renamed build script --- .travis.yml | 6 +++--- module.json | 28 ++++++++++++++++++++++++++-- tools/build/{build.sh => build} | 0 3 files changed, 29 insertions(+), 5 deletions(-) rename tools/build/{build.sh => build} (100%) diff --git a/.travis.yml b/.travis.yml index 4140317847..d2edd18551 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,6 +5,6 @@ services: before_install: - docker pull bitcraze/builder script: - - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build.sh PLATFORM=CF2 - - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build.sh PLATFORM=CF2 clean - - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build.sh PLATFORM=CF1 + - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build PLATFORM=CF2 + - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build PLATFORM=CF2 clean + - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build PLATFORM=CF1 diff --git a/module.json b/module.json index ce6a47200f..07cb754067 100644 --- a/module.json +++ b/module.json @@ -1,4 +1,28 @@ { - "artifactType": "firmware", - "buildScript": "tools/build/build.sh" + "version": "1.0", + "environmentReq": [ + "arm-none-eabi" + ], + "artifacts": { + "file": [ + { + "source": "*.bin", + "insertTag": true, + "destination": "currDir" + }, + { + "source": "*.dfu", + "insertTag": true, + "destination": "currDir" + } + ] + }, + "targets": { + "cf1": [ + "PLATFORM=CF1" + ], + "cf2": [ + "PLATFORM=CF2" + ] + } } diff --git a/tools/build/build.sh b/tools/build/build similarity index 100% rename from tools/build/build.sh rename to tools/build/build From 47716e006bb461ca2c9a090d13492a09317ff0b5 Mon Sep 17 00:00:00 2001 From: Aung Khant Date: Sat, 19 Sep 2015 18:24:54 -0500 Subject: [PATCH 17/58] Update README.md --- README.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index a4ae64f965..f3d260db32 100644 --- a/README.md +++ b/README.md @@ -7,8 +7,10 @@ This project contains the source code for the Crazyflie 1.0/2.0 firmware. You'll need to use either the Crazyflie VM or install some of an ARM toolchain. ### OS X - -> `TODO: Please share!` +```bash +brew tap PX4/homebrew-px4 +brew install gcc-arm-none-eabi +``` ### Debian/Ubuntu From 063abc8cd1a21c72c1cffb258a7a9e93a2b6ac26 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 22 Sep 2015 15:27:03 +0200 Subject: [PATCH 18/58] Added watchdog Closes #71 --- Makefile | 4 +-- drivers/interface/watchdog.h | 44 +++++++++++++++++++++++++++ drivers/src/watchdog.c | 58 ++++++++++++++++++++++++++++++++++++ modules/src/system.c | 17 +++++++++-- tools/build/clean | 6 ++++ 5 files changed, 125 insertions(+), 4 deletions(-) create mode 100644 drivers/interface/watchdog.h create mode 100644 drivers/src/watchdog.c create mode 100755 tools/build/clean diff --git a/Makefile b/Makefile index 24de36295a..6154c37fc0 100644 --- a/Makefile +++ b/Makefile @@ -122,10 +122,10 @@ PROJ_OBJ_CF2 += platform_cf2.o # Drivers PROJ_OBJ += exti.o nvic.o motors.o PROJ_OBJ_CF1 += led_f103.o i2cdev_f103.o i2croutines.o adc_f103.o mpu6050.o -PROJ_OBJ_CF1 += hmc5883l.o ms5611.o nrf24l01.o eeprom.o +PROJ_OBJ_CF1 += hmc5883l.o ms5611.o nrf24l01.o eeprom.o watchdog.o PROJ_OBJ_CF2 += led_f405.o mpu6500.o i2cdev_f405.o ws2812_cf2.o lps25h.o PROJ_OBJ_CF2 += ak8963.o eeprom.o maxsonar.o piezo.o -PROJ_OBJ_CF2 += uart_syslink.o swd.o uart1.o uart2.o +PROJ_OBJ_CF2 += uart_syslink.o swd.o uart1.o uart2.o watchdog.o # USB Files PROJ_OBJ_CF2 += usb_bsp.o usblink.o usbd_desc.o usb.o diff --git a/drivers/interface/watchdog.h b/drivers/interface/watchdog.h new file mode 100644 index 0000000000..1afd5f218c --- /dev/null +++ b/drivers/interface/watchdog.h @@ -0,0 +1,44 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie Firmware + * + * Copyright (C) Bitcraze AB + * + * 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, in version 3. + * + * 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 . + * + * @file watchdog.h - Hardware watchdog + * + */ +#ifndef WATCHDOG_H_ +#define WATCHDOG_H_ + +#include +#include "stm32fxxx.h" + +#define WATCHDOG_CLOCK_FRQ 1024 +#define WATCHDOG_TIMEOUT_MS 100 +#define WATCHDOG_RESET_PERIOD_MS 80 +#define WATCHDOG_TIMEOUT_CYCLES ((WATCHDOG_CLOCK_FRQ * WATCHDOG_TIMEOUT_MS) / 1000) + + +void watchdogInit(void); +bool watchdogNormalStartTest(void); +#define watchdogReset() (IWDG_ReloadCounter()) + +#endif // WATCHDOG_H_ + diff --git a/drivers/src/watchdog.c b/drivers/src/watchdog.c new file mode 100644 index 0000000000..9276a9c186 --- /dev/null +++ b/drivers/src/watchdog.c @@ -0,0 +1,58 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie Firmware + * + * Copyright (C) Bitcraze AB + * + * 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, in version 3. + * + * 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 . + * + * @file watchdog.c - Implementaion of hardware watchdog + * + */ +#define DEBUG_MODULE "SYS" + +#include "debug.h" + +#include "watchdog.h" + + +bool watchdogNormalStartTest(void) +{ + bool wasNormalStart = true; + + if (RCC_GetFlagStatus(RCC_FLAG_IWDGRST)) { + RCC_ClearFlag(); + wasNormalStart = false; + DEBUG_PRINT("The system resumed after watchdog timeout [WARNING]\n"); + } + + return wasNormalStart; +} + + +void watchdogInit(void) +{ + IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable); + + // IWDG counter clock: LSI/32 = 1024Hz + IWDG_SetPrescaler(IWDG_Prescaler_32); + IWDG_SetReload(WATCHDOG_TIMEOUT_CYCLES); + + watchdogReset(); + IWDG_Enable(); +} diff --git a/modules/src/system.c b/modules/src/system.c index 7f3dd0c527..8c5b440d11 100644 --- a/modules/src/system.c +++ b/modules/src/system.c @@ -55,6 +55,7 @@ #include "usb.h" #include "mem.h" #include "proximity.h" +#include "watchdog.h" #ifdef PLATFORM_CF2 #include "deck.h" @@ -175,6 +176,7 @@ void systemTask(void *arg) pass &= deckTest(); #endif pass &= memTest(); + pass &= watchdogNormalStartTest(); //Start the firmware if(pass) @@ -222,6 +224,7 @@ void systemTask(void *arg) void systemStart() { xSemaphoreGive(canStartMutex); + watchdogInit(); } void systemWaitStart(void) @@ -249,12 +252,22 @@ void vApplicationIdleHook( void ) { extern size_t debugPrintTCBInfo(void); static uint32_t timeToPrint = M2T(5000); + static uint32_t tickOfLatestWatchdogReset = M2T(0); - if (xTaskGetTickCount() - timeToPrint > M2T(10000)) + portTickType tickCount = xTaskGetTickCount(); + + if (tickCount - tickOfLatestWatchdogReset > M2T(WATCHDOG_RESET_PERIOD_MS)) { - timeToPrint = xTaskGetTickCount(); + tickOfLatestWatchdogReset = tickCount; + watchdogReset(); + } + + if (tickCount - timeToPrint > M2T(10000)) + { + timeToPrint = tickCount; debugPrintTCBInfo(); } + // Enter sleep mode. Does not work when debugging chip with SWD. // Currently saves about 20mA STM32F405 current consumption (~30%). #ifndef DEBUG diff --git a/tools/build/clean b/tools/build/clean new file mode 100755 index 0000000000..fb96e661fb --- /dev/null +++ b/tools/build/clean @@ -0,0 +1,6 @@ +#!/usr/bin/env bash +set -e + +scriptDir=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) + +make --directory=${scriptDir}/../.. clean ${@} From 68bbd9e864b62869119e6bc8601b48607b4119f1 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Mon, 28 Sep 2015 10:04:13 +0200 Subject: [PATCH 19/58] Disabled watchdog when compiled in Debug mode Related to #71 --- modules/src/system.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/modules/src/system.c b/modules/src/system.c index 8c5b440d11..69bcbfb681 100644 --- a/modules/src/system.c +++ b/modules/src/system.c @@ -224,7 +224,9 @@ void systemTask(void *arg) void systemStart() { xSemaphoreGive(canStartMutex); +#ifndef DEBUG watchdogInit(); +#endif } void systemWaitStart(void) From 61e23e51d7d340392b147586ceb4673ada3570da Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Mon, 28 Sep 2015 10:09:07 +0200 Subject: [PATCH 20/58] Revert "Fixed syslink priority issue causing corrupt syslink packages." This reverts commit c3f66f060189a40dbd5dd65efe381045ed099256. This causes freeze related to i2c. See bug #69. --- config/FreeRTOSConfig.h | 2 +- config/config.h | 2 +- drivers/src/uart_syslink.c | 8 +------- 3 files changed, 3 insertions(+), 9 deletions(-) diff --git a/config/FreeRTOSConfig.h b/config/FreeRTOSConfig.h index 6ef9965c10..985fb8a960 100644 --- a/config/FreeRTOSConfig.h +++ b/config/FreeRTOSConfig.h @@ -87,7 +87,7 @@ #define configUSE_MALLOC_FAILED_HOOK 1 #define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE -#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 6 ) +#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 ) #define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) /* Set the following definitions to 1 to include the API function, or zero diff --git a/config/config.h b/config/config.h index 02b3dd16a4..c07bf3c138 100644 --- a/config/config.h +++ b/config/config.h @@ -74,7 +74,7 @@ #define MEM_TASK_PRI 1 #define PARAM_TASK_PRI 1 #define STABILIZER_TASK_PRI 4 -#define SYSLINK_TASK_PRI 5 +#define SYSLINK_TASK_PRI 3 #define USBLINK_TASK_PRI 3 #define PROXIMITY_TASK_PRI 0 //CF1 diff --git a/drivers/src/uart_syslink.c b/drivers/src/uart_syslink.c index bda3a79eaf..ce0d92e658 100644 --- a/drivers/src/uart_syslink.c +++ b/drivers/src/uart_syslink.c @@ -322,19 +322,13 @@ void uartIsr(void) xHigherPriorityTaskWoken = pdFALSE; xSemaphoreGiveFromISR(waitUntilSendDone, &xHigherPriorityTaskWoken); } - USART_ClearITPendingBit(UART_TYPE, USART_IT_TXE); } + USART_ClearITPendingBit(UART_TYPE, USART_IT_TXE); if (USART_GetITStatus(UART_TYPE, USART_IT_RXNE)) { - // Note: UART interrupt pending bit cleared by reading DR rxDataInterrupt = USART_ReceiveData(UART_TYPE) & 0x00FF; xQueueSendFromISR(uartDataDelivery, &rxDataInterrupt, &xHigherPriorityTaskWoken); } - - if (xHigherPriorityTaskWoken) - { - vPortYieldFromISR(); - } } void uartTxenFlowctrlIsr() From 55cfe57b0a49cd2d53d5a2a4e4c26c51bd1c9363 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Wed, 30 Sep 2015 21:42:19 +0200 Subject: [PATCH 21/58] Corrected YAW rate control drift Yaw control is now always using the angle PID control loop and rate control is simulated by moving the angle setpoint. The setpoint is reset to the current yaw angle when the thrust is 0 (ie. same logic as for reseting all PIDs) --- modules/interface/pid.h | 10 +++++----- modules/src/stabilizer.c | 19 +++++++++++++++---- 2 files changed, 20 insertions(+), 9 deletions(-) diff --git a/modules/interface/pid.h b/modules/interface/pid.h index 9e82fc5461..aebdd20d7f 100644 --- a/modules/interface/pid.h +++ b/modules/interface/pid.h @@ -1,6 +1,6 @@ /** - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ @@ -54,9 +54,9 @@ #define PID_PITCH_KD 0.0 #define PID_PITCH_INTEGRATION_LIMIT 20.0 -#define PID_YAW_KP 0.0 -#define PID_YAW_KI 0.0 -#define PID_YAW_KD 0.0 +#define PID_YAW_KP 10.0 +#define PID_YAW_KI 1.0 +#define PID_YAW_KD 0.35 #define PID_YAW_INTEGRATION_LIMIT 360.0 diff --git a/modules/src/stabilizer.c b/modules/src/stabilizer.c index ef35f19ed9..a60dbb861f 100644 --- a/modules/src/stabilizer.c +++ b/modules/src/stabilizer.c @@ -240,6 +240,7 @@ static void stabilizerTask(void* param) uint32_t attitudeCounter = 0; uint32_t altHoldCounter = 0; uint32_t lastWakeTime; + float yawRateAngle = 0; vTaskSetApplicationTaskTag(0, (void*)TASK_STABILIZER_ID_NBR); @@ -260,6 +261,17 @@ static void stabilizerTask(void* param) commanderGetRPY(&eulerRollDesired, &eulerPitchDesired, &eulerYawDesired); commanderGetRPYType(&rollType, &pitchType, &yawType); + // Rate-controled YAW is moving YAW angle setpoint + if (yawType == RATE) { + yawRateAngle -= eulerYawDesired/500.0; + while (yawRateAngle > 180.0) + yawRateAngle -= 360.0; + while (yawRateAngle < -180.0) + yawRateAngle += 360.0; + + eulerYawDesired = -yawRateAngle; + } + // 250HZ if (++attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER) { @@ -298,10 +310,6 @@ static void stabilizerTask(void* param) { pitchRateDesired = eulerPitchDesired; } - if (yawType == RATE) - { - yawRateDesired = -eulerYawDesired; - } // TODO: Investigate possibility to subtract gyro drift. controllerCorrectRatePID(gyro.x, -gyro.y, gyro.z, @@ -339,6 +347,9 @@ static void stabilizerTask(void* param) { distributePower(0, 0, 0, 0); controllerResetAllPID(); + + // Reset the calculated YAW angle for rate control + yawRateAngle = eulerYawActual; } } } From d14c8902a991a0e7c5f95f562137f03848f35f44 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Wed, 30 Sep 2015 21:56:42 +0200 Subject: [PATCH 22/58] Increase syslink queue to reduce corruption probability Related to bug #69 --- config/config.h | 2 +- hal/src/uart.c | 13 ++++++------- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/config/config.h b/config/config.h index c07bf3c138..5549ceb172 100644 --- a/config/config.h +++ b/config/config.h @@ -52,7 +52,7 @@ #define CONFIG_BLOCK_ADDRESS (2048 * (64-1)) #define MCU_ID_ADDRESS 0x1FFF7A10 #define MCU_FLASH_SIZE_ADDRESS 0x1FFF7A22 - #define FREERTOS_HEAP_SIZE 20000 + #define FREERTOS_HEAP_SIZE 22000 #define FREERTOS_MIN_STACK_SIZE 150 // M4-FPU register setup is bigger so stack needs to be bigger #define FREERTOS_MCU_CLOCK_HZ 168000000 diff --git a/hal/src/uart.c b/hal/src/uart.c index 0545cb1c43..ea69ab2302 100644 --- a/hal/src/uart.c +++ b/hal/src/uart.c @@ -1,6 +1,6 @@ /** - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ @@ -146,11 +146,11 @@ void uartInit(void) configMINIMAL_STACK_SIZE, NULL, /*priority*/2, NULL); packetDelivery = xQueueCreate(2, sizeof(CRTPPacket)); - uartDataDelivery = xQueueCreate(40, sizeof(uint8_t)); + uartDataDelivery = xQueueCreate(1024, sizeof(uint8_t)); #endif //Enable it USART_Cmd(UART_TYPE, ENABLE); - + isInit = true; } @@ -289,7 +289,7 @@ static int uartSendCRTPPacket(CRTPPacket *p) USART_SendData(UART_TYPE, outBuffer[0] & 0xFF); USART_ITConfig(UART_TYPE, USART_IT_TXE, ENABLE); xSemaphoreTake(waitUntilSendDone, portMAX_DELAY); - + return 0; } @@ -333,7 +333,7 @@ void uartSendData(uint32_t size, uint8_t* data) int uartPutchar(int ch) { uartSendData(1, (uint8_t *)&ch); - + return (unsigned char)ch; } @@ -364,4 +364,3 @@ void __attribute__((used)) DMA1_Channel2_IRQHandler(void) uartDmaIsr(); #endif } - From ea16ab8b561b29dd1cf71cae9f2025a8ae366dfa Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Wed, 30 Sep 2015 22:25:40 +0200 Subject: [PATCH 23/58] Added parameter to set the asl reference This parameter is intended to be set according to an external altimeter in the same room. --- modules/src/stabilizer.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/modules/src/stabilizer.c b/modules/src/stabilizer.c index a60dbb861f..af5b9c2eb4 100644 --- a/modules/src/stabilizer.c +++ b/modules/src/stabilizer.c @@ -86,6 +86,7 @@ static float pressure; // pressure from barometer in bar static float asl; // smoothed asl static float aslRaw; // raw asl static float aslLong; // long term asl +static float aslRef; // asl reference (ie. offset) // Altitude hold variables static PidObject altHoldPID; // Used for altitute hold mode. I gets reset when the bat status changes @@ -417,6 +418,8 @@ static void stabilizerAltHoldUpdate(void) lps25hGetData(&pressure, &temperature, &aslRaw); #endif + aslRaw -= aslRef; + asl = asl * aslAlpha + aslRaw * (1 - aslAlpha); aslLong = aslLong * aslAlphaLong + aslRaw * (1 - aslAlphaLong); @@ -706,6 +709,7 @@ LOG_GROUP_STOP(autoTO) PARAM_GROUP_START(altHold) PARAM_ADD(PARAM_FLOAT, aslAlpha, &aslAlpha) PARAM_ADD(PARAM_FLOAT, aslAlphaLong, &aslAlphaLong) +PARAM_ADD(PARAM_FLOAT, aslRef, &aslRef) PARAM_ADD(PARAM_FLOAT, errDeadband, &errDeadband) PARAM_ADD(PARAM_FLOAT, altHoldChangeSens, &altHoldChange_SENS) PARAM_ADD(PARAM_FLOAT, altHoldErrMax, &altHoldErrMax) From aa189d67f447ff84bb5df8e44672b2709b541f84 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Wed, 30 Sep 2015 22:45:52 +0200 Subject: [PATCH 24/58] Fixed miscalculation of log and param crc closes #20 --- modules/src/log.c | 138 ++++++++++++++++++++++---------------------- modules/src/param.c | 2 +- 2 files changed, 70 insertions(+), 70 deletions(-) diff --git a/modules/src/log.c b/modules/src/log.c index 2a1676dbd0..bd726834f7 100644 --- a/modules/src/log.c +++ b/modules/src/log.c @@ -1,6 +1,6 @@ /** - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ @@ -144,30 +144,30 @@ static void logReset(); void logInit(void) { int i; - + if(isInit) return; logs = &_log_start; logsLen = &_log_stop - &_log_start; - logsCrc = crcSlow(logs, logsLen); - + logsCrc = crcSlow(logs, logsLen*sizeof(logs[0])); + // Big lock that protects the log datastructures logLock = xSemaphoreCreateMutex(); for (i=0; i= LOG_MAX_BLOCKS) { LOG_ERROR("Trying to append block id %d that doesn't exist.", id); return ENOENT; } - + block = &logBlocks[i]; - + for (i=0; iLOG_MAX_LEN) { LOG_ERROR("Trying to append a full block. Block id %d.\n", id); return E2BIG; } - + ops = opsMalloc(); - + if(!ops) { LOG_ERROR("No more ops memory free!\n"); return ENOMEM; } - + if (settings[i].id != 255) //TOC variable { varId = variableGetIndex(settings[i].id); - + if (varId<0) { LOG_ERROR("Trying to add variable Id %d that does not exists.", settings[i].id); return ENOENT; } - + ops->variable = logs[varId].address; ops->storageType = logs[varId].type; ops->logType = settings[i].logType&0x0F; - + LOG_DEBUG("Appended variable %d to block %d\n", settings[i].id, id); } else { //Memory variable //TODO: Check that the address is in ram @@ -387,14 +387,14 @@ static int logAppendBlock(int id, struct ops_setting * settings, int len) ops->storageType = (settings[i].logType>>4)&0x0F; ops->logType = settings[i].logType&0x0F; i += 2; - + LOG_DEBUG("Appended var addr 0x%x to block %d\n", (int)ops->variable, id); } blockAppendOps(block, ops); - + LOG_DEBUG(" Now lenght %d\n", blockCalcLength(block)); } - + return 0; } @@ -403,15 +403,15 @@ static int logDeleteBlock(int id) int i; struct log_ops * ops; struct log_ops * opsNext; - + for (i=0; i= LOG_MAX_BLOCKS) { LOG_ERROR("trying to delete block id %d that doesn't exist.", id); return ENOENT; } - + ops = logBlocks[i].ops; while (ops) { @@ -419,13 +419,13 @@ static int logDeleteBlock(int id) opsFree(ops); ops = opsNext; } - + if (logBlocks[i].timer != 0) { xTimerStop(logBlocks[i].timer, portMAX_DELAY); xTimerDelete(logBlocks[i].timer, portMAX_DELAY); logBlocks[i].timer = 0; } - + logBlocks[i].id = BLOCK_ID_FREE; return 0; } @@ -433,17 +433,17 @@ static int logDeleteBlock(int id) static int logStartBlock(int id, unsigned int period) { int i; - + for (i=0; i= LOG_MAX_BLOCKS) { LOG_ERROR("Trying to start block id %d that doesn't exist.", id); return ENOENT; } - + LOG_DEBUG("Starting block %d with period %dms\n", id, period); - + if (period>0) { xTimerChangePeriod(logBlocks[i].timer, M2T(period), 100); @@ -452,24 +452,24 @@ static int logStartBlock(int id, unsigned int period) // single-shoot run workerSchedule(logRunBlock, &logBlocks[i]); } - + return 0; } static int logStopBlock(int id) { int i; - + for (i=0; i= LOG_MAX_BLOCKS) { LOG_ERROR("Trying to stop block id %d that doesn't exist.\n", id); return ENOENT; } - + xTimerStop(logBlocks[i].timer, portMAX_DELAY); - + return 0; } @@ -497,11 +497,11 @@ void logRunBlock(void * arg) struct log_ops *ops = blk->ops; static CRTPPacket pk; unsigned int timestamp; - + xSemaphoreTake(logLock, portMAX_DELAY); timestamp = ((long long)xTaskGetTickCount())/portTICK_RATE_MS; - + pk.header = CRTP_HEADER(CRTP_PORT_LOG, LOG_CH); pk.size = 4; pk.data[0] = blk->id; @@ -542,14 +542,14 @@ void logRunBlock(void * arg) valuei = *(float *)&variable; break; } - + if (ops->logType == LOG_FLOAT || ops->logType == LOG_FP16) { if (ops->storageType == LOG_FLOAT) valuef = *(float *)&variable; else valuef = valuei; - + // Try to append the next item to the packet. If we run out of space, // drop this and subsequent items. if (ops->logType == LOG_FLOAT) @@ -566,10 +566,10 @@ void logRunBlock(void * arg) { if (!appendToPacket(&pk, &valuei, typeLength[ops->logType])) break; } - + ops = ops->next; } - + xSemaphoreGive(logLock); // Check if the connection is still up, oherwise disable @@ -577,7 +577,7 @@ void logRunBlock(void * arg) if (!crtpIsConnected()) { logReset(); - crtpReset(); + crtpReset(); } else { @@ -589,20 +589,20 @@ static int variableGetIndex(int id) { int i; int n=0; - + for (i=0; i=logsLen) return -1; - + return i; } @@ -628,7 +628,7 @@ static int blockCalcLength(struct log_block * block) { struct log_ops * ops; int len = 0; - + for (ops = block->ops; ops; ops = ops->next) len += typeLength[ops->logType]; @@ -638,15 +638,15 @@ static int blockCalcLength(struct log_block * block) void blockAppendOps(struct log_block * block, struct log_ops * ops) { struct log_ops * o; - + ops->next = NULL; - + if (block->ops == NULL) block->ops = ops; else { for (o = block->ops; o->next; o = o->next); - + o->next = ops; } } @@ -654,7 +654,7 @@ void blockAppendOps(struct log_block * block, struct log_ops * ops) static void logReset(void) { int i; - + if (isInit) { //Stop and delete all started log blocks @@ -665,11 +665,11 @@ static void logReset(void) logDeleteBlock(logBlocks[i].id); } } - + //Force free all the log block objects for(i=0; i Date: Thu, 1 Oct 2015 13:34:01 +0200 Subject: [PATCH 25/58] Added support for morse when flashing the LED to indicate that the CF if calibrates. Added compile time flag to enable it. Also added config.mk.example that can be used as a template to easily create a personal build configuration. The mores code is manually merged from Pullrequest #54 by ian-ross Closes #54 --- Makefile | 8 +++----- config.mk.example | 13 +++++++++++++ hal/src/ledseq.c | 28 ++++++++++++++++++++++++++++ 3 files changed, 44 insertions(+), 5 deletions(-) create mode 100644 config.mk.example diff --git a/Makefile b/Makefile index 6154c37fc0..65e9db6027 100644 --- a/Makefile +++ b/Makefile @@ -3,7 +3,8 @@ # This Makefile compiles all the objet file to ./bin/ and the resulting firmware # image in ./cfX.elf and ./cfX.bin -#Put your personal build config in config.mk and DO NOT COMMIT IT! +# Put your personal build config in config.mk and DO NOT COMMIT IT! +# Make a copy of config.mk.example to get you started -include config.mk ######### JTAG and environment configuration ########## @@ -26,10 +27,7 @@ OPENOCD_TARGET ?= target/stm32f4x_stlink.cfg USE_FPU ?= 1 endif -## Flag that can be added to config.mk -# CFLAGS += -DUSE_ESKYLINK # Set CRTP link to E-SKY receiver -# CFLAGS += -DDEBUG_PRINT_ON_UART # Redirect the console output to the UART -# CFLAGS += -DDECK_FORCE=bcBuzzer # Load a deck driver that has no OW memory + ifeq ($(PLATFORM), CF1) REV ?= F endif diff --git a/config.mk.example b/config.mk.example new file mode 100644 index 0000000000..22ef41d3c3 --- /dev/null +++ b/config.mk.example @@ -0,0 +1,13 @@ +## Copy this file to config.mk and modify to get you personal build configuration + +## Set CRTP link to E-SKY receiver +# CFLAGS += -DUSE_ESKYLINK + +## Redirect the console output to the UART +# CFLAGS += -DDEBUG_PRINT_ON_UART + +## Load a deck driver that has no OW memory +# CFLAGS += -DDECK_FORCE=bcBuzzer + +## Use morse when flashing the LED to indicate that the Crazyflie is calibrated +# CFLAGS += -DCALIBRATED_LED_MORSE diff --git a/hal/src/ledseq.c b/hal/src/ledseq.c index 9b513c08e9..9ca1dc451c 100644 --- a/hal/src/ledseq.c +++ b/hal/src/ledseq.c @@ -37,6 +37,14 @@ #include #include "led.h" +#ifdef CALIBRATED_LED_MORSE + #define DOT 100 + #define DASH (3 * DOT) + #define GAP DOT + #define LETTER_GAP (3 * DOT) + #define WORD_GAP (7 * DOT) +#endif // #ifdef CALIBRATED_LED_MORSE + /* Led sequence priority */ static ledseq_t * sequences[] = { seq_testPassed, @@ -64,9 +72,29 @@ ledseq_t seq_armed[] = { }; ledseq_t seq_calibrated[] = { +#ifndef CALIBRATED_LED_MORSE { true, LEDSEQ_WAITMS(50)}, {false, LEDSEQ_WAITMS(450)}, { 0, LEDSEQ_LOOP}, +#else + { true, LEDSEQ_WAITMS(DASH)}, + {false, LEDSEQ_WAITMS(GAP)}, + { true, LEDSEQ_WAITMS(DOT)}, + {false, LEDSEQ_WAITMS(GAP)}, + { true, LEDSEQ_WAITMS(DASH)}, + {false, LEDSEQ_WAITMS(GAP)}, + { true, LEDSEQ_WAITMS(DOT)}, + {false, LEDSEQ_WAITMS(LETTER_GAP)}, + { true, LEDSEQ_WAITMS(DOT)}, + {false, LEDSEQ_WAITMS(GAP)}, + { true, LEDSEQ_WAITMS(DOT)}, + {false, LEDSEQ_WAITMS(GAP)}, + { true, LEDSEQ_WAITMS(DASH)}, + {false, LEDSEQ_WAITMS(GAP)}, + { true, LEDSEQ_WAITMS(DOT)}, + {false, LEDSEQ_WAITMS(WORD_GAP)}, + { 0, LEDSEQ_LOOP}, +#endif // ifndef CALIBRATED_LED_MORSE }; ledseq_t seq_alive[] = { From a1ac8edd0090f8a835e05f33d3a357e3b344ed41 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 1 Oct 2015 18:00:06 +0200 Subject: [PATCH 26/58] Store assert data in RAM and log to console after the watch dog resets the system. Created a new section (.nzds) in the memory map, that is not zeroed by the system at startup. The assert data is stored in this section and read at startup, if the system detects that the we are recovering from a watch dog reset. Closes #73 --- drivers/src/watchdog.c | 1 + scripts/F103/linker/sections_FLASH.ld | 25 +++++++++++++--- scripts/F405/linker/sections_FLASH.ld | 24 +++++++++++++-- utils/interface/cfassert.h | 1 + utils/src/cfassert.c | 42 +++++++++++++++++++++++++++ 5 files changed, 86 insertions(+), 7 deletions(-) diff --git a/drivers/src/watchdog.c b/drivers/src/watchdog.c index 9276a9c186..77f9bfdc6e 100644 --- a/drivers/src/watchdog.c +++ b/drivers/src/watchdog.c @@ -39,6 +39,7 @@ bool watchdogNormalStartTest(void) RCC_ClearFlag(); wasNormalStart = false; DEBUG_PRINT("The system resumed after watchdog timeout [WARNING]\n"); + printAssertSnapshotData(); } return wasNormalStart; diff --git a/scripts/F103/linker/sections_FLASH.ld b/scripts/F103/linker/sections_FLASH.ld index 60304236bc..577bb89d3d 100644 --- a/scripts/F103/linker/sections_FLASH.ld +++ b/scripts/F103/linker/sections_FLASH.ld @@ -112,7 +112,7 @@ SECTIONS - /* This is the uninitialized data section */ + /* This is the uninitialized data section. This memory is zeroed at start up */ .bss : { . = ALIGN(4); @@ -127,9 +127,26 @@ SECTIONS /* This is used by the startup in order to initialize the .bss secion */ _ebss = . ; } >RAM - - PROVIDE ( end = _ebss ); - PROVIDE ( _end = _ebss ); + + + + /* This is uninitialized data that is NOT zeroed at start up. */ + .nzds : + { + . = ALIGN(4); + _snzds = .; + + *(.nzds) + *(.nzds.*) + *(COMMON) + + . = ALIGN(4); + _enzds = . ; + } >RAM + + + PROVIDE ( end = _enzds ); + PROVIDE ( _end = _enzds ); /* This is the user stack section This is just to check that there is enough RAM left for the User mode stack diff --git a/scripts/F405/linker/sections_FLASH.ld b/scripts/F405/linker/sections_FLASH.ld index 3f3a43a035..f4a5ea4281 100644 --- a/scripts/F405/linker/sections_FLASH.ld +++ b/scripts/F405/linker/sections_FLASH.ld @@ -116,7 +116,7 @@ SECTIONS - /* This is the uninitialized data section */ + /* This is the uninitialized data section. This memory is zeroed at start up */ .bss : { . = ALIGN(4); @@ -132,8 +132,26 @@ SECTIONS _ebss = . ; } >RAM - PROVIDE ( end = _ebss ); - PROVIDE ( _end = _ebss ); + + + /* This is uninitialized data that is NOT zeroed at start up. */ + .nzds : + { + . = ALIGN(4); + _snzds = .; + + *(.nzds) + *(.nzds.*) + *(COMMON) + + . = ALIGN(4); + _enzds = . ; + } >RAM + + + PROVIDE ( end = _enzds ); + PROVIDE ( _end = _enzds ); + /* This is the user stack section This is just to check that there is enough RAM left for the User mode stack diff --git a/utils/interface/cfassert.h b/utils/interface/cfassert.h index 8f77d5b851..88ff7ff954 100644 --- a/utils/interface/cfassert.h +++ b/utils/interface/cfassert.h @@ -36,5 +36,6 @@ * Assert handler function */ void assertFail(char *exp, char *file, int line); +void printAssertSnapshotData(); #endif //__CFASSERT_H__ diff --git a/utils/src/cfassert.c b/utils/src/cfassert.c index 131b1683dd..ec3b4e1910 100644 --- a/utils/src/cfassert.c +++ b/utils/src/cfassert.c @@ -24,13 +24,55 @@ * cfassert.c - Assert implementation */ +#define DEBUG_MODULE "SYS" + +#include #include "cfassert.h" #include "led.h" +#include "debug.h" + +#define MAGIC_ASSERT_INDICATOR 0x2f8a001f + +typedef struct SNAPSHOT_DATA { + uint32_t magicNumber; + char* fileName; + int line; +} SNAPSHOT_DATA; + +// The .nzds section is not cleared at startup, data here will survive a +// reset (by the watch dog for instance) +SNAPSHOT_DATA snapshot __attribute__((section(".nzds"))) = { + .magicNumber = 0, + .fileName = "", + .line = 0 +}; + +void storeAssertSnapshotData(char *file, int line); + void assertFail(char *exp, char *file, int line) { + storeAssertSnapshotData(file, line); + ledClearAll(); ledSet(ERR_LED1, 1); ledSet(ERR_LED2, 1); while (1); } + +void storeAssertSnapshotData(char *file, int line) +{ + snapshot.magicNumber = MAGIC_ASSERT_INDICATOR; + snapshot.fileName = file; + snapshot.line = line; +} + +void printAssertSnapshotData() +{ + if (MAGIC_ASSERT_INDICATOR == snapshot.magicNumber) { + DEBUG_PRINT("Assert failed at %s:%d\n", snapshot.fileName, snapshot.line); + } +} + + + From 1c20a241481fb59bb27bebd0c22823f3722ca203 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 2 Oct 2015 16:54:09 +0200 Subject: [PATCH 27/58] #73 Added debug print to indicate that no assert data was found. Intention is to remove ambiguities whether information was lost when transfered to the client or if the reset was not triggered by an assert --- utils/src/cfassert.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/utils/src/cfassert.c b/utils/src/cfassert.c index ec3b4e1910..7b80522525 100644 --- a/utils/src/cfassert.c +++ b/utils/src/cfassert.c @@ -71,6 +71,8 @@ void printAssertSnapshotData() { if (MAGIC_ASSERT_INDICATOR == snapshot.magicNumber) { DEBUG_PRINT("Assert failed at %s:%d\n", snapshot.fileName, snapshot.line); + } else { + DEBUG_PRINT("No assert information found\n"); } } From ab9324e0579101187664a1ef1a62c7e07505a498 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Fri, 2 Oct 2015 17:15:50 +0200 Subject: [PATCH 28/58] #73 Added missing include --- drivers/src/watchdog.c | 1 + 1 file changed, 1 insertion(+) diff --git a/drivers/src/watchdog.c b/drivers/src/watchdog.c index 77f9bfdc6e..2fa0a61f5f 100644 --- a/drivers/src/watchdog.c +++ b/drivers/src/watchdog.c @@ -29,6 +29,7 @@ #include "debug.h" #include "watchdog.h" +#include "cfassert.h" bool watchdogNormalStartTest(void) From 59a166ba41728dba94f465a9c9e81524c056aba7 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sat, 3 Oct 2015 02:27:59 +0200 Subject: [PATCH 29/58] Moved scripts into tools directory --- .gitignore | 2 +- Makefile | 18 ++++---- README.md | 2 +- scripts/clean_version.sh | 11 ----- scripts/update_version.sh | 44 ------------------- .../eclipse}/InterruptsInEclipse | 0 {scripts => tools/make}/F103/jtagkey.cfg | 0 {scripts => tools/make}/F103/linker/COMMON.ld | 0 {scripts => tools/make}/F103/linker/DEF.ld | 0 .../make}/F103/linker/DEF_CLOAD.ld | 0 {scripts => tools/make}/F103/linker/FLASH.ld | 2 +- .../make}/F103/linker/FLASH_CLOAD.ld | 2 +- .../make}/F103/linker/sections_FLASH.ld | 0 {scripts => tools/make}/F103/st_obj.mk | 0 .../make}/F103/stm32f1x-rtos.cfg | 0 {scripts => tools/make}/F103/stm32f1x.cfg | 0 {scripts => tools/make}/F405/linker/COMMON.ld | 0 {scripts => tools/make}/F405/linker/DEF.ld | 0 .../make}/F405/linker/DEF_CLOAD.ld | 0 {scripts => tools/make}/F405/linker/FLASH.ld | 2 +- .../make}/F405/linker/FLASH_CLOAD.ld | 2 +- .../make}/F405/linker/sections_FLASH.ld | 0 {scripts => tools/make}/F405/st_obj.mk | 0 .../make}/F405/stm32f4discovery.cfg | 0 .../make}/F405/stm32f4x_stlink.cfg | 0 .../make/config.mk.example | 0 {scripts => tools/make}/dfu-convert.py | 0 {scripts => tools/make}/print_revision.sh | 0 {scripts => tools/make}/targets.mk | 6 +-- {scripts => tools/make}/versionTemplate.py | 0 30 files changed, 18 insertions(+), 73 deletions(-) delete mode 100644 scripts/clean_version.sh delete mode 100644 scripts/update_version.sh rename {scripts => tools/eclipse}/InterruptsInEclipse (100%) rename {scripts => tools/make}/F103/jtagkey.cfg (100%) rename {scripts => tools/make}/F103/linker/COMMON.ld (100%) rename {scripts => tools/make}/F103/linker/DEF.ld (100%) rename {scripts => tools/make}/F103/linker/DEF_CLOAD.ld (100%) rename {scripts => tools/make}/F103/linker/FLASH.ld (95%) rename {scripts => tools/make}/F103/linker/FLASH_CLOAD.ld (95%) rename {scripts => tools/make}/F103/linker/sections_FLASH.ld (100%) rename {scripts => tools/make}/F103/st_obj.mk (100%) rename {scripts => tools/make}/F103/stm32f1x-rtos.cfg (100%) rename {scripts => tools/make}/F103/stm32f1x.cfg (100%) rename {scripts => tools/make}/F405/linker/COMMON.ld (100%) rename {scripts => tools/make}/F405/linker/DEF.ld (100%) rename {scripts => tools/make}/F405/linker/DEF_CLOAD.ld (100%) rename {scripts => tools/make}/F405/linker/FLASH.ld (95%) rename {scripts => tools/make}/F405/linker/FLASH_CLOAD.ld (95%) rename {scripts => tools/make}/F405/linker/sections_FLASH.ld (100%) rename {scripts => tools/make}/F405/st_obj.mk (100%) rename {scripts => tools/make}/F405/stm32f4discovery.cfg (100%) rename {scripts => tools/make}/F405/stm32f4x_stlink.cfg (100%) rename config.mk.example => tools/make/config.mk.example (100%) rename {scripts => tools/make}/dfu-convert.py (100%) rename {scripts => tools/make}/print_revision.sh (100%) rename {scripts => tools/make}/targets.mk (90%) rename {scripts => tools/make}/versionTemplate.py (100%) diff --git a/.gitignore b/.gitignore index 8dbc93a9c0..1392e607cf 100644 --- a/.gitignore +++ b/.gitignore @@ -5,7 +5,7 @@ bin/* .project .settings/* *~ -config.mk +tools/make/config.mk cflie.* version.c diff --git a/Makefile b/Makefile index 65e9db6027..cbe1d7c731 100644 --- a/Makefile +++ b/Makefile @@ -3,9 +3,9 @@ # This Makefile compiles all the objet file to ./bin/ and the resulting firmware # image in ./cfX.elf and ./cfX.bin -# Put your personal build config in config.mk and DO NOT COMMIT IT! -# Make a copy of config.mk.example to get you started --include config.mk +# Put your personal build config in tools/make/config.mk and DO NOT COMMIT IT! +# Make a copy of tools/make/config.mk.example to get you started +-include tools/make/config.mk ######### JTAG and environment configuration ########## OPENOCD ?= openocd @@ -50,12 +50,12 @@ PORT = $(FREERTOS)/portable/GCC/ARM_CM3 endif ifeq ($(PLATFORM), CF1) -LINKER_DIR = scripts/F103/linker -ST_OBJ_DIR = scripts/F103 +LINKER_DIR = tools/make/F103/linker +ST_OBJ_DIR = tools/make/F103 endif ifeq ($(PLATFORM), CF2) -LINKER_DIR = scripts/F405/linker -ST_OBJ_DIR = scripts/F405 +LINKER_DIR = tools/make/F405/linker +ST_OBJ_DIR = tools/make/F405 endif STLIB = lib @@ -292,7 +292,7 @@ endif print_version: compile ifeq ($(SHELL),/bin/sh) - @./scripts/print_revision.sh + @./tools/make/print_revision.sh endif ifeq ($(PLATFORM), CF1) @echo "Crazyflie Nano (1.0) build!" @@ -338,7 +338,7 @@ openocd: prep: @$(CC) -dD -include scripts/targets.mk +include tools/make/targets.mk #include dependencies -include $(DEPS) diff --git a/README.md b/README.md index f3d260db32..8e017ed68b 100644 --- a/README.md +++ b/README.md @@ -68,7 +68,7 @@ More information can be found on the | + src | Utils source code | + interface | Utils header files. Interface with the other parts of the program + platform | Platform specific files. Not really used yet - + scripts | Misc. scripts for LD, OpenOCD, make, version control, ... + + tools | Misc. scripts for LD, OpenOCD, make, version control, ... | | *** The two following folders contains the unmodified files *** + lib | Libraries | + FreeRTOS | Source FreeRTOS folder. Cleaned up from the useless files diff --git a/scripts/clean_version.sh b/scripts/clean_version.sh deleted file mode 100644 index 294fbbe432..0000000000 --- a/scripts/clean_version.sh +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env bash - -FILE=utils/src/version.c - -#Patch version.c to delete the versions informations -sed -i "s/SLOCAL_REVISION=\".*\";\$/SLOCAL_REVISION=\"\";/" $FILE -sed -i "s/STAG=\".*\";\$/STAG=\"\";/" $FILE -sed -i "s/SREVISION=\".*\";\$/SREVISION=\"\";/" $FILE -sed -i "s/MODIFIED=.*;\$/MODIFIED=1;/" $FILE - -true diff --git a/scripts/update_version.sh b/scripts/update_version.sh deleted file mode 100644 index bc4627a320..0000000000 --- a/scripts/update_version.sh +++ /dev/null @@ -1,44 +0,0 @@ -#!/usr/bin/env bash - -FILE=utils/src/version.c - -MODIFIED=0 - -#Get the relevant informations -if test -d .git ; then - # git - ID=$(git describe --tags --abbrev=12 HEAD) - REV=${ID##*-} - REV=${REV:1} - LOCAL=${ID#*-*-} - TAG=${ID%-$LOCAL} - LOCAL=${LOCAL%-*} - git update-index -q --refresh - if ! test -z "$(git diff-index --name-only HEAD --)" ; then - MODIFIED=1 - fi -else - # mercury - ID=$(hg identify -nit) - - REV=$(echo -n $ID | cut -d' ' -f1) - LOCAL=$(echo -n $ID | cut -d' ' -f2) - TAG=$(echo -n $ID | cut -d' ' -f3) - - #LOCAL=$(hg identify -n) - #REV=$(hg identify -i) - #TAG=$(hg identify -t) - #BRANCH=$(hg identify -b) - - if echo -n $REV | grep +\$>/dev/null; then - MODIFIED=1 - fi -fi - -#Patch version.c -sed -i "s/SLOCAL_REVISION=\".*\";\$/SLOCAL_REVISION=\"$LOCAL\";/" $FILE -sed -i "s/STAG=\".*\";\$/STAG=\"$TAG\";/" $FILE -sed -i "s/SREVISION=\".*\";\$/SREVISION=\"$REV\";/" $FILE -sed -i "s/MODIFIED=.*;\$/MODIFIED=$MODIFIED;/" $FILE - -true diff --git a/scripts/InterruptsInEclipse b/tools/eclipse/InterruptsInEclipse similarity index 100% rename from scripts/InterruptsInEclipse rename to tools/eclipse/InterruptsInEclipse diff --git a/scripts/F103/jtagkey.cfg b/tools/make/F103/jtagkey.cfg similarity index 100% rename from scripts/F103/jtagkey.cfg rename to tools/make/F103/jtagkey.cfg diff --git a/scripts/F103/linker/COMMON.ld b/tools/make/F103/linker/COMMON.ld similarity index 100% rename from scripts/F103/linker/COMMON.ld rename to tools/make/F103/linker/COMMON.ld diff --git a/scripts/F103/linker/DEF.ld b/tools/make/F103/linker/DEF.ld similarity index 100% rename from scripts/F103/linker/DEF.ld rename to tools/make/F103/linker/DEF.ld diff --git a/scripts/F103/linker/DEF_CLOAD.ld b/tools/make/F103/linker/DEF_CLOAD.ld similarity index 100% rename from scripts/F103/linker/DEF_CLOAD.ld rename to tools/make/F103/linker/DEF_CLOAD.ld diff --git a/scripts/F103/linker/FLASH.ld b/tools/make/F103/linker/FLASH.ld similarity index 95% rename from scripts/F103/linker/FLASH.ld rename to tools/make/F103/linker/FLASH.ld index db78308bae..132e3f2ee1 100644 --- a/scripts/F103/linker/FLASH.ld +++ b/tools/make/F103/linker/FLASH.ld @@ -9,7 +9,7 @@ If you need to use your own script please configure your Ride7 project linker op You can take this one as example for wrting your custom linker script You can use, copy and distribute this file freely, but without any waranty.*/ -SEARCH_DIR("./scripts/F103/linker") +SEARCH_DIR("./tools/make/F103/linker") /* include the common STMxxx sub-script */ INCLUDE "COMMON.ld" diff --git a/scripts/F103/linker/FLASH_CLOAD.ld b/tools/make/F103/linker/FLASH_CLOAD.ld similarity index 95% rename from scripts/F103/linker/FLASH_CLOAD.ld rename to tools/make/F103/linker/FLASH_CLOAD.ld index 5a3272d1d3..4858440589 100644 --- a/scripts/F103/linker/FLASH_CLOAD.ld +++ b/tools/make/F103/linker/FLASH_CLOAD.ld @@ -9,7 +9,7 @@ If you need to use your own script please configure your Ride7 project linker op You can take this one as example for wrting your custom linker script You can use, copy and distribute this file freely, but without any waranty.*/ -SEARCH_DIR("./scripts/F103/linker") +SEARCH_DIR("./tools/make/F103/linker") /* include the common STMxxx sub-script */ INCLUDE "COMMON.ld" diff --git a/scripts/F103/linker/sections_FLASH.ld b/tools/make/F103/linker/sections_FLASH.ld similarity index 100% rename from scripts/F103/linker/sections_FLASH.ld rename to tools/make/F103/linker/sections_FLASH.ld diff --git a/scripts/F103/st_obj.mk b/tools/make/F103/st_obj.mk similarity index 100% rename from scripts/F103/st_obj.mk rename to tools/make/F103/st_obj.mk diff --git a/scripts/F103/stm32f1x-rtos.cfg b/tools/make/F103/stm32f1x-rtos.cfg similarity index 100% rename from scripts/F103/stm32f1x-rtos.cfg rename to tools/make/F103/stm32f1x-rtos.cfg diff --git a/scripts/F103/stm32f1x.cfg b/tools/make/F103/stm32f1x.cfg similarity index 100% rename from scripts/F103/stm32f1x.cfg rename to tools/make/F103/stm32f1x.cfg diff --git a/scripts/F405/linker/COMMON.ld b/tools/make/F405/linker/COMMON.ld similarity index 100% rename from scripts/F405/linker/COMMON.ld rename to tools/make/F405/linker/COMMON.ld diff --git a/scripts/F405/linker/DEF.ld b/tools/make/F405/linker/DEF.ld similarity index 100% rename from scripts/F405/linker/DEF.ld rename to tools/make/F405/linker/DEF.ld diff --git a/scripts/F405/linker/DEF_CLOAD.ld b/tools/make/F405/linker/DEF_CLOAD.ld similarity index 100% rename from scripts/F405/linker/DEF_CLOAD.ld rename to tools/make/F405/linker/DEF_CLOAD.ld diff --git a/scripts/F405/linker/FLASH.ld b/tools/make/F405/linker/FLASH.ld similarity index 95% rename from scripts/F405/linker/FLASH.ld rename to tools/make/F405/linker/FLASH.ld index 37b7fcb46f..7dc8434be5 100644 --- a/scripts/F405/linker/FLASH.ld +++ b/tools/make/F405/linker/FLASH.ld @@ -9,7 +9,7 @@ If you need to use your own script please configure your Ride7 project linker op You can take this one as example for wrting your custom linker script You can use, copy and distribute this file freely, but without any waranty.*/ -SEARCH_DIR("./scripts/F405/linker") +SEARCH_DIR("./tools/make/F405/linker") /* include the common STMxxx sub-script */ INCLUDE "COMMON.ld" diff --git a/scripts/F405/linker/FLASH_CLOAD.ld b/tools/make/F405/linker/FLASH_CLOAD.ld similarity index 95% rename from scripts/F405/linker/FLASH_CLOAD.ld rename to tools/make/F405/linker/FLASH_CLOAD.ld index 2a52d4d7d2..2ecaaadd02 100644 --- a/scripts/F405/linker/FLASH_CLOAD.ld +++ b/tools/make/F405/linker/FLASH_CLOAD.ld @@ -9,7 +9,7 @@ If you need to use your own script please configure your Ride7 project linker op You can take this one as example for wrting your custom linker script You can use, copy and distribute this file freely, but without any waranty.*/ -SEARCH_DIR("./scripts/F405/linker") +SEARCH_DIR("./tools/make/F405/linker") /* include the common STMxxx sub-script */ INCLUDE "COMMON.ld" diff --git a/scripts/F405/linker/sections_FLASH.ld b/tools/make/F405/linker/sections_FLASH.ld similarity index 100% rename from scripts/F405/linker/sections_FLASH.ld rename to tools/make/F405/linker/sections_FLASH.ld diff --git a/scripts/F405/st_obj.mk b/tools/make/F405/st_obj.mk similarity index 100% rename from scripts/F405/st_obj.mk rename to tools/make/F405/st_obj.mk diff --git a/scripts/F405/stm32f4discovery.cfg b/tools/make/F405/stm32f4discovery.cfg similarity index 100% rename from scripts/F405/stm32f4discovery.cfg rename to tools/make/F405/stm32f4discovery.cfg diff --git a/scripts/F405/stm32f4x_stlink.cfg b/tools/make/F405/stm32f4x_stlink.cfg similarity index 100% rename from scripts/F405/stm32f4x_stlink.cfg rename to tools/make/F405/stm32f4x_stlink.cfg diff --git a/config.mk.example b/tools/make/config.mk.example similarity index 100% rename from config.mk.example rename to tools/make/config.mk.example diff --git a/scripts/dfu-convert.py b/tools/make/dfu-convert.py similarity index 100% rename from scripts/dfu-convert.py rename to tools/make/dfu-convert.py diff --git a/scripts/print_revision.sh b/tools/make/print_revision.sh similarity index 100% rename from scripts/print_revision.sh rename to tools/make/print_revision.sh diff --git a/scripts/targets.mk b/tools/make/targets.mk similarity index 90% rename from scripts/targets.mk rename to tools/make/targets.mk index 382e8d1d80..4fb89b9764 100644 --- a/scripts/targets.mk +++ b/tools/make/targets.mk @@ -13,7 +13,7 @@ endif target = @$(if $(QUIET), ,echo $($1_COMMAND$(VERBOSE)) ); @$($1_COMMAND) -VTMPL_COMMAND=$(PYTHON2) scripts/versionTemplate.py $< $@ +VTMPL_COMMAND=$(PYTHON2) tools/make/versionTemplate.py $< $@ #$(BIN)/$(lastword $(subst /, ,$@)) VTMPL_COMMAND_SILENT=" VTMPL $@" %.c: %.vtpl @@ -44,7 +44,7 @@ $(PROG).bin: $(PROG).elf @$(if $(QUIET), ,echo $(BIN_COMMAND$(VERBOSE)) ) @$(BIN_COMMAND) -DFU_COMMAND=$(PYTHON2) scripts/dfu-convert.py -b $(LOAD_ADDRESS):$< $@ +DFU_COMMAND=$(PYTHON2) tools/make/dfu-convert.py -b $(LOAD_ADDRESS):$< $@ DFU_COMMAND_SILENT=" DFUse $@" $(PROG).dfu: $(PROG).bin @$(if $(QUIET), ,echo $(DFU_COMMAND$(VERBOSE)) ) @@ -68,7 +68,7 @@ clean: @$(if $(QUIET), ,echo $(CLEAN_COMMAND$(VERBOSE)) ) @$(CLEAN_COMMAND) -MRPROPER_COMMAND=rm -f *~ hal/src/*~ hal/interface/*~ tasks/src/*~ tasks/inc/*~ utils/src/*~ utils/inc/*~ scripts/*~; rm -rf bin/dep +MRPROPER_COMMAND=rm -f *~ hal/src/*~ hal/interface/*~ tasks/src/*~ tasks/inc/*~ utils/src/*~ utils/inc/*~ tools/make/*~; rm -rf bin/dep MRPROPER_COMMAND_SILENT=" MRPROPER" mrproper: clean @$(if $(QUIET), ,echo $(MRPROPER_COMMAND$(VERBOSE)) ) diff --git a/scripts/versionTemplate.py b/tools/make/versionTemplate.py similarity index 100% rename from scripts/versionTemplate.py rename to tools/make/versionTemplate.py From d5eb851c07b04153a9b6411a8a04d928cc645330 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sun, 4 Oct 2015 17:26:56 +0200 Subject: [PATCH 30/58] Use build information from the build tools if available. Closes #74 --- .gitignore | 1 + Makefile | 3 -- tools/make/print_revision.sh | 44 ------------------ tools/make/versionTemplate.py | 86 +++++++++++++++++++++++++++-------- 4 files changed, 67 insertions(+), 67 deletions(-) delete mode 100755 tools/make/print_revision.sh diff --git a/.gitignore b/.gitignore index 1392e607cf..be0f848a05 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ +build_info.json ^bin/dep/all bin/* diff --git a/Makefile b/Makefile index cbe1d7c731..ec2ce0d24d 100644 --- a/Makefile +++ b/Makefile @@ -291,9 +291,6 @@ ifeq ($(SHELL),/bin/sh) endif print_version: compile -ifeq ($(SHELL),/bin/sh) - @./tools/make/print_revision.sh -endif ifeq ($(PLATFORM), CF1) @echo "Crazyflie Nano (1.0) build!" endif diff --git a/tools/make/print_revision.sh b/tools/make/print_revision.sh deleted file mode 100755 index 800d8b1969..0000000000 --- a/tools/make/print_revision.sh +++ /dev/null @@ -1,44 +0,0 @@ -#!/usr/bin/env bash - -MODIFIED=0 - -if test -d .git ; then - # git - ID=$(git describe --tags --abbrev=12 HEAD) - REV=${ID##*-} - REV=${REV:1} - LOCAL=${ID#*-*-} - TAG=${ID%-$LOCAL} - LOCAL=${LOCAL%-*} - git update-index -q --refresh - if ! test -z "$(git diff-index --name-only HEAD --)" ; then - MODIFIED=1 - fi -elif test -d .hg ; then - # mercury - ID=$(hg identify -nit) - - REV=$(echo -n $ID | cut -d' ' -f1) - LOCAL=$(echo -n $ID | cut -d' ' -f2) - TAG=$(echo -n $ID | cut -d' ' -f3) - - #LOCAL=$(hg identify -n) - #REV=$(hg identify -i) - #TAG=$(hg identify -t) - if echo -n $REV | grep +\$>/dev/null; then - MODIFIED=1 - fi -else - LOCAL="Tarball build" - REV=" Build from GIT tree for accurate versioning" - TAG=`pwd | grep -o "20[0-9][0-9]\\.[0-9][0-9]\?\(\\.[0-9][0-9]\?\)\?$"` - MODIFIED=1 -fi - -echo -n Build $LOCAL:$REV \($TAG\) - -if test $MODIFIED -eq 1 ; then - echo -e " \033[1;31mMODIFIED\033[m" -else - echo -e " \033[1;32mCLEAN\033[m" -fi diff --git a/tools/make/versionTemplate.py b/tools/make/versionTemplate.py index 60b2e71cbb..e62503598a 100644 --- a/tools/make/versionTemplate.py +++ b/tools/make/versionTemplate.py @@ -5,38 +5,48 @@ import os from os import path import re +import json -version = { } +version = {} header = """/* This file is automatically generated by {0}! * Do not edit manually, any manual change will be overwritten. */ """ -if len(sys.argv)<3: - print("Usage:") - print(" {0} ".format(sys.argv[0])) - sys.exit(1) +BUILD_TOOL_INFO_FILE_NAME = "build_info.json" + +def extract_information_from_build_tool(): + with open(BUILD_TOOL_INFO_FILE_NAME, 'r') as file: + build_info = json.load(file) -#Get the build repos information -if os.path.isdir(".git"): - # git + version['tag'] = build_info['tag'] + version['revision'] = 'NA' + version['irevision0'] = "0x" + '0' + version['irevision1'] = "0x" + '0' + version['local_revision'] = 'NA' + version['branch'] = 'NA' + version['modified'] = 'false' + + +def extract_information_from_git(): revision = subprocess.check_output(["git", "rev-parse", "HEAD"]).strip() version['revision'] = revision[0:12] version['irevision0'] = "0x" + revision[0:8] version['irevision1'] = "0x" + revision[8:12] - identify = subprocess.check_output(["git", "describe", "--abbrev=12", "--tags", "HEAD"]) + identify = subprocess.check_output( + ["git", "describe", "--abbrev=12", "--tags", "HEAD"]) identify = identify.split('-') if len(identify) > 2: - version['local_revision'] = identify[len(identify)-2] + version['local_revision'] = identify[len(identify) - 2] else: version['local_revision'] = '0' version['tag'] = identify[0] - for x in range(1, len(identify)-2): + for x in range(1, len(identify) - 2): version['tag'] += '-' version['tag'] += identify[x] @@ -45,17 +55,20 @@ if version['local_revision'] != '0': version['tag'] = version['tag'] + '-' + version['local_revision'] - branch = subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"]).strip() + branch = subprocess.check_output( + ["git", "rev-parse", "--abbrev-ref", "HEAD"]).strip() version['branch'] = branch subprocess.call(["git", "update-index", "-q", "--refresh"]) - changes = subprocess.check_output(["git", "diff-index", "--name-only", "HEAD", "--"]).strip() + changes = subprocess.check_output( + ["git", "diff-index", "--name-only", "HEAD", "--"]).strip() if len(changes): version['modified'] = 'true' else: version['modified'] = 'false' -elif os.path.isdir(".hg"): - # Mercurial + + +def extract_information_from_mercurial(): identify = subprocess.check_output(["hg", "identify", "-nitb"]) identify = identify.split() version['revision'] = identify[0] @@ -63,7 +76,7 @@ version['irevision1'] = "0x" + identify[0][8:12] version['local_revision'] = identify[1] version['branch'] = identify[2] - if len(identify)>3: + if len(identify) > 3: version['tag'] = identify[3] else: version['tag'] = "" @@ -73,9 +86,12 @@ version['modified'] = 'true' except Exception: version['modified'] = 'false' -else: + + +def extract_information_from_folder_name(): sourcefolder = path.basename(path.abspath(path.dirname(__file__) + '/..')) - match = re.match(".*(20[0-9][0-9]\\.[0-9][0-9]?(\\.[0-9][0-9]?)?)$", sourcefolder) + match = re.match(".*(20[0-9][0-9]\\.[0-9][0-9]?(\\.[0-9][0-9]?)?)$", + sourcefolder) version['revision'] = 'NA' version['irevision0'] = "0x" + '0' @@ -89,8 +105,36 @@ else: version['tag'] = 'NA' -#Apply information to the file template -infile = open(sys.argv[1], 'r') + +def print_version(): + status = "\033[1;32mCLEAN\033[m" + if (version['modified'] == 'true'): + status = "\033[1;31mMODIFIED\033[m" + + print("Build " + version['local_revision'] + ":" + version['revision'] + + " (" + version['tag'] + ") " + status) + + +if len(sys.argv) < 3: + print("Usage:") + print(" {0} ".format(sys.argv[0])) + sys.exit(1) + +if os.path.isfile(BUILD_TOOL_INFO_FILE_NAME): + print("Extracting version information from build tool info file") + extract_information_from_build_tool() +elif os.path.isdir(".git"): + print("Extracting version information from git") + extract_information_from_git() +elif os.path.isdir(".hg"): + print("Extracting version information from mercurial") + extract_information_from_mercurial() +else: + print("Extracting version information from folder name") + extract_information_from_folder_name() + +# Apply information to the file template +infile = open(sys.argv[1], 'r') outfile = open(sys.argv[2], 'w') outfile.write(header.format(sys.argv[0], sys.argv[1])) @@ -98,3 +142,5 @@ infile.close() outfile.close() + +print_version() From 824e18830825f09cb4eb51f94571a0df403220fe Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Sat, 10 Oct 2015 18:20:08 +0200 Subject: [PATCH 31/58] Moved print-version info at the end of the build --- Makefile | 1 + tools/make/versionTemplate.py | 71 +++++++++++++++++++---------------- 2 files changed, 39 insertions(+), 33 deletions(-) diff --git a/Makefile b/Makefile index ec2ce0d24d..700fc07fbc 100644 --- a/Makefile +++ b/Makefile @@ -296,6 +296,7 @@ ifeq ($(PLATFORM), CF1) endif ifeq ($(PLATFORM), CF2) @echo "Crazyflie 2.0 build!" + @$(PYTHON2) tools/make/versionTemplate.py --print-version endif ifeq ($(CLOAD), 1) @echo "Crazyloader build!" diff --git a/tools/make/versionTemplate.py b/tools/make/versionTemplate.py index e62503598a..64d8c171e0 100644 --- a/tools/make/versionTemplate.py +++ b/tools/make/versionTemplate.py @@ -111,36 +111,41 @@ def print_version(): if (version['modified'] == 'true'): status = "\033[1;31mMODIFIED\033[m" - print("Build " + version['local_revision'] + ":" + version['revision'] + - " (" + version['tag'] + ") " + status) - - -if len(sys.argv) < 3: - print("Usage:") - print(" {0} ".format(sys.argv[0])) - sys.exit(1) - -if os.path.isfile(BUILD_TOOL_INFO_FILE_NAME): - print("Extracting version information from build tool info file") - extract_information_from_build_tool() -elif os.path.isdir(".git"): - print("Extracting version information from git") - extract_information_from_git() -elif os.path.isdir(".hg"): - print("Extracting version information from mercurial") - extract_information_from_mercurial() -else: - print("Extracting version information from folder name") - extract_information_from_folder_name() - -# Apply information to the file template -infile = open(sys.argv[1], 'r') -outfile = open(sys.argv[2], 'w') - -outfile.write(header.format(sys.argv[0], sys.argv[1])) -outfile.write(infile.read().format(**version)) - -infile.close() -outfile.close() - -print_version() + print("Build {local_revision}:{revision} ({tag}) {}".format(status, + **version)) + print("Version extracted from {source}".format(**version)) + +if __name__ == "__main__": + version_source = "" + if os.path.isfile(BUILD_TOOL_INFO_FILE_NAME): + version['source'] = "build tool info file" + extract_information_from_build_tool() + elif os.path.isdir(".git"): + version['source'] = "git" + extract_information_from_git() + elif os.path.isdir(".hg"): + version['source'] = "mercurial" + extract_information_from_mercurial() + else: + version['source'] = "folder name" + extract_information_from_folder_name() + + if len(sys.argv) == 2 and sys.argv[1] == "--print-version": + print_version() + sys.exit(0) + + if len(sys.argv) < 3: + print("Usage:") + print(" {0} ".format(sys.argv[0])) + sys.exit(1) + + + # Apply information to the file template + infile = open(sys.argv[1], 'r') + outfile = open(sys.argv[2], 'w') + + outfile.write(header.format(sys.argv[0], sys.argv[1])) + outfile.write(infile.read().format(**version)) + + infile.close() + outfile.close() From 56d2fc0bcb25d3d0604fb1d636669870b31540fd Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Mon, 12 Oct 2015 22:49:14 +0200 Subject: [PATCH 32/58] Implemented rudimentary SWO FreeRTOS task trace Works in DEBUG build: $ make DEBUG=1 $ make cload $ make trace $ tools/trace/decodeItm.py trace.out ITM 1 0x4c535953 # Task: SYSL ITM 1 0x454c4449 # Task: IDLE ITM 1 0x454c4449 # Task: IDLE ITM 1 0x42415453 # Task: STAB ITM 1 0x454c4449 # Task: IDLE ITM 1 0x454c4449 # Task: IDLE ITM 1 0x42415453 # Task: STAB ... --- Makefile | 5 +- config/FreeRTOSConfig.h | 2 + tools/trace/decodeItm.py | 102 +++++++++++++++++++++++++++++++++++ tools/trace/enable_trace.cfg | 51 ++++++++++++++++++ 4 files changed, 159 insertions(+), 1 deletion(-) create mode 100755 tools/trace/decodeItm.py create mode 100644 tools/trace/enable_trace.cfg diff --git a/Makefile b/Makefile index 700fc07fbc..915b9f65af 100644 --- a/Makefile +++ b/Makefile @@ -296,8 +296,8 @@ ifeq ($(PLATFORM), CF1) endif ifeq ($(PLATFORM), CF2) @echo "Crazyflie 2.0 build!" - @$(PYTHON2) tools/make/versionTemplate.py --print-version endif + @$(PYTHON2) tools/make/versionTemplate.py --print-version ifeq ($(CLOAD), 1) @echo "Crazyloader build!" endif @@ -332,6 +332,9 @@ reset: openocd: $(OPENOCD) -d2 -f $(OPENOCD_INTERFACE) -f $(OPENOCD_TARGET) -c init -c targets +trace: + $(OPENOCD) -d2 -f $(OPENOCD_INTERFACE) -f $(OPENOCD_TARGET) -c init -c targets -f tools/trace/enable_trace.cfg + #Print preprocessor #defines prep: @$(CC) -dD diff --git a/config/FreeRTOSConfig.h b/config/FreeRTOSConfig.h index 985fb8a960..0eea288b8f 100644 --- a/config/FreeRTOSConfig.h +++ b/config/FreeRTOSConfig.h @@ -136,4 +136,6 @@ to exclude the API function. */ debugSendTraceInfo((int)pxCurrentTCB->pxTaskTag); \ } */ +// Send 4 first chatacters of task name to ITM port 1 +#define traceTASK_SWITCHED_IN() *((uint32_t*)0xE0000004) = *((uint32_t*)pxCurrentTCB->pcTaskName); #endif /* FREERTOS_CONFIG_H */ diff --git a/tools/trace/decodeItm.py b/tools/trace/decodeItm.py new file mode 100755 index 0000000000..a391ad0a06 --- /dev/null +++ b/tools/trace/decodeItm.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python3 +# Decodes ITM bit flow and print it on the console +import sys +import struct + + +class EOFException(Exception): + pass + + +def read_u8(file): + data = file.read(1) + if len(data) < 1: + raise EOFException() + return struct.unpack("".format(sys.argv[0])) + sys.exit(1) + +trace = open(sys.argv[1], "rb") + +# Decoding... +ctn = 0 +try: + while True: + b = read_u8(trace) + # sys.stdout.write("0x{:04x} ".format(ctn)) + ctn += 1 + + if b == 0 or b == 0x80: + pass + elif b == 0b01110000: + print("OVF") + elif (b & 0x0f) == 0: + print ("LTS") + while (b & 0x7F) != 0: + b = read_u8(trace) + ctn += 1 + elif (b & 0b00001011) == 0b00001000: + print("EXT") + while (b & 0x7F) != 0: + b = read_u8(trace) + ctn += 1 + elif (b & 0b11011111) == 0b10010100: + print("GTS") + while (b & 0x7F) != 0: + b = read_u8(trace) + ctn += 1 + elif (b & 0x03) != 0: # Source packet + s = b & 0x03 + a = b >> 3 + data = 0 + data_str = "" + info = "" + if s == 1: + data = read_u8(trace) + data_str = "0x{:02x}".format(data) + ctn += 1 + elif s == 2: + data = read_u16(trace) + data_str = "0x{:02x}".format(data) + ctn += 2 + elif s == 3: + data = read_u32(trace) + data_str = "0x{:02x}".format(data) + ctn += 4 + if (b & 0x04) == 0: # ITM/SW + info = "" + if a == 1: + info = "\t# Task: " + struct.pack(" Date: Tue, 13 Oct 2015 13:44:27 +0200 Subject: [PATCH 33/58] Fixed build on Ubuntu arm-none-eabi-gcc --- config/FreeRTOSConfig.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/config/FreeRTOSConfig.h b/config/FreeRTOSConfig.h index 0eea288b8f..1ac7af08ba 100644 --- a/config/FreeRTOSConfig.h +++ b/config/FreeRTOSConfig.h @@ -54,6 +54,8 @@ #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H +#include + #include "config.h" #include "cfassert.h" /*----------------------------------------------------------- From 226cbbe45d690d0db8763b6731c87b8628e98837 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sun, 25 Oct 2015 08:35:45 +0100 Subject: [PATCH 34/58] #69 Refactoring - clean up of task defines --- config/config.h | 45 ++++++++++++++++++++++++++++++------------ drivers/src/adc_f103.c | 4 ++-- hal/src/uart.c | 4 ++-- modules/src/info.c | 5 +++-- modules/src/pidctrl.c | 4 ++-- 5 files changed, 41 insertions(+), 21 deletions(-) diff --git a/config/config.h b/config/config.h index 5549ceb172..36214243d5 100644 --- a/config/config.h +++ b/config/config.h @@ -67,25 +67,37 @@ #endif -//Task priorities. Higher number higher priority +// Task priorities. Higher number higher priority +#define STABILIZER_TASK_PRI 4 +#define ADC_TASK_PRI 3 #define SYSTEM_TASK_PRI 2 -#define CRTP_RXTX_TASK_PRI 2 +#define CRTP_TX_TASK_PRI 2 +#define CRTP_RX_TASK_PRI 2 #define LOG_TASK_PRI 1 #define MEM_TASK_PRI 1 #define PARAM_TASK_PRI 1 -#define STABILIZER_TASK_PRI 4 -#define SYSLINK_TASK_PRI 3 -#define USBLINK_TASK_PRI 3 #define PROXIMITY_TASK_PRI 0 -//CF1 -#define ADC_TASK_PRI 0 #define PM_TASK_PRI 0 -#define NRF24LINK_TASK_PRI 2 -#define ESKYLINK_TASK_PRI 1 -#define CRTP_TX_TASK_PRI 2 -#define CRTP_RX_TASK_PRI 2 -//Task names +#ifdef PLATFORM_CF2 + #define SYSLINK_TASK_PRI 3 + #define USBLINK_TASK_PRI 3 +#endif + +#ifdef PLATFORM_CF1 + #define NRF24LINK_TASK_PRI 2 + #define ESKYLINK_TASK_PRI 1 + #define UART_RX_TASK_PRI 2 +#endif + +// Not compiled +#if 0 + #define INFO_TASK_PRI 2 + #define PID_CTRL_TASK_PRI 2 +#endif + + +// Task names #define SYSTEM_TASK_NAME "SYSTEM" #define ADC_TASK_NAME "ADC" #define PM_TASK_NAME "PWRMGNT" @@ -101,7 +113,11 @@ #define SYSLINK_TASK_NAME "SYSLINK" #define USBLINK_TASK_NAME "USBLINK" #define PROXIMITY_TASK_NAME "PROXIMITY" -//Task stack sizes +#define UART_RX_TASK_NAME "UART-RX" +#define INFO_TASK_NAME "INFO" +#define PID_CTRL_TASK_NAME "PID-CTRL" + +// Task stack sizes #define SYSTEM_TASK_STACKSIZE (2* configMINIMAL_STACK_SIZE) #define ADC_TASK_STACKSIZE configMINIMAL_STACK_SIZE #define PM_TASK_STACKSIZE configMINIMAL_STACK_SIZE @@ -117,6 +133,9 @@ #define SYSLINK_TASK_STACKSIZE configMINIMAL_STACK_SIZE #define USBLINK_TASK_STACKSIZE configMINIMAL_STACK_SIZE #define PROXIMITY_TASK_STACKSIZE configMINIMAL_STACK_SIZE +#define UART_RX_TASK_STACKSIZE configMINIMAL_STACK_SIZE +#define INFO_TASK_STACKSIZE configMINIMAL_STACK_SIZE +#define PID_CTRL_TASK_STACKSIZE configMINIMAL_STACK_SIZE //The radio channel. From 0 to 125 #define RADIO_CHANNEL 80 diff --git a/drivers/src/adc_f103.c b/drivers/src/adc_f103.c index 1ea6eb0b33..2a3f04733b 100644 --- a/drivers/src/adc_f103.c +++ b/drivers/src/adc_f103.c @@ -209,8 +209,8 @@ void adcInit(void) adcQueue = xQueueCreate(1, sizeof(AdcGroup*)); - xTaskCreate(adcTask, (const signed char * const)"ADC", - configMINIMAL_STACK_SIZE, NULL, /*priority*/3, NULL); + xTaskCreate(adcTask, (const signed char * const)ADC_TASK_NAME, + ADC_TASK_STACKSIZE, NULL, ADC_TASK_PRI, NULL); isInit = true; } diff --git a/hal/src/uart.c b/hal/src/uart.c index ea69ab2302..6bb9ade492 100644 --- a/hal/src/uart.c +++ b/hal/src/uart.c @@ -142,8 +142,8 @@ void uartInit(void) vSemaphoreCreateBinary(waitUntilSendDone); - xTaskCreate(uartRxTask, (const signed char * const)"UART-Rx", - configMINIMAL_STACK_SIZE, NULL, /*priority*/2, NULL); + xTaskCreate(uartRxTask, (const signed char * const)UART_RX_TASK_NAME, + UART_RX_TASK_STACKSIZE, NULL, UART_RX_TASK_PRI, NULL); packetDelivery = xQueueCreate(2, sizeof(CRTPPacket)); uartDataDelivery = xQueueCreate(1024, sizeof(uint8_t)); diff --git a/modules/src/info.c b/modules/src/info.c index a87524179e..ca1c56b331 100644 --- a/modules/src/info.c +++ b/modules/src/info.c @@ -23,6 +23,7 @@ * * info.c - Receive information requests and send them back to the client */ + #include #include @@ -65,8 +66,8 @@ void infoTask(void *param); void infoInit() { - xTaskCreate(infoTask, (const signed char * const)"Info", - configMINIMAL_STACK_SIZE, NULL, /*priority*/2, NULL); + xTaskCreate(infoTask, (const signed char * const)INFO_TASK_NAME, + INFO_TASK_STACKSIZE, NULL, INFO_TASK_PRI, NULL); crtpInitTaskQueue(crtpInfo); } diff --git a/modules/src/pidctrl.c b/modules/src/pidctrl.c index 8e2b27f9f3..19904551e7 100644 --- a/modules/src/pidctrl.c +++ b/modules/src/pidctrl.c @@ -40,8 +40,8 @@ void pidCrtlTask(void *param); void pidCtrlInit() { - xTaskCreate(pidCrtlTask, (const signed char * const)"PIDCrtl", - configMINIMAL_STACK_SIZE, NULL, /*priority*/2, NULL); + xTaskCreate(pidCrtlTask, (const signed char * const)PID_CTRL_TASK_NAME, + PID_CTRL_TASK_STACKSIZE, NULL, PID_CTRL_TASK_PRI, NULL); crtpInitTaskQueue(6); } From 2ffcd782b06b3c3143e7bcd6efa4dabf88f7360f Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sun, 25 Oct 2015 08:41:23 +0100 Subject: [PATCH 35/58] #69 Added debug print to assert --- utils/src/cfassert.c | 1 + 1 file changed, 1 insertion(+) diff --git a/utils/src/cfassert.c b/utils/src/cfassert.c index 7b80522525..0438884fea 100644 --- a/utils/src/cfassert.c +++ b/utils/src/cfassert.c @@ -53,6 +53,7 @@ void storeAssertSnapshotData(char *file, int line); void assertFail(char *exp, char *file, int line) { storeAssertSnapshotData(file, line); + DEBUG_PRINT("Assert failed %s:%d\n", file, line); ledClearAll(); ledSet(ERR_LED1, 1); From 297cd602c9046eed2096796ee0176d95190bf4ea Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sun, 25 Oct 2015 10:16:42 +0100 Subject: [PATCH 36/58] #69 Clean up. removed unused imports --- drivers/src/i2cdev_f103.c | 4 ---- drivers/src/i2cdev_f405.c | 3 --- drivers/src/led_f103.c | 1 - drivers/src/motors.c | 1 - drivers/src/uart_syslink.c | 3 --- drivers/src/ws2812_cf2.c | 2 -- hal/src/imu_cf1.c | 1 - hal/src/ledseq.c | 1 - hal/src/usb.c | 1 - modules/src/console.c | 1 - modules/src/controller.c | 1 - modules/src/crtpservice.c | 1 - modules/src/pid.c | 5 ----- modules/src/platformservice.c | 1 - modules/src/sensfusion6.c | 1 - modules/src/stabilizer.c | 2 -- platform/cf1/platform_cf1.c | 11 ----------- platform/cf2/platform_cf2.c | 10 ---------- 18 files changed, 50 deletions(-) diff --git a/drivers/src/i2cdev_f103.c b/drivers/src/i2cdev_f103.c index 6158239e43..2401eeca5f 100644 --- a/drivers/src/i2cdev_f103.c +++ b/drivers/src/i2cdev_f103.c @@ -34,10 +34,6 @@ #include "FreeRTOS.h" #include "semphr.h" -#include "task.h" - -#include "led.h" -#include "ledseq.h" #include "stm32f10x.h" diff --git a/drivers/src/i2cdev_f405.c b/drivers/src/i2cdev_f405.c index 03e8926dee..f3e85498de 100644 --- a/drivers/src/i2cdev_f405.c +++ b/drivers/src/i2cdev_f405.c @@ -37,9 +37,6 @@ #include "semphr.h" #include "task.h" -#include "led.h" -#include "ledseq.h" - #include "debug.h" #include "cpal.h" #include "cpal_i2c.h" diff --git a/drivers/src/led_f103.c b/drivers/src/led_f103.c index d0f95c4456..915b11f090 100644 --- a/drivers/src/led_f103.c +++ b/drivers/src/led_f103.c @@ -32,7 +32,6 @@ /*FreeRtos includes*/ #include "FreeRTOS.h" -#include "task.h" static bool isInit=false; diff --git a/drivers/src/motors.c b/drivers/src/motors.c index 52ff33785f..cf7254531a 100644 --- a/drivers/src/motors.c +++ b/drivers/src/motors.c @@ -35,7 +35,6 @@ #include "pm.h" //FreeRTOS includes -#include "FreeRTOS.h" #include "task.h" static uint16_t motorsBLConvBitsTo16(uint16_t bits); diff --git a/drivers/src/uart_syslink.c b/drivers/src/uart_syslink.c index ce0d92e658..37f65df532 100644 --- a/drivers/src/uart_syslink.c +++ b/drivers/src/uart_syslink.c @@ -40,7 +40,6 @@ #include "cfassert.h" #include "nvicconf.h" #include "config.h" -#include "ledseq.h" #define UART_DATA_TIMEOUT_MS 1000 @@ -337,12 +336,10 @@ void uartTxenFlowctrlIsr() if (GPIO_ReadInputDataBit(UART_TXEN_PORT, UART_TXEN_PIN) == Bit_SET) { uartPauseDma(); - //ledSet(LED_GREEN_R, 1); } else { uartResumeDma(); - //ledSet(LED_GREEN_R, 0); } } diff --git a/drivers/src/ws2812_cf2.c b/drivers/src/ws2812_cf2.c index 806f8c873c..958741f4f8 100644 --- a/drivers/src/ws2812_cf2.c +++ b/drivers/src/ws2812_cf2.c @@ -32,8 +32,6 @@ #include "FreeRTOS.h" #include "semphr.h" -#include "led.h" -#include "ledseq.h" //#define TIM1_CCR1_Address 0x40012C34 // physical memory address of Timer 3 CCR1 register diff --git a/hal/src/imu_cf1.c b/hal/src/imu_cf1.c index 4e7dd8f846..b3da956db1 100644 --- a/hal/src/imu_cf1.c +++ b/hal/src/imu_cf1.c @@ -43,7 +43,6 @@ #include "ledseq.h" #include "uart.h" #include "param.h" -#include "log.h" #define IMU_ENABLE_MAG_HMC5883 #define IMU_ENABLE_PRESSURE_MS5611 diff --git a/hal/src/ledseq.c b/hal/src/ledseq.c index 9ca1dc451c..bbd1efd56f 100644 --- a/hal/src/ledseq.c +++ b/hal/src/ledseq.c @@ -34,7 +34,6 @@ #include "timers.h" #include "semphr.h" -#include #include "led.h" #ifdef CALIBRATED_LED_MORSE diff --git a/hal/src/usb.c b/hal/src/usb.c index fc665e192d..9970c9ebf6 100644 --- a/hal/src/usb.c +++ b/hal/src/usb.c @@ -43,7 +43,6 @@ #include "usb_conf.h" #include "usbd_desc.h" #include "usb_dcd.h" -#include "ledseq.h" #include "crtp.h" diff --git a/modules/src/console.c b/modules/src/console.c index 97d99ce515..dd097ee6e7 100644 --- a/modules/src/console.c +++ b/modules/src/console.c @@ -28,7 +28,6 @@ /*FreeRtos includes*/ #include "FreeRTOS.h" -#include "task.h" #include "semphr.h" #include "crtp.h" diff --git a/modules/src/controller.c b/modules/src/controller.c index b01bea56a4..91d236e4d4 100644 --- a/modules/src/controller.c +++ b/modules/src/controller.c @@ -26,7 +26,6 @@ #include #include "FreeRTOS.h" -#include "task.h" #include "controller.h" #include "pid.h" diff --git a/modules/src/crtpservice.c b/modules/src/crtpservice.c index 8ba6b0ef96..881d242f3d 100644 --- a/modules/src/crtpservice.c +++ b/modules/src/crtpservice.c @@ -29,7 +29,6 @@ /* FreeRtos includes */ #include "FreeRTOS.h" -#include "task.h" #include "crtp.h" #include "crtpservice.h" diff --git a/modules/src/pid.c b/modules/src/pid.c index caca7a0402..476b7f9817 100644 --- a/modules/src/pid.c +++ b/modules/src/pid.c @@ -24,13 +24,8 @@ * * pid.c - implementation of the PID regulator */ -#include -#include "FreeRTOS.h" -#include "task.h" #include "pid.h" -#include "led.h" -#include "motors.h" void pidInit(PidObject* pid, const float desired, const float kp, const float ki, const float kd, const float dt) diff --git a/modules/src/platformservice.c b/modules/src/platformservice.c index 7e24557903..43bf9d4294 100644 --- a/modules/src/platformservice.c +++ b/modules/src/platformservice.c @@ -28,7 +28,6 @@ /* FreeRtos includes */ #include "FreeRTOS.h" -#include "task.h" #include #include diff --git a/modules/src/sensfusion6.c b/modules/src/sensfusion6.c index e0c0ac00aa..f1ae83eb7e 100644 --- a/modules/src/sensfusion6.c +++ b/modules/src/sensfusion6.c @@ -26,7 +26,6 @@ #include #include "sensfusion6.h" -#include "imu.h" #include "param.h" #define M_PI_F ((float) M_PI) diff --git a/modules/src/stabilizer.c b/modules/src/stabilizer.c index af5b9c2eb4..c4e74993f5 100644 --- a/modules/src/stabilizer.c +++ b/modules/src/stabilizer.c @@ -39,9 +39,7 @@ #include "motors.h" #include "log.h" #include "pid.h" -#include "ledseq.h" #include "param.h" -#include "debug.h" #include "sitaw.h" #ifdef PLATFORM_CF1 #include "ms5611.h" diff --git a/platform/cf1/platform_cf1.c b/platform/cf1/platform_cf1.c index ab91070aeb..ab57570252 100644 --- a/platform/cf1/platform_cf1.c +++ b/platform/cf1/platform_cf1.c @@ -24,20 +24,9 @@ * TODO: Add description */ -/* Personal configs */ -#include "FreeRTOSConfig.h" - -/* FreeRtos includes */ -#include "FreeRTOS.h" -#include "task.h" - /* Project includes */ -#include "config.h" #include "nvic.h" -/* ST includes */ -#include "stm32fxxx.h" - // TODO: Implement! int platformInit(void) { diff --git a/platform/cf2/platform_cf2.c b/platform/cf2/platform_cf2.c index ab91070aeb..eeb037e4d4 100644 --- a/platform/cf2/platform_cf2.c +++ b/platform/cf2/platform_cf2.c @@ -25,19 +25,9 @@ */ /* Personal configs */ -#include "FreeRTOSConfig.h" - -/* FreeRtos includes */ -#include "FreeRTOS.h" -#include "task.h" - /* Project includes */ -#include "config.h" #include "nvic.h" -/* ST includes */ -#include "stm32fxxx.h" - // TODO: Implement! int platformInit(void) { From e2cfd1935b3dce65ba617759c4bf4e722d76c476 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 26 Oct 2015 21:57:32 -0700 Subject: [PATCH 37/58] fix to compile using older compiler, fixes Issue 78 https://github.com/bitcraze/crazyflie-firmware/issues/78 --- hal/interface/usb_conf.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/hal/interface/usb_conf.h b/hal/interface/usb_conf.h index e752a30dea..573237f1af 100644 --- a/hal/interface/usb_conf.h +++ b/hal/interface/usb_conf.h @@ -256,6 +256,9 @@ #define __packed __packed #elif defined ( __GNUC__ ) /* GNU Compiler */ // #define __packed __attribute__ ((__packed__)) +#ifndef __packed + #define __packed __attribute__ ((__packed__)) +#endif #elif defined (__TASKING__) /* TASKING Compiler */ #define __packed __unaligned #endif /* __CC_ARM */ From 77fb0a2fa47eaf145d5642d0fa99676256a6e3c3 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 27 Oct 2015 07:03:16 +0100 Subject: [PATCH 38/58] #69 Added builds to travis where "all" compile flags are turned on. This will reduce the risk of corrupting code that is normally not built during refactoring. --- .travis.yml | 13 ++++++++++++- Makefile | 2 ++ hal/src/freeRTOSdebug.c | 4 +++- tools/build/build | 2 +- tools/build/clean | 2 +- 5 files changed, 19 insertions(+), 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index d2edd18551..5ce028ab0f 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,6 +5,17 @@ services: before_install: - docker pull bitcraze/builder script: + # Build CF2 basic - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build PLATFORM=CF2 - - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build PLATFORM=CF2 clean + + # Build CF2 with "all" features enabled + - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build clean + - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build PLATFORM=CF2 DEBUG=1 "EXTRA_CFLAGS=-DCALIBRATED_LED_MORSE -DIMU_TAKE_ACCEL_BIAS -DIMU_MPU6500_DLPF_256HZ -DMADWICK_QUATERNION_IMU" + + # Build CF1 + - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build clean - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build PLATFORM=CF1 + + # Build CF1 with "all" features enabled + - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build clean + - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build PLATFORM=CF1 DEBUG=1 diff --git a/Makefile b/Makefile index 915b9f65af..d27c96c70e 100644 --- a/Makefile +++ b/Makefile @@ -7,6 +7,8 @@ # Make a copy of tools/make/config.mk.example to get you started -include tools/make/config.mk +CFLAGS = $(EXTRA_CFLAGS) + ######### JTAG and environment configuration ########## OPENOCD ?= openocd OPENOCD_INTERFACE ?= interface/stlink-v2.cfg diff --git a/hal/src/freeRTOSdebug.c b/hal/src/freeRTOSdebug.c index efe1f310f2..4ea013593f 100644 --- a/hal/src/freeRTOSdebug.c +++ b/hal/src/freeRTOSdebug.c @@ -34,7 +34,9 @@ #include "nvicconf.h" #include "led.h" -//#include "stm32f10x.h" +#ifdef UART_OUTPUT_TRACE_DATA + #include "uart.h" +#endif uint32_t traceTickCount; diff --git a/tools/build/build b/tools/build/build index c4e1b0fcf8..83fb74cbfa 100755 --- a/tools/build/build +++ b/tools/build/build @@ -3,4 +3,4 @@ set -e scriptDir=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) -make --directory=${scriptDir}/../.. ${@} +make --directory=${scriptDir}/../.. "${@}" diff --git a/tools/build/clean b/tools/build/clean index fb96e661fb..5ea42efe06 100755 --- a/tools/build/clean +++ b/tools/build/clean @@ -3,4 +3,4 @@ set -e scriptDir=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) -make --directory=${scriptDir}/../.. clean ${@} +make --directory=${scriptDir}/../.. clean "${@}" From 51e565d6388bd503c20e3069540e11e189ca4328 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 29 Oct 2015 09:31:23 +0100 Subject: [PATCH 39/58] Closes #72: Fixed freeze when connecting USB When connecting with USB there is a 1 second pause caused by the client (see bitcraze/crazyflie-clients/python#213). During this pause no packets are transfered and this resulted in an interrup storm of the 'tx fifo empty' interrupt. It freezes the Copter and became visible when we enabled the watchdog (see #71). This commit is not a real fix but more a workaround. It makes sure there is always at least one packet to transfer to the PC for every USB frame. This is done in the SOF interrupt, a NULL CRTP packet is sent if nothing else has to be sent. This way the interrupt storm is avoided. --- hal/src/usb.c | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/hal/src/usb.c b/hal/src/usb.c index 9970c9ebf6..8f52d42970 100644 --- a/hal/src/usb.c +++ b/hal/src/usb.c @@ -1,6 +1,6 @@ /** - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ @@ -219,6 +219,7 @@ static uint8_t usbd_cf_DataIn (void *pdev, uint8_t epnum) static uint8_t usbd_cf_SOF (void *pdev) { + char * const nullPacket = "\xff"; portBASE_TYPE xTaskWokenByReceive = pdFALSE; if (xQueueReceiveFromISR(usbDataTx, &outPacket, &xTaskWokenByReceive) == pdTRUE) @@ -227,6 +228,14 @@ static uint8_t usbd_cf_SOF (void *pdev) IN_EP, (uint8_t*)outPacket.data, outPacket.size); + } else { + // If there is not at least one packet transfer per SOF there + // is an interrupt storm on TX fifo empty. + // This is a workaround. + DCD_EP_Tx (pdev, + IN_EP, + (uint8_t*) nullPacket, + 1); } return USBD_OK; From db840bd81948bdcbd3356b8ceb599a3c0f38fdc3 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 29 Oct 2015 10:51:26 +0100 Subject: [PATCH 40/58] #69 Refactoring Moved duplicated code into functions to make it more DRY --- drivers/src/i2cdev_f405.c | 106 +++++++++++++------------------------- 1 file changed, 37 insertions(+), 69 deletions(-) diff --git a/drivers/src/i2cdev_f405.c b/drivers/src/i2cdev_f405.c index f3e85498de..4b5ec186a3 100644 --- a/drivers/src/i2cdev_f405.c +++ b/drivers/src/i2cdev_f405.c @@ -68,10 +68,11 @@ xSemaphoreHandle i2cdevDmaEventI2c3; static bool i2cdevWriteTransfer(I2C_Dev *dev); static bool i2cdevReadTransfer(I2C_Dev *dev); static inline void i2cdevRuffLoopDelay(uint32_t us); -static void i2cdevReleaseSemaphoreI2C1(void); -static void i2cdevReleaseSemaphoreI2C2(void); -static void i2cdevReleaseSemaphoreI2C3(void); +#define SEMAPHORE_TIMEOUT M2T(30) +static void semaphoreGiveFromISR(xSemaphoreHandle semaphore); +static void i2cDevTakeSemaphore(I2C_Dev* dev); +static void i2cDevGiveSemaphoreFromISR(I2C_Dev* dev); int i2cdevInit(I2C_Dev *dev) { @@ -169,18 +170,7 @@ static bool i2cdevReadTransfer(I2C_Dev *dev) if (status == CPAL_PASS) { - if (dev == I2C1_DEV) - { - xSemaphoreTake(i2cdevDmaEventI2c1, M2T(30)); - } - else if (dev == I2C2_DEV) - { - xSemaphoreTake(i2cdevDmaEventI2c2, M2T(30)); - } - else if (dev == I2C3_DEV) - { - xSemaphoreTake(i2cdevDmaEventI2c3, M2T(30)); - } + i2cDevTakeSemaphore(dev); //TODO: Remove spin loop below. It does not work without it at the moment while(dev->CPAL_State != CPAL_STATE_READY && dev->CPAL_State != CPAL_STATE_ERROR); @@ -268,18 +258,7 @@ static bool i2cdevWriteTransfer(I2C_Dev *dev) if (status == CPAL_PASS) { - if (dev == I2C1_DEV) - { - xSemaphoreTake(i2cdevDmaEventI2c1, M2T(30)); - } - else if (dev == I2C2_DEV) - { - xSemaphoreTake(i2cdevDmaEventI2c2, M2T(30)); - } - else if (dev == I2C3_DEV) - { - xSemaphoreTake(i2cdevDmaEventI2c3, M2T(30)); - } + i2cDevTakeSemaphore(dev); //TODO: Remove spin loop below. It does not work without it at the moment while(dev->CPAL_State != CPAL_STATE_READY && dev->CPAL_State != CPAL_STATE_ERROR); @@ -331,11 +310,12 @@ void i2cdevUnlockBus(GPIO_TypeDef* portSCL, GPIO_TypeDef* portSDA, uint16_t pinS GPIO_WAIT_FOR_HIGH(portSDA, pinSDA, 10 * I2CDEV_LOOPS_PER_MS); } -static void i2cdevReleaseSemaphoreI2C1(void) + +static void semaphoreGiveFromISR(xSemaphoreHandle semaphore) { portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(i2cdevDmaEventI2c1, &xHigherPriorityTaskWoken); + xSemaphoreGiveFromISR(semaphore, &xHigherPriorityTaskWoken); if(xHigherPriorityTaskWoken) { @@ -343,27 +323,35 @@ static void i2cdevReleaseSemaphoreI2C1(void) } } -static void i2cdevReleaseSemaphoreI2C2(void) +static void i2cDevTakeSemaphore(I2C_Dev* dev) { - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; - - xSemaphoreGiveFromISR(i2cdevDmaEventI2c2, &xHigherPriorityTaskWoken); - - if(xHigherPriorityTaskWoken) - { - vPortYieldFromISR(); - } + if (dev == I2C1_DEV) + { + xSemaphoreTake(i2cdevDmaEventI2c1, SEMAPHORE_TIMEOUT); + } + else if (dev == I2C2_DEV) + { + xSemaphoreTake(i2cdevDmaEventI2c2, SEMAPHORE_TIMEOUT); + } + else if (dev == I2C3_DEV) + { + xSemaphoreTake(i2cdevDmaEventI2c3, SEMAPHORE_TIMEOUT); + } } -static void i2cdevReleaseSemaphoreI2C3(void) +static void i2cDevGiveSemaphoreFromISR(I2C_Dev* dev) { - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; - - xSemaphoreGiveFromISR(i2cdevDmaEventI2c3, &xHigherPriorityTaskWoken); - - if(xHigherPriorityTaskWoken) + if (dev == I2C1_DEV) { - vPortYieldFromISR(); + semaphoreGiveFromISR(i2cdevDmaEventI2c1); + } + else if (dev == I2C2_DEV) + { + semaphoreGiveFromISR(i2cdevDmaEventI2c2); + } + else if (dev == I2C3_DEV) + { + semaphoreGiveFromISR(i2cdevDmaEventI2c3); } } @@ -387,18 +375,7 @@ void CPAL_I2C_ERR_UserCallback(CPAL_DevTypeDef pDevInstance, uint32_t DeviceErro */ void CPAL_I2C_TXTC_UserCallback(CPAL_InitTypeDef* pDevInitStruct) { - if (pDevInitStruct == I2C1_DEV) - { - i2cdevReleaseSemaphoreI2C1(); - } - else if (pDevInitStruct == I2C2_DEV) - { - i2cdevReleaseSemaphoreI2C2(); - } - else if (pDevInitStruct == I2C3_DEV) - { - i2cdevReleaseSemaphoreI2C3(); - } + i2cDevGiveSemaphoreFromISR(pDevInitStruct); } /** @@ -408,16 +385,7 @@ void CPAL_I2C_TXTC_UserCallback(CPAL_InitTypeDef* pDevInitStruct) */ void CPAL_I2C_RXTC_UserCallback(CPAL_InitTypeDef* pDevInitStruct) { - if (pDevInitStruct == I2C1_DEV) - { - i2cdevReleaseSemaphoreI2C1(); - } - else if (pDevInitStruct == I2C2_DEV) - { - i2cdevReleaseSemaphoreI2C2(); - } - else if (pDevInitStruct == I2C3_DEV) - { - i2cdevReleaseSemaphoreI2C3(); - } + i2cDevGiveSemaphoreFromISR(pDevInitStruct); } + + From ef83b9268d3726a096d29d4828a9bdf0931d936f Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 29 Oct 2015 14:50:08 +0100 Subject: [PATCH 41/58] Revert "Fixed freeze when connecting USB" This reverts commit 51e565d6388bd503c20e3069540e11e189ca4328. --- hal/src/usb.c | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/hal/src/usb.c b/hal/src/usb.c index 8f52d42970..9970c9ebf6 100644 --- a/hal/src/usb.c +++ b/hal/src/usb.c @@ -1,6 +1,6 @@ /** - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ @@ -219,7 +219,6 @@ static uint8_t usbd_cf_DataIn (void *pdev, uint8_t epnum) static uint8_t usbd_cf_SOF (void *pdev) { - char * const nullPacket = "\xff"; portBASE_TYPE xTaskWokenByReceive = pdFALSE; if (xQueueReceiveFromISR(usbDataTx, &outPacket, &xTaskWokenByReceive) == pdTRUE) @@ -228,14 +227,6 @@ static uint8_t usbd_cf_SOF (void *pdev) IN_EP, (uint8_t*)outPacket.data, outPacket.size); - } else { - // If there is not at least one packet transfer per SOF there - // is an interrupt storm on TX fifo empty. - // This is a workaround. - DCD_EP_Tx (pdev, - IN_EP, - (uint8_t*) nullPacket, - 1); } return USBD_OK; From 76bfe95639df966f651182cacb055edc323a7884 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 29 Oct 2015 15:14:46 +0100 Subject: [PATCH 42/58] #69 Release semaphore in error callbacks The semaphore has previously not been released in the error callback. After an error condition the semaphore could be taken and the next call to ic2 read/write would lock the calling task. Also added a timeout callback, and release the semaphore there as well. --- drivers/interface/i2cdev.h | 5 -- drivers/src/i2cdev_f405.c | 83 +++++++++++-------- .../src/cpal_usercallback_template.c | 4 +- utils/interface/cfassert.h | 2 + 4 files changed, 54 insertions(+), 40 deletions(-) diff --git a/drivers/interface/i2cdev.h b/drivers/interface/i2cdev.h index e74bff51c5..afe35086b9 100644 --- a/drivers/interface/i2cdev.h +++ b/drivers/interface/i2cdev.h @@ -31,12 +31,7 @@ #include #include -//#if defined (STM32F40_41xxx) #include "cpal.h" -//#else -//#include "stm32f10x_i2c.h" -//#endif - // Delay is approx 0.2us per loop @64Mhz #define I2CDEV_LOOPS_PER_US 5 diff --git a/drivers/src/i2cdev_f405.c b/drivers/src/i2cdev_f405.c index 4b5ec186a3..b71be660b1 100644 --- a/drivers/src/i2cdev_f405.c +++ b/drivers/src/i2cdev_f405.c @@ -71,8 +71,9 @@ static inline void i2cdevRuffLoopDelay(uint32_t us); #define SEMAPHORE_TIMEOUT M2T(30) static void semaphoreGiveFromISR(xSemaphoreHandle semaphore); -static void i2cDevTakeSemaphore(I2C_Dev* dev); -static void i2cDevGiveSemaphoreFromISR(I2C_Dev* dev); +static void i2cDevTakeSemaphore(CPAL_DevTypeDef CPAL_Dev); +static void i2cDevGiveSemaphore(CPAL_DevTypeDef CPAL_Dev); +static xSemaphoreHandle getSemaphore(CPAL_DevTypeDef CPAL_Dev); int i2cdevInit(I2C_Dev *dev) { @@ -170,7 +171,7 @@ static bool i2cdevReadTransfer(I2C_Dev *dev) if (status == CPAL_PASS) { - i2cDevTakeSemaphore(dev); + i2cDevTakeSemaphore(dev->CPAL_Dev); //TODO: Remove spin loop below. It does not work without it at the moment while(dev->CPAL_State != CPAL_STATE_READY && dev->CPAL_State != CPAL_STATE_ERROR); @@ -258,7 +259,7 @@ static bool i2cdevWriteTransfer(I2C_Dev *dev) if (status == CPAL_PASS) { - i2cDevTakeSemaphore(dev); + i2cDevTakeSemaphore(dev->CPAL_Dev); //TODO: Remove spin loop below. It does not work without it at the moment while(dev->CPAL_State != CPAL_STATE_READY && dev->CPAL_State != CPAL_STATE_ERROR); @@ -323,35 +324,38 @@ static void semaphoreGiveFromISR(xSemaphoreHandle semaphore) } } -static void i2cDevTakeSemaphore(I2C_Dev* dev) -{ - if (dev == I2C1_DEV) - { - xSemaphoreTake(i2cdevDmaEventI2c1, SEMAPHORE_TIMEOUT); - } - else if (dev == I2C2_DEV) - { - xSemaphoreTake(i2cdevDmaEventI2c2, SEMAPHORE_TIMEOUT); - } - else if (dev == I2C3_DEV) - { - xSemaphoreTake(i2cdevDmaEventI2c3, SEMAPHORE_TIMEOUT); - } + +static xSemaphoreHandle getSemaphore(CPAL_DevTypeDef CPAL_Dev) { + xSemaphoreHandle result = NULL; + + if (CPAL_Dev == (I2C1_DEV)->CPAL_Dev) { + result = i2cdevDmaEventI2c1; + } else if (CPAL_Dev == (I2C2_DEV)->CPAL_Dev) { + result = i2cdevDmaEventI2c2; + } else if (CPAL_Dev == (I2C3_DEV)->CPAL_Dev) { + result = i2cdevDmaEventI2c3; + } else { + ASSERT_FAILED(); + } + + return result; } -static void i2cDevGiveSemaphoreFromISR(I2C_Dev* dev) + +static void i2cDevTakeSemaphore(CPAL_DevTypeDef CPAL_Dev) { + xSemaphoreHandle semaphore = getSemaphore(CPAL_Dev); + xSemaphoreTake(semaphore, SEMAPHORE_TIMEOUT); +} + +static void i2cDevGiveSemaphore(CPAL_DevTypeDef CPAL_Dev) { - if (dev == I2C1_DEV) - { - semaphoreGiveFromISR(i2cdevDmaEventI2c1); - } - else if (dev == I2C2_DEV) - { - semaphoreGiveFromISR(i2cdevDmaEventI2c2); - } - else if (dev == I2C3_DEV) - { - semaphoreGiveFromISR(i2cdevDmaEventI2c3); + xSemaphoreHandle semaphore = getSemaphore(CPAL_Dev); + bool isInInterrupt = (SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) != 0; + + if (isInInterrupt) { + semaphoreGiveFromISR(semaphore); + } else { + xSemaphoreGive(semaphore); } } @@ -365,7 +369,20 @@ static void i2cDevGiveSemaphoreFromISR(I2C_Dev* dev) */ void CPAL_I2C_ERR_UserCallback(CPAL_DevTypeDef pDevInstance, uint32_t DeviceError) { - DEBUG_PRINT("Error callback nr: %i\n", (int)DeviceError); + DEBUG_PRINT("I2C error callback dev: %i, err: %i\n", (int)pDevInstance , (int)DeviceError); + i2cDevGiveSemaphore(pDevInstance); +} + +/** + * @brief User callback that manages the Timeout error. + * NOTE: This method is called from both interrupts and tasks! + * @param pDevInitStruct . + * @retval None. + */ +uint32_t CPAL_TIMEOUT_UserCallback(CPAL_InitTypeDef* pDevInitStruct) { + DEBUG_PRINT("I2C timeout callback dev: %i\n", (int)pDevInitStruct->CPAL_Dev); + i2cDevGiveSemaphore(pDevInitStruct->CPAL_Dev); + return CPAL_PASS; } /** @@ -375,7 +392,7 @@ void CPAL_I2C_ERR_UserCallback(CPAL_DevTypeDef pDevInstance, uint32_t DeviceErro */ void CPAL_I2C_TXTC_UserCallback(CPAL_InitTypeDef* pDevInitStruct) { - i2cDevGiveSemaphoreFromISR(pDevInitStruct); + i2cDevGiveSemaphore(pDevInitStruct->CPAL_Dev); } /** @@ -385,7 +402,7 @@ void CPAL_I2C_TXTC_UserCallback(CPAL_InitTypeDef* pDevInitStruct) */ void CPAL_I2C_RXTC_UserCallback(CPAL_InitTypeDef* pDevInitStruct) { - i2cDevGiveSemaphoreFromISR(pDevInitStruct); + i2cDevGiveSemaphore(pDevInitStruct->CPAL_Dev); } diff --git a/lib/STM32_CPAL_Driver/src/cpal_usercallback_template.c b/lib/STM32_CPAL_Driver/src/cpal_usercallback_template.c index 36ce78d757..63a012e429 100644 --- a/lib/STM32_CPAL_Driver/src/cpal_usercallback_template.c +++ b/lib/STM32_CPAL_Driver/src/cpal_usercallback_template.c @@ -50,11 +50,11 @@ * @param pDevInitStruct . * @retval None. */ -uint32_t CPAL_TIMEOUT_UserCallback(CPAL_InitTypeDef* pDevInitStruct) +/*uint32_t CPAL_TIMEOUT_UserCallback(CPAL_InitTypeDef* pDevInitStruct) { return CPAL_PASS; -} +}*/ /*=========== Transfer UserCallback ===========*/ diff --git a/utils/interface/cfassert.h b/utils/interface/cfassert.h index 88ff7ff954..5651a2503a 100644 --- a/utils/interface/cfassert.h +++ b/utils/interface/cfassert.h @@ -32,6 +32,8 @@ #define ASSERT(e) if (e) ; \ else assertFail( #e, __FILE__, __LINE__ ) +#define ASSERT_FAILED() assertFail( "", __FILE__, __LINE__ ) + /** * Assert handler function */ From 16819a296ee060a7a5508ae1a8c29533431d58d2 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sat, 31 Oct 2015 16:13:53 +0100 Subject: [PATCH 43/58] #69 Added queue monitor Added possibility to monitor queues in runtime. To use the functionality, define DEBUG_QUEUE_MONITOR. Queues of interest must be added to the queue monitor with the DEBUG_QUEUE_MONITOR_REGISTER macro. The following data is recorded and output to the console every 10 seconds: * nr of items added to the queue * the peak number of items waiting in the queue * nr of times that items could not be added to the queue --- .travis.yml | 2 +- Makefile | 2 +- config/FreeRTOSConfig.h | 11 ++- drivers/src/uart_syslink.c | 2 + hal/src/radiolink.c | 4 + hal/src/usb.c | 3 + hal/src/usblink.c | 2 + modules/interface/queuemonitor.h | 46 ++++++++++ modules/src/crtp.c | 10 ++- modules/src/queuemonitor.c | 141 +++++++++++++++++++++++++++++++ modules/src/system.c | 5 ++ modules/src/worker.c | 2 + tools/make/config.mk.example | 3 + 13 files changed, 227 insertions(+), 6 deletions(-) create mode 100644 modules/interface/queuemonitor.h create mode 100644 modules/src/queuemonitor.c diff --git a/.travis.yml b/.travis.yml index 5ce028ab0f..5e3d71a687 100644 --- a/.travis.yml +++ b/.travis.yml @@ -10,7 +10,7 @@ script: # Build CF2 with "all" features enabled - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build clean - - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build PLATFORM=CF2 DEBUG=1 "EXTRA_CFLAGS=-DCALIBRATED_LED_MORSE -DIMU_TAKE_ACCEL_BIAS -DIMU_MPU6500_DLPF_256HZ -DMADWICK_QUATERNION_IMU" + - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build PLATFORM=CF2 DEBUG=1 "EXTRA_CFLAGS=-DCALIBRATED_LED_MORSE -DIMU_TAKE_ACCEL_BIAS -DIMU_MPU6500_DLPF_256HZ -DMADWICK_QUATERNION_IMU -DDEBUG_QUEUE_MONITOR" # Build CF1 - docker run --rm -v ${PWD}:/module bitcraze/builder ./tools/build/build clean diff --git a/Makefile b/Makefile index d27c96c70e..5f8a02a0ba 100644 --- a/Makefile +++ b/Makefile @@ -137,7 +137,7 @@ PROJ_OBJ_CF2 += imu_cf2.o pm_f405.o syslink.o radiolink.o ow_syslink.o proximity # Modules PROJ_OBJ += system.o comm.o console.o pid.o crtpservice.o param.o mem.o PROJ_OBJ += commander.o controller.o sensfusion6.o stabilizer.o -PROJ_OBJ += log.o worker.o trigger.o sitaw.o +PROJ_OBJ += log.o worker.o trigger.o sitaw.o queuemonitor.o PROJ_OBJ_CF2 += platformservice.o # Deck Core diff --git a/config/FreeRTOSConfig.h b/config/FreeRTOSConfig.h index 1ac7af08ba..a2a42b3028 100644 --- a/config/FreeRTOSConfig.h +++ b/config/FreeRTOSConfig.h @@ -78,7 +78,6 @@ #define configMINIMAL_STACK_SIZE ( ( unsigned short ) FREERTOS_MIN_STACK_SIZE ) #define configTOTAL_HEAP_SIZE ( ( size_t ) ( FREERTOS_HEAP_SIZE ) ) #define configMAX_TASK_NAME_LEN ( 10 ) -#define configUSE_TRACE_FACILITY 0 #define configUSE_16_BIT_TICKS 0 #define configIDLE_SHOULD_YIELD 0 #define configUSE_CO_ROUTINES 0 @@ -140,4 +139,14 @@ to exclude the API function. */ */ // Send 4 first chatacters of task name to ITM port 1 #define traceTASK_SWITCHED_IN() *((uint32_t*)0xE0000004) = *((uint32_t*)pxCurrentTCB->pcTaskName); + +// Queue monitoring +#ifdef DEBUG_QUEUE_MONITOR + #define configUSE_TRACE_FACILITY 1 + #define traceQUEUE_SEND(xQueue) qm_traceQUEUE_SEND(xQueue) + void qm_traceQUEUE_SEND(void* xQueue); + #define traceQUEUE_SEND_FAILED(xQueue) qm_traceQUEUE_SEND_FAILED(xQueue) + void qm_traceQUEUE_SEND_FAILED(void* xQueue); +#endif // DEBUG_QUEUE_MONITOR + #endif /* FREERTOS_CONFIG_H */ diff --git a/drivers/src/uart_syslink.c b/drivers/src/uart_syslink.c index 37f65df532..5948ec76e0 100644 --- a/drivers/src/uart_syslink.c +++ b/drivers/src/uart_syslink.c @@ -40,6 +40,7 @@ #include "cfassert.h" #include "nvicconf.h" #include "config.h" +#include "queuemonitor.h" #define UART_DATA_TIMEOUT_MS 1000 @@ -154,6 +155,7 @@ void uartInit(void) vSemaphoreCreateBinary(waitUntilSendDone); uartDataDelivery = xQueueCreate(40, sizeof(uint8_t)); + DEBUG_QUEUE_MONITOR_REGISTER(uartDataDelivery); USART_ITConfig(UART_TYPE, USART_IT_RXNE, ENABLE); diff --git a/hal/src/radiolink.c b/hal/src/radiolink.c index 957ffea37f..3d525382f6 100644 --- a/hal/src/radiolink.c +++ b/hal/src/radiolink.c @@ -41,6 +41,7 @@ #include "log.h" #include "led.h" #include "ledseq.h" +#include "queuemonitor.h" #define RADIOLINK_TX_QUEUE_SIZE (1) @@ -69,7 +70,10 @@ void radiolinkInit(void) return; txQueue = xQueueCreate(RADIOLINK_TX_QUEUE_SIZE, sizeof(SyslinkPacket)); + DEBUG_QUEUE_MONITOR_REGISTER(txQueue); crtpPacketDelivery = xQueueCreate(5, sizeof(CRTPPacket)); + DEBUG_QUEUE_MONITOR_REGISTER(crtpPacketDelivery); + ASSERT(crtpPacketDelivery); diff --git a/hal/src/usb.c b/hal/src/usb.c index 9970c9ebf6..e22a4a70c5 100644 --- a/hal/src/usb.c +++ b/hal/src/usb.c @@ -33,6 +33,7 @@ #include "task.h" #include "semphr.h" #include "queue.h" +#include "queuemonitor.h" #include "config.h" #include "usblink.h" @@ -351,7 +352,9 @@ void usbInit(void) // This should probably be reduced to a CRTP packet size usbDataRx = xQueueCreate(5, sizeof(USBPacket)); /* Buffer USB packets (max 64 bytes) */ + DEBUG_QUEUE_MONITOR_REGISTER(usbDataRx); usbDataTx = xQueueCreate(1, sizeof(USBPacket)); /* Buffer USB packets (max 64 bytes) */ + DEBUG_QUEUE_MONITOR_REGISTER(usbDataTx); isInit = true; } diff --git a/hal/src/usblink.c b/hal/src/usblink.c index fb5dd72a34..770fa2b95b 100644 --- a/hal/src/usblink.c +++ b/hal/src/usblink.c @@ -37,6 +37,7 @@ #include "FreeRTOS.h" #include "task.h" #include "queue.h" +#include "queuemonitor.h" #include "semphr.h" #include "usb.h" @@ -126,6 +127,7 @@ void usblinkInit() usbInit(); crtpPacketDelivery = xQueueCreate(5, sizeof(CRTPPacket)); + DEBUG_QUEUE_MONITOR_REGISTER(crtpPacketDelivery); xTaskCreate(usblinkTask, (const signed char * const)USBLINK_TASK_NAME, USBLINK_TASK_STACKSIZE, NULL, USBLINK_TASK_PRI, NULL); diff --git a/modules/interface/queuemonitor.h b/modules/interface/queuemonitor.h new file mode 100644 index 0000000000..5018e53d8c --- /dev/null +++ b/modules/interface/queuemonitor.h @@ -0,0 +1,46 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2012 BitCraze AB + * + * 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, in version 3. + * + * 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 . + * + * log.h - Dynamic log system + */ + +#ifndef __QUEUE_MONITOR_H__ +#define __QUEUE_MONITOR_H__ + + +#include "FreeRTOS.h" + +#ifdef DEBUG_QUEUE_MONITOR + #include "queue.h" + + void queueMonitorInit(); + #define DEBUG_QUEUE_MONITOR_REGISTER(queue) qmRegisterQueue(queue, __FILE__, #queue) + + void qm_traceQUEUE_SEND(void* xQueue); + void qm_traceQUEUE_SEND_FAILED(void* xQueue); + void qmRegisterQueue(xQueueHandle* xQueue, char* fileName, char* queueName); +#else + #define DEBUG_QUEUE_MONITOR_REGISTER(queue) +#endif // DEBUG_QUEUE_MONITOR + +#endif // __QUEUE_MONITOR_H__ \ No newline at end of file diff --git a/modules/src/crtp.c b/modules/src/crtp.c index 4565a1c31b..5a6f51bf06 100644 --- a/modules/src/crtp.c +++ b/modules/src/crtp.c @@ -38,6 +38,7 @@ #include "crtp.h" #include "info.h" #include "cfassert.h" +#include "queuemonitor.h" static bool isInit; @@ -69,7 +70,9 @@ void crtpInit(void) return; txQueue = xQueueCreate(CRTP_TX_QUEUE_SIZE, sizeof(CRTPPacket)); + DEBUG_QUEUE_MONITOR_REGISTER(txQueue); rxQueue = xQueueCreate(CRTP_RX_QUEUE_SIZE, sizeof(CRTPPacket)); + DEBUG_QUEUE_MONITOR_REGISTER(rxQueue); xTaskCreate(crtpTxTask, (const signed char * const)CRTP_TX_TASK_NAME, CRTP_TX_TASK_STACKSIZE, NULL, CRTP_TX_TASK_PRI, NULL); @@ -92,6 +95,7 @@ void crtpInitTaskQueue(CRTPPort portId) ASSERT(queues[portId] == NULL); queues[portId] = xQueueCreate(1, sizeof(CRTPPacket)); + DEBUG_QUEUE_MONITOR_REGISTER(queues[portId]); } int crtpReceivePacket(CRTPPort portId, CRTPPacket *p) @@ -180,7 +184,7 @@ void crtpRegisterPortCB(int port, CrtpCallback cb) int crtpSendPacket(CRTPPacket *p) { ASSERT(p); - ASSERT(p->size <= CRTP_MAX_DATA_SIZE); + ASSERT(p->size <= CRTP_MAX_DATA_SIZE); return xQueueSend(txQueue, p, 0); } @@ -188,13 +192,13 @@ int crtpSendPacket(CRTPPacket *p) int crtpSendPacketBlock(CRTPPacket *p) { ASSERT(p); - ASSERT(p->size <= CRTP_MAX_DATA_SIZE); + ASSERT(p->size <= CRTP_MAX_DATA_SIZE); return xQueueSend(txQueue, p, portMAX_DELAY); } int crtpReset(void) -{ +{ xQueueReset(txQueue); if (link->reset) { link->reset(); diff --git a/modules/src/queuemonitor.c b/modules/src/queuemonitor.c new file mode 100644 index 0000000000..32d46cbbf4 --- /dev/null +++ b/modules/src/queuemonitor.c @@ -0,0 +1,141 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2011-2012 Bitcraze AB + * + * 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, in version 3. + * + * 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 . + * + * + * queuemonitor.c - Monitoring functionality for queues + */ +#define DEBUG_MODULE "QM" + +#include "queuemonitor.h" + +#ifdef DEBUG_QUEUE_MONITOR + +#include +#include "timers.h" +#include "debug.h" +#include "cfassert.h" + +#define MAX_NR_OF_QUEUES 20 +#define TIMER_PERIOD M2T(10000) + +typedef struct +{ + char* fileName; + char* queueName; + int sendCount; + int maxWaiting; + int fullCount; +} Data; + +static Data data[MAX_NR_OF_QUEUES]; + +static xTimerHandle timer; +static unsigned char nrOfQueues = 1; // Unregistered queues will end up at 0 +static bool initialized = false; + +static void timerHandler(xTimerHandle timer); +static void debugPrintData(); +static Data* getQueueData(xQueueHandle* xQueue); +static int getMaxWaiting(xQueueHandle* xQueue, int prevPeak); + +unsigned char ucQueueGetQueueNumber( xQueueHandle xQueue ); + + +void queueMonitorInit() { + ASSERT(!initialized); + timer = xTimerCreate( (const signed char *)"queueMonitorTimer", TIMER_PERIOD, + pdTRUE, NULL, timerHandler ); + xTimerStart(timer, 100); + + data[0].fileName = "Na"; + data[0].queueName = "Na"; + + initialized = true; +} + +void qm_traceQUEUE_SEND(void* xQueue) { + if(initialized) { + Data* queueData = getQueueData(xQueue); + + queueData->sendCount++; + queueData->maxWaiting = getMaxWaiting(xQueue, queueData->maxWaiting); + } +} + +void qm_traceQUEUE_SEND_FAILED(void* xQueue) { + if(initialized) { + Data* queueData = getQueueData(xQueue); + + queueData->fullCount++; + } +} + +void qmRegisterQueue(xQueueHandle* xQueue, char* fileName, char* queueName) { + ASSERT(initialized); + ASSERT(nrOfQueues < MAX_NR_OF_QUEUES); + Data* queueData = &data[nrOfQueues]; + + queueData->fileName = fileName; + queueData->queueName = queueName; + vQueueSetQueueNumber(xQueue, nrOfQueues); + + nrOfQueues++; +} + +static Data* getQueueData(xQueueHandle* xQueue) { + unsigned char number = ucQueueGetQueueNumber(xQueue); + ASSERT(number < MAX_NR_OF_QUEUES); + return &data[number]; +} + +static int getMaxWaiting(xQueueHandle* xQueue, int prevPeak) { + unsigned portBASE_TYPE waiting = uxQueueMessagesWaitingFromISR(xQueue); + + if (waiting > prevPeak) { + return waiting; + } + return prevPeak; +} + +static void debugPrintData() { + DEBUG_PRINT("Report\n"); + + int i; + for (i = 0; i < nrOfQueues; i++) { + Data* curr = &data[i]; + + // The nr of items waiting in a queue is measured BEFORE adding next item. + // Must add 1 to get peak value. + int peak = curr->maxWaiting + 1; + + DEBUG_PRINT("%s:%s, sent: %i, peak: %i, full: %i\n", + curr->fileName, curr->queueName, curr->sendCount, peak, + curr->fullCount); + } +} + +static void timerHandler(xTimerHandle timer) { + debugPrintData(); +} + +#endif // DEBUG_QUEUE_MONITOR \ No newline at end of file diff --git a/modules/src/system.c b/modules/src/system.c index 69bcbfb681..45509279b1 100644 --- a/modules/src/system.c +++ b/modules/src/system.c @@ -56,6 +56,7 @@ #include "mem.h" #include "proximity.h" #include "watchdog.h" +#include "queuemonitor.h" #ifdef PLATFORM_CF2 #include "deck.h" @@ -134,6 +135,10 @@ void systemTask(void *arg) ledInit(); ledSet(CHG_LED, 1); +#ifdef DEBUG_QUEUE_MONITOR + queueMonitorInit(); +#endif + uartInit(); #ifdef ENABLE_UART1 uart1Init(); diff --git a/modules/src/worker.c b/modules/src/worker.c index d1ab65c5f9..e707ec7da3 100644 --- a/modules/src/worker.c +++ b/modules/src/worker.c @@ -30,6 +30,7 @@ #include "FreeRTOS.h" #include "task.h" #include "queue.h" +#include "queuemonitor.h" #include "console.h" @@ -48,6 +49,7 @@ void workerInit() return; workerQueue = xQueueCreate(WORKER_QUEUE_LENGTH, sizeof(struct worker_work)); + DEBUG_QUEUE_MONITOR_REGISTER(workerQueue); } bool workerTest() diff --git a/tools/make/config.mk.example b/tools/make/config.mk.example index 22ef41d3c3..b75bf14a83 100644 --- a/tools/make/config.mk.example +++ b/tools/make/config.mk.example @@ -11,3 +11,6 @@ ## Use morse when flashing the LED to indicate that the Crazyflie is calibrated # CFLAGS += -DCALIBRATED_LED_MORSE + +## Turn on monitoring of queue usages +# CFLAGS += -DDEBUG_QUEUE_MONITOR \ No newline at end of file From 1d5e0b8be72e92e3cdc8f2c33b467bef33da19b5 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sat, 31 Oct 2015 16:55:33 +0100 Subject: [PATCH 44/58] #69 Use compile options from config.mk Corrected bug in make file that ignored options in config.mk file. --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 5f8a02a0ba..2247d23ba5 100644 --- a/Makefile +++ b/Makefile @@ -7,7 +7,7 @@ # Make a copy of tools/make/config.mk.example to get you started -include tools/make/config.mk -CFLAGS = $(EXTRA_CFLAGS) +CFLAGS += $(EXTRA_CFLAGS) ######### JTAG and environment configuration ########## OPENOCD ?= openocd From ea0580e6856ed9a3feed40d4499a7f3788e04d9c Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sat, 31 Oct 2015 18:17:27 +0100 Subject: [PATCH 45/58] #69 Improved queue monitor Added config to reset counters after display and only display queues over flow queues. --- modules/src/queuemonitor.c | 61 +++++++++++++++++++++++++++++--------- 1 file changed, 47 insertions(+), 14 deletions(-) diff --git a/modules/src/queuemonitor.c b/modules/src/queuemonitor.c index 32d46cbbf4..15fd3cf7ee 100644 --- a/modules/src/queuemonitor.c +++ b/modules/src/queuemonitor.c @@ -38,6 +38,9 @@ #define MAX_NR_OF_QUEUES 20 #define TIMER_PERIOD M2T(10000) +#define RESET_COUNTERS_AFTER_DISPLAY true +#define DISPLAY_ONLY_OVERFLOW_QUEUES true + typedef struct { char* fileName; @@ -54,9 +57,12 @@ static unsigned char nrOfQueues = 1; // Unregistered queues will end up at 0 static bool initialized = false; static void timerHandler(xTimerHandle timer); -static void debugPrintData(); +static void debugPrint(); +static bool filter(Data* queueData); +static void debugPrintQueue(Data* queueData); static Data* getQueueData(xQueueHandle* xQueue); static int getMaxWaiting(xQueueHandle* xQueue, int prevPeak); +static void resetCounters(); unsigned char ucQueueGetQueueNumber( xQueueHandle xQueue ); @@ -78,7 +84,10 @@ void qm_traceQUEUE_SEND(void* xQueue) { Data* queueData = getQueueData(xQueue); queueData->sendCount++; - queueData->maxWaiting = getMaxWaiting(xQueue, queueData->maxWaiting); + + // The nr of items waiting in a queue is measured BEFORE adding next item. + // Must add 1 to get peak value. + queueData->maxWaiting = getMaxWaiting(xQueue, queueData->maxWaiting) + 1; } } @@ -117,25 +126,49 @@ static int getMaxWaiting(xQueueHandle* xQueue, int prevPeak) { return prevPeak; } -static void debugPrintData() { - DEBUG_PRINT("Report\n"); - - int i; +static void debugPrint() { + int i = 0; for (i = 0; i < nrOfQueues; i++) { - Data* curr = &data[i]; + Data* queueData = &data[i]; + if (filter(queueData)) { + debugPrintQueue(queueData); + } + } - // The nr of items waiting in a queue is measured BEFORE adding next item. - // Must add 1 to get peak value. - int peak = curr->maxWaiting + 1; + if (RESET_COUNTERS_AFTER_DISPLAY) { + resetCounters(); + } +} + +static bool filter(Data* queueData) { + bool doDisplay = false; + if (DISPLAY_ONLY_OVERFLOW_QUEUES) { + doDisplay = (queueData->fullCount != 0); + } else { + doDisplay = true; + } + return doDisplay; +} + +static void debugPrintQueue(Data* queueData) { + DEBUG_PRINT("%s:%s, sent: %i, peak: %i, full: %i\n", + queueData->fileName, queueData->queueName, queueData->sendCount, + queueData->maxWaiting, queueData->fullCount); +} + +static void resetCounters() { + int i = 0; + for (i = 0; i < nrOfQueues; i++) { + Data* queueData = &data[i]; - DEBUG_PRINT("%s:%s, sent: %i, peak: %i, full: %i\n", - curr->fileName, curr->queueName, curr->sendCount, peak, - curr->fullCount); + queueData->sendCount = 0; + queueData->maxWaiting = 0; + queueData->fullCount = 0; } } static void timerHandler(xTimerHandle timer) { - debugPrintData(); + debugPrint(); } #endif // DEBUG_QUEUE_MONITOR \ No newline at end of file From f31fdf2200654bf5b3d1c13be0badbce46035fe6 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sat, 31 Oct 2015 19:20:07 +0100 Subject: [PATCH 46/58] #69 Corrected peak value in queue monitor --- modules/src/queuemonitor.c | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/modules/src/queuemonitor.c b/modules/src/queuemonitor.c index 15fd3cf7ee..9f30120c6c 100644 --- a/modules/src/queuemonitor.c +++ b/modules/src/queuemonitor.c @@ -84,10 +84,7 @@ void qm_traceQUEUE_SEND(void* xQueue) { Data* queueData = getQueueData(xQueue); queueData->sendCount++; - - // The nr of items waiting in a queue is measured BEFORE adding next item. - // Must add 1 to get peak value. - queueData->maxWaiting = getMaxWaiting(xQueue, queueData->maxWaiting) + 1; + queueData->maxWaiting = getMaxWaiting(xQueue, queueData->maxWaiting); } } @@ -118,7 +115,9 @@ static Data* getQueueData(xQueueHandle* xQueue) { } static int getMaxWaiting(xQueueHandle* xQueue, int prevPeak) { - unsigned portBASE_TYPE waiting = uxQueueMessagesWaitingFromISR(xQueue); + // We get here before the current item is added to the queue. + // Must add 1 to get the peak value. + unsigned portBASE_TYPE waiting = uxQueueMessagesWaitingFromISR(xQueue) + 1; if (waiting > prevPeak) { return waiting; From d514fc8b959f408050b5772c2b4484caa3935cf0 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Sun, 1 Nov 2015 14:57:10 +0100 Subject: [PATCH 47/58] #69 Removed unused queue --- modules/interface/crtp.h | 2 -- modules/src/crtp.c | 11 ----------- 2 files changed, 13 deletions(-) diff --git a/modules/interface/crtp.h b/modules/interface/crtp.h index 663996b31d..4cf42daee4 100644 --- a/modules/interface/crtp.h +++ b/modules/interface/crtp.h @@ -153,8 +153,6 @@ int crtpGetFreeTxQueuePackets(void); */ int crtpReceivePacketBlock(CRTPPort taskId, CRTPPacket *p); -void crtpPacketReveived(CRTPPacket *p); - /** * Function pointer structure to be filled by the CRTP link to permits CRTP to * use manu link diff --git a/modules/src/crtp.c b/modules/src/crtp.c index 5a6f51bf06..717dcce4b6 100644 --- a/modules/src/crtp.c +++ b/modules/src/crtp.c @@ -52,7 +52,6 @@ static struct crtpLinkOperations nopLink = { static struct crtpLinkOperations *link = &nopLink; static xQueueHandle txQueue; -static xQueueHandle rxQueue; #define CRTP_NBR_OF_PORTS 16 #define CRTP_TX_QUEUE_SIZE 60 @@ -71,8 +70,6 @@ void crtpInit(void) txQueue = xQueueCreate(CRTP_TX_QUEUE_SIZE, sizeof(CRTPPacket)); DEBUG_QUEUE_MONITOR_REGISTER(txQueue); - rxQueue = xQueueCreate(CRTP_RX_QUEUE_SIZE, sizeof(CRTPPacket)); - DEBUG_QUEUE_MONITOR_REGISTER(rxQueue); xTaskCreate(crtpTxTask, (const signed char * const)CRTP_TX_TASK_NAME, CRTP_TX_TASK_STACKSIZE, NULL, CRTP_TX_TASK_PRI, NULL); @@ -214,14 +211,6 @@ bool crtpIsConnected(void) return true; } -void crtpPacketReveived(CRTPPacket *p) -{ - portBASE_TYPE xHigherPriorityTaskWoken; - - xHigherPriorityTaskWoken = pdFALSE; - xQueueSendFromISR(rxQueue, p, &xHigherPriorityTaskWoken); -} - void crtpSetLink(struct crtpLinkOperations * lk) { if(link) From 08a8a6b61d6155409084b6febe617551600c83a9 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 10 Nov 2015 15:33:29 +0100 Subject: [PATCH 48/58] Added real time queue trace with ITM SWO Also added a mode in which the ITM trace does not overflow at the cost of slowing down the program. Related to #69 --- config/FreeRTOSConfig.h | 4 +-- config/config.h | 2 ++ config/trace.h | 57 ++++++++++++++++++++++++++++++++++++++++ tools/trace/decodeItm.py | 17 +++++++++--- 4 files changed, 75 insertions(+), 5 deletions(-) create mode 100644 config/trace.h diff --git a/config/FreeRTOSConfig.h b/config/FreeRTOSConfig.h index a2a42b3028..7ef3cfc89e 100644 --- a/config/FreeRTOSConfig.h +++ b/config/FreeRTOSConfig.h @@ -137,11 +137,11 @@ to exclude the API function. */ debugSendTraceInfo((int)pxCurrentTCB->pxTaskTag); \ } */ -// Send 4 first chatacters of task name to ITM port 1 -#define traceTASK_SWITCHED_IN() *((uint32_t*)0xE0000004) = *((uint32_t*)pxCurrentTCB->pcTaskName); // Queue monitoring #ifdef DEBUG_QUEUE_MONITOR + #undef traceQUEUE_SEND + #undef traceQUEUE_SEND_FAILED #define configUSE_TRACE_FACILITY 1 #define traceQUEUE_SEND(xQueue) qm_traceQUEUE_SEND(xQueue) void qm_traceQUEUE_SEND(void* xQueue); diff --git a/config/config.h b/config/config.h index 36214243d5..8c45665e93 100644 --- a/config/config.h +++ b/config/config.h @@ -43,6 +43,8 @@ #define CONFIG_H_ #include "nrf24l01.h" +#include "trace.h" + #define PROTOCOL_VERSION 2 #ifdef STM32F4XX diff --git a/config/trace.h b/config/trace.h new file mode 100644 index 0000000000..8c68af0bca --- /dev/null +++ b/config/trace.h @@ -0,0 +1,57 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2011-2012 Bitcraze AB + * + * 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, in version 3. + * + * 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 . + * + * trace.h - ITM trace implementation/definition + */ + +#ifndef __TRACE_H__ +#define __TRACE_H__ + +#define configUSE_TRACE_FACILITY 1 + +// ITM useful macros +#ifndef ITM_NO_OVERFLOW +#define ITM_SEND(CH, DATA) ((uint32_t*)0xE0000000)[CH] = DATA +#else +#define ITM_SEND(CH, DATA) while(((uint32_t*)0xE0000000)[CH] == 0);\ + ((uint32_t*)0xE0000000)[CH] = DATA +#endif + +// Send 4 first chatacters of task name to ITM port 1 +#define traceTASK_SWITCHED_IN() ITM_SEND(1, *((uint32_t*)pxCurrentTCB->pcTaskName)) + +// Systick value on port 2 +#define traceTASK_INCREMENT_TICK(xTickCount) ITM_SEND(2, xTickCount) + +// Queue trace on port 3 +#define ITM_QUEUE_SEND 0x0100 +#define ITM_QUEUE_FAILED 0x0200 +#define ITM_BLOCKING_ON_QUEUE_RECEIVE 0x0300 +#define ITM_BLOCKING_ON_QUEUE_SEND 0x0400 + +#define traceQUEUE_SEND(xQueue) ITM_SEND(3, ITM_QUEUE_SEND | ((xQUEUE *) xQueue)->ucQueueNumber) +#define traceQUEUE_SEND_FAILED(xQueue) ITM_SEND(3, ITM_QUEUE_FAILED | ((xQUEUE *) xQueue)->ucQueueNumber) +#define traceBLOCKING_ON_QUEUE_RECEIVE(xQueue) ITM_SEND(3, ITM_BLOCKING_ON_QUEUE_RECEIVE | ((xQUEUE *) xQueue)->ucQueueNumber) +#define traceBLOCKING_ON_QUEUE_SEND(xQueue) ITM_SEND(3, ITM_BLOCKING_ON_QUEUE_SEND | ((xQUEUE *) xQueue)->ucQueueNumber) + +#endif diff --git a/tools/trace/decodeItm.py b/tools/trace/decodeItm.py index a391ad0a06..05d59c1786 100755 --- a/tools/trace/decodeItm.py +++ b/tools/trace/decodeItm.py @@ -3,11 +3,9 @@ import sys import struct - class EOFException(Exception): pass - def read_u8(file): data = file.read(1) if len(data) < 1: @@ -34,6 +32,15 @@ def read_u32(file): trace = open(sys.argv[1], "rb") +# OS messages +OS_MASK = 0xFF00 +OS_MESSAGES = { + 0x0100: "ITM_QUEUE_SEND", + 0x0200: "QUEUE_FAILED", + 0x0300: "BLOCKING_ON_QUEUE_RECEIVE", + 0x0400: "BLOCKING_ON_QUEUE_SEND" +} + # Decoding... ctn = 0 try: @@ -83,6 +90,10 @@ def read_u32(file): info = "" if a == 1: info = "\t# Task: " + struct.pack(" Date: Thu, 12 Nov 2015 17:49:14 +0100 Subject: [PATCH 49/58] Enable FreeRTOS debug support to OpenOCD Related to #69 This show threads when debugging with gdb. To test: $ make openocd In a new terminal: $ arm-none-eabi-gdb cf2.elf (gdb) target remote localhost:3333 Remote debugging using localhost:3333 0x00000000 in ?? () (gdb) c Continuing. WARNING! The target is already running. All changes GDB did to registers will be discarded! Waiting for target to halt. ^C Program received signal SIGINT, Interrupt. [Switching to Thread 536875088] 0x080072de in vPortEnterCritical () at lib/FreeRTOS/portable/GCC/ARM_CM4F/port.c:290 290 { (gdb) info threads [New Thread 536884680] [New Thread 536876168] [New Thread 536881784] [New Thread 536879304] [New Thread 536880024] [New Thread 536887600] Id Target Id Frame 7 Thread 536887600 (STABILIZE) 0x00000000 in ?? () 6 Thread 536880024 (CRTP-RX) 0x080065f8 in xQueueGenericReceive (xQueue=0x20002628 , pvBuffer=0x64, xTicksToWait=0, xJustPeeking=536880608) at lib/FreeRTOS/queue.c:1146 5 Thread 536879304 (CRTP-TX) 0x0800634c in xQueueGenericSend (xQueue=0x20006bc8 , pvItemToQueue=0x64, xTicksToWait=0, xCopyPosition=536879896) at lib/FreeRTOS/queue.c:683 4 Thread 536881784 (PWRMGNT) 0x00000000 in ?? () 3 Thread 536876168 (Tmr Svc) 0x08006bec in prvProcessTimerOrBlockTask (xNextExpireTime=0, xListWasEmpty=536876816) at lib/FreeRTOS/timers.c:412 2 Thread 536884680 (SYSLINK) 0x00000000 in ?? () * 1 Thread 536875088 (IDLE : : Running) 0x080072de in vPortEnterCritical () at lib/FreeRTOS/portable/GCC/ARM_CM4F/port.c:290 --- Makefile | 6 +++--- utils/src/FreeRTOS-openocd.c | 20 ++++++++++++++++++++ 2 files changed, 23 insertions(+), 3 deletions(-) create mode 100644 utils/src/FreeRTOS-openocd.c diff --git a/Makefile b/Makefile index 2247d23ba5..e468c4fbf5 100644 --- a/Makefile +++ b/Makefile @@ -156,7 +156,7 @@ PROJ_OBJ_CF2 += ledring12.o # Utilities PROJ_OBJ += filter.o cpuid.o cfassert.o eprintf.o crc.o fp16.o debug.o -PROJ_OBJ += version.o +PROJ_OBJ += version.o FreeRTOS-openocd.o PROJ_OBJ_CF1 += configblockflash.o PROJ_OBJ_CF2 += configblockeeprom.o @@ -244,7 +244,7 @@ CFLAGS += -MD -MP -MF $(BIN)/dep/$(@).d -MQ $(@) CFLAGS += -ffunction-sections -fdata-sections ASFLAGS = $(PROCESSOR) $(INCLUDES) -LDFLAGS = --specs=nano.specs $(PROCESSOR) -Wl,-Map=$(PROG).map,--cref,--gc-sections +LDFLAGS = --specs=nano.specs $(PROCESSOR) -Wl,-Map=$(PROG).map,--cref,--gc-sections,--undefined=uxTopUsedPriority #Flags required by the ST library ifeq ($(CLOAD), 1) @@ -332,7 +332,7 @@ reset: $(OPENOCD) -d0 -f $(OPENOCD_INTERFACE) -f $(OPENOCD_TARGET) -c init -c targets -c "reset" -c shutdown openocd: - $(OPENOCD) -d2 -f $(OPENOCD_INTERFACE) -f $(OPENOCD_TARGET) -c init -c targets + $(OPENOCD) -d2 -f $(OPENOCD_INTERFACE) -f $(OPENOCD_TARGET) -c init -c targets -c "\$$_TARGETNAME configure -rtos auto" trace: $(OPENOCD) -d2 -f $(OPENOCD_INTERFACE) -f $(OPENOCD_TARGET) -c init -c targets -f tools/trace/enable_trace.cfg diff --git a/utils/src/FreeRTOS-openocd.c b/utils/src/FreeRTOS-openocd.c new file mode 100644 index 0000000000..567e710b0b --- /dev/null +++ b/utils/src/FreeRTOS-openocd.c @@ -0,0 +1,20 @@ +/* + * Since at least FreeRTOS V7.5.3 uxTopUsedPriority is no longer + * present in the kernel, so it has to be supplied by other means for + * OpenOCD's threads awareness. + * + * Add this file to your project, and, if you're using --gc-sections, + * ``--undefined=uxTopUsedPriority'' (or + * ``-Wl,--undefined=uxTopUsedPriority'' when using gcc for final + * linking) to your LDFLAGS; same with all the other symbols you need. + */ + +#include "FreeRTOS.h" + +#ifdef __GNUC__ +#define USED __attribute__((used)) +#else +#define USED +#endif + +const int USED uxTopUsedPriority = configMAX_PRIORITIES; From c079b508e446dc3c134f458dbbe5f2716bfae33b Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 17 Nov 2015 10:38:53 +0100 Subject: [PATCH 50/58] Increased back the Syslink task priority This has caused random crash before, see #69. Now that we have added watchdog and debug code it is time to try to trigger the bug again. --- config/FreeRTOSConfig.h | 3 +-- config/config.h | 2 +- hal/src/uart.c | 8 +++++++- 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/config/FreeRTOSConfig.h b/config/FreeRTOSConfig.h index 7ef3cfc89e..dffa3f85a6 100644 --- a/config/FreeRTOSConfig.h +++ b/config/FreeRTOSConfig.h @@ -88,7 +88,7 @@ #define configUSE_MALLOC_FAILED_HOOK 1 #define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE -#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 ) +#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 6 ) #define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) /* Set the following definitions to 1 to include the API function, or zero @@ -142,7 +142,6 @@ to exclude the API function. */ #ifdef DEBUG_QUEUE_MONITOR #undef traceQUEUE_SEND #undef traceQUEUE_SEND_FAILED - #define configUSE_TRACE_FACILITY 1 #define traceQUEUE_SEND(xQueue) qm_traceQUEUE_SEND(xQueue) void qm_traceQUEUE_SEND(void* xQueue); #define traceQUEUE_SEND_FAILED(xQueue) qm_traceQUEUE_SEND_FAILED(xQueue) diff --git a/config/config.h b/config/config.h index 8c45665e93..beb6b1369a 100644 --- a/config/config.h +++ b/config/config.h @@ -82,7 +82,7 @@ #define PM_TASK_PRI 0 #ifdef PLATFORM_CF2 - #define SYSLINK_TASK_PRI 3 + #define SYSLINK_TASK_PRI 5 #define USBLINK_TASK_PRI 3 #endif diff --git a/hal/src/uart.c b/hal/src/uart.c index 6bb9ade492..e822d8967d 100644 --- a/hal/src/uart.c +++ b/hal/src/uart.c @@ -264,12 +264,18 @@ void uartIsr(void) xHigherPriorityTaskWoken = pdFALSE; xSemaphoreGiveFromISR(waitUntilSendDone, &xHigherPriorityTaskWoken); } + USART_ClearITPendingBit(UART_TYPE, USART_IT_TXE); } - USART_ClearITPendingBit(UART_TYPE, USART_IT_TXE); + if (USART_GetITStatus(UART_TYPE, USART_IT_RXNE)) { rxDataInterrupt = USART_ReceiveData(UART_TYPE) & 0xFF; xQueueSendFromISR(uartDataDelivery, &rxDataInterrupt, &xHigherPriorityTaskWoken); + + if (xHigherPriorityTaskWoken) + { + vPortYieldFromISR(); + } } } From ddc7ba060bbc1fc7461242ae0ff3ad1e2b543f1a Mon Sep 17 00:00:00 2001 From: Marcus Eliasson Date: Wed, 25 Nov 2015 13:04:57 +0100 Subject: [PATCH 51/58] Refactored buzzer/sound --- Makefile | 6 +- deck/drivers/src/buzzdeck.c | 69 ++ hal/interface/buzzer.h | 45 ++ hal/src/buzzer.c | 56 ++ modules/interface/sound.h | 40 ++ .../src/buzzer.c => modules/src/sound.c | 629 +++++++++--------- modules/src/system.c | 6 + 7 files changed, 531 insertions(+), 320 deletions(-) create mode 100644 deck/drivers/src/buzzdeck.c create mode 100644 hal/interface/buzzer.h create mode 100644 hal/src/buzzer.c create mode 100644 modules/interface/sound.h rename deck/drivers/src/buzzer.c => modules/src/sound.c (86%) diff --git a/Makefile b/Makefile index e468c4fbf5..d5dd7ede52 100644 --- a/Makefile +++ b/Makefile @@ -130,7 +130,7 @@ PROJ_OBJ_CF2 += uart_syslink.o swd.o uart1.o uart2.o watchdog.o PROJ_OBJ_CF2 += usb_bsp.o usblink.o usbd_desc.o usb.o # Hal -PROJ_OBJ += crtp.o ledseq.o freeRTOSdebug.o +PROJ_OBJ += crtp.o ledseq.o freeRTOSdebug.o buzzer.o PROJ_OBJ_CF1 += imu_cf1.o pm_f103.o nrf24link.o ow_none.o uart.o PROJ_OBJ_CF2 += imu_cf2.o pm_f405.o syslink.o radiolink.o ow_syslink.o proximity.o @@ -138,7 +138,7 @@ PROJ_OBJ_CF2 += imu_cf2.o pm_f405.o syslink.o radiolink.o ow_syslink.o proximity PROJ_OBJ += system.o comm.o console.o pid.o crtpservice.o param.o mem.o PROJ_OBJ += commander.o controller.o sensfusion6.o stabilizer.o PROJ_OBJ += log.o worker.o trigger.o sitaw.o queuemonitor.o -PROJ_OBJ_CF2 += platformservice.o +PROJ_OBJ_CF2 += platformservice.o sound.o # Deck Core PROJ_OBJ_CF2 += deck.o deck_info.o deck_drivers.o @@ -147,7 +147,7 @@ PROJ_OBJ_CF2 += deck.o deck_info.o deck_drivers.o PROJ_OBJ_CF2 += deck_constants.o PROJ_OBJ_CF2 += deck_digital.o PROJ_OBJ_CF2 += deck_analog.o -PROJ_OBJ_CF2 += buzzer.o +PROJ_OBJ_CF2 += buzzdeck.o # Decks PROJ_OBJ_CF2 += bigquad.o diff --git a/deck/drivers/src/buzzdeck.c b/deck/drivers/src/buzzdeck.c new file mode 100644 index 0000000000..904ec22fa0 --- /dev/null +++ b/deck/drivers/src/buzzdeck.c @@ -0,0 +1,69 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2012 BitCraze AB + * + * 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, in version 3. + * + * 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 . + * + * buzzer.c: Play tones or melodies + */ + +#include +#include +#include "stm32fxxx.h" + +#include "deck.h" + +#include "buzzer.h" +#include "piezo.h" + +static void buzzDeckOn(uint32_t freq) +{ + piezoSetRatio(128); + piezoSetFreq(freq); +} + +static void buzzDeckOff() +{ + piezoSetRatio(0); +} + +static struct buzzerControl buzzDeckCtrl = +{ + .on = buzzDeckOn, + .off = buzzDeckOff +}; + +static void buzzDeckInit(DeckInfo *info) +{ + piezoInit(); + buzzerSetControl(&buzzDeckCtrl); +} + +static const DeckDriver buzzer_deck = { + .vid = 0, + .pid = 0, + .name = "bcBuzzer", + + .usedGpio = DECK_USING_PA2 | DECK_USING_PA3, + + .init = buzzDeckInit, +}; + +DECK_DRIVER(buzzer_deck); diff --git a/hal/interface/buzzer.h b/hal/interface/buzzer.h new file mode 100644 index 0000000000..1e54ab44e1 --- /dev/null +++ b/hal/interface/buzzer.h @@ -0,0 +1,45 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2011-2012 Bitcraze AB + * + * 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, in version 3. + * + * 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 . + * + * buzzer.c - Functions for interfacing with decks with buzzers + */ +#ifndef __BUZZER_H__ +#define __BUZZER_H__ + +#include +#include + +struct buzzerControl +{ + void (*off)(); + void (*on)(uint32_t freq); +}; + +void buzzerInit(); +bool buzzerTest(); +void buzzerOff(); +void buzzerOn(uint32_t freq); +void buzzerSetControl(struct buzzerControl * bc); + +#endif //__BUZZER_H__ + diff --git a/hal/src/buzzer.c b/hal/src/buzzer.c new file mode 100644 index 0000000000..3357549804 --- /dev/null +++ b/hal/src/buzzer.c @@ -0,0 +1,56 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2011-2012 Bitcraze AB + * + * 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, in version 3. + * + * 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 . + * + * buzzer.c - Functions for interfacing with decks with buzzers + */ +#define DEBUG_MODULE "BUZZER" + +#include + +#include "buzzer.h" + +static struct buzzerControl * ctrl; + +void buzzerInit() +{ +} + +bool buzzerTest() +{ + return true; +} + +void buzzerOff() { + if (ctrl) + ctrl->off(); +} +void buzzerOn(uint32_t freq) +{ + if (ctrl) + ctrl->on(freq); +} + +void buzzerSetControl(struct buzzerControl * bc) +{ + ctrl = bc; +} diff --git a/modules/interface/sound.h b/modules/interface/sound.h new file mode 100644 index 0000000000..2b58fb0447 --- /dev/null +++ b/modules/interface/sound.h @@ -0,0 +1,40 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2015 Bitcraze AB + * + * 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, in version 3. + * + * 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 . + * + * sound.h - Module used to play melodies and system sounds though a buzzer + */ + +#ifndef __SOUND_H__ +#define __SOUND_H__ + +#include + +/** + * Initialize the platform CRTP port + */ +void soundInit(void); + +bool soundTest(void); + +#endif /* __SOUND_H__ */ + diff --git a/deck/drivers/src/buzzer.c b/modules/src/sound.c similarity index 86% rename from deck/drivers/src/buzzer.c rename to modules/src/sound.c index 30267db6f8..87eaab449c 100644 --- a/deck/drivers/src/buzzer.c +++ b/modules/src/sound.c @@ -1,317 +1,312 @@ -/** - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ - * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ - * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ - * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ - * - * Crazyflie control firmware - * - * Copyright (C) 2012 BitCraze AB - * - * 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, in version 3. - * - * 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 . - * - * buzzer.c: Play tones or melodies - */ - -#include -#include -#include "stm32fxxx.h" - -#include "FreeRTOS.h" -#include "timers.h" - -#include "deck.h" - -#include "param.h" -#include "pm.h" -#include "log.h" -#include "piezo.h" - -typedef void (*BuzzerEffect)(uint32_t timer); - -/** - * Credit to http://tny.cz/e525c1b2 for supplying the tones - */ -#define C0 16 -#define Db0 17 -#define D0 18 -#define Eb0 19 -#define E0 20 -#define F0 21 -#define Gb0 23 -#define G0 24 -#define Ab0 25 -#define A0 27 -#define Bb0 29 -#define B0 30 -#define C1 32 -#define Db1 34 -#define D1 36 -#define Eb1 38 -#define E1 41 -#define F1 43 -#define Gb1 46 -#define G1 49 -#define Ab1 51 -#define A1 55 -#define Bb1 58 -#define B1 61 -#define C2 65 -#define Db2 69 -#define D2 73 -#define Eb2 77 -#define E2 82 -#define F2 87 -#define Gb2 92 -#define G2 98 -#define Ab2 103 -#define A2 110 -#define Bb2 116 -#define B2 123 -#define C3 130 -#define Db3 138 -#define D3 146 -#define Eb3 155 -#define E3 164 -#define F3 174 -#define Gb3 185 -#define G3 196 -#define Ab3 207 -#define A3 220 -#define Bb3 233 -#define B3 246 -#define C4 261 -#define Db4 277 -#define D4 293 -#define Eb4 311 -#define E4 329 -#define F4 349 -#define Gb4 369 -#define G4 392 -#define Ab4 415 -#define A4 440 -#define Bb4 466 -#define B4 493 -#define C5 523 -#define Db5 554 -#define D5 587 -#define Eb5 622 -#define E5 659 -#define F5 698 -#define Gb5 739 -#define G5 783 -#define Ab5 830 -#define A5 880 -#define Bb5 932 -#define B5 987 -#define C6 1046 -#define Db6 1108 -#define D6 1174 -#define Eb6 1244 -#define E6 1318 -#define F6 1396 -#define Gb6 1479 -#define G6 1567 -#define Ab6 1661 -#define A6 1760 -#define Bb6 1864 -#define B6 1975 -#define C7 2093 -#define Db7 2217 -#define D7 2349 -#define Eb7 2489 -#define E7 2637 -#define F7 2793 -#define Gb7 2959 -#define G7 3135 -#define Ab7 3322 -#define A7 3520 -#define Bb7 3729 -#define B7 3951 -#define C8 4186 -#define Db8 4434 -#define D8 4698 -#define Eb8 4978 -/* Duration of notes */ -#define W (60) -#define H (W * 2) -#define Q (W * 4) -#define E (W * 8) -#define S (W * 16) -#define ES (W*6) - - -typedef struct { - uint16_t tone; - uint16_t duration; -} Note; - -typedef struct { - uint32_t bpm; - uint32_t ni; - uint32_t delay; - Note notes[80]; -} Melody; - -Melody melodies[] = { - {.bpm = 120, .ni = 0, .delay = 1, .notes = {{C4, H}, {D4, H}, {E4, H}, {F4, H}, {G4, H}, {A4, H}, {B4, H}, {0xFF, 0}}}, - {.bpm = 120, .ni = 0, .delay = 1, .notes = {{C4, S}, {D4, S}, {E4, S}, {F4, S}, {G4, S}, {A4, S}, {B4, S}, {0xFF, 0}}}, - /* Imperial march from http://tny.cz/e525c1b2A */ - {.bpm = 120, .ni = 0, .delay = 1, .notes = {{A3, Q}, {A3, Q}, {A3, Q},{F3, ES}, {C4, S}, - {A3, Q}, {F3, ES}, {C4, S}, {A3, H}, - {E4, Q}, {E4, Q}, {E4, Q}, {F4, ES}, {C4, S}, - {Ab3, Q}, {F3, ES}, {C4, S}, {A3, H}, - {A4, Q}, {A3, ES}, {A3, S}, {A4, Q}, {Ab4, ES}, {G4, S}, - {Gb4, S}, {E4, S}, {F4, E}, {0, E}, {Bb3, E}, {Eb4, Q}, {D4, ES}, {Db4, S}, - {C4, S}, {B3, S}, {C4, E}, {0, E}, {F3, E}, {Ab3, Q}, {F3, ES}, {A3, S}, - {C4, Q}, {A3, ES}, {C4, S}, {E4, H}, - {A4, Q}, {A3, ES}, {A3, S}, {A4, Q}, {Ab4, ES}, {G4, S}, - {Gb4, S}, {E4, S}, {F4, E}, {0, E}, {Bb3, E}, {Eb4, Q}, {D4, ES}, {Db4, S}, - {Gb4, S}, {E4, S}, {F4, E}, {0, E}, {Bb3, E}, {Eb4, Q}, {D4, ES}, {Db4, S}, - {C4,S}, {B3, S}, {C4, E}, {0, E}, {F3, E}, {Ab3, Q}, {F3, ES}, {C4, S}, - {A3, Q}, {F3, ES}, {C4, S}, {A3, H}, {0, H}, - {0xFF, 0}}} - -}; - -uint8_t melody = 2; -uint8_t nmelody = 3; -unsigned int mcounter = 0; -static bool playing_sound = false; -static void melodyplayer(uint32_t counter) { - // Sync one melody for the loop - Melody* m = &melodies[melody]; - if (mcounter == 0) { - if (m->notes[m->ni].tone == 0xFF) - { - // Loop the melody - m->ni = 0; - } - // Play current note - piezoSetFreq(m->notes[m->ni].tone); - piezoSetRatio(127); - mcounter = (m->bpm * 100) / m->notes[m->ni].duration; - playing_sound = true; - m->ni++; - } - else if (mcounter == 1) - { - piezoSetRatio(0); - } - mcounter--; -} - -uint8_t static_ratio = 0; -uint16_t static_freq = 4000; -static void bypass(uint32_t counter) -{ - /* Just set the params from the host */ - piezoSetRatio(static_ratio); - piezoSetFreq(static_freq); -} - -uint16_t siren_start = 2000; -uint16_t siren_freq = 2000; -uint16_t siren_stop = 4000; -int16_t siren_step = 40; -static void siren(uint32_t counter) -{ - siren_freq += siren_step; - if (siren_freq > siren_stop) - { - siren_step *= -1; - siren_freq = siren_stop; - } - if (siren_freq < siren_start) - { - siren_step *= -1; - siren_freq = siren_start; - } - piezoSetRatio(127); - piezoSetFreq(siren_freq); -} - - -static int pitchid; -static int rollid; -static int pitch; -static int roll; -static int tilt_freq; -static int tilt_ratio; -static void tilt(uint32_t counter) -{ - pitchid = logGetVarId("stabilizer", "pitch"); - rollid = logGetVarId("stabilizer", "roll"); - - pitch = logGetInt(pitchid); - roll = logGetInt(rollid); - tilt_freq = 0; - tilt_ratio = 127; - - if (abs(pitch) > 5) { - tilt_freq = 3000 - 50 * pitch; - } - - piezoSetRatio(tilt_ratio); - piezoSetFreq(tilt_freq); -} - - -unsigned int neffect = 0; -unsigned int effect = 3; -BuzzerEffect effects[] = {bypass, siren, melodyplayer, tilt}; - -static xTimerHandle timer; -static uint32_t counter = 0; - -static void buzzTimer(xTimerHandle timer) -{ - counter++; - - if (effects[effect] != 0) - effects[effect](counter*10); -} - -static void buzzerInit(DeckInfo *info) -{ - piezoInit(); - - neffect = sizeof(effects)/sizeof(effects[0])-1; - nmelody = sizeof(melodies)/sizeof(melodies[0])-1; - - timer = xTimerCreate( (const signed char *)"buzztimer", M2T(10), - pdTRUE, NULL, buzzTimer ); - xTimerStart(timer, 100); -} - -PARAM_GROUP_START(buzzer) -PARAM_ADD(PARAM_UINT8, effect, &effect) -PARAM_ADD(PARAM_UINT32 | PARAM_RONLY, neffect, &neffect) -PARAM_ADD(PARAM_UINT8, melody, &melody) -PARAM_ADD(PARAM_UINT32 | PARAM_RONLY, nmelody, &melody) -PARAM_ADD(PARAM_UINT16, freq, &static_freq) -PARAM_ADD(PARAM_UINT8, ratio, &static_ratio) -PARAM_GROUP_STOP(buzzer) - -static const DeckDriver buzzer_deck = { - .vid = 0, - .pid = 0, - .name = "bcBuzzer", - - .usedGpio = DECK_USING_PA2 | DECK_USING_PA3, - - .init = buzzerInit, -}; - -DECK_DRIVER(buzzer_deck); +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2015 Bitcraze AB + * + * 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, in version 3. + * + * 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 . + * + * sound.c - Module used to play melodies and system sounds though a buzzer + */ + +#include + +/* FreeRtos includes */ +#include +#include + +#include "FreeRTOS.h" +#include "timers.h" + +#include "config.h" +#include "param.h" +#include "log.h" +#include "sound.h" +#include "buzzer.h" + +/** + * Credit to http://tny.cz/e525c1b2 for supplying the tones + */ +#define C0 16 +#define Db0 17 +#define D0 18 +#define Eb0 19 +#define E0 20 +#define F0 21 +#define Gb0 23 +#define G0 24 +#define Ab0 25 +#define A0 27 +#define Bb0 29 +#define B0 30 +#define C1 32 +#define Db1 34 +#define D1 36 +#define Eb1 38 +#define E1 41 +#define F1 43 +#define Gb1 46 +#define G1 49 +#define Ab1 51 +#define A1 55 +#define Bb1 58 +#define B1 61 +#define C2 65 +#define Db2 69 +#define D2 73 +#define Eb2 77 +#define E2 82 +#define F2 87 +#define Gb2 92 +#define G2 98 +#define Ab2 103 +#define A2 110 +#define Bb2 116 +#define B2 123 +#define C3 130 +#define Db3 138 +#define D3 146 +#define Eb3 155 +#define E3 164 +#define F3 174 +#define Gb3 185 +#define G3 196 +#define Ab3 207 +#define A3 220 +#define Bb3 233 +#define B3 246 +#define C4 261 +#define Db4 277 +#define D4 293 +#define Eb4 311 +#define E4 329 +#define F4 349 +#define Gb4 369 +#define G4 392 +#define Ab4 415 +#define A4 440 +#define Bb4 466 +#define B4 493 +#define C5 523 +#define Db5 554 +#define D5 587 +#define Eb5 622 +#define E5 659 +#define F5 698 +#define Gb5 739 +#define G5 783 +#define Ab5 830 +#define A5 880 +#define Bb5 932 +#define B5 987 +#define C6 1046 +#define Db6 1108 +#define D6 1174 +#define Eb6 1244 +#define E6 1318 +#define F6 1396 +#define Gb6 1479 +#define G6 1567 +#define Ab6 1661 +#define A6 1760 +#define Bb6 1864 +#define B6 1975 +#define C7 2093 +#define Db7 2217 +#define D7 2349 +#define Eb7 2489 +#define E7 2637 +#define F7 2793 +#define Gb7 2959 +#define G7 3135 +#define Ab7 3322 +#define A7 3520 +#define Bb7 3729 +#define B7 3951 +#define C8 4186 +#define Db8 4434 +#define D8 4698 +#define Eb8 4978 +/* Duration of notes */ +#define W (60) +#define H (W * 2) +#define Q (W * 4) +#define E (W * 8) +#define S (W * 16) +#define ES (W*6) + +static bool isInit=false; + +typedef void (*BuzzerEffect)(uint32_t timer); + +typedef struct { + uint16_t tone; + uint16_t duration; +} Note; + +typedef struct { + uint32_t bpm; + uint32_t ni; + uint32_t delay; + Note notes[80]; +} Melody; + +Melody melodies[] = { + {.bpm = 120, .ni = 0, .delay = 1, .notes = {{C4, H}, {D4, H}, {E4, H}, {F4, H}, {G4, H}, {A4, H}, {B4, H}, {0xFF, 0}}}, + {.bpm = 120, .ni = 0, .delay = 1, .notes = {{C4, S}, {D4, S}, {E4, S}, {F4, S}, {G4, S}, {A4, S}, {B4, S}, {0xFF, 0}}}, + /* Imperial march from http://tny.cz/e525c1b2A */ + {.bpm = 120, .ni = 0, .delay = 1, .notes = {{A3, Q}, {A3, Q}, {A3, Q},{F3, ES}, {C4, S}, + {A3, Q}, {F3, ES}, {C4, S}, {A3, H}, + {E4, Q}, {E4, Q}, {E4, Q}, {F4, ES}, {C4, S}, + {Ab3, Q}, {F3, ES}, {C4, S}, {A3, H}, + {A4, Q}, {A3, ES}, {A3, S}, {A4, Q}, {Ab4, ES}, {G4, S}, + {Gb4, S}, {E4, S}, {F4, E}, {0, E}, {Bb3, E}, {Eb4, Q}, {D4, ES}, {Db4, S}, + {C4, S}, {B3, S}, {C4, E}, {0, E}, {F3, E}, {Ab3, Q}, {F3, ES}, {A3, S}, + {C4, Q}, {A3, ES}, {C4, S}, {E4, H}, + {A4, Q}, {A3, ES}, {A3, S}, {A4, Q}, {Ab4, ES}, {G4, S}, + {Gb4, S}, {E4, S}, {F4, E}, {0, E}, {Bb3, E}, {Eb4, Q}, {D4, ES}, {Db4, S}, + {Gb4, S}, {E4, S}, {F4, E}, {0, E}, {Bb3, E}, {Eb4, Q}, {D4, ES}, {Db4, S}, + {C4,S}, {B3, S}, {C4, E}, {0, E}, {F3, E}, {Ab3, Q}, {F3, ES}, {C4, S}, + {A3, Q}, {F3, ES}, {C4, S}, {A3, H}, {0, H}, + {0xFF, 0}}} + +}; + +uint8_t melody = 2; +uint8_t nmelody = 3; +unsigned int mcounter = 0; +static bool playing_sound = false; +static void melodyplayer(uint32_t counter) { + // Sync one melody for the loop + Melody* m = &melodies[melody]; + if (mcounter == 0) { + if (m->notes[m->ni].tone == 0xFF) + { + // Loop the melody + m->ni = 0; + } + // Play current note + buzzerOn(m->notes[m->ni].tone); + mcounter = (m->bpm * 100) / m->notes[m->ni].duration; + playing_sound = true; + m->ni++; + } + else if (mcounter == 1) + { + buzzerOff(); + } + mcounter--; +} + +uint8_t static_ratio = 0; +uint16_t static_freq = 4000; +static void bypass(uint32_t counter) +{ + /* Just set the params from the host */ + buzzerOn(static_freq); +} + +uint16_t siren_start = 2000; +uint16_t siren_freq = 2000; +uint16_t siren_stop = 4000; +int16_t siren_step = 40; +static void siren(uint32_t counter) +{ + siren_freq += siren_step; + if (siren_freq > siren_stop) + { + siren_step *= -1; + siren_freq = siren_stop; + } + if (siren_freq < siren_start) + { + siren_step *= -1; + siren_freq = siren_start; + } + buzzerOn(siren_freq); +} + + +static int pitchid; +static int rollid; +static int pitch; +static int roll; +static int tilt_freq; +static int tilt_ratio; +static void tilt(uint32_t counter) +{ + pitchid = logGetVarId("stabilizer", "pitch"); + rollid = logGetVarId("stabilizer", "roll"); + + pitch = logGetInt(pitchid); + roll = logGetInt(rollid); + tilt_freq = 0; + tilt_ratio = 127; + + if (abs(pitch) > 5) { + tilt_freq = 3000 - 50 * pitch; + } + + buzzerOn(tilt_freq); +} + + +unsigned int neffect = 0; +unsigned int effect = 3; +static BuzzerEffect effects[] = {bypass, siren, melodyplayer, tilt}; + +static xTimerHandle timer; +static uint32_t counter = 0; + +static void soundTimer(xTimerHandle timer) +{ + counter++; + + if (effects[effect] != 0) + effects[effect](counter*10); +} + +void soundInit(void) +{ + if (isInit) + return; + + neffect = sizeof(effects)/sizeof(effects[0])-1; + nmelody = sizeof(melodies)/sizeof(melodies[0])-1; + + timer = xTimerCreate( (const signed char *)"SoundTimer", M2T(10), + pdTRUE, NULL, soundTimer ); + xTimerStart(timer, 100); + + isInit = true; +} + +bool soundTest(void) +{ + return isInit; +} + +PARAM_GROUP_START(sound) +PARAM_ADD(PARAM_UINT8, effect, &effect) +PARAM_ADD(PARAM_UINT32 | PARAM_RONLY, neffect, &neffect) +PARAM_ADD(PARAM_UINT8, melody, &melody) +PARAM_ADD(PARAM_UINT32 | PARAM_RONLY, nmelody, &melody) +PARAM_ADD(PARAM_UINT16, freq, &static_freq) +PARAM_ADD(PARAM_UINT8, ratio, &static_ratio) +PARAM_GROUP_STOP(sound) + diff --git a/modules/src/system.c b/modules/src/system.c index 45509279b1..b231f17ed0 100644 --- a/modules/src/system.c +++ b/modules/src/system.c @@ -54,6 +54,8 @@ #include "console.h" #include "usb.h" #include "mem.h" +#include "buzzer.h" +#include "sound.h" #include "proximity.h" #include "watchdog.h" #include "queuemonitor.h" @@ -108,6 +110,7 @@ void systemInit(void) adcInit(); ledseqInit(); pmInit(); + buzzerInit(); isInit = true; } @@ -122,6 +125,7 @@ bool systemTest() pass &= ledseqTest(); pass &= pmTest(); pass &= workerTest(); + pass &= buzzerTest(); return pass; } @@ -166,6 +170,7 @@ void systemTask(void *arg) deckInit(); #endif memInit(); + soundInit(); #ifdef PROXIMITY_ENABLED proximityInit(); @@ -181,6 +186,7 @@ void systemTask(void *arg) pass &= deckTest(); #endif pass &= memTest(); + pass &= soundTest(); pass &= watchdogNormalStartTest(); //Start the firmware From 5540737545ecb93dde02d7e286daac6d46025b2e Mon Sep 17 00:00:00 2001 From: Marcus Eliasson Date: Wed, 25 Nov 2015 13:34:09 +0100 Subject: [PATCH 52/58] Fixed CF1 broken build --- Makefile | 4 ++-- modules/src/system.c | 18 ++++++++++-------- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/Makefile b/Makefile index d5dd7ede52..de4f76853f 100644 --- a/Makefile +++ b/Makefile @@ -130,9 +130,9 @@ PROJ_OBJ_CF2 += uart_syslink.o swd.o uart1.o uart2.o watchdog.o PROJ_OBJ_CF2 += usb_bsp.o usblink.o usbd_desc.o usb.o # Hal -PROJ_OBJ += crtp.o ledseq.o freeRTOSdebug.o buzzer.o +PROJ_OBJ += crtp.o ledseq.o freeRTOSdebug.o PROJ_OBJ_CF1 += imu_cf1.o pm_f103.o nrf24link.o ow_none.o uart.o -PROJ_OBJ_CF2 += imu_cf2.o pm_f405.o syslink.o radiolink.o ow_syslink.o proximity.o +PROJ_OBJ_CF2 += imu_cf2.o pm_f405.o syslink.o radiolink.o ow_syslink.o proximity.o buzzer.o # Modules PROJ_OBJ += system.o comm.o console.o pid.o crtpservice.o param.o mem.o diff --git a/modules/src/system.c b/modules/src/system.c index b231f17ed0..2a97bd829e 100644 --- a/modules/src/system.c +++ b/modules/src/system.c @@ -54,14 +54,14 @@ #include "console.h" #include "usb.h" #include "mem.h" -#include "buzzer.h" -#include "sound.h" #include "proximity.h" #include "watchdog.h" #include "queuemonitor.h" #ifdef PLATFORM_CF2 #include "deck.h" +#include "buzzer.h" +#include "sound.h" #endif /* Private variable */ @@ -110,8 +110,9 @@ void systemInit(void) adcInit(); ledseqInit(); pmInit(); +#ifdef PLATFORM_CF2 buzzerInit(); - +#endif isInit = true; } @@ -125,8 +126,9 @@ bool systemTest() pass &= ledseqTest(); pass &= pmTest(); pass &= workerTest(); +#ifdef PLATFORM_CF2 pass &= buzzerTest(); - +#endif return pass; } @@ -168,9 +170,9 @@ void systemTask(void *arg) stabilizerInit(); #ifdef PLATFORM_CF2 deckInit(); -#endif - memInit(); soundInit(); + #endif + memInit(); #ifdef PROXIMITY_ENABLED proximityInit(); @@ -184,9 +186,9 @@ void systemTask(void *arg) pass &= stabilizerTest(); #ifdef PLATFORM_CF2 pass &= deckTest(); -#endif - pass &= memTest(); pass &= soundTest(); + #endif + pass &= memTest(); pass &= watchdogNormalStartTest(); //Start the firmware From c4f1ea504ebb37d4cea4d17c9fa6337a86f72068 Mon Sep 17 00:00:00 2001 From: Marcus Eliasson Date: Wed, 25 Nov 2015 13:38:25 +0100 Subject: [PATCH 53/58] Removed warning from missing include --- modules/src/sound.c | 1 + 1 file changed, 1 insertion(+) diff --git a/modules/src/sound.c b/modules/src/sound.c index 87eaab449c..98d21a9aa1 100644 --- a/modules/src/sound.c +++ b/modules/src/sound.c @@ -29,6 +29,7 @@ /* FreeRtos includes */ #include #include +#include #include "FreeRTOS.h" #include "timers.h" From 9acebbf5bbc650d7dfa09570298f8d7def25fbd6 Mon Sep 17 00:00:00 2001 From: Marcus Eliasson Date: Wed, 25 Nov 2015 17:26:44 +0100 Subject: [PATCH 54/58] Cleaned up sound interface and inserted system sounds --- Makefile | 7 +- deck/drivers/src/buzzdeck.c | 7 +- hal/interface/buzzer.h | 24 ++- hal/src/buzzer.c | 6 +- hal/src/imu_cf2.c | 2 + hal/src/pm_f405.c | 5 + modules/interface/sound.h | 24 ++- modules/src/sound.c | 313 ------------------------------- modules/src/sound_cf1.c | 52 ++++++ modules/src/sound_cf2.c | 354 ++++++++++++++++++++++++++++++++++++ modules/src/system.c | 14 +- 11 files changed, 474 insertions(+), 334 deletions(-) delete mode 100644 modules/src/sound.c create mode 100644 modules/src/sound_cf1.c create mode 100644 modules/src/sound_cf2.c diff --git a/Makefile b/Makefile index de4f76853f..53d653cb7f 100644 --- a/Makefile +++ b/Makefile @@ -130,15 +130,16 @@ PROJ_OBJ_CF2 += uart_syslink.o swd.o uart1.o uart2.o watchdog.o PROJ_OBJ_CF2 += usb_bsp.o usblink.o usbd_desc.o usb.o # Hal -PROJ_OBJ += crtp.o ledseq.o freeRTOSdebug.o +PROJ_OBJ += crtp.o ledseq.o freeRTOSdebug.o buzzer.o PROJ_OBJ_CF1 += imu_cf1.o pm_f103.o nrf24link.o ow_none.o uart.o -PROJ_OBJ_CF2 += imu_cf2.o pm_f405.o syslink.o radiolink.o ow_syslink.o proximity.o buzzer.o +PROJ_OBJ_CF2 += imu_cf2.o pm_f405.o syslink.o radiolink.o ow_syslink.o proximity.o # Modules PROJ_OBJ += system.o comm.o console.o pid.o crtpservice.o param.o mem.o PROJ_OBJ += commander.o controller.o sensfusion6.o stabilizer.o PROJ_OBJ += log.o worker.o trigger.o sitaw.o queuemonitor.o -PROJ_OBJ_CF2 += platformservice.o sound.o +PROJ_OBJ_CF1 += sound_cf1.o +PROJ_OBJ_CF2 += platformservice.o sound_cf2.o # Deck Core PROJ_OBJ_CF2 += deck.o deck_info.o deck_drivers.o diff --git a/deck/drivers/src/buzzdeck.c b/deck/drivers/src/buzzdeck.c index 904ec22fa0..ae6cf973bd 100644 --- a/deck/drivers/src/buzzdeck.c +++ b/deck/drivers/src/buzzdeck.c @@ -7,7 +7,7 @@ * * Crazyflie control firmware * - * Copyright (C) 2012 BitCraze AB + * Copyright (C) 2015 BitCraze AB * * 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 @@ -21,7 +21,7 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . * - * buzzer.c: Play tones or melodies + * buzzdeck.c - Deck driver for the Crazyflie 2.0 buzzer deck */ #include @@ -44,8 +44,7 @@ static void buzzDeckOff() piezoSetRatio(0); } -static struct buzzerControl buzzDeckCtrl = -{ +static struct buzzerControl buzzDeckCtrl = { .on = buzzDeckOn, .off = buzzDeckOff }; diff --git a/hal/interface/buzzer.h b/hal/interface/buzzer.h index 1e54ab44e1..07331ad18f 100644 --- a/hal/interface/buzzer.h +++ b/hal/interface/buzzer.h @@ -7,7 +7,7 @@ * * Crazyflie control firmware * - * Copyright (C) 2011-2012 Bitcraze AB + * Copyright (C) 2015 Bitcraze AB * * 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 @@ -21,7 +21,7 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . * - * buzzer.c - Functions for interfacing with decks with buzzers + * buzzdeck.h - Functions for interfacing with decks with buzzers */ #ifndef __BUZZER_H__ #define __BUZZER_H__ @@ -29,16 +29,36 @@ #include #include +/** Functionpointers used to control the buzzer */ struct buzzerControl { void (*off)(); void (*on)(uint32_t freq); }; +/** + * Initilize the buzzer sub-system. + */ void buzzerInit(); + +/** + * Test the buzzer sub-system. + */ bool buzzerTest(); + +/** + * Turn the buzzer off. + */ void buzzerOff(); + +/** + * Turn the buzzer on and set it to a specific frequency (if supported). + */ void buzzerOn(uint32_t freq); + +/** + * Set function pointers for controlling the buzzer hardware. + */ void buzzerSetControl(struct buzzerControl * bc); #endif //__BUZZER_H__ diff --git a/hal/src/buzzer.c b/hal/src/buzzer.c index 3357549804..63daeca586 100644 --- a/hal/src/buzzer.c +++ b/hal/src/buzzer.c @@ -25,8 +25,6 @@ */ #define DEBUG_MODULE "BUZZER" -#include - #include "buzzer.h" static struct buzzerControl * ctrl; @@ -40,10 +38,12 @@ bool buzzerTest() return true; } -void buzzerOff() { +void buzzerOff() +{ if (ctrl) ctrl->off(); } + void buzzerOn(uint32_t freq) { if (ctrl) diff --git a/hal/src/imu_cf2.c b/hal/src/imu_cf2.c index b604a890ed..f094447d61 100644 --- a/hal/src/imu_cf2.c +++ b/hal/src/imu_cf2.c @@ -45,6 +45,7 @@ #include "ledseq.h" #include "param.h" #include "log.h" +#include "sound.h" #define IMU_ENABLE_PRESSURE_LPS25H #define IMU_ENABLE_MAG_AK8963 @@ -328,6 +329,7 @@ void imu6Read(Axis3f* gyroOut, Axis3f* accOut) imuFindBiasValue(&gyroBias); if (gyroBias.isBiasValueFound) { + soundSetEffect(SND_CALIB); ledseqRun(SYS_LED, seq_calibrated); } } diff --git a/hal/src/pm_f405.c b/hal/src/pm_f405.c index 77e3e2f655..ee67d7196d 100644 --- a/hal/src/pm_f405.c +++ b/hal/src/pm_f405.c @@ -40,6 +40,7 @@ #include "adc.h" #include "ledseq.h" #include "commander.h" +#include "sound.h" typedef struct _PmSyslinkInfo { @@ -286,21 +287,25 @@ void pmTask(void *param) case charged: ledseqStop(CHG_LED, seq_charging); ledseqRun(CHG_LED, seq_charged); + soundSetEffect(SND_BAT_FULL); systemSetCanFly(false); break; case charging: ledseqStop(LOWBAT_LED, seq_lowbat); ledseqStop(CHG_LED, seq_charged); ledseqRun(CHG_LED, seq_charging); + soundSetEffect(SND_USB_CONN); systemSetCanFly(false); break; case lowPower: ledseqRun(LOWBAT_LED, seq_lowbat); + soundSetEffect(SND_BAT_LOW); systemSetCanFly(true); break; case battery: ledseqStop(CHG_LED, seq_charging); ledseqRun(CHG_LED, seq_charged); + soundSetEffect(SND_USB_DISC); systemSetCanFly(true); break; default: diff --git a/modules/interface/sound.h b/modules/interface/sound.h index 2b58fb0447..ba8c4cd364 100644 --- a/modules/interface/sound.h +++ b/modules/interface/sound.h @@ -27,14 +27,36 @@ #ifndef __SOUND_H__ #define __SOUND_H__ +#include #include +#define SND_OFF 0 +#define SND_USB_DISC 1 +#define SND_USB_CONN 2 +#define SND_BAT_FULL 3 +#define SND_BAT_LOW 4 +#define SND_STARTUP 5 +#define SND_CALIB 6 + /** - * Initialize the platform CRTP port + * Initialize sound sub-system. */ void soundInit(void); +/** + * Test the sound sub-system. + */ bool soundTest(void); +/** + * Set the effect that should be played by the sound sub-system. + */ +void soundSetEffect(uint32_t effect); + +/** + * Manufally set a frequency that should be played by the sound syb-system. + */ +void soundSetFreq(uint32_t freq); + #endif /* __SOUND_H__ */ diff --git a/modules/src/sound.c b/modules/src/sound.c deleted file mode 100644 index 98d21a9aa1..0000000000 --- a/modules/src/sound.c +++ /dev/null @@ -1,313 +0,0 @@ -/** - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ - * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ - * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ - * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ - * - * Crazyflie control firmware - * - * Copyright (C) 2015 Bitcraze AB - * - * 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, in version 3. - * - * 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 . - * - * sound.c - Module used to play melodies and system sounds though a buzzer - */ - -#include - -/* FreeRtos includes */ -#include -#include -#include - -#include "FreeRTOS.h" -#include "timers.h" - -#include "config.h" -#include "param.h" -#include "log.h" -#include "sound.h" -#include "buzzer.h" - -/** - * Credit to http://tny.cz/e525c1b2 for supplying the tones - */ -#define C0 16 -#define Db0 17 -#define D0 18 -#define Eb0 19 -#define E0 20 -#define F0 21 -#define Gb0 23 -#define G0 24 -#define Ab0 25 -#define A0 27 -#define Bb0 29 -#define B0 30 -#define C1 32 -#define Db1 34 -#define D1 36 -#define Eb1 38 -#define E1 41 -#define F1 43 -#define Gb1 46 -#define G1 49 -#define Ab1 51 -#define A1 55 -#define Bb1 58 -#define B1 61 -#define C2 65 -#define Db2 69 -#define D2 73 -#define Eb2 77 -#define E2 82 -#define F2 87 -#define Gb2 92 -#define G2 98 -#define Ab2 103 -#define A2 110 -#define Bb2 116 -#define B2 123 -#define C3 130 -#define Db3 138 -#define D3 146 -#define Eb3 155 -#define E3 164 -#define F3 174 -#define Gb3 185 -#define G3 196 -#define Ab3 207 -#define A3 220 -#define Bb3 233 -#define B3 246 -#define C4 261 -#define Db4 277 -#define D4 293 -#define Eb4 311 -#define E4 329 -#define F4 349 -#define Gb4 369 -#define G4 392 -#define Ab4 415 -#define A4 440 -#define Bb4 466 -#define B4 493 -#define C5 523 -#define Db5 554 -#define D5 587 -#define Eb5 622 -#define E5 659 -#define F5 698 -#define Gb5 739 -#define G5 783 -#define Ab5 830 -#define A5 880 -#define Bb5 932 -#define B5 987 -#define C6 1046 -#define Db6 1108 -#define D6 1174 -#define Eb6 1244 -#define E6 1318 -#define F6 1396 -#define Gb6 1479 -#define G6 1567 -#define Ab6 1661 -#define A6 1760 -#define Bb6 1864 -#define B6 1975 -#define C7 2093 -#define Db7 2217 -#define D7 2349 -#define Eb7 2489 -#define E7 2637 -#define F7 2793 -#define Gb7 2959 -#define G7 3135 -#define Ab7 3322 -#define A7 3520 -#define Bb7 3729 -#define B7 3951 -#define C8 4186 -#define Db8 4434 -#define D8 4698 -#define Eb8 4978 -/* Duration of notes */ -#define W (60) -#define H (W * 2) -#define Q (W * 4) -#define E (W * 8) -#define S (W * 16) -#define ES (W*6) - -static bool isInit=false; - -typedef void (*BuzzerEffect)(uint32_t timer); - -typedef struct { - uint16_t tone; - uint16_t duration; -} Note; - -typedef struct { - uint32_t bpm; - uint32_t ni; - uint32_t delay; - Note notes[80]; -} Melody; - -Melody melodies[] = { - {.bpm = 120, .ni = 0, .delay = 1, .notes = {{C4, H}, {D4, H}, {E4, H}, {F4, H}, {G4, H}, {A4, H}, {B4, H}, {0xFF, 0}}}, - {.bpm = 120, .ni = 0, .delay = 1, .notes = {{C4, S}, {D4, S}, {E4, S}, {F4, S}, {G4, S}, {A4, S}, {B4, S}, {0xFF, 0}}}, - /* Imperial march from http://tny.cz/e525c1b2A */ - {.bpm = 120, .ni = 0, .delay = 1, .notes = {{A3, Q}, {A3, Q}, {A3, Q},{F3, ES}, {C4, S}, - {A3, Q}, {F3, ES}, {C4, S}, {A3, H}, - {E4, Q}, {E4, Q}, {E4, Q}, {F4, ES}, {C4, S}, - {Ab3, Q}, {F3, ES}, {C4, S}, {A3, H}, - {A4, Q}, {A3, ES}, {A3, S}, {A4, Q}, {Ab4, ES}, {G4, S}, - {Gb4, S}, {E4, S}, {F4, E}, {0, E}, {Bb3, E}, {Eb4, Q}, {D4, ES}, {Db4, S}, - {C4, S}, {B3, S}, {C4, E}, {0, E}, {F3, E}, {Ab3, Q}, {F3, ES}, {A3, S}, - {C4, Q}, {A3, ES}, {C4, S}, {E4, H}, - {A4, Q}, {A3, ES}, {A3, S}, {A4, Q}, {Ab4, ES}, {G4, S}, - {Gb4, S}, {E4, S}, {F4, E}, {0, E}, {Bb3, E}, {Eb4, Q}, {D4, ES}, {Db4, S}, - {Gb4, S}, {E4, S}, {F4, E}, {0, E}, {Bb3, E}, {Eb4, Q}, {D4, ES}, {Db4, S}, - {C4,S}, {B3, S}, {C4, E}, {0, E}, {F3, E}, {Ab3, Q}, {F3, ES}, {C4, S}, - {A3, Q}, {F3, ES}, {C4, S}, {A3, H}, {0, H}, - {0xFF, 0}}} - -}; - -uint8_t melody = 2; -uint8_t nmelody = 3; -unsigned int mcounter = 0; -static bool playing_sound = false; -static void melodyplayer(uint32_t counter) { - // Sync one melody for the loop - Melody* m = &melodies[melody]; - if (mcounter == 0) { - if (m->notes[m->ni].tone == 0xFF) - { - // Loop the melody - m->ni = 0; - } - // Play current note - buzzerOn(m->notes[m->ni].tone); - mcounter = (m->bpm * 100) / m->notes[m->ni].duration; - playing_sound = true; - m->ni++; - } - else if (mcounter == 1) - { - buzzerOff(); - } - mcounter--; -} - -uint8_t static_ratio = 0; -uint16_t static_freq = 4000; -static void bypass(uint32_t counter) -{ - /* Just set the params from the host */ - buzzerOn(static_freq); -} - -uint16_t siren_start = 2000; -uint16_t siren_freq = 2000; -uint16_t siren_stop = 4000; -int16_t siren_step = 40; -static void siren(uint32_t counter) -{ - siren_freq += siren_step; - if (siren_freq > siren_stop) - { - siren_step *= -1; - siren_freq = siren_stop; - } - if (siren_freq < siren_start) - { - siren_step *= -1; - siren_freq = siren_start; - } - buzzerOn(siren_freq); -} - - -static int pitchid; -static int rollid; -static int pitch; -static int roll; -static int tilt_freq; -static int tilt_ratio; -static void tilt(uint32_t counter) -{ - pitchid = logGetVarId("stabilizer", "pitch"); - rollid = logGetVarId("stabilizer", "roll"); - - pitch = logGetInt(pitchid); - roll = logGetInt(rollid); - tilt_freq = 0; - tilt_ratio = 127; - - if (abs(pitch) > 5) { - tilt_freq = 3000 - 50 * pitch; - } - - buzzerOn(tilt_freq); -} - - -unsigned int neffect = 0; -unsigned int effect = 3; -static BuzzerEffect effects[] = {bypass, siren, melodyplayer, tilt}; - -static xTimerHandle timer; -static uint32_t counter = 0; - -static void soundTimer(xTimerHandle timer) -{ - counter++; - - if (effects[effect] != 0) - effects[effect](counter*10); -} - -void soundInit(void) -{ - if (isInit) - return; - - neffect = sizeof(effects)/sizeof(effects[0])-1; - nmelody = sizeof(melodies)/sizeof(melodies[0])-1; - - timer = xTimerCreate( (const signed char *)"SoundTimer", M2T(10), - pdTRUE, NULL, soundTimer ); - xTimerStart(timer, 100); - - isInit = true; -} - -bool soundTest(void) -{ - return isInit; -} - -PARAM_GROUP_START(sound) -PARAM_ADD(PARAM_UINT8, effect, &effect) -PARAM_ADD(PARAM_UINT32 | PARAM_RONLY, neffect, &neffect) -PARAM_ADD(PARAM_UINT8, melody, &melody) -PARAM_ADD(PARAM_UINT32 | PARAM_RONLY, nmelody, &melody) -PARAM_ADD(PARAM_UINT16, freq, &static_freq) -PARAM_ADD(PARAM_UINT8, ratio, &static_ratio) -PARAM_GROUP_STOP(sound) - diff --git a/modules/src/sound_cf1.c b/modules/src/sound_cf1.c new file mode 100644 index 0000000000..7dc93d23d1 --- /dev/null +++ b/modules/src/sound_cf1.c @@ -0,0 +1,52 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2015 Bitcraze AB + * + * 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, in version 3. + * + * 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 . + * + * sound_cf1.c - Empty module used on Crazyflie 2.0 to play melodies and + * system sounds + */ + +#include +#include + +static bool isInit=false; + +void soundInit(void) +{ + if (isInit) + return; + + isInit = true; +} + +bool soundTest(void) +{ + return isInit; +} + +void soundSetEffect(uint32_t effect) +{ +} + +void soundSetFreq(uint32_t freq) +{ +} diff --git a/modules/src/sound_cf2.c b/modules/src/sound_cf2.c new file mode 100644 index 0000000000..f6b41be84a --- /dev/null +++ b/modules/src/sound_cf2.c @@ -0,0 +1,354 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2015 Bitcraze AB + * + * 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, in version 3. + * + * 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 . + * + * sound_cf2.c - Module used to play melodies and system sounds though a buzzer + */ + +#include + +/* FreeRtos includes */ +#include +#include +#include + +#include "FreeRTOS.h" +#include "timers.h" + +#include "config.h" +#include "param.h" +#include "log.h" +#include "sound.h" +#include "buzzer.h" + +/** + * Credit to http://tny.cz/e525c1b2 for supplying the tones + */ +#define C0 16 +#define Db0 17 +#define D0 18 +#define Eb0 19 +#define E0 20 +#define F0 21 +#define Gb0 23 +#define G0 24 +#define Ab0 25 +#define A0 27 +#define Bb0 29 +#define B0 30 +#define C1 32 +#define Db1 34 +#define D1 36 +#define Eb1 38 +#define E1 41 +#define F1 43 +#define Gb1 46 +#define G1 49 +#define Ab1 51 +#define A1 55 +#define Bb1 58 +#define B1 61 +#define C2 65 +#define Db2 69 +#define D2 73 +#define Eb2 77 +#define E2 82 +#define F2 87 +#define Gb2 92 +#define G2 98 +#define Ab2 103 +#define A2 110 +#define Bb2 116 +#define B2 123 +#define C3 130 +#define Db3 138 +#define D3 146 +#define Eb3 155 +#define E3 164 +#define F3 174 +#define Gb3 185 +#define G3 196 +#define Ab3 207 +#define A3 220 +#define Bb3 233 +#define B3 246 +#define C4 261 +#define Db4 277 +#define D4 293 +#define Eb4 311 +#define E4 329 +#define F4 349 +#define Gb4 369 +#define G4 392 +#define Ab4 415 +#define A4 440 +#define Bb4 466 +#define B4 493 +#define C5 523 +#define Db5 554 +#define D5 587 +#define Eb5 622 +#define E5 659 +#define F5 698 +#define Gb5 739 +#define G5 783 +#define Ab5 830 +#define A5 880 +#define Bb5 932 +#define B5 987 +#define C6 1046 +#define Db6 1108 +#define D6 1174 +#define Eb6 1244 +#define E6 1318 +#define F6 1396 +#define Gb6 1479 +#define G6 1567 +#define Ab6 1661 +#define A6 1760 +#define Bb6 1864 +#define B6 1975 +#define C7 2093 +#define Db7 2217 +#define D7 2349 +#define Eb7 2489 +#define E7 2637 +#define F7 2793 +#define Gb7 2959 +#define G7 3135 +#define Ab7 3322 +#define A7 3520 +#define Bb7 3729 +#define B7 3951 +#define C8 4186 +#define Db8 4434 +#define D8 4698 +#define Eb8 4978 +/* Duration of notes */ +#define W (60) +#define H (W * 2) +#define Q (W * 4) +#define E (W * 8) +#define S (W * 16) +#define ES (W*6) + +#define MAX_NOTE_LENGTH 80 + +static bool isInit=false; + +typedef const struct { + uint16_t tone; + uint16_t duration; +} Note; + +typedef const struct { + uint32_t bpm; + uint32_t delay; + Note notes[MAX_NOTE_LENGTH]; +} Melody; + +static uint32_t neffect = 0; +static uint32_t sys_effect = 0; +static uint32_t user_effect = 0; + +static Melody range_slow = {.bpm = 120, .delay = 1, .notes = {{C4, H}, {D4, H}, {E4, H}, {F4, H}, {G4, H}, {A4, H}, {B4, H}, {0xFF, 0}}}; +static Melody range_fast = {.bpm = 120, .delay = 1, .notes = {{C4, S}, {D4, S}, {E4, S}, {F4, S}, {G4, S}, {A4, S}, {B4, S}, {0xFF, 0}}}; +static Melody startup = {.bpm = 120, .delay = 1, .notes = {{C4, S}, {D4, S}, {E4, S}, {F4, S}, {0xFE, 0}}}; +static Melody calibrated = {.bpm = 120, .delay = 1, .notes = {{C4, S}, {D4, S}, {E4, S}, {F4, S}, {0xFE, 0}}}; +static Melody chg_done = {.bpm = 120, .delay = 1, .notes = {{D4, Q}, {A4, Q}, {0xFE, 0}}}; +static Melody lowbatt = {.bpm = 120, .delay = 1, .notes = {{D4, E}, {A4, E}, {D4, E}, {0xFF, 0}}}; +static Melody usb_disconnect = {.bpm = 120, .delay = 1, .notes = {{C4, E}, {0xFE, 0}}}; +static Melody usb_connect = {.bpm = 120, .delay = 1, .notes = {{A4, E}, {0xFE, 0}}}; +/* Imperial march from http://tny.cz/e525c1b2A */ +static Melody starwars = {.bpm = 120, .delay = 1, .notes = {{A3, Q}, {A3, Q}, {A3, Q},{F3, ES}, {C4, S}, + {A3, Q}, {F3, ES}, {C4, S}, {A3, H}, + {E4, Q}, {E4, Q}, {E4, Q}, {F4, ES}, {C4, S}, + {Ab3, Q}, {F3, ES}, {C4, S}, {A3, H}, + {A4, Q}, {A3, ES}, {A3, S}, {A4, Q}, {Ab4, ES}, {G4, S}, + {Gb4, S}, {E4, S}, {F4, E}, {0, E}, {Bb3, E}, {Eb4, Q}, {D4, ES}, {Db4, S}, + {C4, S}, {B3, S}, {C4, E}, {0, E}, {F3, E}, {Ab3, Q}, {F3, ES}, {A3, S}, + {C4, Q}, {A3, ES}, {C4, S}, {E4, H}, + {A4, Q}, {A3, ES}, {A3, S}, {A4, Q}, {Ab4, ES}, {G4, S}, + {Gb4, S}, {E4, S}, {F4, E}, {0, E}, {Bb3, E}, {Eb4, Q}, {D4, ES}, {Db4, S}, + {Gb4, S}, {E4, S}, {F4, E}, {0, E}, {Bb3, E}, {Eb4, Q}, {D4, ES}, {Db4, S}, + {C4,S}, {B3, S}, {C4, E}, {0, E}, {F3, E}, {Ab3, Q}, {F3, ES}, {C4, S}, + {A3, Q}, {F3, ES}, {C4, S}, {A3, H}, {0, H}, + {0xFF, 0}}}; + +typedef void (*BuzzerEffect)(uint32_t timer, uint32_t * mi, Melody * melody); + +static void off(uint32_t counter, uint32_t * mi, Melody * m) { + buzzerOff(); +} + +static uint32_t mcounter = 0; +static void melodyplayer(uint32_t counter, uint32_t * mi, Melody * m) { + if (m->notes[(*mi)].tone == 0xFE) { + // Turn off buzzer since we're at the end + (*mi) = 0; + if (sys_effect != 0) + sys_effect = 0; + else + user_effect = 0; + } else if (mcounter == 0) { + if (m->notes[(*mi)].tone == 0xFF) + { + // Loop the melody + (*mi) = 0; + } + // Play current note + buzzerOn(m->notes[(*mi)].tone); + mcounter = (m->bpm * 100) / m->notes[(*mi)].duration; + (*mi)++; + } + else if (mcounter == 1) + { + buzzerOff(); + } + mcounter--; +} + +static uint8_t static_ratio = 0; +static uint16_t static_freq = 4000; +static void bypass(uint32_t counter, uint32_t * mi, Melody * melody) +{ + buzzerOn(static_freq); +} + +static uint16_t siren_start = 2000; +static uint16_t siren_freq = 2000; +static uint16_t siren_stop = 4000; +static int16_t siren_step = 40; +static void siren(uint32_t counter, uint32_t * mi, Melody * melody) +{ + siren_freq += siren_step; + if (siren_freq > siren_stop) + { + siren_step *= -1; + siren_freq = siren_stop; + } + if (siren_freq < siren_start) + { + siren_step *= -1; + siren_freq = siren_start; + } + buzzerOn(siren_freq); +} + +static int pitchid; +static int rollid; +static int pitch; +static int roll; +static int tilt_freq; +static int tilt_ratio; +static void tilt(uint32_t counter, uint32_t * mi, Melody * melody) +{ + pitchid = logGetVarId("stabilizer", "pitch"); + rollid = logGetVarId("stabilizer", "roll"); + + pitch = logGetInt(pitchid); + roll = logGetInt(rollid); + tilt_freq = 0; + tilt_ratio = 127; + + if (abs(pitch) > 5) { + tilt_freq = 3000 - 50 * pitch; + } + + buzzerOn(tilt_freq); +} + +typedef struct { + BuzzerEffect call; + uint32_t mi; + Melody * melody; +} EffectCall; + +static EffectCall effects[] = { + [SND_OFF] = {.call = &off}, + [SND_USB_CONN] = {.call = &melodyplayer, .melody = &usb_connect}, + [SND_USB_DISC] = {.call = &melodyplayer, .melody = &usb_disconnect}, + [SND_BAT_FULL] = {.call = &melodyplayer, .melody = &chg_done}, + [SND_BAT_LOW] = {.call = &melodyplayer, .melody = &lowbatt}, + [SND_STARTUP] = {.call = &melodyplayer, .melody = &startup}, + [SND_CALIB] = {.call = &melodyplayer, .melody = &calibrated}, + {.call = &melodyplayer, .melody = &range_slow}, + {.call = &melodyplayer, .melody = &range_fast}, + {.call = &melodyplayer, .melody = &starwars}, + {.call = &bypass}, + {.call = &siren}, + {.call = &tilt} +}; + +static xTimerHandle timer; +static uint32_t counter = 0; + +static void soundTimer(xTimerHandle timer) +{ + int effect; + counter++; + + if (sys_effect != 0) { + effect = sys_effect; + } else { + effect = user_effect; + } + if (effects[effect].call != 0) + effects[effect].call(counter*10, &effects[effect].mi, + effects[effect].melody); +} + +void soundInit(void) +{ + if (isInit) + return; + + neffect = sizeof(effects)/sizeof(effects[0])-1; + + timer = xTimerCreate( (const signed char *)"SoundTimer", M2T(10), + pdTRUE, NULL, soundTimer); + xTimerStart(timer, 100); + + isInit = true; +} + +bool soundTest(void) +{ + return isInit; +} + +void soundSetEffect(uint32_t effect) +{ + sys_effect = effect; +} + +void soundSetFreq(uint32_t freq) { + +} + +PARAM_GROUP_START(sound) +PARAM_ADD(PARAM_UINT8, effect, &user_effect) +PARAM_ADD(PARAM_UINT32 | PARAM_RONLY, neffect, &neffect) +PARAM_ADD(PARAM_UINT16, freq, &static_freq) +PARAM_ADD(PARAM_UINT8, ratio, &static_ratio) +PARAM_GROUP_STOP(sound) + diff --git a/modules/src/system.c b/modules/src/system.c index 2a97bd829e..0e453544ef 100644 --- a/modules/src/system.c +++ b/modules/src/system.c @@ -57,11 +57,11 @@ #include "proximity.h" #include "watchdog.h" #include "queuemonitor.h" +#include "buzzer.h" +#include "sound.h" #ifdef PLATFORM_CF2 #include "deck.h" -#include "buzzer.h" -#include "sound.h" #endif /* Private variable */ @@ -110,9 +110,8 @@ void systemInit(void) adcInit(); ledseqInit(); pmInit(); -#ifdef PLATFORM_CF2 buzzerInit(); -#endif + isInit = true; } @@ -126,9 +125,7 @@ bool systemTest() pass &= ledseqTest(); pass &= pmTest(); pass &= workerTest(); -#ifdef PLATFORM_CF2 pass &= buzzerTest(); -#endif return pass; } @@ -170,8 +167,8 @@ void systemTask(void *arg) stabilizerInit(); #ifdef PLATFORM_CF2 deckInit(); - soundInit(); #endif + soundInit(); memInit(); #ifdef PROXIMITY_ENABLED @@ -186,8 +183,8 @@ void systemTask(void *arg) pass &= stabilizerTest(); #ifdef PLATFORM_CF2 pass &= deckTest(); - pass &= soundTest(); #endif + pass &= soundTest(); pass &= memTest(); pass &= watchdogNormalStartTest(); @@ -196,6 +193,7 @@ void systemTask(void *arg) { selftestPassed = 1; systemStart(); + soundSetEffect(SND_STARTUP); ledseqRun(SYS_LED, seq_alive); ledseqRun(LINK_LED, seq_testPassed); } From 3820974eb85f672a9ab7bb244fc1a3062a4d60e6 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Wed, 25 Nov 2015 14:47:04 +0100 Subject: [PATCH 55/58] Updated FreeRTOS to v8.2.3 This update revealed miss-configuration of the NVIC interrupt group by the USB driver. It is fixed to the point that the system starts though the USB is now broken. --- config/FreeRTOSConfig.h | 3 +- config/stm32f40x_i2c_cpal_conf.h | 324 +- config/trace.h | 8 +- deck/drivers/src/buzzer.c | 317 ++ deck/drivers/src/ledring12.c | 2 +- drivers/src/adc_f103.c | 6 +- drivers/src/i2cdev_f405.c | 2 +- hal/src/eskylink.c | 46 +- hal/src/ledseq.c | 2 +- hal/src/nrf24link.c | 4 +- hal/src/pm_f103.c | 2 +- hal/src/pm_f405.c | 2 +- hal/src/proximity.c | 2 +- hal/src/syslink.c | 2 +- hal/src/uart.c | 4 +- hal/src/usb_bsp.c | 188 +- hal/src/usblink.c | 2 +- lib/FreeRTOS/croutine.c | 174 +- lib/FreeRTOS/event_groups.c | 683 ++++ lib/FreeRTOS/include/FreeRTOS.h | 438 +- lib/FreeRTOS/include/StackMacros.h | 194 +- lib/FreeRTOS/include/croutine.h | 213 +- lib/FreeRTOS/include/deprecated_definitions.h | 321 ++ lib/FreeRTOS/include/event_groups.h | 730 ++++ lib/FreeRTOS/include/list.h | 291 +- lib/FreeRTOS/include/mpu_wrappers.h | 120 +- lib/FreeRTOS/include/portable.h | 390 +- lib/FreeRTOS/include/projdefs.h | 178 +- lib/FreeRTOS/include/queue.h | 673 ++-- lib/FreeRTOS/include/semphr.h | 453 ++- lib/FreeRTOS/include/task.h | 1232 ++++-- lib/FreeRTOS/include/timers.h | 560 ++- lib/FreeRTOS/list.c | 218 +- lib/FreeRTOS/portable/GCC/ARM_CM3/port.c | 500 ++- lib/FreeRTOS/portable/GCC/ARM_CM3/portmacro.h | 143 +- lib/FreeRTOS/portable/GCC/ARM_CM4F/port.c | 535 ++- .../portable/GCC/ARM_CM4F/portmacro.h | 221 +- lib/FreeRTOS/portable/MemMang/heap_1.c | 105 +- lib/FreeRTOS/portable/MemMang/heap_2.c | 146 +- lib/FreeRTOS/portable/MemMang/heap_3.c | 105 +- lib/FreeRTOS/portable/MemMang/heap_4.c | 403 +- lib/FreeRTOS/portable/MemMang/heap_5.c | 523 +++ lib/FreeRTOS/queue.c | 1431 +++++-- lib/FreeRTOS/tasks.c | 3505 ++++++++++++----- lib/FreeRTOS/timers.c | 648 ++- modules/src/crtp.c | 4 +- modules/src/info.c | 2 +- modules/src/log.c | 4 +- modules/src/mem.c | 2 +- modules/src/param.c | 2 +- modules/src/pidctrl.c | 2 +- modules/src/queuemonitor.c | 2 +- modules/src/stabilizer.c | 2 +- modules/src/system.c | 10 +- 54 files changed, 11579 insertions(+), 4500 deletions(-) create mode 100644 deck/drivers/src/buzzer.c create mode 100644 lib/FreeRTOS/event_groups.c create mode 100644 lib/FreeRTOS/include/deprecated_definitions.h create mode 100644 lib/FreeRTOS/include/event_groups.h create mode 100644 lib/FreeRTOS/portable/MemMang/heap_5.c diff --git a/config/FreeRTOSConfig.h b/config/FreeRTOSConfig.h index dffa3f85a6..fae712f863 100644 --- a/config/FreeRTOSConfig.h +++ b/config/FreeRTOSConfig.h @@ -88,7 +88,7 @@ #define configUSE_MALLOC_FAILED_HOOK 1 #define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE -#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 6 ) +#define configMAX_PRIORITIES ( 6 ) #define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) /* Set the following definitions to 1 to include the API function, or zero @@ -130,6 +130,7 @@ to exclude the API function. */ #define TASK_PROXIMITY_ID_NBR 6 #define configASSERT( x ) if( ( x ) == 0 ) assertFail(#x, __FILE__, __LINE__ ) + /* #define traceTASK_SWITCHED_IN() \ { \ diff --git a/config/stm32f40x_i2c_cpal_conf.h b/config/stm32f40x_i2c_cpal_conf.h index bef99414ce..164678d042 100644 --- a/config/stm32f40x_i2c_cpal_conf.h +++ b/config/stm32f40x_i2c_cpal_conf.h @@ -1,6 +1,6 @@ /** ****************************************************************************** - * @file cpal_conf_template.h + * @file cpal_conf_template.h * @author MCD Application Team * @version V1.2.0 * @date 21-December-2012 @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. @@ -31,7 +31,7 @@ #ifdef __cplusplus extern "C" { -#endif +#endif /* Includes ------------------------------------------------------------------*/ /* Exported types ------------------------------------------------------------*/ @@ -40,24 +40,24 @@ /*======================================================================================================================================= User NOTES ========================================================================================================================================= - + ------------------------------- 1. How To use the CPAL Library: ------------------------------- -------- Refer to the user manual of the library and (eventually) the example to check if +------- Refer to the user manual of the library and (eventually) the example to check if this firmware is appropriate for your hardware (device and (eventually) evaluation board). - + - Section 1 : Select the Device instances to be used and the total number of devices. - + - Section 2 : Configure Transfer Options. - + - Section 3 : Select and configure transfer and error user Callbacks. - Section 4 : Configure Timeout mechanism and TimeoutCallback. - + - Section 5 : NVIC Priority Group Selection and Interrupt Priority Offset. - + - Section 6 : Configure CPAL_LOG Macro. ------ After configuring CPAL firmware functionality , You should proceed by configuring hardware used with CPAL @@ -65,22 +65,22 @@ ------ After configuring CPAL Firmware Library, you should follow these steps to use the Firmware correctly : - -1- STRUCTURE INITIALIZATION - Start by initializing the Device. To perform this action, the global variable PPPx_DevStructure declared + -1- STRUCTURE INITIALIZATION + Start by initializing the Device. To perform this action, the global variable PPPx_DevStructure declared in CPAL Firmware as CPAL_InitTypeDef (I2C1_DevStructure for I2C1, I2C2_DevStructure for I2C2 ...) must be used . There are two ways to proceed : - + ** Call the function CPAL_PPP_StructInit() using as parameter PPPx_DevStructure (where PPP = device type (ie. I2C...) - and where x could be 1 for PPP1, 2 for PPP2 ...). This function sets the default values for all fields of this structure. - + and where x could be 1 for PPP1, 2 for PPP2 ...). This function sets the default values for all fields of this structure. + Default values for I2C devices are : - I2Cx_DevStructure.CPAL_Direction = CPAL_DIRECTION_TXRX - I2Cx_DevStructure.CPAL_Mode = CPAL_MODE_MASTER - I2Cx_DevStructure.CPAL_ProgModel = CPAL_PROGMODEL_DMA + I2Cx_DevStructure.CPAL_Direction = CPAL_DIRECTION_TXRX + I2Cx_DevStructure.CPAL_Mode = CPAL_MODE_MASTER + I2Cx_DevStructure.CPAL_ProgModel = CPAL_PROGMODEL_DMA I2Cx_DevStructure.pCPAL_TransferTx = pNULL I2Cx_DevStructure.pCPAL_TransferRx = pNULL - I2Cx_DevStructure.CPAL_State = CPAL_STATE_DISABLED - I2Cx_DevStructure.wCPAL_DevError = CPAL_I2C_ERR_NONE + I2Cx_DevStructure.CPAL_State = CPAL_STATE_DISABLED + I2Cx_DevStructure.wCPAL_DevError = CPAL_I2C_ERR_NONE I2Cx_DevStructure.wCPAL_Options = 0 (all options disabled) I2Cx_DevStructure.wCPAL_Timeout = CPAL_TIMEOUT_DEFAULT I2Cx_DevStructure.pCPAL_I2C_Struct->I2C_ClockSpeed = 100000 @@ -88,26 +88,26 @@ I2Cx_DevStructure.pCPAL_I2C_Struct->I2C_DutyCycle = I2C_DutyCycle_2 I2Cx_DevStructure.pCPAL_I2C_Struct->I2C_OwnAddress1 = 0 I2Cx_DevStructure.pCPAL_I2C_Struct->I2C_Ack = I2C_Ack_Enable - I2Cx_DevStructure.pCPAL_I2C_Struct->I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit - - - pCPAL_TransferTx and pCPAL_TransferRx fields have to be updated in order to point to valid structures + I2Cx_DevStructure.pCPAL_I2C_Struct->I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit + + + pCPAL_TransferTx and pCPAL_TransferRx fields have to be updated in order to point to valid structures (these structures should be local/global variables in the user application). - - ** Another way of configuration is without calling CPAL_PPP_StructInit() function. + + ** Another way of configuration is without calling CPAL_PPP_StructInit() function. Declare the following structures: - A PPP_InitTypeDef structure for the device configuration (ie. I2C_InitTypeDef structure) - One or two CPAL_TransferTypeDef variables (one for Tx and one for Rx). - - Use the extern structure provided by the CPAL library: PPPx_InitStructure (ie. I2C1_DevStructure). + - Use the extern structure provided by the CPAL library: PPPx_InitStructure (ie. I2C1_DevStructure). Fill in all the fields for these structures (one by one). - Use the pointers to these structures to fill in the fields pCPAL_PPP_Struct and pCPAL_TransferTx and/or - pCPAL_TransferRx of the PPPx_DevStructure. - After that CPAL_State must be set to CPAL_STATE_DISABLED. + Use the pointers to these structures to fill in the fields pCPAL_PPP_Struct and pCPAL_TransferTx and/or + pCPAL_TransferRx of the PPPx_DevStructure. + After that CPAL_State must be set to CPAL_STATE_DISABLED. Finally, call the CPAL_PPP_Init() with pointer to the PPPx_DevStructure as argument. - + Example: - // Declare local structures - I2C_InitTypeDef I2C1_InitStructure; + // Declare local structures + I2C_InitTypeDef I2C1_InitStructure; CPAL_TransferTypeDef TX_Transfer , RX_Transfer ; // Fill in all the fields of to these structures I2C1_InitStructure.I2C_ClockSpeed = 5000; @@ -122,54 +122,54 @@ ..... // Use these structures and fill all fields of I2C1_DevStructure. I2C1_DevStructure.CPAL_Dev = CPAL_I2C1; - I2C1_DevStructure.CPAL_Direction = CPAL_DIRECTION_TXRX; - I2C1_DevStructure.CPAL_Mode = CPAL_MODE_SLAVE; + I2C1_DevStructure.CPAL_Direction = CPAL_DIRECTION_TXRX; + I2C1_DevStructure.CPAL_Mode = CPAL_MODE_SLAVE; I2C1_DevStructure.wCPAL_Options = CPAL_OPT_DMATX_HTIT ; ..... I2C1_DevStructure.pCPAL_TransferTx = &TX_Transfer ; I2C1_DevStructure.pCPAL_TransferRx = &RX_Transfer ; - I2C1_DevStructure.pCPAL_I2C_Struct = &I2C1_InitStructure; + I2C1_DevStructure.pCPAL_I2C_Struct = &I2C1_InitStructure; ... I2C1_DevStructure.wCPAL_State = CPAL_STATE_DISABLED; .... CPAL_I2C_Init(&I2C1_DevStructure); - - -2- DEVICE CONFIGURATION - Call the function CPAL_PPP_Init() to configure the selected device with the selected configuration by calling + + -2- DEVICE CONFIGURATION + Call the function CPAL_PPP_Init() to configure the selected device with the selected configuration by calling CPAL_PPP_Init(). This function also enables device clock and initialize all related peripherals ( GPIO, DMA , IT and NVIC ). - This function tests on CPAL_State, if it is equal to CPAL_STATE_BUSY it exit, otherwise device initialization is + This function tests on CPAL_State, if it is equal to CPAL_STATE_BUSY it exit, otherwise device initialization is performed and CPAL_State is set to CPAL_STATE_READY. This function returns CPAL_PASS state when the operation is correctly performed, or CPAL_FAIL when the current state of the device doesn't allow configuration (ie. state different from READY, DISABLED or ERROR). - After calling this function, you may check on the new state of device, when it is equal to CPAL_STATE_READY, Transfer operations + After calling this function, you may check on the new state of device, when it is equal to CPAL_STATE_READY, Transfer operations can be started, otherwise you can call CPAL_PPP_DeInit() to deinitialize device and call CPAL_PPP_Init() once again. - - -3- 1- READ / WRITE OPERATIONS - Call the function CPAL_PPP_Write() or CPAL_PPP_Read() to perform transfer operations. - These functions handle communication events using device event interrupts (independently of programming model used: DMA, - Interrupt). These functions start preparing communication (send start condition, send salve address in case of - master mode ...) if connection is established between devices CPAL_State is set CPAL_STATE_BUSY_XX and data transfer starts. - By default, Error interrupts are enabled to manage device errors (Error interrupts can be disabled by affecting - CPAL_OPT_I2C_ERRIT_DISABLE to wCPAL_Options). When transfer is completed successfully, CPAL_State is set to CPAL_STATE_READY + + -3- 1- READ / WRITE OPERATIONS + Call the function CPAL_PPP_Write() or CPAL_PPP_Read() to perform transfer operations. + These functions handle communication events using device event interrupts (independently of programming model used: DMA, + Interrupt). These functions start preparing communication (send start condition, send salve address in case of + master mode ...) if connection is established between devices CPAL_State is set CPAL_STATE_BUSY_XX and data transfer starts. + By default, Error interrupts are enabled to manage device errors (Error interrupts can be disabled by affecting + CPAL_OPT_I2C_ERRIT_DISABLE to wCPAL_Options). When transfer is completed successfully, CPAL_State is set to CPAL_STATE_READY and another operation can be started. These functions return CPAL_PASS if the current state of the device allows starting a new operation and the operation is correctly - started (but not finished). It returns CPAL_FAIL when the state of the device doesn't allow starting a new communication (ie. + started (but not finished). It returns CPAL_FAIL when the state of the device doesn't allow starting a new communication (ie. BUSY, DISABLED, ERROR) or when an error occurs during operation start. Once operation is started, user application may perform other tasks while CPAL is sending/receiving data on device through interrupt - or DMA. + or DMA. -3- 2- Listen Mode for Slave This mode allows slave device to start a communication without knowing in advance the nature of the operation (read or write). - Slave enter in idle state and wait until it receive its own address, CPAL_State is set CPAL_STATE_BUSY. In accordance to the type - of received request from master device, the slave device state is changed to CPAL_STATE_BUSY_RX and CPAL_I2C_SLAVE_READ_UserCallback + Slave enter in idle state and wait until it receive its own address, CPAL_State is set CPAL_STATE_BUSY. In accordance to the type + of received request from master device, the slave device state is changed to CPAL_STATE_BUSY_RX and CPAL_I2C_SLAVE_READ_UserCallback is called for a read request or state is changed to CPAL_STATE_BUSY_TX and CPAL_I2C_SLAVE_WRITE_UserCallback for a write request. In these callbacks user must configure DMA, interrupts (Prepare DMA channel, DMA request and I2C interrupts) and transfer parameters. - To configure DMA and Interrupts user can call "CPAL_I2C_Enable_DMA_IT" function which is implemented in Communication layer of CPAL Library. - Listen mode is enabled by uncommenting "CPAL_I2C_LISTEN_MODE" define in "cpal_conf.h" file. When this mode is enabled "CPAL_I2C_Read" and + To configure DMA and Interrupts user can call "CPAL_I2C_Enable_DMA_IT" function which is implemented in Communication layer of CPAL Library. + Listen mode is enabled by uncommenting "CPAL_I2C_LISTEN_MODE" define in "cpal_conf.h" file. When this mode is enabled "CPAL_I2C_Read" and "CPAL_I2C_Write" functions are replaced by one function "CPAL_I2C_Listen". - - -4- DEVICE DEINITIALIZATION - When transfer operations are finished, you may call CPAL_PPP_DeInit() to disable PPPx device and related resources + + -4- DEVICE DEINITIALIZATION + When transfer operations are finished, you may call CPAL_PPP_DeInit() to disable PPPx device and related resources ( GPIO, DMA , IT and NVIC). CPAL_State is then set to CPAL_STATE_DISABLED by this function. @@ -177,83 +177,83 @@ ------ Callbacks are routines that let you insert your own code in different stages of communication and for handling device errors. Their prototypes are declared by the CPAL library (if the relative define in this cpal_conf.h is enabled) but their body is not implemented by CPAL library. It may be done by user when needed. - There are three types of Callbacks: Transfer User Callbacks, Error User Callbacks and Timeout User Callbacks: - - -a- Transfer User Callbacks : + There are three types of Callbacks: Transfer User Callbacks, Error User Callbacks and Timeout User Callbacks: + + -a- Transfer User Callbacks : - ** CPAL_I2C_TX_UserCallback(CPAL_InitTypeDef* pDevInitStruct) + ** CPAL_I2C_TX_UserCallback(CPAL_InitTypeDef* pDevInitStruct) This function is called before sending data when Interrupt Programming Model is selected. - ** CPAL_I2C_RX_UserCallback(CPAL_InitTypeDef* pDevInitStruct) + ** CPAL_I2C_RX_UserCallback(CPAL_InitTypeDef* pDevInitStruct) This function is called after receiving data when Interrupt Programming Model is selected. - ** CPAL_I2C_TXTC_UserCallback(CPAL_InitTypeDef* pDevInitStruct) + ** CPAL_I2C_TXTC_UserCallback(CPAL_InitTypeDef* pDevInitStruct) ** CPAL_I2C_RXTC_UserCallback(CPAL_InitTypeDef* pDevInitStruct) - These functions are called when a transfer is complete when using Interrupt programming model or DMA + These functions are called when a transfer is complete when using Interrupt programming model or DMA programming model. - - ** CPAL_I2C_DMATXTC_UserCallback(CPAL_InitTypeDef* pDevInitStruct) - ** CPAL_I2C_DMATXTC_UserCallback(CPAL_InitTypeDef* pDevInitStruct) - These functions are called when Transfer complete Interrupt occurred in transmission/reception operation + + ** CPAL_I2C_DMATXTC_UserCallback(CPAL_InitTypeDef* pDevInitStruct) + ** CPAL_I2C_DMATXTC_UserCallback(CPAL_InitTypeDef* pDevInitStruct) + These functions are called when Transfer complete Interrupt occurred in transmission/reception operation if DMA Programming Model is selected - ** CPAL_I2C_DMATXHT_UserCallback(CPAL_InitTypeDef* pDevInitStruct) + ** CPAL_I2C_DMATXHT_UserCallback(CPAL_InitTypeDef* pDevInitStruct) ** CPAL_I2C_DMARXHT_UserCallback(CPAL_InitTypeDef* pDevInitStruct) - These functions are called when Half transfer Interrupt occurred in transmission/reception operation + These functions are called when Half transfer Interrupt occurred in transmission/reception operation if DMA Programming Model is selected. - ** CPAL_I2C_DMATXTE_UserCallback(CPAL_InitTypeDef* pDevInitStruct) + ** CPAL_I2C_DMATXTE_UserCallback(CPAL_InitTypeDef* pDevInitStruct) ** CPAL_I2C_DMARXTE_UserCallback(CPAL_InitTypeDef* pDevInitStruct) - These functions are called when a transfer error Interrupt occurred in transmission/reception operation + These functions are called when a transfer error Interrupt occurred in transmission/reception operation if DMA Programming Model is selected. - ** CPAL_I2C_GENCALL_UserCallback(CPAL_InitTypeDef* pDevInitStruct) - This function is called when an Address Event interrupt occurred and General Call Address Flag is set + ** CPAL_I2C_GENCALL_UserCallback(CPAL_InitTypeDef* pDevInitStruct) + This function is called when an Address Event interrupt occurred and General Call Address Flag is set (available in Slave mode only and when the option CPAL_OPT_I2C_GENCALL is enabled). - ** CPAL_I2C_DUALF_UserCallback(CPAL_InitTypeDef* pDevInitStruct) - This function is called when an Address Event interrupt occurred and Dual Address Flag is set + ** CPAL_I2C_DUALF_UserCallback(CPAL_InitTypeDef* pDevInitStruct) + This function is called when an Address Event interrupt occurred and Dual Address Flag is set (available in Slave mode only and when the option CPAL_OPT_I2C_DUALADDR is enabled). ** CPAL_I2C_SLAVE_WRITE_UserCallback(CPAL_InitTypeDef* pDevInitStruct) This function is called when a write operation is requested in Listen mode only. - + ** CPAL_I2C_SLAVE_READ_UserCallback(CPAL_InitTypeDef* pDevInitStruct) This function is called when a read operation is requested in Listen mode only. - - - -b- Error User Callbacks : - ** CPAL_I2C_ERR_UserCallback(CPAL_DevTypeDef pDevInstance, uint32_t Device_Error) + + -b- Error User Callbacks : + + ** CPAL_I2C_ERR_UserCallback(CPAL_DevTypeDef pDevInstance, uint32_t Device_Error) This function is called either when an Error Interrupt occurred (If Error Interrupts enabled) or after - a read or write operations to handle device errors (If Error Interrupts disabled). This callback + a read or write operations to handle device errors (If Error Interrupts disabled). This callback can be used to handle all device errors. It is available only when the define USE_SINGLE_ERROR_CALLBACK is enabled (Section 5). - ** CPAL_I2C_BERR_UserCallback(CPAL_DevTypeDef pDevInstance) - This function is called either when a Bus Error Interrupt occurred (If Error Interrupts enabled) or + ** CPAL_I2C_BERR_UserCallback(CPAL_DevTypeDef pDevInstance) + This function is called either when a Bus Error Interrupt occurred (If Error Interrupts enabled) or after a read or write operations to handle this error (If Error Interrupts disabled). This callback is available only when USE_MULTIPLE_ERROR_CALLBACK is enabled (Section 5). - ** CPAL_I2C_ARLO_UserCallback(CPAL_DevTypeDef pDevInstance) - This function is called either when an Arbitration Lost Interrupt occurred (If Error Interrupts + ** CPAL_I2C_ARLO_UserCallback(CPAL_DevTypeDef pDevInstance) + This function is called either when an Arbitration Lost Interrupt occurred (If Error Interrupts enabled) or after a read or write operations to handle this error (If Error Interrupts disabled). - ** CPAL_I2C_OVR_UserCallback(CPAL_DevTypeDef pDevInstance) - This function is called either when an Overrun Interrupt occurred (If Error Interrupts enabled) or + ** CPAL_I2C_OVR_UserCallback(CPAL_DevTypeDef pDevInstance) + This function is called either when an Overrun Interrupt occurred (If Error Interrupts enabled) or after a read or write operations to handle this error (If Error Interrupts disabled). This callback is available only when USE_MULTIPLE_ERROR_CALLBACK is enabled (Section 5). - ** CPAL_I2C_AF_UserCallback(CPAL_DevTypeDef pDevInstance) - This function is called either when an Acknowledge Failure Interrupt occurred (If Error Interrupts + ** CPAL_I2C_AF_UserCallback(CPAL_DevTypeDef pDevInstance) + This function is called either when an Acknowledge Failure Interrupt occurred (If Error Interrupts enabled) or after a read or write operations to handle this error (If Error Interrupts disabled). This callback is available only when USE_MULTIPLE_ERROR_CALLBACK is enabled (Section 5). - -c- Timeout User Callbacks : + -c- Timeout User Callbacks : ** CPAL_TIMEOUT_UserCallback(void) This function is called when a Timeout occurred in communication. - + ** CPAL_TIMEOUT_INIT() This function allows to configure and enable the counting peripheral/function (ie. SysTick Timer) It is called into all CPAL_PPP_Init() functions. @@ -265,25 +265,25 @@ ** CPAL_PPP_TIMEOUT_Manager() WARNING: DO NOT IMPLEMENT THIS FUNCTION (already implemented in CPAL drivers) This function is already implemented in the CPAL drivers (cpal_i2c.c file). It should be called periodically - (using the count mechanism interrupt for example). This function checks all PPP devices and - manages timeout conditions. In case of timeout occurring, this function calls the + (using the count mechanism interrupt for example). This function checks all PPP devices and + manages timeout conditions. In case of timeout occurring, this function calls the CPAL_TIMEOUT_UserCallback() function that may be implemented by user to manage the cases of timeout errors (ie. reset the device/microcontroller...). In order to facilitate implementation, this function (instead to be called periodically by user - application), may be mapped directly to a periodic event/interrupt: - Example: + application), may be mapped directly to a periodic event/interrupt: + Example: #define CPAL_I2C_TIMEOUT_Manager SysTick_Handler To implement Transfer and Error Callbacks, you should comment relative defines in Section 4 and implement Callback function (body) into - your application (their prototypes are declared in cpal_i2c.h file). - + your application (their prototypes are declared in cpal_i2c.h file). + Example: How to implement CPAL_I2C_TX_UserCallback() callback: - + -1- Comment the relative define in this file : - //#define CPAL_I2C_TX_UserCallback (void) - + //#define CPAL_I2C_TX_UserCallback (void) + -2- Add CPAL_I2C_TX_UserCallback code source in application file ( example : main.c ) void CPAL_I2C_TX_UserCallback (CPAL_InitTypeDef* pDevInitStruct) { @@ -291,20 +291,20 @@ // user code //.......... } - + There are two types of Error Callbacks : -1- Single Error Callback : Only one Callback is used to manage all device errors. - -2- Multiple Error Callback : Each device error is managed by its own separate Callback. - + -2- Multiple Error Callback : Each device error is managed by its own separate Callback. + Example of using CPAL_I2C_BERR_UserCallback : - + -1- Select Multiple Error Callback type : - //#define USE_SINGLE_ERROR_CALLBACK + //#define USE_SINGLE_ERROR_CALLBACK #define USE_MULTIPLE_ERROR_CALLBACK - + -2- Comment define relative to CPAL_I2C_BERR_UserCallback in cpal_conf.h file: - //#define CPAL_I2C_BERR_UserCallback (void) - + //#define CPAL_I2C_BERR_UserCallback (void) + -3- Add CPAL_I2C_BERR_UserCallback code source in application file ( example: main.c ) void CPAL_I2C_BERR_UserCallback (CPAL_DevTypeDef pDevInstance) { @@ -329,7 +329,7 @@ /* -- Section 1 : **** I2Cx Device Selection **** - Description: This section provide an easy way to select I2Cx devices in user application. + Description: This section provide an easy way to select I2Cx devices in user application. Choosing device allows to save memory resources. If you need I2C1 device, uncomment relative define: #define CPAL_USE_I2C1. All available I2Cx device can be used at the same time. @@ -343,8 +343,8 @@ /* -- Section 2 : **** Transfer Options Configuration **** - Description: This section allows user to enable/disable some Transfer Options. The benefits of these - defines is to minimize the size of the source code */ + Description: This section allows user to enable/disable some Transfer Options. The benefits of these + defines is to minimize the size of the source code */ /* Enable the use of Master Mode */ #define CPAL_I2C_MASTER_MODE @@ -375,7 +375,7 @@ //#define CPAL_I2C_10BIT_ADDR_MODE -/* Enable the use of 16Bit Address memory register option +/* Enable the use of 16Bit Address memory register option !! This define is available only when CPAL_I2C_MASTER_MODE is enabled !! */ #define CPAL_16BIT_REG_OPTION @@ -387,7 +387,7 @@ the I2C interrupts have the highest priority in the application */ //#define CPAL_I2C_CLOSECOM_METHOD1 -/* Method2 used for closing communication with master receiver: This method is for the case when +/* Method2 used for closing communication with master receiver: This method is for the case when the I2C interrupts do not have the highest priority in the application */ #define CPAL_I2C_CLOSECOM_METHOD2 @@ -402,14 +402,14 @@ Description: This section provides an easy way to enable UserCallbacks and select type of Error UserCallbacks. By default, All UserCallbacks are disabled (UserCallbacks are defined as void functions). - To implement a UserCallbacks in your application, comment the relative define and + To implement a UserCallbacks in your application, comment the relative define and implement the callback body in your application file.*/ - + /* Error UserCallbacks Type : Uncomment to select UserCallbacks type. One type must be selected */ -/* Note : if Error UserCallbacks are not used the two following defines must be commented - - WARNING: These two defines are EXCLUSIVE, only one define should be uncommented ! +/* Note : if Error UserCallbacks are not used the two following defines must be commented + + WARNING: These two defines are EXCLUSIVE, only one define should be uncommented ! */ #define USE_SINGLE_ERROR_CALLBACK /*CTRL = 0 /*CTRL = 0 /*Libraries->Small printf - // set to 'Yes') calls __io_putchar() + // set to 'Yes') calls __io_putchar() #define PUTCHAR_PROTOTYPE int __io_putchar(int ch) #else #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f) - #endif - + #endif + WARNING Be aware that enabling this feature may slow down the communication process, increase the code size significantly, and may in some cases cause communication errors (when print/display mechanism is too slow)*/ - + /* To Enable CPAL_DEBUG Option Uncomment the define below */ //#define CPAL_DEBUG @@ -580,7 +580,7 @@ #include /* This header file must be included when using CPAL_DEBUG option */ #else #define CPAL_LOG(Str) ((void)0) -#endif /* CPAL_DEBUG */ +#endif /* CPAL_DEBUG */ /*-----------------------------------------------------------------------------------------------------------------------*/ diff --git a/config/trace.h b/config/trace.h index 8c68af0bca..3311cc6d5c 100644 --- a/config/trace.h +++ b/config/trace.h @@ -49,9 +49,9 @@ #define ITM_BLOCKING_ON_QUEUE_RECEIVE 0x0300 #define ITM_BLOCKING_ON_QUEUE_SEND 0x0400 -#define traceQUEUE_SEND(xQueue) ITM_SEND(3, ITM_QUEUE_SEND | ((xQUEUE *) xQueue)->ucQueueNumber) -#define traceQUEUE_SEND_FAILED(xQueue) ITM_SEND(3, ITM_QUEUE_FAILED | ((xQUEUE *) xQueue)->ucQueueNumber) -#define traceBLOCKING_ON_QUEUE_RECEIVE(xQueue) ITM_SEND(3, ITM_BLOCKING_ON_QUEUE_RECEIVE | ((xQUEUE *) xQueue)->ucQueueNumber) -#define traceBLOCKING_ON_QUEUE_SEND(xQueue) ITM_SEND(3, ITM_BLOCKING_ON_QUEUE_SEND | ((xQUEUE *) xQueue)->ucQueueNumber) +// #define traceQUEUE_SEND(xQueue) ITM_SEND(3, ITM_QUEUE_SEND | ((xQUEUE *) xQueue)->ucQueueNumber) +// #define traceQUEUE_SEND_FAILED(xQueue) ITM_SEND(3, ITM_QUEUE_FAILED | ((xQUEUE *) xQueue)->ucQueueNumber) +// #define traceBLOCKING_ON_QUEUE_RECEIVE(xQueue) ITM_SEND(3, ITM_BLOCKING_ON_QUEUE_RECEIVE | ((xQUEUE *) xQueue)->ucQueueNumber) +// #define traceBLOCKING_ON_QUEUE_SEND(xQueue) ITM_SEND(3, ITM_BLOCKING_ON_QUEUE_SEND | ((xQUEUE *) xQueue)->ucQueueNumber) #endif diff --git a/deck/drivers/src/buzzer.c b/deck/drivers/src/buzzer.c new file mode 100644 index 0000000000..867783e675 --- /dev/null +++ b/deck/drivers/src/buzzer.c @@ -0,0 +1,317 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2012 BitCraze AB + * + * 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, in version 3. + * + * 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 . + * + * buzzer.c: Play tones or melodies + */ + +#include +#include +#include "stm32fxxx.h" + +#include "FreeRTOS.h" +#include "timers.h" + +#include "deck.h" + +#include "param.h" +#include "pm.h" +#include "log.h" +#include "piezo.h" + +typedef void (*BuzzerEffect)(uint32_t timer); + +/** + * Credit to http://tny.cz/e525c1b2 for supplying the tones + */ +#define C0 16 +#define Db0 17 +#define D0 18 +#define Eb0 19 +#define E0 20 +#define F0 21 +#define Gb0 23 +#define G0 24 +#define Ab0 25 +#define A0 27 +#define Bb0 29 +#define B0 30 +#define C1 32 +#define Db1 34 +#define D1 36 +#define Eb1 38 +#define E1 41 +#define F1 43 +#define Gb1 46 +#define G1 49 +#define Ab1 51 +#define A1 55 +#define Bb1 58 +#define B1 61 +#define C2 65 +#define Db2 69 +#define D2 73 +#define Eb2 77 +#define E2 82 +#define F2 87 +#define Gb2 92 +#define G2 98 +#define Ab2 103 +#define A2 110 +#define Bb2 116 +#define B2 123 +#define C3 130 +#define Db3 138 +#define D3 146 +#define Eb3 155 +#define E3 164 +#define F3 174 +#define Gb3 185 +#define G3 196 +#define Ab3 207 +#define A3 220 +#define Bb3 233 +#define B3 246 +#define C4 261 +#define Db4 277 +#define D4 293 +#define Eb4 311 +#define E4 329 +#define F4 349 +#define Gb4 369 +#define G4 392 +#define Ab4 415 +#define A4 440 +#define Bb4 466 +#define B4 493 +#define C5 523 +#define Db5 554 +#define D5 587 +#define Eb5 622 +#define E5 659 +#define F5 698 +#define Gb5 739 +#define G5 783 +#define Ab5 830 +#define A5 880 +#define Bb5 932 +#define B5 987 +#define C6 1046 +#define Db6 1108 +#define D6 1174 +#define Eb6 1244 +#define E6 1318 +#define F6 1396 +#define Gb6 1479 +#define G6 1567 +#define Ab6 1661 +#define A6 1760 +#define Bb6 1864 +#define B6 1975 +#define C7 2093 +#define Db7 2217 +#define D7 2349 +#define Eb7 2489 +#define E7 2637 +#define F7 2793 +#define Gb7 2959 +#define G7 3135 +#define Ab7 3322 +#define A7 3520 +#define Bb7 3729 +#define B7 3951 +#define C8 4186 +#define Db8 4434 +#define D8 4698 +#define Eb8 4978 +/* Duration of notes */ +#define W (60) +#define H (W * 2) +#define Q (W * 4) +#define E (W * 8) +#define S (W * 16) +#define ES (W*6) + + +typedef struct { + uint16_t tone; + uint16_t duration; +} Note; + +typedef struct { + uint32_t bpm; + uint32_t ni; + uint32_t delay; + Note notes[80]; +} Melody; + +Melody melodies[] = { + {.bpm = 120, .ni = 0, .delay = 1, .notes = {{C4, H}, {D4, H}, {E4, H}, {F4, H}, {G4, H}, {A4, H}, {B4, H}, {0xFF, 0}}}, + {.bpm = 120, .ni = 0, .delay = 1, .notes = {{C4, S}, {D4, S}, {E4, S}, {F4, S}, {G4, S}, {A4, S}, {B4, S}, {0xFF, 0}}}, + /* Imperial march from http://tny.cz/e525c1b2A */ + {.bpm = 120, .ni = 0, .delay = 1, .notes = {{A3, Q}, {A3, Q}, {A3, Q},{F3, ES}, {C4, S}, + {A3, Q}, {F3, ES}, {C4, S}, {A3, H}, + {E4, Q}, {E4, Q}, {E4, Q}, {F4, ES}, {C4, S}, + {Ab3, Q}, {F3, ES}, {C4, S}, {A3, H}, + {A4, Q}, {A3, ES}, {A3, S}, {A4, Q}, {Ab4, ES}, {G4, S}, + {Gb4, S}, {E4, S}, {F4, E}, {0, E}, {Bb3, E}, {Eb4, Q}, {D4, ES}, {Db4, S}, + {C4, S}, {B3, S}, {C4, E}, {0, E}, {F3, E}, {Ab3, Q}, {F3, ES}, {A3, S}, + {C4, Q}, {A3, ES}, {C4, S}, {E4, H}, + {A4, Q}, {A3, ES}, {A3, S}, {A4, Q}, {Ab4, ES}, {G4, S}, + {Gb4, S}, {E4, S}, {F4, E}, {0, E}, {Bb3, E}, {Eb4, Q}, {D4, ES}, {Db4, S}, + {Gb4, S}, {E4, S}, {F4, E}, {0, E}, {Bb3, E}, {Eb4, Q}, {D4, ES}, {Db4, S}, + {C4,S}, {B3, S}, {C4, E}, {0, E}, {F3, E}, {Ab3, Q}, {F3, ES}, {C4, S}, + {A3, Q}, {F3, ES}, {C4, S}, {A3, H}, {0, H}, + {0xFF, 0}}} + +}; + +uint8_t melody = 2; +uint8_t nmelody = 3; +unsigned int mcounter = 0; +static bool playing_sound = false; +static void melodyplayer(uint32_t counter) { + // Sync one melody for the loop + Melody* m = &melodies[melody]; + if (mcounter == 0) { + if (m->notes[m->ni].tone == 0xFF) + { + // Loop the melody + m->ni = 0; + } + // Play current note + piezoSetFreq(m->notes[m->ni].tone); + piezoSetRatio(127); + mcounter = (m->bpm * 100) / m->notes[m->ni].duration; + playing_sound = true; + m->ni++; + } + else if (mcounter == 1) + { + piezoSetRatio(0); + } + mcounter--; +} + +uint8_t static_ratio = 0; +uint16_t static_freq = 4000; +static void bypass(uint32_t counter) +{ + /* Just set the params from the host */ + piezoSetRatio(static_ratio); + piezoSetFreq(static_freq); +} + +uint16_t siren_start = 2000; +uint16_t siren_freq = 2000; +uint16_t siren_stop = 4000; +int16_t siren_step = 40; +static void siren(uint32_t counter) +{ + siren_freq += siren_step; + if (siren_freq > siren_stop) + { + siren_step *= -1; + siren_freq = siren_stop; + } + if (siren_freq < siren_start) + { + siren_step *= -1; + siren_freq = siren_start; + } + piezoSetRatio(127); + piezoSetFreq(siren_freq); +} + + +static int pitchid; +static int rollid; +static int pitch; +static int roll; +static int tilt_freq; +static int tilt_ratio; +static void tilt(uint32_t counter) +{ + pitchid = logGetVarId("stabilizer", "pitch"); + rollid = logGetVarId("stabilizer", "roll"); + + pitch = logGetInt(pitchid); + roll = logGetInt(rollid); + tilt_freq = 0; + tilt_ratio = 127; + + if (abs(pitch) > 5) { + tilt_freq = 3000 - 50 * pitch; + } + + piezoSetRatio(tilt_ratio); + piezoSetFreq(tilt_freq); +} + + +unsigned int neffect = 0; +unsigned int effect = 3; +BuzzerEffect effects[] = {bypass, siren, melodyplayer, tilt}; + +static xTimerHandle timer; +static uint32_t counter = 0; + +static void buzzTimer(xTimerHandle timer) +{ + counter++; + + if (effects[effect] != 0) + effects[effect](counter*10); +} + +static void buzzerInit(DeckInfo *info) +{ + piezoInit(); + + neffect = sizeof(effects)/sizeof(effects[0])-1; + nmelody = sizeof(melodies)/sizeof(melodies[0])-1; + + timer = xTimerCreate( "buzztimer", M2T(10), + pdTRUE, NULL, buzzTimer ); + xTimerStart(timer, 100); +} + +PARAM_GROUP_START(buzzer) +PARAM_ADD(PARAM_UINT8, effect, &effect) +PARAM_ADD(PARAM_UINT32 | PARAM_RONLY, neffect, &neffect) +PARAM_ADD(PARAM_UINT8, melody, &melody) +PARAM_ADD(PARAM_UINT32 | PARAM_RONLY, nmelody, &melody) +PARAM_ADD(PARAM_UINT16, freq, &static_freq) +PARAM_ADD(PARAM_UINT8, ratio, &static_ratio) +PARAM_GROUP_STOP(buzzer) + +static const DeckDriver buzzer_deck = { + .vid = 0, + .pid = 0, + .name = "bcBuzzer", + + .usedGpio = DECK_USING_PA2 | DECK_USING_PA3, + + .init = buzzerInit, +}; + +DECK_DRIVER(buzzer_deck); diff --git a/deck/drivers/src/ledring12.c b/deck/drivers/src/ledring12.c index f3271e7759..9e731c9047 100644 --- a/deck/drivers/src/ledring12.c +++ b/deck/drivers/src/ledring12.c @@ -631,7 +631,7 @@ static void ledring12Init(DeckInfo *info) GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; GPIO_Init(GPIOB, &GPIO_InitStructure); - timer = xTimerCreate( (const signed char *)"ringTimer", M2T(50), + timer = xTimerCreate( "ringTimer", M2T(50), pdTRUE, NULL, ledring12Timer ); xTimerStart(timer, 100); } diff --git a/drivers/src/adc_f103.c b/drivers/src/adc_f103.c index 2a3f04733b..ed29759bf5 100644 --- a/drivers/src/adc_f103.c +++ b/drivers/src/adc_f103.c @@ -1,6 +1,6 @@ /** - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ @@ -209,7 +209,7 @@ void adcInit(void) adcQueue = xQueueCreate(1, sizeof(AdcGroup*)); - xTaskCreate(adcTask, (const signed char * const)ADC_TASK_NAME, + xTaskCreate(adcTask, ADC_TASK_NAME, ADC_TASK_STACKSIZE, NULL, ADC_TASK_PRI, NULL); isInit = true; diff --git a/drivers/src/i2cdev_f405.c b/drivers/src/i2cdev_f405.c index b71be660b1..8e418597f5 100644 --- a/drivers/src/i2cdev_f405.c +++ b/drivers/src/i2cdev_f405.c @@ -320,7 +320,7 @@ static void semaphoreGiveFromISR(xSemaphoreHandle semaphore) if(xHigherPriorityTaskWoken) { - vPortYieldFromISR(); + portYIELD(); } } diff --git a/hal/src/eskylink.c b/hal/src/eskylink.c index f6c2da9962..c36194090e 100644 --- a/hal/src/eskylink.c +++ b/hal/src/eskylink.c @@ -1,6 +1,6 @@ /* - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ @@ -66,7 +66,7 @@ xQueueHandle rxQueue; static struct { bool enabled; - + bool paired; uint8_t band; } state; @@ -79,7 +79,7 @@ static void interruptCallback() xSemaphoreGiveFromISR(dataRdy, &xHigherPriorityTaskWoken); if(xHigherPriorityTaskWoken) - vPortYieldFromISR(); + portYIELD(); } // 'Class' functions, called from callbacks @@ -95,9 +95,9 @@ static int sendPacket(CRTPPacket * pk) { if (!state.enabled) return ENETDOWN; - + // NOP! - + return 0; } @@ -127,20 +127,20 @@ static int eskylinkFetchData(char * packet, int dataLen) //clear the interruptions flags nrfWrite1Reg(REG_STATUS, 0x70); - + nrfSetEnable(true); - + return dataLen; } static void eskylinkInitPairing(void) { int i; - + //Power the radio, Enable the DR interruption, set the radio in PRX mode with 2bytes CRC nrfWrite1Reg(REG_CONFIG, 0x3F); vTaskDelay(M2T(2)); //Wait for the chip to be ready - + //Set the radio channel, pairing channel is 50 nrfSetChannel(50); //Set the radio data rate @@ -183,33 +183,33 @@ static void eskylinkDecode(char* packet) static CRTPPacket crtpPacket; float pitch, roll, yaw; uint16_t thrust; - + pitch = ((packet[2]<<8) | packet[3])-PPM_ZERO; if (roll<(-PPM_RANGE)) roll = -PPM_RANGE; if (roll>PPM_RANGE) roll = PPM_RANGE; pitch *= 20.0/PPM_RANGE; - + roll = ((packet[0]<<8) | packet[1])-PPM_ZERO; if (roll<(-PPM_RANGE)) roll = -PPM_RANGE; if (roll>PPM_RANGE) roll = PPM_RANGE; roll *= 20.0/PPM_RANGE; - + yaw = ((packet[6]<<8) | packet[7])-PPM_ZERO; if (yaw<(-PPM_RANGE)) yaw = -PPM_RANGE; if (yaw>PPM_RANGE) yaw = PPM_RANGE; yaw *= 200.0/PPM_RANGE; - + thrust = ((packet[4]<<8) | packet[5])-PPM_MIN; if (thrust<0) thrust = 0; if (thrust>(2*PPM_RANGE)) thrust = 2*PPM_RANGE; thrust *= 55000/(2*PPM_RANGE); - + crtpPacket.port = CRTP_PORT_COMMANDER; memcpy(&crtpPacket.data[0], (char*)&roll, 4); memcpy(&crtpPacket.data[4], (char*)&pitch, 4); memcpy(&crtpPacket.data[8], (char*)&yaw, 4); memcpy(&crtpPacket.data[12], (char*)&thrust, 2); - + xQueueSend(rxQueue, &crtpPacket, 0); } @@ -224,7 +224,7 @@ static void eskylinkTask(void * arg) { xSemaphoreTake(dataRdy, portMAX_DELAY); ledseqRun(LED_GREEN, seq_linkup); - + eskylinkFetchData(packet, 13); if (packet[4]==0x18 && packet[5]==0x29) @@ -249,10 +249,10 @@ static void eskylinkTask(void * arg) if (xSemaphoreTake(dataRdy, M2T(10))==pdTRUE) { ledseqRun(LED_GREEN, seq_linkup); - + eskylinkFetchData(packet, 13); eskylinkDecode(packet); - + if (channel1<0) //Channels found! { channel1 = channel; @@ -261,7 +261,7 @@ static void eskylinkTask(void * arg) } } else - { + { if (channel1<0) { channel++; @@ -276,12 +276,12 @@ static void eskylinkTask(void * arg) channel = channel2; else channel = channel1; - + nrfSetEnable(false); nrfSetChannel(channel); nrfSetEnable(true); } - + } } } @@ -310,7 +310,7 @@ void eskylinkInit() eskylinkInitPairing(); /* Launch the Radio link task */ - xTaskCreate(eskylinkTask, (const signed char * const)ESKYLINK_TASK_NAME, + xTaskCreate(eskylinkTask, ESKYLINK_TASK_NAME, ESKYLINK_TASK_STACKSIZE, NULL, ESKYLINK_TASK_PRI, NULL); isInit = true; diff --git a/hal/src/ledseq.c b/hal/src/ledseq.c index bbd1efd56f..ce030950e6 100644 --- a/hal/src/ledseq.c +++ b/hal/src/ledseq.c @@ -196,7 +196,7 @@ void ledseqInit() //Init the soft timers that runs the led sequences for each leds for(i=0; i>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. + + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. http://www.SafeRTOS.com - High Integrity Systems also provide a safety engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ #include "FreeRTOS.h" #include "task.h" #include "croutine.h" +/* Remove the whole file is co-routines are not being used. */ +#if( configUSE_CO_ROUTINES != 0 ) + /* * Some kernel aware debuggers require data to be viewed to be global, rather * than file scope. @@ -86,17 +84,17 @@ /* Lists for ready and blocked co-routines. --------------------*/ -static xList pxReadyCoRoutineLists[ configMAX_CO_ROUTINE_PRIORITIES ]; /*< Prioritised ready co-routines. */ -static xList xDelayedCoRoutineList1; /*< Delayed co-routines. */ -static xList xDelayedCoRoutineList2; /*< Delayed co-routines (two lists are used - one for delays that have overflowed the current tick count. */ -static xList * pxDelayedCoRoutineList; /*< Points to the delayed co-routine list currently being used. */ -static xList * pxOverflowDelayedCoRoutineList; /*< Points to the delayed co-routine list currently being used to hold co-routines that have overflowed the current tick count. */ -static xList xPendingReadyCoRoutineList; /*< Holds co-routines that have been readied by an external event. They cannot be added directly to the ready lists as the ready lists cannot be accessed by interrupts. */ +static List_t pxReadyCoRoutineLists[ configMAX_CO_ROUTINE_PRIORITIES ]; /*< Prioritised ready co-routines. */ +static List_t xDelayedCoRoutineList1; /*< Delayed co-routines. */ +static List_t xDelayedCoRoutineList2; /*< Delayed co-routines (two lists are used - one for delays that have overflowed the current tick count. */ +static List_t * pxDelayedCoRoutineList; /*< Points to the delayed co-routine list currently being used. */ +static List_t * pxOverflowDelayedCoRoutineList; /*< Points to the delayed co-routine list currently being used to hold co-routines that have overflowed the current tick count. */ +static List_t xPendingReadyCoRoutineList; /*< Holds co-routines that have been readied by an external event. They cannot be added directly to the ready lists as the ready lists cannot be accessed by interrupts. */ /* Other file private variables. --------------------------------*/ -corCRCB * pxCurrentCoRoutine = NULL; -static unsigned portBASE_TYPE uxTopCoRoutineReadyPriority = 0; -static portTickType xCoRoutineTickCount = 0, xLastTickCount = 0, xPassedTicks = 0; +CRCB_t * pxCurrentCoRoutine = NULL; +static UBaseType_t uxTopCoRoutineReadyPriority = 0; +static TickType_t xCoRoutineTickCount = 0, xLastTickCount = 0, xPassedTicks = 0; /* The initial state of the co-routine when it is created. */ #define corINITIAL_STATE ( 0 ) @@ -114,7 +112,7 @@ static portTickType xCoRoutineTickCount = 0, xLastTickCount = 0, xPassedTicks = { \ uxTopCoRoutineReadyPriority = pxCRCB->uxPriority; \ } \ - vListInsertEnd( ( xList * ) &( pxReadyCoRoutineLists[ pxCRCB->uxPriority ] ), &( pxCRCB->xGenericListItem ) ); \ + vListInsertEnd( ( List_t * ) &( pxReadyCoRoutineLists[ pxCRCB->uxPriority ] ), &( pxCRCB->xGenericListItem ) ); \ } /* @@ -143,13 +141,13 @@ static void prvCheckDelayedList( void ); /*-----------------------------------------------------------*/ -signed portBASE_TYPE xCoRoutineCreate( crCOROUTINE_CODE pxCoRoutineCode, unsigned portBASE_TYPE uxPriority, unsigned portBASE_TYPE uxIndex ) +BaseType_t xCoRoutineCreate( crCOROUTINE_CODE pxCoRoutineCode, UBaseType_t uxPriority, UBaseType_t uxIndex ) { -signed portBASE_TYPE xReturn; -corCRCB *pxCoRoutine; +BaseType_t xReturn; +CRCB_t *pxCoRoutine; /* Allocate the memory that will store the co-routine control block. */ - pxCoRoutine = ( corCRCB * ) pvPortMalloc( sizeof( corCRCB ) ); + pxCoRoutine = ( CRCB_t * ) pvPortMalloc( sizeof( CRCB_t ) ); if( pxCoRoutine ) { /* If pxCurrentCoRoutine is NULL then this is the first co-routine to @@ -176,14 +174,14 @@ corCRCB *pxCoRoutine; vListInitialiseItem( &( pxCoRoutine->xGenericListItem ) ); vListInitialiseItem( &( pxCoRoutine->xEventListItem ) ); - /* Set the co-routine control block as a link back from the xListItem. + /* Set the co-routine control block as a link back from the ListItem_t. This is so we can get back to the containing CRCB from a generic item in a list. */ listSET_LIST_ITEM_OWNER( &( pxCoRoutine->xGenericListItem ), pxCoRoutine ); listSET_LIST_ITEM_OWNER( &( pxCoRoutine->xEventListItem ), pxCoRoutine ); /* Event lists are always in priority order. */ - listSET_LIST_ITEM_VALUE( &( pxCoRoutine->xEventListItem ), configMAX_PRIORITIES - ( portTickType ) uxPriority ); + listSET_LIST_ITEM_VALUE( &( pxCoRoutine->xEventListItem ), ( ( TickType_t ) configMAX_CO_ROUTINE_PRIORITIES - ( TickType_t ) uxPriority ) ); /* Now the co-routine has been initialised it can be added to the ready list at the correct priority. */ @@ -200,9 +198,9 @@ corCRCB *pxCoRoutine; } /*-----------------------------------------------------------*/ -void vCoRoutineAddToDelayedList( portTickType xTicksToDelay, xList *pxEventList ) +void vCoRoutineAddToDelayedList( TickType_t xTicksToDelay, List_t *pxEventList ) { -portTickType xTimeToWake; +TickType_t xTimeToWake; /* Calculate the time to wake - this may overflow but this is not a problem. */ @@ -211,7 +209,7 @@ portTickType xTimeToWake; /* 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. */ - uxListRemove( ( xListItem * ) &( pxCurrentCoRoutine->xGenericListItem ) ); + ( void ) uxListRemove( ( ListItem_t * ) &( pxCurrentCoRoutine->xGenericListItem ) ); /* The list item will be inserted in wake time order. */ listSET_LIST_ITEM_VALUE( &( pxCurrentCoRoutine->xGenericListItem ), xTimeToWake ); @@ -220,13 +218,13 @@ portTickType xTimeToWake; { /* Wake time has overflowed. Place this item in the overflow list. */ - vListInsert( ( xList * ) pxOverflowDelayedCoRoutineList, ( xListItem * ) &( pxCurrentCoRoutine->xGenericListItem ) ); + vListInsert( ( List_t * ) pxOverflowDelayedCoRoutineList, ( ListItem_t * ) &( pxCurrentCoRoutine->xGenericListItem ) ); } else { /* The wake time has not overflowed, so we can use the current block list. */ - vListInsert( ( xList * ) pxDelayedCoRoutineList, ( xListItem * ) &( pxCurrentCoRoutine->xGenericListItem ) ); + vListInsert( ( List_t * ) pxDelayedCoRoutineList, ( ListItem_t * ) &( pxCurrentCoRoutine->xGenericListItem ) ); } if( pxEventList ) @@ -245,17 +243,17 @@ static void prvCheckPendingReadyList( void ) the ready lists itself. */ while( listLIST_IS_EMPTY( &xPendingReadyCoRoutineList ) == pdFALSE ) { - corCRCB *pxUnblockedCRCB; + CRCB_t *pxUnblockedCRCB; /* The pending ready list can be accessed by an ISR. */ portDISABLE_INTERRUPTS(); { - pxUnblockedCRCB = ( corCRCB * ) listGET_OWNER_OF_HEAD_ENTRY( (&xPendingReadyCoRoutineList) ); - uxListRemove( &( pxUnblockedCRCB->xEventListItem ) ); + pxUnblockedCRCB = ( CRCB_t * ) listGET_OWNER_OF_HEAD_ENTRY( (&xPendingReadyCoRoutineList) ); + ( void ) uxListRemove( &( pxUnblockedCRCB->xEventListItem ) ); } portENABLE_INTERRUPTS(); - uxListRemove( &( pxUnblockedCRCB->xGenericListItem ) ); + ( void ) uxListRemove( &( pxUnblockedCRCB->xGenericListItem ) ); prvAddCoRoutineToReadyQueue( pxUnblockedCRCB ); } } @@ -263,7 +261,7 @@ static void prvCheckPendingReadyList( void ) static void prvCheckDelayedList( void ) { -corCRCB *pxCRCB; +CRCB_t *pxCRCB; xPassedTicks = xTaskGetTickCount() - xLastTickCount; while( xPassedTicks ) @@ -274,7 +272,7 @@ corCRCB *pxCRCB; /* If the tick count has overflowed we need to swap the ready lists. */ if( xCoRoutineTickCount == 0 ) { - xList * pxTemp; + List_t * pxTemp; /* Tick count has overflowed so we need to swap the delay lists. If there are any items in pxDelayedCoRoutineList here then there is an error! */ @@ -286,7 +284,7 @@ corCRCB *pxCRCB; /* See if this tick has made a timeout expire. */ while( listLIST_IS_EMPTY( pxDelayedCoRoutineList ) == pdFALSE ) { - pxCRCB = ( corCRCB * ) listGET_OWNER_OF_HEAD_ENTRY( pxDelayedCoRoutineList ); + pxCRCB = ( CRCB_t * ) listGET_OWNER_OF_HEAD_ENTRY( pxDelayedCoRoutineList ); if( xCoRoutineTickCount < listGET_LIST_ITEM_VALUE( &( pxCRCB->xGenericListItem ) ) ) { @@ -301,12 +299,12 @@ corCRCB *pxCRCB; have been moved to the pending ready list and the following line is still valid. Also the pvContainer parameter will have been set to NULL so the following lines are also valid. */ - uxListRemove( &( pxCRCB->xGenericListItem ) ); + ( void ) uxListRemove( &( pxCRCB->xGenericListItem ) ); /* Is the co-routine waiting on an event also? */ if( pxCRCB->xEventListItem.pvContainer ) { - uxListRemove( &( pxCRCB->xEventListItem ) ); + ( void ) uxListRemove( &( pxCRCB->xEventListItem ) ); } } portENABLE_INTERRUPTS(); @@ -351,16 +349,16 @@ void vCoRoutineSchedule( void ) static void prvInitialiseCoRoutineLists( void ) { -unsigned portBASE_TYPE uxPriority; +UBaseType_t uxPriority; for( uxPriority = 0; uxPriority < configMAX_CO_ROUTINE_PRIORITIES; uxPriority++ ) { - vListInitialise( ( xList * ) &( pxReadyCoRoutineLists[ uxPriority ] ) ); + vListInitialise( ( List_t * ) &( pxReadyCoRoutineLists[ uxPriority ] ) ); } - vListInitialise( ( xList * ) &xDelayedCoRoutineList1 ); - vListInitialise( ( xList * ) &xDelayedCoRoutineList2 ); - vListInitialise( ( xList * ) &xPendingReadyCoRoutineList ); + vListInitialise( ( List_t * ) &xDelayedCoRoutineList1 ); + vListInitialise( ( List_t * ) &xDelayedCoRoutineList2 ); + vListInitialise( ( List_t * ) &xPendingReadyCoRoutineList ); /* Start with pxDelayedCoRoutineList using list1 and the pxOverflowDelayedCoRoutineList using list2. */ @@ -369,17 +367,17 @@ unsigned portBASE_TYPE uxPriority; } /*-----------------------------------------------------------*/ -signed portBASE_TYPE xCoRoutineRemoveFromEventList( const xList *pxEventList ) +BaseType_t xCoRoutineRemoveFromEventList( const List_t *pxEventList ) { -corCRCB *pxUnblockedCRCB; -signed portBASE_TYPE xReturn; +CRCB_t *pxUnblockedCRCB; +BaseType_t xReturn; /* This function is called from within an interrupt. It can only access event lists and the pending ready list. This function assumes that a check has already been made to ensure pxEventList is not empty. */ - pxUnblockedCRCB = ( corCRCB * ) listGET_OWNER_OF_HEAD_ENTRY( pxEventList ); - uxListRemove( &( pxUnblockedCRCB->xEventListItem ) ); - vListInsertEnd( ( xList * ) &( xPendingReadyCoRoutineList ), &( pxUnblockedCRCB->xEventListItem ) ); + pxUnblockedCRCB = ( CRCB_t * ) listGET_OWNER_OF_HEAD_ENTRY( pxEventList ); + ( void ) uxListRemove( &( pxUnblockedCRCB->xEventListItem ) ); + vListInsertEnd( ( List_t * ) &( xPendingReadyCoRoutineList ), &( pxUnblockedCRCB->xEventListItem ) ); if( pxUnblockedCRCB->uxPriority >= pxCurrentCoRoutine->uxPriority ) { @@ -393,3 +391,5 @@ signed portBASE_TYPE xReturn; return xReturn; } +#endif /* configUSE_CO_ROUTINES == 0 */ + diff --git a/lib/FreeRTOS/event_groups.c b/lib/FreeRTOS/event_groups.c new file mode 100644 index 0000000000..625548c56c --- /dev/null +++ b/lib/FreeRTOS/event_groups.c @@ -0,0 +1,683 @@ +/* + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. + + 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 modification 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. Full license text is available on the following + link: http://www.freertos.org/a00114.html + + *************************************************************************** + * * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * + * * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * + * * + *************************************************************************** + + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. + + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. + + http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and + mission critical applications that require provable dependability. + + 1 tab == 4 spaces! +*/ + +/* Standard includes. */ +#include + +/* Defining MPU_WRAPPERS_INCLUDED_FROM_API_FILE prevents task.h from redefining +all the API functions to use the MPU wrappers. That should only be done when +task.h is included from an application file. */ +#define MPU_WRAPPERS_INCLUDED_FROM_API_FILE + +/* FreeRTOS includes. */ +#include "FreeRTOS.h" +#include "task.h" +#include "timers.h" +#include "event_groups.h" + +/* Lint e961 and e750 are suppressed as a MISRA exception justified because the +MPU ports require MPU_WRAPPERS_INCLUDED_FROM_API_FILE to be defined for the +header files above, but not in this file, in order to generate the correct +privileged Vs unprivileged linkage and placement. */ +#undef MPU_WRAPPERS_INCLUDED_FROM_API_FILE /*lint !e961 !e750. */ + +#if ( INCLUDE_xEventGroupSetBitFromISR == 1 ) && ( configUSE_TIMERS == 0 ) + #error configUSE_TIMERS must be set to 1 to make the xEventGroupSetBitFromISR() function available. +#endif + +#if ( INCLUDE_xEventGroupSetBitFromISR == 1 ) && ( INCLUDE_xTimerPendFunctionCall == 0 ) + #error INCLUDE_xTimerPendFunctionCall must also be set to one to make the xEventGroupSetBitFromISR() function available. +#endif + +/* The following bit fields convey control information in a task's event list +item value. It is important they don't clash with the +taskEVENT_LIST_ITEM_VALUE_IN_USE definition. */ +#if configUSE_16_BIT_TICKS == 1 + #define eventCLEAR_EVENTS_ON_EXIT_BIT 0x0100U + #define eventUNBLOCKED_DUE_TO_BIT_SET 0x0200U + #define eventWAIT_FOR_ALL_BITS 0x0400U + #define eventEVENT_BITS_CONTROL_BYTES 0xff00U +#else + #define eventCLEAR_EVENTS_ON_EXIT_BIT 0x01000000UL + #define eventUNBLOCKED_DUE_TO_BIT_SET 0x02000000UL + #define eventWAIT_FOR_ALL_BITS 0x04000000UL + #define eventEVENT_BITS_CONTROL_BYTES 0xff000000UL +#endif + +typedef struct xEventGroupDefinition +{ + EventBits_t uxEventBits; + List_t xTasksWaitingForBits; /*< List of tasks waiting for a bit to be set. */ + + #if( configUSE_TRACE_FACILITY == 1 ) + UBaseType_t uxEventGroupNumber; + #endif + +} EventGroup_t; + +/*-----------------------------------------------------------*/ + +/* + * Test the bits set in uxCurrentEventBits to see if the wait condition is met. + * The wait condition is defined by xWaitForAllBits. If xWaitForAllBits is + * pdTRUE then the wait condition is met if all the bits set in uxBitsToWaitFor + * are also set in uxCurrentEventBits. If xWaitForAllBits is pdFALSE then the + * wait condition is met if any of the bits set in uxBitsToWait for are also set + * in uxCurrentEventBits. + */ +static BaseType_t prvTestWaitCondition( const EventBits_t uxCurrentEventBits, const EventBits_t uxBitsToWaitFor, const BaseType_t xWaitForAllBits ); + +/*-----------------------------------------------------------*/ + +EventGroupHandle_t xEventGroupCreate( void ) +{ +EventGroup_t *pxEventBits; + + pxEventBits = ( EventGroup_t * ) pvPortMalloc( sizeof( EventGroup_t ) ); + if( pxEventBits != NULL ) + { + pxEventBits->uxEventBits = 0; + vListInitialise( &( pxEventBits->xTasksWaitingForBits ) ); + traceEVENT_GROUP_CREATE( pxEventBits ); + } + else + { + traceEVENT_GROUP_CREATE_FAILED(); + } + + return ( EventGroupHandle_t ) pxEventBits; +} +/*-----------------------------------------------------------*/ + +EventBits_t xEventGroupSync( EventGroupHandle_t xEventGroup, const EventBits_t uxBitsToSet, const EventBits_t uxBitsToWaitFor, TickType_t xTicksToWait ) +{ +EventBits_t uxOriginalBitValue, uxReturn; +EventGroup_t *pxEventBits = ( EventGroup_t * ) xEventGroup; +BaseType_t xAlreadyYielded; +BaseType_t xTimeoutOccurred = pdFALSE; + + configASSERT( ( uxBitsToWaitFor & eventEVENT_BITS_CONTROL_BYTES ) == 0 ); + configASSERT( uxBitsToWaitFor != 0 ); + #if ( ( INCLUDE_xTaskGetSchedulerState == 1 ) || ( configUSE_TIMERS == 1 ) ) + { + configASSERT( !( ( xTaskGetSchedulerState() == taskSCHEDULER_SUSPENDED ) && ( xTicksToWait != 0 ) ) ); + } + #endif + + vTaskSuspendAll(); + { + uxOriginalBitValue = pxEventBits->uxEventBits; + + ( void ) xEventGroupSetBits( xEventGroup, uxBitsToSet ); + + if( ( ( uxOriginalBitValue | uxBitsToSet ) & uxBitsToWaitFor ) == uxBitsToWaitFor ) + { + /* All the rendezvous bits are now set - no need to block. */ + uxReturn = ( uxOriginalBitValue | uxBitsToSet ); + + /* Rendezvous always clear the bits. They will have been cleared + already unless this is the only task in the rendezvous. */ + pxEventBits->uxEventBits &= ~uxBitsToWaitFor; + + xTicksToWait = 0; + } + else + { + if( xTicksToWait != ( TickType_t ) 0 ) + { + traceEVENT_GROUP_SYNC_BLOCK( xEventGroup, uxBitsToSet, uxBitsToWaitFor ); + + /* Store the bits that the calling task is waiting for in the + task's event list item so the kernel knows when a match is + found. Then enter the blocked state. */ + vTaskPlaceOnUnorderedEventList( &( pxEventBits->xTasksWaitingForBits ), ( uxBitsToWaitFor | eventCLEAR_EVENTS_ON_EXIT_BIT | eventWAIT_FOR_ALL_BITS ), xTicksToWait ); + + /* This assignment is obsolete as uxReturn will get set after + the task unblocks, but some compilers mistakenly generate a + warning about uxReturn being returned without being set if the + assignment is omitted. */ + uxReturn = 0; + } + else + { + /* The rendezvous bits were not set, but no block time was + specified - just return the current event bit value. */ + uxReturn = pxEventBits->uxEventBits; + } + } + } + xAlreadyYielded = xTaskResumeAll(); + + if( xTicksToWait != ( TickType_t ) 0 ) + { + if( xAlreadyYielded == pdFALSE ) + { + portYIELD_WITHIN_API(); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + /* The task blocked to wait for its required bits to be set - at this + point either the required bits were set or the block time expired. If + the required bits were set they will have been stored in the task's + event list item, and they should now be retrieved then cleared. */ + uxReturn = uxTaskResetEventItemValue(); + + if( ( uxReturn & eventUNBLOCKED_DUE_TO_BIT_SET ) == ( EventBits_t ) 0 ) + { + /* The task timed out, just return the current event bit value. */ + taskENTER_CRITICAL(); + { + uxReturn = pxEventBits->uxEventBits; + + /* Although the task got here because it timed out before the + bits it was waiting for were set, it is possible that since it + unblocked another task has set the bits. If this is the case + then it needs to clear the bits before exiting. */ + if( ( uxReturn & uxBitsToWaitFor ) == uxBitsToWaitFor ) + { + pxEventBits->uxEventBits &= ~uxBitsToWaitFor; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + taskEXIT_CRITICAL(); + + xTimeoutOccurred = pdTRUE; + } + else + { + /* The task unblocked because the bits were set. */ + } + + /* Control bits might be set as the task had blocked should not be + returned. */ + uxReturn &= ~eventEVENT_BITS_CONTROL_BYTES; + } + + traceEVENT_GROUP_SYNC_END( xEventGroup, uxBitsToSet, uxBitsToWaitFor, xTimeoutOccurred ); + + return uxReturn; +} +/*-----------------------------------------------------------*/ + +EventBits_t xEventGroupWaitBits( EventGroupHandle_t xEventGroup, const EventBits_t uxBitsToWaitFor, const BaseType_t xClearOnExit, const BaseType_t xWaitForAllBits, TickType_t xTicksToWait ) +{ +EventGroup_t *pxEventBits = ( EventGroup_t * ) xEventGroup; +EventBits_t uxReturn, uxControlBits = 0; +BaseType_t xWaitConditionMet, xAlreadyYielded; +BaseType_t xTimeoutOccurred = pdFALSE; + + /* Check the user is not attempting to wait on the bits used by the kernel + itself, and that at least one bit is being requested. */ + configASSERT( xEventGroup ); + configASSERT( ( uxBitsToWaitFor & eventEVENT_BITS_CONTROL_BYTES ) == 0 ); + configASSERT( uxBitsToWaitFor != 0 ); + #if ( ( INCLUDE_xTaskGetSchedulerState == 1 ) || ( configUSE_TIMERS == 1 ) ) + { + configASSERT( !( ( xTaskGetSchedulerState() == taskSCHEDULER_SUSPENDED ) && ( xTicksToWait != 0 ) ) ); + } + #endif + + vTaskSuspendAll(); + { + const EventBits_t uxCurrentEventBits = pxEventBits->uxEventBits; + + /* Check to see if the wait condition is already met or not. */ + xWaitConditionMet = prvTestWaitCondition( uxCurrentEventBits, uxBitsToWaitFor, xWaitForAllBits ); + + if( xWaitConditionMet != pdFALSE ) + { + /* The wait condition has already been met so there is no need to + block. */ + uxReturn = uxCurrentEventBits; + xTicksToWait = ( TickType_t ) 0; + + /* Clear the wait bits if requested to do so. */ + if( xClearOnExit != pdFALSE ) + { + pxEventBits->uxEventBits &= ~uxBitsToWaitFor; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else if( xTicksToWait == ( TickType_t ) 0 ) + { + /* The wait condition has not been met, but no block time was + specified, so just return the current value. */ + uxReturn = uxCurrentEventBits; + } + else + { + /* The task is going to block to wait for its required bits to be + set. uxControlBits are used to remember the specified behaviour of + this call to xEventGroupWaitBits() - for use when the event bits + unblock the task. */ + if( xClearOnExit != pdFALSE ) + { + uxControlBits |= eventCLEAR_EVENTS_ON_EXIT_BIT; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + if( xWaitForAllBits != pdFALSE ) + { + uxControlBits |= eventWAIT_FOR_ALL_BITS; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + /* Store the bits that the calling task is waiting for in the + task's event list item so the kernel knows when a match is + found. Then enter the blocked state. */ + vTaskPlaceOnUnorderedEventList( &( pxEventBits->xTasksWaitingForBits ), ( uxBitsToWaitFor | uxControlBits ), xTicksToWait ); + + /* This is obsolete as it will get set after the task unblocks, but + some compilers mistakenly generate a warning about the variable + being returned without being set if it is not done. */ + uxReturn = 0; + + traceEVENT_GROUP_WAIT_BITS_BLOCK( xEventGroup, uxBitsToWaitFor ); + } + } + xAlreadyYielded = xTaskResumeAll(); + + if( xTicksToWait != ( TickType_t ) 0 ) + { + if( xAlreadyYielded == pdFALSE ) + { + portYIELD_WITHIN_API(); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + /* The task blocked to wait for its required bits to be set - at this + point either the required bits were set or the block time expired. If + the required bits were set they will have been stored in the task's + event list item, and they should now be retrieved then cleared. */ + uxReturn = uxTaskResetEventItemValue(); + + if( ( uxReturn & eventUNBLOCKED_DUE_TO_BIT_SET ) == ( EventBits_t ) 0 ) + { + taskENTER_CRITICAL(); + { + /* The task timed out, just return the current event bit value. */ + uxReturn = pxEventBits->uxEventBits; + + /* It is possible that the event bits were updated between this + task leaving the Blocked state and running again. */ + if( prvTestWaitCondition( uxReturn, uxBitsToWaitFor, xWaitForAllBits ) != pdFALSE ) + { + if( xClearOnExit != pdFALSE ) + { + pxEventBits->uxEventBits &= ~uxBitsToWaitFor; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + taskEXIT_CRITICAL(); + + /* Prevent compiler warnings when trace macros are not used. */ + xTimeoutOccurred = pdFALSE; + } + else + { + /* The task unblocked because the bits were set. */ + } + + /* The task blocked so control bits may have been set. */ + uxReturn &= ~eventEVENT_BITS_CONTROL_BYTES; + } + traceEVENT_GROUP_WAIT_BITS_END( xEventGroup, uxBitsToWaitFor, xTimeoutOccurred ); + + return uxReturn; +} +/*-----------------------------------------------------------*/ + +EventBits_t xEventGroupClearBits( EventGroupHandle_t xEventGroup, const EventBits_t uxBitsToClear ) +{ +EventGroup_t *pxEventBits = ( EventGroup_t * ) xEventGroup; +EventBits_t uxReturn; + + /* Check the user is not attempting to clear the bits used by the kernel + itself. */ + configASSERT( xEventGroup ); + configASSERT( ( uxBitsToClear & eventEVENT_BITS_CONTROL_BYTES ) == 0 ); + + taskENTER_CRITICAL(); + { + traceEVENT_GROUP_CLEAR_BITS( xEventGroup, uxBitsToClear ); + + /* The value returned is the event group value prior to the bits being + cleared. */ + uxReturn = pxEventBits->uxEventBits; + + /* Clear the bits. */ + pxEventBits->uxEventBits &= ~uxBitsToClear; + } + taskEXIT_CRITICAL(); + + return uxReturn; +} +/*-----------------------------------------------------------*/ + +#if ( ( configUSE_TRACE_FACILITY == 1 ) && ( INCLUDE_xTimerPendFunctionCall == 1 ) && ( configUSE_TIMERS == 1 ) ) + + BaseType_t xEventGroupClearBitsFromISR( EventGroupHandle_t xEventGroup, const EventBits_t uxBitsToClear ) + { + BaseType_t xReturn; + + traceEVENT_GROUP_CLEAR_BITS_FROM_ISR( xEventGroup, uxBitsToClear ); + xReturn = xTimerPendFunctionCallFromISR( vEventGroupClearBitsCallback, ( void * ) xEventGroup, ( uint32_t ) uxBitsToClear, NULL ); + + return xReturn; + } + +#endif +/*-----------------------------------------------------------*/ + +EventBits_t xEventGroupGetBitsFromISR( EventGroupHandle_t xEventGroup ) +{ +UBaseType_t uxSavedInterruptStatus; +EventGroup_t *pxEventBits = ( EventGroup_t * ) xEventGroup; +EventBits_t uxReturn; + + uxSavedInterruptStatus = portSET_INTERRUPT_MASK_FROM_ISR(); + { + uxReturn = pxEventBits->uxEventBits; + } + portCLEAR_INTERRUPT_MASK_FROM_ISR( uxSavedInterruptStatus ); + + return uxReturn; +} +/*-----------------------------------------------------------*/ + +EventBits_t xEventGroupSetBits( EventGroupHandle_t xEventGroup, const EventBits_t uxBitsToSet ) +{ +ListItem_t *pxListItem, *pxNext; +ListItem_t const *pxListEnd; +List_t *pxList; +EventBits_t uxBitsToClear = 0, uxBitsWaitedFor, uxControlBits; +EventGroup_t *pxEventBits = ( EventGroup_t * ) xEventGroup; +BaseType_t xMatchFound = pdFALSE; + + /* Check the user is not attempting to set the bits used by the kernel + itself. */ + configASSERT( xEventGroup ); + configASSERT( ( uxBitsToSet & eventEVENT_BITS_CONTROL_BYTES ) == 0 ); + + pxList = &( pxEventBits->xTasksWaitingForBits ); + pxListEnd = listGET_END_MARKER( pxList ); /*lint !e826 !e740 The mini list structure is used as the list end to save RAM. This is checked and valid. */ + vTaskSuspendAll(); + { + traceEVENT_GROUP_SET_BITS( xEventGroup, uxBitsToSet ); + + pxListItem = listGET_HEAD_ENTRY( pxList ); + + /* Set the bits. */ + pxEventBits->uxEventBits |= uxBitsToSet; + + /* See if the new bit value should unblock any tasks. */ + while( pxListItem != pxListEnd ) + { + pxNext = listGET_NEXT( pxListItem ); + uxBitsWaitedFor = listGET_LIST_ITEM_VALUE( pxListItem ); + xMatchFound = pdFALSE; + + /* Split the bits waited for from the control bits. */ + uxControlBits = uxBitsWaitedFor & eventEVENT_BITS_CONTROL_BYTES; + uxBitsWaitedFor &= ~eventEVENT_BITS_CONTROL_BYTES; + + if( ( uxControlBits & eventWAIT_FOR_ALL_BITS ) == ( EventBits_t ) 0 ) + { + /* Just looking for single bit being set. */ + if( ( uxBitsWaitedFor & pxEventBits->uxEventBits ) != ( EventBits_t ) 0 ) + { + xMatchFound = pdTRUE; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else if( ( uxBitsWaitedFor & pxEventBits->uxEventBits ) == uxBitsWaitedFor ) + { + /* All bits are set. */ + xMatchFound = pdTRUE; + } + else + { + /* Need all bits to be set, but not all the bits were set. */ + } + + if( xMatchFound != pdFALSE ) + { + /* The bits match. Should the bits be cleared on exit? */ + if( ( uxControlBits & eventCLEAR_EVENTS_ON_EXIT_BIT ) != ( EventBits_t ) 0 ) + { + uxBitsToClear |= uxBitsWaitedFor; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + /* Store the actual event flag value in the task's event list + item before removing the task from the event list. The + eventUNBLOCKED_DUE_TO_BIT_SET bit is set so the task knows + that is was unblocked due to its required bits matching, rather + than because it timed out. */ + ( void ) xTaskRemoveFromUnorderedEventList( pxListItem, pxEventBits->uxEventBits | eventUNBLOCKED_DUE_TO_BIT_SET ); + } + + /* Move onto the next list item. Note pxListItem->pxNext is not + used here as the list item may have been removed from the event list + and inserted into the ready/pending reading list. */ + pxListItem = pxNext; + } + + /* Clear any bits that matched when the eventCLEAR_EVENTS_ON_EXIT_BIT + bit was set in the control word. */ + pxEventBits->uxEventBits &= ~uxBitsToClear; + } + ( void ) xTaskResumeAll(); + + return pxEventBits->uxEventBits; +} +/*-----------------------------------------------------------*/ + +void vEventGroupDelete( EventGroupHandle_t xEventGroup ) +{ +EventGroup_t *pxEventBits = ( EventGroup_t * ) xEventGroup; +const List_t *pxTasksWaitingForBits = &( pxEventBits->xTasksWaitingForBits ); + + vTaskSuspendAll(); + { + traceEVENT_GROUP_DELETE( xEventGroup ); + + while( listCURRENT_LIST_LENGTH( pxTasksWaitingForBits ) > ( UBaseType_t ) 0 ) + { + /* Unblock the task, returning 0 as the event list is being deleted + and cannot therefore have any bits set. */ + configASSERT( pxTasksWaitingForBits->xListEnd.pxNext != ( ListItem_t * ) &( pxTasksWaitingForBits->xListEnd ) ); + ( void ) xTaskRemoveFromUnorderedEventList( pxTasksWaitingForBits->xListEnd.pxNext, eventUNBLOCKED_DUE_TO_BIT_SET ); + } + + vPortFree( pxEventBits ); + } + ( void ) xTaskResumeAll(); +} +/*-----------------------------------------------------------*/ + +/* For internal use only - execute a 'set bits' command that was pended from +an interrupt. */ +void vEventGroupSetBitsCallback( void *pvEventGroup, const uint32_t ulBitsToSet ) +{ + ( void ) xEventGroupSetBits( pvEventGroup, ( EventBits_t ) ulBitsToSet ); +} +/*-----------------------------------------------------------*/ + +/* For internal use only - execute a 'clear bits' command that was pended from +an interrupt. */ +void vEventGroupClearBitsCallback( void *pvEventGroup, const uint32_t ulBitsToClear ) +{ + ( void ) xEventGroupClearBits( pvEventGroup, ( EventBits_t ) ulBitsToClear ); +} +/*-----------------------------------------------------------*/ + +static BaseType_t prvTestWaitCondition( const EventBits_t uxCurrentEventBits, const EventBits_t uxBitsToWaitFor, const BaseType_t xWaitForAllBits ) +{ +BaseType_t xWaitConditionMet = pdFALSE; + + if( xWaitForAllBits == pdFALSE ) + { + /* Task only has to wait for one bit within uxBitsToWaitFor to be + set. Is one already set? */ + if( ( uxCurrentEventBits & uxBitsToWaitFor ) != ( EventBits_t ) 0 ) + { + xWaitConditionMet = pdTRUE; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + /* Task has to wait for all the bits in uxBitsToWaitFor to be set. + Are they set already? */ + if( ( uxCurrentEventBits & uxBitsToWaitFor ) == uxBitsToWaitFor ) + { + xWaitConditionMet = pdTRUE; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + + return xWaitConditionMet; +} +/*-----------------------------------------------------------*/ + +#if ( ( configUSE_TRACE_FACILITY == 1 ) && ( INCLUDE_xTimerPendFunctionCall == 1 ) && ( configUSE_TIMERS == 1 ) ) + + BaseType_t xEventGroupSetBitsFromISR( EventGroupHandle_t xEventGroup, const EventBits_t uxBitsToSet, BaseType_t *pxHigherPriorityTaskWoken ) + { + BaseType_t xReturn; + + traceEVENT_GROUP_SET_BITS_FROM_ISR( xEventGroup, uxBitsToSet ); + xReturn = xTimerPendFunctionCallFromISR( vEventGroupSetBitsCallback, ( void * ) xEventGroup, ( uint32_t ) uxBitsToSet, pxHigherPriorityTaskWoken ); + + return xReturn; + } + +#endif +/*-----------------------------------------------------------*/ + +#if (configUSE_TRACE_FACILITY == 1) + + UBaseType_t uxEventGroupGetNumber( void* xEventGroup ) + { + UBaseType_t xReturn; + EventGroup_t *pxEventBits = ( EventGroup_t * ) xEventGroup; + + if( xEventGroup == NULL ) + { + xReturn = 0; + } + else + { + xReturn = pxEventBits->uxEventGroupNumber; + } + + return xReturn; + } + +#endif + diff --git a/lib/FreeRTOS/include/FreeRTOS.h b/lib/FreeRTOS/include/FreeRTOS.h index 5024639deb..7d34ec63bb 100644 --- a/lib/FreeRTOS/include/FreeRTOS.h +++ b/lib/FreeRTOS/include/FreeRTOS.h @@ -1,158 +1,174 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ #ifndef INC_FREERTOS_H #define INC_FREERTOS_H - /* * Include the generic headers required for the FreeRTOS port being used. */ #include -/* Basic FreeRTOS definitions. */ -#include "projdefs.h" +/* + * If stdint.h cannot be located then: + * + If using GCC ensure the -nostdint options is *not* being used. + * + Ensure the project's include path includes the directory in which your + * compiler stores stdint.h. + * + Set any compiler options necessary for it to support C99, as technically + * stdint.h is only mandatory with C99 (FreeRTOS does not require C99 in any + * other way). + * + The FreeRTOS download includes a simple stdint.h definition that can be + * used in cases where none is provided by the compiler. The files only + * contains the typedefs required to build FreeRTOS. Read the instructions + * in FreeRTOS/source/stdint.readme for more information. + */ +#include /* READ COMMENT ABOVE. */ + +#ifdef __cplusplus +extern "C" { +#endif /* Application specific configuration options. */ #include "FreeRTOSConfig.h" -/* configUSE_PORT_OPTIMISED_TASK_SELECTION must be defined before portable.h -is included as it is used by the port layer. */ -#ifndef configUSE_PORT_OPTIMISED_TASK_SELECTION - #define configUSE_PORT_OPTIMISED_TASK_SELECTION 0 -#endif +/* Basic FreeRTOS definitions. */ +#include "projdefs.h" /* Definitions specific to the port being used. */ #include "portable.h" - -/* Defines the prototype to which the application task hook function must -conform. */ -typedef portBASE_TYPE (*pdTASK_HOOK_CODE)( void * ); - - - - - /* * Check all the required application specific macros have been defined. * These macros are application specific and (as downloaded) are defined * within FreeRTOSConfig.h. */ +#ifndef configMINIMAL_STACK_SIZE + #error Missing definition: configMINIMAL_STACK_SIZE must be defined in FreeRTOSConfig.h. configMINIMAL_STACK_SIZE defines the size (in words) of the stack allocated to the idle task. Refer to the demo project provided for your port for a suitable value. +#endif + +#ifndef configMAX_PRIORITIES + #error Missing definition: configMAX_PRIORITIES must be defined in FreeRTOSConfig.h. See the Configuration section of the FreeRTOS API documentation for details. +#endif + #ifndef configUSE_PREEMPTION - #error Missing definition: configUSE_PREEMPTION should be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. + #error Missing definition: configUSE_PREEMPTION must be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. #endif #ifndef configUSE_IDLE_HOOK - #error Missing definition: configUSE_IDLE_HOOK should be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. + #error Missing definition: configUSE_IDLE_HOOK must be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. #endif #ifndef configUSE_TICK_HOOK - #error Missing definition: configUSE_TICK_HOOK should be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. -#endif - -#ifndef configUSE_CO_ROUTINES - #error Missing definition: configUSE_CO_ROUTINES should be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. + #error Missing definition: configUSE_TICK_HOOK must be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. #endif #ifndef INCLUDE_vTaskPrioritySet - #error Missing definition: INCLUDE_vTaskPrioritySet should be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. + #error Missing definition: INCLUDE_vTaskPrioritySet must be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. #endif #ifndef INCLUDE_uxTaskPriorityGet - #error Missing definition: INCLUDE_uxTaskPriorityGet should be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. + #error Missing definition: INCLUDE_uxTaskPriorityGet must be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. #endif #ifndef INCLUDE_vTaskDelete - #error Missing definition: INCLUDE_vTaskDelete should be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. + #error Missing definition: INCLUDE_vTaskDelete must be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. #endif #ifndef INCLUDE_vTaskSuspend - #error Missing definition: INCLUDE_vTaskSuspend should be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. + #error Missing definition: INCLUDE_vTaskSuspend must be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. #endif #ifndef INCLUDE_vTaskDelayUntil - #error Missing definition: INCLUDE_vTaskDelayUntil should be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. + #error Missing definition: INCLUDE_vTaskDelayUntil must be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. #endif #ifndef INCLUDE_vTaskDelay - #error Missing definition: INCLUDE_vTaskDelay should be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. + #error Missing definition: INCLUDE_vTaskDelay must be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. #endif #ifndef configUSE_16_BIT_TICKS - #error Missing definition: configUSE_16_BIT_TICKS should be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. + #error Missing definition: configUSE_16_BIT_TICKS must be defined in FreeRTOSConfig.h as either 1 or 0. See the Configuration section of the FreeRTOS API documentation for details. +#endif + +#ifndef configMAX_PRIORITIES + #error configMAX_PRIORITIES must be defined to be greater than or equal to 1. +#endif + +#ifndef configUSE_CO_ROUTINES + #define configUSE_CO_ROUTINES 0 +#endif + +#if configUSE_CO_ROUTINES != 0 + #ifndef configMAX_CO_ROUTINE_PRIORITIES + #error configMAX_CO_ROUTINE_PRIORITIES must be greater than or equal to 1. + #endif #endif #ifndef INCLUDE_xTaskGetIdleTaskHandle @@ -179,6 +195,10 @@ typedef portBASE_TYPE (*pdTASK_HOOK_CODE)( void * ); #define configUSE_APPLICATION_TASK_TAG 0 #endif +#ifndef configNUM_THREAD_LOCAL_STORAGE_POINTERS + #define configNUM_THREAD_LOCAL_STORAGE_POINTERS 0 +#endif + #ifndef INCLUDE_uxTaskGetStackHighWaterMark #define INCLUDE_uxTaskGetStackHighWaterMark 0 #endif @@ -227,12 +247,19 @@ typedef portBASE_TYPE (*pdTASK_HOOK_CODE)( void * ); #define INCLUDE_xTaskResumeFromISR 1 #endif -#ifndef configASSERT - #define configASSERT( x ) +#ifndef INCLUDE_xEventGroupSetBitFromISR + #define INCLUDE_xEventGroupSetBitFromISR 0 +#endif + +#ifndef INCLUDE_xTimerPendFunctionCall + #define INCLUDE_xTimerPendFunctionCall 0 #endif -#ifndef portALIGNMENT_ASSERT_pxCurrentTCB - #define portALIGNMENT_ASSERT_pxCurrentTCB configASSERT +#ifndef configASSERT + #define configASSERT( x ) + #define configASSERT_DEFINED 0 +#else + #define configASSERT_DEFINED 1 #endif /* The timers module relies on xTaskGetSchedulerState(). */ @@ -273,6 +300,10 @@ typedef portBASE_TYPE (*pdTASK_HOOK_CODE)( void * ); #define portCLEAN_UP_TCB( pxTCB ) ( void ) pxTCB #endif +#ifndef portPRE_TASK_DELETE_HOOK + #define portPRE_TASK_DELETE_HOOK( pvTaskToDelete, pxYieldPending ) +#endif + #ifndef portSETUP_TCB #define portSETUP_TCB( pxTCB ) ( void ) pxTCB #endif @@ -287,7 +318,7 @@ typedef portBASE_TYPE (*pdTASK_HOOK_CODE)( void * ); #endif #ifndef portPOINTER_SIZE_TYPE - #define portPOINTER_SIZE_TYPE unsigned long + #define portPOINTER_SIZE_TYPE uint32_t #endif /* Remove any unused trace macros. */ @@ -309,6 +340,22 @@ typedef portBASE_TYPE (*pdTASK_HOOK_CODE)( void * ); #define traceTASK_SWITCHED_IN() #endif +#ifndef traceINCREASE_TICK_COUNT + /* Called before stepping the tick count after waking from tickless idle + sleep. */ + #define traceINCREASE_TICK_COUNT( x ) +#endif + +#ifndef traceLOW_POWER_IDLE_BEGIN + /* Called immediately before entering tickless idle. */ + #define traceLOW_POWER_IDLE_BEGIN() +#endif + +#ifndef traceLOW_POWER_IDLE_END + /* Called when returning to the Idle task after a tickless idle. */ + #define traceLOW_POWER_IDLE_END() +#endif + #ifndef traceTASK_SWITCHED_OUT /* Called before a task has been selected to run. pxCurrentTCB holds a pointer to the task control block of the task being switched out. */ @@ -414,6 +461,10 @@ typedef portBASE_TYPE (*pdTASK_HOOK_CODE)( void * ); #define traceQUEUE_PEEK( pxQueue ) #endif +#ifndef traceQUEUE_PEEK_FROM_ISR + #define traceQUEUE_PEEK_FROM_ISR( pxQueue ) +#endif + #ifndef traceQUEUE_RECEIVE_FAILED #define traceQUEUE_RECEIVE_FAILED( pxQueue ) #endif @@ -434,6 +485,10 @@ typedef portBASE_TYPE (*pdTASK_HOOK_CODE)( void * ); #define traceQUEUE_RECEIVE_FROM_ISR_FAILED( pxQueue ) #endif +#ifndef traceQUEUE_PEEK_FROM_ISR_FAILED + #define traceQUEUE_PEEK_FROM_ISR_FAILED( pxQueue ) +#endif + #ifndef traceQUEUE_DELETE #define traceQUEUE_DELETE( pxQueue ) #endif @@ -498,6 +553,98 @@ typedef portBASE_TYPE (*pdTASK_HOOK_CODE)( void * ); #define traceTIMER_COMMAND_RECEIVED( pxTimer, xMessageID, xMessageValue ) #endif +#ifndef traceMALLOC + #define traceMALLOC( pvAddress, uiSize ) +#endif + +#ifndef traceFREE + #define traceFREE( pvAddress, uiSize ) +#endif + +#ifndef traceEVENT_GROUP_CREATE + #define traceEVENT_GROUP_CREATE( xEventGroup ) +#endif + +#ifndef traceEVENT_GROUP_CREATE_FAILED + #define traceEVENT_GROUP_CREATE_FAILED() +#endif + +#ifndef traceEVENT_GROUP_SYNC_BLOCK + #define traceEVENT_GROUP_SYNC_BLOCK( xEventGroup, uxBitsToSet, uxBitsToWaitFor ) +#endif + +#ifndef traceEVENT_GROUP_SYNC_END + #define traceEVENT_GROUP_SYNC_END( xEventGroup, uxBitsToSet, uxBitsToWaitFor, xTimeoutOccurred ) ( void ) xTimeoutOccurred +#endif + +#ifndef traceEVENT_GROUP_WAIT_BITS_BLOCK + #define traceEVENT_GROUP_WAIT_BITS_BLOCK( xEventGroup, uxBitsToWaitFor ) +#endif + +#ifndef traceEVENT_GROUP_WAIT_BITS_END + #define traceEVENT_GROUP_WAIT_BITS_END( xEventGroup, uxBitsToWaitFor, xTimeoutOccurred ) ( void ) xTimeoutOccurred +#endif + +#ifndef traceEVENT_GROUP_CLEAR_BITS + #define traceEVENT_GROUP_CLEAR_BITS( xEventGroup, uxBitsToClear ) +#endif + +#ifndef traceEVENT_GROUP_CLEAR_BITS_FROM_ISR + #define traceEVENT_GROUP_CLEAR_BITS_FROM_ISR( xEventGroup, uxBitsToClear ) +#endif + +#ifndef traceEVENT_GROUP_SET_BITS + #define traceEVENT_GROUP_SET_BITS( xEventGroup, uxBitsToSet ) +#endif + +#ifndef traceEVENT_GROUP_SET_BITS_FROM_ISR + #define traceEVENT_GROUP_SET_BITS_FROM_ISR( xEventGroup, uxBitsToSet ) +#endif + +#ifndef traceEVENT_GROUP_DELETE + #define traceEVENT_GROUP_DELETE( xEventGroup ) +#endif + +#ifndef tracePEND_FUNC_CALL + #define tracePEND_FUNC_CALL(xFunctionToPend, pvParameter1, ulParameter2, ret) +#endif + +#ifndef tracePEND_FUNC_CALL_FROM_ISR + #define tracePEND_FUNC_CALL_FROM_ISR(xFunctionToPend, pvParameter1, ulParameter2, ret) +#endif + +#ifndef traceQUEUE_REGISTRY_ADD + #define traceQUEUE_REGISTRY_ADD(xQueue, pcQueueName) +#endif + +#ifndef traceTASK_NOTIFY_TAKE_BLOCK + #define traceTASK_NOTIFY_TAKE_BLOCK() +#endif + +#ifndef traceTASK_NOTIFY_TAKE + #define traceTASK_NOTIFY_TAKE() +#endif + +#ifndef traceTASK_NOTIFY_WAIT_BLOCK + #define traceTASK_NOTIFY_WAIT_BLOCK() +#endif + +#ifndef traceTASK_NOTIFY_WAIT + #define traceTASK_NOTIFY_WAIT() +#endif + +#ifndef traceTASK_NOTIFY + #define traceTASK_NOTIFY() +#endif + +#ifndef traceTASK_NOTIFY_FROM_ISR + #define traceTASK_NOTIFY_FROM_ISR() +#endif + +#ifndef traceTASK_NOTIFY_GIVE_FROM_ISR + #define traceTASK_NOTIFY_GIVE_FROM_ISR() +#endif + #ifndef configGENERATE_RUN_TIME_STATS #define configGENERATE_RUN_TIME_STATS 0 #endif @@ -525,7 +672,7 @@ typedef portBASE_TYPE (*pdTASK_HOOK_CODE)( void * ); #endif #ifndef portPRIVILEGE_BIT - #define portPRIVILEGE_BIT ( ( unsigned portBASE_TYPE ) 0x00 ) + #define portPRIVILEGE_BIT ( ( UBaseType_t ) 0x00 ) #endif #ifndef portYIELD_WITHIN_API @@ -568,8 +715,121 @@ typedef portBASE_TYPE (*pdTASK_HOOK_CODE)( void * ); #define configUSE_QUEUE_SETS 0 #endif -/* For backward compatability. */ -#define eTaskStateGet eTaskGetState +#ifndef portTASK_USES_FLOATING_POINT + #define portTASK_USES_FLOATING_POINT() +#endif + +#ifndef configUSE_TIME_SLICING + #define configUSE_TIME_SLICING 1 +#endif + +#ifndef configINCLUDE_APPLICATION_DEFINED_PRIVILEGED_FUNCTIONS + #define configINCLUDE_APPLICATION_DEFINED_PRIVILEGED_FUNCTIONS 0 +#endif + +#ifndef configUSE_NEWLIB_REENTRANT + #define configUSE_NEWLIB_REENTRANT 0 +#endif + +#ifndef configUSE_STATS_FORMATTING_FUNCTIONS + #define configUSE_STATS_FORMATTING_FUNCTIONS 0 +#endif + +#ifndef portASSERT_IF_INTERRUPT_PRIORITY_INVALID + #define portASSERT_IF_INTERRUPT_PRIORITY_INVALID() +#endif + +#ifndef configUSE_TRACE_FACILITY + #define configUSE_TRACE_FACILITY 0 +#endif + +#ifndef mtCOVERAGE_TEST_MARKER + #define mtCOVERAGE_TEST_MARKER() +#endif + +#ifndef mtCOVERAGE_TEST_DELAY + #define mtCOVERAGE_TEST_DELAY() +#endif + +#ifndef portASSERT_IF_IN_ISR + #define portASSERT_IF_IN_ISR() +#endif + +#ifndef configUSE_PORT_OPTIMISED_TASK_SELECTION + #define configUSE_PORT_OPTIMISED_TASK_SELECTION 0 +#endif + +#ifndef configAPPLICATION_ALLOCATED_HEAP + #define configAPPLICATION_ALLOCATED_HEAP 0 +#endif + +#ifndef configUSE_TASK_NOTIFICATIONS + #define configUSE_TASK_NOTIFICATIONS 1 +#endif + +#ifndef portTICK_TYPE_IS_ATOMIC + #define portTICK_TYPE_IS_ATOMIC 0 +#endif + +#if( portTICK_TYPE_IS_ATOMIC == 0 ) + /* Either variables of tick type cannot be read atomically, or + portTICK_TYPE_IS_ATOMIC was not set - map the critical sections used when + the tick count is returned to the standard critical section macros. */ + #define portTICK_TYPE_ENTER_CRITICAL() portENTER_CRITICAL() + #define portTICK_TYPE_EXIT_CRITICAL() portEXIT_CRITICAL() + #define portTICK_TYPE_SET_INTERRUPT_MASK_FROM_ISR() portSET_INTERRUPT_MASK_FROM_ISR() + #define portTICK_TYPE_CLEAR_INTERRUPT_MASK_FROM_ISR( x ) portCLEAR_INTERRUPT_MASK_FROM_ISR( ( x ) ) +#else + /* The tick type can be read atomically, so critical sections used when the + tick count is returned can be defined away. */ + #define portTICK_TYPE_ENTER_CRITICAL() + #define portTICK_TYPE_EXIT_CRITICAL() + #define portTICK_TYPE_SET_INTERRUPT_MASK_FROM_ISR() 0 + #define portTICK_TYPE_CLEAR_INTERRUPT_MASK_FROM_ISR( x ) ( void ) x +#endif + +/* Definitions to allow backward compatibility with FreeRTOS versions prior to +V8 if desired. */ +#ifndef configENABLE_BACKWARD_COMPATIBILITY + #define configENABLE_BACKWARD_COMPATIBILITY 1 +#endif + +#if configENABLE_BACKWARD_COMPATIBILITY == 1 + #define eTaskStateGet eTaskGetState + #define portTickType TickType_t + #define xTaskHandle TaskHandle_t + #define xQueueHandle QueueHandle_t + #define xSemaphoreHandle SemaphoreHandle_t + #define xQueueSetHandle QueueSetHandle_t + #define xQueueSetMemberHandle QueueSetMemberHandle_t + #define xTimeOutType TimeOut_t + #define xMemoryRegion MemoryRegion_t + #define xTaskParameters TaskParameters_t + #define xTaskStatusType TaskStatus_t + #define xTimerHandle TimerHandle_t + #define xCoRoutineHandle CoRoutineHandle_t + #define pdTASK_HOOK_CODE TaskHookFunction_t + #define portTICK_RATE_MS portTICK_PERIOD_MS + + /* Backward compatibility within the scheduler code only - these definitions + are not really required but are included for completeness. */ + #define tmrTIMER_CALLBACK TimerCallbackFunction_t + #define pdTASK_CODE TaskFunction_t + #define xListItem ListItem_t + #define xList List_t +#endif /* configENABLE_BACKWARD_COMPATIBILITY */ + +/* Set configUSE_TASK_FPU_SUPPORT to 0 to omit floating point support even +if floating point hardware is otherwise supported by the FreeRTOS port in use. +This constant is not supported by all FreeRTOS ports that include floating +point support. */ +#ifndef configUSE_TASK_FPU_SUPPORT + #define configUSE_TASK_FPU_SUPPORT 1 +#endif + +#ifdef __cplusplus +} +#endif #endif /* INC_FREERTOS_H */ diff --git a/lib/FreeRTOS/include/StackMacros.h b/lib/FreeRTOS/include/StackMacros.h index d8080096b6..1359090c32 100644 --- a/lib/FreeRTOS/include/StackMacros.h +++ b/lib/FreeRTOS/include/StackMacros.h @@ -1,75 +1,70 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. + + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ #ifndef STACK_MACROS_H @@ -91,49 +86,31 @@ /*-----------------------------------------------------------*/ -#if( configCHECK_FOR_STACK_OVERFLOW == 0 ) - - /* FreeRTOSConfig.h is not set to check for stack overflows. */ - #define taskFIRST_CHECK_FOR_STACK_OVERFLOW() - #define taskSECOND_CHECK_FOR_STACK_OVERFLOW() - -#endif /* configCHECK_FOR_STACK_OVERFLOW == 0 */ -/*-----------------------------------------------------------*/ - -#if( configCHECK_FOR_STACK_OVERFLOW == 1 ) - - /* FreeRTOSConfig.h is only set to use the first method of - overflow checking. */ - #define taskSECOND_CHECK_FOR_STACK_OVERFLOW() - -#endif -/*-----------------------------------------------------------*/ - -#if( ( configCHECK_FOR_STACK_OVERFLOW > 0 ) && ( portSTACK_GROWTH < 0 ) ) +#if( ( configCHECK_FOR_STACK_OVERFLOW == 1 ) && ( portSTACK_GROWTH < 0 ) ) /* Only the current stack state is to be checked. */ - #define taskFIRST_CHECK_FOR_STACK_OVERFLOW() \ + #define taskCHECK_FOR_STACK_OVERFLOW() \ { \ /* Is the currently saved stack pointer within the stack limit? */ \ if( pxCurrentTCB->pxTopOfStack <= pxCurrentTCB->pxStack ) \ { \ - vApplicationStackOverflowHook( ( xTaskHandle ) pxCurrentTCB, pxCurrentTCB->pcTaskName ); \ + vApplicationStackOverflowHook( ( TaskHandle_t ) pxCurrentTCB, pxCurrentTCB->pcTaskName ); \ } \ } -#endif /* configCHECK_FOR_STACK_OVERFLOW > 0 */ +#endif /* configCHECK_FOR_STACK_OVERFLOW == 1 */ /*-----------------------------------------------------------*/ -#if( ( configCHECK_FOR_STACK_OVERFLOW > 0 ) && ( portSTACK_GROWTH > 0 ) ) +#if( ( configCHECK_FOR_STACK_OVERFLOW == 1 ) && ( portSTACK_GROWTH > 0 ) ) /* Only the current stack state is to be checked. */ - #define taskFIRST_CHECK_FOR_STACK_OVERFLOW() \ + #define taskCHECK_FOR_STACK_OVERFLOW() \ { \ \ /* Is the currently saved stack pointer within the stack limit? */ \ if( pxCurrentTCB->pxTopOfStack >= pxCurrentTCB->pxEndOfStack ) \ { \ - vApplicationStackOverflowHook( ( xTaskHandle ) pxCurrentTCB, pxCurrentTCB->pcTaskName ); \ + vApplicationStackOverflowHook( ( TaskHandle_t ) pxCurrentTCB, pxCurrentTCB->pcTaskName ); \ } \ } @@ -142,20 +119,18 @@ #if( ( configCHECK_FOR_STACK_OVERFLOW > 1 ) && ( portSTACK_GROWTH < 0 ) ) - #define taskSECOND_CHECK_FOR_STACK_OVERFLOW() \ - { \ - static const unsigned char ucExpectedStackBytes[] = { tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, \ - tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, \ - tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, \ - tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, \ - tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE }; \ - \ - \ - /* Has the extremity of the task stack ever been written over? */ \ - if( memcmp( ( void * ) pxCurrentTCB->pxStack, ( void * ) ucExpectedStackBytes, sizeof( ucExpectedStackBytes ) ) != 0 ) \ - { \ - vApplicationStackOverflowHook( ( xTaskHandle ) pxCurrentTCB, pxCurrentTCB->pcTaskName ); \ - } \ + #define taskCHECK_FOR_STACK_OVERFLOW() \ + { \ + const uint32_t * const pulStack = ( uint32_t * ) pxCurrentTCB->pxStack; \ + const uint32_t ulCheckValue = ( uint32_t ) 0xa5a5a5a5; \ + \ + if( ( pulStack[ 0 ] != ulCheckValue ) || \ + ( pulStack[ 1 ] != ulCheckValue ) || \ + ( pulStack[ 2 ] != ulCheckValue ) || \ + ( pulStack[ 3 ] != ulCheckValue ) ) \ + { \ + vApplicationStackOverflowHook( ( TaskHandle_t ) pxCurrentTCB, pxCurrentTCB->pcTaskName ); \ + } \ } #endif /* #if( configCHECK_FOR_STACK_OVERFLOW > 1 ) */ @@ -163,27 +138,34 @@ #if( ( configCHECK_FOR_STACK_OVERFLOW > 1 ) && ( portSTACK_GROWTH > 0 ) ) - #define taskSECOND_CHECK_FOR_STACK_OVERFLOW() \ - { \ - char *pcEndOfStack = ( char * ) pxCurrentTCB->pxEndOfStack; \ - static const unsigned char ucExpectedStackBytes[] = { tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, \ - tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, \ - tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, \ - tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, \ - tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE }; \ - \ - \ - pcEndOfStack -= sizeof( ucExpectedStackBytes ); \ - \ - /* Has the extremity of the task stack ever been written over? */ \ - if( memcmp( ( void * ) pcEndOfStack, ( void * ) ucExpectedStackBytes, sizeof( ucExpectedStackBytes ) ) != 0 ) \ - { \ - vApplicationStackOverflowHook( ( xTaskHandle ) pxCurrentTCB, pxCurrentTCB->pcTaskName ); \ - } \ + #define taskCHECK_FOR_STACK_OVERFLOW() \ + { \ + int8_t *pcEndOfStack = ( int8_t * ) pxCurrentTCB->pxEndOfStack; \ + static const uint8_t ucExpectedStackBytes[] = { tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, \ + tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, \ + tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, \ + tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, \ + tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE }; \ + \ + \ + pcEndOfStack -= sizeof( ucExpectedStackBytes ); \ + \ + /* Has the extremity of the task stack ever been written over? */ \ + if( memcmp( ( void * ) pcEndOfStack, ( void * ) ucExpectedStackBytes, sizeof( ucExpectedStackBytes ) ) != 0 ) \ + { \ + vApplicationStackOverflowHook( ( TaskHandle_t ) pxCurrentTCB, pxCurrentTCB->pcTaskName ); \ + } \ } #endif /* #if( configCHECK_FOR_STACK_OVERFLOW > 1 ) */ /*-----------------------------------------------------------*/ +/* Remove stack overflow macro if not being used. */ +#ifndef taskCHECK_FOR_STACK_OVERFLOW + #define taskCHECK_FOR_STACK_OVERFLOW() +#endif + + + #endif /* STACK_MACROS_H */ diff --git a/lib/FreeRTOS/include/croutine.h b/lib/FreeRTOS/include/croutine.h index 361bd1ea1a..dc655ae20c 100644 --- a/lib/FreeRTOS/include/croutine.h +++ b/lib/FreeRTOS/include/croutine.h @@ -1,75 +1,70 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. + + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ #ifndef CO_ROUTINE_H @@ -88,28 +83,28 @@ extern "C" { /* Used to hide the implementation of the co-routine control block. The control block structure however has to be included in the header due to the macro implementation of the co-routine functionality. */ -typedef void * xCoRoutineHandle; +typedef void * CoRoutineHandle_t; /* Defines the prototype to which co-routine functions must conform. */ -typedef void (*crCOROUTINE_CODE)( xCoRoutineHandle, unsigned portBASE_TYPE ); +typedef void (*crCOROUTINE_CODE)( CoRoutineHandle_t, UBaseType_t ); typedef struct corCoRoutineControlBlock { - crCOROUTINE_CODE pxCoRoutineFunction; - xListItem xGenericListItem; /*< List item used to place the CRCB in ready and blocked queues. */ - xListItem xEventListItem; /*< List item used to place the CRCB in event lists. */ - unsigned portBASE_TYPE uxPriority; /*< The priority of the co-routine in relation to other co-routines. */ - unsigned portBASE_TYPE uxIndex; /*< Used to distinguish between co-routines when multiple co-routines use the same co-routine function. */ - unsigned short uxState; /*< Used internally by the co-routine implementation. */ -} corCRCB; /* Co-routine control block. Note must be identical in size down to uxPriority with tskTCB. */ + crCOROUTINE_CODE pxCoRoutineFunction; + ListItem_t xGenericListItem; /*< List item used to place the CRCB in ready and blocked queues. */ + ListItem_t xEventListItem; /*< List item used to place the CRCB in event lists. */ + UBaseType_t uxPriority; /*< The priority of the co-routine in relation to other co-routines. */ + UBaseType_t uxIndex; /*< Used to distinguish between co-routines when multiple co-routines use the same co-routine function. */ + uint16_t uxState; /*< Used internally by the co-routine implementation. */ +} CRCB_t; /* Co-routine control block. Note must be identical in size down to uxPriority with TCB_t. */ /** * croutine. h *
- portBASE_TYPE xCoRoutineCreate(
+ BaseType_t xCoRoutineCreate(
                                  crCOROUTINE_CODE pxCoRoutineCode,
-                                 unsigned portBASE_TYPE uxPriority,
-                                 unsigned portBASE_TYPE uxIndex
+                                 UBaseType_t uxPriority,
+                                 UBaseType_t uxIndex
                                );
* * Create a new co-routine and add it to the list of co-routines that are @@ -132,12 +127,12 @@ typedef struct corCoRoutineControlBlock * Example usage:
  // Co-routine to be created.
- void vFlashCoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
+ void vFlashCoRoutine( CoRoutineHandle_t xHandle, UBaseType_t uxIndex )
  {
  // Variables in co-routines must be declared static if they must maintain value across a blocking call.
  // This may not be necessary for const variables.
  static const char cLedToFlash[ 2 ] = { 5, 6 };
- static const portTickType uxFlashRates[ 2 ] = { 200, 400 };
+ static const TickType_t uxFlashRates[ 2 ] = { 200, 400 };
 
      // Must start every co-routine with a call to crSTART();
      crSTART( xHandle );
@@ -147,7 +142,7 @@ typedef struct corCoRoutineControlBlock
          // This co-routine just delays for a fixed period, then toggles
          // an LED.  Two co-routines are created using this function, so
          // the uxIndex parameter is used to tell the co-routine which
-         // LED to flash and how long to delay.  This assumes xQueue has
+         // LED to flash and how int32_t to delay.  This assumes xQueue has
          // already been created.
          vParTestToggleLED( cLedToFlash[ uxIndex ] );
          crDELAY( xHandle, uxFlashRates[ uxIndex ] );
@@ -160,9 +155,9 @@ typedef struct corCoRoutineControlBlock
  // Function that creates two co-routines.
  void vOtherFunction( void )
  {
- unsigned char ucParameterToPass;
- xTaskHandle xHandle;
-		
+ uint8_t ucParameterToPass;
+ TaskHandle_t xHandle;
+
      // Create two co-routines at priority 0.  The first is given index 0
      // so (from the code above) toggles LED 5 every 200 ticks.  The second
      // is given index 1 so toggles LED 6 every 400 ticks.
@@ -175,7 +170,7 @@ typedef struct corCoRoutineControlBlock
  * \defgroup xCoRoutineCreate xCoRoutineCreate
  * \ingroup Tasks
  */
-signed portBASE_TYPE xCoRoutineCreate( crCOROUTINE_CODE pxCoRoutineCode, unsigned portBASE_TYPE uxPriority, unsigned portBASE_TYPE uxIndex );
+BaseType_t xCoRoutineCreate( crCOROUTINE_CODE pxCoRoutineCode, UBaseType_t uxPriority, UBaseType_t uxIndex );
 
 
 /**
@@ -222,17 +217,17 @@ void vCoRoutineSchedule( void );
 /**
  * croutine. h
  * 
- crSTART( xCoRoutineHandle xHandle );
+ crSTART( CoRoutineHandle_t xHandle );
* * This macro MUST always be called at the start of a co-routine function. * * Example usage:
  // Co-routine to be created.
- void vACoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
+ void vACoRoutine( CoRoutineHandle_t xHandle, UBaseType_t uxIndex )
  {
  // Variables in co-routines must be declared static if they must maintain value across a blocking call.
- static long ulAVariable;
+ static int32_t ulAVariable;
 
      // Must start every co-routine with a call to crSTART();
      crSTART( xHandle );
@@ -248,7 +243,7 @@ void vCoRoutineSchedule( void );
  * \defgroup crSTART crSTART
  * \ingroup Tasks
  */
-#define crSTART( pxCRCB ) switch( ( ( corCRCB * )( pxCRCB ) )->uxState ) { case 0:
+#define crSTART( pxCRCB ) switch( ( ( CRCB_t * )( pxCRCB ) )->uxState ) { case 0:
 
 /**
  * croutine. h
@@ -260,10 +255,10 @@ void vCoRoutineSchedule( void );
  * Example usage:
    
  // Co-routine to be created.
- void vACoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
+ void vACoRoutine( CoRoutineHandle_t xHandle, UBaseType_t uxIndex )
  {
  // Variables in co-routines must be declared static if they must maintain value across a blocking call.
- static long ulAVariable;
+ static int32_t ulAVariable;
 
      // Must start every co-routine with a call to crSTART();
      crSTART( xHandle );
@@ -285,13 +280,13 @@ void vCoRoutineSchedule( void );
  * These macros are intended for internal use by the co-routine implementation
  * only.  The macros should not be used directly by application writers.
  */
-#define crSET_STATE0( xHandle ) ( ( corCRCB * )( xHandle ) )->uxState = (__LINE__ * 2); return; case (__LINE__ * 2):
-#define crSET_STATE1( xHandle ) ( ( corCRCB * )( xHandle ) )->uxState = ((__LINE__ * 2)+1); return; case ((__LINE__ * 2)+1):
+#define crSET_STATE0( xHandle ) ( ( CRCB_t * )( xHandle ) )->uxState = (__LINE__ * 2); return; case (__LINE__ * 2):
+#define crSET_STATE1( xHandle ) ( ( CRCB_t * )( xHandle ) )->uxState = ((__LINE__ * 2)+1); return; case ((__LINE__ * 2)+1):
 
 /**
  * croutine. h
  *
- crDELAY( xCoRoutineHandle xHandle, portTickType xTicksToDelay );
+ crDELAY( CoRoutineHandle_t xHandle, TickType_t xTicksToDelay );
* * Delay a co-routine for a fixed period of time. * @@ -304,18 +299,18 @@ void vCoRoutineSchedule( void ); * * @param xTickToDelay The number of ticks that the co-routine should delay * for. The actual amount of time this equates to is defined by - * configTICK_RATE_HZ (set in FreeRTOSConfig.h). The constant portTICK_RATE_MS + * configTICK_RATE_HZ (set in FreeRTOSConfig.h). The constant portTICK_PERIOD_MS * can be used to convert ticks to milliseconds. * * Example usage:
  // Co-routine to be created.
- void vACoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
+ void vACoRoutine( CoRoutineHandle_t xHandle, UBaseType_t uxIndex )
  {
  // Variables in co-routines must be declared static if they must maintain value across a blocking call.
  // This may not be necessary for const variables.
  // We are to delay for 200ms.
- static const xTickType xDelayTime = 200 / portTICK_RATE_MS;
+ static const xTickType xDelayTime = 200 / portTICK_PERIOD_MS;
 
      // Must start every co-routine with a call to crSTART();
      crSTART( xHandle );
@@ -344,11 +339,11 @@ void vCoRoutineSchedule( void );
 /**
  * 
  crQUEUE_SEND(
-                  xCoRoutineHandle xHandle,
-                  xQueueHandle pxQueue,
+                  CoRoutineHandle_t xHandle,
+                  QueueHandle_t pxQueue,
                   void *pvItemToQueue,
-                  portTickType xTicksToWait,
-                  portBASE_TYPE *pxResult
+                  TickType_t xTicksToWait,
+                  BaseType_t *pxResult
              )
* * The macro's crQUEUE_SEND() and crQUEUE_RECEIVE() are the co-routine @@ -381,7 +376,7 @@ void vCoRoutineSchedule( void ); * to wait for space to become available on the queue, should space not be * available immediately. The actual amount of time this equates to is defined * by configTICK_RATE_HZ (set in FreeRTOSConfig.h). The constant - * portTICK_RATE_MS can be used to convert ticks to milliseconds (see example + * portTICK_PERIOD_MS can be used to convert ticks to milliseconds (see example * below). * * @param pxResult The variable pointed to by pxResult will be set to pdPASS if @@ -392,11 +387,11 @@ void vCoRoutineSchedule( void );
  // Co-routine function that blocks for a fixed period then posts a number onto
  // a queue.
- static void prvCoRoutineFlashTask( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
+ static void prvCoRoutineFlashTask( CoRoutineHandle_t xHandle, UBaseType_t uxIndex )
  {
  // Variables in co-routines must be declared static if they must maintain value across a blocking call.
- static portBASE_TYPE xNumberToPost = 0;
- static portBASE_TYPE xResult;
+ static BaseType_t xNumberToPost = 0;
+ static BaseType_t xResult;
 
     // Co-routines must begin with a call to crSTART().
     crSTART( xHandle );
@@ -443,11 +438,11 @@ void vCoRoutineSchedule( void );
  * croutine. h
  * 
   crQUEUE_RECEIVE(
-                     xCoRoutineHandle xHandle,
-                     xQueueHandle pxQueue,
+                     CoRoutineHandle_t xHandle,
+                     QueueHandle_t pxQueue,
                      void *pvBuffer,
-                     portTickType xTicksToWait,
-                     portBASE_TYPE *pxResult
+                     TickType_t xTicksToWait,
+                     BaseType_t *pxResult
                  )
* * The macro's crQUEUE_SEND() and crQUEUE_RECEIVE() are the co-routine @@ -479,7 +474,7 @@ void vCoRoutineSchedule( void ); * to wait for data to become available from the queue, should data not be * available immediately. The actual amount of time this equates to is defined * by configTICK_RATE_HZ (set in FreeRTOSConfig.h). The constant - * portTICK_RATE_MS can be used to convert ticks to milliseconds (see the + * portTICK_PERIOD_MS can be used to convert ticks to milliseconds (see the * crQUEUE_SEND example). * * @param pxResult The variable pointed to by pxResult will be set to pdPASS if @@ -490,11 +485,11 @@ void vCoRoutineSchedule( void );
  // A co-routine receives the number of an LED to flash from a queue.  It
  // blocks on the queue until the number is received.
- static void prvCoRoutineFlashWorkTask( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
+ static void prvCoRoutineFlashWorkTask( CoRoutineHandle_t xHandle, UBaseType_t uxIndex )
  {
  // Variables in co-routines must be declared static if they must maintain value across a blocking call.
- static portBASE_TYPE xResult;
- static unsigned portBASE_TYPE uxLEDToFlash;
+ static BaseType_t xResult;
+ static UBaseType_t uxLEDToFlash;
 
     // All co-routines must start with a call to crSTART().
     crSTART( xHandle );
@@ -535,9 +530,9 @@ void vCoRoutineSchedule( void );
  * croutine. h
  * 
   crQUEUE_SEND_FROM_ISR(
-                            xQueueHandle pxQueue,
+                            QueueHandle_t pxQueue,
                             void *pvItemToQueue,
-                            portBASE_TYPE xCoRoutinePreviouslyWoken
+                            BaseType_t xCoRoutinePreviouslyWoken
                        )
* * The macro's crQUEUE_SEND_FROM_ISR() and crQUEUE_RECEIVE_FROM_ISR() are the @@ -575,10 +570,10 @@ void vCoRoutineSchedule( void ); * Example usage:
  // A co-routine that blocks on a queue waiting for characters to be received.
- static void vReceivingCoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
+ static void vReceivingCoRoutine( CoRoutineHandle_t xHandle, UBaseType_t uxIndex )
  {
  char cRxedChar;
- portBASE_TYPE xResult;
+ BaseType_t xResult;
 
      // All co-routines must start with a call to crSTART().
      crSTART( xHandle );
@@ -605,7 +600,7 @@ void vCoRoutineSchedule( void );
  void vUART_ISR( void )
  {
  char cRxedChar;
- portBASE_TYPE xCRWokenByPost = pdFALSE;
+ BaseType_t xCRWokenByPost = pdFALSE;
 
      // We loop around reading characters until there are none left in the UART.
      while( UART_RX_REG_NOT_EMPTY() )
@@ -632,9 +627,9 @@ void vCoRoutineSchedule( void );
  * croutine. h
  * 
   crQUEUE_SEND_FROM_ISR(
-                            xQueueHandle pxQueue,
+                            QueueHandle_t pxQueue,
                             void *pvBuffer,
-                            portBASE_TYPE * pxCoRoutineWoken
+                            BaseType_t * pxCoRoutineWoken
                        )
* * The macro's crQUEUE_SEND_FROM_ISR() and crQUEUE_RECEIVE_FROM_ISR() are the @@ -673,12 +668,12 @@ void vCoRoutineSchedule( void );
  // A co-routine that posts a character to a queue then blocks for a fixed
  // period.  The character is incremented each time.
- static void vSendingCoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
+ static void vSendingCoRoutine( CoRoutineHandle_t xHandle, UBaseType_t uxIndex )
  {
  // cChar holds its value while this co-routine is blocked and must therefore
  // be declared static.
  static char cCharToTx = 'a';
- portBASE_TYPE xResult;
+ BaseType_t xResult;
 
      // All co-routines must start with a call to crSTART().
      crSTART( xHandle );
@@ -721,7 +716,7 @@ void vCoRoutineSchedule( void );
  void vUART_ISR( void )
  {
  char cCharToTx;
- portBASE_TYPE xCRWokenByPost = pdFALSE;
+ BaseType_t xCRWokenByPost = pdFALSE;
 
      while( UART_TX_REG_EMPTY() )
      {
@@ -749,7 +744,7 @@ void vCoRoutineSchedule( void );
  * Removes the current co-routine from its ready list and places it in the
  * appropriate delayed list.
  */
-void vCoRoutineAddToDelayedList( portTickType xTicksToDelay, xList *pxEventList );
+void vCoRoutineAddToDelayedList( TickType_t xTicksToDelay, List_t *pxEventList );
 
 /*
  * This function is intended for internal use by the queue implementation only.
@@ -758,7 +753,7 @@ void vCoRoutineAddToDelayedList( portTickType xTicksToDelay, xList *pxEventList
  * Removes the highest priority co-routine from the event list and places it in
  * the pending ready list.
  */
-signed portBASE_TYPE xCoRoutineRemoveFromEventList( const xList *pxEventList );
+BaseType_t xCoRoutineRemoveFromEventList( const List_t *pxEventList );
 
 #ifdef __cplusplus
 }
diff --git a/lib/FreeRTOS/include/deprecated_definitions.h b/lib/FreeRTOS/include/deprecated_definitions.h
new file mode 100644
index 0000000000..0fad862693
--- /dev/null
+++ b/lib/FreeRTOS/include/deprecated_definitions.h
@@ -0,0 +1,321 @@
+/*
+    FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd.
+    All rights reserved
+
+    VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION.
+
+    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 modification 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.  Full license text is available on the following
+    link: http://www.freertos.org/a00114.html
+
+    ***************************************************************************
+     *                                                                       *
+     *    FreeRTOS provides completely free yet professionally developed,    *
+     *    robust, strictly quality controlled, supported, and cross          *
+     *    platform software that is more than just the market leader, it     *
+     *    is the industry's de facto standard.                               *
+     *                                                                       *
+     *    Help yourself get started quickly while simultaneously helping     *
+     *    to support the FreeRTOS project by purchasing a FreeRTOS           *
+     *    tutorial book, reference manual, or both:                          *
+     *    http://www.FreeRTOS.org/Documentation                              *
+     *                                                                       *
+    ***************************************************************************
+
+    http://www.FreeRTOS.org/FAQHelp.html - Having a problem?  Start by reading
+    the FAQ page "My application does not run, what could be wrong?".  Have you
+    defined configASSERT()?
+
+    http://www.FreeRTOS.org/support - In return for receiving this top quality
+    embedded software for free we request you assist our global community by
+    participating in the support forum.
+
+    http://www.FreeRTOS.org/training - Investing in training allows your team to
+    be as productive as possible as early as possible.  Now you can receive
+    FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers
+    Ltd, and the world's leading authority on the world's leading RTOS.
+
+    http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products,
+    including FreeRTOS+Trace - an indispensable productivity tool, a DOS
+    compatible FAT file system, and our tiny thread aware UDP/IP stack.
+
+    http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate.
+    Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS.
+
+    http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High
+    Integrity Systems ltd. to sell under the OpenRTOS brand.  Low cost OpenRTOS
+    licenses offer ticketed support, indemnification and commercial middleware.
+
+    http://www.SafeRTOS.com - High Integrity Systems also provide a safety
+    engineered and independently SIL3 certified version for use in safety and
+    mission critical applications that require provable dependability.
+
+    1 tab == 4 spaces!
+*/
+
+#ifndef DEPRECATED_DEFINITIONS_H
+#define DEPRECATED_DEFINITIONS_H
+
+
+/* Each FreeRTOS port has a unique portmacro.h header file.  Originally a
+pre-processor definition was used to ensure the pre-processor found the correct
+portmacro.h file for the port being used.  That scheme was deprecated in favour
+of setting the compiler's include path such that it found the correct
+portmacro.h file - removing the need for the constant and allowing the
+portmacro.h file to be located anywhere in relation to the port being used.  The
+definitions below remain in the code for backward compatibility only.  New
+projects should not use them. */
+
+#ifdef OPEN_WATCOM_INDUSTRIAL_PC_PORT
+	#include "..\..\Source\portable\owatcom\16bitdos\pc\portmacro.h"
+	typedef void ( __interrupt __far *pxISR )();
+#endif
+
+#ifdef OPEN_WATCOM_FLASH_LITE_186_PORT
+	#include "..\..\Source\portable\owatcom\16bitdos\flsh186\portmacro.h"
+	typedef void ( __interrupt __far *pxISR )();
+#endif
+
+#ifdef GCC_MEGA_AVR
+	#include "../portable/GCC/ATMega323/portmacro.h"
+#endif
+
+#ifdef IAR_MEGA_AVR
+	#include "../portable/IAR/ATMega323/portmacro.h"
+#endif
+
+#ifdef MPLAB_PIC24_PORT
+	#include "../../Source/portable/MPLAB/PIC24_dsPIC/portmacro.h"
+#endif
+
+#ifdef MPLAB_DSPIC_PORT
+	#include "../../Source/portable/MPLAB/PIC24_dsPIC/portmacro.h"
+#endif
+
+#ifdef MPLAB_PIC18F_PORT
+	#include "../../Source/portable/MPLAB/PIC18F/portmacro.h"
+#endif
+
+#ifdef MPLAB_PIC32MX_PORT
+	#include "../../Source/portable/MPLAB/PIC32MX/portmacro.h"
+#endif
+
+#ifdef _FEDPICC
+	#include "libFreeRTOS/Include/portmacro.h"
+#endif
+
+#ifdef SDCC_CYGNAL
+	#include "../../Source/portable/SDCC/Cygnal/portmacro.h"
+#endif
+
+#ifdef GCC_ARM7
+	#include "../../Source/portable/GCC/ARM7_LPC2000/portmacro.h"
+#endif
+
+#ifdef GCC_ARM7_ECLIPSE
+	#include "portmacro.h"
+#endif
+
+#ifdef ROWLEY_LPC23xx
+	#include "../../Source/portable/GCC/ARM7_LPC23xx/portmacro.h"
+#endif
+
+#ifdef IAR_MSP430
+	#include "..\..\Source\portable\IAR\MSP430\portmacro.h"
+#endif
+
+#ifdef GCC_MSP430
+	#include "../../Source/portable/GCC/MSP430F449/portmacro.h"
+#endif
+
+#ifdef ROWLEY_MSP430
+	#include "../../Source/portable/Rowley/MSP430F449/portmacro.h"
+#endif
+
+#ifdef ARM7_LPC21xx_KEIL_RVDS
+	#include "..\..\Source\portable\RVDS\ARM7_LPC21xx\portmacro.h"
+#endif
+
+#ifdef SAM7_GCC
+	#include "../../Source/portable/GCC/ARM7_AT91SAM7S/portmacro.h"
+#endif
+
+#ifdef SAM7_IAR
+	#include "..\..\Source\portable\IAR\AtmelSAM7S64\portmacro.h"
+#endif
+
+#ifdef SAM9XE_IAR
+	#include "..\..\Source\portable\IAR\AtmelSAM9XE\portmacro.h"
+#endif
+
+#ifdef LPC2000_IAR
+	#include "..\..\Source\portable\IAR\LPC2000\portmacro.h"
+#endif
+
+#ifdef STR71X_IAR
+	#include "..\..\Source\portable\IAR\STR71x\portmacro.h"
+#endif
+
+#ifdef STR75X_IAR
+	#include "..\..\Source\portable\IAR\STR75x\portmacro.h"
+#endif
+
+#ifdef STR75X_GCC
+	#include "..\..\Source\portable\GCC\STR75x\portmacro.h"
+#endif
+
+#ifdef STR91X_IAR
+	#include "..\..\Source\portable\IAR\STR91x\portmacro.h"
+#endif
+
+#ifdef GCC_H8S
+	#include "../../Source/portable/GCC/H8S2329/portmacro.h"
+#endif
+
+#ifdef GCC_AT91FR40008
+	#include "../../Source/portable/GCC/ARM7_AT91FR40008/portmacro.h"
+#endif
+
+#ifdef RVDS_ARMCM3_LM3S102
+	#include "../../Source/portable/RVDS/ARM_CM3/portmacro.h"
+#endif
+
+#ifdef GCC_ARMCM3_LM3S102
+	#include "../../Source/portable/GCC/ARM_CM3/portmacro.h"
+#endif
+
+#ifdef GCC_ARMCM3
+	#include "../../Source/portable/GCC/ARM_CM3/portmacro.h"
+#endif
+
+#ifdef IAR_ARM_CM3
+	#include "../../Source/portable/IAR/ARM_CM3/portmacro.h"
+#endif
+
+#ifdef IAR_ARMCM3_LM
+	#include "../../Source/portable/IAR/ARM_CM3/portmacro.h"
+#endif
+
+#ifdef HCS12_CODE_WARRIOR
+	#include "../../Source/portable/CodeWarrior/HCS12/portmacro.h"
+#endif
+
+#ifdef MICROBLAZE_GCC
+	#include "../../Source/portable/GCC/MicroBlaze/portmacro.h"
+#endif
+
+#ifdef TERN_EE
+	#include "..\..\Source\portable\Paradigm\Tern_EE\small\portmacro.h"
+#endif
+
+#ifdef GCC_HCS12
+	#include "../../Source/portable/GCC/HCS12/portmacro.h"
+#endif
+
+#ifdef GCC_MCF5235
+    #include "../../Source/portable/GCC/MCF5235/portmacro.h"
+#endif
+
+#ifdef COLDFIRE_V2_GCC
+	#include "../../../Source/portable/GCC/ColdFire_V2/portmacro.h"
+#endif
+
+#ifdef COLDFIRE_V2_CODEWARRIOR
+	#include "../../Source/portable/CodeWarrior/ColdFire_V2/portmacro.h"
+#endif
+
+#ifdef GCC_PPC405
+	#include "../../Source/portable/GCC/PPC405_Xilinx/portmacro.h"
+#endif
+
+#ifdef GCC_PPC440
+	#include "../../Source/portable/GCC/PPC440_Xilinx/portmacro.h"
+#endif
+
+#ifdef _16FX_SOFTUNE
+	#include "..\..\Source\portable\Softune\MB96340\portmacro.h"
+#endif
+
+#ifdef BCC_INDUSTRIAL_PC_PORT
+	/* A short file name has to be used in place of the normal
+	FreeRTOSConfig.h when using the Borland compiler. */
+	#include "frconfig.h"
+	#include "..\portable\BCC\16BitDOS\PC\prtmacro.h"
+    typedef void ( __interrupt __far *pxISR )();
+#endif
+
+#ifdef BCC_FLASH_LITE_186_PORT
+	/* A short file name has to be used in place of the normal
+	FreeRTOSConfig.h when using the Borland compiler. */
+	#include "frconfig.h"
+	#include "..\portable\BCC\16BitDOS\flsh186\prtmacro.h"
+    typedef void ( __interrupt __far *pxISR )();
+#endif
+
+#ifdef __GNUC__
+   #ifdef __AVR32_AVR32A__
+	   #include "portmacro.h"
+   #endif
+#endif
+
+#ifdef __ICCAVR32__
+   #ifdef __CORE__
+      #if __CORE__ == __AVR32A__
+	      #include "portmacro.h"
+      #endif
+   #endif
+#endif
+
+#ifdef __91467D
+	#include "portmacro.h"
+#endif
+
+#ifdef __96340
+	#include "portmacro.h"
+#endif
+
+
+#ifdef __IAR_V850ES_Fx3__
+	#include "../../Source/portable/IAR/V850ES/portmacro.h"
+#endif
+
+#ifdef __IAR_V850ES_Jx3__
+	#include "../../Source/portable/IAR/V850ES/portmacro.h"
+#endif
+
+#ifdef __IAR_V850ES_Jx3_L__
+	#include "../../Source/portable/IAR/V850ES/portmacro.h"
+#endif
+
+#ifdef __IAR_V850ES_Jx2__
+	#include "../../Source/portable/IAR/V850ES/portmacro.h"
+#endif
+
+#ifdef __IAR_V850ES_Hx2__
+	#include "../../Source/portable/IAR/V850ES/portmacro.h"
+#endif
+
+#ifdef __IAR_78K0R_Kx3__
+	#include "../../Source/portable/IAR/78K0R/portmacro.h"
+#endif
+
+#ifdef __IAR_78K0R_Kx3L__
+	#include "../../Source/portable/IAR/78K0R/portmacro.h"
+#endif
+
+#endif /* DEPRECATED_DEFINITIONS_H */
+
diff --git a/lib/FreeRTOS/include/event_groups.h b/lib/FreeRTOS/include/event_groups.h
new file mode 100644
index 0000000000..a381ed0726
--- /dev/null
+++ b/lib/FreeRTOS/include/event_groups.h
@@ -0,0 +1,730 @@
+/*
+    FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd.
+    All rights reserved
+
+    VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION.
+
+    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 modification 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.  Full license text is available on the following
+    link: http://www.freertos.org/a00114.html
+
+    ***************************************************************************
+     *                                                                       *
+     *    FreeRTOS provides completely free yet professionally developed,    *
+     *    robust, strictly quality controlled, supported, and cross          *
+     *    platform software that is more than just the market leader, it     *
+     *    is the industry's de facto standard.                               *
+     *                                                                       *
+     *    Help yourself get started quickly while simultaneously helping     *
+     *    to support the FreeRTOS project by purchasing a FreeRTOS           *
+     *    tutorial book, reference manual, or both:                          *
+     *    http://www.FreeRTOS.org/Documentation                              *
+     *                                                                       *
+    ***************************************************************************
+
+    http://www.FreeRTOS.org/FAQHelp.html - Having a problem?  Start by reading
+    the FAQ page "My application does not run, what could be wrong?".  Have you
+    defined configASSERT()?
+
+    http://www.FreeRTOS.org/support - In return for receiving this top quality
+    embedded software for free we request you assist our global community by
+    participating in the support forum.
+
+    http://www.FreeRTOS.org/training - Investing in training allows your team to
+    be as productive as possible as early as possible.  Now you can receive
+    FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers
+    Ltd, and the world's leading authority on the world's leading RTOS.
+
+    http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products,
+    including FreeRTOS+Trace - an indispensable productivity tool, a DOS
+    compatible FAT file system, and our tiny thread aware UDP/IP stack.
+
+    http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate.
+    Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS.
+
+    http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High
+    Integrity Systems ltd. to sell under the OpenRTOS brand.  Low cost OpenRTOS
+    licenses offer ticketed support, indemnification and commercial middleware.
+
+    http://www.SafeRTOS.com - High Integrity Systems also provide a safety
+    engineered and independently SIL3 certified version for use in safety and
+    mission critical applications that require provable dependability.
+
+    1 tab == 4 spaces!
+*/
+
+#ifndef EVENT_GROUPS_H
+#define EVENT_GROUPS_H
+
+#ifndef INC_FREERTOS_H
+	#error "include FreeRTOS.h" must appear in source files before "include event_groups.h"
+#endif
+
+#include "timers.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * An event group is a collection of bits to which an application can assign a
+ * meaning.  For example, an application may create an event group to convey
+ * the status of various CAN bus related events in which bit 0 might mean "A CAN
+ * message has been received and is ready for processing", bit 1 might mean "The
+ * application has queued a message that is ready for sending onto the CAN
+ * network", and bit 2 might mean "It is time to send a SYNC message onto the
+ * CAN network" etc.  A task can then test the bit values to see which events
+ * are active, and optionally enter the Blocked state to wait for a specified
+ * bit or a group of specified bits to be active.  To continue the CAN bus
+ * example, a CAN controlling task can enter the Blocked state (and therefore
+ * not consume any processing time) until either bit 0, bit 1 or bit 2 are
+ * active, at which time the bit that was actually active would inform the task
+ * which action it had to take (process a received message, send a message, or
+ * send a SYNC).
+ *
+ * The event groups implementation contains intelligence to avoid race
+ * conditions that would otherwise occur were an application to use a simple
+ * variable for the same purpose.  This is particularly important with respect
+ * to when a bit within an event group is to be cleared, and when bits have to
+ * be set and then tested atomically - as is the case where event groups are
+ * used to create a synchronisation point between multiple tasks (a
+ * 'rendezvous').
+ *
+ * \defgroup EventGroup
+ */
+
+
+
+/**
+ * event_groups.h
+ *
+ * Type by which event groups are referenced.  For example, a call to
+ * xEventGroupCreate() returns an EventGroupHandle_t variable that can then
+ * be used as a parameter to other event group functions.
+ *
+ * \defgroup EventGroupHandle_t EventGroupHandle_t
+ * \ingroup EventGroup
+ */
+typedef void * EventGroupHandle_t;
+
+/* 
+ * The type that holds event bits always matches TickType_t - therefore the
+ * number of bits it holds is set by configUSE_16_BIT_TICKS (16 bits if set to 1,
+ * 32 bits if set to 0. 
+ *
+ * \defgroup EventBits_t EventBits_t
+ * \ingroup EventGroup
+ */
+typedef TickType_t EventBits_t;
+
+/**
+ * event_groups.h
+ *
+ EventGroupHandle_t xEventGroupCreate( void );
+ 
+ * + * Create a new event group. This function cannot be called from an interrupt. + * + * Although event groups are not related to ticks, for internal implementation + * reasons the number of bits available for use in an event group is dependent + * on the configUSE_16_BIT_TICKS setting in FreeRTOSConfig.h. If + * configUSE_16_BIT_TICKS is 1 then each event group contains 8 usable bits (bit + * 0 to bit 7). If configUSE_16_BIT_TICKS is set to 0 then each event group has + * 24 usable bits (bit 0 to bit 23). The EventBits_t type is used to store + * event bits within an event group. + * + * @return If the event group was created then a handle to the event group is + * returned. If there was insufficient FreeRTOS heap available to create the + * event group then NULL is returned. See http://www.freertos.org/a00111.html + * + * Example usage: +
+	// Declare a variable to hold the created event group.
+	EventGroupHandle_t xCreatedEventGroup;
+
+	// Attempt to create the event group.
+	xCreatedEventGroup = xEventGroupCreate();
+
+	// Was the event group created successfully?
+	if( xCreatedEventGroup == NULL )
+	{
+		// The event group was not created because there was insufficient
+		// FreeRTOS heap available.
+	}
+	else
+	{
+		// The event group was created.
+	}
+   
+ * \defgroup xEventGroupCreate xEventGroupCreate + * \ingroup EventGroup + */ +EventGroupHandle_t xEventGroupCreate( void ) PRIVILEGED_FUNCTION; + +/** + * event_groups.h + *
+	EventBits_t xEventGroupWaitBits( 	EventGroupHandle_t xEventGroup,
+										const EventBits_t uxBitsToWaitFor,
+										const BaseType_t xClearOnExit,
+										const BaseType_t xWaitForAllBits,
+										const TickType_t xTicksToWait );
+ 
+ * + * [Potentially] block to wait for one or more bits to be set within a + * previously created event group. + * + * This function cannot be called from an interrupt. + * + * @param xEventGroup The event group in which the bits are being tested. The + * event group must have previously been created using a call to + * xEventGroupCreate(). + * + * @param uxBitsToWaitFor A bitwise value that indicates the bit or bits to test + * inside the event group. For example, to wait for bit 0 and/or bit 2 set + * uxBitsToWaitFor to 0x05. To wait for bits 0 and/or bit 1 and/or bit 2 set + * uxBitsToWaitFor to 0x07. Etc. + * + * @param xClearOnExit If xClearOnExit is set to pdTRUE then any bits within + * uxBitsToWaitFor that are set within the event group will be cleared before + * xEventGroupWaitBits() returns if the wait condition was met (if the function + * returns for a reason other than a timeout). If xClearOnExit is set to + * pdFALSE then the bits set in the event group are not altered when the call to + * xEventGroupWaitBits() returns. + * + * @param xWaitForAllBits If xWaitForAllBits is set to pdTRUE then + * xEventGroupWaitBits() will return when either all the bits in uxBitsToWaitFor + * are set or the specified block time expires. If xWaitForAllBits is set to + * pdFALSE then xEventGroupWaitBits() will return when any one of the bits set + * in uxBitsToWaitFor is set or the specified block time expires. The block + * time is specified by the xTicksToWait parameter. + * + * @param xTicksToWait The maximum amount of time (specified in 'ticks') to wait + * for one/all (depending on the xWaitForAllBits value) of the bits specified by + * uxBitsToWaitFor to become set. + * + * @return The value of the event group at the time either the bits being waited + * for became set, or the block time expired. Test the return value to know + * which bits were set. If xEventGroupWaitBits() returned because its timeout + * expired then not all the bits being waited for will be set. If + * xEventGroupWaitBits() returned because the bits it was waiting for were set + * then the returned value is the event group value before any bits were + * automatically cleared in the case that xClearOnExit parameter was set to + * pdTRUE. + * + * Example usage: +
+   #define BIT_0	( 1 << 0 )
+   #define BIT_4	( 1 << 4 )
+
+   void aFunction( EventGroupHandle_t xEventGroup )
+   {
+   EventBits_t uxBits;
+   const TickType_t xTicksToWait = 100 / portTICK_PERIOD_MS;
+
+		// Wait a maximum of 100ms for either bit 0 or bit 4 to be set within
+		// the event group.  Clear the bits before exiting.
+		uxBits = xEventGroupWaitBits(
+					xEventGroup,	// The event group being tested.
+					BIT_0 | BIT_4,	// The bits within the event group to wait for.
+					pdTRUE,			// BIT_0 and BIT_4 should be cleared before returning.
+					pdFALSE,		// Don't wait for both bits, either bit will do.
+					xTicksToWait );	// Wait a maximum of 100ms for either bit to be set.
+
+		if( ( uxBits & ( BIT_0 | BIT_4 ) ) == ( BIT_0 | BIT_4 ) )
+		{
+			// xEventGroupWaitBits() returned because both bits were set.
+		}
+		else if( ( uxBits & BIT_0 ) != 0 )
+		{
+			// xEventGroupWaitBits() returned because just BIT_0 was set.
+		}
+		else if( ( uxBits & BIT_4 ) != 0 )
+		{
+			// xEventGroupWaitBits() returned because just BIT_4 was set.
+		}
+		else
+		{
+			// xEventGroupWaitBits() returned because xTicksToWait ticks passed
+			// without either BIT_0 or BIT_4 becoming set.
+		}
+   }
+   
+ * \defgroup xEventGroupWaitBits xEventGroupWaitBits + * \ingroup EventGroup + */ +EventBits_t xEventGroupWaitBits( EventGroupHandle_t xEventGroup, const EventBits_t uxBitsToWaitFor, const BaseType_t xClearOnExit, const BaseType_t xWaitForAllBits, TickType_t xTicksToWait ) PRIVILEGED_FUNCTION; + +/** + * event_groups.h + *
+	EventBits_t xEventGroupClearBits( EventGroupHandle_t xEventGroup, const EventBits_t uxBitsToClear );
+ 
+ * + * Clear bits within an event group. This function cannot be called from an + * interrupt. + * + * @param xEventGroup The event group in which the bits are to be cleared. + * + * @param uxBitsToClear A bitwise value that indicates the bit or bits to clear + * in the event group. For example, to clear bit 3 only, set uxBitsToClear to + * 0x08. To clear bit 3 and bit 0 set uxBitsToClear to 0x09. + * + * @return The value of the event group before the specified bits were cleared. + * + * Example usage: +
+   #define BIT_0	( 1 << 0 )
+   #define BIT_4	( 1 << 4 )
+
+   void aFunction( EventGroupHandle_t xEventGroup )
+   {
+   EventBits_t uxBits;
+
+		// Clear bit 0 and bit 4 in xEventGroup.
+		uxBits = xEventGroupClearBits(
+								xEventGroup,	// The event group being updated.
+								BIT_0 | BIT_4 );// The bits being cleared.
+
+		if( ( uxBits & ( BIT_0 | BIT_4 ) ) == ( BIT_0 | BIT_4 ) )
+		{
+			// Both bit 0 and bit 4 were set before xEventGroupClearBits() was
+			// called.  Both will now be clear (not set).
+		}
+		else if( ( uxBits & BIT_0 ) != 0 )
+		{
+			// Bit 0 was set before xEventGroupClearBits() was called.  It will
+			// now be clear.
+		}
+		else if( ( uxBits & BIT_4 ) != 0 )
+		{
+			// Bit 4 was set before xEventGroupClearBits() was called.  It will
+			// now be clear.
+		}
+		else
+		{
+			// Neither bit 0 nor bit 4 were set in the first place.
+		}
+   }
+   
+ * \defgroup xEventGroupClearBits xEventGroupClearBits + * \ingroup EventGroup + */ +EventBits_t xEventGroupClearBits( EventGroupHandle_t xEventGroup, const EventBits_t uxBitsToClear ) PRIVILEGED_FUNCTION; + +/** + * event_groups.h + *
+	BaseType_t xEventGroupClearBitsFromISR( EventGroupHandle_t xEventGroup, const EventBits_t uxBitsToSet );
+ 
+ * + * A version of xEventGroupClearBits() that can be called from an interrupt. + * + * Setting bits in an event group is not a deterministic operation because there + * are an unknown number of tasks that may be waiting for the bit or bits being + * set. FreeRTOS does not allow nondeterministic operations to be performed + * while interrupts are disabled, so protects event groups that are accessed + * from tasks by suspending the scheduler rather than disabling interrupts. As + * a result event groups cannot be accessed directly from an interrupt service + * routine. Therefore xEventGroupClearBitsFromISR() sends a message to the + * timer task to have the clear operation performed in the context of the timer + * task. + * + * @param xEventGroup The event group in which the bits are to be cleared. + * + * @param uxBitsToClear A bitwise value that indicates the bit or bits to clear. + * For example, to clear bit 3 only, set uxBitsToClear to 0x08. To clear bit 3 + * and bit 0 set uxBitsToClear to 0x09. + * + * @return If the request to execute the function was posted successfully then + * pdPASS is returned, otherwise pdFALSE is returned. pdFALSE will be returned + * if the timer service queue was full. + * + * Example usage: +
+   #define BIT_0	( 1 << 0 )
+   #define BIT_4	( 1 << 4 )
+
+   // An event group which it is assumed has already been created by a call to
+   // xEventGroupCreate().
+   EventGroupHandle_t xEventGroup;
+
+   void anInterruptHandler( void )
+   {
+		// Clear bit 0 and bit 4 in xEventGroup.
+		xResult = xEventGroupClearBitsFromISR(
+							xEventGroup,	 // The event group being updated.
+							BIT_0 | BIT_4 ); // The bits being set.
+
+		if( xResult == pdPASS )
+		{
+			// The message was posted successfully.
+		}
+  }
+   
+ * \defgroup xEventGroupSetBitsFromISR xEventGroupSetBitsFromISR + * \ingroup EventGroup + */ +#if( configUSE_TRACE_FACILITY == 1 ) + BaseType_t xEventGroupClearBitsFromISR( EventGroupHandle_t xEventGroup, const EventBits_t uxBitsToSet ) PRIVILEGED_FUNCTION; +#else + #define xEventGroupClearBitsFromISR( xEventGroup, uxBitsToClear ) xTimerPendFunctionCallFromISR( vEventGroupClearBitsCallback, ( void * ) xEventGroup, ( uint32_t ) uxBitsToClear, NULL ) +#endif + +/** + * event_groups.h + *
+	EventBits_t xEventGroupSetBits( EventGroupHandle_t xEventGroup, const EventBits_t uxBitsToSet );
+ 
+ * + * Set bits within an event group. + * This function cannot be called from an interrupt. xEventGroupSetBitsFromISR() + * is a version that can be called from an interrupt. + * + * Setting bits in an event group will automatically unblock tasks that are + * blocked waiting for the bits. + * + * @param xEventGroup The event group in which the bits are to be set. + * + * @param uxBitsToSet A bitwise value that indicates the bit or bits to set. + * For example, to set bit 3 only, set uxBitsToSet to 0x08. To set bit 3 + * and bit 0 set uxBitsToSet to 0x09. + * + * @return The value of the event group at the time the call to + * xEventGroupSetBits() returns. There are two reasons why the returned value + * might have the bits specified by the uxBitsToSet parameter cleared. First, + * if setting a bit results in a task that was waiting for the bit leaving the + * blocked state then it is possible the bit will be cleared automatically + * (see the xClearBitOnExit parameter of xEventGroupWaitBits()). Second, any + * unblocked (or otherwise Ready state) task that has a priority above that of + * the task that called xEventGroupSetBits() will execute and may change the + * event group value before the call to xEventGroupSetBits() returns. + * + * Example usage: +
+   #define BIT_0	( 1 << 0 )
+   #define BIT_4	( 1 << 4 )
+
+   void aFunction( EventGroupHandle_t xEventGroup )
+   {
+   EventBits_t uxBits;
+
+		// Set bit 0 and bit 4 in xEventGroup.
+		uxBits = xEventGroupSetBits(
+							xEventGroup,	// The event group being updated.
+							BIT_0 | BIT_4 );// The bits being set.
+
+		if( ( uxBits & ( BIT_0 | BIT_4 ) ) == ( BIT_0 | BIT_4 ) )
+		{
+			// Both bit 0 and bit 4 remained set when the function returned.
+		}
+		else if( ( uxBits & BIT_0 ) != 0 )
+		{
+			// Bit 0 remained set when the function returned, but bit 4 was
+			// cleared.  It might be that bit 4 was cleared automatically as a
+			// task that was waiting for bit 4 was removed from the Blocked
+			// state.
+		}
+		else if( ( uxBits & BIT_4 ) != 0 )
+		{
+			// Bit 4 remained set when the function returned, but bit 0 was
+			// cleared.  It might be that bit 0 was cleared automatically as a
+			// task that was waiting for bit 0 was removed from the Blocked
+			// state.
+		}
+		else
+		{
+			// Neither bit 0 nor bit 4 remained set.  It might be that a task
+			// was waiting for both of the bits to be set, and the bits were
+			// cleared as the task left the Blocked state.
+		}
+   }
+   
+ * \defgroup xEventGroupSetBits xEventGroupSetBits + * \ingroup EventGroup + */ +EventBits_t xEventGroupSetBits( EventGroupHandle_t xEventGroup, const EventBits_t uxBitsToSet ) PRIVILEGED_FUNCTION; + +/** + * event_groups.h + *
+	BaseType_t xEventGroupSetBitsFromISR( EventGroupHandle_t xEventGroup, const EventBits_t uxBitsToSet, BaseType_t *pxHigherPriorityTaskWoken );
+ 
+ * + * A version of xEventGroupSetBits() that can be called from an interrupt. + * + * Setting bits in an event group is not a deterministic operation because there + * are an unknown number of tasks that may be waiting for the bit or bits being + * set. FreeRTOS does not allow nondeterministic operations to be performed in + * interrupts or from critical sections. Therefore xEventGroupSetBitFromISR() + * sends a message to the timer task to have the set operation performed in the + * context of the timer task - where a scheduler lock is used in place of a + * critical section. + * + * @param xEventGroup The event group in which the bits are to be set. + * + * @param uxBitsToSet A bitwise value that indicates the bit or bits to set. + * For example, to set bit 3 only, set uxBitsToSet to 0x08. To set bit 3 + * and bit 0 set uxBitsToSet to 0x09. + * + * @param pxHigherPriorityTaskWoken As mentioned above, calling this function + * will result in a message being sent to the timer daemon task. If the + * priority of the timer daemon task is higher than the priority of the + * currently running task (the task the interrupt interrupted) then + * *pxHigherPriorityTaskWoken will be set to pdTRUE by + * xEventGroupSetBitsFromISR(), indicating that a context switch should be + * requested before the interrupt exits. For that reason + * *pxHigherPriorityTaskWoken must be initialised to pdFALSE. See the + * example code below. + * + * @return If the request to execute the function was posted successfully then + * pdPASS is returned, otherwise pdFALSE is returned. pdFALSE will be returned + * if the timer service queue was full. + * + * Example usage: +
+   #define BIT_0	( 1 << 0 )
+   #define BIT_4	( 1 << 4 )
+
+   // An event group which it is assumed has already been created by a call to
+   // xEventGroupCreate().
+   EventGroupHandle_t xEventGroup;
+
+   void anInterruptHandler( void )
+   {
+   BaseType_t xHigherPriorityTaskWoken, xResult;
+
+		// xHigherPriorityTaskWoken must be initialised to pdFALSE.
+		xHigherPriorityTaskWoken = pdFALSE;
+
+		// Set bit 0 and bit 4 in xEventGroup.
+		xResult = xEventGroupSetBitsFromISR(
+							xEventGroup,	// The event group being updated.
+							BIT_0 | BIT_4   // The bits being set.
+							&xHigherPriorityTaskWoken );
+
+		// Was the message posted successfully?
+		if( xResult == pdPASS )
+		{
+			// If xHigherPriorityTaskWoken is now set to pdTRUE then a context
+			// switch should be requested.  The macro used is port specific and 
+			// will be either portYIELD_FROM_ISR() or portEND_SWITCHING_ISR() - 
+			// refer to the documentation page for the port being used.
+			portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
+		}
+  }
+   
+ * \defgroup xEventGroupSetBitsFromISR xEventGroupSetBitsFromISR + * \ingroup EventGroup + */ +#if( configUSE_TRACE_FACILITY == 1 ) + BaseType_t xEventGroupSetBitsFromISR( EventGroupHandle_t xEventGroup, const EventBits_t uxBitsToSet, BaseType_t *pxHigherPriorityTaskWoken ) PRIVILEGED_FUNCTION; +#else + #define xEventGroupSetBitsFromISR( xEventGroup, uxBitsToSet, pxHigherPriorityTaskWoken ) xTimerPendFunctionCallFromISR( vEventGroupSetBitsCallback, ( void * ) xEventGroup, ( uint32_t ) uxBitsToSet, pxHigherPriorityTaskWoken ) +#endif + +/** + * event_groups.h + *
+	EventBits_t xEventGroupSync(	EventGroupHandle_t xEventGroup,
+									const EventBits_t uxBitsToSet,
+									const EventBits_t uxBitsToWaitFor,
+									TickType_t xTicksToWait );
+ 
+ * + * Atomically set bits within an event group, then wait for a combination of + * bits to be set within the same event group. This functionality is typically + * used to synchronise multiple tasks, where each task has to wait for the other + * tasks to reach a synchronisation point before proceeding. + * + * This function cannot be used from an interrupt. + * + * The function will return before its block time expires if the bits specified + * by the uxBitsToWait parameter are set, or become set within that time. In + * this case all the bits specified by uxBitsToWait will be automatically + * cleared before the function returns. + * + * @param xEventGroup The event group in which the bits are being tested. The + * event group must have previously been created using a call to + * xEventGroupCreate(). + * + * @param uxBitsToSet The bits to set in the event group before determining + * if, and possibly waiting for, all the bits specified by the uxBitsToWait + * parameter are set. + * + * @param uxBitsToWaitFor A bitwise value that indicates the bit or bits to test + * inside the event group. For example, to wait for bit 0 and bit 2 set + * uxBitsToWaitFor to 0x05. To wait for bits 0 and bit 1 and bit 2 set + * uxBitsToWaitFor to 0x07. Etc. + * + * @param xTicksToWait The maximum amount of time (specified in 'ticks') to wait + * for all of the bits specified by uxBitsToWaitFor to become set. + * + * @return The value of the event group at the time either the bits being waited + * for became set, or the block time expired. Test the return value to know + * which bits were set. If xEventGroupSync() returned because its timeout + * expired then not all the bits being waited for will be set. If + * xEventGroupSync() returned because all the bits it was waiting for were + * set then the returned value is the event group value before any bits were + * automatically cleared. + * + * Example usage: +
+ // Bits used by the three tasks.
+ #define TASK_0_BIT		( 1 << 0 )
+ #define TASK_1_BIT		( 1 << 1 )
+ #define TASK_2_BIT		( 1 << 2 )
+
+ #define ALL_SYNC_BITS ( TASK_0_BIT | TASK_1_BIT | TASK_2_BIT )
+
+ // Use an event group to synchronise three tasks.  It is assumed this event
+ // group has already been created elsewhere.
+ EventGroupHandle_t xEventBits;
+
+ void vTask0( void *pvParameters )
+ {
+ EventBits_t uxReturn;
+ TickType_t xTicksToWait = 100 / portTICK_PERIOD_MS;
+
+	 for( ;; )
+	 {
+		// Perform task functionality here.
+
+		// Set bit 0 in the event flag to note this task has reached the
+		// sync point.  The other two tasks will set the other two bits defined
+		// by ALL_SYNC_BITS.  All three tasks have reached the synchronisation
+		// point when all the ALL_SYNC_BITS are set.  Wait a maximum of 100ms
+		// for this to happen.
+		uxReturn = xEventGroupSync( xEventBits, TASK_0_BIT, ALL_SYNC_BITS, xTicksToWait );
+
+		if( ( uxReturn & ALL_SYNC_BITS ) == ALL_SYNC_BITS )
+		{
+			// All three tasks reached the synchronisation point before the call
+			// to xEventGroupSync() timed out.
+		}
+	}
+ }
+
+ void vTask1( void *pvParameters )
+ {
+	 for( ;; )
+	 {
+		// Perform task functionality here.
+
+		// Set bit 1 in the event flag to note this task has reached the
+		// synchronisation point.  The other two tasks will set the other two
+		// bits defined by ALL_SYNC_BITS.  All three tasks have reached the
+		// synchronisation point when all the ALL_SYNC_BITS are set.  Wait
+		// indefinitely for this to happen.
+		xEventGroupSync( xEventBits, TASK_1_BIT, ALL_SYNC_BITS, portMAX_DELAY );
+
+		// xEventGroupSync() was called with an indefinite block time, so
+		// this task will only reach here if the syncrhonisation was made by all
+		// three tasks, so there is no need to test the return value.
+	 }
+ }
+
+ void vTask2( void *pvParameters )
+ {
+	 for( ;; )
+	 {
+		// Perform task functionality here.
+
+		// Set bit 2 in the event flag to note this task has reached the
+		// synchronisation point.  The other two tasks will set the other two
+		// bits defined by ALL_SYNC_BITS.  All three tasks have reached the
+		// synchronisation point when all the ALL_SYNC_BITS are set.  Wait
+		// indefinitely for this to happen.
+		xEventGroupSync( xEventBits, TASK_2_BIT, ALL_SYNC_BITS, portMAX_DELAY );
+
+		// xEventGroupSync() was called with an indefinite block time, so
+		// this task will only reach here if the syncrhonisation was made by all
+		// three tasks, so there is no need to test the return value.
+	}
+ }
+
+ 
+ * \defgroup xEventGroupSync xEventGroupSync + * \ingroup EventGroup + */ +EventBits_t xEventGroupSync( EventGroupHandle_t xEventGroup, const EventBits_t uxBitsToSet, const EventBits_t uxBitsToWaitFor, TickType_t xTicksToWait ) PRIVILEGED_FUNCTION; + + +/** + * event_groups.h + *
+	EventBits_t xEventGroupGetBits( EventGroupHandle_t xEventGroup );
+ 
+ * + * Returns the current value of the bits in an event group. This function + * cannot be used from an interrupt. + * + * @param xEventGroup The event group being queried. + * + * @return The event group bits at the time xEventGroupGetBits() was called. + * + * \defgroup xEventGroupGetBits xEventGroupGetBits + * \ingroup EventGroup + */ +#define xEventGroupGetBits( xEventGroup ) xEventGroupClearBits( xEventGroup, 0 ) + +/** + * event_groups.h + *
+	EventBits_t xEventGroupGetBitsFromISR( EventGroupHandle_t xEventGroup );
+ 
+ * + * A version of xEventGroupGetBits() that can be called from an ISR. + * + * @param xEventGroup The event group being queried. + * + * @return The event group bits at the time xEventGroupGetBitsFromISR() was called. + * + * \defgroup xEventGroupGetBitsFromISR xEventGroupGetBitsFromISR + * \ingroup EventGroup + */ +EventBits_t xEventGroupGetBitsFromISR( EventGroupHandle_t xEventGroup ) PRIVILEGED_FUNCTION; + +/** + * event_groups.h + *
+	void xEventGroupDelete( EventGroupHandle_t xEventGroup );
+ 
+ * + * Delete an event group that was previously created by a call to + * xEventGroupCreate(). Tasks that are blocked on the event group will be + * unblocked and obtain 0 as the event group's value. + * + * @param xEventGroup The event group being deleted. + */ +void vEventGroupDelete( EventGroupHandle_t xEventGroup ) PRIVILEGED_FUNCTION; + +/* For internal use only. */ +void vEventGroupSetBitsCallback( void *pvEventGroup, const uint32_t ulBitsToSet ) PRIVILEGED_FUNCTION; +void vEventGroupClearBitsCallback( void *pvEventGroup, const uint32_t ulBitsToClear ) PRIVILEGED_FUNCTION; + +#if (configUSE_TRACE_FACILITY == 1) + UBaseType_t uxEventGroupGetNumber( void* xEventGroup ) PRIVILEGED_FUNCTION; +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* EVENT_GROUPS_H */ + + diff --git a/lib/FreeRTOS/include/list.h b/lib/FreeRTOS/include/list.h index a08a7710cd..75d391d4ce 100644 --- a/lib/FreeRTOS/include/list.h +++ b/lib/FreeRTOS/include/list.h @@ -1,75 +1,70 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ /* @@ -77,7 +72,7 @@ * heavily for the schedulers needs, it is also available for use by * application code. * - * xLists can only store pointers to xListItems. Each xListItem contains a + * list_ts can only store pointers to list_item_ts. Each ListItem_t contains a * numeric value (xItemValue). Most of the time the lists are sorted in * descending item value order. * @@ -100,43 +95,121 @@ * \ingroup FreeRTOSIntro */ +#ifndef INC_FREERTOS_H + #error FreeRTOS.h must be included before list.h +#endif #ifndef LIST_H #define LIST_H +/* + * The list structure members are modified from within interrupts, and therefore + * by rights should be declared volatile. However, they are only modified in a + * functionally atomic way (within critical sections of with the scheduler + * suspended) and are either passed by reference into a function or indexed via + * a volatile variable. Therefore, in all use cases tested so far, the volatile + * qualifier can be omitted in order to provide a moderate performance + * improvement without adversely affecting functional behaviour. The assembly + * instructions generated by the IAR, ARM and GCC compilers when the respective + * compiler's options were set for maximum optimisation has been inspected and + * deemed to be as intended. That said, as compiler technology advances, and + * especially if aggressive cross module optimisation is used (a use case that + * has not been exercised to any great extend) then it is feasible that the + * volatile qualifier will be needed for correct optimisation. It is expected + * that a compiler removing essential code because, without the volatile + * qualifier on the list structure members and with aggressive cross module + * optimisation, the compiler deemed the code unnecessary will result in + * complete and obvious failure of the scheduler. If this is ever experienced + * then the volatile qualifier can be inserted in the relevant places within the + * list structures by simply defining configLIST_VOLATILE to volatile in + * FreeRTOSConfig.h (as per the example at the bottom of this comment block). + * If configLIST_VOLATILE is not defined then the preprocessor directives below + * will simply #define configLIST_VOLATILE away completely. + * + * To use volatile list structure members then add the following line to + * FreeRTOSConfig.h (without the quotes): + * "#define configLIST_VOLATILE volatile" + */ +#ifndef configLIST_VOLATILE + #define configLIST_VOLATILE +#endif /* configSUPPORT_CROSS_MODULE_OPTIMISATION */ + #ifdef __cplusplus extern "C" { #endif + +/* Macros that can be used to place known values within the list structures, +then check that the known values do not get corrupted during the execution of +the application. These may catch the list data structures being overwritten in +memory. They will not catch data errors caused by incorrect configuration or +use of FreeRTOS.*/ +#if( configUSE_LIST_DATA_INTEGRITY_CHECK_BYTES == 0 ) + /* Define the macros to do nothing. */ + #define listFIRST_LIST_ITEM_INTEGRITY_CHECK_VALUE + #define listSECOND_LIST_ITEM_INTEGRITY_CHECK_VALUE + #define listFIRST_LIST_INTEGRITY_CHECK_VALUE + #define listSECOND_LIST_INTEGRITY_CHECK_VALUE + #define listSET_FIRST_LIST_ITEM_INTEGRITY_CHECK_VALUE( pxItem ) + #define listSET_SECOND_LIST_ITEM_INTEGRITY_CHECK_VALUE( pxItem ) + #define listSET_LIST_INTEGRITY_CHECK_1_VALUE( pxList ) + #define listSET_LIST_INTEGRITY_CHECK_2_VALUE( pxList ) + #define listTEST_LIST_ITEM_INTEGRITY( pxItem ) + #define listTEST_LIST_INTEGRITY( pxList ) +#else + /* Define macros that add new members into the list structures. */ + #define listFIRST_LIST_ITEM_INTEGRITY_CHECK_VALUE TickType_t xListItemIntegrityValue1; + #define listSECOND_LIST_ITEM_INTEGRITY_CHECK_VALUE TickType_t xListItemIntegrityValue2; + #define listFIRST_LIST_INTEGRITY_CHECK_VALUE TickType_t xListIntegrityValue1; + #define listSECOND_LIST_INTEGRITY_CHECK_VALUE TickType_t xListIntegrityValue2; + + /* Define macros that set the new structure members to known values. */ + #define listSET_FIRST_LIST_ITEM_INTEGRITY_CHECK_VALUE( pxItem ) ( pxItem )->xListItemIntegrityValue1 = pdINTEGRITY_CHECK_VALUE + #define listSET_SECOND_LIST_ITEM_INTEGRITY_CHECK_VALUE( pxItem ) ( pxItem )->xListItemIntegrityValue2 = pdINTEGRITY_CHECK_VALUE + #define listSET_LIST_INTEGRITY_CHECK_1_VALUE( pxList ) ( pxList )->xListIntegrityValue1 = pdINTEGRITY_CHECK_VALUE + #define listSET_LIST_INTEGRITY_CHECK_2_VALUE( pxList ) ( pxList )->xListIntegrityValue2 = pdINTEGRITY_CHECK_VALUE + + /* Define macros that will assert if one of the structure members does not + contain its expected value. */ + #define listTEST_LIST_ITEM_INTEGRITY( pxItem ) configASSERT( ( ( pxItem )->xListItemIntegrityValue1 == pdINTEGRITY_CHECK_VALUE ) && ( ( pxItem )->xListItemIntegrityValue2 == pdINTEGRITY_CHECK_VALUE ) ) + #define listTEST_LIST_INTEGRITY( pxList ) configASSERT( ( ( pxList )->xListIntegrityValue1 == pdINTEGRITY_CHECK_VALUE ) && ( ( pxList )->xListIntegrityValue2 == pdINTEGRITY_CHECK_VALUE ) ) +#endif /* configUSE_LIST_DATA_INTEGRITY_CHECK_BYTES */ + + /* * Definition of the only type of object that a list can contain. */ struct xLIST_ITEM { - portTickType xItemValue; /*< The value being listed. In most cases this is used to sort the list in descending order. */ - volatile struct xLIST_ITEM * pxNext; /*< Pointer to the next xListItem in the list. */ - volatile struct xLIST_ITEM * pxPrevious;/*< Pointer to the previous xListItem in the list. */ - void * pvOwner; /*< Pointer to the object (normally a TCB) that contains the list item. There is therefore a two way link between the object containing the list item and the list item itself. */ - void * pvContainer; /*< Pointer to the list in which this list item is placed (if any). */ + listFIRST_LIST_ITEM_INTEGRITY_CHECK_VALUE /*< Set to a known value if configUSE_LIST_DATA_INTEGRITY_CHECK_BYTES is set to 1. */ + configLIST_VOLATILE TickType_t xItemValue; /*< The value being listed. In most cases this is used to sort the list in descending order. */ + struct xLIST_ITEM * configLIST_VOLATILE pxNext; /*< Pointer to the next ListItem_t in the list. */ + struct xLIST_ITEM * configLIST_VOLATILE pxPrevious; /*< Pointer to the previous ListItem_t in the list. */ + void * pvOwner; /*< Pointer to the object (normally a TCB) that contains the list item. There is therefore a two way link between the object containing the list item and the list item itself. */ + void * configLIST_VOLATILE pvContainer; /*< Pointer to the list in which this list item is placed (if any). */ + listSECOND_LIST_ITEM_INTEGRITY_CHECK_VALUE /*< Set to a known value if configUSE_LIST_DATA_INTEGRITY_CHECK_BYTES is set to 1. */ }; -typedef struct xLIST_ITEM xListItem; /* For some reason lint wants this as two separate definitions. */ +typedef struct xLIST_ITEM ListItem_t; /* For some reason lint wants this as two separate definitions. */ struct xMINI_LIST_ITEM { - portTickType xItemValue; - volatile struct xLIST_ITEM *pxNext; - volatile struct xLIST_ITEM *pxPrevious; + listFIRST_LIST_ITEM_INTEGRITY_CHECK_VALUE /*< Set to a known value if configUSE_LIST_DATA_INTEGRITY_CHECK_BYTES is set to 1. */ + configLIST_VOLATILE TickType_t xItemValue; + struct xLIST_ITEM * configLIST_VOLATILE pxNext; + struct xLIST_ITEM * configLIST_VOLATILE pxPrevious; }; -typedef struct xMINI_LIST_ITEM xMiniListItem; +typedef struct xMINI_LIST_ITEM MiniListItem_t; /* * Definition of the type of queue used by the scheduler. */ typedef struct xLIST { - volatile unsigned portBASE_TYPE uxNumberOfItems; - volatile xListItem * pxIndex; /*< Used to walk through the list. Points to the last item returned by a call to pvListGetOwnerOfNextEntry (). */ - volatile xMiniListItem xListEnd; /*< List item that contains the maximum possible item value meaning it is always at the end of the list and is therefore used as a marker. */ -} xList; + listFIRST_LIST_INTEGRITY_CHECK_VALUE /*< Set to a known value if configUSE_LIST_DATA_INTEGRITY_CHECK_BYTES is set to 1. */ + configLIST_VOLATILE UBaseType_t uxNumberOfItems; + ListItem_t * configLIST_VOLATILE pxIndex; /*< Used to walk through the list. Points to the last item returned by a call to listGET_OWNER_OF_NEXT_ENTRY (). */ + MiniListItem_t xListEnd; /*< List item that contains the maximum possible item value meaning it is always at the end of the list and is therefore used as a marker. */ + listSECOND_LIST_INTEGRITY_CHECK_VALUE /*< Set to a known value if configUSE_LIST_DATA_INTEGRITY_CHECK_BYTES is set to 1. */ +} List_t; /* * Access macro to set the owner of a list item. The owner of a list item @@ -145,7 +218,7 @@ typedef struct xLIST * \page listSET_LIST_ITEM_OWNER listSET_LIST_ITEM_OWNER * \ingroup LinkedList */ -#define listSET_LIST_ITEM_OWNER( pxListItem, pxOwner ) ( pxListItem )->pvOwner = ( void * ) ( pxOwner ) +#define listSET_LIST_ITEM_OWNER( pxListItem, pxOwner ) ( ( pxListItem )->pvOwner = ( void * ) ( pxOwner ) ) /* * Access macro to get the owner of a list item. The owner of a list item @@ -154,7 +227,7 @@ typedef struct xLIST * \page listSET_LIST_ITEM_OWNER listSET_LIST_ITEM_OWNER * \ingroup LinkedList */ -#define listGET_LIST_ITEM_OWNER( pxListItem ) ( pxListItem )->pvOwner +#define listGET_LIST_ITEM_OWNER( pxListItem ) ( ( pxListItem )->pvOwner ) /* * Access macro to set the value of the list item. In most cases the value is @@ -163,26 +236,50 @@ typedef struct xLIST * \page listSET_LIST_ITEM_VALUE listSET_LIST_ITEM_VALUE * \ingroup LinkedList */ -#define listSET_LIST_ITEM_VALUE( pxListItem, xValue ) ( pxListItem )->xItemValue = ( xValue ) +#define listSET_LIST_ITEM_VALUE( pxListItem, xValue ) ( ( pxListItem )->xItemValue = ( xValue ) ) /* * Access macro to retrieve the value of the list item. The value can - * represent anything - for example a the priority of a task, or the time at + * represent anything - for example the priority of a task, or the time at * which a task should be unblocked. * * \page listGET_LIST_ITEM_VALUE listGET_LIST_ITEM_VALUE * \ingroup LinkedList */ -#define listGET_LIST_ITEM_VALUE( pxListItem ) ( ( pxListItem )->xItemValue ) +#define listGET_LIST_ITEM_VALUE( pxListItem ) ( ( pxListItem )->xItemValue ) /* - * Access macro the retrieve the value of the list item at the head of a given + * Access macro to retrieve the value of the list item at the head of a given * list. * * \page listGET_LIST_ITEM_VALUE listGET_LIST_ITEM_VALUE * \ingroup LinkedList */ -#define listGET_ITEM_VALUE_OF_HEAD_ENTRY( pxList ) ( (&( ( pxList )->xListEnd ))->pxNext->xItemValue ) +#define listGET_ITEM_VALUE_OF_HEAD_ENTRY( pxList ) ( ( ( pxList )->xListEnd ).pxNext->xItemValue ) + +/* + * Return the list item at the head of the list. + * + * \page listGET_HEAD_ENTRY listGET_HEAD_ENTRY + * \ingroup LinkedList + */ +#define listGET_HEAD_ENTRY( pxList ) ( ( ( pxList )->xListEnd ).pxNext ) + +/* + * Return the list item at the head of the list. + * + * \page listGET_NEXT listGET_NEXT + * \ingroup LinkedList + */ +#define listGET_NEXT( pxListItem ) ( ( pxListItem )->pxNext ) + +/* + * Return the list item that marks the end of the list + * + * \page listGET_END_MARKER listGET_END_MARKER + * \ingroup LinkedList + */ +#define listGET_END_MARKER( pxList ) ( ( ListItem_t const * ) ( &( ( pxList )->xListEnd ) ) ) /* * Access macro to determine if a list contains any items. The macro will @@ -191,19 +288,19 @@ typedef struct xLIST * \page listLIST_IS_EMPTY listLIST_IS_EMPTY * \ingroup LinkedList */ -#define listLIST_IS_EMPTY( pxList ) ( ( pxList )->uxNumberOfItems == ( unsigned portBASE_TYPE ) 0 ) +#define listLIST_IS_EMPTY( pxList ) ( ( BaseType_t ) ( ( pxList )->uxNumberOfItems == ( UBaseType_t ) 0 ) ) /* * Access macro to return the number of items in the list. */ -#define listCURRENT_LIST_LENGTH( pxList ) ( ( pxList )->uxNumberOfItems ) +#define listCURRENT_LIST_LENGTH( pxList ) ( ( pxList )->uxNumberOfItems ) /* * Access function to obtain the owner of the next entry in a list. * * The list member pxIndex is used to walk through a list. Calling * listGET_OWNER_OF_NEXT_ENTRY increments pxIndex to the next item in the list - * and returns that entries pxOwner parameter. Using multiple calls to this + * and returns that entry's pxOwner parameter. Using multiple calls to this * function it is therefore possible to move through every item contained in * a list. * @@ -212,22 +309,23 @@ typedef struct xLIST * The pxOwner parameter effectively creates a two way link between the list * item and its owner. * + * @param pxTCB pxTCB is set to the address of the owner of the next list item. * @param pxList The list from which the next item owner is to be returned. * * \page listGET_OWNER_OF_NEXT_ENTRY listGET_OWNER_OF_NEXT_ENTRY * \ingroup LinkedList */ -#define listGET_OWNER_OF_NEXT_ENTRY( pxTCB, pxList ) \ -{ \ -xList * const pxConstList = ( pxList ); \ - /* Increment the index to the next item and return the item, ensuring */ \ - /* we don't return the marker used at the end of the list. */ \ - ( pxConstList )->pxIndex = ( pxConstList )->pxIndex->pxNext; \ - if( ( pxConstList )->pxIndex == ( xListItem * ) &( ( pxConstList )->xListEnd ) ) \ - { \ - ( pxConstList )->pxIndex = ( pxConstList )->pxIndex->pxNext; \ - } \ - ( pxTCB ) = ( pxConstList )->pxIndex->pvOwner; \ +#define listGET_OWNER_OF_NEXT_ENTRY( pxTCB, pxList ) \ +{ \ +List_t * const pxConstList = ( pxList ); \ + /* Increment the index to the next item and return the item, ensuring */ \ + /* we don't return the marker used at the end of the list. */ \ + ( pxConstList )->pxIndex = ( pxConstList )->pxIndex->pxNext; \ + if( ( void * ) ( pxConstList )->pxIndex == ( void * ) &( ( pxConstList )->xListEnd ) ) \ + { \ + ( pxConstList )->pxIndex = ( pxConstList )->pxIndex->pxNext; \ + } \ + ( pxTCB ) = ( pxConstList )->pxIndex->pvOwner; \ } @@ -256,16 +354,15 @@ xList * const pxConstList = ( pxList ); \ * * @param pxList The list we want to know if the list item is within. * @param pxListItem The list item we want to know if is in the list. - * @return pdTRUE is the list item is in the list, otherwise pdFALSE. - * pointer against + * @return pdTRUE if the list item is in the list, otherwise pdFALSE. */ -#define listIS_CONTAINED_WITHIN( pxList, pxListItem ) ( ( pxListItem )->pvContainer == ( void * ) ( pxList ) ) +#define listIS_CONTAINED_WITHIN( pxList, pxListItem ) ( ( BaseType_t ) ( ( pxListItem )->pvContainer == ( void * ) ( pxList ) ) ) /* * Return the list a list item is contained within (referenced from). * * @param pxListItem The list item being queried. - * @return A pointer to the xList object that references the pxListItem + * @return A pointer to the List_t object that references the pxListItem */ #define listLIST_ITEM_CONTAINER( pxListItem ) ( ( pxListItem )->pvContainer ) @@ -286,7 +383,7 @@ xList * const pxConstList = ( pxList ); \ * \page vListInitialise vListInitialise * \ingroup LinkedList */ -void vListInitialise( xList *pxList ); +void vListInitialise( List_t * const pxList ) PRIVILEGED_FUNCTION; /* * Must be called before a list item is used. This sets the list container to @@ -297,7 +394,7 @@ void vListInitialise( xList *pxList ); * \page vListInitialiseItem vListInitialiseItem * \ingroup LinkedList */ -void vListInitialiseItem( xListItem *pxItem ); +void vListInitialiseItem( ListItem_t * const pxItem ) PRIVILEGED_FUNCTION; /* * Insert a list item into a list. The item will be inserted into the list in @@ -305,12 +402,12 @@ void vListInitialiseItem( xListItem *pxItem ); * * @param pxList The list into which the item is to be inserted. * - * @param pxNewListItem The item to that is to be placed in the list. + * @param pxNewListItem The item that is to be placed in the list. * * \page vListInsert vListInsert * \ingroup LinkedList */ -void vListInsert( xList *pxList, xListItem *pxNewListItem ); +void vListInsert( List_t * const pxList, ListItem_t * const pxNewListItem ) PRIVILEGED_FUNCTION; /* * Insert a list item into a list. The item will be inserted in a position @@ -331,7 +428,7 @@ void vListInsert( xList *pxList, xListItem *pxNewListItem ); * \page vListInsertEnd vListInsertEnd * \ingroup LinkedList */ -void vListInsertEnd( xList *pxList, xListItem *pxNewListItem ); +void vListInsertEnd( List_t * const pxList, ListItem_t * const pxNewListItem ) PRIVILEGED_FUNCTION; /* * Remove an item from a list. The list item has a pointer to the list that @@ -339,14 +436,14 @@ void vListInsertEnd( xList *pxList, xListItem *pxNewListItem ); * * @param uxListRemove The item to be removed. The item will remove itself from * the list pointed to by it's pxContainer parameter. - * + * * @return The number of items that remain in the list after the list item has * been removed. * * \page uxListRemove uxListRemove * \ingroup LinkedList */ -unsigned portBASE_TYPE uxListRemove( xListItem *pxItemToRemove ); +UBaseType_t uxListRemove( ListItem_t * const pxItemToRemove ) PRIVILEGED_FUNCTION; #ifdef __cplusplus } diff --git a/lib/FreeRTOS/include/mpu_wrappers.h b/lib/FreeRTOS/include/mpu_wrappers.h index b5bc82d806..5b2ad474d8 100644 --- a/lib/FreeRTOS/include/mpu_wrappers.h +++ b/lib/FreeRTOS/include/mpu_wrappers.h @@ -1,75 +1,70 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. + + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ #ifndef MPU_WRAPPERS_H @@ -93,7 +88,6 @@ only for ports that are using the MPU. */ #define vTaskPrioritySet MPU_vTaskPrioritySet #define eTaskGetState MPU_eTaskGetState #define vTaskSuspend MPU_vTaskSuspend - #define xTaskIsTaskSuspended MPU_xTaskIsTaskSuspended #define vTaskResume MPU_vTaskResume #define vTaskSuspendAll MPU_vTaskSuspendAll #define xTaskResumeAll MPU_xTaskResumeAll @@ -108,6 +102,10 @@ only for ports that are using the MPU. */ #define xTaskGetCurrentTaskHandle MPU_xTaskGetCurrentTaskHandle #define xTaskGetSchedulerState MPU_xTaskGetSchedulerState #define xTaskGetIdleTaskHandle MPU_xTaskGetIdleTaskHandle + #define uxTaskGetSystemState MPU_uxTaskGetSystemState + #define xTaskGenericNotify MPU_xTaskGenericNotify + #define xTaskNotifyWait MPU_xTaskNotifyWait + #define ulTaskNotifyTake MPU_ulTaskNotifyTake #define xQueueGenericCreate MPU_xQueueGenericCreate #define xQueueCreateMutex MPU_xQueueCreateMutex @@ -125,17 +123,36 @@ only for ports that are using the MPU. */ #define xQueueSelectFromSet MPU_xQueueSelectFromSet #define xQueueAddToSet MPU_xQueueAddToSet #define xQueueRemoveFromSet MPU_xQueueRemoveFromSet + #define xQueueGetMutexHolder MPU_xQueueGetMutexHolder + #define xQueueGetMutexHolder MPU_xQueueGetMutexHolder #define pvPortMalloc MPU_pvPortMalloc #define vPortFree MPU_vPortFree #define xPortGetFreeHeapSize MPU_xPortGetFreeHeapSize #define vPortInitialiseBlocks MPU_vPortInitialiseBlocks + #define xPortGetMinimumEverFreeHeapSize MPU_xPortGetMinimumEverFreeHeapSize #if configQUEUE_REGISTRY_SIZE > 0 #define vQueueAddToRegistry MPU_vQueueAddToRegistry #define vQueueUnregisterQueue MPU_vQueueUnregisterQueue #endif + #define xTimerCreate MPU_xTimerCreate + #define pvTimerGetTimerID MPU_pvTimerGetTimerID + #define vTimerSetTimerID MPU_vTimerSetTimerID + #define xTimerIsTimerActive MPU_xTimerIsTimerActive + #define xTimerGetTimerDaemonTaskHandle MPU_xTimerGetTimerDaemonTaskHandle + #define xTimerPendFunctionCall MPU_xTimerPendFunctionCall + #define pcTimerGetTimerName MPU_pcTimerGetTimerName + #define xTimerGenericCommand MPU_xTimerGenericCommand + + #define xEventGroupCreate MPU_xEventGroupCreate + #define xEventGroupWaitBits MPU_xEventGroupWaitBits + #define xEventGroupClearBits MPU_xEventGroupClearBits + #define xEventGroupSetBits MPU_xEventGroupSetBits + #define xEventGroupSync MPU_xEventGroupSync + #define vEventGroupDelete MPU_vEventGroupDelete + /* Remove the privileged function macro. */ #define PRIVILEGED_FUNCTION @@ -144,7 +161,6 @@ only for ports that are using the MPU. */ /* Ensure API functions go in the privileged execution section. */ #define PRIVILEGED_FUNCTION __attribute__((section("privileged_functions"))) #define PRIVILEGED_DATA __attribute__((section("privileged_data"))) - //#define PRIVILEGED_DATA #endif /* MPU_WRAPPERS_INCLUDED_FROM_API_FILE */ diff --git a/lib/FreeRTOS/include/portable.h b/lib/FreeRTOS/include/portable.h index cb353891cf..cd6a9344c4 100644 --- a/lib/FreeRTOS/include/portable.h +++ b/lib/FreeRTOS/include/portable.h @@ -1,75 +1,70 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. + + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ /*----------------------------------------------------------- @@ -79,255 +74,34 @@ #ifndef PORTABLE_H #define PORTABLE_H -/* Include the macro file relevant to the port being used. */ - -#ifdef OPEN_WATCOM_INDUSTRIAL_PC_PORT - #include "..\..\Source\portable\owatcom\16bitdos\pc\portmacro.h" - typedef void ( __interrupt __far *pxISR )(); -#endif - -#ifdef OPEN_WATCOM_FLASH_LITE_186_PORT - #include "..\..\Source\portable\owatcom\16bitdos\flsh186\portmacro.h" - typedef void ( __interrupt __far *pxISR )(); -#endif - -#ifdef GCC_MEGA_AVR - #include "../portable/GCC/ATMega323/portmacro.h" -#endif - -#ifdef IAR_MEGA_AVR - #include "../portable/IAR/ATMega323/portmacro.h" -#endif - -#ifdef MPLAB_PIC24_PORT - #include "..\..\Source\portable\MPLAB\PIC24_dsPIC\portmacro.h" -#endif - -#ifdef MPLAB_DSPIC_PORT - #include "..\..\Source\portable\MPLAB\PIC24_dsPIC\portmacro.h" -#endif - -#ifdef MPLAB_PIC18F_PORT - #include "..\..\Source\portable\MPLAB\PIC18F\portmacro.h" -#endif - -#ifdef MPLAB_PIC32MX_PORT - #include "..\..\Source\portable\MPLAB\PIC32MX\portmacro.h" -#endif - -#ifdef _FEDPICC - #include "libFreeRTOS/Include/portmacro.h" -#endif - -#ifdef SDCC_CYGNAL - #include "../../Source/portable/SDCC/Cygnal/portmacro.h" -#endif - -#ifdef GCC_ARM7 - #include "../../Source/portable/GCC/ARM7_LPC2000/portmacro.h" -#endif - -#ifdef GCC_ARM7_ECLIPSE - #include "portmacro.h" -#endif - -#ifdef ROWLEY_LPC23xx - #include "../../Source/portable/GCC/ARM7_LPC23xx/portmacro.h" -#endif - -#ifdef IAR_MSP430 - #include "..\..\Source\portable\IAR\MSP430\portmacro.h" -#endif - -#ifdef GCC_MSP430 - #include "../../Source/portable/GCC/MSP430F449/portmacro.h" -#endif - -#ifdef ROWLEY_MSP430 - #include "../../Source/portable/Rowley/MSP430F449/portmacro.h" -#endif - -#ifdef ARM7_LPC21xx_KEIL_RVDS - #include "..\..\Source\portable\RVDS\ARM7_LPC21xx\portmacro.h" -#endif - -#ifdef SAM7_GCC - #include "../../Source/portable/GCC/ARM7_AT91SAM7S/portmacro.h" -#endif - -#ifdef SAM7_IAR - #include "..\..\Source\portable\IAR\AtmelSAM7S64\portmacro.h" -#endif - -#ifdef SAM9XE_IAR - #include "..\..\Source\portable\IAR\AtmelSAM9XE\portmacro.h" -#endif - -#ifdef LPC2000_IAR - #include "..\..\Source\portable\IAR\LPC2000\portmacro.h" -#endif - -#ifdef STR71X_IAR - #include "..\..\Source\portable\IAR\STR71x\portmacro.h" -#endif - -#ifdef STR75X_IAR - #include "..\..\Source\portable\IAR\STR75x\portmacro.h" -#endif - -#ifdef STR75X_GCC - #include "..\..\Source\portable\GCC\STR75x\portmacro.h" -#endif - -#ifdef STR91X_IAR - #include "..\..\Source\portable\IAR\STR91x\portmacro.h" -#endif - -#ifdef GCC_H8S - #include "../../Source/portable/GCC/H8S2329/portmacro.h" -#endif - -#ifdef GCC_AT91FR40008 - #include "../../Source/portable/GCC/ARM7_AT91FR40008/portmacro.h" -#endif - -#ifdef RVDS_ARMCM3_LM3S102 - #include "../../Source/portable/RVDS/ARM_CM3/portmacro.h" -#endif - -#ifdef GCC_ARMCM3_LM3S102 - #include "../../Source/portable/GCC/ARM_CM3/portmacro.h" -#endif - -#ifdef GCC_ARMCM3 - #include "../../Source/portable/GCC/ARM_CM3/portmacro.h" -#endif - -#ifdef IAR_ARM_CM3 - #include "../../Source/portable/IAR/ARM_CM3/portmacro.h" -#endif - -#ifdef IAR_ARMCM3_LM - #include "../../Source/portable/IAR/ARM_CM3/portmacro.h" -#endif - -#ifdef HCS12_CODE_WARRIOR - #include "../../Source/portable/CodeWarrior/HCS12/portmacro.h" -#endif - -#ifdef MICROBLAZE_GCC - #include "../../Source/portable/GCC/MicroBlaze/portmacro.h" -#endif - -#ifdef TERN_EE - #include "..\..\Source\portable\Paradigm\Tern_EE\small\portmacro.h" -#endif - -#ifdef GCC_HCS12 - #include "../../Source/portable/GCC/HCS12/portmacro.h" -#endif - -#ifdef GCC_MCF5235 - #include "../../Source/portable/GCC/MCF5235/portmacro.h" -#endif - -#ifdef COLDFIRE_V2_GCC - #include "../../../Source/portable/GCC/ColdFire_V2/portmacro.h" -#endif - -#ifdef COLDFIRE_V2_CODEWARRIOR - #include "../../Source/portable/CodeWarrior/ColdFire_V2/portmacro.h" -#endif - -#ifdef GCC_PPC405 - #include "../../Source/portable/GCC/PPC405_Xilinx/portmacro.h" -#endif - -#ifdef GCC_PPC440 - #include "../../Source/portable/GCC/PPC440_Xilinx/portmacro.h" -#endif - -#ifdef _16FX_SOFTUNE - #include "..\..\Source\portable\Softune\MB96340\portmacro.h" -#endif - -#ifdef BCC_INDUSTRIAL_PC_PORT - /* A short file name has to be used in place of the normal - FreeRTOSConfig.h when using the Borland compiler. */ - #include "frconfig.h" - #include "..\portable\BCC\16BitDOS\PC\prtmacro.h" - typedef void ( __interrupt __far *pxISR )(); -#endif - -#ifdef BCC_FLASH_LITE_186_PORT - /* A short file name has to be used in place of the normal - FreeRTOSConfig.h when using the Borland compiler. */ - #include "frconfig.h" - #include "..\portable\BCC\16BitDOS\flsh186\prtmacro.h" - typedef void ( __interrupt __far *pxISR )(); -#endif - -#ifdef __GNUC__ - #ifdef __AVR32_AVR32A__ - #include "portmacro.h" - #endif -#endif - -#ifdef __ICCAVR32__ - #ifdef __CORE__ - #if __CORE__ == __AVR32A__ - #include "portmacro.h" - #endif - #endif -#endif - -#ifdef __91467D - #include "portmacro.h" -#endif - -#ifdef __96340 +/* Each FreeRTOS port has a unique portmacro.h header file. Originally a +pre-processor definition was used to ensure the pre-processor found the correct +portmacro.h file for the port being used. That scheme was deprecated in favour +of setting the compiler's include path such that it found the correct +portmacro.h file - removing the need for the constant and allowing the +portmacro.h file to be located anywhere in relation to the port being used. +Purely for reasons of backward compatibility the old method is still valid, but +to make it clear that new projects should not use it, support for the port +specific constants has been moved into the deprecated_definitions.h header +file. */ +#include "deprecated_definitions.h" + +/* If portENTER_CRITICAL is not defined then including deprecated_definitions.h +did not result in a portmacro.h header file being included - and it should be +included here. In this case the path to the correct portmacro.h header file +must be set in the compiler's include path. */ +#ifndef portENTER_CRITICAL #include "portmacro.h" #endif - -#ifdef __IAR_V850ES_Fx3__ - #include "../../Source/portable/IAR/V850ES/portmacro.h" -#endif - -#ifdef __IAR_V850ES_Jx3__ - #include "../../Source/portable/IAR/V850ES/portmacro.h" -#endif - -#ifdef __IAR_V850ES_Jx3_L__ - #include "../../Source/portable/IAR/V850ES/portmacro.h" -#endif - -#ifdef __IAR_V850ES_Jx2__ - #include "../../Source/portable/IAR/V850ES/portmacro.h" +#if portBYTE_ALIGNMENT == 32 + #define portBYTE_ALIGNMENT_MASK ( 0x001f ) #endif -#ifdef __IAR_V850ES_Hx2__ - #include "../../Source/portable/IAR/V850ES/portmacro.h" +#if portBYTE_ALIGNMENT == 16 + #define portBYTE_ALIGNMENT_MASK ( 0x000f ) #endif -#ifdef __IAR_78K0R_Kx3__ - #include "../../Source/portable/IAR/78K0R/portmacro.h" -#endif - -#ifdef __IAR_78K0R_Kx3L__ - #include "../../Source/portable/IAR/78K0R/portmacro.h" -#endif - -/* Catch all to ensure portmacro.h is included in the build. Newer demos -have the path as part of the project options, rather than as relative from -the project location. If portENTER_CRITICAL() has not been defined then -portmacro.h has not yet been included - as every portmacro.h provides a -portENTER_CRITICAL() definition. Check the demo application for your demo -to find the path to the correct portmacro.h file. */ -#ifndef portENTER_CRITICAL - #include "portmacro.h" -#endif - #if portBYTE_ALIGNMENT == 8 #define portBYTE_ALIGNMENT_MASK ( 0x0007 ) #endif @@ -365,11 +139,32 @@ extern "C" { * */ #if( portUSING_MPU_WRAPPERS == 1 ) - portSTACK_TYPE *pxPortInitialiseStack( portSTACK_TYPE *pxTopOfStack, pdTASK_CODE pxCode, void *pvParameters, portBASE_TYPE xRunPrivileged ) PRIVILEGED_FUNCTION; + StackType_t *pxPortInitialiseStack( StackType_t *pxTopOfStack, TaskFunction_t pxCode, void *pvParameters, BaseType_t xRunPrivileged ) PRIVILEGED_FUNCTION; #else - portSTACK_TYPE *pxPortInitialiseStack( portSTACK_TYPE *pxTopOfStack, pdTASK_CODE pxCode, void *pvParameters ); + StackType_t *pxPortInitialiseStack( StackType_t *pxTopOfStack, TaskFunction_t pxCode, void *pvParameters ) PRIVILEGED_FUNCTION; #endif +/* Used by heap_5.c. */ +typedef struct HeapRegion +{ + uint8_t *pucStartAddress; + size_t xSizeInBytes; +} HeapRegion_t; + +/* + * Used to define multiple heap regions for use by heap_5.c. This function + * must be called before any calls to pvPortMalloc() - not creating a task, + * queue, semaphore, mutex, software timer, event group, etc. will result in + * pvPortMalloc being called. + * + * pxHeapRegions passes in an array of HeapRegion_t structures - each of which + * defines a region of memory that can be used as the heap. The array is + * terminated by a HeapRegions_t structure that has a size of 0. The region + * with the lowest start address must appear first in the array. + */ +void vPortDefineHeapRegions( const HeapRegion_t * const pxHeapRegions ) PRIVILEGED_FUNCTION; + + /* * Map to the memory management routines required for the port. */ @@ -377,12 +172,13 @@ void *pvPortMalloc( size_t xSize ) PRIVILEGED_FUNCTION; void vPortFree( void *pv ) PRIVILEGED_FUNCTION; void vPortInitialiseBlocks( void ) PRIVILEGED_FUNCTION; size_t xPortGetFreeHeapSize( void ) PRIVILEGED_FUNCTION; +size_t xPortGetMinimumEverFreeHeapSize( void ) PRIVILEGED_FUNCTION; /* * Setup the hardware ready for the scheduler to take control. This generally * sets up a tick interrupt and sets timers for the correct tick frequency. */ -portBASE_TYPE xPortStartScheduler( void ) PRIVILEGED_FUNCTION; +BaseType_t xPortStartScheduler( void ) PRIVILEGED_FUNCTION; /* * Undo any hardware/ISR setup that was performed by xPortStartScheduler() so @@ -398,9 +194,9 @@ void vPortEndScheduler( void ) PRIVILEGED_FUNCTION; * Fills the xMPUSettings structure with the memory region information * contained in xRegions. */ -#if( portUSING_MPU_WRAPPERS == 1 ) +#if( portUSING_MPU_WRAPPERS == 1 ) struct xMEMORY_REGION; - void vPortStoreTaskMPUSettings( xMPU_SETTINGS *xMPUSettings, const struct xMEMORY_REGION * const xRegions, portSTACK_TYPE *pxBottomOfStack, unsigned short usStackDepth ) PRIVILEGED_FUNCTION; + void vPortStoreTaskMPUSettings( xMPU_SETTINGS *xMPUSettings, const struct xMEMORY_REGION * const xRegions, StackType_t *pxBottomOfStack, uint16_t usStackDepth ) PRIVILEGED_FUNCTION; #endif #ifdef __cplusplus diff --git a/lib/FreeRTOS/include/projdefs.h b/lib/FreeRTOS/include/projdefs.h index 2d5ae42aa7..956264b953 100644 --- a/lib/FreeRTOS/include/projdefs.h +++ b/lib/FreeRTOS/include/projdefs.h @@ -1,97 +1,155 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ #ifndef PROJDEFS_H #define PROJDEFS_H -/* Defines the prototype to which task functions must conform. */ -typedef void (*pdTASK_CODE)( void * ); +/* + * Defines the prototype to which task functions must conform. Defined in this + * file to ensure the type is known before portable.h is included. + */ +typedef void (*TaskFunction_t)( void * ); -#define pdTRUE ( 1 ) -#define pdFALSE ( 0 ) +/* Converts a time in milliseconds to a time in ticks. */ +#define pdMS_TO_TICKS( xTimeInMs ) ( ( TickType_t ) ( ( ( TickType_t ) ( xTimeInMs ) * ( TickType_t ) configTICK_RATE_HZ ) / ( TickType_t ) 1000 ) ) -#define pdPASS ( 1 ) -#define pdFAIL ( 0 ) -#define errQUEUE_EMPTY ( 0 ) -#define errQUEUE_FULL ( 0 ) +#define pdFALSE ( ( BaseType_t ) 0 ) +#define pdTRUE ( ( BaseType_t ) 1 ) -/* Error definitions. */ +#define pdPASS ( pdTRUE ) +#define pdFAIL ( pdFALSE ) +#define errQUEUE_EMPTY ( ( BaseType_t ) 0 ) +#define errQUEUE_FULL ( ( BaseType_t ) 0 ) + +/* FreeRTOS error definitions. */ #define errCOULD_NOT_ALLOCATE_REQUIRED_MEMORY ( -1 ) -#define errNO_TASK_TO_RUN ( -2 ) #define errQUEUE_BLOCKED ( -4 ) #define errQUEUE_YIELD ( -5 ) +/* Macros used for basic data corruption checks. */ +#ifndef configUSE_LIST_DATA_INTEGRITY_CHECK_BYTES + #define configUSE_LIST_DATA_INTEGRITY_CHECK_BYTES 0 +#endif + +#if( configUSE_16_BIT_TICKS == 1 ) + #define pdINTEGRITY_CHECK_VALUE 0x5a5a +#else + #define pdINTEGRITY_CHECK_VALUE 0x5a5a5a5aUL +#endif + +/* The following errno values are used by FreeRTOS+ components, not FreeRTOS +itself. */ +#define pdFREERTOS_ERRNO_NONE 0 /* No errors */ +#define pdFREERTOS_ERRNO_ENOENT 2 /* No such file or directory */ +#define pdFREERTOS_ERRNO_EIO 5 /* I/O error */ +#define pdFREERTOS_ERRNO_ENXIO 6 /* No such device or address */ +#define pdFREERTOS_ERRNO_EBADF 9 /* Bad file number */ +#define pdFREERTOS_ERRNO_EAGAIN 11 /* No more processes */ +#define pdFREERTOS_ERRNO_EWOULDBLOCK 11 /* Operation would block */ +#define pdFREERTOS_ERRNO_ENOMEM 12 /* Not enough memory */ +#define pdFREERTOS_ERRNO_EACCES 13 /* Permission denied */ +#define pdFREERTOS_ERRNO_EFAULT 14 /* Bad address */ +#define pdFREERTOS_ERRNO_EBUSY 16 /* Mount device busy */ +#define pdFREERTOS_ERRNO_EEXIST 17 /* File exists */ +#define pdFREERTOS_ERRNO_EXDEV 18 /* Cross-device link */ +#define pdFREERTOS_ERRNO_ENODEV 19 /* No such device */ +#define pdFREERTOS_ERRNO_ENOTDIR 20 /* Not a directory */ +#define pdFREERTOS_ERRNO_EISDIR 21 /* Is a directory */ +#define pdFREERTOS_ERRNO_EINVAL 22 /* Invalid argument */ +#define pdFREERTOS_ERRNO_ENOSPC 28 /* No space left on device */ +#define pdFREERTOS_ERRNO_ESPIPE 29 /* Illegal seek */ +#define pdFREERTOS_ERRNO_EROFS 30 /* Read only file system */ +#define pdFREERTOS_ERRNO_EUNATCH 42 /* Protocol driver not attached */ +#define pdFREERTOS_ERRNO_EBADE 50 /* Invalid exchange */ +#define pdFREERTOS_ERRNO_EFTYPE 79 /* Inappropriate file type or format */ +#define pdFREERTOS_ERRNO_ENMFILE 89 /* No more files */ +#define pdFREERTOS_ERRNO_ENOTEMPTY 90 /* Directory not empty */ +#define pdFREERTOS_ERRNO_ENAMETOOLONG 91 /* File or path name too long */ +#define pdFREERTOS_ERRNO_EOPNOTSUPP 95 /* Operation not supported on transport endpoint */ +#define pdFREERTOS_ERRNO_ENOBUFS 105 /* No buffer space available */ +#define pdFREERTOS_ERRNO_ENOPROTOOPT 109 /* Protocol not available */ +#define pdFREERTOS_ERRNO_EADDRINUSE 112 /* Address already in use */ +#define pdFREERTOS_ERRNO_ETIMEDOUT 116 /* Connection timed out */ +#define pdFREERTOS_ERRNO_EINPROGRESS 119 /* Connection already in progress */ +#define pdFREERTOS_ERRNO_EALREADY 120 /* Socket already connected */ +#define pdFREERTOS_ERRNO_EADDRNOTAVAIL 125 /* Address not available */ +#define pdFREERTOS_ERRNO_EISCONN 127 /* Socket is already connected */ +#define pdFREERTOS_ERRNO_ENOTCONN 128 /* Socket is not connected */ +#define pdFREERTOS_ERRNO_ENOMEDIUM 135 /* No medium inserted */ +#define pdFREERTOS_ERRNO_EILSEQ 138 /* An invalid UTF-16 sequence was encountered. */ +#define pdFREERTOS_ERRNO_ECANCELED 140 /* Operation canceled. */ + +/* The following endian values are used by FreeRTOS+ components, not FreeRTOS +itself. */ +#define pdFREERTOS_LITTLE_ENDIAN 0 +#define pdFREERTOS_BIG_ENDIAN 1 + #endif /* PROJDEFS_H */ diff --git a/lib/FreeRTOS/include/queue.h b/lib/FreeRTOS/include/queue.h index bb4a0ff772..2d7b5e90a7 100644 --- a/lib/FreeRTOS/include/queue.h +++ b/lib/FreeRTOS/include/queue.h @@ -1,75 +1,70 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ @@ -85,47 +80,46 @@ extern "C" { #endif -#include "mpu_wrappers.h" - /** * Type by which queues are referenced. For example, a call to xQueueCreate() - * returns an xQueueHandle variable that can then be used as a parameter to + * returns an QueueHandle_t variable that can then be used as a parameter to * xQueueSend(), xQueueReceive(), etc. */ -typedef void * xQueueHandle; +typedef void * QueueHandle_t; /** * Type by which queue sets are referenced. For example, a call to * xQueueCreateSet() returns an xQueueSet variable that can then be used as a * parameter to xQueueSelectFromSet(), xQueueAddToSet(), etc. */ -typedef void * xQueueSetHandle; +typedef void * QueueSetHandle_t; /** * Queue sets can contain both queues and semaphores, so the - * xQueueSetMemberHandle is defined as a type to be used where a parameter or - * return value can be either an xQueueHandle or an xSemaphoreHandle. + * QueueSetMemberHandle_t is defined as a type to be used where a parameter or + * return value can be either an QueueHandle_t or an SemaphoreHandle_t. */ -typedef void * xQueueSetMemberHandle; +typedef void * QueueSetMemberHandle_t; /* For internal use only. */ -#define queueSEND_TO_BACK ( 0 ) -#define queueSEND_TO_FRONT ( 1 ) +#define queueSEND_TO_BACK ( ( BaseType_t ) 0 ) +#define queueSEND_TO_FRONT ( ( BaseType_t ) 1 ) +#define queueOVERWRITE ( ( BaseType_t ) 2 ) /* For internal use only. These definitions *must* match those in queue.c. */ -#define queueQUEUE_TYPE_BASE ( 0U ) -#define queueQUEUE_TYPE_SET ( 0U ) -#define queueQUEUE_TYPE_MUTEX ( 1U ) -#define queueQUEUE_TYPE_COUNTING_SEMAPHORE ( 2U ) -#define queueQUEUE_TYPE_BINARY_SEMAPHORE ( 3U ) -#define queueQUEUE_TYPE_RECURSIVE_MUTEX ( 4U ) +#define queueQUEUE_TYPE_BASE ( ( uint8_t ) 0U ) +#define queueQUEUE_TYPE_SET ( ( uint8_t ) 0U ) +#define queueQUEUE_TYPE_MUTEX ( ( uint8_t ) 1U ) +#define queueQUEUE_TYPE_COUNTING_SEMAPHORE ( ( uint8_t ) 2U ) +#define queueQUEUE_TYPE_BINARY_SEMAPHORE ( ( uint8_t ) 3U ) +#define queueQUEUE_TYPE_RECURSIVE_MUTEX ( ( uint8_t ) 4U ) /** * queue. h *
- xQueueHandle xQueueCreate(
-							  unsigned portBASE_TYPE uxQueueLength,
-							  unsigned portBASE_TYPE uxItemSize
+ QueueHandle_t xQueueCreate(
+							  UBaseType_t uxQueueLength,
+							  UBaseType_t uxItemSize
 						  );
  * 
* @@ -153,10 +147,10 @@ typedef void * xQueueSetMemberHandle; void vATask( void *pvParameters ) { - xQueueHandle xQueue1, xQueue2; + QueueHandle_t xQueue1, xQueue2; - // Create a queue capable of containing 10 unsigned long values. - xQueue1 = xQueueCreate( 10, sizeof( unsigned long ) ); + // Create a queue capable of containing 10 uint32_t values. + xQueue1 = xQueueCreate( 10, sizeof( uint32_t ) ); if( xQueue1 == 0 ) { // Queue was not created and must not be used. @@ -181,10 +175,10 @@ typedef void * xQueueSetMemberHandle; /** * queue. h *
- portBASE_TYPE xQueueSendToToFront(
-								   xQueueHandle	xQueue,
-								   const void	*	pvItemToQueue,
-								   portTickType	xTicksToWait
+ BaseType_t xQueueSendToToFront(
+								   QueueHandle_t	xQueue,
+								   const void		*pvItemToQueue,
+								   TickType_t		xTicksToWait
 							   );
  * 
* @@ -206,7 +200,7 @@ typedef void * xQueueSetMemberHandle; * waiting for space to become available on the queue, should it already * be full. The call will return immediately if this is set to 0 and the * queue is full. The time is defined in tick periods so the constant - * portTICK_RATE_MS should be used to convert to real time if this is required. + * portTICK_PERIOD_MS should be used to convert to real time if this is required. * * @return pdTRUE if the item was successfully posted, otherwise errQUEUE_FULL. * @@ -218,15 +212,15 @@ typedef void * xQueueSetMemberHandle; char ucData[ 20 ]; } xMessage; - unsigned long ulVar = 10UL; + uint32_t ulVar = 10UL; void vATask( void *pvParameters ) { - xQueueHandle xQueue1, xQueue2; + QueueHandle_t xQueue1, xQueue2; struct AMessage *pxMessage; - // Create a queue capable of containing 10 unsigned long values. - xQueue1 = xQueueCreate( 10, sizeof( unsigned long ) ); + // Create a queue capable of containing 10 uint32_t values. + xQueue1 = xQueueCreate( 10, sizeof( uint32_t ) ); // Create a queue capable of containing 10 pointers to AMessage structures. // These should be passed by pointer as they contain a lot of data. @@ -236,9 +230,9 @@ typedef void * xQueueSetMemberHandle; if( xQueue1 != 0 ) { - // Send an unsigned long. Wait for 10 ticks for space to become + // Send an uint32_t. Wait for 10 ticks for space to become // available if necessary. - if( xQueueSendToFront( xQueue1, ( void * ) &ulVar, ( portTickType ) 10 ) != pdPASS ) + if( xQueueSendToFront( xQueue1, ( void * ) &ulVar, ( TickType_t ) 10 ) != pdPASS ) { // Failed to post the message, even after 10 ticks. } @@ -249,7 +243,7 @@ typedef void * xQueueSetMemberHandle; // Send a pointer to a struct AMessage object. Don't block if the // queue is already full. pxMessage = & xMessage; - xQueueSendToFront( xQueue2, ( void * ) &pxMessage, ( portTickType ) 0 ); + xQueueSendToFront( xQueue2, ( void * ) &pxMessage, ( TickType_t ) 0 ); } // ... Rest of task code. @@ -263,10 +257,10 @@ typedef void * xQueueSetMemberHandle; /** * queue. h *
- portBASE_TYPE xQueueSendToBack(
-								   xQueueHandle	xQueue,
-								   const	void	*	pvItemToQueue,
-								   portTickType	xTicksToWait
+ BaseType_t xQueueSendToBack(
+								   QueueHandle_t	xQueue,
+								   const void		*pvItemToQueue,
+								   TickType_t		xTicksToWait
 							   );
  * 
* @@ -288,7 +282,7 @@ typedef void * xQueueSetMemberHandle; * waiting for space to become available on the queue, should it already * be full. The call will return immediately if this is set to 0 and the queue * is full. The time is defined in tick periods so the constant - * portTICK_RATE_MS should be used to convert to real time if this is required. + * portTICK_PERIOD_MS should be used to convert to real time if this is required. * * @return pdTRUE if the item was successfully posted, otherwise errQUEUE_FULL. * @@ -300,15 +294,15 @@ typedef void * xQueueSetMemberHandle; char ucData[ 20 ]; } xMessage; - unsigned long ulVar = 10UL; + uint32_t ulVar = 10UL; void vATask( void *pvParameters ) { - xQueueHandle xQueue1, xQueue2; + QueueHandle_t xQueue1, xQueue2; struct AMessage *pxMessage; - // Create a queue capable of containing 10 unsigned long values. - xQueue1 = xQueueCreate( 10, sizeof( unsigned long ) ); + // Create a queue capable of containing 10 uint32_t values. + xQueue1 = xQueueCreate( 10, sizeof( uint32_t ) ); // Create a queue capable of containing 10 pointers to AMessage structures. // These should be passed by pointer as they contain a lot of data. @@ -318,9 +312,9 @@ typedef void * xQueueSetMemberHandle; if( xQueue1 != 0 ) { - // Send an unsigned long. Wait for 10 ticks for space to become + // Send an uint32_t. Wait for 10 ticks for space to become // available if necessary. - if( xQueueSendToBack( xQueue1, ( void * ) &ulVar, ( portTickType ) 10 ) != pdPASS ) + if( xQueueSendToBack( xQueue1, ( void * ) &ulVar, ( TickType_t ) 10 ) != pdPASS ) { // Failed to post the message, even after 10 ticks. } @@ -331,7 +325,7 @@ typedef void * xQueueSetMemberHandle; // Send a pointer to a struct AMessage object. Don't block if the // queue is already full. pxMessage = & xMessage; - xQueueSendToBack( xQueue2, ( void * ) &pxMessage, ( portTickType ) 0 ); + xQueueSendToBack( xQueue2, ( void * ) &pxMessage, ( TickType_t ) 0 ); } // ... Rest of task code. @@ -345,10 +339,10 @@ typedef void * xQueueSetMemberHandle; /** * queue. h *
- portBASE_TYPE xQueueSend(
-							  xQueueHandle xQueue,
+ BaseType_t xQueueSend(
+							  QueueHandle_t xQueue,
 							  const void * pvItemToQueue,
-							  portTickType xTicksToWait
+							  TickType_t xTicksToWait
 						 );
  * 
* @@ -372,7 +366,7 @@ typedef void * xQueueSetMemberHandle; * waiting for space to become available on the queue, should it already * be full. The call will return immediately if this is set to 0 and the * queue is full. The time is defined in tick periods so the constant - * portTICK_RATE_MS should be used to convert to real time if this is required. + * portTICK_PERIOD_MS should be used to convert to real time if this is required. * * @return pdTRUE if the item was successfully posted, otherwise errQUEUE_FULL. * @@ -384,15 +378,15 @@ typedef void * xQueueSetMemberHandle; char ucData[ 20 ]; } xMessage; - unsigned long ulVar = 10UL; + uint32_t ulVar = 10UL; void vATask( void *pvParameters ) { - xQueueHandle xQueue1, xQueue2; + QueueHandle_t xQueue1, xQueue2; struct AMessage *pxMessage; - // Create a queue capable of containing 10 unsigned long values. - xQueue1 = xQueueCreate( 10, sizeof( unsigned long ) ); + // Create a queue capable of containing 10 uint32_t values. + xQueue1 = xQueueCreate( 10, sizeof( uint32_t ) ); // Create a queue capable of containing 10 pointers to AMessage structures. // These should be passed by pointer as they contain a lot of data. @@ -402,9 +396,9 @@ typedef void * xQueueSetMemberHandle; if( xQueue1 != 0 ) { - // Send an unsigned long. Wait for 10 ticks for space to become + // Send an uint32_t. Wait for 10 ticks for space to become // available if necessary. - if( xQueueSend( xQueue1, ( void * ) &ulVar, ( portTickType ) 10 ) != pdPASS ) + if( xQueueSend( xQueue1, ( void * ) &ulVar, ( TickType_t ) 10 ) != pdPASS ) { // Failed to post the message, even after 10 ticks. } @@ -415,7 +409,7 @@ typedef void * xQueueSetMemberHandle; // Send a pointer to a struct AMessage object. Don't block if the // queue is already full. pxMessage = & xMessage; - xQueueSend( xQueue2, ( void * ) &pxMessage, ( portTickType ) 0 ); + xQueueSend( xQueue2, ( void * ) &pxMessage, ( TickType_t ) 0 ); } // ... Rest of task code. @@ -426,15 +420,98 @@ typedef void * xQueueSetMemberHandle; */ #define xQueueSend( xQueue, pvItemToQueue, xTicksToWait ) xQueueGenericSend( ( xQueue ), ( pvItemToQueue ), ( xTicksToWait ), queueSEND_TO_BACK ) +/** + * queue. h + *
+ BaseType_t xQueueOverwrite(
+							  QueueHandle_t xQueue,
+							  const void * pvItemToQueue
+						 );
+ * 
+ * + * Only for use with queues that have a length of one - so the queue is either + * empty or full. + * + * Post an item on a queue. If the queue is already full then overwrite the + * value held in the queue. The item is queued by copy, not by reference. + * + * This function must not be called from an interrupt service routine. + * See xQueueOverwriteFromISR () for an alternative which may be used in an ISR. + * + * @param xQueue The handle of the queue to which the data is being sent. + * + * @param pvItemToQueue A pointer to the item that is to be placed on the + * queue. The size of the items the queue will hold was defined when the + * queue was created, so this many bytes will be copied from pvItemToQueue + * into the queue storage area. + * + * @return xQueueOverwrite() is a macro that calls xQueueGenericSend(), and + * therefore has the same return values as xQueueSendToFront(). However, pdPASS + * is the only value that can be returned because xQueueOverwrite() will write + * to the queue even when the queue is already full. + * + * Example usage: +
+
+ void vFunction( void *pvParameters )
+ {
+ QueueHandle_t xQueue;
+ uint32_t ulVarToSend, ulValReceived;
+
+	// Create a queue to hold one uint32_t value.  It is strongly
+	// recommended *not* to use xQueueOverwrite() on queues that can
+	// contain more than one value, and doing so will trigger an assertion
+	// if configASSERT() is defined.
+	xQueue = xQueueCreate( 1, sizeof( uint32_t ) );
+
+	// Write the value 10 to the queue using xQueueOverwrite().
+	ulVarToSend = 10;
+	xQueueOverwrite( xQueue, &ulVarToSend );
+
+	// Peeking the queue should now return 10, but leave the value 10 in
+	// the queue.  A block time of zero is used as it is known that the
+	// queue holds a value.
+	ulValReceived = 0;
+	xQueuePeek( xQueue, &ulValReceived, 0 );
+
+	if( ulValReceived != 10 )
+	{
+		// Error unless the item was removed by a different task.
+	}
+
+	// The queue is still full.  Use xQueueOverwrite() to overwrite the
+	// value held in the queue with 100.
+	ulVarToSend = 100;
+	xQueueOverwrite( xQueue, &ulVarToSend );
+
+	// This time read from the queue, leaving the queue empty once more.
+	// A block time of 0 is used again.
+	xQueueReceive( xQueue, &ulValReceived, 0 );
+
+	// The value read should be the last value written, even though the
+	// queue was already full when the value was written.
+	if( ulValReceived != 100 )
+	{
+		// Error!
+	}
+
+	// ...
+}
+ 
+ * \defgroup xQueueOverwrite xQueueOverwrite + * \ingroup QueueManagement + */ +#define xQueueOverwrite( xQueue, pvItemToQueue ) xQueueGenericSend( ( xQueue ), ( pvItemToQueue ), 0, queueOVERWRITE ) + /** * queue. h *
- portBASE_TYPE xQueueGenericSend(
-									xQueueHandle xQueue,
+ BaseType_t xQueueGenericSend(
+									QueueHandle_t xQueue,
 									const void * pvItemToQueue,
-									portTickType xTicksToWait
-									portBASE_TYPE xCopyPosition
+									TickType_t xTicksToWait
+									BaseType_t xCopyPosition
 								);
  * 
* @@ -456,7 +533,7 @@ typedef void * xQueueSetMemberHandle; * waiting for space to become available on the queue, should it already * be full. The call will return immediately if this is set to 0 and the * queue is full. The time is defined in tick periods so the constant - * portTICK_RATE_MS should be used to convert to real time if this is required. + * portTICK_PERIOD_MS should be used to convert to real time if this is required. * * @param xCopyPosition Can take the value queueSEND_TO_BACK to place the * item at the back of the queue, or queueSEND_TO_FRONT to place the item @@ -472,15 +549,15 @@ typedef void * xQueueSetMemberHandle; char ucData[ 20 ]; } xMessage; - unsigned long ulVar = 10UL; + uint32_t ulVar = 10UL; void vATask( void *pvParameters ) { - xQueueHandle xQueue1, xQueue2; + QueueHandle_t xQueue1, xQueue2; struct AMessage *pxMessage; - // Create a queue capable of containing 10 unsigned long values. - xQueue1 = xQueueCreate( 10, sizeof( unsigned long ) ); + // Create a queue capable of containing 10 uint32_t values. + xQueue1 = xQueueCreate( 10, sizeof( uint32_t ) ); // Create a queue capable of containing 10 pointers to AMessage structures. // These should be passed by pointer as they contain a lot of data. @@ -490,9 +567,9 @@ typedef void * xQueueSetMemberHandle; if( xQueue1 != 0 ) { - // Send an unsigned long. Wait for 10 ticks for space to become + // Send an uint32_t. Wait for 10 ticks for space to become // available if necessary. - if( xQueueGenericSend( xQueue1, ( void * ) &ulVar, ( portTickType ) 10, queueSEND_TO_BACK ) != pdPASS ) + if( xQueueGenericSend( xQueue1, ( void * ) &ulVar, ( TickType_t ) 10, queueSEND_TO_BACK ) != pdPASS ) { // Failed to post the message, even after 10 ticks. } @@ -503,7 +580,7 @@ typedef void * xQueueSetMemberHandle; // Send a pointer to a struct AMessage object. Don't block if the // queue is already full. pxMessage = & xMessage; - xQueueGenericSend( xQueue2, ( void * ) &pxMessage, ( portTickType ) 0, queueSEND_TO_BACK ); + xQueueGenericSend( xQueue2, ( void * ) &pxMessage, ( TickType_t ) 0, queueSEND_TO_BACK ); } // ... Rest of task code. @@ -512,15 +589,15 @@ typedef void * xQueueSetMemberHandle; * \defgroup xQueueSend xQueueSend * \ingroup QueueManagement */ -signed portBASE_TYPE xQueueGenericSend( xQueueHandle xQueue, const void * const pvItemToQueue, portTickType xTicksToWait, portBASE_TYPE xCopyPosition ); +BaseType_t xQueueGenericSend( QueueHandle_t xQueue, const void * const pvItemToQueue, TickType_t xTicksToWait, const BaseType_t xCopyPosition ) PRIVILEGED_FUNCTION; /** * queue. h *
- portBASE_TYPE xQueuePeek(
-							 xQueueHandle xQueue,
+ BaseType_t xQueuePeek(
+							 QueueHandle_t xQueue,
 							 void *pvBuffer,
-							 portTickType xTicksToWait
+							 TickType_t xTicksToWait
 						 );
* * This is a macro that calls the xQueueGenericReceive() function. @@ -533,7 +610,9 @@ signed portBASE_TYPE xQueueGenericSend( xQueueHandle xQueue, const void * const * Successfully received items remain on the queue so will be returned again * by the next call, or a call to xQueueReceive(). * - * This macro must not be used in an interrupt service routine. + * This macro must not be used in an interrupt service routine. See + * xQueuePeekFromISR() for an alternative that can be called from an interrupt + * service routine. * * @param xQueue The handle to the queue from which the item is to be * received. @@ -544,7 +623,7 @@ signed portBASE_TYPE xQueueGenericSend( xQueueHandle xQueue, const void * const * @param xTicksToWait The maximum amount of time the task should block * waiting for an item to receive should the queue be empty at the time * of the call. The time is defined in tick periods so the constant - * portTICK_RATE_MS should be used to convert to real time if this is required. + * portTICK_PERIOD_MS should be used to convert to real time if this is required. * xQueuePeek() will return immediately if xTicksToWait is 0 and the queue * is empty. * @@ -559,7 +638,7 @@ signed portBASE_TYPE xQueueGenericSend( xQueueHandle xQueue, const void * const char ucData[ 20 ]; } xMessage; - xQueueHandle xQueue; + QueueHandle_t xQueue; // Task to create a queue and post a value. void vATask( void *pvParameters ) @@ -579,7 +658,7 @@ signed portBASE_TYPE xQueueGenericSend( xQueueHandle xQueue, const void * const // Send a pointer to a struct AMessage object. Don't block if the // queue is already full. pxMessage = & xMessage; - xQueueSend( xQueue, ( void * ) &pxMessage, ( portTickType ) 0 ); + xQueueSend( xQueue, ( void * ) &pxMessage, ( TickType_t ) 0 ); // ... Rest of task code. } @@ -593,7 +672,7 @@ signed portBASE_TYPE xQueueGenericSend( xQueueHandle xQueue, const void * const { // Peek a message on the created queue. Block for 10 ticks if a // message is not immediately available. - if( xQueuePeek( xQueue, &( pxRxedMessage ), ( portTickType ) 10 ) ) + if( xQueuePeek( xQueue, &( pxRxedMessage ), ( TickType_t ) 10 ) ) { // pcRxedMessage now points to the struct AMessage variable posted // by vATask, but the item still remains on the queue. @@ -611,10 +690,43 @@ signed portBASE_TYPE xQueueGenericSend( xQueueHandle xQueue, const void * const /** * queue. h *
- portBASE_TYPE xQueueReceive(
-								 xQueueHandle xQueue,
+ BaseType_t xQueuePeekFromISR(
+									QueueHandle_t xQueue,
+									void *pvBuffer,
+								);
+ * + * A version of xQueuePeek() that can be called from an interrupt service + * routine (ISR). + * + * Receive an item from a queue without removing the item from the queue. + * The item is received by copy so a buffer of adequate size must be + * provided. The number of bytes copied into the buffer was defined when + * the queue was created. + * + * Successfully received items remain on the queue so will be returned again + * by the next call, or a call to xQueueReceive(). + * + * @param xQueue The handle to the queue from which the item is to be + * received. + * + * @param pvBuffer Pointer to the buffer into which the received item will + * be copied. + * + * @return pdTRUE if an item was successfully received from the queue, + * otherwise pdFALSE. + * + * \defgroup xQueuePeekFromISR xQueuePeekFromISR + * \ingroup QueueManagement + */ +BaseType_t xQueuePeekFromISR( QueueHandle_t xQueue, void * const pvBuffer ) PRIVILEGED_FUNCTION; + +/** + * queue. h + *
+ BaseType_t xQueueReceive(
+								 QueueHandle_t xQueue,
 								 void *pvBuffer,
-								 portTickType xTicksToWait
+								 TickType_t xTicksToWait
 							);
* * This is a macro that calls the xQueueGenericReceive() function. @@ -638,7 +750,7 @@ signed portBASE_TYPE xQueueGenericSend( xQueueHandle xQueue, const void * const * waiting for an item to receive should the queue be empty at the time * of the call. xQueueReceive() will return immediately if xTicksToWait * is zero and the queue is empty. The time is defined in tick periods so the - * constant portTICK_RATE_MS should be used to convert to real time if this is + * constant portTICK_PERIOD_MS should be used to convert to real time if this is * required. * * @return pdTRUE if an item was successfully received from the queue, @@ -652,7 +764,7 @@ signed portBASE_TYPE xQueueGenericSend( xQueueHandle xQueue, const void * const char ucData[ 20 ]; } xMessage; - xQueueHandle xQueue; + QueueHandle_t xQueue; // Task to create a queue and post a value. void vATask( void *pvParameters ) @@ -672,7 +784,7 @@ signed portBASE_TYPE xQueueGenericSend( xQueueHandle xQueue, const void * const // Send a pointer to a struct AMessage object. Don't block if the // queue is already full. pxMessage = & xMessage; - xQueueSend( xQueue, ( void * ) &pxMessage, ( portTickType ) 0 ); + xQueueSend( xQueue, ( void * ) &pxMessage, ( TickType_t ) 0 ); // ... Rest of task code. } @@ -686,7 +798,7 @@ signed portBASE_TYPE xQueueGenericSend( xQueueHandle xQueue, const void * const { // Receive a message on the created queue. Block for 10 ticks if a // message is not immediately available. - if( xQueueReceive( xQueue, &( pxRxedMessage ), ( portTickType ) 10 ) ) + if( xQueueReceive( xQueue, &( pxRxedMessage ), ( TickType_t ) 10 ) ) { // pcRxedMessage now points to the struct AMessage variable posted // by vATask. @@ -705,11 +817,11 @@ signed portBASE_TYPE xQueueGenericSend( xQueueHandle xQueue, const void * const /** * queue. h *
- portBASE_TYPE xQueueGenericReceive(
-									   xQueueHandle	xQueue,
+ BaseType_t xQueueGenericReceive(
+									   QueueHandle_t	xQueue,
 									   void	*pvBuffer,
-									   portTickType	xTicksToWait
-									   portBASE_TYPE	xJustPeek
+									   TickType_t	xTicksToWait
+									   BaseType_t	xJustPeek
 									);
* * It is preferred that the macro xQueueReceive() be used rather than calling @@ -731,7 +843,7 @@ signed portBASE_TYPE xQueueGenericSend( xQueueHandle xQueue, const void * const * @param xTicksToWait The maximum amount of time the task should block * waiting for an item to receive should the queue be empty at the time * of the call. The time is defined in tick periods so the constant - * portTICK_RATE_MS should be used to convert to real time if this is required. + * portTICK_PERIOD_MS should be used to convert to real time if this is required. * xQueueGenericReceive() will return immediately if the queue is empty and * xTicksToWait is 0. * @@ -751,7 +863,7 @@ signed portBASE_TYPE xQueueGenericSend( xQueueHandle xQueue, const void * const char ucData[ 20 ]; } xMessage; - xQueueHandle xQueue; + QueueHandle_t xQueue; // Task to create a queue and post a value. void vATask( void *pvParameters ) @@ -771,7 +883,7 @@ signed portBASE_TYPE xQueueGenericSend( xQueueHandle xQueue, const void * const // Send a pointer to a struct AMessage object. Don't block if the // queue is already full. pxMessage = & xMessage; - xQueueSend( xQueue, ( void * ) &pxMessage, ( portTickType ) 0 ); + xQueueSend( xQueue, ( void * ) &pxMessage, ( TickType_t ) 0 ); // ... Rest of task code. } @@ -785,7 +897,7 @@ signed portBASE_TYPE xQueueGenericSend( xQueueHandle xQueue, const void * const { // Receive a message on the created queue. Block for 10 ticks if a // message is not immediately available. - if( xQueueGenericReceive( xQueue, &( pxRxedMessage ), ( portTickType ) 10 ) ) + if( xQueueGenericReceive( xQueue, &( pxRxedMessage ), ( TickType_t ) 10 ) ) { // pcRxedMessage now points to the struct AMessage variable posted // by vATask. @@ -798,11 +910,11 @@ signed portBASE_TYPE xQueueGenericSend( xQueueHandle xQueue, const void * const * \defgroup xQueueReceive xQueueReceive * \ingroup QueueManagement */ -signed portBASE_TYPE xQueueGenericReceive( xQueueHandle xQueue, void * const pvBuffer, portTickType xTicksToWait, portBASE_TYPE xJustPeek ); +BaseType_t xQueueGenericReceive( QueueHandle_t xQueue, void * const pvBuffer, TickType_t xTicksToWait, const BaseType_t xJustPeek ) PRIVILEGED_FUNCTION; /** * queue. h - *
unsigned portBASE_TYPE uxQueueMessagesWaiting( const xQueueHandle xQueue );
+ *
UBaseType_t uxQueueMessagesWaiting( const QueueHandle_t xQueue );
* * Return the number of messages stored in a queue. * @@ -810,32 +922,49 @@ signed portBASE_TYPE xQueueGenericReceive( xQueueHandle xQueue, void * const pvB * * @return The number of messages available in the queue. * - * \page uxQueueMessagesWaiting uxQueueMessagesWaiting + * \defgroup uxQueueMessagesWaiting uxQueueMessagesWaiting * \ingroup QueueManagement */ -unsigned portBASE_TYPE uxQueueMessagesWaiting( const xQueueHandle xQueue ); +UBaseType_t uxQueueMessagesWaiting( const QueueHandle_t xQueue ) PRIVILEGED_FUNCTION; /** * queue. h - *
void vQueueDelete( xQueueHandle xQueue );
+ *
UBaseType_t uxQueueSpacesAvailable( const QueueHandle_t xQueue );
+ * + * Return the number of free spaces available in a queue. This is equal to the + * number of items that can be sent to the queue before the queue becomes full + * if no items are removed. + * + * @param xQueue A handle to the queue being queried. + * + * @return The number of spaces available in the queue. + * + * \defgroup uxQueueMessagesWaiting uxQueueMessagesWaiting + * \ingroup QueueManagement + */ +UBaseType_t uxQueueSpacesAvailable( const QueueHandle_t xQueue ) PRIVILEGED_FUNCTION; + +/** + * queue. h + *
void vQueueDelete( QueueHandle_t xQueue );
* * Delete a queue - freeing all the memory allocated for storing of items * placed on the queue. * * @param xQueue A handle to the queue to be deleted. * - * \page vQueueDelete vQueueDelete + * \defgroup vQueueDelete vQueueDelete * \ingroup QueueManagement */ -void vQueueDelete( xQueueHandle xQueue ); +void vQueueDelete( QueueHandle_t xQueue ) PRIVILEGED_FUNCTION; /** * queue. h *
- portBASE_TYPE xQueueSendToFrontFromISR(
-										 xQueueHandle xQueue,
+ BaseType_t xQueueSendToFrontFromISR(
+										 QueueHandle_t xQueue,
 										 const void *pvItemToQueue,
-										 portBASE_TYPE *pxHigherPriorityTaskWoken
+										 BaseType_t *pxHigherPriorityTaskWoken
 									  );
  
* @@ -870,7 +999,7 @@ void vQueueDelete( xQueueHandle xQueue ); void vBufferISR( void ) { char cIn; - portBASE_TYPE xHigherPrioritTaskWoken; + BaseType_t xHigherPrioritTaskWoken; // We have not woken a task at the start of the ISR. xHigherPriorityTaskWoken = pdFALSE; @@ -903,10 +1032,10 @@ void vQueueDelete( xQueueHandle xQueue ); /** * queue. h *
- portBASE_TYPE xQueueSendToBackFromISR(
-										 xQueueHandle xQueue,
+ BaseType_t xQueueSendToBackFromISR(
+										 QueueHandle_t xQueue,
 										 const void *pvItemToQueue,
-										 portBASE_TYPE *pxHigherPriorityTaskWoken
+										 BaseType_t *pxHigherPriorityTaskWoken
 									  );
  
* @@ -941,7 +1070,7 @@ void vQueueDelete( xQueueHandle xQueue ); void vBufferISR( void ) { char cIn; - portBASE_TYPE xHigherPriorityTaskWoken; + BaseType_t xHigherPriorityTaskWoken; // We have not woken a task at the start of the ISR. xHigherPriorityTaskWoken = pdFALSE; @@ -973,10 +1102,97 @@ void vQueueDelete( xQueueHandle xQueue ); /** * queue. h *
- portBASE_TYPE xQueueSendFromISR(
-									 xQueueHandle xQueue,
+ BaseType_t xQueueOverwriteFromISR(
+							  QueueHandle_t xQueue,
+							  const void * pvItemToQueue,
+							  BaseType_t *pxHigherPriorityTaskWoken
+						 );
+ * 
+ * + * A version of xQueueOverwrite() that can be used in an interrupt service + * routine (ISR). + * + * Only for use with queues that can hold a single item - so the queue is either + * empty or full. + * + * Post an item on a queue. If the queue is already full then overwrite the + * value held in the queue. The item is queued by copy, not by reference. + * + * @param xQueue The handle to the queue on which the item is to be posted. + * + * @param pvItemToQueue A pointer to the item that is to be placed on the + * queue. The size of the items the queue will hold was defined when the + * queue was created, so this many bytes will be copied from pvItemToQueue + * into the queue storage area. + * + * @param pxHigherPriorityTaskWoken xQueueOverwriteFromISR() will set + * *pxHigherPriorityTaskWoken to pdTRUE if sending to the queue caused a task + * to unblock, and the unblocked task has a priority higher than the currently + * running task. If xQueueOverwriteFromISR() sets this value to pdTRUE then + * a context switch should be requested before the interrupt is exited. + * + * @return xQueueOverwriteFromISR() is a macro that calls + * xQueueGenericSendFromISR(), and therefore has the same return values as + * xQueueSendToFrontFromISR(). However, pdPASS is the only value that can be + * returned because xQueueOverwriteFromISR() will write to the queue even when + * the queue is already full. + * + * Example usage: +
+
+ QueueHandle_t xQueue;
+
+ void vFunction( void *pvParameters )
+ {
+ 	// Create a queue to hold one uint32_t value.  It is strongly
+	// recommended *not* to use xQueueOverwriteFromISR() on queues that can
+	// contain more than one value, and doing so will trigger an assertion
+	// if configASSERT() is defined.
+	xQueue = xQueueCreate( 1, sizeof( uint32_t ) );
+}
+
+void vAnInterruptHandler( void )
+{
+// xHigherPriorityTaskWoken must be set to pdFALSE before it is used.
+BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+uint32_t ulVarToSend, ulValReceived;
+
+	// Write the value 10 to the queue using xQueueOverwriteFromISR().
+	ulVarToSend = 10;
+	xQueueOverwriteFromISR( xQueue, &ulVarToSend, &xHigherPriorityTaskWoken );
+
+	// The queue is full, but calling xQueueOverwriteFromISR() again will still
+	// pass because the value held in the queue will be overwritten with the
+	// new value.
+	ulVarToSend = 100;
+	xQueueOverwriteFromISR( xQueue, &ulVarToSend, &xHigherPriorityTaskWoken );
+
+	// Reading from the queue will now return 100.
+
+	// ...
+
+	if( xHigherPrioritytaskWoken == pdTRUE )
+	{
+		// Writing to the queue caused a task to unblock and the unblocked task
+		// has a priority higher than or equal to the priority of the currently
+		// executing task (the task this interrupt interrupted).  Perform a context
+		// switch so this interrupt returns directly to the unblocked task.
+		portYIELD_FROM_ISR(); // or portEND_SWITCHING_ISR() depending on the port.
+	}
+}
+ 
+ * \defgroup xQueueOverwriteFromISR xQueueOverwriteFromISR + * \ingroup QueueManagement + */ +#define xQueueOverwriteFromISR( xQueue, pvItemToQueue, pxHigherPriorityTaskWoken ) xQueueGenericSendFromISR( ( xQueue ), ( pvItemToQueue ), ( pxHigherPriorityTaskWoken ), queueOVERWRITE ) + +/** + * queue. h + *
+ BaseType_t xQueueSendFromISR(
+									 QueueHandle_t xQueue,
 									 const void *pvItemToQueue,
-									 portBASE_TYPE *pxHigherPriorityTaskWoken
+									 BaseType_t *pxHigherPriorityTaskWoken
 								);
  
* @@ -1014,7 +1230,7 @@ void vQueueDelete( xQueueHandle xQueue ); void vBufferISR( void ) { char cIn; - portBASE_TYPE xHigherPriorityTaskWoken; + BaseType_t xHigherPriorityTaskWoken; // We have not woken a task at the start of the ISR. xHigherPriorityTaskWoken = pdFALSE; @@ -1034,7 +1250,7 @@ void vQueueDelete( xQueueHandle xQueue ); if( xHigherPriorityTaskWoken ) { // Actual macro used here is port specific. - taskYIELD_FROM_ISR (); + portYIELD_FROM_ISR (); } }
@@ -1047,17 +1263,18 @@ void vQueueDelete( xQueueHandle xQueue ); /** * queue. h *
- portBASE_TYPE xQueueGenericSendFromISR(
-										   xQueueHandle		xQueue,
+ BaseType_t xQueueGenericSendFromISR(
+										   QueueHandle_t		xQueue,
 										   const	void	*pvItemToQueue,
-										   portBASE_TYPE	*pxHigherPriorityTaskWoken,
-										   portBASE_TYPE	xCopyPosition
+										   BaseType_t	*pxHigherPriorityTaskWoken,
+										   BaseType_t	xCopyPosition
 									   );
  
* * It is preferred that the macros xQueueSendFromISR(), * xQueueSendToFrontFromISR() and xQueueSendToBackFromISR() be used in place - * of calling this function directly. + * of calling this function directly. xQueueGiveFromISR() is an + * equivalent for use by semaphores that don't actually copy any data. * * Post an item on a queue. It is safe to use this function from within an * interrupt service routine. @@ -1092,7 +1309,7 @@ void vQueueDelete( xQueueHandle xQueue ); void vBufferISR( void ) { char cIn; - portBASE_TYPE xHigherPriorityTaskWokenByPost; + BaseType_t xHigherPriorityTaskWokenByPost; // We have not woken a task at the start of the ISR. xHigherPriorityTaskWokenByPost = pdFALSE; @@ -1120,15 +1337,16 @@ void vQueueDelete( xQueueHandle xQueue ); * \defgroup xQueueSendFromISR xQueueSendFromISR * \ingroup QueueManagement */ -signed portBASE_TYPE xQueueGenericSendFromISR( xQueueHandle xQueue, const void * const pvItemToQueue, signed portBASE_TYPE *pxHigherPriorityTaskWoken, portBASE_TYPE xCopyPosition ); +BaseType_t xQueueGenericSendFromISR( QueueHandle_t xQueue, const void * const pvItemToQueue, BaseType_t * const pxHigherPriorityTaskWoken, const BaseType_t xCopyPosition ) PRIVILEGED_FUNCTION; +BaseType_t xQueueGiveFromISR( QueueHandle_t xQueue, BaseType_t * const pxHigherPriorityTaskWoken ) PRIVILEGED_FUNCTION; /** * queue. h *
- portBASE_TYPE xQueueReceiveFromISR(
-									   xQueueHandle	xQueue,
+ BaseType_t xQueueReceiveFromISR(
+									   QueueHandle_t	xQueue,
 									   void	*pvBuffer,
-									   portBASE_TYPE *pxTaskWoken
+									   BaseType_t *pxTaskWoken
 								   );
  * 
* @@ -1152,13 +1370,13 @@ signed portBASE_TYPE xQueueGenericSendFromISR( xQueueHandle xQueue, const void * * Example usage:
 
- xQueueHandle xQueue;
+ QueueHandle_t xQueue;
 
  // Function to create a queue and post some values.
  void vAFunction( void *pvParameters )
  {
  char cValueToPost;
- const portTickType xBlockTime = ( portTickType )0xff;
+ const TickType_t xTicksToWait = ( TickType_t )0xff;
 
 	// Create a queue capable of containing 10 characters.
 	xQueue = xQueueCreate( 10, sizeof( char ) );
@@ -1170,23 +1388,23 @@ signed portBASE_TYPE xQueueGenericSendFromISR( xQueueHandle xQueue, const void *
 	// ...
 
 	// Post some characters that will be used within an ISR.  If the queue
-	// is full then this task will block for xBlockTime ticks.
+	// is full then this task will block for xTicksToWait ticks.
 	cValueToPost = 'a';
-	xQueueSend( xQueue, ( void * ) &cValueToPost, xBlockTime );
+	xQueueSend( xQueue, ( void * ) &cValueToPost, xTicksToWait );
 	cValueToPost = 'b';
-	xQueueSend( xQueue, ( void * ) &cValueToPost, xBlockTime );
+	xQueueSend( xQueue, ( void * ) &cValueToPost, xTicksToWait );
 
 	// ... keep posting characters ... this task may block when the queue
 	// becomes full.
 
 	cValueToPost = 'c';
-	xQueueSend( xQueue, ( void * ) &cValueToPost, xBlockTime );
+	xQueueSend( xQueue, ( void * ) &cValueToPost, xTicksToWait );
  }
 
  // ISR that outputs all the characters received on the queue.
  void vISR_Routine( void )
  {
- portBASE_TYPE xTaskWokenByReceive = pdFALSE;
+ BaseType_t xTaskWokenByReceive = pdFALSE;
  char cRxedChar;
 
 	while( xQueueReceiveFromISR( xQueue, ( void * ) &cRxedChar, &xTaskWokenByReceive) )
@@ -1209,15 +1427,15 @@ signed portBASE_TYPE xQueueGenericSendFromISR( xQueueHandle xQueue, const void *
  * \defgroup xQueueReceiveFromISR xQueueReceiveFromISR
  * \ingroup QueueManagement
  */
-signed portBASE_TYPE xQueueReceiveFromISR( xQueueHandle xQueue, void * const pvBuffer, signed portBASE_TYPE *pxHigherPriorityTaskWoken );
+BaseType_t xQueueReceiveFromISR( QueueHandle_t xQueue, void * const pvBuffer, BaseType_t * const pxHigherPriorityTaskWoken ) PRIVILEGED_FUNCTION;
 
 /*
  * Utilities to query queues that are safe to use from an ISR.  These utilities
  * should be used only from witin an ISR, or within a critical section.
  */
-signed portBASE_TYPE xQueueIsQueueEmptyFromISR( const xQueueHandle xQueue );
-signed portBASE_TYPE xQueueIsQueueFullFromISR( const xQueueHandle xQueue );
-unsigned portBASE_TYPE uxQueueMessagesWaitingFromISR( const xQueueHandle xQueue );
+BaseType_t xQueueIsQueueEmptyFromISR( const QueueHandle_t xQueue ) PRIVILEGED_FUNCTION;
+BaseType_t xQueueIsQueueFullFromISR( const QueueHandle_t xQueue ) PRIVILEGED_FUNCTION;
+UBaseType_t uxQueueMessagesWaitingFromISR( const QueueHandle_t xQueue ) PRIVILEGED_FUNCTION;
 
 
 /*
@@ -1234,8 +1452,8 @@ unsigned portBASE_TYPE uxQueueMessagesWaitingFromISR( const xQueueHandle xQueue
  * responsiveness to gain execution speed, whereas the fully featured API
  * sacrifices execution speed to ensure better interrupt responsiveness.
  */
-signed portBASE_TYPE xQueueAltGenericSend( xQueueHandle xQueue, const void * const pvItemToQueue, portTickType xTicksToWait, portBASE_TYPE xCopyPosition );
-signed portBASE_TYPE xQueueAltGenericReceive( xQueueHandle xQueue, void * const pvBuffer, portTickType xTicksToWait, portBASE_TYPE xJustPeeking );
+BaseType_t xQueueAltGenericSend( QueueHandle_t xQueue, const void * const pvItemToQueue, TickType_t xTicksToWait, BaseType_t xCopyPosition ) PRIVILEGED_FUNCTION;
+BaseType_t xQueueAltGenericReceive( QueueHandle_t xQueue, void * const pvBuffer, TickType_t xTicksToWait, BaseType_t xJustPeeking ) PRIVILEGED_FUNCTION;
 #define xQueueAltSendToFront( xQueue, pvItemToQueue, xTicksToWait ) xQueueAltGenericSend( ( xQueue ), ( pvItemToQueue ), ( xTicksToWait ), queueSEND_TO_FRONT )
 #define xQueueAltSendToBack( xQueue, pvItemToQueue, xTicksToWait ) xQueueAltGenericSend( ( xQueue ), ( pvItemToQueue ), ( xTicksToWait ), queueSEND_TO_BACK )
 #define xQueueAltReceive( xQueue, pvBuffer, xTicksToWait ) xQueueAltGenericReceive( ( xQueue ), ( pvBuffer ), ( xTicksToWait ), pdFALSE )
@@ -1250,32 +1468,30 @@ signed portBASE_TYPE xQueueAltGenericReceive( xQueueHandle xQueue, void * const
  * should not be called directly from application code.  Instead use the macro
  * wrappers defined within croutine.h.
  */
-signed portBASE_TYPE xQueueCRSendFromISR( xQueueHandle xQueue, const void *pvItemToQueue, signed portBASE_TYPE xCoRoutinePreviouslyWoken );
-signed portBASE_TYPE xQueueCRReceiveFromISR( xQueueHandle xQueue, void *pvBuffer, signed portBASE_TYPE *pxTaskWoken );
-signed portBASE_TYPE xQueueCRSend( xQueueHandle xQueue, const void *pvItemToQueue, portTickType xTicksToWait );
-signed portBASE_TYPE xQueueCRReceive( xQueueHandle xQueue, void *pvBuffer, portTickType xTicksToWait );
+BaseType_t xQueueCRSendFromISR( QueueHandle_t xQueue, const void *pvItemToQueue, BaseType_t xCoRoutinePreviouslyWoken );
+BaseType_t xQueueCRReceiveFromISR( QueueHandle_t xQueue, void *pvBuffer, BaseType_t *pxTaskWoken );
+BaseType_t xQueueCRSend( QueueHandle_t xQueue, const void *pvItemToQueue, TickType_t xTicksToWait );
+BaseType_t xQueueCRReceive( QueueHandle_t xQueue, void *pvBuffer, TickType_t xTicksToWait );
 
 /*
  * For internal use only.  Use xSemaphoreCreateMutex(),
  * xSemaphoreCreateCounting() or xSemaphoreGetMutexHolder() instead of calling
  * these functions directly.
  */
-xQueueHandle xQueueCreateMutex( unsigned char ucQueueType );
-xQueueHandle xQueueCreateCountingSemaphore( unsigned portBASE_TYPE uxCountValue, unsigned portBASE_TYPE uxInitialCount );
-void* xQueueGetMutexHolder( xQueueHandle xSemaphore );
+QueueHandle_t xQueueCreateMutex( const uint8_t ucQueueType ) PRIVILEGED_FUNCTION;
+QueueHandle_t xQueueCreateCountingSemaphore( const UBaseType_t uxMaxCount, const UBaseType_t uxInitialCount ) PRIVILEGED_FUNCTION;
+void* xQueueGetMutexHolder( QueueHandle_t xSemaphore ) PRIVILEGED_FUNCTION;
 
 /*
  * For internal use only.  Use xSemaphoreTakeMutexRecursive() or
  * xSemaphoreGiveMutexRecursive() instead of calling these functions directly.
  */
-portBASE_TYPE xQueueTakeMutexRecursive( xQueueHandle xMutex, portTickType xBlockTime );
-portBASE_TYPE xQueueGiveMutexRecursive( xQueueHandle pxMutex );
+BaseType_t xQueueTakeMutexRecursive( QueueHandle_t xMutex, TickType_t xTicksToWait ) PRIVILEGED_FUNCTION;
+BaseType_t xQueueGiveMutexRecursive( QueueHandle_t pxMutex ) PRIVILEGED_FUNCTION;
 
 /*
- * Reset a queue back to its original empty state.  pdPASS is returned if the
- * queue is successfully reset.  pdFAIL is returned if the queue could not be
- * reset because there are tasks blocked on the queue waiting to either
- * receive from the queue or send to the queue.
+ * Reset a queue back to its original empty state.  The return value is now
+ * obsolete and is always set to pdPASS.
  */
 #define xQueueReset( xQueue ) xQueueGenericReset( xQueue, pdFALSE )
 
@@ -1297,17 +1513,33 @@ portBASE_TYPE xQueueGiveMutexRecursive( xQueueHandle pxMutex );
  * handles can also be passed in here.
  *
  * @param pcName The name to be associated with the handle.  This is the
- * name that the kernel aware debugger will display.
+ * name that the kernel aware debugger will display.  The queue registry only
+ * stores a pointer to the string - so the string must be persistent (global or
+ * preferably in ROM/Flash), not on the stack.
+ */
+#if configQUEUE_REGISTRY_SIZE > 0
+	void vQueueAddToRegistry( QueueHandle_t xQueue, const char *pcName ) PRIVILEGED_FUNCTION; /*lint !e971 Unqualified char types are allowed for strings and single characters only. */
+#endif
+
+/*
+ * The registry is provided as a means for kernel aware debuggers to
+ * locate queues, semaphores and mutexes.  Call vQueueAddToRegistry() add
+ * a queue, semaphore or mutex handle to the registry if you want the handle
+ * to be available to a kernel aware debugger, and vQueueUnregisterQueue() to
+ * remove the queue, semaphore or mutex from the register.  If you are not using
+ * a kernel aware debugger then this function can be ignored.
+ *
+ * @param xQueue The handle of the queue being removed from the registry.
  */
-#if configQUEUE_REGISTRY_SIZE > 0U
-	void vQueueAddToRegistry( xQueueHandle xQueue, signed char *pcName );
+#if configQUEUE_REGISTRY_SIZE > 0
+	void vQueueUnregisterQueue( QueueHandle_t xQueue ) PRIVILEGED_FUNCTION;
 #endif
 
 /*
  * Generic version of the queue creation function, which is in turn called by
  * any queue, semaphore or mutex creation function or macro.
  */
-xQueueHandle xQueueGenericCreate( unsigned portBASE_TYPE uxQueueLength, unsigned portBASE_TYPE uxItemSize, unsigned char ucQueueType );
+QueueHandle_t xQueueGenericCreate( const UBaseType_t uxQueueLength, const UBaseType_t uxItemSize, const uint8_t ucQueueType ) PRIVILEGED_FUNCTION;
 
 /*
  * Queue sets provide a mechanism to allow a task to block (pend) on a read
@@ -1324,7 +1556,7 @@ xQueueHandle xQueueGenericCreate( unsigned portBASE_TYPE uxQueueLength, unsigned
  * semaphore take operation would be successful.
  *
  * Note 1:  See the documentation on http://wwwFreeRTOS.org/RTOS-queue-sets.html
- * for reasons why queue sets are very rarely needed in practice as there are 
+ * for reasons why queue sets are very rarely needed in practice as there are
  * simpler methods of blocking on multiple objects.
  *
  * Note 2:  Blocking on a queue set that contains a mutex will not cause the
@@ -1357,7 +1589,7 @@ xQueueHandle xQueueGenericCreate( unsigned portBASE_TYPE uxQueueLength, unsigned
  * @return If the queue set is created successfully then a handle to the created
  * queue set is returned.  Otherwise NULL is returned.
  */
-xQueueSetHandle xQueueCreateSet( unsigned portBASE_TYPE uxEventQueueLength );
+QueueSetHandle_t xQueueCreateSet( const UBaseType_t uxEventQueueLength ) PRIVILEGED_FUNCTION;
 
 /*
  * Adds a queue or semaphore to a queue set that was previously created by a
@@ -1371,7 +1603,7 @@ xQueueSetHandle xQueueCreateSet( unsigned portBASE_TYPE uxEventQueueLength );
  * a call to xQueueSelectFromSet() has first returned a handle to that set member.
  *
  * @param xQueueOrSemaphore The handle of the queue or semaphore being added to
- * the queue set (cast to an xQueueSetMemberHandle type).
+ * the queue set (cast to an QueueSetMemberHandle_t type).
  *
  * @param xQueueSet The handle of the queue set to which the queue or semaphore
  * is being added.
@@ -1381,17 +1613,17 @@ xQueueSetHandle xQueueCreateSet( unsigned portBASE_TYPE uxEventQueueLength );
  * queue set because it is already a member of a different queue set then pdFAIL
  * is returned.
  */
-portBASE_TYPE xQueueAddToSet( xQueueSetMemberHandle xQueueOrSemaphore, xQueueSetHandle xQueueSet );
+BaseType_t xQueueAddToSet( QueueSetMemberHandle_t xQueueOrSemaphore, QueueSetHandle_t xQueueSet ) PRIVILEGED_FUNCTION;
 
 /*
- * Removes a queue or semaphore from a queue set.  A queue or semaphore can only 
+ * Removes a queue or semaphore from a queue set.  A queue or semaphore can only
  * be removed from a set if the queue or semaphore is empty.
  *
  * See FreeRTOS/Source/Demo/Common/Minimal/QueueSet.c for an example using this
  * function.
  *
  * @param xQueueOrSemaphore The handle of the queue or semaphore being removed
- * from the queue set (cast to an xQueueSetMemberHandle type).
+ * from the queue set (cast to an QueueSetMemberHandle_t type).
  *
  * @param xQueueSet The handle of the queue set in which the queue or semaphore
  * is included.
@@ -1400,7 +1632,7 @@ portBASE_TYPE xQueueAddToSet( xQueueSetMemberHandle xQueueOrSemaphore, xQueueSet
  * then pdPASS is returned.  If the queue was not in the queue set, or the
  * queue (or semaphore) was not empty, then pdFAIL is returned.
  */
-portBASE_TYPE xQueueRemoveFromSet( xQueueSetMemberHandle xQueueOrSemaphore, xQueueSetHandle xQueueSet );
+BaseType_t xQueueRemoveFromSet( QueueSetMemberHandle_t xQueueOrSemaphore, QueueSetHandle_t xQueueSet ) PRIVILEGED_FUNCTION;
 
 /*
  * xQueueSelectFromSet() selects from the members of a queue set a queue or
@@ -1413,7 +1645,7 @@ portBASE_TYPE xQueueRemoveFromSet( xQueueSetMemberHandle xQueueOrSemaphore, xQue
  * function.
  *
  * Note 1:  See the documentation on http://wwwFreeRTOS.org/RTOS-queue-sets.html
- * for reasons why queue sets are very rarely needed in practice as there are 
+ * for reasons why queue sets are very rarely needed in practice as there are
  * simpler methods of blocking on multiple objects.
  *
  * Note 2:  Blocking on a queue set that contains a mutex will not cause the
@@ -1425,29 +1657,30 @@ portBASE_TYPE xQueueRemoveFromSet( xQueueSetMemberHandle xQueueOrSemaphore, xQue
  *
  * @param xQueueSet The queue set on which the task will (potentially) block.
  *
- * @param xBlockTimeTicks The maximum time, in ticks, that the calling task will
+ * @param xTicksToWait The maximum time, in ticks, that the calling task will
  * remain in the Blocked state (with other tasks executing) to wait for a member
  * of the queue set to be ready for a successful queue read or semaphore take
  * operation.
  *
  * @return xQueueSelectFromSet() will return the handle of a queue (cast to
- * a xQueueSetMemberHandle type) contained in the queue set that contains data,
- * or the handle of a semaphore (cast to a xQueueSetMemberHandle type) contained
+ * a QueueSetMemberHandle_t type) contained in the queue set that contains data,
+ * or the handle of a semaphore (cast to a QueueSetMemberHandle_t type) contained
  * in the queue set that is available, or NULL if no such queue or semaphore
  * exists before before the specified block time expires.
  */
-xQueueSetMemberHandle xQueueSelectFromSet( xQueueSetHandle xQueueSet, portTickType xBlockTimeTicks );
+QueueSetMemberHandle_t xQueueSelectFromSet( QueueSetHandle_t xQueueSet, const TickType_t xTicksToWait ) PRIVILEGED_FUNCTION;
 
 /*
  * A version of xQueueSelectFromSet() that can be used from an ISR.
  */
-xQueueSetMemberHandle xQueueSelectFromSetFromISR( xQueueSetHandle xQueueSet );
+QueueSetMemberHandle_t xQueueSelectFromSetFromISR( QueueSetHandle_t xQueueSet ) PRIVILEGED_FUNCTION;
 
 /* Not public API functions. */
-void vQueueWaitForMessageRestricted( xQueueHandle xQueue, portTickType xTicksToWait );
-portBASE_TYPE xQueueGenericReset( xQueueHandle xQueue, portBASE_TYPE xNewQueue );
-void vQueueSetQueueNumber( xQueueHandle xQueue, unsigned char ucQueueNumber ) PRIVILEGED_FUNCTION;
-unsigned char ucQueueGetQueueType( xQueueHandle xQueue ) PRIVILEGED_FUNCTION;
+void vQueueWaitForMessageRestricted( QueueHandle_t xQueue, TickType_t xTicksToWait, const BaseType_t xWaitIndefinitely ) PRIVILEGED_FUNCTION;
+BaseType_t xQueueGenericReset( QueueHandle_t xQueue, BaseType_t xNewQueue ) PRIVILEGED_FUNCTION;
+void vQueueSetQueueNumber( QueueHandle_t xQueue, UBaseType_t uxQueueNumber ) PRIVILEGED_FUNCTION;
+UBaseType_t uxQueueGetQueueNumber( QueueHandle_t xQueue ) PRIVILEGED_FUNCTION;
+uint8_t ucQueueGetQueueType( QueueHandle_t xQueue ) PRIVILEGED_FUNCTION;
 
 
 #ifdef __cplusplus
diff --git a/lib/FreeRTOS/include/semphr.h b/lib/FreeRTOS/include/semphr.h
index 7ac1ab012c..ab00d09d76 100644
--- a/lib/FreeRTOS/include/semphr.h
+++ b/lib/FreeRTOS/include/semphr.h
@@ -1,75 +1,70 @@
 /*
-    FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd.
-
-    FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME.  PLEASE VISIT
-    http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION.
-
-    ***************************************************************************
-     *                                                                       *
-     *    FreeRTOS tutorial books are available in pdf and paperback.        *
-     *    Complete, revised, and edited pdf reference manuals are also       *
-     *    available.                                                         *
-     *                                                                       *
-     *    Purchasing FreeRTOS documentation will not only help you, by       *
-     *    ensuring you get running as quickly as possible and with an        *
-     *    in-depth knowledge of how to use FreeRTOS, it will also help       *
-     *    the FreeRTOS project to continue with its mission of providing     *
-     *    professional grade, cross platform, de facto standard solutions    *
-     *    for microcontrollers - completely free of charge!                  *
-     *                                                                       *
-     *    >>> See http://www.FreeRTOS.org/Documentation for details. <<<     *
-     *                                                                       *
-     *    Thank you for using FreeRTOS, and thank you for your support!      *
-     *                                                                       *
-    ***************************************************************************
+    FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd.
+    All rights reserved
 
+    VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION.
 
     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.
+    Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception.
 
-    >>>>>>NOTE<<<<<< The modification 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.
+    ***************************************************************************
+    >>!   NOTE: The modification 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 itcan be
-    viewed here: http://www.freertos.org/a00114.html and also obtained by
-    writing to Real Time Engineers Ltd., contact details for whom are available
-    on the FreeRTOS WEB site.
-
-    1 tab == 4 spaces!
+    FOR A PARTICULAR PURPOSE.  Full license text is available on the following
+    link: http://www.freertos.org/a00114.html
 
     ***************************************************************************
      *                                                                       *
-     *    Having a problem?  Start by reading the FAQ "My application does   *
-     *    not run, what could be wrong?"                                     *
+     *    FreeRTOS provides completely free yet professionally developed,    *
+     *    robust, strictly quality controlled, supported, and cross          *
+     *    platform software that is more than just the market leader, it     *
+     *    is the industry's de facto standard.                               *
      *                                                                       *
-     *    http://www.FreeRTOS.org/FAQHelp.html                               *
+     *    Help yourself get started quickly while simultaneously helping     *
+     *    to support the FreeRTOS project by purchasing a FreeRTOS           *
+     *    tutorial book, reference manual, or both:                          *
+     *    http://www.FreeRTOS.org/Documentation                              *
      *                                                                       *
     ***************************************************************************
 
+    http://www.FreeRTOS.org/FAQHelp.html - Having a problem?  Start by reading
+    the FAQ page "My application does not run, what could be wrong?".  Have you
+    defined configASSERT()?
+
+    http://www.FreeRTOS.org/support - In return for receiving this top quality
+    embedded software for free we request you assist our global community by
+    participating in the support forum.
 
-    http://www.FreeRTOS.org - Documentation, books, training, latest versions, 
-    license and Real Time Engineers Ltd. contact details.
+    http://www.FreeRTOS.org/training - Investing in training allows your team to
+    be as productive as possible as early as possible.  Now you can receive
+    FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers
+    Ltd, and the world's leading authority on the world's leading RTOS.
 
     http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products,
-    including FreeRTOS+Trace - an indispensable productivity tool, and our new
-    fully thread aware and reentrant UDP/IP stack.
-
-    http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High 
-    Integrity Systems, who sell the code with commercial support, 
-    indemnification and middleware, under the OpenRTOS brand.
-    
-    http://www.SafeRTOS.com - High Integrity Systems also provide a safety 
-    engineered and independently SIL3 certified version for use in safety and 
+    including FreeRTOS+Trace - an indispensable productivity tool, a DOS
+    compatible FAT file system, and our tiny thread aware UDP/IP stack.
+
+    http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate.
+    Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS.
+
+    http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High
+    Integrity Systems ltd. to sell under the OpenRTOS brand.  Low cost OpenRTOS
+    licenses offer ticketed support, indemnification and commercial middleware.
+
+    http://www.SafeRTOS.com - High Integrity Systems also provide a safety
+    engineered and independently SIL3 certified version for use in safety and
     mission critical applications that require provable dependability.
+
+    1 tab == 4 spaces!
 */
 
 #ifndef SEMAPHORE_H
@@ -81,16 +76,23 @@
 
 #include "queue.h"
 
-typedef xQueueHandle xSemaphoreHandle;
+typedef QueueHandle_t SemaphoreHandle_t;
 
-#define semBINARY_SEMAPHORE_QUEUE_LENGTH	( ( unsigned char ) 1U )
-#define semSEMAPHORE_QUEUE_ITEM_LENGTH		( ( unsigned char ) 0U )
-#define semGIVE_BLOCK_TIME					( ( portTickType ) 0U )
+#define semBINARY_SEMAPHORE_QUEUE_LENGTH	( ( uint8_t ) 1U )
+#define semSEMAPHORE_QUEUE_ITEM_LENGTH		( ( uint8_t ) 0U )
+#define semGIVE_BLOCK_TIME					( ( TickType_t ) 0U )
 
 
 /**
  * semphr. h
- * 
vSemaphoreCreateBinary( xSemaphoreHandle xSemaphore )
+ *
vSemaphoreCreateBinary( SemaphoreHandle_t xSemaphore )
+ * + * This old vSemaphoreCreateBinary() macro is now deprecated in favour of the + * xSemaphoreCreateBinary() function. Note that binary semaphores created using + * the vSemaphoreCreateBinary() macro are created in a state such that the + * first call to 'take' the semaphore would pass, whereas binary semaphores + * created using xSemaphoreCreateBinary() are created in a state such that the + * the semaphore must first be 'given' before it can be 'taken'. * * Macro that implements a semaphore by using the existing queue mechanism. * The queue length is 1 as this is a binary semaphore. The data size is 0 @@ -104,11 +106,11 @@ typedef xQueueHandle xSemaphoreHandle; * semaphore does not use a priority inheritance mechanism. For an alternative * that does use priority inheritance see xSemaphoreCreateMutex(). * - * @param xSemaphore Handle to the created semaphore. Should be of type xSemaphoreHandle. + * @param xSemaphore Handle to the created semaphore. Should be of type SemaphoreHandle_t. * * Example usage:
- xSemaphoreHandle xSemaphore;
+ SemaphoreHandle_t xSemaphore = NULL;
 
  void vATask( void * pvParameters )
  {
@@ -119,27 +121,74 @@ typedef xQueueHandle xSemaphoreHandle;
     if( xSemaphore != NULL )
     {
         // The semaphore was created successfully.
-        // The semaphore can now be used.  
+        // The semaphore can now be used.
     }
  }
  
* \defgroup vSemaphoreCreateBinary vSemaphoreCreateBinary * \ingroup Semaphores */ -#define vSemaphoreCreateBinary( xSemaphore ) \ - { \ - ( xSemaphore ) = xQueueGenericCreate( ( unsigned portBASE_TYPE ) 1, semSEMAPHORE_QUEUE_ITEM_LENGTH, queueQUEUE_TYPE_BINARY_SEMAPHORE ); \ - if( ( xSemaphore ) != NULL ) \ - { \ - xSemaphoreGive( ( xSemaphore ) ); \ - } \ +#define vSemaphoreCreateBinary( xSemaphore ) \ + { \ + ( xSemaphore ) = xQueueGenericCreate( ( UBaseType_t ) 1, semSEMAPHORE_QUEUE_ITEM_LENGTH, queueQUEUE_TYPE_BINARY_SEMAPHORE ); \ + if( ( xSemaphore ) != NULL ) \ + { \ + ( void ) xSemaphoreGive( ( xSemaphore ) ); \ + } \ } /** * semphr. h - *
xSemaphoreTake( 
- *                   xSemaphoreHandle xSemaphore, 
- *                   portTickType xBlockTime 
+ * 
SemaphoreHandle_t xSemaphoreCreateBinary( void )
+ * + * The old vSemaphoreCreateBinary() macro is now deprecated in favour of this + * xSemaphoreCreateBinary() function. Note that binary semaphores created using + * the vSemaphoreCreateBinary() macro are created in a state such that the + * first call to 'take' the semaphore would pass, whereas binary semaphores + * created using xSemaphoreCreateBinary() are created in a state such that the + * the semaphore must first be 'given' before it can be 'taken'. + * + * Function that creates a semaphore by using the existing queue mechanism. + * The queue length is 1 as this is a binary semaphore. The data size is 0 + * as nothing is actually stored - all that is important is whether the queue is + * empty or full (the binary semaphore is available or not). + * + * This type of semaphore can be used for pure synchronisation between tasks or + * between an interrupt and a task. The semaphore need not be given back once + * obtained, so one task/interrupt can continuously 'give' the semaphore while + * another continuously 'takes' the semaphore. For this reason this type of + * semaphore does not use a priority inheritance mechanism. For an alternative + * that does use priority inheritance see xSemaphoreCreateMutex(). + * + * @return Handle to the created semaphore. + * + * Example usage: +
+ SemaphoreHandle_t xSemaphore = NULL;
+
+ void vATask( void * pvParameters )
+ {
+    // Semaphore cannot be used before a call to vSemaphoreCreateBinary ().
+    // This is a macro so pass the variable in directly.
+    xSemaphore = xSemaphoreCreateBinary();
+
+    if( xSemaphore != NULL )
+    {
+        // The semaphore was created successfully.
+        // The semaphore can now be used.
+    }
+ }
+ 
+ * \defgroup vSemaphoreCreateBinary vSemaphoreCreateBinary + * \ingroup Semaphores + */ +#define xSemaphoreCreateBinary() xQueueGenericCreate( ( UBaseType_t ) 1, semSEMAPHORE_QUEUE_ITEM_LENGTH, queueQUEUE_TYPE_BINARY_SEMAPHORE ) + +/** + * semphr. h + *
xSemaphoreTake(
+ *                   SemaphoreHandle_t xSemaphore,
+ *                   TickType_t xBlockTime
  *               )
* * Macro to obtain a semaphore. The semaphore must have previously been @@ -150,7 +199,7 @@ typedef xQueueHandle xSemaphoreHandle; * the semaphore was created. * * @param xBlockTime The time in ticks to wait for the semaphore to become - * available. The macro portTICK_RATE_MS can be used to convert this to a + * available. The macro portTICK_PERIOD_MS can be used to convert this to a * real time. A block time of zero can be used to poll the semaphore. A block * time of portMAX_DELAY can be used to block indefinitely (provided * INCLUDE_vTaskSuspend is set to 1 in FreeRTOSConfig.h). @@ -160,7 +209,7 @@ typedef xQueueHandle xSemaphoreHandle; * * Example usage:
- xSemaphoreHandle xSemaphore = NULL;
+ SemaphoreHandle_t xSemaphore = NULL;
 
  // A task that creates a semaphore.
  void vATask( void * pvParameters )
@@ -177,15 +226,15 @@ typedef xQueueHandle xSemaphoreHandle;
     if( xSemaphore != NULL )
     {
         // See if we can obtain the semaphore.  If the semaphore is not available
-        // wait 10 ticks to see if it becomes free.	
-        if( xSemaphoreTake( xSemaphore, ( portTickType ) 10 ) == pdTRUE )
+        // wait 10 ticks to see if it becomes free.
+        if( xSemaphoreTake( xSemaphore, ( TickType_t ) 10 ) == pdTRUE )
         {
             // We were able to obtain the semaphore and can now access the
             // shared resource.
 
             // ...
 
-            // We have finished accessing the shared resource.  Release the 
+            // We have finished accessing the shared resource.  Release the
             // semaphore.
             xSemaphoreGive( xSemaphore );
         }
@@ -200,28 +249,28 @@ typedef xQueueHandle xSemaphoreHandle;
  * \defgroup xSemaphoreTake xSemaphoreTake
  * \ingroup Semaphores
  */
-#define xSemaphoreTake( xSemaphore, xBlockTime )		xQueueGenericReceive( ( xQueueHandle ) ( xSemaphore ), NULL, ( xBlockTime ), pdFALSE )
+#define xSemaphoreTake( xSemaphore, xBlockTime )		xQueueGenericReceive( ( QueueHandle_t ) ( xSemaphore ), NULL, ( xBlockTime ), pdFALSE )
 
 /**
  * semphr. h
- * xSemaphoreTakeRecursive( 
- *                          xSemaphoreHandle xMutex, 
- *                          portTickType xBlockTime 
+ * xSemaphoreTakeRecursive(
+ *                          SemaphoreHandle_t xMutex,
+ *                          TickType_t xBlockTime
  *                        )
  *
- * Macro to recursively obtain, or 'take', a mutex type semaphore.  
- * The mutex must have previously been created using a call to 
+ * Macro to recursively obtain, or 'take', a mutex type semaphore.
+ * The mutex must have previously been created using a call to
  * xSemaphoreCreateRecursiveMutex();
- * 
+ *
  * configUSE_RECURSIVE_MUTEXES must be set to 1 in FreeRTOSConfig.h for this
  * macro to be available.
- * 
+ *
  * This macro must not be used on mutexes created using xSemaphoreCreateMutex().
  *
- * A mutex used recursively can be 'taken' repeatedly by the owner. The mutex 
- * doesn't become available again until the owner has called 
- * xSemaphoreGiveRecursive() for each successful 'take' request.  For example, 
- * if a task successfully 'takes' the same mutex 5 times then the mutex will 
+ * A mutex used recursively can be 'taken' repeatedly by the owner. The mutex
+ * doesn't become available again until the owner has called
+ * xSemaphoreGiveRecursive() for each successful 'take' request.  For example,
+ * if a task successfully 'takes' the same mutex 5 times then the mutex will
  * not be available to any other task until it has also  'given' the mutex back
  * exactly five times.
  *
@@ -229,17 +278,17 @@ typedef xQueueHandle xSemaphoreHandle;
  * handle returned by xSemaphoreCreateRecursiveMutex();
  *
  * @param xBlockTime The time in ticks to wait for the semaphore to become
- * available.  The macro portTICK_RATE_MS can be used to convert this to a
+ * available.  The macro portTICK_PERIOD_MS can be used to convert this to a
  * real time.  A block time of zero can be used to poll the semaphore.  If
  * the task already owns the semaphore then xSemaphoreTakeRecursive() will
- * return immediately no matter what the value of xBlockTime. 
+ * return immediately no matter what the value of xBlockTime.
  *
  * @return pdTRUE if the semaphore was obtained.  pdFALSE if xBlockTime
  * expired without the semaphore becoming available.
  *
  * Example usage:
  
- xSemaphoreHandle xMutex = NULL;
+ SemaphoreHandle_t xMutex = NULL;
 
  // A task that creates a mutex.
  void vATask( void * pvParameters )
@@ -256,22 +305,22 @@ typedef xQueueHandle xSemaphoreHandle;
     if( xMutex != NULL )
     {
         // See if we can obtain the mutex.  If the mutex is not available
-        // wait 10 ticks to see if it becomes free.	
-        if( xSemaphoreTakeRecursive( xSemaphore, ( portTickType ) 10 ) == pdTRUE )
+        // wait 10 ticks to see if it becomes free.
+        if( xSemaphoreTakeRecursive( xSemaphore, ( TickType_t ) 10 ) == pdTRUE )
         {
             // We were able to obtain the mutex and can now access the
             // shared resource.
 
             // ...
-            // For some reason due to the nature of the code further calls to 
+            // For some reason due to the nature of the code further calls to
 			// xSemaphoreTakeRecursive() are made on the same mutex.  In real
 			// code these would not be just sequential calls as this would make
 			// no sense.  Instead the calls are likely to be buried inside
 			// a more complex call structure.
-            xSemaphoreTakeRecursive( xMutex, ( portTickType ) 10 );
-            xSemaphoreTakeRecursive( xMutex, ( portTickType ) 10 );
+            xSemaphoreTakeRecursive( xMutex, ( TickType_t ) 10 );
+            xSemaphoreTakeRecursive( xMutex, ( TickType_t ) 10 );
 
-            // The mutex has now been 'taken' three times, so will not be 
+            // The mutex has now been 'taken' three times, so will not be
 			// available to another task until it has also been given back
 			// three times.  Again it is unlikely that real code would have
 			// these calls sequentially, but instead buried in a more complex
@@ -296,23 +345,23 @@ typedef xQueueHandle xSemaphoreHandle;
 #define xSemaphoreTakeRecursive( xMutex, xBlockTime )	xQueueTakeMutexRecursive( ( xMutex ), ( xBlockTime ) )
 
 
-/* 
+/*
  * xSemaphoreAltTake() is an alternative version of xSemaphoreTake().
  *
- * The source code that implements the alternative (Alt) API is much 
- * simpler	because it executes everything from within a critical section.  
- * This is	the approach taken by many other RTOSes, but FreeRTOS.org has the 
- * preferred fully featured API too.  The fully featured API has more 
- * complex	code that takes longer to execute, but makes much less use of 
- * critical sections.  Therefore the alternative API sacrifices interrupt 
+ * The source code that implements the alternative (Alt) API is much
+ * simpler	because it executes everything from within a critical section.
+ * This is	the approach taken by many other RTOSes, but FreeRTOS.org has the
+ * preferred fully featured API too.  The fully featured API has more
+ * complex	code that takes longer to execute, but makes much less use of
+ * critical sections.  Therefore the alternative API sacrifices interrupt
  * responsiveness to gain execution speed, whereas the fully featured API
  * sacrifices execution speed to ensure better interrupt responsiveness.
  */
-#define xSemaphoreAltTake( xSemaphore, xBlockTime )		xQueueAltGenericReceive( ( xQueueHandle ) ( xSemaphore ), NULL, ( xBlockTime ), pdFALSE )
+#define xSemaphoreAltTake( xSemaphore, xBlockTime )		xQueueAltGenericReceive( ( QueueHandle_t ) ( xSemaphore ), NULL, ( xBlockTime ), pdFALSE )
 
 /**
  * semphr. h
- * 
xSemaphoreGive( xSemaphoreHandle xSemaphore )
+ *
xSemaphoreGive( SemaphoreHandle_t xSemaphore )
* * Macro to release a semaphore. The semaphore must have previously been * created with a call to vSemaphoreCreateBinary(), xSemaphoreCreateMutex() or @@ -321,7 +370,7 @@ typedef xQueueHandle xSemaphoreHandle; * This macro must not be used from an ISR. See xSemaphoreGiveFromISR () for * an alternative which can be used from an ISR. * - * This macro must also not be used on semaphores created using + * This macro must also not be used on semaphores created using * xSemaphoreCreateRecursiveMutex(). * * @param xSemaphore A handle to the semaphore being released. This is the @@ -329,12 +378,12 @@ typedef xQueueHandle xSemaphoreHandle; * * @return pdTRUE if the semaphore was released. pdFALSE if an error occurred. * Semaphores are implemented using queues. An error can occur if there is - * no space on the queue to post a message - indicating that the + * no space on the queue to post a message - indicating that the * semaphore was not first obtained correctly. * * Example usage:
- xSemaphoreHandle xSemaphore = NULL;
+ SemaphoreHandle_t xSemaphore = NULL;
 
  void vATask( void * pvParameters )
  {
@@ -351,7 +400,7 @@ typedef xQueueHandle xSemaphoreHandle;
 
         // Obtain the semaphore - don't block if the semaphore is not
         // immediately available.
-        if( xSemaphoreTake( xSemaphore, ( portTickType ) 0 ) )
+        if( xSemaphoreTake( xSemaphore, ( TickType_t ) 0 ) )
         {
             // We now have the semaphore and can access the shared resource.
 
@@ -371,25 +420,25 @@ typedef xQueueHandle xSemaphoreHandle;
  * \defgroup xSemaphoreGive xSemaphoreGive
  * \ingroup Semaphores
  */
-#define xSemaphoreGive( xSemaphore )		xQueueGenericSend( ( xQueueHandle ) ( xSemaphore ), NULL, semGIVE_BLOCK_TIME, queueSEND_TO_BACK )
+#define xSemaphoreGive( xSemaphore )		xQueueGenericSend( ( QueueHandle_t ) ( xSemaphore ), NULL, semGIVE_BLOCK_TIME, queueSEND_TO_BACK )
 
 /**
  * semphr. h
- * 
xSemaphoreGiveRecursive( xSemaphoreHandle xMutex )
+ *
xSemaphoreGiveRecursive( SemaphoreHandle_t xMutex )
* * Macro to recursively release, or 'give', a mutex type semaphore. - * The mutex must have previously been created using a call to + * The mutex must have previously been created using a call to * xSemaphoreCreateRecursiveMutex(); - * + * * configUSE_RECURSIVE_MUTEXES must be set to 1 in FreeRTOSConfig.h for this * macro to be available. * * This macro must not be used on mutexes created using xSemaphoreCreateMutex(). - * - * A mutex used recursively can be 'taken' repeatedly by the owner. The mutex - * doesn't become available again until the owner has called - * xSemaphoreGiveRecursive() for each successful 'take' request. For example, - * if a task successfully 'takes' the same mutex 5 times then the mutex will + * + * A mutex used recursively can be 'taken' repeatedly by the owner. The mutex + * doesn't become available again until the owner has called + * xSemaphoreGiveRecursive() for each successful 'take' request. For example, + * if a task successfully 'takes' the same mutex 5 times then the mutex will * not be available to any other task until it has also 'given' the mutex back * exactly five times. * @@ -400,7 +449,7 @@ typedef xQueueHandle xSemaphoreHandle; * * Example usage:
- xSemaphoreHandle xMutex = NULL;
+ SemaphoreHandle_t xMutex = NULL;
 
  // A task that creates a mutex.
  void vATask( void * pvParameters )
@@ -417,22 +466,22 @@ typedef xQueueHandle xSemaphoreHandle;
     if( xMutex != NULL )
     {
         // See if we can obtain the mutex.  If the mutex is not available
-        // wait 10 ticks to see if it becomes free.	
-        if( xSemaphoreTakeRecursive( xMutex, ( portTickType ) 10 ) == pdTRUE )
+        // wait 10 ticks to see if it becomes free.
+        if( xSemaphoreTakeRecursive( xMutex, ( TickType_t ) 10 ) == pdTRUE )
         {
             // We were able to obtain the mutex and can now access the
             // shared resource.
 
             // ...
-            // For some reason due to the nature of the code further calls to 
+            // For some reason due to the nature of the code further calls to
 			// xSemaphoreTakeRecursive() are made on the same mutex.  In real
 			// code these would not be just sequential calls as this would make
 			// no sense.  Instead the calls are likely to be buried inside
 			// a more complex call structure.
-            xSemaphoreTakeRecursive( xMutex, ( portTickType ) 10 );
-            xSemaphoreTakeRecursive( xMutex, ( portTickType ) 10 );
+            xSemaphoreTakeRecursive( xMutex, ( TickType_t ) 10 );
+            xSemaphoreTakeRecursive( xMutex, ( TickType_t ) 10 );
 
-            // The mutex has now been 'taken' three times, so will not be 
+            // The mutex has now been 'taken' three times, so will not be
 			// available to another task until it has also been given back
 			// three times.  Again it is unlikely that real code would have
 			// these calls sequentially, it would be more likely that the calls
@@ -457,26 +506,26 @@ typedef xQueueHandle xSemaphoreHandle;
  */
 #define xSemaphoreGiveRecursive( xMutex )	xQueueGiveMutexRecursive( ( xMutex ) )
 
-/* 
+/*
  * xSemaphoreAltGive() is an alternative version of xSemaphoreGive().
  *
- * The source code that implements the alternative (Alt) API is much 
- * simpler	because it executes everything from within a critical section.  
- * This is	the approach taken by many other RTOSes, but FreeRTOS.org has the 
- * preferred fully featured API too.  The fully featured API has more 
- * complex	code that takes longer to execute, but makes much less use of 
- * critical sections.  Therefore the alternative API sacrifices interrupt 
+ * The source code that implements the alternative (Alt) API is much
+ * simpler	because it executes everything from within a critical section.
+ * This is	the approach taken by many other RTOSes, but FreeRTOS.org has the
+ * preferred fully featured API too.  The fully featured API has more
+ * complex	code that takes longer to execute, but makes much less use of
+ * critical sections.  Therefore the alternative API sacrifices interrupt
  * responsiveness to gain execution speed, whereas the fully featured API
  * sacrifices execution speed to ensure better interrupt responsiveness.
  */
-#define xSemaphoreAltGive( xSemaphore )		xQueueAltGenericSend( ( xQueueHandle ) ( xSemaphore ), NULL, semGIVE_BLOCK_TIME, queueSEND_TO_BACK )
+#define xSemaphoreAltGive( xSemaphore )		xQueueAltGenericSend( ( QueueHandle_t ) ( xSemaphore ), NULL, semGIVE_BLOCK_TIME, queueSEND_TO_BACK )
 
 /**
  * semphr. h
  * 
- xSemaphoreGiveFromISR( 
-                          xSemaphoreHandle xSemaphore, 
-                          signed portBASE_TYPE *pxHigherPriorityTaskWoken
+ xSemaphoreGiveFromISR(
+                          SemaphoreHandle_t xSemaphore,
+                          BaseType_t *pxHigherPriorityTaskWoken
                       )
* * Macro to release a semaphore. The semaphore must have previously been @@ -502,14 +551,14 @@ typedef xQueueHandle xSemaphoreHandle;
  \#define LONG_TIME 0xffff
  \#define TICKS_TO_WAIT	10
- xSemaphoreHandle xSemaphore = NULL;
+ SemaphoreHandle_t xSemaphore = NULL;
 
  // Repetitive task.
  void vATask( void * pvParameters )
  {
     for( ;; )
     {
-        // We want this task to run every 10 ticks of a timer.  The semaphore 
+        // We want this task to run every 10 ticks of a timer.  The semaphore
         // was created before this task was started.
 
         // Block waiting for the semaphore to become available.
@@ -520,7 +569,7 @@ typedef xQueueHandle xSemaphoreHandle;
             // ...
 
             // We have finished our task.  Return to the top of the loop where
-            // we will block on the semaphore until it is time to execute 
+            // we will block on the semaphore until it is time to execute
             // again.  Note when using the semaphore for synchronisation with an
 			// ISR in this manner there is no need to 'give' the semaphore back.
         }
@@ -530,8 +579,8 @@ typedef xQueueHandle xSemaphoreHandle;
  // Timer ISR
  void vTimerISR( void * pvParameters )
  {
- static unsigned char ucLocalTickCount = 0;
- static signed portBASE_TYPE xHigherPriorityTaskWoken;
+ static uint8_t ucLocalTickCount = 0;
+ static BaseType_t xHigherPriorityTaskWoken;
 
     // A timer tick has occurred.
 
@@ -560,18 +609,18 @@ typedef xQueueHandle xSemaphoreHandle;
  * \defgroup xSemaphoreGiveFromISR xSemaphoreGiveFromISR
  * \ingroup Semaphores
  */
-#define xSemaphoreGiveFromISR( xSemaphore, pxHigherPriorityTaskWoken )			xQueueGenericSendFromISR( ( xQueueHandle ) ( xSemaphore ), NULL, ( pxHigherPriorityTaskWoken ), queueSEND_TO_BACK )
+#define xSemaphoreGiveFromISR( xSemaphore, pxHigherPriorityTaskWoken )	xQueueGiveFromISR( ( QueueHandle_t ) ( xSemaphore ), ( pxHigherPriorityTaskWoken ) )
 
 /**
  * semphr. h
  * 
- xSemaphoreTakeFromISR( 
-                          xSemaphoreHandle xSemaphore, 
-                          signed portBASE_TYPE *pxHigherPriorityTaskWoken
+ xSemaphoreTakeFromISR(
+                          SemaphoreHandle_t xSemaphore,
+                          BaseType_t *pxHigherPriorityTaskWoken
                       )
* - * Macro to take a semaphore from an ISR. The semaphore must have - * previously been created with a call to vSemaphoreCreateBinary() or + * Macro to take a semaphore from an ISR. The semaphore must have + * previously been created with a call to vSemaphoreCreateBinary() or * xSemaphoreCreateCounting(). * * Mutex type semaphores (those created using a call to xSemaphoreCreateMutex()) @@ -591,39 +640,39 @@ typedef xQueueHandle xSemaphoreHandle; * running task. If xSemaphoreTakeFromISR() sets this value to pdTRUE then * a context switch should be requested before the interrupt is exited. * - * @return pdTRUE if the semaphore was successfully taken, otherwise + * @return pdTRUE if the semaphore was successfully taken, otherwise * pdFALSE */ -#define xSemaphoreTakeFromISR( xSemaphore, pxHigherPriorityTaskWoken ) xQueueReceiveFromISR( ( xQueueHandle ) ( xSemaphore ), NULL, ( pxHigherPriorityTaskWoken ) ) +#define xSemaphoreTakeFromISR( xSemaphore, pxHigherPriorityTaskWoken ) xQueueReceiveFromISR( ( QueueHandle_t ) ( xSemaphore ), NULL, ( pxHigherPriorityTaskWoken ) ) /** * semphr. h - *
xSemaphoreHandle xSemaphoreCreateMutex( void )
+ *
SemaphoreHandle_t xSemaphoreCreateMutex( void )
* - * Macro that implements a mutex semaphore by using the existing queue + * Macro that implements a mutex semaphore by using the existing queue * mechanism. * * Mutexes created using this macro can be accessed using the xSemaphoreTake() - * and xSemaphoreGive() macros. The xSemaphoreTakeRecursive() and + * and xSemaphoreGive() macros. The xSemaphoreTakeRecursive() and * xSemaphoreGiveRecursive() macros should not be used. - * - * This type of semaphore uses a priority inheritance mechanism so a task - * 'taking' a semaphore MUST ALWAYS 'give' the semaphore back once the - * semaphore it is no longer required. * - * Mutex type semaphores cannot be used from within interrupt service routines. + * This type of semaphore uses a priority inheritance mechanism so a task + * 'taking' a semaphore MUST ALWAYS 'give' the semaphore back once the + * semaphore it is no longer required. * - * See vSemaphoreCreateBinary() for an alternative implementation that can be - * used for pure synchronisation (where one task or interrupt always 'gives' the - * semaphore and another always 'takes' the semaphore) and from within interrupt + * Mutex type semaphores cannot be used from within interrupt service routines. + * + * See vSemaphoreCreateBinary() for an alternative implementation that can be + * used for pure synchronisation (where one task or interrupt always 'gives' the + * semaphore and another always 'takes' the semaphore) and from within interrupt * service routines. * - * @return xSemaphore Handle to the created mutex semaphore. Should be of type - * xSemaphoreHandle. + * @return xSemaphore Handle to the created mutex semaphore. Should be of type + * SemaphoreHandle_t. * * Example usage:
- xSemaphoreHandle xSemaphore;
+ SemaphoreHandle_t xSemaphore;
 
  void vATask( void * pvParameters )
  {
@@ -634,7 +683,7 @@ typedef xQueueHandle xSemaphoreHandle;
     if( xSemaphore != NULL )
     {
         // The semaphore was created successfully.
-        // The semaphore can now be used.  
+        // The semaphore can now be used.
     }
  }
  
@@ -646,39 +695,39 @@ typedef xQueueHandle xSemaphoreHandle; /** * semphr. h - *
xSemaphoreHandle xSemaphoreCreateRecursiveMutex( void )
+ *
SemaphoreHandle_t xSemaphoreCreateRecursiveMutex( void )
* - * Macro that implements a recursive mutex by using the existing queue + * Macro that implements a recursive mutex by using the existing queue * mechanism. * - * Mutexes created using this macro can be accessed using the - * xSemaphoreTakeRecursive() and xSemaphoreGiveRecursive() macros. The + * Mutexes created using this macro can be accessed using the + * xSemaphoreTakeRecursive() and xSemaphoreGiveRecursive() macros. The * xSemaphoreTake() and xSemaphoreGive() macros should not be used. * - * A mutex used recursively can be 'taken' repeatedly by the owner. The mutex - * doesn't become available again until the owner has called - * xSemaphoreGiveRecursive() for each successful 'take' request. For example, - * if a task successfully 'takes' the same mutex 5 times then the mutex will + * A mutex used recursively can be 'taken' repeatedly by the owner. The mutex + * doesn't become available again until the owner has called + * xSemaphoreGiveRecursive() for each successful 'take' request. For example, + * if a task successfully 'takes' the same mutex 5 times then the mutex will * not be available to any other task until it has also 'given' the mutex back * exactly five times. - * - * This type of semaphore uses a priority inheritance mechanism so a task - * 'taking' a semaphore MUST ALWAYS 'give' the semaphore back once the - * semaphore it is no longer required. * - * Mutex type semaphores cannot be used from within interrupt service routines. + * This type of semaphore uses a priority inheritance mechanism so a task + * 'taking' a semaphore MUST ALWAYS 'give' the semaphore back once the + * semaphore it is no longer required. + * + * Mutex type semaphores cannot be used from within interrupt service routines. * - * See vSemaphoreCreateBinary() for an alternative implementation that can be - * used for pure synchronisation (where one task or interrupt always 'gives' the - * semaphore and another always 'takes' the semaphore) and from within interrupt + * See vSemaphoreCreateBinary() for an alternative implementation that can be + * used for pure synchronisation (where one task or interrupt always 'gives' the + * semaphore and another always 'takes' the semaphore) and from within interrupt * service routines. * - * @return xSemaphore Handle to the created mutex semaphore. Should be of type - * xSemaphoreHandle. + * @return xSemaphore Handle to the created mutex semaphore. Should be of type + * SemaphoreHandle_t. * * Example usage:
- xSemaphoreHandle xSemaphore;
+ SemaphoreHandle_t xSemaphore;
 
  void vATask( void * pvParameters )
  {
@@ -689,7 +738,7 @@ typedef xQueueHandle xSemaphoreHandle;
     if( xSemaphore != NULL )
     {
         // The semaphore was created successfully.
-        // The semaphore can now be used.  
+        // The semaphore can now be used.
     }
  }
  
@@ -700,34 +749,34 @@ typedef xQueueHandle xSemaphoreHandle; /** * semphr. h - *
xSemaphoreHandle xSemaphoreCreateCounting( unsigned portBASE_TYPE uxMaxCount, unsigned portBASE_TYPE uxInitialCount )
+ *
SemaphoreHandle_t xSemaphoreCreateCounting( UBaseType_t uxMaxCount, UBaseType_t uxInitialCount )
* - * Macro that creates a counting semaphore by using the existing - * queue mechanism. + * Macro that creates a counting semaphore by using the existing + * queue mechanism. * * Counting semaphores are typically used for two things: * - * 1) Counting events. + * 1) Counting events. * * In this usage scenario an event handler will 'give' a semaphore each time - * an event occurs (incrementing the semaphore count value), and a handler - * task will 'take' a semaphore each time it processes an event - * (decrementing the semaphore count value). The count value is therefore - * the difference between the number of events that have occurred and the - * number that have been processed. In this case it is desirable for the + * an event occurs (incrementing the semaphore count value), and a handler + * task will 'take' a semaphore each time it processes an event + * (decrementing the semaphore count value). The count value is therefore + * the difference between the number of events that have occurred and the + * number that have been processed. In this case it is desirable for the * initial count value to be zero. * * 2) Resource management. * * In this usage scenario the count value indicates the number of resources - * available. To obtain control of a resource a task must first obtain a + * available. To obtain control of a resource a task must first obtain a * semaphore - decrementing the semaphore count value. When the count value * reaches zero there are no free resources. When a task finishes with the * resource it 'gives' the semaphore back - incrementing the semaphore count * value. In this case it is desirable for the initial count value to be * equal to the maximum count value, indicating that all resources are free. * - * @param uxMaxCount The maximum count value that can be reached. When the + * @param uxMaxCount The maximum count value that can be reached. When the * semaphore reaches this value it can no longer be 'given'. * * @param uxInitialCount The count value assigned to the semaphore when it is @@ -735,14 +784,14 @@ typedef xQueueHandle xSemaphoreHandle; * * @return Handle to the created semaphore. Null if the semaphore could not be * created. - * + * * Example usage:
- xSemaphoreHandle xSemaphore;
+ SemaphoreHandle_t xSemaphore;
 
  void vATask( void * pvParameters )
  {
- xSemaphoreHandle xSemaphore = NULL;
+ SemaphoreHandle_t xSemaphore = NULL;
 
     // Semaphore cannot be used before a call to xSemaphoreCreateCounting().
     // The max value to which the semaphore can count should be 10, and the
@@ -752,7 +801,7 @@ typedef xQueueHandle xSemaphoreHandle;
     if( xSemaphore != NULL )
     {
         // The semaphore was created successfully.
-        // The semaphore can now be used.  
+        // The semaphore can now be used.
     }
  }
  
@@ -763,27 +812,27 @@ typedef xQueueHandle xSemaphoreHandle; /** * semphr. h - *
void vSemaphoreDelete( xSemaphoreHandle xSemaphore );
+ *
void vSemaphoreDelete( SemaphoreHandle_t xSemaphore );
* * Delete a semaphore. This function must be used with care. For example, * do not delete a mutex type semaphore if the mutex is held by a task. * * @param xSemaphore A handle to the semaphore to be deleted. * - * \page vSemaphoreDelete vSemaphoreDelete + * \defgroup vSemaphoreDelete vSemaphoreDelete * \ingroup Semaphores */ -#define vSemaphoreDelete( xSemaphore ) vQueueDelete( ( xQueueHandle ) ( xSemaphore ) ) +#define vSemaphoreDelete( xSemaphore ) vQueueDelete( ( QueueHandle_t ) ( xSemaphore ) ) /** * semphr.h - *
xTaskHandle xSemaphoreGetMutexHolder( xSemaphoreHandle xMutex );
+ *
TaskHandle_t xSemaphoreGetMutexHolder( SemaphoreHandle_t xMutex );
* * If xMutex is indeed a mutex type semaphore, return the current mutex holder. * If xMutex is not a mutex type semaphore, or the mutex is available (not held * by a task), return NULL. * - * Note: This Is is a good way of determining if the calling task is the mutex + * Note: This is a good way of determining if the calling task is the mutex * holder, but not a good way of determining the identity of the mutex holder as * the holder may change between the function exiting and the returned value * being tested. diff --git a/lib/FreeRTOS/include/task.h b/lib/FreeRTOS/include/task.h index 42a57e44ba..b8d6dd8909 100644 --- a/lib/FreeRTOS/include/task.h +++ b/lib/FreeRTOS/include/task.h @@ -1,86 +1,80 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ -#ifndef TASK_H -#define TASK_H +#ifndef INC_TASK_H +#define INC_TASK_H #ifndef INC_FREERTOS_H #error "include FreeRTOS.h must appear in source files before include task.h" #endif -#include "portable.h" #include "list.h" #ifdef __cplusplus @@ -91,28 +85,57 @@ extern "C" { * MACROS AND DEFINITIONS *----------------------------------------------------------*/ -#define tskKERNEL_VERSION_NUMBER "V7.4.0" +#define tskKERNEL_VERSION_NUMBER "V8.2.3" +#define tskKERNEL_VERSION_MAJOR 8 +#define tskKERNEL_VERSION_MINOR 2 +#define tskKERNEL_VERSION_BUILD 3 /** * task. h * * Type by which tasks are referenced. For example, a call to xTaskCreate - * returns (via a pointer parameter) an xTaskHandle variable that can then + * returns (via a pointer parameter) an TaskHandle_t variable that can then * be used as a parameter to vTaskDelete to delete the task. * - * \page xTaskHandle xTaskHandle + * \defgroup TaskHandle_t TaskHandle_t * \ingroup Tasks */ -typedef void * xTaskHandle; +typedef void * TaskHandle_t; + +/* + * Defines the prototype to which the application task hook function must + * conform. + */ +typedef BaseType_t (*TaskHookFunction_t)( void * ); + +/* Task states returned by eTaskGetState. */ +typedef enum +{ + eRunning = 0, /* A task is querying the state of itself, so must be running. */ + eReady, /* The task being queried is in a read or pending ready list. */ + eBlocked, /* The task being queried is in the Blocked state. */ + eSuspended, /* The task being queried is in the Suspended state, or is in the Blocked state with an infinite time out. */ + eDeleted /* The task being queried has been deleted, but its TCB has not yet been freed. */ +} eTaskState; + +/* Actions that can be performed when vTaskNotify() is called. */ +typedef enum +{ + eNoAction = 0, /* Notify the task without updating its notify value. */ + eSetBits, /* Set bits in the task's notification value. */ + eIncrement, /* Increment the task's notification value. */ + eSetValueWithOverwrite, /* Set the task's notification value to a specific value even if the previous value has not yet been read by the task. */ + eSetValueWithoutOverwrite /* Set the task's notification value if the previous value has been read by the task. */ +} eNotifyAction; /* * Used internally only. */ typedef struct xTIME_OUT { - portBASE_TYPE xOverflowCount; - portTickType xTimeOnEntering; -} xTimeOutType; + BaseType_t xOverflowCount; + TickType_t xTimeOnEntering; +} TimeOut_t; /* * Defines the memory ranges allocated to the task when an MPU is used. @@ -120,33 +143,37 @@ typedef struct xTIME_OUT typedef struct xMEMORY_REGION { void *pvBaseAddress; - unsigned long ulLengthInBytes; - unsigned long ulParameters; -} xMemoryRegion; + uint32_t ulLengthInBytes; + uint32_t ulParameters; +} MemoryRegion_t; /* * Parameters required to create an MPU protected task. */ -typedef struct xTASK_PARAMTERS +typedef struct xTASK_PARAMETERS { - pdTASK_CODE pvTaskCode; - const signed char * const pcName; - unsigned short usStackDepth; + TaskFunction_t pvTaskCode; + const char * const pcName; /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ + uint16_t usStackDepth; void *pvParameters; - unsigned portBASE_TYPE uxPriority; - portSTACK_TYPE *puxStackBuffer; - xMemoryRegion xRegions[ portNUM_CONFIGURABLE_REGIONS ]; -} xTaskParameters; - -/* Task states returned by eTaskGetState. */ -typedef enum + UBaseType_t uxPriority; + StackType_t *puxStackBuffer; + MemoryRegion_t xRegions[ portNUM_CONFIGURABLE_REGIONS ]; +} TaskParameters_t; + +/* Used with the uxTaskGetSystemState() function to return the state of each task +in the system. */ +typedef struct xTASK_STATUS { - eRunning = 0, /* A task is querying the state of itself, so must be running. */ - eReady, /* The task being queried is in a read or pending ready list. */ - eBlocked, /* The task being queried is in the Blocked state. */ - eSuspended, /* The task being queried is in the Suspended state, or is in the Blocked state with an infinite time out. */ - eDeleted /* The task being queried has been deleted, but its TCB has not yet been freed. */ -} eTaskState; + TaskHandle_t xHandle; /* The handle of the task to which the rest of the information in the structure relates. */ + const char *pcTaskName; /* A pointer to the task's name. This value will be invalid if the task was deleted since the structure was populated! */ /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ + UBaseType_t xTaskNumber; /* A number unique to the task. */ + eTaskState eCurrentState; /* The state in which the task existed when the structure was populated. */ + UBaseType_t uxCurrentPriority; /* The priority at which the task was running (may be inherited) when the structure was populated. */ + UBaseType_t uxBasePriority; /* The priority to which the task will return if the task's current priority has been inherited to avoid unbounded priority inversion when obtaining a mutex. Only valid if configUSE_MUTEXES is defined as 1 in FreeRTOSConfig.h. */ + uint32_t ulRunTimeCounter; /* The total run time allocated to the task so far, as defined by the run time stats clock. See http://www.freertos.org/rtos-run-time-stats.html. Only valid when configGENERATE_RUN_TIME_STATS is defined as 1 in FreeRTOSConfig.h. */ + uint16_t usStackHighWaterMark; /* The minimum amount of stack space that has remained for the task since the task was created. The closer this value is to zero the closer the task has come to overflowing its stack. */ +} TaskStatus_t; /* Possible return values for eTaskConfirmSleepModeStatus(). */ typedef enum @@ -157,19 +184,19 @@ typedef enum } eSleepModeStatus; -/* +/** * Defines the priority used by the idle task. This must not be modified. * * \ingroup TaskUtils */ -#define tskIDLE_PRIORITY ( ( unsigned portBASE_TYPE ) 0U ) +#define tskIDLE_PRIORITY ( ( UBaseType_t ) 0U ) /** * task. h * * Macro for forcing a context switch. * - * \page taskYIELD taskYIELD + * \defgroup taskYIELD taskYIELD * \ingroup SchedulerControl */ #define taskYIELD() portYIELD() @@ -183,10 +210,11 @@ typedef enum * NOTE: This may alter the stack (depending on the portable implementation) * so must be used with care! * - * \page taskENTER_CRITICAL taskENTER_CRITICAL + * \defgroup taskENTER_CRITICAL taskENTER_CRITICAL * \ingroup SchedulerControl */ #define taskENTER_CRITICAL() portENTER_CRITICAL() +#define taskENTER_CRITICAL_FROM_ISR() portSET_INTERRUPT_MASK_FROM_ISR() /** * task. h @@ -197,17 +225,17 @@ typedef enum * NOTE: This may alter the stack (depending on the portable implementation) * so must be used with care! * - * \page taskEXIT_CRITICAL taskEXIT_CRITICAL + * \defgroup taskEXIT_CRITICAL taskEXIT_CRITICAL * \ingroup SchedulerControl */ #define taskEXIT_CRITICAL() portEXIT_CRITICAL() - +#define taskEXIT_CRITICAL_FROM_ISR( x ) portCLEAR_INTERRUPT_MASK_FROM_ISR( x ) /** * task. h * * Macro to disable all maskable interrupts. * - * \page taskDISABLE_INTERRUPTS taskDISABLE_INTERRUPTS + * \defgroup taskDISABLE_INTERRUPTS taskDISABLE_INTERRUPTS * \ingroup SchedulerControl */ #define taskDISABLE_INTERRUPTS() portDISABLE_INTERRUPTS() @@ -217,15 +245,18 @@ typedef enum * * Macro to enable microcontroller interrupts. * - * \page taskENABLE_INTERRUPTS taskENABLE_INTERRUPTS + * \defgroup taskENABLE_INTERRUPTS taskENABLE_INTERRUPTS * \ingroup SchedulerControl */ #define taskENABLE_INTERRUPTS() portENABLE_INTERRUPTS() -/* Definitions returned by xTaskGetSchedulerState(). */ -#define taskSCHEDULER_NOT_STARTED 0 -#define taskSCHEDULER_RUNNING 1 -#define taskSCHEDULER_SUSPENDED 2 +/* Definitions returned by xTaskGetSchedulerState(). taskSCHEDULER_SUSPENDED is +0 to generate more optimal code when configASSERT() is defined as the constant +is used in assert() statements. */ +#define taskSCHEDULER_SUSPENDED ( ( BaseType_t ) 0 ) +#define taskSCHEDULER_NOT_STARTED ( ( BaseType_t ) 1 ) +#define taskSCHEDULER_RUNNING ( ( BaseType_t ) 2 ) + /*----------------------------------------------------------- * TASK CREATION API @@ -234,13 +265,13 @@ typedef enum /** * task. h *
- portBASE_TYPE xTaskCreate(
-							  pdTASK_CODE pvTaskCode,
+ BaseType_t xTaskCreate(
+							  TaskFunction_t pvTaskCode,
 							  const char * const pcName,
-							  unsigned short usStackDepth,
+							  uint16_t usStackDepth,
 							  void *pvParameters,
-							  unsigned portBASE_TYPE uxPriority,
-							  xTaskHandle *pvCreatedTask
+							  UBaseType_t uxPriority,
+							  TaskHandle_t *pvCreatedTask
 						  );
* * Create a new task and add it to the list of tasks that are ready to run. @@ -254,7 +285,7 @@ typedef enum * must be implemented to never return (i.e. continuous loop). * * @param pcName A descriptive name for the task. This is mainly used to - * facilitate debugging. Max length defined by tskMAX_TASK_NAME_LEN - default + * facilitate debugging. Max length defined by configMAX_TASK_NAME_LEN - default * is 16. * * @param usStackDepth The size of the task stack specified as the number of @@ -275,7 +306,7 @@ typedef enum * can be referenced. * * @return pdPASS if the task was successfully created and added to a ready - * list, otherwise an error code defined in the file errors. h + * list, otherwise an error code defined in the file projdefs.h * * Example usage:
@@ -291,17 +322,21 @@ typedef enum
  // Function that creates a task.
  void vOtherFunction( void )
  {
- static unsigned char ucParameterToPass;
- xTaskHandle xHandle;
+ static uint8_t ucParameterToPass;
+ TaskHandle_t xHandle = NULL;
 
 	 // Create the task, storing the handle.  Note that the passed parameter ucParameterToPass
 	 // must exist for the lifetime of the task, so in this case is declared static.  If it was just an
 	 // an automatic stack variable it might no longer exist, or at least have been corrupted, by the time
 	 // the new task attempts to access it.
 	 xTaskCreate( vTaskCode, "NAME", STACK_SIZE, &ucParameterToPass, tskIDLE_PRIORITY, &xHandle );
+     configASSERT( xHandle );
 
 	 // Use the handle to delete the task.
-	 vTaskDelete( xHandle );
+     if( xHandle != NULL )
+     {
+	     vTaskDelete( xHandle );
+     }
  }
    
* \defgroup xTaskCreate xTaskCreate @@ -312,7 +347,7 @@ typedef enum /** * task. h *
- portBASE_TYPE xTaskCreateRestricted( xTaskParameters *pxTaskDefinition, xTaskHandle *pxCreatedTask );
+ BaseType_t xTaskCreateRestricted( TaskParameters_t *pxTaskDefinition, TaskHandle_t *pxCreatedTask );
* * xTaskCreateRestricted() should only be used in systems that include an MPU * implementation. @@ -330,12 +365,12 @@ typedef enum * can be referenced. * * @return pdPASS if the task was successfully created and added to a ready - * list, otherwise an error code defined in the file errors. h + * list, otherwise an error code defined in the file projdefs.h * * Example usage:
-// Create an xTaskParameters structure that defines the task to be created.
-static const xTaskParameters xCheckTaskParameters =
+// Create an TaskParameters_t structure that defines the task to be created.
+static const TaskParameters_t xCheckTaskParameters =
 {
 	vATask,		// pvTaskCode - the function that implements the task.
 	"ATask",	// pcName - just a text name for the task to assist debugging.
@@ -358,7 +393,7 @@ static const xTaskParameters xCheckTaskParameters =
 
 int main( void )
 {
-xTaskHandle xHandle;
+TaskHandle_t xHandle;
 
 	// Create a task from the const structure defined above.  The task handle
 	// is requested (the second parameter is not NULL) but in this case just for
@@ -369,7 +404,7 @@ xTaskHandle xHandle;
 	vTaskStartScheduler();
 
 	// Will only get here if there was insufficient memory to create the idle
-	// task.
+	// and/or timer task.
 	for( ;; );
 }
    
@@ -381,7 +416,7 @@ xTaskHandle xHandle; /** * task. h *
- void vTaskAllocateMPURegions( xTaskHandle xTask, const xMemoryRegion * const pxRegions );
+ void vTaskAllocateMPURegions( TaskHandle_t xTask, const MemoryRegion_t * const pxRegions );
* * Memory regions are assigned to a restricted task when the task is created by * a call to xTaskCreateRestricted(). These regions can be redefined using @@ -389,16 +424,16 @@ xTaskHandle xHandle; * * @param xTask The handle of the task being updated. * - * @param xRegions A pointer to an xMemoryRegion structure that contains the + * @param xRegions A pointer to an MemoryRegion_t structure that contains the * new memory region definitions. * * Example usage:
-// Define an array of xMemoryRegion structures that configures an MPU region
+// Define an array of MemoryRegion_t structures that configures an MPU region
 // allowing read/write access for 1024 bytes starting at the beginning of the
 // ucOneKByte array.  The other two of the maximum 3 definable regions are
 // unused so set to zero.
-static const xMemoryRegion xAltRegions[ portNUM_CONFIGURABLE_REGIONS ] =
+static const MemoryRegion_t xAltRegions[ portNUM_CONFIGURABLE_REGIONS ] =
 {
 	// Base address		Length		Parameters
 	{ ucOneKByte,		1024,		portMPU_REGION_READ_WRITE },
@@ -424,16 +459,16 @@ void vATask( void *pvParameters )
  * \defgroup xTaskCreateRestricted xTaskCreateRestricted
  * \ingroup Tasks
  */
-void vTaskAllocateMPURegions( xTaskHandle xTask, const xMemoryRegion * const pxRegions ) PRIVILEGED_FUNCTION;
+void vTaskAllocateMPURegions( TaskHandle_t xTask, const MemoryRegion_t * const pxRegions ) PRIVILEGED_FUNCTION;
 
 /**
  * task. h
- * 
void vTaskDelete( xTaskHandle xTask );
+ *
void vTaskDelete( TaskHandle_t xTask );
* * INCLUDE_vTaskDelete must be defined as 1 for this function to be available. * See the configuration section for more information. * - * Remove a task from the RTOS real time kernels management. The task being + * Remove a task from the RTOS real time kernel's management. The task being * deleted will be removed from all ready, blocked, suspended and event lists. * * NOTE: The idle task is responsible for freeing the kernel allocated @@ -453,7 +488,7 @@ void vTaskAllocateMPURegions( xTaskHandle xTask, const xMemoryRegion * const pxR
  void vOtherFunction( void )
  {
- xTaskHandle xHandle;
+ TaskHandle_t xHandle;
 
 	 // Create the task, storing the handle.
 	 xTaskCreate( vTaskCode, "NAME", STACK_SIZE, NULL, tskIDLE_PRIORITY, &xHandle );
@@ -465,7 +500,7 @@ void vTaskAllocateMPURegions( xTaskHandle xTask, const xMemoryRegion * const pxR
  * \defgroup vTaskDelete vTaskDelete
  * \ingroup Tasks
  */
-void vTaskDelete( xTaskHandle xTaskToDelete ) PRIVILEGED_FUNCTION;
+void vTaskDelete( TaskHandle_t xTaskToDelete ) PRIVILEGED_FUNCTION;
 
 /*-----------------------------------------------------------
  * TASK CONTROL API
@@ -473,11 +508,11 @@ void vTaskDelete( xTaskHandle xTaskToDelete ) PRIVILEGED_FUNCTION;
 
 /**
  * task. h
- * 
void vTaskDelay( portTickType xTicksToDelay );
+ *
void vTaskDelay( const TickType_t xTicksToDelay );
* * Delay a task for a given number of ticks. The actual time that the * task remains blocked depends on the tick rate. The constant - * portTICK_RATE_MS can be used to calculate real time from the tick + * portTICK_PERIOD_MS can be used to calculate real time from the tick * rate - with the resolution of one tick period. * * INCLUDE_vTaskDelay must be defined as 1 for this function to be available. @@ -488,7 +523,7 @@ void vTaskDelete( xTaskHandle xTaskToDelete ) PRIVILEGED_FUNCTION; * the time at which vTaskDelay() is called. For example, specifying a block * period of 100 ticks will cause the task to unblock 100 ticks after * vTaskDelay() is called. vTaskDelay() does not therefore provide a good method - * of controlling the frequency of a cyclical task as the path taken through the + * of controlling the frequency of a periodic task as the path taken through the * code, as well as other task and interrupt activity, will effect the frequency * at which vTaskDelay() gets called and therefore the time at which the task * next executes. See vTaskDelayUntil() for an alternative API function designed @@ -501,12 +536,10 @@ void vTaskDelete( xTaskHandle xTaskToDelete ) PRIVILEGED_FUNCTION; * * Example usage: - void vTaskFunction( void * pvParameters ) - { void vTaskFunction( void * pvParameters ) { // Block for 500ms. - const portTickType xDelay = 500 / portTICK_RATE_MS; + const TickType_t xDelay = 500 / portTICK_PERIOD_MS; for( ;; ) { @@ -519,16 +552,16 @@ void vTaskDelete( xTaskHandle xTaskToDelete ) PRIVILEGED_FUNCTION; * \defgroup vTaskDelay vTaskDelay * \ingroup TaskCtrl */ -void vTaskDelay( portTickType xTicksToDelay ) PRIVILEGED_FUNCTION; +void vTaskDelay( const TickType_t xTicksToDelay ) PRIVILEGED_FUNCTION; /** * task. h - *
void vTaskDelayUntil( portTickType *pxPreviousWakeTime, portTickType xTimeIncrement );
+ *
void vTaskDelayUntil( TickType_t *pxPreviousWakeTime, const TickType_t xTimeIncrement );
* * INCLUDE_vTaskDelayUntil must be defined as 1 for this function to be available. * See the configuration section for more information. * - * Delay a task until a specified time. This function can be used by cyclical + * Delay a task until a specified time. This function can be used by periodic * tasks to ensure a constant execution frequency. * * This function differs from vTaskDelay () in one important aspect: vTaskDelay () will @@ -543,7 +576,7 @@ void vTaskDelay( portTickType xTicksToDelay ) PRIVILEGED_FUNCTION; * is called, vTaskDelayUntil () specifies the absolute (exact) time at which it wishes to * unblock. * - * The constant portTICK_RATE_MS can be used to calculate real time from the tick + * The constant portTICK_PERIOD_MS can be used to calculate real time from the tick * rate - with the resolution of one tick period. * * @param pxPreviousWakeTime Pointer to a variable that holds the time at which the @@ -561,8 +594,8 @@ void vTaskDelay( portTickType xTicksToDelay ) PRIVILEGED_FUNCTION; // Perform an action every 10 ticks. void vTaskFunction( void * pvParameters ) { - portTickType xLastWakeTime; - const portTickType xFrequency = 10; + TickType_t xLastWakeTime; + const TickType_t xFrequency = 10; // Initialise the xLastWakeTime variable with the current time. xLastWakeTime = xTaskGetTickCount (); @@ -578,13 +611,13 @@ void vTaskDelay( portTickType xTicksToDelay ) PRIVILEGED_FUNCTION; * \defgroup vTaskDelayUntil vTaskDelayUntil * \ingroup TaskCtrl */ -void vTaskDelayUntil( portTickType * const pxPreviousWakeTime, portTickType xTimeIncrement ) PRIVILEGED_FUNCTION; +void vTaskDelayUntil( TickType_t * const pxPreviousWakeTime, const TickType_t xTimeIncrement ) PRIVILEGED_FUNCTION; /** * task. h - *
unsigned portBASE_TYPE uxTaskPriorityGet( xTaskHandle xTask );
+ *
UBaseType_t uxTaskPriorityGet( TaskHandle_t xTask );
* - * INCLUDE_xTaskPriorityGet must be defined as 1 for this function to be available. + * INCLUDE_uxTaskPriorityGet must be defined as 1 for this function to be available. * See the configuration section for more information. * * Obtain the priority of any task. @@ -598,7 +631,7 @@ void vTaskDelayUntil( portTickType * const pxPreviousWakeTime, portTickType xTim
  void vAFunction( void )
  {
- xTaskHandle xHandle;
+ TaskHandle_t xHandle;
 
 	 // Create a task, storing the handle.
 	 xTaskCreate( vTaskCode, "NAME", STACK_SIZE, NULL, tskIDLE_PRIORITY, &xHandle );
@@ -625,11 +658,19 @@ void vTaskDelayUntil( portTickType * const pxPreviousWakeTime, portTickType xTim
  * \defgroup uxTaskPriorityGet uxTaskPriorityGet
  * \ingroup TaskCtrl
  */
-unsigned portBASE_TYPE uxTaskPriorityGet( xTaskHandle xTask ) PRIVILEGED_FUNCTION;
+UBaseType_t uxTaskPriorityGet( TaskHandle_t xTask ) PRIVILEGED_FUNCTION;
 
 /**
  * task. h
- * 
eTaskState eTaskGetState( xTaskHandle xTask );
+ *
UBaseType_t uxTaskPriorityGetFromISR( TaskHandle_t xTask );
+ * + * A version of uxTaskPriorityGet() that can be used from an ISR. + */ +UBaseType_t uxTaskPriorityGetFromISR( TaskHandle_t xTask ) PRIVILEGED_FUNCTION; + +/** + * task. h + *
eTaskState eTaskGetState( TaskHandle_t xTask );
* * INCLUDE_eTaskGetState must be defined as 1 for this function to be available. * See the configuration section for more information. @@ -643,11 +684,11 @@ unsigned portBASE_TYPE uxTaskPriorityGet( xTaskHandle xTask ) PRIVILEGED_FUNCTIO * state of the task might change between the function being called, and the * functions return value being tested by the calling task. */ -eTaskState eTaskGetState( xTaskHandle xTask ) PRIVILEGED_FUNCTION; +eTaskState eTaskGetState( TaskHandle_t xTask ) PRIVILEGED_FUNCTION; /** * task. h - *
void vTaskPrioritySet( xTaskHandle xTask, unsigned portBASE_TYPE uxNewPriority );
+ *
void vTaskPrioritySet( TaskHandle_t xTask, UBaseType_t uxNewPriority );
* * INCLUDE_vTaskPrioritySet must be defined as 1 for this function to be available. * See the configuration section for more information. @@ -666,7 +707,7 @@ eTaskState eTaskGetState( xTaskHandle xTask ) PRIVILEGED_FUNCTION;
  void vAFunction( void )
  {
- xTaskHandle xHandle;
+ TaskHandle_t xHandle;
 
 	 // Create a task, storing the handle.
 	 xTaskCreate( vTaskCode, "NAME", STACK_SIZE, NULL, tskIDLE_PRIORITY, &xHandle );
@@ -685,11 +726,11 @@ eTaskState eTaskGetState( xTaskHandle xTask ) PRIVILEGED_FUNCTION;
  * \defgroup vTaskPrioritySet vTaskPrioritySet
  * \ingroup TaskCtrl
  */
-void vTaskPrioritySet( xTaskHandle xTask, unsigned portBASE_TYPE uxNewPriority ) PRIVILEGED_FUNCTION;
+void vTaskPrioritySet( TaskHandle_t xTask, UBaseType_t uxNewPriority ) PRIVILEGED_FUNCTION;
 
 /**
  * task. h
- * 
void vTaskSuspend( xTaskHandle xTaskToSuspend );
+ *
void vTaskSuspend( TaskHandle_t xTaskToSuspend );
* * INCLUDE_vTaskSuspend must be defined as 1 for this function to be available. * See the configuration section for more information. @@ -708,7 +749,7 @@ void vTaskPrioritySet( xTaskHandle xTask, unsigned portBASE_TYPE uxNewPriority )
  void vAFunction( void )
  {
- xTaskHandle xHandle;
+ TaskHandle_t xHandle;
 
 	 // Create a task, storing the handle.
 	 xTaskCreate( vTaskCode, "NAME", STACK_SIZE, NULL, tskIDLE_PRIORITY, &xHandle );
@@ -736,18 +777,18 @@ void vTaskPrioritySet( xTaskHandle xTask, unsigned portBASE_TYPE uxNewPriority )
  * \defgroup vTaskSuspend vTaskSuspend
  * \ingroup TaskCtrl
  */
-void vTaskSuspend( xTaskHandle xTaskToSuspend ) PRIVILEGED_FUNCTION;
+void vTaskSuspend( TaskHandle_t xTaskToSuspend ) PRIVILEGED_FUNCTION;
 
 /**
  * task. h
- * 
void vTaskResume( xTaskHandle xTaskToResume );
+ *
void vTaskResume( TaskHandle_t xTaskToResume );
* * INCLUDE_vTaskSuspend must be defined as 1 for this function to be available. * See the configuration section for more information. * * Resumes a suspended task. * - * A task that has been suspended by one of more calls to vTaskSuspend () + * A task that has been suspended by one or more calls to vTaskSuspend () * will be made available for running again by a single call to * vTaskResume (). * @@ -757,7 +798,7 @@ void vTaskSuspend( xTaskHandle xTaskToSuspend ) PRIVILEGED_FUNCTION;
  void vAFunction( void )
  {
- xTaskHandle xHandle;
+ TaskHandle_t xHandle;
 
 	 // Create a task, storing the handle.
 	 xTaskCreate( vTaskCode, "NAME", STACK_SIZE, NULL, tskIDLE_PRIORITY, &xHandle );
@@ -779,33 +820,42 @@ void vTaskSuspend( xTaskHandle xTaskToSuspend ) PRIVILEGED_FUNCTION;
 	 vTaskResume( xHandle );
 
 	 // The created task will once again get microcontroller processing
-	 // time in accordance with it priority within the system.
+	 // time in accordance with its priority within the system.
  }
    
* \defgroup vTaskResume vTaskResume * \ingroup TaskCtrl */ -void vTaskResume( xTaskHandle xTaskToResume ) PRIVILEGED_FUNCTION; +void vTaskResume( TaskHandle_t xTaskToResume ) PRIVILEGED_FUNCTION; /** * task. h - *
void xTaskResumeFromISR( xTaskHandle xTaskToResume );
+ *
void xTaskResumeFromISR( TaskHandle_t xTaskToResume );
* * INCLUDE_xTaskResumeFromISR must be defined as 1 for this function to be * available. See the configuration section for more information. * * An implementation of vTaskResume() that can be called from within an ISR. * - * A task that has been suspended by one of more calls to vTaskSuspend () + * A task that has been suspended by one or more calls to vTaskSuspend () * will be made available for running again by a single call to * xTaskResumeFromISR (). * + * xTaskResumeFromISR() should not be used to synchronise a task with an + * interrupt if there is a chance that the interrupt could arrive prior to the + * task being suspended - as this can lead to interrupts being missed. Use of a + * semaphore as a synchronisation mechanism would avoid this eventuality. + * * @param xTaskToResume Handle to the task being readied. * + * @return pdTRUE if resuming the task should result in a context switch, + * otherwise pdFALSE. This is used by the ISR to determine if a context switch + * may be required following the ISR. + * * \defgroup vTaskResumeFromISR vTaskResumeFromISR * \ingroup TaskCtrl */ -portBASE_TYPE xTaskResumeFromISR( xTaskHandle xTaskToResume ) PRIVILEGED_FUNCTION; +BaseType_t xTaskResumeFromISR( TaskHandle_t xTaskToResume ) PRIVILEGED_FUNCTION; /*----------------------------------------------------------- * SCHEDULER CONTROL @@ -816,12 +866,7 @@ portBASE_TYPE xTaskResumeFromISR( xTaskHandle xTaskToResume ) PRIVILEGED_FUNCTIO *
void vTaskStartScheduler( void );
* * Starts the real time kernel tick processing. After calling the kernel - * has control over which tasks are executed and when. This function - * does not return until an executing task calls vTaskEndScheduler (). - * - * At least one task should be created via a call to xTaskCreate () - * before calling vTaskStartScheduler (). The idle task is created - * automatically when the first application task is created. + * has control over which tasks are executed and when. * * See the demo application file main.c for an example of creating * tasks and starting the kernel. @@ -849,6 +894,9 @@ void vTaskStartScheduler( void ) PRIVILEGED_FUNCTION; * task. h *
void vTaskEndScheduler( void );
* + * NOTE: At the time of writing only the x86 real mode port, which runs on a PC + * in place of DOS, implements this function. + * * Stops the real time kernel tick. All created tasks will be automatically * deleted and multitasking (either preemptive or cooperative) will * stop. Execution then resumes from the point where vTaskStartScheduler () @@ -902,8 +950,8 @@ void vTaskEndScheduler( void ) PRIVILEGED_FUNCTION; * task. h *
void vTaskSuspendAll( void );
* - * Suspends all real time kernel activity while keeping interrupts (including the - * kernel tick) enabled. + * Suspends the scheduler without disabling interrupts. Context switches will + * not occur while the scheduler is suspended. * * After calling vTaskSuspendAll () the calling task will continue to execute * without risk of being swapped out until a call to xTaskResumeAll () has been @@ -951,11 +999,13 @@ void vTaskSuspendAll( void ) PRIVILEGED_FUNCTION; /** * task. h - *
char xTaskResumeAll( void );
+ *
BaseType_t xTaskResumeAll( void );
+ * + * Resumes scheduler activity after it was suspended by a call to + * vTaskSuspendAll(). * - * Resumes real time kernel activity following a call to vTaskSuspendAll (). - * After a call to vTaskSuspendAll () the kernel will take control of which - * task is executing at any time. + * xTaskResumeAll() only resumes the scheduler. It does not unsuspend tasks + * that were previously suspended by a call to vTaskSuspend(). * * @return If resuming the scheduler caused a context switch then pdTRUE is * returned, otherwise pdFALSE is returned. @@ -999,18 +1049,7 @@ void vTaskSuspendAll( void ) PRIVILEGED_FUNCTION; * \defgroup xTaskResumeAll xTaskResumeAll * \ingroup SchedulerControl */ -signed portBASE_TYPE xTaskResumeAll( void ) PRIVILEGED_FUNCTION; - -/** - * task. h - *
signed portBASE_TYPE xTaskIsTaskSuspended( xTaskHandle xTask );
- * - * Utility task that simply returns pdTRUE if the task referenced by xTask is - * currently in the Suspended state, or pdFALSE if the task referenced by xTask - * is in any other state. - * - */ -signed portBASE_TYPE xTaskIsTaskSuspended( xTaskHandle xTask ) PRIVILEGED_FUNCTION; +BaseType_t xTaskResumeAll( void ) PRIVILEGED_FUNCTION; /*----------------------------------------------------------- * TASK UTILITIES @@ -1018,67 +1057,250 @@ signed portBASE_TYPE xTaskIsTaskSuspended( xTaskHandle xTask ) PRIVILEGED_FUNCTI /** * task. h - *
portTickType xTaskGetTickCount( void );
+ *
TickType_t xTaskGetTickCount( void );
* * @return The count of ticks since vTaskStartScheduler was called. * - * \page xTaskGetTickCount xTaskGetTickCount + * \defgroup xTaskGetTickCount xTaskGetTickCount * \ingroup TaskUtils */ -portTickType xTaskGetTickCount( void ) PRIVILEGED_FUNCTION; +TickType_t xTaskGetTickCount( void ) PRIVILEGED_FUNCTION; /** * task. h - *
portTickType xTaskGetTickCountFromISR( void );
+ *
TickType_t xTaskGetTickCountFromISR( void );
* * @return The count of ticks since vTaskStartScheduler was called. * * This is a version of xTaskGetTickCount() that is safe to be called from an - * ISR - provided that portTickType is the natural word size of the + * ISR - provided that TickType_t is the natural word size of the * microcontroller being used or interrupt nesting is either not supported or * not being used. * - * \page xTaskGetTickCount xTaskGetTickCount + * \defgroup xTaskGetTickCountFromISR xTaskGetTickCountFromISR * \ingroup TaskUtils */ -portTickType xTaskGetTickCountFromISR( void ) PRIVILEGED_FUNCTION; +TickType_t xTaskGetTickCountFromISR( void ) PRIVILEGED_FUNCTION; /** * task. h - *
unsigned short uxTaskGetNumberOfTasks( void );
+ *
uint16_t uxTaskGetNumberOfTasks( void );
* * @return The number of tasks that the real time kernel is currently managing. * This includes all ready, blocked and suspended tasks. A task that * has been deleted but not yet freed by the idle task will also be * included in the count. * - * \page uxTaskGetNumberOfTasks uxTaskGetNumberOfTasks + * \defgroup uxTaskGetNumberOfTasks uxTaskGetNumberOfTasks * \ingroup TaskUtils */ -unsigned portBASE_TYPE uxTaskGetNumberOfTasks( void ) PRIVILEGED_FUNCTION; +UBaseType_t uxTaskGetNumberOfTasks( void ) PRIVILEGED_FUNCTION; /** * task. h - *
signed char *pcTaskGetTaskName( xTaskHandle xTaskToQuery );
+ *
char *pcTaskGetTaskName( TaskHandle_t xTaskToQuery );
* * @return The text (human readable) name of the task referenced by the handle - * xTaskToQueury. A task can query its own name by either passing in its own + * xTaskToQuery. A task can query its own name by either passing in its own * handle, or by setting xTaskToQuery to NULL. INCLUDE_pcTaskGetTaskName must be * set to 1 in FreeRTOSConfig.h for pcTaskGetTaskName() to be available. * - * \page pcTaskGetTaskName pcTaskGetTaskName + * \defgroup pcTaskGetTaskName pcTaskGetTaskName * \ingroup TaskUtils */ -signed char *pcTaskGetTaskName( xTaskHandle xTaskToQuery ); +char *pcTaskGetTaskName( TaskHandle_t xTaskToQuery ) PRIVILEGED_FUNCTION; /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ + +/** + * task.h + *
UBaseType_t uxTaskGetStackHighWaterMark( TaskHandle_t xTask );
+ * + * INCLUDE_uxTaskGetStackHighWaterMark must be set to 1 in FreeRTOSConfig.h for + * this function to be available. + * + * Returns the high water mark of the stack associated with xTask. That is, + * the minimum free stack space there has been (in words, so on a 32 bit machine + * a value of 1 means 4 bytes) since the task started. The smaller the returned + * number the closer the task has come to overflowing its stack. + * + * @param xTask Handle of the task associated with the stack to be checked. + * Set xTask to NULL to check the stack of the calling task. + * + * @return The smallest amount of free stack space there has been (in words, so + * actual spaces on the stack rather than bytes) since the task referenced by + * xTask was created. + */ +UBaseType_t uxTaskGetStackHighWaterMark( TaskHandle_t xTask ) PRIVILEGED_FUNCTION; + +/* When using trace macros it is sometimes necessary to include task.h before +FreeRTOS.h. When this is done TaskHookFunction_t will not yet have been defined, +so the following two prototypes will cause a compilation error. This can be +fixed by simply guarding against the inclusion of these two prototypes unless +they are explicitly required by the configUSE_APPLICATION_TASK_TAG configuration +constant. */ +#ifdef configUSE_APPLICATION_TASK_TAG + #if configUSE_APPLICATION_TASK_TAG == 1 + /** + * task.h + *
void vTaskSetApplicationTaskTag( TaskHandle_t xTask, TaskHookFunction_t pxHookFunction );
+ * + * Sets pxHookFunction to be the task hook function used by the task xTask. + * Passing xTask as NULL has the effect of setting the calling tasks hook + * function. + */ + void vTaskSetApplicationTaskTag( TaskHandle_t xTask, TaskHookFunction_t pxHookFunction ) PRIVILEGED_FUNCTION; + + /** + * task.h + *
void xTaskGetApplicationTaskTag( TaskHandle_t xTask );
+ * + * Returns the pxHookFunction value assigned to the task xTask. + */ + TaskHookFunction_t xTaskGetApplicationTaskTag( TaskHandle_t xTask ) PRIVILEGED_FUNCTION; + #endif /* configUSE_APPLICATION_TASK_TAG ==1 */ +#endif /* ifdef configUSE_APPLICATION_TASK_TAG */ + +#if( configNUM_THREAD_LOCAL_STORAGE_POINTERS > 0 ) + + /* Each task contains an array of pointers that is dimensioned by the + configNUM_THREAD_LOCAL_STORAGE_POINTERS setting in FreeRTOSConfig.h. The + kernel does not use the pointers itself, so the application writer can use + the pointers for any purpose they wish. The following two functions are + used to set and query a pointer respectively. */ + void vTaskSetThreadLocalStoragePointer( TaskHandle_t xTaskToSet, BaseType_t xIndex, void *pvValue ) PRIVILEGED_FUNCTION; + void *pvTaskGetThreadLocalStoragePointer( TaskHandle_t xTaskToQuery, BaseType_t xIndex ) PRIVILEGED_FUNCTION; + +#endif + +/** + * task.h + *
BaseType_t xTaskCallApplicationTaskHook( TaskHandle_t xTask, void *pvParameter );
+ * + * Calls the hook function associated with xTask. Passing xTask as NULL has + * the effect of calling the Running tasks (the calling task) hook function. + * + * pvParameter is passed to the hook function for the task to interpret as it + * wants. The return value is the value returned by the task hook function + * registered by the user. + */ +BaseType_t xTaskCallApplicationTaskHook( TaskHandle_t xTask, void *pvParameter ) PRIVILEGED_FUNCTION; + +/** + * xTaskGetIdleTaskHandle() is only available if + * INCLUDE_xTaskGetIdleTaskHandle is set to 1 in FreeRTOSConfig.h. + * + * Simply returns the handle of the idle task. It is not valid to call + * xTaskGetIdleTaskHandle() before the scheduler has been started. + */ +TaskHandle_t xTaskGetIdleTaskHandle( void ) PRIVILEGED_FUNCTION; + +/** + * configUSE_TRACE_FACILITY must be defined as 1 in FreeRTOSConfig.h for + * uxTaskGetSystemState() to be available. + * + * uxTaskGetSystemState() populates an TaskStatus_t structure for each task in + * the system. TaskStatus_t structures contain, among other things, members + * for the task handle, task name, task priority, task state, and total amount + * of run time consumed by the task. See the TaskStatus_t structure + * definition in this file for the full member list. + * + * NOTE: This function is intended for debugging use only as its use results in + * the scheduler remaining suspended for an extended period. + * + * @param pxTaskStatusArray A pointer to an array of TaskStatus_t structures. + * The array must contain at least one TaskStatus_t structure for each task + * that is under the control of the RTOS. The number of tasks under the control + * of the RTOS can be determined using the uxTaskGetNumberOfTasks() API function. + * + * @param uxArraySize The size of the array pointed to by the pxTaskStatusArray + * parameter. The size is specified as the number of indexes in the array, or + * the number of TaskStatus_t structures contained in the array, not by the + * number of bytes in the array. + * + * @param pulTotalRunTime If configGENERATE_RUN_TIME_STATS is set to 1 in + * FreeRTOSConfig.h then *pulTotalRunTime is set by uxTaskGetSystemState() to the + * total run time (as defined by the run time stats clock, see + * http://www.freertos.org/rtos-run-time-stats.html) since the target booted. + * pulTotalRunTime can be set to NULL to omit the total run time information. + * + * @return The number of TaskStatus_t structures that were populated by + * uxTaskGetSystemState(). This should equal the number returned by the + * uxTaskGetNumberOfTasks() API function, but will be zero if the value passed + * in the uxArraySize parameter was too small. + * + * Example usage: +
+    // This example demonstrates how a human readable table of run time stats
+	// information is generated from raw data provided by uxTaskGetSystemState().
+	// The human readable table is written to pcWriteBuffer
+	void vTaskGetRunTimeStats( char *pcWriteBuffer )
+	{
+	TaskStatus_t *pxTaskStatusArray;
+	volatile UBaseType_t uxArraySize, x;
+	uint32_t ulTotalRunTime, ulStatsAsPercentage;
+
+		// Make sure the write buffer does not contain a string.
+		*pcWriteBuffer = 0x00;
+
+		// Take a snapshot of the number of tasks in case it changes while this
+		// function is executing.
+		uxArraySize = uxTaskGetNumberOfTasks();
+
+		// Allocate a TaskStatus_t structure for each task.  An array could be
+		// allocated statically at compile time.
+		pxTaskStatusArray = pvPortMalloc( uxArraySize * sizeof( TaskStatus_t ) );
+
+		if( pxTaskStatusArray != NULL )
+		{
+			// Generate raw status information about each task.
+			uxArraySize = uxTaskGetSystemState( pxTaskStatusArray, uxArraySize, &ulTotalRunTime );
+
+			// For percentage calculations.
+			ulTotalRunTime /= 100UL;
+
+			// Avoid divide by zero errors.
+			if( ulTotalRunTime > 0 )
+			{
+				// For each populated position in the pxTaskStatusArray array,
+				// format the raw data as human readable ASCII data
+				for( x = 0; x < uxArraySize; x++ )
+				{
+					// What percentage of the total run time has the task used?
+					// This will always be rounded down to the nearest integer.
+					// ulTotalRunTimeDiv100 has already been divided by 100.
+					ulStatsAsPercentage = pxTaskStatusArray[ x ].ulRunTimeCounter / ulTotalRunTime;
+
+					if( ulStatsAsPercentage > 0UL )
+					{
+						sprintf( pcWriteBuffer, "%s\t\t%lu\t\t%lu%%\r\n", pxTaskStatusArray[ x ].pcTaskName, pxTaskStatusArray[ x ].ulRunTimeCounter, ulStatsAsPercentage );
+					}
+					else
+					{
+						// If the percentage is zero here then the task has
+						// consumed less than 1% of the total run time.
+						sprintf( pcWriteBuffer, "%s\t\t%lu\t\t<1%%\r\n", pxTaskStatusArray[ x ].pcTaskName, pxTaskStatusArray[ x ].ulRunTimeCounter );
+					}
+
+					pcWriteBuffer += strlen( ( char * ) pcWriteBuffer );
+				}
+			}
+
+			// The array is no longer needed, free the memory it consumes.
+			vPortFree( pxTaskStatusArray );
+		}
+	}
+	
+ */ +UBaseType_t uxTaskGetSystemState( TaskStatus_t * const pxTaskStatusArray, const UBaseType_t uxArraySize, uint32_t * const pulTotalRunTime ) PRIVILEGED_FUNCTION; /** * task. h *
void vTaskList( char *pcWriteBuffer );
* - * configUSE_TRACE_FACILITY must be defined as 1 for this function to be - * available. See the configuration section for more information. + * configUSE_TRACE_FACILITY and configUSE_STATS_FORMATTING_FUNCTIONS must + * both be defined as 1 for this function to be available. See the + * configuration section of the FreeRTOS.org website for more information. * - * NOTE: This function will disable interrupts for its duration. It is + * NOTE 1: This function will disable interrupts for its duration. It is * not intended for normal application runtime use but as a debug aid. * * Lists all the current tasks, along with their current state and stack @@ -1087,28 +1309,49 @@ signed char *pcTaskGetTaskName( xTaskHandle xTaskToQuery ); * Tasks are reported as blocked ('B'), ready ('R'), deleted ('D') or * suspended ('S'). * + * PLEASE NOTE: + * + * This function is provided for convenience only, and is used by many of the + * demo applications. Do not consider it to be part of the scheduler. + * + * vTaskList() calls uxTaskGetSystemState(), then formats part of the + * uxTaskGetSystemState() output into a human readable table that displays task + * names, states and stack usage. + * + * vTaskList() has a dependency on the sprintf() C library function that might + * bloat the code size, use a lot of stack, and provide different results on + * different platforms. An alternative, tiny, third party, and limited + * functionality implementation of sprintf() is provided in many of the + * FreeRTOS/Demo sub-directories in a file called printf-stdarg.c (note + * printf-stdarg.c does not provide a full snprintf() implementation!). + * + * It is recommended that production systems call uxTaskGetSystemState() + * directly to get access to raw stats data, rather than indirectly through a + * call to vTaskList(). + * * @param pcWriteBuffer A buffer into which the above mentioned details - * will be written, in ascii form. This buffer is assumed to be large + * will be written, in ASCII form. This buffer is assumed to be large * enough to contain the generated report. Approximately 40 bytes per * task should be sufficient. * - * \page vTaskList vTaskList + * \defgroup vTaskList vTaskList * \ingroup TaskUtils */ -void vTaskList( signed char *pcWriteBuffer ) PRIVILEGED_FUNCTION; +void vTaskList( char * pcWriteBuffer ) PRIVILEGED_FUNCTION; /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ /** * task. h *
void vTaskGetRunTimeStats( char *pcWriteBuffer );
* - * configGENERATE_RUN_TIME_STATS must be defined as 1 for this function - * to be available. The application must also then provide definitions - * for portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() and - * portGET_RUN_TIME_COUNTER_VALUE to configure a peripheral timer/counter - * and return the timers current count value respectively. The counter - * should be at least 10 times the frequency of the tick count. + * configGENERATE_RUN_TIME_STATS and configUSE_STATS_FORMATTING_FUNCTIONS + * must both be defined as 1 for this function to be available. The application + * must also then provide definitions for + * portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() and portGET_RUN_TIME_COUNTER_VALUE() + * to configure a peripheral timer/counter and return the timers current count + * value respectively. The counter should be at least 10 times the frequency of + * the tick count. * - * NOTE: This function will disable interrupts for its duration. It is + * NOTE 1: This function will disable interrupts for its duration. It is * not intended for normal application runtime use but as a debug aid. * * Setting configGENERATE_RUN_TIME_STATS to 1 will result in a total @@ -1119,84 +1362,471 @@ void vTaskList( signed char *pcWriteBuffer ) PRIVILEGED_FUNCTION; * task into a buffer, both as an absolute count value and as a percentage * of the total system execution time. * + * NOTE 2: + * + * This function is provided for convenience only, and is used by many of the + * demo applications. Do not consider it to be part of the scheduler. + * + * vTaskGetRunTimeStats() calls uxTaskGetSystemState(), then formats part of the + * uxTaskGetSystemState() output into a human readable table that displays the + * amount of time each task has spent in the Running state in both absolute and + * percentage terms. + * + * vTaskGetRunTimeStats() has a dependency on the sprintf() C library function + * that might bloat the code size, use a lot of stack, and provide different + * results on different platforms. An alternative, tiny, third party, and + * limited functionality implementation of sprintf() is provided in many of the + * FreeRTOS/Demo sub-directories in a file called printf-stdarg.c (note + * printf-stdarg.c does not provide a full snprintf() implementation!). + * + * It is recommended that production systems call uxTaskGetSystemState() directly + * to get access to raw stats data, rather than indirectly through a call to + * vTaskGetRunTimeStats(). + * * @param pcWriteBuffer A buffer into which the execution times will be - * written, in ascii form. This buffer is assumed to be large enough to + * written, in ASCII form. This buffer is assumed to be large enough to * contain the generated report. Approximately 40 bytes per task should * be sufficient. * - * \page vTaskGetRunTimeStats vTaskGetRunTimeStats + * \defgroup vTaskGetRunTimeStats vTaskGetRunTimeStats * \ingroup TaskUtils */ -void vTaskGetRunTimeStats( signed char *pcWriteBuffer ) PRIVILEGED_FUNCTION; +void vTaskGetRunTimeStats( char *pcWriteBuffer ) PRIVILEGED_FUNCTION; /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ /** - * task.h - *
unsigned portBASE_TYPE uxTaskGetStackHighWaterMark( xTaskHandle xTask );
+ * task. h + *
BaseType_t xTaskNotify( TaskHandle_t xTaskToNotify, uint32_t ulValue, eNotifyAction eAction );
+ * + * configUSE_TASK_NOTIFICATIONS must be undefined or defined as 1 for this + * function to be available. + * + * When configUSE_TASK_NOTIFICATIONS is set to one each task has its own private + * "notification value", which is a 32-bit unsigned integer (uint32_t). + * + * Events can be sent to a task using an intermediary object. Examples of such + * objects are queues, semaphores, mutexes and event groups. Task notifications + * are a method of sending an event directly to a task without the need for such + * an intermediary object. + * + * A notification sent to a task can optionally perform an action, such as + * update, overwrite or increment the task's notification value. In that way + * task notifications can be used to send data to a task, or be used as light + * weight and fast binary or counting semaphores. + * + * A notification sent to a task will remain pending until it is cleared by the + * task calling xTaskNotifyWait() or ulTaskNotifyTake(). If the task was + * already in the Blocked state to wait for a notification when the notification + * arrives then the task will automatically be removed from the Blocked state + * (unblocked) and the notification cleared. + * + * A task can use xTaskNotifyWait() to [optionally] block to wait for a + * notification to be pending, or ulTaskNotifyTake() to [optionally] block + * to wait for its notification value to have a non-zero value. The task does + * not consume any CPU time while it is in the Blocked state. + * + * See http://www.FreeRTOS.org/RTOS-task-notifications.html for details. + * + * @param xTaskToNotify The handle of the task being notified. The handle to a + * task can be returned from the xTaskCreate() API function used to create the + * task, and the handle of the currently running task can be obtained by calling + * xTaskGetCurrentTaskHandle(). + * + * @param ulValue Data that can be sent with the notification. How the data is + * used depends on the value of the eAction parameter. + * + * @param eAction Specifies how the notification updates the task's notification + * value, if at all. Valid values for eAction are as follows: + * + * eSetBits - + * The task's notification value is bitwise ORed with ulValue. xTaskNofify() + * always returns pdPASS in this case. + * + * eIncrement - + * The task's notification value is incremented. ulValue is not used and + * xTaskNotify() always returns pdPASS in this case. + * + * eSetValueWithOverwrite - + * The task's notification value is set to the value of ulValue, even if the + * task being notified had not yet processed the previous notification (the + * task already had a notification pending). xTaskNotify() always returns + * pdPASS in this case. + * + * eSetValueWithoutOverwrite - + * If the task being notified did not already have a notification pending then + * the task's notification value is set to ulValue and xTaskNotify() will + * return pdPASS. If the task being notified already had a notification + * pending then no action is performed and pdFAIL is returned. + * + * eNoAction - + * The task receives a notification without its notification value being + * updated. ulValue is not used and xTaskNotify() always returns pdPASS in + * this case. + * + * pulPreviousNotificationValue - + * Can be used to pass out the subject task's notification value before any + * bits are modified by the notify function. + * + * @return Dependent on the value of eAction. See the description of the + * eAction parameter. + * + * \defgroup xTaskNotify xTaskNotify + * \ingroup TaskNotifications + */ +BaseType_t xTaskGenericNotify( TaskHandle_t xTaskToNotify, uint32_t ulValue, eNotifyAction eAction, uint32_t *pulPreviousNotificationValue ) PRIVILEGED_FUNCTION; +#define xTaskNotify( xTaskToNotify, ulValue, eAction ) xTaskGenericNotify( ( xTaskToNotify ), ( ulValue ), ( eAction ), NULL ) +#define xTaskNotifyAndQuery( xTaskToNotify, ulValue, eAction, pulPreviousNotifyValue ) xTaskGenericNotify( ( xTaskToNotify ), ( ulValue ), ( eAction ), ( pulPreviousNotifyValue ) ) + +/** + * task. h + *
BaseType_t xTaskNotifyFromISR( TaskHandle_t xTaskToNotify, uint32_t ulValue, eNotifyAction eAction, BaseType_t *pxHigherPriorityTaskWoken );
+ * + * configUSE_TASK_NOTIFICATIONS must be undefined or defined as 1 for this + * function to be available. + * + * When configUSE_TASK_NOTIFICATIONS is set to one each task has its own private + * "notification value", which is a 32-bit unsigned integer (uint32_t). + * + * A version of xTaskNotify() that can be used from an interrupt service routine + * (ISR). + * + * Events can be sent to a task using an intermediary object. Examples of such + * objects are queues, semaphores, mutexes and event groups. Task notifications + * are a method of sending an event directly to a task without the need for such + * an intermediary object. + * + * A notification sent to a task can optionally perform an action, such as + * update, overwrite or increment the task's notification value. In that way + * task notifications can be used to send data to a task, or be used as light + * weight and fast binary or counting semaphores. + * + * A notification sent to a task will remain pending until it is cleared by the + * task calling xTaskNotifyWait() or ulTaskNotifyTake(). If the task was + * already in the Blocked state to wait for a notification when the notification + * arrives then the task will automatically be removed from the Blocked state + * (unblocked) and the notification cleared. + * + * A task can use xTaskNotifyWait() to [optionally] block to wait for a + * notification to be pending, or ulTaskNotifyTake() to [optionally] block + * to wait for its notification value to have a non-zero value. The task does + * not consume any CPU time while it is in the Blocked state. + * + * See http://www.FreeRTOS.org/RTOS-task-notifications.html for details. + * + * @param xTaskToNotify The handle of the task being notified. The handle to a + * task can be returned from the xTaskCreate() API function used to create the + * task, and the handle of the currently running task can be obtained by calling + * xTaskGetCurrentTaskHandle(). + * + * @param ulValue Data that can be sent with the notification. How the data is + * used depends on the value of the eAction parameter. + * + * @param eAction Specifies how the notification updates the task's notification + * value, if at all. Valid values for eAction are as follows: + * + * eSetBits - + * The task's notification value is bitwise ORed with ulValue. xTaskNofify() + * always returns pdPASS in this case. + * + * eIncrement - + * The task's notification value is incremented. ulValue is not used and + * xTaskNotify() always returns pdPASS in this case. + * + * eSetValueWithOverwrite - + * The task's notification value is set to the value of ulValue, even if the + * task being notified had not yet processed the previous notification (the + * task already had a notification pending). xTaskNotify() always returns + * pdPASS in this case. + * + * eSetValueWithoutOverwrite - + * If the task being notified did not already have a notification pending then + * the task's notification value is set to ulValue and xTaskNotify() will + * return pdPASS. If the task being notified already had a notification + * pending then no action is performed and pdFAIL is returned. + * + * eNoAction - + * The task receives a notification without its notification value being + * updated. ulValue is not used and xTaskNotify() always returns pdPASS in + * this case. + * + * @param pxHigherPriorityTaskWoken xTaskNotifyFromISR() will set + * *pxHigherPriorityTaskWoken to pdTRUE if sending the notification caused the + * task to which the notification was sent to leave the Blocked state, and the + * unblocked task has a priority higher than the currently running task. If + * xTaskNotifyFromISR() sets this value to pdTRUE then a context switch should + * be requested before the interrupt is exited. How a context switch is + * requested from an ISR is dependent on the port - see the documentation page + * for the port in use. + * + * @return Dependent on the value of eAction. See the description of the + * eAction parameter. + * + * \defgroup xTaskNotify xTaskNotify + * \ingroup TaskNotifications + */ +BaseType_t xTaskGenericNotifyFromISR( TaskHandle_t xTaskToNotify, uint32_t ulValue, eNotifyAction eAction, uint32_t *pulPreviousNotificationValue, BaseType_t *pxHigherPriorityTaskWoken ) PRIVILEGED_FUNCTION; +#define xTaskNotifyFromISR( xTaskToNotify, ulValue, eAction, pxHigherPriorityTaskWoken ) xTaskGenericNotifyFromISR( ( xTaskToNotify ), ( ulValue ), ( eAction ), NULL, ( pxHigherPriorityTaskWoken ) ) +#define xTaskNotifyAndQueryFromISR( xTaskToNotify, ulValue, eAction, pulPreviousNotificationValue, pxHigherPriorityTaskWoken ) xTaskGenericNotifyFromISR( ( xTaskToNotify ), ( ulValue ), ( eAction ), ( pulPreviousNotificationValue ), ( pxHigherPriorityTaskWoken ) ) + +/** + * task. h + *
BaseType_t xTaskNotifyWait( uint32_t ulBitsToClearOnEntry, uint32_t ulBitsToClearOnExit, uint32_t *pulNotificationValue, TickType_t xTicksToWait );
+ * + * configUSE_TASK_NOTIFICATIONS must be undefined or defined as 1 for this + * function to be available. + * + * When configUSE_TASK_NOTIFICATIONS is set to one each task has its own private + * "notification value", which is a 32-bit unsigned integer (uint32_t). + * + * Events can be sent to a task using an intermediary object. Examples of such + * objects are queues, semaphores, mutexes and event groups. Task notifications + * are a method of sending an event directly to a task without the need for such + * an intermediary object. + * + * A notification sent to a task can optionally perform an action, such as + * update, overwrite or increment the task's notification value. In that way + * task notifications can be used to send data to a task, or be used as light + * weight and fast binary or counting semaphores. + * + * A notification sent to a task will remain pending until it is cleared by the + * task calling xTaskNotifyWait() or ulTaskNotifyTake(). If the task was + * already in the Blocked state to wait for a notification when the notification + * arrives then the task will automatically be removed from the Blocked state + * (unblocked) and the notification cleared. + * + * A task can use xTaskNotifyWait() to [optionally] block to wait for a + * notification to be pending, or ulTaskNotifyTake() to [optionally] block + * to wait for its notification value to have a non-zero value. The task does + * not consume any CPU time while it is in the Blocked state. + * + * See http://www.FreeRTOS.org/RTOS-task-notifications.html for details. + * + * @param ulBitsToClearOnEntry Bits that are set in ulBitsToClearOnEntry value + * will be cleared in the calling task's notification value before the task + * checks to see if any notifications are pending, and optionally blocks if no + * notifications are pending. Setting ulBitsToClearOnEntry to ULONG_MAX (if + * limits.h is included) or 0xffffffffUL (if limits.h is not included) will have + * the effect of resetting the task's notification value to 0. Setting + * ulBitsToClearOnEntry to 0 will leave the task's notification value unchanged. + * + * @param ulBitsToClearOnExit If a notification is pending or received before + * the calling task exits the xTaskNotifyWait() function then the task's + * notification value (see the xTaskNotify() API function) is passed out using + * the pulNotificationValue parameter. Then any bits that are set in + * ulBitsToClearOnExit will be cleared in the task's notification value (note + * *pulNotificationValue is set before any bits are cleared). Setting + * ulBitsToClearOnExit to ULONG_MAX (if limits.h is included) or 0xffffffffUL + * (if limits.h is not included) will have the effect of resetting the task's + * notification value to 0 before the function exits. Setting + * ulBitsToClearOnExit to 0 will leave the task's notification value unchanged + * when the function exits (in which case the value passed out in + * pulNotificationValue will match the task's notification value). + * + * @param pulNotificationValue Used to pass the task's notification value out + * of the function. Note the value passed out will not be effected by the + * clearing of any bits caused by ulBitsToClearOnExit being non-zero. + * + * @param xTicksToWait The maximum amount of time that the task should wait in + * the Blocked state for a notification to be received, should a notification + * not already be pending when xTaskNotifyWait() was called. The task + * will not consume any processing time while it is in the Blocked state. This + * is specified in kernel ticks, the macro pdMS_TO_TICSK( value_in_ms ) can be + * used to convert a time specified in milliseconds to a time specified in + * ticks. + * + * @return If a notification was received (including notifications that were + * already pending when xTaskNotifyWait was called) then pdPASS is + * returned. Otherwise pdFAIL is returned. + * + * \defgroup xTaskNotifyWait xTaskNotifyWait + * \ingroup TaskNotifications + */ +BaseType_t xTaskNotifyWait( uint32_t ulBitsToClearOnEntry, uint32_t ulBitsToClearOnExit, uint32_t *pulNotificationValue, TickType_t xTicksToWait ) PRIVILEGED_FUNCTION; + +/** + * task. h + *
BaseType_t xTaskNotifyGive( TaskHandle_t xTaskToNotify );
* - * INCLUDE_uxTaskGetStackHighWaterMark must be set to 1 in FreeRTOSConfig.h for - * this function to be available. + * configUSE_TASK_NOTIFICATIONS must be undefined or defined as 1 for this macro + * to be available. * - * Returns the high water mark of the stack associated with xTask. That is, - * the minimum free stack space there has been (in words, so on a 32 bit machine - * a value of 1 means 4 bytes) since the task started. The smaller the returned - * number the closer the task has come to overflowing its stack. + * When configUSE_TASK_NOTIFICATIONS is set to one each task has its own private + * "notification value", which is a 32-bit unsigned integer (uint32_t). * - * @param xTask Handle of the task associated with the stack to be checked. - * Set xTask to NULL to check the stack of the calling task. + * Events can be sent to a task using an intermediary object. Examples of such + * objects are queues, semaphores, mutexes and event groups. Task notifications + * are a method of sending an event directly to a task without the need for such + * an intermediary object. * - * @return The smallest amount of free stack space there has been (in bytes) - * since the task referenced by xTask was created. + * A notification sent to a task can optionally perform an action, such as + * update, overwrite or increment the task's notification value. In that way + * task notifications can be used to send data to a task, or be used as light + * weight and fast binary or counting semaphores. + * + * xTaskNotifyGive() is a helper macro intended for use when task notifications + * are used as light weight and faster binary or counting semaphore equivalents. + * Actual FreeRTOS semaphores are given using the xSemaphoreGive() API function, + * the equivalent action that instead uses a task notification is + * xTaskNotifyGive(). + * + * When task notifications are being used as a binary or counting semaphore + * equivalent then the task being notified should wait for the notification + * using the ulTaskNotificationTake() API function rather than the + * xTaskNotifyWait() API function. + * + * See http://www.FreeRTOS.org/RTOS-task-notifications.html for more details. + * + * @param xTaskToNotify The handle of the task being notified. The handle to a + * task can be returned from the xTaskCreate() API function used to create the + * task, and the handle of the currently running task can be obtained by calling + * xTaskGetCurrentTaskHandle(). + * + * @return xTaskNotifyGive() is a macro that calls xTaskNotify() with the + * eAction parameter set to eIncrement - so pdPASS is always returned. + * + * \defgroup xTaskNotifyGive xTaskNotifyGive + * \ingroup TaskNotifications */ -unsigned portBASE_TYPE uxTaskGetStackHighWaterMark( xTaskHandle xTask ) PRIVILEGED_FUNCTION; - -/* When using trace macros it is sometimes necessary to include tasks.h before -FreeRTOS.h. When this is done pdTASK_HOOK_CODE will not yet have been defined, -so the following two prototypes will cause a compilation error. This can be -fixed by simply guarding against the inclusion of these two prototypes unless -they are explicitly required by the configUSE_APPLICATION_TASK_TAG configuration -constant. */ -#ifdef configUSE_APPLICATION_TASK_TAG - #if configUSE_APPLICATION_TASK_TAG == 1 - /** - * task.h - *
void vTaskSetApplicationTaskTag( xTaskHandle xTask, pdTASK_HOOK_CODE pxHookFunction );
- * - * Sets pxHookFunction to be the task hook function used by the task xTask. - * Passing xTask as NULL has the effect of setting the calling tasks hook - * function. - */ - void vTaskSetApplicationTaskTag( xTaskHandle xTask, pdTASK_HOOK_CODE pxHookFunction ) PRIVILEGED_FUNCTION; +#define xTaskNotifyGive( xTaskToNotify ) xTaskGenericNotify( ( xTaskToNotify ), ( 0 ), eIncrement, NULL ) - /** - * task.h - *
void xTaskGetApplicationTaskTag( xTaskHandle xTask );
- * - * Returns the pxHookFunction value assigned to the task xTask. - */ - pdTASK_HOOK_CODE xTaskGetApplicationTaskTag( xTaskHandle xTask ) PRIVILEGED_FUNCTION; - #endif /* configUSE_APPLICATION_TASK_TAG ==1 */ -#endif /* ifdef configUSE_APPLICATION_TASK_TAG */ +/** + * task. h + *
void vTaskNotifyGiveFromISR( TaskHandle_t xTaskHandle, BaseType_t *pxHigherPriorityTaskWoken );
+ *
+ * configUSE_TASK_NOTIFICATIONS must be undefined or defined as 1 for this macro
+ * to be available.
+ *
+ * When configUSE_TASK_NOTIFICATIONS is set to one each task has its own private
+ * "notification value", which is a 32-bit unsigned integer (uint32_t).
+ *
+ * A version of xTaskNotifyGive() that can be called from an interrupt service
+ * routine (ISR).
+ *
+ * Events can be sent to a task using an intermediary object.  Examples of such
+ * objects are queues, semaphores, mutexes and event groups.  Task notifications
+ * are a method of sending an event directly to a task without the need for such
+ * an intermediary object.
+ *
+ * A notification sent to a task can optionally perform an action, such as
+ * update, overwrite or increment the task's notification value.  In that way
+ * task notifications can be used to send data to a task, or be used as light
+ * weight and fast binary or counting semaphores.
+ *
+ * vTaskNotifyGiveFromISR() is intended for use when task notifications are
+ * used as light weight and faster binary or counting semaphore equivalents.
+ * Actual FreeRTOS semaphores are given from an ISR using the
+ * xSemaphoreGiveFromISR() API function, the equivalent action that instead uses
+ * a task notification is vTaskNotifyGiveFromISR().
+ *
+ * When task notifications are being used as a binary or counting semaphore
+ * equivalent then the task being notified should wait for the notification
+ * using the ulTaskNotificationTake() API function rather than the
+ * xTaskNotifyWait() API function.
+ *
+ * See http://www.FreeRTOS.org/RTOS-task-notifications.html for more details.
+ *
+ * @param xTaskToNotify The handle of the task being notified.  The handle to a
+ * task can be returned from the xTaskCreate() API function used to create the
+ * task, and the handle of the currently running task can be obtained by calling
+ * xTaskGetCurrentTaskHandle().
+ *
+ * @param pxHigherPriorityTaskWoken  vTaskNotifyGiveFromISR() will set
+ * *pxHigherPriorityTaskWoken to pdTRUE if sending the notification caused the
+ * task to which the notification was sent to leave the Blocked state, and the
+ * unblocked task has a priority higher than the currently running task.  If
+ * vTaskNotifyGiveFromISR() sets this value to pdTRUE then a context switch
+ * should be requested before the interrupt is exited.  How a context switch is
+ * requested from an ISR is dependent on the port - see the documentation page
+ * for the port in use.
+ *
+ * \defgroup xTaskNotifyWait xTaskNotifyWait
+ * \ingroup TaskNotifications
+ */
+void vTaskNotifyGiveFromISR( TaskHandle_t xTaskToNotify, BaseType_t *pxHigherPriorityTaskWoken ) PRIVILEGED_FUNCTION;
 
 /**
- * task.h
- * 
portBASE_TYPE xTaskCallApplicationTaskHook( xTaskHandle xTask, pdTASK_HOOK_CODE pxHookFunction );
- * - * Calls the hook function associated with xTask. Passing xTask as NULL has - * the effect of calling the Running tasks (the calling task) hook function. - * - * pvParameter is passed to the hook function for the task to interpret as it - * wants. + * task. h + *
uint32_t ulTaskNotifyTake( BaseType_t xClearCountOnExit, TickType_t xTicksToWait );
+ * + * configUSE_TASK_NOTIFICATIONS must be undefined or defined as 1 for this + * function to be available. + * + * When configUSE_TASK_NOTIFICATIONS is set to one each task has its own private + * "notification value", which is a 32-bit unsigned integer (uint32_t). + * + * Events can be sent to a task using an intermediary object. Examples of such + * objects are queues, semaphores, mutexes and event groups. Task notifications + * are a method of sending an event directly to a task without the need for such + * an intermediary object. + * + * A notification sent to a task can optionally perform an action, such as + * update, overwrite or increment the task's notification value. In that way + * task notifications can be used to send data to a task, or be used as light + * weight and fast binary or counting semaphores. + * + * ulTaskNotifyTake() is intended for use when a task notification is used as a + * faster and lighter weight binary or counting semaphore alternative. Actual + * FreeRTOS semaphores are taken using the xSemaphoreTake() API function, the + * equivalent action that instead uses a task notification is + * ulTaskNotifyTake(). + * + * When a task is using its notification value as a binary or counting semaphore + * other tasks should send notifications to it using the xTaskNotifyGive() + * macro, or xTaskNotify() function with the eAction parameter set to + * eIncrement. + * + * ulTaskNotifyTake() can either clear the task's notification value to + * zero on exit, in which case the notification value acts like a binary + * semaphore, or decrement the task's notification value on exit, in which case + * the notification value acts like a counting semaphore. + * + * A task can use ulTaskNotifyTake() to [optionally] block to wait for a + * the task's notification value to be non-zero. The task does not consume any + * CPU time while it is in the Blocked state. + * + * Where as xTaskNotifyWait() will return when a notification is pending, + * ulTaskNotifyTake() will return when the task's notification value is + * not zero. + * + * See http://www.FreeRTOS.org/RTOS-task-notifications.html for details. + * + * @param xClearCountOnExit if xClearCountOnExit is pdFALSE then the task's + * notification value is decremented when the function exits. In this way the + * notification value acts like a counting semaphore. If xClearCountOnExit is + * not pdFALSE then the task's notification value is cleared to zero when the + * function exits. In this way the notification value acts like a binary + * semaphore. + * + * @param xTicksToWait The maximum amount of time that the task should wait in + * the Blocked state for the task's notification value to be greater than zero, + * should the count not already be greater than zero when + * ulTaskNotifyTake() was called. The task will not consume any processing + * time while it is in the Blocked state. This is specified in kernel ticks, + * the macro pdMS_TO_TICSK( value_in_ms ) can be used to convert a time + * specified in milliseconds to a time specified in ticks. + * + * @return The task's notification count before it is either cleared to zero or + * decremented (see the xClearCountOnExit parameter). + * + * \defgroup ulTaskNotifyTake ulTaskNotifyTake + * \ingroup TaskNotifications */ -portBASE_TYPE xTaskCallApplicationTaskHook( xTaskHandle xTask, void *pvParameter ) PRIVILEGED_FUNCTION; +uint32_t ulTaskNotifyTake( BaseType_t xClearCountOnExit, TickType_t xTicksToWait ) PRIVILEGED_FUNCTION; /** - * xTaskGetIdleTaskHandle() is only available if - * INCLUDE_xTaskGetIdleTaskHandle is set to 1 in FreeRTOSConfig.h. + * task. h + *
BaseType_t xTaskNotifyStateClear( TaskHandle_t xTask );
* - * Simply returns the handle of the idle task. It is not valid to call - * xTaskGetIdleTaskHandle() before the scheduler has been started. + * If the notification state of the task referenced by the handle xTask is + * eNotified, then set the task's notification state to eNotWaitingNotification. + * The task's notification value is not altered. Set xTask to NULL to clear the + * notification state of the calling task. + * + * @return pdTRUE if the task's notification state was set to + * eNotWaitingNotification, otherwise pdFALSE. + * \defgroup xTaskNotifyStateClear xTaskNotifyStateClear + * \ingroup TaskNotifications */ -xTaskHandle xTaskGetIdleTaskHandle( void ); +BaseType_t xTaskNotifyStateClear( TaskHandle_t xTask ); /*----------------------------------------------------------- * SCHEDULER INTERNALS AVAILABLE FOR PORTING PURPOSES @@ -1210,9 +1840,14 @@ xTaskHandle xTaskGetIdleTaskHandle( void ); * Called from the real time kernel tick (either preemptive or cooperative), * this increments the tick count and checks if any tasks that are blocked * for a finite period required removing from a blocked list and placing on - * a ready list. + * a ready list. If a non-zero value is returned then a context switch is + * required because either: + * + A task was removed from a blocked list because its timeout had expired, + * or + * + Time slicing is in use and there is a task of equal priority to the + * currently running task. */ -void vTaskIncrementTick( void ) PRIVILEGED_FUNCTION; +BaseType_t xTaskIncrementTick( void ) PRIVILEGED_FUNCTION; /* * THIS FUNCTION MUST NOT BE USED FROM APPLICATION CODE. IT IS AN @@ -1227,15 +1862,26 @@ void vTaskIncrementTick( void ) PRIVILEGED_FUNCTION; * there be no higher priority tasks waiting on the same event) or * the delay period expires. * + * The 'unordered' version replaces the event list item value with the + * xItemValue value, and inserts the list item at the end of the list. + * + * The 'ordered' version uses the existing event list item value (which is the + * owning tasks priority) to insert the list item into the event list is task + * priority order. + * * @param pxEventList The list containing tasks that are blocked waiting * for the event to occur. * + * @param xItemValue The item value to use for the event list item when the + * event list is not ordered by task priority. + * * @param xTicksToWait The maximum amount of time that the task should wait * for the event to occur. This is specified in kernel ticks,the constant - * portTICK_RATE_MS can be used to convert kernel ticks into a real time + * portTICK_PERIOD_MS can be used to convert kernel ticks into a real time * period. */ -void vTaskPlaceOnEventList( const xList * const pxEventList, portTickType xTicksToWait ) PRIVILEGED_FUNCTION; +void vTaskPlaceOnEventList( List_t * const pxEventList, const TickType_t xTicksToWait ) PRIVILEGED_FUNCTION; +void vTaskPlaceOnUnorderedEventList( List_t * pxEventList, const TickType_t xItemValue, const TickType_t xTicksToWait ) PRIVILEGED_FUNCTION; /* * THIS FUNCTION MUST NOT BE USED FROM APPLICATION CODE. IT IS AN @@ -1247,10 +1893,8 @@ void vTaskPlaceOnEventList( const xList * const pxEventList, portTickType xTicks * The difference being that this function does not permit tasks to block * indefinitely, whereas vTaskPlaceOnEventList() does. * - * @return pdTRUE if the task being removed has a higher priority than the task - * making the call, otherwise pdFALSE. */ -void vTaskPlaceOnEventListRestricted( const xList * const pxEventList, portTickType xTicksToWait ) PRIVILEGED_FUNCTION; +void vTaskPlaceOnEventListRestricted( List_t * const pxEventList, const TickType_t xTicksToWait, const BaseType_t xWaitIndefinitely ) PRIVILEGED_FUNCTION; /* * THIS FUNCTION MUST NOT BE USED FROM APPLICATION CODE. IT IS AN @@ -1261,13 +1905,23 @@ void vTaskPlaceOnEventListRestricted( const xList * const pxEventList, portTickT * Removes a task from both the specified event list and the list of blocked * tasks, and places it on a ready queue. * - * xTaskRemoveFromEventList () will be called if either an event occurs to - * unblock a task, or the block timeout period expires. + * xTaskRemoveFromEventList()/xTaskRemoveFromUnorderedEventList() will be called + * if either an event occurs to unblock a task, or the block timeout period + * expires. + * + * xTaskRemoveFromEventList() is used when the event list is in task priority + * order. It removes the list item from the head of the event list as that will + * have the highest priority owning task of all the tasks on the event list. + * xTaskRemoveFromUnorderedEventList() is used when the event list is not + * ordered and the event list items hold something other than the owning tasks + * priority. In this case the event list item value is updated to the value + * passed in the xItemValue parameter. * * @return pdTRUE if the task being removed has a higher priority than the task * making the call, otherwise pdFALSE. */ -signed portBASE_TYPE xTaskRemoveFromEventList( const xList * const pxEventList ) PRIVILEGED_FUNCTION; +BaseType_t xTaskRemoveFromEventList( const List_t * const pxEventList ) PRIVILEGED_FUNCTION; +BaseType_t xTaskRemoveFromUnorderedEventList( ListItem_t * pxEventListItem, const TickType_t xItemValue ) PRIVILEGED_FUNCTION; /* * THIS FUNCTION MUST NOT BE USED FROM APPLICATION CODE. IT IS ONLY @@ -1279,21 +1933,27 @@ signed portBASE_TYPE xTaskRemoveFromEventList( const xList * const pxEventList ) */ void vTaskSwitchContext( void ) PRIVILEGED_FUNCTION; +/* + * THESE FUNCTIONS MUST NOT BE USED FROM APPLICATION CODE. THEY ARE USED BY + * THE EVENT BITS MODULE. + */ +TickType_t uxTaskResetEventItemValue( void ) PRIVILEGED_FUNCTION; + /* * Return the handle of the calling task. */ -xTaskHandle xTaskGetCurrentTaskHandle( void ) PRIVILEGED_FUNCTION; +TaskHandle_t xTaskGetCurrentTaskHandle( void ) PRIVILEGED_FUNCTION; /* * Capture the current time status for future reference. */ -void vTaskSetTimeOutState( xTimeOutType * const pxTimeOut ) PRIVILEGED_FUNCTION; +void vTaskSetTimeOutState( TimeOut_t * const pxTimeOut ) PRIVILEGED_FUNCTION; /* * Compare the time status now with that previously captured to see if the * timeout has expired. */ -portBASE_TYPE xTaskCheckForTimeOut( xTimeOutType * const pxTimeOut, portTickType * const pxTicksToWait ) PRIVILEGED_FUNCTION; +BaseType_t xTaskCheckForTimeOut( TimeOut_t * const pxTimeOut, TickType_t * const pxTicksToWait ) PRIVILEGED_FUNCTION; /* * Shortcut used by the queue implementation to prevent unnecessary call to @@ -1305,47 +1965,49 @@ void vTaskMissedYield( void ) PRIVILEGED_FUNCTION; * Returns the scheduler state as taskSCHEDULER_RUNNING, * taskSCHEDULER_NOT_STARTED or taskSCHEDULER_SUSPENDED. */ -portBASE_TYPE xTaskGetSchedulerState( void ) PRIVILEGED_FUNCTION; +BaseType_t xTaskGetSchedulerState( void ) PRIVILEGED_FUNCTION; /* * Raises the priority of the mutex holder to that of the calling task should * the mutex holder have a priority less than the calling task. */ -void vTaskPriorityInherit( xTaskHandle * const pxMutexHolder ) PRIVILEGED_FUNCTION; +void vTaskPriorityInherit( TaskHandle_t const pxMutexHolder ) PRIVILEGED_FUNCTION; /* * Set the priority of a task back to its proper priority in the case that it * inherited a higher priority while it was holding a semaphore. */ -void vTaskPriorityDisinherit( xTaskHandle * const pxMutexHolder ) PRIVILEGED_FUNCTION; +BaseType_t xTaskPriorityDisinherit( TaskHandle_t const pxMutexHolder ) PRIVILEGED_FUNCTION; /* * Generic version of the task creation function which is in turn called by the * xTaskCreate() and xTaskCreateRestricted() macros. */ -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 ) PRIVILEGED_FUNCTION; +BaseType_t xTaskGenericCreate( TaskFunction_t pxTaskCode, const char * const pcName, const uint16_t usStackDepth, void * const pvParameters, UBaseType_t uxPriority, TaskHandle_t * const pxCreatedTask, StackType_t * const puxStackBuffer, const MemoryRegion_t * const xRegions ) PRIVILEGED_FUNCTION; /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ /* * Get the uxTCBNumber assigned to the task referenced by the xTask parameter. */ -unsigned portBASE_TYPE uxTaskGetTaskNumber( xTaskHandle xTask ); +UBaseType_t uxTaskGetTaskNumber( TaskHandle_t xTask ) PRIVILEGED_FUNCTION; /* - * Set the uxTCBNumber of the task referenced by the xTask parameter to - * ucHandle. + * Set the uxTaskNumber of the task referenced by the xTask parameter to + * uxHandle. */ -void vTaskSetTaskNumber( xTaskHandle xTask, unsigned portBASE_TYPE uxHandle ); +void vTaskSetTaskNumber( TaskHandle_t xTask, const UBaseType_t uxHandle ) PRIVILEGED_FUNCTION; /* + * Only available when configUSE_TICKLESS_IDLE is set to 1. * If tickless mode is being used, or a low power mode is implemented, then * the tick interrupt will not execute during idle periods. When this is the * case, the tick count value maintained by the scheduler needs to be kept up - * to date with the actual execution time by being skipped forward by the by - * a time equal to the idle period. + * to date with the actual execution time by being skipped forward by a time + * equal to the idle period. */ -void vTaskStepTick( portTickType xTicksToJump ); +void vTaskStepTick( const TickType_t xTicksToJump ) PRIVILEGED_FUNCTION; /* + * Only avilable when configUSE_TICKLESS_IDLE is set to 1. * Provided for use within portSUPPRESS_TICKS_AND_SLEEP() to allow the port * specific sleep function to determine if it is ok to proceed with the sleep, * and if it is ok to proceed, if it is ok to sleep indefinitely. @@ -1358,12 +2020,18 @@ void vTaskStepTick( portTickType xTicksToJump ); * critical section between the timer being stopped and the sleep mode being * entered to ensure it is ok to proceed into the sleep mode. */ -eSleepModeStatus eTaskConfirmSleepModeStatus( void ); +eSleepModeStatus eTaskConfirmSleepModeStatus( void ) PRIVILEGED_FUNCTION; + +/* + * For internal use only. Increment the mutex held count when a mutex is + * taken and return the handle of the task that has taken the mutex. + */ +void *pvTaskIncrementMutexHeldCount( void ) PRIVILEGED_FUNCTION; #ifdef __cplusplus } #endif -#endif /* TASK_H */ +#endif /* INC_TASK_H */ diff --git a/lib/FreeRTOS/include/timers.h b/lib/FreeRTOS/include/timers.h index b8bd73e593..3c6c6bf308 100644 --- a/lib/FreeRTOS/include/timers.h +++ b/lib/FreeRTOS/include/timers.h @@ -1,75 +1,70 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. + + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ @@ -80,43 +75,65 @@ #error "include FreeRTOS.h must appear in source files before include timers.h" #endif -#include "portable.h" -#include "list.h" +/*lint -e537 This headers are only multiply included if the application code +happens to also be including task.h. */ #include "task.h" +/*lint +e537 */ #ifdef __cplusplus extern "C" { #endif -/* IDs for commands that can be sent/received on the timer queue. These are to -be used solely through the macros that make up the public software timer API, -as defined below. */ -#define tmrCOMMAND_START 0 -#define tmrCOMMAND_STOP 1 -#define tmrCOMMAND_CHANGE_PERIOD 2 -#define tmrCOMMAND_DELETE 3 - /*----------------------------------------------------------- * MACROS AND DEFINITIONS *----------------------------------------------------------*/ - /** +/* IDs for commands that can be sent/received on the timer queue. These are to +be used solely through the macros that make up the public software timer API, +as defined below. The commands that are sent from interrupts must use the +highest numbers as tmrFIRST_FROM_ISR_COMMAND is used to determine if the task +or interrupt version of the queue send function should be used. */ +#define tmrCOMMAND_EXECUTE_CALLBACK_FROM_ISR ( ( BaseType_t ) -2 ) +#define tmrCOMMAND_EXECUTE_CALLBACK ( ( BaseType_t ) -1 ) +#define tmrCOMMAND_START_DONT_TRACE ( ( BaseType_t ) 0 ) +#define tmrCOMMAND_START ( ( BaseType_t ) 1 ) +#define tmrCOMMAND_RESET ( ( BaseType_t ) 2 ) +#define tmrCOMMAND_STOP ( ( BaseType_t ) 3 ) +#define tmrCOMMAND_CHANGE_PERIOD ( ( BaseType_t ) 4 ) +#define tmrCOMMAND_DELETE ( ( BaseType_t ) 5 ) + +#define tmrFIRST_FROM_ISR_COMMAND ( ( BaseType_t ) 6 ) +#define tmrCOMMAND_START_FROM_ISR ( ( BaseType_t ) 6 ) +#define tmrCOMMAND_RESET_FROM_ISR ( ( BaseType_t ) 7 ) +#define tmrCOMMAND_STOP_FROM_ISR ( ( BaseType_t ) 8 ) +#define tmrCOMMAND_CHANGE_PERIOD_FROM_ISR ( ( BaseType_t ) 9 ) + + +/** * Type by which software timers are referenced. For example, a call to - * xTimerCreate() returns an xTimerHandle variable that can then be used to + * xTimerCreate() returns an TimerHandle_t variable that can then be used to * reference the subject timer in calls to other software timer API functions * (for example, xTimerStart(), xTimerReset(), etc.). */ -typedef void * xTimerHandle; +typedef void * TimerHandle_t; + +/* + * Defines the prototype to which timer callback functions must conform. + */ +typedef void (*TimerCallbackFunction_t)( TimerHandle_t xTimer ); -/* Define the prototype to which timer callback functions must conform. */ -typedef void (*tmrTIMER_CALLBACK)( xTimerHandle xTimer ); +/* + * Defines the prototype to which functions used with the + * xTimerPendFunctionCallFromISR() function must conform. + */ +typedef void (*PendedFunction_t)( void *, uint32_t ); /** - * xTimerHandle xTimerCreate( const signed char *pcTimerName, - * portTickType xTimerPeriodInTicks, - * unsigned portBASE_TYPE uxAutoReload, + * TimerHandle_t xTimerCreate( const char * const pcTimerName, + * TickType_t xTimerPeriodInTicks, + * UBaseType_t uxAutoReload, * void * pvTimerID, - * tmrTIMER_CALLBACK pxCallbackFunction ); + * TimerCallbackFunction_t pxCallbackFunction ); * * Creates a new software timer instance. This allocates the storage required * by the new timer, initialises the new timers internal state, and returns a @@ -124,23 +141,24 @@ typedef void (*tmrTIMER_CALLBACK)( xTimerHandle xTimer ); * * Timers are created in the dormant state. The xTimerStart(), xTimerReset(), * xTimerStartFromISR(), xTimerResetFromISR(), xTimerChangePeriod() and - * xTimerChangePeriodFromISR() API functions can all be used to transition a timer into the - * active state. + * xTimerChangePeriodFromISR() API functions can all be used to transition a + * timer into the active state. * * @param pcTimerName A text name that is assigned to the timer. This is done - * purely to assist debugging. The kernel itself only ever references a timer by - * its handle, and never by its name. + * purely to assist debugging. The kernel itself only ever references a timer + * by its handle, and never by its name. * - * @param xTimerPeriodInTicks The timer period. The time is defined in tick periods so - * the constant portTICK_RATE_MS can be used to convert a time that has been - * specified in milliseconds. For example, if the timer must expire after 100 - * ticks, then xTimerPeriodInTicks should be set to 100. Alternatively, if the timer - * must expire after 500ms, then xPeriod can be set to ( 500 / portTICK_RATE_MS ) - * provided configTICK_RATE_HZ is less than or equal to 1000. + * @param xTimerPeriodInTicks The timer period. The time is defined in tick + * periods so the constant portTICK_PERIOD_MS can be used to convert a time that + * has been specified in milliseconds. For example, if the timer must expire + * after 100 ticks, then xTimerPeriodInTicks should be set to 100. + * Alternatively, if the timer must expire after 500ms, then xPeriod can be set + * to ( 500 / portTICK_PERIOD_MS ) provided configTICK_RATE_HZ is less than or + * equal to 1000. * * @param uxAutoReload If uxAutoReload is set to pdTRUE then the timer will - * expire repeatedly with a frequency set by the xTimerPeriodInTicks parameter. If - * uxAutoReload is set to pdFALSE then the timer will be a one-shot timer and + * expire repeatedly with a frequency set by the xTimerPeriodInTicks parameter. + * If uxAutoReload is set to pdFALSE then the timer will be a one-shot timer and * enter the dormant state after it expires. * * @param pvTimerID An identifier that is assigned to the timer being created. @@ -149,38 +167,38 @@ typedef void (*tmrTIMER_CALLBACK)( xTimerHandle xTimer ); * timer. * * @param pxCallbackFunction The function to call when the timer expires. - * Callback functions must have the prototype defined by tmrTIMER_CALLBACK, - * which is "void vCallbackFunction( xTimerHandle xTimer );". + * Callback functions must have the prototype defined by TimerCallbackFunction_t, + * which is "void vCallbackFunction( TimerHandle_t xTimer );". * - * @return If the timer is successfully create then a handle to the newly + * @return If the timer is successfully created then a handle to the newly * created timer is returned. If the timer cannot be created (because either * there is insufficient FreeRTOS heap remaining to allocate the timer - * structures, or the timer period was set to 0) then 0 is returned. + * structures, or the timer period was set to 0) then NULL is returned. * * Example usage: - * + * @verbatim * #define NUM_TIMERS 5 * * // An array to hold handles to the created timers. - * xTimerHandle xTimers[ NUM_TIMERS ]; + * TimerHandle_t xTimers[ NUM_TIMERS ]; * * // An array to hold a count of the number of times each timer expires. - * long lExpireCounters[ NUM_TIMERS ] = { 0 }; + * int32_t lExpireCounters[ NUM_TIMERS ] = { 0 }; * * // Define a callback function that will be used by multiple timer instances. * // The callback function does nothing but count the number of times the * // associated timer expires, and stop the timer once the timer has expired * // 10 times. - * void vTimerCallback( xTimerHandle pxTimer ) + * void vTimerCallback( TimerHandle_t pxTimer ) * { - * long lArrayIndex; - * const long xMaxExpiryCountBeforeStopping = 10; + * int32_t lArrayIndex; + * const int32_t xMaxExpiryCountBeforeStopping = 10; * * // Optionally do something if the pxTimer parameter is NULL. * configASSERT( pxTimer ); - * + * * // Which timer expired? - * lArrayIndex = ( long ) pvTimerGetTimerID( pxTimer ); + * lArrayIndex = ( int32_t ) pvTimerGetTimerID( pxTimer ); * * // Increment the number of times that pxTimer has expired. * lExpireCounters[ lArrayIndex ] += 1; @@ -196,18 +214,18 @@ typedef void (*tmrTIMER_CALLBACK)( xTimerHandle xTimer ); * * void main( void ) * { - * long x; + * int32_t x; * * // Create then start some timers. Starting the timers before the scheduler * // has been started means the timers will start running immediately that * // the scheduler starts. * for( x = 0; x < NUM_TIMERS; x++ ) * { - * xTimers[ x ] = xTimerCreate( "Timer", // Just a text name, not used by the kernel. - * ( 100 * x ), // The timer period in ticks. - * pdTRUE, // The timers will auto-reload themselves when they expire. - * ( void * ) x, // Assign each timer a unique id equal to its array index. - * vTimerCallback // Each timer calls the same callback when it expires. + * xTimers[ x ] = xTimerCreate( "Timer", // Just a text name, not used by the kernel. + * ( 100 * x ), // The timer period in ticks. + * pdTRUE, // The timers will auto-reload themselves when they expire. + * ( void * ) x, // Assign each timer a unique id equal to its array index. + * vTimerCallback // Each timer calls the same callback when it expires. * ); * * if( xTimers[ x ] == NULL ) @@ -237,20 +255,21 @@ typedef void (*tmrTIMER_CALLBACK)( xTimerHandle xTimer ); * // Should not reach here. * for( ;; ); * } + * @endverbatim */ -xTimerHandle xTimerCreate( const signed char * const pcTimerName, portTickType xTimerPeriodInTicks, unsigned portBASE_TYPE uxAutoReload, void * pvTimerID, tmrTIMER_CALLBACK pxCallbackFunction ) PRIVILEGED_FUNCTION; +TimerHandle_t xTimerCreate( const char * const pcTimerName, const TickType_t xTimerPeriodInTicks, const UBaseType_t uxAutoReload, void * const pvTimerID, TimerCallbackFunction_t pxCallbackFunction ) PRIVILEGED_FUNCTION; /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ /** - * void *pvTimerGetTimerID( xTimerHandle xTimer ); + * void *pvTimerGetTimerID( TimerHandle_t xTimer ); * * Returns the ID assigned to the timer. * * IDs are assigned to timers using the pvTimerID parameter of the call to - * xTimerCreated() that was used to create the timer. + * xTimerCreated() that was used to create the timer, and by calling the + * vTimerSetTimerID() API function. * * If the same callback function is assigned to multiple timers then the timer - * ID can be used within the callback function to identify which timer actually - * expired. + * ID can be used as time specific (timer local) storage. * * @param xTimer The timer being queried. * @@ -260,16 +279,37 @@ xTimerHandle xTimerCreate( const signed char * const pcTimerName, portTickType x * * See the xTimerCreate() API function example usage scenario. */ -void *pvTimerGetTimerID( xTimerHandle xTimer ) PRIVILEGED_FUNCTION; +void *pvTimerGetTimerID( const TimerHandle_t xTimer ) PRIVILEGED_FUNCTION; /** - * portBASE_TYPE xTimerIsTimerActive( xTimerHandle xTimer ); + * void vTimerSetTimerID( TimerHandle_t xTimer, void *pvNewID ); + * + * Sets the ID assigned to the timer. + * + * IDs are assigned to timers using the pvTimerID parameter of the call to + * xTimerCreated() that was used to create the timer. + * + * If the same callback function is assigned to multiple timers then the timer + * ID can be used as time specific (timer local) storage. + * + * @param xTimer The timer being updated. + * + * @param pvNewID The ID to assign to the timer. + * + * Example usage: + * + * See the xTimerCreate() API function example usage scenario. + */ +void vTimerSetTimerID( TimerHandle_t xTimer, void *pvNewID ) PRIVILEGED_FUNCTION; + +/** + * BaseType_t xTimerIsTimerActive( TimerHandle_t xTimer ); * * Queries a timer to see if it is active or dormant. * * A timer will be dormant if: * 1) It has been created but not started, or - * 2) It is an expired on-shot timer that has not been restarted. + * 2) It is an expired one-shot timer that has not been restarted. * * Timers are created in the dormant state. The xTimerStart(), xTimerReset(), * xTimerStartFromISR(), xTimerResetFromISR(), xTimerChangePeriod() and @@ -282,9 +322,9 @@ void *pvTimerGetTimerID( xTimerHandle xTimer ) PRIVILEGED_FUNCTION; * pdFALSE will be returned if the timer is active. * * Example usage: - * + * @verbatim * // This function assumes xTimer has already been created. - * void vAFunction( xTimerHandle xTimer ) + * void vAFunction( TimerHandle_t xTimer ) * { * if( xTimerIsTimerActive( xTimer ) != pdFALSE ) // or more simply and equivalently "if( xTimerIsTimerActive( xTimer ) )" * { @@ -295,24 +335,27 @@ void *pvTimerGetTimerID( xTimerHandle xTimer ) PRIVILEGED_FUNCTION; * // xTimer is not active, do something else. * } * } + * @endverbatim */ -portBASE_TYPE xTimerIsTimerActive( xTimerHandle xTimer ) PRIVILEGED_FUNCTION; +BaseType_t xTimerIsTimerActive( TimerHandle_t xTimer ) PRIVILEGED_FUNCTION; /** - * xTimerGetTimerDaemonTaskHandle() is only available if + * TaskHandle_t xTimerGetTimerDaemonTaskHandle( void ); + * + * xTimerGetTimerDaemonTaskHandle() is only available if * INCLUDE_xTimerGetTimerDaemonTaskHandle is set to 1 in FreeRTOSConfig.h. * * Simply returns the handle of the timer service/daemon task. It it not valid * to call xTimerGetTimerDaemonTaskHandle() before the scheduler has been started. */ -xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); +TaskHandle_t xTimerGetTimerDaemonTaskHandle( void ) PRIVILEGED_FUNCTION; /** - * portBASE_TYPE xTimerStart( xTimerHandle xTimer, portTickType xBlockTime ); + * BaseType_t xTimerStart( TimerHandle_t xTimer, TickType_t xTicksToWait ); * * Timer functionality is provided by a timer service/daemon task. Many of the * public FreeRTOS timer API functions send commands to the timer service task - * though a queue called the timer command queue. The timer command queue is + * through a queue called the timer command queue. The timer command queue is * private to the kernel itself and is not directly accessible to application * code. The length of the timer command queue is set by the * configTIMER_QUEUE_LENGTH configuration constant. @@ -337,14 +380,14 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * * @param xTimer The handle of the timer being started/restarted. * - * @param xBlockTime Specifies the time, in ticks, that the calling task should + * @param xTicksToWait Specifies the time, in ticks, that the calling task should * be held in the Blocked state to wait for the start command to be successfully * sent to the timer command queue, should the queue already be full when - * xTimerStart() was called. xBlockTime is ignored if xTimerStart() is called + * xTimerStart() was called. xTicksToWait is ignored if xTimerStart() is called * before the scheduler is started. * * @return pdFAIL will be returned if the start command could not be sent to - * the timer command queue even after xBlockTime ticks had passed. pdPASS will + * the timer command queue even after xTicksToWait ticks had passed. pdPASS will * be returned if the command was successfully sent to the timer command queue. * When the command is actually processed will depend on the priority of the * timer service/daemon task relative to other tasks in the system, although the @@ -357,14 +400,14 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * See the xTimerCreate() API function example usage scenario. * */ -#define xTimerStart( xTimer, xBlockTime ) xTimerGenericCommand( ( xTimer ), tmrCOMMAND_START, ( xTaskGetTickCount() ), NULL, ( xBlockTime ) ) +#define xTimerStart( xTimer, xTicksToWait ) xTimerGenericCommand( ( xTimer ), tmrCOMMAND_START, ( xTaskGetTickCount() ), NULL, ( xTicksToWait ) ) /** - * portBASE_TYPE xTimerStop( xTimerHandle xTimer, portTickType xBlockTime ); + * BaseType_t xTimerStop( TimerHandle_t xTimer, TickType_t xTicksToWait ); * * Timer functionality is provided by a timer service/daemon task. Many of the * public FreeRTOS timer API functions send commands to the timer service task - * though a queue called the timer command queue. The timer command queue is + * through a queue called the timer command queue. The timer command queue is * private to the kernel itself and is not directly accessible to application * code. The length of the timer command queue is set by the * configTIMER_QUEUE_LENGTH configuration constant. @@ -380,14 +423,14 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * * @param xTimer The handle of the timer being stopped. * - * @param xBlockTime Specifies the time, in ticks, that the calling task should + * @param xTicksToWait Specifies the time, in ticks, that the calling task should * be held in the Blocked state to wait for the stop command to be successfully * sent to the timer command queue, should the queue already be full when - * xTimerStop() was called. xBlockTime is ignored if xTimerStop() is called + * xTimerStop() was called. xTicksToWait is ignored if xTimerStop() is called * before the scheduler is started. * * @return pdFAIL will be returned if the stop command could not be sent to - * the timer command queue even after xBlockTime ticks had passed. pdPASS will + * the timer command queue even after xTicksToWait ticks had passed. pdPASS will * be returned if the command was successfully sent to the timer command queue. * When the command is actually processed will depend on the priority of the * timer service/daemon task relative to other tasks in the system. The timer @@ -399,16 +442,16 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * See the xTimerCreate() API function example usage scenario. * */ -#define xTimerStop( xTimer, xBlockTime ) xTimerGenericCommand( ( xTimer ), tmrCOMMAND_STOP, 0U, NULL, ( xBlockTime ) ) +#define xTimerStop( xTimer, xTicksToWait ) xTimerGenericCommand( ( xTimer ), tmrCOMMAND_STOP, 0U, NULL, ( xTicksToWait ) ) /** - * portBASE_TYPE xTimerChangePeriod( xTimerHandle xTimer, - * portTickType xNewPeriod, - * portTickType xBlockTime ); + * BaseType_t xTimerChangePeriod( TimerHandle_t xTimer, + * TickType_t xNewPeriod, + * TickType_t xTicksToWait ); * * Timer functionality is provided by a timer service/daemon task. Many of the * public FreeRTOS timer API functions send commands to the timer service task - * though a queue called the timer command queue. The timer command queue is + * through a queue called the timer command queue. The timer command queue is * private to the kernel itself and is not directly accessible to application * code. The length of the timer command queue is set by the * configTIMER_QUEUE_LENGTH configuration constant. @@ -425,21 +468,21 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * @param xTimer The handle of the timer that is having its period changed. * * @param xNewPeriod The new period for xTimer. Timer periods are specified in - * tick periods, so the constant portTICK_RATE_MS can be used to convert a time + * tick periods, so the constant portTICK_PERIOD_MS can be used to convert a time * that has been specified in milliseconds. For example, if the timer must * expire after 100 ticks, then xNewPeriod should be set to 100. Alternatively, * if the timer must expire after 500ms, then xNewPeriod can be set to - * ( 500 / portTICK_RATE_MS ) provided configTICK_RATE_HZ is less than + * ( 500 / portTICK_PERIOD_MS ) provided configTICK_RATE_HZ is less than * or equal to 1000. * - * @param xBlockTime Specifies the time, in ticks, that the calling task should + * @param xTicksToWait Specifies the time, in ticks, that the calling task should * be held in the Blocked state to wait for the change period command to be * successfully sent to the timer command queue, should the queue already be - * full when xTimerChangePeriod() was called. xBlockTime is ignored if + * full when xTimerChangePeriod() was called. xTicksToWait is ignored if * xTimerChangePeriod() is called before the scheduler is started. * * @return pdFAIL will be returned if the change period command could not be - * sent to the timer command queue even after xBlockTime ticks had passed. + * sent to the timer command queue even after xTicksToWait ticks had passed. * pdPASS will be returned if the command was successfully sent to the timer * command queue. When the command is actually processed will depend on the * priority of the timer service/daemon task relative to other tasks in the @@ -447,13 +490,13 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * configTIMER_TASK_PRIORITY configuration constant. * * Example usage: - * + * @verbatim * // This function assumes xTimer has already been created. If the timer * // referenced by xTimer is already active when it is called, then the timer * // is deleted. If the timer referenced by xTimer is not active when it is * // called, then the period of the timer is set to 500ms and the timer is * // started. - * void vAFunction( xTimerHandle xTimer ) + * void vAFunction( TimerHandle_t xTimer ) * { * if( xTimerIsTimerActive( xTimer ) != pdFALSE ) // or more simply and equivalently "if( xTimerIsTimerActive( xTimer ) )" * { @@ -466,7 +509,7 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * // cause the timer to start. Block for a maximum of 100 ticks if the * // change period command cannot immediately be sent to the timer * // command queue. - * if( xTimerChangePeriod( xTimer, 500 / portTICK_RATE_MS, 100 ) == pdPASS ) + * if( xTimerChangePeriod( xTimer, 500 / portTICK_PERIOD_MS, 100 ) == pdPASS ) * { * // The command was successfully sent. * } @@ -477,15 +520,16 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * } * } * } + * @endverbatim */ - #define xTimerChangePeriod( xTimer, xNewPeriod, xBlockTime ) xTimerGenericCommand( ( xTimer ), tmrCOMMAND_CHANGE_PERIOD, ( xNewPeriod ), NULL, ( xBlockTime ) ) + #define xTimerChangePeriod( xTimer, xNewPeriod, xTicksToWait ) xTimerGenericCommand( ( xTimer ), tmrCOMMAND_CHANGE_PERIOD, ( xNewPeriod ), NULL, ( xTicksToWait ) ) /** - * portBASE_TYPE xTimerDelete( xTimerHandle xTimer, portTickType xBlockTime ); + * BaseType_t xTimerDelete( TimerHandle_t xTimer, TickType_t xTicksToWait ); * * Timer functionality is provided by a timer service/daemon task. Many of the * public FreeRTOS timer API functions send commands to the timer service task - * though a queue called the timer command queue. The timer command queue is + * through a queue called the timer command queue. The timer command queue is * private to the kernel itself and is not directly accessible to application * code. The length of the timer command queue is set by the * configTIMER_QUEUE_LENGTH configuration constant. @@ -498,14 +542,14 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * * @param xTimer The handle of the timer being deleted. * - * @param xBlockTime Specifies the time, in ticks, that the calling task should + * @param xTicksToWait Specifies the time, in ticks, that the calling task should * be held in the Blocked state to wait for the delete command to be * successfully sent to the timer command queue, should the queue already be - * full when xTimerDelete() was called. xBlockTime is ignored if xTimerDelete() + * full when xTimerDelete() was called. xTicksToWait is ignored if xTimerDelete() * is called before the scheduler is started. * * @return pdFAIL will be returned if the delete command could not be sent to - * the timer command queue even after xBlockTime ticks had passed. pdPASS will + * the timer command queue even after xTicksToWait ticks had passed. pdPASS will * be returned if the command was successfully sent to the timer command queue. * When the command is actually processed will depend on the priority of the * timer service/daemon task relative to other tasks in the system. The timer @@ -516,14 +560,14 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * * See the xTimerChangePeriod() API function example usage scenario. */ -#define xTimerDelete( xTimer, xBlockTime ) xTimerGenericCommand( ( xTimer ), tmrCOMMAND_DELETE, 0U, NULL, ( xBlockTime ) ) +#define xTimerDelete( xTimer, xTicksToWait ) xTimerGenericCommand( ( xTimer ), tmrCOMMAND_DELETE, 0U, NULL, ( xTicksToWait ) ) /** - * portBASE_TYPE xTimerReset( xTimerHandle xTimer, portTickType xBlockTime ); + * BaseType_t xTimerReset( TimerHandle_t xTimer, TickType_t xTicksToWait ); * * Timer functionality is provided by a timer service/daemon task. Many of the * public FreeRTOS timer API functions send commands to the timer service task - * though a queue called the timer command queue. The timer command queue is + * through a queue called the timer command queue. The timer command queue is * private to the kernel itself and is not directly accessible to application * code. The length of the timer command queue is set by the * configTIMER_QUEUE_LENGTH configuration constant. @@ -550,14 +594,14 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * * @param xTimer The handle of the timer being reset/started/restarted. * - * @param xBlockTime Specifies the time, in ticks, that the calling task should + * @param xTicksToWait Specifies the time, in ticks, that the calling task should * be held in the Blocked state to wait for the reset command to be successfully * sent to the timer command queue, should the queue already be full when - * xTimerReset() was called. xBlockTime is ignored if xTimerReset() is called + * xTimerReset() was called. xTicksToWait is ignored if xTimerReset() is called * before the scheduler is started. * * @return pdFAIL will be returned if the reset command could not be sent to - * the timer command queue even after xBlockTime ticks had passed. pdPASS will + * the timer command queue even after xTicksToWait ticks had passed. pdPASS will * be returned if the command was successfully sent to the timer command queue. * When the command is actually processed will depend on the priority of the * timer service/daemon task relative to other tasks in the system, although the @@ -566,16 +610,16 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * configuration constant. * * Example usage: - * + * @verbatim * // When a key is pressed, an LCD back-light is switched on. If 5 seconds pass * // without a key being pressed, then the LCD back-light is switched off. In * // this case, the timer is a one-shot timer. * - * xTimerHandle xBacklightTimer = NULL; + * TimerHandle_t xBacklightTimer = NULL; * * // The callback function assigned to the one-shot timer. In this case the * // parameter is not used. - * void vBacklightTimerCallback( xTimerHandle pxTimer ) + * void vBacklightTimerCallback( TimerHandle_t pxTimer ) * { * // The timer expired, therefore 5 seconds must have passed since a key * // was pressed. Switch off the LCD back-light. @@ -601,12 +645,12 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * * void main( void ) * { - * long x; + * int32_t x; * * // Create then start the one-shot timer that is responsible for turning * // the back-light off if no keys are pressed within a 5 second period. * xBacklightTimer = xTimerCreate( "BacklightTimer", // Just a text name, not used by the kernel. - * ( 5000 / portTICK_RATE_MS), // The timer period in ticks. + * ( 5000 / portTICK_PERIOD_MS), // The timer period in ticks. * pdFALSE, // The timer is a one-shot timer. * 0, // The id is not used by the callback so can take any value. * vBacklightTimerCallback // The callback function that switches the LCD back-light off. @@ -638,12 +682,13 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * // Should not reach here. * for( ;; ); * } + * @endverbatim */ -#define xTimerReset( xTimer, xBlockTime ) xTimerGenericCommand( ( xTimer ), tmrCOMMAND_START, ( xTaskGetTickCount() ), NULL, ( xBlockTime ) ) +#define xTimerReset( xTimer, xTicksToWait ) xTimerGenericCommand( ( xTimer ), tmrCOMMAND_RESET, ( xTaskGetTickCount() ), NULL, ( xTicksToWait ) ) /** - * portBASE_TYPE xTimerStartFromISR( xTimerHandle xTimer, - * portBASE_TYPE *pxHigherPriorityTaskWoken ); + * BaseType_t xTimerStartFromISR( TimerHandle_t xTimer, + * BaseType_t *pxHigherPriorityTaskWoken ); * * A version of xTimerStart() that can be called from an interrupt service * routine. @@ -667,11 +712,12 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * successfully sent to the timer command queue. When the command is actually * processed will depend on the priority of the timer service/daemon task * relative to other tasks in the system, although the timers expiry time is - * relative to when xTimerStartFromISR() is actually called. The timer service/daemon - * task priority is set by the configTIMER_TASK_PRIORITY configuration constant. + * relative to when xTimerStartFromISR() is actually called. The timer + * service/daemon task priority is set by the configTIMER_TASK_PRIORITY + * configuration constant. * * Example usage: - * + * @verbatim * // This scenario assumes xBacklightTimer has already been created. When a * // key is pressed, an LCD back-light is switched on. If 5 seconds pass * // without a key being pressed, then the LCD back-light is switched off. In @@ -681,7 +727,7 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * * // The callback function assigned to the one-shot timer. In this case the * // parameter is not used. - * void vBacklightTimerCallback( xTimerHandle pxTimer ) + * void vBacklightTimerCallback( TimerHandle_t pxTimer ) * { * // The timer expired, therefore 5 seconds must have passed since a key * // was pressed. Switch off the LCD back-light. @@ -691,7 +737,7 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * // The key press interrupt service routine. * void vKeyPressEventInterruptHandler( void ) * { - * portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + * BaseType_t xHigherPriorityTaskWoken = pdFALSE; * * // Ensure the LCD back-light is on, then restart the timer that is * // responsible for turning the back-light off after 5 seconds of @@ -719,15 +765,16 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * if( xHigherPriorityTaskWoken != pdFALSE ) * { * // Call the interrupt safe yield function here (actual function - * // depends on the FreeRTOS port being used. + * // depends on the FreeRTOS port being used). * } * } + * @endverbatim */ -#define xTimerStartFromISR( xTimer, pxHigherPriorityTaskWoken ) xTimerGenericCommand( ( xTimer ), tmrCOMMAND_START, ( xTaskGetTickCountFromISR() ), ( pxHigherPriorityTaskWoken ), 0U ) +#define xTimerStartFromISR( xTimer, pxHigherPriorityTaskWoken ) xTimerGenericCommand( ( xTimer ), tmrCOMMAND_START_FROM_ISR, ( xTaskGetTickCountFromISR() ), ( pxHigherPriorityTaskWoken ), 0U ) /** - * portBASE_TYPE xTimerStopFromISR( xTimerHandle xTimer, - * portBASE_TYPE *pxHigherPriorityTaskWoken ); + * BaseType_t xTimerStopFromISR( TimerHandle_t xTimer, + * BaseType_t *pxHigherPriorityTaskWoken ); * * A version of xTimerStop() that can be called from an interrupt service * routine. @@ -754,14 +801,14 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * priority is set by the configTIMER_TASK_PRIORITY configuration constant. * * Example usage: - * + * @verbatim * // This scenario assumes xTimer has already been created and started. When * // an interrupt occurs, the timer should be simply stopped. * * // The interrupt service routine that stops the timer. * void vAnExampleInterruptServiceRoutine( void ) * { - * portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + * BaseType_t xHigherPriorityTaskWoken = pdFALSE; * * // The interrupt has occurred - simply stop the timer. * // xHigherPriorityTaskWoken was set to pdFALSE where it was defined @@ -781,16 +828,17 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * if( xHigherPriorityTaskWoken != pdFALSE ) * { * // Call the interrupt safe yield function here (actual function - * // depends on the FreeRTOS port being used. + * // depends on the FreeRTOS port being used). * } * } + * @endverbatim */ -#define xTimerStopFromISR( xTimer, pxHigherPriorityTaskWoken ) xTimerGenericCommand( ( xTimer ), tmrCOMMAND_STOP, 0, ( pxHigherPriorityTaskWoken ), 0U ) +#define xTimerStopFromISR( xTimer, pxHigherPriorityTaskWoken ) xTimerGenericCommand( ( xTimer ), tmrCOMMAND_STOP_FROM_ISR, 0, ( pxHigherPriorityTaskWoken ), 0U ) /** - * portBASE_TYPE xTimerChangePeriodFromISR( xTimerHandle xTimer, - * portTickType xNewPeriod, - * portBASE_TYPE *pxHigherPriorityTaskWoken ); + * BaseType_t xTimerChangePeriodFromISR( TimerHandle_t xTimer, + * TickType_t xNewPeriod, + * BaseType_t *pxHigherPriorityTaskWoken ); * * A version of xTimerChangePeriod() that can be called from an interrupt * service routine. @@ -798,11 +846,11 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * @param xTimer The handle of the timer that is having its period changed. * * @param xNewPeriod The new period for xTimer. Timer periods are specified in - * tick periods, so the constant portTICK_RATE_MS can be used to convert a time + * tick periods, so the constant portTICK_PERIOD_MS can be used to convert a time * that has been specified in milliseconds. For example, if the timer must * expire after 100 ticks, then xNewPeriod should be set to 100. Alternatively, * if the timer must expire after 500ms, then xNewPeriod can be set to - * ( 500 / portTICK_RATE_MS ) provided configTICK_RATE_HZ is less than + * ( 500 / portTICK_PERIOD_MS ) provided configTICK_RATE_HZ is less than * or equal to 1000. * * @param pxHigherPriorityTaskWoken The timer service/daemon task spends most @@ -826,14 +874,14 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * priority is set by the configTIMER_TASK_PRIORITY configuration constant. * * Example usage: - * + * @verbatim * // This scenario assumes xTimer has already been created and started. When * // an interrupt occurs, the period of xTimer should be changed to 500ms. * * // The interrupt service routine that changes the period of xTimer. * void vAnExampleInterruptServiceRoutine( void ) * { - * portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + * BaseType_t xHigherPriorityTaskWoken = pdFALSE; * * // The interrupt has occurred - change the period of xTimer to 500ms. * // xHigherPriorityTaskWoken was set to pdFALSE where it was defined @@ -853,15 +901,16 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * if( xHigherPriorityTaskWoken != pdFALSE ) * { * // Call the interrupt safe yield function here (actual function - * // depends on the FreeRTOS port being used. + * // depends on the FreeRTOS port being used). * } * } + * @endverbatim */ -#define xTimerChangePeriodFromISR( xTimer, xNewPeriod, pxHigherPriorityTaskWoken ) xTimerGenericCommand( ( xTimer ), tmrCOMMAND_CHANGE_PERIOD, ( xNewPeriod ), ( pxHigherPriorityTaskWoken ), 0U ) +#define xTimerChangePeriodFromISR( xTimer, xNewPeriod, pxHigherPriorityTaskWoken ) xTimerGenericCommand( ( xTimer ), tmrCOMMAND_CHANGE_PERIOD_FROM_ISR, ( xNewPeriod ), ( pxHigherPriorityTaskWoken ), 0U ) /** - * portBASE_TYPE xTimerResetFromISR( xTimerHandle xTimer, - * portBASE_TYPE *pxHigherPriorityTaskWoken ); + * BaseType_t xTimerResetFromISR( TimerHandle_t xTimer, + * BaseType_t *pxHigherPriorityTaskWoken ); * * A version of xTimerReset() that can be called from an interrupt service * routine. @@ -890,7 +939,7 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * task priority is set by the configTIMER_TASK_PRIORITY configuration constant. * * Example usage: - * + * @verbatim * // This scenario assumes xBacklightTimer has already been created. When a * // key is pressed, an LCD back-light is switched on. If 5 seconds pass * // without a key being pressed, then the LCD back-light is switched off. In @@ -900,7 +949,7 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * * // The callback function assigned to the one-shot timer. In this case the * // parameter is not used. - * void vBacklightTimerCallback( xTimerHandle pxTimer ) + * void vBacklightTimerCallback( TimerHandle_t pxTimer ) * { * // The timer expired, therefore 5 seconds must have passed since a key * // was pressed. Switch off the LCD back-light. @@ -910,7 +959,7 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * // The key press interrupt service routine. * void vKeyPressEventInterruptHandler( void ) * { - * portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + * BaseType_t xHigherPriorityTaskWoken = pdFALSE; * * // Ensure the LCD back-light is on, then reset the timer that is * // responsible for turning the back-light off after 5 seconds of @@ -938,18 +987,155 @@ xTaskHandle xTimerGetTimerDaemonTaskHandle( void ); * if( xHigherPriorityTaskWoken != pdFALSE ) * { * // Call the interrupt safe yield function here (actual function - * // depends on the FreeRTOS port being used. + * // depends on the FreeRTOS port being used). * } * } + * @endverbatim + */ +#define xTimerResetFromISR( xTimer, pxHigherPriorityTaskWoken ) xTimerGenericCommand( ( xTimer ), tmrCOMMAND_RESET_FROM_ISR, ( xTaskGetTickCountFromISR() ), ( pxHigherPriorityTaskWoken ), 0U ) + + +/** + * BaseType_t xTimerPendFunctionCallFromISR( PendedFunction_t xFunctionToPend, + * void *pvParameter1, + * uint32_t ulParameter2, + * BaseType_t *pxHigherPriorityTaskWoken ); + * + * + * Used from application interrupt service routines to defer the execution of a + * function to the RTOS daemon task (the timer service task, hence this function + * is implemented in timers.c and is prefixed with 'Timer'). + * + * Ideally an interrupt service routine (ISR) is kept as short as possible, but + * sometimes an ISR either has a lot of processing to do, or needs to perform + * processing that is not deterministic. In these cases + * xTimerPendFunctionCallFromISR() can be used to defer processing of a function + * to the RTOS daemon task. + * + * A mechanism is provided that allows the interrupt to return directly to the + * task that will subsequently execute the pended callback function. This + * allows the callback function to execute contiguously in time with the + * interrupt - just as if the callback had executed in the interrupt itself. + * + * @param xFunctionToPend The function to execute from the timer service/ + * daemon task. The function must conform to the PendedFunction_t + * prototype. + * + * @param pvParameter1 The value of the callback function's first parameter. + * The parameter has a void * type to allow it to be used to pass any type. + * For example, unsigned longs can be cast to a void *, or the void * can be + * used to point to a structure. + * + * @param ulParameter2 The value of the callback function's second parameter. + * + * @param pxHigherPriorityTaskWoken As mentioned above, calling this function + * will result in a message being sent to the timer daemon task. If the + * priority of the timer daemon task (which is set using + * configTIMER_TASK_PRIORITY in FreeRTOSConfig.h) is higher than the priority of + * the currently running task (the task the interrupt interrupted) then + * *pxHigherPriorityTaskWoken will be set to pdTRUE within + * xTimerPendFunctionCallFromISR(), indicating that a context switch should be + * requested before the interrupt exits. For that reason + * *pxHigherPriorityTaskWoken must be initialised to pdFALSE. See the + * example code below. + * + * @return pdPASS is returned if the message was successfully sent to the + * timer daemon task, otherwise pdFALSE is returned. + * + * Example usage: + * @verbatim + * + * // The callback function that will execute in the context of the daemon task. + * // Note callback functions must all use this same prototype. + * void vProcessInterface( void *pvParameter1, uint32_t ulParameter2 ) + * { + * BaseType_t xInterfaceToService; + * + * // The interface that requires servicing is passed in the second + * // parameter. The first parameter is not used in this case. + * xInterfaceToService = ( BaseType_t ) ulParameter2; + * + * // ...Perform the processing here... + * } + * + * // An ISR that receives data packets from multiple interfaces + * void vAnISR( void ) + * { + * BaseType_t xInterfaceToService, xHigherPriorityTaskWoken; + * + * // Query the hardware to determine which interface needs processing. + * xInterfaceToService = prvCheckInterfaces(); + * + * // The actual processing is to be deferred to a task. Request the + * // vProcessInterface() callback function is executed, passing in the + * // number of the interface that needs processing. The interface to + * // service is passed in the second parameter. The first parameter is + * // not used in this case. + * xHigherPriorityTaskWoken = pdFALSE; + * xTimerPendFunctionCallFromISR( vProcessInterface, NULL, ( uint32_t ) xInterfaceToService, &xHigherPriorityTaskWoken ); + * + * // If xHigherPriorityTaskWoken is now set to pdTRUE then a context + * // switch should be requested. The macro used is port specific and will + * // be either portYIELD_FROM_ISR() or portEND_SWITCHING_ISR() - refer to + * // the documentation page for the port being used. + * portYIELD_FROM_ISR( xHigherPriorityTaskWoken ); + * + * } + * @endverbatim + */ +BaseType_t xTimerPendFunctionCallFromISR( PendedFunction_t xFunctionToPend, void *pvParameter1, uint32_t ulParameter2, BaseType_t *pxHigherPriorityTaskWoken ) PRIVILEGED_FUNCTION; + + /** + * BaseType_t xTimerPendFunctionCall( PendedFunction_t xFunctionToPend, + * void *pvParameter1, + * uint32_t ulParameter2, + * TickType_t xTicksToWait ); + * + * + * Used to defer the execution of a function to the RTOS daemon task (the timer + * service task, hence this function is implemented in timers.c and is prefixed + * with 'Timer'). + * + * @param xFunctionToPend The function to execute from the timer service/ + * daemon task. The function must conform to the PendedFunction_t + * prototype. + * + * @param pvParameter1 The value of the callback function's first parameter. + * The parameter has a void * type to allow it to be used to pass any type. + * For example, unsigned longs can be cast to a void *, or the void * can be + * used to point to a structure. + * + * @param ulParameter2 The value of the callback function's second parameter. + * + * @param xTicksToWait Calling this function will result in a message being + * sent to the timer daemon task on a queue. xTicksToWait is the amount of + * time the calling task should remain in the Blocked state (so not using any + * processing time) for space to become available on the timer queue if the + * queue is found to be full. + * + * @return pdPASS is returned if the message was successfully sent to the + * timer daemon task, otherwise pdFALSE is returned. + * + */ +BaseType_t xTimerPendFunctionCall( PendedFunction_t xFunctionToPend, void *pvParameter1, uint32_t ulParameter2, TickType_t xTicksToWait ) PRIVILEGED_FUNCTION; + +/** + * const char * const pcTimerGetTimerName( TimerHandle_t xTimer ); + * + * Returns the name that was assigned to a timer when the timer was created. + * + * @param xTimer The handle of the timer being queried. + * + * @return The name assigned to the timer specified by the xTimer parameter. */ -#define xTimerResetFromISR( xTimer, pxHigherPriorityTaskWoken ) xTimerGenericCommand( ( xTimer ), tmrCOMMAND_START, ( xTaskGetTickCountFromISR() ), ( pxHigherPriorityTaskWoken ), 0U ) +const char * pcTimerGetTimerName( TimerHandle_t xTimer ) PRIVILEGED_FUNCTION; /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ /* * Functions beyond this part are not part of the public API and are intended * for use by the kernel only. */ -portBASE_TYPE xTimerCreateTimerTask( void ) PRIVILEGED_FUNCTION; -portBASE_TYPE xTimerGenericCommand( xTimerHandle xTimer, portBASE_TYPE xCommandID, portTickType xOptionalValue, signed portBASE_TYPE *pxHigherPriorityTaskWoken, portTickType xBlockTime ) PRIVILEGED_FUNCTION; +BaseType_t xTimerCreateTimerTask( void ) PRIVILEGED_FUNCTION; +BaseType_t xTimerGenericCommand( TimerHandle_t xTimer, const BaseType_t xCommandID, const TickType_t xOptionalValue, BaseType_t * const pxHigherPriorityTaskWoken, const TickType_t xTicksToWait ) PRIVILEGED_FUNCTION; #ifdef __cplusplus } diff --git a/lib/FreeRTOS/list.c b/lib/FreeRTOS/list.c index 07baf5f4d2..ebacafddec 100644 --- a/lib/FreeRTOS/list.c +++ b/lib/FreeRTOS/list.c @@ -1,75 +1,70 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ @@ -81,12 +76,12 @@ * PUBLIC LIST API documented in list.h *----------------------------------------------------------*/ -void vListInitialise( xList *pxList ) +void vListInitialise( List_t * const pxList ) { /* The list structure contains a list item which is used to mark the end of the list. To initialise the list the list end is inserted as the only list entry. */ - pxList->pxIndex = ( xListItem * ) &( pxList->xListEnd ); + pxList->pxIndex = ( ListItem_t * ) &( pxList->xListEnd ); /*lint !e826 !e740 The mini list structure is used as the list end to save RAM. This is checked and valid. */ /* The list end value is the highest possible value in the list to ensure it remains at the end of the list. */ @@ -94,35 +89,51 @@ void vListInitialise( xList *pxList ) /* The list end next and previous pointers point to itself so we know when the list is empty. */ - pxList->xListEnd.pxNext = ( xListItem * ) &( pxList->xListEnd ); - pxList->xListEnd.pxPrevious = ( xListItem * ) &( pxList->xListEnd ); + pxList->xListEnd.pxNext = ( ListItem_t * ) &( pxList->xListEnd ); /*lint !e826 !e740 The mini list structure is used as the list end to save RAM. This is checked and valid. */ + pxList->xListEnd.pxPrevious = ( ListItem_t * ) &( pxList->xListEnd );/*lint !e826 !e740 The mini list structure is used as the list end to save RAM. This is checked and valid. */ + + pxList->uxNumberOfItems = ( UBaseType_t ) 0U; - pxList->uxNumberOfItems = ( unsigned portBASE_TYPE ) 0U; + /* Write known values into the list if + configUSE_LIST_DATA_INTEGRITY_CHECK_BYTES is set to 1. */ + listSET_LIST_INTEGRITY_CHECK_1_VALUE( pxList ); + listSET_LIST_INTEGRITY_CHECK_2_VALUE( pxList ); } /*-----------------------------------------------------------*/ -void vListInitialiseItem( xListItem *pxItem ) +void vListInitialiseItem( ListItem_t * const pxItem ) { /* Make sure the list item is not recorded as being on a list. */ pxItem->pvContainer = NULL; + + /* Write known values into the list item if + configUSE_LIST_DATA_INTEGRITY_CHECK_BYTES is set to 1. */ + listSET_FIRST_LIST_ITEM_INTEGRITY_CHECK_VALUE( pxItem ); + listSET_SECOND_LIST_ITEM_INTEGRITY_CHECK_VALUE( pxItem ); } /*-----------------------------------------------------------*/ -void vListInsertEnd( xList *pxList, xListItem *pxNewListItem ) +void vListInsertEnd( List_t * const pxList, ListItem_t * const pxNewListItem ) { -volatile xListItem * pxIndex; +ListItem_t * const pxIndex = pxList->pxIndex; + + /* Only effective when configASSERT() is also defined, these tests may catch + the list data structures being overwritten in memory. They will not catch + data errors caused by incorrect configuration or use of FreeRTOS. */ + listTEST_LIST_INTEGRITY( pxList ); + listTEST_LIST_ITEM_INTEGRITY( pxNewListItem ); /* Insert a new list item into pxList, but rather than sort the list, makes the new list item the last item to be removed by a call to - pvListGetOwnerOfNextEntry. This means it has to be the item pointed to by - the pxIndex member. */ - pxIndex = pxList->pxIndex; + listGET_OWNER_OF_NEXT_ENTRY(). */ + pxNewListItem->pxNext = pxIndex; + pxNewListItem->pxPrevious = pxIndex->pxPrevious; + + /* Only used during decision coverage testing. */ + mtCOVERAGE_TEST_DELAY(); - pxNewListItem->pxNext = pxIndex->pxNext; - pxNewListItem->pxPrevious = pxList->pxIndex; - pxIndex->pxNext->pxPrevious = ( volatile xListItem * ) pxNewListItem; - pxIndex->pxNext = ( volatile xListItem * ) pxNewListItem; - pxList->pxIndex = ( volatile xListItem * ) pxNewListItem; + pxIndex->pxPrevious->pxNext = pxNewListItem; + pxIndex->pxPrevious = pxNewListItem; /* Remember which list the item is in. */ pxNewListItem->pvContainer = ( void * ) pxList; @@ -131,21 +142,25 @@ volatile xListItem * pxIndex; } /*-----------------------------------------------------------*/ -void vListInsert( xList *pxList, xListItem *pxNewListItem ) +void vListInsert( List_t * const pxList, ListItem_t * const pxNewListItem ) { -volatile xListItem *pxIterator; -portTickType xValueOfInsertion; - - /* Insert the new list item into the list, sorted in ulListItem order. */ - xValueOfInsertion = pxNewListItem->xItemValue; - - /* If the list already contains a list item with the same item value then - the new list item should be placed after it. This ensures that TCB's which - are stored in ready lists (all of which have the same ulListItem value) - get an equal share of the CPU. However, if the xItemValue is the same as - the back marker the iteration loop below will not end. This means we need - to guard against this by checking the value first and modifying the - algorithm slightly if necessary. */ +ListItem_t *pxIterator; +const TickType_t xValueOfInsertion = pxNewListItem->xItemValue; + + /* Only effective when configASSERT() is also defined, these tests may catch + the list data structures being overwritten in memory. They will not catch + data errors caused by incorrect configuration or use of FreeRTOS. */ + listTEST_LIST_INTEGRITY( pxList ); + listTEST_LIST_ITEM_INTEGRITY( pxNewListItem ); + + /* Insert the new list item into the list, sorted in xItemValue order. + + If the list already contains a list item with the same item value then the + new list item should be placed after it. This ensures that TCB's which are + stored in ready lists (all of which have the same xItemValue value) get a + share of the CPU. However, if the xItemValue is the same as the back marker + the iteration loop below will not end. Therefore the value is checked + first, and the algorithm slightly modified if necessary. */ if( xValueOfInsertion == portMAX_DELAY ) { pxIterator = pxList->xListEnd.pxPrevious; @@ -153,32 +168,38 @@ portTickType xValueOfInsertion; else { /* *** NOTE *********************************************************** - If you find your application is crashing here then likely causes are: + If you find your application is crashing here then likely causes are + listed below. In addition see http://www.freertos.org/FAQHelp.html for + more tips, and ensure configASSERT() is defined! + http://www.freertos.org/a00110.html#configASSERT + 1) Stack overflow - see http://www.freertos.org/Stacks-and-stack-overflow-checking.html - 2) Incorrect interrupt priority assignment, especially on Cortex-M3 + 2) Incorrect interrupt priority assignment, especially on Cortex-M parts where numerically high priority values denote low actual - interrupt priories, which can seem counter intuitive. See - configMAX_SYSCALL_INTERRUPT_PRIORITY on http://www.freertos.org/a00110.html + interrupt priorities, which can seem counter intuitive. See + http://www.freertos.org/RTOS-Cortex-M3-M4.html and the definition + of configMAX_SYSCALL_INTERRUPT_PRIORITY on + http://www.freertos.org/a00110.html 3) Calling an API function from within a critical section or when - the scheduler is suspended. + the scheduler is suspended, or calling an API function that does + not end in "FromISR" from an interrupt. 4) Using a queue or semaphore before it has been initialised or before the scheduler has been started (are interrupts firing before vTaskStartScheduler() has been called?). - See http://www.freertos.org/FAQHelp.html for more tips. **********************************************************************/ - for( pxIterator = ( xListItem * ) &( pxList->xListEnd ); pxIterator->pxNext->xItemValue <= xValueOfInsertion; pxIterator = pxIterator->pxNext ) + for( pxIterator = ( ListItem_t * ) &( pxList->xListEnd ); pxIterator->pxNext->xItemValue <= xValueOfInsertion; pxIterator = pxIterator->pxNext ) /*lint !e826 !e740 The mini list structure is used as the list end to save RAM. This is checked and valid. */ { - /* There is nothing to do here, we are just iterating to the - wanted insertion position. */ + /* There is nothing to do here, just iterating to the wanted + insertion position. */ } } pxNewListItem->pxNext = pxIterator->pxNext; - pxNewListItem->pxNext->pxPrevious = ( volatile xListItem * ) pxNewListItem; + pxNewListItem->pxNext->pxPrevious = pxNewListItem; pxNewListItem->pxPrevious = pxIterator; - pxIterator->pxNext = ( volatile xListItem * ) pxNewListItem; + pxIterator->pxNext = pxNewListItem; /* Remember which list the item is in. This allows fast removal of the item later. */ @@ -188,22 +209,27 @@ portTickType xValueOfInsertion; } /*-----------------------------------------------------------*/ -unsigned portBASE_TYPE uxListRemove( xListItem *pxItemToRemove ) +UBaseType_t uxListRemove( ListItem_t * const pxItemToRemove ) { -xList * pxList; +/* The list item knows which list it is in. Obtain the list from the list +item. */ +List_t * const pxList = ( List_t * ) pxItemToRemove->pvContainer; pxItemToRemove->pxNext->pxPrevious = pxItemToRemove->pxPrevious; pxItemToRemove->pxPrevious->pxNext = pxItemToRemove->pxNext; - /* The list item knows which list it is in. Obtain the list from the list - item. */ - pxList = ( xList * ) pxItemToRemove->pvContainer; + /* Only used during decision coverage testing. */ + mtCOVERAGE_TEST_DELAY(); /* Make sure the index is left pointing to a valid item. */ if( pxList->pxIndex == pxItemToRemove ) { pxList->pxIndex = pxItemToRemove->pxPrevious; } + else + { + mtCOVERAGE_TEST_MARKER(); + } pxItemToRemove->pvContainer = NULL; ( pxList->uxNumberOfItems )--; diff --git a/lib/FreeRTOS/portable/GCC/ARM_CM3/port.c b/lib/FreeRTOS/portable/GCC/ARM_CM3/port.c index 2be8ad9032..263b9ed60b 100644 --- a/lib/FreeRTOS/portable/GCC/ARM_CM3/port.c +++ b/lib/FreeRTOS/portable/GCC/ARM_CM3/port.c @@ -1,75 +1,70 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ /*----------------------------------------------------------- @@ -89,36 +84,65 @@ FreeRTOS.org versions prior to V4.4.0 did not include this definition. */ #ifndef configSYSTICK_CLOCK_HZ #define configSYSTICK_CLOCK_HZ configCPU_CLOCK_HZ + /* Ensure the SysTick is clocked at the same frequency as the core. */ + #define portNVIC_SYSTICK_CLK_BIT ( 1UL << 2UL ) +#else + /* The way the SysTick is clocked is not modified in case it is not the same + as the core. */ + #define portNVIC_SYSTICK_CLK_BIT ( 0 ) #endif /* Constants required to manipulate the core. Registers first... */ -#define portNVIC_SYSTICK_CTRL_REG ( * ( ( volatile unsigned long * ) 0xe000e010 ) ) -#define portNVIC_SYSTICK_LOAD_REG ( * ( ( volatile unsigned long * ) 0xe000e014 ) ) -#define portNVIC_SYSTICK_CURRENT_VALUE_REG ( * ( ( volatile unsigned long * ) 0xe000e018 ) ) -#define portNVIC_INT_CTRL_REG ( * ( ( volatile unsigned long * ) 0xe000ed04 ) ) -#define portNVIC_SYSPRI2_REG ( * ( ( volatile unsigned long * ) 0xe000ed20 ) ) +#define portNVIC_SYSTICK_CTRL_REG ( * ( ( volatile uint32_t * ) 0xe000e010 ) ) +#define portNVIC_SYSTICK_LOAD_REG ( * ( ( volatile uint32_t * ) 0xe000e014 ) ) +#define portNVIC_SYSTICK_CURRENT_VALUE_REG ( * ( ( volatile uint32_t * ) 0xe000e018 ) ) +#define portNVIC_SYSPRI2_REG ( * ( ( volatile uint32_t * ) 0xe000ed20 ) ) /* ...then bits in the registers. */ -#define portNVIC_SYSTICK_CLK_BIT ( 1UL << 2UL ) #define portNVIC_SYSTICK_INT_BIT ( 1UL << 1UL ) #define portNVIC_SYSTICK_ENABLE_BIT ( 1UL << 0UL ) #define portNVIC_SYSTICK_COUNT_FLAG_BIT ( 1UL << 16UL ) -#define portNVIC_PENDSVSET_BIT ( 1UL << 28UL ) #define portNVIC_PENDSVCLEAR_BIT ( 1UL << 27UL ) #define portNVIC_PEND_SYSTICK_CLEAR_BIT ( 1UL << 25UL ) -#define portNVIC_PENDSV_PRI ( ( ( unsigned long ) configKERNEL_INTERRUPT_PRIORITY ) << 16UL ) -#define portNVIC_SYSTICK_PRI ( ( ( unsigned long ) configKERNEL_INTERRUPT_PRIORITY ) << 24UL ) +#define portNVIC_PENDSV_PRI ( ( ( uint32_t ) configKERNEL_INTERRUPT_PRIORITY ) << 16UL ) +#define portNVIC_SYSTICK_PRI ( ( ( uint32_t ) configKERNEL_INTERRUPT_PRIORITY ) << 24UL ) -/* Constants required to set up the initial stack. */ -#define portINITIAL_XPSR ( 0x01000000 ) +/* Constants required to check the validity of an interrupt priority. */ +#define portFIRST_USER_INTERRUPT_NUMBER ( 16 ) +#define portNVIC_IP_REGISTERS_OFFSET_16 ( 0xE000E3F0 ) +#define portAIRCR_REG ( * ( ( volatile uint32_t * ) 0xE000ED0C ) ) +#define portMAX_8_BIT_VALUE ( ( uint8_t ) 0xff ) +#define portTOP_BIT_OF_BYTE ( ( uint8_t ) 0x80 ) +#define portMAX_PRIGROUP_BITS ( ( uint8_t ) 7 ) +#define portPRIORITY_GROUP_MASK ( 0x07UL << 8UL ) +#define portPRIGROUP_SHIFT ( 8UL ) -/* The priority used by the kernel is assigned to a variable to make access -from inline assembler easier. */ -const unsigned long ulKernelPriority = configKERNEL_INTERRUPT_PRIORITY; +/* Masks off all bits but the VECTACTIVE bits in the ICSR register. */ +#define portVECTACTIVE_MASK ( 0xFFUL ) + +/* Constants required to set up the initial stack. */ +#define portINITIAL_XPSR ( 0x01000000UL ) + +/* The systick is a 24-bit counter. */ +#define portMAX_24_BIT_NUMBER ( 0xffffffUL ) + +/* A fiddle factor to estimate the number of SysTick counts that would have +occurred while the SysTick counter is stopped during tickless idle +calculations. */ +#define portMISSED_COUNTS_FACTOR ( 45UL ) + +/* Let the user override the pre-loading of the initial LR with the address of +prvTaskExitError() in case it messes up unwinding of the stack in the +debugger. */ +#ifdef configTASK_RETURN_ADDRESS + #define portTASK_RETURN_ADDRESS configTASK_RETURN_ADDRESS +#else + #define portTASK_RETURN_ADDRESS prvTaskExitError +#endif /* Each task maintains its own interrupt status in the critical nesting variable. */ -static unsigned portBASE_TYPE uxCriticalNesting = 0xaaaaaaaa; +static UBaseType_t uxCriticalNesting = 0xaaaaaaaa; /* * Setup the timer to generate the tick interrupts. The implementation in this @@ -130,30 +154,35 @@ void vPortSetupTimerInterrupt( void ); /* * Exception handlers. */ -void xPortPendSVHandler( void ) __attribute__ (( naked, used )); -void xPortSysTickHandler( void ) __attribute__ ((used)); -void vPortSVCHandler( void ) __attribute__ (( naked, used )); +void xPortPendSVHandler( void ) __attribute__ (( naked )); +void xPortSysTickHandler( void ); +void vPortSVCHandler( void ) __attribute__ (( naked )); /* * Start first task is a separate function so it can be tested in isolation. */ static void prvPortStartFirstTask( void ) __attribute__ (( naked )); +/* + * Used to catch tasks that attempt to return from their implementing function. + */ +static void prvTaskExitError( void ); + /*-----------------------------------------------------------*/ /* * The number of SysTick increments that make up one tick period. */ #if configUSE_TICKLESS_IDLE == 1 - static unsigned long ulTimerReloadValueForOneTick = 0; -#endif + static uint32_t ulTimerCountsForOneTick = 0; +#endif /* configUSE_TICKLESS_IDLE */ /* * The maximum number of tick periods that can be suppressed is limited by the * 24 bit resolution of the SysTick timer. */ #if configUSE_TICKLESS_IDLE == 1 - static unsigned long xMaximumPossibleSuppressedTicks = 0; + static uint32_t xMaximumPossibleSuppressedTicks = 0; #endif /* configUSE_TICKLESS_IDLE */ /* @@ -161,32 +190,57 @@ static void prvPortStartFirstTask( void ) __attribute__ (( naked )); * power functionality only. */ #if configUSE_TICKLESS_IDLE == 1 - static unsigned long ulStoppedTimerCompensation = 0; + static uint32_t ulStoppedTimerCompensation = 0; #endif /* configUSE_TICKLESS_IDLE */ +/* + * Used by the portASSERT_IF_INTERRUPT_PRIORITY_INVALID() macro to ensure + * FreeRTOS API functions are not called from interrupts that have been assigned + * a priority above configMAX_SYSCALL_INTERRUPT_PRIORITY. + */ +#if ( configASSERT_DEFINED == 1 ) + static uint8_t ucMaxSysCallPriority = 0; + static uint32_t ulMaxPRIGROUPValue = 0; + static const volatile uint8_t * const pcInterruptPriorityRegisters = ( const volatile uint8_t * const ) portNVIC_IP_REGISTERS_OFFSET_16; +#endif /* configASSERT_DEFINED */ + /*-----------------------------------------------------------*/ /* * See header file for description. */ -portSTACK_TYPE *pxPortInitialiseStack( portSTACK_TYPE *pxTopOfStack, pdTASK_CODE pxCode, void *pvParameters ) +StackType_t *pxPortInitialiseStack( StackType_t *pxTopOfStack, TaskFunction_t pxCode, void *pvParameters ) { /* Simulate the stack frame as it would be created by a context switch interrupt. */ pxTopOfStack--; /* Offset added to account for the way the MCU uses the stack on entry/exit of interrupts. */ *pxTopOfStack = portINITIAL_XPSR; /* xPSR */ pxTopOfStack--; - *pxTopOfStack = ( portSTACK_TYPE ) pxCode; /* PC */ + *pxTopOfStack = ( StackType_t ) pxCode; /* PC */ pxTopOfStack--; - *pxTopOfStack = 0; /* LR */ + *pxTopOfStack = ( StackType_t ) portTASK_RETURN_ADDRESS; /* LR */ pxTopOfStack -= 5; /* R12, R3, R2 and R1. */ - *pxTopOfStack = ( portSTACK_TYPE ) pvParameters; /* R0 */ + *pxTopOfStack = ( StackType_t ) pvParameters; /* R0 */ pxTopOfStack -= 8; /* R11, R10, R9, R8, R7, R6, R5 and R4. */ return pxTopOfStack; } /*-----------------------------------------------------------*/ +static void prvTaskExitError( void ) +{ + /* A function that implements a task must not exit or attempt to return to + its caller as there is nothing to return to. If a task wants to exit it + should instead call vTaskDelete( NULL ). + + Artificially force an assert() to be triggered if configASSERT() is + defined, then stop here so application writers can catch the error. */ + configASSERT( uxCriticalNesting == ~0UL ); + portDISABLE_INTERRUPTS(); + for( ;; ); +} +/*-----------------------------------------------------------*/ + void vPortSVCHandler( void ) { __asm volatile ( @@ -195,6 +249,7 @@ void vPortSVCHandler( void ) " ldr r0, [r1] \n" /* The first item in pxCurrentTCB is the task top of stack. */ " ldmia r0!, {r4-r11} \n" /* Pop the registers that are not automatically saved on exception entry and the critical nesting count. */ " msr psp, r0 \n" /* Restore the task stack pointer. */ + " isb \n" " mov r0, #0 \n" " msr basepri, r0 \n" " orr r14, #0xd \n" @@ -214,6 +269,9 @@ static void prvPortStartFirstTask( void ) " ldr r0, [r0] \n" " msr msp, r0 \n" /* Set the msp back to the start of the stack. */ " cpsie i \n" /* Globally enable interrupts. */ + " cpsie f \n" + " dsb \n" + " isb \n" " svc 0 \n" /* System call to start first task. */ " nop \n" ); @@ -223,13 +281,57 @@ static void prvPortStartFirstTask( void ) /* * See header file for description. */ -portBASE_TYPE xPortStartScheduler( void ) +BaseType_t xPortStartScheduler( void ) { /* configMAX_SYSCALL_INTERRUPT_PRIORITY must not be set to 0. See http://www.FreeRTOS.org/RTOS-Cortex-M3-M4.html */ configASSERT( configMAX_SYSCALL_INTERRUPT_PRIORITY ); - /* Make PendSV, CallSV and SysTick the same priroity as the kernel. */ + #if( configASSERT_DEFINED == 1 ) + { + volatile uint32_t ulOriginalPriority; + volatile uint8_t * const pucFirstUserPriorityRegister = ( volatile uint8_t * const ) ( portNVIC_IP_REGISTERS_OFFSET_16 + portFIRST_USER_INTERRUPT_NUMBER ); + volatile uint8_t ucMaxPriorityValue; + + /* Determine the maximum priority from which ISR safe FreeRTOS API + functions can be called. ISR safe functions are those that end in + "FromISR". FreeRTOS maintains separate thread and ISR API functions to + ensure interrupt entry is as fast and simple as possible. + + Save the interrupt priority value that is about to be clobbered. */ + ulOriginalPriority = *pucFirstUserPriorityRegister; + + /* Determine the number of priority bits available. First write to all + possible bits. */ + *pucFirstUserPriorityRegister = portMAX_8_BIT_VALUE; + + /* Read the value back to see how many bits stuck. */ + ucMaxPriorityValue = *pucFirstUserPriorityRegister; + + /* Use the same mask on the maximum system call priority. */ + ucMaxSysCallPriority = configMAX_SYSCALL_INTERRUPT_PRIORITY & ucMaxPriorityValue; + + /* Calculate the maximum acceptable priority group value for the number + of bits read back. */ + ulMaxPRIGROUPValue = portMAX_PRIGROUP_BITS; + while( ( ucMaxPriorityValue & portTOP_BIT_OF_BYTE ) == portTOP_BIT_OF_BYTE ) + { + ulMaxPRIGROUPValue--; + ucMaxPriorityValue <<= ( uint8_t ) 0x01; + } + + /* Shift the priority group value back to its position within the AIRCR + register. */ + ulMaxPRIGROUPValue <<= portPRIGROUP_SHIFT; + ulMaxPRIGROUPValue &= portPRIORITY_GROUP_MASK; + + /* Restore the clobbered interrupt priority register to its original + value. */ + *pucFirstUserPriorityRegister = ulOriginalPriority; + } + #endif /* conifgASSERT_DEFINED */ + + /* Make PendSV and SysTick the lowest priority interrupts. */ portNVIC_SYSPRI2_REG |= portNVIC_PENDSV_PRI; portNVIC_SYSPRI2_REG |= portNVIC_SYSTICK_PRI; @@ -243,6 +345,12 @@ portBASE_TYPE xPortStartScheduler( void ) /* Start the first task. */ prvPortStartFirstTask(); + /* Should never get here as the tasks will now be executing! Call the task + exit error function to prevent compiler warnings about a static function + not being called in the case that the application writer overrides this + functionality by defining configTASK_RETURN_ADDRESS. */ + prvTaskExitError(); + /* Should not get here! */ return 0; } @@ -250,15 +358,21 @@ portBASE_TYPE xPortStartScheduler( void ) void vPortEndScheduler( void ) { - /* It is unlikely that the CM3 port will require this function as there - is nothing to return to. */ + /* Not implemented in ports where there is nothing to return to. + Artificially force an assert. */ + configASSERT( uxCriticalNesting == 1000UL ); } /*-----------------------------------------------------------*/ -void vPortYieldFromISR( void ) +void vPortYield( void ) { /* Set a PendSV to request a context switch. */ portNVIC_INT_CTRL_REG = portNVIC_PENDSVSET_BIT; + + /* Barriers are normally not required but do ensure the code is completely + within the specified behaviour for the architecture. */ + __asm volatile( "dsb" ); + __asm volatile( "isb" ); } /*-----------------------------------------------------------*/ @@ -266,11 +380,24 @@ void vPortEnterCritical( void ) { portDISABLE_INTERRUPTS(); uxCriticalNesting++; + __asm volatile( "dsb" ); + __asm volatile( "isb" ); + + /* This is not the interrupt safe version of the enter critical function so + assert() if it is being called from an interrupt context. Only API + functions that end in "FromISR" can be used in an interrupt. Only assert if + the critical nesting count is 1 to protect against recursive calls if the + assert function also uses a critical section. */ + if( uxCriticalNesting == 1 ) + { + configASSERT( ( portNVIC_INT_CTRL_REG & portVECTACTIVE_MASK ) == 0 ); + } } /*-----------------------------------------------------------*/ void vPortExitCritical( void ) { + configASSERT( uxCriticalNesting ); uxCriticalNesting--; if( uxCriticalNesting == 0 ) { @@ -279,7 +406,7 @@ void vPortExitCritical( void ) } /*-----------------------------------------------------------*/ -__attribute__(( naked )) unsigned long ulPortSetInterruptMask( void ) +__attribute__(( naked )) uint32_t ulPortSetInterruptMask( void ) { __asm volatile \ ( \ @@ -296,7 +423,7 @@ __attribute__(( naked )) unsigned long ulPortSetInterruptMask( void ) } /*-----------------------------------------------------------*/ -__attribute__(( naked )) void vPortClearInterruptMask( unsigned long ulNewMaskValue ) +__attribute__(( naked )) void vPortClearInterruptMask( uint32_t ulNewMaskValue ) { __asm volatile \ ( \ @@ -317,6 +444,7 @@ void xPortPendSVHandler( void ) __asm volatile ( " mrs r0, psp \n" + " isb \n" " \n" " ldr r3, pxCurrentTCBConst \n" /* Get the location of the current TCB. */ " ldr r2, [r3] \n" @@ -336,6 +464,7 @@ void xPortPendSVHandler( void ) " ldr r0, [r1] \n" /* The first item in pxCurrentTCB is the task top of stack. */ " ldmia r0!, {r4-r11} \n" /* Pop the registers. */ " msr psp, r0 \n" + " isb \n" " bx r14 \n" " \n" " .align 2 \n" @@ -347,22 +476,19 @@ void xPortPendSVHandler( void ) void xPortSysTickHandler( void ) { - /* If using preemption, also force a context switch. */ - #if configUSE_PREEMPTION == 1 - portNVIC_INT_CTRL_REG = portNVIC_PENDSVSET_BIT; - #endif - - /* Only reset the systick load register if configUSE_TICKLESS_IDLE is set to - 1. If it is set to 0 tickless idle is not being used. If it is set to a - value other than 0 or 1 then a timer other than the SysTick is being used - to generate the tick interrupt. */ - #if configUSE_TICKLESS_IDLE == 1 - portNVIC_SYSTICK_LOAD_REG = ulTimerReloadValueForOneTick; - #endif - + /* The SysTick runs at the lowest interrupt priority, so when this interrupt + executes all interrupts must be unmasked. There is therefore no need to + save and then restore the interrupt mask value as its value is already + known. */ ( void ) portSET_INTERRUPT_MASK_FROM_ISR(); { - vTaskIncrementTick(); + /* Increment the RTOS tick. */ + if( xTaskIncrementTick() != pdFALSE ) + { + /* A context switch is required. Context switching is performed in + the PendSV interrupt. Pend the PendSV interrupt. */ + portNVIC_INT_CTRL_REG = portNVIC_PENDSVSET_BIT; + } } portCLEAR_INTERRUPT_MASK_FROM_ISR( 0 ); } @@ -370,10 +496,10 @@ void xPortSysTickHandler( void ) #if configUSE_TICKLESS_IDLE == 1 - __attribute__((weak)) void vPortSuppressTicksAndSleep( portTickType xExpectedIdleTime ) + __attribute__((weak)) void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime ) { - unsigned long ulReloadValue, ulCompleteTickPeriods, ulCompletedSysTickIncrements; - portTickType xModifiableIdleTime; + uint32_t ulReloadValue, ulCompleteTickPeriods, ulCompletedSysTickDecrements, ulSysTickCTRL; + TickType_t xModifiableIdleTime; /* Make sure the SysTick reload value does not overflow the counter. */ if( xExpectedIdleTime > xMaximumPossibleSuppressedTicks ) @@ -381,26 +507,21 @@ void xPortSysTickHandler( void ) xExpectedIdleTime = xMaximumPossibleSuppressedTicks; } + /* Stop the SysTick momentarily. The time the SysTick is stopped for + is accounted for as best it can be, but using the tickless mode will + inevitably result in some tiny drift of the time maintained by the + kernel with respect to calendar time. */ + portNVIC_SYSTICK_CTRL_REG &= ~portNVIC_SYSTICK_ENABLE_BIT; + /* Calculate the reload value required to wait xExpectedIdleTime tick periods. -1 is used because this code will execute part way - through one of the tick periods, and the fraction of a tick period is - accounted for later. */ - ulReloadValue = ( ulTimerReloadValueForOneTick * ( xExpectedIdleTime - 1UL ) ); + through one of the tick periods. */ + ulReloadValue = portNVIC_SYSTICK_CURRENT_VALUE_REG + ( ulTimerCountsForOneTick * ( xExpectedIdleTime - 1UL ) ); if( ulReloadValue > ulStoppedTimerCompensation ) { ulReloadValue -= ulStoppedTimerCompensation; } - /* Stop the SysTick momentarily. The time the SysTick is stopped for - is accounted for as best it can be, but using the tickless mode will - inevitably result in some tiny drift of the time maintained by the - kernel with respect to calendar time. */ - portNVIC_SYSTICK_CTRL_REG = portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT; - - /* Adjust the reload value to take into account that the current time - slice is already partially complete. */ - ulReloadValue += ( portNVIC_SYSTICK_LOAD_REG - ( portNVIC_SYSTICK_LOAD_REG - portNVIC_SYSTICK_CURRENT_VALUE_REG ) ); - /* Enter a critical section but don't use the taskENTER_CRITICAL() method as that will mask interrupts that should exit sleep mode. */ __asm volatile( "cpsid i" ); @@ -409,8 +530,16 @@ void xPortSysTickHandler( void ) to be unsuspended then abandon the low power entry. */ if( eTaskConfirmSleepModeStatus() == eAbortSleep ) { + /* Restart from whatever is left in the count register to complete + this tick period. */ + portNVIC_SYSTICK_LOAD_REG = portNVIC_SYSTICK_CURRENT_VALUE_REG; + /* Restart SysTick. */ - portNVIC_SYSTICK_CTRL_REG = portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT | portNVIC_SYSTICK_ENABLE_BIT; + portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT; + + /* Reset the reload register to the value required for normal tick + periods. */ + portNVIC_SYSTICK_LOAD_REG = ulTimerCountsForOneTick - 1UL; /* Re-enable interrupts - see comments above the cpsid instruction() above. */ @@ -426,7 +555,7 @@ void xPortSysTickHandler( void ) portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL; /* Restart SysTick. */ - portNVIC_SYSTICK_CTRL_REG = portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT | portNVIC_SYSTICK_ENABLE_BIT; + portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT; /* Sleep until something happens. configPRE_SLEEP_PROCESSING() can set its parameter to 0 to indicate that its implementation contains @@ -437,7 +566,9 @@ void xPortSysTickHandler( void ) configPRE_SLEEP_PROCESSING( xModifiableIdleTime ); if( xModifiableIdleTime > 0 ) { + __asm volatile( "dsb" ); __asm volatile( "wfi" ); + __asm volatile( "isb" ); } configPOST_SLEEP_PROCESSING( xExpectedIdleTime ); @@ -445,19 +576,32 @@ void xPortSysTickHandler( void ) accounted for as best it can be, but using the tickless mode will inevitably result in some tiny drift of the time maintained by the kernel with respect to calendar time. */ - portNVIC_SYSTICK_CTRL_REG = portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT; + ulSysTickCTRL = portNVIC_SYSTICK_CTRL_REG; + portNVIC_SYSTICK_CTRL_REG = ( ulSysTickCTRL & ~portNVIC_SYSTICK_ENABLE_BIT ); /* Re-enable interrupts - see comments above the cpsid instruction() above. */ __asm volatile( "cpsie i" ); - if( ( portNVIC_SYSTICK_CTRL_REG & portNVIC_SYSTICK_COUNT_FLAG_BIT ) != 0 ) + if( ( ulSysTickCTRL & portNVIC_SYSTICK_COUNT_FLAG_BIT ) != 0 ) { + uint32_t ulCalculatedLoadValue; + /* The tick interrupt has already executed, and the SysTick - count reloaded with the portNVIC_SYSTICK_LOAD_REG value. - Reset the portNVIC_SYSTICK_LOAD_REG with whatever remains of - this tick period. */ - portNVIC_SYSTICK_LOAD_REG = ulTimerReloadValueForOneTick - ( ulReloadValue - portNVIC_SYSTICK_CURRENT_VALUE_REG ); + count reloaded with ulReloadValue. Reset the + portNVIC_SYSTICK_LOAD_REG with whatever remains of this tick + period. */ + ulCalculatedLoadValue = ( ulTimerCountsForOneTick - 1UL ) - ( ulReloadValue - portNVIC_SYSTICK_CURRENT_VALUE_REG ); + + /* Don't allow a tiny value, or values that have somehow + underflowed because the post sleep hook did something + that took too long. */ + if( ( ulCalculatedLoadValue < ulStoppedTimerCompensation ) || ( ulCalculatedLoadValue > ulTimerCountsForOneTick ) ) + { + ulCalculatedLoadValue = ( ulTimerCountsForOneTick - 1UL ); + } + + portNVIC_SYSTICK_LOAD_REG = ulCalculatedLoadValue; /* The tick interrupt handler will already have pended the tick processing in the kernel. As the pending tick will be @@ -469,25 +613,33 @@ void xPortSysTickHandler( void ) else { /* Something other than the tick interrupt ended the sleep. - Work out how long the sleep lasted. */ - ulCompletedSysTickIncrements = ( xExpectedIdleTime * ulTimerReloadValueForOneTick ) - portNVIC_SYSTICK_CURRENT_VALUE_REG; + Work out how long the sleep lasted rounded to complete tick + periods (not the ulReload value which accounted for part + ticks). */ + ulCompletedSysTickDecrements = ( xExpectedIdleTime * ulTimerCountsForOneTick ) - portNVIC_SYSTICK_CURRENT_VALUE_REG; /* How many complete tick periods passed while the processor was waiting? */ - ulCompleteTickPeriods = ulCompletedSysTickIncrements / ulTimerReloadValueForOneTick; + ulCompleteTickPeriods = ulCompletedSysTickDecrements / ulTimerCountsForOneTick; /* The reload value is set to whatever fraction of a single tick period remains. */ - portNVIC_SYSTICK_LOAD_REG = ( ( ulCompleteTickPeriods + 1 ) * ulTimerReloadValueForOneTick ) - ulCompletedSysTickIncrements; + portNVIC_SYSTICK_LOAD_REG = ( ( ulCompleteTickPeriods + 1 ) * ulTimerCountsForOneTick ) - ulCompletedSysTickDecrements; } /* Restart SysTick so it runs from portNVIC_SYSTICK_LOAD_REG again, then set portNVIC_SYSTICK_LOAD_REG back to its standard - value. */ + value. The critical section is used to ensure the tick interrupt + can only execute once in the case that the reload register is near + zero. */ portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL; - portNVIC_SYSTICK_CTRL_REG = portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT | portNVIC_SYSTICK_ENABLE_BIT; - - vTaskStepTick( ulCompleteTickPeriods ); + portENTER_CRITICAL(); + { + portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT; + vTaskStepTick( ulCompleteTickPeriods ); + portNVIC_SYSTICK_LOAD_REG = ulTimerCountsForOneTick - 1UL; + } + portEXIT_CRITICAL(); } } @@ -503,15 +655,95 @@ __attribute__(( weak )) void vPortSetupTimerInterrupt( void ) /* Calculate the constants required to configure the tick interrupt. */ #if configUSE_TICKLESS_IDLE == 1 { - ulTimerReloadValueForOneTick = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ) - 1UL; - xMaximumPossibleSuppressedTicks = 0xffffffUL / ( ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ) - 1UL ); - ulStoppedTimerCompensation = 45UL / ( configCPU_CLOCK_HZ / configSYSTICK_CLOCK_HZ ); + ulTimerCountsForOneTick = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ); + xMaximumPossibleSuppressedTicks = portMAX_24_BIT_NUMBER / ulTimerCountsForOneTick; + ulStoppedTimerCompensation = portMISSED_COUNTS_FACTOR / ( configCPU_CLOCK_HZ / configSYSTICK_CLOCK_HZ ); } #endif /* configUSE_TICKLESS_IDLE */ /* Configure SysTick to interrupt at the requested rate. */ - portNVIC_SYSTICK_LOAD_REG = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ) - 1UL;; - portNVIC_SYSTICK_CTRL_REG = portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT | portNVIC_SYSTICK_ENABLE_BIT; + portNVIC_SYSTICK_LOAD_REG = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ) - 1UL; + portNVIC_SYSTICK_CTRL_REG = ( portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT | portNVIC_SYSTICK_ENABLE_BIT ); } /*-----------------------------------------------------------*/ + +#if( configASSERT_DEFINED == 1 ) + + void vPortValidateInterruptPriority( void ) + { + uint32_t ulCurrentInterrupt; + uint8_t ucCurrentPriority; + + /* Obtain the number of the currently executing interrupt. */ + __asm volatile( "mrs %0, ipsr" : "=r"( ulCurrentInterrupt ) ); + + /* Is the interrupt number a user defined interrupt? */ + if( ulCurrentInterrupt >= portFIRST_USER_INTERRUPT_NUMBER ) + { + /* Look up the interrupt's priority. */ + ucCurrentPriority = pcInterruptPriorityRegisters[ ulCurrentInterrupt ]; + + /* The following assertion will fail if a service routine (ISR) for + an interrupt that has been assigned a priority above + configMAX_SYSCALL_INTERRUPT_PRIORITY calls an ISR safe FreeRTOS API + function. ISR safe FreeRTOS API functions must *only* be called + from interrupts that have been assigned a priority at or below + configMAX_SYSCALL_INTERRUPT_PRIORITY. + + Numerically low interrupt priority numbers represent logically high + interrupt priorities, therefore the priority of the interrupt must + be set to a value equal to or numerically *higher* than + configMAX_SYSCALL_INTERRUPT_PRIORITY. + + Interrupts that use the FreeRTOS API must not be left at their + default priority of zero as that is the highest possible priority, + which is guaranteed to be above configMAX_SYSCALL_INTERRUPT_PRIORITY, + and therefore also guaranteed to be invalid. + + FreeRTOS maintains separate thread and ISR API functions to ensure + interrupt entry is as fast and simple as possible. + + The following links provide detailed information: + http://www.freertos.org/RTOS-Cortex-M3-M4.html + http://www.freertos.org/FAQHelp.html */ + configASSERT( ucCurrentPriority >= ucMaxSysCallPriority ); + } + + /* Priority grouping: The interrupt controller (NVIC) allows the bits + that define each interrupt's priority to be split between bits that + define the interrupt's pre-emption priority bits and bits that define + the interrupt's sub-priority. For simplicity all bits must be defined + to be pre-emption priority bits. The following assertion will fail if + this is not the case (if some bits represent a sub-priority). + + If the application only uses CMSIS libraries for interrupt + configuration then the correct setting can be achieved on all Cortex-M + devices by calling NVIC_SetPriorityGrouping( 0 ); before starting the + scheduler. Note however that some vendor specific peripheral libraries + assume a non-zero priority group setting, in which cases using a value + of zero will result in unpredicable behaviour. */ + configASSERT( ( portAIRCR_REG & portPRIORITY_GROUP_MASK ) <= ulMaxPRIGROUPValue ); + } + +#endif /* configASSERT_DEFINED */ + + + + + + + + + + + + + + + + + + + + diff --git a/lib/FreeRTOS/portable/GCC/ARM_CM3/portmacro.h b/lib/FreeRTOS/portable/GCC/ARM_CM3/portmacro.h index d436e732ac..a8ebb722cf 100644 --- a/lib/FreeRTOS/portable/GCC/ARM_CM3/portmacro.h +++ b/lib/FreeRTOS/portable/GCC/ARM_CM3/portmacro.h @@ -1,75 +1,70 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. + + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ @@ -96,36 +91,47 @@ extern "C" { #define portDOUBLE double #define portLONG long #define portSHORT short -#define portSTACK_TYPE unsigned portLONG +#define portSTACK_TYPE uint32_t #define portBASE_TYPE long +typedef portSTACK_TYPE StackType_t; +typedef long BaseType_t; +typedef unsigned long UBaseType_t; + #if( configUSE_16_BIT_TICKS == 1 ) - typedef unsigned portSHORT portTickType; - #define portMAX_DELAY ( portTickType ) 0xffff + typedef uint16_t TickType_t; + #define portMAX_DELAY ( TickType_t ) 0xffff #else - typedef unsigned portLONG portTickType; - #define portMAX_DELAY ( portTickType ) 0xffffffff + typedef uint32_t TickType_t; + #define portMAX_DELAY ( TickType_t ) 0xffffffffUL + + /* 32-bit tick type on a 32-bit architecture, so reads of the tick count do + not need to be guarded with a critical section. */ + #define portTICK_TYPE_IS_ATOMIC 1 #endif /*-----------------------------------------------------------*/ /* Architecture specifics. */ #define portSTACK_GROWTH ( -1 ) -#define portTICK_RATE_MS ( ( portTickType ) 1000 / configTICK_RATE_HZ ) +#define portTICK_PERIOD_MS ( ( TickType_t ) 1000 / configTICK_RATE_HZ ) #define portBYTE_ALIGNMENT 8 /*-----------------------------------------------------------*/ /* Scheduler utilities. */ -extern void vPortYieldFromISR( void ); -#define portYIELD() vPortYieldFromISR() -#define portEND_SWITCHING_ISR( xSwitchRequired ) if( xSwitchRequired ) vPortYieldFromISR() +extern void vPortYield( void ); +#define portNVIC_INT_CTRL_REG ( * ( ( volatile uint32_t * ) 0xe000ed04 ) ) +#define portNVIC_PENDSVSET_BIT ( 1UL << 28UL ) +#define portYIELD() vPortYield() +#define portEND_SWITCHING_ISR( xSwitchRequired ) if( xSwitchRequired ) portNVIC_INT_CTRL_REG = portNVIC_PENDSVSET_BIT +#define portYIELD_FROM_ISR( x ) portEND_SWITCHING_ISR( x ) /*-----------------------------------------------------------*/ /* Critical section management. */ extern void vPortEnterCritical( void ); extern void vPortExitCritical( void ); -extern unsigned long ulPortSetInterruptMask( void ); -extern void vPortClearInterruptMask( unsigned long ulNewMaskValue ); +extern uint32_t ulPortSetInterruptMask( void ); +extern void vPortClearInterruptMask( uint32_t ulNewMaskValue ); #define portSET_INTERRUPT_MASK_FROM_ISR() ulPortSetInterruptMask() #define portCLEAR_INTERRUPT_MASK_FROM_ISR(x) vPortClearInterruptMask(x) #define portDISABLE_INTERRUPTS() ulPortSetInterruptMask() @@ -143,18 +149,22 @@ not necessary for to use this port. They are defined so the common demo files /* Tickless idle/low power functionality. */ #ifndef portSUPPRESS_TICKS_AND_SLEEP - extern void vPortSuppressTicksAndSleep( portTickType xExpectedIdleTime ); + extern void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime ); #define portSUPPRESS_TICKS_AND_SLEEP( xExpectedIdleTime ) vPortSuppressTicksAndSleep( xExpectedIdleTime ) #endif /*-----------------------------------------------------------*/ /* Architecture specific optimisations. */ +#ifndef configUSE_PORT_OPTIMISED_TASK_SELECTION + #define configUSE_PORT_OPTIMISED_TASK_SELECTION 1 +#endif + #if configUSE_PORT_OPTIMISED_TASK_SELECTION == 1 /* Generic helper function. */ - __attribute__( ( always_inline ) ) static inline unsigned char ucPortCountLeadingZeros( unsigned long ulBitmap ) + __attribute__( ( always_inline ) ) static inline uint8_t ucPortCountLeadingZeros( uint32_t ulBitmap ) { - unsigned char ucReturn; + uint8_t ucReturn; __asm volatile ( "clz %0, %1" : "=r" ( ucReturn ) : "r" ( ulBitmap ) ); return ucReturn; @@ -177,6 +187,11 @@ not necessary for to use this port. They are defined so the common demo files /*-----------------------------------------------------------*/ +#ifdef configASSERT + void vPortValidateInterruptPriority( void ); + #define portASSERT_IF_INTERRUPT_PRIORITY_INVALID() vPortValidateInterruptPriority() +#endif + /* portNOP() is not required by this port. */ #define portNOP() diff --git a/lib/FreeRTOS/portable/GCC/ARM_CM4F/port.c b/lib/FreeRTOS/portable/GCC/ARM_CM4F/port.c index 213b712778..4732d98e0a 100644 --- a/lib/FreeRTOS/portable/GCC/ARM_CM4F/port.c +++ b/lib/FreeRTOS/portable/GCC/ARM_CM4F/port.c @@ -1,75 +1,70 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ /*----------------------------------------------------------- @@ -86,41 +81,76 @@ #ifndef configSYSTICK_CLOCK_HZ #define configSYSTICK_CLOCK_HZ configCPU_CLOCK_HZ + /* Ensure the SysTick is clocked at the same frequency as the core. */ + #define portNVIC_SYSTICK_CLK_BIT ( 1UL << 2UL ) +#else + /* The way the SysTick is clocked is not modified in case it is not the same + as the core. */ + #define portNVIC_SYSTICK_CLK_BIT ( 0 ) #endif /* Constants required to manipulate the core. Registers first... */ -#define portNVIC_SYSTICK_CTRL_REG ( * ( ( volatile unsigned long * ) 0xe000e010 ) ) -#define portNVIC_SYSTICK_LOAD_REG ( * ( ( volatile unsigned long * ) 0xe000e014 ) ) -#define portNVIC_SYSTICK_CURRENT_VALUE_REG ( * ( ( volatile unsigned long * ) 0xe000e018 ) ) -#define portNVIC_INT_CTRL_REG ( * ( ( volatile unsigned long * ) 0xe000ed04 ) ) -#define portNVIC_SYSPRI2_REG ( * ( ( volatile unsigned long * ) 0xe000ed20 ) ) +#define portNVIC_SYSTICK_CTRL_REG ( * ( ( volatile uint32_t * ) 0xe000e010 ) ) +#define portNVIC_SYSTICK_LOAD_REG ( * ( ( volatile uint32_t * ) 0xe000e014 ) ) +#define portNVIC_SYSTICK_CURRENT_VALUE_REG ( * ( ( volatile uint32_t * ) 0xe000e018 ) ) +#define portNVIC_SYSPRI2_REG ( * ( ( volatile uint32_t * ) 0xe000ed20 ) ) /* ...then bits in the registers. */ -#define portNVIC_SYSTICK_CLK_BIT ( 1UL << 2UL ) #define portNVIC_SYSTICK_INT_BIT ( 1UL << 1UL ) #define portNVIC_SYSTICK_ENABLE_BIT ( 1UL << 0UL ) #define portNVIC_SYSTICK_COUNT_FLAG_BIT ( 1UL << 16UL ) -#define portNVIC_PENDSVSET_BIT ( 1UL << 28UL ) #define portNVIC_PENDSVCLEAR_BIT ( 1UL << 27UL ) #define portNVIC_PEND_SYSTICK_CLEAR_BIT ( 1UL << 25UL ) -#define portNVIC_PENDSV_PRI ( ( ( unsigned long ) configKERNEL_INTERRUPT_PRIORITY ) << 16UL ) -#define portNVIC_SYSTICK_PRI ( ( ( unsigned long ) configKERNEL_INTERRUPT_PRIORITY ) << 24UL ) +/* Constants used to detect a Cortex-M7 r0p1 core, which should use the ARM_CM7 +r0p1 port. */ +#define portCPUID ( * ( ( volatile uint32_t * ) 0xE000ed00 ) ) +#define portCORTEX_M7_r0p1_ID ( 0x410FC271UL ) +#define portCORTEX_M7_r0p0_ID ( 0x410FC270UL ) + +#define portNVIC_PENDSV_PRI ( ( ( uint32_t ) configKERNEL_INTERRUPT_PRIORITY ) << 16UL ) +#define portNVIC_SYSTICK_PRI ( ( ( uint32_t ) configKERNEL_INTERRUPT_PRIORITY ) << 24UL ) + +/* Constants required to check the validity of an interrupt priority. */ +#define portFIRST_USER_INTERRUPT_NUMBER ( 16 ) +#define portNVIC_IP_REGISTERS_OFFSET_16 ( 0xE000E3F0 ) +#define portAIRCR_REG ( * ( ( volatile uint32_t * ) 0xE000ED0C ) ) +#define portMAX_8_BIT_VALUE ( ( uint8_t ) 0xff ) +#define portTOP_BIT_OF_BYTE ( ( uint8_t ) 0x80 ) +#define portMAX_PRIGROUP_BITS ( ( uint8_t ) 7 ) +#define portPRIORITY_GROUP_MASK ( 0x07UL << 8UL ) +#define portPRIGROUP_SHIFT ( 8UL ) + +/* Masks off all bits but the VECTACTIVE bits in the ICSR register. */ +#define portVECTACTIVE_MASK ( 0xFFUL ) /* Constants required to manipulate the VFP. */ -#define portFPCCR ( ( volatile unsigned long * ) 0xe000ef34 ) /* Floating point context control register. */ -#define portASPEN_AND_LSPEN_BITS ( 0x3UL << 30UL ) +#define portFPCCR ( ( volatile uint32_t * ) 0xe000ef34 ) /* Floating point context control register. */ +#define portASPEN_AND_LSPEN_BITS ( 0x3UL << 30UL ) /* Constants required to set up the initial stack. */ -#define portINITIAL_XPSR ( 0x01000000 ) -#define portINITIAL_EXEC_RETURN ( 0xfffffffd ) - -/* The priority used by the kernel is assigned to a variable to make access -from inline assembler easier. */ -const unsigned long ulKernelPriority = configKERNEL_INTERRUPT_PRIORITY; +#define portINITIAL_XPSR ( 0x01000000 ) +#define portINITIAL_EXEC_RETURN ( 0xfffffffd ) + +/* The systick is a 24-bit counter. */ +#define portMAX_24_BIT_NUMBER ( 0xffffffUL ) + +/* A fiddle factor to estimate the number of SysTick counts that would have +occurred while the SysTick counter is stopped during tickless idle +calculations. */ +#define portMISSED_COUNTS_FACTOR ( 45UL ) + +/* Let the user override the pre-loading of the initial LR with the address of +prvTaskExitError() in case it messes up unwinding of the stack in the +debugger. */ +#ifdef configTASK_RETURN_ADDRESS + #define portTASK_RETURN_ADDRESS configTASK_RETURN_ADDRESS +#else + #define portTASK_RETURN_ADDRESS prvTaskExitError +#endif /* Each task maintains its own interrupt status in the critical nesting variable. */ -static unsigned portBASE_TYPE uxCriticalNesting = 0xaaaaaaaa; +static UBaseType_t uxCriticalNesting = 0xaaaaaaaa; /* * Setup the timer to generate the tick interrupts. The implementation in this @@ -146,21 +176,26 @@ static void prvPortStartFirstTask( void ) __attribute__ (( naked )); */ static void vPortEnableVFP( void ) __attribute__ (( naked )); +/* + * Used to catch tasks that attempt to return from their implementing function. + */ +static void prvTaskExitError( void ); + /*-----------------------------------------------------------*/ /* * The number of SysTick increments that make up one tick period. */ #if configUSE_TICKLESS_IDLE == 1 - static unsigned long ulTimerReloadValueForOneTick = 0; -#endif + static uint32_t ulTimerCountsForOneTick = 0; +#endif /* configUSE_TICKLESS_IDLE */ /* * The maximum number of tick periods that can be suppressed is limited by the * 24 bit resolution of the SysTick timer. */ #if configUSE_TICKLESS_IDLE == 1 - static unsigned long xMaximumPossibleSuppressedTicks = 0; + static uint32_t xMaximumPossibleSuppressedTicks = 0; #endif /* configUSE_TICKLESS_IDLE */ /* @@ -168,15 +203,26 @@ static void prvPortStartFirstTask( void ) __attribute__ (( naked )); * power functionality only. */ #if configUSE_TICKLESS_IDLE == 1 - static unsigned long ulStoppedTimerCompensation = 0; + static uint32_t ulStoppedTimerCompensation = 0; #endif /* configUSE_TICKLESS_IDLE */ +/* + * Used by the portASSERT_IF_INTERRUPT_PRIORITY_INVALID() macro to ensure + * FreeRTOS API functions are not called from interrupts that have been assigned + * a priority above configMAX_SYSCALL_INTERRUPT_PRIORITY. + */ +#if ( configASSERT_DEFINED == 1 ) + static uint8_t ucMaxSysCallPriority = 0; + static uint32_t ulMaxPRIGROUPValue = 0; + static const volatile uint8_t * const pcInterruptPriorityRegisters = ( const volatile uint8_t * const ) portNVIC_IP_REGISTERS_OFFSET_16; +#endif /* configASSERT_DEFINED */ + /*-----------------------------------------------------------*/ /* * See header file for description. */ -portSTACK_TYPE *pxPortInitialiseStack( portSTACK_TYPE *pxTopOfStack, pdTASK_CODE pxCode, void *pvParameters ) +StackType_t *pxPortInitialiseStack( StackType_t *pxTopOfStack, TaskFunction_t pxCode, void *pvParameters ) { /* Simulate the stack frame as it would be created by a context switch interrupt. */ @@ -187,13 +233,13 @@ portSTACK_TYPE *pxPortInitialiseStack( portSTACK_TYPE *pxTopOfStack, pdTASK_CODE *pxTopOfStack = portINITIAL_XPSR; /* xPSR */ pxTopOfStack--; - *pxTopOfStack = ( portSTACK_TYPE ) pxCode; /* PC */ + *pxTopOfStack = ( StackType_t ) pxCode; /* PC */ pxTopOfStack--; - *pxTopOfStack = 0; /* LR */ + *pxTopOfStack = ( StackType_t ) portTASK_RETURN_ADDRESS; /* LR */ /* Save code space by skipping register initialisation. */ pxTopOfStack -= 5; /* R12, R3, R2 and R1. */ - *pxTopOfStack = ( portSTACK_TYPE ) pvParameters; /* R0 */ + *pxTopOfStack = ( StackType_t ) pvParameters; /* R0 */ /* A save method is being used that requires each task to maintain its own exec return value. */ @@ -206,6 +252,20 @@ portSTACK_TYPE *pxPortInitialiseStack( portSTACK_TYPE *pxTopOfStack, pdTASK_CODE } /*-----------------------------------------------------------*/ +static void prvTaskExitError( void ) +{ + /* A function that implements a task must not exit or attempt to return to + its caller as there is nothing to return to. If a task wants to exit it + should instead call vTaskDelete( NULL ). + + Artificially force an assert() to be triggered if configASSERT() is + defined, then stop here so application writers can catch the error. */ + configASSERT( uxCriticalNesting == ~0UL ); + portDISABLE_INTERRUPTS(); + for( ;; ); +} +/*-----------------------------------------------------------*/ + void vPortSVCHandler( void ) { __asm volatile ( @@ -214,6 +274,7 @@ void vPortSVCHandler( void ) " ldr r0, [r1] \n" /* The first item in pxCurrentTCB is the task top of stack. */ " ldmia r0!, {r4-r11, r14} \n" /* Pop the registers that are not automatically saved on exception entry and the critical nesting count. */ " msr psp, r0 \n" /* Restore the task stack pointer. */ + " isb \n" " mov r0, #0 \n" " msr basepri, r0 \n" " bx r14 \n" @@ -232,6 +293,9 @@ static void prvPortStartFirstTask( void ) " ldr r0, [r0] \n" " msr msp, r0 \n" /* Set the msp back to the start of the stack. */ " cpsie i \n" /* Globally enable interrupts. */ + " cpsie f \n" + " dsb \n" + " isb \n" " svc 0 \n" /* System call to start first task. */ " nop \n" ); @@ -241,13 +305,63 @@ static void prvPortStartFirstTask( void ) /* * See header file for description. */ -portBASE_TYPE xPortStartScheduler( void ) +BaseType_t xPortStartScheduler( void ) { /* configMAX_SYSCALL_INTERRUPT_PRIORITY must not be set to 0. See http://www.FreeRTOS.org/RTOS-Cortex-M3-M4.html */ configASSERT( configMAX_SYSCALL_INTERRUPT_PRIORITY ); - /* Make PendSV, CallSV and SysTick the same priroity as the kernel. */ + /* This port can be used on all revisions of the Cortex-M7 core other than + the r0p1 parts. r0p1 parts should use the port from the + /source/portable/GCC/ARM_CM7/r0p1 directory. */ + configASSERT( portCPUID != portCORTEX_M7_r0p1_ID ); + configASSERT( portCPUID != portCORTEX_M7_r0p0_ID ); + + #if( configASSERT_DEFINED == 1 ) + { + volatile uint32_t ulOriginalPriority; + volatile uint8_t * const pucFirstUserPriorityRegister = ( volatile uint8_t * const ) ( portNVIC_IP_REGISTERS_OFFSET_16 + portFIRST_USER_INTERRUPT_NUMBER ); + volatile uint8_t ucMaxPriorityValue; + + /* Determine the maximum priority from which ISR safe FreeRTOS API + functions can be called. ISR safe functions are those that end in + "FromISR". FreeRTOS maintains separate thread and ISR API functions to + ensure interrupt entry is as fast and simple as possible. + + Save the interrupt priority value that is about to be clobbered. */ + ulOriginalPriority = *pucFirstUserPriorityRegister; + + /* Determine the number of priority bits available. First write to all + possible bits. */ + *pucFirstUserPriorityRegister = portMAX_8_BIT_VALUE; + + /* Read the value back to see how many bits stuck. */ + ucMaxPriorityValue = *pucFirstUserPriorityRegister; + + /* Use the same mask on the maximum system call priority. */ + ucMaxSysCallPriority = configMAX_SYSCALL_INTERRUPT_PRIORITY & ucMaxPriorityValue; + + /* Calculate the maximum acceptable priority group value for the number + of bits read back. */ + ulMaxPRIGROUPValue = portMAX_PRIGROUP_BITS; + while( ( ucMaxPriorityValue & portTOP_BIT_OF_BYTE ) == portTOP_BIT_OF_BYTE ) + { + ulMaxPRIGROUPValue--; + ucMaxPriorityValue <<= ( uint8_t ) 0x01; + } + + /* Shift the priority group value back to its position within the AIRCR + register. */ + ulMaxPRIGROUPValue <<= portPRIGROUP_SHIFT; + ulMaxPRIGROUPValue &= portPRIORITY_GROUP_MASK; + + /* Restore the clobbered interrupt priority register to its original + value. */ + *pucFirstUserPriorityRegister = ulOriginalPriority; + } + #endif /* conifgASSERT_DEFINED */ + + /* Make PendSV and SysTick the lowest priority interrupts. */ portNVIC_SYSPRI2_REG |= portNVIC_PENDSV_PRI; portNVIC_SYSPRI2_REG |= portNVIC_SYSTICK_PRI; @@ -267,6 +381,12 @@ portBASE_TYPE xPortStartScheduler( void ) /* Start the first task. */ prvPortStartFirstTask(); + /* Should never get here as the tasks will now be executing! Call the task + exit error function to prevent compiler warnings about a static function + not being called in the case that the application writer overrides this + functionality by defining configTASK_RETURN_ADDRESS. */ + prvTaskExitError(); + /* Should not get here! */ return 0; } @@ -274,15 +394,9 @@ portBASE_TYPE xPortStartScheduler( void ) void vPortEndScheduler( void ) { - /* It is unlikely that the CM4F port will require this function as there - is nothing to return to. */ -} -/*-----------------------------------------------------------*/ - -void vPortYieldFromISR( void ) -{ - /* Set a PendSV to request a context switch. */ - portNVIC_INT_CTRL_REG = portNVIC_PENDSVSET_BIT; + /* Not implemented in ports where there is nothing to return to. + Artificially force an assert. */ + configASSERT( uxCriticalNesting == 1000UL ); } /*-----------------------------------------------------------*/ @@ -290,11 +404,22 @@ void vPortEnterCritical( void ) { portDISABLE_INTERRUPTS(); uxCriticalNesting++; + + /* This is not the interrupt safe version of the enter critical function so + assert() if it is being called from an interrupt context. Only API + functions that end in "FromISR" can be used in an interrupt. Only assert if + the critical nesting count is 1 to protect against recursive calls if the + assert function also uses a critical section. */ + if( uxCriticalNesting == 1 ) + { + configASSERT( ( portNVIC_INT_CTRL_REG & portVECTACTIVE_MASK ) == 0 ); + } } /*-----------------------------------------------------------*/ void vPortExitCritical( void ) { + configASSERT( uxCriticalNesting ); uxCriticalNesting--; if( uxCriticalNesting == 0 ) { @@ -303,37 +428,6 @@ void vPortExitCritical( void ) } /*-----------------------------------------------------------*/ -__attribute__(( naked )) unsigned long ulPortSetInterruptMask( void ) -{ - __asm volatile \ - ( \ - " mrs r0, basepri \n" \ - " mov r1, %0 \n" \ - " msr basepri, r1 \n" \ - " bx lr \n" \ - :: "i" ( configMAX_SYSCALL_INTERRUPT_PRIORITY ) : "r0", "r1" \ - ); - - /* This return will not be reached but is necessary to prevent compiler - warnings. */ - return 0; -} -/*-----------------------------------------------------------*/ - -__attribute__(( naked )) void vPortClearInterruptMask( unsigned long ulNewMaskValue ) -{ - __asm volatile \ - ( \ - " msr basepri, r0 \n" \ - " bx lr \n" \ - :::"r0" \ - ); - - /* Just to avoid compiler warnings. */ - ( void ) ulNewMaskValue; -} -/*-----------------------------------------------------------*/ - void xPortPendSVHandler( void ) { /* This is a naked function. */ @@ -341,8 +435,9 @@ void xPortPendSVHandler( void ) __asm volatile ( " mrs r0, psp \n" + " isb \n" " \n" - " ldr r3, pxCurrentTCBConst \n" /* Get the location of the current TCB. */ + " ldr r3, pxCurrentTCBConst \n" /* Get the location of the current TCB. */ " ldr r2, [r3] \n" " \n" " tst r14, #0x10 \n" /* Is the task using the FPU context? If so, push high vfp registers. */ @@ -353,13 +448,15 @@ void xPortPendSVHandler( void ) " \n" " str r0, [r2] \n" /* Save the new top of stack into the first member of the TCB. */ " \n" - " stmdb sp!, {r3, r14} \n" + " stmdb sp!, {r3} \n" " mov r0, %0 \n" " msr basepri, r0 \n" + " dsb \n" + " isb \n" " bl vTaskSwitchContext \n" " mov r0, #0 \n" " msr basepri, r0 \n" - " ldmia sp!, {r3, r14} \n" + " ldmia sp!, {r3} \n" " \n" " ldr r1, [r3] \n" /* The first item in pxCurrentTCB is the task top of stack. */ " ldr r0, [r1] \n" @@ -371,6 +468,15 @@ void xPortPendSVHandler( void ) " vldmiaeq r0!, {s16-s31} \n" " \n" " msr psp, r0 \n" + " isb \n" + " \n" + #ifdef WORKAROUND_PMU_CM001 /* XMC4000 specific errata workaround. */ + #if WORKAROUND_PMU_CM001 == 1 + " push { r14 } \n" + " pop { pc } \n" + #endif + #endif + " \n" " bx r14 \n" " \n" " .align 2 \n" @@ -382,22 +488,19 @@ void xPortPendSVHandler( void ) void xPortSysTickHandler( void ) { - /* If using preemption, also force a context switch. */ - #if configUSE_PREEMPTION == 1 - portNVIC_INT_CTRL_REG = portNVIC_PENDSVSET_BIT; - #endif - - /* Only reset the systick load register if configUSE_TICKLESS_IDLE is set to - 1. If it is set to 0 tickless idle is not being used. If it is set to a - value other than 0 or 1 then a timer other than the SysTick is being used - to generate the tick interrupt. */ - #if configUSE_TICKLESS_IDLE == 1 - portNVIC_SYSTICK_LOAD_REG = ulTimerReloadValueForOneTick; - #endif - + /* The SysTick runs at the lowest interrupt priority, so when this interrupt + executes all interrupts must be unmasked. There is therefore no need to + save and then restore the interrupt mask value as its value is already + known. */ ( void ) portSET_INTERRUPT_MASK_FROM_ISR(); { - vTaskIncrementTick(); + /* Increment the RTOS tick. */ + if( xTaskIncrementTick() != pdFALSE ) + { + /* A context switch is required. Context switching is performed in + the PendSV interrupt. Pend the PendSV interrupt. */ + portNVIC_INT_CTRL_REG = portNVIC_PENDSVSET_BIT; + } } portCLEAR_INTERRUPT_MASK_FROM_ISR( 0 ); } @@ -405,10 +508,10 @@ void xPortSysTickHandler( void ) #if configUSE_TICKLESS_IDLE == 1 - __attribute__((weak)) void vPortSuppressTicksAndSleep( portTickType xExpectedIdleTime ) + __attribute__((weak)) void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime ) { - unsigned long ulReloadValue, ulCompleteTickPeriods, ulCompletedSysTickIncrements; - portTickType xModifiableIdleTime; + uint32_t ulReloadValue, ulCompleteTickPeriods, ulCompletedSysTickDecrements, ulSysTickCTRL; + TickType_t xModifiableIdleTime; /* Make sure the SysTick reload value does not overflow the counter. */ if( xExpectedIdleTime > xMaximumPossibleSuppressedTicks ) @@ -416,26 +519,21 @@ void xPortSysTickHandler( void ) xExpectedIdleTime = xMaximumPossibleSuppressedTicks; } + /* Stop the SysTick momentarily. The time the SysTick is stopped for + is accounted for as best it can be, but using the tickless mode will + inevitably result in some tiny drift of the time maintained by the + kernel with respect to calendar time. */ + portNVIC_SYSTICK_CTRL_REG &= ~portNVIC_SYSTICK_ENABLE_BIT; + /* Calculate the reload value required to wait xExpectedIdleTime tick periods. -1 is used because this code will execute part way - through one of the tick periods, and the fraction of a tick period is - accounted for later. */ - ulReloadValue = ( ulTimerReloadValueForOneTick * ( xExpectedIdleTime - 1UL ) ); + through one of the tick periods. */ + ulReloadValue = portNVIC_SYSTICK_CURRENT_VALUE_REG + ( ulTimerCountsForOneTick * ( xExpectedIdleTime - 1UL ) ); if( ulReloadValue > ulStoppedTimerCompensation ) { ulReloadValue -= ulStoppedTimerCompensation; } - /* Stop the SysTick momentarily. The time the SysTick is stopped for - is accounted for as best it can be, but using the tickless mode will - inevitably result in some tiny drift of the time maintained by the - kernel with respect to calendar time. */ - portNVIC_SYSTICK_CTRL_REG = portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT; - - /* Adjust the reload value to take into account that the current time - slice is already partially complete. */ - ulReloadValue += ( portNVIC_SYSTICK_LOAD_REG - ( portNVIC_SYSTICK_LOAD_REG - portNVIC_SYSTICK_CURRENT_VALUE_REG ) ); - /* Enter a critical section but don't use the taskENTER_CRITICAL() method as that will mask interrupts that should exit sleep mode. */ __asm volatile( "cpsid i" ); @@ -444,8 +542,16 @@ void xPortSysTickHandler( void ) to be unsuspended then abandon the low power entry. */ if( eTaskConfirmSleepModeStatus() == eAbortSleep ) { + /* Restart from whatever is left in the count register to complete + this tick period. */ + portNVIC_SYSTICK_LOAD_REG = portNVIC_SYSTICK_CURRENT_VALUE_REG; + /* Restart SysTick. */ - portNVIC_SYSTICK_CTRL_REG = portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT | portNVIC_SYSTICK_ENABLE_BIT; + portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT; + + /* Reset the reload register to the value required for normal tick + periods. */ + portNVIC_SYSTICK_LOAD_REG = ulTimerCountsForOneTick - 1UL; /* Re-enable interrupts - see comments above the cpsid instruction() above. */ @@ -461,7 +567,7 @@ void xPortSysTickHandler( void ) portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL; /* Restart SysTick. */ - portNVIC_SYSTICK_CTRL_REG = portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT | portNVIC_SYSTICK_ENABLE_BIT; + portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT; /* Sleep until something happens. configPRE_SLEEP_PROCESSING() can set its parameter to 0 to indicate that its implementation contains @@ -472,7 +578,9 @@ void xPortSysTickHandler( void ) configPRE_SLEEP_PROCESSING( xModifiableIdleTime ); if( xModifiableIdleTime > 0 ) { + __asm volatile( "dsb" ); __asm volatile( "wfi" ); + __asm volatile( "isb" ); } configPOST_SLEEP_PROCESSING( xExpectedIdleTime ); @@ -480,19 +588,32 @@ void xPortSysTickHandler( void ) accounted for as best it can be, but using the tickless mode will inevitably result in some tiny drift of the time maintained by the kernel with respect to calendar time. */ - portNVIC_SYSTICK_CTRL_REG = portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT; + ulSysTickCTRL = portNVIC_SYSTICK_CTRL_REG; + portNVIC_SYSTICK_CTRL_REG = ( ulSysTickCTRL & ~portNVIC_SYSTICK_ENABLE_BIT ); /* Re-enable interrupts - see comments above the cpsid instruction() above. */ __asm volatile( "cpsie i" ); - if( ( portNVIC_SYSTICK_CTRL_REG & portNVIC_SYSTICK_COUNT_FLAG_BIT ) != 0 ) + if( ( ulSysTickCTRL & portNVIC_SYSTICK_COUNT_FLAG_BIT ) != 0 ) { + uint32_t ulCalculatedLoadValue; + /* The tick interrupt has already executed, and the SysTick - count reloaded with the portNVIC_SYSTICK_LOAD_REG value. - Reset the portNVIC_SYSTICK_LOAD_REG with whatever remains of - this tick period. */ - portNVIC_SYSTICK_LOAD_REG = ulTimerReloadValueForOneTick - ( ulReloadValue - portNVIC_SYSTICK_CURRENT_VALUE_REG ); + count reloaded with ulReloadValue. Reset the + portNVIC_SYSTICK_LOAD_REG with whatever remains of this tick + period. */ + ulCalculatedLoadValue = ( ulTimerCountsForOneTick - 1UL ) - ( ulReloadValue - portNVIC_SYSTICK_CURRENT_VALUE_REG ); + + /* Don't allow a tiny value, or values that have somehow + underflowed because the post sleep hook did something + that took too long. */ + if( ( ulCalculatedLoadValue < ulStoppedTimerCompensation ) || ( ulCalculatedLoadValue > ulTimerCountsForOneTick ) ) + { + ulCalculatedLoadValue = ( ulTimerCountsForOneTick - 1UL ); + } + + portNVIC_SYSTICK_LOAD_REG = ulCalculatedLoadValue; /* The tick interrupt handler will already have pended the tick processing in the kernel. As the pending tick will be @@ -504,25 +625,33 @@ void xPortSysTickHandler( void ) else { /* Something other than the tick interrupt ended the sleep. - Work out how long the sleep lasted. */ - ulCompletedSysTickIncrements = ( xExpectedIdleTime * ulTimerReloadValueForOneTick ) - portNVIC_SYSTICK_CURRENT_VALUE_REG; + Work out how long the sleep lasted rounded to complete tick + periods (not the ulReload value which accounted for part + ticks). */ + ulCompletedSysTickDecrements = ( xExpectedIdleTime * ulTimerCountsForOneTick ) - portNVIC_SYSTICK_CURRENT_VALUE_REG; /* How many complete tick periods passed while the processor was waiting? */ - ulCompleteTickPeriods = ulCompletedSysTickIncrements / ulTimerReloadValueForOneTick; + ulCompleteTickPeriods = ulCompletedSysTickDecrements / ulTimerCountsForOneTick; /* The reload value is set to whatever fraction of a single tick period remains. */ - portNVIC_SYSTICK_LOAD_REG = ( ( ulCompleteTickPeriods + 1 ) * ulTimerReloadValueForOneTick ) - ulCompletedSysTickIncrements; + portNVIC_SYSTICK_LOAD_REG = ( ( ulCompleteTickPeriods + 1 ) * ulTimerCountsForOneTick ) - ulCompletedSysTickDecrements; } /* Restart SysTick so it runs from portNVIC_SYSTICK_LOAD_REG again, then set portNVIC_SYSTICK_LOAD_REG back to its standard - value. */ + value. The critical section is used to ensure the tick interrupt + can only execute once in the case that the reload register is near + zero. */ portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL; - portNVIC_SYSTICK_CTRL_REG = portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT | portNVIC_SYSTICK_ENABLE_BIT; - - vTaskStepTick( ulCompleteTickPeriods ); + portENTER_CRITICAL(); + { + portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT; + vTaskStepTick( ulCompleteTickPeriods ); + portNVIC_SYSTICK_LOAD_REG = ulTimerCountsForOneTick - 1UL; + } + portEXIT_CRITICAL(); } } @@ -538,15 +667,15 @@ __attribute__(( weak )) void vPortSetupTimerInterrupt( void ) /* Calculate the constants required to configure the tick interrupt. */ #if configUSE_TICKLESS_IDLE == 1 { - ulTimerReloadValueForOneTick = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ) - 1UL; - xMaximumPossibleSuppressedTicks = 0xffffffUL / ( ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ) - 1UL ); - ulStoppedTimerCompensation = 45UL / ( configCPU_CLOCK_HZ / configSYSTICK_CLOCK_HZ ); + ulTimerCountsForOneTick = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ); + xMaximumPossibleSuppressedTicks = portMAX_24_BIT_NUMBER / ulTimerCountsForOneTick; + ulStoppedTimerCompensation = portMISSED_COUNTS_FACTOR / ( configCPU_CLOCK_HZ / configSYSTICK_CLOCK_HZ ); } #endif /* configUSE_TICKLESS_IDLE */ /* Configure SysTick to interrupt at the requested rate. */ - portNVIC_SYSTICK_LOAD_REG = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ) - 1UL;; - portNVIC_SYSTICK_CTRL_REG = portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT | portNVIC_SYSTICK_ENABLE_BIT; + portNVIC_SYSTICK_LOAD_REG = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ) - 1UL; + portNVIC_SYSTICK_CTRL_REG = ( portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT | portNVIC_SYSTICK_ENABLE_BIT ); } /*-----------------------------------------------------------*/ @@ -563,4 +692,66 @@ static void vPortEnableVFP( void ) " bx r14 " ); } +/*-----------------------------------------------------------*/ + +#if( configASSERT_DEFINED == 1 ) + + void vPortValidateInterruptPriority( void ) + { + uint32_t ulCurrentInterrupt; + uint8_t ucCurrentPriority; + + /* Obtain the number of the currently executing interrupt. */ + __asm volatile( "mrs %0, ipsr" : "=r"( ulCurrentInterrupt ) ); + + /* Is the interrupt number a user defined interrupt? */ + if( ulCurrentInterrupt >= portFIRST_USER_INTERRUPT_NUMBER ) + { + /* Look up the interrupt's priority. */ + ucCurrentPriority = pcInterruptPriorityRegisters[ ulCurrentInterrupt ]; + + /* The following assertion will fail if a service routine (ISR) for + an interrupt that has been assigned a priority above + configMAX_SYSCALL_INTERRUPT_PRIORITY calls an ISR safe FreeRTOS API + function. ISR safe FreeRTOS API functions must *only* be called + from interrupts that have been assigned a priority at or below + configMAX_SYSCALL_INTERRUPT_PRIORITY. + + Numerically low interrupt priority numbers represent logically high + interrupt priorities, therefore the priority of the interrupt must + be set to a value equal to or numerically *higher* than + configMAX_SYSCALL_INTERRUPT_PRIORITY. + + Interrupts that use the FreeRTOS API must not be left at their + default priority of zero as that is the highest possible priority, + which is guaranteed to be above configMAX_SYSCALL_INTERRUPT_PRIORITY, + and therefore also guaranteed to be invalid. + + FreeRTOS maintains separate thread and ISR API functions to ensure + interrupt entry is as fast and simple as possible. + + The following links provide detailed information: + http://www.freertos.org/RTOS-Cortex-M3-M4.html + http://www.freertos.org/FAQHelp.html */ + configASSERT( ucCurrentPriority >= ucMaxSysCallPriority ); + } + + /* Priority grouping: The interrupt controller (NVIC) allows the bits + that define each interrupt's priority to be split between bits that + define the interrupt's pre-emption priority bits and bits that define + the interrupt's sub-priority. For simplicity all bits must be defined + to be pre-emption priority bits. The following assertion will fail if + this is not the case (if some bits represent a sub-priority). + + If the application only uses CMSIS libraries for interrupt + configuration then the correct setting can be achieved on all Cortex-M + devices by calling NVIC_SetPriorityGrouping( 0 ); before starting the + scheduler. Note however that some vendor specific peripheral libraries + assume a non-zero priority group setting, in which cases using a value + of zero will result in unpredicable behaviour. */ + configASSERT( ( portAIRCR_REG & portPRIORITY_GROUP_MASK ) <= ulMaxPRIGROUPValue ); + } + +#endif /* configASSERT_DEFINED */ + diff --git a/lib/FreeRTOS/portable/GCC/ARM_CM4F/portmacro.h b/lib/FreeRTOS/portable/GCC/ARM_CM4F/portmacro.h index e99354d6f0..073e408abb 100644 --- a/lib/FreeRTOS/portable/GCC/ARM_CM4F/portmacro.h +++ b/lib/FreeRTOS/portable/GCC/ARM_CM4F/portmacro.h @@ -1,75 +1,70 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ @@ -81,7 +76,7 @@ extern "C" { #endif /*----------------------------------------------------------- - * Port specific definitions. + * Port specific definitions. * * The settings in this file configure FreeRTOS correctly for the * given hardware and compiler. @@ -96,47 +91,60 @@ extern "C" { #define portDOUBLE double #define portLONG long #define portSHORT short -#define portSTACK_TYPE unsigned portLONG +#define portSTACK_TYPE uint32_t #define portBASE_TYPE long +typedef portSTACK_TYPE StackType_t; +typedef long BaseType_t; +typedef unsigned long UBaseType_t; + #if( configUSE_16_BIT_TICKS == 1 ) - typedef unsigned portSHORT portTickType; - #define portMAX_DELAY ( portTickType ) 0xffff + typedef uint16_t TickType_t; + #define portMAX_DELAY ( TickType_t ) 0xffff #else - typedef unsigned portLONG portTickType; - #define portMAX_DELAY ( portTickType ) 0xffffffff + typedef uint32_t TickType_t; + #define portMAX_DELAY ( TickType_t ) 0xffffffffUL + + /* 32-bit tick type on a 32-bit architecture, so reads of the tick count do + not need to be guarded with a critical section. */ + #define portTICK_TYPE_IS_ATOMIC 1 #endif -/*-----------------------------------------------------------*/ +/*-----------------------------------------------------------*/ /* Architecture specifics. */ #define portSTACK_GROWTH ( -1 ) -#define portTICK_RATE_MS ( ( portTickType ) 1000 / configTICK_RATE_HZ ) +#define portTICK_PERIOD_MS ( ( TickType_t ) 1000 / configTICK_RATE_HZ ) #define portBYTE_ALIGNMENT 8 -/*-----------------------------------------------------------*/ - +/*-----------------------------------------------------------*/ /* Scheduler utilities. */ -extern void vPortYieldFromISR( void ); -#define portYIELD() vPortYieldFromISR() -#define portEND_SWITCHING_ISR( xSwitchRequired ) if( xSwitchRequired ) vPortYieldFromISR() +#define portYIELD() \ +{ \ + /* Set a PendSV to request a context switch. */ \ + portNVIC_INT_CTRL_REG = portNVIC_PENDSVSET_BIT; \ + \ + /* Barriers are normally not required but do ensure the code is completely \ + within the specified behaviour for the architecture. */ \ + __asm volatile( "dsb" ); \ + __asm volatile( "isb" ); \ +} + +#define portNVIC_INT_CTRL_REG ( * ( ( volatile uint32_t * ) 0xe000ed04 ) ) +#define portNVIC_PENDSVSET_BIT ( 1UL << 28UL ) +#define portEND_SWITCHING_ISR( xSwitchRequired ) if( xSwitchRequired != pdFALSE ) portYIELD() +#define portYIELD_FROM_ISR( x ) portEND_SWITCHING_ISR( x ) /*-----------------------------------------------------------*/ /* Critical section management. */ extern void vPortEnterCritical( void ); extern void vPortExitCritical( void ); -extern unsigned long ulPortSetInterruptMask( void ); -extern void vPortClearInterruptMask( unsigned long ulNewMaskValue ); -#define portSET_INTERRUPT_MASK_FROM_ISR() ulPortSetInterruptMask() -#define portCLEAR_INTERRUPT_MASK_FROM_ISR(x) vPortClearInterruptMask(x) -#define portDISABLE_INTERRUPTS() ulPortSetInterruptMask() -#define portENABLE_INTERRUPTS() vPortClearInterruptMask(0) +#define portSET_INTERRUPT_MASK_FROM_ISR() ulPortRaiseBASEPRI() +#define portCLEAR_INTERRUPT_MASK_FROM_ISR(x) vPortSetBASEPRI(x) +#define portDISABLE_INTERRUPTS() vPortRaiseBASEPRI() +#define portENABLE_INTERRUPTS() vPortSetBASEPRI(0) #define portENTER_CRITICAL() vPortEnterCritical() #define portEXIT_CRITICAL() vPortExitCritical() -/* There are an uneven number of items on the initial stack, so -portALIGNMENT_ASSERT_pxCurrentTCB() will trigger false positive asserts. */ -#define portALIGNMENT_ASSERT_pxCurrentTCB ( void ) - /*-----------------------------------------------------------*/ /* Task function macros as described on the FreeRTOS.org WEB site. These are @@ -148,18 +156,22 @@ not necessary for to use this port. They are defined so the common demo files /* Tickless idle/low power functionality. */ #ifndef portSUPPRESS_TICKS_AND_SLEEP - extern void vPortSuppressTicksAndSleep( portTickType xExpectedIdleTime ); + extern void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime ); #define portSUPPRESS_TICKS_AND_SLEEP( xExpectedIdleTime ) vPortSuppressTicksAndSleep( xExpectedIdleTime ) #endif /*-----------------------------------------------------------*/ /* Architecture specific optimisations. */ +#ifndef configUSE_PORT_OPTIMISED_TASK_SELECTION + #define configUSE_PORT_OPTIMISED_TASK_SELECTION 1 +#endif + #if configUSE_PORT_OPTIMISED_TASK_SELECTION == 1 /* Generic helper function. */ - __attribute__( ( always_inline ) ) static inline unsigned char ucPortCountLeadingZeros( unsigned long ulBitmap ) + __attribute__( ( always_inline ) ) static inline uint8_t ucPortCountLeadingZeros( uint32_t ulBitmap ) { - unsigned char ucReturn; + uint8_t ucReturn; __asm volatile ( "clz %0, %1" : "=r" ( ucReturn ) : "r" ( ulBitmap ) ); return ucReturn; @@ -182,9 +194,66 @@ not necessary for to use this port. They are defined so the common demo files /*-----------------------------------------------------------*/ +#ifdef configASSERT + void vPortValidateInterruptPriority( void ); + #define portASSERT_IF_INTERRUPT_PRIORITY_INVALID() vPortValidateInterruptPriority() +#endif + /* portNOP() is not required by this port. */ #define portNOP() +#ifndef portFORCE_INLINE + #define portFORCE_INLINE inline __attribute__(( always_inline)) +#endif + +/*-----------------------------------------------------------*/ + +portFORCE_INLINE static void vPortRaiseBASEPRI( void ) +{ +uint32_t ulNewBASEPRI; + + __asm volatile + ( + " mov %0, %1 \n" \ + " msr basepri, %0 \n" \ + " isb \n" \ + " dsb \n" \ + :"=r" (ulNewBASEPRI) : "i" ( configMAX_SYSCALL_INTERRUPT_PRIORITY ) + ); +} + +/*-----------------------------------------------------------*/ + +portFORCE_INLINE static uint32_t ulPortRaiseBASEPRI( void ) +{ +uint32_t ulOriginalBASEPRI, ulNewBASEPRI; + + __asm volatile + ( + " mrs %0, basepri \n" \ + " mov %1, %2 \n" \ + " msr basepri, %1 \n" \ + " isb \n" \ + " dsb \n" \ + :"=r" (ulOriginalBASEPRI), "=r" (ulNewBASEPRI) : "i" ( configMAX_SYSCALL_INTERRUPT_PRIORITY ) + ); + + /* This return will not be reached but is necessary to prevent compiler + warnings. */ + return ulOriginalBASEPRI; +} +/*-----------------------------------------------------------*/ + +portFORCE_INLINE static void vPortSetBASEPRI( uint32_t ulNewMaskValue ) +{ + __asm volatile + ( + " msr basepri, %0 " :: "r" ( ulNewMaskValue ) + ); +} +/*-----------------------------------------------------------*/ + + #ifdef __cplusplus } #endif diff --git a/lib/FreeRTOS/portable/MemMang/heap_1.c b/lib/FreeRTOS/portable/MemMang/heap_1.c index 5577ef35c1..aeca28735c 100644 --- a/lib/FreeRTOS/portable/MemMang/heap_1.c +++ b/lib/FreeRTOS/portable/MemMang/heap_1.c @@ -1,75 +1,70 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. + + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ @@ -96,7 +91,7 @@ task.h is included from an application file. */ #define configADJUSTED_HEAP_SIZE ( configTOTAL_HEAP_SIZE - portBYTE_ALIGNMENT ) /* Allocate the memory for the heap. */ -static unsigned char ucHeap[ configTOTAL_HEAP_SIZE ]; +static uint8_t ucHeap[ configTOTAL_HEAP_SIZE ]; static size_t xNextFreeByte = ( size_t ) 0; /*-----------------------------------------------------------*/ @@ -104,7 +99,7 @@ static size_t xNextFreeByte = ( size_t ) 0; void *pvPortMalloc( size_t xWantedSize ) { void *pvReturn = NULL; -static unsigned char *pucAlignedHeap = NULL; +static uint8_t *pucAlignedHeap = NULL; /* Ensure that blocks are always aligned to the required number of bytes. */ #if portBYTE_ALIGNMENT != 1 @@ -120,7 +115,7 @@ static unsigned char *pucAlignedHeap = NULL; if( pucAlignedHeap == NULL ) { /* Ensure the heap starts on a correctly aligned boundary. */ - pucAlignedHeap = ( unsigned char * ) ( ( ( portPOINTER_SIZE_TYPE ) &ucHeap[ portBYTE_ALIGNMENT ] ) & ( ( portPOINTER_SIZE_TYPE ) ~portBYTE_ALIGNMENT_MASK ) ); + pucAlignedHeap = ( uint8_t * ) ( ( ( portPOINTER_SIZE_TYPE ) &ucHeap[ portBYTE_ALIGNMENT ] ) & ( ~( ( portPOINTER_SIZE_TYPE ) portBYTE_ALIGNMENT_MASK ) ) ); } /* Check there is enough room left for the allocation. */ @@ -132,8 +127,10 @@ static unsigned char *pucAlignedHeap = NULL; pvReturn = pucAlignedHeap + xNextFreeByte; xNextFreeByte += xWantedSize; } + + traceMALLOC( pvReturn, xWantedSize ); } - xTaskResumeAll(); + ( void ) xTaskResumeAll(); #if( configUSE_MALLOC_FAILED_HOOK == 1 ) { diff --git a/lib/FreeRTOS/portable/MemMang/heap_2.c b/lib/FreeRTOS/portable/MemMang/heap_2.c index 749872e962..2e7aa19c83 100644 --- a/lib/FreeRTOS/portable/MemMang/heap_2.c +++ b/lib/FreeRTOS/portable/MemMang/heap_2.c @@ -1,82 +1,77 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ /* * A sample implementation of pvPortMalloc() and vPortFree() that permits * allocated blocks to be freed, but does not combine adjacent free blocks * into a single larger block (and so will fragment memory). See heap_4.c for - * an aquivalent that does combine adjacent blocks into single larger blocks. + * an equivalent that does combine adjacent blocks into single larger blocks. * * See heap_1.c, heap_3.c and heap_4.c for alternative implementations, and the * memory management pages of http://www.FreeRTOS.org for more information. @@ -96,13 +91,13 @@ task.h is included from an application file. */ /* A few bytes might be lost to byte aligning the heap start address. */ #define configADJUSTED_HEAP_SIZE ( configTOTAL_HEAP_SIZE - portBYTE_ALIGNMENT ) -/* +/* * Initialises the heap structures before their first use. */ static void prvHeapInit( void ); /* Allocate the memory for the heap. */ -static unsigned char ucHeap[ configTOTAL_HEAP_SIZE ]; +static uint8_t ucHeap[ configTOTAL_HEAP_SIZE ]; /* Define the linked list structure. This is used to link free blocks in order of their size. */ @@ -110,14 +105,14 @@ typedef struct A_BLOCK_LINK { struct A_BLOCK_LINK *pxNextFreeBlock; /*<< The next free block in the list. */ size_t xBlockSize; /*<< The size of the free block. */ -} xBlockLink; +} BlockLink_t; -static const unsigned short heapSTRUCT_SIZE = ( sizeof( xBlockLink ) + portBYTE_ALIGNMENT - ( sizeof( xBlockLink ) % portBYTE_ALIGNMENT ) ); +static const uint16_t heapSTRUCT_SIZE = ( ( sizeof ( BlockLink_t ) + ( portBYTE_ALIGNMENT - 1 ) ) & ~portBYTE_ALIGNMENT_MASK ); #define heapMINIMUM_BLOCK_SIZE ( ( size_t ) ( heapSTRUCT_SIZE * 2 ) ) /* Create a couple of list links to mark the start and end of the list. */ -static xBlockLink xStart, xEnd; +static BlockLink_t xStart, xEnd; /* Keeps track of the number of free bytes remaining, but says nothing about fragmentation. */ @@ -132,7 +127,7 @@ static size_t xFreeBytesRemaining = configADJUSTED_HEAP_SIZE; */ #define prvInsertBlockIntoFreeList( pxBlockToInsert ) \ { \ -xBlockLink *pxIterator; \ +BlockLink_t *pxIterator; \ size_t xBlockSize; \ \ xBlockSize = pxBlockToInsert->xBlockSize; \ @@ -153,8 +148,8 @@ size_t xBlockSize; \ void *pvPortMalloc( size_t xWantedSize ) { -xBlockLink *pxBlock, *pxPreviousBlock, *pxNewBlockLink; -static portBASE_TYPE xHeapHasBeenInitialised = pdFALSE; +BlockLink_t *pxBlock, *pxPreviousBlock, *pxNewBlockLink; +static BaseType_t xHeapHasBeenInitialised = pdFALSE; void *pvReturn = NULL; vTaskSuspendAll(); @@ -167,14 +162,14 @@ void *pvReturn = NULL; xHeapHasBeenInitialised = pdTRUE; } - /* The wanted size is increased so it can contain a xBlockLink + /* The wanted size is increased so it can contain a BlockLink_t structure in addition to the requested amount of bytes. */ if( xWantedSize > 0 ) { xWantedSize += heapSTRUCT_SIZE; /* Ensure that blocks are always aligned to the required number of bytes. */ - if( xWantedSize & portBYTE_ALIGNMENT_MASK ) + if( ( xWantedSize & portBYTE_ALIGNMENT_MASK ) != 0 ) { /* Byte alignment required. */ xWantedSize += ( portBYTE_ALIGNMENT - ( xWantedSize & portBYTE_ALIGNMENT_MASK ) ); @@ -196,9 +191,9 @@ void *pvReturn = NULL; /* If we found the end marker then a block of adequate size was not found. */ if( pxBlock != &xEnd ) { - /* Return the memory space - jumping over the xBlockLink structure + /* Return the memory space - jumping over the BlockLink_t structure at its start. */ - pvReturn = ( void * ) ( ( ( unsigned char * ) pxPreviousBlock->pxNextFreeBlock ) + heapSTRUCT_SIZE ); + pvReturn = ( void * ) ( ( ( uint8_t * ) pxPreviousBlock->pxNextFreeBlock ) + heapSTRUCT_SIZE ); /* This block is being returned for use so must be taken out of the list of free blocks. */ @@ -210,7 +205,7 @@ void *pvReturn = NULL; /* This block is to be split into two. Create a new block following the number of bytes requested. The void cast is used to prevent byte alignment warnings from the compiler. */ - pxNewBlockLink = ( void * ) ( ( ( unsigned char * ) pxBlock ) + xWantedSize ); + pxNewBlockLink = ( void * ) ( ( ( uint8_t * ) pxBlock ) + xWantedSize ); /* Calculate the sizes of two blocks split from the single block. */ @@ -224,8 +219,10 @@ void *pvReturn = NULL; xFreeBytesRemaining -= pxBlock->xBlockSize; } } + + traceMALLOC( pvReturn, xWantedSize ); } - xTaskResumeAll(); + ( void ) xTaskResumeAll(); #if( configUSE_MALLOC_FAILED_HOOK == 1 ) { @@ -243,26 +240,27 @@ void *pvReturn = NULL; void vPortFree( void *pv ) { -unsigned char *puc = ( unsigned char * ) pv; -xBlockLink *pxLink; +uint8_t *puc = ( uint8_t * ) pv; +BlockLink_t *pxLink; if( pv != NULL ) { - /* The memory being freed will have an xBlockLink structure immediately + /* The memory being freed will have an BlockLink_t structure immediately before it. */ puc -= heapSTRUCT_SIZE; - /* This unexpected casting is to keep some compilers from issuing + /* This unexpected casting is to keep some compilers from issuing byte alignment warnings. */ pxLink = ( void * ) puc; vTaskSuspendAll(); { /* Add this block to the list of free blocks. */ - prvInsertBlockIntoFreeList( ( ( xBlockLink * ) pxLink ) ); + prvInsertBlockIntoFreeList( ( ( BlockLink_t * ) pxLink ) ); xFreeBytesRemaining += pxLink->xBlockSize; + traceFREE( pv, pxLink->xBlockSize ); } - xTaskResumeAll(); + ( void ) xTaskResumeAll(); } } /*-----------------------------------------------------------*/ @@ -281,11 +279,11 @@ void vPortInitialiseBlocks( void ) static void prvHeapInit( void ) { -xBlockLink *pxFirstFreeBlock; -unsigned char *pucAlignedHeap; +BlockLink_t *pxFirstFreeBlock; +uint8_t *pucAlignedHeap; /* Ensure the heap starts on a correctly aligned boundary. */ - pucAlignedHeap = ( unsigned char * ) ( ( ( portPOINTER_SIZE_TYPE ) &ucHeap[ portBYTE_ALIGNMENT ] ) & ( ( portPOINTER_SIZE_TYPE ) ~portBYTE_ALIGNMENT_MASK ) ); + pucAlignedHeap = ( uint8_t * ) ( ( ( portPOINTER_SIZE_TYPE ) &ucHeap[ portBYTE_ALIGNMENT ] ) & ( ~( ( portPOINTER_SIZE_TYPE ) portBYTE_ALIGNMENT_MASK ) ) ); /* xStart is used to hold a pointer to the first item in the list of free blocks. The void cast is used to prevent compiler warnings. */ diff --git a/lib/FreeRTOS/portable/MemMang/heap_3.c b/lib/FreeRTOS/portable/MemMang/heap_3.c index cc62634476..df3a9460cd 100644 --- a/lib/FreeRTOS/portable/MemMang/heap_3.c +++ b/lib/FreeRTOS/portable/MemMang/heap_3.c @@ -1,75 +1,70 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. + + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ @@ -80,7 +75,7 @@ * This file can only be used if the linker is configured to to generate * a heap memory area. * - * See heap_1.c, heap_2.c and heap_4.c for alternative implementations, and the + * See heap_1.c, heap_2.c and heap_4.c for alternative implementations, and the * memory management pages of http://www.FreeRTOS.org for more information. */ @@ -105,8 +100,9 @@ void *pvReturn; vTaskSuspendAll(); { pvReturn = malloc( xWantedSize ); + traceMALLOC( pvReturn, xWantedSize ); } - xTaskResumeAll(); + ( void ) xTaskResumeAll(); #if( configUSE_MALLOC_FAILED_HOOK == 1 ) { @@ -117,7 +113,7 @@ void *pvReturn; } } #endif - + return pvReturn; } /*-----------------------------------------------------------*/ @@ -129,8 +125,9 @@ void vPortFree( void *pv ) vTaskSuspendAll(); { free( pv ); + traceFREE( pv, 0 ); } - xTaskResumeAll(); + ( void ) xTaskResumeAll(); } } diff --git a/lib/FreeRTOS/portable/MemMang/heap_4.c b/lib/FreeRTOS/portable/MemMang/heap_4.c index b56bd28000..f139ef0512 100644 --- a/lib/FreeRTOS/portable/MemMang/heap_4.c +++ b/lib/FreeRTOS/portable/MemMang/heap_4.c @@ -1,83 +1,78 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. + + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ /* - * A sample implementation of pvPortMalloc() and vPortFree() that combines - * (coalescences) adjacent memory blocks as they are freed, and in so doing + * A sample implementation of pvPortMalloc() and vPortFree() that combines + * (coalescences) adjacent memory blocks as they are freed, and in so doing * limits memory fragmentation. * - * See heap_1.c, heap_2.c and heap_3.c for alternative implementations, and the + * See heap_1.c, heap_2.c and heap_3.c for alternative implementations, and the * memory management pages of http://www.FreeRTOS.org for more information. */ #include @@ -93,13 +88,19 @@ task.h is included from an application file. */ #undef MPU_WRAPPERS_INCLUDED_FROM_API_FILE /* Block sizes must not get too small. */ -#define heapMINIMUM_BLOCK_SIZE ( ( size_t ) ( heapSTRUCT_SIZE * 2 ) ) +#define heapMINIMUM_BLOCK_SIZE ( ( size_t ) ( xHeapStructSize << 1 ) ) -/* A few bytes might be lost to byte aligning the heap start address. */ -#define configADJUSTED_HEAP_SIZE ( configTOTAL_HEAP_SIZE - portBYTE_ALIGNMENT ) +/* Assumes 8bit bytes! */ +#define heapBITS_PER_BYTE ( ( size_t ) 8 ) /* Allocate the memory for the heap. */ -static unsigned char ucHeap[ configTOTAL_HEAP_SIZE ]; +#if( configAPPLICATION_ALLOCATED_HEAP == 1 ) + /* The application writer has already defined the array used for the RTOS + heap - probably so it can be placed in a special segment or address. */ + extern uint8_t ucHeap[ configTOTAL_HEAP_SIZE ]; +#else + static uint8_t ucHeap[ configTOTAL_HEAP_SIZE ]; +#endif /* configAPPLICATION_ALLOCATED_HEAP */ /* Define the linked list structure. This is used to link free blocks in order of their memory address. */ @@ -107,17 +108,17 @@ typedef struct A_BLOCK_LINK { struct A_BLOCK_LINK *pxNextFreeBlock; /*<< The next free block in the list. */ size_t xBlockSize; /*<< The size of the free block. */ -} xBlockLink; +} BlockLink_t; /*-----------------------------------------------------------*/ /* - * Inserts a block of memory that is being freed into the correct position in + * Inserts a block of memory that is being freed into the correct position in * the list of free memory blocks. The block being freed will be merged with * the block in front it and/or the block behind it if the memory blocks are * adjacent to each other. */ -static void prvInsertBlockIntoFreeList( xBlockLink *pxBlockToInsert ); +static void prvInsertBlockIntoFreeList( BlockLink_t *pxBlockToInsert ); /* * Called automatically to setup the required heap structures the first time @@ -129,25 +130,27 @@ static void prvHeapInit( void ); /* The size of the structure placed at the beginning of each allocated memory block must by correctly byte aligned. */ -static const unsigned short heapSTRUCT_SIZE = ( sizeof( xBlockLink ) + portBYTE_ALIGNMENT - ( sizeof( xBlockLink ) % portBYTE_ALIGNMENT ) ); - -/* Ensure the pxEnd pointer will end up on the correct byte alignment. */ -static const size_t xTotalHeapSize = ( ( size_t ) configADJUSTED_HEAP_SIZE ) & ( ( size_t ) ~portBYTE_ALIGNMENT_MASK ); +static const size_t xHeapStructSize = ( sizeof( BlockLink_t ) + ( ( size_t ) ( portBYTE_ALIGNMENT - 1 ) ) ) & ~( ( size_t ) portBYTE_ALIGNMENT_MASK ); /* Create a couple of list links to mark the start and end of the list. */ -static xBlockLink xStart, *pxEnd = NULL; +static BlockLink_t xStart, *pxEnd = NULL; /* Keeps track of the number of free bytes remaining, but says nothing about fragmentation. */ -static size_t xFreeBytesRemaining = ( ( size_t ) configADJUSTED_HEAP_SIZE ) & ( ( size_t ) ~portBYTE_ALIGNMENT_MASK ); +static size_t xFreeBytesRemaining = 0U; +static size_t xMinimumEverFreeBytesRemaining = 0U; -/* STATIC FUNCTIONS ARE DEFINED AS MACROS TO MINIMIZE THE FUNCTION CALL DEPTH. */ +/* Gets set to the top bit of an size_t type. When this bit in the xBlockSize +member of an BlockLink_t structure is set then the block belongs to the +application. When the bit is free the block is still part of the free heap +space. */ +static size_t xBlockAllocatedBit = 0; /*-----------------------------------------------------------*/ void *pvPortMalloc( size_t xWantedSize ) { -xBlockLink *pxBlock, *pxPreviousBlock, *pxNewBlockLink; +BlockLink_t *pxBlock, *pxPreviousBlock, *pxNewBlockLink; void *pvReturn = NULL; vTaskSuspendAll(); @@ -158,68 +161,123 @@ void *pvReturn = NULL; { prvHeapInit(); } - - /* The wanted size is increased so it can contain a xBlockLink - structure in addition to the requested amount of bytes. */ - if( xWantedSize > 0 ) + else { - xWantedSize += heapSTRUCT_SIZE; - - /* Ensure that blocks are always aligned to the required number of - bytes. */ - if( xWantedSize & portBYTE_ALIGNMENT_MASK ) - { - /* Byte alignment required. */ - xWantedSize += ( portBYTE_ALIGNMENT - ( xWantedSize & portBYTE_ALIGNMENT_MASK ) ); - } + mtCOVERAGE_TEST_MARKER(); } - if( ( xWantedSize > 0 ) && ( xWantedSize < xTotalHeapSize ) ) + /* Check the requested block size is not so large that the top bit is + set. The top bit of the block size member of the BlockLink_t structure + is used to determine who owns the block - the application or the + kernel, so it must be free. */ + if( ( xWantedSize & xBlockAllocatedBit ) == 0 ) { - /* Traverse the list from the start (lowest address) block until one - of adequate size is found. */ - pxPreviousBlock = &xStart; - pxBlock = xStart.pxNextFreeBlock; - while( ( pxBlock->xBlockSize < xWantedSize ) && ( pxBlock->pxNextFreeBlock != NULL ) ) + /* The wanted size is increased so it can contain a BlockLink_t + structure in addition to the requested amount of bytes. */ + if( xWantedSize > 0 ) { - pxPreviousBlock = pxBlock; - pxBlock = pxBlock->pxNextFreeBlock; - } + xWantedSize += xHeapStructSize; - /* If the end marker was reached then a block of adequate size was - not found. */ - if( pxBlock != pxEnd ) + /* Ensure that blocks are always aligned to the required number + of bytes. */ + if( ( xWantedSize & portBYTE_ALIGNMENT_MASK ) != 0x00 ) + { + /* Byte alignment required. */ + xWantedSize += ( portBYTE_ALIGNMENT - ( xWantedSize & portBYTE_ALIGNMENT_MASK ) ); + configASSERT( ( xWantedSize & portBYTE_ALIGNMENT_MASK ) == 0 ); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else { - /* Return the memory space - jumping over the xBlockLink structure - at its start. */ - pvReturn = ( void * ) ( ( ( unsigned char * ) pxPreviousBlock->pxNextFreeBlock ) + heapSTRUCT_SIZE ); - - /* This block is being returned for use so must be taken out of - the list of free blocks. */ - pxPreviousBlock->pxNextFreeBlock = pxBlock->pxNextFreeBlock; + mtCOVERAGE_TEST_MARKER(); + } - /* If the block is larger than required it can be split into two. */ - if( ( pxBlock->xBlockSize - xWantedSize ) > heapMINIMUM_BLOCK_SIZE ) + if( ( xWantedSize > 0 ) && ( xWantedSize <= xFreeBytesRemaining ) ) + { + /* Traverse the list from the start (lowest address) block until + one of adequate size is found. */ + pxPreviousBlock = &xStart; + pxBlock = xStart.pxNextFreeBlock; + while( ( pxBlock->xBlockSize < xWantedSize ) && ( pxBlock->pxNextFreeBlock != NULL ) ) { - /* This block is to be split into two. Create a new block - following the number of bytes requested. The void cast is - used to prevent byte alignment warnings from the compiler. */ - pxNewBlockLink = ( void * ) ( ( ( unsigned char * ) pxBlock ) + xWantedSize ); - - /* Calculate the sizes of two blocks split from the single - block. */ - pxNewBlockLink->xBlockSize = pxBlock->xBlockSize - xWantedSize; - pxBlock->xBlockSize = xWantedSize; - - /* Insert the new block into the list of free blocks. */ - prvInsertBlockIntoFreeList( ( pxNewBlockLink ) ); + pxPreviousBlock = pxBlock; + pxBlock = pxBlock->pxNextFreeBlock; } - xFreeBytesRemaining -= pxBlock->xBlockSize; + /* If the end marker was reached then a block of adequate size + was not found. */ + if( pxBlock != pxEnd ) + { + /* Return the memory space pointed to - jumping over the + BlockLink_t structure at its start. */ + pvReturn = ( void * ) ( ( ( uint8_t * ) pxPreviousBlock->pxNextFreeBlock ) + xHeapStructSize ); + + /* This block is being returned for use so must be taken out + of the list of free blocks. */ + pxPreviousBlock->pxNextFreeBlock = pxBlock->pxNextFreeBlock; + + /* If the block is larger than required it can be split into + two. */ + if( ( pxBlock->xBlockSize - xWantedSize ) > heapMINIMUM_BLOCK_SIZE ) + { + /* This block is to be split into two. Create a new + block following the number of bytes requested. The void + cast is used to prevent byte alignment warnings from the + compiler. */ + pxNewBlockLink = ( void * ) ( ( ( uint8_t * ) pxBlock ) + xWantedSize ); + configASSERT( ( ( ( size_t ) pxNewBlockLink ) & portBYTE_ALIGNMENT_MASK ) == 0 ); + + /* Calculate the sizes of two blocks split from the + single block. */ + pxNewBlockLink->xBlockSize = pxBlock->xBlockSize - xWantedSize; + pxBlock->xBlockSize = xWantedSize; + + /* Insert the new block into the list of free blocks. */ + prvInsertBlockIntoFreeList( pxNewBlockLink ); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + xFreeBytesRemaining -= pxBlock->xBlockSize; + + if( xFreeBytesRemaining < xMinimumEverFreeBytesRemaining ) + { + xMinimumEverFreeBytesRemaining = xFreeBytesRemaining; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + /* The block is being returned - it is allocated and owned + by the application and has no "next" block. */ + pxBlock->xBlockSize |= xBlockAllocatedBit; + pxBlock->pxNextFreeBlock = NULL; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + traceMALLOC( pvReturn, xWantedSize ); } - xTaskResumeAll(); + ( void ) xTaskResumeAll(); #if( configUSE_MALLOC_FAILED_HOOK == 1 ) { @@ -228,34 +286,62 @@ void *pvReturn = NULL; extern void vApplicationMallocFailedHook( void ); vApplicationMallocFailedHook(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } } #endif + configASSERT( ( ( ( uint32_t ) pvReturn ) & portBYTE_ALIGNMENT_MASK ) == 0 ); return pvReturn; } /*-----------------------------------------------------------*/ void vPortFree( void *pv ) { -unsigned char *puc = ( unsigned char * ) pv; -xBlockLink *pxLink; +uint8_t *puc = ( uint8_t * ) pv; +BlockLink_t *pxLink; if( pv != NULL ) { - /* The memory being freed will have an xBlockLink structure immediately + /* The memory being freed will have an BlockLink_t structure immediately before it. */ - puc -= heapSTRUCT_SIZE; + puc -= xHeapStructSize; /* This casting is to keep the compiler from issuing warnings. */ pxLink = ( void * ) puc; - vTaskSuspendAll(); + /* Check the block is actually allocated. */ + configASSERT( ( pxLink->xBlockSize & xBlockAllocatedBit ) != 0 ); + configASSERT( pxLink->pxNextFreeBlock == NULL ); + + if( ( pxLink->xBlockSize & xBlockAllocatedBit ) != 0 ) { - /* Add this block to the list of free blocks. */ - xFreeBytesRemaining += pxLink->xBlockSize; - prvInsertBlockIntoFreeList( ( ( xBlockLink * ) pxLink ) ); + if( pxLink->pxNextFreeBlock == NULL ) + { + /* The block is being returned to the heap - it is no longer + allocated. */ + pxLink->xBlockSize &= ~xBlockAllocatedBit; + + vTaskSuspendAll(); + { + /* Add this block to the list of free blocks. */ + xFreeBytesRemaining += pxLink->xBlockSize; + traceFREE( pv, pxLink->xBlockSize ); + prvInsertBlockIntoFreeList( ( ( BlockLink_t * ) pxLink ) ); + } + ( void ) xTaskResumeAll(); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } - xTaskResumeAll(); } } /*-----------------------------------------------------------*/ @@ -266,6 +352,12 @@ size_t xPortGetFreeHeapSize( void ) } /*-----------------------------------------------------------*/ +size_t xPortGetMinimumEverFreeHeapSize( void ) +{ + return xMinimumEverFreeBytesRemaining; +} +/*-----------------------------------------------------------*/ + void vPortInitialiseBlocks( void ) { /* This just exists to keep the linker quiet. */ @@ -274,11 +366,22 @@ void vPortInitialiseBlocks( void ) static void prvHeapInit( void ) { -xBlockLink *pxFirstFreeBlock; -unsigned char *pucHeapEnd, *pucAlignedHeap; +BlockLink_t *pxFirstFreeBlock; +uint8_t *pucAlignedHeap; +size_t uxAddress; +size_t xTotalHeapSize = configTOTAL_HEAP_SIZE; /* Ensure the heap starts on a correctly aligned boundary. */ - pucAlignedHeap = ( unsigned char * ) ( ( ( portPOINTER_SIZE_TYPE ) &ucHeap[ portBYTE_ALIGNMENT ] ) & ( ( portPOINTER_SIZE_TYPE ) ~portBYTE_ALIGNMENT_MASK ) ); + uxAddress = ( size_t ) ucHeap; + + if( ( uxAddress & portBYTE_ALIGNMENT_MASK ) != 0 ) + { + uxAddress += ( portBYTE_ALIGNMENT - 1 ); + uxAddress &= ~( ( size_t ) portBYTE_ALIGNMENT_MASK ); + xTotalHeapSize -= uxAddress - ( size_t ) ucHeap; + } + + pucAlignedHeap = ( uint8_t * ) uxAddress; /* xStart is used to hold a pointer to the first item in the list of free blocks. The void cast is used to prevent compiler warnings. */ @@ -287,28 +390,32 @@ unsigned char *pucHeapEnd, *pucAlignedHeap; /* pxEnd is used to mark the end of the list of free blocks and is inserted at the end of the heap space. */ - pucHeapEnd = pucAlignedHeap + xTotalHeapSize; - pucHeapEnd -= heapSTRUCT_SIZE; - pxEnd = ( void * ) pucHeapEnd; - configASSERT( ( ( ( unsigned long ) pxEnd ) & ( ( unsigned long ) portBYTE_ALIGNMENT_MASK ) ) == 0UL ); + uxAddress = ( ( size_t ) pucAlignedHeap ) + xTotalHeapSize; + uxAddress -= xHeapStructSize; + uxAddress &= ~( ( size_t ) portBYTE_ALIGNMENT_MASK ); + pxEnd = ( void * ) uxAddress; pxEnd->xBlockSize = 0; pxEnd->pxNextFreeBlock = NULL; /* To start with there is a single free block that is sized to take up the entire heap space, minus the space taken by pxEnd. */ pxFirstFreeBlock = ( void * ) pucAlignedHeap; - pxFirstFreeBlock->xBlockSize = xTotalHeapSize - heapSTRUCT_SIZE; + pxFirstFreeBlock->xBlockSize = uxAddress - ( size_t ) pxFirstFreeBlock; pxFirstFreeBlock->pxNextFreeBlock = pxEnd; - /* The heap now contains pxEnd. */ - xFreeBytesRemaining -= heapSTRUCT_SIZE; + /* Only one block exists - and it covers the entire usable heap space. */ + xMinimumEverFreeBytesRemaining = pxFirstFreeBlock->xBlockSize; + xFreeBytesRemaining = pxFirstFreeBlock->xBlockSize; + + /* Work out the position of the top bit in a size_t variable. */ + xBlockAllocatedBit = ( ( size_t ) 1 ) << ( ( sizeof( size_t ) * heapBITS_PER_BYTE ) - 1 ); } /*-----------------------------------------------------------*/ -static void prvInsertBlockIntoFreeList( xBlockLink *pxBlockToInsert ) +static void prvInsertBlockIntoFreeList( BlockLink_t *pxBlockToInsert ) { -xBlockLink *pxIterator; -unsigned char *puc; +BlockLink_t *pxIterator; +uint8_t *puc; /* Iterate through the list until a block is found that has a higher address than the block being inserted. */ @@ -318,18 +425,22 @@ unsigned char *puc; } /* Do the block being inserted, and the block it is being inserted after - make a contiguous block of memory? */ - puc = ( unsigned char * ) pxIterator; - if( ( puc + pxIterator->xBlockSize ) == ( unsigned char * ) pxBlockToInsert ) + make a contiguous block of memory? */ + puc = ( uint8_t * ) pxIterator; + if( ( puc + pxIterator->xBlockSize ) == ( uint8_t * ) pxBlockToInsert ) { pxIterator->xBlockSize += pxBlockToInsert->xBlockSize; pxBlockToInsert = pxIterator; } + else + { + mtCOVERAGE_TEST_MARKER(); + } /* Do the block being inserted, and the block it is being inserted before make a contiguous block of memory? */ - puc = ( unsigned char * ) pxBlockToInsert; - if( ( puc + pxBlockToInsert->xBlockSize ) == ( unsigned char * ) pxIterator->pxNextFreeBlock ) + puc = ( uint8_t * ) pxBlockToInsert; + if( ( puc + pxBlockToInsert->xBlockSize ) == ( uint8_t * ) pxIterator->pxNextFreeBlock ) { if( pxIterator->pxNextFreeBlock != pxEnd ) { @@ -344,7 +455,7 @@ unsigned char *puc; } else { - pxBlockToInsert->pxNextFreeBlock = pxIterator->pxNextFreeBlock; + pxBlockToInsert->pxNextFreeBlock = pxIterator->pxNextFreeBlock; } /* If the block being inserted plugged a gab, so was merged with the block @@ -355,5 +466,9 @@ unsigned char *puc; { pxIterator->pxNextFreeBlock = pxBlockToInsert; } + else + { + mtCOVERAGE_TEST_MARKER(); + } } diff --git a/lib/FreeRTOS/portable/MemMang/heap_5.c b/lib/FreeRTOS/portable/MemMang/heap_5.c new file mode 100644 index 0000000000..2f7b50abf0 --- /dev/null +++ b/lib/FreeRTOS/portable/MemMang/heap_5.c @@ -0,0 +1,523 @@ +/* + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. + + 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 modification 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. Full license text is available on the following + link: http://www.freertos.org/a00114.html + + *************************************************************************** + * * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * + * * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * + * * + *************************************************************************** + + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. + + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. + + http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and + mission critical applications that require provable dependability. + + 1 tab == 4 spaces! +*/ + +/* + * A sample implementation of pvPortMalloc() that allows the heap to be defined + * across multiple non-contigous blocks and combines (coalescences) adjacent + * memory blocks as they are freed. + * + * See heap_1.c, heap_2.c, heap_3.c and heap_4.c for alternative + * implementations, and the memory management pages of http://www.FreeRTOS.org + * for more information. + * + * Usage notes: + * + * vPortDefineHeapRegions() ***must*** be called before pvPortMalloc(). + * pvPortMalloc() will be called if any task objects (tasks, queues, event + * groups, etc.) are created, therefore vPortDefineHeapRegions() ***must*** be + * called before any other objects are defined. + * + * vPortDefineHeapRegions() takes a single parameter. The parameter is an array + * of HeapRegion_t structures. HeapRegion_t is defined in portable.h as + * + * typedef struct HeapRegion + * { + * uint8_t *pucStartAddress; << Start address of a block of memory that will be part of the heap. + * size_t xSizeInBytes; << Size of the block of memory. + * } HeapRegion_t; + * + * The array is terminated using a NULL zero sized region definition, and the + * memory regions defined in the array ***must*** appear in address order from + * low address to high address. So the following is a valid example of how + * to use the function. + * + * HeapRegion_t xHeapRegions[] = + * { + * { ( uint8_t * ) 0x80000000UL, 0x10000 }, << Defines a block of 0x10000 bytes starting at address 0x80000000 + * { ( uint8_t * ) 0x90000000UL, 0xa0000 }, << Defines a block of 0xa0000 bytes starting at address of 0x90000000 + * { NULL, 0 } << Terminates the array. + * }; + * + * vPortDefineHeapRegions( xHeapRegions ); << Pass the array into vPortDefineHeapRegions(). + * + * Note 0x80000000 is the lower address so appears in the array first. + * + */ +#include + +/* Defining MPU_WRAPPERS_INCLUDED_FROM_API_FILE prevents task.h from redefining +all the API functions to use the MPU wrappers. That should only be done when +task.h is included from an application file. */ +#define MPU_WRAPPERS_INCLUDED_FROM_API_FILE + +#include "FreeRTOS.h" +#include "task.h" + +#undef MPU_WRAPPERS_INCLUDED_FROM_API_FILE + +/* Block sizes must not get too small. */ +#define heapMINIMUM_BLOCK_SIZE ( ( size_t ) ( xHeapStructSize << 1 ) ) + +/* Assumes 8bit bytes! */ +#define heapBITS_PER_BYTE ( ( size_t ) 8 ) + +/* Define the linked list structure. This is used to link free blocks in order +of their memory address. */ +typedef struct A_BLOCK_LINK +{ + struct A_BLOCK_LINK *pxNextFreeBlock; /*<< The next free block in the list. */ + size_t xBlockSize; /*<< The size of the free block. */ +} BlockLink_t; + +/*-----------------------------------------------------------*/ + +/* + * Inserts a block of memory that is being freed into the correct position in + * the list of free memory blocks. The block being freed will be merged with + * the block in front it and/or the block behind it if the memory blocks are + * adjacent to each other. + */ +static void prvInsertBlockIntoFreeList( BlockLink_t *pxBlockToInsert ); + +/*-----------------------------------------------------------*/ + +/* The size of the structure placed at the beginning of each allocated memory +block must by correctly byte aligned. */ +static const size_t xHeapStructSize = ( sizeof( BlockLink_t ) + ( ( size_t ) ( portBYTE_ALIGNMENT - 1 ) ) ) & ~( ( size_t ) portBYTE_ALIGNMENT_MASK ); + +/* Create a couple of list links to mark the start and end of the list. */ +static BlockLink_t xStart, *pxEnd = NULL; + +/* Keeps track of the number of free bytes remaining, but says nothing about +fragmentation. */ +static size_t xFreeBytesRemaining = 0U; +static size_t xMinimumEverFreeBytesRemaining = 0U; + +/* Gets set to the top bit of an size_t type. When this bit in the xBlockSize +member of an BlockLink_t structure is set then the block belongs to the +application. When the bit is free the block is still part of the free heap +space. */ +static size_t xBlockAllocatedBit = 0; + +/*-----------------------------------------------------------*/ + +void *pvPortMalloc( size_t xWantedSize ) +{ +BlockLink_t *pxBlock, *pxPreviousBlock, *pxNewBlockLink; +void *pvReturn = NULL; + + /* The heap must be initialised before the first call to + prvPortMalloc(). */ + configASSERT( pxEnd ); + + vTaskSuspendAll(); + { + /* Check the requested block size is not so large that the top bit is + set. The top bit of the block size member of the BlockLink_t structure + is used to determine who owns the block - the application or the + kernel, so it must be free. */ + if( ( xWantedSize & xBlockAllocatedBit ) == 0 ) + { + /* The wanted size is increased so it can contain a BlockLink_t + structure in addition to the requested amount of bytes. */ + if( xWantedSize > 0 ) + { + xWantedSize += xHeapStructSize; + + /* Ensure that blocks are always aligned to the required number + of bytes. */ + if( ( xWantedSize & portBYTE_ALIGNMENT_MASK ) != 0x00 ) + { + /* Byte alignment required. */ + xWantedSize += ( portBYTE_ALIGNMENT - ( xWantedSize & portBYTE_ALIGNMENT_MASK ) ); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + if( ( xWantedSize > 0 ) && ( xWantedSize <= xFreeBytesRemaining ) ) + { + /* Traverse the list from the start (lowest address) block until + one of adequate size is found. */ + pxPreviousBlock = &xStart; + pxBlock = xStart.pxNextFreeBlock; + while( ( pxBlock->xBlockSize < xWantedSize ) && ( pxBlock->pxNextFreeBlock != NULL ) ) + { + pxPreviousBlock = pxBlock; + pxBlock = pxBlock->pxNextFreeBlock; + } + + /* If the end marker was reached then a block of adequate size + was not found. */ + if( pxBlock != pxEnd ) + { + /* Return the memory space pointed to - jumping over the + BlockLink_t structure at its start. */ + pvReturn = ( void * ) ( ( ( uint8_t * ) pxPreviousBlock->pxNextFreeBlock ) + xHeapStructSize ); + + /* This block is being returned for use so must be taken out + of the list of free blocks. */ + pxPreviousBlock->pxNextFreeBlock = pxBlock->pxNextFreeBlock; + + /* If the block is larger than required it can be split into + two. */ + if( ( pxBlock->xBlockSize - xWantedSize ) > heapMINIMUM_BLOCK_SIZE ) + { + /* This block is to be split into two. Create a new + block following the number of bytes requested. The void + cast is used to prevent byte alignment warnings from the + compiler. */ + pxNewBlockLink = ( void * ) ( ( ( uint8_t * ) pxBlock ) + xWantedSize ); + + /* Calculate the sizes of two blocks split from the + single block. */ + pxNewBlockLink->xBlockSize = pxBlock->xBlockSize - xWantedSize; + pxBlock->xBlockSize = xWantedSize; + + /* Insert the new block into the list of free blocks. */ + prvInsertBlockIntoFreeList( ( pxNewBlockLink ) ); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + xFreeBytesRemaining -= pxBlock->xBlockSize; + + if( xFreeBytesRemaining < xMinimumEverFreeBytesRemaining ) + { + xMinimumEverFreeBytesRemaining = xFreeBytesRemaining; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + /* The block is being returned - it is allocated and owned + by the application and has no "next" block. */ + pxBlock->xBlockSize |= xBlockAllocatedBit; + pxBlock->pxNextFreeBlock = NULL; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + traceMALLOC( pvReturn, xWantedSize ); + } + ( void ) xTaskResumeAll(); + + #if( configUSE_MALLOC_FAILED_HOOK == 1 ) + { + if( pvReturn == NULL ) + { + extern void vApplicationMallocFailedHook( void ); + vApplicationMallocFailedHook(); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + #endif + + return pvReturn; +} +/*-----------------------------------------------------------*/ + +void vPortFree( void *pv ) +{ +uint8_t *puc = ( uint8_t * ) pv; +BlockLink_t *pxLink; + + if( pv != NULL ) + { + /* The memory being freed will have an BlockLink_t structure immediately + before it. */ + puc -= xHeapStructSize; + + /* This casting is to keep the compiler from issuing warnings. */ + pxLink = ( void * ) puc; + + /* Check the block is actually allocated. */ + configASSERT( ( pxLink->xBlockSize & xBlockAllocatedBit ) != 0 ); + configASSERT( pxLink->pxNextFreeBlock == NULL ); + + if( ( pxLink->xBlockSize & xBlockAllocatedBit ) != 0 ) + { + if( pxLink->pxNextFreeBlock == NULL ) + { + /* The block is being returned to the heap - it is no longer + allocated. */ + pxLink->xBlockSize &= ~xBlockAllocatedBit; + + vTaskSuspendAll(); + { + /* Add this block to the list of free blocks. */ + xFreeBytesRemaining += pxLink->xBlockSize; + traceFREE( pv, pxLink->xBlockSize ); + prvInsertBlockIntoFreeList( ( ( BlockLink_t * ) pxLink ) ); + } + ( void ) xTaskResumeAll(); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } +} +/*-----------------------------------------------------------*/ + +size_t xPortGetFreeHeapSize( void ) +{ + return xFreeBytesRemaining; +} +/*-----------------------------------------------------------*/ + +size_t xPortGetMinimumEverFreeHeapSize( void ) +{ + return xMinimumEverFreeBytesRemaining; +} +/*-----------------------------------------------------------*/ + +static void prvInsertBlockIntoFreeList( BlockLink_t *pxBlockToInsert ) +{ +BlockLink_t *pxIterator; +uint8_t *puc; + + /* Iterate through the list until a block is found that has a higher address + than the block being inserted. */ + for( pxIterator = &xStart; pxIterator->pxNextFreeBlock < pxBlockToInsert; pxIterator = pxIterator->pxNextFreeBlock ) + { + /* Nothing to do here, just iterate to the right position. */ + } + + /* Do the block being inserted, and the block it is being inserted after + make a contiguous block of memory? */ + puc = ( uint8_t * ) pxIterator; + if( ( puc + pxIterator->xBlockSize ) == ( uint8_t * ) pxBlockToInsert ) + { + pxIterator->xBlockSize += pxBlockToInsert->xBlockSize; + pxBlockToInsert = pxIterator; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + /* Do the block being inserted, and the block it is being inserted before + make a contiguous block of memory? */ + puc = ( uint8_t * ) pxBlockToInsert; + if( ( puc + pxBlockToInsert->xBlockSize ) == ( uint8_t * ) pxIterator->pxNextFreeBlock ) + { + if( pxIterator->pxNextFreeBlock != pxEnd ) + { + /* Form one big block from the two blocks. */ + pxBlockToInsert->xBlockSize += pxIterator->pxNextFreeBlock->xBlockSize; + pxBlockToInsert->pxNextFreeBlock = pxIterator->pxNextFreeBlock->pxNextFreeBlock; + } + else + { + pxBlockToInsert->pxNextFreeBlock = pxEnd; + } + } + else + { + pxBlockToInsert->pxNextFreeBlock = pxIterator->pxNextFreeBlock; + } + + /* If the block being inserted plugged a gab, so was merged with the block + before and the block after, then it's pxNextFreeBlock pointer will have + already been set, and should not be set here as that would make it point + to itself. */ + if( pxIterator != pxBlockToInsert ) + { + pxIterator->pxNextFreeBlock = pxBlockToInsert; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } +} +/*-----------------------------------------------------------*/ + +void vPortDefineHeapRegions( const HeapRegion_t * const pxHeapRegions ) +{ +BlockLink_t *pxFirstFreeBlockInRegion = NULL, *pxPreviousFreeBlock; +size_t xAlignedHeap; +size_t xTotalRegionSize, xTotalHeapSize = 0; +BaseType_t xDefinedRegions = 0; +size_t xAddress; +const HeapRegion_t *pxHeapRegion; + + /* Can only call once! */ + configASSERT( pxEnd == NULL ); + + pxHeapRegion = &( pxHeapRegions[ xDefinedRegions ] ); + + while( pxHeapRegion->xSizeInBytes > 0 ) + { + xTotalRegionSize = pxHeapRegion->xSizeInBytes; + + /* Ensure the heap region starts on a correctly aligned boundary. */ + xAddress = ( size_t ) pxHeapRegion->pucStartAddress; + if( ( xAddress & portBYTE_ALIGNMENT_MASK ) != 0 ) + { + xAddress += ( portBYTE_ALIGNMENT - 1 ); + xAddress &= ~portBYTE_ALIGNMENT_MASK; + + /* Adjust the size for the bytes lost to alignment. */ + xTotalRegionSize -= xAddress - ( size_t ) pxHeapRegion->pucStartAddress; + } + + xAlignedHeap = xAddress; + + /* Set xStart if it has not already been set. */ + if( xDefinedRegions == 0 ) + { + /* xStart is used to hold a pointer to the first item in the list of + free blocks. The void cast is used to prevent compiler warnings. */ + xStart.pxNextFreeBlock = ( BlockLink_t * ) xAlignedHeap; + xStart.xBlockSize = ( size_t ) 0; + } + else + { + /* Should only get here if one region has already been added to the + heap. */ + configASSERT( pxEnd != NULL ); + + /* Check blocks are passed in with increasing start addresses. */ + configASSERT( xAddress > ( size_t ) pxEnd ); + } + + /* Remember the location of the end marker in the previous region, if + any. */ + pxPreviousFreeBlock = pxEnd; + + /* pxEnd is used to mark the end of the list of free blocks and is + inserted at the end of the region space. */ + xAddress = xAlignedHeap + xTotalRegionSize; + xAddress -= xHeapStructSize; + xAddress &= ~portBYTE_ALIGNMENT_MASK; + pxEnd = ( BlockLink_t * ) xAddress; + pxEnd->xBlockSize = 0; + pxEnd->pxNextFreeBlock = NULL; + + /* To start with there is a single free block in this region that is + sized to take up the entire heap region minus the space taken by the + free block structure. */ + pxFirstFreeBlockInRegion = ( BlockLink_t * ) xAlignedHeap; + pxFirstFreeBlockInRegion->xBlockSize = xAddress - ( size_t ) pxFirstFreeBlockInRegion; + pxFirstFreeBlockInRegion->pxNextFreeBlock = pxEnd; + + /* If this is not the first region that makes up the entire heap space + then link the previous region to this region. */ + if( pxPreviousFreeBlock != NULL ) + { + pxPreviousFreeBlock->pxNextFreeBlock = pxFirstFreeBlockInRegion; + } + + xTotalHeapSize += pxFirstFreeBlockInRegion->xBlockSize; + + /* Move onto the next HeapRegion_t structure. */ + xDefinedRegions++; + pxHeapRegion = &( pxHeapRegions[ xDefinedRegions ] ); + } + + xMinimumEverFreeBytesRemaining = xTotalHeapSize; + xFreeBytesRemaining = xTotalHeapSize; + + /* Check something was actually defined before it is accessed. */ + configASSERT( xTotalHeapSize ); + + /* Work out the position of the top bit in a size_t variable. */ + xBlockAllocatedBit = ( ( size_t ) 1 ) << ( ( sizeof( size_t ) * heapBITS_PER_BYTE ) - 1 ); +} + diff --git a/lib/FreeRTOS/queue.c b/lib/FreeRTOS/queue.c index 75bebd12d5..c71ef50203 100644 --- a/lib/FreeRTOS/queue.c +++ b/lib/FreeRTOS/queue.c @@ -1,75 +1,70 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ #include @@ -88,52 +83,75 @@ task.h is included from an application file. */ #include "croutine.h" #endif -#undef MPU_WRAPPERS_INCLUDED_FROM_API_FILE - -/* Constants used with the cRxLock and xTxLock structure members. */ -#define queueUNLOCKED ( ( signed portBASE_TYPE ) -1 ) -#define queueLOCKED_UNMODIFIED ( ( signed portBASE_TYPE ) 0 ) - -#define queueERRONEOUS_UNBLOCK ( -1 ) - -/* Effectively make a union out of the xQUEUE structure. */ +/* Lint e961 and e750 are suppressed as a MISRA exception justified because the +MPU ports require MPU_WRAPPERS_INCLUDED_FROM_API_FILE to be defined for the +header files above, but not in this file, in order to generate the correct +privileged Vs unprivileged linkage and placement. */ +#undef MPU_WRAPPERS_INCLUDED_FROM_API_FILE /*lint !e961 !e750. */ + + +/* Constants used with the xRxLock and xTxLock structure members. */ +#define queueUNLOCKED ( ( BaseType_t ) -1 ) +#define queueLOCKED_UNMODIFIED ( ( BaseType_t ) 0 ) + +/* When the Queue_t structure is used to represent a base queue its pcHead and +pcTail members are used as pointers into the queue storage area. When the +Queue_t structure is used to represent a mutex pcHead and pcTail pointers are +not necessary, and the pcHead pointer is set to NULL to indicate that the +pcTail pointer actually points to the mutex holder (if any). Map alternative +names to the pcHead and pcTail structure members to ensure the readability of +the code is maintained despite this dual use of two structure members. An +alternative implementation would be to use a union, but use of a union is +against the coding standard (although an exception to the standard has been +permitted where the dual use also significantly changes the type of the +structure member). */ #define pxMutexHolder pcTail #define uxQueueType pcHead -#define uxRecursiveCallCount pcReadFrom #define queueQUEUE_IS_MUTEX NULL -/* Semaphores do not actually store or copy data, so have an items size of +/* Semaphores do not actually store or copy data, so have an item size of zero. */ -#define queueSEMAPHORE_QUEUE_ITEM_LENGTH ( ( unsigned portBASE_TYPE ) 0 ) -#define queueDONT_BLOCK ( ( portTickType ) 0U ) -#define queueMUTEX_GIVE_BLOCK_TIME ( ( portTickType ) 0U ) - +#define queueSEMAPHORE_QUEUE_ITEM_LENGTH ( ( UBaseType_t ) 0 ) +#define queueMUTEX_GIVE_BLOCK_TIME ( ( TickType_t ) 0U ) + +#if( configUSE_PREEMPTION == 0 ) + /* If the cooperative scheduler is being used then a yield should not be + performed just because a higher priority task has been woken. */ + #define queueYIELD_IF_USING_PREEMPTION() +#else + #define queueYIELD_IF_USING_PREEMPTION() portYIELD_WITHIN_API() +#endif /* * Definition of the queue used by the scheduler. - * Items are queued by copy, not reference. + * Items are queued by copy, not reference. See the following link for the + * rationale: http://www.freertos.org/Embedded-RTOS-Queues.html */ typedef struct QueueDefinition { - signed char *pcHead; /*< Points to the beginning of the queue storage area. */ - signed char *pcTail; /*< Points to the byte at the end of the queue storage area. Once more byte is allocated than necessary to store the queue items, this is used as a marker. */ + int8_t *pcHead; /*< Points to the beginning of the queue storage area. */ + int8_t *pcTail; /*< Points to the byte at the end of the queue storage area. Once more byte is allocated than necessary to store the queue items, this is used as a marker. */ + int8_t *pcWriteTo; /*< Points to the free next place in the storage area. */ - signed char *pcWriteTo; /*< Points to the free next place in the storage area. */ - signed char *pcReadFrom; /*< Points to the last place that a queued item was read from. */ + union /* Use of a union is an exception to the coding standard to ensure two mutually exclusive structure members don't appear simultaneously (wasting RAM). */ + { + int8_t *pcReadFrom; /*< Points to the last place that a queued item was read from when the structure is used as a queue. */ + UBaseType_t uxRecursiveCallCount;/*< Maintains a count of the number of times a recursive mutex has been recursively 'taken' when the structure is used as a mutex. */ + } u; - xList xTasksWaitingToSend; /*< List of tasks that are blocked waiting to post onto this queue. Stored in priority order. */ - xList xTasksWaitingToReceive; /*< List of tasks that are blocked waiting to read from this queue. Stored in priority order. */ + List_t xTasksWaitingToSend; /*< List of tasks that are blocked waiting to post onto this queue. Stored in priority order. */ + List_t xTasksWaitingToReceive; /*< List of tasks that are blocked waiting to read from this queue. Stored in priority order. */ - volatile unsigned portBASE_TYPE uxMessagesWaiting;/*< The number of items currently in the queue. */ - unsigned portBASE_TYPE uxLength; /*< The length of the queue defined as the number of items it will hold, not the number of bytes. */ - unsigned portBASE_TYPE uxItemSize; /*< The size of each items that the queue will hold. */ + volatile UBaseType_t uxMessagesWaiting;/*< The number of items currently in the queue. */ + UBaseType_t uxLength; /*< The length of the queue defined as the number of items it will hold, not the number of bytes. */ + UBaseType_t uxItemSize; /*< The size of each items that the queue will hold. */ - volatile signed portBASE_TYPE xRxLock; /*< Stores the number of items received from the queue (removed from the queue) while the queue was locked. Set to queueUNLOCKED when the queue is not locked. */ - volatile signed portBASE_TYPE xTxLock; /*< Stores the number of items transmitted to the queue (added to the queue) while the queue was locked. Set to queueUNLOCKED when the queue is not locked. */ + volatile BaseType_t xRxLock; /*< Stores the number of items received from the queue (removed from the queue) while the queue was locked. Set to queueUNLOCKED when the queue is not locked. */ + volatile BaseType_t xTxLock; /*< Stores the number of items transmitted to the queue (added to the queue) while the queue was locked. Set to queueUNLOCKED when the queue is not locked. */ #if ( configUSE_TRACE_FACILITY == 1 ) - unsigned char ucQueueNumber; - unsigned char ucQueueType; + UBaseType_t uxQueueNumber; + uint8_t ucQueueType; #endif #if ( configUSE_QUEUE_SETS == 1 ) @@ -141,6 +159,11 @@ typedef struct QueueDefinition #endif } xQUEUE; + +/* The old xQUEUE name is maintained above then typedefed to the new Queue_t +name below to enable the use of older kernel aware debuggers. */ +typedef xQUEUE Queue_t; + /*-----------------------------------------------------------*/ /* @@ -154,18 +177,19 @@ typedef struct QueueDefinition more user friendly. */ typedef struct QUEUE_REGISTRY_ITEM { - signed char *pcQueueName; - xQueueHandle xHandle; + const char *pcQueueName; /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ + QueueHandle_t xHandle; } xQueueRegistryItem; - /* The queue registry is simply an array of xQueueRegistryItem structures. + /* The old xQueueRegistryItem name is maintained above then typedefed to the + new xQueueRegistryItem name below to enable the use of older kernel aware + debuggers. */ + typedef xQueueRegistryItem QueueRegistryItem_t; + + /* The queue registry is simply an array of QueueRegistryItem_t structures. The pcQueueName member of a structure being NULL is indicative of the array position being vacant. */ - xQueueRegistryItem xQueueRegistry[ configQUEUE_REGISTRY_SIZE ]; - - /* Removes a queue from the registry by simply setting the pcQueueName - member to NULL. */ - static void prvQueueUnregisterQueue( xQueueHandle xQueue ) PRIVILEGED_FUNCTION; + PRIVILEGED_DATA QueueRegistryItem_t xQueueRegistry[ configQUEUE_REGISTRY_SIZE ]; #endif /* configQUEUE_REGISTRY_SIZE */ @@ -177,39 +201,39 @@ typedef struct QueueDefinition * to indicate that a task may require unblocking. When the queue in unlocked * these lock counts are inspected, and the appropriate action taken. */ -static void prvUnlockQueue( xQUEUE *pxQueue ) PRIVILEGED_FUNCTION; +static void prvUnlockQueue( Queue_t * const pxQueue ) PRIVILEGED_FUNCTION; /* * Uses a critical section to determine if there is any data in a queue. * * @return pdTRUE if the queue contains no items, otherwise pdFALSE. */ -static signed portBASE_TYPE prvIsQueueEmpty( const xQUEUE *pxQueue ) PRIVILEGED_FUNCTION; +static BaseType_t prvIsQueueEmpty( const Queue_t *pxQueue ) PRIVILEGED_FUNCTION; /* * Uses a critical section to determine if there is any space in a queue. * * @return pdTRUE if there is no space, otherwise pdFALSE; */ -static signed portBASE_TYPE prvIsQueueFull( const xQUEUE *pxQueue ) PRIVILEGED_FUNCTION; +static BaseType_t prvIsQueueFull( const Queue_t *pxQueue ) PRIVILEGED_FUNCTION; /* * Copies an item into the queue, either at the front of the queue or the * back of the queue. */ -static void prvCopyDataToQueue( xQUEUE *pxQueue, const void *pvItemToQueue, portBASE_TYPE xPosition ) PRIVILEGED_FUNCTION; +static BaseType_t prvCopyDataToQueue( Queue_t * const pxQueue, const void *pvItemToQueue, const BaseType_t xPosition ) PRIVILEGED_FUNCTION; /* * Copies an item out of a queue. */ -static void prvCopyDataFromQueue( xQUEUE * const pxQueue, const void *pvBuffer ) PRIVILEGED_FUNCTION; +static void prvCopyDataFromQueue( Queue_t * const pxQueue, void * const pvBuffer ) PRIVILEGED_FUNCTION; #if ( configUSE_QUEUE_SETS == 1 ) /* * Checks to see if a queue is a member of a queue set, and if so, notifies * the queue set that the queue contains data. */ - static portBASE_TYPE prvNotifyQueueSetContainer( xQUEUE *pxQueue, portBASE_TYPE xCopyPosition ); + static BaseType_t prvNotifyQueueSetContainer( const Queue_t * const pxQueue, const BaseType_t xCopyPosition ) PRIVILEGED_FUNCTION; #endif /*-----------------------------------------------------------*/ @@ -233,19 +257,18 @@ static void prvCopyDataFromQueue( xQUEUE * const pxQueue, const void *pvBuffer ) taskEXIT_CRITICAL() /*-----------------------------------------------------------*/ -portBASE_TYPE xQueueGenericReset( xQueueHandle xQueue, portBASE_TYPE xNewQueue ) +BaseType_t xQueueGenericReset( QueueHandle_t xQueue, BaseType_t xNewQueue ) { -xQUEUE *pxQueue; +Queue_t * const pxQueue = ( Queue_t * ) xQueue; - pxQueue = ( xQUEUE * ) xQueue; configASSERT( pxQueue ); taskENTER_CRITICAL(); { pxQueue->pcTail = pxQueue->pcHead + ( pxQueue->uxLength * pxQueue->uxItemSize ); - pxQueue->uxMessagesWaiting = ( unsigned portBASE_TYPE ) 0U; + pxQueue->uxMessagesWaiting = ( UBaseType_t ) 0U; pxQueue->pcWriteTo = pxQueue->pcHead; - pxQueue->pcReadFrom = pxQueue->pcHead + ( ( pxQueue->uxLength - ( unsigned portBASE_TYPE ) 1U ) * pxQueue->uxItemSize ); + pxQueue->u.pcReadFrom = pxQueue->pcHead + ( ( pxQueue->uxLength - ( UBaseType_t ) 1U ) * pxQueue->uxItemSize ); pxQueue->xRxLock = queueUNLOCKED; pxQueue->xTxLock = queueUNLOCKED; @@ -253,16 +276,24 @@ xQUEUE *pxQueue; { /* If there are tasks blocked waiting to read from the queue, then the tasks will remain blocked as after this function exits the queue - will still be empty. If there are tasks blocked waiting to write to + will still be empty. If there are tasks blocked waiting to write to the queue, then one should be unblocked as after this function exits it will be possible to write to it. */ if( listLIST_IS_EMPTY( &( pxQueue->xTasksWaitingToSend ) ) == pdFALSE ) { if( xTaskRemoveFromEventList( &( pxQueue->xTasksWaitingToSend ) ) == pdTRUE ) { - portYIELD_WITHIN_API(); + queueYIELD_IF_USING_PREEMPTION(); + } + else + { + mtCOVERAGE_TEST_MARKER(); } } + else + { + mtCOVERAGE_TEST_MARKER(); + } } else { @@ -279,56 +310,74 @@ xQUEUE *pxQueue; } /*-----------------------------------------------------------*/ -xQueueHandle xQueueGenericCreate( unsigned portBASE_TYPE uxQueueLength, unsigned portBASE_TYPE uxItemSize, unsigned char ucQueueType ) +QueueHandle_t xQueueGenericCreate( const UBaseType_t uxQueueLength, const UBaseType_t uxItemSize, const uint8_t ucQueueType ) { -xQUEUE *pxNewQueue; +Queue_t *pxNewQueue; size_t xQueueSizeInBytes; -xQueueHandle xReturn = NULL; +QueueHandle_t xReturn = NULL; /* Remove compiler warnings about unused parameters should configUSE_TRACE_FACILITY not be set to 1. */ ( void ) ucQueueType; - /* Allocate the new queue structure. */ - if( uxQueueLength > ( unsigned portBASE_TYPE ) 0 ) + configASSERT( uxQueueLength > ( UBaseType_t ) 0 ); + + if( uxItemSize == ( UBaseType_t ) 0 ) { - pxNewQueue = ( xQUEUE * ) pvPortMalloc( sizeof( xQUEUE ) ); - if( pxNewQueue != NULL ) - { - /* Create the list of pointers to queue items. The queue is one byte - longer than asked for to make wrap checking easier/faster. */ - xQueueSizeInBytes = ( size_t ) ( uxQueueLength * uxItemSize ) + ( size_t ) 1; + /* There is not going to be a queue storage area. */ + xQueueSizeInBytes = ( size_t ) 0; + } + else + { + /* The queue is one byte longer than asked for to make wrap checking + easier/faster. */ + xQueueSizeInBytes = ( size_t ) ( uxQueueLength * uxItemSize ) + ( size_t ) 1; /*lint !e961 MISRA exception as the casts are only redundant for some ports. */ + } - pxNewQueue->pcHead = ( signed char * ) pvPortMalloc( xQueueSizeInBytes ); - if( pxNewQueue->pcHead != NULL ) - { - /* Initialise the queue members as described above where the - queue type is defined. */ - pxNewQueue->uxLength = uxQueueLength; - pxNewQueue->uxItemSize = uxItemSize; - xQueueGenericReset( pxNewQueue, pdTRUE ); + /* Allocate the new queue structure and storage area. */ + pxNewQueue = ( Queue_t * ) pvPortMalloc( sizeof( Queue_t ) + xQueueSizeInBytes ); - #if ( configUSE_TRACE_FACILITY == 1 ) - { - pxNewQueue->ucQueueType = ucQueueType; - } - #endif /* configUSE_TRACE_FACILITY */ + if( pxNewQueue != NULL ) + { + if( uxItemSize == ( UBaseType_t ) 0 ) + { + /* No RAM was allocated for the queue storage area, but PC head + cannot be set to NULL because NULL is used as a key to say the queue + is used as a mutex. Therefore just set pcHead to point to the queue + as a benign value that is known to be within the memory map. */ + pxNewQueue->pcHead = ( int8_t * ) pxNewQueue; + } + else + { + /* Jump past the queue structure to find the location of the queue + storage area. */ + pxNewQueue->pcHead = ( ( int8_t * ) pxNewQueue ) + sizeof( Queue_t ); + } - #if( configUSE_QUEUE_SETS == 1 ) - { - pxNewQueue->pxQueueSetContainer = NULL; - } - #endif /* configUSE_QUEUE_SETS */ + /* Initialise the queue members as described above where the queue type + is defined. */ + pxNewQueue->uxLength = uxQueueLength; + pxNewQueue->uxItemSize = uxItemSize; + ( void ) xQueueGenericReset( pxNewQueue, pdTRUE ); - traceQUEUE_CREATE( pxNewQueue ); - xReturn = pxNewQueue; - } - else - { - traceQUEUE_CREATE_FAILED( ucQueueType ); - vPortFree( pxNewQueue ); - } + #if ( configUSE_TRACE_FACILITY == 1 ) + { + pxNewQueue->ucQueueType = ucQueueType; + } + #endif /* configUSE_TRACE_FACILITY */ + + #if( configUSE_QUEUE_SETS == 1 ) + { + pxNewQueue->pxQueueSetContainer = NULL; } + #endif /* configUSE_QUEUE_SETS */ + + traceQUEUE_CREATE( pxNewQueue ); + xReturn = pxNewQueue; + } + else + { + mtCOVERAGE_TEST_MARKER(); } configASSERT( xReturn ); @@ -339,16 +388,16 @@ xQueueHandle xReturn = NULL; #if ( configUSE_MUTEXES == 1 ) - xQueueHandle xQueueCreateMutex( unsigned char ucQueueType ) + QueueHandle_t xQueueCreateMutex( const uint8_t ucQueueType ) { - xQUEUE *pxNewQueue; + Queue_t *pxNewQueue; /* Prevent compiler warnings about unused parameters if configUSE_TRACE_FACILITY does not equal 1. */ ( void ) ucQueueType; /* Allocate the new queue structure. */ - pxNewQueue = ( xQUEUE * ) pvPortMalloc( sizeof( xQUEUE ) ); + pxNewQueue = ( Queue_t * ) pvPortMalloc( sizeof( Queue_t ) ); if( pxNewQueue != NULL ) { /* Information required for priority inheritance. */ @@ -358,14 +407,14 @@ xQueueHandle xReturn = NULL; /* Queues used as a mutex no data is actually copied into or out of the queue. */ pxNewQueue->pcWriteTo = NULL; - pxNewQueue->pcReadFrom = NULL; + pxNewQueue->u.pcReadFrom = NULL; /* Each mutex has a length of 1 (like a binary semaphore) and an item size of 0 as nothing is actually copied into or out of the mutex. */ - pxNewQueue->uxMessagesWaiting = ( unsigned portBASE_TYPE ) 0U; - pxNewQueue->uxLength = ( unsigned portBASE_TYPE ) 1U; - pxNewQueue->uxItemSize = ( unsigned portBASE_TYPE ) 0U; + pxNewQueue->uxMessagesWaiting = ( UBaseType_t ) 0U; + pxNewQueue->uxLength = ( UBaseType_t ) 1U; + pxNewQueue->uxItemSize = ( UBaseType_t ) 0U; pxNewQueue->xRxLock = queueUNLOCKED; pxNewQueue->xTxLock = queueUNLOCKED; @@ -388,14 +437,13 @@ xQueueHandle xReturn = NULL; traceCREATE_MUTEX( pxNewQueue ); /* Start with the semaphore in the expected state. */ - xQueueGenericSend( pxNewQueue, NULL, ( portTickType ) 0U, queueSEND_TO_BACK ); + ( void ) xQueueGenericSend( pxNewQueue, NULL, ( TickType_t ) 0U, queueSEND_TO_BACK ); } else { traceCREATE_MUTEX_FAILED(); } - configASSERT( pxNewQueue ); return pxNewQueue; } @@ -404,20 +452,20 @@ xQueueHandle xReturn = NULL; #if ( ( configUSE_MUTEXES == 1 ) && ( INCLUDE_xSemaphoreGetMutexHolder == 1 ) ) - void* xQueueGetMutexHolder( xQueueHandle xSemaphore ) + void* xQueueGetMutexHolder( QueueHandle_t xSemaphore ) { void *pxReturn; /* This function is called by xSemaphoreGetMutexHolder(), and should not - be called directly. Note: This is is a good way of determining if the + be called directly. Note: This is a good way of determining if the calling task is the mutex holder, but not a good way of determining the identity of the mutex holder, as the holder may change between the following critical section exiting and the function returning. */ taskENTER_CRITICAL(); { - if( ( ( xQUEUE * ) xSemaphore )->uxQueueType == queueQUEUE_IS_MUTEX ) + if( ( ( Queue_t * ) xSemaphore )->uxQueueType == queueQUEUE_IS_MUTEX ) { - pxReturn = ( void * ) ( ( xQUEUE * ) xSemaphore )->pxMutexHolder; + pxReturn = ( void * ) ( ( Queue_t * ) xSemaphore )->pxMutexHolder; } else { @@ -427,19 +475,18 @@ xQueueHandle xReturn = NULL; taskEXIT_CRITICAL(); return pxReturn; - } + } /*lint !e818 xSemaphore cannot be a pointer to const because it is a typedef. */ #endif /*-----------------------------------------------------------*/ #if ( configUSE_RECURSIVE_MUTEXES == 1 ) - portBASE_TYPE xQueueGiveMutexRecursive( xQueueHandle xMutex ) + BaseType_t xQueueGiveMutexRecursive( QueueHandle_t xMutex ) { - portBASE_TYPE xReturn; - xQUEUE *pxMutex; + BaseType_t xReturn; + Queue_t * const pxMutex = ( Queue_t * ) xMutex; - pxMutex = ( xQUEUE * ) xMutex; configASSERT( pxMutex ); /* If this is the task that holds the mutex then pxMutexHolder will not @@ -448,7 +495,7 @@ xQueueHandle xReturn = NULL; this is the only condition we are interested in it does not matter if pxMutexHolder is accessed simultaneously by another task. Therefore no mutual exclusion is required to test the pxMutexHolder variable. */ - if( pxMutex->pxMutexHolder == xTaskGetCurrentTaskHandle() ) + if( pxMutex->pxMutexHolder == ( void * ) xTaskGetCurrentTaskHandle() ) /*lint !e961 Not a redundant cast as TaskHandle_t is a typedef. */ { traceGIVE_MUTEX_RECURSIVE( pxMutex ); @@ -457,21 +504,26 @@ xQueueHandle xReturn = NULL; uxRecursiveCallCount is only modified by the mutex holder, and as there can only be one, no mutual exclusion is required to modify the uxRecursiveCallCount member. */ - ( pxMutex->uxRecursiveCallCount )--; + ( pxMutex->u.uxRecursiveCallCount )--; /* Have we unwound the call count? */ - if( pxMutex->uxRecursiveCallCount == 0 ) + if( pxMutex->u.uxRecursiveCallCount == ( UBaseType_t ) 0 ) { /* Return the mutex. This will automatically unblock any other task that might be waiting to access the mutex. */ - xQueueGenericSend( pxMutex, NULL, queueMUTEX_GIVE_BLOCK_TIME, queueSEND_TO_BACK ); + ( void ) xQueueGenericSend( pxMutex, NULL, queueMUTEX_GIVE_BLOCK_TIME, queueSEND_TO_BACK ); + } + else + { + mtCOVERAGE_TEST_MARKER(); } xReturn = pdPASS; } else { - /* We cannot give the mutex because we are not the holder. */ + /* The mutex cannot be given because the calling task is not the + holder. */ xReturn = pdFAIL; traceGIVE_MUTEX_RECURSIVE_FAILED( pxMutex ); @@ -485,12 +537,11 @@ xQueueHandle xReturn = NULL; #if ( configUSE_RECURSIVE_MUTEXES == 1 ) - portBASE_TYPE xQueueTakeMutexRecursive( xQueueHandle xMutex, portTickType xBlockTime ) + BaseType_t xQueueTakeMutexRecursive( QueueHandle_t xMutex, TickType_t xTicksToWait ) { - portBASE_TYPE xReturn; - xQUEUE *pxMutex; + BaseType_t xReturn; + Queue_t * const pxMutex = ( Queue_t * ) xMutex; - pxMutex = ( xQUEUE * ) xMutex; configASSERT( pxMutex ); /* Comments regarding mutual exclusion as per those within @@ -498,20 +549,21 @@ xQueueHandle xReturn = NULL; traceTAKE_MUTEX_RECURSIVE( pxMutex ); - if( pxMutex->pxMutexHolder == xTaskGetCurrentTaskHandle() ) + if( pxMutex->pxMutexHolder == ( void * ) xTaskGetCurrentTaskHandle() ) /*lint !e961 Cast is not redundant as TaskHandle_t is a typedef. */ { - ( pxMutex->uxRecursiveCallCount )++; + ( pxMutex->u.uxRecursiveCallCount )++; xReturn = pdPASS; } else { - xReturn = xQueueGenericReceive( pxMutex, NULL, xBlockTime, pdFALSE ); + xReturn = xQueueGenericReceive( pxMutex, NULL, xTicksToWait, pdFALSE ); - /* pdPASS will only be returned if we successfully obtained the mutex, - we may have blocked to reach here. */ + /* pdPASS will only be returned if the mutex was successfully + obtained. The calling task may have entered the Blocked state + before reaching here. */ if( xReturn == pdPASS ) { - ( pxMutex->uxRecursiveCallCount )++; + ( pxMutex->u.uxRecursiveCallCount )++; } else { @@ -527,15 +579,18 @@ xQueueHandle xReturn = NULL; #if ( configUSE_COUNTING_SEMAPHORES == 1 ) - xQueueHandle xQueueCreateCountingSemaphore( unsigned portBASE_TYPE uxCountValue, unsigned portBASE_TYPE uxInitialCount ) + QueueHandle_t xQueueCreateCountingSemaphore( const UBaseType_t uxMaxCount, const UBaseType_t uxInitialCount ) { - xQueueHandle xHandle; + QueueHandle_t xHandle; + + configASSERT( uxMaxCount != 0 ); + configASSERT( uxInitialCount <= uxMaxCount ); - xHandle = xQueueGenericCreate( ( unsigned portBASE_TYPE ) uxCountValue, queueSEMAPHORE_QUEUE_ITEM_LENGTH, queueQUEUE_TYPE_COUNTING_SEMAPHORE ); + xHandle = xQueueGenericCreate( uxMaxCount, queueSEMAPHORE_QUEUE_ITEM_LENGTH, queueQUEUE_TYPE_COUNTING_SEMAPHORE ); if( xHandle != NULL ) { - ( ( xQUEUE * ) xHandle )->uxMessagesWaiting = uxInitialCount; + ( ( Queue_t * ) xHandle )->uxMessagesWaiting = uxInitialCount; traceCREATE_COUNTING_SEMAPHORE(); } @@ -551,15 +606,21 @@ xQueueHandle xReturn = NULL; #endif /* configUSE_COUNTING_SEMAPHORES */ /*-----------------------------------------------------------*/ -signed portBASE_TYPE xQueueGenericSend( xQueueHandle xQueue, const void * const pvItemToQueue, portTickType xTicksToWait, portBASE_TYPE xCopyPosition ) +BaseType_t xQueueGenericSend( QueueHandle_t xQueue, const void * const pvItemToQueue, TickType_t xTicksToWait, const BaseType_t xCopyPosition ) { -signed portBASE_TYPE xEntryTimeSet = pdFALSE; -xTimeOutType xTimeOut; -xQUEUE *pxQueue; +BaseType_t xEntryTimeSet = pdFALSE, xYieldRequired; +TimeOut_t xTimeOut; +Queue_t * const pxQueue = ( Queue_t * ) xQueue; - pxQueue = ( xQUEUE * ) xQueue; configASSERT( pxQueue ); - configASSERT( !( ( pvItemToQueue == NULL ) && ( pxQueue->uxItemSize != ( unsigned portBASE_TYPE ) 0U ) ) ); + configASSERT( !( ( pvItemToQueue == NULL ) && ( pxQueue->uxItemSize != ( UBaseType_t ) 0U ) ) ); + configASSERT( !( ( xCopyPosition == queueOVERWRITE ) && ( pxQueue->uxLength != 1 ) ) ); + #if ( ( INCLUDE_xTaskGetSchedulerState == 1 ) || ( configUSE_TIMERS == 1 ) ) + { + configASSERT( !( ( xTaskGetSchedulerState() == taskSCHEDULER_SUSPENDED ) && ( xTicksToWait != 0 ) ) ); + } + #endif + /* This function relaxes the coding standard somewhat to allow return statements within the function itself. This is done in the interest @@ -568,12 +629,14 @@ xQUEUE *pxQueue; { taskENTER_CRITICAL(); { - /* Is there room on the queue now? To be running we must be - the highest priority task wanting to access the queue. */ - if( pxQueue->uxMessagesWaiting < pxQueue->uxLength ) + /* Is there room on the queue now? The running task must be the + highest priority task wanting to access the queue. If the head item + in the queue is to be overwritten then it does not matter if the + queue is full. */ + if( ( pxQueue->uxMessagesWaiting < pxQueue->uxLength ) || ( xCopyPosition == queueOVERWRITE ) ) { traceQUEUE_SEND( pxQueue ); - prvCopyDataToQueue( pxQueue, pvItemToQueue, xCopyPosition ); + xYieldRequired = prvCopyDataToQueue( pxQueue, pvItemToQueue, xCopyPosition ); #if ( configUSE_QUEUE_SETS == 1 ) { @@ -584,7 +647,11 @@ xQUEUE *pxQueue; /* The queue is a member of a queue set, and posting to the queue set caused a higher priority task to unblock. A context switch is required. */ - portYIELD_WITHIN_API(); + queueYIELD_IF_USING_PREEMPTION(); + } + else + { + mtCOVERAGE_TEST_MARKER(); } } else @@ -599,9 +666,25 @@ xQUEUE *pxQueue; our own so yield immediately. Yes it is ok to do this from within the critical section - the kernel takes care of that. */ - portYIELD_WITHIN_API(); + queueYIELD_IF_USING_PREEMPTION(); + } + else + { + mtCOVERAGE_TEST_MARKER(); } } + else if( xYieldRequired != pdFALSE ) + { + /* This path is a special case that will only get + executed if the task was holding multiple mutexes + and the mutexes were given back in an order that is + different to that in which they were taken. */ + queueYIELD_IF_USING_PREEMPTION(); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } } } #else /* configUSE_QUEUE_SETS */ @@ -616,21 +699,34 @@ xQUEUE *pxQueue; our own so yield immediately. Yes it is ok to do this from within the critical section - the kernel takes care of that. */ - portYIELD_WITHIN_API(); + queueYIELD_IF_USING_PREEMPTION(); + } + else + { + mtCOVERAGE_TEST_MARKER(); } } + else if( xYieldRequired != pdFALSE ) + { + /* This path is a special case that will only get + executed if the task was holding multiple mutexes and + the mutexes were given back in an order that is + different to that in which they were taken. */ + queueYIELD_IF_USING_PREEMPTION(); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } } #endif /* configUSE_QUEUE_SETS */ taskEXIT_CRITICAL(); - - /* Return to the original privilege level before exiting the - function. */ return pdPASS; } else { - if( xTicksToWait == ( portTickType ) 0 ) + if( xTicksToWait == ( TickType_t ) 0 ) { /* The queue was full and no block time is specified (or the block time has expired) so leave now. */ @@ -648,6 +744,11 @@ xQUEUE *pxQueue; vTaskSetTimeOutState( &xTimeOut ); xEntryTimeSet = pdTRUE; } + else + { + /* Entry time was already set. */ + mtCOVERAGE_TEST_MARKER(); + } } } taskEXIT_CRITICAL(); @@ -707,15 +808,14 @@ xQUEUE *pxQueue; #if ( configUSE_ALTERNATIVE_API == 1 ) - signed portBASE_TYPE xQueueAltGenericSend( xQueueHandle xQueue, const void * const pvItemToQueue, portTickType xTicksToWait, portBASE_TYPE xCopyPosition ) + BaseType_t xQueueAltGenericSend( QueueHandle_t xQueue, const void * const pvItemToQueue, TickType_t xTicksToWait, BaseType_t xCopyPosition ) { - signed portBASE_TYPE xEntryTimeSet = pdFALSE; - xTimeOutType xTimeOut; - xQUEUE *pxQueue; + BaseType_t xEntryTimeSet = pdFALSE; + TimeOut_t xTimeOut; + Queue_t * const pxQueue = ( Queue_t * ) xQueue; - pxQueue = ( xQUEUE * ) xQueue; configASSERT( pxQueue ); - configASSERT( !( ( pvItemToQueue == NULL ) && ( pxQueue->uxItemSize != ( unsigned portBASE_TYPE ) 0U ) ) ); + configASSERT( !( ( pvItemToQueue == NULL ) && ( pxQueue->uxItemSize != ( UBaseType_t ) 0U ) ) ); for( ;; ) { @@ -738,6 +838,14 @@ xQUEUE *pxQueue; our own so yield immediately. */ portYIELD_WITHIN_API(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } taskEXIT_CRITICAL(); @@ -745,7 +853,7 @@ xQUEUE *pxQueue; } else { - if( xTicksToWait == ( portTickType ) 0 ) + if( xTicksToWait == ( TickType_t ) 0 ) { taskEXIT_CRITICAL(); return errQUEUE_FULL; @@ -769,6 +877,10 @@ xQUEUE *pxQueue; vTaskPlaceOnEventList( &( pxQueue->xTasksWaitingToSend ), xTicksToWait ); portYIELD_WITHIN_API(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } } else { @@ -786,25 +898,24 @@ xQUEUE *pxQueue; #if ( configUSE_ALTERNATIVE_API == 1 ) - signed portBASE_TYPE xQueueAltGenericReceive( xQueueHandle xQueue, void * const pvBuffer, portTickType xTicksToWait, portBASE_TYPE xJustPeeking ) + BaseType_t xQueueAltGenericReceive( QueueHandle_t xQueue, void * const pvBuffer, TickType_t xTicksToWait, BaseType_t xJustPeeking ) { - signed portBASE_TYPE xEntryTimeSet = pdFALSE; - xTimeOutType xTimeOut; - signed char *pcOriginalReadPosition; - xQUEUE *pxQueue; + BaseType_t xEntryTimeSet = pdFALSE; + TimeOut_t xTimeOut; + int8_t *pcOriginalReadPosition; + Queue_t * const pxQueue = ( Queue_t * ) xQueue; - pxQueue = ( xQUEUE * ) xQueue; configASSERT( pxQueue ); - configASSERT( !( ( pvBuffer == NULL ) && ( pxQueue->uxItemSize != ( unsigned portBASE_TYPE ) 0U ) ) ); + configASSERT( !( ( pvBuffer == NULL ) && ( pxQueue->uxItemSize != ( UBaseType_t ) 0U ) ) ); for( ;; ) { taskENTER_CRITICAL(); { - if( pxQueue->uxMessagesWaiting > ( unsigned portBASE_TYPE ) 0 ) + if( pxQueue->uxMessagesWaiting > ( UBaseType_t ) 0 ) { /* Remember our read position in case we are just peeking. */ - pcOriginalReadPosition = pxQueue->pcReadFrom; + pcOriginalReadPosition = pxQueue->u.pcReadFrom; prvCopyDataFromQueue( pxQueue, pvBuffer ); @@ -812,7 +923,7 @@ xQUEUE *pxQueue; { traceQUEUE_RECEIVE( pxQueue ); - /* We are actually removing data. */ + /* Data is actually being removed (not just peeked). */ --( pxQueue->uxMessagesWaiting ); #if ( configUSE_MUTEXES == 1 ) @@ -821,7 +932,11 @@ xQUEUE *pxQueue; { /* Record the information required to implement priority inheritance should it become necessary. */ - pxQueue->pxMutexHolder = xTaskGetCurrentTaskHandle(); + pxQueue->pxMutexHolder = ( int8_t * ) xTaskGetCurrentTaskHandle(); + } + else + { + mtCOVERAGE_TEST_MARKER(); } } #endif @@ -832,15 +947,19 @@ xQUEUE *pxQueue; { portYIELD_WITHIN_API(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } } } else { traceQUEUE_PEEK( pxQueue ); - /* We are not removing the data, so reset our read + /* The data is not being removed, so reset our read pointer. */ - pxQueue->pcReadFrom = pcOriginalReadPosition; + pxQueue->u.pcReadFrom = pcOriginalReadPosition; /* The data is being left in the queue, so see if there are any other tasks waiting for the data. */ @@ -853,8 +972,15 @@ xQUEUE *pxQueue; /* The task waiting has a higher priority than this task. */ portYIELD_WITHIN_API(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } - } taskEXIT_CRITICAL(); @@ -862,7 +988,7 @@ xQUEUE *pxQueue; } else { - if( xTicksToWait == ( portTickType ) 0 ) + if( xTicksToWait == ( TickType_t ) 0 ) { taskEXIT_CRITICAL(); traceQUEUE_RECEIVE_FAILED( pxQueue ); @@ -889,11 +1015,15 @@ xQUEUE *pxQueue; { if( pxQueue->uxQueueType == queueQUEUE_IS_MUTEX ) { - portENTER_CRITICAL(); + taskENTER_CRITICAL(); { vTaskPriorityInherit( ( void * ) pxQueue->pxMutexHolder ); } - portEXIT_CRITICAL(); + taskEXIT_CRITICAL(); + } + else + { + mtCOVERAGE_TEST_MARKER(); } } #endif @@ -901,6 +1031,10 @@ xQUEUE *pxQueue; vTaskPlaceOnEventList( &( pxQueue->xTasksWaitingToReceive ), xTicksToWait ); portYIELD_WITHIN_API(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } } else { @@ -917,30 +1051,51 @@ xQUEUE *pxQueue; #endif /* configUSE_ALTERNATIVE_API */ /*-----------------------------------------------------------*/ -signed portBASE_TYPE xQueueGenericSendFromISR( xQueueHandle xQueue, const void * const pvItemToQueue, signed portBASE_TYPE *pxHigherPriorityTaskWoken, portBASE_TYPE xCopyPosition ) +BaseType_t xQueueGenericSendFromISR( QueueHandle_t xQueue, const void * const pvItemToQueue, BaseType_t * const pxHigherPriorityTaskWoken, const BaseType_t xCopyPosition ) { -signed portBASE_TYPE xReturn; -unsigned portBASE_TYPE uxSavedInterruptStatus; -xQUEUE *pxQueue; +BaseType_t xReturn; +UBaseType_t uxSavedInterruptStatus; +Queue_t * const pxQueue = ( Queue_t * ) xQueue; - pxQueue = ( xQUEUE * ) xQueue; configASSERT( pxQueue ); - configASSERT( !( ( pvItemToQueue == NULL ) && ( pxQueue->uxItemSize != ( unsigned portBASE_TYPE ) 0U ) ) ); - - /* Similar to xQueueGenericSend, except we don't block if there is no room - in the queue. Also we don't directly wake a task that was blocked on a - queue read, instead we return a flag to say whether a context switch is - required or not (i.e. has a task with a higher priority than us been woken - by this post). */ + configASSERT( !( ( pvItemToQueue == NULL ) && ( pxQueue->uxItemSize != ( UBaseType_t ) 0U ) ) ); + configASSERT( !( ( xCopyPosition == queueOVERWRITE ) && ( pxQueue->uxLength != 1 ) ) ); + + /* RTOS ports that support interrupt nesting have the concept of a maximum + system call (or maximum API call) interrupt priority. Interrupts that are + above the maximum system call priority are kept permanently enabled, even + when the RTOS kernel is in a critical section, but cannot make any calls to + FreeRTOS API functions. If configASSERT() is defined in FreeRTOSConfig.h + then portASSERT_IF_INTERRUPT_PRIORITY_INVALID() will result in an assertion + failure if a FreeRTOS API function is called from an interrupt that has been + assigned a priority above the configured maximum system call priority. + Only FreeRTOS functions that end in FromISR can be called from interrupts + that have been assigned a priority at or (logically) below the maximum + system call interrupt priority. FreeRTOS maintains a separate interrupt + safe API to ensure interrupt entry is as fast and as simple as possible. + More information (albeit Cortex-M specific) is provided on the following + link: http://www.freertos.org/RTOS-Cortex-M3-M4.html */ + portASSERT_IF_INTERRUPT_PRIORITY_INVALID(); + + /* Similar to xQueueGenericSend, except without blocking if there is no room + in the queue. Also don't directly wake a task that was blocked on a queue + read, instead return a flag to say whether a context switch is required or + not (i.e. has a task with a higher priority than us been woken by this + post). */ uxSavedInterruptStatus = portSET_INTERRUPT_MASK_FROM_ISR(); { - if( pxQueue->uxMessagesWaiting < pxQueue->uxLength ) + if( ( pxQueue->uxMessagesWaiting < pxQueue->uxLength ) || ( xCopyPosition == queueOVERWRITE ) ) { traceQUEUE_SEND_FROM_ISR( pxQueue ); - prvCopyDataToQueue( pxQueue, pvItemToQueue, xCopyPosition ); + /* Semaphores use xQueueGiveFromISR(), so pxQueue will not be a + semaphore or mutex. That means prvCopyDataToQueue() cannot result + in a task disinheriting a priority and prvCopyDataToQueue() can be + called here even though the disinherit function does not check if + the scheduler is suspended before accessing the ready lists. */ + ( void ) prvCopyDataToQueue( pxQueue, pvItemToQueue, xCopyPosition ); - /* If the queue is locked we do not alter the event list. This will + /* The event list is not altered if the queue is locked. This will be done when the queue is unlocked later. */ if( pxQueue->xTxLock == queueUNLOCKED ) { @@ -957,6 +1112,14 @@ xQUEUE *pxQueue; { *pxHigherPriorityTaskWoken = pdTRUE; } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } } else @@ -965,13 +1128,25 @@ xQUEUE *pxQueue; { if( xTaskRemoveFromEventList( &( pxQueue->xTasksWaitingToReceive ) ) != pdFALSE ) { - /* The task waiting has a higher priority so record that a - context switch is required. */ + /* The task waiting has a higher priority so + record that a context switch is required. */ if( pxHigherPriorityTaskWoken != NULL ) { *pxHigherPriorityTaskWoken = pdTRUE; } + else + { + mtCOVERAGE_TEST_MARKER(); + } } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } } } @@ -987,8 +1162,20 @@ xQUEUE *pxQueue; { *pxHigherPriorityTaskWoken = pdTRUE; } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } } + else + { + mtCOVERAGE_TEST_MARKER(); + } } #endif /* configUSE_QUEUE_SETS */ } @@ -1013,16 +1200,181 @@ xQUEUE *pxQueue; } /*-----------------------------------------------------------*/ -signed portBASE_TYPE xQueueGenericReceive( xQueueHandle xQueue, void * const pvBuffer, portTickType xTicksToWait, portBASE_TYPE xJustPeeking ) +BaseType_t xQueueGiveFromISR( QueueHandle_t xQueue, BaseType_t * const pxHigherPriorityTaskWoken ) { -signed portBASE_TYPE xEntryTimeSet = pdFALSE; -xTimeOutType xTimeOut; -signed char *pcOriginalReadPosition; -xQUEUE *pxQueue; +BaseType_t xReturn; +UBaseType_t uxSavedInterruptStatus; +Queue_t * const pxQueue = ( Queue_t * ) xQueue; + + /* Similar to xQueueGenericSendFromISR() but used with semaphores where the + item size is 0. Don't directly wake a task that was blocked on a queue + read, instead return a flag to say whether a context switch is required or + not (i.e. has a task with a higher priority than us been woken by this + post). */ - pxQueue = ( xQUEUE * ) xQueue; configASSERT( pxQueue ); - configASSERT( !( ( pvBuffer == NULL ) && ( pxQueue->uxItemSize != ( unsigned portBASE_TYPE ) 0U ) ) ); + + /* xQueueGenericSendFromISR() should be used instead of xQueueGiveFromISR() + if the item size is not 0. */ + configASSERT( pxQueue->uxItemSize == 0 ); + + /* Normally a mutex would not be given from an interrupt, especially if + there is a mutex holder, as priority inheritance makes no sense for an + interrupts, only tasks. */ + configASSERT( !( ( pxQueue->uxQueueType == queueQUEUE_IS_MUTEX ) && ( pxQueue->pxMutexHolder != NULL ) ) ); + + /* RTOS ports that support interrupt nesting have the concept of a maximum + system call (or maximum API call) interrupt priority. Interrupts that are + above the maximum system call priority are kept permanently enabled, even + when the RTOS kernel is in a critical section, but cannot make any calls to + FreeRTOS API functions. If configASSERT() is defined in FreeRTOSConfig.h + then portASSERT_IF_INTERRUPT_PRIORITY_INVALID() will result in an assertion + failure if a FreeRTOS API function is called from an interrupt that has been + assigned a priority above the configured maximum system call priority. + Only FreeRTOS functions that end in FromISR can be called from interrupts + that have been assigned a priority at or (logically) below the maximum + system call interrupt priority. FreeRTOS maintains a separate interrupt + safe API to ensure interrupt entry is as fast and as simple as possible. + More information (albeit Cortex-M specific) is provided on the following + link: http://www.freertos.org/RTOS-Cortex-M3-M4.html */ + portASSERT_IF_INTERRUPT_PRIORITY_INVALID(); + + uxSavedInterruptStatus = portSET_INTERRUPT_MASK_FROM_ISR(); + { + /* When the queue is used to implement a semaphore no data is ever + moved through the queue but it is still valid to see if the queue 'has + space'. */ + if( pxQueue->uxMessagesWaiting < pxQueue->uxLength ) + { + traceQUEUE_SEND_FROM_ISR( pxQueue ); + + /* A task can only have an inherited priority if it is a mutex + holder - and if there is a mutex holder then the mutex cannot be + given from an ISR. As this is the ISR version of the function it + can be assumed there is no mutex holder and no need to determine if + priority disinheritance is needed. Simply increase the count of + messages (semaphores) available. */ + ++( pxQueue->uxMessagesWaiting ); + + /* The event list is not altered if the queue is locked. This will + be done when the queue is unlocked later. */ + if( pxQueue->xTxLock == queueUNLOCKED ) + { + #if ( configUSE_QUEUE_SETS == 1 ) + { + if( pxQueue->pxQueueSetContainer != NULL ) + { + if( prvNotifyQueueSetContainer( pxQueue, queueSEND_TO_BACK ) == pdTRUE ) + { + /* The semaphore is a member of a queue set, and + posting to the queue set caused a higher priority + task to unblock. A context switch is required. */ + if( pxHigherPriorityTaskWoken != NULL ) + { + *pxHigherPriorityTaskWoken = pdTRUE; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + if( listLIST_IS_EMPTY( &( pxQueue->xTasksWaitingToReceive ) ) == pdFALSE ) + { + if( xTaskRemoveFromEventList( &( pxQueue->xTasksWaitingToReceive ) ) != pdFALSE ) + { + /* The task waiting has a higher priority so + record that a context switch is required. */ + if( pxHigherPriorityTaskWoken != NULL ) + { + *pxHigherPriorityTaskWoken = pdTRUE; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + } + #else /* configUSE_QUEUE_SETS */ + { + if( listLIST_IS_EMPTY( &( pxQueue->xTasksWaitingToReceive ) ) == pdFALSE ) + { + if( xTaskRemoveFromEventList( &( pxQueue->xTasksWaitingToReceive ) ) != pdFALSE ) + { + /* The task waiting has a higher priority so record that a + context switch is required. */ + if( pxHigherPriorityTaskWoken != NULL ) + { + *pxHigherPriorityTaskWoken = pdTRUE; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + #endif /* configUSE_QUEUE_SETS */ + } + else + { + /* Increment the lock count so the task that unlocks the queue + knows that data was posted while it was locked. */ + ++( pxQueue->xTxLock ); + } + + xReturn = pdPASS; + } + else + { + traceQUEUE_SEND_FROM_ISR_FAILED( pxQueue ); + xReturn = errQUEUE_FULL; + } + } + portCLEAR_INTERRUPT_MASK_FROM_ISR( uxSavedInterruptStatus ); + + return xReturn; +} +/*-----------------------------------------------------------*/ + +BaseType_t xQueueGenericReceive( QueueHandle_t xQueue, void * const pvBuffer, TickType_t xTicksToWait, const BaseType_t xJustPeeking ) +{ +BaseType_t xEntryTimeSet = pdFALSE; +TimeOut_t xTimeOut; +int8_t *pcOriginalReadPosition; +Queue_t * const pxQueue = ( Queue_t * ) xQueue; + + configASSERT( pxQueue ); + configASSERT( !( ( pvBuffer == NULL ) && ( pxQueue->uxItemSize != ( UBaseType_t ) 0U ) ) ); + #if ( ( INCLUDE_xTaskGetSchedulerState == 1 ) || ( configUSE_TIMERS == 1 ) ) + { + configASSERT( !( ( xTaskGetSchedulerState() == taskSCHEDULER_SUSPENDED ) && ( xTicksToWait != 0 ) ) ); + } + #endif /* This function relaxes the coding standard somewhat to allow return statements within the function itself. This is done in the interest @@ -1032,12 +1384,13 @@ xQUEUE *pxQueue; { taskENTER_CRITICAL(); { - /* Is there data in the queue now? To be running we must be - the highest priority task wanting to access the queue. */ - if( pxQueue->uxMessagesWaiting > ( unsigned portBASE_TYPE ) 0 ) + /* Is there data in the queue now? To be running the calling task + must be the highest priority task wanting to access the queue. */ + if( pxQueue->uxMessagesWaiting > ( UBaseType_t ) 0 ) { - /* Remember our read position in case we are just peeking. */ - pcOriginalReadPosition = pxQueue->pcReadFrom; + /* Remember the read position in case the queue is only being + peeked. */ + pcOriginalReadPosition = pxQueue->u.pcReadFrom; prvCopyDataFromQueue( pxQueue, pvBuffer ); @@ -1045,7 +1398,7 @@ xQUEUE *pxQueue; { traceQUEUE_RECEIVE( pxQueue ); - /* We are actually removing data. */ + /* Actually removing data, not just peeking. */ --( pxQueue->uxMessagesWaiting ); #if ( configUSE_MUTEXES == 1 ) @@ -1054,17 +1407,29 @@ xQUEUE *pxQueue; { /* Record the information required to implement priority inheritance should it become necessary. */ - pxQueue->pxMutexHolder = xTaskGetCurrentTaskHandle(); + pxQueue->pxMutexHolder = ( int8_t * ) pvTaskIncrementMutexHeldCount(); /*lint !e961 Cast is not redundant as TaskHandle_t is a typedef. */ + } + else + { + mtCOVERAGE_TEST_MARKER(); } } - #endif + #endif /* configUSE_MUTEXES */ if( listLIST_IS_EMPTY( &( pxQueue->xTasksWaitingToSend ) ) == pdFALSE ) { if( xTaskRemoveFromEventList( &( pxQueue->xTasksWaitingToSend ) ) == pdTRUE ) { - portYIELD_WITHIN_API(); + queueYIELD_IF_USING_PREEMPTION(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } } else @@ -1073,20 +1438,26 @@ xQUEUE *pxQueue; /* The data is not being removed, so reset the read pointer. */ - pxQueue->pcReadFrom = pcOriginalReadPosition; + pxQueue->u.pcReadFrom = pcOriginalReadPosition; /* The data is being left in the queue, so see if there are any other tasks waiting for the data. */ if( listLIST_IS_EMPTY( &( pxQueue->xTasksWaitingToReceive ) ) == pdFALSE ) { - /* Tasks that are removed from the event list will get added to - the pending ready list as the scheduler is still suspended. */ if( xTaskRemoveFromEventList( &( pxQueue->xTasksWaitingToReceive ) ) != pdFALSE ) { /* The task waiting has a higher priority than this task. */ - portYIELD_WITHIN_API(); + queueYIELD_IF_USING_PREEMPTION(); + } + else + { + mtCOVERAGE_TEST_MARKER(); } } + else + { + mtCOVERAGE_TEST_MARKER(); + } } taskEXIT_CRITICAL(); @@ -1094,7 +1465,7 @@ xQUEUE *pxQueue; } else { - if( xTicksToWait == ( portTickType ) 0 ) + if( xTicksToWait == ( TickType_t ) 0 ) { /* The queue was empty and no block time is specified (or the block time has expired) so leave now. */ @@ -1109,6 +1480,11 @@ xQUEUE *pxQueue; vTaskSetTimeOutState( &xTimeOut ); xEntryTimeSet = pdTRUE; } + else + { + /* Entry time was already set. */ + mtCOVERAGE_TEST_MARKER(); + } } } taskEXIT_CRITICAL(); @@ -1130,11 +1506,15 @@ xQUEUE *pxQueue; { if( pxQueue->uxQueueType == queueQUEUE_IS_MUTEX ) { - portENTER_CRITICAL(); + taskENTER_CRITICAL(); { vTaskPriorityInherit( ( void * ) pxQueue->pxMutexHolder ); } - portEXIT_CRITICAL(); + taskEXIT_CRITICAL(); + } + else + { + mtCOVERAGE_TEST_MARKER(); } } #endif @@ -1145,6 +1525,10 @@ xQUEUE *pxQueue; { portYIELD_WITHIN_API(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } } else { @@ -1164,29 +1548,45 @@ xQUEUE *pxQueue; } /*-----------------------------------------------------------*/ -signed portBASE_TYPE xQueueReceiveFromISR( xQueueHandle xQueue, void * const pvBuffer, signed portBASE_TYPE *pxHigherPriorityTaskWoken ) +BaseType_t xQueueReceiveFromISR( QueueHandle_t xQueue, void * const pvBuffer, BaseType_t * const pxHigherPriorityTaskWoken ) { -signed portBASE_TYPE xReturn; -unsigned portBASE_TYPE uxSavedInterruptStatus; -xQUEUE *pxQueue; +BaseType_t xReturn; +UBaseType_t uxSavedInterruptStatus; +Queue_t * const pxQueue = ( Queue_t * ) xQueue; - pxQueue = ( xQUEUE * ) xQueue; configASSERT( pxQueue ); - configASSERT( !( ( pvBuffer == NULL ) && ( pxQueue->uxItemSize != ( unsigned portBASE_TYPE ) 0U ) ) ); + configASSERT( !( ( pvBuffer == NULL ) && ( pxQueue->uxItemSize != ( UBaseType_t ) 0U ) ) ); + + /* RTOS ports that support interrupt nesting have the concept of a maximum + system call (or maximum API call) interrupt priority. Interrupts that are + above the maximum system call priority are kept permanently enabled, even + when the RTOS kernel is in a critical section, but cannot make any calls to + FreeRTOS API functions. If configASSERT() is defined in FreeRTOSConfig.h + then portASSERT_IF_INTERRUPT_PRIORITY_INVALID() will result in an assertion + failure if a FreeRTOS API function is called from an interrupt that has been + assigned a priority above the configured maximum system call priority. + Only FreeRTOS functions that end in FromISR can be called from interrupts + that have been assigned a priority at or (logically) below the maximum + system call interrupt priority. FreeRTOS maintains a separate interrupt + safe API to ensure interrupt entry is as fast and as simple as possible. + More information (albeit Cortex-M specific) is provided on the following + link: http://www.freertos.org/RTOS-Cortex-M3-M4.html */ + portASSERT_IF_INTERRUPT_PRIORITY_INVALID(); uxSavedInterruptStatus = portSET_INTERRUPT_MASK_FROM_ISR(); { - /* We cannot block from an ISR, so check there is data available. */ - if( pxQueue->uxMessagesWaiting > ( unsigned portBASE_TYPE ) 0 ) + /* Cannot block in an ISR, so check there is data available. */ + if( pxQueue->uxMessagesWaiting > ( UBaseType_t ) 0 ) { traceQUEUE_RECEIVE_FROM_ISR( pxQueue ); prvCopyDataFromQueue( pxQueue, pvBuffer ); --( pxQueue->uxMessagesWaiting ); - /* If the queue is locked we will not modify the event list. Instead - we update the lock count so the task that unlocks the queue will know - that an ISR has removed data while the queue was locked. */ + /* If the queue is locked the event list will not be modified. + Instead update the lock count so the task that unlocks the queue + will know that an ISR has removed data while the queue was + locked. */ if( pxQueue->xRxLock == queueUNLOCKED ) { if( listLIST_IS_EMPTY( &( pxQueue->xTasksWaitingToSend ) ) == pdFALSE ) @@ -1199,8 +1599,20 @@ xQUEUE *pxQueue; { *pxHigherPriorityTaskWoken = pdTRUE; } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } } + else + { + mtCOVERAGE_TEST_MARKER(); + } } else { @@ -1223,55 +1635,127 @@ xQUEUE *pxQueue; } /*-----------------------------------------------------------*/ -unsigned portBASE_TYPE uxQueueMessagesWaiting( const xQueueHandle xQueue ) +BaseType_t xQueuePeekFromISR( QueueHandle_t xQueue, void * const pvBuffer ) +{ +BaseType_t xReturn; +UBaseType_t uxSavedInterruptStatus; +int8_t *pcOriginalReadPosition; +Queue_t * const pxQueue = ( Queue_t * ) xQueue; + + configASSERT( pxQueue ); + configASSERT( !( ( pvBuffer == NULL ) && ( pxQueue->uxItemSize != ( UBaseType_t ) 0U ) ) ); + configASSERT( pxQueue->uxItemSize != 0 ); /* Can't peek a semaphore. */ + + /* RTOS ports that support interrupt nesting have the concept of a maximum + system call (or maximum API call) interrupt priority. Interrupts that are + above the maximum system call priority are kept permanently enabled, even + when the RTOS kernel is in a critical section, but cannot make any calls to + FreeRTOS API functions. If configASSERT() is defined in FreeRTOSConfig.h + then portASSERT_IF_INTERRUPT_PRIORITY_INVALID() will result in an assertion + failure if a FreeRTOS API function is called from an interrupt that has been + assigned a priority above the configured maximum system call priority. + Only FreeRTOS functions that end in FromISR can be called from interrupts + that have been assigned a priority at or (logically) below the maximum + system call interrupt priority. FreeRTOS maintains a separate interrupt + safe API to ensure interrupt entry is as fast and as simple as possible. + More information (albeit Cortex-M specific) is provided on the following + link: http://www.freertos.org/RTOS-Cortex-M3-M4.html */ + portASSERT_IF_INTERRUPT_PRIORITY_INVALID(); + + uxSavedInterruptStatus = portSET_INTERRUPT_MASK_FROM_ISR(); + { + /* Cannot block in an ISR, so check there is data available. */ + if( pxQueue->uxMessagesWaiting > ( UBaseType_t ) 0 ) + { + traceQUEUE_PEEK_FROM_ISR( pxQueue ); + + /* Remember the read position so it can be reset as nothing is + actually being removed from the queue. */ + pcOriginalReadPosition = pxQueue->u.pcReadFrom; + prvCopyDataFromQueue( pxQueue, pvBuffer ); + pxQueue->u.pcReadFrom = pcOriginalReadPosition; + + xReturn = pdPASS; + } + else + { + xReturn = pdFAIL; + traceQUEUE_PEEK_FROM_ISR_FAILED( pxQueue ); + } + } + portCLEAR_INTERRUPT_MASK_FROM_ISR( uxSavedInterruptStatus ); + + return xReturn; +} +/*-----------------------------------------------------------*/ + +UBaseType_t uxQueueMessagesWaiting( const QueueHandle_t xQueue ) { -unsigned portBASE_TYPE uxReturn; +UBaseType_t uxReturn; configASSERT( xQueue ); taskENTER_CRITICAL(); - uxReturn = ( ( xQUEUE * ) xQueue )->uxMessagesWaiting; + { + uxReturn = ( ( Queue_t * ) xQueue )->uxMessagesWaiting; + } taskEXIT_CRITICAL(); return uxReturn; -} +} /*lint !e818 Pointer cannot be declared const as xQueue is a typedef not pointer. */ /*-----------------------------------------------------------*/ -unsigned portBASE_TYPE uxQueueMessagesWaitingFromISR( const xQueueHandle xQueue ) +UBaseType_t uxQueueSpacesAvailable( const QueueHandle_t xQueue ) { -unsigned portBASE_TYPE uxReturn; +UBaseType_t uxReturn; +Queue_t *pxQueue; + + pxQueue = ( Queue_t * ) xQueue; + configASSERT( pxQueue ); + + taskENTER_CRITICAL(); + { + uxReturn = pxQueue->uxLength - pxQueue->uxMessagesWaiting; + } + taskEXIT_CRITICAL(); + + return uxReturn; +} /*lint !e818 Pointer cannot be declared const as xQueue is a typedef not pointer. */ +/*-----------------------------------------------------------*/ + +UBaseType_t uxQueueMessagesWaitingFromISR( const QueueHandle_t xQueue ) +{ +UBaseType_t uxReturn; configASSERT( xQueue ); - uxReturn = ( ( xQUEUE * ) xQueue )->uxMessagesWaiting; + uxReturn = ( ( Queue_t * ) xQueue )->uxMessagesWaiting; return uxReturn; -} +} /*lint !e818 Pointer cannot be declared const as xQueue is a typedef not pointer. */ /*-----------------------------------------------------------*/ -void vQueueDelete( xQueueHandle xQueue ) +void vQueueDelete( QueueHandle_t xQueue ) { -xQUEUE *pxQueue; +Queue_t * const pxQueue = ( Queue_t * ) xQueue; - pxQueue = ( xQUEUE * ) xQueue; configASSERT( pxQueue ); traceQUEUE_DELETE( pxQueue ); #if ( configQUEUE_REGISTRY_SIZE > 0 ) { - prvQueueUnregisterQueue( pxQueue ); + vQueueUnregisterQueue( pxQueue ); } #endif - vPortFree( pxQueue->pcHead ); vPortFree( pxQueue ); } /*-----------------------------------------------------------*/ #if ( configUSE_TRACE_FACILITY == 1 ) - unsigned char ucQueueGetQueueNumber( xQueueHandle xQueue ) + UBaseType_t uxQueueGetQueueNumber( QueueHandle_t xQueue ) { - return ( ( xQUEUE * ) xQueue )->ucQueueNumber; + return ( ( Queue_t * ) xQueue )->uxQueueNumber; } #endif /* configUSE_TRACE_FACILITY */ @@ -1279,9 +1763,9 @@ xQUEUE *pxQueue; #if ( configUSE_TRACE_FACILITY == 1 ) - void vQueueSetQueueNumber( xQueueHandle xQueue, unsigned char ucQueueNumber ) + void vQueueSetQueueNumber( QueueHandle_t xQueue, UBaseType_t uxQueueNumber ) { - ( ( xQUEUE * ) xQueue )->ucQueueNumber = ucQueueNumber; + ( ( Queue_t * ) xQueue )->uxQueueNumber = uxQueueNumber; } #endif /* configUSE_TRACE_FACILITY */ @@ -1289,67 +1773,107 @@ xQUEUE *pxQueue; #if ( configUSE_TRACE_FACILITY == 1 ) - unsigned char ucQueueGetQueueType( xQueueHandle xQueue ) + uint8_t ucQueueGetQueueType( QueueHandle_t xQueue ) { - return ( ( xQUEUE * ) xQueue )->ucQueueType; + return ( ( Queue_t * ) xQueue )->ucQueueType; } #endif /* configUSE_TRACE_FACILITY */ /*-----------------------------------------------------------*/ -static void prvCopyDataToQueue( xQUEUE *pxQueue, const void *pvItemToQueue, portBASE_TYPE xPosition ) +static BaseType_t prvCopyDataToQueue( Queue_t * const pxQueue, const void *pvItemToQueue, const BaseType_t xPosition ) { - if( pxQueue->uxItemSize == ( unsigned portBASE_TYPE ) 0 ) +BaseType_t xReturn = pdFALSE; + + if( pxQueue->uxItemSize == ( UBaseType_t ) 0 ) { #if ( configUSE_MUTEXES == 1 ) { if( pxQueue->uxQueueType == queueQUEUE_IS_MUTEX ) { /* The mutex is no longer being held. */ - vTaskPriorityDisinherit( ( void * ) pxQueue->pxMutexHolder ); + xReturn = xTaskPriorityDisinherit( ( void * ) pxQueue->pxMutexHolder ); pxQueue->pxMutexHolder = NULL; } + else + { + mtCOVERAGE_TEST_MARKER(); + } } - #endif + #endif /* configUSE_MUTEXES */ } else if( xPosition == queueSEND_TO_BACK ) { - memcpy( ( void * ) pxQueue->pcWriteTo, pvItemToQueue, ( unsigned ) pxQueue->uxItemSize ); + ( void ) memcpy( ( void * ) pxQueue->pcWriteTo, pvItemToQueue, ( size_t ) pxQueue->uxItemSize ); /*lint !e961 !e418 MISRA exception as the casts are only redundant for some ports, plus previous logic ensures a null pointer can only be passed to memcpy() if the copy size is 0. */ pxQueue->pcWriteTo += pxQueue->uxItemSize; - if( pxQueue->pcWriteTo >= pxQueue->pcTail ) + if( pxQueue->pcWriteTo >= pxQueue->pcTail ) /*lint !e946 MISRA exception justified as comparison of pointers is the cleanest solution. */ { pxQueue->pcWriteTo = pxQueue->pcHead; } + else + { + mtCOVERAGE_TEST_MARKER(); + } } else { - memcpy( ( void * ) pxQueue->pcReadFrom, pvItemToQueue, ( unsigned ) pxQueue->uxItemSize ); - pxQueue->pcReadFrom -= pxQueue->uxItemSize; - if( pxQueue->pcReadFrom < pxQueue->pcHead ) + ( void ) memcpy( ( void * ) pxQueue->u.pcReadFrom, pvItemToQueue, ( size_t ) pxQueue->uxItemSize ); /*lint !e961 MISRA exception as the casts are only redundant for some ports. */ + pxQueue->u.pcReadFrom -= pxQueue->uxItemSize; + if( pxQueue->u.pcReadFrom < pxQueue->pcHead ) /*lint !e946 MISRA exception justified as comparison of pointers is the cleanest solution. */ + { + pxQueue->u.pcReadFrom = ( pxQueue->pcTail - pxQueue->uxItemSize ); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + if( xPosition == queueOVERWRITE ) + { + if( pxQueue->uxMessagesWaiting > ( UBaseType_t ) 0 ) + { + /* An item is not being added but overwritten, so subtract + one from the recorded number of items in the queue so when + one is added again below the number of recorded items remains + correct. */ + --( pxQueue->uxMessagesWaiting ); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else { - pxQueue->pcReadFrom = ( pxQueue->pcTail - pxQueue->uxItemSize ); + mtCOVERAGE_TEST_MARKER(); } } ++( pxQueue->uxMessagesWaiting ); + + return xReturn; } /*-----------------------------------------------------------*/ -static void prvCopyDataFromQueue( xQUEUE * const pxQueue, const void *pvBuffer ) +static void prvCopyDataFromQueue( Queue_t * const pxQueue, void * const pvBuffer ) { - if( pxQueue->uxQueueType != queueQUEUE_IS_MUTEX ) + if( pxQueue->uxItemSize != ( UBaseType_t ) 0 ) { - pxQueue->pcReadFrom += pxQueue->uxItemSize; - if( pxQueue->pcReadFrom >= pxQueue->pcTail ) + pxQueue->u.pcReadFrom += pxQueue->uxItemSize; + if( pxQueue->u.pcReadFrom >= pxQueue->pcTail ) /*lint !e946 MISRA exception justified as use of the relational operator is the cleanest solutions. */ + { + pxQueue->u.pcReadFrom = pxQueue->pcHead; + } + else { - pxQueue->pcReadFrom = pxQueue->pcHead; + mtCOVERAGE_TEST_MARKER(); } - memcpy( ( void * ) pvBuffer, ( void * ) pxQueue->pcReadFrom, ( unsigned ) pxQueue->uxItemSize ); + ( void ) memcpy( ( void * ) pvBuffer, ( void * ) pxQueue->u.pcReadFrom, ( size_t ) pxQueue->uxItemSize ); /*lint !e961 !e418 MISRA exception as the casts are only redundant for some ports. Also previous logic ensures a null pointer can only be passed to memcpy() when the count is 0. */ } } /*-----------------------------------------------------------*/ -static void prvUnlockQueue( xQUEUE *pxQueue ) +static void prvUnlockQueue( Queue_t * const pxQueue ) { /* THIS FUNCTION MUST BE CALLED WITH THE SCHEDULER SUSPENDED. */ @@ -1375,6 +1899,10 @@ static void prvUnlockQueue( xQUEUE *pxQueue ) A context switch is required. */ vTaskMissedYield(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } } else { @@ -1388,6 +1916,10 @@ static void prvUnlockQueue( xQUEUE *pxQueue ) context switch is required. */ vTaskMissedYield(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } } else { @@ -1407,6 +1939,10 @@ static void prvUnlockQueue( xQUEUE *pxQueue ) context switch is required. */ vTaskMissedYield(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } } else { @@ -1433,6 +1969,10 @@ static void prvUnlockQueue( xQUEUE *pxQueue ) { vTaskMissedYield(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } --( pxQueue->xRxLock ); } @@ -1448,13 +1988,13 @@ static void prvUnlockQueue( xQUEUE *pxQueue ) } /*-----------------------------------------------------------*/ -static signed portBASE_TYPE prvIsQueueEmpty( const xQUEUE *pxQueue ) +static BaseType_t prvIsQueueEmpty( const Queue_t *pxQueue ) { -signed portBASE_TYPE xReturn; +BaseType_t xReturn; taskENTER_CRITICAL(); { - if( pxQueue->uxMessagesWaiting == 0 ) + if( pxQueue->uxMessagesWaiting == ( UBaseType_t ) 0 ) { xReturn = pdTRUE; } @@ -1469,12 +2009,12 @@ signed portBASE_TYPE xReturn; } /*-----------------------------------------------------------*/ -signed portBASE_TYPE xQueueIsQueueEmptyFromISR( const xQueueHandle xQueue ) +BaseType_t xQueueIsQueueEmptyFromISR( const QueueHandle_t xQueue ) { -signed portBASE_TYPE xReturn; +BaseType_t xReturn; configASSERT( xQueue ); - if( ( ( xQUEUE * ) xQueue )->uxMessagesWaiting == 0 ) + if( ( ( Queue_t * ) xQueue )->uxMessagesWaiting == ( UBaseType_t ) 0 ) { xReturn = pdTRUE; } @@ -1484,12 +2024,12 @@ signed portBASE_TYPE xReturn; } return xReturn; -} +} /*lint !e818 xQueue could not be pointer to const because it is a typedef. */ /*-----------------------------------------------------------*/ -static signed portBASE_TYPE prvIsQueueFull( const xQUEUE *pxQueue ) +static BaseType_t prvIsQueueFull( const Queue_t *pxQueue ) { -signed portBASE_TYPE xReturn; +BaseType_t xReturn; taskENTER_CRITICAL(); { @@ -1508,12 +2048,12 @@ signed portBASE_TYPE xReturn; } /*-----------------------------------------------------------*/ -signed portBASE_TYPE xQueueIsQueueFullFromISR( const xQueueHandle xQueue ) +BaseType_t xQueueIsQueueFullFromISR( const QueueHandle_t xQueue ) { -signed portBASE_TYPE xReturn; +BaseType_t xReturn; configASSERT( xQueue ); - if( ( ( xQUEUE * ) xQueue )->uxMessagesWaiting == ( ( xQUEUE * ) xQueue )->uxLength ) + if( ( ( Queue_t * ) xQueue )->uxMessagesWaiting == ( ( Queue_t * ) xQueue )->uxLength ) { xReturn = pdTRUE; } @@ -1523,17 +2063,15 @@ signed portBASE_TYPE xReturn; } return xReturn; -} +} /*lint !e818 xQueue could not be pointer to const because it is a typedef. */ /*-----------------------------------------------------------*/ #if ( configUSE_CO_ROUTINES == 1 ) - signed portBASE_TYPE xQueueCRSend( xQueueHandle xQueue, const void *pvItemToQueue, portTickType xTicksToWait ) + BaseType_t xQueueCRSend( QueueHandle_t xQueue, const void *pvItemToQueue, TickType_t xTicksToWait ) { - signed portBASE_TYPE xReturn; - xQUEUE *pxQueue; - - pxQueue = ( xQUEUE * ) xQueue; + BaseType_t xReturn; + Queue_t * const pxQueue = ( Queue_t * ) xQueue; /* If the queue is already full we may have to block. A critical section is required to prevent an interrupt removing something from the queue @@ -1544,7 +2082,7 @@ signed portBASE_TYPE xReturn; { /* The queue is full - do we want to block or just leave without posting? */ - if( xTicksToWait > ( portTickType ) 0 ) + if( xTicksToWait > ( TickType_t ) 0 ) { /* As this is called from a coroutine we cannot block directly, but return indicating that we need to block. */ @@ -1582,6 +2120,14 @@ signed portBASE_TYPE xReturn; that a yield might be appropriate. */ xReturn = errQUEUE_YIELD; } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } } else @@ -1599,23 +2145,21 @@ signed portBASE_TYPE xReturn; #if ( configUSE_CO_ROUTINES == 1 ) - signed portBASE_TYPE xQueueCRReceive( xQueueHandle xQueue, void *pvBuffer, portTickType xTicksToWait ) + BaseType_t xQueueCRReceive( QueueHandle_t xQueue, void *pvBuffer, TickType_t xTicksToWait ) { - signed portBASE_TYPE xReturn; - xQUEUE *pxQueue; - - pxQueue = ( xQUEUE * ) xQueue; + BaseType_t xReturn; + Queue_t * const pxQueue = ( Queue_t * ) xQueue; /* If the queue is already empty we may have to block. A critical section is required to prevent an interrupt adding something to the queue between the check to see if the queue is empty and blocking on the queue. */ portDISABLE_INTERRUPTS(); { - if( pxQueue->uxMessagesWaiting == ( unsigned portBASE_TYPE ) 0 ) + if( pxQueue->uxMessagesWaiting == ( UBaseType_t ) 0 ) { /* There are no messages in the queue, do we want to block or just leave with nothing? */ - if( xTicksToWait > ( portTickType ) 0 ) + if( xTicksToWait > ( TickType_t ) 0 ) { /* As this is a co-routine we cannot block directly, but return indicating that we need to block. */ @@ -1629,21 +2173,29 @@ signed portBASE_TYPE xReturn; return errQUEUE_FULL; } } + else + { + mtCOVERAGE_TEST_MARKER(); + } } portENABLE_INTERRUPTS(); portDISABLE_INTERRUPTS(); { - if( pxQueue->uxMessagesWaiting > ( unsigned portBASE_TYPE ) 0 ) + if( pxQueue->uxMessagesWaiting > ( UBaseType_t ) 0 ) { /* Data is available from the queue. */ - pxQueue->pcReadFrom += pxQueue->uxItemSize; - if( pxQueue->pcReadFrom >= pxQueue->pcTail ) + pxQueue->u.pcReadFrom += pxQueue->uxItemSize; + if( pxQueue->u.pcReadFrom >= pxQueue->pcTail ) + { + pxQueue->u.pcReadFrom = pxQueue->pcHead; + } + else { - pxQueue->pcReadFrom = pxQueue->pcHead; + mtCOVERAGE_TEST_MARKER(); } --( pxQueue->uxMessagesWaiting ); - memcpy( ( void * ) pvBuffer, ( void * ) pxQueue->pcReadFrom, ( unsigned ) pxQueue->uxItemSize ); + ( void ) memcpy( ( void * ) pvBuffer, ( void * ) pxQueue->u.pcReadFrom, ( unsigned ) pxQueue->uxItemSize ); xReturn = pdPASS; @@ -1658,6 +2210,14 @@ signed portBASE_TYPE xReturn; { xReturn = errQUEUE_YIELD; } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } } else @@ -1675,11 +2235,9 @@ signed portBASE_TYPE xReturn; #if ( configUSE_CO_ROUTINES == 1 ) - signed portBASE_TYPE xQueueCRSendFromISR( xQueueHandle xQueue, const void *pvItemToQueue, signed portBASE_TYPE xCoRoutinePreviouslyWoken ) + BaseType_t xQueueCRSendFromISR( QueueHandle_t xQueue, const void *pvItemToQueue, BaseType_t xCoRoutinePreviouslyWoken ) { - xQUEUE *pxQueue; - - pxQueue = ( xQUEUE * ) xQueue; + Queue_t * const pxQueue = ( Queue_t * ) xQueue; /* Cannot block within an ISR so if there is no space on the queue then exit without doing anything. */ @@ -1697,9 +2255,25 @@ signed portBASE_TYPE xReturn; { return pdTRUE; } + else + { + mtCOVERAGE_TEST_MARKER(); + } } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } } + else + { + mtCOVERAGE_TEST_MARKER(); + } return xCoRoutinePreviouslyWoken; } @@ -1709,25 +2283,27 @@ signed portBASE_TYPE xReturn; #if ( configUSE_CO_ROUTINES == 1 ) - signed portBASE_TYPE xQueueCRReceiveFromISR( xQueueHandle xQueue, void *pvBuffer, signed portBASE_TYPE *pxCoRoutineWoken ) + BaseType_t xQueueCRReceiveFromISR( QueueHandle_t xQueue, void *pvBuffer, BaseType_t *pxCoRoutineWoken ) { - signed portBASE_TYPE xReturn; - xQUEUE * pxQueue; - - pxQueue = ( xQUEUE * ) xQueue; + BaseType_t xReturn; + Queue_t * const pxQueue = ( Queue_t * ) xQueue; /* We cannot block from an ISR, so check there is data available. If not then just leave without doing anything. */ - if( pxQueue->uxMessagesWaiting > ( unsigned portBASE_TYPE ) 0 ) + if( pxQueue->uxMessagesWaiting > ( UBaseType_t ) 0 ) { /* Copy the data from the queue. */ - pxQueue->pcReadFrom += pxQueue->uxItemSize; - if( pxQueue->pcReadFrom >= pxQueue->pcTail ) + pxQueue->u.pcReadFrom += pxQueue->uxItemSize; + if( pxQueue->u.pcReadFrom >= pxQueue->pcTail ) + { + pxQueue->u.pcReadFrom = pxQueue->pcHead; + } + else { - pxQueue->pcReadFrom = pxQueue->pcHead; + mtCOVERAGE_TEST_MARKER(); } --( pxQueue->uxMessagesWaiting ); - memcpy( ( void * ) pvBuffer, ( void * ) pxQueue->pcReadFrom, ( unsigned ) pxQueue->uxItemSize ); + ( void ) memcpy( ( void * ) pvBuffer, ( void * ) pxQueue->u.pcReadFrom, ( unsigned ) pxQueue->uxItemSize ); if( ( *pxCoRoutineWoken ) == pdFALSE ) { @@ -1737,8 +2313,20 @@ signed portBASE_TYPE xReturn; { *pxCoRoutineWoken = pdTRUE; } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } } + else + { + mtCOVERAGE_TEST_MARKER(); + } xReturn = pdPASS; } @@ -1755,21 +2343,27 @@ signed portBASE_TYPE xReturn; #if ( configQUEUE_REGISTRY_SIZE > 0 ) - void vQueueAddToRegistry( xQueueHandle xQueue, signed char *pcQueueName ) + void vQueueAddToRegistry( QueueHandle_t xQueue, const char *pcQueueName ) /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ { - unsigned portBASE_TYPE ux; + UBaseType_t ux; /* See if there is an empty space in the registry. A NULL name denotes a free slot. */ - for( ux = ( unsigned portBASE_TYPE ) 0U; ux < ( unsigned portBASE_TYPE ) configQUEUE_REGISTRY_SIZE; ux++ ) + for( ux = ( UBaseType_t ) 0U; ux < ( UBaseType_t ) configQUEUE_REGISTRY_SIZE; ux++ ) { if( xQueueRegistry[ ux ].pcQueueName == NULL ) { /* Store the information on this queue. */ xQueueRegistry[ ux ].pcQueueName = pcQueueName; xQueueRegistry[ ux ].xHandle = xQueue; + + traceQUEUE_REGISTRY_ADD( xQueue, pcQueueName ); break; } + else + { + mtCOVERAGE_TEST_MARKER(); + } } } @@ -1778,13 +2372,13 @@ signed portBASE_TYPE xReturn; #if ( configQUEUE_REGISTRY_SIZE > 0 ) - static void prvQueueUnregisterQueue( xQueueHandle xQueue ) + void vQueueUnregisterQueue( QueueHandle_t xQueue ) { - unsigned portBASE_TYPE ux; + UBaseType_t ux; /* See if the handle of the queue being unregistered in actually in the registry. */ - for( ux = ( unsigned portBASE_TYPE ) 0U; ux < ( unsigned portBASE_TYPE ) configQUEUE_REGISTRY_SIZE; ux++ ) + for( ux = ( UBaseType_t ) 0U; ux < ( UBaseType_t ) configQUEUE_REGISTRY_SIZE; ux++ ) { if( xQueueRegistry[ ux ].xHandle == xQueue ) { @@ -1792,20 +2386,22 @@ signed portBASE_TYPE xReturn; xQueueRegistry[ ux ].pcQueueName = NULL; break; } + else + { + mtCOVERAGE_TEST_MARKER(); + } } - } + } /*lint !e818 xQueue could not be pointer to const because it is a typedef. */ #endif /* configQUEUE_REGISTRY_SIZE */ /*-----------------------------------------------------------*/ #if ( configUSE_TIMERS == 1 ) - void vQueueWaitForMessageRestricted( xQueueHandle xQueue, portTickType xTicksToWait ) + void vQueueWaitForMessageRestricted( QueueHandle_t xQueue, TickType_t xTicksToWait, const BaseType_t xWaitIndefinitely ) { - xQUEUE *pxQueue; - - pxQueue = ( xQUEUE * ) xQueue; + Queue_t * const pxQueue = ( Queue_t * ) xQueue; /* This function should not be called by application code hence the 'Restricted' in its name. It is not part of the public API. It is @@ -1822,10 +2418,14 @@ signed portBASE_TYPE xReturn; the queue is locked, and the calling task blocks on the queue, then the calling task will be immediately unblocked when the queue is unlocked. */ prvLockQueue( pxQueue ); - if( pxQueue->uxMessagesWaiting == ( unsigned portBASE_TYPE ) 0U ) + if( pxQueue->uxMessagesWaiting == ( UBaseType_t ) 0U ) { /* There is nothing in the queue, block for the specified period. */ - vTaskPlaceOnEventListRestricted( &( pxQueue->xTasksWaitingToReceive ), xTicksToWait ); + vTaskPlaceOnEventListRestricted( &( pxQueue->xTasksWaitingToReceive ), xTicksToWait, xWaitIndefinitely ); + } + else + { + mtCOVERAGE_TEST_MARKER(); } prvUnlockQueue( pxQueue ); } @@ -1835,11 +2435,11 @@ signed portBASE_TYPE xReturn; #if ( configUSE_QUEUE_SETS == 1 ) - xQueueSetHandle xQueueCreateSet( unsigned portBASE_TYPE uxEventQueueLength ) + QueueSetHandle_t xQueueCreateSet( const UBaseType_t uxEventQueueLength ) { - xQueueSetHandle pxQueue; + QueueSetHandle_t pxQueue; - pxQueue = xQueueGenericCreate( uxEventQueueLength, sizeof( xQUEUE * ), queueQUEUE_TYPE_SET ); + pxQueue = xQueueGenericCreate( uxEventQueueLength, sizeof( Queue_t * ), queueQUEUE_TYPE_SET ); return pxQueue; } @@ -1849,23 +2449,30 @@ signed portBASE_TYPE xReturn; #if ( configUSE_QUEUE_SETS == 1 ) - portBASE_TYPE xQueueAddToSet( xQueueSetMemberHandle xQueueOrSemaphore, xQueueSetHandle xQueueSet ) + BaseType_t xQueueAddToSet( QueueSetMemberHandle_t xQueueOrSemaphore, QueueSetHandle_t xQueueSet ) { - portBASE_TYPE xReturn; + BaseType_t xReturn; - if( ( ( xQUEUE * ) xQueueOrSemaphore )->pxQueueSetContainer != NULL ) - { - xReturn = pdFAIL; - } - else + taskENTER_CRITICAL(); { - taskENTER_CRITICAL(); + if( ( ( Queue_t * ) xQueueOrSemaphore )->pxQueueSetContainer != NULL ) { - ( ( xQUEUE * ) xQueueOrSemaphore )->pxQueueSetContainer = xQueueSet; + /* Cannot add a queue/semaphore to more than one queue set. */ + xReturn = pdFAIL; + } + else if( ( ( Queue_t * ) xQueueOrSemaphore )->uxMessagesWaiting != ( UBaseType_t ) 0 ) + { + /* Cannot add a queue/semaphore to a queue set if there are already + items in the queue/semaphore. */ + xReturn = pdFAIL; + } + else + { + ( ( Queue_t * ) xQueueOrSemaphore )->pxQueueSetContainer = xQueueSet; + xReturn = pdPASS; } - taskEXIT_CRITICAL(); - xReturn = pdPASS; } + taskEXIT_CRITICAL(); return xReturn; } @@ -1875,19 +2482,17 @@ signed portBASE_TYPE xReturn; #if ( configUSE_QUEUE_SETS == 1 ) - portBASE_TYPE xQueueRemoveFromSet( xQueueSetMemberHandle xQueueOrSemaphore, xQueueSetHandle xQueueSet ) + BaseType_t xQueueRemoveFromSet( QueueSetMemberHandle_t xQueueOrSemaphore, QueueSetHandle_t xQueueSet ) { - portBASE_TYPE xReturn; - xQUEUE *pxQueueOrSemaphore; - - pxQueueOrSemaphore = ( xQUEUE * ) xQueueOrSemaphore; + BaseType_t xReturn; + Queue_t * const pxQueueOrSemaphore = ( Queue_t * ) xQueueOrSemaphore; if( pxQueueOrSemaphore->pxQueueSetContainer != xQueueSet ) { /* The queue was not a member of the set. */ xReturn = pdFAIL; } - else if( pxQueueOrSemaphore->uxMessagesWaiting != 0 ) + else if( pxQueueOrSemaphore->uxMessagesWaiting != ( UBaseType_t ) 0 ) { /* It is dangerous to remove a queue from a set when the queue is not empty because the queue set will still hold pending events for @@ -1906,18 +2511,18 @@ signed portBASE_TYPE xReturn; } return xReturn; - } + } /*lint !e818 xQueueSet could not be declared as pointing to const as it is a typedef. */ #endif /* configUSE_QUEUE_SETS */ /*-----------------------------------------------------------*/ #if ( configUSE_QUEUE_SETS == 1 ) - xQueueSetMemberHandle xQueueSelectFromSet( xQueueSetHandle xQueueSet, portTickType xBlockTimeTicks ) + QueueSetMemberHandle_t xQueueSelectFromSet( QueueSetHandle_t xQueueSet, TickType_t const xTicksToWait ) { - xQueueSetMemberHandle xReturn = NULL; + QueueSetMemberHandle_t xReturn = NULL; - xQueueGenericReceive( ( xQueueHandle ) xQueueSet, &xReturn, xBlockTimeTicks, pdFALSE ); + ( void ) xQueueGenericReceive( ( QueueHandle_t ) xQueueSet, &xReturn, xTicksToWait, pdFALSE ); /*lint !e961 Casting from one typedef to another is not redundant. */ return xReturn; } @@ -1926,11 +2531,11 @@ signed portBASE_TYPE xReturn; #if ( configUSE_QUEUE_SETS == 1 ) - xQueueSetMemberHandle xQueueSelectFromSetFromISR( xQueueSetHandle xQueueSet ) + QueueSetMemberHandle_t xQueueSelectFromSetFromISR( QueueSetHandle_t xQueueSet ) { - xQueueSetMemberHandle xReturn = NULL; + QueueSetMemberHandle_t xReturn = NULL; - xQueueReceiveFromISR( ( xQueueHandle ) xQueueSet, &xReturn, NULL ); + ( void ) xQueueReceiveFromISR( ( QueueHandle_t ) xQueueSet, &xReturn, NULL ); /*lint !e961 Casting from one typedef to another is not redundant. */ return xReturn; } @@ -1939,10 +2544,12 @@ signed portBASE_TYPE xReturn; #if ( configUSE_QUEUE_SETS == 1 ) - static portBASE_TYPE prvNotifyQueueSetContainer( xQUEUE *pxQueue, portBASE_TYPE xCopyPosition ) + static BaseType_t prvNotifyQueueSetContainer( const Queue_t * const pxQueue, const BaseType_t xCopyPosition ) { - xQUEUE *pxQueueSetContainer = pxQueue->pxQueueSetContainer; - portBASE_TYPE xReturn = pdFALSE; + Queue_t *pxQueueSetContainer = pxQueue->pxQueueSetContainer; + BaseType_t xReturn = pdFALSE; + + /* This function must be called form a critical section. */ configASSERT( pxQueueSetContainer ); configASSERT( pxQueueSetContainer->uxMessagesWaiting < pxQueueSetContainer->uxLength ); @@ -1950,16 +2557,37 @@ signed portBASE_TYPE xReturn; if( pxQueueSetContainer->uxMessagesWaiting < pxQueueSetContainer->uxLength ) { traceQUEUE_SEND( pxQueueSetContainer ); - /* The data copies is the handle of the queue that contains data. */ - prvCopyDataToQueue( pxQueueSetContainer, &pxQueue, xCopyPosition ); - if( listLIST_IS_EMPTY( &( pxQueueSetContainer->xTasksWaitingToReceive ) ) == pdFALSE ) + + /* The data copied is the handle of the queue that contains data. */ + xReturn = prvCopyDataToQueue( pxQueueSetContainer, &pxQueue, xCopyPosition ); + + if( pxQueueSetContainer->xTxLock == queueUNLOCKED ) { - if( xTaskRemoveFromEventList( &( pxQueueSetContainer->xTasksWaitingToReceive ) ) != pdFALSE ) + if( listLIST_IS_EMPTY( &( pxQueueSetContainer->xTasksWaitingToReceive ) ) == pdFALSE ) + { + if( xTaskRemoveFromEventList( &( pxQueueSetContainer->xTasksWaitingToReceive ) ) != pdFALSE ) + { + /* The task waiting has a higher priority. */ + xReturn = pdTRUE; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else { - /* The task waiting has a higher priority */ - xReturn = pdTRUE; + mtCOVERAGE_TEST_MARKER(); } } + else + { + ( pxQueueSetContainer->xTxLock )++; + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } return xReturn; @@ -1967,3 +2595,14 @@ signed portBASE_TYPE xReturn; #endif /* configUSE_QUEUE_SETS */ + + + + + + + + + + + diff --git a/lib/FreeRTOS/tasks.c b/lib/FreeRTOS/tasks.c index 522380863f..11ca0e976b 100644 --- a/lib/FreeRTOS/tasks.c +++ b/lib/FreeRTOS/tasks.c @@ -1,82 +1,75 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ /* Standard includes. */ -#include #include #include -#include "debug.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 @@ -89,11 +82,25 @@ task.h is included from an application file. */ #include "timers.h" #include "StackMacros.h" -#undef MPU_WRAPPERS_INCLUDED_FROM_API_FILE +/* Lint e961 and e750 are suppressed as a MISRA exception justified because the +MPU ports require MPU_WRAPPERS_INCLUDED_FROM_API_FILE to be defined for the +header files above, but not in this file, in order to generate the correct +privileged Vs unprivileged linkage and placement. */ +#undef MPU_WRAPPERS_INCLUDED_FROM_API_FILE /*lint !e961 !e750. */ + +/* Set configUSE_STATS_FORMATTING_FUNCTIONS to 2 to include the stats formatting +functions but without including stdio.h here. */ +#if ( configUSE_STATS_FORMATTING_FUNCTIONS == 1 ) + /* At the bottom of this file are two optional functions that can be used + to generate human readable text from the raw data generated by the + uxTaskGetSystemState() function. Note the formatting functions are provided + for convenience only, and are NOT considered part of the kernel. */ + #include +#endif /* configUSE_STATS_FORMATTING_FUNCTIONS == 1 ) */ /* Sanity check the configuration. */ -#if configUSE_TICKLESS_IDLE != 0 - #if INCLUDE_vTaskSuspend != 1 +#if( configUSE_TICKLESS_IDLE != 0 ) + #if( INCLUDE_vTaskSuspend != 1 ) #error INCLUDE_vTaskSuspend must be set to 1 if configUSE_TICKLESS_IDLE is not set to 0 #endif /* INCLUDE_vTaskSuspend */ #endif /* configUSE_TICKLESS_IDLE */ @@ -103,6 +110,22 @@ task.h is included from an application file. */ */ #define tskIDLE_STACK_SIZE configMINIMAL_STACK_SIZE +#if( configUSE_PREEMPTION == 0 ) + /* If the cooperative scheduler is being used then a yield should not be + performed just because a higher priority task has been woken. */ + #define taskYIELD_IF_USING_PREEMPTION() +#else + #define taskYIELD_IF_USING_PREEMPTION() portYIELD_WITHIN_API() +#endif + +/* Value that can be assigned to the eNotifyState member of the TCB. */ +typedef enum +{ + eNotWaitingNotification = 0, + eWaitingNotification, + eNotified +} eNotifyValue; + /* * Task control block. A task control block (TCB) is allocated for each task, * and stores task state information, including a pointer to the task's context @@ -110,45 +133,71 @@ task.h is included from an application file. */ */ 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 TCB STRUCT. */ + volatile StackType_t *pxTopOfStack; /*< Points to the location of the last item placed on the tasks stack. THIS MUST BE THE FIRST MEMBER OF THE TCB 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 TCB STRUCT. */ + xMPU_SETTINGS xMPUSettings; /*< The MPU settings are defined as part of the port layer. THIS MUST BE THE SECOND MEMBER OF THE TCB STRUCT. */ + BaseType_t xUsingStaticallyAllocatedStack; /* Set to pdTRUE if the stack is a statically allocated array, and pdFALSE if the stack is dynamically allocated. */ #endif - xListItem xGenericListItem; /*< The list that the state list item of a task is reference from denotes the state of that task (Ready, Blocked, Suspended ). */ - xListItem xEventListItem; /*< Used to reference a task from an event list. */ - unsigned portBASE_TYPE uxPriority; /*< The priority of the task. 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. */ - size_t stacksize; + ListItem_t xGenericListItem; /*< The list that the state list item of a task is reference from denotes the state of that task (Ready, Blocked, Suspended ). */ + ListItem_t xEventListItem; /*< Used to reference a task from an event list. */ + UBaseType_t uxPriority; /*< The priority of the task. 0 is the lowest priority. */ + StackType_t *pxStack; /*< Points to the start of the stack. */ + char pcTaskName[ configMAX_TASK_NAME_LEN ];/*< Descriptive name given to the task when created. Facilitates debugging only. */ /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ + #if ( portSTACK_GROWTH > 0 ) - portSTACK_TYPE *pxEndOfStack; /*< Points to the end of the stack on architectures where the stack grows up from low memory. */ + StackType_t *pxEndOfStack; /*< Points to the end of the stack on architectures where the stack grows up from low memory. */ #endif #if ( portCRITICAL_NESTING_IN_TCB == 1 ) - unsigned portBASE_TYPE uxCriticalNesting; /*< Holds the critical section nesting depth for ports that do not maintain their own count in the port layer. */ + UBaseType_t uxCriticalNesting; /*< Holds the critical section nesting depth for ports that do not maintain their own count in the port layer. */ #endif #if ( configUSE_TRACE_FACILITY == 1 ) - unsigned portBASE_TYPE uxTCBNumber; /*< Stores a number that increments each time a TCB is created. It allows debuggers to determine when a task has been deleted and then recreated. */ - unsigned portBASE_TYPE uxTaskNumber; /*< Stores a number specifically for use by third party trace code. */ + UBaseType_t uxTCBNumber; /*< Stores a number that increments each time a TCB is created. It allows debuggers to determine when a task has been deleted and then recreated. */ + UBaseType_t uxTaskNumber; /*< Stores a number specifically for use by third party trace code. */ #endif #if ( configUSE_MUTEXES == 1 ) - unsigned portBASE_TYPE uxBasePriority; /*< The priority last assigned to the task - used by the priority inheritance mechanism. */ + UBaseType_t uxBasePriority; /*< The priority last assigned to the task - used by the priority inheritance mechanism. */ + UBaseType_t uxMutexesHeld; #endif #if ( configUSE_APPLICATION_TASK_TAG == 1 ) - pdTASK_HOOK_CODE pxTaskTag; + TaskHookFunction_t pxTaskTag; + #endif + + #if( configNUM_THREAD_LOCAL_STORAGE_POINTERS > 0 ) + void *pvThreadLocalStoragePointers[ configNUM_THREAD_LOCAL_STORAGE_POINTERS ]; #endif #if ( configGENERATE_RUN_TIME_STATS == 1 ) - unsigned long ulRunTimeCounter; /*< Stores the amount of time the task has spent in the Running state. */ + uint32_t ulRunTimeCounter; /*< Stores the amount of time the task has spent in the Running state. */ + #endif + + #if ( configUSE_NEWLIB_REENTRANT == 1 ) + /* Allocate a Newlib reent structure that is specific to this task. + Note Newlib support has been included by popular demand, but is not + used by the FreeRTOS maintainers themselves. FreeRTOS is not + responsible for resulting newlib operation. User must be familiar with + newlib and must provide system-wide implementations of the necessary + stubs. Be warned that (at the time of writing) the current newlib design + implements a system-wide malloc() that must be provided with locks. */ + struct _reent xNewLib_reent; + #endif + + #if ( configUSE_TASK_NOTIFICATIONS == 1 ) + volatile uint32_t ulNotifiedValue; + volatile eNotifyValue eNotifyState; #endif } tskTCB; +/* The old tskTCB name is maintained above then typedefed to the new TCB_t name +below to enable the use of older kernel aware debuggers. */ +typedef tskTCB TCB_t; + /* * Some kernel aware debuggers require the data the debugger needs access to to * be global, rather than file scope. @@ -157,124 +206,83 @@ typedef struct tskTaskControlBlock #define static #endif -/*lint -e956 */ -PRIVILEGED_DATA tskTCB * volatile pxCurrentTCB = NULL; +/*lint -e956 A manual analysis and inspection has been used to determine which +static variables must be declared volatile. */ + +PRIVILEGED_DATA TCB_t * 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. */ -PRIVILEGED_DATA static volatile unsigned portBASE_TYPE uxCurrentNumberOfTasks = ( unsigned portBASE_TYPE ) 0U; +PRIVILEGED_DATA static List_t pxReadyTasksLists[ configMAX_PRIORITIES ];/*< Prioritised ready tasks. */ +PRIVILEGED_DATA static List_t xDelayedTaskList1; /*< Delayed tasks. */ +PRIVILEGED_DATA static List_t xDelayedTaskList2; /*< Delayed tasks (two lists are used - one for delays that have overflowed the current tick count. */ +PRIVILEGED_DATA static List_t * volatile pxDelayedTaskList; /*< Points to the delayed task list currently being used. */ +PRIVILEGED_DATA static List_t * volatile pxOverflowDelayedTaskList; /*< Points to the delayed task list currently being used to hold tasks that have overflowed the current tick count. */ +PRIVILEGED_DATA static List_t xPendingReadyList; /*< Tasks that have been readied while the scheduler was suspended. They will be moved to the ready list when the scheduler is resumed. */ #if ( INCLUDE_vTaskDelete == 1 ) - PRIVILEGED_DATA static 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 ) 0U; + PRIVILEGED_DATA static List_t xTasksWaitingTermination; /*< Tasks that have been deleted - but their memory not yet freed. */ + PRIVILEGED_DATA static volatile UBaseType_t uxTasksDeleted = ( UBaseType_t ) 0U; #endif #if ( INCLUDE_vTaskSuspend == 1 ) - PRIVILEGED_DATA static xList xSuspendedTaskList; /*< Tasks that are currently suspended. */ + PRIVILEGED_DATA static List_t xSuspendedTaskList; /*< Tasks that are currently suspended. */ #endif #if ( INCLUDE_xTaskGetIdleTaskHandle == 1 ) - PRIVILEGED_DATA static xTaskHandle xIdleTaskHandle = NULL; /*< Holds the handle of the idle task. The idle task is created automatically when the scheduler is started. */ + PRIVILEGED_DATA static TaskHandle_t xIdleTaskHandle = NULL; /*< Holds the handle of the idle task. The idle task is created automatically when the scheduler is started. */ #endif -/* File private variables. --------------------------------*/ - -PRIVILEGED_DATA static volatile portTickType xTickCount = ( portTickType ) 0U; -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 ) 0U; -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 ) 0U; -PRIVILEGED_DATA static volatile portTickType xNextTaskUnblockTime = ( portTickType ) portMAX_DELAY; +/* Other file private variables. --------------------------------*/ +PRIVILEGED_DATA static volatile UBaseType_t uxCurrentNumberOfTasks = ( UBaseType_t ) 0U; +PRIVILEGED_DATA static volatile TickType_t xTickCount = ( TickType_t ) 0U; +PRIVILEGED_DATA static volatile UBaseType_t uxTopReadyPriority = tskIDLE_PRIORITY; +PRIVILEGED_DATA static volatile BaseType_t xSchedulerRunning = pdFALSE; +PRIVILEGED_DATA static volatile UBaseType_t uxPendedTicks = ( UBaseType_t ) 0U; +PRIVILEGED_DATA static volatile BaseType_t xYieldPending = pdFALSE; +PRIVILEGED_DATA static volatile BaseType_t xNumOfOverflows = ( BaseType_t ) 0; +PRIVILEGED_DATA static UBaseType_t uxTaskNumber = ( UBaseType_t ) 0U; +PRIVILEGED_DATA static volatile TickType_t xNextTaskUnblockTime = ( TickType_t ) 0U; /* Initialised to portMAX_DELAY before the scheduler starts. */ + +/* Context switches are held pending while the scheduler is suspended. Also, +interrupts must not manipulate the xGenericListItem of a TCB, or any of the +lists the xGenericListItem can be referenced from, if the scheduler is suspended. +If an interrupt needs to unblock a task while the scheduler is suspended then it +moves the task's event list item into the xPendingReadyList, ready for the +kernel to move the task from the pending ready list into the real ready list +when the scheduler is unsuspended. The pending ready list itself can only be +accessed from a critical section. */ +PRIVILEGED_DATA static volatile UBaseType_t uxSchedulerSuspended = ( UBaseType_t ) pdFALSE; #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. */ - PRIVILEGED_DATA static unsigned long ulTotalRunTime; /*< Holds the total amount of execution time as defined by the run time counter clock. */ - static void prvGenerateRunTimeStatsForTasksInList( const signed char *pcWriteBuffer, xList *pxList, unsigned long ulTotalRunTimeDiv100 ) PRIVILEGED_FUNCTION; + PRIVILEGED_DATA static uint32_t ulTaskSwitchedInTime = 0UL; /*< Holds the value of a timer/counter the last time a task was switched in. */ + PRIVILEGED_DATA static uint32_t ulTotalRunTime = 0UL; /*< Holds the total amount of execution time as defined by the run time counter clock. */ #endif +/*lint +e956 */ + /* 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 ( 0xbeU ) - -static tskTCB* tcbs[15]; -static size_t tot_task_mem; -static size_t tot_alloc_stack; -static size_t tot_alloc_overhead; - -size_t debugPrintTCBInfo(void) -{ - tskTCB* tcb = pxCurrentTCB; - int i, lowestFreeSpace, percent, totalfree, totalstack; - char* b; - - DEBUG_PRINT_OS("-Stack info- START\n"); - - totalfree = 0; - totalstack = 0; - - for (i = 0; i < uxCurrentNumberOfTasks; i++) - { - - tcb = tcbs[i]; - - b = (char *)tcb->pxStack; - - while (*b == tskSTACK_FILL_BYTE) - b++; - lowestFreeSpace = b - (char*)tcb->pxStack; - totalfree += lowestFreeSpace; - percent = (lowestFreeSpace*100)/tcb->stacksize; - (void)percent; - totalstack += tcb->stacksize; - -#if 0 - DEBUG_PRINT_OS("-%s-\n", tcb->pcTaskName); - DEBUG_PRINT_OS("Stack start %x\n", tcb->pxStack); - DEBUG_PRINT_OS("Stack start %x\n", tcb->pxStack+tcb->stacksize); - DEBUG_PRINT_OS("Stack pointer %x\n", tcb->pxTopOfStack); - DEBUG_PRINT_OS("Stack size %x\n", tcb->stacksize); - DEBUG_PRINT_OS("Hit high watermark at %x\n", b); -#endif - DEBUG_PRINT_OS("%s: size=%d, lowest free=%d percent (%d bytes)\n", tcb->pcTaskName, tcb->stacksize, percent, lowestFreeSpace); - } - DEBUG_PRINT_OS("Total free bytes: %d\n", totalfree); - DEBUG_PRINT_OS("Total allocated stack: %d\n", totalstack); - DEBUG_PRINT_OS("Total allocated stackmem: %d\n", tot_alloc_stack); - DEBUG_PRINT_OS("Total allocated overhead: %d\n", tot_alloc_overhead); - DEBUG_PRINT_OS("-Stack info- END\n"); - return (tot_alloc_stack+tot_alloc_overhead); -} - +#define tskSTACK_FILL_BYTE ( 0xa5U ) /* * 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' ) +#define tskBLOCKED_CHAR ( 'B' ) +#define tskREADY_CHAR ( 'R' ) +#define tskDELETED_CHAR ( 'D' ) +#define tskSUSPENDED_CHAR ( 'S' ) /*-----------------------------------------------------------*/ @@ -286,28 +294,28 @@ size_t debugPrintTCBInfo(void) /* uxTopReadyPriority holds the priority of the highest priority ready state task. */ - #define taskRECORD_READY_PRIORITY( uxPriority ) \ - { \ - if( ( uxPriority ) > uxTopReadyPriority ) \ - { \ - uxTopReadyPriority = ( uxPriority ); \ - } \ + #define taskRECORD_READY_PRIORITY( uxPriority ) \ + { \ + if( ( uxPriority ) > uxTopReadyPriority ) \ + { \ + uxTopReadyPriority = ( uxPriority ); \ + } \ } /* taskRECORD_READY_PRIORITY */ /*-----------------------------------------------------------*/ - #define taskSELECT_HIGHEST_PRIORITY_TASK() \ - { \ - /* Find the highest priority queue that contains ready tasks. */ \ - while( listLIST_IS_EMPTY( &( pxReadyTasksLists[ uxTopReadyPriority ] ) ) ) \ - { \ - configASSERT( uxTopReadyPriority ); \ - --uxTopReadyPriority; \ - } \ - \ - /* listGET_OWNER_OF_NEXT_ENTRY indexes 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 ] ) ); \ + #define taskSELECT_HIGHEST_PRIORITY_TASK() \ + { \ + /* Find the highest priority queue that contains ready tasks. */ \ + while( listLIST_IS_EMPTY( &( pxReadyTasksLists[ uxTopReadyPriority ] ) ) ) \ + { \ + configASSERT( uxTopReadyPriority ); \ + --uxTopReadyPriority; \ + } \ + \ + /* listGET_OWNER_OF_NEXT_ENTRY indexes 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 ] ) ); \ } /* taskSELECT_HIGHEST_PRIORITY_TASK */ /*-----------------------------------------------------------*/ @@ -331,7 +339,7 @@ size_t debugPrintTCBInfo(void) #define taskSELECT_HIGHEST_PRIORITY_TASK() \ { \ - unsigned portBASE_TYPE uxTopPriority; \ + UBaseType_t uxTopPriority; \ \ /* Find the highest priority queue that contains ready tasks. */ \ portGET_HIGHEST_PRIORITY( uxTopPriority, uxTopReadyPriority ); \ @@ -344,101 +352,76 @@ size_t debugPrintTCBInfo(void) /* A port optimised version is provided, call it only if the TCB being reset is being referenced from a ready list. If it is referenced from a delayed or suspended list then it won't be in a ready list. */ - #define taskRESET_READY_PRIORITY( uxPriority ) \ - { \ - if( listCURRENT_LIST_LENGTH( &( pxReadyTasksLists[ ( uxPriority ) ] ) ) == 0 ) \ - { \ - portRESET_READY_PRIORITY( ( uxPriority ), ( uxTopReadyPriority ) ); \ - } \ + #define taskRESET_READY_PRIORITY( uxPriority ) \ + { \ + if( listCURRENT_LIST_LENGTH( &( pxReadyTasksLists[ ( uxPriority ) ] ) ) == ( UBaseType_t ) 0 ) \ + { \ + portRESET_READY_PRIORITY( ( uxPriority ), ( uxTopReadyPriority ) ); \ + } \ } #endif /* configUSE_PORT_OPTIMISED_TASK_SELECTION */ -/* - * 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 ) \ - traceMOVED_TASK_TO_READY_STATE( pxTCB ) \ - taskRECORD_READY_PRIORITY( ( pxTCB )->uxPriority ); \ - vListInsertEnd( ( xList * ) &( pxReadyTasksLists[ ( pxTCB )->uxPriority ] ), &( ( pxTCB )->xGenericListItem ) ) +/*-----------------------------------------------------------*/ + +/* pxDelayedTaskList and pxOverflowDelayedTaskList are switched when the tick +count overflows. */ +#define taskSWITCH_DELAYED_LISTS() \ +{ \ + List_t *pxTemp; \ + \ + /* The delayed tasks list should be empty when the lists are switched. */ \ + configASSERT( ( listLIST_IS_EMPTY( pxDelayedTaskList ) ) ); \ + \ + pxTemp = pxDelayedTaskList; \ + pxDelayedTaskList = pxOverflowDelayedTaskList; \ + pxOverflowDelayedTaskList = pxTemp; \ + xNumOfOverflows++; \ + prvResetNextTaskUnblockTime(); \ +} + /*-----------------------------------------------------------*/ /* - * 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. + * Place the task represented by pxTCB into the appropriate ready list for + * the task. It is inserted at the end of the list. */ -#define prvCheckDelayedTasks() \ -{ \ -portTickType xItemValue; \ - \ - /* Is the tick count greater than or equal to the wake time of the first \ - task referenced from the delayed tasks list? */ \ - if( xTickCount >= xNextTaskUnblockTime ) \ - { \ - for( ;; ) \ - { \ - if( listLIST_IS_EMPTY( pxDelayedTaskList ) != pdFALSE ) \ - { \ - /* The delayed list is empty. Set xNextTaskUnblockTime to the \ - maximum possible value so it is extremely unlikely that the \ - if( xTickCount >= xNextTaskUnblockTime ) test will pass next \ - time through. */ \ - xNextTaskUnblockTime = portMAX_DELAY; \ - break; \ - } \ - else \ - { \ - /* The delayed list is not empty, get the value of the item at \ - the head of the delayed list. This is the time at which the \ - task at the head of the delayed list should be removed from \ - the Blocked state. */ \ - pxTCB = ( tskTCB * ) listGET_OWNER_OF_HEAD_ENTRY( pxDelayedTaskList ); \ - xItemValue = listGET_LIST_ITEM_VALUE( &( pxTCB->xGenericListItem ) ); \ - \ - if( xTickCount < xItemValue ) \ - { \ - /* It is not time to unblock this item yet, but the item \ - value is the time at which the task at the head of the \ - blocked list should be removed from the Blocked state - \ - so record the item value in xNextTaskUnblockTime. */ \ - xNextTaskUnblockTime = xItemValue; \ - break; \ - } \ - \ - /* It is time to remove the item from the Blocked state. */ \ - uxListRemove( &( pxTCB->xGenericListItem ) ); \ - \ - /* Is the task waiting on an event also? */ \ - if( pxTCB->xEventListItem.pvContainer != NULL ) \ - { \ - uxListRemove( &( pxTCB->xEventListItem ) ); \ - } \ - prvAddTaskToReadyQueue( pxTCB ); \ - } \ - } \ - } \ -} +#define prvAddTaskToReadyList( pxTCB ) \ + traceMOVED_TASK_TO_READY_STATE( pxTCB ); \ + taskRECORD_READY_PRIORITY( ( pxTCB )->uxPriority ); \ + vListInsertEnd( &( pxReadyTasksLists[ ( pxTCB )->uxPriority ] ), &( ( pxTCB )->xGenericListItem ) ) /*-----------------------------------------------------------*/ /* - * Several functions take an xTaskHandle parameter that can optionally be NULL, + * Several functions take an TaskHandle_t 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 ) ) +#define prvGetTCBFromHandle( pxHandle ) ( ( ( pxHandle ) == NULL ) ? ( TCB_t * ) pxCurrentTCB : ( TCB_t * ) ( pxHandle ) ) + +/* The item value of the event list item is normally used to hold the priority +of the task to which it belongs (coded to allow it to be held in reverse +priority order). However, it is occasionally borrowed for other purposes. It +is important its value is not updated due to a task priority change while it is +being used for another purpose. The following bit definition is used to inform +the scheduler that the value should not be changed - in which case it is the +responsibility of whichever module is using the value to ensure it gets set back +to its original value when it is released. */ +#if configUSE_16_BIT_TICKS == 1 + #define taskEVENT_LIST_ITEM_VALUE_IN_USE 0x8000U +#else + #define taskEVENT_LIST_ITEM_VALUE_IN_USE 0x80000000UL +#endif /* Callback function prototypes. --------------------------*/ -extern void vApplicationStackOverflowHook( xTaskHandle xTask, signed char *pcTaskName ); -extern void vApplicationTickHook( void ); +#if configCHECK_FOR_STACK_OVERFLOW > 0 + extern void vApplicationStackOverflowHook( TaskHandle_t xTask, char *pcTaskName ); +#endif + +#if configUSE_TICK_HOOK > 0 + extern void vApplicationTickHook( void ); +#endif /* File private functions. --------------------------------*/ @@ -446,7 +429,16 @@ extern void vApplicationTickHook( void ); * 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; +static void prvInitialiseTCBVariables( TCB_t * const pxTCB, const char * const pcName, UBaseType_t uxPriority, const MemoryRegion_t * const xRegions, const uint16_t usStackDepth ) PRIVILEGED_FUNCTION; /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ + +/** + * Utility task that simply returns pdTRUE if the task referenced by xTask is + * currently in the Suspended state, or pdFALSE if the task referenced by xTask + * is in any other state. + */ +#if ( INCLUDE_vTaskSuspend == 1 ) + static BaseType_t prvTaskIsTaskSuspended( const TaskHandle_t xTask ) PRIVILEGED_FUNCTION; +#endif /* INCLUDE_vTaskSuspend */ /* * Utility to ready all the lists used by the scheduler. This is called @@ -476,7 +468,7 @@ static portTASK_FUNCTION_PROTO( prvIdleTask, pvParameters ); */ #if ( INCLUDE_vTaskDelete == 1 ) - static void prvDeleteTCB( tskTCB *pxTCB ) PRIVILEGED_FUNCTION; + static void prvDeleteTCB( TCB_t *pxTCB ) PRIVILEGED_FUNCTION; #endif @@ -491,26 +483,25 @@ static void prvCheckTasksWaitingTermination( void ) PRIVILEGED_FUNCTION; * The currently executing task is entering the Blocked state. Add the task to * either the current or the overflow delayed task list. */ -static void prvAddCurrentTaskToDelayedList( portTickType xTimeToWake ) PRIVILEGED_FUNCTION; +static void prvAddCurrentTaskToDelayedList( const TickType_t xTimeToWake ) 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; +static TCB_t *prvAllocateTCBAndStack( const uint16_t usStackDepth, StackType_t * const 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. + * Fills an TaskStatus_t structure with information on each task that is + * referenced from the pxList list (which may be a ready list, a delayed list, + * a suspended list, etc.). * * 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; + static UBaseType_t prvListTaskWithinSingleList( TaskStatus_t *pxTaskStatusArray, List_t *pxList, eTaskState eState ) PRIVILEGED_FUNCTION; #endif @@ -521,7 +512,7 @@ static tskTCB *prvAllocateTCBAndStack( unsigned short usStackDepth, portSTACK_TY */ #if ( ( configUSE_TRACE_FACILITY == 1 ) || ( INCLUDE_uxTaskGetStackHighWaterMark == 1 ) ) - static unsigned short usTaskCheckFreeStackSpace( const unsigned char * pucStackByte ) PRIVILEGED_FUNCTION; + static uint16_t prvTaskCheckFreeStackSpace( const uint8_t * pucStackByte ) PRIVILEGED_FUNCTION; #endif @@ -536,36 +527,45 @@ static tskTCB *prvAllocateTCBAndStack( unsigned short usStackDepth, portSTACK_TY */ #if ( configUSE_TICKLESS_IDLE != 0 ) - static portTickType prvGetExpectedIdleTime( void ) PRIVILEGED_FUNCTION; + static TickType_t prvGetExpectedIdleTime( void ) PRIVILEGED_FUNCTION; #endif -/*lint +e956 */ +/* + * Set xNextTaskUnblockTime to the time at which the next Blocked state task + * will exit the Blocked state. + */ +static void prvResetNextTaskUnblockTime( void ); + +#if ( ( configUSE_TRACE_FACILITY == 1 ) && ( configUSE_STATS_FORMATTING_FUNCTIONS > 0 ) ) + + /* + * Helper function used to pad task names with spaces when printing out + * human readable tables of task information. + */ + static char *prvWriteNameToBuffer( char *pcBuffer, const char *pcTaskName ); +#endif +/*-----------------------------------------------------------*/ -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 ) +BaseType_t xTaskGenericCreate( TaskFunction_t pxTaskCode, const char * const pcName, const uint16_t usStackDepth, void * const pvParameters, UBaseType_t uxPriority, TaskHandle_t * const pxCreatedTask, StackType_t * const puxStackBuffer, const MemoryRegion_t * const xRegions ) /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ { -signed portBASE_TYPE xReturn; -tskTCB * pxNewTCB; +BaseType_t xReturn; +TCB_t * pxNewTCB; +StackType_t *pxTopOfStack; configASSERT( pxTaskCode ); - configASSERT( ( ( uxPriority & ( ~portPRIVILEGE_BIT ) ) < configMAX_PRIORITIES ) ); + configASSERT( ( ( uxPriority & ( UBaseType_t ) ( ~portPRIVILEGE_BIT ) ) < ( UBaseType_t ) configMAX_PRIORITIES ) ); /* Allocate the memory required by the TCB and stack for the new task, checking that the allocation was successful. */ pxNewTCB = prvAllocateTCBAndStack( usStackDepth, puxStackBuffer ); - tot_task_mem += usStackDepth; - DEBUG_PRINT_OS("Task: %d/%d\n", usStackDepth*4, tot_task_mem*4); - configASSERT(pxNewTCB); - if( pxNewTCB != NULL ) { - portSTACK_TYPE *pxTopOfStack; - #if( portUSING_MPU_WRAPPERS == 1 ) /* Should the task be created in privileged mode? */ - portBASE_TYPE xRunPrivileged; + BaseType_t xRunPrivileged; if( ( uxPriority & portPRIVILEGE_BIT ) != 0U ) { xRunPrivileged = pdTRUE; @@ -575,28 +575,40 @@ tskTCB * pxNewTCB; xRunPrivileged = pdFALSE; } uxPriority &= ~portPRIVILEGE_BIT; + + if( puxStackBuffer != NULL ) + { + /* The application provided its own stack. Note this so no + attempt is made to delete the stack should that task be + deleted. */ + pxNewTCB->xUsingStaticallyAllocatedStack = pdTRUE; + } + else + { + /* The stack was allocated dynamically. Note this so it can be + deleted again if the task is deleted. */ + pxNewTCB->xUsingStaticallyAllocatedStack = pdFALSE; + } #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. + stack grows from high memory to low (as per the 80x86) or vice 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 - ( unsigned short ) 1 ); - pxNewTCB->stacksize= (usStackDepth*4) - 1; - tcbs[uxCurrentNumberOfTasks] =pxNewTCB; - pxTopOfStack = ( portSTACK_TYPE * ) ( ( ( portPOINTER_SIZE_TYPE ) pxTopOfStack ) & ( ( portPOINTER_SIZE_TYPE ) ~portBYTE_ALIGNMENT_MASK ) ); + pxTopOfStack = pxNewTCB->pxStack + ( usStackDepth - ( uint16_t ) 1 ); + pxTopOfStack = ( StackType_t * ) ( ( ( portPOINTER_SIZE_TYPE ) pxTopOfStack ) & ( ~( ( portPOINTER_SIZE_TYPE ) portBYTE_ALIGNMENT_MASK ) ) ); /*lint !e923 MISRA exception. Avoiding casts between pointers and integers is not practical. Size differences accounted for using portPOINTER_SIZE_TYPE type. */ /* Check the alignment of the calculated top of stack is correct. */ - configASSERT( ( ( ( unsigned long ) pxTopOfStack & ( unsigned long ) portBYTE_ALIGNMENT_MASK ) == 0UL ) ); + configASSERT( ( ( ( portPOINTER_SIZE_TYPE ) pxTopOfStack & ( portPOINTER_SIZE_TYPE ) portBYTE_ALIGNMENT_MASK ) == 0UL ) ); } #else /* portSTACK_GROWTH */ { pxTopOfStack = pxNewTCB->pxStack; /* Check the alignment of the stack buffer is correct. */ - configASSERT( ( ( ( unsigned long ) pxNewTCB->pxStack & ( unsigned long ) portBYTE_ALIGNMENT_MASK ) == 0UL ) ); + configASSERT( ( ( ( portPOINTER_SIZE_TYPE ) pxNewTCB->pxStack & ( portPOINTER_SIZE_TYPE ) portBYTE_ALIGNMENT_MASK ) == 0UL ) ); /* If we want to use stack checking on architectures that use a positive stack growth direction then we also need to store the @@ -622,19 +634,20 @@ tskTCB * pxNewTCB; } #endif /* portUSING_MPU_WRAPPERS */ - /* Check the alignment of the initialised stack. */ - portALIGNMENT_ASSERT_pxCurrentTCB( ( ( ( unsigned long ) pxNewTCB->pxTopOfStack & ( unsigned long ) portBYTE_ALIGNMENT_MASK ) == 0UL ) ); - 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; + *pxCreatedTask = ( TaskHandle_t ) pxNewTCB; + } + else + { + mtCOVERAGE_TEST_MARKER(); } - /* We are going to manipulate the task queues to add this task to a - ready list, so must make sure no interrupts occur. */ + /* Ensure interrupts don't access the task lists while they are being + updated. */ taskENTER_CRITICAL(); { uxCurrentNumberOfTasks++; @@ -644,13 +657,17 @@ tskTCB * pxNewTCB; the suspended state - make this the current task. */ pxCurrentTCB = pxNewTCB; - if( uxCurrentNumberOfTasks == ( unsigned portBASE_TYPE ) 1 ) + if( uxCurrentNumberOfTasks == ( UBaseType_t ) 1 ) { /* 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 + { + mtCOVERAGE_TEST_MARKER(); + } } else { @@ -663,14 +680,15 @@ tskTCB * pxNewTCB; { pxCurrentTCB = pxNewTCB; } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } - } - - /* 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; } uxTaskNumber++; @@ -683,7 +701,7 @@ tskTCB * pxNewTCB; #endif /* configUSE_TRACE_FACILITY */ traceTASK_CREATE( pxNewTCB ); - prvAddTaskToReadyQueue( pxNewTCB ); + prvAddTaskToReadyList( pxNewTCB ); xReturn = pdPASS; portSETUP_TCB( pxNewTCB ); @@ -704,9 +722,17 @@ tskTCB * pxNewTCB; then it should run now. */ if( pxCurrentTCB->uxPriority < uxPriority ) { - portYIELD_WITHIN_API(); + taskYIELD_IF_USING_PREEMPTION(); + } + else + { + mtCOVERAGE_TEST_MARKER(); } } + else + { + mtCOVERAGE_TEST_MARKER(); + } } return xReturn; @@ -715,38 +741,40 @@ tskTCB * pxNewTCB; #if ( INCLUDE_vTaskDelete == 1 ) - void vTaskDelete( xTaskHandle xTaskToDelete ) + void vTaskDelete( TaskHandle_t xTaskToDelete ) { - tskTCB *pxTCB; + TCB_t *pxTCB; taskENTER_CRITICAL(); { - /* Ensure a yield is performed if the current task is being - deleted. */ - if( xTaskToDelete == pxCurrentTCB ) - { - xTaskToDelete = NULL; - } - - /* If null is passed in here then we are deleting ourselves. */ + /* If null is passed in here then it is the calling task that is + being deleted. */ pxTCB = prvGetTCBFromHandle( xTaskToDelete ); /* 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. */ - if( uxListRemove( ( xListItem * ) &( pxTCB->xGenericListItem ) ) == 0 ) + if( uxListRemove( &( pxTCB->xGenericListItem ) ) == ( UBaseType_t ) 0 ) { taskRESET_READY_PRIORITY( pxTCB->uxPriority ); } + else + { + mtCOVERAGE_TEST_MARKER(); + } /* Is the task waiting on an event also? */ - if( pxTCB->xEventListItem.pvContainer != NULL ) + if( listLIST_ITEM_CONTAINER( &( pxTCB->xEventListItem ) ) != NULL ) + { + ( void ) uxListRemove( &( pxTCB->xEventListItem ) ); + } + else { - uxListRemove( &( pxTCB->xEventListItem ) ); + mtCOVERAGE_TEST_MARKER(); } - vListInsertEnd( ( xList * ) &xTasksWaitingTermination, &( pxTCB->xGenericListItem ) ); + vListInsertEnd( &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 @@ -761,13 +789,32 @@ tskTCB * pxNewTCB; } taskEXIT_CRITICAL(); - /* Force a reschedule if we have just deleted the current task. */ + /* Force a reschedule if it is the currently running task that has just + been deleted. */ if( xSchedulerRunning != pdFALSE ) { - if( ( void * ) xTaskToDelete == NULL ) + if( pxTCB == pxCurrentTCB ) { + configASSERT( uxSchedulerSuspended == 0 ); + + /* The pre-delete hook is primarily for the Windows simulator, + in which Windows specific clean up operations are performed, + after which it is not possible to yield away from this task - + hence xYieldPending is used to latch that a context switch is + required. */ + portPRE_TASK_DELETE_HOOK( pxTCB, &xYieldPending ); portYIELD_WITHIN_API(); } + else + { + /* Reset the next expected unblock time in case it referred to + the task that has just been deleted. */ + taskENTER_CRITICAL(); + { + prvResetNextTaskUnblockTime(); + } + taskEXIT_CRITICAL(); + } } } @@ -776,40 +823,53 @@ tskTCB * pxNewTCB; #if ( INCLUDE_vTaskDelayUntil == 1 ) - void vTaskDelayUntil( portTickType * const pxPreviousWakeTime, portTickType xTimeIncrement ) + void vTaskDelayUntil( TickType_t * const pxPreviousWakeTime, const TickType_t xTimeIncrement ) { - portTickType xTimeToWake; - portBASE_TYPE xAlreadyYielded, xShouldDelay = pdFALSE; + TickType_t xTimeToWake; + BaseType_t xAlreadyYielded, xShouldDelay = pdFALSE; configASSERT( pxPreviousWakeTime ); configASSERT( ( xTimeIncrement > 0U ) ); + configASSERT( uxSchedulerSuspended == 0 ); vTaskSuspendAll(); { + /* Minor optimisation. The tick count cannot change in this + block. */ + const TickType_t xConstTickCount = xTickCount; + /* Generate the tick time at which the task wants to wake. */ xTimeToWake = *pxPreviousWakeTime + xTimeIncrement; - if( xTickCount < *pxPreviousWakeTime ) + if( xConstTickCount < *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 ) ) + if( ( xTimeToWake < *pxPreviousWakeTime ) && ( xTimeToWake > xConstTickCount ) ) { xShouldDelay = pdTRUE; } + else + { + mtCOVERAGE_TEST_MARKER(); + } } 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 ) ) + if( ( xTimeToWake < *pxPreviousWakeTime ) || ( xTimeToWake > xConstTickCount ) ) { xShouldDelay = pdTRUE; } + else + { + mtCOVERAGE_TEST_MARKER(); + } } /* Update the wake time ready for the next call. */ @@ -819,19 +879,26 @@ tskTCB * pxNewTCB; { 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. */ - if( uxListRemove( ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ) == 0 ) + /* Remove the task from the ready list before adding it to the + blocked list as the same list item is used for both lists. */ + if( uxListRemove( &( pxCurrentTCB->xGenericListItem ) ) == ( UBaseType_t ) 0 ) { /* The current task must be in a ready list, so there is no need to check, and the port reset macro can be called directly. */ portRESET_READY_PRIORITY( pxCurrentTCB->uxPriority, uxTopReadyPriority ); } + else + { + mtCOVERAGE_TEST_MARKER(); + } prvAddCurrentTaskToDelayedList( xTimeToWake ); } + else + { + mtCOVERAGE_TEST_MARKER(); + } } xAlreadyYielded = xTaskResumeAll(); @@ -841,6 +908,10 @@ tskTCB * pxNewTCB; { portYIELD_WITHIN_API(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } } #endif /* INCLUDE_vTaskDelayUntil */ @@ -848,14 +919,16 @@ tskTCB * pxNewTCB; #if ( INCLUDE_vTaskDelay == 1 ) - void vTaskDelay( portTickType xTicksToDelay ) + void vTaskDelay( const TickType_t xTicksToDelay ) { - portTickType xTimeToWake; - signed portBASE_TYPE xAlreadyYielded = pdFALSE; + TickType_t xTimeToWake; + BaseType_t xAlreadyYielded = pdFALSE; + /* A delay time of zero just forces a reschedule. */ - if( xTicksToDelay > ( portTickType ) 0U ) + if( xTicksToDelay > ( TickType_t ) 0U ) { + configASSERT( uxSchedulerSuspended == 0 ); vTaskSuspendAll(); { traceTASK_DELAY(); @@ -875,17 +948,25 @@ tskTCB * pxNewTCB; /* 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. */ - if( uxListRemove( ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ) == 0 ) + if( uxListRemove( &( pxCurrentTCB->xGenericListItem ) ) == ( UBaseType_t ) 0 ) { /* The current task must be in a ready list, so there is no need to check, and the port reset macro can be called directly. */ portRESET_READY_PRIORITY( pxCurrentTCB->uxPriority, uxTopReadyPriority ); } + else + { + mtCOVERAGE_TEST_MARKER(); + } prvAddCurrentTaskToDelayedList( xTimeToWake ); } xAlreadyYielded = xTaskResumeAll(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } /* Force a reschedule if xTaskResumeAll has not already done so, we may have put ourselves to sleep. */ @@ -893,6 +974,10 @@ tskTCB * pxNewTCB; { portYIELD_WITHIN_API(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } } #endif /* INCLUDE_vTaskDelay */ @@ -900,13 +985,13 @@ tskTCB * pxNewTCB; #if ( INCLUDE_eTaskGetState == 1 ) - eTaskState eTaskGetState( xTaskHandle xTask ) + eTaskState eTaskGetState( TaskHandle_t xTask ) { eTaskState eReturn; - xList *pxStateList; - tskTCB *pxTCB; + List_t *pxStateList; + const TCB_t * const pxTCB = ( TCB_t * ) xTask; - pxTCB = ( tskTCB * ) xTask; + configASSERT( pxTCB ); if( pxTCB == pxCurrentTCB ) { @@ -917,7 +1002,7 @@ tskTCB * pxNewTCB; { taskENTER_CRITICAL(); { - pxStateList = ( xList * ) listLIST_ITEM_CONTAINER( &( pxTCB->xGenericListItem ) ); + pxStateList = ( List_t * ) listLIST_ITEM_CONTAINER( &( pxTCB->xGenericListItem ) ); } taskEXIT_CRITICAL(); @@ -932,8 +1017,16 @@ tskTCB * pxNewTCB; else if( pxStateList == &xSuspendedTaskList ) { /* The task being queried is referenced from the suspended - list. */ - eReturn = eSuspended; + list. Is it genuinely suspended or is it block + indefinitely? */ + if( listLIST_ITEM_CONTAINER( &( pxTCB->xEventListItem ) ) == NULL ) + { + eReturn = eSuspended; + } + else + { + eReturn = eBlocked; + } } #endif @@ -946,7 +1039,7 @@ tskTCB * pxNewTCB; } #endif - else + else /*lint !e525 Negative indentation is intended to make use of pre-processor clearer. */ { /* If the task is not in any other state, it must be in the Ready (including pending ready) state. */ @@ -955,22 +1048,22 @@ tskTCB * pxNewTCB; } return eReturn; - } + } /*lint !e818 xTask cannot be a pointer to const because it is a typedef. */ #endif /* INCLUDE_eTaskGetState */ /*-----------------------------------------------------------*/ #if ( INCLUDE_uxTaskPriorityGet == 1 ) - unsigned portBASE_TYPE uxTaskPriorityGet( xTaskHandle xTask ) + UBaseType_t uxTaskPriorityGet( TaskHandle_t xTask ) { - tskTCB *pxTCB; - unsigned portBASE_TYPE uxReturn; + TCB_t *pxTCB; + UBaseType_t uxReturn; taskENTER_CRITICAL(); { - /* If null is passed in here then we are changing the - priority of the calling function. */ + /* If null is passed in here then it is the priority of the that + called uxTaskPriorityGet() that is being queried. */ pxTCB = prvGetTCBFromHandle( xTask ); uxReturn = pxTCB->uxPriority; } @@ -982,66 +1075,124 @@ tskTCB * pxNewTCB; #endif /* INCLUDE_uxTaskPriorityGet */ /*-----------------------------------------------------------*/ +#if ( INCLUDE_uxTaskPriorityGet == 1 ) + + UBaseType_t uxTaskPriorityGetFromISR( TaskHandle_t xTask ) + { + TCB_t *pxTCB; + UBaseType_t uxReturn, uxSavedInterruptState; + + /* RTOS ports that support interrupt nesting have the concept of a + maximum system call (or maximum API call) interrupt priority. + Interrupts that are above the maximum system call priority are keep + permanently enabled, even when the RTOS kernel is in a critical section, + but cannot make any calls to FreeRTOS API functions. If configASSERT() + is defined in FreeRTOSConfig.h then + portASSERT_IF_INTERRUPT_PRIORITY_INVALID() will result in an assertion + failure if a FreeRTOS API function is called from an interrupt that has + been assigned a priority above the configured maximum system call + priority. Only FreeRTOS functions that end in FromISR can be called + from interrupts that have been assigned a priority at or (logically) + below the maximum system call interrupt priority. FreeRTOS maintains a + separate interrupt safe API to ensure interrupt entry is as fast and as + simple as possible. More information (albeit Cortex-M specific) is + provided on the following link: + http://www.freertos.org/RTOS-Cortex-M3-M4.html */ + portASSERT_IF_INTERRUPT_PRIORITY_INVALID(); + + uxSavedInterruptState = portSET_INTERRUPT_MASK_FROM_ISR(); + { + /* If null is passed in here then it is the priority of the calling + task that is being queried. */ + pxTCB = prvGetTCBFromHandle( xTask ); + uxReturn = pxTCB->uxPriority; + } + portCLEAR_INTERRUPT_MASK_FROM_ISR( uxSavedInterruptState ); + + return uxReturn; + } + +#endif /* INCLUDE_uxTaskPriorityGet */ +/*-----------------------------------------------------------*/ + #if ( INCLUDE_vTaskPrioritySet == 1 ) - void vTaskPrioritySet( xTaskHandle xTask, unsigned portBASE_TYPE uxNewPriority ) + void vTaskPrioritySet( TaskHandle_t xTask, UBaseType_t uxNewPriority ) { - tskTCB *pxTCB; - unsigned portBASE_TYPE uxCurrentPriority, uxPriorityUsedOnEntry; - portBASE_TYPE xYieldRequired = pdFALSE; + TCB_t *pxTCB; + UBaseType_t uxCurrentBasePriority, uxPriorityUsedOnEntry; + BaseType_t xYieldRequired = pdFALSE; configASSERT( ( uxNewPriority < configMAX_PRIORITIES ) ); /* Ensure the new priority is valid. */ - if( uxNewPriority >= configMAX_PRIORITIES ) + if( uxNewPriority >= ( UBaseType_t ) configMAX_PRIORITIES ) { - uxNewPriority = configMAX_PRIORITIES - ( unsigned portBASE_TYPE ) 1U; + uxNewPriority = ( UBaseType_t ) configMAX_PRIORITIES - ( UBaseType_t ) 1U; + } + else + { + mtCOVERAGE_TEST_MARKER(); } taskENTER_CRITICAL(); { - if( xTask == ( xTaskHandle ) pxCurrentTCB ) - { - xTask = NULL; - } - - /* If null is passed in here then we are changing the - priority of the calling function. */ + /* If null is passed in here then it is the priority of the calling + task that is being changed. */ pxTCB = prvGetTCBFromHandle( xTask ); traceTASK_PRIORITY_SET( pxTCB, uxNewPriority ); #if ( configUSE_MUTEXES == 1 ) { - uxCurrentPriority = pxTCB->uxBasePriority; + uxCurrentBasePriority = pxTCB->uxBasePriority; } #else { - uxCurrentPriority = pxTCB->uxPriority; + uxCurrentBasePriority = pxTCB->uxPriority; } #endif - if( uxCurrentPriority != uxNewPriority ) + if( uxCurrentBasePriority != uxNewPriority ) { /* The priority change may have readied a task of higher priority than the calling task. */ - if( uxNewPriority > uxCurrentPriority ) + if( uxNewPriority > uxCurrentBasePriority ) { - if( xTask != NULL ) + if( pxTCB != pxCurrentTCB ) { - /* 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; + /* The priority of a task other than the currently + running task is being raised. Is the priority being + raised above that of the running task? */ + if( uxNewPriority >= pxCurrentTCB->uxPriority ) + { + xYieldRequired = pdTRUE; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + /* The priority of the running task is being raised, + but the running task must already be the highest + priority task able to run so no yield is required. */ } } - else if( xTask == NULL ) + else if( pxTCB == pxCurrentTCB ) { - /* Setting our own priority down means there may now be another - task of higher priority that is ready to execute. */ + /* Setting the priority of the running task down means + there may now be another task of higher priority that + is ready to execute. */ xYieldRequired = pdTRUE; } + else + { + /* Setting the priority of any other task down does not + require a yield as the running task must be above the + new priority of the task being modified. */ + } /* Remember the ready list the task might be referenced from before its uxPriority member is changed so the @@ -1056,6 +1207,10 @@ tskTCB * pxNewTCB; { pxTCB->uxPriority = uxNewPriority; } + else + { + mtCOVERAGE_TEST_MARKER(); + } /* The base priority gets set whatever. */ pxTCB->uxBasePriority = uxNewPriority; @@ -1066,35 +1221,59 @@ tskTCB * pxNewTCB; } #endif - listSET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ), ( configMAX_PRIORITIES - ( portTickType ) uxNewPriority ) ); + /* Only reset the event list item value if the value is not + being used for anything else. */ + if( ( listGET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ) ) & taskEVENT_LIST_ITEM_VALUE_IN_USE ) == 0UL ) + { + listSET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ), ( ( TickType_t ) configMAX_PRIORITIES - ( TickType_t ) uxNewPriority ) ); /*lint !e961 MISRA exception as the casts are only redundant for some ports. */ + } + else + { + mtCOVERAGE_TEST_MARKER(); + } /* 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 ) ) ) + in the list appropriate to its new priority. */ + if( listIS_CONTAINED_WITHIN( &( pxReadyTasksLists[ uxPriorityUsedOnEntry ] ), &( pxTCB->xGenericListItem ) ) != pdFALSE ) { /* 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. */ - if( uxListRemove( ( xListItem * ) &( pxTCB->xGenericListItem ) ) == 0 ) + if( uxListRemove( &( pxTCB->xGenericListItem ) ) == ( UBaseType_t ) 0 ) + { + /* It is known that the task is in its ready list so + there is no need to check again and the port level + reset macro can be called directly. */ + portRESET_READY_PRIORITY( uxPriorityUsedOnEntry, uxTopReadyPriority ); + } + else { - taskRESET_READY_PRIORITY( uxPriorityUsedOnEntry ); + mtCOVERAGE_TEST_MARKER(); } - prvAddTaskToReadyQueue( pxTCB ); + prvAddTaskToReadyList( pxTCB ); + } + else + { + mtCOVERAGE_TEST_MARKER(); } if( xYieldRequired == pdTRUE ) { - portYIELD_WITHIN_API(); + taskYIELD_IF_USING_PREEMPTION(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + /* Remove compiler warning about unused variables when the port + optimised task selection is not being used. */ + ( void ) uxPriorityUsedOnEntry; } } taskEXIT_CRITICAL(); - - /* Remove compiler warning about unused parameter when the port - optimised task selection is not being used. */ - ( void ) uxPriorityUsedOnEntry; } #endif /* INCLUDE_vTaskPrioritySet */ @@ -1102,45 +1281,49 @@ tskTCB * pxNewTCB; #if ( INCLUDE_vTaskSuspend == 1 ) - void vTaskSuspend( xTaskHandle xTaskToSuspend ) + void vTaskSuspend( TaskHandle_t xTaskToSuspend ) { - tskTCB *pxTCB; + TCB_t *pxTCB; taskENTER_CRITICAL(); { - /* Ensure a yield is performed if the current task is being - suspended. */ - if( xTaskToSuspend == ( xTaskHandle ) pxCurrentTCB ) - { - xTaskToSuspend = NULL; - } - - /* If null is passed in here then we are suspending ourselves. */ + /* If null is passed in here then it is the running task that is + being suspended. */ pxTCB = prvGetTCBFromHandle( xTaskToSuspend ); traceTASK_SUSPEND( pxTCB ); - /* Remove task from the ready/delayed list and place in the suspended list. */ - if( uxListRemove( ( xListItem * ) &( pxTCB->xGenericListItem ) ) == 0 ) + /* Remove task from the ready/delayed list and place in the + suspended list. */ + if( uxListRemove( &( pxTCB->xGenericListItem ) ) == ( UBaseType_t ) 0 ) { taskRESET_READY_PRIORITY( pxTCB->uxPriority ); } + else + { + mtCOVERAGE_TEST_MARKER(); + } /* Is the task waiting on an event also? */ - if( pxTCB->xEventListItem.pvContainer != NULL ) + if( listLIST_ITEM_CONTAINER( &( pxTCB->xEventListItem ) ) != NULL ) + { + ( void ) uxListRemove( &( pxTCB->xEventListItem ) ); + } + else { - uxListRemove( &( pxTCB->xEventListItem ) ); + mtCOVERAGE_TEST_MARKER(); } - vListInsertEnd( ( xList * ) &xSuspendedTaskList, &( pxTCB->xGenericListItem ) ); + vListInsertEnd( &xSuspendedTaskList, &( pxTCB->xGenericListItem ) ); } taskEXIT_CRITICAL(); - if( ( void * ) xTaskToSuspend == NULL ) + if( pxTCB == pxCurrentTCB ) { if( xSchedulerRunning != pdFALSE ) { - /* We have just suspended the current task. */ + /* The current task has just been suspended. */ + configASSERT( uxSchedulerSuspended == 0 ); portYIELD_WITHIN_API(); } else @@ -1162,6 +1345,24 @@ tskTCB * pxNewTCB; } } } + else + { + if( xSchedulerRunning != pdFALSE ) + { + /* A task other than the currently running task was suspended, + reset the next expected unblock time in case it referred to the + task that is now in the Suspended state. */ + taskENTER_CRITICAL(); + { + prvResetNextTaskUnblockTime(); + } + taskEXIT_CRITICAL(); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } } #endif /* INCLUDE_vTaskSuspend */ @@ -1169,77 +1370,98 @@ tskTCB * pxNewTCB; #if ( INCLUDE_vTaskSuspend == 1 ) - signed portBASE_TYPE xTaskIsTaskSuspended( xTaskHandle xTask ) + static BaseType_t prvTaskIsTaskSuspended( const TaskHandle_t xTask ) { - portBASE_TYPE xReturn = pdFALSE; - const tskTCB * const pxTCB = ( tskTCB * ) xTask; + BaseType_t xReturn = pdFALSE; + const TCB_t * const pxTCB = ( TCB_t * ) xTask; + + /* Accesses xPendingReadyList so must be called from a critical + section. */ /* It does not make sense to check if the calling task is suspended. */ configASSERT( xTask ); - /* Is the task we are attempting to resume actually in the - suspended list? */ + /* Is the task being resumed 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 ) + if( listIS_CONTAINED_WITHIN( &xPendingReadyList, &( pxTCB->xEventListItem ) ) == pdFALSE ) { - /* 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 ) + /* Is it in the suspended list because it is in the Suspended + state, or because is is blocked with no timeout? */ + if( listIS_CONTAINED_WITHIN( NULL, &( pxTCB->xEventListItem ) ) != pdFALSE ) { xReturn = pdTRUE; } + else + { + mtCOVERAGE_TEST_MARKER(); + } } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } return xReturn; - } + } /*lint !e818 xTask cannot be a pointer to const because it is a typedef. */ #endif /* INCLUDE_vTaskSuspend */ /*-----------------------------------------------------------*/ #if ( INCLUDE_vTaskSuspend == 1 ) - void vTaskResume( xTaskHandle xTaskToResume ) + void vTaskResume( TaskHandle_t xTaskToResume ) { - tskTCB *pxTCB; + TCB_t * const pxTCB = ( TCB_t * ) xTaskToResume; /* It does not make sense to resume the calling task. */ configASSERT( xTaskToResume ); - /* Remove the task from whichever list it is currently in, and place - it in the ready list. */ - pxTCB = ( tskTCB * ) xTaskToResume; - /* The parameter cannot be NULL as it is impossible to resume the currently executing task. */ if( ( pxTCB != NULL ) && ( pxTCB != pxCurrentTCB ) ) { taskENTER_CRITICAL(); { - if( xTaskIsTaskSuspended( pxTCB ) == pdTRUE ) + if( prvTaskIsTaskSuspended( pxTCB ) == pdTRUE ) { traceTASK_RESUME( pxTCB ); /* As we are in a critical section we can access the ready lists even if the scheduler is suspended. */ - uxListRemove( &( pxTCB->xGenericListItem ) ); - prvAddTaskToReadyQueue( pxTCB ); + ( void ) uxListRemove( &( pxTCB->xGenericListItem ) ); + prvAddTaskToReadyList( 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(); + /* This yield may not cause the task just resumed to run, + but will leave the lists in the correct state for the + next yield. */ + taskYIELD_IF_USING_PREEMPTION(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } } taskEXIT_CRITICAL(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } } #endif /* INCLUDE_vTaskSuspend */ @@ -1248,35 +1470,66 @@ tskTCB * pxNewTCB; #if ( ( INCLUDE_xTaskResumeFromISR == 1 ) && ( INCLUDE_vTaskSuspend == 1 ) ) - portBASE_TYPE xTaskResumeFromISR( xTaskHandle xTaskToResume ) + BaseType_t xTaskResumeFromISR( TaskHandle_t xTaskToResume ) { - portBASE_TYPE xYieldRequired = pdFALSE; - tskTCB *pxTCB; - unsigned portBASE_TYPE uxSavedInterruptStatus; + BaseType_t xYieldRequired = pdFALSE; + TCB_t * const pxTCB = ( TCB_t * ) xTaskToResume; + UBaseType_t uxSavedInterruptStatus; configASSERT( xTaskToResume ); - pxTCB = ( tskTCB * ) xTaskToResume; + /* RTOS ports that support interrupt nesting have the concept of a + maximum system call (or maximum API call) interrupt priority. + Interrupts that are above the maximum system call priority are keep + permanently enabled, even when the RTOS kernel is in a critical section, + but cannot make any calls to FreeRTOS API functions. If configASSERT() + is defined in FreeRTOSConfig.h then + portASSERT_IF_INTERRUPT_PRIORITY_INVALID() will result in an assertion + failure if a FreeRTOS API function is called from an interrupt that has + been assigned a priority above the configured maximum system call + priority. Only FreeRTOS functions that end in FromISR can be called + from interrupts that have been assigned a priority at or (logically) + below the maximum system call interrupt priority. FreeRTOS maintains a + separate interrupt safe API to ensure interrupt entry is as fast and as + simple as possible. More information (albeit Cortex-M specific) is + provided on the following link: + http://www.freertos.org/RTOS-Cortex-M3-M4.html */ + portASSERT_IF_INTERRUPT_PRIORITY_INVALID(); uxSavedInterruptStatus = portSET_INTERRUPT_MASK_FROM_ISR(); { - if( xTaskIsTaskSuspended( pxTCB ) == pdTRUE ) + if( prvTaskIsTaskSuspended( pxTCB ) == pdTRUE ) { traceTASK_RESUME_FROM_ISR( pxTCB ); - if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) - { - xYieldRequired = ( pxTCB->uxPriority >= pxCurrentTCB->uxPriority ); - uxListRemove( &( pxTCB->xGenericListItem ) ); - prvAddTaskToReadyQueue( pxTCB ); - } - else + /* Check the ready lists can be accessed. */ + if( uxSchedulerSuspended == ( UBaseType_t ) pdFALSE ) { - /* 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 ) ); + /* Ready lists can be accessed so move the task from the + suspended list to the ready list directly. */ + if( pxTCB->uxPriority >= pxCurrentTCB->uxPriority ) + { + xYieldRequired = pdTRUE; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + ( void ) uxListRemove( &( pxTCB->xGenericListItem ) ); + prvAddTaskToReadyList( pxTCB ); } + else + { + /* The delayed or ready lists cannot be accessed so the task + is held in the pending ready list until the scheduler is + unsuspended. */ + vListInsertEnd( &( xPendingReadyList ), &( pxTCB->xEventListItem ) ); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } } portCLEAR_INTERRUPT_MASK_FROM_ISR( uxSavedInterruptStatus ); @@ -1289,19 +1542,19 @@ tskTCB * pxNewTCB; void vTaskStartScheduler( void ) { -portBASE_TYPE xReturn; +BaseType_t xReturn; /* Add the idle task at the lowest priority. */ #if ( INCLUDE_xTaskGetIdleTaskHandle == 1 ) { /* Create the idle task, storing its handle in xIdleTaskHandle so it can be returned by the xTaskGetIdleTaskHandle() function. */ - xReturn = xTaskCreate( prvIdleTask, ( signed char * ) "IDLE", tskIDLE_STACK_SIZE, ( void * ) NULL, ( tskIDLE_PRIORITY | portPRIVILEGE_BIT ), &xIdleTaskHandle ); + xReturn = xTaskCreate( prvIdleTask, "IDLE", tskIDLE_STACK_SIZE, ( void * ) NULL, ( tskIDLE_PRIORITY | portPRIVILEGE_BIT ), &xIdleTaskHandle ); /*lint !e961 MISRA exception, justified as it is not a redundant explicit cast to all supported compilers. */ } #else { /* Create the idle task without storing its handle. */ - xReturn = xTaskCreate( prvIdleTask, ( signed char * ) "IDLE", tskIDLE_STACK_SIZE, ( void * ) NULL, ( tskIDLE_PRIORITY | portPRIVILEGE_BIT ), NULL ); + xReturn = xTaskCreate( prvIdleTask, "IDLE", tskIDLE_STACK_SIZE, ( void * ) NULL, ( tskIDLE_PRIORITY | portPRIVILEGE_BIT ), NULL ); /*lint !e961 MISRA exception, justified as it is not a redundant explicit cast to all supported compilers. */ } #endif /* INCLUDE_xTaskGetIdleTaskHandle */ @@ -1311,6 +1564,10 @@ portBASE_TYPE xReturn; { xReturn = xTimerCreateTimerTask(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } } #endif /* configUSE_TIMERS */ @@ -1320,14 +1577,20 @@ portBASE_TYPE xReturn; 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. */ + starts to run. */ portDISABLE_INTERRUPTS(); + #if ( configUSE_NEWLIB_REENTRANT == 1 ) + { + /* Switch Newlib's _impure_ptr variable to point to the _reent + structure specific to the task that will run first. */ + _impure_ptr = &( pxCurrentTCB->xNewLib_reent ); + } + #endif /* configUSE_NEWLIB_REENTRANT */ + + xNextTaskUnblockTime = portMAX_DELAY; xSchedulerRunning = pdTRUE; - xTickCount = ( portTickType ) 0U; + xTickCount = ( TickType_t ) 0U; /* If configGENERATE_RUN_TIME_STATS is defined then the following macro must be defined to configure the timer/counter used to generate @@ -1370,16 +1633,18 @@ void vTaskEndScheduler( void ) void vTaskSuspendAll( void ) { /* A critical section is not required as the variable is of type - portBASE_TYPE. */ + BaseType_t. Please read Richard Barry's reply in the following link to a + post in the FreeRTOS support forum before reporting this as a bug! - + http://goo.gl/wu4acr */ ++uxSchedulerSuspended; } /*----------------------------------------------------------*/ #if ( configUSE_TICKLESS_IDLE != 0 ) - static portTickType prvGetExpectedIdleTime( void ) + static TickType_t prvGetExpectedIdleTime( void ) { - portTickType xReturn; + TickType_t xReturn; if( pxCurrentTCB->uxPriority > tskIDLE_PRIORITY ) { @@ -1403,10 +1668,10 @@ void vTaskSuspendAll( void ) #endif /* configUSE_TICKLESS_IDLE */ /*----------------------------------------------------------*/ -signed portBASE_TYPE xTaskResumeAll( void ) +BaseType_t xTaskResumeAll( void ) { -register tskTCB *pxTCB; -signed portBASE_TYPE xAlreadyYielded = pdFALSE; +TCB_t *pxTCB; +BaseType_t xAlreadyYielded = pdFALSE; /* If uxSchedulerSuspended is zero then this function does not match a previous call to vTaskSuspendAll(). */ @@ -1421,58 +1686,74 @@ signed portBASE_TYPE xAlreadyYielded = pdFALSE; { --uxSchedulerSuspended; - if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) + if( uxSchedulerSuspended == ( UBaseType_t ) pdFALSE ) { - if( uxCurrentNumberOfTasks > ( unsigned portBASE_TYPE ) 0U ) + if( uxCurrentNumberOfTasks > ( UBaseType_t ) 0U ) { - portBASE_TYPE xYieldRequired = pdFALSE; - /* Move any readied tasks from the pending list into the appropriate ready list. */ - while( listLIST_IS_EMPTY( ( xList * ) &xPendingReadyList ) == pdFALSE ) + while( listLIST_IS_EMPTY( &xPendingReadyList ) == pdFALSE ) { - pxTCB = ( tskTCB * ) listGET_OWNER_OF_HEAD_ENTRY( ( ( xList * ) &xPendingReadyList ) ); - uxListRemove( &( pxTCB->xEventListItem ) ); - uxListRemove( &( pxTCB->xGenericListItem ) ); - prvAddTaskToReadyQueue( pxTCB ); + pxTCB = ( TCB_t * ) listGET_OWNER_OF_HEAD_ENTRY( ( &xPendingReadyList ) ); + ( void ) uxListRemove( &( pxTCB->xEventListItem ) ); + ( void ) uxListRemove( &( pxTCB->xGenericListItem ) ); + prvAddTaskToReadyList( pxTCB ); - /* If we have moved a task that has a priority higher than - the current task then we should yield. */ + /* If the moved task has a priority higher than the current + task then a yield must be performed. */ if( pxTCB->uxPriority >= pxCurrentTCB->uxPriority ) { - xYieldRequired = pdTRUE; + xYieldPending = pdTRUE; + } + else + { + mtCOVERAGE_TEST_MARKER(); } } /* 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 ) 0U ) + 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( uxPendedTicks > ( UBaseType_t ) 0U ) { - while( uxMissedTicks > ( unsigned portBASE_TYPE ) 0U ) + while( uxPendedTicks > ( UBaseType_t ) 0U ) { - vTaskIncrementTick(); - --uxMissedTicks; + if( xTaskIncrementTick() != pdFALSE ) + { + xYieldPending = pdTRUE; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + --uxPendedTicks; } + } + else + { + mtCOVERAGE_TEST_MARKER(); + } - /* 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 + if( xYieldPending == pdTRUE ) + { + #if( configUSE_PREEMPTION != 0 ) { - xYieldRequired = pdTRUE; + xAlreadyYielded = pdTRUE; } #endif + taskYIELD_IF_USING_PREEMPTION(); } - - if( ( xYieldRequired == pdTRUE ) || ( xMissedYield == pdTRUE ) ) + else { - xAlreadyYielded = pdTRUE; - xMissedYield = pdFALSE; - portYIELD_WITHIN_API(); + mtCOVERAGE_TEST_MARKER(); } } } + else + { + mtCOVERAGE_TEST_MARKER(); + } } taskEXIT_CRITICAL(); @@ -1480,47 +1761,65 @@ signed portBASE_TYPE xAlreadyYielded = pdFALSE; } /*-----------------------------------------------------------*/ -portTickType xTaskGetTickCount( void ) +TickType_t xTaskGetTickCount( void ) { -portTickType xTicks; +TickType_t xTicks; /* Critical section required if running on a 16 bit processor. */ - taskENTER_CRITICAL(); + portTICK_TYPE_ENTER_CRITICAL(); { xTicks = xTickCount; } - taskEXIT_CRITICAL(); + portTICK_TYPE_EXIT_CRITICAL(); return xTicks; } /*-----------------------------------------------------------*/ -portTickType xTaskGetTickCountFromISR( void ) +TickType_t xTaskGetTickCountFromISR( void ) { -portTickType xReturn; -unsigned portBASE_TYPE uxSavedInterruptStatus; - - uxSavedInterruptStatus = portSET_INTERRUPT_MASK_FROM_ISR(); - xReturn = xTickCount; - portCLEAR_INTERRUPT_MASK_FROM_ISR( uxSavedInterruptStatus ); +TickType_t xReturn; +UBaseType_t uxSavedInterruptStatus; + + /* RTOS ports that support interrupt nesting have the concept of a maximum + system call (or maximum API call) interrupt priority. Interrupts that are + above the maximum system call priority are kept permanently enabled, even + when the RTOS kernel is in a critical section, but cannot make any calls to + FreeRTOS API functions. If configASSERT() is defined in FreeRTOSConfig.h + then portASSERT_IF_INTERRUPT_PRIORITY_INVALID() will result in an assertion + failure if a FreeRTOS API function is called from an interrupt that has been + assigned a priority above the configured maximum system call priority. + Only FreeRTOS functions that end in FromISR can be called from interrupts + that have been assigned a priority at or (logically) below the maximum + system call interrupt priority. FreeRTOS maintains a separate interrupt + safe API to ensure interrupt entry is as fast and as simple as possible. + More information (albeit Cortex-M specific) is provided on the following + link: http://www.freertos.org/RTOS-Cortex-M3-M4.html */ + portASSERT_IF_INTERRUPT_PRIORITY_INVALID(); + + uxSavedInterruptStatus = portTICK_TYPE_SET_INTERRUPT_MASK_FROM_ISR(); + { + xReturn = xTickCount; + } + portTICK_TYPE_CLEAR_INTERRUPT_MASK_FROM_ISR( uxSavedInterruptStatus ); return xReturn; } /*-----------------------------------------------------------*/ -unsigned portBASE_TYPE uxTaskGetNumberOfTasks( void ) +UBaseType_t uxTaskGetNumberOfTasks( void ) { /* A critical section is not required because the variables are of type - portBASE_TYPE. */ + BaseType_t. */ return uxCurrentNumberOfTasks; } /*-----------------------------------------------------------*/ #if ( INCLUDE_pcTaskGetTaskName == 1 ) - signed char *pcTaskGetTaskName( xTaskHandle xTaskToQuery ) + char *pcTaskGetTaskName( TaskHandle_t xTaskToQuery ) /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ { - tskTCB *pxTCB; + TCB_t *pxTCB; /* If null is passed in here then the name of the calling task is being queried. */ pxTCB = prvGetTCBFromHandle( xTaskToQuery ); @@ -1533,145 +1832,81 @@ unsigned portBASE_TYPE uxTaskGetNumberOfTasks( void ) #if ( configUSE_TRACE_FACILITY == 1 ) - void vTaskList( signed char *pcWriteBuffer ) + UBaseType_t uxTaskGetSystemState( TaskStatus_t * const pxTaskStatusArray, const UBaseType_t uxArraySize, uint32_t * const pulTotalRunTime ) { - 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. */ + UBaseType_t uxTask = 0, uxQueue = configMAX_PRIORITIES; vTaskSuspendAll(); { - /* Run through all the lists that could potentially contain a TCB and - report the task name, state and stack high water mark. */ - - *pcWriteBuffer = ( signed char ) 0x00; - strcat( ( char * ) pcWriteBuffer, ( const char * ) "\r\n" ); - - uxQueue = uxTopUsedPriority + ( unsigned portBASE_TYPE ) 1U; - - do + /* Is there a space in the array for each task in the system? */ + if( uxArraySize >= uxCurrentNumberOfTasks ) { - uxQueue--; - - if( listLIST_IS_EMPTY( &( pxReadyTasksLists[ uxQueue ] ) ) == pdFALSE ) + /* Fill in an TaskStatus_t structure with information on each + task in the Ready state. */ + do { - prvListTaskWithinSingleList( pcWriteBuffer, ( xList * ) &( pxReadyTasksLists[ uxQueue ] ), tskREADY_CHAR ); - } - }while( uxQueue > ( unsigned short ) tskIDLE_PRIORITY ); + uxQueue--; + uxTask += prvListTaskWithinSingleList( &( pxTaskStatusArray[ uxTask ] ), &( pxReadyTasksLists[ uxQueue ] ), eReady ); - if( listLIST_IS_EMPTY( pxDelayedTaskList ) == pdFALSE ) - { - prvListTaskWithinSingleList( pcWriteBuffer, ( xList * ) pxDelayedTaskList, tskBLOCKED_CHAR ); - } + } while( uxQueue > ( UBaseType_t ) tskIDLE_PRIORITY ); /*lint !e961 MISRA exception as the casts are only redundant for some ports. */ - if( listLIST_IS_EMPTY( pxOverflowDelayedTaskList ) == pdFALSE ) - { - prvListTaskWithinSingleList( pcWriteBuffer, ( xList * ) pxOverflowDelayedTaskList, tskBLOCKED_CHAR ); - } + /* Fill in an TaskStatus_t structure with information on each + task in the Blocked state. */ + uxTask += prvListTaskWithinSingleList( &( pxTaskStatusArray[ uxTask ] ), ( List_t * ) pxDelayedTaskList, eBlocked ); + uxTask += prvListTaskWithinSingleList( &( pxTaskStatusArray[ uxTask ] ), ( List_t * ) pxOverflowDelayedTaskList, eBlocked ); - #if( INCLUDE_vTaskDelete == 1 ) - { - if( listLIST_IS_EMPTY( &xTasksWaitingTermination ) == pdFALSE ) + #if( INCLUDE_vTaskDelete == 1 ) { - prvListTaskWithinSingleList( pcWriteBuffer, &xTasksWaitingTermination, tskDELETED_CHAR ); + /* Fill in an TaskStatus_t structure with information on + each task that has been deleted but not yet cleaned up. */ + uxTask += prvListTaskWithinSingleList( &( pxTaskStatusArray[ uxTask ] ), &xTasksWaitingTermination, eDeleted ); } - } - #endif + #endif - #if ( INCLUDE_vTaskSuspend == 1 ) - { - if( listLIST_IS_EMPTY( &xSuspendedTaskList ) == pdFALSE ) + #if ( INCLUDE_vTaskSuspend == 1 ) { - prvListTaskWithinSingleList( pcWriteBuffer, &xSuspendedTaskList, tskSUSPENDED_CHAR ); + /* Fill in an TaskStatus_t structure with information on + each task in the Suspended state. */ + uxTask += prvListTaskWithinSingleList( &( pxTaskStatusArray[ uxTask ] ), &xSuspendedTaskList, eSuspended ); } - } - #endif - } - xTaskResumeAll(); - } - -#endif /* configUSE_TRACE_FACILITY */ -/*----------------------------------------------------------*/ - -#if ( configGENERATE_RUN_TIME_STATS == 1 ) - - void vTaskGetRunTimeStats( signed char *pcWriteBuffer ) - { - unsigned portBASE_TYPE uxQueue; - unsigned long ulTotalRunTimeDiv100; - - /* This is a VERY costly function that should be used for debug only. - It leaves interrupts disabled for a LONG time. */ - - vTaskSuspendAll(); - { - #ifdef portALT_GET_RUN_TIME_COUNTER_VALUE - portALT_GET_RUN_TIME_COUNTER_VALUE( ulTotalRunTime ); - #else - ulTotalRunTime = portGET_RUN_TIME_COUNTER_VALUE(); - #endif - - /* Divide ulTotalRunTime by 100 to make the percentage caluclations - simpler in the prvGenerateRunTimeStatsForTasksInList() function. */ - ulTotalRunTimeDiv100 = ulTotalRunTime / 100UL; - - /* Run through all the lists that could potentially contain a TCB, - generating a table of run timer percentages in the provided - buffer. */ - - *pcWriteBuffer = ( signed char ) 0x00; - strcat( ( char * ) pcWriteBuffer, ( const char * ) "\r\n" ); - - uxQueue = uxTopUsedPriority + ( unsigned portBASE_TYPE ) 1U; - - do - { - uxQueue--; + #endif - if( listLIST_IS_EMPTY( &( pxReadyTasksLists[ uxQueue ] ) ) == pdFALSE ) + #if ( configGENERATE_RUN_TIME_STATS == 1) { - prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, ( xList * ) &( pxReadyTasksLists[ uxQueue ] ), ulTotalRunTimeDiv100 ); + if( pulTotalRunTime != NULL ) + { + #ifdef portALT_GET_RUN_TIME_COUNTER_VALUE + portALT_GET_RUN_TIME_COUNTER_VALUE( ( *pulTotalRunTime ) ); + #else + *pulTotalRunTime = portGET_RUN_TIME_COUNTER_VALUE(); + #endif + } } - }while( uxQueue > ( unsigned short ) tskIDLE_PRIORITY ); - - if( listLIST_IS_EMPTY( pxDelayedTaskList ) == pdFALSE ) - { - prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, ( xList * ) pxDelayedTaskList, ulTotalRunTimeDiv100 ); - } - - if( listLIST_IS_EMPTY( pxOverflowDelayedTaskList ) == pdFALSE ) - { - prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, ( xList * ) pxOverflowDelayedTaskList, ulTotalRunTimeDiv100 ); - } - - #if ( INCLUDE_vTaskDelete == 1 ) - { - if( listLIST_IS_EMPTY( &xTasksWaitingTermination ) == pdFALSE ) + #else { - prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, &xTasksWaitingTermination, ulTotalRunTimeDiv100 ); + if( pulTotalRunTime != NULL ) + { + *pulTotalRunTime = 0; + } } + #endif } - #endif - - #if ( INCLUDE_vTaskSuspend == 1 ) + else { - if( listLIST_IS_EMPTY( &xSuspendedTaskList ) == pdFALSE ) - { - prvGenerateRunTimeStatsForTasksInList( pcWriteBuffer, &xSuspendedTaskList, ulTotalRunTimeDiv100 ); - } + mtCOVERAGE_TEST_MARKER(); } - #endif } - xTaskResumeAll(); + ( void ) xTaskResumeAll(); + + return uxTask; } -#endif /* configGENERATE_RUN_TIME_STATS */ +#endif /* configUSE_TRACE_FACILITY */ /*----------------------------------------------------------*/ #if ( INCLUDE_xTaskGetIdleTaskHandle == 1 ) - xTaskHandle xTaskGetIdleTaskHandle( void ) + TaskHandle_t xTaskGetIdleTaskHandle( void ) { /* If xTaskGetIdleTaskHandle() is called before the scheduler has been started, then xIdleTaskHandle will be NULL. */ @@ -1688,66 +1923,166 @@ implementations require configUSE_TICKLESS_IDLE to be set to a value other than 1. */ #if ( configUSE_TICKLESS_IDLE != 0 ) - void vTaskStepTick( portTickType xTicksToJump ) + void vTaskStepTick( const TickType_t xTicksToJump ) { + /* Correct the tick count value after a period during which the tick + was suppressed. Note this does *not* call the tick hook function for + each stepped tick. */ configASSERT( ( xTickCount + xTicksToJump ) <= xNextTaskUnblockTime ); xTickCount += xTicksToJump; + traceINCREASE_TICK_COUNT( xTicksToJump ); } #endif /* configUSE_TICKLESS_IDLE */ /*----------------------------------------------------------*/ -void vTaskIncrementTick( void ) +BaseType_t xTaskIncrementTick( void ) { -tskTCB * pxTCB; +TCB_t * pxTCB; +TickType_t xItemValue; +BaseType_t xSwitchRequired = pdFALSE; /* 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. */ traceTASK_INCREMENT_TICK( xTickCount ); - if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) + if( uxSchedulerSuspended == ( UBaseType_t ) pdFALSE ) { + /* Increment the RTOS tick, switching the delayed and overflowed + delayed lists if it wraps to 0. */ ++xTickCount; - if( xTickCount == ( portTickType ) 0U ) + { - xList *pxTemp; + /* Minor optimisation. The tick count cannot change in this + block. */ + const TickType_t xConstTickCount = xTickCount; + + if( xConstTickCount == ( TickType_t ) 0U ) + { + taskSWITCH_DELAYED_LISTS(); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + /* See if this tick has made a timeout expire. Tasks are stored in + the queue in the order of their wake time - meaning once one task + has been found whose block time has not expired there is no need to + look any further down the list. */ + if( xConstTickCount >= xNextTaskUnblockTime ) + { + for( ;; ) + { + if( listLIST_IS_EMPTY( pxDelayedTaskList ) != pdFALSE ) + { + /* The delayed list is empty. Set xNextTaskUnblockTime + to the maximum possible value so it is extremely + unlikely that the + if( xTickCount >= xNextTaskUnblockTime ) test will pass + next time through. */ + xNextTaskUnblockTime = portMAX_DELAY; + break; + } + else + { + /* The delayed list is not empty, get the value of the + item at the head of the delayed list. This is the time + at which the task at the head of the delayed list must + be removed from the Blocked state. */ + pxTCB = ( TCB_t * ) listGET_OWNER_OF_HEAD_ENTRY( pxDelayedTaskList ); + xItemValue = listGET_LIST_ITEM_VALUE( &( pxTCB->xGenericListItem ) ); + + if( xConstTickCount < xItemValue ) + { + /* It is not time to unblock this item yet, but the + item value is the time at which the task at the head + of the blocked list must be removed from the Blocked + state - so record the item value in + xNextTaskUnblockTime. */ + xNextTaskUnblockTime = xItemValue; + break; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + /* It is time to remove the item from the Blocked state. */ + ( void ) uxListRemove( &( pxTCB->xGenericListItem ) ); + + /* Is the task waiting on an event also? If so remove + it from the event list. */ + if( listLIST_ITEM_CONTAINER( &( pxTCB->xEventListItem ) ) != NULL ) + { + ( void ) uxListRemove( &( pxTCB->xEventListItem ) ); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } - /* 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! */ - configASSERT( ( listLIST_IS_EMPTY( pxDelayedTaskList ) ) ); + /* Place the unblocked task into the appropriate ready + list. */ + prvAddTaskToReadyList( pxTCB ); - pxTemp = pxDelayedTaskList; - pxDelayedTaskList = pxOverflowDelayedTaskList; - pxOverflowDelayedTaskList = pxTemp; - xNumOfOverflows++; + /* A task being unblocked cannot cause an immediate + context switch if preemption is turned off. */ + #if ( configUSE_PREEMPTION == 1 ) + { + /* Preemption is on, but a context switch should + only be performed if the unblocked task has a + priority that is equal to or higher than the + currently executing task. */ + if( pxTCB->uxPriority >= pxCurrentTCB->uxPriority ) + { + xSwitchRequired = pdTRUE; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + #endif /* configUSE_PREEMPTION */ + } + } + } + } - if( listLIST_IS_EMPTY( pxDelayedTaskList ) != pdFALSE ) + /* Tasks of equal priority to the currently running task will share + processing time (time slice) if preemption is on, and the application + writer has not explicitly turned time slicing off. */ + #if ( ( configUSE_PREEMPTION == 1 ) && ( configUSE_TIME_SLICING == 1 ) ) + { + if( listCURRENT_LIST_LENGTH( &( pxReadyTasksLists[ pxCurrentTCB->uxPriority ] ) ) > ( UBaseType_t ) 1 ) { - /* The new current delayed list is empty. Set - xNextTaskUnblockTime to the maximum possible value so it is - extremely unlikely that the - if( xTickCount >= xNextTaskUnblockTime ) test will pass until - there is an item in the delayed list. */ - xNextTaskUnblockTime = portMAX_DELAY; + xSwitchRequired = pdTRUE; } else { - /* The new current delayed list is not empty, get the value of - the item at the head of the delayed list. This is the time at - which the task at the head of the delayed list should be removed - from the Blocked state. */ - pxTCB = ( tskTCB * ) listGET_OWNER_OF_HEAD_ENTRY( pxDelayedTaskList ); - xNextTaskUnblockTime = listGET_LIST_ITEM_VALUE( &( pxTCB->xGenericListItem ) ); + mtCOVERAGE_TEST_MARKER(); } } + #endif /* ( ( configUSE_PREEMPTION == 1 ) && ( configUSE_TIME_SLICING == 1 ) ) */ - /* See if this tick has made a timeout expire. */ - prvCheckDelayedTasks(); + #if ( configUSE_TICK_HOOK == 1 ) + { + /* Guard against the tick hook being called when the pended tick + count is being unwound (when the scheduler is being unlocked). */ + if( uxPendedTicks == ( UBaseType_t ) 0U ) + { + vApplicationTickHook(); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + #endif /* configUSE_TICK_HOOK */ } else { - ++uxMissedTicks; + ++uxPendedTicks; /* The tick hook gets called at regular intervals, even if the scheduler is locked. */ @@ -1758,33 +2093,38 @@ tskTCB * pxTCB; #endif } - #if ( configUSE_TICK_HOOK == 1 ) + #if ( configUSE_PREEMPTION == 1 ) { - /* Guard against the tick hook being called when the missed tick - count is being unwound (when the scheduler is being unlocked. */ - if( uxMissedTicks == ( unsigned portBASE_TYPE ) 0U ) + if( xYieldPending != pdFALSE ) { - vApplicationTickHook(); + xSwitchRequired = pdTRUE; + } + else + { + mtCOVERAGE_TEST_MARKER(); } } - #endif /* configUSE_TICK_HOOK */ + #endif /* configUSE_PREEMPTION */ + + return xSwitchRequired; } /*-----------------------------------------------------------*/ #if ( configUSE_APPLICATION_TASK_TAG == 1 ) - void vTaskSetApplicationTaskTag( xTaskHandle xTask, pdTASK_HOOK_CODE pxHookFunction ) + void vTaskSetApplicationTaskTag( TaskHandle_t xTask, TaskHookFunction_t pxHookFunction ) { - tskTCB *xTCB; + TCB_t *xTCB; - /* If xTask is NULL then we are setting our own task hook. */ + /* If xTask is NULL then it is the task hook of the calling task that is + getting set. */ if( xTask == NULL ) { - xTCB = ( tskTCB * ) pxCurrentTCB; + xTCB = ( TCB_t * ) pxCurrentTCB; } else { - xTCB = ( tskTCB * ) xTask; + xTCB = ( TCB_t * ) xTask; } /* Save the hook function in the TCB. A critical section is required as @@ -1799,25 +2139,27 @@ tskTCB * pxTCB; #if ( configUSE_APPLICATION_TASK_TAG == 1 ) - pdTASK_HOOK_CODE xTaskGetApplicationTaskTag( xTaskHandle xTask ) + TaskHookFunction_t xTaskGetApplicationTaskTag( TaskHandle_t xTask ) { - tskTCB *xTCB; - pdTASK_HOOK_CODE xReturn; + TCB_t *xTCB; + TaskHookFunction_t xReturn; /* If xTask is NULL then we are setting our own task hook. */ if( xTask == NULL ) { - xTCB = ( tskTCB * ) pxCurrentTCB; + xTCB = ( TCB_t * ) pxCurrentTCB; } else { - xTCB = ( tskTCB * ) xTask; + xTCB = ( TCB_t * ) xTask; } /* Save the hook function in the TCB. A critical section is required as the value can be accessed from an interrupt. */ taskENTER_CRITICAL(); + { xReturn = xTCB->pxTaskTag; + } taskEXIT_CRITICAL(); return xReturn; @@ -1828,19 +2170,19 @@ tskTCB * pxTCB; #if ( configUSE_APPLICATION_TASK_TAG == 1 ) - portBASE_TYPE xTaskCallApplicationTaskHook( xTaskHandle xTask, void *pvParameter ) + BaseType_t xTaskCallApplicationTaskHook( TaskHandle_t xTask, void *pvParameter ) { - tskTCB *xTCB; - portBASE_TYPE xReturn; + TCB_t *xTCB; + BaseType_t xReturn; /* If xTask is NULL then we are calling our own task hook. */ if( xTask == NULL ) { - xTCB = ( tskTCB * ) pxCurrentTCB; + xTCB = ( TCB_t * ) pxCurrentTCB; } else { - xTCB = ( tskTCB * ) xTask; + xTCB = ( TCB_t * ) xTask; } if( xTCB->pxTaskTag != NULL ) @@ -1858,16 +2200,17 @@ tskTCB * pxTCB; #endif /* configUSE_APPLICATION_TASK_TAG */ /*-----------------------------------------------------------*/ -void __attribute__((used)) vTaskSwitchContext( void ) +void vTaskSwitchContext( void ) { - if( uxSchedulerSuspended != ( unsigned portBASE_TYPE ) pdFALSE ) + if( uxSchedulerSuspended != ( UBaseType_t ) pdFALSE ) { /* The scheduler is currently suspended - do not allow a context switch. */ - xMissedYield = pdTRUE; + xYieldPending = pdTRUE; } else { + xYieldPending = pdFALSE; traceTASK_SWITCHED_OUT(); #if ( configGENERATE_RUN_TIME_STATS == 1 ) @@ -1878,63 +2221,153 @@ void __attribute__((used)) vTaskSwitchContext( void ) ulTotalRunTime = portGET_RUN_TIME_COUNTER_VALUE(); #endif - /* 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 += ( ulTotalRunTime - ulTaskSwitchedInTime ); + /* 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. The guard against negative values is to protect + against suspect run time stat counter implementations - which + are provided by the application, not the kernel. */ + if( ulTotalRunTime > ulTaskSwitchedInTime ) + { + pxCurrentTCB->ulRunTimeCounter += ( ulTotalRunTime - ulTaskSwitchedInTime ); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } ulTaskSwitchedInTime = ulTotalRunTime; } #endif /* configGENERATE_RUN_TIME_STATS */ - taskFIRST_CHECK_FOR_STACK_OVERFLOW(); - taskSECOND_CHECK_FOR_STACK_OVERFLOW(); + /* Check for stack overflow, if configured. */ + taskCHECK_FOR_STACK_OVERFLOW(); + /* Select a new task to run using either the generic C or port + optimised asm code. */ taskSELECT_HIGHEST_PRIORITY_TASK(); - traceTASK_SWITCHED_IN(); + + #if ( configUSE_NEWLIB_REENTRANT == 1 ) + { + /* Switch Newlib's _impure_ptr variable to point to the _reent + structure specific to this task. */ + _impure_ptr = &( pxCurrentTCB->xNewLib_reent ); + } + #endif /* configUSE_NEWLIB_REENTRANT */ } } /*-----------------------------------------------------------*/ -void vTaskPlaceOnEventList( const xList * const pxEventList, portTickType xTicksToWait ) +void vTaskPlaceOnEventList( List_t * const pxEventList, const TickType_t xTicksToWait ) { -portTickType xTimeToWake; +TickType_t xTimeToWake; configASSERT( pxEventList ); - /* THIS FUNCTION MUST BE CALLED WITH INTERRUPTS DISABLED OR THE - SCHEDULER SUSPENDED. */ + /* THIS FUNCTION MUST BE CALLED WITH EITHER INTERRUPTS DISABLED OR THE + SCHEDULER SUSPENDED AND THE QUEUE BEING ACCESSED LOCKED. */ /* 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 ) ); + is the first to be woken by the event. The queue that contains the event + list is locked, preventing simultaneous access from interrupts. */ + vListInsert( pxEventList, &( pxCurrentTCB->xEventListItem ) ); + + /* The task must be removed from from the ready list before it is added to + the blocked list as the same list item is used for both lists. Exclusive + access to the ready lists guaranteed because the scheduler is locked. */ + if( uxListRemove( &( pxCurrentTCB->xGenericListItem ) ) == ( UBaseType_t ) 0 ) + { + /* The current task must be in a ready list, so there is no need to + check, and the port reset macro can be called directly. */ + portRESET_READY_PRIORITY( pxCurrentTCB->uxPriority, uxTopReadyPriority ); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + #if ( INCLUDE_vTaskSuspend == 1 ) + { + if( xTicksToWait == portMAX_DELAY ) + { + /* Add the task to the suspended task list instead of a delayed task + list to ensure the task is not woken by a timing event. It will + block indefinitely. */ + vListInsertEnd( &xSuspendedTaskList, &( 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, the + scheduler will handle it. */ + xTimeToWake = xTickCount + xTicksToWait; + prvAddCurrentTaskToDelayedList( xTimeToWake ); + } + } + #else /* INCLUDE_vTaskSuspend */ + { + /* Calculate the time at which the task should be woken if the event does + not occur. This may overflow but this doesn't matter, the scheduler + will handle it. */ + xTimeToWake = xTickCount + xTicksToWait; + prvAddCurrentTaskToDelayedList( xTimeToWake ); + } + #endif /* INCLUDE_vTaskSuspend */ +} +/*-----------------------------------------------------------*/ - /* 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. */ - if( uxListRemove( ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ) == 0 ) +void vTaskPlaceOnUnorderedEventList( List_t * pxEventList, const TickType_t xItemValue, const TickType_t xTicksToWait ) +{ +TickType_t xTimeToWake; + + configASSERT( pxEventList ); + + /* THIS FUNCTION MUST BE CALLED WITH THE SCHEDULER SUSPENDED. It is used by + the event groups implementation. */ + configASSERT( uxSchedulerSuspended != 0 ); + + /* Store the item value in the event list item. It is safe to access the + event list item here as interrupts won't access the event list item of a + task that is not in the Blocked state. */ + listSET_LIST_ITEM_VALUE( &( pxCurrentTCB->xEventListItem ), xItemValue | taskEVENT_LIST_ITEM_VALUE_IN_USE ); + + /* Place the event list item of the TCB at the end of the appropriate event + list. It is safe to access the event list here because it is part of an + event group implementation - and interrupts don't access event groups + directly (instead they access them indirectly by pending function calls to + the task level). */ + vListInsertEnd( pxEventList, &( pxCurrentTCB->xEventListItem ) ); + + /* The task must be removed from the ready list before it is added to the + blocked list. Exclusive access can be assured to the ready list as the + scheduler is locked. */ + if( uxListRemove( &( pxCurrentTCB->xGenericListItem ) ) == ( UBaseType_t ) 0 ) { /* The current task must be in a ready list, so there is no need to check, and the port reset macro can be called directly. */ portRESET_READY_PRIORITY( pxCurrentTCB->uxPriority, uxTopReadyPriority ); } + else + { + mtCOVERAGE_TEST_MARKER(); + } #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 + /* Add the task to the suspended task list instead of a delayed task + list to ensure it is not woken by a timing event. It will block indefinitely. */ - vListInsertEnd( ( xList * ) &xSuspendedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); + vListInsertEnd( &xSuspendedTaskList, &( 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. */ + /* Calculate the time at which the task should be woken if the event + does not occur. This may overflow but this doesn't matter, the + kernel will manage it correctly. */ xTimeToWake = xTickCount + xTicksToWait; prvAddCurrentTaskToDelayedList( xTimeToWake ); } @@ -1942,7 +2375,8 @@ portTickType xTimeToWake; #else /* INCLUDE_vTaskSuspend */ { /* Calculate the time at which the task should be woken if the event does - not occur. This may overflow but this doesn't matter. */ + not occur. This may overflow but this doesn't matter, the kernel + will manage it correctly. */ xTimeToWake = xTickCount + xTicksToWait; prvAddCurrentTaskToDelayedList( xTimeToWake ); } @@ -1952,86 +2386,185 @@ portTickType xTimeToWake; #if configUSE_TIMERS == 1 - void vTaskPlaceOnEventListRestricted( const xList * const pxEventList, portTickType xTicksToWait ) + void vTaskPlaceOnEventListRestricted( List_t * const pxEventList, const TickType_t xTicksToWait, const BaseType_t xWaitIndefinitely ) { - portTickType xTimeToWake; + TickType_t xTimeToWake; configASSERT( pxEventList ); /* This function should not be called by application code hence the 'Restricted' in its name. It is not part of the public API. It is designed for use by kernel code, and has special calling requirements - - it should be called from a critical section. */ + it should be called with the scheduler suspended. */ /* Place the event list item of the TCB in the appropriate event list. In this case it is assume that this is the only task that is going to be waiting on this event list, so the faster vListInsertEnd() function can be used in place of vListInsert. */ - vListInsertEnd( ( xList * ) pxEventList, ( xListItem * ) &( pxCurrentTCB->xEventListItem ) ); + vListInsertEnd( pxEventList, &( pxCurrentTCB->xEventListItem ) ); /* We must remove this task from the ready list before adding it to the blocked list as the same list item is used for both lists. This - function is called form a critical section. */ - if( uxListRemove( ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ) == 0 ) + function is called with the scheduler locked so interrupts will not + access the lists at the same time. */ + if( uxListRemove( &( pxCurrentTCB->xGenericListItem ) ) == ( UBaseType_t ) 0 ) { /* The current task must be in a ready list, so there is no need to check, and the port reset macro can be called directly. */ portRESET_READY_PRIORITY( pxCurrentTCB->uxPriority, uxTopReadyPriority ); } + else + { + mtCOVERAGE_TEST_MARKER(); + } - /* 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; + /* If vTaskSuspend() is available then the suspended task list is also + available and a task that is blocking indefinitely can enter the + suspended state (it is not really suspended as it will re-enter the + Ready state when the event it is waiting indefinitely for occurs). + Blocking indefinitely is useful when using tickless idle mode as when + all tasks are blocked indefinitely all timers can be turned off. */ + #if( INCLUDE_vTaskSuspend == 1 ) + { + if( xWaitIndefinitely == pdTRUE ) + { + /* Add the task to the suspended task list instead of a delayed + task list to ensure the task is not woken by a timing event. It + will block indefinitely. */ + vListInsertEnd( &xSuspendedTaskList, &( 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; + traceTASK_DELAY_UNTIL(); + prvAddCurrentTaskToDelayedList( xTimeToWake ); + } + } + #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; + traceTASK_DELAY_UNTIL(); + prvAddCurrentTaskToDelayedList( xTimeToWake ); - traceTASK_DELAY_UNTIL(); - prvAddCurrentTaskToDelayedList( xTimeToWake ); + /* Remove compiler warnings when INCLUDE_vTaskSuspend() is not + defined. */ + ( void ) xWaitIndefinitely; + } + #endif } #endif /* configUSE_TIMERS */ /*-----------------------------------------------------------*/ -signed portBASE_TYPE xTaskRemoveFromEventList( const xList * const pxEventList ) +BaseType_t xTaskRemoveFromEventList( const List_t * const pxEventList ) { -tskTCB *pxUnblockedTCB; -portBASE_TYPE xReturn; +TCB_t *pxUnblockedTCB; +BaseType_t xReturn; - /* THIS FUNCTION MUST BE CALLED WITH INTERRUPTS DISABLED OR THE - SCHEDULER SUSPENDED. It can also be called from within an ISR. */ + /* THIS FUNCTION MUST BE CALLED FROM A CRITICAL SECTION. It can also be + called from a critical section 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. + /* The event list is sorted in priority order, so the first in the list can + be removed as it is known to be the highest priority. 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. + means exclusive access to the event list is guaranteed here. This function assumes that a check has already been made to ensure that pxEventList is not empty. */ - pxUnblockedTCB = ( tskTCB * ) listGET_OWNER_OF_HEAD_ENTRY( pxEventList ); + pxUnblockedTCB = ( TCB_t * ) listGET_OWNER_OF_HEAD_ENTRY( pxEventList ); configASSERT( pxUnblockedTCB ); - uxListRemove( &( pxUnblockedTCB->xEventListItem ) ); + ( void ) uxListRemove( &( pxUnblockedTCB->xEventListItem ) ); + + if( uxSchedulerSuspended == ( UBaseType_t ) pdFALSE ) + { + ( void ) uxListRemove( &( pxUnblockedTCB->xGenericListItem ) ); + prvAddTaskToReadyList( pxUnblockedTCB ); + } + else + { + /* The delayed and ready lists cannot be accessed, so hold this task + pending until the scheduler is resumed. */ + vListInsertEnd( &( xPendingReadyList ), &( pxUnblockedTCB->xEventListItem ) ); + } - if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) + if( pxUnblockedTCB->uxPriority > pxCurrentTCB->uxPriority ) { - uxListRemove( &( pxUnblockedTCB->xGenericListItem ) ); - prvAddTaskToReadyQueue( pxUnblockedTCB ); + /* 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; + + /* Mark that a yield is pending in case the user is not using the + "xHigherPriorityTaskWoken" parameter to an ISR safe FreeRTOS function. */ + xYieldPending = pdTRUE; } 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 ) ); + xReturn = pdFALSE; + } + + #if( configUSE_TICKLESS_IDLE != 0 ) + { + /* If a task is blocked on a kernel object then xNextTaskUnblockTime + might be set to the blocked task's time out time. If the task is + unblocked for a reason other than a timeout xNextTaskUnblockTime is + normally left unchanged, because it is automatically reset to a new + value when the tick count equals xNextTaskUnblockTime. However if + tickless idling is used it might be more important to enter sleep mode + at the earliest possible time - so reset xNextTaskUnblockTime here to + ensure it is updated at the earliest possible time. */ + prvResetNextTaskUnblockTime(); } + #endif + + return xReturn; +} +/*-----------------------------------------------------------*/ + +BaseType_t xTaskRemoveFromUnorderedEventList( ListItem_t * pxEventListItem, const TickType_t xItemValue ) +{ +TCB_t *pxUnblockedTCB; +BaseType_t xReturn; + + /* THIS FUNCTION MUST BE CALLED WITH THE SCHEDULER SUSPENDED. It is used by + the event flags implementation. */ + configASSERT( uxSchedulerSuspended != pdFALSE ); + + /* Store the new item value in the event list. */ + listSET_LIST_ITEM_VALUE( pxEventListItem, xItemValue | taskEVENT_LIST_ITEM_VALUE_IN_USE ); + + /* Remove the event list form the event flag. Interrupts do not access + event flags. */ + pxUnblockedTCB = ( TCB_t * ) listGET_LIST_ITEM_OWNER( pxEventListItem ); + configASSERT( pxUnblockedTCB ); + ( void ) uxListRemove( pxEventListItem ); + + /* Remove the task from the delayed list and add it to the ready list. The + scheduler is suspended so interrupts will not be accessing the ready + lists. */ + ( void ) uxListRemove( &( pxUnblockedTCB->xGenericListItem ) ); + prvAddTaskToReadyList( pxUnblockedTCB ); - if( pxUnblockedTCB->uxPriority >= pxCurrentTCB->uxPriority ) + 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; + + /* Mark that a yield is pending in case the user is not using the + "xHigherPriorityTaskWoken" parameter to an ISR safe FreeRTOS function. */ + xYieldPending = pdTRUE; } else { @@ -2042,7 +2575,7 @@ portBASE_TYPE xReturn; } /*-----------------------------------------------------------*/ -void vTaskSetTimeOutState( xTimeOutType * const pxTimeOut ) +void vTaskSetTimeOutState( TimeOut_t * const pxTimeOut ) { configASSERT( pxTimeOut ); pxTimeOut->xOverflowCount = xNumOfOverflows; @@ -2050,15 +2583,18 @@ void vTaskSetTimeOutState( xTimeOutType * const pxTimeOut ) } /*-----------------------------------------------------------*/ -portBASE_TYPE xTaskCheckForTimeOut( xTimeOutType * const pxTimeOut, portTickType * const pxTicksToWait ) +BaseType_t xTaskCheckForTimeOut( TimeOut_t * const pxTimeOut, TickType_t * const pxTicksToWait ) { -portBASE_TYPE xReturn; +BaseType_t xReturn; configASSERT( pxTimeOut ); configASSERT( pxTicksToWait ); taskENTER_CRITICAL(); { + /* Minor optimisation. The tick count cannot change in this block. */ + const TickType_t xConstTickCount = xTickCount; + #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 @@ -2070,7 +2606,7 @@ portBASE_TYPE xReturn; else /* We are not blocking indefinitely, perform the checks below. */ #endif - if( ( xNumOfOverflows != pxTimeOut->xOverflowCount ) && ( ( portTickType ) xTickCount >= ( portTickType ) pxTimeOut->xTimeOnEntering ) ) + if( ( xNumOfOverflows != pxTimeOut->xOverflowCount ) && ( xConstTickCount >= pxTimeOut->xTimeOnEntering ) ) /*lint !e525 Indentation preferred as is to make code within pre-processor directives clearer. */ { /* The tick count is greater than the time at which vTaskSetTimeout() was called, but has also overflowed since vTaskSetTimeOut() was called. @@ -2078,10 +2614,10 @@ portBASE_TYPE xReturn; passed since vTaskSetTimeout() was called. */ xReturn = pdTRUE; } - else if( ( ( portTickType ) ( ( portTickType ) xTickCount - ( portTickType ) pxTimeOut->xTimeOnEntering ) ) < ( portTickType ) *pxTicksToWait ) + else if( ( xConstTickCount - pxTimeOut->xTimeOnEntering ) < *pxTicksToWait ) { /* Not a genuine timeout. Adjust parameters for time remaining. */ - *pxTicksToWait -= ( ( portTickType ) xTickCount - ( portTickType ) pxTimeOut->xTimeOnEntering ); + *pxTicksToWait -= ( xConstTickCount - pxTimeOut->xTimeOnEntering ); vTaskSetTimeOutState( pxTimeOut ); xReturn = pdFALSE; } @@ -2098,20 +2634,20 @@ portBASE_TYPE xReturn; void vTaskMissedYield( void ) { - xMissedYield = pdTRUE; + xYieldPending = pdTRUE; } /*-----------------------------------------------------------*/ #if ( configUSE_TRACE_FACILITY == 1 ) - unsigned portBASE_TYPE uxTaskGetTaskNumber( xTaskHandle xTask ) + UBaseType_t uxTaskGetTaskNumber( TaskHandle_t xTask ) { - unsigned portBASE_TYPE uxReturn; - tskTCB *pxTCB; + UBaseType_t uxReturn; + TCB_t *pxTCB; if( xTask != NULL ) { - pxTCB = ( tskTCB * ) xTask; + pxTCB = ( TCB_t * ) xTask; uxReturn = pxTCB->uxTaskNumber; } else @@ -2127,13 +2663,13 @@ void vTaskMissedYield( void ) #if ( configUSE_TRACE_FACILITY == 1 ) - void vTaskSetTaskNumber( xTaskHandle xTask, unsigned portBASE_TYPE uxHandle ) + void vTaskSetTaskNumber( TaskHandle_t xTask, const UBaseType_t uxHandle ) { - tskTCB *pxTCB; + TCB_t *pxTCB; if( xTask != NULL ) { - pxTCB = ( tskTCB * ) xTask; + pxTCB = ( TCB_t * ) xTask; pxTCB->uxTaskNumber = uxHandle; } } @@ -2182,10 +2718,14 @@ static portTASK_FUNCTION( prvIdleTask, pvParameters ) 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 ) + if( listCURRENT_LIST_LENGTH( &( pxReadyTasksLists[ tskIDLE_PRIORITY ] ) ) > ( UBaseType_t ) 1 ) { taskYIELD(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } } #endif /* ( ( configUSE_PREEMPTION == 1 ) && ( configIDLE_SHOULD_YIELD == 1 ) ) */ @@ -2208,7 +2748,8 @@ static portTASK_FUNCTION( prvIdleTask, pvParameters ) configUSE_TICKLESS_IDLE to be set to a value other than 1. */ #if ( configUSE_TICKLESS_IDLE != 0 ) { - portTickType xExpectedIdleTime; + TickType_t xExpectedIdleTime; + /* It is not desirable to suspend then resume the scheduler on each iteration of the idle task. Therefore, a preliminary test of the expected idle time is performed without the @@ -2228,21 +2769,33 @@ static portTASK_FUNCTION( prvIdleTask, pvParameters ) if( xExpectedIdleTime >= configEXPECTED_IDLE_TIME_BEFORE_SLEEP ) { + traceLOW_POWER_IDLE_BEGIN(); portSUPPRESS_TICKS_AND_SLEEP( xExpectedIdleTime ); + traceLOW_POWER_IDLE_END(); + } + else + { + mtCOVERAGE_TEST_MARKER(); } } - xTaskResumeAll(); + ( void ) xTaskResumeAll(); + } + else + { + mtCOVERAGE_TEST_MARKER(); } } #endif /* configUSE_TICKLESS_IDLE */ } -} /*lint !e715 pvParameters is not accessed but all task functions require the same prototype. */ +} /*-----------------------------------------------------------*/ -#if configUSE_TICKLESS_IDLE != 0 +#if( configUSE_TICKLESS_IDLE != 0 ) eSleepModeStatus eTaskConfirmSleepModeStatus( void ) { + /* The idle task exists in addition to the application tasks. */ + const UBaseType_t uxNonApplicationTasks = 1; eSleepModeStatus eReturn = eStandardSleep; if( listCURRENT_LIST_LENGTH( &xPendingReadyList ) != 0 ) @@ -2250,74 +2803,92 @@ static portTASK_FUNCTION( prvIdleTask, pvParameters ) /* A task was made ready while the scheduler was suspended. */ eReturn = eAbortSleep; } - else if( xMissedYield != pdFALSE ) + else if( xYieldPending != pdFALSE ) { /* A yield was pended while the scheduler was suspended. */ eReturn = eAbortSleep; } else { - #if configUSE_TIMERS == 0 + /* If all the tasks are in the suspended list (which might mean they + have an infinite block time rather than actually being suspended) + then it is safe to turn all clocks off and just wait for external + interrupts. */ + if( listCURRENT_LIST_LENGTH( &xSuspendedTaskList ) == ( uxCurrentNumberOfTasks - uxNonApplicationTasks ) ) { - /* The idle task exists in addition to the application tasks. */ - const unsigned portBASE_TYPE uxNonApplicationTasks = 1; - - /* If timers are not being used and all the tasks are in the - suspended list (which might mean they have an infinite block - time rather than actually being suspended) then it is safe to - turn all clocks off and just wait for external interrupts. */ - if( listCURRENT_LIST_LENGTH( &xSuspendedTaskList ) == ( uxCurrentNumberOfTasks - uxNonApplicationTasks ) ) - { - eReturn = eNoTasksWaitingTimeout; - } + eReturn = eNoTasksWaitingTimeout; + } + else + { + mtCOVERAGE_TEST_MARKER(); } - #endif /* configUSE_TIMERS */ } return eReturn; } + #endif /* configUSE_TICKLESS_IDLE */ /*-----------------------------------------------------------*/ -static void prvInitialiseTCBVariables( tskTCB *pxTCB, const signed char * const pcName, unsigned portBASE_TYPE uxPriority, const xMemoryRegion * const xRegions, unsigned short usStackDepth ) +static void prvInitialiseTCBVariables( TCB_t * const pxTCB, const char * const pcName, UBaseType_t uxPriority, const MemoryRegion_t * const xRegions, const uint16_t usStackDepth ) /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ { - /* Store the function name in the TCB. */ - #if configMAX_TASK_NAME_LEN > 1 +UBaseType_t x; + + /* Store the task name in the TCB. */ + for( x = ( UBaseType_t ) 0; x < ( UBaseType_t ) configMAX_TASK_NAME_LEN; x++ ) { - /* Don't bring strncpy into the build unnecessarily. */ - strncpy( ( char * ) pxTCB->pcTaskName, ( const char * ) pcName, ( unsigned short ) configMAX_TASK_NAME_LEN ); + pxTCB->pcTaskName[ x ] = pcName[ x ]; + + /* Don't copy all configMAX_TASK_NAME_LEN if the string is shorter than + configMAX_TASK_NAME_LEN characters just in case the memory after the + string is not accessible (extremely unlikely). */ + if( pcName[ x ] == 0x00 ) + { + break; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } } - #endif /* configMAX_TASK_NAME_LEN */ - pxTCB->pcTaskName[ ( unsigned short ) configMAX_TASK_NAME_LEN - ( unsigned short ) 1 ] = ( signed char ) '\0'; + + /* Ensure the name string is terminated in the case that the string length + was greater or equal to configMAX_TASK_NAME_LEN. */ + pxTCB->pcTaskName[ configMAX_TASK_NAME_LEN - 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 ) + if( uxPriority >= ( UBaseType_t ) configMAX_PRIORITIES ) + { + uxPriority = ( UBaseType_t ) configMAX_PRIORITIES - ( UBaseType_t ) 1U; + } + else { - uxPriority = configMAX_PRIORITIES - ( unsigned portBASE_TYPE ) 1U; + mtCOVERAGE_TEST_MARKER(); } pxTCB->uxPriority = uxPriority; #if ( configUSE_MUTEXES == 1 ) { pxTCB->uxBasePriority = uxPriority; + pxTCB->uxMutexesHeld = 0; } #endif /* configUSE_MUTEXES */ vListInitialiseItem( &( pxTCB->xGenericListItem ) ); vListInitialiseItem( &( pxTCB->xEventListItem ) ); - /* Set the pxTCB as a link back from the xListItem. This is so we can get + /* Set the pxTCB as a link back from the ListItem_t. 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_VALUE( &( pxTCB->xEventListItem ), ( TickType_t ) configMAX_PRIORITIES - ( TickType_t ) uxPriority ); /*lint !e961 MISRA exception as the casts are only redundant for some ports. */ listSET_LIST_ITEM_OWNER( &( pxTCB->xEventListItem ), pxTCB ); #if ( portCRITICAL_NESTING_IN_TCB == 1 ) { - pxTCB->uxCriticalNesting = ( unsigned portBASE_TYPE ) 0U; + pxTCB->uxCriticalNesting = ( UBaseType_t ) 0U; } #endif /* portCRITICAL_NESTING_IN_TCB */ @@ -2343,21 +2914,79 @@ static void prvInitialiseTCBVariables( tskTCB *pxTCB, const signed char * const ( void ) usStackDepth; } #endif /* portUSING_MPU_WRAPPERS */ + + #if( configNUM_THREAD_LOCAL_STORAGE_POINTERS != 0 ) + { + for( x = 0; x < ( UBaseType_t ) configNUM_THREAD_LOCAL_STORAGE_POINTERS; x++ ) + { + pxTCB->pvThreadLocalStoragePointers[ x ] = NULL; + } + } + #endif + + #if ( configUSE_TASK_NOTIFICATIONS == 1 ) + { + pxTCB->ulNotifiedValue = 0; + pxTCB->eNotifyState = eNotWaitingNotification; + } + #endif + + #if ( configUSE_NEWLIB_REENTRANT == 1 ) + { + /* Initialise this task's Newlib reent structure. */ + _REENT_INIT_PTR( ( &( pxTCB->xNewLib_reent ) ) ); + } + #endif /* configUSE_NEWLIB_REENTRANT */ } /*-----------------------------------------------------------*/ -#if ( portUSING_MPU_WRAPPERS == 1 ) +#if ( configNUM_THREAD_LOCAL_STORAGE_POINTERS != 0 ) + + void vTaskSetThreadLocalStoragePointer( TaskHandle_t xTaskToSet, BaseType_t xIndex, void *pvValue ) + { + TCB_t *pxTCB; + + if( xIndex < configNUM_THREAD_LOCAL_STORAGE_POINTERS ) + { + pxTCB = prvGetTCBFromHandle( xTaskToSet ); + pxTCB->pvThreadLocalStoragePointers[ xIndex ] = pvValue; + } + } + +#endif /* configNUM_THREAD_LOCAL_STORAGE_POINTERS */ +/*-----------------------------------------------------------*/ + +#if ( configNUM_THREAD_LOCAL_STORAGE_POINTERS != 0 ) - void vTaskAllocateMPURegions( xTaskHandle xTaskToModify, const xMemoryRegion * const xRegions ) + void *pvTaskGetThreadLocalStoragePointer( TaskHandle_t xTaskToQuery, BaseType_t xIndex ) { - tskTCB *pxTCB; + void *pvReturn = NULL; + TCB_t *pxTCB; - if( xTaskToModify == pxCurrentTCB ) + if( xIndex < configNUM_THREAD_LOCAL_STORAGE_POINTERS ) { - xTaskToModify = NULL; + pxTCB = prvGetTCBFromHandle( xTaskToQuery ); + pvReturn = pxTCB->pvThreadLocalStoragePointers[ xIndex ]; } + else + { + pvReturn = NULL; + } + + return pvReturn; + } + +#endif /* configNUM_THREAD_LOCAL_STORAGE_POINTERS */ +/*-----------------------------------------------------------*/ + +#if ( portUSING_MPU_WRAPPERS == 1 ) + + void vTaskAllocateMPURegions( TaskHandle_t xTaskToModify, const MemoryRegion_t * const xRegions ) + { + TCB_t *pxTCB; - /* If null is passed in here then we are deleting ourselves. */ + /* If null is passed in here then we are modifying the MPU settings of + the calling task. */ pxTCB = prvGetTCBFromHandle( xTaskToModify ); vPortStoreTaskMPUSettings( &( pxTCB->xMPUSettings ), xRegions, NULL, 0 ); @@ -2368,26 +2997,26 @@ static void prvInitialiseTCBVariables( tskTCB *pxTCB, const signed char * const static void prvInitialiseTaskLists( void ) { -unsigned portBASE_TYPE uxPriority; +UBaseType_t uxPriority; - for( uxPriority = ( unsigned portBASE_TYPE ) 0U; uxPriority < configMAX_PRIORITIES; uxPriority++ ) + for( uxPriority = ( UBaseType_t ) 0U; uxPriority < ( UBaseType_t ) configMAX_PRIORITIES; uxPriority++ ) { - vListInitialise( ( xList * ) &( pxReadyTasksLists[ uxPriority ] ) ); + vListInitialise( &( pxReadyTasksLists[ uxPriority ] ) ); } - vListInitialise( ( xList * ) &xDelayedTaskList1 ); - vListInitialise( ( xList * ) &xDelayedTaskList2 ); - vListInitialise( ( xList * ) &xPendingReadyList ); + vListInitialise( &xDelayedTaskList1 ); + vListInitialise( &xDelayedTaskList2 ); + vListInitialise( &xPendingReadyList ); #if ( INCLUDE_vTaskDelete == 1 ) { - vListInitialise( ( xList * ) &xTasksWaitingTermination ); + vListInitialise( &xTasksWaitingTermination ); } #endif /* INCLUDE_vTaskDelete */ #if ( INCLUDE_vTaskSuspend == 1 ) { - vListInitialise( ( xList * ) &xSuspendedTaskList ); + vListInitialise( &xSuspendedTaskList ); } #endif /* INCLUDE_vTaskSuspend */ @@ -2402,24 +3031,26 @@ static void prvCheckTasksWaitingTermination( void ) { #if ( INCLUDE_vTaskDelete == 1 ) { - portBASE_TYPE xListIsEmpty; + BaseType_t xListIsEmpty; /* ucTasksDeleted is used to prevent vTaskSuspendAll() being called too often in the idle task. */ - while( uxTasksDeleted > ( unsigned portBASE_TYPE ) 0U ) + while( uxTasksDeleted > ( UBaseType_t ) 0U ) { vTaskSuspendAll(); + { xListIsEmpty = listLIST_IS_EMPTY( &xTasksWaitingTermination ); - xTaskResumeAll(); + } + ( void ) xTaskResumeAll(); if( xListIsEmpty == pdFALSE ) { - tskTCB *pxTCB; + TCB_t *pxTCB; taskENTER_CRITICAL(); { - pxTCB = ( tskTCB * ) listGET_OWNER_OF_HEAD_ENTRY( ( ( xList * ) &xTasksWaitingTermination ) ); - uxListRemove( &( pxTCB->xGenericListItem ) ); + pxTCB = ( TCB_t * ) listGET_OWNER_OF_HEAD_ENTRY( ( &xTasksWaitingTermination ) ); + ( void ) uxListRemove( &( pxTCB->xGenericListItem ) ); --uxCurrentNumberOfTasks; --uxTasksDeleted; } @@ -2427,13 +3058,17 @@ static void prvCheckTasksWaitingTermination( void ) prvDeleteTCB( pxTCB ); } + else + { + mtCOVERAGE_TEST_MARKER(); + } } } #endif /* vTaskDelete */ } /*-----------------------------------------------------------*/ -static void prvAddCurrentTaskToDelayedList( portTickType xTimeToWake ) +static void prvAddCurrentTaskToDelayedList( const TickType_t xTimeToWake ) { /* The list item will be inserted in wake time order. */ listSET_LIST_ITEM_VALUE( &( pxCurrentTCB->xGenericListItem ), xTimeToWake ); @@ -2441,12 +3076,12 @@ static void prvAddCurrentTaskToDelayedList( portTickType xTimeToWake ) if( xTimeToWake < xTickCount ) { /* Wake time has overflowed. Place this item in the overflow list. */ - vListInsert( ( xList * ) pxOverflowDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); + vListInsert( pxOverflowDelayedTaskList, &( pxCurrentTCB->xGenericListItem ) ); } else { - /* The wake time has not overflowed, so we can use the current block list. */ - vListInsert( ( xList * ) pxDelayedTaskList, ( xListItem * ) &( pxCurrentTCB->xGenericListItem ) ); + /* The wake time has not overflowed, so the current block list is used. */ + vListInsert( pxDelayedTaskList, &( pxCurrentTCB->xGenericListItem ) ); /* If the task entering the blocked state was placed at the head of the list of blocked tasks then xNextTaskUnblockTime needs to be updated @@ -2455,160 +3090,189 @@ static void prvAddCurrentTaskToDelayedList( portTickType xTimeToWake ) { xNextTaskUnblockTime = xTimeToWake; } + else + { + mtCOVERAGE_TEST_MARKER(); + } } } /*-----------------------------------------------------------*/ -static tskTCB *prvAllocateTCBAndStack( unsigned short usStackDepth, portSTACK_TYPE *puxStackBuffer ) +static TCB_t *prvAllocateTCBAndStack( const uint16_t usStackDepth, StackType_t * const puxStackBuffer ) { -tskTCB *pxNewTCB; +TCB_t *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 ) ); - tot_alloc_overhead += sizeof( tskTCB ); - if( pxNewTCB != NULL ) + /* If the stack grows down then allocate the stack then the TCB so the stack + does not grow into the TCB. Likewise if the stack grows up then allocate + the TCB then the stack. */ + #if( portSTACK_GROWTH > 0 ) { - /* 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 ); - tot_alloc_stack += (( size_t )usStackDepth ) * sizeof( portSTACK_TYPE ); - if( pxNewTCB->pxStack == NULL ) - { - /* Could not allocate the stack. Delete the allocated TCB. */ - vPortFree( pxNewTCB ); - pxNewTCB = NULL; - } - else + /* Allocate space for the TCB. Where the memory comes from depends on + the implementation of the port malloc function. */ + pxNewTCB = ( TCB_t * ) pvPortMalloc( sizeof( TCB_t ) ); + + if( pxNewTCB != NULL ) { - /* Just to help debugging. */ - memset( pxNewTCB->pxStack, ( int ) tskSTACK_FILL_BYTE, ( size_t ) usStackDepth * sizeof( portSTACK_TYPE ) ); + /* 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 = ( StackType_t * ) pvPortMallocAligned( ( ( ( size_t ) usStackDepth ) * sizeof( StackType_t ) ), puxStackBuffer ); /*lint !e961 MISRA exception as the casts are only redundant for some ports. */ + + if( pxNewTCB->pxStack == NULL ) + { + /* Could not allocate the stack. Delete the allocated TCB. */ + vPortFree( pxNewTCB ); + pxNewTCB = NULL; + } } } - - return pxNewTCB; -} -/*-----------------------------------------------------------*/ - -#if ( configUSE_TRACE_FACILITY == 1 ) - - static void prvListTaskWithinSingleList( const signed char *pcWriteBuffer, xList *pxList, signed char cStatus ) + #else /* portSTACK_GROWTH */ { - volatile tskTCB *pxNextTCB, *pxFirstTCB; - unsigned short usStackRemaining; - PRIVILEGED_DATA static char pcStatusString[ configMAX_TASK_NAME_LEN + 30 ]; + StackType_t *pxStack; - /* Write the details of all the TCB's in pxList into the buffer. */ - listGET_OWNER_OF_NEXT_ENTRY( pxFirstTCB, pxList ); - do + /* Allocate space for the stack used by the task being created. */ + pxStack = ( StackType_t * ) pvPortMallocAligned( ( ( ( size_t ) usStackDepth ) * sizeof( StackType_t ) ), puxStackBuffer ); /*lint !e961 MISRA exception as the casts are only redundant for some ports. */ + + if( pxStack != NULL ) { - listGET_OWNER_OF_NEXT_ENTRY( pxNextTCB, pxList ); - #if ( portSTACK_GROWTH > 0 ) + /* Allocate space for the TCB. Where the memory comes from depends + on the implementation of the port malloc function. */ + pxNewTCB = ( TCB_t * ) pvPortMalloc( sizeof( TCB_t ) ); + + if( pxNewTCB != NULL ) { - usStackRemaining = usTaskCheckFreeStackSpace( ( unsigned char * ) pxNextTCB->pxEndOfStack ); + /* Store the stack location in the TCB. */ + pxNewTCB->pxStack = pxStack; } - #else + else { - usStackRemaining = usTaskCheckFreeStackSpace( ( unsigned char * ) pxNextTCB->pxStack ); + /* The stack cannot be used as the TCB was not created. Free it + again. */ + vPortFree( pxStack ); } - #endif - - sprintf( pcStatusString, ( char * ) "%s\t\t%c\t%u\t%u\t%u\r\n", pxNextTCB->pcTaskName, cStatus, ( unsigned int ) pxNextTCB->uxPriority, ( unsigned int ) usStackRemaining, ( unsigned int ) pxNextTCB->uxTCBNumber ); - strcat( ( char * ) pcWriteBuffer, ( char * ) pcStatusString ); + } + else + { + pxNewTCB = NULL; + } + } + #endif /* portSTACK_GROWTH */ - } while( pxNextTCB != pxFirstTCB ); + if( pxNewTCB != NULL ) + { + /* Avoid dependency on memset() if it is not required. */ + #if( ( configCHECK_FOR_STACK_OVERFLOW > 1 ) || ( configUSE_TRACE_FACILITY == 1 ) || ( INCLUDE_uxTaskGetStackHighWaterMark == 1 ) ) + { + /* Just to help debugging. */ + ( void ) memset( pxNewTCB->pxStack, ( int ) tskSTACK_FILL_BYTE, ( size_t ) usStackDepth * sizeof( StackType_t ) ); + } + #endif /* ( ( configCHECK_FOR_STACK_OVERFLOW > 1 ) || ( ( configUSE_TRACE_FACILITY == 1 ) || ( INCLUDE_uxTaskGetStackHighWaterMark == 1 ) ) ) */ } -#endif /* configUSE_TRACE_FACILITY */ + return pxNewTCB; +} /*-----------------------------------------------------------*/ -#if ( configGENERATE_RUN_TIME_STATS == 1 ) +#if ( configUSE_TRACE_FACILITY == 1 ) - static void prvGenerateRunTimeStatsForTasksInList( const signed char *pcWriteBuffer, xList *pxList, unsigned long ulTotalRunTimeDiv100 ) + static UBaseType_t prvListTaskWithinSingleList( TaskStatus_t *pxTaskStatusArray, List_t *pxList, eTaskState eState ) { - volatile tskTCB *pxNextTCB, *pxFirstTCB; - unsigned long ulStatsAsPercentage; + volatile TCB_t *pxNextTCB, *pxFirstTCB; + UBaseType_t uxTask = 0; - /* Write the run time stats of all the TCB's in pxList into the buffer. */ - listGET_OWNER_OF_NEXT_ENTRY( pxFirstTCB, pxList ); - do + if( listCURRENT_LIST_LENGTH( pxList ) > ( UBaseType_t ) 0 ) { - /* Get next TCB in from the list. */ - listGET_OWNER_OF_NEXT_ENTRY( pxNextTCB, pxList ); + listGET_OWNER_OF_NEXT_ENTRY( pxFirstTCB, pxList ); - /* Divide by zero check. */ - if( ulTotalRunTimeDiv100 > 0UL ) + /* Populate an TaskStatus_t structure within the + pxTaskStatusArray array for each task that is referenced from + pxList. See the definition of TaskStatus_t in task.h for the + meaning of each TaskStatus_t structure member. */ + do { - /* Has the task run at all? */ - if( pxNextTCB->ulRunTimeCounter == 0UL ) - { - /* 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 has the task used? - This will always be rounded down to the nearest integer. - ulTotalRunTimeDiv100 has already been divided by 100. */ - ulStatsAsPercentage = pxNextTCB->ulRunTimeCounter / ulTotalRunTimeDiv100; + listGET_OWNER_OF_NEXT_ENTRY( pxNextTCB, pxList ); - if( ulStatsAsPercentage > 0UL ) - { - #ifdef portLU_PRINTF_SPECIFIER_REQUIRED - { - sprintf( pcStatsString, ( char * ) "%s\t\t%lu\t\t%lu%%\r\n", pxNextTCB->pcTaskName, pxNextTCB->ulRunTimeCounter, ulStatsAsPercentage ); - } - #else - { - /* sizeof( int ) == sizeof( long ) so a smaller - printf() library can be used. */ - sprintf( pcStatsString, ( char * ) "%s\t\t%u\t\t%u%%\r\n", pxNextTCB->pcTaskName, ( unsigned int ) pxNextTCB->ulRunTimeCounter, ( unsigned int ) ulStatsAsPercentage ); - } - #endif - } - else + pxTaskStatusArray[ uxTask ].xHandle = ( TaskHandle_t ) pxNextTCB; + pxTaskStatusArray[ uxTask ].pcTaskName = ( const char * ) &( pxNextTCB->pcTaskName [ 0 ] ); + pxTaskStatusArray[ uxTask ].xTaskNumber = pxNextTCB->uxTCBNumber; + pxTaskStatusArray[ uxTask ].eCurrentState = eState; + pxTaskStatusArray[ uxTask ].uxCurrentPriority = pxNextTCB->uxPriority; + + #if ( INCLUDE_vTaskSuspend == 1 ) + { + /* If the task is in the suspended list then there is a chance + it is actually just blocked indefinitely - so really it should + be reported as being in the Blocked state. */ + if( eState == eSuspended ) { - /* If the percentage is zero here then the task has - consumed less than 1% of the total run time. */ - #ifdef portLU_PRINTF_SPECIFIER_REQUIRED + if( listLIST_ITEM_CONTAINER( &( pxNextTCB->xEventListItem ) ) != NULL ) { - sprintf( pcStatsString, ( char * ) "%s\t\t%lu\t\t<1%%\r\n", pxNextTCB->pcTaskName, pxNextTCB->ulRunTimeCounter ); - } - #else - { - /* sizeof( int ) == sizeof( long ) so a smaller - printf() library can be used. */ - sprintf( pcStatsString, ( char * ) "%s\t\t%u\t\t<1%%\r\n", pxNextTCB->pcTaskName, ( unsigned int ) pxNextTCB->ulRunTimeCounter ); + pxTaskStatusArray[ uxTask ].eCurrentState = eBlocked; } - #endif } } + #endif /* INCLUDE_vTaskSuspend */ - strcat( ( char * ) pcWriteBuffer, ( char * ) pcStatsString ); - } + #if ( configUSE_MUTEXES == 1 ) + { + pxTaskStatusArray[ uxTask ].uxBasePriority = pxNextTCB->uxBasePriority; + } + #else + { + pxTaskStatusArray[ uxTask ].uxBasePriority = 0; + } + #endif + + #if ( configGENERATE_RUN_TIME_STATS == 1 ) + { + pxTaskStatusArray[ uxTask ].ulRunTimeCounter = pxNextTCB->ulRunTimeCounter; + } + #else + { + pxTaskStatusArray[ uxTask ].ulRunTimeCounter = 0; + } + #endif + + #if ( portSTACK_GROWTH > 0 ) + { + pxTaskStatusArray[ uxTask ].usStackHighWaterMark = prvTaskCheckFreeStackSpace( ( uint8_t * ) pxNextTCB->pxEndOfStack ); + } + #else + { + pxTaskStatusArray[ uxTask ].usStackHighWaterMark = prvTaskCheckFreeStackSpace( ( uint8_t * ) pxNextTCB->pxStack ); + } + #endif - } while( pxNextTCB != pxFirstTCB ); + uxTask++; + + } while( pxNextTCB != pxFirstTCB ); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + return uxTask; } -#endif /* configGENERATE_RUN_TIME_STATS */ +#endif /* configUSE_TRACE_FACILITY */ /*-----------------------------------------------------------*/ #if ( ( configUSE_TRACE_FACILITY == 1 ) || ( INCLUDE_uxTaskGetStackHighWaterMark == 1 ) ) - static unsigned short usTaskCheckFreeStackSpace( const unsigned char * pucStackByte ) + static uint16_t prvTaskCheckFreeStackSpace( const uint8_t * pucStackByte ) { - register unsigned short usCount = 0U; + uint32_t ulCount = 0U; - while( *pucStackByte == tskSTACK_FILL_BYTE ) + while( *pucStackByte == ( uint8_t ) tskSTACK_FILL_BYTE ) { pucStackByte -= portSTACK_GROWTH; - usCount++; + ulCount++; } - usCount /= sizeof( portSTACK_TYPE ); + ulCount /= ( uint32_t ) sizeof( StackType_t ); /*lint !e961 Casting is not redundant on smaller architectures. */ - return usCount; + return ( uint16_t ) ulCount; } #endif /* ( ( configUSE_TRACE_FACILITY == 1 ) || ( INCLUDE_uxTaskGetStackHighWaterMark == 1 ) ) */ @@ -2616,25 +3280,25 @@ tskTCB *pxNewTCB; #if ( INCLUDE_uxTaskGetStackHighWaterMark == 1 ) - unsigned portBASE_TYPE uxTaskGetStackHighWaterMark( xTaskHandle xTask ) + UBaseType_t uxTaskGetStackHighWaterMark( TaskHandle_t xTask ) { - tskTCB *pxTCB; - unsigned char *pcEndOfStack; - unsigned portBASE_TYPE uxReturn; + TCB_t *pxTCB; + uint8_t *pucEndOfStack; + UBaseType_t uxReturn; pxTCB = prvGetTCBFromHandle( xTask ); #if portSTACK_GROWTH < 0 { - pcEndOfStack = ( unsigned char * ) pxTCB->pxStack; + pucEndOfStack = ( uint8_t * ) pxTCB->pxStack; } #else { - pcEndOfStack = ( unsigned char * ) pxTCB->pxEndOfStack; + pucEndOfStack = ( uint8_t * ) pxTCB->pxEndOfStack; } #endif - uxReturn = ( unsigned portBASE_TYPE ) usTaskCheckFreeStackSpace( pcEndOfStack ); + uxReturn = ( UBaseType_t ) prvTaskCheckFreeStackSpace( pucEndOfStack ); return uxReturn; } @@ -2644,27 +3308,71 @@ tskTCB *pxNewTCB; #if ( INCLUDE_vTaskDelete == 1 ) - static void prvDeleteTCB( tskTCB *pxTCB ) + static void prvDeleteTCB( TCB_t *pxTCB ) { /* This call is required specifically for the TriCore port. It must be above the vPortFree() calls. The call is also used by ports/demos that want to allocate and clean RAM statically. */ portCLEAN_UP_TCB( 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 ); + /* 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. */ + #if ( configUSE_NEWLIB_REENTRANT == 1 ) + { + _reclaim_reent( &( pxTCB->xNewLib_reent ) ); + } + #endif /* configUSE_NEWLIB_REENTRANT */ + + #if( portUSING_MPU_WRAPPERS == 1 ) + { + /* Only free the stack if it was allocated dynamically in the first + place. */ + if( pxTCB->xUsingStaticallyAllocatedStack == pdFALSE ) + { + vPortFreeAligned( pxTCB->pxStack ); + } + } + #else + { + vPortFreeAligned( pxTCB->pxStack ); + } + #endif + vPortFree( pxTCB ); } #endif /* INCLUDE_vTaskDelete */ /*-----------------------------------------------------------*/ +static void prvResetNextTaskUnblockTime( void ) +{ +TCB_t *pxTCB; + + if( listLIST_IS_EMPTY( pxDelayedTaskList ) != pdFALSE ) + { + /* The new current delayed list is empty. Set xNextTaskUnblockTime to + the maximum possible value so it is extremely unlikely that the + if( xTickCount >= xNextTaskUnblockTime ) test will pass until + there is an item in the delayed list. */ + xNextTaskUnblockTime = portMAX_DELAY; + } + else + { + /* The new current delayed list is not empty, get the value of + the item at the head of the delayed list. This is the time at + which the task at the head of the delayed list should be removed + from the Blocked state. */ + ( pxTCB ) = ( TCB_t * ) listGET_OWNER_OF_HEAD_ENTRY( pxDelayedTaskList ); + xNextTaskUnblockTime = listGET_LIST_ITEM_VALUE( &( ( pxTCB )->xGenericListItem ) ); + } +} +/*-----------------------------------------------------------*/ + #if ( ( INCLUDE_xTaskGetCurrentTaskHandle == 1 ) || ( configUSE_MUTEXES == 1 ) ) - xTaskHandle xTaskGetCurrentTaskHandle( void ) + TaskHandle_t xTaskGetCurrentTaskHandle( void ) { - xTaskHandle xReturn; + TaskHandle_t 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 @@ -2679,9 +3387,9 @@ tskTCB *pxNewTCB; #if ( ( INCLUDE_xTaskGetSchedulerState == 1 ) || ( configUSE_TIMERS == 1 ) ) - portBASE_TYPE xTaskGetSchedulerState( void ) + BaseType_t xTaskGetSchedulerState( void ) { - portBASE_TYPE xReturn; + BaseType_t xReturn; if( xSchedulerRunning == pdFALSE ) { @@ -2689,7 +3397,7 @@ tskTCB *pxNewTCB; } else { - if( uxSchedulerSuspended == ( unsigned portBASE_TYPE ) pdFALSE ) + if( uxSchedulerSuspended == ( UBaseType_t ) pdFALSE ) { xReturn = taskSCHEDULER_RUNNING; } @@ -2707,31 +3415,47 @@ tskTCB *pxNewTCB; #if ( configUSE_MUTEXES == 1 ) - void vTaskPriorityInherit( xTaskHandle * const pxMutexHolder ) + void vTaskPriorityInherit( TaskHandle_t const pxMutexHolder ) { - tskTCB * const pxTCB = ( tskTCB * ) pxMutexHolder; + TCB_t * const pxTCB = ( TCB_t * ) pxMutexHolder; /* If the mutex was given back by an interrupt while the queue was locked then the mutex holder might now be NULL. */ if( pxMutexHolder != NULL ) { + /* If the holder of the mutex has a priority below the priority of + the task attempting to obtain the mutex then it will temporarily + inherit the priority of the task attempting to obtain the mutex. */ 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 ); + /* Adjust the mutex holder state to account for its new + priority. Only reset the event list item value if the value is + not being used for anything else. */ + if( ( listGET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ) ) & taskEVENT_LIST_ITEM_VALUE_IN_USE ) == 0UL ) + { + listSET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ), ( TickType_t ) configMAX_PRIORITIES - ( TickType_t ) pxCurrentTCB->uxPriority ); /*lint !e961 MISRA exception as the casts are only redundant for some ports. */ + } + else + { + mtCOVERAGE_TEST_MARKER(); + } - /* If the task being modified is in the ready state it will need to - be moved into a new list. */ + /* If the task being modified is in the ready state it will need + to be moved into a new list. */ if( listIS_CONTAINED_WITHIN( &( pxReadyTasksLists[ pxTCB->uxPriority ] ), &( pxTCB->xGenericListItem ) ) != pdFALSE ) { - if( uxListRemove( ( xListItem * ) &( pxTCB->xGenericListItem ) ) == 0 ) + if( uxListRemove( &( pxTCB->xGenericListItem ) ) == ( UBaseType_t ) 0 ) { taskRESET_READY_PRIORITY( pxTCB->uxPriority ); } + else + { + mtCOVERAGE_TEST_MARKER(); + } /* Inherit the priority before being moved into the new list. */ pxTCB->uxPriority = pxCurrentTCB->uxPriority; - prvAddTaskToReadyQueue( pxTCB ); + prvAddTaskToReadyList( pxTCB ); } else { @@ -2741,6 +3465,14 @@ tskTCB *pxNewTCB; traceTASK_PRIORITY_INHERIT( pxTCB, pxCurrentTCB->uxPriority ); } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } } @@ -2749,29 +3481,80 @@ tskTCB *pxNewTCB; #if ( configUSE_MUTEXES == 1 ) - void vTaskPriorityDisinherit( xTaskHandle * const pxMutexHolder ) + BaseType_t xTaskPriorityDisinherit( TaskHandle_t const pxMutexHolder ) { - tskTCB * const pxTCB = ( tskTCB * ) pxMutexHolder; + TCB_t * const pxTCB = ( TCB_t * ) pxMutexHolder; + BaseType_t xReturn = pdFALSE; if( pxMutexHolder != NULL ) { + /* A task can only have an inherited priority if it holds the mutex. + If the mutex is held by a task then it cannot be given from an + interrupt, and if a mutex is given by the holding task then it must + be the running state task. */ + configASSERT( pxTCB == pxCurrentTCB ); + + configASSERT( pxTCB->uxMutexesHeld ); + ( pxTCB->uxMutexesHeld )--; + + /* Has the holder of the mutex inherited the priority of another + task? */ 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. */ - if( uxListRemove( ( xListItem * ) &( pxTCB->xGenericListItem ) ) == 0 ) + /* Only disinherit if no other mutexes are held. */ + if( pxTCB->uxMutexesHeld == ( UBaseType_t ) 0 ) { - taskRESET_READY_PRIORITY( pxTCB->uxPriority ); - } + /* A task can only have an inherited priority if it holds + the mutex. If the mutex is held by a task then it cannot be + given from an interrupt, and if a mutex is given by the + holding task then it must be the running state task. Remove + the holding task from the ready list. */ + if( uxListRemove( &( pxTCB->xGenericListItem ) ) == ( UBaseType_t ) 0 ) + { + taskRESET_READY_PRIORITY( pxTCB->uxPriority ); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } - /* Disinherit the priority before adding the task into the new - ready list. */ - traceTASK_PRIORITY_DISINHERIT( pxTCB, pxTCB->uxBasePriority ); - pxTCB->uxPriority = pxTCB->uxBasePriority; - listSET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ), configMAX_PRIORITIES - ( portTickType ) pxTCB->uxPriority ); - prvAddTaskToReadyQueue( pxTCB ); + /* Disinherit the priority before adding the task into the + new ready list. */ + traceTASK_PRIORITY_DISINHERIT( pxTCB, pxTCB->uxBasePriority ); + pxTCB->uxPriority = pxTCB->uxBasePriority; + + /* Reset the event list item value. It cannot be in use for + any other purpose if this task is running, and it must be + running to give back the mutex. */ + listSET_LIST_ITEM_VALUE( &( pxTCB->xEventListItem ), ( TickType_t ) configMAX_PRIORITIES - ( TickType_t ) pxTCB->uxPriority ); /*lint !e961 MISRA exception as the casts are only redundant for some ports. */ + prvAddTaskToReadyList( pxTCB ); + + /* Return true to indicate that a context switch is required. + This is only actually required in the corner case whereby + multiple mutexes were held and the mutexes were given back + in an order different to that in which they were taken. + If a context switch did not occur when the first mutex was + returned, even if a task was waiting on it, then a context + switch should occur when the last mutex is returned whether + a task is waiting on it or not. */ + xReturn = pdTRUE; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + return xReturn; } #endif /* configUSE_MUTEXES */ @@ -2786,6 +3569,21 @@ tskTCB *pxNewTCB; if( xSchedulerRunning != pdFALSE ) { ( pxCurrentTCB->uxCriticalNesting )++; + + /* This is not the interrupt safe version of the enter critical + function so assert() if it is being called from an interrupt + context. Only API functions that end in "FromISR" can be used in an + interrupt. Only assert if the critical nesting count is 1 to + protect against recursive calls if the assert function also uses a + critical section. */ + if( pxCurrentTCB->uxCriticalNesting == 1 ) + { + portASSERT_IF_IN_ISR(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } } @@ -2806,13 +3604,874 @@ tskTCB *pxNewTCB; { portENABLE_INTERRUPTS(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } } + else + { + mtCOVERAGE_TEST_MARKER(); + } } #endif /* portCRITICAL_NESTING_IN_TCB */ /*-----------------------------------------------------------*/ +#if ( ( configUSE_TRACE_FACILITY == 1 ) && ( configUSE_STATS_FORMATTING_FUNCTIONS > 0 ) ) + + static char *prvWriteNameToBuffer( char *pcBuffer, const char *pcTaskName ) + { + size_t x; + + /* Start by copying the entire string. */ + strcpy( pcBuffer, pcTaskName ); + + /* Pad the end of the string with spaces to ensure columns line up when + printed out. */ + for( x = strlen( pcBuffer ); x < ( size_t ) ( configMAX_TASK_NAME_LEN - 1 ); x++ ) + { + pcBuffer[ x ] = ' '; + } + + /* Terminate. */ + pcBuffer[ x ] = 0x00; + + /* Return the new end of string. */ + return &( pcBuffer[ x ] ); + } + +#endif /* ( configUSE_TRACE_FACILITY == 1 ) && ( configUSE_STATS_FORMATTING_FUNCTIONS > 0 ) */ +/*-----------------------------------------------------------*/ + +#if ( ( configUSE_TRACE_FACILITY == 1 ) && ( configUSE_STATS_FORMATTING_FUNCTIONS > 0 ) ) + void vTaskList( char * pcWriteBuffer ) + { + TaskStatus_t *pxTaskStatusArray; + volatile UBaseType_t uxArraySize, x; + char cStatus; + + /* + * PLEASE NOTE: + * + * This function is provided for convenience only, and is used by many + * of the demo applications. Do not consider it to be part of the + * scheduler. + * + * vTaskList() calls uxTaskGetSystemState(), then formats part of the + * uxTaskGetSystemState() output into a human readable table that + * displays task names, states and stack usage. + * + * vTaskList() has a dependency on the sprintf() C library function that + * might bloat the code size, use a lot of stack, and provide different + * results on different platforms. An alternative, tiny, third party, + * and limited functionality implementation of sprintf() is provided in + * many of the FreeRTOS/Demo sub-directories in a file called + * printf-stdarg.c (note printf-stdarg.c does not provide a full + * snprintf() implementation!). + * + * It is recommended that production systems call uxTaskGetSystemState() + * directly to get access to raw stats data, rather than indirectly + * through a call to vTaskList(). + */ + + + /* Make sure the write buffer does not contain a string. */ + *pcWriteBuffer = 0x00; + + /* Take a snapshot of the number of tasks in case it changes while this + function is executing. */ + uxArraySize = uxCurrentNumberOfTasks; + + /* Allocate an array index for each task. */ + pxTaskStatusArray = pvPortMalloc( uxCurrentNumberOfTasks * sizeof( TaskStatus_t ) ); + + if( pxTaskStatusArray != NULL ) + { + /* Generate the (binary) data. */ + uxArraySize = uxTaskGetSystemState( pxTaskStatusArray, uxArraySize, NULL ); + /* Create a human readable table from the binary data. */ + for( x = 0; x < uxArraySize; x++ ) + { + switch( pxTaskStatusArray[ x ].eCurrentState ) + { + case eReady: cStatus = tskREADY_CHAR; + break; + + case eBlocked: cStatus = tskBLOCKED_CHAR; + break; + + case eSuspended: cStatus = tskSUSPENDED_CHAR; + break; + + case eDeleted: cStatus = tskDELETED_CHAR; + break; + + default: /* Should not get here, but it is included + to prevent static checking errors. */ + cStatus = 0x00; + break; + } + + /* Write the task name to the string, padding with spaces so it + can be printed in tabular form more easily. */ + pcWriteBuffer = prvWriteNameToBuffer( pcWriteBuffer, pxTaskStatusArray[ x ].pcTaskName ); + + /* Write the rest of the string. */ + sprintf( pcWriteBuffer, "\t%c\t%u\t%u\t%u\r\n", cStatus, ( unsigned int ) pxTaskStatusArray[ x ].uxCurrentPriority, ( unsigned int ) pxTaskStatusArray[ x ].usStackHighWaterMark, ( unsigned int ) pxTaskStatusArray[ x ].xTaskNumber ); + pcWriteBuffer += strlen( pcWriteBuffer ); + } + + /* Free the array again. */ + vPortFree( pxTaskStatusArray ); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + +#endif /* ( ( configUSE_TRACE_FACILITY == 1 ) && ( configUSE_STATS_FORMATTING_FUNCTIONS > 0 ) ) */ +/*----------------------------------------------------------*/ + +#if ( ( configGENERATE_RUN_TIME_STATS == 1 ) && ( configUSE_STATS_FORMATTING_FUNCTIONS > 0 ) ) + + void vTaskGetRunTimeStats( char *pcWriteBuffer ) + { + TaskStatus_t *pxTaskStatusArray; + volatile UBaseType_t uxArraySize, x; + uint32_t ulTotalTime, ulStatsAsPercentage; + + #if( configUSE_TRACE_FACILITY != 1 ) + { + #error configUSE_TRACE_FACILITY must also be set to 1 in FreeRTOSConfig.h to use vTaskGetRunTimeStats(). + } + #endif + + /* + * PLEASE NOTE: + * + * This function is provided for convenience only, and is used by many + * of the demo applications. Do not consider it to be part of the + * scheduler. + * + * vTaskGetRunTimeStats() calls uxTaskGetSystemState(), then formats part + * of the uxTaskGetSystemState() output into a human readable table that + * displays the amount of time each task has spent in the Running state + * in both absolute and percentage terms. + * + * vTaskGetRunTimeStats() has a dependency on the sprintf() C library + * function that might bloat the code size, use a lot of stack, and + * provide different results on different platforms. An alternative, + * tiny, third party, and limited functionality implementation of + * sprintf() is provided in many of the FreeRTOS/Demo sub-directories in + * a file called printf-stdarg.c (note printf-stdarg.c does not provide + * a full snprintf() implementation!). + * + * It is recommended that production systems call uxTaskGetSystemState() + * directly to get access to raw stats data, rather than indirectly + * through a call to vTaskGetRunTimeStats(). + */ + + /* Make sure the write buffer does not contain a string. */ + *pcWriteBuffer = 0x00; + + /* Take a snapshot of the number of tasks in case it changes while this + function is executing. */ + uxArraySize = uxCurrentNumberOfTasks; + + /* Allocate an array index for each task. */ + pxTaskStatusArray = pvPortMalloc( uxCurrentNumberOfTasks * sizeof( TaskStatus_t ) ); + + if( pxTaskStatusArray != NULL ) + { + /* Generate the (binary) data. */ + uxArraySize = uxTaskGetSystemState( pxTaskStatusArray, uxArraySize, &ulTotalTime ); + + /* For percentage calculations. */ + ulTotalTime /= 100UL; + + /* Avoid divide by zero errors. */ + if( ulTotalTime > 0 ) + { + /* Create a human readable table from the binary data. */ + for( x = 0; x < uxArraySize; x++ ) + { + /* What percentage of the total run time has the task used? + This will always be rounded down to the nearest integer. + ulTotalRunTimeDiv100 has already been divided by 100. */ + ulStatsAsPercentage = pxTaskStatusArray[ x ].ulRunTimeCounter / ulTotalTime; + + /* Write the task name to the string, padding with + spaces so it can be printed in tabular form more + easily. */ + pcWriteBuffer = prvWriteNameToBuffer( pcWriteBuffer, pxTaskStatusArray[ x ].pcTaskName ); + + if( ulStatsAsPercentage > 0UL ) + { + #ifdef portLU_PRINTF_SPECIFIER_REQUIRED + { + sprintf( pcWriteBuffer, "\t%lu\t\t%lu%%\r\n", pxTaskStatusArray[ x ].ulRunTimeCounter, ulStatsAsPercentage ); + } + #else + { + /* sizeof( int ) == sizeof( long ) so a smaller + printf() library can be used. */ + sprintf( pcWriteBuffer, "\t%u\t\t%u%%\r\n", ( unsigned int ) pxTaskStatusArray[ x ].ulRunTimeCounter, ( unsigned int ) ulStatsAsPercentage ); + } + #endif + } + else + { + /* If the percentage is zero here then the task has + consumed less than 1% of the total run time. */ + #ifdef portLU_PRINTF_SPECIFIER_REQUIRED + { + sprintf( pcWriteBuffer, "\t%lu\t\t<1%%\r\n", pxTaskStatusArray[ x ].ulRunTimeCounter ); + } + #else + { + /* sizeof( int ) == sizeof( long ) so a smaller + printf() library can be used. */ + sprintf( pcWriteBuffer, "\t%u\t\t<1%%\r\n", ( unsigned int ) pxTaskStatusArray[ x ].ulRunTimeCounter ); + } + #endif + } + + pcWriteBuffer += strlen( pcWriteBuffer ); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + /* Free the array again. */ + vPortFree( pxTaskStatusArray ); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + +#endif /* ( ( configGENERATE_RUN_TIME_STATS == 1 ) && ( configUSE_STATS_FORMATTING_FUNCTIONS > 0 ) ) */ +/*-----------------------------------------------------------*/ + +TickType_t uxTaskResetEventItemValue( void ) +{ +TickType_t uxReturn; + + uxReturn = listGET_LIST_ITEM_VALUE( &( pxCurrentTCB->xEventListItem ) ); + + /* Reset the event list item to its normal value - so it can be used with + queues and semaphores. */ + listSET_LIST_ITEM_VALUE( &( pxCurrentTCB->xEventListItem ), ( ( TickType_t ) configMAX_PRIORITIES - ( TickType_t ) pxCurrentTCB->uxPriority ) ); /*lint !e961 MISRA exception as the casts are only redundant for some ports. */ + + return uxReturn; +} +/*-----------------------------------------------------------*/ + +#if ( configUSE_MUTEXES == 1 ) + + void *pvTaskIncrementMutexHeldCount( void ) + { + /* If xSemaphoreCreateMutex() is called before any tasks have been created + then pxCurrentTCB will be NULL. */ + if( pxCurrentTCB != NULL ) + { + ( pxCurrentTCB->uxMutexesHeld )++; + } + + return pxCurrentTCB; + } + +#endif /* configUSE_MUTEXES */ +/*-----------------------------------------------------------*/ + +#if( configUSE_TASK_NOTIFICATIONS == 1 ) + + uint32_t ulTaskNotifyTake( BaseType_t xClearCountOnExit, TickType_t xTicksToWait ) + { + TickType_t xTimeToWake; + uint32_t ulReturn; + + taskENTER_CRITICAL(); + { + /* Only block if the notification count is not already non-zero. */ + if( pxCurrentTCB->ulNotifiedValue == 0UL ) + { + /* Mark this task as waiting for a notification. */ + pxCurrentTCB->eNotifyState = eWaitingNotification; + + if( xTicksToWait > ( TickType_t ) 0 ) + { + /* The task is going to block. First it must be removed + from the ready list. */ + if( uxListRemove( &( pxCurrentTCB->xGenericListItem ) ) == ( UBaseType_t ) 0 ) + { + /* The current task must be in a ready list, so there is + no need to check, and the port reset macro can be called + directly. */ + portRESET_READY_PRIORITY( pxCurrentTCB->uxPriority, uxTopReadyPriority ); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + #if ( INCLUDE_vTaskSuspend == 1 ) + { + if( xTicksToWait == portMAX_DELAY ) + { + /* Add the task to the suspended task list instead + of a delayed task list to ensure the task is not + woken by a timing event. It will block + indefinitely. */ + vListInsertEnd( &xSuspendedTaskList, &( pxCurrentTCB->xGenericListItem ) ); + } + else + { + /* Calculate the time at which the task should be + woken if no notification events occur. This may + overflow but this doesn't matter, the scheduler will + handle it. */ + xTimeToWake = xTickCount + xTicksToWait; + prvAddCurrentTaskToDelayedList( xTimeToWake ); + } + } + #else /* INCLUDE_vTaskSuspend */ + { + /* Calculate the time at which the task should be + woken if the event does not occur. This may + overflow but this doesn't matter, the scheduler will + handle it. */ + xTimeToWake = xTickCount + xTicksToWait; + prvAddCurrentTaskToDelayedList( xTimeToWake ); + } + #endif /* INCLUDE_vTaskSuspend */ + + traceTASK_NOTIFY_TAKE_BLOCK(); + + /* All ports are written to allow a yield in a critical + section (some will yield immediately, others wait until the + critical section exits) - but it is not something that + application code should ever do. */ + portYIELD_WITHIN_API(); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + taskEXIT_CRITICAL(); + + taskENTER_CRITICAL(); + { + traceTASK_NOTIFY_TAKE(); + ulReturn = pxCurrentTCB->ulNotifiedValue; + + if( ulReturn != 0UL ) + { + if( xClearCountOnExit != pdFALSE ) + { + pxCurrentTCB->ulNotifiedValue = 0UL; + } + else + { + ( pxCurrentTCB->ulNotifiedValue )--; + } + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + pxCurrentTCB->eNotifyState = eNotWaitingNotification; + } + taskEXIT_CRITICAL(); + + return ulReturn; + } + +#endif /* configUSE_TASK_NOTIFICATIONS */ +/*-----------------------------------------------------------*/ + +#if( configUSE_TASK_NOTIFICATIONS == 1 ) + + BaseType_t xTaskNotifyWait( uint32_t ulBitsToClearOnEntry, uint32_t ulBitsToClearOnExit, uint32_t *pulNotificationValue, TickType_t xTicksToWait ) + { + TickType_t xTimeToWake; + BaseType_t xReturn; + + taskENTER_CRITICAL(); + { + /* Only block if a notification is not already pending. */ + if( pxCurrentTCB->eNotifyState != eNotified ) + { + /* Clear bits in the task's notification value as bits may get + set by the notifying task or interrupt. This can be used to + clear the value to zero. */ + pxCurrentTCB->ulNotifiedValue &= ~ulBitsToClearOnEntry; + + /* Mark this task as waiting for a notification. */ + pxCurrentTCB->eNotifyState = eWaitingNotification; + + if( xTicksToWait > ( TickType_t ) 0 ) + { + /* The task is going to block. First it must be removed + from the ready list. */ + if( uxListRemove( &( pxCurrentTCB->xGenericListItem ) ) == ( UBaseType_t ) 0 ) + { + /* The current task must be in a ready list, so there is + no need to check, and the port reset macro can be called + directly. */ + portRESET_READY_PRIORITY( pxCurrentTCB->uxPriority, uxTopReadyPriority ); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + #if ( INCLUDE_vTaskSuspend == 1 ) + { + if( xTicksToWait == portMAX_DELAY ) + { + /* Add the task to the suspended task list instead + of a delayed task list to ensure the task is not + woken by a timing event. It will block + indefinitely. */ + vListInsertEnd( &xSuspendedTaskList, &( pxCurrentTCB->xGenericListItem ) ); + } + else + { + /* Calculate the time at which the task should be + woken if no notification events occur. This may + overflow but this doesn't matter, the scheduler will + handle it. */ + xTimeToWake = xTickCount + xTicksToWait; + prvAddCurrentTaskToDelayedList( xTimeToWake ); + } + } + #else /* INCLUDE_vTaskSuspend */ + { + /* Calculate the time at which the task should be + woken if the event does not occur. This may + overflow but this doesn't matter, the scheduler will + handle it. */ + xTimeToWake = xTickCount + xTicksToWait; + prvAddCurrentTaskToDelayedList( xTimeToWake ); + } + #endif /* INCLUDE_vTaskSuspend */ + + traceTASK_NOTIFY_WAIT_BLOCK(); + + /* All ports are written to allow a yield in a critical + section (some will yield immediately, others wait until the + critical section exits) - but it is not something that + application code should ever do. */ + portYIELD_WITHIN_API(); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + taskEXIT_CRITICAL(); + + taskENTER_CRITICAL(); + { + traceTASK_NOTIFY_WAIT(); + + if( pulNotificationValue != NULL ) + { + /* Output the current notification value, which may or may not + have changed. */ + *pulNotificationValue = pxCurrentTCB->ulNotifiedValue; + } + + /* If eNotifyValue is set then either the task never entered the + blocked state (because a notification was already pending) or the + task unblocked because of a notification. Otherwise the task + unblocked because of a timeout. */ + if( pxCurrentTCB->eNotifyState == eWaitingNotification ) + { + /* A notification was not received. */ + xReturn = pdFALSE; + } + else + { + /* A notification was already pending or a notification was + received while the task was waiting. */ + pxCurrentTCB->ulNotifiedValue &= ~ulBitsToClearOnExit; + xReturn = pdTRUE; + } + + pxCurrentTCB->eNotifyState = eNotWaitingNotification; + } + taskEXIT_CRITICAL(); + + return xReturn; + } + +#endif /* configUSE_TASK_NOTIFICATIONS */ +/*-----------------------------------------------------------*/ + +#if( configUSE_TASK_NOTIFICATIONS == 1 ) + + BaseType_t xTaskGenericNotify( TaskHandle_t xTaskToNotify, uint32_t ulValue, eNotifyAction eAction, uint32_t *pulPreviousNotificationValue ) + { + TCB_t * pxTCB; + eNotifyValue eOriginalNotifyState; + BaseType_t xReturn = pdPASS; + + configASSERT( xTaskToNotify ); + pxTCB = ( TCB_t * ) xTaskToNotify; + + taskENTER_CRITICAL(); + { + if( pulPreviousNotificationValue != NULL ) + { + *pulPreviousNotificationValue = pxTCB->ulNotifiedValue; + } + + eOriginalNotifyState = pxTCB->eNotifyState; + + pxTCB->eNotifyState = eNotified; + + switch( eAction ) + { + case eSetBits : + pxTCB->ulNotifiedValue |= ulValue; + break; + + case eIncrement : + ( pxTCB->ulNotifiedValue )++; + break; + + case eSetValueWithOverwrite : + pxTCB->ulNotifiedValue = ulValue; + break; + + case eSetValueWithoutOverwrite : + if( eOriginalNotifyState != eNotified ) + { + pxTCB->ulNotifiedValue = ulValue; + } + else + { + /* The value could not be written to the task. */ + xReturn = pdFAIL; + } + break; + + case eNoAction: + /* The task is being notified without its notify value being + updated. */ + break; + } + + traceTASK_NOTIFY(); + + /* If the task is in the blocked state specifically to wait for a + notification then unblock it now. */ + if( eOriginalNotifyState == eWaitingNotification ) + { + ( void ) uxListRemove( &( pxTCB->xGenericListItem ) ); + prvAddTaskToReadyList( pxTCB ); + + /* The task should not have been on an event list. */ + configASSERT( listLIST_ITEM_CONTAINER( &( pxTCB->xEventListItem ) ) == NULL ); + + #if( configUSE_TICKLESS_IDLE != 0 ) + { + /* If a task is blocked waiting for a notification then + xNextTaskUnblockTime might be set to the blocked task's time + out time. If the task is unblocked for a reason other than + a timeout xNextTaskUnblockTime is normally left unchanged, + because it will automatically get reset to a new value when + the tick count equals xNextTaskUnblockTime. However if + tickless idling is used it might be more important to enter + sleep mode at the earliest possible time - so reset + xNextTaskUnblockTime here to ensure it is updated at the + earliest possible time. */ + prvResetNextTaskUnblockTime(); + } + #endif + + if( pxTCB->uxPriority > pxCurrentTCB->uxPriority ) + { + /* The notified task has a priority above the currently + executing task so a yield is required. */ + taskYIELD_IF_USING_PREEMPTION(); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + taskEXIT_CRITICAL(); + + return xReturn; + } + +#endif /* configUSE_TASK_NOTIFICATIONS */ +/*-----------------------------------------------------------*/ + +#if( configUSE_TASK_NOTIFICATIONS == 1 ) + + BaseType_t xTaskGenericNotifyFromISR( TaskHandle_t xTaskToNotify, uint32_t ulValue, eNotifyAction eAction, uint32_t *pulPreviousNotificationValue, BaseType_t *pxHigherPriorityTaskWoken ) + { + TCB_t * pxTCB; + eNotifyValue eOriginalNotifyState; + BaseType_t xReturn = pdPASS; + UBaseType_t uxSavedInterruptStatus; + + configASSERT( xTaskToNotify ); + + /* RTOS ports that support interrupt nesting have the concept of a + maximum system call (or maximum API call) interrupt priority. + Interrupts that are above the maximum system call priority are keep + permanently enabled, even when the RTOS kernel is in a critical section, + but cannot make any calls to FreeRTOS API functions. If configASSERT() + is defined in FreeRTOSConfig.h then + portASSERT_IF_INTERRUPT_PRIORITY_INVALID() will result in an assertion + failure if a FreeRTOS API function is called from an interrupt that has + been assigned a priority above the configured maximum system call + priority. Only FreeRTOS functions that end in FromISR can be called + from interrupts that have been assigned a priority at or (logically) + below the maximum system call interrupt priority. FreeRTOS maintains a + separate interrupt safe API to ensure interrupt entry is as fast and as + simple as possible. More information (albeit Cortex-M specific) is + provided on the following link: + http://www.freertos.org/RTOS-Cortex-M3-M4.html */ + portASSERT_IF_INTERRUPT_PRIORITY_INVALID(); + + pxTCB = ( TCB_t * ) xTaskToNotify; + + uxSavedInterruptStatus = portSET_INTERRUPT_MASK_FROM_ISR(); + { + if( pulPreviousNotificationValue != NULL ) + { + *pulPreviousNotificationValue = pxTCB->ulNotifiedValue; + } + + eOriginalNotifyState = pxTCB->eNotifyState; + pxTCB->eNotifyState = eNotified; + + switch( eAction ) + { + case eSetBits : + pxTCB->ulNotifiedValue |= ulValue; + break; + + case eIncrement : + ( pxTCB->ulNotifiedValue )++; + break; + + case eSetValueWithOverwrite : + pxTCB->ulNotifiedValue = ulValue; + break; + + case eSetValueWithoutOverwrite : + if( eOriginalNotifyState != eNotified ) + { + pxTCB->ulNotifiedValue = ulValue; + } + else + { + /* The value could not be written to the task. */ + xReturn = pdFAIL; + } + break; + + case eNoAction : + /* The task is being notified without its notify value being + updated. */ + break; + } + + traceTASK_NOTIFY_FROM_ISR(); + + /* If the task is in the blocked state specifically to wait for a + notification then unblock it now. */ + if( eOriginalNotifyState == eWaitingNotification ) + { + /* The task should not have been on an event list. */ + configASSERT( listLIST_ITEM_CONTAINER( &( pxTCB->xEventListItem ) ) == NULL ); + + if( uxSchedulerSuspended == ( UBaseType_t ) pdFALSE ) + { + ( void ) uxListRemove( &( pxTCB->xGenericListItem ) ); + prvAddTaskToReadyList( pxTCB ); + } + else + { + /* The delayed and ready lists cannot be accessed, so hold + this task pending until the scheduler is resumed. */ + vListInsertEnd( &( xPendingReadyList ), &( pxTCB->xEventListItem ) ); + } + + if( pxTCB->uxPriority > pxCurrentTCB->uxPriority ) + { + /* The notified task has a priority above the currently + executing task so a yield is required. */ + if( pxHigherPriorityTaskWoken != NULL ) + { + *pxHigherPriorityTaskWoken = pdTRUE; + } + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + } + portCLEAR_INTERRUPT_MASK_FROM_ISR( uxSavedInterruptStatus ); + + return xReturn; + } + +#endif /* configUSE_TASK_NOTIFICATIONS */ +/*-----------------------------------------------------------*/ + +#if( configUSE_TASK_NOTIFICATIONS == 1 ) + + void vTaskNotifyGiveFromISR( TaskHandle_t xTaskToNotify, BaseType_t *pxHigherPriorityTaskWoken ) + { + TCB_t * pxTCB; + eNotifyValue eOriginalNotifyState; + UBaseType_t uxSavedInterruptStatus; + + configASSERT( xTaskToNotify ); + + /* RTOS ports that support interrupt nesting have the concept of a + maximum system call (or maximum API call) interrupt priority. + Interrupts that are above the maximum system call priority are keep + permanently enabled, even when the RTOS kernel is in a critical section, + but cannot make any calls to FreeRTOS API functions. If configASSERT() + is defined in FreeRTOSConfig.h then + portASSERT_IF_INTERRUPT_PRIORITY_INVALID() will result in an assertion + failure if a FreeRTOS API function is called from an interrupt that has + been assigned a priority above the configured maximum system call + priority. Only FreeRTOS functions that end in FromISR can be called + from interrupts that have been assigned a priority at or (logically) + below the maximum system call interrupt priority. FreeRTOS maintains a + separate interrupt safe API to ensure interrupt entry is as fast and as + simple as possible. More information (albeit Cortex-M specific) is + provided on the following link: + http://www.freertos.org/RTOS-Cortex-M3-M4.html */ + portASSERT_IF_INTERRUPT_PRIORITY_INVALID(); + + pxTCB = ( TCB_t * ) xTaskToNotify; + + uxSavedInterruptStatus = portSET_INTERRUPT_MASK_FROM_ISR(); + { + eOriginalNotifyState = pxTCB->eNotifyState; + pxTCB->eNotifyState = eNotified; + + /* 'Giving' is equivalent to incrementing a count in a counting + semaphore. */ + ( pxTCB->ulNotifiedValue )++; + + traceTASK_NOTIFY_GIVE_FROM_ISR(); + + /* If the task is in the blocked state specifically to wait for a + notification then unblock it now. */ + if( eOriginalNotifyState == eWaitingNotification ) + { + /* The task should not have been on an event list. */ + configASSERT( listLIST_ITEM_CONTAINER( &( pxTCB->xEventListItem ) ) == NULL ); + + if( uxSchedulerSuspended == ( UBaseType_t ) pdFALSE ) + { + ( void ) uxListRemove( &( pxTCB->xGenericListItem ) ); + prvAddTaskToReadyList( pxTCB ); + } + else + { + /* The delayed and ready lists cannot be accessed, so hold + this task pending until the scheduler is resumed. */ + vListInsertEnd( &( xPendingReadyList ), &( pxTCB->xEventListItem ) ); + } + + if( pxTCB->uxPriority > pxCurrentTCB->uxPriority ) + { + /* The notified task has a priority above the currently + executing task so a yield is required. */ + if( pxHigherPriorityTaskWoken != NULL ) + { + *pxHigherPriorityTaskWoken = pdTRUE; + } + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + } + portCLEAR_INTERRUPT_MASK_FROM_ISR( uxSavedInterruptStatus ); + } + +#endif /* configUSE_TASK_NOTIFICATIONS */ + +/*-----------------------------------------------------------*/ + +#if( configUSE_TASK_NOTIFICATIONS == 1 ) + + BaseType_t xTaskNotifyStateClear( TaskHandle_t xTask ) + { + TCB_t *pxTCB; + BaseType_t xReturn; + + pxTCB = ( TCB_t * ) xTask; + + /* If null is passed in here then it is the calling task that is having + its notification state cleared. */ + pxTCB = prvGetTCBFromHandle( pxTCB ); + + taskENTER_CRITICAL(); + { + if( pxTCB->eNotifyState == eNotified ) + { + pxTCB->eNotifyState = eNotWaitingNotification; + xReturn = pdPASS; + } + else + { + xReturn = pdFAIL; + } + } + taskEXIT_CRITICAL(); + + return xReturn; + } + +#endif /* configUSE_TASK_NOTIFICATIONS */ + +#ifdef FREERTOS_MODULE_TEST + #include "tasks_test_access_functions.h" +#endif diff --git a/lib/FreeRTOS/timers.c b/lib/FreeRTOS/timers.c index ddfdb70470..c7ab9023f0 100644 --- a/lib/FreeRTOS/timers.c +++ b/lib/FreeRTOS/timers.c @@ -1,77 +1,75 @@ /* - FreeRTOS V7.4.0 - Copyright (C) 2013 Real Time Engineers Ltd. - - FEATURES AND PORTS ARE ADDED TO FREERTOS ALL THE TIME. PLEASE VISIT - http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. - - *************************************************************************** - * * - * FreeRTOS tutorial books are available in pdf and paperback. * - * Complete, revised, and edited pdf reference manuals are also * - * available. * - * * - * Purchasing FreeRTOS documentation will not only help you, by * - * ensuring you get running as quickly as possible and with an * - * in-depth knowledge of how to use FreeRTOS, it will also help * - * the FreeRTOS project to continue with its mission of providing * - * professional grade, cross platform, de facto standard solutions * - * for microcontrollers - completely free of charge! * - * * - * >>> See http://www.FreeRTOS.org/Documentation for details. <<< * - * * - * Thank you for using FreeRTOS, and thank you for your support! * - * * - *************************************************************************** + FreeRTOS V8.2.3 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. 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. + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. - >>>>>>NOTE<<<<<< The modification 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. + *************************************************************************** + >>! NOTE: The modification 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 itcan be - viewed here: http://www.freertos.org/a00114.html and also obtained by - writing to Real Time Engineers Ltd., contact details for whom are available - on the FreeRTOS WEB site. - - 1 tab == 4 spaces! + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html *************************************************************************** * * - * Having a problem? Start by reading the FAQ "My application does * - * not run, what could be wrong?" * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * * * - * http://www.FreeRTOS.org/FAQHelp.html * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * * * *************************************************************************** + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? - http://www.FreeRTOS.org - Documentation, books, training, latest versions, - license and Real Time Engineers Ltd. contact details. + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. + + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, - including FreeRTOS+Trace - an indispensable productivity tool, and our new - fully thread aware and reentrant UDP/IP stack. - - http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High - Integrity Systems, who sell the code with commercial support, - indemnification and middleware, under the OpenRTOS brand. - - http://www.SafeRTOS.com - High Integrity Systems also provide a safety - engineered and independently SIL3 certified version for use in safety and + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and mission critical applications that require provable dependability. + + 1 tab == 4 spaces! */ +/* Standard includes. */ +#include + /* Defining MPU_WRAPPERS_INCLUDED_FROM_API_FILE prevents task.h from redefining all the API functions to use the MPU wrappers. That should only be done when task.h is included from an application file. */ @@ -82,7 +80,16 @@ task.h is included from an application file. */ #include "queue.h" #include "timers.h" -#undef MPU_WRAPPERS_INCLUDED_FROM_API_FILE +#if ( INCLUDE_xTimerPendFunctionCall == 1 ) && ( configUSE_TIMERS == 0 ) + #error configUSE_TIMERS must be set to 1 to make the xTimerPendFunctionCall() function available. +#endif + +/* Lint e961 and e750 are suppressed as a MISRA exception justified because the +MPU ports require MPU_WRAPPERS_INCLUDED_FROM_API_FILE to be defined for the +header files above, but not in this file, in order to generate the correct +privileged Vs unprivileged linkage and placement. */ +#undef MPU_WRAPPERS_INCLUDED_FROM_API_FILE /*lint !e961 !e750. */ + /* This entire source file will be skipped if the application is not configured to include software timer functionality. This #if is closed at the very bottom @@ -91,46 +98,84 @@ configUSE_TIMERS is set to 1 in FreeRTOSConfig.h. */ #if ( configUSE_TIMERS == 1 ) /* Misc definitions. */ -#define tmrNO_DELAY ( portTickType ) 0U +#define tmrNO_DELAY ( TickType_t ) 0U /* The definition of the timers themselves. */ typedef struct tmrTimerControl { - const signed char *pcTimerName; /*<< Text name. This is not used by the kernel, it is included simply to make debugging easier. */ - xListItem xTimerListItem; /*<< Standard linked list item as used by all kernel features for event management. */ - portTickType xTimerPeriodInTicks;/*<< How quickly and often the timer expires. */ - unsigned portBASE_TYPE uxAutoReload; /*<< Set to pdTRUE if the timer should be automatically restarted once expired. Set to pdFALSE if the timer is, in effect, a one shot timer. */ + const char *pcTimerName; /*<< Text name. This is not used by the kernel, it is included simply to make debugging easier. */ /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ + ListItem_t xTimerListItem; /*<< Standard linked list item as used by all kernel features for event management. */ + TickType_t xTimerPeriodInTicks;/*<< How quickly and often the timer expires. */ + UBaseType_t uxAutoReload; /*<< Set to pdTRUE if the timer should be automatically restarted once expired. Set to pdFALSE if the timer is, in effect, a one-shot timer. */ void *pvTimerID; /*<< An ID to identify the timer. This allows the timer to be identified when the same callback is used for multiple timers. */ - tmrTIMER_CALLBACK pxCallbackFunction; /*<< The function that will be called when the timer expires. */ + TimerCallbackFunction_t pxCallbackFunction; /*<< The function that will be called when the timer expires. */ + #if( configUSE_TRACE_FACILITY == 1 ) + UBaseType_t uxTimerNumber; /*<< An ID assigned by trace tools such as FreeRTOS+Trace */ + #endif } xTIMER; -/* The definition of messages that can be sent and received on the timer -queue. */ +/* The old xTIMER name is maintained above then typedefed to the new Timer_t +name below to enable the use of older kernel aware debuggers. */ +typedef xTIMER Timer_t; + +/* The definition of messages that can be sent and received on the timer queue. +Two types of message can be queued - messages that manipulate a software timer, +and messages that request the execution of a non-timer related callback. The +two message types are defined in two separate structures, xTimerParametersType +and xCallbackParametersType respectively. */ +typedef struct tmrTimerParameters +{ + TickType_t xMessageValue; /*<< An optional value used by a subset of commands, for example, when changing the period of a timer. */ + Timer_t * pxTimer; /*<< The timer to which the command will be applied. */ +} TimerParameter_t; + + +typedef struct tmrCallbackParameters +{ + PendedFunction_t pxCallbackFunction; /* << The callback function to execute. */ + void *pvParameter1; /* << The value that will be used as the callback functions first parameter. */ + uint32_t ulParameter2; /* << The value that will be used as the callback functions second parameter. */ +} CallbackParameters_t; + +/* The structure that contains the two message types, along with an identifier +that is used to determine which message type is valid. */ typedef struct tmrTimerQueueMessage { - portBASE_TYPE xMessageID; /*<< The command being sent to the timer service task. */ - portTickType xMessageValue; /*<< An optional value used by a subset of commands, for example, when changing the period of a timer. */ - xTIMER * pxTimer; /*<< The timer to which the command will be applied. */ -} xTIMER_MESSAGE; + BaseType_t xMessageID; /*<< The command being sent to the timer service task. */ + union + { + TimerParameter_t xTimerParameters; + + /* Don't include xCallbackParameters if it is not going to be used as + it makes the structure (and therefore the timer queue) larger. */ + #if ( INCLUDE_xTimerPendFunctionCall == 1 ) + CallbackParameters_t xCallbackParameters; + #endif /* INCLUDE_xTimerPendFunctionCall */ + } u; +} DaemonTaskMessage_t; +/*lint -e956 A manual analysis and inspection has been used to determine which +static variables must be declared volatile. */ /* The list in which active timers are stored. Timers are referenced in expire time order, with the nearest expiry time at the front of the list. Only the -timer service task is allowed to access xActiveTimerList. */ -PRIVILEGED_DATA static xList xActiveTimerList1; -PRIVILEGED_DATA static xList xActiveTimerList2; -PRIVILEGED_DATA static xList *pxCurrentTimerList; -PRIVILEGED_DATA static xList *pxOverflowTimerList; +timer service task is allowed to access these lists. */ +PRIVILEGED_DATA static List_t xActiveTimerList1; +PRIVILEGED_DATA static List_t xActiveTimerList2; +PRIVILEGED_DATA static List_t *pxCurrentTimerList; +PRIVILEGED_DATA static List_t *pxOverflowTimerList; /* A queue that is used to send commands to the timer service task. */ -PRIVILEGED_DATA static xQueueHandle xTimerQueue = NULL; +PRIVILEGED_DATA static QueueHandle_t xTimerQueue = NULL; #if ( INCLUDE_xTimerGetTimerDaemonTaskHandle == 1 ) - PRIVILEGED_DATA static xTaskHandle xTimerTaskHandle = NULL; + PRIVILEGED_DATA static TaskHandle_t xTimerTaskHandle = NULL; #endif +/*lint +e956 */ + /*-----------------------------------------------------------*/ /* @@ -150,31 +195,31 @@ static void prvTimerTask( void *pvParameters ) PRIVILEGED_FUNCTION; * Called by the timer service task to interpret and process a command it * received on the timer queue. */ -static void prvProcessReceivedCommands( void ) PRIVILEGED_FUNCTION; +static void prvProcessReceivedCommands( void ) PRIVILEGED_FUNCTION; /* * Insert the timer into either xActiveTimerList1, or xActiveTimerList2, * depending on if the expire time causes a timer counter overflow. */ -static portBASE_TYPE prvInsertTimerInActiveList( xTIMER *pxTimer, portTickType xNextExpiryTime, portTickType xTimeNow, portTickType xCommandTime ) PRIVILEGED_FUNCTION; +static BaseType_t prvInsertTimerInActiveList( Timer_t * const pxTimer, const TickType_t xNextExpiryTime, const TickType_t xTimeNow, const TickType_t xCommandTime ) PRIVILEGED_FUNCTION; /* * An active timer has reached its expire time. Reload the timer if it is an * auto reload timer, then call its callback. */ -static void prvProcessExpiredTimer( portTickType xNextExpireTime, portTickType xTimeNow ) PRIVILEGED_FUNCTION; +static void prvProcessExpiredTimer( const TickType_t xNextExpireTime, const TickType_t xTimeNow ) PRIVILEGED_FUNCTION; /* * The tick count has overflowed. Switch the timer lists after ensuring the * current timer list does not still reference some timers. */ -static void prvSwitchTimerLists( portTickType xLastTime ) PRIVILEGED_FUNCTION; +static void prvSwitchTimerLists( void ) PRIVILEGED_FUNCTION; /* * Obtain the current tick count, setting *pxTimerListsWereSwitched to pdTRUE * if a tick count overflow occurred since prvSampleTimeNow() was last called. */ -static portTickType prvSampleTimeNow( portBASE_TYPE *pxTimerListsWereSwitched ) PRIVILEGED_FUNCTION; +static TickType_t prvSampleTimeNow( BaseType_t * const pxTimerListsWereSwitched ) PRIVILEGED_FUNCTION; /* * If the timer list contains any active timers then return the expire time of @@ -182,19 +227,19 @@ static portTickType prvSampleTimeNow( portBASE_TYPE *pxTimerListsWereSwitched ) * timer list does not contain any timers then return 0 and set *pxListWasEmpty * to pdTRUE. */ -static portTickType prvGetNextExpireTime( portBASE_TYPE *pxListWasEmpty ) PRIVILEGED_FUNCTION; +static TickType_t prvGetNextExpireTime( BaseType_t * const pxListWasEmpty ) PRIVILEGED_FUNCTION; /* * If a timer has expired, process it. Otherwise, block the timer service task * until either a timer does expire or a command is received. */ -static void prvProcessTimerOrBlockTask( portTickType xNextExpireTime, portBASE_TYPE xListWasEmpty ) PRIVILEGED_FUNCTION; +static void prvProcessTimerOrBlockTask( const TickType_t xNextExpireTime, BaseType_t xListWasEmpty ) PRIVILEGED_FUNCTION; /*-----------------------------------------------------------*/ -portBASE_TYPE xTimerCreateTimerTask( void ) +BaseType_t xTimerCreateTimerTask( void ) { -portBASE_TYPE xReturn = pdFAIL; +BaseType_t xReturn = pdFAIL; /* This function is called when the scheduler is started if configUSE_TIMERS is set to 1. Check that the infrastructure used by the @@ -208,34 +253,37 @@ portBASE_TYPE xReturn = pdFAIL; { /* Create the timer task, storing its handle in xTimerTaskHandle so it can be returned by the xTimerGetTimerDaemonTaskHandle() function. */ - xReturn = xTaskCreate( prvTimerTask, ( const signed char * ) "Tmr Svc", ( unsigned short ) configTIMER_TASK_STACK_DEPTH, NULL, ( ( unsigned portBASE_TYPE ) configTIMER_TASK_PRIORITY ) | portPRIVILEGE_BIT, &xTimerTaskHandle ); + xReturn = xTaskCreate( prvTimerTask, "Tmr Svc", ( uint16_t ) configTIMER_TASK_STACK_DEPTH, NULL, ( ( UBaseType_t ) configTIMER_TASK_PRIORITY ) | portPRIVILEGE_BIT, &xTimerTaskHandle ); } #else { /* Create the timer task without storing its handle. */ - xReturn = xTaskCreate( prvTimerTask, ( const signed char * ) "Tmr Svc", ( unsigned short ) configTIMER_TASK_STACK_DEPTH, NULL, ( ( unsigned portBASE_TYPE ) configTIMER_TASK_PRIORITY ) | portPRIVILEGE_BIT, NULL); + xReturn = xTaskCreate( prvTimerTask, "Tmr Svc", ( uint16_t ) configTIMER_TASK_STACK_DEPTH, NULL, ( ( UBaseType_t ) configTIMER_TASK_PRIORITY ) | portPRIVILEGE_BIT, NULL); } #endif } + else + { + mtCOVERAGE_TEST_MARKER(); + } configASSERT( xReturn ); return xReturn; } /*-----------------------------------------------------------*/ -xTimerHandle xTimerCreate( const signed char * const pcTimerName, portTickType xTimerPeriodInTicks, unsigned portBASE_TYPE uxAutoReload, void *pvTimerID, tmrTIMER_CALLBACK pxCallbackFunction ) +TimerHandle_t xTimerCreate( const char * const pcTimerName, const TickType_t xTimerPeriodInTicks, const UBaseType_t uxAutoReload, void * const pvTimerID, TimerCallbackFunction_t pxCallbackFunction ) /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ { -xTIMER *pxNewTimer; +Timer_t *pxNewTimer; /* Allocate the timer structure. */ - if( xTimerPeriodInTicks == ( portTickType ) 0U ) + if( xTimerPeriodInTicks == ( TickType_t ) 0U ) { pxNewTimer = NULL; - configASSERT( ( xTimerPeriodInTicks > 0 ) ); } else { - pxNewTimer = ( xTIMER * ) pvPortMalloc( sizeof( xTIMER ) ); + pxNewTimer = ( Timer_t * ) pvPortMalloc( sizeof( Timer_t ) ); if( pxNewTimer != NULL ) { /* Ensure the infrastructure used by the timer service task has been @@ -258,14 +306,19 @@ xTIMER *pxNewTimer; } } - return ( xTimerHandle ) pxNewTimer; + /* 0 is not a valid value for xTimerPeriodInTicks. */ + configASSERT( ( xTimerPeriodInTicks > 0 ) ); + + return ( TimerHandle_t ) pxNewTimer; } /*-----------------------------------------------------------*/ -portBASE_TYPE xTimerGenericCommand( xTimerHandle xTimer, portBASE_TYPE xCommandID, portTickType xOptionalValue, signed portBASE_TYPE *pxHigherPriorityTaskWoken, portTickType xBlockTime ) +BaseType_t xTimerGenericCommand( TimerHandle_t xTimer, const BaseType_t xCommandID, const TickType_t xOptionalValue, BaseType_t * const pxHigherPriorityTaskWoken, const TickType_t xTicksToWait ) { -portBASE_TYPE xReturn = pdFAIL; -xTIMER_MESSAGE xMessage; +BaseType_t xReturn = pdFAIL; +DaemonTaskMessage_t xMessage; + + configASSERT( xTimer ); /* Send a message to the timer service task to perform a particular action on a particular timer definition. */ @@ -273,14 +326,14 @@ xTIMER_MESSAGE xMessage; { /* Send a command to the timer service task to start the xTimer timer. */ xMessage.xMessageID = xCommandID; - xMessage.xMessageValue = xOptionalValue; - xMessage.pxTimer = ( xTIMER * ) xTimer; + xMessage.u.xTimerParameters.xMessageValue = xOptionalValue; + xMessage.u.xTimerParameters.pxTimer = ( Timer_t * ) xTimer; - if( pxHigherPriorityTaskWoken == NULL ) + if( xCommandID < tmrFIRST_FROM_ISR_COMMAND ) { if( xTaskGetSchedulerState() == taskSCHEDULER_RUNNING ) { - xReturn = xQueueSendToBack( xTimerQueue, &xMessage, xBlockTime ); + xReturn = xQueueSendToBack( xTimerQueue, &xMessage, xTicksToWait ); } else { @@ -294,6 +347,10 @@ xTIMER_MESSAGE xMessage; traceTIMER_COMMAND_SEND( xTimer, xCommandID, xOptionalValue, xReturn ); } + else + { + mtCOVERAGE_TEST_MARKER(); + } return xReturn; } @@ -301,7 +358,7 @@ xTIMER_MESSAGE xMessage; #if ( INCLUDE_xTimerGetTimerDaemonTaskHandle == 1 ) - xTaskHandle xTimerGetTimerDaemonTaskHandle( void ) + TaskHandle_t xTimerGetTimerDaemonTaskHandle( void ) { /* If xTimerGetTimerDaemonTaskHandle() is called before the scheduler has been started, then xTimerTaskHandle will be NULL. */ @@ -312,46 +369,59 @@ xTIMER_MESSAGE xMessage; #endif /*-----------------------------------------------------------*/ -static void prvProcessExpiredTimer( portTickType xNextExpireTime, portTickType xTimeNow ) +const char * pcTimerGetTimerName( TimerHandle_t xTimer ) +{ +Timer_t *pxTimer = ( Timer_t * ) xTimer; + + configASSERT( xTimer ); + return pxTimer->pcTimerName; +} +/*-----------------------------------------------------------*/ + +static void prvProcessExpiredTimer( const TickType_t xNextExpireTime, const TickType_t xTimeNow ) { -xTIMER *pxTimer; -portBASE_TYPE xResult; +BaseType_t xResult; +Timer_t * const pxTimer = ( Timer_t * ) listGET_OWNER_OF_HEAD_ENTRY( pxCurrentTimerList ); /* Remove the timer from the list of active timers. A check has already been performed to ensure the list is not empty. */ - pxTimer = ( xTIMER * ) listGET_OWNER_OF_HEAD_ENTRY( pxCurrentTimerList ); - uxListRemove( &( pxTimer->xTimerListItem ) ); + ( void ) uxListRemove( &( pxTimer->xTimerListItem ) ); traceTIMER_EXPIRED( pxTimer ); /* If the timer is an auto reload timer then calculate the next expiry time and re-insert the timer in the list of active timers. */ - if( pxTimer->uxAutoReload == ( unsigned portBASE_TYPE ) pdTRUE ) + if( pxTimer->uxAutoReload == ( UBaseType_t ) pdTRUE ) { - /* This is the only time a timer is inserted into a list using - a time relative to anything other than the current time. It - will therefore be inserted into the correct list relative to - the time this task thinks it is now, even if a command to - switch lists due to a tick count overflow is already waiting in - the timer queue. */ + /* The timer is inserted into a list using a time relative to anything + other than the current time. It will therefore be inserted into the + correct list relative to the time this task thinks it is now. */ if( prvInsertTimerInActiveList( pxTimer, ( xNextExpireTime + pxTimer->xTimerPeriodInTicks ), xTimeNow, xNextExpireTime ) == pdTRUE ) { /* The timer expired before it was added to the active timer list. Reload it now. */ - xResult = xTimerGenericCommand( pxTimer, tmrCOMMAND_START, xNextExpireTime, NULL, tmrNO_DELAY ); + xResult = xTimerGenericCommand( pxTimer, tmrCOMMAND_START_DONT_TRACE, xNextExpireTime, NULL, tmrNO_DELAY ); configASSERT( xResult ); ( void ) xResult; } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + else + { + mtCOVERAGE_TEST_MARKER(); } /* Call the timer callback. */ - pxTimer->pxCallbackFunction( ( xTimerHandle ) pxTimer ); + pxTimer->pxCallbackFunction( ( TimerHandle_t ) pxTimer ); } /*-----------------------------------------------------------*/ static void prvTimerTask( void *pvParameters ) { -portTickType xNextExpireTime; -portBASE_TYPE xListWasEmpty; +TickType_t xNextExpireTime; +BaseType_t xListWasEmpty; /* Just to avoid compiler warnings. */ ( void ) pvParameters; @@ -372,10 +442,10 @@ portBASE_TYPE xListWasEmpty; } /*-----------------------------------------------------------*/ -static void prvProcessTimerOrBlockTask( portTickType xNextExpireTime, portBASE_TYPE xListWasEmpty ) +static void prvProcessTimerOrBlockTask( const TickType_t xNextExpireTime, BaseType_t xListWasEmpty ) { -portTickType xTimeNow; -portBASE_TYPE xTimerListsWereSwitched; +TickType_t xTimeNow; +BaseType_t xTimerListsWereSwitched; vTaskSuspendAll(); { @@ -383,14 +453,14 @@ portBASE_TYPE xTimerListsWereSwitched; has expired or not. If obtaining the time causes the lists to switch then don't process this timer as any timers that remained in the list when the lists were switched will have been processed within the - prvSampelTimeNow() function. */ + prvSampleTimeNow() function. */ xTimeNow = prvSampleTimeNow( &xTimerListsWereSwitched ); if( xTimerListsWereSwitched == pdFALSE ) { /* The tick count has not overflowed, has the timer expired? */ if( ( xListWasEmpty == pdFALSE ) && ( xNextExpireTime <= xTimeNow ) ) { - xTaskResumeAll(); + ( void ) xTaskResumeAll(); prvProcessExpiredTimer( xNextExpireTime, xTimeNow ); } else @@ -401,29 +471,40 @@ portBASE_TYPE xTimerListsWereSwitched; received - whichever comes first. The following line cannot be reached unless xNextExpireTime > xTimeNow, except in the case when the current timer list is empty. */ - vQueueWaitForMessageRestricted( xTimerQueue, ( xNextExpireTime - xTimeNow ) ); + if( xListWasEmpty != pdFALSE ) + { + /* The current timer list is empty - is the overflow list + also empty? */ + xListWasEmpty = listLIST_IS_EMPTY( pxOverflowTimerList ); + } + + vQueueWaitForMessageRestricted( xTimerQueue, ( xNextExpireTime - xTimeNow ), xListWasEmpty ); if( xTaskResumeAll() == pdFALSE ) { - /* Yield to wait for either a command to arrive, or the block time - to expire. If a command arrived between the critical section being - exited and this yield then the yield will not cause the task - to block. */ + /* Yield to wait for either a command to arrive, or the + block time to expire. If a command arrived between the + critical section being exited and this yield then the yield + will not cause the task to block. */ portYIELD_WITHIN_API(); } + else + { + mtCOVERAGE_TEST_MARKER(); + } } } else { - xTaskResumeAll(); + ( void ) xTaskResumeAll(); } } } /*-----------------------------------------------------------*/ -static portTickType prvGetNextExpireTime( portBASE_TYPE *pxListWasEmpty ) +static TickType_t prvGetNextExpireTime( BaseType_t * const pxListWasEmpty ) { -portTickType xNextExpireTime; +TickType_t xNextExpireTime; /* Timers are listed in expiry time order, with the head of the list referencing the task that will expire first. Obtain the time at which @@ -440,23 +521,23 @@ portTickType xNextExpireTime; else { /* Ensure the task unblocks when the tick count rolls over. */ - xNextExpireTime = ( portTickType ) 0U; + xNextExpireTime = ( TickType_t ) 0U; } return xNextExpireTime; } /*-----------------------------------------------------------*/ -static portTickType prvSampleTimeNow( portBASE_TYPE *pxTimerListsWereSwitched ) +static TickType_t prvSampleTimeNow( BaseType_t * const pxTimerListsWereSwitched ) { -portTickType xTimeNow; -PRIVILEGED_DATA static portTickType xLastTime = ( portTickType ) 0U; +TickType_t xTimeNow; +PRIVILEGED_DATA static TickType_t xLastTime = ( TickType_t ) 0U; /*lint !e956 Variable is only accessible to one task. */ xTimeNow = xTaskGetTickCount(); if( xTimeNow < xLastTime ) { - prvSwitchTimerLists( xLastTime ); + prvSwitchTimerLists(); *pxTimerListsWereSwitched = pdTRUE; } else @@ -470,9 +551,9 @@ PRIVILEGED_DATA static portTickType xLastTime = ( portTickType ) 0U; } /*-----------------------------------------------------------*/ -static portBASE_TYPE prvInsertTimerInActiveList( xTIMER *pxTimer, portTickType xNextExpiryTime, portTickType xTimeNow, portTickType xCommandTime ) +static BaseType_t prvInsertTimerInActiveList( Timer_t * const pxTimer, const TickType_t xNextExpiryTime, const TickType_t xTimeNow, const TickType_t xCommandTime ) { -portBASE_TYPE xProcessTimerNow = pdFALSE; +BaseType_t xProcessTimerNow = pdFALSE; listSET_LIST_ITEM_VALUE( &( pxTimer->xTimerListItem ), xNextExpiryTime ); listSET_LIST_ITEM_OWNER( &( pxTimer->xTimerListItem ), pxTimer ); @@ -481,7 +562,7 @@ portBASE_TYPE xProcessTimerNow = pdFALSE; { /* Has the expiry time elapsed between the command to start/reset a timer was issued, and the time the command was processed? */ - if( ( ( portTickType ) ( xTimeNow - xCommandTime ) ) >= pxTimer->xTimerPeriodInTicks ) + if( ( xTimeNow - xCommandTime ) >= pxTimer->xTimerPeriodInTicks ) { /* The time between a command being issued and the command being processed actually exceeds the timers period. */ @@ -513,84 +594,136 @@ portBASE_TYPE xProcessTimerNow = pdFALSE; static void prvProcessReceivedCommands( void ) { -xTIMER_MESSAGE xMessage; -xTIMER *pxTimer; -portBASE_TYPE xTimerListsWereSwitched, xResult; -portTickType xTimeNow; +DaemonTaskMessage_t xMessage; +Timer_t *pxTimer; +BaseType_t xTimerListsWereSwitched, xResult; +TickType_t xTimeNow; - while( xQueueReceive( xTimerQueue, &xMessage, tmrNO_DELAY ) != pdFAIL ) + while( xQueueReceive( xTimerQueue, &xMessage, tmrNO_DELAY ) != pdFAIL ) /*lint !e603 xMessage does not have to be initialised as it is passed out, not in, and it is not used unless xQueueReceive() returns pdTRUE. */ { - pxTimer = xMessage.pxTimer; - - if( listIS_CONTAINED_WITHIN( NULL, &( pxTimer->xTimerListItem ) ) == pdFALSE ) + #if ( INCLUDE_xTimerPendFunctionCall == 1 ) { - /* The timer is in a list, remove it. */ - uxListRemove( &( pxTimer->xTimerListItem ) ); - } + /* Negative commands are pended function calls rather than timer + commands. */ + if( xMessage.xMessageID < ( BaseType_t ) 0 ) + { + const CallbackParameters_t * const pxCallback = &( xMessage.u.xCallbackParameters ); - traceTIMER_COMMAND_RECEIVED( pxTimer, xMessage.xMessageID, xMessage.xMessageValue ); + /* The timer uses the xCallbackParameters member to request a + callback be executed. Check the callback is not NULL. */ + configASSERT( pxCallback ); - /* In this case the xTimerListsWereSwitched parameter is not used, but - it must be present in the function call. prvSampleTimeNow() must be - called after the message is received from xTimerQueue so there is no - possibility of a higher priority task adding a message to the message - queue with a time that is ahead of the timer daemon task (because it - pre-empted the timer daemon task after the xTimeNow value was set). */ - xTimeNow = prvSampleTimeNow( &xTimerListsWereSwitched ); + /* Call the function. */ + pxCallback->pxCallbackFunction( pxCallback->pvParameter1, pxCallback->ulParameter2 ); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + #endif /* INCLUDE_xTimerPendFunctionCall */ - switch( xMessage.xMessageID ) + /* Commands that are positive are timer commands rather than pended + function calls. */ + if( xMessage.xMessageID >= ( BaseType_t ) 0 ) { - case tmrCOMMAND_START : - /* Start or restart a timer. */ - if( prvInsertTimerInActiveList( pxTimer, xMessage.xMessageValue + pxTimer->xTimerPeriodInTicks, xTimeNow, xMessage.xMessageValue ) == pdTRUE ) - { - /* The timer expired before it was added to the active timer - list. Process it now. */ - pxTimer->pxCallbackFunction( ( xTimerHandle ) pxTimer ); + /* The messages uses the xTimerParameters member to work on a + software timer. */ + pxTimer = xMessage.u.xTimerParameters.pxTimer; + + if( listIS_CONTAINED_WITHIN( NULL, &( pxTimer->xTimerListItem ) ) == pdFALSE ) + { + /* The timer is in a list, remove it. */ + ( void ) uxListRemove( &( pxTimer->xTimerListItem ) ); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + + traceTIMER_COMMAND_RECEIVED( pxTimer, xMessage.xMessageID, xMessage.u.xTimerParameters.xMessageValue ); + + /* In this case the xTimerListsWereSwitched parameter is not used, but + it must be present in the function call. prvSampleTimeNow() must be + called after the message is received from xTimerQueue so there is no + possibility of a higher priority task adding a message to the message + queue with a time that is ahead of the timer daemon task (because it + pre-empted the timer daemon task after the xTimeNow value was set). */ + xTimeNow = prvSampleTimeNow( &xTimerListsWereSwitched ); - if( pxTimer->uxAutoReload == ( unsigned portBASE_TYPE ) pdTRUE ) + switch( xMessage.xMessageID ) + { + case tmrCOMMAND_START : + case tmrCOMMAND_START_FROM_ISR : + case tmrCOMMAND_RESET : + case tmrCOMMAND_RESET_FROM_ISR : + case tmrCOMMAND_START_DONT_TRACE : + /* Start or restart a timer. */ + if( prvInsertTimerInActiveList( pxTimer, xMessage.u.xTimerParameters.xMessageValue + pxTimer->xTimerPeriodInTicks, xTimeNow, xMessage.u.xTimerParameters.xMessageValue ) == pdTRUE ) { - xResult = xTimerGenericCommand( pxTimer, tmrCOMMAND_START, xMessage.xMessageValue + pxTimer->xTimerPeriodInTicks, NULL, tmrNO_DELAY ); - configASSERT( xResult ); - ( void ) xResult; + /* The timer expired before it was added to the active + timer list. Process it now. */ + pxTimer->pxCallbackFunction( ( TimerHandle_t ) pxTimer ); + traceTIMER_EXPIRED( pxTimer ); + + if( pxTimer->uxAutoReload == ( UBaseType_t ) pdTRUE ) + { + xResult = xTimerGenericCommand( pxTimer, tmrCOMMAND_START_DONT_TRACE, xMessage.u.xTimerParameters.xMessageValue + pxTimer->xTimerPeriodInTicks, NULL, tmrNO_DELAY ); + configASSERT( xResult ); + ( void ) xResult; + } + else + { + mtCOVERAGE_TEST_MARKER(); + } } - } - break; - - case tmrCOMMAND_STOP : - /* The timer has already been removed from the active list. - There is nothing to do here. */ - break; - - case tmrCOMMAND_CHANGE_PERIOD : - pxTimer->xTimerPeriodInTicks = xMessage.xMessageValue; - configASSERT( ( pxTimer->xTimerPeriodInTicks > 0 ) ); - prvInsertTimerInActiveList( pxTimer, ( xTimeNow + pxTimer->xTimerPeriodInTicks ), xTimeNow, xTimeNow ); - break; - - case tmrCOMMAND_DELETE : - /* The timer has already been removed from the active list, - just free up the memory. */ - vPortFree( pxTimer ); - break; - - default : - /* Don't expect to get here. */ - break; + else + { + mtCOVERAGE_TEST_MARKER(); + } + break; + + case tmrCOMMAND_STOP : + case tmrCOMMAND_STOP_FROM_ISR : + /* The timer has already been removed from the active list. + There is nothing to do here. */ + break; + + case tmrCOMMAND_CHANGE_PERIOD : + case tmrCOMMAND_CHANGE_PERIOD_FROM_ISR : + pxTimer->xTimerPeriodInTicks = xMessage.u.xTimerParameters.xMessageValue; + configASSERT( ( pxTimer->xTimerPeriodInTicks > 0 ) ); + + /* The new period does not really have a reference, and can be + longer or shorter than the old one. The command time is + therefore set to the current time, and as the period cannot be + zero the next expiry time can only be in the future, meaning + (unlike for the xTimerStart() case above) there is no fail case + that needs to be handled here. */ + ( void ) prvInsertTimerInActiveList( pxTimer, ( xTimeNow + pxTimer->xTimerPeriodInTicks ), xTimeNow, xTimeNow ); + break; + + case tmrCOMMAND_DELETE : + /* The timer has already been removed from the active list, + just free up the memory. */ + vPortFree( pxTimer ); + break; + + default : + /* Don't expect to get here. */ + break; + } } } } /*-----------------------------------------------------------*/ -static void prvSwitchTimerLists( portTickType xLastTime ) +static void prvSwitchTimerLists( void ) { -portTickType xNextExpireTime, xReloadTime; -xList *pxTemp; -xTIMER *pxTimer; -portBASE_TYPE xResult; - - /* Remove compiler warnings if configASSERT() is not defined. */ - ( void ) xLastTime; +TickType_t xNextExpireTime, xReloadTime; +List_t *pxTemp; +Timer_t *pxTimer; +BaseType_t xResult; /* The tick count has overflowed. The timer lists must be switched. If there are any timers still referenced from the current timer list @@ -601,15 +734,16 @@ portBASE_TYPE xResult; xNextExpireTime = listGET_ITEM_VALUE_OF_HEAD_ENTRY( pxCurrentTimerList ); /* Remove the timer from the list. */ - pxTimer = ( xTIMER * ) listGET_OWNER_OF_HEAD_ENTRY( pxCurrentTimerList ); - uxListRemove( &( pxTimer->xTimerListItem ) ); + pxTimer = ( Timer_t * ) listGET_OWNER_OF_HEAD_ENTRY( pxCurrentTimerList ); + ( void ) uxListRemove( &( pxTimer->xTimerListItem ) ); + traceTIMER_EXPIRED( pxTimer ); /* Execute its callback, then send a command to restart the timer if it is an auto-reload timer. It cannot be restarted here as the lists have not yet been switched. */ - pxTimer->pxCallbackFunction( ( xTimerHandle ) pxTimer ); + pxTimer->pxCallbackFunction( ( TimerHandle_t ) pxTimer ); - if( pxTimer->uxAutoReload == ( unsigned portBASE_TYPE ) pdTRUE ) + if( pxTimer->uxAutoReload == ( UBaseType_t ) pdTRUE ) { /* Calculate the reload value, and if the reload value results in the timer going into the same timer list then it has already expired @@ -626,11 +760,15 @@ portBASE_TYPE xResult; } else { - xResult = xTimerGenericCommand( pxTimer, tmrCOMMAND_START, xNextExpireTime, NULL, tmrNO_DELAY ); + xResult = xTimerGenericCommand( pxTimer, tmrCOMMAND_START_DONT_TRACE, xNextExpireTime, NULL, tmrNO_DELAY ); configASSERT( xResult ); ( void ) xResult; } } + else + { + mtCOVERAGE_TEST_MARKER(); + } } pxTemp = pxCurrentTimerList; @@ -652,17 +790,37 @@ static void prvCheckForValidListAndQueue( void ) vListInitialise( &xActiveTimerList2 ); pxCurrentTimerList = &xActiveTimerList1; pxOverflowTimerList = &xActiveTimerList2; - xTimerQueue = xQueueCreate( ( unsigned portBASE_TYPE ) configTIMER_QUEUE_LENGTH, sizeof( xTIMER_MESSAGE ) ); + xTimerQueue = xQueueCreate( ( UBaseType_t ) configTIMER_QUEUE_LENGTH, sizeof( DaemonTaskMessage_t ) ); + configASSERT( xTimerQueue ); + + #if ( configQUEUE_REGISTRY_SIZE > 0 ) + { + if( xTimerQueue != NULL ) + { + vQueueAddToRegistry( xTimerQueue, "TmrQ" ); + } + else + { + mtCOVERAGE_TEST_MARKER(); + } + } + #endif /* configQUEUE_REGISTRY_SIZE */ + } + else + { + mtCOVERAGE_TEST_MARKER(); } } taskEXIT_CRITICAL(); } /*-----------------------------------------------------------*/ -portBASE_TYPE xTimerIsTimerActive( xTimerHandle xTimer ) +BaseType_t xTimerIsTimerActive( TimerHandle_t xTimer ) { -portBASE_TYPE xTimerIsInActiveList; -xTIMER *pxTimer = ( xTIMER * ) xTimer; +BaseType_t xTimerIsInActiveList; +Timer_t *pxTimer = ( Timer_t * ) xTimer; + + configASSERT( xTimer ); /* Is the timer in the list of active timers? */ taskENTER_CRITICAL(); @@ -670,22 +828,98 @@ xTIMER *pxTimer = ( xTIMER * ) xTimer; /* Checking to see if it is in the NULL list in effect checks to see if it is referenced from either the current or the overflow timer lists in one go, but the logic has to be reversed, hence the '!'. */ - xTimerIsInActiveList = !( listIS_CONTAINED_WITHIN( NULL, &( pxTimer->xTimerListItem ) ) ); + xTimerIsInActiveList = ( BaseType_t ) !( listIS_CONTAINED_WITHIN( NULL, &( pxTimer->xTimerListItem ) ) ); } taskEXIT_CRITICAL(); return xTimerIsInActiveList; +} /*lint !e818 Can't be pointer to const due to the typedef. */ +/*-----------------------------------------------------------*/ + +void *pvTimerGetTimerID( const TimerHandle_t xTimer ) +{ +Timer_t * const pxTimer = ( Timer_t * ) xTimer; +void *pvReturn; + + configASSERT( xTimer ); + + taskENTER_CRITICAL(); + { + pvReturn = pxTimer->pvTimerID; + } + taskEXIT_CRITICAL(); + + return pvReturn; } /*-----------------------------------------------------------*/ -void *pvTimerGetTimerID( xTimerHandle xTimer ) +void vTimerSetTimerID( TimerHandle_t xTimer, void *pvNewID ) { -xTIMER *pxTimer = ( xTIMER * ) xTimer; +Timer_t * const pxTimer = ( Timer_t * ) xTimer; - return pxTimer->pvTimerID; + configASSERT( xTimer ); + + taskENTER_CRITICAL(); + { + pxTimer->pvTimerID = pvNewID; + } + taskEXIT_CRITICAL(); } /*-----------------------------------------------------------*/ +#if( INCLUDE_xTimerPendFunctionCall == 1 ) + + BaseType_t xTimerPendFunctionCallFromISR( PendedFunction_t xFunctionToPend, void *pvParameter1, uint32_t ulParameter2, BaseType_t *pxHigherPriorityTaskWoken ) + { + DaemonTaskMessage_t xMessage; + BaseType_t xReturn; + + /* Complete the message with the function parameters and post it to the + daemon task. */ + xMessage.xMessageID = tmrCOMMAND_EXECUTE_CALLBACK_FROM_ISR; + xMessage.u.xCallbackParameters.pxCallbackFunction = xFunctionToPend; + xMessage.u.xCallbackParameters.pvParameter1 = pvParameter1; + xMessage.u.xCallbackParameters.ulParameter2 = ulParameter2; + + xReturn = xQueueSendFromISR( xTimerQueue, &xMessage, pxHigherPriorityTaskWoken ); + + tracePEND_FUNC_CALL_FROM_ISR( xFunctionToPend, pvParameter1, ulParameter2, xReturn ); + + return xReturn; + } + +#endif /* INCLUDE_xTimerPendFunctionCall */ +/*-----------------------------------------------------------*/ + +#if( INCLUDE_xTimerPendFunctionCall == 1 ) + + BaseType_t xTimerPendFunctionCall( PendedFunction_t xFunctionToPend, void *pvParameter1, uint32_t ulParameter2, TickType_t xTicksToWait ) + { + DaemonTaskMessage_t xMessage; + BaseType_t xReturn; + + /* This function can only be called after a timer has been created or + after the scheduler has been started because, until then, the timer + queue does not exist. */ + configASSERT( xTimerQueue ); + + /* Complete the message with the function parameters and post it to the + daemon task. */ + xMessage.xMessageID = tmrCOMMAND_EXECUTE_CALLBACK; + xMessage.u.xCallbackParameters.pxCallbackFunction = xFunctionToPend; + xMessage.u.xCallbackParameters.pvParameter1 = pvParameter1; + xMessage.u.xCallbackParameters.ulParameter2 = ulParameter2; + + xReturn = xQueueSendToBack( xTimerQueue, &xMessage, xTicksToWait ); + + tracePEND_FUNC_CALL( xFunctionToPend, pvParameter1, ulParameter2, xReturn ); + + return xReturn; + } + +#endif /* INCLUDE_xTimerPendFunctionCall */ +/*-----------------------------------------------------------*/ + /* This entire source file will be skipped if the application is not configured to include software timer functionality. If you want to include software timer functionality then ensure configUSE_TIMERS is set to 1 in FreeRTOSConfig.h. */ diff --git a/modules/src/crtp.c b/modules/src/crtp.c index 717dcce4b6..1743b1bbf2 100644 --- a/modules/src/crtp.c +++ b/modules/src/crtp.c @@ -71,9 +71,9 @@ void crtpInit(void) txQueue = xQueueCreate(CRTP_TX_QUEUE_SIZE, sizeof(CRTPPacket)); DEBUG_QUEUE_MONITOR_REGISTER(txQueue); - xTaskCreate(crtpTxTask, (const signed char * const)CRTP_TX_TASK_NAME, + xTaskCreate(crtpTxTask, CRTP_TX_TASK_NAME, CRTP_TX_TASK_STACKSIZE, NULL, CRTP_TX_TASK_PRI, NULL); - xTaskCreate(crtpRxTask, (const signed char * const)CRTP_RX_TASK_NAME, + xTaskCreate(crtpRxTask, CRTP_RX_TASK_NAME, CRTP_RX_TASK_STACKSIZE, NULL, CRTP_RX_TASK_PRI, NULL); /* Start Rx/Tx tasks */ diff --git a/modules/src/info.c b/modules/src/info.c index ca1c56b331..e52b36a538 100644 --- a/modules/src/info.c +++ b/modules/src/info.c @@ -66,7 +66,7 @@ void infoTask(void *param); void infoInit() { - xTaskCreate(infoTask, (const signed char * const)INFO_TASK_NAME, + xTaskCreate(infoTask, INFO_TASK_NAME, INFO_TASK_STACKSIZE, NULL, INFO_TASK_PRI, NULL); crtpInitTaskQueue(crtpInfo); } diff --git a/modules/src/log.c b/modules/src/log.c index bd726834f7..22f8bb9a5f 100644 --- a/modules/src/log.c +++ b/modules/src/log.c @@ -169,7 +169,7 @@ void logInit(void) logReset(); //Start the log task - xTaskCreate(logTask, (const signed char * const)LOG_TASK_NAME, + xTaskCreate(logTask, LOG_TASK_NAME, LOG_TASK_STACKSIZE, NULL, LOG_TASK_PRI, NULL); isInit = true; @@ -311,7 +311,7 @@ static int logCreateBlock(unsigned char id, struct ops_setting * settings, int l return ENOMEM; logBlocks[i].id = id; - logBlocks[i].timer = xTimerCreate( (const signed char *)"logTimer", M2T(1000), + logBlocks[i].timer = xTimerCreate( "logTimer", M2T(1000), pdTRUE, &logBlocks[i], logBlockTimed ); logBlocks[i].ops = NULL; diff --git a/modules/src/mem.c b/modules/src/mem.c index 4e84266bf9..a80fa3ae68 100644 --- a/modules/src/mem.c +++ b/modules/src/mem.c @@ -119,7 +119,7 @@ void memInit(void) isInit = false; //Start the mem task - xTaskCreate(memTask, (const signed char * const)MEM_TASK_NAME, + xTaskCreate(memTask, MEM_TASK_NAME, MEM_TASK_STACKSIZE, NULL, MEM_TASK_PRI, NULL); } diff --git a/modules/src/param.c b/modules/src/param.c index 9c3ce06dae..d5a47630ea 100644 --- a/modules/src/param.c +++ b/modules/src/param.c @@ -93,7 +93,7 @@ void paramInit(void) //Start the param task - xTaskCreate(paramTask, (const signed char * const)PARAM_TASK_NAME, + xTaskCreate(paramTask, PARAM_TASK_NAME, PARAM_TASK_STACKSIZE, NULL, PARAM_TASK_PRI, NULL); //TODO: Handle stored parameters! diff --git a/modules/src/pidctrl.c b/modules/src/pidctrl.c index 19904551e7..65536dd795 100644 --- a/modules/src/pidctrl.c +++ b/modules/src/pidctrl.c @@ -40,7 +40,7 @@ void pidCrtlTask(void *param); void pidCtrlInit() { - xTaskCreate(pidCrtlTask, (const signed char * const)PID_CTRL_TASK_NAME, + xTaskCreate(pidCrtlTask, PID_CTRL_TASK_NAME, PID_CTRL_TASK_STACKSIZE, NULL, PID_CTRL_TASK_PRI, NULL); crtpInitTaskQueue(6); } diff --git a/modules/src/queuemonitor.c b/modules/src/queuemonitor.c index 9f30120c6c..6e74036fa4 100644 --- a/modules/src/queuemonitor.c +++ b/modules/src/queuemonitor.c @@ -69,7 +69,7 @@ unsigned char ucQueueGetQueueNumber( xQueueHandle xQueue ); void queueMonitorInit() { ASSERT(!initialized); - timer = xTimerCreate( (const signed char *)"queueMonitorTimer", TIMER_PERIOD, + timer = xTimerCreate( "queueMonitorTimer", TIMER_PERIOD, pdTRUE, NULL, timerHandler ); xTimerStart(timer, 100); diff --git a/modules/src/stabilizer.c b/modules/src/stabilizer.c index c4e74993f5..adeab4b3bd 100644 --- a/modules/src/stabilizer.c +++ b/modules/src/stabilizer.c @@ -175,7 +175,7 @@ void stabilizerInit(void) pitchRateDesired = 0; yawRateDesired = 0; - xTaskCreate(stabilizerTask, (const signed char * const)STABILIZER_TASK_NAME, + xTaskCreate(stabilizerTask, STABILIZER_TASK_NAME, STABILIZER_TASK_STACKSIZE, NULL, STABILIZER_TASK_PRI, NULL); isInit = true; diff --git a/modules/src/system.c b/modules/src/system.c index 0e453544ef..04362c1874 100644 --- a/modules/src/system.c +++ b/modules/src/system.c @@ -78,7 +78,7 @@ static void systemTask(void *arg); /* Public functions */ void systemLaunch(void) { - xTaskCreate(systemTask, (const signed char * const)SYSTEM_TASK_NAME, + xTaskCreate(systemTask, SYSTEM_TASK_NAME, SYSTEM_TASK_STACKSIZE, NULL, SYSTEM_TASK_PRI, NULL); @@ -263,8 +263,6 @@ bool systemCanFly(void) void vApplicationIdleHook( void ) { - extern size_t debugPrintTCBInfo(void); - static uint32_t timeToPrint = M2T(5000); static uint32_t tickOfLatestWatchdogReset = M2T(0); portTickType tickCount = xTaskGetTickCount(); @@ -275,12 +273,6 @@ void vApplicationIdleHook( void ) watchdogReset(); } - if (tickCount - timeToPrint > M2T(10000)) - { - timeToPrint = tickCount; - debugPrintTCBInfo(); - } - // Enter sleep mode. Does not work when debugging chip with SWD. // Currently saves about 20mA STM32F405 current consumption (~30%). #ifndef DEBUG From 93619a201b95700d2de2922df8b15703875c8753 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Wed, 25 Nov 2015 17:26:14 +0100 Subject: [PATCH 56/58] Fix interrupt priority, fixes USB Interrupt priority are now documented in nvicconf.h Most are standardised in LOW, MID and HIGH priority. --- config/nvicconf.h | 49 +++++++++++++++++++++++++++++--- config/stm32f40x_i2c_cpal_conf.h | 8 ++++-- drivers/src/i2croutines.c | 8 +++--- drivers/src/uart_syslink.c | 16 +++++------ drivers/src/ws2812_cf2.c | 41 +++++++++++++------------- hal/src/uart.c | 4 +-- hal/src/usb_bsp.c | 12 ++++---- 7 files changed, 90 insertions(+), 48 deletions(-) diff --git a/config/nvicconf.h b/config/nvicconf.h index 2000e6fc48..3cf28a1af6 100644 --- a/config/nvicconf.h +++ b/config/nvicconf.h @@ -1,6 +1,6 @@ /** - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ @@ -33,12 +33,53 @@ #ifndef NVIC_CONF_H_ #define NVIC_CONF_H_ +/* + Interrupt priority organisation in Crazyflie: + + In Cortex-M low priority number is higher priority. Hence priority 0 is the + highest priority interrupt and priority 15 the lowest (STM32 implements + 4 priority bits) + + Interrupts bellow MAX_SYSCALL_INTERRUPT_PRIORITY cannot call any RTOS + functions! They should be handled like some kind of softdevice, running above + the OS. + + 3 Interrupt level are defined + - NVIC_LOW_PRI + - NVIC_MID_PRI + - NVIC_HIGH_PRI + The aim is to simplify interrupt handling and to document why any special case + is required. + + 15 - + 14 - + 13 - NVIC_LOW_PRI + 12 - NVIC_ADC_PRI + 11 - NVIC_RADIO_PRI + 10 - NVIC_MID_PRI + 9 - + 8 - + 7 - NVIC_HIGH_PRI + 6 - + 5 - <-- MAX_SYSCALL_INTERRUPT_PRIORITY + 4 ! NVIC_I2C_PRI_LOW NVIC_TRACE_TIM_PRI --- Does not call any RTOS function + 3 ! NVIC_I2C_PRI_HIGH + 2 ! + 1 ! + 0 ! +*/ + +// Standard interrupt levels +#define NVIC_LOW_PRI 13 +#define NVIC_MID_PRI 10 +#define NVIC_HIGH_PRI 7 + // Priorities used for Crazyflie -#define NVIC_I2C_PRI 3 +#define NVIC_I2C_HIGH_PRI 3 +#define NVIC_I2C_LOW_PRI 4 #define NVIC_TRACE_TIM_PRI 4 // Priorities for Crazyflie 2.0 -#define NVIC_UART_PRI 6 #define NVIC_RADIO_PRI 11 #define NVIC_ADC_PRI 12 diff --git a/config/stm32f40x_i2c_cpal_conf.h b/config/stm32f40x_i2c_cpal_conf.h index 164678d042..8336637863 100644 --- a/config/stm32f40x_i2c_cpal_conf.h +++ b/config/stm32f40x_i2c_cpal_conf.h @@ -533,19 +533,21 @@ /*-----------Interrupt Priority Offset-------------*/ +#include "nvicconf.h" + /* This defines can be used to decrease the Level of Interrupt Priority for I2Cx Device (ERR, EVT, DMA_TX, DMA_RX). The value of I2Cx_IT_OFFSET_SUBPRIO is added to I2Cx_IT_XXX_SUBPRIO and the value of I2Cx_IT_OFFSET_PREPRIO is added to I2Cx_IT_XXX_PREPRIO (XXX: ERR, EVT, DMATX, DMARX). I2Cx Interrupt Priority are defined in cpal_i2c_hal_stm32f10x.h file in Section 3 */ #define I2C1_IT_OFFSET_SUBPRIO 0 /* I2C1 SUB-PRIORITY Offset */ -#define I2C1_IT_OFFSET_PREPRIO 7 /* I2C1 PREEMPTION PRIORITY Offset */ +#define I2C1_IT_OFFSET_PREPRIO NVIC_HIGH_PRI /* I2C1 PREEMPTION PRIORITY Offset */ #define I2C2_IT_OFFSET_SUBPRIO 0 /* I2C2 SUB-PRIORITY Offset */ -#define I2C2_IT_OFFSET_PREPRIO 7 /* I2C2 PREEMPTION PRIORITY Offset */ +#define I2C2_IT_OFFSET_PREPRIO NVIC_HIGH_PRI /* I2C2 PREEMPTION PRIORITY Offset */ #define I2C3_IT_OFFSET_SUBPRIO 0 /* I2C3 SUB-PRIORITY Offset */ -#define I2C3_IT_OFFSET_PREPRIO 7 /* I2C3 PREEMPTION PRIORITY Offset */ +#define I2C3_IT_OFFSET_PREPRIO NVIC_HIGH_PRI /* I2C3 PREEMPTION PRIORITY Offset */ /*-----------------------------------------------------------------------------------------------------------------------*/ /*-----------------------------------------------------------------------------------------------------------------------*/ diff --git a/drivers/src/i2croutines.c b/drivers/src/i2croutines.c index 4b37a172f2..5034ce28ec 100644 --- a/drivers/src/i2croutines.c +++ b/drivers/src/i2croutines.c @@ -362,12 +362,12 @@ void I2C_LowLevel_Init(I2C_TypeDef* I2Cx) RCC_APB1PeriphResetCmd(RCC_APB1Periph_I2C1, DISABLE); NVIC_InitStructure.NVIC_IRQChannel = I2C1_EV_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_I2C_PRI; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_I2C_HIGH_PRI; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); NVIC_InitStructure.NVIC_IRQChannel = I2C1_ER_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_I2C_PRI + 1; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_I2C_LOW_PRI; NVIC_Init(&NVIC_InitStructure); } else /* I2Cx = I2C2 */ @@ -390,12 +390,12 @@ void I2C_LowLevel_Init(I2C_TypeDef* I2Cx) RCC_APB1PeriphResetCmd(RCC_APB1Periph_I2C2, DISABLE); NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_I2C_PRI; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_I2C_HIGH_PRI; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); NVIC_InitStructure.NVIC_IRQChannel = I2C2_ER_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_I2C_PRI + 1; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_I2C_LOW_PRI; NVIC_Init(&NVIC_InitStructure); } diff --git a/drivers/src/uart_syslink.c b/drivers/src/uart_syslink.c index 5948ec76e0..609e44bdf1 100644 --- a/drivers/src/uart_syslink.c +++ b/drivers/src/uart_syslink.c @@ -1,6 +1,6 @@ /** - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ @@ -93,7 +93,7 @@ void uartDmaInit(void) DMA_InitStructureShare.DMA_Channel = UART_DMA_CH; NVIC_InitStructure.NVIC_IRQChannel = UART_DMA_IRQ; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_UART_PRI; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_LOW_PRI; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); @@ -145,10 +145,9 @@ void uartInit(void) uartDmaInit(); - // TODO: Enable - // Configure Tx buffer empty interrupt + // Configure Rx buffer not empty interrupt NVIC_InitStructure.NVIC_IRQChannel = UART_IRQ; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_UART_PRI; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_HIGH_PRI; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); @@ -178,7 +177,7 @@ void uartInit(void) //Enable UART USART_Cmd(UART_TYPE, ENABLE); - + isInit = true; } @@ -227,7 +226,7 @@ void uartSendDataIsrBlocking(uint32_t size, uint8_t* data) int uartPutchar(int ch) { uartSendData(1, (uint8_t *)&ch); - + return (unsigned char)ch; } @@ -369,4 +368,3 @@ void __attribute__((used)) DMA2_Stream7_IRQHandler(void) { uartDmaIsr(); } - diff --git a/drivers/src/ws2812_cf2.c b/drivers/src/ws2812_cf2.c index 958741f4f8..6d5d521292 100644 --- a/drivers/src/ws2812_cf2.c +++ b/drivers/src/ws2812_cf2.c @@ -1,6 +1,6 @@ /** - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ @@ -30,6 +30,8 @@ // ST lib includes #include "stm32fxxx.h" +#include "nvicconf.h" + #include "FreeRTOS.h" #include "semphr.h" @@ -45,7 +47,7 @@ static xSemaphoreHandle allLedDone = NULL; // The minimum is to have 2 leds (1 per half buffer) in the buffer, this // consume 42Bytes and will trigger the DMA interrupt at ~2KHz. -// Putting 2 there will divide by 2 the interrupt frequency but will also +// Putting 2 there will divide by 2 the interrupt frequency but will also // double the memory consumption (no free lunch ;-) #define LED_PER_HALF 1 @@ -63,7 +65,7 @@ static union { void ws2812Init(void) { uint16_t PrescalerValue; - + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); /* GPIOB Configuration: TIM3 Channel 1 as alternate function push-pull */ @@ -82,7 +84,7 @@ void ws2812Init(void) //Map timer to alternate functions GPIO_PinAFConfig(GPIOB, GPIO_PinSource5, GPIO_AF_TIM3); - + /* Compute the prescaler value */ PrescalerValue = 0; /* Time base configuration */ @@ -130,7 +132,7 @@ void ws2812Init(void) DMA_Init(DMA1_Stream5, &DMA_InitStructure); NVIC_InitStructure.NVIC_IRQChannel = DMA1_Stream5_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 11; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_LOW_PRI; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); @@ -140,15 +142,15 @@ void ws2812Init(void) /* TIM3 CC2 DMA Request enable */ TIM_DMACmd(TIM3, TIM_DMA_CC2, ENABLE); - + vSemaphoreCreateBinary(allLedDone); - + } static void fillLed(uint16_t *buffer, uint8_t *color) { int i; - + for(i=0; i<8; i++) // GREEN data { buffer[i] = ((color[1]<NDTR = sizeof(led_dma.buffer) / sizeof(led_dma.buffer[0]); // load number of bytes to be transferred DMA_Cmd(DMA1_Stream5, ENABLE); // enable DMA channel 2 TIM_Cmd(TIM3, ENABLE); // Go!!! @@ -204,19 +206,19 @@ void ws2812DmaIsr(void) portBASE_TYPE xHigherPriorityTaskWoken; uint16_t * buffer; int i; - + if (total_led == 0) { TIM_Cmd(TIM3, DISABLE); DMA_Cmd(DMA1_Stream5, DISABLE); } - + if (DMA_GetITStatus(DMA1_Stream5, DMA_IT_HTIF5)) { DMA_ClearITPendingBit(DMA1_Stream5, DMA_IT_HTIF5); buffer = led_dma.begin; } - + if (DMA_GetITStatus(DMA1_Stream5, DMA_IT_TCIF5)) { DMA_ClearITPendingBit(DMA1_Stream5, DMA_IT_TCIF5); @@ -229,14 +231,13 @@ void ws2812DmaIsr(void) else bzero(buffer+(24*i), sizeof(led_dma.end)); } - + if (current_led >= total_led+2) { xSemaphoreGiveFromISR(allLedDone, &xHigherPriorityTaskWoken); - + TIM_Cmd(TIM3, DISABLE); // disable Timer 3 DMA_Cmd(DMA1_Stream5, DISABLE); // disable DMA stream4 - + total_led = 0; } } - diff --git a/hal/src/uart.c b/hal/src/uart.c index f0c5fa65ee..3e46ff3a1b 100644 --- a/hal/src/uart.c +++ b/hal/src/uart.c @@ -86,7 +86,7 @@ void uartDmaInit(void) DMA_InitStructureShare.DMA_M2M = DMA_M2M_Disable; NVIC_InitStructure.NVIC_IRQChannel = UART_DMA_IRQ; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_UART_PRI; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_LOW_PRI; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); @@ -133,7 +133,7 @@ void uartInit(void) #else // Configure Tx buffer empty interrupt NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_HIGH_PRI; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); diff --git a/hal/src/usb_bsp.c b/hal/src/usb_bsp.c index c043d43d24..b574b8c2d6 100644 --- a/hal/src/usb_bsp.c +++ b/hal/src/usb_bsp.c @@ -262,22 +262,22 @@ void USB_OTG_BSP_EnableInterrupt(USB_OTG_CORE_HANDLE *pdev) #else NVIC_InitStructure.NVIC_IRQChannel = OTG_FS_IRQn; #endif - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_MID_PRI; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); #ifdef USB_OTG_HS_DEDICATED_EP1_ENABLED NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); NVIC_InitStructure.NVIC_IRQChannel = OTG_HS_EP1_OUT_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_MID_PRI; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); NVIC_InitStructure.NVIC_IRQChannel = OTG_HS_EP1_IN_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_MID_PRI; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); #endif From 2ead4bf59b77ce36e6d07580af81f4c4f3a6bbb5 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Wed, 25 Nov 2015 17:33:54 +0100 Subject: [PATCH 57/58] Fix warning when creating task. Related to FreeRTOS 8 --- modules/src/sound_cf2.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/modules/src/sound_cf2.c b/modules/src/sound_cf2.c index f6b41be84a..bff189bf72 100644 --- a/modules/src/sound_cf2.c +++ b/modules/src/sound_cf2.c @@ -324,7 +324,7 @@ void soundInit(void) neffect = sizeof(effects)/sizeof(effects[0])-1; - timer = xTimerCreate( (const signed char *)"SoundTimer", M2T(10), + timer = xTimerCreate("SoundTimer", M2T(10), pdTRUE, NULL, soundTimer); xTimerStart(timer, 100); @@ -351,4 +351,3 @@ PARAM_ADD(PARAM_UINT32 | PARAM_RONLY, neffect, &neffect) PARAM_ADD(PARAM_UINT16, freq, &static_freq) PARAM_ADD(PARAM_UINT8, ratio, &static_ratio) PARAM_GROUP_STOP(sound) - From 6630b3aabcc1b88617cd7a69a0a698a8426cc427 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Thu, 26 Nov 2015 09:16:19 +0100 Subject: [PATCH 58/58] Fix queue monitoring and trace for FreeRTOS 8 --- config/trace.h | 8 ++++---- modules/src/queuemonitor.c | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/config/trace.h b/config/trace.h index 3311cc6d5c..65fa9ffb24 100644 --- a/config/trace.h +++ b/config/trace.h @@ -49,9 +49,9 @@ #define ITM_BLOCKING_ON_QUEUE_RECEIVE 0x0300 #define ITM_BLOCKING_ON_QUEUE_SEND 0x0400 -// #define traceQUEUE_SEND(xQueue) ITM_SEND(3, ITM_QUEUE_SEND | ((xQUEUE *) xQueue)->ucQueueNumber) -// #define traceQUEUE_SEND_FAILED(xQueue) ITM_SEND(3, ITM_QUEUE_FAILED | ((xQUEUE *) xQueue)->ucQueueNumber) -// #define traceBLOCKING_ON_QUEUE_RECEIVE(xQueue) ITM_SEND(3, ITM_BLOCKING_ON_QUEUE_RECEIVE | ((xQUEUE *) xQueue)->ucQueueNumber) -// #define traceBLOCKING_ON_QUEUE_SEND(xQueue) ITM_SEND(3, ITM_BLOCKING_ON_QUEUE_SEND | ((xQUEUE *) xQueue)->ucQueueNumber) +#define traceQUEUE_SEND(xQueue) ITM_SEND(3, ITM_QUEUE_SEND | ((xQUEUE *) xQueue)->uxQueueNumber) +#define traceQUEUE_SEND_FAILED(xQueue) ITM_SEND(3, ITM_QUEUE_FAILED | ((xQUEUE *) xQueue)->uxQueueNumber) +#define traceBLOCKING_ON_QUEUE_RECEIVE(xQueue) ITM_SEND(3, ITM_BLOCKING_ON_QUEUE_RECEIVE | ((xQUEUE *) xQueue)->uxQueueNumber) +#define traceBLOCKING_ON_QUEUE_SEND(xQueue) ITM_SEND(3, ITM_BLOCKING_ON_QUEUE_SEND | ((xQUEUE *) xQueue)->uxQueueNumber) #endif diff --git a/modules/src/queuemonitor.c b/modules/src/queuemonitor.c index 6e74036fa4..961851cec4 100644 --- a/modules/src/queuemonitor.c +++ b/modules/src/queuemonitor.c @@ -109,7 +109,7 @@ void qmRegisterQueue(xQueueHandle* xQueue, char* fileName, char* queueName) { } static Data* getQueueData(xQueueHandle* xQueue) { - unsigned char number = ucQueueGetQueueNumber(xQueue); + unsigned char number = uxQueueGetQueueNumber(xQueue); ASSERT(number < MAX_NR_OF_QUEUES); return &data[number]; } @@ -170,4 +170,4 @@ static void timerHandler(xTimerHandle timer) { debugPrint(); } -#endif // DEBUG_QUEUE_MONITOR \ No newline at end of file +#endif // DEBUG_QUEUE_MONITOR