1/*-
2 * server.c
3 *
4 * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
5 *
6 * Copyright (c) 2004 Maksim Yevmenkin <m_evmenkin@yahoo.com>
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
57static void	server_accept_client		(server_p srv, int32_t fd);
58static int32_t	server_process_request		(server_p srv, int32_t fd);
59static int32_t	server_send_error_response	(server_p srv, int32_t fd,
60						 uint16_t error);
61static void	server_close_fd			(server_p srv, int32_t fd);
62
63/*
64 * Initialize server
65 */
66
67int32_t
68server_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
228void
229server_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
249int32_t
250server_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
292static void
293server_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
408static int32_t
409server_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
537static int32_t
538server_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
564static void
565server_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