PX4-Autopilot/platforms/common/shutdown.cpp

/****************************************************************************
 *
 * Copyright (C) 2017 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file shutdown.cpp
 * Implementation of the API declared in px4_shutdown.h.
 */

#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/workqueue.h>
#include <px4_platform_common/shutdown.h>
#include <px4_platform_common/tasks.h>

#include <drivers/drv_hrt.h>

#ifndef MODULE_NAME
#define MODULE_NAME "shutdown"
#endif

#include <px4_platform_common/external_reset_lockout.h>
#include <px4_platform_common/log.h>
#include <uORB/uORB.h>

#include <stdint.h>
#include <errno.h>
#include <pthread.h>

#ifdef __PX4_NUTTX
#include <nuttx/board.h>
#endif

#include <sys/boardctl.h>

// Clang provides __has_feature; GCC does not. Define a fallback so the
// AddressSanitizer check below is portable: GCC's preprocessor otherwise
// chokes on __has_feature(...) even when guarded by defined(__has_feature),
// because both operands of && are tokenized before the && is evaluated.
#ifndef __has_feature
#define __has_feature(x) 0
#endif

using namespace time_literals;

static pthread_mutex_t shutdown_mutex =
	PTHREAD_MUTEX_INITIALIZER; // protects access to shutdown_hooks & shutdown_lock_counter
static uint8_t shutdown_lock_counter = 0;
static bool shutdown_lock_watchdog_cb_triggered = false;

static struct work_s shutdown_lock_watchdog_work = {};
static constexpr hrt_abstime SHUTDOWN_LOCK_WATCHDOG_S = 60_s;

/**
 * Watchdog callback fired when a shutdown lock is held for more than 60s.
 * Force-releases the shutdown_lock and set shutdown_lock_counter to 0.
 * @param arg unused
 */
static void shutdown_lock_watchdog_cb(void *arg)
{
	int ret;

	ret = pthread_mutex_lock(&shutdown_mutex);

	if (ret != 0) {
		return;
	}

	if (shutdown_lock_counter != 0) {
		shutdown_lock_counter = 0;
		shutdown_lock_watchdog_cb_triggered = true;
		PX4_ERR("shutdown_lock_watchdog: lock held for more than 60s, force-releasing");
		px4_indicate_external_reset_lockout(LockoutComponent::SystemShutdownLock, false);
	}

	ret = pthread_mutex_unlock(&shutdown_mutex);
}

int px4_shutdown_lock()
{
	int ret;

	ret = pthread_mutex_lock(&shutdown_mutex);

	if (ret != 0) {
		return ret;
	}

	// schedule watchdog, if after 60s lock is still held -> unlock
	if (shutdown_lock_counter == 0) {
		ret = work_queue(HPWORK, &shutdown_lock_watchdog_work, shutdown_lock_watchdog_cb, nullptr,
				 USEC2TICK(SHUTDOWN_LOCK_WATCHDOG_S));

		if (ret != 0) {
			goto out;
		}
	}

	++shutdown_lock_counter;
	shutdown_lock_watchdog_cb_triggered = false;

	px4_indicate_external_reset_lockout(LockoutComponent::SystemShutdownLock, true);

out:
	ret |= pthread_mutex_unlock(&shutdown_mutex);

	return ret;
}

int px4_shutdown_unlock()
{
	int ret;

	ret = pthread_mutex_lock(&shutdown_mutex);

	if (ret != 0) {
		return ret;
	}

	if (shutdown_lock_counter > 0) {
		--shutdown_lock_counter;

		if (shutdown_lock_counter == 0) {
			ret = work_cancel(HPWORK, &shutdown_lock_watchdog_work);

			px4_indicate_external_reset_lockout(LockoutComponent::SystemShutdownLock, false);
		}

	} else if (!shutdown_lock_watchdog_cb_triggered) {
		PX4_ERR("unmatched number of px4_shutdown_unlock() calls");
	}

	ret |= pthread_mutex_unlock(&shutdown_mutex);

	return ret;
}

#if defined(CONFIG_SCHED_WORKQUEUE) || (!defined(CONFIG_BUILD_FLAT) && defined(CONFIG_LIBC_USRWORK))

static struct work_s shutdown_work = {};
static uint16_t shutdown_counter = 0; ///< count how many times the shutdown worker was executed

#define SHUTDOWN_ARG_IN_PROGRESS (1<<0)
#define SHUTDOWN_ARG_REBOOT (1<<1)
#define SHUTDOWN_ARG_TO_BOOTLOADER (1<<2)
#define SHUTDOWN_ARG_TO_ISP (1<<3)
static uint8_t shutdown_args = 0;

static constexpr int max_shutdown_hooks = 1;
static shutdown_hook_t shutdown_hooks[max_shutdown_hooks] = {};

static hrt_abstime shutdown_time_us = 0;
static constexpr hrt_abstime shutdown_timeout_us =
	5_s; ///< force shutdown after this time if modules do not respond in time

int px4_register_shutdown_hook(shutdown_hook_t hook)
{
	int ret = 0;
	bool hooklist_full = true;

	ret = pthread_mutex_lock(&shutdown_mutex);

	if (ret != 0) {
		return ret;
	}

	for (int i = 0; i < max_shutdown_hooks; ++i) {
		if (!shutdown_hooks[i]) {
			shutdown_hooks[i] = hook;
			hooklist_full = false;
			break;
		}
	}

	ret = pthread_mutex_unlock(&shutdown_mutex);

	return hooklist_full ? -ENOMEM : ret;
}

