Merge branch 'master' of git://git.denx.de/u-boot-net

6 files changed, 293 insertions, 174 deletions

diff --git a/common/miiphyutil.c b/common/miiphyutil.c
index 08aa854..d8ebb38 100644
--- a/common/miiphyutil.c
+++ b/common/miiphyutil.c
@@ -380,7 +380,7 @@ int miiphy_reset(const char *devname, unsigned char addr)
  */
 int miiphy_speed(const char *devname, unsigned char addr)
 {
-	u16 bmcr, anlpar;
+	u16 bmcr, anlpar, adv;
 
 #if defined(CONFIG_PHY_GIGE)
 	u16 btsr;
@@ -417,7 +417,12 @@ int miiphy_speed(const char *devname, unsigned char addr)
 			printf("PHY AN speed");
 			goto miiphy_read_failed;
 		}
-		return (anlpar & LPA_100) ? _100BASET : _10BASET;
+
+		if (miiphy_read(devname, addr, MII_ADVERTISE, &adv)) {
+			puts("PHY AN adv speed");
+			goto miiphy_read_failed;
+		}
+		return ((anlpar & adv) & LPA_100) ? _100BASET : _10BASET;
 	}
 	/* Get speed from basic control settings. */
 	return (bmcr & BMCR_SPEED100) ? _100BASET : _10BASET;
@@ -433,7 +438,7 @@ miiphy_read_failed:
  */
 int miiphy_duplex(const char *devname, unsigned char addr)
 {
-	u16 bmcr, anlpar;
+	u16 bmcr, anlpar, adv;
 
 #if defined(CONFIG_PHY_GIGE)
 	u16 btsr;
@@ -475,7 +480,12 @@ int miiphy_duplex(const char *devname, unsigned char addr)
 			puts("PHY AN duplex");
 			goto miiphy_read_failed;
 		}
-		return (anlpar & (LPA_10FULL | LPA_100FULL)) ?
+
+		if (miiphy_read(devname, addr, MII_ADVERTISE, &adv)) {
+			puts("PHY AN adv duplex");
+			goto miiphy_read_failed;
+		}
+		return ((anlpar & adv) & (LPA_10FULL | LPA_100FULL)) ?
 		    FULL : HALF;
 	}
 	/* Get speed from basic control settings. */
diff --git a/drivers/net/davinci_emac.c b/drivers/net/davinci_emac.c
index ca457b8..5e7ebc8 100644
--- a/drivers/net/davinci_emac.c
+++ b/drivers/net/davinci_emac.c
@@ -108,26 +108,6 @@ static u_int8_t	num_phy;
 
 phy_t				phy[CONFIG_SYS_DAVINCI_EMAC_PHY_COUNT];
 
-static inline void davinci_flush_rx_descs(void)
-{
-	/* flush the whole RX descs area */
-	flush_dcache_range(EMAC_WRAPPER_RAM_ADDR + EMAC_RX_DESC_BASE,
-			EMAC_WRAPPER_RAM_ADDR + EMAC_TX_DESC_BASE);
-}
-
-static inline void davinci_invalidate_rx_descs(void)
-{
-	/* invalidate the whole RX descs area */
-	invalidate_dcache_range(EMAC_WRAPPER_RAM_ADDR + EMAC_RX_DESC_BASE,
-			EMAC_WRAPPER_RAM_ADDR + EMAC_TX_DESC_BASE);
-}
-
-static inline void davinci_flush_desc(emac_desc *desc)
-{
-	flush_dcache_range((unsigned long)desc,
-			(unsigned long)desc + sizeof(*desc));
-}
-
 static int davinci_eth_set_mac_addr(struct eth_device *dev)
 {
 	unsigned long		mac_hi;
@@ -243,10 +223,10 @@ int davinci_eth_phy_read(u_int8_t phy_addr, u_int8_t reg_num, u_int16_t *data)
 
 	if (tmp & MDIO_USERACCESS0_ACK) {
 		*data = tmp & 0xffff;
-		return 0;
+		return 1;
 	}
 
-	return -EIO;
+	return 0;
 }
 
 /* Write to a PHY register via MDIO inteface. Blocks until operation is complete. */
@@ -267,7 +247,7 @@ int davinci_eth_phy_write(u_int8_t phy_addr, u_int8_t reg_num, u_int16_t data)
 	while (readl(&adap_mdio->USERACCESS0) & MDIO_USERACCESS0_GO)
 		;
 
-	return 0;
+	return 1;
 }
 
 /* PHY functions for a generic PHY */
@@ -394,15 +374,14 @@ static int davinci_mii_phy_read(struct mii_dev *bus, int addr, int devad,
 {
 	unsigned short value = 0;
 	int retval = davinci_eth_phy_read(addr, reg, &value);
-	if (retval < 0)
-		return retval;
-	return value;
+
+	return retval ? value : -EIO;
 }
 
 static int davinci_mii_phy_write(struct mii_dev *bus, int addr, int devad,
 				 int reg, u16 value)
 {
-	return davinci_eth_phy_write(addr, reg, value);
+	return davinci_eth_phy_write(addr, reg, value) ? 0 : 1;
 }
 #endif
 
@@ -496,8 +475,6 @@ static int davinci_eth_open(struct eth_device *dev, bd_t *bis)
 	emac_rx_active_tail = rx_desc;
 	emac_rx_queue_active = 1;
 
-	davinci_flush_rx_descs();
-
 	/* Enable TX/RX */
 	writel(EMAC_MAX_ETHERNET_PKT_SIZE, &adap_emac->RXMAXLEN);
 	writel(0, &adap_emac->RXBUFFEROFFSET);
@@ -659,8 +636,7 @@ static int davinci_eth_send_packet (struct eth_device *dev,
 				      EMAC_CPPI_EOP_BIT);
 
 	flush_dcache_range((unsigned long)packet,
-			(unsigned long)packet + length);
-	davinci_flush_desc(emac_tx_desc);
+			   (unsigned long)packet + ALIGN(length, PKTALIGN));
 
 	/* Send the packet */
 	writel(BD_TO_HW((unsigned long)emac_tx_desc), &adap_emac->TX0HDP);
