/** @brief Implementation of TFTP protocol to send and receive application binary, application configuration, operation logs, and error logs
 * @copyright 2020 Remsdaq Ltd.
 *
 * @file c4_tftp.c
 *
 *  @date 10 Sep 2020
 *  @author Dan Smithers <dwhs@remsdaq.com>
 *
 *  TFTP Formats using RFC 1350 with options as specified in RFC 2847-9
 *
 * | Type  | Op #    | Format without header |||||||||||
 * | ------| --------|-----       |||||||||||
 * | RRQ   | 2 bytes | string     | 1 byte  | string | 1 byte | string | 1 byte | ...    | string | 1 byte | string | 1 byte |
 * |   ^   | 01      | Filename   | 0       | Opt 1  | 0      | Val 1  | 0      | ^      | Opt n  | 0      | Val n  | 0      |
 * | WRQ   | 2 bytes | string     | 1 byte  | string | 1 byte | string | 1 byte | ...    | string | 1 byte | string | 1 byte |
 * |   ^   | 02      | Filename   | 0       | Opt 1  | 0      | Val 1  | 0      | ^      | Opt n  | 0      | Val n  | 0      |
 * | DATA  | 2 bytes | 2 bytes    | n bytes |   |||||||||
 * | ^     | 03      | Block #    | Data    | ^ |||||||||
 * | ACK   | 2 bytes | 2 bytes    |   ||||||||||
 * | ^     | 04      | Block #    | ^ ||||||||||
 * | ERROR | 2 bytes | 2 bytes    | string  | 1 byte |   ||||||||
 * | ^     | 05      | Error Code | ErrMsg  | 0      | ^ ||||||||
 * | OACK  | 2 bytes | string     | 1 byte  | string | 1 byte | ...    | string | 1 byte | string | 1 byte |   ||
 * | ^     | 06      | Opt 1      | 0       | Val 1  | 0      | ^      | Opt n  | 0      | Val n  | 0      | ^ ||
 *
 */

/*******************************************************************************
 * Includes
 ******************************************************************************/
#include "c4_tftp.h"

#include "c4_qspi.h"

#include "platform-memory-map.h"

#include "lwip/tcpip.h"
//#include "lwip/init.h"
#include "lwip/netif.h"
#include "lwip/sockets.h"
#include "enet_ethernetif.h"

/*******************************************************************************
 * External Definitions
 ******************************************************************************/

/*******************************************************************************
 * Defines
 ******************************************************************************/
/** @brief Maximum
 *
 * @param[in] a first value
 * @param[in] b second value
 *
 * @return a if a > b otherwise b
 */
#ifndef max
#define max(a,b)             \
({                           \
    __typeof__ (a) _a = (a); \
    __typeof__ (b) _b = (b); \
    _a > _b ? _a : _b;       \
})
#endif
/** @brief Minimum
 *
 * @param[in] a first value
 * @param[in] b second value
 *
 * @return a if a < b otherwise b
 */
#ifndef min
#define min(a,b)             \
({                           \
    __typeof__ (a) _a = (a); \
    __typeof__ (b) _b = (b); \
    _a < _b ? _a : _b;       \
})
#endif

/** System clock name. */
#define EXAMPLE_CLOCK_NAME      kCLOCK_CoreSysClk
static const uint32_t TFTP_PORT = 69; ///< Well known TFTP port
static const uint16_t TFTP_DATA_MAX = 1020; ///< Maximum TFTP packet size.
static const uint16_t TFTP_PACKET_MAX = (TFTP_DATA_MAX + 4); ///< Maximum TFTP packet size.
#define TFTP_ERR_MAX 128 ///< Maximum TFTP error message size to send

/*******************************************************************************
 * Structures
 ******************************************************************************/