int px4_unregister_shutdown_hook(shutdown_hook_t hook)
{
	int ret = 0;
	bool hook_found = false;

	ret = pthread_mutex_lock(&shutdown_mutex);

	if (ret != 0) {
		return ret;
	}

	for (int i = 0; i < max_shutdown_hooks; ++i) {
		if (shutdown_hooks[i] == hook) {
			shutdown_hooks[i] = nullptr;
			hook_found = true;
			break;
		}
	}

	ret = pthread_mutex_unlock(&shutdown_mutex);

	return hook_found ? ret : -EINVAL;
}

/**
 * work queue callback method to shutdown.
 * @param arg unused
 */
static void shutdown_worker(void *arg)
{
	PX4_DEBUG("shutdown worker (%i)", shutdown_counter);
	bool done = true;
	int ret;

	ret = pthread_mutex_lock(&shutdown_mutex);

	if (ret != 0) {
		return;
	}

	for (int i = 0; i < max_shutdown_hooks; ++i) {
		if (shutdown_hooks[i]) {
			if (!shutdown_hooks[i]()) {
				done = false;
			}
		}
	}

	const hrt_abstime now = hrt_absolute_time();
	const bool delay_elapsed = (now > shutdown_time_us);

	if (delay_elapsed && ((done && shutdown_lock_counter == 0) || (now > (shutdown_time_us + shutdown_timeout_us)))) {
		uorb_shutdown();

		if (shutdown_args & SHUTDOWN_ARG_REBOOT) {
#if defined(CONFIG_BOARDCTL_RESET)
			PX4_INFO_RAW("Reboot NOW.");

			if (shutdown_args & SHUTDOWN_ARG_TO_BOOTLOADER) {
				boardctl(BOARDIOC_RESET, (uintptr_t)REBOOT_TO_BOOTLOADER);

			} else if (shutdown_args & SHUTDOWN_ARG_TO_ISP) {
				boardctl(BOARDIOC_RESET, (uintptr_t)REBOOT_TO_ISP);

			} else {
				boardctl(BOARDIOC_RESET, (uintptr_t)REBOOT_REQUEST);
			}

#else
			PX4_PANIC("board reset not available");
#endif

		} else {
#if defined(BOARD_HAS_POWER_CONTROL)
			PX4_INFO_RAW("Powering off NOW.");
#if defined(CONFIG_BOARDCTL_POWEROFF)
			boardctl(BOARDIOC_POWEROFF, 0);

#else
			board_power_off(0);

#endif
#elif defined(__PX4_POSIX)
			// simply exit on posix if real shutdown (poweroff) not available
			PX4_INFO_RAW("Exiting NOW.");
#if defined(__SANITIZE_ADDRESS__) || __has_feature(address_sanitizer)
			exit(0); // Use exit() instead of _exit() so ASAN can report errors
#else
			system_exit(0);
#endif
#else
			PX4_PANIC("board shutdown not available");
#endif
		}

		pthread_mutex_unlock(&shutdown_mutex);

	} else {
		pthread_mutex_unlock(&shutdown_mutex);

		ret = work_queue(HPWORK, &shutdown_work, (worker_t)&shutdown_worker, nullptr, USEC2TICK(10000));

		if (ret != 0) {
			PX4_ERR("failed to queue shutdown worker: %d", ret);
		}
	}
}

#if defined(CONFIG_BOARDCTL_RESET)
int px4_reboot_request(reboot_request_t request, uint32_t delay_us)
{
	int ret;

	ret = pthread_mutex_lock(&shutdown_mutex);

	if (ret != 0) {
		return ret;
	}

	if (!(shutdown_args & SHUTDOWN_ARG_IN_PROGRESS)) {
		shutdown_args |= SHUTDOWN_ARG_REBOOT;

		if (request == REBOOT_TO_BOOTLOADER) {
			shutdown_args |= SHUTDOWN_ARG_TO_BOOTLOADER;

		} else if (request == REBOOT_TO_ISP) {
			shutdown_args |= SHUTDOWN_ARG_TO_ISP;
		}

		shutdown_time_us = hrt_absolute_time();

		if (delay_us > 0) {
			shutdown_time_us += delay_us;
		}

		ret = work_queue(HPWORK, &shutdown_work, (worker_t)&shutdown_worker, nullptr, 1);

		if (ret != 0) {
			PX4_ERR("failed to queue shutdown worker: %d", ret);
		}
	}

	ret = pthread_mutex_unlock(&shutdown_mutex);

	return ret;
}
#endif // CONFIG_BOARDCTL_RESET

#if defined(BOARD_HAS_POWER_CONTROL) || defined(__PX4_POSIX)
int px4_shutdown_request(uint32_t delay_us)
{
	int ret;

	ret = pthread_mutex_lock(&shutdown_mutex);

	if (ret != 0) {
		return ret;
	}

	if (!(shutdown_args & SHUTDOWN_ARG_IN_PROGRESS)) {
		shutdown_args |= SHUTDOWN_ARG_IN_PROGRESS;

		shutdown_time_us = hrt_absolute_time();

		if (delay_us > 0) {
			shutdown_time_us += delay_us;
		}

		ret = work_queue(HPWORK, &shutdown_work, (worker_t)&shutdown_worker, nullptr, 1);

		if (ret != 0) {
			PX4_ERR("failed to queue shutdown worker: %d", ret);
		}
	}

	ret = pthread_mutex_unlock(&shutdown_mutex);

	return ret;
}
#endif // BOARD_HAS_POWER_CONTROL

#endif // CONFIG_SCHED_WORKQUEUE)