michaelh/zephyr
1
0
Fork 0
Code Releases Activity

mgmt/osdp: Add inline methods for flag checks

Browse source 
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 <sidcha.dev@gmail.com>
This commit is contained in:
Siddharth Chandrasekaran 2022-05-28 21:25:01 +02:00 • committed by Carles Cufí
commit f4e3f2b828
7 changed files with 119 additions and 96 deletions
Download patch file Download diff file Expand all files Collapse all files

9
subsys/mgmt/osdp/src/osdp.c
View file

@ -41,7 +41,6 @@ struct osdp_device {
}; };
static struct osdp osdp_ctx; 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_pd osdp_pd_ctx[CONFIG_OSDP_NUM_CONNECTED_PD];
static struct osdp_device osdp_device; static struct osdp_device osdp_device;
static struct k_thread osdp_refresh_thread; static struct k_thread osdp_refresh_thread;
@ -142,17 +141,15 @@ static struct osdp *osdp_build_ctx(struct osdp_channel *channel)
} }
#endif #endif
ctx = &osdp_ctx; ctx = &osdp_ctx;
ctx->cp = &osdp_cp_ctx; ctx->num_pd = CONFIG_OSDP_NUM_CONNECTED_PD;
ctx->cp->__parent = ctx;
ctx->cp->num_pd = CONFIG_OSDP_NUM_CONNECTED_PD;
ctx->pd = &osdp_pd_ctx[0]; ctx->pd = &osdp_pd_ctx[0];
SET_CURRENT_PD(ctx, 0); SET_CURRENT_PD(ctx, 0);
for (i = 0; i < CONFIG_OSDP_NUM_CONNECTED_PD; i++) { 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->idx = i;
pd->seq_number = -1; pd->seq_number = -1;
pd->__parent = ctx; pd->osdp_ctx = ctx;
pd->address = pd_adddres[i]; pd->address = pd_adddres[i];
pd->baud_rate = CONFIG_OSDP_UART_BAUD_RATE; pd->baud_rate = CONFIG_OSDP_UART_BAUD_RATE;
memcpy(&pd->channel, channel, sizeof(struct osdp_channel)); memcpy(&pd->channel, channel, sizeof(struct osdp_channel));

2
subsys/mgmt/osdp/src/osdp_common.c
View file

@ -189,7 +189,7 @@ uint32_t osdp_get_sc_status_mask(void)
struct osdp *ctx = osdp_get_ctx(); struct osdp *ctx = osdp_get_ctx();
for (i = 0; i < NUM_PD(ctx); i++) { 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)) { if (ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE)) {
mask |= 1 << i; mask |= 1 << i;
} }

72
subsys/mgmt/osdp/src/osdp_common.h
View file

@ -24,22 +24,15 @@
#define BYTE_2(x) (uint8_t)(((x) >> 16) & 0xFF) #define BYTE_2(x) (uint8_t)(((x) >> 16) & 0xFF)
#define BYTE_3(x) (uint8_t)(((x) >> 24) & 0xFF) #define BYTE_3(x) (uint8_t)(((x) >> 24) & 0xFF)
/* casting helpers */ #define GET_CURRENT_PD(p) ((p)->current_pd)
#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 SET_CURRENT_PD(p, i) \ #define SET_CURRENT_PD(p, i) \
do { \ do { \
TO_CP(p)->current_pd = TO_PD(p, i); \ (p)->current_pd = osdp_to_pd(p, i); \
TO_CP(p)->pd_offset = i; \
} while (0) } while (0)
#define PD_MASK(ctx) \ #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 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) #define OSDP_COMMAND_DATA_MAX_LEN sizeof(struct osdp_cmd)
/** /**
@ -414,7 +407,7 @@ struct osdp_secure_channel {
#endif #endif
struct osdp_pd { struct osdp_pd {
void *__parent; void *osdp_ctx;
int idx; int idx;
uint32_t flags; uint32_t flags;
@ -449,23 +442,16 @@ struct osdp_pd {
#endif #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 { struct osdp {
int magic; int magic;
uint32_t flags; uint32_t flags;
struct osdp_cp *cp; int num_pd;
struct osdp_pd *current_pd; /* current operational pd's pointer */
struct osdp_pd *pd; struct osdp_pd *pd;
#ifdef CONFIG_OSDP_SC_ENABLED #ifdef CONFIG_OSDP_SC_ENABLED
uint8_t sc_master_key[16]; uint8_t sc_master_key[16];
#endif #endif
struct osdp_notifiers notifier;
}; };
/* from osdp_phy.c */ /* 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 */ /* from osdp_sc.c */
void osdp_compute_scbk(struct osdp_pd *pd, uint8_t *scbk); 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); void osdp_compute_cp_cryptogram(struct osdp_pd *pd);
int osdp_verify_cp_cryptogram(struct osdp_pd *pd); int osdp_verify_cp_cryptogram(struct osdp_pd *pd);
void osdp_compute_pd_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); int osdp_setup(struct osdp *ctx, uint8_t *key);
void osdp_update(struct osdp *ctx); 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_ */ #endif /* _OSDP_COMMON_H_ */

