tftp.c 9.4 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480
  1. /**
  2. * nmrpflash - Netgear Unbrick Utility
  3. * Copyright (C) 2016 Joseph Lehner <joseph.c.lehner@gmail.com>
  4. *
  5. * nmrpflash is free software: you can redistribute it and/or modify
  6. * it under the terms of the GNU General Public License as published by
  7. * the Free Software Foundation, either version 3 of the License, or
  8. * (at your option) any later version.
  9. *
  10. * nmrpflash is distributed in the hope that it will be useful,
  11. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  13. * GNU General Public License for more details.
  14. *
  15. * You should have received a copy of the GNU General Public License
  16. * along with nmrpflash. If not, see <http://www.gnu.org/licenses/>.
  17. *
  18. */
  19. #include <string.h>
  20. #include <unistd.h>
  21. #include <stdlib.h>
  22. #include <stdio.h>
  23. #include <errno.h>
  24. #include <fcntl.h>
  25. #include <ctype.h>
  26. #include "nmrpd.h"
  27. #ifndef O_BINARY
  28. #define O_BINARY 0
  29. #endif
  30. #define TFTP_BLKSIZE 1456
  31. static const char *opcode_names[] = {
  32. "RRQ", "WRQ", "DATA", "ACK", "ERR", "OACK"
  33. };
  34. enum tftp_opcode {
  35. RRQ = 1,
  36. WRQ = 2,
  37. DATA = 3,
  38. ACK = 4,
  39. ERR = 5,
  40. OACK = 6
  41. };
  42. static bool is_netascii(const char *str)
  43. {
  44. uint8_t *p = (uint8_t*)str;
  45. for (; *p; ++p) {
  46. if (*p < 0x20 || *p > 0x7f) {
  47. return false;
  48. }
  49. }
  50. return true;
  51. }
  52. static inline char *pkt_mknum(char *pkt, uint16_t n)
  53. {
  54. *(uint16_t*)pkt = htons(n);
  55. return pkt + 2;
  56. }
  57. static inline uint16_t pkt_num(char *pkt)
  58. {
  59. return ntohs(*(uint16_t*)pkt);
  60. }
  61. static char *pkt_mkopt(char *pkt, const char *opt, const char* val)
  62. {
  63. strcpy(pkt, opt);
  64. pkt += strlen(pkt) + 1;
  65. strcpy(pkt, val);
  66. pkt += strlen(val) + 1;
  67. return pkt;
  68. }
  69. static bool pkt_nextstr(char **pkt, char **str, size_t *rem)
  70. {
  71. size_t len;
  72. if (!isprint(**pkt) || !(len = strnlen(*pkt, *rem))) {
  73. return false;
  74. } else if (str) {
  75. *str = *pkt;
  76. }
  77. *pkt += len + 1;
  78. if (*rem > 1) {
  79. *rem -= len + 1;
  80. } else {
  81. *rem = 0;
  82. }
  83. return true;
  84. }
  85. static bool pkt_nextopt(char **pkt, char **opt, char **val, size_t *rem)
  86. {
  87. return pkt_nextstr(pkt, opt, rem) && pkt_nextstr(pkt, val, rem);
  88. }
  89. static char *pkt_optval(char* pkt, const char* name)
  90. {
  91. size_t rem = 512;
  92. char *opt, *val;
  93. pkt += 2;
  94. while (pkt_nextopt(&pkt, &opt, &val, &rem)) {
  95. if (!strcasecmp(name, opt)) {
  96. return val;
  97. }
  98. }
  99. return NULL;
  100. }
  101. static size_t pkt_xrqlen(char *pkt)
  102. {
  103. size_t rem = 512;
  104. pkt += 2;
  105. while (pkt_nextopt(&pkt, NULL, NULL, &rem)) {
  106. ;
  107. }
  108. return 514 - rem;
  109. }
  110. static void pkt_mkwrq(char *pkt, const char *filename, unsigned blksize)
  111. {
  112. filename = leafname(filename);
  113. if (!tftp_is_valid_filename(filename)) {
  114. fprintf(stderr, "Overlong/illegal filename; using 'firmware'.\n");
  115. filename = "firmware";
  116. } else if (!strcmp(filename, "-")) {
  117. filename = "firmware";
  118. }
  119. pkt = pkt_mknum(pkt, WRQ);
  120. pkt = pkt_mkopt(pkt, filename, "octet");
  121. if (blksize && blksize != 512) {
  122. pkt = pkt_mkopt(pkt, "blksize", lltostr(blksize, 10));
  123. }
  124. }
  125. static inline void pkt_print(char *pkt, FILE *fp)
  126. {
  127. uint16_t opcode = pkt_num(pkt);
  128. size_t rem;
  129. char *opt, *val;
  130. if (!opcode || opcode > OACK) {
  131. fprintf(fp, "(%d)", opcode);
  132. } else {
  133. fprintf(fp, "%s", opcode_names[opcode - 1]);
  134. if (opcode == ACK || opcode == DATA) {
  135. fprintf(fp, "(%d)", pkt_num(pkt + 2));
  136. } else if (opcode == WRQ || opcode == RRQ) {
  137. fprintf(fp, "(%s, %s)", pkt + 2, pkt + 2 + strlen(pkt + 2) + 1);
  138. } else if (opcode == OACK) {
  139. fprintf(fp, "(");
  140. rem = 512;
  141. pkt += 2;
  142. while (pkt_nextopt(&pkt, &opt, &val, &rem)) {
  143. fprintf(fp, " %s=%s ", opt, val);
  144. }
  145. fprintf(fp, ")");
  146. }
  147. }
  148. }
  149. static ssize_t tftp_recvfrom(int sock, char *pkt, uint16_t* port,
  150. unsigned timeout, size_t pktlen)
  151. {
  152. ssize_t len;
  153. struct sockaddr_in src;
  154. #ifndef NMRPFLASH_WINDOWS
  155. socklen_t alen;
  156. #else
  157. int alen;
  158. #endif
  159. len = select_fd(sock, timeout);
  160. if (len < 0) {
  161. return -1;
  162. } else if (!len) {
  163. return 0;
  164. }
  165. alen = sizeof(src);
  166. len = recvfrom(sock, pkt, pktlen, 0, (struct sockaddr*)&src, &alen);
  167. if (len < 0) {
  168. sock_perror("recvfrom");
  169. return -1;
  170. }
  171. *port = ntohs(src.sin_port);
  172. uint16_t opcode = pkt_num(pkt);
  173. if (opcode == ERR) {
  174. fprintf(stderr, "Error (%d): %.511s\n", pkt_num(pkt + 2), pkt + 4);
  175. return -1;
  176. } else if (isprint(pkt[0])) {
  177. /* In case of a firmware checksum error, the EX2700 I've tested this
  178. * on sends a raw UDP packet containing just an error message starting
  179. * at offset 0. The limit of 32 chars is arbitrary.
  180. */
  181. fprintf(stderr, "Error: %.32s\n", pkt);
  182. return -2;
  183. } else if (!opcode || opcode > OACK) {
  184. fprintf(stderr, "Received invalid packet: ");
  185. pkt_print(pkt, stderr);
  186. fprintf(stderr, ".\n");
  187. return -1;
  188. }
  189. if (verbosity > 2) {
  190. printf(">> ");
  191. pkt_print(pkt, stdout);
  192. printf("\n");
  193. }
  194. return len;
  195. }
  196. static ssize_t tftp_sendto(int sock, char *pkt, size_t len,
  197. struct sockaddr_in *dst)
  198. {
  199. ssize_t sent;
  200. switch (pkt_num(pkt)) {
  201. case RRQ:
  202. case WRQ:
  203. case OACK:
  204. len = pkt_xrqlen(pkt);
  205. break;
  206. case DATA:
  207. len += 4;
  208. break;
  209. case ACK:
  210. len = 4;
  211. break;
  212. case ERR:
  213. len = 4 + strlen(pkt + 4);
  214. break;
  215. default:
  216. fprintf(stderr, "Attempted to send invalid packet ");
  217. pkt_print(pkt, stderr);
  218. fprintf(stderr, "; this is a bug!\n");
  219. return -1;
  220. }
  221. if (verbosity > 2) {
  222. printf("<< ");
  223. pkt_print(pkt, stdout);
  224. printf("\n");
  225. }
  226. sent = sendto(sock, pkt, len, 0, (struct sockaddr*)dst, sizeof(*dst));
  227. if (sent < 0) {
  228. sock_perror("sendto");
  229. }
  230. return sent;
  231. }
  232. const char *leafname(const char *path)
  233. {
  234. const char *slash, *bslash;
  235. slash = strrchr(path, '/');
  236. bslash = strrchr(path, '\\');
  237. if (slash && bslash) {
  238. path = 1 + (slash > bslash ? slash : bslash);
  239. } else if (slash) {
  240. path = 1 + slash;
  241. } else if (bslash) {
  242. path = 1 + bslash;
  243. }
  244. return path;
  245. }
  246. #ifdef NMRPFLASH_WINDOWS
  247. void sock_perror(const char *msg)
  248. {
  249. win_perror2(msg, WSAGetLastError());
  250. }
  251. #endif
  252. inline bool tftp_is_valid_filename(const char *filename)
  253. {
  254. return strlen(filename) <= 255 && is_netascii(filename);
  255. }
  256. int tftp_put(struct nmrpd_args *args)
  257. {
  258. struct sockaddr_in addr;
  259. uint16_t block, port, op, blksize;
  260. ssize_t len, last_len;
  261. int fd, sock, ret, timeouts, errors, ackblock;
  262. char rx[2048], tx[2048];
  263. const char *file_remote = args->file_remote;
  264. char *val, *end;
  265. sock = -1;
  266. ret = -1;
  267. fd = -1;
  268. if (g_interrupted) {
  269. goto cleanup;
  270. }
  271. if (!strcmp(args->file_local, "-")) {
  272. fd = STDIN_FILENO;
  273. if (!file_remote) {
  274. file_remote = "firmware";
  275. }
  276. } else {
  277. fd = open(args->file_local, O_RDONLY | O_BINARY);
  278. if (fd < 0) {
  279. xperror("open");
  280. ret = fd;
  281. goto cleanup;
  282. } else if (!file_remote) {
  283. file_remote = args->file_local;
  284. }
  285. }
  286. sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
  287. if (sock < 0) {
  288. sock_perror("socket");
  289. ret = sock;
  290. goto cleanup;
  291. }
  292. memset(&addr, 0, sizeof(addr));
  293. addr.sin_family = AF_INET;
  294. if (args->ipaddr_intf) {
  295. if ((addr.sin_addr.s_addr = inet_addr(args->ipaddr_intf)) == INADDR_NONE) {
  296. xperror("inet_addr");
  297. goto cleanup;
  298. }
  299. if (bind(sock, (struct sockaddr*)&addr, sizeof(addr)) != 0) {
  300. sock_perror("bind");
  301. goto cleanup;
  302. }
  303. }
  304. if ((addr.sin_addr.s_addr = inet_addr(args->ipaddr)) == INADDR_NONE) {
  305. xperror("inet_addr");
  306. goto cleanup;
  307. }
  308. addr.sin_port = htons(args->port);
  309. blksize = 512;
  310. block = 0;
  311. last_len = -1;
  312. len = 0;
  313. errors = 0;
  314. /* Not really, but this way the loop sends our WRQ before receiving */
  315. timeouts = 1;
  316. pkt_mkwrq(tx, file_remote, TFTP_BLKSIZE);
  317. while (!g_interrupted) {
  318. ackblock = -1;
  319. op = pkt_num(rx);
  320. if (!timeouts) {
  321. if (op == ACK) {
  322. ackblock = pkt_num(rx + 2);
  323. } else if (op == OACK) {
  324. ackblock = 0;
  325. if ((val = pkt_optval(rx, "blksize"))) {
  326. blksize = strtol(val, &end, 10);
  327. if (*end != '\0' || blksize < 8 || blksize > TFTP_BLKSIZE) {
  328. fprintf(stderr, "Error: invalid blksize in OACK: %s\n", val);
  329. ret = -1;
  330. goto cleanup;
  331. }
  332. }
  333. }
  334. }
  335. if (timeouts || ackblock == block) {
  336. if (!timeouts) {
  337. // TODO: set block to 1 if ++block == 0 ?
  338. ++block;
  339. pkt_mknum(tx, DATA);
  340. pkt_mknum(tx + 2, block);
  341. len = read(fd, tx + 4, blksize);
  342. if (len < 0) {
  343. xperror("read");
  344. ret = len;
  345. goto cleanup;
  346. } else if (!len) {
  347. if (last_len != blksize && last_len != -1) {
  348. break;
  349. }
  350. }
  351. last_len = len;
  352. }
  353. ret = tftp_sendto(sock, tx, len, &addr);
  354. if (ret < 0) {
  355. goto cleanup;
  356. }
  357. } else if ((op != OACK && op != ACK) || ackblock > block) {
  358. if (verbosity) {
  359. fprintf(stderr, "Expected ACK(%d), got ", block);
  360. pkt_print(rx, stderr);
  361. fprintf(stderr, ".\n");
  362. }
  363. if (ackblock != -1 && ++errors > 5) {
  364. fprintf(stderr, "Protocol error; bailing out.\n");
  365. ret = -1;
  366. goto cleanup;
  367. }
  368. }
  369. ret = tftp_recvfrom(sock, rx, &port, args->rx_timeout, blksize + 4);
  370. if (ret < 0) {
  371. goto cleanup;
  372. } else if (!ret) {
  373. if (++timeouts < 5 || (!block && timeouts < 10)) {
  374. continue;
  375. } else if (block) {
  376. fprintf(stderr, "Timeout while waiting for ACK(%d).\n", block);
  377. } else {
  378. fprintf(stderr, "Timeout while waiting for ACK(0)/OACK.\n");
  379. }
  380. ret = -1;
  381. goto cleanup;
  382. } else {
  383. timeouts = 0;
  384. ret = 0;
  385. if (!block && port != args->port) {
  386. if (verbosity > 1) {
  387. printf("Switching to port %d\n", port);
  388. }
  389. addr.sin_port = htons(port);
  390. }
  391. }
  392. }
  393. ret = !g_interrupted ? 0 : -1;
  394. cleanup:
  395. if (fd >= 0) {
  396. close(fd);
  397. }
  398. if (sock >= 0) {
  399. #ifndef NMRPFLASH_WINDOWS
  400. shutdown(sock, SHUT_RDWR);
  401. close(sock);
  402. #else
  403. shutdown(sock, SD_BOTH);
  404. closesocket(sock);
  405. #endif
  406. }
  407. return ret;
  408. }