コード例 #1
0
ファイル: bnep_test.c プロジェクト: hearingthings/btstack
static void send_arp_probe_ipv4(void){

    // "random address"
    static uint8_t requested_address[4] = {169, 254, 1, 0};
    requested_address[3]++;

    int pos = setup_ethernet_header(1, 0, 1, NETWORK_TYPE_IPv4); 
    net_store_16(network_buffer, pos, HARDWARE_TYPE_ETHERNET);
    pos += 2;
    net_store_16(network_buffer, pos, NETWORK_TYPE_IPv4);
    pos += 2;
    network_buffer[pos++] = 6; // Hardware length (HLEN) - 6 MAC  Address
    network_buffer[pos++] = 4; // Protocol length (PLEN) - 4 IPv4 Address
    net_store_16(network_buffer, pos, ARP_OPERATION_REQUEST); 
    pos += 2;
    BD_ADDR_COPY(&network_buffer[pos], local_addr); // Sender Hardware Address (SHA)
    pos += 6;
    bzero(&network_buffer[pos], 4);                 // Sender Protocol Adress (SPA)
    pos += 4;
    BD_ADDR_COPY(&network_buffer[pos], other_addr); // Target Hardware Address (THA) (ignored for requests)
    pos += 6;
    memcpy(&network_buffer[pos], requested_address, 4);
    pos += 4;
    // magically, add some extra bytes for Ethernet padding
    pos += 18;
    send_buffer(pos);
}
コード例 #2
0
ファイル: sdp.c プロジェクト: yourskp/btstack
static int sdp_create_error_response(uint16_t transaction_id, uint16_t error_code) {
    sdp_response_buffer[0] = SDP_ErrorResponse;
    net_store_16(sdp_response_buffer, 1, transaction_id);
    net_store_16(sdp_response_buffer, 3, 2);
    net_store_16(sdp_response_buffer, 5, error_code); // invalid syntax
    return 7;
}
コード例 #3
0
ファイル: sdp_client.c プロジェクト: Echoskope/KreyosFirmware
static void sdpc_trysend()
{
  uint8_t buf[128];
  if (state != SENDING) return;
  if (!l2cap_cid) return;
  if (!l2cap_can_send_packet_now(l2cap_cid)) return;

  buf[0] = SDP_ServiceSearchAttributeRequest;
  net_store_16(buf, 1, transitionid++);

  uint8_t *param = &buf[5];
  de_create_sequence(param);
  de_add_number(param, DE_UUID, DE_SIZE_16, serviceids[current_server]);
  uint16_t size = de_get_len(param);
  net_store_16(param, size, 30); // max length is 30 bytes
  size+=2;
  de_create_sequence(param + size);
  //de_add_number(param + size, DE_UINT, DE_SIZE_16, SDP_ServiceClassIDList);
  de_add_number(param + size, DE_UINT, DE_SIZE_16, SDP_ProtocolDescriptorList);
  size += de_get_len(param + size);
  param[size++] = 0;

  net_store_16(buf, 3, size);
  hexdump(buf, size + 5);
  int err = l2cap_send_internal(l2cap_cid, buf, size + 5);
  if (!err)
  {
    state = RECV;
    log_info("sdp request sent.\n");
  }
  else
  {
    log_info("sdpc_trysend l2cap_send_internal error: %d\n", err);
  }
}
コード例 #4
0
ファイル: sdp_client.c プロジェクト: XHelaoshi/USER
static uint16_t setup_service_search_request(uint8_t * data){
    uint16_t offset = 0;
    transactionID++;
    // uint8_t SDP_PDU_ID_t.SDP_ServiceSearchRequest;
    data[offset++] = SDP_ServiceSearchRequest;
    // uint16_t transactionID
    net_store_16(data, offset, transactionID);
    offset += 2;

    // param legnth
    offset += 2;

    // parameters: 
    //     ServiceSearchPattern - DES (min 1 UUID, max 12)
    uint16_t serviceSearchPatternLen = de_get_len(serviceSearchPattern);
    memcpy(data + offset, serviceSearchPattern, serviceSearchPatternLen);
    offset += serviceSearchPatternLen;

    //     MaximumAttributeByteCount - uint16_t  0x0007 - 0xffff -> mtu
    net_store_16(data, offset, mtu);
    offset += 2;

    //     ContinuationState - uint8_t number of cont. bytes N<=16 
    data[offset++] = continuationStateLen;
    //                       - N-bytes previous response from server
    memcpy(data + offset, continuationState, continuationStateLen);
    offset += continuationStateLen;

    // uint16_t paramLength 
    net_store_16(data, 3, offset - 5);

    return offset;
}
コード例 #5
0
ファイル: bnep_test.c プロジェクト: hearingthings/btstack
static void send_ndp_probe_ipv6(void){

    uint8_t ipv6_header[] = {
        // ip
        0x60, 0x00, 0x00, 0x00, // version (6) + traffic class (8) + flow label (24)
        0x00, 0x00,   58, 0x01, // payload length(16), next header = IPv6-ICMP, hop limit
        0x00, 0x00, 0x00, 0x00, // source IP address
        0x00, 0x00, 0x00, 0x00, // source IP address
        0x00, 0x00, 0x00, 0x00, // source IP address
        0x00, 0x00, 0x00, 0x00, // source IP address
        0xfe, 0x80, 0x00, 0x00, // destination IP address
        0x00, 0x00, 0x00, 0x00, // destination IP address
        0x00, 0x00, 0x00, 0x00, // destination IP address
        0x00, 0x00, 0x00, 0x00, // destination IP address
    };

    uint8_t icmp_packet[] = {
        // icmp
        0x87, 0x00, 0x00, 0x00, // type: 0x80 PING Request, code = 0, checksum(16)
        0x00, 0x00, 0x00, 0x00  // message
    };

    // ethernet header
    int pos = setup_ethernet_header(1, 0, 0, NETWORK_TYPE_IPv6);

    // ipv6
    int payload_length = sizeof(icmp_packet);
    net_store_16(ipv6_header, 4, payload_length);
    // source address ::
    // dest addresss - Modified EUI-64
    // ipv6_header[24..31] = FE80::
    ipv6_header[32] = local_addr[0] ^ 0x2;
    ipv6_header[33] = local_addr[1];
    ipv6_header[34] = local_addr[2];
    ipv6_header[35] = 0xff;
    ipv6_header[36] = 0xfe;
    ipv6_header[37] = local_addr[3];
    ipv6_header[38] = local_addr[4];
    ipv6_header[39] = local_addr[5];
    int checksum = calc_internet_checksum(&ipv6_header[8], 32);
    checksum = sum_ones_complement(checksum, payload_length);
    checksum = sum_ones_complement(checksum, ipv6_header[6] << 8);
    memcpy(&network_buffer[pos], ipv6_header, sizeof(ipv6_header));
    pos += sizeof(ipv6_header);

    // icmp
    uint16_t icmp_checksum = calc_internet_checksum(icmp_packet, sizeof(icmp_packet));
    net_store_16(icmp_packet, 2, icmp_checksum);    
    memcpy(&network_buffer[pos], icmp_packet, sizeof(icmp_packet));
    pos += sizeof(icmp_packet);

    // send
    send_buffer(pos);    
}
コード例 #6
0
ファイル: bnep_test.c プロジェクト: hearingthings/btstack
/* Untested */
static void send_ping_request_ipv6(void){

    uint8_t ipv6_header[] = {
        // ip
        0x60, 0x00, 0x00, 0x00, // version (4) + traffic class (8) + flow label (24)
        0x00, 0x00,   58, 0x01, // payload length(16), next header = IPv6-ICMP, hop limit
        0x00, 0x00, 0x00, 0x00, // source IP address
        0x00, 0x00, 0x00, 0x00, // source IP address
        0x00, 0x00, 0x00, 0x00, // source IP address
        0x00, 0x00, 0x00, 0x00, // source IP address
        0x00, 0x00, 0x00, 0x00, // destination IP address
        0x00, 0x00, 0x00, 0x00, // destination IP address
        0x00, 0x00, 0x00, 0x00, // destination IP address
        0x00, 0x00, 0x00, 0x00, // destination IP address
    };

    uint8_t icmp_packet[] = {
        // icmp
        0x80, 0x00, 0x00, 0x00, // type: 0x80 PING Request, codde = 0, checksum(16)
        0x00, 0x00, 0x00, 0x00  // message
    };

    // ethernet header
    int pos = setup_ethernet_header(1, 0, 0, NETWORK_TYPE_IPv4); // IPv4
    
    // ipv6
    int payload_length = sizeof(icmp_packet);
    net_store_16(ipv6_header, 4, payload_length);
    // TODO: also set src/dest ip address
    int checksum = calc_internet_checksum(&ipv6_header[8], 32);
    checksum = sum_ones_complement(checksum, payload_length);
    checksum = sum_ones_complement(checksum, 58 << 8);
    net_store_16(icmp_packet, 2, checksum);
    memcpy(&network_buffer[pos], ipv6_header, sizeof(ipv6_header));
    pos += sizeof(ipv6_header);

    // icmp
    uint16_t icmp_checksum = calc_internet_checksum(icmp_packet, sizeof(icmp_packet));
    net_store_16(icmp_packet, 2, icmp_checksum);    
    memcpy(&network_buffer[pos], icmp_packet, sizeof(icmp_packet));
    pos += sizeof(icmp_packet);

    // send
    send_buffer(pos);
}
コード例 #7
0
ファイル: bnep_test.c プロジェクト: hearingthings/btstack
static void send_llmnr_request_ipv4(void){

    uint8_t ipv4_header[] = {
        0x45, 0x00, 0x00, 0x00, // version + ihl, dscp } ecn, total len
        0x00, 0x00, 0x00, 0x00, // identification (16), flags + fragment offset
        0x01, 0x11, 0x00, 0x00, // time to live, procotol: UDP, checksum (16),
        192,   168, 167,  152,  // source IP address
        224,     0,   0,  252,  // destination IP address
    };

    uint8_t udp_header[8];
    uint8_t llmnr_packet[12];

    uint8_t dns_data[] = { 0x08, 0x61, 0x61, 0x70,  0x70, 0x6c, 0x65, 0x74, 0x76,
                           0x05, 0x6c, 0x6f, 0x63,  0x61, 0x6c, 0x00, 0x00,
                           0x01, 0x00, 0x01 }; 

    // ethernet header
    int pos = setup_ethernet_header(1, 0, 0, NETWORK_TYPE_IPv4); // IPv4

    // ipv4
    int total_length = sizeof(ipv4_header) + sizeof(udp_header) + sizeof (llmnr_packet) + sizeof(dns_data);
    net_store_16(ipv4_header, 2, total_length);
    uint16_t ipv4_checksum = calc_internet_checksum(ipv4_header, sizeof(ipv4_header));
    net_store_16(ipv4_header, 10, ~ipv4_checksum);    
    // TODO: also set src/dest ip address
    memcpy(&network_buffer[pos], ipv4_header, sizeof(ipv4_header));
    pos += sizeof(ipv4_header);

    // udp packet
    net_store_16(udp_header, 0, 5355);   // source port
    net_store_16(udp_header, 2, 5355);   // destination port
    net_store_16(udp_header, 4, sizeof(udp_header) + sizeof(llmnr_packet) + sizeof(dns_data));
    net_store_16(udp_header, 6, 0);      // no checksum
    memcpy(&network_buffer[pos], udp_header, sizeof(udp_header));
    pos += sizeof(udp_header);

    // llmnr packet
    bzero(llmnr_packet, sizeof(llmnr_packet));
    net_store_16(llmnr_packet, 0, 0x1234);  // transaction id
    net_store_16(llmnr_packet, 4, 1);   // one query

    memcpy(&network_buffer[pos], llmnr_packet, sizeof(llmnr_packet));
    pos += sizeof(llmnr_packet);

    memcpy(&network_buffer[pos], dns_data, sizeof(dns_data));
    pos += sizeof(dns_data);

    // send
    send_buffer(pos);
}
コード例 #8
0
ファイル: bnep_test.c プロジェクト: hearingthings/btstack
static void send_ping_response_ipv4(void){

    uint8_t ipv4_header[] = {
        // ip
        0x45, 0x00, 0x00, 0x00,   // version + ihl, dscp } ecn, total len
        0x00, 0x00, 0x00, 0x00, // identification (16), flags + fragment offset
        0x01, 0x01, 0x00, 0x00, // time to live, procotol: icmp, checksum (16),
        0x00, 0x00, 0x00, 0x00, // source IP address
        0x00, 0x00, 0x00, 0x00, // destination IP address
    };

    uint8_t icmp_packet[] = {
        // icmp
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00
    };

    // ethernet header
    int pos = setup_ethernet_header(1, 0, 0, NETWORK_TYPE_IPv4); // IPv4
    
    // ipv4
    int total_length = sizeof(ipv4_header) + sizeof(icmp_packet);
    net_store_16(ipv4_header, 2, total_length);
    uint16_t ipv4_checksum = calc_internet_checksum(ipv4_header, sizeof(ipv4_header));
    net_store_16(ipv4_header, 10, ipv4_checksum);    
    // TODO: also set src/dest ip address
    memcpy(&network_buffer[pos], ipv4_header, sizeof(ipv4_header));
    pos += sizeof(ipv4_header);

    // icmp
    uint16_t icmp_checksum = calc_internet_checksum(icmp_packet, sizeof(icmp_packet));
    net_store_16(icmp_packet, 2, icmp_checksum);    
    memcpy(&network_buffer[pos], icmp_packet, sizeof(icmp_packet));
    pos += sizeof(icmp_packet);

    // send
    send_buffer(pos);
}
コード例 #9
0
ファイル: sdp_client.c プロジェクト: XHelaoshi/USER
static uint16_t setup_service_attribute_request(uint8_t * data){

    uint16_t offset = 0;
    transactionID++;
    // uint8_t SDP_PDU_ID_t.SDP_ServiceSearchRequest;
    data[offset++] = SDP_ServiceAttributeRequest;
    // uint16_t transactionID
    net_store_16(data, offset, transactionID);
    offset += 2;

    // param legnth
    offset += 2;

    // parameters: 
    //     ServiceRecordHandle
    net_store_32(data, offset, serviceRecordHandle);
    offset += 4;

    //     MaximumAttributeByteCount - uint16_t  0x0007 - 0xffff -> mtu
    net_store_16(data, offset, mtu);
    offset += 2;

    //     AttibuteIDList  
    uint16_t attributeIDListLen = de_get_len(attributeIDList);
    memcpy(data + offset, attributeIDList, attributeIDListLen);
    offset += attributeIDListLen;

    //     ContinuationState - uint8_t number of cont. bytes N<=16 
    data[offset++] = continuationStateLen;
    //                       - N-bytes previous response from server
    memcpy(data + offset, continuationState, continuationStateLen);
    offset += continuationStateLen;

    // uint16_t paramLength 
    net_store_16(data, 3, offset - 5);

    return offset;
}
コード例 #10
0
ファイル: bnep_test.c プロジェクト: hearingthings/btstack
static uint16_t setup_ethernet_header(int src_compressed, int dst_compressed, int broadcast, uint16_t network_protocol_type){
    // setup packet
    int pos = 0;
    // destination
    if (broadcast){
        BD_ADDR_COPY(&network_buffer[pos], broadcast_addr);
    } else {
        BD_ADDR_COPY(&network_buffer[pos], dst_compressed ? pts_addr : other_addr);
    }
    pos += 6;
    // source
    BD_ADDR_COPY(&network_buffer[pos], src_compressed ? local_addr   : other_addr);
    pos += 6;
    net_store_16(network_buffer, pos, network_protocol_type);
    pos += 2;
    return pos;
}
コード例 #11
0
ファイル: sdp_query_rfcomm.c プロジェクト: 4hasann/ioio
void sdp_query_rfcomm_channel_and_name_for_uuid(bd_addr_t remote, uint16_t uuid){
    net_store_16(des_serviceSearchPattern, 3, uuid);
    sdp_query_rfcomm_channel_and_name_for_search_pattern(remote, (uint8_t*)des_serviceSearchPattern);
}
コード例 #12
0
ファイル: bnep_test.c プロジェクト: hearingthings/btstack
static void send_llmnr_request_ipv6(void){
    // https://msdn.microsoft.com/en-us/library/dd240361.aspx
    uint8_t ipv6_header[] = {
        0x60, 0x00, 0x00, 0x00, // version (6) + traffic class (8) + flow label (24)
        0x00, 0x00,   17, 0x01, // payload length(16), next header = UDP, hop limit
        0xfe, 0x80, 0x00, 0x00, // source IP address
        0x00, 0x00, 0x00, 0x00, // source IP address
        0xd9, 0xf6, 0xce, 0x2e, // source IP address
        0x48, 0x75, 0xab, 0x03, // source IP address
        0xff, 0x02, 0x00, 0x00, // destination IP address
        0x00, 0x00, 0x00, 0x00, // destination IP address
        0x00, 0x00, 0x00, 0x00, // destination IP address
        0x00, 0x01, 0x00, 0x03, // destination IP address
    };

    uint8_t udp_header[8];
    uint8_t llmnr_packet[12];

    uint8_t dns_data[] = { 0x08, 0x61, 0x61, 0x70,  0x70, 0x6c, 0x65, 0x74, 0x76,
                           0x05, 0x6c, 0x6f, 0x63,  0x61, 0x6c, 0x00, 0x00,
                           0x01, 0x00, 0x01 }; 

    int payload_length = sizeof(udp_header) + sizeof(llmnr_packet) + sizeof(dns_data);

    // llmnr header
    bzero(llmnr_packet, sizeof(llmnr_packet));
    net_store_16(llmnr_packet, 0, 0x1235);  // transaction id
    net_store_16(llmnr_packet, 4, 1);   // one query

    // ipv6 header
    net_store_16(ipv6_header, 4, payload_length);

    // udp header
    bzero(udp_header, sizeof(udp_header));
    net_store_16(udp_header, 0, 5355);   // source port
    net_store_16(udp_header, 2, 5355);   // destination port
    net_store_16(udp_header, 4, payload_length);
    int checksum = calc_internet_checksum(&ipv6_header[8], 32);
    checksum = sum_ones_complement(checksum, payload_length);       // payload len
    checksum = sum_ones_complement(checksum, ipv6_header[6] << 8);  // next header 
    checksum = sum_ones_complement(checksum, calc_internet_checksum(udp_header, sizeof(udp_header)));
    checksum = sum_ones_complement(checksum, calc_internet_checksum(llmnr_packet, sizeof(llmnr_packet)));
    checksum = sum_ones_complement(checksum, calc_internet_checksum(dns_data, sizeof(dns_data)));
    net_store_16(udp_header, 6, ~checksum);

    // ethernet header
    int pos = setup_ethernet_header(1, 0, 1, NETWORK_TYPE_IPv6); // IPv6

    memcpy(&network_buffer[pos], ipv6_header, sizeof(ipv6_header));
    pos += sizeof(ipv6_header);

    memcpy(&network_buffer[pos], udp_header, sizeof(udp_header));
    pos += sizeof(udp_header);

    memcpy(&network_buffer[pos], llmnr_packet, sizeof(llmnr_packet));
    pos += sizeof(llmnr_packet);

    memcpy(&network_buffer[pos], dns_data, sizeof(dns_data));
    pos += sizeof(dns_data);

    // send
    send_buffer(pos);
}
コード例 #13
0
ファイル: sdp.c プロジェクト: yourskp/btstack
int sdp_handle_service_search_attribute_request(uint8_t * packet, uint16_t remote_mtu) {

    // SDP header before attribute sevice list: 7
    // Continuation, worst case: 5

    // get request details
    uint16_t  transaction_id = READ_NET_16(packet, 1);
    // not used yet - uint16_t  param_len = READ_NET_16(packet, 3);
    uint8_t * serviceSearchPattern = &packet[5];
    uint16_t  serviceSearchPatternLen = de_get_len(serviceSearchPattern);
    uint16_t  maximumAttributeByteCount = READ_NET_16(packet, 5 + serviceSearchPatternLen);
    uint8_t * attributeIDList = &packet[5+serviceSearchPatternLen+2];
    uint16_t  attributeIDListLen = de_get_len(attributeIDList);
    uint8_t * continuationState = &packet[5+serviceSearchPatternLen+2+attributeIDListLen];

    // calc maximumAttributeByteCount based on remote MTU, SDP header and reserved Continuation block
    uint16_t maximumAttributeByteCount2 = remote_mtu - 12;
    if (maximumAttributeByteCount2 < maximumAttributeByteCount) {
        maximumAttributeByteCount = maximumAttributeByteCount2;
    }

    // continuation state contains: index of next service record to examine
    // continuation state contains: byte offset into this service record
    uint16_t continuation_service_index = 0;
    uint16_t continuation_offset = 0;
    if (continuationState[0] == 4) {
        continuation_service_index = READ_NET_16(continuationState, 1);
        continuation_offset = READ_NET_16(continuationState, 3);
    }

    // log_info("--> sdp_handle_service_search_attribute_request, cont %u/%u, max %u", continuation_service_index, continuation_offset, maximumAttributeByteCount);

    // AttributeLists - starts at offset 7
    uint16_t pos = 7;

    // add DES with total size for first request
    if (continuation_service_index == 0 && continuation_offset == 0) {
        uint16_t total_response_size = sdp_get_size_for_service_search_attribute_response(serviceSearchPattern, attributeIDList);
        de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, total_response_size);
        // log_info("total response size %u", total_response_size);
        pos += 3;
        maximumAttributeByteCount -= 3;
    }

    // create attribute list
    int      first_answer = 1;
    int      continuation = 0;
    uint16_t current_service_index = 0;
    linked_item_t *it = (linked_item_t *) sdp_service_records;
    for ( ; it ; it = it->next, ++current_service_index) {
        service_record_item_t * item = (service_record_item_t *) it;

        if (current_service_index < continuation_service_index ) continue;
        if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue;

        if (continuation_offset == 0) {

            // get size of this record
            uint16_t filtered_attributes_size = spd_get_filtered_size(item->service_record, attributeIDList);

            // stop if complete record doesn't fits into response but we already have a partial response
            if ((filtered_attributes_size + 3 > maximumAttributeByteCount) && !first_answer) {
                continuation = 1;
                break;
            }

            // store DES
            de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, filtered_attributes_size);
            pos += 3;
            maximumAttributeByteCount -= 3;
        }

        first_answer = 0;

        // copy maximumAttributeByteCount from record
        uint16_t bytes_used;
        int complete = sdp_filter_attributes_in_attributeIDList(item->service_record, attributeIDList, continuation_offset, maximumAttributeByteCount, &bytes_used, &sdp_response_buffer[pos]);
        pos += bytes_used;
        maximumAttributeByteCount -= bytes_used;

        if (complete) {
            continuation_offset = 0;
            continue;
        }

        continuation = 1;
        continuation_offset += bytes_used;
        break;
    }

    uint16_t attributeListsByteCount = pos - 7;

    // Continuation State
    if (continuation) {
        sdp_response_buffer[pos++] = 4;
        net_store_16(sdp_response_buffer, pos, (uint16_t) current_service_index);
        pos += 2;
        net_store_16(sdp_response_buffer, pos, continuation_offset);
        pos += 2;
    } else {
        // complete
        sdp_response_buffer[pos++] = 0;
    }

    // create SDP header
    sdp_response_buffer[0] = SDP_ServiceSearchAttributeResponse;
    net_store_16(sdp_response_buffer, 1, transaction_id);
    net_store_16(sdp_response_buffer, 3, pos - 5);  // size of variable payload
    net_store_16(sdp_response_buffer, 5, attributeListsByteCount);

    return pos;
}
コード例 #14
0
ファイル: sdp.c プロジェクト: yourskp/btstack
int sdp_handle_service_attribute_request(uint8_t * packet, uint16_t remote_mtu) {

    // get request details
    uint16_t  transaction_id = READ_NET_16(packet, 1);
    // not used yet - uint16_t  param_len = READ_NET_16(packet, 3);
    uint32_t  serviceRecordHandle = READ_NET_32(packet, 5);
    uint16_t  maximumAttributeByteCount = READ_NET_16(packet, 9);
    uint8_t * attributeIDList = &packet[11];
    uint16_t  attributeIDListLen = de_get_len(attributeIDList);
    uint8_t * continuationState = &packet[11+attributeIDListLen];

    // calc maximumAttributeByteCount based on remote MTU
    uint16_t maximumAttributeByteCount2 = remote_mtu - (7+3);
    if (maximumAttributeByteCount2 < maximumAttributeByteCount) {
        maximumAttributeByteCount = maximumAttributeByteCount2;
    }

    // continuation state contains the offset into the complete response
    uint16_t continuation_offset = 0;
    if (continuationState[0] == 2) {
        continuation_offset = READ_NET_16(continuationState, 1);
    }

    // get service record
    service_record_item_t * item = sdp_get_record_for_handle(serviceRecordHandle);
    if (!item) {
        // service record handle doesn't exist
        return sdp_create_error_response(transaction_id, 0x0002); /// invalid Service Record Handle
    }


    // AttributeList - starts at offset 7
    uint16_t pos = 7;

    if (continuation_offset == 0) {

        // get size of this record
        uint16_t filtered_attributes_size = spd_get_filtered_size(item->service_record, attributeIDList);

        // store DES
        de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, filtered_attributes_size);
        maximumAttributeByteCount -= 3;
        pos += 3;
    }

    // copy maximumAttributeByteCount from record
    uint16_t bytes_used;
    int complete = sdp_filter_attributes_in_attributeIDList(item->service_record, attributeIDList, continuation_offset, maximumAttributeByteCount, &bytes_used, &sdp_response_buffer[pos]);
    pos += bytes_used;

    uint16_t attributeListByteCount = pos - 7;

    if (complete) {
        sdp_response_buffer[pos++] = 0;
    } else {
        continuation_offset += bytes_used;
        sdp_response_buffer[pos++] = 2;
        net_store_16(sdp_response_buffer, pos, continuation_offset);
        pos += 2;
    }

    // header
    sdp_response_buffer[0] = SDP_ServiceAttributeResponse;
    net_store_16(sdp_response_buffer, 1, transaction_id);
    net_store_16(sdp_response_buffer, 3, pos - 5);  // size of variable payload
    net_store_16(sdp_response_buffer, 5, attributeListByteCount);

    return pos;
}
コード例 #15
0
ファイル: sdp.c プロジェクト: yourskp/btstack
int sdp_handle_service_search_request(uint8_t * packet, uint16_t remote_mtu) {

    // get request details
    uint16_t  transaction_id = READ_NET_16(packet, 1);
    // not used yet - uint16_t  param_len = READ_NET_16(packet, 3);
    uint8_t * serviceSearchPattern = &packet[5];
    uint16_t  serviceSearchPatternLen = de_get_len(serviceSearchPattern);
    uint16_t  maximumServiceRecordCount = READ_NET_16(packet, 5 + serviceSearchPatternLen);
    uint8_t * continuationState = &packet[5+serviceSearchPatternLen+2];

    // calc maxumumServiceRecordCount based on remote MTU
    uint16_t maxNrServiceRecordsPerResponse = (remote_mtu - (9+3))/4;

    // continuation state contains index of next service record to examine
    int      continuation = 0;
    uint16_t continuation_index = 0;
    if (continuationState[0] == 2) {
        continuation_index = READ_NET_16(continuationState, 1);
    }

    // get and limit total count
    linked_item_t *it;
    uint16_t total_service_count   = 0;
    for (it = (linked_item_t *) sdp_service_records; it ; it = it->next) {
        service_record_item_t * item = (service_record_item_t *) it;
        if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue;
        total_service_count++;
    }
    if (total_service_count > maximumServiceRecordCount) {
        total_service_count = maximumServiceRecordCount;
    }

    // ServiceRecordHandleList at 9
    uint16_t pos = 9;
    uint16_t current_service_count  = 0;
    uint16_t current_service_index  = 0;
    uint16_t matching_service_count = 0;
    for (it = (linked_item_t *) sdp_service_records; it ; it = it->next, ++current_service_index) {
        service_record_item_t * item = (service_record_item_t *) it;

        if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue;
        matching_service_count++;

        if (current_service_index < continuation_index) continue;

        net_store_32(sdp_response_buffer, pos, item->service_record_handle);
        pos += 4;
        current_service_count++;

        if (matching_service_count >= total_service_count) break;

        if (current_service_count >= maxNrServiceRecordsPerResponse) {
            continuation = 1;
            continuation_index = current_service_index + 1;
            break;
        }
    }

    // Store continuation state
    if (continuation) {
        sdp_response_buffer[pos++] = 2;
        net_store_16(sdp_response_buffer, pos, continuation_index);
        pos += 2;
    } else {
        sdp_response_buffer[pos++] = 0;
    }

    // header
    sdp_response_buffer[0] = SDP_ServiceSearchResponse;
    net_store_16(sdp_response_buffer, 1, transaction_id);
    net_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload
    net_store_16(sdp_response_buffer, 5, total_service_count);
    net_store_16(sdp_response_buffer, 7, current_service_count);

    return pos;
}
コード例 #16
0
ファイル: sdp.c プロジェクト: ajsb85/ioio
void sdp_test(){
    
    const uint16_t remote_mtu = 48;
    uint8_t allAttributeIDs[20];  //
    
    // create an attribute list
    de_create_sequence(allAttributeIDs);
    de_add_number(allAttributeIDs, DE_UINT, DE_SIZE_32, 0x0000ffff);
        
    // create two records with 2 attributes each
    de_create_sequence(record);
    de_add_number(record, DE_UINT, DE_SIZE_16, SDP_ServiceRecordHandle); 
    de_add_number(record, DE_UINT, DE_SIZE_32, 0x10001);
    de_add_number(record, DE_UINT, DE_SIZE_16, SDP_ServiceClassIDList);
    de_add_number(record, DE_UUID, DE_SIZE_16, 0x0001);
    de_add_number(record, DE_UINT, DE_SIZE_16, SDP_BrowseGroupList);
    de_add_number(record, DE_UUID, DE_SIZE_16, 0x0001);
    uint32_t handle_1 = sdp_register_service_internal(NULL, record);
    de_dump_data_element(record);
    
    de_create_sequence(record);
    de_add_number(record, DE_UINT, DE_SIZE_16, SDP_ServiceRecordHandle);
    de_add_number(record, DE_UINT, DE_SIZE_32, 0x10002);
    de_add_number(record, DE_UINT, DE_SIZE_16, SDP_ServiceClassIDList);
    de_add_number(record, DE_UUID, DE_SIZE_16, 0x0002);
    de_add_number(record, DE_UINT, DE_SIZE_16, SDP_BrowseGroupList);
    de_add_number(record, DE_UUID, DE_SIZE_16, 0x0001);
    sdp_register_service_internal(NULL, record);
    de_dump_data_element(record);

    uint16_t size = spd_get_filtered_size(record, allAttributeIDs);
    printf("Attribute size %u\n", size);
    
    uint16_t transactionID = 1;
    uint8_t * attributeIDList;
    uint16_t attributeIDListLen;
    uint16_t response_pos;
    uint8_t * serviceSearchPattern;
    uint16_t serviceSearchPatternLen;
    
#if 1
    // sdp_handle_service_search_request
    uint16_t nr_services = 1;
    request[0] = SDP_ServiceSearchRequest;
    net_store_16(request, 1, transactionID++); // transaction ID
    serviceSearchPattern = &request[5];
    de_create_sequence(serviceSearchPattern);
    {
        de_add_number(serviceSearchPattern, DE_UUID, DE_SIZE_16, 0x0001);
    }
    serviceSearchPatternLen = de_get_len(serviceSearchPattern);
    net_store_16(request, 5 + serviceSearchPatternLen, 2);  // max 
    request[5 + serviceSearchPatternLen + 2] = 0;   // cont
    sdp_handle_service_search_request(request, 16);
    dump_service_search_response();
    memcpy(request + 5 + serviceSearchPatternLen + 2, sdp_response_buffer + 9 + nr_services*4, 3); 
    sdp_handle_service_search_request(request, remote_mtu);
    dump_service_search_response();
#endif
    
#if 1
    // sdp_handle_service_attribute_request
    request[0] = SDP_ServiceAttributeRequest;
    net_store_16(request, 1, transactionID++); // transaction ID
    net_store_32(request, 5, handle_1); // record handle
    net_store_16(request, 9, 11); // max bytes
    attributeIDList = request + 11;
    de_create_sequence(attributeIDList);
    de_add_number(attributeIDList, DE_UINT, DE_SIZE_32, 0x0000ffff);
    attributeIDListLen = de_get_len(attributeIDList);
    request[11+attributeIDListLen] = 0;
    response_pos = 0;
    while(1) {
        sdp_handle_service_attribute_request(request, remote_mtu);

        uint16_t attributeListByteCount = READ_NET_16(sdp_response_buffer, 5);
        memcpy( &response[response_pos], &sdp_response_buffer[7], attributeListByteCount);
        response_pos += attributeListByteCount;
        
        printf("attributeListByteCount %u\n", attributeListByteCount);
        printf("Continuation %u\n", sdp_response_buffer[7+attributeListByteCount]);
        if (sdp_response_buffer[7+attributeListByteCount] == 0) break;
        printf("Continuation {%u}\n", READ_NET_16(sdp_response_buffer, 7+attributeListByteCount+1));
        memcpy(request+11+attributeIDListLen, sdp_response_buffer+7+attributeListByteCount, 3);
    }
    de_dump_data_element(response);
#endif
    
#if 1
    // sdp_handle_service_search_attribute_request
    request[0] = SDP_ServiceSearchAttributeRequest;
    net_store_16(request, 1, transactionID++); // transaction ID
    serviceSearchPattern = &request[5];
    de_create_sequence(serviceSearchPattern);
    {
        de_add_number(serviceSearchPattern, DE_UUID, DE_SIZE_16, 0x0001);
    }
    serviceSearchPatternLen = de_get_len(serviceSearchPattern);
    net_store_16(request, 5 + serviceSearchPatternLen, 11); // MaximumAttributeByteCount:
    attributeIDList = request + 5 + serviceSearchPatternLen + 2;
    de_create_sequence(attributeIDList);
    de_add_number(attributeIDList, DE_UINT, DE_SIZE_32, 0x0000ffff);
    attributeIDListLen = de_get_len(attributeIDList);
    request[5 + serviceSearchPatternLen + 2 + attributeIDListLen] = 0;
    response_pos = 0;
    while (1) {
        sdp_handle_service_search_attribute_request(request, remote_mtu);
        uint16_t attributeListByteCount = READ_NET_16(sdp_response_buffer, 5);
        memcpy( &response[response_pos], &sdp_response_buffer[7], attributeListByteCount);
        response_pos += attributeListByteCount;
        
        printf("attributeListByteCount %u\n", attributeListByteCount);
        printf("Continuation %u\n", sdp_response_buffer[7+attributeListByteCount]);
        if (sdp_response_buffer[7+attributeListByteCount] == 0) break;
        printf("Continuation {%u,%u}\n", READ_NET_16(sdp_response_buffer, 7+attributeListByteCount+1),
               READ_NET_16(sdp_response_buffer, 7+attributeListByteCount+3));
        memcpy(request+5 + serviceSearchPatternLen + 2 + attributeIDListLen, sdp_response_buffer+7+attributeListByteCount, 5);
    }
    de_dump_data_element(response);
#endif
    exit(0);
}