@@ -694,8 +670,6 @@ static int davinci_eth_rcv_packet (struct eth_device *dev)
 	volatile emac_desc *tail_desc;
 	int status, ret = -1;
 
-	davinci_invalidate_rx_descs();
-
 	rx_curr_desc = emac_rx_active_head;
 	if (!rx_curr_desc)
 		return 0;
@@ -706,12 +680,12 @@ static int davinci_eth_rcv_packet (struct eth_device *dev)
 			printf ("WARN: emac_rcv_pkt: Error in packet\n");
 		} else {
 			unsigned long tmp = (unsigned long)rx_curr_desc->buffer;
+			unsigned short len =
+				rx_curr_desc->buff_off_len & 0xffff;
 
-			invalidate_dcache_range(tmp, tmp + EMAC_RXBUF_SIZE);
-			net_process_received_packet(
-				rx_curr_desc->buffer,
-				rx_curr_desc->buff_off_len & 0xffff);
-			ret = rx_curr_desc->buff_off_len & 0xffff;
+			invalidate_dcache_range(tmp, tmp + ALIGN(len, PKTALIGN));
+			net_process_received_packet(rx_curr_desc->buffer, len);
+			ret = len;
 		}
 
 		/* Ack received packet descriptor */
@@ -734,7 +708,6 @@ static int davinci_eth_rcv_packet (struct eth_device *dev)
 		rx_curr_desc->buff_off_len = EMAC_MAX_ETHERNET_PKT_SIZE;
 		rx_curr_desc->pkt_flag_len = EMAC_CPPI_OWNERSHIP_BIT;
 		rx_curr_desc->next = 0;
-		davinci_flush_desc(rx_curr_desc);
 
 		if (emac_rx_active_head == 0) {
 			printf ("INFO: emac_rcv_pkt: active queue head = 0\n");
@@ -752,13 +725,11 @@ static int davinci_eth_rcv_packet (struct eth_device *dev)
 			tail_desc->next = BD_TO_HW((ulong) curr_desc);
 			status = tail_desc->pkt_flag_len;
 			if (status & EMAC_CPPI_EOQ_BIT) {
-				davinci_flush_desc(tail_desc);
 				writel(BD_TO_HW((ulong)curr_desc),
 				       &adap_emac->RX0HDP);
 				status &= ~EMAC_CPPI_EOQ_BIT;
 				tail_desc->pkt_flag_len = status;
 			}
-			davinci_flush_desc(tail_desc);
 		}
 		return (ret);
 	}