/// @brief TFTP opcodes.
typedef enum
{
    E_TFTP_OP_RRQ   = 1, ///< TFTP Read request
    E_TFTP_OP_WRQ   = 2, ///< TFTP Write request
    E_TFTP_OP_DATA  = 3, ///< TFTP Data packet
    E_TFTP_OP_ACK   = 4, ///< TFTP Acknowledge packet
    E_TFTP_OP_ERROR = 5, ///< TFTP Error packet
    E_TFTP_OP_OACK  = 6, ///< TFTP Optiona Acknowledge packet
    E_TFTP_OP_NUM        ///< Number of supported TFTP op codes
} tftp_opcode_t;

/** @def CALLISTO_TFTP_ERR
 * @brief X-Macro to construct error codes, printable strings, and flash codes.
 *
 * @param name Error ID
 * @param text Printable string
 */
/** @brief TFTP error codes and strings.
 *
 * The macro values are used to construct matching enumeration and string array
 * using the x-macro strategy.
 */
#define TFTP_ERROR_LIST \
    CALLISTO_TFTP_ERR(UNDEF,        "Not defined, see error message (if any).") \
    CALLISTO_TFTP_ERR(NOT_FOUND,    "File not found.") \
    CALLISTO_TFTP_ERR(ACCESS,       "Access violation.") \
    CALLISTO_TFTP_ERR(ALLOC,        "Disk full or allocation exceeded.") \
    CALLISTO_TFTP_ERR(ILLEGAL,      "Illegal TFTP operation.") \
    CALLISTO_TFTP_ERR(BAD_TRANSFER, "Unknown transfer ID.") \
    CALLISTO_TFTP_ERR(EXISTS,       "File already exists.") \
    CALLISTO_TFTP_ERR(USER,         "No such user.") \
    CALLISTO_TFTP_ERR(OPT,          "Option error.")

/// @brief TFTP error codes.
typedef enum
{
#define CALLISTO_TFTP_ERR(name, text) E_TFTP_ERR_##name,
    TFTP_ERROR_LIST
#undef CALLISTO_TFTP_ERR

    E_TFTP_ERR_NUM               ///< Number of supported TFTP error codes
} tftp_err_t;

/// @brief TFTP Options.
typedef enum
{
    E_TFTP_OPT_BLKSIZE, ///< Negotiate blocksize
    E_TFTP_OPT_TIMEOUT, ///< Negotiate timeout
    E_TFTP_OPT_TSIZE,   ///< Negotiate transfer size total
    E_TFTP_OPT_NUM      ///< Total number of TFTP options
} tftp_opt_t;

/// @brief TFTP mode strings
const char *TFTP_MODE[E_TFTP_MODE_NUM] = {"netascii", "octet", "mail"};
/// @brief TFTP option strings
const char *TFTP_OPT[E_TFTP_OPT_NUM] = {"blksize", "timeout", "tsize"};

/// @brief TFTP error strings.
const char *TFTP_ERR_STRING[E_TFTP_ERR_NUM] =
    {
#define CALLISTO_TFTP_ERR(name, text) #text,
   TFTP_ERROR_LIST
#undef CALLISTO_TFTP_ERR

    };

/*******************************************************************************
 * Prototypes
 ******************************************************************************/

/** @brief Display the current IP address, subnet mask, and gateway address
 */
void ipaddress_show(void);

/** @brief Construct TFTP acknowledge packet.
 *
 * @param[in]  block    Block number to acknowledge
 * @param[out] packet   Array to accept formatted packet.
 * @param[in]  max_pkt  Length of packet
 *
 * @return length of packet constructed.
 */
static int TFTP_makePacketAck(uint16_t block, uint8_t *packet, size_t max_pkt);

/** @brief Construct TFTP data packet.
 *
 * @param[in]  block   Block number. This is the block number of this data packet
 * @param[in]  data    Pointer to data
 * @param[in]  len     Number of bytes to write
 * @param[out] packet  Array to accept formatted packet.
 * @param[in]  max_pkt Length of packet
 *
 * @return length of packet constructed.
 */
static int TFTP_makePacketData(uint16_t block, const uint8_t *data, size_t len, uint8_t *packet, size_t max_pkt);

