blufi_security.c 7.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254
  1. /*
  2. * SPDX-FileCopyrightText: 2021-2025 Espressif Systems (Shanghai) CO LTD
  3. *
  4. * SPDX-License-Identifier: Unlicense OR CC0-1.0
  5. */
  6. #include <stdio.h>
  7. #include <stdlib.h>
  8. #include <string.h>
  9. #include "freertos/FreeRTOS.h"
  10. #include "freertos/task.h"
  11. #include "freertos/event_groups.h"
  12. #include "esp_system.h"
  13. #include "esp_wifi.h"
  14. #include "esp_event.h"
  15. #include "esp_log.h"
  16. #include "nvs_flash.h"
  17. #include "esp_random.h"
  18. #if CONFIG_BT_CONTROLLER_ENABLED || !CONFIG_BT_NIMBLE_ENABLED
  19. #include "esp_bt.h"
  20. #endif
  21. #include "esp_blufi_api.h"
  22. #include "blufi.h"
  23. #include "mbedtls/aes.h"
  24. #include "mbedtls/dhm.h"
  25. #include "mbedtls/md5.h"
  26. #include "esp_crc.h"
  27. /*
  28. The SEC_TYPE_xxx is for self-defined packet data type in the procedure of "BLUFI negotiate key"
  29. If user use other negotiation procedure to exchange(or generate) key, should redefine the type by yourself.
  30. */
  31. #define SEC_TYPE_DH_PARAM_LEN 0x00
  32. #define SEC_TYPE_DH_PARAM_DATA 0x01
  33. #define SEC_TYPE_DH_P 0x02
  34. #define SEC_TYPE_DH_G 0x03
  35. #define SEC_TYPE_DH_PUBLIC 0x04
  36. struct blufi_security {
  37. #define DH_SELF_PUB_KEY_LEN 128
  38. uint8_t self_public_key[DH_SELF_PUB_KEY_LEN];
  39. #define SHARE_KEY_LEN 128
  40. uint8_t share_key[SHARE_KEY_LEN];
  41. size_t share_len;
  42. #define PSK_LEN 16
  43. uint8_t psk[PSK_LEN];
  44. uint8_t *dh_param;
  45. int dh_param_len;
  46. uint8_t iv[16];
  47. mbedtls_dhm_context dhm;
  48. mbedtls_aes_context aes;
  49. };
  50. static struct blufi_security *blufi_sec;
  51. static int myrand( void *rng_state, unsigned char *output, size_t len )
  52. {
  53. esp_fill_random(output, len);
  54. return( 0 );
  55. }
  56. extern void btc_blufi_report_error(esp_blufi_error_state_t state);
  57. void blufi_dh_negotiate_data_handler(uint8_t *data, int len, uint8_t **output_data, int *output_len, bool *need_free)
  58. {
  59. if (data == NULL || len < 3) {
  60. BLUFI_ERROR("BLUFI Invalid data format");
  61. btc_blufi_report_error(ESP_BLUFI_DATA_FORMAT_ERROR);
  62. return;
  63. }
  64. int ret;
  65. uint8_t type = data[0];
  66. if (blufi_sec == NULL) {
  67. BLUFI_ERROR("BLUFI Security is not initialized");
  68. btc_blufi_report_error(ESP_BLUFI_INIT_SECURITY_ERROR);
  69. return;
  70. }
  71. switch (type) {
  72. case SEC_TYPE_DH_PARAM_LEN:
  73. blufi_sec->dh_param_len = ((data[1]<<8)|data[2]);
  74. if (blufi_sec->dh_param) {
  75. free(blufi_sec->dh_param);
  76. blufi_sec->dh_param = NULL;
  77. }
  78. blufi_sec->dh_param = (uint8_t *)malloc(blufi_sec->dh_param_len);
  79. if (blufi_sec->dh_param == NULL) {
  80. blufi_sec->dh_param_len = 0; /* Reset length to avoid using unallocated memory */
  81. btc_blufi_report_error(ESP_BLUFI_DH_MALLOC_ERROR);
  82. BLUFI_ERROR("%s, malloc failed\n", __func__);
  83. return;
  84. }
  85. break;
  86. case SEC_TYPE_DH_PARAM_DATA:{
  87. if (blufi_sec->dh_param == NULL) {
  88. BLUFI_ERROR("%s, blufi_sec->dh_param == NULL\n", __func__);
  89. btc_blufi_report_error(ESP_BLUFI_DH_PARAM_ERROR);
  90. return;
  91. }
  92. if (len < (blufi_sec->dh_param_len + 1)) {
  93. BLUFI_ERROR("%s, invalid dh param len\n", __func__);
  94. btc_blufi_report_error(ESP_BLUFI_DH_PARAM_ERROR);
  95. return;
  96. }
  97. uint8_t *param = blufi_sec->dh_param;
  98. memcpy(blufi_sec->dh_param, &data[1], blufi_sec->dh_param_len);
  99. ret = mbedtls_dhm_read_params(&blufi_sec->dhm, &param, &param[blufi_sec->dh_param_len]);
  100. if (ret) {
  101. BLUFI_ERROR("%s read param failed %d\n", __func__, ret);
  102. btc_blufi_report_error(ESP_BLUFI_READ_PARAM_ERROR);
  103. return;
  104. }
  105. free(blufi_sec->dh_param);
  106. blufi_sec->dh_param = NULL;
  107. const int dhm_len = mbedtls_dhm_get_len(&blufi_sec->dhm);
  108. if (dhm_len > DH_SELF_PUB_KEY_LEN) {
  109. BLUFI_ERROR("%s dhm len not support %d\n", __func__, dhm_len);
  110. btc_blufi_report_error(ESP_BLUFI_DH_PARAM_ERROR);
  111. return;
  112. }
  113. ret = mbedtls_dhm_make_public(&blufi_sec->dhm, dhm_len, blufi_sec->self_public_key, DH_SELF_PUB_KEY_LEN, myrand, NULL);
  114. if (ret) {
  115. BLUFI_ERROR("%s make public failed %d\n", __func__, ret);
  116. btc_blufi_report_error(ESP_BLUFI_MAKE_PUBLIC_ERROR);
  117. return;
  118. }
  119. ret = mbedtls_dhm_calc_secret( &blufi_sec->dhm,
  120. blufi_sec->share_key,
  121. SHARE_KEY_LEN,
  122. &blufi_sec->share_len,
  123. myrand, NULL);
  124. if (ret) {
  125. BLUFI_ERROR("%s mbedtls_dhm_calc_secret failed %d\n", __func__, ret);
  126. btc_blufi_report_error(ESP_BLUFI_DH_PARAM_ERROR);
  127. return;
  128. }
  129. ret = mbedtls_md5(blufi_sec->share_key, blufi_sec->share_len, blufi_sec->psk);
  130. if (ret) {
  131. BLUFI_ERROR("%s mbedtls_md5 failed %d\n", __func__, ret);
  132. btc_blufi_report_error(ESP_BLUFI_CALC_MD5_ERROR);
  133. return;
  134. }
  135. mbedtls_aes_setkey_enc(&blufi_sec->aes, blufi_sec->psk, PSK_LEN * 8);
  136. /* alloc output data */
  137. *output_data = &blufi_sec->self_public_key[0];
  138. *output_len = dhm_len;
  139. *need_free = false;
  140. }
  141. break;
  142. case SEC_TYPE_DH_P:
  143. break;
  144. case SEC_TYPE_DH_G:
  145. break;
  146. case SEC_TYPE_DH_PUBLIC:
  147. break;
  148. }
  149. }
  150. int blufi_aes_encrypt(uint8_t iv8, uint8_t *crypt_data, int crypt_len)
  151. {
  152. int ret;
  153. size_t iv_offset = 0;
  154. uint8_t iv0[16];
  155. if (!blufi_sec) {
  156. return -1;
  157. }
  158. memcpy(iv0, blufi_sec->iv, sizeof(blufi_sec->iv));
  159. iv0[0] = iv8; /* set iv8 as the iv0[0] */
  160. ret = mbedtls_aes_crypt_cfb128(&blufi_sec->aes, MBEDTLS_AES_ENCRYPT, crypt_len, &iv_offset, iv0, crypt_data, crypt_data);
  161. if (ret) {
  162. return -1;
  163. }
  164. return crypt_len;
  165. }
  166. int blufi_aes_decrypt(uint8_t iv8, uint8_t *crypt_data, int crypt_len)
  167. {
  168. int ret;
  169. size_t iv_offset = 0;
  170. uint8_t iv0[16];
  171. if (!blufi_sec) {
  172. return -1;
  173. }
  174. memcpy(iv0, blufi_sec->iv, sizeof(blufi_sec->iv));
  175. iv0[0] = iv8; /* set iv8 as the iv0[0] */
  176. ret = mbedtls_aes_crypt_cfb128(&blufi_sec->aes, MBEDTLS_AES_DECRYPT, crypt_len, &iv_offset, iv0, crypt_data, crypt_data);
  177. if (ret) {
  178. return -1;
  179. }
  180. return crypt_len;
  181. }
  182. uint16_t blufi_crc_checksum(uint8_t iv8, uint8_t *data, int len)
  183. {
  184. /* This iv8 ignore, not used */
  185. return esp_crc16_be(0, data, len);
  186. }
  187. esp_err_t blufi_security_init(void)
  188. {
  189. blufi_sec = (struct blufi_security *)malloc(sizeof(struct blufi_security));
  190. if (blufi_sec == NULL) {
  191. return ESP_FAIL;
  192. }
  193. memset(blufi_sec, 0x0, sizeof(struct blufi_security));
  194. mbedtls_dhm_init(&blufi_sec->dhm);
  195. mbedtls_aes_init(&blufi_sec->aes);
  196. memset(blufi_sec->iv, 0x0, sizeof(blufi_sec->iv));
  197. return 0;
  198. }
  199. void blufi_security_deinit(void)
  200. {
  201. if (blufi_sec == NULL) {
  202. return;
  203. }
  204. if (blufi_sec->dh_param){
  205. free(blufi_sec->dh_param);
  206. blufi_sec->dh_param = NULL;
  207. }
  208. mbedtls_dhm_free(&blufi_sec->dhm);
  209. mbedtls_aes_free(&blufi_sec->aes);
  210. memset(blufi_sec, 0x0, sizeof(struct blufi_security));
  211. free(blufi_sec);
  212. blufi_sec = NULL;
  213. }