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         sprintf(line, "%s ", prefix);
  96         bytes = line + strlen(prefix) + 1;
  97
  98         for (i = 0; i < len; ++i) {
  99                 sprintf(bytes, "%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         if (!n || !opcode || sz < 1) return false;
 135
 136         switch (buf[0] & 0xc0) {
 137         case 0x00:
 138         case 0x40:
 139                 /* RFU */
 140                 if (buf[0] == 0x7f)
 141                         return false;
 142
 143                 *n = 1;
 144                 *opcode = buf[0];
 145                 break;
 146
 147         case 0x80:
 148                 if (sz < 2)
 149                         return false;
 150
 151                 *n = 2;
 152                 *opcode = l_get_be16(buf);
 153                 break;
 154
 155         case 0xc0:
 156                 if (sz < 3)
 157                         return false;
 158
 159                 *n = 3;
 160                 *opcode = l_get_be16(buf + 1);
 161                 *opcode |= buf[0] << 16;
 162                 break;
 163
 164         default:
 165                 BT_ERR("Mesh: Bad Packet:\n");
 166                 _bt_mesh_util_print_byte_array("\t", (void *) buf, sz);
 167                 return false;
 168         }
 169
 170         return true;
 171 }
 172
 173
 174 uint32_t _bt_mesh_util_get_timestamp_secs(void)
 175 {
 176         struct timespec ts;
 177
 178         clock_gettime(CLOCK_MONOTONIC, &ts);
 179         return ts.tv_sec;
 180 }
 181
 182 bool _bt_mesh_util_convert_string_to_hex(const char *str,
 183         uint16_t in_len, uint8_t *out,
 184                 uint16_t out_len)
 185 {
 186         uint16_t i;
 187
 188         if (in_len < out_len * 2)
 189                 return false;
 190
 191         for (i = 0; i < out_len; i++) {
 192                 if (sscanf(&str[i * 2], "%02hhx", &out[i]) != 1)
 193                         return false;
 194         }
 195
 196         return true;
 197 }
 198
 199 size_t _bt_mesh_util_convert_hex_to_string(uint8_t *in,
 200                 size_t in_len, char *out, size_t out_len)
 201 {
 202         static const char hexdigits[] = "0123456789abcdef";
 203         size_t i;
 204
 205         if (in_len * 2 > (out_len - 1))
 206                 return 0;
 207
 208         for (i = 0; i < in_len; i++) {
 209                 out[i * 2] = hexdigits[in[i] >> 4];
 210                 out[i * 2 + 1] = hexdigits[in[i] & 0xf];
 211         }
 212
 213         out[in_len * 2] = '\0';
 214         return i;
 215 }
 216
 217 void _bt_mesh_util_print_packet(const char *label,
 218                 const void *data, uint16_t size)
 219 {
 220         struct timeval pkt_time;
 221
 222         gettimeofday(&pkt_time, NULL);
 223
 224         if (size > 0) {
 225                 char *str;
 226
 227                 str = l_util_hexstring(data, size);
 228                 BT_DBG("%05d.%03d %s: %s",
 229                                 (uint32_t) pkt_time.tv_sec % 100000,
 230                                 (uint32_t) pkt_time.tv_usec/1000, label, str);
 231                 l_free(str);
 232         } else
 233                 BT_DBG("%05d.%03d %s: empty",
 234                                 (uint32_t) pkt_time.tv_sec % 100000,
 235                                 (uint32_t) pkt_time.tv_usec/1000, label);
 236 }
 237
 238 bool _bt_mesh_util_create_directory(const char *mesh_dir)
 239 {
 240         struct stat st;
 241         BT_ERR("Mesh: Create [%s]", mesh_dir);
 242
 243         if (stat(mesh_dir, &st) == 0) {
 244                 if (!S_ISDIR(st.st_mode)) {
 245                         BT_ERR("Mesh: [%s] not a directory", mesh_dir);
 246                         return false;
 247                 }
 248         } else if (errno == ENOENT) {
 249                 BT_ERR("Mesh: Dir not available: [%s]", mesh_dir);
 250                 if (mkdir(mesh_dir, 0755) != 0) {
 251                         BT_ERR("Mesh: Dir creation failed: [%s] error [%d]",
 252                                 mesh_dir, errno);
 253                         return false;
 254                 }
 255         } else {
 256                 BT_ERR("Mesh: Cannot open config directory");
 257                 return false;
 258         }
 259
 260         return true;
 261 }
 262
 263 bool _bt_mesh_util_is_directory_exists(const char *dir_path)
 264 {
 265         struct stat st;
 266
 267         if (stat(dir_path, &st) == 0) {
 268                 if (!S_ISDIR(st.st_mode)) {
 269                         BT_ERR("Mesh: [%s] not a directory", dir_path);
 270                         return false;
 271                 }
 272         } else if (errno == ENOENT) {
 273                 return false;
 274         }
 275
 276         return true;
 277 }