mirror of
https://github.com/qemu/qemu.git
synced 2024-12-05 17:53:36 +08:00
93bfef4c6e
QEMU exposes its version to the guest's hardware and in some cases that is wrong (e.g. Windows prints messages about driver updates when you switch the QEMU version). There is a new field now on the struct QEmuMachine, hw_version, which may contain the version that the specific machine should report. If that field is set, then that machine will report that version to the guest. Signed-off-by: Crístian Viana <vianac@linux.vnet.ibm.com> Signed-off-by: Anthony Liguori <aliguori@us.ibm.com>
968 lines
29 KiB
C
968 lines
29 KiB
C
/*
|
|
* Service Discover Protocol server for QEMU L2CAP devices
|
|
*
|
|
* Copyright (C) 2008 Andrzej Zaborowski <balrog@zabor.org>
|
|
*
|
|
* This program is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU General Public License as
|
|
* published by the Free Software Foundation; either version 2 of
|
|
* the License, or (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License along
|
|
* with this program; if not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "qemu-common.h"
|
|
#include "bt.h"
|
|
|
|
struct bt_l2cap_sdp_state_s {
|
|
struct bt_l2cap_conn_params_s *channel;
|
|
|
|
struct sdp_service_record_s {
|
|
int match;
|
|
|
|
int *uuid;
|
|
int uuids;
|
|
struct sdp_service_attribute_s {
|
|
int match;
|
|
|
|
int attribute_id;
|
|
int len;
|
|
void *pair;
|
|
} *attribute_list;
|
|
int attributes;
|
|
} *service_list;
|
|
int services;
|
|
};
|
|
|
|
static ssize_t sdp_datalen(const uint8_t **element, ssize_t *left)
|
|
{
|
|
size_t len = *(*element) ++ & SDP_DSIZE_MASK;
|
|
|
|
if (!*left)
|
|
return -1;
|
|
(*left) --;
|
|
|
|
if (len < SDP_DSIZE_NEXT1)
|
|
return 1 << len;
|
|
else if (len == SDP_DSIZE_NEXT1) {
|
|
if (*left < 1)
|
|
return -1;
|
|
(*left) --;
|
|
|
|
return *(*element) ++;
|
|
} else if (len == SDP_DSIZE_NEXT2) {
|
|
if (*left < 2)
|
|
return -1;
|
|
(*left) -= 2;
|
|
|
|
len = (*(*element) ++) << 8;
|
|
return len | (*(*element) ++);
|
|
} else {
|
|
if (*left < 4)
|
|
return -1;
|
|
(*left) -= 4;
|
|
|
|
len = (*(*element) ++) << 24;
|
|
len |= (*(*element) ++) << 16;
|
|
len |= (*(*element) ++) << 8;
|
|
return len | (*(*element) ++);
|
|
}
|
|
}
|
|
|
|
static const uint8_t bt_base_uuid[12] = {
|
|
0x00, 0x00, 0x10, 0x00, 0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb,
|
|
};
|
|
|
|
static int sdp_uuid_match(struct sdp_service_record_s *record,
|
|
const uint8_t *uuid, ssize_t datalen)
|
|
{
|
|
int *lo, hi, val;
|
|
|
|
if (datalen == 16 || datalen == 4) {
|
|
if (datalen == 16 && memcmp(uuid + 4, bt_base_uuid, 12))
|
|
return 0;
|
|
|
|
if (uuid[0] | uuid[1])
|
|
return 0;
|
|
uuid += 2;
|
|
}
|
|
|
|
val = (uuid[0] << 8) | uuid[1];
|
|
lo = record->uuid;
|
|
hi = record->uuids;
|
|
while (hi >>= 1)
|
|
if (lo[hi] <= val)
|
|
lo += hi;
|
|
|
|
return *lo == val;
|
|
}
|
|
|
|
#define CONTINUATION_PARAM_SIZE (1 + sizeof(int))
|
|
#define MAX_PDU_OUT_SIZE 96 /* Arbitrary */
|
|
#define PDU_HEADER_SIZE 5
|
|
#define MAX_RSP_PARAM_SIZE (MAX_PDU_OUT_SIZE - PDU_HEADER_SIZE - \
|
|
CONTINUATION_PARAM_SIZE)
|
|
|
|
static int sdp_svc_match(struct bt_l2cap_sdp_state_s *sdp,
|
|
const uint8_t **req, ssize_t *len)
|
|
{
|
|
size_t datalen;
|
|
int i;
|
|
|
|
if ((**req & ~SDP_DSIZE_MASK) != SDP_DTYPE_UUID)
|
|
return 1;
|
|
|
|
datalen = sdp_datalen(req, len);
|
|
if (datalen != 2 && datalen != 4 && datalen != 16)
|
|
return 1;
|
|
|
|
for (i = 0; i < sdp->services; i ++)
|
|
if (sdp_uuid_match(&sdp->service_list[i], *req, datalen))
|
|
sdp->service_list[i].match = 1;
|
|
|
|
(*req) += datalen;
|
|
(*len) -= datalen;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static ssize_t sdp_svc_search(struct bt_l2cap_sdp_state_s *sdp,
|
|
uint8_t *rsp, const uint8_t *req, ssize_t len)
|
|
{
|
|
ssize_t seqlen;
|
|
int i, count, start, end, max;
|
|
int32_t handle;
|
|
|
|
/* Perform the search */
|
|
for (i = 0; i < sdp->services; i ++)
|
|
sdp->service_list[i].match = 0;
|
|
|
|
if (len < 1)
|
|
return -SDP_INVALID_SYNTAX;
|
|
if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
|
|
seqlen = sdp_datalen(&req, &len);
|
|
if (seqlen < 3 || len < seqlen)
|
|
return -SDP_INVALID_SYNTAX;
|
|
len -= seqlen;
|
|
|
|
while (seqlen)
|
|
if (sdp_svc_match(sdp, &req, &seqlen))
|
|
return -SDP_INVALID_SYNTAX;
|
|
} else if (sdp_svc_match(sdp, &req, &seqlen))
|
|
return -SDP_INVALID_SYNTAX;
|
|
|
|
if (len < 3)
|
|
return -SDP_INVALID_SYNTAX;
|
|
max = (req[0] << 8) | req[1];
|
|
req += 2;
|
|
len -= 2;
|
|
|
|
if (*req) {
|
|
if (len <= sizeof(int))
|
|
return -SDP_INVALID_SYNTAX;
|
|
len -= sizeof(int);
|
|
memcpy(&start, req + 1, sizeof(int));
|
|
} else
|
|
start = 0;
|
|
|
|
if (len > 1)
|
|
return -SDP_INVALID_SYNTAX;
|
|
|
|
/* Output the results */
|
|
len = 4;
|
|
count = 0;
|
|
end = start;
|
|
for (i = 0; i < sdp->services; i ++)
|
|
if (sdp->service_list[i].match) {
|
|
if (count >= start && count < max && len + 4 < MAX_RSP_PARAM_SIZE) {
|
|
handle = i;
|
|
memcpy(rsp + len, &handle, 4);
|
|
len += 4;
|
|
end = count + 1;
|
|
}
|
|
|
|
count ++;
|
|
}
|
|
|
|
rsp[0] = count >> 8;
|
|
rsp[1] = count & 0xff;
|
|
rsp[2] = (end - start) >> 8;
|
|
rsp[3] = (end - start) & 0xff;
|
|
|
|
if (end < count) {
|
|
rsp[len ++] = sizeof(int);
|
|
memcpy(rsp + len, &end, sizeof(int));
|
|
len += 4;
|
|
} else
|
|
rsp[len ++] = 0;
|
|
|
|
return len;
|
|
}
|
|
|
|
static int sdp_attr_match(struct sdp_service_record_s *record,
|
|
const uint8_t **req, ssize_t *len)
|
|
{
|
|
int i, start, end;
|
|
|
|
if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_2)) {
|
|
(*req) ++;
|
|
if (*len < 3)
|
|
return 1;
|
|
|
|
start = (*(*req) ++) << 8;
|
|
start |= *(*req) ++;
|
|
end = start;
|
|
*len -= 3;
|
|
} else if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_4)) {
|
|
(*req) ++;
|
|
if (*len < 5)
|
|
return 1;
|
|
|
|
start = (*(*req) ++) << 8;
|
|
start |= *(*req) ++;
|
|
end = (*(*req) ++) << 8;
|
|
end |= *(*req) ++;
|
|
*len -= 5;
|
|
} else
|
|
return 1;
|
|
|
|
for (i = 0; i < record->attributes; i ++)
|
|
if (record->attribute_list[i].attribute_id >= start &&
|
|
record->attribute_list[i].attribute_id <= end)
|
|
record->attribute_list[i].match = 1;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static ssize_t sdp_attr_get(struct bt_l2cap_sdp_state_s *sdp,
|
|
uint8_t *rsp, const uint8_t *req, ssize_t len)
|
|
{
|
|
ssize_t seqlen;
|
|
int i, start, end, max;
|
|
int32_t handle;
|
|
struct sdp_service_record_s *record;
|
|
uint8_t *lst;
|
|
|
|
/* Perform the search */
|
|
if (len < 7)
|
|
return -SDP_INVALID_SYNTAX;
|
|
memcpy(&handle, req, 4);
|
|
req += 4;
|
|
len -= 4;
|
|
|
|
if (handle < 0 || handle > sdp->services)
|
|
return -SDP_INVALID_RECORD_HANDLE;
|
|
record = &sdp->service_list[handle];
|
|
|
|
for (i = 0; i < record->attributes; i ++)
|
|
record->attribute_list[i].match = 0;
|
|
|
|
max = (req[0] << 8) | req[1];
|
|
req += 2;
|
|
len -= 2;
|
|
if (max < 0x0007)
|
|
return -SDP_INVALID_SYNTAX;
|
|
|
|
if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
|
|
seqlen = sdp_datalen(&req, &len);
|
|
if (seqlen < 3 || len < seqlen)
|
|
return -SDP_INVALID_SYNTAX;
|
|
len -= seqlen;
|
|
|
|
while (seqlen)
|
|
if (sdp_attr_match(record, &req, &seqlen))
|
|
return -SDP_INVALID_SYNTAX;
|
|
} else if (sdp_attr_match(record, &req, &seqlen))
|
|
return -SDP_INVALID_SYNTAX;
|
|
|
|
if (len < 1)
|
|
return -SDP_INVALID_SYNTAX;
|
|
|
|
if (*req) {
|
|
if (len <= sizeof(int))
|
|
return -SDP_INVALID_SYNTAX;
|
|
len -= sizeof(int);
|
|
memcpy(&start, req + 1, sizeof(int));
|
|
} else
|
|
start = 0;
|
|
|
|
if (len > 1)
|
|
return -SDP_INVALID_SYNTAX;
|
|
|
|
/* Output the results */
|
|
lst = rsp + 2;
|
|
max = MIN(max, MAX_RSP_PARAM_SIZE);
|
|
len = 3 - start;
|
|
end = 0;
|
|
for (i = 0; i < record->attributes; i ++)
|
|
if (record->attribute_list[i].match) {
|
|
if (len >= 0 && len + record->attribute_list[i].len < max) {
|
|
memcpy(lst + len, record->attribute_list[i].pair,
|
|
record->attribute_list[i].len);
|
|
end = len + record->attribute_list[i].len;
|
|
}
|
|
len += record->attribute_list[i].len;
|
|
}
|
|
if (0 >= start) {
|
|
lst[0] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;
|
|
lst[1] = (len + start - 3) >> 8;
|
|
lst[2] = (len + start - 3) & 0xff;
|
|
}
|
|
|
|
rsp[0] = end >> 8;
|
|
rsp[1] = end & 0xff;
|
|
|
|
if (end < len) {
|
|
len = end + start;
|
|
lst[end ++] = sizeof(int);
|
|
memcpy(lst + end, &len, sizeof(int));
|
|
end += sizeof(int);
|
|
} else
|
|
lst[end ++] = 0;
|
|
|
|
return end + 2;
|
|
}
|
|
|
|
static int sdp_svc_attr_match(struct bt_l2cap_sdp_state_s *sdp,
|
|
const uint8_t **req, ssize_t *len)
|
|
{
|
|
int i, j, start, end;
|
|
struct sdp_service_record_s *record;
|
|
|
|
if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_2)) {
|
|
(*req) ++;
|
|
if (*len < 3)
|
|
return 1;
|
|
|
|
start = (*(*req) ++) << 8;
|
|
start |= *(*req) ++;
|
|
end = start;
|
|
*len -= 3;
|
|
} else if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_4)) {
|
|
(*req) ++;
|
|
if (*len < 5)
|
|
return 1;
|
|
|
|
start = (*(*req) ++) << 8;
|
|
start |= *(*req) ++;
|
|
end = (*(*req) ++) << 8;
|
|
end |= *(*req) ++;
|
|
*len -= 5;
|
|
} else
|
|
return 1;
|
|
|
|
for (i = 0; i < sdp->services; i ++)
|
|
if ((record = &sdp->service_list[i])->match)
|
|
for (j = 0; j < record->attributes; j ++)
|
|
if (record->attribute_list[j].attribute_id >= start &&
|
|
record->attribute_list[j].attribute_id <= end)
|
|
record->attribute_list[j].match = 1;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static ssize_t sdp_svc_search_attr_get(struct bt_l2cap_sdp_state_s *sdp,
|
|
uint8_t *rsp, const uint8_t *req, ssize_t len)
|
|
{
|
|
ssize_t seqlen;
|
|
int i, j, start, end, max;
|
|
struct sdp_service_record_s *record;
|
|
uint8_t *lst;
|
|
|
|
/* Perform the search */
|
|
for (i = 0; i < sdp->services; i ++) {
|
|
sdp->service_list[i].match = 0;
|
|
for (j = 0; j < sdp->service_list[i].attributes; j ++)
|
|
sdp->service_list[i].attribute_list[j].match = 0;
|
|
}
|
|
|
|
if (len < 1)
|
|
return -SDP_INVALID_SYNTAX;
|
|
if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
|
|
seqlen = sdp_datalen(&req, &len);
|
|
if (seqlen < 3 || len < seqlen)
|
|
return -SDP_INVALID_SYNTAX;
|
|
len -= seqlen;
|
|
|
|
while (seqlen)
|
|
if (sdp_svc_match(sdp, &req, &seqlen))
|
|
return -SDP_INVALID_SYNTAX;
|
|
} else if (sdp_svc_match(sdp, &req, &seqlen))
|
|
return -SDP_INVALID_SYNTAX;
|
|
|
|
if (len < 3)
|
|
return -SDP_INVALID_SYNTAX;
|
|
max = (req[0] << 8) | req[1];
|
|
req += 2;
|
|
len -= 2;
|
|
if (max < 0x0007)
|
|
return -SDP_INVALID_SYNTAX;
|
|
|
|
if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
|
|
seqlen = sdp_datalen(&req, &len);
|
|
if (seqlen < 3 || len < seqlen)
|
|
return -SDP_INVALID_SYNTAX;
|
|
len -= seqlen;
|
|
|
|
while (seqlen)
|
|
if (sdp_svc_attr_match(sdp, &req, &seqlen))
|
|
return -SDP_INVALID_SYNTAX;
|
|
} else if (sdp_svc_attr_match(sdp, &req, &seqlen))
|
|
return -SDP_INVALID_SYNTAX;
|
|
|
|
if (len < 1)
|
|
return -SDP_INVALID_SYNTAX;
|
|
|
|
if (*req) {
|
|
if (len <= sizeof(int))
|
|
return -SDP_INVALID_SYNTAX;
|
|
len -= sizeof(int);
|
|
memcpy(&start, req + 1, sizeof(int));
|
|
} else
|
|
start = 0;
|
|
|
|
if (len > 1)
|
|
return -SDP_INVALID_SYNTAX;
|
|
|
|
/* Output the results */
|
|
/* This assumes empty attribute lists are never to be returned even
|
|
* for matching Service Records. In practice this shouldn't happen
|
|
* as the requestor will usually include the always present
|
|
* ServiceRecordHandle AttributeID in AttributeIDList. */
|
|
lst = rsp + 2;
|
|
max = MIN(max, MAX_RSP_PARAM_SIZE);
|
|
len = 3 - start;
|
|
end = 0;
|
|
for (i = 0; i < sdp->services; i ++)
|
|
if ((record = &sdp->service_list[i])->match) {
|
|
len += 3;
|
|
seqlen = len;
|
|
for (j = 0; j < record->attributes; j ++)
|
|
if (record->attribute_list[j].match) {
|
|
if (len >= 0)
|
|
if (len + record->attribute_list[j].len < max) {
|
|
memcpy(lst + len, record->attribute_list[j].pair,
|
|
record->attribute_list[j].len);
|
|
end = len + record->attribute_list[j].len;
|
|
}
|
|
len += record->attribute_list[j].len;
|
|
}
|
|
if (seqlen == len)
|
|
len -= 3;
|
|
else if (seqlen >= 3 && seqlen < max) {
|
|
lst[seqlen - 3] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;
|
|
lst[seqlen - 2] = (len - seqlen) >> 8;
|
|
lst[seqlen - 1] = (len - seqlen) & 0xff;
|
|
}
|
|
}
|
|
if (len == 3 - start)
|
|
len -= 3;
|
|
else if (0 >= start) {
|
|
lst[0] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;
|
|
lst[1] = (len + start - 3) >> 8;
|
|
lst[2] = (len + start - 3) & 0xff;
|
|
}
|
|
|
|
rsp[0] = end >> 8;
|
|
rsp[1] = end & 0xff;
|
|
|
|
if (end < len) {
|
|
len = end + start;
|
|
lst[end ++] = sizeof(int);
|
|
memcpy(lst + end, &len, sizeof(int));
|
|
end += sizeof(int);
|
|
} else
|
|
lst[end ++] = 0;
|
|
|
|
return end + 2;
|
|
}
|
|
|
|
static void bt_l2cap_sdp_sdu_in(void *opaque, const uint8_t *data, int len)
|
|
{
|
|
struct bt_l2cap_sdp_state_s *sdp = opaque;
|
|
enum bt_sdp_cmd pdu_id;
|
|
uint8_t rsp[MAX_PDU_OUT_SIZE - PDU_HEADER_SIZE], *sdu_out;
|
|
int transaction_id, plen;
|
|
int err = 0;
|
|
int rsp_len = 0;
|
|
|
|
if (len < 5) {
|
|
fprintf(stderr, "%s: short SDP PDU (%iB).\n", __FUNCTION__, len);
|
|
return;
|
|
}
|
|
|
|
pdu_id = *data ++;
|
|
transaction_id = (data[0] << 8) | data[1];
|
|
plen = (data[2] << 8) | data[3];
|
|
data += 4;
|
|
len -= 5;
|
|
|
|
if (len != plen) {
|
|
fprintf(stderr, "%s: wrong SDP PDU length (%iB != %iB).\n",
|
|
__FUNCTION__, plen, len);
|
|
err = SDP_INVALID_PDU_SIZE;
|
|
goto respond;
|
|
}
|
|
|
|
switch (pdu_id) {
|
|
case SDP_SVC_SEARCH_REQ:
|
|
rsp_len = sdp_svc_search(sdp, rsp, data, len);
|
|
pdu_id = SDP_SVC_SEARCH_RSP;
|
|
break;
|
|
|
|
case SDP_SVC_ATTR_REQ:
|
|
rsp_len = sdp_attr_get(sdp, rsp, data, len);
|
|
pdu_id = SDP_SVC_ATTR_RSP;
|
|
break;
|
|
|
|
case SDP_SVC_SEARCH_ATTR_REQ:
|
|
rsp_len = sdp_svc_search_attr_get(sdp, rsp, data, len);
|
|
pdu_id = SDP_SVC_SEARCH_ATTR_RSP;
|
|
break;
|
|
|
|
case SDP_ERROR_RSP:
|
|
case SDP_SVC_ATTR_RSP:
|
|
case SDP_SVC_SEARCH_RSP:
|
|
case SDP_SVC_SEARCH_ATTR_RSP:
|
|
default:
|
|
fprintf(stderr, "%s: unexpected SDP PDU ID %02x.\n",
|
|
__FUNCTION__, pdu_id);
|
|
err = SDP_INVALID_SYNTAX;
|
|
break;
|
|
}
|
|
|
|
if (rsp_len < 0) {
|
|
err = -rsp_len;
|
|
rsp_len = 0;
|
|
}
|
|
|
|
respond:
|
|
if (err) {
|
|
pdu_id = SDP_ERROR_RSP;
|
|
rsp[rsp_len ++] = err >> 8;
|
|
rsp[rsp_len ++] = err & 0xff;
|
|
}
|
|
|
|
sdu_out = sdp->channel->sdu_out(sdp->channel, rsp_len + PDU_HEADER_SIZE);
|
|
|
|
sdu_out[0] = pdu_id;
|
|
sdu_out[1] = transaction_id >> 8;
|
|
sdu_out[2] = transaction_id & 0xff;
|
|
sdu_out[3] = rsp_len >> 8;
|
|
sdu_out[4] = rsp_len & 0xff;
|
|
memcpy(sdu_out + PDU_HEADER_SIZE, rsp, rsp_len);
|
|
|
|
sdp->channel->sdu_submit(sdp->channel);
|
|
}
|
|
|
|
static void bt_l2cap_sdp_close_ch(void *opaque)
|
|
{
|
|
struct bt_l2cap_sdp_state_s *sdp = opaque;
|
|
int i;
|
|
|
|
for (i = 0; i < sdp->services; i ++) {
|
|
g_free(sdp->service_list[i].attribute_list->pair);
|
|
g_free(sdp->service_list[i].attribute_list);
|
|
g_free(sdp->service_list[i].uuid);
|
|
}
|
|
g_free(sdp->service_list);
|
|
g_free(sdp);
|
|
}
|
|
|
|
struct sdp_def_service_s {
|
|
uint16_t class_uuid;
|
|
struct sdp_def_attribute_s {
|
|
uint16_t id;
|
|
struct sdp_def_data_element_s {
|
|
uint8_t type;
|
|
union {
|
|
uint32_t uint;
|
|
const char *str;
|
|
struct sdp_def_data_element_s *list;
|
|
} value;
|
|
} data;
|
|
} attributes[];
|
|
};
|
|
|
|
/* Calculate a safe byte count to allocate that will store the given
|
|
* element, at the same time count elements of a UUID type. */
|
|
static int sdp_attr_max_size(struct sdp_def_data_element_s *element,
|
|
int *uuids)
|
|
{
|
|
int type = element->type & ~SDP_DSIZE_MASK;
|
|
int len;
|
|
|
|
if (type == SDP_DTYPE_UINT || type == SDP_DTYPE_UUID ||
|
|
type == SDP_DTYPE_BOOL) {
|
|
if (type == SDP_DTYPE_UUID)
|
|
(*uuids) ++;
|
|
return 1 + (1 << (element->type & SDP_DSIZE_MASK));
|
|
}
|
|
|
|
if (type == SDP_DTYPE_STRING || type == SDP_DTYPE_URL) {
|
|
if (element->type & SDP_DSIZE_MASK) {
|
|
for (len = 0; element->value.str[len] |
|
|
element->value.str[len + 1]; len ++);
|
|
return len;
|
|
} else
|
|
return 2 + strlen(element->value.str);
|
|
}
|
|
|
|
if (type != SDP_DTYPE_SEQ)
|
|
exit(-1);
|
|
len = 2;
|
|
element = element->value.list;
|
|
while (element->type)
|
|
len += sdp_attr_max_size(element ++, uuids);
|
|
if (len > 255)
|
|
exit (-1);
|
|
|
|
return len;
|
|
}
|
|
|
|
static int sdp_attr_write(uint8_t *data,
|
|
struct sdp_def_data_element_s *element, int **uuid)
|
|
{
|
|
int type = element->type & ~SDP_DSIZE_MASK;
|
|
int len = 0;
|
|
|
|
if (type == SDP_DTYPE_UINT || type == SDP_DTYPE_BOOL) {
|
|
data[len ++] = element->type;
|
|
if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_1)
|
|
data[len ++] = (element->value.uint >> 0) & 0xff;
|
|
else if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_2) {
|
|
data[len ++] = (element->value.uint >> 8) & 0xff;
|
|
data[len ++] = (element->value.uint >> 0) & 0xff;
|
|
} else if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_4) {
|
|
data[len ++] = (element->value.uint >> 24) & 0xff;
|
|
data[len ++] = (element->value.uint >> 16) & 0xff;
|
|
data[len ++] = (element->value.uint >> 8) & 0xff;
|
|
data[len ++] = (element->value.uint >> 0) & 0xff;
|
|
}
|
|
|
|
return len;
|
|
}
|
|
|
|
if (type == SDP_DTYPE_UUID) {
|
|
*(*uuid) ++ = element->value.uint;
|
|
|
|
data[len ++] = element->type;
|
|
data[len ++] = (element->value.uint >> 24) & 0xff;
|
|
data[len ++] = (element->value.uint >> 16) & 0xff;
|
|
data[len ++] = (element->value.uint >> 8) & 0xff;
|
|
data[len ++] = (element->value.uint >> 0) & 0xff;
|
|
memcpy(data + len, bt_base_uuid, 12);
|
|
|
|
return len + 12;
|
|
}
|
|
|
|
data[0] = type | SDP_DSIZE_NEXT1;
|
|
if (type == SDP_DTYPE_STRING || type == SDP_DTYPE_URL) {
|
|
if (element->type & SDP_DSIZE_MASK)
|
|
for (len = 0; element->value.str[len] |
|
|
element->value.str[len + 1]; len ++);
|
|
else
|
|
len = strlen(element->value.str);
|
|
memcpy(data + 2, element->value.str, data[1] = len);
|
|
|
|
return len + 2;
|
|
}
|
|
|
|
len = 2;
|
|
element = element->value.list;
|
|
while (element->type)
|
|
len += sdp_attr_write(data + len, element ++, uuid);
|
|
data[1] = len - 2;
|
|
|
|
return len;
|
|
}
|
|
|
|
static int sdp_attributeid_compare(const struct sdp_service_attribute_s *a,
|
|
const struct sdp_service_attribute_s *b)
|
|
{
|
|
return (int) b->attribute_id - a->attribute_id;
|
|
}
|
|
|
|
static int sdp_uuid_compare(const int *a, const int *b)
|
|
{
|
|
return *a - *b;
|
|
}
|
|
|
|
static void sdp_service_record_build(struct sdp_service_record_s *record,
|
|
struct sdp_def_service_s *def, int handle)
|
|
{
|
|
int len = 0;
|
|
uint8_t *data;
|
|
int *uuid;
|
|
|
|
record->uuids = 0;
|
|
while (def->attributes[record->attributes].data.type) {
|
|
len += 3;
|
|
len += sdp_attr_max_size(&def->attributes[record->attributes ++].data,
|
|
&record->uuids);
|
|
}
|
|
record->uuids = 1 << ffs(record->uuids - 1);
|
|
record->attribute_list =
|
|
g_malloc0(record->attributes * sizeof(*record->attribute_list));
|
|
record->uuid =
|
|
g_malloc0(record->uuids * sizeof(*record->uuid));
|
|
data = g_malloc(len);
|
|
|
|
record->attributes = 0;
|
|
uuid = record->uuid;
|
|
while (def->attributes[record->attributes].data.type) {
|
|
record->attribute_list[record->attributes].pair = data;
|
|
|
|
len = 0;
|
|
data[len ++] = SDP_DTYPE_UINT | SDP_DSIZE_2;
|
|
data[len ++] = def->attributes[record->attributes].id >> 8;
|
|
data[len ++] = def->attributes[record->attributes].id & 0xff;
|
|
len += sdp_attr_write(data + len,
|
|
&def->attributes[record->attributes].data, &uuid);
|
|
|
|
/* Special case: assign a ServiceRecordHandle in sequence */
|
|
if (def->attributes[record->attributes].id == SDP_ATTR_RECORD_HANDLE)
|
|
def->attributes[record->attributes].data.value.uint = handle;
|
|
/* Note: we could also assign a ServiceDescription based on
|
|
* sdp->device.device->lmp_name. */
|
|
|
|
record->attribute_list[record->attributes ++].len = len;
|
|
data += len;
|
|
}
|
|
|
|
/* Sort the attribute list by the AttributeID */
|
|
qsort(record->attribute_list, record->attributes,
|
|
sizeof(*record->attribute_list),
|
|
(void *) sdp_attributeid_compare);
|
|
/* Sort the searchable UUIDs list for bisection */
|
|
qsort(record->uuid, record->uuids,
|
|
sizeof(*record->uuid),
|
|
(void *) sdp_uuid_compare);
|
|
}
|
|
|
|
static void sdp_service_db_build(struct bt_l2cap_sdp_state_s *sdp,
|
|
struct sdp_def_service_s **service)
|
|
{
|
|
sdp->services = 0;
|
|
while (service[sdp->services])
|
|
sdp->services ++;
|
|
sdp->service_list =
|
|
g_malloc0(sdp->services * sizeof(*sdp->service_list));
|
|
|
|
sdp->services = 0;
|
|
while (*service) {
|
|
sdp_service_record_build(&sdp->service_list[sdp->services],
|
|
*service, sdp->services);
|
|
service ++;
|
|
sdp->services ++;
|
|
}
|
|
}
|
|
|
|
#define LAST { .type = 0 }
|
|
#define SERVICE(name, attrs) \
|
|
static struct sdp_def_service_s glue(glue(sdp_service_, name), _s) = { \
|
|
.attributes = { attrs { .data = LAST } }, \
|
|
};
|
|
#define ATTRIBUTE(attrid, val) { .id = glue(SDP_ATTR_, attrid), .data = val },
|
|
#define UINT8(val) { \
|
|
.type = SDP_DTYPE_UINT | SDP_DSIZE_1, \
|
|
.value.uint = val, \
|
|
},
|
|
#define UINT16(val) { \
|
|
.type = SDP_DTYPE_UINT | SDP_DSIZE_2, \
|
|
.value.uint = val, \
|
|
},
|
|
#define UINT32(val) { \
|
|
.type = SDP_DTYPE_UINT | SDP_DSIZE_4, \
|
|
.value.uint = val, \
|
|
},
|
|
#define UUID128(val) { \
|
|
.type = SDP_DTYPE_UUID | SDP_DSIZE_16, \
|
|
.value.uint = val, \
|
|
},
|
|
#define SDP_TRUE { \
|
|
.type = SDP_DTYPE_BOOL | SDP_DSIZE_1, \
|
|
.value.uint = 1, \
|
|
},
|
|
#define SDP_FALSE { \
|
|
.type = SDP_DTYPE_BOOL | SDP_DSIZE_1, \
|
|
.value.uint = 0, \
|
|
},
|
|
#define STRING(val) { \
|
|
.type = SDP_DTYPE_STRING, \
|
|
.value.str = val, \
|
|
},
|
|
#define ARRAY(...) { \
|
|
.type = SDP_DTYPE_STRING | SDP_DSIZE_2, \
|
|
.value.str = (char []) { __VA_ARGS__, 0, 0 }, \
|
|
},
|
|
#define URL(val) { \
|
|
.type = SDP_DTYPE_URL, \
|
|
.value.str = val, \
|
|
},
|
|
#if 1
|
|
#define LIST(val) { \
|
|
.type = SDP_DTYPE_SEQ, \
|
|
.value.list = (struct sdp_def_data_element_s []) { val LAST }, \
|
|
},
|
|
#endif
|
|
|
|
/* Try to keep each single attribute below MAX_PDU_OUT_SIZE bytes
|
|
* in resulting SDP data representation size. */
|
|
|
|
SERVICE(hid,
|
|
ATTRIBUTE(RECORD_HANDLE, UINT32(0)) /* Filled in later */
|
|
ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(HID_SVCLASS_ID)))
|
|
ATTRIBUTE(RECORD_STATE, UINT32(1))
|
|
ATTRIBUTE(PROTO_DESC_LIST, LIST(
|
|
LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_HID_CTRL))
|
|
LIST(UUID128(HIDP_UUID))
|
|
))
|
|
ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002)))
|
|
ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST(
|
|
UINT16(0x656e) UINT16(0x006a) UINT16(0x0100)
|
|
))
|
|
ATTRIBUTE(PFILE_DESC_LIST, LIST(
|
|
LIST(UUID128(HID_PROFILE_ID) UINT16(0x0100))
|
|
))
|
|
ATTRIBUTE(DOC_URL, URL("http://bellard.org/qemu/user-doc.html"))
|
|
ATTRIBUTE(SVCNAME_PRIMARY, STRING("QEMU Bluetooth HID"))
|
|
ATTRIBUTE(SVCDESC_PRIMARY, STRING("QEMU Keyboard/Mouse"))
|
|
ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU"))
|
|
|
|
/* Profile specific */
|
|
ATTRIBUTE(DEVICE_RELEASE_NUMBER, UINT16(0x0091)) /* Deprecated, remove */
|
|
ATTRIBUTE(PARSER_VERSION, UINT16(0x0111))
|
|
/* TODO: extract from l2cap_device->device.class[0] */
|
|
ATTRIBUTE(DEVICE_SUBCLASS, UINT8(0x40))
|
|
ATTRIBUTE(COUNTRY_CODE, UINT8(0x15))
|
|
ATTRIBUTE(VIRTUAL_CABLE, SDP_TRUE)
|
|
ATTRIBUTE(RECONNECT_INITIATE, SDP_FALSE)
|
|
/* TODO: extract from hid->usbdev->report_desc */
|
|
ATTRIBUTE(DESCRIPTOR_LIST, LIST(
|
|
LIST(UINT8(0x22) ARRAY(
|
|
0x05, 0x01, /* Usage Page (Generic Desktop) */
|
|
0x09, 0x06, /* Usage (Keyboard) */
|
|
0xa1, 0x01, /* Collection (Application) */
|
|
0x75, 0x01, /* Report Size (1) */
|
|
0x95, 0x08, /* Report Count (8) */
|
|
0x05, 0x07, /* Usage Page (Key Codes) */
|
|
0x19, 0xe0, /* Usage Minimum (224) */
|
|
0x29, 0xe7, /* Usage Maximum (231) */
|
|
0x15, 0x00, /* Logical Minimum (0) */
|
|
0x25, 0x01, /* Logical Maximum (1) */
|
|
0x81, 0x02, /* Input (Data, Variable, Absolute) */
|
|
0x95, 0x01, /* Report Count (1) */
|
|
0x75, 0x08, /* Report Size (8) */
|
|
0x81, 0x01, /* Input (Constant) */
|
|
0x95, 0x05, /* Report Count (5) */
|
|
0x75, 0x01, /* Report Size (1) */
|
|
0x05, 0x08, /* Usage Page (LEDs) */
|
|
0x19, 0x01, /* Usage Minimum (1) */
|
|
0x29, 0x05, /* Usage Maximum (5) */
|
|
0x91, 0x02, /* Output (Data, Variable, Absolute) */
|
|
0x95, 0x01, /* Report Count (1) */
|
|
0x75, 0x03, /* Report Size (3) */
|
|
0x91, 0x01, /* Output (Constant) */
|
|
0x95, 0x06, /* Report Count (6) */
|
|
0x75, 0x08, /* Report Size (8) */
|
|
0x15, 0x00, /* Logical Minimum (0) */
|
|
0x25, 0xff, /* Logical Maximum (255) */
|
|
0x05, 0x07, /* Usage Page (Key Codes) */
|
|
0x19, 0x00, /* Usage Minimum (0) */
|
|
0x29, 0xff, /* Usage Maximum (255) */
|
|
0x81, 0x00, /* Input (Data, Array) */
|
|
0xc0 /* End Collection */
|
|
))))
|
|
ATTRIBUTE(LANG_ID_BASE_LIST, LIST(
|
|
LIST(UINT16(0x0409) UINT16(0x0100))
|
|
))
|
|
ATTRIBUTE(SDP_DISABLE, SDP_FALSE)
|
|
ATTRIBUTE(BATTERY_POWER, SDP_TRUE)
|
|
ATTRIBUTE(REMOTE_WAKEUP, SDP_TRUE)
|
|
ATTRIBUTE(BOOT_DEVICE, SDP_TRUE) /* XXX: untested */
|
|
ATTRIBUTE(SUPERVISION_TIMEOUT, UINT16(0x0c80))
|
|
ATTRIBUTE(NORMALLY_CONNECTABLE, SDP_TRUE)
|
|
ATTRIBUTE(PROFILE_VERSION, UINT16(0x0100))
|
|
)
|
|
|
|
SERVICE(sdp,
|
|
ATTRIBUTE(RECORD_HANDLE, UINT32(0)) /* Filled in later */
|
|
ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(SDP_SERVER_SVCLASS_ID)))
|
|
ATTRIBUTE(RECORD_STATE, UINT32(1))
|
|
ATTRIBUTE(PROTO_DESC_LIST, LIST(
|
|
LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_SDP))
|
|
LIST(UUID128(SDP_UUID))
|
|
))
|
|
ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002)))
|
|
ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST(
|
|
UINT16(0x656e) UINT16(0x006a) UINT16(0x0100)
|
|
))
|
|
ATTRIBUTE(PFILE_DESC_LIST, LIST(
|
|
LIST(UUID128(SDP_SERVER_PROFILE_ID) UINT16(0x0100))
|
|
))
|
|
ATTRIBUTE(DOC_URL, URL("http://bellard.org/qemu/user-doc.html"))
|
|
ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU"))
|
|
|
|
/* Profile specific */
|
|
ATTRIBUTE(VERSION_NUM_LIST, LIST(UINT16(0x0100)))
|
|
ATTRIBUTE(SVCDB_STATE , UINT32(1))
|
|
)
|
|
|
|
SERVICE(pnp,
|
|
ATTRIBUTE(RECORD_HANDLE, UINT32(0)) /* Filled in later */
|
|
ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(PNP_INFO_SVCLASS_ID)))
|
|
ATTRIBUTE(RECORD_STATE, UINT32(1))
|
|
ATTRIBUTE(PROTO_DESC_LIST, LIST(
|
|
LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_SDP))
|
|
LIST(UUID128(SDP_UUID))
|
|
))
|
|
ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002)))
|
|
ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST(
|
|
UINT16(0x656e) UINT16(0x006a) UINT16(0x0100)
|
|
))
|
|
ATTRIBUTE(PFILE_DESC_LIST, LIST(
|
|
LIST(UUID128(PNP_INFO_PROFILE_ID) UINT16(0x0100))
|
|
))
|
|
ATTRIBUTE(DOC_URL, URL("http://bellard.org/qemu/user-doc.html"))
|
|
ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU"))
|
|
|
|
/* Profile specific */
|
|
ATTRIBUTE(SPECIFICATION_ID, UINT16(0x0100))
|
|
ATTRIBUTE(VERSION, UINT16(0x0100))
|
|
ATTRIBUTE(PRIMARY_RECORD, SDP_TRUE)
|
|
)
|
|
|
|
static int bt_l2cap_sdp_new_ch(struct bt_l2cap_device_s *dev,
|
|
struct bt_l2cap_conn_params_s *params)
|
|
{
|
|
struct bt_l2cap_sdp_state_s *sdp = g_malloc0(sizeof(*sdp));
|
|
struct sdp_def_service_s *services[] = {
|
|
&sdp_service_sdp_s,
|
|
&sdp_service_hid_s,
|
|
&sdp_service_pnp_s,
|
|
NULL,
|
|
};
|
|
|
|
sdp->channel = params;
|
|
sdp->channel->opaque = sdp;
|
|
sdp->channel->close = bt_l2cap_sdp_close_ch;
|
|
sdp->channel->sdu_in = bt_l2cap_sdp_sdu_in;
|
|
|
|
sdp_service_db_build(sdp, services);
|
|
|
|
return 0;
|
|
}
|
|
|
|
void bt_l2cap_sdp_init(struct bt_l2cap_device_s *dev)
|
|
{
|
|
bt_l2cap_psm_register(dev, BT_PSM_SDP,
|
|
MAX_PDU_OUT_SIZE, bt_l2cap_sdp_new_ch);
|
|
}
|