/**************************************************************************** * * Copyright (c) 2017 Intel Corporation. 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 "arm_auth.h" #include #include #include #include #include #include #include static orb_advert_t handle_vehicle_command_pub; static orb_advert_t *mavlink_log_pub; static int command_ack_sub = -1; static param_t param_arm_parameters; static hrt_abstime auth_timeout; static enum { ARM_AUTH_IDLE = 0, ARM_AUTH_WAITING_AUTH, ARM_AUTH_WAITING_AUTH_WITH_ACK, ARM_AUTH_MISSION_APPROVED } state = ARM_AUTH_IDLE; struct packed_struct { uint8_t authorizer_system_id; union { uint16_t auth_method_arm_timeout_msec; uint16_t auth_method_two_arm_timeout_msec; } auth_method_param; uint8_t authentication_method; } arm_parameters; static uint32_t *system_id; static uint8_t _auth_method_arm_req_check(); static uint8_t _auth_method_two_arm_check(); static uint8_t (*arm_check_method[ARM_AUTH_METHOD_LAST])() = { _auth_method_arm_req_check, _auth_method_two_arm_check, }; static void arm_auth_request_msg_send() { struct vehicle_command_s cmd = { .timestamp = 0, .param5 = 0, .param6 = 0, .param1 = 0, .param2 = 0, .param3 = 0, .param4 = 0, .param7 = 0, .command = vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST, .target_system = arm_parameters.authorizer_system_id }; if (handle_vehicle_command_pub == nullptr) { handle_vehicle_command_pub = orb_advertise(ORB_ID(vehicle_command), &cmd); } else { orb_publish(ORB_ID(vehicle_command), handle_vehicle_command_pub, &cmd); } } static uint8_t _auth_method_arm_req_check() { switch (state) { case ARM_AUTH_IDLE: /* no authentication in process? handle bellow */ break; case ARM_AUTH_MISSION_APPROVED: return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; default: return vehicle_command_ack_s::VEHICLE_RESULT_DENIED; } /* handling ARM_AUTH_IDLE */ arm_auth_request_msg_send(); hrt_abstime now = hrt_absolute_time(); auth_timeout = now + (arm_parameters.auth_method_param.auth_method_arm_timeout_msec * 1000); state = ARM_AUTH_WAITING_AUTH; while (now < auth_timeout) { arm_auth_update(now); if (state != ARM_AUTH_WAITING_AUTH && state != ARM_AUTH_WAITING_AUTH_WITH_ACK) { break; } /* 0.5ms */ usleep(500); now = hrt_absolute_time(); } switch (state) { case ARM_AUTH_WAITING_AUTH: case ARM_AUTH_WAITING_AUTH_WITH_ACK: state = ARM_AUTH_IDLE; mavlink_log_critical(mavlink_log_pub, "Arm auth: No response"); break; default: break; } return state == ARM_AUTH_MISSION_APPROVED ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_DENIED; } static uint8_t _auth_method_two_arm_check() { switch (state) { case ARM_AUTH_IDLE: /* no authentication in process? handle bellow */ break; case ARM_AUTH_MISSION_APPROVED: return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; case ARM_AUTH_WAITING_AUTH: case ARM_AUTH_WAITING_AUTH_WITH_ACK: return vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED; default: return vehicle_command_ack_s::VEHICLE_RESULT_DENIED; } /* handling ARM_AUTH_IDLE */ arm_auth_request_msg_send(); hrt_abstime now = hrt_absolute_time(); auth_timeout = now + (arm_parameters.auth_method_param.auth_method_arm_timeout_msec * 1000); state = ARM_AUTH_WAITING_AUTH; mavlink_log_critical(mavlink_log_pub, "Arm auth: Requesting authorization..."); return vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED; } uint8_t arm_auth_check() { if (arm_parameters.authentication_method < ARM_AUTH_METHOD_LAST) { return arm_check_method[arm_parameters.authentication_method](); } return vehicle_command_ack_s::VEHICLE_RESULT_DENIED; } void arm_auth_update(hrt_abstime now) { param_get(param_arm_parameters, &arm_parameters); switch (state) { case ARM_AUTH_WAITING_AUTH: case ARM_AUTH_WAITING_AUTH_WITH_ACK: /* handle bellow */ break; case ARM_AUTH_MISSION_APPROVED: if (now > auth_timeout) { state = ARM_AUTH_IDLE; } return; case ARM_AUTH_IDLE: default: return; } /* * handling ARM_AUTH_WAITING_AUTH, ARM_AUTH_WAITING_AUTH_WITH_ACK */ vehicle_command_ack_s command_ack; bool updated = false; orb_check(command_ack_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_command_ack), command_ack_sub, &command_ack); } if (updated && command_ack.command == vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST && command_ack.target_system == *system_id) { switch (command_ack.result) { case vehicle_command_ack_s::VEHICLE_RESULT_IN_PROGRESS: state = ARM_AUTH_WAITING_AUTH_WITH_ACK; break; case vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED: mavlink_log_critical(mavlink_log_pub, "Arm auth: Authorized for the next %u seconds", command_ack.result_param2); state = ARM_AUTH_MISSION_APPROVED; auth_timeout = now + (command_ack.result_param2 * 1000000); return; case vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED: mavlink_log_critical(mavlink_log_pub, "Arm auth: Temporarily rejected"); state = ARM_AUTH_IDLE; return; case vehicle_command_ack_s::VEHICLE_RESULT_DENIED: default: switch (command_ack.result_param1) { case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_NONE: /* Authorizer will send reason to ground station */ break; case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_INVALID_WAYPOINT: mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied, waypoint %i have a invalid value", command_ack.result_param2); break; case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_TIMEOUT: mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied by timeout in authorizer"); break; case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE: mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied because airspace is in use"); break; case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_BAD_WEATHER: mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied because of bad weather"); break; case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_GENERIC: default: mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied"); } state = ARM_AUTH_IDLE; return; } } if (now > auth_timeout) { mavlink_log_critical(mavlink_log_pub, "Arm auth: No response"); state = ARM_AUTH_IDLE; } } void arm_auth_init(orb_advert_t *mav_log_pub, uint32_t *sys_id) { system_id = sys_id; param_arm_parameters = param_find("COM_ARM_AUTH"); command_ack_sub = orb_subscribe(ORB_ID(vehicle_command_ack)); mavlink_log_pub = mav_log_pub; } enum arm_auth_methods arm_auth_method_get() { return (enum arm_auth_methods) arm_parameters.authentication_method; }