vaseboot/VasEBoot-core/net/ethernet.c

/*
 *  VAS_EBOOT  --  GRand Unified Bootloader
 *  Copyright (C) 2010,2011  Free Software Foundation, Inc.
 *
 *  VAS_EBOOT 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.
 *
 *  VAS_EBOOT 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 VAS_EBOOT.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <VasEBoot/misc.h>
#include <VasEBoot/mm.h>
#include <VasEBoot/env.h>
#include <VasEBoot/net/ethernet.h>
#include <VasEBoot/net/ip.h>
#include <VasEBoot/net/arp.h>
#include <VasEBoot/net/netbuff.h>
#include <VasEBoot/net.h>
#include <VasEBoot/time.h>
#include <VasEBoot/net/arp.h>

#define LLCADDRMASK 0x7f

struct etherhdr
{
  VasEBoot_uint8_t dst[6];
  VasEBoot_uint8_t src[6];
  VasEBoot_uint16_t type;
} VAS_EBOOT_PACKED;

struct llchdr
{
  VasEBoot_uint8_t dsap;
  VasEBoot_uint8_t ssap;
  VasEBoot_uint8_t ctrl;
} VAS_EBOOT_PACKED;

struct snaphdr
{
  VasEBoot_uint8_t oui[3];
  VasEBoot_uint16_t type;
} VAS_EBOOT_PACKED;

VasEBoot_err_t
send_ethernet_packet (struct VasEBoot_net_network_level_interface *inf,
		      struct VasEBoot_net_buff *nb,
		      VasEBoot_net_link_level_address_t target_addr,
		      VasEBoot_net_ethertype_t ethertype)
{
  struct etherhdr *eth;
  VasEBoot_err_t err;
  VasEBoot_uint8_t etherhdr_size;
  VasEBoot_uint16_t vlantag_id = VasEBoot_cpu_to_be16_compile_time (VLANTAG_IDENTIFIER);

  etherhdr_size = sizeof (*eth);
  COMPILE_TIME_ASSERT (sizeof (*eth) + 4 < VAS_EBOOT_NET_MAX_LINK_HEADER_SIZE);

  /* Increase ethernet header in case of vlantag */
  if (inf->vlantag != 0)
    etherhdr_size += 4;

  err = VasEBoot_netbuff_push (nb, etherhdr_size);
  if (err)
    return err;
  eth = (struct etherhdr *) nb->data;
  VasEBoot_memcpy (eth->dst, target_addr.mac, 6);
  VasEBoot_memcpy (eth->src, inf->hwaddress.mac, 6);

  eth->type = VasEBoot_cpu_to_be16 (ethertype);
  if (!inf->card->opened)
    {
      err = VAS_EBOOT_ERR_NONE;
      if (inf->card->driver->open)
	err = inf->card->driver->open (inf->card);
      if (err)
	return err;
      inf->card->opened = 1;
    }

  /* Check and add a vlan-tag if needed. */
  if (inf->vlantag != 0)
    {
      /* Move eth type to the right */
      VasEBoot_memcpy ((char *) nb->data + etherhdr_size - 2,
                   (char *) nb->data + etherhdr_size - 6, 2);

      /* Add the tag in the middle */
      VasEBoot_uint16_t vlan = VasEBoot_cpu_to_be16 (inf->vlantag);
      VasEBoot_memcpy ((char *) nb->data + etherhdr_size - 6, &vlantag_id, 2);
      VasEBoot_memcpy ((char *) nb->data + etherhdr_size - 4, &vlan, 2);
    }

  return inf->card->driver->send (inf->card, nb);
}

VasEBoot_err_t
VasEBoot_net_recv_ethernet_packet (struct VasEBoot_net_buff *nb,
			       struct VasEBoot_net_card *card)
{
  struct etherhdr *eth;
  struct llchdr *llch;
  struct snaphdr *snaph;
  VasEBoot_net_ethertype_t type;
  VasEBoot_net_link_level_address_t hwaddress;
  VasEBoot_net_link_level_address_t src_hwaddress;
  VasEBoot_err_t err;
  VasEBoot_uint8_t etherhdr_size = sizeof (*eth);
  VasEBoot_uint16_t vlantag = 0;


  /* Check if a vlan-tag is present. If so, the ethernet header is 4 bytes */
  /* longer than the original one. The vlantag id is extracted and the header */
  /* is reseted to the original size. */
  if (VasEBoot_get_unaligned16 (nb->data + etherhdr_size - 2) == VasEBoot_cpu_to_be16_compile_time (VLANTAG_IDENTIFIER))
    {
      vlantag = VasEBoot_be_to_cpu16 (VasEBoot_get_unaligned16 (nb->data + etherhdr_size));
      etherhdr_size += 4;
      /* Move eth type to the original position */
      VasEBoot_memcpy((char *) nb->data + etherhdr_size - 6,
                  (char *) nb->data + etherhdr_size - 2, 2);
    }

  eth = (struct etherhdr *) nb->data;
  type = VasEBoot_be_to_cpu16 (eth->type);
  err = VasEBoot_netbuff_pull (nb, etherhdr_size);
  if (err)
    return err;

  if (type <= 1500)
    {
      llch = (struct llchdr *) nb->data;
      type = llch->dsap & LLCADDRMASK;

      if (llch->dsap == 0xaa && llch->ssap == 0xaa && llch->ctrl == 0x3)
	{
	  err = VasEBoot_netbuff_pull (nb, sizeof (*llch));
	  if (err)
	    return err;
	  snaph = (struct snaphdr *) nb->data;
	  type = snaph->type;
	}
    }

  hwaddress.type = VAS_EBOOT_NET_LINK_LEVEL_PROTOCOL_ETHERNET;
  VasEBoot_memcpy (hwaddress.mac, eth->dst, sizeof (hwaddress.mac));
  src_hwaddress.type = VAS_EBOOT_NET_LINK_LEVEL_PROTOCOL_ETHERNET;
  VasEBoot_memcpy (src_hwaddress.mac, eth->src, sizeof (src_hwaddress.mac));

  switch (type)
    {
      /* ARP packet. */
    case VAS_EBOOT_NET_ETHERTYPE_ARP:
      VasEBoot_net_arp_receive (nb, card, &vlantag);
      VasEBoot_netbuff_free (nb);
      return VAS_EBOOT_ERR_NONE;
      /* IP packet.  */
    case VAS_EBOOT_NET_ETHERTYPE_IP:
    case VAS_EBOOT_NET_ETHERTYPE_IP6:
      return VasEBoot_net_recv_ip_packets (nb, card, &hwaddress, &src_hwaddress,
                                       &vlantag);
    }
  VasEBoot_netbuff_free (nb);
  return VAS_EBOOT_ERR_NONE;
}