Quelle  usbip_network.c   Sprache: C

// SPDX-License-Identifier: GPL-2.0-or-later
/*
 * Copyright (C) 2011 matt mooney <mfm@muteddisk.com>
 *               2005-2007 Takahiro Hirofuchi
 */

#include <sys/socket.h>

#include <string.h>

#include <arpa/inet.h>
#include <netdb.h>
#include <netinet/tcp.h>
#include <unistd.h>

#ifdef HAVE_LIBWRAP
#include <tcpd.h>
#endif

#include "usbip_common.h"
#include "usbip_network.h"

int usbip_port = 3240;
char *usbip_port_string = "3240";

void usbip_setup_port_number(char *arg)
{
 dbg("parsing port arg '%s'", arg);
 char *end;
 unsigned long int port = strtoul(arg, &end, 10);

 if (end == arg) {
  err("port: could not parse '%s' as a decimal integer", arg);
  return;
 }

 if (*end != '\0') {
  err("port: garbage at end of '%s'", arg);
  return;
 }

 if (port > UINT16_MAX) {
  err("port: %s too high (max=%d)",
      arg, UINT16_MAX);
  return;
 }

 usbip_port = port;
 usbip_port_string = arg;
 info("using port %d (\"%s\")", usbip_port, usbip_port_string);
}

uint32_t usbip_net_pack_uint32_t(int pack, uint32_t num)
{
 uint32_t i;

 if (pack)
  i = htonl(num);
 else
  i = ntohl(num);

 return i;
}

uint16_t usbip_net_pack_uint16_t(int pack, uint16_t num)
{
 uint16_t i;

 if (pack)
  i = htons(num);
 else
  i = ntohs(num);

 return i;
}

void usbip_net_pack_usb_device(int pack, struct usbip_usb_device *udev)
{
 udev->busnum = usbip_net_pack_uint32_t(pack, udev->busnum);
 udev->devnum = usbip_net_pack_uint32_t(pack, udev->devnum);
 udev->speed = usbip_net_pack_uint32_t(pack, udev->speed);

 udev->idVendor = usbip_net_pack_uint16_t(pack, udev->idVendor);
 udev->idProduct = usbip_net_pack_uint16_t(pack, udev->idProduct);
 udev->bcdDevice = usbip_net_pack_uint16_t(pack, udev->bcdDevice);
}

void usbip_net_pack_usb_interface(int pack __attribute__((unused)),
      struct usbip_usb_interface *udev
      __attribute__((unused)))
{
 /* uint8_t members need nothing */
}

static ssize_t usbip_net_xmit(int sockfd, void *buff, size_t bufflen,
         int sending)
{
 ssize_t nbytes;
 ssize_t total = 0;

 if (!bufflen)
  return 0;

 do {
  if (sending)
   nbytes = send(sockfd, buff, bufflen, 0);
  else
   nbytes = recv(sockfd, buff, bufflen, MSG_WAITALL);

  if (nbytes <= 0)
   return -1;

  buff  = (void *)((intptr_t) buff + nbytes);
  bufflen -= nbytes;
  total += nbytes;

 } while (bufflen > 0);

 return total;
}

ssize_t usbip_net_recv(int sockfd, void *buff, size_t bufflen)
{
 return usbip_net_xmit(sockfd, buff, bufflen, 0);
}

ssize_t usbip_net_send(int sockfd, void *buff, size_t bufflen)
{
 return usbip_net_xmit(sockfd, buff, bufflen, 1);
}

static inline void usbip_net_pack_op_common(int pack,
         struct op_common *op_common)
{
 op_common->version = usbip_net_pack_uint16_t(pack, op_common->version);
 op_common->code = usbip_net_pack_uint16_t(pack, op_common->code);
 op_common->status = usbip_net_pack_uint32_t(pack, op_common->status);
}

