1 /*
2 * Copyright (c) 2020 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_PKT_MARK 0xFF
14 #define OSDP_PKT_SOM 0x53
15 #define PKT_CONTROL_SQN 0x03
16 #define PKT_CONTROL_CRC 0x04
17 #define PKT_CONTROL_SCB 0x08
18
19 struct osdp_packet_header {
20 uint8_t som;
21 uint8_t pd_address;
22 uint8_t len_lsb;
23 uint8_t len_msb;
24 uint8_t control;
25 uint8_t data[];
26 } __packed;
27
packet_has_mark(struct osdp_pd * pd)28 static inline bool packet_has_mark(struct osdp_pd *pd)
29 {
30 return ISSET_FLAG(pd, PD_FLAG_PKT_HAS_MARK);
31 }
32
packet_set_mark(struct osdp_pd * pd,bool mark)33 static inline void packet_set_mark(struct osdp_pd *pd, bool mark)
34 {
35 if (mark) {
36 SET_FLAG(pd, PD_FLAG_PKT_HAS_MARK);
37 } else {
38 CLEAR_FLAG(pd, PD_FLAG_PKT_HAS_MARK);
39 }
40 }
41
osdp_compute_checksum(uint8_t * msg,int length)42 uint8_t osdp_compute_checksum(uint8_t *msg, int length)
43 {
44 uint8_t checksum = 0;
45 int i, whole_checksum;
46
47 whole_checksum = 0;
48 for (i = 0; i < length; i++) {
49 whole_checksum += msg[i];
50 checksum = ~(0xff & whole_checksum) + 1;
51 }
52 return checksum;
53 }
54
osdp_phy_get_seq_number(struct osdp_pd * pd,int do_inc)55 static int osdp_phy_get_seq_number(struct osdp_pd *pd, int do_inc)
56 {
57 /* pd->seq_num is set to -1 to reset phy cmd state */
58 if (do_inc) {
59 pd->seq_number += 1;
60 if (pd->seq_number > 3) {
61 pd->seq_number = 1;
62 }
63 }
64 return pd->seq_number & PKT_CONTROL_SQN;
65 }
66
osdp_phy_packet_get_data_offset(struct osdp_pd * pd,const uint8_t * buf)67 int osdp_phy_packet_get_data_offset(struct osdp_pd *pd, const uint8_t *buf)
68 {
69 int sb_len = 0, mark_byte_len = 0;
70 struct osdp_packet_header *pkt;
71
72 ARG_UNUSED(pd);
73 if (packet_has_mark(pd)) {
74 mark_byte_len = 1;
75 buf += 1;
76 }
77 pkt = (struct osdp_packet_header *)buf;
78 if (pkt->control & PKT_CONTROL_SCB) {
79 sb_len = pkt->data[0];
80 }
81 return mark_byte_len + sizeof(struct osdp_packet_header) + sb_len;
82 }
83
osdp_phy_packet_get_smb(struct osdp_pd * pd,const uint8_t * buf)84 uint8_t *osdp_phy_packet_get_smb(struct osdp_pd *pd, const uint8_t *buf)
85 {
86 struct osdp_packet_header *pkt;
87
88 ARG_UNUSED(pd);
89 if (packet_has_mark(pd)) {
90 buf += 1;
91 }
92 pkt = (struct osdp_packet_header *)buf;
93 if (pkt->control & PKT_CONTROL_SCB) {
94 return pkt->data;
95 }
96 return NULL;
97 }
98
osdp_phy_in_sc_handshake(int is_reply,int id)99 int osdp_phy_in_sc_handshake(int is_reply, int id)
100 {
101 if (is_reply) {
102 return (id == REPLY_CCRYPT || id == REPLY_RMAC_I);
103 } else {
104 return (id == CMD_CHLNG || id == CMD_SCRYPT);
105 }
106 }
107
osdp_phy_packet_init(struct osdp_pd * pd,uint8_t * buf,int max_len)108 int osdp_phy_packet_init(struct osdp_pd *pd, uint8_t *buf, int max_len)
109 {
110 int exp_len, id, scb_len = 0, mark_byte_len = 0;
111 struct osdp_packet_header *pkt;
112
113 exp_len = sizeof(struct osdp_packet_header) + 64; /* 64 is estimated */
114 if (max_len < exp_len) {
115 LOG_ERR("packet_init: out of space! CMD: %02x", pd->cmd_id);
116 return OSDP_ERR_PKT_FMT;
117 }
118
119 /**
120 * In PD mode just follow what we received from CP. In CP mode, as we
121 * initiate the transaction, choose based on CONFIG_OSDP_SKIP_MARK_BYTE.
122 */
123 if ((is_pd_mode(pd) && packet_has_mark(pd)) ||
124 (is_cp_mode(pd) && !ISSET_FLAG(pd, PD_FLAG_PKT_SKIP_MARK))) {
125 buf[0] = OSDP_PKT_MARK;
126 buf++;
127 mark_byte_len = 1;
128 packet_set_mark(pd, true);
129 }
130
131 /* Fill packet header */
132 pkt = (struct osdp_packet_header *)buf;
133 pkt->som = OSDP_PKT_SOM;
134 pkt->pd_address = pd->address & 0x7F; /* Use only the lower 7 bits */
135 if (is_pd_mode(pd)) {
136 /* PD must reply with MSB of it's address set */
137 pkt->pd_address |= 0x80;
138 id = pd->reply_id;
139 } else {
140 id = pd->cmd_id;
141 }
142 pkt->control = osdp_phy_get_seq_number(pd, is_cp_mode(pd));
143 pkt->control |= PKT_CONTROL_CRC;
144
145 if (sc_is_active(pd)) {
146 pkt->control |= PKT_CONTROL_SCB;
147 pkt->data[0] = scb_len = 2;
148 pkt->data[1] = SCS_15;
149 } else if (osdp_phy_in_sc_handshake(is_pd_mode(pd), id)) {
150 pkt->control |= PKT_CONTROL_SCB;
151 pkt->data[0] = scb_len = 3;
152 pkt->data[1] = SCS_11;
153 }
154
155 return mark_byte_len + sizeof(struct osdp_packet_header) + scb_len;
156 }
157
osdp_phy_packet_finalize(struct osdp_pd * pd,uint8_t * buf,int len,int max_len)158 int osdp_phy_packet_finalize(struct osdp_pd *pd, uint8_t *buf,
159 int len, int max_len)
160 {
161 uint16_t crc16;
162 struct osdp_packet_header *pkt;
163
164 /* Do a sanity check only; we expect header to be pre-filled */
165 if ((unsigned long)len <= sizeof(struct osdp_packet_header)) {
166 LOG_ERR("PKT_F: Invalid header");
167 return OSDP_ERR_PKT_FMT;
168 }
169
170 if (packet_has_mark(pd)) {
171 if (buf[0] != OSDP_PKT_MARK) {
172 LOG_ERR("PKT_F: MARK validation failed! ID: 0x%02x",
173 is_cp_mode(pd) ? pd->cmd_id : pd->reply_id);
174 return OSDP_ERR_PKT_FMT;
175 }
176 /* temporarily get rid of mark byte */
177 buf += 1;
178 len -= 1;
179 max_len -= 1;
180 }
181 pkt = (struct osdp_packet_header *)buf;
182 if (pkt->som != OSDP_PKT_SOM) {
183 LOG_ERR("PKT_F: header SOM validation failed! ID: 0x%02x",
184 is_cp_mode(pd) ? pd->cmd_id : pd->reply_id);
185 return OSDP_ERR_PKT_FMT;
186 }
187
188 /* len: with 2 byte CRC */
189 pkt->len_lsb = BYTE_0(len + 2);
190 pkt->len_msb = BYTE_1(len + 2);
191
192 #ifdef CONFIG_OSDP_SC_ENABLED
193 uint8_t *data;
194 int data_len;
195
196 if (sc_is_active(pd) && (pkt->control & PKT_CONTROL_SCB) && pkt->data[1] >= SCS_15) {
197 if (pkt->data[1] == SCS_17 || pkt->data[1] == SCS_18) {
198 /**
199 * Only the data portion of message (after id byte)
200 * is encrypted. While (en/de)crypting, we must skip
201 * header, security block, and cmd/reply ID byte.
202 *
203 * Note: if cmd/reply has no data, we must set type to
204 * SCS_15/SCS_16 and send them.
205 */
206 data = pkt->data + pkt->data[0] + 1;
207 data_len = len - (sizeof(struct osdp_packet_header) +
208 pkt->data[0] + 1);
209 len -= data_len;
210 /**
211 * check if the passed buffer can hold the encrypted
212 * data where length may be rounded up to the nearest
213 * 16 byte block boundary.
214 */
215 if (AES_PAD_LEN(data_len + 1) > max_len) {
216 /* data_len + 1 for OSDP_SC_EOM_MARKER */
217 goto out_of_space_error;
218 }
219 len += osdp_encrypt_data(pd, is_cp_mode(pd), data, data_len);
220 }
221 /* len: with 4bytes MAC; with 2 byte CRC; without 1 byte mark */
222 if (len + 4 > max_len) {
223 goto out_of_space_error;
224 }
225
226 /* len: with 2 byte CRC; with 4 byte MAC */
227 pkt->len_lsb = BYTE_0(len + 2 + 4);
228 pkt->len_msb = BYTE_1(len + 2 + 4);
229
230 /* compute and extend the buf with 4 MAC bytes */
231 osdp_compute_mac(pd, is_cp_mode(pd), buf, len);
232 data = is_cp_mode(pd) ? pd->sc.c_mac : pd->sc.r_mac;
233 memcpy(buf + len, data, 4);
234 len += 4;
235 }
236 #endif /* CONFIG_OSDP_SC_ENABLED */
237
238 /* fill crc16 */
239 if (len + 2 > max_len) {
240 goto out_of_space_error;
241 }
242 crc16 = osdp_compute_crc16(buf, len);
243 buf[len + 0] = BYTE_0(crc16);
244 buf[len + 1] = BYTE_1(crc16);
245 len += 2;
246
247 if (packet_has_mark(pd)) {
248 len += 1; /* put back mark byte */
249 }
250
251 return len;
252
253 out_of_space_error:
254 LOG_ERR("PKT_F: Out of buffer space! CMD(%02x)", pd->cmd_id);
255 return OSDP_ERR_PKT_FMT;
256 }
257
osdp_phy_check_packet(struct osdp_pd * pd,uint8_t * buf,int len,int * one_pkt_len)258 int osdp_phy_check_packet(struct osdp_pd *pd, uint8_t *buf, int len,
259 int *one_pkt_len)
260 {
261 uint16_t comp, cur;
262 int pd_addr, pkt_len;
263 struct osdp_packet_header *pkt;
264
265 /* wait till we have the header */
266 if ((unsigned long)len < sizeof(struct osdp_packet_header)) {
267 /* incomplete data */
268 return OSDP_ERR_PKT_WAIT;
269 }
270
271 packet_set_mark(pd, false);
272 if (buf[0] == OSDP_PKT_MARK) {
273 buf += 1;
274 len -= 1;
275 packet_set_mark(pd, true);
276 }
277
278 pkt = (struct osdp_packet_header *)buf;
279
280 /* validate packet header */
281 if (pkt->som != OSDP_PKT_SOM) {
282 LOG_ERR("Invalid SOM 0x%02x", pkt->som);
283 return OSDP_ERR_PKT_FMT;
284 }
285
286 if (is_cp_mode(pd) && !(pkt->pd_address & 0x80)) {
287 LOG_ERR("Reply without address MSB set!");
288 return OSDP_ERR_PKT_FMT;
289 }
290
291 /* validate packet length */
292 pkt_len = (pkt->len_msb << 8) | pkt->len_lsb;
293 if (len < pkt_len) {
294 /* wait for more data? */
295 return OSDP_ERR_PKT_WAIT;
296 }
297
298 if (pkt_len > OSDP_PACKET_BUF_SIZE ||
299 (unsigned long)pkt_len < sizeof(struct osdp_packet_header)) {
300 pd->reply_id = REPLY_NAK;
301 pd->ephemeral_data[0] = OSDP_PD_NAK_CMD_LEN;
302 return OSDP_ERR_PKT_NACK;
303 }
304
305 *one_pkt_len = pkt_len + (packet_has_mark(pd) ? 1 : 0);
306
307 /* validate CRC/checksum */
308 if (pkt->control & PKT_CONTROL_CRC) {
309 pkt_len -= 2; /* consume CRC */
310 cur = (buf[pkt_len + 1] << 8) | buf[pkt_len];
311 comp = osdp_compute_crc16(buf, pkt_len);
312 if (comp != cur) {
313 LOG_ERR("Invalid crc 0x%04x/0x%04x", comp, cur);
314 pd->reply_id = REPLY_NAK;
315 pd->ephemeral_data[0] = OSDP_PD_NAK_MSG_CHK;
316 return OSDP_ERR_PKT_NACK;
317 }
318 } else {
319 pkt_len -= 1; /* consume checksum */
320 cur = buf[pkt_len];
321 comp = osdp_compute_checksum(buf, pkt_len);
322 if (comp != cur) {
323 LOG_ERR("Invalid checksum %02x/%02x", comp, cur);
324 pd->reply_id = REPLY_NAK;
325 pd->ephemeral_data[0] = OSDP_PD_NAK_MSG_CHK;
326 return OSDP_ERR_PKT_NACK;
327 }
328 }
329
330 /* validate PD address */
331 pd_addr = pkt->pd_address & 0x7F;
332 if (pd_addr != pd->address && pd_addr != 0x7F) {
333 /* not addressed to us and was not broadcasted */
334 if (is_cp_mode(pd)) {
335 LOG_ERR("Invalid pd address %d", pd_addr);
336 return OSDP_ERR_PKT_CHECK;
337 }
338 return OSDP_ERR_PKT_SKIP;
339 }
340
341 /* validate sequence number */
342 comp = pkt->control & PKT_CONTROL_SQN;
343 if (is_pd_mode(pd)) {
344 if (comp == 0) {
345 /**
346 * CP is trying to restart communication by sending a 0.
347 * The current PD implementation does not hold any state
348 * between commands so we can just set seq_number to -1
349 * (so it gets incremented to 0 with a call to
350 * phy_get_seq_number()) and invalidate any established
351 * secure channels.
352 */
353 pd->seq_number = -1;
354 sc_deactivate(pd);
355 }
356 if (comp == pd->seq_number) {
357 /**
358 * TODO: PD must resend the last response if CP send the
359 * same sequence number again.
360 */
361 LOG_ERR("seq-repeat/reply-resend not supported!");
362 pd->reply_id = REPLY_NAK;
363 pd->ephemeral_data[0] = OSDP_PD_NAK_SEQ_NUM;
364 return OSDP_ERR_PKT_NACK;
365 }
366 } else {
367 if (comp == 0) {
368 /**
369 * Check for receiving a busy reply from the PD which would
370 * have a sequence number of 0, come in an unsecured packet
371 * of minimum length, and have the reply ID REPLY_BUSY.
372 */
373 if ((pkt_len == 6) && (pkt->data[0] == REPLY_BUSY)) {
374 pd->seq_number -= 1;
375 return OSDP_ERR_PKT_BUSY;
376 }
377 }
378 }
379 cur = osdp_phy_get_seq_number(pd, is_pd_mode(pd));
380 if (cur != comp && !ISSET_FLAG(pd, PD_FLAG_SKIP_SEQ_CHECK)) {
381 LOG_ERR("packet seq mismatch %d/%d", cur, comp);
382 pd->reply_id = REPLY_NAK;
383 pd->ephemeral_data[0] = OSDP_PD_NAK_SEQ_NUM;
384 return OSDP_ERR_PKT_NACK;
385 }
386
387 return OSDP_ERR_PKT_NONE;
388 }
389
osdp_phy_decode_packet(struct osdp_pd * pd,uint8_t * buf,int len,uint8_t ** pkt_start)390 int osdp_phy_decode_packet(struct osdp_pd *pd, uint8_t *buf, int len,
391 uint8_t **pkt_start)
392 {
393 uint8_t *data;
394 int mac_offset;
395 struct osdp_packet_header *pkt;
396
397 if (packet_has_mark(pd)) {
398 /* Consume mark byte */
399 buf += 1;
400 len -= 1;
401 }
402
403 pkt = (struct osdp_packet_header *)buf;
404 len -= pkt->control & PKT_CONTROL_CRC ? 2 : 1;
405 mac_offset = len - 4;
406 data = pkt->data;
407 len -= sizeof(struct osdp_packet_header);
408
409 #ifdef CONFIG_OSDP_SC_ENABLED
410 uint8_t *mac;
411 int is_cmd;
412
413 if (pkt->control & PKT_CONTROL_SCB) {
414 if (is_pd_mode(pd) && !sc_is_capable(pd)) {
415 LOG_ERR("PD is not SC capable");
416 pd->reply_id = REPLY_NAK;
417 pd->ephemeral_data[0] = OSDP_PD_NAK_SC_UNSUP;
418 return OSDP_ERR_PKT_NACK;
419 }
420 if (pkt->data[1] < SCS_11 || pkt->data[1] > SCS_18) {
421 LOG_ERR("Invalid SB Type");
422 pd->reply_id = REPLY_NAK;
423 pd->ephemeral_data[0] = OSDP_PD_NAK_SC_COND;
424 return OSDP_ERR_PKT_NACK;
425 }
426 if (!sc_is_active(pd) && pkt->data[1] > SCS_14) {
427 LOG_ERR("Received invalid secure message!");
428 pd->reply_id = REPLY_NAK;
429 pd->ephemeral_data[0] = OSDP_PD_NAK_SC_COND;
430 return OSDP_ERR_PKT_NACK;
431 }
432 if (pkt->data[1] == SCS_11 || pkt->data[1] == SCS_13) {
433 /**
434 * CP signals PD to use SCBKD by setting SB data byte
435 * to 0. In CP, PD_FLAG_SC_USE_SCBKD comes from FSM; on
436 * PD we extract it from the command itself. But this
437 * usage of SCBKD is allowed only when the PD is in
438 * install mode (indicated by PD_FLAG_INSTALL_MODE).
439 */
440 if (ISSET_FLAG(pd, PD_FLAG_INSTALL_MODE) &&
441 pkt->data[2] == 0) {
442 SET_FLAG(pd, PD_FLAG_SC_USE_SCBKD);
443 }
444 }
445 data = pkt->data + pkt->data[0];
446 len -= pkt->data[0]; /* consume security block */
447 } else {
448 /**
449 * If the current packet is an ACK for a KEYSET, the PD might
450 * have discarded the secure channel session keys in favour of
451 * the new key we sent and hence this packet may reach us in
452 * plain text. To work with such PDs, we must also discard our
453 * secure session.
454 *
455 * The way we do this is by calling osdp_keyset_complete() which
456 * copies the key in ephemeral_data to the current SCBK.
457 */
458 if (is_cp_mode(pd) && pd->cmd_id == CMD_KEYSET &&
459 pkt->data[0] == REPLY_ACK) {
460 osdp_keyset_complete(pd);
461 }
462
463 if (sc_is_active(pd)) {
464 LOG_ERR("Received plain-text message in SC");
465 pd->reply_id = REPLY_NAK;
466 pd->ephemeral_data[0] = OSDP_PD_NAK_SC_COND;
467 return OSDP_ERR_PKT_NACK;
468 }
469 }
470
471 if (sc_is_active(pd) &&
472 pkt->control & PKT_CONTROL_SCB && pkt->data[1] >= SCS_15) {
473 /* validate MAC */
474 is_cmd = is_pd_mode(pd);
475 osdp_compute_mac(pd, is_cmd, buf, mac_offset);
476 mac = is_cmd ? pd->sc.c_mac : pd->sc.r_mac;
477 if (memcmp(buf + mac_offset, mac, 4) != 0) {
478 LOG_ERR("Invalid MAC; discarding SC");
479 sc_deactivate(pd);
480 pd->reply_id = REPLY_NAK;
481 pd->ephemeral_data[0] = OSDP_PD_NAK_SC_COND;
482 return OSDP_ERR_PKT_NACK;
483 }
484 len -= 4; /* consume MAC */
485
486 /* decrypt data block */
487 if (pkt->data[1] == SCS_17 || pkt->data[1] == SCS_18) {
488 /**
489 * Only the data portion of message (after id byte)
490 * is encrypted. While (en/de)crypting, we must skip
491 * header (6), security block (2) and cmd/reply id (1)
492 * bytes if cmd/reply has no data, use SCS_15/SCS_16.
493 *
494 * At this point, the header and security block is
495 * already consumed. So we can just skip the cmd/reply
496 * ID (data[0]) when calling osdp_decrypt_data().
497 */
498 len = osdp_decrypt_data(pd, is_cmd, data + 1, len - 1);
499 if (len < 0) {
500 LOG_ERR("Failed at decrypt; discarding SC");
501 sc_deactivate(pd);
502 pd->reply_id = REPLY_NAK;
503 pd->ephemeral_data[0] = OSDP_PD_NAK_SC_COND;
504 return OSDP_ERR_PKT_NACK;
505 }
506 if (len == 0) {
507 /**
508 * If cmd/reply has no data, PD "should" have
509 * used SCS_15/SCS_16 but we will be tolerant
510 * towards those faulty implementations.
511 */
512 LOG_INF("Received encrypted data block with 0 "
513 "length; tolerating non-conformance!");
514 }
515 len += 1; /* put back cmd/reply ID */
516 }
517 }
518 #endif /* CONFIG_OSDP_SC_ENABLED */
519
520 *pkt_start = data;
521 return len;
522 }
523
osdp_phy_state_reset(struct osdp_pd * pd)524 void osdp_phy_state_reset(struct osdp_pd *pd)
525 {
526 #ifdef CONFIG_OSDP_MODE_CP
527 pd->phy_state = 0;
528 #endif
529 pd->seq_number = -1;
530 pd->rx_buf_len = 0;
531 }
532
