From 1ab89d6a623a009283cf6394a5e1418c05b61573 Mon Sep 17 00:00:00 2001
From: Werner Almesberger <werner@almesberger.net>
Date: Tue, 16 Apr 2013 13:55:28 -0300
Subject: [PATCH] tftp.h, tftp.c: add tftp_put

---
 software/bios/tftp.c | 82 ++++++++++++++++++++++++++++++++++++++++++++
 software/bios/tftp.h |  1 +
 2 files changed, 83 insertions(+)

diff --git a/software/bios/tftp.c b/software/bios/tftp.c
index 2ceadb94..a2e1bfe1 100644
--- a/software/bios/tftp.c
+++ b/software/bios/tftp.c
@@ -45,10 +45,22 @@ static int format_ack(uint8_t *buf, uint16_t block)
 	return 4;
 }
 
+static int format_data(uint8_t *buf, uint16_t block, const void *data, int len)
+{
+	*buf++ = 0x00; /* Opcode: Data*/
+	*buf++ = TFTP_DATA;
+	*buf++ = (block & 0xff00) >> 8;
+	*buf++ = (block & 0x00ff);
+	memcpy(buf, data, len);
+	return len+4;
+}
+
 static uint8_t *packet_data;
 static int total_length;
 static int transfer_finished;
 static uint8_t *dst_buffer;
+static int last_ack; /* signed, so we can use -1 */
+static uint16_t data_port;
 
 static void rx_callback(uint32_t src_ip, uint16_t src_port,
     uint16_t dst_port, void *_data, unsigned int length)
@@ -63,6 +75,11 @@ static void rx_callback(uint32_t src_ip, uint16_t src_port,
 	if(dst_port != PORT_IN) return;
 	opcode = data[0] << 8 | data[1];
 	block = data[2] << 8 | data[3];
+	if(opcode == TFTP_ACK) { /* Acknowledgement */
+		data_port = src_port;
+		last_ack = block;
+		return;
+	}
 	if(block < 1) return;
 	if(opcode == TFTP_DATA) { /* Data */
 		length -= 4;
@@ -132,3 +149,68 @@ int tftp_get(uint32_t ip, const char *filename, void *buffer)
 
 	return total_length;
 }
+
+int tftp_put(uint32_t ip, const char *filename, const void *buffer, int size)
+{
+	int len, send;
+	int tries;
+	int i;
+	int block = 0, sent = 0;
+	
+	if(!microudp_arp_resolve(ip))
+		return -1;
+
+	microudp_set_callback(rx_callback);
+
+	packet_data = microudp_get_tx_buffer();
+
+	total_length = 0;
+	transfer_finished = 0;
+	tries = 5;
+	while(1) {
+		len = format_request(packet_data, TFTP_WRQ, filename);
+		microudp_send(PORT_IN, PORT_OUT, len);
+		for(i=0;i<2000000;i++) {
+			last_ack = -1;
+			microudp_service();
+			if(last_ack == block)
+				goto send_data;
+			if(transfer_finished)
+				goto fail;
+		}
+		tries--;
+		if(tries == 0)
+			goto fail;
+	}
+
+send_data:
+	do {
+		block++;
+		send = sent+BLOCK_SIZE > size ? size-sent : BLOCK_SIZE;
+		tries = 5;
+		while(1) {
+			len = format_data(packet_data, block, buffer, send);
+			microudp_send(PORT_IN, data_port, len);
+			for(i=0;i<12000000;i++) {
+				microudp_service();
+				if(transfer_finished)
+					goto fail;
+				if(last_ack == block)
+					goto next;
+			}
+			if (!--tries)
+				goto fail;
+		}
+next:
+		sent += send;
+		buffer += send;
+	} while (send == BLOCK_SIZE);
+
+	microudp_set_callback(NULL);
+
+	return sent;
+
+fail:
+	microudp_set_callback(NULL);
+	return -1;
+}
diff --git a/software/bios/tftp.h b/software/bios/tftp.h
index a32837f1..7babb2d7 100644
--- a/software/bios/tftp.h
+++ b/software/bios/tftp.h
@@ -4,6 +4,7 @@
 #include <stdint.h>
 
 int tftp_get(uint32_t ip, const char *filename, void *buffer);
+int tftp_put(uint32_t ip, const char *filename, const void *buffer, int size);
 
 #endif /* __TFTP_H */
 
-- 
2.30.2