board/gdsys/a38x/keyprogram.c

   1 /*
   2  * (C) Copyright 2016
   3  * Mario Six, Guntermann & Drunck GmbH, mario.six@gdsys.cc
   4  *
   5  * SPDX-License-Identifier:    GPL-2.0+
   6  */
   7
   8 #include <common.h>
   9 #include <tpm.h>
  10 #include <malloc.h>
  11 #include <linux/ctype.h>
  12 #include <asm/unaligned.h>
  13
  14 #include "hre.h"
  15
  16 int flush_keys(void)
  17 {
  18         u16 key_count;
  19         u8 buf[288];
  20         u8 *ptr;
  21         u32 err;
  22         uint i;
  23
  24         /* fetch list of already loaded keys in the TPM */
  25         err = tpm_get_capability(TPM_CAP_HANDLE, TPM_RT_KEY, buf, sizeof(buf));
  26         if (err)
  27                 return -1;
  28         key_count = get_unaligned_be16(buf);
  29         ptr = buf + 2;
  30         for (i = 0; i < key_count; ++i, ptr += 4) {
  31                 err = tpm_flush_specific(get_unaligned_be32(ptr), TPM_RT_KEY);
  32                 if (err && err != TPM_KEY_OWNER_CONTROL)
  33                         return err;
  34         }
  35
  36         return 0;
  37 }
  38
  39 int decode_hexstr(char *hexstr, u8 **result)
  40 {
  41         int len = strlen(hexstr);
  42         int bytes = len / 2;
  43         int i;
  44         u8 acc = 0;
  45
  46         if (len % 2 == 1)
  47                 return 1;
  48
  49         *result = (u8 *)malloc(bytes);
  50
  51         for (i = 0; i < len; i++) {
  52                 char cur = tolower(hexstr[i]);
  53                 u8 val;
  54
  55                 if ((cur >= 'a' && cur <= 'f') || (cur >= '0' && cur <= '9')) {
  56                         val = cur - (cur > '9' ? 87 : 48);
  57
  58                         if (i % 2 == 0)
  59                                 acc = 16 * val;
  60                         else
  61                                 (*result)[i / 2] = acc + val;
  62                 } else {
  63                         free(*result);
  64                         return 1;
  65                 }
  66         }
  67
  68         return 0;
  69 }
  70
  71 int extract_subprogram(u8 **progdata, u32 expected_magic,
  72                        struct key_program **result)
  73 {
  74         struct key_program *prog = *result;
  75         u32 magic, code_crc, code_size;
  76
  77         magic = get_unaligned_be32(*progdata);
  78         code_crc = get_unaligned_be32(*progdata + 4);
  79         code_size = get_unaligned_be32(*progdata + 8);
  80
  81         *progdata += 12;
  82
  83         if (magic != expected_magic)
  84                 return -1;
  85
  86         *result = malloc(sizeof(struct key_program) + code_size);
  87
  88         if (!*result)
  89                 return -1;
  90
  91         prog->magic = magic;
  92         prog->code_crc = code_crc;
  93         prog->code_size = code_size;
  94         memcpy(prog->code, *progdata, code_size);
  95
  96         *progdata += code_size;
  97
  98         if (hre_verify_program(prog)) {
  99                 free(prog);
 100                 return -1;
 101         }
 102
 103         return 0;
 104 }
 105
 106 struct key_program *parse_and_check_keyprog(u8 *progdata)
 107 {
 108         struct key_program *result = NULL, *hmac = NULL;
 109
 110         /* Part 1: Load key program */
 111
 112         if (extract_subprogram(&progdata, MAGIC_KEY_PROGRAM, &result))
 113                 return NULL;
 114
 115         /* Part 2: Load hmac program */
 116
 117         if (extract_subprogram(&progdata, MAGIC_HMAC, &hmac))
 118                 return NULL;
 119
 120         free(hmac);
 121
 122         return result;
 123 }
 124
 125 int load_and_run_keyprog(void)
 126 {
 127         char *cmd = NULL;
 128         u8 *binprog = NULL;
 129         char *hexprog;
 130         struct key_program *prog;
 131
 132         cmd = env_get("loadkeyprogram");
 133
 134         if (!cmd || run_command(cmd, 0))
 135                 return 1;
 136
 137         hexprog = env_get("keyprogram");
 138
 139         if (decode_hexstr(hexprog, &binprog))
 140                 return 1;
 141
 142         prog = parse_and_check_keyprog(binprog);
 143         free(binprog);
 144
 145         if (!prog)
 146                 return 1;
 147
 148         if (hre_run_program(prog->code, prog->code_size)) {
 149                 free(prog);
 150                 return 1;
 151         }
 152
 153         printf("\nSD code ran successfully\n");
 154
 155         free(prog);
 156
 157         return 0;
 158 }