1 /* 2 * Copyright (C) 2014 BlueKitchen GmbH 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 3. Neither the name of the copyright holders nor the names of 14 * contributors may be used to endorse or promote products derived 15 * from this software without specific prior written permission. 16 * 4. Any redistribution, use, or modification is done solely for 17 * personal benefit and not for any commercial purpose or for 18 * monetary gain. 19 * 20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS 24 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 * SUCH DAMAGE. 32 * 33 * Please inquire about commercial licensing options at 34 * [email protected] 35 * 36 */ 37 38 #define __BTSTACK_FILE__ "obex_iterator.c" 39 40 #include "btstack_config.h" 41 42 #include <stdint.h> 43 #include <stdlib.h> 44 45 #include "hci_cmd.h" 46 #include "btstack_debug.h" 47 #include "hci.h" 48 #include "bluetooth_sdp.h" 49 #include "classic/sdp_client_rfcomm.h" 50 #include "btstack_event.h" 51 52 #include "classic/obex.h" 53 #include "classic/obex_iterator.h" 54 55 static int obex_packet_header_offset_for_opcode(uint8_t opcode){ 56 switch (opcode){ 57 case OBEX_OPCODE_SETPATH: 58 return 5; 59 case OBEX_OPCODE_CONNECT: 60 return 7; 61 default: 62 return 3; 63 } 64 } 65 66 static void obex_iterator_init(obex_iterator_t *context, int header_offset, const uint8_t * packet_data, uint16_t packet_len){ 67 memset(context, 0, sizeof(obex_iterator_t)); 68 context->data = packet_data + header_offset; 69 context->length = packet_len - header_offset; 70 } 71 72 void obex_iterator_init_with_request_packet(obex_iterator_t *context, const uint8_t * packet_data, uint16_t packet_len){ 73 int header_offset = obex_packet_header_offset_for_opcode(packet_data[0]); 74 obex_iterator_init(context, header_offset, packet_data, packet_len); 75 } 76 77 void obex_iterator_init_with_response_packet(obex_iterator_t *context, uint8_t request_opcode, const uint8_t * packet_data, uint16_t packet_len){ 78 int header_offset = request_opcode == OBEX_OPCODE_CONNECT ? 7 : 3; 79 obex_iterator_init(context, header_offset, packet_data, packet_len); 80 } 81 82 int obex_iterator_has_more(const obex_iterator_t * context){ 83 return context->offset < context->length; 84 } 85 86 void obex_iterator_next(obex_iterator_t * context){ 87 int len = 0; 88 const uint8_t * data = context->data + context->offset; 89 int encoding = data[0] >> 6; 90 switch (encoding){ 91 case 0: 92 case 1: 93 // 16-bit length info prefixed 94 len = 2 + big_endian_read_16(data, 1); 95 break; 96 case 2: 97 // 8-bit value 98 len = 1; 99 break; 100 case 3: 101 // 32-bit value 102 len = 4; 103 break; 104 // avoid compiler warning about unused cases (by unclever compilers) 105 default: 106 break; 107 } 108 context->offset += 1 + len; 109 } 110 111 // OBEX packet header access functions 112 113 // @note BODY/END-OF-BODY headers might be incomplete 114 uint8_t obex_iterator_get_hi(const obex_iterator_t * context){ 115 return context->data[context->offset]; 116 } 117 uint8_t obex_iterator_get_data_8(const obex_iterator_t * context){ 118 return context->data[context->offset+1]; 119 } 120 uint32_t obex_iterator_get_data_32(const obex_iterator_t * context){ 121 return big_endian_read_32(context->data, context->offset + 1); 122 } 123 uint32_t obex_iterator_get_data_len(const obex_iterator_t * context){ 124 const uint8_t * data = context->data + context->offset; 125 int encoding = data[0] >> 6; 126 switch (encoding){ 127 case 0: 128 case 1: 129 // 16-bit length info prefixed 130 return big_endian_read_16(data, 1) - 3; 131 case 2: 132 // 8-bit value 133 return 1; 134 case 3: 135 // 32-bit value 136 return 4; 137 // avoid compiler warning about unused cases (by unclever compilers) 138 default: 139 return 0; 140 } 141 } 142 143 const uint8_t * obex_iterator_get_data(const obex_iterator_t * context){ 144 const uint8_t * data = context->data + context->offset; 145 int encoding = data[0] >> 6; 146 switch (encoding){ 147 case 0: 148 case 1: 149 // 16-bit length info prefixed 150 return &data[3]; 151 default: 152 // 8-bit value 153 // 32-bit value 154 return &data[1]; 155 } 156 } 157 158 void obex_dump_packet(uint8_t request_opcode, uint8_t * packet, uint16_t size){ 159 // printf("RCV: '"); 160 // printf_hexdump(packet, size); 161 obex_iterator_t it; 162 printf("Opcode: 0x%02x\n", packet[0]); 163 for (obex_iterator_init_with_response_packet(&it, request_opcode, packet, size); obex_iterator_has_more(&it) ; obex_iterator_next(&it)){ 164 uint8_t hi = obex_iterator_get_hi(&it); 165 printf("HI: %x - ", hi); 166 uint8_t encoding = hi >> 6; 167 uint16_t len; 168 switch (encoding){ 169 case 0: 170 case 1: 171 len = obex_iterator_get_data_len(&it); 172 printf_hexdump(obex_iterator_get_data(&it), len); 173 break; 174 case 2: 175 printf("%02x\n", obex_iterator_get_data_8(&it)); 176 break; 177 case 3: 178 printf("%08x\n", (int) obex_iterator_get_data_32(&it)); 179 break; 180 } 181 182 } 183 } 184