4 * Copyright (c) 2004 Maksim Yevmenkin <m_evmenkin@yahoo.com>
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
10 * 1. Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer.
12 * 2. Redistributions in binary form must reproduce the above copyright
13 * notice, this list of conditions and the following disclaimer in the
14 * documentation and/or other materials provided with the distribution.
16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
17 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
20 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
21 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
22 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
24 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
25 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
28 * $Id: server.c,v 1.6 2004/01/13 01:54:39 max Exp $
32 #include <sys/select.h>
33 #include <sys/queue.h>
35 #include <netinet/in.h>
36 #include <arpa/inet.h>
38 #include <bluetooth.h>
50 static void server_accept_client (server_p srv, int32_t fd);
51 static int32_t server_process_request (server_p srv, int32_t fd);
52 static int32_t server_send_error_response (server_p srv, int32_t fd,
54 static void server_close_fd (server_p srv, int32_t fd);
61 server_init(server_p srv, char const *control)
63 struct sockaddr_un un;
64 struct sockaddr_l2cap l2;
65 int32_t unsock, l2sock, size;
69 assert(control != NULL);
71 memset(srv, 0, sizeof(srv));
73 /* Open control socket */
74 if (unlink(control) < 0 && errno != ENOENT) {
75 log_crit("Could not unlink(%s). %s (%d)",
76 control, strerror(errno), errno);
80 unsock = socket(PF_LOCAL, SOCK_STREAM, 0);
82 log_crit("Could not create control socket. %s (%d)",
83 strerror(errno), errno);
87 memset(&un, 0, sizeof(un));
88 un.sun_len = sizeof(un);
89 un.sun_family = AF_LOCAL;
90 strlcpy(un.sun_path, control, sizeof(un.sun_path));
92 if (bind(unsock, (struct sockaddr *) &un, sizeof(un)) < 0) {
93 log_crit("Could not bind control socket. %s (%d)",
94 strerror(errno), errno);
99 if (listen(unsock, 10) < 0) {
100 log_crit("Could not listen on control socket. %s (%d)",
101 strerror(errno), errno);
106 /* Open L2CAP socket */
107 l2sock = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BLUETOOTH_PROTO_L2CAP);
109 log_crit("Could not create L2CAP socket. %s (%d)",
110 strerror(errno), errno);
116 if (getsockopt(l2sock, SOL_L2CAP, SO_L2CAP_IMTU, &imtu, &size) < 0) {
117 log_crit("Could not get L2CAP IMTU. %s (%d)",
118 strerror(errno), errno);
124 memset(&l2, 0, sizeof(l2));
125 l2.l2cap_len = sizeof(l2);
126 l2.l2cap_family = AF_BLUETOOTH;
127 memcpy(&l2.l2cap_bdaddr, NG_HCI_BDADDR_ANY, sizeof(l2.l2cap_bdaddr));
128 l2.l2cap_psm = NG_L2CAP_PSM_SDP;
130 if (bind(l2sock, (struct sockaddr *) &l2, sizeof(l2)) < 0) {
131 log_crit("Could not bind L2CAP socket. %s (%d)",
132 strerror(errno), errno);
138 if (listen(l2sock, 10) < 0) {
139 log_crit("Could not listen on L2CAP socket. %s (%d)",
140 strerror(errno), errno);
146 /* Allocate incoming buffer */
147 srv->imtu = (imtu > SDP_LOCAL_MTU)? imtu : SDP_LOCAL_MTU;
148 srv->req = (uint8_t *) calloc(srv->imtu, sizeof(srv->req[0]));
149 if (srv->req == NULL) {
150 log_crit("Could not allocate request buffer");
156 /* Allocate memory for descriptor index */
157 srv->fdidx = (fd_idx_p) calloc(FD_SETSIZE, sizeof(srv->fdidx[0]));
158 if (srv->fdidx == NULL) {
159 log_crit("Could not allocate fd index");
166 /* Register Service Discovery profile (attach it to control socket) */
167 if (provider_register_sd(unsock) < 0) {
168 log_crit("Could not register Service Discovery profile");
177 * If we got here then everything is fine. Add both control sockets
181 FD_ZERO(&srv->fdset);
182 srv->maxfd = (unsock > l2sock)? unsock : l2sock;
184 FD_SET(unsock, &srv->fdset);
185 srv->fdidx[unsock].valid = 1;
186 srv->fdidx[unsock].server = 1;
187 srv->fdidx[unsock].control = 1;
188 srv->fdidx[unsock].rsp_cs = 0;
189 srv->fdidx[unsock].rsp_size = 0;
190 srv->fdidx[unsock].rsp_limit = 0;
191 srv->fdidx[unsock].omtu = SDP_LOCAL_MTU;
192 srv->fdidx[unsock].rsp = NULL;
194 FD_SET(l2sock, &srv->fdset);
195 srv->fdidx[l2sock].valid = 1;
196 srv->fdidx[l2sock].server = 1;
197 srv->fdidx[l2sock].control = 0;
198 srv->fdidx[l2sock].rsp_cs = 0;
199 srv->fdidx[l2sock].rsp_size = 0;
200 srv->fdidx[l2sock].rsp_limit = 0;
201 srv->fdidx[l2sock].omtu = 0; /* unknown */
202 srv->fdidx[l2sock].rsp = NULL;
212 server_shutdown(server_p srv)
218 for (fd = 0; fd < srv->maxfd + 1; fd ++)
219 if (srv->fdidx[fd].valid)
220 server_close_fd(srv, fd);
225 memset(srv, 0, sizeof(*srv));
229 * Do one server iteration
233 server_do(server_p srv)
240 /* Copy cached version of the fd set and call select */
241 memcpy(&fdset, &srv->fdset, sizeof(fdset));
242 n = select(srv->maxfd + 1, &fdset, NULL, NULL, NULL);
247 log_err("Could not select(%d, %p). %s (%d)",
248 srv->maxfd + 1, &fdset, strerror(errno), errno);
253 /* Process descriptors */
254 for (fd = 0; fd < srv->maxfd + 1 && n > 0; fd ++) {
255 if (!FD_ISSET(fd, &fdset))
258 assert(srv->fdidx[fd].valid);
261 if (srv->fdidx[fd].server)
262 server_accept_client(srv, fd);
263 else if (server_process_request(srv, fd) != 0)
264 server_close_fd(srv, fd);
272 * Accept new client connection and register it with index
276 server_accept_client(server_p srv, int32_t fd)
283 cfd = accept(fd, NULL, NULL);
284 } while (cfd < 0 && errno == EINTR);
287 log_err("Could not accept connection on %s socket. %s (%d)",
288 srv->fdidx[fd].control? "control" : "L2CAP",
289 strerror(errno), errno);
293 assert(!FD_ISSET(cfd, &srv->fdset));
294 assert(!srv->fdidx[cfd].valid);
296 if (!srv->fdidx[fd].control) {
297 /* Get local BD_ADDR */
298 size = sizeof(srv->req_sa);
299 if (getsockname(cfd,(struct sockaddr*)&srv->req_sa,&size) < 0) {
300 log_err("Could not get local BD_ADDR. %s (%d)",
301 strerror(errno), errno);
306 /* Get outgoing MTU */
308 if (getsockopt(cfd,SOL_L2CAP,SO_L2CAP_OMTU,&omtu,&size) < 0) {
309 log_err("Could not get L2CAP OMTU. %s (%d)",
310 strerror(errno), errno);
316 * The maximum size of the L2CAP packet is 65536 bytes.
317 * The minimum L2CAP MTU is 43 bytes. That means we need
318 * 65536 / 43 = ~1524 chunks to transfer maximum packet
319 * size with minimum MTU. The "rsp_cs" field in fd_idx_t
320 * is 11 bit wide that gives us upto 2048 chunks.
323 if (omtu < NG_L2CAP_MTU_MINIMUM) {
324 log_err("L2CAP OMTU is too small (%d bytes)", omtu);
329 memcpy(&srv->req_sa.l2cap_bdaddr, NG_HCI_BDADDR_ANY,
330 sizeof(srv->req_sa.l2cap_bdaddr));
332 omtu = srv->fdidx[fd].omtu;
336 * Allocate buffer. This is an overkill, but we can not know how
337 * big our reply is going to be.
340 rsp = (uint8_t *) calloc(NG_L2CAP_MTU_MAXIMUM, sizeof(rsp[0]));
342 log_crit("Could not allocate response buffer");
347 /* Add client descriptor to the index */
348 FD_SET(cfd, &srv->fdset);
349 if (srv->maxfd < cfd)
351 srv->fdidx[cfd].valid = 1;
352 srv->fdidx[cfd].server = 0;
353 srv->fdidx[cfd].control = srv->fdidx[fd].control;
354 srv->fdidx[cfd].rsp_cs = 0;
355 srv->fdidx[cfd].rsp_size = 0;
356 srv->fdidx[cfd].rsp_limit = 0;
357 srv->fdidx[cfd].omtu = omtu;
358 srv->fdidx[cfd].rsp = rsp;
362 * Process request from the client
366 server_process_request(server_p srv, int32_t fd)
368 sdp_pdu_p pdu = (sdp_pdu_p) srv->req;
371 assert(srv->imtu > 0);
372 assert(srv->req != NULL);
373 assert(FD_ISSET(fd, &srv->fdset));
374 assert(srv->fdidx[fd].valid);
375 assert(!srv->fdidx[fd].server);
376 assert(srv->fdidx[fd].rsp != NULL);
377 assert(srv->fdidx[fd].omtu >= NG_L2CAP_MTU_MINIMUM);
380 len = read(fd, srv->req, srv->imtu);
381 } while (len < 0 && errno == EINTR);
384 log_err("Could not receive SDP request from %s socket. %s (%d)",
385 srv->fdidx[fd].control? "control" : "L2CAP",
386 strerror(errno), errno);
390 log_info("Client on %s socket has disconnected",
391 srv->fdidx[fd].control? "control" : "L2CAP");
395 if (sizeof(*pdu) + (pdu->len = ntohs(pdu->len)) == len) {
397 case SDP_PDU_SERVICE_SEARCH_REQUEST:
398 error = server_prepare_service_search_response(srv, fd);
401 case SDP_PDU_SERVICE_ATTRIBUTE_REQUEST:
402 error = server_prepare_service_attribute_response(srv, fd);
405 case SDP_PDU_SERVICE_SEARCH_ATTRIBUTE_REQUEST:
406 error = server_prepare_service_search_attribute_response(srv, fd);
409 case SDP_PDU_SERVICE_REGISTER_REQUEST:
410 error = server_prepare_service_register_response(srv, fd);
413 case SDP_PDU_SERVICE_UNREGISTER_REQUEST:
414 error = server_prepare_service_unregister_response(srv, fd);
417 case SDP_PDU_SERVICE_CHANGE_REQUEST:
418 error = server_prepare_service_change_response(srv, fd);
422 error = SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX;
426 error = SDP_ERROR_CODE_INVALID_PDU_SIZE;
430 case SDP_PDU_SERVICE_SEARCH_REQUEST:
431 error = server_send_service_search_response(srv, fd);
434 case SDP_PDU_SERVICE_ATTRIBUTE_REQUEST:
435 error = server_send_service_attribute_response(srv, fd);
438 case SDP_PDU_SERVICE_SEARCH_ATTRIBUTE_REQUEST:
439 error = server_send_service_search_attribute_response(srv, fd);
442 case SDP_PDU_SERVICE_REGISTER_REQUEST:
443 error = server_send_service_register_response(srv, fd);
446 case SDP_PDU_SERVICE_UNREGISTER_REQUEST:
447 error = server_send_service_unregister_response(srv, fd);
450 case SDP_PDU_SERVICE_CHANGE_REQUEST:
451 error = server_send_service_change_response(srv, fd);
454 error = SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX;
459 log_err("Could not send SDP response to %s socket, " \
460 "pdu->pid=%d, pdu->tid=%d, error=%d",
461 srv->fdidx[fd].control? "control" : "L2CAP",
462 pdu->pid, ntohs(pdu->tid), error);
464 log_err("Could not process SDP request from %s socket, " \
465 "pdu->pid=%d, pdu->tid=%d, pdu->len=%d, len=%d, " \
467 srv->fdidx[fd].control? "control" : "L2CAP",
468 pdu->pid, ntohs(pdu->tid), pdu->len, len, error);
470 error = server_send_error_response(srv, fd, error);
472 log_err("Could not send SDP error response to %s " \
473 "socket, pdu->pid=%d, pdu->tid=%d, error=%d",
474 srv->fdidx[fd].control? "control" : "L2CAP",
475 pdu->pid, ntohs(pdu->tid), error);
478 /* On error forget response (if any) */
480 srv->fdidx[fd].rsp_cs = 0;
481 srv->fdidx[fd].rsp_size = 0;
482 srv->fdidx[fd].rsp_limit = 0;
489 * Send SDP_Error_Response PDU
493 server_send_error_response(server_p srv, int32_t fd, uint16_t error)
500 } __attribute__ ((packed)) rsp;
502 /* Prepare and send SDP error response */
503 rsp.pdu.pid = SDP_PDU_ERROR_RESPONSE;
504 rsp.pdu.tid = ((sdp_pdu_p)(srv->req))->tid;
505 rsp.pdu.len = htons(sizeof(rsp.error));
506 rsp.error = htons(error);
509 size = write(fd, &rsp, sizeof(rsp));
510 } while (size < 0 && errno == EINTR);
512 return ((size < 0)? errno : 0);
516 * Close descriptor and remove it from index
520 server_close_fd(server_p srv, int32_t fd)
522 provider_p provider = NULL, provider_next = NULL;
524 assert(FD_ISSET(fd, &srv->fdset));
525 assert(srv->fdidx[fd].valid);
529 FD_CLR(fd, &srv->fdset);
530 if (fd == srv->maxfd)
533 if (srv->fdidx[fd].rsp != NULL)
534 free(srv->fdidx[fd].rsp);
536 memset(&srv->fdidx[fd], 0, sizeof(srv->fdidx[fd]));
538 for (provider = provider_get_first();
540 provider = provider_next) {
541 provider_next = provider_get_next(provider);
543 if (provider->fd == fd)
544 provider_unregister(provider);