From f4e3f2b8283f050244f9695571aa1d5eff40a1c1 Mon Sep 17 00:00:00 2001 From: Siddharth Chandrasekaran Date: Sat, 28 May 2022 21:25:01 +0200 Subject: [PATCH] mgmt/osdp: Add inline methods for flag checks The raw, flags check has become a bit excessive and has begun to affect code readability. Provide inline functions for those accesses that are frequent. Also, get rid of `struct osdp_cp` as it can be fully represented by `struct osdp` itself. Signed-off-by: Siddharth Chandrasekaran --- subsys/mgmt/osdp/src/osdp.c | 9 ++-- subsys/mgmt/osdp/src/osdp_common.c | 2 +- subsys/mgmt/osdp/src/osdp_common.h | 72 ++++++++++++++++++++---------- subsys/mgmt/osdp/src/osdp_cp.c | 50 ++++++++++++--------- subsys/mgmt/osdp/src/osdp_pd.c | 24 +++++----- subsys/mgmt/osdp/src/osdp_phy.c | 51 ++++++++++----------- subsys/mgmt/osdp/src/osdp_sc.c | 7 ++- 7 files changed, 119 insertions(+), 96 deletions(-) diff --git a/subsys/mgmt/osdp/src/osdp.c b/subsys/mgmt/osdp/src/osdp.c index 22749201eff..711fd537391 100644 --- a/subsys/mgmt/osdp/src/osdp.c +++ b/subsys/mgmt/osdp/src/osdp.c @@ -41,7 +41,6 @@ struct osdp_device { }; static struct osdp osdp_ctx; -static struct osdp_cp osdp_cp_ctx; static struct osdp_pd osdp_pd_ctx[CONFIG_OSDP_NUM_CONNECTED_PD]; static struct osdp_device osdp_device; static struct k_thread osdp_refresh_thread; @@ -142,17 +141,15 @@ static struct osdp *osdp_build_ctx(struct osdp_channel *channel) } #endif ctx = &osdp_ctx; - ctx->cp = &osdp_cp_ctx; - ctx->cp->__parent = ctx; - ctx->cp->num_pd = CONFIG_OSDP_NUM_CONNECTED_PD; + ctx->num_pd = CONFIG_OSDP_NUM_CONNECTED_PD; ctx->pd = &osdp_pd_ctx[0]; SET_CURRENT_PD(ctx, 0); for (i = 0; i < CONFIG_OSDP_NUM_CONNECTED_PD; i++) { - pd = TO_PD(ctx, i); + pd = osdp_to_pd(ctx, i); pd->idx = i; pd->seq_number = -1; - pd->__parent = ctx; + pd->osdp_ctx = ctx; pd->address = pd_adddres[i]; pd->baud_rate = CONFIG_OSDP_UART_BAUD_RATE; memcpy(&pd->channel, channel, sizeof(struct osdp_channel)); diff --git a/subsys/mgmt/osdp/src/osdp_common.c b/subsys/mgmt/osdp/src/osdp_common.c index 70fd1942322..9600714fda1 100644 --- a/subsys/mgmt/osdp/src/osdp_common.c +++ b/subsys/mgmt/osdp/src/osdp_common.c @@ -189,7 +189,7 @@ uint32_t osdp_get_sc_status_mask(void) struct osdp *ctx = osdp_get_ctx(); for (i = 0; i < NUM_PD(ctx); i++) { - pd = TO_PD(ctx, i); + pd = osdp_to_pd(ctx, i); if (ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE)) { mask |= 1 << i; } diff --git a/subsys/mgmt/osdp/src/osdp_common.h b/subsys/mgmt/osdp/src/osdp_common.h index 85e8537201a..163b6a342ec 100644 --- a/subsys/mgmt/osdp/src/osdp_common.h +++ b/subsys/mgmt/osdp/src/osdp_common.h @@ -24,22 +24,15 @@ #define BYTE_2(x) (uint8_t)(((x) >> 16) & 0xFF) #define BYTE_3(x) (uint8_t)(((x) >> 24) & 0xFF) -/* casting helpers */ -#define TO_OSDP(p) ((struct osdp *)p) -#define TO_CP(p) (((struct osdp *)(p))->cp) -#define TO_PD(p, i) (((struct osdp *)(p))->pd + i) -#define TO_CTX(p) ((struct osdp *)p->__parent) - -#define GET_CURRENT_PD(p) (TO_CP(p)->current_pd) +#define GET_CURRENT_PD(p) ((p)->current_pd) #define SET_CURRENT_PD(p, i) \ do { \ - TO_CP(p)->current_pd = TO_PD(p, i); \ - TO_CP(p)->pd_offset = i; \ + (p)->current_pd = osdp_to_pd(p, i); \ } while (0) #define PD_MASK(ctx) \ - (uint32_t)((1 << (TO_CP(ctx)->num_pd)) - 1) + (uint32_t)((1 << ((ctx)->num_pd)) - 1) #define AES_PAD_LEN(x) ((x + 16 - 1) & (~(16 - 1))) -#define NUM_PD(ctx) (TO_CP(ctx)->num_pd) +#define NUM_PD(ctx) ((ctx)->num_pd) #define OSDP_COMMAND_DATA_MAX_LEN sizeof(struct osdp_cmd) /** @@ -414,7 +407,7 @@ struct osdp_secure_channel { #endif struct osdp_pd { - void *__parent; + void *osdp_ctx; int idx; uint32_t flags; @@ -449,23 +442,16 @@ struct osdp_pd { #endif }; -struct osdp_cp { - void *__parent; - uint32_t flags; - int num_pd; - struct osdp_pd *current_pd; /* current operational pd's pointer */ - int pd_offset; /* current pd's offset into ctx->pd */ - struct osdp_notifiers notifier; -}; - struct osdp { int magic; uint32_t flags; - struct osdp_cp *cp; + int num_pd; + struct osdp_pd *current_pd; /* current operational pd's pointer */ struct osdp_pd *pd; #ifdef CONFIG_OSDP_SC_ENABLED uint8_t sc_master_key[16]; #endif + struct osdp_notifiers notifier; }; /* from osdp_phy.c */ @@ -503,7 +489,7 @@ void osdp_decrypt(uint8_t *key, uint8_t *iv, uint8_t *data, int len); /* from osdp_sc.c */ void osdp_compute_scbk(struct osdp_pd *pd, uint8_t *scbk); -void osdp_compute_session_keys(struct osdp *ctx); +void osdp_compute_session_keys(struct osdp_pd *pd); void osdp_compute_cp_cryptogram(struct osdp_pd *pd); int osdp_verify_cp_cryptogram(struct osdp_pd *pd); void osdp_compute_pd_cryptogram(struct osdp_pd *pd); @@ -520,4 +506,44 @@ void osdp_fill_random(uint8_t *buf, int len); int osdp_setup(struct osdp *ctx, uint8_t *key); void osdp_update(struct osdp *ctx); +static inline struct osdp *pd_to_osdp(struct osdp_pd *pd) +{ + return pd->osdp_ctx; +} + +static inline struct osdp_pd *osdp_to_pd(struct osdp *ctx, int pd_idx) +{ + return ctx->pd + pd_idx; +} + +static inline bool is_pd_mode(struct osdp_pd *pd) +{ + return ISSET_FLAG(pd, PD_FLAG_PD_MODE); +} + +static inline bool is_cp_mode(struct osdp_pd *pd) +{ + return !ISSET_FLAG(pd, PD_FLAG_PD_MODE); +} + +static inline bool sc_is_capable(struct osdp_pd *pd) +{ + return ISSET_FLAG(pd, PD_FLAG_SC_CAPABLE); +} + +static inline bool sc_is_active(struct osdp_pd *pd) +{ + return ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE); +} + +static inline void sc_activate(struct osdp_pd *pd) +{ + SET_FLAG(pd, PD_FLAG_SC_ACTIVE); +} + +static inline void sc_deactivate(struct osdp_pd *pd) +{ + CLEAR_FLAG(pd, PD_FLAG_SC_ACTIVE); +} + #endif /* _OSDP_COMMON_H_ */ diff --git a/subsys/mgmt/osdp/src/osdp_cp.c b/subsys/mgmt/osdp/src/osdp_cp.c index 85fc73eed8d..3c8cfc2e8cc 100644 --- a/subsys/mgmt/osdp/src/osdp_cp.c +++ b/subsys/mgmt/osdp/src/osdp_cp.c @@ -213,7 +213,7 @@ static int cp_build_command(struct osdp_pd *pd, uint8_t *buf, int max_len) break; #ifdef CONFIG_OSDP_SC_ENABLED case CMD_KEYSET: - if (!ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE)) { + if (!sc_is_active(pd)) { LOG_ERR("Cannot perform KEYSET without SC!"); return OSDP_CP_ERR_GENERIC; } @@ -262,7 +262,7 @@ static int cp_build_command(struct osdp_pd *pd, uint8_t *buf, int max_len) } #ifdef CONFIG_OSDP_SC_ENABLED - if (smb && (smb[1] > SCS_14) && ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE)) { + if (smb && (smb[1] > SCS_14) && sc_is_active(pd)) { /** * When SC active and current cmd is not a handshake (<= SCS_14) * then we must set SCS type to 17 if this message has data @@ -283,7 +283,7 @@ static int cp_build_command(struct osdp_pd *pd, uint8_t *buf, int max_len) static int cp_decode_response(struct osdp_pd *pd, uint8_t *buf, int len) { uint32_t temp32; - struct osdp_cp *cp = TO_CTX(pd)->cp; + struct osdp *ctx = pd_to_osdp(pd); int i, ret = OSDP_CP_ERR_GENERIC, pos = 0, t1, t2; if (len < 1) { @@ -404,10 +404,10 @@ static int cp_decode_response(struct osdp_pd *pd, uint8_t *buf, int len) if ((len - REPLY_KEYPPAD_DATA_LEN) != t1) { break; } - if (cp->notifier.keypress) { + if (ctx->notifier.keypress) { for (i = 0; i < t1; i++) { t2 = buf[pos + i]; /* key data */ - cp->notifier.keypress(pd->idx, t2); + ctx->notifier.keypress(pd->idx, t2); } } ret = OSDP_CP_ERR_NONE; @@ -423,8 +423,8 @@ static int cp_decode_response(struct osdp_pd *pd, uint8_t *buf, int len) if ((len - REPLY_RAW_DATA_LEN) != t2) { break; } - if (cp->notifier.cardread) { - cp->notifier.cardread(pd->idx, t1, buf + pos, t2); + if (ctx->notifier.cardread) { + ctx->notifier.cardread(pd->idx, t1, buf + pos, t2); } ret = OSDP_CP_ERR_NONE; break; @@ -438,8 +438,8 @@ static int cp_decode_response(struct osdp_pd *pd, uint8_t *buf, int len) if ((len - REPLY_FMT_DATA_LEN) != t1) { break; } - if (cp->notifier.cardread) { - cp->notifier.cardread(pd->idx, OSDP_CARD_FMT_ASCII, + if (ctx->notifier.cardread) { + ctx->notifier.cardread(pd->idx, OSDP_CARD_FMT_ASCII, buf + pos, t1); } ret = OSDP_CP_ERR_NONE; @@ -465,7 +465,7 @@ static int cp_decode_response(struct osdp_pd *pd, uint8_t *buf, int len) for (i = 0; i < 16; i++) { pd->sc.pd_cryptogram[i] = buf[pos++]; } - osdp_compute_session_keys(TO_CTX(pd)); + osdp_compute_session_keys(pd); if (osdp_verify_pd_cryptogram(pd) != 0) { LOG_ERR("Failed to verify PD cryptogram"); return OSDP_CP_ERR_GENERIC; @@ -479,7 +479,7 @@ static int cp_decode_response(struct osdp_pd *pd, uint8_t *buf, int len) for (i = 0; i < 16; i++) { pd->sc.r_mac[i] = buf[pos++]; } - SET_FLAG(pd, PD_FLAG_SC_ACTIVE); + sc_activate(pd); ret = OSDP_CP_ERR_NONE; break; #endif /* CONFIG_OSDP_SC_ENABLED */ @@ -592,7 +592,7 @@ static void cp_flush_command_queue(struct osdp_pd *pd) static inline void cp_set_offline(struct osdp_pd *pd) { - CLEAR_FLAG(pd, PD_FLAG_SC_ACTIVE); + sc_deactivate(pd); pd->state = OSDP_CP_STATE_OFFLINE; pd->tstamp = osdp_millis_now(); } @@ -609,6 +609,14 @@ static inline void cp_set_state(struct osdp_pd *pd, enum osdp_cp_state_e state) CLEAR_FLAG(pd, PD_FLAG_AWAIT_RESP); } +#ifdef CONFIG_OSDP_SC_ENABLED +static inline bool cp_sc_should_retry(struct osdp_pd *pd) +{ + return (sc_is_capable(pd) && !sc_is_active(pd) && + osdp_millis_since(pd->sc_tstamp) > OSDP_PD_SC_RETRY_MS); +} +#endif + /** * Note: This method must not dequeue cmd unless it reaches an invalid state. */ @@ -729,9 +737,7 @@ static int state_update(struct osdp_pd *pd) switch (pd->state) { case OSDP_CP_STATE_ONLINE: #ifdef CONFIG_OSDP_SC_ENABLED - if (ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE) == false && - ISSET_FLAG(pd, PD_FLAG_SC_CAPABLE) == true && - osdp_millis_since(pd->sc_tstamp) > OSDP_PD_SC_RETRY_MS) { + if (cp_sc_should_retry(pd)) { LOG_INF("Retry SC after retry timeout"); cp_set_state(pd, OSDP_CP_STATE_SC_INIT); break; @@ -773,7 +779,7 @@ static int state_update(struct osdp_pd *pd) cp_set_offline(pd); } #ifdef CONFIG_OSDP_SC_ENABLED - if (ISSET_FLAG(pd, PD_FLAG_SC_CAPABLE)) { + if (sc_is_capable(pd)) { CLEAR_FLAG(pd, PD_FLAG_SC_SCBKD_DONE); CLEAR_FLAG(pd, PD_FLAG_SC_USE_SCBKD); cp_set_state(pd, OSDP_CP_STATE_SC_INIT); @@ -870,7 +876,7 @@ static int osdp_cp_send_command_keyset(struct osdp_cmd_keyset *cmd) } for (i = 0; i < NUM_PD(ctx); i++) { - pd = TO_PD(ctx, i); + pd = osdp_to_pd(ctx, i); p = osdp_cmd_alloc(pd); if (p == NULL) { return -1; @@ -915,7 +921,7 @@ int osdp_cp_set_callback_key_press(int (*cb)(int address, uint8_t key)) { struct osdp *ctx = osdp_get_ctx(); - ctx->cp->notifier.keypress = cb; + ctx->notifier.keypress = cb; return 0; } @@ -925,7 +931,7 @@ int osdp_cp_set_callback_card_read( { struct osdp *ctx = osdp_get_ctx(); - TO_CP(ctx)->notifier.cardread = cb; + ctx->notifier.cardread = cb; return 0; } @@ -940,7 +946,7 @@ int osdp_cp_send_command(int pd, struct osdp_cmd *cmd) LOG_ERR("Invalid PD number"); return -1; } - if (TO_PD(ctx, pd)->state != OSDP_CP_STATE_ONLINE) { + if (osdp_to_pd(ctx, pd)->state != OSDP_CP_STATE_ONLINE) { LOG_WRN("PD not online"); return -1; } @@ -970,12 +976,12 @@ int osdp_cp_send_command(int pd, struct osdp_cmd *cmd) return -1; } - p = osdp_cmd_alloc(TO_PD(ctx, pd)); + p = osdp_cmd_alloc(osdp_to_pd(ctx, pd)); if (p == NULL) { return -1; } memcpy(p, cmd, sizeof(struct osdp_cmd)); p->id = cmd_id; /* translate to internal */ - osdp_cmd_enqueue(TO_PD(ctx, pd), p); + osdp_cmd_enqueue(osdp_to_pd(ctx, pd), p); return 0; } diff --git a/subsys/mgmt/osdp/src/osdp_pd.c b/subsys/mgmt/osdp/src/osdp_pd.c index 0e09057c481..228cf525f4c 100644 --- a/subsys/mgmt/osdp/src/osdp_pd.c +++ b/subsys/mgmt/osdp/src/osdp_pd.c @@ -306,7 +306,7 @@ static int pd_decode_command(struct osdp_pd *pd, uint8_t *buf, int len) * For CMD_KEYSET to be accepted, PD must be * ONLINE and SC_ACTIVE. */ - if (ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE) == 0) { + if (!sc_is_active(pd)) { pd->reply_id = REPLY_NAK; pd->cmd_data[0] = OSDP_PD_NAK_SC_COND; LOG_ERR("Keyset with SC inactive"); @@ -346,7 +346,7 @@ static int pd_decode_command(struct osdp_pd *pd, uint8_t *buf, int len) break; } osdp_sc_init(pd); - CLEAR_FLAG(pd, PD_FLAG_SC_ACTIVE); + sc_deactivate(pd); for (i = 0; i < 8; i++) { pd->sc.cp_random[i] = buf[pos++]; } @@ -535,7 +535,7 @@ static int pd_build_reply(struct osdp_pd *pd, uint8_t *buf, int max_len) return -1; } osdp_fill_random(pd->sc.pd_random, 8); - osdp_compute_session_keys(TO_CTX(pd)); + osdp_compute_session_keys(pd); osdp_compute_pd_cryptogram(pd); buf[len++] = pd->reply_id; for (i = 0; i < 8; i++) { @@ -569,7 +569,7 @@ static int pd_build_reply(struct osdp_pd *pd, uint8_t *buf, int max_len) smb[1] = SCS_14; /* type */ if (osdp_verify_cp_cryptogram(pd) == 0) { smb[2] = 1; /* CP auth succeeded */ - SET_FLAG(pd, PD_FLAG_SC_ACTIVE); + sc_activate(pd); if (ISSET_FLAG(pd, PD_FLAG_SC_USE_SCBKD)) { LOG_WRN("SC Active with SCBK-D"); } else { @@ -585,7 +585,7 @@ static int pd_build_reply(struct osdp_pd *pd, uint8_t *buf, int max_len) } #ifdef CONFIG_OSDP_SC_ENABLED - if (smb && (smb[1] > SCS_14) && ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE)) { + if (smb && (smb[1] > SCS_14) && sc_is_active(pd)) { smb[0] = 2; /* length */ smb[1] = (len > 1) ? SCS_18 : SCS_16; } @@ -709,14 +709,14 @@ static int pd_receve_packet(struct osdp_pd *pd) void osdp_update(struct osdp *ctx) { int ret; - struct osdp_pd *pd = TO_PD(ctx, 0); + struct osdp_pd *pd = osdp_to_pd(ctx, 0); switch (pd->state) { case OSDP_PD_STATE_IDLE: ret = pd_receve_packet(pd); - if (ret == OSDP_PD_ERR_GENERIC || ((pd->rx_buf_len > 0 || - ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE)) && - osdp_millis_since(pd->tstamp) > OSDP_RESP_TOUT_MS)) { + if (ret == OSDP_PD_ERR_GENERIC || + ((pd->rx_buf_len > 0 || sc_is_active(pd)) && + osdp_millis_since(pd->tstamp) > OSDP_RESP_TOUT_MS)) { /** * When we receive a command from PD after a timeout, * any established secure channel must be discarded. @@ -781,10 +781,10 @@ int osdp_setup(struct osdp *ctx, uint8_t *key) ARG_UNUSED(key); struct osdp_pd *pd; - if (ctx->cp->num_pd != 1) { + if (NUM_PD(ctx) != 1) { return -1; } - pd = TO_PD(ctx, 0); + pd = osdp_to_pd(ctx, 0); osdp_pd_set_attributes(pd, osdp_pd_cap, &osdp_pd_id); SET_FLAG(pd, PD_FLAG_PD_MODE); #ifdef CONFIG_OSDP_SC_ENABLED @@ -804,7 +804,7 @@ int osdp_setup(struct osdp *ctx, uint8_t *key) int osdp_pd_get_cmd(struct osdp_cmd *cmd) { struct osdp_cmd *c; - struct osdp_pd *pd = TO_PD(osdp_get_ctx(), 0); + struct osdp_pd *pd = osdp_to_pd(osdp_get_ctx(), 0); if (osdp_cmd_dequeue(pd, &c)) { return -1; diff --git a/subsys/mgmt/osdp/src/osdp_phy.c b/subsys/mgmt/osdp/src/osdp_phy.c index 2e2813bb4ad..f3b8bf78ed2 100644 --- a/subsys/mgmt/osdp/src/osdp_phy.c +++ b/subsys/mgmt/osdp/src/osdp_phy.c @@ -87,10 +87,9 @@ int osdp_phy_in_sc_handshake(int is_reply, int id) int osdp_phy_packet_init(struct osdp_pd *pd, uint8_t *buf, int max_len) { - int exp_len, pd_mode, sb_len, id; + int exp_len, sb_len, id; struct osdp_packet_header *pkt; - pd_mode = ISSET_FLAG(pd, PD_FLAG_PD_MODE); exp_len = sizeof(struct osdp_packet_header) + 64; /* 64 is estimated */ if (max_len < exp_len) { LOG_ERR("packet_init: out of space! CMD: %02x", pd->cmd_id); @@ -102,22 +101,22 @@ int osdp_phy_packet_init(struct osdp_pd *pd, uint8_t *buf, int max_len) pkt->mark = OSDP_PKT_MARK; pkt->som = OSDP_PKT_SOM; pkt->pd_address = pd->address & 0x7F; /* Use only the lower 7 bits */ - if (pd_mode) { + if (is_pd_mode(pd)) { /* PD must reply with MSB of it's address set */ pkt->pd_address |= 0x80; id = pd->reply_id; } else { id = pd->cmd_id; } - pkt->control = osdp_phy_get_seq_number(pd, !pd_mode); + pkt->control = osdp_phy_get_seq_number(pd, is_cp_mode(pd)); pkt->control |= PKT_CONTROL_CRC; sb_len = 0; - if (ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE)) { + if (sc_is_active(pd)) { pkt->control |= PKT_CONTROL_SCB; pkt->data[0] = sb_len = 2; pkt->data[1] = SCS_15; - } else if (osdp_phy_in_sc_handshake(pd_mode, id)) { + } else if (osdp_phy_in_sc_handshake(is_pd_mode(pd), id)) { pkt->control |= PKT_CONTROL_SCB; pkt->data[0] = sb_len = 3; pkt->data[1] = SCS_11; @@ -145,12 +144,10 @@ int osdp_phy_packet_finalize(struct osdp_pd *pd, uint8_t *buf, #ifdef CONFIG_OSDP_SC_ENABLED uint8_t *data; - int i, is_cmd, data_len; + int i, data_len; - if (ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE) && - pkt->control & PKT_CONTROL_SCB && - pkt->data[1] >= SCS_15) { - is_cmd = !ISSET_FLAG(pd, PD_FLAG_PD_MODE); + if (sc_is_active(pd) && + (pkt->control & PKT_CONTROL_SCB) && pkt->data[1] >= SCS_15) if (pkt->data[1] == SCS_17 || pkt->data[1] == SCS_18) { /** * Only the data portion of message (after id byte) @@ -161,8 +158,8 @@ int osdp_phy_packet_finalize(struct osdp_pd *pd, uint8_t *buf, * SCS_15/SCS_16 and send them. */ data = pkt->data + pkt->data[0] + 1; - data_len = len - (sizeof(struct osdp_packet_header) - + pkt->data[0] + 1); + data_len = len - (sizeof(struct osdp_packet_header) + + pkt->data[0] + 1); len -= data_len; /** * check if the passed buffer can hold the encrypted @@ -173,7 +170,7 @@ int osdp_phy_packet_finalize(struct osdp_pd *pd, uint8_t *buf, /* data_len + 1 for OSDP_SC_EOM_MARKER */ goto out_of_space_error; } - len += osdp_encrypt_data(pd, is_cmd, data, data_len); + len += osdp_encrypt_data(pd, is_cp_mode(pd), data, data_len); } /* len: with 4bytes MAC; with 2 byte CRC; without 1 byte mark */ if (len + 4 > max_len) { @@ -185,8 +182,8 @@ int osdp_phy_packet_finalize(struct osdp_pd *pd, uint8_t *buf, pkt->len_msb = BYTE_1(len - 1 + 2 + 4); /* compute and extend the buf with 4 MAC bytes */ - osdp_compute_mac(pd, is_cmd, buf + 1, len - 1); - data = is_cmd ? pd->sc.c_mac : pd->sc.r_mac; + osdp_compute_mac(pd, is_cp_mode(pd), buf + 1, len - 1); + data = is_cp_mode(pd) ? pd->sc.c_mac : pd->sc.r_mac; for (i = 0; i < 4; i++) { buf[len + i] = data[i]; } @@ -214,10 +211,9 @@ int osdp_phy_decode_packet(struct osdp_pd *pd, uint8_t *buf, int len) { uint8_t *data; uint16_t comp, cur; - int pkt_len, pd_mode, pd_addr, mac_offset; + int pkt_len, pd_addr, mac_offset; struct osdp_packet_header *pkt; - pd_mode = ISSET_FLAG(pd, PD_FLAG_PD_MODE); pkt = (struct osdp_packet_header *)buf; /* wait till we have the header */ @@ -232,7 +228,7 @@ int osdp_phy_decode_packet(struct osdp_pd *pd, uint8_t *buf, int len) return OSDP_ERR_PKT_FMT; } - if (!pd_mode && !(pkt->pd_address & 0x80)) { + if (is_cp_mode(pd) && !(pkt->pd_address & 0x80)) { LOG_ERR("Reply without address MSB set!"); return OSDP_ERR_PKT_FMT; } @@ -248,7 +244,7 @@ int osdp_phy_decode_packet(struct osdp_pd *pd, uint8_t *buf, int len) pd_addr = pkt->pd_address & 0x7F; if (pd_addr != pd->address && pd_addr != 0x7F) { /* not addressed to us and was not broadcasted */ - if (!pd_mode) { + if (is_cp_mode(pd)) { LOG_ERR("Invalid pd address %d", pd_addr); return OSDP_ERR_PKT_FMT; } @@ -258,7 +254,7 @@ int osdp_phy_decode_packet(struct osdp_pd *pd, uint8_t *buf, int len) /* validate sequence number */ cur = pkt->control & PKT_CONTROL_SQN; - if (pd_mode && cur == 0) { + if (is_pd_mode(pd) && cur == 0) { /** * CP is trying to restart communication by sending a 0. The * current PD implementation does not hold any state between @@ -269,7 +265,7 @@ int osdp_phy_decode_packet(struct osdp_pd *pd, uint8_t *buf, int len) pd->seq_number = -1; CLEAR_FLAG(pd, PD_FLAG_SC_ACTIVE); } - if (pd_mode && cur == pd->seq_number) { + if (is_pd_mode(pd) && cur == pd->seq_number) { /** * TODO: PD must resend the last response if CP send the same * sequence number again. @@ -279,7 +275,7 @@ int osdp_phy_decode_packet(struct osdp_pd *pd, uint8_t *buf, int len) pd->cmd_data[0] = OSDP_PD_NAK_SEQ_NUM; return OSDP_ERR_PKT_FMT; } - comp = osdp_phy_get_seq_number(pd, pd_mode); + comp = osdp_phy_get_seq_number(pd, is_pd_mode(pd)); if (comp != cur && !ISSET_FLAG(pd, PD_FLAG_SKIP_SEQ_CHECK)) { LOG_ERR("packet seq mismatch %d/%d", comp, cur); pd->reply_id = REPLY_NAK; @@ -319,7 +315,7 @@ int osdp_phy_decode_packet(struct osdp_pd *pd, uint8_t *buf, int len) int is_cmd; if (pkt->control & PKT_CONTROL_SCB) { - if (pd_mode && !ISSET_FLAG(pd, PD_FLAG_SC_CAPABLE)) { + if (is_pd_mode(pd) && !sc_is_capable(pd)) { LOG_ERR("PD is not SC capable"); pd->reply_id = REPLY_NAK; pd->cmd_data[0] = OSDP_PD_NAK_SC_UNSUP; @@ -347,7 +343,7 @@ int osdp_phy_decode_packet(struct osdp_pd *pd, uint8_t *buf, int len) data = pkt->data + pkt->data[0]; len -= pkt->data[0]; /* consume security block */ } else { - if (ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE)) { + if (sc_is_active(pd)) { LOG_ERR("Received plain-text message in SC"); pd->reply_id = REPLY_NAK; pd->cmd_data[0] = OSDP_PD_NAK_SC_COND; @@ -355,9 +351,8 @@ int osdp_phy_decode_packet(struct osdp_pd *pd, uint8_t *buf, int len) } } - if (ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE) && - pkt->control & PKT_CONTROL_SCB && - pkt->data[1] >= SCS_15) { + if (sc_is_active(pd) && + pkt->control & PKT_CONTROL_SCB && pkt->data[1] >= SCS_15) { /* validate MAC */ is_cmd = ISSET_FLAG(pd, PD_FLAG_PD_MODE); osdp_compute_mac(pd, is_cmd, buf + 1, mac_offset); diff --git a/subsys/mgmt/osdp/src/osdp_sc.c b/subsys/mgmt/osdp/src/osdp_sc.c index e72c5be20bf..27c26e6486a 100644 --- a/subsys/mgmt/osdp/src/osdp_sc.c +++ b/subsys/mgmt/osdp/src/osdp_sc.c @@ -21,7 +21,7 @@ static const uint8_t osdp_scbk_default[16] = { void osdp_compute_scbk(struct osdp_pd *pd, uint8_t *scbk) { int i; - struct osdp *ctx = TO_CTX(pd); + struct osdp *ctx = pd_to_osdp(pd); memcpy(scbk, pd->sc.pd_client_uid, 8); for (i = 8; i < 16; i++) { @@ -30,10 +30,9 @@ void osdp_compute_scbk(struct osdp_pd *pd, uint8_t *scbk) osdp_encrypt(ctx->sc_master_key, NULL, scbk, 16); } -void osdp_compute_session_keys(struct osdp *ctx) +void osdp_compute_session_keys(struct osdp_pd *pd) { int i; - struct osdp_pd *pd = GET_CURRENT_PD(ctx); if (ISSET_FLAG(pd, PD_FLAG_SC_USE_SCBKD)) { memcpy(pd->sc.scbk, osdp_scbk_default, 16); @@ -231,7 +230,7 @@ void osdp_sc_init(struct osdp_pd *pd) if (ISSET_FLAG(pd, PD_FLAG_PD_MODE)) { memcpy(pd->sc.scbk, key, 16); } - if (ISSET_FLAG(pd, PD_FLAG_PD_MODE)) { + if (is_pd_mode(pd)) { pd->sc.pd_client_uid[0] = BYTE_0(pd->id.vendor_code); pd->sc.pd_client_uid[1] = BYTE_1(pd->id.vendor_code); pd->sc.pd_client_uid[2] = BYTE_0(pd->id.model);