int attach_child(pid_t pid, const char *pty, int force_stdio) { struct ptrace_child child; child_addr_t scratch_page = -1; int *child_tty_fds = NULL, n_fds, child_fd, statfd = -1; int i; int err = 0; long page_size = sysconf(_SC_PAGE_SIZE); #ifdef __linux__ char stat_path[PATH_MAX]; #endif if ((err = check_pgroup(pid))) { return err; } if ((err = preflight_check(pid))) { return err; } debug("Using tty: %s", pty); if ((err = copy_tty_state(pid, pty))) { if (err == ENOTTY && !force_stdio) { error("Target is not connected to a terminal.\n" " Use -s to force attaching anyways."); return err; } } #ifdef __linux__ snprintf(stat_path, sizeof stat_path, "/proc/%d/stat", pid); statfd = open(stat_path, O_RDONLY); if (statfd < 0) { error("Unable to open %s: %s", stat_path, strerror(errno)); return -statfd; } #endif kill(pid, SIGTSTP); wait_for_stop(pid, statfd); if ((err = grab_pid(pid, &child, &scratch_page))) { goto out_cont; } if (force_stdio) { child_tty_fds = malloc(3 * sizeof(int)); if (!child_tty_fds) { err = ENOMEM; goto out_unmap; } n_fds = 3; child_tty_fds[0] = 0; child_tty_fds[1] = 1; child_tty_fds[2] = 2; } else { child_tty_fds = get_child_tty_fds(&child, statfd, &n_fds); if (!child_tty_fds) { err = child.error; goto out_unmap; } } if (ptrace_memcpy_to_child(&child, scratch_page, pty, strlen(pty) + 1)) { err = child.error; error("Unable to memcpy the pty path to child."); goto out_free_fds; } child_fd = do_syscall(&child, openat, -1, scratch_page, O_RDWR | O_NOCTTY, 0, 0, 0); if (child_fd < 0) { err = child_fd; error("Unable to open the tty in the child."); goto out_free_fds; } debug("Opened the new tty in the child: %d", child_fd); err = ignore_hup(&child, scratch_page); if (err < 0) goto out_close; err = do_syscall(&child, getsid, 0, 0, 0, 0, 0, 0); if (err != child.pid) { debug("Target is not a session leader, attempting to setsid."); err = do_setsid(&child); } else { do_syscall(&child, ioctl, child_tty_fds[0], TIOCNOTTY, 0, 0, 0, 0); } if (err < 0) goto out_close; err = do_syscall(&child, ioctl, child_fd, TIOCSCTTY, 1, 0, 0, 0); if (err != 0) { /* Seems to be returning >0 for error */ error("Unable to set controlling terminal: %s", strerror(err)); goto out_close; } debug("Set the controlling tty"); for (i = 0; i < n_fds; i++) { err = do_dup2(&child, child_fd, child_tty_fds[i]); if (err < 0) error("Problem moving child fd number %d to new tty: %s", child_tty_fds[i], strerror(errno)); } err = 0; out_close: do_syscall(&child, close, child_fd, 0, 0, 0, 0, 0); out_free_fds: free(child_tty_fds); out_unmap: do_unmap(&child, scratch_page, page_size); ptrace_restore_regs(&child); ptrace_detach_child(&child); if (err == 0) { kill(child.pid, SIGSTOP); wait_for_stop(child.pid, statfd); } kill(child.pid, SIGWINCH); out_cont: kill(child.pid, SIGCONT); #ifdef __linux__ close(statfd); #endif return err < 0 ? -err : err; }
int steal_pty(pid_t pid, int *pty) { int err = 0; struct steal_pty_state steal = {}; long page_size = sysconf(_SC_PAGE_SIZE); if ((err = preflight_check(pid))) goto out; if ((err = get_terminal_state(&steal, pid))) goto out; if ((err = setup_steal_socket(&steal))) goto out; debug("Listening on socket: %s", steal.addr_un.sun_path); debug("Attaching terminal emulator pid=%d", steal.emulator_pid); if ((err = grab_pid(steal.emulator_pid, &steal.child, &steal.child_scratch))) goto out; debug("Attached to terminal emulator (pid %d)", (int)steal.emulator_pid); if ((err = find_master_fd(&steal))) { error("Unable to find the fd for the pty!"); goto out; } if ((err = setup_steal_socket_child(&steal))) goto out; if ((err = steal_child_pty(&steal))) goto out; if ((err = steal_block_hup(&steal))) goto out; if ((err = steal_cleanup_child(&steal))) goto out; goto out_no_child; out: if (steal.ptyfd) { close(steal.ptyfd); steal.ptyfd = 0; } if (steal.child_fd > 0) do_syscall(&steal.child, close, steal.child_fd, 0, 0, 0, 0, 0); if (steal.child_scratch > 0) do_unmap(&steal.child, steal.child_scratch, page_size); if (steal.child.state != ptrace_detached) { ptrace_restore_regs(&steal.child); ptrace_detach_child(&steal.child); } out_no_child: if (steal.sockfd > 0) { close(steal.sockfd); unlink(steal.addr_un.sun_path); } if (steal.tmpdir[0]) { rmdir(steal.tmpdir); } if (steal.ptyfd) *pty = steal.ptyfd; free(steal.master_fds.fds); return err; }
transition_result_t arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status const struct safety_s *safety, ///< current safety settings arming_state_t new_arming_state, ///< arming state requested struct actuator_armed_s *armed, ///< current armed status bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none { // Double check that our static arrays are still valid ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0); ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1); transition_result_t ret = TRANSITION_DENIED; arming_state_t current_arming_state = status->arming_state; bool feedback_provided = false; /* only check transition if the new state is actually different from the current one */ if (new_arming_state == current_arming_state) { ret = TRANSITION_NOT_CHANGED; } else { /* * Get sensing state if necessary */ int prearm_ret = OK; /* only perform the pre-arm check if we have to */ if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { prearm_ret = preflight_check(status, mavlink_fd, true /* pre-arm */ ); } /* re-run the pre-flight check as long as sensors are failing */ if (!status->condition_system_sensors_initialized && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */); status->condition_system_sensors_initialized = !prearm_ret; } /* * Perform an atomic state update */ #ifdef __PX4_NUTTX irqstate_t flags = irqsave(); #endif /* enforce lockdown in HIL */ if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { armed->lockdown = true; prearm_ret = OK; status->condition_system_sensors_initialized = true; /* recover from a prearm fail */ if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY; } } else { armed->lockdown = false; } // Check that we have a valid state transition bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; if (valid_transition) { // We have a good transition. Now perform any secondary validation. if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { // Do not perform pre-arm checks if coming from in air restore // Allow if vehicle_status_s::HIL_STATE_ON if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { // Fail transition if pre-arm check fails if (prearm_ret) { /* the prearm check already prints the reject reason */ feedback_provided = true; valid_transition = false; // Fail transition if we need safety switch press } else if (safety->safety_switch_available && !safety->safety_off) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); feedback_provided = true; valid_transition = false; } // Perform power checks only if circuit breaker is not // engaged for these checks if (!status->circuit_breaker_engaged_power_check) { // Fail transition if power is not good if (!status->condition_power_input_valid) { mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); feedback_provided = true; valid_transition = false; } // Fail transition if power levels on the avionics rail // are measured but are insufficient if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { // Check avionics rail voltages if (status->avionics_power_rail_voltage < 4.75f) { mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; valid_transition = false; } else if (status->avionics_power_rail_voltage < 4.9f) { mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; } else if (status->avionics_power_rail_voltage > 5.4f) { mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; } } } } } else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } } // HIL can always go to standby if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { valid_transition = true; } // Check if we are trying to arm, checks look good but we are in STANDBY_ERROR if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { if (status->condition_system_sensors_initialized) { mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot before arming"); } else { mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); } feedback_provided = true; } else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && status->condition_system_sensors_initialized) { mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot to complete"); feedback_provided = true; } else { // Silent ignore feedback_provided = true; } // Sensors need to be initialized for STANDBY state, except for HIL } else if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) && (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && (!status->condition_system_sensors_initialized)) { if ((!status->condition_system_prearm_error_reported && status->condition_system_hotplug_timeout) || (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); status->condition_system_prearm_error_reported = true; } feedback_provided = true; valid_transition = false; } // Finish up the state transition if (valid_transition) { armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR; armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY; ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; } /* reset feedback state */ if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR && valid_transition) { status->condition_system_prearm_error_reported = false; } if(status->data_link_found_new == true) { status->condition_system_prearm_error_reported = false; } /* end of atomic state update */ #ifdef __PX4_NUTTX irqrestore(flags); #endif } if (ret == TRANSITION_DENIED) { /* print to MAVLink and console if we didn't provide any feedback yet */ if (!feedback_provided) { mavlink_and_console_log_critical(mavlink_fd, "INVAL: %s - %s", state_names[status->arming_state], state_names[new_arming_state]); } } return ret; }