1 /*
2 * Copyright (c) 2018, Linaro Ltd.
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 *
8 * 1. Redistributions of source code must retain the above copyright notice,
9 * this list of conditions and the following disclaimer.
10 *
11 * 2. Redistributions in binary form must reproduce the above copyright notice,
12 * this list of conditions and the following disclaimer in the documentation
13 * and/or other materials provided with the distribution.
14 *
15 * 3. Neither the name of the copyright holder nor the names of its contributors
16 * may be used to endorse or promote products derived from this software without
17 * specific prior written permission.
18 *
19 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
23 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29 * POSSIBILITY OF SUCH DAMAGE.
30 */
31 #include <arpa/inet.h>
32 #include <sys/stat.h>
33 #include <errno.h>
34 #include <fcntl.h>
35 #include <libgen.h>
36 #include <libqrtr.h>
37 #include <stdio.h>
38 #include <stdlib.h>
39 #include <string.h>
40 #include <unistd.h>
41
42 #include "list.h"
43 #include "translate.h"
44
45 #define MAX(x, y) ((x) > (y) ? (x) : (y))
46
47 enum {
48 OP_RRQ = 1,
49 OP_WRQ,
50 OP_DATA,
51 OP_ACK,
52 OP_ERROR,
53 OP_OACK,
54 };
55
56 struct tftp_client {
57 struct list_head node;
58
59 struct sockaddr_qrtr sq;
60
61 int sock;
62 int fd;
63
64 size_t block;
65
66 size_t blksize;
67 size_t rsize;
68 size_t wsize;
69 unsigned int timeoutms;
70 };
71
72 static struct list_head readers = LIST_INIT(readers);
73 static struct list_head writers = LIST_INIT(writers);
74
tftp_send_data(struct tftp_client * client,unsigned int block,size_t offset)75 static ssize_t tftp_send_data(struct tftp_client *client,
76 unsigned int block, size_t offset)
77 {
78 ssize_t len;
79 char *buf;
80 char *p;
81
82 buf = malloc(4 + client->blksize);
83 p = buf;
84
85 *p++ = 0;
86 *p++ = OP_DATA;
87
88 *p++ = (block >> 8) & 0xff;
89 *p++ = block & 0xff;
90
91 len = pread(client->fd, p, client->blksize, offset);
92 if (len <= 0) {
93 if (len < 0)
94 printf("[TQFTP] failed to read data\n");
95 free(buf);
96 return len;
97 }
98
99 p += len;
100
101 // printf("[TQFTP] Sending %zd bytes of DATA\n", p - buf);
102 len = send(client->sock, buf, p - buf, 0);
103
104 free(buf);
105
106 return len;
107 }
108
109
tftp_send_ack(int sock,int block)110 static int tftp_send_ack(int sock, int block)
111 {
112 struct {
113 uint16_t opcode;
114 uint16_t block;
115 } ack = { htons(OP_ACK), htons(block) };
116
117 return send(sock, &ack, sizeof(ack), 0);
118 }
119
tftp_send_oack(int sock,size_t * blocksize,size_t * tsize,size_t * wsize,unsigned int * timeoutms,size_t * rsize)120 static int tftp_send_oack(int sock, size_t *blocksize, size_t *tsize,
121 size_t *wsize, unsigned int *timeoutms, size_t *rsize)
122 {
123 char buf[512];
124 char *p = buf;
125 int n;
126
127 *p++ = 0;
128 *p++ = OP_OACK;
129
130 if (blocksize) {
131 strcpy(p, "blksize");
132 p += 8;
133
134 n = sprintf(p, "%zd", *blocksize);
135 p += n;
136 *p++ = '\0';
137 }
138
139 if (timeoutms) {
140 strcpy(p, "timeoutms");
141 p += 10;
142
143 n = sprintf(p, "%d", *timeoutms);
144 p += n;
145 *p++ = '\0';
146 }
147
148 if (tsize && *tsize != -1) {
149 strcpy(p, "tsize");
150 p += 6;
151
152 n = sprintf(p, "%zd", *tsize);
153 p += n;
154 *p++ = '\0';
155 }
156
157 if (wsize) {
158 strcpy(p, "wsize");
159 p += 6;
160
161 n = sprintf(p, "%zd", *wsize);
162 p += n;
163 *p++ = '\0';
164 }
165
166 if (rsize) {
167 strcpy(p, "rsize");
168 p += 6;
169
170 n = sprintf(p, "%zd", *rsize);
171 p += n;
172 *p++ = '\0';
173 }
174
175 return send(sock, buf, p - buf, 0);
176 }
177
tftp_send_error(int sock,int code,const char * msg)178 static int tftp_send_error(int sock, int code, const char *msg)
179 {
180 size_t len;
181 char *buf;
182 int rc;
183
184 len = 4 + strlen(msg) + 1;
185
186 buf = calloc(1, len);
187 if (!buf)
188 return -1;
189
190 *(uint16_t*)buf = htons(OP_ERROR);
191 *(uint16_t*)(buf + 2) = htons(code);
192 strcpy(buf + 4, msg);
193
194 rc = send(sock, buf, len, 0);
195 free(buf);
196 return rc;
197 }
198
handle_rrq(const char * buf,size_t len,struct sockaddr_qrtr * sq)199 static void handle_rrq(const char *buf, size_t len, struct sockaddr_qrtr *sq)
200 {
201 struct tftp_client *client;
202 const char *filename;
203 const char *value;
204 const char *mode;
205 const char *opt;
206 struct stat sb;
207 const char *p;
208 ssize_t tsize = -1;
209 size_t blksize = 512;
210 unsigned int timeoutms = 1000;
211 size_t rsize = 0;
212 size_t wsize = 0;
213 bool do_oack = false;
214 int sock;
215 int ret;
216 int fd;
217
218 p = buf + 2;
219
220 filename = p;
221 p += strlen(p) + 1;
222
223 mode = p;
224 p += strlen(p) + 1;
225
226 if (strcasecmp(mode, "octet")) {
227 /* XXX: error */
228 printf("[TQFTP] not octet, reject\n");
229 return;
230 }
231
232 printf("[TQFTP] RRQ: %s (%s)\n", filename, mode);
233
234 if (p < buf + len) {
235 do_oack = true;
236
237 while (p < buf + len) {
238 /* XXX: ensure we're not running off the end */
239 opt = p;
240 p += strlen(p) + 1;
241
242 /* XXX: ensure we're not running off the end */
243 value = p;
244 p += strlen(p) + 1;
245
246 if (!strcmp(opt, "blksize")) {
247 blksize = atoi(value);
248 } else if (!strcmp(opt, "timeoutms")) {
249 timeoutms = atoi(value);
250 } else if (!strcmp(opt, "tsize")) {
251 tsize = atoi(value);
252 } else if (!strcmp(opt, "rsize")) {
253 rsize = atoi(value);
254 } else if (!strcmp(opt, "wsize")) {
255 wsize = atoi(value);
256 } else {
257 printf("[TQFTP] Ignoring unknown option '%s'\n", opt);
258 }
259 }
260 }
261
262 sock = qrtr_open(0);
263 if (sock < 0) {
264 /* XXX: error */
265 printf("[TQFTP] unable to create new qrtr socket, reject\n");
266 return;
267 }
268
269 ret = connect(sock, (struct sockaddr *)sq, sizeof(*sq));
270 if (ret < 0) {
271 /* XXX: error */
272 printf("[TQFTP] unable to connect new qrtr socket to remote\n");
273 return;
274 }
275
276 fd = translate_open(filename, O_RDONLY);
277 if (fd < 0) {
278 printf("[TQFTP] unable to open %s (%d), reject\n", filename, errno);
279 tftp_send_error(sock, 1, "file not found");
280 return;
281 }
282
283 if (tsize != -1) {
284 fstat(fd, &sb);
285 tsize = sb.st_size;
286 }
287
288 client = calloc(1, sizeof(*client));
289 client->sq = *sq;
290 client->sock = sock;
291 client->fd = fd;
292 client->blksize = blksize;
293 client->rsize = rsize;
294 client->wsize = wsize;
295 client->timeoutms = timeoutms;
296
297 // printf("[TQFTP] new reader added\n");
298
299 list_add(&readers, &client->node);
300
301 if (do_oack) {
302 tftp_send_oack(client->sock, &blksize,
303 tsize ? (size_t*)&tsize : NULL,
304 wsize ? &wsize : NULL,
305 &client->timeoutms,
306 rsize ? &rsize: NULL);
307 } else {
308 tftp_send_data(client, 1, 0);
309 }
310 }
311
handle_wrq(const char * buf,size_t len __unused,struct sockaddr_qrtr * sq)312 static void handle_wrq(const char *buf, size_t len __unused, struct sockaddr_qrtr *sq)
313 {
314 struct tftp_client *client;
315 const char *filename;
316 const char *mode;
317 int sock;
318 int ret;
319 int fd;
320
321 filename = buf + 2;
322 mode = buf + 2 + strlen(filename) + 1;
323
324 if (strcasecmp(mode, "octet")) {
325 /* XXX: error */
326 printf("[TQFTP] not octet, reject\n");
327 return;
328 }
329
330 printf("[TQFTP] WRQ: %s (%s)\n", filename, mode);
331
332 fd = translate_open(filename, O_WRONLY | O_CREAT);
333 if (fd < 0) {
334 /* XXX: error */
335 printf("[TQFTP] unable to open %s (%d), reject\n", filename, errno);
336 return;
337 }
338
339 sock = qrtr_open(0);
340 if (sock < 0) {
341 /* XXX: error */
342 printf("[TQFTP] unable to create new qrtr socket, reject\n");
343 return;
344 }
345
346 ret = connect(sock, (struct sockaddr *)sq, sizeof(*sq));
347 if (ret < 0) {
348 /* XXX: error */
349 printf("[TQFTP] unable to connect new qrtr socket to remote\n");
350 return;
351 }
352
353 client = calloc(1, sizeof(*client));
354 client->sq = *sq;
355 client->sock = sock;
356 client->fd = fd;
357
358 ret = tftp_send_ack(client->sock, 0);
359 if (ret < 0) {
360 printf("[TQFTP] unable to send ack\n");
361 close(sock);
362 close(fd);
363 free(client);
364 return;
365 }
366
367 // printf("[TQFTP] new writer added\n");
368
369 list_add(&writers, &client->node);
370 }
371
handle_reader(struct tftp_client * client)372 static int handle_reader(struct tftp_client *client)
373 {
374 struct sockaddr_qrtr sq;
375 uint16_t block;
376 uint16_t last;
377 char buf[128];
378 socklen_t sl;
379 ssize_t len;
380 ssize_t n = 0;
381 int opcode;
382 int ret;
383
384 sl = sizeof(sq);
385 len = recvfrom(client->sock, buf, sizeof(buf), 0, (void *)&sq, &sl);
386 if (len < 0) {
387 ret = -errno;
388 if (ret != -ENETRESET)
389 fprintf(stderr, "[TQFTP] recvfrom failed: %d\n", ret);
390 return -1;
391 }
392
393 /* Drop unsolicited messages */
394 if (sq.sq_node != client->sq.sq_node ||
395 sq.sq_port != client->sq.sq_port) {
396 printf("[TQFTP] Discarding spoofed message\n");
397 return -1;
398 }
399
400 opcode = buf[0] << 8 | buf[1];
401 if (opcode == OP_ERROR) {
402 buf[len] = '\0';
403 printf("[TQFTP] Remote returned an error: %s\n", buf + 4);
404 return -1;
405 } else if (opcode != OP_ACK) {
406 printf("[TQFTP] Expected ACK, got %d\n", opcode);
407 return -1;
408 }
409
410 last = buf[2] << 8 | buf[3];
411 // printf("[TQFTP] Got ack for %d\n", last);
412
413 for (block = last; block < last + client->wsize; block++) {
414 n = tftp_send_data(client, block + 1,
415 block * client->blksize);
416 if (n < 0) {
417 printf("[TQFTP] Sent block %d failed: %zd\n", block + 1, n);
418 break;
419 }
420 // printf("[TQFTP] Sent block %d of %zd\n", block + 1, n);
421 if (n == 0)
422 break;
423 }
424
425 return 1;
426 }
427
handle_writer(struct tftp_client * client)428 static int handle_writer(struct tftp_client *client)
429 {
430 struct sockaddr_qrtr sq;
431 uint16_t block;
432 size_t payload;
433 char buf[516];
434 socklen_t sl;
435 ssize_t len;
436 int opcode;
437 int ret;
438
439 sl = sizeof(sq);
440 len = recvfrom(client->sock, buf, sizeof(buf), 0, (void *)&sq, &sl);
441 if (len < 0) {
442 ret = -errno;
443 if (ret != -ENETRESET)
444 fprintf(stderr, "[TQFTP] recvfrom failed: %d\n", ret);
445 return -1;
446 }
447
448 /* Drop unsolicited messages */
449 if (sq.sq_node != client->sq.sq_node ||
450 sq.sq_port != client->sq.sq_port)
451 return -1;
452
453 opcode = buf[0] << 8 | buf[1];
454 block = buf[2] << 8 | buf[3];
455 if (opcode != OP_DATA) {
456 printf("[TQFTP] Expected DATA opcode, got %d\n", opcode);
457 tftp_send_error(client->sock, 4, "Expected DATA opcode");
458 return -1;
459 }
460
461 payload = len - 4;
462
463 ret = write(client->fd, buf + 4, payload);
464 if (ret < 0) {
465 /* XXX: report error */
466 printf("[TQFTP] failed to write data\n");
467 return -1;
468 }
469
470 tftp_send_ack(client->sock, block);
471
472 return payload == 512 ? 1 : 0;
473 }
474
client_close_and_free(struct tftp_client * client)475 static void client_close_and_free(struct tftp_client *client)
476 {
477 list_del(&client->node);
478 close(client->sock);
479 close(client->fd);
480 free(client);
481 }
482
main(int argc __unused,char ** argv __unused)483 int main(int argc __unused, char **argv __unused)
484 {
485 struct tftp_client *client;
486 struct tftp_client *next;
487 struct sockaddr_qrtr sq;
488 struct qrtr_packet pkt;
489 socklen_t sl;
490 ssize_t len;
491 char buf[4096];
492 fd_set rfds;
493 int nfds;
494 int opcode;
495 int ret;
496 int fd;
497
498 fd = qrtr_open(0);
499 if (fd < 0) {
500 fprintf(stderr, "failed to open qrtr socket\n");
501 exit(1);
502 }
503
504 ret = qrtr_publish(fd, 4096, 1, 0);
505 if (ret < 0) {
506 fprintf(stderr, "failed to publish service registry service\n");
507 exit(1);
508 }
509
510 for (;;) {
511 FD_ZERO(&rfds);
512 FD_SET(fd, &rfds);
513 nfds = fd;
514
515 list_for_each_entry(client, &writers, node) {
516 FD_SET(client->sock, &rfds);
517 nfds = MAX(nfds, client->sock);
518 }
519
520 list_for_each_entry(client, &readers, node) {
521 FD_SET(client->sock, &rfds);
522 nfds = MAX(nfds, client->sock);
523 }
524
525 ret = select(nfds + 1, &rfds, NULL, NULL, NULL);
526 if (ret < 0) {
527 if (errno == EINTR) {
528 continue;
529 } else {
530 fprintf(stderr, "select failed\n");
531 break;
532 }
533 }
534
535 list_for_each_entry_safe(client, next, &writers, node) {
536 if (FD_ISSET(client->sock, &rfds)) {
537 ret = handle_writer(client);
538 if (ret <= 0)
539 client_close_and_free(client);
540 }
541 }
542
543 list_for_each_entry_safe(client, next, &readers, node) {
544 if (FD_ISSET(client->sock, &rfds)) {
545 ret = handle_reader(client);
546 if (ret <= 0)
547 client_close_and_free(client);
548 }
549 }
550
551 if (FD_ISSET(fd, &rfds)) {
552 sl = sizeof(sq);
553 len = recvfrom(fd, buf, sizeof(buf), 0, (void *)&sq, &sl);
554 if (len < 0) {
555 ret = -errno;
556 if (ret != -ENETRESET)
557 fprintf(stderr, "[TQFTP] recvfrom failed: %d\n", ret);
558 return ret;
559 }
560
561 /* Ignore control messages */
562 if (sq.sq_port == QRTR_PORT_CTRL) {
563 ret = qrtr_decode(&pkt, buf, len, &sq);
564 if (ret < 0) {
565 fprintf(stderr, "[TQFTP] unable to decode qrtr packet\n");
566 return ret;
567 }
568
569 switch (pkt.type) {
570 case QRTR_TYPE_BYE:
571 // fprintf(stderr, "[TQFTP] got bye\n");
572 list_for_each_entry_safe(client, next, &writers, node) {
573 if (client->sq.sq_node == sq.sq_node)
574 client_close_and_free(client);
575 }
576 break;
577 case QRTR_TYPE_DEL_CLIENT:
578 // fprintf(stderr, "[TQFTP] got del_client\n");
579 list_for_each_entry_safe(client, next, &writers, node) {
580 if (!memcmp(&client->sq, &sq, sizeof(sq)))
581 client_close_and_free(client);
582 }
583 break;
584 }
585 } else {
586 if (len < 2)
587 continue;
588
589 opcode = buf[0] << 8 | buf[1];
590 switch (opcode) {
591 case OP_RRQ:
592 handle_rrq(buf, len, &sq);
593 break;
594 case OP_WRQ:
595 // printf("[TQFTP] write\n");
596 handle_wrq(buf, len, &sq);
597 break;
598 default:
599 printf("[TQFTP] unhandled op %d\n", opcode);
600 break;
601 }
602 }
603 }
604 }
605
606 close(fd);
607
608 return 0;
609 }
610