File elilo-ipv6.diff of Package elilo

---
 fs/netfs.c   |  257 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++---
 glue_netfs.c |   53 ++++++++++--
 strops.c     |   11 ++
 strops.h     |    1 
 4 files changed, 302 insertions(+), 20 deletions(-)

--- a/glue_netfs.c
+++ b/glue_netfs.c
@@ -133,6 +133,17 @@ netfs_set_default_path(netfs_interface_t
  	return EFI_SUCCESS;
 }
 
+static INTN
+non_zero(UINT8 *c, UINTN len)
+{
+	int i;
+	for (i=0 ; i < len ; i++) {
+		if (c[i])
+			return 1;
+	}
+	return 0;
+}
+
 static EFI_STATUS
 netfs_setdefaults(VOID *intf, config_file_t *config, CHAR16 *kname, UINTN maxlen, CHAR16 *devpath)
 {
@@ -140,7 +151,7 @@ netfs_setdefaults(VOID *intf, config_fil
 	netfs_info_t info;
 	EFI_STATUS status;
 	UINT8 *ipaddr;
-	UINTN m;
+	UINTN m, i;
 	CHAR16 ip_var[64], str[64];
 	UINT8 *ip;
 
@@ -165,6 +176,9 @@ netfs_setdefaults(VOID *intf, config_fil
 	set_var(VAR_NETFS_HOSTNAME, info.hostname);
 	set_var(VAR_NETFS_DOMAINAME, info.domainame);
 
+	DBG_PRT((L"netfs_setdefaults: hostname=%s (IPv%a)", info.hostname,
+		(info.using_ipv6 ? "6" : "4")));
+
 	if (info.using_pxe) {
 		DBG_PRT((L"netfs_setdefaults: using_pxe"));
 
@@ -184,13 +198,10 @@ netfs_setdefaults(VOID *intf, config_fil
 
 #  if defined(CONFIG_ia64)
 #    define CONFIG_ARCH_EXTENSION L"-ia64.conf\0"
-#    define EXTENSION_LENGTH 11
 #  elif defined (CONFIG_ia32)
 #    define CONFIG_ARCH_EXTENSION L"-ia32.conf\0"
-#    define EXTENSION_LENGTH 11
 #  elif defined (CONFIG_x86_64)
 #    define CONFIG_ARCH_EXTENSION L"-x86_64.conf\0"
-#    define EXTENSION_LENGTH 13
 #  else
 #    error "You need to specfy your default arch config file"
 #  endif            
@@ -203,31 +214,57 @@ netfs_setdefaults(VOID *intf, config_fil
 		 * the filenames are constructed based on the IP(v4) address
 		 */
 		convert_ip2hex(ipaddr, m, str);
+#if 1
+		i = 0;
+		if (non_zero(ipaddr, m)) {
+			int nr = (m==16)? 10 : 6;
+			int st = (m==16)? 2 : 1;
+			m <<= 1;
+			StrnCpy(config[i].fname, str, m);
+			StrnCat(config[i++].fname,
+				CONFIG_EXTENSION, maxlen - m);
+			while (i <= nr) {
+				int stub = m - (i+1)*st;
+				StrnCpy(config[i].fname, str, stub);
+				StrnCat(config[i++].fname,
+					CONFIG_ARCH_EXTENSION, maxlen - m);
+				StrnCpy(config[i].fname, str, stub);
+				StrnCat(config[i++].fname,
+					CONFIG_EXTENSION, maxlen - m);
+			}
+		}
+		if (non_zero(info.hw_addr, 6)) {
+			convert_mac2hex(info.hw_addr,6,str);
+			StrnCpy(config[i].fname, str, maxlen-1);
+			StrnCpy(config[i++].fname+17, CONFIG_EXTENSION, 6);
+		}
+#else
 		StrnCpy(config[0].fname, str, maxlen-1);
 		StrnCpy(config[0].fname+8, CONFIG_EXTENSION, 6);
 
 		StrnCpy(config[1].fname, str, maxlen-1);
-		StrnCpy(config[1].fname+6, CONFIG_ARCH_EXTENSION, EXTENSION_LENGTH);
+		StrnCpy(config[1].fname+6, CONFIG_ARCH_EXTENSION, maxlen-7);
 
 		StrnCpy(config[2].fname, str, maxlen-1);
 		StrnCpy(config[2].fname+6, CONFIG_EXTENSION, 6);
 
 		StrnCpy(config[3].fname, str, maxlen-1);
-		StrnCpy(config[3].fname+4, CONFIG_ARCH_EXTENSION, EXTENSION_LENGTH);
+		StrnCpy(config[3].fname+4, CONFIG_ARCH_EXTENSION, maxlen-5);
 
 		StrnCpy(config[4].fname, str, maxlen-1);
 		StrnCpy(config[4].fname+4, CONFIG_EXTENSION, 6);
                 
 		StrnCpy(config[5].fname, str, maxlen-1);
-		StrnCpy(config[5].fname+2, CONFIG_ARCH_EXTENSION, EXTENSION_LENGTH);
+		StrnCpy(config[5].fname+2, CONFIG_ARCH_EXTENSION, maxlen-3);
 
 		StrnCpy(config[6].fname, str, maxlen-1);
-		StrnCpy(config[6].fname+2, CONFIG_EXTENSION, 6);
+		StrnCpy(config[6].fname+2, CONFIG_EXTENSION, maxlen-3);
 
 		/* use the MAC address as a possible file name as well */
 		convert_mac2hex(info.hw_addr,6,str);
 		StrnCpy(config[7].fname, str, maxlen-1);
 		StrnCpy(config[7].fname+17, CONFIG_EXTENSION, 6);
+#endif
 
 #else
 		StrnCpy(config[0].fname, NETFS_DEFAULT_CONFIG, maxlen-1);
--- a/strops.c
+++ b/strops.c
@@ -50,6 +50,17 @@ StrnCpy(OUT CHAR16 *dst, IN const CHAR16
 	return res;
 }
 
+CHAR16 *
+StrnCat(OUT CHAR16 *dst, IN const CHAR16 *src, IN UINTN size)
+{
+	CHAR16 *res;
+
+	while (size && size-- && (*dst++) != CHAR_NULL);
+	res = StrnCpy(--dst, src, size);
+
+	return res;
+}
+
 CHAR8 *
 StrnXCpy(OUT CHAR8 *dst, IN const CHAR16 *src, IN UINTN size)
 {
--- a/strops.h
+++ b/strops.h
@@ -28,6 +28,7 @@
 
 extern CHAR16 *StrChr(IN const CHAR16 *s, const CHAR16 c);
 extern CHAR16 *StrnCpy(OUT CHAR16 *dst, IN const CHAR16 *src, UINTN count);
+extern CHAR16 *StrnCat(OUT CHAR16 *dst, IN const CHAR16 *src, UINTN count);
 extern CHAR8 *StrnXCpy(OUT CHAR8 *dst, IN const CHAR16 *src, UINTN count);
 
 extern CHAR8 *strtok_simple(CHAR8 *in, CHAR8 c);
--- a/fs/netfs.c
+++ b/fs/netfs.c
@@ -75,6 +75,8 @@ typedef struct {
 	EFI_IP_ADDRESS	netmask;
 	UINT8		hw_addr[16];
 
+	CHAR8		bootfile[NETFS_BOOTFILE_MAXLEN];
+
 	netfs_fd_t	fd_tab[NETFS_FD_MAX];
 	netfs_fd_t	*free_fd;
 	UINTN		free_fd_count;
@@ -206,11 +208,239 @@ netfs_name(netfs_interface_t *this, CHAR
 	return EFI_SUCCESS;
 }
 
+typedef struct {
+    UINT16  code;
+    UINT16  len;
+    UINT8   data[];
+} DHCPv6_OPTION;
+
+#define DHCPv6_REPLY		 7
+#define OPT_BOOTFILE_URL	59
+#define PROTO "tftp://"  /* only TFTP supported for now */
+
+static inline UINT16
+bswap16(UINT16 x)
+{
+	return ((((x) >> 8) & 0xff) | (((x) & 0xff) << 8));
+}
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+# define ntohs(x)     bswap16(x)
+# define htons(x)     bswap16(x)
+#else
+# if __BYTE_ORDER == __BIG_ENDIAN
+#  define ntohs(x)       (x)
+#  define htons(x)       (x)
+# endif
+#endif
+/* string 2 net short */
+static UINT16
+stra2ns(CHAR8 *str)
+{
+	UINT16 ret = 0;
+	UINT8 v;
+	for(;*str;str++) {
+		if ('0' <= *str && *str <= '9')
+			v = *str - '0';
+		else if ('A' <= *str && *str <= 'F')
+			v = *str - 'A' + 10;
+		else if ('a' <= *str && *str <= 'f')
+			v = *str - 'a' + 10;
+		else
+			v = 0;
+		ret = (ret << 4) + v;
+	}
+	/* DBG_PRT((L"stra2ns: '%a'='%d'", str, ret)); */
+	return htons(ret);
+}
+
+/* IPv4/6 address to string */
+static CHAR8 *
+ip2stra(UINT8 octets, UINT8 *ip)
+{
+	int i, j;
+	static CHAR8 *hexa= (CHAR8 *)"0123456789ABCDEF";
+	static CHAR8 str[42];
+
+	if (octets == 16) {
+		for(i=j=0; i < 16; i++) {
+			str[j++] = hexa[(ip[i] & 0xf0)>>4];
+			str[j++] = hexa[ip[i] & 0x0f];
+			if (i && i%2 == 0)
+				str[j++] = ':';
+		}
+	} else if (octets == 4) {
+		int val, v;
+		for(i=j=0; i < 4; i++) {
+			val = ip[i];
+			v = val / 100;
+			if (v)
+				str[j++] = '0' + v;
+			v = (val % 100) / 10;
+			if (v || val > 99)
+				str[j++] = '0' + v;
+			val %= 10;
+			str[j++] = '0' + val;
+			str[j++] = '.';
+		}
+	} else
+		j = 1;
+	str[--j] = '\0';
+	/* DBG_PRT((L"ip2stra: '%a'", str)); */
+	return str;
+}
+
+/* string 2 IPv6 address */
+static UINT8 *
+stra2ip6(CHAR8 *str)
+{
+	UINTN i, j, p, len;
+	CHAR8 *a, *b, t;
+	static UINT16 ip[8];
+
+	for(i=0; i < 8; i++) {
+		ip[i] = 0;
+	}
+	len = strlena( str);
+	a = b = str;
+	for(i=p=0; i < len; i++, b++) {
+		if (*b != ':')
+			continue;
+		*b = '\0';
+		ip[p++] = stra2ns(a);
+		*b = ':';
+		a = b + 1;
+		if ( *(b+1) == ':' )
+			break;
+	}
+	a = b = str + len;
+	for(j=len, p=7; j > i; j--, a--) {
+		if (*a != ':')
+			continue;
+		t = *b;
+		*b = '\0';
+		ip[p--] = stra2ns(a+1);
+		*b = t;
+		b = a;
+	}
+	DBG_PRT((L"stra2ip6: '%a'='%a'", str, ip2stra(16, (UINT8 *)ip)));
+	return (UINT8 *)ip;
+}
+
+static VOID
+hexdump(UINT8 *dp, int size)
+{
+	int i = 0, j;
+	unsigned char *d = (unsigned char *)dp;
+	char hex[64], chr[17];
+	static CHAR8 *hexa= (CHAR8 *)"0123456789ABCDEF";
+
+	hex[58] = '\0';
+	chr[16] = '\0';
+	while ( i < size ) {
+		 if ( i > 0 && i % 16 == 0 )
+			 Print(L"%a >%a<\n", hex, chr);
+		 if ( i % 16 == 0 ) {
+			 Memset( hex, ' ', 58);
+			 hex[56] = '\0';
+			 if (i>0xFFFFFF) return;
+			 for (j = 0; j < 6; j++)
+			 	hex[j] = hexa[(i>>(4*(5-j))) & 0xF];
+		 }
+#define pos(i) ((((i%16)<8)?8:9)+(i%16)*3)
+		 hex[pos(i)] = hexa[d[i]>>4&0xF];
+		 hex[pos(i)+1] = hexa[d[i]&0xF];
+
+		 chr[i%16] = ((d[i]>31&&d[i]<127)?d[i]:'.');
+		 i++;
+	}
+	if ( i % 16 != 1 ) {
+		 chr[(i%16)] = '\0';
+		 Print(L"%a >%a<\n", hex, chr);
+	}
+}
+
+static INTN
+find_dhcp6_option(
+	EFI_PXE_BASE_CODE_PACKET *packet,
+	UINT8 option,
+	CHAR8 *str,
+	INTN *length)
+{
+	EFI_PXE_BASE_CODE_DHCPV6_PACKET *v6 =
+		(EFI_PXE_BASE_CODE_DHCPV6_PACKET *)packet->Raw;
+	int code, len;
+	DHCPv6_OPTION *p;
+
+	/* Consistency check */
+	if (v6->MessageType != DHCPv6_REPLY) {
+		 VERB_PRT(2, Print(L"DHCPv6: MessageType: %d != %d\n",
+				   v6->MessageType, DHCPv6_REPLY));
+		 VERB_PRT(3, hexdump( (UINT8 *)v6, 1024));
+		 return -1;
+	}
+	p = (DHCPv6_OPTION *)&v6->DhcpOptions;
+	while (0 != (code = ntohs(p->code))) {
+		len = ntohs(p->len);
+
+		VERB_PRT(4, { Print(L"DHCPv6: REPLY: Code=%d Len=%d\n",
+				    code, len);
+			      hexdump( p->data, (len<1020)?len:144);
+			    });
+		if ( code == option ) {
+			len = (len > *length - 1) ? *length - 1 : len;
+			Memcpy(str, p->data, len);
+			str[len] = '\0';
+			*length = len;
+		}
+		p = (DHCPv6_OPTION *)((UINT8 *)p + len + 2*sizeof(UINT16));
+	}
+	return 0;
+}
+static  INTN
+parse_bootfile_url(netfs_priv_state_t *nfs, CHAR8 *url) {
+	CHAR8 *end;
+	VERB_PRT(3, Print(L"DHCPv6: bootfile-url: '%a'\n", url));
+
+	/* check protocol */
+	if (strncmpa(url, (CHAR8 *)PROTO, sizeof(PROTO)-1)) {
+		ERR_PRT((L"Warning: bootfile-url must use TFTP for now! (%a)\n",
+			 PROTO));
+		return -1;
+	}
+	/* fill in v6 address */
+	end = url+sizeof(PROTO);
+	while (*end != ']' && *end != '\0')
+	++end;
+	if (url[sizeof(PROTO)-1] != '[' || *end != ']') {
+		ERR_PRT((L"Warning: bootfile-url must use '[IPv6::addr]'!\n"));
+		return -1;
+	}
+	*end = '\0';
+	Memcpy( nfs->srv_ip.v6.Addr, stra2ip6(url+sizeof(PROTO)), 16);
+	*end = ']';
+
+	/* remember bootfile path */
+	strncpya( nfs->bootfile, end + 1, NETFS_BOOTFILE_MAXLEN);
+	return 0;
+}
+
 static  VOID
 netfs_extract_ip(netfs_priv_state_t *nfs)
 {
 	EFI_PXE_BASE_CODE *pxe = nfs->pxe;
 
+	Memcpy(&nfs->cln_ip, &pxe->Mode->StationIp, sizeof(EFI_IP_ADDRESS));
+
+	if (pxe->Mode->UsingIpv6) {
+		CHAR8 str[NETFS_BOOTFILE_MAXLEN];
+		INTN len = NETFS_BOOTFILE_MAXLEN;
+		if (find_dhcp6_option(&nfs->pxe->Mode->DhcpAck,
+					OPT_BOOTFILE_URL, str, &len) != 0)
+			return;
+		parse_bootfile_url(nfs, str);
+		return;
+	}
+
 	if (pxe->Mode->PxeDiscoverValid) {
 		nfs->using_pxe = TRUE;
 		Memcpy(&nfs->srv_ip, pxe->Mode->PxeReply.Dhcpv4.BootpSiAddr, sizeof(EFI_IP_ADDRESS)); 
@@ -218,9 +453,9 @@ netfs_extract_ip(netfs_priv_state_t *nfs
 	} else {
 		Memcpy(&nfs->srv_ip, pxe->Mode->DhcpAck.Dhcpv4.BootpSiAddr, sizeof(EFI_IP_ADDRESS)); 
 		Memcpy(&nfs->hw_addr, pxe->Mode->DhcpAck.Dhcpv4.BootpHwAddr, sizeof(nfs->hw_addr));
+		strncpya(nfs->bootfile, pxe->Mode->DhcpAck.Dhcpv4.BootpBootFile, NETFS_BOOTFILE_MAXLEN);
 	}
 
-	Memcpy(&nfs->cln_ip, &pxe->Mode->StationIp, sizeof(EFI_IP_ADDRESS)); 
 	Memcpy(&nfs->netmask, &pxe->Mode->SubnetMask, sizeof(EFI_IP_ADDRESS)); 
 
 	/*
@@ -326,11 +561,9 @@ netfs_open(netfs_interface_t *this, CHAR
 
 	U2ascii(name, ascii_name, FILENAME_MAXLEN);
 
-	VERB_PRT(2, Print(L"downloading %a from %d.%d.%d.%d...\n", ascii_name, 
-				nfs->srv_ip.v4.Addr[0], 
-				nfs->srv_ip.v4.Addr[1], 
-				nfs->srv_ip.v4.Addr[2], 
-				nfs->srv_ip.v4.Addr[3]));
+	VERB_PRT(2, Print(L"downloading %a from %a...\n", ascii_name,
+			ip2stra((nfs->pxe->Mode->UsingIpv6 ? 16 : 4),
+				(UINT8 *)nfs->srv_ip.Addr)));
 retry:
 	if (retries == 2) {
 		netfs_fd_free(nfs, f);
@@ -571,6 +804,9 @@ find_dhcp_option(EFI_PXE_BASE_CODE_PACKE
 	UINT8 tag, length;
 	UINT8 *opts = packet->Dhcpv4.DhcpOptions;
 
+	if (use_ipv6)
+		return find_dhcp6_option(packet, option, str, len);
+
 	*len = 0;
 
 	for(;;) {
@@ -637,15 +873,12 @@ netfs_getinfo(netfs_interface_t *this, n
 
 	VERB_PRT(3, Print(L"hostname(12): %a\n", str));
 
-	/*
-	 * extract bootfile name from DHCP exchanges
-	 */
-	if (nfs->using_pxe == 0) {
- 		ascii2U(nfs->pxe->Mode->DhcpAck.Dhcpv4.BootpBootFile, info->bootfile, NETFS_BOOTFILE_MAXLEN);
+skip_options:
+	if (!info->using_pxe && nfs->bootfile) {
+		ascii2U(nfs->bootfile, info->bootfile, NETFS_BOOTFILE_MAXLEN);
 		VERB_PRT(3, Print(L"bootfile: %s\n", info->bootfile));
 	}
 
-skip_options:
 	return EFI_SUCCESS;
 }