board/gdsys/a38x/keyprogram.c

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