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 common_profile_create_service_record_handle( uint8_t *buf, uint8_t const * const eob, uint8_t const *data, uint32_t datalen) { if (buf + 5 > eob) return (-1); SDP_PUT8(SDP_DATA_UINT32, buf); SDP_PUT32(((provider_p) data)->handle, buf); return (5); }
static int32_t sd_profile_create_service_database_state( uint8_t *buf, uint8_t const * const eob, uint8_t const *data, uint32_t datalen) { uint32_t change_state = provider_get_change_state(); if (buf + 5 > eob) return (-1); SDP_PUT8(SDP_DATA_UINT32, buf); SDP_PUT32(change_state, buf); return (5); }
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); }