50
subsys/mgmt/osdp/src/osdp_cp.c
View file

@ -213,7 +213,7 @@ static int cp_build_command(struct osdp_pd *pd, uint8_t *buf, int max_len)
break; break;
#ifdef CONFIG_OSDP_SC_ENABLED #ifdef CONFIG_OSDP_SC_ENABLED
case CMD_KEYSET: case CMD_KEYSET:
if (!ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE)) { if (!sc_is_active(pd)) {
LOG_ERR("Cannot perform KEYSET without SC!"); LOG_ERR("Cannot perform KEYSET without SC!");
return OSDP_CP_ERR_GENERIC; 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 #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) * 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 * 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) static int cp_decode_response(struct osdp_pd *pd, uint8_t *buf, int len)
{ {
uint32_t temp32; 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; int i, ret = OSDP_CP_ERR_GENERIC, pos = 0, t1, t2;
if (len < 1) { 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) { if ((len - REPLY_KEYPPAD_DATA_LEN) != t1) {
break; break;
} }
if (cp->notifier.keypress) { if (ctx->notifier.keypress) {
for (i = 0; i < t1; i++) { for (i = 0; i < t1; i++) {
t2 = buf[pos + i]; /* key data */ t2 = buf[pos + i]; /* key data */
cp->notifier.keypress(pd->idx, t2); ctx->notifier.keypress(pd->idx, t2);
} }
} }
ret = OSDP_CP_ERR_NONE; 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) { if ((len - REPLY_RAW_DATA_LEN) != t2) {
break; break;
} }
if (cp->notifier.cardread) { if (ctx->notifier.cardread) {
cp->notifier.cardread(pd->idx, t1, buf + pos, t2); ctx->notifier.cardread(pd->idx, t1, buf + pos, t2);
} }
ret = OSDP_CP_ERR_NONE; ret = OSDP_CP_ERR_NONE;
break; 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) { if ((len - REPLY_FMT_DATA_LEN) != t1) {
break; break;
} }
if (cp->notifier.cardread) { if (ctx->notifier.cardread) {
cp->notifier.cardread(pd->idx, OSDP_CARD_FMT_ASCII, ctx->notifier.cardread(pd->idx, OSDP_CARD_FMT_ASCII,
buf + pos, t1); buf + pos, t1);
} }
ret = OSDP_CP_ERR_NONE; 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++) { for (i = 0; i < 16; i++) {
pd->sc.pd_cryptogram[i] = buf[pos++]; 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) { if (osdp_verify_pd_cryptogram(pd) != 0) {
LOG_ERR("Failed to verify PD cryptogram"); LOG_ERR("Failed to verify PD cryptogram");
return OSDP_CP_ERR_GENERIC; 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++) { for (i = 0; i < 16; i++) {
pd->sc.r_mac[i] = buf[pos++]; pd->sc.r_mac[i] = buf[pos++];
} }
SET_FLAG(pd, PD_FLAG_SC_ACTIVE); sc_activate(pd);
ret = OSDP_CP_ERR_NONE; ret = OSDP_CP_ERR_NONE;
break; break;
#endif /* CONFIG_OSDP_SC_ENABLED */ #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) 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->state = OSDP_CP_STATE_OFFLINE;
pd->tstamp = osdp_millis_now(); 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); 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. * 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) { switch (pd->state) {
case OSDP_CP_STATE_ONLINE: case OSDP_CP_STATE_ONLINE:
#ifdef CONFIG_OSDP_SC_ENABLED #ifdef CONFIG_OSDP_SC_ENABLED
if (ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE) == false && if (cp_sc_should_retry(pd)) {
ISSET_FLAG(pd, PD_FLAG_SC_CAPABLE) == true &&
osdp_millis_since(pd->sc_tstamp) > OSDP_PD_SC_RETRY_MS) {
LOG_INF("Retry SC after retry timeout"); LOG_INF("Retry SC after retry timeout");
cp_set_state(pd, OSDP_CP_STATE_SC_INIT); cp_set_state(pd, OSDP_CP_STATE_SC_INIT);
break; break;
@ -773,7 +779,7 @@ static int state_update(struct osdp_pd *pd)
cp_set_offline(pd); cp_set_offline(pd);
} }
#ifdef CONFIG_OSDP_SC_ENABLED #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_SCBKD_DONE);
CLEAR_FLAG(pd, PD_FLAG_SC_USE_SCBKD); CLEAR_FLAG(pd, PD_FLAG_SC_USE_SCBKD);
cp_set_state(pd, OSDP_CP_STATE_SC_INIT); 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++) { for (i = 0; i < NUM_PD(ctx); i++) {
pd = TO_PD(ctx, i); pd = osdp_to_pd(ctx, i);
p = osdp_cmd_alloc(pd); p = osdp_cmd_alloc(pd);
if (p == NULL) { if (p == NULL) {
return -1; 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(); struct osdp *ctx = osdp_get_ctx();
ctx->cp->notifier.keypress = cb; ctx->notifier.keypress = cb;
return 0; return 0;
} }
@ -925,7 +931,7 @@ int osdp_cp_set_callback_card_read(
{ {
struct osdp *ctx = osdp_get_ctx(); struct osdp *ctx = osdp_get_ctx();
TO_CP(ctx)->notifier.cardread = cb; ctx->notifier.cardread = cb;
return 0; return 0;
} }
@ -940,7 +946,7 @@ int osdp_cp_send_command(int pd, struct osdp_cmd *cmd)
LOG_ERR("Invalid PD number"); LOG_ERR("Invalid PD number");
return -1; 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"); LOG_WRN("PD not online");
return -1; return -1;
} }
@ -970,12 +976,12 @@ int osdp_cp_send_command(int pd, struct osdp_cmd *cmd)
return -1; return -1;
} }
p = osdp_cmd_alloc(TO_PD(ctx, pd)); p = osdp_cmd_alloc(osdp_to_pd(ctx, pd));
if (p == NULL) { if (p == NULL) {
return -1; return -1;
} }
memcpy(p, cmd, sizeof(struct osdp_cmd)); memcpy(p, cmd, sizeof(struct osdp_cmd));
p->id = cmd_id; /* translate to internal */ 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; return 0;
} }

