[people/pmueller/ipfire-2.x.git] / src / hwinfo / src / hd / pppoe.c


/*
 *  License: GPL
 *
 *  Much inspired by rp-pppoe from Roaring Penguin Software Inc.
 *  which itself is inspired by earlier code from Luke Stras.
 */

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <fcntl.h>
#include <errno.h>
#include <string.h>
#include <linux/if.h>
#include <net/ethernet.h>
#include <net/if_arp.h>
#include <netinet/in.h>
#include <netpacket/packet.h>

#include "hd.h"
#include "hd_int.h"
#include "pppoe.h"

static hd_data_t *hd_data;

/* Ethernet Frame Types */
#define ETH_PPPOE_DISCOVERY	0x8863
#define ETH_PPPOE_SESSION	0x8864

/* PPPoE Codes */
#define CODE_PADI		0x09
#define CODE_PADO		0x07
#define CODE_PADR		0x19
#define CODE_PADS		0x65
#define CODE_PADT		0xA7

/* PPPoE Tags */
#define TAG_END_OF_LIST		0x0000
#define TAG_SERVICE_NAME	0x0101
#define TAG_AC_NAME		0x0102
#define TAG_HOST_UNIQ		0x0103
#define TAG_AC_COOKIE		0x0104
#define TAG_VENDOR_SPECIFIC	0x0105
#define TAG_RELAY_SESSION_ID	0x0110
#define TAG_SERVICE_NAME_ERROR	0x0201
#define TAG_AC_SYSTEM_ERROR	0x0202
#define TAG_GENERIC_ERROR	0x0203

/* Number of Attempts */
#define MAX_ATTEMPTS		2

/* Timeout for PADO Packets */
#define PADO_TIMEOUT		3

/* A PPPoE Packet, including Ethernet headers */
typedef struct PPPoEPacketStruct {
    struct ethhdr ethHdr;	/* Ethernet header */
    unsigned int ver:4;		/* PPPoE Version (must be 1) */
    unsigned int type:4;	/* PPPoE Type (must be 1) */
    unsigned int code:8;	/* PPPoE code */
    unsigned int session:16;	/* PPPoE session */
    unsigned int length:16;	/* Payload length */
    unsigned char payload[ETH_DATA_LEN];	/* A bit of room to spare */
} PPPoEPacket;

/* Header size of a PPPoE Packet */
#define PPPOE_OVERHEAD 6	/* type, code, session, length */
#define HDR_SIZE (sizeof (struct ethhdr) + PPPOE_OVERHEAD)
#define MAX_PPPOE_PAYLOAD (ETH_DATA_LEN - PPPOE_OVERHEAD)

/* PPPoE Tag */
typedef struct PPPoETagStruct {
    unsigned int type:16;	/* tag type */
    unsigned int length:16;	/* Length of payload */
    unsigned char payload[ETH_DATA_LEN];	/* A LOT of room to spare */
} PPPoETag;

/* Header size of a PPPoE Tag */
#define TAG_HDR_SIZE 4

/* Function passed to parse_packet */
typedef void parse_func (uint16_t type, uint16_t len,
			 unsigned char* data, void* extra);

/* Keep track of the state of a connection */
typedef struct PPPoEConnectionStruct {
    char* ifname;		/* Interface name */
    int fd;			/* Raw socket for discovery frames */
    int received_pado;		/* Where we are in discovery */
    unsigned char my_mac[ETH_ALEN];	/* My MAC address */
    unsigned char peer_mac[ETH_ALEN];	/* Peer's MAC address */
    hd_t *hd;
} PPPoEConnection;

/* Structure used to determine acceptable PADO packet */
typedef struct PacketCriteriaStruct {
    PPPoEConnection* conn;
    int acname_ok;
    int servicename_ok;
    int error;
} PacketCriteria;

/* True if Ethernet address is broadcast or multicast */
#define NOT_UNICAST(e) ((e[0] & 0x01) != 0)


static int
check_room (PPPoEConnection* conn, unsigned char* cursor, unsigned char* start,
	    uint16_t len)
{
    if (cursor - start + len > MAX_PPPOE_PAYLOAD) {
	ADD2LOG ("%s: Would create too-long packet\n", conn->ifname);
	return 0;
    }
    return 1;
}


static int
parse_packet (PPPoEConnection* conn, PPPoEPacket* packet, parse_func* func,
	      void* extra)
{
    uint16_t len = ntohs (packet->length);
    unsigned char* curTag;
    uint16_t tagType, tagLen;

    if (packet->ver != 1) {
	ADD2LOG ("%s: Invalid PPPoE version (%d)\n", conn->ifname,
		 (int) packet->ver);
	return 0;
    }

    if (packet->type != 1) {
	ADD2LOG ("%s: Invalid PPPoE type (%d)\n", conn->ifname,
		 (int) packet->type);
	return 0;
    }

    /* Do some sanity checks on packet. */
    if (len > ETH_DATA_LEN - 6) { /* 6-byte overhead for PPPoE header */
	ADD2LOG ("%s: Invalid PPPoE packet length (%u)\n", conn->ifname, len);
	return 0;
    }

    /* Step through the tags. */
    curTag = packet->payload;
    while (curTag - packet->payload < len) {
	/* Alignment is not guaranteed, so do this by hand. */
	tagType = (((uint16_t) curTag[0]) << 8) + (uint16_t) curTag[1];
	tagLen = (((uint16_t) curTag[2]) << 8) + (uint16_t) curTag[3];
	if (tagType == TAG_END_OF_LIST)
	    break;
	if ((curTag - packet->payload) + tagLen + TAG_HDR_SIZE > len) {
	    ADD2LOG ("%s: Invalid PPPoE tag length (%u)\n", conn->ifname,
		     tagLen);
	    return 0;
	}
	func (tagType, tagLen, curTag + TAG_HDR_SIZE, extra);
	curTag = curTag + TAG_HDR_SIZE + tagLen;
    }

    return 1;
}


