src/eir.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2011  Nokia Corporation
   6  *  Copyright (C) 2011  Marcel Holtmann <marcel@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
  25 #ifdef HAVE_CONFIG_H
  26 #include <config.h>
  27 #endif
  28
  29 #include <errno.h>
  30 #include <stdlib.h>
  31 #include <stdint.h>
  32 #include <glib.h>
  33
  34 #include <bluetooth/bluetooth.h>
  35 #include <bluetooth/hci.h>
  36 #include <bluetooth/sdp.h>
  37
  38 #include "glib-helper.h"
  39 #include "eir.h"
  40
  41 void eir_data_free(struct eir_data *eir)
  42 {
  43         g_slist_free_full(eir->services, g_free);
  44         eir->services = NULL;
  45         g_free(eir->name);
  46         eir->name = NULL;
  47 }
  48
  49 static void eir_parse_uuid16(struct eir_data *eir, void *data, uint8_t len)
  50 {
  51         uint16_t *uuid16 = data;
  52         uuid_t service;
  53         char *uuid_str;
  54         unsigned int i;
  55
  56         service.type = SDP_UUID16;
  57         for (i = 0; i < len / 2; i++, uuid16++) {
  58                 service.value.uuid16 = btohs(bt_get_unaligned(uuid16));
  59                 uuid_str = bt_uuid2string(&service);
  60                 eir->services = g_slist_append(eir->services, uuid_str);
  61         }
  62 }
  63
  64 static void eir_parse_uuid32(struct eir_data *eir, void *data, uint8_t len)
  65 {
  66         uint32_t *uuid32 = data;
  67         uuid_t service;
  68         char *uuid_str;
  69         unsigned int i;
  70
  71         service.type = SDP_UUID32;
  72         for (i = 0; i < len / 4; i++, uuid32++) {
  73                 service.value.uuid32 = btohl(bt_get_unaligned(uuid32));
  74                 uuid_str = bt_uuid2string(&service);
  75                 eir->services = g_slist_append(eir->services, uuid_str);
  76         }
  77 }
  78
  79 static void eir_parse_uuid128(struct eir_data *eir, uint8_t *data, uint8_t len)
  80 {
  81         uint8_t *uuid_ptr = data;
  82         uuid_t service;
  83         char *uuid_str;
  84         unsigned int i;
  85         int k;
  86
  87         service.type = SDP_UUID128;
  88         for (i = 0; i < len / 16; i++) {
  89                 for (k = 0; k < 16; k++)
  90                         service.value.uuid128.data[k] = uuid_ptr[16 - k - 1];
  91                 uuid_str = bt_uuid2string(&service);
  92                 eir->services = g_slist_append(eir->services, uuid_str);
  93                 uuid_ptr += 16;
  94         }
  95 }
  96
  97 int eir_parse(struct eir_data *eir, uint8_t *eir_data, uint8_t eir_len)
  98 {
  99         uint16_t len = 0;
 100
 101         eir->flags = -1;
 102
 103         /* No EIR data to parse */
 104         if (eir_data == NULL)
 105                 return 0;
 106
 107         while (len < eir_len - 1) {
 108                 uint8_t field_len = eir_data[0];
 109                 uint8_t data_len, *data = &eir_data[2];
 110
 111                 /* Check for the end of EIR */
 112                 if (field_len == 0)
 113                         break;
 114
 115                 len += field_len + 1;
 116
 117                 /* Do not continue EIR Data parsing if got incorrect length */
 118                 if (len > eir_len)
 119                         break;
 120
 121                 data_len = field_len - 1;
 122
 123                 switch (eir_data[1]) {
 124                 case EIR_UUID16_SOME:
 125                 case EIR_UUID16_ALL:
 126                         eir_parse_uuid16(eir, data, data_len);
 127                         break;
 128
 129                 case EIR_UUID32_SOME:
 130                 case EIR_UUID32_ALL:
 131                         eir_parse_uuid32(eir, data, data_len);
 132                         break;
 133
 134                 case EIR_UUID128_SOME:
 135                 case EIR_UUID128_ALL:
 136                         eir_parse_uuid128(eir, data, data_len);
 137                         break;
 138
 139                 case EIR_FLAGS:
 140                         if (data_len > 0)
 141                                 eir->flags = *data;
 142                         break;
 143
 144                 case EIR_NAME_SHORT:
 145                 case EIR_NAME_COMPLETE:
 146                         /* Some vendors put a NUL byte terminator into
 147                          * the name */
 148                         while (data_len > 0 && data[data_len - 1] == '\0')
 149                                 data_len--;
 150
 151                         if (!g_utf8_validate((char *) data, data_len, NULL))
 152                                 break;
 153
 154                         g_free(eir->name);
 155
 156                         eir->name = g_strndup((char *) data, data_len);
 157                         eir->name_complete = eir_data[1] == EIR_NAME_COMPLETE;
 158                         break;
 159
 160                 case EIR_CLASS_OF_DEV:
 161                         if (data_len < 3)
 162                                 break;
 163                         memcpy(eir->dev_class, data, 3);
 164                         break;
 165
 166                 case EIR_GAP_APPEARANCE:
 167                         if (data_len < 2)
 168                                 break;
 169                         eir->appearance = bt_get_le16(data);
 170                         break;
 171                 }
 172
 173                 eir_data += field_len + 1;
 174         }
 175
 176         return 0;
 177 }
 178
 179 #define SIZEOF_UUID128 16
 180
 181 static void eir_generate_uuid128(GSList *list, uint8_t *ptr, uint16_t *eir_len)
 182 {
 183         int i, k, uuid_count = 0;
 184         uint16_t len = *eir_len;
 185         uint8_t *uuid128;
 186         gboolean truncated = FALSE;
 187
 188         /* Store UUIDs in place, skip 2 bytes to write type and length later */
 189         uuid128 = ptr + 2;
 190
 191         for (; list; list = list->next) {
 192                 struct uuid_info *uuid = list->data;
 193                 uint8_t *uuid128_data = uuid->uuid.value.uuid128.data;
 194
 195                 if (uuid->uuid.type != SDP_UUID128)
 196                         continue;
 197
 198                 /* Stop if not enough space to put next UUID128 */
 199                 if ((len + 2 + SIZEOF_UUID128) > HCI_MAX_EIR_LENGTH) {
 200                         truncated = TRUE;
 201                         break;
 202                 }
 203
 204                 /* Check for duplicates, EIR data is Little Endian */
 205                 for (i = 0; i < uuid_count; i++) {
 206                         for (k = 0; k < SIZEOF_UUID128; k++) {
 207                                 if (uuid128[i * SIZEOF_UUID128 + k] !=
 208                                         uuid128_data[SIZEOF_UUID128 - 1 - k])
 209                                         break;
 210                         }
 211                         if (k == SIZEOF_UUID128)
 212                                 break;
 213                 }
 214
 215                 if (i < uuid_count)
 216                         continue;
 217
 218                 /* EIR data is Little Endian */
 219                 for (k = 0; k < SIZEOF_UUID128; k++)
 220                         uuid128[uuid_count * SIZEOF_UUID128 + k] =
 221                                 uuid128_data[SIZEOF_UUID128 - 1 - k];
 222
 223                 len += SIZEOF_UUID128;
 224                 uuid_count++;
 225         }
 226
 227         if (uuid_count > 0 || truncated) {
 228                 /* EIR Data length */
 229                 ptr[0] = (uuid_count * SIZEOF_UUID128) + 1;
 230                 /* EIR Data type */
 231                 ptr[1] = truncated ? EIR_UUID128_SOME : EIR_UUID128_ALL;
 232                 len += 2;
 233                 *eir_len = len;
 234         }
 235 }
 236
 237 void eir_create(const char *name, int8_t tx_power, uint16_t did_vendor,
 238                         uint16_t did_product, uint16_t did_version,
 239                         uint16_t did_source, GSList *uuids, uint8_t *data)
 240 {
 241         GSList *l;
 242         uint8_t *ptr = data;
 243         uint16_t eir_len = 0;
 244         uint16_t uuid16[HCI_MAX_EIR_LENGTH / 2];
 245         int i, uuid_count = 0;
 246         gboolean truncated = FALSE;
 247         size_t name_len;
 248
 249         name_len = strlen(name);
 250
 251         if (name_len > 0) {
 252                 /* EIR Data type */
 253                 if (name_len > 48) {
 254                         name_len = 48;
 255                         ptr[1] = EIR_NAME_SHORT;
 256                 } else
 257                         ptr[1] = EIR_NAME_COMPLETE;
 258
 259                 /* EIR Data length */
 260                 ptr[0] = name_len + 1;
 261
 262                 memcpy(ptr + 2, name, name_len);
 263
 264                 eir_len += (name_len + 2);
 265                 ptr += (name_len + 2);
 266         }
 267
 268         if (tx_power != 0) {
 269                 *ptr++ = 2;
 270                 *ptr++ = EIR_TX_POWER;
 271                 *ptr++ = (uint8_t) tx_power;
 272                 eir_len += 3;
 273         }
 274
 275         if (did_vendor != 0x0000) {
 276                 *ptr++ = 9;
 277                 *ptr++ = EIR_DEVICE_ID;
 278                 *ptr++ = (did_source & 0x00ff);
 279                 *ptr++ = (did_source & 0xff00) >> 8;
 280                 *ptr++ = (did_vendor & 0x00ff);
 281                 *ptr++ = (did_vendor & 0xff00) >> 8;
 282                 *ptr++ = (did_product & 0x00ff);
 283                 *ptr++ = (did_product & 0xff00) >> 8;
 284                 *ptr++ = (did_version & 0x00ff);
 285                 *ptr++ = (did_version & 0xff00) >> 8;
 286                 eir_len += 10;
 287         }
 288
 289         /* Group all UUID16 types */
 290         for (l = uuids; l != NULL; l = g_slist_next(l)) {
 291                 struct uuid_info *uuid = l->data;
 292
 293                 if (uuid->uuid.type != SDP_UUID16)
 294                         continue;
 295
 296                 if (uuid->uuid.value.uuid16 < 0x1100)
 297                         continue;
 298
 299                 if (uuid->uuid.value.uuid16 == PNP_INFO_SVCLASS_ID)
 300                         continue;
 301
 302                 /* Stop if not enough space to put next UUID16 */
 303                 if ((eir_len + 2 + sizeof(uint16_t)) > HCI_MAX_EIR_LENGTH) {
 304                         truncated = TRUE;
 305                         break;
 306                 }
 307
 308                 /* Check for duplicates */
 309                 for (i = 0; i < uuid_count; i++)
 310                         if (uuid16[i] == uuid->uuid.value.uuid16)
 311                                 break;
 312
 313                 if (i < uuid_count)
 314                         continue;
 315
 316                 uuid16[uuid_count++] = uuid->uuid.value.uuid16;
 317                 eir_len += sizeof(uint16_t);
 318         }
 319
 320         if (uuid_count > 0) {
 321                 /* EIR Data length */
 322                 ptr[0] = (uuid_count * sizeof(uint16_t)) + 1;
 323                 /* EIR Data type */
 324                 ptr[1] = truncated ? EIR_UUID16_SOME : EIR_UUID16_ALL;
 325
 326                 ptr += 2;
 327                 eir_len += 2;
 328
 329                 for (i = 0; i < uuid_count; i++) {
 330                         *ptr++ = (uuid16[i] & 0x00ff);
 331                         *ptr++ = (uuid16[i] & 0xff00) >> 8;
 332                 }
 333         }
 334
 335         /* Group all UUID128 types */
 336         if (eir_len <= HCI_MAX_EIR_LENGTH - 2)
 337                 eir_generate_uuid128(uuids, ptr, &eir_len);
 338 }
 339
 340 gboolean eir_has_data_type(uint8_t *data, size_t len, uint8_t type)
 341 {
 342         uint8_t field_len;
 343         size_t parsed = 0;
 344 #ifdef __TIZEN_PATCH__
 345         if (len < 2)
 346                 return FALSE;
 347 #endif
 348         while (parsed < len - 1) {
 349                 field_len = data[0];
 350
 351                 if (field_len == 0)
 352                         break;
 353
 354                 parsed += field_len + 1;
 355
 356                 if (parsed > len)
 357                         break;
 358
 359                 if (data[1] == type)
 360                         return TRUE;
 361
 362                 data += field_len + 1;
 363         }
 364
 365         return FALSE;
 366 }
 367
 368 size_t eir_append_data(uint8_t *eir, size_t eir_len, uint8_t type,
 369                                                 uint8_t *data, size_t data_len)
 370 {
 371         eir[eir_len++] = sizeof(type) + data_len;
 372         eir[eir_len++] = type;
 373         memcpy(&eir[eir_len], data, data_len);
 374         eir_len += data_len;
 375
 376         return eir_len;
 377 }
 378
 379 size_t eir_length(uint8_t *eir, size_t maxlen)
 380 {
 381         uint8_t field_len;
 382         size_t parsed = 0, length = 0;
 383
 384         while (parsed < maxlen - 1) {
 385                 field_len = eir[0];
 386
 387                 if (field_len == 0)
 388                         break;
 389
 390                 parsed += field_len + 1;
 391
 392                 if (parsed > maxlen)
 393                         break;
 394
 395                 length = parsed;
 396                 eir += field_len + 1;
 397         }
 398
 399         return length;
 400 }