tty-fixup.cpp

#include <charconv>
#include <string_view>

#include <asm/ioctls.h>
#include <asm/termbits.h>
#include <errno.h>
#include <fcntl.h>
#include <limits.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/syscall.h>
#include <unistd.h>

using namespace std::literals;

namespace
{
	// Copied from /usr/include/asm-generic/termbits.h
	constexpr int KERNEL_NCCS = 19;

	struct kernel_termios
	{
		tcflag_t c_iflag;		/* input mode flags */
		tcflag_t c_oflag;		/* output mode flags */
		tcflag_t c_cflag;		/* control mode flags */
		tcflag_t c_lflag;		/* local mode flags */
		cc_t c_line;			/* line discipline */
		cc_t c_cc[KERNEL_NCCS];		/* control characters */
	};

	char ttyname_buf[PATH_MAX];
}

extern "C" __attribute__((visibility("default"))) int tcgetattr(int fd, struct termios *termios_p)
{
	// copied from glibc commit 5cf101a85aae0d703cdd8ed7b25fe288e41fdacb^ with minor fixups
	struct kernel_termios k_termios;
	int retval;

	retval = syscall(SYS_ioctl, fd, TCGETS, &k_termios);

	if (retval == 0) [[likely]]
	{
		termios_p->c_iflag = k_termios.c_iflag;
		termios_p->c_oflag = k_termios.c_oflag;
		termios_p->c_cflag = k_termios.c_cflag;
		termios_p->c_lflag = k_termios.c_lflag;
		termios_p->c_line = k_termios.c_line;
		if constexpr (sizeof (cc_t) == 1 || _POSIX_VDISABLE == 0
				|| (unsigned char) _POSIX_VDISABLE == (unsigned char) -1)
			memset (mempcpy (&termios_p->c_cc[0], &k_termios.c_cc[0],
						KERNEL_NCCS * sizeof (cc_t)),
					_POSIX_VDISABLE, (NCCS - KERNEL_NCCS) * sizeof (cc_t));
		else
		{
			memcpy (&termios_p->c_cc[0], &k_termios.c_cc[0],
					KERNEL_NCCS * sizeof (cc_t));

			for (size_t cnt = KERNEL_NCCS; cnt < NCCS; ++cnt)
				termios_p->c_cc[cnt] = _POSIX_VDISABLE;
		}
	}

	return retval;
}

extern "C" __attribute__((visibility("default"))) int isatty(int fd)
{
	termios tmp;
	return tcgetattr(fd, &tmp) == 0;
}

extern "C" __attribute__((visibility("default"))) int ttyname_r(int fd, char *buf, size_t buflen)
{
	{
		termios tmp;
		if (tcgetattr(fd, &tmp) < 0)
			return errno;
	}
	static constexpr auto prefix = "/proc/self/fd/"sv;
	// prefix + decimal int + terminating NUL
	char link_buf[prefix.size() + (sizeof(int)*CHAR_BIT + 2) / 3 + 1];

	auto out_start = std::copy(begin(prefix), end(prefix), std::begin(link_buf));
	auto result = std::to_chars(out_start, std::end(link_buf), fd);
	if (result.ec != std::errc{})
		return static_cast<int>(result.ec);
	*result.ptr = '\0';

	auto readlink_result = readlink(link_buf, buf, buflen);
	if (readlink_result == -1)
		return errno;
	if (readlink_result >= buflen)
		return ERANGE;
	buf[readlink_result] = '\0';
	return 0;
}

extern "C" __attribute__((visibility("default"))) char *ttyname(int fd)
{
	auto err = ttyname_r(fd, ttyname_buf, sizeof(ttyname_buf));
	if (err == 0)
	{
		errno = 0;
		return ttyname_buf;
	}
	errno = err;
	return nullptr;
}

extern "C" __attribute__((visibility("default"))) int tcsetattr(int fd, int optional_actions, const struct termios *termios_p)
{
  struct kernel_termios k_termios;
  unsigned long int cmd;

  switch (optional_actions)
    {
    case TCSANOW:
      cmd = TCSETS;
      break;
    case TCSADRAIN:
      cmd = TCSETSW;
      break;
    case TCSAFLUSH:
      cmd = TCSETSF;
      break;
    default:
      errno = EINVAL;
      return -1;
    }

  static constexpr int IBAUD0 = 020000000000;
  k_termios.c_iflag = termios_p->c_iflag & ~IBAUD0;
  k_termios.c_oflag = termios_p->c_oflag;
  k_termios.c_cflag = termios_p->c_cflag;
  k_termios.c_lflag = termios_p->c_lflag;
  k_termios.c_line = termios_p->c_line;
  memcpy (&k_termios.c_cc[0], &termios_p->c_cc[0],
	  KERNEL_NCCS * sizeof (cc_t));
  return syscall(SYS_ioctl, fd, cmd, &k_termios);
}

extern "C" __attribute__((visibility("default"))) int openpty (int *pptmx, int *pterminal, char *name, const struct termios *termp, const struct winsize *winp)
{
  char _buf[PATH_MAX];
  int ptmx, ret = -1, terminal = -1;

  *_buf = '\0';

  ptmx = getpt ();
  if (ptmx == -1)
    return -1;

  if (grantpt (ptmx))
    goto on_error;

  if (unlockpt (ptmx))
    goto on_error;

#ifdef TIOCGPTPEER
  /* Try to allocate terminal fd solely based on PTMX fd first. */
  terminal = ioctl(ptmx, TIOCGPTPEER, O_RDWR | O_NOCTTY);
#endif
  if (terminal == -1)
    {
      /* Fallback to path-based terminal fd allocation in case kernel doesn't
       * support TIOCGPTPEER.
       */
      if (ptsname_r (ptmx, _buf, sizeof (_buf)))
        goto on_error;

      terminal = open (_buf, O_RDWR | O_NOCTTY);
      if (terminal == -1)
        goto on_error;
    }

  /* XXX Should we ignore errors here?  */
  if (termp)
    tcsetattr (terminal, TCSAFLUSH, termp);
#ifdef TIOCSWINSZ
  if (winp)
    ioctl (terminal, TIOCSWINSZ, winp);
#endif

  *pptmx = ptmx;
  *pterminal = terminal;
  if (name != NULL)
    {
      if (*_buf == '\0')
        if (ptsname_r (ptmx, _buf, sizeof (_buf)))
          goto on_error;

      strcpy (name, _buf);
    }

  ret = 0;

 on_error:
  if (ret == -1) {
    close (ptmx);

    if (terminal != -1)
      close (terminal);
  }

  return ret;
}