From 6369c65dd8c42254ab59346c7ab20c74c54c0112 Mon Sep 17 00:00:00 2001 From: patacongo Date: Sun, 7 Aug 2011 12:58:54 +0000 Subject: Fix message queue/signal handling bugs git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@3849 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/ChangeLog | 8 ++ nuttx/arch/arm/src/Makefile | 4 +- nuttx/arch/arm/src/kinetis/chip.h | 24 ++-- nuttx/arch/arm/src/kinetis/kinetis_memorymap.h | 152 +++++++++++++++++++++++-- nuttx/sched/mq_waitirq.c | 76 ++++++++++--- nuttx/sched/os_internal.h | 1 + nuttx/sched/sig_initialize.c | 4 +- nuttx/sched/sig_received.c | 13 ++- 8 files changed, 236 insertions(+), 46 deletions(-) (limited to 'nuttx') diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog index 56d0d2ff4..d87323ee2 100644 --- a/nuttx/ChangeLog +++ b/nuttx/ChangeLog @@ -1990,4 +1990,12 @@ directories and files to armv7-m; Change name of of all CORTEXM3 constants to ARMV7M. This is a major namespace change needed to cleanly support the ARM Cortex-M4 which is also in the ARMv7 M Series (specifically, ARMv7E-M). + * sched/sig_initialize.c, sig_received.c, and mq_waitirq.c. Fixed several + critical bugs related to signal handling initialization and for signals + the wake up tasks that are waiting to send or receive message queues. In + the first two files, errors would prevent proper allocation of signal-related + structures from interrupt handlers. In the second, there was missing + "clean-up" logic after a signal occurred, leaving the message queue in + a bad state and resulting in PANICs. All are important. (submitted by + hkwilton). diff --git a/nuttx/arch/arm/src/Makefile b/nuttx/arch/arm/src/Makefile index 093ec0433..730d41ba1 100644 --- a/nuttx/arch/arm/src/Makefile +++ b/nuttx/arch/arm/src/Makefile @@ -37,10 +37,10 @@ -include chip/Make.defs ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src -ifeq ($(CONFIG_ARCH_CORTEXM3),y) /* Cortex-M3 is ARMv7-M */ +ifeq ($(CONFIG_ARCH_CORTEXM3),y) # Cortex-M3 is ARMv7-M ARCH_SUBDIR = armv7-m else -ifeq ($(CONFIG_ARCH_CORTEXM4),y) /* Cortex-M4 is ARMv7E-M */ +ifeq ($(CONFIG_ARCH_CORTEXM4),y) # Cortex-M4 is ARMv7E-M ARCH_SUBDIR = armv7-m else ARCH_SUBDIR = arm diff --git a/nuttx/arch/arm/src/kinetis/chip.h b/nuttx/arch/arm/src/kinetis/chip.h index 97d4601a9..93efce742 100755 --- a/nuttx/arch/arm/src/kinetis/chip.h +++ b/nuttx/arch/arm/src/kinetis/chip.h @@ -301,7 +301,7 @@ # undef KINETIS_NTAMPER /* No tamper detect */ # define KINETIS_NCRC 1 /* CRC */ -#elif defined(MK60N256VLL100) +#elif defined(CONFIG_ARCH_CHIP_MK60N256VLL100) # undef KIENTIS_K40 /* Not Kinetics K40 family */ # define KIENTIS_K60 1 /* Kinetis K60 family */ # define KINETIS_FLASH_SIZE (256*1024) /* 256Kb */ @@ -344,7 +344,7 @@ # undef KINETIS_NTAMPER /* No tamper detect */ # define KINETIS_NCRC 1 /* CRC */ -#elif defined(MK60X256VLL100) +#elif defined(CONFIG_ARCH_CHIP_MK60X256VLL100) # undef KIENTIS_K40 /* Not Kinetics K40 family */ # define KIENTIS_K60 1 /* Kinetis K60 family */ # define KINETIS_FLASH_SIZE (256*1024) /* 256Kb */ @@ -387,7 +387,7 @@ # undef KINETIS_NTAMPER /* No tamper detect */ # define KINETIS_NCRC 1 /* CRC */ -#elif defined(MK60N512VLL100) +#elif defined(CONFIG_ARCH_CHIP_MK60N512VLL100) # undef KIENTIS_K40 /* Not Kinetics K40 family */ # define KIENTIS_K60 1 /* Kinetis K60 family */ # define KINETIS_FLASH_SIZE (512*1024) /* 256Kb */ @@ -430,7 +430,7 @@ # undef KINETIS_NTAMPER /* No tamper detect */ # define KINETIS_NCRC 1 /* CRC */ -#elif defined(MK60N256VML100) +#elif defined(CONFIG_ARCH_CHIP_MK60N256VML100) # undef KIENTIS_K40 /* Not Kinetics K40 family */ # define KIENTIS_K60 1 /* Kinetis K60 family */ # define KINETIS_FLASH_SIZE (256*1024) /* 256Kb */ @@ -473,7 +473,7 @@ # undef KINETIS_NTAMPER /* No tamper detect */ # define KINETIS_NCRC 1 /* CRC */ -#elif defined(MK60X256VML100) +#elif defined(CONFIG_ARCH_CHIP_MK60X256VML100) # undef KIENTIS_K40 /* Not Kinetics K40 family */ # define KIENTIS_K60 1 /* Kinetis K60 family */ # define KINETIS_FLASH_SIZE (256*1024) /* 256Kb */ @@ -516,7 +516,7 @@ # undef KINETIS_NTAMPER /* No tamper detect */ # define KINETIS_NCRC 1 /* CRC */ -#elif defined(MK60N512VML100) +#elif defined(CONFIG_ARCH_CHIP_MK60N512VML100) # undef KIENTIS_K40 /* Not Kinetics K40 family */ # define KIENTIS_K60 1 /* Kinetis K60 family */ # define KINETIS_FLASH_SIZE (512*1024) /* 256Kb */ @@ -559,7 +559,7 @@ # undef KINETIS_NTAMPER /* No tamper detect */ # define KINETIS_NCRC 1 /* CRC */ -#elif defined(MK60N256VLQ100) +#elif defined(CONFIG_ARCH_CHIP_MK60N256VLQ100) # undef KIENTIS_K40 /* Not Kinetics K40 family */ # define KIENTIS_K60 1 /* Kinetis K60 family */ # define KINETIS_FLASH_SIZE (256*1024) /* 256Kb */ @@ -602,7 +602,7 @@ # undef KINETIS_NTAMPER /* No tamper detect */ # define KINETIS_NCRC 1 /* CRC */ -#elif defined(MK60X256VLQ100) +#elif defined(CONFIG_ARCH_CHIP_MK60X256VLQ100) # undef KIENTIS_K40 /* Not Kinetics K40 family */ # define KIENTIS_K60 1 /* Kinetis K60 family */ # define KINETIS_FLASH_SIZE (256*1024) /* 256Kb */ @@ -645,7 +645,7 @@ # undef KINETIS_NTAMPER /* No tamper detect */ # define KINETIS_NCRC 1 /* CRC */ -#elif defined(MK60N512VLQ100) +#elif defined(CONFIG_ARCH_CHIP_MK60N512VLQ100) # undef KIENTIS_K40 /* Not Kinetics K40 family */ # define KIENTIS_K60 1 /* Kinetis K60 family */ # define KINETIS_FLASH_SIZE (512*1024) /* 512Kb */ @@ -688,7 +688,7 @@ # undef KINETIS_NTAMPER /* No tamper detect */ # define KINETIS_NCRC 1 /* CRC */ -#elif defined(MK60N256VMD100) +#elif defined(CONFIG_ARCH_CHIP_MK60N256VMD100) # undef KIENTIS_K40 /* Not Kinetics K40 family */ # define KIENTIS_K60 1 /* Kinetis K60 family */ # define KINETIS_FLASH_SIZE (256*1024) /* 256Kb */ @@ -731,7 +731,7 @@ # undef KINETIS_NTAMPER /* No tamper detect */ # define KINETIS_NCRC 1 /* CRC */ -#elif defined(MK60X256VMD100) +#elif defined(CONFIG_ARCH_CHIP_MK60X256VMD100) # undef KIENTIS_K40 /* Not Kinetics K40 family */ # define KIENTIS_K60 1 /* Kinetis K60 family */ # define KINETIS_FLASH_SIZE (256*1024) /* 256Kb */ @@ -774,7 +774,7 @@ # undef KINETIS_NTAMPER /* No tamper detect */ # define KINETIS_NCRC 1 /* CRC */ -#elif defined(MK60N512VMD100) +#elif defined(CONFIG_ARCH_CHIP_MK60N512VMD100) # undef KIENTIS_K40 /* Not Kinetics K40 family */ # define KIENTIS_K60 1 /* Kinetis K60 family */ # define KINETIS_FLASH_SIZE (512*1024) /* 512Kb */ diff --git a/nuttx/arch/arm/src/kinetis/kinetis_memorymap.h b/nuttx/arch/arm/src/kinetis/kinetis_memorymap.h index f172d6742..9a495f442 100755 --- a/nuttx/arch/arm/src/kinetis/kinetis_memorymap.h +++ b/nuttx/arch/arm/src/kinetis/kinetis_memorymap.h @@ -49,7 +49,11 @@ ************************************************************************************/ /* Memory Map ***********************************************************************/ -/* The memory for the following parts is defined in Freescale document K40P144M100SF2RM */ +/* K40 Family + * + * The memory map for the following parts is defined in Freescale document + * K40P144M100SF2RM + */ #if defined(CONFIG_ARCH_CHIP_MK40X128VLQ100) || defined(CONFIG_ARCH_CHIP_MK40X128VMD100) \ defined(CONFIG_ARCH_CHIP_MK40X256VLQ100) || defined(CONFIG_ARCH_CHIP_MK40X256VMD100) \ @@ -58,7 +62,7 @@ # define KINETIS_FLASH_BASE 0x00000000 /* –0x0fffffff Program flash and read- * only data (Includes exception * vectors in first 1024 bytes) */ -# if !defined(CONFIG_ARCH_CHIP_MK40N512VLQ100) && !defined(CONFIG_ARCH_CHIP_MK40N512VMD100) +# if !defined(KINETIS_FLEXMEM_SIZE) # define KINETIS_FLEXNVM_BASE 0x10000000 /* –0x13ffffff FlexNVM */ # define KINETIS_FLEXRAM_BASE 0x14000000 /* –0x17ffffff FlexRAM */ # endif @@ -79,14 +83,14 @@ # define KINETIS_ALIAS1_BASE 0x42000000 /* –0x43ffffff Aliased to peripheral bridge * (AIPS-Lite) and general purpose * input/output (GPIO) bitband */ - /* 0x40400000 * –0x5fffffff Reserved */ + /* 0x44000000 * –0x5fffffff Reserved */ # define KINETIS_FLEXBUS_WBBASE 0x60000000 /* –0x7fffffff FlexBus (External Memory - * Write-back) */ # define KINETIS_FLEXBUS_WTBASE 0x80000000 /* –0x9fffffff FlexBus (External Memory - * Write-through) */ # define KINETIS_FLEXBUS_NXBASE 0xa0000000 /* –0xdfffffff FlexBus (External Memory - * Non-executable) */ -# define KINETIS_PERIPH_BASE 0xe000000 /* –0xe00fffff Private peripherals */ +# define KINETIS_PERIPH_BASE 0xe0000000 /* –0xe00fffff Private peripherals */ /* 0xe0100000 * –0xffffffff Reserved */ /* Peripheral Bridge 0 Memory Map ***************************************************/ @@ -107,6 +111,7 @@ # define KINETIS_CRC_BASE 0x40032000 /* CRC */ # define KINETIS_USBDCD_BASE 0x40035000 /* USB DCD */ # define KINETIS_PDB_BASE 0x40036000 /* Programmable delay block */ +# define KINETIS_PIT_BASE 0x40037000 /* Periodic interrupt timers (PIT) */ # define KINETIS_FTM0_BASE 0x40038000 /* FlexTimer 0 */ # define KINETIS_FTM1_BASE 0x40039000 /* FlexTimer 1 */ # define KINETIS_ADC0_BASE 0x4003b000 /* Analog-to-digital converter (ADC) 0 */ @@ -125,7 +130,7 @@ # define KINETIS_PDMUX_BASE 0x4004c000 /* Port D multiplexing control */ # define KINETIS_PEMUX_BASE 0x4004d000 /* Port E multiplexing control */ # define KINETIS_SWWDOG_BASE 0x40052000 /* Software watchdog */ -# define KINETIS_EXTWDOG_BASE 0x40061000 /* Software watchdog */ +# define KINETIS_EXTWDOG_BASE 0x40061000 /* External watchdog */ # define KINETIS_CMT_BASE 0x40062000 /* Carrier modulator timer (CMT) */ # define KINETIS_MCG_BASE 0x40064000 /* Multi-purpose Clock Generator (MCG) */ # define KINETIS_OSC_BASE 0x40065000 /* System oscillator (OSC) */ @@ -172,15 +177,138 @@ # define KINETIS_TFUN_BASE 0xe0043000 /* Embedded Trace Funnel */ # define KINETIS_MISC_BASE 0xe0080000 /* Miscellaneous Control Module (including ETB Almost Full) */ # define KINETIS_ROMTAB_BASE 0xe00ff000 /* ROM Table - allows auto-detection of debug components */ -# define KINETIS_CRC_BASE 0xe00c0000 /* CRC */ -# define KINETIS_CRC_BASE 0xe00c0000 /* CRC */ -# define KINETIS_CRC_BASE 0xe00c0000 /* CRC */ -# define KINETIS_CRC_BASE 0xe00c0000 /* CRC */ + +/* K60 Family + * + * The memory map for the following parts is defined in Freescale document + * K60P144M100SF2RM + */ + +#elif defined(CONFIG_ARCH_CHIP_MK60N256VLQ100) || defined(CONFIG_ARCH_CHIP_MK60X256VLQ100) \ + defined(CONFIG_ARCH_CHIP_MK60N512VLQ100) || defined(CONFIG_ARCH_CHIP_MK60N256VMD100) \ + defined(CONFIG_ARCH_CHIP_MK60X256VMD100) || defined(CONFIG_ARCH_CHIP_MK60N512VMD100) + +# define KINETIS_FLASH_BASE 0x00000000 /* –0x0fffffff Program flash and read- + * only data (Includes exception + * vectors in first 1024 bytes) */ +# if !defined(KINETIS_FLEXMEM_SIZE) +# define KINETIS_FLEXNVM_BASE 0x10000000 /* –0x13ffffff FlexNVM */ +# define KINETIS_FLEXRAM_BASE 0x14000000 /* –0x17ffffff FlexRAM */ +# endif +# define KINETIS_SRAML_BASE 0x18000000 /* –0x1fffffff SRAM_L: Lower SRAM + * (ICODE/DCODE) */ +# define KINETIS_SRAMU_BASE 0x20000000 /* –0x200fffff SRAM_U: Upper SRAM bitband + * region */ + /* 0x20100000 * –0x21ffffff Reserved */ +# define KINETIS_ALIAS1_BASE 0x22000000 /* –0x23ffffff Aliased to SRAM_U bitband */ + /* 0x24000000 * –0x3fffffff Reserved */ +# define KINETIS_BRIDGE0_BASE 0x40000000 /* –0x4007ffff Bitband region for peripheral + * bridge 0 (AIPS-Lite0) */ +# define KINETIS_BRIDGE1_BASE 0x40080000 /* –0x400fffff Bitband region for peripheral + * bridge 1 (AIPS-Lite1) */ +# define KINETIS_GPIOBB_BASE 0x400ff000 /* –0x400fffff Bitband region for general + * purpose input/output (GPIO) */ + /* 0x40100000 * –0x41ffffff Reserved */ +# define KINETIS_ALIAS1_BASE 0x42000000 /* –0x43ffffff Aliased to peripheral bridge + * (AIPS-Lite) and general purpose + * input/output (GPIO) bitband */ + /* 0x44000000 * –0x5fffffff Reserved */ +# define KINETIS_FLEXBUS_BASE 0x60000000 /* –0x7fffffff FlexBus */ +# define KINETIS_PERIPH_BASE 0xe0000000 /* –0xe00fffff Private peripherals */ + /* 0xe0100000 * –0xffffffff Reserved */ + +/* Peripheral Bridge 0 Memory Map ***************************************************/ + +# define KINETIS_PBRIDGE0_BASE 0x40000000 /* Peripheral bridge 0 (AIPS-Lite 0) */ +# define KINETIS_XBAR_BASE 0x40004000 /* Crossbar switch */ +# define KINETIS_DMAC_BASE 0x40008000 /* DMA controller */ +# define KINETIS_DMADESC_BASE 0x40009000 /* DMA controller transfer control descriptors */ +# define KINETIS_FLEXBUS_BASE 0x4000c000 /* FlexBus */ +# define KINETIS_MPU_BASE 0x4000d000 /* MPU */ +# define KINETIS_FLASHC_BASE 0x4001f000 /* Flash memory controller */ +# define KINETIS_FLASH_BASE 0x40020000 /* Flash memory */ +# define KINETIS_DMAMUX0_BASE 0x40021000 /* DMA channel mutiplexer 0 */ +# define KINETIS_FLEXCAN0_BASE 0x40024000 /* FlexCAN 0 */ +# define KINETIS_SPI0_BASE 0x4002c000 /* DSPI 0 */ +# define KINETIS_SPI1_BASE 0x4002d000 /* DSPI 1 */ +# define KINETIS_I2S0_BASE 0x4002f000 /* I2S 0 */ +# define KINETIS_CRC_BASE 0x40032000 /* CRC */ +# define KINETIS_USBDCD_BASE 0x40035000 /* USB DCD */ +# define KINETIS_PDB_BASE 0x40036000 /* Programmable delay block */ +# define KINETIS_PIT_BASE 0x40037000 /* Periodic interrupt timers (PIT) */ +# define KINETIS_FTM0_BASE 0x40038000 /* FlexTimer (FTM) 0 */ +# define KINETIS_FTM1_BASE 0x40039000 /* FlexTimer (FTM) 1 */ +# define KINETIS_ADC0_BASE 0x4003b000 /* Analog-to-digital converter (ADC) 0 */ +# define KINETIS_RTC_BASE 0x4003d000 /* Real time clock */ +# define KINETIS_VBATR_BASE 0x4003e000 /* VBAT register file */ +# define KINETIS_LPTMR_BASE 0x40040000 /* Low power timer */ +# define KINETIS_SYSR_BASE 0x40041000 /* System register file */ +# define KINETIS_DRYICE_BASE 0x40042000 /* DryIce */ +# define KINETIS_DRYICESS_BASE 0x40043000 /* DryIce secure storage */ +# define KINETIS_TSI_BASE 0x40045000 /* Touch sense interface */ +# define KINETIS_SIMLP_BASE 0x40047000 /* SIM low-power logic */ +# define KINETIS_SIM_BASE 0x40048000 /* System integration module (SIM) */ +# define KINETIS_PAMUX_BASE 0x40049000 /* Port A multiplexing control */ +# define KINETIS_PBMUX_BASE 0x4004a000 /* Port B multiplexing control */ +# define KINETIS_PCMUX_BASE 0x4004b000 /* Port C multiplexing control */ +# define KINETIS_PDMUX_BASE 0x4004c000 /* Port D multiplexing control */ +# define KINETIS_PEMUX_BASE 0x4004d000 /* Port E multiplexing control */ +# define KINETIS_SWWDOG_BASE 0x40052000 /* Software watchdog */ +# define KINETIS_EXTWDOG_BASE 0x40061000 /* External watchdog */ +# define KINETIS_CMT_BASE 0x40062000 /* Carrier modulator timer (CMT) */ +# define KINETIS_MCG_BASE 0x40064000 /* Multi-purpose Clock Generator (MCG) */ +# define KINETIS_OSC_BASE 0x40065000 /* System oscillator (XOSC) */ +# define KINETIS_I2C0_BASE 0x40066000 /* I2C 0 */ +# define KINETIS_I2C1_BASE 0x40067000 /* I2C 1 */ +# define KINETIS_UART0_BASE 0x4006a000 /* UART0 */ +# define KINETIS_UART1_BASE 0x4006b000 /* UART1 */ +# define KINETIS_UART2_BASE 0x4006c000 /* UART2 */ +# define KINETIS_UART3_BASE 0x4006d000 /* UART3 */ +# define KINETIS_USBOTG_BASE 0x40072000 /* USB OTG FS/LS */ +# define KINETIS_CMP_BASE 0x40073000 /* Analog comparator (CMP) / 6-bit digital-to-analog converter (DAC) */ +# define KINETIS_VREF_BASE 0x40074000 /* Voltage reference (VREF) */ +# define KINETIS_LLWU_BASE 0x4007c000 /* Low-leakage wakeup unit (LLWU) */ +# define KINETIS_PMC_BASE 0x4007d000 /* Power management controller (PMC) */ +# define KINETIS_SMC_BASE 0x4007e000 /* System Mode controller (SMC) */ + +/* Peripheral Bridge 1 Memory Map ***************************************************/ + +# define KINETIS_PBRIDGE1_BASE 0x40080000 /* Peripheral bridge 1 (AIPS-Lite 1) */ +# define KINETIS_RNGB_BASE 0x400a0000 /* Random number generator (RNGB) */ +# define KINETIS_FLEXCAN1_BASE 0x400a4000 /* FlexCAN 1 */ +# define KINETIS_SPI2_BASE 0x400ac000 /* DSPI 2 */ +# define KINETIS_SDHC_BASE 0x400b1000 /* SDHC */ +# define KINETIS_FTM2_BASE 0x400b8000 /* FlexTimer 2 */ +# define KINETIS_ADC1_BASE 0x400bb000 /* Analog-to-digital converter (ADC) 1 */ +# define KINETIS_EMAC_BASE 0x400c0000 /* Ethernet MAC and IEEE 1588 timers */ +# define KINETIS_DAC0_BASE 0x400cc000 /* 12-bit digital-to-analog converter (DAC) 0 */ +# define KINETIS_DAC1_BASE 0x400cd000 /* 12-bit digital-to-analog converter (DAC) 1 */ +# define KINETIS_UART4_BASE 0x400ea000 /* UART4 */ +# define KINETIS_UART5_BASE 0x400eb000 /* UART5 */ +# define KINETIS_XBAR_BASE 0x400ff000 /* Not an AIPS-Lite slot. The 32-bit general + * purpose input/output module that shares the + * crossbar switch slave port with the AIPS-Lite + * is accessed at this address. */ + +/* Private Peripheral Bus (PPB) Memory Map ******************************************/ + +# define KINETIS_ITM_BASE 0xe0000000 /* Instrumentation Trace Macrocell (ITM) */ +# define KINETIS_DWT_BASE 0xe0001000 /* Data Watchpoint and Trace (DWT) */ +# define KINETIS_FPB_BASE 0xe0002000 /* Flash Patch and Breakpoint (FPB) */ +# define KINETIS_SCS_BASE 0xe000e000 /* System Control Space (SCS) (for NVIC) */ +# define KINETIS_TPIU_BASE 0xe0040000 /* Trace Port Interface Unit (TPIU) */ +# define KINETIS_ETM_BASE 0xe0041000 /* Embedded Trace Macrocell (ETM) */ +# define KINETIS_ETB_BASE 0xe0042000 /* Embedded Trace Buffer (ETB) */ +# define KINETIS_TFUN_BASE 0xe0043000 /* Embedded Trace Funnel */ +# define KINETIS_MISC_BASE 0xe0080000 /* Miscellaneous Control Module (including ETB Almost Full) */ +# define KINETIS_MMCAU_BASE 0xe0081000 /* Memory Mapped Cryptographic Acceleration Unit (MMCAU) */ +# define KINETIS_ROMTAB_BASE 0xe00ff000 /* ROM Table - allows auto-detection of debug components */ #else - /* The memory map for other parts is defined in other documents and may or may net - * be the same as above. This error means that you have to look at the document and - * determine that for yourself. + /* The memory map for other parts is defined in other documents and may or may not + * be the same as above (the family members are all very similar) This error just + * means that you have to look at the document and determine for yourself if the + * memory map is the same. */ # error "No memory map for this Kinetis part" diff --git a/nuttx/sched/mq_waitirq.c b/nuttx/sched/mq_waitirq.c index 40bd88e24..9d436c280 100644 --- a/nuttx/sched/mq_waitirq.c +++ b/nuttx/sched/mq_waitirq.c @@ -1,7 +1,7 @@ /**************************************************************************** * sched/mq_waitirq.c * - * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Copyright (C) 2007-2009, 2011 Gregory Nutt. All rights reserved. * Author: Gregory Nutt * * Redistribution and use in source and binary forms, with or without @@ -41,8 +41,11 @@ #include #include + #include -#include "sem_internal.h" +#include + +#include "os_internal.h" /**************************************************************************** * Definitions @@ -72,14 +75,13 @@ * Function: sem_waitirq * * Description: - * This function is called when a signal is received by a - * task that is waiting on a message queue -- either for a - * queue to becoming not full (on mq_send) or not empty - * (on mq_receive). + * This function is called when a signal is received by a task that is + * waiting on a message queue -- either for a queue to becoming not full + * (on mq_send) or not empty (on mq_receive). * * Parameters: - * wtcb - A pointer to the TCB of the task that is waiting - * on a message queue, but has received a signal instead. + * wtcb - A pointer to the TCB of the task that is waiting on a message + * queue, but has received a signal instead. * * Return Value: * None @@ -90,22 +92,70 @@ void mq_waitirq(FAR _TCB *wtcb) { + FAR msgq_t *msgq; irqstate_t saved_state; - /* Disable interrupts. This is necessary because an - * interrupt handler may attempt to send a message while we are - * doing this. + /* Disable interrupts. This is necessary because an interrupt handler may + * attempt to send a message while we are doing this. */ saved_state = irqsave(); - /* It is possible that an interrupt/context switch beat us to the - * punch and already changed the task's state. + /* It is possible that an interrupt/context switch beat us to the punch and + * already changed the task's state. NOTE: The operations within the if + * are safe because interrupts are always disabled with the msgwaitq, + * nwaitnotempty, and nwaitnotfull fields are modified. */ if (wtcb->task_state == TSTATE_WAIT_MQNOTEMPTY || wtcb->task_state == TSTATE_WAIT_MQNOTFULL) { + /* Get the message queue associated with the waiter from the TCB */ + + msgq = wtcb->msgwaitq; +#ifdef CONFIG_DEBUG + if (!msgq) + { + /* In these states there must always be an associated message queue */ + + PANIC((uint32_t)OSERR_MQNOWAITER); + } +#endif + wtcb->msgwaitq = NULL; + + /* Decrement the count of waiters and cancel the wait */ + + if (wtcb->task_state == TSTATE_WAIT_MQNOTEMPTY) + { +#ifdef CONFIG_DEBUG + if (msgq->nwaitnotempty <= 0) + { + /* This state, there should be a positive, non-zero waiter + * count. + */ + + PANIC((uint32_t)OSERR_MQNONEMPTYCOUNT); + + } +#endif + msgq->nwaitnotempty--; + } + else + { +#ifdef CONFIG_DEBUG + if (msgq->nwaitnotfull <= 0) + { + /* This state, there should be a positive, non-zero waiter + * count. + */ + + PANIC((uint32_t)OSERR_MQNOTFULLCOUNT); + + } +#endif + msgq->nwaitnotfull--; + } + /* Mark the errno value for the thread. */ wtcb->pterrno = EINTR; diff --git a/nuttx/sched/os_internal.h b/nuttx/sched/os_internal.h index 3287507a9..4ad8f087b 100644 --- a/nuttx/sched/os_internal.h +++ b/nuttx/sched/os_internal.h @@ -67,6 +67,7 @@ enum os_crash_codes_e OSERR_NOIDLETASK, /* There is no idle task */ OSERR_MQNONEMPTYCOUNT, /* Expected waiter for non-empty queue */ OSERR_MQNOTFULLCOUNT, /* Expected waiter for non-full queue */ + OSERR_MQNOWAITER, /* Expected a queue for the waiter */ OSERR_BADWAITSEM, /* Already waiting for a semaphore */ OSERR_BADMSGTYPE, /* Tried to free a bad message type */ OSERR_FAILEDTOADDSIGNAL, /* Failed to add pending signal */ diff --git a/nuttx/sched/sig_initialize.c b/nuttx/sched/sig_initialize.c index a0e762744..4c225350a 100644 --- a/nuttx/sched/sig_initialize.c +++ b/nuttx/sched/sig_initialize.c @@ -1,7 +1,7 @@ /************************************************************************ * sched/sig_initialize.c * - * Copyright (C) 2007, 2009 Gregory Nutt. All rights reserved. + * Copyright (C) 2007, 2009, 2011 Gregory Nutt. All rights reserved. * Author: Gregory Nutt * * Redistribution and use in source and binary forms, with or without @@ -223,7 +223,7 @@ void sig_initialize(void) NUM_PENDING_ACTIONS, SIG_ALLOC_FIXED); g_sigpendingirqactionalloc = - sig_allocateblock(&g_sigpendingaction, + sig_allocateblock(&g_sigpendingirqaction, NUM_PENDING_INT_ACTIONS, SIG_ALLOC_IRQ); sig_allocateactionblock(); diff --git a/nuttx/sched/sig_received.c b/nuttx/sched/sig_received.c index 56f7f1dea..1aa76c900 100644 --- a/nuttx/sched/sig_received.c +++ b/nuttx/sched/sig_received.c @@ -178,12 +178,15 @@ static FAR sigpendq_t *sig_allocatependingsignal(void) /* Try to get the pending signal structure from the free list */ sigpend = (FAR sigpendq_t*)sq_remfirst(&g_sigpendingsignal); + if (!sigpend) + { + /* If no pending signal structure is available in the free list, + * then try the special list of structures reserved for + * interrupt handlers + */ - /* If so, then try the special list of structures reserved for - * interrupt handlers - */ - - sigpend = (FAR sigpendq_t*)sq_remfirst(&g_sigpendingirqsignal); + sigpend = (FAR sigpendq_t*)sq_remfirst(&g_sigpendingirqsignal); + } } /* If we were not called from an interrupt handler, then we are -- cgit v1.2.3