usr.sbin/bluetooth/hccontrol/adv_data.c

   1 /*-
   2  * adv_data.c
   3  *
   4  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
   5
   6  * Copyright (c) 2020 Marc Veldman <marc@bumblingdork.com>
   7  * All rights reserved.
   8  *
   9  * Redistribution and use in source and binary forms, with or without
  10  * modification, are permitted provided that the following conditions
  11  * are met:
  12  * 1. Redistributions of source code must retain the above copyright
  13  *    notice, this list of conditions and the following disclaimer.
  14  * 2. Redistributions in binary form must reproduce the above copyright
  15  *    notice, this list of conditions and the following disclaimer in the
  16  *    documentation and/or other materials provided with the distribution.
  17  *
  18  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  19  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  20  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  21  * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  22  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  23  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  24  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  25  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  26  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  27  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  28  * SUCH DAMAGE.
  29  *
  30  * $Id$
  31  * $FreeBSD$
  32  */
  33
  34 #include <sys/types.h>
  35 #include <stdio.h>
  36 #include <string.h>
  37 #include <uuid.h>
  38 #define L2CAP_SOCKET_CHECKED
  39 #include <bluetooth.h>
  40 #include "hccontrol.h"
  41
  42 static char* const adv_data2str(int len, uint8_t* data, char* buffer,
  43         int size);
  44 static char* const adv_name2str(int len, uint8_t* advdata, char* buffer,
  45         int size);
  46 static char* const adv_uuid2str(int datalen, uint8_t* data, char* buffer,
  47         int size);
  48
  49 void dump_adv_data(int len, uint8_t* advdata)
  50 {
  51         int n=0;
  52         fprintf(stdout, "\tADV Data: ");
  53         for (n = 0; n < len+1; n++) {
  54                 fprintf(stdout, "%02x ", advdata[n]);
  55         }
  56         fprintf(stdout, "\n");
  57 }
  58
  59 void print_adv_data(int len, uint8_t* advdata)
  60 {
  61         int n=0;
  62         while(n < len)
  63         {
  64                 char buffer[2048];
  65                 uint8_t datalen = advdata[n];
  66                 uint8_t datatype = advdata[++n];
  67                 /* Skip type */
  68                 ++n;
  69                 datalen--;
  70                 switch (datatype) {
  71                         case 0x01:
  72                                 fprintf(stdout,
  73                                         "\tFlags: %s\n",
  74                                         adv_data2str(
  75                                                 datalen,
  76                                                 &advdata[n],
  77                                                 buffer,
  78                                                 sizeof(buffer)));
  79                                 break;
  80                         case 0x02:
  81                                 fprintf(stdout,
  82                                         "\tIncomplete list of service"
  83                                         " class UUIDs (16-bit): %s\n",
  84                                         adv_data2str(
  85                                                 datalen,
  86                                                 &advdata[n],
  87                                                 buffer,
  88                                                 sizeof(buffer)));
  89                                 break;
  90                         case 0x03:
  91                                 fprintf(stdout,
  92                                         "\tComplete list of service "
  93                                         "class UUIDs (16-bit): %s\n",
  94                                         adv_data2str(
  95                                                 datalen,
  96                                                 &advdata[n],
  97                                                 buffer,
  98                                                 sizeof(buffer)));
  99                                 break;
 100                         case 0x07:
 101                                 fprintf(stdout,
 102                                         "\tComplete list of service "
 103                                         "class UUIDs (128 bit): %s\n",
 104                                         adv_uuid2str(
 105                                                 datalen,
 106                                                 &advdata[n],
 107                                                 buffer,
 108                                                 sizeof(buffer)));
 109                                 break;
 110                         case 0x08:
 111                                 fprintf(stdout,
 112                                         "\tShortened local name: %s\n",
 113                                         adv_name2str(
 114                                                 datalen,
 115                                                 &advdata[n],
 116                                                 buffer,
 117                                                 sizeof(buffer)));
 118                                 break;
 119                         case 0x09:
 120                                 fprintf(stdout,
 121                                         "\tComplete local name: %s\n",
 122                                         adv_name2str(
 123                                                 datalen,
 124                                                 &advdata[n],
 125                                                 buffer,
 126                                                 sizeof(buffer)));
 127                                 break;
 128                         case 0x0a:
 129                                 fprintf(stdout,
 130                                         "\tTx Power level: %d dBm\n",
 131                                                 (int8_t)advdata[n]);
 132                                 break;
 133                         case 0x0d:
 134                                 fprintf(stdout,
 135                                         "\tClass of device: %s\n",
 136                                         adv_data2str(
 137                                                 datalen,
 138                                                 &advdata[n],
 139                                                 buffer,
 140                                                 sizeof(buffer)));
 141                                 break;
 142                         case 0x16:
 143                                 fprintf(stdout,
 144                                         "\tService data: %s\n",
 145                                         adv_data2str(
 146                                                 datalen,
 147                                                 &advdata[n],
 148                                                 buffer,
 149                                                 sizeof(buffer)));
 150                                 break;
 151                         case 0x19:
 152                                 fprintf(stdout,
 153                                         "\tAppearance: %s\n",
 154                                         adv_data2str(
 155                                                 datalen,
 156                                                 &advdata[n],
 157                                                 buffer,
 158                                                 sizeof(buffer)));
 159                                 break;
 160                         case 0xff:
 161                                 fprintf(stdout,
 162                                         "\tManufacturer: %s\n",
 163                                         hci_manufacturer2str(
 164                                                 advdata[n]|advdata[n+1]<<8));
 165                                 fprintf(stdout,
 166                                         "\tManufacturer specific data: %s\n",
 167                                         adv_data2str(
 168                                                 datalen-2,
 169                                                 &advdata[n+2],
 170                                                 buffer,
 171                                                 sizeof(buffer)));
 172                                 break;
 173                         default:
 174                                 fprintf(stdout,
 175                                         "\tUNKNOWN datatype: %02x data %s\n",
 176                                         datatype,
 177                                         adv_data2str(
 178                                                 datalen,
 179                                                 &advdata[n],
 180                                                 buffer,
 181                                                 sizeof(buffer)));
 182                 }
 183                 n += datalen;
 184         }
 185 }
 186
 187 static char* const adv_data2str(int datalen, uint8_t* data, char* buffer,
 188         int size)
 189 {
 190         int i = 0;
 191         char tmpbuf[5];
 192
 193         if (buffer == NULL)
 194                 return NULL;
 195
 196         memset(buffer, 0, size);
 197
 198         while(i < datalen) {
 199                 (void)snprintf(tmpbuf, sizeof(tmpbuf), "%02x ", data[i]);
 200                 /* Check if buffer is full */
 201                 if (strlcat(buffer, tmpbuf, size) > size)
 202                         break;
 203                 i++;
 204         }
 205         return buffer;
 206 }
 207
 208 static char* const adv_name2str(int datalen, uint8_t* data, char* buffer,
 209         int size)
 210 {
 211         if (buffer == NULL)
 212                 return NULL;
 213
 214         memset(buffer, 0, size);
 215
 216         (void)strlcpy(buffer, (char*)data, datalen+1);
 217         return buffer;
 218 }
 219
 220 static char* const adv_uuid2str(int datalen, uint8_t* data, char* buffer,
 221         int size)
 222 {
 223         int i;
 224         uuid_t uuid;
 225         uint32_t ustatus;
 226         char* tmpstr;
 227
 228         if (buffer == NULL)
 229                 return NULL;
 230
 231         memset(buffer, 0, size);
 232         if (datalen < 16)
 233                 return buffer;
 234         uuid.time_low = le32dec(data+12);
 235         uuid.time_mid = le16dec(data+10);
 236         uuid.time_hi_and_version = le16dec(data+8);
 237         uuid.clock_seq_hi_and_reserved = data[7];
 238         uuid.clock_seq_low = data[6];
 239         for(i = 0; i < _UUID_NODE_LEN; i++){
 240                 uuid.node[i] = data[5 - i];
 241         }
 242         uuid_to_string(&uuid, &tmpstr, &ustatus);
 243         if(ustatus == uuid_s_ok) {
 244                 strlcpy(buffer, tmpstr, size);
 245         }
 246         free(tmpstr);
 247
 248         return buffer;
 249 }