tools/BCLSockServer/mt_tftp.c

   1 /******************************************************************************
   2
   3                                Copyright (c) 2012
   4                             Lantiq Deutschland GmbH
   5
   6   For licensing information, see the file 'LICENSE' in the root folder of
   7   this software module.
   8
   9 ******************************************************************************/
  10 /*****************************************************************************
  11 *       MT_TFTP.C
  12 *
  13 *       TFTP server as part of the BCP project
  14 *   Currently supported only in linux
  15 *       Author: Ron Vainapel, 2008
  16 *
  17 *****************************************************************************/
  18 #include "mtlkinc.h"
  19 #include <stdio.h>
  20 #include <string.h>
  21 #include <unistd.h>
  22 #include <sys/socket.h>
  23 #include <sys/ioctl.h>
  24 #include <sys/stat.h>
  25 #include <netinet/in.h>
  26 #include <pthread.h>
  27 #include <stdlib.h>
  28
  29 #include "mt_cnfg.h"    // MTLK specific definitions
  30 #include "BCLSockServer.h"
  31
  32 extern int BCL_nExit; /* if this variable sets to 1, BCL will exit */
  33 extern unsigned int BCL_numOfThreads; /* the number of threads in the system */
  34 int BCL_areWeLittleEndian; // TRUE when we are running on little endian system
  35
  36 #define TFTP_PORT 69
  37 #define SOCKET_INVALID -1
  38 #define TFTP_BUF_SIZE 576
  39 #define TFTP_MAX_FILE_BLOCK_SIZE 512
  40 #define TFTP_TEMP_WRITE_FILE "/tmp/tftp_temp_file"
  41
  42 typedef struct TFTP_t
  43 {
  44         int                     sock;
  45         MT_UBYTE*       inBuf;
  46         int                     inBufLen;
  47         MT_UBYTE*       outBuf;
  48         MT_UBYTE*       filename;
  49         FILE*           pFile;
  50         int                     outBufLen;
  51         int                     blockNum;
  52         int                     lastBlockSize;
  53         int                     isSendingFile; // 1 for send, 0 for get
  54         struct sockaddr_in theiraddr_in;     /* for local socket address */
  55
  56 } TFTP_t;
  57
  58 #define TFTP_CLOSEFILE if (TFTP->pFile) { fclose(TFTP->pFile); TFTP->pFile = 0; }
  59
  60 enum OpCodes_t
  61 {
  62         readRequestCode_e = 1,
  63         writeRequestCode_e,
  64         dataBlockCode_e,
  65         ackCode_e,
  66         errorCode_e,
  67 };
  68
  69 enum errorCodes_t
  70 {
  71         fileNotFound_e = 1,
  72         fileAccessViolation_e
  73 };
  74
  75 void* TFTP_Error(TFTP_t* TFTP, const char* errStr)
  76 {
  77         printf(errStr);
  78         if (TFTP->sock != SOCKET_INVALID) close(TFTP->sock);
  79         BCL_numOfThreads--;
  80         BCL_nExit = 1;
  81         return MT_THREAD_RET;
  82 }
  83
  84 static MT_UINT16 getRevWORD(MT_UINT16 word)
  85 {
  86         if (BCL_areWeLittleEndian) return ((word & 0x00FF) << 8) | ((word & 0xFF00) >> 8);
  87         else return word; // WE ARE ALREADY IN A BIG-ENDIAN CPU !
  88 }
  89
  90 static void putRevWORD(MT_UBYTE* buf, MT_UINT16 word)
  91 {
  92         *((MT_UINT16*)buf) = getRevWORD(word);
  93 }
  94
  95 void TFTP_SendBuffer(TFTP_t* TFTP)
  96 {
  97         sendto(TFTP->sock, (const char*)TFTP->outBuf, TFTP->outBufLen, 0, (struct sockaddr *)&TFTP->theiraddr_in, sizeof(TFTP->theiraddr_in));
  98 }
  99
 100 void TFTP_SendError(TFTP_t* TFTP, int errorIndex, const char* errStr)
 101 {
 102         putRevWORD(TFTP->outBuf, errorCode_e);
 103         putRevWORD(TFTP->outBuf + 2, errorIndex);
 104         strcpy((char*) (TFTP->outBuf + 4), errStr);
 105         TFTP->outBufLen = strlen(errStr) + 5;
 106         TFTP_SendBuffer(TFTP);
 107 }
 108
 109 void TFTP_SendNextBlock(TFTP_t* TFTP)
 110 {
 111         if (!TFTP->isSendingFile || !TFTP->pFile)
 112         {
 113                 printf("TFTP: Reached SendNextBlock when no file is sent.\n");
 114                 TFTP_CLOSEFILE;
 115                 return;
 116         }
 117         if (TFTP->blockNum > 0 && TFTP->lastBlockSize < TFTP_MAX_FILE_BLOCK_SIZE)
 118         {
 119                 printf("TFTP: File has been sent successfully.\n");
 120                 TFTP_CLOSEFILE;
 121                 return;
 122         }
 123         ++TFTP->blockNum;
 124         putRevWORD(TFTP->outBuf, dataBlockCode_e);
 125         putRevWORD(TFTP->outBuf + 2, TFTP->blockNum);
 126         TFTP->lastBlockSize = fread(TFTP->outBuf+4, 1, TFTP_MAX_FILE_BLOCK_SIZE, TFTP->pFile);
 127 //      printf("TFTP: Sending file block %d, blockSize = %d\n", TFTP->blockNum, TFTP->lastBlockSize);
 128         TFTP->outBufLen = TFTP->lastBlockSize + 4;
 129         TFTP_SendBuffer(TFTP);
 130 }
 131
 132 void TFTP_HandleReadRequest(TFTP_t* TFTP)
 133 {
 134         const char* filename = (const char*)(TFTP->inBuf + 2);
 135         printf("TFTP: Client requests file: %s\n", filename);
 136         if (TFTP->pFile) printf("TFTP: Aborting previous file operation.\n");
 137         TFTP->pFile = fopen(filename, "rb");
 138         if (!TFTP->pFile)
 139         {
 140                 printf("TFTP: Requested file not found.\n");
 141                 TFTP_SendError(TFTP, fileNotFound_e, "MTLK TFTP: File not found !");
 142                 return;
 143         }
 144         TFTP->blockNum = 0;
 145         TFTP->isSendingFile = 1;
 146         TFTP_SendNextBlock(TFTP);
 147 }
 148
 149 void TFTP_HandleACK(TFTP_t* TFTP)
 150 {
 151         MT_UINT16 ackBlock = getRevWORD(*((MT_UINT16*)(TFTP->inBuf + 2)));
 152
 153         if (!TFTP->isSendingFile || !TFTP->pFile)
 154         {
 155                 printf("TFTP: Reached ACK when no file is sent!\n");
 156                 TFTP_CLOSEFILE;
 157         }
 158
 159         if (ackBlock == TFTP->blockNum) TFTP_SendNextBlock(TFTP);
 160         else if (ackBlock == TFTP->blockNum - 1) TFTP_SendBuffer(TFTP); // Send the last block again
 161         else
 162         {
 163                 printf("TFTP: Got ACK for block %d, when we already sent block %d\n", ackBlock, TFTP->blockNum);
 164                 TFTP_CLOSEFILE;
 165         }
 166 }
 167
 168 void TFTP_SendAck(TFTP_t* TFTP)
 169 {
 170         putRevWORD(TFTP->outBuf, ackCode_e);
 171         putRevWORD(TFTP->outBuf + 2, TFTP->blockNum);
 172         TFTP->outBufLen = 4;
 173         TFTP_SendBuffer(TFTP);
 174 }
 175
 176 void TFTP_HandleWriteRequest(TFTP_t* TFTP)
 177 {
 178         const char* filename = (const char*)(TFTP->inBuf + 2);
 179         printf("TFTP: Client sends file: %s\n", filename);
 180         strcpy((char*) TFTP->filename, filename);
 181         if (TFTP->pFile) printf("TFTP: Aborting previous file operation.\n");
 182         unlink(TFTP_TEMP_WRITE_FILE);
 183         TFTP->pFile = fopen(TFTP_TEMP_WRITE_FILE, "wb");
 184         if (!TFTP->pFile)
 185         {
 186                 printf("TFTP: Cannot create temporary file.\n");
 187                 TFTP_SendError(TFTP, fileAccessViolation_e, "MTLK TFTP: Cannot create temporary file !");
 188                 return;
 189         }
 190         TFTP->blockNum = 0;
 191         TFTP->isSendingFile = 0;
 192         TFTP_SendAck(TFTP);
 193 }
 194
 195 void TFTP_HandleDataBlock(TFTP_t* TFTP)
 196 {
 197         MT_UINT16 dataBlock;
 198
 199         if (TFTP->isSendingFile || !TFTP->pFile)
 200         {
 201                 printf("TFTP: Reached SendNextBlock when no file is sent.\n");
 202                 TFTP_CLOSEFILE;
 203                 return;
 204         }
 205
 206         ++TFTP->blockNum;
 207         dataBlock = getRevWORD(*((MT_UINT16*)(TFTP->inBuf + 2)));
 208         if (dataBlock != TFTP->blockNum)
 209         {
 210                 if (dataBlock == TFTP->blockNum - 1) TFTP_SendBuffer(TFTP); // Send the last ack again
 211                 else
 212                 {
 213                         printf("TFTP: Got data block %d, when we already sent ack for block %d\n", dataBlock, TFTP->blockNum);
 214                         TFTP_CLOSEFILE;
 215                 }
 216                 return;
 217         }
 218
 219         TFTP->lastBlockSize = TFTP->inBufLen - 4;
 220         if (TFTP->lastBlockSize > 0)
 221         {
 222                 int writeSize = fwrite(TFTP->inBuf + 4, 1, TFTP->lastBlockSize, TFTP->pFile);
 223                 if (writeSize != TFTP->lastBlockSize)
 224                 {
 225                         printf("Failed to write file block.\n");
 226                         TFTP_CLOSEFILE;
 227                 }
 228         }
 229
 230 //      printf("TFTP: Received file block %d, blockSize = %d\n", TFTP->blockNum, TFTP->lastBlockSize);
 231         if (TFTP->lastBlockSize < TFTP_MAX_FILE_BLOCK_SIZE)
 232         {
 233                 struct stat fileStat;
 234                 int statRet = stat((char*) TFTP->filename, &fileStat);
 235                 TFTP_CLOSEFILE;
 236                 unlink((char*)TFTP->filename);
 237                 sprintf((char*)TFTP->outBuf, "mv %s %s", TFTP_TEMP_WRITE_FILE, TFTP->filename);
 238                 system((char*)TFTP->outBuf);
 239                 if (!statRet) chmod((char*)TFTP->filename, fileStat.st_mode);
 240                 printf("TFTP: File has been received successfully.\n");
 241         }
 242         TFTP_SendAck(TFTP);
 243 }
 244
 245 void TFTP_ParsePacket(TFTP_t* TFTP)
 246 {
 247         int opCode = (int)getRevWORD(*((MT_UINT16*)TFTP->inBuf));
 248         switch (opCode)
 249         {
 250         case readRequestCode_e: TFTP_HandleReadRequest(TFTP); break;
 251         case writeRequestCode_e: TFTP_HandleWriteRequest(TFTP); break;
 252         case dataBlockCode_e: TFTP_HandleDataBlock(TFTP); break;
 253         case ackCode_e: TFTP_HandleACK(TFTP); break;
 254         default: printf("Received a wrong opcode: %04X", opCode); break;
 255         }
 256 }
 257
 258 // Called when a client is connected.
 259 // Parameter: sock - the connected socket
 260 void TFTP_HandleSession(TFTP_t* TFTP)
 261 {
 262         socklen_t sockAddrLen = sizeof(TFTP->theiraddr_in);
 263
 264         memset((char *)&(TFTP->theiraddr_in), 0, sizeof(struct sockaddr_in));
 265         TFTP->theiraddr_in.sin_port = htons(TFTP_PORT);
 266         TFTP->theiraddr_in.sin_family = AF_INET;
 267         TFTP->theiraddr_in.sin_addr.s_addr = htonl(INADDR_ANY);
 268
 269         TFTP->inBufLen = recvfrom(TFTP->sock, (char*)(TFTP->inBuf), TFTP_BUF_SIZE-2, 0, (struct sockaddr *)&(TFTP->theiraddr_in), &sockAddrLen);
 270
 271         if (TFTP->inBufLen >= 2)
 272         {
 273                 TFTP->inBuf[TFTP->inBufLen] = 0;
 274                 TFTP->inBuf[TFTP->inBufLen+1] = 0; // Termiante UNICODE string
 275                 TFTP_ParsePacket(TFTP);
 276         }
 277         else printf("TFTP: Received a packet with %d bytes\n", TFTP->inBufLen);
 278 }
 279
 280 // The TFTP Server function (main thread function)
 281 // Starts the server socket, and handle its listen/accept stuff
 282 MT_THREAD_RET_T TFTP_Server_Thread_Func(void* pParam)
 283 {
 284         struct sockaddr_in myaddr_in;     /* for local socket address */
 285         long lResultStatus;
 286
 287         TFTP_t TFTPobj;
 288         TFTP_t* TFTP = &TFTPobj;
 289         memset(TFTP, 0, sizeof(TFTP_t));
 290         TFTP->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
 291         if (TFTP->sock == SOCKET_INVALID) return TFTP_Error(TFTP, "BCL Server Error --> can't create TFTP Socket!\n");
 292
 293         /* Setup for bind */
 294         memset((char *)&myaddr_in, 0, sizeof(struct sockaddr_in));
 295         myaddr_in.sin_port = htons(TFTP_PORT);
 296         myaddr_in.sin_family = AF_INET;
 297         myaddr_in.sin_addr.s_addr = htonl(INADDR_ANY);
 298
 299         /* Bind the socket */
 300         lResultStatus = bind(TFTP->sock,(struct sockaddr *)&myaddr_in, sizeof(myaddr_in));
 301
 302         if (lResultStatus == -1)
 303   {
 304       printf("BCL: Failed to start TFTP server. Working without it.\n");
 305       BCL_numOfThreads--;
 306       return MT_THREAD_RET;
 307   }
 308         //      setsockopt (TFTP_listenSocket, SOL_SOCKET, SO_REUSEADDR, (char*)&nOptVal, sizeof(int));
 309
 310         TFTP->inBuf = (MT_UBYTE*)malloc(TFTP_BUF_SIZE);
 311         TFTP->outBuf = (MT_UBYTE*)malloc(TFTP_BUF_SIZE);
 312         TFTP->filename = (MT_UBYTE*)malloc(TFTP_MAX_FILE_BLOCK_SIZE);
 313
 314         if (!TFTP->inBuf || !TFTP->outBuf || !TFTP->filename) return TFTP_Error(TFTP, "TFTP: NOT ENOUGH MEMORY!!!\n");
 315
 316         while(!BCL_nExit)
 317         {
 318                 TFTP_HandleSession(TFTP);
 319         }
 320
 321         close(TFTP->sock);
 322         TFTP_CLOSEFILE;
 323         free(TFTP->inBuf);
 324         free(TFTP->outBuf);
 325         free(TFTP->filename);
 326         BCL_numOfThreads--;
 327         return MT_THREAD_RET;
 328 }
 329
 330 void StartTFTPServer(void)
 331 {
 332         pthread_t TFTP_ThreadID;
 333
 334         // Check if we are little endian or big endian
 335         int testInt = 0x12345678;
 336         MT_UBYTE* pTestByte = (MT_UBYTE*)&testInt;
 337         BCL_areWeLittleEndian = (*pTestByte == 0x78);
 338
 339         printf("BCL: Starting TFTP Server (system is %s-endian)\n", BCL_areWeLittleEndian ? "little" : "big");
 340         ++BCL_numOfThreads;
 341         pthread_create(&TFTP_ThreadID, NULL, TFTP_Server_Thread_Func, (void*)0);
 342 }