1 /*
2 * Copyright (c) 2020 Siddharth Chandrasekaran <siddharth@embedjournal.com>
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <stdlib.h>
8 #include <zephyr/logging/log.h>
9 LOG_MODULE_DECLARE(osdp, CONFIG_OSDP_LOG_LEVEL);
10
11 #include "osdp_common.h"
12
13 #define OSDP_PD_POLL_TIMEOUT_MS (1000 / CONFIG_OSDP_PD_POLL_RATE)
14 #define OSDP_CMD_RETRY_WAIT_MS (CONFIG_OSDP_CMD_RETRY_WAIT_SEC * 1000)
15
16 #ifdef CONFIG_OSDP_SC_ENABLED
17 #define OSDP_PD_SC_RETRY_MS (CONFIG_OSDP_SC_RETRY_WAIT_SEC * 1000)
18 #endif
19
20 #define CMD_POLL_LEN 1
21 #define CMD_LSTAT_LEN 1
22 #define CMD_ISTAT_LEN 1
23 #define CMD_OSTAT_LEN 1
24 #define CMD_RSTAT_LEN 1
25 #define CMD_ID_LEN 2
26 #define CMD_CAP_LEN 2
27 #define CMD_DIAG_LEN 2
28 #define CMD_OUT_LEN 5
29 #define CMD_LED_LEN 15
30 #define CMD_BUZ_LEN 6
31 #define CMD_TEXT_LEN 7 /* variable length command */
32 #define CMD_COMSET_LEN 6
33 #define CMD_KEYSET_LEN 19
34 #define CMD_CHLNG_LEN 9
35 #define CMD_SCRYPT_LEN 17
36
37 #define REPLY_ACK_DATA_LEN 0
38 #define REPLY_PDID_DATA_LEN 12
39 #define REPLY_PDCAP_ENTITY_LEN 3
40 #define REPLY_LSTATR_DATA_LEN 2
41 #define REPLY_RSTATR_DATA_LEN 1
42 #define REPLY_COM_DATA_LEN 5
43 #define REPLY_NAK_DATA_LEN 1
44 #define REPLY_CCRYPT_DATA_LEN 32
45 #define REPLY_RMAC_I_DATA_LEN 16
46 #define REPLY_KEYPPAD_DATA_LEN 2 /* variable length command */
47 #define REPLY_RAW_DATA_LEN 4 /* variable length command */
48 #define REPLY_FMT_DATA_LEN 3 /* variable length command */
49 #define REPLY_BUSY_DATA_LEN 0
50
51 enum osdp_cp_error_e {
52 OSDP_CP_ERR_NONE = 0,
53 OSDP_CP_ERR_GENERIC = -1,
54 OSDP_CP_ERR_NO_DATA = -2,
55 OSDP_CP_ERR_RETRY_CMD = -3,
56 OSDP_CP_ERR_CAN_YIELD = -4,
57 OSDP_CP_ERR_INPROG = -5,
58 OSDP_CP_ERR_UNKNOWN = -6,
59 };
60
61
cp_cmd_alloc(struct osdp_pd * pd)62 static struct osdp_cmd *cp_cmd_alloc(struct osdp_pd *pd)
63 {
64 struct osdp_cmd *cmd = NULL;
65
66 if (k_mem_slab_alloc(&pd->cmd.slab, (void **)&cmd, K_MSEC(100))) {
67 LOG_ERR("Memory allocation time-out");
68 return NULL;
69 }
70 return cmd;
71 }
72
cp_cmd_free(struct osdp_pd * pd,struct osdp_cmd * cmd)73 static void cp_cmd_free(struct osdp_pd *pd, struct osdp_cmd *cmd)
74 {
75 k_mem_slab_free(&pd->cmd.slab, (void *)cmd);
76 }
77
cp_cmd_enqueue(struct osdp_pd * pd,struct osdp_cmd * cmd)78 static void cp_cmd_enqueue(struct osdp_pd *pd, struct osdp_cmd *cmd)
79 {
80 sys_slist_append(&pd->cmd.queue, &cmd->node);
81 }
82
cp_cmd_dequeue(struct osdp_pd * pd,struct osdp_cmd ** cmd)83 static int cp_cmd_dequeue(struct osdp_pd *pd, struct osdp_cmd **cmd)
84 {
85 sys_snode_t *node;
86
87 node = sys_slist_peek_head(&pd->cmd.queue);
88 if (node == NULL) {
89 return -1;
90 }
91 sys_slist_remove(&pd->cmd.queue, NULL, node);
92 *cmd = CONTAINER_OF(node, struct osdp_cmd, node);
93 return 0;
94 }
95
osdp_extract_address(int * address)96 int osdp_extract_address(int *address)
97 {
98 int pd_offset = 0;
99 unsigned long addr;
100 char *tok, *s1, *s2, addr_buf[32 * CONFIG_OSDP_NUM_CONNECTED_PD];
101
102 strncpy(addr_buf, CONFIG_OSDP_PD_ADDRESS_LIST, sizeof(addr_buf) - 1);
103 addr_buf[sizeof(addr_buf) - 1] = '\0';
104 tok = strtok_r(addr_buf, ", ", &s1);
105 while (tok && pd_offset < CONFIG_OSDP_NUM_CONNECTED_PD) {
106 addr = strtoul(tok, &s2, 10);
107 if (*s2 != '\0') { /* tok must be number-ish */
108 return -1;
109 }
110 address[pd_offset] = addr;
111 pd_offset++;
112 tok = strtok_r(NULL, ", ", &s1);
113 }
114 return (pd_offset == CONFIG_OSDP_NUM_CONNECTED_PD) ? 0 : -1;
115 }
116
check_buf_len(int need,int have)117 static inline bool check_buf_len(int need, int have)
118 {
119 if (need > have) {
120 LOG_ERR("OOM at build command: need:%d have:%d", need, have);
121 return false;
122 }
123 return true;
124 }
125
cp_build_command(struct osdp_pd * pd,uint8_t * buf,int max_len)126 static int cp_build_command(struct osdp_pd *pd, uint8_t *buf, int max_len)
127 {
128 struct osdp_cmd *cmd = NULL;
129 int len = 0;
130 int data_off = osdp_phy_packet_get_data_offset(pd, buf);
131 #ifdef CONFIG_OSDP_SC_ENABLED
132 uint8_t *smb = osdp_phy_packet_get_smb(pd, buf);
133 #endif
134
135 buf += data_off;
136 max_len -= data_off;
137 if (max_len <= 0) {
138 return OSDP_CP_ERR_GENERIC;
139 }
140
141 switch (pd->cmd_id) {
142 case CMD_POLL:
143 if (!check_buf_len(CMD_POLL_LEN, max_len)) {
144 return OSDP_CP_ERR_GENERIC;
145 }
146 buf[len++] = pd->cmd_id;
147 break;
148 case CMD_LSTAT:
149 if (!check_buf_len(CMD_LSTAT_LEN, max_len)) {
150 return OSDP_CP_ERR_GENERIC;
151 }
152 buf[len++] = pd->cmd_id;
153 break;
154 case CMD_ISTAT:
155 if (!check_buf_len(CMD_ISTAT_LEN, max_len)) {
156 return OSDP_CP_ERR_GENERIC;
157 }
158 buf[len++] = pd->cmd_id;
159 break;
160 case CMD_OSTAT:
161 if (!check_buf_len(CMD_OSTAT_LEN, max_len)) {
162 return OSDP_CP_ERR_GENERIC;
163 }
164 buf[len++] = pd->cmd_id;
165 break;
166 case CMD_RSTAT:
167 if (!check_buf_len(CMD_RSTAT_LEN, max_len)) {
168 return OSDP_CP_ERR_GENERIC;
169 }
170 buf[len++] = pd->cmd_id;
171 break;
172 case CMD_ID:
173 if (!check_buf_len(CMD_ID_LEN, max_len)) {
174 return OSDP_CP_ERR_GENERIC;
175 }
176 buf[len++] = pd->cmd_id;
177 buf[len++] = 0x00;
178 break;
179 case CMD_CAP:
180 if (!check_buf_len(CMD_CAP_LEN, max_len)) {
181 return OSDP_CP_ERR_GENERIC;
182 }
183 buf[len++] = pd->cmd_id;
184 buf[len++] = 0x00;
185 break;
186 case CMD_DIAG:
187 if (!check_buf_len(CMD_DIAG_LEN, max_len)) {
188 return OSDP_CP_ERR_GENERIC;
189 }
190 buf[len++] = pd->cmd_id;
191 buf[len++] = 0x00;
192 break;
193 case CMD_OUT:
194 if (!check_buf_len(CMD_OUT_LEN, max_len)) {
195 return OSDP_CP_ERR_GENERIC;
196 }
197 cmd = (struct osdp_cmd *)pd->ephemeral_data;
198 buf[len++] = pd->cmd_id;
199 buf[len++] = cmd->output.output_no;
200 buf[len++] = cmd->output.control_code;
201 buf[len++] = BYTE_0(cmd->output.timer_count);
202 buf[len++] = BYTE_1(cmd->output.timer_count);
203 break;
204 case CMD_LED:
205 if (!check_buf_len(CMD_LED_LEN, max_len)) {
206 return OSDP_CP_ERR_GENERIC;
207 }
208 cmd = (struct osdp_cmd *)pd->ephemeral_data;
209 buf[len++] = pd->cmd_id;
210 buf[len++] = cmd->led.reader;
211 buf[len++] = cmd->led.led_number;
212
213 buf[len++] = cmd->led.temporary.control_code;
214 buf[len++] = cmd->led.temporary.on_count;
215 buf[len++] = cmd->led.temporary.off_count;
216 buf[len++] = cmd->led.temporary.on_color;
217 buf[len++] = cmd->led.temporary.off_color;
218 buf[len++] = BYTE_0(cmd->led.temporary.timer_count);
219 buf[len++] = BYTE_1(cmd->led.temporary.timer_count);
220
221 buf[len++] = cmd->led.permanent.control_code;
222 buf[len++] = cmd->led.permanent.on_count;
223 buf[len++] = cmd->led.permanent.off_count;
224 buf[len++] = cmd->led.permanent.on_color;
225 buf[len++] = cmd->led.permanent.off_color;
226 break;
227 case CMD_BUZ:
228 if (!check_buf_len(CMD_BUZ_LEN, max_len)) {
229 return OSDP_CP_ERR_GENERIC;
230 }
231 cmd = (struct osdp_cmd *)pd->ephemeral_data;
232 buf[len++] = pd->cmd_id;
233 buf[len++] = cmd->buzzer.reader;
234 buf[len++] = cmd->buzzer.control_code;
235 buf[len++] = cmd->buzzer.on_count;
236 buf[len++] = cmd->buzzer.off_count;
237 buf[len++] = cmd->buzzer.rep_count;
238 break;
239 case CMD_TEXT:
240 cmd = (struct osdp_cmd *)pd->ephemeral_data;
241 if (!check_buf_len(CMD_TEXT_LEN + cmd->text.length, max_len)) {
242 return OSDP_CP_ERR_GENERIC;
243 }
244 buf[len++] = pd->cmd_id;
245 buf[len++] = cmd->text.reader;
246 buf[len++] = cmd->text.control_code;
247 buf[len++] = cmd->text.temp_time;
248 buf[len++] = cmd->text.offset_row;
249 buf[len++] = cmd->text.offset_col;
250 buf[len++] = cmd->text.length;
251 memcpy(buf + len, cmd->text.data, cmd->text.length);
252 len += cmd->text.length;
253 break;
254 case CMD_COMSET:
255 if (!check_buf_len(CMD_COMSET_LEN, max_len)) {
256 return OSDP_CP_ERR_GENERIC;
257 }
258 cmd = (struct osdp_cmd *)pd->ephemeral_data;
259 buf[len++] = pd->cmd_id;
260 buf[len++] = cmd->comset.address;
261 buf[len++] = BYTE_0(cmd->comset.baud_rate);
262 buf[len++] = BYTE_1(cmd->comset.baud_rate);
263 buf[len++] = BYTE_2(cmd->comset.baud_rate);
264 buf[len++] = BYTE_3(cmd->comset.baud_rate);
265 break;
266 #ifdef CONFIG_OSDP_SC_ENABLED
267 case CMD_KEYSET:
268 if (!sc_is_active(pd)) {
269 LOG_ERR("Cannot perform KEYSET without SC!");
270 return OSDP_CP_ERR_GENERIC;
271 }
272 if (!check_buf_len(CMD_KEYSET_LEN, max_len)) {
273 return OSDP_CP_ERR_GENERIC;
274 }
275 cmd = (struct osdp_cmd *)pd->ephemeral_data;
276 if (cmd->keyset.length != 16) {
277 LOG_ERR("Invalid key length");
278 return OSDP_CP_ERR_GENERIC;
279 }
280 buf[len++] = pd->cmd_id;
281 buf[len++] = 1; /* key type (1: SCBK) */
282 buf[len++] = 16; /* key length in bytes */
283 if (cmd->keyset.type == 1) { /* SCBK */
284 memcpy(buf + len, cmd->keyset.data, 16);
285 } else if (cmd->keyset.type == 0) { /* master_key */
286 osdp_compute_scbk(pd, cmd->keyset.data, buf + len);
287 } else {
288 LOG_ERR("Unknown key type (%d)", cmd->keyset.type);
289 return OSDP_CP_ERR_GENERIC;
290 }
291 len += 16;
292 break;
293 case CMD_CHLNG:
294 if (!check_buf_len(CMD_CHLNG_LEN, max_len)) {
295 return OSDP_CP_ERR_GENERIC;
296 }
297 if (smb == NULL) {
298 LOG_ERR("Invalid secure message block!");
299 return -1;
300 }
301 smb[0] = 3; /* length */
302 smb[1] = SCS_11; /* type */
303 smb[2] = ISSET_FLAG(pd, PD_FLAG_SC_USE_SCBKD) ? 0 : 1;
304 buf[len++] = pd->cmd_id;
305 memcpy(buf + len, pd->sc.cp_random, 8);
306 len += 8;
307 break;
308 case CMD_SCRYPT:
309 if (!check_buf_len(CMD_SCRYPT_LEN, max_len)) {
310 return OSDP_CP_ERR_GENERIC;
311 }
312 if (smb == NULL) {
313 LOG_ERR("Invalid secure message block!");
314 return -1;
315 }
316 osdp_compute_cp_cryptogram(pd);
317 smb[0] = 3; /* length */
318 smb[1] = SCS_13; /* type */
319 smb[2] = ISSET_FLAG(pd, PD_FLAG_SC_USE_SCBKD) ? 0 : 1;
320 buf[len++] = pd->cmd_id;
321 memcpy(buf + len, pd->sc.cp_cryptogram, 16);
322 len += 16;
323 break;
324 #endif /* CONFIG_OSDP_SC_ENABLED */
325 default:
326 LOG_ERR("Unknown/Unsupported CMD(%02x)", pd->cmd_id);
327 return OSDP_CP_ERR_GENERIC;
328 }
329
330 #ifdef CONFIG_OSDP_SC_ENABLED
331 if (smb && (smb[1] > SCS_14) && sc_is_active(pd)) {
332 /**
333 * When SC active and current cmd is not a handshake (<= SCS_14)
334 * then we must set SCS type to 17 if this message has data
335 * bytes and 15 otherwise.
336 */
337 smb[0] = 2;
338 smb[1] = (len > 1) ? SCS_17 : SCS_15;
339 }
340 #endif /* CONFIG_OSDP_SC_ENABLED */
341
342 return len;
343 }
344
cp_decode_response(struct osdp_pd * pd,uint8_t * buf,int len)345 static int cp_decode_response(struct osdp_pd *pd, uint8_t *buf, int len)
346 {
347 uint32_t temp32;
348 struct osdp *ctx = pd_to_osdp(pd);
349 int ret = OSDP_CP_ERR_GENERIC, pos = 0, t1, t2;
350 struct osdp_event event;
351
352 pd->reply_id = buf[pos++];
353 len--; /* consume reply id from the head */
354
355 switch (pd->reply_id) {
356 case REPLY_ACK:
357 if (len != REPLY_ACK_DATA_LEN) {
358 break;
359 }
360 ret = OSDP_CP_ERR_NONE;
361 break;
362 case REPLY_NAK:
363 if (len != REPLY_NAK_DATA_LEN) {
364 break;
365 }
366 LOG_WRN("PD replied with NAK(%d) for CMD(%02x)",
367 buf[pos], pd->cmd_id);
368 ret = OSDP_CP_ERR_NONE;
369 break;
370 case REPLY_PDID:
371 if (len != REPLY_PDID_DATA_LEN) {
372 break;
373 }
374 pd->id.vendor_code = buf[pos++];
375 pd->id.vendor_code |= buf[pos++] << 8;
376 pd->id.vendor_code |= buf[pos++] << 16;
377
378 pd->id.model = buf[pos++];
379 pd->id.version = buf[pos++];
380
381 pd->id.serial_number = buf[pos++];
382 pd->id.serial_number |= buf[pos++] << 8;
383 pd->id.serial_number |= buf[pos++] << 16;
384 pd->id.serial_number |= buf[pos++] << 24;
385
386 pd->id.firmware_version = buf[pos++] << 16;
387 pd->id.firmware_version |= buf[pos++] << 8;
388 pd->id.firmware_version |= buf[pos++];
389 ret = OSDP_CP_ERR_NONE;
390 break;
391 case REPLY_PDCAP:
392 if ((len % REPLY_PDCAP_ENTITY_LEN) != 0) {
393 LOG_ERR("PDCAP response length is not a multiple of 3");
394 return OSDP_CP_ERR_GENERIC;
395 }
396 while (pos < len) {
397 t1 = buf[pos++]; /* func_code */
398 if (t1 >= OSDP_PD_CAP_SENTINEL) {
399 break;
400 }
401 pd->cap[t1].function_code = t1;
402 pd->cap[t1].compliance_level = buf[pos++];
403 pd->cap[t1].num_items = buf[pos++];
404 }
405
406 /* post-capabilities hooks */
407 t2 = OSDP_PD_CAP_COMMUNICATION_SECURITY;
408 if (pd->cap[t2].compliance_level & 0x01) {
409 SET_FLAG(pd, PD_FLAG_SC_CAPABLE);
410 } else {
411 CLEAR_FLAG(pd, PD_FLAG_SC_CAPABLE);
412 }
413 ret = OSDP_CP_ERR_NONE;
414 break;
415 case REPLY_LSTATR:
416 if (len != REPLY_LSTATR_DATA_LEN) {
417 break;
418 }
419 if (buf[pos++]) {
420 SET_FLAG(pd, PD_FLAG_TAMPER);
421 } else {
422 CLEAR_FLAG(pd, PD_FLAG_TAMPER);
423 }
424 if (buf[pos++]) {
425 SET_FLAG(pd, PD_FLAG_POWER);
426 } else {
427 CLEAR_FLAG(pd, PD_FLAG_POWER);
428 }
429 ret = OSDP_CP_ERR_NONE;
430 break;
431 case REPLY_RSTATR:
432 if (len != REPLY_RSTATR_DATA_LEN) {
433 break;
434 }
435 if (buf[pos++]) {
436 SET_FLAG(pd, PD_FLAG_R_TAMPER);
437 } else {
438 CLEAR_FLAG(pd, PD_FLAG_R_TAMPER);
439 }
440 ret = OSDP_CP_ERR_NONE;
441 break;
442 case REPLY_COM:
443 if (len != REPLY_COM_DATA_LEN) {
444 break;
445 }
446 t1 = buf[pos++];
447 temp32 = buf[pos++];
448 temp32 |= buf[pos++] << 8;
449 temp32 |= buf[pos++] << 16;
450 temp32 |= buf[pos++] << 24;
451 LOG_WRN("COMSET responded with ID:%d Baud:%d", t1, temp32);
452 pd->address = t1;
453 pd->baud_rate = temp32;
454 ret = OSDP_CP_ERR_NONE;
455 break;
456 case REPLY_KEYPPAD:
457 if (len < REPLY_KEYPPAD_DATA_LEN || !ctx->event_callback) {
458 break;
459 }
460 event.type = OSDP_EVENT_KEYPRESS;
461 event.keypress.reader_no = buf[pos++];
462 event.keypress.length = buf[pos++];
463 if ((len - REPLY_KEYPPAD_DATA_LEN) != event.keypress.length) {
464 break;
465 }
466 memcpy(event.keypress.data, buf + pos, event.keypress.length);
467 ctx->event_callback(ctx->event_callback_arg, pd->idx, &event);
468 ret = OSDP_CP_ERR_NONE;
469 break;
470 case REPLY_RAW:
471 if (len < REPLY_RAW_DATA_LEN || !ctx->event_callback) {
472 break;
473 }
474 event.type = OSDP_EVENT_CARDREAD;
475 event.cardread.reader_no = buf[pos++];
476 event.cardread.format = buf[pos++];
477 event.cardread.length = buf[pos++]; /* bits LSB */
478 event.cardread.length |= buf[pos++] << 8; /* bits MSB */
479 event.cardread.direction = 0; /* un-specified */
480 t1 = (event.cardread.length + 7) / 8; /* len: bytes */
481 if (t1 != (len - REPLY_RAW_DATA_LEN)) {
482 break;
483 }
484 memcpy(event.cardread.data, buf + pos, t1);
485 ctx->event_callback(ctx->event_callback_arg, pd->idx, &event);
486 ret = OSDP_CP_ERR_NONE;
487 break;
488 case REPLY_FMT:
489 if (len < REPLY_FMT_DATA_LEN || !ctx->event_callback) {
490 break;
491 }
492 event.type = OSDP_EVENT_CARDREAD;
493 event.cardread.reader_no = buf[pos++];
494 event.cardread.direction = buf[pos++];
495 event.cardread.length = buf[pos++];
496 event.cardread.format = OSDP_CARD_FMT_ASCII;
497 if (event.cardread.length != (len - REPLY_FMT_DATA_LEN) ||
498 event.cardread.length > OSDP_EVENT_MAX_DATALEN) {
499 break;
500 }
501 memcpy(event.cardread.data, buf + pos, event.cardread.length);
502 ctx->event_callback(ctx->event_callback_arg, pd->idx, &event);
503 ret = OSDP_CP_ERR_NONE;
504 break;
505 case REPLY_BUSY:
506 /* PD busy; signal upper layer to retry command */
507 if (len != REPLY_BUSY_DATA_LEN) {
508 break;
509 }
510 ret = OSDP_CP_ERR_RETRY_CMD;
511 break;
512 #ifdef CONFIG_OSDP_SC_ENABLED
513 case REPLY_CCRYPT:
514 if (sc_is_active(pd) || pd->cmd_id != CMD_CHLNG) {
515 LOG_ERR("Out of order REPLY_CCRYPT; has PD gone rogue?");
516 break;
517 }
518 if (len != REPLY_CCRYPT_DATA_LEN) {
519 break;
520 }
521 memcpy(pd->sc.pd_client_uid, buf + pos, 8);
522 memcpy(pd->sc.pd_random, buf + pos + 8, 8);
523 memcpy(pd->sc.pd_cryptogram, buf + pos + 16, 16);
524 pos += 32;
525 osdp_compute_session_keys(pd);
526 if (osdp_verify_pd_cryptogram(pd) != 0) {
527 LOG_ERR("Failed to verify PD cryptogram");
528 return OSDP_CP_ERR_GENERIC;
529 }
530 ret = OSDP_CP_ERR_NONE;
531 break;
532 case REPLY_RMAC_I:
533 if (sc_is_active(pd) || pd->cmd_id != CMD_SCRYPT) {
534 LOG_ERR("Out of order REPLY_RMAC_I; has PD gone rogue?");
535 break;
536 }
537 if (len != REPLY_RMAC_I_DATA_LEN) {
538 break;
539 }
540 memcpy(pd->sc.r_mac, buf + pos, 16);
541 sc_activate(pd);
542 ret = OSDP_CP_ERR_NONE;
543 break;
544 #endif /* CONFIG_OSDP_SC_ENABLED */
545 default:
546 LOG_WRN("Unexpected REPLY(%02x) for CMD(%02x)",
547 pd->cmd_id, pd->reply_id);
548 return OSDP_CP_ERR_UNKNOWN;
549 }
550
551 if (ret != OSDP_CP_ERR_NONE) {
552 LOG_ERR("Failed to decode response: REPLY(%02x) for CMD(%02x)",
553 pd->cmd_id, pd->reply_id);
554 }
555
556 if (pd->cmd_id != CMD_POLL) {
557 LOG_DBG("CMD(%02x) REPLY(%02x)", pd->cmd_id, pd->reply_id);
558 }
559
560 return ret;
561 }
562
cp_build_packet(struct osdp_pd * pd)563 static int cp_build_packet(struct osdp_pd *pd)
564 {
565 int ret, len;
566
567 /* init packet buf with header */
568 len = osdp_phy_packet_init(pd, pd->rx_buf, sizeof(pd->rx_buf));
569 if (len < 0) {
570 return OSDP_CP_ERR_GENERIC;
571 }
572
573 /* fill command data */
574 ret = cp_build_command(pd, pd->rx_buf, sizeof(pd->rx_buf));
575 if (ret < 0) {
576 return OSDP_CP_ERR_GENERIC;
577 }
578 len += ret;
579
580 /* finalize packet */
581 len = osdp_phy_packet_finalize(pd, pd->rx_buf, len, sizeof(pd->rx_buf));
582 if (len < 0) {
583 return OSDP_CP_ERR_GENERIC;
584 }
585
586 pd->rx_buf_len = len;
587
588 return OSDP_CP_ERR_NONE;
589 }
590
cp_send_command(struct osdp_pd * pd)591 static int cp_send_command(struct osdp_pd *pd)
592 {
593 int ret;
594
595 /* flush rx to remove any invalid data. */
596 if (pd->channel.flush) {
597 pd->channel.flush(pd->channel.data);
598 }
599
600 ret = pd->channel.send(pd->channel.data, pd->rx_buf, pd->rx_buf_len);
601 if (ret != pd->rx_buf_len) {
602 LOG_ERR("Channel send for %d bytes failed! ret: %d",
603 pd->rx_buf_len, ret);
604 return OSDP_CP_ERR_GENERIC;
605 }
606
607 if (IS_ENABLED(CONFIG_OSDP_PACKET_TRACE)) {
608 if (pd->cmd_id != CMD_POLL) {
609 LOG_DBG("bytes sent");
610 osdp_dump(NULL, pd->rx_buf, pd->rx_buf_len);
611 }
612 }
613
614 return OSDP_CP_ERR_NONE;
615 }
616
cp_process_reply(struct osdp_pd * pd)617 static int cp_process_reply(struct osdp_pd *pd)
618 {
619 uint8_t *buf;
620 int err, len, one_pkt_len, remaining;
621
622 buf = pd->rx_buf + pd->rx_buf_len;
623 remaining = sizeof(pd->rx_buf) - pd->rx_buf_len;
624
625 len = pd->channel.recv(pd->channel.data, buf, remaining);
626 if (len <= 0) { /* No data received */
627 return OSDP_CP_ERR_NO_DATA;
628 }
629 pd->rx_buf_len += len;
630
631 if (IS_ENABLED(CONFIG_OSDP_PACKET_TRACE)) {
632 if (pd->cmd_id != CMD_POLL) {
633 LOG_DBG("bytes received");
634 osdp_dump(NULL, pd->rx_buf, pd->rx_buf_len);
635 }
636 }
637
638 err = osdp_phy_check_packet(pd, pd->rx_buf, pd->rx_buf_len, &len);
639
640 /* Translate phy error codes to CP errors */
641 switch (err) {
642 case OSDP_ERR_PKT_NONE:
643 break;
644 case OSDP_ERR_PKT_WAIT:
645 return OSDP_CP_ERR_NO_DATA;
646 case OSDP_ERR_PKT_BUSY:
647 return OSDP_CP_ERR_RETRY_CMD;
648 default:
649 return OSDP_CP_ERR_GENERIC;
650 }
651
652 one_pkt_len = len;
653
654 /* Valid OSDP packet in buffer */
655 len = osdp_phy_decode_packet(pd, pd->rx_buf, len, &buf);
656 if (len <= 0) {
657 return OSDP_CP_ERR_GENERIC;
658 }
659 err = cp_decode_response(pd, buf, len);
660
661 /* We are done with the packet (error or not). Remove processed bytes */
662 len = pd->rx_buf_len - one_pkt_len;
663 if (len) {
664 memmove(pd->rx_buf, pd->rx_buf + one_pkt_len, len);
665 }
666 pd->rx_buf_len = len;
667
668 return err;
669 }
670
cp_flush_command_queue(struct osdp_pd * pd)671 static void cp_flush_command_queue(struct osdp_pd *pd)
672 {
673 struct osdp_cmd *cmd;
674
675 while (cp_cmd_dequeue(pd, &cmd) == 0) {
676 cp_cmd_free(pd, cmd);
677 }
678 }
679
cp_set_state(struct osdp_pd * pd,enum osdp_cp_state_e state)680 static inline void cp_set_state(struct osdp_pd *pd, enum osdp_cp_state_e state)
681 {
682 pd->state = state;
683 CLEAR_FLAG(pd, PD_FLAG_AWAIT_RESP);
684 }
685
cp_set_online(struct osdp_pd * pd)686 static inline void cp_set_online(struct osdp_pd *pd)
687 {
688 cp_set_state(pd, OSDP_CP_STATE_ONLINE);
689 pd->wait_ms = 0;
690 pd->tstamp = 0;
691 }
692
cp_set_offline(struct osdp_pd * pd)693 static inline void cp_set_offline(struct osdp_pd *pd)
694 {
695 sc_deactivate(pd);
696 pd->state = OSDP_CP_STATE_OFFLINE;
697 pd->tstamp = osdp_millis_now();
698 if (pd->wait_ms == 0) {
699 pd->wait_ms = 1000; /* retry after 1 second initially */
700 } else {
701 pd->wait_ms <<= 1;
702 if (pd->wait_ms > OSDP_ONLINE_RETRY_WAIT_MAX_MS) {
703 pd->wait_ms = OSDP_ONLINE_RETRY_WAIT_MAX_MS;
704 }
705 }
706 }
707
708 #ifdef CONFIG_OSDP_SC_ENABLED
cp_sc_should_retry(struct osdp_pd * pd)709 static inline bool cp_sc_should_retry(struct osdp_pd *pd)
710 {
711 return (sc_is_capable(pd) && !sc_is_active(pd) &&
712 osdp_millis_since(pd->sc_tstamp) > OSDP_PD_SC_RETRY_MS);
713 }
714 #endif
715
cp_phy_state_update(struct osdp_pd * pd)716 static int cp_phy_state_update(struct osdp_pd *pd)
717 {
718 int64_t elapsed;
719 int rc, ret = OSDP_CP_ERR_CAN_YIELD;
720 struct osdp_cmd *cmd = NULL;
721
722 switch (pd->phy_state) {
723 case OSDP_CP_PHY_STATE_WAIT:
724 elapsed = osdp_millis_since(pd->phy_tstamp);
725 if (elapsed < OSDP_CMD_RETRY_WAIT_MS) {
726 break;
727 }
728 pd->phy_state = OSDP_CP_PHY_STATE_SEND_CMD;
729 break;
730 case OSDP_CP_PHY_STATE_ERR:
731 ret = OSDP_CP_ERR_GENERIC;
732 break;
733 case OSDP_CP_PHY_STATE_IDLE:
734 if (cp_cmd_dequeue(pd, &cmd)) {
735 ret = OSDP_CP_ERR_NONE; /* command queue is empty */
736 break;
737 }
738 pd->cmd_id = cmd->id;
739 memcpy(pd->ephemeral_data, cmd, sizeof(struct osdp_cmd));
740 cp_cmd_free(pd, cmd);
741 /* fall-thru */
742 case OSDP_CP_PHY_STATE_SEND_CMD:
743 if (cp_build_packet(pd)) {
744 LOG_ERR("Failed to build packet for CMD(%d)",
745 pd->cmd_id);
746 ret = OSDP_CP_ERR_GENERIC;
747 break;
748 }
749 if (cp_send_command(pd)) {
750 LOG_ERR("Failed to send CMD(%d)", pd->cmd_id);
751 pd->phy_state = OSDP_CP_PHY_STATE_ERR;
752 ret = OSDP_CP_ERR_GENERIC;
753 break;
754 }
755 ret = OSDP_CP_ERR_INPROG;
756 pd->phy_state = OSDP_CP_PHY_STATE_REPLY_WAIT;
757 pd->rx_buf_len = 0; /* reset buf_len for next use */
758 pd->phy_tstamp = osdp_millis_now();
759 break;
760 case OSDP_CP_PHY_STATE_REPLY_WAIT:
761 rc = cp_process_reply(pd);
762 if (rc == OSDP_CP_ERR_NONE) {
763 #ifdef CONFIG_OSDP_SC_ENABLED
764 if (sc_is_active(pd)) {
765 pd->sc_tstamp = osdp_millis_now();
766 }
767 #endif
768 pd->phy_state = OSDP_CP_PHY_STATE_IDLE;
769 break;
770 }
771 if (rc == OSDP_CP_ERR_RETRY_CMD) {
772 LOG_INF("PD busy; retry last command");
773 pd->phy_tstamp = osdp_millis_now();
774 pd->phy_state = OSDP_CP_PHY_STATE_WAIT;
775 break;
776 }
777 if (rc == OSDP_CP_ERR_GENERIC || rc == OSDP_CP_ERR_UNKNOWN ||
778 osdp_millis_since(pd->phy_tstamp) > OSDP_RESP_TOUT_MS) {
779 if (rc != OSDP_CP_ERR_GENERIC) {
780 LOG_ERR("Response timeout for CMD(%02x)",
781 pd->cmd_id);
782 }
783 pd->rx_buf_len = 0;
784 if (pd->channel.flush) {
785 pd->channel.flush(pd->channel.data);
786 }
787 cp_flush_command_queue(pd);
788 pd->phy_state = OSDP_CP_PHY_STATE_ERR;
789 ret = OSDP_CP_ERR_GENERIC;
790 break;
791 }
792 ret = OSDP_CP_ERR_INPROG;
793 break;
794 }
795
796 return ret;
797 }
798
cp_cmd_dispatcher(struct osdp_pd * pd,int cmd)799 static int cp_cmd_dispatcher(struct osdp_pd *pd, int cmd)
800 {
801 struct osdp_cmd *c;
802
803 if (ISSET_FLAG(pd, PD_FLAG_AWAIT_RESP)) {
804 CLEAR_FLAG(pd, PD_FLAG_AWAIT_RESP);
805 return OSDP_CP_ERR_NONE; /* nothing to be done here */
806 }
807
808 c = cp_cmd_alloc(pd);
809 if (c == NULL) {
810 return OSDP_CP_ERR_GENERIC;
811 }
812
813 c->id = cmd;
814
815 if (c->id == CMD_KEYSET) {
816 memcpy(&c->keyset, pd->ephemeral_data, sizeof(c->keyset));
817 }
818
819 cp_cmd_enqueue(pd, c);
820 SET_FLAG(pd, PD_FLAG_AWAIT_RESP);
821 return OSDP_CP_ERR_INPROG;
822 }
823
state_update(struct osdp_pd * pd)824 static int state_update(struct osdp_pd *pd)
825 {
826 bool soft_fail;
827 int phy_state;
828 #ifdef CONFIG_OSDP_SC_ENABLED
829 struct osdp *ctx = pd_to_osdp(pd);
830 struct osdp_cmd_keyset *keyset;
831 #endif
832
833 phy_state = cp_phy_state_update(pd);
834 if (phy_state == OSDP_CP_ERR_INPROG ||
835 phy_state == OSDP_CP_ERR_CAN_YIELD) {
836 return phy_state;
837 }
838
839 /* Certain states can fail without causing PD offline */
840 soft_fail = (pd->state == OSDP_CP_STATE_SC_CHLNG);
841
842 /* phy state error -- cleanup */
843 if (pd->state != OSDP_CP_STATE_OFFLINE &&
844 phy_state == OSDP_CP_ERR_GENERIC && !soft_fail) {
845 cp_set_offline(pd);
846 return OSDP_CP_ERR_CAN_YIELD;
847 }
848
849 /* command queue is empty and last command was successful */
850
851 switch (pd->state) {
852 case OSDP_CP_STATE_ONLINE:
853 #ifdef CONFIG_OSDP_SC_ENABLED
854 if (cp_sc_should_retry(pd)) {
855 LOG_INF("Retry SC after retry timeout");
856 cp_set_state(pd, OSDP_CP_STATE_SC_INIT);
857 break;
858 }
859 #endif
860 if (osdp_millis_since(pd->tstamp) < OSDP_PD_POLL_TIMEOUT_MS) {
861 break;
862 }
863 if (cp_cmd_dispatcher(pd, CMD_POLL) == 0) {
864 pd->tstamp = osdp_millis_now();
865 }
866 break;
867 case OSDP_CP_STATE_OFFLINE:
868 if (osdp_millis_since(pd->tstamp) > pd->wait_ms) {
869 cp_set_state(pd, OSDP_CP_STATE_INIT);
870 osdp_phy_state_reset(pd);
871 }
872 break;
873 case OSDP_CP_STATE_INIT:
874 if (cp_cmd_dispatcher(pd, CMD_POLL) != 0) {
875 break;
876 }
877 cp_set_state(pd, OSDP_CP_STATE_IDREQ);
878 __fallthrough;
879 case OSDP_CP_STATE_IDREQ:
880 if (cp_cmd_dispatcher(pd, CMD_ID) != 0) {
881 break;
882 }
883 if (pd->reply_id != REPLY_PDID) {
884 LOG_ERR("Unexpected REPLY(%02x) for cmd "
885 STRINGIFY(CMD_ID), pd->reply_id);
886 cp_set_offline(pd);
887 break;
888 }
889 cp_set_state(pd, OSDP_CP_STATE_CAPDET);
890 __fallthrough;
891 case OSDP_CP_STATE_CAPDET:
892 if (cp_cmd_dispatcher(pd, CMD_CAP) != 0) {
893 break;
894 }
895 if (pd->reply_id != REPLY_PDCAP) {
896 LOG_ERR("Unexpected REPLY(%02x) for cmd "
897 STRINGIFY(CMD_CAP), pd->reply_id);
898 cp_set_offline(pd);
899 break;
900 }
901 #ifdef CONFIG_OSDP_SC_ENABLED
902 if (sc_is_capable(pd)) {
903 CLEAR_FLAG(pd, PD_FLAG_SC_SCBKD_DONE);
904 CLEAR_FLAG(pd, PD_FLAG_SC_USE_SCBKD);
905 cp_set_state(pd, OSDP_CP_STATE_SC_INIT);
906 break;
907 }
908 #endif /* CONFIG_OSDP_SC_ENABLED */
909 cp_set_online(pd);
910 break;
911 #ifdef CONFIG_OSDP_SC_ENABLED
912 case OSDP_CP_STATE_SC_INIT:
913 osdp_sc_setup(pd);
914 cp_set_state(pd, OSDP_CP_STATE_SC_CHLNG);
915 __fallthrough;
916 case OSDP_CP_STATE_SC_CHLNG:
917 if (cp_cmd_dispatcher(pd, CMD_CHLNG) != 0) {
918 break;
919 }
920 if (phy_state < 0) {
921 if (ISSET_FLAG(pd, PD_FLAG_SC_USE_SCBKD)) {
922 LOG_INF("SC Failed. Online without SC");
923 pd->sc_tstamp = osdp_millis_now();
924 osdp_phy_state_reset(pd);
925 cp_set_online(pd);
926 break;
927 }
928 SET_FLAG(pd, PD_FLAG_SC_USE_SCBKD);
929 cp_set_state(pd, OSDP_CP_STATE_SC_INIT);
930 pd->phy_state = 0; /* soft reset phy state */
931 LOG_WRN("SC Failed. Retry with SCBK-D");
932 break;
933 }
934 if (pd->reply_id != REPLY_CCRYPT) {
935 LOG_ERR("CHLNG failed. Online without SC");
936 pd->sc_tstamp = osdp_millis_now();
937 osdp_phy_state_reset(pd);
938 cp_set_online(pd);
939 break;
940 }
941 cp_set_state(pd, OSDP_CP_STATE_SC_SCRYPT);
942 __fallthrough;
943 case OSDP_CP_STATE_SC_SCRYPT:
944 if (cp_cmd_dispatcher(pd, CMD_SCRYPT) != 0) {
945 break;
946 }
947 if (pd->reply_id != REPLY_RMAC_I) {
948 LOG_ERR("SCRYPT failed. Online without SC");
949 osdp_phy_state_reset(pd);
950 pd->sc_tstamp = osdp_millis_now();
951 cp_set_online(pd);
952 break;
953 }
954 if (ISSET_FLAG(pd, PD_FLAG_SC_USE_SCBKD)) {
955 LOG_WRN("SC ACtive with SCBK-D. Set SCBK");
956 cp_set_state(pd, OSDP_CP_STATE_SET_SCBK);
957 break;
958 }
959 LOG_INF("SC Active");
960 pd->sc_tstamp = osdp_millis_now();
961 cp_set_online(pd);
962 break;
963 case OSDP_CP_STATE_SET_SCBK:
964 if (!ISSET_FLAG(pd, PD_FLAG_AWAIT_RESP)) {
965 keyset = (struct osdp_cmd_keyset *)pd->ephemeral_data;
966 if (ISSET_FLAG(pd, PD_FLAG_HAS_SCBK)) {
967 memcpy(keyset->data, pd->sc.scbk, 16);
968 keyset->type = 1;
969 } else {
970 keyset->type = 0;
971 memcpy(keyset->data, ctx->sc_master_key, 16);
972 }
973 keyset->length = 16;
974 }
975 if (cp_cmd_dispatcher(pd, CMD_KEYSET) != 0) {
976 break;
977 }
978 if (pd->reply_id == REPLY_NAK) {
979 LOG_WRN("Failed to set SCBK; continue with SCBK-D");
980 cp_set_online(pd);
981 break;
982 }
983 cp_keyset_complete(pd);
984 pd->seq_number = -1;
985 break;
986 #endif /* CONFIG_OSDP_SC_ENABLED */
987 default:
988 break;
989 }
990
991 return OSDP_CP_ERR_CAN_YIELD;
992 }
993
994 #ifdef CONFIG_OSDP_SC_ENABLED
osdp_cp_send_command_keyset(struct osdp_cmd_keyset * p)995 static int osdp_cp_send_command_keyset(struct osdp_cmd_keyset *p)
996 {
997 int i, res = 0;
998 struct osdp_cmd *cmd[OSDP_PD_MAX] = { 0 };
999 struct osdp_pd *pd;
1000 struct osdp *ctx = osdp_get_ctx();
1001
1002 for (i = 0; i < NUM_PD(ctx); i++) {
1003 pd = osdp_to_pd(ctx, i);
1004 if (!ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE)) {
1005 LOG_WRN("master_key based key set can be performed only"
1006 " when all PDs are ONLINE, SC_ACTIVE");
1007 return -1;
1008 }
1009 }
1010
1011 LOG_INF("master_key based key set is a global command; "
1012 "all connected PDs will be affected.");
1013
1014 for (i = 0; i < NUM_PD(ctx); i++) {
1015 pd = osdp_to_pd(ctx, i);
1016 cmd[i] = cp_cmd_alloc(pd);
1017 if (cmd[i] == NULL) {
1018 res = -1;
1019 break;
1020 }
1021 cmd[i]->id = CMD_KEYSET;
1022 memcpy(&cmd[i]->keyset, p, sizeof(struct osdp_cmd_keyset));
1023 }
1024
1025 for (i = 0; i < NUM_PD(ctx); i++) {
1026 pd = osdp_to_pd(ctx, i);
1027 if (res == 0) {
1028 cp_cmd_enqueue(pd, cmd[i]);
1029 } else if (cmd[i]) {
1030 cp_cmd_free(pd, cmd[i]);
1031 }
1032 }
1033
1034 return res;
1035 }
1036
cp_keyset_complete(struct osdp_pd * pd)1037 void cp_keyset_complete(struct osdp_pd *pd)
1038 {
1039 struct osdp_cmd *c = (struct osdp_cmd *)pd->ephemeral_data;
1040
1041 sc_deactivate(pd);
1042 CLEAR_FLAG(pd, PD_FLAG_SC_USE_SCBKD);
1043 memcpy(pd->sc.scbk, c->keyset.data, 16);
1044 cp_set_state(pd, OSDP_CP_STATE_SC_INIT);
1045 LOG_INF("SCBK set; restarting SC to verify new SCBK");
1046 }
1047 #endif /* CONFIG_OSDP_SC_ENABLED */
1048
osdp_update(struct osdp * ctx)1049 void osdp_update(struct osdp *ctx)
1050 {
1051 int i;
1052
1053 for (i = 0; i < NUM_PD(ctx); i++) {
1054 SET_CURRENT_PD(ctx, i);
1055 state_update(GET_CURRENT_PD(ctx));
1056 }
1057 }
1058
osdp_setup(struct osdp * ctx,uint8_t * key)1059 int osdp_setup(struct osdp *ctx, uint8_t *key)
1060 {
1061 ARG_UNUSED(ctx);
1062 ARG_UNUSED(key);
1063
1064 #ifdef CONFIG_OSDP_SC_ENABLED
1065 if (key == NULL) {
1066 LOG_ERR("Master key cannot be null");
1067 return -1;
1068 }
1069 memcpy(ctx->sc_master_key, key, 16);
1070 #endif
1071 return 0;
1072 }
1073
1074 /* --- Exported Methods --- */
1075
osdp_cp_set_event_callback(cp_event_callback_t cb,void * arg)1076 void osdp_cp_set_event_callback(cp_event_callback_t cb, void *arg)
1077 {
1078 struct osdp *ctx = osdp_get_ctx();
1079
1080 ctx->event_callback = cb;
1081 ctx->event_callback_arg = arg;
1082 }
1083
osdp_cp_send_command(int pd,struct osdp_cmd * cmd)1084 int osdp_cp_send_command(int pd, struct osdp_cmd *cmd)
1085 {
1086 struct osdp *ctx = osdp_get_ctx();
1087 struct osdp_cmd *p;
1088 int cmd_id;
1089
1090 if (pd < 0 || pd >= NUM_PD(ctx)) {
1091 LOG_ERR("Invalid PD number");
1092 return -1;
1093 }
1094 if (osdp_to_pd(ctx, pd)->state != OSDP_CP_STATE_ONLINE) {
1095 LOG_WRN("PD not online");
1096 return -1;
1097 }
1098
1099 switch (cmd->id) {
1100 case OSDP_CMD_OUTPUT:
1101 cmd_id = CMD_OUT;
1102 break;
1103 case OSDP_CMD_LED:
1104 cmd_id = CMD_LED;
1105 break;
1106 case OSDP_CMD_BUZZER:
1107 cmd_id = CMD_BUZ;
1108 break;
1109 case OSDP_CMD_TEXT:
1110 cmd_id = CMD_TEXT;
1111 break;
1112 case OSDP_CMD_COMSET:
1113 cmd_id = CMD_COMSET;
1114 break;
1115 #ifdef CONFIG_OSDP_SC_ENABLED
1116 case OSDP_CMD_KEYSET:
1117 return osdp_cp_send_command_keyset(&cmd->keyset);
1118 #endif
1119 default:
1120 LOG_ERR("Invalid CMD_ID:%d", cmd->id);
1121 return -1;
1122 }
1123
1124 p = cp_cmd_alloc(osdp_to_pd(ctx, pd));
1125 if (p == NULL) {
1126 return -1;
1127 }
1128 memcpy(p, cmd, sizeof(struct osdp_cmd));
1129 p->id = cmd_id; /* translate to internal */
1130 cp_cmd_enqueue(osdp_to_pd(ctx, pd), p);
1131 return 0;
1132 }
1133
