Diff: Codes/tftpsrv.cpp

Revision:

0:fac116e94d44

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Codes/tftpsrv.cpp	Tue Jan 05 11:47:35 2016 +0000
@@ -0,0 +1,301 @@
+#include "tftpsrv.h"
+#include <ctype.h>
+
+void tftpsrv::tftpsrv_Thread(void const* arg)
+{
+    printf("\r\nTFTP start...\r\n");
+
+    tftpInicia();    
+
+    int tam = 0;
+    char buffer[516];
+
+    int instancias = 10;
+    while (instancias > 0) {
+        // DEBUG ("\r\nEsperando pacote...\r\n");
+        tam = server.receiveFrom(client, buffer, sizeof(buffer));
+        while ( tam > 0 ) {
+            // DEBUG ("\r\nPacote recebido...\r\n");
+            switch (estado) {
+                case ESCUTANDO:
+                    tftpEscutando(buffer);
+                    break;
+                case LENDO:
+                    tftpLendo(buffer);
+                    break;
+                case ESCREVENDO:
+                    tftpEscrevendo(tam, buffer);
+                    break;
+                default:
+                    tftpErr("Desconhecido ou nao implementado");
+            }
+            // DEBUG ("\r\nEsperando pacote...\r\n");
+            tam = server.receiveFrom(client, buffer, sizeof(buffer));
+        }
+        tftpReinicia();
+        instancias--;
+        printf("\r\nRestam %d instancias...\r\n", instancias);
+        //DEBUG ("\r\nRestam %d instancias...\r\n", instancias);
+    }
+    printf("Caiu fora TFTP\n");
+
+    // DEBUG ("\r\nTFTP end...\r\n");
+}
+
+
+
+void tftpsrv::tftpInicia()
+{
+    // DEBUG ("\r\nIniciando TFTP...\r\n");
+    //server.init();
+    server.bind(69);
+    estado = ESCUTANDO;
+    server.set_blocking(false, 20000);
+}
+
+void tftpsrv::tftpPara()
+{
+    // DEBUG ("\r\nParando TFTP...\r\n");
+    server.close();
+    if (fp!=NULL) fclose(fp);
+}
+
+void tftpsrv::tftpReinicia()
+{
+    tftpPara();
+    tftpInicia();
+}
+
+// send ACK to remote
+void tftpsrv::tftpAck(int val)
+{
+    char ack[4];
+    ack[0] = 0x00;
+    ack[1] = 0x04;
+    if ((val>603135) || (val<0)) val = 0;
+    ack[2] = val >> 8;
+    ack[3] = val & 255;
+
+    // DEBUG ("\r\nEnviando ACK...\r\n");
+    server.sendTo(client, ack, 4);
+}
+
+// send ERR message to named client
+void tftpsrv::tftpErr(char* msg)
+{
+    char message[32];
+    strncpy(message, msg, 32);
+    char err[37];
+    sprintf(err, "0000%s0", message);
+    err[0] = 0x00;
+    err[1] = 0x05;
+    err[2]=0x00;
+    err[3]=0x00;
+    int len = strlen(err);
+    err[len-1] = 0x00;
+    // DEBUG ("\r\nEnviando ERR ( %s )...\r\n", message);
+    server.sendTo(client, err, len);
+}
+
+int tftpsrv::tftpModo(char* buff)
+{
+    int x = 2;
+    while (buff[x++] != 0); // get beginning of mode field
+    int y = x;
+    while (buff[y] != 0) {
+        buff[y] = tolower(buff[y]);
+        y++;
+    } // make mode field lowercase
+    return (strcmp(&buff[x++], "octet") == 0);
+}
+
+// get DATA block from file on disk into memory
+void tftpsrv::tftpGetBlock()
+{
+    // DEBUG ("\r\nLendo bloco... (em %d)\r\n", blockcnt);
+    blockcnt++;
+    char *p;
+    p = &sendbuff[4];
+    int len = fread(p, 1, 512, fp);
+    sendbuff[0] = 0x00;
+    sendbuff[1] = 0x03;
+    sendbuff[2] = blockcnt >> 8;
+    sendbuff[3] = blockcnt & 255;
+    blocksize = len+4;
+}
+
+// send DATA block to the client
+void tftpsrv::tftpSendBlock()
+{
+    // DEBUG ("\r\nEnviando bloco ( tam = %d )...\r\n", blocksize);
+    server.sendTo(client, sendbuff, blocksize);
+}
+
+
+void tftpsrv::tftpLer(char* buff)
+{
+    tftpAck(0);
+
+    blockcnt = 0;
+    dupcnt = 0;
+
+    sprintf(filename, "/local/%s", &buff[2]);
+
+    fp = fopen(filename, "rb");
+    if (fp == NULL) {
+        estado  = ESCUTANDO;
+        tftpErr("Could not read file");
+    } else {
+        // file ready for reading
+        estado = LENDO;
+        // DEBUG ("\r\nRequisitado arquivo %s para %s\r\n", filename, client.get_address());
+        tftpGetBlock();
+        tftpSendBlock();
+    }
+}
+
+void tftpsrv::tftpEscrever(char* buff)
+{
+    struct dirent *p;
+    DIR *dir = opendir("/local");
+    while ((p = readdir(dir)) != NULL) {
+        char *str = p->d_name;
+        if ((strstr(str, ".bin") != NULL) || (strstr(str, ".BIN") != NULL)) {
+            char buf[BUFSIZ];
+            snprintf(buf, sizeof(buf) - 1, "/local/%s", str);
+            remove(buf) == 0;
+        }
+    }
+    closedir(dir);
+    
+    tftpAck(0);
+    blockcnt = 0;
+    dupcnt = 0;
+
+    sprintf(filename, "/local/%s", &buff[2]);
+
+    fp = fopen(filename, "ab");
+    if (fp == NULL) {
+        tftpErr("Could not open file to write");
+        estado = ESCUTANDO;
+    } else {
+        // file ready for writing
+        fseek(fp, 0, SEEK_SET);
+        blockcnt = 0;
+        estado = ESCREVENDO;
+        // DEBUG ("\r\nRecebendo arquivo %s de %s\r\n", filename, client.get_address());
+    }
+}
+
+void tftpsrv::tftpEscutando(char* buff)
+{
+    // DEBUG ("\r\nEscutando...\r\n");
+    switch (buff[1]) {
+        case 0x01: // RRQ
+            if (tftpModo(buff))
+                tftpLer(buff);
+            else
+                tftpErr("Not in octet mode");
+            break;
+        case 0x02: // WRQ
+            if (tftpModo(buff))
+                tftpEscrever(buff);
+            else
+                tftpErr("Not in octet mode");
+            break;
+        case 0x03: // DATA before connection established
+            tftpErr("No data expected");
+            break;
+        case 0x04:  // ACK before connection established
+            tftpErr("No ack expected");
+            break;
+        case 0x05: // ERROR packet received
+            // DEBUG ("TFTP Eror received\n\r");
+            break;
+        default:    // unknown TFTP packet type
+            tftpErr("Unknown TFTP packet type");
+            break;
+    } // switch buff[1]
+}
+
+void tftpsrv::tftpLendo(char *buff)
+{
+    // DEBUG ("\r\nLendo...\r\n");
+    switch (buff[1]) {
+        case 0x01:
+            // to-do: verificar host e ip
+            break;
+        case 0x02:
+            // this should never happen, ignore
+            break; // case 0x02
+        case 0x03:
+            // we are the sending side, ignore
+            break;
+        case 0x04:
+            // last packet received, send next if there is one
+            dupcnt = 0;
+            if (blocksize == 516) {
+                tftpGetBlock();
+                tftpSendBlock();
+            } else { //EOF
+                // DEBUG ("\r\nFim de arquivo\r\n");
+                fclose(fp);
+                estado = ESCUTANDO;
+            }
+            break;
+        default:  // this includes 0x05 errors
+            fclose(fp);
+            estado = ESCUTANDO;
+            break;
+    } // switch (buff[1])
+}
+
+void tftpsrv::tftpEscrevendo(int tam, char *buff)
+{
+    // DEBUG ("\r\nEscrevendo...\r\n");
+
+    int block;
+    switch (buff[1]) {
+        case 0x02:
+            // a fazer: verificar host, ip e porta
+            break; // case 0x02
+        case 0x03:
+            block = (buff[2] << 8) + buff[3];
+            if ((blockcnt+1) == block) {
+                tftpAck(block);
+                // new packet
+                char *data = &buff[4];
+                fwrite(data, 1,tam-4, fp);
+                blockcnt++;
+                dupcnt = 0;
+            } else {
+                if ((blockcnt+1) < block) { // high block nr
+                    // we missed a packet, error
+                    // DEBUG ("\r\nMissed packet!\r\n");
+                    fclose(fp);
+                    estado = ESCUTANDO;
+                } else { // duplicate packet, do nothing
+                    // DEBUG ("\r\nPacote duplicado ( %d )\r\n", blockcnt);
+                    if (dupcnt > 10) {
+                        tftpErr("Too many dups");
+                        fclose(fp);
+                        estado = ESCUTANDO;
+                    } else {
+                        tftpAck(blockcnt);
+                    }
+                    dupcnt++;
+                }
+            }
+            // DEBUG  ("\r\nLendo pacote %d com blocksize = %d\n\r", blockcnt, tam);
+            if (tam<516) {
+                // DEBUG ("\r\nFim do arquivo -> %d\r\n", tam);
+                fflush(fp);
+                fclose(fp);
+                estado = ESCUTANDO;
+            }
+            break; // case 0x03
+        default:
+            tftpErr("No idea why you're sending me this!");
+            break; // default
+    } // switch (buff[1])
+}