tftp.ino

// ----------------------------------------------------------------------------
// 
// ----------------------------------------------------------------------------
//
//        2 bytes    string   1 byte     string   1 byte
//        -----------------------------------------------
// RRQ/  | 01/02 |  Filename  |   0  |    Mode    |   0  |
// WRQ    -----------------------------------------------
//
//        2 bytes    2 bytes       n bytes
//        ---------------------------------
// DATA  | 03    |   Block #  |    Data    |
//        ---------------------------------
//
//        2 bytes    2 bytes
//        -------------------
// ACK   | 04    |   Block #  |
//        --------------------
// 
//        2 bytes  2 bytes        string    1 byte
//        ----------------------------------------
// ERROR | 05    |  ErrorCode |   ErrMsg   |   0  |
//        ----------------------------------------
// 
// ----------------------------------------------------------------------------
// [email protected] wrote this file.
// ----------------------------------------------------------------------------

// ----------------------------------------------------------------------------
#include <ESP8266WiFi.h>
#include <WiFiUdp.h>

// ----------------------------------------------------------------------------
#define DEBUG 1
#define TFTP_PORT 69

// ----------------------------------------------------------------------------
typedef enum Opcode_t{
  Opcode_WriteRequest = 2,
  Opcode_Data = 3,
  Opcode_Ack = 4,
  Opcode_Error = 5
}Opcode_t;

// ----------------------------------------------------------------------------
typedef struct __attribute__((packed)) tftp{
  uint16_t op_code;
  uint16_t block_number;
  char tftp_data[];
}tftp_t;

// ----------------------------------------------------------------------------
WiFiUDP Udp;

// ----------------------------------------------------------------------------
const char* ssid = "*******";
const char* password = "*******";

// ----------------------------------------------------------------------------
static void udp_handle(void);
static void send_ack(uint16_t block_number);

// ----------------------------------------------------------------------------
void setup()
{
  #if DEBUG
    Serial.begin(115200);
    Serial.println();
    Serial.printf("Connecting to %s ", ssid);
  #endif

  // ...
  WiFi.begin(ssid, password);
  while(WiFi.status() != WL_CONNECTED)
  {
    delay(500);
    #if DEBUG
      Serial.print(".");
    #endif
  }

  // ...
  #if DEBUG
    Serial.println(" connected");
  #endif

  // ...
  Udp.begin(TFTP_PORT);
  #if DEBUG
    Serial.printf(
      "Now listening at IP %s, UDP port %d\n", 
      WiFi.localIP().toString().c_str(), TFTP_PORT
    );
  #endif
}

// ----------------------------------------------------------------------------
void loop()
{
  udp_handle();
}

// ----------------------------------------------------------------------------
static void send_ack(const char* ack_buf, uint8_t size)
{
  Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
  Udp.write(ack_buf, size);
  if(Udp.endPacket() == 1)
  {
    #if DEBUG
      Serial.printf("\n UDP packet send successfully \n");
    #endif
  }
  else
  {
    #if DEBUG
      Serial.printf("\n UDP packet send failed \n");
    #endif
  }
}

// ----------------------------------------------------------------------------
static void udp_handle(void)
{
  int packet_size = Udp.parsePacket();
  static uint16_t old_block_number = 0;

  if(packet_size)
  {
    #if DEBUG
      // Receive incoming UDP packets
      Serial.printf(
        "Received %d bytes from %s, port %d\n", 
        packet_size, Udp.remoteIP().toString().c_str(), Udp.remotePort()
      );
    #endif

    // op_code + block_number + data
    char incoming_packet[4 + 512]; 

    // ...
    int len = Udp.read(incoming_packet, 516);
    if(len > 0)
    {
      tftp_t* tftp_packet = (tftp_t*)(incoming_packet);

      tftp_packet->op_code = (
        (tftp_packet->op_code >> 8) | (tftp_packet->op_code << 8)
      );
      
      tftp_packet->block_number = (
        (tftp_packet->block_number >> 8) | (tftp_packet->block_number << 8)
      );

      #if DEBUG
        Serial.printf(
          "\nComing Data : op_code : %d, block_number : %d, \n", 
          tftp_packet->op_code, tftp_packet->block_number
        );
      #endif

      // ...
      if(tftp_packet->op_code == Opcode_WriteRequest)
      {
        tftp_t ack_packet;
        ack_packet.op_code = ((Opcode_Ack >> 8) | (Opcode_Ack << 8));
        ack_packet.block_number = 0;
        send_ack((const char*)&ack_packet, sizeof(ack_packet));
      }
      else if (tftp_packet->op_code == Opcode_Data)
      {
        tftp_t ack_packet;
        ack_packet.op_code = ((Opcode_Ack >> 8) | (Opcode_Ack << 8));
        ack_packet.block_number = (
          (tftp_packet->block_number >> 8) | (tftp_packet->block_number << 8)
        );

        // If Received Packet not duplicated
        if(tftp_packet->block_number != old_block_number)
        {
          old_block_number = tftp_packet->block_number;
          #if DEBUG
            Serial.printf("\n [ Data ->");
            for(int i = 0; i < packet_size - 4; ++i)
            {
              Serial.printf("%02x, ", tftp_packet->tftp_data[i]);
            }
            Serial.printf(" ]");
            Serial.printf("\n New packet Received\n");
          #endif

          // TODO: Write flash section
        }
        else
        {
          #if DEBUG
            Serial.printf("\n Duplicated \n");
          #endif
        }

        // ...
        if(packet_size < 516)
        {
          #if DEBUG
            Serial.printf("Last Packet Received \n");
          #endif
        }

        // ...
        send_ack((const char*)&ack_packet, sizeof(ack_packet));
      }
      else if(tftp_packet->op_code == Opcode_Error)
      {
        #if DEBUG
          Serial.printf("\n Error");
        #endif
      }

      // ...
      memset(incoming_packet, 0, sizeof(incoming_packet));
    }
  }
}