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