tftp.h, tftp.c: add tftp_put

diff --git a/software/bios/tftp.c b/software/bios/tftp.c
index 2ceadb94a2fe465a7ff9a2afccd5adff58ca1add..a2e1bfe1becf25e541838c6a27d464db7cb52d65 100644 (file)
--- 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 a32837f1051ab47cf2dc391a0a70924bb8f626f7..7babb2d777c2fad9636aa45a3931337cd15c3166 100644 (file)
--- 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 */