static int
open_interfaces (int n, PPPoEConnection* conns)
{
    int ret = 0, i;

    for (i = 0; i < n; i++)
    {
	PPPoEConnection* conn = &conns[i];

	conn->fd = socket (PF_PACKET, SOCK_RAW, htons (ETH_PPPOE_DISCOVERY));
	if (conn->fd < 0) {
	    ADD2LOG ("%s: socket failed: %m\n", conn->ifname);
	    continue;
	}

	int one = 1;
	if (setsockopt (conn->fd, SOL_SOCKET, SO_BROADCAST, &one,
			sizeof (one)) < 0) {
	    ADD2LOG ("%s: setsockopt failed: %m\n", conn->ifname);
	    goto error;
	}

	/* Fill in hardware address */
	struct ifreq ifr;
	struct sockaddr_ll sa;
	memset (&sa, 0, sizeof (sa));
	strncpy (ifr.ifr_name, conn->ifname, sizeof (ifr.ifr_name));
	if (ioctl (conn->fd, SIOCGIFHWADDR, &ifr) < 0) {
	    ADD2LOG ("%s: ioctl (SIOCGIFHWADDR) failed: %m\n", conn->ifname);
	    goto error;
	}

	memcpy (conn->my_mac, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
	if (ifr.ifr_hwaddr.sa_family != ARPHRD_ETHER) {
	    ADD2LOG ("%s: Interface is not ethernet\n", conn->ifname);
	    goto error;
	}

	if (NOT_UNICAST (conn->my_mac)) {
	    ADD2LOG ("%s: Interface has broadcast/multicast MAC address?\n",
		    conn->ifname);
	    goto error;
	}

	/* Sanity check on MTU */
	strncpy (ifr.ifr_name, conn->ifname, sizeof (ifr.ifr_name));
	if (ioctl (conn->fd, SIOCGIFMTU, &ifr) < 0) {
	    ADD2LOG ("%s: ioctl (SIOCGIFMTU) failed: %m\n", conn->ifname);
	    goto error;
	}
	if (ifr.ifr_mtu < ETH_DATA_LEN) {
	    ADD2LOG ("%s: Interface has to low MTU\n", conn->ifname);
	    goto error;
	}

	/* Get interface index */
	sa.sll_family = AF_PACKET;
	sa.sll_protocol = htons (ETH_PPPOE_DISCOVERY);
	strncpy (ifr.ifr_name, conn->ifname, sizeof (ifr.ifr_name));
	if (ioctl (conn->fd, SIOCGIFINDEX, &ifr) < 0) {
	    ADD2LOG ("%s: ioctl (SIOCFIGINDEX) failed: Could not get interface "
		     "index\n", conn->ifname);
	    goto error;
	}
	sa.sll_ifindex = ifr.ifr_ifindex;

	/* We're only interested in packets on specified interface */
	if (bind (conn->fd, (struct sockaddr*) &sa, sizeof (sa)) < 0) {
	    ADD2LOG ("%s: bind failed: %m\n", conn->ifname);
	    goto error;
	}

	ret = 1;
	continue;

error:

	close (conn->fd);
	conn->fd = -1;

    }

    return ret;
}


static void
close_intefaces (int n, PPPoEConnection* conns)
{
    int i;

    for (i = 0; i < n; i++)
    {
	PPPoEConnection* conn = &conns[i];

	if (conn->fd != -1) {
	    close (conn->fd);
	    conn->fd = -1;
	}
    }
}


static int
send_packet (int fd, PPPoEPacket* pkt, size_t size)
{
    if (send (fd, pkt, size, 0) < 0) {
	ADD2LOG ("send failed: %m\n");
	return 0;
    }

    return 1;
}


static int
receive_packet (int fd, PPPoEPacket* pkt, size_t* size)
{
    int r = recv (fd, pkt, sizeof (PPPoEPacket), 0);
    if (r < 0) {
	ADD2LOG ("recv failed: %m\n");
	return 0;
    }

    *size = r;
    return 1;
}


static void
parse_hostuniq (uint16_t type, uint16_t len, unsigned char* data, void* extra)
{
    if (type == TAG_HOST_UNIQ && len == sizeof (pid_t)) {
	pid_t tmp;
	memcpy (&tmp, data, len);
	if (tmp == getpid ()) {
	    int* val = (int*) extra;
	    *val = 1;
	}
    }
}


static int
packet_for_me (PPPoEConnection* conn, PPPoEPacket* packet)
{
    /* If packet is not directed to our MAC address, forget it. */
    if (memcmp (packet->ethHdr.h_dest, conn->my_mac, ETH_ALEN))
	return 0;

    /* Check for HostUniq tag. */
    int for_me = 0;
    parse_packet (conn, packet, parse_hostuniq, &for_me);
    return for_me;
}


static void
parse_pado_tags (uint16_t type, uint16_t len, unsigned char* data, void* extra)
{
    PacketCriteria* pc = (PacketCriteria*) extra;
    PPPoEConnection *conn = pc->conn;

    switch (type) {
	case TAG_AC_NAME:
	    pc->acname_ok = 1;
	    ADD2LOG ("%s: Service-Name is: %.*s\n", conn->ifname, (int) len,
		     data);
	    break;
	case TAG_SERVICE_NAME:
	    pc->servicename_ok = len == 0;
	    break;
	case TAG_SERVICE_NAME_ERROR:
	    ADD2LOG ("%s: Service-Name-Error: %.*s\n", conn->ifname, (int) len,
		     data);
	    pc->error = 1;
	    break;
	case TAG_AC_SYSTEM_ERROR:
	    ADD2LOG ("%s: System-Error: %.*s\n", conn->ifname, (int) len, data);
	    pc->error = 1;
	    break;
	case TAG_GENERIC_ERROR:
	    ADD2LOG ("%s: Generic-Error: %.*s\n", conn->ifname, (int) len, data);
	    pc->error = 1;
	    break;
    }
}


static int
send_padi (int n, PPPoEConnection* conns)
{
    int ret = 0, i;

    for (i = 0; i < n; i++)
    {
	PPPoEConnection* conn = &conns[i];

	if (conn->fd == -1 || conn->received_pado)
	    continue;

	PPPoEPacket packet;
	unsigned char* cursor = packet.payload;
	PPPoETag* svc = (PPPoETag*) (&packet.payload);
	uint16_t namelen = 0;
	uint16_t plen;

	namelen = 0;
	plen = TAG_HDR_SIZE + namelen;
	if (!check_room (conn, cursor, packet.payload, TAG_HDR_SIZE))
	    continue;

	/* Set destination to Ethernet broadcast address */
	memset (packet.ethHdr.h_dest, 0xFF, ETH_ALEN);
	memcpy (packet.ethHdr.h_source, conn->my_mac, ETH_ALEN);

	packet.ethHdr.h_proto = htons (ETH_PPPOE_DISCOVERY);
	packet.ver = 1;
	packet.type = 1;
	packet.code = CODE_PADI;
	packet.session = 0;

	svc->type = TAG_SERVICE_NAME;
	svc->length = htons (0);
	if (!check_room (conn, cursor, packet.payload, namelen + TAG_HDR_SIZE))
	    continue;

	cursor += namelen + TAG_HDR_SIZE;

	PPPoETag hostUniq;
	pid_t pid = getpid ();
	hostUniq.type = htons (TAG_HOST_UNIQ);
	hostUniq.length = htons (sizeof (pid));
	memcpy (hostUniq.payload, &pid, sizeof (pid));
	if (!check_room (conn, cursor, packet.payload, sizeof (pid) + TAG_HDR_SIZE))
	    continue;
	memcpy (cursor, &hostUniq, sizeof (pid) + TAG_HDR_SIZE);
	cursor += sizeof (pid) + TAG_HDR_SIZE;
	plen += sizeof (pid) + TAG_HDR_SIZE;

	packet.length = htons (plen);

	ADD2LOG ("%s: Sending PADI packet\n", conn->ifname);

	if (send_packet (conn->fd, &packet, (int) (plen + HDR_SIZE)))
	    ret = 1;
    }

    return ret;
}


static int
wait_for_pado (int n, PPPoEConnection* conns)
{
    int r, i, all;
    size_t len;
    fd_set readable;
    PPPoEPacket packet;
    PacketCriteria pc;

    struct timeval tv;
    tv.tv_sec = PADO_TIMEOUT;
    tv.tv_usec = 0;

    while (1)
    {
	FD_ZERO (&readable);
	for (i = 0; i < n; i++)
	    if (conns[i].fd != -1)
		FD_SET (conns[i].fd, &readable);

	do {
	    r = select (FD_SETSIZE, &readable, NULL, NULL, &tv);
	} while (r == -1 && errno == EINTR);

	if (r < 0) {
	    ADD2LOG ("select: %m\n");
	    return 0;
	}

	if (r == 0) {
	    ADD2LOG ("Timeout waiting for PADO packets\n");
	    return 0;
	}

	for (i = 0; i < n; i++)
	{
	    PPPoEConnection* conn = &conns[i];

	    if (conn->fd == -1 || !FD_ISSET (conn->fd, &readable))
		continue;

	    pc.conn = conn;
	    pc.acname_ok = 0;
	    pc.servicename_ok = 0;
	    pc.error = 0;

	    /* Get the packet */
	    if (!receive_packet (conn->fd, &packet, &len))
		continue;

	    /* Check length */
	    if (ntohs (packet.length) + HDR_SIZE > len) {
		ADD2LOG ("%s: Bogus PPPoE length field (%u)\n", conn->ifname,
			(unsigned int) ntohs (packet.length));
		continue;
	    }

	    /* If it's not for us, loop again */
	    if (!packet_for_me (conn, &packet))
		continue;

	    if (packet.code != CODE_PADO)
		continue;

	    if (NOT_UNICAST (packet.ethHdr.h_source)) {
		ADD2LOG ("%s: Ignoring PADO packet from non-unicast MAC "
			 "address\n", conn->ifname);
		continue;
	    }

	    parse_packet (conn, &packet, parse_pado_tags, &pc);

	    if (!pc.acname_ok) {
		ADD2LOG ("%s: Wrong or missing AC-Name tag\n", conn->ifname);
		continue;
	    }

	    if (!pc.servicename_ok) {
		ADD2LOG ("%s: Wrong or missing Service-Name tag\n",
			 conn->ifname);
		continue;
	    }

	    if (pc.error) {
		ADD2LOG ("%s: Ignoring PADO packet with some Error tag\n",
			 conn->ifname);
		continue;
	    }

	    memcpy (conn->peer_mac, packet.ethHdr.h_source, ETH_ALEN);
	    ADD2LOG ("%s: Received correct PADO packet\n", conn->ifname);
	    conn->received_pado = 1;
	}

	all = 1;
	for (i = 0; i < n; i++)
	    if (conns[i].fd != -1 && !conns[i].received_pado)
		all = 0;
	if (all)
	    return 1;
    }
}


static void
discovery (int n, PPPoEConnection* conns)
{
    int a;

    if (open_interfaces (n, conns))
    {
	for (a = 0; a < MAX_ATTEMPTS; a++)
	{
	    ADD2LOG ("Attempt number %d\n", a + 1);

	    if (!send_padi (n, conns))
		break;

	    if (wait_for_pado (n, conns))
		break;
	}
    }

    close_intefaces (n, conns);
}


void hd_scan_pppoe(hd_data_t *hd_data2)
{
  hd_t *hd;
  int cnt, interfaces;
  PPPoEConnection *conn;

  hd_data = hd_data2;

  if(!hd_probe_feature(hd_data, pr_pppoe)) return;

  hd_data->module = mod_pppoe;

  PROGRESS(1, 0, "looking for pppoe");

  for(interfaces = 0, hd = hd_data->hd; hd; hd = hd->next) {
    if(
      hd->base_class.id == bc_network_interface &&
      hd->sub_class.id == sc_nif_ethernet &&
      hd->unix_dev_name
    ) {
      interfaces++;
    }
  }

  if(!interfaces) return;

  conn = new_mem(interfaces * sizeof *conn);

  for(cnt = 0, hd = hd_data->hd; hd && cnt < interfaces; hd = hd->next) {
    if(
      hd->base_class.id == bc_network_interface &&
      hd->sub_class.id == sc_nif_ethernet &&
      hd->unix_dev_name
    ) {
      conn[cnt].hd = hd;
      conn[cnt].fd = -1;
      conn[cnt].ifname = hd->unix_dev_name;
      cnt++;
    }
  }

  PROGRESS(2, 0, "discovery");

  discovery(interfaces, conn);

  for(cnt = 0; cnt < interfaces; cnt++) {
    conn[cnt].hd->is.pppoe = 0;

    if(conn[cnt].received_pado) {
      conn[cnt].hd->is.pppoe = 1;
      ADD2LOG(
        "pppoe %s: my mac %02x:%02x:%02x:%02x:%02x:%02x, "
        "peer mac %02x:%02x:%02x:%02x:%02x:%02x\n",
        conn[cnt].ifname,
        conn[cnt].my_mac[0], conn[cnt].my_mac[1], conn[cnt].my_mac[2],
        conn[cnt].my_mac[3], conn[cnt].my_mac[4], conn[cnt].my_mac[5],
        conn[cnt].peer_mac[0], conn[cnt].peer_mac[1], conn[cnt].peer_mac[2],
        conn[cnt].peer_mac[3], conn[cnt].peer_mac[4], conn[cnt].peer_mac[5]
      );
    }
  }
}
Commit	Line	Data
a6316ce4 MT	1
	2	/*
	3	* License: GPL
	4	*
	5	* Much inspired by rp-pppoe from Roaring Penguin Software Inc.
	6	* which itself is inspired by earlier code from Luke Stras.
	7	*/
	8
	9	#include <stdio.h>
	10	#include <stdlib.h>
	11	#include <stdint.h>
	12	#include <unistd.h>
	13	#include <sys/types.h>
	14	#include <sys/ioctl.h>
	15	#include <sys/socket.h>
	16	#include <fcntl.h>
	17	#include <errno.h>
	18	#include <string.h>
	19	#include <linux/if.h>
	20	#include <net/ethernet.h>
	21	#include <net/if_arp.h>
	22	#include <netinet/in.h>
	23	#include <netpacket/packet.h>
	24
	25	#include "hd.h"
	26	#include "hd_int.h"
	27	#include "pppoe.h"
	28
	29	static hd_data_t *hd_data;
	30
	31	/* Ethernet Frame Types */
	32	#define ETH_PPPOE_DISCOVERY 0x8863
	33	#define ETH_PPPOE_SESSION 0x8864
	34
	35	/* PPPoE Codes */
	36	#define CODE_PADI 0x09
	37	#define CODE_PADO 0x07
	38	#define CODE_PADR 0x19
	39	#define CODE_PADS 0x65
	40	#define CODE_PADT 0xA7
	41
	42	/* PPPoE Tags */
	43	#define TAG_END_OF_LIST 0x0000
	44	#define TAG_SERVICE_NAME 0x0101
	45	#define TAG_AC_NAME 0x0102
	46	#define TAG_HOST_UNIQ 0x0103
	47	#define TAG_AC_COOKIE 0x0104
	48	#define TAG_VENDOR_SPECIFIC 0x0105
	49	#define TAG_RELAY_SESSION_ID 0x0110
	50	#define TAG_SERVICE_NAME_ERROR 0x0201
	51	#define TAG_AC_SYSTEM_ERROR 0x0202
	52	#define TAG_GENERIC_ERROR 0x0203
	53
	54	/* Number of Attempts */
	55	#define MAX_ATTEMPTS 2
	56
	57	/* Timeout for PADO Packets */
	58	#define PADO_TIMEOUT 3
	59
	60	/* A PPPoE Packet, including Ethernet headers */
	61	typedef struct PPPoEPacketStruct {
	62	struct ethhdr ethHdr; /* Ethernet header */
	63	unsigned int ver:4; /* PPPoE Version (must be 1) */
	64	unsigned int type:4; /* PPPoE Type (must be 1) */
65	unsigned int code:8; /* PPPoE code */
66	unsigned int session:16; /* PPPoE session */
67	unsigned int length:16; /* Payload length */
68	unsigned char payload[ETH_DATA_LEN]; /* A bit of room to spare */
69	} PPPoEPacket;
70
71	/* Header size of a PPPoE Packet */
72	#define PPPOE_OVERHEAD 6 /* type, code, session, length */
73	#define HDR_SIZE (sizeof (struct ethhdr) + PPPOE_OVERHEAD)
74	#define MAX_PPPOE_PAYLOAD (ETH_DATA_LEN - PPPOE_OVERHEAD)
75
76	/* PPPoE Tag */
77	typedef struct PPPoETagStruct {
78	unsigned int type:16; /* tag type */
79	unsigned int length:16; /* Length of payload */
80	unsigned char payload[ETH_DATA_LEN]; /* A LOT of room to spare */
81	} PPPoETag;
82
83	/* Header size of a PPPoE Tag */
84	#define TAG_HDR_SIZE 4
85
86	/* Function passed to parse_packet */
87	typedef void parse_func (uint16_t type, uint16_t len,
88	unsigned char* data, void* extra);
89
90	/* Keep track of the state of a connection */
91	typedef struct PPPoEConnectionStruct {
92	char* ifname; /* Interface name */
93	int fd; /* Raw socket for discovery frames */
94	int received_pado; /* Where we are in discovery */
95	unsigned char my_mac[ETH_ALEN]; /* My MAC address */
96	unsigned char peer_mac[ETH_ALEN]; /* Peer's MAC address */
97	hd_t *hd;
98	} PPPoEConnection;
99
100	/* Structure used to determine acceptable PADO packet */
101	typedef struct PacketCriteriaStruct {
102	PPPoEConnection* conn;
103	int acname_ok;
104	int servicename_ok;
105	int error;
106	} PacketCriteria;
107
108	/* True if Ethernet address is broadcast or multicast */
109	#define NOT_UNICAST(e) ((e[0] & 0x01) != 0)
110
111
112	static int
113	check_room (PPPoEConnection* conn, unsigned char* cursor, unsigned char* start,
114	uint16_t len)
115	{
116	if (cursor - start + len > MAX_PPPOE_PAYLOAD) {
117	ADD2LOG ("%s: Would create too-long packet\n", conn->ifname);
118	return 0;
119	}
120	return 1;
121	}
122
123
124	static int
125	parse_packet (PPPoEConnection* conn, PPPoEPacket* packet, parse_func* func,
126	void* extra)
127	{
128	uint16_t len = ntohs (packet->length);
129	unsigned char* curTag;
130	uint16_t tagType, tagLen;
131
132	if (packet->ver != 1) {
133	ADD2LOG ("%s: Invalid PPPoE version (%d)\n", conn->ifname,
134	(int) packet->ver);
135	return 0;
136	}
137
138	if (packet->type != 1) {
139	ADD2LOG ("%s: Invalid PPPoE type (%d)\n", conn->ifname,
140	(int) packet->type);
141	return 0;
142	}
143
144	/* Do some sanity checks on packet. */
145	if (len > ETH_DATA_LEN - 6) { /* 6-byte overhead for PPPoE header */
146	ADD2LOG ("%s: Invalid PPPoE packet length (%u)\n", conn->ifname, len);
147	return 0;
148	}
149
150	/* Step through the tags. */
151	curTag = packet->payload;
152	while (curTag - packet->payload < len) {
153	/* Alignment is not guaranteed, so do this by hand. */
154	tagType = (((uint16_t) curTag[0]) << 8) + (uint16_t) curTag[1];
155	tagLen = (((uint16_t) curTag[2]) << 8) + (uint16_t) curTag[3];
156	if (tagType == TAG_END_OF_LIST)
157	break;
158	if ((curTag - packet->payload) + tagLen + TAG_HDR_SIZE > len) {
159	ADD2LOG ("%s: Invalid PPPoE tag length (%u)\n", conn->ifname,
160	tagLen);
161	return 0;
162	}
163	func (tagType, tagLen, curTag + TAG_HDR_SIZE, extra);
164	curTag = curTag + TAG_HDR_SIZE + tagLen;
165	}
166
167	return 1;
168	}
169
170
171	static int
172	open_interfaces (int n, PPPoEConnection* conns)
173	{
174	int ret = 0, i;
175
176	for (i = 0; i < n; i++)
177	{
178	PPPoEConnection* conn = &conns[i];
179
180	conn->fd = socket (PF_PACKET, SOCK_RAW, htons (ETH_PPPOE_DISCOVERY));
181	if (conn->fd < 0) {
182	ADD2LOG ("%s: socket failed: %m\n", conn->ifname);
183	continue;
184	}
185
186	int one = 1;
187	if (setsockopt (conn->fd, SOL_SOCKET, SO_BROADCAST, &one,
188	sizeof (one)) < 0) {
189	ADD2LOG ("%s: setsockopt failed: %m\n", conn->ifname);
190	goto error;
191	}
192
193	/* Fill in hardware address */
194	struct ifreq ifr;
195	struct sockaddr_ll sa;
196	memset (&sa, 0, sizeof (sa));
197	strncpy (ifr.ifr_name, conn->ifname, sizeof (ifr.ifr_name));
198	if (ioctl (conn->fd, SIOCGIFHWADDR, &ifr) < 0) {
199	ADD2LOG ("%s: ioctl (SIOCGIFHWADDR) failed: %m\n", conn->ifname);
200	goto error;
201	}
202
203	memcpy (conn->my_mac, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
204	if (ifr.ifr_hwaddr.sa_family != ARPHRD_ETHER) {
205	ADD2LOG ("%s: Interface is not ethernet\n", conn->ifname);
206	goto error;
207	}
208
209	if (NOT_UNICAST (conn->my_mac)) {
210	ADD2LOG ("%s: Interface has broadcast/multicast MAC address?\n",
211	conn->ifname);
212	goto error;
213	}
214
215	/* Sanity check on MTU */
216	strncpy (ifr.ifr_name, conn->ifname, sizeof (ifr.ifr_name));
217	if (ioctl (conn->fd, SIOCGIFMTU, &ifr) < 0) {
218	ADD2LOG ("%s: ioctl (SIOCGIFMTU) failed: %m\n", conn->ifname);
219	goto error;
220	}
221	if (ifr.ifr_mtu < ETH_DATA_LEN) {
222	ADD2LOG ("%s: Interface has to low MTU\n", conn->ifname);
223	goto error;
224	}
225
226	/* Get interface index */
227	sa.sll_family = AF_PACKET;
228	sa.sll_protocol = htons (ETH_PPPOE_DISCOVERY);
229	strncpy (ifr.ifr_name, conn->ifname, sizeof (ifr.ifr_name));
230	if (ioctl (conn->fd, SIOCGIFINDEX, &ifr) < 0) {
231	ADD2LOG ("%s: ioctl (SIOCFIGINDEX) failed: Could not get interface "
232	"index\n", conn->ifname);
233	goto error;
234	}
235	sa.sll_ifindex = ifr.ifr_ifindex;
236
237	/* We're only interested in packets on specified interface */
238	if (bind (conn->fd, (struct sockaddr*) &sa, sizeof (sa)) < 0) {
239	ADD2LOG ("%s: bind failed: %m\n", conn->ifname);
240	goto error;
241	}
242
243	ret = 1;
244	continue;
245
246	error:
247
248	close (conn->fd);
249	conn->fd = -1;
250
251	}
252
253	return ret;
254	}
255
256
257	static void
258	close_intefaces (int n, PPPoEConnection* conns)
259	{
260	int i;
261
262	for (i = 0; i < n; i++)
263	{
264	PPPoEConnection* conn = &conns[i];
265
266	if (conn->fd != -1) {
267	close (conn->fd);
268	conn->fd = -1;
269	}
270	}
271	}
272
273
274	static int
275	send_packet (int fd, PPPoEPacket* pkt, size_t size)
276	{
277	if (send (fd, pkt, size, 0) < 0) {
278	ADD2LOG ("send failed: %m\n");
279	return 0;
280	}
281
282	return 1;
283	}
284
285
286	static int
287	receive_packet (int fd, PPPoEPacket* pkt, size_t* size)
288	{
289	int r = recv (fd, pkt, sizeof (PPPoEPacket), 0);
290	if (r < 0) {
291	ADD2LOG ("recv failed: %m\n");
292	return 0;
293	}
294
295	*size = r;
296	return 1;
297	}
298
299
300	static void
301	parse_hostuniq (uint16_t type, uint16_t len, unsigned char* data, void* extra)
302	{
303	if (type == TAG_HOST_UNIQ && len == sizeof (pid_t)) {
304	pid_t tmp;
305	memcpy (&tmp, data, len);
306	if (tmp == getpid ()) {
307	int* val = (int*) extra;
308	*val = 1;
309	}
310	}
311	}
312
313
314	static int
315	packet_for_me (PPPoEConnection* conn, PPPoEPacket* packet)
316	{
317	/* If packet is not directed to our MAC address, forget it. */
318	if (memcmp (packet->ethHdr.h_dest, conn->my_mac, ETH_ALEN))
319	return 0;
320
321	/* Check for HostUniq tag. */
322	int for_me = 0;
323	parse_packet (conn, packet, parse_hostuniq, &for_me);
324	return for_me;
325	}
326
327
328	static void
329	parse_pado_tags (uint16_t type, uint16_t len, unsigned char* data, void* extra)
330	{
331	PacketCriteria* pc = (PacketCriteria*) extra;
332	PPPoEConnection *conn = pc->conn;
333
334	switch (type) {
335	case TAG_AC_NAME:
336	pc->acname_ok = 1;
337	ADD2LOG ("%s: Service-Name is: %.*s\n", conn->ifname, (int) len,
338	data);
339	break;
340	case TAG_SERVICE_NAME:
341	pc->servicename_ok = len == 0;
342	break;
343	case TAG_SERVICE_NAME_ERROR:
344	ADD2LOG ("%s: Service-Name-Error: %.*s\n", conn->ifname, (int) len,
345	data);
346	pc->error = 1;
347	break;
348	case TAG_AC_SYSTEM_ERROR:
349	ADD2LOG ("%s: System-Error: %.*s\n", conn->ifname, (int) len, data);
350	pc->error = 1;
351	break;
352	case TAG_GENERIC_ERROR:
353	ADD2LOG ("%s: Generic-Error: %.*s\n", conn->ifname, (int) len, data);
354	pc->error = 1;
355	break;
356	}
357	}
358
359
360	static int
361	send_padi (int n, PPPoEConnection* conns)
362	{
363	int ret = 0, i;
364
365	for (i = 0; i < n; i++)
366	{
367	PPPoEConnection* conn = &conns[i];
368
369	if (conn->fd == -1 \|\| conn->received_pado)
370	continue;
371
372	PPPoEPacket packet;
373	unsigned char* cursor = packet.payload;
374	PPPoETag* svc = (PPPoETag*) (&packet.payload);
375	uint16_t namelen = 0;
376	uint16_t plen;
377
378	namelen = 0;
379	plen = TAG_HDR_SIZE + namelen;
380	if (!check_room (conn, cursor, packet.payload, TAG_HDR_SIZE))
381	continue;
382
383	/* Set destination to Ethernet broadcast address */
384	memset (packet.ethHdr.h_dest, 0xFF, ETH_ALEN);
385	memcpy (packet.ethHdr.h_source, conn->my_mac, ETH_ALEN);
386
387	packet.ethHdr.h_proto = htons (ETH_PPPOE_DISCOVERY);
388	packet.ver = 1;
389	packet.type = 1;
390	packet.code = CODE_PADI;
391	packet.session = 0;
392
393	svc->type = TAG_SERVICE_NAME;
394	svc->length = htons (0);
395	if (!check_room (conn, cursor, packet.payload, namelen + TAG_HDR_SIZE))
396	continue;
397
398	cursor += namelen + TAG_HDR_SIZE;
399
400	PPPoETag hostUniq;
401	pid_t pid = getpid ();
402	hostUniq.type = htons (TAG_HOST_UNIQ);
403	hostUniq.length = htons (sizeof (pid));
404	memcpy (hostUniq.payload, &pid, sizeof (pid));
405	if (!check_room (conn, cursor, packet.payload, sizeof (pid) + TAG_HDR_SIZE))
406	continue;
407	memcpy (cursor, &hostUniq, sizeof (pid) + TAG_HDR_SIZE);
408	cursor += sizeof (pid) + TAG_HDR_SIZE;
409	plen += sizeof (pid) + TAG_HDR_SIZE;
410
411	packet.length = htons (plen);
412
413	ADD2LOG ("%s: Sending PADI packet\n", conn->ifname);
414
415	if (send_packet (conn->fd, &packet, (int) (plen + HDR_SIZE)))
416	ret = 1;
417	}
418
419	return ret;
420	}
421
422
423	static int
424	wait_for_pado (int n, PPPoEConnection* conns)
425	{
426	int r, i, all;
427	size_t len;
428	fd_set readable;
429	PPPoEPacket packet;
430	PacketCriteria pc;
431
432	struct timeval tv;
433	tv.tv_sec = PADO_TIMEOUT;
434	tv.tv_usec = 0;
435
436	while (1)
437	{
438	FD_ZERO (&readable);
439	for (i = 0; i < n; i++)
440	if (conns[i].fd != -1)
441	FD_SET (conns[i].fd, &readable);
442
443	do {
444	r = select (FD_SETSIZE, &readable, NULL, NULL, &tv);
445	} while (r == -1 && errno == EINTR);
446
447	if (r < 0) {
448	ADD2LOG ("select: %m\n");
449	return 0;
450	}
451
452	if (r == 0) {
453	ADD2LOG ("Timeout waiting for PADO packets\n");
454	return 0;
455	}
456
457	for (i = 0; i < n; i++)
458	{
459	PPPoEConnection* conn = &conns[i];
460
461	if (conn->fd == -1 \|\| !FD_ISSET (conn->fd, &readable))
462	continue;
463
464	pc.conn = conn;
465	pc.acname_ok = 0;
466	pc.servicename_ok = 0;
467	pc.error = 0;
468
469	/* Get the packet */
470	if (!receive_packet (conn->fd, &packet, &len))
471	continue;
472
473	/* Check length */
474	if (ntohs (packet.length) + HDR_SIZE > len) {
475	ADD2LOG ("%s: Bogus PPPoE length field (%u)\n", conn->ifname,
476	(unsigned int) ntohs (packet.length));
477	continue;
478	}
479
480	/* If it's not for us, loop again */
481	if (!packet_for_me (conn, &packet))
482	continue;
483
484	if (packet.code != CODE_PADO)
485	continue;
486
487	if (NOT_UNICAST (packet.ethHdr.h_source)) {
488	ADD2LOG ("%s: Ignoring PADO packet from non-unicast MAC "
489	"address\n", conn->ifname);
490	continue;
491	}
492
493	parse_packet (conn, &packet, parse_pado_tags, &pc);
494
495	if (!pc.acname_ok) {
496	ADD2LOG ("%s: Wrong or missing AC-Name tag\n", conn->ifname);
497	continue;
498	}
499
500	if (!pc.servicename_ok) {
501	ADD2LOG ("%s: Wrong or missing Service-Name tag\n",
502	conn->ifname);
503	continue;
504	}
505
506	if (pc.error) {
507	ADD2LOG ("%s: Ignoring PADO packet with some Error tag\n",
508	conn->ifname);
509	continue;
510	}
511
512	memcpy (conn->peer_mac, packet.ethHdr.h_source, ETH_ALEN);
513	ADD2LOG ("%s: Received correct PADO packet\n", conn->ifname);
514	conn->received_pado = 1;
515	}
516
517	all = 1;
518	for (i = 0; i < n; i++)
519	if (conns[i].fd != -1 && !conns[i].received_pado)
520	all = 0;
521	if (all)
522	return 1;
523	}
524	}
525
526
527	static void
528	discovery (int n, PPPoEConnection* conns)
529	{
530	int a;
531
532	if (open_interfaces (n, conns))
533	{
534	for (a = 0; a < MAX_ATTEMPTS; a++)
535	{
536	ADD2LOG ("Attempt number %d\n", a + 1);
537
538	if (!send_padi (n, conns))
539	break;
540
541	if (wait_for_pado (n, conns))
542	break;
543	}
544	}
545
546	close_intefaces (n, conns);
547	}
548
549
550	void hd_scan_pppoe(hd_data_t *hd_data2)
551	{
552	hd_t *hd;
553	int cnt, interfaces;
554	PPPoEConnection *conn;
555
556	hd_data = hd_data2;
557
558	if(!hd_probe_feature(hd_data, pr_pppoe)) return;
559
560	hd_data->module = mod_pppoe;
561
562	PROGRESS(1, 0, "looking for pppoe");
563
564	for(interfaces = 0, hd = hd_data->hd; hd; hd = hd->next) {
565	if(
566	hd->base_class.id == bc_network_interface &&
567	hd->sub_class.id == sc_nif_ethernet &&
568	hd->unix_dev_name
569	) {
570	interfaces++;
571	}
572	}
573
574	if(!interfaces) return;
575
576	conn = new_mem(interfaces * sizeof *conn);
577
578	for(cnt = 0, hd = hd_data->hd; hd && cnt < interfaces; hd = hd->next) {
579	if(
580	hd->base_class.id == bc_network_interface &&
581	hd->sub_class.id == sc_nif_ethernet &&
582	hd->unix_dev_name
583	) {
584	conn[cnt].hd = hd;
585	conn[cnt].fd = -1;
586	conn[cnt].ifname = hd->unix_dev_name;
587	cnt++;
588	}
589	}
590
591	PROGRESS(2, 0, "discovery");
592
593	discovery(interfaces, conn);
594
595	for(cnt = 0; cnt < interfaces; cnt++) {
596	conn[cnt].hd->is.pppoe = 0;
597
598	if(conn[cnt].received_pado) {
599	conn[cnt].hd->is.pppoe = 1;
600	ADD2LOG(
601	"pppoe %s: my mac %02x:%02x:%02x:%02x:%02x:%02x, "
602	"peer mac %02x:%02x:%02x:%02x:%02x:%02x\n",
603	conn[cnt].ifname,
604	conn[cnt].my_mac[0], conn[cnt].my_mac[1], conn[cnt].my_mac[2],
605	conn[cnt].my_mac[3], conn[cnt].my_mac[4], conn[cnt].my_mac[5],
606	conn[cnt].peer_mac[0], conn[cnt].peer_mac[1], conn[cnt].peer_mac[2],
607	conn[cnt].peer_mac[3], conn[cnt].peer_mac[4], conn[cnt].peer_mac[5]
608	);
609	}
610	}
611	}