/** @brief Construct TFTP error packet.
 *
 * @param[in]  code    Error code.
 * @param[in]  msg     Error string
 * @param[out] packet  Array to accept formatted packet.
 * @param[in]  max_pkt Length of packet
 *
 * @return length of packet constructed.
 */
static int TFTP_makePacketErr(tftp_err_t code, const char *msg, uint8_t *packet, size_t max_pkt);

/** @brief Construct TFTP read or write request packet.
 *
 * @param[in]  name    File name to read or write.
 * @param[in]  op      Read or write opcode
 * @param[in]  mode    Transfer mode
 * @param[out] packet  Array to accept formatted packet.
 * @param[in]  max_pkt Length of packet
 *
 * @return length of packet constructed.
 */
static int TFTP_makePacketRQ(const char *name, tftp_opcode_t op, tftp_mode_t mode, uint8_t *packet, size_t max_pkt);

/** @brief Parse TFTP Acknowledge packet.
 *
 * @param[in]  packet Pointer to data
 * @param[in]  len    Number of bytes in packet
 * @param[out] block  Block number acknowwledged.
 *
 * @return length of packet constructed.
 */
static int TFTP_parsePacketAck(const uint8_t *packet, size_t len, uint16_t *block);

/** @brief Parse TFTP data packet.
 *
 * The TFTP data packet contains the block number and up to the negotiated packet size bytes of data.
 * Packets may be repeated if an acknowledgement has not been received.
 *
 * \p block contains the last received block and is used to determine whether this packet contains new data.
 * If this packet contains the next block, the value pointed to is updated and \p pData is updated to the start of the new data.
 *
 * If this packet contains duplicate data, \p pData is set to NULL and the caller must
 *
 * The return value is always the length of data received so the retrieving loop can determine when the
 * transmission is complete.
 *
 * @param[in]    packet   Pointer to packet to parse
 * @param[in]    len      Number of bytes in packet
 * @param[inout] block    Block number. This contains the last block successfully received and is both input and output.
 * @param[out]   pData    Pointer to the start of data, or NULL.
 *
 * @return length of data received or -1 if this is an invalid packet.
 */
static int TFTP_parsePacketData(const uint8_t *packet, size_t len, uint16_t *block, const uint8_t **pData);

/** @brief Parse TFTP error request packet.
 *
 * @param[in]  packet   Array to accept formatted packet.
 * @param[in]  len      Number of bytes in packet
 * @param[out] errcode  Error code.
 * @param[out] pMsg     Pointer to error string.
 *
 * @return length of error string received.
 */
static int TFTP_parsePacketError(const uint8_t *packet, size_t len, tftp_err_t *errcode, char **pMsg);

/** @brief Parse TFTP Option acknowledgement packet.
 *
 * @param[in]  packet    Array to accept formatted packet.
 * @param[in]  len       Number of bytes in packet
 * @param[out] blocksize Blocksize received.
 * @param[out] timeout   Timout received
 * @param[out] totalsize Total data size received.
 *
 * @return length of packet constructed.
 */
static int TFTP_parsePacketOptAck(const uint8_t *packet, size_t len, int *blocksize, int *timeout, int *totalsize);

/*******************************************************************************
 * Variables
 ******************************************************************************/

static struct netif fsl_netif1;
static ip4_addr_t fsl_netif1_ipaddr;
static ip4_addr_t fsl_netif1_netmask;
static ip4_addr_t fsl_netif1_gw;

static char tftp_err_msg[TFTP_ERR_MAX]; ///<pre-allocated string for TFTP error msg string to send.

/*******************************************************************************
 * Code
 ******************************************************************************/

/*******************************************************************************
 * Interface Implementation
 ******************************************************************************/

/** The download function determines whether the address in \p addr is a location in RAM or on QSPI flash.
 *
 * If the location is in RAM, then the TFTP transfer will be made directly into the
 * memory starting at \p addr and \p alloc is the length already allocated at that address.
 * Data will overwrite anything that is already stored in this location.
 *
 * If the location is in QSPI flash, then \p addr specifies the location to receive the
 * data and \p alloc is the extent available. In this case, the region specified by these
 * two parameters will be erased and the data received written block-by-block.
 */
