int
Mavlink::set_hil_enabled(bool hil_enabled)
{
int ret = OK;
/* enable HIL */
if (hil_enabled && !_hil_enabled) {
_hil_enabled = true;
configure_stream("HIL_CONTROLS", 200.0f);
}
/* disable HIL */
if (!hil_enabled && _hil_enabled) {
_hil_enabled = false;
configure_stream("HIL_CONTROLS", 0.0f);
} else {
ret = ERROR;
}
return ret;
}
int
Mavlink::set_hil_enabled(bool hil_enabled)
{
int ret = OK;
/* enable HIL */
if (hil_enabled && !_hil_enabled) {
_hil_enabled = true;
float rate_mult = _datarate / 1000.0f;
configure_stream("HIL_CONTROLS", 15.0f * rate_mult);
}
/* disable HIL */
if (!hil_enabled && _hil_enabled) {
_hil_enabled = false;
configure_stream("HIL_CONTROLS", 0.0f);
} else {
ret = ERROR;
}
return ret;
}
static void
doit(int f,
struct sockaddr_storage *fromp,
krb5_context krb_context,
int encr_flag,
krb5_keytab keytab)
{
int p, t, on = 1;
char c;
char abuf[INET6_ADDRSTRLEN];
struct sockaddr_in *sin;
struct sockaddr_in6 *sin6;
int fromplen;
in_port_t port;
struct termios tp;
boolean_t bad_port;
boolean_t no_name;
char rhost_addra[INET6_ADDRSTRLEN];
if (!(rlbuf = malloc(BUFSIZ))) {
syslog(LOG_ERR, "rlbuf malloc failed\n");
exit(EXIT_FAILURE);
}
(void) alarm(60);
if (read(f, &c, 1) != 1 || c != 0) {
syslog(LOG_ERR, "failed to receive protocol zero byte\n");
exit(EXIT_FAILURE);
}
(void) alarm(0);
if (fromp->ss_family == AF_INET) {
sin = (struct sockaddr_in *)fromp;
port = sin->sin_port = ntohs((ushort_t)sin->sin_port);
fromplen = sizeof (struct sockaddr_in);
if (!inet_ntop(AF_INET, &sin->sin_addr,
rhost_addra, sizeof (rhost_addra)))
goto badconversion;
} else if (fromp->ss_family == AF_INET6) {
sin6 = (struct sockaddr_in6 *)fromp;
port = sin6->sin6_port = ntohs((ushort_t)sin6->sin6_port);
fromplen = sizeof (struct sockaddr_in6);
if (IN6_IS_ADDR_V4MAPPED(&sin6->sin6_addr)) {
struct in_addr ipv4_addr;
IN6_V4MAPPED_TO_INADDR(&sin6->sin6_addr,
&ipv4_addr);
if (!inet_ntop(AF_INET, &ipv4_addr, rhost_addra,
sizeof (rhost_addra)))
goto badconversion;
} else {
if (!inet_ntop(AF_INET6, &sin6->sin6_addr,
rhost_addra, sizeof (rhost_addra)))
goto badconversion;
}
} else {
syslog(LOG_ERR, "unknown address family %d\n",
fromp->ss_family);
fatal(f, "Permission denied");
}
/*
* Allow connections only from the "ephemeral" reserved
* ports(ports 512 - 1023) by checking the remote port
* because other utilities(e.g. in.ftpd) can be used to
* allow a unprivileged user to originate a connection
* from a privileged port and provide untrustworthy
* authentication.
*/
bad_port = (use_auth != KRB5_RECVAUTH_V5 &&
(port >= (in_port_t)IPPORT_RESERVED) ||
(port < (in_port_t)(IPPORT_RESERVED/2)));
no_name = getnameinfo((const struct sockaddr *) fromp,
fromplen, hostname, sizeof (hostname),
NULL, 0, 0) != 0;
if (no_name || bad_port) {
(void) strlcpy(abuf, rhost_addra, sizeof (abuf));
/* If no host name, use IP address for name later on. */
if (no_name)
(void) strlcpy(hostname, abuf, sizeof (hostname));
}
if (!no_name) {
/*
* Even if getnameinfo() succeeded, we still have to check
* for spoofing.
*/
check_address("rlogind", fromp, sin, sin6, rhost_addra,
hostname, sizeof (hostname));
}
if (bad_port) {
if (no_name)
syslog(LOG_NOTICE,
"connection from %s - bad port\n",
abuf);
else
syslog(LOG_NOTICE,
"connection from %s(%s) - bad port\n",
hostname, abuf);
fatal(f, "Permission denied");
}
if (use_auth == KRB5_RECVAUTH_V5) {
do_krb_login(f, rhost_addra, hostname,
krb_context, encr_flag, keytab);
if (krusername != NULL && strlen(krusername)) {
/*
* Kerberos Authentication succeeded,
* so set the proper program name to use
* with pam (important during 'cleanup'
* routine later).
*/
pam_prog_name = KRB5_PROG_NAME;
}
}
if (write(f, "", 1) != 1) {
syslog(LOG_NOTICE,
"send of the zero byte(to %s) failed:"
" cannot start data transfer mode\n",
(no_name ? abuf : hostname));
exit(EXIT_FAILURE);
}
if ((p = open("/dev/ptmx", O_RDWR)) == -1)
fatalperror(f, "cannot open /dev/ptmx");
if (grantpt(p) == -1)
fatal(f, "could not grant slave pty");
if (unlockpt(p) == -1)
fatal(f, "could not unlock slave pty");
if ((line = ptsname(p)) == NULL)
fatal(f, "could not enable slave pty");
if ((t = open(line, O_RDWR)) == -1)
fatal(f, "could not open slave pty");
if (ioctl(t, I_PUSH, "ptem") == -1)
fatalperror(f, "ioctl I_PUSH ptem");
if (ioctl(t, I_PUSH, "ldterm") == -1)
fatalperror(f, "ioctl I_PUSH ldterm");
if (ioctl(t, I_PUSH, "ttcompat") == -1)
fatalperror(f, "ioctl I_PUSH ttcompat");
/*
* POP the sockmod and push the rlmod module.
*
* Note that sockmod has to be removed since readstream assumes
* a "raw" TPI endpoint(e.g. it uses getmsg).
*/
if (removemod(f, "sockmod") < 0)
fatalperror(f, "couldn't remove sockmod");
if (encr_flag) {
if (ioctl(f, I_PUSH, "cryptmod") < 0)
fatalperror(f, "ioctl I_PUSH rlmod");
}
if (ioctl(f, I_PUSH, "rlmod") < 0)
fatalperror(f, "ioctl I_PUSH rlmod");
if (encr_flag) {
/*
* Make sure rlmod will pass unrecognized IOCTLs to cryptmod
*/
uchar_t passthru = 1;
struct strioctl rlmodctl;
rlmodctl.ic_cmd = CRYPTPASSTHRU;
rlmodctl.ic_timout = -1;
rlmodctl.ic_len = sizeof (uchar_t);
rlmodctl.ic_dp = (char *)&passthru;
if (ioctl(f, I_STR, &rlmodctl) < 0)
fatal(f, "ioctl CRYPTPASSTHRU failed\n");
}
/*
* readstream will do a getmsg till it receives
* M_PROTO type T_DATA_REQ from rloginmodopen()
* indicating all data on the stream prior to pushing rlmod has
* been drained at the stream head.
*/
if ((nsize = readstream(f, rlbuf, BUFSIZ)) < 0)
fatalperror(f, "readstream failed");
/*
* Make sure the pty doesn't modify the strings passed
* to login as part of the "rlogin protocol." The login
* program should set these flags to apropriate values
* after it has read the strings.
*/
if (ioctl(t, TCGETS, &tp) == -1)
fatalperror(f, "ioctl TCGETS");
tp.c_lflag &= ~(ECHO|ICANON);
tp.c_oflag &= ~(XTABS|OCRNL);
tp.c_iflag &= ~(IGNPAR|ICRNL);
if (ioctl(t, TCSETS, &tp) == -1)
fatalperror(f, "ioctl TCSETS");
/*
* System V ptys allow the TIOC{SG}WINSZ ioctl to be
* issued on the master side of the pty. Luckily, that's
* the only tty ioctl we need to do do, so we can close the
* slave side in the parent process after the fork.
*/
(void) ioctl(p, TIOCSWINSZ, &win);
pid = fork();
if (pid < 0)
fatalperror(f, "fork");
if (pid == 0) {
int tt;
struct utmpx ut;
/* System V login expects a utmp entry to already be there */
(void) memset(&ut, 0, sizeof (ut));
(void) strncpy(ut.ut_user, ".rlogin", sizeof (ut.ut_user));
(void) strncpy(ut.ut_line, line, sizeof (ut.ut_line));
ut.ut_pid = getpid();
ut.ut_id[0] = 'r';
ut.ut_id[1] = (char)SC_WILDC;
ut.ut_id[2] = (char)SC_WILDC;
ut.ut_id[3] = (char)SC_WILDC;
ut.ut_type = LOGIN_PROCESS;
ut.ut_exit.e_termination = 0;
ut.ut_exit.e_exit = 0;
(void) time(&ut.ut_tv.tv_sec);
if (makeutx(&ut) == NULL)
syslog(LOG_INFO, "in.rlogind:\tmakeutx failed");
/* controlling tty */
if (setsid() == -1)
fatalperror(f, "setsid");
if ((tt = open(line, O_RDWR)) == -1)
fatalperror(f, "could not re-open slave pty");
if (close(p) == -1)
fatalperror(f, "error closing pty master");
if (close(t) == -1)
fatalperror(f, "error closing pty slave"
" opened before session established");
/*
* If this fails we may or may not be able to output an
* error message.
*/
if (close(f) == -1)
fatalperror(f, "error closing deamon stdout");
if (dup2(tt, STDIN_FILENO) == -1 ||
dup2(tt, STDOUT_FILENO) == -1 ||
dup2(tt, STDERR_FILENO) == -1)
exit(EXIT_FAILURE); /* Disaster! No stderr! */
(void) close(tt);
if (use_auth == KRB5_RECVAUTH_V5 &&
krusername != NULL && strlen(krusername)) {
(void) execl(LOGIN_PROGRAM, "login",
"-d", line,
"-r", hostname,
"-u", krusername, /* KRB5 principal name */
"-s", pam_prog_name,
"-t", term, /* Remote Terminal */
"-U", rusername, /* Remote User */
"-R", KRB5_REPOSITORY_NAME,
lusername, /* local user */
NULL);
} else {
(void) execl(LOGIN_PROGRAM, "login",
"-d", line,
"-r", hostname,
NULL);
}
fatalperror(STDERR_FILENO, "/bin/login");
/*NOTREACHED*/
}
(void) close(t);
(void) ioctl(f, FIONBIO, &on);
(void) ioctl(p, FIONBIO, &on);
/*
* Must ignore SIGTTOU, otherwise we'll stop
* when we try and set slave pty's window shape
* (our controlling tty is the master pty).
* Likewise, we don't want any of the tty-generated
* signals from chars passing through.
*/
(void) sigset(SIGTSTP, SIG_IGN);
(void) sigset(SIGINT, SIG_IGN);
(void) sigset(SIGQUIT, SIG_IGN);
(void) sigset(SIGTTOU, SIG_IGN);
(void) sigset(SIGTTIN, SIG_IGN);
(void) sigset(SIGCHLD, cleanup);
(void) setpgrp();
if (encr_flag) {
krb5_data ivec, *ivptr;
uint_t ivec_usage;
stop_stream(f, CRYPT_ENCRYPT|CRYPT_DECRYPT);
/*
* Configure the STREAMS crypto module. For now,
* don't use any IV parameter. KCMDV0.2 support
* will require the use of Initialization Vectors
* for both encrypt and decrypt modes.
*/
if (kcmd_protocol == KCMD_OLD_PROTOCOL) {
if (session_key->enctype == ENCTYPE_DES_CBC_CRC) {
/*
* This is gross but necessary for MIT compat.
*/
ivec.length = session_key->length;
ivec.data = (char *)session_key->contents;
ivec_usage = IVEC_REUSE;
ivptr = &ivec;
} else {
ivptr = NULL; /* defaults to all 0's */
ivec_usage = IVEC_NEVER;
}
/*
* configure both sides of stream together
* since they share the same IV.
* This is what makes the OLD KCMD protocol
* less secure than the newer one - Bad ivecs.
*/
if (configure_stream(f, session_key,
CRYPT_ENCRYPT|CRYPT_DECRYPT,
ivptr, ivec_usage) != 0)
fatal(f, "Cannot initialize encryption -"
" exiting.\n");
} else {
size_t blocksize;
if (session_key->enctype == ENCTYPE_ARCFOUR_HMAC ||
session_key->enctype == ENCTYPE_ARCFOUR_HMAC_EXP) {
if (configure_stream(f, session_key,
CRYPT_ENCRYPT|CRYPT_DECRYPT,
NULL, IVEC_NEVER) != 0)
fatal(f,
"Cannot initialize encryption -"
" exiting.\n");
goto startcrypto;
}
if (krb5_c_block_size(krb_context,
session_key->enctype,
&blocksize)) {
syslog(LOG_ERR, "Cannot determine blocksize "
"for encryption type %d",
session_key->enctype);
fatal(f, "Cannot determine blocksize "
"for encryption - exiting.\n");
}
ivec.data = (char *)malloc(blocksize);
ivec.length = blocksize;
if (ivec.data == NULL)
fatal(f, "memory error - exiting\n");
/*
* Following MIT convention -
* encrypt IV = 0x01 x blocksize
* decrypt IV = 0x00 x blocksize
* ivec_usage = IVEC_ONETIME
*
* configure_stream separately for encrypt and
* decrypt because there are 2 different IVs.
*
* AES uses 0's for IV.
*/
if (session_key->enctype ==
ENCTYPE_AES128_CTS_HMAC_SHA1_96 ||
session_key->enctype ==
ENCTYPE_AES256_CTS_HMAC_SHA1_96)
(void) memset(ivec.data, 0x00, blocksize);
else
(void) memset(ivec.data, 0x01, blocksize);
if (configure_stream(f, session_key, CRYPT_ENCRYPT,
&ivec, IVEC_ONETIME) != 0)
fatal(f, "Cannot initialize encryption -"
" exiting.\n");
(void) memset(ivec.data, 0x00, blocksize);
if (configure_stream(f, session_key, CRYPT_DECRYPT,
&ivec, IVEC_ONETIME) != 0)
fatal(f, "Cannot initialize encryption -"
" exiting.\n");
(void) free(ivec.data);
}
startcrypto:
start_stream(f, CRYPT_ENCRYPT);
start_stream(f, CRYPT_DECRYPT);
}
protocol(f, p, encr_flag);
cleanup(0);
/*NOTREACHED*/
badconversion:
fatalperror(f, "address conversion");
/*NOTREACHED*/
}
int
Mavlink::task_main(int argc, char *argv[])
{
int ch;
_baudrate = 57600;
_datarate = 0;
_mode = MAVLINK_MODE_NORMAL;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
/* don't exit from getopt loop to leave getopt global variables in consistent state,
* set error flag instead */
bool err_flag = false;
while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) {
switch (ch) {
case 'b':
_baudrate = strtoul(optarg, NULL, 10);
if (_baudrate < 9600 || _baudrate > 921600) {
warnx("invalid baud rate '%s'", optarg);
err_flag = true;
}
break;
case 'r':
_datarate = strtoul(optarg, NULL, 10);
if (_datarate < 10 || _datarate > MAX_DATA_RATE) {
warnx("invalid data rate '%s'", optarg);
err_flag = true;
}
break;
case 'd':
_device_name = optarg;
break;
// case 'e':
// mavlink_link_termination_allowed = true;
// break;
case 'm':
if (strcmp(optarg, "custom") == 0) {
_mode = MAVLINK_MODE_CUSTOM;
} else if (strcmp(optarg, "camera") == 0) {
_mode = MAVLINK_MODE_CAMERA;
}
break;
case 'f':
_forwarding_on = true;
break;
case 'p':
_passing_on = true;
break;
case 'v':
_verbose = true;
break;
case 'w':
_wait_to_transmit = true;
break;
case 'x':
_ftp_on = true;
break;
default:
err_flag = true;
break;
}
}
if (err_flag) {
usage();
return ERROR;
}
if (_datarate == 0) {
/* convert bits to bytes and use 1/2 of bandwidth by default */
_datarate = _baudrate / 20;
}
if (_datarate > MAX_DATA_RATE) {
_datarate = MAX_DATA_RATE;
}
if (Mavlink::instance_exists(_device_name, this)) {
warnx("mavlink instance for %s already running", _device_name);
return ERROR;
}
/* inform about mode */
switch (_mode) {
case MAVLINK_MODE_NORMAL:
warnx("mode: NORMAL");
break;
case MAVLINK_MODE_CUSTOM:
warnx("mode: CUSTOM");
break;
case MAVLINK_MODE_CAMERA:
warnx("mode: CAMERA");
break;
default:
warnx("ERROR: Unknown mode");
break;
}
warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
/* flush stdout in case MAVLink is about to take it over */
fflush(stdout);
struct termios uart_config_original;
/* default values for arguments */
_uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &_is_usb_uart);
if (_uart_fd < 0) {
warn("could not open %s", _device_name);
return ERROR;
}
/* initialize mavlink text message buffering */
mavlink_logbuffer_init(&_logbuffer, 5);
/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
if (_passing_on || _ftp_on) {
/* initialize message buffer if multiplexing is on or its needed for FTP.
* make space for two messages plus off-by-one space as we use the empty element
* marker ring buffer approach.
*/
if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) {
errx(1, "can't allocate message buffer, exiting");
}
/* initialize message buffer mutex */
pthread_mutex_init(&_message_buffer_mutex, NULL);
}
/* create the device node that's used for sending text log messages, etc. */
register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
/* initialize logging device */
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
/* Initialize system properties */
mavlink_update_system();
/* start the MAVLink receiver */
_receive_thread = MavlinkReceiver::receive_start(this);
_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
/* create mission manager */
_mission_manager = new MavlinkMissionManager(this);
_mission_manager->set_verbose(_verbose);
_task_running = true;
MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
uint64_t param_time = 0;
MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
uint64_t status_time = 0;
struct vehicle_status_s status;
status_sub->update(&status_time, &status);
MavlinkCommandsStream commands_stream(this, _channel);
/* add default streams depending on mode and intervals depending on datarate */
float rate_mult = get_rate_mult();
configure_stream("HEARTBEAT", 1.0f);
switch (_mode) {
case MAVLINK_MODE_NORMAL:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
configure_stream("ATTITUDE", 10.0f * rate_mult);
configure_stream("VFR_HUD", 8.0f * rate_mult);
configure_stream("GPS_RAW_INT", 1.0f * rate_mult);
configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
configure_stream("DISTANCE_SENSOR", 0.5f);
break;
case MAVLINK_MODE_CAMERA:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("ATTITUDE", 15.0f * rate_mult);
configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult);
configure_stream("CAMERA_CAPTURE", 1.0f);
break;
default:
break;
}
/* don't send parameters on startup without request */
_mavlink_param_queue_index = param_count();
MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult);
/* set main loop delay depending on data rate to minimize CPU overhead */
_main_loop_delay = MAIN_LOOP_DELAY / rate_mult;
/* now the instance is fully initialized and we can bump the instance count */
LL_APPEND(_mavlink_instances, this);
while (!_task_should_exit) {
/* main loop */
usleep(_main_loop_delay);
perf_begin(_loop_perf);
hrt_abstime t = hrt_absolute_time();
if (param_sub->update(¶m_time, nullptr)) {
/* parameters updated */
mavlink_update_system();
}
if (status_sub->update(&status_time, &status)) {
/* switch HIL mode if required */
set_hil_enabled(status.hil_state == HIL_STATE_ON);
}
/* update commands stream */
commands_stream.update(t);
/* check for requested subscriptions */
if (_subscribe_to_stream != nullptr) {
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
if (_subscribe_to_stream_rate > 0.0f) {
warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
(double)_subscribe_to_stream_rate);
} else {
warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
}
} else {
warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name);
}
delete _subscribe_to_stream;
_subscribe_to_stream = nullptr;
}
/* update streams */
MavlinkStream *stream;
LL_FOREACH(_streams, stream) {
stream->update(t);
}
if (fast_rate_limiter.check(t)) {
mavlink_pm_queued_send();
_mission_manager->eventloop();
if (!mavlink_logbuffer_is_empty(&_logbuffer)) {
struct mavlink_logmessage msg;
int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
if (lb_ret == OK) {
send_statustext(msg.severity, msg.text);
}
}
}
/* pass messages from other UARTs or FTP worker */
if (_passing_on || _ftp_on) {
bool is_part;
uint8_t *read_ptr;
uint8_t *write_ptr;
pthread_mutex_lock(&_message_buffer_mutex);
int available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
pthread_mutex_unlock(&_message_buffer_mutex);
if (available > 0) {
// Reconstruct message from buffer
mavlink_message_t msg;
write_ptr = (uint8_t *)&msg;
// Pull a single message from the buffer
size_t read_count = available;
if (read_count > sizeof(mavlink_message_t)) {
read_count = sizeof(mavlink_message_t);
}
memcpy(write_ptr, read_ptr, read_count);
// We hold the mutex until after we complete the second part of the buffer. If we don't
// we may end up breaking the empty slot overflow detection semantics when we mark the
// possibly partial read below.
pthread_mutex_lock(&_message_buffer_mutex);
message_buffer_mark_read(read_count);
/* write second part of buffer if there is some */
if (is_part && read_count < sizeof(mavlink_message_t)) {
write_ptr += read_count;
available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
read_count = sizeof(mavlink_message_t) - read_count;
memcpy(write_ptr, read_ptr, read_count);
message_buffer_mark_read(available);
}
pthread_mutex_unlock(&_message_buffer_mutex);
_mavlink_resend_uart(_channel, &msg);
}
}
/* update TX/RX rates*/
if (t > _bytes_timestamp + 1000000) {
if (_bytes_timestamp != 0) {
float dt = (t - _bytes_timestamp) / 1000.0f;
_rate_tx = _bytes_tx / dt;
_rate_txerr = _bytes_txerr / dt;
_rate_rx = _bytes_rx / dt;
_bytes_tx = 0;
_bytes_txerr = 0;
_bytes_rx = 0;
}
_bytes_timestamp = t;
}
perf_end(_loop_perf);
}
