From 1d226bfc2d31778cdd06e35ec428583968fac88d Mon Sep 17 00:00:00 2001 From: Jari Nippula Date: Tue, 14 May 2024 13:51:09 +0300 Subject: [PATCH] usr_state_ctrl: moved under platform common --- boards/ssrc/common | 2 +- .../include/px4_platform_common/state_ctrl.h | 95 +++++++++++++++++++ .../src/px4/common/px4_protected_layers.cmake | 1 + .../nuttx/src/px4/common/usr_state_ctrl.c | 67 +++++++++++++ 4 files changed, 164 insertions(+), 1 deletion(-) create mode 100644 platforms/common/include/px4_platform_common/state_ctrl.h create mode 100644 platforms/nuttx/src/px4/common/usr_state_ctrl.c diff --git a/boards/ssrc/common b/boards/ssrc/common index 1c0afeaa83ed..c1477209910b 160000 --- a/boards/ssrc/common +++ b/boards/ssrc/common @@ -1 +1 @@ -Subproject commit 1c0afeaa83edba369bff661df71a1b80280c2e77 +Subproject commit c1477209910b64d3ddd56038bd2005f5481748ca diff --git a/platforms/common/include/px4_platform_common/state_ctrl.h b/platforms/common/include/px4_platform_common/state_ctrl.h new file mode 100644 index 000000000000..f44087219cbc --- /dev/null +++ b/platforms/common/include/px4_platform_common/state_ctrl.h @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * BSD 3-Clause License + * + * Copyright (c) 2024, Technology Innovation Institute + * + * 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 of the copyright holder 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 HOLDER 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 state_ctrl.h + */ + +#ifndef STATE_CTRL_H +#define STATE_CTRL_H + +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) + +#include +#include + +#define _STATECTRLIOC(_n) (_PX4_IOC(_STATECTRLIOCBASE, _n)) + +#define STATECTRLIOCREADMOISTATE _STATECTRLIOC(1) +typedef struct statectrliocreadmoistate { + uint8_t *state; + int ret; +} statectrliocreadmoistate_t; + +#define STATECTRLIOCWRITEMOISTATE _STATECTRLIOC(2) +typedef struct statectrliocwritemoistate { + uint8_t state; + int ret; +} statectrliocwritemoistate_t; + +#define STATECTRLIOCREADFWUPD _STATECTRLIOC(3) +typedef struct statectrliocreadfwupd { + uint8_t *fwupd; + int ret; +} statectrliocreadfwupd_t; + +#define STATECTRLIOCWRITEFWUPD _STATECTRLIOC(4) +typedef struct statectrliocwritefwupd { + uint8_t *fwupd; + int ret; +} statectrliocwritefwupd_t; + +#endif // __PX4_NUTTX && !CONFIG_BUILD_FLAT + +#ifdef __cplusplus +extern "C" +{ +#endif + +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) +int statectrl_ioctl(unsigned int cmd, unsigned long arg); +#endif // __PX4_NUTTX && !CONFIG_BUILD_FLAT + +int statectrl_get_moi_state(uint8_t *state); +int statectrl_set_moi_state(uint8_t state); +int statectrl_get_fwupd(uint8_t *fwupd); +int statectrl_set_fwupd(uint8_t *fwupd); + +void statectrl_init(void); + +#ifdef __cplusplus +} +#endif + + +#endif // STATE_CTRL_H diff --git a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake index 7f54024709d6..cf0f5e73dda8 100644 --- a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake +++ b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake @@ -15,6 +15,7 @@ add_library(px4_layer usr_board_ctrl.c usr_hrt.cpp usr_mcu_version.cpp + usr_state_ctrl.c ) target_link_libraries(px4_layer diff --git a/platforms/nuttx/src/px4/common/usr_state_ctrl.c b/platforms/nuttx/src/px4/common/usr_state_ctrl.c new file mode 100644 index 000000000000..a0a2af5c0ea3 --- /dev/null +++ b/platforms/nuttx/src/px4/common/usr_state_ctrl.c @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (c) 2024 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include + +int statectrl_get_moi_state(uint8_t *state) +{ + statectrliocreadmoistate_t data = {state, 0}; + boardctl(STATECTRLIOCREADMOISTATE, (uintptr_t)&data); + return data.ret; +} + +int statectrl_set_moi_state(uint8_t state) +{ + statectrliocwritemoistate_t data = {state, 0}; + boardctl(STATECTRLIOCWRITEMOISTATE, (uintptr_t)&data); + return data.ret; + +} + +int statectrl_get_fwupd(uint8_t *fwupd) +{ + statectrliocreadfwupd_t data = {fwupd, 0}; + boardctl(STATECTRLIOCREADFWUPD, (uintptr_t)&data); + return data.ret; +} + +int statectrl_set_fwupd(uint8_t *fwupd) +{ + statectrliocwritefwupd_t data = {fwupd, 0}; + boardctl(STATECTRLIOCWRITEFWUPD, (uintptr_t)&data); + return data.ret; +}