diff options
Diffstat (limited to 'src/rcm.c')
-rw-r--r-- | src/rcm.c | 145 |
1 files changed, 133 insertions, 12 deletions
@@ -34,6 +34,8 @@ #include "aes-cmac.h" static int rcm_sign_msg(uint8_t *buf); +static int rcm1_sign_msg(uint8_t *buf); +static int rcm35_sign_msg(uint8_t *buf); static void rcm_init_msg( uint8_t *buf, uint32_t msg_len, @@ -41,14 +43,53 @@ static void rcm_init_msg( void *args, uint32_t args_len, uint32_t payload_len); +static void rcm1_init_msg( + uint8_t *buf, + uint32_t msg_len, + uint32_t opcode, + void *args, + uint32_t args_len, + uint32_t payload_len); +static void rcm35_init_msg( + uint8_t *buf, + uint32_t msg_len, + uint32_t opcode, + void *args, + uint32_t args_len, + uint32_t payload_len); static uint8_t *rcm_get_msg_payload(uint8_t *buf); static void rcm_msg_pad(uint8_t *data, uint32_t len); static uint32_t rcm_get_pad_len(uint32_t payload_len); static uint32_t rcm_get_msg_buf_len(uint32_t payload_len); +static uint32_t rcm_version = 0; +static uint32_t message_size = 0; + +int rcm_init(uint32_t version) +{ + int ret = -EINVAL; + + if (version == RCM_VERSION_1) { + rcm_version = version; + message_size = sizeof(rcm1_msg_t); + ret = 0; + } + else if (version == RCM_VERSION_35) { + rcm_version = version; + message_size = sizeof(rcm35_msg_t); + ret = 0; + } + return ret; +} + uint32_t rcm_get_msg_len(uint8_t *msg) { - return ((rcm_msg_t*)msg)->len_insecure; + if (rcm_version == RCM_VERSION_1) + return ((rcm1_msg_t*)msg)->len_insecure; + else if (rcm_version == RCM_VERSION_35) + return ((rcm35_msg_t*)msg)->len_insecure; + else + return 0; } int rcm_create_msg( @@ -96,10 +137,20 @@ done: static int rcm_sign_msg(uint8_t *buf) { - rcm_msg_t *msg; + if (rcm_version == RCM_VERSION_35) + return rcm35_sign_msg(buf); + else if (rcm_version == RCM_VERSION_1) + return rcm1_sign_msg(buf); + else + return -EINVAL; +} + +static int rcm1_sign_msg(uint8_t *buf) +{ + rcm1_msg_t *msg; uint32_t crypto_len; - msg = (rcm_msg_t*)buf; + msg = (rcm1_msg_t*)buf; // signing does not include the len_insecure and // cmac_hash fields at the beginning of the message. @@ -113,15 +164,35 @@ static int rcm_sign_msg(uint8_t *buf) return 0; } +static int rcm35_sign_msg(uint8_t *buf) +{ + rcm35_msg_t *msg; + uint32_t crypto_len; + + msg = (rcm35_msg_t*)buf; + + // signing does not include the len_insecure, modulus + // and object signature at the beginning of the message + crypto_len = msg->len_insecure - sizeof(msg->len_insecure) - + sizeof(msg->modulus) - + sizeof(msg->object_sig); + if (crypto_len % RCM_AES_BLOCK_SIZE) { + return -EMSGSIZE; + } + + cmac_hash(msg->reserved, crypto_len, msg->object_sig.cmac_hash); + return 0; +} + static uint32_t rcm_get_msg_buf_len(uint32_t payload_len) { - return sizeof(rcm_msg_t) + payload_len + + return message_size + payload_len + rcm_get_pad_len(payload_len); } static uint8_t *rcm_get_msg_payload(uint8_t *buf) { - return buf + sizeof(rcm_msg_t); + return buf + message_size; } static void rcm_msg_pad(uint8_t *data, uint32_t len) @@ -141,14 +212,64 @@ static void rcm_init_msg( uint32_t args_len, uint32_t payload_len) { + if (rcm_version == RCM_VERSION_35) + rcm35_init_msg(buf, msg_len, opcode, args, + args_len, payload_len); + else if (rcm_version == RCM_VERSION_1) + rcm1_init_msg(buf, msg_len, opcode, args, + args_len, payload_len); +} + +static void rcm35_init_msg( + uint8_t *buf, + uint32_t msg_len, + uint32_t opcode, + void *args, + uint32_t args_len, + uint32_t payload_len) +{ + uint32_t padding_len; + rcm35_msg_t *msg; + + msg = (rcm35_msg_t *)buf; + + padding_len = rcm_get_pad_len(payload_len); + + msg->len_insecure = sizeof(rcm35_msg_t) + payload_len + + padding_len; + + memset(&msg->object_sig.cmac_hash, 0x0, sizeof(msg->object_sig.cmac_hash)); + memset(&msg->reserved, 0x0, sizeof(msg->reserved)); + + msg->opcode = opcode; + msg->len_secure = msg->len_insecure; + msg->payload_len = payload_len; + msg->rcm_version = RCM_VERSION_35; + + if (args_len) + memcpy(msg->args, args, args_len); + memset(msg->args + args_len, 0x0, sizeof(msg->args) - args_len); + + rcm_msg_pad(msg->padding, sizeof(msg->padding)); + rcm_msg_pad(buf + sizeof(rcm35_msg_t) + payload_len, padding_len); +} + +static void rcm1_init_msg( + uint8_t *buf, + uint32_t msg_len, + uint32_t opcode, + void *args, + uint32_t args_len, + uint32_t payload_len) +{ uint32_t padding_len; - rcm_msg_t *msg; + rcm1_msg_t *msg; - msg = (rcm_msg_t *)buf; + msg = (rcm1_msg_t *)buf; padding_len = rcm_get_pad_len(payload_len); - msg->len_insecure = sizeof(rcm_msg_t) + payload_len + + msg->len_insecure = sizeof(rcm1_msg_t) + payload_len + padding_len; memset(&msg->cmac_hash, 0x0, sizeof(msg->cmac_hash)); @@ -157,20 +278,20 @@ static void rcm_init_msg( msg->opcode = opcode; msg->len_secure = msg->len_insecure; msg->payload_len = payload_len; - msg->rcm_version = RCM_VERSION; + msg->rcm_version = RCM_VERSION_1; if (args_len) memcpy(msg->args, args, args_len); memset(msg->args + args_len, 0x0, sizeof(msg->args) - args_len); rcm_msg_pad(msg->padding, sizeof(msg->padding)); - rcm_msg_pad(buf + sizeof(rcm_msg_t) + payload_len, padding_len); + rcm_msg_pad(buf + sizeof(rcm1_msg_t) + payload_len, padding_len); } static uint32_t rcm_get_pad_len(uint32_t payload_len) { uint32_t pad_len = 0; - uint32_t msg_len = sizeof(rcm_msg_t) + payload_len; + uint32_t msg_len = message_size + payload_len; // First, use padding to bump the message size up to the minimum. if (msg_len < RCM_MIN_MSG_LENGTH) { @@ -184,7 +305,7 @@ static uint32_t rcm_get_pad_len(uint32_t payload_len) * rcm_msg_t size handles the initial data that is not part of * the hashing and encryption. */ - pad_len += 16 - ((msg_len - sizeof(rcm_msg_t)) & 0xf); + pad_len += 16 - ((msg_len - message_size) & 0xf); return pad_len; } |