| /* |
| * drivers/storagekey/normal_key.c |
| * |
| * Copyright (C) 2015 Amlogic, Inc. All rights reserved. |
| * |
| * This program is free software; you can redistribute it and/or modify |
| * it under the terms of the GNU General Public License as published by |
| * the Free Software Foundation; either version 2 of the License, or |
| * (at your option) any later version. |
| * |
| * This program is distributed in the hope that it will be useful, but WITHOUT |
| * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or |
| * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for |
| * more details. |
| * |
| * You should have received a copy of the GNU General Public License along |
| * with this program; if not, write to the Free Software Foundation, Inc., |
| * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. |
| */ |
| |
| #include <common.h> |
| #include <linux/types.h> |
| #include <linux/list.h> |
| #include <u-boot/sha256.h> |
| #include <malloc.h> |
| #include "normal_key.h" |
| |
| #undef pr_info |
| #define pr_info(fmt ...) printf("[KM]Msg:"fmt) |
| #undef pr_err |
| #define pr_err(fmt ...) printf("[KM]Error:f[%s]L%d:", __func__, __LINE__),printf(fmt) |
| |
| #define DBG 0 |
| |
| /* Storage BLOCK RAW HEAD: fixed 512B*/ |
| #define ENC_TYPE_DEFAULT 0 |
| #define ENC_TYPE_EFUSE 1 |
| #define ENC_TYPE_FIXED 2 |
| |
| #define STORAGE_BLOCK_RAW_HEAD_SIZE 512 |
| |
| #define BLOCK_VERSION_0 0 |
| |
| #define ERR_HEADER 0x1 |
| #define ERR_KEYMEMFAIL 0x2 |
| #define ERR_KEYRDFAIL 0x4 |
| #define ERR_KEYCHKFAIL 0x8 |
| #define ERR_ENCHDFAIL 0x10 |
| #define ERR_DATASZ 0x20 |
| struct storage_block_raw_head { |
| u8 mark[16]; /* AMLNORMAL*/ |
| u32 version; |
| u32 enctype; /*from EFUSE, from default, from fixed*/ |
| u32 keycnt; |
| u32 initcnt; |
| u32 wrtcnt; |
| u32 errcnt; |
| u32 flags; |
| u8 headhash[32]; |
| u8 hash[32]; |
| }; |
| |
| /* Storage BLOCK ENC HEAD: fixed 512B*/ |
| #define STORAGE_BLOCK_ENC_HEAD_SIZE 512 |
| struct storage_block_enc_head { |
| u32 blocksize; |
| u32 flashsize; |
| }; |
| |
| /* Storage Format: TLV*/ |
| enum emTLVTag { |
| EMTLVNONE, |
| |
| EMTLVHEAD, |
| EMTLVHEADSIZE, |
| |
| EMTLVOBJECT, |
| EMTLVOBJNAMESIZE, |
| EMTLVOBJNAME, |
| EMTLVOBJDATASIZE, |
| EMTLVOBJDATABUF, |
| EMTLVOBJTYPE, |
| EMTLVOBJATTR, |
| EMTLVOBJHASHBUF, |
| |
| EMTLVHEADFLASHSIZE, |
| }; |
| |
| struct storage_node { |
| struct list_head node; |
| struct storage_object object; |
| }; |
| |
| static LIST_HEAD(keys); |
| static int blockinited; |
| static struct storage_block_raw_head rawhead; |
| static struct storage_block_enc_head enchead; |
| static char *blockmark = "AMLNORMAL"; |
| |
| #if DBG |
| static void dump_raw_head(struct storage_block_raw_head *prawhead) |
| { |
| pr_info("rawhead:\n"); |
| pr_info("mark: %s\n", prawhead->mark); |
| pr_info("keycnt: %u\n", prawhead->keycnt); |
| pr_info("initcnt: %u\n", prawhead->initcnt); |
| pr_info("wrtcnt: %u\n", prawhead->wrtcnt); |
| pr_info("errcnt: %u\n", prawhead->errcnt); |
| pr_info("flags: 0x%x\n", prawhead->flags); |
| } |
| |
| static void dump_enc_head(struct storage_block_enc_head *penchead) |
| { |
| pr_info("enchead:\n"); |
| pr_info("blocksize: %u\n", penchead->blocksize); |
| pr_info("flashsize: %u\n", penchead->flashsize); |
| } |
| |
| static void dump_mem(const u8 *p, int len) |
| { |
| int idx = 0, j, tmp; |
| char buf[64]; |
| int total; |
| |
| while (idx < len) { |
| total = 0; |
| tmp = min(((int)len - idx), 16); |
| for (j = 0; j < tmp; j++) |
| total += snprintf(buf + total, 64 - total, |
| "%02x ", p[idx + j]); |
| buf[total] = 0; |
| pr_info("%s\n", buf); |
| idx += tmp; |
| } |
| } |
| |
| static void dump_object(struct storage_object *obj) |
| { |
| pr_info("key: [%u, %.*s, %x, %x, %u]\n", |
| obj->namesize, obj->namesize, obj->name, |
| obj->type, obj->attribute, obj->datasize); |
| if (obj->dataptr) { |
| pr_info("data:\n"); |
| dump_mem(obj->dataptr, obj->datasize); |
| } |
| } |
| #endif |
| |
| static u32 Tlv_WriteUint32(u8 *output, s32 len, |
| u32 tag, u32 value) |
| { |
| u32 *out = (u32 *)output; |
| |
| if (len < 12) |
| return 0; |
| |
| out[0] = tag; |
| out[1] = 4; |
| out[2] = value; |
| return 12; |
| } |
| |
| static u32 Tlv_WriteBuf(u8 *output, s32 len, |
| u32 tag, |
| u32 length, u8 *input) |
| { |
| u8 *out = output; |
| u32 tmplen = (((length + 3) / 4) * 4); |
| |
| if (len < (s32)(8 + tmplen)) |
| return 0; |
| |
| *((u32 *)out) = tag; |
| *((u32 *)(out + 4)) = tmplen; |
| memset(out + 8, 0, tmplen); |
| memcpy(out + 8, input, length); |
| |
| return tmplen + 8; |
| } |
| |
| static u32 Tlv_ReadTl(u8 *input, int32_t len, |
| u32 *tag, u32 *length, |
| u32 *idx) |
| { |
| if (len < 8) |
| return 0; |
| |
| *tag = *((u32 *)input); |
| *length = *((u32 *)(input + 4)); |
| |
| if ((8 + *length) > len) |
| return 0; |
| *idx += 8; |
| |
| return 8; |
| } |
| |
| static u32 Tlv_ReadHead(u8 *input, int32_t len, |
| struct storage_block_enc_head *pblockhead) |
| { |
| u32 tag; |
| u32 sum; |
| u32 length; |
| u32 idx = 0; |
| u32 ret; |
| |
| ret = Tlv_ReadTl(input, len, |
| &tag, &sum, &idx); |
| if (!ret) |
| return 0; |
| |
| if (tag != EMTLVHEAD) |
| return 0; |
| |
| sum += ret; |
| while (idx < sum) { |
| ret = Tlv_ReadTl(input + idx, len - idx, |
| &tag, &length, &idx); |
| if (!ret) |
| return 0; |
| |
| switch (tag) { |
| case EMTLVHEADSIZE: |
| pblockhead->blocksize = *((u32 *)(input + idx)); |
| break; |
| case EMTLVHEADFLASHSIZE: |
| pblockhead->flashsize = *((u32 *)(input + idx)); |
| break; |
| default: |
| break; |
| } |
| idx += length; |
| } |
| return sum; |
| } |
| |
| static u32 Tlv_ReadObject(u8 *input, int32_t len, |
| struct storage_object *pcontent) |
| { |
| u32 tag; |
| u32 length; |
| u32 sum; |
| u32 idx = 0; |
| u32 ret; |
| |
| memset(pcontent, 0, sizeof(*pcontent)); |
| ret = Tlv_ReadTl(input, len, |
| &tag, &sum, &idx); |
| if (!ret) |
| return 0; |
| |
| if (tag != EMTLVOBJECT) |
| return 0; |
| |
| sum += ret; |
| while (idx < sum) { |
| ret = Tlv_ReadTl(input + idx, len - idx, |
| &tag, &length, &idx); |
| if (!ret) |
| goto tlv_readkeycontent_err; |
| |
| switch (tag) { |
| case EMTLVOBJNAMESIZE: |
| pcontent->namesize = *((u32 *)(input + idx)); |
| break; |
| case EMTLVOBJNAME: |
| memset(pcontent->name, 0, MAX_OBJ_NAME_LEN); |
| memcpy(pcontent->name, input + idx, pcontent->namesize); |
| break; |
| case EMTLVOBJTYPE: |
| pcontent->type = *((u32 *)(input + idx)); |
| break; |
| case EMTLVOBJATTR: |
| pcontent->attribute = *((u32 *)(input + idx)); |
| break; |
| case EMTLVOBJDATASIZE: |
| pcontent->datasize = *((u32 *)(input + idx)); |
| break; |
| case EMTLVOBJHASHBUF: |
| if (length != 32) |
| goto tlv_readkeycontent_err; |
| memcpy(pcontent->hashptr, input + idx, length); |
| break; |
| case EMTLVOBJDATABUF: |
| pcontent->dataptr = malloc(pcontent->datasize); |
| if (!pcontent->dataptr) |
| goto tlv_readkeycontent_err; |
| memcpy(pcontent->dataptr, |
| input + idx, pcontent->datasize); |
| break; |
| default: |
| break; |
| } |
| idx += length; |
| } |
| return sum; |
| |
| tlv_readkeycontent_err: |
| free(pcontent->dataptr); |
| return 0; |
| } |
| |
| #define WRT_UINT32(tag, field) \ |
| ({ \ |
| u32 __tmp; \ |
| __tmp = Tlv_WriteUint32(output + idx, len - idx, \ |
| tag, field); \ |
| if (__tmp) \ |
| idx += __tmp; \ |
| __tmp; \ |
| }) |
| |
| #define WRT_BUF(tag, buflen, buf) \ |
| ({ \ |
| u32 __tmp; \ |
| __tmp = Tlv_WriteBuf(output + idx, len - idx, \ |
| tag, buflen, buf); \ |
| if (__tmp) \ |
| idx += __tmp; \ |
| __tmp; \ |
| }) |
| |
| u32 Tlv_WriteHead(struct storage_block_enc_head *enchead, |
| u8 *output, int32_t len) |
| { |
| u32 *sum; |
| u32 idx = 0; |
| |
| if (len < 8) |
| return 0; |
| |
| *(u32 *)output = EMTLVHEAD; |
| sum = (u32 *)(output + 4); |
| idx += 8; |
| |
| if (!WRT_UINT32(EMTLVHEADSIZE, enchead->blocksize)) |
| return 0; |
| if (!WRT_UINT32(EMTLVHEADFLASHSIZE, enchead->flashsize)) |
| return 0; |
| |
| *sum = idx - 8; |
| return idx; |
| } |
| |
| u32 Tlv_WriteObject(struct storage_object *object, |
| u8 *output, int32_t len) |
| { |
| u32 *sum; |
| u32 idx = 0; |
| |
| if (len < 8) |
| return 0; |
| |
| *(u32 *)output = EMTLVOBJECT; |
| sum = (u32 *)(output + 4); |
| idx += 8; |
| |
| if (object->namesize != 0) { |
| if (!WRT_UINT32(EMTLVOBJNAMESIZE, object->namesize)) |
| return 0; |
| if (!WRT_BUF(EMTLVOBJNAME, object->namesize, |
| (u8 *)object->name)) |
| return 0; |
| } |
| |
| if (object->dataptr && object->datasize != 0) { |
| if (!WRT_UINT32(EMTLVOBJDATASIZE, object->datasize)) |
| return 0; |
| if (!WRT_BUF(EMTLVOBJDATABUF, object->datasize, |
| object->dataptr)) |
| return 0; |
| } |
| |
| if (!WRT_BUF(EMTLVOBJHASHBUF, 32, object->hashptr)) |
| return 0; |
| if (!WRT_UINT32(EMTLVOBJTYPE, object->type)) |
| return 0; |
| if (!WRT_UINT32(EMTLVOBJATTR, object->attribute)) |
| return 0; |
| |
| *sum = idx - 8; |
| return idx; |
| } |
| |
| static int normalkey_hash(u8 *data, u32 len, u8 *hash) |
| { |
| sha256_context ctx; |
| sha256_starts(&ctx); |
| sha256_update(&ctx, data, len); |
| sha256_finish(&ctx, hash); |
| return 0; |
| } |
| |
| int normalkey_init(void) |
| { |
| if (blockinited) |
| return 0; |
| |
| blockinited = 1; |
| return 0; |
| } |
| |
| void normalkey_deinit(void) |
| { |
| struct storage_node *pos, *n; |
| |
| if (!blockinited) |
| return; |
| |
| blockinited = 0; |
| |
| list_for_each_entry_safe(pos, n, &keys, node) { |
| list_del(&pos->node); |
| free(pos->object.dataptr); |
| free(pos); |
| } |
| } |
| |
| struct storage_object *normalkey_get(const u8 *name) |
| { |
| struct storage_node *pos; |
| struct storage_object *obj; |
| u32 len; |
| |
| if (!name) |
| return NULL; |
| |
| len = strlen((const char*)name); |
| list_for_each_entry(pos, &keys, node) { |
| obj = &pos->object; |
| if (len == obj->namesize && |
| !memcmp(name, obj->name, len)) |
| return obj; |
| } |
| |
| return NULL; |
| } |
| |
| int normalkey_add(const u8 *name, u8 *buffer, u32 len, u32 attr) |
| { |
| struct storage_object *obj; |
| struct storage_node *node; |
| u32 namelen; |
| u8 *data; |
| |
| if (blockinited != 2) |
| return -1; |
| |
| if (!name || !buffer || !len || (attr & OBJ_ATTR_SECURE)) |
| return -1; |
| |
| namelen = strlen((const char*)name); |
| if (namelen > MAX_OBJ_NAME_LEN) |
| return -1; |
| |
| obj = normalkey_get(name); |
| if (obj) { |
| if (attr != obj->attribute) |
| return -1; |
| if (len > obj->datasize) { |
| data = malloc(len); |
| if (!data) |
| return -1; |
| free(obj->dataptr); |
| obj->dataptr = data; |
| } |
| } else { |
| node = malloc(sizeof(*node)); |
| if (!node) |
| return -1; |
| data = malloc(len); |
| if (!data) { |
| free(node); |
| return -1; |
| } |
| obj = &node->object; |
| memcpy(obj->name, name, namelen); |
| obj->namesize = namelen; |
| obj->attribute = attr; |
| obj->type = OBJ_TYPE_GENERIC; |
| obj->dataptr = data; |
| list_add(&node->node, &keys); |
| } |
| obj->datasize = len; |
| memcpy(obj->dataptr, buffer, len); |
| normalkey_hash(buffer, len, obj->hashptr); |
| return 0; |
| } |
| |
| int normalkey_del(const u8 *name) |
| { |
| struct storage_object *obj; |
| struct storage_node *node; |
| |
| if (blockinited != 2) |
| return -1; |
| |
| obj = normalkey_get(name); |
| if (!obj) |
| return -1; |
| |
| node = container_of(obj, struct storage_node, object); |
| list_del(&node->node); |
| free(obj->dataptr); |
| free(node); |
| |
| return 0; |
| } |
| |
| int normalkey_readfromblock(void *block, unsigned long size) |
| { |
| struct storage_block_raw_head *prawhead; |
| u8 *penchead, *pdata; |
| struct storage_node *node = NULL; |
| u8 hash[32]; |
| u32 idx; |
| u32 ret; |
| |
| if (blockinited != 1) |
| return -1; |
| |
| prawhead = (struct storage_block_raw_head *)block; |
| penchead = (u8 *)block + STORAGE_BLOCK_RAW_HEAD_SIZE; |
| pdata = penchead + STORAGE_BLOCK_ENC_HEAD_SIZE; |
| |
| if (!block || size <= |
| (STORAGE_BLOCK_ENC_HEAD_SIZE + STORAGE_BLOCK_RAW_HEAD_SIZE)) |
| return -1; |
| |
| blockinited = 2; |
| |
| memset(&rawhead, 0, sizeof(rawhead)); |
| strncpy((char*)rawhead.mark, blockmark, 15); |
| rawhead.version = BLOCK_VERSION_0; |
| |
| enchead.flashsize = size; |
| if (strcmp((const char *)prawhead->mark, blockmark) != 0) { |
| pr_info("mark is not found\n"); |
| return 0; |
| } |
| |
| normalkey_hash((u8 *)prawhead, sizeof(*prawhead) - 64, |
| rawhead.headhash); |
| if (memcmp(rawhead.headhash, prawhead->headhash, 32)) { |
| pr_info("rawhead hash check fail\n"); |
| rawhead.flags |= ERR_HEADER; |
| } else { |
| pr_info("rawhead hash check successful\n"); |
| rawhead.keycnt = prawhead->keycnt; |
| rawhead.initcnt = prawhead->initcnt; |
| rawhead.wrtcnt = prawhead->wrtcnt; |
| rawhead.errcnt = prawhead->errcnt; |
| rawhead.flags = prawhead->flags; |
| } |
| |
| rawhead.initcnt++; |
| |
| #if DBG |
| dump_raw_head(&rawhead); |
| #endif |
| |
| normalkey_hash(penchead, size - STORAGE_BLOCK_RAW_HEAD_SIZE, |
| rawhead.hash); |
| if (memcmp(rawhead.hash, prawhead->hash, 32)) { |
| pr_info("data hash check fail\n"); |
| rawhead.errcnt++; |
| return 0; |
| } |
| |
| ret = Tlv_ReadHead(penchead, STORAGE_BLOCK_ENC_HEAD_SIZE, |
| &enchead); |
| if (!ret) { |
| pr_info("read head fail\n"); |
| rawhead.flags |= ERR_ENCHDFAIL; |
| return 0; |
| } |
| |
| #if DBG |
| dump_enc_head(&enchead); |
| #endif |
| |
| if (size < (enchead.blocksize + STORAGE_BLOCK_ENC_HEAD_SIZE + |
| STORAGE_BLOCK_RAW_HEAD_SIZE)) { |
| rawhead.flags |= ERR_DATASZ; |
| return 0; |
| } |
| |
| idx = 0; |
| while (idx < enchead.blocksize) { |
| struct storage_object *obj = NULL; |
| |
| if (!node) { |
| node = malloc(sizeof(*node)); |
| if (!node) { |
| rawhead.flags |= ERR_KEYMEMFAIL; |
| break; |
| } |
| } |
| obj = &node->object; |
| ret = Tlv_ReadObject(pdata + idx, |
| enchead.blocksize - idx, obj); |
| if (!ret) { |
| rawhead.flags |= ERR_KEYRDFAIL; |
| break; |
| } |
| idx += ret; |
| |
| normalkey_hash(obj->dataptr, obj->datasize, hash); |
| if (memcmp(hash, obj->hashptr, 32)) { |
| free(obj->dataptr); |
| rawhead.flags |= ERR_KEYCHKFAIL; |
| continue; |
| } |
| #if DBG |
| dump_object(obj); |
| #endif |
| list_add(&node->node, &keys); |
| node = NULL; |
| } |
| |
| free(node); |
| return 0; |
| } |
| |
| int normalkey_writetoblock(void *block, unsigned long size) |
| { |
| u8 *prawhead; |
| u8 *penchead, *pdata; |
| struct storage_object *obj = NULL; |
| struct storage_node *node = NULL; |
| u32 idx; |
| u32 ret; |
| |
| if (blockinited != 2) |
| return -1; |
| |
| prawhead = (u8 *)block; |
| penchead = prawhead + STORAGE_BLOCK_RAW_HEAD_SIZE; |
| pdata = penchead + STORAGE_BLOCK_ENC_HEAD_SIZE; |
| |
| if (!block || size <= |
| (STORAGE_BLOCK_ENC_HEAD_SIZE + STORAGE_BLOCK_RAW_HEAD_SIZE)) |
| return -1; |
| |
| enchead.flashsize = size; |
| size -= (STORAGE_BLOCK_ENC_HEAD_SIZE + STORAGE_BLOCK_RAW_HEAD_SIZE); |
| idx = 0; |
| rawhead.keycnt = 0; |
| list_for_each_entry(node, &keys, node) { |
| obj = &node->object; |
| ret = Tlv_WriteObject(obj, pdata + idx, size - idx); |
| if (!ret) |
| return -1; |
| idx += ret; |
| rawhead.keycnt++; |
| } |
| enchead.blocksize = idx; |
| |
| ret = Tlv_WriteHead(&enchead, penchead, STORAGE_BLOCK_ENC_HEAD_SIZE); |
| if (!ret) |
| return -1; |
| |
| rawhead.wrtcnt++; |
| normalkey_hash((u8 *)&rawhead, sizeof(rawhead) - 64, |
| rawhead.headhash); |
| normalkey_hash(penchead, |
| enchead.flashsize - STORAGE_BLOCK_RAW_HEAD_SIZE, |
| rawhead.hash); |
| memcpy(prawhead, &rawhead, sizeof(rawhead)); |
| |
| return 0; |
| } |