1 /*
2 * Copyright (c) 2019 Siddharth Chandrasekaran <siddharth@embedjournal.com>
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_DECLARE(osdp, CONFIG_OSDP_LOG_LEVEL);
9
10 #include <string.h>
11 #include "osdp_common.h"
12
13 #define OSDP_SC_EOM_MARKER 0x80 /* End of Message Marker */
14
15 /* Default key as specified in OSDP specification */
16 static const uint8_t osdp_scbk_default[16] = {
17 0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37,
18 0x38, 0x39, 0x3A, 0x3B, 0x3C, 0x3D, 0x3E, 0x3F
19 };
20
21 /**
22 * Tail memsets can be optimized away by compilers. To sidestep this problem,
23 * we cast the pointer to volatile and then operate on it.
24 */
osdp_memzero(void * mem,size_t size)25 static void osdp_memzero(void *mem, size_t size)
26 {
27 size_t i;
28 volatile uint8_t *p = mem;
29
30 for (i = 0; i < size; i++) {
31 p[i] = 0;
32 }
33 }
34
osdp_compute_scbk(struct osdp_pd * pd,uint8_t * master_key,uint8_t * scbk)35 void osdp_compute_scbk(struct osdp_pd *pd, uint8_t *master_key, uint8_t *scbk)
36 {
37 int i;
38
39 memcpy(scbk, pd->sc.pd_client_uid, 8);
40 for (i = 8; i < 16; i++) {
41 scbk[i] = ~scbk[i - 8];
42 }
43 osdp_encrypt(master_key, NULL, scbk, 16);
44 }
45
osdp_compute_session_keys(struct osdp_pd * pd)46 void osdp_compute_session_keys(struct osdp_pd *pd)
47 {
48 int i;
49 struct osdp *ctx = pd_to_osdp(pd);
50 uint8_t scbk[16];
51
52 if (ISSET_FLAG(pd, PD_FLAG_SC_USE_SCBKD)) {
53 memcpy(scbk, osdp_scbk_default, 16);
54 } else {
55 if (is_cp_mode(pd) && !ISSET_FLAG(pd, PD_FLAG_HAS_SCBK)) {
56 osdp_compute_scbk(pd, ctx->sc_master_key, scbk);
57 } else {
58 memcpy(scbk, pd->sc.scbk, 16);
59 }
60 }
61
62 memset(pd->sc.s_enc, 0, 16);
63 memset(pd->sc.s_mac1, 0, 16);
64 memset(pd->sc.s_mac2, 0, 16);
65
66 pd->sc.s_enc[0] = 0x01;
67 pd->sc.s_enc[1] = 0x82;
68 pd->sc.s_mac1[0] = 0x01;
69 pd->sc.s_mac1[1] = 0x01;
70 pd->sc.s_mac2[0] = 0x01;
71 pd->sc.s_mac2[1] = 0x02;
72
73 for (i = 2; i < 8; i++) {
74 pd->sc.s_enc[i] = pd->sc.cp_random[i - 2];
75 pd->sc.s_mac1[i] = pd->sc.cp_random[i - 2];
76 pd->sc.s_mac2[i] = pd->sc.cp_random[i - 2];
77 }
78
79 osdp_encrypt(scbk, NULL, pd->sc.s_enc, 16);
80 osdp_encrypt(scbk, NULL, pd->sc.s_mac1, 16);
81 osdp_encrypt(scbk, NULL, pd->sc.s_mac2, 16);
82 }
83
osdp_compute_cp_cryptogram(struct osdp_pd * pd)84 void osdp_compute_cp_cryptogram(struct osdp_pd *pd)
85 {
86 /* cp_cryptogram = AES-ECB( pd_random[8] || cp_random[8], s_enc ) */
87 memcpy(pd->sc.cp_cryptogram + 0, pd->sc.pd_random, 8);
88 memcpy(pd->sc.cp_cryptogram + 8, pd->sc.cp_random, 8);
89 osdp_encrypt(pd->sc.s_enc, NULL, pd->sc.cp_cryptogram, 16);
90 }
91
92 /**
93 * Like memcmp; but operates at constant time.
94 *
95 * Returns 0 if memory pointed to by s1 and s2 are identical; non-zero
96 * otherwise.
97 */
osdp_ct_compare(const void * s1,const void * s2,size_t len)98 static int osdp_ct_compare(const void *s1, const void *s2, size_t len)
99 {
100 size_t i, ret = 0;
101 const uint8_t *_s1 = s1;
102 const uint8_t *_s2 = s2;
103
104 for (i = 0; i < len; i++) {
105 ret |= _s1[i] ^ _s2[i];
106 }
107 return (int)ret;
108 }
109
osdp_verify_cp_cryptogram(struct osdp_pd * pd)110 int osdp_verify_cp_cryptogram(struct osdp_pd *pd)
111 {
112 uint8_t cp_crypto[16];
113
114 /* cp_cryptogram = AES-ECB( pd_random[8] || cp_random[8], s_enc ) */
115 memcpy(cp_crypto + 0, pd->sc.pd_random, 8);
116 memcpy(cp_crypto + 8, pd->sc.cp_random, 8);
117 osdp_encrypt(pd->sc.s_enc, NULL, cp_crypto, 16);
118
119 if (osdp_ct_compare(pd->sc.cp_cryptogram, cp_crypto, 16) != 0) {
120 return -1;
121 }
122 return 0;
123 }
124
osdp_compute_pd_cryptogram(struct osdp_pd * pd)125 void osdp_compute_pd_cryptogram(struct osdp_pd *pd)
126 {
127 /* pd_cryptogram = AES-ECB( cp_random[8] || pd_random[8], s_enc ) */
128 memcpy(pd->sc.pd_cryptogram + 0, pd->sc.cp_random, 8);
129 memcpy(pd->sc.pd_cryptogram + 8, pd->sc.pd_random, 8);
130 osdp_encrypt(pd->sc.s_enc, NULL, pd->sc.pd_cryptogram, 16);
131 }
132
osdp_verify_pd_cryptogram(struct osdp_pd * pd)133 int osdp_verify_pd_cryptogram(struct osdp_pd *pd)
134 {
135 uint8_t pd_crypto[16];
136
137 /* pd_cryptogram = AES-ECB( cp_random[8] || pd_random[8], s_enc ) */
138 memcpy(pd_crypto + 0, pd->sc.cp_random, 8);
139 memcpy(pd_crypto + 8, pd->sc.pd_random, 8);
140 osdp_encrypt(pd->sc.s_enc, NULL, pd_crypto, 16);
141
142 if (osdp_ct_compare(pd->sc.pd_cryptogram, pd_crypto, 16) != 0) {
143 return -1;
144 }
145 return 0;
146 }
147
osdp_compute_rmac_i(struct osdp_pd * pd)148 void osdp_compute_rmac_i(struct osdp_pd *pd)
149 {
150 /* rmac_i = AES-ECB( AES-ECB( cp_cryptogram, s_mac1 ), s_mac2 ) */
151 memcpy(pd->sc.r_mac, pd->sc.cp_cryptogram, 16);
152 osdp_encrypt(pd->sc.s_mac1, NULL, pd->sc.r_mac, 16);
153 osdp_encrypt(pd->sc.s_mac2, NULL, pd->sc.r_mac, 16);
154 }
155
osdp_decrypt_data(struct osdp_pd * pd,int is_cmd,uint8_t * data,int length)156 int osdp_decrypt_data(struct osdp_pd *pd, int is_cmd, uint8_t *data, int length)
157 {
158 int i;
159 uint8_t iv[16];
160
161 if (length <= 0 || length % 16 != 0) {
162 return -1;
163 }
164
165 memcpy(iv, is_cmd ? pd->sc.r_mac : pd->sc.c_mac, 16);
166 for (i = 0; i < 16; i++) {
167 iv[i] = ~iv[i];
168 }
169
170 osdp_decrypt(pd->sc.s_enc, iv, data, length);
171
172 length--;
173 while (length && data[length] == 0x00) {
174 length--;
175 }
176 if (data[length] != OSDP_SC_EOM_MARKER) {
177 return -1;
178 }
179 data[length] = 0;
180
181 return length;
182 }
183
osdp_encrypt_data(struct osdp_pd * pd,int is_cmd,uint8_t * data,int length)184 int osdp_encrypt_data(struct osdp_pd *pd, int is_cmd, uint8_t *data, int length)
185 {
186 int i, pad_len;
187 uint8_t iv[16];
188
189 data[length] = OSDP_SC_EOM_MARKER; /* append EOM marker */
190 pad_len = AES_PAD_LEN(length + 1);
191 if ((pad_len - length - 1) > 0) {
192 memset(data + length + 1, 0, pad_len - length - 1);
193 }
194 memcpy(iv, is_cmd ? pd->sc.r_mac : pd->sc.c_mac, 16);
195 for (i = 0; i < 16; i++) {
196 iv[i] = ~iv[i];
197 }
198
199 osdp_encrypt(pd->sc.s_enc, iv, data, pad_len);
200
201 return pad_len;
202 }
203
osdp_compute_mac(struct osdp_pd * pd,int is_cmd,const uint8_t * data,int len)204 int osdp_compute_mac(struct osdp_pd *pd, int is_cmd,
205 const uint8_t *data, int len)
206 {
207 int pad_len;
208 uint8_t buf[OSDP_PACKET_BUF_SIZE] = { 0 };
209 uint8_t iv[16];
210
211 memcpy(buf, data, len);
212 pad_len = (len % 16 == 0) ? len : AES_PAD_LEN(len);
213 if (len % 16 != 0) {
214 buf[len] = 0x80; /* end marker */
215 }
216 /**
217 * MAC for data blocks B[1] .. B[N] (post padding) is computed as:
218 * IV1 = R_MAC (or) C_MAC -- depending on is_cmd
219 * IV2 = B[N-1] after -- AES-CBC ( IV1, B[1] to B[N-1], SMAC-1 )
220 * MAC = AES-ECB ( IV2, B[N], SMAC-2 )
221 */
222
223 memcpy(iv, is_cmd ? pd->sc.r_mac : pd->sc.c_mac, 16);
224 if (pad_len > 16) {
225 /* N-1 blocks -- encrypted with SMAC-1 */
226 osdp_encrypt(pd->sc.s_mac1, iv, buf, pad_len - 16);
227 /* N-1 th block is the IV for N th block */
228 memcpy(iv, buf + pad_len - 32, 16);
229 }
230
231 /* N-th Block encrypted with SMAC-2 == MAC */
232 osdp_encrypt(pd->sc.s_mac2, iv, buf + pad_len - 16, 16);
233 memcpy(is_cmd ? pd->sc.c_mac : pd->sc.r_mac, buf + pad_len - 16, 16);
234
235 return 0;
236 }
237
osdp_sc_setup(struct osdp_pd * pd)238 void osdp_sc_setup(struct osdp_pd *pd)
239 {
240 uint8_t scbk[16];
241 bool preserve_scbk = is_pd_mode(pd) || ISSET_FLAG(pd, PD_FLAG_HAS_SCBK);
242
243 if (preserve_scbk) {
244 memcpy(scbk, pd->sc.scbk, 16);
245 }
246 memset(&pd->sc, 0, sizeof(struct osdp_secure_channel));
247 if (preserve_scbk) {
248 memcpy(pd->sc.scbk, scbk, 16);
249 }
250 osdp_memzero(scbk, sizeof(scbk));
251 if (is_pd_mode(pd)) {
252 pd->sc.pd_client_uid[0] = BYTE_0(pd->id.vendor_code);
253 pd->sc.pd_client_uid[1] = BYTE_1(pd->id.vendor_code);
254 pd->sc.pd_client_uid[2] = BYTE_0(pd->id.model);
255 pd->sc.pd_client_uid[3] = BYTE_1(pd->id.version);
256 pd->sc.pd_client_uid[4] = BYTE_0(pd->id.serial_number);
257 pd->sc.pd_client_uid[5] = BYTE_1(pd->id.serial_number);
258 pd->sc.pd_client_uid[6] = BYTE_2(pd->id.serial_number);
259 pd->sc.pd_client_uid[7] = BYTE_3(pd->id.serial_number);
260 } else {
261 osdp_fill_random(pd->sc.cp_random, 8);
262 }
263 }
264