exit_code_t c4_tftpDownload(const uint8_t server[4], const char *name, char *addr, uint32_t alloc, tftp_mode_t mode)
{
    exit_code_t rc = C4_ERR_OK;
    bool qspi_addr = (((uint32_t)addr >= FlexSPI_AMBA_BASE) &&
                      (((uint32_t)addr + alloc) < (FlexSPI_AMBA_BASE + QSPI_FLASH_SIZE)));

    int sockfd;
    struct sockaddr_in servaddr, cliaddr;

    int blocksize = TFTP_DATA_MAX; ///< Negotiated blocksize

    if (qspi_addr)
    {
        C4_QSPI_erase((uintptr_t)addr, alloc);
    }

    // create socket to connect to TFTP server

    if (-1 == (sockfd = lwip_socket(PF_INET, SOCK_DGRAM, 0)))
            rc = C4_ERR_SOCKET;
    if (C4_ERR_OK == rc)
    {
        // Set up structure for local client address
        memset(&cliaddr, 0, sizeof(cliaddr));
        cliaddr.sin_family = AF_INET;
        inet_pton(AF_INET, "169.254.154.150", &cliaddr.sin_addr);
        cliaddr.sin_port = 0;
        if (-1 == lwip_bind(sockfd, (struct sockaddr  *)&cliaddr, sizeof(cliaddr)))
            rc = C4_ERR_SOCKET;

        // Set up structure for remote server address
        memset(&servaddr, 0, sizeof(servaddr));
        servaddr.sin_family = AF_INET;
        inet_pton(AF_INET, "169.254.154.157", &servaddr.sin_addr);
        servaddr.sin_port = htons(TFTP_PORT);

    }

    uint8_t tftp_msg[TFTP_PACKET_MAX];
    int tx_len, rx_len;
    if (C4_ERR_OK == rc)
    {
        // Send request
        tx_len = TFTP_makePacketRQ(name, E_TFTP_OP_RRQ, mode, tftp_msg, TFTP_PACKET_MAX);
        if (-1 == lwip_sendto(sockfd, tftp_msg, tx_len, 0, (struct sockaddr *)&servaddr, sizeof(servaddr)))
            rc = C4_ERR_SOCKET;

        // Clear port in structure for receive server address
        servaddr.sin_port = 0;
    }
    socklen_t len = sizeof(servaddr);
    int offset = 0;
    uint16_t block = 0;
    bool data_left = true;
    // Loop to retrieve packets
    do
    {
        int data_len = 0;
        const uint8_t *pData = '\0';
        tftp_err_t tftp_err = E_TFTP_ERR_NUM;
        *tftp_err_msg = '\0';

        rx_len = lwip_recvfrom(sockfd, tftp_msg, 1024, 0, (struct sockaddr *)&servaddr, &len);
        switch(ntohs(*(uint16_t *)tftp_msg))
        {
        case E_TFTP_OP_DATA:
            data_len = TFTP_parsePacketData(tftp_msg, rx_len, &block, &pData);
            if (data_len < 0)
            {
                tftp_err = E_TFTP_ERR_ILLEGAL;
            }
            else
            {
                // Send Ack
                tx_len = TFTP_makePacketAck(block, tftp_msg, TFTP_PACKET_MAX);
                if (-1 == lwip_sendto(sockfd, tftp_msg, tx_len, 0, (struct sockaddr *)&servaddr, sizeof(servaddr)))
                    rc = C4_ERR_SOCKET;

                // test whether the pointer has been set to a non-NULL value.
                if (pData)
                {
                    if (qspi_addr)
                    {
                        if (offset < alloc)
                        {
                            if ((offset + data_len) > alloc)
                            {
                                C4_QSPI_write((uintptr_t)addr + offset, pData, alloc - offset);
                                offset = alloc;
                            }
                            else
                            {
                                C4_QSPI_write((uintptr_t)addr+offset, pData, data_len);
                                offset += data_len;
                            }
                        }
                    }
                    else
                    {
                        if (offset < alloc)
                        {
                            if ((offset + data_len) > alloc)
                            {
                                memcpy(addr + offset, pData, alloc - offset);
                                offset = alloc;
                            }
                            else
                            {
                                memcpy(addr+offset, pData, data_len);
                                offset += data_len;
                            }
                        }
                    }
                }

                if (TFTP_DATA_MAX > rx_len)
                {
                    data_left = false;
                }

                break;
            }
        case E_TFTP_OP_OACK:
        {
            int timeout = -1;
            int totalsize = -1;
            data_len = TFTP_parsePacketOptAck(tftp_msg, rx_len, &blocksize, &timeout, &totalsize);
            if (data_len < 0)
            {
                tftp_err = E_TFTP_ERR_ILLEGAL;
            }
            else
            {
                /* We have specified a timeout and the server has responded.
                 * Not Implemented
                 */
                /* We have specified a blocksize and the server has responded.
                 * The specification says that this value is no larger than our request.
                 */
                /* We requested the total file size and the server has responded. */
                if (totalsize > alloc)
                {
                    /* The file is too big for our buffer.
                     * Send an error.
                     */
                    TFTP_makePacketErr(E_TFTP_ERR_ALLOC, "File exceeds allocation", tftp_msg, TFTP_PACKET_MAX);
                }
                /* Send a block 0 ack. */
                tx_len = TFTP_makePacketAck(0, tftp_msg, TFTP_PACKET_MAX);
                if (-1 == lwip_sendto(sockfd, tftp_msg, tx_len, 0, (struct sockaddr *)&servaddr, sizeof(servaddr)))
                    rc = C4_ERR_SOCKET;
            }

            break;
        }
        case E_TFTP_OP_ERROR:
        {
            data_len = TFTP_parsePacketError(tftp_msg, rx_len, &tftp_err, (char **)&pData);
            if (data_len < 0)
            {
                tftp_err = E_TFTP_ERR_ILLEGAL;
            }
            else
            {
                printf("RX TFTP Err %s (%d) \"%s\"\n", TFTP_ERR_STRING[tftp_err], tftp_err,
                        ((pData) ? (const char *)pData : "No message"));
            }

            break;
        }
        case E_TFTP_OP_ACK:
        {
            data_len = TFTP_parsePacketAck(tftp_msg, rx_len, &block);
            if (data_len < 0)
            {
                tftp_err = E_TFTP_ERR_ILLEGAL;
            }

            break;
        }
        case E_TFTP_OP_RRQ:
        case E_TFTP_OP_WRQ:
        default:
            rc = C4_ERR_NOT_IMPLEMENTED;
        }

        if (tftp_err < E_TFTP_ERR_NUM)
        {

        }
    } while (data_left);
    return rc;
}

