csi_rpmb.c 6.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293
  1. #include <stdio.h>
  2. #include <stdlib.h>
  3. #include <assert.h>
  4. #include <string.h>
  5. #include <sys/ioctl.h>
  6. #include <sys/types.h>
  7. #include <sys/stat.h>
  8. #include <unistd.h>
  9. #include <fcntl.h>
  10. #include <errno.h>
  11. #include "3rdparty/hmac_sha/hmac_sha2.h"
  12. #include "mmc.h"
  13. #include "csi_rpmb.h"
  14. #ifndef offsetof
  15. #define offsetof(TYPE, MEMBER) ((size_t) &((TYPE *)0)->MEMBER)
  16. #endif
  17. struct rpmb_frame {
  18. u_int8_t stuff[196];
  19. u_int8_t key_mac[32];
  20. u_int8_t data[256];
  21. u_int8_t nonce[16];
  22. u_int32_t write_counter;
  23. u_int16_t addr;
  24. u_int16_t block_count;
  25. u_int16_t result;
  26. u_int16_t req_resp;
  27. };
  28. extern int rpmb_read_counter(int dev_fd, unsigned int *cnt);
  29. extern int do_rpmb_op(int fd, const struct rpmb_frame *frame_in,
  30. struct rpmb_frame *frame_out, unsigned int out_cnt);
  31. /**
  32. \brief Initialize rpmb interface.
  33. \param[in] ctx Context to operate
  34. \return Error code
  35. */
  36. hal_error_t csi_rpmb_init(csi_hal_rpmb_ctx_t *ctx, char *device)
  37. {
  38. int dev_fd;
  39. assert(device != NULL);
  40. dev_fd = open(device, O_RDWR);
  41. if (dev_fd < 0) {
  42. perror("device open");
  43. return CSI_HAL_ERROR;
  44. }
  45. ctx->device = device;
  46. ctx->dev_fd = dev_fd;
  47. return CSI_HAL_SUCCESS;
  48. }
  49. /**
  50. \brief De-initialize rpmb.
  51. \param[in] ctx Context to operate
  52. \return None
  53. */
  54. void csi_rpmb_uninit(csi_hal_rpmb_ctx_t *ctx)
  55. {
  56. close(ctx->dev_fd);
  57. }
  58. int _do_csi_rpmb_write_key(int dev_fd, uint8_t *key)
  59. {
  60. int ret;
  61. struct rpmb_frame frame_in = {
  62. .req_resp = htobe16(MMC_RPMB_WRITE_KEY)
  63. }, frame_out;
  64. /* Read the auth key */
  65. memcpy(frame_in.key_mac, key, sizeof(frame_in.key_mac));
  66. /* Execute RPMB op */
  67. ret = do_rpmb_op(dev_fd, &frame_in, &frame_out, 1);
  68. if (ret != 0) {
  69. perror("RPMB ioctl failed");
  70. exit(1);
  71. }
  72. /* Check RPMB response */
  73. if (frame_out.result != 0) {
  74. printf("RPMB operation failed, retcode 0x%04x\n",
  75. be16toh(frame_out.result));
  76. exit(1);
  77. }
  78. return ret;
  79. }
  80. int do_csi_rpmb_write_block(int dev_fd, enum hal_rpmb_op_type type, uint8_t *data, uint8_t *key_value, uint16_t addr, uint32_t blocks)
  81. {
  82. int ret;
  83. unsigned char key[32];
  84. unsigned int cnt;
  85. struct rpmb_frame frame_in = {
  86. .req_resp = htobe16(MMC_RPMB_WRITE),
  87. .block_count = htobe16(1)
  88. }, frame_out;
  89. assert(data != NULL && key_value != NULL && dev_fd >= 0);
  90. assert(type == MMC_RPMB_WRITE_KEY || type == MMC_RPMB_WRITE);
  91. if (type == MMC_RPMB_WRITE_KEY)
  92. return _do_csi_rpmb_write_key(dev_fd, key_value);
  93. /* Get key mac */
  94. memcpy(key, key_value, sizeof(key));
  95. while (blocks) {
  96. ret = rpmb_read_counter(dev_fd, &cnt);
  97. /* Check RPMB response */
  98. if (ret != 0) {
  99. printf("RPMB read counter operation failed, retcode 0x%04x\n", ret);
  100. exit(1);
  101. }
  102. frame_in.write_counter = htobe32(cnt);
  103. /* Get block address */
  104. frame_in.addr = htobe16(addr);
  105. /* Read 256b data */
  106. memcpy(frame_in.data, data, sizeof(frame_in.data));
  107. /* Calculate HMAC SHA256 */
  108. hmac_sha256(
  109. key, sizeof(key),
  110. frame_in.data, sizeof(frame_in) - offsetof(struct rpmb_frame, data),
  111. frame_in.key_mac, sizeof(frame_in.key_mac));
  112. /* Execute RPMB op */
  113. ret = do_rpmb_op(dev_fd, &frame_in, &frame_out, 1);
  114. if (ret != 0) {
  115. perror("RPMB ioctl failed");
  116. exit(1);
  117. }
  118. /* Check RPMB response */
  119. if (frame_out.result != 0) {
  120. printf("RPMB operation failed, retcode 0x%04x\n", be16toh(frame_out.result));
  121. exit(1);
  122. }
  123. addr += 1; /* half sector */
  124. blocks -= 1;
  125. data += sizeof(frame_in.data);
  126. memset(&frame_out, 0, sizeof(frame_out));
  127. memset(&frame_in, 0, sizeof(frame_in));
  128. frame_in.req_resp = htobe16(MMC_RPMB_WRITE);
  129. frame_in.block_count = htobe16(1);
  130. }
  131. return ret;
  132. }
  133. int do_csi_rpmb_read_block(int dev_fd, uint8_t *data, uint8_t *key_value, uint16_t addr, uint32_t blocks)
  134. {
  135. int i, ret;
  136. unsigned char mac[32];
  137. hmac_sha256_ctx ctx;
  138. struct rpmb_frame *frame_out = NULL;
  139. /*
  140. * for reading RPMB, number of blocks is set by CMD23 only, the packet
  141. * frame field for that is set to 0. So, the type is not u16 but uint!
  142. */
  143. unsigned int blocks_cnt;
  144. unsigned char key[32];
  145. struct rpmb_frame frame_in = {
  146. .req_resp = htobe16(MMC_RPMB_READ),
  147. }, *frame_out_p;
  148. /* Get key mac */
  149. memcpy(key, key_value, sizeof(key));
  150. /* Get block address */
  151. frame_in.addr = htobe16(addr);
  152. /* Get blocks count */
  153. blocks_cnt = blocks;
  154. if (!blocks_cnt) {
  155. printf("please, specify valid blocks count number\n");
  156. exit(1);
  157. }
  158. frame_out_p = calloc(sizeof(*frame_out_p), blocks_cnt);
  159. if (!frame_out_p) {
  160. printf("can't allocate memory for RPMB outer frames\n");
  161. exit(1);
  162. }
  163. /* Execute RPMB op */
  164. ret = do_rpmb_op(dev_fd, &frame_in, frame_out_p, blocks_cnt);
  165. if (ret != 0) {
  166. perror("RPMB ioctl failed");
  167. exit(1);
  168. }
  169. /* Check RPMB response */
  170. if (frame_out_p[blocks_cnt - 1].result != 0) {
  171. printf("RPMB operation failed, retcode 0x%04x\n",
  172. be16toh(frame_out_p[blocks_cnt - 1].result));
  173. exit(1);
  174. }
  175. /* Do we have to verify data against key? */
  176. hmac_sha256_init(&ctx, key, sizeof(key));
  177. for (i = 0; i < blocks_cnt; i++) {
  178. frame_out = &frame_out_p[i];
  179. hmac_sha256_update(&ctx, frame_out->data, sizeof(*frame_out) - offsetof(struct rpmb_frame, data));
  180. }
  181. hmac_sha256_final(&ctx, mac, sizeof(mac));
  182. /* Impossible */
  183. assert(frame_out);
  184. /* Compare calculated MAC and MAC from last frame */
  185. if (memcmp(mac, frame_out->key_mac, sizeof(mac))) {
  186. printf("RPMB MAC missmatch\n");
  187. exit(1);
  188. }
  189. /* Write data */
  190. for (i = 0; i < blocks_cnt; i++) {
  191. struct rpmb_frame *frame_out = &frame_out_p[i];
  192. memcpy(data, frame_out->data, sizeof(frame_out->data));
  193. data += sizeof(frame_out->data);
  194. }
  195. free(frame_out_p);
  196. return ret;
  197. }
  198. /**
  199. \brief RPMB write data, authenticated data and write to RPMB.
  200. \param[in] ctx Context to operate
  201. \param[in] addr [in] address, address in block.
  202. \param[in] blocks [in] write block number. block size 256 bytes.
  203. \param[in] data [in]
  204. \return Error code
  205. */
  206. hal_error_t csi_rpmb_write_block(csi_hal_rpmb_ctx_t *ctx, uint16_t addr,
  207. uint32_t blocks, uint8_t *data)
  208. {
  209. int ret, dev_fd;
  210. uint8_t *key_value;
  211. enum hal_rpmb_op_type type;
  212. assert(ctx != NULL && data != NULL);
  213. dev_fd = ctx->dev_fd;
  214. key_value = ctx->key_mac;
  215. type = ctx->rpmb_op_type;
  216. ret = do_csi_rpmb_write_block(dev_fd, type, data, key_value, addr, blocks);
  217. if (ret)
  218. return CSI_HAL_ERROR;
  219. else
  220. return CSI_HAL_SUCCESS;
  221. }
  222. /**
  223. \brief RPMB read data, check .authentication tag and return data.
  224. \param[in] ctx Context to operate
  225. \param[in] addr [in] address
  226. \param[in] blocks [in] write block number.
  227. \return Error code
  228. */
  229. hal_error_t csi_rpmb_read_block(csi_hal_rpmb_ctx_t *ctx, uint16_t addr,
  230. uint32_t blocks, uint8_t *data)
  231. {
  232. int ret, dev_fd;
  233. uint8_t *key_value;
  234. assert(ctx != NULL && data != NULL);
  235. dev_fd = ctx->dev_fd;
  236. key_value = ctx->key_mac;
  237. ret = do_csi_rpmb_read_block(dev_fd, data, key_value, addr, blocks);
  238. if (ret)
  239. return CSI_HAL_ERROR;
  240. else
  241. return CSI_HAL_SUCCESS;
  242. }