static void
print_protocol_descriptor_list(uint8_t const *start, uint8_t const *end)
{
uint32_t type, len;
if (end - start < 2) {
fprintf(stderr, "Invalid Protocol Descriptor List. " \
"Too short, len=%zd\n", end - start);
return;
}
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_SEQ8:
SDP_GET8(len, start);
break;
case SDP_DATA_SEQ16:
SDP_GET16(len, start);
break;
case SDP_DATA_SEQ32:
SDP_GET32(len, start);
break;
default:
fprintf(stderr, "Invalid Protocol Descriptor List. " \
"Not a sequence, type=%#x\n", type);
return;
/* NOT REACHED */
}
while (start < end) {
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_SEQ8:
SDP_GET8(len, start);
break;
case SDP_DATA_SEQ16:
SDP_GET16(len, start);
break;
case SDP_DATA_SEQ32:
SDP_GET32(len, start);
break;
default:
fprintf(stderr, "Invalid Protocol Descriptor List. " \
"Not a sequence, type=%#x\n", type);
return;
/* NOT REACHED */
}
print_protocol_descriptor(start, start + len);
start += len;
}
} /* print_protocol_descriptor_list */
int32_t
server_prepare_attr_list(provider_p const provider,
uint8_t const *req, uint8_t const * const req_end,
uint8_t *rsp, uint8_t const * const rsp_end)
{
uint8_t *ptr = rsp + 3;
int32_t type, hi, lo, len;
if (ptr > rsp_end)
return (-1);
while (req < req_end) {
SDP_GET8(type, req);
switch (type) {
case SDP_DATA_UINT16:
if (req + 2 > req_end)
return (-1);
SDP_GET16(lo, req);
hi = lo;
break;
case SDP_DATA_UINT32:
if (req + 4 > req_end)
return (-1);
SDP_GET16(lo, req);
SDP_GET16(hi, req);
break;
default:
return (-1);
/* NOT REACHED */
}
for (; lo <= hi; lo ++) {
len = server_prepare_attr_value_pair(provider, lo, ptr, rsp_end);
if (len < 0)
return (-1);
ptr += len;
}
}
len = ptr - rsp; /* we put this much bytes in rsp */
/* Fix SEQ16 header for the rsp */
SDP_PUT8(SDP_DATA_SEQ16, rsp);
SDP_PUT16(len - 3, rsp);
return (len);
}
int32_t
server_prepare_service_register_response(server_p srv, int32_t fd)
{
uint8_t const *req = srv->req + sizeof(sdp_pdu_t);
uint8_t const *req_end = req + ((sdp_pdu_p)(srv->req))->len;
uint8_t *rsp = srv->fdidx[fd].rsp;
profile_t *profile = NULL;
provider_t *provider = NULL;
bdaddr_t const *bdaddr = NULL;
int32_t uuid;
/*
* Minimal Service Register Request
*
* value16 - uuid 2 bytes
* bdaddr - BD_ADDR 6 bytes
*/
if (!srv->fdidx[fd].control ||
!srv->fdidx[fd].priv || req_end - req < 8)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
/* Get ServiceClass UUID */
SDP_GET16(uuid, req);
/* Get BD_ADDR */
bdaddr = (bdaddr_t const *) req;
req += sizeof(*bdaddr);
/* Lookup profile descriptror */
profile = profile_get_descriptor(uuid);
if (profile == NULL)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
/* Validate user data */
if (req_end - req < profile->dsize ||
profile->valid == NULL ||
(profile->valid)(req, req_end - req) == 0)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
/* Register provider */
provider = provider_register(profile, bdaddr, fd, req, req_end - req);
if (provider == NULL)
return (SDP_ERROR_CODE_INSUFFICIENT_RESOURCES);
SDP_PUT16(0, rsp);
SDP_PUT32(provider->handle, rsp);
/* Set reply size */
srv->fdidx[fd].rsp_limit = srv->fdidx[fd].omtu - sizeof(sdp_pdu_t);
srv->fdidx[fd].rsp_size = rsp - srv->fdidx[fd].rsp;
srv->fdidx[fd].rsp_cs = 0;
return (0);
}
int32_t
server_prepare_service_search_attribute_response(server_p srv, int32_t fd)
{
uint8_t const *req = srv->req + sizeof(sdp_pdu_t);
uint8_t const *req_end = req + ((sdp_pdu_p)(srv->req))->len;
uint8_t *rsp = srv->fdidx[fd].rsp;
uint8_t const *rsp_end = rsp + L2CAP_MTU_MAXIMUM;
uint8_t const *aidptr = NULL;
provider_t *provider = NULL;
int32_t type, rsp_limit, ucount, aidlen, cslen, cs;
uint128_t ulist[12];
/*
* Minimal Service Search Attribute Request request
*
* seq8 len8 - 2 bytes
* uuid16 value16 - 3 bytes ServiceSearchPattern
* value16 - 2 bytes MaximumAttributeByteCount
* seq8 len8 - 2 bytes
* uint16 value16 - 3 bytes AttributeIDList
* value8 - 1 byte ContinuationState
*/
/* Get ServiceSearchPattern */
ucount = server_get_service_search_pattern(&req, req_end, ulist);
if (ucount < 1 || ucount > 12)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
/* Get MaximumAttributeByteCount */
if (req + 2 > req_end)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
SDP_GET16(rsp_limit, req);
if (rsp_limit <= 0)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
/* Get size of AttributeIDList */
if (req + 1 > req_end)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
aidlen = 0;
SDP_GET8(type, req);
switch (type) {
case SDP_DATA_SEQ8:
if (req + 1 > req_end)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
SDP_GET8(aidlen, req);
break;
case SDP_DATA_SEQ16:
if (req + 2 > req_end)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
SDP_GET16(aidlen, req);
break;
case SDP_DATA_SEQ32:
if (req + 4 > req_end)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
SDP_GET32(aidlen, req);
break;
}
if (aidlen <= 0)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
aidptr = req;
req += aidlen;
/* Get ContinuationState */
if (req + 1 > req_end)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
SDP_GET8(cslen, req);
if (cslen == 2 && req + 2 == req_end)
SDP_GET16(cs, req);
else if (cslen == 0 && req == req_end)
cs = 0;
else
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
/* Process the request. First, check continuation state */
if (srv->fdidx[fd].rsp_cs != cs)
return (SDP_ERROR_CODE_INVALID_CONTINUATION_STATE);
if (srv->fdidx[fd].rsp_size > 0)
return (0);
/*
* Service Search Attribute Response format
*
* value16 - 2 bytes AttributeListByteCount (not incl.)
* seq8 len16 - 3 bytes
* attr list - 3+ bytes AttributeLists
* [ attr list ]
*/
rsp += 3; /* leave space for sequence header */
for (provider = provider_get_first();
provider != NULL;
provider = provider_get_next(provider)) {
if (!provider_match_bdaddr(provider, &srv->req_sa.bt_bdaddr))
continue;
if (!provider_match_uuid(provider, ulist, ucount))
continue;
cs = server_prepare_attr_list(provider,
aidptr, aidptr + aidlen, rsp, rsp_end);
if (cs < 0)
return (SDP_ERROR_CODE_INSUFFICIENT_RESOURCES);
rsp += cs;
}
/* Set reply size (not counting PDU header and continuation state) */
srv->fdidx[fd].rsp_limit = srv->fdidx[fd].omtu - sizeof(sdp_pdu_t) - 2;
if (srv->fdidx[fd].rsp_limit > rsp_limit)
srv->fdidx[fd].rsp_limit = rsp_limit;
srv->fdidx[fd].rsp_size = rsp - srv->fdidx[fd].rsp;
srv->fdidx[fd].rsp_cs = 0;
/* Fix AttributeLists sequence header */
rsp = srv->fdidx[fd].rsp;
SDP_PUT8(SDP_DATA_SEQ16, rsp);
SDP_PUT16(srv->fdidx[fd].rsp_size - 3, rsp);
return (0);
}
static void
print_service_class_id_list(uint8_t const *start, uint8_t const *end)
{
uint32_t type, len, value;
if (end - start < 2) {
fprintf(stderr, "Invalid Service Class ID List. " \
"Too short, len=%zd\n", end - start);
return;
}
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_SEQ8:
SDP_GET8(len, start);
break;
case SDP_DATA_SEQ16:
SDP_GET16(len, start);
break;
case SDP_DATA_SEQ32:
SDP_GET32(len, start);
break;
default:
fprintf(stderr, "Invalid Service Class ID List. " \
"Not a sequence, type=%#x\n", type);
return;
/* NOT REACHED */
}
while (start < end) {
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_UUID16:
SDP_GET16(value, start);
fprintf(stdout, "\t%s (%#4.4x)\n",
sdp_uuid2desc(value), value);
break;
case SDP_DATA_UUID32:
SDP_GET32(value, start);
fprintf(stdout, "\t%#8.8x\n", value);
break;
case SDP_DATA_UUID128: {
int128_t uuid;
SDP_GET_UUID128(&uuid, start);
fprintf(stdout, "\t%#8.8x-%4.4x-%4.4x-%4.4x-%4.4x%8.8x\n",
ntohl(*(uint32_t *)&uuid.b[0]),
ntohs(*(uint16_t *)&uuid.b[4]),
ntohs(*(uint16_t *)&uuid.b[6]),
ntohs(*(uint16_t *)&uuid.b[8]),
ntohs(*(uint16_t *)&uuid.b[10]),
ntohl(*(uint32_t *)&uuid.b[12]));
} break;
default:
fprintf(stderr, "Invalid Service Class ID List. " \
"Not a UUID, type=%#x\n", type);
return;
/* NOT REACHED */
}
}
} /* print_service_class_id_list */
static void
print_bluetooth_profile_descriptor_list(uint8_t const *start, uint8_t const *end)
{
uint32_t type, len, value;
if (end - start < 2) {
fprintf(stderr, "Invalid Bluetooth Profile Descriptor List. " \
"Too short, len=%zd\n", end - start);
return;
}
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_SEQ8:
SDP_GET8(len, start);
break;
case SDP_DATA_SEQ16:
SDP_GET16(len, start);
break;
case SDP_DATA_SEQ32:
SDP_GET32(len, start);
break;
default:
fprintf(stderr, "Invalid Bluetooth Profile Descriptor List. " \
"Not a sequence, type=%#x\n", type);
return;
/* NOT REACHED */
}
while (start < end) {
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_SEQ8:
SDP_GET8(len, start);
break;
case SDP_DATA_SEQ16:
SDP_GET16(len, start);
break;
case SDP_DATA_SEQ32:
SDP_GET32(len, start);
break;
default:
fprintf(stderr, "Invalid Bluetooth Profile " \
"Descriptor List. " \
"Not a sequence, type=%#x\n", type);
return;
/* NOT REACHED */
}
/* Get UUID */
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_UUID16:
SDP_GET16(value, start);
fprintf(stdout, "\t%s (%#4.4x) ",
sdp_uuid2desc(value), value);
break;
case SDP_DATA_UUID32:
SDP_GET32(value, start);
fprintf(stdout, "\t%#8.8x ", value);
break;
case SDP_DATA_UUID128: {
int128_t uuid;
SDP_GET_UUID128(&uuid, start);
fprintf(stdout, "\t%#8.8x-%4.4x-%4.4x-%4.4x-%4.4x%8.8x ",
ntohl(*(uint32_t *)&uuid.b[0]),
ntohs(*(uint16_t *)&uuid.b[4]),
ntohs(*(uint16_t *)&uuid.b[6]),
ntohs(*(uint16_t *)&uuid.b[8]),
ntohs(*(uint16_t *)&uuid.b[10]),
ntohl(*(uint32_t *)&uuid.b[12]));
} break;
default:
fprintf(stderr, "Invalid Bluetooth Profile " \
"Descriptor List. " \
"Not a UUID, type=%#x\n", type);
return;
/* NOT REACHED */
}
/* Get version */
SDP_GET8(type, start);
if (type != SDP_DATA_UINT16) {
fprintf(stderr, "Invalid Bluetooth Profile " \
"Descriptor List. " \
"Invalid version type=%#x\n", type);
return;
}
SDP_GET16(value, start);
fprintf(stdout, "ver. %d.%d\n",
(value >> 8) & 0xff, value & 0xff);
}
} /* print_bluetooth_profile_descriptor_list */
static void
print_protocol_descriptor(uint8_t const *start, uint8_t const *end)
{
union {
uint8_t uint8;
uint16_t uint16;
uint32_t uint32;
uint64_t uint64;
int128_t int128;
} value;
uint32_t type, len, param;
/* Get Protocol UUID */
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_UUID16:
SDP_GET16(value.uint16, start);
fprintf(stdout, "\t%s (%#4.4x)\n", sdp_uuid2desc(value.uint16),
value.uint16);
break;
case SDP_DATA_UUID32:
SDP_GET32(value.uint32, start);
fprintf(stdout, "\t%#8.8x\n", value.uint32);
break;
case SDP_DATA_UUID128:
SDP_GET_UUID128(&value.int128, start);
fprintf(stdout, "\t%#8.8x-%4.4x-%4.4x-%4.4x-%4.4x%8.8x\n",
ntohl(*(uint32_t *)&value.int128.b[0]),
ntohs(*(uint16_t *)&value.int128.b[4]),
ntohs(*(uint16_t *)&value.int128.b[6]),
ntohs(*(uint16_t *)&value.int128.b[8]),
ntohs(*(uint16_t *)&value.int128.b[10]),
ntohl(*(uint32_t *)&value.int128.b[12]));
break;
default:
fprintf(stderr, "Invalid Protocol Descriptor. " \
"Not a UUID, type=%#x\n", type);
return;
/* NOT REACHED */
}
/* Protocol specific parameters */
for (param = 1; start < end; param ++) {
fprintf(stdout, "\t\tProtocol specific parameter #%d: ", param);
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_NIL:
fprintf(stdout, "nil\n");
break;
case SDP_DATA_UINT8:
case SDP_DATA_INT8:
case SDP_DATA_BOOL:
SDP_GET8(value.uint8, start);
fprintf(stdout, "u/int8/bool %u\n", value.uint8);
break;
case SDP_DATA_UINT16:
case SDP_DATA_INT16:
case SDP_DATA_UUID16:
SDP_GET16(value.uint16, start);
fprintf(stdout, "u/int/uuid16 %u\n", value.uint16);
break;
case SDP_DATA_UINT32:
case SDP_DATA_INT32:
case SDP_DATA_UUID32:
SDP_GET32(value.uint32, start);
fprintf(stdout, "u/int/uuid32 %u\n", value.uint32);
break;
case SDP_DATA_UINT64:
case SDP_DATA_INT64:
SDP_GET64(value.uint64, start);
fprintf(stdout, "u/int64 %ju\n", value.uint64);
break;
case SDP_DATA_UINT128:
case SDP_DATA_INT128:
SDP_GET128(&value.int128, start);
fprintf(stdout, "u/int128 %#8.8x%8.8x%8.8x%8.8x\n",
*(uint32_t *)&value.int128.b[0],
*(uint32_t *)&value.int128.b[4],
*(uint32_t *)&value.int128.b[8],
*(uint32_t *)&value.int128.b[12]);
break;
case SDP_DATA_UUID128:
SDP_GET_UUID128(&value.int128, start);
fprintf(stdout, "uuid128 %#8.8x-%4.4x-%4.4x-%4.4x-%4.4x%8.8x\n",
ntohl(*(uint32_t *)&value.int128.b[0]),
ntohs(*(uint16_t *)&value.int128.b[4]),
ntohs(*(uint16_t *)&value.int128.b[6]),
ntohs(*(uint16_t *)&value.int128.b[8]),
ntohs(*(uint16_t *)&value.int128.b[10]),
ntohl(*(uint32_t *)&value.int128.b[12]));
break;
case SDP_DATA_STR8:
case SDP_DATA_URL8:
SDP_GET8(len, start);
fprintf(stdout, "%*.*s\n", len, len, (char *) start);
start += len;
break;
case SDP_DATA_STR16:
case SDP_DATA_URL16:
SDP_GET16(len, start);
fprintf(stdout, "%*.*s\n", len, len, (char *) start);
start += len;
break;
case SDP_DATA_STR32:
case SDP_DATA_URL32:
SDP_GET32(len, start);
fprintf(stdout, "%*.*s\n", len, len, (char *) start);
start += len;
break;
case SDP_DATA_SEQ8:
case SDP_DATA_ALT8:
SDP_GET8(len, start);
for (; len > 0; start ++, len --)
fprintf(stdout, "%#2.2x ", *start);
fprintf(stdout, "\n");
break;
case SDP_DATA_SEQ16:
case SDP_DATA_ALT16:
SDP_GET16(len, start);
for (; len > 0; start ++, len --)
fprintf(stdout, "%#2.2x ", *start);
fprintf(stdout, "\n");
break;
case SDP_DATA_SEQ32:
case SDP_DATA_ALT32:
SDP_GET32(len, start);
for (; len > 0; start ++, len --)
fprintf(stdout, "%#2.2x ", *start);
fprintf(stdout, "\n");
break;
default:
fprintf(stderr, "Invalid Protocol Descriptor. " \
"Unknown data type: %#02x\n", type);
return;
/* NOT REACHED */
}
}
} /* print_protocol_descriptor */
static int find_service_channel(bdaddr_t *adapter, bdaddr_t *device, int only_gnapplet, uint16_t svclass_id)
{
int i, channel = -1;
char name[64];
void *ss = NULL;
uint32_t attrs[] = {
SDP_ATTR_RANGE( SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST,
SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST),
SDP_ATTR_RANGE( SDP_ATTR_PRIMARY_LANGUAGE_BASE_ID + SDP_ATTR_SERVICE_NAME_OFFSET,
SDP_ATTR_PRIMARY_LANGUAGE_BASE_ID + SDP_ATTR_SERVICE_NAME_OFFSET),
};
/* Buffer for the attributes */
static uint8_t buffer[NRECS * attrs_len][BSIZE];
/* SDP attributes */
static sdp_attr_t values[NRECS * attrs_len];
/* Initialize attribute values array */
for (i = 0; i < values_len; i ++) {
values[i].flags = SDP_ATTR_INVALID;
values[i].attr = 0;
values[i].vlen = BSIZE;
values[i].value = buffer[i];
}
if ((ss = sdp_open(adapter, device)) == NULL)
return -1;
if (sdp_error(ss) != 0)
goto end;
if (sdp_search(ss, 1, &svclass_id, attrs_len, attrs, values_len, values) != 0)
goto end;
for (i = 0; i < values_len; i++) {
union {
uint8_t uint8;
uint16_t uint16;
uint32_t uint32;
uint64_t uint64;
int128_t int128;
} value;
uint8_t *start, *end;
uint32_t type, len;
if (values[i].flags != SDP_ATTR_OK)
break;
start = values[i].value;
end = values[i].value + values[i].vlen;
switch (values[i].attr) {
case SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST:
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_SEQ8:
SDP_GET8(len, start);
break;
case SDP_DATA_SEQ16:
SDP_GET16(len, start);
break;
case SDP_DATA_SEQ32:
SDP_GET32(len, start);
break;
default:
goto end;
break;
}
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_SEQ8:
SDP_GET8(len, start);
break;
case SDP_DATA_SEQ16:
SDP_GET16(len, start);
break;
case SDP_DATA_SEQ32:
SDP_GET32(len, start);
break;
default:
goto end;
}
while (start < end) {
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_UUID16:
SDP_GET16(value.uint16, start);
break;
case SDP_DATA_UUID32:
SDP_GET32(value.uint32, start);
break;
case SDP_DATA_UUID128:
SDP_GET_UUID128(&value.int128, start);
break;
default:
goto end;
}
if (value.uint16 == 3) {
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_UINT8:
case SDP_DATA_INT8:
SDP_GET8(value.uint8, start);
channel = value.uint8;
break;
case SDP_DATA_UINT16:
case SDP_DATA_INT16:
SDP_GET16(value.uint16, start);
channel = value.uint16;
break;
case SDP_DATA_UINT32:
case SDP_DATA_INT32:
SDP_GET32(value.uint32, start);
channel = value.uint32;
break;
default:
goto end;
}
} else {
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_SEQ8:
case SDP_DATA_UINT8:
case SDP_DATA_INT8:
case SDP_DATA_BOOL:
SDP_GET8(value.uint8, start);
break;
case SDP_DATA_SEQ16:
case SDP_DATA_UINT16:
case SDP_DATA_INT16:
case SDP_DATA_UUID16:
SDP_GET16(value.uint16, start);
break;
case SDP_DATA_SEQ32:
case SDP_DATA_UINT32:
case SDP_DATA_INT32:
case SDP_DATA_UUID32:
SDP_GET32(value.uint32, start);
break;
case SDP_DATA_UINT64:
case SDP_DATA_INT64:
SDP_GET64(value.uint64, start);
break;
case SDP_DATA_UINT128:
case SDP_DATA_INT128:
SDP_GET128(&value.int128, start);
break;
default:
goto end;
}
}
}
start += len;
break;
case SDP_ATTR_PRIMARY_LANGUAGE_BASE_ID + SDP_ATTR_SERVICE_NAME_OFFSET:
if (channel == -1)
break;
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_STR8:
case SDP_DATA_URL8:
SDP_GET8(len, start);
snprintf(name, sizeof(name), "%*.*s", len, len, (char *) start);
start += len;
break;
case SDP_DATA_STR16:
case SDP_DATA_URL16:
SDP_GET16(len, start);
snprintf(name, sizeof(name), "%*.*s", len, len, (char *) start);
start += len;
break;
case SDP_DATA_STR32:
case SDP_DATA_URL32:
SDP_GET32(len, start);
snprintf(name, sizeof(name), "%*.*s", len, len, (char *) start);
start += len;
break;
default:
goto end;
}
if (name == NULL)
break;
if (only_gnapplet != 0) {
if (strcmp(name, "gnapplet") == 0)
goto end;
else {
channel = -1;
break;
}
}
if (strstr(name, "Nokia PC Suite") != NULL) {
channel = -1;
break;
}
if (strstr(name, "Bluetooth Serial Port") != NULL) {
channel = -1;
break;
}
if (strstr(name, "m-Router Connectivity") != NULL) {
channel = -1;
break;
}
goto end;
}
}
end:
sdp_close(ss);
return channel;
}
int32_t
server_prepare_service_attribute_response(server_p srv, int32_t fd)
{
uint8_t const *req = srv->req + sizeof(sdp_pdu_t);
uint8_t const *req_end = req + ((sdp_pdu_p)(srv->req))->len;
uint8_t *rsp = srv->fdidx[fd].rsp;
uint8_t const *rsp_end = rsp + NG_L2CAP_MTU_MAXIMUM;
uint8_t *ptr = NULL;
provider_t *provider = NULL;
uint32_t handle;
int32_t type, rsp_limit, aidlen, cslen, cs;
/*
* Minimal Service Attribute Request request
*
* value32 - 4 bytes ServiceRecordHandle
* value16 - 2 bytes MaximumAttributeByteCount
* seq8 len8 - 2 bytes
* uint16 value16 - 3 bytes AttributeIDList
* value8 - 1 byte ContinuationState
*/
if (req_end - req < 12)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
/* Get ServiceRecordHandle and MaximumAttributeByteCount */
SDP_GET32(handle, req);
SDP_GET16(rsp_limit, req);
if (rsp_limit <= 0)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
/* Get size of AttributeIDList */
aidlen = 0;
SDP_GET8(type, req);
switch (type) {
case SDP_DATA_SEQ8:
SDP_GET8(aidlen, req);
break;
case SDP_DATA_SEQ16:
SDP_GET16(aidlen, req);
break;
case SDP_DATA_SEQ32:
SDP_GET32(aidlen, req);
break;
}
if (aidlen <= 0)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
ptr = (uint8_t *) req + aidlen;
/* Get ContinuationState */
if (ptr + 1 > req_end)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
SDP_GET8(cslen, ptr);
if (cslen != 0) {
if (cslen != 2 || req_end - ptr != 2)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
SDP_GET16(cs, ptr);
} else
cs = 0;
/* Process the request. First, check continuation state */
if (srv->fdidx[fd].rsp_cs != cs)
return (SDP_ERROR_CODE_INVALID_CONTINUATION_STATE);
if (srv->fdidx[fd].rsp_size > 0)
return (0);
/* Lookup record handle */
if ((provider = provider_by_handle(handle)) == NULL)
return (SDP_ERROR_CODE_INVALID_SERVICE_RECORD_HANDLE);
/*
* Service Attribute Response format
*
* value16 - 2 bytes AttributeListByteCount (not incl.)
* seq8 len16 - 3 bytes
* attr value - 3+ bytes AttributeList
* [ attr value ]
*/
cs = server_prepare_attr_list(provider, req, req+aidlen, rsp, rsp_end);
if (cs < 0)
return (SDP_ERROR_CODE_INSUFFICIENT_RESOURCES);
/* Set reply size (not counting PDU header and continuation state) */
srv->fdidx[fd].rsp_limit = srv->fdidx[fd].omtu - sizeof(sdp_pdu_t) - 2;
if (srv->fdidx[fd].rsp_limit > rsp_limit)
srv->fdidx[fd].rsp_limit = rsp_limit;
srv->fdidx[fd].rsp_size = cs;
srv->fdidx[fd].rsp_cs = 0;
return (0);
}
int
rfcomm_channel_lookup(bdaddr_t const *local, bdaddr_t const *remote,
int service, int *channel, int *error)
{
uint8_t buffer[PROTOCOL_DESCRIPTOR_LIST_BUFFER_SIZE];
void *ss = NULL;
uint16_t serv = (uint16_t) service;
uint32_t attr = SDP_ATTR_RANGE(
SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST,
SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST);
sdp_attr_t proto = { SDP_ATTR_INVALID,0,sizeof(buffer),buffer };
uint32_t type, len;
if (local == NULL)
local = NG_HCI_BDADDR_ANY;
if (remote == NULL || channel == NULL)
rfcomm_channel_lookup_exit(EINVAL);
if ((ss = sdp_open(local, remote)) == NULL)
rfcomm_channel_lookup_exit(ENOMEM);
if (sdp_error(ss) != 0)
rfcomm_channel_lookup_exit(sdp_error(ss));
if (sdp_search(ss, 1, &serv, 1, &attr, 1, &proto) != 0)
rfcomm_channel_lookup_exit(sdp_error(ss));
if (proto.flags != SDP_ATTR_OK)
rfcomm_channel_lookup_exit(ENOATTR);
sdp_close(ss);
ss = NULL;
/*
* If it is possible for more than one kind of protocol stack to be
* used to gain access to the service, the ProtocolDescriptorList
* takes the form of a data element alternative. We always use the
* first protocol stack.
*
* A minimal Protocol Descriptor List for RFCOMM based service would
* look like
*
* seq8 len8 - 2 bytes
* seq8 len8 - 2 bytes
* uuid16 value16 - 3 bytes L2CAP
* seq8 len8 - 2 bytes
* uuid16 value16 - 3 bytes RFCOMM
* uint8 value8 - 2 bytes RFCOMM param #1
* =========
* 14 bytes
*
* Lets not count first [seq8 len8] wrapper, so the minimal size of
* the Protocol Descriptor List (the data we are actually interested
* in) for RFCOMM based service would be 12 bytes.
*/
if (proto.vlen < PROTOCOL_DESCRIPTOR_LIST_MINIMAL_SIZE)
rfcomm_channel_lookup_exit(EINVAL);
SDP_GET8(type, proto.value);
if (type == SDP_DATA_ALT8) {
SDP_GET8(len, proto.value);
} else if (type == SDP_DATA_ALT16) {
SDP_GET16(len, proto.value);
} else if (type == SDP_DATA_ALT32) {
SDP_GET32(len, proto.value);
} else
len = 0;
if (len > 0)
SDP_GET8(type, proto.value);
switch (type) {
case SDP_DATA_SEQ8:
SDP_GET8(len, proto.value);
break;
case SDP_DATA_SEQ16:
SDP_GET16(len, proto.value);
break;
case SDP_DATA_SEQ32:
SDP_GET32(len, proto.value);
break;
default:
rfcomm_channel_lookup_exit(ENOATTR);
/* NOT REACHED */
}
if (len < PROTOCOL_DESCRIPTOR_LIST_MINIMAL_SIZE)
rfcomm_channel_lookup_exit(EINVAL);
return (rfcomm_proto_list_parse(proto.value,
buffer + proto.vlen, channel, error));
}
static int
rfcomm_proto_list_parse(uint8_t const *start, uint8_t const *end,
int *channel, int *error)
{
int type, len, value;
while (start < end) {
/*
* Parse protocol descriptor
*
* A protocol descriptor identifies a communications protocol
* and provides protocol specific parameters. A protocol
* descriptor is represented as a data element sequence. The
* first data element in the sequence must be the UUID that
* identifies the protocol. Additional data elements optionally
* provide protocol specific information, such as the L2CAP
* protocol/service multiplexer (PSM) and the RFCOMM server
* channel number (CN).
*/
/* We must have at least one byte (type) */
if (end - start < 1)
rfcomm_proto_list_parse_exit(EINVAL)
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_SEQ8:
SDP_GET8(len, start);
break;
case SDP_DATA_SEQ16:
SDP_GET16(len, start);
break;
case SDP_DATA_SEQ32:
SDP_GET32(len, start);
break;
default:
rfcomm_proto_list_parse_exit(ENOATTR)
/* NOT REACHED */
}
/* We must have at least 3 bytes (type + UUID16) */
if (end - start < 3)
rfcomm_proto_list_parse_exit(EINVAL);
/* Get protocol UUID */
SDP_GET8(type, start); len -= sizeof(uint8_t);
switch (type) {
case SDP_DATA_UUID16:
SDP_GET16(value, start); len -= sizeof(uint16_t);
if (value != SDP_UUID_PROTOCOL_RFCOMM)
goto next_protocol;
break;
case SDP_DATA_UUID32: /* XXX FIXME can we have 32-bit UUID */
case SDP_DATA_UUID128: /* XXX FIXME can we have 128-bit UUID */
default:
rfcomm_proto_list_parse_exit(ENOATTR);
/* NOT REACHED */
}
/*
* First protocol specific parameter for RFCOMM procotol must
* be uint8 that represents RFCOMM channel number. So we must
* have at least two bytes.
*/
if (end - start < 2)
rfcomm_proto_list_parse_exit(EINVAL);
SDP_GET8(type, start);
if (type != SDP_DATA_UINT8)
rfcomm_proto_list_parse_exit(ENOATTR);
SDP_GET8(*channel, start);
rfcomm_proto_list_parse_exit(0);
/* NOT REACHED */
next_protocol:
start += len;
}
/*
* If we got here then it means we could not find RFCOMM protocol
* descriptor, but the reply format was actually valid.
*/
rfcomm_proto_list_parse_exit(ENOATTR);
}
static int32_t
hid_sdp_parse_hid_descriptor(sdp_attr_p a)
{
uint8_t *ptr = a->value;
uint8_t *end = a->value + a->vlen;
int32_t type, len, descriptor_type;
if (end - ptr < 9)
return (-1);
SDP_GET8(type, ptr);
switch (type) {
case SDP_DATA_SEQ8:
SDP_GET8(len, ptr);
break;
case SDP_DATA_SEQ16:
SDP_GET16(len, ptr);
break;
case SDP_DATA_SEQ32:
SDP_GET32(len, ptr);
break;
default:
return (-1);
}
if (ptr + len > end)
return (-1);
while (ptr < end) {
/* Descriptor */
SDP_GET8(type, ptr);
switch (type) {
case SDP_DATA_SEQ8:
if (ptr + 1 > end)
return (-1);
SDP_GET8(len, ptr);
break;
case SDP_DATA_SEQ16:
if (ptr + 2 > end)
return (-1);
SDP_GET16(len, ptr);
break;
case SDP_DATA_SEQ32:
if (ptr + 4 > end)
return (-1);
SDP_GET32(len, ptr);
break;
default:
return (-1);
}
/* Descripor type */
if (ptr + 1 > end)
return (-1);
SDP_GET8(type, ptr);
if (type != SDP_DATA_UINT8 || ptr + 1 > end)
return (-1);
SDP_GET8(descriptor_type, ptr);
/* Descriptor value */
if (ptr + 1 > end)
return (-1);
SDP_GET8(type, ptr);
switch (type) {
case SDP_DATA_STR8:
if (ptr + 1 > end)
return (-1);
SDP_GET8(len, ptr);
break;
case SDP_DATA_STR16:
if (ptr + 2 > end)
return (-1);
SDP_GET16(len, ptr);
break;
case SDP_DATA_STR32:
if (ptr + 4 > end)
return (-1);
SDP_GET32(len, ptr);
break;
default:
return (-1);
}
if (ptr + len > end)
return (-1);
if (descriptor_type == UDESC_REPORT && len > 0) {
a->value = ptr;
a->vlen = len;
return (0);
}
ptr += len;
}
return (-1);
}
static int32_t
hid_sdp_parse_protocol_descriptor_list(sdp_attr_p a)
{
uint8_t *ptr = a->value;
uint8_t *end = a->value + a->vlen;
int32_t type, len, uuid, psm;
if (end - ptr < 15)
return (-1);
if (a->attr == SDP_ATTR_ADDITIONAL_PROTOCOL_DESCRIPTOR_LISTS) {
SDP_GET8(type, ptr);
switch (type) {
case SDP_DATA_SEQ8:
SDP_GET8(len, ptr);
break;
case SDP_DATA_SEQ16:
SDP_GET16(len, ptr);
break;
case SDP_DATA_SEQ32:
SDP_GET32(len, ptr);
break;
default:
return (-1);
}
if (ptr + len > end)
return (-1);
}
SDP_GET8(type, ptr);
switch (type) {
case SDP_DATA_SEQ8:
SDP_GET8(len, ptr);
break;
case SDP_DATA_SEQ16:
SDP_GET16(len, ptr);
break;
case SDP_DATA_SEQ32:
SDP_GET32(len, ptr);
break;
default:
return (-1);
}
if (ptr + len > end)
return (-1);
/* Protocol */
SDP_GET8(type, ptr);
switch (type) {
case SDP_DATA_SEQ8:
SDP_GET8(len, ptr);
break;
case SDP_DATA_SEQ16:
SDP_GET16(len, ptr);
break;
case SDP_DATA_SEQ32:
SDP_GET32(len, ptr);
break;
default:
return (-1);
}
if (ptr + len > end)
return (-1);
/* UUID */
if (ptr + 3 > end)
return (-1);
SDP_GET8(type, ptr);
switch (type) {
case SDP_DATA_UUID16:
SDP_GET16(uuid, ptr);
if (uuid != SDP_UUID_PROTOCOL_L2CAP)
return (-1);
break;
case SDP_DATA_UUID32: /* XXX FIXME can we have 32-bit UUID */
case SDP_DATA_UUID128: /* XXX FIXME can we have 128-bit UUID */
default:
return (-1);
}
/* PSM */
if (ptr + 3 > end)
return (-1);
SDP_GET8(type, ptr);
if (type != SDP_DATA_UINT16)
return (-1);
SDP_GET16(psm, ptr);
return (psm);
}
int32_t
server_prepare_service_search_attribute_response(server_p srv, int32_t fd)
{
uint8_t const *req = srv->req + sizeof(sdp_pdu_t);
uint8_t const *req_end = req + ((sdp_pdu_p)(srv->req))->len;
uint8_t *rsp = srv->fdidx[fd].rsp;
uint8_t const *rsp_end = rsp + NG_L2CAP_MTU_MAXIMUM;
uint8_t const *sspptr = NULL, *aidptr = NULL;
uint8_t *ptr = NULL;
provider_t *provider = NULL;
int32_t type, rsp_limit, ssplen, aidlen, cslen, cs;
uint128_t uuid, puuid;
/*
* Minimal Service Search Attribute Request request
*
* seq8 len8 - 2 bytes
* uuid16 value16 - 3 bytes ServiceSearchPattern
* value16 - 2 bytes MaximumAttributeByteCount
* seq8 len8 - 2 bytes
* uint16 value16 - 3 bytes AttributeIDList
* value8 - 1 byte ContinuationState
*/
if (req_end - req < 13)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
/* Get size of ServiceSearchPattern */
ssplen = 0;
SDP_GET8(type, req);
switch (type) {
case SDP_DATA_SEQ8:
SDP_GET8(ssplen, req);
break;
case SDP_DATA_SEQ16:
SDP_GET16(ssplen, req);
break;
case SDP_DATA_SEQ32:
SDP_GET32(ssplen, req);
break;
}
if (ssplen <= 0)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
sspptr = req;
req += ssplen;
/* Get MaximumAttributeByteCount */
if (req + 2 > req_end)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
SDP_GET16(rsp_limit, req);
if (rsp_limit <= 0)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
/* Get size of AttributeIDList */
if (req + 1 > req_end)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
aidlen = 0;
SDP_GET8(type, req);
switch (type) {
case SDP_DATA_SEQ8:
if (req + 1 > req_end)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
SDP_GET8(aidlen, req);
break;
case SDP_DATA_SEQ16:
if (req + 2 > req_end)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
SDP_GET16(aidlen, req);
break;
case SDP_DATA_SEQ32:
if (req + 4 > req_end)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
SDP_GET32(aidlen, req);
break;
}
if (aidlen <= 0)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
aidptr = req;
req += aidlen;
/* Get ContinuationState */
if (req + 1 > req_end)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
SDP_GET8(cslen, req);
if (cslen != 0) {
if (cslen != 2 || req_end - req != 2)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
SDP_GET16(cs, req);
} else
cs = 0;
/* Process the request. First, check continuation state */
if (srv->fdidx[fd].rsp_cs != cs)
return (SDP_ERROR_CODE_INVALID_CONTINUATION_STATE);
if (srv->fdidx[fd].rsp_size > 0)
return (0);
/*
* Service Search Attribute Response format
*
* value16 - 2 bytes AttributeListByteCount (not incl.)
* seq8 len16 - 3 bytes
* attr list - 3+ bytes AttributeLists
* [ attr list ]
*/
ptr = rsp + 3;
while (ssplen > 0) {
SDP_GET8(type, sspptr);
ssplen --;
switch (type) {
case SDP_DATA_UUID16:
if (ssplen < 2)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
memcpy(&uuid, &uuid_base, sizeof(uuid));
uuid.b[2] = *sspptr ++;
uuid.b[3] = *sspptr ++;
ssplen -= 2;
break;
case SDP_DATA_UUID32:
if (ssplen < 4)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
memcpy(&uuid, &uuid_base, sizeof(uuid));
uuid.b[0] = *sspptr ++;
uuid.b[1] = *sspptr ++;
uuid.b[2] = *sspptr ++;
uuid.b[3] = *sspptr ++;
ssplen -= 4;
break;
case SDP_DATA_UUID128:
if (ssplen < 16)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
memcpy(uuid.b, sspptr, 16);
sspptr += 16;
ssplen -= 16;
break;
default:
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
/* NOT REACHED */
}
for (provider = provider_get_first();
provider != NULL;
provider = provider_get_next(provider)) {
if (!provider_match_bdaddr(provider, &srv->req_sa.l2cap_bdaddr))
continue;
memcpy(&puuid, &uuid_base, sizeof(puuid));
puuid.b[2] = provider->profile->uuid >> 8;
puuid.b[3] = provider->profile->uuid;
if (memcmp(&uuid, &puuid, sizeof(uuid)) != 0 &&
memcmp(&uuid, &uuid_public_browse_group, sizeof(uuid)) != 0)
continue;
cs = server_prepare_attr_list(provider,
aidptr, aidptr + aidlen, ptr, rsp_end);
if (cs < 0)
return (SDP_ERROR_CODE_INSUFFICIENT_RESOURCES);
ptr += cs;
}
}
/* Set reply size (not counting PDU header and continuation state) */
srv->fdidx[fd].rsp_limit = srv->fdidx[fd].omtu - sizeof(sdp_pdu_t) - 2;
if (srv->fdidx[fd].rsp_limit > rsp_limit)
srv->fdidx[fd].rsp_limit = rsp_limit;
srv->fdidx[fd].rsp_size = ptr - rsp;
srv->fdidx[fd].rsp_cs = 0;
/* Fix AttributeLists sequence header */
ptr = rsp;
SDP_PUT8(SDP_DATA_SEQ16, ptr);
SDP_PUT16(srv->fdidx[fd].rsp_size - 3, ptr);
return (0);
}
int32_t
server_prepare_service_search_response(server_p srv, int32_t fd)
{
uint8_t const *req = srv->req + sizeof(sdp_pdu_t);
uint8_t const *req_end = req + ((sdp_pdu_p)(srv->req))->len;
uint8_t *rsp = srv->fdidx[fd].rsp;
uint8_t const *rsp_end = rsp + NG_L2CAP_MTU_MAXIMUM;
uint8_t *ptr = NULL;
provider_t *provider = NULL;
int32_t type, ssplen, rsp_limit, rcount, cslen, cs;
uint128_t uuid, puuid;
/*
* Minimal SDP Service Search Request
*
* seq8 len8 - 2 bytes
* uuid16 value16 - 3 bytes ServiceSearchPattern
* value16 - 2 bytes MaximumServiceRecordCount
* value8 - 1 byte ContinuationState
*/
if (req_end - req < 8)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
/* Get size of ServiceSearchPattern */
ssplen = 0;
SDP_GET8(type, req);
switch (type) {
case SDP_DATA_SEQ8:
SDP_GET8(ssplen, req);
break;
case SDP_DATA_SEQ16:
SDP_GET16(ssplen, req);
break;
case SDP_DATA_SEQ32:
SDP_GET32(ssplen, req);
break;
}
if (ssplen <= 0)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
ptr = (uint8_t *) req + ssplen;
/* Get MaximumServiceRecordCount */
if (ptr + 2 > req_end)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
SDP_GET16(rsp_limit, ptr);
if (rsp_limit <= 0)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
/* Get ContinuationState */
if (ptr + 1 > req_end)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
SDP_GET8(cslen, ptr);
if (cslen != 0) {
if (cslen != 2 || req_end - ptr != 2)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
SDP_GET16(cs, ptr);
} else
cs = 0;
/* Process the request. First, check continuation state */
if (srv->fdidx[fd].rsp_cs != cs)
return (SDP_ERROR_CODE_INVALID_CONTINUATION_STATE);
if (srv->fdidx[fd].rsp_size > 0)
return (0);
/*
* Service Search Response format
*
* value16 - 2 bytes TotalServiceRecordCount (not incl.)
* value16 - 2 bytes CurrentServiceRecordCount (not incl.)
* value32 - 4 bytes handle
* [ value32 ]
*
* Calculate how many record handles we can fit
* in our reply buffer and adjust rlimit.
*/
ptr = rsp;
rcount = (rsp_end - ptr) / 4;
if (rcount < rsp_limit)
rsp_limit = rcount;
/* Look for the record handles */
for (rcount = 0; ssplen > 0 && rcount < rsp_limit; ) {
SDP_GET8(type, req);
ssplen --;
switch (type) {
case SDP_DATA_UUID16:
if (ssplen < 2)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
memcpy(&uuid, &uuid_base, sizeof(uuid));
uuid.b[2] = *req ++;
uuid.b[3] = *req ++;
ssplen -= 2;
break;
case SDP_DATA_UUID32:
if (ssplen < 4)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
memcpy(&uuid, &uuid_base, sizeof(uuid));
uuid.b[0] = *req ++;
uuid.b[1] = *req ++;
uuid.b[2] = *req ++;
uuid.b[3] = *req ++;
ssplen -= 4;
break;
case SDP_DATA_UUID128:
if (ssplen < 16)
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
memcpy(uuid.b, req, 16);
req += 16;
ssplen -= 16;
break;
default:
return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
/* NOT REACHED */
}
for (provider = provider_get_first();
provider != NULL && rcount < rsp_limit;
provider = provider_get_next(provider)) {
if (!provider_match_bdaddr(provider, &srv->req_sa.l2cap_bdaddr))
continue;
memcpy(&puuid, &uuid_base, sizeof(puuid));
puuid.b[2] = provider->profile->uuid >> 8;
puuid.b[3] = provider->profile->uuid;
if (memcmp(&uuid, &puuid, sizeof(uuid)) == 0 ||
memcmp(&uuid, &uuid_public_browse_group, sizeof(uuid)) == 0) {
SDP_PUT32(provider->handle, ptr);
rcount ++;
}
}
}
/* Set reply size (not counting PDU header and continuation state) */
srv->fdidx[fd].rsp_limit = srv->fdidx[fd].omtu - sizeof(sdp_pdu_t) - 4;
srv->fdidx[fd].rsp_size = ptr - rsp;
srv->fdidx[fd].rsp_cs = 0;
return (0);
}
static int
bt_find_psm(const uint8_t *start, const uint8_t *end)
{
uint32_t type;
uint32_t len;
int protover = 0;
int psm = -1;
if ((end - start) < 2)
return (-1);
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_SEQ8:
SDP_GET8(len, start);
break;
case SDP_DATA_SEQ16:
SDP_GET16(len, start);
break;
case SDP_DATA_SEQ32:
SDP_GET32(len, start);
break;
default:
return (-1);
}
while (start < end) {
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_SEQ8:
SDP_GET8(len, start);
break;
case SDP_DATA_SEQ16:
SDP_GET16(len, start);
break;
case SDP_DATA_SEQ32:
SDP_GET32(len, start);
break;
default:
return (-1);
}
/* check range */
if (len > (end - start))
break;
if (len >= 6) {
const uint8_t *ptr = start;
SDP_GET8(type, ptr);
if (type == SDP_DATA_UUID16) {
uint16_t temp;
SDP_GET16(temp, ptr);
switch (temp) {
case SDP_UUID_PROTOCOL_L2CAP:
SDP_GET8(type, ptr);
SDP_GET16(psm, ptr);
break;
case SDP_UUID_PROTOCOL_AVDTP:
SDP_GET8(type, ptr);
SDP_GET16(protover, ptr);
break;
default:
break;
}
}
}
start += len;
if (protover >= 0x0100 && psm > -1)
return (htole16(psm));
}
return (-1);
}
void
sdp_print(uint32_t level, uint8_t const *start, uint8_t const *end)
{
union {
int8_t int8;
int16_t int16;
int32_t int32;
int64_t int64;
int128_t int128;
uint8_t uint8;
uint16_t uint16;
uint32_t uint32;
uint64_t uint64;
} value;
uint8_t type;
uint32_t i;
if (start == NULL || end == NULL)
return;
while (start < end) {
for (i = 0; i < level; i++)
printf("\t");
SDP_GET8(type, start);
switch (type) {
case SDP_DATA_NIL:
printf("nil\n");
break;
case SDP_DATA_UINT8:
SDP_GET8(value.uint8, start);
printf("uint8 %u\n", value.uint8);
break;
case SDP_DATA_UINT16:
SDP_GET16(value.uint16, start);
printf("uint16 %u\n", value.uint16);
break;
case SDP_DATA_UINT32:
SDP_GET32(value.uint32, start);
printf("uint32 %u\n", value.uint32);
break;
case SDP_DATA_UINT64:
SDP_GET64(value.uint64, start);
printf("uint64 %ju\n", value.uint64);
break;
case SDP_DATA_UINT128:
case SDP_DATA_INT128:
SDP_GET128(&value.int128, start);
printf("u/int128 %#8.8x%8.8x%8.8x%8.8x\n",
*(uint32_t *)&value.int128.b[0],
*(uint32_t *)&value.int128.b[4],
*(uint32_t *)&value.int128.b[8],
*(uint32_t *)&value.int128.b[12]);
break;
case SDP_DATA_UUID128:
SDP_GET_UUID128(&value.int128, start);
printf("uuid128 %#8.8x-%4.4x-%4.4x-%4.4x-%4.4x%8.8x\n",
ntohl(*(uint32_t *)&value.int128.b[0]),
ntohs(*(uint16_t *)&value.int128.b[4]),
ntohs(*(uint16_t *)&value.int128.b[6]),
ntohs(*(uint16_t *)&value.int128.b[8]),
ntohs(*(uint16_t *)&value.int128.b[10]),
ntohl(*(uint32_t *)&value.int128.b[12]));
break;
case SDP_DATA_INT8:
SDP_GET8(value.int8, start);
printf("int8 %d\n", value.int8);
break;
case SDP_DATA_INT16:
SDP_GET16(value.int16, start);
printf("int16 %d\n", value.int16);
break;
case SDP_DATA_INT32:
SDP_GET32(value.int32, start);
printf("int32 %d\n", value.int32);
break;
case SDP_DATA_INT64:
SDP_GET64(value.int64, start);
printf("int64 %ju\n", value.int64);
break;
case SDP_DATA_UUID16:
SDP_GET16(value.uint16, start);
printf("uuid16 %#4.4x - %s\n", value.uint16,
sdp_uuid2desc(value.uint16));
break;
case SDP_DATA_UUID32:
SDP_GET32(value.uint32, start);
printf("uuid32 %#8.8x\n", value.uint32);
break;
case SDP_DATA_STR8:
SDP_GET8(value.uint8, start);
printf("str8 %*.*s\n", value.uint8, value.uint8, start);
start += value.uint8;
break;
case SDP_DATA_STR16:
SDP_GET16(value.uint16, start);
printf("str16 %*.*s\n", value.uint16, value.uint16, start);
start += value.uint16;
break;
case SDP_DATA_STR32:
SDP_GET32(value.uint32, start);
printf("str32 %*.*s\n", value.uint32, value.uint32, start);
start += value.uint32;
break;
case SDP_DATA_BOOL:
SDP_GET8(value.uint8, start);
printf("bool %d\n", value.uint8);
break;
case SDP_DATA_SEQ8:
SDP_GET8(value.uint8, start);
printf("seq8 %d\n", value.uint8);
sdp_print(level + 1, start, start + value.uint8);
start += value.uint8;
break;
case SDP_DATA_SEQ16:
SDP_GET16(value.uint16, start);
printf("seq16 %d\n", value.uint16);
sdp_print(level + 1, start, start + value.uint16);
start += value.uint16;
break;
case SDP_DATA_SEQ32:
SDP_GET32(value.uint32, start);
printf("seq32 %d\n", value.uint32);
sdp_print(level + 1, start, start + value.uint32);
start += value.uint32;
break;
case SDP_DATA_ALT8:
SDP_GET8(value.uint8, start);
printf("alt8 %d\n", value.uint8);
sdp_print(level + 1, start, start + value.uint8);
start += value.uint8;
break;
case SDP_DATA_ALT16:
SDP_GET16(value.uint16, start);
printf("alt16 %d\n", value.uint16);
sdp_print(level + 1, start, start + value.uint16);
start += value.uint16;
break;
case SDP_DATA_ALT32:
SDP_GET32(value.uint32, start);
printf("alt32 %d\n", value.uint32);
sdp_print(level + 1, start, start + value.uint32);
start += value.uint32;
break;
case SDP_DATA_URL8:
SDP_GET8(value.uint8, start);
printf("url8 %*.*s\n", value.uint8, value.uint8, start);
start += value.uint8;
break;
case SDP_DATA_URL16:
SDP_GET16(value.uint16, start);
printf("url16 %*.*s\n", value.uint16, value.uint16, start);
start += value.uint16;
break;
case SDP_DATA_URL32:
SDP_GET32(value.uint32, start);
printf("url32 %*.*s\n", value.uint32, value.uint32, start);
start += value.uint32;
break;
default:
printf("unknown data type: %#02x\n", *start ++);
break;
}
}
}