/* ========================================================================= */

exit_code_t c4_stack_init(const uint8_t au8MACAddress[6],
        const uint8_t au8StaticIPAddress[4],
        const uint8_t au8StaticNetMask[4],
        const uint8_t au8StaticGateway[4])
{
    exit_code_t rc = C4_ERR_OK;

    ethernetif_config_t fsl_enet_config1 = {
        .clockName  = EXAMPLE_CLOCK_NAME,
        .macAddress =
        {
            au8MACAddress[0],
            au8MACAddress[1],
            au8MACAddress[2],
            au8MACAddress[3],
            au8MACAddress[4],
            au8MACAddress[5]
        }
    };
#if LWIP_DHCP
    int err;
#endif

    // Start tcpip_thread (lwIP) task. (Also calls lwip_init() etc.)
    tcpip_init(NULL, NULL);

//    if (u8UseDHCP == 0)
    {
        IP4_ADDR(&fsl_netif1_ipaddr,
                au8StaticIPAddress[0],
                au8StaticIPAddress[1],
                au8StaticIPAddress[2],
                au8StaticIPAddress[3]);
        IP4_ADDR(&fsl_netif1_netmask,
                au8StaticNetMask[0],
                au8StaticNetMask[1],
                au8StaticNetMask[2],
                au8StaticNetMask[3]);
        IP4_ADDR(&fsl_netif1_gw,
                au8StaticGateway[0],
                au8StaticGateway[1],
                au8StaticGateway[2],
                au8StaticGateway[3]);
    }

    netif_add(&fsl_netif1, &fsl_netif1_ipaddr, &fsl_netif1_netmask, &fsl_netif1_gw,
              &fsl_enet_config1, ethernetif1_init, tcpip_input);
/*
#if LWIP_NETIF_HOSTNAME
    netif_set_hostname(&fsl_netif1, (const char *) achHostName);
#endif
*/
#if LWIP_IPV6
    netif_create_ip6_linklocal_address(&fsl_netif1, 1);    // LWIP_IPV6
//    netif_ip6_addr_set_state((&EMAC_if), 0, IP6_ADDR_TENTATIVE);    LWIP_IPV6 Don't think I need to do this...
#endif

    netif_set_default(&fsl_netif1);

// Note: according to https://www.nongnu.org/lwip/2_1_x/sys_init.html ...
// ...we should add the following call in here: (NickP)
//    netif_set_link_up(&fsl_netif1);

    netif_set_up(&fsl_netif1);
/*
    if (u8UseDHCP != 0)
    {
#if LWIP_DHCP
        err = dhcp_start(&fsl_netif1);
        LWIP_PLATFORM_DIAG(( "dhcp_start...........[result=%d]\r\n", err ));
#else
        LWIP_PLATFORM_DIAG(( "Error: DHCP enabled but not compiled in\r\n" ));
#endif
    }
*/
    printf("\r\n************************************************\r\n");

    printf(" MAC Address      : %02X:%02X:%02X:%02X:%02X:%02X\r\n",
                (int) fsl_enet_config1.macAddress[0],
                (int) fsl_enet_config1.macAddress[1],
                (int) fsl_enet_config1.macAddress[2],
                (int) fsl_enet_config1.macAddress[3],
                (int) fsl_enet_config1.macAddress[4],
                (int) fsl_enet_config1.macAddress[5]);

//    if (u8UseDHCP == 0)
    {
        // No DHCP, so display static IP address
        ipaddress_show();
    }

#if LWIP_IPV6
    printf(" IPv6 Link Local Address : %s\r\n", ip6addr_ntoa(&(fsl_netif1.ip6_addr[0].u_addr.ip6)) );
#endif
    printf("************************************************\r\n");

#if STACK_INIT_USE_TASK
    vTaskDelete(NULL);
#endif

    return rc;
}

