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