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