1 /*-
2 * server.c
3 *
4 * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
5 *
6 * Copyright (c) 2004 Maksim Yevmenkin <[email protected]>
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 * 1. Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * 2. Redistributions in binary form must reproduce the above copyright
15 * notice, this list of conditions and the following disclaimer in the
16 * documentation and/or other materials provided with the distribution.
17 *
18 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
19 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
22 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
24 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
25 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
26 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
27 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
28 * SUCH DAMAGE.
29 *
30 * $Id: server.c,v 1.6 2004/01/13 01:54:39 max Exp $
31 * $FreeBSD$
32 */
33
34 #include <sys/param.h>
35 #include <sys/select.h>
36 #include <sys/stat.h>
37 #include <sys/queue.h>
38 #include <sys/ucred.h>
39 #include <sys/un.h>
40 #include <netinet/in.h>
41 #include <arpa/inet.h>
42 #include <assert.h>
43 #define L2CAP_SOCKET_CHECKED
44 #include <bluetooth.h>
45 #include <errno.h>
46 #include <pwd.h>
47 #include <sdp.h>
48 #include <stdio.h>
49 #include <stdlib.h>
50 #include <string.h>
51 #include <unistd.h>
52 #include "log.h"
53 #include "profile.h"
54 #include "provider.h"
55 #include "server.h"
56
57 static void server_accept_client (server_p srv, int32_t fd);
58 static int32_t server_process_request (server_p srv, int32_t fd);
59 static int32_t server_send_error_response (server_p srv, int32_t fd,
60 uint16_t error);
61 static void server_close_fd (server_p srv, int32_t fd);
62
63 /*
64 * Initialize server
65 */
66
67 int32_t
server_init(server_p srv,char const * control)68 server_init(server_p srv, char const *control)
69 {
70 struct sockaddr_un un;
71 struct sockaddr_l2cap l2;
72 int32_t unsock, l2sock;
73 socklen_t size;
74 uint16_t imtu;
75
76 assert(srv != NULL);
77 assert(control != NULL);
78
79 memset(srv, 0, sizeof(*srv));
80
81 /* Open control socket */
82 if (unlink(control) < 0 && errno != ENOENT) {
83 log_crit("Could not unlink(%s). %s (%d)",
84 control, strerror(errno), errno);
85 return (-1);
86 }
87
88 unsock = socket(PF_LOCAL, SOCK_STREAM, 0);
89 if (unsock < 0) {
90 log_crit("Could not create control socket. %s (%d)",
91 strerror(errno), errno);
92 return (-1);
93 }
94
95 memset(&un, 0, sizeof(un));
96 un.sun_len = sizeof(un);
97 un.sun_family = AF_LOCAL;
98 strlcpy(un.sun_path, control, sizeof(un.sun_path));
99
100 if (bind(unsock, (struct sockaddr *) &un, sizeof(un)) < 0) {
101 log_crit("Could not bind control socket. %s (%d)",
102 strerror(errno), errno);
103 close(unsock);
104 return (-1);
105 }
106
107 if (chmod(control, S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP|S_IROTH|S_IWOTH) < 0) {
108 log_crit("Could not change permissions on control socket. " \
109 "%s (%d)", strerror(errno), errno);
110 close(unsock);
111 return (-1);
112 }
113
114 if (listen(unsock, 10) < 0) {
115 log_crit("Could not listen on control socket. %s (%d)",
116 strerror(errno), errno);
117 close(unsock);
118 return (-1);
119 }
120
121 /* Open L2CAP socket */
122 l2sock = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BLUETOOTH_PROTO_L2CAP);
123 if (l2sock < 0) {
124 log_crit("Could not create L2CAP socket. %s (%d)",
125 strerror(errno), errno);
126 close(unsock);
127 return (-1);
128 }
129
130 size = sizeof(imtu);
131 if (getsockopt(l2sock, SOL_L2CAP, SO_L2CAP_IMTU, &imtu, &size) < 0) {
132 log_crit("Could not get L2CAP IMTU. %s (%d)",
133 strerror(errno), errno);
134 close(unsock);
135 close(l2sock);
136 return (-1);
137 }
138
139 memset(&l2, 0, sizeof(l2));
140 l2.l2cap_len = sizeof(l2);
141 l2.l2cap_family = AF_BLUETOOTH;
142 memcpy(&l2.l2cap_bdaddr, NG_HCI_BDADDR_ANY, sizeof(l2.l2cap_bdaddr));
143 l2.l2cap_psm = htole16(NG_L2CAP_PSM_SDP);
144
145 if (bind(l2sock, (struct sockaddr *) &l2, sizeof(l2)) < 0) {
146 log_crit("Could not bind L2CAP socket. %s (%d)",
147 strerror(errno), errno);
148 close(unsock);
149 close(l2sock);
150 return (-1);
151 }
152
153 if (listen(l2sock, 10) < 0) {
154 log_crit("Could not listen on L2CAP socket. %s (%d)",
155 strerror(errno), errno);
156 close(unsock);
157 close(l2sock);
158 return (-1);
159 }
160
161 /* Allocate incoming buffer */
162 srv->imtu = (imtu > SDP_LOCAL_MTU)? imtu : SDP_LOCAL_MTU;
163 srv->req = (uint8_t *) calloc(srv->imtu, sizeof(srv->req[0]));
164 if (srv->req == NULL) {
165 log_crit("Could not allocate request buffer");
166 close(unsock);
167 close(l2sock);
168 return (-1);
169 }
170
171 /* Allocate memory for descriptor index */
172 srv->fdidx = (fd_idx_p) calloc(FD_SETSIZE, sizeof(srv->fdidx[0]));
173 if (srv->fdidx == NULL) {
174 log_crit("Could not allocate fd index");
175 free(srv->req);
176 close(unsock);
177 close(l2sock);
178 return (-1);
179 }
180
181 /* Register Service Discovery profile (attach it to control socket) */
182 if (provider_register_sd(unsock) < 0) {
183 log_crit("Could not register Service Discovery profile");
184 free(srv->fdidx);
185 free(srv->req);
186 close(unsock);
187 close(l2sock);
188 return (-1);
189 }
190
191 /*
192 * If we got here then everything is fine. Add both control sockets
193 * to the index.
194 */
195
196 FD_ZERO(&srv->fdset);
197 srv->maxfd = (unsock > l2sock)? unsock : l2sock;
198
199 FD_SET(unsock, &srv->fdset);
200 srv->fdidx[unsock].valid = 1;
201 srv->fdidx[unsock].server = 1;
202 srv->fdidx[unsock].control = 1;
203 srv->fdidx[unsock].priv = 0;
204 srv->fdidx[unsock].rsp_cs = 0;
205 srv->fdidx[unsock].rsp_size = 0;
206 srv->fdidx[unsock].rsp_limit = 0;
207 srv->fdidx[unsock].omtu = SDP_LOCAL_MTU;
208 srv->fdidx[unsock].rsp = NULL;
209
210 FD_SET(l2sock, &srv->fdset);
211 srv->fdidx[l2sock].valid = 1;
212 srv->fdidx[l2sock].server = 1;
213 srv->fdidx[l2sock].control = 0;
214 srv->fdidx[l2sock].priv = 0;
215 srv->fdidx[l2sock].rsp_cs = 0;
216 srv->fdidx[l2sock].rsp_size = 0;
217 srv->fdidx[l2sock].rsp_limit = 0;
218 srv->fdidx[l2sock].omtu = 0; /* unknown */
219 srv->fdidx[l2sock].rsp = NULL;
220
221 return (0);
222 }
223
224 /*
225 * Shutdown server
226 */
227
228 void
server_shutdown(server_p srv)229 server_shutdown(server_p srv)
230 {
231 int fd;
232
233 assert(srv != NULL);
234
235 for (fd = 0; fd < srv->maxfd + 1; fd ++)
236 if (srv->fdidx[fd].valid)
237 server_close_fd(srv, fd);
238
239 free(srv->req);
240 free(srv->fdidx);
241
242 memset(srv, 0, sizeof(*srv));
243 }
244
245 /*
246 * Do one server iteration
247 */
248
249 int32_t
server_do(server_p srv)250 server_do(server_p srv)
251 {
252 fd_set fdset;
253 int32_t n, fd;
254
255 assert(srv != NULL);
256
257 /* Copy cached version of the fd set and call select */
258 memcpy(&fdset, &srv->fdset, sizeof(fdset));
259 n = select(srv->maxfd + 1, &fdset, NULL, NULL, NULL);
260 if (n < 0) {
261 if (errno == EINTR)
262 return (0);
263
264 log_err("Could not select(%d, %p). %s (%d)",
265 srv->maxfd + 1, &fdset, strerror(errno), errno);
266
267 return (-1);
268 }
269
270 /* Process descriptors */
271 for (fd = 0; fd < srv->maxfd + 1 && n > 0; fd ++) {
272 if (!FD_ISSET(fd, &fdset))
273 continue;
274
275 assert(srv->fdidx[fd].valid);
276 n --;
277
278 if (srv->fdidx[fd].server)
279 server_accept_client(srv, fd);
280 else if (server_process_request(srv, fd) != 0)
281 server_close_fd(srv, fd);
282 }
283
284 return (0);
285
286 }
287
288 /*
289 * Accept new client connection and register it with index
290 */
291
292 static void
server_accept_client(server_p srv,int32_t fd)293 server_accept_client(server_p srv, int32_t fd)
294 {
295 uint8_t *rsp = NULL;
296 int32_t cfd, priv;
297 uint16_t omtu;
298 socklen_t size;
299
300 do {
301 cfd = accept(fd, NULL, NULL);
302 } while (cfd < 0 && errno == EINTR);
303
304 if (cfd < 0) {
305 log_err("Could not accept connection on %s socket. %s (%d)",
306 srv->fdidx[fd].control? "control" : "L2CAP",
307 strerror(errno), errno);
308 return;
309 }
310
311 assert(!FD_ISSET(cfd, &srv->fdset));
312 assert(!srv->fdidx[cfd].valid);
313
314 priv = 0;
315
316 if (!srv->fdidx[fd].control) {
317 /* Get local BD_ADDR */
318 size = sizeof(srv->req_sa);
319 if (getsockname(cfd,(struct sockaddr*)&srv->req_sa,&size) < 0) {
320 log_err("Could not get local BD_ADDR. %s (%d)",
321 strerror(errno), errno);
322 close(cfd);
323 return;
324 }
325
326 /* Get outgoing MTU */
327 size = sizeof(omtu);
328 if (getsockopt(cfd,SOL_L2CAP,SO_L2CAP_OMTU,&omtu,&size) < 0) {
329 log_err("Could not get L2CAP OMTU. %s (%d)",
330 strerror(errno), errno);
331 close(cfd);
332 return;
333 }
334
335 /*
336 * The maximum size of the L2CAP packet is 65536 bytes.
337 * The minimum L2CAP MTU is 43 bytes. That means we need
338 * 65536 / 43 = ~1524 chunks to transfer maximum packet
339 * size with minimum MTU. The "rsp_cs" field in fd_idx_t
340 * is 11 bits wide, which gives us up to 2048 chunks.
341 */
342
343 if (omtu < NG_L2CAP_MTU_MINIMUM) {
344 log_err("L2CAP OMTU is too small (%d bytes)", omtu);
345 close(cfd);
346 return;
347 }
348 } else {
349 struct xucred cr;
350 struct passwd *pw;
351
352 /* Get peer's credentials */
353 memset(&cr, 0, sizeof(cr));
354 size = sizeof(cr);
355
356 if (getsockopt(cfd, 0, LOCAL_PEERCRED, &cr, &size) < 0) {
357 log_err("Could not get peer's credentials. %s (%d)",
358 strerror(errno), errno);
359 close(cfd);
360 return;
361 }
362
363 /* Check credentials */
364 pw = getpwuid(cr.cr_uid);
365 if (pw != NULL)
366 priv = (strcmp(pw->pw_name, "root") == 0);
367 else
368 log_warning("Could not verify credentials for uid %d",
369 cr.cr_uid);
370
371 memcpy(&srv->req_sa.l2cap_bdaddr, NG_HCI_BDADDR_ANY,
372 sizeof(srv->req_sa.l2cap_bdaddr));
373
374 omtu = srv->fdidx[fd].omtu;
375 }
376
377 /*
378 * Allocate buffer. This is an overkill, but we can not know how
379 * big our reply is going to be.
380 */
381
382 rsp = (uint8_t *) calloc(NG_L2CAP_MTU_MAXIMUM, sizeof(rsp[0]));
383 if (rsp == NULL) {
384 log_crit("Could not allocate response buffer");
385 close(cfd);
386 return;
387 }
388
389 /* Add client descriptor to the index */
390 FD_SET(cfd, &srv->fdset);
391 if (srv->maxfd < cfd)
392 srv->maxfd = cfd;
393 srv->fdidx[cfd].valid = 1;
394 srv->fdidx[cfd].server = 0;
395 srv->fdidx[cfd].control = srv->fdidx[fd].control;
396 srv->fdidx[cfd].priv = priv;
397 srv->fdidx[cfd].rsp_cs = 0;
398 srv->fdidx[cfd].rsp_size = 0;
399 srv->fdidx[cfd].rsp_limit = 0;
400 srv->fdidx[cfd].omtu = omtu;
401 srv->fdidx[cfd].rsp = rsp;
402 }
403
404 /*
405 * Process request from the client
406 */
407
408 static int32_t
server_process_request(server_p srv,int32_t fd)409 server_process_request(server_p srv, int32_t fd)
410 {
411 sdp_pdu_p pdu = (sdp_pdu_p) srv->req;
412 int32_t len, error;
413
414 assert(srv->imtu > 0);
415 assert(srv->req != NULL);
416 assert(FD_ISSET(fd, &srv->fdset));
417 assert(srv->fdidx[fd].valid);
418 assert(!srv->fdidx[fd].server);
419 assert(srv->fdidx[fd].rsp != NULL);
420 assert(srv->fdidx[fd].omtu >= NG_L2CAP_MTU_MINIMUM);
421
422 do {
423 len = read(fd, srv->req, srv->imtu);
424 } while (len < 0 && errno == EINTR);
425
426 if (len < 0) {
427 log_err("Could not receive SDP request from %s socket. %s (%d)",
428 srv->fdidx[fd].control? "control" : "L2CAP",
429 strerror(errno), errno);
430 return (-1);
431 }
432 if (len == 0) {
433 log_info("Client on %s socket has disconnected",
434 srv->fdidx[fd].control? "control" : "L2CAP");
435 return (-1);
436 }
437
438 if (len >= sizeof(*pdu) &&
439 sizeof(*pdu) + (pdu->len = ntohs(pdu->len)) == len) {
440 switch (pdu->pid) {
441 case SDP_PDU_SERVICE_SEARCH_REQUEST:
442 error = server_prepare_service_search_response(srv, fd);
443 break;
444
445 case SDP_PDU_SERVICE_ATTRIBUTE_REQUEST:
446 error = server_prepare_service_attribute_response(srv, fd);
447 break;
448
449 case SDP_PDU_SERVICE_SEARCH_ATTRIBUTE_REQUEST:
450 error = server_prepare_service_search_attribute_response(srv, fd);
451 break;
452
453 case SDP_PDU_SERVICE_REGISTER_REQUEST:
454 error = server_prepare_service_register_response(srv, fd);
455 break;
456
457 case SDP_PDU_SERVICE_UNREGISTER_REQUEST:
458 error = server_prepare_service_unregister_response(srv, fd);
459 break;
460
461 case SDP_PDU_SERVICE_CHANGE_REQUEST:
462 error = server_prepare_service_change_response(srv, fd);
463 break;
464
465 default:
466 error = SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX;
467 break;
468 }
469 } else
470 error = SDP_ERROR_CODE_INVALID_PDU_SIZE;
471
472 if (error == 0) {
473 switch (pdu->pid) {
474 case SDP_PDU_SERVICE_SEARCH_REQUEST:
475 error = server_send_service_search_response(srv, fd);
476 break;
477
478 case SDP_PDU_SERVICE_ATTRIBUTE_REQUEST:
479 error = server_send_service_attribute_response(srv, fd);
480 break;
481
482 case SDP_PDU_SERVICE_SEARCH_ATTRIBUTE_REQUEST:
483 error = server_send_service_search_attribute_response(srv, fd);
484 break;
485
486 case SDP_PDU_SERVICE_REGISTER_REQUEST:
487 error = server_send_service_register_response(srv, fd);
488 break;
489
490 case SDP_PDU_SERVICE_UNREGISTER_REQUEST:
491 error = server_send_service_unregister_response(srv, fd);
492 break;
493
494 case SDP_PDU_SERVICE_CHANGE_REQUEST:
495 error = server_send_service_change_response(srv, fd);
496 break;
497
498 default:
499 error = SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX;
500 break;
501 }
502
503 if (error != 0)
504 log_err("Could not send SDP response to %s socket, " \
505 "pdu->pid=%d, pdu->tid=%d, error=%d",
506 srv->fdidx[fd].control? "control" : "L2CAP",
507 pdu->pid, ntohs(pdu->tid), error);
508 } else {
509 log_err("Could not process SDP request from %s socket, " \
510 "pdu->pid=%d, pdu->tid=%d, pdu->len=%d, len=%d, " \
511 "error=%d",
512 srv->fdidx[fd].control? "control" : "L2CAP",
513 pdu->pid, ntohs(pdu->tid), pdu->len, len, error);
514
515 error = server_send_error_response(srv, fd, error);
516 if (error != 0)
517 log_err("Could not send SDP error response to %s " \
518 "socket, pdu->pid=%d, pdu->tid=%d, error=%d",
519 srv->fdidx[fd].control? "control" : "L2CAP",
520 pdu->pid, ntohs(pdu->tid), error);
521 }
522
523 /* On error forget response (if any) */
524 if (error != 0) {
525 srv->fdidx[fd].rsp_cs = 0;
526 srv->fdidx[fd].rsp_size = 0;
527 srv->fdidx[fd].rsp_limit = 0;
528 }
529
530 return (error);
531 }
532
533 /*
534 * Send SDP_Error_Response PDU
535 */
536
537 static int32_t
server_send_error_response(server_p srv,int32_t fd,uint16_t error)538 server_send_error_response(server_p srv, int32_t fd, uint16_t error)
539 {
540 int32_t size;
541
542 struct {
543 sdp_pdu_t pdu;
544 uint16_t error;
545 } __attribute__ ((packed)) rsp;
546
547 /* Prepare and send SDP error response */
548 rsp.pdu.pid = SDP_PDU_ERROR_RESPONSE;
549 rsp.pdu.tid = ((sdp_pdu_p)(srv->req))->tid;
550 rsp.pdu.len = htons(sizeof(rsp.error));
551 rsp.error = htons(error);
552
553 do {
554 size = write(fd, &rsp, sizeof(rsp));
555 } while (size < 0 && errno == EINTR);
556
557 return ((size < 0)? errno : 0);
558 }
559
560 /*
561 * Close descriptor and remove it from index
562 */
563
564 static void
server_close_fd(server_p srv,int32_t fd)565 server_close_fd(server_p srv, int32_t fd)
566 {
567 provider_p provider = NULL, provider_next = NULL;
568
569 assert(FD_ISSET(fd, &srv->fdset));
570 assert(srv->fdidx[fd].valid);
571
572 close(fd);
573
574 FD_CLR(fd, &srv->fdset);
575 if (fd == srv->maxfd)
576 srv->maxfd --;
577
578 if (srv->fdidx[fd].rsp != NULL)
579 free(srv->fdidx[fd].rsp);
580
581 memset(&srv->fdidx[fd], 0, sizeof(srv->fdidx[fd]));
582
583 for (provider = provider_get_first();
584 provider != NULL;
585 provider = provider_next) {
586 provider_next = provider_get_next(provider);
587
588 if (provider->fd == fd)
589 provider_unregister(provider);
590 }
591 }
592
593