diff --git a/drivers/net/fm/fm.c b/drivers/net/fm/fm.c
index 00cdfd4..5eb773e 100644
--- a/drivers/net/fm/fm.c
+++ b/drivers/net/fm/fm.c
@@ -336,9 +336,6 @@ static int fm_init_bmi(int fm_idx, struct fm_bmi_common *bmi)
 
 static void fm_init_qmi(struct fm_qmi_common *qmi)
 {
-	/* disable enqueue and dequeue of QMI */
-	clrbits_be32(&qmi->fmqm_gc, FMQM_GC_ENQ_EN | FMQM_GC_DEQ_EN);
-
 	/* disable all error interrupts */
 	out_be32(&qmi->fmqm_eien, FMQM_EIEN_DISABLE_ALL);
 	/* clear all error events */
diff --git a/net/net.c b/net/net.c
index 1e1d23d..671d45d 100644
--- a/net/net.c
+++ b/net/net.c
@@ -834,15 +834,7 @@ int net_send_udp_packet(uchar *ether, struct in_addr dest, int dport, int sport,
 #ifndef CONFIG_NET_MAXDEFRAG
 #define CONFIG_NET_MAXDEFRAG 16384
 #endif
-/*
- * MAXDEFRAG, above, is chosen in the config file and  is real data
- * so we need to add the NFS overhead, which is more than TFTP.
- * To use sizeof in the internal unnamed structures, we need a real
- * instance (can't do "sizeof(struct rpc_t.u.reply))", unfortunately).
- * The compiler doesn't complain nor allocates the actual structure
- */
-static struct rpc_t rpc_specimen;
-#define IP_PKTSIZE (CONFIG_NET_MAXDEFRAG + sizeof(rpc_specimen.u.reply))
+#define IP_PKTSIZE (CONFIG_NET_MAXDEFRAG)
 
 #define IP_MAXUDP (IP_PKTSIZE - IP_HDR_SIZE)
 
diff --git a/net/nfs.c b/net/nfs.c
index 4a5a1ab..814751b 100644
--- a/net/nfs.c
+++ b/net/nfs.c
@@ -22,6 +22,10 @@
  * possible, maximum 16 steps). There is no clearing of ".."'s inside the
  * path, so please DON'T DO THAT. thx. */
 
+/* NOTE 4: NFSv3 support added by Guillaume GARDET, 2016-June-20.
+ * NFSv2 is still used by default. But if server does not support NFSv2, then
+ * NFSv3 is used, if available on NFS server. */
+
 #include <common.h>
 #include <command.h>
 #include <net.h>
@@ -47,8 +51,9 @@ static int nfs_offset = -1;
 static int nfs_len;
 static ulong nfs_timeout = NFS_TIMEOUT;
 
-static char dirfh[NFS_FHSIZE];	/* file handle of directory */
-static char filefh[NFS_FHSIZE]; /* file handle of kernel image */
+static char dirfh[NFS_FHSIZE];	/* NFSv2 / NFSv3 file handle of directory */
+static char filefh[NFS3_FHSIZE]; /* NFSv2 / NFSv3 file handle */
+static int filefh3_length;	/* (variable) length of filefh when NFSv3 */
 
 static enum net_loop_state nfs_download_state;
 static struct in_addr nfs_server_ip;
@@ -65,11 +70,14 @@ static int nfs_state;
 #define STATE_READ_REQ			6
 #define STATE_READLINK_REQ		7
 
-static char default_filename[64];
 static char *nfs_filename;
 static char *nfs_path;
 static char nfs_path_buff[2048];
 
+#define NFSV2_FLAG 1
+#define NFSV3_FLAG 1 << 1
+static char supported_nfs_versions = NFSV2_FLAG | NFSV3_FLAG;
+
 static inline int store_block(uchar *src, unsigned offset, unsigned len)
 {
 	ulong newsize = offset + len;
@@ -134,13 +142,6 @@ RPC_ADD_CREDENTIALS - Add RPC authentication/verifier entries
 **************************************************************************/
 static uint32_t *rpc_add_credentials(uint32_t *p)
 {
-	int hl;
-	int hostnamelen;
-	char hostname[256];
-
-	strcpy(hostname, "");
-	hostnamelen = strlen(hostname);
-
 	/* Here's the executive summary on authentication requirements of the
 	 * various NFS server implementations:	Linux accepts both AUTH_NONE
 	 * and AUTH_UNIX authentication (also accepts an empty hostname field
@@ -150,17 +151,11 @@ static uint32_t *rpc_add_credentials(uint32_t *p)
 	 * it (if the BOOTP/DHCP reply didn't give one, just use an empty
 	 * hostname).  */
 
-	hl = (hostnamelen + 3) & ~3;
-
 	/* Provide an AUTH_UNIX credential.  */
 	*p++ = htonl(1);		/* AUTH_UNIX */
-	*p++ = htonl(hl+20);		/* auth length */
-	*p++ = htonl(0);		/* stamp */
-	*p++ = htonl(hostnamelen);	/* hostname string */
-	if (hostnamelen & 3)
-		*(p + hostnamelen / 4) = 0; /* add zero padding */
-	memcpy(p, hostname, hostnamelen);
-	p += hl / 4;
+	*p++ = htonl(20);		/* auth length */
+	*p++ = 0;			/* stamp */
+	*p++ = 0;			/* hostname string */
 	*p++ = 0;			/* uid */
 	*p++ = 0;			/* gid */
 	*p++ = 0;			/* auxiliary gid list */
@@ -175,30 +170,41 @@ static uint32_t *rpc_add_credentials(uint32_t *p)
 /**************************************************************************
 RPC_LOOKUP - Lookup RPC Port numbers
 **************************************************************************/
-static void rpc_req(int rpc_prog, int rpc_proc, uint32_t *data, int datalen)
+static struct rpc_t *rpc_req_prep(void)
+{
+	return (struct rpc_t *)(net_tx_packet + net_eth_hdr_size() +
+				IP_UDP_HDR_SIZE);
+}
+
+static void rpc_req(int rpc_prog, int rpc_proc, struct rpc_t *rpc_pkt,
+		    int datalen)
 {
-	struct rpc_t pkt;
 	unsigned long id;
-	uint32_t *p;
 	int pktlen;
 	int sport;
 
 	id = ++rpc_id;
-	pkt.u.call.id = htonl(id);
-	pkt.u.call.type = htonl(MSG_CALL);
-	pkt.u.call.rpcvers = htonl(2);	/* use RPC version 2 */
-	pkt.u.call.prog = htonl(rpc_prog);
-	pkt.u.call.vers = htonl(2);	/* portmapper is version 2 */
-	pkt.u.call.proc = htonl(rpc_proc);
-	p = (uint32_t *)&(pkt.u.call.data);
-
-	if (datalen)
-		memcpy((char *)p, (char *)data, datalen*sizeof(uint32_t));
-
-	pktlen = (char *)p + datalen*sizeof(uint32_t) - (char *)&pkt;
+	rpc_pkt->u.call.id = htonl(id);
+	rpc_pkt->u.call.type = htonl(MSG_CALL);
+	rpc_pkt->u.call.rpcvers = htonl(2);	/* use RPC version 2 */
+	rpc_pkt->u.call.prog = htonl(rpc_prog);
+	switch (rpc_prog) {
+	case PROG_NFS:
+		if (supported_nfs_versions & NFSV2_FLAG)
+			rpc_pkt->u.call.vers = htonl(2);	/* NFS v2 */
+		else /* NFSV3_FLAG */
+			rpc_pkt->u.call.vers = htonl(3);	/* NFS v3 */
+		break;
+	case PROG_PORTMAP:
+	case PROG_MOUNT:
+	default:
+		/* portmapper is version 2 */
+		rpc_pkt->u.call.vers = htonl(2);
+	}
+	rpc_pkt->u.call.proc = htonl(rpc_proc);
 
-	memcpy((char *)net_tx_packet + net_eth_hdr_size() + IP_UDP_HDR_SIZE,
-	       (char *)&pkt, pktlen);
+	pktlen = ((char *)&rpc_pkt->u.call.data - (char *)&rpc_pkt) +
+		datalen * sizeof(uint32_t);
 
 	if (rpc_prog == PROG_PORTMAP)
 		sport = SUNRPC_PORT;
@@ -216,16 +222,17 @@ RPC_LOOKUP - Lookup RPC Port numbers
 **************************************************************************/
 static void rpc_lookup_req(int prog, int ver)
 {
-	uint32_t data[16];
+	uint32_t *data;
+	struct rpc_t *rpc_pkt = rpc_req_prep();
 
+	data = rpc_pkt->u.call.data;
 	data[0] = 0; data[1] = 0;	/* auth credential */
 	data[2] = 0; data[3] = 0;	/* auth verifier */
 	data[4] = htonl(prog);
 	data[5] = htonl(ver);
 	data[6] = htonl(17);	/* IP_UDP */
 	data[7] = 0;
-
-	rpc_req(PROG_PORTMAP, PORTMAP_GETPORT, data, 8);
+	rpc_req(PROG_PORTMAP, PORTMAP_GETPORT, rpc_pkt, 8);
 }
 
 /**************************************************************************
@@ -233,14 +240,14 @@ NFS_MOUNT - Mount an NFS Filesystem
 **************************************************************************/
 static void nfs_mount_req(char *path)
 {
-	uint32_t data[1024];
 	uint32_t *p;
 	int len;
 	int pathlen;
+	struct rpc_t *rpc_pkt = rpc_req_prep();
 
 	pathlen = strlen(path);
 
-	p = &(data[0]);
+	p = rpc_pkt->u.call.data;
 	p = rpc_add_credentials(p);
 
 	*p++ = htonl(pathlen);
@@ -249,9 +256,9 @@ static void nfs_mount_req(char *path)
 	memcpy(p, path, pathlen);
 	p += (pathlen + 3) / 4;
 
-	len = (uint32_t *)p - (uint32_t *)&(data[0]);
+	len = (uint32_t *)p - (uint32_t *)&(rpc_pkt->u.call.data);
 
-	rpc_req(PROG_MOUNT, MOUNT_ADDENTRY, data, len);
+	rpc_req(PROG_MOUNT, MOUNT_ADDENTRY, rpc_pkt, len);
 }
 
 /**************************************************************************
@@ -259,20 +266,20 @@ NFS_UMOUNTALL - Unmount all our NFS Filesystems on the Server
 **************************************************************************/
 static void nfs_umountall_req(void)
 {
-	uint32_t data[1024];
 	uint32_t *p;
 	int len;
+	struct rpc_t *rpc_pkt = rpc_req_prep();
 
 	if ((nfs_server_mount_port == -1) || (!fs_mounted))
 		/* Nothing mounted, nothing to umount */
 		return;
 
-	p = &(data[0]);
+	p = rpc_pkt->u.call.data;
 	p = rpc_add_credentials(p);
 
-	len = (uint32_t *)p - (uint32_t *)&(data[0]);
+	len = (uint32_t *)p - (uint32_t *)&(rpc_pkt->u.call.data);
 
-	rpc_req(PROG_MOUNT, MOUNT_UMOUNTALL, data, len);
+	rpc_req(PROG_MOUNT, MOUNT_UMOUNTALL, rpc_pkt, len);
 }
 
 /***************************************************************************
@@ -284,19 +291,25 @@ static void nfs_umountall_req(void)
  **************************************************************************/
 static void nfs_readlink_req(void)
 {
-	uint32_t data[1024];
 	uint32_t *p;
 	int len;
+	struct rpc_t *rpc_pkt = rpc_req_prep();
 
-	p = &(data[0]);
+	p = rpc_pkt->u.call.data;
 	p = rpc_add_credentials(p);
 
-	memcpy(p, filefh, NFS_FHSIZE);
-	p += (NFS_FHSIZE / 4);
+	if (supported_nfs_versions & NFSV2_FLAG) {
+		memcpy(p, filefh, NFS_FHSIZE);
+		p += (NFS_FHSIZE / 4);
+	} else { /* NFSV3_FLAG */
+		*p++ = htonl(filefh3_length);
+		memcpy(p, filefh, filefh3_length);
+		p += (filefh3_length / 4);
+	}
 
-	len = (uint32_t *)p - (uint32_t *)&(data[0]);
+	len = (uint32_t *)p - (uint32_t *)&(rpc_pkt->u.call.data);
 
-	rpc_req(PROG_NFS, NFS_READLINK, data, len);
+	rpc_req(PROG_NFS, NFS_READLINK, rpc_pkt, len);
 }
 
 /**************************************************************************
@@ -304,27 +317,42 @@ NFS_LOOKUP - Lookup Pathname
 **************************************************************************/
 static void nfs_lookup_req(char *fname)
 {
-	uint32_t data[1024];
 	uint32_t *p;
 	int len;
 	int fnamelen;
+	struct rpc_t *rpc_pkt = rpc_req_prep();
 
 	fnamelen = strlen(fname);
 
-	p = &(data[0]);
+	p = rpc_pkt->u.call.data;
 	p = rpc_add_credentials(p);
 
-	memcpy(p, dirfh, NFS_FHSIZE);
-	p += (NFS_FHSIZE / 4);
-	*p++ = htonl(fnamelen);
-	if (fnamelen & 3)
-		*(p + fnamelen / 4) = 0;
-	memcpy(p, fname, fnamelen);
-	p += (fnamelen + 3) / 4;
-
-	len = (uint32_t *)p - (uint32_t *)&(data[0]);
-
-	rpc_req(PROG_NFS, NFS_LOOKUP, data, len);
+	if (supported_nfs_versions & NFSV2_FLAG) {
+		memcpy(p, dirfh, NFS_FHSIZE);
+		p += (NFS_FHSIZE / 4);
+		*p++ = htonl(fnamelen);
+		if (fnamelen & 3)
+			*(p + fnamelen / 4) = 0;
+		memcpy(p, fname, fnamelen);
+		p += (fnamelen + 3) / 4;
+
+		len = (uint32_t *)p - (uint32_t *)&(rpc_pkt->u.call.data);
+
+		rpc_req(PROG_NFS, NFS_LOOKUP, rpc_pkt, len);
+	} else {  /* NFSV3_FLAG */
+		*p++ = htonl(NFS_FHSIZE);	/* Dir handle length */
+		memcpy(p, dirfh, NFS_FHSIZE);
+		p += (NFS_FHSIZE / 4);
+		*p++ = htonl(fnamelen);
+		if (fnamelen & 3)
+			*(p + fnamelen / 4) = 0;
+		memcpy(p, fname, fnamelen);
+		p += (fnamelen + 3) / 4;
+
+		len = (uint32_t *)p - (uint32_t *)&(rpc_pkt->u.call.data);
+
+		rpc_req(PROG_NFS, NFS3PROC_LOOKUP, rpc_pkt, len);
+	}
 }
 
 /**************************************************************************
@@ -332,22 +360,32 @@ NFS_READ - Read File on NFS Server
 **************************************************************************/
 static void nfs_read_req(int offset, int readlen)
 {
-	uint32_t data[1024];
 	uint32_t *p;
 	int len;
+	struct rpc_t *rpc_pkt = rpc_req_prep();
 
-	p = &(data[0]);
+	p = rpc_pkt->u.call.data;
 	p = rpc_add_credentials(p);
 
-	memcpy(p, filefh, NFS_FHSIZE);
-	p += (NFS_FHSIZE / 4);
-	*p++ = htonl(offset);
-	*p++ = htonl(readlen);
-	*p++ = 0;
+	if (supported_nfs_versions & NFSV2_FLAG) {
+		memcpy(p, filefh, NFS_FHSIZE);
+		p += (NFS_FHSIZE / 4);
+		*p++ = htonl(offset);
+		*p++ = htonl(readlen);
+		*p++ = 0;
+	} else { /* NFSV3_FLAG */
+		*p++ = htonl(filefh3_length);
+		memcpy(p, filefh, filefh3_length);
+		p += (filefh3_length / 4);
+		*p++ = htonl(0); /* offset is 64-bit long, so fill with 0 */
+		*p++ = htonl(offset);
+		*p++ = htonl(readlen);
+		*p++ = 0;
+	}
 
-	len = (uint32_t *)p - (uint32_t *)&(data[0]);
+	len = (uint32_t *)p - (uint32_t *)&(rpc_pkt->u.call.data);
 
-	rpc_req(PROG_NFS, NFS_READ, data, len);
+	rpc_req(PROG_NFS, NFS_READ, rpc_pkt, len);
 }
 
 /**************************************************************************
@@ -359,10 +397,16 @@ static void nfs_send(void)
 
 	switch (nfs_state) {
 	case STATE_PRCLOOKUP_PROG_MOUNT_REQ:
-		rpc_lookup_req(PROG_MOUNT, 1);
+		if (supported_nfs_versions & NFSV2_FLAG)
+			rpc_lookup_req(PROG_MOUNT, 1);
+		else  /* NFSV3_FLAG */
+			rpc_lookup_req(PROG_MOUNT, 3);
 		break;
 	case STATE_PRCLOOKUP_PROG_NFS_REQ:
-		rpc_lookup_req(PROG_NFS, 2);
+		if (supported_nfs_versions & NFSV2_FLAG)
+			rpc_lookup_req(PROG_NFS, 2);
+		else  /* NFSV3_FLAG */
+			rpc_lookup_req(PROG_NFS, 3);
 		break;
 	case STATE_MOUNT_REQ:
 		nfs_mount_req(nfs_path);
@@ -390,7 +434,7 @@ static int rpc_lookup_reply(int prog, uchar *pkt, unsigned len)
 {
 	struct rpc_t rpc_pkt;
 
-	memcpy((unsigned char *)&rpc_pkt, pkt, len);
+	memcpy(&rpc_pkt.u.data[0], pkt, len);
 
 	debug("%s\n", __func__);
 
@@ -422,7 +466,7 @@ static int nfs_mount_reply(uchar *pkt, unsigned len)
 
 	debug("%s\n", __func__);
 
-	memcpy((unsigned char *)&rpc_pkt, pkt, len);
+	memcpy(&rpc_pkt.u.data[0], pkt, len);
 
 	if (ntohl(rpc_pkt.u.reply.id) > rpc_id)
 		return -NFS_RPC_ERR;
@@ -436,6 +480,7 @@ static int nfs_mount_reply(uchar *pkt, unsigned len)
 		return -1;
 
 	fs_mounted = 1;
+	/*  NFSv2 and NFSv3 use same structure */
 	memcpy(dirfh, rpc_pkt.u.reply.data + 1, NFS_FHSIZE);
 
 	return 0;
@@ -447,7 +492,7 @@ static int nfs_umountall_reply(uchar *pkt, unsigned len)
 
 	debug("%s\n", __func__);
 
-	memcpy((unsigned char *)&rpc_pkt, pkt, len);
+	memcpy(&rpc_pkt.u.data[0], pkt, len);
 
 	if (ntohl(rpc_pkt.u.reply.id) > rpc_id)
 		return -NFS_RPC_ERR;
@@ -471,7 +516,7 @@ static int nfs_lookup_reply(uchar *pkt, unsigned len)
 
 	debug("%s\n", __func__);
 
-	memcpy((unsigned char *)&rpc_pkt, pkt, len);
+	memcpy(&rpc_pkt.u.data[0], pkt, len);
 
 	if (ntohl(rpc_pkt.u.reply.id) > rpc_id)
 		return -NFS_RPC_ERR;
@@ -483,31 +528,90 @@ static int nfs_lookup_reply(uchar *pkt, unsigned len)
 	    rpc_pkt.u.reply.astatus  ||
 	    rpc_pkt.u.reply.data[0]) {
 		switch (ntohl(rpc_pkt.u.reply.astatus)) {
-		case 0: /* Not an error */
+		case NFS_RPC_SUCCESS: /* Not an error */
 			break;
-		case 2: /* Remote can't support NFS version */
-			printf("*** ERROR: NFS version not supported: Requested: V%d, accepted: min V%d - max V%d\n",
-			       2,
-			       ntohl(rpc_pkt.u.reply.data[0]),
-			       ntohl(rpc_pkt.u.reply.data[1]));
+		case NFS_RPC_PROG_MISMATCH:
+			/* Remote can't support NFS version */
+			switch (ntohl(rpc_pkt.u.reply.data[0])) {
+			/* Minimal supported NFS version */
+			case 3:
+				debug("*** Waring: NFS version not supported: Requested: V%d, accepted: min V%d - max V%d\n",
+				      (supported_nfs_versions & NFSV2_FLAG) ?
+						2 : 3,
+				      ntohl(rpc_pkt.u.reply.data[0]),
+				      ntohl(rpc_pkt.u.reply.data[1]));
+				debug("Will retry with NFSv3\n");
+				/* Clear NFSV2_FLAG from supported versions */
+				supported_nfs_versions &= ~NFSV2_FLAG;
+				return -NFS_RPC_PROG_MISMATCH;
+			case 4:
+			default:
+				puts("*** ERROR: NFS version not supported");
+				debug(": Requested: V%d, accepted: min V%d - max V%d\n",
+				      (supported_nfs_versions & NFSV2_FLAG) ?
+						2 : 3,
+				      ntohl(rpc_pkt.u.reply.data[0]),
+				      ntohl(rpc_pkt.u.reply.data[1]));
+				puts("\n");
+			}
 			break;
+		case NFS_RPC_PROG_UNAVAIL:
+		case NFS_RPC_PROC_UNAVAIL:
+		case NFS_RPC_GARBAGE_ARGS:
+		case NFS_RPC_SYSTEM_ERR:
 		default: /* Unknown error on 'accept state' flag */
-			printf("*** ERROR: accept state error (%d)\n",
-			       ntohl(rpc_pkt.u.reply.astatus));
+			debug("*** ERROR: accept state error (%d)\n",
+			      ntohl(rpc_pkt.u.reply.astatus));
 			break;
 		}
 		return -1;
 	}
 
-	memcpy(filefh, rpc_pkt.u.reply.data + 1, NFS_FHSIZE);
+	if (supported_nfs_versions & NFSV2_FLAG) {
+		memcpy(filefh, rpc_pkt.u.reply.data + 1, NFS_FHSIZE);
+	} else {  /* NFSV3_FLAG */
+		filefh3_length = ntohl(rpc_pkt.u.reply.data[1]);
+		if (filefh3_length > NFS3_FHSIZE)
+			filefh3_length  = NFS3_FHSIZE;
+		memcpy(filefh, rpc_pkt.u.reply.data + 2, filefh3_length);
+	}
 
 	return 0;
 }
 
+static int nfs3_get_attributes_offset(uint32_t *data)
+{
+	if (ntohl(data[1]) != 0) {
+		/* 'attributes_follow' flag is TRUE,
+		 * so we have attributes on 21 dwords */
+		/* Skip unused values :
+			type;	32 bits value,
+			mode;	32 bits value,
+			nlink;	32 bits value,
+			uid;	32 bits value,
+			gid;	32 bits value,
+			size;	64 bits value,
+			used;	64 bits value,
+			rdev;	64 bits value,
+			fsid;	64 bits value,
+			fileid;	64 bits value,
+			atime;	64 bits value,
+			mtime;	64 bits value,
+			ctime;	64 bits value,
+		*/
+		return 22;
+	} else {
+		/* 'attributes_follow' flag is FALSE,
+		 * so we don't have any attributes */
+		return 1;
+	}
+}
+
 static int nfs_readlink_reply(uchar *pkt, unsigned len)
 {
 	struct rpc_t rpc_pkt;
 	int rlen;
+	int nfsv3_data_offset = 0;
 
 	debug("%s\n", __func__);
 
@@ -524,17 +628,27 @@ static int nfs_readlink_reply(uchar *pkt, unsigned len)
 	    rpc_pkt.u.reply.data[0])
 		return -1;
 
-	rlen = ntohl(rpc_pkt.u.reply.data[1]); /* new path length */
+	if (!(supported_nfs_versions & NFSV2_FLAG)) { /* NFSV3_FLAG */
+		nfsv3_data_offset =
+			nfs3_get_attributes_offset(rpc_pkt.u.reply.data);
+	}
+
+	/* new path length */
+	rlen = ntohl(rpc_pkt.u.reply.data[1 + nfsv3_data_offset]);
 
-	if (*((char *)&(rpc_pkt.u.reply.data[2])) != '/') {
+	if (*((char *)&(rpc_pkt.u.reply.data[2 + nfsv3_data_offset])) != '/') {
 		int pathlen;
+
 		strcat(nfs_path, "/");
 		pathlen = strlen(nfs_path);
-		memcpy(nfs_path + pathlen, (uchar *)&(rpc_pkt.u.reply.data[2]),
+		memcpy(nfs_path + pathlen,
+		       (uchar *)&(rpc_pkt.u.reply.data[2 + nfsv3_data_offset]),
 		       rlen);
 		nfs_path[pathlen + rlen] = 0;
 	} else {
-		memcpy(nfs_path, (uchar *)&(rpc_pkt.u.reply.data[2]), rlen);
+		memcpy(nfs_path,
+		       (uchar *)&(rpc_pkt.u.reply.data[2 + nfsv3_data_offset]),
+		       rlen);
 		nfs_path[rlen] = 0;
 	}
 	return 0;
@@ -544,10 +658,11 @@ static int nfs_read_reply(uchar *pkt, unsigned len)
 {
 	struct rpc_t rpc_pkt;
 	int rlen;
+	uchar *data_ptr;
 
 	debug("%s\n", __func__);
 
-	memcpy((uchar *)&rpc_pkt, pkt, sizeof(rpc_pkt.u.reply));
+	memcpy(&rpc_pkt.u.data[0], pkt, sizeof(rpc_pkt.u.reply));
 
 	if (ntohl(rpc_pkt.u.reply.id) > rpc_id)
 		return -NFS_RPC_ERR;
@@ -571,10 +686,25 @@ static int nfs_read_reply(uchar *pkt, unsigned len)
 	if (!(nfs_offset % ((NFS_READ_SIZE / 2) * 10)))
 		putc('#');
 
-	rlen = ntohl(rpc_pkt.u.reply.data[18]);
-	if (store_block((uchar *)pkt + sizeof(rpc_pkt.u.reply),
-			nfs_offset, rlen))
-		return -9999;
+	if (supported_nfs_versions & NFSV2_FLAG) {
+		rlen = ntohl(rpc_pkt.u.reply.data[18]);
+		data_ptr = (uchar *)&(rpc_pkt.u.reply.data[19]);
+	} else {  /* NFSV3_FLAG */
+		int nfsv3_data_offset =
+			nfs3_get_attributes_offset(rpc_pkt.u.reply.data);
+
+		/* count value */
+		rlen = ntohl(rpc_pkt.u.reply.data[1 + nfsv3_data_offset]);
+		/* Skip unused values :
+			EOF:		32 bits value,
+			data_size:	32 bits value,
+		*/
+		data_ptr = (uchar *)
+			&(rpc_pkt.u.reply.data[4 + nfsv3_data_offset]);
+	}
+
+	if (store_block(data_ptr, nfs_offset, rlen))
+			return -9999;
 
 	return rlen;
 }
@@ -642,7 +772,7 @@ static void nfs_handler(uchar *pkt, unsigned dest, struct in_addr sip,
 		if (reply == -NFS_RPC_DROP) {
 			break;
 		} else if (reply == -NFS_RPC_ERR) {
-			puts("*** ERROR: Cannot umount\n");
+			debug("*** ERROR: Cannot umount\n");
 			net_set_state(NETLOOP_FAIL);
 		} else {
 			puts("\ndone\n");
@@ -658,6 +788,14 @@ static void nfs_handler(uchar *pkt, unsigned dest, struct in_addr sip,
 			puts("*** ERROR: File lookup fail\n");
 			nfs_state = STATE_UMOUNT_REQ;
 			nfs_send();
+		} else if (reply == -NFS_RPC_PROG_MISMATCH &&
+			   supported_nfs_versions != 0) {
+			/* umount */
+			nfs_state = STATE_UMOUNT_REQ;
+			nfs_send();
+			/* And retry with another supported version */
+			nfs_state = STATE_PRCLOOKUP_PROG_MOUNT_REQ;
+			nfs_send();
 		} else {
 			nfs_state = STATE_READ_REQ;
 			nfs_offset = 0;
@@ -697,6 +835,8 @@ static void nfs_handler(uchar *pkt, unsigned dest, struct in_addr sip,
 		} else {
 			if (!rlen)
 				nfs_download_state = NETLOOP_SUCCESS;
+			if (rlen < 0)
+				debug("NFS READ error (%d)\n", rlen);
 			nfs_state = STATE_UMOUNT_REQ;
 			nfs_send();
 		}
@@ -715,20 +855,19 @@ void nfs_start(void)
 
 	if (nfs_path == NULL) {
 		net_set_state(NETLOOP_FAIL);
-		puts("*** ERROR: Fail allocate memory\n");
+		debug("*** ERROR: Fail allocate memory\n");
 		return;
 	}
 
 	if (net_boot_file_name[0] == '\0') {
-		sprintf(default_filename, "/nfsroot/%02X%02X%02X%02X.img",
+		sprintf(nfs_path, "/nfsroot/%02X%02X%02X%02X.img",
 			net_ip.s_addr & 0xFF,
 			(net_ip.s_addr >>  8) & 0xFF,
 			(net_ip.s_addr >> 16) & 0xFF,
 			(net_ip.s_addr >> 24) & 0xFF);
-		strcpy(nfs_path, default_filename);
 
-		printf("*** Warning: no boot file name; using '%s'\n",
-		       nfs_path);
+		debug("*** Warning: no boot file name; using '%s'\n",
+		      nfs_path);
 	} else {
 		char *p = net_boot_file_name;
 
@@ -746,10 +885,10 @@ void nfs_start(void)
 	nfs_filename = basename(nfs_path);
 	nfs_path     = dirname(nfs_path);
 
-	printf("Using %s device\n", eth_get_name());
+	debug("Using %s device\n", eth_get_name());
 
-	printf("File transfer via NFS from server %pI4; our IP address is %pI4",
-	       &nfs_server_ip, &net_ip);
+	debug("File transfer via NFS from server %pI4; our IP address is %pI4",
+	      &nfs_server_ip, &net_ip);
 
 	/* Check if we need to send across this subnet */
 	if (net_gateway.s_addr && net_netmask.s_addr) {
@@ -759,18 +898,17 @@ void nfs_start(void)
 		our_net.s_addr = net_ip.s_addr & net_netmask.s_addr;
 		server_net.s_addr = net_server_ip.s_addr & net_netmask.s_addr;
 		if (our_net.s_addr != server_net.s_addr)
-			printf("; sending through gateway %pI4",
-			       &net_gateway);
+			debug("; sending through gateway %pI4",
+			      &net_gateway);
 	}
-	printf("\nFilename '%s/%s'.", nfs_path, nfs_filename);
+	debug("\nFilename '%s/%s'.", nfs_path, nfs_filename);
 
 	if (net_boot_file_expected_size_in_blocks) {
-		printf(" Size is 0x%x Bytes = ",
-		       net_boot_file_expected_size_in_blocks << 9);
+		debug(" Size is 0x%x Bytes = ",
+		      net_boot_file_expected_size_in_blocks << 9);
 		print_size(net_boot_file_expected_size_in_blocks << 9, "");
 	}
-	printf("\nLoad address: 0x%lx\n"
-		"Loading: *\b", load_addr);
+	debug("\nLoad address: 0x%lx\nLoading: *\b", load_addr);
 
 	net_set_timeout_handler(nfs_timeout, nfs_timeout_handler);
 	net_set_udp_handler(nfs_handler);
diff --git a/net/nfs.h b/net/nfs.h
index d69b422..aa4e450 100644
--- a/net/nfs.h
+++ b/net/nfs.h
@@ -25,7 +25,10 @@
 #define NFS_READLINK    5
 #define NFS_READ        6
 
+#define NFS3PROC_LOOKUP 3
+
 #define NFS_FHSIZE      32
+#define NFS3_FHSIZE     64
 
 #define NFSERR_PERM     1
 #define NFSERR_NOENT    2
@@ -44,7 +47,15 @@
 #define NFS_READ_SIZE 1024 /* biggest power of two that fits Ether frame */
 #endif
 
-#define NFS_MAXLINKDEPTH 16
+/* Values for Accept State flag on RPC answers (See: rfc1831) */
+enum rpc_accept_stat {
+	NFS_RPC_SUCCESS = 0,	/* RPC executed successfully */
+	NFS_RPC_PROG_UNAVAIL = 1,	/* remote hasn't exported program */
+	NFS_RPC_PROG_MISMATCH = 2,	/* remote can't support version # */
+	NFS_RPC_PROC_UNAVAIL = 3,	/* program can't support procedure */
+	NFS_RPC_GARBAGE_ARGS = 4,	/* procedure can't decode params */
+	NFS_RPC_SYSTEM_ERR = 5	/* errors like memory allocation failure */
+};
 
 struct rpc_t {
 	union {
@@ -65,7 +76,7 @@ struct rpc_t {
 			uint32_t verifier;
 			uint32_t v2;
 			uint32_t astatus;
-			uint32_t data[19];
+			uint32_t data[NFS_READ_SIZE / sizeof(uint32_t)];
 		} reply;
 	} u;
 };