Lines Matching full:pd
62 static struct osdp_cmd *cp_cmd_alloc(struct osdp_pd *pd) in cp_cmd_alloc() argument
66 if (k_mem_slab_alloc(&pd->cmd.slab, (void **)&cmd, K_MSEC(100))) { in cp_cmd_alloc()
73 static void cp_cmd_free(struct osdp_pd *pd, struct osdp_cmd *cmd) in cp_cmd_free() argument
75 k_mem_slab_free(&pd->cmd.slab, (void *)cmd); in cp_cmd_free()
78 static void cp_cmd_enqueue(struct osdp_pd *pd, struct osdp_cmd *cmd) in cp_cmd_enqueue() argument
80 sys_slist_append(&pd->cmd.queue, &cmd->node); in cp_cmd_enqueue()
83 static int cp_cmd_dequeue(struct osdp_pd *pd, struct osdp_cmd **cmd) in cp_cmd_dequeue() argument
87 node = sys_slist_peek_head(&pd->cmd.queue); in cp_cmd_dequeue()
91 sys_slist_remove(&pd->cmd.queue, NULL, node); in cp_cmd_dequeue()
126 static int cp_build_command(struct osdp_pd *pd, uint8_t *buf, int max_len) in cp_build_command() argument
130 int data_off = osdp_phy_packet_get_data_offset(pd, buf); in cp_build_command()
132 uint8_t *smb = osdp_phy_packet_get_smb(pd, buf); in cp_build_command()
141 switch (pd->cmd_id) { in cp_build_command()
146 buf[len++] = pd->cmd_id; in cp_build_command()
152 buf[len++] = pd->cmd_id; in cp_build_command()
158 buf[len++] = pd->cmd_id; in cp_build_command()
164 buf[len++] = pd->cmd_id; in cp_build_command()
170 buf[len++] = pd->cmd_id; in cp_build_command()
176 buf[len++] = pd->cmd_id; in cp_build_command()
183 buf[len++] = pd->cmd_id; in cp_build_command()
190 buf[len++] = pd->cmd_id; in cp_build_command()
197 cmd = (struct osdp_cmd *)pd->ephemeral_data; in cp_build_command()
198 buf[len++] = pd->cmd_id; in cp_build_command()
208 cmd = (struct osdp_cmd *)pd->ephemeral_data; in cp_build_command()
209 buf[len++] = pd->cmd_id; in cp_build_command()
231 cmd = (struct osdp_cmd *)pd->ephemeral_data; in cp_build_command()
232 buf[len++] = pd->cmd_id; in cp_build_command()
240 cmd = (struct osdp_cmd *)pd->ephemeral_data; in cp_build_command()
244 buf[len++] = pd->cmd_id; in cp_build_command()
258 cmd = (struct osdp_cmd *)pd->ephemeral_data; in cp_build_command()
259 buf[len++] = pd->cmd_id; in cp_build_command()
268 if (!sc_is_active(pd)) { in cp_build_command()
275 cmd = (struct osdp_cmd *)pd->ephemeral_data; in cp_build_command()
280 buf[len++] = pd->cmd_id; in cp_build_command()
286 osdp_compute_scbk(pd, cmd->keyset.data, buf + len); in cp_build_command()
303 smb[2] = ISSET_FLAG(pd, PD_FLAG_SC_USE_SCBKD) ? 0 : 1; in cp_build_command()
304 buf[len++] = pd->cmd_id; in cp_build_command()
305 memcpy(buf + len, pd->sc.cp_random, 8); in cp_build_command()
316 osdp_compute_cp_cryptogram(pd); in cp_build_command()
319 smb[2] = ISSET_FLAG(pd, PD_FLAG_SC_USE_SCBKD) ? 0 : 1; in cp_build_command()
320 buf[len++] = pd->cmd_id; in cp_build_command()
321 memcpy(buf + len, pd->sc.cp_cryptogram, 16); in cp_build_command()
326 LOG_ERR("Unknown/Unsupported CMD(%02x)", pd->cmd_id); in cp_build_command()
331 if (smb && (smb[1] > SCS_14) && sc_is_active(pd)) { in cp_build_command()
345 static int cp_decode_response(struct osdp_pd *pd, uint8_t *buf, int len) in cp_decode_response() argument
348 struct osdp *ctx = pd_to_osdp(pd); in cp_decode_response()
352 pd->reply_id = buf[pos++]; in cp_decode_response()
355 switch (pd->reply_id) { in cp_decode_response()
366 LOG_WRN("PD replied with NAK(%d) for CMD(%02x)", in cp_decode_response()
367 buf[pos], pd->cmd_id); in cp_decode_response()
374 pd->id.vendor_code = buf[pos++]; in cp_decode_response()
375 pd->id.vendor_code |= buf[pos++] << 8; in cp_decode_response()
376 pd->id.vendor_code |= buf[pos++] << 16; in cp_decode_response()
378 pd->id.model = buf[pos++]; in cp_decode_response()
379 pd->id.version = buf[pos++]; in cp_decode_response()
381 pd->id.serial_number = buf[pos++]; in cp_decode_response()
382 pd->id.serial_number |= buf[pos++] << 8; in cp_decode_response()
383 pd->id.serial_number |= buf[pos++] << 16; in cp_decode_response()
384 pd->id.serial_number |= buf[pos++] << 24; in cp_decode_response()
386 pd->id.firmware_version = buf[pos++] << 16; in cp_decode_response()
387 pd->id.firmware_version |= buf[pos++] << 8; in cp_decode_response()
388 pd->id.firmware_version |= buf[pos++]; in cp_decode_response()
401 pd->cap[t1].function_code = t1; in cp_decode_response()
402 pd->cap[t1].compliance_level = buf[pos++]; in cp_decode_response()
403 pd->cap[t1].num_items = buf[pos++]; in cp_decode_response()
408 if (pd->cap[t2].compliance_level & 0x01) { in cp_decode_response()
409 SET_FLAG(pd, PD_FLAG_SC_CAPABLE); in cp_decode_response()
411 CLEAR_FLAG(pd, PD_FLAG_SC_CAPABLE); in cp_decode_response()
420 SET_FLAG(pd, PD_FLAG_TAMPER); in cp_decode_response()
422 CLEAR_FLAG(pd, PD_FLAG_TAMPER); in cp_decode_response()
425 SET_FLAG(pd, PD_FLAG_POWER); in cp_decode_response()
427 CLEAR_FLAG(pd, PD_FLAG_POWER); in cp_decode_response()
436 SET_FLAG(pd, PD_FLAG_R_TAMPER); in cp_decode_response()
438 CLEAR_FLAG(pd, PD_FLAG_R_TAMPER); in cp_decode_response()
452 pd->address = t1; in cp_decode_response()
453 pd->baud_rate = temp32; in cp_decode_response()
467 ctx->event_callback(ctx->event_callback_arg, pd->idx, &event); in cp_decode_response()
485 ctx->event_callback(ctx->event_callback_arg, pd->idx, &event); in cp_decode_response()
502 ctx->event_callback(ctx->event_callback_arg, pd->idx, &event); in cp_decode_response()
506 /* PD busy; signal upper layer to retry command */ in cp_decode_response()
514 if (sc_is_active(pd) || pd->cmd_id != CMD_CHLNG) { in cp_decode_response()
515 LOG_ERR("Out of order REPLY_CCRYPT; has PD gone rogue?"); in cp_decode_response()
521 memcpy(pd->sc.pd_client_uid, buf + pos, 8); in cp_decode_response()
522 memcpy(pd->sc.pd_random, buf + pos + 8, 8); in cp_decode_response()
523 memcpy(pd->sc.pd_cryptogram, buf + pos + 16, 16); in cp_decode_response()
525 osdp_compute_session_keys(pd); in cp_decode_response()
526 if (osdp_verify_pd_cryptogram(pd) != 0) { in cp_decode_response()
527 LOG_ERR("Failed to verify PD cryptogram"); in cp_decode_response()
533 if (sc_is_active(pd) || pd->cmd_id != CMD_SCRYPT) { in cp_decode_response()
534 LOG_ERR("Out of order REPLY_RMAC_I; has PD gone rogue?"); in cp_decode_response()
540 memcpy(pd->sc.r_mac, buf + pos, 16); in cp_decode_response()
541 sc_activate(pd); in cp_decode_response()
547 pd->cmd_id, pd->reply_id); in cp_decode_response()
553 pd->cmd_id, pd->reply_id); in cp_decode_response()
556 if (pd->cmd_id != CMD_POLL) { in cp_decode_response()
557 LOG_DBG("CMD(%02x) REPLY(%02x)", pd->cmd_id, pd->reply_id); in cp_decode_response()
563 static int cp_build_packet(struct osdp_pd *pd) in cp_build_packet() argument
568 len = osdp_phy_packet_init(pd, pd->rx_buf, sizeof(pd->rx_buf)); in cp_build_packet()
574 ret = cp_build_command(pd, pd->rx_buf, sizeof(pd->rx_buf)); in cp_build_packet()
581 len = osdp_phy_packet_finalize(pd, pd->rx_buf, len, sizeof(pd->rx_buf)); in cp_build_packet()
586 pd->rx_buf_len = len; in cp_build_packet()
591 static int cp_send_command(struct osdp_pd *pd) in cp_send_command() argument
596 if (pd->channel.flush) { in cp_send_command()
597 pd->channel.flush(pd->channel.data); in cp_send_command()
600 ret = pd->channel.send(pd->channel.data, pd->rx_buf, pd->rx_buf_len); in cp_send_command()
601 if (ret != pd->rx_buf_len) { in cp_send_command()
603 pd->rx_buf_len, ret); in cp_send_command()
608 if (pd->cmd_id != CMD_POLL) { in cp_send_command()
610 osdp_dump(NULL, pd->rx_buf, pd->rx_buf_len); in cp_send_command()
617 static int cp_process_reply(struct osdp_pd *pd) in cp_process_reply() argument
622 buf = pd->rx_buf + pd->rx_buf_len; in cp_process_reply()
623 remaining = sizeof(pd->rx_buf) - pd->rx_buf_len; in cp_process_reply()
625 len = pd->channel.recv(pd->channel.data, buf, remaining); in cp_process_reply()
629 pd->rx_buf_len += len; in cp_process_reply()
632 if (pd->cmd_id != CMD_POLL) { in cp_process_reply()
634 osdp_dump(NULL, pd->rx_buf, pd->rx_buf_len); in cp_process_reply()
638 err = osdp_phy_check_packet(pd, pd->rx_buf, pd->rx_buf_len, &len); in cp_process_reply()
655 len = osdp_phy_decode_packet(pd, pd->rx_buf, len, &buf); in cp_process_reply()
659 err = cp_decode_response(pd, buf, len); in cp_process_reply()
662 len = pd->rx_buf_len - one_pkt_len; in cp_process_reply()
664 memmove(pd->rx_buf, pd->rx_buf + one_pkt_len, len); in cp_process_reply()
666 pd->rx_buf_len = len; in cp_process_reply()
671 static void cp_flush_command_queue(struct osdp_pd *pd) in cp_flush_command_queue() argument
675 while (cp_cmd_dequeue(pd, &cmd) == 0) { in cp_flush_command_queue()
676 cp_cmd_free(pd, cmd); in cp_flush_command_queue()
680 static inline void cp_set_state(struct osdp_pd *pd, enum osdp_cp_state_e state) in cp_set_state() argument
682 pd->state = state; in cp_set_state()
683 CLEAR_FLAG(pd, PD_FLAG_AWAIT_RESP); in cp_set_state()
686 static inline void cp_set_online(struct osdp_pd *pd) in cp_set_online() argument
688 cp_set_state(pd, OSDP_CP_STATE_ONLINE); in cp_set_online()
689 pd->wait_ms = 0; in cp_set_online()
690 pd->tstamp = 0; in cp_set_online()
693 static inline void cp_set_offline(struct osdp_pd *pd) in cp_set_offline() argument
695 sc_deactivate(pd); in cp_set_offline()
696 pd->state = OSDP_CP_STATE_OFFLINE; in cp_set_offline()
697 pd->tstamp = osdp_millis_now(); in cp_set_offline()
698 if (pd->wait_ms == 0) { in cp_set_offline()
699 pd->wait_ms = 1000; /* retry after 1 second initially */ in cp_set_offline()
701 pd->wait_ms <<= 1; in cp_set_offline()
702 if (pd->wait_ms > OSDP_ONLINE_RETRY_WAIT_MAX_MS) { in cp_set_offline()
703 pd->wait_ms = OSDP_ONLINE_RETRY_WAIT_MAX_MS; in cp_set_offline()
709 static inline bool cp_sc_should_retry(struct osdp_pd *pd) in cp_sc_should_retry() argument
711 return (sc_is_capable(pd) && !sc_is_active(pd) && in cp_sc_should_retry()
712 osdp_millis_since(pd->sc_tstamp) > OSDP_PD_SC_RETRY_MS); in cp_sc_should_retry()
716 static int cp_phy_state_update(struct osdp_pd *pd) in cp_phy_state_update() argument
722 switch (pd->phy_state) { in cp_phy_state_update()
724 elapsed = osdp_millis_since(pd->phy_tstamp); in cp_phy_state_update()
728 pd->phy_state = OSDP_CP_PHY_STATE_SEND_CMD; in cp_phy_state_update()
734 if (cp_cmd_dequeue(pd, &cmd)) { in cp_phy_state_update()
738 pd->cmd_id = cmd->id; in cp_phy_state_update()
739 memcpy(pd->ephemeral_data, cmd, sizeof(struct osdp_cmd)); in cp_phy_state_update()
740 cp_cmd_free(pd, cmd); in cp_phy_state_update()
743 if (cp_build_packet(pd)) { in cp_phy_state_update()
745 pd->cmd_id); in cp_phy_state_update()
749 if (cp_send_command(pd)) { in cp_phy_state_update()
750 LOG_ERR("Failed to send CMD(%d)", pd->cmd_id); in cp_phy_state_update()
751 pd->phy_state = OSDP_CP_PHY_STATE_ERR; in cp_phy_state_update()
756 pd->phy_state = OSDP_CP_PHY_STATE_REPLY_WAIT; in cp_phy_state_update()
757 pd->rx_buf_len = 0; /* reset buf_len for next use */ in cp_phy_state_update()
758 pd->phy_tstamp = osdp_millis_now(); in cp_phy_state_update()
761 rc = cp_process_reply(pd); in cp_phy_state_update()
764 if (sc_is_active(pd)) { in cp_phy_state_update()
765 pd->sc_tstamp = osdp_millis_now(); in cp_phy_state_update()
768 pd->phy_state = OSDP_CP_PHY_STATE_IDLE; in cp_phy_state_update()
772 LOG_INF("PD busy; retry last command"); in cp_phy_state_update()
773 pd->phy_tstamp = osdp_millis_now(); in cp_phy_state_update()
774 pd->phy_state = OSDP_CP_PHY_STATE_WAIT; in cp_phy_state_update()
778 osdp_millis_since(pd->phy_tstamp) > OSDP_RESP_TOUT_MS) { in cp_phy_state_update()
781 pd->cmd_id); in cp_phy_state_update()
783 pd->rx_buf_len = 0; in cp_phy_state_update()
784 if (pd->channel.flush) { in cp_phy_state_update()
785 pd->channel.flush(pd->channel.data); in cp_phy_state_update()
787 cp_flush_command_queue(pd); in cp_phy_state_update()
788 pd->phy_state = OSDP_CP_PHY_STATE_ERR; in cp_phy_state_update()
799 static int cp_cmd_dispatcher(struct osdp_pd *pd, int cmd) in cp_cmd_dispatcher() argument
803 if (ISSET_FLAG(pd, PD_FLAG_AWAIT_RESP)) { in cp_cmd_dispatcher()
804 CLEAR_FLAG(pd, PD_FLAG_AWAIT_RESP); in cp_cmd_dispatcher()
808 c = cp_cmd_alloc(pd); in cp_cmd_dispatcher()
816 memcpy(&c->keyset, pd->ephemeral_data, sizeof(c->keyset)); in cp_cmd_dispatcher()
819 cp_cmd_enqueue(pd, c); in cp_cmd_dispatcher()
820 SET_FLAG(pd, PD_FLAG_AWAIT_RESP); in cp_cmd_dispatcher()
824 static int state_update(struct osdp_pd *pd) in state_update() argument
829 struct osdp *ctx = pd_to_osdp(pd); in state_update()
833 phy_state = cp_phy_state_update(pd); in state_update()
839 /* Certain states can fail without causing PD offline */ in state_update()
840 soft_fail = (pd->state == OSDP_CP_STATE_SC_CHLNG); in state_update()
843 if (pd->state != OSDP_CP_STATE_OFFLINE && in state_update()
845 cp_set_offline(pd); in state_update()
851 switch (pd->state) { in state_update()
854 if (cp_sc_should_retry(pd)) { in state_update()
856 cp_set_state(pd, OSDP_CP_STATE_SC_INIT); in state_update()
860 if (osdp_millis_since(pd->tstamp) < OSDP_PD_POLL_TIMEOUT_MS) { in state_update()
863 if (cp_cmd_dispatcher(pd, CMD_POLL) == 0) { in state_update()
864 pd->tstamp = osdp_millis_now(); in state_update()
868 if (osdp_millis_since(pd->tstamp) > pd->wait_ms) { in state_update()
869 cp_set_state(pd, OSDP_CP_STATE_INIT); in state_update()
870 osdp_phy_state_reset(pd); in state_update()
874 if (cp_cmd_dispatcher(pd, CMD_POLL) != 0) { in state_update()
877 cp_set_state(pd, OSDP_CP_STATE_IDREQ); in state_update()
880 if (cp_cmd_dispatcher(pd, CMD_ID) != 0) { in state_update()
883 if (pd->reply_id != REPLY_PDID) { in state_update()
885 STRINGIFY(CMD_ID), pd->reply_id); in state_update()
886 cp_set_offline(pd); in state_update()
889 cp_set_state(pd, OSDP_CP_STATE_CAPDET); in state_update()
892 if (cp_cmd_dispatcher(pd, CMD_CAP) != 0) { in state_update()
895 if (pd->reply_id != REPLY_PDCAP) { in state_update()
897 STRINGIFY(CMD_CAP), pd->reply_id); in state_update()
898 cp_set_offline(pd); in state_update()
902 if (sc_is_capable(pd)) { in state_update()
903 CLEAR_FLAG(pd, PD_FLAG_SC_SCBKD_DONE); in state_update()
904 CLEAR_FLAG(pd, PD_FLAG_SC_USE_SCBKD); in state_update()
905 cp_set_state(pd, OSDP_CP_STATE_SC_INIT); in state_update()
909 cp_set_online(pd); in state_update()
913 osdp_sc_setup(pd); in state_update()
914 cp_set_state(pd, OSDP_CP_STATE_SC_CHLNG); in state_update()
917 if (cp_cmd_dispatcher(pd, CMD_CHLNG) != 0) { in state_update()
921 if (ISSET_FLAG(pd, PD_FLAG_SC_USE_SCBKD)) { in state_update()
923 pd->sc_tstamp = osdp_millis_now(); in state_update()
924 osdp_phy_state_reset(pd); in state_update()
925 cp_set_online(pd); in state_update()
928 SET_FLAG(pd, PD_FLAG_SC_USE_SCBKD); in state_update()
929 cp_set_state(pd, OSDP_CP_STATE_SC_INIT); in state_update()
930 pd->phy_state = 0; /* soft reset phy state */ in state_update()
934 if (pd->reply_id != REPLY_CCRYPT) { in state_update()
936 pd->sc_tstamp = osdp_millis_now(); in state_update()
937 osdp_phy_state_reset(pd); in state_update()
938 cp_set_online(pd); in state_update()
941 cp_set_state(pd, OSDP_CP_STATE_SC_SCRYPT); in state_update()
944 if (cp_cmd_dispatcher(pd, CMD_SCRYPT) != 0) { in state_update()
947 if (pd->reply_id != REPLY_RMAC_I) { in state_update()
949 osdp_phy_state_reset(pd); in state_update()
950 pd->sc_tstamp = osdp_millis_now(); in state_update()
951 cp_set_online(pd); in state_update()
954 if (ISSET_FLAG(pd, PD_FLAG_SC_USE_SCBKD)) { in state_update()
956 cp_set_state(pd, OSDP_CP_STATE_SET_SCBK); in state_update()
960 pd->sc_tstamp = osdp_millis_now(); in state_update()
961 cp_set_online(pd); in state_update()
964 if (!ISSET_FLAG(pd, PD_FLAG_AWAIT_RESP)) { in state_update()
965 keyset = (struct osdp_cmd_keyset *)pd->ephemeral_data; in state_update()
966 if (ISSET_FLAG(pd, PD_FLAG_HAS_SCBK)) { in state_update()
967 memcpy(keyset->data, pd->sc.scbk, 16); in state_update()
975 if (cp_cmd_dispatcher(pd, CMD_KEYSET) != 0) { in state_update()
978 if (pd->reply_id == REPLY_NAK) { in state_update()
980 cp_set_online(pd); in state_update()
983 cp_keyset_complete(pd); in state_update()
984 pd->seq_number = -1; in state_update()
999 struct osdp_pd *pd; in osdp_cp_send_command_keyset() local
1003 pd = osdp_to_pd(ctx, i); in osdp_cp_send_command_keyset()
1004 if (!ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE)) { in osdp_cp_send_command_keyset()
1015 pd = osdp_to_pd(ctx, i); in osdp_cp_send_command_keyset()
1016 cmd[i] = cp_cmd_alloc(pd); in osdp_cp_send_command_keyset()
1026 pd = osdp_to_pd(ctx, i); in osdp_cp_send_command_keyset()
1028 cp_cmd_enqueue(pd, cmd[i]); in osdp_cp_send_command_keyset()
1030 cp_cmd_free(pd, cmd[i]); in osdp_cp_send_command_keyset()
1037 void cp_keyset_complete(struct osdp_pd *pd) in cp_keyset_complete() argument
1039 struct osdp_cmd *c = (struct osdp_cmd *)pd->ephemeral_data; in cp_keyset_complete()
1041 sc_deactivate(pd); in cp_keyset_complete()
1042 CLEAR_FLAG(pd, PD_FLAG_SC_USE_SCBKD); in cp_keyset_complete()
1043 memcpy(pd->sc.scbk, c->keyset.data, 16); in cp_keyset_complete()
1044 cp_set_state(pd, OSDP_CP_STATE_SC_INIT); in cp_keyset_complete()
1084 int osdp_cp_send_command(int pd, struct osdp_cmd *cmd) in osdp_cp_send_command() argument
1090 if (pd < 0 || pd >= NUM_PD(ctx)) { in osdp_cp_send_command()
1091 LOG_ERR("Invalid PD number"); in osdp_cp_send_command()
1094 if (osdp_to_pd(ctx, pd)->state != OSDP_CP_STATE_ONLINE) { in osdp_cp_send_command()
1095 LOG_WRN("PD not online"); in osdp_cp_send_command()
1124 p = cp_cmd_alloc(osdp_to_pd(ctx, pd)); in osdp_cp_send_command()
1130 cp_cmd_enqueue(osdp_to_pd(ctx, pd), p); in osdp_cp_send_command()