carp: add carp_group_demote_adj()
[dragonfly.git] / usr.sbin / sdpd / sar.c
1 /* $NetBSD: sar.c,v 1.1 2006/06/19 15:44:56 gdamore Exp $ */
2 /* $DragonFly: src/usr.sbin/sdpd/sar.c,v 1.1 2008/01/06 21:51:30 hasso Exp $ */
3
4 /*
5  * sar.c
6  *
7  * Copyright (c) 2004 Maksim Yevmenkin <m_evmenkin@yahoo.com>
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  * 1. Redistributions of source code must retain the above copyright
14  *    notice, this list of conditions and the following disclaimer.
15  * 2. Redistributions in binary form must reproduce the above copyright
16  *    notice, this list of conditions and the following disclaimer in the
17  *    documentation and/or other materials provided with the distribution.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
20  * 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 AUTHOR OR CONTRIBUTORS BE LIABLE
23  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
24  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
25  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
26  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
27  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
28  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
29  * SUCH DAMAGE.
30  *
31  * $Id: sar.c,v 1.2 2007/11/30 07:39:37 griffin Exp $
32  * $FreeBSD: src/usr.sbin/bluetooth/sdpd/sar.c,v 1.2 2004/02/26 20:44:55 emax Exp $
33  */
34
35 #include <sys/queue.h>
36 #include <sys/uio.h>
37 #include <netinet/in.h>
38 #include <arpa/inet.h>
39 #include <assert.h>
40 #include <bluetooth.h>
41 #include <errno.h>
42 #include <sdp.h>
43 #include <stdio.h> /* for NULL */
44 #include "profile.h"
45 #include "provider.h"
46 #include "server.h"
47
48 /*
49  * Prepare SDP attr/value pair. Check if profile implements the attribute
50  * and if so call the attribute value function.
51  *
52  * uint16 value16       - 3 bytes (attribute)
53  * value                - N bytes (value)
54  */
55
56 static int32_t
57 server_prepare_attr_value_pair(
58                 provider_p const provider, uint16_t attr,
59                 uint8_t *buf, uint8_t const * const eob)
60 {
61         profile_attr_create_p   cf = profile_get_attr(provider->profile, attr);
62         int32_t                 len;
63
64         if (cf == NULL)
65                 return (0); /* no attribute */
66
67         if (buf + 3 > eob)
68                 return (-1);
69
70         SDP_PUT8(SDP_DATA_UINT16, buf);
71         SDP_PUT16(attr, buf);
72
73         len = cf(buf, eob, (uint8_t const *) provider, sizeof(*provider));
74         if (len < 0)
75                 return (-1);
76
77         return (3 + len);
78 }
79
80 /*
81  * seq16 value16        - 3 bytes
82  *      attr value      - 3+ bytes
83  *      [ attr value ]
84  */
85
86 int32_t
87 server_prepare_attr_list(provider_p const provider,
88                 uint8_t const *req, uint8_t const * const req_end,
89                 uint8_t *rsp, uint8_t const * const rsp_end)
90 {
91         uint8_t *ptr = rsp + 3;
92         int32_t  type, hi, lo, len;
93
94         if (ptr > rsp_end)
95                 return (-1);
96
97         while (req < req_end) {
98                 SDP_GET8(type, req);
99
100                 switch (type) {
101                 case SDP_DATA_UINT16:
102                         if (req + 2 > req_end)
103                                 return (-1);
104
105                         SDP_GET16(lo, req);
106                         hi = lo;
107                         break;
108
109                 case SDP_DATA_UINT32:
110                         if (req + 4 > req_end)
111                                 return (-1);
112
113                         SDP_GET16(lo, req);
114                         SDP_GET16(hi, req);
115                         break;
116
117                 default:
118                         return (-1);
119                         /* NOT REACHED */
120                 }
121
122                 for (; lo <= hi; lo ++) {
123                         len = server_prepare_attr_value_pair(provider, lo, ptr, rsp_end);
124                         if (len < 0)
125                                 return (-1);
126
127                         ptr += len;
128                 }
129         }
130
131         len = ptr - rsp; /* we put this much bytes in rsp */
132
133         /* Fix SEQ16 header for the rsp */
134         SDP_PUT8(SDP_DATA_SEQ16, rsp);
135         SDP_PUT16(len - 3, rsp);
136
137         return (len);
138 }
139
140 /*
141  * Prepare SDP Service Attribute Response
142  */
143
144 int32_t
145 server_prepare_service_attribute_response(server_p srv, int32_t fd)
146 {
147         uint8_t const   *req = srv->req + sizeof(sdp_pdu_t);
148         uint8_t const   *req_end = req + ((sdp_pdu_p)(srv->req))->len;
149         uint8_t         *rsp = srv->fdidx[fd].rsp;
150         uint8_t const   *rsp_end = rsp + L2CAP_MTU_MAXIMUM;
151
152         uint8_t const   *ptr = NULL;
153         provider_t      *provider = NULL;
154         uint32_t         handle;
155         int32_t          type, rsp_limit, aidlen, cslen, cs;
156
157         /*
158          * Minimal Service Attribute Request request
159          *
160          * value32              - 4 bytes ServiceRecordHandle
161          * value16              - 2 bytes MaximumAttributeByteCount
162          * seq8 len8            - 2 bytes
163          *      uint16 value16  - 3 bytes AttributeIDList
164          * value8               - 1 byte  ContinuationState
165          */
166
167         if (req_end - req < 12)
168                 return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
169
170         /* Get ServiceRecordHandle and MaximumAttributeByteCount */
171         SDP_GET32(handle, req);
172         SDP_GET16(rsp_limit, req);
173         if (rsp_limit <= 0)
174                 return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
175
176         /* Get size of AttributeIDList */
177         aidlen = 0;
178         SDP_GET8(type, req);
179         switch (type) {
180         case SDP_DATA_SEQ8:
181                 SDP_GET8(aidlen, req);
182                 break;
183
184         case SDP_DATA_SEQ16:
185                 SDP_GET16(aidlen, req);
186                 break;
187
188         case SDP_DATA_SEQ32:
189                 SDP_GET32(aidlen, req);
190                 break;
191         }
192         if (aidlen <= 0)
193                 return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
194
195         ptr = (uint8_t const *) req + aidlen;
196
197         /* Get ContinuationState */
198         if (ptr + 1 > req_end)
199                 return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
200
201         SDP_GET8(cslen, ptr);
202         if (cslen != 0) {
203                 if (cslen != 2 || req_end - ptr != 2)
204                         return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
205
206                 SDP_GET16(cs, ptr);
207         } else
208                 cs = 0;
209
210         /* Process the request. First, check continuation state */
211         if (srv->fdidx[fd].rsp_cs != cs)
212                 return (SDP_ERROR_CODE_INVALID_CONTINUATION_STATE);
213         if (srv->fdidx[fd].rsp_size > 0)
214                 return (0);
215
216         /* Lookup record handle */
217         if ((provider = provider_by_handle(handle)) == NULL)
218                 return (SDP_ERROR_CODE_INVALID_SERVICE_RECORD_HANDLE);
219
220         /*
221          * Service Attribute Response format
222          *
223          * value16              - 2 bytes  AttributeListByteCount (not incl.)
224          * seq8 len16           - 3 bytes
225          *      attr value      - 3+ bytes AttributeList
226          *      [ attr value ]
227          */
228
229         cs = server_prepare_attr_list(provider, req, req+aidlen, rsp, rsp_end);
230         if (cs < 0)
231                 return (SDP_ERROR_CODE_INSUFFICIENT_RESOURCES);
232
233         /* Set reply size (not counting PDU header and continuation state) */
234         srv->fdidx[fd].rsp_limit = srv->fdidx[fd].omtu - sizeof(sdp_pdu_t) - 2;
235         if (srv->fdidx[fd].rsp_limit > rsp_limit)
236                 srv->fdidx[fd].rsp_limit = rsp_limit;
237
238         srv->fdidx[fd].rsp_size = cs;
239         srv->fdidx[fd].rsp_cs = 0;
240
241         return (0);
242 }
243
244 /*
245  * Send SDP Service [Search] Attribute Response
246  */
247
248 int32_t
249 server_send_service_attribute_response(server_p srv, int32_t fd)
250 {
251         uint8_t         *rsp = srv->fdidx[fd].rsp + srv->fdidx[fd].rsp_cs;
252         uint8_t         *rsp_end = srv->fdidx[fd].rsp + srv->fdidx[fd].rsp_size;
253
254         struct iovec    iov[4];
255         sdp_pdu_t       pdu;
256         uint16_t        bcount;
257         uint8_t         cs[3];
258         int32_t         size;
259
260         /* First update continuation state  (assume we will send all data) */
261         size = rsp_end - rsp;
262         srv->fdidx[fd].rsp_cs += size;
263
264         if (size + 1 > srv->fdidx[fd].rsp_limit) {
265                 /*
266                  * We need to split out response. Add 3 more bytes for the
267                  * continuation state and move rsp_end and rsp_cs backwards.
268                  */
269
270                 while ((rsp_end - rsp) + 3 > srv->fdidx[fd].rsp_limit) {
271                         rsp_end --;
272                         srv->fdidx[fd].rsp_cs --;
273                 }
274
275                 cs[0] = 2;
276                 cs[1] = srv->fdidx[fd].rsp_cs >> 8;
277                 cs[2] = srv->fdidx[fd].rsp_cs & 0xff;
278         } else
279                 cs[0] = 0;
280
281         assert(rsp_end >= rsp);
282
283         bcount = rsp_end - rsp;
284
285         if (((sdp_pdu_p)(srv->req))->pid == SDP_PDU_SERVICE_ATTRIBUTE_REQUEST)
286                 pdu.pid = SDP_PDU_SERVICE_ATTRIBUTE_RESPONSE;
287         else
288                 pdu.pid = SDP_PDU_SERVICE_SEARCH_ATTRIBUTE_RESPONSE;
289
290         pdu.tid = ((sdp_pdu_p)(srv->req))->tid;
291         pdu.len = htons(sizeof(bcount) + bcount + 1 + cs[0]);
292
293         bcount = htons(bcount);
294
295         iov[0].iov_base = &pdu;
296         iov[0].iov_len = sizeof(pdu);
297
298         iov[1].iov_base = &bcount;
299         iov[1].iov_len = sizeof(bcount);
300
301         iov[2].iov_base = rsp;
302         iov[2].iov_len = rsp_end - rsp;
303
304         iov[3].iov_base = cs;
305         iov[3].iov_len = 1 + cs[0];
306
307         do {
308                 size = writev(fd, (struct iovec const *) &iov, sizeof(iov)/sizeof(iov[0]));
309         } while (size < 0 && errno == EINTR);
310
311         /* Check if we have sent (or failed to sent) last response chunk */
312         if (srv->fdidx[fd].rsp_cs == srv->fdidx[fd].rsp_size) {
313                 srv->fdidx[fd].rsp_cs = 0;
314                 srv->fdidx[fd].rsp_size = 0;
315                 srv->fdidx[fd].rsp_limit = 0;
316         }
317
318         return ((size < 0)? errno : 0);
319 }