bt-service/services/mesh/bt-service-mesh-util.c

   1 /*
   2  * Bluetooth-frwk
   3  *
   4  * Copyright (c) 2020 Samsung Electronics Co., Ltd.
   5  *
   6  * @author: Anupam Roy <anupam.r@samsung.com>
   7  *
   8  * Licensed under the Apache License, Version 2.0 (the "License");
   9  * you may not use this file except in compliance with the License.
  10  * You may obtain a copy of the License at
  11  *
  12  *              http://www.apache.org/licenses/LICENSE-2.0
  13  *
  14  * Unless required by applicable law or agreed to in writing, software
  15  * distributed under the License is distributed on an "AS IS" BASIS,
  16  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  17  * See the License for the specific language governing prov_err_strs and
  18  * limitations under the License.
  19  *
  20  */
  21 #include <dirent.h>
  22 #include <ftw.h>
  23 #include <unistd.h>
  24 #include <stdio.h>
  25
  26 #include <glib.h>
  27 #include <dlog.h>
  28 #include <limits.h>
  29 #include <time.h>
  30 #include <sys/time.h>
  31
  32 #include "bt-service-common.h"
  33 #include "bt-service-util.h"
  34
  35 #include "bt-service-mesh-util.h"
  36 #include <ell/ell.h>
  37
  38 /* Multiply used Zero array */
  39 static const uint8_t zero[16] = { 0, };
  40
  41 static bool __mesh_util_crypto_aes_cmac_one(
  42         const uint8_t key[16], const void *msg,
  43                 size_t msg_len, uint8_t res[16])
  44 {
  45         void *checksum;
  46         bool result;
  47
  48         checksum = l_checksum_new_cmac_aes(key, 16);
  49         if (!checksum)
  50                 return false;
  51
  52         result = l_checksum_update(checksum, msg, msg_len);
  53
  54         if (result) {
  55                 ssize_t len = l_checksum_get_digest(checksum, res, 16);
  56                 result = !!(len == 16);
  57         }
  58
  59         l_checksum_free(checksum);
  60
  61         return result;
  62 }
  63
  64 bool _bt_mesh_util_crypto_s1(const void *info,
  65                 size_t len, uint8_t salt[16])
  66 {
  67         return __mesh_util_crypto_aes_cmac_one(zero,
  68                         info, len, salt);
  69 }
  70
  71 bool _bt_mesh_util_crypto_create_virtual_address(
  72         const uint8_t virtual_label[16], uint16_t *addr)
  73 {
  74         uint8_t tmp[16];
  75
  76         if (!_bt_mesh_util_crypto_s1("vtad", 4, tmp))
  77                 return false;
  78
  79         if (!addr || !__mesh_util_crypto_aes_cmac_one(tmp,
  80                         virtual_label, 16, tmp))
  81                 return false;
  82
  83         *addr = (l_get_be16(tmp + 14) & 0x3fff) | 0x8000;
  84         return true;
  85 }
  86
  87 void _bt_mesh_util_print_byte_array(const char *prefix,
  88                 const void *ptr, int len)
  89 {
  90         const uint8_t *data = ptr;
  91         char *line, *bytes;
  92         int i;
  93
  94         line = g_malloc(strlen(prefix) + (16 * 3) + 2);
  95         snprintf(line, strlen(prefix), "%s ", prefix);
  96         bytes = line + strlen(prefix) + 1;
  97
  98         for (i = 0; i < len; ++i) {
  99                 snprintf(bytes, 4, "%2.2x ", data[i]);
 100                 if ((i + 1) % 16) {
 101                         bytes += 3;
 102                 } else {
 103                         BT_INFO("\r%s\n", line);
 104                         bytes = line + strlen(prefix) + 1;
 105                 }
 106         }
 107
 108         if (i % 16)
 109                 BT_INFO("\r%s\n", line);
 110
 111         g_free(line);
 112 }
 113
 114 uint16_t _bt_mesh_util_opcode_set(uint32_t opcode,
 115                 uint8_t *buf)
 116 {
 117         if (opcode <= 0x7e) {
 118                 buf[0] = opcode;
 119                 return 1;
 120         } else if (opcode >= 0x8000 && opcode <= 0xbfff) {
 121                 l_put_be16(opcode, buf);
 122                 return 2;
 123         } else if (opcode >= 0xc00000 && opcode <= 0xffffff) {
 124                 buf[0] = (opcode >> 16) & 0xff;
 125                 l_put_be16(opcode, buf + 1);
 126                 return 3;
 127         } else
 128                 return 0;
 129 }
 130
 131 bool _bt_mesh_util_opcode_get(const uint8_t *buf,
 132                 uint16_t sz, uint32_t *opcode, int *n)
 133 {
 134         BT_INFO("Mesh: Opcode Get DatLen [%d] Buf0 [0x%x]",
 135                         sz, buf[0]);
 136         if (!n || !opcode || sz < 1) return false;
 137
 138         switch (buf[0] & 0xc0) {
 139         case 0x00:
 140         case 0x40:
 141                 /* RFU */
 142                 if (buf[0] == 0x7f)
 143                         return false;
 144
 145                 *n = 1;
 146                 *opcode = buf[0];
 147                 break;
 148
 149         case 0x80:
 150                 if (sz < 2)
 151                         return false;
 152
 153                 *n = 2;
 154                 *opcode = l_get_be16(buf);
 155                 break;
 156
 157         case 0xc0:
 158                 if (sz < 3)
 159                         return false;
 160
 161                 *n = 3;
 162                 *opcode = l_get_be16(buf + 1);
 163                 *opcode |= buf[0] << 16;
 164                 break;
 165
 166         default:
 167                 BT_ERR("Mesh: Bad Packet:\n");
 168                 _bt_mesh_util_print_byte_array("\t", (void *) buf, sz);
 169                 return false;
 170         }
 171
 172         return true;
 173 }
 174
 175
 176 uint32_t _bt_mesh_util_get_timestamp_secs(void)
 177 {
 178         struct timespec ts;
 179
 180         clock_gettime(CLOCK_MONOTONIC, &ts);
 181         return ts.tv_sec;
 182 }
 183
 184 bool _bt_mesh_util_convert_string_to_hex(const char *str,
 185         uint16_t in_len, uint8_t *out,
 186                 uint16_t out_len)
 187 {
 188         uint16_t i;
 189
 190         if (in_len < out_len * 2)
 191                 return false;
 192
 193         for (i = 0; i < out_len; i++) {
 194                 if (sscanf(&str[i * 2], "%02hhx", &out[i]) != 1)
 195                         return false;
 196         }
 197
 198         return true;
 199 }
 200
 201 size_t _bt_mesh_util_convert_hex_to_string(uint8_t *in,
 202                 size_t in_len, char *out, size_t out_len)
 203 {
 204         static const char hexdigits[] = "0123456789abcdef";
 205         size_t i;
 206
 207         if (in_len * 2 > (out_len - 1))
 208                 return 0;
 209
 210         for (i = 0; i < in_len; i++) {
 211                 out[i * 2] = hexdigits[in[i] >> 4];
 212                 out[i * 2 + 1] = hexdigits[in[i] & 0xf];
 213         }
 214
 215         out[in_len * 2] = '\0';
 216         return i;
 217 }
 218
 219 void _bt_mesh_util_print_packet(const char *label,
 220                 const void *data, uint16_t size)
 221 {
 222         struct timeval pkt_time;
 223
 224         gettimeofday(&pkt_time, NULL);
 225
 226         if (size > 0) {
 227                 char *str;
 228
 229                 str = l_util_hexstring(data, size);
 230                 BT_DBG("%05d.%03d %s: %s",
 231                                 (uint32_t) pkt_time.tv_sec % 100000,
 232                                 (uint32_t) pkt_time.tv_usec/1000, label, str);
 233                 l_free(str);
 234         } else
 235                 BT_DBG("%05d.%03d %s: empty",
 236                                 (uint32_t) pkt_time.tv_sec % 100000,
 237                                 (uint32_t) pkt_time.tv_usec/1000, label);
 238 }
 239
 240 bool _bt_mesh_util_create_directory(const char *mesh_dir)
 241 {
 242         struct stat st;
 243         BT_ERR("Mesh: Create [%s]", mesh_dir);
 244
 245         if (stat(mesh_dir, &st) == 0) {
 246                 if (!S_ISDIR(st.st_mode)) {
 247                         BT_ERR("Mesh: [%s] not a directory", mesh_dir);
 248                         return false;
 249                 }
 250         } else if (errno == ENOENT) {
 251                 BT_ERR("Mesh: Dir not available: [%s]", mesh_dir);
 252                 if (mkdir(mesh_dir, 0755) != 0) {
 253                         BT_ERR("Mesh: Dir creation failed: [%s] error [%d]",
 254                                 mesh_dir, errno);
 255                         return false;
 256                 }
 257         } else {
 258                 BT_ERR("Mesh: Cannot open config directory");
 259                 return false;
 260         }
 261
 262         return true;
 263 }
 264
 265 bool _bt_mesh_util_is_directory_exists(const char *dir_path)
 266 {
 267         struct stat st;
 268
 269         if (stat(dir_path, &st) == 0) {
 270                 if (!S_ISDIR(st.st_mode)) {
 271                         BT_ERR("Mesh: [%s] not a directory", dir_path);
 272                         return false;
 273                 }
 274         } else if (errno == ENOENT) {
 275                 return false;
 276         }
 277
 278         return true;
 279 }