/*******************************************************************************
 * Local Functions
 ******************************************************************************/

static int TFTP_makePacketAck(uint16_t block, uint8_t *packet, size_t max_pkt)
{
    // check that the packet is long enough
    if ((4) > max_pkt)
        return 0;
    *(uint32_t *)packet = htons((uint16_t)E_TFTP_OP_ACK) | (htons(block)<<16);
    return  4;
}

/*****************************************************************************/

static int TFTP_makePacketData(uint16_t block, const uint8_t *data, size_t len, uint8_t *packet, size_t max_pkt)
{

    return -1;
}

/*****************************************************************************/

static int TFTP_makePacketErr(tftp_err_t code, const char *msg, uint8_t *packet, size_t max_pkt)
{
    // check that the packet is long enough - allow extra 6 for maximum blocksize (65535) and 2 to send
    // tsize request (0)
    if ((5 + strlen(msg)) > max_pkt)
        return 0;


    char *ptr = (char *)packet;
    *(uint16_t *)ptr = htons((uint16_t)E_TFTP_OP_ERROR);
    ptr +=2;
    *(uint16_t *)ptr = htons((uint16_t)code);
    ptr +=2;
    ptr += sprintf(ptr, "%s", msg) + 1;

    return ((uint8_t *)ptr - packet) + 1;
}