24
subsys/mgmt/osdp/src/osdp_pd.c
View file

@ -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 * For CMD_KEYSET to be accepted, PD must be
* ONLINE and SC_ACTIVE. * ONLINE and SC_ACTIVE.
*/ */
if (ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE) == 0) { if (!sc_is_active(pd)) {
pd->reply_id = REPLY_NAK; pd->reply_id = REPLY_NAK;
pd->cmd_data[0] = OSDP_PD_NAK_SC_COND; pd->cmd_data[0] = OSDP_PD_NAK_SC_COND;
LOG_ERR("Keyset with SC inactive"); 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; break;
} }
osdp_sc_init(pd); osdp_sc_init(pd);
CLEAR_FLAG(pd, PD_FLAG_SC_ACTIVE); sc_deactivate(pd);
for (i = 0; i < 8; i++) { for (i = 0; i < 8; i++) {
pd->sc.cp_random[i] = buf[pos++]; 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; return -1;
} }
osdp_fill_random(pd->sc.pd_random, 8); 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); osdp_compute_pd_cryptogram(pd);
buf[len++] = pd->reply_id; buf[len++] = pd->reply_id;
for (i = 0; i < 8; i++) { 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 */ smb[1] = SCS_14; /* type */
if (osdp_verify_cp_cryptogram(pd) == 0) { if (osdp_verify_cp_cryptogram(pd) == 0) {
smb[2] = 1; /* CP auth succeeded */ 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)) { if (ISSET_FLAG(pd, PD_FLAG_SC_USE_SCBKD)) {
LOG_WRN("SC Active with SCBK-D"); LOG_WRN("SC Active with SCBK-D");
} else { } 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 #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[0] = 2; /* length */
smb[1] = (len > 1) ? SCS_18 : SCS_16; 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) void osdp_update(struct osdp *ctx)
{ {
int ret; int ret;
struct osdp_pd *pd = TO_PD(ctx, 0); struct osdp_pd *pd = osdp_to_pd(ctx, 0);
switch (pd->state) { switch (pd->state) {
case OSDP_PD_STATE_IDLE: case OSDP_PD_STATE_IDLE:
ret = pd_receve_packet(pd); ret = pd_receve_packet(pd);
if (ret == OSDP_PD_ERR_GENERIC || ((pd->rx_buf_len > 0 || if (ret == OSDP_PD_ERR_GENERIC ||
ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE)) && ((pd->rx_buf_len > 0 || sc_is_active(pd)) &&
osdp_millis_since(pd->tstamp) > OSDP_RESP_TOUT_MS)) { osdp_millis_since(pd->tstamp) > OSDP_RESP_TOUT_MS)) {
/** /**
* When we receive a command from PD after a timeout, * When we receive a command from PD after a timeout,
* any established secure channel must be discarded. * any established secure channel must be discarded.
@ -781,10 +781,10 @@ int osdp_setup(struct osdp *ctx, uint8_t *key)
ARG_UNUSED(key); ARG_UNUSED(key);
struct osdp_pd *pd; struct osdp_pd *pd;
if (ctx->cp->num_pd != 1) { if (NUM_PD(ctx) != 1) {
return -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); osdp_pd_set_attributes(pd, osdp_pd_cap, &osdp_pd_id);
SET_FLAG(pd, PD_FLAG_PD_MODE); SET_FLAG(pd, PD_FLAG_PD_MODE);
#ifdef CONFIG_OSDP_SC_ENABLED #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) int osdp_pd_get_cmd(struct osdp_cmd *cmd)
{ {
struct osdp_cmd *c; 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)) { if (osdp_cmd_dequeue(pd, &c)) {
return -1; return -1;

51
subsys/mgmt/osdp/src/osdp_phy.c
View file

@ -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 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; 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 */ exp_len = sizeof(struct osdp_packet_header) + 64; /* 64 is estimated */
if (max_len < exp_len) { if (max_len < exp_len) {
LOG_ERR("packet_init: out of space! CMD: %02x", pd->cmd_id); 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->mark = OSDP_PKT_MARK;
pkt->som = OSDP_PKT_SOM; pkt->som = OSDP_PKT_SOM;
pkt->pd_address = pd->address & 0x7F; /* Use only the lower 7 bits */ 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 */ /* PD must reply with MSB of it's address set */
pkt->pd_address |= 0x80; pkt->pd_address |= 0x80;
id = pd->reply_id; id = pd->reply_id;
} else { } else {
id = pd->cmd_id; 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; pkt->control |= PKT_CONTROL_CRC;
sb_len = 0; sb_len = 0;
if (ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE)) { if (sc_is_active(pd)) {
pkt->control |= PKT_CONTROL_SCB; pkt->control |= PKT_CONTROL_SCB;
pkt->data[0] = sb_len = 2; pkt->data[0] = sb_len = 2;
pkt->data[1] = SCS_15; 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->control |= PKT_CONTROL_SCB;
pkt->data[0] = sb_len = 3; pkt->data[0] = sb_len = 3;
pkt->data[1] = SCS_11; 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 #ifdef CONFIG_OSDP_SC_ENABLED
uint8_t *data; uint8_t *data;
int i, is_cmd, data_len; int i, data_len;
if (ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE) && if (sc_is_active(pd) &&
pkt->control & PKT_CONTROL_SCB && (pkt->control & PKT_CONTROL_SCB) && pkt->data[1] >= SCS_15)
pkt->data[1] >= SCS_15) {
is_cmd = !ISSET_FLAG(pd, PD_FLAG_PD_MODE);
if (pkt->data[1] == SCS_17 || pkt->data[1] == SCS_18) { if (pkt->data[1] == SCS_17 || pkt->data[1] == SCS_18) {
/** /**
* Only the data portion of message (after id byte) * 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. * SCS_15/SCS_16 and send them.
*/ */
data = pkt->data + pkt->data[0] + 1; data = pkt->data + pkt->data[0] + 1;
data_len = len - (sizeof(struct osdp_packet_header) data_len = len - (sizeof(struct osdp_packet_header) +
+ pkt->data[0] + 1); pkt->data[0] + 1);
len -= data_len; len -= data_len;
/** /**
* check if the passed buffer can hold the encrypted * 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 */ /* data_len + 1 for OSDP_SC_EOM_MARKER */
goto out_of_space_error; 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 */ /* len: with 4bytes MAC; with 2 byte CRC; without 1 byte mark */
if (len + 4 > max_len) { 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); pkt->len_msb = BYTE_1(len - 1 + 2 + 4);
/* compute and extend the buf with 4 MAC bytes */ /* compute and extend the buf with 4 MAC bytes */
osdp_compute_mac(pd, is_cmd, buf + 1, len - 1); osdp_compute_mac(pd, is_cp_mode(pd), buf + 1, len - 1);
data = is_cmd ? pd->sc.c_mac : pd->sc.r_mac; data = is_cp_mode(pd) ? pd->sc.c_mac : pd->sc.r_mac;
for (i = 0; i < 4; i++) { for (i = 0; i < 4; i++) {
buf[len + i] = data[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; uint8_t *data;
uint16_t comp, cur; 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; struct osdp_packet_header *pkt;
pd_mode = ISSET_FLAG(pd, PD_FLAG_PD_MODE);
pkt = (struct osdp_packet_header *)buf; pkt = (struct osdp_packet_header *)buf;
/* wait till we have the header */ /* 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; 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!"); LOG_ERR("Reply without address MSB set!");
return OSDP_ERR_PKT_FMT; 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; pd_addr = pkt->pd_address & 0x7F;
if (pd_addr != pd->address && pd_addr != 0x7F) { if (pd_addr != pd->address && pd_addr != 0x7F) {
/* not addressed to us and was not broadcasted */ /* not addressed to us and was not broadcasted */
if (!pd_mode) { if (is_cp_mode(pd)) {
LOG_ERR("Invalid pd address %d", pd_addr); LOG_ERR("Invalid pd address %d", pd_addr);
return OSDP_ERR_PKT_FMT; 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 */ /* validate sequence number */
cur = pkt->control & PKT_CONTROL_SQN; 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 * CP is trying to restart communication by sending a 0. The
* current PD implementation does not hold any state between * 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; pd->seq_number = -1;
CLEAR_FLAG(pd, PD_FLAG_SC_ACTIVE); 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 * TODO: PD must resend the last response if CP send the same
* sequence number again. * 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; pd->cmd_data[0] = OSDP_PD_NAK_SEQ_NUM;
return OSDP_ERR_PKT_FMT; 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)) { if (comp != cur && !ISSET_FLAG(pd, PD_FLAG_SKIP_SEQ_CHECK)) {
LOG_ERR("packet seq mismatch %d/%d", comp, cur); LOG_ERR("packet seq mismatch %d/%d", comp, cur);
pd->reply_id = REPLY_NAK; 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; int is_cmd;
if (pkt->control & PKT_CONTROL_SCB) { 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"); LOG_ERR("PD is not SC capable");
pd->reply_id = REPLY_NAK; pd->reply_id = REPLY_NAK;
pd->cmd_data[0] = OSDP_PD_NAK_SC_UNSUP; 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]; data = pkt->data + pkt->data[0];
len -= pkt->data[0]; /* consume security block */ len -= pkt->data[0]; /* consume security block */
} else { } else {
if (ISSET_FLAG(pd, PD_FLAG_SC_ACTIVE)) { if (sc_is_active(pd)) {
LOG_ERR("Received plain-text message in SC"); LOG_ERR("Received plain-text message in SC");
pd->reply_id = REPLY_NAK; pd->reply_id = REPLY_NAK;
pd->cmd_data[0] = OSDP_PD_NAK_SC_COND; 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) && if (sc_is_active(pd) &&
pkt->control & PKT_CONTROL_SCB && pkt->control & PKT_CONTROL_SCB && pkt->data[1] >= SCS_15) {
pkt->data[1] >= SCS_15) {
/* validate MAC */ /* validate MAC */
is_cmd = ISSET_FLAG(pd, PD_FLAG_PD_MODE); is_cmd = ISSET_FLAG(pd, PD_FLAG_PD_MODE);
osdp_compute_mac(pd, is_cmd, buf + 1, mac_offset); osdp_compute_mac(pd, is_cmd, buf + 1, mac_offset);

7
subsys/mgmt/osdp/src/osdp_sc.c
View file

@ -21,7 +21,7 @@ static const uint8_t osdp_scbk_default[16] = {
void osdp_compute_scbk(struct osdp_pd *pd, uint8_t *scbk) void osdp_compute_scbk(struct osdp_pd *pd, uint8_t *scbk)
{ {
int i; int i;
struct osdp *ctx = TO_CTX(pd); struct osdp *ctx = pd_to_osdp(pd);
memcpy(scbk, pd->sc.pd_client_uid, 8); memcpy(scbk, pd->sc.pd_client_uid, 8);
for (i = 8; i < 16; i++) { 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); 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; int i;
struct osdp_pd *pd = GET_CURRENT_PD(ctx);
if (ISSET_FLAG(pd, PD_FLAG_SC_USE_SCBKD)) { if (ISSET_FLAG(pd, PD_FLAG_SC_USE_SCBKD)) {
memcpy(pd->sc.scbk, osdp_scbk_default, 16); 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)) { if (ISSET_FLAG(pd, PD_FLAG_PD_MODE)) {
memcpy(pd->sc.scbk, key, 16); 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[0] = BYTE_0(pd->id.vendor_code);
pd->sc.pd_client_uid[1] = BYTE_1(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); pd->sc.pd_client_uid[2] = BYTE_0(pd->id.model);