1 /* 2 * 3 * BlueZ - Bluetooth protocol stack for Linux 4 * 5 * Copyright (C) 2011 Nokia Corporation 6 * Copyright (C) 2011 Marcel Holtmann <marcel (at) holtmann.org> 7 * 8 * 9 * This program is free software; you can redistribute it and/or modify 10 * it under the terms of the GNU General Public License as published by 11 * the Free Software Foundation; either version 2 of the License, or 12 * (at your option) any later version. 13 * 14 * This program is distributed in the hope that it will be useful, 15 * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 * GNU General Public License for more details. 18 * 19 * You should have received a copy of the GNU General Public License 20 * along with this program; if not, write to the Free Software 21 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 22 * 23 */ 24 #include <errno.h> 25 #include <stdlib.h> 26 #include <stdint.h> 27 #include <glib.h> 28 29 #include <bluetooth/bluetooth.h> 30 #include <bluetooth/hci.h> 31 #include <bluetooth/sdp.h> 32 33 #include "glib-helper.h" 34 #include "eir.h" 35 36 #define EIR_FLAGS 0x01 /* flags */ 37 #define EIR_UUID16_SOME 0x02 /* 16-bit UUID, more available */ 38 #define EIR_UUID16_ALL 0x03 /* 16-bit UUID, all listed */ 39 #define EIR_UUID32_SOME 0x04 /* 32-bit UUID, more available */ 40 #define EIR_UUID32_ALL 0x05 /* 32-bit UUID, all listed */ 41 #define EIR_UUID128_SOME 0x06 /* 128-bit UUID, more available */ 42 #define EIR_UUID128_ALL 0x07 /* 128-bit UUID, all listed */ 43 #define EIR_NAME_SHORT 0x08 /* shortened local name */ 44 #define EIR_NAME_COMPLETE 0x09 /* complete local name */ 45 #define EIR_TX_POWER 0x0A /* transmit power level */ 46 #define EIR_DEVICE_ID 0x10 /* device ID */ 47 48 void eir_data_free(struct eir_data *eir) 49 { 50 g_slist_foreach(eir->services, (GFunc) g_free, NULL); 51 g_slist_free(eir->services); 52 g_free(eir->name); 53 } 54 55 int eir_parse(struct eir_data *eir, uint8_t *eir_data) 56 { 57 uint16_t len = 0; 58 size_t total; 59 size_t uuid16_count = 0; 60 size_t uuid32_count = 0; 61 size_t uuid128_count = 0; 62 uint8_t *uuid16 = NULL; 63 uint8_t *uuid32 = NULL; 64 uint8_t *uuid128 = NULL; 65 uuid_t service; 66 char *uuid_str; 67 unsigned int i; 68 69 eir->flags = -1; 70 71 /* No EIR data to parse */ 72 if (eir_data == NULL) 73 return 0; 74 75 while (len < HCI_MAX_EIR_LENGTH - 1) { 76 uint8_t field_len = eir_data[0]; 77 78 /* Check for the end of EIR */ 79 if (field_len == 0) 80 break; 81 82 switch (eir_data[1]) { 83 case EIR_UUID16_SOME: 84 case EIR_UUID16_ALL: 85 uuid16_count = field_len / 2; 86 uuid16 = &eir_data[2]; 87 break; 88 case EIR_UUID32_SOME: 89 case EIR_UUID32_ALL: 90 uuid32_count = field_len / 4; 91 uuid32 = &eir_data[2]; 92 break; 93 case EIR_UUID128_SOME: 94 case EIR_UUID128_ALL: 95 uuid128_count = field_len / 16; 96 uuid128 = &eir_data[2]; 97 break; 98 case EIR_FLAGS: 99 eir->flags = eir_data[2]; 100 break; 101 case EIR_NAME_SHORT: 102 case EIR_NAME_COMPLETE: 103 if (g_utf8_validate((char *) &eir_data[2], 104 field_len - 1, NULL)) 105 eir->name = g_strndup((char *) &eir_data[2], 106 field_len - 1); 107 else 108 eir->name = g_strdup(""); 109 eir->name_complete = eir_data[1] == EIR_NAME_COMPLETE; 110 break; 111 } 112 113 len += field_len + 1; 114 eir_data += field_len + 1; 115 } 116 117 /* Bail out if got incorrect length */ 118 if (len > HCI_MAX_EIR_LENGTH) 119 return -EINVAL; 120 121 total = uuid16_count + uuid32_count + uuid128_count; 122 123 /* No UUIDs were parsed, so skip code below */ 124 if (!total) 125 return 0; 126 127 /* Generate uuids in SDP format (EIR data is Little Endian) */ 128 service.type = SDP_UUID16; 129 for (i = 0; i < uuid16_count; i++) { 130 uint16_t val16 = uuid16[1]; 131 132 val16 = (val16 << 8) + uuid16[0]; 133 service.value.uuid16 = val16; 134 uuid_str = bt_uuid2string(&service); 135 eir->services = g_slist_append(eir->services, uuid_str); 136 uuid16 += 2; 137 } 138 139 service.type = SDP_UUID32; 140 for (i = uuid16_count; i < uuid32_count + uuid16_count; i++) { 141 uint32_t val32 = uuid32[3]; 142 int k; 143 144 for (k = 2; k >= 0; k--) 145 val32 = (val32 << 8) + uuid32[k]; 146 147 service.value.uuid32 = val32; 148 uuid_str = bt_uuid2string(&service); 149 eir->services = g_slist_append(eir->services, uuid_str); 150 uuid32 += 4; 151 } 152 153 service.type = SDP_UUID128; 154 for (i = uuid32_count + uuid16_count; i < total; i++) { 155 int k; 156 157 for (k = 0; k < 16; k++) 158 service.value.uuid128.data[k] = uuid128[16 - k - 1]; 159 160 uuid_str = bt_uuid2string(&service); 161 eir->services = g_slist_append(eir->services, uuid_str); 162 uuid128 += 16; 163 } 164 165 return 0; 166 } 167 168 #define SIZEOF_UUID128 16 169 170 static void eir_generate_uuid128(GSList *list, uint8_t *ptr, uint16_t *eir_len) 171 { 172 int i, k, uuid_count = 0; 173 uint16_t len = *eir_len; 174 uint8_t *uuid128; 175 gboolean truncated = FALSE; 176 177 /* Store UUIDs in place, skip 2 bytes to write type and length later */ 178 uuid128 = ptr + 2; 179 180 for (; list; list = list->next) { 181 struct uuid_info *uuid = list->data; 182 uint8_t *uuid128_data = uuid->uuid.value.uuid128.data; 183 184 if (uuid->uuid.type != SDP_UUID128) 185 continue; 186 187 /* Stop if not enough space to put next UUID128 */ 188 if ((len + 2 + SIZEOF_UUID128) > HCI_MAX_EIR_LENGTH) { 189 truncated = TRUE; 190 break; 191 } 192 193 /* Check for duplicates, EIR data is Little Endian */ 194 for (i = 0; i < uuid_count; i++) { 195 for (k = 0; k < SIZEOF_UUID128; k++) { 196 if (uuid128[i * SIZEOF_UUID128 + k] != 197 uuid128_data[SIZEOF_UUID128 - 1 - k]) 198 break; 199 } 200 if (k == SIZEOF_UUID128) 201 break; 202 } 203 204 if (i < uuid_count) 205 continue; 206 207 /* EIR data is Little Endian */ 208 for (k = 0; k < SIZEOF_UUID128; k++) 209 uuid128[uuid_count * SIZEOF_UUID128 + k] = 210 uuid128_data[SIZEOF_UUID128 - 1 - k]; 211 212 len += SIZEOF_UUID128; 213 uuid_count++; 214 } 215 216 if (uuid_count > 0 || truncated) { 217 /* EIR Data length */ 218 ptr[0] = (uuid_count * SIZEOF_UUID128) + 1; 219 /* EIR Data type */ 220 ptr[1] = truncated ? EIR_UUID128_SOME : EIR_UUID128_ALL; 221 len += 2; 222 *eir_len = len; 223 } 224 } 225 226 void eir_create(const char *name, int8_t tx_power, uint16_t did_vendor, 227 uint16_t did_product, uint16_t did_version, 228 GSList *uuids, uint8_t *data) 229 { 230 GSList *l; 231 uint8_t *ptr = data; 232 uint16_t eir_len = 0; 233 uint16_t uuid16[HCI_MAX_EIR_LENGTH / 2]; 234 int i, uuid_count = 0; 235 gboolean truncated = FALSE; 236 size_t name_len; 237 238 name_len = strlen(name); 239 240 if (name_len > 0) { 241 /* EIR Data type */ 242 if (name_len > 48) { 243 name_len = 48; 244 ptr[1] = EIR_NAME_SHORT; 245 } else 246 ptr[1] = EIR_NAME_COMPLETE; 247 248 /* EIR Data length */ 249 ptr[0] = name_len + 1; 250 251 memcpy(ptr + 2, name, name_len); 252 253 eir_len += (name_len + 2); 254 ptr += (name_len + 2); 255 } 256 257 if (tx_power != 0) { 258 *ptr++ = 2; 259 *ptr++ = EIR_TX_POWER; 260 *ptr++ = (uint8_t) tx_power; 261 eir_len += 3; 262 } 263 264 if (did_vendor != 0x0000) { 265 uint16_t source = 0x0002; 266 *ptr++ = 9; 267 *ptr++ = EIR_DEVICE_ID; 268 *ptr++ = (source & 0x00ff); 269 *ptr++ = (source & 0xff00) >> 8; 270 *ptr++ = (did_vendor & 0x00ff); 271 *ptr++ = (did_vendor & 0xff00) >> 8; 272 *ptr++ = (did_product & 0x00ff); 273 *ptr++ = (did_product & 0xff00) >> 8; 274 *ptr++ = (did_version & 0x00ff); 275 *ptr++ = (did_version & 0xff00) >> 8; 276 eir_len += 10; 277 } 278 279 /* Group all UUID16 types */ 280 for (l = uuids; l != NULL; l = g_slist_next(l)) { 281 struct uuid_info *uuid = l->data; 282 283 if (uuid->uuid.type != SDP_UUID16) 284 continue; 285 286 if (uuid->uuid.value.uuid16 < 0x1100) 287 continue; 288 289 if (uuid->uuid.value.uuid16 == PNP_INFO_SVCLASS_ID) 290 continue; 291 292 /* Stop if not enough space to put next UUID16 */ 293 if ((eir_len + 2 + sizeof(uint16_t)) > HCI_MAX_EIR_LENGTH) { 294 truncated = TRUE; 295 break; 296 } 297 298 /* Check for duplicates */ 299 for (i = 0; i < uuid_count; i++) 300 if (uuid16[i] == uuid->uuid.value.uuid16) 301 break; 302 303 if (i < uuid_count) 304 continue; 305 306 uuid16[uuid_count++] = uuid->uuid.value.uuid16; 307 eir_len += sizeof(uint16_t); 308 } 309 310 if (uuid_count > 0) { 311 /* EIR Data length */ 312 ptr[0] = (uuid_count * sizeof(uint16_t)) + 1; 313 /* EIR Data type */ 314 ptr[1] = truncated ? EIR_UUID16_SOME : EIR_UUID16_ALL; 315 316 ptr += 2; 317 eir_len += 2; 318 319 for (i = 0; i < uuid_count; i++) { 320 *ptr++ = (uuid16[i] & 0x00ff); 321 *ptr++ = (uuid16[i] & 0xff00) >> 8; 322 } 323 } 324 325 /* Group all UUID128 types */ 326 if (eir_len <= HCI_MAX_EIR_LENGTH - 2) 327 eir_generate_uuid128(uuids, ptr, &eir_len); 328 } 329
