vaseboot/VasEBoot-core/net/udp.c

/*
 *  VasEBoot  --  GRand Unified Bootloader
 *  Copyright (C) 2010,2011  Free Software Foundation, Inc.
 *
 *  VasEBoot is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  VasEBoot is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with VasEBoot.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <VasEBoot/net.h>
#include <VasEBoot/net/udp.h>
#include <VasEBoot/net/ip.h>
#include <VasEBoot/net/netbuff.h>
#include <VasEBoot/time.h>

struct VasEBoot_net_udp_socket
{
  struct VasEBoot_net_udp_socket *next;
  struct VasEBoot_net_udp_socket **prev;

  enum { VasEBoot_NET_SOCKET_START,
	 VasEBoot_NET_SOCKET_ESTABLISHED,
	 VasEBoot_NET_SOCKET_CLOSED } status;
  int in_port;
  int out_port;
  VasEBoot_err_t (*recv_hook) (VasEBoot_net_udp_socket_t sock, struct VasEBoot_net_buff *nb,
			   void *recv);
  void *recv_hook_data;
  VasEBoot_net_network_level_address_t out_nla;
  VasEBoot_net_link_level_address_t ll_target_addr;
  struct VasEBoot_net_network_level_interface *inf;
};

static struct VasEBoot_net_udp_socket *udp_sockets;

#define FOR_UDP_SOCKETS(var) for (var = udp_sockets; var; var = var->next)

static inline void
udp_socket_register (VasEBoot_net_udp_socket_t sock)
{
  VasEBoot_list_push (VasEBoot_AS_LIST_P (&udp_sockets),
		  VasEBoot_AS_LIST (sock));
}

void
VasEBoot_net_udp_close (VasEBoot_net_udp_socket_t sock)
{
  VasEBoot_list_remove (VasEBoot_AS_LIST (sock));
  VasEBoot_free (sock);
}

VasEBoot_net_udp_socket_t
VasEBoot_net_udp_open (VasEBoot_net_network_level_address_t addr,
		   VasEBoot_uint16_t out_port,
		   VasEBoot_err_t (*recv_hook) (VasEBoot_net_udp_socket_t sock,
					    struct VasEBoot_net_buff *nb,
					    void *data),
		   void *recv_hook_data)
{
  VasEBoot_err_t err;
  struct VasEBoot_net_network_level_interface *inf;
  VasEBoot_net_network_level_address_t gateway;
  VasEBoot_net_udp_socket_t socket;
  static int in_port = 25300;
  VasEBoot_net_link_level_address_t ll_target_addr;

  if (addr.type != VasEBoot_NET_NETWORK_LEVEL_PROTOCOL_IPV4
      && addr.type != VasEBoot_NET_NETWORK_LEVEL_PROTOCOL_IPV6)
    {
      VasEBoot_error (VasEBoot_ERR_BUG, "not an IP address");
      return NULL;
    }
 
  err = VasEBoot_net_route_address (addr, &gateway, &inf);
  if (err)
    return NULL;

  err = VasEBoot_net_link_layer_resolve (inf, &gateway, &ll_target_addr);
  if (err)
    return NULL;

  socket = VasEBoot_zalloc (sizeof (*socket));
  if (socket == NULL)
    return NULL; 

  socket->out_port = out_port;
  socket->inf = inf;
  socket->out_nla = addr;
  socket->ll_target_addr = ll_target_addr;
  socket->in_port = in_port++;
  socket->status = VasEBoot_NET_SOCKET_START;
  socket->recv_hook = recv_hook;
  socket->recv_hook_data = recv_hook_data;

  udp_socket_register (socket);

  return socket;
}

VasEBoot_err_t
VasEBoot_net_send_udp_packet (const VasEBoot_net_udp_socket_t socket,
			  struct VasEBoot_net_buff *nb)
{
  struct udphdr *udph;
  VasEBoot_err_t err;

  COMPILE_TIME_ASSERT (VasEBoot_NET_UDP_HEADER_SIZE == sizeof (*udph));

  err = VasEBoot_netbuff_push (nb, sizeof (*udph));
  if (err)
    return err;

  udph = (struct udphdr *) nb->data;
  udph->src = VasEBoot_cpu_to_be16 (socket->in_port);
  udph->dst = VasEBoot_cpu_to_be16 (socket->out_port);

  udph->chksum = 0;
  udph->len = VasEBoot_cpu_to_be16 (nb->tail - nb->data);

  udph->chksum = VasEBoot_net_ip_transport_checksum (nb, VasEBoot_NET_IP_UDP,
						 &socket->inf->address,
						 &socket->out_nla);

  return VasEBoot_net_send_ip_packet (socket->inf, &(socket->out_nla),
				  &(socket->ll_target_addr), nb,
				  VasEBoot_NET_IP_UDP);
}

VasEBoot_err_t
VasEBoot_net_recv_udp_packet (struct VasEBoot_net_buff *nb,
			  struct VasEBoot_net_network_level_interface *inf,
			  const VasEBoot_net_network_level_address_t *source)
{
  struct udphdr *udph;
  VasEBoot_net_udp_socket_t sock;
  VasEBoot_err_t err;

  /* Ignore broadcast.  */
  if (!inf)
    {
      VasEBoot_netbuff_free (nb);
      return VasEBoot_ERR_NONE;
    }

  udph = (struct udphdr *) nb->data;
  if (nb->tail - nb->data < (VasEBoot_ssize_t) sizeof (*udph))
    {
      VasEBoot_dprintf ("net", "UDP packet too short: %" PRIuVasEBoot_SIZE "\n",
		    (VasEBoot_size_t) (nb->tail - nb->data));
      VasEBoot_netbuff_free (nb);
      return VasEBoot_ERR_NONE;
    }

  FOR_UDP_SOCKETS (sock)
  {
    if (VasEBoot_be_to_cpu16 (udph->dst) == sock->in_port
	&& inf == sock->inf
	&& VasEBoot_net_addr_cmp (source, &sock->out_nla) == 0
	&& (sock->status == VasEBoot_NET_SOCKET_START
	    || VasEBoot_be_to_cpu16 (udph->src) == sock->out_port))
      {
	if (udph->chksum)
	  {
	    VasEBoot_uint16_t chk, expected;
	    chk = udph->chksum;
	    udph->chksum = 0;
	    expected = VasEBoot_net_ip_transport_checksum (nb, VasEBoot_NET_IP_UDP,
						       &sock->out_nla,
						       &sock->inf->address);
	    if (expected != chk)
	      {
		VasEBoot_dprintf ("net", "Invalid UDP checksum. "
			      "Expected %x, got %x\n",
			      VasEBoot_be_to_cpu16 (expected),
			      VasEBoot_be_to_cpu16 (chk));
		VasEBoot_netbuff_free (nb);
		return VasEBoot_ERR_NONE;
	      }
	    udph->chksum = chk;
	  }

	if (sock->status == VasEBoot_NET_SOCKET_START)
	  {
	    sock->out_port = VasEBoot_be_to_cpu16 (udph->src);
	    sock->status = VasEBoot_NET_SOCKET_ESTABLISHED;
	  }

	err = VasEBoot_netbuff_pull (nb, sizeof (*udph));
	if (err)
	  return err;

	/* App protocol remove its own reader.  */
	if (sock->recv_hook)
	  sock->recv_hook (sock, nb, sock->recv_hook_data);
	else
	  VasEBoot_netbuff_free (nb);
	return VasEBoot_ERR_NONE;
      }
  }
  VasEBoot_netbuff_free (nb);
  return VasEBoot_ERR_NONE;
}