Diff: udp/tftp.c

Revision:

61:aad055f1b0d1

Parent:

59:e0e556c8bd46

Child:

65:37acccf2752f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/udp/tftp.c	Thu Jan 11 17:38:21 2018 +0000
@@ -0,0 +1,313 @@
+#include <stdint.h>
+#include <stdbool.h>
+
+#include     "log.h"
+#include     "net.h"
+#include  "action.h"
+#include "ip6addr.h"
+#include     "udp.h"
+#include     "ar4.h"
+#include     "ar6.h"
+#include     "arp.h"
+#include     "eth.h"
+#include     "nr4.h"
+#include     "nr6.h"
+#include     "dns.h"
+#include     "mac.h"
+#include    "tftp.h"
+#include   "clock.h"
+
+bool TftpTrace = false;
+
+#define WRITE_TIMEOUT_SECS 5
+
+#define TFTP_RRQ   1
+#define TFTP_WRQ   2
+#define TFTP_DATA  3
+#define TFTP_ACK   4
+#define TFTP_ERROR 5
+
+static void logOp(char* p)
+{
+    if (*p)
+    {
+        LogF("Unknown op code %02x%02x", *p, *++p);
+        return;
+    }
+    p++;
+    switch (*p)
+    {
+        case TFTP_RRQ:   Log ("RRQ")                         ; break;
+        case TFTP_WRQ:   Log ("WRQ")                         ; break;
+        case TFTP_DATA:  Log ("DATA")                        ; break;
+        case TFTP_ACK:   Log ("ACK")                         ; break;
+        case TFTP_ERROR: Log ("ERROR")                       ; break;
+        default:         LogF("Unknown op code 00%02x", *p); break;
+    }
+}
+
+static void logError(char* p)
+{
+    if (*p)
+    {
+        LogF("Unknown error code %02x%02x", *p, *++p);
+        return;
+    }
+    p++;
+    switch (*p)
+    {
+        case 0:  Log ("Not defined, see error message."  ); break;
+        case 1:  Log ("File not found."                  ); break;
+        case 2:  Log ("Access violation."                ); break;
+        case 3:  Log ("Disk full or allocation exceeded."); break;
+        case 4:  Log ("Illegal TFTP operation."          ); break;
+        case 5:  Log ("Unknown transfer ID."             ); break;
+        case 6:  Log ("File already exists."             ); break;
+        case 7:  Log ("No such user."                    ); break;
+        default: LogF("Unknown error code 00%02x", *p    ); break;
+    }
+}
+uint32_t   TftpServerIp4;
+char       TftpServerIp6[16];
+int        TftpWriteStatus = TFTP_WRITE_STATUS_NONE;
+char       TftpServerName[DNS_MAX_LABEL_LENGTH+1];
+char       TftpFileName  [DNS_MAX_LABEL_LENGTH+1];
+
+int      (*TftpGetNextByteFunction)();
+
+static int   size;
+
+static void logHeader(char* p)
+{
+    if (NetTraceVerbose)
+    {
+        Log ("TFTP header\r\n");
+        Log ("  Op code "); logOp(p);         Log("\r\n");
+        Log ("  Size    "); LogF("%d", size); Log("\r\n");
+    }
+    else
+    {
+        Log ("TFTP  header");
+        Log (": Op "); logOp(p);
+        LogF(", %d bytes", size);
+        Log ("\r\n");
+    }
+}
+static int sendBlock(uint16_t block, char* pHeader)
+{
+    /*2 bytes    2 bytes     n bytes
+     ----------------------------------
+    | Opcode |   Block #  |   Data     |
+     ---------------------------------- */
+     char* p = pHeader;
+    *p++ = 0;
+    *p++ = TFTP_DATA;
+    *p++ = block >> 8;
+    *p++ = block & 0xFF;
+    
+    int len = 0;
+    while (len < 512)
+    {
+        int c = TftpGetNextByteFunction();
+        if (c == -1) break;
+        *p++ = c;
+        len++;
+    }
+    if (len < 512) TftpWriteStatus = TFTP_WRITE_STATUS_NONE;
+    size = p - pHeader;
+    return UNICAST_TFTP;
+}
+static void handleError(char* p)
+{
+     /*2 bytes   2 bytes      string    1 byte
+     -----------------------------------------
+    |  ERROR |  ErrorCode |   ErrMsg   |   0  |
+     ----------------------------------------- */
+    p += 2; //Skip the op code which we already know
+    if (NetTraceNewLine) Log("\r\n");
+    LogTime("TFTP error - ");
+    logError(p); p += 2;
+    Log(p);
+    Log("\r\n");
+    TftpWriteStatus = TFTP_WRITE_STATUS_NONE;
+}
+static int handleAck(char* pHeaderRx, char* pHeaderTx)
+{
+    /* 2 bytes    2 bytes
+     -----------------------
+    |   ACK    |   Block #  |
+     ----------------------- */
+    char* p = pHeaderRx;
+    p += 2; //Skip the op code which we already know
+    uint16_t block = *p++;
+    block <<= 8;
+    block += *p++;
+    
+    return sendBlock(block + 1, pHeaderTx);
+}
+static int sendRequest(char* pHeader)
+{
+    /*2 bytes    string   1 byte     string   1 byte
+     -----------------------------------------------
+    |   WRQ  |  Filename  |   0  |    Mode    |  0  |
+     ----------------------------------------------- */
+     char* p = pHeader;
+    *p++ = 0;
+    *p++ = TFTP_WRQ;
+    char* pName = TftpFileName;
+    while (*pName) *p++ = *pName++;
+    *p++ = 0;
+    const char* pMode = "octet";
+    while (*pMode) *p++ = *pMode++;
+    *p++ = 0;
+    size = p - pHeader;
+    return UNICAST_TFTP;
+}
+static void (*pTraceBack)(void);
+int TftpHandlePacketReceived(void (*traceback)(void), int sizeRx, void * pPacketRx, int* pSizeTx, void* pPacketTx)
+{
+    pTraceBack = traceback;
+    char* pHeaderRx = (char*)pPacketRx;
+    char* pHeaderTx = (char*)pPacketTx;
+    size = sizeRx;
+    
+    char* p = pHeaderRx;
+    
+    if (*p)
+    {
+        LogTimeF("Expected high byte of op code to be zero not %u\r\n", *p);
+        return DO_NOTHING;
+    }
+    if (TftpTrace)
+    {
+        if (NetTraceNewLine) Log("\r\n");
+        LogTimeF("TFTP received packet\r\n");
+        if (NetTraceStack) pTraceBack();
+        logHeader(pHeaderRx);
+    }
+    p++;
+    int dest = DO_NOTHING;
+    switch (*p)
+    {
+        case TFTP_ACK:
+            if (TftpWriteStatus == TFTP_WRITE_STATUS_NONE) return DO_NOTHING;
+            dest = handleAck(pHeaderRx, pHeaderTx);
+            break;
+        case TFTP_ERROR:
+            handleError(pHeaderRx); 
+            return DO_NOTHING;
+        default:
+            LogTimeF("\r\nTFTP packet unknown mode %d\r\n", *p);
+            return DO_NOTHING;
+    }
+
+    if (TftpTrace) logHeader(pHeaderTx);
+
+    *pSizeTx = size;
+    return ActionMakeFromDestAndTrace(dest, TftpTrace && NetTraceStack);
+}
+static bool resolve4(char* server, uint32_t* pIp)
+{
+    //Check if have IP, if not, then request it and stop
+    Nr4NameToIp(server, pIp);
+    if (!*pIp)
+    {
+        Nr4MakeRequestForIpFromName(server); //The request is only repeated if made after a freeze time - call as often as you want.
+        return false;
+    }
+
+    //Check if have MAC and, if not, request it and stop
+    char mac[6];
+    Ar4IpToMac(*pIp, mac);
+    if (MacIsEmpty(mac))
+    {
+        Ar4MakeRequestForMacFromIp(*pIp); //The request is only repeated if made after a freeze time - call as often as you want.
+        return false;
+    }
+    
+    return true;
+}
+static bool resolve6(char* server, char* ip)
+{
+    //Check if have IP, if not, then request it and stop
+    Nr6NameToIp(server, ip);
+    if (Ip6AddressIsEmpty(ip))
+    {
+        Nr6MakeRequestForIpFromName(server); //The request is only repeated if made after a freeze time - call as often as you want.
+        return false;
+    }
+
+    //Check if have MAC and, if not, request it and stop
+    char mac[6];
+    Ar6IpToMac(ip, mac);
+    if (MacIsEmpty(mac))
+    {
+        Ar6MakeRequestForMacFromIp(ip); //The request is only repeated if made after a freeze time - call as often as you want.
+        return false;
+    }
+    
+    return true;
+}
+int TftpPollForPacketToSend(int type, void* pPacket, int* pSize)
+{
+    if (TftpWriteStatus != TFTP_WRITE_STATUS_REQUEST) return DO_NOTHING; //Wait until a request to send a file
+    
+    char* pHeader = (char*)pPacket;
+    size = *pSize;
+
+    if (!TftpServerName[0])
+    {
+        LogTimeF("TftpPollForRequestToSend - A request to send a client message has been made but no server name has been specified\r\n");
+        TftpWriteStatus = TFTP_WRITE_STATUS_NONE;
+        return DO_NOTHING;
+    }
+    
+    if (!TftpGetNextByteFunction)
+    {
+        LogTimeF("TftpPollForRequestToSend - A request to send a client message has been made but TFTP has not been plumbed into a file stream\r\n");
+        TftpWriteStatus = TFTP_WRITE_STATUS_NONE;
+        return DO_NOTHING;
+    }
+    
+    if (type == IPV4)
+    {
+        if (!resolve4(TftpServerName, &TftpServerIp4)) return DO_NOTHING;
+    }
+    else if (type == IPV6)
+    {
+        if (!resolve6(TftpServerName, TftpServerIp6)) return DO_NOTHING;
+    }
+    else
+    {
+        return DO_NOTHING;
+    }
+
+    //Have IP and MAC so send request
+    TftpWriteStatus = TFTP_WRITE_STATUS_IN_PROGRESS;
+    int dest = sendRequest(pHeader);
+    if (TftpTrace)
+    {
+        if (NetTraceNewLine) Log("\r\n");
+        LogTimeF("TFTP Sending request\r\n");
+        logHeader(pHeader);
+    }
+    *pSize = size;
+    return ActionMakeFromDestAndTrace(dest, TftpTrace && NetTraceStack);
+}
+int elapsed = 0;
+void TftpMain()
+{
+    if (TftpWriteStatus == TFTP_WRITE_STATUS_IN_PROGRESS)
+    {
+        if (ClockTicked) elapsed++;
+        if (elapsed > WRITE_TIMEOUT_SECS)
+        {
+            TftpWriteStatus = TFTP_WRITE_STATUS_NONE;
+            LogTime("TFTP - write operation timed out so reset\r\n");
+        }
+    }
+    else
+    {
+        elapsed = 0;
+    }
+}
\ No newline at end of file