From dc6b688a6ac484c5f578bfa4acc39a93b8956606 Mon Sep 17 00:00:00 2001 From: Werner Stern Date: Tue, 2 May 2017 11:50:42 +0200 Subject: [PATCH] fixed px4io firmware read-modify-write bug --- src/modules/px4iofirmware/controls.c | 40 ++++++++++++++-------------- src/modules/px4iofirmware/mixer.cpp | 22 +++++++-------- src/modules/px4iofirmware/px4io.h | 2 ++ src/modules/px4iofirmware/safety.c | 4 +-- 4 files changed, 35 insertions(+), 33 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 72ea8445ec..2c66cac9bb 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -116,7 +116,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *rssi = st24_rssi; r_raw_rc_count = st24_channel_count; - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24; + PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } @@ -141,7 +141,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool /* not setting RSSI since SUMD does not provide one */ r_raw_rc_count = sumd_channel_count; - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD; + PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); if (sumd_failsafe_state) { @@ -233,15 +233,15 @@ controls_tick() (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated); if (dsm_updated) { - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM); } if (st24_updated) { - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24; + PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24); } if (sumd_updated) { - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD; + PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD); } perf_end(c_gather_dsm); @@ -253,7 +253,7 @@ controls_tick() PX4IO_RC_INPUT_CHANNELS); if (sbus_updated) { - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; + PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS); unsigned sbus_rssi = RC_INPUT_RSSI_MAX; @@ -291,7 +291,7 @@ controls_tick() if (ppm_updated) { - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; + PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } @@ -424,7 +424,7 @@ controls_tick() } /* set RC OK flag, as we got an update */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK); r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_OK; /* if we have enough channels (5) to control the vehicle, the mapping is ok */ @@ -449,10 +449,10 @@ controls_tick() rc_input_lost = true; /* clear the input-kind flags here */ - r_status_flags &= ~( - PX4IO_P_STATUS_FLAGS_RC_PPM | - PX4IO_P_STATUS_FLAGS_RC_DSM | - PX4IO_P_STATUS_FLAGS_RC_SBUS); + PX4_CRITICAL_SECTION(r_status_flags &= ~( + PX4IO_P_STATUS_FLAGS_RC_PPM | + PX4IO_P_STATUS_FLAGS_RC_DSM | + PX4IO_P_STATUS_FLAGS_RC_SBUS)); } @@ -462,15 +462,15 @@ controls_tick() /* if we are in failsafe, clear the override flag */ if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) { - r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); + PX4_CRITICAL_SECTION(r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE)); } /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ - r_status_flags &= ~( - PX4IO_P_STATUS_FLAGS_OVERRIDE | - PX4IO_P_STATUS_FLAGS_RC_OK); + PX4_CRITICAL_SECTION(r_status_flags &= ~( + PX4IO_P_STATUS_FLAGS_OVERRIDE | + PX4IO_P_STATUS_FLAGS_RC_OK)); /* flag raw RC as lost */ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK); @@ -482,7 +482,7 @@ controls_tick() r_raw_rc_count = 0; /* Set the RC_LOST alarm */ - r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; + PX4_CRITICAL_SECTION(r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST); } /* @@ -525,14 +525,14 @@ controls_tick() } if (override) { - r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; + PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE); } else { - r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); + PX4_CRITICAL_SECTION(r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE)); } } else { - r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); + PX4_CRITICAL_SECTION(r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE)); } } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 7eb302ed6e..d279a8fd0b 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -129,14 +129,14 @@ mixer_tick(void) isr_debug(1, "AP RX timeout"); } - r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK); - r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; + PX4_CRITICAL_SECTION(r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK)); + PX4_CRITICAL_SECTION(r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST); } else { - r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; + PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK); /* this flag is never cleared once OK */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED; + PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED); if (system_state.fmu_data_received_time > last_fmu_update) { new_fmu_data = true; @@ -231,7 +231,7 @@ mixer_tick(void) should_arm && /* and FMU is initialized */ (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) { - r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + PX4_CRITICAL_SECTION(r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); } /* @@ -245,10 +245,10 @@ mixer_tick(void) * Set failsafe status flag depending on mixing source */ if (source == MIX_FAILSAFE) { - r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; + PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE); } else { - r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); + PX4_CRITICAL_SECTION(r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE)); } /* @@ -336,14 +336,14 @@ mixer_tick(void) /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; - r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; + PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); isr_debug(5, "> PWM enabled"); } else if (!needs_to_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; - r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); + PX4_CRITICAL_SECTION(r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)); isr_debug(5, "> PWM disabled"); } @@ -501,7 +501,7 @@ mixer_handle_text(const void *buffer, size_t length) } /* disable mixing, will be enabled once load is complete */ - r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_MIXER_OK); + PX4_CRITICAL_SECTION(r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_MIXER_OK)); /* abort if we're in the mixer - the caller is expected to retry */ if (in_mixer) { @@ -532,7 +532,7 @@ mixer_handle_text(const void *buffer, size_t length) /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + PX4_CRITICAL_SECTION(r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK); return 0; } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 5ca8564a79..ba0e3cf48e 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -201,6 +201,8 @@ extern pwm_limit_t pwm_limit; #define CONTROL_PAGE_INDEX(_group, _channel) (_group * PX4IO_CONTROL_CHANNELS + _channel) +#define PX4_CRITICAL_SECTION(cmd) { irqstate_t flags = px4_enter_critical_section(); cmd; px4_leave_critical_section(flags); } + /* * Mixer */ diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index f4ab74a45a..93715eb9dc 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -117,7 +117,7 @@ safety_check_button(void *arg) } else if (counter == ARM_COUNTER_THRESHOLD) { /* switch to armed state */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF); counter++; } @@ -128,7 +128,7 @@ safety_check_button(void *arg) } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + PX4_CRITICAL_SECTION(r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF); counter++; }