int usbip_net_send_op_common(int sockfd, uint32_t code, uint32_t status)
{
 struct op_common op_common;
 int rc;

 memset(&op_common, 0, sizeof(op_common));

 op_common.version = USBIP_VERSION;
 op_common.code    = code;
 op_common.status  = status;

 usbip_net_pack_op_common(1, &op_common);

 rc = usbip_net_send(sockfd, &op_common, sizeof(op_common));
 if (rc < 0) {
  dbg("usbip_net_send failed: %d", rc);
  return -1;
 }

 return 0;
}

int usbip_net_recv_op_common(int sockfd, uint16_t *code, int *status)
{
 struct op_common op_common;
 int rc;

 memset(&op_common, 0, sizeof(op_common));

 rc = usbip_net_recv(sockfd, &op_common, sizeof(op_common));
 if (rc < 0) {
  dbg("usbip_net_recv failed: %d", rc);
  goto err;
 }

 usbip_net_pack_op_common(0, &op_common);

 if (op_common.version != USBIP_VERSION) {
  err("USBIP Kernel and tool version mismatch: %d %d:",
      op_common.version, USBIP_VERSION);
  goto err;
 }

 switch (*code) {
 case OP_UNSPEC:
  break;
 default:
  if (op_common.code != *code) {
   dbg("unexpected pdu %#0x for %#0x", op_common.code,
       *code);
   /* return error status */
   *status = ST_ERROR;
   goto err;
  }
 }

 *status = op_common.status;

 if (op_common.status != ST_OK) {
  dbg("request failed at peer: %d", op_common.status);
  goto err;
 }

 *code = op_common.code;

 return 0;
err:
 return -1;
}

int usbip_net_set_reuseaddr(int sockfd)
{
 const int val = 1;
 int ret;

 ret = setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &val, sizeof(val));
 if (ret < 0)
  dbg("setsockopt: SO_REUSEADDR");

 return ret;
}

int usbip_net_set_nodelay(int sockfd)
{
 const int val = 1;
 int ret;

 ret = setsockopt(sockfd, IPPROTO_TCP, TCP_NODELAY, &val, sizeof(val));
 if (ret < 0)
  dbg("setsockopt: TCP_NODELAY");

 return ret;
}

int usbip_net_set_keepalive(int sockfd)
{
 const int val = 1;
 int ret;

 ret = setsockopt(sockfd, SOL_SOCKET, SO_KEEPALIVE, &val, sizeof(val));
 if (ret < 0)
  dbg("setsockopt: SO_KEEPALIVE");

 return ret;
}

int usbip_net_set_v6only(int sockfd)
{
 const int val = 1;
 int ret;

 ret = setsockopt(sockfd, IPPROTO_IPV6, IPV6_V6ONLY, &val, sizeof(val));
 if (ret < 0)
  dbg("setsockopt: IPV6_V6ONLY");

 return ret;
}

/*
 * IPv6 Ready
 */
int usbip_net_tcp_connect(char *hostname, char *service)
{
 struct addrinfo hints, *res, *rp;
 int sockfd;
 int ret;

 memset(&hints, 0, sizeof(hints));
 hints.ai_family = AF_UNSPEC;
 hints.ai_socktype = SOCK_STREAM;

 /* get all possible addresses */
 ret = getaddrinfo(hostname, service, &hints, &res);
 if (ret < 0) {
  dbg("getaddrinfo: %s service %s: %s", hostname, service,
      gai_strerror(ret));
  return ret;
 }

 /* try the addresses */
 for (rp = res; rp; rp = rp->ai_next) {
  sockfd = socket(rp->ai_family, rp->ai_socktype,
    rp->ai_protocol);
  if (sockfd < 0)
   continue;

  /* should set TCP_NODELAY for usbip */
  usbip_net_set_nodelay(sockfd);
  /* TODO: write code for heartbeat */
  usbip_net_set_keepalive(sockfd);

  if (connect(sockfd, rp->ai_addr, rp->ai_addrlen) == 0)
   break;

  close(sockfd);
 }

 freeaddrinfo(res);

 if (!rp)
  return EAI_SYSTEM;

 return sockfd;
}