/*****************************************************************************/

static int TFTP_makePacketRQ(const char *name, tftp_opcode_t op, tftp_mode_t mode, uint8_t *packet, size_t max_pkt)
{


    // check that the packet is long enough
    // Add one to the lengths to allow for the terminating null for each string
    // - allow extra 6 for maximum blocksize (65535) and 2 to send
    // tsize request (0)
    if ((2 + strlen(name) + 1 +
            strlen(TFTP_MODE[mode]) + 1 +
            strlen(TFTP_OPT[E_TFTP_OPT_BLKSIZE]) + 7 +
            strlen(TFTP_OPT[E_TFTP_OPT_TSIZE]) + 3) > max_pkt)
        return 0;

    char *ptr = (char *)packet;
    *(uint16_t *)ptr = htons((uint16_t)(op));
    ptr +=2;
    ptr += sprintf(ptr, "%s", name) + 1;
    ptr += sprintf(ptr, "%s", TFTP_MODE[mode]) + 1;
    ptr += sprintf(ptr, "%s", TFTP_OPT[E_TFTP_OPT_BLKSIZE]) + 1;
    ptr += sprintf(ptr, "%d", TFTP_DATA_MAX) + 1 ;
    ptr += sprintf(ptr, "%s", TFTP_OPT[E_TFTP_OPT_TSIZE]) + 1;
    ptr += sprintf(ptr, "0") + 1;

    return ((uint8_t *)ptr - packet) + 1;
}

/*****************************************************************************/

static int TFTP_parsePacketAck(const uint8_t *data, size_t len, uint16_t *block)
{
    int rc = -1;
    if (4 == len)
    {
        *block = ntohs(*(uint16_t *)(data+2));
        rc = 0;
    }
    return rc;
}

/*****************************************************************************/

static int TFTP_parsePacketData(const uint8_t *packet, size_t len, uint16_t *block,  const uint8_t **pDest)
{
    int rc = -1;
    if (4 <= len)
    {
        uint16_t rx_block = ntohs(*(uint16_t *)(packet+2));
        if(rx_block == *block + 1)
        {
            ++*block;
            *pDest = packet + 4;
            return (len - 4);
        }
        else if (rx_block == *block)
        {
            /* Don't copy duplicate block but return length of data. */
            *pDest = NULL;
            rc = (len - 4);
        }
    }
    return rc;
}

/*****************************************************************************/

static int TFTP_parsePacketError(const uint8_t *packet, size_t len, tftp_err_t *errcode, char **pMsg)
{
    int rc = -1;
    if (5 <= len)
    {
        *errcode = ntohs(*(uint16_t *)(packet+2));
        *pMsg = (char *)(packet + 4);
        rc = len - 4;
    }
    return rc;
}

/*****************************************************************************/

static int TFTP_parsePacketOptAck(const uint8_t *packet, size_t len, int *blocksize, int *timeout, int *totalsize)
{
    int rc = -1;
    if (4 <= len)
    {
        const char *ptr = (char *)packet+2;
        while (ptr < (char *)(packet + len - 1))
        {
            int opt_len = strlen(ptr) + 1;
            int opt_val;
            int val_len = strlen(ptr + opt_len) + 1;
            sscanf((ptr + opt_len), "%d", &opt_val);
            if (0 == strcmp(ptr, TFTP_OPT[E_TFTP_OPT_BLKSIZE]))
            {
                *blocksize = opt_val;
            }
            else if (0 == strcmp(ptr, TFTP_OPT[E_TFTP_OPT_TIMEOUT]))
            {
                *timeout = opt_val;
            }
            else if (0 == strcmp(ptr, TFTP_OPT[E_TFTP_OPT_TSIZE]))
            {
                *totalsize = opt_val;
            }
            ptr += opt_len + val_len;
        }
        rc = 0;
    }
    return rc;
}
